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

create .c and .h archives to RTH TrackBack

Browse source
This commit is contained in:
Julio Cesar 2023-10-30 20:32:50 -03:00
parent 45a0ddd608
commit 9aa64c66cf
9 changed files with 254 additions and 170 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -55,6 +55,7 @@
#include "navigation/navigation.h"
#include "navigation/navigation_private.h"
#include "navigation/rth_trackback.h"
#include "rx/rx.h"
@ -254,10 +255,8 @@ 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 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);
@ -269,11 +268,6 @@ bool validateRTHSanityChecker(void);
void updateHomePosition(void);
bool abortLaunchAllowed(void);
static bool rthAltControlStickOverrideCheck(unsigned axis);
static void updateRthTrackback(bool forceSaveTrackPoint);
static fpVector3_t * rthGetTrackbackPos(void);
/*************************************************************************************************/
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_IDLE(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(navigationFSMState_t previousState);
@ -1250,16 +1244,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;
}
@ -1374,36 +1363,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;
@ -2379,7 +2340,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);
@ -2716,12 +2677,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) {
@ -2760,110 +2722,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
*-----------------------------------------------------------*/
@ -3519,7 +3377,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)) {
@ -3612,7 +3470,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);
}
/*-----------------------------------------------------------
@ -3669,9 +3527,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;
}
@ -4199,7 +4055,7 @@ void updateWaypointsAndNavigationMode(void)
updateWpMissionPlanner();
// Update RTH trackback
updateRthTrackback(false);
rthTrackBackUpdate(false);
//Update Blackbox data
navCurrentState = (int16_t)posControl.navPersistentId;