aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

added new PID nav functions to split lat and long based on Randy's work.

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1488 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-01-11 21:15:08 +00:00
parent 3ed6e0fc87
commit a80b4fbb69
1 changed files with 52 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

82
ArduCopterMega/navigation.pde
View File

@ -12,42 +12,64 @@ void navigate()
GPS.new_data = false; GPS.new_data = false;
return; return;
} }
if(next_WP.lat == 0){ if(next_WP.lat == 0){
return; return;
} }
// We only perform most nav computations if we have new gps data to work with // waypoint distance from plane
// -------------------------------------------------------------------------- // ----------------------------
if(GPS.new_data){ GPS_wp_distance = getDistance(&current_loc, &next_WP);
GPS.new_data = false;
// waypoint distance from plane
// ----------------------------
GPS_wp_distance = getDistance(&current_loc, &next_WP);
if (GPS_wp_distance < 0){ if (GPS_wp_distance < 0){
send_message(SEVERITY_HIGH,"<navigate> WP error - distance < 0"); send_message(SEVERITY_HIGH,"<navigate> WP error - distance < 0");
//Serial.println(wp_distance,DEC); //Serial.println(wp_distance,DEC);
//print_current_waypoints(); //print_current_waypoints();
return; return;
}
// target_bearing is where we should be heading
// --------------------------------------------
target_bearing = get_bearing(&current_loc, &next_WP);
// nav_bearing will includes xtrack correction
// -------------------------------------------
nav_bearing = target_bearing;
// look for a big angle change
// ---------------------------
//verify_missed_wp();
// control mode specific updates to nav_bearing
// --------------------------------------------
update_navigation();
} }
// target_bearing is where we should be heading
// --------------------------------------------
target_bearing = get_bearing(&current_loc, &next_WP);
// nav_bearing will includes xtrack correction
// -------------------------------------------
nav_bearing = target_bearing;
// control mode specific updates to nav_bearing
// --------------------------------------------
update_navigation();
// calc pitch and roll to target
// -----------------------------
calc_nav();
}
void calc_nav()
{
/*
Becuase we are using lat and lon to do our distance errors here's a quick chart:
100 = 1m
1000 = 11m
10000 = 111m
pitch_max = 22° (2200)
*/
float cos_yaw = cos(dcm.yaw);
float sin_yaw = sin(dcm.yaw);
// ROLL
nav_lon = pid_nav_lon.get_pid((long)((float)(next_WP.lng - GPS.longitude) * scaleLongDown), dTnav, 1.0);
nav_lon = constrain(nav_lon, -pitch_max, pitch_max); // Limit max command
// PITCH
nav_lat = pid_nav_lat.get_pid(next_WP.lat - GPS.latitude, dTnav, 1.0);
nav_lat = constrain(nav_lat, -pitch_max, pitch_max); // Limit max command
// rotate the vector
nav_roll = (float)nav_lon * cos_yaw - (float)nav_lat * sin_yaw;
nav_pitch = (float)nav_lon * sin_yaw + (float)nav_lat * cos_yaw;
} }
/* /*