From cc2d7b01de370ac6f55904f00daae6e869803179 Mon Sep 17 00:00:00 2001 From: "Konstantin Sharlaimov (DigitalEntity)" Date: Wed, 1 Feb 2017 01:02:19 +1000 Subject: [PATCH] Move 3d_deadband_throttle to rcControlsConfig --- src/main/fc/cli.c | 2 +- src/main/fc/fc_core.c | 2 +- src/main/fc/fc_init.c | 2 +- src/main/fc/fc_msp.c | 4 ++-- src/main/fc/fc_tasks.c | 2 +- src/main/fc/rc_controls.c | 6 ++++-- src/main/fc/rc_controls.h | 3 ++- src/main/flight/failsafe.c | 7 ++----- src/main/flight/failsafe.h | 2 +- src/main/flight/mixer.c | 11 +++++------ src/main/flight/mixer.h | 1 - src/main/navigation/navigation_multicopter.c | 2 +- src/main/sensors/battery.c | 4 ++-- src/main/sensors/battery.h | 2 +- src/main/telemetry/frsky.c | 6 +++--- 15 files changed, 27 insertions(+), 29 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 0e1c8a725c..f9c94b8d87 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -630,7 +630,6 @@ static const clivalue_t valueTable[] = { { "3d_deadband_low", VAR_UINT16 | MASTER_VALUE, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX }, PG_MOTOR_3D_CONFIG, offsetof(flight3DConfig_t, deadband3d_low) }, // FIXME upper limit should match code in the mixer, 1500 currently { "3d_deadband_high", VAR_UINT16 | MASTER_VALUE, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX }, PG_MOTOR_3D_CONFIG, offsetof(flight3DConfig_t, deadband3d_high) }, // FIXME lower limit should match code in the mixer, 1500 currently, { "3d_neutral", VAR_UINT16 | MASTER_VALUE, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX }, PG_MOTOR_3D_CONFIG, offsetof(flight3DConfig_t, neutral3d) }, - { "3d_deadband_throttle", VAR_UINT16 | MASTER_VALUE, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX }, PG_MOTOR_3D_CONFIG, offsetof(flight3DConfig_t, deadband3d_throttle) }, #ifdef USE_SERVOS // PG_SERVO_CONFIG @@ -686,6 +685,7 @@ static const clivalue_t valueTable[] = { { "yaw_deadband", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 100 }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, yaw_deadband) }, { "pos_hold_deadband", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 10, 250 }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, pos_hold_deadband) }, { "alt_hold_deadband", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 10, 250 }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, alt_hold_deadband) }, + { "3d_deadband_throttle", VAR_UINT16 | MASTER_VALUE, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX }, PG_MOTOR_3D_CONFIG, offsetof(rcControlsConfig_t, deadband3d_throttle) }, // PG_PID_PROFILE // { "default_rate_profile", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, MAX_CONTROL_RATE_PROFILE_COUNT - 1 }, PG_PID_CONFIG, offsetof(pidProfile_t, defaultRateProfileIndex) }, diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index d40721e087..ecc450b421 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -319,7 +319,7 @@ void processRx(timeUs_t currentTimeUs) failsafeUpdateState(); - const throttleStatus_e throttleStatus = calculateThrottleStatus(flight3DConfig()->deadband3d_throttle); + const throttleStatus_e throttleStatus = calculateThrottleStatus(); // When armed and motors aren't spinning, do beeps and then disarm // board after delay so users without buzzer won't lose fingers. diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index a7942c5ff7..fc53003012 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -512,7 +512,7 @@ void init(void) cliInit(serialConfig()); #endif - failsafeInit(flight3DConfig()->deadband3d_throttle); + failsafeInit(); rxInit(); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 56853597e6..16217b8bbd 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1037,7 +1037,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn sbufWriteU8(dst, rcControlsConfig()->deadband); sbufWriteU8(dst, rcControlsConfig()->yaw_deadband); sbufWriteU8(dst, rcControlsConfig()->alt_hold_deadband); - sbufWriteU16(dst, flight3DConfig()->deadband3d_throttle); + sbufWriteU16(dst, rcControlsConfig()->deadband3d_throttle); break; case MSP_SENSOR_ALIGNMENT: @@ -1515,7 +1515,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) rcControlsConfigMutable()->deadband = sbufReadU8(src); rcControlsConfigMutable()->yaw_deadband = sbufReadU8(src); rcControlsConfigMutable()->alt_hold_deadband = sbufReadU8(src); - flight3DConfigMutable()->deadband3d_throttle = sbufReadU16(src); + rcControlsConfigMutable()->deadband3d_throttle = sbufReadU16(src); break; case MSP_SET_RESET_CURR_PID: diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 0aa4e72d5d..55baa2c9b6 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -112,7 +112,7 @@ void taskUpdateBattery(timeUs_t currentTimeUs) if (ibatTimeSinceLastServiced >= IBATINTERVAL) { ibatLastServiced = currentTimeUs; - currentMeterUpdate(ibatTimeSinceLastServiced, flight3DConfig()->deadband3d_throttle); + currentMeterUpdate(ibatTimeSinceLastServiced); } } } diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index abd30b5306..33cf1ff0ec 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -78,7 +78,8 @@ PG_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, .deadband = 5, .yaw_deadband = 5, .pos_hold_deadband = 20, - .alt_hold_deadband = 50 + .alt_hold_deadband = 50, + .deadband3d_throttle = 50 ); PG_REGISTER_WITH_RESET_TEMPLATE(armingConfig_t, armingConfig, PG_ARMING_CONFIG, 0); @@ -108,8 +109,9 @@ bool areSticksInApModePosition(uint16_t ap_mode) return ABS(rcCommand[ROLL]) < ap_mode && ABS(rcCommand[PITCH]) < ap_mode; } -throttleStatus_e calculateThrottleStatus(uint16_t deadband3d_throttle) +throttleStatus_e calculateThrottleStatus(void) { + const uint16_t deadband3d_throttle = rcControlsConfig()->deadband3d_throttle; if (feature(FEATURE_3D) && (rcData[THROTTLE] > (rxConfig()->midrc - deadband3d_throttle) && rcData[THROTTLE] < (rxConfig()->midrc + deadband3d_throttle))) return THROTTLE_LOW; else if (!feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig()->mincheck)) diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index 26640164c0..d867ebc6a5 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -143,6 +143,7 @@ typedef struct rcControlsConfig_s { uint8_t yaw_deadband; // introduce a deadband around the stick center for yaw axis. Must be greater than zero. uint8_t pos_hold_deadband; // Adds ability to adjust the Hold-position when moving the sticks (assisted mode) uint8_t alt_hold_deadband; // Defines the neutral zone of throttle stick during altitude hold + uint16_t deadband3d_throttle; // default throttle deadband from MIDRC } rcControlsConfig_t; PG_DECLARE(rcControlsConfig_t, rcControlsConfig); @@ -158,7 +159,7 @@ PG_DECLARE(armingConfig_t, armingConfig); bool areUsingSticksToArm(void); bool areSticksInApModePosition(uint16_t ap_mode); -throttleStatus_e calculateThrottleStatus(uint16_t deadband3d_throttle); +throttleStatus_e calculateThrottleStatus(void); rollPitchStatus_e calculateRollPitchCenterStatus(void); void processRcStickPositions(throttleStatus_e throttleStatus, bool disarm_kill_switch, bool fixed_wing_auto_arm); diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 9ef07c2b8a..62930f81ab 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -60,8 +60,6 @@ static failsafeState_t failsafeState; -static uint16_t deadband3dThrottle; // default throttle deadband from MIDRC - PG_REGISTER_WITH_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig, PG_FAILSAFE_CONFIG, 0); PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig, @@ -148,9 +146,8 @@ void failsafeReset(void) failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN; } -void failsafeInit(uint16_t deadband3d_throttle) +void failsafeInit(void) { - deadband3dThrottle = deadband3d_throttle; failsafeState.events = 0; failsafeState.monitoring = false; @@ -332,7 +329,7 @@ void failsafeUpdateState(void) case FAILSAFE_IDLE: if (armed) { // Track throttle command below minimum time - if (THROTTLE_HIGH == calculateThrottleStatus(deadband3dThrottle)) { + if (THROTTLE_HIGH == calculateThrottleStatus()) { failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND; } // Kill switch logic (must be independent of receivingRxData to skip PERIOD_RXDATA_FAILURE delay before disarming) diff --git a/src/main/flight/failsafe.h b/src/main/flight/failsafe.h index fe9924b699..e798e96d70 100644 --- a/src/main/flight/failsafe.h +++ b/src/main/flight/failsafe.h @@ -95,7 +95,7 @@ typedef struct failsafeState_s { int16_t lastGoodRcCommand[4]; } failsafeState_t; -void failsafeInit(uint16_t deadband3d_throttle); +void failsafeInit(void); void failsafeReset(void); void failsafeStartMonitoring(void); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 84cd754c28..64f1fd2cf1 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -68,8 +68,7 @@ PG_REGISTER_WITH_RESET_TEMPLATE(flight3DConfig_t, flight3DConfig, PG_MOTOR_3D_CO PG_RESET_TEMPLATE(flight3DConfig_t, flight3DConfig, .deadband3d_low = 1406, .deadband3d_high = 1514, - .neutral3d = 1460, - .deadband3d_throttle = 50 + .neutral3d = 1460 ); @@ -482,15 +481,15 @@ void mixTable(void) if (feature(FEATURE_3D)) { if (!ARMING_FLAG(ARMED)) throttlePrevious = rxConfig()->midrc; // When disarmed set to mid_rc. It always results in positive direction after arming. - if ((rcCommand[THROTTLE] <= (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle))) { // Out of band handling + if ((rcCommand[THROTTLE] <= (rxConfig()->midrc - rcControlsConfig()->deadband3d_throttle))) { // Out of band handling throttleMax = flight3DConfig()->deadband3d_low; throttleMin = motorConfig()->minthrottle; throttlePrevious = throttleCommand = rcCommand[THROTTLE]; - } else if (rcCommand[THROTTLE] >= (rxConfig()->midrc + flight3DConfig()->deadband3d_throttle)) { // Positive handling + } else if (rcCommand[THROTTLE] >= (rxConfig()->midrc + rcControlsConfig()->deadband3d_throttle)) { // Positive handling throttleMax = motorConfig()->maxthrottle; throttleMin = flight3DConfig()->deadband3d_high; throttlePrevious = throttleCommand = rcCommand[THROTTLE]; - } else if ((throttlePrevious <= (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle))) { // Deadband handling from negative to positive + } else if ((throttlePrevious <= (rxConfig()->midrc - rcControlsConfig()->deadband3d_throttle))) { // Deadband handling from negative to positive throttleCommand = throttleMax = flight3DConfig()->deadband3d_low; throttleMin = motorConfig()->minthrottle; } else { // Deadband handling from positive to negative @@ -534,7 +533,7 @@ void mixTable(void) if (isFailsafeActive) { motor[i] = constrain(motor[i], motorConfig()->mincommand, motorConfig()->maxthrottle); } else if (feature(FEATURE_3D)) { - if (throttlePrevious <= (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle)) { + if (throttlePrevious <= (rxConfig()->midrc - rcControlsConfig()->deadband3d_throttle)) { motor[i] = constrain(motor[i], motorConfig()->minthrottle, flight3DConfig()->deadband3d_low); } else { motor[i] = constrain(motor[i], flight3DConfig()->deadband3d_high, motorConfig()->maxthrottle); diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 545d5ae048..bdb3503529 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -98,7 +98,6 @@ typedef struct flight3DConfig_s { uint16_t deadband3d_low; // min 3d value uint16_t deadband3d_high; // max 3d value uint16_t neutral3d; // center 3d value - uint16_t deadband3d_throttle; // default throttle deadband from MIDRC } flight3DConfig_t; PG_DECLARE(flight3DConfig_t, flight3DConfig); diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index 85a2cfbf76..fe9a7741c8 100755 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -149,7 +149,7 @@ bool adjustMulticopterAltitudeFromRCInput(void) void setupMulticopterAltitudeController(void) { - const throttleStatus_e throttleStatus = calculateThrottleStatus(flight3DConfig()->deadband3d_throttle); + const throttleStatus_e throttleStatus = calculateThrottleStatus(); if (navConfig()->general.flags.use_thr_mid_for_althold) { altHoldThrottleRCZero = rcLookupThrottleMid(); diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 3a0a399454..631244ff2c 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -185,7 +185,7 @@ int32_t currentSensorToCentiamps(uint16_t src) return (millivolts * 1000) / (int32_t)batteryConfig()->currentMeterScale; // current in 0.01A steps } -void currentMeterUpdate(int32_t lastUpdateAt, uint16_t deadband3d_throttle) +void currentMeterUpdate(int32_t lastUpdateAt) { static int32_t amperageRaw = 0; static int64_t mAhdrawnRaw = 0; @@ -201,7 +201,7 @@ void currentMeterUpdate(int32_t lastUpdateAt, uint16_t deadband3d_throttle) case CURRENT_SENSOR_VIRTUAL: amperage = (int32_t)batteryConfig()->currentMeterOffset; if (ARMING_FLAG(ARMED)) { - throttleStatus_e throttleStatus = calculateThrottleStatus(deadband3d_throttle); + throttleStatus_e throttleStatus = calculateThrottleStatus(); if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP)) throttleOffset = 0; throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50); diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index 79ad566c70..4a200d3fee 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -74,7 +74,7 @@ batteryState_e getBatteryState(void); void batteryUpdate(uint32_t vbatTimeDelta); void batteryInit(void); -void currentMeterUpdate(int32_t lastUpdateAt, uint16_t deadband3d_throttle); +void currentMeterUpdate(int32_t lastUpdateAt); int32_t currentMeterToCentiamps(uint16_t src); uint8_t calculateBatteryPercentage(void); diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index 69e5466a8a..78c2c7e0ab 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky.c @@ -188,12 +188,12 @@ static void sendGpsAltitude(void) } #endif -static void sendThrottleOrBatterySizeAsRpm(uint16_t deadband3d_throttle) +static void sendThrottleOrBatterySizeAsRpm(void) { uint16_t throttleForRPM = rcCommand[THROTTLE] / BLADE_NUMBER_DIVIDER; sendDataHead(ID_RPM); if (ARMING_FLAG(ARMED)) { - const throttleStatus_e throttleStatus = calculateThrottleStatus(deadband3d_throttle); + const throttleStatus_e throttleStatus = calculateThrottleStatus(); if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP)) throttleForRPM = 0; serialize16(throttleForRPM); @@ -519,7 +519,7 @@ void handleFrSkyTelemetry(void) if ((cycleNum % 8) == 0) { // Sent every 1s sendTemperature1(); - sendThrottleOrBatterySizeAsRpm(flight3DConfig()->deadband3d_throttle); + sendThrottleOrBatterySizeAsRpm(); if (feature(FEATURE_VBAT)) { sendVoltage();