Browse Source

Copter: replace input filtering with smoothing gain

mission-4.1.18
Randy Mackay 11 years ago
parent
commit
3910ab807f
  1. 40
      ArduCopter/Attitude.pde

40
ArduCopter/Attitude.pde

@ -1,13 +1,10 @@ @@ -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 @@ -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 @@ -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

Loading…
Cancel
Save