Enforce orbit waypoint relative heading to target, fixes possibly fly…

…-away condition;

Fix maximum horizontal speed for orbit waypoint (it used global default instead of wpt setting);
Increase tolerance for orbit waypoint arrival detection (still not very reliable at higher orbit speeds);
On wpt arrival, only freeze heading if it was relative (prevents unnecessary yawing in some cases).

(cherry picked from commit ff9e02a)

CHANGES.md

@@ -10,6 +10,13 @@ This file describes changes in the firmware, typically since the previous minor
 	! : important change, possible danger, change of default behavior, etc.
 
 
+##### 7.1.1898 - Nov. 15, 2016
+
+`*` Enforce orbit waypoint relative heading to target, fixes possible fly-away condition in all firmware since v6.6.  
+`*` Fix maximum horizontal speed for orbit waypoint (it used global default instead of wpt setting in all firmware since v6.6).  
+`~` Increase tolerance for orbit waypoint arrival detection (still not very reliable at higher orbit speeds).  
+`~` On wpt arrival, only freeze heading if it was relative (prevents unnecessary yawing in some cases).  
+
 ##### 7.1.1897 - Apr. 13, 2016
 
 `*` Fix bug in 7.1.1895 preventing gimbal roll/pitch reversing.  

src/buildnum.h

@@ -1 +1 @@
-#define BUILDNUMBER 1897
+#define BUILDNUMBER 1898

src/nav.c

@@ -201,8 +201,7 @@ navMission_t *navLoadLeg(uint8_t leg) {
     else if (curLeg->type == NAV_LEG_ORBIT) {
         if (curLeg->targetLat != (double)0.0 && curLeg->targetLon != (double)0.0)
             navUkfSetGlobalPositionTarget(curLeg->targetLat, curLeg->targetLon);
-        navData.targetHeading = curLeg->poiHeading;
-        navData.holdMaxHorizSpeed = NAV_DFLT_HOR_SPEED;
+        navData.targetHeading = -0.0f;  // must point to target
     }
     else if (curLeg->type == NAV_LEG_TAKEOFF) {
         // store this position as the takeoff position
@@ -508,16 +507,16 @@ void navNavigate(void) {
                 fabsf(navData.holdAlt - ALTITUDE) < curLeg->targetRadius) ||
             // orbit test
                 (curLeg->type == NAV_LEG_ORBIT &&
-                fabsf(navData.holdDistance - curLeg->targetRadius) +
-                fabsf(navData.holdAlt - ALTITUDE) < 2.0f)  ||
+                fabsf(navData.holdDistance - curLeg->targetRadius) <= 2.0f &&
+                fabsf(navData.holdAlt - ALTITUDE) <= 1.0f) ||
             // takeoff test
                 (curLeg->type == NAV_LEG_TAKEOFF &&
                 navData.holdDistance < curLeg->targetRadius &&
                 fabsf(navData.holdAlt - ALTITUDE) < curLeg->targetRadius)
                 ) {
-                    // freeze heading unless orbiting
-                    if (curLeg->type != NAV_LEG_ORBIT)
-                        navSetHoldHeading(AQ_YAW);
+                    // freeze heading if relative, unless orbiting
+                    if (curLeg->type != NAV_LEG_ORBIT && signbit(navData.targetHeading))
+                	navData.targetHeading = AQ_YAW;
 
                     // start the loiter clock
                     navData.loiterCompleteTime = currentTime + curLeg->loiterTime;