aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

APM: ArduPlane updates for new compass interface

Browse Source
This commit is contained in:
Andrew Tridgell 2012-06-20 19:31:19 +10:00
parent d9583ae5eb
commit 44b7d94b1c
3 changed files with 5 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduPlane/ArduPlane.pde
View File

@ -793,9 +793,6 @@ static void medium_loop()
#if HIL_MODE != HIL_MODE_ATTITUDE #if HIL_MODE != HIL_MODE_ATTITUDE
if (g.compass_enabled && compass.read()) { if (g.compass_enabled && compass.read()) {
ahrs.set_compass(&compass); ahrs.set_compass(&compass);
// Calculate heading
Matrix3f m = ahrs.get_dcm_matrix();
compass.calculate(m);
compass.null_offsets(); compass.null_offsets();
} else { } else {
ahrs.set_compass(NULL); ahrs.set_compass(NULL);

1
ArduPlane/system.pde
View File

@ -173,7 +173,6 @@ static void init_ardupilot()
g.compass_enabled = false; g.compass_enabled = false;
} else { } else {
ahrs.set_compass(&compass); ahrs.set_compass(&compass);
compass.null_offsets_enable();
} }
} }
#endif #endif

12
ArduPlane/test.pde
View File

@ -533,10 +533,7 @@ test_imu(uint8_t argc, const Menu::arg *argv)
if(g.compass_enabled) { if(g.compass_enabled) {
medium_loopCounter++; medium_loopCounter++;
if(medium_loopCounter == 5){ if(medium_loopCounter == 5){
if (compass.read()) { compass.read();
// Calculate heading
compass.calculate(ahrs.get_dcm_matrix());
}
medium_loopCounter = 0; medium_loopCounter = 0;
} }
} }
@ -573,7 +570,6 @@ test_mag(uint8_t argc, const Menu::arg *argv)
Serial.println_P(PSTR("Compass initialisation failed!")); Serial.println_P(PSTR("Compass initialisation failed!"));
return 0; return 0;
} }
compass.null_offsets_enable();
ahrs.set_compass(&compass); ahrs.set_compass(&compass);
report_compass(); report_compass();
@ -582,6 +578,8 @@ test_mag(uint8_t argc, const Menu::arg *argv)
ahrs.reset(); ahrs.reset();
int counter = 0; int counter = 0;
float heading = 0;
//Serial.printf_P(PSTR("MAG_ORIENTATION: %d\n"), MAG_ORIENTATION); //Serial.printf_P(PSTR("MAG_ORIENTATION: %d\n"), MAG_ORIENTATION);
print_hit_enter(); print_hit_enter();
@ -602,7 +600,7 @@ test_mag(uint8_t argc, const Menu::arg *argv)
if (compass.read()) { if (compass.read()) {
// Calculate heading // Calculate heading
Matrix3f m = ahrs.get_dcm_matrix(); Matrix3f m = ahrs.get_dcm_matrix();
compass.calculate(m); heading = compass.calculate_heading(m);
compass.null_offsets(); compass.null_offsets();
} }
medium_loopCounter = 0; medium_loopCounter = 0;
@ -613,7 +611,7 @@ test_mag(uint8_t argc, const Menu::arg *argv)
if (compass.healthy) { if (compass.healthy) {
Vector3f maggy = compass.get_offsets(); Vector3f maggy = compass.get_offsets();
Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d,\tXYZoff: %6.2f, %6.2f, %6.2f\n"), Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d,\tXYZoff: %6.2f, %6.2f, %6.2f\n"),
(wrap_360(ToDeg(compass.heading) * 100)) /100, (wrap_360(ToDeg(heading) * 100)) /100,
(int)compass.mag_x, (int)compass.mag_x,
(int)compass.mag_y, (int)compass.mag_y,
(int)compass.mag_z, (int)compass.mag_z,