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

Merge pull request #557 from blckmn/development

Browse source 
Re-added use_unsyncedPwm
This commit is contained in:
J Blackman 2016-06-22 08:08:44 +10:00 • committed by GitHub
commit 5d385d0019
16 changed files with 280 additions and 203 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -180,29 +180,34 @@ static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
static void resetPidProfile(pidProfile_t *pidProfile) static void resetPidProfile(pidProfile_t *pidProfile)
{ {
pidProfile->pidController = 1;
#if (defined(STM32F10X))
pidProfile->pidController = PID_CONTROLLER_MWREWRITE;
#else
pidProfile->pidController = PID_CONTROLLER_LUX_FLOAT;
#endif
pidProfile->P8[ROLL] = 45; pidProfile->P8[ROLL] = 45;
pidProfile->I8[ROLL] = 40; pidProfile->I8[ROLL] = 40;
pidProfile->D8[ROLL] = 15; pidProfile->D8[ROLL] = 18;
pidProfile->P8[PITCH] = 45; pidProfile->P8[PITCH] = 50;
pidProfile->I8[PITCH] = 40; pidProfile->I8[PITCH] = 40;
pidProfile->D8[PITCH] = 15; pidProfile->D8[PITCH] = 18;
pidProfile->P8[YAW] = 90; pidProfile->P8[YAW] = 90;
pidProfile->I8[YAW] = 45; pidProfile->I8[YAW] = 45;
pidProfile->D8[YAW] = 20; pidProfile->D8[YAW] = 20;
pidProfile->P8[PIDALT] = 50; pidProfile->P8[PIDALT] = 50;
pidProfile->I8[PIDALT] = 0; pidProfile->I8[PIDALT] = 0;
pidProfile->D8[PIDALT] = 0; pidProfile->D8[PIDALT] = 0;
pidProfile->P8[PIDPOS] = 15; // POSHOLD_P * 100; pidProfile->P8[PIDPOS] = 15; // POSHOLD_P * 100;
pidProfile->I8[PIDPOS] = 0; // POSHOLD_I * 100; pidProfile->I8[PIDPOS] = 0; // POSHOLD_I * 100;
pidProfile->D8[PIDPOS] = 0; pidProfile->D8[PIDPOS] = 0;
pidProfile->P8[PIDPOSR] = 34; // POSHOLD_RATE_P * 10; pidProfile->P8[PIDPOSR] = 34; // POSHOLD_RATE_P * 10;
pidProfile->I8[PIDPOSR] = 14; // POSHOLD_RATE_I * 100; pidProfile->I8[PIDPOSR] = 14; // POSHOLD_RATE_I * 100;
pidProfile->D8[PIDPOSR] = 53; // POSHOLD_RATE_D * 1000; pidProfile->D8[PIDPOSR] = 53; // POSHOLD_RATE_D * 1000;
pidProfile->P8[PIDNAVR] = 25; // NAV_P * 10; pidProfile->P8[PIDNAVR] = 25; // NAV_P * 10;
pidProfile->I8[PIDNAVR] = 33; // NAV_I * 100; pidProfile->I8[PIDNAVR] = 33; // NAV_I * 100;
pidProfile->D8[PIDNAVR] = 83; // NAV_D * 1000; pidProfile->D8[PIDNAVR] = 83; // NAV_D * 1000;
pidProfile->P8[PIDLEVEL] = 50; pidProfile->P8[PIDLEVEL] = 50;
pidProfile->I8[PIDLEVEL] = 50; pidProfile->I8[PIDLEVEL] = 50;
pidProfile->D8[PIDLEVEL] = 100; pidProfile->D8[PIDLEVEL] = 100;
@ -214,8 +219,8 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX; pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX;
pidProfile->yaw_lpf_hz = 80; pidProfile->yaw_lpf_hz = 80;
pidProfile->rollPitchItermIgnoreRate = 200; pidProfile->rollPitchItermIgnoreRate = 200;
pidProfile->yawItermIgnoreRate = 45; pidProfile->yawItermIgnoreRate = 35;
pidProfile->dterm_lpf_hz = 110; // filtering ON by default pidProfile->dterm_lpf_hz = 50; // filtering ON by default
pidProfile->dynamic_pid = 1; pidProfile->dynamic_pid = 1;
#ifdef GTUNE #ifdef GTUNE
@ -265,7 +270,7 @@ void resetEscAndServoConfig(escAndServoConfig_t *escAndServoConfig)
escAndServoConfig->maxthrottle = 1850; escAndServoConfig->maxthrottle = 1850;
escAndServoConfig->mincommand = 1000; escAndServoConfig->mincommand = 1000;
escAndServoConfig->servoCenterPulse = 1500; escAndServoConfig->servoCenterPulse = 1500;
escAndServoConfig->escDesyncProtection = 10000; escAndServoConfig->escDesyncProtection = 0;
} }
void resetFlight3DConfig(flight3DConfig_t *flight3DConfig) void resetFlight3DConfig(flight3DConfig_t *flight3DConfig)
@ -388,14 +393,14 @@ uint8_t getCurrentControlRateProfile(void)
return currentControlRateProfileIndex; return currentControlRateProfileIndex;
} }
static void setControlRateProfile(uint8_t profileIndex) static void setControlRateProfile(uint8_t profileIndex)
{ {
currentControlRateProfileIndex = profileIndex; currentControlRateProfileIndex = profileIndex;
masterConfig.profile[getCurrentProfile()].activeRateProfile = profileIndex; masterConfig.profile[getCurrentProfile()].activeRateProfile = profileIndex;
currentControlRateProfile = &masterConfig.profile[getCurrentProfile()].controlRateProfile[profileIndex]; currentControlRateProfile = &masterConfig.profile[getCurrentProfile()].controlRateProfile[profileIndex];
} }
controlRateConfig_t *getControlRateConfig(uint8_t profileIndex) controlRateConfig_t *getControlRateConfig(uint8_t profileIndex)
{ {
return &masterConfig.profile[profileIndex].controlRateProfile[masterConfig.profile[profileIndex].activeRateProfile]; return &masterConfig.profile[profileIndex].controlRateProfile[masterConfig.profile[profileIndex].activeRateProfile];
} }
@ -489,10 +494,10 @@ static void resetConf(void)
masterConfig.rxConfig.rssi_channel = 0; masterConfig.rxConfig.rssi_channel = 0;
masterConfig.rxConfig.rssi_scale = RSSI_SCALE_DEFAULT; masterConfig.rxConfig.rssi_scale = RSSI_SCALE_DEFAULT;
masterConfig.rxConfig.rssi_ppm_invert = 0; masterConfig.rxConfig.rssi_ppm_invert = 0;
masterConfig.rxConfig.rcSmoothing = 0; masterConfig.rxConfig.rcSmoothing = 0; // TODO - Cleanup with next EEPROM changes
masterConfig.rxConfig.fpvCamAngleDegrees = 0; masterConfig.rxConfig.fpvCamAngleDegrees = 0;
#ifdef STM32F4 #ifdef STM32F4
masterConfig.rxConfig.max_aux_channel = 99; masterConfig.rxConfig.max_aux_channel = 99;
#else #else
masterConfig.rxConfig.max_aux_channel = 6; masterConfig.rxConfig.max_aux_channel = 6;
#endif #endif
@ -541,11 +546,11 @@ static void resetConf(void)
masterConfig.emf_avoidance = 0; // TODO - needs removal masterConfig.emf_avoidance = 0; // TODO - needs removal
resetPidProfile(&currentProfile->pidProfile); resetPidProfile(&currentProfile->pidProfile);
uint8_t rI; uint8_t rI;
for (rI = 0; rI<MAX_RATEPROFILES; rI++) { for (rI = 0; rI<MAX_RATEPROFILES; rI++) {
resetControlRateConfig(&masterConfig.profile[0].controlRateProfile[rI]); resetControlRateConfig(&masterConfig.profile[0].controlRateProfile[rI]);
} }
resetRollAndPitchTrims(&masterConfig.accelerometerTrims); resetRollAndPitchTrims(&masterConfig.accelerometerTrims);
masterConfig.mag_declination = 0; masterConfig.mag_declination = 0;
@ -887,22 +892,22 @@ void validateAndFixConfig(void)
#if defined(CC3D) && defined(SONAR) && defined(USE_SOFTSERIAL1) && defined(RSSI_ADC_GPIO) #if defined(CC3D) && defined(SONAR) && defined(USE_SOFTSERIAL1) && defined(RSSI_ADC_GPIO)
// shared pin // shared pin
if ((featureConfigured(FEATURE_SONAR) + featureConfigured(FEATURE_SOFTSERIAL) + featureConfigured(FEATURE_RSSI_ADC)) > 1) { if ((featureConfigured(FEATURE_SONAR) + featureConfigured(FEATURE_SOFTSERIAL) + featureConfigured(FEATURE_RSSI_ADC)) > 1) {
featureClear(FEATURE_SONAR); featureClear(FEATURE_SONAR);
featureClear(FEATURE_SOFTSERIAL); featureClear(FEATURE_SOFTSERIAL);
featureClear(FEATURE_RSSI_ADC); featureClear(FEATURE_RSSI_ADC);
} }
#endif #endif
#if defined(COLIBRI_RACE) #if defined(COLIBRI_RACE)
masterConfig.serialConfig.portConfigs[0].functionMask = FUNCTION_MSP; masterConfig.serialConfig.portConfigs[0].functionMask = FUNCTION_MSP;
if(featureConfigured(FEATURE_RX_PARALLEL_PWM) || featureConfigured(FEATURE_RX_MSP)) { if(featureConfigured(FEATURE_RX_PARALLEL_PWM) || featureConfigured(FEATURE_RX_MSP)) {
featureClear(FEATURE_RX_PARALLEL_PWM); featureClear(FEATURE_RX_PARALLEL_PWM);
featureClear(FEATURE_RX_MSP); featureClear(FEATURE_RX_MSP);
featureSet(FEATURE_RX_PPM); featureSet(FEATURE_RX_PPM);
} }
if(featureConfigured(FEATURE_RX_SERIAL)) { if(featureConfigured(FEATURE_RX_SERIAL)) {
masterConfig.serialConfig.portConfigs[2].functionMask = FUNCTION_RX_SERIAL; masterConfig.serialConfig.portConfigs[2].functionMask = FUNCTION_RX_SERIAL;
//masterConfig.rxConfig.serialrx_provider = SERIALRX_SBUS; //masterConfig.rxConfig.serialrx_provider = SERIALRX_SBUS;
} }
#endif #endif
@ -980,11 +985,11 @@ void writeEEPROM(void)
for (wordOffset = 0; wordOffset < sizeof(master_t); wordOffset += 4) { for (wordOffset = 0; wordOffset < sizeof(master_t); wordOffset += 4) {
if (wordOffset % FLASH_PAGE_SIZE == 0) { if (wordOffset % FLASH_PAGE_SIZE == 0) {
#if defined(STM32F40_41xxx) #if defined(STM32F40_41xxx)
status = FLASH_EraseSector(FLASH_Sector_8, VoltageRange_3); //0x08080000 to 0x080A0000 status = FLASH_EraseSector(FLASH_Sector_8, VoltageRange_3); //0x08080000 to 0x080A0000
#elif defined (STM32F411xE) #elif defined (STM32F411xE)
status = FLASH_EraseSector(FLASH_Sector_7, VoltageRange_3); //0x08060000 to 0x08080000 status = FLASH_EraseSector(FLASH_Sector_7, VoltageRange_3); //0x08060000 to 0x08080000
#else #else
status = FLASH_ErasePage(CONFIG_START_FLASH_ADDRESS + wordOffset); status = FLASH_ErasePage(CONFIG_START_FLASH_ADDRESS + wordOffset);
#endif #endif
if (status != FLASH_COMPLETE) { if (status != FLASH_COMPLETE) {
break; break;
@ -1040,12 +1045,12 @@ void changeProfile(uint8_t profileIndex)
beeperConfirmationBeeps(profileIndex + 1); beeperConfirmationBeeps(profileIndex + 1);
} }
void changeControlRateProfile(uint8_t profileIndex) { void changeControlRateProfile(uint8_t profileIndex) {
if (profileIndex > MAX_RATEPROFILES) { if (profileIndex > MAX_RATEPROFILES) {
profileIndex = MAX_RATEPROFILES - 1; profileIndex = MAX_RATEPROFILES - 1;
} }
setControlRateProfile(profileIndex); setControlRateProfile(profileIndex);
activateControlRateConfig(); activateControlRateConfig();
} }
void handleOneshotFeatureChangeOnRestart(void) void handleOneshotFeatureChangeOnRestart(void)

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

@ -35,6 +35,7 @@ typedef struct master_t {
uint16_t motor_pwm_rate; // The update rate of motor outputs (50-498Hz) uint16_t motor_pwm_rate; // The update rate of motor outputs (50-498Hz)
uint16_t servo_pwm_rate; // The update rate of servo outputs (50-498Hz) uint16_t servo_pwm_rate; // The update rate of servo outputs (50-498Hz)
uint8_t motor_pwm_protocol; // Pwm Protocol uint8_t motor_pwm_protocol; // Pwm Protocol
uint8_t use_unsyncedPwm;
#ifdef USE_SERVOS #ifdef USE_SERVOS
servoMixer_t customServoMixer[MAX_SERVO_RULES]; servoMixer_t customServoMixer[MAX_SERVO_RULES];

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

@ -38,7 +38,7 @@
#define PWM_TIMER_MHZ 1 #define PWM_TIMER_MHZ 1
#define PWM_BRUSHED_TIMER_MHZ 8 #define PWM_BRUSHED_TIMER_MHZ 8
#define MULTISHOT_TIMER_MHZ 12 #define MULTISHOT_TIMER_MHZ 72
#define ONESHOT42_TIMER_MHZ 24 #define ONESHOT42_TIMER_MHZ 24
#define ONESHOT125_TIMER_MHZ 8 #define ONESHOT125_TIMER_MHZ 8

3
src/main/drivers/pwm_output.c
View file

@ -19,6 +19,7 @@
#include <stdint.h> #include <stdint.h>
#include <stdlib.h> #include <stdlib.h>
#include <math.h>
#include "platform.h" #include "platform.h"
@ -135,7 +136,7 @@ static void pwmWriteStandard(uint8_t index, uint16_t value)
static void pwmWriteMultiShot(uint8_t index, uint16_t value) static void pwmWriteMultiShot(uint8_t index, uint16_t value)
{ {
*motors[index]->ccr = 60001 * (value - 1000) / 250000 + 60; *motors[index]->ccr = lrintf(((float)(value-1000) / 0.69444f) + 360);
} }
void pwmWriteMotor(uint8_t index, uint16_t value) void pwmWriteMotor(uint8_t index, uint16_t value)

1
src/main/drivers/timer.h
View file

@ -97,6 +97,7 @@ typedef struct {
#define HARDWARE_TIMER_DEFINITION_COUNT 14 #define HARDWARE_TIMER_DEFINITION_COUNT 14
#endif #endif
extern const timerHardware_t timerHardware[]; extern const timerHardware_t timerHardware[];
extern const timerDef_t timerDefinitions[]; extern const timerDef_t timerDefinitions[];

24
src/main/flight/mixer.c
View file

@ -66,10 +66,13 @@ static flight3DConfig_t *flight3DConfig;
static escAndServoConfig_t *escAndServoConfig; static escAndServoConfig_t *escAndServoConfig;
static airplaneConfig_t *airplaneConfig; static airplaneConfig_t *airplaneConfig;
static rxConfig_t *rxConfig; static rxConfig_t *rxConfig;
static bool syncPwm = false;
static mixerMode_e currentMixerMode; static mixerMode_e currentMixerMode;
static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS]; static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
float errorLimiter = 1.0f;
#ifdef USE_SERVOS #ifdef USE_SERVOS
static uint8_t servoRuleCount = 0; static uint8_t servoRuleCount = 0;
static servoMixer_t currentServoMixer[MAX_SERVO_RULES]; static servoMixer_t currentServoMixer[MAX_SERVO_RULES];
@ -418,13 +421,15 @@ void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMotorMixers, se
} }
} }
void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration) void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration, bool use_unsyncedPwm)
{ {
int i; int i;
motorCount = 0; motorCount = 0;
servoCount = pwmOutputConfiguration->servoCount; servoCount = pwmOutputConfiguration->servoCount;
syncPwm = use_unsyncedPwm;
if (currentMixerMode == MIXER_CUSTOM || currentMixerMode == MIXER_CUSTOM_TRI || currentMixerMode == MIXER_CUSTOM_AIRPLANE) { if (currentMixerMode == MIXER_CUSTOM || currentMixerMode == MIXER_CUSTOM_TRI || currentMixerMode == MIXER_CUSTOM_AIRPLANE) {
// load custom mixer into currentMixer // load custom mixer into currentMixer
for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) { for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
@ -633,13 +638,6 @@ void writeServos(void)
} }
#endif #endif
static bool syncPwm = false;
void syncMotors(bool enabled)
{
syncPwm = enabled;
}
void writeMotors(void) void writeMotors(void)
{ {
uint8_t i; uint8_t i;
@ -823,7 +821,6 @@ void mixTable(void)
throttleRange = throttleMax - throttleMin; throttleRange = throttleMax - throttleMin;
if (rollPitchYawMixRange > throttleRange) { if (rollPitchYawMixRange > throttleRange) {
motorLimitReached = true;
mixReduction = qConstruct(throttleRange, rollPitchYawMixRange); mixReduction = qConstruct(throttleRange, rollPitchYawMixRange);
for (i = 0; i < motorCount; i++) { for (i = 0; i < motorCount; i++) {
@ -831,14 +828,15 @@ void mixTable(void)
} }
// Get the maximum correction by setting offset to center // Get the maximum correction by setting offset to center
if (!escAndServoConfig->escDesyncProtection) throttleMin = throttleMax = throttleMin + (throttleRange / 2); if (!escAndServoConfig->escDesyncProtection) throttleMin = throttleMax = throttleMin + (throttleRange / 2);
if (debugMode == DEBUG_AIRMODE && i < 3) debug[1] = rollPitchYawMixRange;
} else { } else {
motorLimitReached = false;
throttleMin = throttleMin + (rollPitchYawMixRange / 2); throttleMin = throttleMin + (rollPitchYawMixRange / 2);
throttleMax = throttleMax - (rollPitchYawMixRange / 2); throttleMax = throttleMax - (rollPitchYawMixRange / 2);
} }
// adjust feedback to scale PID error inputs to our limitations.
errorLimiter = constrainf(((float)throttleRange / rollPitchYawMixRange), 0.1f, 1.0f);
if (debugMode == DEBUG_AIRMODE) debug[1] = errorLimiter * 100;
// Now add in the desired throttle, but keep in a range that doesn't clip adjusted // Now add in the desired throttle, but keep in a range that doesn't clip adjusted
// roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips. // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips.
for (i = 0; i < motorCount; i++) { for (i = 0; i < motorCount; i++) {

1
src/main/flight/mixer.h
View file

@ -189,7 +189,6 @@ void filterServos(void);
extern int16_t motor[MAX_SUPPORTED_MOTORS]; extern int16_t motor[MAX_SUPPORTED_MOTORS];
extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
bool motorLimitReached;
struct escAndServoConfig_s; struct escAndServoConfig_s;
struct rxConfig_s; struct rxConfig_s;

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

@ -49,8 +49,9 @@
#include "config/runtime_config.h" #include "config/runtime_config.h"
extern uint8_t motorCount; extern uint8_t motorCount;
extern bool motorLimitReached;
uint32_t targetPidLooptime; uint32_t targetPidLooptime;
extern float errorLimiter;
extern float angleRate[3], angleRateSmooth[2];
int16_t axisPID[3]; int16_t axisPID[3];
@ -61,13 +62,13 @@ int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
// PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction // PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction
uint8_t PIDweight[3]; uint8_t PIDweight[3];
static int32_t errorGyroI[3], errorGyroILimit[3]; static int32_t errorGyroI[3];
#ifndef SKIP_PID_LUXFLOAT #ifndef SKIP_PID_LUXFLOAT
static float errorGyroIf[3], errorGyroIfLimit[3]; static float errorGyroIf[3];
#endif #endif
static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRateConfig_t *controlRateConfig, static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig); const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig);
pidControllerFuncPtr pid_controller = pidMultiWiiRewrite; // which pid controller are we using, defaultMultiWii pidControllerFuncPtr pid_controller = pidMultiWiiRewrite; // which pid controller are we using, defaultMultiWii
@ -75,38 +76,14 @@ void setTargetPidLooptime(uint8_t pidProcessDenom) {
targetPidLooptime = targetLooptime * pidProcessDenom; targetPidLooptime = targetLooptime * pidProcessDenom;
} }
float calculateRate(int axis, const controlRateConfig_t *controlRateConfig) { uint16_t getDynamicKi(int axis, const pidProfile_t *pidProfile, int32_t angleRate) {
float angleRate;
if (isSuperExpoActive()) {
float rcFactor = (axis == YAW) ? (ABS(rcCommand[axis]) / (500.0f * (controlRateConfig->rcYawRate8 / 100.0f))) : (ABS(rcCommand[axis]) / (500.0f * (controlRateConfig->rcRate8 / 100.0f)));
rcFactor = 1.0f / (constrainf(1.0f - (rcFactor * (controlRateConfig->rates[axis] / 100.0f)), 0.01f, 1.00f));
angleRate = rcFactor * ((27 * rcCommand[axis]) / 16.0f);
} else {
angleRate = (float)((controlRateConfig->rates[axis] + 27) * rcCommand[axis]) / 16.0f;
}
return constrainf(angleRate, -8190.0f, 8190.0f); // Rate limit protection
}
uint16_t getDynamicKp(int axis, const 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;
}
uint16_t getDynamicKi(int axis, const pidProfile_t *pidProfile) {
uint16_t dynamicKi; uint16_t dynamicKi;
uint16_t resetRate; uint16_t resetRate;
resetRate = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; resetRate = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate;
uint32_t dynamicFactor = ((resetRate << 16) / (resetRate + ABS(gyroADC[axis]))) >> 8; uint16_t dynamicFactor = (1 << 8) - constrain((ABS(angleRate) << 6) / resetRate, 0, 1 << 8);
dynamicKi = (pidProfile->I8[axis] * dynamicFactor) >> 8; dynamicKi = (pidProfile->I8[axis] * dynamicFactor) >> 8;
return dynamicKi; return dynamicKi;
@ -137,12 +114,12 @@ static filterStatePt1_t deltaFilterState[3];
static filterStatePt1_t yawFilterState; static filterStatePt1_t yawFilterState;
#ifndef SKIP_PID_LUXFLOAT #ifndef SKIP_PID_LUXFLOAT
static void pidLuxFloat(const pidProfile_t *pidProfile, const controlRateConfig_t *controlRateConfig, static void pidLuxFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig) const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig)
{ {
float RateError, AngleRate, gyroRate; float RateError, gyroRate, RateErrorSmooth;
float ITerm,PTerm,DTerm; float ITerm,PTerm,DTerm;
static float lastRate[3]; static float lastRateError[2];
float delta; float delta;
int axis; int axis;
float horizonLevelStrength = 1; float horizonLevelStrength = 1;
@ -171,40 +148,43 @@ static void pidLuxFloat(const pidProfile_t *pidProfile, const controlRateConfig_
// ----------PID controller---------- // ----------PID controller----------
for (axis = 0; axis < 3; axis++) { for (axis = 0; axis < 3; axis++) {
// ACRO mode, control is GYRO based, direct sticks control is applied to rate PID // Yaw control is GYRO based, direct sticks control is applied to rate PID
AngleRate = calculateRate(axis, controlRateConfig);
if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) { if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) {
// calculate error angle and limit the angle to the max inclination // calculate error angle and limit the angle to the max inclination
#ifdef GPS #ifdef GPS
const float errorAngle = (constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), const float errorAngle = (constrain(2 * rcCommandSmooth[axis] + GPS_angle[axis], -((int) max_angle_inclination),
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]); // 16 bits is ok here +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]); // 16 bits is ok here
#else #else
const float errorAngle = (constrain(2 * rcCommand[axis], -((int) max_angle_inclination), const float errorAngle = (constrain(2 * rcCommandSmooth[axis], -((int) max_angle_inclination),
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]); // 16 bits is ok here +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]); // 16 bits is ok here
#endif #endif
if (FLIGHT_MODE(ANGLE_MODE)) { if (FLIGHT_MODE(ANGLE_MODE)) {
// ANGLE mode - control is angle based, so control loop is needed // ANGLE mode - control is angle based, so control loop is needed
AngleRate = errorAngle * pidProfile->P8[PIDLEVEL] / 16.0f; angleRate[axis] = errorAngle * pidProfile->P8[PIDLEVEL] / 16.0f;
} else { } else {
// HORIZON mode - direct sticks control is applied to rate PID // HORIZON mode - direct sticks control is applied to rate PID
// mix up angle error to desired AngleRate to add a little auto-level feel // mix up angle error to desired AngleRate to add a little auto-level feel
AngleRate += errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 16.0f; angleRate[axis] = angleRateSmooth[axis] + (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 16.0f);
} }
} }
gyroRate = gyroADCf[axis] / 4.0f; // gyro output scaled to rewrite scale gyroRate = gyroADCf[axis] / 4.0f; // gyro output scaled to old rewrite scale
// --------low-level gyro-based PID. ---------- // --------low-level gyro-based PID. ----------
// Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
// -----calculate scaled error.AngleRates // -----calculate scaled error.AngleRates
// 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[axis] - gyroRate;
uint16_t kP = (pidProfile->dynamic_pid) ? getDynamicKp(axis, pidProfile) : pidProfile->P8[axis]; // Smoothed Error for Derivative
if (flightModeFlags && axis != YAW) {
RateErrorSmooth = RateError;
} else {
RateErrorSmooth = angleRateSmooth[axis] - gyroRate;
}
// -----calculate P component // -----calculate P component
PTerm = luxPTermScale * RateError * kP * tpaFactor; PTerm = luxPTermScale * RateError * pidProfile->P8[axis] * 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
if((motorCount >= 4 && pidProfile->yaw_p_limit) && axis == YAW) { if((motorCount >= 4 && pidProfile->yaw_p_limit) && axis == YAW) {
@ -212,15 +192,9 @@ static void pidLuxFloat(const pidProfile_t *pidProfile, const controlRateConfig_
} }
// -----calculate I component. // -----calculate I component.
uint16_t kI = (pidProfile->dynamic_pid) ? getDynamicKi(axis, pidProfile) : pidProfile->I8[axis]; uint16_t kI = (pidProfile->dynamic_pid) ? getDynamicKi(axis, pidProfile, (int32_t)angleRate[axis]) : pidProfile->I8[axis];
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + luxITermScale * RateError * getdT() * kI, -250.0f, 250.0f); errorGyroIf[axis] = constrainf(errorGyroIf[axis] + luxITermScale * errorLimiter * RateError * getdT() * kI, -250.0f, 250.0f);
if (motorLimitReached) {
errorGyroIf[axis] = constrainf(errorGyroIf[axis], -errorGyroIfLimit[axis], errorGyroIfLimit[axis]);
} else {
errorGyroIfLimit[axis] = ABS(errorGyroIf[axis]);
}
// limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
// I coefficient (I8) moved before integration to make limiting independent from PID settings // I coefficient (I8) moved before integration to make limiting independent from PID settings
@ -241,8 +215,8 @@ static void pidLuxFloat(const pidProfile_t *pidProfile, const controlRateConfig_
DTerm = 0.0f; // needed for blackbox DTerm = 0.0f; // needed for blackbox
} else { } else {
delta = -(gyroRate - lastRate[axis]); delta = RateErrorSmooth - lastRateError[axis];
lastRate[axis] = gyroRate; lastRateError[axis] = RateErrorSmooth;
// 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());
@ -271,13 +245,13 @@ static void pidLuxFloat(const pidProfile_t *pidProfile, const controlRateConfig_
} }
#endif #endif
static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig) const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig)
{ {
int axis; int axis;
int32_t PTerm, ITerm, DTerm, delta; int32_t PTerm, ITerm, DTerm, delta;
static int32_t lastRate[3]; static int32_t lastRateError[3];
int32_t AngleRateTmp, RateError, gyroRate; int32_t AngleRateTmp, AngleRateTmpSmooth, RateError, gyroRate, RateErrorSmooth;
int8_t horizonLevelStrength = 100; int8_t horizonLevelStrength = 100;
@ -297,15 +271,16 @@ static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRate
for (axis = 0; axis < 3; axis++) { for (axis = 0; axis < 3; axis++) {
// -----Get the desired angle rate depending on flight mode // -----Get the desired angle rate depending on flight mode
AngleRateTmp = (int32_t)calculateRate(axis, controlRateConfig); AngleRateTmp = (int32_t)angleRate[axis];
if (axis != YAW) AngleRateTmpSmooth = (int32_t)angleRateSmooth[axis];
if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) { if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) {
// calculate error angle and limit the angle to max configured inclination // calculate error angle and limit the angle to max configured inclination
#ifdef GPS #ifdef GPS
const int32_t errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), const int32_t errorAngle = constrain(2 * rcCommandSmooth[axis] + GPS_angle[axis], -((int) max_angle_inclination),
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis];
#else #else
const int32_t errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination), const int32_t errorAngle = constrain(2 * rcCommandSmooth[axis], -((int) max_angle_inclination),
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis];
#endif #endif
if (FLIGHT_MODE(ANGLE_MODE)) { if (FLIGHT_MODE(ANGLE_MODE)) {
@ -314,7 +289,7 @@ static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRate
} else { } else {
// HORIZON mode - mix up angle error to desired AngleRateTmp to add a little auto-level feel, // HORIZON mode - mix up angle error to desired AngleRateTmp to add a little auto-level feel,
// horizonLevelStrength is scaled to the stick input // horizonLevelStrength is scaled to the stick input
AngleRateTmp += (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 100) >> 4; AngleRateTmp = AngleRateTmpSmooth + ((errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 100) >> 4);
} }
} }
@ -323,12 +298,18 @@ static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRate
// -----calculate scaled error.AngleRates // -----calculate scaled error.AngleRates
// multiplication of rcCommand corresponds to changing the sticks scaling here // multiplication of rcCommand corresponds to changing the sticks scaling here
gyroRate = gyroADC[axis] / 4; gyroRate = gyroADC[axis] / 4;
RateError = AngleRateTmp - gyroRate; RateError = AngleRateTmp - gyroRate;
uint16_t kP = (pidProfile->dynamic_pid) ? getDynamicKp(axis, pidProfile) : pidProfile->P8[axis]; // Smoothed Error for Derivative
if (flightModeFlags && axis != YAW) {
RateErrorSmooth = RateError;
} else {
RateErrorSmooth = AngleRateTmpSmooth - gyroRate;
}
// -----calculate P component // -----calculate P component
PTerm = (RateError * kP * PIDweight[axis] / 100) >> 7; PTerm = (RateError * pidProfile->P8[axis] * 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
if((motorCount >= 4 && pidProfile->yaw_p_limit) && axis == YAW) { if((motorCount >= 4 && pidProfile->yaw_p_limit) && axis == YAW) {
@ -340,20 +321,16 @@ static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRate
// Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used. // Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used.
// Time correction (to avoid different I scaling for different builds based on average cycle time) // Time correction (to avoid different I scaling for different builds based on average cycle time)
// is normalized to cycle time = 2048. // is normalized to cycle time = 2048.
uint16_t kI = (pidProfile->dynamic_pid) ? getDynamicKi(axis, pidProfile) : pidProfile->I8[axis]; uint16_t kI = (pidProfile->dynamic_pid) ? getDynamicKi(axis, pidProfile, AngleRateTmp) : pidProfile->I8[axis];
errorGyroI[axis] = errorGyroI[axis] + ((RateError * (uint16_t)targetPidLooptime) >> 11) * kI; int32_t rateErrorLimited = errorLimiter * RateError;
errorGyroI[axis] = errorGyroI[axis] + ((rateErrorLimited * (uint16_t)targetPidLooptime) >> 11) * kI;
// limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
// I coefficient (I8) moved before integration to make limiting independent from PID settings // I coefficient (I8) moved before integration to make limiting independent from PID settings
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 (motorLimitReached) {
errorGyroI[axis] = constrain(errorGyroI[axis], -errorGyroILimit[axis], errorGyroILimit[axis]);
} else {
errorGyroILimit[axis] = ABS(errorGyroI[axis]);
}
ITerm = errorGyroI[axis] >> 13; ITerm = errorGyroI[axis] >> 13;
//-----calculate D-term //-----calculate D-term
@ -371,8 +348,8 @@ static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRate
DTerm = 0; // needed for blackbox DTerm = 0; // needed for blackbox
} else { } else {
delta = -(gyroRate - lastRate[axis]); delta = RateErrorSmooth - lastRateError[axis];
lastRate[axis] = gyroRate; lastRateError[axis] = RateErrorSmooth;
// 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;

4
src/main/flight/pid.h
View file

@ -86,8 +86,8 @@ typedef struct pidProfile_s {
struct controlRateConfig_s; struct controlRateConfig_s;
union rollAndPitchTrims_u; union rollAndPitchTrims_u;
struct rxConfig_s; struct rxConfig_s;
typedef void (*pidControllerFuncPtr)(const pidProfile_t *pidProfile, const struct controlRateConfig_s *controlRateConfig, typedef void (*pidControllerFuncPtr)(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
uint16_t max_angle_inclination, const union rollAndPitchTrims_u *angleTrim, const struct rxConfig_s *rxConfig); // pid controller function prototype const union rollAndPitchTrims_u *angleTrim, const struct rxConfig_s *rxConfig); // pid controller function prototype
extern int16_t axisPID[XYZ_AXIS_COUNT]; extern int16_t axisPID[XYZ_AXIS_COUNT];
extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];

1
src/main/io/rc_controls.c
View file

@ -68,6 +68,7 @@ static pidProfile_t *pidProfile;
static bool isUsingSticksToArm = true; static bool isUsingSticksToArm = true;
int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW
int16_t rcCommandSmooth[4];
uint32_t rcModeActivationMask; // one bit per mode defined in boxId_e uint32_t rcModeActivationMask; // one bit per mode defined in boxId_e

1
src/main/io/rc_controls.h
View file

@ -147,6 +147,7 @@ typedef struct controlRateConfig_s {
} controlRateConfig_t; } controlRateConfig_t;
extern int16_t rcCommand[4]; extern int16_t rcCommand[4];
extern int16_t rcCommandSmooth[4];
typedef struct rcControlsConfig_s { typedef struct rcControlsConfig_s {
uint8_t deadband; // introduce a deadband around the stick center for pitch and roll axis. Must be greater than zero. uint8_t deadband; // introduce a deadband around the stick center for pitch and roll axis. Must be greater than zero.

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

@ -301,8 +301,8 @@ const clicmd_t cmdTable[] = {
"[<index>]\r\n", cliPlaySound), "[<index>]\r\n", cliPlaySound),
CLI_COMMAND_DEF("profile", "change profile", CLI_COMMAND_DEF("profile", "change profile",
"[<index>]", cliProfile), "[<index>]", cliProfile),
CLI_COMMAND_DEF("rateprofile", "change rate profile", "[<index>]", cliRateProfile), CLI_COMMAND_DEF("rateprofile", "change rate profile", "[<index>]", cliRateProfile),
CLI_COMMAND_DEF("resource", "view currently used resources", NULL, cliResource), CLI_COMMAND_DEF("resource", "view currently used resources", NULL, cliResource),
CLI_COMMAND_DEF("rxrange", "configure rx channel ranges", NULL, cliRxRange), CLI_COMMAND_DEF("rxrange", "configure rx channel ranges", NULL, cliRxRange),
CLI_COMMAND_DEF("rxfail", "show/set rx failsafe settings", NULL, cliRxFail), CLI_COMMAND_DEF("rxfail", "show/set rx failsafe settings", NULL, cliRxFail),
CLI_COMMAND_DEF("save", "save and reboot", NULL, cliSave), CLI_COMMAND_DEF("save", "save and reboot", NULL, cliSave),
@ -413,20 +413,20 @@ static const char * const lookupTableGyroLpf[] = {
}; };
static const char * const lookupTableAccHardware[] = { static const char * const lookupTableAccHardware[] = {
"AUTO", "AUTO",
"NONE", "NONE",
"ADXL345", "ADXL345",
"MPU6050", "MPU6050",
"MMA8452", "MMA8452",
"BMA280", "BMA280",
"LSM303DLHC", "LSM303DLHC",
"MPU6000", "MPU6000",
"MPU6500", "MPU6500",
"FAKE" "FAKE"
}; };
static const char * const lookupTableBaroHardware[] = { static const char * const lookupTableBaroHardware[] = {
"AUTO", "AUTO",
"NONE", "NONE",
"BMP085", "BMP085",
"MS5611", "MS5611",
@ -491,7 +491,7 @@ typedef enum {
TABLE_ACC_HARDWARE, TABLE_ACC_HARDWARE,
TABLE_BARO_HARDWARE, TABLE_BARO_HARDWARE,
TABLE_MAG_HARDWARE, TABLE_MAG_HARDWARE,
TABLE_DEBUG, TABLE_DEBUG,
TABLE_SUPEREXPO_YAW, TABLE_SUPEREXPO_YAW,
TABLE_MOTOR_PWM_PROTOCOL, TABLE_MOTOR_PWM_PROTOCOL,
#ifdef OSD #ifdef OSD
@ -542,7 +542,7 @@ typedef enum {
// value section // value section
MASTER_VALUE = (0 << VALUE_SECTION_OFFSET), MASTER_VALUE = (0 << VALUE_SECTION_OFFSET),
PROFILE_VALUE = (1 << VALUE_SECTION_OFFSET), PROFILE_VALUE = (1 << VALUE_SECTION_OFFSET),
PROFILE_RATE_VALUE = (2 << VALUE_SECTION_OFFSET), PROFILE_RATE_VALUE = (2 << VALUE_SECTION_OFFSET),
// value mode // value mode
MODE_DIRECT = (0 << VALUE_MODE_OFFSET), MODE_DIRECT = (0 << VALUE_MODE_OFFSET),
MODE_LOOKUP = (1 << VALUE_MODE_OFFSET) MODE_LOOKUP = (1 << VALUE_MODE_OFFSET)
@ -584,7 +584,6 @@ const clivalue_t valueTable[] = {
{ "rssi_scale", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.rssi_scale, .config.minmax = { RSSI_SCALE_MIN, RSSI_SCALE_MAX } }, { "rssi_scale", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.rssi_scale, .config.minmax = { RSSI_SCALE_MIN, RSSI_SCALE_MAX } },
{ "rssi_ppm_invert", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rxConfig.rssi_ppm_invert, .config.lookup = { TABLE_OFF_ON } }, { "rssi_ppm_invert", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rxConfig.rssi_ppm_invert, .config.lookup = { TABLE_OFF_ON } },
{ "input_filtering_mode", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.inputFilteringMode, .config.lookup = { TABLE_OFF_ON } }, { "input_filtering_mode", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.inputFilteringMode, .config.lookup = { TABLE_OFF_ON } },
{ "rc_smoothing", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rxConfig.rcSmoothing, .config.lookup = { TABLE_OFF_ON } },
{ "roll_yaw_cam_mix_degrees", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.fpvCamAngleDegrees, .config.minmax = { 0, 50 } }, { "roll_yaw_cam_mix_degrees", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.fpvCamAngleDegrees, .config.minmax = { 0, 50 } },
{ "max_aux_channels", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.max_aux_channel, .config.minmax = { 0, 13 } }, { "max_aux_channels", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.max_aux_channel, .config.minmax = { 0, 13 } },
{ "debug_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.debug_mode, .config.lookup = { TABLE_DEBUG } }, { "debug_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.debug_mode, .config.lookup = { TABLE_DEBUG } },
@ -603,8 +602,9 @@ const clivalue_t valueTable[] = {
#ifdef CC3D #ifdef CC3D
{ "enable_buzzer_p6", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.use_buzzer_p6, .config.lookup = { TABLE_OFF_ON } }, { "enable_buzzer_p6", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.use_buzzer_p6, .config.lookup = { TABLE_OFF_ON } },
#endif #endif
{ "use_unsynced_pwm", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.use_unsyncedPwm, .config.lookup = { TABLE_OFF_ON } },
{ "motor_pwm_protocol", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.motor_pwm_protocol, .config.lookup = { TABLE_MOTOR_PWM_PROTOCOL } }, { "motor_pwm_protocol", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.motor_pwm_protocol, .config.lookup = { TABLE_MOTOR_PWM_PROTOCOL } },
{ "motor_pwm_rate", VAR_UINT16 | MASTER_VALUE, &masterConfig.motor_pwm_rate, .config.minmax = { 0, 32000 } }, { "motor_pwm_rate", VAR_UINT16 | MASTER_VALUE, &masterConfig.motor_pwm_rate, .config.minmax = { 200, 32000 } },
{ "servo_pwm_rate", VAR_UINT16 | MASTER_VALUE, &masterConfig.servo_pwm_rate, .config.minmax = { 50, 498 } }, { "servo_pwm_rate", VAR_UINT16 | MASTER_VALUE, &masterConfig.servo_pwm_rate, .config.minmax = { 50, 498 } },
{ "disarm_kill_switch", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.disarm_kill_switch, .config.lookup = { TABLE_OFF_ON } }, { "disarm_kill_switch", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.disarm_kill_switch, .config.lookup = { TABLE_OFF_ON } },
@ -755,9 +755,9 @@ 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", VAR_INT16 | 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 } },
{ "dynamic_pid", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.dynamic_pid, .config.lookup = { TABLE_OFF_ON } }, { "dynamic_iterm", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.dynamic_pid, .config.lookup = { TABLE_OFF_ON } },
{ "iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermIgnoreRate, .config.minmax = {50, 1000 } }, { "iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermIgnoreRate, .config.minmax = {15, 1000 } },
{ "yaw_iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermIgnoreRate, .config.minmax = {25, 1000 } }, { "yaw_iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermIgnoreRate, .config.minmax = {15, 1000 } },
{ "yaw_lowpass", VAR_UINT16 | 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 } },
@ -2122,10 +2122,9 @@ static void cliDump(char *cmdline)
cliDumpProfile(masterConfig.current_profile_index); cliDumpProfile(masterConfig.current_profile_index);
} }
if (dumpMask & DUMP_RATES) { if (dumpMask & DUMP_RATES) {
cliDumpRateProfile(currentProfile->activeRateProfile); cliDumpRateProfile(currentProfile->activeRateProfile);
} }
} }
void cliDumpProfile(uint8_t profileIndex) { void cliDumpProfile(uint8_t profileIndex) {
@ -2148,9 +2147,9 @@ void cliDumpRateProfile(uint8_t rateProfileIndex) {
return; return;
changeControlRateProfile(rateProfileIndex); changeControlRateProfile(rateProfileIndex);
cliPrint("\r\n# rateprofile\r\n"); cliPrint("\r\n# rateprofile\r\n");
cliRateProfile(""); cliRateProfile("");
printSectionBreak(); printSectionBreak();
dumpValues(PROFILE_RATE_VALUE); dumpValues(PROFILE_RATE_VALUE);
} }
@ -2267,11 +2266,11 @@ static void cliBeeper(char *cmdline)
if (len == 0) { if (len == 0) {
cliPrintf("Disabled:"); cliPrintf("Disabled:");
for (int i = 0; ; i++) { for (int i = 0; ; i++) {
if (i == beeperCount-2){ if (i == beeperCount-2){
if (mask == 0) if (mask == 0)
cliPrint(" none"); cliPrint(" none");
break; break;
} }
if (mask & (1 << i)) if (mask & (1 << i))
cliPrintf(" %s", beeperNameForTableIndex(i)); cliPrintf(" %s", beeperNameForTableIndex(i));
} }
@ -2300,7 +2299,7 @@ static void cliBeeper(char *cmdline)
if (i == BEEPER_ALL-1) if (i == BEEPER_ALL-1)
beeperOffSetAll(beeperCount-2); beeperOffSetAll(beeperCount-2);
else else
if (i == BEEPER_PREFERENCE-1) if (i == BEEPER_PREFERENCE-1)
setBeeperOffMask(getPreferredBeeperOffMask()); setBeeperOffMask(getPreferredBeeperOffMask());
else { else {
mask = 1 << i; mask = 1 << i;
@ -2312,7 +2311,7 @@ static void cliBeeper(char *cmdline)
if (i == BEEPER_ALL-1) if (i == BEEPER_ALL-1)
beeperOffClearAll(); beeperOffClearAll();
else else
if (i == BEEPER_PREFERENCE-1) if (i == BEEPER_PREFERENCE-1)
setPreferredBeeperOffMask(getBeeperOffMask()); setPreferredBeeperOffMask(getBeeperOffMask());
else { else {
mask = 1 << i; mask = 1 << i;
@ -2521,19 +2520,19 @@ static void cliProfile(char *cmdline)
} }
} }
static void cliRateProfile(char *cmdline) { static void cliRateProfile(char *cmdline) {
int i; int i;
if (isEmpty(cmdline)) { if (isEmpty(cmdline)) {
cliPrintf("rateprofile %d\r\n", getCurrentControlRateProfile()); cliPrintf("rateprofile %d\r\n", getCurrentControlRateProfile());
return; return;
} else { } else {
i = atoi(cmdline); i = atoi(cmdline);
if (i >= 0 && i < MAX_RATEPROFILES) { if (i >= 0 && i < MAX_RATEPROFILES) {
changeControlRateProfile(i); changeControlRateProfile(i);
cliRateProfile(""); cliRateProfile("");
} }
} }
} }
static void cliReboot(void) { static void cliReboot(void) {
@ -2598,9 +2597,9 @@ static void cliPrintVar(const clivalue_t *var, uint32_t full)
ptr = ((uint8_t *)ptr) + (sizeof(profile_t) * masterConfig.current_profile_index); ptr = ((uint8_t *)ptr) + (sizeof(profile_t) * masterConfig.current_profile_index);
} }
if ((var->type & VALUE_SECTION_MASK) == PROFILE_RATE_VALUE) { if ((var->type & VALUE_SECTION_MASK) == PROFILE_RATE_VALUE) {
ptr = ((uint8_t *)ptr) + (sizeof(profile_t) * masterConfig.current_profile_index) + (sizeof(controlRateConfig_t) * getCurrentControlRateProfile()); ptr = ((uint8_t *)ptr) + (sizeof(profile_t) * masterConfig.current_profile_index) + (sizeof(controlRateConfig_t) * getCurrentControlRateProfile());
} }
switch (var->type & VALUE_TYPE_MASK) { switch (var->type & VALUE_TYPE_MASK) {
case VAR_UINT8: case VAR_UINT8:
@ -2671,9 +2670,9 @@ static void cliSetVar(const clivalue_t *var, const int_float_value_t value)
if ((var->type & VALUE_SECTION_MASK) == PROFILE_VALUE) { if ((var->type & VALUE_SECTION_MASK) == PROFILE_VALUE) {
ptr = ((uint8_t *)ptr) + (sizeof(profile_t) * masterConfig.current_profile_index); ptr = ((uint8_t *)ptr) + (sizeof(profile_t) * masterConfig.current_profile_index);
} }
if ((var->type & VALUE_SECTION_MASK) == PROFILE_RATE_VALUE) { if ((var->type & VALUE_SECTION_MASK) == PROFILE_RATE_VALUE) {
ptr = ((uint8_t *)ptr) + (sizeof(profile_t) * masterConfig.current_profile_index) + (sizeof(controlRateConfig_t) * getCurrentControlRateProfile()); ptr = ((uint8_t *)ptr) + (sizeof(profile_t) * masterConfig.current_profile_index) + (sizeof(controlRateConfig_t) * getCurrentControlRateProfile());
} }
switch (var->type & VALUE_TYPE_MASK) { switch (var->type & VALUE_TYPE_MASK) {
case VAR_UINT8: case VAR_UINT8:
@ -2734,7 +2733,7 @@ static void cliSet(char *cmdline)
if (strncasecmp(cmdline, valueTable[i].name, strlen(valueTable[i].name)) == 0 && variableNameLength == strlen(valueTable[i].name)) { if (strncasecmp(cmdline, valueTable[i].name, strlen(valueTable[i].name)) == 0 && variableNameLength == strlen(valueTable[i].name)) {
bool changeValue = false; bool changeValue = false;
int_float_value_t tmp = { 0 }; int_float_value_t tmp = { 0 };
switch (valueTable[i].type & VALUE_MODE_MASK) { switch (valueTable[i].type & VALUE_MODE_MASK) {
case MODE_DIRECT: { case MODE_DIRECT: {
int32_t value = 0; int32_t value = 0;
@ -2810,7 +2809,7 @@ static void cliGet(char *cmdline)
if (matchedCommands) { if (matchedCommands) {
return; return;
} }
cliPrint("Invalid name\r\n"); cliPrint("Invalid name\r\n");

56
src/main/io/serial_msp.c
View file

@ -1257,6 +1257,36 @@ static bool processOutCommand(uint8_t cmdMSP)
serialize8(masterConfig.sensorAlignmentConfig.acc_align); serialize8(masterConfig.sensorAlignmentConfig.acc_align);
serialize8(masterConfig.sensorAlignmentConfig.mag_align); serialize8(masterConfig.sensorAlignmentConfig.mag_align);
break; break;
case MSP_PID_ADVANCED_CONFIG :
headSerialReply(6);
serialize8(masterConfig.gyro_sync_denom);
serialize8(masterConfig.pid_process_denom);
serialize8(masterConfig.use_unsyncedPwm);
serialize8(masterConfig.motor_pwm_protocol);
serialize16(masterConfig.motor_pwm_rate);
break;
case MSP_FILTER_CONFIG :
headSerialReply(5);
serialize8(masterConfig.gyro_soft_lpf_hz);
serialize16(currentProfile->pidProfile.dterm_lpf_hz);
serialize16(currentProfile->pidProfile.yaw_lpf_hz);
break;
case MSP_ADVANCED_TUNING:
headSerialReply(3 * 2);
serialize16(currentProfile->pidProfile.rollPitchItermIgnoreRate);
serialize16(currentProfile->pidProfile.yawItermIgnoreRate);
serialize16(currentProfile->pidProfile.yaw_p_limit);
break;
case MSP_TEMPORARY_COMMANDS:
headSerialReply(1);
serialize8(currentControlRateProfile->rcYawRate8);
break;
case MSP_SENSOR_CONFIG:
headSerialReply(3);
serialize8(masterConfig.acc_hardware);
serialize8(masterConfig.baro_hardware);
serialize8(masterConfig.mag_hardware);
break;
default: default:
return false; return false;
@ -1803,6 +1833,32 @@ static bool processInCommand(void)
// proceed as usual with MSP commands // proceed as usual with MSP commands
break; break;
#endif #endif
case MSP_SET_PID_ADVANCED_CONFIG :
masterConfig.gyro_sync_denom = read8();
masterConfig.pid_process_denom = read8();
masterConfig.use_unsyncedPwm = read8();
masterConfig.motor_pwm_protocol = read8();
masterConfig.motor_pwm_rate = read16();
break;
case MSP_SET_FILTER_CONFIG :
masterConfig.gyro_soft_lpf_hz = read8();
currentProfile->pidProfile.dterm_lpf_hz = read16();
currentProfile->pidProfile.yaw_lpf_hz = read16();
break;
case MSP_SET_ADVANCED_TUNING:
currentProfile->pidProfile.rollPitchItermIgnoreRate = read16();
currentProfile->pidProfile.yawItermIgnoreRate = read16();
currentProfile->pidProfile.yaw_p_limit = read16();
break;
case MSP_SET_TEMPORARY_COMMANDS:
currentControlRateProfile->rcYawRate8 = read8();
break;
case MSP_SET_SENSOR_CONFIG:
masterConfig.acc_hardware = read8();
masterConfig.baro_hardware = read8();
masterConfig.mag_hardware = read8();
break;
default: default:
// we do not know how to handle the (valid) message, indicate error MSP $M! // we do not know how to handle the (valid) message, indicate error MSP $M!
return false; return false;

16
src/main/io/serial_msp.h
View file

@ -193,6 +193,22 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER;
// DEPRECATED - Use MSP_BUILD_INFO instead // DEPRECATED - Use MSP_BUILD_INFO instead
#define MSP_BF_BUILD_INFO 69 //out message build date as well as some space for future expansion #define MSP_BF_BUILD_INFO 69 //out message build date as well as some space for future expansion
// Betaflight Additional Commands
#define MSP_PID_ADVANCED_CONFIG 90
#define MSP_SET_PID_ADVANCED_CONFIG 91
#define MSP_FILTER_CONFIG 92
#define MSP_SET_FILTER_CONFIG 93
#define MSP_ADVANCED_TUNING 94
#define MSP_SET_ADVANCED_TUNING 95
#define MSP_SENSOR_CONFIG 96
#define MSP_SET_SENSOR_CONFIG 97
#define MSP_TEMPORARY_COMMANDS 98 // Temporary Commands before cleanup
#define MSP_SET_TEMPORARY_COMMANDS 99 // Temporary Commands before cleanup
// //
// Multwii original MSP commands // Multwii original MSP commands
// //

11
src/main/main.c
View file

@ -129,7 +129,7 @@ void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMotorMixers, servoMixe
#else #else
void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMotorMixers); void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMotorMixers);
#endif #endif
void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration); void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration, bool use_unsyncedPwm);
void rxInit(rxConfig_t *rxConfig, modeActivationCondition_t *modeActivationConditions); void rxInit(rxConfig_t *rxConfig, modeActivationCondition_t *modeActivationConditions);
void gpsInit(serialConfig_t *serialConfig, gpsConfig_t *initialGpsConfig); void gpsInit(serialConfig_t *serialConfig, gpsConfig_t *initialGpsConfig);
void navigationInit(gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile); void navigationInit(gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile);
@ -319,6 +319,8 @@ void init(void)
featureClear(FEATURE_ONESHOT125); featureClear(FEATURE_ONESHOT125);
} }
bool use_unsyncedPwm = masterConfig.use_unsyncedPwm;
// Configurator feature abused for enabling Fast PWM // Configurator feature abused for enabling Fast PWM
pwm_params.useFastPwm = (masterConfig.motor_pwm_protocol != PWM_TYPE_CONVENTIONAL && masterConfig.motor_pwm_protocol != PWM_TYPE_BRUSHED); pwm_params.useFastPwm = (masterConfig.motor_pwm_protocol != PWM_TYPE_CONVENTIONAL && masterConfig.motor_pwm_protocol != PWM_TYPE_BRUSHED);
pwm_params.pwmProtocolType = masterConfig.motor_pwm_protocol; pwm_params.pwmProtocolType = masterConfig.motor_pwm_protocol;
@ -326,8 +328,10 @@ void init(void)
pwm_params.idlePulse = masterConfig.escAndServoConfig.mincommand; pwm_params.idlePulse = masterConfig.escAndServoConfig.mincommand;
if (feature(FEATURE_3D)) if (feature(FEATURE_3D))
pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d; pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d;
if (masterConfig.motor_pwm_protocol == PWM_TYPE_BRUSHED) if (masterConfig.motor_pwm_protocol == PWM_TYPE_BRUSHED) {
pwm_params.idlePulse = 0; // brushed motors pwm_params.idlePulse = 0; // brushed motors
use_unsyncedPwm = false;
}
#ifdef CC3D #ifdef CC3D
pwm_params.useBuzzerP6 = masterConfig.use_buzzer_p6 ? true : false; pwm_params.useBuzzerP6 = masterConfig.use_buzzer_p6 ? true : false;
#endif #endif
@ -335,8 +339,7 @@ void init(void)
pwmOutputConfiguration_t *pwmOutputConfiguration = pwmInit(&pwm_params); pwmOutputConfiguration_t *pwmOutputConfiguration = pwmInit(&pwm_params);
syncMotors(pwm_params.motorPwmRate == 0 && pwm_params.motorPwmRate != PWM_TYPE_BRUSHED); mixerUsePWMOutputConfiguration(pwmOutputConfiguration, use_unsyncedPwm);
mixerUsePWMOutputConfiguration(pwmOutputConfiguration);
if (!feature(FEATURE_ONESHOT125)) if (!feature(FEATURE_ONESHOT125))
motorControlEnable = true; motorControlEnable = true;

35
src/main/mw.c
View file

@ -123,6 +123,7 @@ extern uint8_t PIDweight[3];
uint16_t filteredCycleTime; uint16_t filteredCycleTime;
static bool isRXDataNew; static bool isRXDataNew;
static bool armingCalibrationWasInitialised; static bool armingCalibrationWasInitialised;
float angleRate[3], angleRateSmooth[2];
extern pidControllerFuncPtr pid_controller; extern pidControllerFuncPtr pid_controller;
@ -171,12 +172,29 @@ bool isCalibrating()
return (!isAccelerationCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete()); return (!isAccelerationCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete());
} }
void filterRc(void) float calculateRate(int axis, int16_t rc) {
float angleRate;
if (isSuperExpoActive()) {
float rcFactor = (axis == YAW) ? (ABS(rc) / (500.0f * (currentControlRateProfile->rcYawRate8 / 100.0f))) : (ABS(rc) / (500.0f * (currentControlRateProfile->rcRate8 / 100.0f)));
rcFactor = 1.0f / (constrainf(1.0f - (rcFactor * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f));
angleRate = rcFactor * ((27 * rc) / 16.0f);
} else {
angleRate = (float)((currentControlRateProfile->rates[axis] + 27) * rc) / 16.0f;
}
return constrainf(angleRate, -8190.0f, 8190.0f); // Rate limit protection
}
void processRcCommand(void)
{ {
static int16_t lastCommand[4] = { 0, 0, 0, 0 }; static int16_t lastCommand[4] = { 0, 0, 0, 0 };
static int16_t deltaRC[4] = { 0, 0, 0, 0 }; static int16_t deltaRC[4] = { 0, 0, 0, 0 };
static int16_t factor, rcInterpolationFactor; static int16_t factor, rcInterpolationFactor;
uint16_t rxRefreshRate; uint16_t rxRefreshRate;
int axis;
// Set RC refresh rate for sampling and channels to filter // Set RC refresh rate for sampling and channels to filter
initRxRefreshRate(&rxRefreshRate); initRxRefreshRate(&rxRefreshRate);
@ -184,6 +202,8 @@ void filterRc(void)
rcInterpolationFactor = rxRefreshRate / targetPidLooptime + 1; rcInterpolationFactor = rxRefreshRate / targetPidLooptime + 1;
if (isRXDataNew) { if (isRXDataNew) {
for (axis = 0; axis < 3; axis++) angleRate[axis] = calculateRate(axis, rcCommand[axis]);
for (int channel=0; channel < 4; channel++) { for (int channel=0; channel < 4; channel++) {
deltaRC[channel] = rcCommand[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor); deltaRC[channel] = rcCommand[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor);
lastCommand[channel] = rcCommand[channel]; lastCommand[channel] = rcCommand[channel];
@ -198,8 +218,9 @@ void filterRc(void)
// Interpolate steps of rcCommand // Interpolate steps of rcCommand
if (factor > 0) { if (factor > 0) {
for (int channel=0; channel < 4; channel++) { for (int channel=0; channel < 4; channel++) {
rcCommand[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor; rcCommandSmooth[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor;
} }
for (axis = 0; axis < 2; axis++) angleRateSmooth[axis] = calculateRate(axis, rcCommandSmooth[axis]);
} else { } else {
factor = 0; factor = 0;
} }
@ -644,7 +665,6 @@ void subTaskPidController(void)
// PID - note this is function pointer set by setPIDController() // PID - note this is function pointer set by setPIDController()
pid_controller( pid_controller(
&currentProfile->pidProfile, &currentProfile->pidProfile,
currentControlRateProfile,
masterConfig.max_angle_inclination, masterConfig.max_angle_inclination,
&masterConfig.accelerometerTrims, &masterConfig.accelerometerTrims,
&masterConfig.rxConfig &masterConfig.rxConfig
@ -656,9 +676,7 @@ void subTaskMainSubprocesses(void) {
const uint32_t startTime = micros(); const uint32_t startTime = micros();
if (masterConfig.rxConfig.rcSmoothing || flightModeFlags) { processRcCommand();
filterRc();
}
// Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities. // Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities.
if (gyro.temperature) { if (gyro.temperature) {
@ -696,7 +714,8 @@ void subTaskMainSubprocesses(void) {
&& masterConfig.mixerMode != MIXER_FLYING_WING && masterConfig.mixerMode != MIXER_FLYING_WING
#endif #endif
) { ) {
rcCommand[YAW] = 0; rcCommand[YAW] = rcCommandSmooth[YAW] = 0;
angleRate[YAW] = angleRateSmooth[YAW] = 0;
} }
if (masterConfig.throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { if (masterConfig.throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {