rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Introduced cli parameter <gyro_offset_yaw> 

It allows to manually compensate gyro drift over time.
rebased squashed.
This commit is contained in:
Adrian Miriuta 2018-01-25 12:45:00 +01:00
parent b47cabffef
commit b21d681153
11 changed files with 39 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/main/drivers/accgyro/accgyro.h
View File

@ -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;

1
src/main/interface/settings.c
View File

@ -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

4
src/main/io/dashboard.c
View File

@ -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);

2
src/main/sensors/acceleration.h
View File

@ -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;

22
src/main/sensors/boardalignment.c
View File

@ -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:

2
src/main/sensors/boardalignment.h
View File

@ -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);

3
src/main/sensors/compass.h
View File

@ -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);

18
src/main/sensors/gyro.c
View File

@ -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));

1
src/main/sensors/gyro.h
View File

@ -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);

2
src/main/sensors/sensors.h
View File

@ -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

12
src/test/unit/alignsensor_unittest.cc
View File

@ -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;