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

RangeFinder_PX4: set status and consume out of range samples

Browse Source
This commit is contained in:
Randy Mackay 2015-04-13 15:06:29 +09:00
parent 71de0ab815
commit aa04bef5af
1 changed files with 16 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AP_RangeFinder/AP_RangeFinder_PX4.cpp
View File

@ -50,18 +50,19 @@ AP_RangeFinder_PX4::AP_RangeFinder_PX4(RangeFinder &_ranger, uint8_t instance, R
if (_fd == -1) {
hal.console->printf("Unable to open PX4 rangefinder %u\n", num_px4_instances);
state.healthy = false;
set_status(RangeFinder::RangeFinder_NotConnected);
return;
}
// average over up to 20 samples
if (ioctl(_fd, SENSORIOCSQUEUEDEPTH, 20) != 0) {
hal.console->printf("Failed to setup range finder queue\n");
state.healthy = false;
set_status(RangeFinder::RangeFinder_NotConnected);
return;
}
state.healthy = true;
// initialise to connected but no data
set_status(RangeFinder::RangeFinder_NoData);
}
/*
@ -101,7 +102,7 @@ bool AP_RangeFinder_PX4::detect(RangeFinder &_ranger, uint8_t instance)
void AP_RangeFinder_PX4::update(void)
{
if (_fd == -1) {
state.healthy = false;
set_status(RangeFinder::RangeFinder_NotConnected);
return;
}
@ -123,20 +124,23 @@ void AP_RangeFinder_PX4::update(void)
while (::read(_fd, &range_report, sizeof(range_report)) == sizeof(range_report) &&
range_report.timestamp != _last_timestamp) {
// Only take valid readings
if (range_report.valid == 1) {
sum += range_report.distance;
count++;
_last_timestamp = range_report.timestamp;
}
// take reading
sum += range_report.distance;
count++;
_last_timestamp = range_report.timestamp;
}
// consider the range finder healthy if we got a reading in the last 0.2s
state.healthy = (hal.scheduler->micros64() - _last_timestamp < 200000);
// if we have not taken a reading in the last 0.2s set status to No Data
if (hal.scheduler->micros64() - _last_timestamp >= 200000) {
set_status(RangeFinder::RangeFinder_NoData);
}
if (count != 0) {
state.distance_cm = sum / count * 100.0f;
state.distance_cm += ranger._offset[state.instance];
// update range_valid state based on distance measured
update_status();
}
}