Merge branch 'master' into owpr-radiorcchannels3

ArduPilot · Mar 6, 2024 · 1899955 · 1899955
2 parents e2a895c + cbe5cf8
commit 1899955
Show file tree

Hide file tree

Showing 87 changed files with 1,466 additions and 185 deletions.
diff --git a/ArduCopter/AP_Arming.cpp b/ArduCopter/AP_Arming.cpp
@@ -231,12 +231,6 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure)
             return false;
         }
 
-        // Inverted flight feature disabled for Heli Single and Dual frames
-        if (copter.g2.frame_class.get() != AP_Motors::MOTOR_FRAME_HELI_QUAD &&
-            rc().find_channel_for_option(RC_Channel::AUX_FUNC::INVERTED) != nullptr) {
-            check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Inverted flight option not supported");
-            return false;
-        }
         // Ensure an Aux Channel is configured for motor interlock
         if (rc().find_channel_for_option(RC_Channel::AUX_FUNC::MOTOR_INTERLOCK) == nullptr) {
             check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Motor Interlock not configured");

diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h
@@ -580,9 +580,8 @@ class Copter : public AP_Vehicle {
     // Tradheli flags
     typedef struct {
         uint8_t dynamic_flight          : 1;    // 0   // true if we are moving at a significant speed (used to turn on/off leaky I terms)
-        uint8_t inverted_flight         : 1;    // 1   // true for inverted flight mode
-        uint8_t in_autorotation         : 1;    // 2   // true when heli is in autorotation
-        bool coll_stk_low                  ;    // 3   // true when collective stick is on lower limit
+        uint8_t in_autorotation         : 1;    // 1   // true when heli is in autorotation
+        bool coll_stk_low                  ;    // 2   // true when collective stick is on lower limit
     } heli_flags_t;
     heli_flags_t heli_flags;
 

diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp
@@ -544,7 +544,9 @@ static const ap_message STREAM_EXTRA3_msgs[] = {
     MSG_AHRS,
     MSG_SYSTEM_TIME,
     MSG_WIND,
+#if AP_RANGEFINDER_ENABLED
     MSG_RANGEFINDER,
+#endif
     MSG_DISTANCE_SENSOR,
 #if AP_TERRAIN_AVAILABLE
     MSG_TERRAIN,

diff --git a/ArduCopter/RC_Channel.cpp b/ArduCopter/RC_Channel.cpp
@@ -421,17 +421,17 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc
 #if FRAME_CONFIG == HELI_FRAME
             switch (ch_flag) {
             case AuxSwitchPos::HIGH:
-                copter.motors->set_inverted_flight(true);
-                copter.attitude_control->set_inverted_flight(true);
-                copter.heli_flags.inverted_flight = true;
+                if (copter.flightmode->allows_inverted()) {
+                    copter.attitude_control->set_inverted_flight(true);
+                } else {
+                    gcs().send_text(MAV_SEVERITY_WARNING, "Inverted flight not available in %s mode", copter.flightmode->name());
+                }
                 break;
             case AuxSwitchPos::MIDDLE:
                 // nothing
                 break;
             case AuxSwitchPos::LOW:
-                copter.motors->set_inverted_flight(false);
                 copter.attitude_control->set_inverted_flight(false);
-                copter.heli_flags.inverted_flight = false;
                 break;
             }
 #endif

diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp
@@ -457,6 +457,11 @@ void Copter::exit_mode(Mode *&old_flightmode,
             input_manager.set_stab_col_ramp(0.0);
         }
     }
+
+    // Make sure inverted flight is disabled if not supported in the new mode
+    if (!new_flightmode->allows_inverted()) {
+        attitude_control->set_inverted_flight(false);
+    }
 #endif //HELI_FRAME
 }
 

diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h
@@ -128,6 +128,10 @@ class Mode {
     virtual bool allows_autotune() const { return false; }
     virtual bool allows_flip() const { return false; }
 
+#if FRAME_CONFIG == HELI_FRAME
+    virtual bool allows_inverted() const { return false; };
+#endif
+
     // return a string for this flightmode
     virtual const char *name() const = 0;
     virtual const char *name4() const = 0;
@@ -1574,6 +1578,8 @@ class ModeStabilize_Heli : public ModeStabilize {
     bool init(bool ignore_checks) override;
     void run() override;
 
+    bool allows_inverted() const override { return true; };
+
 protected:
 
 private:

diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp
@@ -135,6 +135,9 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = {
 #if AP_LANDINGGEAR_ENABLED
     SCHED_TASK(landing_gear_update, 5, 50, 159),
 #endif
+#if AC_PRECLAND_ENABLED
+    SCHED_TASK(precland_update, 400, 50, 160),
+#endif
 };
 
 void Plane::get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
@@ -597,7 +600,8 @@ void Plane::update_alt()
                                                  get_takeoff_pitch_min_cd(),
                                                  throttle_nudge,
                                                  tecs_hgt_afe(),
-                                                 aerodynamic_load_factor);
+                                                 aerodynamic_load_factor,
+                                                 g.pitch_trim.get());
     }
 }
 
@@ -853,11 +857,22 @@ bool Plane::get_target_location(Location& target_loc)
  */
 bool Plane::update_target_location(const Location &old_loc, const Location &new_loc)
 {
-    if (!old_loc.same_loc_as(next_WP_loc)) {
+    /*
+      by checking the caller has provided the correct old target
+      location we prevent a race condition where the user changes mode
+      or commands a different target in the controlling lua script
+     */
+    if (!old_loc.same_loc_as(next_WP_loc) ||
+        old_loc.get_alt_frame() != new_loc.get_alt_frame()) {
         return false;
     }
     next_WP_loc = new_loc;
-    next_WP_loc.change_alt_frame(old_loc.get_alt_frame());
+
+#if HAL_QUADPLANE_ENABLED
+    if (control_mode == &mode_qland) {
+        mode_qland.last_target_loc_set_ms = AP_HAL::millis();
+    }
+#endif
 
     return true;
 }
@@ -879,7 +894,8 @@ bool Plane::set_velocity_match(const Vector2f &velocity)
 bool Plane::set_land_descent_rate(float descent_rate)
 {
 #if HAL_QUADPLANE_ENABLED
-    if (quadplane.in_vtol_land_descent()) {
+    if (quadplane.in_vtol_land_descent() ||
+        control_mode == &mode_qland) {
         quadplane.poscontrol.override_descent_rate = descent_rate;
         quadplane.poscontrol.last_override_descent_ms = AP_HAL::millis();
         return true;
@@ -946,4 +962,13 @@ bool Plane::flight_option_enabled(FlightOptions flight_option) const
     return g2.flight_options & flight_option;
 }
 
+#if AC_PRECLAND_ENABLED
+void Plane::precland_update(void)
+{
+    // alt will be unused if we pass false through as the second parameter:
+    return g2.precland.update(rangefinder_state.height_estimate*100,
+                              rangefinder_state.in_range && rangefinder_state.in_use);
+}
+#endif
+
 AP_HAL_MAIN_CALLBACKS(&plane);
diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp
@@ -675,7 +675,9 @@ static const ap_message STREAM_EXTRA2_msgs[] = {
 static const ap_message STREAM_EXTRA3_msgs[] = {
     MSG_AHRS,
     MSG_WIND,
+#if AP_RANGEFINDER_ENABLED
     MSG_RANGEFINDER,
+#endif
     MSG_DISTANCE_SENSOR,
     MSG_SYSTEM_TIME,
 #if AP_TERRAIN_AVAILABLE
@@ -746,6 +748,16 @@ void GCS_MAVLINK_Plane::handle_change_alt_request(AP_Mission::Mission_Command &c
 }
 
 
+/*
+  handle a LANDING_TARGET command. The timestamp has been jitter corrected
+*/
+void GCS_MAVLINK_Plane::handle_landing_target(const mavlink_landing_target_t &packet, uint32_t timestamp_ms)
+{
+#if AC_PRECLAND_ENABLED
+    plane.g2.precland.handle_msg(packet, timestamp_ms);
+#endif
+}
+
 MAV_RESULT GCS_MAVLINK_Plane::handle_command_preflight_calibration(const mavlink_command_int_t &packet, const mavlink_message_t &msg)
 {
     plane.in_calibration = true;

diff --git a/ArduPlane/GCS_Mavlink.h b/ArduPlane/GCS_Mavlink.h
@@ -50,6 +50,7 @@ class GCS_MAVLINK_Plane : public GCS_MAVLINK
     void send_pid_tuning() override;
 
     void handle_manual_control_axes(const mavlink_manual_control_t &packet, const uint32_t tnow) override;
+    void handle_landing_target(const mavlink_landing_target_t &packet, uint32_t timestamp_ms) override;
 
 private:
 

diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp
@@ -1242,7 +1242,13 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @Bitmask: 0:Roll,1:Pitch,2:Yaw
     // @User: Standard
     AP_GROUPINFO("AUTOTUNE_AXES", 34, ParametersG2, axis_bitmask, 7),
-
+
+#if AC_PRECLAND_ENABLED
+    // @Group: PLND_
+    // @Path: ../libraries/AC_PrecLand/AC_PrecLand.cpp
+    AP_SUBGROUPINFO(precland, "PLND_", 35, ParametersG2, AC_PrecLand),
+#endif
+
     AP_GROUPEND
 };
 

diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h
@@ -520,6 +520,10 @@ class ParametersG2 {
     AP_LandingGear landing_gear;
 #endif
 
+#if AC_PRECLAND_ENABLED
+    AC_PrecLand precland;
+#endif
+
     // crow flaps weighting
     AP_Int8 crow_flap_weight_outer;
     AP_Int8 crow_flap_weight_inner;

diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
@@ -87,6 +87,11 @@
 #include "AP_ExternalControl_Plane.h"
 #endif
 
+#include <AC_PrecLand/AC_PrecLand_config.h>
+#if AC_PRECLAND_ENABLED
+ # include <AC_PrecLand/AC_PrecLand.h>
+#endif
+
 #include "GCS_Mavlink.h"
 #include "GCS_Plane.h"
 #include "quadplane.h"
@@ -251,6 +256,10 @@ class Plane : public AP_Vehicle {
     AP_Rally rally;
 #endif
 
+#if AC_PRECLAND_ENABLED
+    void precland_update(void);
+#endif
+
     // returns a Location for a rally point or home; if
     // HAL_RALLY_ENABLED is false, just home.
     Location calc_best_rally_or_home_location(const Location &current_loc, float rtl_home_alt_amsl_cm) const;

diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h
@@ -400,7 +400,7 @@ class ModeManual : public Mode
     void run() override;
 
     // true if throttle min/max limits should be applied
-    bool use_throttle_limits() const override { return false; }
+    bool use_throttle_limits() const override;
 
     // true if voltage correction should be applied to throttle
     bool use_battery_compensation() const override { return false; }
@@ -669,6 +669,7 @@ friend class ModeQLand;
 class ModeQLand : public Mode
 {
 public:
+    friend class Plane;
 
     Number mode_number() const override { return Number::QLAND; }
     const char *name() const override { return "QLAND"; }
@@ -686,6 +687,7 @@ class ModeQLand : public Mode
     bool _enter() override;
     bool _pre_arm_checks(size_t buflen, char *buffer) const override { return false; }
 
+    uint32_t last_target_loc_set_ms;
 };
 
 class ModeQRTL : public Mode

diff --git a/ArduPlane/mode_manual.cpp b/ArduPlane/mode_manual.cpp
@@ -6,22 +6,8 @@ void ModeManual::update()
     SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, plane.roll_in_expo(false));
     SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, plane.pitch_in_expo(false));
     output_rudder_and_steering(plane.rudder_in_expo(false));
-    float throttle = plane.get_throttle_input(true);
 
-
-#if HAL_QUADPLANE_ENABLED
-    if (quadplane.available() && quadplane.option_is_set(QuadPlane::OPTION::IDLE_GOV_MANUAL)) {
-        // for quadplanes it can be useful to run the idle governor in MANUAL mode
-        // as it prevents the VTOL motors from running
-        int8_t min_throttle = plane.aparm.throttle_min.get();
-
-        // apply idle governor
-#if AP_ICENGINE_ENABLED
-        plane.g2.ice_control.update_idle_governor(min_throttle);
-#endif
-        throttle = MAX(throttle, min_throttle);
-    }
-#endif
+    const float throttle = plane.get_throttle_input(true);
     SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, throttle);
 
     plane.nav_roll_cd = ahrs.roll_sensor;
@@ -32,3 +18,14 @@ void ModeManual::run()
 {
     reset_controllers();
 }
+
+// true if throttle min/max limits should be applied
+bool ModeManual::use_throttle_limits() const
+{
+#if HAL_QUADPLANE_ENABLED
+    if (quadplane.available() && quadplane.option_is_set(QuadPlane::OPTION::IDLE_GOV_MANUAL)) {
+        return true;
+    }
+#endif
+    return false;
+}
diff --git a/ArduPlane/mode_qland.cpp b/ArduPlane/mode_qland.cpp
@@ -19,6 +19,10 @@ bool ModeQLand::_enter()
 #if AP_FENCE_ENABLED
     plane.fence.auto_disable_fence_for_landing();
 #endif
+
+    // clear precland timestamp
+    last_target_loc_set_ms = 0;
+
     return true;
 }
 
@@ -29,6 +33,33 @@ void ModeQLand::update()
 
 void ModeQLand::run()
 {
+    /*
+      see if precision landing is active with an override of the
+      target location
+     */
+    const uint32_t last_pos_set_ms = last_target_loc_set_ms;
+    const uint32_t last_vel_set_ms = quadplane.poscontrol.last_velocity_match_ms;
+    const uint32_t now_ms = AP_HAL::millis();
+
+    if (last_pos_set_ms != 0 && now_ms - last_pos_set_ms < 500) {
+        // we have an active landing target override
+        Vector2f rel_origin;
+        if (plane.next_WP_loc.get_vector_xy_from_origin_NE(rel_origin)) {
+            quadplane.pos_control->set_pos_target_xy_cm(rel_origin.x, rel_origin.y);
+        }
+    }
+
+    // allow for velocity override as well
+    if (last_vel_set_ms != 0 && now_ms - last_vel_set_ms < 500) {
+        // we have an active landing velocity override
+        Vector2f target_accel;
+        Vector2f target_speed_xy_cms{quadplane.poscontrol.velocity_match.x*100, quadplane.poscontrol.velocity_match.y*100};
+        quadplane.pos_control->input_vel_accel_xy(target_speed_xy_cms, target_accel);
+    }
+
+    /*
+      use QLOITER to do the main control
+     */
     plane.mode_qloiter.run();
 }
 

diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp
@@ -1761,10 +1761,8 @@ void SLT_Transition::update()
     }
 
     case TRANSITION_DONE:
-        if (!quadplane.tiltrotor.motors_active()) {
-            quadplane.set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
-            motors->output();
-        }
+        quadplane.set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
+        motors->output();
         set_last_fw_pitch();
         in_forced_transition = false;
         return;
@@ -1851,7 +1849,7 @@ void QuadPlane::update(void)
             plane.control_mode == &plane.mode_acro ||
             plane.control_mode == &plane.mode_training) {
             // in manual modes quad motors are always off
-            if (!tiltrotor.motors_active() && !tailsitter.enabled()) {
+            if (!tailsitter.enabled()) {
                 set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
                 motors->output();
             }

diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp
@@ -49,16 +49,9 @@ void Plane::set_control_channels(void)
     quadplane.rc_fwd_thr_ch = rc().find_channel_for_option(RC_Channel::AUX_FUNC::FWD_THR);
 #endif
 
-    bool set_throttle_esc_scaling = true;
-#if HAL_QUADPLANE_ENABLED
-    set_throttle_esc_scaling = !quadplane.enable;
-#endif
-    if (set_throttle_esc_scaling) {
-        // setup correct scaling for ESCs like the UAVCAN ESCs which
-        // take a proportion of speed. For quadplanes we use AP_Motors
-        // scaling
-        g2.servo_channels.set_esc_scaling_for(SRV_Channel::k_throttle);
-    }
+    // setup correct scaling for ESCs like the UAVCAN ESCs which
+    // take a proportion of speed.
+    g2.servo_channels.set_esc_scaling_for(SRV_Channel::k_throttle);
 }
 
 /*