From ab5716c42d8765bd0fb9ca74aaefe27123929370 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 21 Jan 2012 11:57:21 -0800 Subject: [PATCH 1/3] Calc error for logs --- ArduCopter/ArduCopter.pde | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index aaa8f9b42a..e17b9e6481 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -2007,7 +2007,7 @@ static void update_nav_wp() { if(wp_control == LOITER_MODE){ - // calc a pitch to the target + // calc error to target calc_location_error(&next_WP); // use error as the desired rate towards the target @@ -2064,6 +2064,9 @@ static void update_nav_wp() //Serial.printf("CIRCLE: angle:%d, dist:%d, X:%d, Y:%d, P:%d, R:%d \n", angleTest, (int)wp_distance , (int)long_error, (int)lat_error, npitch, nroll); }else if(wp_control == WP_MODE){ + // calc error to target + calc_location_error(&next_WP); + int16_t speed = calc_desired_speed(g.waypoint_speed_max); // use error as the desired rate towards the target calc_nav_rate(speed); From 1aa6d0ea083b489347e9961cb9d61756edb7d7cf Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 21 Jan 2012 11:57:42 -0800 Subject: [PATCH 2/3] limiting the pitch throttle compensation --- ArduCopter/Attitude.pde | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index d6efca9a10..e32ea0cd9d 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -321,9 +321,9 @@ static int get_angle_boost(int value) { float temp = cos_pitch_x * cos_roll_x; temp = 1.0 - constrain(temp, .5, 1.0); -// int16_t output = temp * value; -// return constrain(output, 0, 100); - return (int)(temp * value); + int16_t output = temp * value; + return constrain(output, 0, 100); +// return (int)(temp * value); } #define NUM_G_SAMPLES 40 From 8cb645f3c2a92cd51370452d1cbb4fbee88ed5cc Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 21 Jan 2012 11:58:32 -0800 Subject: [PATCH 3/3] increasing the velocity near WP --- ArduCopter/navigation.pde | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 32140818aa..c6ca18db00 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -261,7 +261,7 @@ static void calc_loiter_pitch_roll() static int16_t calc_desired_speed(int16_t max_speed) { /* - |< WP Radius + |< WP Radius 0 1 2 3 4 5 6 7 8m ...|...|...|...|...|...|...|...| 100 | 200 300 400cm/s @@ -274,15 +274,15 @@ static int16_t calc_desired_speed(int16_t max_speed) // wp_distance is always in m/s and not cm/s - I know it's stupid that way // for example 4m from target = 200cm/s speed // we choose the lowest speed based on disance - max_speed = min(max_speed, (wp_distance * 50)); + max_speed = min(max_speed, (wp_distance * 100)); // limit the ramp up of the speed // waypoint_speed_gov is reset to 0 at each new WP command if(waypoint_speed_gov < max_speed){ - waypoint_speed_gov += (int)(50.0 * dTnav); // increase at .5/ms + waypoint_speed_gov += (int)(100.0 * dTnav); // increase at .5/ms - // go at least 50cm/s - max_speed = max(50, waypoint_speed_gov); + // go at least 100cm/s + max_speed = max(100, waypoint_speed_gov); // limit with governer max_speed = min(max_speed, waypoint_speed_gov); } @@ -305,7 +305,6 @@ static void calc_nav_rate(int max_speed) nav_lon = nav_lon_p + x_iterm; nav_lon = constrain(nav_lon, -3000, 3000); - y_rate_error = (sin(temp) * max_speed) - y_actual_speed; // 413 y_rate_error = constrain(y_rate_error, -1000, 1000); // added a rate error limit to keep pitching down to a minimum int16_t y_iterm = g.pi_loiter_lat.get_i(y_rate_error, dTnav);