Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-26 18:48:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_HAL: support mavcan message forwarding

Browse Source
This commit is contained in:
Andrew Tridgell 2022-02-07 08:22:53 +11:00
parent 33ebc83a68
commit dbe17d072f
2 changed files with 66 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
libraries/AP_HAL/CANIface.cpp
View File

@ -16,6 +16,7 @@
*/
#include "CANIface.h"
#include "system.h"
bool AP_HAL::CANFrame::priorityHigherThan(const CANFrame& rhs) const
{
@ -51,3 +52,53 @@ bool AP_HAL::CANFrame::priorityHigherThan(const CANFrame& rhs) const
*/
return clean_id < rhs_clean_id;
}
/*
parent class receive handling for MAVCAN
*/
int16_t AP_HAL::CANIface::receive(CANFrame& out_frame, uint64_t& out_ts_monotonic, CanIOFlags& out_flags)
{
if (frame_callback && (out_flags & IsMAVCAN)==0) {
frame_callback(get_iface_num(), out_frame);
}
return 1;
}
/*
parent class send handling for MAVCAN
*/
int16_t AP_HAL::CANIface::send(const CANFrame& frame, uint64_t tx_deadline, CanIOFlags flags)
{
if (frame_callback) {
if ((flags & IsMAVCAN) == 0) {
if (frame_counter++ == 100) {
// check every 100 frames for disabling CAN_FRAME send
// we stop sending after 5s if the client stops
// sending MAV_CMD_CAN_FORWARD requests
if (AP_HAL::millis() - last_callback_enable_ms > 5000) {
frame_callback = nullptr;
return 1;
}
frame_counter = 0;
}
frame_callback(get_iface_num(), frame);
} else {
CanRxItem rx_item;
rx_item.frame = frame;
rx_item.timestamp_us = AP_HAL::native_micros64();
rx_item.flags = AP_HAL::CANIface::IsMAVCAN;
add_to_rx_queue(rx_item);
}
}
return 1;
}
/*
register a callback for for sending CAN_FRAME messages
*/
bool AP_HAL::CANIface::register_frame_callback(FrameCb cb)
{
last_callback_enable_ms = AP_HAL::millis();
frame_callback = cb;
return true;
}

17
libraries/AP_HAL/CANIface.h
View File

@ -105,6 +105,7 @@ public:
typedef uint16_t CanIOFlags;
static const CanIOFlags Loopback = 1;
static const CanIOFlags AbortOnError = 2;
static const CanIOFlags IsMAVCAN = 4;
// Single Rx Frame with related info
struct CanRxItem {
@ -167,11 +168,11 @@ public:
}
// Put frame in queue to be sent, return negative if error occured, 0 if no space, and 1 if successful
virtual int16_t send(const CANFrame& frame, uint64_t tx_deadline, CanIOFlags flags) = 0;
virtual int16_t send(const CANFrame& frame, uint64_t tx_deadline, CanIOFlags flags);
// Non blocking receive frame that pops the frames received inside the buffer, return negative if error occured,
// 0 if no frame available, 1 if successful
virtual int16_t receive(CANFrame& out_frame, uint64_t& out_ts_monotonic, CanIOFlags& out_flags) = 0;
virtual int16_t receive(CANFrame& out_frame, uint64_t& out_ts_monotonic, CanIOFlags& out_flags);
//Configure filters so as to reject frames that are not going to be handled by us
virtual bool configureFilters(const CanFilterConfig* filter_configs, uint16_t num_configs)
@ -208,7 +209,19 @@ public:
// return true if init was called and successful
virtual bool is_initialized() const = 0;
FUNCTOR_TYPEDEF(FrameCb, void, uint8_t, const AP_HAL::CANFrame &);
// register a frame callback function
virtual bool register_frame_callback(FrameCb cb);
protected:
virtual int8_t get_iface_num() const = 0;
virtual bool add_to_rx_queue(const CanRxItem &rx_item) = 0;
FrameCb frame_callback;
uint8_t frame_counter;
uint32_t last_callback_enable_ms;
uint32_t bitrate_;
OperatingMode mode_;
};