fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-06 15:40:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

ll40ls: simplified probe logic and lowered max readable distance.

Browse Source 
Signed-off-by: Claudio Micheli <claudio@auterion.com>
This commit is contained in:
Claudio Micheli
2019-09-02 13:38:17 +02:00
committed by Nuno Marques
parent 59f61b3107
commit 6a8b25a1b9
2 changed files with 30 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/distance_sensor/ll40ls/LidarLite.h
+2 -1
View File
@@ -46,7 +46,8 @@
/* Device limits */
#define LL40LS_MIN_DISTANCE (0.05f)
#define LL40LS_MAX_DISTANCE (35.00f)
#define LL40LS_MAX_DISTANCE (25.00f)
#define LL40LS_MAX_DISTANCE_V2 (35.00f)
// normal conversion wait time
src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp
+28 -14
View File
@@ -140,21 +140,35 @@ LidarLiteI2C::probe()
/*
check for hw and sw versions for v1, v2 and v3
*/
if ((read_reg(LL40LS_HW_VERSION, _hw_version) == OK && _hw_version) > 0 &&
(read_reg(LL40LS_SW_VERSION, _sw_version) == OK && _sw_version > 0)) {
PX4_INFO("probe success - hw: %u, sw:%u", _hw_version, _sw_version);
_retries = 3;
return reset_sensor();
}
if ((read_reg(LL40LS_HW_VERSION, _hw_version) == OK) &&
(read_reg(LL40LS_SW_VERSION, _sw_version) == OK)) {
if (read_reg(LL40LS_UNIT_ID_HIGH, id_high) == OK &&
read_reg(LL40LS_UNIT_ID_LOW, id_low) == OK) {
_unit_id = (uint16_t)((id_high << 8) | id_low) & 0xFFFF;
}
if (_hw_version > 0) {
if (_unit_id > 0) {
// v2
PX4_INFO("probe success - hw: %u, sw:%u, id: %u", _hw_version, _sw_version, _unit_id);
_px4_rangefinder.set_max_distance(LL40LS_MAX_DISTANCE_V2);
} else {
// v1 and v3
PX4_INFO("probe success - hw: %u, sw:%u", _hw_version, _sw_version);
}
} else {
if (_unit_id > 0) {
// v3hp
_is_V3hp = true;
PX4_INFO("probe success - id: %u", _unit_id);
}
}
/*
check for unit id for v3HP
*/
if (read_reg(LL40LS_UNIT_ID_HIGH, id_high) == OK &&
read_reg(LL40LS_UNIT_ID_LOW, id_low) == OK) {
_unit_id = (uint16_t)((id_high << 8) | id_low) & 0xFFFF;
_is_V3hp = true;
PX4_INFO("probe success - id: %u", _unit_id);
_retries = 3;
return reset_sensor();
}