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

Merge pull request #6024 from mikeller/add_filter_msp_and_defaults 

Added defaults and MSP support to filter settings.
This commit is contained in:
Michael Keller 2018-06-04 11:28:29 +12:00 committed by GitHub
parent 6bac0cd392 f32731638d
commit b21b626e12
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 46 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
src/main/interface/msp.c
View File

@ -1230,8 +1230,14 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
sbufWriteU16(dst, motorConfig()->digitalIdleOffsetValue);
sbufWriteU8(dst, gyroConfig()->gyro_use_32khz);
sbufWriteU8(dst, motorConfig()->dev.motorPwmInversion);
break;
sbufWriteU8(dst, gyroConfig()->gyro_to_use);
sbufWriteU8(dst, gyroConfig()->gyro_high_fsr);
sbufWriteU8(dst, gyroConfig()->gyroMovementCalibrationThreshold);
sbufWriteU16(dst, gyroConfig()->gyroCalibrationDuration);
sbufWriteU16(dst, gyroConfig()->gyro_offset_yaw);
sbufWriteU8(dst, gyroConfig()->checkOverflow);
break;
case MSP_FILTER_CONFIG :
sbufWriteU8(dst, gyroConfig()->gyro_lowpass_hz);
sbufWriteU16(dst, currentPidProfile->dterm_lowpass_hz);
@ -1243,8 +1249,14 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_hz_2);
sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_cutoff_2);
sbufWriteU8(dst, currentPidProfile->dterm_filter_type);
break;
sbufWriteU8(dst, gyroConfig()->gyro_hardware_lpf);
sbufWriteU8(dst, gyroConfig()->gyro_32khz_hardware_lpf);
sbufWriteU16(dst, gyroConfig()->gyro_lowpass2_hz);
sbufWriteU8(dst, gyroConfig()->gyro_lowpass_type);
sbufWriteU8(dst, gyroConfig()->gyro_lowpass2_type);
sbufWriteU16(dst, currentPidProfile->dterm_lowpass2_hz);
break;
case MSP_PID_ADVANCED:
sbufWriteU16(dst, 0);
sbufWriteU16(dst, 0);
@ -1671,13 +1683,21 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
if (sbufBytesRemaining(src)) {
gyroConfigMutable()->gyro_use_32khz = sbufReadU8(src);
}
validateAndFixGyroConfig();
if (sbufBytesRemaining(src)) {
motorConfigMutable()->dev.motorPwmInversion = sbufReadU8(src);
}
break;
if (sbufBytesRemaining(src) >= 8) {
gyroConfigMutable()->gyro_to_use = sbufReadU8(src);
gyroConfigMutable()->gyro_high_fsr = sbufReadU8(src);
gyroConfigMutable()->gyroMovementCalibrationThreshold = sbufReadU8(src);
gyroConfigMutable()->gyroCalibrationDuration = sbufReadU16(src);
gyroConfigMutable()->gyro_offset_yaw = sbufReadU16(src);
gyroConfigMutable()->checkOverflow = sbufReadU8(src);
}
validateAndFixGyroConfig();
break;
case MSP_SET_FILTER_CONFIG:
gyroConfigMutable()->gyro_lowpass_hz = sbufReadU8(src);
currentPidProfile->dterm_lowpass_hz = sbufReadU16(src);
@ -1695,13 +1715,21 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
if (sbufBytesRemaining(src) >= 1) {
currentPidProfile->dterm_filter_type = sbufReadU8(src);
}
if (sbufBytesRemaining(src) >= 8) {
gyroConfigMutable()->gyro_hardware_lpf = sbufReadU8(src);
gyroConfigMutable()->gyro_32khz_hardware_lpf = sbufReadU8(src);
gyroConfigMutable()->gyro_lowpass2_hz = sbufReadU16(src);
gyroConfigMutable()->gyro_lowpass_type = sbufReadU8(src);
gyroConfigMutable()->gyro_lowpass2_type = sbufReadU8(src);
currentPidProfile->dterm_lowpass2_hz = sbufReadU16(src);
}
// reinitialize the gyro filters with the new values
validateAndFixGyroConfig();
gyroInitFilters();
// reinitialize the PID filters with the new values
pidInitFilters(currentPidProfile);
break;
break;
case MSP_SET_PID_ADVANCED:
sbufReadU16(src);
sbufReadU16(src);

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

@ -169,7 +169,7 @@ static void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int typ
#define GYRO_OVERFLOW_TRIGGER_THRESHOLD 31980 // 97.5% full scale (1950dps for 2000dps gyro)
#define GYRO_OVERFLOW_RESET_THRESHOLD 30340 // 92.5% full scale (1850dps for 2000dps gyro)
PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 3);
PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 4);
#ifndef GYRO_CONFIG_USE_GYRO_DEFAULT
#define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_1

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

@ -68,20 +68,16 @@ typedef enum {
} filterSlots;
typedef struct gyroConfig_s {
sensor_align_e gyro_align; // gyro alignment
uint8_t gyro_align; // gyro alignment
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_sync_denom; // Gyro sample divider
uint8_t gyro_hardware_lpf; // gyro DLPF setting
uint8_t gyro_32khz_hardware_lpf; // gyro 32khz DLPF setting
bool gyro_high_fsr;
bool gyro_use_32khz;
uint8_t gyro_high_fsr;
uint8_t gyro_use_32khz;
uint8_t gyro_to_use;
// Lowpass primary/secondary
uint8_t gyro_lowpass_type;
uint8_t gyro_lowpass2_type;
uint16_t gyro_lowpass_hz;
uint16_t gyro_lowpass2_hz;
@ -89,14 +85,17 @@ typedef struct gyroConfig_s {
uint16_t gyro_soft_notch_cutoff_1;
uint16_t gyro_soft_notch_hz_2;
uint16_t gyro_soft_notch_cutoff_2;
gyroOverflowCheck_e checkOverflow;
int16_t gyro_offset_yaw;
uint8_t checkOverflow;
bool yaw_spin_recovery;
// Lowpass primary/secondary
uint8_t gyro_lowpass_type;
uint8_t gyro_lowpass2_type;
uint8_t yaw_spin_recovery;
int16_t yaw_spin_threshold;
uint16_t gyroCalibrationDuration; // Gyro calibration duration in 1/100 second
} gyroConfig_t;
PG_DECLARE(gyroConfig_t, gyroConfig);