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

Stability patch updates, 

Cleanup
This commit is contained in:
Jason Short 2011-12-06 21:03:56 -08:00
parent 05409b8c1c
commit 6dfdd754ea
5 changed files with 41 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

71
ArduCopter/Attitude.pde
View File

@ -13,18 +13,15 @@ get_stabilize_roll(int32_t target_angle)
error = constrain(error, -2500, 2500);
// conver to desired Rate:
rate = g.pi_stabilize_roll.get_pi(error, G_Dt);
rate = g.pi_stabilize_roll.get_p(error);
// experiment to pipe iterm directly into the output
int16_t iterm = g.pi_stabilize_roll.get_integrator();
int16_t iterm = g.pi_stabilize_roll.get_i(error, G_Dt);
// remove iterm from PI output
rate -= iterm;
#if FRAME_CONFIG != HELI_FRAME // cannot use rate control for helicopters
#if FRAME_CONFIG != HELI_FRAME // cannot use rate control for helicopters
error = rate - (omega.x * DEGX100);
rate = g.pi_rate_roll.get_pi(error, G_Dt);
#endif
#endif
// output control:
rate = constrain(rate, -2500, 2500);
@ -43,19 +40,16 @@ get_stabilize_pitch(int32_t target_angle)
// limit the error we're feeding to the PID
error = constrain(error, -2500, 2500);
// conver to desired Rate:
rate = g.pi_stabilize_pitch.get_pi(error, G_Dt);
// convert to desired Rate:
rate = g.pi_stabilize_pitch.get_p(error);
// experiment to pipe iterm directly into the output
int16_t iterm = g.pi_stabilize_roll.get_integrator();
int16_t iterm = g.pi_stabilize_roll.get_i(error, G_Dt);
// remove iterm from PI output
rate -= iterm;
#if FRAME_CONFIG != HELI_FRAME // cannot use rate control for helicopters
#if FRAME_CONFIG != HELI_FRAME // cannot use rate control for helicopters
error = rate - (omega.y * DEGX100);
rate = g.pi_rate_pitch.get_pi(error, G_Dt);
#endif
#endif
// output control:
rate = constrain(rate, -2500, 2500);
@ -70,32 +64,34 @@ get_stabilize_yaw(int32_t target_angle)
int32_t error;
int32_t rate;
yaw_error = wrap_180(target_angle - dcm.yaw_sensor);
// angle error
error = wrap_180(target_angle - dcm.yaw_sensor);
// limit the error we're feeding to the PID
yaw_error = constrain(yaw_error, -YAW_ERROR_MAX, YAW_ERROR_MAX);
rate = g.pi_stabilize_yaw.get_pi(yaw_error, G_Dt);
//Serial.printf("%u\t%d\t%d\t", (int)target_angle, (int)error, (int)rate);
error = constrain(error, -YAW_ERROR_MAX, YAW_ERROR_MAX);
// convert to desired Rate:
rate = g.pi_stabilize_yaw.get_p(error);
// experiment to pipe iterm directly into the output
int16_t iterm = g.pi_stabilize_yaw.get_i(error, G_Dt);
#if FRAME_CONFIG == HELI_FRAME // cannot use rate control for helicopters
if( ! g.heli_ext_gyro_enabled ) {
// Rate P:
error = rate - (degrees(omega.z) * 100.0);
rate = g.pi_rate_yaw.get_pi(error, G_Dt);
if(!g.heli_ext_gyro_enabled ) {
error = rate - (degrees(omega.z) * 100.0);
rate = g.pi_rate_yaw.get_pi(error, G_Dt);
}
// output control:
return (int)constrain(rate, -4500, 4500);
rate = constrain(rate, -4500, 4500);
return (int)rate + iterm;
#else
// Rate P:
error = rate - (degrees(omega.z) * 100.0);
rate = g.pi_rate_yaw.get_pi(error, G_Dt);
//Serial.printf("%d\t%d\n", (int)error, (int)rate);
error = rate - (omega.z * DEGX100);;
rate = g.pi_rate_yaw.get_pi(error, G_Dt);
// output control:
return (int)constrain(rate, -2500, 2500);
rate = constrain(rate, -2500, 2500);
return (int)rate + iterm;
#endif
}
#define ALT_ERROR_MAX 300
@ -104,19 +100,16 @@ get_nav_throttle(int32_t z_error)
{
int16_t rate_error;
bool calc_i = (abs(z_error) < ALT_ERROR_MAX);
float dt = (abs(z_error) < 200) ? .1 : 0.0;
// limit error to prevent I term run up
z_error = constrain(z_error, -ALT_ERROR_MAX, ALT_ERROR_MAX);
// desired rate
rate_error = g.pi_alt_hold.get_pi(z_error, .1, calc_i); //_p = .85
// convert to desired Rate:
rate_error = g.pi_alt_hold.get_p(z_error); //_p = .85
// get I-term controller - will fix up later
int16_t iterm = g.pi_alt_hold.get_integrator();
// remove iterm from PI output
rate_error -= iterm;
// experiment to pipe iterm directly into the output
int16_t iterm = g.pi_alt_hold.get_i(z_error, dt);
// calculate rate error
rate_error = rate_error - climb_rate;

2
ArduCopter/commands_logic.pde
View File

@ -470,8 +470,8 @@ static bool verify_RTL()
{
// loiter at the WP
wp_control = WP_MODE;
// Did we pass the WP? // Distance checking
// Did we pass the WP? // Distance checking
if((wp_distance <= g.waypoint_radius) || check_missed_wp()){
wp_control = LOITER_MODE;

8
ArduCopter/navigation.pde
View File

@ -61,8 +61,10 @@ static void calc_loiter(int x_error, int y_error)
x_error = constrain(x_error, -NAV_ERR_MAX, NAV_ERR_MAX);
y_error = constrain(y_error, -NAV_ERR_MAX, NAV_ERR_MAX);
int x_target_speed = g.pi_loiter_lon.get_pi(x_error, dTnav);
int y_target_speed = g.pi_loiter_lat.get_pi(y_error, dTnav);
int x_target_speed = g.pi_loiter_lon.get_p(x_error);
int y_target_speed = g.pi_loiter_lat.get_p(y_error);
int x_iterm = g.pi_loiter_lon.get_i(x_error, dTnav);
int y_iterm = g.pi_loiter_lon.get_i(y_error, dTnav);
// find the rates:
float temp = g_gps->ground_course * RADX100;
@ -85,11 +87,13 @@ static void calc_loiter(int x_error, int y_error)
y_rate_error = constrain(y_rate_error, -250, 250); // added a rate error limit to keep pitching down to a minimum
nav_lat = g.pi_nav_lat.get_pi(y_rate_error, dTnav);
nav_lat = constrain(nav_lat, -3500, 3500);
nav_lat += y_iterm;
x_rate_error = x_target_speed - x_actual_speed;
x_rate_error = constrain(x_rate_error, -250, 250);
nav_lon = g.pi_nav_lon.get_pi(x_rate_error, dTnav);
nav_lon = constrain(nav_lon, -3500, 3500);
nav_lon += x_iterm;
}
static void calc_loiter2(int x_error, int y_error)

2
ArduCopter/planner.pde
View File

@ -35,7 +35,7 @@ planner_gcs(uint8_t argc, const Menu::arg *argv)
fast_loopTimer = millis();
gcs_update();
read_radio();
gcs_data_stream_send(45, 1000);

2
ArduCopter/test.pde
View File

@ -923,7 +923,7 @@ test_mag(uint8_t argc, const Menu::arg *argv)
}
/*
static int8_t
//static int8_t
//test_reverse(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();