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

RangeFinder: add support for rangefinder sensor over can

This commit is contained in:
Siddharth Purohit 2019-04-08 15:12:46 +05:30 committed by Randy Mackay
parent e866a574dd
commit 479c238177
7 changed files with 228 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
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:PX4-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
// @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLiteV2-I2C,5:PX4-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
// @User: Standard
AP_GROUPINFO("TYPE", 1, AP_RangeFinder_Params, type, 0),
@ -97,7 +97,7 @@ const AP_Param::GroupInfo AP_RangeFinder_Params::var_info[] = {
// @Param: ADDR
// @DisplayName: Bus address of sensor
// @Description: This sets the bus address of the sensor, where applicable. Used for the LightWare I2C sensor to allow for multiple sensors on different addresses. A value of 0 disables the sensor.
// @Description: This sets the bus address of the sensor, where applicable. Used for the LightWare I2C sensor and UAVCAN Range Sensors to allow for multiple sensors on different addresses. A value of 0 disables the sensor.
// @Range: 0 127
// @Increment: 1
// @User: Standard

181
libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.cpp Normal file
View File

@ -0,0 +1,181 @@
#include <AP_HAL/AP_HAL.h>
#if HAL_WITH_UAVCAN
#include "AP_RangeFinder_UAVCAN.h"
#include <AP_BoardConfig/AP_BoardConfig_CAN.h>
#include <AP_UAVCAN/AP_UAVCAN.h>
#include <uavcan/equipment/range_sensor/Measurement.hpp>
extern const AP_HAL::HAL& hal;
#define debug_range_finder_uavcan(level_debug, can_driver, fmt, args...) do { if ((level_debug) <= AP::can().get_debug_level_driver(can_driver)) { hal.console->printf(fmt, ##args); }} while (0)
//UAVCAN Frontend Registry Binder
UC_REGISTRY_BINDER(MeasurementCb, uavcan::equipment::range_sensor::Measurement);
/*
constructor - registers instance at top RangeFinder driver
*/
AP_RangeFinder_UAVCAN::AP_RangeFinder_UAVCAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) :
AP_RangeFinder_Backend(_state, _params)
{}
//links the rangefinder uavcan message to this backend
void AP_RangeFinder_UAVCAN::subscribe_msgs(AP_UAVCAN* ap_uavcan)
{
if (ap_uavcan == nullptr) {
return;
}
auto* node = ap_uavcan->get_node();
uavcan::Subscriber<uavcan::equipment::range_sensor::Measurement, MeasurementCb> *measurement_listener;
measurement_listener = new uavcan::Subscriber<uavcan::equipment::range_sensor::Measurement, MeasurementCb>(*node);
// Register method to handle incoming RangeFinder measurement
const int measurement_listener_res = measurement_listener->start(MeasurementCb(ap_uavcan, &handle_measurement));
if (measurement_listener_res < 0) {
AP_HAL::panic("UAVCAN RangeFinder subscriber start problem\n\r");
return;
}
}
//Method to find the backend relating to the node id
AP_RangeFinder_UAVCAN* AP_RangeFinder_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id, uint8_t address, bool create_new)
{
if (ap_uavcan == nullptr) {
return nullptr;
}
AP_RangeFinder_UAVCAN* driver = nullptr;
//Scan through the Rangefinder params to find UAVCAN RFND with matching address.
for (uint8_t i = 0; i < RANGEFINDER_MAX_INSTANCES; i++) {
if (AP::rangefinder()->params[i].type == RangeFinder::RangeFinder_TYPE_UAVCAN &&
AP::rangefinder()->params[i].address == address) {
driver = (AP_RangeFinder_UAVCAN*)AP::rangefinder()->drivers[i];
}
//Double check if the driver was initialised as UAVCAN Type
if (driver != nullptr && (driver->_backend_type == RangeFinder::RangeFinder_TYPE_UAVCAN)) {
if (driver->_ap_uavcan == ap_uavcan &&
driver->_node_id == node_id) {
return driver;
} else {
//we found a possible duplicate addressed sensor
//we return nothing in such scenario
return nullptr;
}
}
}
if (create_new) {
for (uint8_t i = 0; i < RANGEFINDER_MAX_INSTANCES; i++) {
if (AP::rangefinder()->params[i].type == RangeFinder::RangeFinder_TYPE_UAVCAN &&
AP::rangefinder()->params[i].address == address) {
if (AP::rangefinder()->drivers[i] != nullptr) {
//we probably initialised this driver as something else, reboot is required for setting
//it up as UAVCAN type
return nullptr;
}
AP::rangefinder()->drivers[i] = new AP_RangeFinder_UAVCAN(AP::rangefinder()->state[i], AP::rangefinder()->params[i]);
driver = (AP_RangeFinder_UAVCAN*)AP::rangefinder()->drivers[i];
if (driver == nullptr) {
break;
}
AP::rangefinder()->num_instances = MAX(i+1, AP::rangefinder()->num_instances);
//Assign node id and respective uavcan driver, for identification
if (driver->_ap_uavcan == nullptr) {
driver->_ap_uavcan = ap_uavcan;
driver->_node_id = node_id;
break;
}
}
}
}
return driver;
}
//Called from frontend to update with the readings received by handler
void AP_RangeFinder_UAVCAN::update()
{
WITH_SEMAPHORE(_sem);
if ((AP_HAL::millis() - _last_reading_ms) > 500) {
//if data is older than 500ms, report NoData
set_status(RangeFinder::RangeFinder_NoData);
} else if (_status == RangeFinder::RangeFinder_Good && new_data) {
//copy over states
state.distance_cm = _distance_cm;
state.last_reading_ms = _last_reading_ms;
update_status();
new_data = false;
} else if (_status != RangeFinder::RangeFinder_Good) {
//handle additional states received by measurement handler
set_status(_status);
}
}
//RangeFinder message handler
void AP_RangeFinder_UAVCAN::handle_measurement(AP_UAVCAN* ap_uavcan, uint8_t node_id, const MeasurementCb &cb)
{
//fetch the matching uavcan driver, node id and sensor id backend instance
AP_RangeFinder_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id, cb.msg->sensor_id, true);
if (driver == nullptr) {
return;
}
WITH_SEMAPHORE(driver->_sem);
switch (cb.msg->reading_type) {
case uavcan::equipment::range_sensor::Measurement::READING_TYPE_VALID_RANGE:
{
//update the states in backend instance
driver->_distance_cm = cb.msg->range*100.0f;
driver->_last_reading_ms = AP_HAL::millis();
driver->_status = RangeFinder::RangeFinder_Good;
driver->new_data = true;
break;
}
//Additional states supported by RFND message
case uavcan::equipment::range_sensor::Measurement::READING_TYPE_TOO_CLOSE:
{
driver->_last_reading_ms = AP_HAL::millis();
driver->_status = RangeFinder::RangeFinder_OutOfRangeLow;
break;
}
case uavcan::equipment::range_sensor::Measurement::READING_TYPE_TOO_FAR:
{
driver->_last_reading_ms = AP_HAL::millis();
driver->_status = RangeFinder::RangeFinder_OutOfRangeHigh;
break;
}
default:
{
break;
}
}
//copy over the sensor type of Rangefinder
switch (cb.msg->sensor_type) {
case uavcan::equipment::range_sensor::Measurement::SENSOR_TYPE_SONAR:
{
driver->_sensor_type = MAV_DISTANCE_SENSOR_ULTRASOUND;
break;
}
case uavcan::equipment::range_sensor::Measurement::SENSOR_TYPE_LIDAR:
{
driver->_sensor_type = MAV_DISTANCE_SENSOR_LASER;
break;
}
case uavcan::equipment::range_sensor::Measurement::SENSOR_TYPE_RADAR:
{
driver->_sensor_type = MAV_DISTANCE_SENSOR_RADAR;
break;
}
default:
{
driver->_sensor_type = MAV_DISTANCE_SENSOR_UNKNOWN;
break;
}
}
}
#endif // HAL_WITH_UAVCAN

37
libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.h Normal file
View File

@ -0,0 +1,37 @@
#pragma once
#include "RangeFinder_Backend.h"
#if HAL_WITH_UAVCAN
#include <AP_UAVCAN/AP_UAVCAN.h>
#include <AP_Common/Semaphore.h>
class MeasurementCb;
class AP_RangeFinder_UAVCAN : public AP_RangeFinder_Backend {
public:
AP_RangeFinder_UAVCAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params);
void update() override;
static void subscribe_msgs(AP_UAVCAN* ap_uavcan);
static AP_RangeFinder_UAVCAN* get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id, uint8_t address, bool create_new);
static AP_RangeFinder_Backend* detect(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params);
static void handle_measurement(AP_UAVCAN* ap_uavcan, uint8_t node_id, const MeasurementCb &cb);
protected:
virtual MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override {
return _sensor_type;
}
private:
uint8_t _instance;
RangeFinder::RangeFinder_Status _status;
uint16_t _distance_cm;
uint32_t _last_reading_ms;
AP_UAVCAN* _ap_uavcan;
uint8_t _node_id;
bool new_data;
MAV_DISTANCE_SENSOR _sensor_type;
};
#endif //HAL_WITH_UAVCAN

1
libraries/AP_RangeFinder/RangeFinder.cpp
View File

@ -37,6 +37,7 @@
#include "AP_RangeFinder_Benewake.h"
#include "AP_RangeFinder_PWM.h"
#include "AP_RangeFinder_BLPing.h"
#include "AP_RangeFinder_UAVCAN.h"
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <AP_Vehicle/AP_Vehicle_Type.h>

4
libraries/AP_RangeFinder/RangeFinder.h
View File

@ -35,7 +35,8 @@ class AP_RangeFinder_Backend;
class RangeFinder
{
friend class AP_RangeFinder_Backend;
//UAVCAN drivers are initialised in the Backend, hence list of drivers is needed there.
friend class AP_RangeFinder_UAVCAN;
public:
RangeFinder(AP_SerialManager &_serial_manager);
@ -69,6 +70,7 @@ public:
RangeFinder_TYPE_PLI2CV3HP = 21,
RangeFinder_TYPE_PWM = 22,
RangeFinder_TYPE_BLPing = 23,
RangeFinder_TYPE_UAVCAN = 24
};
enum RangeFinder_Function {

1
libraries/AP_RangeFinder/RangeFinder_Backend.cpp
View File

@ -28,6 +28,7 @@ AP_RangeFinder_Backend::AP_RangeFinder_Backend(RangeFinder::RangeFinder_State &_
state(_state),
params(_params)
{
_backend_type = (RangeFinder::RangeFinder_Type)params.type.get();
}
MAV_DISTANCE_SENSOR AP_RangeFinder_Backend::get_mav_distance_sensor_type() const {

3
libraries/AP_RangeFinder/RangeFinder_Backend.h
View File

@ -70,5 +70,8 @@ protected:
// semaphore for access to shared frontend data
HAL_Semaphore _sem;
//Type Backend initialised with
RangeFinder::RangeFinder_Type _backend_type;
virtual MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const = 0;
};