diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 96e9da7277..2f540bf545 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -1,13 +1,10 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -// local variables -float roll_in_filtered; // roll-in in filtered with RC_FEEL_RP parameter -float pitch_in_filtered; // pitch-in filtered with RC_FEEL_RP parameter - -static void reset_roll_pitch_in_filters(int16_t roll_in, int16_t pitch_in) +// get_smoothing_gain - returns smoothing gain to be passed into attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth +// result is a number from 2 to 12 with 2 being very sluggish and 12 being very crisp +float get_smoothing_gain() { - roll_in_filtered = constrain_int16(roll_in, -ROLL_PITCH_INPUT_MAX, ROLL_PITCH_INPUT_MAX); - pitch_in_filtered = constrain_int16(pitch_in, -ROLL_PITCH_INPUT_MAX, ROLL_PITCH_INPUT_MAX); + 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 @@ -21,31 +18,10 @@ static void get_pilot_desired_lean_angles(int16_t roll_in, int16_t pitch_in, int roll_in = constrain_int16(roll_in, -ROLL_PITCH_INPUT_MAX, ROLL_PITCH_INPUT_MAX); pitch_in = constrain_int16(pitch_in, -ROLL_PITCH_INPUT_MAX, ROLL_PITCH_INPUT_MAX); - // filter input for feel - if (g.rc_feel_rp >= RC_FEEL_RP_VERY_CRISP) { - // no filtering required - roll_in_filtered = roll_in; - pitch_in_filtered = pitch_in; - }else{ - float filter_gain; - if (g.rc_feel_rp >= RC_FEEL_RP_CRISP) { - filter_gain = 0.5; - } else if(g.rc_feel_rp >= RC_FEEL_RP_MEDIUM) { - filter_gain = 0.3; - } else if(g.rc_feel_rp >= RC_FEEL_RP_SOFT) { - filter_gain = 0.05; - } else { - // must be RC_FEEL_RP_VERY_SOFT - filter_gain = 0.02; - } - roll_in_filtered = roll_in_filtered * (1.0 - filter_gain) + (float)roll_in * filter_gain; - pitch_in_filtered = pitch_in_filtered * (1.0 - filter_gain) + (float)pitch_in * filter_gain; - } - // return filtered roll if no scaling required if (aparm.angle_max == ROLL_PITCH_INPUT_MAX) { - roll_out = (int16_t)roll_in_filtered; - pitch_out = (int16_t)pitch_in_filtered; + roll_out = roll_in; + pitch_out = pitch_in; return; } @@ -56,8 +32,8 @@ static void get_pilot_desired_lean_angles(int16_t roll_in, int16_t pitch_in, int } // convert pilot input to lean angle - roll_out = (int16_t)(roll_in_filtered * _scaler); - pitch_out = (int16_t)(pitch_in_filtered * _scaler); + roll_out = (int16_t)((float)roll_in * _scaler); + pitch_out = (int16_t)((float)pitch_in * _scaler); } // get_pilot_desired_heading - transform pilot's yaw input into a desired heading