Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Implemented new DCM based yaw independent heading controller.

Browse Source 
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 cbdf462acc
commit cf7488317d
2 changed files with 35 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
ArduCopterMega/navigation.pde
View File

@ -46,30 +46,39 @@ void navigate()
} }
#define DIST_ERROR_MAX 3000
void calc_nav() void calc_nav()
{ {
/* /*
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:
100 = 1m 100 = 1m
1000 = 11m 1000 = 11m
3000 = 33m
10000 = 111m 10000 = 111m
pitch_max = 22° (2200) pitch_max = 22° (2200)
*/ */
float cos_yaw = cos(dcm.yaw); Vector2f yawvector;
float sin_yaw = sin(dcm.yaw); Matrix3f temp = dcm.get_dcm_matrix();
yawvector.x = temp.a.x;
yawvector.y = temp.b.x;
yawvector.normalize();
// ROLL // ROLL
nav_lon = pid_nav_lon.get_pid((long)((float)(next_WP.lng - GPS.longitude) * scaleLongDown), dTnav, 1.0); 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 // PITCH
nav_lat = pid_nav_lat.get_pid(next_WP.lat - GPS.latitude, dTnav, 1.0); 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 // rotate the vector
nav_roll = (float)nav_lon * cos_yaw - (float)nav_lat * sin_yaw; nav_roll = (float)nav_lon * yawvector.x - (float)nav_lat * yawvector.y;
nav_pitch = (float)nav_lon * sin_yaw + (float)nav_lat * cos_yaw; 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++; ts_num++;
if (ts_num > 10){ if (ts_num > 10){
ts_num = 0; 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)(roll_sensor/100),
(int)(pitch_sensor/100), (int)(pitch_sensor/100),
rc_1.pwm_out, rc_1.pwm_out,
@ -219,6 +219,16 @@ test_stabilize(uint8_t argc, const Menu::arg *argv)
motor_out[RIGHT], motor_out[RIGHT],
motor_out[LEFT], motor_out[LEFT],
motor_out[FRONT], 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]); motor_out[BACK]);
} }
@ -390,9 +400,15 @@ test_imu(uint8_t argc, const Menu::arg *argv)
// We are using the IMU // 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){ if(Serial.available() > 0){