From a046b28466364672c9fe55e76934c022023439b2 Mon Sep 17 00:00:00 2001 From: Willian Galvani Date: Tue, 6 May 2025 13:29:42 -0300 Subject: [PATCH 1/3] Sub: allow surface mode to operate with no baro Co-authored-by: Peter Barker --- ArduSub/Parameters.cpp | 8 ++++++++ ArduSub/Parameters.h | 3 ++- ArduSub/mode.h | 4 ++-- ArduSub/mode_surface.cpp | 42 +++++++++++++++++++++------------------- 4 files changed, 34 insertions(+), 23 deletions(-) diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp index 307a723e9de7a..9df34b9d72552 100644 --- a/ArduSub/Parameters.cpp +++ b/ArduSub/Parameters.cpp @@ -775,6 +775,14 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @User: Standard AP_GROUPINFO("ORIGIN_ALT", 21, ParametersG2, backup_origin_alt, 0), + // @Param: SFC_NOBARO_THST + // @DisplayName: Surface mode throttle output when no barometer is available + // @Description: Surface mode throttle output when no borometer is available. 100% is full throttle. -100% is maximum throttle downwards + // @Units: % + // @User: Standard + // @Range: -100 100 + AP_GROUPINFO("SFC_NOBARO_THST", 22, ParametersG2, surface_nobaro_thrust, 10), + AP_GROUPEND }; diff --git a/ArduSub/Parameters.h b/ArduSub/Parameters.h index 242d1766edc55..8150270ef505a 100644 --- a/ArduSub/Parameters.h +++ b/ArduSub/Parameters.h @@ -209,7 +209,7 @@ class Parameters { k_param_cam_tilt_center, // deprecated k_param_frame_configuration, k_param_surface_max_throttle, - + k_param_surface_nobaro_thrust, // 200: flight modes k_param_flight_mode1 = 200, k_param_flight_mode2, @@ -404,6 +404,7 @@ class ParametersG2 { AP_Float backup_origin_lat; AP_Float backup_origin_lon; AP_Float backup_origin_alt; + AP_Float surface_nobaro_thrust; }; extern const AP_Param::Info var_info[]; diff --git a/ArduSub/mode.h b/ArduSub/mode.h index 299a3f84a7752..563662d56de81 100644 --- a/ArduSub/mode.h +++ b/ArduSub/mode.h @@ -469,16 +469,16 @@ class ModeSurface : public Mode virtual void run() override; bool init(bool ignore_checks) override; - bool requires_GPS() const override { return true; } + bool requires_GPS() const override { return false; } bool has_manual_throttle() const override { return false; } bool allows_arming(bool from_gcs) const override { return true; } bool is_autopilot() const override { return true; } protected: - const char *name() const override { return "SURFACE"; } const char *name4() const override { return "SURF"; } Mode::Number number() const override { return Mode::Number::SURFACE; } + bool sensorless_mode; }; diff --git a/ArduSub/mode_surface.cpp b/ArduSub/mode_surface.cpp index 2a70fd1d1a17c..500ae27a9e421 100644 --- a/ArduSub/mode_surface.cpp +++ b/ArduSub/mode_surface.cpp @@ -3,9 +3,7 @@ bool ModeSurface::init(bool ignore_checks) { - if(!sub.control_check_barometer()) { - return false; - } + sensorless_mode = !sub.control_check_barometer(); // initialize vertical speeds and acceleration position_control->set_max_speed_accel_U_cm(-sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); @@ -32,28 +30,32 @@ void ModeSurface::run() return; } - // Already at surface, hold depth at surface - if (sub.ap.at_surface) { - set_mode(Mode::Number::ALT_HOLD, ModeReason::SURFACE_COMPLETE); - } - - // convert pilot input to lean angles - // To-Do: convert sub.get_pilot_desired_lean_angles to return angles as floats - sub.get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, sub.aparm.angle_max); + if (sensorless_mode) { + float thrust_output = 0.5f + g2.surface_sensorless_thrust * 0.005f; // map -100, 100 to 0, 1 + attitude_control->set_throttle_out(thrust_output, true, g.throttle_filt); + } else { + // Already at surface, hold depth at surface + if (sub.ap.at_surface) { + set_mode(Mode::Number::ALT_HOLD, ModeReason::SURFACE_COMPLETE); + } - // get pilot's desired yaw rate - float target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); + // convert pilot input to lean angles + // To-Do: convert sub.get_pilot_desired_lean_angles to return angles as floats + sub.get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, sub.aparm.angle_max); - // call attitude controller - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(target_roll, target_pitch, target_yaw_rate); + // get pilot's desired yaw rate + float target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); - // set target climb rate - float cmb_rate_cms = constrain_float(fabsf(sub.wp_nav.get_default_speed_up_cms()), 1, position_control->get_max_speed_up_cms()); + // call attitude controller + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(target_roll, target_pitch, target_yaw_rate); - // update altitude target and call position controller - position_control->set_pos_target_U_from_climb_rate_cm(cmb_rate_cms); - position_control->update_U_controller(); + // set target climb rate + float cmb_rate_cms = constrain_float(fabsf(sub.wp_nav.get_default_speed_up_cms()), 1, position_control->get_max_speed_up_cms()); + // update altitude target and call position controller + position_control->set_pos_target_U_from_climb_rate_cm(cmb_rate_cms); + position_control->update_U_controller(); + } // pilot has control for repositioning motors.set_forward(channel_forward->norm_input()); motors.set_lateral(channel_lateral->norm_input()); From 9aa6bcba1b6bafad13a9107d418d2d539ccc8f0c Mon Sep 17 00:00:00 2001 From: Willian Galvani Date: Tue, 6 May 2025 14:18:48 -0300 Subject: [PATCH 2/3] Tools: Autotest: Sub: add test for Surface mode with no baro --- Tools/autotest/ardusub.py | 27 +++++++++++++++++++++++++++ 1 file changed, 27 insertions(+) diff --git a/Tools/autotest/ardusub.py b/Tools/autotest/ardusub.py index 9fc4332faf30a..86044a47fadde 100644 --- a/Tools/autotest/ardusub.py +++ b/Tools/autotest/ardusub.py @@ -1107,6 +1107,32 @@ def SHT3X(self): if m is None: raise NotAchievedException("Did not get good TEMP message") + def SurfaceSensorless(self): + """Test surface mode with sensorless thrust""" + # set GCS failsafe to SURFACE + self.wait_ready_to_arm() + self.arm_vehicle() + self.change_mode("STABILIZE") + self.set_parameter("MAV_GCS_SYSID", self.mav.source_system) + + self.set_rc(Joystick.Throttle, 1100) + self.wait_altitude(altitude_min=-10, altitude_max=-9, relative=False, timeout=60) + self.set_rc(Joystick.Throttle, 1500) + + self.context_push() + self.setGCSfailsafe(4) + self.set_parameter("SIM_BARO_DISABLE", 1) + self.set_heartbeat_rate(0) + self.wait_mode("SURFACE") + self.progress("Surface mode engaged") + self.wait_altitude(altitude_min=-1, altitude_max=0, relative=False, timeout=60) + self.progress("Vehicle resurfaced") + self.set_heartbeat_rate(self.speedup) + self.wait_statustext("GCS Failsafe Cleared", timeout=60) + self.progress("Baro-less Surface mode OK") + self.disarm_vehicle() + self.context_pop() + def tests(self): '''return list of all tests''' ret = super(AutoTestSub, self).tests() @@ -1143,6 +1169,7 @@ def tests(self): self.INA3221, self.PosHoldBounceBack, self.SHT3X, + self.SurfaceSensorless, ]) return ret From 9d328f977b6ec88d75fb8c668c7638e138b9903f Mon Sep 17 00:00:00 2001 From: Willian Galvani Date: Tue, 6 May 2025 14:22:37 -0300 Subject: [PATCH 3/3] Sub: rename has_manual_throttle to requires_altitude() --- ArduSub/Sub.cpp | 2 +- ArduSub/mode.cpp | 4 ++-- ArduSub/mode.h | 24 ++++++++++++------------ ArduSub/mode_surface.cpp | 7 ++++--- 4 files changed, 19 insertions(+), 18 deletions(-) diff --git a/ArduSub/Sub.cpp b/ArduSub/Sub.cpp index 54c5959d5f7e0..2e8393d69b8b6 100644 --- a/ArduSub/Sub.cpp +++ b/ArduSub/Sub.cpp @@ -246,7 +246,7 @@ void Sub::ten_hz_logging_loop() if (should_log(MASK_LOG_RCOUT)) { logger.Write_RCOUT(); } - if (should_log(MASK_LOG_NTUN) && (sub.flightmode->requires_GPS() || !sub.flightmode->has_manual_throttle())) { + if (should_log(MASK_LOG_NTUN) && (sub.flightmode->requires_GPS() || sub.flightmode->requires_altitude())) { pos_control.write_log(); } if (should_log(MASK_LOG_IMU) || should_log(MASK_LOG_IMU_FAST) || should_log(MASK_LOG_IMU_RAW)) { diff --git a/ArduSub/mode.cpp b/ArduSub/mode.cpp index b540cebe4acfc..786de67dda534 100644 --- a/ArduSub/mode.cpp +++ b/ArduSub/mode.cpp @@ -95,8 +95,8 @@ bool Sub::set_mode(Mode::Number mode, ModeReason reason) // check for valid altitude if old mode did not require it but new one does // we only want to stop changing modes if it could make things worse - if (flightmode->has_manual_throttle() && - !new_flightmode->has_manual_throttle() && + if (!flightmode->requires_altitude() && + new_flightmode->requires_altitude() && !sub.control_check_barometer()) { // maybe use ekf_alt_ok() instead? gcs().send_text(MAV_SEVERITY_WARNING, "Mode change failed: %s need alt estimate", new_flightmode->name()); LOGGER_WRITE_ERROR(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode)); diff --git a/ArduSub/mode.h b/ArduSub/mode.h index 563662d56de81..b5bb28449b021 100644 --- a/ArduSub/mode.h +++ b/ArduSub/mode.h @@ -64,7 +64,7 @@ class Mode virtual bool init(bool ignore_checks) { return true; } virtual void run() = 0; virtual bool requires_GPS() const = 0; - virtual bool has_manual_throttle() const = 0; + virtual bool requires_altitude() const = 0; virtual bool allows_arming(bool from_gcs) const = 0; virtual bool is_autopilot() const { return false; } virtual bool in_guided_mode() const { return false; } @@ -198,7 +198,7 @@ class ModeManual : public Mode virtual void run() override; bool init(bool ignore_checks) override; bool requires_GPS() const override { return false; } - bool has_manual_throttle() const override { return true; } + bool requires_altitude() const override { return false; } bool allows_arming(bool from_gcs) const override { return true; } bool is_autopilot() const override { return false; } @@ -221,7 +221,7 @@ class ModeAcro : public Mode bool init(bool ignore_checks) override; bool requires_GPS() const override { return false; } - bool has_manual_throttle() const override { return true; } + bool requires_altitude() const override { return false; } bool allows_arming(bool from_gcs) const override { return true; } bool is_autopilot() const override { return false; } @@ -244,7 +244,7 @@ class ModeStabilize : public Mode bool init(bool ignore_checks) override; bool requires_GPS() const override { return false; } - bool has_manual_throttle() const override { return true; } + bool requires_altitude() const override { return false; } bool allows_arming(bool from_gcs) const override { return true; } bool is_autopilot() const override { return false; } @@ -267,7 +267,7 @@ class ModeAlthold : public Mode bool init(bool ignore_checks) override; bool requires_GPS() const override { return false; } - bool has_manual_throttle() const override { return false; } + bool requires_altitude() const override { return true; } bool allows_arming(bool from_gcs) const override { return true; } bool is_autopilot() const override { return false; } void control_depth(); @@ -326,7 +326,7 @@ class ModeGuided : public Mode bool init(bool ignore_checks) override; bool requires_GPS() const override { return true; } - bool has_manual_throttle() const override { return false; } + bool requires_altitude() const override { return true; } bool allows_arming(bool from_gcs) const override { return true; } bool is_autopilot() const override { return true; } bool in_guided_mode() const override { return true; } @@ -380,7 +380,7 @@ class ModeAuto : public ModeGuided bool init(bool ignore_checks) override; bool requires_GPS() const override { return true; } - bool has_manual_throttle() const override { return false; } + bool requires_altitude() const override { return true; } bool allows_arming(bool from_gcs) const override { return true; } bool is_autopilot() const override { return true; } bool auto_loiter_start(); @@ -421,7 +421,7 @@ class ModePoshold : public ModeAlthold bool init(bool ignore_checks) override; bool requires_GPS() const override { return true; } - bool has_manual_throttle() const override { return false; } + bool requires_altitude() const override { return true; } bool allows_arming(bool from_gcs) const override { return true; } bool is_autopilot() const override { return true; } @@ -448,7 +448,7 @@ class ModeCircle : public Mode bool init(bool ignore_checks) override; bool requires_GPS() const override { return true; } - bool has_manual_throttle() const override { return false; } + bool requires_altitude() const override { return true; } bool allows_arming(bool from_gcs) const override { return true; } bool is_autopilot() const override { return true; } @@ -470,7 +470,7 @@ class ModeSurface : public Mode bool init(bool ignore_checks) override; bool requires_GPS() const override { return false; } - bool has_manual_throttle() const override { return false; } + bool requires_altitude() const override { return false; } bool allows_arming(bool from_gcs) const override { return true; } bool is_autopilot() const override { return true; } @@ -478,7 +478,7 @@ class ModeSurface : public Mode const char *name() const override { return "SURFACE"; } const char *name4() const override { return "SURF"; } Mode::Number number() const override { return Mode::Number::SURFACE; } - bool sensorless_mode; + bool nobaro_mode; }; @@ -493,7 +493,7 @@ class ModeMotordetect : public Mode bool init(bool ignore_checks) override; bool requires_GPS() const override { return false; } - bool has_manual_throttle() const override { return false; } + bool requires_altitude() const override { return false; } bool allows_arming(bool from_gcs) const override { return true; } bool is_autopilot() const override { return true; } diff --git a/ArduSub/mode_surface.cpp b/ArduSub/mode_surface.cpp index 500ae27a9e421..6615553363e70 100644 --- a/ArduSub/mode_surface.cpp +++ b/ArduSub/mode_surface.cpp @@ -3,7 +3,7 @@ bool ModeSurface::init(bool ignore_checks) { - sensorless_mode = !sub.control_check_barometer(); + nobaro_mode = !sub.control_check_barometer(); // initialize vertical speeds and acceleration position_control->set_max_speed_accel_U_cm(-sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); @@ -30,8 +30,9 @@ void ModeSurface::run() return; } - if (sensorless_mode) { - float thrust_output = 0.5f + g2.surface_sensorless_thrust * 0.005f; // map -100, 100 to 0, 1 + // If no barometer is available, use the surface_nobaro_thrust parameter to set the throttle output + if (nobaro_mode) { + float thrust_output = 0.5f + g2.surface_nobaro_thrust * 0.005f; // map -100, 100 to 0, 1 attitude_control->set_throttle_out(thrust_output, true, g.throttle_filt); } else { // Already at surface, hold depth at surface