Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-26 01:35:35 +03:00
Code Issues Wiki Activity

Merge remote-tracking branch 'upstream/master' into abo_fw_coursehold_change

Browse source
This commit is contained in:
breadoven 2023-01-13 11:38:57 +00:00
commit eaf42803c3
75 changed files with 2241 additions and 568 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -99,7 +99,7 @@ STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_rang
PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 2);
#endif
PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 2);
PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 3);
PG_RESET_TEMPLATE(navConfig_t, navConfig,
.general = {
@ -127,7 +127,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
// General navigation parameters
.pos_failure_timeout = SETTING_NAV_POSITION_TIMEOUT_DEFAULT, // 5 sec
.waypoint_radius = SETTING_NAV_WP_RADIUS_DEFAULT, // 2m diameter
.waypoint_safe_distance = SETTING_NAV_WP_SAFE_DISTANCE_DEFAULT, // centimeters - first waypoint should be closer than this
.waypoint_safe_distance = SETTING_NAV_WP_MAX_SAFE_DISTANCE_DEFAULT, // Metres - first waypoint should be closer than this
#ifdef USE_MULTI_MISSION
.waypoint_multi_mission_index = SETTING_NAV_WP_MULTI_MISSION_INDEX_DEFAULT, // mission index selected from multi mission WP entry
#endif
@ -185,7 +185,9 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.control_smoothness = SETTING_NAV_FW_CONTROL_SMOOTHNESS_DEFAULT,
.pitch_to_throttle_smooth = SETTING_NAV_FW_PITCH2THR_SMOOTHING_DEFAULT,
.pitch_to_throttle_thresh = SETTING_NAV_FW_PITCH2THR_THRESHOLD_DEFAULT,
.minThrottleDownPitchAngle = SETTING_FW_MIN_THROTTLE_DOWN_PITCH_DEFAULT,
.loiter_radius = SETTING_NAV_FW_LOITER_RADIUS_DEFAULT, // 75m
.loiter_direction = SETTING_FW_LOITER_DIRECTION_DEFAULT,
//Fixed wing landing
.land_dive_angle = SETTING_NAV_FW_LAND_DIVE_ANGLE_DEFAULT, // 2 degrees dive by default
@ -254,8 +256,8 @@ static void clearJumpCounters(void);
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);
static bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * waypointYaw);
void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t bearing, int32_t distance);
static bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * waypointBearing);
bool isWaypointAltitudeReached(void);
static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint, geoAltitudeConversionMode_e altConv);
static navigationFSMEvent_t nextForNonGeoStates(void);
@ -1067,9 +1069,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(
resetPositionController();
posControl.cruise.yaw = posControl.actualState.yaw; // Store the yaw to follow
posControl.cruise.previousYaw = posControl.cruise.yaw;
posControl.cruise.lastYawAdjustmentTime = 0;
posControl.cruise.course = posControl.actualState.cog; // Store the course to follow
posControl.cruise.previousCourse = posControl.cruise.course;
posControl.cruise.lastCourseAdjustmentTime = 0;
return NAV_FSM_EVENT_SUCCESS; // Go to NAV_STATE_COURSE_HOLD_IN_PROGRESS state
}
@ -1093,15 +1095,15 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS
// 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 - posControl.cruise.lastYawAdjustmentTime;
timeMs_t timeDifference = currentTimeMs - posControl.cruise.lastCourseAdjustmentTime;
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 * MS2S(timeDifference);
posControl.cruise.yaw = wrap_36000(posControl.cruise.yaw - centidegsPerIteration);
DEBUG_SET(DEBUG_CRUISE, 1, CENTIDEGREES_TO_DEGREES(posControl.cruise.yaw));
posControl.cruise.lastYawAdjustmentTime = currentTimeMs;
} else if (currentTimeMs - posControl.cruise.lastYawAdjustmentTime > 4000) {
posControl.cruise.previousYaw = posControl.cruise.yaw;
posControl.cruise.course = wrap_36000(posControl.cruise.course - centidegsPerIteration);
DEBUG_SET(DEBUG_CRUISE, 1, CENTIDEGREES_TO_DEGREES(posControl.cruise.course));
posControl.cruise.lastCourseAdjustmentTime = currentTimeMs;
} else if (currentTimeMs - posControl.cruise.lastCourseAdjustmentTime > 4000)
posControl.cruise.previousCourse = posControl.cruise.course;
}
setDesiredPosition(NULL, posControl.cruise.yaw, NAV_POS_UPDATE_HEADING);
@ -1116,8 +1118,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_ADJUSTING(n
// User is rolling, changing manually direction. Wait until it is done and then restore CRUISE
if (posControl.flags.isAdjustingPosition) {
posControl.cruise.yaw = posControl.actualState.yaw; //store current heading
posControl.cruise.lastYawAdjustmentTime = millis();
posControl.cruise.course = posControl.actualState.cog; //store current course
posControl.cruise.lastCourseAdjustmentTime = millis();
return NAV_FSM_EVENT_NONE; // reprocess the state
}
@ -1206,7 +1208,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
// Spiral climb centered at xy of RTH activation
calculateInitialHoldPosition(&targetHoldPos);
} else {
calculateFarAwayTarget(&targetHoldPos, posControl.actualState.yaw, 100000.0f); // 1km away Linear climb
calculateFarAwayTarget(&targetHoldPos, posControl.actualState.cog, 100000.0f); // 1km away Linear climb
}
} else {
// Multicopter, hover and climb
@ -1325,7 +1327,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_TRACKBACK(navigatio
return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE; // procede to home after final trackback point
}
if (isWaypointReached(&posControl.activeWaypoint.pos, &posControl.activeWaypoint.yaw)) {
if (isWaypointReached(&posControl.activeWaypoint.pos, &posControl.activeWaypoint.bearing)) {
posControl.activeRthTBPointIndex--;
if (posControl.rthTBWrapAroundCounter > -1 && posControl.activeRthTBPointIndex < 0) {
@ -1361,7 +1363,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio
if (isWaypointReached(tmpHomePos, 0)) {
// Successfully reached position target - update XYZ-position
setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING
} else {
setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_XY);
@ -1392,13 +1394,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND
// If position ok OR within valid timeout - continue
// Wait until target heading is reached for MR (with 15 deg margin for error), or continue for Fixed Wing
if ((ABS(wrap_18000(posControl.rthState.homePosition.yaw - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) || STATE(FIXED_WING_LEGACY)) {
if ((ABS(wrap_18000(posControl.rthState.homePosition.heading - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) || STATE(FIXED_WING_LEGACY)) {
resetLandingDetector(); // force reset landing detector just in case
updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET);
return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME; // success = land
} else {
fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_FINAL);
setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
return NAV_FSM_EVENT_NONE;
}
}
@ -1613,7 +1615,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
case NAV_WP_ACTION_HOLD_TIME:
case NAV_WP_ACTION_WAYPOINT:
case NAV_WP_ACTION_LAND:
if (isWaypointReached(&posControl.activeWaypoint.pos, &posControl.activeWaypoint.yaw)) {
if (isWaypointReached(&posControl.activeWaypoint.pos, &posControl.activeWaypoint.bearing)) {
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_REACHED
}
else {
@ -1918,7 +1920,9 @@ static void navProcessFSMEvents(navigationFSMEvent_t injectedEvent)
if (posControl.flags.isAdjustingPosition) NAV_Status.flags |= MW_NAV_FLAG_ADJUSTING_POSITION;
if (posControl.flags.isAdjustingAltitude) NAV_Status.flags |= MW_NAV_FLAG_ADJUSTING_ALTITUDE;
NAV_Status.activeWpNumber = posControl.activeWaypointIndex - posControl.startWpIndex + 1;
NAV_Status.activeWpIndex = posControl.activeWaypointIndex - posControl.startWpIndex;
NAV_Status.activeWpNumber = NAV_Status.activeWpIndex + 1;
NAV_Status.activeWpAction = 0;
if ((posControl.activeWaypointIndex >= 0) && (posControl.activeWaypointIndex < NAV_MAX_WAYPOINTS)) {
NAV_Status.activeWpAction = posControl.waypointList[posControl.activeWaypointIndex].action;
@ -2103,7 +2107,7 @@ void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, flo
/*-----------------------------------------------------------
* Processes an update to estimated heading
*-----------------------------------------------------------*/
void updateActualHeading(bool headingValid, int32_t newHeading)
void updateActualHeading(bool headingValid, int32_t newHeading, int32_t newGroundCourse)
{
/* Update heading. Check if we're acquiring a valid heading for the
* first time and update home heading accordingly.
@ -2124,17 +2128,14 @@ void updateActualHeading(bool headingValid, int32_t newHeading)
// the offset from the fake to the actual yaw and apply the same rotation
// to the home point.
int32_t fakeToRealYawOffset = newHeading - posControl.actualState.yaw;
posControl.rthState.homePosition.heading += fakeToRealYawOffset;
posControl.rthState.homePosition.heading = wrap_36000(posControl.rthState.homePosition.heading);
posControl.rthState.homePosition.yaw += fakeToRealYawOffset;
if (posControl.rthState.homePosition.yaw < 0) {
posControl.rthState.homePosition.yaw += DEGREES_TO_CENTIDEGREES(360);
}
if (posControl.rthState.homePosition.yaw >= DEGREES_TO_CENTIDEGREES(360)) {
posControl.rthState.homePosition.yaw -= DEGREES_TO_CENTIDEGREES(360);
}
posControl.rthState.homeFlags |= NAV_HOME_VALID_HEADING;
}
posControl.actualState.yaw = newHeading;
posControl.actualState.cog = newGroundCourse;
posControl.flags.estHeadingStatus = newEstHeading;
/* Precompute sin/cos of yaw angle */
@ -2189,7 +2190,7 @@ int32_t calculateBearingBetweenLocalPositions(const fpVector3_t * startPos, cons
return calculateBearingFromDelta(deltaX, deltaY);
}
bool navCalculatePathToDestination(navDestinationPath_t *result, const fpVector3_t * destinationPos)
bool navCalculatePathToDestination(navDestinationPath_t *result, const fpVector3_t * destinationPos) // NOT USED ANYWHERE
{
if (posControl.flags.estPosStatus == EST_NONE ||
posControl.flags.estHeadingStatus == EST_NONE) {
@ -2209,7 +2210,7 @@ bool navCalculatePathToDestination(navDestinationPath_t *result, const fpVector3
static bool getLocalPosNextWaypoint(fpVector3_t * nextWpPos)
{
// Only for WP Mode not Trackback. Ignore non geo waypoints except RTH and JUMP.
if (FLIGHT_MODE(NAV_WP_MODE) && !isLastMissionWaypoint()) {
if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP && !isLastMissionWaypoint()) {
navWaypointActions_e nextWpAction = posControl.waypointList[posControl.activeWaypointIndex + 1].action;
if (!(nextWpAction == NAV_WP_ACTION_SET_POI || nextWpAction == NAV_WP_ACTION_SET_HEAD)) {
@ -2236,9 +2237,9 @@ static bool getLocalPosNextWaypoint(fpVector3_t * nextWpPos)
/*-----------------------------------------------------------
* Check if waypoint is/was reached.
* waypointYaw stores initial bearing to waypoint
* waypointBearing stores initial bearing to waypoint
*-----------------------------------------------------------*/
static bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * waypointYaw)
static bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * waypointBearing)
{
posControl.wpDistance = calculateDistanceToDestination(waypointPos);
@ -2257,7 +2258,7 @@ static bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * w
// Check if waypoint was missed based on bearing to WP exceeding 100 degrees relative to waypoint Yaw
// Same method for turn smoothing option but relative bearing set at 60 degrees
uint16_t relativeBearing = posControl.flags.wpTurnSmoothingActive ? 6000 : 10000;
if (ABS(wrap_18000(calculateBearingToDestination(waypointPos) - *waypointYaw)) > relativeBearing) {
if (ABS(wrap_18000(calculateBearingToDestination(waypointPos) - *waypointBearing)) > relativeBearing) {
return true;
}
}
@ -2382,7 +2383,7 @@ bool validateRTHSanityChecker(void)
/*-----------------------------------------------------------
* Reset home position to current position
*-----------------------------------------------------------*/
void setHomePosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t useMask, navigationHomeFlags_t homeFlags)
void setHomePosition(const fpVector3_t * pos, int32_t heading, navSetWaypointFlags_t useMask, navigationHomeFlags_t homeFlags)
{
// XY-position
if ((useMask & NAV_POS_UPDATE_XY) != 0) {
@ -2408,7 +2409,7 @@ void setHomePosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t
// Heading
if ((useMask & NAV_POS_UPDATE_HEADING) != 0) {
// Heading
posControl.rthState.homePosition.yaw = yaw;
posControl.rthState.homePosition.heading = heading;
if (homeFlags & NAV_HOME_VALID_HEADING) {
posControl.rthState.homeFlags |= NAV_HOME_VALID_HEADING;
} else {
@ -2641,7 +2642,7 @@ static bool rthAltControlStickOverrideCheck(unsigned axis)
suspendTracking = false;
}
if (navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_OFF || FLIGHT_MODE(NAV_RTH_MODE) || !ARMING_FLAG(ARMED || suspendTracking)) {
if (navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_OFF || FLIGHT_MODE(NAV_RTH_MODE) || !ARMING_FLAG(ARMED) || suspendTracking) {
return;
}
@ -2659,7 +2660,7 @@ static bool rthAltControlStickOverrideCheck(unsigned axis)
if (posControl.activeRthTBPointIndex < 0) {
saveTrackpoint = posControl.homeDistance > METERS_TO_CENTIMETERS(50);
previousTBCourse = CENTIDEGREES_TO_DEGREES(posControl.actualState.yaw);
previousTBCourse = CENTIDEGREES_TO_DEGREES(posControl.actualState.cog);
previousTBTripDist = posControl.totalTripDistance;
} else {
// Minimum distance increment between course change track points when GPS course valid - set to 10m
@ -2790,10 +2791,10 @@ void setDesiredPosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlag
}
}
void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t yaw, int32_t distance)
void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t bearing, int32_t distance)
{
farAwayPos->x = navGetCurrentActualPositionAndVelocity()->pos.x + distance * cos_approx(CENTIDEGREES_TO_RADIANS(yaw));
farAwayPos->y = navGetCurrentActualPositionAndVelocity()->pos.y + distance * sin_approx(CENTIDEGREES_TO_RADIANS(yaw));
farAwayPos->x = navGetCurrentActualPositionAndVelocity()->pos.x + distance * cos_approx(CENTIDEGREES_TO_RADIANS(bearing));
farAwayPos->y = navGetCurrentActualPositionAndVelocity()->pos.y + distance * sin_approx(CENTIDEGREES_TO_RADIANS(bearing));
farAwayPos->z = navGetCurrentActualPositionAndVelocity()->pos.z;
}
@ -2886,10 +2887,19 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti
if (STATE(FIXED_WING_LEGACY)) {
// Fixed wing climb rate controller is open-loop. We simply move the known altitude target
float timeDelta = US2S(currentTimeUs - lastUpdateTimeUs);
static bool targetHoldActive = false;
if (timeDelta <= HZ2S(MIN_POSITION_UPDATE_RATE_HZ)) {
posControl.desiredState.pos.z += desiredClimbRate * timeDelta;
posControl.desiredState.pos.z = constrainf(posControl.desiredState.pos.z, altitudeToUse - 500, altitudeToUse + 500); // FIXME: calculate sanity limits in a smarter way
if (timeDelta <= HZ2S(MIN_POSITION_UPDATE_RATE_HZ) && desiredClimbRate) {
// Update target altitude only if actual altitude moving in same direction and lagging by < 5 m, otherwise hold target
if (navGetCurrentActualPositionAndVelocity()->vel.z * desiredClimbRate >= 0 && fabsf(posControl.desiredState.pos.z - altitudeToUse) < 500) {
posControl.desiredState.pos.z += desiredClimbRate * timeDelta;
targetHoldActive = false;
} else if (!targetHoldActive) { // Reset and hold target to actual + climb rate boost until actual catches up
posControl.desiredState.pos.z = altitudeToUse + desiredClimbRate;
targetHoldActive = true;
}
} else {
targetHoldActive = false;
}
}
else {
@ -3356,18 +3366,18 @@ static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint
static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos)
{
// Calculate bearing towards waypoint and store it in waypoint yaw parameter (this will further be used to detect missed waypoints)
// Calculate bearing towards waypoint and store it in waypoint bearing parameter (this will further be used to detect missed waypoints)
if (isWaypointNavTrackingActive() && !(posControl.activeWaypoint.pos.x == pos->x && posControl.activeWaypoint.pos.y == pos->y)) {
posControl.activeWaypoint.yaw = calculateBearingBetweenLocalPositions(&posControl.activeWaypoint.pos, pos);
posControl.activeWaypoint.bearing = calculateBearingBetweenLocalPositions(&posControl.activeWaypoint.pos, pos);
} else {
posControl.activeWaypoint.yaw = calculateBearingToDestination(pos);
posControl.activeWaypoint.bearing = calculateBearingToDestination(pos);
}
posControl.activeWaypoint.nextTurnAngle = -1; // no turn angle set (-1), will be set by WP mode as required
posControl.activeWaypoint.pos = *pos;
// Set desired position to next waypoint (XYZ-controller)
setDesiredPosition(&posControl.activeWaypoint.pos, posControl.activeWaypoint.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
setDesiredPosition(&posControl.activeWaypoint.pos, posControl.activeWaypoint.bearing, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
}
geoAltitudeConversionMode_e waypointMissionAltConvMode(geoAltitudeDatumFlag_e datumFlag)
@ -3385,7 +3395,7 @@ static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint)
fpVector3_t posNextWp;
if (getLocalPosNextWaypoint(&posNextWp)) {
int32_t bearingToNextWp = calculateBearingBetweenLocalPositions(&posControl.activeWaypoint.pos, &posNextWp);
posControl.activeWaypoint.nextTurnAngle = wrap_18000(bearingToNextWp - posControl.activeWaypoint.yaw);
posControl.activeWaypoint.nextTurnAngle = wrap_18000(bearingToNextWp - posControl.activeWaypoint.bearing);
}
}
}
@ -3597,8 +3607,9 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
const bool canActivatePosHold = canActivatePosHoldMode();
const bool canActivateNavigation = canActivateNavigationModes();
const bool isExecutingRTH = navGetStateFlags(posControl.navState) & NAV_AUTO_RTH;
#ifdef USE_SAFE_HOME
checkSafeHomeState(isExecutingRTH || posControl.flags.forcedRTHActivated);
#endif
// deactivate rth trackback if RTH not active
if (posControl.flags.rthTrackbackActive) {
posControl.flags.rthTrackbackActive = isExecutingRTH;
@ -3823,7 +3834,6 @@ bool navigationPositionEstimateIsHealthy(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(BOXNAVCOURSEHOLD) || 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(BOXNAVCOURSEHOLD) || IS_RC_MODE_ACTIVE(BOXNAVCRUISE));
if (usedBypass) {
*usedBypass = false;
@ -3842,13 +3852,13 @@ navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
}
// Don't allow arming if any of NAV modes is active
if (!ARMING_FLAG(ARMED) && navBoxModesEnabled && !navLaunchComboModesEnabled) {
if (!ARMING_FLAG(ARMED) && navBoxModesEnabled) {
return NAV_ARMING_BLOCKER_NAV_IS_ALREADY_ACTIVE;
}
// Don't allow arming if first waypoint is farther than configured safe distance
if ((posControl.waypointCount > 0) && (navConfig()->general.waypoint_safe_distance != 0)) {
if (distanceToFirstWP() > navConfig()->general.waypoint_safe_distance && !checkStickPosition(YAW_HI)) {
if (distanceToFirstWP() > METERS_TO_CENTIMETERS(navConfig()->general.waypoint_safe_distance) && !checkStickPosition(YAW_HI)) {
return NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR;
}
}
@ -4187,7 +4197,9 @@ void activateForcedRTH(void)
{
abortFixedWingLaunch();
posControl.flags.forcedRTHActivated = true;
#ifdef USE_SAFE_HOME
checkSafeHomeState(true);
#endif
navProcessFSMEvents(selectNavEventFromBoxModeInput());
}
@ -4196,7 +4208,9 @@ void abortForcedRTH(void)
// Disable failsafe RTH and make sure we back out of navigation mode to IDLE
// If any navigation mode was active prior to RTH it will be re-enabled with next RX update
posControl.flags.forcedRTHActivated = false;
#ifdef USE_SAFE_HOME
checkSafeHomeState(false);
#endif
navProcessFSMEvents(NAV_FSM_EVENT_SWITCH_TO_IDLE);
}
@ -4304,13 +4318,12 @@ bool isNavLaunchEnabled(void)
bool abortLaunchAllowed(void)
{
// allow NAV_LAUNCH_MODE to be aborted if throttle is low or throttle stick position is < launch idle throttle setting
throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
return throttleStatus == THROTTLE_LOW || throttleStickMixedValue() < currentBatteryProfile->nav.fw.launch_idle_throttle;
return throttleStickIsLow() || throttleStickMixedValue() < currentBatteryProfile->nav.fw.launch_idle_throttle;
}
int32_t navigationGetHomeHeading(void)
{
return posControl.rthState.homePosition.yaw;
return posControl.rthState.homePosition.heading;
}
// returns m/s
@ -4333,5 +4346,5 @@ bool isAdjustingHeading(void) {
}
int32_t getCruiseHeadingAdjustment(void) {
return wrap_18000(posControl.cruise.yaw - posControl.cruise.previousYaw);
return wrap_18000(posControl.cruise.course - posControl.cruise.previousCourse);
}