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

ACM: check compass health before using it

Browse Source
This commit is contained in:
Andrew Tridgell 2011-12-28 20:35:14 +11:00
parent 03a16c6e45
commit 955dfe0226
2 changed files with 15 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
ArduCopter/ArduCopter.pde
View File

@ -703,9 +703,10 @@ static void medium_loop()
#if HIL_MODE != HIL_MODE_ATTITUDE // don't execute in HIL mode
if(g.compass_enabled){
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());
}
}
#endif

19
ArduCopter/test.pde
View File

@ -903,14 +903,17 @@ test_mag(uint8_t argc, const Menu::arg *argv)
while(1){
delay(100);
compass.read();
compass.calculate(dcm.get_dcm_matrix());
Vector3f maggy = compass.get_offsets();
Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d\n"),
(wrap_360(ToDeg(compass.heading) * 100)) /100,
compass.mag_x,
compass.mag_y,
compass.mag_z);
if (compass.read()) {
compass.calculate(dcm.get_dcm_matrix());
Vector3f maggy = compass.get_offsets();
Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d\n"),
(wrap_360(ToDeg(compass.heading) * 100)) /100,
compass.mag_x,
compass.mag_y,
compass.mag_z);
} else {
Serial.println_P(PSTR("not healthy"));
}
if(Serial.available() > 0){
return (0);