From 4422296b95a73702e542579071dbe4c70de52d70 Mon Sep 17 00:00:00 2001 From: rishabsingh3003 Date: Mon, 19 Jun 2023 22:39:46 +0530 Subject: [PATCH] AP_RangeFinder: Add TOFSenseP CAN Rangefinder --- libraries/AP_RangeFinder/AP_RangeFinder.cpp | 30 +++--- libraries/AP_RangeFinder/AP_RangeFinder.h | 1 + .../AP_RangeFinder_Backend_CAN.cpp | 96 +++++++++++++++++++ .../AP_RangeFinder_Backend_CAN.h | 78 +++++++++++++++ .../AP_RangeFinder_Benewake_CAN.cpp | 79 ++------------- .../AP_RangeFinder_Benewake_CAN.h | 48 +--------- .../AP_RangeFinder/AP_RangeFinder_Params.cpp | 2 +- .../AP_RangeFinder_TOFSenseP_CAN.cpp | 58 +++++++++++ .../AP_RangeFinder_TOFSenseP_CAN.h | 25 +++++ .../AP_RangeFinder_USD1_CAN.cpp | 62 ++---------- .../AP_RangeFinder/AP_RangeFinder_USD1_CAN.h | 40 +------- 11 files changed, 300 insertions(+), 219 deletions(-) create mode 100644 libraries/AP_RangeFinder/AP_RangeFinder_Backend_CAN.cpp create mode 100644 libraries/AP_RangeFinder/AP_RangeFinder_Backend_CAN.h create mode 100644 libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseP_CAN.cpp create mode 100644 libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseP_CAN.h diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.cpp b/libraries/AP_RangeFinder/AP_RangeFinder.cpp index 7bdc554743..98c90cc702 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder.cpp @@ -53,6 +53,7 @@ #include "AP_RangeFinder_Benewake_CAN.h" #include "AP_RangeFinder_Lua.h" #include "AP_RangeFinder_NoopLoop.h" +#include "AP_RangeFinder_TOFSenseP_CAN.h" #include #include @@ -71,7 +72,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,AP_RangeFinder_Benewake_CAN.cpp,AP_RangeFinder_USD1_CAN.cpp + // @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Backend_CAN.cpp AP_SUBGROUPVARPTR(drivers[0], "1_", 57, RangeFinder, backend_var_info[0]), #if RANGEFINDER_MAX_INSTANCES > 1 @@ -80,7 +81,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,AP_RangeFinder_Benewake_CAN.cpp,AP_RangeFinder_USD1_CAN.cpp + // @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Backend_CAN.cpp AP_SUBGROUPVARPTR(drivers[1], "2_", 58, RangeFinder, backend_var_info[1]), #endif @@ -90,7 +91,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,AP_RangeFinder_Benewake_CAN.cpp,AP_RangeFinder_USD1_CAN.cpp + // @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Backend_CAN.cpp AP_SUBGROUPVARPTR(drivers[2], "3_", 59, RangeFinder, backend_var_info[2]), #endif @@ -100,7 +101,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,AP_RangeFinder_Benewake_CAN.cpp,AP_RangeFinder_USD1_CAN.cpp + // @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Backend_CAN.cpp AP_SUBGROUPVARPTR(drivers[3], "4_", 60, RangeFinder, backend_var_info[3]), #endif @@ -110,7 +111,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,AP_RangeFinder_Benewake_CAN.cpp,AP_RangeFinder_USD1_CAN.cpp + // @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Backend_CAN.cpp AP_SUBGROUPVARPTR(drivers[4], "5_", 34, RangeFinder, backend_var_info[4]), #endif @@ -120,7 +121,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,AP_RangeFinder_Benewake_CAN.cpp,AP_RangeFinder_USD1_CAN.cpp + // @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Backend_CAN.cpp AP_SUBGROUPVARPTR(drivers[5], "6_", 36, RangeFinder, backend_var_info[5]), #endif @@ -130,7 +131,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,AP_RangeFinder_Benewake_CAN.cpp,AP_RangeFinder_USD1_CAN.cpp + // @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Backend_CAN.cpp AP_SUBGROUPVARPTR(drivers[6], "7_", 38, RangeFinder, backend_var_info[6]), #endif @@ -140,7 +141,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,AP_RangeFinder_Benewake_CAN.cpp,AP_RangeFinder_USD1_CAN.cpp + // @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Backend_CAN.cpp AP_SUBGROUPVARPTR(drivers[7], "8_", 40, RangeFinder, backend_var_info[7]), #endif @@ -150,7 +151,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,AP_RangeFinder_Benewake_CAN.cpp,AP_RangeFinder_USD1_CAN.cpp + // @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Backend_CAN.cpp AP_SUBGROUPVARPTR(drivers[8], "9_", 42, RangeFinder, backend_var_info[8]), #endif @@ -160,7 +161,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,AP_RangeFinder_Benewake_CAN.cpp,AP_RangeFinder_USD1_CAN.cpp + // @Path: AP_RangeFinder_Wasp.cpp,AP_RangeFinder_Backend_CAN.cpp AP_SUBGROUPVARPTR(drivers[9], "A_", 44, RangeFinder, backend_var_info[9]), #endif @@ -537,12 +538,19 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) _add_backend(new AP_RangeFinder_Lua(state[instance], params[instance]), instance); #endif break; + case Type::NoopLoop_P: #if AP_RANGEFINDER_NOOPLOOP_ENABLED serial_create_fn = AP_RangeFinder_NoopLoop::create; #endif break; + case Type::TOFSenseP_CAN: +#if AP_RANGEFINDER_TOFSENSEP_CAN_ENABLED + _add_backend(new AP_RangeFinder_TOFSenseP_CAN(state[instance], params[instance]), instance); +#endif + break; + case Type::NONE: break; } @@ -805,7 +813,7 @@ bool RangeFinder::prearm_healthy(char *failure_msg, const uint8_t failure_msg_le } break; } - default: +default: break; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.h b/libraries/AP_RangeFinder/AP_RangeFinder.h index 7d3b252feb..3c0859924c 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder.h @@ -94,6 +94,7 @@ public: TeraRanger_Serial = 35, Lua_Scripting = 36, NoopLoop_P = 37, + TOFSenseP_CAN = 38, SIM = 100, }; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Backend_CAN.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Backend_CAN.cpp new file mode 100644 index 0000000000..e3abf795b7 --- /dev/null +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Backend_CAN.cpp @@ -0,0 +1,96 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#include +#include "AP_RangeFinder_Backend_CAN.h" + +#if HAL_MAX_CAN_PROTOCOL_DRIVERS + +const AP_Param::GroupInfo AP_RangeFinder_Backend_CAN::var_info[] = { + + // @Param: RECV_ID + // @DisplayName: RangeFinder 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_Backend_CAN, receive_id, 0), + + // @Param: SNR_MIN + // @DisplayName: RangeFinder Minimum signal strength + // @Description: RangeFinder Minimum signal strength (SNR) to accept distance + // @Range: 0 65535 + // @User: Advanced + AP_GROUPINFO("SNR_MIN", 11, AP_RangeFinder_Backend_CAN, snr_min, 0), + + AP_GROUPEND +}; + +// constructor +AP_RangeFinder_Backend_CAN::AP_RangeFinder_Backend_CAN( + RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) : + AP_RangeFinder_Backend(_state, _params) +{ + AP_Param::setup_object_defaults(this, var_info); + state.var_info = var_info; +} + +// update the state of the sensor +void AP_RangeFinder_Backend_CAN::update(void) +{ + if (get_reading(state.distance_m)) { + // update range_valid state based on distance measured + state.last_reading_ms = AP_HAL::millis(); + update_status(); + } else if (AP_HAL::millis() - state.last_reading_ms >= read_timeout_ms()) { + set_status(RangeFinder::Status::NoData); + } +} + +// get distance measurement +bool AP_RangeFinder_Backend_CAN::get_reading(float &reading_m) +{ + WITH_SEMAPHORE(_sem); + if (_distance_count != 0) { + reading_m = _distance_sum / _distance_count; + _distance_sum = 0; + _distance_count = 0; + return true; + } + + return false; +} + +// return true if the CAN ID is correct +bool AP_RangeFinder_Backend_CAN::is_correct_id(uint32_t id) const +{ + if (receive_id != 0 && id != uint32_t(receive_id.get())) { + // incorrect receive ID + return false; + } + return true; +} + +// handle frames from CANSensor, passing to the drivers +void RangeFinder_MultiCAN::handle_frame(AP_HAL::CANFrame &frame) +{ + WITH_SEMAPHORE(sem); + for (auto *d = drivers; d; d=d->next) { + if (d->handle_frame(frame)) { + break; + } + } +} + +#endif // HAL_MAX_CAN_PROTOCOL_DRIVERS diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Backend_CAN.h b/libraries/AP_RangeFinder/AP_RangeFinder_Backend_CAN.h new file mode 100644 index 0000000000..6e71afbd4e --- /dev/null +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Backend_CAN.h @@ -0,0 +1,78 @@ +#pragma once + +#include "AP_RangeFinder_Backend.h" + +#if HAL_MAX_CAN_PROTOCOL_DRIVERS + +#include +#include + +class RangeFinder_MultiCAN; + +class AP_RangeFinder_Backend_CAN : public AP_RangeFinder_Backend +{ +public: + // constructor + AP_RangeFinder_Backend_CAN(RangeFinder::RangeFinder_State &_state, + AP_RangeFinder_Params &_params); + + friend class RangeFinder_MultiCAN; + + static const struct AP_Param::GroupInfo var_info[]; + +protected: + + // update state + virtual void update(void) override; + + // get distance measurement + bool get_reading(float &reading_m); + + // it is essential that anyone relying on the base-class update to implement this + virtual bool handle_frame(AP_HAL::CANFrame &frame) = 0; + + // maximum time between readings before we change state to NoData: + virtual uint16_t read_timeout_ms() const { return 200; } + + virtual MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override { + return MAV_DISTANCE_SENSOR_RADAR; + } + + // return true if the CAN ID is correct + bool is_correct_id(uint32_t can_id) const; + + // set distance and count + void set_distance_m(float distance_m) { + _distance_sum += distance_m; + _distance_count++; + }; + + // linked list + AP_RangeFinder_Backend_CAN *next; + + AP_Int32 receive_id; // CAN ID to receive for this backend + AP_Int32 snr_min; // minimum signal strength to accept packet + +private: + + float _distance_sum; // meters + uint32_t _distance_count; +}; + +// a class to allow for multiple CAN backends with one +// CANSensor driver +class RangeFinder_MultiCAN : public CANSensor { +public: + RangeFinder_MultiCAN(AP_CAN::Protocol can_type, const char *driver_name) : CANSensor(driver_name) { + register_driver(can_type); + } + + // handler for incoming frames + void handle_frame(AP_HAL::CANFrame &frame) override; + + // Semaphore for access to shared backend data + HAL_Semaphore sem; + AP_RangeFinder_Backend_CAN *drivers; +}; + +#endif // HAL_MAX_CAN_PROTOCOL_DRIVERS diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.cpp index 40c8118ec6..53c2d371fc 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.cpp @@ -5,35 +5,16 @@ #if AP_RANGEFINDER_BENEWAKE_CAN_ENABLED -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 -}; - -Benewake_MultiCAN *AP_RangeFinder_Benewake_CAN::multican; +RangeFinder_MultiCAN *AP_RangeFinder_Benewake_CAN::multican; /* constructor */ AP_RangeFinder_Benewake_CAN::AP_RangeFinder_Benewake_CAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) : - AP_RangeFinder_Backend(_state, _params) + AP_RangeFinder_Backend_CAN(_state, _params) { if (multican == nullptr) { - multican = new Benewake_MultiCAN(); + multican = new RangeFinder_MultiCAN(AP_CAN::Protocol::Benewake, "Benewake MultiCAN"); if (multican == nullptr) { AP_BoardConfig::allocation_error("Benewake_CAN"); } @@ -46,26 +27,6 @@ AP_RangeFinder_Benewake_CAN::AP_RangeFinder_Benewake_CAN(RangeFinder::RangeFinde next = prev; multican->drivers = this; } - - AP_Param::setup_object_defaults(this, var_info); - 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_m = 0.01 * (_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 for H30 radar @@ -83,8 +44,7 @@ bool AP_RangeFinder_Benewake_CAN::handle_frame_H30(AP_HAL::CANFrame &frame) //uint16_t target2 = be16toh_ptr(&frame.data[2]); //uint16_t target3 = be16toh_ptr(&frame.data[4]); - _distance_sum_cm += target1_cm; - _distance_count++; + set_distance_m(target1_cm * 0.01); return true; } @@ -96,28 +56,16 @@ bool AP_RangeFinder_Benewake_CAN::handle_frame(AP_HAL::CANFrame &frame) if (frame.isExtended()) { // H30 radar uses extended frames const int32_t id = int32_t(frame.id & AP_HAL::CANFrame::MaskExtID); - if (receive_id != 0 && id != receive_id.get()) { - // incorrect receive ID + if (!is_correct_id(id)) { return false; } - if (last_recv_id != -1 && id != last_recv_id) { - // changing ID - return false; - } - last_recv_id = id; return handle_frame_H30(frame); } const uint16_t id = frame.id & AP_HAL::CANFrame::MaskStdID; - if (receive_id != 0 && id != uint16_t(receive_id.get())) { - // incorrect receive ID + if (!is_correct_id(id)) { return false; } - if (last_recv_id != -1 && id != last_recv_id) { - // changing ID - return false; - } - last_recv_id = id; const uint16_t dist_cm = le16toh_ptr(&frame.data[0]); const uint16_t snr = le16toh_ptr(&frame.data[2]); @@ -125,20 +73,9 @@ bool AP_RangeFinder_Benewake_CAN::handle_frame(AP_HAL::CANFrame &frame) // too low signal strength return true; } - _distance_sum_cm += dist_cm; - _distance_count++; + + set_distance_m(dist_cm * 0.01); return true; } -// handle frames from CANSensor, passing to the drivers -void Benewake_MultiCAN::handle_frame(AP_HAL::CANFrame &frame) -{ - WITH_SEMAPHORE(sem); - for (auto *d = drivers; d; d=d->next) { - if (d->handle_frame(frame)) { - break; - } - } -} - #endif // AP_RANGEFINDER_BENEWAKE_CAN_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.h b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.h index 7830ab99b0..6bc7057bed 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.h @@ -3,58 +3,18 @@ #include "AP_RangeFinder_config.h" #if AP_RANGEFINDER_BENEWAKE_CAN_ENABLED +#include "AP_RangeFinder_Backend_CAN.h" -#include "AP_RangeFinder_Backend.h" -#include - -class Benewake_MultiCAN; - -class AP_RangeFinder_Benewake_CAN : public AP_RangeFinder_Backend { +class AP_RangeFinder_Benewake_CAN : public AP_RangeFinder_Backend_CAN { public: - friend class Benewake_MultiCAN; - AP_RangeFinder_Benewake_CAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params); - void update() override; - // handler for incoming frames. Return true if consumed - bool handle_frame(AP_HAL::CANFrame &frame); + bool handle_frame(AP_HAL::CANFrame &frame) override; bool handle_frame_H30(AP_HAL::CANFrame &frame); - 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; - - static Benewake_MultiCAN *multican; - AP_RangeFinder_Benewake_CAN *next; -}; - -// a class to allow for multiple Benewake_CAN backends with one -// CANSensor driver -class Benewake_MultiCAN : public CANSensor { -public: - Benewake_MultiCAN() : CANSensor("Benewake") { - register_driver(AP_CAN::Protocol::Benewake); - } - - // handler for incoming frames - void handle_frame(AP_HAL::CANFrame &frame) override; - - HAL_Semaphore sem; - AP_RangeFinder_Benewake_CAN *drivers; + static RangeFinder_MultiCAN *multican; }; #endif // AP_RANGEFINDER_BENEWAKE_CAN_ENABLED - - diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp index 34fc046004..e9ae291e3d 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp @@ -10,7 +10,7 @@ const AP_Param::GroupInfo AP_RangeFinder_Params::var_info[] = { // @Param: TYPE // @DisplayName: Rangefinder type // @Description: Type of connected rangefinder - // @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLite-I2C,5:PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:USD1_Serial,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:DroneCAN,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,35:TeraRangerSerial,36:Lua_Scripting,37:NoopLoop_TOFSense, 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:USD1_Serial,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:DroneCAN,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,35:TeraRangerSerial,36:Lua_Scripting,37:NoopLoop_TOFSense, 38:NoopLoop_TOFSense_CAN, 100:SITL // @User: Standard AP_GROUPINFO_FLAGS("TYPE", 1, AP_RangeFinder_Params, type, 0, AP_PARAM_FLAG_ENABLE), diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseP_CAN.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseP_CAN.cpp new file mode 100644 index 0000000000..17e43169af --- /dev/null +++ b/libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseP_CAN.cpp @@ -0,0 +1,58 @@ +#include "AP_RangeFinder_TOFSenseP_CAN.h" +#include +#include +#include + +#if AP_RANGEFINDER_TOFSENSEP_CAN_ENABLED + +#include + +RangeFinder_MultiCAN *AP_RangeFinder_TOFSenseP_CAN::multican_TOFSenseP; + +/* + constructor + */ +AP_RangeFinder_TOFSenseP_CAN::AP_RangeFinder_TOFSenseP_CAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) : + AP_RangeFinder_Backend_CAN(_state, _params) +{ + if (multican_TOFSenseP == nullptr) { + multican_TOFSenseP = new RangeFinder_MultiCAN(AP_CAN::Protocol::TOFSenseP, "TOFSenseP MultiCAN"); + if (multican_TOFSenseP == nullptr) { + AP_BoardConfig::allocation_error("Rangefinder_MultiCAN"); + } + } + + { + // add to linked list of drivers + WITH_SEMAPHORE(multican_TOFSenseP->sem); + auto *prev = multican_TOFSenseP->drivers; + next = prev; + multican_TOFSenseP->drivers = this; + } +} + + +// handler for incoming frames. These come in at 10-30Hz +bool AP_RangeFinder_TOFSenseP_CAN::handle_frame(AP_HAL::CANFrame &frame) +{ + WITH_SEMAPHORE(_sem); + const uint32_t id = frame.id - 0x200U; + + if (!is_correct_id(id)) { + return false; + } + + const int32_t dist_cm = (int32_t)(frame.data[0] << 8U | frame.data[1] << 16U | frame.data[2] << 24U) / 2560; + const uint8_t status = frame.data[3]; + const uint16_t snr = le16toh_ptr(&frame.data[4]); + + if ((snr_min != 0 && snr < uint16_t(snr_min.get())) || status > 0) { + // too low signal strength or bad status + return false; + } + + set_distance_m(dist_cm * 0.01); + return true; +} + +#endif // AP_RANGEFINDER_TOFSenseP_CAN_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseP_CAN.h b/libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseP_CAN.h new file mode 100644 index 0000000000..a08c479fd5 --- /dev/null +++ b/libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseP_CAN.h @@ -0,0 +1,25 @@ +#pragma once + +#include "AP_RangeFinder_Backend_CAN.h" + +#ifndef AP_RANGEFINDER_TOFSENSEP_CAN_ENABLED +#define AP_RANGEFINDER_TOFSENSEP_CAN_ENABLED (HAL_MAX_CAN_PROTOCOL_DRIVERS && AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED) +#endif + +#if AP_RANGEFINDER_TOFSENSEP_CAN_ENABLED + +class AP_RangeFinder_TOFSenseP_CAN : public AP_RangeFinder_Backend_CAN { +public: + AP_RangeFinder_TOFSenseP_CAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params); + + // handler for incoming frames + bool handle_frame(AP_HAL::CANFrame &frame) override; + + static const struct AP_Param::GroupInfo var_info[]; + +private: + static RangeFinder_MultiCAN *multican_TOFSenseP; + +}; + +#endif // AP_RANGEFINDER_USD1_CAN_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.cpp index ac7dcda430..4ad14de2e7 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.cpp @@ -1,32 +1,19 @@ #include "AP_RangeFinder_USD1_CAN.h" -#include #if AP_RANGEFINDER_USD1_CAN_ENABLED #include -const AP_Param::GroupInfo AP_RangeFinder_USD1_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", 12, AP_RangeFinder_USD1_CAN, receive_id, 0), - - AP_GROUPEND -}; - -USD1_MultiCAN *AP_RangeFinder_USD1_CAN::multican; +RangeFinder_MultiCAN *AP_RangeFinder_USD1_CAN::multican; /* constructor */ AP_RangeFinder_USD1_CAN::AP_RangeFinder_USD1_CAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) : - AP_RangeFinder_Backend(_state, _params) + AP_RangeFinder_Backend_CAN(_state, _params) { if (multican == nullptr) { - multican = new USD1_MultiCAN(); + multican = new RangeFinder_MultiCAN(AP_CAN::Protocol::USD1, "USD1 MultiCAN"); if (multican == nullptr) { AP_BoardConfig::allocation_error("USD1_CAN"); } @@ -39,26 +26,6 @@ AP_RangeFinder_USD1_CAN::AP_RangeFinder_USD1_CAN(RangeFinder::RangeFinder_State next = prev; multican->drivers = this; } - - AP_Param::setup_object_defaults(this, var_info); - state.var_info = var_info; -} - -// update state -void AP_RangeFinder_USD1_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_m = _distance_sum / _distance_count; - state.last_reading_ms = AP_HAL::millis(); - _distance_sum = 0; - _distance_count = 0; - update_status(); - } } // handler for incoming frames. These come in at 100Hz @@ -66,31 +33,14 @@ bool AP_RangeFinder_USD1_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 + + if (!is_correct_id(id)) { return false; } - if (last_recv_id != -1 && id != last_recv_id) { - // changing ID - return false; - } - last_recv_id = id; const uint16_t dist_cm = (frame.data[0]<<8) | frame.data[1]; - _distance_sum += dist_cm * 0.01; - _distance_count++; + set_distance_m(dist_cm * 0.01); return true; } -// handle frames from CANSensor, passing to the drivers -void USD1_MultiCAN::handle_frame(AP_HAL::CANFrame &frame) -{ - WITH_SEMAPHORE(sem); - for (auto *d = drivers; d; d=d->next) { - if (d->handle_frame(frame)) { - break; - } - } -} - #endif // AP_RANGEFINDER_USD1_CAN_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.h b/libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.h index 4ca6890fbe..5a3827585a 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.h @@ -3,54 +3,22 @@ #include "AP_RangeFinder_config.h" #if AP_RANGEFINDER_USD1_CAN_ENABLED +#include "AP_RangeFinder_Backend_CAN.h" -#include "AP_RangeFinder_Backend.h" -#include - -class USD1_MultiCAN; - -class AP_RangeFinder_USD1_CAN : public AP_RangeFinder_Backend { +class AP_RangeFinder_USD1_CAN : public AP_RangeFinder_Backend_CAN { public: - friend class USD1_MultiCAN; AP_RangeFinder_USD1_CAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params); - void update() override; - // handler for incoming frames - bool handle_frame(AP_HAL::CANFrame &frame); + bool 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; - uint32_t _distance_count; - int32_t last_recv_id = -1; - AP_Int32 receive_id; + static RangeFinder_MultiCAN *multican; - static USD1_MultiCAN *multican; - AP_RangeFinder_USD1_CAN *next; -}; - -// a class to allow for multiple USD1_CAN backends with one -// CANSensor driver -class USD1_MultiCAN : public CANSensor { -public: - USD1_MultiCAN() : CANSensor("USD1") { - register_driver(AP_CAN::Protocol::USD1); - } - - // handler for incoming frames - void handle_frame(AP_HAL::CANFrame &frame) override; - - HAL_Semaphore sem; - AP_RangeFinder_USD1_CAN *drivers; }; #endif // AP_RANGEFINDER_USD1_CAN_ENABLED