From c485cc57c9213cd30f57a2482b0bc7b48f1c2a15 Mon Sep 17 00:00:00 2001
From: Jean-Louis Naudin <jlnaudin@gmail.com>
Date: Wed, 2 May 2012 21:39:10 +0200
Subject: [PATCH] APMrover v2.0b - Special update for rover heading calculation
if compass is enabled
Signed-off-by: Jean-Louis Naudin <jlnaudin@gmail.com>
---
APMrover2/APM_Config_Rover.h | 3 +++
APMrover2/APMrover2.pde | 10 ++++++++--
APMrover2/GCS_Mavlink.pde | 11 ++++++++---
APMrover2/Log.pde | 4 ++--
4 files changed, 21 insertions(+), 7 deletions(-)
diff --git a/APMrover2/APM_Config_Rover.h b/APMrover2/APM_Config_Rover.h
index af081a9ff..b32fd97cb 100644
--- a/APMrover2/APM_Config_Rover.h
+++ b/APMrover2/APM_Config_Rover.h
@@ -14,6 +14,9 @@
#define MAGNETOMETER ENABLED
#define LOGGING_ENABLED ENABLED
+#define MAG_ORIENTATION AP_COMPASS_COMPONENTS_UP_PINS_FORWARD
+#define PARAM_DECLINATION 0.18 // Paris
+
//////////////////////////////////////////////////////////////////////////////
// Serial port speeds.
//
diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde
index 74667a573..58c4e9f46 100644
--- a/APMrover2/APMrover2.pde
+++ b/APMrover2/APMrover2.pde
@@ -1,6 +1,6 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
-#define THISFIRMWARE "APMrover v2.0a JL NAUDIN" //New version of the APMrover for the APM v1 or APM v2 and magnetometer
+#define THISFIRMWARE "APMrover v2.0b JL NAUDIN" //New version of the APMrover for the APM v1 or APM v2 and magnetometer
// This is a full version of Arduplane v2.32 specially adapted for a Rover by Jean-Louis Naudin (JLN)
@@ -24,6 +24,7 @@ version 2.1 of the License, or (at your option) any later version.
//-------------------------------------------------------------------------------------------------------------------------
// Dev Startup : 2012-04-21
//
+// 2012-05-01: special update for rover about ground_course if compass is enabled
// 2012-04-30: Successfully tested in autonomous nav with a waypoints list recorded in live mode
// 2012-04-30: Now a full version for APM v1 or APM v2 with magnetometer
// 2012-04-27: Cosmetic changes
@@ -1046,8 +1047,13 @@ static void update_GPS(void)
current_loc.lng = g_gps->longitude; // Lon * 10**7
current_loc.lat = g_gps->latitude; // Lat * 10**7
current_loc.alt = max((g_gps->altitude - home.alt),0);
- ground_course = g_gps->ground_course;
ground_speed = g_gps->ground_speed;
+ if (g.compass_enabled) {
+ ground_course = (wrap_360(ToDeg(compass.heading) * 100));
+ } else {
+ ground_course = g_gps->ground_course;
+ }
+
// see if we've breached the geo-fence
geofence_check(false);
diff --git a/APMrover2/GCS_Mavlink.pde b/APMrover2/GCS_Mavlink.pde
index 53fd2a7a4..866dc07a8 100644
--- a/APMrover2/GCS_Mavlink.pde
+++ b/APMrover2/GCS_Mavlink.pde
@@ -317,7 +317,9 @@ static void NOINLINE send_location(mavlink_channel_t chan)
g_gps->ground_speed * rot.a.x, // X speed cm/s
g_gps->ground_speed * rot.b.x, // Y speed cm/s
g_gps->ground_speed * rot.c.x,
- g_gps->ground_course); // course in 1/100 degree
+ //g_gps->ground_course); // course in 1/100 degree
+ ground_course); // course in 1/100 degree
+
}
static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
@@ -355,7 +357,8 @@ static void NOINLINE send_gps_raw(mavlink_channel_t chan)
g_gps->hdop,
65535,
g_gps->ground_speed, // cm/s
- g_gps->ground_course, // 1/100 degrees,
+ //g_gps->ground_course, // 1/100 degrees,
+ ground_course, // 1/100 degrees,
g_gps->num_sats);
#else // MAVLINK10
@@ -369,7 +372,9 @@ static void NOINLINE send_gps_raw(mavlink_channel_t chan)
g_gps->hdop,
0.0,
g_gps->ground_speed / 100.0,
- g_gps->ground_course / 100.0);
+ //ground_course = (wrap_360(ToDeg(compass.heading) * 100));
+ //g_gps->ground_course / 100.0);
+ ground_course / 100.0);
#endif // MAVLINK10
}
diff --git a/APMrover2/Log.pde b/APMrover2/Log.pde
index 1ce9537e9..b8e7f9280 100644
--- a/APMrover2/Log.pde
+++ b/APMrover2/Log.pde
@@ -356,8 +356,8 @@ static void Log_Write_GPS( int32_t log_Time, int32_t log_Lattitude, int32_t log_
DataFlash.WriteLong(log_gps_alt);
DataFlash.WriteLong(log_Ground_Speed);
DataFlash.WriteLong(log_Ground_Course);
- DataFlash.WriteInt((int16_t)Vz);
- DataFlash.WriteInt((int16_t)delta_Vz);
+ DataFlash.WriteInt(0);
+ DataFlash.WriteInt(0);
DataFlash.WriteInt((int)airspeed);
DataFlash.WriteByte(END_BYTE);
}
--
GitLab