From fc4f111d1501f010fa0d6ed3f7349acb7a47e29d Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Mon, 16 Jul 2012 20:20:20 -0700 Subject: [PATCH] Changed mavlink GLOBAL_POSITION_INT.relative_alt message to correctly return altitude above ground. --- APMrover2/GCS_Mavlink.pde | 4 ++-- ArduCopter/GCS_Mavlink.pde | 4 ++-- ArduPlane/GCS_Mavlink.pde | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/APMrover2/GCS_Mavlink.pde b/APMrover2/GCS_Mavlink.pde index 29dd212ad7..0ff2b5f84e 100644 --- a/APMrover2/GCS_Mavlink.pde +++ b/APMrover2/GCS_Mavlink.pde @@ -236,8 +236,8 @@ static void NOINLINE send_location(mavlink_channel_t chan) millis(), current_loc.lat, // in 1E7 degrees current_loc.lng, // in 1E7 degrees - g_gps->altitude*10, // millimeters above sea level - current_loc.alt * 10, // millimeters above ground + g_gps->altitude * 10, // millimeters above sea level + (current_loc.alt - home.alt) * 10, // millimeters above ground 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, diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index d9b7c5913f..ee5a3cb220 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -198,8 +198,8 @@ static void NOINLINE send_location(mavlink_channel_t chan) millis(), current_loc.lat, // in 1E7 degrees current_loc.lng, // in 1E7 degrees - g_gps->altitude*10, // millimeters above sea level - current_loc.alt * 10, // millimeters above ground + g_gps->altitude * 10, // millimeters above sea level + (current_loc.alt - home.alt) * 10, // millimeters above ground 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, diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 77e4f42069..a54641492b 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -241,8 +241,8 @@ static void NOINLINE send_location(mavlink_channel_t chan) millis(), current_loc.lat, // in 1E7 degrees current_loc.lng, // in 1E7 degrees - g_gps->altitude*10, // millimeters above sea level - current_loc.alt * 10, // millimeters above ground + g_gps->altitude * 10, // millimeters above sea level + (current_loc.alt-home.alt) * 10, // millimeters above ground 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,