From 58bcb981ed023298737cd93595aa9c513124fa1c Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 1 Feb 2017 23:53:29 +0100 Subject: [PATCH 1/4] Decouple min_check from throttle // Add modern expo calculation to throttle curve --- Makefile | 2 -- src/main/fc/config.c | 9 ------ src/main/fc/fc_core.c | 1 - src/main/fc/fc_rc.c | 11 +++++-- src/main/fc/rc_controls.c | 2 -- src/main/fc/rc_curves.c | 60 --------------------------------------- src/main/fc/rc_curves.h | 25 ---------------- 7 files changed, 9 insertions(+), 101 deletions(-) delete mode 100644 src/main/fc/rc_curves.c delete mode 100644 src/main/fc/rc_curves.h diff --git a/Makefile b/Makefile index 56c02912f..340dbcacb 100644 --- a/Makefile +++ b/Makefile @@ -594,7 +594,6 @@ COMMON_SRC = \ fc/fc_msp.c \ fc/fc_tasks.c \ fc/rc_controls.c \ - fc/rc_curves.c \ fc/runtime_config.c \ fc/cli.c \ flight/altitudehold.c \ @@ -714,7 +713,6 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \ fc/fc_tasks.c \ fc/fc_rc.c \ fc/rc_controls.c \ - fc/rc_curves.c \ fc/runtime_config.c \ flight/altitudehold.c \ flight/failsafe.c \ diff --git a/src/main/fc/config.c b/src/main/fc/config.c index e4d2b69be..1ccd2592f 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -53,7 +53,6 @@ #include "fc/config.h" #include "fc/rc_controls.h" -#include "fc/rc_curves.h" #include "fc/runtime_config.h" #include "sensors/sensors.h" @@ -882,15 +881,8 @@ static void resetConf(void) #endif } -void activateControlRateConfig(void) -{ - generateThrottleCurve(currentControlRateProfile, &masterConfig.motorConfig); -} - void activateConfig(void) { - activateControlRateConfig(); - resetAdjustmentStates(); useRcControlsConfig( @@ -1168,7 +1160,6 @@ void changeControlRateProfile(uint8_t profileIndex) profileIndex = MAX_RATEPROFILES - 1; } setControlRateProfile(profileIndex); - activateControlRateConfig(); } void beeperOffSet(uint32_t mask) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 94897ca30..304e98de2 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -41,7 +41,6 @@ #include "fc/config.h" #include "fc/rc_controls.h" -#include "fc/rc_curves.h" #include "fc/runtime_config.h" #include "fc/cli.h" #include "fc/fc_rc.h" diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c index ebf3fb34d..950054e3e 100755 --- a/src/main/fc/fc_rc.c +++ b/src/main/fc/fc_rc.c @@ -17,6 +17,7 @@ #include #include +#include #include "platform.h" @@ -29,7 +30,6 @@ #include "fc/config.h" #include "fc/rc_controls.h" -#include "fc/rc_curves.h" #include "fc/runtime_config.h" #include "fc/fc_rc.h" #include "fc/fc_core.h" @@ -266,7 +266,14 @@ void updateRcCommands(void) tmp = constrain(rcData[THROTTLE], rxConfig()->mincheck, PWM_RANGE_MAX); tmp = (uint32_t)(tmp - rxConfig()->mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - rxConfig()->mincheck); } - rcCommand[THROTTLE] = rcLookupThrottle(tmp); + + if (currentControlRateProfile->thrExpo8) { + float expof = currentControlRateProfile->thrExpo8 / 100.0f; + float tmpf = (tmp / (PWM_RANGE_MAX - PWM_RANGE_MIN)); + tmp = lrintf(tmp * sq(tmpf) * expof + tmp * (1-expof)); + } + + rcCommand[THROTTLE] = tmp + (PWM_RANGE_MAX - PWM_RANGE_MIN); if (feature(FEATURE_3D) && IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH) && !failsafeIsActive()) { fix12_t throttleScaler = qConstruct(rcCommand[THROTTLE] - 1000, 1000); diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index fea3bdf4b..8a64ed70c 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -37,7 +37,6 @@ #include "fc/config.h" #include "fc/fc_core.h" #include "fc/rc_controls.h" -#include "fc/rc_curves.h" #include "fc/runtime_config.h" #include "io/gps.h" @@ -524,7 +523,6 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t case ADJUSTMENT_THROTTLE_EXPO: newValue = constrain((int)controlRateConfig->thrExpo8 + delta, 0, 100); // FIXME magic numbers repeated in cli.c controlRateConfig->thrExpo8 = newValue; - generateThrottleCurve(controlRateConfig, motorConfig); blackboxLogInflightAdjustmentEvent(ADJUSTMENT_THROTTLE_EXPO, newValue); break; case ADJUSTMENT_PITCH_ROLL_RATE: diff --git a/src/main/fc/rc_curves.c b/src/main/fc/rc_curves.c deleted file mode 100644 index 8f447cb2c..000000000 --- a/src/main/fc/rc_curves.c +++ /dev/null @@ -1,60 +0,0 @@ -/* - * This file is part of Cleanflight. - * - * Cleanflight is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Cleanflight is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . - */ - -#include -#include - -#include "platform.h" - -#include "config/feature.h" - -#include "io/motors.h" - -#include "fc/config.h" -#include "fc/rc_curves.h" -#include "fc/rc_controls.h" - -#include "rx/rx.h" - - -#define THROTTLE_LOOKUP_LENGTH 12 -static int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for expo & mid THROTTLE - -void generateThrottleCurve(controlRateConfig_t *controlRateConfig, motorConfig_t *motorConfig) -{ - uint8_t i; - uint16_t minThrottle = (feature(FEATURE_3D && IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) ? PWM_RANGE_MIN : motorConfig->minthrottle); - - for (i = 0; i < THROTTLE_LOOKUP_LENGTH; i++) { - int16_t tmp = 10 * i - controlRateConfig->thrMid8; - uint8_t y = 1; - if (tmp > 0) - y = 100 - controlRateConfig->thrMid8; - if (tmp < 0) - y = controlRateConfig->thrMid8; - lookupThrottleRC[i] = 10 * controlRateConfig->thrMid8 + tmp * (100 - controlRateConfig->thrExpo8 + (int32_t) controlRateConfig->thrExpo8 * (tmp * tmp) / (y * y)) / 10; - lookupThrottleRC[i] = minThrottle + (int32_t) (motorConfig->maxthrottle - minThrottle) * lookupThrottleRC[i] / 1000; // [MINTHROTTLE;MAXTHROTTLE] - } -} - -int16_t rcLookupThrottle(int32_t tmp) -{ - const int32_t tmp2 = tmp / 100; - // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE] - return lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; -} - diff --git a/src/main/fc/rc_curves.h b/src/main/fc/rc_curves.h deleted file mode 100644 index 41132a98f..000000000 --- a/src/main/fc/rc_curves.h +++ /dev/null @@ -1,25 +0,0 @@ -/* - * This file is part of Cleanflight. - * - * Cleanflight is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Cleanflight is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . - */ - -#pragma once - -struct controlRateConfig_s; -struct motorConfig_s; -void generateThrottleCurve(struct controlRateConfig_s *controlRateConfig, struct motorConfig_s *motorConfig); - -int16_t rcLookupThrottle(int32_t tmp); - From 0b3d3859240eb6b7409fd726a839e0e8644954be Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 1 Feb 2017 23:53:57 +0100 Subject: [PATCH 2/4] Bump Version --- src/main/build/version.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/build/version.h b/src/main/build/version.h index 3df77b8cf..09d937058 100644 --- a/src/main/build/version.h +++ b/src/main/build/version.h @@ -18,7 +18,7 @@ #define FC_FIRMWARE_NAME "Betaflight" #define FC_VERSION_MAJOR 3 // increment when a major release is made (big new feature, etc) #define FC_VERSION_MINOR 1 // increment when a minor release is made (small new feature, change etc) -#define FC_VERSION_PATCH_LEVEL 1 // increment when a bug is fixed +#define FC_VERSION_PATCH_LEVEL 2 // increment when a bug is fixed #define STR_HELPER(x) #x #define STR(x) STR_HELPER(x) From f0cd1c8b03e96f78a8d6bc7cc0c431bb657cf669 Mon Sep 17 00:00:00 2001 From: mikeller Date: Wed, 1 Feb 2017 22:20:12 +1300 Subject: [PATCH 3/4] Fixed CLI setting names. --- src/main/fc/cli.c | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 700711f1a..e39a2f6c3 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -583,28 +583,28 @@ const clivalue_t valueTable[] = { { "frsky_unit", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &telemetryConfig()->frsky_unit, .config.lookup = { TABLE_UNIT } }, { "frsky_vfas_precision", VAR_UINT8 | MASTER_VALUE, &telemetryConfig()->frsky_vfas_precision, .config.minmax = { FRSKY_VFAS_PRECISION_LOW, FRSKY_VFAS_PRECISION_HIGH } }, { "frsky_vfas_cell_voltage", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &telemetryConfig()->frsky_vfas_cell_voltage, .config.lookup = { TABLE_OFF_ON } }, - { "hott_alarm_sound_int", VAR_UINT8 | MASTER_VALUE, &telemetryConfig()->hottAlarmSoundInterval, .config.minmax = { 0, 120 } }, - { "pid_in_telemetry", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &telemetryConfig()->pidValuesAsTelemetry, .config.lookup = {TABLE_OFF_ON } }, + { "hott_alarm_int", VAR_UINT8 | MASTER_VALUE, &telemetryConfig()->hottAlarmSoundInterval, .config.minmax = { 0, 120 } }, + { "pid_in_tlm", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &telemetryConfig()->pidValuesAsTelemetry, .config.lookup = {TABLE_OFF_ON } }, #if defined(TELEMETRY_IBUS) { "ibus_report_cell_voltage", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &ibusTelemetryConfig()->report_cell_voltage, .config.lookup = { TABLE_OFF_ON } }, #endif #endif - { "vbat_capacity", VAR_UINT16 | MASTER_VALUE, &batteryConfig()->batteryCapacity, .config.minmax = { 0, 20000 } }, + { "bat_capacity", VAR_UINT16 | MASTER_VALUE, &batteryConfig()->batteryCapacity, .config.minmax = { 0, 20000 } }, { "vbat_scale", VAR_UINT8 | MASTER_VALUE, &batteryConfig()->vbatscale, .config.minmax = { VBAT_SCALE_MIN, VBAT_SCALE_MAX } }, { "vbat_max_cell_voltage", VAR_UINT8 | MASTER_VALUE, &batteryConfig()->vbatmaxcellvoltage, .config.minmax = { 10, 50 } }, { "vbat_min_cell_voltage", VAR_UINT8 | MASTER_VALUE, &batteryConfig()->vbatmincellvoltage, .config.minmax = { 10, 50 } }, { "vbat_warning_cell_voltage", VAR_UINT8 | MASTER_VALUE, &batteryConfig()->vbatwarningcellvoltage, .config.minmax = { 10, 50 } }, { "vbat_hysteresis", VAR_UINT8 | MASTER_VALUE, &batteryConfig()->vbathysteresis, .config.minmax = { 0, 250 } }, - { "current_scale", VAR_INT16 | MASTER_VALUE, &batteryConfig()->currentMeterScale, .config.minmax = { -16000, 16000 } }, - { "current_offset", VAR_INT16 | MASTER_VALUE, &batteryConfig()->currentMeterOffset, .config.minmax = { -16000, 16000 } }, - { "mwii_current_output", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &batteryConfig()->multiwiiCurrentMeterOutput, .config.lookup = { TABLE_OFF_ON } }, + { "ibat_scale", VAR_INT16 | MASTER_VALUE, &batteryConfig()->currentMeterScale, .config.minmax = { -16000, 16000 } }, + { "ibat_offset", VAR_INT16 | MASTER_VALUE, &batteryConfig()->currentMeterOffset, .config.minmax = { -16000, 16000 } }, + { "mwii_ibat_output", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &batteryConfig()->multiwiiCurrentMeterOutput, .config.lookup = { TABLE_OFF_ON } }, { "current_meter_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &batteryConfig()->currentMeterType, .config.lookup = { TABLE_CURRENT_SENSOR } }, { "battery_meter_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &batteryConfig()->batteryMeterType, .config.lookup = { TABLE_BATTERY_SENSOR } }, - { "no_vbat_level", VAR_UINT8 | MASTER_VALUE, &batteryConfig()->batterynotpresentlevel, .config.minmax = { 0, 200 } }, + { "bat_detect_thresh", VAR_UINT8 | MASTER_VALUE, &batteryConfig()->batterynotpresentlevel, .config.minmax = { 0, 200 } }, { "use_vbat_alerts", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &batteryConfig()->useVBatAlerts, .config.lookup = { TABLE_OFF_ON } }, - { "vbat_usage_alerts", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &batteryConfig()->useConsumptionAlerts, .config.lookup = { TABLE_OFF_ON } }, - { "vbat_usage_percent", VAR_UINT8 | MASTER_VALUE, &batteryConfig()->consumptionWarningPercentage, .config.minmax = { 0, 100 } }, + { "use_cbat_alerts", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &batteryConfig()->useConsumptionAlerts, .config.lookup = { TABLE_OFF_ON } }, + { "cbat_alert_percent", VAR_UINT8 | MASTER_VALUE, &batteryConfig()->consumptionWarningPercentage, .config.minmax = { 0, 100 } }, { "align_gyro", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &gyroConfig()->gyro_align, .config.lookup = { TABLE_ALIGNMENT } }, { "align_acc", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &accelerometerConfig()->acc_align, .config.lookup = { TABLE_ALIGNMENT } }, { "align_mag", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &compassConfig()->mag_align, .config.lookup = { TABLE_ALIGNMENT } }, @@ -634,8 +634,8 @@ const clivalue_t valueTable[] = { { "deadband", VAR_UINT8 | MASTER_VALUE, &rcControlsConfig()->deadband, .config.minmax = { 0, 32 } }, { "yaw_deadband", VAR_UINT8 | MASTER_VALUE, &rcControlsConfig()->yaw_deadband, .config.minmax = { 0, 100 } }, - { "throttle_correct_value", VAR_UINT8 | MASTER_VALUE, &throttleCorrectionConfig()->throttle_correction_value, .config.minmax = { 0, 150 } }, - { "throttle_correct_angle", VAR_UINT16 | MASTER_VALUE, &throttleCorrectionConfig()->throttle_correction_angle, .config.minmax = { 1, 900 } }, + { "thr_corr_value", VAR_UINT8 | MASTER_VALUE, &throttleCorrectionConfig()->throttle_correction_value, .config.minmax = { 0, 150 } }, + { "thr_corr_angle", VAR_UINT16 | MASTER_VALUE, &throttleCorrectionConfig()->throttle_correction_angle, .config.minmax = { 1, 900 } }, { "yaw_control_direction", VAR_INT8 | MASTER_VALUE, &rcControlsConfig()->yaw_control_direction, .config.minmax = { -1, 1 } }, @@ -697,7 +697,7 @@ const clivalue_t valueTable[] = { { "d_lowpass_type", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.dterm_filter_type, .config.lookup = { TABLE_LOWPASS_TYPE } }, { "d_lowpass", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 16000 } }, { "d_notch_hz", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_notch_hz, .config.minmax = { 0, 16000 } }, - { "d_notch_cutoff", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_notch_cutoff, .config.minmax = { 1, 16000 } }, + { "d_notch_cut", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_notch_cutoff, .config.minmax = { 1, 16000 } }, { "vbat_pid_gain", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.vbatPidCompensation, .config.lookup = { TABLE_OFF_ON } }, { "pid_at_min_throttle", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.pidAtMinThrottle, .config.lookup = { TABLE_OFF_ON } }, { "anti_gravity_thresh", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermThrottleThreshold, .config.minmax = {20, 1000 } }, From 70807d016bc4ae70546cca6dac9a0f0727e0021d Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 2 Feb 2017 00:09:35 +0100 Subject: [PATCH 4/4] Slightly less agressive defaults --- src/main/fc/config.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 1ccd2592f..68125b84b 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -178,11 +178,11 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->levelAngleLimit = 55; pidProfile->levelSensitivity = 55; pidProfile->setpointRelaxRatio = 30; - pidProfile->dtermSetpointWeight = 200; + pidProfile->dtermSetpointWeight = 190; pidProfile->yawRateAccelLimit = 10.0f; pidProfile->rateAccelLimit = 0.0f; - pidProfile->itermThrottleThreshold = 300; - pidProfile->itermAcceleratorGain = 3.0f; + pidProfile->itermThrottleThreshold = 350; + pidProfile->itermAcceleratorGain = 1.5f; pidProfile->itermAcceleratorRateLimit = 80; }