From bf9e193062774726c108198836e6d14ca88da391 Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Sun, 5 Jan 2020 19:11:51 +0530 Subject: [PATCH] collision_prevention: fix sensor range float rounding --- src/lib/collision_prevention/CollisionPrevention.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index 70729202ba..56af69b804 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -167,7 +167,7 @@ CollisionPrevention::_enterData(int map_index, float sensor_range, float sensor_ //3. this sensor data is out of range, the last reading was as well and this is the sensor with longest range //4. this sensor data is out of range, the last reading was valid and coming from the same sensor - uint16_t sensor_range_cm = (int)(100 * sensor_range); //convert to cm + uint16_t sensor_range_cm = static_cast(100.0f * sensor_range + 0.5f); //convert to cm if (sensor_reading < sensor_range) { if ((_obstacle_map_body_frame.distances[map_index] < _data_maxranges[map_index] @@ -272,13 +272,13 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, distance_reading = distance_reading * sensor_dist_scale; } - uint16_t sensor_range = (int)(100 * distance_sensor.max_distance); // convert to cm + uint16_t sensor_range = static_cast(100.0f * distance_sensor.max_distance + 0.5f); // convert to cm for (int bin = lower_bound; bin <= upper_bound; ++bin) { int wrapped_bin = wrap_bin(bin); if (_enterData(wrapped_bin, distance_sensor.max_distance, distance_reading)) { - _obstacle_map_body_frame.distances[wrapped_bin] = (int)(100 * distance_reading); + _obstacle_map_body_frame.distances[wrapped_bin] = static_cast(100.0f * distance_reading + 0.5f); _data_timestamps[wrapped_bin] = _obstacle_map_body_frame.timestamp; _data_maxranges[wrapped_bin] = sensor_range; }