Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 21:05:35 +03:00
Code Issues Wiki Activity

change divisions by acc_1G to multiply by the reciprocal value

Browse source
This commit is contained in:
leocb 2018-08-09 14:41:05 -03:00
parent 946cbd257f
commit 34b0707cb6
8 changed files with 13 additions and 11 deletions
Download patch file Download diff file Expand all files Collapse all files

1
src/main/drivers/accgyro/accgyro.h
View file

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

2
src/main/flight/gps_rescue.c
View file

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

8
src/main/flight/imu.c
View file

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

2
src/main/io/osd.c
View file

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

1
src/main/sensors/acceleration.c
View file

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

2
src/main/telemetry/frsky_hub.c
View file

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

2
src/main/telemetry/ibus_shared.c
View file

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

6
src/main/telemetry/smartport.c
View file

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