Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

APM: added error checking on compass read

Browse Source
This commit is contained in:
Andrew Tridgell 2011-12-28 20:33:47 +11:00
parent 3cb96fb8af
commit 98353b7ba8
2 changed files with 25 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
ArduPlane/ArduPlane.pde
View File

@ -621,11 +621,10 @@ static void medium_loop()
}
#if HIL_MODE != HIL_MODE_ATTITUDE
if(g.compass_enabled){
compass.read(); // Read magnetometer
compass.calculate(dcm.get_dcm_matrix()); // Calculate heading
compass.null_offsets(dcm.get_dcm_matrix());
}
if (g.compass_enabled && compass.read()) {
compass.calculate(dcm.get_dcm_matrix()); // Calculate heading
compass.null_offsets(dcm.get_dcm_matrix());
}
#endif
/*{
Serial.print(dcm.roll_sensor, DEC); Serial.printf_P(PSTR("\t"));

36
ArduPlane/test.pde
View File

@ -547,9 +547,10 @@ test_imu(uint8_t argc, const Menu::arg *argv)
if(g.compass_enabled) {
medium_loopCounter++;
if(medium_loopCounter == 5){
compass.read(); // Read magnetometer
compass.calculate(dcm.get_dcm_matrix()); // Calculate heading
medium_loopCounter = 0;
if (compass.read()) {
compass.calculate(dcm.get_dcm_matrix()); // Calculate heading
}
medium_loopCounter = 0;
}
}
@ -610,23 +611,28 @@ test_mag(uint8_t argc, const Menu::arg *argv)
medium_loopCounter++;
if(medium_loopCounter == 5){
compass.read(); // Read magnetometer
compass.calculate(dcm.get_dcm_matrix()); // Calculate heading
compass.null_offsets(dcm.get_dcm_matrix());
if (compass.read()) {
compass.calculate(dcm.get_dcm_matrix()); // Calculate heading
compass.null_offsets(dcm.get_dcm_matrix());
}
medium_loopCounter = 0;
}
counter++;
if (counter>20) {
Vector3f maggy = compass.get_offsets();
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,
compass.mag_x,
compass.mag_y,
compass.mag_z,
maggy.x,
maggy.y,
maggy.z);
if (compass.healthy) {
Vector3f maggy = compass.get_offsets();
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,
compass.mag_x,
compass.mag_y,
compass.mag_z,
maggy.x,
maggy.y,
maggy.z);
} else {
Serial.println_P(PSTR("compass not healthy"));
}
counter=0;
}
}