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])
{
// Apply sensor & board alignment (for axis detection only)
alignSensors(sample, acc.dev.accAlign);
// 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])) {
@ -511,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