Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: replace input filtering with smoothing gain

Browse Source
This commit is contained in:
Randy Mackay 2014-02-19 20:51:13 +09:00
parent 429e8d5e50
commit 3910ab807f
1 changed files with 8 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
ArduCopter/Attitude.pde
View File

@ -1,13 +1,10 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// local variables // get_smoothing_gain - returns smoothing gain to be passed into attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth
float roll_in_filtered; // roll-in in filtered with RC_FEEL_RP parameter // result is a number from 2 to 12 with 2 being very sluggish and 12 being very crisp
float pitch_in_filtered; // pitch-in filtered with RC_FEEL_RP parameter float get_smoothing_gain()
static void reset_roll_pitch_in_filters(int16_t roll_in, int16_t pitch_in)
{ {
roll_in_filtered = constrain_int16(roll_in, -ROLL_PITCH_INPUT_MAX, ROLL_PITCH_INPUT_MAX); return (2.0f + (float)g.rc_feel_rp/10.0f);
pitch_in_filtered = constrain_int16(pitch_in, -ROLL_PITCH_INPUT_MAX, ROLL_PITCH_INPUT_MAX);
} }
// get_pilot_desired_angle - transform pilot's roll or pitch input into a desired lean angle // 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); 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); 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 // return filtered roll if no scaling required
if (aparm.angle_max == ROLL_PITCH_INPUT_MAX) { if (aparm.angle_max == ROLL_PITCH_INPUT_MAX) {
roll_out = (int16_t)roll_in_filtered; roll_out = roll_in;
pitch_out = (int16_t)pitch_in_filtered; pitch_out = pitch_in;
return; 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 // convert pilot input to lean angle
roll_out = (int16_t)(roll_in_filtered * _scaler); roll_out = (int16_t)((float)roll_in * _scaler);
pitch_out = (int16_t)(pitch_in_filtered * _scaler); pitch_out = (int16_t)((float)pitch_in * _scaler);
} }
// get_pilot_desired_heading - transform pilot's yaw input into a desired heading // get_pilot_desired_heading - transform pilot's yaw input into a desired heading