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

Add New Default Filter combination

This commit is contained in:
borisbstyle 2016-09-26 23:28:07 +02:00
parent 63e6182ee5
commit c191da6c58
8 changed files with 73 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1291,8 +1291,10 @@ static bool blackboxWriteSysinfo()
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", (int)(masterConfig.gyro_soft_lpf_hz * 100.0f));
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz:%d", (int)(masterConfig.gyro_soft_notch_hz * 100.0f));
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff:%d", (int)(masterConfig.gyro_soft_notch_cutoff * 100.0f));
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz_1:%d", (int)(masterConfig.gyro_soft_notch_hz_1 * 100.0f));
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff_1:%d", (int)(masterConfig.gyro_soft_notch_cutoff_1 * 100.0f));
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz_2:%d", (int)(masterConfig.gyro_soft_notch_hz_2 * 100.0f));
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff_2:%d", (int)(masterConfig.gyro_soft_notch_cutoff_2 * 100.0f));
BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz:%d", (int)(masterConfig.acc_lpf_hz * 100.0f));
BLACKBOX_PRINT_HEADER_LINE("acc_hardware:%d", masterConfig.acc_hardware);
BLACKBOX_PRINT_HEADER_LINE("baro_hardware:%d", masterConfig.baro_hardware);

27
src/main/config/config.c
View File

@ -207,10 +207,10 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->P8[ROLL] = 45;
pidProfile->I8[ROLL] = 40;
pidProfile->D8[ROLL] = 20;
pidProfile->D8[ROLL] = 16;
pidProfile->P8[PITCH] = 60;
pidProfile->I8[PITCH] = 65;
pidProfile->D8[PITCH] = 22;
pidProfile->D8[PITCH] = 19;
pidProfile->P8[YAW] = 70;
pidProfile->I8[YAW] = 45;
pidProfile->D8[YAW] = 20;
@ -487,9 +487,11 @@ void createDefaultConfig(master_t *config)
config->pid_process_denom = 2;
#endif
config->gyro_soft_type = FILTER_PT1;
config->gyro_soft_lpf_hz = 90;
config->gyro_soft_notch_hz = 0;
config->gyro_soft_notch_cutoff = 130;
config->gyro_soft_lpf_hz = 80;
config->gyro_soft_notch_hz_1 = 400;
config->gyro_soft_notch_cutoff_1 = 300;
config->gyro_soft_notch_hz_2 = 0;
config->gyro_soft_notch_cutoff_2 = 100;
config->debug_mode = DEBUG_NONE;
@ -762,7 +764,20 @@ void activateConfig(void)
&currentProfile->pidProfile
);
gyroUseConfig(&masterConfig.gyroConfig, masterConfig.gyro_soft_lpf_hz, masterConfig.gyro_soft_notch_hz, masterConfig.gyro_soft_notch_cutoff, masterConfig.gyro_soft_type);
// 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.gyro_soft_notch_cutoff_2 >= masterConfig.gyro_soft_notch_hz_2)
masterConfig.gyro_soft_notch_hz_2 = 0;
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);
#ifdef TELEMETRY
telemetryUseConfig(&masterConfig.telemetryConfig);

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

@ -59,8 +59,10 @@ typedef struct master_t {
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; // Biquad gyro notch hz
uint16_t gyro_soft_notch_cutoff; // Biquad gyro notch low cutoff
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)

2
src/main/io/msp_protocol.h
View File

@ -59,7 +59,7 @@
#define MSP_PROTOCOL_VERSION 0
#define API_VERSION_MAJOR 1 // increment when major changes are made
#define API_VERSION_MINOR 20 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR
#define API_VERSION_MINOR 21 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR
#define API_VERSION_LENGTH 2

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

@ -773,8 +773,10 @@ const clivalue_t valueTable[] = {
{ "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_sync_denom, .config.minmax = { 1, 8 } },
{ "gyro_lowpass_level", 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_notch_hz", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_hz, .config.minmax = { 0, 500 } },
{ "gyro_notch_cutoff", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_cutoff, .config.minmax = { 1, 500 } },
{ "gyro_notch_hz_1", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_hz_1, .config.minmax = { 0, 1000 } },
{ "gyro_notch_cutoff_1", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_cutoff_1, .config.minmax = { 1, 1000 } },
{ "gyro_notch_hz_2", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_hz_2, .config.minmax = { 0, 1000 } },
{ "gyro_notch_cutoff_2", VAR_UINT16 | MASTER_VALUE, &masterConfig.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 } },

16
src/main/io/serial_msp.c
View File

@ -1258,14 +1258,16 @@ static bool processOutCommand(uint8_t cmdMSP)
serialize16(masterConfig.motor_pwm_rate);
break;
case MSP_FILTER_CONFIG :
headSerialReply(13);
headSerialReply(17);
serialize8(masterConfig.gyro_soft_lpf_hz);
serialize16(currentProfile->pidProfile.dterm_lpf_hz);
serialize16(currentProfile->pidProfile.yaw_lpf_hz);
serialize16(masterConfig.gyro_soft_notch_hz);
serialize16(masterConfig.gyro_soft_notch_cutoff);
serialize16(masterConfig.gyro_soft_notch_hz_1);
serialize16(masterConfig.gyro_soft_notch_cutoff_1);
serialize16(currentProfile->pidProfile.dterm_notch_hz);
serialize16(currentProfile->pidProfile.dterm_notch_cutoff);
serialize16(masterConfig.gyro_soft_notch_hz_2);
serialize16(masterConfig.gyro_soft_notch_cutoff_2);
break;
case MSP_PID_ADVANCED:
headSerialReply(17);
@ -1865,11 +1867,15 @@ static bool processInCommand(void)
currentProfile->pidProfile.dterm_lpf_hz = read16();
currentProfile->pidProfile.yaw_lpf_hz = read16();
if (currentPort->dataSize > 5) {
masterConfig.gyro_soft_notch_hz = read16();
masterConfig.gyro_soft_notch_cutoff = read16();
masterConfig.gyro_soft_notch_hz_1 = read16();
masterConfig.gyro_soft_notch_cutoff_1 = read16();
currentProfile->pidProfile.dterm_notch_hz = read16();
currentProfile->pidProfile.dterm_notch_cutoff = read16();
}
if (currentPort->dataSize > 13) {
serialize16(masterConfig.gyro_soft_notch_hz_2);
serialize16(masterConfig.gyro_soft_notch_cutoff_2);
}
break;
case MSP_SET_PID_ADVANCED:
currentProfile->pidProfile.rollPitchItermIgnoreRate = read16();

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

@ -46,29 +46,38 @@ float gyroADCf[XYZ_AXIS_COUNT];
static int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 };
static const gyroConfig_t *gyroConfig;
static biquadFilter_t gyroFilterLPF[XYZ_AXIS_COUNT];
static biquadFilter_t gyroFilterNotch[XYZ_AXIS_COUNT];
static biquadFilter_t gyroFilterNotch_1[XYZ_AXIS_COUNT], gyroFilterNotch_2[XYZ_AXIS_COUNT];
static pt1Filter_t gyroFilterPt1[XYZ_AXIS_COUNT];
static uint8_t gyroSoftLpfType;
static uint16_t gyroSoftNotchHz;
static float gyroSoftNotchQ;
static uint16_t gyroSoftNotchHz_1, gyroSoftNotchHz_2;
static float gyroSoftNotchQ_1, gyroSoftNotchQ_2;
static uint8_t gyroSoftLpfHz;
static uint16_t calibratingG = 0;
static float gyroDt;
void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz, uint16_t gyro_soft_notch_hz, uint16_t gyro_soft_notch_cutoff, uint8_t gyro_soft_lpf_type)
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)
{
gyroConfig = gyroConfigToUse;
gyroSoftLpfHz = gyro_soft_lpf_hz;
gyroSoftNotchHz = gyro_soft_notch_hz;
gyroSoftNotchHz_1 = gyro_soft_notch_hz_1;
gyroSoftNotchHz_2 = gyro_soft_notch_hz_2;
gyroSoftLpfType = gyro_soft_lpf_type;
gyroSoftNotchQ = filterGetNotchQ(gyro_soft_notch_hz, gyro_soft_notch_cutoff);
gyroSoftNotchQ_1 = filterGetNotchQ(gyro_soft_notch_hz_1, gyro_soft_notch_cutoff_1);
gyroSoftNotchQ_2 = filterGetNotchQ(gyro_soft_notch_hz_2, gyro_soft_notch_cutoff_2);
}
void gyroInit(void)
{
if (gyroSoftLpfHz && gyro.targetLooptime) { // Initialisation needs to happen once samplingrate is known
for (int axis = 0; axis < 3; axis++) {
biquadFilterInit(&gyroFilterNotch[axis], gyroSoftNotchHz, gyro.targetLooptime, gyroSoftNotchQ, FILTER_NOTCH);
biquadFilterInit(&gyroFilterNotch_1[axis], gyroSoftNotchHz_1, gyro.targetLooptime, gyroSoftNotchQ_1, FILTER_NOTCH);
biquadFilterInit(&gyroFilterNotch_2[axis], gyroSoftNotchHz_2, gyro.targetLooptime, gyroSoftNotchQ_2, FILTER_NOTCH);
if (gyroSoftLpfType == FILTER_BIQUAD)
biquadFilterInitLPF(&gyroFilterLPF[axis], gyroSoftLpfHz, gyro.targetLooptime);
else
@ -183,8 +192,11 @@ void gyroUpdate(void)
if (debugMode == DEBUG_NOTCH)
debug[axis] = lrintf(gyroADCf[axis]);
if (gyroSoftNotchHz)
gyroADCf[axis] = biquadFilterApply(&gyroFilterNotch[axis], gyroADCf[axis]);
if (gyroSoftNotchHz_1)
gyroADCf[axis] = biquadFilterApply(&gyroFilterNotch_1[axis], gyroADCf[axis]);
if (gyroSoftNotchHz_2)
gyroADCf[axis] = biquadFilterApply(&gyroFilterNotch_2[axis], gyroADCf[axis]);
gyroADC[axis] = lrintf(gyroADCf[axis]);
}

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

@ -40,7 +40,13 @@ 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.
} gyroConfig_t;
void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz, uint16_t gyro_soft_notch_hz, uint16_t gyro_soft_notch_cutoff, uint8_t gyro_soft_lpf_type);
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 gyroSetCalibrationCycles(void);
void gyroInit(void);
void gyroUpdate(void);