aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Proximity: use rangefinder backend accessors

Browse Source
This commit is contained in:
Peter Barker 2017-08-08 14:52:19 +10:00 committed by Francisco Ferreira
parent c0aa10d84b
commit e0bea597c0
1 changed files with 13 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
libraries/AP_Proximity/AP_Proximity_RangeFinder.cpp
View File

@ -18,6 +18,7 @@
#include <AP_SerialManager/AP_SerialManager.h>
#include <ctype.h>
#include <stdio.h>
#include <AP_RangeFinder/RangeFinder_Backend.h>
extern const AP_HAL::HAL& hal;
@ -39,21 +40,25 @@ void AP_Proximity_RangeFinder::update(void)
// look through all rangefinders
for (uint8_t i=0; i<rngfnd->num_sensors(); i++) {
if (rngfnd->has_data(i)) {
AP_RangeFinder_Backend *sensor = rngfnd->get_backend(i);
if (sensor == nullptr) {
continue;
}
if (sensor->has_data()) {
// check for horizontal range finders
if (rngfnd->get_orientation(i) <= ROTATION_YAW_315) {
uint8_t sector = (uint8_t)rngfnd->get_orientation(i);
if (sensor->orientation() <= ROTATION_YAW_315) {
uint8_t sector = (uint8_t)sensor->orientation();
_angle[sector] = sector * 45;
_distance[sector] = rngfnd->distance_cm(i) / 100.0f;
_distance_min = rngfnd->min_distance_cm(i) / 100.0f;
_distance_max = rngfnd->max_distance_cm(i) / 100.0f;
_distance[sector] = sensor->distance_cm() / 100.0f;
_distance_min = sensor->min_distance_cm() / 100.0f;
_distance_max = sensor->max_distance_cm() / 100.0f;
_distance_valid[sector] = (_distance[sector] >= _distance_min) && (_distance[sector] <= _distance_max);
_last_update_ms = AP_HAL::millis();
update_boundary_for_sector(sector);
}
// check upward facing range finder
if (rngfnd->get_orientation(i) == ROTATION_PITCH_90) {
_distance_upward = rngfnd->distance_cm(i) / 100.0f;
if (sensor->orientation() == ROTATION_PITCH_90) {
_distance_upward = sensor->distance_cm() / 100.0f;
_last_upward_update_ms = AP_HAL::millis();
}
}