wip

ArduSub/Sub.h

@@ -557,6 +557,7 @@ class Sub : public AP_Vehicle {
     bool get_wp_distance_m(float &distance) const override;
     bool get_wp_bearing_deg(float &bearing) const override;
     bool get_wp_crosstrack_error_m(float &xtrack_error) const override;
+    float stopping_distance();
 
     enum Failsafe_Action {
         Failsafe_Action_None    = 0,

ArduSub/mode_althold.cpp

@@ -28,6 +28,14 @@ void ModeAlthold::run()
     run_post();
 }
 
+
+float Sub::stopping_distance() {
+    const float curr_pos_z = inertial_nav.get_position_z_up_cm();
+    float curr_vel_z = inertial_nav.get_velocity_z_up_cms();
+    float distance = - (curr_vel_z * curr_vel_z) / (2 * g.pilot_accel_z);
+    return curr_pos_z  + distance;
+}
+
 void ModeAlthold::run_pre()
 {
     uint32_t tnow = AP_HAL::millis();
@@ -38,10 +46,14 @@ void ModeAlthold::run_pre()
     if (!motors.armed()) {
         motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
         // 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.5,true,g.throttle_filt);
+        attitude_control->set_throttle_out(0.75, true, 100.0);
+        position_control->init_z_controller();
         attitude_control->relax_attitude_controllers();
         position_control->relax_z_controller(motors.get_throttle_hover());
         sub.last_pilot_heading = ahrs.yaw_sensor;
+        float pos = sub.stopping_distance();
+        float zero = 0;
+        position_control->input_pos_vel_accel_z(pos, zero, zero);
         return;
     }
 

ArduSub/mode_poshold.cpp

@@ -24,8 +24,11 @@ bool ModePoshold::init(bool ignore_checks)
     position_control->init_xy_controller_stopping_point();
     position_control->init_z_controller();
 
-    // Stop all thrusters
-    attitude_control->set_throttle_out(0.5f ,true, g.throttle_filt);
+    // initialise position and desired velocity
+    float pos = sub.stopping_distance();
+    float zero = 0;
+    position_control->input_pos_vel_accel_z(pos, zero, zero);
+
     attitude_control->relax_attitude_controllers();
     position_control->relax_z_controller(0.5f);