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

* 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

src/main/cli/settings.c

@@ -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

src/main/fc/parameter_names.h

@@ -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

src/main/flight/imu.c

@@ -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)

src/main/flight/imu.h

@@ -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);

src/main/msp/msp.c

@@ -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;
     }