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

Tidied initialisation, especially PID filters

This commit is contained in:
Martin Budden 2016-11-23 14:09:08 +00:00
parent 81f6a1886a
commit e9407f3065
11 changed files with 82 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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;

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

@ -564,7 +564,7 @@ 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->gyro_lpf = GYRO_LPF_256HZ; // 256HZ default
#ifdef STM32F10X
config->gyro_sync_denom = 8;
config->pid_process_denom = 1;
@ -830,13 +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,
@ -998,12 +991,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.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;
}
if (masterConfig.gyro_lpf != GYRO_LPF_256HZ && masterConfig.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.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);

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

@ -1458,6 +1458,18 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
masterConfig.gyro_soft_notch_hz_2 = sbufReadU16(src);
masterConfig.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);
gyroInit();
// reinitialize the PID filters with the new values
pidInitFilters(&currentProfile->pidProfile);
break;
case MSP_SET_PID_ADVANCED:

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);

23
src/main/main.c
View File

@ -421,7 +421,8 @@ void init(void)
&masterConfig.sensorSelectionConfig,
masterConfig.compassConfig.mag_declination,
masterConfig.gyro_lpf,
masterConfig.gyro_sync_denom)) {
masterConfig.gyro_sync_denom,
&masterConfig.sonarConfig)) {
// if gyro was not detected due to whatever reason, we give up now.
failureMode(FAILURE_MISSING_ACC);
}
@ -443,10 +444,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 +478,6 @@ void init(void)
}
#endif
#ifdef SONAR
if (feature(FEATURE_SONAR)) {
sonarInit(&masterConfig.sonarConfig);
}
#endif
#ifdef LED_STRIP
ledStripInit(&masterConfig.ledStripConfig);
@ -537,13 +531,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

28
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,6 +612,19 @@ retry:
}
#endif
#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
void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig)
{
if (sensorAlignmentConfig->gyro_align != ALIGN_DEFAULT) {
@ -626,7 +642,8 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig,
sensorSelectionConfig_t *sensorSelectionConfig,
int16_t magDeclinationFromConfig,
uint8_t gyroLpf,
uint8_t gyroSyncDenominator)
uint8_t gyroSyncDenominator,
const sonarConfig_t *sonarConfig)
{
memset(&acc, 0, sizeof(acc));
memset(&gyro, 0, sizeof(gyro));
@ -664,6 +681,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 +691,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;

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

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

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

@ -43,7 +43,7 @@ void targetConfiguration(master_t *config)
config->motorConfig.minthrottle = 1049;
config->gyro_lpf = 1;
config->gyro_lpf = GYRO_LPF_188HZ;
config->gyro_soft_lpf_hz = 100;
config->gyro_soft_notch_hz_1 = 0;
config->gyro_soft_notch_hz_2 = 0;