Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-24 00:35:34 +03:00
Code Issues Wiki Activity

Scale acc value in acc sensor code

Browse source
This commit is contained in:
Martin Budden 2017-11-28 06:50:43 +00:00
parent 61f895333b
commit 089680ffce
7 changed files with 41 additions and 39 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/blackbox/blackbox.c
View file

@ -1134,7 +1134,7 @@ static void loadMainState(timeUs_t currentTimeUs)
blackboxCurrent->axisPID_I[i] = axisPID_I[i]; blackboxCurrent->axisPID_I[i] = axisPID_I[i];
blackboxCurrent->axisPID_D[i] = axisPID_D[i]; blackboxCurrent->axisPID_D[i] = axisPID_D[i];
blackboxCurrent->gyroADC[i] = lrintf(gyro.gyroADCf[i]); blackboxCurrent->gyroADC[i] = lrintf(gyro.gyroADCf[i]);
blackboxCurrent->accADC[i] = acc.accADC[i]; blackboxCurrent->accADC[i] = lrintf(acc.accADCf[i] * acc.dev.acc_1G);
#ifdef MAG #ifdef MAG
blackboxCurrent->magADC[i] = mag.magADC[i]; blackboxCurrent->magADC[i] = mag.magADC[i];
#endif #endif

2
src/main/fc/fc_msp.c
View file

@ -404,7 +404,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
// Hack scale due to choice of units for sensor data in multiwii // Hack scale due to choice of units for sensor data in multiwii
const uint8_t scale = (acc.dev.acc_1G > 1024) ? 8 : 1; const uint8_t scale = (acc.dev.acc_1G > 1024) ? 8 : 1;
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
sbufWriteU16(dst, acc.accADC[i] / scale); sbufWriteU16(dst, acc.accADCf[i] * acc.dev.acc_1G / scale);
} }
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
sbufWriteU16(dst, gyroRateDps(i)); sbufWriteU16(dst, gyroRateDps(i));

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

@ -455,15 +455,14 @@ STATIC_UNIT_TESTED void imuUpdateEulerAngles(void)
static bool imuCanUseAccelerometerForCorrection(void) static bool imuCanUseAccelerometerForCorrection(void)
{ {
int32_t axis; float accMagnitudeSq = 0;
int32_t accMagnitudeSq = 0;
for (axis = 0; axis < 3; axis++) { for (int axis = 0; axis < 3; axis++) {
accMagnitudeSq += (int32_t)acc.accADC[axis] * acc.accADC[axis]; accMagnitudeSq += acc.accADCf[axis] * acc.accADCf[axis];
} }
// Magnitude^2 in percent of G^2 // Magnitude^2 in percent of G^2
const int nearness = ABS(100 - (accMagnitudeSq * 100 / ((int32_t)acc.dev.acc_1G * acc.dev.acc_1G))); const float nearness = ABS(100 - (accMagnitudeSq * 100));
return (nearness > MAX_ACC_SQ_NEARNESS) ? false : true; return (nearness > MAX_ACC_SQ_NEARNESS) ? false : true;
} }
@ -568,7 +567,7 @@ static void imuUpdateMeasuredAcceleration(void)
#else #else
/* Convert acceleration to cm/s/s */ /* Convert acceleration to cm/s/s */
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
imuMeasuredAccelBF.A[axis] = acc.accADC[axis] * (GRAVITY_CMSS / acc.dev.acc_1G); imuMeasuredAccelBF.A[axis] = acc.accADCf[axis] * GRAVITY_CMSS;
} }
#endif #endif
} }
@ -585,9 +584,9 @@ void imuHILUpdate(void)
imuComputeQuaternionFromRPY(attitude.values.roll, attitude.values.pitch, attitude.values.yaw); imuComputeQuaternionFromRPY(attitude.values.roll, attitude.values.pitch, attitude.values.yaw);
/* Fake accADC readings */ /* Fake accADC readings */
accADC[X] = hilToFC.bodyAccel[X] * (acc.acc_1G / GRAVITY_CMSS); accADCf[X] = hilToFC.bodyAccel[X] / GRAVITY_CMSS;
accADC[Y] = hilToFC.bodyAccel[Y] * (acc.acc_1G / GRAVITY_CMSS); accADCf[Y] = hilToFC.bodyAccel[Y] / GRAVITY_CMSS;
accADC[Z] = hilToFC.bodyAccel[Z] * (acc.acc_1G / GRAVITY_CMSS); accADCf[Z] = hilToFC.bodyAccel[Z] / GRAVITY_CMSS;
} }
#endif #endif
@ -607,7 +606,7 @@ void imuUpdateAccelerometer(void)
#ifdef ASYNC_GYRO_PROCESSING #ifdef ASYNC_GYRO_PROCESSING
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
imuAccumulatedAcc[axis] += acc.accADC[axis] * (GRAVITY_CMSS / acc.dev.acc_1G); imuAccumulatedAcc[axis] += acc.accADCf[axis] * GRAVITY_CMSS;
} }
imuAccumulatedAccCount++; imuAccumulatedAccCount++;
#endif #endif
@ -637,9 +636,9 @@ void imuUpdateAttitude(timeUs_t currentTimeUs)
imuCalculateEstimatedAttitude(dT); // Update attitude estimate imuCalculateEstimatedAttitude(dT); // Update attitude estimate
#endif #endif
} else { } else {
acc.accADC[X] = 0; acc.accADCf[X] = 0.0f;
acc.accADC[Y] = 0; acc.accADCf[Y] = 0.0f;
acc.accADC[Z] = 0; acc.accADCf[Z] = 0.0f;
} }
} }

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

@ -69,6 +69,8 @@ FASTRAM acc_t acc; // acc access functions
static uint16_t calibratingA = 0; // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode. static uint16_t calibratingA = 0; // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode.
STATIC_FASTRAM int32_t accADC[XYZ_AXIS_COUNT];
STATIC_FASTRAM biquadFilter_t accFilter[XYZ_AXIS_COUNT]; STATIC_FASTRAM biquadFilter_t accFilter[XYZ_AXIS_COUNT];
#ifdef USE_ACC_NOTCH #ifdef USE_ACC_NOTCH
@ -376,7 +378,7 @@ int getPrimaryAxisIndex(int32_t sample[3])
static void performAcclerationCalibration(void) static void performAcclerationCalibration(void)
{ {
int axisIndex = getPrimaryAxisIndex(acc.accADC); int axisIndex = getPrimaryAxisIndex(accADC);
// Check if sample is usable // Check if sample is usable
if (axisIndex < 0) { if (axisIndex < 0) {
@ -398,10 +400,10 @@ static void performAcclerationCalibration(void)
} }
if (!calibratedAxis[axisIndex]) { if (!calibratedAxis[axisIndex]) {
sensorCalibrationPushSampleForOffsetCalculation(&calState, acc.accADC); sensorCalibrationPushSampleForOffsetCalculation(&calState, accADC);
accSamples[axisIndex][X] += acc.accADC[X]; accSamples[axisIndex][X] += accADC[X];
accSamples[axisIndex][Y] += acc.accADC[Y]; accSamples[axisIndex][Y] += accADC[Y];
accSamples[axisIndex][Z] += acc.accADC[Z]; accSamples[axisIndex][Z] += accADC[Z];
if (isOnFinalAccelerationCalibrationCycle()) { if (isOnFinalAccelerationCalibrationCycle()) {
calibratedAxis[axisIndex] = true; calibratedAxis[axisIndex] = true;
@ -447,9 +449,9 @@ static void performAcclerationCalibration(void)
static void applyAccelerationZero(const flightDynamicsTrims_t * accZero, const flightDynamicsTrims_t * accGain) static void applyAccelerationZero(const flightDynamicsTrims_t * accZero, const flightDynamicsTrims_t * accGain)
{ {
acc.accADC[X] = (acc.accADC[X] - accZero->raw[X]) * accGain->raw[X] / 4096; accADC[X] = (accADC[X] - accZero->raw[X]) * accGain->raw[X] / 4096;
acc.accADC[Y] = (acc.accADC[Y] - accZero->raw[Y]) * accGain->raw[Y] / 4096; accADC[Y] = (accADC[Y] - accZero->raw[Y]) * accGain->raw[Y] / 4096;
acc.accADC[Z] = (acc.accADC[Z] - accZero->raw[Z]) * accGain->raw[Z] / 4096; accADC[Z] = (accADC[Z] - accZero->raw[Z]) * accGain->raw[Z] / 4096;
} }
void accUpdate(void) void accUpdate(void)
@ -459,30 +461,31 @@ void accUpdate(void)
} }
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
acc.accADC[axis] = acc.dev.ADCRaw[axis]; accADC[axis] = acc.dev.ADCRaw[axis];
}
if (!accIsCalibrationComplete()) {
performAcclerationCalibration();
return;
}
applyAccelerationZero(&accelerometerConfig()->accZero, &accelerometerConfig()->accGain);
alignSensors(accADC, acc.dev.accAlign);
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
acc.accADCf[axis] = accADC[axis] / acc.dev.acc_1G;
} }
if (accelerometerConfig()->acc_lpf_hz) { if (accelerometerConfig()->acc_lpf_hz) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
acc.accADC[axis] = lrintf(biquadFilterApply(&accFilter[axis], (float)acc.accADC[axis])); acc.accADCf[axis] = biquadFilterApply(&accFilter[axis], acc.accADCf[axis]);
} }
} }
#ifdef USE_ACC_NOTCH #ifdef USE_ACC_NOTCH
if (accelerometerConfig()->acc_notch_hz) { if (accelerometerConfig()->acc_notch_hz) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
acc.accADC[axis] = lrintf(accNotchFilterApplyFn(accNotchFilter[axis], (float)acc.accADC[axis])); acc.accADCf[axis] = accNotchFilterApplyFn(accNotchFilter[axis], acc.accADCf[axis]);
} }
} }
#endif #endif
if (!accIsCalibrationComplete()) {
performAcclerationCalibration();
}
applyAccelerationZero(&accelerometerConfig()->accZero, &accelerometerConfig()->accGain);
alignSensors(acc.accADC, acc.dev.accAlign);
} }
void accSetCalibrationValues(void) void accSetCalibrationValues(void)

2
src/main/sensors/acceleration.h
View file

@ -41,7 +41,7 @@ typedef enum {
typedef struct acc_s { typedef struct acc_s {
accDev_t dev; accDev_t dev;
uint32_t accTargetLooptime; uint32_t accTargetLooptime;
int32_t accADC[XYZ_AXIS_COUNT]; float accADCf[XYZ_AXIS_COUNT]; // acceleration in g
} acc_t; } acc_t;
extern acc_t acc; extern acc_t acc;

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

@ -158,7 +158,7 @@ static void sendAccel(void)
for (i = 0; i < 3; i++) { for (i = 0; i < 3; i++) {
sendDataHead(ID_ACC_X + i); sendDataHead(ID_ACC_X + i);
serialize16(acc.accADC[i] * 1000 / acc.dev.acc_1G); serialize16(lrintf(acc.accADCf[i] * 1000));
} }
} }

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

@ -640,13 +640,13 @@ void handleSmartPortTelemetry(void)
smartPortSendPackage(id, attitude.values.yaw * 10); // given in 10*deg, requested in 10000 = 100 deg smartPortSendPackage(id, attitude.values.yaw * 10); // given in 10*deg, requested in 10000 = 100 deg
break; break;
case FSSP_DATAID_ACCX : case FSSP_DATAID_ACCX :
smartPortSendPackage(id, 100 * acc.accADC[X] / acc.dev.acc_1G); smartPortSendPackage(id, lrintf(100 * acc.accADCf[X]));
break; break;
case FSSP_DATAID_ACCY : case FSSP_DATAID_ACCY :
smartPortSendPackage(id, 100 * acc.accADC[Y] / acc.dev.acc_1G); smartPortSendPackage(id, lrintf(100 * acc.accADCf[Y]));
break; break;
case FSSP_DATAID_ACCZ : case FSSP_DATAID_ACCZ :
smartPortSendPackage(id, 100 * acc.accADC[Z] / acc.dev.acc_1G); smartPortSendPackage(id, lrintf(100 * acc.accADCf[Z]));
break; break;
case FSSP_DATAID_T1 : case FSSP_DATAID_T1 :
{ {