src/main/navigation/rth_trackback.c

   1 /*
   2  * This file is part of INAV.
   3  *
   4  * INAV is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * INAV is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with INAV.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 /* --------------------------------------------------------------------------------
  19  * == RTH Trackback ==
  20  * Saves track during flight which is used during RTH to back track
  21  * along arrival route rather than immediately heading directly toward home.
  22  * Max desired trackback distance set by user or limited by number of available points.
  23  * Reverts to normal RTH heading direct to home when end of track reached.
  24  * Trackpoints logged with precedence for course/altitude changes. Distance based changes
  25  * only logged if no course/altitude changes logged over an extended distance.
  26  * Tracking suspended during fixed wing loiter (PosHold and WP Mode timed hold).
  27  * --------------------------------------------------------------------------------- */
  28
  29 #include "platform.h"
  30
  31 #include "fc/multifunction.h"
  32 #include "fc/rc_controls.h"
  33
  34 #include "navigation/rth_trackback.h"
  35 #include "navigation/navigation.h"
  36 #include "navigation/navigation_private.h"
  37
  38 rth_trackback_t rth_trackback;
  39
  40 bool rthTrackBackCanBeActivated(void)
  41 {
  42     return posControl.flags.estPosStatus >= EST_USABLE &&
  43            (navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_ON || (navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_FS && posControl.flags.forcedRTHActivated));
  44 }
  45
  46 void rthTrackBackUpdate(bool forceSaveTrackPoint)
  47 {
  48     static bool suspendTracking = false;
  49     bool fwLoiterIsActive = STATE(AIRPLANE) && (NAV_Status.state == MW_NAV_STATE_HOLD_TIMED || FLIGHT_MODE(NAV_POSHOLD_MODE));
  50
  51     if (!fwLoiterIsActive && suspendTracking) {
  52         suspendTracking = false;
  53     }
  54
  55     if (navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_OFF || FLIGHT_MODE(NAV_RTH_MODE) || !ARMING_FLAG(ARMED) || suspendTracking) {
  56         return;
  57     }
  58
  59     // Record trackback points based on significant change in course/altitude until points limit reached. Overwrite older points from then on.
  60     if (posControl.flags.estPosStatus >= EST_USABLE && posControl.flags.estAltStatus >= EST_USABLE) {
  61         static int32_t previousTBTripDist;      // cm
  62         static int16_t previousTBCourse;        // degrees
  63         static int16_t previousTBAltitude;      // meters
  64         static uint8_t distanceCounter = 0;
  65         bool saveTrackpoint = forceSaveTrackPoint;
  66         bool GPSCourseIsValid = isGPSHeadingValid();
  67
  68         // Start recording when some distance from home
  69         if (rth_trackback.activePointIndex < 0) {
  70             saveTrackpoint = posControl.homeDistance > METERS_TO_CENTIMETERS(NAV_RTH_TRACKBACK_MIN_DIST_TO_START);
  71             previousTBCourse = CENTIDEGREES_TO_DEGREES(posControl.actualState.cog);
  72             previousTBTripDist = posControl.totalTripDistance;
  73         } else {
  74             // Minimum distance increment between course change track points when GPS course valid
  75             const bool distanceIncrement = posControl.totalTripDistance - previousTBTripDist > METERS_TO_CENTIMETERS(NAV_RTH_TRACKBACK_MIN_TRIP_DIST_TO_SAVE);
  76
  77             // Altitude change
  78             if (ABS(previousTBAltitude - CENTIMETERS_TO_METERS(posControl.actualState.abs.pos.z)) > NAV_RTH_TRACKBACK_MIN_Z_DIST_TO_SAVE) {
  79                 saveTrackpoint = true;
  80             } else if (distanceIncrement && GPSCourseIsValid) {
  81                 // Course change - set to 45 degrees
  82                 if (ABS(wrap_18000(DEGREES_TO_CENTIDEGREES(DECIDEGREES_TO_DEGREES(gpsSol.groundCourse) - previousTBCourse))) > DEGREES_TO_CENTIDEGREES(45)) {
  83                     saveTrackpoint = true;
  84                 } else if (distanceCounter >= 9) {
  85                     // Distance based trackpoint logged if at least 10 distance increments occur without altitude or course change and deviation from projected course path > 20m
  86                     float distToPrevPoint = calculateDistanceToDestination(&rth_trackback.pointsList[rth_trackback.activePointIndex]);
  87
  88                     fpVector3_t virtualCoursePoint;
  89                     virtualCoursePoint.x = rth_trackback.pointsList[rth_trackback.activePointIndex].x + distToPrevPoint * cos_approx(DEGREES_TO_RADIANS(previousTBCourse));
  90                     virtualCoursePoint.y = rth_trackback.pointsList[rth_trackback.activePointIndex].y + distToPrevPoint * sin_approx(DEGREES_TO_RADIANS(previousTBCourse));
  91
  92                     saveTrackpoint = calculateDistanceToDestination(&virtualCoursePoint) > METERS_TO_CENTIMETERS(NAV_RTH_TRACKBACK_MIN_XY_DIST_TO_SAVE);
  93                 }
  94                 distanceCounter++;
  95                 previousTBTripDist = posControl.totalTripDistance;
  96             } else if (!GPSCourseIsValid) {
  97                 // If no reliable course revert to basic distance logging based on direct distance from last point
  98                 saveTrackpoint = calculateDistanceToDestination(&rth_trackback.pointsList[rth_trackback.activePointIndex]) > METERS_TO_CENTIMETERS(NAV_RTH_TRACKBACK_MIN_XY_DIST_TO_SAVE);
  99                 previousTBTripDist = posControl.totalTripDistance;
 100             }
 101
 102             // Suspend tracking during loiter on fixed wing. Save trackpoint at start of loiter.
 103             if (distanceCounter && fwLoiterIsActive) {
 104                 saveTrackpoint = suspendTracking = true;
 105             }
 106         }
 107
 108         // When trackpoint store full, overwrite from start of store using 'WrapAroundCounter' to track overwrite position
 109         if (saveTrackpoint) {
 110             if (rth_trackback.activePointIndex == (NAV_RTH_TRACKBACK_POINTS - 1)) {   // Wraparound to start
 111                 rth_trackback.WrapAroundCounter = rth_trackback.activePointIndex = 0;
 112             } else {
 113                 rth_trackback.activePointIndex++;
 114                 if (rth_trackback.WrapAroundCounter > -1) {   // Track wraparound overwrite position after store first filled
 115                     rth_trackback.WrapAroundCounter = rth_trackback.activePointIndex;
 116                 }
 117             }
 118
 119             rth_trackback.pointsList[rth_trackback.activePointIndex] = posControl.actualState.abs.pos;
 120             rth_trackback.lastSavedIndex = rth_trackback.activePointIndex;
 121             previousTBAltitude = CENTIMETERS_TO_METERS(posControl.actualState.abs.pos.z);
 122             previousTBCourse = GPSCourseIsValid ? DECIDEGREES_TO_DEGREES(gpsSol.groundCourse) : previousTBCourse;
 123             distanceCounter = 0;
 124         }
 125     }
 126 }
 127
 128 bool rthTrackBackSetNewPosition(void)
 129 {
 130     if (posControl.flags.estPosStatus == EST_NONE) {
 131         return false;   // will fall back to RTH initialize allowing full RTH to handle position loss correctly
 132     }
 133
 134     const int32_t distFromStartTrackback = CENTIMETERS_TO_METERS(calculateDistanceToDestination(&rth_trackback.pointsList[rth_trackback.lastSavedIndex]));
 135
 136 #ifdef USE_MULTI_FUNCTIONS
 137     const bool overrideTrackback = rthAltControlStickOverrideCheck(ROLL) || MULTI_FUNC_FLAG(MF_SUSPEND_TRACKBACK);
 138 #else
 139     const bool overrideTrackback = rthAltControlStickOverrideCheck(ROLL);
 140 #endif
 141     const bool cancelTrackback = distFromStartTrackback > navConfig()->general.rth_trackback_distance || (overrideTrackback && !posControl.flags.forcedRTHActivated);
 142
 143     if (rth_trackback.activePointIndex < 0 || cancelTrackback) {
 144         rth_trackback.WrapAroundCounter = rth_trackback.activePointIndex = -1;
 145         posControl.flags.rthTrackbackActive = false;
 146         return false;    // No more trackback points to set, procede to home
 147     }
 148
 149     if (isWaypointReached(&posControl.activeWaypoint.pos, &posControl.activeWaypoint.bearing)) {
 150         rth_trackback.activePointIndex--;
 151
 152         if (rth_trackback.WrapAroundCounter > -1 && rth_trackback.activePointIndex < 0) {
 153             rth_trackback.activePointIndex = NAV_RTH_TRACKBACK_POINTS - 1;
 154         }
 155
 156         calculateAndSetActiveWaypointToLocalPosition(getRthTrackBackPosition());
 157
 158         if (rth_trackback.activePointIndex - rth_trackback.WrapAroundCounter == 0) {
 159             rth_trackback.WrapAroundCounter = rth_trackback.activePointIndex = -1;
 160         }
 161     } else {
 162         setDesiredPosition(getRthTrackBackPosition(), 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
 163     }
 164
 165     return true;
 166 }
 167
 168 fpVector3_t *getRthTrackBackPosition(void)
 169 {
 170     // Ensure trackback altitude never lower than altitude of start point
 171     if (rth_trackback.pointsList[rth_trackback.activePointIndex].z < rth_trackback.pointsList[rth_trackback.lastSavedIndex].z) {
 172         rth_trackback.pointsList[rth_trackback.activePointIndex].z = rth_trackback.pointsList[rth_trackback.lastSavedIndex].z;
 173     }
 174
 175     return &rth_trackback.pointsList[rth_trackback.activePointIndex];
 176 }
 177
 178 void resetRthTrackBack(void)
 179 {
 180     rth_trackback.activePointIndex = -1;
 181     posControl.flags.rthTrackbackActive = false;
 182     rth_trackback.WrapAroundCounter = -1;
 183 }