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

AP_CANManager: add an output buffer for MAVCAN 

this fixes firmware update of peripheral nodes using MAVCAN
This commit is contained in:
Andrew Tridgell 2022-11-28 15:36:26 +11:00
parent 590d2fcf77
commit 34bb5e10cd
2 changed files with 72 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

67
libraries/AP_CANManager/AP_CANManager.cpp
View File

@ -417,9 +417,29 @@ bool AP_CANManager::handle_can_forward(mavlink_channel_t chan, const mavlink_com
/*
handle a CAN_FRAME packet
*/
void AP_CANManager::handle_can_frame(const mavlink_message_t &msg) const
void AP_CANManager::handle_can_frame(const mavlink_message_t &msg)
{
const uint16_t timeout_us = 2000;
if (frame_buffer == nullptr) {
// allocate frame buffer
WITH_SEMAPHORE(_sem);
// 20 is good for firmware upload
uint8_t buffer_size = 20;
while (frame_buffer == nullptr && buffer_size > 0) {
// we'd like 20 frames, but will live with less
frame_buffer = new ObjectBuffer<BufferFrame>(buffer_size);
if (frame_buffer != nullptr) {
// register a callback for when frames can't be sent immediately
hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&AP_CANManager::process_frame_buffer, void));
break;
}
buffer_size /= 2;
}
if (frame_buffer == nullptr) {
// disard the frames
return;
}
}
switch (msg.msgid) {
case MAVLINK_MSG_ID_CAN_FRAME: {
mavlink_can_frame_t p;
@ -427,8 +447,12 @@ void AP_CANManager::handle_can_frame(const mavlink_message_t &msg) const
if (p.bus >= HAL_NUM_CAN_IFACES || hal.can[p.bus] == nullptr) {
return;
}
AP_HAL::CANFrame frame{p.id, p.data, p.len};
hal.can[p.bus]->send(frame, AP_HAL::native_micros64() + timeout_us, AP_HAL::CANIface::IsMAVCAN);
struct BufferFrame frame {
bus : p.bus,
frame : AP_HAL::CANFrame(p.id, p.data, p.len)
};
WITH_SEMAPHORE(_sem);
frame_buffer->push(frame);
break;
}
case MAVLINK_MSG_ID_CANFD_FRAME: {
@ -437,11 +461,42 @@ void AP_CANManager::handle_can_frame(const mavlink_message_t &msg) const
if (p.bus >= HAL_NUM_CAN_IFACES || hal.can[p.bus] == nullptr) {
return;
}
AP_HAL::CANFrame frame{p.id, p.data, p.len, true};
hal.can[p.bus]->send(frame, AP_HAL::native_micros64() + timeout_us, AP_HAL::CANIface::IsMAVCAN);
struct BufferFrame frame {
bus : p.bus,
frame : AP_HAL::CANFrame(p.id, p.data, p.len, true)
};
WITH_SEMAPHORE(_sem);
frame_buffer->push(frame);
break;
}
}
process_frame_buffer();
}
/*
process the frame buffer
*/
void AP_CANManager::process_frame_buffer(void)
{
while (frame_buffer) {
WITH_SEMAPHORE(_sem);
struct BufferFrame frame;
const uint16_t timeout_us = 2000;
if (!frame_buffer->peek(frame)) {
// no frames in the queue
break;
}
const int16_t retcode = hal.can[frame.bus]->send(frame.frame,
AP_HAL::native_micros64() + timeout_us,
frame.frame.isCanFDFrame()?AP_HAL::CANIface::IsMAVCAN:0);
if (retcode == 0) {
// no space in the CAN output slots, try again later
break;
}
// retcode == 1 means sent, -1 means a frame that can't be
// sent. Either way we should remove from the queue
frame_buffer->pop();
}
}
/*

12
libraries/AP_CANManager/AP_CANManager.h
View File

@ -27,6 +27,7 @@
#include <GCS_MAVLink/GCS_config.h>
#if HAL_GCS_ENABLED
#include <GCS_MAVLink/GCS_MAVLink.h>
#include <AP_HAL/utility/RingBuffer.h>
#endif
class AP_CANManager
@ -113,7 +114,7 @@ public:
#if HAL_GCS_ENABLED
bool handle_can_forward(mavlink_channel_t chan, const mavlink_command_long_t &packet, const mavlink_message_t &msg);
void handle_can_frame(const mavlink_message_t &msg) const;
void handle_can_frame(const mavlink_message_t &msg);
void handle_can_filter_modify(const mavlink_message_t &msg);
#endif
@ -190,6 +191,15 @@ private:
uint16_t num_filter_ids;
uint16_t *filter_ids;
} can_forward;
// buffer for MAVCAN frames
struct BufferFrame {
uint8_t bus;
AP_HAL::CANFrame frame;
};
ObjectBuffer<BufferFrame> *frame_buffer;
void process_frame_buffer(void);
#endif // HAL_GCS_ENABLED
};