Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

re-implemented WII Dampening filter for Marco.

This commit is contained in:
Jason Short 2012-02-16 22:38:23 -08:00
parent b052dab80d
commit 5218220f0f
1 changed files with 41 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
ArduCopter/Attitude.pde
View File

@ -107,17 +107,34 @@ get_acro_yaw(int32_t target_rate)
static int static int
get_rate_roll(int32_t target_rate) get_rate_roll(int32_t target_rate)
{ {
int16_t rate_d1 = 0;
static int16_t rate_d2 = 0;
static int16_t rate_d3 = 0;
static int32_t last_rate = 0; static int32_t last_rate = 0;
int32_t current_rate = (omega.x * DEGX100); int32_t current_rate = (omega.x * DEGX100);
// History of last 3 dir
rate_d3 = rate_d2;
rate_d2 = rate_d1;
rate_d1 = current_rate - last_rate;
last_rate = current_rate;
// rate control // rate control
target_rate = target_rate - current_rate; target_rate = target_rate - current_rate;
target_rate = g.pid_rate_roll.get_pid(target_rate, G_Dt); target_rate = g.pid_rate_roll.get_pid(target_rate, G_Dt);
// Dampening // Dampening
int16_t d_temp = (float)(current_rate - last_rate) * g.stabilize_d; //int16_t d_temp = (float)(current_rate - last_rate) * g.stabilize_d;
target_rate -= constrain(d_temp, -500, 500); //target_rate -= constrain(d_temp, -500, 500);
last_rate = current_rate; //last_rate = current_rate;
// D term
// I had tried this before with little result. Recently, someone mentioned to me that
// MultiWii uses a filter of the last three to get around noise and get a stronger signal.
// Works well! Thanks!
int16_t d_temp = (rate_d1 + rate_d2 + rate_d3) * g.stabilize_d;
target_rate -= d_temp;
// output control: // output control:
return constrain(target_rate, -2500, 2500); return constrain(target_rate, -2500, 2500);
@ -126,17 +143,31 @@ get_rate_roll(int32_t target_rate)
static int static int
get_rate_pitch(int32_t target_rate) get_rate_pitch(int32_t target_rate)
{ {
int16_t rate_d1 = 0;
static int16_t rate_d2 = 0;
static int16_t rate_d3 = 0;
static int32_t last_rate = 0; static int32_t last_rate = 0;
int32_t current_rate = (omega.y * DEGX100); int32_t current_rate = (omega.y * DEGX100);
// History of last 3 dir
rate_d3 = rate_d2;
rate_d2 = rate_d1;
rate_d1 = current_rate - last_rate;
last_rate = current_rate;
// rate control // rate control
target_rate = target_rate - current_rate; target_rate = target_rate - current_rate;
target_rate = g.pid_rate_pitch.get_pid(target_rate, G_Dt); target_rate = g.pid_rate_pitch.get_pid(target_rate, G_Dt);
// Dampening // Dampening
int16_t d_temp = (float)(current_rate - last_rate) * g.stabilize_d; //int16_t d_temp = (float)(current_rate - last_rate) * g.stabilize_d;
target_rate -= constrain(d_temp, -500, 500); //target_rate -= constrain(d_temp, -500, 500);
last_rate = current_rate; //last_rate = current_rate;
// D term
int16_t d_temp = (rate_d1 + rate_d2 + rate_d3) * g.stabilize_d;
target_rate -= d_temp;
// output control: // output control:
return constrain(target_rate, -2500, 2500); return constrain(target_rate, -2500, 2500);