[LAUNCH] Some readability refactoring; Fix bug with throttling up at zero throttle

Author: digitalentity

src/main/navigation/navigation_fw_launch.c

@@ -119,6 +119,7 @@ static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE
         },
         .messageType                                = FW_LAUNCH_MESSAGE_TYPE_NONE
     },
+
     [FW_LAUNCH_STATE_WAIT_THROTTLE] = {
         .onEntry                                    = fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE,
         .onEvent = {
@@ -127,6 +128,7 @@ static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE
         },
         .messageType                                = FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE
     },
+
     [FW_LAUNCH_STATE_MOTOR_IDLE] = {
         .onEntry                                    = fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE,
         .onEvent = {
@@ -135,6 +137,7 @@ static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE
         },
         .messageType                                = FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE
     },
+
     [FW_LAUNCH_STATE_WAIT_DETECTION] = {
         .onEntry                                    = fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION,
         .onEvent = {
@@ -143,13 +146,15 @@ static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE
         },
         .messageType                                = FW_LAUNCH_MESSAGE_TYPE_WAIT_DETECTION
     },
+
     [FW_LAUNCH_STATE_DETECTED] = {
         .onEntry                                    = fwLaunchState_FW_LAUNCH_STATE_DETECTED,
         .onEvent = {
             // waiting for the navigation to move on the next state FW_LAUNCH_STATE_MOTOR_DELAY
         },
         .messageType                                = FW_LAUNCH_MESSAGE_TYPE_WAIT_DETECTION
     },
+
     [FW_LAUNCH_STATE_MOTOR_DELAY] = {
         .onEntry                                    = fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY,
         .onEvent = {
@@ -158,6 +163,7 @@ static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE
         },
         .messageType                                = FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS
     },
+
     [FW_LAUNCH_STATE_MOTOR_SPINUP] = {
         .onEntry                                    = fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP,
         .onEvent = {
@@ -166,6 +172,7 @@ static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE
         },
         .messageType                                = FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS
     },
+
     [FW_LAUNCH_STATE_IN_PROGRESS] = {
         .onEntry                                    = fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS,
         .onEvent = {
@@ -174,6 +181,7 @@ static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE
         },
         .messageType                                = FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS
     },
+
     [FW_LAUNCH_STATE_FINISH] = {
         .onEntry                                    = fwLaunchState_FW_LAUNCH_STATE_FINISH,
         .onEvent = {
@@ -186,39 +194,52 @@ static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE
 
 /* Current State Handlers */
 
-static timeMs_t currentStateElapsedMs(timeUs_t currentTimeUs) {
+static timeMs_t currentStateElapsedMs(timeUs_t currentTimeUs)
+{
     return US2MS(currentTimeUs - fwLaunch.currentStateTimeUs);
 }
 
-static void setCurrentState(fixedWingLaunchState_t nextState, timeUs_t currentTimeUs) {
+static void setCurrentState(fixedWingLaunchState_t nextState, timeUs_t currentTimeUs)
+{
     fwLaunch.currentState = nextState;
     fwLaunch.currentStateTimeUs = currentTimeUs;
 }
 
 /* Wing control Helpers */
 
-static bool isThrottleIdleEnabled(void) {
+static bool isThrottleIdleEnabled(void)
+{
     return navConfig()->fw.launch_idle_throttle > getThrottleIdleValue();
 }
 
-static void applyThrottleIdle(void) {
+static void forceMotorStopOrIdle(void)
+{
+    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 at minthrottle
+}
+
+static void applyThrottleIdleLogic(void)
+{
     if (isThrottleIdleEnabled()) {
         rcCommand[THROTTLE] = navConfig()->fw.launch_idle_throttle;
-    } else {
-      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 at minthrottle
+    }
+    else {
+        forceMotorStopOrIdle();
     }
 }
 
-static inline bool isThrottleLow(void) {
+static inline bool isThrottleLow(void)
+{
     return calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW;
 }
 
-static inline bool isLaunchMaxAltitudeReached(void) {
+static inline bool isLaunchMaxAltitudeReached(void)
+{
     return (navConfig()->fw.launch_max_altitude > 0) && (getEstimatedActualPosition(Z) >= navConfig()->fw.launch_max_altitude);
 }
 
-static inline bool areSticksMoved(timeMs_t initialTime, timeUs_t currentTimeUs) {
+static inline bool areSticksMoved(timeMs_t initialTime, timeUs_t currentTimeUs)
+{
     return (initialTime + currentStateElapsedMs(currentTimeUs)) > navConfig()->fw.launch_min_time && areSticksDeflectedMoreThanPosHoldDeadband();
 }
 
@@ -230,7 +251,8 @@ static void resetPidsIfNeeded(void) {
     }
 }
 
-static void updateRcCommand(void) {
+static void updateRcCommand(void)
+{
     // lock roll and yaw and apply needed pitch angle
     rcCommand[ROLL] = 0;
     rcCommand[PITCH] = pidAngleToRcCommand(-DEGREES_TO_DECIDEGREES(fwLaunch.pitchAngle), pidProfile()->max_angle_inclination[FD_PITCH]);
@@ -239,57 +261,68 @@ static void updateRcCommand(void) {
 
 /* onEntry state handlers */
 
-static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE(timeUs_t currentTimeUs) {
+static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE(timeUs_t currentTimeUs)
+{
     UNUSED(currentTimeUs);
 
     return FW_LAUNCH_EVENT_NONE;
 }
 
-static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs_t currentTimeUs) {
+static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs_t currentTimeUs)
+{
     UNUSED(currentTimeUs);
 
     if (!isThrottleLow()) {
         if (isThrottleIdleEnabled()) {
             return FW_LAUNCH_EVENT_SUCCESS;
-        } else {
+        }
+        else {
             fwLaunch.pitchAngle = navConfig()->fw.launch_climb_angle;
             return FW_LAUNCH_EVENT_GOTO_DETECTION;
         }
     }
+    else {
+        forceMotorStopOrIdle();
+    }
+
     fwLaunch.pitchAngle = 0;
 
     return FW_LAUNCH_EVENT_NONE;
 }
 
-static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t currentTimeUs) {
+static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t currentTimeUs)
+{
     if (isThrottleLow()) {
         return FW_LAUNCH_EVENT_THROTTLE_LOW; // go back to FW_LAUNCH_STATE_WAIT_THROTTLE
     }
+
     const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs);
     if (elapsedTimeMs > LAUNCH_MOTOR_IDLE_SPINUP_TIME) {
-        applyThrottleIdle();
+        applyThrottleIdleLogic();
         return FW_LAUNCH_EVENT_SUCCESS;
-    } else {
+    }
+    else {
         rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle);
         fwLaunch.pitchAngle = scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, 0, navConfig()->fw.launch_climb_angle);
     }
 
     return FW_LAUNCH_EVENT_NONE;
 }
 
-static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeUs_t currentTimeUs) {
+static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeUs_t currentTimeUs)
+{
     if (isThrottleLow()) {
         return FW_LAUNCH_EVENT_THROTTLE_LOW; // go back to FW_LAUNCH_STATE_WAIT_THROTTLE
     }
-    
+
     const float swingVelocity = (fabsf(imuMeasuredRotationBF.z) > SWING_LAUNCH_MIN_ROTATION_RATE) ? (imuMeasuredAccelBF.y / imuMeasuredRotationBF.z) : 0;
     const bool isForwardAccelerationHigh = (imuMeasuredAccelBF.x > navConfig()->fw.launch_accel_thresh);
     const bool isAircraftAlmostLevel = (calculateCosTiltAngle() >= cos_approx(DEGREES_TO_RADIANS(navConfig()->fw.launch_max_angle)));
 
     const bool isBungeeLaunched = isForwardAccelerationHigh && isAircraftAlmostLevel;
     const bool isSwingLaunched = (swingVelocity > navConfig()->fw.launch_velocity_thresh) && (imuMeasuredAccelBF.x > 0);
 
-    applyThrottleIdle();
+    applyThrottleIdleLogic();
 
     if (isBungeeLaunched || isSwingLaunched) {
         if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_time_thresh) {
@@ -302,28 +335,32 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeU
     return FW_LAUNCH_EVENT_NONE;
 }
 
-static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_DETECTED(timeUs_t currentTimeUs) {
+static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_DETECTED(timeUs_t currentTimeUs)
+{
     UNUSED(currentTimeUs);
     // waiting for the navigation to move it to next step FW_LAUNCH_STATE_MOTOR_DELAY
-    applyThrottleIdle();
+    applyThrottleIdleLogic();
 
     return FW_LAUNCH_EVENT_NONE;
 }
 
-static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY(timeUs_t currentTimeUs) {
-    applyThrottleIdle();
+static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY(timeUs_t currentTimeUs)
+{
+    applyThrottleIdleLogic();
 
     if (areSticksMoved(0, currentTimeUs)) {
         return FW_LAUNCH_EVENT_ABORT; // jump to FW_LAUNCH_STATE_IDLE
     }
+
     if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_motor_timer) {
         return FW_LAUNCH_EVENT_SUCCESS;
     }
 
     return FW_LAUNCH_EVENT_NONE;
 }
 
-static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_t currentTimeUs) {
+static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_t currentTimeUs)
+{
     if (areSticksMoved(navConfig()->fw.launch_motor_timer, currentTimeUs)) {
         return FW_LAUNCH_EVENT_ABORT; // jump to FW_LAUNCH_STATE_IDLE
     }
@@ -335,31 +372,40 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_
     if (elapsedTimeMs > motorSpinUpMs) {
         rcCommand[THROTTLE] = launchThrottle;
         return FW_LAUNCH_EVENT_SUCCESS;
-    } else {
+    }
+    else {
         const uint16_t minIdleThrottle = MAX(getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle);
         rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, motorSpinUpMs,  minIdleThrottle, launchThrottle);
     }
 
     return FW_LAUNCH_EVENT_NONE;
 }
 
-static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t currentTimeUs) {
+static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t currentTimeUs)
+{
     rcCommand[THROTTLE] = navConfig()->fw.launch_throttle;
 
+    if (isLaunchMaxAltitudeReached()) {
+        return FW_LAUNCH_EVENT_SUCCESS; // cancel the launch and do the FW_LAUNCH_STATE_FINISH state
+    }
+
     if (areSticksMoved(navConfig()->fw.launch_motor_timer + navConfig()->fw.launch_motor_spinup_time, currentTimeUs)) {
         return FW_LAUNCH_EVENT_ABORT; // cancel the launch and do the FW_LAUNCH_STATE_IDLE state
     }
+
     if (isLaunchMaxAltitudeReached()) {
         return FW_LAUNCH_EVENT_SUCCESS; // cancel the launch and do the FW_LAUNCH_STATE_FINISH state
     }
+
     if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_timeout) {
         return FW_LAUNCH_EVENT_SUCCESS; // launch timeout. go to FW_LAUNCH_STATE_FINISH
     }
 
     return FW_LAUNCH_EVENT_NONE;
 }
 
-static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t currentTimeUs) {
+static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t currentTimeUs)
+{
     const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs);
     const timeMs_t endTimeMs = navConfig()->fw.launch_end_time;
 
@@ -368,7 +414,8 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t curr
     }
     if (elapsedTimeMs > endTimeMs) {
         return FW_LAUNCH_EVENT_SUCCESS;
-    } else {
+    }
+    else {
         // make a smooth transition from the launch state to the current state for throttle and the pitch angle
         rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs,  navConfig()->fw.launch_throttle, rcCommand[THROTTLE]);
         fwLaunch.pitchAngle = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, navConfig()->fw.launch_climb_angle, rcCommand[PITCH]);
@@ -379,9 +426,10 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t curr
 
 // Public methods ---------------------------------------------------------------
 
-void applyFixedWingLaunchController(timeUs_t currentTimeUs) {
+void applyFixedWingLaunchController(timeUs_t currentTimeUs)
+{
     // Called at PID rate
-    
+
     // process the current state, set the next state or exit if FW_LAUNCH_EVENT_NONE
     while (launchStateMachine[fwLaunch.currentState].onEntry) {
         fixedWingLaunchEvent_t newEvent = launchStateMachine[fwLaunch.currentState].onEntry(currentTimeUs);
@@ -397,41 +445,52 @@ void applyFixedWingLaunchController(timeUs_t currentTimeUs) {
     // Control beeper
     if (fwLaunch.currentState == FW_LAUNCH_STATE_WAIT_THROTTLE) {
         beeper(BEEPER_LAUNCH_MODE_LOW_THROTTLE);
-    } else {
+    }
+    else {
         beeper(BEEPER_LAUNCH_MODE_ENABLED);
     }
 }
 
-void resetFixedWingLaunchController(timeUs_t currentTimeUs) {
+void resetFixedWingLaunchController(timeUs_t currentTimeUs)
+{
     setCurrentState(FW_LAUNCH_STATE_WAIT_THROTTLE, currentTimeUs);
 }
 
-bool isFixedWingLaunchDetected(void) {
+bool isFixedWingLaunchDetected(void)
+{
     return fwLaunch.currentState == FW_LAUNCH_STATE_DETECTED;
 }
 
-void enableFixedWingLaunchController(timeUs_t currentTimeUs) {
+void enableFixedWingLaunchController(timeUs_t currentTimeUs)
+{
     setCurrentState(FW_LAUNCH_STATE_MOTOR_DELAY, currentTimeUs);
 }
 
-bool isFixedWingLaunchFinishedOrAborted(void) {
+bool isFixedWingLaunchFinishedOrAborted(void)
+{
     return fwLaunch.currentState == FW_LAUNCH_STATE_IDLE;
 }
 
-void abortFixedWingLaunch(void) {
+void abortFixedWingLaunch(void)
+{
     setCurrentState(FW_LAUNCH_STATE_IDLE, 0);
 }
 
-const char * fixedWingLaunchStateMessage(void) {
+const char * fixedWingLaunchStateMessage(void)
+{
     switch (launchStateMachine[fwLaunch.currentState].messageType) {
         case FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE:
             return FW_LAUNCH_MESSAGE_TEXT_WAIT_THROTTLE;
+
         case FW_LAUNCH_MESSAGE_TYPE_WAIT_DETECTION:
             return FW_LAUNCH_MESSAGE_TEXT_WAIT_DETECTION;
+
         case FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS:
             return FW_LAUNCH_MESSAGE_TEXT_IN_PROGRESS;
+
         case FW_LAUNCH_MESSAGE_TYPE_FINISHING:
             return FW_LAUNCH_MESSAGE_TEXT_FINISHING;
+
         default:
             return NULL;
     }