diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 7b0082bb3..57026446d 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -35,7 +35,8 @@ #include "sensors/acceleration.h" -int16_t accADC[XYZ_AXIS_COUNT]; +int16_t accADCRaw[XYZ_AXIS_COUNT]; +int32_t accADC[XYZ_AXIS_COUNT]; acc_t acc; // acc access functions sensor_align_e accAlign = 0; @@ -172,10 +173,14 @@ void applyAccelerationTrims(flightDynamicsTrims_t *accelerationTrims) void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims) { - if (!acc.read(accADC)) { + int axis; + + 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 b2bba274a..c29b99fd2 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -38,7 +38,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..3756e8e5e 100644 --- a/src/main/sensors/boardalignment.h +++ b/src/main/sensors/boardalignment.h @@ -23,5 +23,5 @@ typedef struct boardAlignment_s { int16_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..cfe24ab51 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]; // int32_t copy to work with 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 ef1f25443..712e796b5 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -38,8 +38,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 gyroBiQuadState[3]; @@ -135,10 +136,13 @@ static void applyGyroZero(void) void gyroUpdate(void) { // range: +/- 8192; +/- 2000 deg/sec - if (!gyro.read(gyroADC)) { + if (!gyro.read(gyroADCRaw)) { return; } + // Prepare a copy of int32_t gyroADC for mangling to prevent overflow + 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 44a6fc011..62dd2d25d 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -33,8 +33,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. diff --git a/src/test/unit/alignsensor_unittest.cc b/src/test/unit/alignsensor_unittest.cc index 674ed9572..3b1d16022 100644 --- a/src/test/unit/alignsensor_unittest.cc +++ b/src/test/unit/alignsensor_unittest.cc @@ -39,9 +39,9 @@ extern "C" { #define DEG2RAD 0.01745329251 -static void rotateVector(int16_t mat[3][3], int16_t vec[3], int16_t *out) +static void rotateVector(int32_t mat[3][3], int32_t vec[3], int32_t *out) { - int16_t tmp[3]; + int32_t tmp[3]; for(int i=0; i<3; i++) { tmp[i] = 0; @@ -69,7 +69,7 @@ static void rotateVector(int16_t mat[3][3], int16_t vec[3], int16_t *out) // mat[2][2] = cos(angle*DEG2RAD); //} -static void initYAxisRotation(int16_t mat[][3], int16_t angle) +static void initYAxisRotation(int32_t mat[][3], int32_t angle) { mat[0][0] = cos(angle*DEG2RAD); mat[0][1] = 0; @@ -82,7 +82,7 @@ static void initYAxisRotation(int16_t mat[][3], int16_t angle) mat[2][2] = cos(angle*DEG2RAD); } -static void initZAxisRotation(int16_t mat[][3], int16_t angle) +static void initZAxisRotation(int32_t mat[][3], int32_t angle) { mat[0][0] = cos(angle*DEG2RAD); mat[0][1] = -sin(angle*DEG2RAD); @@ -95,18 +95,18 @@ static void initZAxisRotation(int16_t mat[][3], int16_t angle) mat[2][2] = 1; } -static void testCW(sensor_align_e rotation, int16_t angle) +static void testCW(sensor_align_e rotation, int32_t angle) { - int16_t src[XYZ_AXIS_COUNT]; - int16_t dest[XYZ_AXIS_COUNT]; - int16_t test[XYZ_AXIS_COUNT]; + int32_t src[XYZ_AXIS_COUNT]; + int32_t dest[XYZ_AXIS_COUNT]; + int32_t test[XYZ_AXIS_COUNT]; // unit vector along x-axis src[X] = 1; src[Y] = 0; src[Z] = 0; - int16_t matrix[3][3]; + int32_t matrix[3][3]; initZAxisRotation(matrix, angle); rotateVector(matrix, src, test); @@ -153,18 +153,18 @@ static void testCW(sensor_align_e rotation, int16_t angle) * Since the order of flip and rotation matters, these tests make the * assumption that the 'flip' occurs first, followed by clockwise rotation */ -static void testCWFlip(sensor_align_e rotation, int16_t angle) +static void testCWFlip(sensor_align_e rotation, int32_t angle) { - int16_t src[XYZ_AXIS_COUNT]; - int16_t dest[XYZ_AXIS_COUNT]; - int16_t test[XYZ_AXIS_COUNT]; + int32_t src[XYZ_AXIS_COUNT]; + int32_t dest[XYZ_AXIS_COUNT]; + int32_t test[XYZ_AXIS_COUNT]; // unit vector along x-axis src[X] = 1; src[Y] = 0; src[Z] = 0; - int16_t matrix[3][3]; + int32_t matrix[3][3]; initYAxisRotation(matrix, 180); rotateVector(matrix, src, test); initZAxisRotation(matrix, angle); diff --git a/src/test/unit/flight_imu_unittest.cc b/src/test/unit/flight_imu_unittest.cc index c575a74f5..7566cda5f 100644 --- a/src/test/unit/flight_imu_unittest.cc +++ b/src/test/unit/flight_imu_unittest.cc @@ -82,7 +82,7 @@ int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; uint16_t acc_1G; int16_t heading; gyro_t gyro; -int16_t magADC[XYZ_AXIS_COUNT]; +int32_t magADC[XYZ_AXIS_COUNT]; int32_t BaroAlt; int16_t debug[DEBUG16_VALUE_COUNT]; @@ -91,8 +91,8 @@ uint16_t flightModeFlags; uint8_t armingFlags; int32_t sonarAlt; -int16_t accADC[XYZ_AXIS_COUNT]; -int16_t gyroADC[XYZ_AXIS_COUNT]; +int32_t accADC[XYZ_AXIS_COUNT]; +int32_t gyroADC[XYZ_AXIS_COUNT]; uint16_t enableFlightMode(flightModeFlags_e mask)