diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 7b0082bb3..5ea120f83 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -35,11 +35,13 @@ #include "sensors/acceleration.h" -int16_t accADC[XYZ_AXIS_COUNT]; +int32_t accADC[XYZ_AXIS_COUNT]; +int16_t accADCRaw[XYZ_AXIS_COUNT]; acc_t acc; // acc access functions sensor_align_e accAlign = 0; uint16_t acc_1G = 256; // this is the 1G measured acceleration. +int axis; 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. @@ -80,7 +82,6 @@ void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims) void performAcclerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims) { static int32_t a[3]; - uint8_t axis; for (axis = 0; axis < 3; axis++) { @@ -172,10 +173,12 @@ void applyAccelerationTrims(flightDynamicsTrims_t *accelerationTrims) void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims) { - if (!acc.read(accADC)) { + if (!acc.read(accADCRaw)) { return; } + for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) accADC[axis] = accADCRaw[axis]; + alignSensors(accADC, accADC, accAlign); if (!isAccelerationCalibrationComplete()) { diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index 5a66f581c..b9644eccb 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -37,7 +37,7 @@ extern sensor_align_e accAlign; extern acc_t acc; extern uint16_t acc_1G; -extern int16_t accADC[XYZ_AXIS_COUNT]; +extern int32_t accADC[XYZ_AXIS_COUNT]; typedef struct rollAndPitchTrims_s { int16_t roll; diff --git a/src/main/sensors/boardalignment.c b/src/main/sensors/boardalignment.c index 649d0d7b9..d7a6784c8 100644 --- a/src/main/sensors/boardalignment.c +++ b/src/main/sensors/boardalignment.c @@ -51,20 +51,20 @@ void initBoardAlignment(boardAlignment_t *boardAlignment) buildRotationMatrix(&rotationAngles, boardRotation); } -static void alignBoard(int16_t *vec) +static void alignBoard(int32_t *vec) { - int16_t x = vec[X]; - int16_t y = vec[Y]; - int16_t z = vec[Z]; + int32_t x = vec[X]; + int32_t y = vec[Y]; + int32_t z = vec[Z]; vec[X] = lrintf(boardRotation[0][X] * x + boardRotation[1][X] * y + boardRotation[2][X] * z); vec[Y] = lrintf(boardRotation[0][Y] * x + boardRotation[1][Y] * y + boardRotation[2][Y] * z); vec[Z] = lrintf(boardRotation[0][Z] * x + boardRotation[1][Z] * y + boardRotation[2][Z] * z); } -void alignSensors(int16_t *src, int16_t *dest, uint8_t rotation) +void alignSensors(int32_t *src, int32_t *dest, uint8_t rotation) { - static uint16_t swap[3]; + static uint32_t swap[3]; memcpy(swap, src, sizeof(swap)); switch (rotation) { diff --git a/src/main/sensors/boardalignment.h b/src/main/sensors/boardalignment.h index 2284d90cc..eea1f23c8 100644 --- a/src/main/sensors/boardalignment.h +++ b/src/main/sensors/boardalignment.h @@ -18,10 +18,10 @@ #pragma once typedef struct boardAlignment_s { - int16_t rollDegrees; - int16_t pitchDegrees; - int16_t yawDegrees; + int32_t rollDegrees; + int32_t pitchDegrees; + int32_t yawDegrees; } boardAlignment_t; -void alignSensors(int16_t *src, int16_t *dest, uint8_t rotation); +void alignSensors(int32_t *src, int32_t *dest, uint8_t rotation); void initBoardAlignment(boardAlignment_t *boardAlignment); diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 31d7688c6..091dd26ed 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -43,7 +43,8 @@ mag_t mag; // mag access functions extern uint32_t currentTime; // FIXME dependency on global variable, pass it in instead. -int16_t magADC[XYZ_AXIS_COUNT]; +int16_t magADCRaw[XYZ_AXIS_COUNT]; +int32_t magADC[XYZ_AXIS_COUNT]; sensor_align_e magAlign = 0; #ifdef MAG static uint8_t magInit = 0; @@ -71,7 +72,8 @@ void updateCompass(flightDynamicsTrims_t *magZero) nextUpdateAt = currentTime + COMPASS_UPDATE_FREQUENCY_10HZ; - mag.read(magADC); + mag.read(magADCRaw); + for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) magADC[axis] = magADCRaw[axis]; alignSensors(magADC, magADC, magAlign); if (STATE(CALIBRATE_MAG)) { diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h index 45af108d4..0807ba410 100644 --- a/src/main/sensors/compass.h +++ b/src/main/sensors/compass.h @@ -33,7 +33,7 @@ void compassInit(void); void updateCompass(flightDynamicsTrims_t *magZero); #endif -extern int16_t magADC[XYZ_AXIS_COUNT]; +extern int32_t magADC[XYZ_AXIS_COUNT]; extern sensor_align_e magAlign; extern mag_t mag; diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 6f1ac06e9..f4062efcc 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -37,8 +37,9 @@ #include "sensors/gyro.h" uint16_t calibratingG = 0; -int16_t gyroADC[XYZ_AXIS_COUNT]; -int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 }; +int16_t gyroADCRaw[XYZ_AXIS_COUNT]; +int32_t gyroADC[XYZ_AXIS_COUNT]; +int32_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 }; static gyroConfig_t *gyroConfig; static biquad_t gyroFilterState[3]; @@ -133,10 +134,12 @@ static void applyGyroZero(void) void gyroUpdate(void) { // range: +/- 8192; +/- 2000 deg/sec - if (!gyro.read(gyroADC)) { + if (!gyro.read(gyroADCRaw)) { return; } + for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) gyroADC[axis] = gyroADCRaw[axis]; + alignSensors(gyroADC, gyroADC, gyroAlign); if (gyroLpfCutFreq) { diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 8131089e5..bb7a152f0 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -32,8 +32,8 @@ typedef enum { extern gyro_t gyro; extern sensor_align_e gyroAlign; -extern int16_t gyroADC[XYZ_AXIS_COUNT]; -extern int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT]; +extern int32_t gyroADC[XYZ_AXIS_COUNT]; +extern int32_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT]; typedef struct gyroConfig_s { uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.