Implement vectored depth-hold

ArduSub/Sub.h

@@ -389,6 +389,9 @@ private:
     AP_Param param_loader;
 
     uint32_t last_pilot_heading;
+    uint32_t last_input_ms;
+    int32_t last_roll;
+    int32_t last_pitch;
     uint32_t last_pilot_yaw_input_ms;
     uint32_t fs_terrain_recover_start_ms;
 
@@ -453,6 +456,9 @@ private:
     void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out);
     bool althold_init(void);
     void althold_run();
+
+    // Handles attitude control for stabilize and althold modes
+    void handle_attitude();
     bool auto_init(void);
     void auto_run();
     void auto_wp_start(const Vector3f& destination);

ArduSub/control_althold.cpp

@@ -18,40 +18,30 @@ bool Sub::althold_init()
     pos_control.set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
 
     // initialise position and desired velocity
-    pos_control.init_z_controller();
+    pos_control.init_z_controller_stopping_point();
 
+    last_roll = 0;
+    last_pitch = 0;
     last_pilot_heading = ahrs.yaw_sensor;
+    last_input_ms = AP_HAL::millis();
 
     return true;
 }
 
-// althold_run - runs the althold controller
+
-// should be called at 100hz or more
+void Sub::handle_attitude()
-void Sub::althold_run()
 {
     uint32_t tnow = AP_HAL::millis();
 
     // initialize vertical speeds and acceleration
     pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
-
-    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.relax_attitude_controllers();
-        pos_control.relax_z_controller(motors.get_throttle_hover());
-        last_pilot_heading = ahrs.yaw_sensor;
-        return;
-    }
-
     motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
 
     // get pilot desired lean angles
-    float target_roll, target_pitch;
+    float target_roll, target_pitch, target_yaw;
 
     // Check if set_attitude_target_no_gps is valid
     if (tnow - sub.set_attitude_target_no_gps.last_message_ms < 5000) {
-        float target_yaw;
         Quaternion(
             set_attitude_target_no_gps.packet.q
         ).to_euler(
@@ -59,51 +49,59 @@ void Sub::althold_run()
             target_pitch,
             target_yaw
         );
-        target_roll = degrees(target_roll);
+        target_roll = 100 * degrees(target_roll);
-        target_pitch = degrees(target_pitch);
+        target_pitch = 100 * degrees(target_pitch);
-        target_yaw = degrees(target_yaw);
+        target_yaw = 100 * degrees(target_yaw);
+        attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, target_yaw, true);
+    } else {
+        // If we don't have a mavlink attitude target, we use the pilot's input instead
+        get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control.get_althold_lean_angle_max());
+        target_yaw = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
+        if (abs(target_roll) > 50 || abs(target_pitch) > 50 || abs(target_yaw) > 50) {
+            last_roll = ahrs.roll_sensor;
+            last_pitch = ahrs.pitch_sensor;
+            last_pilot_heading = ahrs.yaw_sensor;
+            last_input_ms = tnow;
+            attitude_control.input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw);
+        } else if (tnow < last_input_ms + 250) {
+            // just brake for a few mooments so we don't bounce
+            attitude_control.input_rate_bf_roll_pitch_yaw(0, 0, 0);
+        } else {
+            // Lock attitude
+            attitude_control.input_euler_angle_roll_pitch_yaw(last_roll, last_pitch, last_pilot_heading, true);
+        }
+    }
+}
 
-        attitude_control.input_euler_angle_roll_pitch_yaw(target_roll * 1e2f, target_pitch * 1e2f, target_yaw * 1e2f, true);
+// althold_run - runs the althold controller
+// should be called at 100hz or more
+void Sub::althold_run()
+{
+    // When unarmed, disable motors and stabilization
+    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.relax_attitude_controllers();
+        pos_control.relax_z_controller(motors.get_throttle_hover());
+        last_roll = 0;
+        last_pitch = 0;
+        last_pilot_heading = ahrs.yaw_sensor;
         return;
     }
 
-    get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control.get_althold_lean_angle_max());
+    handle_attitude();
-
-    // get pilot's desired yaw rate
-    float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
-
-    // call attitude controller
-    if (!is_zero(target_yaw_rate)) { // call attitude controller with rate yaw determined by pilot input
-        attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
-        last_pilot_heading = ahrs.yaw_sensor;
-        last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading
-
-    } else { // hold current heading
-
-        // this check is required to prevent bounce back after very fast yaw maneuvers
-        // the inertia of the vehicle causes the heading to move slightly past the point when pilot input actually stopped
-        if (tnow < last_pilot_yaw_input_ms + 250) { // give 250ms to slow down, then set target heading
-            target_yaw_rate = 0; // Stop rotation on yaw axis
-
-            // call attitude controller with target yaw rate = 0 to decelerate on yaw axis
-            attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
-            last_pilot_heading = ahrs.yaw_sensor; // update heading to hold
-
-        } else { // call attitude controller holding absolute absolute bearing
-            attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, last_pilot_heading, true);
-        }
-    }
 
     control_depth();
-
-    motors.set_forward(channel_forward->norm_input());
-    motors.set_lateral(channel_lateral->norm_input());
 }
 
 void Sub::control_depth() {
-    float target_climb_rate_cm_s = get_pilot_desired_climb_rate(channel_throttle->get_control_in());
+    // We rotate the RC inputs to the earth frame to check if the user is giving an input that would change the depth.
+    // Output the Z controller + pilot input to all motors.
+    Vector3f earth_frame_rc_inputs = ahrs.get_rotation_body_to_ned() * Vector3f(channel_forward->norm_input(), channel_lateral->norm_input(), (2.0f*(-0.5f+channel_throttle->norm_input())));
+    float target_climb_rate_cm_s = get_pilot_desired_climb_rate(500 + g.pilot_speed_up * earth_frame_rc_inputs.z);
     target_climb_rate_cm_s = constrain_float(target_climb_rate_cm_s, -get_pilot_speed_dn(), g.pilot_speed_up);
-
+    pos_control.set_pos_target_z_from_climb_rate_cm(target_climb_rate_cm_s);
     // desired_climb_rate returns 0 when within the deadzone.
     //we allow full control to the pilot, but as soon as there's no input, we handle being at surface/bottom
     if (fabsf(target_climb_rate_cm_s) < 0.05f)  {
@@ -113,8 +111,11 @@ void Sub::control_depth() {
             pos_control.set_pos_target_z_cm(MAX(inertial_nav.get_altitude() + 10.0f, pos_control.get_pos_target_z_cm())); // set target to 10 cm above bottom
         }
     }
-
-    pos_control.set_pos_target_z_from_climb_rate_cm(target_climb_rate_cm_s);
     pos_control.update_z_controller();
-
+    // Read the output of the z controller and rotate it so it always points up
-}
+    Vector3f throttle_vehicle_frame = ahrs.get_rotation_body_to_ned().transposed() * Vector3f(0, 0, motors.get_throttle_in_bidirectional());
+    //TODO: scale throttle with the ammount of thrusters in the given direction
+    motors.set_throttle(0.5+throttle_vehicle_frame.z + channel_throttle->norm_input()-0.5);
+    motors.set_forward(-throttle_vehicle_frame.x + channel_forward->norm_input());
+    motors.set_lateral(-throttle_vehicle_frame.y + channel_lateral->norm_input());
+}

ArduSub/control_stabilize.cpp

@@ -5,8 +5,9 @@ bool Sub::stabilize_init()
 {
     // set target altitude to zero for reporting
     pos_control.set_pos_target_z_cm(0);
+    last_roll = 0;
+    last_pitch = 0;
     last_pilot_heading = ahrs.yaw_sensor;
-
     return true;
 }
 
@@ -14,51 +15,18 @@ bool Sub::stabilize_init()
 // should be called at 100hz or more
 void Sub::stabilize_run()
 {
-    uint32_t tnow = AP_HAL::millis();
-    float target_roll, target_pitch;
-    float target_yaw_rate;
-
     // if not armed set throttle to zero and exit immediately
     if (!motors.armed()) {
         motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
         attitude_control.set_throttle_out(0,true,g.throttle_filt);
         attitude_control.relax_attitude_controllers();
         last_pilot_heading = ahrs.yaw_sensor;
+        last_roll = 0;
+        last_pitch = 0;
         return;
     }
 
-    motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
+    handle_attitude();
-
-    // convert pilot input to lean angles
-    // To-Do: convert get_pilot_desired_lean_angles to return angles as floats
-    get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max);
-
-    // get pilot's desired yaw rate
-    target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
-
-    // call attitude controller
-    // update attitude controller targets
-
-    if (!is_zero(target_yaw_rate)) { // call attitude controller with rate yaw determined by pilot input
-        attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
-        last_pilot_heading = ahrs.yaw_sensor;
-        last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading
-
-    } else { // hold current heading
-
-        // this check is required to prevent bounce back after very fast yaw maneuvers
-        // the inertia of the vehicle causes the heading to move slightly past the point when pilot input actually stopped
-        if (tnow < last_pilot_yaw_input_ms + 250) { // give 250ms to slow down, then set target heading
-            target_yaw_rate = 0;  // Stop rotation on yaw axis
-
-            // call attitude controller with target yaw rate = 0 to decelerate on yaw axis
-            attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
-            last_pilot_heading = ahrs.yaw_sensor; // update heading to hold
-
-        } else { // call attitude controller holding absolute absolute bearing
-            attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, last_pilot_heading, true);
-        }
-    }
 
     // output pilot's throttle
     attitude_control.set_throttle_out(channel_throttle->norm_input(), false, g.throttle_filt);

ArduSub/joystick.cpp

@@ -345,6 +345,12 @@ void Sub::handle_jsbutton_press(uint8_t _button, bool shift, bool held)
         if(!motors.armed()) {
             break;
         }
+        if(roll_pitch_flag) {
+            last_pitch = 0;
+            last_roll = 0;
+            last_input_ms = 0;
+            break;
+        }
         if (!held) {
             zTrim = abs(z_last-500) > 50 ? z_last-500 : 0;
             xTrim = abs(x_last) > 50 ? x_last : 0;