Copter: new ROLL_PITCH_WP_INAV roll-pitch mode

small corrections to allow waypoints to work with new inertial nav wp
controller

ArduCopter/ArduCopter.pde

@@ -1614,6 +1614,7 @@ bool set_roll_pitch_mode(uint8_t new_roll_pitch_mode)
             break;
 
         case ROLL_PITCH_LOITER_INAV:
+        case ROLL_PITCH_WP_INAV:
             // require gps lock
             if( ap.home_is_set ) {
                 roll_pitch_initialised = true;
@@ -1733,6 +1734,15 @@ void update_roll_pitch_mode(void)
         get_stabilize_roll(nav_roll);
         get_stabilize_pitch(nav_pitch);
         break;
+
+    case ROLL_PITCH_WP_INAV:
+        // To-Do: allow pilot to take control of target location
+        // copy latest output from nav controller to stabilize controller
+        nav_roll    = auto_roll;
+        nav_pitch   = auto_pitch;
+        get_stabilize_roll(nav_roll);
+        get_stabilize_pitch(nav_pitch);
+        break;
     }
 
 	#if FRAME_CONFIG != HELI_FRAME

ArduCopter/commands.pde

@@ -150,25 +150,30 @@ static void set_next_WP(struct Location *wp)
     // To-Do: remove the restriction on negative altitude targets (after testing)
     set_new_altitude(max(wp->alt,100));
 
-    // this is handy for the groundstation
-    // -----------------------------------
+    // recalculate waypoint distance and bearing
     wp_distance             = get_distance_cm(&current_loc, &next_WP);
     wp_bearing              = get_bearing_cd(&prev_WP, &next_WP);
+    original_wp_bearing     = wp_bearing;       // to check if we have missed the WP
 
     // calc the location error:
     calc_location_error(&next_WP);
-
-    // to check if we have missed the WP
-    // ---------------------------------
-    original_wp_bearing = wp_bearing;
 }
 
 // set_next_WP_latlon - update just lat and lon targets for nav controllers
 static void set_next_WP_latlon(uint32_t lat, uint32_t lon)
 {
+    // save current location as previous location
+    prev_WP = current_loc;
+
     // Load the next_WP slot
     next_WP.lat = lat;
     next_WP.lng = lon;
+
+    // recalculate waypoint distance and bearing
+    // To-Do: these functions are too expensive to be called every time we update the next_WP
+    wp_distance             = get_distance_cm(&current_loc, &next_WP);
+    wp_bearing              = get_bearing_cd(&prev_WP, &next_WP);
+    original_wp_bearing     = wp_bearing;       // to check if we have missed the WP
 }
 
 // run this at setup on the ground

ArduCopter/defines.h

@@ -31,6 +31,7 @@
 #define ROLL_PITCH_STABLE_OF        3
 #define ROLL_PITCH_TOY              4       // THOR This is the Roll and Pitch mode
 #define ROLL_PITCH_LOITER_INAV      5       // pilot inputs the desired horizontal velocities
+#define ROLL_PITCH_WP_INAV          6       // pilot inputs the desired horizontal velocities which temporarily interrupt the autopilot
 
 #define THROTTLE_MANUAL                     0   // manual throttle mode - pilot input goes directly to motors
 #define THROTTLE_MANUAL_TILT_COMPENSATED    1   // mostly manual throttle but with some tilt compensation

ArduCopter/navigation.pde

@@ -264,7 +264,8 @@ static bool set_nav_mode(uint8_t new_nav_mode)
             break;
 
         case NAV_WP_INAV:
-            nav_initialised = true;
+            // To-Do: below allows user to move around set point but do we want to allow this when in Auto flight mode?
+            nav_initialised = set_roll_pitch_mode(ROLL_PITCH_WP_INAV);
             break;
     }
 
@@ -397,7 +398,7 @@ static bool check_missed_wp()
     int32_t temp;
     temp = wp_bearing - original_wp_bearing;
     temp = wrap_180(temp);
-    return (labs(temp) > 9000);         // we passed the waypoint by 100 degrees
+    return (labs(temp) > 9000);         // we passed the waypoint by 90 degrees
 }
 
 ////////////////////////////////////////////////////////////////
@@ -538,7 +539,7 @@ static void calc_nav_rate(int16_t max_speed)
 static void calc_nav_pitch_roll()
 {
     // To-Do: remove this hack dependent upon nav_mode
-    if( nav_mode != NAV_LOITER_INAV ) {
+    if( nav_mode != NAV_LOITER_INAV && nav_mode != NAV_WP_INAV ) {
         // rotate the vector
         auto_roll       = (float)nav_lon * sin_yaw_y - (float)nav_lat * cos_yaw_x;
         auto_pitch      = (float)nav_lon * cos_yaw_x + (float)nav_lat * sin_yaw_y;