mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
added ground_course in proper 0-360 degrees * 100 for Ardupilot
git-svn-id: https://arducopter.googlecode.com/svn/trunk@348 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
parent
cf7eefd94b
commit
2ca8ac7259
@ -107,6 +107,7 @@ AP_Compass::calculate(float roll, float pitch)
|
|||||||
|
|
||||||
// Magnetic heading
|
// Magnetic heading
|
||||||
heading = atan2(-head_Y, head_X);
|
heading = atan2(-head_Y, head_X);
|
||||||
|
ground_course = degrees(heading) + 180;
|
||||||
|
|
||||||
// Optimization for external DCM use. calculate normalized components
|
// Optimization for external DCM use. calculate normalized components
|
||||||
heading_X = cos(heading);
|
heading_X = cos(heading);
|
||||||
|
@ -13,6 +13,7 @@ class Compass
|
|||||||
int16_t mag_X;
|
int16_t mag_X;
|
||||||
int16_t mag_Y;
|
int16_t mag_Y;
|
||||||
int16_t mag_Z;
|
int16_t mag_Z;
|
||||||
|
int32_t ground_course;
|
||||||
float heading;
|
float heading;
|
||||||
float heading_X;
|
float heading_X;
|
||||||
float heading_Y;
|
float heading_Y;
|
||||||
|
Loading…
Reference in New Issue
Block a user