Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 08:38:36 -04:00
Code Issues Packages Projects Releases Wiki Activity

2.0.39

Browse Source 
Change the way Rate nav works. we now take into account both forward *and lateral* rates. The signs may be backward so be careful! Let me know if the quad shoots off to one side and I'll flip the signs.

git-svn-id: https://arducopter.googlecode.com/svn/trunk@3049 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-08-08 16:54:56 +00:00
parent d02d4b895c
commit b7ce6e036e
3 changed files with 44 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
ArduCopterMega/APM_Config.h
View File

@ -9,7 +9,7 @@
//#define BROKEN_SLIDER 0 // 1 = yes (use Yaw to enter CLI mode)
//#define FRAME_CONFIG QUAD_FRAME
#define FRAME_CONFIG QUAD_FRAME
/*
options:
QUAD_FRAME
@ -20,7 +20,7 @@
HELI_FRAME
*/
//#define FRAME_ORIENTATION X_FRAME
#define FRAME_ORIENTATION X_FRAME
/*
PLUS_FRAME
X_FRAME
@ -28,7 +28,7 @@
*/
//#define CHANNEL_6_TUNING CH6_NONE
#define CHANNEL_6_TUNING CH6_NONE
/*
CH6_NONE
CH6_STABILIZE_KP
@ -43,7 +43,7 @@
CH6_YAW_RATE_KI
CH6_TOP_BOTTOM_RATIO
CH6_PMAX
CH6_RELAY
CH6_RELAY
*/
// experimental!!

29
ArduCopterMega/ArduCopterMega.pde
View File

@ -1336,24 +1336,36 @@ static void update_navigation()
case GUIDED:
case RTL:
if(wp_distance > 20){
if(wp_distance > 5){
// calculates desired Yaw
// XXX this is an experiment
#if FRAME_CONFIG == HELI_FRAME
update_nav_yaw();
#endif
}else{
// Don't Yaw anymore
// hack to elmininate crosstrack effect
crosstrack_bearing = target_bearing;
}
//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;
//wp_control = (wp_distance < 4 ) ? LOITER_MODE : WP_MODE;
// calculates the desired Roll and Pitch
update_nav_wp();
//update_nav_wp();
// 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();
break;
// switch passthrough to LOITER
@ -1378,11 +1390,10 @@ static void update_navigation()
update_loiter();
// calc a rate dampened pitch to the target
calc_rate_nav(200);
calc_rate_nav(400);
// rotate that pitch to the copter frame of reference
calc_nav_output();
break;
}

29
ArduCopterMega/navigation.pde
View File

@ -133,20 +133,25 @@ static void calc_nav_output()
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_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
static void calc_rate_nav(int speed)
{
// which direction are we moving?
long target_error = nav_bearing - g_gps->ground_course;
target_error = wrap_180(target_error);
long heading_error = nav_bearing - g_gps->ground_course;
heading_error = wrap_180(heading_error);
// calc the cos of the error to tell how fast we are moving towards the target in cm
int groundspeed = (float)g_gps->ground_speed * cos(radians((float)target_error/100));
groundspeed = max(groundspeed, 0);
int targetspeed = (float)g_gps->ground_speed * cos(radians((float)heading_error/100));
// calc the sin of the error to tell how fast we are moving laterally to the target in cm
int lateralspeed = (float)g_gps->ground_speed * sin(radians((float)heading_error/100));
//targetspeed = max(targetspeed, 0);
// Reduce speed on RTL
if(control_mode == RTL){
@ -160,12 +165,15 @@ static void calc_rate_nav(int speed)
//waypoint_speed = g.waypoint_speed_max.get();
}
int error = constrain(waypoint_speed - groundspeed, -1000, 1000);
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
//Serial.printf("dTnav: %ld, gs: %d, err: %d, int: %d, pitch: %ld", dTnav, groundspeed, error, (int)g.pid_nav_wp.get_integrator(), (long)nav_lat);
nav_lon += lateralspeed * 2; // 2 is our fake PID gain
nav_lon >>= 1;
//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
@ -239,7 +247,10 @@ static void update_crosstrack(void)
// Crosstrack Error
// ----------------
if (cross_track_test() < 9000) { // If we are too far off or too close we don't do track following
crosstrack_error = sin(radians((target_bearing - crosstrack_bearing) / (float)100)) * (float)wp_distance; // Meters we are off track line
// Meters we are off track line
crosstrack_error = sin(radians((target_bearing - crosstrack_bearing) / (float)100)) * (float)wp_distance;
// take meters * 100 to get adjustment to nav_bearing
long xtrack = g.pid_crosstrack.get_pid(crosstrack_error, dTnav, 1.0) * 100;
nav_bearing += constrain(xtrack, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get());
nav_bearing = wrap_360(nav_bearing);