Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-13 10:03:57 -03:00
Code Issues Packages Projects Releases Wiki Activity

AP_Proximity: correct output of DISTANCE_SENSOR message

Browse Source
This commit is contained in:
Peter Barker 2020-08-10 19:29:26 +10:00 committed by Peter Barker
parent ea5aa594a3
commit 574f3a30f9
1 changed files with 2 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AP_Proximity/AP_Proximity_Backend.cpp
View File

@ -103,7 +103,8 @@ bool AP_Proximity_Backend::get_horizontal_distances(AP_Proximity::Proximity_Dist
for (uint8_t i=0; i<PROXIMITY_NUM_SECTORS; i++) {
if (_distance_valid[i]) {
// convert angle to orientation
int16_t orientation = static_cast<int16_t>(_angle[i] * (PROXIMITY_MAX_DIRECTION / 360.0f));
int16_t orientation = static_cast<int16_t>((_angle[i]+PROXIMITY_SECTOR_WIDTH_DEG*0.5) * (PROXIMITY_MAX_DIRECTION / 360.0f));
orientation %= PROXIMITY_MAX_DIRECTION;
if ((orientation >= 0) && (orientation < PROXIMITY_MAX_DIRECTION) && (_distance[i] < prx_dist_array.distance[orientation])) {
prx_dist_array.distance[orientation] = _distance[i];
dist_set[orientation] = true;