Merge pull request #1640 from martinbudden/bf_init_tidy
Tidied initialisation, especially gyro and PID filters
This commit is contained in:
commit
3db231dcdc
|
@ -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);
|
||||
|
|
|
@ -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 }
|
||||
|
|
|
@ -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)
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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)
|
|||
¤tProfile->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
|
||||
|
@ -998,12 +983,30 @@ void validateAndFixConfig(void)
|
|||
if (!isSerialConfigValid(serialConfig)) {
|
||||
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
|
||||
|
|
|
@ -75,6 +75,7 @@ void ensureEEPROMContainsValidData(void);
|
|||
|
||||
void saveConfigAndNotify(void);
|
||||
void validateAndFixConfig(void);
|
||||
void validateAndFixGyroConfig(void);
|
||||
void activateConfig(void);
|
||||
|
||||
uint8_t getCurrentProfile(void);
|
||||
|
|
|
@ -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(¤tProfile->pidProfile);
|
||||
break;
|
||||
|
||||
case MSP_SET_PID_ADVANCED:
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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);
|
||||
|
||||
|
|
|
@ -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 } },
|
||||
|
|
|
@ -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(¤tProfile->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
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
@ -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);
|
||||
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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:
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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;
|
||||
|
|
Loading…
Reference in New Issue