fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-19 05:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

collision_prevention: fix sensor range float rounding

Browse Source
This commit is contained in:
Mohammed Kabir
2020-01-05 20:22:51 +05:30
committed by Kabir Mohammed
parent a96dc9c0c8
commit bf9e193062
1 changed files with 3 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/collision_prevention/CollisionPrevention.cpp
+3 -3
View File
@@ -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<uint16_t>(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<uint16_t>(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<uint16_t>(100.0f * distance_reading + 0.5f);
_data_timestamps[wrapped_bin] = _obstacle_map_body_frame.timestamp;
_data_maxranges[wrapped_bin] = sensor_range;
}