Plane: after takeoff, xtrack to a virtual wp 10k ahead instead of hol…

…d heading

ArduPlane/commands_logic.cpp

@@ -526,17 +526,19 @@ bool Plane::verify_takeoff()
             // course. This keeps wings level until we are ready to
             // rotate, and also allows us to cope with arbitrary
             // compass errors for auto takeoff
-            float takeoff_course = wrap_PI(radians(gps.ground_course())) - steer_state.locked_course_err;
-            takeoff_course = wrap_PI(takeoff_course);
-            steer_state.hold_course_cd = wrap_360_cd(degrees(takeoff_course)*100);
+            const float ground_course_rad = wrap_PI(ahrs.groundspeed_vector().angle());
+            const float ground_course = wrap_360(degrees(ground_course_rad));
+            const float takeoff_course_rad = wrap_PI(ground_course_rad - steer_state.locked_course_err);
+
+            steer_state.hold_course_cd = wrap_360_cd(degrees(takeoff_course_rad)*100);
 
             // when true, we will follow the recorded takeoff heading.
             // when false, we xtrack to a virtual waypoint 10km ahead of us on this takeoff heading
             // TODO: turn this hardcoded value into a param/option?
             steer_state.locked_course = false;
 
             next_WP_loc = current_loc;
-            next_WP_loc.offset_bearing(gps.ground_course(), 10000); // push it out 10km
+            next_WP_loc.offset_bearing(ground_course, 10000); // push it out 10km
             set_next_WP(next_WP_loc);
             auto_state.crosstrack = true;