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

AP_RangeFinder: benewake returns at least 12m or 22m when out-of-range 

value logged and shown to the user will be at least 12m for TFmini or 22m for TF02 which ensure the user won't see the range drop even if they have configured a very short useable distance
This commit is contained in:
Randy Mackay 2018-11-14 11:51:54 +09:00
parent bb61b7e5e9
commit 54494a71e6
1 changed files with 6 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp
View File

@ -24,6 +24,8 @@ extern const AP_HAL::HAL& hal;
#define BENEWAKE_FRAME_HEADER 0x59 #define BENEWAKE_FRAME_HEADER 0x59
#define BENEWAKE_FRAME_LENGTH 9 #define BENEWAKE_FRAME_LENGTH 9
#define BENEWAKE_DIST_MAX_CM 32768 #define BENEWAKE_DIST_MAX_CM 32768
#define BENEWAKE_TFMINI_OUT_OF_RANGE_CM 1200
#define BENEWAKE_TF02_OUT_OF_RANGE_CM 2200
#define BENEWAKE_OUT_OF_RANGE_ADD_CM 100 #define BENEWAKE_OUT_OF_RANGE_ADD_CM 100
// format of serial packets received from benewake lidar // format of serial packets received from benewake lidar
@ -147,8 +149,10 @@ bool AP_RangeFinder_Benewake::get_reading(uint16_t &reading_cm)
} }
if (count_out_of_range > 0) { if (count_out_of_range > 0) {
// if only out of range readings return max range + 1m // if only out of range readings return larger of
reading_cm = max_distance_cm() + BENEWAKE_OUT_OF_RANGE_ADD_CM; // driver defined maximum range for the model and user defined max range + 1m
float model_dist_max_cm = (model_type == BENEWAKE_TFmini) ? BENEWAKE_TFMINI_OUT_OF_RANGE_CM : BENEWAKE_TF02_OUT_OF_RANGE_CM;
reading_cm = MAX(model_dist_max_cm, max_distance_cm() + BENEWAKE_OUT_OF_RANGE_ADD_CM);
return true; return true;
} }