From 2ca8ac7259abe0f8839419ca3dbfaaeaf0ae3221 Mon Sep 17 00:00:00 2001 From: jasonshort Date: Mon, 30 Aug 2010 00:09:47 +0000 Subject: [PATCH] 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 --- libraries/AP_Compass/AP_Compass.cpp | 1 + libraries/AP_Compass/Compass.h | 1 + 2 files changed, 2 insertions(+) diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index 27d41c13d6..34bfbcca00 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -107,6 +107,7 @@ AP_Compass::calculate(float roll, float pitch) // Magnetic heading heading = atan2(-head_Y, head_X); + ground_course = degrees(heading) + 180; // Optimization for external DCM use. calculate normalized components heading_X = cos(heading); diff --git a/libraries/AP_Compass/Compass.h b/libraries/AP_Compass/Compass.h index 8ce8913b54..0f7b320ad4 100644 --- a/libraries/AP_Compass/Compass.h +++ b/libraries/AP_Compass/Compass.h @@ -13,6 +13,7 @@ class Compass int16_t mag_X; int16_t mag_Y; int16_t mag_Z; + int32_t ground_course; float heading; float heading_X; float heading_Y;