From 69fbcc86838d3e9968970ea0fe06813b40083f09 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 20 Nov 2012 18:23:31 +1100 Subject: [PATCH] MAVLink: fixed accel and gyro sensor offsets in MAVLink --- APMrover2/GCS_Mavlink.pde | 13 +++++++++---- ArduCopter/GCS_Mavlink.pde | 10 ++++++++-- ArduPlane/GCS_Mavlink.pde | 10 ++++++++-- 3 files changed, 25 insertions(+), 8 deletions(-) diff --git a/APMrover2/GCS_Mavlink.pde b/APMrover2/GCS_Mavlink.pde index 85a166d75b..67cfb399f9 100644 --- a/APMrover2/GCS_Mavlink.pde +++ b/APMrover2/GCS_Mavlink.pde @@ -378,16 +378,21 @@ static void NOINLINE send_raw_imu1(mavlink_channel_t chan) static void NOINLINE send_raw_imu3(mavlink_channel_t chan) { Vector3f mag_offsets = compass.get_offsets(); + Vector3f accel_offsets = ins.get_accel_offsets(); + Vector3f gyro_offsets = ins.get_gyro_offsets(); mavlink_msg_sensor_offsets_send(chan, mag_offsets.x, mag_offsets.y, mag_offsets.z, compass.get_declination(), - 0, - 0, - ins.gx(), ins.gy(), ins.gz(), - ins.ax(), ins.ay(), ins.az()); + 0, 0, + gyro_offsets.x, + gyro_offsets.y, + gyro_offsets.z, + accel_offsets.x, + accel_offsets.y, + accel_offsets.z); } static void NOINLINE send_ahrs(mavlink_channel_t chan) diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 8cace919bd..7707a63ad3 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -442,6 +442,8 @@ static void NOINLINE send_raw_imu2(mavlink_channel_t chan) static void NOINLINE send_raw_imu3(mavlink_channel_t chan) { Vector3f mag_offsets = compass.get_offsets(); + Vector3f accel_offsets = ins.get_accel_offsets(); + Vector3f gyro_offsets = ins.get_gyro_offsets(); mavlink_msg_sensor_offsets_send(chan, mag_offsets.x, @@ -450,8 +452,12 @@ static void NOINLINE send_raw_imu3(mavlink_channel_t chan) compass.get_declination(), barometer.get_raw_pressure(), barometer.get_raw_temp(), - ins.gx(), ins.gy(), ins.gz(), - ins.ax(), ins.ay(), ins.az()); + gyro_offsets.x, + gyro_offsets.y, + gyro_offsets.z, + accel_offsets.x, + accel_offsets.y, + accel_offsets.z); } static void NOINLINE send_gps_status(mavlink_channel_t chan) diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 1f8712744a..7b89921b02 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -405,6 +405,8 @@ static void NOINLINE send_raw_imu2(mavlink_channel_t chan) static void NOINLINE send_raw_imu3(mavlink_channel_t chan) { Vector3f mag_offsets = compass.get_offsets(); + Vector3f accel_offsets = ins.get_accel_offsets(); + Vector3f gyro_offsets = ins.get_gyro_offsets(); mavlink_msg_sensor_offsets_send(chan, mag_offsets.x, @@ -413,8 +415,12 @@ static void NOINLINE send_raw_imu3(mavlink_channel_t chan) compass.get_declination(), barometer.get_raw_pressure(), barometer.get_raw_temp(), - ins.gx(), ins.gy(), ins.gz(), - ins.ax(), ins.ay(), ins.az()); + gyro_offsets.x, + gyro_offsets.y, + gyro_offsets.z, + accel_offsets.x, + accel_offsets.y, + accel_offsets.z); } static void NOINLINE send_ahrs(mavlink_channel_t chan)