From e3700365d4145028278cd3fb2dab52e1e84a949d Mon Sep 17 00:00:00 2001 From: Jason Short Date: Tue, 6 Dec 2011 21:03:56 -0800 Subject: [PATCH] Stability patch updates, Cleanup --- ArduCopter/Attitude.pde | 71 ++++++++++++++++------------------- ArduCopter/commands_logic.pde | 2 +- ArduCopter/navigation.pde | 8 +++- ArduCopter/planner.pde | 2 +- ArduCopter/test.pde | 2 +- 5 files changed, 41 insertions(+), 44 deletions(-) diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index a4dda88507..944786194f 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -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; diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index de3fb66354..44d40dd3e9 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -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; diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 01718439e2..243f25c8eb 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -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) diff --git a/ArduCopter/planner.pde b/ArduCopter/planner.pde index c9acf210fc..72c780eeaf 100644 --- a/ArduCopter/planner.pde +++ b/ArduCopter/planner.pde @@ -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); diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde index d0dc857c35..7555cb8d28 100644 --- a/ArduCopter/test.pde +++ b/ArduCopter/test.pde @@ -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();