From b21d6811533a1a2af38834b60adea8f87154f282 Mon Sep 17 00:00:00 2001 From: Adrian Miriuta Date: Thu, 25 Jan 2018 12:45:00 +0100 Subject: [PATCH] Introduced cli parameter It allows to manually compensate gyro drift over time. rebased squashed. --- src/main/drivers/accgyro/accgyro.h | 4 ++-- src/main/interface/settings.c | 1 + src/main/io/dashboard.c | 4 ++-- src/main/sensors/acceleration.h | 2 +- src/main/sensors/boardalignment.c | 22 +++++++++++----------- src/main/sensors/boardalignment.h | 2 +- src/main/sensors/compass.h | 3 +-- src/main/sensors/gyro.c | 18 ++++++++++++------ src/main/sensors/gyro.h | 1 + src/main/sensors/sensors.h | 2 +- src/test/unit/alignsensor_unittest.cc | 12 ++++++------ 11 files changed, 39 insertions(+), 32 deletions(-) diff --git a/src/main/drivers/accgyro/accgyro.h b/src/main/drivers/accgyro/accgyro.h index 77ddba882..eeb04b697 100644 --- a/src/main/drivers/accgyro/accgyro.h +++ b/src/main/drivers/accgyro/accgyro.h @@ -63,8 +63,8 @@ typedef struct gyroDev_s { extiCallbackRec_t exti; busDevice_t bus; float scale; // scalefactor - int32_t gyroZero[XYZ_AXIS_COUNT]; - int32_t gyroADC[XYZ_AXIS_COUNT]; // gyro data after calibration and alignment + float gyroZero[XYZ_AXIS_COUNT]; + float gyroADC[XYZ_AXIS_COUNT]; // gyro data after calibration and alignment int32_t gyroADCRawPrevious[XYZ_AXIS_COUNT]; int16_t gyroADCRaw[XYZ_AXIS_COUNT]; int16_t temperature; diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index dcf949a87..6996fc172 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -368,6 +368,7 @@ const clivalue_t valueTable[] = { { "gyro_filter_p", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_filter_p) }, #endif { "moron_threshold", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 200 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyroMovementCalibrationThreshold) }, + { "gyro_offset_yaw", VAR_INT16 | MASTER_VALUE, .config.minmax = { -1000, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_offset_yaw) }, #ifdef USE_GYRO_OVERFLOW_CHECK { "gyro_overflow_detect", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO_OVERFLOW_CHECK }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, checkOverflow) }, #endif diff --git a/src/main/io/dashboard.c b/src/main/io/dashboard.c index 15601d995..07572023d 100644 --- a/src/main/io/dashboard.c +++ b/src/main/io/dashboard.c @@ -486,7 +486,7 @@ static void showSensorsPage(void) i2c_OLED_send_string(bus, " X Y Z"); if (sensors(SENSOR_ACC)) { - tfp_sprintf(lineBuffer, format, "ACC", acc.accADC[X], acc.accADC[Y], acc.accADC[Z]); + tfp_sprintf(lineBuffer, format, "ACC", lrintf(acc.accADC[X]), lrintf(acc.accADC[Y]), lrintf(acc.accADC[Z])); padLineBuffer(); i2c_OLED_set_line(bus, rowIndex++); i2c_OLED_send_string(bus, lineBuffer); @@ -501,7 +501,7 @@ static void showSensorsPage(void) #ifdef USE_MAG if (sensors(SENSOR_MAG)) { - tfp_sprintf(lineBuffer, format, "MAG", mag.magADC[X], mag.magADC[Y], mag.magADC[Z]); + tfp_sprintf(lineBuffer, format, "MAG", lrintf(mag.magADC[X]), lrintf(mag.magADC[Y]), lrintf(mag.magADC[Z])); padLineBuffer(); i2c_OLED_set_line(bus, rowIndex++); i2c_OLED_send_string(bus, lineBuffer); diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index 604542f0f..7722d2fea 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -46,7 +46,7 @@ typedef enum { typedef struct acc_s { accDev_t dev; uint32_t accSamplingInterval; - int32_t accADC[XYZ_AXIS_COUNT]; + float accADC[XYZ_AXIS_COUNT]; bool isAccelUpdatedAtLeastOnce; } acc_t; diff --git a/src/main/sensors/boardalignment.c b/src/main/sensors/boardalignment.c index 5377e8cff..540bda9da 100644 --- a/src/main/sensors/boardalignment.c +++ b/src/main/sensors/boardalignment.c @@ -59,22 +59,22 @@ void initBoardAlignment(const boardAlignment_t *boardAlignment) buildRotationMatrix(&rotationAngles, boardRotation); } -static void alignBoard(int32_t *vec) +static void alignBoard(float *vec) { - int32_t x = vec[X]; - int32_t y = vec[Y]; - int32_t z = vec[Z]; + float x = vec[X]; + float y = vec[Y]; + float 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); + vec[X] = (boardRotation[0][X] * x + boardRotation[1][X] * y + boardRotation[2][X] * z); + vec[Y] = (boardRotation[0][Y] * x + boardRotation[1][Y] * y + boardRotation[2][Y] * z); + vec[Z] = (boardRotation[0][Z] * x + boardRotation[1][Z] * y + boardRotation[2][Z] * z); } -FAST_CODE void alignSensors(int32_t *dest, uint8_t rotation) +FAST_CODE void alignSensors(float *dest, uint8_t rotation) { - const int32_t x = dest[X]; - const int32_t y = dest[Y]; - const int32_t z = dest[Z]; + const float x = dest[X]; + const float y = dest[Y]; + const float z = dest[Z]; switch (rotation) { default: diff --git a/src/main/sensors/boardalignment.h b/src/main/sensors/boardalignment.h index 6ac188ea8..9c1736f82 100644 --- a/src/main/sensors/boardalignment.h +++ b/src/main/sensors/boardalignment.h @@ -27,5 +27,5 @@ typedef struct boardAlignment_s { PG_DECLARE(boardAlignment_t, boardAlignment); -void alignSensors(int32_t *dest, uint8_t rotation); +void alignSensors(float *dest, uint8_t rotation); void initBoardAlignment(const boardAlignment_t *boardAlignment); diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h index a54db5a63..ec1474430 100644 --- a/src/main/sensors/compass.h +++ b/src/main/sensors/compass.h @@ -34,7 +34,7 @@ typedef enum { } magSensor_e; typedef struct mag_s { - int32_t magADC[XYZ_AXIS_COUNT]; + float magADC[XYZ_AXIS_COUNT]; float magneticDeclination; } mag_t; @@ -59,4 +59,3 @@ PG_DECLARE(compassConfig_t, compassConfig); bool compassIsHealthy(void); void compassUpdate(timeUs_t currentTime); bool compassInit(void); - diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 2bf7646fc..58035079a 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -173,6 +173,7 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_filter_q = 0, .gyro_filter_r = 0, .gyro_filter_p = 0, + .gyro_offset_yaw = 0, ); @@ -681,7 +682,12 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroSensor_t *gyroSensor, uint8_t gyroSetCalibrationCycles(gyroSensor); return; } - gyroSensor->gyroDev.gyroZero[axis] = (gyroSensor->calibration.sum[axis] + (gyroCalculateCalibratingCycles() / 2)) / gyroCalculateCalibratingCycles(); + + // please take care with exotic boardalignment !! + gyroSensor->gyroDev.gyroZero[axis] = gyroSensor->calibration.sum[axis] / gyroCalculateCalibratingCycles(); + if (axis == Z) { + gyroSensor->gyroDev.gyroZero[axis] -= ((float)gyroConfig()->gyro_offset_yaw / 100); + } } } @@ -721,9 +727,9 @@ static void checkForOverflow(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs) // This can cause an overflow and sign reversal in the output. // Overflow and sign reversal seems to result in a gyro value of +1996 or -1996. if (gyroSensor->overflowDetected) { - const float gyroRateX = (float)gyroSensor->gyroDev.gyroADC[X] * gyroSensor->gyroDev.scale; - const float gyroRateY = (float)gyroSensor->gyroDev.gyroADC[Y] * gyroSensor->gyroDev.scale; - const float gyroRateZ = (float)gyroSensor->gyroDev.gyroADC[Z] * gyroSensor->gyroDev.scale; + const float gyroRateX = gyroSensor->gyroDev.gyroADC[X] * gyroSensor->gyroDev.scale; + const float gyroRateY = gyroSensor->gyroDev.gyroADC[Y] * gyroSensor->gyroDev.scale; + const float gyroRateZ = gyroSensor->gyroDev.gyroADC[Z] * gyroSensor->gyroDev.scale; static const int overflowResetThreshold = 1800; if (abs(gyroRateX) < overflowResetThreshold && abs(gyroRateY) < overflowResetThreshold @@ -798,7 +804,7 @@ static FAST_CODE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t curren if (gyroDebugMode == DEBUG_NONE) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { // NOTE: this branch optimized for when there is no gyro debugging, ensure it is kept in step with non-optimized branch - float gyroADCf = (float)gyroSensor->gyroDev.gyroADC[axis] * gyroSensor->gyroDev.scale; + float gyroADCf = gyroSensor->gyroDev.gyroADC[axis] * gyroSensor->gyroDev.scale; #if defined(USE_GYRO_FAST_KALMAN) gyroADCf = gyroSensor->fastKalmanApplyFn((filter_t *)&gyroSensor->fastKalman[axis], gyroADCf); #elif defined(USE_GYRO_BIQUAD_RC_FIR2) @@ -821,7 +827,7 @@ static FAST_CODE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t curren for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { DEBUG_SET(DEBUG_GYRO_RAW, axis, gyroSensor->gyroDev.gyroADCRaw[axis]); // scale gyro output to degrees per second - float gyroADCf = (float)gyroSensor->gyroDev.gyroADC[axis] * gyroSensor->gyroDev.scale; + float gyroADCf = gyroSensor->gyroDev.gyroADC[axis] * gyroSensor->gyroDev.scale; // DEBUG_GYRO_NOTCH records the unfiltered gyro output DEBUG_SET(DEBUG_GYRO_NOTCH, axis, lrintf(gyroADCf)); diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index df8dea6ae..4c48892bc 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -73,6 +73,7 @@ typedef struct gyroConfig_s { uint16_t gyro_filter_q; uint16_t gyro_filter_r; uint16_t gyro_filter_p; + int16_t gyro_offset_yaw; } gyroConfig_t; PG_DECLARE(gyroConfig_t, gyroConfig); diff --git a/src/main/sensors/sensors.h b/src/main/sensors/sensors.h index 6ba0a11df..556ef9d4e 100644 --- a/src/main/sensors/sensors.h +++ b/src/main/sensors/sensors.h @@ -40,7 +40,7 @@ typedef union flightDynamicsTrims_u { flightDynamicsTrims_def_t values; } flightDynamicsTrims_t; -#define CALIBRATING_GYRO_CYCLES 1000 +#define CALIBRATING_GYRO_CYCLES 10000 #define CALIBRATING_ACC_CYCLES 400 #define CALIBRATING_BARO_CYCLES 200 // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles diff --git a/src/test/unit/alignsensor_unittest.cc b/src/test/unit/alignsensor_unittest.cc index 4037c760d..62c2365dd 100644 --- a/src/test/unit/alignsensor_unittest.cc +++ b/src/test/unit/alignsensor_unittest.cc @@ -40,9 +40,9 @@ extern "C" { #define DEG2RAD 0.01745329251 -static void rotateVector(int32_t mat[3][3], int32_t vec[3], int32_t *out) +static void rotateVector(int32_t mat[3][3], float vec[3], float *out) { - int32_t tmp[3]; + float tmp[3]; for(int i=0; i<3; i++) { tmp[i] = 0; @@ -98,8 +98,8 @@ static void initZAxisRotation(int32_t mat[][3], int32_t angle) static void testCW(sensor_align_e rotation, int32_t angle) { - int32_t src[XYZ_AXIS_COUNT]; - int32_t test[XYZ_AXIS_COUNT]; + float src[XYZ_AXIS_COUNT]; + float test[XYZ_AXIS_COUNT]; // unit vector along x-axis src[X] = 1; @@ -155,8 +155,8 @@ static void testCW(sensor_align_e rotation, int32_t angle) */ static void testCWFlip(sensor_align_e rotation, int32_t angle) { - int32_t src[XYZ_AXIS_COUNT]; - int32_t test[XYZ_AXIS_COUNT]; + float src[XYZ_AXIS_COUNT]; + float test[XYZ_AXIS_COUNT]; // unit vector along x-axis src[X] = 1;