From d26d0dacf8319587c59668ae8ea6104d4a590aa9 Mon Sep 17 00:00:00 2001 From: Willian Galvani Date: Tue, 14 Feb 2023 13:54:44 -0300 Subject: [PATCH] Sub: introduce CONTROL_FRAME parameter, allowing users to switch control frame for yaw --- ArduSub/Parameters.cpp | 8 +++++ ArduSub/Parameters.h | 2 ++ ArduSub/config.h | 5 +++ ArduSub/control_althold.cpp | 71 +++++++++++++++++++++++++++---------- ArduSub/joystick.cpp | 55 ++++++++++++++++------------ 5 files changed, 101 insertions(+), 40 deletions(-) diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp index 82a10975c2..b39e15adea 100644 --- a/ArduSub/Parameters.cpp +++ b/ArduSub/Parameters.cpp @@ -407,6 +407,14 @@ const AP_Param::Info Sub::var_info[] = { // @User: Advanced GSCALAR(acro_expo, "ACRO_EXPO", ACRO_EXPO_DEFAULT), + // @Param: CONTROL_FRAME + // @DisplayName: Control Frame + // @Description: Control Frame used in stabilized modes + // @Values: 0: Standard, 1: Body frame Yaw/Pitch/Roll + // @User: Standard + + GSCALAR(control_frame, "CONTROL_FRAME", CONTROL_FRAME_DEFAULT), + // variables not in the g class which contain EEPROM saved variables #if CAMERA == ENABLED diff --git a/ArduSub/Parameters.h b/ArduSub/Parameters.h index 13a99de694..3f4e08068d 100644 --- a/ArduSub/Parameters.h +++ b/ArduSub/Parameters.h @@ -191,6 +191,7 @@ public: k_param_throttle_gain, k_param_cam_tilt_center, // deprecated k_param_frame_configuration, + k_param_control_frame, // Acro Mode parameters k_param_acro_yaw_p = 220, // Used in all modes for get_pilot_desired_yaw_rate @@ -302,6 +303,7 @@ public: AP_Float surface_depth; AP_Int8 frame_configuration; + AP_Int8 control_frame; // Note: keep initializers here in the same order as they are declared // above. diff --git a/ArduSub/config.h b/ArduSub/config.h index 54a8bf38d3..7955a46ec5 100644 --- a/ArduSub/config.h +++ b/ArduSub/config.h @@ -153,6 +153,11 @@ #define ACRO_EXPO_DEFAULT 0.3f #endif +// should be a subset of MAV_FRAME +#ifndef CONTROL_FRAME_DEFAULT +#define CONTROL_FRAME_DEFAULT 0.0f +#endif + // AUTO Mode #ifndef WP_YAW_BEHAVIOR_DEFAULT # define WP_YAW_BEHAVIOR_DEFAULT WP_YAW_BEHAVIOR_CORRECT_XTRACK diff --git a/ArduSub/control_althold.cpp b/ArduSub/control_althold.cpp index b20303d29d..369984a466 100644 --- a/ArduSub/control_althold.cpp +++ b/ArduSub/control_althold.cpp @@ -50,7 +50,7 @@ void Sub::handle_attitude() pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); - // get pilot desired lean angles + // get pilot desired lean angles/rates float target_roll, target_pitch, target_yaw; // Check if set_attitude_target_no_gps is valid @@ -69,23 +69,57 @@ void Sub::handle_attitude() last_pitch = target_pitch; last_pilot_heading = 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()); - float yaw_input = channel_yaw->pwm_to_angle_dz_trim(channel_yaw->get_dead_zone() * gain, channel_yaw->get_radio_trim()); - target_yaw = get_pilot_desired_yaw_rate(yaw_input); - 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); + 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()); + float yaw_input = channel_yaw->pwm_to_angle_dz_trim(channel_yaw->get_dead_zone() * gain, channel_yaw->get_radio_trim()); + target_yaw = get_pilot_desired_yaw_rate(yaw_input); + // If we don't have a mavlink attitude target, we use the pilot's input instead + switch (g.control_frame) { + case MAV_FRAME_BODY_FRD: + { + if (abs(target_roll) > 50 || abs(target_pitch) > 50 || abs(target_yaw) > 50) { + last_input_ms = tnow; + attitude_control.input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw); + Quaternion attitude_target = attitude_control.get_attitude_target_quat(); + last_roll = degrees(attitude_target.get_euler_roll()) * 100; + last_pitch = degrees(attitude_target.get_euler_pitch()) * 100; + last_pilot_heading = degrees(attitude_target.get_euler_yaw()) * 100; + } else { + attitude_control.input_euler_angle_roll_pitch_yaw(last_roll, last_pitch, last_pilot_heading, true); + } + } + break; + default: + { + // call attitude controller + if (!is_zero(target_yaw)) { // call attitude controller with rate yaw determined by pilot input + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(last_roll, last_pitch, target_yaw); + last_pilot_heading = ahrs.yaw_sensor; + last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading + + } else { // hold current heading + + 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); + + // 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 + } else if (tnow < last_pilot_yaw_input_ms + 250) { // give 250ms to slow down, then set target heading + target_yaw = 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(last_roll, last_pitch, target_yaw); + 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(last_roll, last_pitch, last_pilot_heading, true); + } + } } } } @@ -99,6 +133,7 @@ void Sub::althold_run() 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.75,true,100.0); + attitude_control.relax_attitude_controllers(); pos_control.init_z_controller(); // initialise position and desired velocity float pos = stopping_distance(); diff --git a/ArduSub/joystick.cpp b/ArduSub/joystick.cpp index 3bc77b8096..2eab85deb0 100644 --- a/ArduSub/joystick.cpp +++ b/ArduSub/joystick.cpp @@ -1,5 +1,7 @@ #include "Sub.h" +#define TRIM_STEP 0.5 // trim step in degrees + // Functions that will handle joystick/gamepad input // ---------------------------------------------------------------------------- @@ -141,12 +143,8 @@ void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t void Sub::handle_jsbutton_press(uint8_t _button, bool shift, bool held) { // Used for trimming level in vehicle frame - Quaternion attitudeTarget; - attitudeTarget.from_euler( - radians(last_roll * 0.01f), - radians(last_pitch * 0.01f), - radians(last_pilot_heading * 0.01f) - ); + Quaternion attitude_target = attitude_control.get_attitude_target_quat(); + Vector3f localPitch = Vector3f(0, 1, 0); Vector3f localRoll = Vector3f(1, 0, 0); @@ -340,28 +338,41 @@ void Sub::handle_jsbutton_press(uint8_t _button, bool shift, bool held) } break; case JSButton::button_function_t::k_trim_roll_inc: - attitudeTarget.rotate(localRoll * radians(1)); - last_roll = degrees(attitudeTarget.get_euler_roll()) * 100; - last_pitch = degrees(attitudeTarget.get_euler_pitch()) * 100; - last_pilot_heading = degrees(attitudeTarget.get_euler_yaw()) * 100; + last_roll += TRIM_STEP * 100; + if (g.control_frame == MAV_FRAME_BODY_FRD) { + // TODO: change these to use input_quaternion() + attitude_target.rotate(localRoll * radians(TRIM_STEP)); + last_roll = degrees(attitude_target.get_euler_roll()) * 100; + last_pitch = degrees(attitude_target.get_euler_pitch()) * 100; + last_pilot_heading = degrees(attitude_target.get_euler_yaw()) * 100; + } break; case JSButton::button_function_t::k_trim_roll_dec: - attitudeTarget.rotate(localRoll * radians(-1)); - last_roll = degrees(attitudeTarget.get_euler_roll()) * 100; - last_pitch = degrees(attitudeTarget.get_euler_pitch()) * 100; - last_pilot_heading = degrees(attitudeTarget.get_euler_yaw()) * 100; + last_roll -= TRIM_STEP * 100; + if (g.control_frame == MAV_FRAME_BODY_FRD) { + attitude_target.rotate(localRoll * radians(-TRIM_STEP)); + last_roll = degrees(attitude_target.get_euler_roll()) * 100; + last_pitch = degrees(attitude_target.get_euler_pitch()) * 100; + last_pilot_heading = degrees(attitude_target.get_euler_yaw()) * 100; + } break; case JSButton::button_function_t::k_trim_pitch_inc: - attitudeTarget.rotate(localPitch * radians(1)); - last_roll = degrees(attitudeTarget.get_euler_roll()) * 100; - last_pitch = degrees(attitudeTarget.get_euler_pitch()) * 100; - last_pilot_heading = degrees(attitudeTarget.get_euler_yaw()) * 100; + last_pitch += TRIM_STEP * 100; + if (g.control_frame == MAV_FRAME_BODY_FRD) { + attitude_target.rotate(localPitch * radians(TRIM_STEP)); + last_roll = degrees(attitude_target.get_euler_roll()) * 100; + last_pitch = degrees(attitude_target.get_euler_pitch()) * 100; + last_pilot_heading = degrees(attitude_target.get_euler_yaw()) * 100; + } break; case JSButton::button_function_t::k_trim_pitch_dec: - attitudeTarget.rotate(localPitch * radians(-1)); - last_roll = degrees(attitudeTarget.get_euler_roll()) * 100; - last_pitch = degrees(attitudeTarget.get_euler_pitch()) * 100; - last_pilot_heading = degrees(attitudeTarget.get_euler_yaw()) * 100; + last_pitch -= TRIM_STEP * 100; + if (g.control_frame == MAV_FRAME_BODY_FRD) { + attitude_target.rotate(localPitch * radians(-TRIM_STEP)); + last_roll = degrees(attitude_target.get_euler_roll()) * 100; + last_pitch = degrees(attitude_target.get_euler_pitch()) * 100; + last_pilot_heading = degrees(attitude_target.get_euler_yaw()) * 100; + } break; case JSButton::button_function_t::k_input_hold_set: if(!motors.armed()) {