Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_RangeFinder: TFMiniPlus: enforce minimum version 1.7.6 

On this version of the firmware the command changed - support the new
command and only that one: it's not expected to have devices with the
older versions.
This commit is contained in:
Lucas De Marchi 2019-04-30 01:29:12 -07:00 committed by Lucas De Marchi
parent b3a8496613
commit 4f73881c4b
1 changed files with 17 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMiniPlus.cpp
View File

@ -99,6 +99,12 @@ bool AP_RangeFinder_Benewake_TFMiniPlus::init()
goto fail; goto fail;
} }
if (val[5] * 10000 + val[4] * 100 + val[3] < 10706) {
hal.console->printf(DRIVER ": minimum required FW version 1.7.6, but version %u.%u.%u found\n",
val[5], val[4], val[3]);
goto fail;
}
hal.console->printf(DRIVER ": found fw version %u.%u.%u\n", hal.console->printf(DRIVER ": found fw version %u.%u.%u\n",
val[5], val[4], val[3]); val[5], val[4], val[3]);
@ -143,12 +149,12 @@ void AP_RangeFinder_Benewake_TFMiniPlus::update()
} }
} }
bool AP_RangeFinder_Benewake_TFMiniPlus::process_raw_measure(be16_t distance_raw, be16_t strength_raw, bool AP_RangeFinder_Benewake_TFMiniPlus::process_raw_measure(le16_t distance_raw, le16_t strength_raw,
uint16_t &output_distance_cm) uint16_t &output_distance_cm)
{ {
uint16_t strength = be16toh(strength_raw); uint16_t strength = le16toh(strength_raw);
output_distance_cm = be16toh(distance_raw); output_distance_cm = le16toh(distance_raw);
if (strength < 100 || strength == 0xFFFF) { if (strength < 100 || strength == 0xFFFF) {
return false; return false;
@ -176,15 +182,14 @@ void AP_RangeFinder_Benewake_TFMiniPlus::timer()
uint8_t CMD_READ_MEASUREMENT[] = { 0x5A, 0x05, 0x00, 0x07, 0x66 }; uint8_t CMD_READ_MEASUREMENT[] = { 0x5A, 0x05, 0x00, 0x07, 0x66 };
union { union {
struct PACKED { struct PACKED {
uint8_t header; uint8_t header1;
uint8_t len; uint8_t header2;
be16_t distance; le16_t distance;
be16_t strength; le16_t strength;
uint8_t reserved1; le32_t timestamp;
le16_t timestamp;
uint8_t checksum; uint8_t checksum;
} val; } val;
uint8_t arr[10]; uint8_t arr[11];
} u; } u;
bool ret; bool ret;
uint16_t distance; uint16_t distance;
@ -194,7 +199,7 @@ void AP_RangeFinder_Benewake_TFMiniPlus::timer()
return; return;
} }
if (u.val.header != 0x5a || u.val.len != 0x0a || !check_checksum(u.arr, sizeof(u))) if (u.val.header1 != 0x59 || u.val.header2 != 0x59 || !check_checksum(u.arr, sizeof(u)))
return; return;
if (process_raw_measure(u.val.distance, u.val.strength, distance)) { if (process_raw_measure(u.val.distance, u.val.strength, distance)) {