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

Refactor board alignment

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2017-12-31 17:55:03 +10:00
parent 1c68e9b720
commit c7e06c453a
5 changed files with 16 additions and 11 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -359,8 +359,8 @@ uint8_t accGetCalibrationAxisFlags(void)
int getPrimaryAxisIndex(int32_t sample[3]) int getPrimaryAxisIndex(int32_t sample[3])
{ {
// Apply sensor & board alignment (for axis detection only) // Apply sensor alignment (for axis detection only)
alignSensors(sample, acc.dev.accAlign); applySensorAlignment(sample, acc.dev.accAlign);
// Tolerate up to atan(1 / 1.5) = 33 deg tilt (in worst case 66 deg separation between points) // 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])) { if ((ABS(sample[Z]) / 1.5f) > ABS(sample[X]) && (ABS(sample[Z]) / 1.5f) > ABS(sample[Y])) {
@ -511,7 +511,8 @@ void accUpdate(void)
} }
applyAccelerationZero(&accelerometerConfig()->accZero, &accelerometerConfig()->accGain); 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++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
acc.accADCf[axis] = (float)accADC[axis] / acc.dev.acc_1G; 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(); initBoardAlignment();
} }
static void alignBoard(int32_t *vec) void applyBoardAlignment(int32_t *vec)
{ {
if (standardBoardAlignment) {
return;
}
int32_t x = vec[X]; int32_t x = vec[X];
int32_t y = vec[Y]; int32_t y = vec[Y];
int32_t z = vec[Z]; 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); 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 x = dest[X];
const int32_t y = dest[Y]; const int32_t y = dest[Y];
@ -129,7 +133,4 @@ void alignSensors(int32_t *dest, uint8_t rotation)
dest[Z] = -z; dest[Z] = -z;
break; 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); PG_DECLARE(boardAlignment_t, boardAlignment);
void alignSensors(int32_t *dest, uint8_t rotation);
void initBoardAlignment(void); void initBoardAlignment(void);
void updateBoardAlignment(int16_t roll, int16_t pitch); 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; 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[X] = (int32_t)gyroDev0.gyroADCRaw[X] - (int32_t)gyroDev0.gyroZero[X];
gyroADC[Y] = (int32_t)gyroDev0.gyroADCRaw[Y] - (int32_t)gyroDev0.gyroZero[Y]; gyroADC[Y] = (int32_t)gyroDev0.gyroADCRaw[Y] - (int32_t)gyroDev0.gyroZero[Y];
gyroADC[Z] = (int32_t)gyroDev0.gyroADCRaw[Z] - (int32_t)gyroDev0.gyroZero[Z]; gyroADC[Z] = (int32_t)gyroDev0.gyroADCRaw[Z] - (int32_t)gyroDev0.gyroZero[Z];
alignSensors(gyroADC, gyroDev0.gyroAlign); applySensorAlignment(gyroADC, gyroDev0.gyroAlign);
applyBoardAlignment(gyroADC);
} else { } else {
performGyroCalibration(&gyroDev0, &gyroCalibration, gyroConfig()->gyroMovementCalibrationThreshold); performGyroCalibration(&gyroDev0, &gyroCalibration, gyroConfig()->gyroMovementCalibrationThreshold);
// Reset gyro values to zero to prevent other code from using uncalibrated data // Reset gyro values to zero to prevent other code from using uncalibrated data