From a4bfab8cdc762609ff6df03a0d07450ad657d22f Mon Sep 17 00:00:00 2001 From: Eric Katzfey Date: Mon, 15 Jul 2024 17:32:17 -0700 Subject: [PATCH 001/124] Tools: Added default parameter file for ModalAI D0013 drone --- Tools/Frame_params/ModalAI/D0013.parm | 109 ++++++++++++++++++++++++++ 1 file changed, 109 insertions(+) create mode 100644 Tools/Frame_params/ModalAI/D0013.parm diff --git a/Tools/Frame_params/ModalAI/D0013.parm b/Tools/Frame_params/ModalAI/D0013.parm new file mode 100644 index 00000000000000..1d3fcf65139cb0 --- /dev/null +++ b/Tools/Frame_params/ModalAI/D0013.parm @@ -0,0 +1,109 @@ +# parameters for the Stinger drone + +# flight modes +FLTMODE1 5 +FLTMODE4 2 +FLTMODE_CH 6 + +# enable PID logging +LOG_BITMASK 65535 + +# mag field varies quite a lot between batteries +ARMING_MAGTHRESH 200 + +# IMU orientation +# 4 = YAW 180 +AHRS_ORIENTATION 4 + +# Forced external compass +COMPASS_EXTERNAL 2 +# compass orientation +# 2 = YAW 90 +COMPASS_ORIENT 8 + +# filtering +INS_GYRO_FILTER 40 +INS_HNTCH_ENABLE 1 +INS_HNTCH_ATT 40.0 +INS_HNTCH_BW 50.0 +INS_HNTCH_FM_RAT 1.0 +INS_HNTCH_FREQ 100.0 +INS_HNTCH_HMNCS 7 +INS_HNTCH_MODE 3 +INS_HNTCH_OPTS 2 +INS_HNTCH_REF 1.0 + +# run IMU at 2kHz +INS_GYRO_RATE 1 + +# a bit more agressive loiter +PILOT_SPEED_UP 500 +LOIT_BRK_ACCEL 500 +LOIT_BRK_JERK 1000 +LOIT_BRK_DELAY 0.200000 + +# Tune parameters from Quik Tune +ATC_ACCEL_Y_MAX 30000.0 +ATC_ACCEL_R_MAX 125000.0 +ATC_ACCEL_P_MAX 125000.0 +ATC_RAT_PIT_D 0.002856164472177625 +ATC_RAT_PIT_FLTD 40.0 +ATC_RAT_PIT_FLTT 40.0 +ATC_RAT_PIT_I 0.124929137527942657 +ATC_RAT_PIT_P 0.124929137527942657 +ATC_RAT_PIT_SMAX 50.0 +ATC_RAT_RLL_D 0.001519999350421131 +ATC_RAT_RLL_FLTD 40.0 +ATC_RAT_RLL_FLTT 40.0 +ATC_RAT_RLL_I 0.075611755251884460 +ATC_RAT_RLL_P 0.075611755251884460 +ATC_RAT_RLL_SMAX 50.0 +ATC_RAT_YAW_D 0.010000008158385754 +ATC_RAT_YAW_FLTD 40.0 +ATC_RAT_YAW_FLTE 2.0 +ATC_RAT_YAW_FLTT 40.0 +ATC_RAT_YAW_I 0.050000030547380447 +ATC_RAT_YAW_P 0.500000298023223877 +ATC_RAT_YAW_SMAX 50.0 + +# battery setup +BATT_LOW_VOLT 14.2 +BATT_OPTIONS 64 +BATT_VOLT_PIN 1 +BATT_CURR_PIN 2 +BATT_VOLT_MULT 1 +BATT_AMP_PERVLT 1 +BATT_AMP_OFFSET 0.0 + +# 4S battery range +MOT_BAT_VOLT_MAX 16.800000 +MOT_BAT_VOLT_MIN 13.200000 + +# Learned hover thrust +MOT_THST_EXPO 0.550000011920928955 +MOT_THST_HOVER 0.130549192428588867 + +# quad-X +FRAME_CLASS 1 + +# tweak R/C inputs +RC1_MIN 1000 +RC1_MAX 2000 +RC1_DZ 40 +RC2_MIN 1000 +RC2_MAX 2000 +RC2_REVERSED 1 +RC3_MIN 1000 +RC3_MAX 2000 +RC4_MIN 1000 +RC4_MAX 2000 +RC4_DZ 40 + +# add arming on right switch +RC7_OPTION 153 + +# Motor mappings +SERVO1_FUNCTION 33 +SERVO2_FUNCTION 36 +SERVO3_FUNCTION 34 +SERVO4_FUNCTION 35 From 2f0f7aa669ef6495c1b4a400c7760ace97fc7210 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 19 Jul 2024 16:58:05 +1000 Subject: [PATCH 002/124] autotest: tidy Copter flip test --- Tools/autotest/arducopter.py | 68 +++++++++++++++--------------------- 1 file changed, 29 insertions(+), 39 deletions(-) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 21efa3ef5fac15..1766e6ac77d749 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -2266,46 +2266,36 @@ def MagFail(self): def ModeFlip(self): '''Fly Flip Mode''' - ex = None - try: - self.set_message_rate_hz(mavutil.mavlink.MAVLINK_MSG_ID_ATTITUDE, 100) + self.context_set_message_rate_hz(mavutil.mavlink.MAVLINK_MSG_ID_ATTITUDE, 100) - self.takeoff(20) - self.hover() - old_speedup = self.get_parameter("SIM_SPEEDUP") - self.set_parameter('SIM_SPEEDUP', 1) - self.progress("Flipping in roll") - self.set_rc(1, 1700) - self.send_cmd_do_set_mode('FLIP') # don't wait for success - self.wait_attitude(despitch=0, desroll=45, tolerance=30) - self.wait_attitude(despitch=0, desroll=90, tolerance=30) - self.wait_attitude(despitch=0, desroll=-45, tolerance=30) - self.progress("Waiting for level") - self.set_rc(1, 1500) # can't change quickly enough! - self.wait_attitude(despitch=0, desroll=0, tolerance=5) - - self.progress("Regaining altitude") - self.change_mode('ALT_HOLD') - self.wait_altitude(19, 60, relative=True) - - self.progress("Flipping in pitch") - self.set_rc(2, 1700) - self.send_cmd_do_set_mode('FLIP') # don't wait for success - self.wait_attitude(despitch=45, desroll=0, tolerance=30) - # can't check roll here as it flips from 0 to -180.. - self.wait_attitude(despitch=90, tolerance=30) - self.wait_attitude(despitch=-45, tolerance=30) - self.progress("Waiting for level") - self.set_rc(2, 1500) # can't change quickly enough! - self.wait_attitude(despitch=0, desroll=0, tolerance=5) - self.set_parameter('SIM_SPEEDUP', old_speedup) - self.do_RTL() - except Exception as e: - self.print_exception_caught(e) - ex = e - self.set_message_rate_hz(mavutil.mavlink.MAVLINK_MSG_ID_ATTITUDE, 0) - if ex is not None: - raise ex + self.takeoff(20) + + self.progress("Flipping in roll") + self.set_rc(1, 1700) + self.send_cmd_do_set_mode('FLIP') # don't wait for success + self.wait_attitude(despitch=0, desroll=45, tolerance=30) + self.wait_attitude(despitch=0, desroll=90, tolerance=30) + self.wait_attitude(despitch=0, desroll=-45, tolerance=30) + self.progress("Waiting for level") + self.set_rc(1, 1500) # can't change quickly enough! + self.wait_attitude(despitch=0, desroll=0, tolerance=5) + + self.progress("Regaining altitude") + self.change_mode('ALT_HOLD') + self.wait_altitude(19, 60, relative=True) + + self.progress("Flipping in pitch") + self.set_rc(2, 1700) + self.send_cmd_do_set_mode('FLIP') # don't wait for success + self.wait_attitude(despitch=45, desroll=0, tolerance=30) + # can't check roll here as it flips from 0 to -180.. + self.wait_attitude(despitch=90, tolerance=30) + self.wait_attitude(despitch=-45, tolerance=30) + self.progress("Waiting for level") + self.set_rc(2, 1500) # can't change quickly enough! + self.wait_attitude(despitch=0, desroll=0, tolerance=5) + + self.do_RTL() def configure_EKFs_to_use_optical_flow_instead_of_GPS(self): '''configure EKF to use optical flow instead of GPS''' From 1626611e0b70c5443ffbd855b11ed52632760c9b Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 19 Jul 2024 09:45:57 +1000 Subject: [PATCH 003/124] SITL: add documentation for SIM_ACC?_RND parameters --- libraries/SITL/SITL.cpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index 49c344b6e7fb0d..a21e744c349b53 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -924,11 +924,21 @@ const AP_Param::GroupInfo SIM::var_ins[] = { #if INS_MAX_INSTANCES > 2 AP_GROUPINFO("GYR3_RND", 10, SIM, gyro_noise[2], 0), #endif + // @Param: ACC1_RND + // @DisplayName: Accel 1 motor noise factor + // @Description: scaling factor for simulated vibration from motors + // @User: Advanced AP_GROUPINFO("ACC1_RND", 11, SIM, accel_noise[0], 0), #if INS_MAX_INSTANCES > 1 + // @Param: ACC2_RND + // @DisplayName: Accel 2 motor noise factor + // @CopyFieldsFrom: SIM_ACC1_RND AP_GROUPINFO("ACC2_RND", 12, SIM, accel_noise[1], 0), #endif #if INS_MAX_INSTANCES > 2 + // @Param: ACC3_RND + // @DisplayName: Accel 3 motor noise factor + // @CopyFieldsFrom: SIM_ACC1_RND AP_GROUPINFO("ACC3_RND", 13, SIM, accel_noise[2], 0), #endif // @Param: GYR1_SCALE @@ -1106,6 +1116,9 @@ const AP_Param::GroupInfo SIM::var_ins[] = { // @Vector3Parameter: 1 AP_GROUPINFO("GYR4_SCALE", 36, SIM, gyro_scale[3], 0), + // @Param: ACC4_RND + // @DisplayName: Accel 4 motor noise factor + // @CopyFieldsFrom: SIM_ACC1_RND AP_GROUPINFO("ACC4_RND", 37, SIM, accel_noise[3], 0), AP_GROUPINFO("GYR4_RND", 38, SIM, gyro_noise[3], 0), @@ -1156,6 +1169,9 @@ const AP_Param::GroupInfo SIM::var_ins[] = { // @Vector3Parameter: 1 AP_GROUPINFO("GYR5_SCALE", 43, SIM, gyro_scale[4], 0), + // @Param: ACC5_RND + // @DisplayName: Accel 5 motor noise factor + // @CopyFieldsFrom: SIM_ACC1_RND AP_GROUPINFO("ACC5_RND", 44, SIM, accel_noise[4], 0), AP_GROUPINFO("GYR5_RND", 45, SIM, gyro_noise[4], 0), From 9c10e641682d443a158d58292421b0238b73a939 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 19 Jul 2024 09:45:57 +1000 Subject: [PATCH 004/124] Tools: add documentation for SIM_ACC?_RND parameters --- Tools/autotest/vehicle_test_suite.py | 5 ----- 1 file changed, 5 deletions(-) diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py index ddb506b58518a2..4677be0ab8afcd 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -2384,11 +2384,6 @@ def test_adsb_send_threatening_adsb_message(self, here, offset_ne=None): def get_sim_parameter_documentation_get_whitelist(self): # common parameters ret = set([ - "SIM_ACC1_RND", - "SIM_ACC2_RND", - "SIM_ACC3_RND", - "SIM_ACC4_RND", - "SIM_ACC5_RND", "SIM_ACC_FILE_RW", "SIM_ACC_TRIM_X", "SIM_ACC_TRIM_Y", From 6eeaa10b7ef228e5c872333a107642f7e70d47f9 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 17 Jul 2024 11:52:36 +1000 Subject: [PATCH 005/124] Tools: add test for final-yaw behaviour in Auto mode RTL --- Tools/autotest/arducopter.py | 53 ++++++++++++++++++++++++++++++++++++ 1 file changed, 53 insertions(+) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 1766e6ac77d749..f733d921837203 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -11620,6 +11620,58 @@ def AutoContinueOnRCFailsafe(self): self.wait_groundspeed(0, 0.1, minimum_duration=30, timeout=450) self.do_RTL() + def MissionRTLYawBehaviour(self): + '''check end-of-mission yaw behaviour''' + self.set_parameters({ + "AUTO_OPTIONS": 3, + }) + + self.start_subtest("behaviour with WP_YAW_BEHAVE set to next-waypoint-except-RTL") + self.upload_simple_relhome_mission([ + # N E U + (mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 10), + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 20, 0, 10), + (mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0), + ]) + self.change_mode('AUTO') + self.wait_ready_to_arm() + original_heading = self.get_heading() + if abs(original_heading) < 5: + raise NotAchievedException(f"Bad original heading {original_heading}") + self.arm_vehicle() + self.wait_current_waypoint(3) + self.wait_rtl_complete() + self.wait_disarmed() + if abs(self.get_heading()) > 5: + raise NotAchievedException("Should have yaw zero without option") + + # must change out of auto and back in again to reset state machine: + self.change_mode('LOITER') + self.change_mode('AUTO') + + self.start_subtest("behaviour with WP_YAW_BEHAVE set to next-waypoint") + self.upload_simple_relhome_mission([ + # N E U + (mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 10), + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 20, 20), + (mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0), + ]) + self.set_parameters({ + "WP_YAW_BEHAVIOR": 1, # look at next waypoint (including in RTL) + }) + self.change_mode('AUTO') + self.wait_ready_to_arm() + original_heading = self.get_heading() + if abs(original_heading) > 1: + raise NotAchievedException("Bad original heading") + self.arm_vehicle() + self.wait_current_waypoint(3) + self.wait_rtl_complete() + self.wait_disarmed() + new_heading = self.get_heading() + if abs(new_heading - original_heading) > 5: + raise NotAchievedException(f"Should return to original heading want={original_heading} got={new_heading}") + def tests2b(self): # this block currently around 9.5mins here '''return list of all tests''' ret = ([ @@ -11714,6 +11766,7 @@ def tests2b(self): # this block currently around 9.5mins here self.GripperReleaseOnThrustLoss, self.REQUIRE_POSITION_FOR_ARMING, self.LoggingFormat, + self.MissionRTLYawBehaviour, ]) return ret From 887a890c4a74df19ef5ff959d9ead9e244959766 Mon Sep 17 00:00:00 2001 From: Stephen Dade Date: Tue, 16 Jul 2024 20:53:20 +1000 Subject: [PATCH 006/124] SITL: Add skid steering motorboat --- libraries/SITL/SIM_Sailboat.cpp | 40 +++++++++++++++++++++++++-------- libraries/SITL/SIM_Sailboat.h | 1 + 2 files changed, 32 insertions(+), 9 deletions(-) diff --git a/libraries/SITL/SIM_Sailboat.cpp b/libraries/SITL/SIM_Sailboat.cpp index 7d3dba67457ad1..846f077f43f967 100644 --- a/libraries/SITL/SIM_Sailboat.cpp +++ b/libraries/SITL/SIM_Sailboat.cpp @@ -32,6 +32,8 @@ namespace SITL { #define STEERING_SERVO_CH 0 // steering controlled by servo output 1 #define MAINSAIL_SERVO_CH 3 // main sail controlled by servo output 4 #define THROTTLE_SERVO_CH 2 // throttle controlled by servo output 3 +#define MOTORLEFT_SERVO_CH 0 // skid-steering left motor controlled by servo output 1 +#define MOTORRIGHT_SERVO_CH 2 // skid-steering right motor controlled by servo output 3 #define DIRECT_WING_SERVO_CH 4 // very roughly sort of a stability factors for waves @@ -45,6 +47,7 @@ Sailboat::Sailboat(const char *frame_str) : sail_area(1.0) { motor_connected = (strcmp(frame_str, "sailboat-motor") == 0); + skid_steering = strstr(frame_str, "skid") != nullptr; lock_step_scheduled = true; } @@ -97,13 +100,19 @@ float Sailboat::get_turn_circle(float steering) const // return yaw rate in deg/sec given a steering input (in the range -1 to +1) and speed in m/s float Sailboat::get_yaw_rate(float steering, float speed) const { - if (is_zero(steering) || is_zero(speed)) { - return 0; + float rate = 0.0f; + if (is_zero(steering) || (!skid_steering && is_zero(speed))) { + return rate; + } + + if (is_zero(speed) && skid_steering) { + rate = steering * M_PI * 5; + } else { + float d = get_turn_circle(steering); + float c = M_PI * d; + float t = c / speed; + rate = 360.0f / t; } - float d = get_turn_circle(steering); - float c = M_PI * d; - float t = c / speed; - float rate = 360.0f / t; return rate; } @@ -179,7 +188,14 @@ void Sailboat::update(const struct sitl_input &input) update_wind(input); // in sailboats the steering controls the rudder, the throttle controls the main sail position - float steering = 2*((input.servos[STEERING_SERVO_CH]-1000)/1000.0f - 0.5f); + float steering = 0.0f; + if (skid_steering) { + float steering_left = 2.0f*((input.servos[MOTORLEFT_SERVO_CH]-1000)/1000.0f - 0.5f); + float steering_right = 2.0f*((input.servos[MOTORRIGHT_SERVO_CH]-1000)/1000.0f - 0.5f); + steering = steering_left - steering_right; + } else { + steering = 2*((input.servos[STEERING_SERVO_CH]-1000)/1000.0f - 0.5f); + } // calculate apparent wind in earth-frame (this is the direction the wind is coming from) // Note than the SITL wind direction is defined as the direction the wind is travelling to @@ -257,8 +273,14 @@ void Sailboat::update(const struct sitl_input &input) // gives throttle force == hull drag at 10m/s float throttle_force = 0.0f; if (motor_connected) { - const uint16_t throttle_out = constrain_int16(input.servos[THROTTLE_SERVO_CH], 1000, 2000); - throttle_force = (throttle_out-1500) * 0.1f; + if (skid_steering) { + const uint16_t throttle_left = constrain_int16(input.servos[MOTORLEFT_SERVO_CH], 1000, 2000); + const uint16_t throttle_right = constrain_int16(input.servos[MOTORRIGHT_SERVO_CH], 1000, 2000); + throttle_force = (0.5f*(throttle_left + throttle_right)-1500) * 0.1f; + } else { + const uint16_t throttle_out = constrain_int16(input.servos[THROTTLE_SERVO_CH], 1000, 2000); + throttle_force = (throttle_out-1500) * 0.1f; + } } // accel in body frame due acceleration from sail and deceleration from hull friction diff --git a/libraries/SITL/SIM_Sailboat.h b/libraries/SITL/SIM_Sailboat.h index 4cb70e0448309a..e17d2c05dcd424 100644 --- a/libraries/SITL/SIM_Sailboat.h +++ b/libraries/SITL/SIM_Sailboat.h @@ -41,6 +41,7 @@ class Sailboat : public Aircraft { protected: bool motor_connected; // true if this frame has a motor + bool skid_steering; // true if this vehicle is a skid-steering vehicle float sail_area; // 1.0 for normal area private: From 8674bb84a6ab666bb42f75d0a52a945b9b813764 Mon Sep 17 00:00:00 2001 From: Stephen Dade Date: Tue, 16 Jul 2024 20:53:34 +1000 Subject: [PATCH 007/124] Tools: Add skid steering motorboat --- Tools/autotest/pysim/vehicleinfo.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/Tools/autotest/pysim/vehicleinfo.py b/Tools/autotest/pysim/vehicleinfo.py index 3682dbb01ac116..73349731a3f927 100644 --- a/Tools/autotest/pysim/vehicleinfo.py +++ b/Tools/autotest/pysim/vehicleinfo.py @@ -407,6 +407,12 @@ def __init__(self): "default_params_filename": ["default_params/rover.parm", "default_params/motorboat.parm"], }, + "motorboat-skid": { + "waf_target": "bin/ardurover", + "default_params_filename": ["default_params/rover.parm", + "default_params/motorboat.parm", + "default_params/rover-skid.parm"], + }, "sailboat": { "waf_target": "bin/ardurover", "default_params_filename": ["default_params/rover.parm", From 5919ef69cbb948ee01d3669b648d039afd68692d Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Tue, 4 Jun 2024 18:22:20 +0100 Subject: [PATCH 008/124] AP_DDS: add param DDS_DOMAIN_ID - Require reboot. - Set DDS_DOMAIN_ID range: 0 to 232.. Signed-off-by: Rhys Mainwaring --- libraries/AP_DDS/AP_DDS_Client.cpp | 11 +++++++++-- libraries/AP_DDS/AP_DDS_Client.h | 3 +++ 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index 76df6f2bca508e..eb54a54eff09f1 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -74,6 +74,14 @@ const AP_Param::GroupInfo AP_DDS_Client::var_info[] { #endif + // @Param: _DOMAIN_ID + // @DisplayName: DDS DOMAIN ID + // @Description: Set the ROS_DOMAIN_ID + // @Range: 0 232 + // @RebootRequired: True + // @User: Standard + AP_GROUPINFO("_DOMAIN_ID", 4, AP_DDS_Client, domain_id, 0), + AP_GROUPEND }; @@ -819,9 +827,8 @@ bool AP_DDS_Client::create() .type = UXR_PARTICIPANT_ID }; const char* participant_name = "ardupilot_dds"; - const uint16_t domain_id = 0; const auto participant_req_id = uxr_buffer_create_participant_bin(&session, reliable_out, participant_id, - domain_id, participant_name, UXR_REPLACE); + static_cast(domain_id), participant_name, UXR_REPLACE); //Participant requests constexpr uint8_t nRequestsParticipant = 1; diff --git a/libraries/AP_DDS/AP_DDS_Client.h b/libraries/AP_DDS/AP_DDS_Client.h index e796b78edb3fd4..ac11e53e572aa1 100644 --- a/libraries/AP_DDS/AP_DDS_Client.h +++ b/libraries/AP_DDS/AP_DDS_Client.h @@ -213,6 +213,9 @@ class AP_DDS_Client //! @brief Parameter storage static const struct AP_Param::GroupInfo var_info[]; + //! @brief ROS_DOMAIN_ID + AP_Int32 domain_id; + //! @brief Enum used to mark a topic as a data reader or writer enum class Topic_rw : uint8_t { DataReader = 0, From 218724a3fe72da511cc7e7911482bd6d2530a057 Mon Sep 17 00:00:00 2001 From: rushyam Date: Fri, 21 Jun 2024 11:27:31 +0530 Subject: [PATCH 009/124] Tools: ros2: correct ros_ws to ros2_ws ros_ws -> ros2_ws --- Tools/ros2/README.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/Tools/ros2/README.md b/Tools/ros2/README.md index 8c13f6275f11be..83014df4d52a87 100644 --- a/Tools/ros2/README.md +++ b/Tools/ros2/README.md @@ -38,7 +38,7 @@ The packages depend on: #### 1. Create a workspace folder ```bash -mkdir -p ~/ros_ws/src && cd ~/ros_ws/src +mkdir -p ~/ros2_ws/src && cd ~/ros2_ws/src ``` The ROS 2 tutorials contain more details regarding [ROS 2 workspaces](https://docs.ros.org/en/humble/Tutorials/Workspace/Creating-A-Workspace.html). @@ -54,7 +54,7 @@ vcs import --recursive < ros2.repos #### 3. Update dependencies ```bash -cd ~/ros_ws +cd ~/ros2_ws source /opt/ros/humble/setup.bash sudo apt update rosdep update @@ -72,7 +72,7 @@ ROS_DISTRO=humble ``` ```bash -cd ~/ros_ws +cd ~/ros2_ws colcon build --cmake-args -DBUILD_TESTING=ON ``` @@ -92,7 +92,7 @@ must be built from source and additional compiler flags are needed. #### 1. Create a workspace folder ```bash -mkdir -p ~/ros_ws/src && cd ~/ros_ws/src +mkdir -p ~/ros2_ws/src && cd ~/ros2_ws/src ``` #### 2. Get the `ros2_macos.repos` file @@ -108,7 +108,7 @@ vcs import --recursive < ros2_macos.repos #### 3. Update dependencies ```bash -cd ~/ros_ws +cd ~/ros2_ws source /{path_to_your_ros_distro_workspace}/install/setup.zsh ``` From aab1ae4018a23e3d0bb9e2883f6eca82115d38dc Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 18 Jul 2024 09:42:46 +1000 Subject: [PATCH 010/124] autotest: test Solo mount --- Tools/autotest/arducopter.py | 100 ++++++++++++++++++----------------- 1 file changed, 51 insertions(+), 49 deletions(-) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index f733d921837203..ade0324672ec4b 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -5245,6 +5245,7 @@ def constrained_mount_pitch(self, pitch_angle_deg, mount_instance=1): def test_mount_pitch(self, despitch, despitch_tolerance, mount_mode, timeout=10, hold=0, constrained=True): tstart = self.get_sim_time() success_start = 0 + while True: now = self.get_sim_time_cached() if now - tstart > timeout: @@ -5257,7 +5258,7 @@ def test_mount_pitch(self, despitch, despitch_tolerance, mount_mode, timeout=10, '''retrieve latest angles from GIMBAL_DEVICE_ATTITUDE_STATUS''' mount_roll, mount_pitch, mount_yaw = self.get_mount_roll_pitch_yaw_deg() - self.progress("despitch=%f roll=%f pitch=%f yaw=%f" % (despitch, mount_roll, mount_pitch, mount_yaw)) + # self.progress("despitch=%f roll=%f pitch=%f yaw=%f" % (despitch, mount_roll, mount_pitch, mount_yaw)) if abs(despitch - mount_pitch) > despitch_tolerance: self.progress("Mount pitch incorrect: got=%f want=%f (+/- %f)" % (mount_pitch, despitch, despitch_tolerance)) @@ -5333,9 +5334,9 @@ def set_mount_mode(self, mount_mode): p3=0, # stabilize pitch (unsupported) ) - def test_mount_rc_targetting(self): + def test_mount_rc_targetting(self, pitch_rc_neutral=1500, do_rate_tests=True): '''called in multipleplaces to make sure that mount RC targetting works''' - try: + if True: self.context_push() self.set_parameters({ 'RC6_OPTION': 0, @@ -5395,6 +5396,13 @@ def test_mount_rc_targetting(self): self.set_rc(12, 1500) + if do_rate_tests: + self.test_mount_rc_targetting_rate_control() + + self.context_pop() + + def test_mount_rc_targetting_rate_control(self, pitch_rc_neutral=1500): + if True: self.progress("Testing RC rate control") self.set_parameter('MNT1_RC_RATE', 10) self.test_mount_pitch(0, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) @@ -5417,46 +5425,21 @@ def test_mount_rc_targetting(self): self.set_rc(12, 1500) self.test_mount_pitch(0, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) - self.context_pop() - - except Exception as e: - self.print_exception_caught(e) - self.context_pop() - raise e - - def Mount(self): - '''Test Camera/Antenna Mount''' - ex = None - self.context_push() - old_srcSystem = self.mav.mav.srcSystem - self.mav.mav.srcSystem = 250 - self.set_parameter("DISARM_DELAY", 0) - try: - '''start by disabling GCS failsafe, otherwise we immediately disarm - due to (apparently) not receiving traffic from the GCS for - too long. This is probably a function of --speedup''' - self.set_parameter("FS_GCS_ENABLE", 0) - - # setup mount parameters - self.setup_servo_mount() - self.reboot_sitl() # to handle MNT_TYPE changing - + def mount_test_body(self, pitch_rc_neutral=1500, do_rate_tests=True, constrain_sysid_target=True): + '''Test Camera/Antenna Mount - assumes a camera is set up and ready to go''' + if True: # make sure we're getting gimbal device attitude status - self.assert_receive_message('GIMBAL_DEVICE_ATTITUDE_STATUS', timeout=5) + self.assert_receive_message('GIMBAL_DEVICE_ATTITUDE_STATUS', timeout=5, very_verbose=True) # change mount to neutral mode (point forward, not stabilising) self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_NEUTRAL) - # test pitch is not stabilising + # test pitch is not neutral to start with mount_roll_deg, mount_pitch_deg, mount_yaw_deg = self.get_mount_roll_pitch_yaw_deg() if mount_roll_deg != 0 or mount_pitch_deg != 0 or mount_yaw_deg != 0: - raise NotAchievedException("Mount stabilising when not requested") - - self.change_mode('GUIDED') - self.wait_ready_to_arm() - self.arm_vehicle() + raise NotAchievedException("Mount not neutral") - self.user_takeoff() + self.takeoff(30, mode='GUIDED') # pitch vehicle back and confirm gimbal is still not stabilising despitch = 10 @@ -5474,7 +5457,7 @@ def Mount(self): # center RC tilt control and change mount to RC_TARGETING mode self.progress("Gimbal to RC Targetting mode") - self.set_rc(6, 1500) + self.set_rc(6, pitch_rc_neutral) self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) # pitch vehicle back and confirm gimbal is stabilising @@ -5520,7 +5503,10 @@ def Mount(self): self.progress("Testing mount RC targetting") self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) - self.test_mount_rc_targetting() + self.test_mount_rc_targetting( + pitch_rc_neutral=pitch_rc_neutral, + do_rate_tests=do_rate_tests, + ) self.progress("Testing mount ROI behaviour") self.test_mount_pitch(0, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) @@ -5658,24 +5644,39 @@ def Mount(self): 0, # vz 0 # heading ) - self.test_mount_pitch(68, 5, mavutil.mavlink.MAV_MOUNT_MODE_SYSID_TARGET, hold=2) + self.test_mount_pitch( + 68, + 5, + mavutil.mavlink.MAV_MOUNT_MODE_SYSID_TARGET, + hold=2, + constrained=constrain_sysid_target, + ) self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_NEUTRAL) self.test_mount_pitch(0, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_NEUTRAL) - except Exception as e: - self.print_exception_caught(e) - ex = e - - self.mav.mav.srcSystem = old_srcSystem - self.disarm_vehicle(force=True) - - self.context_pop() + self.disarm_vehicle(force=True) - self.reboot_sitl() # to handle MNT1_TYPE changing + def Mount(self): + '''test servo mount''' + self.setup_servo_mount() + self.reboot_sitl() # to handle MNT_TYPE changing + self.mount_test_body() - if ex is not None: - raise ex + def MountSolo(self): + '''test type=2, a "Solo" mount''' + self.set_parameters({ + "MNT1_TYPE": 2, + "RC6_OPTION": 213, # MOUNT1_PITCH + }) + self.customise_SITL_commandline([ + "--gimbal" # connects on port 5762 + ]) + self.mount_test_body( + pitch_rc_neutral=1818, + do_rate_tests=False, # solo can't do rate control (yet?) + constrain_sysid_target=False, # not everything constrains all angles + ) def assert_mount_rpy(self, r, p, y, tolerance=1): '''assert mount atttiude in degrees''' @@ -11730,6 +11731,7 @@ def tests2b(self): # this block currently around 9.5mins here self.TerrainDBPreArm, self.ThrottleGainBoost, self.ScriptMountPOI, + self.MountSolo, self.FlyMissionTwice, self.FlyMissionTwiceWithReset, self.MissionIndexValidity, From 70729df4c0891a88c285c7b08d79b9868f1ac8d5 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 18 Jul 2024 10:46:34 +1000 Subject: [PATCH 011/124] AP_HAL: enable Solo Gimbal in SITL --- libraries/AP_HAL/board/sitl.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/libraries/AP_HAL/board/sitl.h b/libraries/AP_HAL/board/sitl.h index 0eae8e932a3c1b..2710a78ff4ccb5 100644 --- a/libraries/AP_HAL/board/sitl.h +++ b/libraries/AP_HAL/board/sitl.h @@ -90,3 +90,5 @@ #ifndef AP_FILTER_ENABLED #define AP_FILTER_ENABLED 1 #endif + +#define HAL_SOLO_GIMBAL_ENABLED 1 From 16a9e53bdb5933de2995bec1d07123deb3374682 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 18 Jul 2024 10:48:29 +1000 Subject: [PATCH 012/124] AP_Mount: tidy Solo defines --- libraries/AP_Mount/AP_Mount_SoloGimbal.cpp | 5 ++++- libraries/AP_Mount/AP_Mount_SoloGimbal.h | 6 ++---- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp b/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp index b188f945a2f23f..02c8c3f2475c1c 100644 --- a/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp +++ b/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp @@ -1,6 +1,9 @@ -#include "AP_Mount_SoloGimbal.h" +#include "AP_Mount_config.h" + #if HAL_SOLO_GIMBAL_ENABLED +#include "AP_Mount_SoloGimbal.h" + #include "SoloGimbal.h" #include #include diff --git a/libraries/AP_Mount/AP_Mount_SoloGimbal.h b/libraries/AP_Mount/AP_Mount_SoloGimbal.h index 4e64233284a9fd..ed32e6697bef67 100644 --- a/libraries/AP_Mount/AP_Mount_SoloGimbal.h +++ b/libraries/AP_Mount/AP_Mount_SoloGimbal.h @@ -3,18 +3,16 @@ */ #pragma once +#include "AP_Mount_config.h" -#include +#if HAL_SOLO_GIMBAL_ENABLED #include "AP_Mount_Backend.h" -#if HAL_SOLO_GIMBAL_ENABLED #include #include #include -#include #include "SoloGimbal.h" - class AP_Mount_SoloGimbal : public AP_Mount_Backend { From 2584cfd786305962eac462278b9541ed379014dc Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 18 Jul 2024 11:22:56 +1000 Subject: [PATCH 013/124] SITL: use distinct source system for gimbal gimbal was sending mavlink into ArduPilot with the target system's own sysid/compid tuple. ArduPilot was simply discarding these as its own messages being looped back to it --- libraries/SITL/SIM_Gimbal.cpp | 6 +++--- libraries/SITL/SIM_Gimbal.h | 3 +++ 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/libraries/SITL/SIM_Gimbal.cpp b/libraries/SITL/SIM_Gimbal.cpp index 5aede11b3b40a3..649159da9fea03 100644 --- a/libraries/SITL/SIM_Gimbal.cpp +++ b/libraries/SITL/SIM_Gimbal.cpp @@ -237,7 +237,7 @@ void Gimbal::param_send(const struct gimbal_param *p) param_value.param_type = MAV_PARAM_TYPE_REAL32; uint16_t len = mavlink_msg_param_value_encode_status(vehicle_system_id, - vehicle_component_id, + gimbal_component_id, &mavlink.status, &msg, ¶m_value); @@ -364,7 +364,7 @@ void Gimbal::send_report(void) heartbeat.custom_mode = 0; len = mavlink_msg_heartbeat_encode_status(vehicle_system_id, - vehicle_component_id, + gimbal_component_id, &mavlink.status, &msg, &heartbeat); @@ -394,7 +394,7 @@ void Gimbal::send_report(void) gimbal_report.joint_az = joint_angles.z; len = mavlink_msg_gimbal_report_encode_status(vehicle_system_id, - vehicle_component_id, + gimbal_component_id, &mavlink.status, &msg, &gimbal_report); diff --git a/libraries/SITL/SIM_Gimbal.h b/libraries/SITL/SIM_Gimbal.h index 2cedd3a1cf9c3f..f77547af0105a9 100644 --- a/libraries/SITL/SIM_Gimbal.h +++ b/libraries/SITL/SIM_Gimbal.h @@ -113,6 +113,9 @@ class Gimbal { uint32_t param_send_last_ms; uint8_t param_send_idx; + // component ID we send from: + const uint8_t gimbal_component_id = 154; // MAV_COMP_ID_GIMBAL + void send_report(void); void param_send(const struct gimbal_param *p); struct gimbal_param *param_find(const char *name); From dcf342d7e3c765f811a2d7c1ed523413e665b786 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 18 Jul 2024 13:30:39 +1000 Subject: [PATCH 014/124] SITL: add instructions on testing simulated mavlink gimbal --- libraries/SITL/SIM_Gimbal.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/libraries/SITL/SIM_Gimbal.h b/libraries/SITL/SIM_Gimbal.h index f77547af0105a9..e69c6f25916b33 100644 --- a/libraries/SITL/SIM_Gimbal.h +++ b/libraries/SITL/SIM_Gimbal.h @@ -14,6 +14,11 @@ */ /* gimbal simulator class + +./Tools/autotest/sim_vehicle.py -D -G -v ArduCopter --mavlink-gimbal +param set MNT1_TYPE 2 +param set RC6_OPTION 213 # MOUNT1_PITCH +rc 6 1818 # for neutral pitch input */ #pragma once From 9c1fe4e1c982a56867c9e3d94a5dda8b6f87be34 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 19 Jul 2024 12:30:56 +1000 Subject: [PATCH 015/124] SIM_VectorNav: stop using nmea_printf on buffer data can't use nmea_printf here as the buffer data won't be null-terminated --- libraries/SITL/SIM_VectorNav.cpp | 22 ++++++++++++++++------ 1 file changed, 16 insertions(+), 6 deletions(-) diff --git a/libraries/SITL/SIM_VectorNav.cpp b/libraries/SITL/SIM_VectorNav.cpp index ed4c1bcaedd53d..b3f9ecf6a4d89f 100644 --- a/libraries/SITL/SIM_VectorNav.cpp +++ b/libraries/SITL/SIM_VectorNav.cpp @@ -214,14 +214,24 @@ void VectorNav::update(void) } char receive_buf[50]; - const ssize_t n = read_from_autopilot(&receive_buf[0], ARRAY_SIZE(receive_buf)); - if (n > 0) { - if (strncmp(receive_buf, "$VNRRG,01", 9) == 0) { + ssize_t n = read_from_autopilot(&receive_buf[0], ARRAY_SIZE(receive_buf)); + if (n <= 0) { + return; + } + + // avoid parsing the NMEA stream here by making assumptions about + // how we receive configuration strings. Generally we can just + // echo back the configuration string to make the driver happy. + if (n >= 9) { + // intercept device-version query, respond with simulated version: + const char *ver_query_string = "$VNRRG,01"; + if (strncmp(receive_buf, ver_query_string, strlen(ver_query_string)) == 0) { nmea_printf("$VNRRG,01,VN-300-SITL"); - } else { - nmea_printf("$%s", receive_buf); + // consume the query so we don't "respond" twice: + memmove(&receive_buf[0], &receive_buf[strlen(ver_query_string)], n - strlen(ver_query_string)); + n -= strlen(ver_query_string); } } - + write_to_autopilot(receive_buf, n); } From aca9940534b451a3ce59aa36d6195d264e8b132c Mon Sep 17 00:00:00 2001 From: Willian Galvani Date: Sat, 20 Jul 2024 16:41:48 -0300 Subject: [PATCH 016/124] AP_Scripting: add Readme.md for generating message definitions --- libraries/AP_Scripting/modules/MAVLink/Readme.md | 6 ++++++ 1 file changed, 6 insertions(+) create mode 100644 libraries/AP_Scripting/modules/MAVLink/Readme.md diff --git a/libraries/AP_Scripting/modules/MAVLink/Readme.md b/libraries/AP_Scripting/modules/MAVLink/Readme.md new file mode 100644 index 00000000000000..97924a1c33a075 --- /dev/null +++ b/libraries/AP_Scripting/modules/MAVLink/Readme.md @@ -0,0 +1,6 @@ +These .lua definition for messages can be generated using pymavlink with the following command: +(change the last path acoordingly to place the files where you need them) +``` +cd ardupilot/modules/mavlink +python ./pymavlink/tools/mavgen.py --lang Lua ./message_definitions/v1.0/all.xml --out ./modules/MAVLink +``` \ No newline at end of file From ae4254bba870cf1f3f225f6c2f947d4b5af2c8ac Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 17 Jul 2024 14:47:48 +1000 Subject: [PATCH 017/124] SITL: rename SIM_Gimbal files to SIM_SoloGimbal --- libraries/SITL/{SIM_Gimbal.cpp => SIM_SoloGimbal.cpp} | 0 libraries/SITL/{SIM_Gimbal.h => SIM_SoloGimbal.h} | 0 2 files changed, 0 insertions(+), 0 deletions(-) rename libraries/SITL/{SIM_Gimbal.cpp => SIM_SoloGimbal.cpp} (100%) rename libraries/SITL/{SIM_Gimbal.h => SIM_SoloGimbal.h} (100%) diff --git a/libraries/SITL/SIM_Gimbal.cpp b/libraries/SITL/SIM_SoloGimbal.cpp similarity index 100% rename from libraries/SITL/SIM_Gimbal.cpp rename to libraries/SITL/SIM_SoloGimbal.cpp diff --git a/libraries/SITL/SIM_Gimbal.h b/libraries/SITL/SIM_SoloGimbal.h similarity index 100% rename from libraries/SITL/SIM_Gimbal.h rename to libraries/SITL/SIM_SoloGimbal.h From 1ce6aa7e38d022c46f6f555e3f457888c19b4f11 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 17 Jul 2024 16:46:42 +1000 Subject: [PATCH 018/124] AP_HAL: adjust for renaming of Gimbal to SoloGimbal --- libraries/AP_HAL/SIMState.cpp | 2 +- libraries/AP_HAL/SIMState.h | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/AP_HAL/SIMState.cpp b/libraries/AP_HAL/SIMState.cpp index 46d7ea4796eba3..f5c47834220047 100644 --- a/libraries/AP_HAL/SIMState.cpp +++ b/libraries/AP_HAL/SIMState.cpp @@ -131,7 +131,7 @@ void SIMState::fdm_input_local(void) // output JSON state to ride along flight controllers // ride_along.send(_sitl->state,sitl_model->get_position_relhome()); -#if HAL_SIM_GIMBAL_ENABLED +#if AP_SIM_SOLOGIMBAL_ENABLED if (gimbal != nullptr) { gimbal->update(); } diff --git a/libraries/AP_HAL/SIMState.h b/libraries/AP_HAL/SIMState.h index 0c84791f91ea39..f4343475715d2f 100644 --- a/libraries/AP_HAL/SIMState.h +++ b/libraries/AP_HAL/SIMState.h @@ -7,7 +7,7 @@ #include #include -#include +#include #include #include #include @@ -109,10 +109,10 @@ class AP_HAL::SIMState { // internal SITL model SITL::Aircraft *sitl_model; -#if HAL_SIM_GIMBAL_ENABLED +#if AP_SIM_SOLOGIMBAL_ENABLED // simulated gimbal bool enable_gimbal; - SITL::Gimbal *gimbal; + SITL::SoloGimbal *gimbal; #endif #if HAL_SIM_ADSB_ENABLED From 9b3809c89ff1c5c3f726dd81ed86b465725ae928 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 17 Jul 2024 16:46:42 +1000 Subject: [PATCH 019/124] AP_HAL_SITL: adjust for renaming of Gimbal to SoloGimbal --- libraries/AP_HAL_SITL/SITL_State.cpp | 4 +++- libraries/AP_HAL_SITL/SITL_State_common.cpp | 2 +- libraries/AP_HAL_SITL/SITL_State_common.h | 8 ++++---- 3 files changed, 8 insertions(+), 6 deletions(-) diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp index 2d49bb606b07cd..dcb11bb79543f6 100644 --- a/libraries/AP_HAL_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_State.cpp @@ -79,9 +79,11 @@ void SITL_State::_sitl_setup() if (_sitl != nullptr) { // setup some initial values _update_airspeed(0); +#if AP_SIM_SOLOGIMBAL_ENABLED if (enable_gimbal) { - gimbal = NEW_NOTHROW SITL::Gimbal(_sitl->state); + gimbal = NEW_NOTHROW SITL::SoloGimbal(_sitl->state); } +#endif sitl_model->set_buzzer(&_sitl->buzzer_sim); sitl_model->set_sprayer(&_sitl->sprayer_sim); diff --git a/libraries/AP_HAL_SITL/SITL_State_common.cpp b/libraries/AP_HAL_SITL/SITL_State_common.cpp index 1a45ed97d97b67..cf143e28bd3c73 100644 --- a/libraries/AP_HAL_SITL/SITL_State_common.cpp +++ b/libraries/AP_HAL_SITL/SITL_State_common.cpp @@ -332,7 +332,7 @@ SITL::SerialDevice *SITL_State_Common::create_serial_sim(const char *name, const */ void SITL_State_Common::sim_update(void) { -#if HAL_SIM_GIMBAL_ENABLED +#if AP_SIM_SOLOGIMBAL_ENABLED if (gimbal != nullptr) { gimbal->update(); } diff --git a/libraries/AP_HAL_SITL/SITL_State_common.h b/libraries/AP_HAL_SITL/SITL_State_common.h index 7bb4c04348f556..95ef129c1cce7a 100644 --- a/libraries/AP_HAL_SITL/SITL_State_common.h +++ b/libraries/AP_HAL_SITL/SITL_State_common.h @@ -9,7 +9,7 @@ #define SITL_SERVO_PORT 20722 #include -#include +#include #include #include #include @@ -104,11 +104,11 @@ class HALSITL::SITL_State_Common { bool new_rc_input; uint16_t pwm_output[SITL_NUM_CHANNELS]; bool output_ready = false; - -#if HAL_SIM_GIMBAL_ENABLED + +#if AP_SIM_SOLOGIMBAL_ENABLED // simulated gimbal bool enable_gimbal; - SITL::Gimbal *gimbal; + SITL::SoloGimbal *gimbal; #endif #if HAL_SIM_ADSB_ENABLED From fc28e2d7b8537889ebf3f75d8fef00d256c6e2b8 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 17 Jul 2024 16:46:42 +1000 Subject: [PATCH 020/124] SITL: adjust for renaming of Gimbal to SoloGimbal --- libraries/SITL/SIM_SoloGimbal.cpp | 26 ++++++++++++-------------- libraries/SITL/SIM_SoloGimbal.h | 19 ++++++++----------- libraries/SITL/SIM_config.h | 4 ++++ 3 files changed, 24 insertions(+), 25 deletions(-) diff --git a/libraries/SITL/SIM_SoloGimbal.cpp b/libraries/SITL/SIM_SoloGimbal.cpp index 649159da9fea03..123469344beba3 100644 --- a/libraries/SITL/SIM_SoloGimbal.cpp +++ b/libraries/SITL/SIM_SoloGimbal.cpp @@ -16,16 +16,14 @@ gimbal simulator class for MAVLink gimbal */ -#include "SIM_Gimbal.h" +#include "SIM_SoloGimbal.h" -#if HAL_SIM_GIMBAL_ENABLED +#if AP_SIM_SOLOGIMBAL_ENABLED #include #include "SIM_Aircraft.h" -#include - extern const AP_HAL::HAL& hal; #define GIMBAL_DEBUG 0 @@ -38,7 +36,7 @@ extern const AP_HAL::HAL& hal; namespace SITL { -Gimbal::Gimbal(const struct sitl_fdm &_fdm) : +SoloGimbal::SoloGimbal(const struct sitl_fdm &_fdm) : fdm(_fdm), target_address("127.0.0.1"), target_port(5762), @@ -58,7 +56,7 @@ Gimbal::Gimbal(const struct sitl_fdm &_fdm) : /* update the gimbal state */ -void Gimbal::update(void) +void SoloGimbal::update(void) { // calculate delta time in seconds uint32_t now_us = AP_HAL::micros(); @@ -213,7 +211,7 @@ static struct gimbal_param { /* find a parameter structure */ -struct gimbal_param *Gimbal::param_find(const char *name) +struct gimbal_param *SoloGimbal::param_find(const char *name) { for (uint8_t i=0; i +#include "SIM_config.h" -#ifndef HAL_SIM_GIMBAL_ENABLED -#define HAL_SIM_GIMBAL_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) && !defined(HAL_BUILD_AP_PERIPH) -#endif - -#if HAL_SIM_GIMBAL_ENABLED +#if AP_SIM_SOLOGIMBAL_ENABLED +#include +#include #include -#include "SIM_Aircraft.h" - namespace SITL { -class Gimbal { +class SoloGimbal { public: - Gimbal(const struct sitl_fdm &_fdm); + SoloGimbal(const struct sitl_fdm &_fdm); void update(void); private: @@ -128,4 +124,5 @@ class Gimbal { } // namespace SITL -#endif // HAL_SIM_GIMBAL_ENABLED +#endif // AP_SIM_SOLOGIMBAL_ENABLED + diff --git a/libraries/SITL/SIM_config.h b/libraries/SITL/SIM_config.h index bd5d07e799c50f..21986735f89236 100644 --- a/libraries/SITL/SIM_config.h +++ b/libraries/SITL/SIM_config.h @@ -121,3 +121,7 @@ #ifndef AP_SIM_GLIDER_ENABLED #define AP_SIM_GLIDER_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) #endif + +#ifndef AP_SIM_SOLOGIMBAL_ENABLED +#define AP_SIM_SOLOGIMBAL_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) +#endif From ed3aeb39fd61d0b9c420956b936e1e65de47a045 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 17 Jul 2024 16:52:20 +1000 Subject: [PATCH 021/124] AP_HAL_SITL: split MAVLink and physical gimbal simulations --- libraries/AP_HAL_SITL/SITL_State.cpp | 2 +- libraries/AP_HAL_SITL/SITL_State_common.cpp | 2 +- libraries/AP_HAL_SITL/SITL_cmdline.cpp | 2 ++ 3 files changed, 4 insertions(+), 2 deletions(-) diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp index dcb11bb79543f6..af55acaa1f8443 100644 --- a/libraries/AP_HAL_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_State.cpp @@ -81,7 +81,7 @@ void SITL_State::_sitl_setup() _update_airspeed(0); #if AP_SIM_SOLOGIMBAL_ENABLED if (enable_gimbal) { - gimbal = NEW_NOTHROW SITL::SoloGimbal(_sitl->state); + gimbal = NEW_NOTHROW SITL::SoloGimbal(); } #endif diff --git a/libraries/AP_HAL_SITL/SITL_State_common.cpp b/libraries/AP_HAL_SITL/SITL_State_common.cpp index cf143e28bd3c73..54a530bc447b17 100644 --- a/libraries/AP_HAL_SITL/SITL_State_common.cpp +++ b/libraries/AP_HAL_SITL/SITL_State_common.cpp @@ -334,7 +334,7 @@ void SITL_State_Common::sim_update(void) { #if AP_SIM_SOLOGIMBAL_ENABLED if (gimbal != nullptr) { - gimbal->update(); + gimbal->update(*sitl_model); } #endif #if HAL_SIM_ADSB_ENABLED diff --git a/libraries/AP_HAL_SITL/SITL_cmdline.cpp b/libraries/AP_HAL_SITL/SITL_cmdline.cpp index 30cea03245060d..042bb2ce7f2128 100644 --- a/libraries/AP_HAL_SITL/SITL_cmdline.cpp +++ b/libraries/AP_HAL_SITL/SITL_cmdline.cpp @@ -439,9 +439,11 @@ void SITL_State::_parse_command_line(int argc, char * const argv[]) case 'v': vehicle_str = gopt.optarg; break; +#if AP_SIM_SOLOGIMBAL_ENABLED case CMDLINE_GIMBAL: enable_gimbal = true; break; +#endif case CMDLINE_FGVIEW: _use_fg_view = true; break; From ba135b90080dd1ec5d596dad66382eb1b0591dcb Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 17 Jul 2024 16:52:20 +1000 Subject: [PATCH 022/124] SITL: split MAVLink and physical gimbal simulations --- libraries/SITL/SIM_Gimbal.cpp | 184 ++++++++++++++++++++++++++++++ libraries/SITL/SIM_Gimbal.h | 96 ++++++++++++++++ libraries/SITL/SIM_SoloGimbal.cpp | 163 +++----------------------- libraries/SITL/SIM_SoloGimbal.h | 62 ++-------- libraries/SITL/SIM_config.h | 7 +- 5 files changed, 313 insertions(+), 199 deletions(-) create mode 100644 libraries/SITL/SIM_Gimbal.cpp create mode 100644 libraries/SITL/SIM_Gimbal.h diff --git a/libraries/SITL/SIM_Gimbal.cpp b/libraries/SITL/SIM_Gimbal.cpp new file mode 100644 index 00000000000000..a905a38d731d09 --- /dev/null +++ b/libraries/SITL/SIM_Gimbal.cpp @@ -0,0 +1,184 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + gimbal simulator class for gimbal +*/ + +#include "SIM_Gimbal.h" + +#if AP_SIM_GIMBAL_ENABLED + +#include + +#include "SIM_Aircraft.h" + +extern const AP_HAL::HAL& hal; + +#define GIMBAL_DEBUG 0 + +#if GIMBAL_DEBUG +#define debug(fmt, args...) do { printf("GIMBAL: " fmt, ##args); } while(0) +#else +#define debug(fmt, args...) do { } while(0) +#endif + +namespace SITL { + +/* + update the gimbal state +*/ +void Gimbal::update(const class Aircraft &aircraft) +{ + // calculate delta time in seconds + uint32_t now_us = AP_HAL::micros(); + + float delta_t = (now_us - last_update_us) * 1.0e-6f; + last_update_us = now_us; + + const Matrix3f &vehicle_dcm = aircraft.get_dcm(); + if (!init_done) { + dcm = vehicle_dcm; + } + + const Vector3f &vehicle_gyro = AP::ins().get_gyro(); + const Vector3f &vehicle_accel_body = AP::ins().get_accel(); + + // take a copy of the demanded rates to bypass the limiter function for testing + Vector3f demRateRaw = demanded_angular_rate; + + // 1) Rotate the copters rotation rates into the gimbals frame of reference + // copterAngRate_G = transpose(DCMgimbal)*DCMcopter*copterAngRate + Vector3f copterAngRate_G = dcm.transposed()*vehicle_dcm*vehicle_gyro; + + // 2) Subtract the copters body rates to obtain a copter relative rotational + // rate vector (X,Y,Z) in gimbal sensor frame + // relativeGimbalRate(X,Y,Z) = gimbalRateDemand - copterAngRate_G + Vector3f relativeGimbalRate = demanded_angular_rate - copterAngRate_G; + + // calculate joint angles (euler312 order) + // calculate copter -> gimbal rotation matrix + Matrix3f rotmat_copter_gimbal = dcm.transposed() * vehicle_dcm; + + joint_angles = rotmat_copter_gimbal.transposed().to_euler312(); + + /* 4) For each of the three joints, calculate upper and lower rate limits + from the corresponding angle limits and current joint angles + + upperRatelimit = (jointAngle - lowerAngleLimit) * travelLimitGain + lowerRatelimit = (jointAngle - upperAngleLimit) * travelLimitGain + + travelLimitGain is equal to the inverse of the bump stop time constant and + should be set to something like 20 initially. If set too high it can cause + the rates to 'ring' when they the limiter is in force, particularly given + we are using a first order numerical integration. + */ + Vector3f upperRatelimit = -(joint_angles - upper_joint_limits) * travelLimitGain; + Vector3f lowerRatelimit = -(joint_angles - lower_joint_limits) * travelLimitGain; + + /* + 5) Calculate the gimbal joint rates (roll, elevation, azimuth) + + gimbalJointRates(roll, elev, azimuth) = Matrix*relativeGimbalRate(X,Y,Z) + + where matrix = + +- -+ + | cos(elevAngle), 0, sin(elevAngle) | + | | + | sin(elevAngle) tan(rollAngle), 1, -cos(elevAngle) tan(rollAngle) | + | | + | sin(elevAngle) cos(elevAngle) | + | - --------------, 0, -------------- | + | cos(rollAngle) cos(rollAngle) | + +- -+ + */ + float rollAngle = joint_angles.x; + float elevAngle = joint_angles.y; + Matrix3f matrix = Matrix3f(Vector3f(cosf(elevAngle), 0, sinf(elevAngle)), + Vector3f(sinf(elevAngle)*tanf(rollAngle), 1, -cosf(elevAngle)*tanf(rollAngle)), + Vector3f(-sinf(elevAngle)/cosf(rollAngle), 0, cosf(elevAngle)/cosf(rollAngle))); + Vector3f gimbalJointRates = matrix * relativeGimbalRate; + + // 6) Apply the rate limits from 4) + gimbalJointRates.x = constrain_float(gimbalJointRates.x, lowerRatelimit.x, upperRatelimit.x); + gimbalJointRates.y = constrain_float(gimbalJointRates.y, lowerRatelimit.y, upperRatelimit.y); + gimbalJointRates.z = constrain_float(gimbalJointRates.z, lowerRatelimit.z, upperRatelimit.z); + /* + 7) Convert the modified gimbal joint rates to body rates (still copter + relative) + relativeGimbalRate(X,Y,Z) = Matrix * gimbalJointRates(roll, elev, azimuth) + + where Matrix = + + +- -+ + | cos(elevAngle), 0, -cos(rollAngle) sin(elevAngle) | + | | + | 0, 1, sin(rollAngle) | + | | + | sin(elevAngle), 0, cos(elevAngle) cos(rollAngle) | + +- -+ + */ + matrix = Matrix3f(Vector3f(cosf(elevAngle), 0, -cosf(rollAngle)*sinf(elevAngle)), + Vector3f(0, 1, sinf(rollAngle)), + Vector3f(sinf(elevAngle), 0, cosf(elevAngle)*cosf(rollAngle))); + relativeGimbalRate = matrix * gimbalJointRates; + + // 8) Add to the result from step 1) to obtain the demanded gimbal body rates + // in an inertial frame of reference + // demandedGimbalRatesInertial(X,Y,Z) = relativeGimbalRate(X,Y,Z) + copterAngRate_G + // Vector3f demandedGimbalRatesInertial = relativeGimbalRate + copterAngRate_G; + + // for the moment we will set gyros equal to demanded_angular_rate + gimbal_angular_rate = demRateRaw; // demandedGimbalRatesInertial + true_gyro_bias - supplied_gyro_bias + + // update rotation of the gimbal + dcm.rotate(gimbal_angular_rate*delta_t); + dcm.normalize(); + + // calculate copter -> gimbal rotation matrix + rotmat_copter_gimbal = dcm.transposed() * vehicle_dcm; + + // calculate joint angles (euler312 order) + joint_angles = rotmat_copter_gimbal.transposed().to_euler312(); + + // update observed gyro + gyro = gimbal_angular_rate + true_gyro_bias; + + // update delta_angle (integrate) + delta_angle += gyro * delta_t; + + // calculate accel in gimbal body frame + Vector3f copter_accel_earth = vehicle_dcm * vehicle_accel_body; + Vector3f accel = dcm.transposed() * copter_accel_earth; + + // integrate velocity + delta_velocity += accel * delta_t; +} + +void Gimbal::get_deltas(Vector3f &_delta_angle, Vector3f &_delta_velocity, uint32_t &_delta_time_us) +{ + const uint32_t now_us = AP_HAL::micros(); + + _delta_angle = delta_angle; + _delta_velocity = delta_velocity; + _delta_time_us = now_us - delta_start_us; + + delta_angle.zero(); + delta_velocity.zero(); + delta_start_us = now_us; +} + +} // namespace SITL + +#endif // AP_SIM_GIMBAL_ENABLED diff --git a/libraries/SITL/SIM_Gimbal.h b/libraries/SITL/SIM_Gimbal.h new file mode 100644 index 00000000000000..d3814383b69ba1 --- /dev/null +++ b/libraries/SITL/SIM_Gimbal.h @@ -0,0 +1,96 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + gimbal simulator class +*/ + +#pragma once + +#include "SIM_config.h" + +#if AP_SIM_GIMBAL_ENABLED + +#include +#include + +namespace SITL { + +class Gimbal { +public: + + void update(const class Aircraft &aircraft); + void set_demanded_rates(const Vector3f &rates) { + demanded_angular_rate = rates; + } + + void get_deltas(Vector3f &_delta_angle, Vector3f &_delta_velocity, uint32_t &_delta_time_us); + void get_joint_angles(Vector3f &_angles) { _angles = joint_angles; } + +private: + + // rotation matrix (gimbal body -> earth) + Matrix3f dcm; + bool init_done; + + // time of last update + uint32_t last_update_us; + + // true angular rate of gimbal in body frame (rad/s) + Vector3f gimbal_angular_rate; + + // observed angular rate (including biases) + Vector3f gyro; + + /* joint angles, in radians. in yaw/roll/pitch order. Relative to fwd. + So 0,0,0 points forward. + Pi/2,0,0 means pointing right + 0, Pi/2, 0 means pointing fwd, but rolled 90 degrees to right + 0, 0, -Pi/2, means pointing down + */ + Vector3f joint_angles; + + // physical constraints on joint angles in (roll, pitch, azimuth) order + Vector3f lower_joint_limits{radians(-40), radians(-135), radians(-7.5)}; + Vector3f upper_joint_limits{radians(40), radians(45), radians(7.5)}; + + const float travelLimitGain = 20; + + // true gyro bias + Vector3f true_gyro_bias; + + // time since delta angles/velocities returned + uint32_t delta_start_us; + + // integral of gyro vector over last time interval. In radians + Vector3f delta_angle; + + // integral of accel vector over last time interval. In m/s + Vector3f delta_velocity; + + /* + control variables from the vehicle + */ + // angular rate in rad/s. In body frame of gimbal + Vector3f demanded_angular_rate; + + // gyro bias provided by EKF on vehicle. In rad/s. + // Should be subtracted from the gyro readings to get true body + // rotatation rates + // Vector3f supplied_gyro_bias; +}; + +} // namespace SITL + +#endif // AP_SIM_GIMBAL_ENABLED diff --git a/libraries/SITL/SIM_SoloGimbal.cpp b/libraries/SITL/SIM_SoloGimbal.cpp index 123469344beba3..dccf15727249ea 100644 --- a/libraries/SITL/SIM_SoloGimbal.cpp +++ b/libraries/SITL/SIM_SoloGimbal.cpp @@ -36,149 +36,12 @@ extern const AP_HAL::HAL& hal; namespace SITL { -SoloGimbal::SoloGimbal(const struct sitl_fdm &_fdm) : - fdm(_fdm), - target_address("127.0.0.1"), - target_port(5762), - lower_joint_limits(radians(-40), radians(-135), radians(-7.5)), - upper_joint_limits(radians(40), radians(45), radians(7.5)), - travelLimitGain(20), - reporting_period_ms(10), - seen_heartbeat(false), - seen_gimbal_control(false), - mav_socket(false) -{ - memset(&mavlink, 0, sizeof(mavlink)); - fdm.quaternion.rotation_matrix(dcm); -} - - /* update the gimbal state */ -void SoloGimbal::update(void) +void SoloGimbal::update(const Aircraft &aircraft) { - // calculate delta time in seconds - uint32_t now_us = AP_HAL::micros(); - - float delta_t = (now_us - last_update_us) * 1.0e-6f; - last_update_us = now_us; - - Matrix3f vehicle_dcm; - fdm.quaternion.rotation_matrix(vehicle_dcm); - - const Vector3f &vehicle_gyro = AP::ins().get_gyro(); - const Vector3f &vehicle_accel_body = AP::ins().get_accel(); - - // take a copy of the demanded rates to bypass the limiter function for testing - Vector3f demRateRaw = demanded_angular_rate; - - // 1) Rotate the copters rotation rates into the gimbals frame of reference - // copterAngRate_G = transpose(DCMgimbal)*DCMcopter*copterAngRate - Vector3f copterAngRate_G = dcm.transposed()*vehicle_dcm*vehicle_gyro; - - // 2) Subtract the copters body rates to obtain a copter relative rotational - // rate vector (X,Y,Z) in gimbal sensor frame - // relativeGimbalRate(X,Y,Z) = gimbalRateDemand - copterAngRate_G - Vector3f relativeGimbalRate = demanded_angular_rate - copterAngRate_G; - - // calculate joint angles (euler312 order) - // calculate copter -> gimbal rotation matrix - Matrix3f rotmat_copter_gimbal = dcm.transposed() * vehicle_dcm; - - joint_angles = rotmat_copter_gimbal.transposed().to_euler312(); - - /* 4) For each of the three joints, calculate upper and lower rate limits - from the corresponding angle limits and current joint angles - - upperRatelimit = (jointAngle - lowerAngleLimit) * travelLimitGain - lowerRatelimit = (jointAngle - upperAngleLimit) * travelLimitGain - - travelLimitGain is equal to the inverse of the bump stop time constant and - should be set to something like 20 initially. If set too high it can cause - the rates to 'ring' when they the limiter is in force, particularly given - we are using a first order numerical integration. - */ - Vector3f upperRatelimit = -(joint_angles - upper_joint_limits) * travelLimitGain; - Vector3f lowerRatelimit = -(joint_angles - lower_joint_limits) * travelLimitGain; - - /* - 5) Calculate the gimbal joint rates (roll, elevation, azimuth) - - gimbalJointRates(roll, elev, azimuth) = Matrix*relativeGimbalRate(X,Y,Z) - - where matrix = - +- -+ - | cos(elevAngle), 0, sin(elevAngle) | - | | - | sin(elevAngle) tan(rollAngle), 1, -cos(elevAngle) tan(rollAngle) | - | | - | sin(elevAngle) cos(elevAngle) | - | - --------------, 0, -------------- | - | cos(rollAngle) cos(rollAngle) | - +- -+ - */ - float rollAngle = joint_angles.x; - float elevAngle = joint_angles.y; - Matrix3f matrix = Matrix3f(Vector3f(cosf(elevAngle), 0, sinf(elevAngle)), - Vector3f(sinf(elevAngle)*tanf(rollAngle), 1, -cosf(elevAngle)*tanf(rollAngle)), - Vector3f(-sinf(elevAngle)/cosf(rollAngle), 0, cosf(elevAngle)/cosf(rollAngle))); - Vector3f gimbalJointRates = matrix * relativeGimbalRate; - - // 6) Apply the rate limits from 4) - gimbalJointRates.x = constrain_float(gimbalJointRates.x, lowerRatelimit.x, upperRatelimit.x); - gimbalJointRates.y = constrain_float(gimbalJointRates.y, lowerRatelimit.y, upperRatelimit.y); - gimbalJointRates.z = constrain_float(gimbalJointRates.z, lowerRatelimit.z, upperRatelimit.z); - /* - 7) Convert the modified gimbal joint rates to body rates (still copter - relative) - relativeGimbalRate(X,Y,Z) = Matrix * gimbalJointRates(roll, elev, azimuth) - - where Matrix = - - +- -+ - | cos(elevAngle), 0, -cos(rollAngle) sin(elevAngle) | - | | - | 0, 1, sin(rollAngle) | - | | - | sin(elevAngle), 0, cos(elevAngle) cos(rollAngle) | - +- -+ - */ - matrix = Matrix3f(Vector3f(cosf(elevAngle), 0, -cosf(rollAngle)*sinf(elevAngle)), - Vector3f(0, 1, sinf(rollAngle)), - Vector3f(sinf(elevAngle), 0, cosf(elevAngle)*cosf(rollAngle))); - relativeGimbalRate = matrix * gimbalJointRates; - - // 8) Add to the result from step 1) to obtain the demanded gimbal body rates - // in an inertial frame of reference - // demandedGimbalRatesInertial(X,Y,Z) = relativeGimbalRate(X,Y,Z) + copterAngRate_G - // Vector3f demandedGimbalRatesInertial = relativeGimbalRate + copterAngRate_G; - - // for the moment we will set gyros equal to demanded_angular_rate - gimbal_angular_rate = demRateRaw; // demandedGimbalRatesInertial + true_gyro_bias - supplied_gyro_bias - - // update rotation of the gimbal - dcm.rotate(gimbal_angular_rate*delta_t); - dcm.normalize(); - - // calculate copter -> gimbal rotation matrix - rotmat_copter_gimbal = dcm.transposed() * vehicle_dcm; - - // calculate joint angles (euler312 order) - joint_angles = rotmat_copter_gimbal.transposed().to_euler312(); - - // update observed gyro - gyro = gimbal_angular_rate + true_gyro_bias; - - // update delta_angle (integrate) - delta_angle += gyro * delta_t; - - // calculate accel in gimbal body frame - Vector3f copter_accel_earth = vehicle_dcm * vehicle_accel_body; - Vector3f accel = dcm.transposed() * copter_accel_earth; - - // integrate velocity - delta_velocity += accel * delta_t; + gimbal.update(aircraft); // see if we should do a report send_report(); @@ -306,11 +169,9 @@ void SoloGimbal::send_report(void) } mavlink_gimbal_control_t pkt; mavlink_msg_gimbal_control_decode(&msg, &pkt); - demanded_angular_rate = Vector3f(pkt.demanded_rate_x, - pkt.demanded_rate_y, - pkt.demanded_rate_z); - // no longer supply a bias - supplied_gyro_bias.zero(); + gimbal.set_demanded_rates(Vector3f(pkt.demanded_rate_x, + pkt.demanded_rate_y, + pkt.demanded_rate_z)); seen_gimbal_control = true; break; } @@ -375,12 +236,20 @@ void SoloGimbal::send_report(void) */ uint32_t now_us = AP_HAL::micros(); if (now_us - last_report_us > reporting_period_ms*1000UL) { - mavlink_gimbal_report_t gimbal_report; - float delta_time = (now_us - last_report_us) * 1.0e-6f; last_report_us = now_us; + + uint32_t delta_time_us; + Vector3f delta_angle; + Vector3f delta_velocity; + gimbal.get_deltas(delta_angle, delta_velocity, delta_time_us); + + Vector3f joint_angles; + gimbal.get_joint_angles(joint_angles); + + mavlink_gimbal_report_t gimbal_report; gimbal_report.target_system = vehicle_system_id; gimbal_report.target_component = vehicle_component_id; - gimbal_report.delta_time = delta_time; + gimbal_report.delta_time = delta_time_us * 1e-6; gimbal_report.delta_angle_x = delta_angle.x; gimbal_report.delta_angle_y = delta_angle.y; gimbal_report.delta_angle_z = delta_angle.z; diff --git a/libraries/SITL/SIM_SoloGimbal.h b/libraries/SITL/SIM_SoloGimbal.h index e8600bbacd8d00..d3fdb6d2d2188a 100644 --- a/libraries/SITL/SIM_SoloGimbal.h +++ b/libraries/SITL/SIM_SoloGimbal.h @@ -27,6 +27,8 @@ rc 6 1818 # for neutral pitch input #if AP_SIM_SOLOGIMBAL_ENABLED +#include "SIM_Gimbal.h" + #include #include #include @@ -35,65 +37,23 @@ namespace SITL { class SoloGimbal { public: - SoloGimbal(const struct sitl_fdm &_fdm); - void update(void); - -private: - const struct sitl_fdm &fdm; - const char *target_address; - const uint16_t target_port; - - // rotation matrix (gimbal body -> earth) - Matrix3f dcm; - - // time of last update - uint32_t last_update_us; - - // true angular rate of gimbal in body frame (rad/s) - Vector3f gimbal_angular_rate; - // observed angular rate (including biases) - Vector3f gyro; + SoloGimbal() {} + void update(const Aircraft &aicraft); - /* joint angles, in radians. in yaw/roll/pitch order. Relative to fwd. - So 0,0,0 points forward. - Pi/2,0,0 means pointing right - 0, Pi/2, 0 means pointing fwd, but rolled 90 degrees to right - 0, 0, -Pi/2, means pointing down - */ - Vector3f joint_angles; - - // physical constraints on joint angles in (roll, pitch, azimuth) order - Vector3f lower_joint_limits; - Vector3f upper_joint_limits; +private: - const float travelLimitGain; + const char *target_address = "127.0.0.1"; + const uint16_t target_port = 5762; - // true gyro bias - Vector3f true_gyro_bias; + // physic simulation of gimbal: + Gimbal gimbal; // reporting variables. gimbal pushes these to vehicle code over // MAVLink at approx 100Hz // reporting period in ms - const float reporting_period_ms; - - // integral of gyro vector over last time interval. In radians - Vector3f delta_angle; - - // integral of accel vector over last time interval. In m/s - Vector3f delta_velocity; - - /* - control variables from the vehicle - */ - // angular rate in rad/s. In body frame of gimbal - Vector3f demanded_angular_rate; - - // gyro bias provided by EKF on vehicle. In rad/s. - // Should be subtracted from the gyro readings to get true body - // rotatation rates - Vector3f supplied_gyro_bias; + const float reporting_period_ms = 10; uint32_t last_report_us; uint32_t last_heartbeat_ms; @@ -102,7 +62,7 @@ class SoloGimbal { uint8_t vehicle_system_id; uint8_t vehicle_component_id; - SocketAPM_native mav_socket; + SocketAPM_native mav_socket{false}; struct { // socket to telem2 on aircraft bool connected; diff --git a/libraries/SITL/SIM_config.h b/libraries/SITL/SIM_config.h index 21986735f89236..c83e83d44767d9 100644 --- a/libraries/SITL/SIM_config.h +++ b/libraries/SITL/SIM_config.h @@ -123,5 +123,10 @@ #endif #ifndef AP_SIM_SOLOGIMBAL_ENABLED -#define AP_SIM_SOLOGIMBAL_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) +#define AP_SIM_SOLOGIMBAL_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL && HAL_MAVLINK_BINDINGS_ENABLED) #endif + +#ifndef AP_SIM_GIMBAL_ENABLED +#define AP_SIM_GIMBAL_ENABLED (AP_SIM_SOLOGIMBAL_ENABLED) +#endif + From 57d681512483f52723a2a916b89f6c0428ee80de Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 25 Mar 2023 18:04:38 +1100 Subject: [PATCH 023/124] autotest: add Mount test for relative yaw --- Tools/autotest/arducopter.py | 33 ++++++++++++++++++++++++++++----- 1 file changed, 28 insertions(+), 5 deletions(-) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index ade0324672ec4b..ccd8147ec045fb 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -5256,7 +5256,7 @@ def test_mount_pitch(self, despitch, despitch_tolerance, mount_mode, timeout=10, despitch = self.constrained_mount_pitch(despitch) '''retrieve latest angles from GIMBAL_DEVICE_ATTITUDE_STATUS''' - mount_roll, mount_pitch, mount_yaw = self.get_mount_roll_pitch_yaw_deg() + mount_roll, mount_pitch, mount_yaw, mount_yaw_is_absolute = self.get_mount_roll_pitch_yaw_deg() # self.progress("despitch=%f roll=%f pitch=%f yaw=%f" % (despitch, mount_roll, mount_pitch, mount_yaw)) if abs(despitch - mount_pitch) > despitch_tolerance: @@ -5314,10 +5314,11 @@ def get_mount_roll_pitch_yaw_deg(self): # wait for gimbal attitude message m = self.assert_receive_message('GIMBAL_DEVICE_ATTITUDE_STATUS', timeout=5) + yaw_is_absolute = m.flags & mavutil.mavlink.GIMBAL_DEVICE_FLAGS_YAW_LOCK # convert quaternion to euler angles and return q = quaternion.Quaternion(m.q) euler = q.euler - return math.degrees(euler[0]), math.degrees(euler[1]), math.degrees(euler[2]) + return math.degrees(euler[0]), math.degrees(euler[1]), math.degrees(euler[2]), yaw_is_absolute def set_mount_mode(self, mount_mode): '''set mount mode''' @@ -5435,7 +5436,7 @@ def mount_test_body(self, pitch_rc_neutral=1500, do_rate_tests=True, constrain_s self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_NEUTRAL) # test pitch is not neutral to start with - mount_roll_deg, mount_pitch_deg, mount_yaw_deg = self.get_mount_roll_pitch_yaw_deg() + mount_roll_deg, mount_pitch_deg, mount_yaw_deg, mount_yaw_is_absolute = self.get_mount_roll_pitch_yaw_deg() if mount_roll_deg != 0 or mount_pitch_deg != 0 or mount_yaw_deg != 0: raise NotAchievedException("Mount not neutral") @@ -5451,7 +5452,7 @@ def mount_test_body(self, pitch_rc_neutral=1500, do_rate_tests=True, constrain_s self.wait_pitch(despitch, despitch_tolerance) # check gimbal is still not stabilising - mount_roll_deg, mount_pitch_deg, mount_yaw_deg = self.get_mount_roll_pitch_yaw_deg() + mount_roll_deg, mount_pitch_deg, mount_yaw_deg, mount_yaw_is_absolute = self.get_mount_roll_pitch_yaw_deg() if mount_roll_deg != 0 or mount_pitch_deg != 0 or mount_yaw_deg != 0: raise NotAchievedException("Mount stabilising when not requested") @@ -5657,6 +5658,28 @@ def mount_test_body(self, pitch_rc_neutral=1500, do_rate_tests=True, constrain_s self.disarm_vehicle(force=True) + self.test_mount_body_yaw() + + def test_mount_body_yaw(self): + '''check reporting of yaw''' + # change mount to neutral mode (point forward, not stabilising) + self.takeoff(10, mode='GUIDED') + + self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_NEUTRAL) + + for heading in 30, 45, 150: + self.guided_achieve_heading(heading) + + r, p , y, yaw_is_absolute = self.get_mount_roll_pitch_yaw_deg() + + if yaw_is_absolute: + raise NotAchievedException("Expected a relative yaw") + + if y > 1: + raise NotAchievedException("Bad yaw (y=%f)") + + self.do_RTL() + def Mount(self): '''test servo mount''' self.setup_servo_mount() @@ -5680,7 +5703,7 @@ def MountSolo(self): def assert_mount_rpy(self, r, p, y, tolerance=1): '''assert mount atttiude in degrees''' - got_r, got_p, got_y = self.get_mount_roll_pitch_yaw_deg() + got_r, got_p, got_y, yaw_is_absolute = self.get_mount_roll_pitch_yaw_deg() for (want, got, name) in (r, got_r, "roll"), (p, got_p, "pitch"), (y, got_y, "yaw"): if abs(want - got) > tolerance: raise NotAchievedException("%s incorrect; want=%f got=%f" % From e94ebe4d1a398072e408ee00564b86686ff229bf Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 17 Jul 2024 13:07:50 +1000 Subject: [PATCH 024/124] AP_Scripting: add battery resistance to bindings --- libraries/AP_Scripting/generator/description/bindings.desc | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index 5528b04cf80b9b..dbeca527799586 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -94,6 +94,7 @@ singleton AP_BattMonitor depends AP_BATTERY_ENABLED singleton AP_BattMonitor method num_instances uint8_t singleton AP_BattMonitor method healthy boolean uint8_t 0 ud->num_instances() singleton AP_BattMonitor method voltage float uint8_t 0 ud->num_instances() +singleton AP_BattMonitor method get_resistance float uint8_t 0 ud->num_instances() singleton AP_BattMonitor method voltage_resting_estimate float uint8_t 0 ud->num_instances() singleton AP_BattMonitor method current_amps boolean float'Null uint8_t 0 ud->num_instances() singleton AP_BattMonitor method consumed_mah boolean float'Null uint8_t 0 ud->num_instances() From 6514a3522bb1292b1a9bc69c125a6e43dd24a66f Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 17 Jul 2024 13:08:09 +1000 Subject: [PATCH 025/124] AP_Scripting: add battery-resistance-checking example --- .../battery_internal_resistance_check.lua | 35 +++++++++++++++++++ 1 file changed, 35 insertions(+) create mode 100644 libraries/AP_Scripting/examples/battery_internal_resistance_check.lua diff --git a/libraries/AP_Scripting/examples/battery_internal_resistance_check.lua b/libraries/AP_Scripting/examples/battery_internal_resistance_check.lua new file mode 100644 index 00000000000000..63a811e2e2a573 --- /dev/null +++ b/libraries/AP_Scripting/examples/battery_internal_resistance_check.lua @@ -0,0 +1,35 @@ +--[[ + script implementing pre-arm check that internal resistance is sensible +--]] + +local MAX_RESISTANCE = 0.03 -- Ohms + +local auth_id = assert(arming:get_aux_auth_id()) + +local warning_last_sent_ms = uint32_t() -- time we last sent a warning message to the user +warning_interval_ms = 10000 + +function update() + local num_batts = battery:num_instances() + local ok = true + for i=0,num_batts do + local resistance = battery:get_resistance(i) + failed = resistance > MAX_RESISTANCE + if failed then + msg = string.format("Batt[%u] high internal resistance %.5f Ohms", i+1, resistance) + if millis() - warning_last_sent_ms > warning_interval_ms then + gcs:send_text(0, msg) + warning_last_sent_ms = millis() + end + arming:set_aux_auth_failed(auth_id, msg) + ok = false + end + end + + if ok then + arming:set_aux_auth_passed(auth_id) + end + return update, 500 +end + +return update() From 56773f09b422ece063b3531213e32618d04b1319 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 17 Jul 2024 18:02:51 +1000 Subject: [PATCH 026/124] AP_Scripting: docs for get_resistance --- libraries/AP_Scripting/docs/docs.lua | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index afd113d5f4c011..5fe0593f11139f 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/libraries/AP_Scripting/docs/docs.lua @@ -3147,6 +3147,11 @@ function battery:current_amps(instance) end ---@return number -- resting voltage function battery:voltage_resting_estimate(instance) end +-- Returns the estimated internal battery resistance in Ohms +---@param instance integer -- battery instance +---@return number -- estimated internal resistance in Ohms +function battery:get_resistance(instance) end + -- Returns the voltage of the selected battery instance. ---@param instance integer -- battery instance ---@return number -- voltage From 7b5c1f16c1bbc986244dd83c690801790d6bc680 Mon Sep 17 00:00:00 2001 From: Willian Galvani Date: Fri, 19 Jul 2024 23:54:59 -0300 Subject: [PATCH 027/124] tools: firmware_version_decoder.py: fix detection for BETA+n firmware --- Tools/scripts/firmware_version_decoder.py | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/Tools/scripts/firmware_version_decoder.py b/Tools/scripts/firmware_version_decoder.py index cf3321701490d7..3ef137f4e30c13 100755 --- a/Tools/scripts/firmware_version_decoder.py +++ b/Tools/scripts/firmware_version_decoder.py @@ -18,6 +18,18 @@ class FirmwareVersionType(enum.Enum): Official = 255 EnumEnd = 256 + @staticmethod + def get_release(version: int) -> str: + """ + Return the closest release type for a given version type, going down. + This is required because it is common in ardupilot to increase the version type + for successive betas, such as here: + https://github.com/ArduPilot/ardupilot/blame/8890c44370a7cf27d5efc872ef6da288ae3bc41f/ArduCopter/version.h#L12 + """ + for release in reversed(FirmwareVersionType): + if version >= release.value: + return release + return "Unknown" class VehicleType(enum.Enum): Rover = 1 @@ -193,7 +205,7 @@ def unpack_fwversion(self) -> None: self.fwversion.major = self.unpack("B") self.fwversion.minor = self.unpack("B") self.fwversion.patch = self.unpack("B") - self.fwversion.firmware_type = FirmwareVersionType(self.unpack("B")) + self.fwversion.firmware_type = FirmwareVersionType.get_release(self.unpack("B")) self.fwversion.os_software_version = self.unpack("I") self.fwversion.firmware_string = self.unpack_string_from_pointer() From fc8147ead0935677f4e10049d10a26042eeec555 Mon Sep 17 00:00:00 2001 From: chiara-septentrio Date: Tue, 25 Jun 2024 09:02:14 +0200 Subject: [PATCH 028/124] AP_GPS:Septentrio constellation choice --- libraries/AP_GPS/AP_GPS_SBF.cpp | 17 +++++++++++++++++ libraries/AP_GPS/AP_GPS_SBF.h | 1 + 2 files changed, 18 insertions(+) diff --git a/libraries/AP_GPS/AP_GPS_SBF.cpp b/libraries/AP_GPS/AP_GPS_SBF.cpp index e5a102998bf96f..4a18a3295526a3 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.cpp +++ b/libraries/AP_GPS/AP_GPS_SBF.cpp @@ -138,6 +138,19 @@ AP_GPS_SBF::read(void) config_string = nullptr; } break; + case Config_State::Constellation: + if ((params.gnss_mode&0x6F)!=0) { + //IMES not taken into account by Septentrio receivers + if (asprintf(&config_string, "sst, %s%s%s%s%s%s\n", (params.gnss_mode&(1U<<0))!=0 ? "GPS" : "", + (params.gnss_mode&(1U<<1))!=0 ? ((params.gnss_mode&0x01)==0 ? "SBAS" : "+SBAS") : "", + (params.gnss_mode&(1U<<2))!=0 ? ((params.gnss_mode&0x03)==0 ? "GALILEO" : "+GALILEO") : "", + (params.gnss_mode&(1U<<3))!=0 ? ((params.gnss_mode&0x07)==0 ? "BEIDOU" : "+BEIDOU") : "", + (params.gnss_mode&(1U<<5))!=0 ? ((params.gnss_mode&0x0F)==0 ? "QZSS" : "+QZSS") : "", + (params.gnss_mode&(1U<<6))!=0 ? ((params.gnss_mode&0x2F)==0 ? "GLONASS" : "+GLONASS") : "") == -1) { + config_string=nullptr; + } + } + break; case Config_State::Blob: if (asprintf(&config_string, "%s\n", _initialisation_blob[_init_blob_index]) == -1) { config_string = nullptr; @@ -364,6 +377,9 @@ AP_GPS_SBF::parse(uint8_t temp) config_step = Config_State::SSO; break; case Config_State::SSO: + config_step = Config_State::Constellation; + break; + case Config_State::Constellation: config_step = Config_State::Blob; break; case Config_State::Blob: @@ -699,4 +715,5 @@ bool AP_GPS_SBF::prepare_for_arming(void) { return is_logging; } + #endif diff --git a/libraries/AP_GPS/AP_GPS_SBF.h b/libraries/AP_GPS/AP_GPS_SBF.h index 8817653f42ed74..b4e1bb28c62b1e 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.h +++ b/libraries/AP_GPS/AP_GPS_SBF.h @@ -76,6 +76,7 @@ class AP_GPS_SBF : public AP_GPS_Backend Blob, SBAS, SGA, + Constellation, Complete }; Config_State config_step; From 50eac0ef31b24a23f7ffbcb74659940fbede3f22 Mon Sep 17 00:00:00 2001 From: Thomas Watson Date: Sun, 30 Jun 2024 17:09:18 -0500 Subject: [PATCH 029/124] AP_Scripting: keep enum definitions in flash Saves ~100B of statically allocated RAM. --- libraries/AP_Scripting/generator/src/main.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Scripting/generator/src/main.c b/libraries/AP_Scripting/generator/src/main.c index b2f4e300103d4c..f94dc7220d5a87 100644 --- a/libraries/AP_Scripting/generator/src/main.c +++ b/libraries/AP_Scripting/generator/src/main.c @@ -2381,7 +2381,7 @@ void emit_methods(struct userdata *node) { } void emit_enum(struct userdata * data) { - fprintf(source, "struct userdata_enum %s_enums[] = {\n", data->sanatized_name); + fprintf(source, "const struct userdata_enum %s_enums[] = {\n", data->sanatized_name); struct userdata_enum *ud_enum = data->enums; while (ud_enum != NULL) { fprintf(source, " {\"%s\", %s::%s},\n", ud_enum->name, data->name, ud_enum->name); From 8e303675fee148ccab3b9b9eda0a04b7ec566c50 Mon Sep 17 00:00:00 2001 From: Thomas Watson Date: Sat, 15 Jun 2024 15:29:12 -0500 Subject: [PATCH 030/124] AP_Scripting: don't put userdata and ap_objects into globals They are never accessed from globals. Only their metatables are accessed, using luaL_getmetatable. Saves ~2.9K of Lua heap. --- libraries/AP_Scripting/generator/src/main.c | 21 +++------------------ 1 file changed, 3 insertions(+), 18 deletions(-) diff --git a/libraries/AP_Scripting/generator/src/main.c b/libraries/AP_Scripting/generator/src/main.c index f94dc7220d5a87..f48f4d72dc28e3 100644 --- a/libraries/AP_Scripting/generator/src/main.c +++ b/libraries/AP_Scripting/generator/src/main.c @@ -2504,46 +2504,31 @@ void emit_loaders(void) { fprintf(source, " // userdata metatables\n"); fprintf(source, " for (uint32_t i = 0; i < ARRAY_SIZE(userdata_fun); i++) {\n"); fprintf(source, " luaL_newmetatable(L, userdata_fun[i].name);\n"); - fprintf(source, " lua_pushcclosure(L, userdata_fun[i].func, 0);\n"); + fprintf(source, " lua_pushcfunction(L, userdata_fun[i].func);\n"); fprintf(source, " lua_setfield(L, -2, \"__index\");\n"); fprintf(source, " if (userdata_fun[i].operators != nullptr) {\n"); fprintf(source, " luaL_setfuncs(L, userdata_fun[i].operators, 0);\n"); fprintf(source, " }\n"); - fprintf(source, " lua_pushstring(L, \"__call\");\n"); - fprintf(source, " lua_pushvalue(L, -2);\n"); - fprintf(source, " lua_settable(L, -3);\n"); - fprintf(source, " lua_pop(L, 1);\n"); - fprintf(source, " lua_newuserdata(L, 0);\n"); - fprintf(source, " luaL_getmetatable(L, userdata_fun[i].name);\n"); - fprintf(source, " lua_setmetatable(L, -2);\n"); - fprintf(source, " lua_setglobal(L, userdata_fun[i].name);\n"); fprintf(source, " }\n"); fprintf(source, "\n"); fprintf(source, " // ap object metatables\n"); fprintf(source, " for (uint32_t i = 0; i < ARRAY_SIZE(ap_object_fun); i++) {\n"); fprintf(source, " luaL_newmetatable(L, ap_object_fun[i].name);\n"); - fprintf(source, " lua_pushcclosure(L, ap_object_fun[i].func, 0);\n"); + fprintf(source, " lua_pushcfunction(L, ap_object_fun[i].func);\n"); fprintf(source, " lua_setfield(L, -2, \"__index\");\n"); - fprintf(source, " lua_pushstring(L, \"__call\");\n"); - fprintf(source, " lua_pushvalue(L, -2);\n"); - fprintf(source, " lua_settable(L, -3);\n"); fprintf(source, " lua_pop(L, 1);\n"); - fprintf(source, " lua_newuserdata(L, 0);\n"); - fprintf(source, " luaL_getmetatable(L, ap_object_fun[i].name);\n"); - fprintf(source, " lua_setmetatable(L, -2);\n"); - fprintf(source, " lua_setglobal(L, ap_object_fun[i].name);\n"); fprintf(source, " }\n"); fprintf(source, "\n"); fprintf(source, " // singleton metatables\n"); fprintf(source, " for (uint32_t i = 0; i < ARRAY_SIZE(singleton_fun); i++) {\n"); fprintf(source, " luaL_newmetatable(L, singleton_fun[i].name);\n"); - fprintf(source, " lua_pushcclosure(L, singleton_fun[i].func, 0);\n"); + fprintf(source, " lua_pushcfunction(L, singleton_fun[i].func);\n"); fprintf(source, " lua_setfield(L, -2, \"__index\");\n"); fprintf(source, " lua_pushstring(L, \"__call\");\n"); fprintf(source, " lua_pushvalue(L, -2);\n"); From 9cbec043d0565b0881fb95f321e0a1cb198666f8 Mon Sep 17 00:00:00 2001 From: Thomas Watson Date: Fri, 28 Jun 2024 13:31:09 -0500 Subject: [PATCH 031/124] AP_Scripting: remove __call metamethod from singleton metatables The __call metamethod was set to the metatable itself. With __call not present, Lua will try to call the metatable (and fail), which is the same behavior as with the __call metamethod set to the metatable. Saves ~2K Lua heap. --- libraries/AP_Scripting/generator/src/main.c | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/libraries/AP_Scripting/generator/src/main.c b/libraries/AP_Scripting/generator/src/main.c index f48f4d72dc28e3..1ddefb351e3e70 100644 --- a/libraries/AP_Scripting/generator/src/main.c +++ b/libraries/AP_Scripting/generator/src/main.c @@ -2527,16 +2527,11 @@ void emit_loaders(void) { fprintf(source, " // singleton metatables\n"); fprintf(source, " for (uint32_t i = 0; i < ARRAY_SIZE(singleton_fun); i++) {\n"); + fprintf(source, " lua_newuserdata(L, 0);\n"); fprintf(source, " luaL_newmetatable(L, singleton_fun[i].name);\n"); fprintf(source, " lua_pushcfunction(L, singleton_fun[i].func);\n"); fprintf(source, " lua_setfield(L, -2, \"__index\");\n"); - fprintf(source, " lua_pushstring(L, \"__call\");\n"); - fprintf(source, " lua_pushvalue(L, -2);\n"); - fprintf(source, " lua_settable(L, -3);\n"); - fprintf(source, " lua_pop(L, 1);\n"); - fprintf(source, " lua_newuserdata(L, 0);\n"); - fprintf(source, " luaL_getmetatable(L, singleton_fun[i].name);\n"); fprintf(source, " lua_setmetatable(L, -2);\n"); fprintf(source, " lua_setglobal(L, singleton_fun[i].name);\n"); fprintf(source, " }\n"); From 059af2117d0e52b50c4cb879c582ed5527371662 Mon Sep 17 00:00:00 2001 From: Thomas Watson Date: Sat, 15 Jun 2024 15:51:21 -0500 Subject: [PATCH 032/124] AP_Scripting: place bindings into global table The global table is then used as the __index metamethod of each state's environment table. Avoids the overhead of loading binding objects into each state. The binding objects are immutable from Lua so sandboxing is not violated. Does have the slight downside that a script can no longer know all the binding names by enumerating _ENV. Saves ~700B of memory per loaded script. --- libraries/AP_Scripting/generator/src/main.c | 32 ++++++++++----------- 1 file changed, 15 insertions(+), 17 deletions(-) diff --git a/libraries/AP_Scripting/generator/src/main.c b/libraries/AP_Scripting/generator/src/main.c index 1ddefb351e3e70..ad817c0ac4585f 100644 --- a/libraries/AP_Scripting/generator/src/main.c +++ b/libraries/AP_Scripting/generator/src/main.c @@ -2535,12 +2535,19 @@ void emit_loaders(void) { fprintf(source, " lua_setmetatable(L, -2);\n"); fprintf(source, " lua_setglobal(L, singleton_fun[i].name);\n"); fprintf(source, " }\n"); + fprintf(source, "\n"); + fprintf(source, " // userdata creation funcs\n"); + fprintf(source, " for (uint32_t i = 0; i < ARRAY_SIZE(new_userdata); i++) {\n"); + fprintf(source, " lua_pushcfunction(L, new_userdata[i].fun);\n"); + fprintf(source, " lua_setglobal(L, new_userdata[i].name);\n"); + fprintf(source, " }\n"); fprintf(source, "\n"); + fprintf(source, "}\n\n"); } -void emit_sandbox(void) { +void emit_userdata_new_funcs(void) { struct userdata *data = parsed_userdata; fprintf(source, "const struct userdata {\n"); fprintf(source, " const char *name;\n"); @@ -2589,23 +2596,14 @@ void emit_sandbox(void) { } } fprintf(source, "};\n\n"); +} +void emit_sandbox(void) { fprintf(source, "void load_generated_sandbox(lua_State *L) {\n"); - // load the singletons - fprintf(source, " for (uint32_t i = 0; i < ARRAY_SIZE(singleton_fun); i++) {\n"); - fprintf(source, " lua_pushstring(L, singleton_fun[i].name);\n"); - fprintf(source, " lua_getglobal(L, singleton_fun[i].name);\n"); - fprintf(source, " lua_settable(L, -3);\n"); - fprintf(source, " }\n"); - - // load the userdata allactors and globals - fprintf(source, " for (uint32_t i = 0; i < ARRAY_SIZE(new_userdata); i++) {\n"); - fprintf(source, " lua_pushstring(L, new_userdata[i].name);\n"); - fprintf(source, " lua_pushcfunction(L, new_userdata[i].fun);\n"); - fprintf(source, " lua_settable(L, -3);\n"); - fprintf(source, " }\n"); - - fprintf(source, "\n"); + fprintf(source, " lua_createtable(L, 0, 1);\n"); + fprintf(source, " lua_pushglobaltable(L);\n"); + fprintf(source, " lua_setfield(L, -2, \"__index\");\n"); + fprintf(source, " lua_setmetatable(L, -2);\n"); fprintf(source, "}\n"); } @@ -3149,7 +3147,7 @@ int main(int argc, char **argv) { emit_methods(parsed_ap_objects); emit_index(parsed_ap_objects); - + emit_userdata_new_funcs(); emit_loaders(); emit_sandbox(); From b64ed6ca565dcaefd9b166ccb32b777ff7a59488 Mon Sep 17 00:00:00 2001 From: Thomas Watson Date: Fri, 28 Jun 2024 15:16:44 -0500 Subject: [PATCH 033/124] AP_Scripting: dynamically load some binding objects Only create the binding object (singleton metatable/userdata or C function reference) once the user first references a particular singleton or userdata creation function. Once created, the object is stored into the script's environment so it doesn't get recreated on the next reference and there isn't any further overhead. The userdatas are no longer shared between scripts which imposes a slight memory penalty for multiple scripts using the same singleton but this avoids an additional lookup time cost. Userdata and ap_objects aren't eligible for this optimization as the C++ code might want a particular metatable at any time. Saves ~9.3K Lua heap. --- libraries/AP_Scripting/generator/src/main.c | 58 ++++++++++++++------- 1 file changed, 39 insertions(+), 19 deletions(-) diff --git a/libraries/AP_Scripting/generator/src/main.c b/libraries/AP_Scripting/generator/src/main.c index ad817c0ac4585f..d9c530978328cc 100644 --- a/libraries/AP_Scripting/generator/src/main.c +++ b/libraries/AP_Scripting/generator/src/main.c @@ -2499,6 +2499,43 @@ void emit_loaders(void) { emit_type_index(parsed_singletons, "singleton"); emit_type_index(parsed_ap_objects, "ap_object"); + fprintf(source, "static int binding_index(lua_State *L) {\n"); + fprintf(source, " const char * name = luaL_checkstring(L, 2);\n"); + fprintf(source, "\n"); + fprintf(source, " bool found = false;\n"); + fprintf(source, " for (uint32_t i = 0; i < ARRAY_SIZE(singleton_fun); i++) {\n"); + fprintf(source, " if (strcmp(name, singleton_fun[i].name) == 0) {\n"); + fprintf(source, " lua_newuserdata(L, 0);\n"); + fprintf(source, " if (luaL_newmetatable(L, name)) { // need to create metatable\n"); + fprintf(source, " lua_pushcfunction(L, singleton_fun[i].func);\n"); + fprintf(source, " lua_setfield(L, -2, \"__index\");\n"); + fprintf(source, " }\n"); + fprintf(source, " lua_setmetatable(L, -2);\n"); + fprintf(source, " found = true;\n"); + fprintf(source, " break;\n"); + fprintf(source, " }\n"); + fprintf(source, " }\n"); + fprintf(source, " if (!found) {\n"); + fprintf(source, " for (uint32_t i = 0; i < ARRAY_SIZE(new_userdata); i++) {\n"); + fprintf(source, " if (strcmp(name, new_userdata[i].name) == 0) {\n"); + fprintf(source, " lua_pushcfunction(L, new_userdata[i].fun);\n"); + fprintf(source, " found = true;\n"); + fprintf(source, " break;\n"); + fprintf(source, " }\n"); + fprintf(source, " }\n"); + fprintf(source, " }\n"); + fprintf(source, " if (!found) {\n"); + fprintf(source, " return 0;\n"); + fprintf(source, " }\n"); + fprintf(source, "\n"); + fprintf(source, " // store found value to avoid a re-index\n"); + fprintf(source, " lua_pushvalue(L, -2);\n"); + fprintf(source, " lua_pushvalue(L, -2);\n"); + fprintf(source, " lua_settable(L, -5);\n"); + fprintf(source, "\n"); + fprintf(source, " return 1;\n"); + fprintf(source, "}\n\n"); + fprintf(source, "void load_generated_bindings(lua_State *L) {\n"); fprintf(source, " luaL_checkstack(L, 5, \"Out of stack\");\n"); // this is more stack space then we need, but should never fail fprintf(source, " // userdata metatables\n"); @@ -2525,24 +2562,7 @@ void emit_loaders(void) { fprintf(source, " }\n"); fprintf(source, "\n"); - fprintf(source, " // singleton metatables\n"); - fprintf(source, " for (uint32_t i = 0; i < ARRAY_SIZE(singleton_fun); i++) {\n"); - fprintf(source, " lua_newuserdata(L, 0);\n"); - fprintf(source, " luaL_newmetatable(L, singleton_fun[i].name);\n"); - fprintf(source, " lua_pushcfunction(L, singleton_fun[i].func);\n"); - fprintf(source, " lua_setfield(L, -2, \"__index\");\n"); - - fprintf(source, " lua_setmetatable(L, -2);\n"); - fprintf(source, " lua_setglobal(L, singleton_fun[i].name);\n"); - fprintf(source, " }\n"); - fprintf(source, "\n"); - - fprintf(source, " // userdata creation funcs\n"); - fprintf(source, " for (uint32_t i = 0; i < ARRAY_SIZE(new_userdata); i++) {\n"); - fprintf(source, " lua_pushcfunction(L, new_userdata[i].fun);\n"); - fprintf(source, " lua_setglobal(L, new_userdata[i].name);\n"); - fprintf(source, " }\n"); - fprintf(source, "\n"); + fprintf(source, " // singletons and userdata creation funcs are loaded dynamically\n"); fprintf(source, "}\n\n"); } @@ -2601,7 +2621,7 @@ void emit_userdata_new_funcs(void) { void emit_sandbox(void) { fprintf(source, "void load_generated_sandbox(lua_State *L) {\n"); fprintf(source, " lua_createtable(L, 0, 1);\n"); - fprintf(source, " lua_pushglobaltable(L);\n"); + fprintf(source, " lua_pushcfunction(L, binding_index);\n"); fprintf(source, " lua_setfield(L, -2, \"__index\");\n"); fprintf(source, " lua_setmetatable(L, -2);\n"); fprintf(source, "}\n"); From 5005809e74b6249bf3b32fd7fcefd63327db4b0a Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Tue, 9 Jul 2024 09:45:15 -0700 Subject: [PATCH 034/124] AP_Vehicle: add stall speed parameter for plane --- libraries/AP_Vehicle/AP_FixedWing.h | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/AP_Vehicle/AP_FixedWing.h b/libraries/AP_Vehicle/AP_FixedWing.h index 7aee38d5c5b35d..df3692a61b6552 100644 --- a/libraries/AP_Vehicle/AP_FixedWing.h +++ b/libraries/AP_Vehicle/AP_FixedWing.h @@ -14,6 +14,7 @@ struct AP_FixedWing { AP_Int16 airspeed_min; AP_Int16 airspeed_max; AP_Float airspeed_cruise; + AP_Float airspeed_stall; AP_Float min_groundspeed; AP_Int8 crash_detection_enable; AP_Float roll_limit; From 80277e6da0e21b4fcc3753a2db8a9840c6c4730e Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Tue, 9 Jul 2024 09:30:06 -0700 Subject: [PATCH 035/124] ArduPlane: Add a stall speed parameter --- ArduPlane/Parameters.cpp | 8 ++++++++ ArduPlane/Parameters.h | 1 + 2 files changed, 9 insertions(+) diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 941dcba774ac08..19288615a62054 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -296,6 +296,14 @@ const AP_Param::Info Plane::var_info[] = { // @User: Standard ASCALAR(airspeed_max, "AIRSPEED_MAX", AIRSPEED_FBW_MAX), + // @Param: AIRSPEED_STALL + // @DisplayName: Stall airspeed + // @Description: If stall prevention is enabled this speed is used to calculate the minimum airspeed while banking. If this is set to 0 then the stall speed is assumed to be the minimum airspeed speed. Typically set slightly higher then true stall speed. Value is as an indicated (calibrated/apparent) airspeed. + // @Units: m/s + // @Range: 5 75 + // @User: Standard + ASCALAR(airspeed_stall, "AIRSPEED_STALL", 0), + // @Param: FBWB_ELEV_REV // @DisplayName: Fly By Wire elevator reverse // @Description: Reverse sense of elevator in FBWB and CRUISE modes. When set to 0 up elevator (pulling back on the stick) means to lower altitude. When set to 1, up elevator means to raise altitude. diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index ba89c457728112..de514b891532cc 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -343,6 +343,7 @@ class Parameters { k_param_mixing_offset, k_param_dspoiler_rud_rate, + k_param_airspeed_stall, k_param_logger = 253, // Logging Group From edaddc043166189938aa7654e9a6e72c06a0b6d0 Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Wed, 3 Jul 2024 10:33:57 -0700 Subject: [PATCH 036/124] AP_TECS: Use aircraft stall speed When stall prevention is enabled we were scaling from the aircraft's minimum flight speed. However this is normally already picked as being above the stall speed, and for a variety of reasons we may want to pin the aircraft at a higher minimum speed. But if the aircraft was commanded to fly to close to that minimum speed as soon as it banked for a pattern it would command a increase in speed to keep it away from stalling. However if your minimum speed is far from stalling this increase was incorrect. To make it worse what this actually results in happening is an aircraft diving for more speed (over 10 m/s on some aircraft) as well as descending to gain that speed resulting in over 200 foot deviations in altitude control. --- libraries/AP_TECS/AP_TECS.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index 9307502caaf0bf..55fb024043b1da 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/libraries/AP_TECS/AP_TECS.cpp @@ -415,7 +415,11 @@ void AP_TECS::_update_speed(float DT) if (aparm.stall_prevention) { // when stall prevention is active we raise the minimum // airspeed based on aerodynamic load factor - _TASmin *= _load_factor; + if (is_positive(aparm.airspeed_stall)) { + _TASmin = MAX(_TASmin, aparm.airspeed_stall*EAS2TAS*_load_factor); + } else { + _TASmin *= _load_factor; + } } if (_TASmax < _TASmin) { From 7731fc09e23da3fec6202a020277669d815fca96 Mon Sep 17 00:00:00 2001 From: muramura Date: Wed, 17 Jul 2024 07:20:26 +0900 Subject: [PATCH 037/124] AP_HAL_SITL: If HAL_CAN_WITH_SOCKETCAN is undefined, treat it as NONE --- libraries/AP_HAL_SITL/CANSocketIface.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_HAL_SITL/CANSocketIface.cpp b/libraries/AP_HAL_SITL/CANSocketIface.cpp index e64253b79bc884..7011c2206b616b 100644 --- a/libraries/AP_HAL_SITL/CANSocketIface.cpp +++ b/libraries/AP_HAL_SITL/CANSocketIface.cpp @@ -219,11 +219,11 @@ bool CANIface::init(const uint32_t bitrate, const OperatingMode mode) case SITL::SIM::CANTransport::MulticastUDP: transport = NEW_NOTHROW CAN_Multicast(); break; - case SITL::SIM::CANTransport::SocketCAN: #if HAL_CAN_WITH_SOCKETCAN + case SITL::SIM::CANTransport::SocketCAN: transport = NEW_NOTHROW CAN_SocketCAN(); -#endif break; +#endif case SITL::SIM::CANTransport::None: default: // if user supplies an invalid value for the parameter transport = nullptr; From 858aff5f4fe47d89226e5adf2ca683b7aa6a6f2c Mon Sep 17 00:00:00 2001 From: xiaozhou <3177821352@qq.com> Date: Mon, 15 Jul 2024 16:20:28 +0900 Subject: [PATCH 038/124] AP_Mount: Topotek handles new version format Co-authored-by: Randy Mackay --- libraries/AP_Mount/AP_Mount_Topotek.cpp | 51 +++++++++++++++++-------- 1 file changed, 36 insertions(+), 15 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_Topotek.cpp b/libraries/AP_Mount/AP_Mount_Topotek.cpp index 853f5e24dd07c0..b785a8d7468286 100755 --- a/libraries/AP_Mount/AP_Mount_Topotek.cpp +++ b/libraries/AP_Mount/AP_Mount_Topotek.cpp @@ -770,7 +770,7 @@ void AP_Mount_Topotek::request_track_status() // request gimbal version void AP_Mount_Topotek::request_gimbal_version() { - // sample command: #TPPD2rVSN00 + // sample command: #TPUD2rVSN00 send_fixedlen_packet(AddressByte::SYSTEM_AND_IMAGE, AP_MOUNT_TOPOTEK_ID3CHAR_GET_VERSION, false, 0); } @@ -1024,29 +1024,50 @@ void AP_Mount_Topotek::gimbal_dist_info_analyse() // gimbal basic information analysis void AP_Mount_Topotek::gimbal_version_analyse() { - uint8_t major_ver = 0; - uint8_t minor_ver = 0; - uint8_t patch_ver = 0; + // version array with index 0=major, 1=minor, 2=patch + uint8_t version[3] {}; // extract firmware version + // the version can be in the format "1.2.3" or "123" const uint8_t data_buf_len = char_to_hex(_msg_buff[5]); - if (data_buf_len >= 1) { - major_ver = char_to_hex(_msg_buff[10]); - } - if (data_buf_len >= 2) { - minor_ver = char_to_hex(_msg_buff[11]); + + // check for "." + bool constains_period = false; + for (uint8_t i = 0; i < data_buf_len; i++) { + constains_period |= _msg_buff[10 + i] == '.'; } - if (data_buf_len >= 3) { - patch_ver = char_to_hex(_msg_buff[12]); + + // if contains period, extract version number + uint32_t ver_num = 0; + uint8_t ver_count = 0; + if (constains_period) { + for (uint8_t i = 0; i < data_buf_len; i++) { + if (_msg_buff[10 + i] != '.') { + ver_num = ver_num * 10 + char_to_hex(_msg_buff[10 + i]); + } else { + version[ver_count++] = ver_num; + ver_num = 0; + } + } + } else { + if (data_buf_len >= 1) { + version[0] = char_to_hex(_msg_buff[10]); + } + if (data_buf_len >= 2) { + version[1] = char_to_hex(_msg_buff[11]); + } + if (data_buf_len >= 3) { + version[2] = char_to_hex(_msg_buff[12]); + } } - _firmware_ver = (patch_ver << 16) | (minor_ver << 8) | (major_ver); + _firmware_ver = (version[2] << 16) | (version[1] << 8) | (version[0]); // display gimbal model and firmware version to user GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s v%u.%u.%u", send_message_prefix, - major_ver, - minor_ver, - patch_ver); + version[0], // major version + version[1], // minor version + version[2]); // patch version _got_gimbal_version = true; } From 69e4005bb60723f955096684aa83ab679316788e Mon Sep 17 00:00:00 2001 From: xiaozhou <3177821352@qq.com> Date: Tue, 16 Jul 2024 19:56:19 +0900 Subject: [PATCH 039/124] AP_Mount: Topotek retrieves model name Co-authored-by: Randy Mackay --- libraries/AP_Mount/AP_Mount_Topotek.cpp | 34 +++++++++++++++++++++++-- libraries/AP_Mount/AP_Mount_Topotek.h | 11 +++++++- 2 files changed, 42 insertions(+), 3 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_Topotek.cpp b/libraries/AP_Mount/AP_Mount_Topotek.cpp index b785a8d7468286..7d95fbd663a9c6 100755 --- a/libraries/AP_Mount/AP_Mount_Topotek.cpp +++ b/libraries/AP_Mount/AP_Mount_Topotek.cpp @@ -35,6 +35,7 @@ extern const AP_HAL::HAL& hal; # define AP_MOUNT_TOPOTEK_ID3CHAR_SD_CARD "SDC" // get SD card state, data bytes: 00:get remaining capacity, 01:get total capacity # define AP_MOUNT_TOPOTEK_ID3CHAR_TIME "UTC" // set time and date, data bytes: HHMMSSDDMMYY # define AP_MOUNT_TOPOTEK_ID3CHAR_GET_VERSION "VSN" // get firmware version, data bytes always 00 +# define AP_MOUNT_TOPOTEK_ID3CHAR_GET_MODEL_NAME "PA2" // get model name, data bytes always 00 # define AP_MOUNT_TOPOTEK_ID3CHAR_GIMBAL_MODE "PTZ" // set gimbal mode, data bytes: 00:stop, 01:up, 02:down, 03:left, 04:right, 05:home position, 06:lock, 07:follow, 08:lock/follow toggle, 09:calibration, 0A:one button down # define AP_MOUNT_TOPOTEK_ID3CHAR_YPR_RATE "YPR" // set the rate yaw, pitch and roll targets of the gimbal yaw in range -99 ~ +99 # define AP_MOUNT_TOPOTEK_ID3CHAR_YAW_ANGLE "GIY" // set the yaw angle target in the range -150 ~ 150, speed 0 ~ 99 (0.1deg/sec) @@ -111,6 +112,12 @@ void AP_Mount_Topotek::update() request_track_status(); } break; + case 8: + // get gimbal model name + if (!_got_gimbal_model_name) { + request_gimbal_model_name(); + } + break; } // change to RC_TARGETING mode if RC input has changed @@ -510,6 +517,11 @@ void AP_Mount_Topotek::send_camera_information(mavlink_channel_t chan) const static uint8_t model_name[32] {}; const char cam_definition_uri[140] {}; + // copy model name if available + if (_got_gimbal_model_name) { + strncpy((char*)model_name, (const char*)_model_name, ARRAY_SIZE(model_name)); + } + // capability flags const uint32_t flags = CAMERA_CAP_FLAGS_CAPTURE_VIDEO | CAMERA_CAP_FLAGS_CAPTURE_IMAGE | @@ -675,8 +687,8 @@ void AP_Mount_Topotek::read_incoming_packets() case ParseState::WAITING_FOR_ID1: case ParseState::WAITING_FOR_ID2: case ParseState::WAITING_FOR_ID3: - // sanity check all capital letters. eg 'GAC' - if (b >= 'A' && b <= 'Z') { + // check all uppercase letters and numbers. eg 'GAC' + if ((b >= 'A' && b <= 'Z') || (b >= '0' && b <= '9')) { // advance to next state _parser.state = (ParseState)((uint8_t)_parser.state+1); break; @@ -774,6 +786,13 @@ void AP_Mount_Topotek::request_gimbal_version() send_fixedlen_packet(AddressByte::SYSTEM_AND_IMAGE, AP_MOUNT_TOPOTEK_ID3CHAR_GET_VERSION, false, 0); } +// request gimbal model name +void AP_Mount_Topotek::request_gimbal_model_name() +{ + // sample command: #TPUG2rPA200 + send_fixedlen_packet(AddressByte::GIMBAL, AP_MOUNT_TOPOTEK_ID3CHAR_GET_MODEL_NAME, false, 0); +} + // send angle target in radians to gimbal void AP_Mount_Topotek::send_angle_target(const MountTarget& angle_rad) { @@ -1072,6 +1091,17 @@ void AP_Mount_Topotek::gimbal_version_analyse() _got_gimbal_version = true; } +// gimbal model name message analysis +void AP_Mount_Topotek::gimbal_model_name_analyse() +{ + strncpy((char *)_model_name, (const char *)_msg_buff + 10, char_to_hex(_msg_buff[5])); + + // display gimbal model name to user + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s %s", send_message_prefix, _model_name); + + _got_gimbal_model_name = true; +} + // calculate checksum uint8_t AP_Mount_Topotek::calculate_crc(const uint8_t *cmd, uint8_t len) const { diff --git a/libraries/AP_Mount/AP_Mount_Topotek.h b/libraries/AP_Mount/AP_Mount_Topotek.h index c1f3e08dc8fa10..cbc62ca839f85e 100755 --- a/libraries/AP_Mount/AP_Mount_Topotek.h +++ b/libraries/AP_Mount/AP_Mount_Topotek.h @@ -27,7 +27,7 @@ #include #define AP_MOUNT_TOPOTEK_PACKETLEN_MAX 36 // maximum number of bytes in a packet sent to or received from the gimbal -#define AP_MOUNT_RECV_GIMBAL_CMD_CATEGORIES_NUM 6 // parse the number of gimbal command types +#define AP_MOUNT_RECV_GIMBAL_CMD_CATEGORIES_NUM 7 // parse the number of gimbal command types class AP_Mount_Topotek : public AP_Mount_Backend_Serial { @@ -168,6 +168,9 @@ class AP_Mount_Topotek : public AP_Mount_Backend_Serial // request gimbal version void request_gimbal_version(); + // request gimbal model name + void request_gimbal_model_name(); + // send angle target in radians to gimbal void send_angle_target(const MountTarget& angle_rad); @@ -195,6 +198,9 @@ class AP_Mount_Topotek : public AP_Mount_Backend_Serial // gimbal basic information analysis void gimbal_version_analyse(); + // gimbal model name message analysis + void gimbal_model_name_analyse(); + // gimbal distance information analysis void gimbal_dist_info_analyse(); @@ -229,8 +235,10 @@ class AP_Mount_Topotek : public AP_Mount_Backend_Serial bool _sdcard_status; // memory card status (received from gimbal) bool _last_lock; // last lock mode sent to gimbal bool _got_gimbal_version; // true if gimbal's version has been received + bool _got_gimbal_model_name; // true if gimbal's model name has been received bool _last_zoom_stop; // true if zoom has been stopped (used to re-send in order to handle lost packets) bool _last_focus_stop; // true if focus has been stopped (used to re-sent in order to handle lost packets) + uint8_t _model_name[16]; // gimbal model name uint8_t _sent_time_count; // count of current time messages sent to gimbal uint32_t _firmware_ver; // firmware version Vector3f _current_angle_rad; // current angles in radians received from gimbal (x=roll, y=pitch, z=yaw) @@ -260,6 +268,7 @@ class AP_Mount_Topotek : public AP_Mount_Backend_Serial {{"LRF"}, &AP_Mount_Topotek::gimbal_dist_info_analyse}, {{"TRC"}, &AP_Mount_Topotek::gimbal_track_analyse}, {{"VSN"}, &AP_Mount_Topotek::gimbal_version_analyse}, + {{"PA2"}, &AP_Mount_Topotek::gimbal_model_name_analyse} }; }; From e2cb4ba232deb815e12d8d75c0de56309d4febb7 Mon Sep 17 00:00:00 2001 From: xiaozhou <3177821352@qq.com> Date: Tue, 16 Jul 2024 19:57:52 +0900 Subject: [PATCH 040/124] AP_Mount: Topotek image tracking fix Co-authored-by: Randy Mackay --- libraries/AP_Mount/AP_Mount_Topotek.cpp | 45 +++++++++++++++---------- libraries/AP_Mount/AP_Mount_Topotek.h | 9 ++++- 2 files changed, 35 insertions(+), 19 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_Topotek.cpp b/libraries/AP_Mount/AP_Mount_Topotek.cpp index 7d95fbd663a9c6..9487e202af65ff 100755 --- a/libraries/AP_Mount/AP_Mount_Topotek.cpp +++ b/libraries/AP_Mount/AP_Mount_Topotek.cpp @@ -108,9 +108,7 @@ void AP_Mount_Topotek::update() break; case 6: // request tracking info - if (_is_tracking) { - request_track_status(); - } + request_track_status(); break; case 8: // get gimbal model name @@ -127,11 +125,10 @@ void AP_Mount_Topotek::update() if (_is_tracking) { // cancel tracking if mode has changed if (_last_mode != _mode) { + _last_mode = _mode; cancel_tracking(); - } else { - // image tracking is active so we do not send attitude targets - return; } + return; } _last_mode = _mode; @@ -395,6 +392,11 @@ bool AP_Mount_Topotek::set_tracking(TrackingType tracking_type, const Vector2f& } if (send_tracking_cmd) { + // set the gimbal to the ready-to-track state when the gimbal tracking status is stopped + if (_last_tracking_state == TrackingStatus::STOPPED_TRACKING) { + send_fixedlen_packet(AddressByte::SYSTEM_AND_IMAGE, AP_MOUNT_TOPOTEK_ID3CHAR_TRACKING, true, 2); + } + // prepare data bytes uint8_t databuff[10]; databuff[0] = HIGHBYTE(track_center_x); @@ -406,7 +408,7 @@ bool AP_Mount_Topotek::set_tracking(TrackingType tracking_type, const Vector2f& databuff[6] = HIGHBYTE(track_height); databuff[7] = LOWBYTE(track_height); databuff[8] = 0; - databuff[9] = 0; + databuff[9] = (tracking_type == TrackingType::TRK_POINT) ? 9 : 1; // when tracking point, enable fuzzy click function // send tracking command bool res = send_variablelen_packet(HeaderType::VARIABLE_LEN, @@ -414,7 +416,12 @@ bool AP_Mount_Topotek::set_tracking(TrackingType tracking_type, const Vector2f& AP_MOUNT_TOPOTEK_ID3CHAR_START_TRACKING, true, (uint8_t*)databuff, ARRAY_SIZE(databuff)); - _is_tracking |= res; + + // display error message on failure + if (!res) { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s tracking failed", send_message_prefix); + } + return res; } @@ -431,11 +438,11 @@ bool AP_Mount_Topotek::cancel_tracking() return false; } + // if gimbal is tracking-in-progress change to waiting state, otherwise stop + const uint8_t track_set = _last_tracking_state == TrackingStatus::TRACKING_IN_PROGRESS ? 1 : 0; + // send tracking command - if (send_fixedlen_packet(AddressByte::SYSTEM_AND_IMAGE, AP_MOUNT_TOPOTEK_ID3CHAR_TRACKING, true, 1)) { - return true; - } - return false; + return send_fixedlen_packet(AddressByte::SYSTEM_AND_IMAGE, AP_MOUNT_TOPOTEK_ID3CHAR_TRACKING, true, track_set); } // set camera picture-in-picture mode @@ -1001,7 +1008,7 @@ void AP_Mount_Topotek::gimbal_sdcard_analyse() void AP_Mount_Topotek::gimbal_track_analyse() { // ignore tracking state if unchanged - uint8_t tracking_state = _msg_buff[11]; + TrackingStatus tracking_state = (TrackingStatus)_msg_buff[11]; if (tracking_state == _last_tracking_state) { return; } @@ -1010,15 +1017,17 @@ void AP_Mount_Topotek::gimbal_track_analyse() // inform user const char* tracking_str = "tracking"; switch (tracking_state) { - case '0': - GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "%s %s error", send_message_prefix, tracking_str); - break; - case '1': + case TrackingStatus::STOPPED_TRACKING: GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s %s stopped", send_message_prefix, tracking_str); _is_tracking = false; break; - case '2': + case TrackingStatus::WAITING_FOR_TRACKING: + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s %s waiting", send_message_prefix, tracking_str); + _is_tracking = false; + break; + case TrackingStatus::TRACKING_IN_PROGRESS: GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s %s started", send_message_prefix, tracking_str); + _is_tracking = true; break; } } diff --git a/libraries/AP_Mount/AP_Mount_Topotek.h b/libraries/AP_Mount/AP_Mount_Topotek.h index cbc62ca839f85e..bcabd3d4e2735f 100755 --- a/libraries/AP_Mount/AP_Mount_Topotek.h +++ b/libraries/AP_Mount/AP_Mount_Topotek.h @@ -147,6 +147,13 @@ class AP_Mount_Topotek : public AP_Mount_Backend_Serial WAITING_FOR_CRC_HIGH, }; + // tracking status + enum class TrackingStatus : uint8_t { + STOPPED_TRACKING = 0x30, // not tracking + WAITING_FOR_TRACKING = 0x31, // wait to track command status + TRACKING_IN_PROGRESS = 0x32 // the status is being tracked. + }; + // identifier bytes typedef char Identifier[3]; @@ -230,7 +237,7 @@ class AP_Mount_Topotek : public AP_Mount_Backend_Serial // members bool _recording; // recording status (received from gimbal) bool _is_tracking; // whether to enable the tracking state - uint8_t _last_tracking_state; // last tracking state received from gimbal + TrackingStatus _last_tracking_state = TrackingStatus::STOPPED_TRACKING; // last tracking state received from gimbal uint8_t _last_mode; // mode during latest update, used to detect mode changes and cancel tracking bool _sdcard_status; // memory card status (received from gimbal) bool _last_lock; // last lock mode sent to gimbal From c771440ea4e8a8d0f709cc485109a74db76846c6 Mon Sep 17 00:00:00 2001 From: James O'Shannessy <12959316+joshanne@users.noreply.github.com> Date: Wed, 17 Jul 2024 17:42:23 +1000 Subject: [PATCH 041/124] AP_InertialSensor: Fix persistent storing of IMU Z Scale Fixes INSn_ACCSCAL_Z not being stored in persistent storage when bootloader is flashed. --- libraries/AP_InertialSensor/AP_InertialSensor_tempcal.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_tempcal.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_tempcal.cpp index 17d89f15ac1cd5..bb71172dc9b009 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_tempcal.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_tempcal.cpp @@ -526,6 +526,7 @@ void AP_InertialSensor::get_persistent_params(ExpandingString &str) const str.printf("INS%u_ACCOFFS_Z=%f\n", imu, aoff.z); str.printf("INS%u_ACCSCAL_X=%f\n", imu, ascl.x); str.printf("INS%u_ACCSCAL_Y=%f\n", imu, ascl.y); + str.printf("INS%u_ACCSCAL_Z=%f\n", imu, ascl.z); str.printf("INS%u_ACC_CALTEMP=%.2f\n", imu, params[i].caltemp_accel.get()); } #endif From ce0ae33c5bd51b72db28d2338f5f1b2bae277202 Mon Sep 17 00:00:00 2001 From: James O'Shannessy <12959316+joshanne@users.noreply.github.com> Date: Wed, 17 Jul 2024 17:37:39 +1000 Subject: [PATCH 042/124] AP_HAL_ChibiOS: Capture the case where the persistent parameter is the newer format INSn_ACC_ID This fixes the handling of the newer INSn_* parameters when loading those stored in persistent memory. --- libraries/AP_HAL_ChibiOS/Util.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/Util.cpp b/libraries/AP_HAL_ChibiOS/Util.cpp index 6775192d2ad7ee..a6d802ea6429b8 100644 --- a/libraries/AP_HAL_ChibiOS/Util.cpp +++ b/libraries/AP_HAL_ChibiOS/Util.cpp @@ -641,8 +641,11 @@ void Util::apply_persistent_params(void) const been done by whether the IDs are configured in storage */ - if (strncmp(pname, "INS_ACC", 7) == 0 && - strcmp(pname+strlen(pname)-3, "_ID") == 0) { + bool legacy_acc_id = strncmp(pname, "INS_ACC", 7) == 0 && + strcmp(pname+strlen(pname)-3, "_ID") == 0; + bool new_acc_id = strncmp(pname, "INS", 3) == 0 && + strcmp(pname+strlen(pname)-6, "ACC_ID") == 0; + if (legacy_acc_id || new_acc_id) { enum ap_var_type ptype; AP_Int32 *ap = (AP_Int32 *)AP_Param::find(pname, &ptype); if (ap && ptype == AP_PARAM_INT32) { From b6adb4c7e72d421b2d115bf617b4187eb38a6570 Mon Sep 17 00:00:00 2001 From: Thomas Watson Date: Sun, 21 Jul 2024 22:01:20 -0500 Subject: [PATCH 043/124] DroneCAN: update DSDL compiler Fixes the following issues: * Compound array elements subject to tail array optimization could be decoded incorrectly, causing a decode failure. * Invalid array lengths could be sent over the wire if a longer-than-max array was encoded (though only the max number of elements was sent). * Lengths were not validated when decoding arrays of compound elements using TAO, causing memory corruption if an invalid length was received. * Union tags were not validated, causing undefined behavior if an invalid tag was received. --- modules/DroneCAN/dronecan_dsdlc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/DroneCAN/dronecan_dsdlc b/modules/DroneCAN/dronecan_dsdlc index 2465ace6c8cb01..43d8a9ed909e18 160000 --- a/modules/DroneCAN/dronecan_dsdlc +++ b/modules/DroneCAN/dronecan_dsdlc @@ -1 +1 @@ -Subproject commit 2465ace6c8cb0148e3ff5865aa9e4dd17d691a71 +Subproject commit 43d8a9ed909e18f169c001a0f418edc72269b36b From 0a9a8d9178d90d1c2314db4ef134f4033546d453 Mon Sep 17 00:00:00 2001 From: Ben Butterworth <24711048+ben-xD@users.noreply.github.com> Date: Mon, 22 Jul 2024 13:08:12 +0100 Subject: [PATCH 044/124] Tools: install-prereqs-ubuntu.sh: fix script after setuptools==71.0.0 release Use newest `packaging` to avoid packaging incompatibility with `setuptools>=71.0` As https://github.com/pypa/setuptools/issues/4496#issuecomment-2240322375 mentions: > setuptools>=71.0 does not work with packaging<24.0, but will not upgrade it if packaging is already installed Also relevant: https://github.com/pypa/setuptools/issues/4496 --- Tools/environment_install/install-prereqs-arch.sh | 2 +- .../install-prereqs-openSUSE-Tumbleweed.sh | 2 +- Tools/environment_install/install-prereqs-ubuntu.sh | 4 ++-- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/Tools/environment_install/install-prereqs-arch.sh b/Tools/environment_install/install-prereqs-arch.sh index 394d9ae3b8d3f8..8bba1d360876b2 100755 --- a/Tools/environment_install/install-prereqs-arch.sh +++ b/Tools/environment_install/install-prereqs-arch.sh @@ -26,7 +26,7 @@ BASE_PKGS="base-devel ccache git gsfonts tk wget gcc" SITL_PKGS="python-pip python-setuptools python-wheel python-wxpython opencv python-numpy python-scipy" PX4_PKGS="lib32-glibc zip zlib ncurses" -PYTHON_PKGS="future lxml pymavlink MAVProxy pexpect argparse matplotlib pyparsing geocoder pyserial empy==3.3.4 dronecan setuptools wheel" +PYTHON_PKGS="future lxml pymavlink MAVProxy pexpect argparse matplotlib pyparsing geocoder pyserial empy==3.3.4 dronecan packaging setuptools wheel" # GNU Tools for ARM Embedded Processors # (see https://launchpad.net/gcc-arm-embedded/) diff --git a/Tools/environment_install/install-prereqs-openSUSE-Tumbleweed.sh b/Tools/environment_install/install-prereqs-openSUSE-Tumbleweed.sh index 5895dbdeb5b2be..acc86a9f4090cb 100755 --- a/Tools/environment_install/install-prereqs-openSUSE-Tumbleweed.sh +++ b/Tools/environment_install/install-prereqs-openSUSE-Tumbleweed.sh @@ -105,7 +105,7 @@ if ! grep -Fxq "$SOURCE_LINE" ~/.bashrc; then fi fi -$PIP3 install -U pip setuptools wheel +$PIP3 install -U pip packaging setuptools wheel $PIP3 install -U attrdict3 $PIP3 install -U $PYTHON_PKGS diff --git a/Tools/environment_install/install-prereqs-ubuntu.sh b/Tools/environment_install/install-prereqs-ubuntu.sh index 0aea76a53b8388..76ea0c3098543c 100755 --- a/Tools/environment_install/install-prereqs-ubuntu.sh +++ b/Tools/environment_install/install-prereqs-ubuntu.sh @@ -399,8 +399,8 @@ if [ -n "$PYTHON_VENV_PACKAGE" ]; then fi fi -# try update setuptools and wheel before installing pip package that may need compilation -$PIP install $PIP_USER_ARGUMENT -U pip setuptools wheel +# try update packaging, setuptools and wheel before installing pip package that may need compilation +$PIP install $PIP_USER_ARGUMENT -U pip packaging setuptools wheel if [ "$GITHUB_ACTIONS" == "true" ]; then PIP_USER_ARGUMENT+=" --progress-bar off" From 0272f59d0c3ae479d94081c922b31e8ccdf884d8 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Tue, 16 Jul 2024 13:38:01 +0100 Subject: [PATCH 045/124] Tools: autotest: add rover-omni3mecanum - Add frame rover-omni3mecanum. - Remove params with default values. Signed-off-by: Rhys Mainwaring --- .../default_params/rover-omni3mecanum.parm | 25 +++++++++++++++++++ Tools/autotest/pysim/vehicleinfo.py | 5 ++++ 2 files changed, 30 insertions(+) create mode 100644 Tools/autotest/default_params/rover-omni3mecanum.parm diff --git a/Tools/autotest/default_params/rover-omni3mecanum.parm b/Tools/autotest/default_params/rover-omni3mecanum.parm new file mode 100644 index 00000000000000..3b5bfb1feb9fa8 --- /dev/null +++ b/Tools/autotest/default_params/rover-omni3mecanum.parm @@ -0,0 +1,25 @@ +ACRO_TURN_RATE 300.000000 +ATC_SPEED_P 0.200000 +ATC_SPEED_D 0.000010 +ATC_STR_RAT_D 0.000010 +ATC_STR_RAT_FF 0.100000 +ATC_STR_RAT_I 0.010000 +ATC_STR_RAT_MAX 360.000000 +ATC_STR_RAT_P 0.030000 +CRUISE_SPEED 1.0 +CRUISE_THROTTLE 42.0 +FRAME_TYPE 4 +PSC_VEL_P 0.500000 +RC2_MAX 2000 +RC2_MIN 1000 +RC4_MAX 2000 +RC4_MIN 1000 +SERVO1_FUNCTION 33 +SERVO2_FUNCTION 34 +SERVO2_MAX 2000 +SERVO2_MIN 1000 +SERVO3_FUNCTION 35 +WP_PIVOT_RATE 120.0 +WP_SPEED 1.0 + + diff --git a/Tools/autotest/pysim/vehicleinfo.py b/Tools/autotest/pysim/vehicleinfo.py index 73349731a3f927..ca07c51b8f19df 100644 --- a/Tools/autotest/pysim/vehicleinfo.py +++ b/Tools/autotest/pysim/vehicleinfo.py @@ -391,6 +391,11 @@ def __init__(self): "default_params_filename": ["default_params/rover.parm", "default_params/rover-skid.parm"], }, + "rover-omni3mecanum": { + "waf_target": "bin/ardurover", + "default_params_filename": ["default_params/rover.parm", + "default_params/rover-omni3mecanum.parm"], + }, "rover-vectored": { "waf_target": "bin/ardurover", "default_params_filename": ["default_params/rover.parm", From 4354072d34c99b5b30fedc0c8ab2411d93c6a733 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Wed, 17 Jul 2024 16:13:40 +0100 Subject: [PATCH 046/124] SITL: SIM_Rover: add simulation for omni3 mecanum rover Signed-off-by: Rhys Mainwaring --- libraries/SITL/SIM_Rover.cpp | 142 ++++++++++++++++++++++++++++------- libraries/SITL/SIM_Rover.h | 9 +++ 2 files changed, 125 insertions(+), 26 deletions(-) diff --git a/libraries/SITL/SIM_Rover.cpp b/libraries/SITL/SIM_Rover.cpp index c1a7afb608f445..3182700dd9ad04 100644 --- a/libraries/SITL/SIM_Rover.cpp +++ b/libraries/SITL/SIM_Rover.cpp @@ -21,6 +21,8 @@ #include #include +#include + namespace SITL { SimRover::SimRover(const char *frame_str) : @@ -41,10 +43,14 @@ SimRover::SimRover(const char *frame_str) : if (vectored_thrust) { printf("Vectored Thrust Rover Simulation Started\n"); } - lock_step_scheduled = true; -} + omni3 = strstr(frame_str, "omni3mecanum") != nullptr; + if (omni3) { + printf("Omni3 Mecanum Rover Simulation Started\n"); + } + lock_step_scheduled = true; +} /* return turning circle (diameter) in meters for steering angle proportion in degrees @@ -92,6 +98,50 @@ float SimRover::calc_lat_accel(float steering_angle, float speed) update the rover simulation by one time step */ void SimRover::update(const struct sitl_input &input) +{ + // how much time has passed? + float delta_time = frame_time_us * 1.0e-6f; + + // update gyro and accel_body according to frame type + if (omni3) { + update_omni3(input, delta_time); + } else { + update_ackermann_or_skid(input, delta_time); + } + + // common to all rovers + + // now in earth frame + Vector3f accel_earth = dcm * accel_body; + accel_earth += Vector3f(0, 0, GRAVITY_MSS); + + // we are on the ground, so our vertical accel is zero + accel_earth.z = 0; + + // work out acceleration as seen by the accelerometers. It sees the kinematic + // acceleration (ie. real movement), plus gravity + accel_body = dcm.transposed() * (accel_earth + Vector3f(0, 0, -GRAVITY_MSS)); + + // new velocity vector + velocity_ef += accel_earth * delta_time; + + // new position vector + position += (velocity_ef * delta_time).todouble(); + + update_external_payload(input); + + // update lat/lon/altitude + update_position(); + time_advance(); + + // update magnetic field + update_mag_field_bf(); +} + +/* + update the ackermann or skid rover simulation by one time step + */ +void SimRover::update_ackermann_or_skid(const struct sitl_input &input, float delta_time) { float steering, throttle; @@ -113,9 +163,6 @@ void SimRover::update(const struct sitl_input &input) } } - // how much time has passed? - float delta_time = frame_time_us * 1.0e-6f; - // speed in m/s in body frame Vector3f velocity_body = dcm.transposed() * velocity_ef; @@ -137,37 +184,80 @@ void SimRover::update(const struct sitl_input &input) dcm.rotate(gyro * delta_time); dcm.normalize(); - // accel in body frame due to motor + // accel in body frame due to motor (excluding gravity) accel_body = Vector3f(accel, 0, 0); // add in accel due to direction change accel_body.y += radians(yaw_rate) * speed; +} - // now in earth frame - Vector3f accel_earth = dcm * accel_body; - accel_earth += Vector3f(0, 0, GRAVITY_MSS); - - // we are on the ground, so our vertical accel is zero - accel_earth.z = 0; - - // work out acceleration as seen by the accelerometers. It sees the kinematic - // acceleration (ie. real movement), plus gravity - accel_body = dcm.transposed() * (accel_earth + Vector3f(0, 0, -GRAVITY_MSS)); +/* + update the omni3 rover simulation by one time step + */ +void SimRover::update_omni3(const struct sitl_input &input, float delta_time) +{ + // in omni3 mode the first three servos are motor speeds + float motor1 = 2*((input.servos[0]-1000)/1000.0f - 0.5f); + float motor2 = 2*((input.servos[1]-1000)/1000.0f - 0.5f); + float motor3 = 2*((input.servos[2]-1000)/1000.0f - 0.5f); + + // use forward kinematics to calculate body frame velocity + Vector3f wheel_ang_vel( + motor1 * omni3_wheel_max_ang_vel, + motor2 * omni3_wheel_max_ang_vel, + motor3 * omni3_wheel_max_ang_vel + ); + + // derivation of forward kinematics for an Omni3Mecanum rover + // A. Gfrerrer. "Geometry and kinematics of the Mecanum wheel", + // Computer Aided Geometric Design 25 (2008) 784–791. + // Retrieved from https://www.geometrie.tugraz.at/gfrerrer/publications/MecanumWheel.pdf. + // + // the frame is equilateral triangle + // + // d[i] = 0.18 m is distance from frame centre to each wheel + // r_w = 0.04725 m is the wheel radius. + // delta = radians(-45) is angle of the roller to the direction of forward rotation + // alpha[i] is the angle the wheel axis is rotated about the body z-axis + // c[i] = cos(alpha[i] + delta) + // s[i] = sin(alpha[i] + delta) + // + // wheel d[i] alpha[i] a_x[i] a_y[i] c[i] s[i] + // 1 0.18 1.04719 0.09 0.15588 0.965925 0.258819 + // 2 0.18 3.14159 -0.18 0.0 -0.707106 0.707106 + // 3 0.18 5.23598 0.09 -0.15588 -0.258819 -0.965925 + // + // k = 1/(r_w * sin(delta)) = -29.930445 is a scale factor + // + // inverse kinematic matrix + // M[i, 0] = k * c[i] + // M[i, 1] = k * s[i] + // M[i, 2] = k * (a_x[i] s[i] - a_y[i] c[i]) + // + // forward kinematics matrix: Minv = M^-1 + constexpr Matrix3f Minv( + -0.0215149, 0.01575, 0.0057649, + -0.0057649, -0.01575, 0.0215149, + 0.0875, 0.0875, 0.0875); + + // twist - this is the target linear and angular velocity + Vector3f twist = Minv * wheel_ang_vel; - // new velocity vector - velocity_ef += accel_earth * delta_time; + // speed in m/s in body frame + Vector3f velocity_body = dcm.transposed() * velocity_ef; - // new position vector - position += (velocity_ef * delta_time).todouble(); + // linear acceleration in m/s/s - very crude model + float accel_x = omni3_max_accel * (twist.x - velocity_body.x) / omni3_max_speed; + float accel_y = omni3_max_accel * (twist.y - velocity_body.y) / omni3_max_speed; - update_external_payload(input); + gyro = Vector3f(0, 0, twist.z); - // update lat/lon/altitude - update_position(); - time_advance(); + // update attitude + dcm.rotate(gyro * delta_time); + dcm.normalize(); - // update magnetic field - update_mag_field_bf(); + // accel in body frame due to motors (excluding gravity) + accel_body = Vector3f(accel_x, accel_y, 0); } } // namespace SITL diff --git a/libraries/SITL/SIM_Rover.h b/libraries/SITL/SIM_Rover.h index 42009a48a177f7..113bc55dfc1a82 100644 --- a/libraries/SITL/SIM_Rover.h +++ b/libraries/SITL/SIM_Rover.h @@ -51,6 +51,15 @@ class SimRover : public Aircraft { float vectored_angle_max = 90.0f; // maximum angle (in degrees) to which thrust can be turned float vectored_turn_rate_max = 90.0f; // maximum turn rate (in deg/sec) with full throttle angled at 90deg + // omni3 Mecanum related members + bool omni3; // true if vehicle is omni-directional with 3 Mecanum wheels + float omni3_max_speed = 2.3625f; // omni vehicle's maximum forward speed in m/s + float omni3_max_accel = 1.0f; // omni vehicle's maximum forward acceleration in m/s/s + float omni3_wheel_max_ang_vel = 50.0f; // omni vehicle's wheel maximum angular velocity in rad/s + + void update_ackermann_or_skid(const struct sitl_input &input, float delta_time); + void update_omni3(const struct sitl_input &input, float delta_time); + float turn_circle(float steering) const; float calc_yaw_rate(float steering, float speed); float calc_lat_accel(float steering_angle, float speed); From 0190e70da4cdbd7609676703953ab86ce2205ccd Mon Sep 17 00:00:00 2001 From: Alex P Date: Tue, 23 Jul 2024 06:10:35 +0300 Subject: [PATCH 047/124] Tools: Reserving board id for Stellar F4 in board_types.txt --- Tools/AP_Bootloader/board_types.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/Tools/AP_Bootloader/board_types.txt b/Tools/AP_Bootloader/board_types.txt index 171f4f1249e107..ed687c3241f95c 100644 --- a/Tools/AP_Bootloader/board_types.txt +++ b/Tools/AP_Bootloader/board_types.txt @@ -312,6 +312,8 @@ AP_HW_LongbowF405 1422 AP_HW_MountainEagleH743 1444 +AP_HW_StellarF4 1500 + AP_HW_SakuraRC-H743 2714 # IDs 4200-4220 reserved for HAKRC From 03bdbea77cf2a9e775fa0566755fa4dcaf8d4b81 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 23 Jul 2024 08:42:28 +1000 Subject: [PATCH 048/124] hwdef: enable relay support in MatekL431-DShot fw the PWM expansion boards can also be used for relay control, often combined with PWM output --- libraries/AP_HAL_ChibiOS/hwdef/MatekL431-DShot/hwdef.dat | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MatekL431-DShot/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/MatekL431-DShot/hwdef.dat index 5c27d1c9e6756e..d74ef0f7cb173c 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/MatekL431-DShot/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/MatekL431-DShot/hwdef.dat @@ -32,3 +32,7 @@ define HAL_PERIPH_ENABLE_NOTIFY define HAL_SERIAL_ESC_COMM_ENABLED 1 define HAL_SUPPORT_RCOUT_SERIAL 1 define HAL_WITH_ESC_TELEM 1 + +# also enable relay output via hardpoint msgs +define HAL_PERIPH_ENABLE_RELAY +define AP_RELAY_ENABLED 1 From 230269bed8ca5e0a4cfc17589646e22d0511629c Mon Sep 17 00:00:00 2001 From: Bob Long Date: Fri, 16 Feb 2024 13:31:14 -0500 Subject: [PATCH 049/124] HAL_ChibiOS: add adjustable wdg timeout for hwdefs This allows the watchdog timeout to be adjusted in the hwdef.dat file, so that critical nodes like ESCs can recover more quickly. --- Tools/AP_Periph/AP_Periph.cpp | 2 ++ .../AP_HAL_ChibiOS/hwdef/common/watchdog.c | 19 +++++++++++++++---- .../AP_HAL_ChibiOS/hwdef/common/watchdog.h | 2 +- 3 files changed, 18 insertions(+), 5 deletions(-) diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp index 976dbc5834bcbc..b952d7ae2495cb 100644 --- a/Tools/AP_Periph/AP_Periph.cpp +++ b/Tools/AP_Periph/AP_Periph.cpp @@ -608,7 +608,9 @@ void AP_Periph_FW::prepare_reboot() // delay to give the ACK a chance to get out, the LEDs to flash, // the IO board safety to be forced on, the parameters to flush, + hal.scheduler->expect_delay_ms(100); hal.scheduler->delay(40); + hal.scheduler->expect_delay_ms(0); } /* diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.c b/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.c index c1a5eeb0493a97..689f9e4acc1c84 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.c +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.c @@ -22,6 +22,16 @@ #error "Unsupported IWDG RCC MCU config" #endif +/* + define for controlling how long the watchdog is set for. +*/ +#ifndef STM32_WDG_TIMEOUT_MS +#define STM32_WDG_TIMEOUT_MS 2048 +#endif +#if STM32_WDG_TIMEOUT_MS > 4096 || STM32_WDG_TIMEOUT_MS < 20 +#error "Watchdog timeout out of range" +#endif + /* defines for working out if the reset was from the watchdog */ @@ -68,16 +78,17 @@ static bool watchdog_enabled; */ void stm32_watchdog_init(void) { - // setup for 2s reset + // setup the watchdog timeout + // t = 4 * 2^PR * (RLR+1) / 32KHz IWDGD.KR = 0x5555; - IWDGD.PR = 2; // div16 - IWDGD.RLR = 0xFFF; + IWDGD.PR = 3; // changing this would change the definition of STM32_WDG_TIMEOUT_MS + IWDGD.RLR = STM32_WDG_TIMEOUT_MS - 1; IWDGD.KR = 0xCCCC; watchdog_enabled = true; } /* - pat the dog, to prevent a reset. If not called for 1s + pat the dog, to prevent a reset. If not called for STM32_WDG_TIMEOUT_MS after stm32_watchdog_init() then MCU will reset */ void stm32_watchdog_pat(void) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.h b/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.h index f015dee9c1a135..de8e3941494b08 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.h @@ -10,7 +10,7 @@ extern "C" { void stm32_watchdog_init(void); /* - pat the dog, to prevent a reset. If not called for 1s + pat the dog, to prevent a reset. If not called for STM32_WDG_TIMEOUT_MS after stm32_watchdog_init() then MCU will reset */ void stm32_watchdog_pat(void); From 738586342c3d5fc478d969fa70074c3c8440cc38 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Tue, 2 Jan 2024 19:47:43 +0000 Subject: [PATCH 050/124] Copter: notify when fence breach has cleared output fence breach type when switching mode without a fence action do not go into manual recovery only recover if there is a fence action implement auto-takeoff fence options output user-friendly fence names auto-enable fences on auto-takeoff use fence enable_configured() simplify message printing --- ArduCopter/fence.cpp | 5 +++-- ArduCopter/mode.cpp | 10 ++++++---- ArduCopter/mode_auto.cpp | 4 ++-- ArduCopter/mode_guided.cpp | 3 +++ 4 files changed, 14 insertions(+), 8 deletions(-) diff --git a/ArduCopter/fence.cpp b/ArduCopter/fence.cpp index 3bfe085c2b8697..aba1ecce5bb84e 100644 --- a/ArduCopter/fence.cpp +++ b/ArduCopter/fence.cpp @@ -24,7 +24,7 @@ void Copter::fence_check() if (new_breaches) { if (!copter.ap.land_complete) { - GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Fence Breached"); + fence.print_fence_message("breached", new_breaches); } // if the user wants some kind of response and motors are armed @@ -81,7 +81,8 @@ void Copter::fence_check() LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode(new_breaches)); - } else if (orig_breaches) { + } else if (orig_breaches && fence.get_breaches() == 0) { + GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Fence breach cleared"); // record clearing of breach LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode::ERROR_RESOLVED); } diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index ce191b0c1ef08d..69cda906803ec9 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -371,10 +371,12 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason) #endif #if AP_FENCE_ENABLED - // pilot requested flight mode change during a fence breach indicates pilot is attempting to manually recover - // this flight mode change could be automatic (i.e. fence, battery, GPS or GCS failsafe) - // but it should be harmless to disable the fence temporarily in these situations as well - fence.manual_recovery_start(); + if (fence.get_action() != AC_FENCE_ACTION_REPORT_ONLY) { + // pilot requested flight mode change during a fence breach indicates pilot is attempting to manually recover + // this flight mode change could be automatic (i.e. fence, battery, GPS or GCS failsafe) + // but it should be harmless to disable the fence temporarily in these situations as well + fence.manual_recovery_start(); + } #endif #if AP_CAMERA_ENABLED diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 3ac943f06dbc58..9a6b684cf1e8a0 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -791,10 +791,10 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd) case MAV_CMD_DO_FENCE_ENABLE: #if AP_FENCE_ENABLED if (cmd.p1 == 0) { //disable - copter.fence.enable(false); + copter.fence.enable_configured(false); gcs().send_text(MAV_SEVERITY_INFO, "Fence Disabled"); } else { //enable fence - copter.fence.enable(true); + copter.fence.enable_configured(true); gcs().send_text(MAV_SEVERITY_INFO, "Fence Enabled"); } #endif //AP_FENCE_ENABLED diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index a771c0fdaa0efa..f22501e47fb70f 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -685,6 +685,9 @@ void ModeGuided::takeoff_run() auto_takeoff.run(); if (auto_takeoff.complete && !takeoff_complete) { takeoff_complete = true; +#if AP_FENCE_ENABLED + copter.fence.auto_enable_fence_after_takeoff(); +#endif #if AP_LANDINGGEAR_ENABLED // optionally retract landing gear copter.landinggear.retract_after_takeoff(); From f73154349bb7b73c8853ce25802f718643d8e539 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 13 Jan 2024 18:02:54 +0000 Subject: [PATCH 051/124] GCS_MAVLink: use bitmask based enablement for fences --- libraries/GCS_MAVLink/GCS_Fence.cpp | 26 +++++++++++++++----------- 1 file changed, 15 insertions(+), 11 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS_Fence.cpp b/libraries/GCS_MAVLink/GCS_Fence.cpp index d6dad1a76990c9..b3bfb78a37194f 100644 --- a/libraries/GCS_MAVLink/GCS_Fence.cpp +++ b/libraries/GCS_MAVLink/GCS_Fence.cpp @@ -16,20 +16,24 @@ MAV_RESULT GCS_MAVLINK::handle_command_do_fence_enable(const mavlink_command_int return MAV_RESULT_UNSUPPORTED; } - switch ((uint16_t)packet.param1) { - case 0: // disable fence - fence->enable(false); + uint8_t fences = AC_FENCE_ALL_FENCES; + if (uint8_t(packet.param2)) { + fences = uint8_t(packet.param2); + } + + switch (AC_Fence::MavlinkFenceActions(packet.param1)) { + case AC_Fence::MavlinkFenceActions::DISABLE_FENCE: + fence->enable(false, fences); return MAV_RESULT_ACCEPTED; - case 1: // enable fence - if (!fence->present()) - { + case AC_Fence::MavlinkFenceActions::ENABLE_FENCE: + if (!(fence->present() & fences)) { return MAV_RESULT_FAILED; } - - fence->enable(true); + + fence->enable(true, fences); return MAV_RESULT_ACCEPTED; - case 2: // disable fence floor only - fence->disable_floor(); + case AC_Fence::MavlinkFenceActions::DISABLE_ALT_MIN_FENCE: + fence->enable(false, AC_FENCE_TYPE_ALT_MIN); return MAV_RESULT_ACCEPTED; default: return MAV_RESULT_FAILED; @@ -82,7 +86,7 @@ void GCS_MAVLINK::send_fence_status() const mavlink_breach_type = FENCE_BREACH_BOUNDARY; } - // report on Avoidance liminting + // report on Avoidance limiting uint8_t breach_mitigation = FENCE_MITIGATE_UNKNOWN; #if AP_AVOIDANCE_ENABLED && !APM_BUILD_TYPE(APM_BUILD_ArduPlane) const AC_Avoid* avoid = AC_Avoid::get_singleton(); From 95b39f64e3cdbf02f0969e816391f96157af5fca Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 13 Jan 2024 18:03:19 +0000 Subject: [PATCH 052/124] AP_Mission: add comment about new fence API --- libraries/AP_Mission/AP_Mission.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index ff42107ff64188..c11b574e8338f0 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -1233,7 +1233,8 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_ break; case MAV_CMD_DO_FENCE_ENABLE: // MAV ID: 207 - cmd.p1 = packet.param1; // action 0=disable, 1=enable + cmd.p1 = packet.param1; // action 0=disable, 1=enable, 2=disable floor + // packet.param2; // bitmask see FENCE_TYPE enum break; case MAV_CMD_DO_AUX_FUNCTION: @@ -1746,7 +1747,7 @@ bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& c break; case MAV_CMD_DO_FENCE_ENABLE: // MAV ID: 207 - packet.param1 = cmd.p1; // action 0=disable, 1=enable + packet.param1 = cmd.p1; // action 0=disable, 1=enable, 2=disable floor, 3=enable except floor break; case MAV_CMD_DO_PARACHUTE: // MAV ID: 208 From 8287d4f2d076c27d1c2a920455df4e63c32fa54d Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Mon, 15 Jan 2024 19:35:03 +0000 Subject: [PATCH 053/124] AC_Avoidance: take into account minimum altitude fence when calculating climb rate --- libraries/AC_Avoidance/AC_Avoid.cpp | 40 +++++++++++++++++++++++------ 1 file changed, 32 insertions(+), 8 deletions(-) diff --git a/libraries/AC_Avoidance/AC_Avoid.cpp b/libraries/AC_Avoidance/AC_Avoid.cpp index 8adc6d7c6dc4a4..2756e4b8fddd83 100644 --- a/libraries/AC_Avoidance/AC_Avoid.cpp +++ b/libraries/AC_Avoidance/AC_Avoid.cpp @@ -379,29 +379,53 @@ void AC_Avoid::adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_c return; } - // do not adjust climb_rate if level or descending - if (climb_rate_cms <= 0.0f) { + // do not adjust climb_rate if level + if (is_zero(climb_rate_cms)) { return; } + const AP_AHRS &_ahrs = AP::ahrs(); // limit acceleration const float accel_cmss_limited = MIN(accel_cmss, AC_AVOID_ACCEL_CMSS_MAX); + // if descending, adjust for minimum altitude as necessary + if (climb_rate_cms < 0.0f) { +#if AP_FENCE_ENABLED + // calculate distance below fence + AC_Fence *fence = AP::fence(); + if ((_enabled & AC_AVOID_STOP_AT_FENCE) > 0 && fence + && (fence->get_enabled_fences() & AC_FENCE_TYPE_ALT_MIN) > 0) { + // calculate distance from vehicle to safe altitude + float veh_alt; + _ahrs.get_relative_position_D_home(veh_alt); + // fence.get_safe_alt_min() is UP, veh_alt is DOWN: + const float alt_min = -(fence->get_safe_alt_min() + veh_alt); + + if (alt_min <= 0.0f) { + climb_rate_cms = MAX(climb_rate_cms, 0.0f); + // also calculate backup speed that will get us back to safe altitude + backup_speed = get_max_speed(kP, accel_cmss_limited, -alt_min*100.0f, dt); + } + } +#endif + return; + } + bool limit_alt = false; float alt_diff = 0.0f; // distance from altitude limit to vehicle in metres (positive means vehicle is below limit) - const AP_AHRS &_ahrs = AP::ahrs(); - #if AP_FENCE_ENABLED // calculate distance below fence AC_Fence *fence = AP::fence(); - if ((_enabled & AC_AVOID_STOP_AT_FENCE) > 0 && fence && (fence->get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) > 0) { + if ((_enabled & AC_AVOID_STOP_AT_FENCE) > 0 && fence) { // calculate distance from vehicle to safe altitude float veh_alt; _ahrs.get_relative_position_D_home(veh_alt); - // _fence.get_safe_alt_max() is UP, veh_alt is DOWN: - alt_diff = fence->get_safe_alt_max() + veh_alt; - limit_alt = true; + if ((fence->get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) > 0) { + // fence.get_safe_alt_max() is UP, veh_alt is DOWN: + alt_diff = fence->get_safe_alt_max() + veh_alt; + limit_alt = true; + } } #endif From 3dbcbe00267ebf30ed65dbc5b2cfc96137e0ce77 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 19 Jan 2024 12:30:46 +0000 Subject: [PATCH 054/124] AP_Arming: do not enable minimim altitude fence on arming call appropriate fence method for auto-enablement --- libraries/AP_Arming/AP_Arming.cpp | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp index fa8cc8aab0e620..83e7cac17447f2 100644 --- a/libraries/AP_Arming/AP_Arming.cpp +++ b/libraries/AP_Arming/AP_Arming.cpp @@ -1797,11 +1797,7 @@ bool AP_Arming::arm(AP_Arming::Method method, const bool do_arming_checks) if (armed) { auto *fence = AP::fence(); if (fence != nullptr) { - // If a fence is set to auto-enable, turn on the fence - if (fence->auto_enabled() == AC_Fence::AutoEnable::ONLY_WHEN_ARMED) { - fence->enable(true); - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Fence: auto-enabled"); - } + fence->auto_enable_fence_on_arming(); } } #endif @@ -1845,7 +1841,7 @@ bool AP_Arming::disarm(const AP_Arming::Method method, bool do_disarm_checks) AC_Fence *fence = AP::fence(); if (fence != nullptr) { if(fence->auto_enabled() == AC_Fence::AutoEnable::ONLY_WHEN_ARMED) { - fence->enable(false); + fence->enable_configured(false); } } #endif From 8c2f686bfa1859f23edc1b4b4e66b4c1d794ddb9 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 19 Jan 2024 20:43:45 +0000 Subject: [PATCH 055/124] Plane: output user-friendly fence messages --- ArduPlane/fence.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduPlane/fence.cpp b/ArduPlane/fence.cpp index 33062460df6b0e..bef88306ebc583 100644 --- a/ArduPlane/fence.cpp +++ b/ArduPlane/fence.cpp @@ -50,7 +50,7 @@ void Plane::fence_check() } if (new_breaches) { - GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Fence Breached"); + fence.print_fence_message("breached", new_breaches); // if the user wants some kind of response and motors are armed const uint8_t fence_act = fence.get_action(); From 0c6ea4790dd6ad4101ac3c76172075e44833c106 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 27 Jan 2024 16:36:35 +0000 Subject: [PATCH 056/124] AP_Common: fix initialization of ExpandingString so that it can be used on the stack zero out passed in buffers for ExpandingString --- libraries/AP_Common/ExpandingString.cpp | 7 +++++++ libraries/AP_Common/ExpandingString.h | 2 ++ 2 files changed, 9 insertions(+) diff --git a/libraries/AP_Common/ExpandingString.cpp b/libraries/AP_Common/ExpandingString.cpp index 9be2b3a91abf17..a3e7a8b4019580 100644 --- a/libraries/AP_Common/ExpandingString.cpp +++ b/libraries/AP_Common/ExpandingString.cpp @@ -25,6 +25,13 @@ extern const AP_HAL::HAL& hal; #define EXPAND_INCREMENT 512 +ExpandingString::ExpandingString(char* s, uint32_t total_len) : buf(0) +{ + set_buffer(s, total_len, 0); + memset(buf, 0, buflen); +} + + /* expand the string buffer */ diff --git a/libraries/AP_Common/ExpandingString.h b/libraries/AP_Common/ExpandingString.h index 2561332f3e89d8..d92882e1b3af6f 100644 --- a/libraries/AP_Common/ExpandingString.h +++ b/libraries/AP_Common/ExpandingString.h @@ -24,6 +24,8 @@ class ExpandingString { public: + ExpandingString() : buf(0), buflen(0), used(0), allocation_failed(false), external_buffer(false) {} + ExpandingString(char* s, uint32_t total_len); const char *get_string(void) const { return buf; From d16615923d6da1602bb9d7b7fe40c5317ffe5012 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 8 Feb 2024 14:44:25 +0000 Subject: [PATCH 057/124] AP_Frsky_Telem: use fence enable_configured() --- libraries/AP_Frsky_Telem/AP_Frsky_MAVliteMsgHandler.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_MAVliteMsgHandler.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_MAVliteMsgHandler.cpp index 623ad0184f748e..afc1f8b7f1a638 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_MAVliteMsgHandler.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_MAVliteMsgHandler.cpp @@ -153,10 +153,10 @@ MAV_RESULT AP_Frsky_MAVliteMsgHandler::handle_command_do_fence_enable(const mavl switch ((uint16_t)mav_command_long.param1) { case 0: - fence->enable(false); + fence->enable_configured(false); return MAV_RESULT_ACCEPTED; case 1: - fence->enable(true); + fence->enable_configured(true); return MAV_RESULT_ACCEPTED; default: return MAV_RESULT_FAILED; From 0288e1e79c632fa7f1469a6a7f7e9fb5fab65e22 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 8 Feb 2024 14:44:48 +0000 Subject: [PATCH 058/124] RC_Channel: use fence enable_configured() --- libraries/RC_Channel/RC_Channel.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index a2de561a77923d..8d27aeb426eb10 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -1139,7 +1139,7 @@ void RC_Channel::do_aux_function_fence(const AuxSwitchPos ch_flag) return; } - fence->enable(ch_flag == AuxSwitchPos::HIGH); + fence->enable_configured(ch_flag == AuxSwitchPos::HIGH); } #endif From 559bd1e252e3e74d47866ed30faab9893c5520c1 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 8 Feb 2024 14:46:01 +0000 Subject: [PATCH 059/124] Plane: use fence enable_configured() avoid fence breach clearing spam --- ArduPlane/commands_logic.cpp | 4 ++-- ArduPlane/fence.cpp | 3 ++- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 4766cea5c1fbae..fc87bb4ad63b2f 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -147,10 +147,10 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd) case MAV_CMD_DO_FENCE_ENABLE: #if AP_FENCE_ENABLED if (cmd.p1 == 0) { // disable fence - plane.fence.enable(false); + plane.fence.enable_configured(false); gcs().send_text(MAV_SEVERITY_INFO, "Fence disabled"); } else if (cmd.p1 == 1) { // enable fence - plane.fence.enable(true); + plane.fence.enable_configured(true); gcs().send_text(MAV_SEVERITY_INFO, "Fence enabled"); } else if (cmd.p1 == 2) { // disable fence floor only plane.fence.disable_floor(); diff --git a/ArduPlane/fence.cpp b/ArduPlane/fence.cpp index bef88306ebc583..9615082244736b 100644 --- a/ArduPlane/fence.cpp +++ b/ArduPlane/fence.cpp @@ -115,7 +115,8 @@ void Plane::fence_check() } LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode(new_breaches)); - } else if (orig_breaches) { + } else if (orig_breaches && fence.get_breaches() == 0) { + GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Fence breach cleared"); // record clearing of breach LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode::ERROR_RESOLVED); } From 4fc59ae67d0d8550ac22d4b40b2577569c62de21 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 8 Feb 2024 17:09:19 +0000 Subject: [PATCH 060/124] Rover: use fence enable_configured() --- Rover/mode_auto.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Rover/mode_auto.cpp b/Rover/mode_auto.cpp index fa1ea8ece212c0..f18ab0a8f9db95 100644 --- a/Rover/mode_auto.cpp +++ b/Rover/mode_auto.cpp @@ -596,10 +596,10 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd) case MAV_CMD_DO_FENCE_ENABLE: #if AP_FENCE_ENABLED if (cmd.p1 == 0) { //disable - rover.fence.enable(false); + rover.fence.enable_configured(false); gcs().send_text(MAV_SEVERITY_INFO, "Fence Disabled"); } else { //enable fence - rover.fence.enable(true); + rover.fence.enable_configured(true); gcs().send_text(MAV_SEVERITY_INFO, "Fence Enabled"); } #endif From a6300e35d0b66224f01cf1961f1313875c858d6f Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Fri, 16 Feb 2024 14:54:56 -0600 Subject: [PATCH 061/124] Plane: prevent Mode Takeoff calling fence autoenable more than once --- ArduPlane/Plane.h | 3 +++ ArduPlane/mode_takeoff.cpp | 6 +++++- 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index b1af208397ff30..443f2d56a1fb4b 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -250,6 +250,9 @@ class Plane : public AP_Vehicle { // are we currently in long failsafe but have postponed it in MODE TAKEOFF until min level alt is reached bool long_failsafe_pending; + + //flag that we have already called autoenable fences once in MODE TAKEOFF + bool have_autoenabled_fences; // GCS selection GCS_Plane _gcs; // avoid using this; use gcs() diff --git a/ArduPlane/mode_takeoff.cpp b/ArduPlane/mode_takeoff.cpp index 37d0203fcf413f..0854e512450a94 100644 --- a/ArduPlane/mode_takeoff.cpp +++ b/ArduPlane/mode_takeoff.cpp @@ -63,6 +63,7 @@ ModeTakeoff::ModeTakeoff() : bool ModeTakeoff::_enter() { takeoff_mode_setup = false; + plane.have_autoenabled_fences = false; return true; } @@ -154,7 +155,10 @@ void ModeTakeoff::update() } else { if ((altitude_cm >= alt * 100 - 200)) { //within 2m of TKOFF_ALT ,or above and loitering #if AP_FENCE_ENABLED - plane.fence.auto_enable_fence_after_takeoff(); + if (!plane.have_autoenabled_fences) { + plane.fence.auto_enable_fence_after_takeoff(); + plane.have_autoenabled_fences = true; + } #endif plane.calc_nav_roll(); plane.calc_nav_pitch(); From 370b0d7b9c8de98cd33d73e41198925ada84bf8c Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 17 Feb 2024 19:55:22 +0000 Subject: [PATCH 062/124] AP_Logger: add events for all fence types --- libraries/AP_Logger/AP_Logger.h | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h index d4c605bcced530..e8a91d62da63d5 100644 --- a/libraries/AP_Logger/AP_Logger.h +++ b/libraries/AP_Logger/AP_Logger.h @@ -79,8 +79,15 @@ enum class LogEvent : uint8_t { STANDBY_ENABLE = 74, STANDBY_DISABLE = 75, - FENCE_FLOOR_ENABLE = 80, - FENCE_FLOOR_DISABLE = 81, + // Fence events + FENCE_ALT_MAX_ENABLE = 76, + FENCE_ALT_MAX_DISABLE = 77, + FENCE_CIRCLE_ENABLE = 78, + FENCE_CIRCLE_DISABLE = 79, + FENCE_ALT_MIN_ENABLE = 80, + FENCE_ALT_MIN_DISABLE = 81, + FENCE_POLYGON_ENABLE = 82, + FENCE_POLYGON_DISABLE = 83, // if the EKF's source input set is changed (e.g. via a switch or // a script), we log an event: From f0f8187c7f2ec0d62ffca11c6cffb98bb9104357 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Mon, 4 Dec 2023 16:58:40 +0000 Subject: [PATCH 063/124] AC_Fence: add ability to auto-enable fence floor while doing fence checks control copter floor fence with autoenable autoenable floor fence with a margin check for manual recovery only after having checked the fences make auto-disabling for minimum altitude fence an option output message when fence floor auto-enabled re-use fence floor auto-enable/disable from plane auto-disable on landing do not update enable parameter when controlling through mavlink make sure get_enabled_fences() actually returns enabled fences. make current fences enabled internal state rather than persistent implement auto options correctly and on copter add fence names utility use ExpandingString for constructing fence names correctly check whether fences are enabled or not and disable min alt for landing in all auto modes add enable_configured() for use by mavlink and rc add events for all fence types make sure that auto fences are no longer candidates after manual updates add fence debug make sure rc switch is the ultimate authority on fences reset auto mask when enabling or disabling fencing ensure auto-enable on arming works as intended simplify printing fence notices reset autofences when auot-enablement is changed --- libraries/AC_Fence/AC_Fence.cpp | 333 +++++++++++++++++++++++--------- libraries/AC_Fence/AC_Fence.h | 61 ++++-- 2 files changed, 287 insertions(+), 107 deletions(-) diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp index 04a238196f58fe..1ba6cc27bfac53 100644 --- a/libraries/AC_Fence/AC_Fence.cpp +++ b/libraries/AC_Fence/AC_Fence.cpp @@ -40,22 +40,24 @@ extern const AP_HAL::HAL& hal; #define AC_FENCE_CIRCLE_RADIUS_BACKUP_DISTANCE 20.0 // after fence is broken we recreate the fence 20m further out #endif +//#define AC_FENCE_DEBUG const AP_Param::GroupInfo AC_Fence::var_info[] = { + // @Param: ENABLE // @DisplayName: Fence enable/disable - // @Description: Allows you to enable (1) or disable (0) the fence functionality + // @Description: Allows you to enable (1) or disable (0) the fence functionality. Fences can still be enabled and disabled via mavlink or an RC option, but these changes are not persisted. // @Values: 0:Disabled,1:Enabled // @User: Standard AP_GROUPINFO("ENABLE", 0, AC_Fence, _enabled, 0), // @Param: TYPE // @DisplayName: Fence Type - // @Description: Enabled fence types held as bitmask + // @Description: Configured fence types held as bitmask. Max altitide, Circle and Polygon fences will be immediately enabled if configured. Min altitude fence will only be enabled once the minimum altitude is reached. // @Bitmask{Rover}: 1:Circle Centered on Home,2:Inclusion/Exclusion Circles+Polygons // @Bitmask{Copter, Plane, Sub}: 0:Max altitude,1:Circle Centered on Home,2:Inclusion/Exclusion Circles+Polygons,3:Min altitude // @User: Standard - AP_GROUPINFO("TYPE", 1, AC_Fence, _enabled_fences, AC_FENCE_TYPE_DEFAULT), + AP_GROUPINFO("TYPE", 1, AC_Fence, _configured_fences, AC_FENCE_TYPE_DEFAULT), // @Param: ACTION // @DisplayName: Fence Action @@ -126,18 +128,18 @@ const AP_Param::GroupInfo AC_Fence::var_info[] = { // @User: Standard AP_GROUPINFO_FRAME("RET_ALT", 9, AC_Fence, _ret_altitude, 0.0f, AP_PARAM_FRAME_PLANE), - // @Param{Plane}: AUTOENABLE + // @Param{Plane, Copter}: AUTOENABLE // @DisplayName: Fence Auto-Enable - // @Description: Auto-enable of fences. AutoEnableOnTakeoff enables all configured fences after autotakeoffs reach altitude. During autolandings the fences will be disabled. AutoEnableDisableFloorOnLanding enables all configured fences after autotakeoffs reach altitude. During autolandings only the Minimum Altitude fence will be disabled. AutoEnableOnlyWhenArmed enables all configured fences, but no fences are disabled during autolandings. However, fence breaches are ignored while executing prior breach recovery actions which may include autolandings. - // @Values: 0:AutoEnableOff,1:AutoEnableOnTakeoff,2:AutoEnableDisableFloorOnLanding,3:AutoEnableOnlyWhenArmed + // @Description: Auto-enable of fences. AutoEnableOnTakeoff enables all configured fences, except the minimum altitude fence (which is enabled when the minimum altitude is reached), after autotakeoffs reach altitude. During autolandings the fences will be disabled. AutoEnableDisableFloorOnLanding enables all configured fences, except the minimum altitude fence (which is enabled when the minimum altitude is reached), after autotakeoffs reach altitude. During autolandings only the Minimum Altitude fence will be disabled. AutoEnableOnlyWhenArmed enables all configured fences on arming, except the minimum altitude fence (which is enabled when the minimum altitude is reached), but no fences are disabled during autolandings. However, fence breaches are ignored while executing prior breach recovery actions which may include autolandings. + // @Values{Plane, Copter}: 0:AutoEnableOff,1:AutoEnableOnTakeoff,2:AutoEnableDisableFloorOnLanding,3:AutoEnableOnlyWhenArmed // @Range: 0 3 // @Increment: 1 // @User: Standard - AP_GROUPINFO_FRAME("AUTOENABLE", 10, AC_Fence, _auto_enabled, static_cast(AutoEnable::ALWAYS_DISABLED), AP_PARAM_FRAME_PLANE), + AP_GROUPINFO_FRAME("AUTOENABLE", 10, AC_Fence, _auto_enabled, static_cast(AutoEnable::ALWAYS_DISABLED), AP_PARAM_FRAME_PLANE | AP_PARAM_FRAME_COPTER | AP_PARAM_FRAME_TRICOPTER | AP_PARAM_FRAME_HELI), // @Param{Plane}: OPTIONS // @DisplayName: Fence options - // @Description: 0:Disable mode change following fence action until fence breach is cleared. When bit 1 is set the allowable flight areas is the union of all polygon and circle fence areas instead of the intersection, which means a fence breach occurs only if you are outside all of the fence areas. + // @Description: When bit 0 is set sisable mode change following fence action until fence breach is cleared. When bit 1 is set the allowable flight areas is the union of all polygon and circle fence areas instead of the intersection, which means a fence breach occurs only if you are outside all of the fence areas. // @Bitmask: 0:Disable mode change following fence action until fence breach is cleared, 1:Allow union of inclusion areas // @User: Standard AP_GROUPINFO_FRAME("OPTIONS", 11, AC_Fence, _options, static_cast(OPTIONS::DISABLE_MODE_CHANGE), AP_PARAM_FRAME_PLANE), @@ -155,49 +157,156 @@ AC_Fence::AC_Fence() #endif _singleton = this; AP_Param::setup_object_defaults(this, var_info); + if (_enabled) { + _enabled_fences = _configured_fences.get() & ~AC_FENCE_TYPE_ALT_MIN; + } +} + +// get a user-friendly list of fences +void AC_Fence::get_fence_names(uint8_t fences, ExpandingString& msg) +{ + if (!fences) { + return; + } + static const char* FENCE_NAMES[] = { + "Max Alt", + "Circle", + "Polygon", + "Min Alt", + }; + uint8_t i = 0; + uint8_t nfences = 0; + while (fences !=0) { + if (fences & 0x1) { + if (nfences > 0) { + if (!(fences & ~1U)) { + msg.printf(" and "); + } else { + msg.printf(", "); + } + } + msg.printf("%s", FENCE_NAMES[i]); + nfences++; + } + fences >>= 1; + i++; + } + msg.printf(" fence"); + if (nfences>1) { + msg.printf("s"); + } +} + +// print a message about the passed in fences +void AC_Fence::print_fence_message(const char* message, uint8_t fences) const +{ + if (!fences) { + return; + } + + char msg[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1]; + ExpandingString e(msg, MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1); + AC_Fence::get_fence_names(fences, e); + GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "%s %s", e.get_writeable_string(), message); +} + +// should be called @10Hz to handle loading from eeprom +void AC_Fence::update() +{ + _poly_loader.update(); + // if someone changes the parameter we want to enable or disable everything + if (_enabled != _last_enabled || _auto_enabled != _last_auto_enabled) { + // reset the auto mask since we just reconfigured all of fencing + _auto_enable_mask = AC_FENCE_ALL_FENCES; + _last_enabled = _enabled; + _last_auto_enabled = _auto_enabled; + if (_enabled) { + _enabled_fences = _configured_fences.get() & ~AC_FENCE_TYPE_ALT_MIN; + } else { + _enabled_fences = 0; + } + } +#ifdef AC_FENCE_DEBUG + static uint32_t last_msg_count = 0; + if (get_enabled_fences() && last_msg_count++ % 10 == 0) { + print_fence_message("fences active", get_enabled_fences()); + } +#endif } /// enable the Fence code generally; a master switch for all fences -void AC_Fence::enable(bool value) +uint8_t AC_Fence::enable(bool value, uint8_t fence_types, bool update_auto_mask) { + uint8_t fences = _configured_fences.get() & fence_types; + uint8_t enabled_fences = _enabled_fences; + if (value) { + enabled_fences |= fences; + } else { + enabled_fences &= ~fences; + } + + // fences that were manually changed are no longer eligible for auto-enablement or disablement + if (update_auto_mask) { + // if we are explicitly enabling or disabling the alt min fence and it was auto-enabled then make sure + // it doesn't get re-enabled or disabled + if (fence_types & _auto_enable_mask & AC_FENCE_TYPE_ALT_MIN) { + _floor_disabled_for_landing = !value; + } + _auto_enable_mask &= ~fences; + } + + uint8_t fences_to_change = _enabled_fences ^ enabled_fences; + + if (!fences_to_change) { + return 0; + } #if HAL_LOGGING_ENABLED - if (_enabled && !value) { - AP::logger().Write_Event(LogEvent::FENCE_DISABLE); - } else if (!_enabled && value) { - AP::logger().Write_Event(LogEvent::FENCE_ENABLE); + AP::logger().Write_Event(value ? LogEvent::FENCE_ENABLE : LogEvent::FENCE_DISABLE); + if (fences_to_change & AC_FENCE_TYPE_ALT_MAX) { + AP::logger().Write_Event(value ? LogEvent::FENCE_ALT_MAX_ENABLE : LogEvent::FENCE_ALT_MAX_DISABLE); + } + if (fences_to_change & AC_FENCE_TYPE_CIRCLE) { + AP::logger().Write_Event(value ? LogEvent::FENCE_CIRCLE_ENABLE : LogEvent::FENCE_CIRCLE_DISABLE); + } + if (fences_to_change & AC_FENCE_TYPE_ALT_MIN) { + AP::logger().Write_Event(value ? LogEvent::FENCE_ALT_MIN_ENABLE : LogEvent::FENCE_ALT_MIN_DISABLE); + } + if (fences_to_change & AC_FENCE_TYPE_POLYGON) { + AP::logger().Write_Event(value ? LogEvent::FENCE_POLYGON_ENABLE : LogEvent::FENCE_POLYGON_DISABLE); } #endif - _enabled.set(value); + + _enabled_fences = enabled_fences; + if (!value) { - clear_breach(AC_FENCE_TYPE_ALT_MIN | AC_FENCE_TYPE_ALT_MAX | AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_POLYGON); - disable_floor(); - } else { - enable_floor(); + clear_breach(fences_to_change); } + + return fences_to_change; } /// enable/disable fence floor only void AC_Fence::enable_floor() { -#if HAL_LOGGING_ENABLED - if (!_floor_enabled) { - // Floor is currently disabled, enable it - AP::logger().Write_Event(LogEvent::FENCE_FLOOR_ENABLE); - } -#endif - _floor_enabled = true; + enable(true, AC_FENCE_TYPE_ALT_MIN); } void AC_Fence::disable_floor() { -#if HAL_LOGGING_ENABLED - if (_floor_enabled) { - // Floor is currently enabled, disable it - AP::logger().Write_Event(LogEvent::FENCE_FLOOR_DISABLE); + enable(false, AC_FENCE_TYPE_ALT_MIN); +} + +/* + called on arming +*/ +void AC_Fence::auto_enable_fence_on_arming(void) +{ + if (auto_enabled() != AC_Fence::AutoEnable::ONLY_WHEN_ARMED) { + return; } -#endif - _floor_enabled = false; - clear_breach(AC_FENCE_TYPE_ALT_MIN); + + const uint8_t fences = enable(true, _auto_enable_mask & ~AC_FENCE_TYPE_ALT_MIN, false); + print_fence_message("auto-enabled", fences); } /* @@ -205,15 +314,18 @@ void AC_Fence::disable_floor() */ void AC_Fence::auto_enable_fence_after_takeoff(void) { - if (_enabled) { - return; - } switch(auto_enabled()) { - case AC_Fence::AutoEnable::ALWAYS_ENABLED: + case AC_Fence::AutoEnable::ENABLE_ON_AUTO_TAKEOFF: case AC_Fence::AutoEnable::ENABLE_DISABLE_FLOOR_ONLY: - enable(true); - GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Fence enabled (auto enabled)"); + case AC_Fence::AutoEnable::ONLY_WHEN_ARMED: { + // auto-enable fences that aren't currently auto-enabled + if (_auto_enable_mask & AC_FENCE_TYPE_ALT_MIN) { + _floor_disabled_for_landing = false; + } + const uint8_t fences = enable(true, _auto_enable_mask, false); + print_fence_message("auto-enabled", fences); break; + } default: // fence does not auto-enable in other takeoff conditions break; @@ -226,13 +338,20 @@ void AC_Fence::auto_enable_fence_after_takeoff(void) void AC_Fence::auto_disable_fence_for_landing(void) { switch (auto_enabled()) { - case AC_Fence::AutoEnable::ALWAYS_ENABLED: - enable(false); - GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Fence disabled (auto disable)"); + case AC_Fence::AutoEnable::ENABLE_ON_AUTO_TAKEOFF: { + // disable only those fences which are allowed to be disabled + if (_auto_enable_mask & _enabled_fences & AC_FENCE_TYPE_ALT_MIN) { + _floor_disabled_for_landing = true; + } + const uint8_t fences = enable(false, _auto_enable_mask & _enabled_fences, false); + print_fence_message("auto-disabled", fences); break; + } case AC_Fence::AutoEnable::ENABLE_DISABLE_FLOOR_ONLY: + case AC_Fence::AutoEnable::ONLY_WHEN_ARMED: disable_floor(); - GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Fence floor disabled (auto disable)"); + _floor_disabled_for_landing = true; + GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Min Alt fence auto-disabled"); break; default: // fence does not auto-disable in other landing conditions @@ -240,36 +359,26 @@ void AC_Fence::auto_disable_fence_for_landing(void) } } -bool AC_Fence::present() const +uint8_t AC_Fence::present() const { - const auto enabled_fences = _enabled_fences.get(); - // A fence is present if any of the conditions are true. - // * tin can (circle) is enabled - // * min or max alt is enabled - // * polygon fences are enabled and any fence has been uploaded - if (enabled_fences & AC_FENCE_TYPE_CIRCLE || - enabled_fences & AC_FENCE_TYPE_ALT_MIN || - enabled_fences & AC_FENCE_TYPE_ALT_MAX || - ((enabled_fences & AC_FENCE_TYPE_POLYGON) && _poly_loader.total_fence_count() > 0)) { - return true; + uint8_t mask = AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_ALT_MIN | AC_FENCE_TYPE_ALT_MAX; + if (_poly_loader.total_fence_count() > 0) { + mask |= AC_FENCE_TYPE_POLYGON; } - return false; + return _configured_fences.get() & mask; } /// get_enabled_fences - returns bitmask of enabled fences uint8_t AC_Fence::get_enabled_fences() const { - if (!_enabled && !_auto_enabled) { - return 0; - } - return _enabled_fences; + return _enabled_fences & present(); } // additional checks for the polygon fence: bool AC_Fence::pre_arm_check_polygon(const char* &fail_msg) const { - if (!(_enabled_fences & AC_FENCE_TYPE_POLYGON)) { + if (!(_configured_fences & AC_FENCE_TYPE_POLYGON)) { // not enabled; all good return true; } @@ -324,20 +433,20 @@ bool AC_Fence::pre_arm_check(const char* &fail_msg) const fail_msg = nullptr; // if fences are enabled but none selected fail pre-arm check - if (enabled() && !present()) { + if (_enabled && !present()) { fail_msg = "Fences enabled, but none selected"; return false; } // if not enabled or not fence set-up always return true - if ((!_enabled && !_auto_enabled) || !_enabled_fences) { + if ((!enabled() && !_auto_enabled) || !_configured_fences) { return true; } // if we have horizontal limits enabled, check we can get a // relative position from the AHRS - if ((_enabled_fences & AC_FENCE_TYPE_CIRCLE) || - (_enabled_fences & AC_FENCE_TYPE_POLYGON)) { + if ((_configured_fences & AC_FENCE_TYPE_CIRCLE) || + (_configured_fences & AC_FENCE_TYPE_POLYGON)) { Vector2f position; if (!AP::ahrs().get_relative_position_NE_home(position)) { fail_msg = "Fence requires position"; @@ -389,13 +498,14 @@ bool AC_Fence::pre_arm_check(const char* &fail_msg) const bool AC_Fence::check_fence_alt_max() { // altitude fence check - if (!(_enabled_fences & AC_FENCE_TYPE_ALT_MAX)) { + if (!(get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX)) { // not enabled; no breach return false; } - AP::ahrs().get_relative_position_D_home(_curr_alt); - _curr_alt = -_curr_alt; // translate Down to Up + float alt; + AP::ahrs().get_relative_position_D_home(alt); + _curr_alt = -alt; // translate Down to Up // check if we are over the altitude fence if (_curr_alt >= _alt_max) { @@ -437,13 +547,14 @@ bool AC_Fence::check_fence_alt_max() bool AC_Fence::check_fence_alt_min() { // altitude fence check - if (!(_enabled_fences & AC_FENCE_TYPE_ALT_MIN)) { + if (!(get_enabled_fences() & AC_FENCE_TYPE_ALT_MIN)) { // not enabled; no breach return false; } - AP::ahrs().get_relative_position_D_home(_curr_alt); - _curr_alt = -_curr_alt; // translate Down to Up + float alt; + AP::ahrs().get_relative_position_D_home(alt); + _curr_alt = -alt; // translate Down to Up // check if we are under the altitude fence if (_curr_alt <= _alt_min) { @@ -479,13 +590,37 @@ bool AC_Fence::check_fence_alt_min() return false; } + +/// auto enable fence floor +bool AC_Fence::auto_enable_fence_floor() +{ + // altitude fence check + if (!(_configured_fences & AC_FENCE_TYPE_ALT_MIN) || !_enabled) { + // not enabled + return false; + } + + float alt; + AP::ahrs().get_relative_position_D_home(alt); + _curr_alt = -alt; // translate Down to Up + + // check if we are over the altitude fence + if (!floor_enabled() && !_floor_disabled_for_landing && _curr_alt >= _alt_min + _margin) { + enable(true, AC_FENCE_TYPE_ALT_MIN); + gcs().send_text(MAV_SEVERITY_NOTICE, "Min Alt fence enabled (auto enable)"); + return true; + } + + return false; +} + // check_fence_polygon - returns true if the poly fence is freshly // breached. That includes being inside exclusion zones and outside // inclusions zones bool AC_Fence::check_fence_polygon() { const bool was_breached = _breached_fences & AC_FENCE_TYPE_POLYGON; - const bool breached = ((_enabled_fences & AC_FENCE_TYPE_POLYGON) && + const bool breached = ((_configured_fences & AC_FENCE_TYPE_POLYGON) && _poly_loader.breached()); if (breached) { if (!was_breached) { @@ -506,7 +641,7 @@ bool AC_Fence::check_fence_polygon() /// fence is breached. bool AC_Fence::check_fence_circle() { - if (!(_enabled_fences & AC_FENCE_TYPE_CIRCLE)) { + if (!(get_enabled_fences() & AC_FENCE_TYPE_CIRCLE)) { // not enabled; no breach return false; } @@ -554,34 +689,29 @@ uint8_t AC_Fence::check() uint8_t ret = 0; // clear any breach from a non-enabled fence - clear_breach(~_enabled_fences); + clear_breach(~_configured_fences); // return immediately if disabled - if ((!_enabled && !_auto_enabled) || !_enabled_fences) { + if ((!enabled() && !_auto_enabled && !(_configured_fences & AC_FENCE_TYPE_ALT_MIN)) || !_configured_fences) { return 0; } - // check if pilot is attempting to recover manually - if (_manual_recovery_start_ms != 0) { - // we ignore any fence breaches during the manual recovery period which is about 10 seconds - if ((AP_HAL::millis() - _manual_recovery_start_ms) < AC_FENCE_MANUAL_RECOVERY_TIME_MIN) { - return 0; - } - // recovery period has passed so reset manual recovery time - // and continue with fence breach checks - _manual_recovery_start_ms = 0; - } - // maximum altitude fence check if (check_fence_alt_max()) { ret |= AC_FENCE_TYPE_ALT_MAX; } - // minimum altitude fence check - if (_floor_enabled && check_fence_alt_min()) { + // minimum altitude fence check, do this before auto-disabling (e.g. because falling) + // so that any action can be taken + if (floor_enabled() && check_fence_alt_min()) { ret |= AC_FENCE_TYPE_ALT_MIN; } + // auto enable floor unless auto enable has been set (which means other behaviour is required) + if (auto_enabled() != AutoEnable::ENABLE_ON_AUTO_TAKEOFF && (_configured_fences & AC_FENCE_TYPE_ALT_MIN)) { + auto_enable_fence_floor(); + } + // circle fence check if (check_fence_circle()) { ret |= AC_FENCE_TYPE_CIRCLE; @@ -592,6 +722,18 @@ uint8_t AC_Fence::check() ret |= AC_FENCE_TYPE_POLYGON; } + // check if pilot is attempting to recover manually + // this is done last so that _breached_fences is correct + if (_manual_recovery_start_ms != 0) { + // we ignore any fence breaches during the manual recovery period which is about 10 seconds + if ((AP_HAL::millis() - _manual_recovery_start_ms) < AC_FENCE_MANUAL_RECOVERY_TIME_MIN) { + return 0; + } + // recovery period has passed so reset manual recovery time + // and continue with fence breach checks + _manual_recovery_start_ms = 0; + } + // return any new breaches that have occurred return ret; } @@ -695,6 +837,8 @@ void AC_Fence::manual_recovery_start() // record time pilot began manual recovery _manual_recovery_start_ms = AP_HAL::millis(); + + gcs().send_text(MAV_SEVERITY_INFO, "Manual recovery started"); } // methods for mavlink SYS_STATUS message (send_sys_status) @@ -712,7 +856,7 @@ bool AC_Fence::sys_status_enabled() const return false; } // Fence is only enabled when the flag is enabled - return _enabled; + return enabled(); } bool AC_Fence::sys_status_failed() const @@ -742,14 +886,17 @@ const AP_Param::GroupInfo AC_Fence::var_info[] = { AP_GROUPEND }; AC_Fence::AC_Fence() {}; -void AC_Fence::enable(bool value) {}; +uint8_t AC_Fence::enable(bool value, uint8_t fence_types, bool update_auto_enable) { return 0; } -void AC_Fence::disable_floor() {}; +void AC_Fence::enable_floor() {} +void AC_Fence::disable_floor() {} +void AC_Fence::update() {} -void AC_Fence::auto_enable_fence_after_takeoff() {}; -void AC_Fence::auto_disable_fence_for_landing() {}; +void AC_Fence::auto_enable_fence_after_takeoff() {} +void AC_Fence::auto_disable_fence_for_landing() {} +void AC_Fence::auto_enable_fence_on_arming() {} -bool AC_Fence::present() const { return false; } +uint8_t AC_Fence::present() const { return 0; } uint8_t AC_Fence::get_enabled_fences() const { return 0; } @@ -758,6 +905,8 @@ bool AC_Fence::pre_arm_check(const char* &fail_msg) const { return true; } uint8_t AC_Fence::check() { return 0; } bool AC_Fence::check_destination_within_fence(const Location& loc) { return true; } float AC_Fence::get_breach_distance(uint8_t fence_type) const { return 0.0; } +void AC_Fence::get_fence_names(uint8_t fences, ExpandingString& msg) { } +void AC_Fence::print_fence_message(const char* msg, uint8_t fences) const {} void AC_Fence::manual_recovery_start() {} diff --git a/libraries/AC_Fence/AC_Fence.h b/libraries/AC_Fence/AC_Fence.h index e8eb88943e133c..23a57312ef072c 100644 --- a/libraries/AC_Fence/AC_Fence.h +++ b/libraries/AC_Fence/AC_Fence.h @@ -6,6 +6,7 @@ #include #include +#include #include #include #include @@ -15,6 +16,8 @@ #define AC_FENCE_TYPE_CIRCLE 2 // circular horizontal fence (usually initiates an RTL) #define AC_FENCE_TYPE_POLYGON 4 // polygon horizontal fence #define AC_FENCE_TYPE_ALT_MIN 8 // low alt fence which usually initiates an RTL +#define AC_FENCE_ARMING_FENCES (AC_FENCE_TYPE_ALT_MAX | AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_POLYGON) +#define AC_FENCE_ALL_FENCES (AC_FENCE_ARMING_FENCES | AC_FENCE_TYPE_ALT_MIN) // valid actions should a fence be breached #define AC_FENCE_ACTION_REPORT_ONLY 0 // report to GCS that boundary has been breached but take no further action @@ -34,12 +37,19 @@ class AC_Fence public: friend class AC_PolyFence_loader; - enum class AutoEnable + enum class AutoEnable : uint8_t { ALWAYS_DISABLED = 0, - ALWAYS_ENABLED = 1, - ENABLE_DISABLE_FLOOR_ONLY = 2, - ONLY_WHEN_ARMED = 3 + ENABLE_ON_AUTO_TAKEOFF = 1, // enable on auto takeoff + ENABLE_DISABLE_FLOOR_ONLY = 2, // enable on takeoff but disable floor on landing + ONLY_WHEN_ARMED = 3 // enable on arming + }; + + enum class MavlinkFenceActions : uint16_t + { + DISABLE_FENCE = 0, + ENABLE_FENCE = 1, + DISABLE_ALT_MIN_FENCE = 2 }; AC_Fence(); @@ -55,7 +65,12 @@ class AC_Fence static AC_Fence *get_singleton() { return _singleton; } /// enable - allows fence to be enabled/disabled. - void enable(bool value); + /// returns a bitmask of fences that were changed + uint8_t enable(bool value, uint8_t fence_types, bool update_auto_mask = true); + + /// enable_configured - allows configured fences to be enabled/disabled. + /// returns a bitmask of fences that were changed + uint8_t enable_configured(bool value) { return enable(value, _configured_fences, true); } /// auto_enabled - automaticaly enable/disable fence depending of flight status AutoEnable auto_enabled() { return static_cast(_auto_enabled.get()); } @@ -72,19 +87,23 @@ class AC_Fence /// auto_disable_fence_for_landing - auto disables respective fence. Called prior to landing. void auto_disable_fence_for_landing(); - /// enabled - returns true if fence is enabled - bool enabled() const { return _enabled; } + /// auto_enable_fences_on_arming - auto enables all applicable fences on arming + void auto_enable_fence_on_arming(); + + /// auto_enable_fence_floor - auto enables fence floor once desired altitude has been reached. + bool auto_enable_fence_floor(); - /// present - returns true if fence is present - bool present() const; + /// enabled - returns whether fencing is enabled or not + bool enabled() const { return _enabled_fences; } + + /// present - returns true if any of the provided types is present + uint8_t present() const; /// get_enabled_fences - returns bitmask of enabled fences uint8_t get_enabled_fences() const; // should be called @10Hz to handle loading from eeprom - void update() { - _poly_loader.update(); - } + void update(); /// pre_arm_check - returns true if all pre-takeoff checks have completed successfully bool pre_arm_check(const char* &fail_msg) const; @@ -133,6 +152,12 @@ class AC_Fence /// get_return_rally - returns whether returning to fence return point or rally point float get_return_altitude() const { return _ret_altitude; } + /// get a user-friendly list of fences + static void get_fence_names(uint8_t fences, ExpandingString& msg); + + // print a message about the fences to the GCS + void print_fence_message(const char* msg, uint8_t fences) const; + /// manual_recovery_start - caller indicates that pilot is re-taking manual control so fence should be disabled for 10 seconds /// should be called whenever the pilot changes the flight mode /// has no effect if no breaches have occurred @@ -190,11 +215,16 @@ class AC_Fence bool pre_arm_check_polygon(const char* &fail_msg) const; bool pre_arm_check_circle(const char* &fail_msg) const; bool pre_arm_check_alt(const char* &fail_msg) const; + // fence floor is enabled + bool floor_enabled() const { return _enabled_fences & AC_FENCE_TYPE_ALT_MIN; } // parameters - AP_Int8 _enabled; // fence enable/disable control + uint8_t _enabled_fences; // fences that are currently enabled/disabled + bool _last_enabled; // value of enabled last time we checked + AP_Int8 _enabled; // overall feature control AP_Int8 _auto_enabled; // top level flag for auto enabling fence - AP_Int8 _enabled_fences; // bit mask holding which fences are enabled + uint8_t _last_auto_enabled; // value of auto_enabled last time we checked + AP_Int8 _configured_fences; // bit mask holding which fences are enabled AP_Int8 _action; // recovery action specified by user AP_Float _alt_max; // altitude upper limit in meters AP_Float _alt_min; // altitude lower limit in meters @@ -216,7 +246,8 @@ class AC_Fence float _circle_breach_distance; // distance beyond the circular fence // other internal variables - bool _floor_enabled; // fence floor is enabled + bool _floor_disabled_for_landing; // fence floor is disabled for landing + uint8_t _auto_enable_mask = AC_FENCE_ALL_FENCES; // fences that can be auto-enabled or auto-disabled float _home_distance; // distance from home in meters (provided by main code) float _curr_alt; From 174d5f07bb145b224c0f8362d48745d180dae00e Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Mon, 11 Mar 2024 12:40:31 +0000 Subject: [PATCH 064/124] AC_Fence: ensure fence enablement on arming is inverted on disarming correct detection of polyfence --- libraries/AC_Fence/AC_Fence.cpp | 28 +++++++++++++++++++++++----- libraries/AC_Fence/AC_Fence.h | 3 +++ 2 files changed, 26 insertions(+), 5 deletions(-) diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp index 1ba6cc27bfac53..b88befe0403c1e 100644 --- a/libraries/AC_Fence/AC_Fence.cpp +++ b/libraries/AC_Fence/AC_Fence.cpp @@ -229,7 +229,8 @@ void AC_Fence::update() #ifdef AC_FENCE_DEBUG static uint32_t last_msg_count = 0; if (get_enabled_fences() && last_msg_count++ % 10 == 0) { - print_fence_message("fences active", get_enabled_fences()); + print_fence_message("active", get_enabled_fences()); + print_fence_message("breached", get_breaches()); } #endif } @@ -309,6 +310,19 @@ void AC_Fence::auto_enable_fence_on_arming(void) print_fence_message("auto-enabled", fences); } +/* + called on disarming +*/ +void AC_Fence::auto_disable_fence_on_disarming(void) +{ + if (auto_enabled() != AC_Fence::AutoEnable::ONLY_WHEN_ARMED) { + return; + } + + const uint8_t fences = enable(false, _auto_enable_mask, false); + print_fence_message("auto-disabled", fences); +} + /* called when an auto-takeoff is complete */ @@ -349,7 +363,7 @@ void AC_Fence::auto_disable_fence_for_landing(void) } case AC_Fence::AutoEnable::ENABLE_DISABLE_FLOOR_ONLY: case AC_Fence::AutoEnable::ONLY_WHEN_ARMED: - disable_floor(); + enable(false, AC_FENCE_TYPE_ALT_MIN, false); _floor_disabled_for_landing = true; GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Min Alt fence auto-disabled"); break; @@ -619,10 +633,13 @@ bool AC_Fence::auto_enable_fence_floor() // inclusions zones bool AC_Fence::check_fence_polygon() { + if (!(get_enabled_fences() & AC_FENCE_TYPE_POLYGON)) { + // not enabled; no breach + return false; + } + const bool was_breached = _breached_fences & AC_FENCE_TYPE_POLYGON; - const bool breached = ((_configured_fences & AC_FENCE_TYPE_POLYGON) && - _poly_loader.breached()); - if (breached) { + if (_poly_loader.breached()) { if (!was_breached) { record_breach(AC_FENCE_TYPE_POLYGON); return true; @@ -895,6 +912,7 @@ void AC_Fence::update() {} void AC_Fence::auto_enable_fence_after_takeoff() {} void AC_Fence::auto_disable_fence_for_landing() {} void AC_Fence::auto_enable_fence_on_arming() {} +void AC_Fence::auto_disable_fence_on_disarming() {} uint8_t AC_Fence::present() const { return 0; } diff --git a/libraries/AC_Fence/AC_Fence.h b/libraries/AC_Fence/AC_Fence.h index 23a57312ef072c..ed933f450feeef 100644 --- a/libraries/AC_Fence/AC_Fence.h +++ b/libraries/AC_Fence/AC_Fence.h @@ -90,6 +90,9 @@ class AC_Fence /// auto_enable_fences_on_arming - auto enables all applicable fences on arming void auto_enable_fence_on_arming(); + /// auto_disable_fences_on_disarming - auto disables all applicable fences on disarming + void auto_disable_fence_on_disarming(); + /// auto_enable_fence_floor - auto enables fence floor once desired altitude has been reached. bool auto_enable_fence_floor(); From 3fabec4158e23691d3f1f06de3dc020b5d002312 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Mon, 11 Mar 2024 12:40:57 +0000 Subject: [PATCH 065/124] AP_Arming: ensure fence enablement on arming is inverted on disarming --- libraries/AP_Arming/AP_Arming.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp index 83e7cac17447f2..2be3a1011fe6e1 100644 --- a/libraries/AP_Arming/AP_Arming.cpp +++ b/libraries/AP_Arming/AP_Arming.cpp @@ -1840,9 +1840,7 @@ bool AP_Arming::disarm(const AP_Arming::Method method, bool do_disarm_checks) #if AP_FENCE_ENABLED AC_Fence *fence = AP::fence(); if (fence != nullptr) { - if(fence->auto_enabled() == AC_Fence::AutoEnable::ONLY_WHEN_ARMED) { - fence->enable_configured(false); - } + fence->auto_disable_fence_on_disarming(); } #endif #if defined(HAL_ARM_GPIO_PIN) From a4c781911762cadfe95c7494ac5bd0b0140fad8a Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 19 May 2024 17:39:50 +1000 Subject: [PATCH 066/124] AC_Fence: clear breach of disabled fence skip breach checks if no fences correct initialisation of _num_fences in the case of no fences in eeprom --- libraries/AC_Fence/AC_Fence.cpp | 1 + libraries/AC_Fence/AC_PolyFence_loader.cpp | 8 +++++--- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp index b88befe0403c1e..36fb7fbb32694a 100644 --- a/libraries/AC_Fence/AC_Fence.cpp +++ b/libraries/AC_Fence/AC_Fence.cpp @@ -635,6 +635,7 @@ bool AC_Fence::check_fence_polygon() { if (!(get_enabled_fences() & AC_FENCE_TYPE_POLYGON)) { // not enabled; no breach + clear_breach(AC_FENCE_TYPE_POLYGON); return false; } diff --git a/libraries/AC_Fence/AC_PolyFence_loader.cpp b/libraries/AC_Fence/AC_PolyFence_loader.cpp index 509e039a843bbf..b97334476e0533 100644 --- a/libraries/AC_Fence/AC_PolyFence_loader.cpp +++ b/libraries/AC_Fence/AC_PolyFence_loader.cpp @@ -227,7 +227,7 @@ bool AC_PolyFence_loader::breached() const // returns true if location is outside the boundary bool AC_PolyFence_loader::breached(const Location& loc) const { - if (!loaded()) { + if (!loaded() || total_fence_count() == 0) { return false; } @@ -493,13 +493,15 @@ bool AC_PolyFence_loader::index_eeprom() if (!count_eeprom_fences()) { return false; } + + void_index(); + if (_eeprom_fence_count == 0) { + _num_fences = 0; _load_attempted = false; return true; } - void_index(); - Debug("Fence: Allocating %u bytes for index", (unsigned)(_eeprom_fence_count*sizeof(FenceIndex))); _index = NEW_NOTHROW FenceIndex[_eeprom_fence_count]; From cedccdb8fe14ba467ffd5f191a99ab7507e41967 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 22 Dec 2023 09:36:34 +0000 Subject: [PATCH 067/124] autotest: add test for auto-disabling min alt fence breaches on disarming clean-up fence manipulation functions and add test for auto-enablement on copter update tests to have some FENCE_ENABLE tests add avoidance minimum and maximum altitude fence add fence switch test while flying add FenceAutoEnableDisableSwitch for auto mode 2 add more scenarios for plane fence auto-enable validate fence rc switch behaviour check fence autoenable by taking off in guided mode more FENCE_AUTOENABLE tests add FenceEnableDisableAux and FenceMinAltAutoEnableAbort --- .../autotest/Generic_Missions/CMAC-fence.txt | 6 + Tools/autotest/arducopter.py | 140 +++++++- Tools/autotest/arduplane.py | 331 ++++++++++++++++++ Tools/autotest/rover.py | 6 + Tools/autotest/vehicle_test_suite.py | 23 +- 5 files changed, 492 insertions(+), 14 deletions(-) create mode 100644 Tools/autotest/Generic_Missions/CMAC-fence.txt diff --git a/Tools/autotest/Generic_Missions/CMAC-fence.txt b/Tools/autotest/Generic_Missions/CMAC-fence.txt new file mode 100644 index 00000000000000..a6169e4e3cbcf7 --- /dev/null +++ b/Tools/autotest/Generic_Missions/CMAC-fence.txt @@ -0,0 +1,6 @@ +-35.363720 149.163651 +-35.358738 149.165070 +-35.359295 149.154434 +-35.372292 149.157135 +-35.368290 149.166809 +-35.358738 149.165070 diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index ccd8147ec045fb..e0eda4cdae5de8 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -1605,6 +1605,7 @@ def MaxAltFence(self): "FENCE_ENABLE": 1, "AVOID_ENABLE": 0, "FENCE_TYPE": 1, + "FENCE_ENABLE" : 1, }) self.change_alt(10) @@ -1692,6 +1693,7 @@ def MinAltFence(self): # enable fence, disable avoidance self.set_parameters({ "AVOID_ENABLE": 0, + "FENCE_ENABLE" : 1, "FENCE_TYPE": 8, "FENCE_ALT_MIN": 20, }) @@ -1737,6 +1739,7 @@ def FenceFloorEnabledLanding(self): self.progress("Test Landing while fence floor enabled") self.set_parameters({ "AVOID_ENABLE": 0, + "FENCE_ENABLE" : 1, "FENCE_TYPE": 15, "FENCE_ALT_MIN": 10, "FENCE_ALT_MAX": 20, @@ -1757,16 +1760,100 @@ def FenceFloorEnabledLanding(self): self.set_rc(3, 1800) self.wait_mode('RTL', timeout=120) + self.wait_landed_and_disarmed() self.assert_fence_enabled() - # Assert fence is not healthy + # Assert fence is not healthy since it was enabled manually self.assert_sensor_state(fence_bit, healthy=False) # Disable the fence using mavlink command to ensure cleaned up SITL state self.do_fence_disable() self.assert_fence_disabled() + def FenceFloorAutoDisableLanding(self): + """Ensures we can initiate and complete an RTL while the fence is enabled""" + + fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE + + self.progress("Test Landing while fence floor enabled") + self.set_parameters({ + "AVOID_ENABLE": 0, + "FENCE_TYPE": 11, + "FENCE_ALT_MIN": 10, + "FENCE_ALT_MAX": 20, + "FENCE_AUTOENABLE" : 1, + }) + + self.change_mode("GUIDED") + self.wait_ready_to_arm() + self.arm_vehicle() + self.takeoff(alt_min=15, mode="GUIDED") + + # Check fence is enabled + self.assert_fence_enabled() + + # Change to RC controlled mode + self.change_mode('LOITER') + + self.set_rc(3, 1800) + + self.wait_mode('RTL', timeout=120) + # Assert fence is not healthy now that we are in RTL + self.assert_sensor_state(fence_bit, healthy=False) + + self.wait_landed_and_disarmed(0) + # the breach should have cleared since we auto-disable the + # fence on landing + self.assert_fence_disabled() + + # Assert fences have gone now that we have landed and disarmed + self.assert_sensor_state(fence_bit, present=True, enabled=False) + + def FenceFloorAutoEnableOnArming(self): + """Ensures we can auto-enable fences on arming and still takeoff and land""" + + fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE + + self.set_parameters({ + "AVOID_ENABLE": 0, + "FENCE_TYPE": 11, + "FENCE_ALT_MIN": 10, + "FENCE_ALT_MAX": 20, + "FENCE_AUTOENABLE" : 3, + }) + + self.change_mode("GUIDED") + # Check fence is not enabled + self.assert_fence_disabled() + + self.wait_ready_to_arm() + self.arm_vehicle() + self.takeoff(alt_min=15, mode="GUIDED") + + # Check fence is enabled + self.assert_fence_enabled() + + # Change to RC controlled mode + self.change_mode('LOITER') + + self.set_rc(3, 1800) + + self.wait_mode('RTL', timeout=120) + # Assert fence is not healthy now that we are in RTL + self.assert_sensor_state(fence_bit, healthy=False) + + self.wait_landed_and_disarmed(0) + # the breach should have cleared since we auto-disable the + # fence on landing + self.assert_fence_disabled() + + # Assert fences have gone now that we have landed and disarmed + self.assert_sensor_state(fence_bit, present=True, enabled=False) + + # Disable the fence using mavlink command to ensure cleaned up SITL state + self.assert_fence_disabled() + def GPSGlitchLoiter(self, timeout=30, max_distance=20): """fly_gps_glitch_loiter_test. Fly south east in loiter and test reaction to gps glitch.""" @@ -7631,6 +7718,54 @@ def AC_Avoidance_Fence(self): self.set_parameter("FENCE_ENABLE", 1) self.check_avoidance_corners() + def AvoidanceAltFence(self): + '''Test fence avoidance at minimum and maximum altitude''' + ex = None + try: + self.set_parameters({ + "FENCE_ENABLE": 1, + "FENCE_TYPE": 9, # min and max alt fence + "FENCE_ALT_MIN": 10, + "FENCE_ALT_MAX": 30, + }) + + self.change_mode('LOITER') + self.wait_ekf_happy() + + tstart = self.get_sim_time() + self.takeoff(15, mode='LOITER') + self.progress("Increasing throttle, vehicle should stay below 30m") + self.set_rc(3, 1920) + + tstart = self.get_sim_time() + while True: + if self.get_sim_time_cached() - tstart > 20: + break + alt = self.get_altitude(relative=True) + self.progress("Altitude %s" % alt) + if alt > 30: + raise NotAchievedException("Breached maximum altitude (%s)" % (str(alt),)) + + self.progress("Decreasing, vehicle should stay above 10m") + self.set_rc(3, 1080) + tstart = self.get_sim_time() + while True: + if self.get_sim_time_cached() - tstart > 20: + break + alt = self.get_altitude(relative=True) + self.progress("Altitude %s" % alt) + if alt < 10: + raise NotAchievedException("Breached minimum altitude (%s)" % (str(alt),)) + + except Exception as e: + self.progress("Caught exception: %s" % + self.get_exception_stacktrace(e)) + ex = e + self.land_and_disarm() + self.disarm_vehicle(force=True) + if ex is not None: + raise ex + def ModeFollow(self): '''Fly follow mode''' foll_ofs_x = 30 # metres @@ -10509,6 +10644,7 @@ def tests1c(self): self.AC_Avoidance_Proximity_AVOID_ALT_MIN, self.AC_Avoidance_Fence, self.AC_Avoidance_Beacon, + self.AvoidanceAltFence, self.BaroWindCorrection, self.SetpointGlobalPos, self.ThrowDoubleDrop, @@ -10528,6 +10664,8 @@ def tests1d(self): self.MaxAltFenceAvoid, self.MinAltFence, self.FenceFloorEnabledLanding, + self.FenceFloorAutoDisableLanding, + self.FenceFloorAutoEnableOnArming, self.AutoTuneSwitch, self.GPSGlitchLoiter, self.GPSGlitchLoiter2, diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index e57294be1121f0..49d13661894a27 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -3698,6 +3698,332 @@ def FenceAltCeilFloor(self): self.fly_home_land_and_disarm(timeout=150) + def FenceMinAltAutoEnable(self): + '''Tests autoenablement of the alt min fence and fences on arming''' + self.set_parameters({ + "FENCE_TYPE": 9, # Set fence type to min alt and max alt + "FENCE_ACTION": 1, # Set action to RTL + "FENCE_ALT_MIN": 25, + "FENCE_ALT_MAX": 100, + "FENCE_AUTOENABLE": 3, + "FENCE_ENABLE" : 0, + "RTL_AUTOLAND" : 2, + }) + + # check we can takeoff again + for i in [1, 2]: + # Grab Home Position + self.mav.recv_match(type='HOME_POSITION', blocking=True) + self.homeloc = self.mav.location() + + self.wait_ready_to_arm() + self.arm_vehicle() + # max alt fence should now be enabled + if i == 1: + self.assert_fence_enabled() + + self.takeoff(alt=50, mode='TAKEOFF') + self.change_mode("FBWA") + self.set_rc(3, 1100) # lower throttle + + self.progress("Waiting for RTL") + tstart = self.get_sim_time() + mode = "RTL" + while not self.mode_is(mode, drain_mav=False): + self.mav.messages['HEARTBEAT'].custom_mode + self.progress("mav.flightmode=%s Want=%s Alt=%f" % ( + self.mav.flightmode, mode, self.get_altitude(relative=True))) + if (self.get_sim_time_cached() > tstart + 120): + raise WaitModeTimeout("Did not change mode") + self.progress("Got mode %s" % mode) + self.fly_home_land_and_disarm() + self.change_mode("FBWA") + self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_ALL) + self.set_current_waypoint(0, check_afterwards=False) + self.set_rc(3, 1000) # lower throttle + + def FenceMinAltAutoEnableAbort(self): + '''Tests autoenablement of the alt min fence and fences on arming''' + self.set_parameters({ + "FENCE_TYPE": 9, # Set fence type to min alt and max alt + "FENCE_ACTION": 1, # Set action to RTL + "FENCE_ALT_MIN": 25, + "FENCE_ALT_MAX": 100, + "FENCE_AUTOENABLE": 3, + "FENCE_ENABLE" : 0, + "RTL_AUTOLAND" : 2, + }) + + # Grab Home Position + self.mav.recv_match(type='HOME_POSITION', blocking=True) + self.homeloc = self.mav.location() + + self.wait_ready_to_arm() + self.arm_vehicle() + # max alt fence should now be enabled + self.assert_fence_enabled() + + self.takeoff(alt=50, mode='TAKEOFF') + self.change_mode("FBWA") + self.set_rc(3, 1100) # lower throttle + + self.progress("Waiting for RTL") + tstart = self.get_sim_time() + mode = "RTL" + while not self.mode_is(mode, drain_mav=False): + self.mav.messages['HEARTBEAT'].custom_mode + self.progress("mav.flightmode=%s Want=%s Alt=%f" % ( + self.mav.flightmode, mode, self.get_altitude(relative=True))) + if (self.get_sim_time_cached() > tstart + 120): + raise WaitModeTimeout("Did not change mode") + self.progress("Got mode %s" % mode) + + self.load_generic_mission("flaps.txt") + self.change_mode("AUTO") + self.wait_distance_to_waypoint(8, 100, 10000000) + self.set_current_waypoint(8) + # abort the landing + self.wait_altitude(10, 20, timeout=200, relative=True) + self.change_mode("CRUISE") + self.set_rc(2, 1200) + # self.set_rc(3, 1600) # raise throttle + self.wait_altitude(30, 40, timeout=200, relative=True) + + self.change_mode("AUTO") + self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_ALL) + self.fly_home_land_and_disarm(timeout=150) + self.wait_disarmed() + + def FenceAutoEnableDisableSwitch(self): + '''Tests autoenablement of regular fences and manual disablement''' + self.set_parameters({ + "FENCE_TYPE": 11, # Set fence type to min alt + "FENCE_ACTION": 1, # Set action to RTL + "FENCE_ALT_MIN": 50, + "FENCE_ALT_MAX": 100, + "FENCE_AUTOENABLE": 2, + "FENCE_OPTIONS" : 1, + "FENCE_ENABLE" : 1, + "FENCE_RADIUS" : 300, + "FENCE_RET_ALT" : 0, + "FENCE_RET_RALLY" : 0, + "FENCE_TOTAL" : 0, + "TKOFF_ALT" : 75, + "RC7_OPTION" : 11, # AC_Fence uses Aux switch functionality + }) + fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE + # Grab Home Position + self.mav.recv_match(type='HOME_POSITION', blocking=True) + self.homeloc = self.mav.location() + self.set_rc_from_map({7: 1000}) # Turn fence off with aux function + + self.wait_ready_to_arm() + cruise_alt = 75 + self.takeoff(cruise_alt, mode='TAKEOFF') + + self.progress("Fly above ceiling and check there is no breach") + self.set_rc(3, 2000) + self.change_altitude(self.homeloc.alt + cruise_alt + 80) + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) + self.progress("Got (%s)" % str(m)) + if (not (m.onboard_control_sensors_health & fence_bit)): + raise NotAchievedException("Fence Ceiling breached") + + self.progress("Return to cruise alt") + self.set_rc(3, 1500) + self.change_altitude(self.homeloc.alt + cruise_alt) + + self.progress("Fly below floor and check for no breach") + self.change_altitude(self.homeloc.alt + 25) + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) + self.progress("Got (%s)" % str(m)) + if (not (m.onboard_control_sensors_health & fence_bit)): + raise NotAchievedException("Fence Ceiling breached") + + self.progress("Fly above floor and check fence is not re-enabled") + self.set_rc(3, 2000) + self.change_altitude(self.homeloc.alt + 75) + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) + self.progress("Got (%s)" % str(m)) + if (m.onboard_control_sensors_enabled & fence_bit): + raise NotAchievedException("Fence Ceiling re-enabled") + + self.progress("Return to cruise alt") + self.set_rc(3, 1500) + self.change_altitude(self.homeloc.alt + cruise_alt) + self.fly_home_land_and_disarm(timeout=250) + + def FenceEnableDisableSwitch(self): + '''Tests enablement and disablement of fences on a switch''' + fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE + + self.set_parameters({ + "FENCE_TYPE": 4, # Set fence type to polyfence + "FENCE_ACTION": 6, # Set action to GUIDED + "FENCE_ALT_MIN": 10, + "FENCE_ENABLE" : 0, + "RC7_OPTION" : 11, # AC_Fence uses Aux switch functionality + }) + + self.progress("Checking fence is not present before being configured") + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) + self.progress("Got (%s)" % str(m)) + if (m.onboard_control_sensors_enabled & fence_bit): + raise NotAchievedException("Fence enabled before being configured") + + self.wait_ready_to_arm() + # takeoff at a lower altitude to avoid immediately breaching polyfence + self.takeoff(alt=25) + self.change_mode("FBWA") + + self.load_fence("CMAC-fence.txt") + + self.set_rc_from_map({ + 3: 1500, + 7: 2000, + }) # Turn fence on with aux function + + m = self.mav.recv_match(type='FENCE_STATUS', blocking=True, timeout=2) + self.progress("Got (%s)" % str(m)) + if m is None: + raise NotAchievedException("Got FENCE_STATUS unexpectedly") + + self.progress("Checking fence is initially OK") + self.wait_sensor_state(mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE, + present=True, + enabled=True, + healthy=True, + verbose=False, + timeout=30) + + self.progress("Waiting for GUIDED") + tstart = self.get_sim_time() + mode = "GUIDED" + while not self.mode_is(mode, drain_mav=False): + self.mav.messages['HEARTBEAT'].custom_mode + self.progress("mav.flightmode=%s Want=%s Alt=%f" % ( + self.mav.flightmode, mode, self.get_altitude(relative=True))) + if (self.get_sim_time_cached() > tstart + 120): + raise WaitModeTimeout("Did not change mode") + self.progress("Got mode %s" % mode) + # check we are in breach + self.assert_fence_enabled() + + self.set_rc_from_map({ + 7: 1000, + }) # Turn fence off with aux function + + # wait to no longer be in breach + self.delay_sim_time(5) + self.assert_fence_disabled() + + self.fly_home_land_and_disarm(timeout=250) + self.do_fence_disable() # Ensure the fence is disabled after test + + def FenceEnableDisableAux(self): + '''Tests enablement and disablement of fences via aux command''' + fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE + + enable = 0 + self.set_parameters({ + "FENCE_TYPE": 12, # Set fence type to polyfence + AltMin + "FENCE_ALT_MIN": 10, + "FENCE_ENABLE" : enable, + }) + + if not enable: + self.progress("Checking fence is not present before being configured") + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) + self.progress("Got (%s)" % str(m)) + if (m.onboard_control_sensors_enabled & fence_bit): + raise NotAchievedException("Fence enabled before being configured") + + self.load_fence("CMAC-fence.txt") + + self.wait_ready_to_arm() + # takeoff at a lower altitude to avoid immediately breaching polyfence + self.takeoff(alt=25) + self.change_mode("CRUISE") + self.wait_distance(150, accuracy=20) + + self.run_auxfunc( + 11, + 2, + want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED + ) + + m = self.mav.recv_match(type='FENCE_STATUS', blocking=True, timeout=2) + self.progress("Got (%s)" % str(m)) + if m is None: + raise NotAchievedException("Got FENCE_STATUS unexpectedly") + + self.progress("Checking fence is initially OK") + self.wait_sensor_state(mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE, + present=True, + enabled=True, + healthy=True, + verbose=False, + timeout=30) + + self.progress("Waiting for RTL") + tstart = self.get_sim_time() + mode = "RTL" + while not self.mode_is(mode, drain_mav=False): + self.mav.messages['HEARTBEAT'].custom_mode + self.progress("mav.flightmode=%s Want=%s Alt=%f" % ( + self.mav.flightmode, mode, self.get_altitude(relative=True))) + if (self.get_sim_time_cached() > tstart + 120): + raise WaitModeTimeout("Did not change mode") + self.progress("Got mode %s" % mode) + # check we are in breach + self.assert_fence_enabled() + self.assert_fence_sys_status(True, True, False) + + # wait until we get home + self.wait_distance_to_home(50, 100, timeout=200) + # now check we are now not in breach + self.assert_fence_sys_status(True, True, True) + # Turn fence off with aux function + self.run_auxfunc( + 11, + 0, + want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED + ) + # switch back to cruise + self.change_mode("CRUISE") + self.wait_distance(150, accuracy=20) + + # re-enable the fences + self.run_auxfunc( + 11, + 2, + want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED + ) + + m = self.mav.recv_match(type='FENCE_STATUS', blocking=True, timeout=2) + self.progress("Got (%s)" % str(m)) + if m is None: + raise NotAchievedException("Got FENCE_STATUS unexpectedly") + + self.progress("Waiting for RTL") + tstart = self.get_sim_time() + mode = "RTL" + while not self.mode_is(mode, drain_mav=False): + self.mav.messages['HEARTBEAT'].custom_mode + self.progress("mav.flightmode=%s Want=%s Alt=%f" % ( + self.mav.flightmode, mode, self.get_altitude(relative=True))) + if (self.get_sim_time_cached() > tstart + 120): + raise WaitModeTimeout("Did not change mode") + self.progress("Got mode %s" % mode) + + # wait to no longer be in breach + self.wait_distance_to_home(50, 100, timeout=200) + self.assert_fence_sys_status(True, True, True) + + # fly home and land with fences still enabled + self.fly_home_land_and_disarm(timeout=250) + self.do_fence_disable() # Ensure the fence is disabled after test + def FenceBreachedChangeMode(self): '''Tests manual mode change after fence breach, as set with FENCE_OPTIONS''' """ Attempts to change mode while a fence is breached. @@ -5447,6 +5773,11 @@ def tests(self): self.FenceRTLRally, self.FenceRetRally, self.FenceAltCeilFloor, + self.FenceMinAltAutoEnable, + self.FenceMinAltAutoEnableAbort, + self.FenceAutoEnableDisableSwitch, + self.FenceEnableDisableSwitch, + self.FenceEnableDisableAux, self.FenceBreachedChangeMode, self.FenceNoFenceReturnPoint, self.FenceNoFenceReturnPointInclusion, diff --git a/Tools/autotest/rover.py b/Tools/autotest/rover.py index 8e62d685be7a77..22c38c5c50b52d 100644 --- a/Tools/autotest/rover.py +++ b/Tools/autotest/rover.py @@ -4061,6 +4061,9 @@ def test_poly_fence_noarms(self, target_system=1, target_component=1): self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_FENCE, target_system=target_system, target_component=target_component) + self.set_parameters({ + "FENCE_TYPE": 2, # circle only + }) self.delay_sim_time(5) # let breaches clear # FIXME: should we allow this? self.progress("Ensure we can arm with no poly in place") @@ -4068,6 +4071,9 @@ def test_poly_fence_noarms(self, target_system=1, target_component=1): self.wait_ready_to_arm() self.arm_vehicle() self.disarm_vehicle() + self.set_parameters({ + "FENCE_TYPE": 6, # polyfence + circle + }) self.test_poly_fence_noarms_exclusion_circle(target_system=target_system, target_component=target_component) diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py index 4677be0ab8afcd..f3458fee9d9575 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -2080,6 +2080,8 @@ def roundtrip_fence_using_fencepoint_protocol(self, loc_list, target_system=1, t def load_fence(self, filename): filepath = os.path.join(testdir, self.current_test_name_directory, filename) + if not os.path.exists(filepath): + filepath = self.generic_mission_filepath_for_filename(filename) self.progress("Loading fence from (%s)" % str(filepath)) locs = [] for line in open(filepath, 'rb'): @@ -6994,22 +6996,17 @@ def do_set_relay_mavproxy(self, relay_num, on_off): self.mavproxy.expect("Loaded module relay") self.mavproxy.send("relay set %d %d\n" % (relay_num, on_off)) - def do_fence_en_or_dis_able(self, value, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED): - if value: - p1 = 1 - else: - p1 = 0 - self.run_cmd( - mavutil.mavlink.MAV_CMD_DO_FENCE_ENABLE, - p1=p1, # param1 - want_result=want_result, - ) - def do_fence_enable(self, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED): - self.do_fence_en_or_dis_able(True, want_result=want_result) + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_FENCE_ENABLE, p1=1, want_result=want_result) def do_fence_disable(self, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED): - self.do_fence_en_or_dis_able(False, want_result=want_result) + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_FENCE_ENABLE, p1=0, want_result=want_result) + + def do_fence_disable_floor(self, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED): + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_FENCE_ENABLE, p1=0, p2=8, want_result=want_result) + + def do_fence_enable_except_floor(self, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED): + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_FENCE_ENABLE, p1=1, p2=7, want_result=want_result) ################################################# # WAIT UTILITIES From d5c6f3fe0683df05bbc3d1d938b849fe0a1bda58 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 31 May 2024 08:20:13 +0100 Subject: [PATCH 068/124] AC_Fence: add reset_fence_floor_enable() and use it in plane when landing is aborted --- libraries/AC_Fence/AC_Fence.cpp | 30 ++++++++++++++++++++++++++++-- libraries/AC_Fence/AC_Fence.h | 3 +++ 2 files changed, 31 insertions(+), 2 deletions(-) diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp index 36fb7fbb32694a..e38789c876fd98 100644 --- a/libraries/AC_Fence/AC_Fence.cpp +++ b/libraries/AC_Fence/AC_Fence.cpp @@ -609,7 +609,9 @@ bool AC_Fence::check_fence_alt_min() bool AC_Fence::auto_enable_fence_floor() { // altitude fence check - if (!(_configured_fences & AC_FENCE_TYPE_ALT_MIN) || !_enabled) { + if (!(_configured_fences & AC_FENCE_TYPE_ALT_MIN) + || (get_enabled_fences() & AC_FENCE_TYPE_ALT_MIN) + || (!_enabled && (auto_enabled() != AC_Fence::AutoEnable::ONLY_WHEN_ARMED))) { // not enabled return false; } @@ -620,7 +622,7 @@ bool AC_Fence::auto_enable_fence_floor() // check if we are over the altitude fence if (!floor_enabled() && !_floor_disabled_for_landing && _curr_alt >= _alt_min + _margin) { - enable(true, AC_FENCE_TYPE_ALT_MIN); + enable(true, AC_FENCE_TYPE_ALT_MIN, false); gcs().send_text(MAV_SEVERITY_NOTICE, "Min Alt fence enabled (auto enable)"); return true; } @@ -628,6 +630,30 @@ bool AC_Fence::auto_enable_fence_floor() return false; } +/// reset fence floor auto-enablement +bool AC_Fence::reset_fence_floor_enable() +{ + // altitude fence check + if (!(_configured_fences & AC_FENCE_TYPE_ALT_MIN) || (!_enabled && !_auto_enabled)) { + // not enabled + return false; + } + + float alt; + AP::ahrs().get_relative_position_D_home(alt); + _curr_alt = -alt; // translate Down to Up + + // check if we are under the altitude fence + if ((floor_enabled() || _floor_disabled_for_landing) && _curr_alt <= _alt_min - _margin) { + enable(false, AC_FENCE_TYPE_ALT_MIN, false); + _floor_disabled_for_landing = false; + gcs().send_text(MAV_SEVERITY_NOTICE, "Min Alt fence disabled (auto disable)"); + return true; + } + + return false; +} + // check_fence_polygon - returns true if the poly fence is freshly // breached. That includes being inside exclusion zones and outside // inclusions zones diff --git a/libraries/AC_Fence/AC_Fence.h b/libraries/AC_Fence/AC_Fence.h index ed933f450feeef..c346474e27d2a6 100644 --- a/libraries/AC_Fence/AC_Fence.h +++ b/libraries/AC_Fence/AC_Fence.h @@ -96,6 +96,9 @@ class AC_Fence /// auto_enable_fence_floor - auto enables fence floor once desired altitude has been reached. bool auto_enable_fence_floor(); + /// reset_fence_floor_enable - auto disables the fence floor if below the desired altitude. + bool reset_fence_floor_enable(); + /// enabled - returns whether fencing is enabled or not bool enabled() const { return _enabled_fences; } From ee7742ac403c0b815bac89e8dd44103c439b78f2 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 31 May 2024 08:21:07 +0100 Subject: [PATCH 069/124] Plane: use reset_fence_floor_enable() --- ArduPlane/mode_auto.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/ArduPlane/mode_auto.cpp b/ArduPlane/mode_auto.cpp index 3153d7de31e069..e6381b62df25e2 100644 --- a/ArduPlane/mode_auto.cpp +++ b/ArduPlane/mode_auto.cpp @@ -54,6 +54,10 @@ void ModeAuto::_exit() } #endif if (restart) { +#if AP_FENCE_ENABLED + // no longer landing to re-enable the possiblity of the fence floor + plane.fence.reset_fence_floor_enable(); +#endif plane.landing.restart_landing_sequence(); } } From c216536a94c935ef7ac90ae123e4a80ecb75b4bc Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 31 May 2024 08:21:30 +0100 Subject: [PATCH 070/124] autotest: test aborted landing with fence correctly --- Tools/autotest/arduplane.py | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 49d13661894a27..24821c55eac4e3 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -3745,7 +3745,7 @@ def FenceMinAltAutoEnable(self): def FenceMinAltAutoEnableAbort(self): '''Tests autoenablement of the alt min fence and fences on arming''' self.set_parameters({ - "FENCE_TYPE": 9, # Set fence type to min alt and max alt + "FENCE_TYPE": 8, # Set fence type to min alt "FENCE_ACTION": 1, # Set action to RTL "FENCE_ALT_MIN": 25, "FENCE_ALT_MAX": 100, @@ -3760,11 +3760,11 @@ def FenceMinAltAutoEnableAbort(self): self.wait_ready_to_arm() self.arm_vehicle() - # max alt fence should now be enabled - self.assert_fence_enabled() self.takeoff(alt=50, mode='TAKEOFF') self.change_mode("FBWA") + # min alt fence should now be enabled + self.assert_fence_enabled() self.set_rc(3, 1100) # lower throttle self.progress("Waiting for RTL") @@ -3788,6 +3788,8 @@ def FenceMinAltAutoEnableAbort(self): self.set_rc(2, 1200) # self.set_rc(3, 1600) # raise throttle self.wait_altitude(30, 40, timeout=200, relative=True) + # min alt fence should now be re-enabled + self.assert_fence_enabled() self.change_mode("AUTO") self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_ALL) From 04dd7de1ed467f4f84e3ba898145fa5f0fda8ed6 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 31 May 2024 14:12:38 +0100 Subject: [PATCH 071/124] AC_Fence: disable fences for landing by suppressing in the fence check rather than using a state machine simplify takeoff auto-enablement --- libraries/AC_Fence/AC_Fence.cpp | 111 +++++++++++--------------------- libraries/AC_Fence/AC_Fence.h | 14 ++-- 2 files changed, 43 insertions(+), 82 deletions(-) diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp index e38789c876fd98..e28ad0b6e23964 100644 --- a/libraries/AC_Fence/AC_Fence.cpp +++ b/libraries/AC_Fence/AC_Fence.cpp @@ -248,11 +248,6 @@ uint8_t AC_Fence::enable(bool value, uint8_t fence_types, bool update_auto_mask) // fences that were manually changed are no longer eligible for auto-enablement or disablement if (update_auto_mask) { - // if we are explicitly enabling or disabling the alt min fence and it was auto-enabled then make sure - // it doesn't get re-enabled or disabled - if (fence_types & _auto_enable_mask & AC_FENCE_TYPE_ALT_MIN) { - _floor_disabled_for_landing = !value; - } _auto_enable_mask &= ~fences; } @@ -328,49 +323,30 @@ void AC_Fence::auto_disable_fence_on_disarming(void) */ void AC_Fence::auto_enable_fence_after_takeoff(void) { - switch(auto_enabled()) { - case AC_Fence::AutoEnable::ENABLE_ON_AUTO_TAKEOFF: - case AC_Fence::AutoEnable::ENABLE_DISABLE_FLOOR_ONLY: - case AC_Fence::AutoEnable::ONLY_WHEN_ARMED: { - // auto-enable fences that aren't currently auto-enabled - if (_auto_enable_mask & AC_FENCE_TYPE_ALT_MIN) { - _floor_disabled_for_landing = false; - } - const uint8_t fences = enable(true, _auto_enable_mask, false); - print_fence_message("auto-enabled", fences); - break; - } - default: - // fence does not auto-enable in other takeoff conditions - break; + if (auto_enabled() != AC_Fence::AutoEnable::ENABLE_ON_AUTO_TAKEOFF) { + return; } + + const uint8_t fences = enable(true, _auto_enable_mask, false); + print_fence_message("auto-enabled", fences); } -/* - called when performing an auto landing - */ -void AC_Fence::auto_disable_fence_for_landing(void) +// return fences that should be auto-disabled when requested +uint8_t AC_Fence::get_auto_disable_fences(void) const { + uint8_t auto_disable = 0; switch (auto_enabled()) { - case AC_Fence::AutoEnable::ENABLE_ON_AUTO_TAKEOFF: { - // disable only those fences which are allowed to be disabled - if (_auto_enable_mask & _enabled_fences & AC_FENCE_TYPE_ALT_MIN) { - _floor_disabled_for_landing = true; - } - const uint8_t fences = enable(false, _auto_enable_mask & _enabled_fences, false); - print_fence_message("auto-disabled", fences); + case AC_Fence::AutoEnable::ENABLE_ON_AUTO_TAKEOFF: + auto_disable = _auto_enable_mask; break; - } case AC_Fence::AutoEnable::ENABLE_DISABLE_FLOOR_ONLY: case AC_Fence::AutoEnable::ONLY_WHEN_ARMED: - enable(false, AC_FENCE_TYPE_ALT_MIN, false); - _floor_disabled_for_landing = true; - GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Min Alt fence auto-disabled"); + auto_disable = _auto_enable_mask & AC_FENCE_TYPE_ALT_MIN; break; default: - // fence does not auto-disable in other landing conditions break; } + return auto_disable; } uint8_t AC_Fence::present() const @@ -609,9 +585,11 @@ bool AC_Fence::check_fence_alt_min() bool AC_Fence::auto_enable_fence_floor() { // altitude fence check - if (!(_configured_fences & AC_FENCE_TYPE_ALT_MIN) - || (get_enabled_fences() & AC_FENCE_TYPE_ALT_MIN) - || (!_enabled && (auto_enabled() != AC_Fence::AutoEnable::ONLY_WHEN_ARMED))) { + if (!(_configured_fences & AC_FENCE_TYPE_ALT_MIN) // not configured + || (get_enabled_fences() & AC_FENCE_TYPE_ALT_MIN) // already enabled + || !(_auto_enable_mask & AC_FENCE_TYPE_ALT_MIN) // has been manually disabled + || (!_enabled && (auto_enabled() == AC_Fence::AutoEnable::ALWAYS_DISABLED + || auto_enabled() == AutoEnable::ENABLE_ON_AUTO_TAKEOFF))) { // not enabled return false; } @@ -621,7 +599,7 @@ bool AC_Fence::auto_enable_fence_floor() _curr_alt = -alt; // translate Down to Up // check if we are over the altitude fence - if (!floor_enabled() && !_floor_disabled_for_landing && _curr_alt >= _alt_min + _margin) { + if (!floor_enabled() && _curr_alt >= _alt_min + _margin) { enable(true, AC_FENCE_TYPE_ALT_MIN, false); gcs().send_text(MAV_SEVERITY_NOTICE, "Min Alt fence enabled (auto enable)"); return true; @@ -630,30 +608,6 @@ bool AC_Fence::auto_enable_fence_floor() return false; } -/// reset fence floor auto-enablement -bool AC_Fence::reset_fence_floor_enable() -{ - // altitude fence check - if (!(_configured_fences & AC_FENCE_TYPE_ALT_MIN) || (!_enabled && !_auto_enabled)) { - // not enabled - return false; - } - - float alt; - AP::ahrs().get_relative_position_D_home(alt); - _curr_alt = -alt; // translate Down to Up - - // check if we are under the altitude fence - if ((floor_enabled() || _floor_disabled_for_landing) && _curr_alt <= _alt_min - _margin) { - enable(false, AC_FENCE_TYPE_ALT_MIN, false); - _floor_disabled_for_landing = false; - gcs().send_text(MAV_SEVERITY_NOTICE, "Min Alt fence disabled (auto disable)"); - return true; - } - - return false; -} - // check_fence_polygon - returns true if the poly fence is freshly // breached. That includes being inside exclusion zones and outside // inclusions zones @@ -728,41 +682,53 @@ bool AC_Fence::check_fence_circle() /// check - returns bitmask of fence types breached (if any) -uint8_t AC_Fence::check() +uint8_t AC_Fence::check(bool disable_auto_fences) { uint8_t ret = 0; + uint8_t disabled_fences = disable_auto_fences ? get_auto_disable_fences() : 0; + uint8_t fences_to_disable = disabled_fences & _enabled_fences; // clear any breach from a non-enabled fence clear_breach(~_configured_fences); + // clear any breach from disabled fences + clear_breach(fences_to_disable); + + // report on any fences that were auto-disabled + if (fences_to_disable) { + print_fence_message("auto-disabled", fences_to_disable); + } // return immediately if disabled if ((!enabled() && !_auto_enabled && !(_configured_fences & AC_FENCE_TYPE_ALT_MIN)) || !_configured_fences) { return 0; } + // disable the (temporarily) disabled fences + enable(false, disabled_fences, false); + // maximum altitude fence check - if (check_fence_alt_max()) { + if (!(disabled_fences & AC_FENCE_TYPE_ALT_MAX) && check_fence_alt_max()) { ret |= AC_FENCE_TYPE_ALT_MAX; } // minimum altitude fence check, do this before auto-disabling (e.g. because falling) // so that any action can be taken - if (floor_enabled() && check_fence_alt_min()) { + if (!(disabled_fences & AC_FENCE_TYPE_ALT_MIN) && check_fence_alt_min()) { ret |= AC_FENCE_TYPE_ALT_MIN; } - // auto enable floor unless auto enable has been set (which means other behaviour is required) - if (auto_enabled() != AutoEnable::ENABLE_ON_AUTO_TAKEOFF && (_configured_fences & AC_FENCE_TYPE_ALT_MIN)) { + // auto enable floor unless auto enable on auto takeoff has been set (which means other behaviour is required) + if (!(disabled_fences & AC_FENCE_TYPE_ALT_MIN)) { auto_enable_fence_floor(); } // circle fence check - if (check_fence_circle()) { + if (!(disabled_fences & AC_FENCE_TYPE_CIRCLE) && check_fence_circle()) { ret |= AC_FENCE_TYPE_CIRCLE; } // polygon fence check - if (check_fence_polygon()) { + if (!(disabled_fences & AC_FENCE_TYPE_POLYGON) && check_fence_polygon()) { ret |= AC_FENCE_TYPE_POLYGON; } @@ -937,7 +903,6 @@ void AC_Fence::disable_floor() {} void AC_Fence::update() {} void AC_Fence::auto_enable_fence_after_takeoff() {} -void AC_Fence::auto_disable_fence_for_landing() {} void AC_Fence::auto_enable_fence_on_arming() {} void AC_Fence::auto_disable_fence_on_disarming() {} @@ -947,7 +912,7 @@ uint8_t AC_Fence::get_enabled_fences() const { return 0; } bool AC_Fence::pre_arm_check(const char* &fail_msg) const { return true; } -uint8_t AC_Fence::check() { return 0; } +uint8_t AC_Fence::check(bool disable_auto_fences) { return 0; } bool AC_Fence::check_destination_within_fence(const Location& loc) { return true; } float AC_Fence::get_breach_distance(uint8_t fence_type) const { return 0.0; } void AC_Fence::get_fence_names(uint8_t fences, ExpandingString& msg) { } diff --git a/libraries/AC_Fence/AC_Fence.h b/libraries/AC_Fence/AC_Fence.h index c346474e27d2a6..aa5c4c254d3c2b 100644 --- a/libraries/AC_Fence/AC_Fence.h +++ b/libraries/AC_Fence/AC_Fence.h @@ -73,7 +73,7 @@ class AC_Fence uint8_t enable_configured(bool value) { return enable(value, _configured_fences, true); } /// auto_enabled - automaticaly enable/disable fence depending of flight status - AutoEnable auto_enabled() { return static_cast(_auto_enabled.get()); } + AutoEnable auto_enabled() const { return static_cast(_auto_enabled.get()); } /// enable_floor - allows fence floor to be enabled/disabled. Note this does not update the eeprom saved value void enable_floor(); @@ -84,21 +84,17 @@ class AC_Fence /// auto_enable_fence_on_takeoff - auto enables the fence. Called after takeoff conditions met void auto_enable_fence_after_takeoff(); - /// auto_disable_fence_for_landing - auto disables respective fence. Called prior to landing. - void auto_disable_fence_for_landing(); - /// auto_enable_fences_on_arming - auto enables all applicable fences on arming void auto_enable_fence_on_arming(); /// auto_disable_fences_on_disarming - auto disables all applicable fences on disarming void auto_disable_fence_on_disarming(); + uint8_t get_auto_disable_fences(void) const; + /// auto_enable_fence_floor - auto enables fence floor once desired altitude has been reached. bool auto_enable_fence_floor(); - /// reset_fence_floor_enable - auto disables the fence floor if below the desired altitude. - bool reset_fence_floor_enable(); - /// enabled - returns whether fencing is enabled or not bool enabled() const { return _enabled_fences; } @@ -119,7 +115,8 @@ class AC_Fence /// /// check - returns the fence type that has been breached (if any) - uint8_t check(); + /// disabled_fences can be used to disable fences for certain conditions (e.g. landing) + uint8_t check(bool disable_auto_fence = false); // returns true if the destination is within fence (used to reject waypoints outside the fence) bool check_destination_within_fence(const class Location& loc); @@ -252,7 +249,6 @@ class AC_Fence float _circle_breach_distance; // distance beyond the circular fence // other internal variables - bool _floor_disabled_for_landing; // fence floor is disabled for landing uint8_t _auto_enable_mask = AC_FENCE_ALL_FENCES; // fences that can be auto-enabled or auto-disabled float _home_distance; // distance from home in meters (provided by main code) float _curr_alt; From 46d6d0bf03ed6e5f7512bd1ad387e298b68ea054 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 31 May 2024 14:13:32 +0100 Subject: [PATCH 072/124] Plane: disable fences for landing by suppressing in the fence check rather than using a state machine --- ArduPlane/commands_logic.cpp | 4 ---- ArduPlane/fence.cpp | 10 +++++++++- ArduPlane/mode_auto.cpp | 4 ---- ArduPlane/mode_qland.cpp | 3 --- ArduPlane/quadplane.cpp | 3 --- 5 files changed, 9 insertions(+), 15 deletions(-) diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index fc87bb4ad63b2f..7c57daf81530be 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -434,10 +434,6 @@ void Plane::do_land(const AP_Mission::Mission_Command& cmd) // if we were in an abort we need to explicitly move out of the abort state, as it's sticky set_flight_stage(AP_FixedWing::FlightStage::LAND); } - -#if AP_FENCE_ENABLED - plane.fence.auto_disable_fence_for_landing(); -#endif } #if HAL_QUADPLANE_ENABLED diff --git a/ArduPlane/fence.cpp b/ArduPlane/fence.cpp index 9615082244736b..0217cbebe4703e 100644 --- a/ArduPlane/fence.cpp +++ b/ArduPlane/fence.cpp @@ -9,8 +9,16 @@ void Plane::fence_check() { const uint8_t orig_breaches = fence.get_breaches(); + uint16_t mission_id = plane.mission.get_current_nav_cmd().id; + bool is_in_landing = plane.flight_stage == AP_FixedWing::FlightStage::LAND +#if HAL_QUADPLANE_ENABLED + || control_mode->mode_number() == Mode::Number::QLAND + || control_mode->mode_number() == Mode::Number::QRTL +#endif + || (plane.is_land_command(mission_id) && plane.mission.state() == AP_Mission::MISSION_RUNNING); + // check for new breaches; new_breaches is bitmask of fence types breached - const uint8_t new_breaches = fence.check(); + const uint8_t new_breaches = fence.check(is_in_landing); if (!fence.enabled()) { // Switch back to the chosen control mode if still in diff --git a/ArduPlane/mode_auto.cpp b/ArduPlane/mode_auto.cpp index e6381b62df25e2..3153d7de31e069 100644 --- a/ArduPlane/mode_auto.cpp +++ b/ArduPlane/mode_auto.cpp @@ -54,10 +54,6 @@ void ModeAuto::_exit() } #endif if (restart) { -#if AP_FENCE_ENABLED - // no longer landing to re-enable the possiblity of the fence floor - plane.fence.reset_fence_floor_enable(); -#endif plane.landing.restart_landing_sequence(); } } diff --git a/ArduPlane/mode_qland.cpp b/ArduPlane/mode_qland.cpp index 4205960f871c30..f3a08aabde7622 100644 --- a/ArduPlane/mode_qland.cpp +++ b/ArduPlane/mode_qland.cpp @@ -16,9 +16,6 @@ bool ModeQLand::_enter() #if AP_LANDINGGEAR_ENABLED plane.g2.landing_gear.deploy_for_landing(); #endif -#if AP_FENCE_ENABLED - plane.fence.auto_disable_fence_for_landing(); -#endif return true; } diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 05a79484577301..c0c69937fa9e4c 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -3554,9 +3554,6 @@ bool QuadPlane::verify_vtol_land(void) poscontrol.pilot_correction_done = false; pos_control->set_lean_angle_max_cd(0); poscontrol.xy_correction.zero(); -#if AP_FENCE_ENABLED - plane.fence.auto_disable_fence_for_landing(); -#endif #if AP_LANDINGGEAR_ENABLED plane.g2.landing_gear.deploy_for_landing(); #endif From eaf001c52fdbf55693aa3e22dd5b1d30d53876c5 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 31 May 2024 14:13:43 +0100 Subject: [PATCH 073/124] Copter: disable fences for landing by suppressing in the fence check rather than using a state machine --- ArduCopter/fence.cpp | 6 +++++- ArduCopter/mode_auto.cpp | 5 ----- ArduCopter/mode_land.cpp | 5 ----- ArduCopter/mode_rtl.cpp | 10 ---------- 4 files changed, 5 insertions(+), 21 deletions(-) diff --git a/ArduCopter/fence.cpp b/ArduCopter/fence.cpp index aba1ecce5bb84e..7cae5dda8dfa8c 100644 --- a/ArduCopter/fence.cpp +++ b/ArduCopter/fence.cpp @@ -10,8 +10,12 @@ void Copter::fence_check() { const uint8_t orig_breaches = fence.get_breaches(); + bool is_in_landing = flightmode->mode_number() == Mode::Number::LAND + || flightmode->mode_number() == Mode::Number::RTL + || flightmode->mode_number() == Mode::Number::SMART_RTL; + // check for new breaches; new_breaches is bitmask of fence types breached - const uint8_t new_breaches = fence.check(); + const uint8_t new_breaches = fence.check(is_in_landing); // we still don't do anything when disarmed, but we do check for fence breaches. // fence pre-arm check actually checks if any fence has been breached diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 9a6b684cf1e8a0..b672ba4d54df59 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -488,11 +488,6 @@ void ModeAuto::land_start() copter.landinggear.deploy_for_landing(); #endif -#if AP_FENCE_ENABLED - // disable the fence on landing - copter.fence.auto_disable_fence_for_landing(); -#endif - // reset flag indicating if pilot has applied roll or pitch inputs during landing copter.ap.land_repo_active = false; diff --git a/ArduCopter/mode_land.cpp b/ArduCopter/mode_land.cpp index 1f34e20fbf3380..2b5b6e8879260f 100644 --- a/ArduCopter/mode_land.cpp +++ b/ArduCopter/mode_land.cpp @@ -41,11 +41,6 @@ bool ModeLand::init(bool ignore_checks) copter.landinggear.deploy_for_landing(); #endif -#if AP_FENCE_ENABLED - // disable the fence on landing - copter.fence.auto_disable_fence_for_landing(); -#endif - #if AC_PRECLAND_ENABLED // initialise precland state machine copter.precland_statemachine.init(); diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index d3250e4265642f..cfee04ff720247 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -255,11 +255,6 @@ void ModeRTL::descent_start() // optionally deploy landing gear copter.landinggear.deploy_for_landing(); #endif - -#if AP_FENCE_ENABLED - // disable the fence on landing - copter.fence.auto_disable_fence_for_landing(); -#endif } // rtl_descent_run - implements the final descent to the RTL_ALT @@ -347,11 +342,6 @@ void ModeRTL::land_start() // optionally deploy landing gear copter.landinggear.deploy_for_landing(); #endif - -#if AP_FENCE_ENABLED - // disable the fence on landing - copter.fence.auto_disable_fence_for_landing(); -#endif } bool ModeRTL::is_landing() const From 255fac215faa44469a9c1f4a210dc903620a3793 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 31 May 2024 15:18:59 +0100 Subject: [PATCH 074/124] AP_Mission: generic fence handling in missions --- libraries/AP_Mission/AP_Mission.cpp | 5 ++++ libraries/AP_Mission/AP_Mission.h | 1 + libraries/AP_Mission/AP_Mission_Commands.cpp | 27 ++++++++++++++++++++ 3 files changed, 33 insertions(+) diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index c11b574e8338f0..438b816176df64 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -16,6 +16,7 @@ #include #include #include +#include const AP_Param::GroupInfo AP_Mission::var_info[] = { @@ -447,6 +448,10 @@ bool AP_Mission::start_command(const Mission_Command& cmd) case MAV_CMD_VIDEO_START_CAPTURE: case MAV_CMD_VIDEO_STOP_CAPTURE: return start_command_camera(cmd); +#endif +#if AP_FENCE_ENABLED + case MAV_CMD_DO_FENCE_ENABLE: + return start_command_fence(cmd); #endif case MAV_CMD_DO_PARACHUTE: return start_command_parachute(cmd); diff --git a/libraries/AP_Mission/AP_Mission.h b/libraries/AP_Mission/AP_Mission.h index e76c3a638f5486..69f8fe2b46b979 100644 --- a/libraries/AP_Mission/AP_Mission.h +++ b/libraries/AP_Mission/AP_Mission.h @@ -936,6 +936,7 @@ class AP_Mission bool start_command_do_sprayer(const AP_Mission::Mission_Command& cmd); bool start_command_do_scripting(const AP_Mission::Mission_Command& cmd); bool start_command_do_gimbal_manager_pitchyaw(const AP_Mission::Mission_Command& cmd); + bool start_command_fence(const AP_Mission::Mission_Command& cmd); /* handle format conversion of storage format to allow us to update diff --git a/libraries/AP_Mission/AP_Mission_Commands.cpp b/libraries/AP_Mission/AP_Mission_Commands.cpp index d44b62e6746867..f296c0c268c809 100644 --- a/libraries/AP_Mission/AP_Mission_Commands.cpp +++ b/libraries/AP_Mission/AP_Mission_Commands.cpp @@ -13,6 +13,7 @@ #include #include #include +#include #if AP_RC_CHANNEL_ENABLED bool AP_Mission::start_command_do_aux_function(const AP_Mission::Mission_Command& cmd) @@ -347,4 +348,30 @@ bool AP_Mission::start_command_do_gimbal_manager_pitchyaw(const AP_Mission::Miss return false; } +bool AP_Mission::start_command_fence(const AP_Mission::Mission_Command& cmd) +{ +#if AP_FENCE_ENABLED + AC_Fence* fence = AP::fence(); + + if (fence == nullptr) { + return false; + } + + if (cmd.p1 == 0) { // disable fence + uint8_t fences = fence->enable_configured(false); + fence->print_fence_message("disabled", fences); + return true; + } else if (cmd.p1 == 1) { // enable fence + uint8_t fences = fence->enable_configured(true); + fence->print_fence_message("enabled", fences); + return true; + } else if (cmd.p1 == 2) { // disable fence floor only + fence->disable_floor(); + fence->print_fence_message("disabled", AC_FENCE_TYPE_ALT_MIN); + return true; + } +#endif // AP_FENCE_ENABLED + return false; +} + #endif // AP_MISSION_ENABLED From a11cc6ea6665c6ed9518416f9b9bf2f0ca4142b8 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 31 May 2024 15:19:26 +0100 Subject: [PATCH 075/124] Plane: use generic fence handling in missions --- ArduPlane/commands_logic.cpp | 15 --------------- 1 file changed, 15 deletions(-) diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 7c57daf81530be..57969f17ce8a4b 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -144,21 +144,6 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd) case MAV_CMD_DO_LAND_START: break; - case MAV_CMD_DO_FENCE_ENABLE: -#if AP_FENCE_ENABLED - if (cmd.p1 == 0) { // disable fence - plane.fence.enable_configured(false); - gcs().send_text(MAV_SEVERITY_INFO, "Fence disabled"); - } else if (cmd.p1 == 1) { // enable fence - plane.fence.enable_configured(true); - gcs().send_text(MAV_SEVERITY_INFO, "Fence enabled"); - } else if (cmd.p1 == 2) { // disable fence floor only - plane.fence.disable_floor(); - gcs().send_text(MAV_SEVERITY_INFO, "Fence floor disabled"); - } -#endif - break; - case MAV_CMD_DO_AUTOTUNE_ENABLE: autotune_enable(cmd.p1); break; From 823ffb6c9e153d29be8e93d7ef34270e53fd3276 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 31 May 2024 15:19:36 +0100 Subject: [PATCH 076/124] Copter: use generic fence handling in missions --- ArduCopter/mode_auto.cpp | 12 ------------ 1 file changed, 12 deletions(-) diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index b672ba4d54df59..4c4ab35ac27631 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -782,18 +782,6 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd) // point the camera to a specified angle do_mount_control(cmd); break; - - case MAV_CMD_DO_FENCE_ENABLE: -#if AP_FENCE_ENABLED - if (cmd.p1 == 0) { //disable - copter.fence.enable_configured(false); - gcs().send_text(MAV_SEVERITY_INFO, "Fence Disabled"); - } else { //enable fence - copter.fence.enable_configured(true); - gcs().send_text(MAV_SEVERITY_INFO, "Fence Enabled"); - } -#endif //AP_FENCE_ENABLED - break; #if AC_NAV_GUIDED == ENABLED case MAV_CMD_DO_GUIDED_LIMITS: // 220 accept guided mode limits From 685fb7e34f9196e49aa2f24a563d809bf8f207f5 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 31 May 2024 15:19:45 +0100 Subject: [PATCH 077/124] Rover: use generic fence handling in missions --- Rover/mode_auto.cpp | 12 ------------ 1 file changed, 12 deletions(-) diff --git a/Rover/mode_auto.cpp b/Rover/mode_auto.cpp index f18ab0a8f9db95..6e97b0a1d24a35 100644 --- a/Rover/mode_auto.cpp +++ b/Rover/mode_auto.cpp @@ -593,18 +593,6 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd) do_set_reverse(cmd); break; - case MAV_CMD_DO_FENCE_ENABLE: -#if AP_FENCE_ENABLED - if (cmd.p1 == 0) { //disable - rover.fence.enable_configured(false); - gcs().send_text(MAV_SEVERITY_INFO, "Fence Disabled"); - } else { //enable fence - rover.fence.enable_configured(true); - gcs().send_text(MAV_SEVERITY_INFO, "Fence Enabled"); - } -#endif - break; - case MAV_CMD_DO_GUIDED_LIMITS: do_guided_limits(cmd); break; From f0456f29dd4e008930bb4d856f03c38e4c2ee24c Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 22 Jun 2024 12:02:21 +0100 Subject: [PATCH 078/124] AC_Fence: always disable Min Alt fence on landing allow precise pre-arm messages --- libraries/AC_Fence/AC_Fence.cpp | 48 ++++++++++++++++----------------- libraries/AC_Fence/AC_Fence.h | 8 +++--- 2 files changed, 28 insertions(+), 28 deletions(-) diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp index e28ad0b6e23964..df03fd493b9ba4 100644 --- a/libraries/AC_Fence/AC_Fence.cpp +++ b/libraries/AC_Fence/AC_Fence.cpp @@ -341,10 +341,9 @@ uint8_t AC_Fence::get_auto_disable_fences(void) const break; case AC_Fence::AutoEnable::ENABLE_DISABLE_FLOOR_ONLY: case AC_Fence::AutoEnable::ONLY_WHEN_ARMED: + default: // when auto disable is not set we still need to disable the altmin fence on landing auto_disable = _auto_enable_mask & AC_FENCE_TYPE_ALT_MIN; break; - default: - break; } return auto_disable; } @@ -366,7 +365,7 @@ uint8_t AC_Fence::get_enabled_fences() const } // additional checks for the polygon fence: -bool AC_Fence::pre_arm_check_polygon(const char* &fail_msg) const +bool AC_Fence::pre_arm_check_polygon(char *failure_msg, const uint8_t failure_msg_len) const { if (!(_configured_fences & AC_FENCE_TYPE_POLYGON)) { // not enabled; all good @@ -374,12 +373,12 @@ bool AC_Fence::pre_arm_check_polygon(const char* &fail_msg) const } if (! _poly_loader.loaded()) { - fail_msg = "Fences invalid"; + hal.util->snprintf(failure_msg, failure_msg_len, "Polygon fence(s) invalid"); return false; } if (!_poly_loader.check_inclusion_circle_margin(_margin)) { - fail_msg = "Margin is less than inclusion circle radius"; + hal.util->snprintf(failure_msg, failure_msg_len, "Polygon fence margin is less than inclusion circle radius"); return false; } @@ -387,14 +386,14 @@ bool AC_Fence::pre_arm_check_polygon(const char* &fail_msg) const } // additional checks for the circle fence: -bool AC_Fence::pre_arm_check_circle(const char* &fail_msg) const +bool AC_Fence::pre_arm_check_circle(char *failure_msg, const uint8_t failure_msg_len) const { if (_circle_radius < 0) { - fail_msg = "Invalid FENCE_RADIUS value"; + hal.util->snprintf(failure_msg, failure_msg_len, "Invalid Circle FENCE_RADIUS value"); return false; } if (_circle_radius < _margin) { - fail_msg = "FENCE_MARGIN is less than FENCE_RADIUS"; + hal.util->snprintf(failure_msg, failure_msg_len, "Circle FENCE_MARGIN is less than FENCE_RADIUS"); return false; } @@ -402,15 +401,15 @@ bool AC_Fence::pre_arm_check_circle(const char* &fail_msg) const } // additional checks for the alt fence: -bool AC_Fence::pre_arm_check_alt(const char* &fail_msg) const +bool AC_Fence::pre_arm_check_alt(char *failure_msg, const uint8_t failure_msg_len) const { if (_alt_max < 0.0f) { - fail_msg = "Invalid FENCE_ALT_MAX value"; + hal.util->snprintf(failure_msg, failure_msg_len, "Invalid FENCE_ALT_MAX value"); return false; } if (_alt_min < -100.0f) { - fail_msg = "Invalid FENCE_ALT_MIN value"; + hal.util->snprintf(failure_msg, failure_msg_len, "Invalid FENCE_ALT_MIN value"); return false; } return true; @@ -418,13 +417,11 @@ bool AC_Fence::pre_arm_check_alt(const char* &fail_msg) const /// pre_arm_check - returns true if all pre-takeoff checks have completed successfully -bool AC_Fence::pre_arm_check(const char* &fail_msg) const +bool AC_Fence::pre_arm_check(char *failure_msg, const uint8_t failure_msg_len) const { - fail_msg = nullptr; - // if fences are enabled but none selected fail pre-arm check if (_enabled && !present()) { - fail_msg = "Fences enabled, but none selected"; + hal.util->snprintf(failure_msg, failure_msg_len, "Fences enabled, but none selected"); return false; } @@ -439,42 +436,45 @@ bool AC_Fence::pre_arm_check(const char* &fail_msg) const (_configured_fences & AC_FENCE_TYPE_POLYGON)) { Vector2f position; if (!AP::ahrs().get_relative_position_NE_home(position)) { - fail_msg = "Fence requires position"; + hal.util->snprintf(failure_msg, failure_msg_len, "Fence requires position"); return false; } } - if (!pre_arm_check_polygon(fail_msg)) { + if (!pre_arm_check_polygon(failure_msg, failure_msg_len)) { return false; } - if (!pre_arm_check_circle(fail_msg)) { + if (!pre_arm_check_circle(failure_msg, failure_msg_len)) { return false; } - if (!pre_arm_check_alt(fail_msg)) { + if (!pre_arm_check_alt(failure_msg, failure_msg_len)) { return false; } // check no limits are currently breached if (_breached_fences) { - fail_msg = "vehicle outside fence"; + char msg[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1]; + ExpandingString e(msg, MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1); + AC_Fence::get_fence_names(_breached_fences, e); + hal.util->snprintf(failure_msg, failure_msg_len, "vehicle outside %s", e.get_writeable_string()); return false; } // validate FENCE_MARGIN parameter range if (_margin < 0.0f) { - fail_msg = "Invalid FENCE_MARGIN value"; + hal.util->snprintf(failure_msg, failure_msg_len, "Invalid FENCE_MARGIN value"); return false; } if (_alt_max < _alt_min) { - fail_msg = "FENCE_ALT_MAX < FENCE_ALT_MIN"; + hal.util->snprintf(failure_msg, failure_msg_len, "FENCE_ALT_MAX < FENCE_ALT_MIN"); return false; } if (_alt_max - _alt_min <= 2.0f * _margin) { - fail_msg = "FENCE_MARGIN too big"; + hal.util->snprintf(failure_msg, failure_msg_len, "FENCE_MARGIN too big"); return false; } @@ -910,7 +910,7 @@ uint8_t AC_Fence::present() const { return 0; } uint8_t AC_Fence::get_enabled_fences() const { return 0; } -bool AC_Fence::pre_arm_check(const char* &fail_msg) const { return true; } +bool AC_Fence::pre_arm_check(char *failure_msg, const uint8_t failure_msg_len) const { return true; } uint8_t AC_Fence::check(bool disable_auto_fences) { return 0; } bool AC_Fence::check_destination_within_fence(const Location& loc) { return true; } diff --git a/libraries/AC_Fence/AC_Fence.h b/libraries/AC_Fence/AC_Fence.h index aa5c4c254d3c2b..3165d7b73d4d33 100644 --- a/libraries/AC_Fence/AC_Fence.h +++ b/libraries/AC_Fence/AC_Fence.h @@ -108,7 +108,7 @@ class AC_Fence void update(); /// pre_arm_check - returns true if all pre-takeoff checks have completed successfully - bool pre_arm_check(const char* &fail_msg) const; + bool pre_arm_check(char *failure_msg, const uint8_t failure_msg_len) const; /// /// methods to check we are within the boundaries and recover @@ -215,9 +215,9 @@ class AC_Fence void clear_breach(uint8_t fence_type); // additional checks for the different fence types: - bool pre_arm_check_polygon(const char* &fail_msg) const; - bool pre_arm_check_circle(const char* &fail_msg) const; - bool pre_arm_check_alt(const char* &fail_msg) const; + bool pre_arm_check_polygon(char *failure_msg, const uint8_t failure_msg_len) const; + bool pre_arm_check_circle(char *failure_msg, const uint8_t failure_msg_len) const; + bool pre_arm_check_alt(char *failure_msg, const uint8_t failure_msg_len) const; // fence floor is enabled bool floor_enabled() const { return _enabled_fences & AC_FENCE_TYPE_ALT_MIN; } From 37e0923ec25745d7e6de1944d8e75c9fc96b5dba Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 22 Jun 2024 16:52:43 +0100 Subject: [PATCH 079/124] AP_Arming: allow precise wording of fence pre-arm messages --- libraries/AP_Arming/AP_Arming.cpp | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp index 2be3a1011fe6e1..41ec47659d650f 100644 --- a/libraries/AP_Arming/AP_Arming.cpp +++ b/libraries/AP_Arming/AP_Arming.cpp @@ -1280,16 +1280,12 @@ bool AP_Arming::fence_checks(bool display_failure) } // check fence is ready - const char *fail_msg = nullptr; - if (fence->pre_arm_check(fail_msg)) { + char fail_msg[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1]; + if (fence->pre_arm_check(fail_msg, ARRAY_SIZE(fail_msg))) { return true; } - if (fail_msg == nullptr) { - check_failed(display_failure, "Check fence"); - } else { - check_failed(display_failure, "%s", fail_msg); - } + check_failed(display_failure, "%s", fail_msg); #if AP_SDCARD_STORAGE_ENABLED if (fence->failed_sdcard_storage() || StorageManager::storage_failed()) { From f8304e12d110f70de7e71646227c1b8dd2ba2b3e Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 22 Jun 2024 16:53:07 +0100 Subject: [PATCH 080/124] Copter: don't breach auto-fences when landed --- ArduCopter/fence.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ArduCopter/fence.cpp b/ArduCopter/fence.cpp index 7cae5dda8dfa8c..54b363ec16e98c 100644 --- a/ArduCopter/fence.cpp +++ b/ArduCopter/fence.cpp @@ -12,7 +12,8 @@ void Copter::fence_check() bool is_in_landing = flightmode->mode_number() == Mode::Number::LAND || flightmode->mode_number() == Mode::Number::RTL - || flightmode->mode_number() == Mode::Number::SMART_RTL; + || flightmode->mode_number() == Mode::Number::SMART_RTL + || ap.land_complete || !motors->armed(); // check for new breaches; new_breaches is bitmask of fence types breached const uint8_t new_breaches = fence.check(is_in_landing); From e30dc2c53675b81fdebc331c99461689c99f6b7f Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 31 May 2024 17:12:10 +0100 Subject: [PATCH 081/124] autotest: fix fence autotests add Plane.FenceMinAltEnableAutoland test that vehicle can be landed manually after descending below fence floor --- Tools/autotest/arducopter.py | 28 ++++++++++++-------- Tools/autotest/arduplane.py | 50 +++++++++++++++++++++++++++++++++--- Tools/autotest/rover.py | 10 ++++---- 3 files changed, 70 insertions(+), 18 deletions(-) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index e0eda4cdae5de8..84303118b7e0ad 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -1454,7 +1454,7 @@ def fly_fence_avoid_test_radius_check(self, timeout=180, avoid_behave=avoid_beha self.wait_altitude(10, 100, relative=True) self.set_rc(3, 1500) self.set_rc(2, 1400) - self.wait_distance_to_home(12, 20) + self.wait_distance_to_home(12, 20, timeout=30) tstart = self.get_sim_time() push_time = 70 # push against barrier for 60 seconds failed_max = False @@ -1513,7 +1513,7 @@ def HorizontalFence(self, timeout=180): self.load_fence("fence-in-middle-of-nowhere.txt") self.delay_sim_time(5) # let fence check run so it loads-from-eeprom - self.assert_prearm_failure("vehicle outside fence") + self.assert_prearm_failure("vehicle outside Polygon fence") self.progress("Failed to arm outside fence (good!)") self.clear_fence() self.delay_sim_time(5) # let fence breach clear @@ -1523,7 +1523,7 @@ def HorizontalFence(self, timeout=180): self.start_subtest("ensure we can't arm with bad radius") self.context_push() self.set_parameter("FENCE_RADIUS", -1) - self.assert_prearm_failure("Invalid FENCE_RADIUS value") + self.assert_prearm_failure("Invalid Circle FENCE_RADIUS value") self.context_pop() self.progress("Failed to arm with bad radius") self.drain_mav() @@ -1741,14 +1741,14 @@ def FenceFloorEnabledLanding(self): "AVOID_ENABLE": 0, "FENCE_ENABLE" : 1, "FENCE_TYPE": 15, - "FENCE_ALT_MIN": 10, - "FENCE_ALT_MAX": 20, + "FENCE_ALT_MIN": 20, + "FENCE_ALT_MAX": 30, }) self.change_mode("GUIDED") self.wait_ready_to_arm() self.arm_vehicle() - self.user_takeoff(alt_min=15) + self.user_takeoff(alt_min=25) # Check fence is enabled self.do_fence_enable() @@ -1760,8 +1760,19 @@ def FenceFloorEnabledLanding(self): self.set_rc(3, 1800) self.wait_mode('RTL', timeout=120) + # center throttle + self.set_rc(3, 1500) - self.wait_landed_and_disarmed() + # wait until we are below the fence floor and re-enter loiter + self.wait_altitude(5, 15, relative=True) + self.change_mode('LOITER') + # wait for manual recovery to expire + self.delay_sim_time(15) + + # lower throttle and try and land + self.set_rc(3, 1300) + self.wait_altitude(0, 2, relative=True) + self.wait_disarmed() self.assert_fence_enabled() # Assert fence is not healthy since it was enabled manually @@ -1799,8 +1810,6 @@ def FenceFloorAutoDisableLanding(self): self.set_rc(3, 1800) self.wait_mode('RTL', timeout=120) - # Assert fence is not healthy now that we are in RTL - self.assert_sensor_state(fence_bit, healthy=False) self.wait_landed_and_disarmed(0) # the breach should have cleared since we auto-disable the @@ -12005,7 +12014,6 @@ def tests(self): def disabled_tests(self): return { "Parachute": "See https://github.com/ArduPilot/ardupilot/issues/4702", - "HorizontalAvoidFence": "See https://github.com/ArduPilot/ardupilot/issues/11525", "AltEstimation": "See https://github.com/ArduPilot/ardupilot/issues/15191", "GroundEffectCompensation_takeOffExpected": "Flapping", "GroundEffectCompensation_touchDownExpected": "Flapping", diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 24821c55eac4e3..6e4122eaf3bd83 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -1601,7 +1601,7 @@ def FenceStatic(self): self.delay_sim_time(2) # Allow breach to propagate self.assert_fence_enabled() - self.try_arm(False, "vehicle outside fence") + self.try_arm(False, "vehicle outside Min Alt fence") self.do_fence_disable() self.set_parameter("FENCE_ALT_MIN", default_fence_alt_min) @@ -1620,7 +1620,7 @@ def FenceStatic(self): self.do_fence_enable() self.assert_fence_enabled() self.delay_sim_time(2) # Allow breach to propagate - self.try_arm(False, "vehicle outside fence") + self.try_arm(False, "vehicle outside Polygon fence") self.do_fence_disable() self.clear_fence() @@ -1640,7 +1640,7 @@ def FenceStatic(self): self.do_fence_enable() self.assert_fence_enabled() self.delay_sim_time(2) # Allow breach to propagate - self.try_arm(False, "vehicle outside fence") + self.try_arm(False, "vehicle outside Polygon fence") self.do_fence_disable() self.clear_fence() @@ -3742,6 +3742,49 @@ def FenceMinAltAutoEnable(self): self.set_current_waypoint(0, check_afterwards=False) self.set_rc(3, 1000) # lower throttle + def FenceMinAltEnableAutoland(self): + '''Tests autolanding when alt min fence is enabled''' + self.set_parameters({ + "FENCE_TYPE": 12, # Set fence type to min alt and max alt + "FENCE_ACTION": 1, # Set action to RTL + "FENCE_ALT_MIN": 20, + "FENCE_AUTOENABLE": 0, + "FENCE_ENABLE" : 1, + "RTL_AUTOLAND" : 2, + }) + + # Grab Home Position + self.mav.recv_match(type='HOME_POSITION', blocking=True) + self.homeloc = self.mav.location() + + self.wait_ready_to_arm() + self.arm_vehicle() + + self.takeoff(alt=50, mode='TAKEOFF') + self.change_mode("FBWA") + self.set_rc(3, 1100) # lower throttle + + self.progress("Waiting for RTL") + tstart = self.get_sim_time() + mode = "RTL" + while not self.mode_is(mode, drain_mav=False): + self.mav.messages['HEARTBEAT'].custom_mode + self.progress("mav.flightmode=%s Want=%s Alt=%f" % ( + self.mav.flightmode, mode, self.get_altitude(relative=True))) + if (self.get_sim_time_cached() > tstart + 120): + raise WaitModeTimeout("Did not change mode") + self.progress("Got mode %s" % mode) + # switch to FBWA + self.change_mode("FBWA") + self.set_rc(3, 1500) # raise throttle + self.wait_altitude(25, 35, timeout=50, relative=True) + self.set_rc(3, 1000) # lower throttle + # Now check we can land + self.fly_home_land_and_disarm() + self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_ALL) + self.set_current_waypoint(0, check_afterwards=False) + self.set_rc(3, 1000) # lower throttle + def FenceMinAltAutoEnableAbort(self): '''Tests autoenablement of the alt min fence and fences on arming''' self.set_parameters({ @@ -5776,6 +5819,7 @@ def tests(self): self.FenceRetRally, self.FenceAltCeilFloor, self.FenceMinAltAutoEnable, + self.FenceMinAltEnableAutoland, self.FenceMinAltAutoEnableAbort, self.FenceAutoEnableDisableSwitch, self.FenceEnableDisableSwitch, diff --git a/Tools/autotest/rover.py b/Tools/autotest/rover.py index 22c38c5c50b52d..941ae2f76695b0 100644 --- a/Tools/autotest/rover.py +++ b/Tools/autotest/rover.py @@ -6881,10 +6881,10 @@ def MissionPolyEnabledPreArm(self): ]), ]) - self.assert_prearm_failure('vehicle outside fence', other_prearm_failures_fatal=False) + self.assert_prearm_failure('vehicle outside Polygon fence', other_prearm_failures_fatal=False) self.reboot_sitl() - self.assert_prearm_failure('vehicle outside fence', other_prearm_failures_fatal=False, timeout=120) + self.assert_prearm_failure('vehicle outside Polygon fence', other_prearm_failures_fatal=False, timeout=120) self.progress("Ensure we can arm when a polyfence fence is cleared when we've previously been in breach") self.clear_fence() @@ -6900,7 +6900,7 @@ def MissionPolyEnabledPreArm(self): ]), ]) self.reboot_sitl() - self.assert_prearm_failure('vehicle outside fence', other_prearm_failures_fatal=False, timeout=120) + self.assert_prearm_failure('vehicle outside Polygon fence', other_prearm_failures_fatal=False, timeout=120) self.clear_fence() self.wait_ready_to_arm() @@ -6914,11 +6914,11 @@ def MissionPolyEnabledPreArm(self): self.offset_location_ne(here, 50, 20), # tl, ]), ]) - self.assert_prearm_failure('vehicle outside fence', other_prearm_failures_fatal=False, timeout=120) + self.assert_prearm_failure('vehicle outside Polygon fence', other_prearm_failures_fatal=False, timeout=120) self.set_parameter('FENCE_TYPE', 2) self.wait_ready_to_arm() self.set_parameter('FENCE_TYPE', 6) - self.assert_prearm_failure('vehicle outside fence', other_prearm_failures_fatal=False, timeout=120) + self.assert_prearm_failure('vehicle outside Polygon fence', other_prearm_failures_fatal=False, timeout=120) def OpticalFlow(self): '''lightly test OpticalFlow''' From 29a320b31051fd4f0194d0f745644bfa11694259 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Mon, 24 Jun 2024 20:46:22 +0100 Subject: [PATCH 082/124] AC_Avoidance: correctly set back away speed for minimum alt fences --- libraries/AC_Avoidance/AC_Avoid.cpp | 96 +++++++++++++++++------------ libraries/AC_Avoidance/AC_Avoid.h | 9 +-- 2 files changed, 56 insertions(+), 49 deletions(-) diff --git a/libraries/AC_Avoidance/AC_Avoid.cpp b/libraries/AC_Avoidance/AC_Avoid.cpp index 2756e4b8fddd83..6169ad161d1fe1 100644 --- a/libraries/AC_Avoidance/AC_Avoid.cpp +++ b/libraries/AC_Avoidance/AC_Avoid.cpp @@ -369,6 +369,19 @@ void AC_Avoid::adjust_speed(float kP, float accel, float heading, float &speed, } } +// adjust vertical climb rate so vehicle does not break the vertical fence +void AC_Avoid::adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_cms, float dt) { + float backup_speed = 0.0f; + adjust_velocity_z(kP, accel_cmss, climb_rate_cms, backup_speed, dt); + if (!is_zero(backup_speed)) { + if (is_negative(backup_speed)) { + climb_rate_cms = MIN(climb_rate_cms, backup_speed); + } else { + climb_rate_cms = MAX(climb_rate_cms, backup_speed); + } + } +} + // adjust vertical climb rate so vehicle does not break the vertical fence void AC_Avoid::adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_cms, float& backup_speed, float dt) { @@ -388,32 +401,10 @@ void AC_Avoid::adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_c // limit acceleration const float accel_cmss_limited = MIN(accel_cmss, AC_AVOID_ACCEL_CMSS_MAX); - // if descending, adjust for minimum altitude as necessary - if (climb_rate_cms < 0.0f) { -#if AP_FENCE_ENABLED - // calculate distance below fence - AC_Fence *fence = AP::fence(); - if ((_enabled & AC_AVOID_STOP_AT_FENCE) > 0 && fence - && (fence->get_enabled_fences() & AC_FENCE_TYPE_ALT_MIN) > 0) { - // calculate distance from vehicle to safe altitude - float veh_alt; - _ahrs.get_relative_position_D_home(veh_alt); - // fence.get_safe_alt_min() is UP, veh_alt is DOWN: - const float alt_min = -(fence->get_safe_alt_min() + veh_alt); - - if (alt_min <= 0.0f) { - climb_rate_cms = MAX(climb_rate_cms, 0.0f); - // also calculate backup speed that will get us back to safe altitude - backup_speed = get_max_speed(kP, accel_cmss_limited, -alt_min*100.0f, dt); - } - } -#endif - return; - } - - bool limit_alt = false; - float alt_diff = 0.0f; // distance from altitude limit to vehicle in metres (positive means vehicle is below limit) - + bool limit_min_alt = false; + bool limit_max_alt = false; + float max_alt_diff = 0.0f; // distance from altitude limit to vehicle in metres (positive means vehicle is below limit) + float min_alt_diff = 0.0f; #if AP_FENCE_ENABLED // calculate distance below fence AC_Fence *fence = AP::fence(); @@ -421,10 +412,15 @@ void AC_Avoid::adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_c // calculate distance from vehicle to safe altitude float veh_alt; _ahrs.get_relative_position_D_home(veh_alt); + if ((fence->get_enabled_fences() & AC_FENCE_TYPE_ALT_MIN) > 0) { + // fence.get_safe_alt_max() is UP, veh_alt is DOWN: + min_alt_diff = -(fence->get_safe_alt_min() + veh_alt); + limit_min_alt = true; + } if ((fence->get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) > 0) { // fence.get_safe_alt_max() is UP, veh_alt is DOWN: - alt_diff = fence->get_safe_alt_max() + veh_alt; - limit_alt = true; + max_alt_diff = fence->get_safe_alt_max() + veh_alt; + limit_max_alt = true; } } #endif @@ -437,9 +433,9 @@ void AC_Avoid::adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_c _ahrs.get_relative_position_D_origin(curr_alt)) { // alt_limit is UP, curr_alt is DOWN: const float ctrl_alt_diff = alt_limit + curr_alt; - if (!limit_alt || ctrl_alt_diff < alt_diff) { - alt_diff = ctrl_alt_diff; - limit_alt = true; + if (!limit_max_alt || ctrl_alt_diff < max_alt_diff) { + max_alt_diff = ctrl_alt_diff; + limit_max_alt = true; } } @@ -449,34 +445,52 @@ void AC_Avoid::adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_c AP_Proximity *proximity = AP::proximity(); if (proximity && proximity_avoidance_enabled() && proximity->get_upward_distance(proximity_alt_diff)) { proximity_alt_diff -= _margin; - if (!limit_alt || proximity_alt_diff < alt_diff) { - alt_diff = proximity_alt_diff; - limit_alt = true; + if (!limit_max_alt || proximity_alt_diff < max_alt_diff) { + max_alt_diff = proximity_alt_diff; + limit_max_alt = true; } } #endif // limit climb rate - if (limit_alt) { + if (limit_max_alt || limit_min_alt) { + const float max_back_spd_cms = _backup_speed_z_max * 100.0; // do not allow climbing if we've breached the safe altitude - if (alt_diff <= 0.0f) { + if (max_alt_diff <= 0.0f && limit_max_alt) { climb_rate_cms = MIN(climb_rate_cms, 0.0f); // also calculate backup speed that will get us back to safe altitude - const float max_back_spd_cms = _backup_speed_z_max * 100.0; if (is_positive(max_back_spd_cms)) { - backup_speed = -1*(get_max_speed(kP, accel_cmss_limited, -alt_diff*100.0f, dt)); + backup_speed = -1*(get_max_speed(kP, accel_cmss_limited, -max_alt_diff*100.0f, dt)); // Constrain to max backup speed backup_speed = MAX(backup_speed, -max_back_spd_cms); } return; + // do not allow descending if we've breached the safe altitude + } else if (min_alt_diff <= 0.0f && limit_min_alt) { + climb_rate_cms = MAX(climb_rate_cms, 0.0f); + // also calculate backup speed that will get us back to safe altitude + if (is_positive(max_back_spd_cms)) { + backup_speed = get_max_speed(kP, accel_cmss_limited, -min_alt_diff*100.0f, dt); + + // Constrain to max backup speed + backup_speed = MIN(backup_speed, max_back_spd_cms); + } + return; } // limit climb rate - const float max_speed = get_max_speed(kP, accel_cmss_limited, alt_diff*100.0f, dt); - climb_rate_cms = MIN(max_speed, climb_rate_cms); + if (limit_max_alt) { + const float max_alt_max_speed = get_max_speed(kP, accel_cmss_limited, max_alt_diff*100.0f, dt); + climb_rate_cms = MIN(max_alt_max_speed, climb_rate_cms); + } + + if (limit_min_alt) { + const float max_alt_min_speed = get_max_speed(kP, accel_cmss_limited, min_alt_diff*100.0f, dt); + climb_rate_cms = MAX(-max_alt_min_speed, climb_rate_cms); + } } -# endif +#endif } // adjust roll-pitch to push vehicle away from objects diff --git a/libraries/AC_Avoidance/AC_Avoid.h b/libraries/AC_Avoidance/AC_Avoid.h index 2321d74989951a..aae55725ec1a14 100644 --- a/libraries/AC_Avoidance/AC_Avoid.h +++ b/libraries/AC_Avoidance/AC_Avoid.h @@ -68,14 +68,7 @@ class AC_Avoid { // adjust vertical climb rate so vehicle does not break the vertical fence void adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_cms, float& backup_speed, float dt); - void adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_cms, float dt) { - float backup_speed = 0.0f; - adjust_velocity_z(kP, accel_cmss, climb_rate_cms, backup_speed, dt); - if (!is_zero(backup_speed)) { - climb_rate_cms = MIN(climb_rate_cms, backup_speed); - } - } - + void adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_cms, float dt); // adjust roll-pitch to push vehicle away from objects // roll and pitch value are in centi-degrees From b7ce3ff2860aea550f369d30d716dd1686e5f9aa Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Mon, 24 Jun 2024 20:46:49 +0100 Subject: [PATCH 083/124] autotest: add test for minimum altitude avoidance fence --- Tools/autotest/arducopter.py | 54 ++++++++++++++++++++++++++++++++++++ 1 file changed, 54 insertions(+) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 84303118b7e0ad..f41d11fbe736b8 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -1730,6 +1730,59 @@ def MinAltFence(self): self.zero_throttle() + # MinAltFenceAvoid - fly down and make sure fence action does not trigger + # Also check that the vehicle will not try and ascend too fast when trying to backup from a min alt fence due to avoidance + def MinAltFenceAvoid(self): + '''Test Min Alt Fence Avoidance''' + self.takeoff(30, mode="LOITER") + """Hold loiter position.""" + + # enable fence, only min altitude + # No action, rely on avoidance to prevent the breach + self.set_parameters({ + "FENCE_ENABLE": 1, + "FENCE_TYPE": 8, + "FENCE_ALT_MIN": 20, + "FENCE_ACTION": 0, + }) + + # Try and fly past the fence + self.set_rc(3, 1120) + + # Avoid should prevent the vehicle flying past the fence, so the altitude wait should timeouts + try: + self.wait_altitude(10, 15, timeout=90, relative=True) + raise NotAchievedException("Avoid should prevent reaching altitude") + except AutoTestTimeoutException: + pass + except Exception as e: + raise e + + # Check ascent is not too fast, allow 10% above the configured backup speed + max_ascent_rate = self.get_parameter("AVOID_BACKUP_SPD") * 1.1 + + def get_climb_rate(mav, m): + m_type = m.get_type() + if m_type != 'VFR_HUD': + return + if m.climb > max_ascent_rate: + raise NotAchievedException("Ascending too fast want %f got %f" % (max_ascent_rate, m.climb)) + + self.context_push() + self.install_message_hook_context(get_climb_rate) + + # Reduce fence alt, this will result in a fence breach, but there is no action. + # Avoid should then backup the vehicle to be over the new fence alt. + self.set_parameters({ + "FENCE_ALT_MIN": 30, + }) + self.wait_altitude(30, 40, timeout=90, relative=True) + + self.context_pop() + + self.set_rc(3, 1500) + self.do_RTL() + def FenceFloorEnabledLanding(self): """Ensures we can initiate and complete an RTL while the fence is enabled. @@ -10672,6 +10725,7 @@ def tests1d(self): self.MaxAltFence, self.MaxAltFenceAvoid, self.MinAltFence, + self.MinAltFenceAvoid, self.FenceFloorEnabledLanding, self.FenceFloorAutoDisableLanding, self.FenceFloorAutoEnableOnArming, From 8c0c84b7abf3fc69545107361db72f68ba8e7c3c Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Tue, 25 Jun 2024 17:06:56 +0100 Subject: [PATCH 084/124] Copter: only disable fences when in landing phase --- ArduCopter/fence.cpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/ArduCopter/fence.cpp b/ArduCopter/fence.cpp index 54b363ec16e98c..242dbef0cbad05 100644 --- a/ArduCopter/fence.cpp +++ b/ArduCopter/fence.cpp @@ -10,10 +10,7 @@ void Copter::fence_check() { const uint8_t orig_breaches = fence.get_breaches(); - bool is_in_landing = flightmode->mode_number() == Mode::Number::LAND - || flightmode->mode_number() == Mode::Number::RTL - || flightmode->mode_number() == Mode::Number::SMART_RTL - || ap.land_complete || !motors->armed(); + bool is_in_landing = flightmode->is_landing() || ap.land_complete || !motors->armed(); // check for new breaches; new_breaches is bitmask of fence types breached const uint8_t new_breaches = fence.check(is_in_landing); From e4cbee133b2ec40583d165e495dcd3e0c3bd36e7 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Tue, 25 Jun 2024 17:07:13 +0100 Subject: [PATCH 085/124] Plane: only disable fences when in landing phase --- ArduPlane/fence.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduPlane/fence.cpp b/ArduPlane/fence.cpp index 0217cbebe4703e..5bc3ab93ffa0ba 100644 --- a/ArduPlane/fence.cpp +++ b/ArduPlane/fence.cpp @@ -13,7 +13,7 @@ void Plane::fence_check() bool is_in_landing = plane.flight_stage == AP_FixedWing::FlightStage::LAND #if HAL_QUADPLANE_ENABLED || control_mode->mode_number() == Mode::Number::QLAND - || control_mode->mode_number() == Mode::Number::QRTL + || quadplane.in_vtol_land_descent() #endif || (plane.is_land_command(mission_id) && plane.mission.state() == AP_Mission::MISSION_RUNNING); From c40b80b10047d75dc37e766d8545d747ebd79432 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Tue, 25 Jun 2024 17:11:46 +0100 Subject: [PATCH 086/124] Copter: don't print fence cleared messages when sitting on the ground --- ArduCopter/fence.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/ArduCopter/fence.cpp b/ArduCopter/fence.cpp index 242dbef0cbad05..5ad1ccb1844c62 100644 --- a/ArduCopter/fence.cpp +++ b/ArduCopter/fence.cpp @@ -84,7 +84,9 @@ void Copter::fence_check() LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode(new_breaches)); } else if (orig_breaches && fence.get_breaches() == 0) { - GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Fence breach cleared"); + if (!copter.ap.land_complete) { + GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Fence breach cleared"); + } // record clearing of breach LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode::ERROR_RESOLVED); } From 5bc372714553baff403d7ec1c3949df72fcad49a Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Wed, 26 Jun 2024 10:09:08 +0100 Subject: [PATCH 087/124] AC_Fence: support FENCE_OPTIONS on copter --- libraries/AC_Fence/AC_Fence.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp index df03fd493b9ba4..808f926fa80cc0 100644 --- a/libraries/AC_Fence/AC_Fence.cpp +++ b/libraries/AC_Fence/AC_Fence.cpp @@ -36,8 +36,10 @@ extern const AP_HAL::HAL& hal; #if APM_BUILD_TYPE(APM_BUILD_ArduPlane) #define AC_FENCE_CIRCLE_RADIUS_BACKUP_DISTANCE 100.0 // after fence is broken we recreate the fence 100m further out +#define AC_FENCE_OPTIONS_DEFAULT OPTIONS::DISABLE_MODE_CHANGE #else #define AC_FENCE_CIRCLE_RADIUS_BACKUP_DISTANCE 20.0 // after fence is broken we recreate the fence 20m further out +#define AC_FENCE_OPTIONS_DEFAULT 0 #endif //#define AC_FENCE_DEBUG @@ -137,12 +139,12 @@ const AP_Param::GroupInfo AC_Fence::var_info[] = { // @User: Standard AP_GROUPINFO_FRAME("AUTOENABLE", 10, AC_Fence, _auto_enabled, static_cast(AutoEnable::ALWAYS_DISABLED), AP_PARAM_FRAME_PLANE | AP_PARAM_FRAME_COPTER | AP_PARAM_FRAME_TRICOPTER | AP_PARAM_FRAME_HELI), - // @Param{Plane}: OPTIONS + // @Param{Plane, Copter}: OPTIONS // @DisplayName: Fence options // @Description: When bit 0 is set sisable mode change following fence action until fence breach is cleared. When bit 1 is set the allowable flight areas is the union of all polygon and circle fence areas instead of the intersection, which means a fence breach occurs only if you are outside all of the fence areas. // @Bitmask: 0:Disable mode change following fence action until fence breach is cleared, 1:Allow union of inclusion areas // @User: Standard - AP_GROUPINFO_FRAME("OPTIONS", 11, AC_Fence, _options, static_cast(OPTIONS::DISABLE_MODE_CHANGE), AP_PARAM_FRAME_PLANE), + AP_GROUPINFO_FRAME("OPTIONS", 11, AC_Fence, _options, static_cast(AC_FENCE_OPTIONS_DEFAULT), AP_PARAM_FRAME_PLANE | AP_PARAM_FRAME_COPTER), AP_GROUPEND }; From 2877472f273e6387fa15b91179933b002419edbb Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Wed, 26 Jun 2024 10:09:26 +0100 Subject: [PATCH 088/124] Copter: support FENCE_OPTIONS on copter --- ArduCopter/mode.cpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 69cda906803ec9..97bac5da02ffd1 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -347,6 +347,20 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason) return false; } +#if AP_FENCE_ENABLED + // may not be allowed to change mode if recovering from fence breach + if (!ignore_checks && + fence.enabled() && + fence.option_enabled(AC_Fence::OPTIONS::DISABLE_MODE_CHANGE) && + fence.get_breaches() && + !flightmode->is_landing() && + get_control_mode_reason() == ModeReason::FENCE_BREACHED && + !ap.land_complete) { + mode_change_failed(new_flightmode, "in fence recovery"); + return false; + } +#endif + if (!new_flightmode->init(ignore_checks)) { mode_change_failed(new_flightmode, "init failed"); return false; From e4f76cd27da17911e4d3001e5fe039e09272d66b Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Tue, 16 Jul 2024 18:41:09 +0200 Subject: [PATCH 089/124] Plane: reject guided destinations outside the fence --- ArduPlane/GCS_Mavlink.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 855b9a62413a19..5dcfcdfa72e6c4 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -841,6 +841,14 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_do_reposition(const mavlink_com return MAV_RESULT_DENIED; } +#if AP_FENCE_ENABLED + // reject destination if outside the fence + if (!plane.fence.check_destination_within_fence(requested_position)) { + LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE); + return MAV_RESULT_DENIED; + } +#endif + // location is valid load and set if (((int32_t)packet.param2 & MAV_DO_REPOSITION_FLAGS_CHANGE_MODE) || (plane.control_mode == &plane.mode_guided)) { From 68f259308967a8a72704e4f7f2470538f1a4f858 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 18 Jul 2024 08:00:11 +1000 Subject: [PATCH 090/124] AC_Fence: fixed FENCE_AUTOENABLE=2 needs to auto-enable on takeoff complete --- libraries/AC_Fence/AC_Fence.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp index 808f926fa80cc0..7cfa677c5097ae 100644 --- a/libraries/AC_Fence/AC_Fence.cpp +++ b/libraries/AC_Fence/AC_Fence.cpp @@ -325,7 +325,8 @@ void AC_Fence::auto_disable_fence_on_disarming(void) */ void AC_Fence::auto_enable_fence_after_takeoff(void) { - if (auto_enabled() != AC_Fence::AutoEnable::ENABLE_ON_AUTO_TAKEOFF) { + if (auto_enabled() != AutoEnable::ENABLE_ON_AUTO_TAKEOFF && + auto_enabled() != AutoEnable::ENABLE_DISABLE_FLOOR_ONLY) { return; } From 5d46e0c64e010382c51e1fd5230098d0e540fc69 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 19 Jul 2024 07:33:48 +1000 Subject: [PATCH 091/124] Plane: reset previous mode fence breach when disarmed when we are disarmed or we are not in fence breach and we are not flying then reset the previous mode fence reason state so that a new flight will get the correct fence breach action behaviour --- ArduPlane/fence.cpp | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/ArduPlane/fence.cpp b/ArduPlane/fence.cpp index 5bc3ab93ffa0ba..930a348440eec9 100644 --- a/ArduPlane/fence.cpp +++ b/ArduPlane/fence.cpp @@ -39,10 +39,23 @@ void Plane::fence_check() return; } + const bool armed = arming.is_armed(); + + /* + if we are either disarmed or we are currently not in breach and + we are not flying then clear the state associated with the + previous mode breach handling. This allows the fence state + machine to reset at the end of a fence breach action such as an + RTL and autoland + */ + if (!armed || (new_breaches == 0 && !plane.is_flying())) { + plane.previous_mode_reason = ModeReason::UNKNOWN; + } + // we still don't do anything when disarmed, but we do check for fence breaches. // fence pre-arm check actually checks if any fence has been breached // that's not ever going to be true if we don't call check on AP_Fence while disarmed - if (!arming.is_armed()) { + if (!armed) { return; } From 0b6a1f4fe73b09bc00157c9123681a4a38815c18 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 18 Jul 2024 19:00:19 +0200 Subject: [PATCH 092/124] autotest: test for circle exclusion fence using AUTOENABLE=2 --- Tools/autotest/arduplane.py | 45 +++++++++++++++++++++++++++++++++++++ 1 file changed, 45 insertions(+) diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 6e4122eaf3bd83..6f90634470327e 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -3898,6 +3898,50 @@ def FenceAutoEnableDisableSwitch(self): self.change_altitude(self.homeloc.alt + cruise_alt) self.fly_home_land_and_disarm(timeout=250) + def FenceCircleExclusionAutoEnable(self): + '''Tests autolanding when alt min fence is enabled''' + self.set_parameters({ + "FENCE_TYPE": 2, # Set fence type to circle + "FENCE_ACTION": 1, # Set action to RTL + "FENCE_AUTOENABLE": 2, + "FENCE_ENABLE" : 0, + "RTL_AUTOLAND" : 2, + }) + + # Grab Home Position + self.mav.recv_match(type='HOME_POSITION', blocking=True) + self.homeloc = self.mav.location() + + fence_loc = self.home_position_as_mav_location() + self.location_offset_ne(fence_loc, 300, 0) + + self.upload_fences_from_locations([( + mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION, { + "radius" : 100, + "loc" : fence_loc + } + )]) + + self.wait_ready_to_arm() + self.arm_vehicle() + + self.takeoff(alt=50, mode='TAKEOFF') + self.change_mode("FBWA") + self.set_rc(3, 1100) # lower throttle + + self.progress("Waiting for RTL") + tstart = self.get_sim_time() + mode = "RTL" + while not self.mode_is(mode, drain_mav=False): + self.mav.messages['HEARTBEAT'].custom_mode + self.progress("mav.flightmode=%s Want=%s Alt=%f" % ( + self.mav.flightmode, mode, self.get_altitude(relative=True))) + if (self.get_sim_time_cached() > tstart + 120): + raise WaitModeTimeout("Did not change mode") + self.progress("Got mode %s" % mode) + # Now check we can land + self.fly_home_land_and_disarm() + def FenceEnableDisableSwitch(self): '''Tests enablement and disablement of fences on a switch''' fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE @@ -5822,6 +5866,7 @@ def tests(self): self.FenceMinAltEnableAutoland, self.FenceMinAltAutoEnableAbort, self.FenceAutoEnableDisableSwitch, + Test(self.FenceCircleExclusionAutoEnable, speedup=20), self.FenceEnableDisableSwitch, self.FenceEnableDisableAux, self.FenceBreachedChangeMode, From d845af9bc342ac6ce7c25a9f21c4474c75998ff6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 21 Jul 2024 08:49:28 +1000 Subject: [PATCH 093/124] Plane: fixed re-enable of fence for FENCE_AUTOENABLE=1 the is an adjustment to the previous fix which only worked when we had at least one fence element enabled when we were not flying or disarmed --- ArduPlane/fence.cpp | 28 +++++++++++++++------------- 1 file changed, 15 insertions(+), 13 deletions(-) diff --git a/ArduPlane/fence.cpp b/ArduPlane/fence.cpp index 930a348440eec9..db8bafc496b532 100644 --- a/ArduPlane/fence.cpp +++ b/ArduPlane/fence.cpp @@ -20,6 +20,21 @@ void Plane::fence_check() // check for new breaches; new_breaches is bitmask of fence types breached const uint8_t new_breaches = fence.check(is_in_landing); + const bool armed = arming.is_armed(); + + /* + if we are either disarmed or we are currently not in breach and + we are not flying then clear the state associated with the + previous mode breach handling. This allows the fence state + machine to reset at the end of a fence breach action such as an + RTL and autoland + */ + if (plane.previous_mode_reason == ModeReason::FENCE_BREACHED) { + if (!armed || ((new_breaches == 0 && orig_breaches == 0) && !plane.is_flying())) { + plane.previous_mode_reason = ModeReason::UNKNOWN; + } + } + if (!fence.enabled()) { // Switch back to the chosen control mode if still in // GUIDED to the return point @@ -39,19 +54,6 @@ void Plane::fence_check() return; } - const bool armed = arming.is_armed(); - - /* - if we are either disarmed or we are currently not in breach and - we are not flying then clear the state associated with the - previous mode breach handling. This allows the fence state - machine to reset at the end of a fence breach action such as an - RTL and autoland - */ - if (!armed || (new_breaches == 0 && !plane.is_flying())) { - plane.previous_mode_reason = ModeReason::UNKNOWN; - } - // we still don't do anything when disarmed, but we do check for fence breaches. // fence pre-arm check actually checks if any fence has been breached // that's not ever going to be true if we don't call check on AP_Fence while disarmed From 55075961b2beadf6454660b320c32ad109835414 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sun, 21 Jul 2024 13:29:21 +0200 Subject: [PATCH 094/124] AP_Mission: address minor review comments --- libraries/AP_Mission/AP_Mission.cpp | 1 - libraries/AP_Mission/AP_Mission_Commands.cpp | 6 +++--- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index 438b816176df64..3e138e7cacb403 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -1239,7 +1239,6 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_ case MAV_CMD_DO_FENCE_ENABLE: // MAV ID: 207 cmd.p1 = packet.param1; // action 0=disable, 1=enable, 2=disable floor - // packet.param2; // bitmask see FENCE_TYPE enum break; case MAV_CMD_DO_AUX_FUNCTION: diff --git a/libraries/AP_Mission/AP_Mission_Commands.cpp b/libraries/AP_Mission/AP_Mission_Commands.cpp index f296c0c268c809..bc0374232050d5 100644 --- a/libraries/AP_Mission/AP_Mission_Commands.cpp +++ b/libraries/AP_Mission/AP_Mission_Commands.cpp @@ -357,15 +357,15 @@ bool AP_Mission::start_command_fence(const AP_Mission::Mission_Command& cmd) return false; } - if (cmd.p1 == 0) { // disable fence + if (cmd.p1 == uint8_t(AC_Fence::MavlinkFenceActions::DISABLE_FENCE)) { // disable fence uint8_t fences = fence->enable_configured(false); fence->print_fence_message("disabled", fences); return true; - } else if (cmd.p1 == 1) { // enable fence + } else if (cmd.p1 == uint8_t(AC_Fence::MavlinkFenceActions::ENABLE_FENCE)) { // enable fence uint8_t fences = fence->enable_configured(true); fence->print_fence_message("enabled", fences); return true; - } else if (cmd.p1 == 2) { // disable fence floor only + } else if (cmd.p1 == uint8_t(AC_Fence::MavlinkFenceActions::DISABLE_ALT_MIN_FENCE)) { // disable fence floor only fence->disable_floor(); fence->print_fence_message("disabled", AC_FENCE_TYPE_ALT_MIN); return true; From ccfbfddf7e4c1916f263ea9c715f74c4410f764a Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sun, 21 Jul 2024 13:29:04 +0200 Subject: [PATCH 095/124] AC_Fence: address minor review comments --- ArduCopter/fence.cpp | 2 +- libraries/AC_Fence/AC_Fence.cpp | 6 ++++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/ArduCopter/fence.cpp b/ArduCopter/fence.cpp index 5ad1ccb1844c62..103ccc556ae72c 100644 --- a/ArduCopter/fence.cpp +++ b/ArduCopter/fence.cpp @@ -84,7 +84,7 @@ void Copter::fence_check() LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode(new_breaches)); } else if (orig_breaches && fence.get_breaches() == 0) { - if (!copter.ap.land_complete) { + if (!copter.ap.land_complete) { GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Fence breach cleared"); } // record clearing of breach diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp index 7cfa677c5097ae..743877cc1e7f83 100644 --- a/libraries/AC_Fence/AC_Fence.cpp +++ b/libraries/AC_Fence/AC_Fence.cpp @@ -144,7 +144,7 @@ const AP_Param::GroupInfo AC_Fence::var_info[] = { // @Description: When bit 0 is set sisable mode change following fence action until fence breach is cleared. When bit 1 is set the allowable flight areas is the union of all polygon and circle fence areas instead of the intersection, which means a fence breach occurs only if you are outside all of the fence areas. // @Bitmask: 0:Disable mode change following fence action until fence breach is cleared, 1:Allow union of inclusion areas // @User: Standard - AP_GROUPINFO_FRAME("OPTIONS", 11, AC_Fence, _options, static_cast(AC_FENCE_OPTIONS_DEFAULT), AP_PARAM_FRAME_PLANE | AP_PARAM_FRAME_COPTER), + AP_GROUPINFO_FRAME("OPTIONS", 11, AC_Fence, _options, static_cast(AC_FENCE_OPTIONS_DEFAULT), AP_PARAM_FRAME_PLANE | AP_PARAM_FRAME_COPTER | AP_PARAM_FRAME_TRICOPTER | AP_PARAM_FRAME_HELI), AP_GROUPEND }; @@ -237,7 +237,9 @@ void AC_Fence::update() #endif } -/// enable the Fence code generally; a master switch for all fences +// enable or disable configured fences present in fence_types +// also updates the bitmask of auto enabled fences if update_auto_mask is true +// returns a bitmask of fences that were changed uint8_t AC_Fence::enable(bool value, uint8_t fence_types, bool update_auto_mask) { uint8_t fences = _configured_fences.get() & fence_types; From 51304848fcc42e97670090c8c0d964cc7e8dcde4 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sun, 21 Jul 2024 16:54:12 +0200 Subject: [PATCH 096/124] Copter: address minor review comments --- ArduCopter/fence.cpp | 4 ++-- ArduCopter/mode.cpp | 1 + 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/ArduCopter/fence.cpp b/ArduCopter/fence.cpp index 103ccc556ae72c..2685f89431be60 100644 --- a/ArduCopter/fence.cpp +++ b/ArduCopter/fence.cpp @@ -10,10 +10,10 @@ void Copter::fence_check() { const uint8_t orig_breaches = fence.get_breaches(); - bool is_in_landing = flightmode->is_landing() || ap.land_complete || !motors->armed(); + bool is_landing_or_landed = flightmode->is_landing() || ap.land_complete || !motors->armed(); // check for new breaches; new_breaches is bitmask of fence types breached - const uint8_t new_breaches = fence.check(is_in_landing); + const uint8_t new_breaches = fence.check(is_landing_or_landed); // we still don't do anything when disarmed, but we do check for fence breaches. // fence pre-arm check actually checks if any fence has been breached diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 97bac5da02ffd1..89bfbc3235f3a0 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -354,6 +354,7 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason) fence.option_enabled(AC_Fence::OPTIONS::DISABLE_MODE_CHANGE) && fence.get_breaches() && !flightmode->is_landing() && + motors->armed() && get_control_mode_reason() == ModeReason::FENCE_BREACHED && !ap.land_complete) { mode_change_failed(new_flightmode, "in fence recovery"); From f8e5c7c1c48d592fbfa94cf6d5e70837671c81d4 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sun, 21 Jul 2024 16:54:23 +0200 Subject: [PATCH 097/124] Plane: address minor review comments --- ArduPlane/fence.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ArduPlane/fence.cpp b/ArduPlane/fence.cpp index db8bafc496b532..1890e709d25b0c 100644 --- a/ArduPlane/fence.cpp +++ b/ArduPlane/fence.cpp @@ -8,9 +8,11 @@ void Plane::fence_check() { const uint8_t orig_breaches = fence.get_breaches(); + const bool armed = arming.is_armed(); uint16_t mission_id = plane.mission.get_current_nav_cmd().id; - bool is_in_landing = plane.flight_stage == AP_FixedWing::FlightStage::LAND + bool landing_or_landed = plane.flight_stage == AP_FixedWing::FlightStage::LAND + || !armed #if HAL_QUADPLANE_ENABLED || control_mode->mode_number() == Mode::Number::QLAND || quadplane.in_vtol_land_descent() @@ -18,9 +20,7 @@ void Plane::fence_check() || (plane.is_land_command(mission_id) && plane.mission.state() == AP_Mission::MISSION_RUNNING); // check for new breaches; new_breaches is bitmask of fence types breached - const uint8_t new_breaches = fence.check(is_in_landing); - - const bool armed = arming.is_armed(); + const uint8_t new_breaches = fence.check(landing_or_landed); /* if we are either disarmed or we are currently not in breach and From a371a3eb894666934de5d7d3669a869f300b26da Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 23 Jul 2024 09:59:50 +1000 Subject: [PATCH 098/124] Copter: fully honour FENCE_OPTION to disable mode changes when the user has chosen to disallow mode change during fence breach it should be fully implemented, without a landing exception. as requested by Pete, and discussed on dev call --- ArduCopter/mode.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 89bfbc3235f3a0..19de3725dbbeba 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -353,7 +353,6 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason) fence.enabled() && fence.option_enabled(AC_Fence::OPTIONS::DISABLE_MODE_CHANGE) && fence.get_breaches() && - !flightmode->is_landing() && motors->armed() && get_control_mode_reason() == ModeReason::FENCE_BREACHED && !ap.land_complete) { From 1afa801b196c21a36761d18e8a7274a3653af4d6 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 23 Jul 2024 20:51:07 +1000 Subject: [PATCH 099/124] AP_LTM_Telem: correct compilation with AP_RRSI_ENABLED false Co-authored-by: David Buzz --- libraries/AP_LTM_Telem/AP_LTM_Telem.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/libraries/AP_LTM_Telem/AP_LTM_Telem.cpp b/libraries/AP_LTM_Telem/AP_LTM_Telem.cpp index 5d3bdc2f41558a..ebc44a3937d554 100644 --- a/libraries/AP_LTM_Telem/AP_LTM_Telem.cpp +++ b/libraries/AP_LTM_Telem/AP_LTM_Telem.cpp @@ -142,10 +142,12 @@ void AP_LTM_Telem::send_Sframe(void) const uint8_t flightmode = AP_Notify::flags.flight_mode; // flight mode uint8_t rssi = 0; // radio RSSI (%a) +#if AP_RSSI_ENABLED AP_RSSI *ap_rssi = AP_RSSI::get_singleton(); if (ap_rssi) { rssi = ap_rssi->read_receiver_rssi_uint8(); } +#endif const uint8_t armstat = AP_Notify::flags.armed; // 0: disarmed, 1: armed const uint8_t failsafe = AP_Notify::flags.failsafe_radio; // 0: normal, 1: failsafe From cc190f537f937e615521efa91658ad13817c4fec Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 23 Jul 2024 20:51:07 +1000 Subject: [PATCH 100/124] AP_OSD: correct compilation with AP_RRSI_ENABLED false Co-authored-by: David Buzz --- libraries/AP_OSD/AP_OSD_Screen.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/libraries/AP_OSD/AP_OSD_Screen.cpp b/libraries/AP_OSD/AP_OSD_Screen.cpp index 0c1aa5bfe80e24..e95363f1febc8c 100644 --- a/libraries/AP_OSD/AP_OSD_Screen.cpp +++ b/libraries/AP_OSD/AP_OSD_Screen.cpp @@ -2584,8 +2584,10 @@ void AP_OSD_Screen::draw(void) DRAW_SETTING(avgcellvolt); DRAW_SETTING(avgcellrestvolt); DRAW_SETTING(restvolt); +#if AP_RSSI_ENABLED DRAW_SETTING(rssi); DRAW_SETTING(link_quality); +#endif DRAW_SETTING(current); DRAW_SETTING(batused); DRAW_SETTING(bat2used); From 6dac230ccb6548c07ee5f31ad5c463138e14cb0d Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 23 Jul 2024 20:51:07 +1000 Subject: [PATCH 101/124] ArduCopter: correct compilation with AP_RRSI_ENABLED false Co-authored-by: David Buzz --- ArduCopter/Copter.cpp | 2 ++ ArduCopter/Parameters.cpp | 4 +++- ArduCopter/system.cpp | 4 +++- 3 files changed, 8 insertions(+), 2 deletions(-) diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index b02628b1bd64a7..86fc3b0e61a782 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -583,9 +583,11 @@ void Copter::ten_hz_logging_loop() } if (should_log(MASK_LOG_RCIN)) { logger.Write_RCIN(); +#if AP_RSSI_ENABLED if (rssi.enabled()) { logger.Write_RSSI(); } +#endif } if (should_log(MASK_LOG_RCOUT)) { logger.Write_RCOUT(); diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index a1dd234b0cf36f..105b660a8e94ef 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -634,10 +634,12 @@ const AP_Param::Info Copter::var_info[] = { GOBJECTN(mode_auto.mission, mission, "MIS_", AP_Mission), #endif +#if AP_RSSI_ENABLED // @Group: RSSI_ // @Path: ../libraries/AP_RSSI/AP_RSSI.cpp GOBJECT(rssi, "RSSI_", AP_RSSI), - +#endif + #if AP_RANGEFINDER_ENABLED // @Group: RNGFND // @Path: ../libraries/AP_RangeFinder/AP_RangeFinder.cpp diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 864b1569f8bb00..1d133932fa40a3 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -27,9 +27,11 @@ void Copter::init_ardupilot() // initialise battery monitor battery.init(); +#if AP_RSSI_ENABLED // Init RSSI rssi.init(); - +#endif + barometer.init(); // setup telem slots with serial ports From c451518bdd470ef7664874a93bcdc9f439481637 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 23 Jul 2024 20:51:07 +1000 Subject: [PATCH 102/124] ArduPlane: correct compilation with AP_RRSI_ENABLED false Co-authored-by: David Buzz --- ArduPlane/Log.cpp | 2 ++ ArduPlane/Parameters.cpp | 2 ++ ArduPlane/system.cpp | 2 ++ 3 files changed, 6 insertions(+) diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index a73c6dc552ce4a..2fdfcd618bba18 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -255,9 +255,11 @@ void Plane::Log_Write_RC(void) { logger.Write_RCIN(); logger.Write_RCOUT(); +#if AP_RSSI_ENABLED if (rssi.enabled()) { logger.Write_RSSI(); } +#endif Log_Write_AETR(); } diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 19288615a62054..65198d9cb53240 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -975,9 +975,11 @@ const AP_Param::Info Plane::var_info[] = { GOBJECT(rpm_sensor, "RPM", AP_RPM), #endif +#if AP_RSSI_ENABLED // @Group: RSSI_ // @Path: ../libraries/AP_RSSI/AP_RSSI.cpp GOBJECT(rssi, "RSSI_", AP_RSSI), +#endif // @Group: NTF_ // @Path: ../libraries/AP_Notify/AP_Notify.cpp diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index d931ac158d62fa..dafe5e8bd7e1c7 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -45,7 +45,9 @@ void Plane::init_ardupilot() // initialise battery monitoring battery.init(); +#if AP_RSSI_ENABLED rssi.init(); +#endif #if AP_RPM_ENABLED rpm_sensor.init(); From 3e54ecfecec70b5db4e7c1624a3320593e6920d7 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 23 Jul 2024 20:51:07 +1000 Subject: [PATCH 103/124] Blimp: correct compilation with AP_RRSI_ENABLED false Co-authored-by: David Buzz --- Blimp/Blimp.cpp | 2 ++ Blimp/Parameters.cpp | 2 ++ Blimp/system.cpp | 2 ++ 3 files changed, 6 insertions(+) diff --git a/Blimp/Blimp.cpp b/Blimp/Blimp.cpp index 1273636c2662cf..83f8e6207394b0 100644 --- a/Blimp/Blimp.cpp +++ b/Blimp/Blimp.cpp @@ -165,9 +165,11 @@ void Blimp::ten_hz_logging_loop() } if (should_log(MASK_LOG_RCIN)) { logger.Write_RCIN(); +#if AP_RSSI_ENABLED if (rssi.enabled()) { logger.Write_RSSI(); } +#endif } if (should_log(MASK_LOG_RCOUT)) { logger.Write_RCOUT(); diff --git a/Blimp/Parameters.cpp b/Blimp/Parameters.cpp index 91a1cc017ed0aa..7a1d34c239f36b 100644 --- a/Blimp/Parameters.cpp +++ b/Blimp/Parameters.cpp @@ -382,9 +382,11 @@ const AP_Param::Info Blimp::var_info[] = { GOBJECTN(ahrs.EKF3, NavEKF3, "EK3_", NavEKF3), #endif +#if AP_RSSI_ENABLED // @Group: RSSI_ // @Path: ../libraries/AP_RSSI/AP_RSSI.cpp GOBJECT(rssi, "RSSI_", AP_RSSI), +#endif // @Group: NTF_ // @Path: ../libraries/AP_Notify/AP_Notify.cpp diff --git a/Blimp/system.cpp b/Blimp/system.cpp index e32fa10a04c3d8..9c1df237c82659 100644 --- a/Blimp/system.cpp +++ b/Blimp/system.cpp @@ -21,8 +21,10 @@ void Blimp::init_ardupilot() // initialise battery monitor battery.init(); +#if AP_RSSI_ENABLED // Init RSSI rssi.init(); +#endif barometer.init(); From 3def5f743373b5a4bab123303d21ed3dc1c0d3e5 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 23 Jul 2024 20:51:07 +1000 Subject: [PATCH 104/124] Rover: correct compilation with AP_RRSI_ENABLED false Co-authored-by: David Buzz --- Rover/GCS_Mavlink.cpp | 7 ++++++- Rover/Log.cpp | 2 ++ Rover/Parameters.cpp | 2 ++ Rover/system.cpp | 2 ++ 4 files changed, 12 insertions(+), 1 deletion(-) diff --git a/Rover/GCS_Mavlink.cpp b/Rover/GCS_Mavlink.cpp index 6189b2d281ef9c..e718273703ce62 100644 --- a/Rover/GCS_Mavlink.cpp +++ b/Rover/GCS_Mavlink.cpp @@ -142,7 +142,12 @@ void GCS_MAVLINK_Rover::send_servo_out() 0, 0, 0, - receiver_rssi()); +#if AP_RSSI_ENABLED + receiver_rssi() +#else + 255 +#endif + ); } int16_t GCS_MAVLINK_Rover::vfr_hud_throttle() const diff --git a/Rover/Log.cpp b/Rover/Log.cpp index 6a3edb0184767e..3490e2a703d33d 100644 --- a/Rover/Log.cpp +++ b/Rover/Log.cpp @@ -237,9 +237,11 @@ void Rover::Log_Write_RC(void) { logger.Write_RCIN(); logger.Write_RCOUT(); +#if AP_RSSI_ENABLED if (rssi.enabled()) { logger.Write_RSSI(); } +#endif } void Rover::Log_Write_Vehicle_Startup_Messages() diff --git a/Rover/Parameters.cpp b/Rover/Parameters.cpp index 49c1f9a0be6378..0c740105e5c4f5 100644 --- a/Rover/Parameters.cpp +++ b/Rover/Parameters.cpp @@ -358,9 +358,11 @@ const AP_Param::Info Rover::var_info[] = { // @Path: ../libraries/AP_Mission/AP_Mission.cpp GOBJECTN(mode_auto.mission, mission, "MIS_", AP_Mission), +#if AP_RSSI_ENABLED // @Group: RSSI_ // @Path: ../libraries/AP_RSSI/AP_RSSI.cpp GOBJECT(rssi, "RSSI_", AP_RSSI), +#endif // @Group: NTF_ // @Path: ../libraries/AP_Notify/AP_Notify.cpp diff --git a/Rover/system.cpp b/Rover/system.cpp index 451df4614e0923..02b93ab644f86c 100644 --- a/Rover/system.cpp +++ b/Rover/system.cpp @@ -18,7 +18,9 @@ void Rover::init_ardupilot() rpm_sensor.init(); #endif +#if AP_RSSI_ENABLED rssi.init(); +#endif g2.windvane.init(serial_manager); From 7f1b7182fbbd60a2f19b1a24c36f700cd3d3d6d1 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 23 Jul 2024 21:54:46 +1000 Subject: [PATCH 105/124] Tools: add AP_RSSI_ENABLED to build_options.py --- Tools/scripts/build_options.py | 1 + Tools/scripts/extract_features.py | 1 + 2 files changed, 2 insertions(+) diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index 1246995e8a9598..bb19e6b4c69bf2 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -213,6 +213,7 @@ def __init__(self, Feature('RC', 'RC_SUMD', 'AP_RCPROTOCOL_SUMD_ENABLED', "Enable SUMD RC Protocol", 0, "RC_Protocol"), # NOQA: E501 Feature('RC', 'RC_GHST', 'AP_RCPROTOCOL_GHST_ENABLED', "Enable Ghost RC Protocol", 0, "RC_Protocol"), # NOQA: E501 Feature('RC', 'RC_MAVLINK_RADIO', 'AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED', "Enable MAVLink RC Protocol", 0, "RC_Protocol"), # NOQA: E501 + Feature('RC', 'RSSI', 'AP_RSSI_ENABLED', 'Enable RSSI handling library', 0, None), Feature('Rangefinder', 'RANGEFINDER', 'AP_RANGEFINDER_ENABLED', "Enable Rangefinders", 0, None), # NOQA: E501 Feature('Rangefinder', 'RANGEFINDER_ANALOG', 'AP_RANGEFINDER_ANALOG_ENABLED', "Enable Rangefinder - Analog", 0, "RANGEFINDER"), # NOQA: E501 diff --git a/Tools/scripts/extract_features.py b/Tools/scripts/extract_features.py index bf21800c19e745..c1f3aa86b6cb5f 100755 --- a/Tools/scripts/extract_features.py +++ b/Tools/scripts/extract_features.py @@ -257,6 +257,7 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"): ('HAL_ENABLE_DRONECAN_DRIVERS', r'AP_DroneCAN::init'), ('AP_MAVLINK_MSG_HIL_GPS_ENABLED', r'mavlink_msg_hil_gps_decode'), ('AP_BARO_PROBE_EXTERNAL_I2C_BUSES', r'AP_Compass::_probe_external_i2c_compasses'), + ('AP_RSSI_ENABLED', r'AP_RSSI::init'), ] def progress(self, msg): From a4577685a77148676be03783606f0926f6a35ba7 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 23 Jul 2024 22:18:41 +1000 Subject: [PATCH 106/124] AP_HAL_ChibiOS: rename LED_1 pin define to AP_NOTIFY_GPIO_LED_1_PIN --- libraries/AP_HAL_ChibiOS/hwdef/BETAFPV-F405/hwdef.dat | 2 +- libraries/AP_HAL_ChibiOS/hwdef/BlitzF745/hwdef.dat | 2 +- libraries/AP_HAL_ChibiOS/hwdef/CBU-H7-Stamp/hwdef.dat | 2 +- libraries/AP_HAL_ChibiOS/hwdef/F35Lightning/hwdef.dat | 2 +- libraries/AP_HAL_ChibiOS/hwdef/FlywooF405Pro/hwdef.dat | 2 +- libraries/AP_HAL_ChibiOS/hwdef/FlywooF405S-AIO/hwdef.dat | 2 +- libraries/AP_HAL_ChibiOS/hwdef/FoxeerH743v1/hwdef.dat | 2 +- libraries/AP_HAL_ChibiOS/hwdef/JHEM_JHEF405/hwdef.dat | 2 +- libraries/AP_HAL_ChibiOS/hwdef/OmnibusNanoV6/hwdef.dat | 2 +- libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405Mini/hwdef.dat | 2 +- libraries/AP_HAL_ChibiOS/hwdef/SuccexF4/hwdef.dat | 2 +- libraries/AP_HAL_ChibiOS/hwdef/airbotf4/hwdef.dat | 2 +- libraries/AP_HAL_ChibiOS/hwdef/omnibusf4v6/hwdef.dat | 2 +- libraries/AP_HAL_ChibiOS/hwdef/speedybeef4v3/hwdef.dat | 2 +- libraries/AP_HAL_ChibiOS/hwdef/speedybeef4v4/hwdef.dat | 2 +- 15 files changed, 15 insertions(+), 15 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/BETAFPV-F405/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/BETAFPV-F405/hwdef.dat index 67443a0e65c390..8e451e2bed4fcf 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/BETAFPV-F405/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/BETAFPV-F405/hwdef.dat @@ -101,7 +101,7 @@ PA2 TIM2_CH3 TIM2 PWM(4) GPIO(53) # M4 # LEDs define AP_NOTIFY_GPIO_LED_1_ENABLED 1 PB5 LED0 OUTPUT LOW GPIO(90) -define HAL_GPIO_A_LED_PIN 90 +define AP_NOTIFY_GPIO_LED_1_PIN 90 # LED strip PB6 TIM4_CH1 TIM4 PWM(5) GPIO(56) # M7 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/BlitzF745/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/BlitzF745/hwdef.dat index 2d5fa82e437fcd..f4f12ed84fd984 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/BlitzF745/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/BlitzF745/hwdef.dat @@ -124,7 +124,7 @@ PC9 TIM8_CH4 TIM8 PWM(9) GPIO(58) # M9 define AP_NOTIFY_GPIO_LED_1_ENABLED 1 PD15 LED0 OUTPUT LOW GPIO(90) -define HAL_GPIO_A_LED_PIN 90 +define AP_NOTIFY_GPIO_LED_1_PIN 90 # Dataflash setup SPIDEV dataflash SPI3 DEVID1 FLASH1_CS MODE3 104*MHZ 104*MHZ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CBU-H7-Stamp/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CBU-H7-Stamp/hwdef.dat index 0131fd18e1868b..474e761991cb7b 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CBU-H7-Stamp/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CBU-H7-Stamp/hwdef.dat @@ -189,7 +189,7 @@ PF5 SAFETY_IN INPUT PULLDOWN PE3 LED_RED OUTPUT HIGH GPIO(90) define AP_NOTIFY_GPIO_LED_1_ENABLED 1 -define HAL_GPIO_A_LED_PIN 90 +define AP_NOTIFY_GPIO_LED_1_PIN 90 define HAL_GPIO_LED_ON 1 # barometers diff --git a/libraries/AP_HAL_ChibiOS/hwdef/F35Lightning/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/F35Lightning/hwdef.dat index a6141f55d57b99..3b2655f8123ff7 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/F35Lightning/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/F35Lightning/hwdef.dat @@ -72,7 +72,7 @@ define BOARD_RSSI_ANA_PIN 13 # LED define AP_NOTIFY_GPIO_LED_1_ENABLED 1 PC10 LED OUTPUT HIGH GPIO(57) -define HAL_GPIO_A_LED_PIN 57 +define AP_NOTIFY_GPIO_LED_1_PIN 57 # SPI PA5 SPI1_SCK SPI1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/FlywooF405Pro/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/FlywooF405Pro/hwdef.dat index a403ff33727a9a..d3cd91aeb33f12 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/FlywooF405Pro/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/FlywooF405Pro/hwdef.dat @@ -120,7 +120,7 @@ PA9 TIM1_CH2 TIM1 PWM(9) GPIO(58) # M9 define AP_NOTIFY_GPIO_LED_1_ENABLED 1 PC14 LED0 OUTPUT LOW GPIO(90) -define HAL_GPIO_A_LED_PIN 90 +define AP_NOTIFY_GPIO_LED_1_PIN 90 # Dataflash setup SPIDEV dataflash SPI3 DEVID1 FLASH1_CS MODE3 104*MHZ 104*MHZ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/FlywooF405S-AIO/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/FlywooF405S-AIO/hwdef.dat index 93ae7491d42edb..6c06ea552a8ea5 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/FlywooF405S-AIO/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/FlywooF405S-AIO/hwdef.dat @@ -108,7 +108,7 @@ PA9 TIM1_CH2 TIM1 PWM(5) GPIO(54) # M5 define AP_NOTIFY_GPIO_LED_1_ENABLED 1 PC14 LED0 OUTPUT LOW GPIO(90) -define HAL_GPIO_A_LED_PIN 90 +define AP_NOTIFY_GPIO_LED_1_PIN 90 # Dataflash setup SPIDEV dataflash SPI3 DEVID1 FLASH1_CS MODE3 104*MHZ 104*MHZ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/FoxeerH743v1/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/FoxeerH743v1/hwdef.dat index 075a3f8cd537a1..97e2b86c68c142 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/FoxeerH743v1/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/FoxeerH743v1/hwdef.dat @@ -138,7 +138,7 @@ PA8 TIM1_CH1 TIM1 PWM(9) GPIO(58) # M9 define AP_NOTIFY_GPIO_LED_1_ENABLED 1 PC13 LED0 OUTPUT LOW GPIO(90) -define HAL_GPIO_A_LED_PIN 90 +define AP_NOTIFY_GPIO_LED_1_PIN 90 # Dataflash setup SPIDEV dataflash SPI3 DEVID1 FLASH1_CS MODE3 104*MHZ 104*MHZ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/JHEM_JHEF405/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/JHEM_JHEF405/hwdef.dat index d1dde0be965000..e55d72117bb2e1 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/JHEM_JHEF405/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/JHEM_JHEF405/hwdef.dat @@ -110,7 +110,7 @@ PA9 TIM1_CH2 TIM1 PWM(9) GPIO(58) # LED define AP_NOTIFY_GPIO_LED_1_ENABLED 1 PC14 LED0 OUTPUT LOW GPIO(90) -define HAL_GPIO_A_LED_PIN 90 +define AP_NOTIFY_GPIO_LED_1_PIN 90 # Dataflash setup SPIDEV dataflash SPI3 DEVID1 FLASH1_CS MODE3 104*MHZ 104*MHZ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/OmnibusNanoV6/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/OmnibusNanoV6/hwdef.dat index 1446c3fe344647..1e068c119b4e56 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/OmnibusNanoV6/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/OmnibusNanoV6/hwdef.dat @@ -139,7 +139,7 @@ define BOARD_RSSI_ANA_PIN 0 define AP_NOTIFY_GPIO_LED_1_ENABLED 1 -define HAL_GPIO_A_LED_PIN 41 +define AP_NOTIFY_GPIO_LED_1_PIN 41 define OSD_ENABLED 1 define HAL_OSD_TYPE_DEFAULT 1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405Mini/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405Mini/hwdef.dat index 8c5885570b81dd..292c0777d674aa 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405Mini/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405Mini/hwdef.dat @@ -115,7 +115,7 @@ PA8 TIM1_CH1 TIM1 PWM(5) GPIO(54) # M5 define AP_NOTIFY_GPIO_LED_1_ENABLED 1 PC13 LED0 OUTPUT LOW GPIO(90) -define HAL_GPIO_A_LED_PIN 90 +define AP_NOTIFY_GPIO_LED_1_PIN 90 # Dataflash setup SPIDEV dataflash SPI3 DEVID1 FLASH1_CS MODE3 104*MHZ 104*MHZ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/SuccexF4/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/SuccexF4/hwdef.dat index b8bf2579594222..75965031cbb94e 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/SuccexF4/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/SuccexF4/hwdef.dat @@ -77,7 +77,7 @@ PA14 JTCK-SWCLK SWD PB4 TIM3_CH1 TIM3 GPIO(56) ALARM define AP_NOTIFY_GPIO_LED_1_ENABLED 1 PB5 LED OUTPUT HIGH GPIO(57) -define HAL_GPIO_A_LED_PIN 57 +define AP_NOTIFY_GPIO_LED_1_PIN 57 PA11 OTG_FS_DM OTG1 PA12 OTG_FS_DP OTG1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/airbotf4/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/airbotf4/hwdef.dat index 347b49385328b6..e1c7ddacac9e31 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/airbotf4/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/airbotf4/hwdef.dat @@ -99,7 +99,7 @@ define HAL_BATT_VOLT_SCALE 11 define HAL_BATT_CURR_SCALE 18.2 define AP_NOTIFY_GPIO_LED_1_ENABLED 1 -define HAL_GPIO_A_LED_PIN 57 +define AP_NOTIFY_GPIO_LED_1_PIN 57 # 64kB FLASH_RESERVE_START_KB means we're lacking a lot of space: include ../include/minimize_features.inc diff --git a/libraries/AP_HAL_ChibiOS/hwdef/omnibusf4v6/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/omnibusf4v6/hwdef.dat index d695fb7b8ab26f..946c15584de5d3 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/omnibusf4v6/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/omnibusf4v6/hwdef.dat @@ -143,7 +143,7 @@ define HAL_BATT_CURR_SCALE 18.2 define BOARD_RSSI_ANA_PIN 0 define AP_NOTIFY_GPIO_LED_1_ENABLED 1 -define HAL_GPIO_A_LED_PIN 41 +define AP_NOTIFY_GPIO_LED_1_PIN 41 define OSD_ENABLED 1 #font for the osd diff --git a/libraries/AP_HAL_ChibiOS/hwdef/speedybeef4v3/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/speedybeef4v3/hwdef.dat index 478a827bb62a8c..752745b3922479 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/speedybeef4v3/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/speedybeef4v3/hwdef.dat @@ -25,7 +25,7 @@ SERIAL_ORDER OTG1 USART1 USART2 USART3 UART4 UART5 USART6 PC8 LED_BLUE OUTPUT LOW GPIO(0) define AP_NOTIFY_GPIO_LED_1_ENABLED 1 -define HAL_GPIO_A_LED_PIN 0 +define AP_NOTIFY_GPIO_LED_1_PIN 0 # buzzer PC5 BUZZER OUTPUT GPIO(80) LOW diff --git a/libraries/AP_HAL_ChibiOS/hwdef/speedybeef4v4/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/speedybeef4v4/hwdef.dat index 15de93a140a2e1..80e58d65d90f08 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/speedybeef4v4/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/speedybeef4v4/hwdef.dat @@ -123,7 +123,7 @@ PA8 TIM1_CH1 TIM1 PWM(9) GPIO(58) # M9 define AP_NOTIFY_GPIO_LED_1_ENABLED 1 PC13 LED0 OUTPUT LOW GPIO(90) -define HAL_GPIO_A_LED_PIN 90 +define AP_NOTIFY_GPIO_LED_1_PIN 90 # OSD setup SPIDEV osd SPI2 DEVID1 OSD1_CS MODE0 10*MHZ 10*MHZ From 478d26d69c19a2ef30593e191857f8efec2f02c8 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 23 Jul 2024 22:18:41 +1000 Subject: [PATCH 107/124] AP_Notify: rename LED_1 pin define to AP_NOTIFY_GPIO_LED_1_PIN --- libraries/AP_Notify/GPIO_LED_1.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/libraries/AP_Notify/GPIO_LED_1.cpp b/libraries/AP_Notify/GPIO_LED_1.cpp index 950a1ce49cad2f..6ea0bd16c9aa0d 100644 --- a/libraries/AP_Notify/GPIO_LED_1.cpp +++ b/libraries/AP_Notify/GPIO_LED_1.cpp @@ -22,8 +22,8 @@ #include #include "AP_Notify.h" -#ifndef AP_NOTIFY_GPIO_LED_A_PIN -#error "define AP_NOTIFY_GPIO_LED_A_PIN" +#ifndef AP_NOTIFY_GPIO_LED_1_PIN +#error "define AP_NOTIFY_GPIO_LED_1_PIN" #endif extern const AP_HAL::HAL& hal; @@ -33,9 +33,9 @@ bool GPIO_LED_1::init(void) // when HAL_GPIO_LED_ON is 0 then we must not use pinMode() // as it could remove the OPENDRAIN attribute on the pin #if HAL_GPIO_LED_ON != 0 - hal.gpio->pinMode(AP_NOTIFY_GPIO_LED_A_PIN, HAL_GPIO_OUTPUT); + hal.gpio->pinMode(AP_NOTIFY_GPIO_LED_1_PIN, HAL_GPIO_OUTPUT); #endif - hal.gpio->write(AP_NOTIFY_GPIO_LED_A_PIN, HAL_GPIO_LED_OFF); + hal.gpio->write(AP_NOTIFY_GPIO_LED_1_PIN, HAL_GPIO_LED_OFF); return true; } @@ -67,7 +67,7 @@ void GPIO_LED_1::update(void) last_timestep_ms = now_ms; const auto new_state = (current_pattern & (1U<write(AP_NOTIFY_GPIO_LED_A_PIN, new_state); + hal.gpio->write(AP_NOTIFY_GPIO_LED_1_PIN, new_state); next_bit++; if (next_bit > 31) { next_bit = 0; From bedc76e0e80e407efdb40232857b40d1d16bc0a4 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 23 Jul 2024 10:18:22 +1000 Subject: [PATCH 108/124] autotest: tidy various tests to take account of new framework capabilities autotest: simplify AutoDock test autotest: tidy AP_Proximity_MAV test autotest: tidy DriveSquare test autotest: tidy BatteryFailsafe test autotest: tidy GPSViconSwitching test autotest: tidy RangeFinder test --- Tools/autotest/arducopter.py | 227 +++++++++++---------------- Tools/autotest/rover.py | 293 +++++++++++++++-------------------- 2 files changed, 222 insertions(+), 298 deletions(-) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index f41d11fbe736b8..17b500367509fc 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -961,21 +961,6 @@ def CustomController(self, timeout=300): # Tests all actions and logic behind the battery failsafe def BatteryFailsafe(self, timeout=300): '''Fly Battery Failsafe''' - self.context_push() - ex = None - try: - self.test_battery_failsafe(timeout=timeout) - except Exception as e: - self.print_exception_caught(e) - self.disarm_vehicle(force=True) - ex = e - self.context_pop() - self.reboot_sitl() - - if ex is not None: - raise ex - - def test_battery_failsafe(self, timeout=300): self.progress("Configure battery failsafe parameters") self.set_parameters({ 'SIM_SPEEDUP': 4, @@ -3730,82 +3715,68 @@ def InvalidJumpTags(self): def GPSViconSwitching(self): """Fly GPS and Vicon switching test""" - self.customise_SITL_commandline(["--serial5=sim:vicon:"]) - """Setup parameters including switching to EKF3""" - self.context_push() - ex = None - try: - self.set_parameters({ - "VISO_TYPE": 2, # enable vicon - "SERIAL5_PROTOCOL": 2, - "EK3_ENABLE": 1, - "EK3_SRC2_POSXY": 6, # External Nav - "EK3_SRC2_POSZ": 6, # External Nav - "EK3_SRC2_VELXY": 6, # External Nav - "EK3_SRC2_VELZ": 6, # External Nav - "EK3_SRC2_YAW": 6, # External Nav - "RC7_OPTION": 80, # RC aux switch 7 set to Viso Align - "RC8_OPTION": 90, # RC aux switch 8 set to EKF source selector - "EK2_ENABLE": 0, - "AHRS_EKF_TYPE": 3, - }) - self.reboot_sitl() - - # switch to use GPS - self.set_rc(8, 1000) + self.set_parameters({ + "VISO_TYPE": 2, # enable vicon + "SERIAL5_PROTOCOL": 2, + "EK3_ENABLE": 1, + "EK3_SRC2_POSXY": 6, # External Nav + "EK3_SRC2_POSZ": 6, # External Nav + "EK3_SRC2_VELXY": 6, # External Nav + "EK3_SRC2_VELZ": 6, # External Nav + "EK3_SRC2_YAW": 6, # External Nav + "RC7_OPTION": 80, # RC aux switch 7 set to Viso Align + "RC8_OPTION": 90, # RC aux switch 8 set to EKF source selector + "EK2_ENABLE": 0, + "AHRS_EKF_TYPE": 3, + }) + self.customise_SITL_commandline(["--serial5=sim:vicon:"]) - # ensure we can get a global position: - self.poll_home_position(timeout=120) + # switch to use GPS + self.set_rc(8, 1000) - # record starting position - old_pos = self.get_global_position_int() - print("old_pos=%s" % str(old_pos)) + # ensure we can get a global position: + self.poll_home_position(timeout=120) - # align vicon yaw with ahrs heading - self.set_rc(7, 2000) + # record starting position + old_pos = self.get_global_position_int() + print("old_pos=%s" % str(old_pos)) - # takeoff to 10m in Loiter - self.progress("Moving to ensure location is tracked") - self.takeoff(10, mode="LOITER", require_absolute=True, timeout=720) + # align vicon yaw with ahrs heading + self.set_rc(7, 2000) - # fly forward in Loiter - self.set_rc(2, 1300) + # takeoff to 10m in Loiter + self.progress("Moving to ensure location is tracked") + self.takeoff(10, mode="LOITER", require_absolute=True, timeout=720) - # disable vicon - self.set_parameter("SIM_VICON_FAIL", 1) + # fly forward in Loiter + self.set_rc(2, 1300) - # ensure vehicle remain in Loiter for 15 seconds - tstart = self.get_sim_time() - while self.get_sim_time() - tstart < 15: - if not self.mode_is('LOITER'): - raise NotAchievedException("Expected to stay in loiter for >15 seconds") + # disable vicon + self.set_parameter("SIM_VICON_FAIL", 1) - # re-enable vicon - self.set_parameter("SIM_VICON_FAIL", 0) + # ensure vehicle remain in Loiter for 15 seconds + tstart = self.get_sim_time() + while self.get_sim_time() - tstart < 15: + if not self.mode_is('LOITER'): + raise NotAchievedException("Expected to stay in loiter for >15 seconds") - # switch to vicon, disable GPS and wait 10sec to ensure vehicle remains in Loiter - self.set_rc(8, 1500) - self.set_parameter("GPS1_TYPE", 0) + # re-enable vicon + self.set_parameter("SIM_VICON_FAIL", 0) - # ensure vehicle remain in Loiter for 15 seconds - tstart = self.get_sim_time() - while self.get_sim_time() - tstart < 15: - if not self.mode_is('LOITER'): - raise NotAchievedException("Expected to stay in loiter for >15 seconds") + # switch to vicon, disable GPS and wait 10sec to ensure vehicle remains in Loiter + self.set_rc(8, 1500) + self.set_parameter("GPS1_TYPE", 0) - # RTL and check vehicle arrives within 10m of home - self.set_rc(2, 1500) - self.do_RTL() + # ensure vehicle remain in Loiter for 15 seconds + tstart = self.get_sim_time() + while self.get_sim_time() - tstart < 15: + if not self.mode_is('LOITER'): + raise NotAchievedException("Expected to stay in loiter for >15 seconds") - except Exception as e: - self.print_exception_caught(e) - ex = e - self.context_pop() - self.disarm_vehicle(force=True) - self.reboot_sitl() - if ex is not None: - raise ex + # RTL and check vehicle arrives within 10m of home + self.set_rc(2, 1500) + self.do_RTL() def RTLSpeed(self): """Test RTL Speed parameters""" @@ -3885,9 +3856,6 @@ def NavDelay(self): def RangeFinder(self): '''Test RangeFinder Basic Functionality''' - ex = None - self.context_push() - self.progress("Making sure we don't ordinarily get RANGEFINDER") m = self.mav.recv_match(type='RANGEFINDER', blocking=True, @@ -3903,68 +3871,59 @@ def RangeFinder(self): if self.current_onboard_log_contains_message("RFND"): raise NotAchievedException("Found unexpected RFND message") - try: - self.set_analog_rangefinder_parameters() - self.set_parameter("RC9_OPTION", 10) # rangefinder - self.set_rc(9, 2000) - - self.reboot_sitl() + self.set_analog_rangefinder_parameters() + self.set_parameter("RC9_OPTION", 10) # rangefinder + self.set_rc(9, 2000) - self.progress("Making sure we now get RANGEFINDER messages") - m = self.assert_receive_message('RANGEFINDER', timeout=10) + self.reboot_sitl() - self.progress("Checking RangeFinder is marked as enabled in mavlink") - m = self.mav.recv_match(type='SYS_STATUS', - blocking=True, - timeout=10) - flags = m.onboard_control_sensors_enabled - if not flags & mavutil.mavlink.MAV_SYS_STATUS_SENSOR_LASER_POSITION: - raise NotAchievedException("Laser not enabled in SYS_STATUS") - self.progress("Disabling laser using switch") - self.set_rc(9, 1000) - self.delay_sim_time(1) - self.progress("Checking RangeFinder is marked as disabled in mavlink") - m = self.mav.recv_match(type='SYS_STATUS', - blocking=True, - timeout=10) - flags = m.onboard_control_sensors_enabled - if flags & mavutil.mavlink.MAV_SYS_STATUS_SENSOR_LASER_POSITION: - raise NotAchievedException("Laser enabled in SYS_STATUS") + self.progress("Making sure we now get RANGEFINDER messages") + m = self.assert_receive_message('RANGEFINDER', timeout=10) - self.progress("Re-enabling rangefinder") - self.set_rc(9, 2000) - self.delay_sim_time(1) - m = self.mav.recv_match(type='SYS_STATUS', - blocking=True, - timeout=10) - flags = m.onboard_control_sensors_enabled - if not flags & mavutil.mavlink.MAV_SYS_STATUS_SENSOR_LASER_POSITION: - raise NotAchievedException("Laser not enabled in SYS_STATUS") + self.progress("Checking RangeFinder is marked as enabled in mavlink") + m = self.mav.recv_match(type='SYS_STATUS', + blocking=True, + timeout=10) + flags = m.onboard_control_sensors_enabled + if not flags & mavutil.mavlink.MAV_SYS_STATUS_SENSOR_LASER_POSITION: + raise NotAchievedException("Laser not enabled in SYS_STATUS") + self.progress("Disabling laser using switch") + self.set_rc(9, 1000) + self.delay_sim_time(1) + self.progress("Checking RangeFinder is marked as disabled in mavlink") + m = self.mav.recv_match(type='SYS_STATUS', + blocking=True, + timeout=10) + flags = m.onboard_control_sensors_enabled + if flags & mavutil.mavlink.MAV_SYS_STATUS_SENSOR_LASER_POSITION: + raise NotAchievedException("Laser enabled in SYS_STATUS") - self.takeoff(10, mode="LOITER") + self.progress("Re-enabling rangefinder") + self.set_rc(9, 2000) + self.delay_sim_time(1) + m = self.mav.recv_match(type='SYS_STATUS', + blocking=True, + timeout=10) + flags = m.onboard_control_sensors_enabled + if not flags & mavutil.mavlink.MAV_SYS_STATUS_SENSOR_LASER_POSITION: + raise NotAchievedException("Laser not enabled in SYS_STATUS") - m_r = self.mav.recv_match(type='RANGEFINDER', - blocking=True) - m_p = self.mav.recv_match(type='GLOBAL_POSITION_INT', - blocking=True) + self.takeoff(10, mode="LOITER") - if abs(m_r.distance - m_p.relative_alt/1000) > 1: - raise NotAchievedException( - "rangefinder/global position int mismatch %0.2f vs %0.2f" % - (m_r.distance, m_p.relative_alt/1000)) + m_r = self.mav.recv_match(type='RANGEFINDER', + blocking=True) + m_p = self.mav.recv_match(type='GLOBAL_POSITION_INT', + blocking=True) - self.land_and_disarm() + if abs(m_r.distance - m_p.relative_alt/1000) > 1: + raise NotAchievedException( + "rangefinder/global position int mismatch %0.2f vs %0.2f" % + (m_r.distance, m_p.relative_alt/1000)) - if not self.current_onboard_log_contains_message("RFND"): - raise NotAchievedException("Did not see expected RFND message") + self.land_and_disarm() - except Exception as e: - self.print_exception_caught(e) - ex = e - self.context_pop() - self.reboot_sitl() - if ex is not None: - raise ex + if not self.current_onboard_log_contains_message("RFND"): + raise NotAchievedException("Did not see expected RFND message") def SplineTerrain(self): '''Test Splines and Terrain''' diff --git a/Tools/autotest/rover.py b/Tools/autotest/rover.py index 941ae2f76695b0..b11920ead0361d 100644 --- a/Tools/autotest/rover.py +++ b/Tools/autotest/rover.py @@ -86,86 +86,76 @@ def get_stick_arming_channel(self): def DriveSquare(self, side=50): """Learn/Drive Square with Ch7 option""" - self.context_push() - ex = None - try: - self.progress("TEST SQUARE") - self.set_parameters({ - "RC7_OPTION": 7, - "RC9_OPTION": 58, - }) + self.progress("TEST SQUARE") + self.set_parameters({ + "RC7_OPTION": 7, + "RC9_OPTION": 58, + }) - self.change_mode('MANUAL') + self.change_mode('MANUAL') - self.wait_ready_to_arm() - self.arm_vehicle() + self.wait_ready_to_arm() + self.arm_vehicle() - self.clear_wp(9) - - # first aim north - self.progress("\nTurn right towards north") - self.reach_heading_manual(10) - # save bottom left corner of box as home AND waypoint - self.progress("Save HOME") - self.save_wp() - - self.progress("Save WP") - self.save_wp() - - # pitch forward to fly north - self.progress("\nGoing north %u meters" % side) - self.reach_distance_manual(side) - # save top left corner of square as waypoint - self.progress("Save WP") - self.save_wp() - - # roll right to fly east - self.progress("\nGoing east %u meters" % side) - self.reach_heading_manual(100) - self.reach_distance_manual(side) - # save top right corner of square as waypoint - self.progress("Save WP") - self.save_wp() - - # pitch back to fly south - self.progress("\nGoing south %u meters" % side) - self.reach_heading_manual(190) - self.reach_distance_manual(side) - # save bottom right corner of square as waypoint - self.progress("Save WP") - self.save_wp() - - # roll left to fly west - self.progress("\nGoing west %u meters" % side) - self.reach_heading_manual(280) - self.reach_distance_manual(side) - # save bottom left corner of square (should be near home) as waypoint - self.progress("Save WP") - self.save_wp() - - self.progress("Checking number of saved waypoints") - mavproxy = self.start_mavproxy() - num_wp = self.save_mission_to_file_using_mavproxy( - mavproxy, - os.path.join(testdir, "ch7_mission.txt")) - self.stop_mavproxy(mavproxy) - expected = 7 # home + 6 toggled in - if num_wp != expected: - raise NotAchievedException("Did not get %u waypoints; got %u" % - (expected, num_wp)) + self.clear_wp(9) + + # first aim north + self.progress("\nTurn right towards north") + self.reach_heading_manual(10) + # save bottom left corner of box as home AND waypoint + self.progress("Save HOME") + self.save_wp() + + self.progress("Save WP") + self.save_wp() + + # pitch forward to fly north + self.progress("\nGoing north %u meters" % side) + self.reach_distance_manual(side) + # save top left corner of square as waypoint + self.progress("Save WP") + self.save_wp() + + # roll right to fly east + self.progress("\nGoing east %u meters" % side) + self.reach_heading_manual(100) + self.reach_distance_manual(side) + # save top right corner of square as waypoint + self.progress("Save WP") + self.save_wp() + + # pitch back to fly south + self.progress("\nGoing south %u meters" % side) + self.reach_heading_manual(190) + self.reach_distance_manual(side) + # save bottom right corner of square as waypoint + self.progress("Save WP") + self.save_wp() + + # roll left to fly west + self.progress("\nGoing west %u meters" % side) + self.reach_heading_manual(280) + self.reach_distance_manual(side) + # save bottom left corner of square (should be near home) as waypoint + self.progress("Save WP") + self.save_wp() + + self.progress("Checking number of saved waypoints") + mavproxy = self.start_mavproxy() + num_wp = self.save_mission_to_file_using_mavproxy( + mavproxy, + os.path.join(testdir, "ch7_mission.txt")) + self.stop_mavproxy(mavproxy) + expected = 7 # home + 6 toggled in + if num_wp != expected: + raise NotAchievedException("Did not get %u waypoints; got %u" % + (expected, num_wp)) - # TODO: actually drive the mission + # TODO: actually drive the mission - self.clear_wp(9) - except Exception as e: - self.print_exception_caught(e) - ex = e + self.clear_wp(9) self.disarm_vehicle() - self.context_pop() - - if ex: - raise ex def drive_left_circuit(self): """Drive a left circuit, 50m on a side.""" @@ -5592,65 +5582,55 @@ def send_obstacle_distances_expect_distance_sensor_messages(self, obstacle_dista def AP_Proximity_MAV(self): '''Test MAV proximity backend''' - self.context_push() - ex = None - try: - self.set_parameters({ - "PRX1_TYPE": 2, # AP_Proximity_MAV - "OA_TYPE": 2, # dijkstra - "OA_DB_OUTPUT": 3, # send all items - }) - self.reboot_sitl() - - # 1 laser pointing straight forward: - self.send_obstacle_distances_expect_distance_sensor_messages( - { - "distances": [234], - "increment_f": 10, - "angle_offset": 0.0, - "min_distance": 0, - "max_distance": 1000, # cm - }, [ - {"orientation": 0, "distance": 234}, - ]) - - # 5 lasers at front of vehicle, spread over 40 degrees: - self.send_obstacle_distances_expect_distance_sensor_messages( - { - "distances": [111, 222, 333, 444, 555], - "increment_f": 10, - "angle_offset": -20.0, - "min_distance": 0, - "max_distance": 1000, # cm - }, [ - {"orientation": 0, "distance": 111}, - ]) - - # lots of dense readings (e.g. vision camera: - distances = [0] * 72 - for i in range(0, 72): - distances[i] = 1000 + 10*abs(36-i) - - self.send_obstacle_distances_expect_distance_sensor_messages( - { - "distances": distances, - "increment_f": 90/72.0, - "angle_offset": -45.0, - "min_distance": 0, - "max_distance": 2000, # cm - }, [ - {"orientation": 0, "distance": 1000}, - {"orientation": 1, "distance": 1190}, - {"orientation": 7, "distance": 1190}, - ]) - except Exception as e: - self.print_exception_caught(e) - ex = e - self.context_pop() + self.set_parameters({ + "PRX1_TYPE": 2, # AP_Proximity_MAV + "OA_TYPE": 2, # dijkstra + "OA_DB_OUTPUT": 3, # send all items + }) self.reboot_sitl() - if ex is not None: - raise ex + + # 1 laser pointing straight forward: + self.send_obstacle_distances_expect_distance_sensor_messages( + { + "distances": [234], + "increment_f": 10, + "angle_offset": 0.0, + "min_distance": 0, + "max_distance": 1000, # cm + }, [ + {"orientation": 0, "distance": 234}, + ]) + + # 5 lasers at front of vehicle, spread over 40 degrees: + self.send_obstacle_distances_expect_distance_sensor_messages( + { + "distances": [111, 222, 333, 444, 555], + "increment_f": 10, + "angle_offset": -20.0, + "min_distance": 0, + "max_distance": 1000, # cm + }, [ + {"orientation": 0, "distance": 111}, + ]) + + # lots of dense readings (e.g. vision camera: + distances = [0] * 72 + for i in range(0, 72): + distances[i] = 1000 + 10*abs(36-i) + + self.send_obstacle_distances_expect_distance_sensor_messages( + { + "distances": distances, + "increment_f": 90/72.0, + "angle_offset": -45.0, + "min_distance": 0, + "max_distance": 2000, # cm + }, [ + {"orientation": 0, "distance": 1000}, + {"orientation": 1, "distance": 1190}, + {"orientation": 7, "distance": 1190}, + ]) def SendToComponents(self): '''Test ArduPilot send_to_components function''' @@ -6295,8 +6275,6 @@ def StickMixingAuto(self): def AutoDock(self): '''Test automatic docking of rover for multiple FOVs of simulated beacon''' - self.context_push() - self.set_parameters({ "PLND_ENABLED": 1, "PLND_TYPE": 4, @@ -6322,40 +6300,27 @@ def AutoDock(self): }) for type in range(0, 3): # CYLINDRICAL FOV, CONICAL FOV, SPHERICAL FOV - ex = None - try: - self.set_parameter("SIM_PLD_TYPE", type) - self.reboot_sitl() - self.change_mode('GUIDED') - self.wait_ready_to_arm() - self.arm_vehicle() - initial_position = self.offset_location_ne(target, -20, -2) - self.drive_to_location(initial_position) - self.change_mode(8) # DOCK mode - max_delta = 1 - self.wait_distance_to_location(target, 0, max_delta, timeout=180) - self.disarm_vehicle() - self.assert_receive_message('GLOBAL_POSITION_INT') - new_pos = self.mav.location() - delta = abs(self.get_distance(target, new_pos) - stopping_dist) - self.progress("Docked %f metres from stopping point" % delta) - if delta > max_delta: - raise NotAchievedException("Did not dock close enough to stopping point (%fm > %fm" % (delta, max_delta)) - - if not self.current_onboard_log_contains_message("PL"): - raise NotAchievedException("Did not see expected PL message") - - except Exception as e: - self.print_exception_caught(e) - ex = e - break - - self.context_pop() + self.set_parameter("SIM_PLD_TYPE", type) + self.reboot_sitl() + self.change_mode('GUIDED') + self.wait_ready_to_arm() + self.arm_vehicle() + initial_position = self.offset_location_ne(target, -20, -2) + self.drive_to_location(initial_position) + self.change_mode(8) # DOCK mode + max_delta = 1 + self.wait_distance_to_location(target, 0, max_delta, timeout=180) + self.disarm_vehicle() + self.assert_receive_message('GLOBAL_POSITION_INT') + new_pos = self.mav.location() + delta = abs(self.get_distance(target, new_pos) - stopping_dist) + self.progress("Docked %f metres from stopping point" % delta) + if delta > max_delta: + raise NotAchievedException("Did not dock close enough to stopping point (%fm > %fm" % (delta, max_delta)) - if ex is not None: - raise ex + if not self.current_onboard_log_contains_message("PL"): + raise NotAchievedException("Did not see expected PL message") - self.reboot_sitl() self.progress("All done") def PrivateChannel(self): From e170710de9ba36ff5341070a7bc7c724d4b59823 Mon Sep 17 00:00:00 2001 From: George Zogopoulos Date: Thu, 18 Jul 2024 09:54:02 +0200 Subject: [PATCH 109/124] SITL: Glider improvements Fixed balloon going below ground. Enabled glider torque logging. --- libraries/SITL/SIM_Aircraft.cpp | 2 +- libraries/SITL/SIM_Glider.cpp | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index a6baf91eb9da24..3e519576e43179 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -181,7 +181,7 @@ void Aircraft::update_position(void) uint32_t now = AP_HAL::millis(); if (now - last_one_hz_ms >= 1000) { // shift origin of position at 1Hz to current location - // this prevents sperical errors building up in the GPS data + // this prevents spherical errors building up in the GPS data last_one_hz_ms = now; Vector2d diffNE = origin.get_distance_NE_double(location); position.xy() -= diffNE; diff --git a/libraries/SITL/SIM_Glider.cpp b/libraries/SITL/SIM_Glider.cpp index c86535b83cc5dc..a1334aac6168c4 100644 --- a/libraries/SITL/SIM_Glider.cpp +++ b/libraries/SITL/SIM_Glider.cpp @@ -231,7 +231,7 @@ void Glider::calculate_forces(const struct sitl_input &input, Vector3f &rot_acce float aileron = 0.5*(filtered_servo_angle(input, 1) + filtered_servo_angle(input, 4)); float elevator = filtered_servo_angle(input, 2); float rudder = filtered_servo_angle(input, 3); - float balloon = filtered_servo_range(input, 5); + float balloon = MAX(0.0f, filtered_servo_range(input, 5)); // Don't let the balloon receive downwards commands. float balloon_cut = filtered_servo_range(input, 9); // Move balloon upwards using balloon velocity from channel 6 @@ -241,7 +241,7 @@ void Glider::calculate_forces(const struct sitl_input &input, Vector3f &rot_acce balloon_velocity = Vector3f(-wind_ef.x, -wind_ef.y, -wind_ef.z -balloon_rate * balloon); balloon_position += balloon_velocity * (1.0e-6 * (float)frame_time_us); const float height_AMSL = 0.01f * (float)home.alt - position.z; - // release at burst height or when channel 9 goes high + // release at burst height or when balloon cut output goes high if (hal.scheduler->is_system_initialized() && (height_AMSL > balloon_burst_amsl || balloon_cut > 0.8)) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "pre-release at %i m AMSL\n", (int)height_AMSL); @@ -380,7 +380,7 @@ bool Glider::update_balloon(float balloon, Vector3f &force, Vector3f &rot_accel) // NED unit vector pointing from tether attachment on plane to attachment on balloon Vector3f tether_unit_vec_ef = relative_position.normalized(); - // NED velocity of attahment point on plane + // NED velocity of attachment point on plane Vector3f attachment_velocity_ef = velocity_ef + dcm * (gyro % tether_pos_bf); // NED velocity of attachment point on balloon as seen by observer on attachemnt point on plane From cb044d4718491c4c57ecb23e989fd9f3b41475af Mon Sep 17 00:00:00 2001 From: George Zogopoulos Date: Thu, 18 Jul 2024 11:53:13 +0200 Subject: [PATCH 110/124] Tools: Optionally include AP_SIM_GLIDER_ENABLED on SIH --- Tools/scripts/sitl-on-hardware/sitl-on-hw.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/Tools/scripts/sitl-on-hardware/sitl-on-hw.py b/Tools/scripts/sitl-on-hardware/sitl-on-hw.py index 291005a7a73739..16707043a929d1 100755 --- a/Tools/scripts/sitl-on-hardware/sitl-on-hw.py +++ b/Tools/scripts/sitl-on-hardware/sitl-on-hw.py @@ -66,6 +66,8 @@ def sohw_path(fname): defaults_write(open(args.defaults,"r").read() + "\n") if args.simclass: + if args.simclass == 'Glider': + hwdef_write("define AP_SIM_GLIDER_ENABLED 1\n") hwdef_write("define AP_SIM_FRAME_CLASS %s\n" % args.simclass) if args.frame: hwdef_write('define AP_SIM_FRAME_STRING "%s"\n' % args.frame) From 5c2b758f4288cc924d54ca350c74c88a5e9fba10 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 8 Jul 2024 19:50:02 +0900 Subject: [PATCH 111/124] SITL: add SlungPayload --- libraries/SITL/SIM_SlungPayload.cpp | 458 ++++++++++++++++++++++++++++ libraries/SITL/SIM_SlungPayload.h | 102 +++++++ 2 files changed, 560 insertions(+) create mode 100644 libraries/SITL/SIM_SlungPayload.cpp create mode 100644 libraries/SITL/SIM_SlungPayload.h diff --git a/libraries/SITL/SIM_SlungPayload.cpp b/libraries/SITL/SIM_SlungPayload.cpp new file mode 100644 index 00000000000000..03b5363300d7cb --- /dev/null +++ b/libraries/SITL/SIM_SlungPayload.cpp @@ -0,0 +1,458 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + simulate a slung payload +*/ + +#include "SIM_config.h" + +#if AP_SIM_SLUNGPAYLOAD_ENABLED + +#include "SIM_SlungPayload.h" +#include "SITL.h" +#include +#include "SIM_Aircraft.h" +#include +#include +#include + +using namespace SITL; + +// SlungPayloadSim parameters +const AP_Param::GroupInfo SlungPayloadSim::var_info[] = { + // @Param: ENABLE + // @DisplayName: Slung Payload Sim enable/disable + // @Description: Slung Payload Sim enable/disable + // @Values: 0:Disabled,1:Enabled + // @User: Advanced + AP_GROUPINFO_FLAGS("ENABLE", 1, SlungPayloadSim, enable, 0, AP_PARAM_FLAG_ENABLE), + + // @Param: WEIGHT + // @DisplayName: Slung Payload weight + // @Description: Slung Payload weight in kg + // @Units: kg + // @Range: 0 15 + // @User: Advanced + AP_GROUPINFO("WEIGHT", 2, SlungPayloadSim, weight_kg, 1.0), + + // @Param: LINELEN + // @DisplayName: Slung Payload line length + // @Description: Slung Payload line length in meters + // @Units: m + // @Range: 0 100 + // @User: Advanced + AP_GROUPINFO("LINELEN", 3, SlungPayloadSim, line_length, 30.0), + + // @Param: DRAG + // @DisplayName: Slung Payload drag coefficient + // @Description: Slung Payload drag coefficient. Higher values increase drag and slow the payload more quickly + // @Units: m + // @Range: 0 10 + // @User: Advanced + AP_GROUPINFO("DRAG", 4, SlungPayloadSim, drag_coef, 1), + + // @Param: SYSID + // @DisplayName: Slung Payload MAVLink system ID + // @Description: Slung Payload MAVLink system id to distinguish it from others on the same network + // @Range: 0 255 + // @User: Advanced + AP_GROUPINFO("SYSID", 5, SlungPayloadSim, sys_id, 2), + + AP_GROUPEND +}; + +// SlungPayloadSim handles interaction with main vehicle +SlungPayloadSim::SlungPayloadSim() +{ + AP_Param::setup_object_defaults(this, var_info); +} + +// update the SlungPayloadSim's state using the vehicle's earth-frame position, velocity and acceleration +void SlungPayloadSim::update(const Vector3p& veh_pos, const Vector3f& veh_vel_ef, const Vector3f& veh_accel_ef) +{ + if (!enable) { + return; + } + + // initialise slung payload location + const uint32_t now_us = AP_HAL::micros(); + if (!initialised) { + // capture EKF origin + auto *sitl = AP::sitl(); + const Location ekf_origin = sitl->state.home; + if (ekf_origin.lat == 0 && ekf_origin.lng == 0) { + return; + } + + // more initialisation + last_update_us = now_us; + initialised = true; + } + + // calculate dt and update slung payload + const float dt = (now_us - last_update_us)*1.0e-6; + last_update_us = now_us; + update_payload(veh_pos, veh_vel_ef, veh_accel_ef, dt); + + // send payload location to GCS at 5hz + const uint32_t now_ms = AP_HAL::millis(); + if (now_ms - last_report_ms >= reporting_period_ms) { + last_report_ms = now_ms; + send_report(); + write_log(); + } +} + +// get earth-frame forces on the vehicle from slung payload +// returns true on success and fills in forces_ef argument, false on failure +bool SlungPayloadSim::get_forces_on_vehicle(Vector3f& forces_ef) const +{ + if (!enable) { + return false; + } + + forces_ef = veh_forces_ef; + return true; +} + +// send a report to the vehicle control code over MAVLink +void SlungPayloadSim::send_report(void) +{ + if (!mavlink_connected && mav_socket.connect(target_address, target_port)) { + ::printf("SlungPayloadSim connected to %s:%u\n", target_address, (unsigned)target_port); + mavlink_connected = true; + } + if (!mavlink_connected) { + return; + } + + // get current time + uint32_t now_ms = AP_HAL::millis(); + + // send heartbeat at 1hz + const uint8_t component_id = MAV_COMP_ID_USER11; + if (now_ms - last_heartbeat_ms >= 1000) { + last_heartbeat_ms = now_ms; + + const mavlink_heartbeat_t heartbeat{ + custom_mode: 0, + type : MAV_TYPE_AIRSHIP, + autopilot : MAV_AUTOPILOT_INVALID, + base_mode: 0, + system_status: 0, + mavlink_version: 0, + }; + + mavlink_message_t msg; + mavlink_msg_heartbeat_encode_status( + sys_id.get(), + component_id, + &mav_status, + &msg, + &heartbeat); + uint8_t buf[300]; + const uint16_t len = mavlink_msg_to_send_buffer(buf, &msg); + mav_socket.send(buf, len); + } + + // send a GLOBAL_POSITION_INT messages + { + Location payload_loc; + int32_t alt_amsl_cm, alt_rel_cm; + if (!get_payload_location(payload_loc) || + !payload_loc.get_alt_cm(Location::AltFrame::ABSOLUTE, alt_amsl_cm) || + !payload_loc.get_alt_cm(Location::AltFrame::ABOVE_HOME, alt_rel_cm)) { + return; + } + const mavlink_global_position_int_t global_position_int{ + time_boot_ms: now_ms, + lat: payload_loc.lat, + lon: payload_loc.lng, + alt: alt_amsl_cm * 10, // amsl alt in mm + relative_alt: alt_rel_cm * 10, // relative alt in mm + vx: int16_t(velocity_NED.x * 100), // velocity in cm/s + vy: int16_t(velocity_NED.y * 100), // velocity in cm/s + vz: int16_t(velocity_NED.z * 100), // velocity in cm/s + hdg: 0 // heading in centi-degrees + }; + mavlink_message_t msg; + mavlink_msg_global_position_int_encode_status(sys_id, component_id, &mav_status, &msg, &global_position_int); + uint8_t buf[300]; + const uint16_t len = mavlink_msg_to_send_buffer(buf, &msg); + if (len > 0) { + mav_socket.send(buf, len); + } + } + + // send ATTITUDE so MissionPlanner can display orientation + { + const mavlink_attitude_t attitude{ + time_boot_ms: now_ms, + roll: 0, + pitch: 0, + yaw: 0, // heading in radians + rollspeed: 0, + pitchspeed: 0, + yawspeed: 0 + }; + mavlink_message_t msg; + mavlink_msg_attitude_encode_status( + sys_id, + component_id, + &mav_status, + &msg, + &attitude); + uint8_t buf[300]; + const uint16_t len = mavlink_msg_to_send_buffer(buf, &msg); + if (len > 0) { + mav_socket.send(buf, len); + } + } +} + +// write onboard log +void SlungPayloadSim::write_log() +{ +#if HAL_LOGGING_ENABLED + // write log of slung payload state + // @LoggerMessage: SLUP + // @Description: Slung payload + // @Field: TimeUS: Time since system startup + // @Field: Land: 1 if payload is landed, 0 otherwise + // @Field: Tens: Tension ratio, 1 if line is taut, 0 if slack + // @Field: Len: Line length + // @Field: PN: Payload position as offset from vehicle in North direction + // @Field: PE: Payload position as offset from vehicle in East direction + // @Field: PD: Payload position as offset from vehicle in Down direction + // @Field: VN: Payload velocity in North direction + // @Field: VE: Payload velocity in East direction + // @Field: VD: Payload velocity in Down direction + // @Field: AN: Payload acceleration in North direction + // @Field: AE: Payload acceleration in East direction + // @Field: AD: Payload acceleration in Down direction + // @Field: VFN: Force on vehicle in North direction + // @Field: VFE: Force on vehicle in East direction + // @Field: VFD: Force on vehicle in Down direction + AP::logger().WriteStreaming("SLUP", + "TimeUS,Land,Tens,Len,PN,PE,PD,VN,VE,VD,AN,AE,AD,VFN,VFE,VFD", // labels + "s-%mmmmnnnooo---", // units + "F-20000000000000", // multipliers + "Qbffffffffffffff", // format + AP_HAL::micros64(), + (uint8_t)landed, + (float)tension_ratio, + (float)payload_to_veh.length(), + (double)-payload_to_veh.x, + (double)-payload_to_veh.y, + (double)-payload_to_veh.z, + (double)velocity_NED.x, + (double)velocity_NED.y, + (double)velocity_NED.z, + (double)accel_NED.x, + (double)accel_NED.y, + (double)accel_NED.z, + (double)veh_forces_ef.x, + (double)veh_forces_ef.y, + (double)veh_forces_ef.z); +#endif +} + +// returns true on success and fills in payload_loc argument, false on failure +bool SlungPayloadSim::get_payload_location(Location& payload_loc) const +{ + // get EKF origin + auto *sitl = AP::sitl(); + if (sitl == nullptr) { + return false; + } + const Location ekf_origin = sitl->state.home; + if (ekf_origin.lat == 0 && ekf_origin.lng == 0) { + return false; + } + + // calculate location + payload_loc = ekf_origin; + payload_loc.offset(position_NED); + return true; +} + +// update the slung payloads position, velocity, acceleration +// vehicle position, velocity and acceleration should be in earth-frame NED frame +void SlungPayloadSim::update_payload(const Vector3p& veh_pos, const Vector3f& veh_vel_ef, const Vector3f& veh_accel_ef, float dt) +{ + // how we calculate the payload's position, velocity and acceleration + // 1. update the payload's position, velocity using the previous iterations acceleration + // 2. check that the payload does not fall below the terrain + // 3. check if the line is taught and that the payload does not move more than the line length from the vehicle + // 4. calculate gravity and drag forces on the payload + // 5. calculate the tension force between the payload and vehicle including force countering gravity, drag and centripetal force + // 6. update the payload's acceleration using the sum of the above forces + + // initialise position_NED from vehicle position + if (position_NED.is_zero()) { + if (!veh_pos.is_zero()) { + position_NED = veh_pos; + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SlungPayload: initialised at %f %f %f", position_NED.x, position_NED.y, position_NED.z); + } + return; + } + + // integrate previous iterations acceleration into velocity and position + velocity_NED += accel_NED * dt; + position_NED += (velocity_NED * dt).todouble(); + + // calculate distance from payload to vehicle + payload_to_veh = veh_pos - position_NED; + float payload_to_veh_length = payload_to_veh.length(); + + // update landed state by checking if payload has dropped below terrain + Location payload_loc; + if (get_payload_location(payload_loc)) { + int32_t alt_terrain_cm; + bool landed_orig = landed; + if (payload_loc.get_alt_cm(Location::AltFrame::ABOVE_TERRAIN, alt_terrain_cm)) { + + // landed if below terrain + if (alt_terrain_cm <= 0) { + landed = true; + + // raise payload to match terrain + position_NED.z += (alt_terrain_cm * 0.01); + + // zero out velocity and acceleration in horizontal and downward direction + velocity_NED.xy().zero(); + velocity_NED.z = MIN(velocity_NED.z, 0); + accel_NED.xy().zero(); + accel_NED.z = MIN(accel_NED.z, 0); + + // zero out forces on vehicle + veh_forces_ef.zero(); + } + + // not landed if above terrain + if (landed && (alt_terrain_cm > 1)) { + landed = false; + } + } + + // inform user if landed state has changed + if (landed != landed_orig) { + if (landed) { + // get payload location again in case it has moved + get_payload_location(payload_loc); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SlungPayload: landed lat:%f lon:%f alt:%4.1f", + (double)payload_loc.lat * 1e-7, + (double)payload_loc.lng * 1e-7, + (double)payload_loc.alt * 1e-2); + } else { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SlungPayload: liftoff"); + } + } + } + + // calculate forces of gravity + Vector3f force_gravity_NED = Vector3f(0.0f, 0.0f, GRAVITY_MSS * weight_kg); + + // tension force on payload (resists gravity, drag, centripetal force) + Vector3f tension_force_NED; + + // tension ratio to smooth transition from line being taut to slack + tension_ratio = 0; + + // calculate drag force (0.5 * drag_coef * air_density * velocity^2 * surface area) + Vector3f force_drag_NED; + if (drag_coef > 0 && !velocity_NED.is_zero()) { + const float air_density = 1.225; // 1.225 kg/m^3 (standard sea-level density) + const float surface_area_m2 = 0.07; // 30cm diameter sphere + const float drag_force = 0.5 * drag_coef * air_density * velocity_NED.length_squared() * surface_area_m2; + force_drag_NED = -velocity_NED.normalized() * drag_force; + } + + // sanity check payload distance from vehicle and calculate tension force + if (is_positive(payload_to_veh_length)) { + + // calculate unit vector from payload to vehicle + const Vector3f payload_to_veh_norm = payload_to_veh.normalized().tofloat(); + + // ensure payload is no more than line_length from vehicle + if (payload_to_veh_length > line_length) { + payload_to_veh *= (line_length / payload_to_veh_length); + position_NED = veh_pos - payload_to_veh; + } + + // calculate tension ratio as value between 0 and 1 + // tension ratio is 0 when payload-to-vehicle distance is 10cm less than line length + // tension ratio is 1 when payload-to-vehicle distance is equal to line length + tension_ratio = constrain_float(1.0 - (line_length - payload_to_veh_length) * 10, 0, 1); + + // calculate tension forces when line is taut + if (is_positive(tension_ratio)) { + + // tension resists gravity if vehicle is above payload + if (is_negative(payload_to_veh_norm.z)) { + tension_force_NED += -force_gravity_NED.projected(payload_to_veh_norm); + } + + // calculate tension force resulting from velocity difference between vehicle and payload + // use time constant to convert velocity to acceleration + const float velocity_to_accel_TC = 2.0; + Vector3f velocity_diff_NED = (veh_vel_ef - velocity_NED).projected(payload_to_veh_norm); + + // add to tension force if the vehicle is moving faster than the payload + if (vectors_same_direction(velocity_diff_NED, payload_to_veh_norm)) { + tension_force_NED += velocity_diff_NED / velocity_to_accel_TC * weight_kg; + } + + // tension force resisting payload drag + tension_force_NED += -force_drag_NED.projected(payload_to_veh_norm); + + // calculate centripetal force + const Vector3f velocity_parallel = velocity_NED.projected(payload_to_veh_norm); + const Vector3f velocity_perpendicular = velocity_NED - velocity_parallel; + const float tension_force_centripetal = velocity_perpendicular.length_squared() * weight_kg / line_length; + const Vector3f tension_force_centripetal_NED = payload_to_veh_norm * tension_force_centripetal; + + // add centripetal force to tension force + tension_force_NED += tension_force_centripetal_NED; + + // scale tension force by tension ratio + tension_force_NED *= tension_ratio; + } + } + + // force on vehicle is opposite to tension force on payload + veh_forces_ef = -tension_force_NED; + + // convert force to acceleration (f=m*a => a=f/m) + accel_NED = (force_gravity_NED + force_drag_NED + tension_force_NED) / weight_kg; + + // if slung payload is landed we zero out downward (e.g positive) acceleration + if (landed) { + accel_NED.z = MIN(accel_NED.z, 0); + // should probably zero out forces_ef vertical component as well? + } +} + +// returns true if the two vectors point in the same direction, false if perpendicular or opposite +bool SlungPayloadSim::vectors_same_direction(const Vector3f& v1, const Vector3f& v2) const +{ + // check both vectors are non-zero + if (v1.is_zero() || v2.is_zero()) { + return false; + } + return v1.dot(v2) > 0; +} + +#endif diff --git a/libraries/SITL/SIM_SlungPayload.h b/libraries/SITL/SIM_SlungPayload.h new file mode 100644 index 00000000000000..3f668af5472945 --- /dev/null +++ b/libraries/SITL/SIM_SlungPayload.h @@ -0,0 +1,102 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + simulate a payload slung from a line under a vehicle +*/ + +#pragma once + +#include "SIM_config.h" + +#if AP_SIM_SLUNGPAYLOAD_ENABLED + +#include +#include +#include +#include + +namespace SITL { + +// SlungPayloadSim handles interaction with main vehicle +class SlungPayloadSim { +public: + friend class SlungPayload; + + // constructor + SlungPayloadSim(); + + // update the SlungPayloadSim's state using thevehicle's earth-frame position, velocity and acceleration + void update(const Vector3p& veh_pos, const Vector3f& veh_vel_ef, const Vector3f& veh_accel_ef); + + // get earth-frame forces on the vehicle from slung payload + // returns true on success and fills in forces_ef argument, false on failure + bool get_forces_on_vehicle(Vector3f& forces_ef) const; + + // parameter table + static const struct AP_Param::GroupInfo var_info[]; + +private: + + // parameters + AP_Int8 enable; // enable parameter + AP_Float weight_kg; // payload weight in kg + AP_Float line_length; // line length in meters + AP_Int8 sys_id; // mavlink system id for reporting to GCS + AP_Float drag_coef; // drag coefficient (spheres=0.5, cubes=1.05, barrels=0.8~1.2) + + // send MAVLink messages to GCS + void send_report(); + + // write onboard log + void write_log(); + + // get payload location + // returns true on success and fills in payload_loc argument, false on failure + bool get_payload_location(Location& payload_loc) const; + + // update the slung payload's position, velocity, acceleration + // vehicle position, velocity and acceleration should be in earth-frame NED frame + void update_payload(const Vector3p& veh_pos, const Vector3f& veh_vel_ef, const Vector3f& veh_accel_ef, float dt); + + // returns true if the two vectors point in the same direction, false if perpendicular or opposite + bool vectors_same_direction(const Vector3f& v1, const Vector3f& v2) const; + + // socket connection variables + const char *target_address = "127.0.0.1"; + const uint16_t target_port = 5763; + SocketAPM_native mav_socket { false }; + bool initialised; // true if this class has been initialised + uint32_t last_update_us; // system time of last update + + // mavlink reporting variables + const float reporting_period_ms = 200; // reporting period in ms + uint32_t last_report_ms; // system time of last MAVLink report sent to GCS + uint32_t last_heartbeat_ms; // system time of last MAVLink heartbeat sent to GCS + bool mavlink_connected; // true if a mavlink connection has been established + mavlink_status_t mav_status; // reported mavlink status + + // payload variables + bool landed = true; // true if the payload is on the ground + float tension_ratio; // 0 if line is loose, 1 if completely taut + Vector3p payload_to_veh;// distance vector (in meters in NED frame) from payload to vehicle (used for reporting purposes) + Vector3p position_NED; // payload's position (as an offset from EKF origin? offset from vehicle?) in meters + Vector3f velocity_NED; // payload velocity + Vector3f accel_NED; // payload's acceleration + Vector3f veh_forces_ef; // earth-frame forces on the vehicle caused by the payload +}; + +} // namespace SITL + +#endif // AP_SIM_SLUNGPAYLOAD_ENABLED From a1579bc31e9a50a4b427e7b18791808cb650cbbd Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 12 Jul 2024 15:18:16 +0900 Subject: [PATCH 112/124] SITL: integrate SlungPayload --- libraries/SITL/SIM_Aircraft.cpp | 18 ++++++++++++++++++ libraries/SITL/SIM_Aircraft.h | 5 +++++ libraries/SITL/SIM_Multicopter.cpp | 5 +++++ libraries/SITL/SIM_config.h | 4 ++++ libraries/SITL/SITL.cpp | 6 ++++++ libraries/SITL/SITL.h | 4 ++++ 6 files changed, 42 insertions(+) diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index 3e519576e43179..9ca985a14591ab 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -796,6 +796,11 @@ void Aircraft::update_dynamics(const Vector3f &rot_accel) } } + // update slung payload +#if AP_SIM_SLUNGPAYLOAD_ENABLED + sitl->models.slung_payload_sim.update(get_position_relhome(), velocity_ef, accel_earth); +#endif + // allow for changes in physics step adjust_frame_time(constrain_float(sitl->loop_rate_hz, rate_hz-1, rate_hz+1)); } @@ -1227,6 +1232,19 @@ void Aircraft::add_twist_forces(Vector3f &rot_accel) } } +#if AP_SIM_SLUNGPAYLOAD_ENABLED +// add body-frame force due to slung payload +void Aircraft::add_slungpayload_forces(Vector3f &body_accel) +{ + Vector3f forces_ef; + sitl->models.slung_payload_sim.get_forces_on_vehicle(forces_ef); + + // convert ef forces to body-frame accelerations (acceleration = force / mass) + const Vector3f accel_bf = dcm.transposed() * forces_ef / mass; + body_accel += accel_bf; +} +#endif + /* get position relative to home */ diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index 90f07089ea60b1..e84a078ac8fafc 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -322,6 +322,11 @@ class Aircraft { void add_shove_forces(Vector3f &rot_accel, Vector3f &body_accel); void add_twist_forces(Vector3f &rot_accel); +#if AP_SIM_SLUNGPAYLOAD_ENABLED + // add body-frame force due to slung payload + void add_slungpayload_forces(Vector3f &body_accel); +#endif + // get local thermal updraft float get_local_updraft(const Vector3d ¤tPos); diff --git a/libraries/SITL/SIM_Multicopter.cpp b/libraries/SITL/SIM_Multicopter.cpp index a866762593e05d..4e019dde9b4de4 100644 --- a/libraries/SITL/SIM_Multicopter.cpp +++ b/libraries/SITL/SIM_Multicopter.cpp @@ -48,6 +48,11 @@ void MultiCopter::calculate_forces(const struct sitl_input &input, Vector3f &rot add_shove_forces(rot_accel, body_accel); add_twist_forces(rot_accel); + +#if AP_SIM_SLUNGPAYLOAD_ENABLED + // add forces from slung payload + add_slungpayload_forces(body_accel); +#endif } /* diff --git a/libraries/SITL/SIM_config.h b/libraries/SITL/SIM_config.h index c83e83d44767d9..8ec2d7ff42c48c 100644 --- a/libraries/SITL/SIM_config.h +++ b/libraries/SITL/SIM_config.h @@ -51,6 +51,10 @@ #define AP_SIM_SHIP_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) #endif +#ifndef AP_SIM_SLUNGPAYLOAD_ENABLED +#define AP_SIM_SLUNGPAYLOAD_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) +#endif + #ifndef AP_SIM_TSYS03_ENABLED #define AP_SIM_TSYS03_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) #endif diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index a21e744c349b53..681d5d3dbf440c 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -1249,6 +1249,12 @@ const AP_Param::GroupInfo SIM::ModelParm::var_info[] = { AP_SUBGROUPPTR(glider_ptr, "GLD_", 3, SIM::ModelParm, Glider), #endif +#if AP_SIM_SLUNGPAYLOAD_ENABLED + // @Group: SLUP_ + // @Path: ./SIM_SlungPayload.cpp + AP_SUBGROUPINFO(slung_payload_sim, "SLUP_", 4, SIM::ModelParm, SlungPayloadSim), +#endif + AP_GROUPEND }; diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index 3b197fe8469e52..d3ad9ebbd962aa 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -27,6 +27,7 @@ #include "SIM_FETtecOneWireESC.h" #include "SIM_IntelligentEnergy24.h" #include "SIM_Ship.h" +#include "SIM_SlungPayload.h" #include "SIM_GPS.h" #include "SIM_DroneCANDevice.h" #include "SIM_ADSB_Sagetech_MXS.h" @@ -322,6 +323,9 @@ class SIM { #endif #if AP_SIM_GLIDER_ENABLED Glider *glider_ptr; +#endif +#if AP_SIM_SLUNGPAYLOAD_ENABLED + SlungPayloadSim slung_payload_sim; #endif }; ModelParm models; From 76abc6850f55adad9118976fdc38e8cfd35a4279 Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Thu, 11 Jul 2024 15:49:36 -0500 Subject: [PATCH 113/124] Filter: add "source" to option 5 --- libraries/Filter/HarmonicNotchFilter.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/Filter/HarmonicNotchFilter.cpp b/libraries/Filter/HarmonicNotchFilter.cpp index 835e53192abfd0..ea42cf249e14fe 100644 --- a/libraries/Filter/HarmonicNotchFilter.cpp +++ b/libraries/Filter/HarmonicNotchFilter.cpp @@ -125,7 +125,7 @@ const AP_Param::GroupInfo HarmonicNotchFilterParams::var_info[] = { // @Param: OPTS // @DisplayName: Harmonic Notch Filter options // @Description: Harmonic Notch Filter options. Triple and double-notches can provide deeper attenuation across a wider bandwidth with reduced latency than single notches and are suitable for larger aircraft. Multi-Source attaches a harmonic notch to each detected noise frequency instead of simply being multiples of the base frequency, in the case of FFT it will attach notches to each of three detected noise peaks, in the case of ESC it will attach notches to each of four motor RPM values. Loop rate update changes the notch center frequency at the scheduler loop rate rather than at the default of 200Hz. If both double and triple notches are specified only double notches will take effect. - // @Bitmask: 0:Double notch,1:Multi-Source,2:Update at loop rate,3:EnableOnAllIMUs,4:Triple notch, 5:Use min freq on RPM failure + // @Bitmask: 0:Double notch,1:Multi-Source,2:Update at loop rate,3:EnableOnAllIMUs,4:Triple notch, 5:Use min freq on RPM source failure // @User: Advanced // @RebootRequired: True AP_GROUPINFO("OPTS", 8, HarmonicNotchFilterParams, _options, 0), From 1b77751aefab6c41fee364dcbc452ed792e6231b Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 23 Jul 2024 21:42:25 +0900 Subject: [PATCH 114/124] AP_SmartRTL: add point made public --- libraries/AP_SmartRTL/AP_SmartRTL.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AP_SmartRTL/AP_SmartRTL.h b/libraries/AP_SmartRTL/AP_SmartRTL.h index 0d83372cf3f88b..d2c5b439e04c97 100644 --- a/libraries/AP_SmartRTL/AP_SmartRTL.h +++ b/libraries/AP_SmartRTL/AP_SmartRTL.h @@ -41,6 +41,9 @@ class AP_SmartRTL { // get a point on the path const Vector3f& get_point(uint16_t index) const { return _path[index]; } + // add point to end of path. returns true on success, false on failure (due to failure to take the semaphore) + bool add_point(const Vector3f& point); + // get next point on the path to home, returns true on success bool pop_point(Vector3f& point); @@ -109,9 +112,6 @@ class AP_SmartRTL { IgnorePilotYaw = (1U << 2), }; - // add point to end of path - bool add_point(const Vector3f& point); - // routine cleanup attempts to remove 10 points (see SMARTRTL_CLEANUP_POINT_MIN definition) by simplification or loop pruning void routine_cleanup(uint16_t path_points_count, uint16_t path_points_complete_limit); From efc7a9071ab2eb6f8ecb234fe86936af634fabc3 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 23 Jul 2024 21:43:59 +0900 Subject: [PATCH 115/124] Copter: SmartRTL mode restores point if interrupted --- ArduCopter/mode.h | 4 ++++ ArduCopter/mode_smart_rtl.cpp | 16 +++++++++++++++- 2 files changed, 19 insertions(+), 1 deletion(-) diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 721cef3f0ad213..25b1b0702770af 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -1541,6 +1541,10 @@ class ModeSmartRTL : public ModeRTL { // point while following our path home. If we take too long we // may choose to land the vehicle. uint32_t path_follow_last_pop_fail_ms; + + // backup last popped point so that it can be restored to the path + // if vehicle exits SmartRTL mode before reaching home. invalid if zero + Vector3f dest_NED_backup; }; diff --git a/ArduCopter/mode_smart_rtl.cpp b/ArduCopter/mode_smart_rtl.cpp index dacd7cc5f29ab0..8c2b1988c30895 100644 --- a/ArduCopter/mode_smart_rtl.cpp +++ b/ArduCopter/mode_smart_rtl.cpp @@ -35,6 +35,14 @@ bool ModeSmartRTL::init(bool ignore_checks) // perform cleanup required when leaving smart_rtl void ModeSmartRTL::exit() { + // restore last point if we hadn't reached it + if (smart_rtl_state == SubMode::PATH_FOLLOW && !dest_NED_backup.is_zero()) { + if (!g2.smart_rtl.add_point(dest_NED_backup)) { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "SmartRTL: lost one point"); + } + } + dest_NED_backup.zero(); + g2.smart_rtl.cancel_request_for_thorough_cleanup(); } @@ -83,10 +91,16 @@ void ModeSmartRTL::path_follow_run() { // if we are close to current target point, switch the next point to be our target. if (wp_nav->reached_wp_destination()) { - Vector3f dest_NED; + + // clear destination backup so that it cannot be restored + dest_NED_backup.zero(); + // this pop_point can fail if the IO task currently has the // path semaphore. + Vector3f dest_NED; if (g2.smart_rtl.pop_point(dest_NED)) { + // backup destination in case we exit smart_rtl mode and need to restore it to the path + dest_NED_backup = dest_NED; path_follow_last_pop_fail_ms = 0; if (g2.smart_rtl.get_num_points() == 0) { // this is the very last point, add 2m to the target alt and move to pre-land state From cfd1e98333bbf6bdacd8edb16a57824044666968 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 23 Jul 2024 16:51:42 +0900 Subject: [PATCH 116/124] AP_Mount: topotek safely parses version --- libraries/AP_Mount/AP_Mount_Topotek.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/libraries/AP_Mount/AP_Mount_Topotek.cpp b/libraries/AP_Mount/AP_Mount_Topotek.cpp index 9487e202af65ff..3aa8f43c05ca6d 100755 --- a/libraries/AP_Mount/AP_Mount_Topotek.cpp +++ b/libraries/AP_Mount/AP_Mount_Topotek.cpp @@ -1076,6 +1076,9 @@ void AP_Mount_Topotek::gimbal_version_analyse() version[ver_count++] = ver_num; ver_num = 0; } + if (ver_count >= ARRAY_SIZE(version)) { + break; + } } } else { if (data_buf_len >= 1) { From 2b64787099e2ef869969aee69a0a49469797c76c Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 23 Jul 2024 16:52:33 +0900 Subject: [PATCH 117/124] AP_Mount: topotek spelling fix --- libraries/AP_Mount/AP_Mount_Topotek.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_Topotek.cpp b/libraries/AP_Mount/AP_Mount_Topotek.cpp index 3aa8f43c05ca6d..e82a3f217cb6ce 100755 --- a/libraries/AP_Mount/AP_Mount_Topotek.cpp +++ b/libraries/AP_Mount/AP_Mount_Topotek.cpp @@ -1060,15 +1060,15 @@ void AP_Mount_Topotek::gimbal_version_analyse() const uint8_t data_buf_len = char_to_hex(_msg_buff[5]); // check for "." - bool constains_period = false; + bool contains_period = false; for (uint8_t i = 0; i < data_buf_len; i++) { - constains_period |= _msg_buff[10 + i] == '.'; + contains_period |= _msg_buff[10 + i] == '.'; } // if contains period, extract version number uint32_t ver_num = 0; uint8_t ver_count = 0; - if (constains_period) { + if (contains_period) { for (uint8_t i = 0; i < data_buf_len; i++) { if (_msg_buff[10 + i] != '.') { ver_num = ver_num * 10 + char_to_hex(_msg_buff[10 + i]); From 3e8b5ccee43fb9d7ef65eb53614cb8daad3ae7d6 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Wed, 24 Jul 2024 09:54:15 +1000 Subject: [PATCH 118/124] AP_Bootloader: fix build for STM32H7 without heap disables ecc check for those boards as well --- Tools/AP_Bootloader/AP_Bootloader.cpp | 2 +- Tools/AP_Bootloader/support.cpp | 4 ++-- Tools/AP_Bootloader/support.h | 2 ++ 3 files changed, 5 insertions(+), 3 deletions(-) diff --git a/Tools/AP_Bootloader/AP_Bootloader.cpp b/Tools/AP_Bootloader/AP_Bootloader.cpp index 79fdb759b03ac1..5c1efea998d3ea 100644 --- a/Tools/AP_Bootloader/AP_Bootloader.cpp +++ b/Tools/AP_Bootloader/AP_Bootloader.cpp @@ -74,7 +74,7 @@ int main(void) flash_init(); -#ifdef STM32H7 +#if defined(STM32H7) && CH_CFG_USE_HEAP check_ecc_errors(); #endif diff --git a/Tools/AP_Bootloader/support.cpp b/Tools/AP_Bootloader/support.cpp index a7873fcb18924b..d3046a228adff1 100644 --- a/Tools/AP_Bootloader/support.cpp +++ b/Tools/AP_Bootloader/support.cpp @@ -533,7 +533,7 @@ void port_setbaud(uint32_t baudrate) } #endif // BOOTLOADER_DEV_LIST -#ifdef STM32H7 +#if defined(STM32H7) && CH_CFG_USE_HEAP /* check if flash has any ECC errors and if it does then erase all of flash @@ -580,5 +580,5 @@ void check_ecc_errors(void) } __enable_fault_irq(); } -#endif // STM32H7 +#endif // defined(STM32H7) && CH_CFG_USE_HEAP diff --git a/Tools/AP_Bootloader/support.h b/Tools/AP_Bootloader/support.h index 5dda7a38b26824..8928ff9442ae47 100644 --- a/Tools/AP_Bootloader/support.h +++ b/Tools/AP_Bootloader/support.h @@ -51,7 +51,9 @@ void thread_sleep_ms(uint32_t ms); void custom_startup(void); +#if defined(STM32H7) && CH_CFG_USE_HEAP void check_ecc_errors(void); +#endif // printf to debug uart (or USB) extern "C" { From 3ab33887527c4f16e805d1cd8bb1498476485797 Mon Sep 17 00:00:00 2001 From: rishabsingh3003 Date: Tue, 23 Jul 2024 10:24:26 -0400 Subject: [PATCH 119/124] AP_ESC_Telem: Add ifndef before defining ESC_TELEM_MAX_ESCS --- libraries/AP_ESC_Telem/AP_ESC_Telem.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.h b/libraries/AP_ESC_Telem/AP_ESC_Telem.h index d25fe5be3d3fc6..65b61fdf30359a 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.h +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.h @@ -7,7 +7,9 @@ #if HAL_WITH_ESC_TELEM -#define ESC_TELEM_MAX_ESCS NUM_SERVO_CHANNELS +#ifndef ESC_TELEM_MAX_ESCS + #define ESC_TELEM_MAX_ESCS NUM_SERVO_CHANNELS +#endif static_assert(ESC_TELEM_MAX_ESCS > 0, "Cannot have 0 ESC telemetry instances"); #define ESC_TELEM_DATA_TIMEOUT_MS 5000UL From e499c2d39f47f9fc46e927706dc1d3b2eddb35cb Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Mon, 22 Jul 2024 10:47:40 +1000 Subject: [PATCH 120/124] AP_Bootloader: fix stuck in bl on reboot from firmware with network and CAN bootlaoders --- Tools/AP_Bootloader/can.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/AP_Bootloader/can.cpp b/Tools/AP_Bootloader/can.cpp index 0026ffc9c5bbc5..80c6c0bb02d344 100644 --- a/Tools/AP_Bootloader/can.cpp +++ b/Tools/AP_Bootloader/can.cpp @@ -747,7 +747,7 @@ bool can_check_update(void) bool ret = false; #if HAL_RAM_RESERVE_START >= 256 struct app_bootloader_comms *comms = (struct app_bootloader_comms *)HAL_RAM0_START; - if (comms->magic == APP_BOOTLOADER_COMMS_MAGIC) { + if (comms->magic == APP_BOOTLOADER_COMMS_MAGIC && comms->my_node_id != 0) { can_set_node_id(comms->my_node_id); fw_update.node_id = comms->server_node_id; for (uint8_t i=0; i Date: Mon, 22 Jul 2024 10:50:29 +1000 Subject: [PATCH 121/124] bootloaders: update PPPGW bootloaders --- Tools/bootloaders/CubePilot-PPPGW_bl.bin | Bin 91212 -> 91212 bytes Tools/bootloaders/CubeRedPrimary-PPPGW_bl.bin | Bin 94256 -> 94256 bytes Tools/bootloaders/Pixhawk6X-PPPGW_bl.bin | Bin 94092 -> 94364 bytes 3 files changed, 0 insertions(+), 0 deletions(-) diff --git a/Tools/bootloaders/CubePilot-PPPGW_bl.bin b/Tools/bootloaders/CubePilot-PPPGW_bl.bin index 785e416ab5b6b1669a629af80f2eed50ecf07b4a..7d19a35945cbd838b94a7c968be1e58eb7e62047 100755 GIT binary patch delta 92 zcmV-i0HgoR$OX*E1+efGA|laXefY5wO8_OwA`wX#a1lr&eF-qY_}fqb_yIgm^ni1d y5ET|8@&4ca$+W`lDyf^g! z!T@A58~cB80MgLTQ z$YwV7|Kb3o#Z4GKDExn*r0J&M#_+-SKcj-hUIv9f&l&d5TI+JOK#f7cBY{CdjG?(m LbbFB~3ZEq9%DCFq%hGjEQA6M!neXUAFzsEc!gj`+43!etbS(&diyaGiS;@ zGjnG4^Ud%*uI7XP$1y*HN=kJ%c)8h{OO74YB^ zV&i^8Y$;$KUwKT0?;HoD8w8QHZtdAb&6~ z$gem;_;@FoB;n*P2rhDxy^`eAD>mAGqyr$tvbWGBgdq&PgR?M8`Bwnf0p|&p=J`cA zJ@#P!H&P(o(~x%KUIFHZdocgIH?suu!(1xfEc$UOUha@_NI#^bPx(8>{C3=r{_n+m znUwO)#=MSn&Ujb#g18Uym%NC-;$Con3hu7V{@Tv)m%5PthxUTH!dIAA&|0R%uN3oL&hPA~jW!b_zrD~Cd)o(NHhG|8 z?-ih`7IwI_lSNAYEswM_8|WJzHRmrala{t#R~bV2PI62(EZEtFbm3){_ZQ3#D+%WR zAd&J4CQVMBmk-3>ksb2EvTwb?{6*_!`D7gYlLP5K@#Xyp@}4GR*%=KlD`30bh?@goApH>=1~eZ~1E6{4KB8BFMn5W-5{TFpAm?$Mmieo^{}zTit%Zs&F;!UU zzn|&lwI28Hjqx6GE1(dsB3XeJe4JbkOv95&NKg)5M1}<=F@w9v%%GkzXb5K0Ztw+t zhX1&?kqhK>3!XaEt69%s{y5ngG#Q^Gir^&tGsy^^212$3Cn<<&czu+b%Vy z;GYqtC3FBWf9)`pc%>S%=V5X|TPok&E8_w<^r=68-HY_49lBST)R>OTt8r>nhZ5R# zIb>+Uewa!3JN!C(Ag{2`Z5ERa5XOF;y^xj*fwmQd>hOB&0xcqc2qAGEurNo>6A94G^#$NM7LOxc~tyqBogtU%KMFT zkBWD)D(@qpl75x<4L~Ea5wGxG(xz~9*7x^fb#5zCyX>1kF8b1xjMNbbsT<3YdO!>2 zmvu!~Fgm1-D%CHUHXG|AOB8$sOxTE0kUa{=lJN3~5=0`({dwLUULHL^#1dNK&xd*x zyo<~V*WeGx+VB`fvkI|v5FU`0@bSHx+*`$fOq6Qi-)@8tfv z@P1;B+!Q^tiNM!MaIuOg*U0}5E4hw)dns#+8!is3B(n^#eW z)QZv&ez}9ri%7Scla|SSA9o19z((hW3HtHdCy}}}1hHR|V^O+st&73jMOGN&58kc5^1Zus4ieQ~q0B#_T`m~7dWiL3DL z8dD%|5fe><9q$h08IcT%j&I0x?6y_jKQ^aTWL20?@=7^<^?$PO{CDE2Ey*~1HBop-eQ2c z^sb9qQNN~3Q!)S0sc6~`^sbX_`pKSGDr-`k{P%1KvzO~70@IYVh2fyiDL{dQWDc2Zzl~gL-8A=EoN}o zRi~7bag%gd8`$ZGGhyUkB*C#sdH)znEut5OL9J$arBlYKEJ%a-8{$Z9uTnkNtMXTI zcO&MbyLxD-o!sf@rhCYY*y0B0m##tD0xq9BdINE&$?#BA1@T`y4R_15unu7f%w(fi zJymTvF-XNO5INRbC%wB}TT3|KggSrCv2yZrM zwzW%HuC5-WRg}^?(7GUgozpL=e2~_!w7U8+>oq5{SJjHzYGM$(4us{!A@=Pd zitZ28L41XS7~=;eV}7tGi_iEaoR{FBcD#AL(f`b`PAS7Q!$x7QflDdEd>@g#7hfMo z+zdD0hIPu)T<#xYR;E|bd!N^`r3R?(Q<9Ys*038iJPzmpXUF7zE{ItN2n~ZGut;lC*88n;h5#{TEzd418-t{HhQd=j?)*! zo{-QG>sT*lX8LhkEG61VpF;V*sT2$y3BDN7a95|5 z!KA{VSdBY%D&BpsHhm)ECzD<3V*C-gseUQ=q&KrsZ|b*lnUTE^$hVT&nmF7@-qb`! z2l4nILbCFnCRusR$}K&Y8JP>JqCkEFY14#K9UNR9*b3ZuL%bWZ^Pg8Olr?ok?@ zmAj{6)-(aB_X4dShLG4Ydq2`)bx$rDRum6d&PK2u8@)(h+>VlTWsSWm@3&7MoE%oC zJ9I`=HDvi8(wStPsAf~O&U7B|9N>hAj%rN)ra~S|{Htk5$HzYk{p9C5t6SKZQ)7~M zN$#~gKnecU=R2r^tVm-auuWBt@UOs7ZL6o>%AH2p1-(gKgtG&Ar~9ED0pH@O*)M|f zNZo)&{Q*5hdnxUPSQyAjIizWVnP<5Ba;R5`^y)aZ7J-qFc540n`Prf_j?t3I{r3#1Gz^op8QEKz4Qyk z{h)}FcY2`!-ph^D>)S@E{W$4Oq}Wj$*df39HS)_m%C=q?mnm-g`6|Ezl>BAy8N2ju zzpw(IB5Wti;%p^<-h0K~ZTeW~TeVil;U-)S+bETiKi%MkNw|@AsgnP~TMJ>@p-TRU zcc(9$1mS~Tyzy~|DtU|7PfZOX9j+j-{zwzaWpWF(Ucm^~Py&OM394`NPK%XkCBE=R z@AUatm(WMwmXLS>Njfo$^LH9!zsK2E_BC8VLv0$)FIxuP>8GVdmw4BU8nm680Ii+? zt(HNnE1=cVzqDF*lZ95(+*7@U+*!Mpvv?O(JZno~CLL7us>SnH=j1XwyHFZZ0aSwVLeMivnr7nrEzU2f}qRqCbl z1Gqym`{om)>U+QJ)P~)wudr6za^pIf$5?&FzGpR?_!FE@eqycl*X+(%jc*tb>=xK; zIy@KLzuMJ3quQ|gu>G-O4OFw!f6S&skq+rhn;TE=x0vr`X6lrtxOM<=^s_L+;ha1nM|h}^n|bag`OTF8;p9L zg!yn%-N+p*hWU@>=jAs~B3c!{M4x=ijt9`5yBw|LYdyFA-M!h+yVeIw8y=55)~S5; zpbbhs%>A>?+7PSIsveDWG>o*uJ$I$`L1Tm6H`a4j{@)#}g0WV?SkG;|YR`}RPX~98 z^;Os7u|8qXY*7}m6>E)w?I9x0A7@~7hI>eg^ZOXWc+!-T9QT7L<(dvDllqoT<5Z?9 z(^O7!g4Jb|rj@{oOukQPE_&U7*`K{};G|&K^$>1?FpE5bUyI-0%1BFPQ08e-;@ftk zw!fAm>w0J68ta$6=VBa8^7?cz?{|@av?4g(R;9(^i)2<>od3M8?RAB*xLwZI6LVTz z!dN%Xq%*dAACJ^sFk1|egK1amMV>OXB=0+f>ns(oW2nb=3d_$4Wrxy z!+Xk`X$$77J^ngLHEdyXjF)!RsdUm$R9c2ii>>4l>>Sy)$#q^Kxr^Cjgo&QVeRl)Z zvQWb_U9$MZU$k-Tr~Np#ujHFUMx)3EfSrzw=N@$Iti%0c3ffgV$%lDdb)AIB`ExF$ z|F)wR7A!BrZh`-j%|>O%yC%h!%|>}gn_V|Q#-?lVx5#WsFTvrO?qrsb!|8o+ExC~% zBlY7m9fZy3gA+(jMkW3SS&@-}ebfj>2cQCK;OpZr7S$5Efsa#+w z-n~W)&qO4cuk6Gt<9A^z=LROhCC>qFz zS)*|^@n+5QG1vmKpzl}A!7;?xZ$l8xtuDaqVA$Iq9AmxKkHhjo5RW&KzB$_X6wzRT zCM+plQaD#S7xvoyvF^l@`c|5yx`52ddGT@X+c|INZuZ`><5Z?Y!}+TPdyGxZL0@^V z+6Tf?9g{~k>r^t`+~m24jLiu~Xh%z}XU!MRvZ#tv>)ep!fEnm)adD2Gh&33?l%M+?c! zA^(~?1`})X3U4}LW38rM7eBs^+!~UCOGxCkA$%BAj8MFL<`d`l>;WBWTU8BFsh=V|FB<4vA6OkMD%|c^eDy z&wcS5Uk)QfhF_Eq0(MOa(N?_T3$837?^lfQ1>Y+nT@{}zX@>iNZ%fFL z5r6oiJxZa9QtQ6TE~Y0<<{Wfx1!IzC`k*&0aUHUq8G%_XU^HMF;59%q09RsmDBzn) z;xV1U8Kk4GEtD3j0OqrR4ZcL{yHginAOBB?2aSZu1sFV%q)+$sq}hidV8n`chw*1d{!%{=1^{#<1}Giw{hGsx`O z)(9%x1;Toaf~G@<(?O~lMF!5f%?unxde2Swm6EKQG{K#nYa1IEAN6&wDX_%$Tq zvDUN(9S-UP5}p7&2Y_Yl*~a&=@^uKCpU3R@86;@wKHNZ#Esc>M2IloKYU?X~1sqIew4x9huoGH*)wPdu3x1;4{11BZ)* z|9~{F8IVRLya)IU-~_}&xBmcisGe<+otKw0SaK7owv5x+6#O(2_(s3z=iE|moJGQq z^&tJ7b_HMhNM0ow|3*Ki!(n~n4K4O*7BG(Ne^V=u9S3`cvE<^L<5Vt@I%{a1*}yCq zN4TbR*?_|mzJk2mlr~s&NjY$Hn_}imcwVAPm4f&l?&n}{c*Cyh3=WobDEI((gzlkT z!iTz%{vxDH!Rtetqa>G`dddEDD&Tr4cx@$aC(o@74><-~zBQ1`3X=7Z%SKkLEl_2P zQ@|%g)ff$GOd3aiSescqAF|$ttWzQDEIt@X_*pJ>^;LVVsd^U50e3(kZQ}zHNUCVd;J=Po$WwS330zm99^s?|Tkt5? zFsGjGrLz}5YU8?bWWl=3uw)QM$14gd?QUNjXx%h3;CdCB>{~>vEeJ{L$99W+e`IHA~!ghZr7Bzl%Be)sy zPFEDPWj?vLei$B2@;A)IJ*}T^n1_StaM@tEwgWoGk-i%fF=rjS@iko0C`zmI>|u$B zmvsqtn(t4@(+D3m-CnU=2-h=S_M?KIF2?HM4z*fwiRq*0@CaSd6H(B-YT!$%>9q%3 zt&xn)G4P$2Z{7sv`s(IXdDM8Y`{PN(7B$W#1GXe&X8WA&)JOF?ET1&_X~0@BEi;Ud z3hvRylrd#q;X82f?b-@gjSb}0E%ALZd^Ob;tizn7{^$)nSyqDhXB>J9_&3y;_qd>A zGwc5*lKik`2<&*n-+m&3)-fH>2=%=QbnzCrh8<5{dHYG&lAU|IoB;!=-THj;QAoQE zkeHAL+=|0k{^uj~Dq6;|>Xlq1+y+vIF8O4uHtn0PC|e2`70esEVEBAzojzc>nG)&{ zZ}+mGOA}OwAqOGut&#l@zZ<4QJ2>m54}xAo514wC1lNNO8fWV<{~LsI!G-<{xMjb% zfyQ9|buw&QLcj}faYQ$@h+kqObGB*2o^hPLfh4H&6=t(h_9JZl_@~JS+tRX%9W-oo zAoW%Ia8P9h02VkCcmy5dU`gRDi)-#b`8%-Xm^E!DjTr4aJ^g|(mujid3}>gl4et1CgTyr9%jp&K$!hDw544y`tRb)Np2@sdO&;!Et}u!cO}JXh z4iIQsf?%|UeK=l*Kef*IbSqA}beM%RG8{xat=Cxt!=FF=_*0z4Qs#yQgRUd(+kc(P02cjmjuAxlEV{eNecmecG8QJOF1Q()dh$F0Q7kT+%`n5oR% z7Bw$HUg4v1P{`xE!6~xr^pmyAK*Z~_r68ipt%sBsK|pRREjS1YK5j~YUUptm{)DK` zj9Tdx&ciIOAGU5HIo>h^TsC-rI1VC_`!#`E9Y4Zdyll6OxP)V*cz;q4k4q2zcFHTK zcJCi2K_owsyt3bjuaaZ?V?s!qtW$;6Tte3hQMOhPo+G#R$Bg~eORw-RwHsH#r4YD| zBxh*n&+-1tUd8q145o*|a(9YvKnz~JC8=z`CJFRk;JRto09O>&IFSA-Or2~fHP%b- zsGc_oLNa;oKvnOjTyNObz<0x@>YH{++1DBwlgwPcL#DHZmc6G5gL1z)YwL8D;Ij8MArSk~dD$fG43O(=%Ccrn zAaFXInb4dk=5%*Ov6INe&$WR9d`RmY>`2`lkKiC{KYs#C$kop^SWWy6WiiF4$-qM+ zbQW+hN3{obN-*At_!5`l5Z)5M$|KZu)lS_KQ#;WkOecE|Bi-!}kzVa-y=Rs-BEt5R(XfU~%!0XXBxvessJ7L;~GhdW8tk!XB{OgoYV z8@;VZ%(#l=v}H6HU?|4I4(Ae3Hz0B{xFrD70qbZOR9@v%xenyMpJf8M6{7CZrdrls z3)~N~CIQzd*3OqgL!N_%Or2c~oIH^#{uJbB&UyyoSt4!NScrd^H38x&;$?EVEw5(1 zH+3;i4d_7NAQy!5-d=}{Tor7wVM%dnE)C=E*guDRE5G*c^jE#z^b0S&&4SlzzEW$w zDC2TF0^fWH$TQ4i;u=J$U)wQ^Y>12>u6-1`B7mNyb=(ETrY-#L8XE) z4GIJTE%s_a0YM1#UbK(kezwzYIttzK9pEM)`1$Vc(gOYK2veh_xAlZ3&IpMvc_`G9 z8ONTBdECjlEpWKzOtuK&&$OxeMbjp_h5Ka7@m}K(dQjRw z+ozch8N03M+q0D*@ncV)Ixt8G_4%#?rJv|PiEX~dl|VzL0_K}Ay9($IfC$gNXmf@_ za}I?Jh0mB=0r5SQ_49}5Zvgd#(`cx_CRjoWwPQ#3O15YPw3(@JxB-lBokqq zWN-^9M{+ERvc+I9Q1U}TOYWSA&I&&wwm$9{6Vys)Upv%#6mSEO+d`j)X>9&wD>VL4 zx0~dB(JOsU)rDcdtl{Kp zl;{=e#kfl}KS_=Y$i{T-kTX^4vGs9uS=~fHlVUMM&sjaKEKiJ$F9@ZC=O#{z2+=7QJ{3@Y%QCAj6V}HQO4q5AA zSC0~LlOUA2!1QSZVZQ)o$Sk`Sq(fCGC3Jl%J;% z`PXXvD(Urg2A)aAew~^!&gB*cI@KUNAk!@jg;=UnXL@S>GUz|U-FFK@;UQqX;UTzi z_k5ibsVYy{7()JT?j|V^A@#90+UMxg0PcJIH^v39%Mz8jx!0u z9_LokFH<+0fhDGghAnanhr8B@euy4%>1TV0>^iAQ+zskUes?yEIDVK_&V*G_4|3Y~5t zsq1mWdP6LNw4I7nr^DwdMe5nA)>*s~!L3yr6-yg5sJl@QPmNFtpLerIn#hRL!|}Id z%juN<2fEzCCdXrm+aR{uLAwg2(ly&~_cZKS=)`#_y!-iMnb1jHr+bDpf@o@Jsx8Q* zwmcMOk)&@j0#5?>3jo{V7FJkmzL|msU_)qfGzjSyXY53O+Jciu*S52*>@x;C5lEPvG?`&i??^mQQM7>9~ zNAOr)(;-PU+`ZQ**^&naBLE$$mTqxYJ6At{Pa*jv&bhQNm%31r|I# z3h&?mkMmbPD(mcA>;F(H=6@-LSfzHAU8Vhp{aX|H@#6b1OP2ha1dGH6ziODXChO^6 z|6rIuO=SP?7lBweTplH5{;ae9VLyv;f6{g%4JTM{-N?r{i=^C)DYYI#2u|Pq0x}u? zV+a=kU2(XZN5c&erfEyM^JUTS!bkb4y7Tp=`O3+fn;AHawBD@3Uyl8~qn`Njr;B(mYz^mp>c)zj2PU;AZNi3MEr+l`5QFu;FWF_#4D_ zYg)<;IH!4q&tUDN2fkdmlb{Fr{fDv60B214%_1=~|DK>B6}J=oLg4g<`NL%H?WAa@ zV|!hc??(5Y@W7D;n-6L!;mIPg+>Z3W{K)Tt`FQgEZ4KOxJh-jH77~3Y&i^AwTQ1g* z#QYsnaA%6FVq#)N(1np(I0rc|+%D=Q8>!>BQzL2fvo!Y`3x*FtT;G;mkL&mE3}()BHy;cX-f z59u+uCXb7F_g&$iUF3I{M_vud%cqiSZeyeYf@Od)z7o{q7|7if)MTh9Jn{h?`tHHK zK0I!Nr!@R9x<9%rD9JQWv1-ear1Z=VKX}fn2i{Q!IqK=dG&{&`Pv4+Epaqqe4K!$o zHQmc$T(f>2UdA|F>$~uByx-h_@ux$tgHI0W^E%Z1@#gL6_c}_U4=(z6869|YDVG2P z@&m-H4?TLD`6fy^)q$Z#2CfrMnP)IK3-2|*!{A6&r*PBzot@!O`i728_L6DluNa&j z)+v1JmESB<;q2aZW5j?2%$v-95}eRuob8L7Ur6!b%0JFZZ+FmhBNQ1Z!94>@3cW&W z8@N@#M|R9SUIMhxf;aJ$(*6er=2%~_$?fftxl!|)SWYi|oyY zn1Akp*WvT#+2OcSIRIQW=RyCD2=nQ1oQxC9o#8kod^kM$Q^1q%Q=KSC%O#p7!8vB8 zIU@qUg!`I5h`?3&7qcS*KZh@v$4BA;!LvlH1NSA@C?>(Su-^PmBxqA*{xTBh<2W;p z!bv@bi1f(|wG?Ne&%Oox7Ik1?6i&zCXgatZ;JJ1Jf>&QHi2n&TT`A_p(RhGjt&2Lh zJVJ=MH5#wNw+{^Ki6b#ydEmJi{3XVx%vEtX%$yqsRaj{_6>sfkHBr_-y2FQQc((am z9G;AanDgWD>;~Bpa8W_KX{RD^&C^<(LRahj`IP4sK6ZC=I>9R{*RU-t=Mq-9nk}eS7bqT``6eqx^glC$ks4>Bb2b^jQe>)GjHCTqR+APuG@l5s%^Rqg96u)&K zArXUR`&n~gFFXbp zm^bypwZY|%gZs8ale8pDd!W1soZ)C+hvS-C_!KVO z)VA%_m+fw0o=ddvs#e4MV=9r#%Z1a-MtAbuvv#^D!}}!DO1KHAhPN|!1UWr$jK-(TS1#(zyVbGBw9}`E9u!&Rnzm5wQ)HS;4~p!D+dsFE z1&VxBoo9-+XyNwn4mi*B;7y(zqa`)iq(g-WneUO4@f$zs)-kbpscsH95n7u3iFu$ zU>R?jclF2ppxGDt;{mb%$wW_Ca`?s1$Sw>YoXm-kId%Z_R*m`N0oeXmY1?wK7U!6c z<>JWbmj_{XE8rZ!0l=m3YYMp9X)$-^;;>Q+IC5YnYT$)jL_p^DYFVd=xh=5Zt*&py zU_DwOYdb6?G;H}xxEaD7kNCT2*j$l^r^%m&k(`%jK9YyW;nn8IfjEtMdZw8hh!ele&b> z2hEf6@n&o=d-HJu&Ne3u0yC0iE}$fe#=^~S55hrAm)EQwgnRfR#w%WU%Gr@>l2~-r zxvghGS*~u*jF)QWOjZ}qnKNt7Ky}h2CHgn7vK$!GC#82|z7nR-fy0CF^6(y$D-e5K zg3!k+x@o~e_|ay20T`zGQTUSl0NiZ4WTEu#$t6R`31~ew^4z)!<~N?h#v#!kB6ioi zh@EUfZ1_&ZY5;j3Aa>Jxi2VilzX5E39e@Y#!!OW)?t&jEcK|*0zB#BG_s+BIgP#lo z1_M?AZUBk_y8v{a)4wqgo&jjt2ESbcO7Gt+2WD5}V4S@YJn*jrngH_wlK>{bbU-6u zIiMP_2v84r0npt-xEfh5Q&NW)b$q>GGCw;7KOfq139)Mcg8}0K^DZ6uVG0hy{a5^n z*x7)O0h<830BZq@0d)WyfGUgWKlrQ71oz2QbI&?FP`>LnbkH61ggPA0Jafmqpbl#z zEFQ!jb0hW{pap=Ba zQ-nrBc$pNTr+|(GS_gDyG$bBH8)z2x5rHJ62RmTyn2w`*EC>5WQ~&xM)0C0_51V@> AqW}N^ delta 15716 zcmb7r2Ut|c7XQrFyGvU@nzSt-3y3U5MbSv}N>Kr$Mokf{3pVV=62OvJVxo?^)V7 zTZkD3conc0Z~;IyIP1HpkGYHf6uI%89C3CO-k2QbXL~9{1<%r`py1eFKo8(mzz2X< zz)yEJ_Uf**0T=r#VoHC7kT}vGlR=`F#R?A{D{hFQ;k_HPyp?;Uh*=YgnDu+e@1l9K zLYqIgvXe{_v$6@m{@O|E#XS-W8)^HI4uBBD{E2QM3_Qf`pNSb7e+O_E@FT&Je)5%_ zE~_6mjATm=R0|E5OLJlFth+}(=6X5&IG0<_s<;_~ls$1wTl8*D?-ra!|HpA|QFPIc zoV3#T%OY2kvCsreFtI&h(ZSDrS#VsgC#XSdMr1s;k zyM4`w+bv*jGUUANR*Z&1|6y0Nh%2=rZlhqc_F_@YWvhh6JzA|YHoJXw`UNYkI!5w( zXkC4e{)&2412a_8#vmmZ;gYna0}XYlSxKR!py{qED}b9!9!rP#)pjCXP+{?-#j^wR z{kSSpBpW8ONy!dbe_Tzj$OcM>y8XCUEFtpA*st7%bf0--KLObivW}TiJy?O5833$A z%qr$D5C~87M$9rV#QY491D^z-GJ~E{Gtl24FO7Q)d@1nH!Zgp{_5Cj%%Csz0e2r;^ zpYu8-y5_Px@k+org=lCh;I_kCWC!@;{{}iZ*0^zHlN2O%bM}8DBeMS@STkJk%4}(cnX>5Hx0i|zW0k&MB4V`$bM3C zV~Nu*87Gk>RZ@6Q+ww59IfKn^Lt;^#g1aN^$&vn~XT3QeGG7%P?Gyy-ygo>smE(u@ z18pop>Z%4Gwi_D-GTn(q^@Hdg2YJ&!RN=+_%PrrJITQKRKR$b{L(Teg3muYtI$oGt zew=1+8IPH(Fin!~am8=fOIdFgeeT80aU*>}tL`;XVtDKAwKy@fRmnli0|w!3?p0^Cx@8K3e*qHk zfZ|6rb5962N=^t5Q;HuU;KgahkFtPAdXg#_EhFq;^zlWA#G@ZzINJdtP9Cka6iCo3NC;WM&Jk%R1}<#NN|xCx7Zm} z6t>9U9FXtD?Q$x(*U7#hO>CUNnEiB#wc>h16CJ2$y*VBD`6|TF9zIF#1&xjC?%XYS z8&F~vx2s0YdU1tL8H@DUtuu=ioZgjb+?JDhNx|tzMexF6tFySUcvztERCmkc#nthb zRAmb8XQznuf<}397oGHfo8zuIak27qb~sAx)-2YEbaQ7bxN}bUWaeURKepc9!5PX7 z&46zCH>5W%C3PVhoJG!uY!7R5jMGa#Ddw<9-#kvOVb=?vBHez( zWs_y0eY5>JrA^JMnp6#ne8o_Po90#&BDJExpA!T%tKjy+Y>*VnJm0QA_uvSf3r6VS z??xl_8h^xWB#%RNW413vOi&nNWXc(HqZ`D=P$TAi94c)Ei@9(+W<~jKeu-OJ|FR&l z|2P0M(GA}0(Dss4T&K+U;f@N%#&q+9HF$ry(TD3R#29%i-tWz=c9W{GsDwkp{>I`* zC+9Q?j0$s~x+JW=>@V^7e~W+NCP%`ehNRmjfss4l&=`yJd!%5_*Irhce{2}ovJ0J< ztFy~D6aoJY@cZn{h63PE0>9g?*q}2i>)8ArX-eY56_Cj8A=Ui^5nE->f?3nk0bc^- zy;X_1*Fbg;(8ZAdhP7XTbc5O`=LQSX9eoQZmnHNm2;QGOoZ-gWgZDGsEB0c}orbmE zbrJhhiha1X4w25kE;+wD`?GMm#)n((5UYJS;s7aw5BH7ZE4h^QzhYp0)Lf>ZF>Vx+ z45vD3#M^S@Ee4A&pWzk>8deK*9=X(AQxR*ge~f0~mi;->R*lhX+j--*AxLj+@!E#D zjn3QF43^u|ci0zmA z94zDa+}RRVtyXb8-BMO%MjFhm5=yjj1$s8F_-A2%4d(XRx@q)~EAc5!BYPtTSF5-V z*8puco5`NIhgh^B>xrQ9tIeC+aqb^MFA}XZT97Y?ef^7 z0a|%MS=qBFAG@et)r6Xp!Vyyf%A}Eq88eWqjT}%=<4R;xS+FRx)hynk;4(TjtQ2N^ zg3}0}=MtB=zUgjNmbkuZ$j{LA)Pl;{Palhytp;O^VrjpL>aw6lJ&9k`%gP^MaYt%j z?vg+jL=BK2?wXq%ii!f}TQ|8HRfPwWg6Qz-BDW8yfaT+zGVr7;6I75BbtzuDiQ;y!k2SP`p- z96@dx|C$r^ENG#*{)B~G3XS2iR1w_Vrf_YdN5yg5{>H3!TY$7J-7_kah&wt+-r=I7 zQAWbm(be_B7QtYUvpdcC+7M3#esEFM1Drk0RPiUESjF{r(y98sAg$+`#M{AQIH4!F zZ8j0xW$l|>dXUmOe}VC3U~c6DN&pi8uL0HqwgBeB__o|b*v(IO#~NX5)$C29ijz6B z+I3nfOdSjrb2<_$@&1m9y(SPC90udS#radgm7(^~=m*Vc0VqGLwUa@Of zzkF}bX(tO})YYAi%fbfrMM09G<3!@GfYQ0SxdJoBaY(%jDAWgW4SVZnA3{2;Mlm%# zy0c-huGG4VAGJ)?6b_6Htk501D5z4rj){$uHdMa7Ol&+Qc(;g+uK*WahK*jvTn>x9 zJe-b;ddl69JE|{>m(-+Z8>OA%4(OU_ud=JHUfgCk<^5q(sT}S#41-N8aHXx@mm|HV zH>&e+I&_Qki4{ToKv(%84U95ughxE=R;{VlvnP5exlZ@QNU>H6$x7~#dmr$z9{zzF41`#v)q6r#2&u%3Hp3IT z1tEV3WqCr^AQS+hd{5{igu02<+7eIbJcL5US=uU3=rn}XAX!gC)m#$p}W=05azPxYf#6enp*Q%c%WK{5qerR4UyHw&qT;@`hfyqlwV{QiQV zOGny}AJdw`D%y*KC6Bkym4gl?x85DcW`={ThU9o(;v!El@Y%hgyf61?is~w6iTkQm z-kuf+g+nH$4=sAO`?i&|^a!q==Lb;9|0Cl4C7#cnq|1BMM$+`51;DBx*w6a4KtOCaORGj zfUAQVHLN^c+NP-%%e7Qo<4$Imgl`cvYTA%nT@9&~-e;g88c?s~RPJ2%N2|OIbV54} zER#;#A%%7z9pB)~N~+~rv@r}Sq~yMI&W*rYxdstEi*!ovkn?n-1**fqFd(91ePWc{ zC(gd?vGD4Hi%5?DC3$*c_5QVYm}8AAb2h{0ht*=3cL&xoF*gvCd4sW3-L={yXu(tg z(sdQS&Uw?S?jG8V)z*Jkg86qPmO>>?H$oT+RK@sv#&wPp>&8dSfnIo4>i@IYxm8wc zN7!G*!caJw^i1}rzlucyv|UP<2u&^Yiksi)OoVE74Z=d#(ni0SX%TX0Iqkz)E?PVt z1d=21^m4V(#Kb(Z-uDzjMP6~y0{*a~zbXEQNAYA(tbeMw2oxJ)z(~UI=sl`)Tyy{v z7C%!gH0fg=!*D77W4MxCe^~hjNwqJFHe#Jhi>ftryzV;a_$aihr{fa_<8#x|mz}fv zSon?{|EvGE;Zecxs9<>J*4(vbM*h`LUBlDl_-l9!4az)b*v1^67JmU}j>%$lSsh3M z{qRykLgaBl!frUGj7=?^#;S~^#!6OviqRz(B;~8P&&a-ny3qVVnEBBi31@Q=QwsbB z;2GrN$%aG&9%4C}I2Yr&q<2!QNNFcbuRJ`T4C@t%&yi`pBICk|a*tZ4iB-*Se?%84@K7cmKxLjg}JR`~Xy-KU=?72~6a>isc@S%mWcB`RbUA+%j zW(h0aEc-lY^fv%nC7wyOmvZmFgh7m+9{1< ze$*`rZ`4(LnWc@f3-zoQoK*Hz;51u{N+L14n^ur;dYi za_+XiXO!>Uk#nc@9Lx%GE4e3LNPG<85_~J#M#Nc~vT4ze+X*b}@e37S|^^4_jdYx`ouT z`l_ZfSf7~&FBY>HHn81nFF~J!n*$sqI4uj^IOv<$+3LeBwJc1*ST>yw$RP4sYN3by zVi5U9>PTEgbZPTF5vK={chb&?=8Ys%(zp6j=^p`MDR3?uX;JoOu}pyQ^(->Fk2We% z$TB0Hw03Z9?p(=S*i-hwx>L1PO;jWzi)`();F)wl-(4BI+z+idQPiqoz0|xlyiv`{ zxm5RW*8Z?E`KRQ==Xeq(VV8Q$uvwTf0H$V8Ve8HtFAgxROfbL(2SY zvZ?|iHCk15n{{wp^f z=?(~AZDb&P)Dyl>Y&(O}kcac}^Z$T=jCyKK)Mv@)lm z0(StRFy;|}mjG(OEfzB|K%IG({zH06QmL!~!l8pPlMSc@I00J$-vT(0Pon975y0cwJe!1?50eBVB`?O4u8$V<_#g|h7b3!WkX0*@lhoes2_rv z)kDay#lL%kEfBmn#PZLQPEmK7EXv1B06+s60IE}fPR+Oc{`^5vFiqbJ;Sz`+383lC z`IdvDbKpaRw3K|(`NAuBy=Cl};;xuG`Q)>)QCbIN@-Be>0JMPPpb9b?0b>BKK%At| z@?h*cqTs&3Q$?$Reh#4RzFuH?@1+EBfTw^$n0ydd3Uq3r<>vTJB2U4K3(0~BQKD^y zWb1@4WnVz*^+Gak;_FJvq9X7&6p`O2rsA3-AhPR)kP1xt;F)A>K%+{ku7s9!BqK? z5*R}OozrwId`ihHb03JFl#t=`dU^7wOUd$i2_81DlpLJLdf2j3;+l6fh^kmx3da+$ z5=yl%8^lVuT5@6j5ZNAx{ic+3U(iS7oJU41NcF@>Mv%1&`inl8PtGhzrKEE~f^5JD z_)g}LgoUXXoDvJ?$EgMMDQ4&g>?RMFy#Q9Bkg%veW#gQQX+6eMw;Td1b zz6sa?I6uO2ZjlHpX`J`-n9)6N`RA*l7(2+r#gQ@^JNKHJDW5{z$9(ZIo zfb6X?mOe|PFg{C0ElY%vs$CWrKy#dcG+JcU2&DeU^OmE_0&wuNGTXtp4}F?`bqx9S zwYRXC%&u<4$4O#MEFNnqtI_!W~eGk7{lU2#xb+Zf*{ zg}FG`Uz*>^*SmDa#IlEQCSC-EE&|c_M-th}5ir}wuACK2BTfLm1Iz;fJJ6dW$<38p zm5oOr4+LR>d~H<^8D-U@$j7Tz>xYA-8`D_eaRQXMYX{HyQLGkoxe$x=(_12_1&p-u zac;}F*LPril01IH?J1~m6c`vlPe3i;tx-g|W{yYm`B7x~nm@3C%v!q}_a;8;(tK%w ze}I(tXqc}n$?$crd)Az!wdDG`?xL}4$&+<6Wf72=J=#+FW~wMf?o!+m*JT(*+zz*d ztvB~JhjQ4DYtgeFuf*G8uHI!ZD>$=D3Z7IkS4S?q)i;T1C<9ah4g;>kIBWtsNY6A# zugZ!1>|?5Wwe zqsjFRTG>@l7crWMHjYuvvr+d2Et3N{N0Z8py`=q)i@9R5dt=hTPKSgA$F4Dak(lGe zszeE>k8zHN&E7q$s?E<&+^XOroWZ&$Rx#JjiS#!iy&L#JXmh-Yd{dnCfnC9MlC(|5 zaTo1U_FHpAKGvZ75*77!=t1ffZ1vVdu2WDF4Id+Qo3d4(2vcE(&}yk(fMI@tbZ#;P zE(XyDAi5YtXK{W=%*`ePNl17#IP{SIdW)DV1BuHZQRe8X!Wc4(B!=ms;Ckqv=OFcW zNPX^UQGJMo#K+P?GN6zQ@I1NsZBThtBXw_CKX#F zlfJSkxPNqpKn)*S_muw$Wo&s1E|b80m!;oQzn0y|Zeyd~k!(<0Q*cL#WosP%klffB zi!Tvu3c{;Nh-nC(M8=!u;snbj(|qhpJ7zv~%u9gvFObpOWAJFplI`no{02c%*3TLk zgE(m?Z>4hkL8b;iSGt>GSbw&Al=QlSTS}65hQwD15l|_5;81XtLZ$8xE7KtMNQn}~ ztxb2CEEhlEn1XwW%-XpfOt)fJqUf4M}6T9Jm)~0r#@x zvF}3kSX6~#F?ZKS!%P+CZUdJAF4muI8SLqMXcXq&BGcZD_Fe#&A9Q=laDiWvt?z0B z$J;L7Lt@nSnrKIk^g8uQze3Kxo0KxdM)^D&QvYUU#)3%#z#3)*kJV$}1=Q!=i{?xv z;Do24WD>t8MILOUd>|>`(>Lv*(3Kw8N;Pr95OX%04;;1`Jf8<$t!Vg$K+~@ZR_kbz zvL}e#+S4uRtS9b_fYdksR=Si8g%uR+445mmIVB4G72sWW6V2XX8itF5SjG%D9l{r# zE4iL!S&FP2xGO`LQ#N8wMBd&TR$U4!z<%gRRfBRU_^!E+;Le8`swh+xFx+Ff`N7;f z5F;&=dhUN1?*8#EdD{8(256%Xg8jZh=gqF@@Lvs`4fh06uEwT=J0mswONEqUoJdF0 zR8431z`+_EYA4HAlp~nG;GiUN?*$oc4!pffUdr8bbV*Y$6qSctsR9Wr@P0PESq^?< zfmhn}jaV%$>ZTRhkpY~Zp1Z#i7!B(EHIAp}4{COPb*s!PjJh&Z0Wyd>LoIvWQ%iIk zKmsARJg@;ht{I6pmQ5GOOisn`o(z5^8H#YQk9Vp>t>2Jj3>b#uT(rIh&4fK3D=+O`?%I`qTto0@}llbBg^wg zENKVEO7Tg{`ug2C_Q&H49D3n+;i8|bh`W6J*=IqkDlsq(67CTxJ{S^w+=kVvCW%49 zU28-L;(>E@lBEZ`2W#yT?hM?c)Ba`P22TPfm)9NSlY>$IuFewk`{9pTVeoE&^OSo+ zZXApr#{FAZQbfhG1WBfZi}hRpqUM*%U?={!_;nti25QcLco8?@OVqHl{$Yor)~KcJ zSxSajVkJCp6iAgNBBT|bE#RUtl~Da=OP)PMoXS;PC z)MzV!wjsY!QTQ*7BA@5qf_qc?ElarY=2H&@|*>%kkg#f z7CAgBI5b-hd8!IgxInf*r z!rr9sVU5pD+jY1@mhP7l2j5I4ACB$jaOk1mzH!T_4SeGZQ2ApydFOBrzD<5P9PYod zS=y$;YBsual_1^7^Wcv;68^$ZZhG~6t0iX*_2ccx5nrU8U*-P6TFNG{S;i-Pr!(F& zAkVq==GekRnpmj+BFB9zb;t_*Ivdje22&>;a*cA+yO$|Oo{u5bM@kdMI^MFXA>IyK zqw_7|!f!NE$eZq10m~z;9y0-szF-*!Ody>{)c64jZ%h%Do+HB>4b^w;x(aZ`OLdI_ zh3{(uA>YsTi4{7tU*U%ue+XT*--e3_Z<(%9Sy-p>ftc^@2I$ikJMGz@;8qRA$l#-S z3vP=T@Fk(<`KJ+61to3)90HsN`~g4>Yw9!1@jA4j0+x~Wg2LeD*AQo;T%`Dh$d0li z+gURFXfMxpv+WzQ{HRtG_AUAFXoBd^Z^^}@Tf293UbVv3L2?EDOC=Yr*=M^7VkVO1 z$Fx3=;X~SNV@h;$Ts%+A$DYTSNRMl96p1^YB8ojr#vLE7``m^Np)EdbVvM&TF5i)L z4DXCuGJuO#P>_r_*|n=a9U|Jf8%oo>V16a@mG0h9q+2vnq%0k_O1 zD*(Sey+gk*@FN?uze3EFlZbg4AU#DlCPJ|vxDoY@8{urQuzUmb-wNBrML1E~@+QpE z@5|Ri%wpT^%EZRiAn|JXS_sdvO*AGpu7vR7@;CDN1ilgkF5Q!+!t!^i8lsn!F9)&l z5W90tV&h_nSyo;RF{8=8FY92hGpt#MAClS4Vfa_Ft~mwQlBQ-8E+(T+Bv<#Gh?zjx z9()J%9zZ+^ymNptfEoa;@)}Q-D?smuDMrv+Bm+~@%lLgwa7a;s`$~Xwe3Ac&(WWRFaPF#i)y;~7@pn3kE?zm$) zY$LmVx(Co?LIcQS$8;1Xl$9pyR|-nH{j* z7WfNf_sK|+OCZNjmf*d_=c|y4El)G6;EtQ;`?xV=^ey1|3drCCG}oZfolwU|bI2{j9TE0K6i|pk>Iz4-P0&JPGoP6Auz0HckWneH+ zk0-pE_LaDSnk3*@S0ImQs8t4-yO`})gDvcMEZ6{;Dj!ULFI7lsn z97X`GZ$<++jg4?{m~Av}YfOAPH@#a0K7f4w_2A^44$PLBHLMKoSlxV;5P3@lExNDq z1Fi4ES_K9TYCq2zYSu%4eoS;{Vm;r51Ike~763yPAAuh!S z!ZV={63q;R6H;goPjPnXn*!S^TITICy5hO{m?O0AF;l=t1b7p`H;5}F>_jsHgNy5E z+3!2BAqX~~PBs4m?6Y*`6DuZ~p>yT9E(4OcKyp|srbVEQyX zKlCYm#euYV8~unjI^ZS9HlCm6kimqRS4RD*?r$QMwCib8zMVpPe5=MQ$&hc8@pQ8A z+r;?M4kw>sSA%kIgOeW!p?JH__~N1!(0^I&?_~$TgSD!xCwvY$|7~wy&sah~NI+rq zRN{L!0_T&&vjYQO?Ub@t&1$0J{OHgkQdJl@*fB5U1DPx4EJy4-C;OKkOSv5f!TH^uj_o)Fg=92k)O|nsC&Za zDM9Mls-{_-5_Q4~OB+QSr0;4}@cH4&$49%O2TUb1&ku!*sblBk`!sYqd6Vs##$6Cv zZKGX<66uL;rASUrV~1;`*+>_r-N#0Xv)mUs5U?0XOO|)CHs5afvwd#z8lKpcCo6ulIw!jGHH&_GMHLP7`y{;+-P_4P)$zsu~>eB)p$=$JwVUf{m(Ko*X_vhhMnImPK8dAz#G{Kd;UeobvOL?LObBU z4>G}jU$hzi`=O2hLe2EKVa>`YWLbKnr>NThC&at}sQB?e@xS7zj1^?B-26*6?rGlE zzs1q?y=}!JMZ&48cu$F-mqPjZPvwfb$G_ z*caA`I5-nLm==EP6_nxOvG6%ZCp0S6G&S75`39_ z-X4UHk+0jMdjQ@QWM;=EMPJy0(R#-b#iPE8 zkf$NJrQlJ#7~dgt`GKOZACrT8I#^#TpDkMRB}owO_C5-4>2JBD1v2m({pc3ybXBW7 zU4)v;;MG(p=-&_T9hrtXha^Q;ry}kQoK!z1Lu}XZLyM*}1#31tF}Kty3I8BRe06?= zgzd$4Se3mb*D&01(w-8?TzgW#mqBWjGdEnu?#q!~S{k7bi(}z3nk$FxfHJvDS^((_ zut}i>3Jo&W2huaiHTzB<=W$}JHG1WdGR#>?ts@OzAPtVBKj_wrC$y7+<=DB@FD=eXxfeO3Xzx7q!rIQBK&#mui%_T`hKnIK6uBTULCCk z&{tPIamlTCM*$lR!`Wh5BF16(lxd3?r|3V1)SvFb%Wr){OCda46yc3T)?DL3Te+h0_x%MhV{0n44fqmf+~L5sjzspO)Z(#lK&cJZPl{ z&UC2kk@-ly4It6D`GN4xj{(|`QgF64o7PCc3D<5qAc5NaX8Kx!b;HjK0X_NFj3%0y zfp2pa6$W||D2#_$#YIc~S`|e&Zqh!XsxUVj)t4M=I-+N>hLW zkH){4Dik=$=dL@g0v-Zh4p6)JUk~q7U^&LGm<}j$sc32CVV4pQmEh&3B0n7OzfAzv*UW1IvBoOz~+A$TFqFx?2jrTDff zDHM;#KbzKv;=X?K1*`+#CR-!K!hUgq=~5`v<^_{26lY?csdpHT?UpBa9=CuMIYDO# z;M0)9GsAE%9P|eS=qu!C1aGPu5O)K1W636ScidNDa&$TCLriVm@f!T-@RD#Gg7I62 zNd*2H&^-9)&}3V;8@T^7|;Ch}U;TaWrb7NfC`F<2=)>XgnKxnSP1JLDSHe zix$zIb@QJ(X+d_L2TLx~Fbxa~t~RGkBUd~4H-IugR{&K2tp=(Dx&&wl(1k$tK<7FY zn#Dk80bL4onp3Vx1-nPxc9TuzY8)tC;&$-E4!^2~F96Ro?bKj`6AmY9G5pmXPSIg0 z#yw1ZWAIp!db(*-3_gLk9ezF*gM&nS_=O%Ynt|uRVNRb*b1V9@c)NNAm?+b}IDE># z%+_r6*ZgWteHuN+v`~*n;Y`y7J)Y<{$o7>rRCCALl_kS8C>|G!COAzS;&Fy(u+wxd z9`^y`f)nuhYE>id0bZ`AoF(yWhSA9fd48zzs5R3#v2365K#uBSAe;`I{8`7uC3-lV z!O6s-E>kz|DZ6cT^2;59^@B3CQQ8&_jnQz?I0Mcu?>c+Ty$tq`u8r`@$haDA0m|S7 z%tQW7deWLpeb7RKAHD6ahTE|JtiTlqb=N(DE#zN?ulsl5uRSgNHz+*MQ+PjPm^pz} z8z1toIR3jLGoT_ZP!Y9RY+RQkX$D98T)F|E53WMtF?IZrQUu4*myTmS99ulMOY|*E zH;17SadLry|K38)kVjTpV^LA4vAN^Lw?RR33z!b3L8eCB7Xns0`6~iKO4tz~?vUwz zBHro0-@a#}zEQ-|AM*@{=zp0^J#n^d=nTwEoMF1z69=2POgvNUs518|_k$;*zI z9`(fk5?yqeeoTTZiNVuN|LBFEPI0!jv%f(>{> z_$J4iqTM;+t@Dl3W?qv^FCSR5Ln^wwA0|z#>7)VYVyQ`*f?vh&nW|G@DvmJ)reYmt zO?^}0OE;K4PsN(3Ni#9C5|9Uo0Q3ML_-jkXjCSVX2dOv;kG_`%H@SdU{|o;sPSG3u zTIra%4|pb@2yv9B@z3Hq(hpBa$0IR*-E_D&9veoNK?4*{Uww9icb}o=6%}4E9IdAK zJ~#=!n-}`vc>iqD|i z2%~mb)Tmr|q1rYNJNWUYjs0*?S~_^YtD0~=ygUcNt1xFMZ~hx!2mH0C@gGp$bgdtr zCd-Fmy_aDs?2pIbL#BQGagr#1hUsE|yci!g4apiHHY0&_@L(@mdjS#ga8pUQ^Sw_=kC@Z5uHVodFJOcR7p0=5Hc z0areN->?E*1;3~@1FieOq%Fq@12&owGYSw1m<%`zhy|Ln>PDmP zw6dwDIYvAsU_JbdcQPOdkOCO?^Woo(*cbPC8O}TffK`CmfMtLRz$idAU?YIm8Pk98 zH_?Dlw+d6QO59(z>`%n}`KM`SC5{sHeqeg55^ICkIH8Xmi0KV943G3a5BSFExH{@=7`&hYT#LqxzYa4qfNlXa z0Y*h19z6q35Sh3cSYkRd3x}56f@{cg1K|4y`UqeHNCsiX?Wr-G7J*+OK_~=h3D8)e mGs7V91lmePn5P7iP&cp%re9{@&~7Wi{L$3EKGm+2lK%sLi-Xkw From 07b6c57173b8251346060af8457601dc9a2b6faf Mon Sep 17 00:00:00 2001 From: rishabsingh3003 Date: Tue, 23 Jul 2024 11:39:18 -0400 Subject: [PATCH 122/124] AP_TemperatureSensor: Extend analog sensor backend to 5th order polynomial --- .../AP_TemperatureSensor_Analog.cpp | 17 +++++++++++------ .../AP_TemperatureSensor_Analog.h | 4 ++-- 2 files changed, 13 insertions(+), 8 deletions(-) diff --git a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Analog.cpp b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Analog.cpp index f255e7e8b29e66..91a7686f1d9582 100644 --- a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Analog.cpp +++ b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Analog.cpp @@ -33,29 +33,34 @@ const AP_Param::GroupInfo AP_TemperatureSensor_Analog::var_info[] = { // @Param: A0 // @DisplayName: Temperature sensor analog 0th polynomial coefficient - // @Description: a0 in polynomial of form temperature in deg = a0 + a1*voltage + a2*voltage^2 + a3*voltage^3 + a4*voltage^4 + // @Description: a0 in polynomial of form temperature in deg = a0 + a1*voltage + a2*voltage^2 + a3*voltage^3 + a4*voltage^4 + a5*voltage^5 AP_GROUPINFO("A0", 2, AP_TemperatureSensor_Analog, _a[0], 0), // @Param: A1 // @DisplayName: Temperature sensor analog 1st polynomial coefficient - // @Description: a1 in polynomial of form temperature in deg = a0 + a1*voltage + a2*voltage^2 + a3*voltage^3 + a4*voltage^4 + // @Description: a1 in polynomial of form temperature in deg = a0 + a1*voltage + a2*voltage^2 + a3*voltage^3 + a4*voltage^4 + a5*voltage^5 AP_GROUPINFO("A1", 3, AP_TemperatureSensor_Analog, _a[1], 0), // @Param: A2 // @DisplayName: Temperature sensor analog 2nd polynomial coefficient - // @Description: a2 in polynomial of form temperature in deg = a0 + a1*voltage + a2*voltage^2 + a3*voltage^3 + a4*voltage^4 + // @Description: a2 in polynomial of form temperature in deg = a0 + a1*voltage + a2*voltage^2 + a3*voltage^3 + a4*voltage^4 + a5*voltage^5 AP_GROUPINFO("A2", 4, AP_TemperatureSensor_Analog, _a[2], 0), // @Param: A3 // @DisplayName: Temperature sensor analog 3rd polynomial coefficient - // @Description: a3 in polynomial of form temperature in deg = a0 + a1*voltage + a2*voltage^2 + a3*voltage^3 + a4*voltage^4 + // @Description: a3 in polynomial of form temperature in deg = a0 + a1*voltage + a2*voltage^2 + a3*voltage^3 + a4*voltage^4 + a5*voltage^5 AP_GROUPINFO("A3", 5, AP_TemperatureSensor_Analog, _a[3], 0), // @Param: A4 // @DisplayName: Temperature sensor analog 4th polynomial coefficient - // @Description: a4 in polynomial of form temperature in deg = a0 + a1*voltage + a2*voltage^2 + a3*voltage^3 + a4*voltage^4 + // @Description: a4 in polynomial of form temperature in deg = a0 + a1*voltage + a2*voltage^2 + a3*voltage^3 + a4*voltage^4 + a5*voltage^5 AP_GROUPINFO("A4", 6, AP_TemperatureSensor_Analog, _a[4], 0), + // @Param: A5 + // @DisplayName: Temperature sensor analog 5th polynomial coefficient + // @Description: a5 in polynomial of form temperature in deg = a0 + a1*voltage + a2*voltage^2 + a3*voltage^3 + a4*voltage^4 + a5*voltage^5 + AP_GROUPINFO("A5", 7, AP_TemperatureSensor_Analog, _a[5], 0), + AP_GROUPEND }; @@ -81,7 +86,7 @@ void AP_TemperatureSensor_Analog::update() const float voltage = _analog_source->voltage_average_ratiometric(); // Evaluate polynomial - // temperature (deg) = a0 + a1*voltage + a2*voltage^2 + a3*voltage^3 + a4*voltage^4 + // temperature (deg) = a0 + a1*voltage + a2*voltage^2 + a3*voltage^3 + a4*voltage^4 + a5*voltage^5 float temp = 0.0; float poly = 1.0; for (uint8_t i = 0; i < ARRAY_SIZE(_a); i++) { diff --git a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Analog.h b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Analog.h index a342c165332e9b..99c3ba4ed12002 100644 --- a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Analog.h +++ b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Analog.h @@ -35,10 +35,10 @@ class AP_TemperatureSensor_Analog : public AP_TemperatureSensor_Backend { AP_HAL::AnalogSource *_analog_source; // Pin used to measure voltage - AP_Int8 _pin; + AP_Int8 _pin; // Polynomial coefficients to calculate temperature from voltage - AP_Float _a[5]; + AP_Float _a[6]; }; From f9bc2471213d726cf0595d642886207b58350a75 Mon Sep 17 00:00:00 2001 From: muramura Date: Sun, 2 Jun 2024 17:09:24 +0900 Subject: [PATCH 123/124] Copter: correct comment about scope of is_landing and is_taking_off --- ArduCopter/Copter.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 86fc3b0e61a782..7c9a0e91b3402f 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -474,13 +474,13 @@ bool Copter::update_target_location(const Location &old_loc, const Location &new #endif // AP_SCRIPTING_ENABLED -// returns true if vehicle is landing. Only used by Lua scripts +// returns true if vehicle is landing. bool Copter::is_landing() const { return flightmode->is_landing(); } -// returns true if vehicle is taking off. Only used by Lua scripts +// returns true if vehicle is taking off. bool Copter::is_taking_off() const { return flightmode->is_taking_off(); From 05d8b0db8a3d3b48da49b20a4bfbd84bfa0385af Mon Sep 17 00:00:00 2001 From: muramura Date: Wed, 19 Jun 2024 21:20:23 +0900 Subject: [PATCH 124/124] Copter: Clearly show the preprocessor --- ArduCopter/AP_Arming.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/ArduCopter/AP_Arming.cpp b/ArduCopter/AP_Arming.cpp index 12d037f07653c8..c711b06f5197ee 100644 --- a/ArduCopter/AP_Arming.cpp +++ b/ArduCopter/AP_Arming.cpp @@ -601,11 +601,11 @@ bool AP_Arming_Copter::arm_checks(AP_Arming::Method method) // check throttle if (check_enabled(ARMING_CHECK_RC)) { - #if FRAME_CONFIG == HELI_FRAME +#if FRAME_CONFIG == HELI_FRAME const char *rc_item = "Collective"; - #else +#else const char *rc_item = "Throttle"; - #endif +#endif // check throttle is not too high - skips checks if arming from GCS/scripting in Guided,Guided_NoGPS or Auto if (!((AP_Arming::method_is_GCS(method) || method == AP_Arming::Method::SCRIPTING) && (copter.flightmode->mode_number() == Mode::Number::GUIDED || copter.flightmode->mode_number() == Mode::Number::GUIDED_NOGPS || copter.flightmode->mode_number() == Mode::Number::AUTO))) { // above top of deadband is too always high @@ -614,12 +614,12 @@ bool AP_Arming_Copter::arm_checks(AP_Arming::Method method) return false; } // in manual modes throttle must be at zero - #if FRAME_CONFIG != HELI_FRAME +#if FRAME_CONFIG != HELI_FRAME if ((copter.flightmode->has_manual_throttle() || copter.flightmode->mode_number() == Mode::Number::DRIFT) && copter.channel_throttle->get_control_in() > 0) { check_failed(ARMING_CHECK_RC, true, "%s too high", rc_item); return false; } - #endif +#endif } }