From 805e9519d1673c4d52cdea2e41ca5144ee9cbac5 Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Wed, 9 May 2018 00:46:20 -0700 Subject: [PATCH] GCS_MAVLink: Send rangefinder data --- libraries/GCS_MAVLink/GCS.h | 9 ++-- libraries/GCS_MAVLink/GCS_Common.cpp | 69 ++++++++++++++++++++-------- 2 files changed, 55 insertions(+), 23 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index e85101af53..fdb49cbe5a 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -166,10 +166,9 @@ public: void send_battery_status(const AP_BattMonitor &battery, const uint8_t instance) const; bool send_battery_status() const; - void send_distance_sensor(const AP_RangeFinder_Backend *sensor) const; - bool send_distance_sensor(const RangeFinder &rangefinder) const; - void send_rangefinder_downward(const RangeFinder &rangefinder) const; - bool send_proximity(const AP_Proximity &proximity) const; + bool send_distance_sensor() const; + void send_rangefinder_downward() const; + bool send_proximity() const; void send_ahrs2(); void send_system_time(); void send_radio_in(); @@ -459,6 +458,8 @@ private: // send an async parameter reply void send_parameter_reply(void); + void send_distance_sensor(const AP_RangeFinder_Backend *sensor, const uint8_t instance) const; + virtual bool handle_guided_request(AP_Mission::Mission_Command &cmd) = 0; virtual void handle_change_alt_request(AP_Mission::Mission_Command &cmd) = 0; void handle_common_mission_message(mavlink_message_t *msg); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 62f4451fa0..3d12f0d1ed 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -223,10 +223,6 @@ bool GCS_MAVLINK::send_battery_status() const void GCS_MAVLINK::send_distance_sensor(const AP_RangeFinder_Backend *sensor, const uint8_t instance) const { - if (sensor == nullptr) { - // should not happen - return; - } if (!sensor->has_data()) { return; } @@ -243,22 +239,47 @@ void GCS_MAVLINK::send_distance_sensor(const AP_RangeFinder_Backend *sensor, con 0); // Measurement covariance in centimeters, 0 for unknown / invalid readings } -bool GCS_MAVLINK::send_distance_sensor(const RangeFinder &rangefinder) const +bool GCS_MAVLINK::send_distance_sensor() const { + RangeFinder *rangefinder = RangeFinder::get_singleton(); + if (rangefinder == nullptr) { + return true; // this is wrong, but pretend we sent data and don't requeue + } + + // if we have a proximity backend that utilizes rangefinders cull sending them here, + // and allow the later proximity code to manage them + bool filter_possible_proximity_sensors = false; + AP_Proximity *proximity = AP_Proximity::get_singleton(); + if (proximity != nullptr) { + for (uint8_t i = 0; i < proximity->num_sensors(); i++) { + if (proximity->get_type(i) == AP_Proximity::Proximity_Type_RangeFinder) { + filter_possible_proximity_sensors = true; + } + } + } + for (uint8_t i = 0; i < RANGEFINDER_MAX_INSTANCES; i++) { CHECK_PAYLOAD_SIZE(DISTANCE_SENSOR); - AP_RangeFinder_Backend *sensor = rangefinder.get_backend(i); + AP_RangeFinder_Backend *sensor = rangefinder->get_backend(i); if (sensor == nullptr) { continue; } - send_distance_sensor(sensor, i); + enum Rotation orient = sensor->orientation(); + if (!filter_possible_proximity_sensors || + (orient > ROTATION_YAW_315 && orient != ROTATION_PITCH_90)) { + send_distance_sensor(sensor, i); + } } return true; } -void GCS_MAVLINK::send_rangefinder_downward(const RangeFinder &rangefinder) const +void GCS_MAVLINK::send_rangefinder_downward() const { - AP_RangeFinder_Backend *s = rangefinder.find_instance(ROTATION_PITCH_270); + RangeFinder *rangefinder = RangeFinder::get_singleton(); + if (rangefinder == nullptr) { + return; + } + AP_RangeFinder_Backend *s = rangefinder->find_instance(ROTATION_PITCH_270); if (s == nullptr) { return; } @@ -268,22 +289,25 @@ void GCS_MAVLINK::send_rangefinder_downward(const RangeFinder &rangefinder) cons s->voltage_mv() * 0.001f); } -bool GCS_MAVLINK::send_proximity(const AP_Proximity &proximity) const +bool GCS_MAVLINK::send_proximity() const { - // return immediately if no proximity sensor is present - if (proximity.get_status() == AP_Proximity::Proximity_NotConnected) { - return false; + AP_Proximity *proximity = AP_Proximity::get_singleton(); + if (proximity == nullptr || proximity->get_status() == AP_Proximity::Proximity_NotConnected) { + return true; // this is wrong, but pretend we sent data and don't requeue } + + const uint16_t dist_min = (uint16_t)(proximity->distance_min() * 100.0f); // minimum distance the sensor can measure in centimeters + const uint16_t dist_max = (uint16_t)(proximity->distance_max() * 100.0f); // maximum distance the sensor can measure in centimeters // send horizontal distances AP_Proximity::Proximity_Distance_Array dist_array; - if (proximity.get_horizontal_distances(dist_array)) { + if (proximity->get_horizontal_distances(dist_array)) { for (uint8_t i = 0; i < PROXIMITY_MAX_DIRECTION; i++) { CHECK_PAYLOAD_SIZE(DISTANCE_SENSOR); mavlink_msg_distance_sensor_send( chan, AP_HAL::millis(), // time since system boot - (uint16_t)(proximity.distance_min() * 100.0f), // minimum distance the sensor can measure in centimeters - (uint16_t)(proximity.distance_max() * 100.0f), // maximum distance the sensor can measure in centimeters + dist_min, // minimum distance the sensor can measure in centimeters + dist_max, // maximum distance the sensor can measure in centimeters (uint16_t)(dist_array.distance[i] * 100.0f), // current distance reading MAV_DISTANCE_SENSOR_LASER, // type from MAV_DISTANCE_SENSOR enum PROXIMITY_SENSOR_ID_START + i, // onboard ID of the sensor @@ -294,13 +318,13 @@ bool GCS_MAVLINK::send_proximity(const AP_Proximity &proximity) const // send upward distance float dist_up; - if (proximity.get_upward_distance(dist_up)) { + if (proximity->get_upward_distance(dist_up)) { CHECK_PAYLOAD_SIZE(DISTANCE_SENSOR); mavlink_msg_distance_sensor_send( chan, AP_HAL::millis(), // time since system boot - (uint16_t)(proximity.distance_min() * 100.0f), // minimum distance the sensor can measure in centimeters - (uint16_t)(proximity.distance_max() * 100.0f), // maximum distance the sensor can measure in centimeters + dist_min, // minimum distance the sensor can measure in centimeters + dist_max, // maximum distance the sensor can measure in centimeters (uint16_t)(dist_up * 100.0f), // current distance reading MAV_DISTANCE_SENSOR_LASER, // type from MAV_DISTANCE_SENSOR enum PROXIMITY_SENSOR_ID_START + PROXIMITY_MAX_DIRECTION + 1, // onboard ID of the sensor @@ -2672,6 +2696,13 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) ret = true; break; + case MSG_RANGEFINDER: + CHECK_PAYLOAD_SIZE(RANGEFINDER); + send_rangefinder_downward(); + ret = send_distance_sensor(); + ret = ret && send_proximity(); + break; + case MSG_CAMERA_FEEDBACK: ret = try_send_camera_message(id); break;