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

AP_HAL_QURT: Added support for dual mavlink streams

This commit is contained in:
Eric Katzfey 2024-07-30 09:33:08 -07:00 committed by Andrew Tridgell
parent d7caf1e6b5
commit 9ef76de0c9
6 changed files with 151 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_HAL_QURT/HAL_QURT_Class.cpp
View File

@ -45,7 +45,8 @@ static void crash_error_handler(void)
using namespace QURT; using namespace QURT;
static UARTDriver_Console consoleDriver; static UARTDriver_Console consoleDriver;
static UARTDriver_MAVLinkUDP serial0Driver; static UARTDriver_MAVLinkUDP serial0Driver(0);
static UARTDriver_MAVLinkUDP serial1Driver(1);
static UARTDriver_Local serial3Driver(QURT_UART_GPS); static UARTDriver_Local serial3Driver(QURT_UART_GPS);
static UARTDriver_Local serial4Driver(QURT_UART_RCIN); static UARTDriver_Local serial4Driver(QURT_UART_RCIN);
@ -64,7 +65,7 @@ bool qurt_ran_overtime;
HAL_QURT::HAL_QURT() : HAL_QURT::HAL_QURT() :
AP_HAL::HAL( AP_HAL::HAL(
&serial0Driver, &serial0Driver,
nullptr, &serial1Driver,
nullptr, nullptr,
&serial3Driver, &serial3Driver,
&serial4Driver, &serial4Driver,

7
libraries/AP_HAL_QURT/UARTDriver.cpp
View File

@ -143,11 +143,11 @@ void UARTDriver_Console::printf(const char *fmt, ...)
methods for UARTDriver_MAVLinkUDP methods for UARTDriver_MAVLinkUDP
*/ */
typedef void (*mavlink_data_callback_t)(const struct qurt_rpc_msg *msg, void* p); typedef void (*mavlink_data_callback_t)(const struct qurt_rpc_msg *msg, void* p);
extern void register_mavlink_data_callback(mavlink_data_callback_t func, void *p); extern void register_mavlink_data_callback(uint8_t instance, mavlink_data_callback_t func, void *p);
UARTDriver_MAVLinkUDP::UARTDriver_MAVLinkUDP(void) UARTDriver_MAVLinkUDP::UARTDriver_MAVLinkUDP(uint8_t instance) : inst(instance)
{ {
register_mavlink_data_callback(_mavlink_data_cb, (void *) this); register_mavlink_data_callback(instance, _mavlink_data_cb, (void *) this);
} }
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)
@ -182,6 +182,7 @@ bool UARTDriver_MAVLinkUDP::_write_pending_bytes(void)
return false; return false;
} }
msg.msg_id = QURT_MSG_ID_MAVLINK_MSG; msg.msg_id = QURT_MSG_ID_MAVLINK_MSG;
msg.inst = inst;
msg.seq = seq++; msg.seq = seq++;
msg.data_length = _writebuf.read(msg.data, n); msg.data_length = _writebuf.read(msg.data, n);

4
libraries/AP_HAL_QURT/UARTDriver.h
View File

@ -75,10 +75,11 @@ public:
/* /*
subclass for MAVLink UDP communications subclass for MAVLink UDP communications
*/ */
class QURT::UARTDriver_MAVLinkUDP : public QURT::UARTDriver class QURT::UARTDriver_MAVLinkUDP : public QURT::UARTDriver
{ {
public: public:
UARTDriver_MAVLinkUDP(void); UARTDriver_MAVLinkUDP(uint8_t instance);
bool _write_pending_bytes(void) override; bool _write_pending_bytes(void) override;
@ -93,6 +94,7 @@ 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;
uint32_t seq; uint32_t seq;
}; };

165
libraries/AP_HAL_QURT/ap_host/src/main.cpp
View File

@ -23,16 +23,25 @@ volatile bool _running = false;
static bool enable_debug = false; static bool enable_debug = false;
static bool enable_remote_debug = false; static bool enable_remote_debug = false;
static int socket_fd; static int gcs_socket_fd;
static bool connected; static int obd_socket_fd;
static struct sockaddr_in remote_addr; static bool gcs_connected;
static bool obd_connected;
static struct sockaddr_in gcs_addr;
static struct sockaddr_in obd_addr;
#define SO_NAME "ArduPilot.so" #define SO_NAME "ArduPilot.so"
// setup for mavlink to localhost // setup for mavlink to localhost
#define MAVLINK_UDP_LOCALHOST 1 #define MAVLINK_UDP_LOCALHOST 1
#define MAVLINK_UDP_PORT_LOCAL 14558
#define MAVLINK_UDP_PORT_REMOTE 14559 // Ports for onboard stream
#define MAVLINK_OBD_UDP_PORT_LOCAL 14556
#define MAVLINK_OBD_UDP_PORT_REMOTE 14557
// Ports for external GCS stream
#define MAVLINK_GCS_UDP_PORT_LOCAL 14558
#define MAVLINK_GCS_UDP_PORT_REMOTE 14559
// directory for logs, parameters, terrain etc // directory for logs, parameters, terrain etc
@ -63,7 +72,7 @@ static void shutdown_signal_handler(int signo)
static void slpi_init(void); static void slpi_init(void);
static uint32_t num_params = 0; static uint32_t num_params = 0;
static uint32_t expected_seq = 0; static uint32_t expected_seq[MAX_MAVLINK_INSTANCES] = {0, 0};
static void receive_callback(const uint8_t *data, uint32_t length_in_bytes) static void receive_callback(const uint8_t *data, uint32_t length_in_bytes)
{ {
@ -79,17 +88,28 @@ static void receive_callback(const uint8_t *data, uint32_t length_in_bytes)
printf("Invalid lengths %d %d\n", msg->data_length, length_in_bytes); printf("Invalid lengths %d %d\n", msg->data_length, length_in_bytes);
return; return;
} }
if (msg->seq != expected_seq) { if (msg->inst < MAX_MAVLINK_INSTANCES) {
printf("Invalid seq %u %u\n", msg->seq, expected_seq); if (msg->seq != expected_seq[msg->inst]) {
printf("Invalid seq %u %u\n", msg->seq, expected_seq[msg->inst]);
}
} else {
printf("Invalid instance %u\n", msg->inst);
} }
expected_seq = msg->seq + 1; expected_seq[msg->inst] = msg->seq + 1;
switch (msg->msg_id) { switch (msg->msg_id) {
case QURT_MSG_ID_MAVLINK_MSG: { case QURT_MSG_ID_MAVLINK_MSG: {
if (_running) { if (_running) {
const auto bytes_sent = sendto(socket_fd, msg->data, msg->data_length, 0, (struct sockaddr *)&remote_addr, sizeof(remote_addr)); if (msg->inst == 0) {
if (bytes_sent <= 0) { const auto bytes_sent = sendto(gcs_socket_fd, msg->data, msg->data_length, 0, (struct sockaddr *)&gcs_addr, sizeof(gcs_addr));
fprintf(stderr, "Send to GCS failed\n"); if (bytes_sent <= 0) {
fprintf(stderr, "Send to GCS failed\n");
}
} else if (msg->inst == 1) {
const auto bytes_sent = sendto(obd_socket_fd, msg->data, msg->data_length, 0, (struct sockaddr *)&obd_addr, sizeof(obd_addr));
if (bytes_sent <= 0) {
fprintf(stderr, "Send to onboard failed\n");
}
} }
} }
break; break;
@ -129,6 +149,45 @@ static void setup_directores(void)
} }
} }
void *obd_recv_thread(void *)
{
uint32_t next_seq = 0;
printf("Waiting for OBD receive\n");
while (_running) {
struct qurt_rpc_msg msg {};
struct sockaddr_in from;
socklen_t fromlen = sizeof(from);
uint32_t bytes_received = recvfrom(obd_socket_fd, msg.data, sizeof(msg.data), 0,
(struct sockaddr*)&from, &fromlen);
if (bytes_received > 0 && !obd_connected) {
obd_addr = from;
obd_connected = true;
printf("Connnected to OBD addr %s\n", inet_ntoa(from.sin_addr));
}
if (bytes_received < 0) {
fprintf(stderr, "OBD receive failed");
continue;
}
if (bytes_received > sizeof(msg.data)) {
printf("Invalid bytes_received %d\n", bytes_received);
continue;
}
msg.msg_id = QURT_MSG_ID_MAVLINK_MSG;
msg.inst = 1;
msg.data_length = bytes_received;
msg.seq = next_seq++;
// printf("Message received. %d bytes\n", bytes_received);
if (slpi_link_send((const uint8_t*) &msg, bytes_received + QURT_RPC_MSG_HEADER_LEN)) {
fprintf(stderr, "slpi_link_send_data failed for instance 1\n");
}
}
return NULL;
}
int main() int main()
{ {
printf("Starting up\n"); printf("Starting up\n");
@ -158,36 +217,57 @@ int main()
slpi_init(); slpi_init();
//initialize socket and structure //initialize sockets and structures
socket_fd = socket(AF_INET, SOCK_DGRAM, 0); gcs_socket_fd = socket(AF_INET, SOCK_DGRAM, 0);
if (socket_fd == -1) { if (gcs_socket_fd == -1) {
fprintf(stderr, "Could not create socket"); fprintf(stderr, "Could not create GCS socket");
return -1;
}
obd_socket_fd = socket(AF_INET, SOCK_DGRAM, 0);
if (obd_socket_fd == -1) {
fprintf(stderr, "Could not create OBD socket");
return -1; return -1;
} }
#if MAVLINK_UDP_LOCALHOST #if MAVLINK_UDP_LOCALHOST
// send to mavlink router on localhost // send to mavlink router on localhost for GCS
remote_addr.sin_addr.s_addr = htonl(INADDR_LOOPBACK); gcs_addr.sin_addr.s_addr = htonl(INADDR_LOOPBACK);
remote_addr.sin_family = AF_INET; gcs_addr.sin_family = AF_INET;
remote_addr.sin_port = htons(MAVLINK_UDP_PORT_REMOTE); gcs_addr.sin_port = htons(MAVLINK_GCS_UDP_PORT_REMOTE);
struct sockaddr_in local {}; struct sockaddr_in gcs_local {};
local.sin_addr.s_addr = INADDR_ANY; gcs_local.sin_addr.s_addr = INADDR_ANY;
local.sin_family = AF_INET; gcs_local.sin_family = AF_INET;
local.sin_port = htons(MAVLINK_UDP_PORT_LOCAL); gcs_local.sin_port = htons(MAVLINK_GCS_UDP_PORT_LOCAL);
if (bind(socket_fd, (struct sockaddr *)&local, sizeof(local)) == 0) { if (bind(gcs_socket_fd, (struct sockaddr *)&gcs_local, sizeof(gcs_local)) == 0) {
printf("Bind localhost OK\n"); printf("Bind localhost GCS socket OK\n");
} else { } else {
printf("Bind failed: %s", strerror(errno)); printf("Bind localhost GCS socket failed: %s", strerror(errno));
}
// send to mavlink router on localhost for onboard stream
obd_addr.sin_addr.s_addr = htonl(INADDR_LOOPBACK);
obd_addr.sin_family = AF_INET;
obd_addr.sin_port = htons(MAVLINK_OBD_UDP_PORT_REMOTE);
struct sockaddr_in obd_local {};
obd_local.sin_addr.s_addr = INADDR_ANY;
obd_local.sin_family = AF_INET;
obd_local.sin_port = htons(MAVLINK_OBD_UDP_PORT_LOCAL);
if (bind(obd_socket_fd, (struct sockaddr *)&obd_local, sizeof(obd_local)) == 0) {
printf("Bind localhost OBD socket OK\n");
} else {
printf("Bind localhost OBD socket failed: %s", strerror(errno));
} }
#else #else
// broadcast directly to the local network broadcast address // broadcast directly to the local network broadcast address
const char *bcast_address = get_ipv4_broadcast(); const char *bcast_address = get_ipv4_broadcast();
printf("Broadcast address=%s\n", bcast_address); printf("Broadcast address=%s\n", bcast_address);
inet_aton(bcast_address, &remote_addr.sin_addr); inet_aton(bcast_address, &gcs_addr.sin_addr);
remote_addr.sin_family = AF_INET; gcs_addr.sin_family = AF_INET;
remote_addr.sin_port = htons(UDP_OUT_PORT); gcs_addr.sin_port = htons(UDP_OUT_PORT);
int one = 1; int one = 1;
setsockopt(socket_fd,SOL_SOCKET,SO_BROADCAST,(char *)&one,sizeof(one)); setsockopt(socket_fd,SOL_SOCKET,SO_BROADCAST,(char *)&one,sizeof(one));
@ -203,25 +283,31 @@ int main()
} }
#endif #endif
printf("Waiting for receive\n");
printf("Enter ctrl-c to exit\n"); printf("Enter ctrl-c to exit\n");
_running = true; _running = true;
pthread_t obd_recv_thread_id;
pthread_attr_t obd_recv_thread_attr;
pthread_attr_init(&obd_recv_thread_attr);
pthread_create(&obd_recv_thread_id, &obd_recv_thread_attr, obd_recv_thread, NULL);
uint32_t next_seq = 0; uint32_t next_seq = 0;
printf("Waiting for GCS receive\n");
while (_running) { while (_running) {
struct qurt_rpc_msg msg {}; struct qurt_rpc_msg msg {};
struct sockaddr_in from; struct sockaddr_in from;
socklen_t fromlen = sizeof(from); socklen_t fromlen = sizeof(from);
uint32_t bytes_received = recvfrom(socket_fd, msg.data, sizeof(msg.data), 0, uint32_t bytes_received = recvfrom(gcs_socket_fd, msg.data, sizeof(msg.data), 0,
(struct sockaddr*)&from, &fromlen); (struct sockaddr*)&from, &fromlen);
if (bytes_received > 0 && !connected) { if (bytes_received > 0 && !gcs_connected) {
remote_addr = from; gcs_addr = from;
connected = true; gcs_connected = true;
printf("Connnected to %s\n", inet_ntoa(from.sin_addr)); printf("Connnected to GCS at %s\n", inet_ntoa(from.sin_addr));
} }
if (bytes_received < 0) { if (bytes_received < 0) {
fprintf(stderr, "Received failed"); fprintf(stderr, "GCS receive failed");
continue; continue;
} }
if (bytes_received > sizeof(msg.data)) { if (bytes_received > sizeof(msg.data)) {
@ -229,11 +315,12 @@ int main()
continue; continue;
} }
msg.msg_id = QURT_MSG_ID_MAVLINK_MSG; msg.msg_id = QURT_MSG_ID_MAVLINK_MSG;
msg.inst = 0;
msg.data_length = bytes_received; msg.data_length = bytes_received;
msg.seq = next_seq++; msg.seq = next_seq++;
// printf("Message received. %d bytes\n", bytes_received); // printf("Message received. %d bytes\n", bytes_received);
if (slpi_link_send((const uint8_t*) &msg, bytes_received + QURT_RPC_MSG_HEADER_LEN)) { if (slpi_link_send((const uint8_t*) &msg, bytes_received + QURT_RPC_MSG_HEADER_LEN)) {
fprintf(stderr, "slpi_link_send_data failed\n"); fprintf(stderr, "slpi_link_send_data failed for instance 0\n");
} }
} }

5
libraries/AP_HAL_QURT/ap_host/src/protocol.h
View File

@ -6,12 +6,15 @@
#define QURT_MSG_ID_MAVLINK_MSG 1 #define QURT_MSG_ID_MAVLINK_MSG 1
#define QURT_MSG_ID_REBOOT 2 #define QURT_MSG_ID_REBOOT 2
#define MAX_MAVLINK_INSTANCES 2
struct __attribute__((__packed__)) qurt_rpc_msg { struct __attribute__((__packed__)) qurt_rpc_msg {
uint8_t msg_id; uint8_t msg_id;
uint8_t inst;
uint16_t data_length; uint16_t data_length;
uint32_t seq; uint32_t seq;
uint8_t data[300]; uint8_t data[300];
}; };
#define QURT_RPC_MSG_HEADER_LEN 7 #define QURT_RPC_MSG_HEADER_LEN 8

18
libraries/AP_HAL_QURT/replace.cpp
View File

@ -149,14 +149,18 @@ int slpi_link_client_init(void)
typedef void (*mavlink_data_callback_t)(const struct qurt_rpc_msg *msg, void* p); typedef void (*mavlink_data_callback_t)(const struct qurt_rpc_msg *msg, void* p);
static mavlink_data_callback_t mav_cb; static mavlink_data_callback_t mav_cb[MAX_MAVLINK_INSTANCES];
static void *mav_cb_ptr; static void *mav_cb_ptr[MAX_MAVLINK_INSTANCES];
static uint32_t expected_seq; static uint32_t expected_seq;
void register_mavlink_data_callback(mavlink_data_callback_t func, void *p) void register_mavlink_data_callback(uint8_t instance, mavlink_data_callback_t func, void *p)
{ {
mav_cb = func; if (instance < MAX_MAVLINK_INSTANCES) {
mav_cb_ptr = p; mav_cb[instance] = func;
mav_cb_ptr[instance] = p;
} else {
HAP_PRINTF("Error: Invalid mavlink instance %u", instance);
}
} }
int slpi_link_client_receive(const uint8_t *data, int data_len_in_bytes) int slpi_link_client_receive(const uint8_t *data, int data_len_in_bytes)
@ -175,8 +179,8 @@ int slpi_link_client_receive(const uint8_t *data, int data_len_in_bytes)
switch (msg->msg_id) { switch (msg->msg_id) {
case QURT_MSG_ID_MAVLINK_MSG: { case QURT_MSG_ID_MAVLINK_MSG: {
if (mav_cb) { if ((msg->inst < MAX_MAVLINK_INSTANCES) && (mav_cb[msg->inst])) {
mav_cb(msg, mav_cb_ptr); mav_cb[msg->inst](msg, mav_cb_ptr[msg->inst]);
} }
break; break;
} }