Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 15:38:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Proximity: Small fixups to the rangefinder backend

Browse Source
This commit is contained in:
Michael du Breuil 2018-05-11 12:29:11 -07:00 committed by Randy Mackay
parent 81e453dee3
commit 00c512c7e2
2 changed files with 19 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
libraries/AP_Proximity/AP_Proximity_RangeFinder.cpp
View File

@ -38,8 +38,10 @@ void AP_Proximity_RangeFinder::update(void)
return; return;
} }
uint32_t now = AP_HAL::millis();
// look through all rangefinders // look through all rangefinders
for (uint8_t i=0; i<rngfnd->num_sensors(); i++) { for (uint8_t i=0; i < RANGEFINDER_MAX_INSTANCES; i++) {
AP_RangeFinder_Backend *sensor = rngfnd->get_backend(i); AP_RangeFinder_Backend *sensor = rngfnd->get_backend(i);
if (sensor == nullptr) { if (sensor == nullptr) {
continue; continue;
@ -53,19 +55,26 @@ void AP_Proximity_RangeFinder::update(void)
_distance_min = sensor->min_distance_cm() / 100.0f; _distance_min = sensor->min_distance_cm() / 100.0f;
_distance_max = sensor->max_distance_cm() / 100.0f; _distance_max = sensor->max_distance_cm() / 100.0f;
_distance_valid[sector] = (_distance[sector] >= _distance_min) && (_distance[sector] <= _distance_max); _distance_valid[sector] = (_distance[sector] >= _distance_min) && (_distance[sector] <= _distance_max);
_last_update_ms = AP_HAL::millis(); _last_update_ms = now;
update_boundary_for_sector(sector); update_boundary_for_sector(sector);
} }
// check upward facing range finder // check upward facing range finder
if (sensor->orientation() == ROTATION_PITCH_90) { if (sensor->orientation() == ROTATION_PITCH_90) {
_distance_upward = sensor->distance_cm() / 100.0f; int16_t distance_upward = sensor->distance_cm();
_last_upward_update_ms = AP_HAL::millis(); int16_t up_distance_min = sensor->min_distance_cm();
int16_t up_distance_max = sensor->max_distance_cm();
if ((distance_upward >= up_distance_min) && (distance_upward <= up_distance_max)) {
_distance_upward = distance_upward * 1e2;
} else {
_distance_upward = -1.0; // mark an valid reading
}
_last_upward_update_ms = now;
} }
} }
} }
// check for timeout and set health status // check for timeout and set health status
if ((_last_update_ms == 0) || (AP_HAL::millis() - _last_update_ms > PROXIMITY_RANGEFIDER_TIMEOUT_MS)) { if ((_last_update_ms == 0) || (now - _last_update_ms > PROXIMITY_RANGEFIDER_TIMEOUT_MS)) {
set_status(AP_Proximity::Proximity_NoData); set_status(AP_Proximity::Proximity_NoData);
} else { } else {
set_status(AP_Proximity::Proximity_Good); set_status(AP_Proximity::Proximity_Good);
@ -75,7 +84,9 @@ void AP_Proximity_RangeFinder::update(void)
// get distance upwards in meters. returns true on success // get distance upwards in meters. returns true on success
bool AP_Proximity_RangeFinder::get_upward_distance(float &distance) const bool AP_Proximity_RangeFinder::get_upward_distance(float &distance) const
{ {
if ((_last_upward_update_ms != 0) && (AP_HAL::millis() - _last_upward_update_ms <= PROXIMITY_RANGEFIDER_TIMEOUT_MS)) { if ((_last_upward_update_ms != 0) &&
(AP_HAL::millis() - _last_upward_update_ms <= PROXIMITY_RANGEFIDER_TIMEOUT_MS) &&
is_positive(_distance_upward)) {
distance = _distance_upward; distance = _distance_upward;
return true; return true;
} }

4
libraries/AP_Proximity/AP_Proximity_RangeFinder.h
View File

@ -3,7 +3,7 @@
#include "AP_Proximity.h" #include "AP_Proximity.h"
#include "AP_Proximity_Backend.h" #include "AP_Proximity_Backend.h"
#define PROXIMITY_RANGEFIDER_TIMEOUT_MS 200 // requests timeout after 0.5 seconds #define PROXIMITY_RANGEFIDER_TIMEOUT_MS 200 // requests timeout after 0.2 seconds
class AP_Proximity_RangeFinder : public AP_Proximity_Backend class AP_Proximity_RangeFinder : public AP_Proximity_Backend
{ {
@ -31,5 +31,5 @@ private:
// upward distance support // upward distance support
uint32_t _last_upward_update_ms; // system time of last update distance uint32_t _last_upward_update_ms; // system time of last update distance
float _distance_upward; // upward distance in meters float _distance_upward; // upward distance in meters, negative if the last reading was out of range
}; };