From 34b0707cb6d0c4eb0fef69915f8e94a611d52d0c Mon Sep 17 00:00:00 2001 From: leocb Date: Thu, 9 Aug 2018 14:41:05 -0300 Subject: [PATCH] change divisions by acc_1G to multiply by the reciprocal value --- src/main/drivers/accgyro/accgyro.h | 1 + src/main/flight/gps_rescue.c | 2 +- src/main/flight/imu.c | 8 ++++---- src/main/io/osd.c | 2 +- src/main/sensors/acceleration.c | 1 + src/main/telemetry/frsky_hub.c | 2 +- src/main/telemetry/ibus_shared.c | 2 +- src/main/telemetry/smartport.c | 6 +++--- 8 files changed, 13 insertions(+), 11 deletions(-) diff --git a/src/main/drivers/accgyro/accgyro.h b/src/main/drivers/accgyro/accgyro.h index fe7560e136..3174fa9646 100644 --- a/src/main/drivers/accgyro/accgyro.h +++ b/src/main/drivers/accgyro/accgyro.h @@ -93,6 +93,7 @@ typedef struct accDev_s { sensorAccReadFuncPtr readFn; // read 3 axis data function busDevice_t bus; uint16_t acc_1G; + float acc_1G_rec; int16_t ADCRaw[XYZ_AXIS_COUNT]; mpuDetectionResult_t mpuDetectionResult; sensor_align_e accAlign; diff --git a/src/main/flight/gps_rescue.c b/src/main/flight/gps_rescue.c index 3ead432096..dbb5105dd2 100644 --- a/src/main/flight/gps_rescue.c +++ b/src/main/flight/gps_rescue.c @@ -237,7 +237,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) (sqrt(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; diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 7b5011b827..8dec160a5f 100644 --- a/src/main/flight/imu.c +++ b/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. diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 6fd374bc09..d689a7fb41 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -699,7 +699,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; } diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index c1616fca0c..e938e17006 100644 --- a/src/main/sensors/acceleration.c +++ b/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: diff --git a/src/main/telemetry/frsky_hub.c b/src/main/telemetry/frsky_hub.c index 6c42c2d7d7..7bbd05edcd 100644 --- a/src/main/telemetry/frsky_hub.c +++ b/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)); } } diff --git a/src/main/telemetry/ibus_shared.c b/src/main/telemetry/ibus_shared.c index 7f3dc9122c..89998f6fe5 100644 --- a/src/main/telemetry/ibus_shared.c +++ b/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) diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 283c0a5e93..a3b8234558 100644 --- a/src/main/telemetry/smartport.c +++ b/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 :