From f0369bc50760d8f793d3c1b7cc492b6d65f6e902 Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Mon, 29 May 2017 19:03:43 +0200 Subject: [PATCH] GCS_MAVLink: add distance sensor msg --- libraries/GCS_MAVLink/GCS.h | 2 ++ libraries/GCS_MAVLink/GCS_Common.cpp | 26 ++++++++++++++++++++++++++ 2 files changed, 28 insertions(+) diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 6115fff7db..29e21994f5 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -142,6 +142,8 @@ public: void send_power_status(void); void send_battery_status(const AP_BattMonitor &battery, const uint8_t instance) const; bool send_battery_status(const AP_BattMonitor &battery) const; + void send_distance_sensor(const RangeFinder &rangefinder, const uint8_t instance) const; + bool send_distance_sensor(const RangeFinder &rangefinder) const; void send_ahrs2(AP_AHRS &ahrs); bool send_gps_raw(AP_GPS &gps); void send_system_time(AP_GPS &gps); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 2129c44339..55c589919e 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -219,6 +219,32 @@ bool GCS_MAVLINK::send_battery_status(const AP_BattMonitor &battery) const return true; } +void GCS_MAVLINK::send_distance_sensor(const RangeFinder &rangefinder, const uint8_t instance) const +{ + if (rangefinder.status(instance) != RangeFinder::RangeFinder_NotConnected && + rangefinder.status(instance) != RangeFinder::RangeFinder_NoData) { + mavlink_msg_distance_sensor_send( + chan, + AP_HAL::millis(), // time since system boot TODO: take time of measurement + rangefinder.min_distance_cm(instance), // minimum distance the sensor can measure in centimeters + rangefinder.max_distance_cm(instance), // maximum distance the sensor can measure in centimeters + rangefinder.distance_cm(instance), // current distance reading (in cm?) + rangefinder.get_sensor_type(instance), // type from MAV_DISTANCE_SENSOR enum + instance, // onboard ID of the sensor == instance + rangefinder.get_orientation(instance), // direction the sensor faces from MAV_SENSOR_ORIENTATION enum + 0); // Measurement covariance in centimeters, 0 for unknown / invalid readings + } +} + +bool GCS_MAVLINK::send_distance_sensor(const RangeFinder &rangefinder) const +{ + for (uint8_t i = 0; i < RANGEFINDER_MAX_INSTANCES; i++) { + CHECK_PAYLOAD_SIZE(DISTANCE_SENSOR); + send_distance_sensor(rangefinder, i); + } + return true; +} + // report AHRS2 state void GCS_MAVLINK::send_ahrs2(AP_AHRS &ahrs) {