Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-23 16:25:26 +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_D[i] = axisPID_D[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
blackboxCurrent->magADC[i] = mag.magADC[i];
#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
const uint8_t scale = (acc.dev.acc_1G > 1024) ? 8 : 1;
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++) {
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)
{
int32_t axis;
int32_t accMagnitudeSq = 0;
float accMagnitudeSq = 0;
for (axis = 0; axis < 3; axis++) {
accMagnitudeSq += (int32_t)acc.accADC[axis] * acc.accADC[axis];
for (int axis = 0; axis < 3; axis++) {
accMagnitudeSq += acc.accADCf[axis] * acc.accADCf[axis];
}
// 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;
}
@ -568,7 +567,7 @@ static void imuUpdateMeasuredAcceleration(void)
#else
/* Convert acceleration to cm/s/s */
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
}
@ -585,9 +584,9 @@ void imuHILUpdate(void)
imuComputeQuaternionFromRPY(attitude.values.roll, attitude.values.pitch, attitude.values.yaw);
/* Fake accADC readings */
accADC[X] = hilToFC.bodyAccel[X] * (acc.acc_1G / GRAVITY_CMSS);
accADC[Y] = hilToFC.bodyAccel[Y] * (acc.acc_1G / GRAVITY_CMSS);
accADC[Z] = hilToFC.bodyAccel[Z] * (acc.acc_1G / GRAVITY_CMSS);
accADCf[X] = hilToFC.bodyAccel[X] / GRAVITY_CMSS;
accADCf[Y] = hilToFC.bodyAccel[Y] / GRAVITY_CMSS;
accADCf[Z] = hilToFC.bodyAccel[Z] / GRAVITY_CMSS;
}
#endif
@ -607,7 +606,7 @@ void imuUpdateAccelerometer(void)
#ifdef ASYNC_GYRO_PROCESSING
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++;
#endif
@ -637,9 +636,9 @@ void imuUpdateAttitude(timeUs_t currentTimeUs)
imuCalculateEstimatedAttitude(dT); // Update attitude estimate
#endif
} else {
acc.accADC[X] = 0;
acc.accADC[Y] = 0;
acc.accADC[Z] = 0;
acc.accADCf[X] = 0.0f;
acc.accADCf[Y] = 0.0f;
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_FASTRAM int32_t accADC[XYZ_AXIS_COUNT];
STATIC_FASTRAM biquadFilter_t accFilter[XYZ_AXIS_COUNT];
#ifdef USE_ACC_NOTCH
@ -376,7 +378,7 @@ int getPrimaryAxisIndex(int32_t sample[3])
static void performAcclerationCalibration(void)
{
int axisIndex = getPrimaryAxisIndex(acc.accADC);
int axisIndex = getPrimaryAxisIndex(accADC);
// Check if sample is usable
if (axisIndex < 0) {
@ -398,10 +400,10 @@ static void performAcclerationCalibration(void)
}
if (!calibratedAxis[axisIndex]) {
sensorCalibrationPushSampleForOffsetCalculation(&calState, acc.accADC);
accSamples[axisIndex][X] += acc.accADC[X];
accSamples[axisIndex][Y] += acc.accADC[Y];
accSamples[axisIndex][Z] += acc.accADC[Z];
sensorCalibrationPushSampleForOffsetCalculation(&calState, accADC);
accSamples[axisIndex][X] += accADC[X];
accSamples[axisIndex][Y] += accADC[Y];
accSamples[axisIndex][Z] += accADC[Z];
if (isOnFinalAccelerationCalibrationCycle()) {
calibratedAxis[axisIndex] = true;
@ -447,9 +449,9 @@ static void performAcclerationCalibration(void)
static void applyAccelerationZero(const flightDynamicsTrims_t * accZero, const flightDynamicsTrims_t * accGain)
{
acc.accADC[X] = (acc.accADC[X] - accZero->raw[X]) * accGain->raw[X] / 4096;
acc.accADC[Y] = (acc.accADC[Y] - accZero->raw[Y]) * accGain->raw[Y] / 4096;
acc.accADC[Z] = (acc.accADC[Z] - accZero->raw[Z]) * accGain->raw[Z] / 4096;
accADC[X] = (accADC[X] - accZero->raw[X]) * accGain->raw[X] / 4096;
accADC[Y] = (accADC[Y] - accZero->raw[Y]) * accGain->raw[Y] / 4096;
accADC[Z] = (accADC[Z] - accZero->raw[Z]) * accGain->raw[Z] / 4096;
}
void accUpdate(void)
@ -459,30 +461,31 @@ void accUpdate(void)
}
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) {
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
if (accelerometerConfig()->acc_notch_hz) {
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
if (!accIsCalibrationComplete()) {
performAcclerationCalibration();
}
applyAccelerationZero(&accelerometerConfig()->accZero, &accelerometerConfig()->accGain);
alignSensors(acc.accADC, acc.dev.accAlign);
}
void accSetCalibrationValues(void)

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

@ -41,7 +41,7 @@ typedef enum {
typedef struct acc_s {
accDev_t dev;
uint32_t accTargetLooptime;
int32_t accADC[XYZ_AXIS_COUNT];
float accADCf[XYZ_AXIS_COUNT]; // acceleration in g
} acc_t;
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++) {
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
break;
case FSSP_DATAID_ACCX :
smartPortSendPackage(id, 100 * acc.accADC[X] / acc.dev.acc_1G);
smartPortSendPackage(id, lrintf(100 * acc.accADCf[X]));
break;
case FSSP_DATAID_ACCY :
smartPortSendPackage(id, 100 * acc.accADC[Y] / acc.dev.acc_1G);
smartPortSendPackage(id, lrintf(100 * acc.accADCf[Y]));
break;
case FSSP_DATAID_ACCZ :
smartPortSendPackage(id, 100 * acc.accADC[Z] / acc.dev.acc_1G);
smartPortSendPackage(id, lrintf(100 * acc.accADCf[Z]));
break;
case FSSP_DATAID_T1 :
{