Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-27 02:58:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' of https://code.google.com/p/ardupilot-mega

Browse Source
This commit is contained in:
Chris Anderson 2012-01-21 19:36:50 -08:00
commit ad990c7e35
3 changed files with 12 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ArduCopter/ArduCopter.pde
View File

@ -2007,7 +2007,7 @@ static void update_nav_wp()
{ {
if(wp_control == LOITER_MODE){ if(wp_control == LOITER_MODE){
// calc a pitch to the target // calc error to target
calc_location_error(&next_WP); calc_location_error(&next_WP);
// use error as the desired rate towards the target // 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); //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){ }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); int16_t speed = calc_desired_speed(g.waypoint_speed_max);
// use error as the desired rate towards the target // use error as the desired rate towards the target
calc_nav_rate(speed); calc_nav_rate(speed);

6
ArduCopter/Attitude.pde
View File

@ -321,9 +321,9 @@ static int get_angle_boost(int value)
{ {
float temp = cos_pitch_x * cos_roll_x; float temp = cos_pitch_x * cos_roll_x;
temp = 1.0 - constrain(temp, .5, 1.0); temp = 1.0 - constrain(temp, .5, 1.0);
// int16_t output = temp * value; int16_t output = temp * value;
// return constrain(output, 0, 100); return constrain(output, 0, 100);
return (int)(temp * value); // return (int)(temp * value);
} }
#define NUM_G_SAMPLES 40 #define NUM_G_SAMPLES 40

11
ArduCopter/navigation.pde
View File

@ -261,7 +261,7 @@ static void calc_loiter_pitch_roll()
static int16_t calc_desired_speed(int16_t max_speed) static int16_t calc_desired_speed(int16_t max_speed)
{ {
/* /*
|< WP Radius |< WP Radius
0 1 2 3 4 5 6 7 8m 0 1 2 3 4 5 6 7 8m
...|...|...|...|...|...|...|...| ...|...|...|...|...|...|...|...|
100 | 200 300 400cm/s 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 // 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 // for example 4m from target = 200cm/s speed
// we choose the lowest speed based on disance // 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 // limit the ramp up of the speed
// waypoint_speed_gov is reset to 0 at each new WP command // waypoint_speed_gov is reset to 0 at each new WP command
if(waypoint_speed_gov < max_speed){ 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 // go at least 100cm/s
max_speed = max(50, waypoint_speed_gov); max_speed = max(100, waypoint_speed_gov);
// limit with governer // limit with governer
max_speed = min(max_speed, waypoint_speed_gov); 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 = nav_lon_p + x_iterm;
nav_lon = constrain(nav_lon, -3000, 3000); nav_lon = constrain(nav_lon, -3000, 3000);
y_rate_error = (sin(temp) * max_speed) - y_actual_speed; // 413 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 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); int16_t y_iterm = g.pi_loiter_lat.get_i(y_rate_error, dTnav);