englebert
/
inav-simplefc
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Merge pull request #7068 from breadoven/abo_improve_launch_status_reporting 

FW launch status reporting improvement
master
Paweł Spychalski 3 years ago
committed by GitHub
parent
9fb072e712 83529656e7
commit
255b5b470b
No known key found for this signature in database GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 64 additions and 48 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 6
      src/main/fc/fc_core.c
  2. 4
      src/main/navigation/navigation.c
  3. 9
      src/main/navigation/navigation.h
  4. 91
      src/main/navigation/navigation_fw_launch.c
  5. 2
      src/main/navigation/navigation_private.h

6
src/main/fc/fc_core.c
View File

@ -864,7 +864,11 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
cycleTime = getTaskDeltaTime(TASK_SELF);
dT = (float)cycleTime * 0.000001f;
if (ARMING_FLAG(ARMED) && (!STATE(FIXED_WING_LEGACY) || !isNavLaunchEnabled() || (isNavLaunchEnabled() && (isFixedWingLaunchDetected() || isFixedWingLaunchFinishedOrAborted())))) {
#if defined(USE_NAV)
if (ARMING_FLAG(ARMED) && (!STATE(FIXED_WING_LEGACY) || !isNavLaunchEnabled() || (isNavLaunchEnabled() && fixedWingLaunchStatus() >= FW_LAUNCH_DETECTED))) {
#else
if (ARMING_FLAG(ARMED)) {
#endif
flightTime += cycleTime;
armTime += cycleTime;
updateAccExtremes();

4
src/main/navigation/navigation.c
View File

@ -1716,7 +1716,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_WAIT(navigationF
const timeUs_t currentTimeUs = micros();
UNUSED(previousState);
if (isFixedWingLaunchDetected()) {
if (fixedWingLaunchStatus() == FW_LAUNCH_DETECTED) {
enableFixedWingLaunchController(currentTimeUs);
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_LAUNCH_IN_PROGRESS
}
@ -1737,7 +1737,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_IN_PROGRESS(navi
{
UNUSED(previousState);
if (isFixedWingLaunchFinishedOrAborted()) {
if (fixedWingLaunchStatus() >= FW_LAUNCH_ABORTED) {
return NAV_FSM_EVENT_SUCCESS;
}

9
src/main/navigation/navigation.h
View File

@ -147,6 +147,12 @@ typedef enum {
ON_FW_SPIRAL,
} navRTHClimbFirst_e;
typedef enum { // keep aligned with fixedWingLaunchState_t
FW_LAUNCH_DETECTED = 4,
FW_LAUNCH_ABORTED = 9,
FW_LAUNCH_FLYING = 10,
} navFwLaunchStatus_e;
typedef enum {
WP_PLAN_WAIT,
WP_PLAN_SAVE,
@ -561,8 +567,7 @@ bool isWaypointMissionRTHActive(void);
bool rthClimbStageActiveAndComplete(void);
bool isNavLaunchEnabled(void);
bool isFixedWingLaunchDetected(void);
bool isFixedWingLaunchFinishedOrAborted(void);
uint8_t fixedWingLaunchStatus(void);
const char * fixedWingLaunchStateMessage(void);
float calculateAverageSpeed(void);

91
src/main/navigation/navigation_fw_launch.c
View File

@ -79,21 +79,21 @@ typedef enum {
FW_LAUNCH_EVENT_COUNT
} fixedWingLaunchEvent_t;
typedef enum {
FW_LAUNCH_STATE_IDLE = 0,
FW_LAUNCH_STATE_WAIT_THROTTLE,
typedef enum { // if changed update navFwLaunchStatus_e
FW_LAUNCH_STATE_WAIT_THROTTLE = 0,
FW_LAUNCH_STATE_IDLE_MOTOR_DELAY,
FW_LAUNCH_STATE_MOTOR_IDLE,
FW_LAUNCH_STATE_WAIT_DETECTION,
FW_LAUNCH_STATE_DETECTED,
FW_LAUNCH_STATE_DETECTED, // 4
FW_LAUNCH_STATE_MOTOR_DELAY,
FW_LAUNCH_STATE_MOTOR_SPINUP,
FW_LAUNCH_STATE_IN_PROGRESS,
FW_LAUNCH_STATE_FINISH,
FW_LAUNCH_STATE_ABORTED, // 9
FW_LAUNCH_STATE_FLYING, // 10
FW_LAUNCH_STATE_COUNT
} fixedWingLaunchState_t;
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE(timeUs_t currentTimeUs);
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs_t currentTimeUs);
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE_MOTOR_DELAY(timeUs_t currentTimeUs);
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t currentTimeUs);
@ -103,6 +103,8 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY(timeUs_t
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_t currentTimeUs);
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t currentTimeUs);
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t currentTimeUs);
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_ABORTED(timeUs_t currentTimeUs);
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FLYING(timeUs_t currentTimeUs);
typedef struct fixedWingLaunchStateDescriptor_s {
fixedWingLaunchEvent_t (*onEntry)(timeUs_t currentTimeUs);
@ -121,14 +123,6 @@ static bool idleMotorAboutToStart;
static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE_COUNT] = {
[FW_LAUNCH_STATE_IDLE] = {
.onEntry = fwLaunchState_FW_LAUNCH_STATE_IDLE,
.onEvent = {
},
.messageType = FW_LAUNCH_MESSAGE_TYPE_NONE
},
[FW_LAUNCH_STATE_WAIT_THROTTLE] = {
.onEntry = fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE,
.onEvent = {
@ -177,7 +171,7 @@ static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE
.onEntry = fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY,
.onEvent = {
[FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_MOTOR_SPINUP,
[FW_LAUNCH_EVENT_ABORT] = FW_LAUNCH_STATE_IDLE
[FW_LAUNCH_EVENT_ABORT] = FW_LAUNCH_STATE_ABORTED
},
.messageType = FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS
},
@ -186,7 +180,7 @@ static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE
.onEntry = fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP,
.onEvent = {
[FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_IN_PROGRESS,
[FW_LAUNCH_EVENT_ABORT] = FW_LAUNCH_STATE_IDLE
[FW_LAUNCH_EVENT_ABORT] = FW_LAUNCH_STATE_ABORTED
},
.messageType = FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS
},
@ -195,7 +189,7 @@ static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE
.onEntry = fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS,
.onEvent = {
[FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_FINISH,
[FW_LAUNCH_EVENT_ABORT] = FW_LAUNCH_STATE_IDLE
[FW_LAUNCH_EVENT_ABORT] = FW_LAUNCH_STATE_ABORTED
},
.messageType = FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS
},
@ -203,10 +197,25 @@ static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE
[FW_LAUNCH_STATE_FINISH] = {
.onEntry = fwLaunchState_FW_LAUNCH_STATE_FINISH,
.onEvent = {
[FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_IDLE,
[FW_LAUNCH_EVENT_ABORT] = FW_LAUNCH_STATE_IDLE
[FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_FLYING
},
.messageType = FW_LAUNCH_MESSAGE_TYPE_FINISHING
},
[FW_LAUNCH_STATE_ABORTED] = {
.onEntry = fwLaunchState_FW_LAUNCH_STATE_ABORTED,
.onEvent = {
},
.messageType = FW_LAUNCH_MESSAGE_TYPE_NONE
},
[FW_LAUNCH_STATE_FLYING] = {
.onEntry = fwLaunchState_FW_LAUNCH_STATE_FLYING,
.onEvent = {
},
.messageType = FW_LAUNCH_MESSAGE_TYPE_NONE
}
};
@ -274,13 +283,6 @@ static void updateRcCommand(void)
/* onEntry state handlers */
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)
{
UNUSED(currentTimeUs);
@ -370,6 +372,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeU
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
applyThrottleIdleLogic(false);
@ -381,7 +384,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY(timeUs_t
applyThrottleIdleLogic(false);
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_ABORTED
}
if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_motor_timer) {
@ -394,7 +397,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY(timeUs_t
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
return FW_LAUNCH_EVENT_ABORT; // jump to FW_LAUNCH_STATE_ABORTED
}
const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs);
@ -422,7 +425,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t
}
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_ABORTED state
}
if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_timeout) {
@ -437,11 +440,8 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t curr
const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs);
const timeMs_t endTimeMs = navConfig()->fw.launch_end_time;
if (isRollPitchStickDeflected()) {
return FW_LAUNCH_EVENT_ABORT; // cancel the launch and do the FW_LAUNCH_STATE_IDLE state
}
if (elapsedTimeMs > endTimeMs) {
return FW_LAUNCH_EVENT_SUCCESS;
if (elapsedTimeMs > endTimeMs || isRollPitchStickDeflected()) {
return FW_LAUNCH_EVENT_SUCCESS; // End launch go to FW_LAUNCH_STATE_FLYING state
}
else {
// make a smooth transition from the launch state to the current state for throttle and the pitch angle
@ -453,6 +453,20 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t curr
return FW_LAUNCH_EVENT_NONE;
}
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_ABORTED(timeUs_t currentTimeUs)
{
UNUSED(currentTimeUs);
return FW_LAUNCH_EVENT_NONE;
}
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FLYING(timeUs_t currentTimeUs)
{
UNUSED(currentTimeUs);
return FW_LAUNCH_EVENT_NONE;
}
// Public methods ---------------------------------------------------------------
void applyFixedWingLaunchController(timeUs_t currentTimeUs)
@ -489,24 +503,19 @@ void resetFixedWingLaunchController(timeUs_t currentTimeUs)
setCurrentState(FW_LAUNCH_STATE_WAIT_THROTTLE, currentTimeUs);
}
bool isFixedWingLaunchDetected(void)
{
return fwLaunch.currentState >= FW_LAUNCH_STATE_DETECTED;
}
void enableFixedWingLaunchController(timeUs_t currentTimeUs)
{
setCurrentState(FW_LAUNCH_STATE_MOTOR_DELAY, currentTimeUs);
}
bool isFixedWingLaunchFinishedOrAborted(void)
uint8_t fixedWingLaunchStatus(void)
{
return fwLaunch.currentState == FW_LAUNCH_STATE_IDLE;
return fwLaunch.currentState;
}
void abortFixedWingLaunch(void)
{
setCurrentState(FW_LAUNCH_STATE_IDLE, 0);
setCurrentState(FW_LAUNCH_STATE_ABORTED, 0);
}
const char * fixedWingLaunchStateMessage(void)

2
src/main/navigation/navigation_private.h
View File

@ -471,9 +471,7 @@ void calculateFixedWingInitialHoldPosition(fpVector3_t * pos);
/* Fixed-wing launch controller */
void resetFixedWingLaunchController(timeUs_t currentTimeUs);
bool isFixedWingLaunchDetected(void);
void enableFixedWingLaunchController(timeUs_t currentTimeUs);
bool isFixedWingLaunchFinishedOrAborted(void);
void abortFixedWingLaunch(void);
void applyFixedWingLaunchController(timeUs_t currentTimeUs);

Write Preview
Loading…
Cancel
Save