From 7ad0b6177f4b588988aaac668906e673b2cdf89c Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Mon, 13 Oct 2014 19:07:38 +1100
Subject: [PATCH] AP_RangeFinder: auto-update PX4 ll40ls max/min distance
this allows the range of the Lidar to be set by the user using
RNGFND_MAX_CM and RNGFND_MIN_CM
---
libraries/AP_RangeFinder/AP_RangeFinder_PX4.cpp | 16 +++++++++++++++-
libraries/AP_RangeFinder/AP_RangeFinder_PX4.h | 3 +++
2 files changed, 18 insertions(+), 1 deletion(-)
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PX4.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_PX4.cpp
index ae620f8bd..cc29868a4 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_PX4.cpp
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_PX4.cpp
@@ -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
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PX4.h b/libraries/AP_RangeFinder/AP_RangeFinder_PX4.h
index f096429c9..48d5fb202 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_PX4.h
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_PX4.h
@@ -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;
--
GitLab