From b1239dcd0696b10d1345f521b6715ff7904cb961 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 3 Jan 2021 15:08:17 +1100 Subject: [PATCH] SITL: correct OBSTACLE_DISTANCE message output from SIM_Morse --- libraries/SITL/SIM_Morse.cpp | 22 ++++++++++++---------- 1 file changed, 12 insertions(+), 10 deletions(-) diff --git a/libraries/SITL/SIM_Morse.cpp b/libraries/SITL/SIM_Morse.cpp index 857c1168eb..9099087ed5 100644 --- a/libraries/SITL/SIM_Morse.cpp +++ b/libraries/SITL/SIM_Morse.cpp @@ -620,7 +620,9 @@ void Morse::send_report(void) mavlink_obstacle_distance_t packet {}; packet.time_usec = AP_HAL::micros64(); packet.min_distance = 1; - packet.max_distance = 0; + // the simulated rangefinder has an imposed 18m limit in + // e.g. rover_scanner.py + packet.max_distance = 1800; packet.sensor_type = MAV_DISTANCE_SENSOR_LASER; packet.increment = 0; // use increment_f @@ -629,25 +631,25 @@ void Morse::send_report(void) for (uint8_t i=0; i= scanner.points.length) { + packet.distances[i] = 65535; continue; } // convert m to cm and sanity check const Vector2f v = Vector2f(scanner.points.data[i].x, scanner.points.data[i].y); const float distance_cm = v.length()*100; - if (distance_cm < packet.min_distance || distance_cm >= 65535) { + if (distance_cm < packet.min_distance) { + packet.distances[i] = packet.max_distance + 1; // "no obstacle" + continue; + } + + if (distance_cm > packet.max_distance) { + packet.distances[i] = packet.max_distance + 1; // "no obstacle" continue; } packet.distances[i] = distance_cm; - const float max_cm = scanner.ranges.data[i] * 100.0; - if (packet.max_distance < max_cm && max_cm > 0 && max_cm < 65535) { - packet.max_distance = max_cm; - } } mavlink_message_t msg; @@ -667,4 +669,4 @@ void Morse::send_report(void) } } -} \ No newline at end of file +}