From 4e568ab547581b961cb730356f18c6eee263c5b3 Mon Sep 17 00:00:00 2001 From: Grant Morphett Date: Wed, 12 Aug 2015 12:42:35 +1000 Subject: [PATCH] Rover: Including the sonar/rangefinder status in SYS_STATUS message --- APMrover2/GCS_Mavlink.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp index 490f5df6b5..0209ab34c0 100644 --- a/APMrover2/GCS_Mavlink.cpp +++ b/APMrover2/GCS_Mavlink.cpp @@ -169,6 +169,16 @@ void Rover::send_extended_status1(mavlink_channel_t chan) battery_current = battery.current_amps() * 100; } + if (sonar.num_sensors() > 0) { + control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; + if (g.sonar_trigger_cm > 0) { + control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; + } + if (sonar.has_data()) { + control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; + } + } + if (AP_Notify::flags.initialising) { // while initialising the gyros and accels are not enabled control_sensors_enabled &= ~(MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL);