Merge branch 'master' into ardupilot_open_source

.github/workflows/test_size.yml

@@ -72,6 +72,7 @@ jobs:
             MatekF405,
             KakuteF7,
             MatekH743-bdshot,
+            MambaH743v4,  # for littlefs support
             Pixhawk1-1M,
             MatekF405-CAN,  # see special "build bootloader" code below
             DrotekP3Pro,  # see special "build bootloader" code below

.gitmodules

@@ -42,3 +42,6 @@
 [submodule "modules/lwip"]
 	path = modules/lwip
 	url = https://github.com/ArduPilot/lwip.git
+[submodule "modules/littlefs"]
+	path = modules/littlefs
+	url = https://github.com/ArduPilot/littlefs.git

ArduCopter/AP_Arming.cpp

@@ -267,8 +267,8 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure)
                     return false;
                 }
                 // check if RTL_ALT is higher than rangefinder's max range
-                if (copter.g.rtl_altitude > copter.rangefinder.max_distance_cm_orient(ROTATION_PITCH_270)) {
-                    check_failed(ARMING_CHECK_PARAMETERS, display_failure, failure_template, "RTL_ALT>RNGFND_MAX_CM");
+                if (copter.g.rtl_altitude > copter.rangefinder.max_distance_orient(ROTATION_PITCH_270) * 100) {
+                    check_failed(ARMING_CHECK_PARAMETERS, display_failure, failure_template, "RTL_ALT (in cm) above RNGFND_MAX (in metres)");
                     return false;
                 }
 #else

ArduCopter/heli.cpp

@@ -46,7 +46,7 @@ void Copter::check_dynamic_flight(void)
     if (!moving && rangefinder_state.enabled && rangefinder.status_orient(ROTATION_PITCH_270) == RangeFinder::Status::Good) {
         // when we are more than 2m from the ground with good
         // rangefinder lock consider it to be dynamic flight
-        moving = (rangefinder.distance_cm_orient(ROTATION_PITCH_270) > 200);
+        moving = (rangefinder.distance_orient(ROTATION_PITCH_270) > 2);
     }
 #endif
 

ArduCopter/mode_guided.cpp

@@ -139,7 +139,7 @@ bool ModeGuided::do_user_takeoff_start(float takeoff_alt_cm)
 #if AP_RANGEFINDER_ENABLED
     if (wp_nav->rangefinder_used_and_healthy() &&
         wp_nav->get_terrain_source() == AC_WPNav::TerrainSource::TERRAIN_FROM_RANGEFINDER &&
-        takeoff_alt_cm < copter.rangefinder.max_distance_cm_orient(ROTATION_PITCH_270)) {
+        takeoff_alt_cm < copter.rangefinder.max_distance_orient(ROTATION_PITCH_270)*100) {
         // can't takeoff downwards
         if (takeoff_alt_cm <= copter.rangefinder_state.alt_cm) {
             return false;

ArduPlane/AP_Arming.cpp

@@ -321,6 +321,10 @@ bool AP_Arming_Plane::arm(const AP_Arming::Method method, const bool do_arming_c
     // rising edge of delay_arming oneshot
     delay_arming = true;
 
+#if MODE_AUTOLAND_ENABLED
+    plane.mode_autoland.arm_check();
+#endif
+
     send_arm_disarm_statustext("Throttle armed");
 
     return true;

ArduPlane/Plane.cpp

@@ -514,6 +514,10 @@ void Plane::update_control_mode(void)
     update_fly_forward();
 
     control_mode->update();
+
+#if MODE_AUTOLAND_ENABLED
+    mode_autoland.check_takeoff_direction();
+#endif
 }
 
 

ArduPlane/Plane.h

@@ -1333,6 +1333,11 @@ class Plane : public AP_Vehicle {
 
 #endif // AP_SCRIPTING_ENABLED
 
+    bool tkoff_option_is_set(AP_FixedWing::TakeoffOption option) const {
+        return (aparm.takeoff_options & int32_t(option)) != 0;
+    }
+
+
 };
 
 extern Plane plane;

ArduPlane/altitude.cpp

@@ -719,10 +719,10 @@ void Plane::rangefinder_height_update(void)
         // to misconfiguration or a faulty sensor
         if (rangefinder_state.in_range_count < 10) {
             if (!is_equal(distance, rangefinder_state.last_distance) &&
-                fabsf(rangefinder_state.initial_range - distance) > 0.05f * rangefinder.max_distance_cm_orient(rangefinder_orientation())*0.01f) {
+                fabsf(rangefinder_state.initial_range - distance) > 0.05f * rangefinder.max_distance_orient(rangefinder_orientation())) {
                 rangefinder_state.in_range_count++;
             }
-            if (fabsf(rangefinder_state.last_distance - distance) > rangefinder.max_distance_cm_orient(rangefinder_orientation())*0.01*0.2) {
+            if (fabsf(rangefinder_state.last_distance - distance) > rangefinder.max_distance_orient(rangefinder_orientation())*0.2) {
                 // changes by more than 20% of full range will reset counter
                 rangefinder_state.in_range_count = 0;
             }

ArduPlane/commands_logic.cpp

@@ -26,15 +26,18 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
         // reset loiter start time. New command is a new loiter
         loiter.start_time_ms = 0;
 
-        AP_Mission::Mission_Command next_nav_cmd;
-        const uint16_t next_index = mission.get_current_nav_index() + 1;
-        const bool have_next_cmd = mission.get_next_nav_cmd(next_index, next_nav_cmd);
-        auto_state.wp_is_land_approach = have_next_cmd && (next_nav_cmd.id == MAV_CMD_NAV_LAND);
+        // Mission lookahead is only valid in auto
+        if (control_mode == &mode_auto) {
+            AP_Mission::Mission_Command next_nav_cmd;
+            const uint16_t next_index = mission.get_current_nav_index() + 1;
+            const bool have_next_cmd = mission.get_next_nav_cmd(next_index, next_nav_cmd);
+            auto_state.wp_is_land_approach = have_next_cmd && (next_nav_cmd.id == MAV_CMD_NAV_LAND);
 #if HAL_QUADPLANE_ENABLED
-        if (have_next_cmd && quadplane.is_vtol_land(next_nav_cmd.id)) {
-            auto_state.wp_is_land_approach = false;
-        }
+            if (have_next_cmd && quadplane.is_vtol_land(next_nav_cmd.id)) {
+                auto_state.wp_is_land_approach = false;
+            }
 #endif
+        }
     }
 
     switch(cmd.id) {
@@ -382,9 +385,6 @@ 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.initial_direction.initialized = false;
-#endif
     auto_state.baro_takeoff_alt = barometer.get_altitude();
 }
 
@@ -559,11 +559,6 @@ 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
-            plane.takeoff_state.initial_direction.heading = wrap_360(plane.gps.ground_course() + plane.mode_autoland.landing_dir_off);
-            takeoff_state.initial_direction.initialized = true;
-            gcs().send_text(MAV_SEVERITY_INFO, "Autoland direction= %u",int(takeoff_state.initial_direction.heading));
-#endif
         }
         if (auto_state.takeoff_speed_time_ms != 0 &&
             millis() - auto_state.takeoff_speed_time_ms >= 2000) {
@@ -1173,6 +1168,13 @@ bool Plane::verify_loiter_heading(bool init)
     }
 #endif
 
+#if MODE_AUTOLAND_ENABLED
+    if (control_mode == &mode_autoland) {
+        // autoland mode has its own lineup criterion
+        return mode_autoland.landing_lined_up();
+    }
+#endif
+
     //Get the lat/lon of next Nav waypoint after this one:
     AP_Mission::Mission_Command next_nav_cmd;
     if (! mission.get_next_nav_cmd(mission.get_current_nav_index() + 1,

ArduPlane/mode.h

@@ -156,6 +156,11 @@ class Mode
 
     // true if voltage correction should be applied to throttle
     virtual bool use_battery_compensation() const;
+
+#if MODE_AUTOLAND_ENABLED   
+    // true if mode allows landing direction to be set on first takeoff after arm in this mode 
+    virtual bool allows_autoland_direction_capture() const { return false; }
+#endif
 
 #if AP_QUICKTUNE_ENABLED
     // does this mode support VTOL quicktune?
@@ -212,6 +217,11 @@ friend class ModeQAcro;
 
     void stabilize_quaternion();
 
+#if MODE_AUTOLAND_ENABLED   
+    // true if mode allows landing direction to be set on first takeoff after arm in this mode 
+    bool allows_autoland_direction_capture() const override { return true; }
+#endif
+
 protected:
 
     // ACRO controller state
@@ -262,6 +272,11 @@ class ModeAuto : public Mode
 
     void run() override;
 
+#if MODE_AUTOLAND_ENABLED   
+    // true if mode allows landing direction to be set on first takeoff after arm in this mode 
+    bool allows_autoland_direction_capture() const override { return true; }
+#endif
+
 #if AP_PLANE_GLIDER_PULLUP_ENABLED
     bool in_pullup() const { return pullup.in_pullup(); }
 #endif
@@ -308,6 +323,11 @@ class ModeAutoTune : public Mode
 
     void run() override;
 
+#if MODE_AUTOLAND_ENABLED   
+    // true if mode allows landing direction to be set on first takeoff after arm in this mode 
+    bool allows_autoland_direction_capture() const override { return true; }
+#endif
+
 protected:
 
     bool _enter() override;
@@ -449,6 +469,11 @@ class ModeManual : public Mode
     // true if voltage correction should be applied to throttle
     bool use_battery_compensation() const override { return false; }
 
+#if MODE_AUTOLAND_ENABLED   
+    // true if mode allows landing direction to be set on first takeoff after arm in this mode 
+    bool allows_autoland_direction_capture() const override { return true; }
+#endif
+
 };
 
 
@@ -495,6 +520,11 @@ class ModeStabilize : public Mode
 
     void run() override;
 
+#if MODE_AUTOLAND_ENABLED   
+    // true if mode allows landing direction to be set on first takeoff after arm in this mode 
+    bool allows_autoland_direction_capture() const override { return true; }
+#endif
+
 private:
     void stabilize_stick_mixing_direct();
 
@@ -513,6 +543,10 @@ class ModeTraining : public Mode
 
     void run() override;
 
+#if MODE_AUTOLAND_ENABLED   
+    // true if mode allows landing direction to be set on first takeoff after arm in this mode 
+    bool allows_autoland_direction_capture() const override { return true; }
+#endif
 };
 
 class ModeInitializing : public Mode
@@ -552,6 +586,11 @@ class ModeFBWA : public Mode
 
     void run() override;
 
+#if MODE_AUTOLAND_ENABLED   
+    // true if mode allows landing direction to be set on first takeoff after arm in this mode 
+    bool allows_autoland_direction_capture() const override { return true; }
+#endif
+
 };
 
 class ModeFBWB : public Mode
@@ -844,6 +883,11 @@ class ModeTakeoff: public Mode
 
     bool does_auto_throttle() const override { return true; }
 
+#if MODE_AUTOLAND_ENABLED   
+    // true if mode allows landing direction to be set on first takeoff after arm in this mode 
+    bool allows_autoland_direction_capture() const override { return true; }
+#endif
+
     // var_info for holding parameter information
     static const struct AP_Param::GroupInfo var_info[];
 
@@ -887,18 +931,43 @@ class ModeAutoLand: public Mode
 
     bool does_auto_throttle() const override { return true; }
 
+    void check_takeoff_direction(void);
+
+    // return true when lined up correctly from the LOITER_TO_ALT
+    bool landing_lined_up(void);
+
+    // see if we should capture the direction
+    void arm_check(void);
 
     // var_info for holding parameter information
     static const struct AP_Param::GroupInfo var_info[];
 
     AP_Int16 final_wp_alt;
     AP_Int16 final_wp_dist;
     AP_Int16 landing_dir_off;
+    AP_Int8  options;
+
+    // Bitfields of AUTOLAND_OPTIONS
+    enum class AutoLandOption {
+        AUTOLAND_DIR_ON_ARM     = (1U << 0), // set dir for autoland on arm if compass in use.
+    };
+
+    enum class AutoLandStage {
+        LOITER,
+        LANDING
+    };
+
+    bool autoland_option_is_set(AutoLandOption option) const {
+        return (options & int8_t(option)) != 0;
+    }
 
 protected:
     bool _enter() override;
-    AP_Mission::Mission_Command cmd[3];
-    uint8_t stage;
+    AP_Mission::Mission_Command cmd_loiter;
+    AP_Mission::Mission_Command cmd_land;
+    Location land_start;
+    AutoLandStage stage;
+    void set_autoland_direction(const float heading);
 };
 #endif
 #if HAL_SOARING_ENABLED