Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 22:35:23 +03:00
Code Issues Wiki Activity

Yaw rate change limiter solution for YTTM (#4058)

Browse source
This commit is contained in:
gke 2017-09-07 23:07:07 +10:00 • committed by Martin Budden
parent b295a159b0
commit 13308883b2
2 changed files with 42 additions and 4 deletions
Download patch file Download diff file Expand all files Collapse all files

1
src/main/drivers/accgyro/accgyro.h
View file

@ -60,6 +60,7 @@ typedef struct gyroDev_s {
int32_t gyroZero[XYZ_AXIS_COUNT]; int32_t gyroZero[XYZ_AXIS_COUNT];
int32_t gyroADC[XYZ_AXIS_COUNT]; // gyro data after calibration and alignment int32_t gyroADC[XYZ_AXIS_COUNT]; // gyro data after calibration and alignment
int16_t gyroADCRaw[XYZ_AXIS_COUNT]; int16_t gyroADCRaw[XYZ_AXIS_COUNT];
int32_t gyroADCRawPrevious[XYZ_AXIS_COUNT];
int16_t temperature; int16_t temperature;
uint8_t lpf; uint8_t lpf;
gyroRateKHz_e gyroRateKHz; gyroRateKHz_e gyroRateKHz;

45
src/main/sensors/gyro.c
View file

@ -19,6 +19,7 @@
#include <stdint.h> #include <stdint.h>
#include <string.h> #include <string.h>
#include <math.h> #include <math.h>
#include <stdlib.h>
#include "platform.h" #include "platform.h"
@ -69,6 +70,10 @@
#include "hardware_revision.h" #include "hardware_revision.h"
#endif #endif
#if ((FLASH_SIZE > 128) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20689)))
#define USE_GYRO_SLEW_LIMITER
#endif
gyro_t gyro; gyro_t gyro;
static uint8_t gyroDebugMode; static uint8_t gyroDebugMode;
@ -365,6 +370,7 @@ static bool gyroInitSensor(gyroSensor_t *gyroSensor)
if (gyroConfig()->gyro_align != ALIGN_DEFAULT) { if (gyroConfig()->gyro_align != ALIGN_DEFAULT) {
gyroSensor->gyroDev.gyroAlign = gyroConfig()->gyro_align; gyroSensor->gyroDev.gyroAlign = gyroConfig()->gyro_align;
} }
gyroInitSensorFilters(gyroSensor); gyroInitSensorFilters(gyroSensor);
#ifdef USE_GYRO_DATA_ANALYSE #ifdef USE_GYRO_DATA_ANALYSE
gyroDataAnalyseInit(gyro.targetLooptime); gyroDataAnalyseInit(gyro.targetLooptime);
@ -437,6 +443,14 @@ static uint16_t calculateNyquistAdjustedNotchHz(uint16_t notchHz, uint16_t notch
return notchHz; return notchHz;
} }
#if defined(USE_GYRO_SLEW_LIMITER)
void gyroInitSlewLimiter(gyroSensor_t *gyroSensor) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++)
gyroSensor->gyroDev.gyroADCRawPrevious[axis] = 0;
}
#endif
static void gyroInitFilterNotch1(gyroSensor_t *gyroSensor, uint16_t notchHz, uint16_t notchCutoffHz) static void gyroInitFilterNotch1(gyroSensor_t *gyroSensor, uint16_t notchHz, uint16_t notchCutoffHz)
{ {
gyroSensor->notchFilter1ApplyFn = nullFilterApply; gyroSensor->notchFilter1ApplyFn = nullFilterApply;
@ -484,6 +498,9 @@ static void gyroInitFilterDynamicNotch(gyroSensor_t *gyroSensor)
static void gyroInitSensorFilters(gyroSensor_t *gyroSensor) static void gyroInitSensorFilters(gyroSensor_t *gyroSensor)
{ {
#if defined(USE_GYRO_SLEW_LIMITER)
gyroInitSlewLimiter(gyroSensor);
#endif
gyroInitFilterLpf(gyroSensor, gyroConfig()->gyro_soft_lpf_hz); gyroInitFilterLpf(gyroSensor, gyroConfig()->gyro_soft_lpf_hz);
gyroInitFilterNotch1(gyroSensor, gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1); gyroInitFilterNotch1(gyroSensor, gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1);
gyroInitFilterNotch2(gyroSensor, gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2); gyroInitFilterNotch2(gyroSensor, gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2);
@ -582,6 +599,21 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroSensor_t *gyroSensor, uint8_t
} }
#if defined(USE_GYRO_SLEW_LIMITER)
int32_t gyroSlewLimiter(int axis, gyroSensor_t *gyroSensor)
{
int32_t newRawGyro = (int32_t) gyroSensor->gyroDev.gyroADCRaw[axis];
if (abs(newRawGyro - gyroSensor->gyroDev.gyroADCRawPrevious[axis]) > (1<<14))
newRawGyro = gyroSensor->gyroDev.gyroADCRawPrevious[axis];
else
gyroSensor->gyroDev.gyroADCRawPrevious[axis] = newRawGyro;
return newRawGyro;
}
#endif
void gyroUpdateSensor(gyroSensor_t *gyroSensor) void gyroUpdateSensor(gyroSensor_t *gyroSensor)
{ {
if (!gyroSensor->gyroDev.readFn(&gyroSensor->gyroDev)) { if (!gyroSensor->gyroDev.readFn(&gyroSensor->gyroDev)) {
@ -590,10 +622,15 @@ void gyroUpdateSensor(gyroSensor_t *gyroSensor)
gyroSensor->gyroDev.dataReady = false; gyroSensor->gyroDev.dataReady = false;
if (isGyroSensorCalibrationComplete(gyroSensor)) { if (isGyroSensorCalibrationComplete(gyroSensor)) {
// move gyro data into 32-bit variables to avoid overflows in calculations // move 16-bit gyro data into 32-bit variables to avoid overflows in calculations
gyroSensor->gyroDev.gyroADC[X] = (int32_t)gyroSensor->gyroDev.gyroADCRaw[X] - (int32_t)gyroSensor->gyroDev.gyroZero[X];
gyroSensor->gyroDev.gyroADC[Y] = (int32_t)gyroSensor->gyroDev.gyroADCRaw[Y] - (int32_t)gyroSensor->gyroDev.gyroZero[Y]; gyroSensor->gyroDev.gyroADC[X] = gyroSensor->gyroDev.gyroADCRaw[X] - gyroSensor->gyroDev.gyroZero[X];
gyroSensor->gyroDev.gyroADC[Z] = (int32_t)gyroSensor->gyroDev.gyroADCRaw[Z] - (int32_t)gyroSensor->gyroDev.gyroZero[Z]; gyroSensor->gyroDev.gyroADC[Y] = gyroSensor->gyroDev.gyroADCRaw[Y] - gyroSensor->gyroDev.gyroZero[Y];
#if defined(USE_GYRO_SLEW_LIMITER)
gyroSensor->gyroDev.gyroADC[Z] = gyroSlewLimiter(Z, gyroSensor) - gyroSensor->gyroDev.gyroZero[Z];
#else
gyroSensor->gyroDev.gyroADC[Z] = gyroSensor->gyroDev.gyroADCRaw[Z] - gyroSensor->gyroDev.gyroZero[Z];
#endif
alignSensors(gyroSensor->gyroDev.gyroADC, gyroSensor->gyroDev.gyroAlign); alignSensors(gyroSensor->gyroDev.gyroADC, gyroSensor->gyroDev.gyroAlign);
} else { } else {