AP_CANManager: move and rename CAN Driver_Type enumeration
This commit is contained in:
parent
c88b7586da
commit
fea2c8d923
31
libraries/AP_CANManager/AP_CAN.h
Normal file
31
libraries/AP_CANManager/AP_CAN.h
Normal file
@ -0,0 +1,31 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
/*
|
||||||
|
* this header contains data common to ArduPilot CAN, rather than to a
|
||||||
|
* specific implementation of the protocols. So we try to share
|
||||||
|
* enumeration values where possible to make parameters similar across
|
||||||
|
* Periph and main firmwares, for example.
|
||||||
|
*
|
||||||
|
* this is *not* to be a one-stop-shop for including all things CAN...
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <stdint.h>
|
||||||
|
|
||||||
|
class AP_CAN {
|
||||||
|
public:
|
||||||
|
enum class Protocol : uint8_t {
|
||||||
|
None = 0,
|
||||||
|
DroneCAN = 1,
|
||||||
|
// 2 was KDECAN -- do not re-use
|
||||||
|
// 3 was ToshibaCAN -- do not re-use
|
||||||
|
PiccoloCAN = 4,
|
||||||
|
// 5 was CANTester
|
||||||
|
EFI_NWPMU = 6,
|
||||||
|
USD1 = 7,
|
||||||
|
KDECAN = 8,
|
||||||
|
// 9 was MPPT_PacketDigital
|
||||||
|
Scripting = 10,
|
||||||
|
Benewake = 11,
|
||||||
|
Scripting2 = 12,
|
||||||
|
};
|
||||||
|
};
|
@ -33,7 +33,7 @@ const AP_Param::GroupInfo AP_CANManager::CANDriver_Params::var_info[] = {
|
|||||||
// @Values: 0:Disabled,1:DroneCAN,4:PiccoloCAN,6:EFI_NWPMU,7:USD1,8:KDECAN,10:Scripting,11:Benewake,12:Scripting2
|
// @Values: 0:Disabled,1:DroneCAN,4:PiccoloCAN,6:EFI_NWPMU,7:USD1,8:KDECAN,10:Scripting,11:Benewake,12:Scripting2
|
||||||
// @User: Advanced
|
// @User: Advanced
|
||||||
// @RebootRequired: True
|
// @RebootRequired: True
|
||||||
AP_GROUPINFO("PROTOCOL", 1, AP_CANManager::CANDriver_Params, _driver_type, AP_CANManager::Driver_Type_DroneCAN),
|
AP_GROUPINFO("PROTOCOL", 1, AP_CANManager::CANDriver_Params, _driver_type, float(AP_CAN::Protocol::DroneCAN)),
|
||||||
|
|
||||||
#if HAL_ENABLE_DRONECAN_DRIVERS
|
#if HAL_ENABLE_DRONECAN_DRIVERS
|
||||||
// @Group: UC_
|
// @Group: UC_
|
||||||
|
@ -141,7 +141,7 @@ void AP_CANManager::init()
|
|||||||
_slcan_interface.reset_params();
|
_slcan_interface.reset_params();
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
Driver_Type drv_type[HAL_MAX_CAN_PROTOCOL_DRIVERS] = {};
|
AP_CAN::Protocol drv_type[HAL_MAX_CAN_PROTOCOL_DRIVERS] = {};
|
||||||
// loop through interfaces and allocate and initialise Iface,
|
// loop through interfaces and allocate and initialise Iface,
|
||||||
// Also allocate Driver objects, and add interfaces to them
|
// Also allocate Driver objects, and add interfaces to them
|
||||||
for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) {
|
for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) {
|
||||||
@ -165,7 +165,7 @@ void AP_CANManager::init()
|
|||||||
AP_HAL::CANIface* iface = hal.can[i];
|
AP_HAL::CANIface* iface = hal.can[i];
|
||||||
|
|
||||||
// Find the driver type that we need to allocate and register this interface with
|
// Find the driver type that we need to allocate and register this interface with
|
||||||
drv_type[drv_num] = (Driver_Type) _drv_param[drv_num]._driver_type.get();
|
drv_type[drv_num] = (AP_CAN::Protocol) _drv_param[drv_num]._driver_type.get();
|
||||||
bool can_initialised = false;
|
bool can_initialised = false;
|
||||||
// Check if this interface need hooking up to slcan passthrough
|
// Check if this interface need hooking up to slcan passthrough
|
||||||
// instead of a driver
|
// instead of a driver
|
||||||
@ -203,7 +203,7 @@ void AP_CANManager::init()
|
|||||||
|
|
||||||
// Allocate the set type of Driver
|
// Allocate the set type of Driver
|
||||||
#if HAL_ENABLE_DRONECAN_DRIVERS
|
#if HAL_ENABLE_DRONECAN_DRIVERS
|
||||||
if (drv_type[drv_num] == Driver_Type_DroneCAN) {
|
if (drv_type[drv_num] == AP_CAN::Protocol::DroneCAN) {
|
||||||
_drivers[drv_num] = _drv_param[drv_num]._uavcan = new AP_DroneCAN(drv_num);
|
_drivers[drv_num] = _drv_param[drv_num]._uavcan = new AP_DroneCAN(drv_num);
|
||||||
|
|
||||||
if (_drivers[drv_num] == nullptr) {
|
if (_drivers[drv_num] == nullptr) {
|
||||||
@ -215,7 +215,7 @@ void AP_CANManager::init()
|
|||||||
} else
|
} else
|
||||||
#endif
|
#endif
|
||||||
#if HAL_PICCOLO_CAN_ENABLE
|
#if HAL_PICCOLO_CAN_ENABLE
|
||||||
if (drv_type[drv_num] == Driver_Type_PiccoloCAN) {
|
if (drv_type[drv_num] == AP_CAN::Protocol::PiccoloCAN) {
|
||||||
_drivers[drv_num] = _drv_param[drv_num]._piccolocan = new AP_PiccoloCAN;
|
_drivers[drv_num] = _drv_param[drv_num]._piccolocan = new AP_PiccoloCAN;
|
||||||
|
|
||||||
if (_drivers[drv_num] == nullptr) {
|
if (_drivers[drv_num] == nullptr) {
|
||||||
@ -268,7 +268,7 @@ void AP_CANManager::init()
|
|||||||
{
|
{
|
||||||
WITH_SEMAPHORE(_sem);
|
WITH_SEMAPHORE(_sem);
|
||||||
for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) {
|
for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) {
|
||||||
if ((Driver_Type) _drv_param[i]._driver_type.get() == Driver_Type_DroneCAN) {
|
if ((AP_CAN::Protocol) _drv_param[i]._driver_type.get() == AP_CAN::Protocol::DroneCAN) {
|
||||||
_drivers[i] = _drv_param[i]._uavcan = new AP_DroneCAN(i);
|
_drivers[i] = _drv_param[i]._uavcan = new AP_DroneCAN(i);
|
||||||
|
|
||||||
if (_drivers[i] == nullptr) {
|
if (_drivers[i] == nullptr) {
|
||||||
@ -278,7 +278,7 @@ void AP_CANManager::init()
|
|||||||
|
|
||||||
AP_Param::load_object_from_eeprom((AP_DroneCAN*)_drivers[i], AP_DroneCAN::var_info);
|
AP_Param::load_object_from_eeprom((AP_DroneCAN*)_drivers[i], AP_DroneCAN::var_info);
|
||||||
_drivers[i]->init(i, true);
|
_drivers[i]->init(i, true);
|
||||||
_driver_type_cache[i] = (Driver_Type) _drv_param[i]._driver_type.get();
|
_driver_type_cache[i] = (AP_CAN::Protocol) _drv_param[i]._driver_type.get();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -286,7 +286,7 @@ void AP_CANManager::init()
|
|||||||
/*
|
/*
|
||||||
register a new CAN driver
|
register a new CAN driver
|
||||||
*/
|
*/
|
||||||
bool AP_CANManager::register_driver(Driver_Type dtype, AP_CANDriver *driver)
|
bool AP_CANManager::register_driver(AP_CAN::Protocol dtype, AP_CANDriver *driver)
|
||||||
{
|
{
|
||||||
WITH_SEMAPHORE(_sem);
|
WITH_SEMAPHORE(_sem);
|
||||||
|
|
||||||
@ -298,7 +298,7 @@ bool AP_CANManager::register_driver(Driver_Type dtype, AP_CANDriver *driver)
|
|||||||
// from 1 based to 0 based
|
// from 1 based to 0 based
|
||||||
drv_num--;
|
drv_num--;
|
||||||
|
|
||||||
if (dtype != (Driver_Type)_drv_param[drv_num]._driver_type.get()) {
|
if (dtype != (AP_CAN::Protocol)_drv_param[drv_num]._driver_type.get()) {
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
if (_drivers[drv_num] != nullptr) {
|
if (_drivers[drv_num] != nullptr) {
|
||||||
|
@ -32,6 +32,8 @@
|
|||||||
#include <AP_HAL/utility/RingBuffer.h>
|
#include <AP_HAL/utility/RingBuffer.h>
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#include "AP_CAN.h"
|
||||||
|
|
||||||
class AP_CANManager
|
class AP_CANManager
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
@ -56,26 +58,10 @@ public:
|
|||||||
LOG_DEBUG,
|
LOG_DEBUG,
|
||||||
};
|
};
|
||||||
|
|
||||||
enum Driver_Type : uint8_t {
|
|
||||||
Driver_Type_None = 0,
|
|
||||||
Driver_Type_DroneCAN = 1,
|
|
||||||
// 2 was KDECAN -- do not re-use
|
|
||||||
// 3 was ToshibaCAN -- do not re-use
|
|
||||||
Driver_Type_PiccoloCAN = 4,
|
|
||||||
// 5 was CANTester
|
|
||||||
Driver_Type_EFI_NWPMU = 6,
|
|
||||||
Driver_Type_USD1 = 7,
|
|
||||||
Driver_Type_KDECAN = 8,
|
|
||||||
// 9 was Driver_Type_MPPT_PacketDigital
|
|
||||||
Driver_Type_Scripting = 10,
|
|
||||||
Driver_Type_Benewake = 11,
|
|
||||||
Driver_Type_Scripting2 = 12,
|
|
||||||
};
|
|
||||||
|
|
||||||
void init(void);
|
void init(void);
|
||||||
|
|
||||||
// register a new driver
|
// register a new driver
|
||||||
bool register_driver(Driver_Type dtype, AP_CANDriver *driver);
|
bool register_driver(AP_CAN::Protocol dtype, AP_CANDriver *driver);
|
||||||
|
|
||||||
// returns number of active CAN Drivers
|
// returns number of active CAN Drivers
|
||||||
uint8_t get_num_drivers(void) const
|
uint8_t get_num_drivers(void) const
|
||||||
@ -104,12 +90,12 @@ public:
|
|||||||
void log_retrieve(ExpandingString &str) const;
|
void log_retrieve(ExpandingString &str) const;
|
||||||
|
|
||||||
// return driver type index i
|
// return driver type index i
|
||||||
Driver_Type get_driver_type(uint8_t i) const
|
AP_CAN::Protocol get_driver_type(uint8_t i) const
|
||||||
{
|
{
|
||||||
if (i < HAL_NUM_CAN_IFACES) {
|
if (i < HAL_NUM_CAN_IFACES) {
|
||||||
return _driver_type_cache[i];
|
return _driver_type_cache[i];
|
||||||
}
|
}
|
||||||
return Driver_Type_None;
|
return AP_CAN::Protocol::None;
|
||||||
}
|
}
|
||||||
|
|
||||||
static const struct AP_Param::GroupInfo var_info[];
|
static const struct AP_Param::GroupInfo var_info[];
|
||||||
@ -163,7 +149,7 @@ private:
|
|||||||
CANIface_Params _interfaces[HAL_NUM_CAN_IFACES];
|
CANIface_Params _interfaces[HAL_NUM_CAN_IFACES];
|
||||||
AP_CANDriver* _drivers[HAL_MAX_CAN_PROTOCOL_DRIVERS];
|
AP_CANDriver* _drivers[HAL_MAX_CAN_PROTOCOL_DRIVERS];
|
||||||
CANDriver_Params _drv_param[HAL_MAX_CAN_PROTOCOL_DRIVERS];
|
CANDriver_Params _drv_param[HAL_MAX_CAN_PROTOCOL_DRIVERS];
|
||||||
Driver_Type _driver_type_cache[HAL_MAX_CAN_PROTOCOL_DRIVERS];
|
AP_CAN::Protocol _driver_type_cache[HAL_MAX_CAN_PROTOCOL_DRIVERS];
|
||||||
|
|
||||||
AP_Int8 _loglevel;
|
AP_Int8 _loglevel;
|
||||||
uint8_t _num_drivers;
|
uint8_t _num_drivers;
|
||||||
|
@ -36,7 +36,7 @@ CANSensor::CANSensor(const char *driver_name, uint16_t stack_size) :
|
|||||||
{}
|
{}
|
||||||
|
|
||||||
|
|
||||||
void CANSensor::register_driver(AP_CANManager::Driver_Type dtype)
|
void CANSensor::register_driver(AP_CAN::Protocol dtype)
|
||||||
{
|
{
|
||||||
#if HAL_CANMANAGER_ENABLED
|
#if HAL_CANMANAGER_ENABLED
|
||||||
if (!AP::can().register_driver(dtype, this)) {
|
if (!AP::can().register_driver(dtype, this)) {
|
||||||
@ -53,7 +53,7 @@ void CANSensor::register_driver(AP_CANManager::Driver_Type dtype)
|
|||||||
#ifdef HAL_BUILD_AP_PERIPH
|
#ifdef HAL_BUILD_AP_PERIPH
|
||||||
CANSensor::CANSensor_Periph CANSensor::_periph[HAL_NUM_CAN_IFACES];
|
CANSensor::CANSensor_Periph CANSensor::_periph[HAL_NUM_CAN_IFACES];
|
||||||
|
|
||||||
void CANSensor::register_driver_periph(const AP_CANManager::Driver_Type dtype)
|
void CANSensor::register_driver_periph(const AP_CAN::Protocol dtype)
|
||||||
{
|
{
|
||||||
for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) {
|
for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) {
|
||||||
if (_periph[i].protocol != dtype) {
|
if (_periph[i].protocol != dtype) {
|
||||||
|
@ -18,7 +18,11 @@
|
|||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
|
#include "AP_CAN.h"
|
||||||
|
#include "AP_CANDriver.h"
|
||||||
|
#ifndef HAL_BUILD_AP_PERIPH
|
||||||
#include "AP_CANManager.h"
|
#include "AP_CANManager.h"
|
||||||
|
#endif
|
||||||
|
|
||||||
#if HAL_MAX_CAN_PROTOCOL_DRIVERS
|
#if HAL_MAX_CAN_PROTOCOL_DRIVERS
|
||||||
|
|
||||||
@ -39,7 +43,7 @@ public:
|
|||||||
bool write_frame(AP_HAL::CANFrame &out_frame, const uint64_t timeout_us);
|
bool write_frame(AP_HAL::CANFrame &out_frame, const uint64_t timeout_us);
|
||||||
|
|
||||||
#ifdef HAL_BUILD_AP_PERIPH
|
#ifdef HAL_BUILD_AP_PERIPH
|
||||||
static void set_periph(const uint8_t i, const AP_CANManager::Driver_Type protocol, AP_HAL::CANIface* iface) {
|
static void set_periph(const uint8_t i, const AP_CAN::Protocol protocol, AP_HAL::CANIface* iface) {
|
||||||
if (i < ARRAY_SIZE(_periph)) {
|
if (i < ARRAY_SIZE(_periph)) {
|
||||||
_periph[i].protocol = protocol;
|
_periph[i].protocol = protocol;
|
||||||
_periph[i].iface = iface;
|
_periph[i].iface = iface;
|
||||||
@ -47,19 +51,19 @@ public:
|
|||||||
}
|
}
|
||||||
|
|
||||||
// return driver type index i
|
// return driver type index i
|
||||||
static AP_CANManager::Driver_Type get_driver_type(const uint8_t i)
|
static AP_CAN::Protocol get_driver_type(const uint8_t i)
|
||||||
{
|
{
|
||||||
if (i < ARRAY_SIZE(_periph)) {
|
if (i < ARRAY_SIZE(_periph)) {
|
||||||
return _periph[i].protocol;
|
return _periph[i].protocol;
|
||||||
}
|
}
|
||||||
return AP_CANManager::Driver_Type_None;
|
return AP_CAN::Protocol::None;
|
||||||
}
|
}
|
||||||
#else
|
#else
|
||||||
static AP_CANManager::Driver_Type get_driver_type(const uint8_t i) { return AP::can().get_driver_type(i); }
|
static AP_CAN::Protocol get_driver_type(const uint8_t i) { return AP::can().get_driver_type(i); }
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
void register_driver(AP_CANManager::Driver_Type dtype);
|
void register_driver(AP_CAN::Protocol dtype);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
void loop();
|
void loop();
|
||||||
@ -73,11 +77,11 @@ private:
|
|||||||
AP_HAL::CANIface* _can_iface;
|
AP_HAL::CANIface* _can_iface;
|
||||||
|
|
||||||
#ifdef HAL_BUILD_AP_PERIPH
|
#ifdef HAL_BUILD_AP_PERIPH
|
||||||
void register_driver_periph(const AP_CANManager::Driver_Type dtype);
|
void register_driver_periph(const AP_CAN::Protocol dtype);
|
||||||
|
|
||||||
struct CANSensor_Periph {
|
struct CANSensor_Periph {
|
||||||
AP_HAL::CANIface* iface;
|
AP_HAL::CANIface* iface;
|
||||||
AP_CANManager::Driver_Type protocol;
|
AP_CAN::Protocol protocol;
|
||||||
} static _periph[HAL_NUM_CAN_IFACES];
|
} static _periph[HAL_NUM_CAN_IFACES];
|
||||||
#endif
|
#endif
|
||||||
};
|
};
|
||||||
|
Loading…
Reference in New Issue
Block a user