aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

HAL_ChibiOS: support mavcan message forwarding

Browse Source
This commit is contained in:
Andrew Tridgell 2022-02-07 08:22:53 +11:00
parent 61871b1d7b
commit 124c9d5d9b
4 changed files with 30 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_HAL_ChibiOS/CANFDIface.cpp
View File

@ -378,7 +378,8 @@ int16_t CANIface::send(const AP_HAL::CANFrame& frame, uint64_t tx_deadline,
pending_tx_[index].aborted = false;
pending_tx_[index].setup = true;
pending_tx_[index].pushed = false;
return 1;
return AP_HAL::CANIface::send(frame, tx_deadline, flags);
}
int16_t CANIface::receive(AP_HAL::CANFrame& out_frame, uint64_t& out_timestamp_us, CanIOFlags& out_flags)
@ -391,7 +392,8 @@ int16_t CANIface::receive(AP_HAL::CANFrame& out_frame, uint64_t& out_timestamp_u
out_frame = rx_item.frame;
out_timestamp_us = rx_item.timestamp_us;
out_flags = rx_item.flags;
return 1;
return AP_HAL::CANIface::receive(out_frame, out_timestamp_us, out_flags);
}
bool CANIface::configureFilters(const CanFilterConfig* filter_configs,
@ -761,7 +763,7 @@ void CANIface::handleTxCompleteInterrupt(const uint64_t timestamp_us)
rx_item.frame = pending_tx_[i].frame;
rx_item.timestamp_us = timestamp_us;
rx_item.flags = AP_HAL::CANIface::Loopback;
rx_queue_.push(rx_item);
add_to_rx_queue(rx_item);
}
if (event_handle_ != nullptr) {
stats.num_events++;
@ -855,7 +857,7 @@ bool CANIface::readRxFIFO(uint8_t fifo_index)
rx_item.frame = frame;
rx_item.timestamp_us = timestamp_us;
rx_item.flags = 0;
if (rx_queue_.push(rx_item)) {
if (add_to_rx_queue(rx_item)) {
stats.rx_received++;
} else {
stats.rx_overflow++;

9
libraries/AP_HAL_ChibiOS/CANFDIface.h
View File

@ -240,6 +240,15 @@ public:
// CAN Peripheral register structure, pointing at base
// register. Indexed by locical interface number
static constexpr CanType* const Can[HAL_NUM_CAN_IFACES] = { HAL_CAN_BASE_LIST };
protected:
bool add_to_rx_queue(const CanRxItem &rx_item) override {
return rx_queue_.push(rx_item);
}
int8_t get_iface_num(void) const override {
return self_index_;
}
};

9
libraries/AP_HAL_ChibiOS/CANIface.h
View File

@ -238,6 +238,15 @@ public:
// CAN Peripheral register structure
static constexpr bxcan::CanType* const Can[HAL_NUM_CAN_IFACES] = { HAL_CAN_BASE_LIST };
protected:
bool add_to_rx_queue(const CanRxItem &rx_item) override {
return rx_queue_.push(rx_item);
}
int8_t get_iface_num(void) const override {
return self_index_;
}
};
#endif //HAL_NUM_CAN_IFACES
#endif //# if defined(STM32H7XX) || defined(STM32G4)

10
libraries/AP_HAL_ChibiOS/CanIface.cpp
View File

@ -362,7 +362,8 @@ int16_t CANIface::send(const AP_HAL::CANFrame& frame, uint64_t tx_deadline,
txi.abort_on_error = (flags & AbortOnError) != 0;
// setup frame initial state
txi.pushed = false;
return 1;
return AP_HAL::CANIface::send(frame, tx_deadline, flags);
}
int16_t CANIface::receive(AP_HAL::CANFrame& out_frame, uint64_t& out_timestamp_us, CanIOFlags& out_flags)
@ -375,7 +376,8 @@ int16_t CANIface::receive(AP_HAL::CANFrame& out_frame, uint64_t& out_timestamp_u
out_frame = rx_item.frame;
out_timestamp_us = rx_item.timestamp_us;
out_flags = rx_item.flags;
return 1;
return AP_HAL::CANIface::receive(out_frame, out_timestamp_us, out_flags);
}
#if !defined(HAL_BOOTLOADER_BUILD)
@ -481,7 +483,7 @@ void CANIface::handleTxMailboxInterrupt(uint8_t mailbox_index, bool txok, const
rx_item.timestamp_us = timestamp_us;
rx_item.flags = AP_HAL::CANIface::Loopback;
PERF_STATS(stats.tx_loopback);
rx_queue_.push(rx_item);
add_to_rx_queue(rx_item);
}
if (txok && !txi.pushed) {
@ -569,7 +571,7 @@ void CANIface::handleRxInterrupt(uint8_t fifo_index, uint64_t timestamp_us)
rx_item.frame = frame;
rx_item.timestamp_us = timestamp_us;
rx_item.flags = 0;
if (rx_queue_.push(rx_item)) {
if (add_to_rx_queue(rx_item)) {
PERF_STATS(stats.rx_received);
} else {
PERF_STATS(stats.rx_overflow);