Browse Source

added ground_course 0-36000 for Ardupilot

git-svn-id: https://arducopter.googlecode.com/svn/trunk@353 f9c3cf11-9bcb-44bc-f272-b75c42450872
mission-4.1.18
jasonshort 15 years ago
parent
commit
11ccbb9ed7
  1. 2
      libraries/AP_Compass/AP_Compass.cpp
  2. 4
      libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde

2
libraries/AP_Compass/AP_Compass.cpp

@ -107,7 +107,7 @@ AP_Compass::calculate(float roll, float pitch) @@ -107,7 +107,7 @@ AP_Compass::calculate(float roll, float pitch)
// Magnetic heading
heading = atan2(-head_Y, head_X);
ground_course = degrees(heading) + 180;
ground_course = (degrees(heading) + 180) * 100;
// Optimization for external DCM use. calculate normalized components
heading_X = cos(heading);

4
libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde

@ -28,13 +28,13 @@ void loop() @@ -28,13 +28,13 @@ void loop()
compass.update();
compass.calculate(0, 0); // roll = 0, pitch = 0 for this example
Serial.print("Heading:");
Serial.print(degrees(compass.heading));
Serial.print(compass.ground_course,DEC);
Serial.print(" (");
Serial.print(compass.mag_X);
Serial.print(",");
Serial.print(compass.mag_Y);
Serial.print(",");
Serial.print(compass.mag_Z);
Serial.println();
Serial.println(" )");
}
}
Loading…
Cancel
Save