From 9544b1763be56c116ea90cf3c1473f07ee22341c Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Mon, 26 Jun 2017 17:48:04 +0900 Subject: [PATCH] Copter: replace smoothing gain with AC_AttitudeControl::set_input_tc --- ArduCopter/Attitude.cpp | 7 ------- ArduCopter/Copter.h | 1 - ArduCopter/Parameters.cpp | 9 --------- ArduCopter/Parameters.h | 3 +-- ArduCopter/defines.h | 7 ------- ArduCopter/mode.cpp | 6 +----- ArduCopter/mode.h | 1 - ArduCopter/mode_acro_heli.cpp | 2 +- ArduCopter/mode_althold.cpp | 8 ++++---- ArduCopter/mode_auto.cpp | 14 +++++++------- ArduCopter/mode_autotune.cpp | 8 ++++---- ArduCopter/mode_brake.cpp | 2 +- ArduCopter/mode_circle.cpp | 4 ++-- ArduCopter/mode_drift.cpp | 2 +- ArduCopter/mode_flip.cpp | 2 +- ArduCopter/mode_flowhold.cpp | 8 ++++---- ArduCopter/mode_guided.cpp | 22 +++++++++++----------- ArduCopter/mode_land.cpp | 6 +++--- ArduCopter/mode_loiter.cpp | 8 ++++---- ArduCopter/mode_poshold.cpp | 4 ++-- ArduCopter/mode_rtl.cpp | 10 +++++----- ArduCopter/mode_smart_rtl.cpp | 8 ++++---- ArduCopter/mode_stabilize.cpp | 2 +- ArduCopter/mode_stabilize_heli.cpp | 2 +- ArduCopter/mode_throw.cpp | 6 +++--- ArduCopter/takeoff.cpp | 2 +- ArduCopter/tuning.cpp | 4 ++-- 27 files changed, 64 insertions(+), 94 deletions(-) diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp index 12e578c0da..87eae0e4cf 100644 --- a/ArduCopter/Attitude.cpp +++ b/ArduCopter/Attitude.cpp @@ -1,12 +1,5 @@ #include "Copter.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 Copter::get_smoothing_gain() -{ - return (2.0f + (float)g.rc_feel_rp/10.0f); -} - // get_pilot_desired_heading - transform pilot's yaw input into a // desired yaw rate // returns desired yaw rate in centi-degrees per second diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 8c82ff0130..2657419e95 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -658,7 +658,6 @@ private: void update_altitude(); // Attitude.cpp - float get_smoothing_gain(); float get_pilot_desired_yaw_rate(int16_t stick_angle); float get_roi_yaw(); float get_look_ahead_yaw(); diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 2b8d28a78c..d6b2455e7a 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -459,15 +459,6 @@ const AP_Param::Info Copter::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: PHLD_BRAKE_RATE // @DisplayName: PosHold braking rate // @Description: PosHold flight mode's rotation rate during braking in deg/sec diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 51576fe60c..7f18801a74 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -111,7 +111,7 @@ public: k_param_fs_batt_mah, k_param_angle_rate_max, // remove k_param_rssi_range, // unused, replaced by rssi_ library parameters - k_param_rc_feel_rp, + k_param_rc_feel_rp, // deprecated k_param_NavEKF, // deprecated - remove k_param_mission, // mission library k_param_rc_13_old, @@ -407,7 +407,6 @@ public: AP_Int16 rtl_climb_min; // rtl minimum climb in cm AP_Int8 wp_yaw_behavior; // controls how the autopilot controls yaw during missions - AP_Int8 rc_feel_rp; // controls vehicle response to user input with 0 being extremely soft and 100 begin extremely crisp AP_Int16 poshold_brake_rate; // PosHold flight mode's rotation rate during braking in deg/sec AP_Int16 poshold_brake_angle_max; // PosHold flight mode's max lean angle during braking in centi-degrees diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index f1a18b311a..426f5bd185 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -184,13 +184,6 @@ enum tuning_func { #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 diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 2b1b9b21b4..63552a88ba 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -372,7 +372,7 @@ void Copter::Mode::zero_throttle_and_relax_ac() { #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f, get_smoothing_gain()); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f); attitude_control->set_throttle_out(0.0f, false, copter.g.throttle_filt); #else motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); @@ -414,10 +414,6 @@ void Copter::Mode::update_simple_mode(void) { copter.update_simple_mode(); } -float Copter::Mode::get_smoothing_gain() { - return copter.get_smoothing_gain(); -} - bool Copter::Mode::set_mode(control_mode_t mode, mode_reason_t reason) { return copter.set_mode(mode, reason); diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 1bd983e42e..6bc6ca481b 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -84,7 +84,6 @@ protected: float get_pilot_desired_throttle(int16_t throttle_control, float thr_mid = 0.0f); float get_non_takeoff_throttle(void); void update_simple_mode(void); - float get_smoothing_gain(void); bool set_mode(control_mode_t mode, mode_reason_t reason); void set_land_complete(bool b); GCS_Copter &gcs(); diff --git a/ArduCopter/mode_acro_heli.cpp b/ArduCopter/mode_acro_heli.cpp index cb9fc858a3..5731a0deba 100644 --- a/ArduCopter/mode_acro_heli.cpp +++ b/ArduCopter/mode_acro_heli.cpp @@ -28,7 +28,7 @@ void Copter::ModeAcro_Heli::run() { float target_roll, target_pitch, target_yaw; float pilot_throttle_scaled; - + // Tradheli should not reset roll, pitch, yaw targets when motors are not runup, because // we may be in autorotation flight. These should be reset only when transitioning from disarmed // to armed, because the pilot will have placed the helicopter down on the landing pad. This is so diff --git a/ArduCopter/mode_althold.cpp b/ArduCopter/mode_althold.cpp index 0aabc7b69d..70ab379297 100644 --- a/ArduCopter/mode_althold.cpp +++ b/ArduCopter/mode_althold.cpp @@ -66,7 +66,7 @@ void Copter::ModeAltHold::run() case AltHold_MotorStopped: motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN); - 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); attitude_control->reset_rate_controller_I_terms(); attitude_control->set_yaw_target_to_current_heading(); #if FRAME_CONFIG == HELI_FRAME @@ -104,7 +104,7 @@ void Copter::ModeAltHold::run() target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); // 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); // call position controller pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); @@ -132,7 +132,7 @@ void Copter::ModeAltHold::run() attitude_control->reset_rate_controller_I_terms(); attitude_control->set_yaw_target_to_current_heading(); #endif - 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); pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero pos_control->update_z_controller(); break; @@ -146,7 +146,7 @@ void Copter::ModeAltHold::run() #endif // 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); // adjust climb rate using rangefinder if (copter.rangefinder_alt_ok()) { diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 8821e8da20..20195feb19 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -280,10 +280,10 @@ void Copter::ModeAuto::wp_run() // call attitude controller if (copter.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(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain()); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate); } else { // roll, pitch from waypoint controller, yaw heading from auto_heading() - attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(),true, get_smoothing_gain()); + attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(),true); } } @@ -345,10 +345,10 @@ void Copter::ModeAuto::spline_run() // call attitude controller if (copter.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(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain()); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate); } else { // roll, pitch from waypoint controller, yaw heading from auto_heading() - attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(), true, get_smoothing_gain()); + attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(), true); } } @@ -505,7 +505,7 @@ void Copter::ModeAuto::circle_run() pos_control->update_z_controller(); // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control->input_euler_angle_roll_pitch_yaw(copter.circle_nav->get_roll(), copter.circle_nav->get_pitch(), copter.circle_nav->get_yaw(),true, get_smoothing_gain()); + attitude_control->input_euler_angle_roll_pitch_yaw(copter.circle_nav->get_roll(), copter.circle_nav->get_pitch(), copter.circle_nav->get_yaw(), true); } #if NAV_GUIDED == ENABLED @@ -576,7 +576,7 @@ void Copter::ModeAuto::loiter_run() copter.failsafe_terrain_set_status(wp_nav->update_wpnav()); pos_control->update_z_controller(); - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain()); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate); } #endif @@ -863,7 +863,7 @@ void Copter::ModeAuto::payload_place_run_loiter() // call attitude controller const float target_yaw_rate = 0; - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain()); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate); // call position controller pos_control->update_z_controller(); diff --git a/ArduCopter/mode_autotune.cpp b/ArduCopter/mode_autotune.cpp index 29cc645cd2..1c51592b79 100644 --- a/ArduCopter/mode_autotune.cpp +++ b/ArduCopter/mode_autotune.cpp @@ -366,7 +366,7 @@ void Copter::ModeAutoTune::run() } attitude_control->reset_rate_controller_I_terms(); attitude_control->set_yaw_target_to_current_heading(); - 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); pos_control->relax_alt_hold_controllers(0.0f); pos_control->update_z_controller(); }else{ @@ -406,7 +406,7 @@ void Copter::ModeAutoTune::run() // if pilot override call attitude controller if (pilot_override || mode != TUNING) { - 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{ // somehow get attitude requests from autotuning autotune_attitude_control(); @@ -483,7 +483,7 @@ void Copter::ModeAutoTune::autotune_attitude_control() get_poshold_attitude(roll_cd, pitch_cd, desired_yaw); // hold level attitude - attitude_control->input_euler_angle_roll_pitch_yaw(roll_cd, pitch_cd, desired_yaw, true, get_smoothing_gain()); + attitude_control->input_euler_angle_roll_pitch_yaw(roll_cd, pitch_cd, desired_yaw, true); // hold the copter level for 0.5 seconds before we begin a twitch // reset counter if we are no longer level @@ -859,7 +859,7 @@ void Copter::ModeAutoTune::autotune_attitude_control() positive_direction = !positive_direction; if (axis == YAW) { - attitude_control->input_euler_angle_roll_pitch_yaw(0.0f, 0.0f, ahrs.yaw_sensor, false, get_smoothing_gain()); + attitude_control->input_euler_angle_roll_pitch_yaw(0.0f, 0.0f, ahrs.yaw_sensor, false); } // set gains to their intra-test values (which are very close to the original gains) diff --git a/ArduCopter/mode_brake.cpp b/ArduCopter/mode_brake.cpp index e8a9194d82..3bbb0a2bba 100644 --- a/ArduCopter/mode_brake.cpp +++ b/ArduCopter/mode_brake.cpp @@ -62,7 +62,7 @@ void Copter::ModeBrake::run() wp_nav->update_brake(ekfGndSpdLimit, ekfNavVelGainScaler); // call attitude controller - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), 0.0f, get_smoothing_gain()); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), 0.0f); // body-frame rate controller is run directly from 100hz loop diff --git a/ArduCopter/mode_circle.cpp b/ArduCopter/mode_circle.cpp index 6515978a2a..286ca2a0e3 100644 --- a/ArduCopter/mode_circle.cpp +++ b/ArduCopter/mode_circle.cpp @@ -79,11 +79,11 @@ void Copter::ModeCircle::run() if (pilot_yaw_override) { attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(copter.circle_nav->get_roll(), copter.circle_nav->get_pitch(), - target_yaw_rate, get_smoothing_gain()); + target_yaw_rate); }else{ attitude_control->input_euler_angle_roll_pitch_yaw(copter.circle_nav->get_roll(), copter.circle_nav->get_pitch(), - copter.circle_nav->get_yaw(),true, get_smoothing_gain()); + copter.circle_nav->get_yaw(), true); } // adjust climb rate using rangefinder diff --git a/ArduCopter/mode_drift.cpp b/ArduCopter/mode_drift.cpp index 43d8b107e1..5be83ed3d1 100644 --- a/ArduCopter/mode_drift.cpp +++ b/ArduCopter/mode_drift.cpp @@ -102,7 +102,7 @@ void Copter::ModeDrift::run() motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // 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); // output pilot's throttle with angle boost attitude_control->set_throttle_out(get_throttle_assist(vel.z, pilot_throttle_scaled), true, g.throttle_filt); diff --git a/ArduCopter/mode_flip.cpp b/ArduCopter/mode_flip.cpp index c680d5b135..51a2840087 100644 --- a/ArduCopter/mode_flip.cpp +++ b/ArduCopter/mode_flip.cpp @@ -180,7 +180,7 @@ void Copter::ModeFlip::run() case Flip_Recover: // use originally captured earth-frame angle targets to recover - attitude_control->input_euler_angle_roll_pitch_yaw(flip_orig_attitude.x, flip_orig_attitude.y, flip_orig_attitude.z, false, get_smoothing_gain()); + attitude_control->input_euler_angle_roll_pitch_yaw(flip_orig_attitude.x, flip_orig_attitude.y, flip_orig_attitude.z, false); // increase throttle to gain any lost altitude throttle_out += FLIP_THR_INC; diff --git a/ArduCopter/mode_flowhold.cpp b/ArduCopter/mode_flowhold.cpp index af0f61dd48..63f4e1a39f 100644 --- a/ArduCopter/mode_flowhold.cpp +++ b/ArduCopter/mode_flowhold.cpp @@ -290,7 +290,7 @@ void Copter::ModeFlowHold::run() case FlowHold_MotorStopped: copter.motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN); - copter.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(bf_angles.x, bf_angles.y, target_yaw_rate, copter.get_smoothing_gain()); + copter.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(bf_angles.x, bf_angles.y, target_yaw_rate); #if FRAME_CONFIG == HELI_FRAME // force descent rate and call position controller copter.pos_control->set_alt_target_from_climb_rate(-abs(copter.g.land_speed), copter.G_Dt, false); @@ -323,7 +323,7 @@ void Copter::ModeFlowHold::run() target_climb_rate = copter.get_avoidance_adjusted_climbrate(target_climb_rate); // call attitude controller - copter.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(bf_angles.x, bf_angles.y, target_yaw_rate, copter.get_smoothing_gain()); + copter.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(bf_angles.x, bf_angles.y, target_yaw_rate); // call position controller copter.pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, copter.G_Dt, false); @@ -341,7 +341,7 @@ void Copter::ModeFlowHold::run() copter.attitude_control->reset_rate_controller_I_terms(); copter.attitude_control->set_yaw_target_to_current_heading(); - copter.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(bf_angles.x, bf_angles.y, target_yaw_rate, copter.get_smoothing_gain()); + copter.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(bf_angles.x, bf_angles.y, target_yaw_rate); copter.pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero copter.pos_control->update_z_controller(); break; @@ -355,7 +355,7 @@ void Copter::ModeFlowHold::run() #endif // call attitude controller - copter.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(bf_angles.x, bf_angles.y, target_yaw_rate, copter.get_smoothing_gain()); + copter.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(bf_angles.x, bf_angles.y, target_yaw_rate); // adjust climb rate using rangefinder if (copter.rangefinder_alt_ok()) { diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index ba88b8ae2b..7e2dc697a5 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -435,13 +435,13 @@ void Copter::ModeGuided::pos_control_run() // call attitude controller if (copter.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(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain()); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate); } else if (auto_yaw_mode == AUTO_YAW_RATE) { // roll & pitch from waypoint controller, yaw rate from mavlink command or mission item - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_yaw_rate_cds(), get_smoothing_gain()); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_yaw_rate_cds()); } else { // roll, pitch from waypoint controller, yaw heading from GCS or auto_heading() - attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(), true, get_smoothing_gain()); + attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(), true); } } @@ -489,13 +489,13 @@ void Copter::ModeGuided::vel_control_run() // call attitude controller if (copter.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(pos_control->get_roll(), pos_control->get_pitch(), target_yaw_rate, get_smoothing_gain()); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), target_yaw_rate); } else if (auto_yaw_mode == AUTO_YAW_RATE) { // roll & pitch from velocity controller, yaw rate from mavlink command or mission item - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), get_auto_yaw_rate_cds(), get_smoothing_gain()); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), get_auto_yaw_rate_cds()); } else { // roll, pitch from waypoint controller, yaw heading from GCS or auto_heading() - attitude_control->input_euler_angle_roll_pitch_yaw(pos_control->get_roll(), pos_control->get_pitch(), get_auto_heading(), true, get_smoothing_gain()); + attitude_control->input_euler_angle_roll_pitch_yaw(pos_control->get_roll(), pos_control->get_pitch(), get_auto_heading(), true); } } @@ -561,13 +561,13 @@ void Copter::ModeGuided::posvel_control_run() // call attitude controller if (copter.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(pos_control->get_roll(), pos_control->get_pitch(), target_yaw_rate, get_smoothing_gain()); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), target_yaw_rate); } else if (auto_yaw_mode == AUTO_YAW_RATE) { // roll & pitch from position-velocity controller, yaw rate from mavlink command or mission item - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), get_auto_yaw_rate_cds(), get_smoothing_gain()); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), get_auto_yaw_rate_cds()); } else { // roll, pitch from waypoint controller, yaw heading from GCS or auto_heading() - attitude_control->input_euler_angle_roll_pitch_yaw(pos_control->get_roll(), pos_control->get_pitch(), get_auto_heading(), true, get_smoothing_gain()); + attitude_control->input_euler_angle_roll_pitch_yaw(pos_control->get_roll(), pos_control->get_pitch(), get_auto_heading(), true); } } @@ -620,9 +620,9 @@ void Copter::ModeGuided::angle_control_run() // call attitude controller if (guided_angle_state.use_yaw_rate) { - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(roll_in, pitch_in, yaw_rate_in, get_smoothing_gain()); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(roll_in, pitch_in, yaw_rate_in); } else { - 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 diff --git a/ArduCopter/mode_land.cpp b/ArduCopter/mode_land.cpp index 8c28e9c89b..c0049391ac 100644 --- a/ArduCopter/mode_land.cpp +++ b/ArduCopter/mode_land.cpp @@ -110,7 +110,7 @@ void Copter::ModeLand::nogps_run() if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) { #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw // 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); attitude_control->set_throttle_out(0,false,g.throttle_filt); #else motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); @@ -129,7 +129,7 @@ void Copter::ModeLand::nogps_run() motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // 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); // pause before beginning land descent if(land_pause && millis()-land_start_time >= LAND_WITH_DELAY_MS) { @@ -288,7 +288,7 @@ void Copter::land_run_horizontal_control() // call attitude controller - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(nav_roll, nav_pitch, target_yaw_rate, get_smoothing_gain()); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(nav_roll, nav_pitch, target_yaw_rate); } // do_not_use_GPS - forces land-mode to not use the GPS but instead rely on pilot input for roll and pitch diff --git a/ArduCopter/mode_loiter.cpp b/ArduCopter/mode_loiter.cpp index b31b09a0cd..e3f5aa2f89 100644 --- a/ArduCopter/mode_loiter.cpp +++ b/ArduCopter/mode_loiter.cpp @@ -128,7 +128,7 @@ void Copter::ModeLoiter::run() pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero #endif wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain()); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate); pos_control->update_z_controller(); break; @@ -155,7 +155,7 @@ void Copter::ModeLoiter::run() wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); // call attitude controller - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain()); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate); // update altitude target and call position controller pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); @@ -173,7 +173,7 @@ void Copter::ModeLoiter::run() wp_nav->init_loiter_target(); attitude_control->reset_rate_controller_I_terms(); attitude_control->set_yaw_target_to_current_heading(); - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain()); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0); pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero pos_control->update_z_controller(); break; @@ -193,7 +193,7 @@ void Copter::ModeLoiter::run() wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); // call attitude controller - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain()); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate); // adjust climb rate using rangefinder if (copter.rangefinder_alt_ok()) { diff --git a/ArduCopter/mode_poshold.cpp b/ArduCopter/mode_poshold.cpp index c494ec84d4..84d5d07601 100644 --- a/ArduCopter/mode_poshold.cpp +++ b/ArduCopter/mode_poshold.cpp @@ -195,7 +195,7 @@ void Copter::ModePosHold::run() wp_nav->init_loiter_target(); attitude_control->reset_rate_controller_I_terms(); attitude_control->set_yaw_target_to_current_heading(); - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain()); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0); pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero pos_control->update_z_controller(); return; @@ -522,7 +522,7 @@ void Copter::ModePosHold::run() poshold.pitch = constrain_int16(poshold.pitch, -angle_max, angle_max); // update attitude controller targets - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(poshold.roll, poshold.pitch, target_yaw_rate, get_smoothing_gain()); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(poshold.roll, poshold.pitch, target_yaw_rate); // adjust climb rate using rangefinder if (copter.rangefinder_alt_ok()) { diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index 5d871100f0..1c721d3f05 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -159,10 +159,10 @@ void Copter::ModeRTL::climb_return_run() // call attitude controller if (copter.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(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain()); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate); }else{ // roll, pitch from waypoint controller, yaw heading from auto_heading() - attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(),true, get_smoothing_gain()); + attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(),true); } // check if we've completed this stage of RTL @@ -217,10 +217,10 @@ void Copter::ModeRTL::loiterathome_run() // call attitude controller if (copter.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(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain()); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate); }else{ // roll, pitch from waypoint controller, yaw heading from auto_heading() - attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(),true, get_smoothing_gain()); + attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(),true); } // check if we've completed this stage of RTL @@ -305,7 +305,7 @@ void Copter::ModeRTL::descent_run() pos_control->update_z_controller(); // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain()); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate); // check if we've reached within 20cm of final altitude _state_complete = labs(rtl_path.descent_target.alt - copter.current_loc.alt) < 20; diff --git a/ArduCopter/mode_smart_rtl.cpp b/ArduCopter/mode_smart_rtl.cpp index ebeed8ad53..fa234c16fe 100644 --- a/ArduCopter/mode_smart_rtl.cpp +++ b/ArduCopter/mode_smart_rtl.cpp @@ -65,7 +65,7 @@ void Copter::ModeSmartRTL::wait_cleanup_run() motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); wp_nav->update_wpnav(); pos_control->update_z_controller(); - attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(),true, get_smoothing_gain()); + attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(),true); // check if return path is computed and if yes, begin journey home if (g2.smart_rtl.request_thorough_cleanup()) { @@ -103,10 +103,10 @@ void Copter::ModeSmartRTL::path_follow_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(wp_nav->get_roll(), wp_nav->get_pitch(), 0, get_smoothing_gain()); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), 0); } else { // roll, pitch from waypoint controller, yaw heading from auto_heading() - attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(),true, get_smoothing_gain()); + attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(),true); } } @@ -129,7 +129,7 @@ void Copter::ModeSmartRTL::pre_land_position_run() motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); wp_nav->update_wpnav(); pos_control->update_z_controller(); - attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(), true, get_smoothing_gain()); + attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(), true); } // save current position for use by the smart_rtl flight mode diff --git a/ArduCopter/mode_stabilize.cpp b/ArduCopter/mode_stabilize.cpp index ca1f6447b0..6740184d7b 100644 --- a/ArduCopter/mode_stabilize.cpp +++ b/ArduCopter/mode_stabilize.cpp @@ -53,7 +53,7 @@ void Copter::ModeStabilize::run() pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->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); // body-frame rate controller is run directly from 100hz loop diff --git a/ArduCopter/mode_stabilize_heli.cpp b/ArduCopter/mode_stabilize_heli.cpp index 7a70c3aa90..fb6461acd9 100644 --- a/ArduCopter/mode_stabilize_heli.cpp +++ b/ArduCopter/mode_stabilize_heli.cpp @@ -65,7 +65,7 @@ void Copter::ModeStabilize_Heli::run() pilot_throttle_scaled = copter.input_manager.get_pilot_desired_collective(channel_throttle->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); // output pilot's throttle - note that TradHeli does not used angle-boost attitude_control->set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt); diff --git a/ArduCopter/mode_throw.cpp b/ArduCopter/mode_throw.cpp index 3902103f74..56e4688964 100644 --- a/ArduCopter/mode_throw.cpp +++ b/ArduCopter/mode_throw.cpp @@ -141,7 +141,7 @@ void Copter::ModeThrow::run() motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // demand a level roll/pitch attitude with zero yaw rate - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f, get_smoothing_gain()); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f); // output 50% throttle and turn off angle boost to maximise righting moment attitude_control->set_throttle_out(0.5f, false, g.throttle_filt); @@ -154,7 +154,7 @@ void Copter::ModeThrow::run() motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // call attitude controller - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f, get_smoothing_gain()); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f); // call height controller pos_control->set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false); @@ -171,7 +171,7 @@ void Copter::ModeThrow::run() wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); // call attitude controller - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), 0.0f, get_smoothing_gain()); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), 0.0f); // call height controller pos_control->set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false); diff --git a/ArduCopter/takeoff.cpp b/ArduCopter/takeoff.cpp index 5116c8120e..44e54bcea3 100644 --- a/ArduCopter/takeoff.cpp +++ b/ArduCopter/takeoff.cpp @@ -182,5 +182,5 @@ void Copter::auto_takeoff_attitude_run(float target_yaw_rate) } // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(nav_roll, nav_pitch, target_yaw_rate, get_smoothing_gain()); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(nav_roll, nav_pitch, target_yaw_rate); } diff --git a/ArduCopter/tuning.cpp b/ArduCopter/tuning.cpp index 9a5a3a95d6..305eb11612 100644 --- a/ArduCopter/tuning.cpp +++ b/ArduCopter/tuning.cpp @@ -179,8 +179,8 @@ void Copter::tuning() { #endif case TUNING_RC_FEEL_RP: - // roll-pitch input smoothing - g.rc_feel_rp = control_in / 10; + // convert from control_in to input time constant + attitude_control->set_input_tc(1.0f / (2.f + MAX((control_in/100.0f),0.0f))); break; case TUNING_RATE_PITCH_KP: