mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-24 00:35:34 +03:00
Implement CRUISE MODE for Fixed Wing (#3311)
* Nav cruise mode (2D/3D) * CRUISE FLM on LTM telemetry
This commit is contained in:
giacomo892
Konstantin Sharlaimov
parent
ee68dcf56d
commit
46ef27db85
12 changed files with 321 additions and 14 deletions
|
|
@ -62,6 +62,7 @@ typedef enum {
|
|||
DEBUG_WIND_ESTIMATOR,
|
||||
DEBUG_SAG_COMP_VOLTAGE,
|
||||
DEBUG_VIBE,
|
||||
DEBUG_CRUISE,
|
||||
DEBUG_COUNT
|
||||
} debugType_e;
|
||||
|
||||
|
|
|
|||
|
|
@ -78,6 +78,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 },
|
||||
{ CHECKBOX_ITEM_COUNT, NULL, 0xFF }
|
||||
};
|
||||
|
||||
|
|
@ -196,6 +197,9 @@ void initActiveBoxIds(void)
|
|||
|
||||
if (feature(FEATURE_GPS)) {
|
||||
activeBoxIds[activeBoxIdCount++] = BOXGCSNAV;
|
||||
if (STATE(FIXED_WING)) {
|
||||
activeBoxIds[activeBoxIdCount++] = BOXNAVCRUISE;
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
|
@ -294,6 +298,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_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);
|
||||
|
|
|
|||
|
|
@ -161,6 +161,7 @@ void updateUsedModeActivationConditionFlags(void)
|
|||
#ifdef USE_NAV
|
||||
isUsingNAVModes = isModeActivationConditionPresent(BOXNAVPOSHOLD) ||
|
||||
isModeActivationConditionPresent(BOXNAVRTH) ||
|
||||
isModeActivationConditionPresent(BOXNAVCRUISE) ||
|
||||
isModeActivationConditionPresent(BOXNAVWP);
|
||||
#endif
|
||||
}
|
||||
|
|
|
|||
|
|
@ -59,6 +59,7 @@ typedef enum {
|
|||
BOXOSDALT1 = 32,
|
||||
BOXOSDALT2 = 33,
|
||||
BOXOSDALT3 = 34,
|
||||
BOXNAVCRUISE = 35,
|
||||
CHECKBOX_ITEM_COUNT
|
||||
} boxId_e;
|
||||
|
||||
|
|
|
|||
|
|
@ -76,7 +76,7 @@ typedef enum {
|
|||
FAILSAFE_MODE = (1 << 9),
|
||||
AUTO_TUNE = (1 << 10), // old G-Tune
|
||||
NAV_WP_MODE = (1 << 11),
|
||||
UNUSED_MODE2 = (1 << 12),
|
||||
NAV_CRUISE_MODE = (1 << 12),
|
||||
FLAPERON = (1 << 13),
|
||||
TURN_ASSISTANT = (1 << 14),
|
||||
} flightModeFlags_e;
|
||||
|
|
|
|||
|
|
@ -67,7 +67,7 @@ tables:
|
|||
- name: i2c_speed
|
||||
values: ["400KHZ", "800KHZ", "100KHZ", "200KHZ"]
|
||||
- name: debug_modes
|
||||
values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "AGL", "FLOW_RAW", "FLOW", "SBUS", "FPORT", "ALWAYS", "STAGE2", "WIND_ESTIMATOR", "SAG_COMP_VOLTAGE", "VIBE"]
|
||||
values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "AGL", "FLOW_RAW", "FLOW", "SBUS", "FPORT", "ALWAYS", "STAGE2", "WIND_ESTIMATOR", "SAG_COMP_VOLTAGE", "VIBE", "CRUISE"]
|
||||
- name: async_mode
|
||||
values: ["NONE", "GYRO", "ALL"]
|
||||
- name: aux_operator
|
||||
|
|
@ -1365,6 +1365,10 @@ groups:
|
|||
field: fw.launch_climb_angle
|
||||
min: 5
|
||||
max: 45
|
||||
- name: nav_fw_cruise_yaw_rate
|
||||
field: fw.cruise_yaw_rate
|
||||
min: 0
|
||||
max: 60
|
||||
|
||||
- name: PG_TELEMETRY_CONFIG
|
||||
type: telemetryConfig_t
|
||||
|
|
|
|||
|
|
@ -1254,17 +1254,22 @@ static bool osdDrawSingleElement(uint8_t item)
|
|||
} else {
|
||||
p = " PH ";
|
||||
}
|
||||
} 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_ALTHOLD_MODE) && navigationRequiresAngleMode()) {
|
||||
// If navigationRequiresAngleMode() returns false when ALTHOLD is active,
|
||||
// it means it can be combined with ANGLE, HORIZON, ACRO, etc...
|
||||
// and its display is handled by OSD_MESSAGES rather than OSD_FLYMODE.
|
||||
p = " AH ";
|
||||
} else if (FLIGHT_MODE(NAV_WP_MODE))
|
||||
} else if (FLIGHT_MODE(NAV_WP_MODE)) {
|
||||
p = " WP ";
|
||||
else if (FLIGHT_MODE(ANGLE_MODE))
|
||||
} else if (FLIGHT_MODE(ANGLE_MODE)) {
|
||||
p = "ANGL";
|
||||
else if (FLIGHT_MODE(HORIZON_MODE))
|
||||
} else if (FLIGHT_MODE(HORIZON_MODE)) {
|
||||
p = "HOR ";
|
||||
}
|
||||
|
||||
displayWrite(osdDisplayPort, elemPosX, elemPosY, p);
|
||||
return true;
|
||||
|
|
|
|||
|
|
@ -76,7 +76,7 @@ PG_DECLARE_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList);
|
|||
PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 0);
|
||||
#endif
|
||||
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 1);
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 2);
|
||||
|
||||
PG_RESET_TEMPLATE(navConfig_t, navConfig,
|
||||
.general = {
|
||||
|
|
@ -144,7 +144,8 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
|
|||
.launch_timeout = 5000, // ms, timeout for launch procedure
|
||||
.launch_max_altitude = 0, // cm, altitude where to consider launch ended
|
||||
.launch_climb_angle = 18, // 18 degrees
|
||||
.launch_max_angle = 45 // 45 deg
|
||||
.launch_max_angle = 45, // 45 deg
|
||||
.cruise_yaw_rate = 20, // 20dps
|
||||
}
|
||||
);
|
||||
|
||||
|
|
@ -177,6 +178,7 @@ static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint);
|
|||
static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos);
|
||||
void calculateInitialHoldPosition(fpVector3_t * pos);
|
||||
void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t yaw, int32_t distance);
|
||||
void calculateNewCruiseTarget(fpVector3_t * origin, int32_t yaw, int32_t distance);
|
||||
|
||||
void initializeRTHSanityChecker(const fpVector3_t * pos);
|
||||
bool validateRTHSanityChecker(void);
|
||||
|
|
@ -187,6 +189,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_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);
|
||||
|
|
@ -226,6 +234,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,
|
||||
}
|
||||
},
|
||||
|
||||
|
|
@ -260,6 +270,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,
|
||||
}
|
||||
},
|
||||
|
||||
|
|
@ -294,6 +306,112 @@ 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,
|
||||
}
|
||||
},
|
||||
/** CRUISE_2D mode ************************************************/
|
||||
[NAV_STATE_CRUISE_2D_INITIALIZE] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_CRUISE_2D_INITIALIZE,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_CRUISE_2D_INITIALIZE,
|
||||
.timeoutMs = 0,
|
||||
.stateFlags = NAV_REQUIRE_ANGLE,
|
||||
.mapToFlightModes = NAV_CRUISE_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_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,
|
||||
.timeoutMs = 10,
|
||||
.stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_YAW,
|
||||
.mapToFlightModes = NAV_CRUISE_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_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_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,
|
||||
.timeoutMs = 10,
|
||||
.stateFlags = NAV_REQUIRE_ANGLE | NAV_RC_POS,
|
||||
.mapToFlightModes = NAV_CRUISE_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_ERROR] = NAV_STATE_IDLE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
|
||||
}
|
||||
},
|
||||
|
||||
/** CRUISE_3D mode ************************************************/
|
||||
[NAV_STATE_CRUISE_3D_INITIALIZE] = {
|
||||
.persistentId = NAV_PERSISTENT_ID_CRUISE_3D_INITIALIZE,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_CRUISE_3D_INITIALIZE,
|
||||
.timeoutMs = 0,
|
||||
.stateFlags = NAV_REQUIRE_ANGLE,
|
||||
.mapToFlightModes = NAV_ALTHOLD_MODE | NAV_CRUISE_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_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,
|
||||
.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,
|
||||
.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_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_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,
|
||||
.timeoutMs = 10,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_ALT,
|
||||
.mapToFlightModes = NAV_ALTHOLD_MODE | NAV_CRUISE_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_ERROR] = NAV_STATE_IDLE,
|
||||
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
|
||||
}
|
||||
},
|
||||
|
||||
|
|
@ -329,6 +447,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
[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_3D] = NAV_STATE_CRUISE_3D_INITIALIZE,
|
||||
}
|
||||
},
|
||||
|
||||
|
|
@ -347,6 +467,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,
|
||||
}
|
||||
},
|
||||
|
||||
|
|
@ -366,6 +488,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,
|
||||
}
|
||||
},
|
||||
|
||||
|
|
@ -383,6 +507,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,
|
||||
}
|
||||
},
|
||||
|
||||
|
|
@ -484,6 +610,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,
|
||||
}
|
||||
},
|
||||
|
||||
|
|
@ -505,6 +633,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,
|
||||
}
|
||||
},
|
||||
|
||||
|
|
@ -524,6 +654,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,
|
||||
}
|
||||
},
|
||||
|
||||
|
|
@ -555,6 +687,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,
|
||||
}
|
||||
},
|
||||
|
||||
|
|
@ -763,6 +897,106 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS(
|
|||
|
||||
return NAV_FSM_EVENT_NONE;
|
||||
}
|
||||
/////////////////
|
||||
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_INITIALIZE(navigationFSMState_t previousState)
|
||||
{
|
||||
const navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState);
|
||||
|
||||
if (!STATE(FIXED_WING)) { return NAV_FSM_EVENT_ERROR; } // Only on FW for now
|
||||
|
||||
DEBUG_SET(DEBUG_CRUISE, 0, 1);
|
||||
if (checkForPositionSensorTimeout()) { return NAV_FSM_EVENT_SWITCH_TO_IDLE; } // Switch to IDLE if we do not have an healty position. Try the next iteration.
|
||||
|
||||
if (!(prevFlags & NAV_CTL_POS)) {
|
||||
resetPositionController();
|
||||
}
|
||||
|
||||
posControl.cruise.yaw = posControl.actualState.yaw; // Store the yaw to follow
|
||||
|
||||
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)
|
||||
{
|
||||
const timeMs_t currentTimeMs = millis();
|
||||
static timeMs_t lastYawChangeTime = 0;
|
||||
|
||||
if (checkForPositionSensorTimeout()) { return NAV_FSM_EVENT_SWITCH_TO_IDLE; } // Switch to IDLE if we do not have an healty position. Do the CRUISE init the next iteration.
|
||||
|
||||
DEBUG_SET(DEBUG_CRUISE, 0, 2);
|
||||
DEBUG_SET(DEBUG_CRUISE, 2, 0);
|
||||
|
||||
if (posControl.flags.isAdjustingPosition) {
|
||||
return NAV_FSM_EVENT_SWITCH_TO_CRUISE_ADJ;
|
||||
}
|
||||
|
||||
// User is yawing. We record the desidered yaw and we change the desidered target in the meanwhile
|
||||
if (posControl.flags.isAdjustingHeading) {
|
||||
timeMs_t timeDifference = currentTimeMs - lastYawChangeTime;
|
||||
if (timeDifference > 100) timeDifference = 0; // if adjustment was called long time ago, reset the time difference.
|
||||
float rateTarget = scaleRangef((float)rcCommand[YAW], -500.0f, 500.0f, -DEGREES_TO_CENTIDEGREES(navConfig()->fw.cruise_yaw_rate), DEGREES_TO_CENTIDEGREES(navConfig()->fw.cruise_yaw_rate));
|
||||
float centidegsPerIteration = rateTarget * timeDifference / 1000.0f;
|
||||
posControl.cruise.yaw = wrap_36000(posControl.cruise.yaw - centidegsPerIteration);
|
||||
DEBUG_SET(DEBUG_CRUISE, 1, CENTIDEGREES_TO_DEGREES(posControl.cruise.yaw));
|
||||
lastYawChangeTime = currentTimeMs;
|
||||
}
|
||||
|
||||
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)
|
||||
|| posControl.flags.isAdjustingHeading) {
|
||||
calculateFarAwayTarget(&posControl.cruise.targetPos, posControl.cruise.yaw, distance);
|
||||
DEBUG_SET(DEBUG_CRUISE, 2, 1);
|
||||
} else if (calculateDistanceToDestination(&posControl.cruise.targetPos) <= (navConfig()->fw.loiter_radius * 1.10f)) { //10% margin
|
||||
calculateNewCruiseTarget(&posControl.cruise.targetPos, posControl.cruise.yaw, distance);
|
||||
DEBUG_SET(DEBUG_CRUISE, 2, 2);
|
||||
}
|
||||
|
||||
setDesiredPosition(&posControl.cruise.targetPos, posControl.cruise.yaw, NAV_POS_UPDATE_XY);
|
||||
|
||||
return NAV_FSM_EVENT_NONE;
|
||||
}
|
||||
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_ADJUSTING(navigationFSMState_t previousState)
|
||||
{
|
||||
UNUSED(previousState);
|
||||
DEBUG_SET(DEBUG_CRUISE, 0, 3);
|
||||
|
||||
// User is rolling, changing manually direction. Wait until it is done and then restore CRUISE
|
||||
if (posControl.flags.isAdjustingPosition) {
|
||||
return NAV_FSM_EVENT_NONE; // reprocess the state
|
||||
}
|
||||
|
||||
posControl.cruise.yaw = posControl.actualState.yaw; //store current heading
|
||||
resetPositionController();
|
||||
|
||||
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)
|
||||
{
|
||||
if (!STATE(FIXED_WING)) { return NAV_FSM_EVENT_ERROR; } // only on FW for now
|
||||
|
||||
navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(previousState);
|
||||
|
||||
return navOnEnteringState_NAV_STATE_CRUISE_2D_INITIALIZE(previousState);
|
||||
}
|
||||
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_3D_IN_PROGRESS(navigationFSMState_t previousState)
|
||||
{
|
||||
navOnEnteringState_NAV_STATE_ALTHOLD_IN_PROGRESS(previousState);
|
||||
|
||||
return navOnEnteringState_NAV_STATE_CRUISE_2D_IN_PROGRESS(previousState);
|
||||
}
|
||||
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_3D_ADJUSTING(navigationFSMState_t previousState)
|
||||
{
|
||||
navOnEnteringState_NAV_STATE_ALTHOLD_IN_PROGRESS(previousState);
|
||||
|
||||
return navOnEnteringState_NAV_STATE_CRUISE_2D_ADJUSTING(previousState);
|
||||
}
|
||||
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigationFSMState_t previousState)
|
||||
{
|
||||
|
|
@ -1913,6 +2147,13 @@ void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t yaw, int32_t dista
|
|||
farAwayPos->z = navGetCurrentActualPositionAndVelocity()->pos.z;
|
||||
}
|
||||
|
||||
void calculateNewCruiseTarget(fpVector3_t * origin, int32_t yaw, int32_t distance)
|
||||
{
|
||||
origin->x = origin->x + distance * cos_approx(CENTIDEGREES_TO_RADIANS(yaw));
|
||||
origin->y = origin->y + distance * sin_approx(CENTIDEGREES_TO_RADIANS(yaw));
|
||||
origin->z = origin->z;
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
* NAV land detector
|
||||
*-----------------------------------------------------------*/
|
||||
|
|
@ -2390,7 +2631,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) & (~enabledNavFlightModes);
|
||||
const flightModeFlags_e disabledFlightModes = (NAV_ALTHOLD_MODE | NAV_RTH_MODE | NAV_POSHOLD_MODE | NAV_WP_MODE | NAV_LAUNCH_MODE | NAV_CRUISE_MODE) & (~enabledNavFlightModes);
|
||||
DISABLE_FLIGHT_MODE(disabledFlightModes);
|
||||
ENABLE_FLIGHT_MODE(enabledNavFlightModes);
|
||||
}
|
||||
|
|
@ -2482,6 +2723,18 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
|
|||
return NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D;
|
||||
}
|
||||
|
||||
// PH has priority over CRUISE
|
||||
// CRUISE has priority on AH
|
||||
if (IS_RC_MODE_ACTIVE(BOXNAVCRUISE)) {
|
||||
|
||||
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 (FLIGHT_MODE(NAV_CRUISE_MODE) || (canActivatePosHold))
|
||||
return NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D;
|
||||
|
||||
}
|
||||
|
||||
if (IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) {
|
||||
if ((FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivateAltHold))
|
||||
return NAV_FSM_EVENT_SWITCH_TO_ALTHOLD;
|
||||
|
|
@ -2560,8 +2813,8 @@ bool navigationTerrainFollowingEnabled(void)
|
|||
|
||||
bool navigationBlockArming(void)
|
||||
{
|
||||
const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) || (STATE(FIXED_WING) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD));
|
||||
const bool navLaunchComboModesEnabled = isNavLaunchEnabled() && (IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVALTHOLD));
|
||||
const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) || (STATE(FIXED_WING) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) || (STATE(FIXED_WING) && 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));
|
||||
bool shouldBlockArming = false;
|
||||
|
||||
if (!navConfig()->general.flags.extra_arming_safety)
|
||||
|
|
|
|||
|
|
@ -169,6 +169,7 @@ typedef struct navConfig_s {
|
|||
uint16_t launch_max_altitude; // cm, altitude where to consider launch ended
|
||||
uint8_t launch_climb_angle; // Target climb angle for launch (deg)
|
||||
uint8_t launch_max_angle; // Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg]
|
||||
uint8_t cruise_yaw_rate; // Max yaw rate (dps) when CRUISE MODE is enabled
|
||||
} fw;
|
||||
} navConfig_t;
|
||||
|
||||
|
|
|
|||
|
|
@ -182,6 +182,10 @@ void applyFixedWingAltitudeAndThrottleController(timeUs_t currentTimeUs)
|
|||
*-----------------------------------------------------------*/
|
||||
bool adjustFixedWingHeadingFromRCInput(void)
|
||||
{
|
||||
if (ABS(rcCommand[YAW]) > rcControlsConfig()->pos_hold_deadband) {
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
|
|
@ -218,7 +222,8 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
|
|||
#define TAN_15DEG 0.26795f
|
||||
bool needToCalculateCircularLoiter = isApproachingLastWaypoint()
|
||||
&& (distanceToActualTarget <= (navConfig()->fw.loiter_radius / TAN_15DEG))
|
||||
&& (distanceToActualTarget > 50.0f);
|
||||
&& (distanceToActualTarget > 50.0f)
|
||||
&& !FLIGHT_MODE(NAV_CRUISE_MODE);
|
||||
|
||||
// Calculate virtual position for straight movement
|
||||
if (needToCalculateCircularLoiter) {
|
||||
|
|
@ -545,6 +550,9 @@ void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags,
|
|||
applyFixedWingPositionController(currentTimeUs);
|
||||
#endif
|
||||
|
||||
if (FLIGHT_MODE(NAV_CRUISE_MODE) && posControl.flags.isAdjustingPosition)
|
||||
rcCommand[ROLL] = applyDeadband(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband);
|
||||
|
||||
//if (navStateFlags & NAV_CTL_YAW)
|
||||
if ((navStateFlags & NAV_CTL_ALT) || (navStateFlags & NAV_CTL_POS))
|
||||
applyFixedWingPitchRollThrottleController(navStateFlags, currentTimeUs);
|
||||
|
|
|
|||
|
|
@ -178,6 +178,9 @@ typedef enum {
|
|||
NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND = NAV_FSM_EVENT_STATE_SPECIFIC_1,
|
||||
NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED = NAV_FSM_EVENT_STATE_SPECIFIC_2,
|
||||
|
||||
NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D,
|
||||
NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D,
|
||||
NAV_FSM_EVENT_SWITCH_TO_CRUISE_ADJ,
|
||||
NAV_FSM_EVENT_COUNT,
|
||||
} navigationFSMEvent_t;
|
||||
|
||||
|
|
@ -222,6 +225,14 @@ typedef enum {
|
|||
NAV_PERSISTENT_ID_LAUNCH_WAIT = 26,
|
||||
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_CRUISE_3D_INITIALIZE = 32,
|
||||
NAV_PERSISTENT_ID_CRUISE_3D_IN_PROGRESS = 33,
|
||||
NAV_PERSISTENT_ID_CRUISE_3D_ADJUSTING = 34,
|
||||
} navigationPersistentId_e;
|
||||
|
||||
typedef enum {
|
||||
|
|
@ -260,6 +271,13 @@ 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_COUNT,
|
||||
} navigationFSMState_t;
|
||||
|
||||
|
|
@ -307,6 +325,11 @@ typedef struct {
|
|||
float minimalDistanceToHome;
|
||||
} rthSanityChecker_t;
|
||||
|
||||
typedef struct {
|
||||
fpVector3_t targetPos;
|
||||
int32_t yaw;
|
||||
} navCruise_t;
|
||||
|
||||
typedef struct {
|
||||
/* Flags and navigation system state */
|
||||
navigationFSMState_t navState;
|
||||
|
|
@ -338,6 +361,9 @@ typedef struct {
|
|||
uint32_t homeDistance; // cm
|
||||
int32_t homeDirection; // deg*100
|
||||
|
||||
/* Cruise */
|
||||
navCruise_t cruise;
|
||||
|
||||
/* Waypoint list */
|
||||
navWaypoint_t waypointList[NAV_MAX_WAYPOINTS];
|
||||
bool waypointListValid;
|
||||
|
|
|
|||
|
|
@ -168,6 +168,8 @@ void ltm_sframe(sbuf_t *dst)
|
|||
lt_flightmode = 13;
|
||||
else if (FLIGHT_MODE(NAV_POSHOLD_MODE))
|
||||
lt_flightmode = 9;
|
||||
else if (FLIGHT_MODE(NAV_CRUISE_MODE))
|
||||
lt_flightmode = 18;
|
||||
else if (FLIGHT_MODE(NAV_ALTHOLD_MODE))
|
||||
lt_flightmode = 8;
|
||||
else if (FLIGHT_MODE(HEADFREE_MODE) || FLIGHT_MODE(HEADING_MODE))
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue