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

Merge pull request #1640 from martinbudden/bf_init_tidy 

Tidied initialisation, especially gyro and PID filters
This commit is contained in:
borisbstyle 2016-11-26 21:25:44 +01:00 committed by GitHub
parent f38e13e571 2d8d03e507
commit 3db231dcdc
21 changed files with 169 additions and 160 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1199,7 +1199,7 @@ static bool blackboxWriteSysinfo()
);
BLACKBOX_PRINT_HEADER_LINE("looptime:%d", gyro.targetLooptime);
BLACKBOX_PRINT_HEADER_LINE("gyro_sync_denom:%d", masterConfig.gyro_sync_denom);
BLACKBOX_PRINT_HEADER_LINE("gyro_sync_denom:%d", masterConfig.gyroConfig.gyro_sync_denom);
BLACKBOX_PRINT_HEADER_LINE("pid_process_denom:%d", masterConfig.pid_process_denom);
BLACKBOX_PRINT_HEADER_LINE("rcRate:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcRate8);
BLACKBOX_PRINT_HEADER_LINE("rcExpo:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcExpo8);
@ -1262,13 +1262,13 @@ 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_lpf:%d", masterConfig.gyroConfig.gyro_lpf);
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 }

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

@ -91,14 +91,7 @@ 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)

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

@ -23,6 +23,15 @@
#define MPU_I2C_INSTANCE I2C_DEVICE
#endif
#define GYRO_LPF_256HZ 0
#define GYRO_LPF_188HZ 1
#define GYRO_LPF_98HZ 2
#define GYRO_LPF_42HZ 3
#define GYRO_LPF_20HZ 4
#define GYRO_LPF_10HZ 5
#define GYRO_LPF_5HZ 6
#define GYRO_LPF_NONE 7
typedef struct gyro_s {
sensorGyroInitFuncPtr init; // initialize function
sensorReadFuncPtr read; // read 3 axis data function

9
src/main/drivers/gyro_sync.c
View File

@ -23,15 +23,6 @@ bool gyroSyncCheckUpdate(const gyro_t *gyro)
return gyro->intStatus();
}
#define GYRO_LPF_256HZ 0
#define GYRO_LPF_188HZ 1
#define GYRO_LPF_98HZ 2
#define GYRO_LPF_42HZ 3
#define GYRO_LPF_20HZ 4
#define GYRO_LPF_10HZ 5
#define GYRO_LPF_5HZ 6
#define GYRO_LPF_NONE 7
uint32_t gyroSetSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator)
{
int gyroSamplePeriod;

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

@ -564,23 +564,23 @@ void createDefaultConfig(master_t *config)
config->current_profile_index = 0; // default profile
config->dcm_kp = 2500; // 1.0 * 10000
config->dcm_ki = 0; // 0.003 * 10000
config->gyro_lpf = 0; // 256HZ default
config->gyroConfig.gyro_lpf = GYRO_LPF_256HZ; // 256HZ default
#ifdef STM32F10X
config->gyro_sync_denom = 8;
config->gyroConfig.gyro_sync_denom = 8;
config->pid_process_denom = 1;
#elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_ICM20689)
config->gyro_sync_denom = 1;
config->gyroConfig.gyro_sync_denom = 1;
config->pid_process_denom = 4;
#else
config->gyro_sync_denom = 4;
config->gyroConfig.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,21 +830,6 @@ void activateConfig(void)
&currentProfile->pidProfile
);
// 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);
#endif
@ -999,11 +984,29 @@ void validateAndFixConfig(void)
resetSerialConfig(serialConfig);
}
validateAndFixGyroConfig();
#if defined(TARGET_VALIDATECONFIG)
targetValidateConfiguration(&masterConfig);
#endif
}
void validateAndFixGyroConfig(void)
{
// Prevent invalid notch cutoff
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.gyroConfig.gyro_soft_notch_cutoff_2 >= masterConfig.gyroConfig.gyro_soft_notch_hz_2) {
masterConfig.gyroConfig.gyro_soft_notch_hz_2 = 0;
}
if (masterConfig.gyroConfig.gyro_lpf != GYRO_LPF_256HZ && masterConfig.gyroConfig.gyro_lpf != GYRO_LPF_NONE) {
masterConfig.pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed
masterConfig.gyroConfig.gyro_sync_denom = 1;
}
}
void readEEPROMAndNotify(void)
{
// re-read written data

1
src/main/fc/config.h
View File

@ -75,6 +75,7 @@ void ensureEEPROMContainsValidData(void);
void saveConfigAndNotify(void);
void validateAndFixConfig(void);
void validateAndFixGyroConfig(void);
void activateConfig(void);
uint8_t getCurrentProfile(void);

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

@ -1081,11 +1081,11 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
break;
case MSP_ADVANCED_CONFIG:
if (masterConfig.gyro_lpf) {
if (masterConfig.gyroConfig.gyro_lpf) {
sbufWriteU8(dst, 8); // If gyro_lpf != OFF then looptime is set to 1000
sbufWriteU8(dst, 1);
} else {
sbufWriteU8(dst, masterConfig.gyro_sync_denom);
sbufWriteU8(dst, masterConfig.gyroConfig.gyro_sync_denom);
sbufWriteU8(dst, masterConfig.pid_process_denom);
}
sbufWriteU8(dst, masterConfig.motorConfig.useUnsyncedPwm);
@ -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:
@ -1433,7 +1433,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
break;
case MSP_SET_ADVANCED_CONFIG:
masterConfig.gyro_sync_denom = sbufReadU8(src);
masterConfig.gyroConfig.gyro_sync_denom = sbufReadU8(src);
masterConfig.pid_process_denom = sbufReadU8(src);
masterConfig.motorConfig.useUnsyncedPwm = sbufReadU8(src);
#ifdef USE_DSHOT
@ -1445,19 +1445,24 @@ 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();
gyroInit(&masterConfig.gyroConfig);
// reinitialize the PID filters with the new values
pidInitFilters(&currentProfile->pidProfile);
break;
case MSP_SET_PID_ADVANCED:

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;

6
src/main/flight/pid.c
View File

@ -56,7 +56,7 @@ int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
static float errorGyroIf[3];
void setTargetPidLooptime(uint32_t pidLooptime)
void pidSetTargetLooptime(uint32_t pidLooptime)
{
targetPidLooptime = pidLooptime;
}
@ -82,7 +82,7 @@ static void *dtermFilterLpf[2];
static filterApplyFnPtr ptermYawFilterApplyFn;
static void *ptermYawFilter;
static void pidInitFilters(const pidProfile_t *pidProfile)
void pidInitFilters(const pidProfile_t *pidProfile)
{
static bool initialized = false; // !!TODO - remove this temporary measure once filter lazy initialization is removed
static biquadFilter_t biquadFilterNotch[2];
@ -166,8 +166,6 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
dT = (float)targetPidLooptime * 0.000001f;
}
pidInitFilters(pidProfile);
float horizonLevelStrength = 1;
if (FLIGHT_MODE(HORIZON_MODE)) {
// Figure out the raw stick positions

3
src/main/flight/pid.h
View File

@ -104,5 +104,6 @@ extern uint8_t PIDweight[3];
void pidResetErrorGyroState(void);
void pidStabilisationState(pidStabilisationState_e pidControllerState);
void setTargetPidLooptime(uint32_t pidLooptime);
void pidSetTargetLooptime(uint32_t pidLooptime);
void pidInitFilters(const pidProfile_t *pidProfile);

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

@ -807,14 +807,14 @@ const clivalue_t valueTable[] = {
{ "align_board_yaw", VAR_INT16 | MASTER_VALUE, &masterConfig.boardAlignment.yawDegrees, .config.minmax = { -180, 360 } },
{ "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_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyroConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } },
{ "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyro_sync_denom, .config.minmax = { 1, 8 } },
{ "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 } },

29
src/main/main.c
View File

@ -417,11 +417,16 @@ void init(void)
}
#endif
#ifdef SONAR
const sonarConfig_t *sonarConfig = &masterConfig.sonarConfig;
#else
const void *sonarConfig = NULL;
#endif
if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig,
&masterConfig.sensorSelectionConfig,
masterConfig.compassConfig.mag_declination,
masterConfig.gyro_lpf,
masterConfig.gyro_sync_denom)) {
&masterConfig.gyroConfig,
sonarConfig)) {
// if gyro was not detected due to whatever reason, we give up now.
failureMode(FAILURE_MISSING_ACC);
}
@ -443,10 +448,9 @@ void init(void)
LED0_OFF;
LED1_OFF;
#ifdef MAG
if (sensors(SENSOR_MAG))
compassInit();
#endif
// gyro.targetLooptime set in sensorsAutodetect(), so we are ready to call pidSetTargetLooptime()
pidSetTargetLooptime((gyro.targetLooptime + LOOPTIME_SUSPEND_TIME) * masterConfig.pid_process_denom); // Initialize pid looptime
pidInitFilters(&currentProfile->pidProfile);
imuInit();
@ -478,12 +482,6 @@ void init(void)
}
#endif
#ifdef SONAR
if (feature(FEATURE_SONAR)) {
sonarInit(&masterConfig.sonarConfig);
}
#endif
#ifdef LED_STRIP
ledStripInit(&masterConfig.ledStripConfig);
@ -537,13 +535,6 @@ void init(void)
}
#endif
if (masterConfig.gyro_lpf > 0 && masterConfig.gyro_lpf < 7) {
masterConfig.pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed
masterConfig.gyro_sync_denom = 1;
}
setTargetPidLooptime((gyro.targetLooptime + LOOPTIME_SUSPEND_TIME) * masterConfig.pid_process_denom); // Initialize pid looptime
#ifdef BLACKBOX
initBlackbox();
#endif

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

@ -44,10 +44,6 @@ float gyroADCf[XYZ_AXIS_COUNT];
static int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 };
static const gyroConfig_t *gyroConfig;
static uint8_t gyroSoftLpfType;
static uint16_t gyroSoftNotchHz1, gyroSoftNotchHz2;
static float gyroSoftNotchQ1, gyroSoftNotchQ2;
static uint8_t gyroSoftLpfHz;
static uint16_t calibratingG = 0;
static filterApplyFnPtr softLpfFilterApplyFn;
@ -57,24 +53,7 @@ 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)
{
gyroConfig = gyroConfigToUse;
gyroSoftLpfHz = gyro_soft_lpf_hz;
gyroSoftNotchHz1 = gyro_soft_notch_hz_1;
gyroSoftNotchHz2 = gyro_soft_notch_hz_2;
gyroSoftLpfType = gyro_soft_lpf_type;
gyroSoftNotchQ1 = filterGetNotchQ(gyro_soft_notch_hz_1, gyro_soft_notch_cutoff_1);
gyroSoftNotchQ2 = filterGetNotchQ(gyro_soft_notch_hz_2, gyro_soft_notch_cutoff_2);
}
void gyroInit(void)
void gyroInit(const gyroConfig_t *gyroConfigToUse)
{
static biquadFilter_t gyroFilterLPF[XYZ_AXIS_COUNT];
static pt1Filter_t gyroFilterPt1[XYZ_AXIS_COUNT];
@ -82,45 +61,49 @@ void gyroInit(void)
static biquadFilter_t gyroFilterNotch_1[XYZ_AXIS_COUNT];
static biquadFilter_t gyroFilterNotch_2[XYZ_AXIS_COUNT];
gyroConfig = gyroConfigToUse;
softLpfFilterApplyFn = nullFilterApply;
notchFilter1ApplyFn = nullFilterApply;
notchFilter2ApplyFn = nullFilterApply;
if (gyroSoftLpfHz) { // Initialisation needs to happen once samplingrate is known
if (gyroSoftLpfType == FILTER_BIQUAD) {
if (gyroConfig->gyro_soft_lpf_hz) { // Initialisation needs to happen once samplingrate is known
if (gyroConfig->gyro_soft_lpf_type == FILTER_BIQUAD) {
softLpfFilterApplyFn = (filterApplyFnPtr)biquadFilterApply;
for (int axis = 0; axis < 3; axis++) {
softLpfFilter[axis] = &gyroFilterLPF[axis];
biquadFilterInitLPF(softLpfFilter[axis], gyroSoftLpfHz, gyro.targetLooptime);
biquadFilterInitLPF(softLpfFilter[axis], gyroConfig->gyro_soft_lpf_hz, gyro.targetLooptime);
}
} else if (gyroSoftLpfType == FILTER_PT1) {
} else if (gyroConfig->gyro_soft_lpf_type == FILTER_BIQUAD) {
softLpfFilterApplyFn = (filterApplyFnPtr)pt1FilterApply;
const float gyroDt = (float) gyro.targetLooptime * 0.000001f;
for (int axis = 0; axis < 3; axis++) {
softLpfFilter[axis] = &gyroFilterPt1[axis];
pt1FilterInit(softLpfFilter[axis], gyroSoftLpfHz, gyroDt);
pt1FilterInit(softLpfFilter[axis], gyroConfig->gyro_soft_lpf_hz, gyroDt);
}
} else {
softLpfFilterApplyFn = (filterApplyFnPtr)firFilterDenoiseUpdate;
for (int axis = 0; axis < 3; axis++) {
softLpfFilter[axis] = &gyroDenoiseState[axis];
firFilterDenoiseInit(softLpfFilter[axis], gyroSoftLpfHz, gyro.targetLooptime);
firFilterDenoiseInit(softLpfFilter[axis], gyroConfig->gyro_soft_lpf_hz, gyro.targetLooptime);
}
}
}
if (gyroSoftNotchHz1) {
if (gyroConfig->gyro_soft_notch_hz_1) {
notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply;
const float gyroSoftNotchQ1 = filterGetNotchQ(gyroConfig->gyro_soft_notch_hz_1, gyroConfig->gyro_soft_notch_cutoff_1);
for (int axis = 0; axis < 3; axis++) {
notchFilter1[axis] = &gyroFilterNotch_1[axis];
biquadFilterInit(notchFilter1[axis], gyroSoftNotchHz1, gyro.targetLooptime, gyroSoftNotchQ1, FILTER_NOTCH);
biquadFilterInit(notchFilter1[axis], gyroConfig->gyro_soft_notch_hz_1, gyro.targetLooptime, gyroSoftNotchQ1, FILTER_NOTCH);
}
}
if (gyroSoftNotchHz1) {
if (gyroConfig->gyro_soft_notch_hz_2) {
notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
const float gyroSoftNotchQ2 = filterGetNotchQ(gyroConfig->gyro_soft_notch_hz_2, gyroConfig->gyro_soft_notch_cutoff_2);
for (int axis = 0; axis < 3; axis++) {
notchFilter2[axis] = &gyroFilterNotch_2[axis];
biquadFilterInit(notchFilter2[axis], gyroSoftNotchHz2, gyro.targetLooptime, gyroSoftNotchQ2, FILTER_NOTCH);
biquadFilterInit(notchFilter2[axis], gyroConfig->gyro_soft_notch_hz_2, gyro.targetLooptime, gyroSoftNotchQ2, FILTER_NOTCH);
}
}
}

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

@ -41,18 +41,19 @@ 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_sync_denom; // Gyro sample divider
uint8_t gyro_soft_lpf_type;
uint8_t gyro_soft_lpf_hz;
uint16_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.
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;
} 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 gyroSetCalibrationCycles(void);
void gyroInit(void);
void gyroInit(const gyroConfig_t *gyroConfigToUse);
void gyroUpdate(void);
bool isGyroCalibrationComplete(void);

41
src/main/sensors/initialisation.c
View File

@ -24,6 +24,8 @@
#include "common/axis.h"
#include "config/feature.h"
#include "drivers/io.h"
#include "drivers/system.h"
#include "drivers/exti.h"
@ -61,6 +63,7 @@
#include "drivers/sonar_hcsr04.h"
#include "fc/config.h"
#include "fc/runtime_config.h"
#include "sensors/sensors.h"
@ -609,7 +612,20 @@ retry:
}
#endif
void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig)
#ifdef SONAR
static bool detectSonar(void)
{
if (feature(FEATURE_SONAR)) {
// the user has set the sonar feature, so assume they have an HC-SR04 plugged in,
// since there is no way to detect it
sensorsSet(SENSOR_SONAR);
return true;
}
return false;
}
#endif
static void reconfigureAlignment(const sensorAlignmentConfig_t *sensorAlignmentConfig)
{
if (sensorAlignmentConfig->gyro_align != ALIGN_DEFAULT) {
gyroAlign = sensorAlignmentConfig->gyro_align;
@ -622,11 +638,11 @@ void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig)
}
}
bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig,
sensorSelectionConfig_t *sensorSelectionConfig,
bool sensorsAutodetect(const sensorAlignmentConfig_t *sensorAlignmentConfig,
const sensorSelectionConfig_t *sensorSelectionConfig,
int16_t magDeclinationFromConfig,
uint8_t gyroLpf,
uint8_t gyroSyncDenominator)
const gyroConfig_t *gyroConfig,
const sonarConfig_t *sonarConfig)
{
memset(&acc, 0, sizeof(acc));
memset(&gyro, 0, sizeof(gyro));
@ -645,9 +661,9 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig,
// Now time to init things
// this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
gyro.targetLooptime = gyroSetSampleRate(gyroLpf, gyroSyncDenominator); // Set gyro sample rate before initialisation
gyro.init(gyroLpf); // driver initialisation
gyroInit(); // sensor initialisation
gyro.targetLooptime = gyroSetSampleRate(gyroConfig->gyro_lpf, gyroConfig->gyro_sync_denom); // Set gyro sample rate before initialisation
gyro.init(gyroConfig->gyro_lpf); // driver initialisation
gyroInit(gyroConfig); // sensor initialisation
if (detectAcc(sensorSelectionConfig->acc_hardware)) {
acc.acc_1G = 256; // set default
@ -664,6 +680,7 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig,
const int16_t deg = magDeclinationFromConfig / 100;
const int16_t min = magDeclinationFromConfig % 100;
magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units
compassInit();
}
#else
UNUSED(magDeclinationFromConfig);
@ -673,6 +690,14 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig,
detectBaro(sensorSelectionConfig->baro_hardware);
#endif
#ifdef SONAR
if (detectSonar()) {
sonarInit(sonarConfig);
}
#else
UNUSED(sonarConfig);
#endif
reconfigureAlignment(sensorAlignmentConfig);
return true;

10
src/main/sensors/initialisation.h
View File

@ -16,5 +16,13 @@
*/
#pragma once
struct sensorAlignmentConfig_s;
struct sensorSelectionConfig_s;
bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, struct sensorSelectionConfig_s *sensorSelectionConfig, int16_t magDeclinationFromConfig, uint8_t gyroLpf, uint8_t gyroSyncDenominator);
struct gyroConfig_s;
struct sonarConfig_s;
bool sensorsAutodetect(const struct sensorAlignmentConfig_s *sensorAlignmentConfig,
const struct sensorSelectionConfig_s *sensorSelectionConfig,
int16_t magDeclinationFromConfig,
const struct gyroConfig_s *gyroConfig,
const struct sonarConfig_s *sonarConfig);

4
src/main/target/COLIBRI_RACE/i2c_bst.c
View File

@ -977,7 +977,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
bstWrite8(masterConfig.rcControlsConfig.yaw_deadband);
break;
case BST_FC_FILTERS:
bstWrite16(constrain(masterConfig.gyro_lpf, 0, 1)); // Extra safety to prevent OSD setting corrupt values
bstWrite16(constrain(masterConfig.gyroConfig.gyro_lpf, 0, 1)); // Extra safety to prevent OSD setting corrupt values
break;
default:
// we do not know how to handle the (valid) message, indicate error BST
@ -1404,7 +1404,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
masterConfig.rcControlsConfig.yaw_deadband = bstRead8();
break;
case BST_SET_FC_FILTERS:
masterConfig.gyro_lpf = bstRead16();
masterConfig.gyroConfig.gyro_lpf = bstRead16();
break;
default:

2
src/main/target/MOTOLAB/config.c
View File

@ -26,6 +26,6 @@
// Motolab target supports 2 different type of boards Tornado / Cyclone.
void targetConfiguration(master_t *config)
{
config->gyro_sync_denom = 4;
config->gyroConfig.gyro_sync_denom = 4;
config->pid_process_denom = 1;
}

2
src/main/target/MULTIFLITEPICO/config.c
View File

@ -70,7 +70,7 @@ void targetConfiguration(master_t *config) {
config->motorConfig.motorPwmRate = 17000;
config->gyro_sync_denom = 4;
config->gyroConfig.gyro_sync_denom = 4;
config->pid_process_denom = 1;
config->profile[0].pidProfile.P8[ROLL] = 70;

8
src/main/target/NAZE/config.c
View File

@ -43,10 +43,10 @@ void targetConfiguration(master_t *config)
config->motorConfig.minthrottle = 1049;
config->gyro_lpf = 1;
config->gyro_soft_lpf_hz = 100;
config->gyro_soft_notch_hz_1 = 0;
config->gyro_soft_notch_hz_2 = 0;
config->gyroConfig.gyro_lpf = GYRO_LPF_188HZ;
config->gyroConfig.gyro_soft_lpf_hz = 100;
config->gyroConfig.gyro_soft_notch_hz_1 = 0;
config->gyroConfig.gyro_soft_notch_hz_2 = 0;
/*for (int channel = 0; channel < NON_AUX_CHANNEL_COUNT; channel++) {
config->rxConfig.channelRanges[channel].min = 1180;