diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 327e8decb1..32c041b5a2 100644 --- a/ArduSub/Sub.h +++ b/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); diff --git a/ArduSub/control_althold.cpp b/ArduSub/control_althold.cpp index 8240e085fd..0f5a57cd10 100644 --- a/ArduSub/control_althold.cpp +++ b/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::althold_run() + +void Sub::handle_attitude() { 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_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_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()); - - // 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); - } - } + handle_attitude(); 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()); +} \ No newline at end of file diff --git a/ArduSub/control_stabilize.cpp b/ArduSub/control_stabilize.cpp index 8d135fa82a..bbb6605d2a 100644 --- a/ArduSub/control_stabilize.cpp +++ b/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); - - // 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); - } - } + handle_attitude(); // output pilot's throttle attitude_control.set_throttle_out(channel_throttle->norm_input(), false, g.throttle_filt); diff --git a/ArduSub/joystick.cpp b/ArduSub/joystick.cpp index 2fc7675b9b..0d76fcbdfc 100644 --- a/ArduSub/joystick.cpp +++ b/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;