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

AP_HAL_QURT: Move DSP to Apps Proc link message sequence checking into UART driver so it can be per link.

This commit is contained in:
Eric Katzfey 2024-11-13 17:09:16 -08:00 committed by Peter Barker
parent ee09497b64
commit d126c11329
3 changed files with 15 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_HAL_QURT/UARTDriver.cpp
View File

@ -150,9 +150,19 @@ UARTDriver_MAVLinkUDP::UARTDriver_MAVLinkUDP(uint8_t instance) : inst(instance)
register_mavlink_data_callback(instance, _mavlink_data_cb, (void *) this); register_mavlink_data_callback(instance, _mavlink_data_cb, (void *) this);
} }
void UARTDriver_MAVLinkUDP::check_rx_seq(uint32_t seq)
{
if (seq != rx_seq)
{
HAP_PRINTF("Sequence mismatch for instance %u. Expected %u, got %u", inst, rx_seq, seq);
}
rx_seq++;
}
void UARTDriver_MAVLinkUDP::_mavlink_data_cb(const struct qurt_rpc_msg *msg, void *p) void UARTDriver_MAVLinkUDP::_mavlink_data_cb(const struct qurt_rpc_msg *msg, void *p)
{ {
auto *driver = (UARTDriver_MAVLinkUDP *)p; auto *driver = (UARTDriver_MAVLinkUDP *)p;
driver->check_rx_seq(msg->seq);
driver->_readbuf.write(msg->data, msg->data_length); driver->_readbuf.write(msg->data, msg->data_length);
} }
@ -183,7 +193,7 @@ bool UARTDriver_MAVLinkUDP::_write_pending_bytes(void)
} }
msg.msg_id = QURT_MSG_ID_MAVLINK_MSG; msg.msg_id = QURT_MSG_ID_MAVLINK_MSG;
msg.inst = inst; msg.inst = inst;
msg.seq = seq++; msg.seq = tx_seq++;
msg.data_length = _writebuf.read(msg.data, n); msg.data_length = _writebuf.read(msg.data, n);
return qurt_rpc_send(msg); return qurt_rpc_send(msg);

5
libraries/AP_HAL_QURT/UARTDriver.h
View File

@ -83,6 +83,8 @@ public:
bool _write_pending_bytes(void) override; bool _write_pending_bytes(void) override;
void check_rx_seq(uint32_t seq);
uint32_t bw_in_bytes_per_second() const override uint32_t bw_in_bytes_per_second() const override
{ {
return 250000 * 3; return 250000 * 3;
@ -95,7 +97,8 @@ public:
private: private:
static void _mavlink_data_cb(const struct qurt_rpc_msg *msg, void *p); static void _mavlink_data_cb(const struct qurt_rpc_msg *msg, void *p);
uint8_t inst; uint8_t inst;
uint32_t seq; uint32_t tx_seq;
uint32_t rx_seq;
}; };
/* /*

5
libraries/AP_HAL_QURT/replace.cpp
View File

@ -151,7 +151,6 @@ typedef void (*mavlink_data_callback_t)(const struct qurt_rpc_msg *msg, void* p)
static mavlink_data_callback_t mav_cb[MAX_MAVLINK_INSTANCES]; static mavlink_data_callback_t mav_cb[MAX_MAVLINK_INSTANCES];
static void *mav_cb_ptr[MAX_MAVLINK_INSTANCES]; static void *mav_cb_ptr[MAX_MAVLINK_INSTANCES];
static uint32_t expected_seq;
void register_mavlink_data_callback(uint8_t instance, mavlink_data_callback_t func, void *p) void register_mavlink_data_callback(uint8_t instance, mavlink_data_callback_t func, void *p)
{ {
@ -172,10 +171,6 @@ int slpi_link_client_receive(const uint8_t *data, int data_len_in_bytes)
if (msg->data_length + QURT_RPC_MSG_HEADER_LEN != data_len_in_bytes) { if (msg->data_length + QURT_RPC_MSG_HEADER_LEN != data_len_in_bytes) {
return 0; return 0;
} }
if (msg->seq != expected_seq) {
HAP_PRINTF("Bad sequence %u %u", msg->seq, expected_seq);
}
expected_seq = msg->seq + 1;
switch (msg->msg_id) { switch (msg->msg_id) {
case QURT_MSG_ID_MAVLINK_MSG: { case QURT_MSG_ID_MAVLINK_MSG: {