Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-23 16:25:26 +03:00
Code Issues Wiki Activity

Force mixer idle in launch mode while throttle stick is low

Browse source
This commit is contained in:
Konstantin (DigitalEntity) Sharlaimov 2020-11-19 09:55:08 +01:00
parent a3e7e3cb15
commit fd73511053
1 changed files with 9 additions and 14 deletions
Download patch file Download diff file Expand all files Collapse all files

23
src/main/navigation/navigation_fw_launch.c
View file

@ -212,19 +212,14 @@ static bool isThrottleIdleEnabled(void)
return navConfig()->fw.launch_idle_throttle > getThrottleIdleValue(); return navConfig()->fw.launch_idle_throttle > getThrottleIdleValue();
} }
static void forceMotorStopOrIdle(void) static void applyThrottleIdleLogic(bool forceMixerIdle)
{ {
ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); // If MOTOR_STOP is enabled mixer will keep motor stopped if (isThrottleIdleEnabled() && !forceMixerIdle) {
rcCommand[THROTTLE] = getThrottleIdleValue(); // If MOTOR_STOP is disabled, motors will spin at minthrottle
}
static void applyThrottleIdleLogic(void)
{
if (isThrottleIdleEnabled()) {
rcCommand[THROTTLE] = navConfig()->fw.launch_idle_throttle; rcCommand[THROTTLE] = navConfig()->fw.launch_idle_throttle;
} }
else { else {
forceMotorStopOrIdle(); ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); // If MOTOR_STOP is enabled mixer will keep motor stopped
rcCommand[THROTTLE] = getThrottleIdleValue(); // If MOTOR_STOP is disabled, motors will spin given throttle value
} }
} }
@ -282,7 +277,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs
} }
} }
else { else {
forceMotorStopOrIdle(); applyThrottleIdleLogic(true); // Stick low, force mixer idle (motor stop or low rpm)
} }
fwLaunch.pitchAngle = 0; fwLaunch.pitchAngle = 0;
@ -298,7 +293,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t
const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs); const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs);
if (elapsedTimeMs > LAUNCH_MOTOR_IDLE_SPINUP_TIME) { if (elapsedTimeMs > LAUNCH_MOTOR_IDLE_SPINUP_TIME) {
applyThrottleIdleLogic(); applyThrottleIdleLogic(false);
return FW_LAUNCH_EVENT_SUCCESS; return FW_LAUNCH_EVENT_SUCCESS;
} }
else { else {
@ -322,7 +317,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeU
const bool isBungeeLaunched = isForwardAccelerationHigh && isAircraftAlmostLevel; const bool isBungeeLaunched = isForwardAccelerationHigh && isAircraftAlmostLevel;
const bool isSwingLaunched = (swingVelocity > navConfig()->fw.launch_velocity_thresh) && (imuMeasuredAccelBF.x > 0); const bool isSwingLaunched = (swingVelocity > navConfig()->fw.launch_velocity_thresh) && (imuMeasuredAccelBF.x > 0);
applyThrottleIdleLogic(); applyThrottleIdleLogic(false);
if (isBungeeLaunched || isSwingLaunched) { if (isBungeeLaunched || isSwingLaunched) {
if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_time_thresh) { if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_time_thresh) {
@ -339,14 +334,14 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_DETECTED(timeUs_t cu
{ {
UNUSED(currentTimeUs); UNUSED(currentTimeUs);
// waiting for the navigation to move it to next step FW_LAUNCH_STATE_MOTOR_DELAY // waiting for the navigation to move it to next step FW_LAUNCH_STATE_MOTOR_DELAY
applyThrottleIdleLogic(); applyThrottleIdleLogic(false);
return FW_LAUNCH_EVENT_NONE; return FW_LAUNCH_EVENT_NONE;
} }
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY(timeUs_t currentTimeUs) static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY(timeUs_t currentTimeUs)
{ {
applyThrottleIdleLogic(); applyThrottleIdleLogic(false);
if (areSticksMoved(0, currentTimeUs)) { if (areSticksMoved(0, currentTimeUs)) {
return FW_LAUNCH_EVENT_ABORT; // jump to FW_LAUNCH_STATE_IDLE return FW_LAUNCH_EVENT_ABORT; // jump to FW_LAUNCH_STATE_IDLE