From c485cc57c9213cd30f57a2482b0bc7b48f1c2a15 Mon Sep 17 00:00:00 2001 From: Jean-Louis Naudin 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 --- 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 af081a9ff3..b32fd97cbc 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 74667a573e..58c4e9f463 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 53fd2a7a45..866dc07a81 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 1ce9537e9e..b8e7f92800 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); }