Sub: update control_poshold to use AC_PosControlSub

ArduSub/control_poshold.cpp

@@ -13,6 +13,9 @@ bool Sub::poshold_init()
     if (!position_ok()) {
         return false;
     }
+    pos_control.init_vel_controller_xyz();
+    pos_control.set_desired_velocity_xy(0, 0);
+    pos_control.set_target_to_stopping_point_xy();
 
     // initialize vertical speeds and acceleration
     pos_control.set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
@@ -22,10 +25,10 @@ bool Sub::poshold_init()
     pos_control.set_alt_target(inertial_nav.get_altitude());
     pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
 
-    // set target to current position
-    // only init here as we can switch to PosHold in flight with a velocity <> 0 that will be used as _last_vel in PosControl and never updated again as we inhibit Reset_I
-    loiter_nav.clear_pilot_desired_acceleration();
-    loiter_nav.init_target();
+    // Stop all thrusters
+    attitude_control.set_throttle_out(0.5f ,true, g.throttle_filt);
+    attitude_control.relax_attitude_controllers();
+    pos_control.relax_alt_hold_controllers();
 
     last_pilot_heading = ahrs.yaw_sensor;
 
@@ -37,24 +40,21 @@ bool Sub::poshold_init()
 void Sub::poshold_run()
 {
     uint32_t tnow = AP_HAL::millis();
-
-    // if not armed set throttle to zero and exit immediately
+    // When unarmed, disable motors and stabilization
     if (!motors.armed()) {
         motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
-        loiter_nav.clear_pilot_desired_acceleration();
-        loiter_nav.init_target();
-        attitude_control.set_throttle_out(0,true,g.throttle_filt);
+        // Sub vehicles do not stabilize roll/pitch/yaw when not auto-armed (i.e. on the ground, pilot has never raised throttle)
+        attitude_control.set_throttle_out(0.5f ,true, g.throttle_filt);
         attitude_control.relax_attitude_controllers();
-        pos_control.relax_alt_hold_controllers(motors.get_throttle_hover());
+        pos_control.set_target_to_stopping_point_xy();
+        pos_control.relax_alt_hold_controllers();
+        last_pilot_heading = ahrs.yaw_sensor;
         return;
     }
 
     // set motors to full range
     motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
 
-    // run loiter controller
-    loiter_nav.update();
-
     ///////////////////////
     // update xy outputs //
     float pilot_lateral = channel_lateral->norm_input();
@@ -63,19 +63,22 @@ void Sub::poshold_run()
     float lateral_out = 0;
     float forward_out = 0;
 
-    // Allow pilot to reposition the sub
-    if (fabsf(pilot_lateral) > 0.1 || fabsf(pilot_forward) > 0.1) {
-        lateral_out = pilot_lateral;
-        forward_out = pilot_forward;
-        loiter_nav.clear_pilot_desired_acceleration();
-        loiter_nav.init_target(); // initialize target to current position after repositioning
+    pos_control.set_desired_velocity_xy(0,0);
+
+    if (position_ok()) {
+        // Allow pilot to reposition the sub
+        if (fabsf(pilot_lateral) > 0.1 || fabsf(pilot_forward) > 0.1) {
+            pos_control.set_target_to_stopping_point_xy();
+        }
+        translate_pos_control_rp(lateral_out, forward_out);
+        pos_control.update_xy_controller();
     } else {
-        translate_wpnav_rp(lateral_out, forward_out);
+        pos_control.init_vel_controller_xyz();
+        pos_control.set_desired_velocity_xy(0, 0);
+        pos_control.set_target_to_stopping_point_xy();
     }
-
-    motors.set_lateral(lateral_out);
-    motors.set_forward(forward_out);
-
+    motors.set_forward(forward_out + pilot_forward);
+    motors.set_lateral(lateral_out + pilot_lateral);
     /////////////////////
     // Update attitude //
 
@@ -109,21 +112,7 @@ void Sub::poshold_run()
         }
     }
 
-    ///////////////////
     // Update z axis //
-
-    // get pilot desired climb rate
-    float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());
-    target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up);
-
-    // call z axis position controller
-    if (ap.at_bottom) {
-        pos_control.relax_alt_hold_controllers(motors.get_throttle_hover()); // clear velocity and position targets, and integrator
-        pos_control.set_alt_target(inertial_nav.get_altitude() + 10.0f); // set target to 10 cm above bottom
-    } else {
-        pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
-    }
-
-    pos_control.update_z_controller();
+    control_depth();
 }
 #endif  // POSHOLD_ENABLED == ENABLED