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_PiccoloCAN: modify to use uavcan agnostic CAN drivers and manager

Browse Source
This commit is contained in:
Siddharth Purohit 2020-05-31 18:06:45 +05:30 committed by Andrew Tridgell
parent 8a31448709
commit 0fcf0b4564
2 changed files with 77 additions and 88 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

147
libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp
View File

@ -23,11 +23,8 @@
#if HAL_PICCOLO_CAN_ENABLE
#include <uavcan/uavcan.hpp>
#include <uavcan/driver/can.hpp>
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <AP_BoardConfig/AP_BoardConfig_CAN.h>
#include <AP_CANManager/AP_CANManager.h>
#include <AP_Common/AP_Common.h>
#include <AP_Scheduler/AP_Scheduler.h>
#include <AP_HAL/utility/sparse-endian.h>
@ -43,59 +40,62 @@
extern const AP_HAL::HAL& hal;
static const uint8_t CAN_IFACE_INDEX = 0;
#define debug_can(level_debug, fmt, args...) do { if ((level_debug) <= AP::can().get_debug_level_driver(_driver_index)) { printf(fmt, ##args); }} while (0)
#define debug_can(level_debug, fmt, args...) do { AP::can().log_text(level_debug, "PiccoloCAN", fmt, ##args); } while (0)
AP_PiccoloCAN::AP_PiccoloCAN()
{
debug_can(2, "PiccoloCAN: constructed\n\r");
debug_can(AP_CANManager::LOG_INFO, "PiccoloCAN: constructed\n\r");
}
AP_PiccoloCAN *AP_PiccoloCAN::get_pcan(uint8_t driver_index)
{
if (driver_index >= AP::can().get_num_drivers() ||
AP::can().get_protocol_type(driver_index) != AP_BoardConfig_CAN::Protocol_Type_PiccoloCAN) {
AP::can().get_driver_type(driver_index) != AP_CANManager::Driver_Type_PiccoloCAN) {
return nullptr;
}
return static_cast<AP_PiccoloCAN*>(AP::can().get_driver(driver_index));
}
bool AP_PiccoloCAN::add_interface(AP_HAL::CANIface* can_iface) {
if (_can_iface != nullptr) {
debug_can(AP_CANManager::LOG_ERROR, "PiccoloCAN: Multiple Interface not supported\n\r");
return false;
}
_can_iface = can_iface;
if (_can_iface == nullptr) {
debug_can(AP_CANManager::LOG_ERROR, "PiccoloCAN: CAN driver not found\n\r");
return false;
}
if (!_can_iface->is_initialized()) {
debug_can(AP_CANManager::LOG_ERROR, "PiccoloCAN: Driver not initialized\n\r");
return false;
}
if (!_can_iface->set_event_handle(&_event_handle)) {
debug_can(AP_CANManager::LOG_ERROR, "PiccoloCAN: Cannot add event handle\n\r");
return false;
}
return true;
}
// initialize PiccoloCAN bus
void AP_PiccoloCAN::init(uint8_t driver_index, bool enable_filters)
{
_driver_index = driver_index;
debug_can(2, "PiccoloCAN: starting init\n\r");
debug_can(AP_CANManager::LOG_DEBUG, "PiccoloCAN: starting init\n\r");
if (_initialized) {
debug_can(1, "PiccoloCAN: already initialized\n\r");
debug_can(AP_CANManager::LOG_ERROR, "PiccoloCAN: already initialized\n\r");
return;
}
AP_HAL::CANManager* can_mgr = hal.can_mgr[driver_index];
if (can_mgr == nullptr) {
debug_can(1, "PiccoloCAN: no mgr for this driver\n\r");
return;
}
if (!can_mgr->is_initialized()) {
debug_can(1, "PiccoloCAN: mgr not initialized\n\r");
return;
}
_can_driver = can_mgr->get_driver();
if (_can_driver == nullptr) {
debug_can(1, "PiccoloCAN: no CAN driver\n\r");
return;
}
// start calls to loop in separate thread
if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_PiccoloCAN::loop, void), _thread_name, 4096, AP_HAL::Scheduler::PRIORITY_MAIN, 1)) {
debug_can(1, "PiccoloCAN: couldn't create thread\n\r");
debug_can(AP_CANManager::LOG_ERROR, "PiccoloCAN: couldn't create thread\n\r");
return;
}
@ -103,14 +103,14 @@ void AP_PiccoloCAN::init(uint8_t driver_index, bool enable_filters)
snprintf(_thread_name, sizeof(_thread_name), "PiccoloCAN_%u", driver_index);
debug_can(2, "PiccoloCAN: init done\n\r");
debug_can(AP_CANManager::LOG_DEBUG, "PiccoloCAN: init done\n\r");
}
// loop to send output to CAN devices in background thread
void AP_PiccoloCAN::loop()
{
uavcan::CanFrame txFrame;
uavcan::CanFrame rxFrame;
AP_HAL::CANFrame txFrame;
AP_HAL::CANFrame rxFrame;
// How often to transmit CAN messages (milliseconds)
#define CMD_TX_PERIOD 10
@ -121,17 +121,17 @@ void AP_PiccoloCAN::loop()
uint8_t frame_id_group; // Piccolo message group
uint16_t frame_id_device; // Device identifier
uavcan::MonotonicTime timeout;
uint64_t timeout;
while (true) {
if (!_initialized) {
debug_can(2, "PiccoloCAN: not initialized\n\r");
debug_can(AP_CANManager::LOG_ERROR, "PiccoloCAN: not initialized\n\r");
hal.scheduler->delay_microseconds(10000);
continue;
}
timeout = uavcan::MonotonicTime::fromUSec(AP_HAL::micros64() + 250);
timeout = AP_HAL::micros64() + 250;
// 1ms loop delay
hal.scheduler->delay_microseconds(1 * 1000);
@ -151,7 +151,7 @@ void AP_PiccoloCAN::loop()
frame_id_device = (rxFrame.id >> 8) & 0xFF;
// Only accept extended messages
if ((rxFrame.id & uavcan::CanFrame::FlagEFF) == 0) {
if ((rxFrame.id & AP_HAL::CANFrame::FlagEFF) == 0) {
continue;
}
@ -179,55 +179,45 @@ void AP_PiccoloCAN::loop()
}
// write frame on CAN bus, returns true on success
bool AP_PiccoloCAN::write_frame(uavcan::CanFrame &out_frame, uavcan::MonotonicTime timeout)
bool AP_PiccoloCAN::write_frame(AP_HAL::CANFrame &out_frame, uint64_t timeout)
{
if (!_initialized) {
debug_can(1, "PiccoloCAN: Driver not initialized for write_frame\n\r");
debug_can(AP_CANManager::LOG_ERROR, "PiccoloCAN: Driver not initialized for write_frame\n\r");
return false;
}
// wait for space in buffer to send command
uavcan::CanSelectMasks inout_mask;
bool read_select = false;
bool write_select = true;
bool ret;
do {
inout_mask.read = 0;
inout_mask.write = (1 << CAN_IFACE_INDEX);
_select_frames[CAN_IFACE_INDEX] = &out_frame;
_can_driver->select(inout_mask, _select_frames, timeout);
if (!inout_mask.write) {
ret = _can_iface->select(read_select, write_select, &out_frame, timeout);
if (!ret || !write_select) {
hal.scheduler->delay_microseconds(50);
}
} while (!inout_mask.write);
} while (!ret || !write_select);
return (_can_driver->getIface(CAN_IFACE_INDEX)->send(out_frame, timeout, uavcan::CanIOFlagAbortOnError) == 1);
return (_can_iface->send(out_frame, timeout, AP_HAL::CANIface::AbortOnError) == 1);
}
// read frame on CAN bus, returns true on succses
bool AP_PiccoloCAN::read_frame(uavcan::CanFrame &recv_frame, uavcan::MonotonicTime timeout)
bool AP_PiccoloCAN::read_frame(AP_HAL::CANFrame &recv_frame, uint64_t timeout)
{
if (!_initialized) {
debug_can(1, "PiccoloCAN: Driver not initialized for read_frame\n\r");
debug_can(AP_CANManager::LOG_ERROR, "PiccoloCAN: Driver not initialized for read_frame\n\r");
return false;
}
uavcan::CanSelectMasks inout_mask;
inout_mask.read = 1 << CAN_IFACE_INDEX;
inout_mask.write = 0;
_select_frames[CAN_IFACE_INDEX] = &recv_frame;
_can_driver->select(inout_mask, _select_frames, timeout);
if (!inout_mask.read) {
bool read_select = true;
bool write_select = false;
bool ret = _can_iface->select(read_select, write_select, nullptr, timeout);
if (!ret || !read_select) {
// No frame available
return false;
}
uavcan::MonotonicTime time;
uavcan::UtcTime utc_time;
uavcan::CanIOFlags flags {};
uint64_t time;
AP_HAL::CANIface::CanIOFlags flags {};
return (_can_driver->getIface(CAN_IFACE_INDEX)->receive(recv_frame, time, utc_time, flags) == 1);
return (_can_iface->receive(recv_frame, time, flags) == 1);
}
// called from SRV_Channels
@ -355,9 +345,9 @@ void AP_PiccoloCAN::send_esc_telemetry_mavlink(uint8_t mav_chan)
// send ESC messages over CAN
void AP_PiccoloCAN::send_esc_messages(void)
{
uavcan::CanFrame txFrame;
AP_HAL::CANFrame txFrame;
uavcan::MonotonicTime timeout = uavcan::MonotonicTime::fromUSec(AP_HAL::micros64() + 250);
uint64_t timeout = AP_HAL::micros64() + 250;
// TODO - How to buffer CAN messages properly?
// Sending more than 2 messages at each loop instance means that sometimes messages are dropped
@ -427,7 +417,7 @@ void AP_PiccoloCAN::send_esc_messages(void)
// interpret an ESC message received over CAN
bool AP_PiccoloCAN::handle_esc_message(uavcan::CanFrame &frame)
bool AP_PiccoloCAN::handle_esc_message(AP_HAL::CANFrame &frame)
{
uint64_t timestamp = AP_HAL::micros64();
@ -455,7 +445,6 @@ bool AP_PiccoloCAN::handle_esc_message(uavcan::CanFrame &frame)
if (decodeESC_StatusAPacketStructure(&frame, &esc.statusA)) {
esc.newTelemetry = true;
} else if (decodeESC_StatusBPacketStructure(&frame, &esc.statusB)) {
esc.newTelemetry = true;
} else if (decodeESC_FirmwarePacketStructure(&frame, &esc.firmware)) {
// TODO
@ -560,7 +549,7 @@ bool AP_PiccoloCAN::pre_arm_check(char* reason, uint8_t reason_len)
//! \return the packet data pointer from the packet
uint8_t* getESCVelocityPacketData(void* pkt)
{
uavcan::CanFrame* frame = (uavcan::CanFrame*) pkt;
AP_HAL::CANFrame* frame = (AP_HAL::CANFrame*) pkt;
return (uint8_t*) frame->data;
}
@ -568,7 +557,7 @@ uint8_t* getESCVelocityPacketData(void* pkt)
//! \return the packet data pointer from the packet, const
const uint8_t* getESCVelocityPacketDataConst(const void* pkt)
{
uavcan::CanFrame* frame = (uavcan::CanFrame*) pkt;
AP_HAL::CANFrame* frame = (AP_HAL::CANFrame*) pkt;
return (const uint8_t*) frame->data;
}
@ -576,10 +565,10 @@ const uint8_t* getESCVelocityPacketDataConst(const void* pkt)
//! Complete a packet after the data have been encoded
void finishESCVelocityPacket(void* pkt, int size, uint32_t packetID)
{
uavcan::CanFrame* frame = (uavcan::CanFrame*) pkt;
AP_HAL::CANFrame* frame = (AP_HAL::CANFrame*) pkt;
if (size > uavcan::CanFrame::MaxDataLen) {
size = uavcan::CanFrame::MaxDataLen;
if (size > AP_HAL::CANFrame::MaxDataLen) {
size = AP_HAL::CANFrame::MaxDataLen;
}
frame->dlc = size;
@ -599,7 +588,7 @@ void finishESCVelocityPacket(void* pkt, int size, uint32_t packetID)
(((uint8_t) AP_PiccoloCAN::ActuatorType::ESC) << 8); // Actuator type
// Extended frame format
id |= uavcan::CanFrame::FlagEFF;
id |= AP_HAL::CANFrame::FlagEFF;
frame->id = id;
}
@ -607,7 +596,7 @@ void finishESCVelocityPacket(void* pkt, int size, uint32_t packetID)
//! \return the size of a packet from the packet header
int getESCVelocityPacketSize(const void* pkt)
{
uavcan::CanFrame* frame = (uavcan::CanFrame*) pkt;
AP_HAL::CANFrame* frame = (AP_HAL::CANFrame*) pkt;
return (int) frame->dlc;
}
@ -615,7 +604,7 @@ int getESCVelocityPacketSize(const void* pkt)
//! \return the ID of a packet from the packet header
uint32_t getESCVelocityPacketID(const void* pkt)
{
uavcan::CanFrame* frame = (uavcan::CanFrame*) pkt;
AP_HAL::CANFrame* frame = (AP_HAL::CANFrame*) pkt;
// Extract the message ID field from the 29-bit ID
return (uint32_t) ((frame->id >> 16) & 0xFF);

18
libraries/AP_PiccoloCAN/AP_PiccoloCAN.h
View File

@ -18,7 +18,7 @@
#pragma once
#include <AP_HAL/AP_HAL.h>
#include <AP_HAL/CAN.h>
#include <AP_CANManager/AP_CANDriver.h>
#include <AP_HAL/Semaphores.h>
#include "piccolo_protocol/ESCPackets.h"
@ -28,12 +28,12 @@
#define PICCOLO_CAN_MAX_GROUP_ESC (PICCOLO_CAN_MAX_NUM_ESC / 4)
#ifndef HAL_PICCOLO_CAN_ENABLE
#define HAL_PICCOLO_CAN_ENABLE (HAL_WITH_UAVCAN && !HAL_MINIMIZE_FEATURES)
#define HAL_PICCOLO_CAN_ENABLE (HAL_NUM_CAN_IFACES && !HAL_MINIMIZE_FEATURES)
#endif
#if HAL_PICCOLO_CAN_ENABLE
class AP_PiccoloCAN : public AP_HAL::CANProtocol
class AP_PiccoloCAN : public AP_CANDriver
{
public:
AP_PiccoloCAN();
@ -65,6 +65,7 @@ public:
// initialize PiccoloCAN bus
void init(uint8_t driver_index, bool enable_filters) override;
bool add_interface(AP_HAL::CANIface* can_iface) override;
// called from SRV_Channels
void update();
@ -87,23 +88,22 @@ private:
void loop();
// write frame on CAN bus, returns true on success
bool write_frame(uavcan::CanFrame &out_frame, uavcan::MonotonicTime timeout);
bool write_frame(AP_HAL::CANFrame &out_frame, uint64_t timeout);
// read frame on CAN bus, returns true on succses
bool read_frame(uavcan::CanFrame &recv_frame, uavcan::MonotonicTime timeout);
bool read_frame(AP_HAL::CANFrame &recv_frame, uint64_t timeout);
// send ESC commands over CAN
void send_esc_messages(void);
// interpret an ESC message received over CAN
bool handle_esc_message(uavcan::CanFrame &frame);
bool handle_esc_message(AP_HAL::CANFrame &frame);
bool _initialized;
char _thread_name[16];
uint8_t _driver_index;
uavcan::ICanDriver* _can_driver;
const uavcan::CanFrame* _select_frames[uavcan::MaxCanIfaces] { };
AP_HAL::CANIface* _can_iface;
HAL_EventHandle _event_handle;
HAL_Semaphore _telem_sem;
struct PiccoloESC_Info_t {