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

Merge remote-tracking branch 'borisbstyle/betaflight' into blackbox-enhancements

This commit is contained in:
Gary Keeble 2016-05-04 12:59:39 +01:00
parent 17b0c03992 e9963f454b
commit e3ea79ce97
12 changed files with 59 additions and 97 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1280,8 +1280,8 @@ static bool blackboxWriteSysinfo()
masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_average_count); masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_average_count);
break; break;
case 36: case 36:
blackboxPrintfHeaderLine("dterm_differentiator:%d", blackboxPrintfHeaderLine("dynamic_pterm:%d",
masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_differentiator); masterConfig.profile[masterConfig.current_profile_index].pidProfile.dynamic_pterm);
break; break;
case 37: case 37:
blackboxPrintfHeaderLine("rollPitchItermResetRate:%d", blackboxPrintfHeaderLine("rollPitchItermResetRate:%d",

5
src/main/common/typeconversion.c
View File

@ -144,11 +144,6 @@ char *itoa(int i, char *a, int base)
#endif #endif
/* Note: the floatString must be at least 13 bytes long to cover all cases.
* This includes up to 10 digits (32 bit ints can hold numbers up to 10
* digits long) + the decimal point + negative sign or space + null
* terminator.
*/
char *ftoa(float x, char *floatString) char *ftoa(float x, char *floatString)
{ {
int32_t value; int32_t value;

10
src/main/common/typeconversion.h
View File

@ -21,17 +21,7 @@ void li2a(long num, char *bf);
void ui2a(unsigned int num, unsigned int base, int uc, char *bf); void ui2a(unsigned int num, unsigned int base, int uc, char *bf);
void i2a(int num, char *bf); void i2a(int num, char *bf);
char a2i(char ch, const char **src, int base, int *nump); char a2i(char ch, const char **src, int base, int *nump);
/* Simple conversion of a float to a string. Will display completely
* inaccurate results for floats larger than about 2.15E6 (2^31 / 1000)
* (same thing for negative values < -2.15E6).
* Will always display 3 decimals, so anything smaller than 1E-3 will
* not be displayed.
* The floatString will be filled in with the result and will be
* returned. It must be at least 13 bytes in length to cover all cases!
*/
char *ftoa(float x, char *floatString); char *ftoa(float x, char *floatString);
float fastA2F(const char *p); float fastA2F(const char *p);
#ifndef HAVE_ITOA_FUNCTION #ifndef HAVE_ITOA_FUNCTION

22
src/main/config/config.c
View File

@ -134,7 +134,7 @@ static uint32_t activeFeaturesLatch = 0;
static uint8_t currentControlRateProfileIndex = 0; static uint8_t currentControlRateProfileIndex = 0;
controlRateConfig_t *currentControlRateProfile; controlRateConfig_t *currentControlRateProfile;
static const uint8_t EEPROM_CONF_VERSION = 133; static const uint8_t EEPROM_CONF_VERSION = 134;
static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
{ {
@ -148,10 +148,10 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->pidController = 1; pidProfile->pidController = 1;
pidProfile->P8[ROLL] = 45; pidProfile->P8[ROLL] = 45;
pidProfile->I8[ROLL] = 30; pidProfile->I8[ROLL] = 35;
pidProfile->D8[ROLL] = 18; pidProfile->D8[ROLL] = 18;
pidProfile->P8[PITCH] = 45; pidProfile->P8[PITCH] = 45;
pidProfile->I8[PITCH] = 30; pidProfile->I8[PITCH] = 35;
pidProfile->D8[PITCH] = 18; pidProfile->D8[PITCH] = 18;
pidProfile->P8[YAW] = 90; pidProfile->P8[YAW] = 90;
pidProfile->I8[YAW] = 40; pidProfile->I8[YAW] = 40;
@ -177,12 +177,12 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->D8[PIDVEL] = 75; pidProfile->D8[PIDVEL] = 75;
pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX; pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX;
pidProfile->yaw_lpf_hz = 70.0f; pidProfile->yaw_lpf_hz = 70;
pidProfile->dterm_differentiator = 1;
pidProfile->rollPitchItermResetRate = 200; pidProfile->rollPitchItermResetRate = 200;
pidProfile->rollPitchItermResetAlways = 0; pidProfile->rollPitchItermResetAlways = 0;
pidProfile->yawItermResetRate = 50; pidProfile->yawItermResetRate = 50;
pidProfile->dterm_lpf_hz = 70.0f; // filtering ON by default pidProfile->dterm_lpf_hz = 80; // filtering ON by default
pidProfile->dynamic_pterm = 1;
pidProfile->H_sensitivity = 75; // TODO - Cleanup during next EEPROM changes pidProfile->H_sensitivity = 75; // TODO - Cleanup during next EEPROM changes
@ -308,9 +308,9 @@ static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) {
controlRateConfig->rcExpo8 = 60; controlRateConfig->rcExpo8 = 60;
controlRateConfig->thrMid8 = 50; controlRateConfig->thrMid8 = 50;
controlRateConfig->thrExpo8 = 0; controlRateConfig->thrExpo8 = 0;
controlRateConfig->dynThrPID = 0; controlRateConfig->dynThrPID = 20;
controlRateConfig->rcYawExpo8 = 20; controlRateConfig->rcYawExpo8 = 20;
controlRateConfig->tpa_breakpoint = 1500; controlRateConfig->tpa_breakpoint = 1650;
for (uint8_t axis = 0; axis < FLIGHT_DYNAMICS_INDEX_COUNT; axis++) { for (uint8_t axis = 0; axis < FLIGHT_DYNAMICS_INDEX_COUNT; axis++) {
controlRateConfig->rates[axis] = 50; controlRateConfig->rates[axis] = 50;
@ -402,10 +402,10 @@ static void resetConf(void)
masterConfig.dcm_kp = 2500; // 1.0 * 10000 masterConfig.dcm_kp = 2500; // 1.0 * 10000
masterConfig.dcm_ki = 0; // 0.003 * 10000 masterConfig.dcm_ki = 0; // 0.003 * 10000
masterConfig.gyro_lpf = 0; // 256HZ default masterConfig.gyro_lpf = 0; // 256HZ default
masterConfig.gyro_sync_denom = 8; masterConfig.gyro_sync_denom = 4;
masterConfig.gyro_soft_lpf_hz = 80.0f; masterConfig.gyro_soft_lpf_hz = 85;
masterConfig.pid_process_denom = 1; masterConfig.pid_process_denom = 2;
masterConfig.debug_mode = 0; masterConfig.debug_mode = 0;

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

@ -56,9 +56,9 @@ typedef struct master_t {
int8_t yaw_control_direction; // change control direction of yaw (inverted, normal) int8_t yaw_control_direction; // change control direction of yaw (inverted, normal)
uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device
uint8_t acc_for_fast_looptime; // shorten acc processing time by using 1 out of 9 samples. For combination with fast looptimes. uint8_t acc_for_fast_looptime; // shorten acc processing time by using 1 out of 9 samples. For combination with fast looptimes.
uint8_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_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_sync_denom; // Gyro sample divider
float gyro_soft_lpf_hz; // Biqyad gyro lpf hz uint8_t gyro_soft_lpf_hz; // Biqyad gyro lpf hz
uint16_t dcm_kp; // DCM filter proportional gain ( x 10000) uint16_t dcm_kp; // DCM filter proportional gain ( x 10000)
uint16_t dcm_ki; // DCM filter integral gain ( x 10000) uint16_t dcm_ki; // DCM filter integral gain ( x 10000)

2
src/main/flight/mixer.h
View File

@ -74,7 +74,7 @@ typedef struct mixerConfig_s {
uint16_t yaw_jump_prevention_limit; // make limit configurable (original fixed value was 100) uint16_t yaw_jump_prevention_limit; // make limit configurable (original fixed value was 100)
#ifdef USE_SERVOS #ifdef USE_SERVOS
uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed
int16_t servo_lowpass_freq; // lowpass servo filter frequency selection; 1/1000ths of loop freq uint16_t servo_lowpass_freq; // lowpass servo filter frequency selection; 1/1000ths of loop freq
int8_t servo_lowpass_enable; // enable/disable lowpass filter int8_t servo_lowpass_enable; // enable/disable lowpass filter
#endif #endif
} mixerConfig_t; } mixerConfig_t;

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

@ -93,6 +93,16 @@ float calculateExpoPlus(int axis, rxConfig_t *rxConfig) {
return propFactor; return propFactor;
} }
uint16_t getDynamicKp(int axis, pidProfile_t *pidProfile) {
uint16_t dynamicKp;
uint32_t dynamicFactor = constrain(ABS(rcCommand[axis] << 8) / DYNAMIC_PTERM_STICK_THRESHOLD, 0, 1 << 7);
dynamicKp = ((pidProfile->P8[axis] << 8) + (pidProfile->P8[axis] * dynamicFactor)) >> 8;
return dynamicKp;
}
void pidResetErrorAngle(void) void pidResetErrorAngle(void)
{ {
errorAngleI[ROLL] = 0; errorAngleI[ROLL] = 0;
@ -136,7 +146,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
{ {
float RateError, AngleRate, gyroRate; float RateError, AngleRate, gyroRate;
float ITerm,PTerm,DTerm; float ITerm,PTerm,DTerm;
static float lastRate[3][PID_LAST_RATE_COUNT]; static float lastRate[3];
float delta; float delta;
int axis; int axis;
float horizonLevelStrength = 1; float horizonLevelStrength = 1;
@ -200,11 +210,13 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
// multiplication of rcCommand corresponds to changing the sticks scaling here // multiplication of rcCommand corresponds to changing the sticks scaling here
RateError = AngleRate - gyroRate; RateError = AngleRate - gyroRate;
uint16_t kP = (pidProfile->dynamic_pterm) ? getDynamicKp(axis, pidProfile) : pidProfile->P8[axis];
// -----calculate P component // -----calculate P component
if ((IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) || (axis == YAW && rxConfig->superExpoYawMode == SUPEREXPO_YAW_ALWAYS)) { if ((IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) || (axis == YAW && rxConfig->superExpoYawMode == SUPEREXPO_YAW_ALWAYS)) {
PTerm = (luxPTermScale * pidProfile->P8[axis] * tpaFactor) * (AngleRate - gyroRate * calculateExpoPlus(axis, rxConfig)); PTerm = (luxPTermScale * kP * tpaFactor) * (AngleRate - gyroRate * calculateExpoPlus(axis, rxConfig));
} else { } else {
PTerm = luxPTermScale * RateError * pidProfile->P8[axis] * tpaFactor; PTerm = luxPTermScale * RateError * kP * tpaFactor;
} }
// Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply // Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply
@ -220,7 +232,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
} }
if (axis == YAW) { if (axis == YAW) {
if (ABS(gyroRate / 4.1f) >= pidProfile->yawItermResetRate) errorGyroIf[axis] = constrainf(errorGyroIf[axis], -ITERM_RESET_THRESHOLD_YAW, ITERM_RESET_THRESHOLD_YAW); if (ABS(gyroRate / 4.1f) >= pidProfile->yawItermResetRate) errorGyroIf[axis] = constrainf(errorGyroIf[axis], -ITERM_RESET_THRESHOLD, ITERM_RESET_THRESHOLD);
} }
if (antiWindupProtection || motorLimitReached) { if (antiWindupProtection || motorLimitReached) {
@ -238,20 +250,8 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
if (pidProfile->yaw_lpf_hz) PTerm = filterApplyPt1(PTerm, &yawFilterState, pidProfile->yaw_lpf_hz, getdT()); if (pidProfile->yaw_lpf_hz) PTerm = filterApplyPt1(PTerm, &yawFilterState, pidProfile->yaw_lpf_hz, getdT());
DTerm = 0; DTerm = 0;
} else { } else {
if (pidProfile->dterm_differentiator) { delta = -(gyroRate - lastRate[axis]);
// Calculate derivative using noise-robust differentiator without time delay (one-sided or forward filters) lastRate[axis] = gyroRate;
// by Pavel Holoborodko, see http://www.holoborodko.com/pavel/numerical-methods/numerical-derivative/smooth-low-noise-differentiators/
// N=5: h[0] = 3/8, h[-1] = 1/2, h[-2] = -1/2, h[-3] = -3/4, h[-4] = 1/8, h[-5] = 1/4
delta = -(3*gyroRate + 4*lastRate[axis][0] - 4*lastRate[axis][1] - 6*lastRate[axis][2] + 1*lastRate[axis][3] + 2*lastRate[axis][4]) / 8;
for (int i = PID_LAST_RATE_COUNT - 1; i > 0; i--) {
lastRate[axis][i] = lastRate[axis][i-1];
}
} else {
// When DTerm low pass filter disabled apply moving average to reduce noise
delta = -(gyroRate - lastRate[axis][0]);
}
lastRate[axis][0] = gyroRate;
// Divide delta by targetLooptime to get differential (ie dr/dt) // Divide delta by targetLooptime to get differential (ie dr/dt)
delta *= (1.0f / getdT()); delta *= (1.0f / getdT());
@ -288,7 +288,7 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
int axis; int axis;
int32_t PTerm, ITerm, DTerm, delta; int32_t PTerm, ITerm, DTerm, delta;
static int32_t lastRate[3][PID_LAST_RATE_COUNT]; static int32_t lastRate[3];
int32_t AngleRateTmp, RateError, gyroRate; int32_t AngleRateTmp, RateError, gyroRate;
int8_t horizonLevelStrength = 100; int8_t horizonLevelStrength = 100;
@ -342,11 +342,13 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
gyroRate = gyroADC[axis] / 4; gyroRate = gyroADC[axis] / 4;
RateError = AngleRateTmp - gyroRate; RateError = AngleRateTmp - gyroRate;
uint16_t kP = (pidProfile->dynamic_pterm) ? getDynamicKp(axis, pidProfile) : pidProfile->P8[axis];
// -----calculate P component // -----calculate P component
if ((IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) || (axis == YAW && rxConfig->superExpoYawMode == SUPEREXPO_YAW_ALWAYS)) { if ((IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) || (axis == YAW && rxConfig->superExpoYawMode == SUPEREXPO_YAW_ALWAYS)) {
PTerm = (pidProfile->P8[axis] * PIDweight[axis] / 100) * (AngleRateTmp - (int32_t)(gyroRate * calculateExpoPlus(axis, rxConfig))) >> 7; PTerm = (kP * PIDweight[axis] / 100) * (AngleRateTmp - (int32_t)(gyroRate * calculateExpoPlus(axis, rxConfig))) >> 7;
} else { } else {
PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7; PTerm = (RateError * kP * PIDweight[axis] / 100) >> 7;
} }
// Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply // Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply
@ -366,11 +368,11 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) - GYRO_I_MAX << 13, (int32_t) + GYRO_I_MAX << 13); errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) - GYRO_I_MAX << 13, (int32_t) + GYRO_I_MAX << 13);
if ((pidProfile->rollPitchItermResetAlways || IS_RC_MODE_ACTIVE(BOXSUPEREXPO)) && axis != YAW) { if ((pidProfile->rollPitchItermResetAlways || IS_RC_MODE_ACTIVE(BOXSUPEREXPO)) && axis != YAW) {
if (ABS(gyroRate *10 / 41) >= pidProfile->rollPitchItermResetRate) errorGyroI[axis] = constrain(errorGyroI[axis], -ITERM_RESET_THRESHOLD, ITERM_RESET_THRESHOLD); if (ABS(gyroRate *10 / 41) >= pidProfile->rollPitchItermResetRate) errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) -ITERM_RESET_THRESHOLD << 13, (int32_t) + ITERM_RESET_THRESHOLD << 13);
} }
if (axis == YAW) { if (axis == YAW) {
if (ABS(gyroRate * 10 / 41) >= pidProfile->yawItermResetRate) errorGyroI[axis] = constrain(errorGyroI[axis], -ITERM_RESET_THRESHOLD_YAW, ITERM_RESET_THRESHOLD_YAW); if (ABS(gyroRate * 10 / 41) >= pidProfile->yawItermResetRate) errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) -ITERM_RESET_THRESHOLD << 13, (int32_t) + ITERM_RESET_THRESHOLD << 13);
} }
if (antiWindupProtection || motorLimitReached) { if (antiWindupProtection || motorLimitReached) {
@ -386,20 +388,8 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
if (pidProfile->yaw_lpf_hz) PTerm = filterApplyPt1(PTerm, &yawFilterState, pidProfile->yaw_lpf_hz, getdT()); if (pidProfile->yaw_lpf_hz) PTerm = filterApplyPt1(PTerm, &yawFilterState, pidProfile->yaw_lpf_hz, getdT());
DTerm = 0; DTerm = 0;
} else { } else {
if (pidProfile->dterm_differentiator) { delta = -(gyroRate - lastRate[axis]);
// Calculate derivative using noise-robust differentiator without time delay (one-sided or forward filters) lastRate[axis] = gyroRate;
// by Pavel Holoborodko, see http://www.holoborodko.com/pavel/numerical-methods/numerical-derivative/smooth-low-noise-differentiators/
// N=5: h[0] = 3/8, h[-1] = 1/2, h[-2] = -1/2, h[-3] = -3/4, h[-4] = 1/8, h[-5] = 1/4
delta = -(3*gyroRate + 4*lastRate[axis][0] - 4*lastRate[axis][1] - 6*lastRate[axis][2] + 1*lastRate[axis][3] + 2*lastRate[axis][4]) / 8;
for (int i = PID_LAST_RATE_COUNT - 1; i > 0; i--) {
lastRate[axis][i] = lastRate[axis][i-1];
}
} else {
// When DTerm low pass filter disabled apply moving average to reduce noise
delta = -(gyroRate - lastRate[axis][0]);
}
lastRate[axis][0] = gyroRate;
// Divide delta by targetLooptime to get differential (ie dr/dt) // Divide delta by targetLooptime to get differential (ie dr/dt)
delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 5; delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 5;

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

@ -22,9 +22,8 @@
#define YAW_P_LIMIT_MIN 100 // Maximum value for yaw P limiter #define YAW_P_LIMIT_MIN 100 // Maximum value for yaw P limiter
#define YAW_P_LIMIT_MAX 400 // Maximum value for yaw P limiter #define YAW_P_LIMIT_MAX 400 // Maximum value for yaw P limiter
#define PID_LAST_RATE_COUNT 7 #define ITERM_RESET_THRESHOLD 15
#define ITERM_RESET_THRESHOLD 20 #define DYNAMIC_PTERM_STICK_THRESHOLD 400
#define ITERM_RESET_THRESHOLD_YAW 10
typedef enum { typedef enum {
PIDROLL, PIDROLL,
@ -74,14 +73,14 @@ typedef struct pidProfile_s {
uint8_t H_sensitivity; uint8_t H_sensitivity;
float dterm_lpf_hz; // Delta Filter in hz uint16_t dterm_lpf_hz; // Delta Filter in hz
float yaw_lpf_hz; // Additional yaw filter when yaw axis too noisy uint16_t yaw_lpf_hz; // Additional yaw filter when yaw axis too noisy
uint16_t rollPitchItermResetRate; // Experimental threshold for resetting iterm for pitch and roll on certain rates uint16_t rollPitchItermResetRate; // Experimental threshold for resetting iterm for pitch and roll on certain rates
uint8_t rollPitchItermResetAlways; // Reset Iterm also without SUPER EXPO uint8_t rollPitchItermResetAlways; // Reset Iterm also without SUPER EXPO
uint16_t yawItermResetRate; // Experimental threshold for resetting iterm for yaw on certain rates uint16_t yawItermResetRate; // Experimental threshold for resetting iterm for yaw on certain rates
uint16_t yaw_p_limit; uint16_t yaw_p_limit;
uint8_t dterm_average_count; // Configurable delta count for dterm uint8_t dterm_average_count; // Configurable delta count for dterm
uint8_t dterm_differentiator; uint8_t dynamic_pterm;
#ifdef GTUNE #ifdef GTUNE
uint8_t gtune_lolimP[3]; // [0..200] Lower limit of P during G tune uint8_t gtune_lolimP[3]; // [0..200] Lower limit of P during G tune

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

@ -653,7 +653,7 @@ const clivalue_t valueTable[] = {
{ "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } }, { "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_sync_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_sync_denom, .config.minmax = { 1, 8 } },
{ "gyro_lowpass_hz", VAR_FLOAT | MASTER_VALUE, &masterConfig.gyro_soft_lpf_hz, .config.minmax = { 0, 500 } }, { "gyro_lowpass", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_soft_lpf_hz, .config.minmax = { 0, 255 } },
{ "moron_threshold", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyroMovementCalibrationThreshold, .config.minmax = { 0, 128 } }, { "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_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 } }, { "imu_dcm_ki", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_ki, .config.minmax = { 0, 50000 } },
@ -673,7 +673,7 @@ const clivalue_t valueTable[] = {
{ "yaw_p_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_p_limit, .config.minmax = { YAW_P_LIMIT_MIN, YAW_P_LIMIT_MAX } }, { "yaw_p_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_p_limit, .config.minmax = { YAW_P_LIMIT_MIN, YAW_P_LIMIT_MAX } },
#ifdef USE_SERVOS #ifdef USE_SERVOS
{ "tri_unarmed_servo", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mixerConfig.tri_unarmed_servo, .config.lookup = { TABLE_OFF_ON } }, { "tri_unarmed_servo", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mixerConfig.tri_unarmed_servo, .config.lookup = { TABLE_OFF_ON } },
{ "servo_lowpass_freq", VAR_INT16 | MASTER_VALUE, &masterConfig.mixerConfig.servo_lowpass_freq, .config.minmax = { 10, 400} }, { "servo_lowpass_freq", VAR_UINT16 | MASTER_VALUE, &masterConfig.mixerConfig.servo_lowpass_freq, .config.minmax = { 10, 400} },
{ "servo_lowpass_enable", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mixerConfig.servo_lowpass_enable, .config.lookup = { TABLE_OFF_ON } }, { "servo_lowpass_enable", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mixerConfig.servo_lowpass_enable, .config.lookup = { TABLE_OFF_ON } },
#endif #endif
@ -721,12 +721,12 @@ const clivalue_t valueTable[] = {
{ "mag_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mag_hardware, .config.lookup = { TABLE_MAG_HARDWARE } }, { "mag_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mag_hardware, .config.lookup = { TABLE_MAG_HARDWARE } },
{ "mag_declination", VAR_INT16 | MASTER_VALUE, &masterConfig.mag_declination, .config.minmax = { -18000, 18000 } }, { "mag_declination", VAR_INT16 | MASTER_VALUE, &masterConfig.mag_declination, .config.minmax = { -18000, 18000 } },
{ "dterm_lowpass_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } }, { "dterm_lowpass", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } },
{ "dterm_differentiator", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.dterm_differentiator, .config.lookup = { TABLE_OFF_ON } }, { "dynamic_pterm", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.dynamic_pterm, .config.lookup = { TABLE_OFF_ON } },
{ "iterm_always_reset", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermResetAlways, .config.lookup = { TABLE_OFF_ON } }, { "iterm_always_reset", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.rollPitchItermResetAlways, .config.lookup = { TABLE_OFF_ON } },
{ "iterm_reset_degrees", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermResetRate, .config.minmax = {50, 1000 } }, { "iterm_reset_degrees", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermResetRate, .config.minmax = {50, 1000 } },
{ "yaw_iterm_reset_degrees", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermResetRate, .config.minmax = {25, 1000 } }, { "yaw_iterm_reset_degrees", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermResetRate, .config.minmax = {25, 1000 } },
{ "yaw_lowpass_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_lpf_hz, .config.minmax = {0, 500 } }, { "yaw_lowpass", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_lpf_hz, .config.minmax = {0, 500 } },
{ "pid_process_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.pid_process_denom, .config.minmax = { 1, 8 } }, { "pid_process_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.pid_process_denom, .config.minmax = { 1, 8 } },
{ "pid_controller", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.pidController, .config.lookup = { TABLE_PID_CONTROLLER } }, { "pid_controller", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.pidController, .config.lookup = { TABLE_PID_CONTROLLER } },
@ -2448,7 +2448,7 @@ static void cliWrite(uint8_t ch)
static void cliPrintVar(const clivalue_t *var, uint32_t full) static void cliPrintVar(const clivalue_t *var, uint32_t full)
{ {
int32_t value = 0; int32_t value = 0;
char buf[13]; char buf[8];
void *ptr = var->ptr; void *ptr = var->ptr;
if ((var->type & VALUE_SECTION_MASK) == PROFILE_VALUE) { if ((var->type & VALUE_SECTION_MASK) == PROFILE_VALUE) {

2
src/main/io/serial_msp.c
View File

@ -152,7 +152,7 @@ void setGyroSamplingSpeed(uint16_t looptime) {
masterConfig.pid_process_denom = 1; masterConfig.pid_process_denom = 1;
if (currentProfile->pidProfile.pidController == 2) masterConfig.pid_process_denom = 2; if (currentProfile->pidProfile.pidController == 2) masterConfig.pid_process_denom = 2;
if (looptime < 250) { if (looptime < 250) {
masterConfig.pid_process_denom = 3; masterConfig.pid_process_denom = 4;
} else if (looptime < 375) { } else if (looptime < 375) {
if (currentProfile->pidProfile.pidController == 2) { if (currentProfile->pidProfile.pidController == 2) {
masterConfig.pid_process_denom = 3; masterConfig.pid_process_denom = 3;

14
src/main/mw.c
View File

@ -770,17 +770,6 @@ uint8_t setPidUpdateCountDown(void) {
} }
} }
// Check for oneshot125 protection. With fast looptimes oneshot125 pulse duration gets more near the pid looptime
bool shouldUpdateMotorsAfterPIDLoop(void) {
if (targetPidLooptime > 375 ) {
return true;
} else if ((masterConfig.use_multiShot || masterConfig.use_oneshot42) && feature(FEATURE_ONESHOT125)) {
return true;
} else {
return false;
}
}
// Function for loop trigger // Function for loop trigger
void taskMainPidLoopCheck(void) { void taskMainPidLoopCheck(void) {
static uint32_t previousTime; static uint32_t previousTime;
@ -801,7 +790,6 @@ void taskMainPidLoopCheck(void) {
static uint8_t pidUpdateCountdown; static uint8_t pidUpdateCountdown;
if (runTaskMainSubprocesses) { if (runTaskMainSubprocesses) {
if (!shouldUpdateMotorsAfterPIDLoop()) taskMotorUpdate();
subTasksMainPidLoop(); subTasksMainPidLoop();
runTaskMainSubprocesses = false; runTaskMainSubprocesses = false;
} }
@ -813,7 +801,7 @@ void taskMainPidLoopCheck(void) {
} else { } else {
pidUpdateCountdown = setPidUpdateCountDown(); pidUpdateCountdown = setPidUpdateCountDown();
taskMainPidLoop(); taskMainPidLoop();
if (shouldUpdateMotorsAfterPIDLoop()) taskMotorUpdate(); taskMotorUpdate();
runTaskMainSubprocesses = true; runTaskMainSubprocesses = true;
} }

4
src/main/version.h
View File

@ -16,8 +16,8 @@
*/ */
#define FC_VERSION_MAJOR 2 // increment when a major release is made (big new feature, etc) #define FC_VERSION_MAJOR 2 // increment when a major release is made (big new feature, etc)
#define FC_VERSION_MINOR 6 // increment when a minor release is made (small new feature, change etc) #define FC_VERSION_MINOR 7 // increment when a minor release is made (small new feature, change etc)
#define FC_VERSION_PATCH_LEVEL 2 // increment when a bug is fixed #define FC_VERSION_PATCH_LEVEL 0 // increment when a bug is fixed
#define STR_HELPER(x) #x #define STR_HELPER(x) #x
#define STR(x) STR_HELPER(x) #define STR(x) STR_HELPER(x)