Fix helio attitude estimation (STABLE-MODE) (ACC/GYRO) (#886)

* Update gyro.c * scale ACC for Helio too --------- Co-authored-by: nerdCopter <[email protected]>
emuflight · May 11, 2023 · a70f6f6 · a70f6f6
1 parent e2d093c
commit a70f6f6
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 12 deletions.
diff --git a/src/main/interface/msp.c b/src/main/interface/msp.c
@@ -745,20 +745,24 @@ bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) {
     case MSP_RAW_IMU: {
         // Hack scale due to choice of units for sensor data in multiwii
         uint8_t scale = 1;
-#ifndef USE_GYRO_IMUF9001
+//#ifndef USE_GYRO_IMUF9001
         if (acc.dev.acc_1G > 512 * 4) {
             scale = 8;
         } else if (acc.dev.acc_1G > 512 * 2) {
             scale = 4;
         } else if (acc.dev.acc_1G >= 512) {
             scale = 2;
         }
-#endif //USE_GYRO_IMUF9001
+//#endif //USE_GYRO_IMUF9001
         for (int i = 0; i < 3; i++) {
             sbufWriteU16(dst, lrintf(acc.accADC[i] / scale));
         }
         for (int i = 0; i < 3; i++) {
+#ifdef USE_GYRO_IMUF9001
+            sbufWriteU16(dst, gyroRateDps(i) * scale);
+#else
             sbufWriteU16(dst, gyroRateDps(i));
+#endif
         }
         for (int i = 0; i < 3; i++) {
             sbufWriteU16(dst, lrintf(mag.magADC[i]));

diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
@@ -1064,11 +1064,6 @@ static FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t* gyroSensor, timeUs
     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
         DEBUG_SET(DEBUG_GYRO_SCALED, axis, lrintf(gyroSensor->gyroDev.gyroADCf[axis]));
-        if (!gyroSensor->overflowDetected) {
-            // integrate using trapezium rule to avoid bias
-            accumulatedMeasurements[axis] += 0.5f * (gyroPrevious[axis] + gyro.gyroADCf[axis]) * gyro.targetLooptime;
-            gyroPrevious[axis] = gyroSensor->gyroDev.gyroADCf[axis];
-        }
     }
     if (!isGyroSensorCalibrationComplete(gyroSensor)) {
         performGyroCalibration(gyroSensor, gyroConfig()->gyroMovementCalibrationThreshold);
@@ -1222,7 +1217,7 @@ FAST_CODE_NOINLINE void gyroUpdate(timeUs_t currentTimeUs) {
     if (!overflowDetected) {
         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
             // integrate using trapezium rule to avoid bias
-            accumulatedMeasurements[axis] += 0.5f * (gyroPrevious[axis] + gyro.gyroADCf[axis]) * gyro.targetLooptime;
+            accumulatedMeasurements[axis] += (gyroPrevious[axis] + gyro.gyroADCf[axis]);
             gyroPrevious[axis] = gyro.gyroADCf[axis];
         }
         accumulatedMeasurementCount++;
@@ -1231,11 +1226,11 @@ FAST_CODE_NOINLINE void gyroUpdate(timeUs_t currentTimeUs) {
 
 bool gyroGetAverage(quaternion *vAverage) {
     if (accumulatedMeasurementCount) {
-        const timeUs_t accumulatedMeasurementTimeUs = accumulatedMeasurementCount * gyro.targetLooptime;
+        const timeUs_t accumulatedMeasurementTimeUs = accumulatedMeasurementCount;
         vAverage->w = 0;
-        vAverage->x = DEGREES_TO_RADIANS(accumulatedMeasurements[X] / accumulatedMeasurementTimeUs);
-        vAverage->y = DEGREES_TO_RADIANS(accumulatedMeasurements[Y] / accumulatedMeasurementTimeUs);
-        vAverage->z = DEGREES_TO_RADIANS(accumulatedMeasurements[Z] / accumulatedMeasurementTimeUs);
+        vAverage->x = 0.5f * DEGREES_TO_RADIANS(accumulatedMeasurements[X] / accumulatedMeasurementTimeUs);
+        vAverage->y = 0.5f * DEGREES_TO_RADIANS(accumulatedMeasurements[Y] / accumulatedMeasurementTimeUs);
+        vAverage->z = 0.5f * DEGREES_TO_RADIANS(accumulatedMeasurements[Z] / accumulatedMeasurementTimeUs);
         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
             accumulatedMeasurements[axis] = 0.0f;
         }