From da17034a3df8872b411bfd428ce56a6ab1cc636e Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 10 Jul 2017 11:49:42 +0900 Subject: [PATCH] Sub: replace smoothing gain with AC_AttitudeControl::set_input_tc --- ArduSub/Attitude.cpp | 7 ------- ArduSub/Parameters.cpp | 9 --------- ArduSub/control_althold.cpp | 6 +++--- ArduSub/control_auto.cpp | 14 +++++++------- ArduSub/control_circle.cpp | 4 ++-- ArduSub/control_guided.cpp | 15 +++++++-------- ArduSub/control_poshold.cpp | 8 ++++---- ArduSub/control_stabilize.cpp | 6 +++--- ArduSub/control_surface.cpp | 2 +- ArduSub/defines.h | 7 ------- 10 files changed, 27 insertions(+), 51 deletions(-) diff --git a/ArduSub/Attitude.cpp b/ArduSub/Attitude.cpp index 4019b006a0..7a1bf44517 100644 --- a/ArduSub/Attitude.cpp +++ b/ArduSub/Attitude.cpp @@ -1,12 +1,5 @@ #include "Sub.h" -// get_smoothing_gain - returns smoothing gain to be passed into attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw -// result is a number from 2 to 12 with 2 being very sluggish and 12 being very crisp -float Sub::get_smoothing_gain() -{ - return (2.0f + (float)g.rc_feel_rp/10.0f); -} - // get_pilot_desired_angle - transform pilot's roll or pitch input into a desired lean angle // returns desired angle in centi-degrees void Sub::get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &roll_out, float &pitch_out, float angle_max) diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp index 72bc198f9a..16d5249c7b 100644 --- a/ArduSub/Parameters.cpp +++ b/ArduSub/Parameters.cpp @@ -255,15 +255,6 @@ const AP_Param::Info Sub::var_info[] = { // @User: Advanced ASCALAR(angle_max, "ANGLE_MAX", DEFAULT_ANGLE_MAX), - // @Param: RC_FEEL_RP - // @DisplayName: RC Feel Roll/Pitch - // @Description: RC feel for roll/pitch which controls vehicle response to user input with 0 being extremely soft and 100 being crisp - // @Range: 0 100 - // @Increment: 10 - // @User: Standard - // @Values: 0:Very Soft, 25:Soft, 50:Medium, 75:Crisp, 100:Very Crisp - GSCALAR(rc_feel_rp, "RC_FEEL_RP", RC_FEEL_RP_MEDIUM), - // @Param: FS_EKF_ACTION // @DisplayName: EKF Failsafe Action // @Description: Controls the action that will be taken when an EKF failsafe is invoked diff --git a/ArduSub/control_althold.cpp b/ArduSub/control_althold.cpp index d9ab9bc63a..02e5886562 100644 --- a/ArduSub/control_althold.cpp +++ b/ArduSub/control_althold.cpp @@ -59,7 +59,7 @@ void Sub::althold_run() // 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, get_smoothing_gain()); + 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 @@ -71,11 +71,11 @@ void Sub::althold_run() 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, get_smoothing_gain()); + 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, get_smoothing_gain()); + attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, last_pilot_heading, true); } } diff --git a/ArduSub/control_auto.cpp b/ArduSub/control_auto.cpp index dd7f09faf8..b9fa2df8d6 100644 --- a/ArduSub/control_auto.cpp +++ b/ArduSub/control_auto.cpp @@ -165,10 +165,10 @@ void Sub::auto_wp_run() // call attitude controller if (auto_yaw_mode == AUTO_YAW_HOLD) { // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); } else { // roll, pitch from waypoint controller, yaw heading from auto_heading() - attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, get_auto_heading(), true, get_smoothing_gain()); + attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, get_auto_heading(), true); } } @@ -242,10 +242,10 @@ void Sub::auto_spline_run() // call attitude controller if (auto_yaw_mode == AUTO_YAW_HOLD) { // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); } else { // roll, pitch from waypoint controller, yaw heading from auto_heading() - attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, get_auto_heading(), true, get_smoothing_gain()); + attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, get_auto_heading(), true); } } @@ -332,7 +332,7 @@ void Sub::auto_circle_run() pos_control.update_z_controller(); // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control.input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), circle_nav.get_yaw(), true, get_smoothing_gain()); + attitude_control.input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), circle_nav.get_yaw(), true); } #if NAV_GUIDED == ENABLED @@ -425,7 +425,7 @@ void Sub::auto_loiter_run() get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max); // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); } // get_default_auto_yaw_mode - returns auto_yaw_mode based on WP_YAW_BEHAVIOR parameter @@ -761,5 +761,5 @@ void Sub::auto_terrain_recover_run() float target_yaw_rate = 0; // call attitude controller - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); } diff --git a/ArduSub/control_circle.cpp b/ArduSub/control_circle.cpp index f2e061f24a..3ba4b121bd 100644 --- a/ArduSub/control_circle.cpp +++ b/ArduSub/control_circle.cpp @@ -78,9 +78,9 @@ void Sub::circle_run() // call attitude controller if (circle_pilot_yaw_override) { - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate, get_smoothing_gain()); + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate); } else { - attitude_control.input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), circle_nav.get_yaw(), true, get_smoothing_gain()); + attitude_control.input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), circle_nav.get_yaw(), true); } // adjust climb rate using rangefinder diff --git a/ArduSub/control_guided.cpp b/ArduSub/control_guided.cpp index 436d3b2ba6..0a7411835d 100644 --- a/ArduSub/control_guided.cpp +++ b/ArduSub/control_guided.cpp @@ -39,7 +39,6 @@ bool Sub::guided_init(bool ignore_checks) if (!position_ok() && !ignore_checks) { return false; } - // initialise yaw set_auto_yaw_mode(get_default_auto_yaw_mode(false)); // start in position control mode @@ -329,10 +328,10 @@ void Sub::guided_pos_control_run() // call attitude controller if (auto_yaw_mode == AUTO_YAW_HOLD) { // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate, get_smoothing_gain()); + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate); } else { // roll, pitch from waypoint controller, yaw heading from auto_heading() - attitude_control.input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), true, get_smoothing_gain()); + attitude_control.input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), true); } } @@ -383,10 +382,10 @@ void Sub::guided_vel_control_run() // call attitude controller if (auto_yaw_mode == AUTO_YAW_HOLD) { // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate, get_smoothing_gain()); + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate); } else { // roll, pitch from waypoint controller, yaw heading from auto_heading() - attitude_control.input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), true, get_smoothing_gain()); + attitude_control.input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), true); } } @@ -459,10 +458,10 @@ void Sub::guided_posvel_control_run() // call attitude controller if (auto_yaw_mode == AUTO_YAW_HOLD) { // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate, get_smoothing_gain()); + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate); } else { // roll, pitch from waypoint controller, yaw heading from auto_heading() - attitude_control.input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), true, get_smoothing_gain()); + attitude_control.input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), true); } } @@ -509,7 +508,7 @@ void Sub::guided_angle_control_run() motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // call attitude controller - attitude_control.input_euler_angle_roll_pitch_yaw(roll_in, pitch_in, yaw_in, true, get_smoothing_gain()); + attitude_control.input_euler_angle_roll_pitch_yaw(roll_in, pitch_in, yaw_in, true); // call position controller pos_control.set_alt_target_from_climb_rate_ff(climb_rate_cms, G_Dt, false); diff --git a/ArduSub/control_poshold.cpp b/ArduSub/control_poshold.cpp index c00a3be197..2413e0091f 100644 --- a/ArduSub/control_poshold.cpp +++ b/ArduSub/control_poshold.cpp @@ -36,7 +36,7 @@ bool Sub::poshold_init() void Sub::poshold_run() { uint32_t tnow = AP_HAL::millis(); - + // if not armed set throttle to zero and exit immediately if (!motors.armed()) { motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); @@ -85,7 +85,7 @@ void Sub::poshold_run() // 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, get_smoothing_gain()); + 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 @@ -97,11 +97,11 @@ void Sub::poshold_run() 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, get_smoothing_gain()); + 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, get_smoothing_gain()); + attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, last_pilot_heading, true); } } diff --git a/ArduSub/control_stabilize.cpp b/ArduSub/control_stabilize.cpp index a422996962..968536146c 100644 --- a/ArduSub/control_stabilize.cpp +++ b/ArduSub/control_stabilize.cpp @@ -39,7 +39,7 @@ void Sub::stabilize_run() // 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, get_smoothing_gain()); + 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 @@ -51,11 +51,11 @@ void Sub::stabilize_run() 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, get_smoothing_gain()); + 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, get_smoothing_gain()); + attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, last_pilot_heading, true); } } diff --git a/ArduSub/control_surface.cpp b/ArduSub/control_surface.cpp index a211e686e7..4e501725b5 100644 --- a/ArduSub/control_surface.cpp +++ b/ArduSub/control_surface.cpp @@ -48,7 +48,7 @@ void Sub::surface_run() target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); // call attitude controller - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); // set target climb rate float cmb_rate = constrain_float(abs(wp_nav.get_speed_up()), 1, pos_control.get_speed_up()); diff --git a/ArduSub/defines.h b/ArduSub/defines.h index 041b24039c..7242b23fe2 100644 --- a/ArduSub/defines.h +++ b/ArduSub/defines.h @@ -65,13 +65,6 @@ enum mode_reason_t { #define ACRO_TRAINER_LEVELING 1 #define ACRO_TRAINER_LIMITED 2 -// RC Feel roll/pitch definitions -#define RC_FEEL_RP_VERY_SOFT 0 -#define RC_FEEL_RP_SOFT 25 -#define RC_FEEL_RP_MEDIUM 50 -#define RC_FEEL_RP_CRISP 75 -#define RC_FEEL_RP_VERY_CRISP 100 - // Yaw behaviours during missions - possible values for WP_YAW_BEHAVIOR parameter #define WP_YAW_BEHAVIOR_NONE 0 // auto pilot will never control yaw during missions or rtl (except for DO_CONDITIONAL_YAW command received) #define WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP 1 // auto pilot will face next waypoint or home during rtl