Introduced cli parameter <gyro_offset_yaw>

It allows to manually compensate gyro drift over time.
rebased squashed.

src/main/drivers/accgyro/accgyro.h

@@ -63,8 +63,8 @@ typedef struct gyroDev_s {
     extiCallbackRec_t exti;
     busDevice_t bus;
     float scale;                                            // scalefactor
-    int32_t gyroZero[XYZ_AXIS_COUNT];
+    float gyroZero[XYZ_AXIS_COUNT];
-    int32_t gyroADC[XYZ_AXIS_COUNT];                        // gyro data after calibration and alignment
+    float gyroADC[XYZ_AXIS_COUNT];                        // gyro data after calibration and alignment
     int32_t gyroADCRawPrevious[XYZ_AXIS_COUNT];
     int16_t gyroADCRaw[XYZ_AXIS_COUNT];
     int16_t temperature;

src/main/interface/settings.c

@@ -368,6 +368,7 @@ const clivalue_t valueTable[] = {
     { "gyro_filter_p",              VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_filter_p) },
 #endif
     { "moron_threshold",            VAR_UINT8  | MASTER_VALUE, .config.minmax = { 0,  200 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyroMovementCalibrationThreshold) },
+    { "gyro_offset_yaw",            VAR_INT16 | MASTER_VALUE, .config.minmax = { -1000, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_offset_yaw) },
 #ifdef USE_GYRO_OVERFLOW_CHECK
     { "gyro_overflow_detect",       VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO_OVERFLOW_CHECK }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, checkOverflow) },
 #endif

src/main/io/dashboard.c

@@ -486,7 +486,7 @@ static void showSensorsPage(void)
     i2c_OLED_send_string(bus, "        X     Y     Z");
 
     if (sensors(SENSOR_ACC)) {
-        tfp_sprintf(lineBuffer, format, "ACC", acc.accADC[X], acc.accADC[Y], acc.accADC[Z]);
+        tfp_sprintf(lineBuffer, format, "ACC", lrintf(acc.accADC[X]), lrintf(acc.accADC[Y]), lrintf(acc.accADC[Z]));
         padLineBuffer();
         i2c_OLED_set_line(bus, rowIndex++);
         i2c_OLED_send_string(bus, lineBuffer);
@@ -501,7 +501,7 @@ static void showSensorsPage(void)
 
 #ifdef USE_MAG
     if (sensors(SENSOR_MAG)) {
-        tfp_sprintf(lineBuffer, format, "MAG", mag.magADC[X], mag.magADC[Y], mag.magADC[Z]);
+        tfp_sprintf(lineBuffer, format, "MAG", lrintf(mag.magADC[X]), lrintf(mag.magADC[Y]), lrintf(mag.magADC[Z]));
         padLineBuffer();
         i2c_OLED_set_line(bus, rowIndex++);
         i2c_OLED_send_string(bus, lineBuffer);

src/main/sensors/acceleration.h

@@ -46,7 +46,7 @@ typedef enum {
 typedef struct acc_s {
     accDev_t dev;
     uint32_t accSamplingInterval;
-    int32_t accADC[XYZ_AXIS_COUNT];
+    float accADC[XYZ_AXIS_COUNT];
     bool isAccelUpdatedAtLeastOnce;
 } acc_t;
 

src/main/sensors/boardalignment.c

@@ -59,22 +59,22 @@ void initBoardAlignment(const boardAlignment_t *boardAlignment)
     buildRotationMatrix(&rotationAngles, boardRotation);
 }
 
-static void alignBoard(int32_t *vec)
+static void alignBoard(float *vec)
 {
-    int32_t x = vec[X];
+    float x = vec[X];
-    int32_t y = vec[Y];
+    float y = vec[Y];
-    int32_t z = vec[Z];
+    float z = vec[Z];
 
-    vec[X] = lrintf(boardRotation[0][X] * x + boardRotation[1][X] * y + boardRotation[2][X] * z);
+    vec[X] = (boardRotation[0][X] * x + boardRotation[1][X] * y + boardRotation[2][X] * z);
-    vec[Y] = lrintf(boardRotation[0][Y] * x + boardRotation[1][Y] * y + boardRotation[2][Y] * z);
+    vec[Y] = (boardRotation[0][Y] * x + boardRotation[1][Y] * y + boardRotation[2][Y] * z);
-    vec[Z] = lrintf(boardRotation[0][Z] * x + boardRotation[1][Z] * y + boardRotation[2][Z] * z);
+    vec[Z] = (boardRotation[0][Z] * x + boardRotation[1][Z] * y + boardRotation[2][Z] * z);
 }
 
-FAST_CODE void alignSensors(int32_t *dest, uint8_t rotation)
+FAST_CODE void alignSensors(float *dest, uint8_t rotation)
 {
-    const int32_t x = dest[X];
+    const float x = dest[X];
-    const int32_t y = dest[Y];
+    const float y = dest[Y];
-    const int32_t z = dest[Z];
+    const float z = dest[Z];
 
     switch (rotation) {
     default:

src/main/sensors/boardalignment.h

@@ -27,5 +27,5 @@ typedef struct boardAlignment_s {
 
 PG_DECLARE(boardAlignment_t, boardAlignment);
 
-void alignSensors(int32_t *dest, uint8_t rotation);
+void alignSensors(float *dest, uint8_t rotation);
 void initBoardAlignment(const boardAlignment_t *boardAlignment);

src/main/sensors/compass.h

@@ -34,7 +34,7 @@ typedef enum {
 } magSensor_e;
 
 typedef struct mag_s {
-    int32_t magADC[XYZ_AXIS_COUNT];
+    float magADC[XYZ_AXIS_COUNT];
     float magneticDeclination;
 } mag_t;
 
@@ -59,4 +59,3 @@ PG_DECLARE(compassConfig_t, compassConfig);
 bool compassIsHealthy(void);
 void compassUpdate(timeUs_t currentTime);
 bool compassInit(void);
-

src/main/sensors/gyro.c

@@ -173,6 +173,7 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
     .gyro_filter_q = 0,
     .gyro_filter_r = 0,
     .gyro_filter_p = 0,
+    .gyro_offset_yaw = 0,
 );
 
 
@@ -681,7 +682,12 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroSensor_t *gyroSensor, uint8_t
                 gyroSetCalibrationCycles(gyroSensor);
                 return;
             }
-            gyroSensor->gyroDev.gyroZero[axis] = (gyroSensor->calibration.sum[axis] + (gyroCalculateCalibratingCycles() / 2)) / gyroCalculateCalibratingCycles();
+
+            // please take care with exotic boardalignment !!
+            gyroSensor->gyroDev.gyroZero[axis] = gyroSensor->calibration.sum[axis] / gyroCalculateCalibratingCycles();
+            if (axis == Z) {
+              gyroSensor->gyroDev.gyroZero[axis] -= ((float)gyroConfig()->gyro_offset_yaw / 100);
+            }
         }
     }
 
@@ -721,9 +727,9 @@ static void checkForOverflow(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
     // This can cause an overflow and sign reversal in the output.
     // Overflow and sign reversal seems to result in a gyro value of +1996 or -1996.
     if (gyroSensor->overflowDetected) {
-        const float gyroRateX = (float)gyroSensor->gyroDev.gyroADC[X] * gyroSensor->gyroDev.scale;
+        const float gyroRateX = gyroSensor->gyroDev.gyroADC[X] * gyroSensor->gyroDev.scale;
-        const float gyroRateY = (float)gyroSensor->gyroDev.gyroADC[Y] * gyroSensor->gyroDev.scale;
+        const float gyroRateY = gyroSensor->gyroDev.gyroADC[Y] * gyroSensor->gyroDev.scale;
-        const float gyroRateZ = (float)gyroSensor->gyroDev.gyroADC[Z] * gyroSensor->gyroDev.scale;
+        const float gyroRateZ = gyroSensor->gyroDev.gyroADC[Z] * gyroSensor->gyroDev.scale;
         static const int overflowResetThreshold = 1800;
         if (abs(gyroRateX) < overflowResetThreshold
               && abs(gyroRateY) < overflowResetThreshold
@@ -798,7 +804,7 @@ static FAST_CODE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t curren
     if (gyroDebugMode == DEBUG_NONE) {
         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
             // NOTE: this branch optimized for when there is no gyro debugging, ensure it is kept in step with non-optimized branch
-            float gyroADCf = (float)gyroSensor->gyroDev.gyroADC[axis] * gyroSensor->gyroDev.scale;
+            float gyroADCf = gyroSensor->gyroDev.gyroADC[axis] * gyroSensor->gyroDev.scale;
 #if defined(USE_GYRO_FAST_KALMAN)
             gyroADCf = gyroSensor->fastKalmanApplyFn((filter_t *)&gyroSensor->fastKalman[axis], gyroADCf);
 #elif defined(USE_GYRO_BIQUAD_RC_FIR2)
@@ -821,7 +827,7 @@ static FAST_CODE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t curren
         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
             DEBUG_SET(DEBUG_GYRO_RAW, axis, gyroSensor->gyroDev.gyroADCRaw[axis]);
             // scale gyro output to degrees per second
-            float gyroADCf = (float)gyroSensor->gyroDev.gyroADC[axis] * gyroSensor->gyroDev.scale;
+            float gyroADCf = gyroSensor->gyroDev.gyroADC[axis] * gyroSensor->gyroDev.scale;
             // DEBUG_GYRO_NOTCH records the unfiltered gyro output
             DEBUG_SET(DEBUG_GYRO_NOTCH, axis, lrintf(gyroADCf));
 

src/main/sensors/gyro.h

@@ -73,6 +73,7 @@ typedef struct gyroConfig_s {
     uint16_t gyro_filter_q;
     uint16_t gyro_filter_r;
     uint16_t gyro_filter_p;
+    int16_t  gyro_offset_yaw;
 } gyroConfig_t;
 
 PG_DECLARE(gyroConfig_t, gyroConfig);

src/main/sensors/sensors.h

@@ -40,7 +40,7 @@ typedef union flightDynamicsTrims_u {
     flightDynamicsTrims_def_t values;
 } flightDynamicsTrims_t;
 
-#define CALIBRATING_GYRO_CYCLES             1000
+#define CALIBRATING_GYRO_CYCLES             10000
 #define CALIBRATING_ACC_CYCLES              400
 #define CALIBRATING_BARO_CYCLES             200 // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles
 

src/test/unit/alignsensor_unittest.cc

@@ -40,9 +40,9 @@ extern "C" {
 
 #define DEG2RAD 0.01745329251
 
-static void rotateVector(int32_t mat[3][3], int32_t vec[3], int32_t *out)
+static void rotateVector(int32_t mat[3][3], float vec[3], float *out)
 {
-    int32_t tmp[3];
+    float tmp[3];
 
     for(int i=0; i<3; i++) {
         tmp[i] = 0;
@@ -98,8 +98,8 @@ static void initZAxisRotation(int32_t mat[][3], int32_t angle)
 
 static void testCW(sensor_align_e rotation, int32_t angle)
 {
-    int32_t src[XYZ_AXIS_COUNT];
+    float src[XYZ_AXIS_COUNT];
-    int32_t test[XYZ_AXIS_COUNT];
+    float test[XYZ_AXIS_COUNT];
 
     // unit vector along x-axis
     src[X] = 1;
@@ -155,8 +155,8 @@ static void testCW(sensor_align_e rotation, int32_t angle)
  */
 static void testCWFlip(sensor_align_e rotation, int32_t angle)
 {
-    int32_t src[XYZ_AXIS_COUNT];
+    float src[XYZ_AXIS_COUNT];
-    int32_t test[XYZ_AXIS_COUNT];
+    float test[XYZ_AXIS_COUNT];
 
     // unit vector along x-axis
     src[X] = 1;