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

Moved gyro filter settings into gyroConfig

This commit is contained in:
Martin Budden 2016-11-23 19:04:14 +00:00
parent fa4d04b5a7
commit 2c7d06a94e
9 changed files with 59 additions and 81 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/main/blackbox/blackbox.c
View File

@ -1263,12 +1263,12 @@ static bool blackboxWriteSysinfo()
BLACKBOX_PRINT_HEADER_LINE("deadband:%d", masterConfig.rcControlsConfig.deadband);
BLACKBOX_PRINT_HEADER_LINE("yaw_deadband:%d", masterConfig.rcControlsConfig.yaw_deadband);
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf:%d", masterConfig.gyro_lpf);
BLACKBOX_PRINT_HEADER_LINE("gyro_soft_type:%d", masterConfig.gyro_soft_type);
BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz:%d", masterConfig.gyro_soft_lpf_hz);
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz:%d,%d", masterConfig.gyro_soft_notch_hz_1,
masterConfig.gyro_soft_notch_hz_2);
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff:%d,%d", masterConfig.gyro_soft_notch_cutoff_1,
masterConfig.gyro_soft_notch_cutoff_2);
BLACKBOX_PRINT_HEADER_LINE("gyro_soft_type:%d", masterConfig.gyroConfig.gyro_soft_lpf_type);
BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz:%d", masterConfig.gyroConfig.gyro_soft_lpf_hz);
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz:%d,%d", masterConfig.gyroConfig.gyro_soft_notch_hz_1,
masterConfig.gyroConfig.gyro_soft_notch_hz_2);
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff:%d,%d", masterConfig.gyroConfig.gyro_soft_notch_cutoff_1,
masterConfig.gyroConfig.gyro_soft_notch_cutoff_2);
BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz:%d", (int)(masterConfig.acc_lpf_hz * 100.0f));
BLACKBOX_PRINT_HEADER_LINE("acc_hardware:%d", masterConfig.sensorSelectionConfig.acc_hardware);
BLACKBOX_PRINT_HEADER_LINE("baro_hardware:%d", masterConfig.sensorSelectionConfig.baro_hardware);

10
src/main/cms/cms_menu_imu.c
View File

@ -282,11 +282,11 @@ static OSD_Entry cmsx_menuFilterGlobalEntries[] =
{
{ "-- FILTER GLB --", OME_Label, NULL, NULL, 0 },
{ "GYRO LPF", OME_UINT8, NULL, &(OSD_UINT8_t) { &masterConfig.gyro_soft_lpf_hz, 0, 255, 1 }, 0 },
{ "GYRO NF1", OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyro_soft_notch_hz_1, 0, 500, 1 }, 0 },
{ "GYRO NF1C", OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyro_soft_notch_cutoff_1, 0, 500, 1 }, 0 },
{ "GYRO NF2", OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyro_soft_notch_hz_2, 0, 500, 1 }, 0 },
{ "GYRO NF2C", OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyro_soft_notch_cutoff_2, 0, 500, 1 }, 0 },
{ "GYRO LPF", OME_UINT8, NULL, &(OSD_UINT8_t) { &masterConfig.gyroConfig.gyro_soft_lpf_hz, 0, 255, 1 }, 0 },
{ "GYRO NF1", OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyroConfig.gyro_soft_notch_hz_1, 0, 500, 1 }, 0 },
{ "GYRO NF1C", OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyroConfig.gyro_soft_notch_cutoff_1, 0, 500, 1 }, 0 },
{ "GYRO NF2", OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyroConfig.gyro_soft_notch_hz_2, 0, 500, 1 }, 0 },
{ "GYRO NF2C", OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyroConfig.gyro_soft_notch_cutoff_2, 0, 500, 1 }, 0 },
{ "BACK", OME_Back, NULL, NULL, 0 },
{ NULL, OME_END, NULL, NULL, 0 }

8
src/main/config/config_master.h
View File

@ -91,14 +91,10 @@ typedef struct master_s {
int8_t yaw_control_direction; // change control direction of yaw (inverted, normal)
uint8_t acc_for_fast_looptime; // shorten acc processing time by using 1 out of 9 samples. For combination with fast looptimes.
uint16_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.
uint8_t gyro_sync_denom; // Gyro sample divider
uint8_t gyro_soft_type; // Gyro Filter Type
uint8_t gyro_soft_lpf_hz; // Biquad gyro lpf hz
uint16_t gyro_soft_notch_hz_1; // Biquad gyro notch hz
uint16_t gyro_soft_notch_cutoff_1; // Biquad gyro notch low cutoff
uint16_t gyro_soft_notch_hz_2; // Biquad gyro notch hz
uint16_t gyro_soft_notch_cutoff_2; // Biquad gyro notch low cutoff
uint16_t dcm_kp; // DCM filter proportional gain ( x 10000)
uint16_t dcm_ki; // DCM filter integral gain ( x 10000)

28
src/main/fc/config.c
View File

@ -575,12 +575,12 @@ void createDefaultConfig(master_t *config)
config->gyro_sync_denom = 4;
config->pid_process_denom = 2;
#endif
config->gyro_soft_type = FILTER_PT1;
config->gyro_soft_lpf_hz = 90;
config->gyro_soft_notch_hz_1 = 400;
config->gyro_soft_notch_cutoff_1 = 300;
config->gyro_soft_notch_hz_2 = 200;
config->gyro_soft_notch_cutoff_2 = 100;
config->gyroConfig.gyro_soft_lpf_type = FILTER_PT1;
config->gyroConfig.gyro_soft_lpf_hz = 90;
config->gyroConfig.gyro_soft_notch_hz_1 = 400;
config->gyroConfig.gyro_soft_notch_cutoff_1 = 300;
config->gyroConfig.gyro_soft_notch_hz_2 = 200;
config->gyroConfig.gyro_soft_notch_cutoff_2 = 100;
config->debug_mode = DEBUG_NONE;
@ -830,13 +830,7 @@ void activateConfig(void)
&currentProfile->pidProfile
);
gyroUseConfig(&masterConfig.gyroConfig,
masterConfig.gyro_soft_lpf_hz,
masterConfig.gyro_soft_notch_hz_1,
masterConfig.gyro_soft_notch_cutoff_1,
masterConfig.gyro_soft_notch_hz_2,
masterConfig.gyro_soft_notch_cutoff_2,
masterConfig.gyro_soft_type);
gyroUseConfig(&masterConfig.gyroConfig);
#ifdef TELEMETRY
telemetryUseConfig(&masterConfig.telemetryConfig);
@ -1002,11 +996,11 @@ void validateAndFixConfig(void)
void validateAndFixGyroConfig(void)
{
// Prevent invalid notch cutoff
if (masterConfig.gyro_soft_notch_cutoff_1 >= masterConfig.gyro_soft_notch_hz_1) {
masterConfig.gyro_soft_notch_hz_1 = 0;
if (masterConfig.gyroConfig.gyro_soft_notch_cutoff_1 >= masterConfig.gyroConfig.gyro_soft_notch_hz_1) {
masterConfig.gyroConfig.gyro_soft_notch_hz_1 = 0;
}
if (masterConfig.gyro_soft_notch_cutoff_2 >= masterConfig.gyro_soft_notch_hz_2) {
masterConfig.gyro_soft_notch_hz_2 = 0;
if (masterConfig.gyroConfig.gyro_soft_notch_cutoff_2 >= masterConfig.gyroConfig.gyro_soft_notch_hz_2) {
masterConfig.gyroConfig.gyro_soft_notch_hz_2 = 0;
}
if (masterConfig.gyro_lpf != GYRO_LPF_256HZ && masterConfig.gyro_lpf != GYRO_LPF_NONE) {

28
src/main/fc/fc_msp.c
View File

@ -1094,15 +1094,15 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
break;
case MSP_FILTER_CONFIG :
sbufWriteU8(dst, masterConfig.gyro_soft_lpf_hz);
sbufWriteU8(dst, masterConfig.gyroConfig.gyro_soft_lpf_hz);
sbufWriteU16(dst, currentProfile->pidProfile.dterm_lpf_hz);
sbufWriteU16(dst, currentProfile->pidProfile.yaw_lpf_hz);
sbufWriteU16(dst, masterConfig.gyro_soft_notch_hz_1);
sbufWriteU16(dst, masterConfig.gyro_soft_notch_cutoff_1);
sbufWriteU16(dst, masterConfig.gyroConfig.gyro_soft_notch_hz_1);
sbufWriteU16(dst, masterConfig.gyroConfig.gyro_soft_notch_cutoff_1);
sbufWriteU16(dst, currentProfile->pidProfile.dterm_notch_hz);
sbufWriteU16(dst, currentProfile->pidProfile.dterm_notch_cutoff);
sbufWriteU16(dst, masterConfig.gyro_soft_notch_hz_2);
sbufWriteU16(dst, masterConfig.gyro_soft_notch_cutoff_2);
sbufWriteU16(dst, masterConfig.gyroConfig.gyro_soft_notch_hz_2);
sbufWriteU16(dst, masterConfig.gyroConfig.gyro_soft_notch_cutoff_2);
break;
case MSP_PID_ADVANCED:
@ -1445,28 +1445,22 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
break;
case MSP_SET_FILTER_CONFIG:
masterConfig.gyro_soft_lpf_hz = sbufReadU8(src);
masterConfig.gyroConfig.gyro_soft_lpf_hz = sbufReadU8(src);
currentProfile->pidProfile.dterm_lpf_hz = sbufReadU16(src);
currentProfile->pidProfile.yaw_lpf_hz = sbufReadU16(src);
if (dataSize > 5) {
masterConfig.gyro_soft_notch_hz_1 = sbufReadU16(src);
masterConfig.gyro_soft_notch_cutoff_1 = sbufReadU16(src);
masterConfig.gyroConfig.gyro_soft_notch_hz_1 = sbufReadU16(src);
masterConfig.gyroConfig.gyro_soft_notch_cutoff_1 = sbufReadU16(src);
currentProfile->pidProfile.dterm_notch_hz = sbufReadU16(src);
currentProfile->pidProfile.dterm_notch_cutoff = sbufReadU16(src);
}
if (dataSize > 13) {
masterConfig.gyro_soft_notch_hz_2 = sbufReadU16(src);
masterConfig.gyro_soft_notch_cutoff_2 = sbufReadU16(src);
masterConfig.gyroConfig.gyro_soft_notch_hz_2 = sbufReadU16(src);
masterConfig.gyroConfig.gyro_soft_notch_cutoff_2 = sbufReadU16(src);
}
// reinitialize the gyro filters with the new values
validateAndFixGyroConfig();
gyroUseConfig(&masterConfig.gyroConfig,
masterConfig.gyro_soft_lpf_hz,
masterConfig.gyro_soft_notch_hz_1,
masterConfig.gyro_soft_notch_cutoff_1,
masterConfig.gyro_soft_notch_hz_2,
masterConfig.gyro_soft_notch_cutoff_2,
masterConfig.gyro_soft_type);
gyroUseConfig(&masterConfig.gyroConfig);
gyroInit();
// reinitialize the PID filters with the new values
pidInitFilters(&currentProfile->pidProfile);

2
src/main/fc/mw.c
View File

@ -803,7 +803,7 @@ void subTaskMotorUpdate(void)
uint8_t setPidUpdateCountDown(void)
{
if (masterConfig.gyro_soft_lpf_hz) {
if (masterConfig.gyroConfig.gyro_soft_lpf_hz) {
return masterConfig.pid_process_denom - 1;
} else {
return 1;

12
src/main/io/serial_cli.c
View File

@ -809,12 +809,12 @@ const clivalue_t valueTable[] = {
{ "max_angle_inclination", VAR_UINT16 | MASTER_VALUE, &masterConfig.max_angle_inclination, .config.minmax = { 100, 900 } },
{ "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } },
{ "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_sync_denom, .config.minmax = { 1, 8 } },
{ "gyro_lowpass_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_soft_type, .config.lookup = { TABLE_LOWPASS_TYPE } },
{ "gyro_lowpass", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_soft_lpf_hz, .config.minmax = { 0, 255 } },
{ "gyro_notch1_hz", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_hz_1, .config.minmax = { 0, 1000 } },
{ "gyro_notch1_cutoff", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_cutoff_1, .config.minmax = { 1, 1000 } },
{ "gyro_notch2_hz", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_hz_2, .config.minmax = { 0, 1000 } },
{ "gyro_notch2_cutoff", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_cutoff_2, .config.minmax = { 1, 1000 } },
{ "gyro_lowpass_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyroConfig.gyro_soft_lpf_type, .config.lookup = { TABLE_LOWPASS_TYPE } },
{ "gyro_lowpass", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyro_soft_lpf_hz, .config.minmax = { 0, 255 } },
{ "gyro_notch1_hz", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyroConfig.gyro_soft_notch_hz_1, .config.minmax = { 0, 1000 } },
{ "gyro_notch1_cutoff", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyroConfig.gyro_soft_notch_cutoff_1, .config.minmax = { 1, 1000 } },
{ "gyro_notch2_hz", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyroConfig.gyro_soft_notch_hz_2, .config.minmax = { 0, 1000 } },
{ "gyro_notch2_cutoff", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyroConfig.gyro_soft_notch_cutoff_2, .config.minmax = { 1, 1000 } },
{ "moron_threshold", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyroMovementCalibrationThreshold, .config.minmax = { 0, 128 } },
{ "imu_dcm_kp", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_kp, .config.minmax = { 0, 50000 } },
{ "imu_dcm_ki", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_ki, .config.minmax = { 0, 50000 } },

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

@ -57,25 +57,19 @@ static void *notchFilter1[3];
static filterApplyFnPtr notchFilter2ApplyFn;
static void *notchFilter2[3];
void gyroUseConfig(const gyroConfig_t *gyroConfigToUse,
uint8_t gyro_soft_lpf_hz,
uint16_t gyro_soft_notch_hz_1,
uint16_t gyro_soft_notch_cutoff_1,
uint16_t gyro_soft_notch_hz_2,
uint16_t gyro_soft_notch_cutoff_2,
uint8_t gyro_soft_lpf_type)
void gyroUseConfig(const gyroConfig_t *gyroConfigToUse)
{
gyroConfig = gyroConfigToUse;
gyroSoftLpfHz = gyro_soft_lpf_hz;
gyroSoftNotchHz1 = gyro_soft_notch_hz_1;
if (gyro_soft_notch_hz_1) {
gyroSoftNotchQ1 = filterGetNotchQ(gyro_soft_notch_hz_1, gyro_soft_notch_cutoff_1);
gyroSoftLpfHz = gyroConfig->gyro_soft_lpf_hz;
gyroSoftNotchHz1 = gyroConfig->gyro_soft_notch_hz_1;
if (gyroConfig->gyro_soft_notch_hz_1) {
gyroSoftNotchQ1 = filterGetNotchQ(gyroConfig->gyro_soft_notch_hz_1, gyroConfig->gyro_soft_notch_cutoff_1);
}
gyroSoftNotchHz2 = gyro_soft_notch_hz_2;
if (gyro_soft_notch_hz_2) {
gyroSoftNotchQ2 = filterGetNotchQ(gyro_soft_notch_hz_2, gyro_soft_notch_cutoff_2);
gyroSoftNotchHz2 = gyroConfig->gyro_soft_notch_hz_2;
if (gyroConfig->gyro_soft_notch_hz_2) {
gyroSoftNotchQ2 = filterGetNotchQ(gyroConfig->gyro_soft_notch_hz_2, gyroConfig->gyro_soft_notch_cutoff_2);
}
gyroSoftLpfType = gyro_soft_lpf_type;
gyroSoftLpfType = gyroConfig->gyro_soft_lpf_type;
}
void gyroInit(void)

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

@ -41,16 +41,16 @@ extern int32_t gyroADC[XYZ_AXIS_COUNT];
extern float gyroADCf[XYZ_AXIS_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.
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.
uint8_t gyro_soft_lpf_hz;
uint16_t gyro_soft_notch_hz_1;
uint16_t gyro_soft_notch_cutoff_1;
uint16_t gyro_soft_notch_hz_2;
uint16_t gyro_soft_notch_cutoff_2;
uint8_t gyro_soft_lpf_type;
} gyroConfig_t;
void gyroUseConfig(const gyroConfig_t *gyroConfigToUse,
uint8_t gyro_soft_lpf_hz,
uint16_t gyro_soft_notch_hz_1,
uint16_t gyro_soft_notch_cutoff_1,
uint16_t gyro_soft_notch_hz_2,
uint16_t gyro_soft_notch_cutoff_2,
uint8_t gyro_soft_lpf_type);
void gyroUseConfig(const gyroConfig_t *gyroConfigToUse);
void gyroSetCalibrationCycles(void);
void gyroInit(void);
void gyroUpdate(void);