GCS_MAVLink: allow for more than 327m range rangefinders

This commit is contained in:
Peter Barker 2024-10-11 14:46:48 +11:00 committed by Andrew Tridgell
parent 8ea5ef2784
commit b54e795a0e

View File

@ -429,9 +429,9 @@ void GCS_MAVLINK::send_distance_sensor(const AP_RangeFinder_Backend *sensor, con
mavlink_msg_distance_sensor_send(
chan,
AP_HAL::millis(), // time since system boot TODO: take time of measurement
sensor->min_distance_cm(), // minimum distance the sensor can measure in centimeters
sensor->max_distance_cm(), // maximum distance the sensor can measure in centimeters
sensor->distance_cm(), // current distance reading
MIN(sensor->min_distance() * 100, 65535),// minimum distance the sensor can measure in centimeters
MIN(sensor->max_distance() * 100, 65535),// maximum distance the sensor can measure in centimeters
MIN(sensor->distance() * 100, 65535), // current distance reading
sensor->get_mav_distance_sensor_type(), // type from MAV_DISTANCE_SENSOR enum
instance, // onboard ID of the sensor == instance
sensor->orientation(), // direction the sensor faces from MAV_SENSOR_ORIENTATION enum