diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 5da0adcbf..ed88dd40b 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1227,7 +1227,7 @@ static bool blackboxWriteSysinfo() BLACKBOX_PRINT_HEADER_LINE("mag_hardware:%d", masterConfig.mag_hardware); BLACKBOX_PRINT_HEADER_LINE("gyro_cal_on_first_arm:%d", masterConfig.gyro_cal_on_first_arm); BLACKBOX_PRINT_HEADER_LINE("vbat_pid_compensation:%d", masterConfig.batteryConfig.vbatPidCompensation); - BLACKBOX_PRINT_HEADER_LINE("rc_smoothing:%d", masterConfig.rxConfig.rcSmoothing); + BLACKBOX_PRINT_HEADER_LINE("rc_smooth_interval:%d", masterConfig.rxConfig.rcSmoothInterval); BLACKBOX_PRINT_HEADER_LINE("airmode_activate_throttle:%d", masterConfig.rxConfig.airModeActivateThreshold); BLACKBOX_PRINT_HEADER_LINE("features:%d", masterConfig.enabledFeatures); diff --git a/src/main/config/config.c b/src/main/config/config.c index aa88fb0c8..1ce7f4f81 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -169,7 +169,7 @@ static uint32_t activeFeaturesLatch = 0; static uint8_t currentControlRateProfileIndex = 0; controlRateConfig_t *currentControlRateProfile; -static const uint8_t EEPROM_CONF_VERSION = 140; +static const uint8_t EEPROM_CONF_VERSION = 141; static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) { @@ -182,9 +182,9 @@ static void resetPidProfile(pidProfile_t *pidProfile) { #if (defined(STM32F10X)) - pidProfile->pidController = PID_CONTROLLER_MWREWRITE; + pidProfile->pidController = PID_CONTROLLER_INTEGER; #else - pidProfile->pidController = PID_CONTROLLER_LUX_FLOAT; + pidProfile->pidController = PID_CONTROLLER_FLOAT; #endif pidProfile->P8[ROLL] = 45; @@ -221,6 +221,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->rollPitchItermIgnoreRate = 200; pidProfile->yawItermIgnoreRate = 35; pidProfile->dterm_lpf_hz = 50; // filtering ON by default + pidProfile->deltaMethod = DELTA_FROM_ERROR; pidProfile->dynamic_pid = 1; #ifdef GTUNE @@ -494,7 +495,7 @@ static void resetConf(void) masterConfig.rxConfig.rssi_channel = 0; masterConfig.rxConfig.rssi_scale = RSSI_SCALE_DEFAULT; masterConfig.rxConfig.rssi_ppm_invert = 0; - masterConfig.rxConfig.rcSmoothing = 0; // TODO - Cleanup with next EEPROM changes + masterConfig.rxConfig.rcSmoothInterval = 0; // 0 is predefined masterConfig.rxConfig.fpvCamAngleDegrees = 0; #ifdef STM32F4 masterConfig.rxConfig.max_aux_channel = 99; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 717f0df01..e67723789 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -855,7 +855,7 @@ void mixTable(void) } // Motor stop handling - if (feature(FEATURE_MOTOR_STOP) && ARMING_FLAG(ARMED) && !feature(FEATURE_3D) && !IS_RC_MODE_ACTIVE(BOXAIRMODE)) { + if (feature(FEATURE_MOTOR_STOP) && ARMING_FLAG(ARMED) && !feature(FEATURE_3D) && !isAirmodeActive()) { if (((rcData[THROTTLE]) < rxConfig->mincheck)) { motor[i] = escAndServoConfig->mincommand; } diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 5b77c8c0b..6d6db11a9 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -67,10 +67,10 @@ static int32_t errorGyroI[3]; static float errorGyroIf[3]; #endif -static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, +static void pidInteger(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig); -pidControllerFuncPtr pid_controller = pidMultiWiiRewrite; // which pid controller are we using, defaultMultiWii +pidControllerFuncPtr pid_controller = pidInteger; // which pid controller are we using void setTargetPidLooptime(uint8_t pidProcessDenom) { targetPidLooptime = targetLooptime * pidProcessDenom; @@ -114,7 +114,7 @@ static filterStatePt1_t deltaFilterState[3]; static filterStatePt1_t yawFilterState; #ifndef SKIP_PID_LUXFLOAT -static void pidLuxFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, +static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig) { float RateError, gyroRate, RateErrorSmooth; @@ -126,7 +126,7 @@ static void pidLuxFloat(const pidProfile_t *pidProfile, uint16_t max_angle_incli float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float - // Scaling factors for Pids to match rewrite and use same defaults + // Scaling factors for Pids for better tunable range in configurator static const float luxPTermScale = 1.0f / 128; static const float luxITermScale = 1000000.0f / 0x1000000; static const float luxDTermScale = (0.000001f * (float)0xFFFF) / 508; @@ -168,7 +168,7 @@ static void pidLuxFloat(const pidProfile_t *pidProfile, uint16_t max_angle_incli } } - gyroRate = gyroADCf[axis] / 4.0f; // gyro output scaled to old rewrite scale + gyroRate = gyroADCf[axis] / 4.0f; // gyro output scaled for easier int conversion // --------low-level gyro-based PID. ---------- // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes @@ -176,11 +176,12 @@ static void pidLuxFloat(const pidProfile_t *pidProfile, uint16_t max_angle_incli // multiplication of rcCommand corresponds to changing the sticks scaling here RateError = angleRate[axis] - gyroRate; - // Smoothed Error for Derivative - if (flightModeFlags && axis != YAW) { - RateErrorSmooth = RateError; - } else { - RateErrorSmooth = angleRateSmooth[axis] - gyroRate; + if (pidProfile->deltaMethod == DELTA_FROM_ERROR) { + // Smoothed Error for Derivative when delta from error selected + if (flightModeFlags && axis != YAW) + RateErrorSmooth = RateError; + else + RateErrorSmooth = angleRateSmooth[axis] - gyroRate; } // -----calculate P component @@ -215,8 +216,13 @@ static void pidLuxFloat(const pidProfile_t *pidProfile, uint16_t max_angle_incli DTerm = 0.0f; // needed for blackbox } else { - delta = RateErrorSmooth - lastRateError[axis]; - lastRateError[axis] = RateErrorSmooth; + if (pidProfile->deltaMethod == DELTA_FROM_ERROR) { + delta = RateErrorSmooth - lastRateError[axis]; + lastRateError[axis] = RateErrorSmooth; + } else { + delta = -(gyroRate - lastRateError[axis]); + lastRateError[axis] = gyroRate; + } // Divide delta by targetLooptime to get differential (ie dr/dt) delta *= (1.0f / getdT()); @@ -245,7 +251,7 @@ static void pidLuxFloat(const pidProfile_t *pidProfile, uint16_t max_angle_incli } #endif -static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, +static void pidInteger(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig) { int axis; @@ -301,11 +307,12 @@ static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, uint16_t max_angl RateError = AngleRateTmp - gyroRate; - // Smoothed Error for Derivative - if (flightModeFlags && axis != YAW) { - RateErrorSmooth = RateError; - } else { - RateErrorSmooth = AngleRateTmpSmooth - gyroRate; + if (pidProfile->deltaMethod == DELTA_FROM_ERROR) { + // Smoothed Error for Derivative when delta from error selected + if (flightModeFlags && axis != YAW) + RateErrorSmooth = RateError; + else + RateErrorSmooth = AngleRateTmpSmooth - gyroRate; } // -----calculate P component @@ -348,8 +355,13 @@ static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, uint16_t max_angl DTerm = 0; // needed for blackbox } else { - delta = RateErrorSmooth - lastRateError[axis]; - lastRateError[axis] = RateErrorSmooth; + if (pidProfile->deltaMethod == DELTA_FROM_ERROR) { + delta = RateErrorSmooth - lastRateError[axis]; + lastRateError[axis] = RateErrorSmooth; + } else { + delta = -(gyroRate - lastRateError[axis]); + lastRateError[axis] = gyroRate; + } // Divide delta by targetLooptime to get differential (ie dr/dt) delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 5; @@ -382,12 +394,12 @@ void pidSetController(pidControllerType_e type) { switch (type) { default: - case PID_CONTROLLER_MWREWRITE: - pid_controller = pidMultiWiiRewrite; + case PID_CONTROLLER_INTEGER: + pid_controller = pidInteger; break; #ifndef SKIP_PID_LUXFLOAT - case PID_CONTROLLER_LUX_FLOAT: - pid_controller = pidLuxFloat; + case PID_CONTROLLER_FLOAT: + pid_controller = pidFloat; #endif } } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 85f569524..a407aeb80 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -41,14 +41,14 @@ typedef enum { } pidIndex_e; typedef enum { - PID_CONTROLLER_MWREWRITE = 1, - PID_CONTROLLER_LUX_FLOAT, + PID_CONTROLLER_INTEGER = 1, // Integer math to gain some more performance from F1 targets + PID_CONTROLLER_FLOAT, PID_COUNT } pidControllerType_e; typedef enum { - DELTA_FROM_ERROR = 0, - DELTA_FROM_MEASUREMENT + DELTA_FROM_ERROR = 0, + DELTA_FROM_MEASUREMENT } pidDeltaType_e; typedef enum { @@ -68,6 +68,7 @@ typedef struct pidProfile_s { uint16_t dterm_lpf_hz; // Delta Filter in hz uint16_t yaw_lpf_hz; // Additional yaw filter when yaw axis too noisy + uint8_t deltaMethod; // Alternative delta Calculation uint16_t rollPitchItermIgnoreRate; // Experimental threshold for resetting iterm for pitch and roll on certain rates uint16_t yawItermIgnoreRate; // Experimental threshold for resetting iterm for yaw on certain rates uint16_t yaw_p_limit; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 027ea44d8..c33e6838d 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -386,7 +386,7 @@ static const char * const lookupTableBlackboxDevice[] = { static const char * const lookupTablePidController[] = { - "UNUSED", "MWREWRITE", "LUX" + "UNUSED", "INT", "FLOAT" }; static const char * const lookupTableSerialRX[] = { @@ -467,6 +467,10 @@ static const char * const lookupTableFastPwm[] = { "OFF", "ONESHOT125", "ONESHOT42", "MULTISHOT", "BRUSHED" }; +static const char * const lookupDeltaMethod[] = { + "ERROR", "MEASUREMENT" +}; + typedef struct lookupTableEntry_s { const char * const *values; const uint8_t valueCount; @@ -494,6 +498,7 @@ typedef enum { TABLE_DEBUG, TABLE_SUPEREXPO_YAW, TABLE_MOTOR_PWM_PROTOCOL, + TABLE_DELTA_METHOD, #ifdef OSD TABLE_OSD, #endif @@ -521,6 +526,7 @@ static const lookupTableEntry_t lookupTables[] = { { lookupTableDebug, sizeof(lookupTableDebug) / sizeof(char *) }, { lookupTableSuperExpoYaw, sizeof(lookupTableSuperExpoYaw) / sizeof(char *) }, { lookupTableFastPwm, sizeof(lookupTableFastPwm) / sizeof(char *) }, + { lookupDeltaMethod, sizeof(lookupDeltaMethod) / sizeof(char *) }, #ifdef OSD { lookupTableOsdType, sizeof(lookupTableOsdType) / sizeof(char *) }, #endif @@ -576,12 +582,12 @@ typedef struct { const clivalue_t valueTable[] = { // { "emf_avoidance", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.emf_avoidance, .config.lookup = { TABLE_OFF_ON } }, - { "mid_rc", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.midrc, .config.minmax = { 1200, 1700 } }, { "min_check", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.mincheck, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, { "max_check", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.maxcheck, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, { "rssi_channel", VAR_INT8 | MASTER_VALUE, &masterConfig.rxConfig.rssi_channel, .config.minmax = { 0, MAX_SUPPORTED_RC_CHANNEL_COUNT } }, { "rssi_scale", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.rssi_scale, .config.minmax = { RSSI_SCALE_MIN, RSSI_SCALE_MAX } }, + { "rc_smooth_interval_ms", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.rcSmoothInterval, .config.minmax = { 0, 255 } }, { "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 } }, { "roll_yaw_cam_mix_degrees", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.fpvCamAngleDegrees, .config.minmax = { 0, 50 } }, @@ -685,7 +691,7 @@ const clivalue_t valueTable[] = { { "align_board_yaw", VAR_INT16 | MASTER_VALUE, &masterConfig.boardAlignment.yawDegrees, .config.minmax = { -180, 360 } }, { "max_angle_inclination", VAR_UINT16 | MASTER_VALUE, &masterConfig.max_angle_inclination, .config.minmax = { 100, 900 } }, - + { "pid_delta_method", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.deltaMethod, .config.lookup = { TABLE_DELTA_METHOD } }, { "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } }, { "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_sync_denom, .config.minmax = { 1, 8 } }, { "gyro_lowpass", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_soft_lpf_hz, .config.minmax = { 0, 255 } }, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 693d988dc..62bd11dca 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1272,14 +1272,20 @@ static bool processOutCommand(uint8_t cmdMSP) serialize16(currentProfile->pidProfile.yaw_lpf_hz); break; case MSP_ADVANCED_TUNING: - headSerialReply(3 * 2); + headSerialReply(4 * 2 + 2); serialize16(currentProfile->pidProfile.rollPitchItermIgnoreRate); serialize16(currentProfile->pidProfile.yawItermIgnoreRate); serialize16(currentProfile->pidProfile.yaw_p_limit); + serialize16(masterConfig.rxConfig.airModeActivateThreshold); + serialize8(currentProfile->pidProfile.deltaMethod); + serialize8(masterConfig.batteryConfig.vbatPidCompensation); break; - case MSP_TEMPORARY_COMMANDS: - headSerialReply(1); + case MSP_SPECIAL_PARAMETERS: + headSerialReply(1 + 2 + 1 + 2); serialize8(currentControlRateProfile->rcYawRate8); + serialize16(masterConfig.rxConfig.airModeActivateThreshold); + serialize8(masterConfig.rxConfig.rcSmoothInterval); + serialize16(masterConfig.escAndServoConfig.escDesyncProtection); break; case MSP_SENSOR_CONFIG: headSerialReply(3); @@ -1850,9 +1856,13 @@ static bool processInCommand(void) currentProfile->pidProfile.rollPitchItermIgnoreRate = read16(); currentProfile->pidProfile.yawItermIgnoreRate = read16(); currentProfile->pidProfile.yaw_p_limit = read16(); + currentProfile->pidProfile.deltaMethod = read8(); break; - case MSP_SET_TEMPORARY_COMMANDS: + case MSP_SET_SPECIAL_PARAMETERS: currentControlRateProfile->rcYawRate8 = read8(); + masterConfig.rxConfig.airModeActivateThreshold = read16(); + masterConfig.rxConfig.rcSmoothInterval = read8(); + masterConfig.escAndServoConfig.escDesyncProtection = read16(); break; case MSP_SET_SENSOR_CONFIG: masterConfig.acc_hardware = read8(); diff --git a/src/main/io/serial_msp.h b/src/main/io/serial_msp.h index 7acb92a25..1a63b2b20 100644 --- a/src/main/io/serial_msp.h +++ b/src/main/io/serial_msp.h @@ -206,8 +206,8 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER; #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 +#define MSP_SPECIAL_PARAMETERS 98 // Temporary betaflight parameters before cleanup and keep CF compatibility +#define MSP_SET_SPECIAL_PARAMETERS 99 // Temporary betaflight parameters before cleanup and keep CF compatibility // // Multwii original MSP commands diff --git a/src/main/mw.c b/src/main/mw.c index 5a95f1c92..b935b0015 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -197,7 +197,11 @@ void processRcCommand(void) int axis; // Set RC refresh rate for sampling and channels to filter - initRxRefreshRate(&rxRefreshRate); + if (masterConfig.rxConfig.rcSmoothInterval) { + rxRefreshRate = 1000 * masterConfig.rxConfig.rcSmoothInterval; + } else { + initRxRefreshRate(&rxRefreshRate); + } rcInterpolationFactor = rxRefreshRate / targetPidLooptime + 1; diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index b76873fb1..1cec24053 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -121,7 +121,7 @@ typedef struct rxConfig_s { uint16_t midrc; // Some radios have not a neutral point centered on 1500. can be changed here uint16_t mincheck; // minimum rc end uint16_t maxcheck; // maximum rc end - uint8_t rcSmoothing; + uint8_t rcSmoothInterval; uint8_t fpvCamAngleDegrees; // Camera angle to be scaled into rc commands uint8_t max_aux_channel; uint16_t airModeActivateThreshold; // Throttle setpoint where airmode gets activated