wip

Hwurzburg · Dec 21, 2024 · 8e8b4b3 · 8e8b4b3
1 parent b3c8775
commit 8e8b4b3
Show file tree

Hide file tree

Showing 13 changed files with 51 additions and 10 deletions.
diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp
@@ -59,7 +59,9 @@ MAV_MODE GCS_MAVLINK_Plane::base_mode() const
     case Mode::Number::GUIDED:
     case Mode::Number::CIRCLE:
     case Mode::Number::TAKEOFF:
+#if MODE_AUTOLAND_ENABLED
     case Mode::Number::AUTOLAND:
+#endif
 #if HAL_QUADPLANE_ENABLED
     case Mode::Number::QRTL:
     case Mode::Number::LOITER_ALT_QLAND:

diff --git a/ArduPlane/GCS_Plane.cpp b/ArduPlane/GCS_Plane.cpp
@@ -70,7 +70,9 @@ void GCS_Plane::update_vehicle_sensor_status_flags(void)
     case Mode::Number::GUIDED:
     case Mode::Number::CIRCLE:
     case Mode::Number::TAKEOFF:
+#if MODE_AUTOLAND_ENABLED
     case Mode::Number::AUTOLAND:
+#endif
 #if HAL_QUADPLANE_ENABLED
     case Mode::Number::QRTL:
     case Mode::Number::LOITER_ALT_QLAND:

diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp
@@ -1027,10 +1027,12 @@ const AP_Param::Info Plane::var_info[] = {
     // @Group: TKOFF_
     // @Path: mode_takeoff.cpp
     GOBJECT(mode_takeoff, "TKOFF_", ModeTakeoff),
-
+
+#if MODE_AUTOLAND_ENABLED    
     // @Group: AUTOLAND_
     // @Path: mode_autoland.cpp
     GOBJECT(mode_autoland, "AUTOLAND_", ModeAutoLand),
+#endif
 
 #if AP_PLANE_GLIDER_PULLUP_ENABLED
     // @Group: PUP_

diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
@@ -172,8 +172,9 @@ class Plane : public AP_Vehicle {
     friend class ModeTakeoff;
     friend class ModeThermal;
     friend class ModeLoiterAltQLand;
+#if MODE_AUTOLAND_ENABLED
     friend class ModeAutoLand;
-
+#endif
 #if AP_EXTERNAL_CONTROL_ENABLED
     friend class AP_ExternalControl_Plane;
 #endif
@@ -457,8 +458,10 @@ class Plane : public AP_Vehicle {
         uint32_t throttle_max_timer_ms;
         uint32_t level_off_start_time_ms;
         // Good candidate for keeping the initial time for TKOFF_THR_MAX_T.
+#if MODE_AUTOLAND_ENABLED
         int32_t takeoff_initial_direction; //deg
         bool takeoff_direction_initialized;
+#endif
     } takeoff_state;
 
     // ground steering controller state

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;
+#if MODE_AUTOLAND_ENABLED
     takeoff_state.takeoff_direction_initialized = false;
+#endif
     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 MODE_AUTOLAND_ENABLED
             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));
+            gcs().send_text(MAV_SEVERITY_INFO, "Autoland direction= %u",int(takeoff_state.takeoff_initial_direction));'
+#endif
         }
         if (auto_state.takeoff_speed_time_ms != 0 &&
             millis() - auto_state.takeoff_speed_time_ms >= 2000) {

diff --git a/ArduPlane/config.h b/ArduPlane/config.h
@@ -220,3 +220,9 @@
 #ifndef THR_CTRL_LAND_THRESH
  #define THR_CTRL_LAND_THRESH 0.7
 #endif
+
+////////////////////////////////////////////////////////////////////////////
+//  AUTOLAND mode
+#ifndef MODE_AUTOLAND_ENABLED
+ #define MODE_AUTOLAND_ENABLED 1
+#endif
diff --git a/ArduPlane/control_modes.cpp b/ArduPlane/control_modes.cpp
@@ -83,9 +83,11 @@ Mode *Plane::mode_from_mode_num(const enum Mode::Number num)
     case Mode::Number::TAKEOFF:
         ret = &mode_takeoff;
         break;
+#if MODE_AUTOLAND_ENABLED
     case Mode::Number::AUTOLAND:
         ret = &mode_autoland;
         break;
+#endif //MODE_AUTOLAND_ENABLED
     case Mode::Number::THERMAL:
 #if HAL_SOARING_ENABLED
         ret = &mode_thermal;

diff --git a/ArduPlane/events.cpp b/ArduPlane/events.cpp
@@ -66,7 +66,10 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype, ModeReason reaso
 #endif // HAL_QUADPLANE_ENABLED
 
     case Mode::Number::AUTO:
-    case Mode::Number::AUTOLAND: {
+#if MODE_AUTOLAND_ENABLED
+    case Mode::Number::AUTOLAND:
+#endif 
+        {
         if (failsafe_in_landing_sequence()) {
             // don't failsafe in a landing sequence
             break;
@@ -146,10 +149,12 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason
             set_mode(mode_fbwa, reason);
         } else if (g.fs_action_long == FS_ACTION_LONG_AUTO) {
             set_mode(mode_auto, reason);
+#if MODE_AUTOLAND_ENABLED
         } else if (g.fs_action_long == FS_ACTION_LONG_AUTOLAND) {
             if(!set_mode(mode_autoland, reason)) {
                set_mode(mode_rtl, reason);
             }
+#endif
         } else {
             set_mode(mode_rtl, reason);
         }
@@ -199,20 +204,23 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason
             set_mode(mode_fbwa, reason);
         } else if (g.fs_action_long == FS_ACTION_LONG_AUTO) {
             set_mode(mode_auto, reason);
+#if MODE_AUTOLAND_ENABLED
         } else if (g.fs_action_long == FS_ACTION_LONG_AUTOLAND) {
             if(!set_mode(mode_autoland, reason)) {
                set_mode(mode_rtl, reason);
             }            
         } else if (g.fs_action_long == FS_ACTION_LONG_RTL) {
             set_mode(mode_rtl, reason);
+#endif
         }
         break;
-
     case Mode::Number::RTL:
         if (g.fs_action_long == FS_ACTION_LONG_AUTO) {
             set_mode(mode_auto, reason);
+#if MODE_AUTOLAND_ENABLED
         } else if (g.fs_action_long == FS_ACTION_LONG_AUTOLAND) {
             set_mode(mode_autoland, reason);
+#endif
         }
         break;
 #if HAL_QUADPLANE_ENABLED
@@ -221,7 +229,9 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason
     case Mode::Number::LOITER_ALT_QLAND:
 #endif
     case Mode::Number::INITIALISING:
+#if MODE_AUTOLAND_ENABLED
     case Mode::Number::AUTOLAND:
+#endif
         break;
     }
     gcs().send_text(MAV_SEVERITY_WARNING, "%s Failsafe On: %s", (reason == ModeReason:: GCS_FAILSAFE) ? "GCS" : "RC Long", control_mode->name());

diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h
@@ -858,7 +858,7 @@ class ModeTakeoff: public Mode
     bool have_autoenabled_fences;
 
 };
-
+#if MODE_AUTOLAND_ENABLED
 class ModeAutoLand: public Mode
 {
 public:
@@ -892,7 +892,7 @@ class ModeAutoLand: public Mode
     AP_Mission::Mission_Command cmd;
     bool land_started;
 };
-
+#endif
 #if HAL_SOARING_ENABLED
 
 class ModeThermal: public Mode

diff --git a/ArduPlane/mode_autoland.cpp b/ArduPlane/mode_autoland.cpp
@@ -2,6 +2,8 @@
 #include "Plane.h"
 #include <GCS_MAVLink/GCS.h>
 
+#if MODE_AUTOLAND_ENABLED
+
 /*
   mode AutoLand parameters
  */
@@ -113,3 +115,4 @@ void ModeAutoLand::navigate()
        plane.verify_command(cmd);
     }
 }
+#endif
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.
+#if MODE_AUTOLAND_ENABLED
                 plane.takeoff_state.takeoff_direction_initialized = false;
+#endif
             }
         }
     }
@@ -143,7 +145,7 @@ void ModeTakeoff::update()
         plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL);
         takeoff_mode_setup = false;
     }
-    
+#if MODE_AUTOLAND_ENABLED   
     // set takeoff_initial_direction
     const float min_gps_speed = GPS_GND_CRS_MIN_SPD;
     if (!(plane.takeoff_state.takeoff_direction_initialized) && (plane.gps.ground_speed() > min_gps_speed)
@@ -152,7 +154,7 @@ void ModeTakeoff::update()
        plane.takeoff_state.takeoff_direction_initialized = true;
        gcs().send_text(MAV_SEVERITY_INFO, "Autoland direction= %u",int(plane.takeoff_state.takeoff_initial_direction));
     }
-
+#endif
     // We update the waypoint to follow once we're past TKOFF_LVL_ALT or we
     // pass the target location. The check for target location prevents us
     // flying towards a wrong location if we can't climb.

diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h
@@ -62,8 +62,9 @@ class QuadPlane
     friend class ModeQAutotune;
     friend class ModeQAcro;
     friend class ModeLoiterAltQLand;
+#if MODE_AUTOLAND_ENABLED
     friend class ModeAutoLand;
-    
+#endif
     QuadPlane(AP_AHRS &_ahrs);
 
     static QuadPlane *get_singleton() {

diff --git a/ArduPlane/takeoff.cpp b/ArduPlane/takeoff.cpp
@@ -23,10 +23,14 @@ bool Plane::auto_takeoff_check(void)
     }
 
     // Reset states if process has been interrupted, except takeoff_direction_initialized if set
+#if MODE_AUTOLAND_ENABLED
     bool takeoff_dir_initialized = takeoff_state.takeoff_direction_initialized;
+#endif
      if (takeoff_state.last_check_ms && (now - takeoff_state.last_check_ms) > 200) {
          memset(&takeoff_state, 0, sizeof(takeoff_state));
+#if MODE_AUTOLAND_ENABLED
          takeoff_state.takeoff_direction_initialized = takeoff_dir_initialized; //restore dir init state
+#endif
          return false;
      }
     takeoff_state.last_check_ms = now;