Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-25 01:05:27 +03:00
Code Issues Wiki Activity

Add gyro and magnetic custom alignment to MSP (#14019)

Browse source 
* Add magnetic dip and alignment to MSP

* Update PG

* Revert inclination addition

* Revert heading units

* space

* Use MSP_SENSOR_ALIGNMENT

* Missed bracket

* Adjust mag_declination range

* Add gyro custom alignment

* Remove precision from declination

* Revert remove precision from declination
This commit is contained in:
Mark Haslinghuis 2025-01-09 22:41:41 +01:00 • committed by GitHub
parent a8d599e187
commit 51c09efe69
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
5 changed files with 60 additions and 5 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/cli/settings.c
View file

@ -1081,7 +1081,7 @@ const clivalue_t valueTable[] = {
{ PARAM_NAME_IMU_SMALL_ANGLE, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 180 }, PG_IMU_CONFIG, offsetof(imuConfig_t, small_angle) },
{ PARAM_NAME_IMU_PROCESS_DENOM, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 1, 4 }, PG_IMU_CONFIG, offsetof(imuConfig_t, imu_process_denom) },
#ifdef USE_MAG
{ PARAM_NAME_IMU_MAG_DECLINATION, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 3599 }, PG_IMU_CONFIG, offsetof(imuConfig_t, mag_declination) },
{ PARAM_NAME_IMU_MAG_DECLINATION, VAR_INT16 | MASTER_VALUE, .config.minmaxUnsigned = { -300, 300 }, PG_IMU_CONFIG, offsetof(imuConfig_t, mag_declination) },
#endif
// PG_ARMING_CONFIG

1
src/main/fc/parameter_names.h
View file

@ -262,6 +262,7 @@
#define PARAM_NAME_IMU_DCM_KI "imu_dcm_ki"
#define PARAM_NAME_IMU_SMALL_ANGLE "small_angle"
#define PARAM_NAME_IMU_PROCESS_DENOM "imu_process_denom"
#ifdef USE_MAG
#define PARAM_NAME_IMU_MAG_DECLINATION "mag_declination"
#endif

2
src/main/flight/imu.c
View file

@ -125,7 +125,7 @@ PG_RESET_TEMPLATE(imuConfig_t, imuConfig,
.imu_dcm_ki = 0, // 0.003 * 10000
.small_angle = DEFAULT_SMALL_ANGLE,
.imu_process_denom = 2,
.mag_declination = 0
.mag_declination = 0,
);
static void imuQuaternionComputeProducts(quaternion_t *quat, quaternionProducts *quatProd)

3
src/main/flight/imu.h
View file

@ -63,7 +63,7 @@ typedef struct imuConfig_s {
uint16_t imu_dcm_ki; // DCM filter integral gain ( x 10000)
uint8_t small_angle;
uint8_t imu_process_denom;
uint16_t mag_declination; // Magnetic declination in degrees * 10
int16_t mag_declination; // Magnetic declination in degrees * 10
} imuConfig_t;
PG_DECLARE(imuConfig_t, imuConfig);
@ -92,5 +92,4 @@ void imuSetHasNewData(uint32_t dt);
bool imuQuaternionHeadfreeOffsetSet(void);
void imuQuaternionHeadfreeTransformVectorEarthToBody(vector3_t *v);
bool shouldInitializeGPSHeading(void);
bool isUpright(void);

57
src/main/msp/msp.c
View file

@ -1847,7 +1847,33 @@ case MSP_NAME:
sbufWriteU8(dst, gyroDeviceConfig(0)->alignment);
sbufWriteU8(dst, ALIGN_DEFAULT);
#endif
// Added in MSP API 1.47
switch (gyroConfig()->gyro_to_use) {
#ifdef USE_MULTI_GYRO
case GYRO_CONFIG_USE_GYRO_2:
sbufWriteU16(dst, gyroDeviceConfig(1)->customAlignment.roll);
sbufWriteU16(dst, gyroDeviceConfig(1)->customAlignment.pitch);
sbufWriteU16(dst, gyroDeviceConfig(1)->customAlignment.yaw);
break;
#endif
case GYRO_CONFIG_USE_GYRO_BOTH:
// for dual-gyro in "BOTH" mode we only read/write gyro 0
default:
sbufWriteU16(dst, gyroDeviceConfig(0)->customAlignment.roll);
sbufWriteU16(dst, gyroDeviceConfig(0)->customAlignment.pitch);
sbufWriteU16(dst, gyroDeviceConfig(0)->customAlignment.yaw);
break;
}
#ifdef USE_MAG
sbufWriteU16(dst, compassConfig()->mag_customAlignment.roll);
sbufWriteU16(dst, compassConfig()->mag_customAlignment.pitch);
sbufWriteU16(dst, compassConfig()->mag_customAlignment.yaw);
#else
sbufWriteU16(dst, 0);
sbufWriteU16(dst, 0);
sbufWriteU16(dst, 0);
#endif
break;
}
case MSP_ADVANCED_CONFIG:
@ -3024,7 +3050,36 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
#else
gyroDeviceConfigMutable(0)->alignment = gyroAlignment;
#endif
}
// Added in API 1.47
if (sbufBytesRemaining(src) >= 6) {
switch (gyroConfig()->gyro_to_use) {
#ifdef USE_MULTI_GYRO
case GYRO_CONFIG_USE_GYRO_2:
gyroDeviceConfigMutable(1)->customAlignment.roll = sbufReadU16(src);
gyroDeviceConfigMutable(1)->customAlignment.pitch = sbufReadU16(src);
gyroDeviceConfigMutable(1)->customAlignment.yaw = sbufReadU16(src);
break;
#endif
case GYRO_CONFIG_USE_GYRO_BOTH:
// For dual-gyro in "BOTH" mode we'll only update gyro 0
default:
gyroDeviceConfigMutable(0)->customAlignment.roll = sbufReadU16(src);
gyroDeviceConfigMutable(0)->customAlignment.pitch = sbufReadU16(src);
gyroDeviceConfigMutable(0)->customAlignment.yaw = sbufReadU16(src);
break;
}
}
if (sbufBytesRemaining(src) >= 6) {
#ifdef USE_MAG
compassConfigMutable()->mag_customAlignment.roll = sbufReadU16(src);
compassConfigMutable()->mag_customAlignment.pitch = sbufReadU16(src);
compassConfigMutable()->mag_customAlignment.yaw = sbufReadU16(src);
#else
sbufReadU16(src);
sbufReadU16(src);
sbufReadU16(src);
#endif
}
break;
}