Sub: Enable depth hold in arbitrary attitudes

ArduSub/Sub.h

@@ -447,6 +447,10 @@ private:
     AP_Param param_loader;
 
     uint32_t last_pilot_heading;
+    uint32_t last_input_ms;
+    int32_t last_roll;
+    int32_t last_pitch;
+    int32_t last_yaw;
     uint32_t last_pilot_yaw_input_ms;
     uint32_t fs_terrain_recover_start_ms;
 
@@ -518,6 +522,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

@@ -21,39 +21,22 @@ bool Sub::althold_init()
     pos_control.set_alt_target(inertial_nav.get_altitude());
     pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
 
-    last_pilot_heading = ahrs.yaw_sensor;
+    last_roll = ahrs.roll_sensor;
+    last_pitch = ahrs.pitch_sensor;
+    last_yaw = 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();
-
+    float target_roll, target_pitch, target_yaw;
-    // initialize vertical speeds and acceleration
-    pos_control.set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
-    pos_control.set_max_accel_z(g.pilot_accel_z);
-
-    if (!motors.armed()) {
-        motors.set_desired_spool_state(AP_Motors::DESIRED_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,true,g.throttle_filt);
-        attitude_control.relax_attitude_controllers();
-        pos_control.relax_alt_hold_controllers(motors.get_throttle_hover());
-        last_pilot_heading = ahrs.yaw_sensor;
-        return;
-    }
-
-    motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
-
-    // get pilot desired lean angles
-    float target_roll, target_pitch;
 
     // 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(
@@ -61,54 +44,75 @@ 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_yaw = 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_yaw, 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::DESIRED_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,true,g.throttle_filt);
+        attitude_control.relax_attitude_controllers();
+        pos_control.relax_alt_hold_controllers(motors.get_throttle_hover());
+        last_roll = ahrs.roll_sensor;
+        last_pitch = ahrs.pitch_sensor;
+        last_yaw = 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());
+    // Vehicle is armed, motors are free to run
+    motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
 
-    // get pilot's desired yaw rate
+    handle_attitude();
-    float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
 
-    // call attitude controller
+    pos_control.update_z_controller();
-    if (!is_zero(target_yaw_rate)) { // call attitude controller with rate yaw determined by pilot input
+    // Read the output of the z controller and rotate it so it always points up
-        attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
+    Vector3f throttle_vehicle_frame = ahrs.get_rotation_body_to_ned().transposed() * Vector3f(0, 0, motors.get_throttle_in_bidirectional());
-        last_pilot_heading = ahrs.yaw_sensor;
+    // Output the Z controller + pilot input to all motors.
-        last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading
 
-    } else { // hold current heading
+    //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);
-        // this check is required to prevent bounce back after very fast yaw maneuvers
+    motors.set_forward(-throttle_vehicle_frame.x + channel_forward->norm_input());
-        // the inertia of the vehicle causes the heading to move slightly past the point when pilot input actually stopped
+    motors.set_lateral(-throttle_vehicle_frame.y + channel_lateral->norm_input());
-        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);
-        }
-    }
 
+    // We rotate the RC inputs to the earth frame to check if the user is giving an input that would change the depth.
+    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())));
     // Hold actual position until zero derivative is detected
     static bool engageStopZ = true;
     // Get last user velocity direction to check for zero derivative points
     static bool lastVelocityZWasNegative = false;
-    if (fabsf(channel_throttle->norm_input()-0.5f) > 0.05f) { // Throttle input above 5%
+
-        // output pilot's throttle
+    if (fabsf(earth_frame_rc_inputs.z) > 0.05f) { // Throttle input  above 5%
-        attitude_control.set_throttle_out(channel_throttle->norm_input(), false, g.throttle_filt);
        // reset z targets to current values
         pos_control.relax_alt_hold_controllers();
         engageStopZ = true;
         lastVelocityZWasNegative = is_negative(inertial_nav.get_velocity_z());
     } else { // hold z
-
         if (ap.at_bottom) {
             pos_control.relax_alt_hold_controllers(); // clear velocity and position targets
             pos_control.set_alt_target(inertial_nav.get_altitude() + 10.0f); // set target to 10 cm above bottom
@@ -126,10 +130,5 @@ void Sub::althold_run()
             engageStopZ = false;
             pos_control.relax_alt_hold_controllers();
         }
-
-        pos_control.update_z_controller();
     }
-
-    motors.set_forward(channel_forward->norm_input());
-    motors.set_lateral(channel_lateral->norm_input());
 }

ArduSub/joystick.cpp

@@ -329,6 +329,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;