diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 687751530e..ada3c713e8 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -7,7 +7,6 @@ Authors: Jason Short Based on code and ideas from the Arducopter team: Jose Julio, Randy Mackay, Jani Hirvinen Thanks to: Chris Anderson, Mike Smith, Jordi Munoz, Doug Weibel, James Goppert, Benjamin Pelletier - This firmware is free software; you can redistribute it and/or modify it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation; either @@ -440,32 +439,32 @@ static byte throttle_mode; static boolean takeoff_complete; // Flag for using take-off controls static boolean land_complete; static int32_t old_alt; // used for managing altitude rates -static int16_t velocity_land; +static int16_t velocity_land; static byte yaw_tracking = MAV_ROI_WPNEXT; // no tracking, point at next wp, or at a target -static int16_t manual_boost; // used in adjust altitude to make changing alt faster -static int16_t angle_boost; // used in adjust altitude to make changing alt faster +static int16_t manual_boost; // used in adjust altitude to make changing alt faster +static int16_t angle_boost; // used in adjust altitude to make changing alt faster // Loiter management // ----------------- static int32_t original_target_bearing; // deg * 100, used to check we are not passing the WP static int32_t old_target_bearing; // used to track difference in angle -static int16_t loiter_total; // deg : how many times to loiter * 360 -static int16_t loiter_sum; // deg : how far we have turned around a waypoint +static int16_t loiter_total; // deg : how many times to loiter * 360 +static int16_t loiter_sum; // deg : how far we have turned around a waypoint static uint32_t loiter_time; // millis : when we started LOITER mode static unsigned loiter_time_max; // millis : how long to stay in LOITER mode // these are the values for navigation control functions // ---------------------------------------------------- -static int32_t nav_roll; // deg * 100 : target roll angle -static int32_t nav_pitch; // deg * 100 : target pitch angle -static int32_t nav_yaw; // deg * 100 : target yaw angle -static int32_t auto_yaw; // deg * 100 : target yaw angle -static int32_t nav_lat; // for error calcs -static int32_t nav_lon; // for error calcs -static int16_t nav_throttle; // 0-1000 for throttle control -static int16_t crosstrack_error; +static int32_t nav_roll; // deg * 100 : target roll angle +static int32_t nav_pitch; // deg * 100 : target pitch angle +static int32_t nav_yaw; // deg * 100 : target yaw angle +static int32_t auto_yaw; // deg * 100 : target yaw angle +static int32_t nav_lat; // for error calcs +static int32_t nav_lon; // for error calcs +static int16_t nav_throttle; // 0-1000 for throttle control +static int16_t crosstrack_error; static uint32_t throttle_integrator; // used to integrate throttle output to predict battery life static bool invalid_throttle; // used to control when we calculate nav_throttle @@ -490,12 +489,12 @@ static int32_t wp_totalDistance; // meters - distance between old and next w // repeating event control // ----------------------- -static byte event_id; // what to do - see defines -static uint32_t event_timer; // when the event was asked for in ms -static uint16_t event_delay; // how long to delay the next firing of event in millis -static int16_t event_repeat; // how many times to fire : 0 = forever, 1 = do once, 2 = do twice -static int16_t event_value; // per command value, such as PWM for servos -static int16_t event_undo_value; // the value used to undo commands +static byte event_id; // what to do - see defines +static uint32_t event_timer; // when the event was asked for in ms +static uint16_t event_delay; // how long to delay the next firing of event in millis +static int16_t event_repeat; // how many times to fire : 0 = forever, 1 = do once, 2 = do twice +static int16_t event_value; // per command value, such as PWM for servos +static int16_t event_undo_value; // the value used to undo commands //static byte repeat_forever; //static byte undo_event; // counter for timing the undo @@ -1398,14 +1397,14 @@ adjust_altitude() // we remove 0 to 100 PWM from hover manual_boost = g.rc_3.control_in - 180; manual_boost = max(-120, manual_boost); - g.throttle_cruise += (g.pi_alt_hold.get_integrator() * g.pi_throttle.kP() + g.pi_throttle.get_integrator()); + g.throttle_cruise += (g.pi_alt_hold.get_integrator()); g.pi_alt_hold.reset_I(); g.pi_throttle.reset_I(); }else if (g.rc_3.control_in >= 650){ // we add 0 to 100 PWM to hover manual_boost = g.rc_3.control_in - 650; - g.throttle_cruise += (g.pi_alt_hold.get_integrator() * g.pi_throttle.kP() + g.pi_throttle.get_integrator()); + g.throttle_cruise += (g.pi_alt_hold.get_integrator()); g.pi_alt_hold.reset_I(); g.pi_throttle.reset_I(); diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 3aec0b6207..a4dda88507 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -99,17 +99,33 @@ get_stabilize_yaw(int32_t target_angle) } #define ALT_ERROR_MAX 300 -static int +static int16_t get_nav_throttle(int32_t z_error) { + int16_t rate_error; + bool calc_i = (abs(z_error) < ALT_ERROR_MAX); + // limit error to prevent I term run up z_error = constrain(z_error, -ALT_ERROR_MAX, ALT_ERROR_MAX); - int rate_error = g.pi_alt_hold.get_pi(z_error, .1, calc_i); //_p = .85 + + // desired rate + rate_error = g.pi_alt_hold.get_pi(z_error, .1, calc_i); //_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; + + // calculate rate error rate_error = rate_error - climb_rate; // limit the rate - return constrain((int)g.pi_throttle.get_pi(rate_error, .1), -120, 180); + rate_error = constrain((int)g.pi_throttle.get_pi(rate_error, .1), -120, 180); + + // output control: + return rate_error + iterm; } static int diff --git a/ArduCopter/config.h b/ArduCopter/config.h index fe2d26598a..3fe6b47297 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -628,7 +628,7 @@ // RATE control #ifndef THROTTLE_P -# define THROTTLE_P 0.5 // +# define THROTTLE_P 0.4 // #endif #ifndef THROTTLE_I # define THROTTLE_I 0.0 //