diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.cpp b/libraries/AP_RangeFinder/AP_RangeFinder.cpp index 7ffb3779b5..52eff01054 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder.cpp @@ -48,6 +48,7 @@ #include "AP_RangeFinder_SITL.h" #include "AP_RangeFinder_MSP.h" #include "AP_RangeFinder_USD1_CAN.h" +#include "AP_RangeFinder_Benewake_CAN.h" #include #include @@ -63,7 +64,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = { AP_SUBGROUPINFO(params[0], "1_", 25, RangeFinder, AP_RangeFinder_Params), // @Group: 1_ - // @Path: AP_RangeFinder_Wasp.cpp + // @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Benewake_CAN.cpp AP_SUBGROUPVARPTR(drivers[0], "1_", 57, RangeFinder, backend_var_info[0]), #if RANGEFINDER_MAX_INSTANCES > 1 @@ -72,7 +73,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = { AP_SUBGROUPINFO(params[1], "2_", 27, RangeFinder, AP_RangeFinder_Params), // @Group: 2_ - // @Path: AP_RangeFinder_Wasp.cpp + // @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Benewake_CAN.cpp AP_SUBGROUPVARPTR(drivers[1], "2_", 58, RangeFinder, backend_var_info[1]), #endif @@ -82,7 +83,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = { AP_SUBGROUPINFO(params[2], "3_", 29, RangeFinder, AP_RangeFinder_Params), // @Group: 3_ - // @Path: AP_RangeFinder_Wasp.cpp + // @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Benewake_CAN.cpp AP_SUBGROUPVARPTR(drivers[2], "3_", 59, RangeFinder, backend_var_info[2]), #endif @@ -92,7 +93,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = { AP_SUBGROUPINFO(params[3], "4_", 31, RangeFinder, AP_RangeFinder_Params), // @Group: 4_ - // @Path: AP_RangeFinder_Wasp.cpp + // @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Benewake_CAN.cpp AP_SUBGROUPVARPTR(drivers[3], "4_", 60, RangeFinder, backend_var_info[3]), #endif @@ -102,7 +103,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = { AP_SUBGROUPINFO(params[4], "5_", 33, RangeFinder, AP_RangeFinder_Params), // @Group: 5_ - // @Path: AP_RangeFinder_Wasp.cpp + // @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Benewake_CAN.cpp AP_SUBGROUPVARPTR(drivers[4], "5_", 34, RangeFinder, backend_var_info[4]), #endif @@ -112,7 +113,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = { AP_SUBGROUPINFO(params[5], "6_", 35, RangeFinder, AP_RangeFinder_Params), // @Group: 6_ - // @Path: AP_RangeFinder_Wasp.cpp + // @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Benewake_CAN.cpp AP_SUBGROUPVARPTR(drivers[5], "6_", 36, RangeFinder, backend_var_info[5]), #endif @@ -122,7 +123,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = { AP_SUBGROUPINFO(params[6], "7_", 37, RangeFinder, AP_RangeFinder_Params), // @Group: 7_ - // @Path: AP_RangeFinder_Wasp.cpp + // @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Benewake_CAN.cpp AP_SUBGROUPVARPTR(drivers[6], "7_", 38, RangeFinder, backend_var_info[6]), #endif @@ -132,7 +133,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = { AP_SUBGROUPINFO(params[7], "8_", 39, RangeFinder, AP_RangeFinder_Params), // @Group: 8_ - // @Path: AP_RangeFinder_Wasp.cpp + // @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Benewake_CAN.cpp AP_SUBGROUPVARPTR(drivers[7], "8_", 40, RangeFinder, backend_var_info[7]), #endif @@ -142,7 +143,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = { AP_SUBGROUPINFO(params[8], "9_", 41, RangeFinder, AP_RangeFinder_Params), // @Group: 9_ - // @Path: AP_RangeFinder_Wasp.cpp + // @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Benewake_CAN.cpp AP_SUBGROUPVARPTR(drivers[8], "9_", 42, RangeFinder, backend_var_info[8]), #endif @@ -152,7 +153,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = { AP_SUBGROUPINFO(params[9], "A_", 43, RangeFinder, AP_RangeFinder_Params), // @Group: A_ - // @Path: AP_RangeFinder_Wasp.cpp + // @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Benewake_CAN.cpp AP_SUBGROUPVARPTR(drivers[9], "A_", 44, RangeFinder, backend_var_info[9]), #endif @@ -574,6 +575,9 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) case Type::USD1_CAN: _add_backend(new AP_RangeFinder_USD1_CAN(state[instance], params[instance]), instance); break; + case Type::Benewake_CAN: + _add_backend(new AP_RangeFinder_Benewake_CAN(state[instance], params[instance]), instance); + break; #endif case Type::NONE: default: diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.h b/libraries/AP_RangeFinder/AP_RangeFinder.h index b1cd63545e..cce539d1fb 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder.h @@ -88,6 +88,7 @@ public: GYUS42v2 = 31, MSP = 32, USD1_CAN = 33, + Benewake_CAN = 34, SITL = 100, }; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.cpp new file mode 100644 index 0000000000..2d4b76fccf --- /dev/null +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.cpp @@ -0,0 +1,79 @@ +#include +#include "AP_RangeFinder_Benewake_CAN.h" + +#if HAL_MAX_CAN_PROTOCOL_DRIVERS + +const AP_Param::GroupInfo AP_RangeFinder_Benewake_CAN::var_info[] = { + + // @Param: RECV_ID + // @DisplayName: CAN receive ID + // @Description: The receive ID of the CAN frames. A value of zero means all IDs are accepted. + // @Range: 0 65535 + // @User: Advanced + AP_GROUPINFO("RECV_ID", 10, AP_RangeFinder_Benewake_CAN, receive_id, 0), + + // @Param: SNR_MIN + // @DisplayName: Minimum signal strength + // @Description: Minimum signal strength (SNR) to accept distance + // @Range: 0 65535 + // @User: Advanced + AP_GROUPINFO("SNR_MIN", 11, AP_RangeFinder_Benewake_CAN, snr_min, 0), + + AP_GROUPEND +}; + + +/* + constructor + */ +AP_RangeFinder_Benewake_CAN::AP_RangeFinder_Benewake_CAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) : + CANSensor("Benewake", 3072), + AP_RangeFinder_Backend(_state, _params) +{ + AP_Param::setup_object_defaults(this, var_info); + register_driver(AP_CANManager::Driver_Type_Benewake); + state.var_info = var_info; +} + +// update state +void AP_RangeFinder_Benewake_CAN::update(void) +{ + WITH_SEMAPHORE(_sem); + const uint32_t now = AP_HAL::millis(); + if (_distance_count == 0 && now - state.last_reading_ms > 500) { + // no new data. + set_status(RangeFinder::Status::NoData); + } else if (_distance_count != 0) { + state.distance_cm = (_distance_sum_cm / _distance_count); + state.last_reading_ms = AP_HAL::millis(); + _distance_sum_cm = 0; + _distance_count = 0; + update_status(); + } +} + +// handler for incoming frames. These come in at 100Hz +void AP_RangeFinder_Benewake_CAN::handle_frame(AP_HAL::CANFrame &frame) +{ + WITH_SEMAPHORE(_sem); + const uint16_t id = frame.id & AP_HAL::CANFrame::MaskStdID; + if (receive_id != 0 && id != uint16_t(receive_id.get())) { + // incorrect receive ID + return; + } + if (last_recv_id != -1 && id != last_recv_id) { + // changing ID + return; + } + last_recv_id = id; + const uint16_t dist_cm = (frame.data[1]<<8) | frame.data[0]; + const uint16_t snr = (frame.data[3]<<8) | frame.data[2]; + if (snr_min != 0 && snr < uint16_t(snr_min.get())) { + // too low signal strength + return; + } + _distance_sum_cm += dist_cm; + _distance_count++; +} + +#endif // HAL_MAX_CAN_PROTOCOL_DRIVERS diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.h b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.h new file mode 100644 index 0000000000..a769b772a0 --- /dev/null +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.h @@ -0,0 +1,32 @@ +#pragma once + +#include "AP_RangeFinder_Backend.h" +#include + +#if HAL_MAX_CAN_PROTOCOL_DRIVERS + +class AP_RangeFinder_Benewake_CAN : public CANSensor, public AP_RangeFinder_Backend { +public: + AP_RangeFinder_Benewake_CAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params); + + void update() override; + + // handler for incoming frames + void handle_frame(AP_HAL::CANFrame &frame) override; + + static const struct AP_Param::GroupInfo var_info[]; + +protected: + virtual MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override { + return MAV_DISTANCE_SENSOR_RADAR; + } +private: + float _distance_sum_cm; + uint32_t _distance_count; + int32_t last_recv_id = -1; + + AP_Int32 snr_min; + AP_Int32 receive_id; +}; +#endif //HAL_MAX_CAN_PROTOCOL_DRIVERS + diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp index 504083208b..921098d36e 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp @@ -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:LidarLite-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 or VL53L1X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:Benewake-Serial,21:LidarLightV3HP,22:PWM,23:BlueRoboticsPing,24:UAVCAN,25:BenewakeTFminiPlus-I2C,26:LanbaoPSK-CM8JL65-CC5,27:BenewakeTF03,28:VL53L1X-ShortRange,29:LeddarVu8-Serial,30:HC-SR04,31:GYUS42v2,32:MSP,33:USD1_CAN,100:SITL + // @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLite-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 or VL53L1X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:Benewake-Serial,21:LidarLightV3HP,22:PWM,23:BlueRoboticsPing,24:UAVCAN,25:BenewakeTFminiPlus-I2C,26:LanbaoPSK-CM8JL65-CC5,27:BenewakeTF03,28:VL53L1X-ShortRange,29:LeddarVu8-Serial,30:HC-SR04,31:GYUS42v2,32:MSP,33:USD1_CAN,34:Benewake_CAN,100:SITL // @User: Standard AP_GROUPINFO_FLAGS("TYPE", 1, AP_RangeFinder_Params, type, 0, AP_PARAM_FLAG_ENABLE),