Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Throttle low clean up #8601

Merged
merged 2 commits into from Dec 8, 2022
Merged

Throttle low clean up #8601

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
a7cef47
first update
breadoven Dec 1, 2022
c321aea
update
breadoven Dec 1, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
18 changes: 9 additions & 9 deletions src/main/fc/fc_core.c
View file
Open in desktop
Expand Up @@ -212,10 +212,10 @@ static void updateArmingStatus(void)
/* CHECK: Throttle */
if (!armingConfig()->fixed_wing_auto_arm) {
// Don't want this check if fixed_wing_auto_arm is in use - machine arms on throttle > LOW
if (calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) != THROTTLE_LOW) {
ENABLE_ARMING_FLAG(ARMING_DISABLED_THROTTLE);
} else {
if (throttleStickIsLow()) {
DISABLE_ARMING_FLAG(ARMING_DISABLED_THROTTLE);
} else {
ENABLE_ARMING_FLAG(ARMING_DISABLED_THROTTLE);
}
}

Expand Down Expand Up @@ -630,13 +630,13 @@ void processRx(timeUs_t currentTimeUs)

failsafeUpdateState();

const throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
const bool throttleIsLow = throttleStickIsLow();

// When armed and motors aren't spinning, do beeps periodically
if (ARMING_FLAG(ARMED) && feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING_LEGACY)) {
static bool armedBeeperOn = false;

if (throttleStatus == THROTTLE_LOW) {
if (throttleIsLow) {
beeper(BEEPER_ARMED);
armedBeeperOn = true;
} else if (armedBeeperOn) {
Expand All @@ -645,7 +645,7 @@ void processRx(timeUs_t currentTimeUs)
}
}

processRcStickPositions(throttleStatus);
processRcStickPositions(throttleIsLow);
processAirmode();
updateActivatedModes();

Expand Down Expand Up @@ -759,7 +759,7 @@ void processRx(timeUs_t currentTimeUs)
pidResetErrorAccumulators();
}
else if (rcControlsConfig()->airmodeHandlingType == STICK_CENTER) {
if (throttleStatus == THROTTLE_LOW) {
if (throttleIsLow) {
if (STATE(AIRMODE_ACTIVE) && !failsafeIsActive()) {
if ((rollPitchStatus == CENTERED) || (feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING_LEGACY))) {
ENABLE_STATE(ANTI_WINDUP);
Expand All @@ -778,7 +778,7 @@ void processRx(timeUs_t currentTimeUs)
}
}
else if (rcControlsConfig()->airmodeHandlingType == STICK_CENTER_ONCE) {
if (throttleStatus == THROTTLE_LOW) {
if (throttleIsLow) {
if (STATE(AIRMODE_ACTIVE) && !failsafeIsActive()) {
if ((rollPitchStatus == CENTERED) && !STATE(ANTI_WINDUP_DEACTIVATED)) {
ENABLE_STATE(ANTI_WINDUP);
Expand All @@ -802,7 +802,7 @@ void processRx(timeUs_t currentTimeUs)
else if (rcControlsConfig()->airmodeHandlingType == THROTTLE_THRESHOLD) {
DISABLE_STATE(ANTI_WINDUP);
//This case applies only to MR when Airmode management is throttle threshold activated
if (throttleStatus == THROTTLE_LOW && !STATE(AIRMODE_ACTIVE)) {
if (throttleIsLow && !STATE(AIRMODE_ACTIVE)) {
pidResetErrorAccumulators();
}
}
Expand Down
23 changes: 13 additions & 10 deletions src/main/fc/rc_controls.c
View file
Open in desktop
Expand Up @@ -111,22 +111,25 @@ bool isRollPitchStickDeflected(uint8_t deadband)

throttleStatus_e FAST_CODE NOINLINE calculateThrottleStatus(throttleStatusType_e type)
{
int value;
if (type == THROTTLE_STATUS_TYPE_RC) {
value = rxGetChannelValue(THROTTLE);
} else {
int value = rxGetChannelValue(THROTTLE); // THROTTLE_STATUS_TYPE_RC
if (type == THROTTLE_STATUS_TYPE_COMMAND) {
value = rcCommand[THROTTLE];
}

const uint16_t mid_throttle_deadband = rcControlsConfig()->mid_throttle_deadband;
if (feature(FEATURE_REVERSIBLE_MOTORS) && (value > (PWM_RANGE_MIDDLE - mid_throttle_deadband) && value < (PWM_RANGE_MIDDLE + mid_throttle_deadband)))
return THROTTLE_LOW;
else if (!feature(FEATURE_REVERSIBLE_MOTORS) && (value < rxConfig()->mincheck))
bool midThrottle = value > (PWM_RANGE_MIDDLE - mid_throttle_deadband) && value < (PWM_RANGE_MIDDLE + mid_throttle_deadband);
if ((feature(FEATURE_REVERSIBLE_MOTORS) && midThrottle) || (!feature(FEATURE_REVERSIBLE_MOTORS) && (value < rxConfig()->mincheck))) {
return THROTTLE_LOW;
}

return THROTTLE_HIGH;
}

bool throttleStickIsLow(void)
{
return calculateThrottleStatus(feature(FEATURE_REVERSIBLE_MOTORS) ? THROTTLE_STATUS_TYPE_COMMAND : THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW;
}

int16_t throttleStickMixedValue(void)
{
int16_t throttleValue;
Expand Down Expand Up @@ -181,7 +184,7 @@ static void updateRcStickPositions(void)
rcStickPositions = tmp;
}

void processRcStickPositions(throttleStatus_e throttleStatus)
void processRcStickPositions(bool isThrottleLow)
{
static timeMs_t lastTickTimeMs = 0;
static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors
Expand Down Expand Up @@ -211,7 +214,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus)

if (STATE(AIRPLANE) && feature(FEATURE_MOTOR_STOP) && armingConfig()->fixed_wing_auto_arm) {
// Auto arm on throttle when using fixedwing and motorstop
if (throttleStatus != THROTTLE_LOW) {
if (!isThrottleLow) {
tryArm();
return;
}
Expand All @@ -227,7 +230,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
if (ARMING_FLAG(ARMED) && !IS_RC_MODE_ACTIVE(BOXFAILSAFE) && rxIsReceivingSignal() && !failsafeIsActive()) {
const timeMs_t disarmDelay = currentTimeMs - rcDisarmTimeMs;
if (disarmDelay > armingConfig()->switchDisarmDelayMs) {
if (armingConfig()->disarm_kill_switch || (throttleStatus == THROTTLE_LOW)) {
if (armingConfig()->disarm_kill_switch || isThrottleLow) {
disarm(DISARM_SWITCH);
}
}
Expand Down
9 changes: 5 additions & 4 deletions src/main/fc/rc_controls.h
View file
Open in desktop
Expand Up @@ -87,16 +87,16 @@ typedef struct rcControlsConfig_s {
uint8_t pos_hold_deadband; // Deadband for position hold
uint8_t control_deadband; // General deadband to check if sticks are deflected, us PWM.
uint8_t alt_hold_deadband; // Defines the neutral zone of throttle stick during altitude hold
uint16_t mid_throttle_deadband; // default throttle deadband from MIDRC
uint16_t mid_throttle_deadband; // default throttle deadband from MIDRC
uint8_t airmodeHandlingType; // Defaults to ANTI_WINDUP triggered at sticks centered
uint16_t airmodeThrottleThreshold; // Throttle threshold for airmode initial activation
} rcControlsConfig_t;

PG_DECLARE(rcControlsConfig_t, rcControlsConfig);

typedef struct armingConfig_s {
bool fixed_wing_auto_arm; // Auto-arm fixed wing aircraft on throttle up and never disarm
bool disarm_kill_switch; // allow disarm via AUX switch regardless of throttle value
bool fixed_wing_auto_arm; // Auto-arm fixed wing aircraft on throttle up and never disarm
bool disarm_kill_switch; // allow disarm via AUX switch regardless of throttle value
uint16_t switchDisarmDelayMs; // additional delay between ARM box going off and actual disarm
uint16_t prearmTimeoutMs; // duration for which Prearm being activated is valid. after this, Prearm needs to be reset. 0 means Prearm does not timeout.
} armingConfig_t;
Expand All @@ -112,6 +112,7 @@ bool isRollPitchStickDeflected(uint8_t deadband);
throttleStatus_e calculateThrottleStatus(throttleStatusType_e type);
int16_t throttleStickMixedValue(void);
rollPitchStatus_e calculateRollPitchCenterStatus(void);
void processRcStickPositions(throttleStatus_e throttleStatus);
void processRcStickPositions(bool isThrottleLow);
bool throttleStickIsLow(void);

int32_t getRcStickDeflection(int32_t axis);
2 changes: 1 addition & 1 deletion src/main/flight/failsafe.c
View file
Open in desktop
Expand Up @@ -387,7 +387,7 @@ void failsafeUpdateState(void)
case FAILSAFE_IDLE:
if (armed) {
// Track throttle command below minimum time
if (THROTTLE_HIGH == calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC)) {
if (!throttleStickIsLow()) {
failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND;
}
if (!receivingRxDataAndNotFailsafeMode) {
Expand Down
6 changes: 1 addition & 5 deletions src/main/flight/mixer.c
View file
Open in desktop
Expand Up @@ -626,11 +626,8 @@ motorStatus_e getMotorStatus(void)
}

const bool fixedWingOrAirmodeNotActive = STATE(FIXED_WING_LEGACY) || !STATE(AIRMODE_ACTIVE);
const bool throttleStickLow =
(calculateThrottleStatus(feature(FEATURE_REVERSIBLE_MOTORS) ? THROTTLE_STATUS_TYPE_COMMAND : THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW);

if (throttleStickLow && fixedWingOrAirmodeNotActive) {

if (throttleStickIsLow() && fixedWingOrAirmodeNotActive) {
if ((navConfig()->general.flags.nav_overrides_motor_stop == NOMS_OFF_ALWAYS) && failsafeIsActive()) {
// If we are in failsafe and user was holding stick low before it was triggered and nav_overrides_motor_stop is set to OFF_ALWAYS
// and either on a plane or on a quad with inactive airmode - stop motor
Expand All @@ -652,7 +649,6 @@ motorStatus_e getMotorStatus(void)
return MOTOR_STOPPED_USER;
}
}

}

return MOTOR_RUNNING;
Expand Down
3 changes: 1 addition & 2 deletions src/main/navigation/navigation.c
View file
Open in desktop
Expand Up @@ -4327,8 +4327,7 @@ bool isNavLaunchEnabled(void)
bool abortLaunchAllowed(void)
{
// allow NAV_LAUNCH_MODE to be aborted if throttle is low or throttle stick position is < launch idle throttle setting
throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
return throttleStatus == THROTTLE_LOW || throttleStickMixedValue() < currentBatteryProfile->nav.fw.launch_idle_throttle;
return throttleStickIsLow() || throttleStickMixedValue() < currentBatteryProfile->nav.fw.launch_idle_throttle;
}

int32_t navigationGetHomeHeading(void)
Expand Down
3 changes: 1 addition & 2 deletions src/main/navigation/navigation_fixedwing.c
View file
Open in desktop
Expand Up @@ -685,12 +685,11 @@ bool isFixedWingLandingDetected(void)
{
DEBUG_SET(DEBUG_LANDING, 4, 0);
static bool fixAxisCheck = false;
const bool throttleIsLow = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW;

// Basic condition to start looking for landing
bool startCondition = (navGetCurrentStateFlags() & (NAV_CTL_LAND | NAV_CTL_EMERG))
|| FLIGHT_MODE(FAILSAFE_MODE)
|| (!navigationIsControllingThrottle() && throttleIsLow);
|| (!navigationIsControllingThrottle() && throttleStickIsLow());

if (!startCondition || posControl.flags.resetLandingDetector) {
return fixAxisCheck = posControl.flags.resetLandingDetector = false;
Expand Down
17 changes: 6 additions & 11 deletions src/main/navigation/navigation_fw_launch.c
View file
Open in desktop
Expand Up @@ -248,11 +248,6 @@ static void applyThrottleIdleLogic(bool forceMixerIdle)
}
}

static inline bool isThrottleLow(void)
{
return calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW;
}

static inline bool isLaunchMaxAltitudeReached(void)
{
return (navConfig()->fw.launch_max_altitude > 0) && (getEstimatedActualPosition(Z) >= navConfig()->fw.launch_max_altitude);
Expand All @@ -272,7 +267,7 @@ static inline bool isProbablyNotFlying(void)

static void resetPidsIfNeeded(void) {
// Don't use PID I-term and reset TPA filter until motors are started or until flight is detected
if (isProbablyNotFlying() || fwLaunch.currentState < FW_LAUNCH_STATE_MOTOR_SPINUP || (navConfig()->fw.launch_manual_throttle && isThrottleLow())) {
if (isProbablyNotFlying() || fwLaunch.currentState < FW_LAUNCH_STATE_MOTOR_SPINUP || (navConfig()->fw.launch_manual_throttle && throttleStickIsLow())) {
pidResetErrorAccumulators();
pidResetTPAFilter();
}
Expand All @@ -292,7 +287,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs
{
UNUSED(currentTimeUs);

if (!isThrottleLow()) {
if (!throttleStickIsLow()) {
if (isThrottleIdleEnabled()) {
return FW_LAUNCH_EVENT_SUCCESS;
}
Expand All @@ -312,7 +307,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs

static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE_MOTOR_DELAY(timeUs_t currentTimeUs)
{
if (isThrottleLow()) {
if (throttleStickIsLow()) {
return FW_LAUNCH_EVENT_THROTTLE_LOW; // go back to FW_LAUNCH_STATE_WAIT_THROTTLE
}

Expand All @@ -330,7 +325,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE_MOTOR_DELAY(tim

static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t currentTimeUs)
{
if (isThrottleLow()) {
if (throttleStickIsLow()) {
return FW_LAUNCH_EVENT_THROTTLE_LOW; // go back to FW_LAUNCH_STATE_WAIT_THROTTLE
}

Expand All @@ -349,7 +344,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t

static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeUs_t currentTimeUs)
{
if (isThrottleLow()) {
if (throttleStickIsLow()) {
return FW_LAUNCH_EVENT_THROTTLE_LOW; // go back to FW_LAUNCH_STATE_WAIT_THROTTLE
}

Expand Down Expand Up @@ -427,7 +422,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t

if (navConfig()->fw.launch_manual_throttle) {
// reset timers when throttle is low or until flight detected and abort launch regardless of launch settings
if (isThrottleLow()) {
if (throttleStickIsLow()) {
fwLaunch.currentStateTimeUs = currentTimeUs;
fwLaunch.pitchAngle = 0;
if (isRollPitchStickDeflected(navConfig()->fw.launch_abort_deadband)) {
Expand Down
11 changes: 5 additions & 6 deletions src/main/navigation/navigation_multicopter.c
View file
Open in desktop
Expand Up @@ -176,14 +176,14 @@ bool adjustMulticopterAltitudeFromRCInput(void)

void setupMulticopterAltitudeController(void)
{
const throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
const bool throttleIsLow = throttleStickIsLow();

if (navConfig()->general.flags.use_thr_mid_for_althold) {
altHoldThrottleRCZero = rcLookupThrottleMid();
}
else {
// If throttle status is THROTTLE_LOW - use Thr Mid anyway
if (throttleStatus == THROTTLE_LOW) {
// If throttle is LOW - use Thr Mid anyway
if (throttleIsLow) {
altHoldThrottleRCZero = rcLookupThrottleMid();
}
else {
Expand All @@ -198,7 +198,7 @@ void setupMulticopterAltitudeController(void)

// Force AH controller to initialize althold integral for pending takeoff on reset
// Signal for that is low throttle _and_ low actual altitude
if (throttleStatus == THROTTLE_LOW && fabsf(navGetCurrentActualPositionAndVelocity()->pos.z) <= 50.0f) {
if (throttleIsLow && fabsf(navGetCurrentActualPositionAndVelocity()->pos.z) <= 50.0f) {
prepareForTakeoffOnReset = true;
}
}
Expand Down Expand Up @@ -724,13 +724,12 @@ bool isMulticopterLandingDetected(void)
{
DEBUG_SET(DEBUG_LANDING, 4, 0);
static timeUs_t landingDetectorStartedAt;
const bool throttleIsLow = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW;

/* Basic condition to start looking for landing
* Prevent landing detection if WP mission allowed during Failsafe (except landing states) */
bool startCondition = (navGetCurrentStateFlags() & (NAV_CTL_LAND | NAV_CTL_EMERG))
|| (FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_WP_MODE))
|| (!navigationIsFlyingAutonomousMode() && throttleIsLow);
|| (!navigationIsFlyingAutonomousMode() && throttleStickIsLow());

if (!startCondition || posControl.flags.resetLandingDetector) {
landingDetectorStartedAt = 0;
Expand Down
3 changes: 1 addition & 2 deletions src/main/sensors/battery.c
View file
Open in desktop
Expand Up @@ -547,7 +547,6 @@ void currentMeterUpdate(timeUs_t timeDelta)
case CURRENT_SENSOR_VIRTUAL:
amperage = batteryMetersConfig()->current.offset;
if (ARMING_FLAG(ARMED)) {
throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags();
bool allNav = navConfig()->general.flags.nav_overrides_motor_stop == NOMS_ALL_NAV && posControl.navState != NAV_STATE_IDLE;
bool autoNav = navConfig()->general.flags.nav_overrides_motor_stop == NOMS_AUTO_ONLY && (stateFlags & (NAV_AUTO_RTH | NAV_AUTO_WP));
Expand All @@ -556,7 +555,7 @@ void currentMeterUpdate(timeUs_t timeDelta)
if (allNav || autoNav) { // account for motors running in Nav modes with throttle low + motor stop
throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000;
} else {
throttleOffset = ((throttleStatus == THROTTLE_LOW) && feature(FEATURE_MOTOR_STOP)) ? 0 : (int32_t)rcCommand[THROTTLE] - 1000;
throttleOffset = (throttleStickIsLow() && feature(FEATURE_MOTOR_STOP)) ? 0 : (int32_t)rcCommand[THROTTLE] - 1000;
}
int32_t throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50);
amperage += throttleFactor * batteryMetersConfig()->current.scale / 1000;
Expand Down
6 changes: 3 additions & 3 deletions src/main/telemetry/frsky_d.c
View file
Open in desktop
Expand Up @@ -195,10 +195,10 @@ static void sendThrottleOrBatterySizeAsRpm(void)
{
sendDataHead(ID_RPM);
if (ARMING_FLAG(ARMED)) {
const throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
uint16_t throttleForRPM = rcCommand[THROTTLE] / BLADE_NUMBER_DIVIDER;
if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP))
throttleForRPM = 0;
if (throttleStickIsLow() && feature(FEATURE_MOTOR_STOP)) {
throttleForRPM = 0;
}
serialize16(throttleForRPM);
} else {
serialize16((currentBatteryProfile->capacity.value / BLADE_NUMBER_DIVIDER));
Expand Down