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

7.1 mergeback

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2024-03-02 19:33:27 +01:00
commit d35a895ad3
80 changed files with 2128 additions and 832 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -56,6 +56,7 @@
#include "navigation/navigation.h"
#include "navigation/navigation_private.h"
#include "navigation/rth_trackback.h"
#include "rx/rx.h"
@ -154,9 +155,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.auto_speed = SETTING_NAV_AUTO_SPEED_DEFAULT, // speed in autonomous modes (3 m/s = 10.8 km/h)
.min_ground_speed = SETTING_NAV_MIN_GROUND_SPEED_DEFAULT, // Minimum ground speed (m/s)
.max_auto_speed = SETTING_NAV_MAX_AUTO_SPEED_DEFAULT, // max allowed speed autonomous modes
.max_auto_climb_rate = SETTING_NAV_AUTO_CLIMB_RATE_DEFAULT, // 5 m/s
.max_manual_speed = SETTING_NAV_MANUAL_SPEED_DEFAULT,
.max_manual_climb_rate = SETTING_NAV_MANUAL_CLIMB_RATE_DEFAULT,
.land_slowdown_minalt = SETTING_NAV_LAND_SLOWDOWN_MINALT_DEFAULT, // altitude in centimeters
.land_slowdown_maxalt = SETTING_NAV_LAND_SLOWDOWN_MAXALT_DEFAULT, // altitude in meters
.land_minalt_vspd = SETTING_NAV_LAND_MINALT_VSPD_DEFAULT, // centimeters/s
@ -182,6 +181,8 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
// MC-specific
.mc = {
.max_bank_angle = SETTING_NAV_MC_BANK_ANGLE_DEFAULT, // degrees
.max_auto_climb_rate = SETTING_NAV_MC_AUTO_CLIMB_RATE_DEFAULT, // 5 m/s
.max_manual_climb_rate = SETTING_NAV_MC_MANUAL_CLIMB_RATE_DEFAULT,
#ifdef USE_MR_BRAKING_MODE
.braking_speed_threshold = SETTING_NAV_MC_BRAKING_SPEED_THRESHOLD_DEFAULT, // Braking can become active above 1m/s
@ -203,6 +204,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
// Fixed wing
.fw = {
.max_bank_angle = SETTING_NAV_FW_BANK_ANGLE_DEFAULT, // degrees
.max_manual_climb_rate = SETTING_NAV_FW_MANUAL_CLIMB_RATE_DEFAULT, // 3 m/s
.max_climb_angle = SETTING_NAV_FW_CLIMB_ANGLE_DEFAULT, // degrees
.max_dive_angle = SETTING_NAV_FW_DIVE_ANGLE_DEFAULT, // degrees
.cruise_speed = SETTING_NAV_FW_CRUISE_SPEED_DEFAULT, // cm/s
@ -283,11 +285,9 @@ static void resetJumpCounter(void);
static void clearJumpCounters(void);
static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint);
static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos);
void calculateInitialHoldPosition(fpVector3_t * pos);
void calculateFarAwayPos(fpVector3_t * farAwayPos, const fpVector3_t *start, int32_t bearing, int32_t distance);
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);
@ -1426,16 +1426,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
return NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING; // NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING
}
else {
// Switch to RTH trackback
bool trackbackActive = navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_ON ||
(navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_FS && posControl.flags.forcedRTHActivated);
if (trackbackActive && posControl.activeRthTBPointIndex >= 0 && !isWaypointMissionRTHActive()) {
updateRthTrackback(true); // save final trackpoint for altitude and max trackback distance reference
} else {
if (rthTrackBackIsActive() && rth_trackback.activePointIndex >= 0 && !isWaypointMissionRTHActive()) {
rthTrackBackUpdate(true); // save final trackpoint for altitude and max trackback distance reference
posControl.flags.rthTrackbackActive = true;
calculateAndSetActiveWaypointToLocalPosition(rthGetTrackbackPos());
calculateAndSetActiveWaypointToLocalPosition(getRthTrackBackPosition());
return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_TRACKBACK;
}
@ -1550,36 +1545,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_TRACKBACK(navigatio
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}
if (posControl.flags.estPosStatus >= EST_USABLE) {
const int32_t distFromStartTrackback = calculateDistanceToDestination(&posControl.rthTBPointsList[posControl.rthTBLastSavedIndex]) / 100;
#ifdef USE_MULTI_FUNCTIONS
const bool overrideTrackback = rthAltControlStickOverrideCheck(ROLL) || MULTI_FUNC_FLAG(MF_SUSPEND_TRACKBACK);
#else
const bool overrideTrackback = rthAltControlStickOverrideCheck(ROLL);
#endif
const bool cancelTrackback = distFromStartTrackback > navConfig()->general.rth_trackback_distance ||
(overrideTrackback && !posControl.flags.forcedRTHActivated);
if (posControl.activeRthTBPointIndex < 0 || cancelTrackback) {
posControl.rthTBWrapAroundCounter = posControl.activeRthTBPointIndex = -1;
posControl.flags.rthTrackbackActive = false;
return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE; // procede to home after final trackback point
}
if (isWaypointReached(&posControl.activeWaypoint.pos, &posControl.activeWaypoint.bearing)) {
posControl.activeRthTBPointIndex--;
if (posControl.rthTBWrapAroundCounter > -1 && posControl.activeRthTBPointIndex < 0) {
posControl.activeRthTBPointIndex = NAV_RTH_TRACKBACK_POINTS - 1;
}
calculateAndSetActiveWaypointToLocalPosition(rthGetTrackbackPos());
if (posControl.activeRthTBPointIndex - posControl.rthTBWrapAroundCounter == 0) {
posControl.rthTBWrapAroundCounter = posControl.activeRthTBPointIndex = -1;
}
} else {
setDesiredPosition(rthGetTrackbackPos(), 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
}
if (rthTrackBackSetNewPosition()) {
return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE;
}
return NAV_FSM_EVENT_NONE;
@ -2832,7 +2799,7 @@ static bool getLocalPosNextWaypoint(fpVector3_t * nextWpPos)
* Check if waypoint is/was reached.
* waypointBearing stores initial bearing to waypoint
*-----------------------------------------------------------*/
static bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * waypointBearing)
bool isWaypointReached(const fpVector3_t *waypointPos, const int32_t *waypointBearing)
{
posControl.wpDistance = calculateDistanceToDestination(waypointPos);
@ -2956,8 +2923,19 @@ bool validateRTHSanityChecker(void)
return true;
}
#ifdef USE_GPS_FIX_ESTIMATION
if (STATE(GPS_ESTIMATED_FIX)) {
//disable sanity checks in GPS estimation mode
//when estimated GPS fix is replaced with real fix, coordinates may jump
posControl.rthSanityChecker.minimalDistanceToHome = 1e10f;
//schedule check in 5 seconds after getting real GPS fix, when position estimation coords stabilise after jump
posControl.rthSanityChecker.lastCheckTime = currentTimeMs + 5000;
return true;
}
#endif
// Check at 10Hz rate
if ((currentTimeMs - posControl.rthSanityChecker.lastCheckTime) > 100) {
if ( ((int32_t)(currentTimeMs - posControl.rthSanityChecker.lastCheckTime)) > 100) {
const float currentDistanceToHome = calculateDistanceToDestination(&posControl.rthState.homePosition.pos);
posControl.rthSanityChecker.lastCheckTime = currentTimeMs;
@ -3181,12 +3159,13 @@ void updateHomePosition(void)
* Climb First override limited to Fixed Wing only
* Roll also cancels RTH trackback on Fixed Wing and Multirotor
*-----------------------------------------------------------*/
static bool rthAltControlStickOverrideCheck(unsigned axis)
bool rthAltControlStickOverrideCheck(uint8_t axis)
{
if (!navConfig()->general.flags.rth_alt_control_override || posControl.flags.forcedRTHActivated ||
(axis == ROLL && STATE(MULTIROTOR) && !posControl.flags.rthTrackbackActive)) {
return false;
}
static timeMs_t rthOverrideStickHoldStartTime[2];
if (rxGetChannelValue(axis) > rxConfig()->maxcheck) {
@ -3225,110 +3204,6 @@ static bool rthAltControlStickOverrideCheck(unsigned axis)
return false;
}
/* --------------------------------------------------------------------------------
* == RTH Trackback ==
* Saves track during flight which is used during RTH to back track
* along arrival route rather than immediately heading directly toward home.
* Max desired trackback distance set by user or limited by number of available points.
* Reverts to normal RTH heading direct to home when end of track reached.
* Trackpoints logged with precedence for course/altitude changes. Distance based changes
* only logged if no course/altitude changes logged over an extended distance.
* Tracking suspended during fixed wing loiter (PosHold and WP Mode timed hold).
* --------------------------------------------------------------------------------- */
static void updateRthTrackback(bool forceSaveTrackPoint)
{
static bool suspendTracking = false;
bool fwLoiterIsActive = STATE(AIRPLANE) && (NAV_Status.state == MW_NAV_STATE_HOLD_TIMED || FLIGHT_MODE(NAV_POSHOLD_MODE));
if (!fwLoiterIsActive && suspendTracking) {
suspendTracking = false;
}
if (navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_OFF || FLIGHT_MODE(NAV_RTH_MODE) || !ARMING_FLAG(ARMED) || suspendTracking) {
return;
}
// Record trackback points based on significant change in course/altitude until
// points limit reached. Overwrite older points from then on.
if (posControl.flags.estPosStatus >= EST_USABLE && posControl.flags.estAltStatus >= EST_USABLE) {
static int32_t previousTBTripDist; // cm
static int16_t previousTBCourse; // degrees
static int16_t previousTBAltitude; // meters
static uint8_t distanceCounter = 0;
bool saveTrackpoint = forceSaveTrackPoint;
bool GPSCourseIsValid = isGPSHeadingValid();
// start recording when some distance from home, 50m seems reasonable.
if (posControl.activeRthTBPointIndex < 0) {
saveTrackpoint = posControl.homeDistance > METERS_TO_CENTIMETERS(50);
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
const bool distanceIncrement = posControl.totalTripDistance - previousTBTripDist > METERS_TO_CENTIMETERS(10);
// Altitude change
if (ABS(previousTBAltitude - CENTIMETERS_TO_METERS(posControl.actualState.abs.pos.z)) > 10) { // meters
saveTrackpoint = true;
} else if (distanceIncrement && GPSCourseIsValid) {
// Course change - set to 45 degrees
if (ABS(wrap_18000(DEGREES_TO_CENTIDEGREES(DECIDEGREES_TO_DEGREES(gpsSol.groundCourse) - previousTBCourse))) > DEGREES_TO_CENTIDEGREES(45)) {
saveTrackpoint = true;
} else if (distanceCounter >= 9) {
// Distance based trackpoint logged if at least 10 distance increments occur without altitude or course change
// and deviation from projected course path > 20m
float distToPrevPoint = calculateDistanceToDestination(&posControl.rthTBPointsList[posControl.activeRthTBPointIndex]);
fpVector3_t virtualCoursePoint;
virtualCoursePoint.x = posControl.rthTBPointsList[posControl.activeRthTBPointIndex].x + distToPrevPoint * cos_approx(DEGREES_TO_RADIANS(previousTBCourse));
virtualCoursePoint.y = posControl.rthTBPointsList[posControl.activeRthTBPointIndex].y + distToPrevPoint * sin_approx(DEGREES_TO_RADIANS(previousTBCourse));
saveTrackpoint = calculateDistanceToDestination(&virtualCoursePoint) > METERS_TO_CENTIMETERS(20);
}
distanceCounter++;
previousTBTripDist = posControl.totalTripDistance;
} else if (!GPSCourseIsValid) {
// if no reliable course revert to basic distance logging based on direct distance from last point - set to 20m
saveTrackpoint = calculateDistanceToDestination(&posControl.rthTBPointsList[posControl.activeRthTBPointIndex]) > METERS_TO_CENTIMETERS(20);
previousTBTripDist = posControl.totalTripDistance;
}
// Suspend tracking during loiter on fixed wing. Save trackpoint at start of loiter.
if (distanceCounter && fwLoiterIsActive) {
saveTrackpoint = suspendTracking = true;
}
}
// when trackpoint store full, overwrite from start of store using 'rthTBWrapAroundCounter' to track overwrite position
if (saveTrackpoint) {
if (posControl.activeRthTBPointIndex == (NAV_RTH_TRACKBACK_POINTS - 1)) { // wraparound to start
posControl.rthTBWrapAroundCounter = posControl.activeRthTBPointIndex = 0;
} else {
posControl.activeRthTBPointIndex++;
if (posControl.rthTBWrapAroundCounter > -1) { // track wraparound overwrite position after store first filled
posControl.rthTBWrapAroundCounter = posControl.activeRthTBPointIndex;
}
}
posControl.rthTBPointsList[posControl.activeRthTBPointIndex] = posControl.actualState.abs.pos;
posControl.rthTBLastSavedIndex = posControl.activeRthTBPointIndex;
previousTBAltitude = CENTIMETERS_TO_METERS(posControl.actualState.abs.pos.z);
previousTBCourse = GPSCourseIsValid ? DECIDEGREES_TO_DEGREES(gpsSol.groundCourse) : previousTBCourse;
distanceCounter = 0;
}
}
}
static fpVector3_t * rthGetTrackbackPos(void)
{
// ensure trackback altitude never lower than altitude of start point
if (posControl.rthTBPointsList[posControl.activeRthTBPointIndex].z < posControl.rthTBPointsList[posControl.rthTBLastSavedIndex].z) {
posControl.rthTBPointsList[posControl.activeRthTBPointIndex].z = posControl.rthTBPointsList[posControl.rthTBLastSavedIndex].z;
}
return &posControl.rthTBPointsList[posControl.activeRthTBPointIndex];
}
/*-----------------------------------------------------------
* Update flight statistics
*-----------------------------------------------------------*/
@ -4007,7 +3882,7 @@ static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint
geoConvertGeodeticToLocal(localPos, &posControl.gpsOrigin, &wpLLH, altConv);
}
static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos)
void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t *pos)
{
// 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)) {
@ -4100,7 +3975,7 @@ bool isWaypointNavTrackingActive(void)
// is set from current position not previous WP. Works for WP Restart intermediate WP as well as first mission WP.
// (NAV_WP_MODE flag isn't set until WP initialisation is finished, i.e. after calculateAndSetActiveWaypoint called)
return FLIGHT_MODE(NAV_WP_MODE) || (posControl.flags.rthTrackbackActive && posControl.activeRthTBPointIndex != posControl.rthTBLastSavedIndex);
return FLIGHT_MODE(NAV_WP_MODE) || (posControl.flags.rthTrackbackActive && rth_trackback.activePointIndex != rth_trackback.lastSavedIndex);
}
/*-----------------------------------------------------------
@ -4140,9 +4015,7 @@ void applyWaypointNavigationAndAltitudeHold(void)
if (posControl.flags.estAglStatus == EST_TRUSTED) navFlags |= (1 << 1);
if (posControl.flags.estPosStatus == EST_TRUSTED) navFlags |= (1 << 2);
if (posControl.flags.isTerrainFollowEnabled) navFlags |= (1 << 3);
#if defined(NAV_GPS_GLITCH_DETECTION)
if (isGPSGlitchDetected()) navFlags |= (1 << 4);
#endif
// naFlags |= (1 << 4); // Old NAV GPS Glitch Detection flag
if (posControl.flags.estHeadingStatus == EST_TRUSTED) navFlags |= (1 << 5);
// Reset all navigation requests - NAV controllers will set them if necessary
@ -4157,9 +4030,7 @@ void applyWaypointNavigationAndAltitudeHold(void)
// ensure WP missions always restart from first waypoint after disarm
posControl.activeWaypointIndex = posControl.startWpIndex;
// Reset RTH trackback
posControl.activeRthTBPointIndex = -1;
posControl.flags.rthTrackbackActive = false;
posControl.rthTBWrapAroundCounter = -1;
resetRthTrackBack();
return;
}
@ -4605,7 +4476,11 @@ void updateWpMissionPlanner(void)
{
static timeMs_t resetTimerStart = 0;
if (IS_RC_MODE_ACTIVE(BOXPLANWPMISSION) && !(FLIGHT_MODE(NAV_WP_MODE) || isWaypointMissionRTHActive())) {
const bool positionTrusted = posControl.flags.estAltStatus == EST_TRUSTED && posControl.flags.estPosStatus == EST_TRUSTED && STATE(GPS_FIX);
const bool positionTrusted = posControl.flags.estAltStatus == EST_TRUSTED && posControl.flags.estPosStatus == EST_TRUSTED && (STATE(GPS_FIX)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
);
posControl.flags.wpMissionPlannerActive = true;
if (millis() - resetTimerStart < 1000 && navConfig()->general.flags.mission_planner_reset) {
@ -4693,7 +4568,7 @@ void updateWaypointsAndNavigationMode(void)
updateWpMissionPlanner();
// Update RTH trackback
updateRthTrackback(false);
rthTrackBackUpdate(false);
//Update Blackbox data
navCurrentState = (int16_t)posControl.navPersistentId;