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

Implemented new DCM based yaw independent heading controller. 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1495 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-01-13 01:31:05 +00:00
parent e552b7c7e1
commit 78095b1dd7
2 changed files with 35 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
ArduCopterMega/navigation.pde
View File

@ -46,30 +46,39 @@ void navigate()
}
#define DIST_ERROR_MAX 3000
void calc_nav()
{
/*
Becuase we are using lat and lon to do our distance errors here's a quick chart:
100 = 1m
1000 = 11m
3000 = 33m
10000 = 111m
pitch_max = 22° (2200)
*/
float cos_yaw = cos(dcm.yaw);
float sin_yaw = sin(dcm.yaw);
Vector2f yawvector;
Matrix3f temp = dcm.get_dcm_matrix();
yawvector.x = temp.a.x;
yawvector.y = temp.b.x;
yawvector.normalize();
// 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
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, -pitch_max, pitch_max); // Limit max command
nav_lat = constrain(nav_lat, -DIST_ERROR_MAX, DIST_ERROR_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;
nav_roll = (float)nav_lon * yawvector.x - (float)nav_lat * yawvector.y;
nav_pitch = (float)nav_lon * yawvector.y + (float)nav_lat * yawvector.x;
nav_roll = constrain(nav_roll, -pitch_max, pitch_max);
nav_pitch = constrain(nav_pitch, -pitch_max, pitch_max);
}
/*

22
ArduCopterMega/test.pde
View File

@ -207,7 +207,7 @@ test_stabilize(uint8_t argc, const Menu::arg *argv)
ts_num++;
if (ts_num > 10){
ts_num = 0;
Serial.printf_P(PSTR("r: %d, p:%d, rc1:%d, rc2:%d, rc4 %d, ny:%ld, ys:%ld, ye:%ld, R: %d, L: %d F: %d B: %d\n"),
/*Serial.printf_P(PSTR("r: %d, p:%d, rc1:%d, rc2:%d, rc4 %d, ny:%ld, ys:%ld, ye:%ld, R: %d, L: %d F: %d B: %d\n"),
(int)(roll_sensor/100),
(int)(pitch_sensor/100),
rc_1.pwm_out,
@ -219,6 +219,16 @@ test_stabilize(uint8_t argc, const Menu::arg *argv)
motor_out[RIGHT],
motor_out[LEFT],
motor_out[FRONT],
motor_out[BACK]);*/
Serial.printf_P(PSTR("r: %d, p:%d, rc1:%d, Int%4.4f, R: %d, L: %d F: %d B: %d\n"),
(int)(roll_sensor/100),
(int)(pitch_sensor/100),
rc_1.pwm_out,
pid_stabilize_roll.get_integrator(),
motor_out[RIGHT],
motor_out[LEFT],
motor_out[FRONT],
motor_out[BACK]);
}
@ -390,9 +400,15 @@ test_imu(uint8_t argc, const Menu::arg *argv)
// We are using the IMU
// ---------------------
Serial.printf_P(PSTR("A: %d,%d,%d\tG: %d,%d,%d\t"), (int)(accels.x*100), (int)(accels.y*100), (int)(accels.z*100),(int)(gyros.x*100), (int)(gyros.y*100), (int)(gyros.z*100));
Serial.printf_P(PSTR("A: %4.4f, %4.4f, %4.4f\t"
"G: %4.4f, %4.4f, %4.4f\t"),
accels.x, accels.y, accels.z,
gyros.x, gyros.y, gyros.z);
Serial.printf_P(PSTR("r: %d\tp: %d\t y: %d\n"), ((int)roll_sensor/100), ((int)pitch_sensor/100), ((uint16_t)yaw_sensor/100));
Serial.printf_P(PSTR("r: %ld\tp: %ld\t y: %ld\n"),
roll_sensor,
pitch_sensor,
yaw_sensor);
}
if(Serial.available() > 0){