Merge branch 'master' into hal_esp32_migration_idf_v5.3_wip

AntennaTracker/Log.cpp

@@ -7,9 +7,7 @@
 // Write an attitude packet
 void Tracker::Log_Write_Attitude()
 {
-    Vector3f targets;
-    targets.y = nav_status.pitch * 100.0f;
-    targets.z = wrap_360_cd(nav_status.bearing * 100.0f);
+    const Vector3f targets{0.0f, nav_status.pitch, nav_status.bearing};
     ahrs.Write_Attitude(targets);
     AP::ahrs().Log_Write();
 }

ArduCopter/Log.cpp

@@ -75,11 +75,9 @@ void Copter::Log_Write_Control_Tuning()
 // Write an attitude packet
 void Copter::Log_Write_Attitude()
 {
-    Vector3f targets = attitude_control->get_att_target_euler_cd();
-    targets.z = wrap_360_cd(targets.z);
-    ahrs.Write_Attitude(targets);
-    ahrs_view->Write_Rate(*motors, *attitude_control, *pos_control);
- }
+    ahrs.Write_Attitude(attitude_control->get_att_target_euler_rad() * RAD_TO_DEG);
+    attitude_control->Write_Rate(*pos_control);
+}
 
 // Write PIDS packets
 void Copter::Log_Write_PIDS()

ArduCopter/mode.h

@@ -15,7 +15,7 @@ class _AutoTakeoff {
 public:
     void run();
     void start(float complete_alt_cm, bool terrain_alt);
-    bool get_position(Vector3p& completion_pos);
+    bool get_completion_pos(Vector3p& pos_neu_cm);
 
     bool complete;          // true when takeoff is complete
 

ArduCopter/mode_auto.cpp

@@ -427,7 +427,7 @@ bool ModeAuto::wp_start(const Location& dest_loc)
         Vector3f stopping_point;
         if (_mode == SubMode::TAKEOFF) {
             Vector3p takeoff_complete_pos;
-            if (auto_takeoff.get_position(takeoff_complete_pos)) {
+            if (auto_takeoff.get_completion_pos(takeoff_complete_pos)) {
                 stopping_point = takeoff_complete_pos.tofloat();
             }
         }

ArduCopter/radio.cpp

@@ -20,10 +20,11 @@ void Copter::default_dead_zones()
 
 void Copter::init_rc_in()
 {
-    channel_roll     = rc().channel(rcmap.roll()-1);
-    channel_pitch    = rc().channel(rcmap.pitch()-1);
-    channel_throttle = rc().channel(rcmap.throttle()-1);
-    channel_yaw      = rc().channel(rcmap.yaw()-1);
+    // the library gaurantees that these are non-nullptr:
+    channel_roll     = &rc().get_roll_channel();
+    channel_pitch    = &rc().get_pitch_channel();
+    channel_throttle = &rc().get_throttle_channel();
+    channel_yaw      = &rc().get_yaw_channel();
 
     // set rc channel ranges
     channel_roll->set_angle(ROLL_PITCH_YAW_INPUT_MAX);

ArduCopter/takeoff.cpp

@@ -239,15 +239,15 @@ void _AutoTakeoff::start(float _complete_alt_cm, bool _terrain_alt)
     }
 }
 
-// return takeoff final position if takeoff has completed successfully
-bool _AutoTakeoff::get_position(Vector3p& _complete_pos)
+// return takeoff final target position in cm from the EKF origin if takeoff has completed successfully
+bool _AutoTakeoff::get_completion_pos(Vector3p& pos_neu_cm)
 {
     // only provide location if takeoff has completed
     if (!complete) {
         return false;
     }
 
-    complete_pos = _complete_pos;
+    pos_neu_cm = complete_pos;
     return true;
 }
 

ArduPlane/ArduPlane.cpp

@@ -216,6 +216,17 @@ void Plane::update_speed_height(void)
         should_run_tecs = false;
     }
 #endif
+
+    if (auto_state.idle_mode) {
+        should_run_tecs = false;
+    }
+
+#if AP_PLANE_GLIDER_PULLUP_ENABLED
+    if (mode_auto.in_pullup()) {
+        should_run_tecs = false;
+    }
+#endif
+
     if (should_run_tecs) {
 	    // Call TECS 50Hz update. Note that we call this regardless of
 	    // throttle suppressed, as this needs to be running for
@@ -512,6 +523,12 @@ void Plane::update_fly_forward(void)
     }
 #endif
 
+    if (auto_state.idle_mode) {
+        // don't fuse airspeed when in balloon lift
+        ahrs.set_fly_forward(false);
+        return;
+    }
+
     if (flight_stage == AP_FixedWing::FlightStage::LAND) {
         ahrs.set_fly_forward(landing.is_flying_forward());
         return;
@@ -578,6 +595,16 @@ void Plane::update_alt()
         should_run_tecs = false;
     }
 #endif
+
+    if (auto_state.idle_mode) {
+        should_run_tecs = false;
+    }
+
+#if AP_PLANE_GLIDER_PULLUP_ENABLED
+    if (mode_auto.in_pullup()) {
+        should_run_tecs = false;
+    }
+#endif
 
     if (should_run_tecs && !throttle_suppressed) {
 

ArduPlane/Log.cpp

@@ -5,10 +5,11 @@
 // Write an attitude packet
 void Plane::Log_Write_Attitude(void)
 {
-    Vector3f targets;       // Package up the targets into a vector for commonality with Copter usage of Log_Wrote_Attitude
-    targets.x = nav_roll_cd;
-    targets.y = nav_pitch_cd;
-    targets.z = 0; //Plane does not have the concept of navyaw. This is a placeholder.
+    Vector3f targets {       // Package up the targets into a vector for commonality with Copter usage of Log_Wrote_Attitude
+        nav_roll_cd * 0.01f,
+        nav_pitch_cd * 0.01f,
+        0 //Plane does not have the concept of navyaw. This is a placeholder.
+    };
 
 #if HAL_QUADPLANE_ENABLED
     if (quadplane.show_vtol_view()) {
@@ -18,8 +19,7 @@ void Plane::Log_Write_Attitude(void)
         // since Euler angles are not used and it is a waste of cpu to compute them at the loop rate.
         // Get them from the quaternion instead:
         quadplane.attitude_control->get_attitude_target_quat().to_euler(targets.x, targets.y, targets.z);
-        targets *= degrees(100.0f);
-        quadplane.ahrs_view->Write_AttitudeView(targets);
+        quadplane.ahrs_view->Write_AttitudeView(targets * RAD_TO_DEG);
     } else
 #endif
             {

ArduPlane/Parameters.cpp

@@ -1019,6 +1019,12 @@ const AP_Param::Info Plane::var_info[] = {
     // @Path: mode_takeoff.cpp
     GOBJECT(mode_takeoff, "TKOFF_", ModeTakeoff),
 
+#if AP_PLANE_GLIDER_PULLUP_ENABLED
+    // @Group: PUP_
+    // @Path: pullup.cpp
+    GOBJECTN(mode_auto.pullup, pullup, "PUP_", GliderPullup),
+#endif
+
     // @Group:
     // @Path: ../libraries/AP_Vehicle/AP_Vehicle.cpp
     PARAM_VEHICLE_INFO,

ArduPlane/Parameters.h

@@ -359,6 +359,8 @@ class Parameters {
         k_param_autotune_options,
         k_param_takeoff_throttle_min,
         k_param_takeoff_options,
+
+        k_param_pullup = 270,
     };
 
     AP_Int16 format_version;

ArduPlane/Plane.h

@@ -121,6 +121,7 @@
 #include "avoidance_adsb.h"
 #endif
 #include "AP_Arming.h"
+#include "pullup.h"
 
 /*
   main APM:Plane class
@@ -175,6 +176,9 @@ class Plane : public AP_Vehicle {
 #if AP_EXTERNAL_CONTROL_ENABLED
     friend class AP_ExternalControl_Plane;
 #endif
+#if AP_PLANE_GLIDER_PULLUP_ENABLED
+    friend class GliderPullup;
+#endif
 
     Plane(void);
 
@@ -515,6 +519,11 @@ class Plane : public AP_Vehicle {
         // have we checked for an auto-land?
         bool checked_for_autoland;
 
+        // Altitude threshold to complete a takeoff command in autonomous modes.  Centimeters
+        // are we in idle mode? used for balloon launch to stop servo
+        // movement until altitude is reached
+        bool idle_mode;
+
         // are we in VTOL mode in AUTO?
         bool vtol_mode;
 
@@ -959,6 +968,7 @@ class Plane : public AP_Vehicle {
     void do_loiter_turns(const AP_Mission::Mission_Command& cmd);
     void do_loiter_time(const AP_Mission::Mission_Command& cmd);
     void do_continue_and_change_alt(const AP_Mission::Mission_Command& cmd);
+    void do_altitude_wait(const AP_Mission::Mission_Command& cmd);
     void do_loiter_to_alt(const AP_Mission::Mission_Command& cmd);
     void do_vtol_takeoff(const AP_Mission::Mission_Command& cmd);
     void do_vtol_land(const AP_Mission::Mission_Command& cmd);

ArduPlane/commands_logic.cpp

@@ -27,6 +27,9 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
 
         nav_controller->set_data_is_stale();
 
+        // start non-idle
+        auto_state.idle_mode = false;
+
         // reset loiter start time. New command is a new loiter
         loiter.start_time_ms = 0;
 
@@ -92,6 +95,7 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
         break;
 
     case MAV_CMD_NAV_ALTITUDE_WAIT:
+        do_altitude_wait(cmd);
         break;
 
 #if HAL_QUADPLANE_ENABLED
@@ -501,6 +505,15 @@ void Plane::do_continue_and_change_alt(const AP_Mission::Mission_Command& cmd)
     reset_offset_altitude();
 }
 
+void Plane::do_altitude_wait(const AP_Mission::Mission_Command& cmd)
+{
+    // set all servos to trim until we reach altitude or descent speed
+    auto_state.idle_mode = true;
+#if AP_PLANE_GLIDER_PULLUP_ENABLED
+    mode_auto.pullup.reset();
+#endif
+}
+
 void Plane::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd)
 {
     //set target alt  
@@ -831,17 +844,46 @@ bool Plane::verify_continue_and_change_alt()
  */
 bool ModeAuto::verify_altitude_wait(const AP_Mission::Mission_Command &cmd)
 {
-    if (plane.current_loc.alt > cmd.content.altitude_wait.altitude*100.0f) {
-        gcs().send_text(MAV_SEVERITY_INFO,"Reached altitude");
-        return true;
+#if AP_PLANE_GLIDER_PULLUP_ENABLED
+    if (pullup.in_pullup()) {
+        return pullup.verify_pullup();
     }
-    if (plane.auto_state.sink_rate > cmd.content.altitude_wait.descent_rate) {
+#endif
+
+    /*
+      the target altitude in param1 is always AMSL
+     */
+    const float alt_diff = plane.current_loc.alt*0.01 - cmd.content.altitude_wait.altitude;
+    bool completed = false;
+    if (alt_diff > 0) {
+        gcs().send_text(MAV_SEVERITY_INFO,"Reached altitude");
+        completed = true;
+    } else if (cmd.content.altitude_wait.descent_rate > 0 &&
+        plane.auto_state.sink_rate > cmd.content.altitude_wait.descent_rate) {
         gcs().send_text(MAV_SEVERITY_INFO, "Reached descent rate %.1f m/s", (double)plane.auto_state.sink_rate);
-        return true;        
+        completed = true;
     }
 
-    // if requested, wiggle servos
-    if (cmd.content.altitude_wait.wiggle_time != 0) {
+    if (completed) {
+#if AP_PLANE_GLIDER_PULLUP_ENABLED
+        if (pullup.pullup_start()) {
+            // we are doing a pullup, ALTITUDE_WAIT not complete until pullup is done
+            return false;
+        }
+#endif
+        return true;
+    }
+
+    const float time_to_alt = alt_diff / MIN(plane.auto_state.sink_rate, -0.01);
+
+    /*
+      if requested, wiggle servos
+
+      we don't start a wiggle if we expect to release soon as we don't
+      want the servos to be off trim at the time of release
+    */
+    if (cmd.content.altitude_wait.wiggle_time != 0 &&
+        (plane.auto_state.sink_rate > 0 || time_to_alt > cmd.content.altitude_wait.wiggle_time*5)) {
         if (wiggle.stage == 0 &&
             AP_HAL::millis() - wiggle.last_ms > cmd.content.altitude_wait.wiggle_time*1000) {
             wiggle.stage = 1;
@@ -1291,3 +1333,4 @@ bool Plane::in_auto_mission_id(uint16_t command) const
 {
     return control_mode == &mode_auto && mission.get_current_nav_id() == command;
 }
+

ArduPlane/mode.cpp

@@ -101,6 +101,9 @@ bool Mode::enter()
     plane.target_altitude.terrain_following_pending = false;
 #endif
 
+    // disable auto mode servo idle during altitude wait command
+    plane.auto_state.idle_mode = false;
+
     bool enter_result = _enter();
 
     if (enter_result) {

ArduPlane/mode.h

@@ -9,6 +9,7 @@
 #include "quadplane.h"
 #include <AP_AHRS/AP_AHRS.h>
 #include <AP_Mission/AP_Mission.h>
+#include "pullup.h"
 
 class AC_PosControl;
 class AC_AttitudeControl_Multi;
@@ -208,6 +209,7 @@ friend class ModeQAcro;
 class ModeAuto : public Mode
 {
 public:
+    friend class Plane;
 
     Number mode_number() const override { return Number::AUTO; }
     const char *name() const override { return "AUTO"; }
@@ -237,6 +239,10 @@ class ModeAuto : public Mode
 
     void run() override;
 
+#if AP_PLANE_GLIDER_PULLUP_ENABLED
+    bool in_pullup() const { return pullup.in_pullup(); }
+#endif
+
 protected:
 
     bool _enter() override;
@@ -257,6 +263,10 @@ class ModeAuto : public Mode
         uint8_t stage;
         uint32_t last_ms;
     } wiggle;
+
+#if AP_PLANE_GLIDER_PULLUP_ENABLED
+    GliderPullup pullup;
+#endif // AP_PLANE_GLIDER_PULLUP_ENABLED
 };
 
 

ArduPlane/mode_auto.cpp

@@ -79,6 +79,12 @@ void ModeAuto::update()
     }
 #endif
 
+#if AP_PLANE_GLIDER_PULLUP_ENABLED
+    if (pullup.in_pullup()) {
+        return;
+    }
+#endif
+
     if (nav_cmd_id == MAV_CMD_NAV_TAKEOFF ||
         (nav_cmd_id == MAV_CMD_NAV_LAND && plane.flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING)) {
         plane.takeoff_calc_roll();
@@ -166,6 +172,13 @@ bool ModeAuto::is_landing() const
 
 void ModeAuto::run()
 {
+#if AP_PLANE_GLIDER_PULLUP_ENABLED
+    if (pullup.in_pullup()) {
+        pullup.stabilize_pullup();
+        return;
+    }
+#endif
+
     if (plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_ALTITUDE_WAIT) {
 
         wiggle_servos();

ArduPlane/navigation.cpp

@@ -321,7 +321,7 @@ void Plane::update_loiter_update_nav(uint16_t radius)
     if ((loiter.start_time_ms == 0 &&
          (control_mode == &mode_auto || control_mode == &mode_guided) &&
          auto_state.crosstrack &&
-         current_loc.get_distance(next_WP_loc) > radius*3) ||
+         current_loc.get_distance(next_WP_loc) > 3 * nav_controller->loiter_radius(radius)) ||
         quadplane_qrtl_switch) {
         /*
           if never reached loiter point and using crosstrack and somewhat far away from loiter point