Merge branch 'master' into clone

ArduPilot · Aug 1, 2024 · 1514831 · 1514831
2 parents 7f1de07 + 963addd
commit 1514831
Show file tree

Hide file tree

Showing 51 changed files with 622 additions and 366 deletions.
diff --git a/AntennaTracker/ReleaseNotes.txt b/AntennaTracker/ReleaseNotes.txt
@@ -1,5 +1,9 @@
 Antenna Tracker Release Notes:
 ------------------------------------------------------------------
+Release 4.5.5 1st Aug 2024
+
+No changes from 4.5.5-beta2
+------------------------------------------------------------------
 Release 4.5.5-beta2 27 July 2024
 
 Changes from 4.5.5-beta1

diff --git a/ArduCopter/ReleaseNotes.txt b/ArduCopter/ReleaseNotes.txt
@@ -1,5 +1,9 @@
 ArduPilot Copter Release Notes:
 ------------------------------------------------------------------
+Release 4.5.5 1st Aug 2024
+
+No changes from 4.5.5-beta2
+------------------------------------------------------------------
 Release 4.5.5-beta2 27 July 2024
 
 Changes from 4.5.5-beta1

diff --git a/ArduCopter/tuning.cpp b/ArduCopter/tuning.cpp
@@ -37,70 +37,70 @@ void Copter::tuning()
 
     // Roll, Pitch tuning
     case TUNING_STABILIZE_ROLL_PITCH_KP:
-        attitude_control->get_angle_roll_p().kP(tuning_value);
-        attitude_control->get_angle_pitch_p().kP(tuning_value);
+        attitude_control->get_angle_roll_p().set_kP(tuning_value);
+        attitude_control->get_angle_pitch_p().set_kP(tuning_value);
         break;
 
     case TUNING_RATE_ROLL_PITCH_KP:
-        attitude_control->get_rate_roll_pid().kP(tuning_value);
-        attitude_control->get_rate_pitch_pid().kP(tuning_value);
+        attitude_control->get_rate_roll_pid().set_kP(tuning_value);
+        attitude_control->get_rate_pitch_pid().set_kP(tuning_value);
         break;
 
     case TUNING_RATE_ROLL_PITCH_KI:
-        attitude_control->get_rate_roll_pid().kI(tuning_value);
-        attitude_control->get_rate_pitch_pid().kI(tuning_value);
+        attitude_control->get_rate_roll_pid().set_kI(tuning_value);
+        attitude_control->get_rate_pitch_pid().set_kI(tuning_value);
         break;
 
     case TUNING_RATE_ROLL_PITCH_KD:
-        attitude_control->get_rate_roll_pid().kD(tuning_value);
-        attitude_control->get_rate_pitch_pid().kD(tuning_value);
+        attitude_control->get_rate_roll_pid().set_kD(tuning_value);
+        attitude_control->get_rate_pitch_pid().set_kD(tuning_value);
         break;
 
     // Yaw tuning
     case TUNING_STABILIZE_YAW_KP:
-        attitude_control->get_angle_yaw_p().kP(tuning_value);
+        attitude_control->get_angle_yaw_p().set_kP(tuning_value);
         break;
 
     case TUNING_YAW_RATE_KP:
-        attitude_control->get_rate_yaw_pid().kP(tuning_value);
+        attitude_control->get_rate_yaw_pid().set_kP(tuning_value);
         break;
 
     case TUNING_YAW_RATE_KD:
-        attitude_control->get_rate_yaw_pid().kD(tuning_value);
+        attitude_control->get_rate_yaw_pid().set_kD(tuning_value);
         break;
 
     // Altitude and throttle tuning
     case TUNING_ALTITUDE_HOLD_KP:
-        pos_control->get_pos_z_p().kP(tuning_value);
+        pos_control->get_pos_z_p().set_kP(tuning_value);
         break;
 
     case TUNING_THROTTLE_RATE_KP:
-        pos_control->get_vel_z_pid().kP(tuning_value);
+        pos_control->get_vel_z_pid().set_kP(tuning_value);
         break;
 
     case TUNING_ACCEL_Z_KP:
-        pos_control->get_accel_z_pid().kP(tuning_value);
+        pos_control->get_accel_z_pid().set_kP(tuning_value);
         break;
 
     case TUNING_ACCEL_Z_KI:
-        pos_control->get_accel_z_pid().kI(tuning_value);
+        pos_control->get_accel_z_pid().set_kI(tuning_value);
         break;
 
     case TUNING_ACCEL_Z_KD:
-        pos_control->get_accel_z_pid().kD(tuning_value);
+        pos_control->get_accel_z_pid().set_kD(tuning_value);
         break;
 
     // Loiter and navigation tuning
     case TUNING_LOITER_POSITION_KP:
-        pos_control->get_pos_xy_p().kP(tuning_value);
+        pos_control->get_pos_xy_p().set_kP(tuning_value);
         break;
 
     case TUNING_VEL_XY_KP:
-        pos_control->get_vel_xy_pid().kP(tuning_value);
+        pos_control->get_vel_xy_pid().set_kP(tuning_value);
         break;
 
     case TUNING_VEL_XY_KI:
-        pos_control->get_vel_xy_pid().kI(tuning_value);
+        pos_control->get_vel_xy_pid().set_kI(tuning_value);
         break;
 
     case TUNING_WP_SPEED:
@@ -127,15 +127,15 @@ void Copter::tuning()
         break;
 
     case TUNING_RATE_PITCH_FF:
-        attitude_control->get_rate_pitch_pid().ff(tuning_value);
+        attitude_control->get_rate_pitch_pid().set_ff(tuning_value);
         break;
 
     case TUNING_RATE_ROLL_FF:
-        attitude_control->get_rate_roll_pid().ff(tuning_value);
+        attitude_control->get_rate_roll_pid().set_ff(tuning_value);
         break;
 
     case TUNING_RATE_YAW_FF:
-        attitude_control->get_rate_yaw_pid().ff(tuning_value);
+        attitude_control->get_rate_yaw_pid().set_ff(tuning_value);
         break;
 #endif
 
@@ -154,27 +154,27 @@ void Copter::tuning()
         break;
 
     case TUNING_RATE_PITCH_KP:
-        attitude_control->get_rate_pitch_pid().kP(tuning_value);
+        attitude_control->get_rate_pitch_pid().set_kP(tuning_value);
         break;
 
     case TUNING_RATE_PITCH_KI:
-        attitude_control->get_rate_pitch_pid().kI(tuning_value);
+        attitude_control->get_rate_pitch_pid().set_kI(tuning_value);
         break;
 
     case TUNING_RATE_PITCH_KD:
-        attitude_control->get_rate_pitch_pid().kD(tuning_value);
+        attitude_control->get_rate_pitch_pid().set_kD(tuning_value);
         break;
 
     case TUNING_RATE_ROLL_KP:
-        attitude_control->get_rate_roll_pid().kP(tuning_value);
+        attitude_control->get_rate_roll_pid().set_kP(tuning_value);
         break;
 
     case TUNING_RATE_ROLL_KI:
-        attitude_control->get_rate_roll_pid().kI(tuning_value);
+        attitude_control->get_rate_roll_pid().set_kI(tuning_value);
         break;
 
     case TUNING_RATE_ROLL_KD:
-        attitude_control->get_rate_roll_pid().kD(tuning_value);
+        attitude_control->get_rate_roll_pid().set_kD(tuning_value);
         break;
 
 #if FRAME_CONFIG != HELI_FRAME
@@ -184,7 +184,7 @@ void Copter::tuning()
 #endif
 
     case TUNING_RATE_YAW_FILT:
-        attitude_control->get_rate_yaw_pid().filt_E_hz(tuning_value);
+        attitude_control->get_rate_yaw_pid().set_filt_E_hz(tuning_value);
         break;
 
     case TUNING_SYSTEM_ID_MAGNITUDE:

diff --git a/ArduPlane/ReleaseNotes.txt b/ArduPlane/ReleaseNotes.txt
@@ -1,5 +1,9 @@
 ArduPilot Plane Release Notes:
 ------------------------------------------------------------------
+Release 4.5.5 1st Aug 2024
+
+No changes from 4.5.5-beta2
+------------------------------------------------------------------
 Release 4.5.5-beta2 27 July 2024
 
 Changes from 4.5.5-beta1

diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp
@@ -521,12 +521,10 @@ float Plane::apply_throttle_limits(float throttle_in)
 
     // Query the conditions where TKOFF_THR_MAX applies.
     const bool use_takeoff_throttle =
-#if HAL_QUADPLANE_ENABLED
-        quadplane.in_transition() ||
-#endif
         (flight_stage == AP_FixedWing::FlightStage::TAKEOFF) ||
         (flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING);
 
+    // Handle throttle limits for takeoff conditions.
     if (use_takeoff_throttle) {
         if (aparm.takeoff_throttle_max != 0) {
             // Replace max throttle with the takeoff max throttle setting.
@@ -539,7 +537,11 @@ float Plane::apply_throttle_limits(float throttle_in)
         const bool use_throttle_range = (aparm.takeoff_options & (uint32_t)AP_FixedWing::TakeoffOption::THROTTLE_RANGE);
         if (!use_throttle_range || !ahrs.using_airspeed_sensor()) {
             // Use a constant max throttle throughout the takeoff or when airspeed readings are not available.
-            min_throttle = MAX(min_throttle, aparm.takeoff_throttle_max.get());
+            if (aparm.takeoff_throttle_max.get() == 0) {
+                min_throttle = MAX(min_throttle, aparm.throttle_max.get());
+            } else {
+                min_throttle = MAX(min_throttle, aparm.takeoff_throttle_max.get());
+            }
         } else if (use_throttle_range) { // Use a throttle range through the takeoff.
             if (aparm.takeoff_throttle_min.get() != 0) { // This is enabled by TKOFF_MODE==1.
                 min_throttle = MAX(min_throttle, aparm.takeoff_throttle_min.get());
@@ -551,6 +553,15 @@ float Plane::apply_throttle_limits(float throttle_in)
         min_throttle = 0;
     }
 
+    // Handle throttle limits for transition conditions.
+#if HAL_QUADPLANE_ENABLED
+    if (quadplane.in_transition()) {
+        if (aparm.takeoff_throttle_max != 0) {
+            max_throttle = aparm.takeoff_throttle_max.get();
+        }
+    }
+#endif
+
     // Compensate the limits for battery voltage drop.
     // This relaxes the limits when the battery is getting depleted.
     g2.fwd_batt_cmp.apply_min_max(min_throttle, max_throttle);

diff --git a/ArduPlane/takeoff.cpp b/ArduPlane/takeoff.cpp
@@ -232,7 +232,8 @@ void Plane::takeoff_calc_throttle(const bool use_max_throttle) {
     // Set the minimum throttle limit.
     const bool use_throttle_range = (aparm.takeoff_options & (uint32_t)AP_FixedWing::TakeoffOption::THROTTLE_RANGE);
     if (!use_throttle_range || !ahrs.using_airspeed_sensor() || use_max_throttle) { // Traditional takeoff throttle limit.
-        TECS_controller.set_throttle_min(0.01f*aparm.takeoff_throttle_max);
+        float min_throttle = (aparm.takeoff_throttle_max != 0) ? 0.01f*aparm.takeoff_throttle_max : 0.01f*aparm.throttle_max;
+        TECS_controller.set_throttle_min(min_throttle);
     } else { // TKOFF_MODE == 1, allow for a throttle range.
         if (aparm.takeoff_throttle_min != 0) { // Override THR_MIN.
             TECS_controller.set_throttle_min(0.01f*aparm.takeoff_throttle_min);

diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp
@@ -78,6 +78,14 @@ const AP_Param::Info Sub::var_info[] = {
     // @User: Standard
     GSCALAR(failsafe_gcs, "FS_GCS_ENABLE", FS_GCS_DISARM),
 
+    // @Param: FS_GCS_TIMEOUT
+    // @DisplayName: GCS failsafe timeout
+    // @Description: Timeout before triggering the GCS failsafe
+    // @Units: s
+    // @Increment: 1
+    // @User: Standard
+    GSCALAR(failsafe_gcs_timeout, "FS_GCS_TIMEOUT", FS_GCS_TIMEOUT_S),
+
     // @Param: FS_LEAK_ENABLE
     // @DisplayName: Leak Failsafe Enable
     // @Description: Controls what action to take if a leak is detected.

diff --git a/ArduSub/Parameters.h b/ArduSub/Parameters.h
@@ -185,6 +185,7 @@ class Parameters {
         k_param_fs_batt_voltage,          // unused - moved to AP_BattMonitor
         k_param_failsafe_pilot_input,
         k_param_failsafe_pilot_input_timeout,
+        k_param_failsafe_gcs_timeout,
 
 
         // Misc Sub settings
@@ -257,6 +258,7 @@ class Parameters {
     AP_Int8         failsafe_terrain;
     AP_Int8         failsafe_pilot_input;       // pilot input failsafe behavior
     AP_Float        failsafe_pilot_input_timeout;
+    AP_Float        failsafe_gcs_timeout;       // ground station failsafe timeout (seconds)
 
     AP_Int8         xtrack_angle_limit;
 

diff --git a/ArduSub/defines.h b/ArduSub/defines.h
@@ -79,8 +79,8 @@ enum LoggingParameters {
 #ifndef FS_GCS
 # define FS_GCS                        DISABLED
 #endif
-#ifndef FS_GCS_TIMEOUT_MS
-# define FS_GCS_TIMEOUT_MS             2500    // gcs failsafe triggers after this number of milliseconds with no GCS heartbeat
+#ifndef FS_GCS_TIMEOUT_S
+# define FS_GCS_TIMEOUT_S             5.0    // gcs failsafe triggers after this number of seconds with no GCS heartbeat
 #endif
 
 // missing terrain data failsafe

diff --git a/ArduSub/failsafe.cpp b/ArduSub/failsafe.cpp
@@ -321,7 +321,8 @@ void Sub::failsafe_gcs_check()
     uint32_t tnow = AP_HAL::millis();
 
     // Check if we have gotten a GCS heartbeat recently (GCS sysid must match SYSID_MYGCS parameter)
-    if (tnow - gcs_last_seen_ms < FS_GCS_TIMEOUT_MS) {
+    const uint32_t gcs_timeout_ms = uint32_t(constrain_float(g.failsafe_gcs_timeout * 1000.0f, 0.0f, UINT32_MAX));
+    if (tnow - gcs_last_seen_ms < gcs_timeout_ms) {
         // Log event if we are recovering from previous gcs failsafe
         if (failsafe.gcs) {
             LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_GCS, LogErrorCode::FAILSAFE_RESOLVED);

diff --git a/Rover/ReleaseNotes.txt b/Rover/ReleaseNotes.txt
@@ -1,5 +1,9 @@
 Rover Release Notes:
 ------------------------------------------------------------------
+Release 4.5.5 1st Aug 2024
+
+No changes from 4.5.5-beta2
+------------------------------------------------------------------
 Release 4.5.5-beta2 27 July 2024
 
 Changes from 4.5.5-beta1

diff --git a/Rover/mode.h b/Rover/mode.h
@@ -457,6 +457,12 @@ class ModeCircle : public Mode
     // initialise mode
     bool _enter() override;
 
+    // Update position controller targets driving to the circle edge
+    void update_drive_to_radius();
+
+    // Update position controller targets while circling
+    void update_circling();
+
     // initialise target_yaw_rad using the vehicle's position and yaw
     // if there is no current position estimate target_yaw_rad is set to vehicle yaw
     void init_target_yaw_rad();

diff --git a/Rover/mode_circle.cpp b/Rover/mode_circle.cpp
@@ -144,19 +144,50 @@ void ModeCircle::update()
         return;
     }
 
-    // check if vehicle has reached edge of circle
+    // Update distance to destination and distance to edge
     const Vector2f center_to_veh = curr_pos - config.center_pos;
     _distance_to_destination = center_to_veh.length();
     dist_to_edge_m = fabsf(_distance_to_destination - config.radius);
+
+    // Update depending on stage
     if (!reached_edge) {
-        const float dist_thresh_m = MAX(g2.turn_radius, AR_CIRCLE_REACHED_EDGE_DIST);
-        reached_edge = dist_to_edge_m <= dist_thresh_m;
+        update_drive_to_radius();
+
+    } else {
+        update_circling();
     }
 
+    g2.pos_control.update(rover.G_Dt);
+
+    // get desired speed and turn rate from pos_control
+    const float desired_speed = g2.pos_control.get_desired_speed();
+    const float desired_turn_rate = g2.pos_control.get_desired_turn_rate_rads();
+
+    // run steering and throttle controllers
+    calc_steering_from_turn_rate(desired_turn_rate);
+    calc_throttle(desired_speed, true);
+}
+
+void ModeCircle::update_drive_to_radius()
+{
+    // check if vehicle has reached edge of circle
+    const float dist_thresh_m = MAX(g2.turn_radius, AR_CIRCLE_REACHED_EDGE_DIST);
+    reached_edge |= dist_to_edge_m <= dist_thresh_m;
+
+    // calculate target point's position, velocity and acceleration
+    target.pos = config.center_pos.topostype();
+    target.pos.offset_bearing(degrees(target.yaw_rad), config.radius);
+
+    g2.pos_control.input_pos_target(target.pos, rover.G_Dt);
+}
+
+// Update position controller targets while circling
+void ModeCircle::update_circling()
+{
+
     // accelerate speed up to desired speed
-    const float speed_max = reached_edge ? config.speed : 0.0;
     const float speed_change_max = (g2.pos_control.get_accel_max() * 0.5 * rover.G_Dt);
-    const float accel_fb = constrain_float(speed_max - target.speed, -speed_change_max, speed_change_max);
+    const float accel_fb = constrain_float(config.speed - target.speed, -speed_change_max, speed_change_max);
     target.speed += accel_fb;
 
     // calculate angular rate and update target angle
@@ -179,15 +210,7 @@ void ModeCircle::update()
     target.accel.rotate(target.yaw_rad);
 
     g2.pos_control.set_pos_vel_accel_target(target.pos, target.vel, target.accel);
-    g2.pos_control.update(rover.G_Dt);
-
-    // get desired speed and turn rate from pos_control
-    const float desired_speed = g2.pos_control.get_desired_speed();
-    const float desired_turn_rate = g2.pos_control.get_desired_turn_rate_rads();
 
-    // run steering and throttle controllers
-    calc_steering_from_turn_rate(desired_turn_rate);
-    calc_throttle(desired_speed, true);
 }
 
 // return desired heading (in degrees) and cross track error (in meters) for reporting to ground station (NAV_CONTROLLER_OUTPUT message)

diff --git a/Tools/ardupilotwaf/qurt.py b/Tools/ardupilotwaf/qurt.py
@@ -27,7 +27,7 @@ def build(bld):
     bld(
         # build ap_host
         source=[STUB_SO, MAIN_CPP],
-        rule="%s -I%s %s %s %s -o %s" % (AARCH64_CXX, STUB_INC.abspath(),
+        rule="%s -I%s %s %s %s -lpthread -o %s" % (AARCH64_CXX, STUB_INC.abspath(),
                                           MAIN_CPP.abspath(), IFADDR_CPP.abspath(), STUB_SO.abspath(), AP_HOST.abspath()),
         target=[AP_HOST],
         group='dynamic_sources'

diff --git a/Tools/autotest/ArduRover_Tests/DriveMission/rover1.txt b/Tools/autotest/ArduRover_Tests/DriveMission/rover1.txt
@@ -18,4 +18,5 @@ QGC WPL 110
 16	0	3	16	0.000000	0.000000	0.000000	0.000000	40.071133	-105.229233	11085.900391	1
 17	0	3	16	0.000000	0.000000	0.000000	0.000000	40.071255	-105.229355	11085.900391	1
 18	0	3	16	0.000000	0.000000	0.000000	0.000000	40.071404	-105.229485	9502.200195	1
-19	0	3	16	0.000000	0.000000	0.000000	0.000000	40.071369	-105.229828	9502.200195	1
+19	0	3	18	1.000000	0.000000	20.000000	0.000000	40.071105	-105.229625	0.000000	1
+20	0	3	16	0.000000	0.000000	0.000000	0.000000	40.071369	-105.229828	9502.200195	1