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

AP_RangeFinder: auto-update PX4 ll40ls max/min distance

Browse Source 
this allows the range of the Lidar to be set by the user using
RNGFND_MAX_CM and RNGFND_MIN_CM
This commit is contained in:
Andrew Tridgell 2014-10-13 19:07:38 +11:00 committed by Randy Mackay
parent 83a5102ec5
commit 7ad0b6177f
2 changed files with 18 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_RangeFinder/AP_RangeFinder_PX4.cpp
View File

@ -39,7 +39,9 @@ uint8_t AP_RangeFinder_PX4::num_px4_instances = 0;
already know that we should setup the rangefinder
*/
AP_RangeFinder_PX4::AP_RangeFinder_PX4(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state) :
AP_RangeFinder_Backend(_ranger, instance, _state)
AP_RangeFinder_Backend(_ranger, instance, _state),
_last_max_distance_cm(-1),
_last_min_distance_cm(-1)
{
_fd = open_driver();
@ -111,6 +113,18 @@ void AP_RangeFinder_PX4::update(void)
float sum = 0;
uint16_t count = 0;
if (_last_max_distance_cm != ranger._max_distance_cm[state.instance] ||
_last_min_distance_cm != ranger._min_distance_cm[state.instance]) {
float max_distance = ranger._max_distance_cm[state.instance]*0.01f;
float min_distance = ranger._min_distance_cm[state.instance]*0.01f;
if (ioctl(_fd, RANGEFINDERIOCSETMAXIUMDISTANCE, (unsigned long)&max_distance) == 0 &&
ioctl(_fd, RANGEFINDERIOCSETMINIUMDISTANCE, (unsigned long)&min_distance) == 0) {
_last_max_distance_cm = ranger._max_distance_cm[state.instance];
_last_min_distance_cm = ranger._min_distance_cm[state.instance];
}
}
while (::read(_fd, &range_report, sizeof(range_report)) == sizeof(range_report) &&
range_report.timestamp != _last_timestamp) {
// Only take valid readings

3
libraries/AP_RangeFinder/AP_RangeFinder_PX4.h
View File

@ -39,6 +39,9 @@ private:
int _fd;
uint64_t _last_timestamp;
int16_t _last_max_distance_cm;
int16_t _last_min_distance_cm;
// we need to keep track of how many PX4 drivers have been loaded
// so we can open the right device filename
static uint8_t num_px4_instances;