mirror of https://github.com/ArduPilot/ardupilot
Refined the D term for stabilize
moved all the rest_I terms from code into 1 central place
This commit is contained in:
parent
ba535ed956
commit
d991a22453
|
@ -3,23 +3,12 @@
|
||||||
static int
|
static int
|
||||||
get_stabilize_roll(int32_t target_angle)
|
get_stabilize_roll(int32_t target_angle)
|
||||||
{
|
{
|
||||||
int32_t error;
|
int32_t error = 0;
|
||||||
int32_t rate;
|
int32_t rate = 0;
|
||||||
int32_t current_rate;
|
|
||||||
|
|
||||||
int16_t rate_d1 = 0;
|
static float current_rate = 0;
|
||||||
static int16_t rate_d2 = 0;
|
|
||||||
static int16_t rate_d3 = 0;
|
|
||||||
static int32_t last_rate = 0;
|
|
||||||
|
|
||||||
current_rate = (omega.x * DEGX100);
|
current_rate = (current_rate *.7) + (omega.x * DEGX100) * .3;
|
||||||
|
|
||||||
// playing with double derivatives.
|
|
||||||
// History of last 3 dir
|
|
||||||
rate_d3 = rate_d2;
|
|
||||||
rate_d2 = rate_d1;
|
|
||||||
rate_d1 = current_rate - last_rate;
|
|
||||||
last_rate = current_rate;
|
|
||||||
|
|
||||||
// angle error
|
// angle error
|
||||||
error = wrap_180(target_angle - dcm.roll_sensor);
|
error = wrap_180(target_angle - dcm.roll_sensor);
|
||||||
|
@ -45,15 +34,11 @@ get_stabilize_roll(int32_t target_angle)
|
||||||
int16_t iterm = g.pi_stabilize_roll.get_i(error, G_Dt);
|
int16_t iterm = g.pi_stabilize_roll.get_i(error, G_Dt);
|
||||||
|
|
||||||
// rate control
|
// rate control
|
||||||
error = rate - current_rate;
|
error = rate - (omega.x * DEGX100);
|
||||||
rate = g.pi_rate_roll.get_pi(error, G_Dt);
|
rate = g.pi_rate_roll.get_pi(error, G_Dt);
|
||||||
|
|
||||||
// D term
|
// D term
|
||||||
// I had tried this before with little result. Recently, someone mentioned to me that
|
int16_t d_temp = current_rate * g.stablize_d;
|
||||||
// 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.stablize_d;
|
|
||||||
|
|
||||||
rate -= d_temp;
|
rate -= d_temp;
|
||||||
|
|
||||||
// output control:
|
// output control:
|
||||||
|
@ -65,23 +50,12 @@ get_stabilize_roll(int32_t target_angle)
|
||||||
static int
|
static int
|
||||||
get_stabilize_pitch(int32_t target_angle)
|
get_stabilize_pitch(int32_t target_angle)
|
||||||
{
|
{
|
||||||
int32_t error;
|
int32_t error = 0;
|
||||||
int32_t rate;
|
int32_t rate = 0;
|
||||||
int32_t current_rate;
|
static float current_rate = 0;
|
||||||
|
|
||||||
int16_t rate_d1 = 0;
|
//current_rate = (omega.y * DEGX100);
|
||||||
static int16_t rate_d2 = 0;
|
current_rate = (current_rate *.7) + (omega.y * DEGX100) * .3;
|
||||||
static int16_t rate_d3 = 0;
|
|
||||||
static int32_t last_rate = 0;
|
|
||||||
|
|
||||||
current_rate = (omega.y * DEGX100);
|
|
||||||
|
|
||||||
// playing with double derivatives.
|
|
||||||
// History of last 3 dir
|
|
||||||
rate_d3 = rate_d2;
|
|
||||||
rate_d2 = rate_d1;
|
|
||||||
rate_d1 = current_rate - last_rate;
|
|
||||||
last_rate = current_rate;
|
|
||||||
|
|
||||||
// angle error
|
// angle error
|
||||||
error = wrap_180(target_angle - dcm.pitch_sensor);
|
error = wrap_180(target_angle - dcm.pitch_sensor);
|
||||||
|
@ -108,11 +82,12 @@ get_stabilize_pitch(int32_t target_angle)
|
||||||
|
|
||||||
// rate control
|
// rate control
|
||||||
error = rate - (omega.y * DEGX100);
|
error = rate - (omega.y * DEGX100);
|
||||||
|
|
||||||
|
//error = rate - (omega.y * DEGX100);
|
||||||
rate = g.pi_rate_pitch.get_pi(error, G_Dt);
|
rate = g.pi_rate_pitch.get_pi(error, G_Dt);
|
||||||
|
|
||||||
// D term testing
|
// D term
|
||||||
int16_t d_temp = (rate_d1 + rate_d2 + rate_d3) * g.stablize_d;
|
int16_t d_temp = current_rate * g.stablize_d;
|
||||||
|
|
||||||
rate -= d_temp;
|
rate -= d_temp;
|
||||||
|
|
||||||
// output control:
|
// output control:
|
||||||
|
@ -177,7 +152,7 @@ get_nav_throttle(int32_t z_error)
|
||||||
// convert to desired Rate:
|
// convert to desired Rate:
|
||||||
rate_error = g.pi_alt_hold.get_p(z_error); //_p = .85
|
rate_error = g.pi_alt_hold.get_p(z_error); //_p = .85
|
||||||
|
|
||||||
// experiment to pipe iterm directly into the output
|
// compensates throttle setpoint error for hovering
|
||||||
int16_t iterm = g.pi_alt_hold.get_i(z_error, .1);
|
int16_t iterm = g.pi_alt_hold.get_i(z_error, .1);
|
||||||
|
|
||||||
// calculate rate error
|
// calculate rate error
|
||||||
|
@ -230,7 +205,7 @@ get_rate_yaw(int32_t target_rate)
|
||||||
}
|
}
|
||||||
|
|
||||||
// Keeps old data out of our calculation / logs
|
// Keeps old data out of our calculation / logs
|
||||||
static void reset_nav(void)
|
static void reset_nav_params(void)
|
||||||
{
|
{
|
||||||
// forces us to update nav throttle
|
// forces us to update nav throttle
|
||||||
invalid_throttle = true;
|
invalid_throttle = true;
|
||||||
|
@ -256,21 +231,66 @@ static void reset_nav(void)
|
||||||
next_WP.alt = 0;
|
next_WP.alt = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
reset all I integrators
|
||||||
|
*/
|
||||||
|
static void reset_I_all(void)
|
||||||
|
{
|
||||||
|
reset_rate_I();
|
||||||
|
reset_stability_I();
|
||||||
|
reset_nav_I();
|
||||||
|
reset_wind_I();
|
||||||
|
reset_throttle_I();
|
||||||
|
reset_optflow_I();
|
||||||
|
|
||||||
|
// This is the only place we reset Yaw
|
||||||
|
g.pi_stabilize_yaw.reset_I();
|
||||||
|
}
|
||||||
|
|
||||||
static void reset_rate_I()
|
static void reset_rate_I()
|
||||||
{
|
{
|
||||||
// balances the quad
|
|
||||||
g.pi_stabilize_roll.reset_I();
|
|
||||||
g.pi_stabilize_pitch.reset_I();
|
|
||||||
|
|
||||||
// compensates rate error
|
|
||||||
g.pi_rate_roll.reset_I();
|
g.pi_rate_roll.reset_I();
|
||||||
g.pi_rate_pitch.reset_I();
|
g.pi_rate_pitch.reset_I();
|
||||||
g.pi_acro_roll.reset_I();
|
g.pi_acro_roll.reset_I();
|
||||||
g.pi_acro_pitch.reset_I();
|
g.pi_acro_pitch.reset_I();
|
||||||
|
g.pi_rate_yaw.reset_I();
|
||||||
|
}
|
||||||
|
|
||||||
|
static void reset_optflow_I(void)
|
||||||
|
{
|
||||||
g.pi_optflow_roll.reset_I();
|
g.pi_optflow_roll.reset_I();
|
||||||
g.pi_optflow_pitch.reset_I();
|
g.pi_optflow_pitch.reset_I();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
static void reset_wind_I(void)
|
||||||
|
{
|
||||||
|
// Wind Compensation
|
||||||
|
g.pi_loiter_lat.reset_I();
|
||||||
|
g.pi_loiter_lon.reset_I();
|
||||||
|
}
|
||||||
|
|
||||||
|
static void reset_nav_I(void)
|
||||||
|
{
|
||||||
|
// Rate control for WP navigation
|
||||||
|
g.pi_nav_lat.reset_I();
|
||||||
|
g.pi_nav_lon.reset_I();
|
||||||
|
}
|
||||||
|
|
||||||
|
static void reset_throttle_I(void)
|
||||||
|
{
|
||||||
|
// For Altitude Hold
|
||||||
|
g.pi_alt_hold.reset_I();
|
||||||
|
g.pi_throttle.reset_I();
|
||||||
|
}
|
||||||
|
|
||||||
|
static void reset_stability_I(void)
|
||||||
|
{
|
||||||
|
// Used to balance a quad
|
||||||
|
// This only needs to be reset during Auto-leveling in flight
|
||||||
|
g.pi_stabilize_roll.reset_I();
|
||||||
|
g.pi_stabilize_pitch.reset_I();
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
/*************************************************************
|
/*************************************************************
|
||||||
throttle control
|
throttle control
|
||||||
|
|
Loading…
Reference in New Issue