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;
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 serial4Driver(QURT_UART_RCIN);
@ -64,7 +65,7 @@ bool qurt_ran_overtime;
HAL_QURT::HAL_QURT() :
AP_HAL::HAL(
&serial0Driver,
nullptr,
&serial1Driver,
nullptr,
&serial3Driver,
&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
*/
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)
@ -182,6 +182,7 @@ bool UARTDriver_MAVLinkUDP::_write_pending_bytes(void)
return false;
}
msg.msg_id = QURT_MSG_ID_MAVLINK_MSG;
msg.inst = inst;
msg.seq = seq++;
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
*/
class QURT::UARTDriver_MAVLinkUDP : public QURT::UARTDriver
{
public:
UARTDriver_MAVLinkUDP(void);
UARTDriver_MAVLinkUDP(uint8_t instance);
bool _write_pending_bytes(void) override;
@ -93,6 +94,7 @@ public:
private:
static void _mavlink_data_cb(const struct qurt_rpc_msg *msg, void *p);
uint8_t inst;
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_remote_debug = false;
static int socket_fd;
static bool connected;
static struct sockaddr_in remote_addr;
static int gcs_socket_fd;
static int obd_socket_fd;
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"
// setup for mavlink to localhost
#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
@ -63,7 +72,7 @@ static void shutdown_signal_handler(int signo)
static void slpi_init(void);
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)
{
@ -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);
return;
}
if (msg->seq != expected_seq) {
printf("Invalid seq %u %u\n", msg->seq, expected_seq);
if (msg->inst < MAX_MAVLINK_INSTANCES) {
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) {
case QURT_MSG_ID_MAVLINK_MSG: {
if (_running) {
const auto bytes_sent = sendto(socket_fd, msg->data, msg->data_length, 0, (struct sockaddr *)&remote_addr, sizeof(remote_addr));
if (bytes_sent <= 0) {
fprintf(stderr, "Send to GCS failed\n");
if (msg->inst == 0) {
const auto bytes_sent = sendto(gcs_socket_fd, msg->data, msg->data_length, 0, (struct sockaddr *)&gcs_addr, sizeof(gcs_addr));
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;
@ -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()
{
printf("Starting up\n");
@ -158,36 +217,57 @@ int main()
slpi_init();
//initialize socket and structure
socket_fd = socket(AF_INET, SOCK_DGRAM, 0);
if (socket_fd == -1) {
fprintf(stderr, "Could not create socket");
//initialize sockets and structures
gcs_socket_fd = socket(AF_INET, SOCK_DGRAM, 0);
if (gcs_socket_fd == -1) {
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;
}
#if MAVLINK_UDP_LOCALHOST
// send to mavlink router on localhost
remote_addr.sin_addr.s_addr = htonl(INADDR_LOOPBACK);
remote_addr.sin_family = AF_INET;
remote_addr.sin_port = htons(MAVLINK_UDP_PORT_REMOTE);
// send to mavlink router on localhost for GCS
gcs_addr.sin_addr.s_addr = htonl(INADDR_LOOPBACK);
gcs_addr.sin_family = AF_INET;
gcs_addr.sin_port = htons(MAVLINK_GCS_UDP_PORT_REMOTE);
struct sockaddr_in local {};
local.sin_addr.s_addr = INADDR_ANY;
local.sin_family = AF_INET;
local.sin_port = htons(MAVLINK_UDP_PORT_LOCAL);
struct sockaddr_in gcs_local {};
gcs_local.sin_addr.s_addr = INADDR_ANY;
gcs_local.sin_family = AF_INET;
gcs_local.sin_port = htons(MAVLINK_GCS_UDP_PORT_LOCAL);
if (bind(socket_fd, (struct sockaddr *)&local, sizeof(local)) == 0) {
printf("Bind localhost OK\n");
if (bind(gcs_socket_fd, (struct sockaddr *)&gcs_local, sizeof(gcs_local)) == 0) {
printf("Bind localhost GCS socket OK\n");
} 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
// broadcast directly to the local network broadcast address
const char *bcast_address = get_ipv4_broadcast();
printf("Broadcast address=%s\n", bcast_address);
inet_aton(bcast_address, &remote_addr.sin_addr);
remote_addr.sin_family = AF_INET;
remote_addr.sin_port = htons(UDP_OUT_PORT);
inet_aton(bcast_address, &gcs_addr.sin_addr);
gcs_addr.sin_family = AF_INET;
gcs_addr.sin_port = htons(UDP_OUT_PORT);
int one = 1;
setsockopt(socket_fd,SOL_SOCKET,SO_BROADCAST,(char *)&one,sizeof(one));
@ -203,25 +283,31 @@ int main()
}
#endif
printf("Waiting for receive\n");
printf("Enter ctrl-c to exit\n");
_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;
printf("Waiting for GCS receive\n");
while (_running) {
struct qurt_rpc_msg msg {};
struct sockaddr_in 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);
if (bytes_received > 0 && !connected) {
remote_addr = from;
connected = true;
printf("Connnected to %s\n", inet_ntoa(from.sin_addr));
if (bytes_received > 0 && !gcs_connected) {
gcs_addr = from;
gcs_connected = true;
printf("Connnected to GCS at %s\n", inet_ntoa(from.sin_addr));
}
if (bytes_received < 0) {
fprintf(stderr, "Received failed");
fprintf(stderr, "GCS receive failed");
continue;
}
if (bytes_received > sizeof(msg.data)) {
@ -229,11 +315,12 @@ int main()
continue;
}
msg.msg_id = QURT_MSG_ID_MAVLINK_MSG;
msg.inst = 0;
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\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_REBOOT 2
#define MAX_MAVLINK_INSTANCES 2
struct __attribute__((__packed__)) qurt_rpc_msg {
uint8_t msg_id;
uint8_t inst;
uint16_t data_length;
uint32_t seq;
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);
static mavlink_data_callback_t mav_cb;
static void *mav_cb_ptr;
static mavlink_data_callback_t mav_cb[MAX_MAVLINK_INSTANCES];
static void *mav_cb_ptr[MAX_MAVLINK_INSTANCES];
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;
mav_cb_ptr = p;
if (instance < MAX_MAVLINK_INSTANCES) {
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)
@ -175,8 +179,8 @@ int slpi_link_client_receive(const uint8_t *data, int data_len_in_bytes)
switch (msg->msg_id) {
case QURT_MSG_ID_MAVLINK_MSG: {
if (mav_cb) {
mav_cb(msg, mav_cb_ptr);
if ((msg->inst < MAX_MAVLINK_INSTANCES) && (mav_cb[msg->inst])) {
mav_cb[msg->inst](msg, mav_cb_ptr[msg->inst]);
}
break;
}