Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-26 09:45:33 +03:00
Code Issues Wiki Activity

Merge pull request #2624 from iNavFlight/de_update_calibration_api

Browse source 
Update calibration API
This commit is contained in:
Konstantin Sharlaimov 2017-12-31 23:32:43 +10:00 • committed by GitHub
commit a198568ac2
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
6 changed files with 22 additions and 12 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1043,6 +1043,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
case MSP_CALIBRATION_DATA:
#ifdef USE_ACC
sbufWriteU8(dst, accGetCalibrationAxisFlags());
sbufWriteU16(dst, accelerometerConfig()->accZero.raw[X]);
sbufWriteU16(dst, accelerometerConfig()->accZero.raw[Y]);
sbufWriteU16(dst, accelerometerConfig()->accZero.raw[Z]);
@ -1050,6 +1051,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
sbufWriteU16(dst, accelerometerConfig()->accGain.raw[Y]);
sbufWriteU16(dst, accelerometerConfig()->accGain.raw[Z]);
#else
sbufWriteU8(dst, 0);
sbufWriteU16(dst, 0);
sbufWriteU16(dst, 0);
sbufWriteU16(dst, 0);

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

@ -349,7 +349,7 @@ uint8_t accGetCalibrationAxisFlags(void)
{
uint8_t flags = 0;
for (int i = 0; i < 6; i++) {
if (accGetCalibrationAxisStatus(0)) {
if (accGetCalibrationAxisStatus(i)) {
flags |= (1 << i);
}
}
@ -359,6 +359,9 @@ uint8_t accGetCalibrationAxisFlags(void)
int getPrimaryAxisIndex(int32_t sample[3])
{
// Apply sensor alignment (for axis detection only)
applySensorAlignment(sample, acc.dev.accAlign);
// Tolerate up to atan(1 / 1.5) = 33 deg tilt (in worst case 66 deg separation between points)
if ((ABS(sample[Z]) / 1.5f) > ABS(sample[X]) && (ABS(sample[Z]) / 1.5f) > ABS(sample[Y])) {
//Z-axis
@ -366,11 +369,11 @@ int getPrimaryAxisIndex(int32_t sample[3])
}
else if ((ABS(sample[X]) / 1.5f) > ABS(sample[Y]) && (ABS(sample[X]) / 1.5f) > ABS(sample[Z])) {
//X-axis
return (sample[X] > 0) ? 2 : 3;
return (sample[X] > 0) ? 3 : 5;
}
else if ((ABS(sample[Y]) / 1.5f) > ABS(sample[X]) && (ABS(sample[Y]) / 1.5f) > ABS(sample[Z])) {
//Y-axis
return (sample[Y] > 0) ? 4 : 5;
return (sample[Y] > 0) ? 2 : 4;
}
else
return -1;
@ -508,7 +511,8 @@ void accUpdate(void)
}
applyAccelerationZero(&accelerometerConfig()->accZero, &accelerometerConfig()->accGain);
alignSensors(accADC, acc.dev.accAlign);
applySensorAlignment(accADC, acc.dev.accAlign);
applyBoardAlignment(accADC);
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
acc.accADCf[axis] = (float)accADC[axis] / acc.dev.acc_1G;

11
src/main/sensors/boardalignment.c
View file

@ -69,8 +69,12 @@ void updateBoardAlignment(int16_t roll, int16_t pitch)
initBoardAlignment();
}
static void alignBoard(int32_t *vec)
void applyBoardAlignment(int32_t *vec)
{
if (standardBoardAlignment) {
return;
}
int32_t x = vec[X];
int32_t y = vec[Y];
int32_t z = vec[Z];
@ -80,7 +84,7 @@ static void alignBoard(int32_t *vec)
vec[Z] = lrintf(boardRotation[0][Z] * x + boardRotation[1][Z] * y + boardRotation[2][Z] * z);
}
void alignSensors(int32_t *dest, uint8_t rotation)
void applySensorAlignment(int32_t *dest, uint8_t rotation)
{
const int32_t x = dest[X];
const int32_t y = dest[Y];
@ -129,7 +133,4 @@ void alignSensors(int32_t *dest, uint8_t rotation)
dest[Z] = -z;
break;
}
if (!standardBoardAlignment)
alignBoard(dest);
}

3
src/main/sensors/boardalignment.h
View file

@ -27,6 +27,7 @@ typedef struct boardAlignment_s {
PG_DECLARE(boardAlignment_t, boardAlignment);
void alignSensors(int32_t *dest, uint8_t rotation);
void initBoardAlignment(void);
void updateBoardAlignment(int16_t roll, int16_t pitch);
void applySensorAlignment(int32_t *dest, uint8_t rotation);
void applyBoardAlignment(int32_t *vec);

3
src/main/sensors/compass.c
View file

@ -366,7 +366,8 @@ void compassUpdate(timeUs_t currentTimeUs)
}
}
alignSensors(mag.magADC, mag.dev.magAlign);
applySensorAlignment(mag.magADC, mag.dev.magAlign);
applyBoardAlignment(mag.magADC);
magUpdatedAtLeastOnce = 1;
}

3
src/main/sensors/gyro.c
View file

@ -399,7 +399,8 @@ void gyroUpdate(timeDelta_t gyroUpdateDeltaUs)
gyroADC[X] = (int32_t)gyroDev0.gyroADCRaw[X] - (int32_t)gyroDev0.gyroZero[X];
gyroADC[Y] = (int32_t)gyroDev0.gyroADCRaw[Y] - (int32_t)gyroDev0.gyroZero[Y];
gyroADC[Z] = (int32_t)gyroDev0.gyroADCRaw[Z] - (int32_t)gyroDev0.gyroZero[Z];
alignSensors(gyroADC, gyroDev0.gyroAlign);
applySensorAlignment(gyroADC, gyroDev0.gyroAlign);
applyBoardAlignment(gyroADC);
} else {
performGyroCalibration(&gyroDev0, &gyroCalibration, gyroConfig()->gyroMovementCalibrationThreshold);
// Reset gyro values to zero to prevent other code from using uncalibrated data