Plane: make auto takeoff independent of compass

this solves a problem of poor initial yaw due to poor compass offsets
causing a takeoff to not be in the direction the plane is pointing. A
summed gyro is used until the GPS speed is above 5m/s for 2 seconds,
then the GPS heading corrected by the summed gyro error is used for L1
based navigation for the rest of the takeoff

ArduPlane/ArduPlane.pde

@@ -514,7 +514,7 @@ static struct {
     int32_t hold_course_cd;
 
     // locked_course and locked_course_cd are used in stabilize mode 
-    // when ground steering is active
+    // when ground steering is active, and for steering in auto-takeoff
     bool locked_course;
     float locked_course_err;
 } steer_state = {
@@ -564,6 +564,9 @@ static struct {
 
     // filtered sink rate for landing
     float land_sink_rate;
+
+    // time when we first pass min GPS speed on takeoff
+    uint32_t takeoff_speed_time_ms;
 } auto_state = {
     takeoff_complete : true,
     land_complete : false,
@@ -576,7 +579,8 @@ static struct {
     highest_airspeed : 0,
     initial_pitch_cd : 0,
     next_turn_angle  : 90.0f,
-    land_sink_rate   : 0
+    land_sink_rate   : 0,
+    takeoff_speed_time_ms : 0
 };
 
 // true if we are in an auto-throttle mode, which means
@@ -887,6 +891,12 @@ static void ahrs_update()
     // calculate a scaled roll limit based on current pitch
     roll_limit_cd = g.roll_limit_cd * cosf(ahrs.pitch);
     pitch_limit_min_cd = aparm.pitch_limit_min_cd * fabsf(cosf(ahrs.roll));
+
+    // updated the summed gyro used for ground steering and
+    // auto-takeoff. Dot product of DCM.c with gyro vector gives earth
+    // frame yaw rate
+    steer_state.locked_course_err += ahrs.get_yaw_rate_earth() * G_Dt;
+    steer_state.locked_course_err = wrap_PI(steer_state.locked_course_err);
 }
 
 /*

ArduPlane/Attitude.pde

@@ -446,7 +446,8 @@ static void calc_nav_yaw_course(void)
 static void calc_nav_yaw_ground(void)
 {
     if (gps.ground_speed() < 1 && 
-        channel_throttle->control_in == 0) {
+        channel_throttle->control_in == 0 &&
+        flight_stage != AP_SpdHgtControl::FLIGHT_TAKEOFF) {
         // manual rudder control while still
         steer_state.locked_course = false;
         steer_state.locked_course_err = 0;
@@ -455,20 +456,24 @@ static void calc_nav_yaw_ground(void)
     }
 
     float steer_rate = (channel_rudder->control_in/4500.0f) * g.ground_steer_dps;
+    if (flight_stage == AP_SpdHgtControl::FLIGHT_TAKEOFF) {
+        steer_rate = 0;
+    }
     if (steer_rate != 0) {
         // pilot is giving rudder input
         steer_state.locked_course = false;        
     } else if (!steer_state.locked_course) {
         // pilot has released the rudder stick or we are still - lock the course
         steer_state.locked_course = true;
-        steer_state.locked_course_err = 0;
+        if (flight_stage != AP_SpdHgtControl::FLIGHT_TAKEOFF) {
+            steer_state.locked_course_err = 0;
+        }
     }
     if (!steer_state.locked_course) {
         // use a rate controller at the pilot specified rate
         steering_control.steering = steerController.get_steering_out_rate(steer_rate);
     } else {
         // use a error controller on the summed error
-        steer_state.locked_course_err += ahrs.get_gyro().z * G_Dt;
         int32_t yaw_error_cd = -ToDeg(steer_state.locked_course_err)*100;
         steering_control.steering = steerController.get_steering_out_angle_error(yaw_error_cd);
     }
@@ -588,11 +593,6 @@ static bool suppress_throttle(void)
         auto_takeoff_check()) {
         // we're in auto takeoff 
         throttle_suppressed = false;
-        if (steer_state.hold_course_cd != -1) {
-            // update takeoff course hold, if already initialised
-            steer_state.hold_course_cd = ahrs.yaw_sensor;
-            gcs_send_text_fmt(PSTR("Holding course %ld"), steer_state.hold_course_cd);
-        }
         return false;
     }
     

ArduPlane/commands_logic.pde

@@ -282,8 +282,13 @@ static void do_takeoff(const AP_Mission::Mission_Command& cmd)
     auto_state.takeoff_altitude_cm     = next_WP_loc.alt;
     next_WP_loc.lat = home.lat + 10;
     next_WP_loc.lng = home.lng + 10;
+    auto_state.takeoff_speed_time_ms = 0;
     auto_state.takeoff_complete = false;                            // set flag to use gps ground course during TO.  IMU will be doing yaw drift correction
     // Flag also used to override "on the ground" throttle disable
+
+    // zero locked course
+    steer_state.locked_course_err = 0;
+    
 }
 
 static void do_nav_wp(const AP_Mission::Mission_Command& cmd)
@@ -332,11 +337,28 @@ static void do_loiter_time(const AP_Mission::Mission_Command& cmd)
 /********************************************************************************/
 static bool verify_takeoff()
 {
-    if (ahrs.yaw_initialised()) {
-        if (steer_state.hold_course_cd == -1) {
-            // save our current course to take off
-            steer_state.hold_course_cd = ahrs.yaw_sensor;
-            gcs_send_text_fmt(PSTR("Holding course %ld"), steer_state.hold_course_cd);
+    if (ahrs.yaw_initialised() && steer_state.hold_course_cd == -1) {
+        const float min_gps_speed = 5;
+        if (auto_state.takeoff_speed_time_ms == 0 && 
+            gps.status() >= AP_GPS::GPS_OK_FIX_3D && 
+            gps.ground_speed() > min_gps_speed) {
+            auto_state.takeoff_speed_time_ms = hal.scheduler->millis();
+        }
+        if (auto_state.takeoff_speed_time_ms != 0 &&
+            hal.scheduler->millis() - auto_state.takeoff_speed_time_ms >= 2000) {
+            // once we reach sufficient speed for good GPS course
+            // estimation we save our current GPS ground course
+            // corrected for summed yaw to set the take off
+            // course. This keeps wings level until we are ready to
+            // rotate, and also allows us to cope with arbitary
+            // compass errors for auto takeoff
+            float takeoff_course = wrap_PI(radians(gps.ground_course_cd()*0.01)) - steer_state.locked_course_err;
+            takeoff_course = wrap_PI(takeoff_course);
+            steer_state.hold_course_cd = wrap_360_cd(degrees(takeoff_course)*100);
+            gcs_send_text_fmt(PSTR("Holding course %ld at %.1fm/s (%.1f)"), 
+                              steer_state.hold_course_cd,
+                              gps.ground_speed(),
+                              degrees(steer_state.locked_course_err));
         }
     }
 

ArduPlane/system.pde

@@ -297,6 +297,9 @@ static void set_mode(enum FlightMode mode)
     // don't cross-track when starting a mission
     auto_state.next_wp_no_crosstrack = true;
 
+    // zero locked course
+    steer_state.locked_course_err = 0;
+
     // set mode
     previous_mode = control_mode;
     control_mode = mode;