ArduPlane: add option for autoland direction capture

Hwurzburg · Dec 12, 2024 · de72245 · de72245
1 parent b40596a
commit de72245
Show file tree

Hide file tree

Showing 7 changed files with 33 additions and 8 deletions.
diff --git a/ArduPlane/AP_Arming.cpp b/ArduPlane/AP_Arming.cpp
@@ -320,6 +320,13 @@ 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) {
+        plane.takeoff_state.takeoff_initial_direction = wrap_360((plane.ahrs.yaw_sensor * 0.01f) + plane.mode_autoland.landing_dir_off);
+        plane.takeoff_state.takeoff_direction_initialized = true;
+        gcs().send_text(MAV_SEVERITY_INFO, "Autoland direction= %u",int(plane.takeoff_state.takeoff_initial_direction));
+    }
 
     send_arm_disarm_statustext("Throttle armed");
 

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

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();
-            takeoff_state.takeoff_initial_direction = gps.ground_course();
+            if (!(aparm.takeoff_options & (uint32_t)AP_FixedWing::TakeoffOption::AUTOLAND_DIR_ON_ARM)) {
+            takeoff_state.takeoff_initial_direction = wrap_360(gps.ground_course() + mode_autoland.landing_dir_off);
             takeoff_state.takeoff_direction_initialized = true;
-            gcs().send_text(MAV_SEVERITY_INFO, "Takeoff initial direction= %u",int(takeoff_state.takeoff_initial_direction));
+            gcs().send_text(MAV_SEVERITY_INFO, "Autoland 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.h b/ArduPlane/mode.h
@@ -885,6 +885,7 @@ class ModeAutoLand: public Mode
 
     AP_Int16 final_wp_alt;
     AP_Int16 final_wp_dist;
+    AP_Int16 landing_dir_off;
 
 protected:
     bool _enter() override;

diff --git a/ArduPlane/mode_autoland.cpp b/ArduPlane/mode_autoland.cpp
@@ -24,6 +24,15 @@ const AP_Param::GroupInfo ModeAutoLand::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("WP_DIST", 2, ModeAutoLand, final_wp_dist, 400),
 
+    // @Param: DIR_OFF
+    // @DisplayName: Landing direction offset from takeoff
+    // @Description: The captured takeoff direction (at arming,if TKOFF_OPTION bit1 is set, or after ground course is established in autotakeoffs)is offset by this amount to create a different landing direction and approach.
+    // @Range: -360 360
+    // @Increment: 1
+    // @Units: m
+    // @User: Standard
+    AP_GROUPINFO("DIR_OFF", 3, ModeAutoLand, landing_dir_off, 0),
+
     AP_GROUPEND
 };
 

diff --git a/ArduPlane/mode_takeoff.cpp b/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;
+                }
             }
         }
     }
@@ -148,9 +150,9 @@ void ModeTakeoff::update()
     const float min_gps_speed = GPS_GND_CRS_MIN_SPD;
     if (!(plane.takeoff_state.takeoff_direction_initialized) && (plane.gps.ground_speed() > min_gps_speed)
        && (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF)) {
-       plane.takeoff_state.takeoff_initial_direction = plane.gps.ground_course();
+       plane.takeoff_state.takeoff_initial_direction = wrap_360(plane.gps.ground_course() + plane.mode_autoland.landing_dir_off);
        plane.takeoff_state.takeoff_direction_initialized = true;
-       gcs().send_text(MAV_SEVERITY_INFO, "Takeoff initial direction= %u",int(plane.takeoff_state.takeoff_initial_direction));
+       gcs().send_text(MAV_SEVERITY_INFO, "Autoland direction= %u",int(plane.takeoff_state.takeoff_initial_direction));
     }
 
     // We update the waypoint to follow once we're past TKOFF_LVL_ALT or we

diff --git a/ArduPlane/takeoff.cpp b/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;
     }