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

AP_RangeFinder: Add TF03-180

Browse Source
This commit is contained in:
murata 2019-10-14 18:30:48 +09:00 committed by Randy Mackay
parent 6588c93e44
commit 000aa4f515
5 changed files with 26 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp
View File

@ -26,6 +26,7 @@ extern const AP_HAL::HAL& hal;
#define BENEWAKE_DIST_MAX_CM 32768
#define BENEWAKE_TFMINI_OUT_OF_RANGE_CM 1200
#define BENEWAKE_TF02_OUT_OF_RANGE_CM 2200
#define BENEWAKE_TF03_OUT_OF_RANGE_CM 18000
#define BENEWAKE_OUT_OF_RANGE_ADD_CM 100
// format of serial packets received from benewake lidar
@ -37,9 +38,12 @@ extern const AP_HAL::HAL& hal;
// byte 2 DIST_L Distance (in cm) low 8 bits
// byte 3 DIST_H Distance (in cm) high 8 bits
// byte 4 STRENGTH_L Strength low 8 bits
// bute 4 (TF03) (Reserved)
// byte 5 STRENGTH_H Strength high 8 bits
// bute 5 (TF03) (Reserved)
// byte 6 (TF02) SIG Reliability in 8 levels, 7 & 8 means reliable
// byte 6 (TFmini) Distance Mode 0x02 for short distance (mm), 0x07 for long distance (cm)
// byte 6 (TF03) (Reserved)
// byte 7 (TF02 only) TIME Exposure time in two levels 0x03 and 0x06
// byte 8 Checksum Checksum byte, sum of bytes 0 to bytes 7
@ -121,7 +125,7 @@ bool AP_RangeFinder_Benewake::get_reading(uint16_t &reading_cm)
if (dist >= BENEWAKE_DIST_MAX_CM) {
// this reading is out of range
count_out_of_range++;
} else if (model_type == BENEWAKE_TFmini) {
} else if (model_type == BENEWAKE_TFmini || model_type == BENEWAKE_TF03) {
// no signal byte from TFmini so add distance to sum
sum_cm += dist;
count++;
@ -152,7 +156,18 @@ bool AP_RangeFinder_Benewake::get_reading(uint16_t &reading_cm)
if (count_out_of_range > 0) {
// if only out of range readings return larger of
// 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;
float model_dist_max_cm = 0.0f;
switch (model_type) {
case BENEWAKE_TFmini:
model_dist_max_cm = BENEWAKE_TFMINI_OUT_OF_RANGE_CM;
break;
case BENEWAKE_TF02:
model_dist_max_cm = BENEWAKE_TF02_OUT_OF_RANGE_CM;
break;
case BENEWAKE_TF03:
model_dist_max_cm = BENEWAKE_TF03_OUT_OF_RANGE_CM;
break;
}
reading_cm = MAX(model_dist_max_cm, max_distance_cm() + BENEWAKE_OUT_OF_RANGE_ADD_CM);
return true;
}

3
libraries/AP_RangeFinder/AP_RangeFinder_Benewake.h
View File

@ -10,7 +10,8 @@ public:
enum benewake_model_type {
BENEWAKE_TF02 = 0,
BENEWAKE_TFmini = 1
BENEWAKE_TFmini = 1,
BENEWAKE_TF03 = 2,
};
// constructor

2
libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp
View File

@ -6,7 +6,7 @@ const AP_Param::GroupInfo AP_RangeFinder_Params::var_info[] = {
// @Param: TYPE
// @DisplayName: Rangefinder type
// @Description: What type of rangefinder device that is connected
// @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLiteV2-I2C,5:PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:BenewakeTFmini,21:LidarLightV3HP,22:PWM,23:BlueRoboticsPing,24:UAVCAN,25:BenewakeTFMiniPlus
// @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLiteV2-I2C,5:PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:BenewakeTFmini,21:LidarLightV3HP,22:PWM,23:BlueRoboticsPing,24:UAVCAN,25:BenewakeTFMiniPlus,27:BenewakeTF03
// @User: Standard
AP_GROUPINFO("TYPE", 1, AP_RangeFinder_Params, type, 0),

5
libraries/AP_RangeFinder/RangeFinder.cpp
View File

@ -503,6 +503,11 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
drivers[instance] = new AP_RangeFinder_Benewake(state[instance], params[instance], serial_instance++, AP_RangeFinder_Benewake::BENEWAKE_TFmini);
}
break;
case RangeFinder_TYPE_BenewakeTF03:
if (AP_RangeFinder_Benewake::detect(serial_instance)) {
drivers[instance] = new AP_RangeFinder_Benewake(state[instance], params[instance], serial_instance++, AP_RangeFinder_Benewake::BENEWAKE_TF03);
}
break;
case RangeFinder_TYPE_PWM:
if (AP_RangeFinder_PWM::detect()) {
drivers[instance] = new AP_RangeFinder_PWM(state[instance], params[instance], estimated_terrain_height);

1
libraries/AP_RangeFinder/RangeFinder.h
View File

@ -73,6 +73,7 @@ public:
RangeFinder_TYPE_UAVCAN = 24,
RangeFinder_TYPE_BenewakeTFminiPlus = 25,
RangeFinder_TYPE_Lanbao = 26,
RangeFinder_TYPE_BenewakeTF03 = 27,
};
enum RangeFinder_Function {