zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Added experimental rate based Loiter and revised RTL. You need to compile with the loiter option set to 1 in APM_Config to try. 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@3099 f9c3cf11-9bcb-44bc-f272-b75c42450872
master
jasonshort 14 years ago
parent
c95bb66441
commit
6b2139221d
2 changed files with 134 additions and 66 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 49
      ArduCopterMega/ArduCopterMega.pde
  2. 151
      ArduCopterMega/navigation.pde

49
ArduCopterMega/ArduCopterMega.pde
Unescape View File

@ -884,9 +884,7 @@ static void slow_loop() @@ -884,9 +884,7 @@ static void slow_loop()
if(control_mode == LOITER){
if((abs(g.rc_2.control_in) + abs(g.rc_1.control_in)) > 1500){
// reset LOITER to current position
//long temp = next_WP.alt;
next_WP = current_loc;
//next_WP.alt = temp;
//next_WP = current_loc;
}
}
#endif
@ -1342,33 +1340,27 @@ static void update_navigation() @@ -1342,33 +1340,27 @@ static void update_navigation()
#if FRAME_CONFIG == HELI_FRAME
update_nav_yaw();
#endif
}
//if(wp_distance < 4){
// clears crosstrack
crosstrack_bearing = target_bearing;
//wp_control = WP_MODE;
//}else{
//wp_control = LOITER_MODE;
//}
wp_control = WP_MODE;
// are we Traversing or Loitering?
//wp_control = (wp_distance < 4 ) ? LOITER_MODE : WP_MODE;
// calculates the desired Roll and Pitch
//update_nav_wp();
#if LOITER_TEST == 0
// calc a rate dampened pitch to the target
calc_rate_nav(g.waypoint_speed_max.get());
// rotate that pitch to the copter frame of reference
calc_nav_output();
//if(wp_distance < 4 )
// set_mode(LOITER);
#else
// rate based test
// calc error to target
calc_loiter_nav2();
// use error as a rate towards the target
calc_rate_nav2(long_error/2, lat_error/2);
// rotate pitch and roll to the copter frame of reference
calc_loiter_output();
#endif
break;
@ -1553,12 +1545,25 @@ static void update_nav_wp() @@ -1553,12 +1545,25 @@ static void update_nav_wp()
{
// XXX Guided mode!!!
if(wp_control == LOITER_MODE){
#if LOITER_TEST == 0
// calc a pitch to the target
calc_loiter_nav();
// rotate pitch and roll to the copter frame of reference
calc_loiter_output();
#else
// calc error to target
calc_loiter_nav2();
// use error as a rate towards the target
calc_rate_nav2(long_error/2, lat_error/2);
// rotate pitch and roll to the copter frame of reference
calc_loiter_output();
#endif
} else {
// how far are we from the ideal trajectory?
// this pushes us back on course

151
ArduCopterMega/navigation.pde
Unescape View File

@ -61,8 +61,10 @@ get_nav_throttle(long error) @@ -61,8 +61,10 @@ get_nav_throttle(long error)
return throttle;
}
#define DIST_ERROR_MAX 1800
static void calc_loiter_nav()
// ------------------------------
// long_error, lat_error
static void calc_loiter_nav2()
{
/*
Becuase we are using lat and lon to do our distance errors here's a quick chart:
@ -83,41 +85,65 @@ static void calc_loiter_nav() @@ -83,41 +85,65 @@ static void calc_loiter_nav()
// constrain input, not output to let I term ramp up and do it's job again wind
long_error = constrain(long_error, -loiter_error_max, loiter_error_max); // +- 20m max error
lat_error = constrain(lat_error, -loiter_error_max, loiter_error_max); // +- 20m max error
}
nav_lon = g.pid_nav_lon.get_pid(long_error, dTnav, 1.0); // X 700 * 2.5 = 1750,
nav_lat = g.pid_nav_lat.get_pid(lat_error, dTnav, 1.0); // Y invert lat (for pitch)
// sets nav_lon, nav_lat
static void calc_rate_nav2(int target_x_speed, int target_y_speed)
{
// find the rates:
// calc the cos of the error to tell how fast we are moving towards the target in cm
int y_speed = (float)g_gps->ground_speed * cos(radians((float)g_gps->ground_course/100.0));
int y_error = constrain(target_y_speed - y_speed, -1000, 1000);
// calc the sin of the error to tell how fast we are moving laterally to the target in cm
int x_speed = (float)g_gps->ground_speed * sin(radians((float)g_gps->ground_course/100.0));
int x_error = constrain(target_x_speed - x_speed, -1000, 1000);
// how fast should we be going?
nav_lat += g.pid_nav_lat.get_pid(y_error, dTnav, 1.0);
nav_lat >>= 1; // divide by two for smooting
nav_lon += g.pid_nav_lon.get_pid(x_error, dTnav, 1.0);
nav_lon >>= 1; // divide by two for smooting
//Serial.printf("dTnav: %ld, gs: %d, err: %d, int: %d, pitch: %ld", dTnav, targetspeed, error, (int)g.pid_nav_wp.get_integrator(), (long)nav_lat);
// limit our output
nav_lat = constrain(nav_lat, -3500, 3500); // +- max error
nav_lon = constrain(nav_lon, -3500, 3500); // +- max error
}
static void calc_loiter_output()
// ------------------------------
//nav_lon, nav_lat
static void calc_loiter_nav()
{
// rotate the vector
nav_roll = (float)nav_lon * sin_yaw_y - (float)nav_lat * -cos_yaw_x;
// BAD
//NORTH -1000 * 1 - 1000 * 0 = -1000 // roll left
//WEST -1000 * 0 - 1000 * -1 = 1000 // roll right - Backwards
//EAST -1000 * 0 - 1000 * 1 = -1000 // roll left - Backwards
//SOUTH -1000 * -1 - 1000 * 0 = 1000 // roll right
/*
Becuase we are using lat and lon to do our distance errors here's a quick chart:
100 = 1m
1000 = 11m = 36 feet
1800 = 19.80m = 60 feet
3000 = 33m
10000 = 111m
pitch_max = 22° (2200)
*/
// GOOD
//NORTH -1000 * 1 - 1000 * 0 = -1000 // roll left
//WEST -1000 * 0 - 1000 * 1 = -1000 // roll right
//EAST -1000 * 0 - 1000 * -1 = 1000 // roll left
//SOUTH -1000 * -1 - 1000 * 0 = 1000 // roll right
// X ROLL
long_error = (float)(next_WP.lng - current_loc.lng) * scaleLongDown; // 500 - 0 = 500 roll EAST
nav_pitch = ((float)nav_lon * -cos_yaw_x + (float)nav_lat * sin_yaw_y);
// BAD
//NORTH -1000 * 0 + 1000 * 1 = 1000 // pitch back
//WEST -1000 * -1 + 1000 * 0 = 1000 // pitch back - Backwards
//EAST -1000 * 1 + 1000 * 0 = -1000 // pitch forward - Backwards
//SOUTH -1000 * 0 + 1000 * -1 = -1000 // pitch forward
// Y PITCH
lat_error = current_loc.lat - next_WP.lat; // 0 - 500 = -500 pitch NORTH
// GOOD
//NORTH -1000 * 0 + 1000 * 1 = 1000 // pitch back
//WEST -1000 * 1 + 1000 * 0 = -1000 // pitch forward
//EAST -1000 * -1 + 1000 * 0 = 1000 // pitch back
//SOUTH -1000 * 0 + 1000 * -1 = -1000 // pitch forward
// constrain input, not output to let I term ramp up and do it's job again wind
long_error = constrain(long_error, -loiter_error_max, loiter_error_max); // +- 20m max error
lat_error = constrain(lat_error, -loiter_error_max, loiter_error_max); // +- 20m max error
nav_lon = g.pid_nav_lon.get_pid(long_error, dTnav, 1.0); // X 700 * 2.5 = 1750,
nav_lat = g.pid_nav_lat.get_pid(lat_error, dTnav, 1.0); // Y invert lat (for pitch)
}
//nav_lat
static void calc_simple_nav()
{
// no dampening here in SIMPLE mode
@ -126,20 +152,7 @@ static void calc_simple_nav() @@ -126,20 +152,7 @@ static void calc_simple_nav()
//nav_lat *= g.pid_nav_lat.kP(); // 1800 * 2 = 3600 or 36°
}
static void calc_nav_output()
{
// get the sin and cos of the bearing error - rotated 90°
float sin_nav_y = sin(radians((float)(9000 - bearing_error) / 100));
float cos_nav_x = cos(radians((float)(bearing_error - 9000) / 100));
// rotate the vector
//nav_roll = (float)nav_lat * cos_nav_x;
//nav_pitch = -(float)nav_lat * sin_nav_y;
nav_roll = (float)nav_lon * sin_nav_y - (float)nav_lat * -cos_nav_x;
nav_pitch = (float)nav_lon * cos_nav_x - (float)nav_lat * sin_nav_y;
}
// called after we get GPS read
// sets nav_lon, nav_lat
static void calc_rate_nav(int speed)
{
// which direction are we moving?
@ -166,12 +179,12 @@ static void calc_rate_nav(int speed) @@ -166,12 +179,12 @@ static void calc_rate_nav(int speed)
}
int error = constrain(waypoint_speed - targetspeed, -1000, 1000);
// Scale response by kP
nav_lat += g.pid_nav_wp.get_pid(error, dTnav, 1.0);
nav_lat >>= 1; // divide by two for smooting
nav_lon += lateralspeed * 2; // 2 is our fake PID gain
nav_lon >>= 1;
nav_lon >>= 1; // divide by two for smooting
//Serial.printf("dTnav: %ld, gs: %d, err: %d, int: %d, pitch: %ld", dTnav, targetspeed, error, (int)g.pid_nav_wp.get_integrator(), (long)nav_lat);
@ -179,6 +192,56 @@ static void calc_rate_nav(int speed) @@ -179,6 +192,56 @@ static void calc_rate_nav(int speed)
nav_lat = constrain(nav_lat, -3500, 3500); // +- max error
}
// output pitch and roll
// ------------------------------
// nav_roll, nav_pitch
static void calc_loiter_output()
{
// rotate the vector
nav_roll = (float)nav_lon * sin_yaw_y - (float)nav_lat * -cos_yaw_x;
// BAD
//NORTH -1000 * 1 - 1000 * 0 = -1000 // roll left
//WEST -1000 * 0 - 1000 * -1 = 1000 // roll right - Backwards
//EAST -1000 * 0 - 1000 * 1 = -1000 // roll left - Backwards
//SOUTH -1000 * -1 - 1000 * 0 = 1000 // roll right
// GOOD
//NORTH -1000 * 1 - 1000 * 0 = -1000 // roll left
//WEST -1000 * 0 - 1000 * 1 = -1000 // roll right
//EAST -1000 * 0 - 1000 * -1 = 1000 // roll left
//SOUTH -1000 * -1 - 1000 * 0 = 1000 // roll right
nav_pitch = ((float)nav_lon * -cos_yaw_x + (float)nav_lat * sin_yaw_y);
// BAD
//NORTH -1000 * 0 + 1000 * 1 = 1000 // pitch back
//WEST -1000 * -1 + 1000 * 0 = 1000 // pitch back - Backwards
//EAST -1000 * 1 + 1000 * 0 = -1000 // pitch forward - Backwards
//SOUTH -1000 * 0 + 1000 * -1 = -1000 // pitch forward
// GOOD
//NORTH -1000 * 0 + 1000 * 1 = 1000 // pitch back
//WEST -1000 * 1 + 1000 * 0 = -1000 // pitch forward
//EAST -1000 * -1 + 1000 * 0 = 1000 // pitch back
//SOUTH -1000 * 0 + 1000 * -1 = -1000 // pitch forward
}
// nav_roll, nav_pitch
static void calc_nav_output()
{
// get the sin and cos of the bearing error - rotated 90°
float sin_nav_y = sin(radians((float)(9000 - bearing_error) / 100));
float cos_nav_x = cos(radians((float)(bearing_error - 9000) / 100));
// rotate the vector
//nav_roll = (float)nav_lat * cos_nav_x;
//nav_pitch = -(float)nav_lat * sin_nav_y;
nav_roll = (float)nav_lon * sin_nav_y - (float)nav_lat * -cos_nav_x;
nav_pitch = (float)nav_lon * cos_nav_x - (float)nav_lat * sin_nav_y;
}
// ------------------------------
static void calc_bearing_error()
{
// 83 99 Yaw = -16

Write Preview
Loading…
Cancel
Save