Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-04 06:58:39 -04:00
Code Issues 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 fb5609d3eb
commit 41bdcfee48
1 changed files with 52 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
ArduCopterMega/navigation.pde
View File

@ -12,15 +12,11 @@ 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
// --------------------------------------------------------------------------
if(GPS.new_data){
GPS.new_data = false;
// waypoint distance from plane // waypoint distance from plane
// ---------------------------- // ----------------------------
GPS_wp_distance = getDistance(&current_loc, &next_WP); GPS_wp_distance = getDistance(&current_loc, &next_WP);
@ -40,14 +36,40 @@ void navigate()
// ------------------------------------------- // -------------------------------------------
nav_bearing = target_bearing; nav_bearing = target_bearing;
// look for a big angle change
// ---------------------------
//verify_missed_wp();
// control mode specific updates to nav_bearing // control mode specific updates to nav_bearing
// -------------------------------------------- // --------------------------------------------
update_navigation(); 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;
} }
/* /*