From 9a07fce7e329193d78a46731ac303eb117c38f25 Mon Sep 17 00:00:00 2001 From: Willian Galvani Date: Wed, 11 Sep 2019 10:59:28 -0300 Subject: [PATCH] Sub: Enable depth hold in arbitrary attitudes --- ArduSub/Sub.h | 7 +++ ArduSub/control_althold.cpp | 103 +++++++++++++++++++++--------------- ArduSub/joystick.cpp | 6 +++ 3 files changed, 73 insertions(+), 43 deletions(-) diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 2f89fca3e0..772ad049a3 100644 --- a/ArduSub/Sub.h +++ b/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; @@ -515,6 +519,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); diff --git a/ArduSub/control_althold.cpp b/ArduSub/control_althold.cpp index e0eef9ede5..c48cf70db1 100644 --- a/ArduSub/control_althold.cpp +++ b/ArduSub/control_althold.cpp @@ -21,14 +21,16 @@ 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::althold_run() + +void Sub::handle_attitude() { uint32_t tnow = AP_HAL::millis(); @@ -49,11 +51,10 @@ void Sub::althold_run() 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( @@ -61,54 +62,75 @@ void Sub::althold_run() target_pitch, target_yaw ); - target_roll = degrees(target_roll); - target_pitch = degrees(target_pitch); - target_yaw = degrees(target_yaw); + target_roll = 100 * degrees(target_roll); + target_pitch = 100 * degrees(target_pitch); + 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 - float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); + handle_attitude(); - // 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 + 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()); + // Output the Z controller + pilot input to all motors. - } 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); - } - } + //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()); + // 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 - attitude_control.set_throttle_out(channel_throttle->norm_input(), false, g.throttle_filt); - // reset z targets to current values + + if (fabsf(earth_frame_rc_inputs.z) > 0.05f) { // Throttle input above 5% + // 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 @@ -122,10 +144,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()); } diff --git a/ArduSub/joystick.cpp b/ArduSub/joystick.cpp index 5ef92e5532..72138b22b3 100644 --- a/ArduSub/joystick.cpp +++ b/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;