Merge pull request #6536 from leocb/Acc-reciprocal

Change divisions by acc_1G to multiply by the reciprocal value

src/main/drivers/accgyro/accgyro.h

@@ -89,6 +89,7 @@ typedef struct accDev_s {
 #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
     pthread_mutex_t lock;
 #endif
+    float acc_1G_rec;
     sensorAccInitFuncPtr initFn;                              // initialize function
     sensorAccReadFuncPtr readFn;                              // read 3 axis data function
     busDevice_t bus;

src/main/flight/gps_rescue.c

@@ -238,7 +238,7 @@ void sensorUpdate()
         rescueState.sensor.zVelocity = (rescueState.sensor.currentAltitude - previousAltitude) * 1000000.0f / dTime;
         rescueState.sensor.zVelocityAvg = 0.8f * rescueState.sensor.zVelocityAvg + rescueState.sensor.zVelocity * 0.2f;
 
-        rescueState.sensor.accMagnitude = (float) sqrt(sq(acc.accADC[Z]) + sq(acc.accADC[X]) + sq(acc.accADC[Y]) / sq(acc.dev.acc_1G));
+        rescueState.sensor.accMagnitude = (float) sqrtf(sq(acc.accADC[Z]) + sq(acc.accADC[X]) + sq(acc.accADC[Y])) * acc.dev.acc_1G_rec;
         rescueState.sensor.accMagnitudeAvg = (rescueState.sensor.accMagnitudeAvg * 0.8f) + (rescueState.sensor.accMagnitude * 0.2f);
 
         previousAltitude = rescueState.sensor.currentAltitude;

src/main/flight/imu.c

@@ -171,7 +171,7 @@ void imuConfigure(uint16_t throttle_correction_angle, uint8_t throttle_correctio
 void imuInit(void)
 {
     smallAngleCosZ = cos_approx(degreesToRadians(imuRuntimeConfig.small_angle));
-    accVelScale = 9.80665f / acc.dev.acc_1G / 10000.0f;
+    accVelScale = 9.80665f * acc.dev.acc_1G_rec / 10000.0f;
 
 #ifdef USE_GPS
     canUseGPSHeading = true;
@@ -361,10 +361,10 @@ static bool imuIsAccelerometerHealthy(float *accAverage)
         accMagnitude += a * a;
     }
 
-    accMagnitude = accMagnitude * 100 / (sq((int32_t)acc.dev.acc_1G));
+    accMagnitude = sqrtf(accMagnitude) * acc.dev.acc_1G_rec;
 
-    // Accept accel readings only in range 0.90g - 1.10g
-    return (81 < accMagnitude) && (accMagnitude < 121);
+    // Accept accel readings only in range 0.80g - 1.20g
+    return (0.80f <= accMagnitude) && (accMagnitude <= 1.20f);
 }
 
 // Calculate the dcmKpGain to use. When armed, the gain is imuRuntimeConfig.dcm_kp * 1.0 scaling.

src/main/io/osd.c

@@ -710,7 +710,7 @@ static bool osdDrawSingleElement(uint8_t item)
                 const float a = accAverage[axis];
                 osdGForce += a * a;
             }
-            osdGForce = sqrtf(osdGForce) / acc.dev.acc_1G;
+            osdGForce = sqrtf(osdGForce) * acc.dev.acc_1G_rec;
             tfp_sprintf(buff, "%01d.%01dG", (int)osdGForce, (int)(osdGForce * 10) % 10);
             break;
         }

src/main/sensors/acceleration.c

@@ -362,6 +362,7 @@ bool accInit(uint32_t gyroSamplingInverval)
     }
     acc.dev.acc_1G = 256; // set default
     acc.dev.initFn(&acc.dev); // driver initialisation
+    acc.dev.acc_1G_rec = 1 / acc.dev.acc_1G;
     // set the acc sampling interval according to the gyro sampling interval
     switch (gyroSamplingInverval) {  // Switch statement kept in place to change acc sampling interval in the future
     case 500:

src/main/telemetry/frsky_hub.c

@@ -177,7 +177,7 @@ static void frSkyHubWriteByteInternal(const char data)
 static void sendAccel(void)
 {
     for (unsigned i = 0; i < 3; i++) {
-        frSkyHubWriteFrame(ID_ACC_X + i, ((int16_t)(acc.accADC[i] / acc.dev.acc_1G) * 1000));
+        frSkyHubWriteFrame(ID_ACC_X + i, ((int16_t)(acc.accADC[i] * acc.dev.acc_1G_rec) * 1000));
     }
 }
 

src/main/telemetry/ibus_shared.c

@@ -270,7 +270,7 @@ static uint16_t getMode()
 
 static int16_t getACC(uint8_t index)
 {
-    return (int16_t)((acc.accADC[index] / acc.dev.acc_1G) * 1000);
+    return (int16_t)((acc.accADC[index] * acc.dev.acc_1G_rec) * 1000);
 }
 
 #if defined(USE_TELEMETRY_IBUS_EXTENDED)

src/main/telemetry/smartport.c

@@ -651,15 +651,15 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
                 *clearToSend = false;
                 break;
             case FSSP_DATAID_ACCX       :
-                smartPortSendPackage(id, lrintf(100 * acc.accADC[X] / acc.dev.acc_1G)); // Multiply by 100 to show as x.xx g on Taranis
+                smartPortSendPackage(id, lrintf(100 * acc.accADC[X] * acc.dev.acc_1G_rec)); // Multiply by 100 to show as x.xx g on Taranis
                 *clearToSend = false;
                 break;
             case FSSP_DATAID_ACCY       :
-                smartPortSendPackage(id, lrintf(100 * acc.accADC[Y] / acc.dev.acc_1G));
+                smartPortSendPackage(id, lrintf(100 * acc.accADC[Y] * acc.dev.acc_1G_rec));
                 *clearToSend = false;
                 break;
             case FSSP_DATAID_ACCZ       :
-                smartPortSendPackage(id, lrintf(100 * acc.accADC[Z] / acc.dev.acc_1G));
+                smartPortSendPackage(id, lrintf(100 * acc.accADC[Z] * acc.dev.acc_1G_rec));
                 *clearToSend = false;
                 break;
             case FSSP_DATAID_T1         :