From 3641d3d5081cb7325a35cb04df9bdaa2017095f2 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 5 Jan 2015 17:50:14 +0900 Subject: [PATCH] Copter: send inav velocities in global position message Also absolute alt now calculated from home alt + current alt above home instead of using GPS absolute alt --- ArduCopter/GCS_Mavlink.pde | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index b2cde83b7a..5353f05a58 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -276,17 +276,17 @@ static void NOINLINE send_location(mavlink_channel_t chan) } else { fix_time = millis(); } - const Vector3f &vel = gps.velocity(); + const Vector3f &vel = inertial_nav.get_velocity(); mavlink_msg_global_position_int_send( chan, fix_time, current_loc.lat, // in 1E7 degrees current_loc.lng, // in 1E7 degrees - gps.location().alt * 10UL, // millimeters above sea level + (ahrs.get_home().alt + current_loc.alt) * 10UL, // millimeters above sea level current_loc.alt * 10, // millimeters above ground - vel.x * 100, // X speed cm/s (+ve North) - vel.y * 100, // Y speed cm/s (+ve East) - vel.x * -100, // Z speed cm/s (+ve up) + vel.x, // X speed cm/s (+ve North) + vel.y, // Y speed cm/s (+ve East) + vel.z, // Z speed cm/s (+ve up) ahrs.yaw_sensor); // compass heading in 1/100 degree }