Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 14:25:20 +03:00
Code Issues Wiki Activity

Dterm scaling correction // Code cleanup

Browse source
This commit is contained in:
borisbstyle 2015-09-19 23:37:41 +02:00
parent f5ad7f6003
commit 0539abc649
13 changed files with 34 additions and 84 deletions
Download patch file Download diff file Expand all files Collapse all files

12
src/main/config/config.c
View file

@ -128,7 +128,7 @@ static uint32_t activeFeaturesLatch = 0;
static uint8_t currentControlRateProfileIndex = 0;
controlRateConfig_t *currentControlRateProfile;
static const uint8_t EEPROM_CONF_VERSION = 107;
static const uint8_t EEPROM_CONF_VERSION = 108;
static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
{
@ -289,12 +289,12 @@ void resetSerialConfig(serialConfig_t *serialConfig)
}
static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) {
controlRateConfig->rcRate8 = 90;
controlRateConfig->rcExpo8 = 65;
controlRateConfig->rcRate8 = 100;
controlRateConfig->rcExpo8 = 70;
controlRateConfig->thrMid8 = 50;
controlRateConfig->thrExpo8 = 0;
controlRateConfig->dynThrPID = 0;
controlRateConfig->rcYawExpo8 = 0;
controlRateConfig->rcYawExpo8 = 20;
controlRateConfig->tpa_breakpoint = 1500;
for (uint8_t axis = 0; axis < FLIGHT_DYNAMICS_INDEX_COUNT; axis++) {
@ -380,7 +380,6 @@ static void resetConf(void)
masterConfig.current_profile_index = 0; // default profile
masterConfig.gyro_cmpf_factor = 600; // default MWC
masterConfig.gyro_cmpfm_factor = 250; // default MWC
masterConfig.gyro_lpf = 188; // supported by all gyro drivers now. In case of ST gyro, will default to 32Hz instead
resetAccelerometerTrims(&masterConfig.accZero);
@ -390,7 +389,6 @@ static void resetConf(void)
masterConfig.boardAlignment.pitchDegrees = 0;
masterConfig.boardAlignment.yawDegrees = 0;
masterConfig.acc_hardware = ACC_DEFAULT; // default/autodetect
masterConfig.acc_for_fast_looptime = 0; // FIXME - Needs to be removed through the code
masterConfig.max_angle_inclination = 500; // 50 degrees
masterConfig.yaw_control_direction = 1;
masterConfig.gyroConfig.gyroMovementCalibrationThreshold = 32;
@ -455,7 +453,6 @@ static void resetConf(void)
resetSerialConfig(&masterConfig.serialConfig);
masterConfig.emf_avoidance = 0;
masterConfig.rcSmoothing = 1;
resetPidProfile(&currentProfile->pidProfile);
@ -546,7 +543,6 @@ static void resetConf(void)
masterConfig.escAndServoConfig.minthrottle = 1000;
masterConfig.escAndServoConfig.maxthrottle = 2000;
masterConfig.motor_pwm_rate = 32000;
masterConfig.looptime = 0;
masterConfig.rcSmoothing = 1;
currentProfile->pidProfile.pidController = 3;
currentProfile->pidProfile.P8[ROLL] = 36;

1
src/main/config/config_master.h
View file

@ -26,7 +26,6 @@ typedef struct master_t {
uint8_t mixerMode;
uint32_t enabledFeatures;
uint8_t emf_avoidance; // change pll settings to avoid noise in the uhf band
uint8_t rcSmoothing; // Enable Interpolation of RC command
motorMixer_t customMotorMixer[MAX_SUPPORTED_MOTORS];
#ifdef USE_SERVOS

52
src/main/drivers/gyro_sync.c
View file

@ -43,58 +43,14 @@ bool gyroSyncCheckUpdate(void) {
return getMpuDataStatus(&gyro);
}
void gyroUpdateSampleRate(uint8_t lpf) {
void gyroUpdateSampleRate(void) {
int gyroSamplePeriod;
int minLooptime;
#if defined(SPRACINGF3) || defined(ALIENWIIF3) || defined(NAZE32PRO) || defined(STM32F3DISCOVERY) || defined(CHEBUZZF3) || defined(PORT103R) || defined(MOTOLAB) || defined(SPARKY)
if (lpf == INV_FILTER_256HZ_NOLPF2) {
gyroSamplePeriod = 125;
gyroSamplePeriod = 1000; // gyro sampling rate 1khz
minLooptime = 1000; // Full 1khz sampling
if(!sensors(SENSOR_ACC)) {
minLooptime = 500; // Max refresh 2khz
} else {
minLooptime = 625; // Max refresh 1,6khz
}
} else {
gyroSamplePeriod = 1000;
minLooptime = 1000; // Full sampling
}
#elif defined(CC3D)
if (lpf == INV_FILTER_256HZ_NOLPF2) {
gyroSamplePeriod = 125;
if(!sensors(SENSOR_ACC)) {
minLooptime = 890; // Max refresh 1,12khz
} else {
minLooptime = 1000; // Max refresh 1khz
}
} else {
gyroSamplePeriod = 1000;
minLooptime = 1000; // Full sampling
}
#elif defined(COLIBRI_RACE) || defined(EUSTM32F103RC)
if (lpf == INV_FILTER_256HZ_NOLPF2) {
gyroSamplePeriod = 125;
if(!sensors(SENSOR_ACC)) { // TODO - increase to 8khz when oneshot125 can be limited
minLooptime = 250; // Max refresh 4khz
} else {
minLooptime = 250; // Max refresh 4khz
}
} else {
gyroSamplePeriod = 1000;
minLooptime = 1000; // Full sampling
}
#else
if (lpf == INV_FILTER_256HZ_NOLPF2) {
gyroSamplePeriod = 125;
minLooptime = 625; // Max refresh 1,6khz
} else {
gyroSamplePeriod = 1000;
minLooptime = 1000; // Full sampling without ACC
}
#endif
// calculate gyro divider and targetLooptime (expected cycleTime)
mpuDividerDrops = (minLooptime + gyroSamplePeriod -1 ) / gyroSamplePeriod - 1;
targetLooptime = (mpuDividerDrops + 1) * gyroSamplePeriod;
}

2
src/main/drivers/gyro_sync.h
View file

@ -9,4 +9,4 @@ extern uint32_t targetLooptime;
bool gyroSyncCheckUpdate(void);
uint8_t gyroMPU6xxxGetDividerDrops(void);
void gyroUpdateSampleRate(uint8_t lpf);
void gyroUpdateSampleRate(void);

10
src/main/flight/imu.c
View file

@ -73,7 +73,7 @@ static pidProfile_t *pidProfile;
static accDeadband_t *accDeadband;
static accProcessor_t accProc;
static void qAccProcessingStateMachine(rollAndPitchTrims_t *accelerometerTrims, uint8_t acc_for_fast_looptime);
static void qAccProcessingStateMachine(rollAndPitchTrims_t *accelerometerTrims);
void imuConfigure(
@ -182,12 +182,12 @@ int16_t imuCalculateHeading(t_fp_vector *vec)
return head;
}
void imuUpdate(rollAndPitchTrims_t *accelerometerTrims, uint8_t acc_for_fast_looptime)
void imuUpdate(rollAndPitchTrims_t *accelerometerTrims)
{
gyroUpdate();
if (sensors(SENSOR_ACC)) {
qAccProcessingStateMachine(accelerometerTrims, acc_for_fast_looptime);
qAccProcessingStateMachine(accelerometerTrims);
} else {
accADC[X] = 0;
accADC[Y] = 0;
@ -578,7 +578,7 @@ void qimuInit()
//////////////////////////////////////////////////////////////////////
static void qAccProcessingStateMachine(rollAndPitchTrims_t *accelerometerTrims, uint8_t acc_for_fast_looptime)
static void qAccProcessingStateMachine(rollAndPitchTrims_t *accelerometerTrims)
{
int axis;
const float gyro_drift_factor = 0.00f;
@ -595,7 +595,7 @@ static void qAccProcessingStateMachine(rollAndPitchTrims_t *accelerometerTrims,
// get time step.. TODO: this should really be fixed to division of MPU sample rate
static float dT;
bool keepProcessing = !acc_for_fast_looptime; // (keepProcessing == true): causes all states to execute (for slow cycle times)
bool keepProcessing = true; // (keepProcessing == true): causes all states to execute (for slow cycle times)
do {
switch (accProc.state) {

2
src/main/flight/imu.h
View file

@ -84,4 +84,4 @@ float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff);
int16_t imuCalculateHeading(t_fp_vector *vec);
void imuResetAccelerationSum(void);
void imuUpdate(rollAndPitchTrims_t *accelerometerTrims, uint8_t acc_for_fast_looptime);
void imuUpdate(rollAndPitchTrims_t *accelerometerTrims);

10
src/main/flight/pid.c
View file

@ -210,7 +210,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
delta = filterApplyPt1(delta, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
}
DTerm = constrainf((delta / 3.0f) * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f);
DTerm = constrainf(delta * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f);
// -----calculate total PID output
axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000);
@ -301,7 +301,7 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
delta = filterApplyPt1(delta, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
}
DTerm = (delta * dynD8[axis]) / 32;
DTerm = (delta * 3 * dynD8[axis]) / 32;
axisPID[axis] = PTerm + ITerm - DTerm;
#ifdef BLACKBOX
@ -386,7 +386,7 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
delta = filterApplyPt1(delta, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
}
DTerm = ((int32_t)delta * dynD8[axis]) >> 5; // 32 bits is needed for calculation
DTerm = ((int32_t)delta * 3 * dynD8[axis]) >> 5; // 32 bits is needed for calculation
axisPID[axis] = PTerm + ITerm - DTerm;
@ -504,7 +504,7 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con
delta = filterApplyPt1(delta, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
}
DTerm = (delta * dynD8[axis]) / 32;
DTerm = (delta * 3 * dynD8[axis]) / 32;
axisPID[axis] = PTerm + ITerm - DTerm;
#ifdef BLACKBOX
@ -775,7 +775,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
delta = filterApplyPt1(delta, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
}
DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8;
DTerm = (delta * 3 * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; // Multiplied by 3 to match old scaling
// -----calculate total PID output
axisPID[axis] = PTerm + ITerm + DTerm;

2
src/main/io/serial_cli.c
View file

@ -325,7 +325,6 @@ typedef struct {
const clivalue_t valueTable[] = {
{ "emf_avoidance", VAR_UINT8 | MASTER_VALUE, &masterConfig.emf_avoidance, 0, 1 },
{ "rc_smoothing", VAR_UINT8 | MASTER_VALUE, &masterConfig.rcSmoothing, 0, 1 },
{ "mid_rc", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.midrc, 1200, 1700 },
{ "min_check", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.mincheck, PWM_RANGE_ZERO, PWM_RANGE_MAX },
@ -411,7 +410,6 @@ const clivalue_t valueTable[] = {
{ "max_angle_inclination", VAR_UINT16 | MASTER_VALUE, &masterConfig.max_angle_inclination, 100, 900 },
{ "gyro_lpf", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_lpf, 0, 256 },
{ "moron_threshold", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyroMovementCalibrationThreshold, 0, 128 },
{ "gyro_cmpf_factor", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_cmpf_factor, 100, 1000 },
{ "gyro_cmpfm_factor", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_cmpfm_factor, 100, 1000 },

2
src/main/main.c
View file

@ -371,7 +371,7 @@ void init(void)
}
#endif
if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf, masterConfig.acc_hardware, masterConfig.mag_hardware, masterConfig.baro_hardware, currentProfile->mag_declination)) {
if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.acc_hardware, masterConfig.mag_hardware, masterConfig.baro_hardware, currentProfile->mag_declination)) {
// if gyro was not detected due to whatever reason, we give up now.
failureMode(FAILURE_MISSING_ACC);
}

6
src/main/mw.c
View file

@ -773,7 +773,7 @@ void loop(void)
if (gyroSyncCheckUpdate() || (int32_t)(currentTime - (loopTime + GYRO_WATCHDOG_DELAY)) >= 0) {
loopTime = currentTime + targetLooptime;
imuUpdate(&currentProfile->accelerometerTrims, masterConfig.acc_for_fast_looptime);
imuUpdate(&currentProfile->accelerometerTrims);
// Measure loop rate just after reading the sensors
@ -781,13 +781,11 @@ void loop(void)
cycleTime = (int32_t)(currentTime - previousTime);
previousTime = currentTime;
dT = (float)cycleTime * 0.000001f;
dT = (float)targetLooptime * 0.000001f;
filterApply7TapFIR(gyroADC);
if (masterConfig.rcSmoothing) {
filterRc();
}
annexCode();
#if defined(BARO) || defined(SONAR)

2
src/main/sensors/gyro.h
View file

@ -29,6 +29,8 @@ typedef enum {
GYRO_FAKE
} gyroSensor_e;
#define GYRO_LPF 188
extern gyro_t gyro;
extern sensor_align_e gyroAlign;

9
src/main/sensors/initialisation.c
View file

@ -195,8 +195,9 @@ bool fakeAccDetect(acc_t *acc)
}
#endif
bool detectGyro(uint16_t gyroLpf)
bool detectGyro(void)
{
uint16_t gyroLpf = GYRO_LPF;
gyroSensor_e gyroHardware = GYRO_DEFAULT;
gyroAlign = ALIGN_DEFAULT;
@ -633,14 +634,14 @@ void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig)
}
}
bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, uint8_t magHardwareToUse, uint8_t baroHardwareToUse, int16_t magDeclinationFromConfig)
bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t accHardwareToUse, uint8_t magHardwareToUse, uint8_t baroHardwareToUse, int16_t magDeclinationFromConfig)
{
int16_t deg, min;
memset(&acc, 0, sizeof(acc));
memset(&gyro, 0, sizeof(gyro));
if (!detectGyro(gyroLpf)) {
if (!detectGyro()) {
return false;
}
detectAcc(accHardwareToUse);
@ -651,7 +652,7 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t
if (sensors(SENSOR_ACC))
acc.init();
// this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
gyroUpdateSampleRate(gyroLpf); // Set gyro refresh rate before initialisation
gyroUpdateSampleRate(); // Set gyro refresh rate before initialisation
gyro.init();
detectMag(magHardwareToUse);

2
src/main/sensors/initialisation.h
View file

@ -17,4 +17,4 @@
#pragma once
bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, uint8_t magHardwareToUse, uint8_t baroHardwareToUse, int16_t magDeclinationFromConfig);
bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t accHardwareToUse, uint8_t magHardwareToUse, uint8_t baroHardwareToUse, int16_t magDeclinationFromConfig);