Sub: allow surface mode to operate with no baro

Co-authored-by: Peter Barker <pb-gh@barker.dropbear.id.au>

Author: 2 people

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
 };
 

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[];

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;
 };
 
 

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());