Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

fixed inverted cos error 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1597 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-02-06 07:02:51 +00:00
parent 76f0d1739b
commit ea8acd664f
1 changed files with 24 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
ArduCopterMega/navigation.pde
View File

@ -41,9 +41,12 @@ void navigate()
update_navigation();
}
#define DIST_ERROR_MAX 3000
#define DIST_ERROR_MAX 1800
void calc_nav()
{
Vector2f yawvector;
long long_error, lat_error;
/*
Becuase we are using lat and lon to do our distance errors here's a quick chart:
100 = 1m
@ -53,23 +56,31 @@ void calc_nav()
pitch_max = 22° (2200)
*/
Vector2f yawvector;
Matrix3f temp = dcm.get_dcm_matrix();
yawvector.x = temp.a.x;
yawvector.y = temp.b.x;
yawvector.x = temp.a.x; // sin
yawvector.y = temp.b.x; // cos
yawvector.normalize();
yawvector.y = -yawvector.y;
//cos =
long_error = (float)(next_WP.lng - current_loc.lng) * scaleLongDown;
lat_error = next_WP.lat - current_loc.lat;
long_error = constrain(long_error, -DIST_ERROR_MAX, DIST_ERROR_MAX); // +- 20m max error
lat_error = constrain(lat_error, -DIST_ERROR_MAX, DIST_ERROR_MAX); // +- 20m max error
// ROLL
nav_lon = pid_nav_lon.get_pid((long)((float)(next_WP.lng - GPS.longitude) * scaleLongDown), dTnav, 1.0);
nav_lon = constrain(nav_lon, -DIST_ERROR_MAX, DIST_ERROR_MAX); // Limit max command
nav_lon = long_error * pid_nav_lon.kP();
//nav_lon = pid_nav_lon.get_pid(long_error, dTnav2, 1.0);
//nav_lon = constrain(nav_lon, -DIST_ERROR_MAX, DIST_ERROR_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, -DIST_ERROR_MAX, DIST_ERROR_MAX); // Limit max command
//nav_lat = pid_nav_lat.get_pid(lat_error, dTnav2, 1.0);
nav_lat = lat_error * pid_nav_lat.kP();
//nav_lat = constrain(nav_lat, -DIST_ERROR_MAX, DIST_ERROR_MAX); // Limit max command
// rotate the vector
nav_roll = (float)nav_lon * yawvector.x + (float)nav_lat * yawvector.y;
nav_roll = (float)nav_lon * yawvector.x - (float)nav_lat * yawvector.y;
nav_pitch = (float)nav_lat * yawvector.x + (float)nav_lon * yawvector.y;
//Serial.printf("vx %4.4f,vy %4.4f ", yawvector.x, yawvector.y);