mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-16 21:05:35 +03:00
change divisions by acc_1G to multiply by the reciprocal value
This commit is contained in:
parent
946cbd257f
commit
34b0707cb6
8 changed files with 13 additions and 11 deletions
|
@ -93,6 +93,7 @@ typedef struct accDev_s {
|
||||||
sensorAccReadFuncPtr readFn; // read 3 axis data function
|
sensorAccReadFuncPtr readFn; // read 3 axis data function
|
||||||
busDevice_t bus;
|
busDevice_t bus;
|
||||||
uint16_t acc_1G;
|
uint16_t acc_1G;
|
||||||
|
float acc_1G_rec;
|
||||||
int16_t ADCRaw[XYZ_AXIS_COUNT];
|
int16_t ADCRaw[XYZ_AXIS_COUNT];
|
||||||
mpuDetectionResult_t mpuDetectionResult;
|
mpuDetectionResult_t mpuDetectionResult;
|
||||||
sensor_align_e accAlign;
|
sensor_align_e accAlign;
|
||||||
|
|
|
@ -237,7 +237,7 @@ void sensorUpdate()
|
||||||
rescueState.sensor.zVelocity = (rescueState.sensor.currentAltitude - previousAltitude) * 1000000.0f / dTime;
|
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.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);
|
rescueState.sensor.accMagnitudeAvg = (rescueState.sensor.accMagnitudeAvg * 0.8f) + (rescueState.sensor.accMagnitude * 0.2f);
|
||||||
|
|
||||||
previousAltitude = rescueState.sensor.currentAltitude;
|
previousAltitude = rescueState.sensor.currentAltitude;
|
||||||
|
|
|
@ -171,7 +171,7 @@ void imuConfigure(uint16_t throttle_correction_angle, uint8_t throttle_correctio
|
||||||
void imuInit(void)
|
void imuInit(void)
|
||||||
{
|
{
|
||||||
smallAngleCosZ = cos_approx(degreesToRadians(imuRuntimeConfig.small_angle));
|
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
|
#ifdef USE_GPS
|
||||||
canUseGPSHeading = true;
|
canUseGPSHeading = true;
|
||||||
|
@ -361,10 +361,10 @@ static bool imuIsAccelerometerHealthy(float *accAverage)
|
||||||
accMagnitude += a * a;
|
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
|
// Accept accel readings only in range 0.80g - 1.20g
|
||||||
return (81 < accMagnitude) && (accMagnitude < 121);
|
return (0.80f <= accMagnitude) && (accMagnitude <= 1.20f);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Calculate the dcmKpGain to use. When armed, the gain is imuRuntimeConfig.dcm_kp * 1.0 scaling.
|
// Calculate the dcmKpGain to use. When armed, the gain is imuRuntimeConfig.dcm_kp * 1.0 scaling.
|
||||||
|
|
|
@ -699,7 +699,7 @@ static bool osdDrawSingleElement(uint8_t item)
|
||||||
const float a = accAverage[axis];
|
const float a = accAverage[axis];
|
||||||
osdGForce += a * a;
|
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);
|
tfp_sprintf(buff, "%01d.%01dG", (int)osdGForce, (int)(osdGForce * 10) % 10);
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
|
@ -362,6 +362,7 @@ bool accInit(uint32_t gyroSamplingInverval)
|
||||||
}
|
}
|
||||||
acc.dev.acc_1G = 256; // set default
|
acc.dev.acc_1G = 256; // set default
|
||||||
acc.dev.initFn(&acc.dev); // driver initialisation
|
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
|
// 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
|
switch (gyroSamplingInverval) { // Switch statement kept in place to change acc sampling interval in the future
|
||||||
case 500:
|
case 500:
|
||||||
|
|
|
@ -177,7 +177,7 @@ static void frSkyHubWriteByteInternal(const char data)
|
||||||
static void sendAccel(void)
|
static void sendAccel(void)
|
||||||
{
|
{
|
||||||
for (unsigned i = 0; i < 3; i++) {
|
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));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -270,7 +270,7 @@ static uint16_t getMode()
|
||||||
|
|
||||||
static int16_t getACC(uint8_t index)
|
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)
|
#if defined(USE_TELEMETRY_IBUS_EXTENDED)
|
||||||
|
|
|
@ -651,15 +651,15 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
|
||||||
*clearToSend = false;
|
*clearToSend = false;
|
||||||
break;
|
break;
|
||||||
case FSSP_DATAID_ACCX :
|
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;
|
*clearToSend = false;
|
||||||
break;
|
break;
|
||||||
case FSSP_DATAID_ACCY :
|
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;
|
*clearToSend = false;
|
||||||
break;
|
break;
|
||||||
case FSSP_DATAID_ACCZ :
|
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;
|
*clearToSend = false;
|
||||||
break;
|
break;
|
||||||
case FSSP_DATAID_T1 :
|
case FSSP_DATAID_T1 :
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue