From 09905bb2f21ff33806341c95fd0c0e56a2040150 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 5 Dec 2018 12:04:00 +1100 Subject: [PATCH] GCS_MAVLink: send data for sensors even if no data for other sensors This is notable when you have a lot of compasses, for example --- libraries/GCS_MAVLink/GCS_Common.cpp | 20 ++++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index b8440abf84..9942fae2d7 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -1659,16 +1659,24 @@ void GCS_MAVLINK::send_scaled_imu(uint8_t instance, void (*send_fn)(mavlink_chan const AP_InertialSensor &ins = AP::ins(); const Compass &compass = AP::compass(); - if (ins.get_gyro_count() <= instance || - ins.get_accel_count() <= instance) { - return; + bool have_data = false; + Vector3f accel{}; + if (ins.get_accel_count() > instance) { + accel = ins.get_accel(instance); + have_data = true; + } + Vector3f gyro{}; + if (ins.get_accel_count() > instance) { + gyro = ins.get_gyro(instance); + have_data = true; } - - const Vector3f &accel = ins.get_accel(instance); - const Vector3f &gyro = ins.get_gyro(instance); Vector3f mag{}; if (compass.get_count() > instance) { mag = compass.get_field(instance); + have_data = true; + } + if (!have_data) { + return; } send_fn( chan,