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

RangeFinder_PX4_PWM: set status and consume out of range samples

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

26
libraries/AP_RangeFinder/AP_RangeFinder_PX4_PWM.cpp
View File

@ -47,19 +47,22 @@ AP_RangeFinder_PX4_PWM::AP_RangeFinder_PX4_PWM(RangeFinder &_ranger, uint8_t ins
_good_sample_count(0), _good_sample_count(0),
_last_sample_distance_cm(0) _last_sample_distance_cm(0)
{ {
state.healthy = false;
_fd = open(PWMIN0_DEVICE_PATH, O_RDONLY); _fd = open(PWMIN0_DEVICE_PATH, O_RDONLY);
if (_fd == -1) { if (_fd == -1) {
hal.console->printf("Unable to open PX4 PWM rangefinder\n"); hal.console->printf("Unable to open PX4 PWM rangefinder\n");
set_status(RangeFinder::RangeFinder_NotConnected);
return; return;
} }
// keep a queue of 20 samples // keep a queue of 20 samples
if (ioctl(_fd, SENSORIOCSQUEUEDEPTH, 20) != 0) { if (ioctl(_fd, SENSORIOCSQUEUEDEPTH, 20) != 0) {
hal.console->printf("Failed to setup range finder queue\n"); hal.console->printf("Failed to setup range finder queue\n");
set_status(RangeFinder::RangeFinder_NotConnected);
return; return;
} }
// initialise to connected but no data
set_status(RangeFinder::RangeFinder_NoData);
} }
/* /*
@ -70,6 +73,7 @@ AP_RangeFinder_PX4_PWM::~AP_RangeFinder_PX4_PWM()
if (_fd != -1) { if (_fd != -1) {
close(_fd); close(_fd);
} }
set_status(RangeFinder::RangeFinder_NotConnected);
} }
/* /*
@ -88,6 +92,7 @@ bool AP_RangeFinder_PX4_PWM::detect(RangeFinder &_ranger, uint8_t instance)
void AP_RangeFinder_PX4_PWM::update(void) void AP_RangeFinder_PX4_PWM::update(void)
{ {
if (_fd == -1) { if (_fd == -1) {
set_status(RangeFinder::RangeFinder_NotConnected);
return; return;
} }
@ -107,12 +112,6 @@ void AP_RangeFinder_PX4_PWM::update(void)
// setup for scaling in meters per millisecond // setup for scaling in meters per millisecond
float distance_cm = pwm.pulse_width * 0.1f * scaling + ranger._offset[state.instance]; float distance_cm = pwm.pulse_width * 0.1f * scaling + ranger._offset[state.instance];
if (distance_cm > ranger._max_distance_cm[state.instance] ||
distance_cm < ranger._min_distance_cm[state.instance]) {
_good_sample_count = 0;
continue;
}
float distance_delta_cm = fabsf(distance_cm - _last_sample_distance_cm); float distance_delta_cm = fabsf(distance_cm - _last_sample_distance_cm);
_last_sample_distance_cm = distance_cm; _last_sample_distance_cm = distance_cm;
@ -141,14 +140,16 @@ void AP_RangeFinder_PX4_PWM::update(void)
// we are above the power saving range. Disable the sensor // we are above the power saving range. Disable the sensor
hal.gpio->pinMode(stop_pin, HAL_GPIO_OUTPUT); hal.gpio->pinMode(stop_pin, HAL_GPIO_OUTPUT);
hal.gpio->write(stop_pin, false); hal.gpio->write(stop_pin, false);
state.healthy = false; set_status(RangeFinder::RangeFinder_NoData);
state.distance_cm = 0; state.distance_cm = 0;
state.voltage_mv = 0; state.voltage_mv = 0;
return; return;
} }
// consider the range finder healthy if we got a reading in the last 0.2s // if we have not taken a reading in the last 0.2s set status to No Data
state.healthy = (hal.scheduler->micros64() - _last_timestamp < 200000); if (hal.scheduler->micros64() - _last_timestamp >= 200000) {
set_status(RangeFinder::RangeFinder_NoData);
}
/* if we haven't seen any pulses for 0.5s then the sensor is /* if we haven't seen any pulses for 0.5s then the sensor is
probably dead. Try resetting it. Tests show the sensor takes probably dead. Try resetting it. Tests show the sensor takes
@ -182,6 +183,9 @@ void AP_RangeFinder_PX4_PWM::update(void)
if (count != 0) { if (count != 0) {
state.distance_cm = sum_cm / count; state.distance_cm = sum_cm / count;
// update range_valid state based on distance measured
update_status();
} }
} }