mirror of https://github.com/ArduPilot/ardupilot
improved rotation vectors
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1618 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
parent
61d65f652c
commit
f03df257f3
|
@ -45,7 +45,7 @@ void navigate()
|
||||||
void calc_nav()
|
void calc_nav()
|
||||||
{
|
{
|
||||||
Vector2f yawvector;
|
Vector2f yawvector;
|
||||||
long long_error, lat_error;
|
Matrix3f temp;
|
||||||
|
|
||||||
/*
|
/*
|
||||||
Becuase we are using lat and lon to do our distance errors here's a quick chart:
|
Becuase we are using lat and lon to do our distance errors here's a quick chart:
|
||||||
|
@ -56,34 +56,33 @@ void calc_nav()
|
||||||
pitch_max = 22° (2200)
|
pitch_max = 22° (2200)
|
||||||
*/
|
*/
|
||||||
|
|
||||||
Matrix3f temp = dcm.get_dcm_matrix();
|
temp = dcm.get_dcm_matrix();
|
||||||
yawvector.x = temp.a.x; // sin
|
|
||||||
yawvector.y = temp.b.x; // cos
|
yawvector.y = temp.b.x; // cos
|
||||||
|
yawvector.x = temp.a.x; // sin
|
||||||
yawvector.normalize();
|
yawvector.normalize();
|
||||||
|
|
||||||
yawvector.y = -yawvector.y;
|
cos_yaw_x = yawvector.y; // 0
|
||||||
//cos =
|
sin_yaw_y = yawvector.x; // 1
|
||||||
long_error = (float)(next_WP.lng - current_loc.lng) * scaleLongDown;
|
|
||||||
lat_error = next_WP.lat - current_loc.lat;
|
long_error = (float)(next_WP.lng - current_loc.lng) * scaleLongDown; // 50 - 30 = 20 pitch right
|
||||||
|
lat_error = next_WP.lat - current_loc.lat; // 50 - 30 = 20 pitch up
|
||||||
|
|
||||||
long_error = constrain(long_error, -DIST_ERROR_MAX, DIST_ERROR_MAX); // +- 20m max error
|
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
|
lat_error = constrain(lat_error, -DIST_ERROR_MAX, DIST_ERROR_MAX); // +- 20m max error
|
||||||
|
|
||||||
// ROLL
|
// Convert distance into ROLL X
|
||||||
nav_lon = long_error * pid_nav_lon.kP();
|
nav_lon = long_error * pid_nav_lon.kP(); // 1800 * 2 = 3600 or 36°
|
||||||
//nav_lon = pid_nav_lon.get_pid(long_error, dTnav2, 1.0);
|
//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
|
//nav_lon = constrain(nav_lon, -DIST_ERROR_MAX, DIST_ERROR_MAX); // Limit max command
|
||||||
|
|
||||||
// PITCH
|
// PITCH Y
|
||||||
//nav_lat = pid_nav_lat.get_pid(lat_error, dTnav2, 1.0);
|
//nav_lat = pid_nav_lat.get_pid(lat_error, dTnav2, 1.0);
|
||||||
nav_lat = lat_error * pid_nav_lat.kP();
|
nav_lat = lat_error * pid_nav_lat.kP(); // 1800 * 2 = 3600 or 36°
|
||||||
//nav_lat = constrain(nav_lat, -DIST_ERROR_MAX, DIST_ERROR_MAX); // Limit max command
|
//nav_lat = constrain(nav_lat, -DIST_ERROR_MAX, DIST_ERROR_MAX); // Limit max command
|
||||||
|
|
||||||
// rotate the vector
|
// rotate the vector
|
||||||
nav_roll = (float)nav_lon * yawvector.x - (float)nav_lat * yawvector.y;
|
nav_roll = (float)nav_lon * sin_yaw_y - (float)nav_lat * cos_yaw_x;
|
||||||
nav_pitch = (float)nav_lat * yawvector.x + (float)nav_lon * yawvector.y;
|
nav_pitch = -((float)nav_lon * cos_yaw_x + (float)nav_lat * sin_yaw_y);
|
||||||
|
|
||||||
//Serial.printf("vx %4.4f,vy %4.4f ", yawvector.x, yawvector.y);
|
|
||||||
|
|
||||||
nav_roll = constrain(nav_roll, -pitch_max, pitch_max);
|
nav_roll = constrain(nav_roll, -pitch_max, pitch_max);
|
||||||
nav_pitch = constrain(nav_pitch, -pitch_max, pitch_max);
|
nav_pitch = constrain(nav_pitch, -pitch_max, pitch_max);
|
||||||
|
|
Loading…
Reference in New Issue