mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-23 16:25:26 +03:00
Rename NAV CRUISE mode to NAV COURSE HOLD (#6297)
This commit is contained in:
parent
d019eeb99d
commit
3aadc493c0
15 changed files with 323 additions and 323 deletions
|
@ -217,8 +217,8 @@ static const OSD_Entry menuOsdElemsEntries[] =
|
|||
#endif // GPS
|
||||
OSD_ELEMENT_ENTRY("HEADING", OSD_HEADING),
|
||||
OSD_ELEMENT_ENTRY("HEADING GR.", OSD_HEADING_GRAPH),
|
||||
OSD_ELEMENT_ENTRY("CRS HEAD. ERR", OSD_CRUISE_HEADING_ERROR),
|
||||
OSD_ELEMENT_ENTRY("CRS HEAD. ADJ", OSD_CRUISE_HEADING_ADJUSTMENT),
|
||||
OSD_ELEMENT_ENTRY("CRS HLD ERR", OSD_COURSE_HOLD_ERROR),
|
||||
OSD_ELEMENT_ENTRY("CRS HLD ADJ", OSD_COURSE_HOLD_ADJUSTMENT),
|
||||
#if defined(USE_BARO) || defined(USE_GPS)
|
||||
OSD_ELEMENT_ENTRY("VARIO", OSD_VARIO),
|
||||
OSD_ELEMENT_ENTRY("VARIO NUM", OSD_VARIO_NUM),
|
||||
|
|
|
@ -82,7 +82,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
|
|||
{ BOXOSDALT1, "OSD ALT 1", 42 },
|
||||
{ BOXOSDALT2, "OSD ALT 2", 43 },
|
||||
{ BOXOSDALT3, "OSD ALT 3", 44 },
|
||||
{ BOXNAVCRUISE, "NAV CRUISE", 45 },
|
||||
{ BOXNAVCOURSEHOLD, "NAV COURSE HOLD", 45 },
|
||||
{ BOXBRAKING, "MC BRAKING", 46 },
|
||||
{ BOXUSER1, "USER1", BOX_PERMANENT_ID_USER1 },
|
||||
{ BOXUSER2, "USER2", BOX_PERMANENT_ID_USER2 },
|
||||
|
@ -219,7 +219,7 @@ void initActiveBoxIds(void)
|
|||
if (feature(FEATURE_GPS)) {
|
||||
activeBoxIds[activeBoxIdCount++] = BOXGCSNAV;
|
||||
if (STATE(AIRPLANE)) {
|
||||
activeBoxIds[activeBoxIdCount++] = BOXNAVCRUISE;
|
||||
activeBoxIds[activeBoxIdCount++] = BOXNAVCOURSEHOLD;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
@ -345,7 +345,7 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags)
|
|||
CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)), BOXFAILSAFE);
|
||||
CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_ALTHOLD_MODE)), BOXNAVALTHOLD);
|
||||
CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_POSHOLD_MODE)), BOXNAVPOSHOLD);
|
||||
CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_CRUISE_MODE)), BOXNAVCRUISE);
|
||||
CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_COURSE_HOLD_MODE)), BOXNAVCOURSEHOLD);
|
||||
CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_RTH_MODE)), BOXNAVRTH);
|
||||
CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_WP_MODE)), BOXNAVWP);
|
||||
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)), BOXAIRMODE);
|
||||
|
|
|
@ -206,7 +206,7 @@ void updateUsedModeActivationConditionFlags(void)
|
|||
#ifdef USE_NAV
|
||||
isUsingNAVModes = isModeActivationConditionPresent(BOXNAVPOSHOLD) ||
|
||||
isModeActivationConditionPresent(BOXNAVRTH) ||
|
||||
isModeActivationConditionPresent(BOXNAVCRUISE) ||
|
||||
isModeActivationConditionPresent(BOXNAVCOURSEHOLD) ||
|
||||
isModeActivationConditionPresent(BOXNAVWP);
|
||||
#endif
|
||||
}
|
||||
|
|
|
@ -61,7 +61,7 @@ typedef enum {
|
|||
BOXOSDALT1 = 32,
|
||||
BOXOSDALT2 = 33,
|
||||
BOXOSDALT3 = 34,
|
||||
BOXNAVCRUISE = 35,
|
||||
BOXNAVCOURSEHOLD = 35,
|
||||
BOXBRAKING = 36,
|
||||
BOXUSER1 = 37,
|
||||
BOXUSER2 = 38,
|
||||
|
|
|
@ -154,7 +154,7 @@ flightModeForTelemetry_e getFlightModeForTelemetry(void)
|
|||
if (FLIGHT_MODE(NAV_POSHOLD_MODE))
|
||||
return FLM_POSITION_HOLD;
|
||||
|
||||
if (FLIGHT_MODE(NAV_CRUISE_MODE))
|
||||
if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE))
|
||||
return FLM_CRUISE;
|
||||
|
||||
if (FLIGHT_MODE(NAV_WP_MODE))
|
||||
|
|
|
@ -94,7 +94,7 @@ typedef enum {
|
|||
FAILSAFE_MODE = (1 << 9),
|
||||
AUTO_TUNE = (1 << 10), // old G-Tune
|
||||
NAV_WP_MODE = (1 << 11),
|
||||
NAV_CRUISE_MODE = (1 << 12),
|
||||
NAV_COURSE_HOLD_MODE = (1 << 12),
|
||||
FLAPERON = (1 << 13),
|
||||
TURN_ASSISTANT = (1 << 14),
|
||||
FLIP_OVER_AFTER_CRASH = (1 << 15),
|
||||
|
|
|
@ -1487,18 +1487,18 @@ static bool osdDrawSingleElement(uint8_t item)
|
|||
break;
|
||||
}
|
||||
|
||||
case OSD_CRUISE_HEADING_ERROR:
|
||||
case OSD_COURSE_HOLD_ERROR:
|
||||
{
|
||||
if (ARMING_FLAG(ARMED) && !FLIGHT_MODE(NAV_CRUISE_MODE)) {
|
||||
if (ARMING_FLAG(ARMED) && !FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
|
||||
displayWrite(osdDisplayPort, elemPosX, elemPosY, " ");
|
||||
return true;
|
||||
}
|
||||
|
||||
buff[0] = SYM_HEADING;
|
||||
|
||||
if ((!ARMING_FLAG(ARMED)) || (FLIGHT_MODE(NAV_CRUISE_MODE) && isAdjustingPosition())) {
|
||||
if ((!ARMING_FLAG(ARMED)) || (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && isAdjustingPosition())) {
|
||||
buff[1] = buff[2] = buff[3] = '-';
|
||||
} else if (FLIGHT_MODE(NAV_CRUISE_MODE)) {
|
||||
} else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
|
||||
int16_t herr = lrintf(CENTIDEGREES_TO_DEGREES((float)navigationGetHeadingError()));
|
||||
if (ABS(herr) > 99)
|
||||
strcpy(buff + 1, ">99");
|
||||
|
@ -1511,11 +1511,11 @@ static bool osdDrawSingleElement(uint8_t item)
|
|||
break;
|
||||
}
|
||||
|
||||
case OSD_CRUISE_HEADING_ADJUSTMENT:
|
||||
case OSD_COURSE_HOLD_ADJUSTMENT:
|
||||
{
|
||||
int16_t heading_adjust = lrintf(CENTIDEGREES_TO_DEGREES((float)getCruiseHeadingAdjustment()));
|
||||
|
||||
if (ARMING_FLAG(ARMED) && ((!FLIGHT_MODE(NAV_CRUISE_MODE)) || !(isAdjustingPosition() || isAdjustingHeading() || (heading_adjust != 0)))) {
|
||||
if (ARMING_FLAG(ARMED) && ((!FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) || !(isAdjustingPosition() || isAdjustingHeading() || (heading_adjust != 0)))) {
|
||||
displayWrite(osdDisplayPort, elemPosX, elemPosY, " ");
|
||||
return true;
|
||||
}
|
||||
|
@ -1524,7 +1524,7 @@ static bool osdDrawSingleElement(uint8_t item)
|
|||
|
||||
if (!ARMING_FLAG(ARMED)) {
|
||||
buff[1] = buff[2] = buff[3] = buff[4] = '-';
|
||||
} else if (FLIGHT_MODE(NAV_CRUISE_MODE)) {
|
||||
} else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
|
||||
tfp_sprintf(buff + 1, "%4d", heading_adjust);
|
||||
}
|
||||
|
||||
|
@ -1675,10 +1675,10 @@ static bool osdDrawSingleElement(uint8_t item)
|
|||
p = "RTH ";
|
||||
else if (FLIGHT_MODE(NAV_POSHOLD_MODE))
|
||||
p = "HOLD";
|
||||
else if (FLIGHT_MODE(NAV_CRUISE_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE))
|
||||
p = "3CRS";
|
||||
else if (FLIGHT_MODE(NAV_CRUISE_MODE))
|
||||
p = "CRS ";
|
||||
else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE))
|
||||
p = "CRUZ";
|
||||
else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE))
|
||||
p = "CRSH";
|
||||
else if (FLIGHT_MODE(NAV_WP_MODE))
|
||||
p = " WP ";
|
||||
else if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && navigationRequiresAngleMode()) {
|
||||
|
@ -2074,7 +2074,7 @@ static bool osdDrawSingleElement(uint8_t item)
|
|||
return true;
|
||||
|
||||
case OSD_NAV_FW_CRUISE_THR:
|
||||
osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "CRS", 0, navConfig()->fw.cruise_throttle, 4, 0, ADJUSTMENT_NAV_FW_CRUISE_THR);
|
||||
osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "CRZ", 0, navConfig()->fw.cruise_throttle, 4, 0, ADJUSTMENT_NAV_FW_CRUISE_THR);
|
||||
return true;
|
||||
|
||||
case OSD_NAV_FW_PITCH2THR:
|
||||
|
@ -2744,8 +2744,8 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig)
|
|||
osdLayoutsConfig->item_pos[0][OSD_THROTTLE_POS] = OSD_POS(1, 2) | OSD_VISIBLE_FLAG;
|
||||
osdLayoutsConfig->item_pos[0][OSD_THROTTLE_POS_AUTO_THR] = OSD_POS(6, 2);
|
||||
osdLayoutsConfig->item_pos[0][OSD_HEADING] = OSD_POS(12, 2);
|
||||
osdLayoutsConfig->item_pos[0][OSD_CRUISE_HEADING_ERROR] = OSD_POS(12, 2);
|
||||
osdLayoutsConfig->item_pos[0][OSD_CRUISE_HEADING_ADJUSTMENT] = OSD_POS(12, 2);
|
||||
osdLayoutsConfig->item_pos[0][OSD_COURSE_HOLD_ERROR] = OSD_POS(12, 2);
|
||||
osdLayoutsConfig->item_pos[0][OSD_COURSE_HOLD_ADJUSTMENT] = OSD_POS(12, 2);
|
||||
osdLayoutsConfig->item_pos[0][OSD_HEADING_GRAPH] = OSD_POS(18, 2);
|
||||
osdLayoutsConfig->item_pos[0][OSD_CURRENT_DRAW] = OSD_POS(2, 3) | OSD_VISIBLE_FLAG;
|
||||
osdLayoutsConfig->item_pos[0][OSD_MAH_DRAWN] = OSD_POS(1, 4) | OSD_VISIBLE_FLAG;
|
||||
|
|
|
@ -151,8 +151,8 @@ typedef enum {
|
|||
OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH,
|
||||
OSD_REMAINING_DISTANCE_BEFORE_RTH,
|
||||
OSD_HOME_HEADING_ERROR,
|
||||
OSD_CRUISE_HEADING_ERROR,
|
||||
OSD_CRUISE_HEADING_ADJUSTMENT,
|
||||
OSD_COURSE_HOLD_ERROR,
|
||||
OSD_COURSE_HOLD_ADJUSTMENT,
|
||||
OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE,
|
||||
OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE,
|
||||
OSD_POWER_SUPPLY_IMPEDANCE,
|
||||
|
|
|
@ -236,12 +236,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(navi
|
|||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_IN_PROGRESS(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_INITIALIZE(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_IN_PROGRESS(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_ADJUSTING(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_3D_INITIALIZE(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_3D_IN_PROGRESS(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_3D_ADJUSTING(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_ADJUSTING(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_INITIALIZE(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_IN_PROGRESS(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_ADJUSTING(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(navigationFSMState_t previousState);
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigationFSMState_t previousState);
|
||||
|
@ -283,8 +283,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_LAUNCH] = NAV_STATE_LAUNCH_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
|
||||
}
|
||||
},
|
||||
|
||||
|
@ -319,8 +319,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
|
||||
}
|
||||
},
|
||||
|
||||
|
@ -355,63 +355,63 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
|
||||
}
|
||||
},
|
||||
/** CRUISE_2D mode ************************************************/
|
||||
[NAV_STATE_CRUISE_2D_INITIALIZE] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_CRUISE_2D_INITIALIZE,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_CRUISE_2D_INITIALIZE,
|
||||
/** CRUISE_HOLD mode ************************************************/
|
||||
[NAV_STATE_COURSE_HOLD_INITIALIZE] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_COURSE_HOLD_INITIALIZE,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE,
|
||||
.timeoutMs = 0,
|
||||
.stateFlags = NAV_REQUIRE_ANGLE,
|
||||
.mapToFlightModes = NAV_CRUISE_MODE,
|
||||
.mapToFlightModes = NAV_COURSE_HOLD_MODE,
|
||||
.mwState = MW_NAV_STATE_NONE,
|
||||
.mwError = MW_NAV_ERROR_NONE,
|
||||
.onEvent = {
|
||||
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_CRUISE_2D_IN_PROGRESS,
|
||||
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_COURSE_HOLD_IN_PROGRESS,
|
||||
[NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
|
||||
}
|
||||
},
|
||||
|
||||
[NAV_STATE_CRUISE_2D_IN_PROGRESS] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_CRUISE_2D_IN_PROGRESS,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_CRUISE_2D_IN_PROGRESS,
|
||||
[NAV_STATE_COURSE_HOLD_IN_PROGRESS] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_COURSE_HOLD_IN_PROGRESS,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS,
|
||||
.timeoutMs = 10,
|
||||
.stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_YAW,
|
||||
.mapToFlightModes = NAV_CRUISE_MODE,
|
||||
.mapToFlightModes = NAV_COURSE_HOLD_MODE,
|
||||
.mwState = MW_NAV_STATE_NONE,
|
||||
.mwError = MW_NAV_ERROR_NONE,
|
||||
.onEvent = {
|
||||
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_CRUISE_2D_IN_PROGRESS, // re-process the state
|
||||
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_COURSE_HOLD_IN_PROGRESS, // re-process the state
|
||||
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_ADJ] = NAV_STATE_CRUISE_2D_ADJUSTING,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_COURSE_ADJ] = NAV_STATE_COURSE_HOLD_ADJUSTING,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
|
||||
}
|
||||
},
|
||||
|
||||
[NAV_STATE_CRUISE_2D_ADJUSTING] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_CRUISE_2D_ADJUSTING,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_CRUISE_2D_ADJUSTING,
|
||||
[NAV_STATE_COURSE_HOLD_ADJUSTING] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_COURSE_HOLD_ADJUSTING,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_COURSE_HOLD_ADJUSTING,
|
||||
.timeoutMs = 10,
|
||||
.stateFlags = NAV_REQUIRE_ANGLE | NAV_RC_POS,
|
||||
.mapToFlightModes = NAV_CRUISE_MODE,
|
||||
.mapToFlightModes = NAV_COURSE_HOLD_MODE,
|
||||
.mwState = MW_NAV_STATE_NONE,
|
||||
.mwError = MW_NAV_ERROR_NONE,
|
||||
.onEvent = {
|
||||
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_CRUISE_2D_IN_PROGRESS,
|
||||
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_CRUISE_2D_ADJUSTING,
|
||||
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_COURSE_HOLD_IN_PROGRESS,
|
||||
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_COURSE_HOLD_ADJUSTING,
|
||||
[NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
|
||||
|
@ -419,58 +419,58 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
},
|
||||
|
||||
/** CRUISE_3D mode ************************************************/
|
||||
[NAV_STATE_CRUISE_3D_INITIALIZE] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_CRUISE_3D_INITIALIZE,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_CRUISE_3D_INITIALIZE,
|
||||
[NAV_STATE_CRUISE_INITIALIZE] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_CRUISE_INITIALIZE,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_CRUISE_INITIALIZE,
|
||||
.timeoutMs = 0,
|
||||
.stateFlags = NAV_REQUIRE_ANGLE,
|
||||
.mapToFlightModes = NAV_ALTHOLD_MODE | NAV_CRUISE_MODE,
|
||||
.mapToFlightModes = NAV_ALTHOLD_MODE | NAV_COURSE_HOLD_MODE,
|
||||
.mwState = MW_NAV_STATE_NONE,
|
||||
.mwError = MW_NAV_ERROR_NONE,
|
||||
.onEvent = {
|
||||
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_CRUISE_3D_IN_PROGRESS,
|
||||
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_CRUISE_IN_PROGRESS,
|
||||
[NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
|
||||
}
|
||||
},
|
||||
|
||||
[NAV_STATE_CRUISE_3D_IN_PROGRESS] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_CRUISE_3D_IN_PROGRESS,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_CRUISE_3D_IN_PROGRESS,
|
||||
[NAV_STATE_CRUISE_IN_PROGRESS] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_CRUISE_IN_PROGRESS,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_CRUISE_IN_PROGRESS,
|
||||
.timeoutMs = 10,
|
||||
.stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_ALT | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_YAW | NAV_RC_ALT,
|
||||
.mapToFlightModes = NAV_ALTHOLD_MODE | NAV_CRUISE_MODE,
|
||||
.mapToFlightModes = NAV_ALTHOLD_MODE | NAV_COURSE_HOLD_MODE,
|
||||
.mwState = MW_NAV_STATE_NONE,
|
||||
.mwError = MW_NAV_ERROR_NONE,
|
||||
.onEvent = {
|
||||
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_CRUISE_3D_IN_PROGRESS, // re-process the state
|
||||
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_CRUISE_IN_PROGRESS, // re-process the state
|
||||
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_ADJ] = NAV_STATE_CRUISE_3D_ADJUSTING,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_COURSE_ADJ] = NAV_STATE_CRUISE_ADJUSTING,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
|
||||
}
|
||||
},
|
||||
|
||||
[NAV_STATE_CRUISE_3D_ADJUSTING] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_CRUISE_3D_ADJUSTING,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_CRUISE_3D_ADJUSTING,
|
||||
[NAV_STATE_CRUISE_ADJUSTING] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_CRUISE_ADJUSTING,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_CRUISE_ADJUSTING,
|
||||
.timeoutMs = 10,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_ALT,
|
||||
.mapToFlightModes = NAV_ALTHOLD_MODE | NAV_CRUISE_MODE,
|
||||
.mapToFlightModes = NAV_ALTHOLD_MODE | NAV_COURSE_HOLD_MODE,
|
||||
.mwState = MW_NAV_STATE_NONE,
|
||||
.mwError = MW_NAV_ERROR_NONE,
|
||||
.onEvent = {
|
||||
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_CRUISE_3D_IN_PROGRESS,
|
||||
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_CRUISE_3D_ADJUSTING,
|
||||
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_CRUISE_IN_PROGRESS,
|
||||
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_CRUISE_ADJUSTING,
|
||||
[NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
|
||||
|
@ -510,8 +510,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
|
||||
}
|
||||
},
|
||||
|
||||
|
@ -530,8 +530,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
|
||||
}
|
||||
},
|
||||
|
||||
|
@ -551,8 +551,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
|
||||
}
|
||||
},
|
||||
|
||||
|
@ -570,8 +570,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
|
||||
}
|
||||
},
|
||||
|
||||
|
@ -674,8 +674,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
|
||||
}
|
||||
},
|
||||
|
||||
|
@ -699,8 +699,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
|
||||
}
|
||||
},
|
||||
|
||||
|
@ -720,8 +720,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
|
||||
}
|
||||
},
|
||||
|
||||
|
@ -741,8 +741,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
|
||||
}
|
||||
},
|
||||
|
||||
|
@ -774,8 +774,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
|
||||
}
|
||||
},
|
||||
|
||||
|
@ -795,8 +795,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
|
||||
}
|
||||
},
|
||||
|
||||
|
@ -1007,7 +1007,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS(
|
|||
}
|
||||
/////////////////
|
||||
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_INITIALIZE(navigationFSMState_t previousState)
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(navigationFSMState_t previousState)
|
||||
{
|
||||
const navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState);
|
||||
|
||||
|
@ -1027,7 +1027,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_INITIALIZE(na
|
|||
return NAV_FSM_EVENT_SUCCESS; // Go to CRUISE_XD_IN_PROGRESS state
|
||||
}
|
||||
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_IN_PROGRESS(navigationFSMState_t previousState)
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS(navigationFSMState_t previousState)
|
||||
{
|
||||
const timeMs_t currentTimeMs = millis();
|
||||
|
||||
|
@ -1037,7 +1037,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_IN_PROGRESS(n
|
|||
DEBUG_SET(DEBUG_CRUISE, 2, 0);
|
||||
|
||||
if (posControl.flags.isAdjustingPosition) {
|
||||
return NAV_FSM_EVENT_SWITCH_TO_CRUISE_ADJ;
|
||||
return NAV_FSM_EVENT_SWITCH_TO_COURSE_ADJ;
|
||||
}
|
||||
|
||||
// User is yawing. We record the desidered yaw and we change the desidered target in the meanwhile
|
||||
|
@ -1056,8 +1056,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_IN_PROGRESS(n
|
|||
|
||||
uint32_t distance = gpsSol.groundSpeed * 60; // next WP to be reached in 60s [cm]
|
||||
|
||||
if ((previousState == NAV_STATE_CRUISE_2D_INITIALIZE) || (previousState == NAV_STATE_CRUISE_2D_ADJUSTING)
|
||||
|| (previousState == NAV_STATE_CRUISE_3D_INITIALIZE) || (previousState == NAV_STATE_CRUISE_3D_ADJUSTING)
|
||||
if ((previousState == NAV_STATE_COURSE_HOLD_INITIALIZE) || (previousState == NAV_STATE_COURSE_HOLD_ADJUSTING)
|
||||
|| (previousState == NAV_STATE_CRUISE_INITIALIZE) || (previousState == NAV_STATE_CRUISE_ADJUSTING)
|
||||
|| posControl.flags.isAdjustingHeading) {
|
||||
calculateFarAwayTarget(&posControl.cruise.targetPos, posControl.cruise.yaw, distance);
|
||||
DEBUG_SET(DEBUG_CRUISE, 2, 1);
|
||||
|
@ -1071,7 +1071,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_IN_PROGRESS(n
|
|||
return NAV_FSM_EVENT_NONE;
|
||||
}
|
||||
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_ADJUSTING(navigationFSMState_t previousState)
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_ADJUSTING(navigationFSMState_t previousState)
|
||||
{
|
||||
UNUSED(previousState);
|
||||
DEBUG_SET(DEBUG_CRUISE, 0, 3);
|
||||
|
@ -1088,27 +1088,27 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_ADJUSTING(nav
|
|||
return NAV_FSM_EVENT_SUCCESS; // go back to the CRUISE_XD_IN_PROGRESS state
|
||||
}
|
||||
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_3D_INITIALIZE(navigationFSMState_t previousState)
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_INITIALIZE(navigationFSMState_t previousState)
|
||||
{
|
||||
if (!STATE(FIXED_WING_LEGACY)) { return NAV_FSM_EVENT_ERROR; } // only on FW for now
|
||||
|
||||
navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(previousState);
|
||||
|
||||
return navOnEnteringState_NAV_STATE_CRUISE_2D_INITIALIZE(previousState);
|
||||
return navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(previousState);
|
||||
}
|
||||
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_3D_IN_PROGRESS(navigationFSMState_t previousState)
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_IN_PROGRESS(navigationFSMState_t previousState)
|
||||
{
|
||||
navOnEnteringState_NAV_STATE_ALTHOLD_IN_PROGRESS(previousState);
|
||||
|
||||
return navOnEnteringState_NAV_STATE_CRUISE_2D_IN_PROGRESS(previousState);
|
||||
return navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS(previousState);
|
||||
}
|
||||
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_3D_ADJUSTING(navigationFSMState_t previousState)
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_ADJUSTING(navigationFSMState_t previousState)
|
||||
{
|
||||
navOnEnteringState_NAV_STATE_ALTHOLD_IN_PROGRESS(previousState);
|
||||
|
||||
return navOnEnteringState_NAV_STATE_CRUISE_2D_ADJUSTING(previousState);
|
||||
return navOnEnteringState_NAV_STATE_COURSE_HOLD_ADJUSTING(previousState);
|
||||
}
|
||||
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigationFSMState_t previousState)
|
||||
|
@ -3065,7 +3065,7 @@ void applyWaypointNavigationAndAltitudeHold(void)
|
|||
void switchNavigationFlightModes(void)
|
||||
{
|
||||
const flightModeFlags_e enabledNavFlightModes = navGetMappedFlightModes(posControl.navState);
|
||||
const flightModeFlags_e disabledFlightModes = (NAV_ALTHOLD_MODE | NAV_RTH_MODE | NAV_POSHOLD_MODE | NAV_WP_MODE | NAV_LAUNCH_MODE | NAV_CRUISE_MODE) & (~enabledNavFlightModes);
|
||||
const flightModeFlags_e disabledFlightModes = (NAV_ALTHOLD_MODE | NAV_RTH_MODE | NAV_POSHOLD_MODE | NAV_WP_MODE | NAV_LAUNCH_MODE | NAV_COURSE_HOLD_MODE) & (~enabledNavFlightModes);
|
||||
DISABLE_FLIGHT_MODE(disabledFlightModes);
|
||||
ENABLE_FLIGHT_MODE(enabledNavFlightModes);
|
||||
}
|
||||
|
@ -3182,13 +3182,13 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
|
|||
|
||||
// PH has priority over CRUISE
|
||||
// CRUISE has priority on AH
|
||||
if (IS_RC_MODE_ACTIVE(BOXNAVCRUISE)) {
|
||||
if (IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD)) {
|
||||
|
||||
if (IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) && ((FLIGHT_MODE(NAV_CRUISE_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivatePosHold && canActivateAltHold)))
|
||||
return NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D;
|
||||
if (IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) && ((FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivatePosHold && canActivateAltHold)))
|
||||
return NAV_FSM_EVENT_SWITCH_TO_CRUISE;
|
||||
|
||||
if (FLIGHT_MODE(NAV_CRUISE_MODE) || (canActivatePosHold))
|
||||
return NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D;
|
||||
if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) || (canActivatePosHold))
|
||||
return NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD;
|
||||
|
||||
}
|
||||
|
||||
|
@ -3270,8 +3270,8 @@ bool navigationTerrainFollowingEnabled(void)
|
|||
|
||||
navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
|
||||
{
|
||||
const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) || (STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) || (STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXNAVCRUISE));
|
||||
const bool navLaunchComboModesEnabled = isNavLaunchEnabled() && (IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) || IS_RC_MODE_ACTIVE(BOXNAVCRUISE));
|
||||
const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) || (STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) || (STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD));
|
||||
const bool navLaunchComboModesEnabled = isNavLaunchEnabled() && (IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) || IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD));
|
||||
|
||||
if (usedBypass) {
|
||||
*usedBypass = false;
|
||||
|
|
|
@ -259,7 +259,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
|
|||
bool needToCalculateCircularLoiter = (isApproachingLastWaypoint() || isWaypointWait())
|
||||
&& (distanceToActualTarget <= (navConfig()->fw.loiter_radius / TAN_15DEG))
|
||||
&& (distanceToActualTarget > 50.0f)
|
||||
&& !FLIGHT_MODE(NAV_CRUISE_MODE);
|
||||
&& !FLIGHT_MODE(NAV_COURSE_HOLD_MODE);
|
||||
|
||||
// Calculate virtual position for straight movement
|
||||
if (needToCalculateCircularLoiter) {
|
||||
|
@ -649,7 +649,7 @@ void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags,
|
|||
posControl.rcAdjustment[ROLL] = 0;
|
||||
}
|
||||
|
||||
if (FLIGHT_MODE(NAV_CRUISE_MODE) && posControl.flags.isAdjustingPosition)
|
||||
if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && posControl.flags.isAdjustingPosition)
|
||||
rcCommand[ROLL] = applyDeadband(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband);
|
||||
|
||||
//if (navStateFlags & NAV_CTL_YAW)
|
||||
|
|
|
@ -140,9 +140,9 @@ typedef enum {
|
|||
NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME = NAV_FSM_EVENT_STATE_SPECIFIC_3,
|
||||
NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOVER_ABOVE_HOME,
|
||||
|
||||
NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D,
|
||||
NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D,
|
||||
NAV_FSM_EVENT_SWITCH_TO_CRUISE_ADJ,
|
||||
NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD,
|
||||
NAV_FSM_EVENT_SWITCH_TO_CRUISE,
|
||||
NAV_FSM_EVENT_SWITCH_TO_COURSE_ADJ,
|
||||
NAV_FSM_EVENT_COUNT,
|
||||
} navigationFSMEvent_t;
|
||||
|
||||
|
@ -187,13 +187,13 @@ typedef enum {
|
|||
NAV_PERSISTENT_ID_UNUSED_3 = 27, // was NAV_STATE_LAUNCH_MOTOR_DELAY
|
||||
NAV_PERSISTENT_ID_LAUNCH_IN_PROGRESS = 28,
|
||||
|
||||
NAV_PERSISTENT_ID_CRUISE_2D_INITIALIZE = 29,
|
||||
NAV_PERSISTENT_ID_CRUISE_2D_IN_PROGRESS = 30,
|
||||
NAV_PERSISTENT_ID_CRUISE_2D_ADJUSTING = 31,
|
||||
NAV_PERSISTENT_ID_COURSE_HOLD_INITIALIZE = 29,
|
||||
NAV_PERSISTENT_ID_COURSE_HOLD_IN_PROGRESS = 30,
|
||||
NAV_PERSISTENT_ID_COURSE_HOLD_ADJUSTING = 31,
|
||||
|
||||
NAV_PERSISTENT_ID_CRUISE_3D_INITIALIZE = 32,
|
||||
NAV_PERSISTENT_ID_CRUISE_3D_IN_PROGRESS = 33,
|
||||
NAV_PERSISTENT_ID_CRUISE_3D_ADJUSTING = 34,
|
||||
NAV_PERSISTENT_ID_CRUISE_INITIALIZE = 32,
|
||||
NAV_PERSISTENT_ID_CRUISE_IN_PROGRESS = 33,
|
||||
NAV_PERSISTENT_ID_CRUISE_ADJUSTING = 34,
|
||||
|
||||
NAV_PERSISTENT_ID_WAYPOINT_HOLD_TIME = 35,
|
||||
NAV_PERSISTENT_ID_RTH_HOVER_ABOVE_HOME = 36,
|
||||
|
@ -239,12 +239,12 @@ typedef enum {
|
|||
NAV_STATE_LAUNCH_WAIT,
|
||||
NAV_STATE_LAUNCH_IN_PROGRESS,
|
||||
|
||||
NAV_STATE_CRUISE_2D_INITIALIZE,
|
||||
NAV_STATE_CRUISE_2D_IN_PROGRESS,
|
||||
NAV_STATE_CRUISE_2D_ADJUSTING,
|
||||
NAV_STATE_CRUISE_3D_INITIALIZE,
|
||||
NAV_STATE_CRUISE_3D_IN_PROGRESS,
|
||||
NAV_STATE_CRUISE_3D_ADJUSTING,
|
||||
NAV_STATE_COURSE_HOLD_INITIALIZE,
|
||||
NAV_STATE_COURSE_HOLD_IN_PROGRESS,
|
||||
NAV_STATE_COURSE_HOLD_ADJUSTING,
|
||||
NAV_STATE_CRUISE_INITIALIZE,
|
||||
NAV_STATE_CRUISE_IN_PROGRESS,
|
||||
NAV_STATE_CRUISE_ADJUSTING,
|
||||
|
||||
NAV_STATE_COUNT,
|
||||
} navigationFSMState_t;
|
||||
|
|
|
@ -560,7 +560,7 @@ static int logicConditionGetFlightModeOperandValue(int operand) {
|
|||
break;
|
||||
|
||||
case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_CRUISE:
|
||||
return (bool) FLIGHT_MODE(NAV_CRUISE_MODE);
|
||||
return (bool) FLIGHT_MODE(NAV_COURSE_HOLD_MODE);
|
||||
break;
|
||||
|
||||
case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ALTHOLD:
|
||||
|
|
|
@ -331,9 +331,9 @@ static void crsfFrameFlightMode(sbuf_t *dst)
|
|||
flightMode = "RTH";
|
||||
} else if (FLIGHT_MODE(NAV_POSHOLD_MODE)) {
|
||||
flightMode = "HOLD";
|
||||
} else if (FLIGHT_MODE(NAV_CRUISE_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) {
|
||||
} else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) {
|
||||
flightMode = "3CRS";
|
||||
} else if (FLIGHT_MODE(NAV_CRUISE_MODE)) {
|
||||
} else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
|
||||
flightMode = "CRS";
|
||||
} else if (FLIGHT_MODE(NAV_ALTHOLD_MODE)) {
|
||||
flightMode = "AH";
|
||||
|
|
|
@ -47,7 +47,7 @@ uint16_t frskyGetFlightMode(void)
|
|||
// thousands column
|
||||
if (FLIGHT_MODE(NAV_RTH_MODE))
|
||||
tmpi += 1000;
|
||||
if (FLIGHT_MODE(NAV_CRUISE_MODE)) // intentionally out of order and 'else-ifs' to prevent column overflow
|
||||
if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) // intentionally out of order and 'else-ifs' to prevent column overflow
|
||||
tmpi += 8000;
|
||||
else if (FLIGHT_MODE(NAV_WP_MODE))
|
||||
tmpi += 2000;
|
||||
|
|
|
@ -168,7 +168,7 @@ void ltm_sframe(sbuf_t *dst)
|
|||
lt_flightmode = LTM_MODE_RTH;
|
||||
else if (FLIGHT_MODE(NAV_POSHOLD_MODE))
|
||||
lt_flightmode = LTM_MODE_GPSHOLD;
|
||||
else if (FLIGHT_MODE(NAV_CRUISE_MODE))
|
||||
else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE))
|
||||
lt_flightmode = LTM_MODE_CRUISE;
|
||||
else if (FLIGHT_MODE(NAV_LAUNCH_MODE))
|
||||
lt_flightmode = LTM_MODE_LAUNCH;
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue