ArduPlane: add option for autoland direction capture

Hwurzburg · Dec 7, 2024 · 886b495 · 886b495
1 parent 9f77da8
commit 886b495
Show file tree

Hide file tree

Showing 3 changed files with 19 additions and 2 deletions.
diff --git a/ArduPlane/AP_Arming.cpp b/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");
 

diff --git a/ArduPlane/commands_logic.cpp b/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) {

diff --git a/ArduPlane/mode_takeoff.cpp b/ArduPlane/mode_takeoff.cpp
@@ -123,7 +123,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;
+                }
             }
         }
     }