Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-24 16:55:29 +03:00
Code Issues Wiki Activity

Implement CRUISE MODE for Fixed Wing (#3311)

Browse source 
* Nav cruise mode (2D/3D)
* CRUISE FLM on LTM telemetry
This commit is contained in:
giacomo892 2018-06-16 18:27:22 +02:00 • committed by Konstantin Sharlaimov
parent ee68dcf56d
commit 46ef27db85
12 changed files with 321 additions and 14 deletions
Download patch file Download diff file Expand all files Collapse all files

269
src/main/navigation/navigation.c
View file

@ -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)
{
@ -1220,10 +1454,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_WAIT(navigationF
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_LAUNCH_IN_PROGRESS
}
//allow to leave NAV_LAUNCH_MODE if it has being enabled as feature by moving sticks with low throttle.
if(feature(FEATURE_FW_LAUNCH)){
//allow to leave NAV_LAUNCH_MODE if it has being enabled as feature by moving sticks with low throttle.
if (feature(FEATURE_FW_LAUNCH)) {
throttleStatus_e throttleStatus = calculateThrottleStatus();
if ((throttleStatus == THROTTLE_LOW) && (areSticksDeflectedMoreThanPosHoldDeadband())){
if ((throttleStatus == THROTTLE_LOW) && (areSticksDeflectedMoreThanPosHoldDeadband())) {
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
}
}
@ -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)