Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Fix sensors saturation by converting to int32_t #199

Open
wants to merge 1 commit into
base: release
Choose a base branch
from
Open

Fix sensors saturation by converting to int32_t #199

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
9 changes: 7 additions & 2 deletions src/main/sensors/acceleration.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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()) {
Expand Down
2 changes: 1 addition & 1 deletion src/main/sensors/acceleration.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
12 changes: 6 additions & 6 deletions src/main/sensors/boardalignment.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand Down
2 changes: 1 addition & 1 deletion src/main/sensors/boardalignment.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
6 changes: 4 additions & 2 deletions src/main/sensors/compass.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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)) {
Expand Down
2 changes: 1 addition & 1 deletion src/main/sensors/compass.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
10 changes: 7 additions & 3 deletions src/main/sensors/gyro.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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];
Expand Down Expand Up @@ -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) {
Expand Down
4 changes: 2 additions & 2 deletions src/main/sensors/gyro.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down
28 changes: 14 additions & 14 deletions src/test/unit/alignsensor_unittest.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand All @@ -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);
Expand All @@ -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);

Expand Down Expand Up @@ -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);
Expand Down
6 changes: 3 additions & 3 deletions src/test/unit/flight_imu_unittest.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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];

Expand All @@ -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)
Expand Down