Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_RangeFinder: Add TOFSenseP CAN Rangefinder

Browse Source
This commit is contained in:
rishabsingh3003 2023-06-19 22:39:46 +05:30 committed by Andrew Tridgell
parent b1a0b35401
commit 4422296b95
11 changed files with 300 additions and 219 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
libraries/AP_RangeFinder/AP_RangeFinder.cpp
View File

@ -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 <AP_BoardConfig/AP_BoardConfig.h>
#include <AP_Logger/AP_Logger.h>
@ -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;
}

1
libraries/AP_RangeFinder/AP_RangeFinder.h
View File

@ -94,6 +94,7 @@ public:
TeraRanger_Serial = 35,
Lua_Scripting = 36,
NoopLoop_P = 37,
TOFSenseP_CAN = 38,
SIM = 100,
};

96
libraries/AP_RangeFinder/AP_RangeFinder_Backend_CAN.cpp Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
#include <AP_HAL/AP_HAL.h>
#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

78
libraries/AP_RangeFinder/AP_RangeFinder_Backend_CAN.h Normal file
View File

@ -0,0 +1,78 @@
#pragma once
#include "AP_RangeFinder_Backend.h"
#if HAL_MAX_CAN_PROTOCOL_DRIVERS
#include <AP_CANManager/AP_CANSensor.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
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

79
libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.cpp
View File

@ -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

48
libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.h
View File

@ -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 <AP_CANManager/AP_CANSensor.h>
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

2
libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp
View File

@ -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),

58
libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseP_CAN.cpp Normal file
View File

@ -0,0 +1,58 @@
#include "AP_RangeFinder_TOFSenseP_CAN.h"
#include <GCS_MAVLink/GCS.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <AP_HAL/utility/sparse-endian.h>
#if AP_RANGEFINDER_TOFSENSEP_CAN_ENABLED
#include <AP_HAL/AP_HAL.h>
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

25
libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseP_CAN.h Normal file
View File

@ -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

62
libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.cpp
View File

@ -1,32 +1,19 @@
#include "AP_RangeFinder_USD1_CAN.h"
#include <AP_BoardConfig/AP_BoardConfig.h>
#if AP_RANGEFINDER_USD1_CAN_ENABLED
#include <AP_HAL/AP_HAL.h>
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

40
libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.h
View File

@ -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 <AP_CANManager/AP_CANSensor.h>
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