ArduPlane: add option for autoland direction capture

ArduPlane/AP_Arming.cpp

@@ -320,6 +320,17 @@ bool AP_Arming_Plane::arm(const AP_Arming::Method method, const bool do_arming_c
 
     // rising edge of delay_arming oneshot
     delay_arming = true;
+
+    //capture heading for autoland mode if option is set
+    if (plane.aparm.takeoff_options & (uint32_t)AP_FixedWing::TakeoffOption::AUTOLAND_DIR_ON_ARM) {
+        if (plane.compass.available() && plane.compass.healthy()){
+            plane.takeoff_state.takeoff_initial_direction = plane.ahrs.yaw_sensor * 0.01f;
+            plane.takeoff_state.takeoff_direction_initialized = true;
+            gcs().send_text(MAV_SEVERITY_INFO, "Autoland direction= %u",int(plane.takeoff_state.takeoff_initial_direction));
+        } else {
+        gcs().send_text(MAV_SEVERITY_WARNING, "Must have compass to set autoland dir on arm");
+        }
+    }
 
     send_arm_disarm_statustext("Throttle armed");
 

ArduPlane/Parameters.cpp

@@ -170,7 +170,7 @@ const AP_Param::Info Plane::var_info[] = {
     // @Param: TKOFF_OPTIONS
     // @DisplayName: Takeoff options
     // @Description: This selects the mode of the takeoff in AUTO and TAKEOFF flight modes. 
-    // @Bitmask: 0: When unset the maximum allowed throttle is always used (THR_MAX or TKOFF_THR_MAX) during takeoff. When set TECS is allowed to operate between a minimum (THR_MIN or TKOFF_THR_MIN) and a maximum (THR_MAX or TKOFF_THR_MAX) limit. Applicable only when using an airspeed sensor.
+    // @Bitmask: 0: When unset the maximum allowed throttle is always used (THR_MAX or TKOFF_THR_MAX) during takeoff. When set TECS is allowed to operate between a minimum (THR_MIN or TKOFF_THR_MIN) and a maximum (THR_MAX or TKOFF_THR_MAX) limit. Applicable only when using an airspeed sensor. 1: When set, if there is a healthy compass available (whether or not its being "USED"), the compass heading will be captured at arming and used for the AUTOLAND mode's initial takeoff direction instead of capturing ground course in NAV_TAKEOFF or Mode TAKEOFF.
     // @User: Advanced
     ASCALAR(takeoff_options,               "TKOFF_OPTIONS",       0),
 

ArduPlane/commands_logic.cpp

@@ -382,7 +382,9 @@ void Plane::do_takeoff(const AP_Mission::Mission_Command& cmd)
     // zero locked course
     steer_state.locked_course_err = 0;
     steer_state.hold_course_cd = -1;
-    takeoff_state.takeoff_direction_initialized = false;
+    if (!(aparm.takeoff_options & (uint32_t)AP_FixedWing::TakeoffOption::AUTOLAND_DIR_ON_ARM)) {
+        plane.takeoff_state.takeoff_direction_initialized = false;
+    }
     auto_state.baro_takeoff_alt = barometer.get_altitude();
 }
 
@@ -557,9 +559,11 @@ bool Plane::verify_takeoff()
             gps.ground_speed() > min_gps_speed &&
             hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED) {
             auto_state.takeoff_speed_time_ms = millis();
+            if (!(aparm.takeoff_options & (uint32_t)AP_FixedWing::TakeoffOption::AUTOLAND_DIR_ON_ARM)) {
             takeoff_state.takeoff_initial_direction = gps.ground_course();
             takeoff_state.takeoff_direction_initialized = true;
             gcs().send_text(MAV_SEVERITY_INFO, "Takeoff initial direction= %u",int(takeoff_state.takeoff_initial_direction));
+            }
         }
         if (auto_state.takeoff_speed_time_ms != 0 &&
             millis() - auto_state.takeoff_speed_time_ms >= 2000) {

ArduPlane/mode_takeoff.cpp

@@ -134,7 +134,9 @@ void ModeTakeoff::update()
                 plane.takeoff_state.throttle_max_timer_ms = millis();
                 takeoff_mode_setup = true;
                 plane.steer_state.hold_course_cd = wrap_360_cd(direction*100); // Necessary to allow Plane::takeoff_calc_roll() to function.
-                plane.takeoff_state.takeoff_direction_initialized = false;
+                if (!(plane.aparm.takeoff_options & (uint32_t)AP_FixedWing::TakeoffOption::AUTOLAND_DIR_ON_ARM)) {
+                    plane.takeoff_state.takeoff_direction_initialized = false;
+                }
             }
         }
     }

ArduPlane/takeoff.cpp

@@ -22,9 +22,11 @@ bool Plane::auto_takeoff_check(void)
         return false;
     }
 
-    // Reset states if process has been interrupted
+    // Reset states if process has been interrupted, except takeoff_direction_initialized if set
+    bool takeoff_dir_initialized = takeoff_state.takeoff_direction_initialized;
     if (takeoff_state.last_check_ms && (now - takeoff_state.last_check_ms) > 200) {
         memset(&takeoff_state, 0, sizeof(takeoff_state));
+        takeoff_state.takeoff_direction_initialized = takeoff_dir_initialized; //restore dir init state
         return false;
     }