Skip to content

Sub: allow SURFACE mode to work with no altitude data #29996

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 3 commits into from
May 27, 2025
Merged

Sub: allow SURFACE mode to work with no altitude data #29996

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
a046b28
Sub: allow surface mode to operate with no baro
Williangalvani May 6, 2025
9aa6bcb
Tools: Autotest: Sub: add test for Surface mode with no baro
Williangalvani May 6, 2025
9d328f9
Sub: rename has_manual_throttle to requires_altitude()
Williangalvani May 6, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 8 additions & 0 deletions ArduSub/Parameters.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
};

Expand Down
3 changes: 2 additions & 1 deletion ArduSub/Parameters.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down Expand Up @@ -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[];
Expand Down
2 changes: 1 addition & 1 deletion ArduSub/Sub.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)) {
Expand Down
4 changes: 2 additions & 2 deletions ArduSub/mode.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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() &&
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Contributor Author

@Williangalvani Williangalvani May 7, 2025 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

!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));
Expand Down
26 changes: 13 additions & 13 deletions ArduSub/mode.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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; }
Expand Down Expand Up @@ -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; }

Expand All @@ -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; }

Expand All @@ -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; }

Expand All @@ -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();
Expand Down Expand Up @@ -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; }
Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -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; }

Expand All @@ -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; }

Expand All @@ -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;
};


Expand All @@ -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; }

Expand Down
43 changes: 23 additions & 20 deletions ArduSub/mode_surface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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());
Expand Down
27 changes: 27 additions & 0 deletions Tools/autotest/ardusub.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand Down Expand Up @@ -1143,6 +1169,7 @@ def tests(self):
self.INA3221,
self.PosHoldBounceBack,
self.SHT3X,
self.SurfaceSensorless,
])

return ret
Loading