Repository: ArduPilot/ardupilot
Pull request #29996: Sub: allow SURFACE mode to work with no altitude data
Author: login: Williangalvani
Labels: name: Sub
Created: 2025-05-06T17:32:51Z | Merged: 2025-05-27T01:26:16Z

This fixes an issue where SURFACE mode doesn't work on vehicles with no baros for altitude sensor.


Diff:
diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp
index 307a723e9de7aa..9df34b9d725521 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 242d1766edc55e..8150270ef505a1 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/Sub.cpp b/ArduSub/Sub.cpp
index 54c5959d5f7e08..2e8393d69b8b6c 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 b540cebe4acfc5..786de67dda534a 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 299a3f84a7752b..b5bb28449b0214 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; }
 
@@ -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 has_manual_throttle() const override { return false; }
+    bool requires_GPS() 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; }
 
 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 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 2a70fd1d1a17ce..6615553363e70a 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;
-    }
+    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);
@@ -32,28 +30,33 @@ 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 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
+        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());
diff --git a/Tools/autotest/ardusub.py b/Tools/autotest/ardusub.py
index 9fc4332faf30a7..86044a47fadde1 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


Reviews:
user: login: Hwurzburg
state: APPROVED
author_association: CONTRIBUTOR
submitted_at: 2025-05-06T21:03:11Z
user: login: rmackay9
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-05-07T03:44:55Z
user: login: rmackay9
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-05-07T03:46:43Z
user: login: rmackay9
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-05-07T03:47:48Z
user: login: rmackay9
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-05-07T03:48:12Z
user: login: Williangalvani
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-05-07T16:04:55Z
user: login: Williangalvani
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-05-07T16:06:27Z
user: login: Williangalvani
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-05-07T16:09:51Z
user: login: Williangalvani
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-05-07T16:15:38Z
user: login: rmackay9
body: looks fine, thanks!
state: APPROVED
author_association: CONTRIBUTOR
submitted_at: 2025-05-20T00:20:28Z
user: login: peterbarker
state: DISMISSED
author_association: CONTRIBUTOR
submitted_at: 2025-05-20T00:20:51Z

Review comments:
diff_hunk: @@ -767,6 +767,13 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("ORIGIN_ALT", 21, ParametersG2, backup_origin_alt, 0),
 
+    // @Param: SURF_NOBARO_THST
+    // @DisplayName: Throttle output when surface mode is engaged and baro is not available
path: ArduSub/Parameters.cpp
user: login: rmackay9
body: not a blocker of course but normally with Parameter descriptions I try to put the most important word first.  So in this case, I'd start it with "Surface mode".  So for example it could be, "Surface mode throttle output when no barometer is available"
created_at: 2025-05-07T03:44:55Z
author_association: CONTRIBUTOR
diff_hunk: @@ -208,7 +208,7 @@ class Parameters {
         k_param_throttle_gain,
         k_param_cam_tilt_center, // deprecated
         k_param_frame_configuration,
-
+        k_param_surface_sensorless_thrust,
path: ArduSub/Parameters.h
user: login: rmackay9
body: sorry I'm going to be pedantic but I wonder if we could be consistent about whether we use  "nobaro" vs "sensorless"?  It's one of those golden rules where we try not to use synonyms.  This helps both with searchability (e.g. searches for "nobaro" will turn up both) and it helps with non-native english speakers who may not know about the synonym.
created_at: 2025-05-07T03:46:43Z
author_association: CONTRIBUTOR
diff_hunk: @@ -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() &&
path: ArduSub/mode.cpp
user: login: rmackay9
body: this rename makes it inconsistent with Copter.  That's not necessarily a blocker but as we try to move vehicles closer to each other, let's try not to move in the opposite direction unless it's really necessary
created_at: 2025-05-07T03:47:48Z
author_association: CONTRIBUTOR
diff_hunk: @@ -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) {
path: ArduSub/mode_surface.cpp
user: login: rmackay9
body: maybe we could put a comment above this block of code?
created_at: 2025-05-07T03:48:12Z
author_association: CONTRIBUTOR
diff_hunk: @@ -767,6 +767,13 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("ORIGIN_ALT", 21, ParametersG2, backup_origin_alt, 0),
 
+    // @Param: SURF_NOBARO_THST
+    // @DisplayName: Throttle output when surface mode is engaged and baro is not available
path: ArduSub/Parameters.cpp
user: login: Williangalvani
body: thanks! updated
created_at: 2025-05-07T16:04:55Z
author_association: CONTRIBUTOR
diff_hunk: @@ -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) {
path: ArduSub/mode_surface.cpp
user: login: Williangalvani
body: done!
created_at: 2025-05-07T16:06:27Z
author_association: CONTRIBUTOR
diff_hunk: @@ -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() &&
path: ArduSub/mode.cpp
user: login: Williangalvani
body: initially I had just created a new `requires_altitude`, which is what ardusub actually wants to know.
but that made `has_manual_throttle` unused, which is why the PR ended up like this.

I'm quite set on making the code "make sense" for someone reading (which means using the new name). but I don't mind keeping has_manual_throttle() for compatibility. Note that for surface mode, these are not equivalent.
created_at: 2025-05-07T16:09:50Z
author_association: CONTRIBUTOR
diff_hunk: @@ -208,7 +208,7 @@ class Parameters {
         k_param_throttle_gain,
         k_param_cam_tilt_center, // deprecated
         k_param_frame_configuration,
-
+        k_param_surface_sensorless_thrust,
path: ArduSub/Parameters.h
user: login: Williangalvani
body: I went with nobaro on the parameter name because of the length constraints. I generally like making variables as meaningful as possible...

That said. I see the point on keeping things consistent, updated.
created_at: 2025-05-07T16:15:38Z
author_association: CONTRIBUTOR
diff_hunk: @@ -775,6 +775,14 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("ORIGIN_ALT", 21, ParametersG2, backup_origin_alt, 0),
 
+    // @Param: SURF_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("SURF_NOBARO_THST", 22, ParametersG2, surface_nobaro_thrust, 10),
path: ArduSub/Parameters.cpp
user: login: peterbarker
body: ```suggestion
    AP_GROUPINFO("SFC_NOBARO_THST", 22, ParametersG2, surface_nobaro_thrust, 10),
```

We're using SURF as a prefix in the "surface tracking" library, which sub might want at some stage.

created_at: 2025-05-20T00:19:14Z
author_association: CONTRIBUTOR

PR comments:
user: login: Hwurzburg
created_at: 2025-05-06T21:02:54Z
body: LGTM...sims fine...did not think you would want to get rid of requires GPS in the mode....if you are happy, I am happy
author_association: CONTRIBUTOR
user: login: rmackay9
created_at: 2025-05-07T03:49:01Z
body: It's really great to see this!

I'm not the Sub maintainer so please take my comments with a grain of salt.  I'm just being as pedantic as I would be if the code were Copter or Rover.


author_association: CONTRIBUTOR