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

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

Browse source
This commit is contained in:
Konstantin (DigitalEntity) Sharlaimov 2020-11-19 08:44:30 +01:00
parent 02e1fb8d13
commit a3e7e3cb15
1 changed files with 97 additions and 38 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -119,6 +119,7 @@ static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE
}, },
.messageType = FW_LAUNCH_MESSAGE_TYPE_NONE .messageType = FW_LAUNCH_MESSAGE_TYPE_NONE
}, },
[FW_LAUNCH_STATE_WAIT_THROTTLE] = { [FW_LAUNCH_STATE_WAIT_THROTTLE] = {
.onEntry = fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE, .onEntry = fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE,
.onEvent = { .onEvent = {
@ -127,6 +128,7 @@ static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE
}, },
.messageType = FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE .messageType = FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE
}, },
[FW_LAUNCH_STATE_MOTOR_IDLE] = { [FW_LAUNCH_STATE_MOTOR_IDLE] = {
.onEntry = fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE, .onEntry = fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE,
.onEvent = { .onEvent = {
@ -135,6 +137,7 @@ static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE
}, },
.messageType = FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE .messageType = FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE
}, },
[FW_LAUNCH_STATE_WAIT_DETECTION] = { [FW_LAUNCH_STATE_WAIT_DETECTION] = {
.onEntry = fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION, .onEntry = fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION,
.onEvent = { .onEvent = {
@ -143,6 +146,7 @@ static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE
}, },
.messageType = FW_LAUNCH_MESSAGE_TYPE_WAIT_DETECTION .messageType = FW_LAUNCH_MESSAGE_TYPE_WAIT_DETECTION
}, },
[FW_LAUNCH_STATE_DETECTED] = { [FW_LAUNCH_STATE_DETECTED] = {
.onEntry = fwLaunchState_FW_LAUNCH_STATE_DETECTED, .onEntry = fwLaunchState_FW_LAUNCH_STATE_DETECTED,
.onEvent = { .onEvent = {
@ -150,6 +154,7 @@ static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE
}, },
.messageType = FW_LAUNCH_MESSAGE_TYPE_WAIT_DETECTION .messageType = FW_LAUNCH_MESSAGE_TYPE_WAIT_DETECTION
}, },
[FW_LAUNCH_STATE_MOTOR_DELAY] = { [FW_LAUNCH_STATE_MOTOR_DELAY] = {
.onEntry = fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY, .onEntry = fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY,
.onEvent = { .onEvent = {
@ -158,6 +163,7 @@ static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE
}, },
.messageType = FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS .messageType = FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS
}, },
[FW_LAUNCH_STATE_MOTOR_SPINUP] = { [FW_LAUNCH_STATE_MOTOR_SPINUP] = {
.onEntry = fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP, .onEntry = fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP,
.onEvent = { .onEvent = {
@ -166,6 +172,7 @@ static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE
}, },
.messageType = FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS .messageType = FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS
}, },
[FW_LAUNCH_STATE_IN_PROGRESS] = { [FW_LAUNCH_STATE_IN_PROGRESS] = {
.onEntry = fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS, .onEntry = fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS,
.onEvent = { .onEvent = {
@ -174,6 +181,7 @@ static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE
}, },
.messageType = FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS .messageType = FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS
}, },
[FW_LAUNCH_STATE_FINISH] = { [FW_LAUNCH_STATE_FINISH] = {
.onEntry = fwLaunchState_FW_LAUNCH_STATE_FINISH, .onEntry = fwLaunchState_FW_LAUNCH_STATE_FINISH,
.onEvent = { .onEvent = {
@ -186,39 +194,52 @@ static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE
/* Current State Handlers */ /* Current State Handlers */
static timeMs_t currentStateElapsedMs(timeUs_t currentTimeUs) { static timeMs_t currentStateElapsedMs(timeUs_t currentTimeUs)
{
return US2MS(currentTimeUs - fwLaunch.currentStateTimeUs); 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.currentState = nextState;
fwLaunch.currentStateTimeUs = currentTimeUs; fwLaunch.currentStateTimeUs = currentTimeUs;
} }
/* Wing control Helpers */ /* Wing control Helpers */
static bool isThrottleIdleEnabled(void) { static bool isThrottleIdleEnabled(void)
{
return navConfig()->fw.launch_idle_throttle > getThrottleIdleValue(); return navConfig()->fw.launch_idle_throttle > getThrottleIdleValue();
} }
static void applyThrottleIdle(void) { static void forceMotorStopOrIdle(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 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 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 {
forceMotorStopOrIdle();
}
} }
static inline bool isThrottleLow(void) { static inline bool isThrottleLow(void)
{
return calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW; 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); 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(); 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 // lock roll and yaw and apply needed pitch angle
rcCommand[ROLL] = 0; rcCommand[ROLL] = 0;
rcCommand[PITCH] = pidAngleToRcCommand(-DEGREES_TO_DECIDEGREES(fwLaunch.pitchAngle), pidProfile()->max_angle_inclination[FD_PITCH]); rcCommand[PITCH] = pidAngleToRcCommand(-DEGREES_TO_DECIDEGREES(fwLaunch.pitchAngle), pidProfile()->max_angle_inclination[FD_PITCH]);
@ -239,37 +261,47 @@ static void updateRcCommand(void) {
/* onEntry state handlers */ /* 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); UNUSED(currentTimeUs);
return FW_LAUNCH_EVENT_NONE; 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); UNUSED(currentTimeUs);
if (!isThrottleLow()) { if (!isThrottleLow()) {
if (isThrottleIdleEnabled()) { if (isThrottleIdleEnabled()) {
return FW_LAUNCH_EVENT_SUCCESS; return FW_LAUNCH_EVENT_SUCCESS;
} else { }
else {
fwLaunch.pitchAngle = navConfig()->fw.launch_climb_angle; fwLaunch.pitchAngle = navConfig()->fw.launch_climb_angle;
return FW_LAUNCH_EVENT_GOTO_DETECTION; return FW_LAUNCH_EVENT_GOTO_DETECTION;
} }
} }
else {
forceMotorStopOrIdle();
}
fwLaunch.pitchAngle = 0; fwLaunch.pitchAngle = 0;
return FW_LAUNCH_EVENT_NONE; 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()) { if (isThrottleLow()) {
return FW_LAUNCH_EVENT_THROTTLE_LOW; // go back to FW_LAUNCH_STATE_WAIT_THROTTLE return FW_LAUNCH_EVENT_THROTTLE_LOW; // go back to FW_LAUNCH_STATE_WAIT_THROTTLE
} }
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) {
applyThrottleIdle(); applyThrottleIdleLogic();
return FW_LAUNCH_EVENT_SUCCESS; return FW_LAUNCH_EVENT_SUCCESS;
} else { }
else {
rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle); 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); fwLaunch.pitchAngle = scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, 0, navConfig()->fw.launch_climb_angle);
} }
@ -277,7 +309,8 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t
return FW_LAUNCH_EVENT_NONE; 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()) { if (isThrottleLow()) {
return FW_LAUNCH_EVENT_THROTTLE_LOW; // go back to FW_LAUNCH_STATE_WAIT_THROTTLE return FW_LAUNCH_EVENT_THROTTLE_LOW; // go back to FW_LAUNCH_STATE_WAIT_THROTTLE
} }
@ -289,7 +322,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);
applyThrottleIdle(); applyThrottleIdleLogic();
if (isBungeeLaunched || isSwingLaunched) { if (isBungeeLaunched || isSwingLaunched) {
if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_time_thresh) { if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_time_thresh) {
@ -302,20 +335,23 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeU
return FW_LAUNCH_EVENT_NONE; 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); 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
applyThrottleIdle(); applyThrottleIdleLogic();
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)
applyThrottleIdle(); {
applyThrottleIdleLogic();
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
} }
if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_motor_timer) { if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_motor_timer) {
return FW_LAUNCH_EVENT_SUCCESS; return FW_LAUNCH_EVENT_SUCCESS;
} }
@ -323,7 +359,8 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY(timeUs_t
return FW_LAUNCH_EVENT_NONE; 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)) { if (areSticksMoved(navConfig()->fw.launch_motor_timer, currentTimeUs)) {
return FW_LAUNCH_EVENT_ABORT; // jump to FW_LAUNCH_STATE_IDLE return FW_LAUNCH_EVENT_ABORT; // jump to FW_LAUNCH_STATE_IDLE
} }
@ -335,7 +372,8 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_
if (elapsedTimeMs > motorSpinUpMs) { if (elapsedTimeMs > motorSpinUpMs) {
rcCommand[THROTTLE] = launchThrottle; rcCommand[THROTTLE] = launchThrottle;
return FW_LAUNCH_EVENT_SUCCESS; return FW_LAUNCH_EVENT_SUCCESS;
} else { }
else {
const uint16_t minIdleThrottle = MAX(getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle); const uint16_t minIdleThrottle = MAX(getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle);
rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, motorSpinUpMs, minIdleThrottle, launchThrottle); rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, motorSpinUpMs, minIdleThrottle, launchThrottle);
} }
@ -343,15 +381,22 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_
return FW_LAUNCH_EVENT_NONE; 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; 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)) { 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 return FW_LAUNCH_EVENT_ABORT; // cancel the launch and do the FW_LAUNCH_STATE_IDLE state
} }
if (isLaunchMaxAltitudeReached()) { if (isLaunchMaxAltitudeReached()) {
return FW_LAUNCH_EVENT_SUCCESS; // cancel the launch and do the FW_LAUNCH_STATE_FINISH state return FW_LAUNCH_EVENT_SUCCESS; // cancel the launch and do the FW_LAUNCH_STATE_FINISH state
} }
if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_timeout) { if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_timeout) {
return FW_LAUNCH_EVENT_SUCCESS; // launch timeout. go to FW_LAUNCH_STATE_FINISH return FW_LAUNCH_EVENT_SUCCESS; // launch timeout. go to FW_LAUNCH_STATE_FINISH
} }
@ -359,7 +404,8 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t
return FW_LAUNCH_EVENT_NONE; 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 elapsedTimeMs = currentStateElapsedMs(currentTimeUs);
const timeMs_t endTimeMs = navConfig()->fw.launch_end_time; 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) { if (elapsedTimeMs > endTimeMs) {
return FW_LAUNCH_EVENT_SUCCESS; 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 // 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]); 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]); fwLaunch.pitchAngle = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, navConfig()->fw.launch_climb_angle, rcCommand[PITCH]);
@ -379,7 +426,8 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t curr
// Public methods --------------------------------------------------------------- // Public methods ---------------------------------------------------------------
void applyFixedWingLaunchController(timeUs_t currentTimeUs) { void applyFixedWingLaunchController(timeUs_t currentTimeUs)
{
// Called at PID rate // Called at PID rate
// process the current state, set the next state or exit if FW_LAUNCH_EVENT_NONE // process the current state, set the next state or exit if FW_LAUNCH_EVENT_NONE
@ -397,41 +445,52 @@ void applyFixedWingLaunchController(timeUs_t currentTimeUs) {
// Control beeper // Control beeper
if (fwLaunch.currentState == FW_LAUNCH_STATE_WAIT_THROTTLE) { if (fwLaunch.currentState == FW_LAUNCH_STATE_WAIT_THROTTLE) {
beeper(BEEPER_LAUNCH_MODE_LOW_THROTTLE); beeper(BEEPER_LAUNCH_MODE_LOW_THROTTLE);
} else { }
else {
beeper(BEEPER_LAUNCH_MODE_ENABLED); beeper(BEEPER_LAUNCH_MODE_ENABLED);
} }
} }
void resetFixedWingLaunchController(timeUs_t currentTimeUs) { void resetFixedWingLaunchController(timeUs_t currentTimeUs)
{
setCurrentState(FW_LAUNCH_STATE_WAIT_THROTTLE, currentTimeUs); setCurrentState(FW_LAUNCH_STATE_WAIT_THROTTLE, currentTimeUs);
} }
bool isFixedWingLaunchDetected(void) { bool isFixedWingLaunchDetected(void)
{
return fwLaunch.currentState == FW_LAUNCH_STATE_DETECTED; return fwLaunch.currentState == FW_LAUNCH_STATE_DETECTED;
} }
void enableFixedWingLaunchController(timeUs_t currentTimeUs) { void enableFixedWingLaunchController(timeUs_t currentTimeUs)
{
setCurrentState(FW_LAUNCH_STATE_MOTOR_DELAY, currentTimeUs); setCurrentState(FW_LAUNCH_STATE_MOTOR_DELAY, currentTimeUs);
} }
bool isFixedWingLaunchFinishedOrAborted(void) { bool isFixedWingLaunchFinishedOrAborted(void)
{
return fwLaunch.currentState == FW_LAUNCH_STATE_IDLE; return fwLaunch.currentState == FW_LAUNCH_STATE_IDLE;
} }
void abortFixedWingLaunch(void) { void abortFixedWingLaunch(void)
{
setCurrentState(FW_LAUNCH_STATE_IDLE, 0); setCurrentState(FW_LAUNCH_STATE_IDLE, 0);
} }
const char * fixedWingLaunchStateMessage(void) { const char * fixedWingLaunchStateMessage(void)
{
switch (launchStateMachine[fwLaunch.currentState].messageType) { switch (launchStateMachine[fwLaunch.currentState].messageType) {
case FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE: case FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE:
return FW_LAUNCH_MESSAGE_TEXT_WAIT_THROTTLE; return FW_LAUNCH_MESSAGE_TEXT_WAIT_THROTTLE;
case FW_LAUNCH_MESSAGE_TYPE_WAIT_DETECTION: case FW_LAUNCH_MESSAGE_TYPE_WAIT_DETECTION:
return FW_LAUNCH_MESSAGE_TEXT_WAIT_DETECTION; return FW_LAUNCH_MESSAGE_TEXT_WAIT_DETECTION;
case FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS: case FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS:
return FW_LAUNCH_MESSAGE_TEXT_IN_PROGRESS; return FW_LAUNCH_MESSAGE_TEXT_IN_PROGRESS;
case FW_LAUNCH_MESSAGE_TYPE_FINISHING: case FW_LAUNCH_MESSAGE_TYPE_FINISHING:
return FW_LAUNCH_MESSAGE_TEXT_FINISHING; return FW_LAUNCH_MESSAGE_TEXT_FINISHING;
default: default:
return NULL; return NULL;
} }