aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

ArduCopter - Attitude.pde - updated get_rate_roll and get_rate_pitch to use new average filter.

Browse Source 
modified all int to int16_t in Attitude.pde
This commit is contained in:
rmackay9 2012-02-28 21:26:37 +09:00
parent 630de04bd1
commit 082cc70f1c
1 changed files with 52 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

113
ArduCopter/Attitude.pde
View File

@ -1,6 +1,6 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
static int
static int16_t
get_stabilize_roll(int32_t target_angle)
{
// angle error
@ -29,7 +29,7 @@ get_stabilize_roll(int32_t target_angle)
#endif
}
static int
static int16_t
get_stabilize_pitch(int32_t target_angle)
{
// angle error
@ -56,7 +56,7 @@ get_stabilize_pitch(int32_t target_angle)
#endif
}
static int
static int16_t
get_stabilize_yaw(int32_t target_angle)
{
// angle error
@ -85,7 +85,7 @@ get_stabilize_yaw(int32_t target_angle)
#endif
}
static int
static int16_t
get_acro_roll(int32_t target_rate)
{
target_rate = target_rate * g.acro_p;
@ -93,7 +93,7 @@ get_acro_roll(int32_t target_rate)
return get_rate_roll(target_rate);
}
static int
static int16_t
get_acro_pitch(int32_t target_rate)
{
target_rate = target_rate * g.acro_p;
@ -101,7 +101,7 @@ get_acro_pitch(int32_t target_rate)
return get_rate_pitch(target_rate);
}
static int
static int16_t
get_acro_yaw(int32_t target_rate)
{
target_rate = g.pi_stabilize_yaw.get_p(target_rate);
@ -109,78 +109,69 @@ get_acro_yaw(int32_t target_rate)
return get_rate_yaw(target_rate);
}
static int
static int16_t
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 AverageFilterInt32_Size3 rate_d_filter; // filtered acceleration
static int32_t last_rate = 0; // previous iterations rate
int32_t current_rate; // this iteration's rate
int32_t rate_d; // roll's acceleration
int32_t output; // output from pid controller
int32_t rate_d_dampener; // value to dampen output based on acceleration
int32_t current_rate = (omega.x * DEGX100);
// get current rate
current_rate = (omega.x * DEGX100);
// History of last 3 dir
rate_d3 = rate_d2;
rate_d2 = rate_d1;
rate_d1 = current_rate - last_rate;
// calculate and filter the acceleration
rate_d = rate_d_filter.apply(current_rate - last_rate);
// store rate for next iteration
last_rate = current_rate;
// rate control
target_rate = target_rate - current_rate;
target_rate = g.pid_rate_roll.get_pid(target_rate, G_Dt);
// call pid controller
output = g.pid_rate_roll.get_pid(target_rate - current_rate, G_Dt);
// Dampening
//int16_t d_temp = (float)(current_rate - last_rate) * g.stabilize_d;
//target_rate -= constrain(d_temp, -500, 500);
//last_rate = current_rate;
// Dampening output with D term
rate_d_dampener = rate_d * g.stabilize_d;
rate_d_dampener = constrain(rate_d_dampener, -400, 400);
output -= rate_d_dampener;
// 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;
d_temp = constrain(d_temp, -400, 400);
target_rate -= d_temp;
// output control:
return constrain(target_rate, -2500, 2500);
// output control
return constrain(output, -2500, 2500);
}
static int
static int16_t
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 AverageFilterInt32_Size3 rate_d_filter; // filtered acceleration
static int32_t last_rate = 0; // previous iterations rate
int32_t current_rate; // this iteration's rate
int32_t rate_d; // roll's acceleration
int32_t output; // output from pid controller
int32_t rate_d_dampener; // value to dampen output based on acceleration
int32_t current_rate = (omega.y * DEGX100);
// get current rate
current_rate = (omega.y * DEGX100);
// History of last 3 dir
rate_d3 = rate_d2;
rate_d2 = rate_d1;
rate_d1 = current_rate - last_rate;
// calculate and filter the acceleration
rate_d = rate_d_filter.apply(current_rate - last_rate);
// store rate for next iteration
last_rate = current_rate;
// rate control
target_rate = target_rate - current_rate;
target_rate = g.pid_rate_pitch.get_pid(target_rate, G_Dt);
// call pid controller
output = g.pid_rate_pitch.get_pid(target_rate - current_rate, G_Dt);
// Dampening
//int16_t d_temp = (float)(current_rate - last_rate) * g.stabilize_d;
//target_rate -= constrain(d_temp, -500, 500);
//last_rate = current_rate;
// Dampening output with D term
rate_d_dampener = rate_d * g.stabilize_d;
rate_d_dampener = constrain(rate_d_dampener, -400, 400);
output -= rate_d_dampener;
// D term
int16_t d_temp = (rate_d1 + rate_d2 + rate_d3) * g.stabilize_d;
d_temp = constrain(d_temp, -400, 400);
target_rate -= d_temp;
// output control:
return constrain(target_rate, -2500, 2500);
// output control
return constrain(output, -2500, 2500);
}
static int
static int16_t
get_rate_yaw(int32_t target_rate)
{
// rate control
@ -346,7 +337,7 @@ get_nav_yaw_offset(int yaw_input, int reset)
}
}
static int get_angle_boost(int value)
static int16_t get_angle_boost(int16_t value)
{
float temp = cos_pitch_x * cos_roll_x;
temp = 1.0 - constrain(temp, .5, 1.0);
@ -496,7 +487,7 @@ get_of_roll(int32_t control_roll)
{
#ifdef OPTFLOW_ENABLED
static float tot_x_cm = 0; // total distance from target
static unsigned long last_of_roll_update = 0;
static uint32_t last_of_roll_update = 0;
int32_t new_roll = 0;
// check if new optflow data available
@ -530,7 +521,7 @@ get_of_pitch(int32_t control_pitch)
{
#ifdef OPTFLOW_ENABLED
static float tot_y_cm = 0; // total distance from target
static unsigned long last_of_pitch_update = 0;
static uint32_t last_of_pitch_update = 0;
int32_t new_pitch = 0;
// check if new optflow data available