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

ac quick redo fix

This commit is contained in:
Michael Oborne 2011-10-16 06:58:03 +08:00
parent c3b93d70df
commit a84d0cb406
1 changed files with 9 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
ArduCopter/navigation.pde
View File

@ -95,9 +95,16 @@ static void calc_loiter(int x_error, int y_error)
// nav_roll, nav_pitch
static void calc_loiter_pitch_roll()
{
float temp = radians((float)(9000 - (dcm.yaw_sensor))/100.0);
float _cos_yaw_x = cos(temp);
float _sin_yaw_y = sin(temp);
// Serial.printf("ys %ld, cyx %1.4f, _cyx %1.4f\n", dcm.yaw_sensor, cos_yaw_x, _cos_yaw_x);
// rotate the vector
nav_roll = (float)nav_lon * sin_yaw_y - (float)nav_lat * cos_yaw_x;
nav_pitch = (float)nav_lon * cos_yaw_x + (float)nav_lat * sin_yaw_y;
nav_roll = (float)nav_lon * _sin_yaw_y - (float)nav_lat * _cos_yaw_x;
nav_pitch = (float)nav_lon * _cos_yaw_x + (float)nav_lat * _sin_yaw_y;
// flip pitch because forward is negative
nav_pitch = -nav_pitch;