From 6206f034a846378a9cfaf24165d01283abe33b99 Mon Sep 17 00:00:00 2001 From: mikeller Date: Thu, 18 Oct 2018 19:37:53 +1300 Subject: [PATCH 1/3] Improved handling of airmode detection. --- src/main/fc/rc_modes.c | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/fc/rc_modes.c b/src/main/fc/rc_modes.c index 44a059f295..1b4831b9fd 100644 --- a/src/main/fc/rc_modes.c +++ b/src/main/fc/rc_modes.c @@ -45,6 +45,8 @@ boxBitmask_t rcModeActivationMask; // one bit per mode defined in boxId_e static boxBitmask_t stickyModesEverDisabled; +static bool airmodeActive; + PG_REGISTER_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions, PG_MODE_ACTIVATION_PROFILE, 1); @@ -59,7 +61,7 @@ void rcModeUpdate(boxBitmask_t *newState) } bool isAirmodeActive(void) { - return (IS_RC_MODE_ACTIVE(BOXAIRMODE) || featureIsEnabled(FEATURE_AIRMODE)); + return airmodeActive; } bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range) { @@ -136,6 +138,8 @@ void updateActivatedModes(void) } rcModeUpdate(&newMask); + + airmodeActive = featureIsEnabled(FEATURE_AIRMODE) || IS_RC_MODE_ACTIVE(BOXAIRMODE); } bool isModeActivationConditionPresent(boxId_e modeId) From 817bb2ed8651dc695d8f2c39425cb32792fefd36 Mon Sep 17 00:00:00 2001 From: mikeller Date: Fri, 19 Oct 2018 00:20:17 +1300 Subject: [PATCH 2/3] Rename 'isAirmodeActive()' to 'airmodeIsEnabled()'. --- src/main/fc/core.c | 8 ++++---- src/main/fc/rc_modes.c | 8 ++++---- src/main/fc/rc_modes.h | 2 +- src/main/flight/mixer.c | 7 ++++--- src/main/io/osd.c | 2 +- src/main/telemetry/crsf.c | 2 +- 6 files changed, 15 insertions(+), 14 deletions(-) diff --git a/src/main/fc/core.c b/src/main/fc/core.c index 87e34e9d4b..31400a7a74 100644 --- a/src/main/fc/core.c +++ b/src/main/fc/core.c @@ -578,7 +578,7 @@ bool processRx(timeUs_t currentTimeUs) const throttleStatus_e throttleStatus = calculateThrottleStatus(); const uint8_t throttlePercent = calculateThrottlePercent(); - if (isAirmodeActive() && ARMING_FLAG(ARMED)) { + if (airmodeIsEnabled() && ARMING_FLAG(ARMED)) { if (throttlePercent >= rxConfig()->airModeActivateThreshold) { airmodeIsActivated = true; // Prevent iterm from being reset } @@ -618,7 +618,7 @@ bool processRx(timeUs_t currentTimeUs) // - sticks are active and have deflection greater than runaway_takeoff_deactivate_stick_percent // - pidSum on all axis is less then runaway_takeoff_deactivate_pidlimit bool inStableFlight = false; - if (!featureIsEnabled(FEATURE_MOTOR_STOP) || isAirmodeActive() || (throttleStatus != THROTTLE_LOW)) { // are motors running? + if (!featureIsEnabled(FEATURE_MOTOR_STOP) || airmodeIsEnabled() || (throttleStatus != THROTTLE_LOW)) { // are motors running? const uint8_t lowThrottleLimit = pidConfig()->runaway_takeoff_deactivate_throttle; const uint8_t midThrottleLimit = constrain(lowThrottleLimit * 2, lowThrottleLimit * 2, RUNAWAY_TAKEOFF_HIGH_THROTTLE_PERCENT); if ((((throttlePercent >= lowThrottleLimit) && areSticksActive(RUNAWAY_TAKEOFF_DEACTIVATE_STICK_PERCENT)) || (throttlePercent >= midThrottleLimit)) @@ -674,7 +674,7 @@ bool processRx(timeUs_t currentTimeUs) && featureIsEnabled(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING) && !featureIsEnabled(FEATURE_3D) - && !isAirmodeActive() + && !airmodeIsEnabled() ) { if (isUsingSticksForArming()) { if (throttleStatus == THROTTLE_LOW) { @@ -872,7 +872,7 @@ static FAST_CODE void subTaskPidController(timeUs_t currentTimeUs) && !runawayTakeoffCheckDisabled && !flipOverAfterCrashMode && !runawayTakeoffTemporarilyDisabled - && (!featureIsEnabled(FEATURE_MOTOR_STOP) || isAirmodeActive() || (calculateThrottleStatus() != THROTTLE_LOW))) { + && (!featureIsEnabled(FEATURE_MOTOR_STOP) || airmodeIsEnabled() || (calculateThrottleStatus() != THROTTLE_LOW))) { if (((fabsf(pidData[FD_PITCH].Sum) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD) || (fabsf(pidData[FD_ROLL].Sum) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD) diff --git a/src/main/fc/rc_modes.c b/src/main/fc/rc_modes.c index 1b4831b9fd..8828415f50 100644 --- a/src/main/fc/rc_modes.c +++ b/src/main/fc/rc_modes.c @@ -45,7 +45,7 @@ boxBitmask_t rcModeActivationMask; // one bit per mode defined in boxId_e static boxBitmask_t stickyModesEverDisabled; -static bool airmodeActive; +static bool airmodeEnabled; PG_REGISTER_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions, PG_MODE_ACTIVATION_PROFILE, 1); @@ -60,8 +60,8 @@ void rcModeUpdate(boxBitmask_t *newState) rcModeActivationMask = *newState; } -bool isAirmodeActive(void) { - return airmodeActive; +bool airmodeIsEnabled(void) { + return airmodeEnabled; } bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range) { @@ -139,7 +139,7 @@ void updateActivatedModes(void) rcModeUpdate(&newMask); - airmodeActive = featureIsEnabled(FEATURE_AIRMODE) || IS_RC_MODE_ACTIVE(BOXAIRMODE); + airmodeEnabled = featureIsEnabled(FEATURE_AIRMODE) || IS_RC_MODE_ACTIVE(BOXAIRMODE); } bool isModeActivationConditionPresent(boxId_e modeId) diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index da3c7ea7af..bac7c6cd37 100644 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -126,7 +126,7 @@ typedef struct modeActivationProfile_s { bool IS_RC_MODE_ACTIVE(boxId_e boxId); void rcModeUpdate(boxBitmask_t *newState); -bool isAirmodeActive(void); +bool airmodeIsEnabled(void); bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range); void updateActivatedModes(void); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index e581725e8b..3d61823474 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -712,6 +712,7 @@ static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS]) } else { motorOutput = constrain(motorOutput, motorRangeMin, motorRangeMax); } + motor[i] = motorOutput; } @@ -788,7 +789,7 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensa #ifdef USE_YAW_SPIN_RECOVERY // 50% throttle provides the maximum authority for yaw recovery when airmode is not active. // When airmode is active the throttle setting doesn't impact recovery authority. - if (yawSpinDetected && !isAirmodeActive()) { + if (yawSpinDetected && !airmodeIsEnabled()) { throttle = 0.5f; // } #endif // USE_YAW_SPIN_RECOVERY @@ -835,11 +836,11 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensa motorMix[i] /= motorMixRange; } // Get the maximum correction by setting offset to center when airmode enabled - if (isAirmodeActive()) { + if (airmodeIsEnabled()) { throttle = 0.5f; } } else { - if (isAirmodeActive() || throttle > 0.5f) { // Only automatically adjust throttle when airmode enabled. Airmode logic is always active on high throttle + if (airmodeIsEnabled() || throttle > 0.5f) { // Only automatically adjust throttle when airmode enabled. Airmode logic is always active on high throttle const float throttleLimitOffset = motorMixRange / 2.0f; throttle = constrainf(throttle, 0.0f + throttleLimitOffset, 1.0f - throttleLimitOffset); } diff --git a/src/main/io/osd.c b/src/main/io/osd.c index dfc41763df..207d434a00 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -637,7 +637,7 @@ static bool osdDrawSingleElement(uint8_t item) strcpy(buff, "HOR "); } else if (IS_RC_MODE_ACTIVE(BOXACROTRAINER)) { strcpy(buff, "ATRN"); - } else if (isAirmodeActive()) { + } else if (airmodeIsEnabled()) { strcpy(buff, "AIR "); } else { strcpy(buff, "ACRO"); diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index 0c86995d26..e60643f87a 100644 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -268,7 +268,7 @@ void crsfFrameFlightMode(sbuf_t *dst) // use same logic as OSD, so telemetry displays same flight text as OSD const char *flightMode = "ACRO"; - if (isAirmodeActive()) { + if (airmodeIsEnabled()) { flightMode = "AIR"; } if (FLIGHT_MODE(FAILSAFE_MODE)) { From feaa082ac3abdae3c603bfd9c571b33131940ffd Mon Sep 17 00:00:00 2001 From: mikeller Date: Mon, 22 Oct 2018 12:38:05 +1300 Subject: [PATCH 3/3] Fixes from review. --- src/main/flight/mixer.c | 11 ++++++----- src/test/unit/osd_unittest.cc | 2 +- src/test/unit/telemetry_crsf_msp_unittest.cc | 2 +- src/test/unit/telemetry_crsf_unittest.cc | 2 +- 4 files changed, 9 insertions(+), 8 deletions(-) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 3d61823474..e02fc2c774 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -712,7 +712,6 @@ static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS]) } else { motorOutput = constrain(motorOutput, motorRangeMin, motorRangeMax); } - motor[i] = motorOutput; } @@ -786,10 +785,12 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensa throttle = applyThrottleLimit(throttle); } + const bool airmodeEnabled = airmodeIsEnabled(); + #ifdef USE_YAW_SPIN_RECOVERY // 50% throttle provides the maximum authority for yaw recovery when airmode is not active. // When airmode is active the throttle setting doesn't impact recovery authority. - if (yawSpinDetected && !airmodeIsEnabled()) { + if (yawSpinDetected && !airmodeEnabled) { throttle = 0.5f; // } #endif // USE_YAW_SPIN_RECOVERY @@ -836,11 +837,11 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensa motorMix[i] /= motorMixRange; } // Get the maximum correction by setting offset to center when airmode enabled - if (airmodeIsEnabled()) { + if (airmodeEnabled) { throttle = 0.5f; } } else { - if (airmodeIsEnabled() || throttle > 0.5f) { // Only automatically adjust throttle when airmode enabled. Airmode logic is always active on high throttle + if (airmodeEnabled || throttle > 0.5f) { // Only automatically adjust throttle when airmode enabled. Airmode logic is always active on high throttle const float throttleLimitOffset = motorMixRange / 2.0f; throttle = constrainf(throttle, 0.0f + throttleLimitOffset, 1.0f - throttleLimitOffset); } @@ -849,7 +850,7 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensa if (featureIsEnabled(FEATURE_MOTOR_STOP) && ARMING_FLAG(ARMED) && !featureIsEnabled(FEATURE_3D) - && !isAirmodeActive() + && !airmodeEnabled && (rcData[THROTTLE] < rxConfig()->mincheck)) { // motor_stop handling applyMotorStop(); diff --git a/src/test/unit/osd_unittest.cc b/src/test/unit/osd_unittest.cc index 9beb0bf418..528020399e 100644 --- a/src/test/unit/osd_unittest.cc +++ b/src/test/unit/osd_unittest.cc @@ -972,7 +972,7 @@ extern "C" { return false; } - bool isAirmodeActive() { + bool airmodeIsEnabled() { return false; } diff --git a/src/test/unit/telemetry_crsf_msp_unittest.cc b/src/test/unit/telemetry_crsf_msp_unittest.cc index fe6f65d614..ddd5225daf 100644 --- a/src/test/unit/telemetry_crsf_msp_unittest.cc +++ b/src/test/unit/telemetry_crsf_msp_unittest.cc @@ -278,7 +278,7 @@ extern "C" { bool featureIsEnabled(uint32_t) {return false;} - bool isAirmodeActive(void) {return true;} + bool airmodeIsEnabled(void) {return true;} mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn) { diff --git a/src/test/unit/telemetry_crsf_unittest.cc b/src/test/unit/telemetry_crsf_unittest.cc index a48e1d122a..fe2cf8332b 100644 --- a/src/test/unit/telemetry_crsf_unittest.cc +++ b/src/test/unit/telemetry_crsf_unittest.cc @@ -312,7 +312,7 @@ bool telemetryCheckRxPortShared(const serialPortConfig_t *) {return true;} portSharing_e determinePortSharing(const serialPortConfig_t *, serialPortFunction_e) {return PORTSHARING_NOT_SHARED;} -bool isAirmodeActive(void) {return airMode;} +bool airmodeIsEnabled(void) {return airMode;} int32_t getAmperage(void) { return testAmperage;