From cd8b93e1c5224bbf16dc6117d0250ac5a7b04177 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 13 Jul 2024 10:45:16 +1000 Subject: [PATCH] HAL_QURT: adjustments to support different frames added docs and service files, and support reboot --- libraries/AP_HAL/board/qurt.h | 6 +- libraries/AP_HAL_QURT/I2CDevice.cpp | 11 ++- libraries/AP_HAL_QURT/RCOutput.cpp | 3 +- libraries/AP_HAL_QURT/Scheduler.cpp | 8 +++ libraries/AP_HAL_QURT/UARTDriver.cpp | 20 +++--- libraries/AP_HAL_QURT/UARTDriver.h | 2 +- .../AP_HAL_QURT/ap_host/service/README.md | 25 +++++++ .../ap_host/service/voxl-ardupilot | 8 +++ .../ap_host/service/voxl-ardupilot.service | 13 ++++ libraries/AP_HAL_QURT/ap_host/src/main.cpp | 70 ++++++++++++++----- libraries/AP_HAL_QURT/ap_host/src/protocol.h | 13 ++-- libraries/AP_HAL_QURT/replace.cpp | 19 +++-- libraries/AP_HAL_QURT/replace.h | 7 ++ 13 files changed, 157 insertions(+), 48 deletions(-) create mode 100644 libraries/AP_HAL_QURT/ap_host/service/README.md create mode 100755 libraries/AP_HAL_QURT/ap_host/service/voxl-ardupilot create mode 100644 libraries/AP_HAL_QURT/ap_host/service/voxl-ardupilot.service diff --git a/libraries/AP_HAL/board/qurt.h b/libraries/AP_HAL/board/qurt.h index b4bd9cc13f..8b1bf38a14 100644 --- a/libraries/AP_HAL/board/qurt.h +++ b/libraries/AP_HAL/board/qurt.h @@ -86,19 +86,19 @@ compass list */ #define PROBE_MAG_I2C(driver, bus, addr, args ...) ADD_BACKEND(DRIVER_ ##driver, AP_Compass_ ## driver::probe(GET_I2C_DEVICE(bus, addr),##args)) -#define HAL_MAG_PROBE_LIST PROBE_MAG_I2C(QMC5883L, 0, 0x0d, false, ROTATION_PITCH_180_YAW_90) +#define HAL_MAG_PROBE_LIST PROBE_MAG_I2C(QMC5883L, 0, 0x0d, true, ROTATION_NONE) /* barometer list */ #define PROBE_BARO_I2C(driver, bus, addr, args ...) ADD_BACKEND(AP_Baro_ ## driver::probe(*this,std::move(GET_I2C_DEVICE(bus, addr)),##args)) -#define HAL_BARO_PROBE_LIST PROBE_BARO_I2C(ICP101XX, 3, 0x63) +#define HAL_BARO_PROBE_LIST PROBE_BARO_I2C(ICP101XX, 2, 0x63) /* IMU list */ #define PROBE_IMU_SPI(driver, devname, args ...) ADD_BACKEND(AP_InertialSensor_ ## driver::probe(*this,hal.spi->get_device(devname),##args)) -#define HAL_INS_PROBE_LIST PROBE_IMU_SPI(Invensensev3, "INV3", ROTATION_ROLL_180) +#define HAL_INS_PROBE_LIST PROBE_IMU_SPI(Invensensev3, "INV3", ROTATION_NONE) /* bring in missing standard library functions diff --git a/libraries/AP_HAL_QURT/I2CDevice.cpp b/libraries/AP_HAL_QURT/I2CDevice.cpp index 312c8defb7..1da0b004f6 100644 --- a/libraries/AP_HAL_QURT/I2CDevice.cpp +++ b/libraries/AP_HAL_QURT/I2CDevice.cpp @@ -27,10 +27,10 @@ using namespace QURT; bus1: mag bus2: power manager - bus4: external spare bus - bus5: barometer (internal) + bus5: barometer (internal)* + bus4: external spare bus (unused) */ -static uint8_t i2c_bus_ids[] = { 1, 2, 4, 5 }; +static uint8_t i2c_bus_ids[] = { 1, 2, 5 }; static uint32_t i2c_internal_mask = (1U<<3); @@ -44,6 +44,11 @@ I2CDevice::I2CDevice(uint8_t busnum, uint8_t address, uint32_t bus_clock, bool u _address(address), bus(I2CDeviceManager::businfo[busnum]) { + if (busnum >= ARRAY_SIZE(i2c_bus_ids)) { + bus.fd = -1; + HAP_PRINTF("Invalid I2C bus %u", unsigned(busnum)); + return; + } HAP_PRINTF("Constructing I2CDevice %u 0x%02x %u", unsigned(busnum), unsigned(address), unsigned(bus_clock)); if (bus.fd == -2) { diff --git a/libraries/AP_HAL_QURT/RCOutput.cpp b/libraries/AP_HAL_QURT/RCOutput.cpp index 65187c329a..9ccaef98a5 100644 --- a/libraries/AP_HAL_QURT/RCOutput.cpp +++ b/libraries/AP_HAL_QURT/RCOutput.cpp @@ -163,8 +163,7 @@ void RCOutput::handle_esc_feedback(const struct esc_response_v2 &pkt) void RCOutput::handle_power_status(const struct esc_power_status &pkt) { esc_voltage = pkt.voltage * 0.001; - const int16_t current_offset = 2048; // why this offset? - esc_current = (pkt.current+current_offset) * 0.008; + esc_current = pkt.current * 0.008; } // check for responses diff --git a/libraries/AP_HAL_QURT/Scheduler.cpp b/libraries/AP_HAL_QURT/Scheduler.cpp index f4fa39aaff..2930861a27 100644 --- a/libraries/AP_HAL_QURT/Scheduler.cpp +++ b/libraries/AP_HAL_QURT/Scheduler.cpp @@ -213,6 +213,14 @@ void Scheduler::reboot(bool hold_in_bootloader) HAP_PRINTF("**** REBOOT REQUESTED ****"); // delay for printf to appear on USB monitor qurt_timer_sleep(10000); + + // tell host we want to reboot + struct qurt_rpc_msg msg {}; + msg.msg_id = QURT_MSG_ID_REBOOT; + qurt_rpc_send(msg); + + // wait for RPC to get through + qurt_timer_sleep(10000); exit(1); } diff --git a/libraries/AP_HAL_QURT/UARTDriver.cpp b/libraries/AP_HAL_QURT/UARTDriver.cpp index 9bb56957f4..d4cfe0ff3c 100644 --- a/libraries/AP_HAL_QURT/UARTDriver.cpp +++ b/libraries/AP_HAL_QURT/UARTDriver.cpp @@ -142,7 +142,7 @@ void UARTDriver_Console::printf(const char *fmt, ...) /* methods for UARTDriver_MAVLinkUDP */ -typedef void (*mavlink_data_callback_t)(const struct qurt_mavlink_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); UARTDriver_MAVLinkUDP::UARTDriver_MAVLinkUDP(void) @@ -150,10 +150,10 @@ UARTDriver_MAVLinkUDP::UARTDriver_MAVLinkUDP(void) register_mavlink_data_callback(_mavlink_data_cb, (void *) this); } -void UARTDriver_MAVLinkUDP::_mavlink_data_cb(const struct qurt_mavlink_msg *msg, void *p) +void UARTDriver_MAVLinkUDP::_mavlink_data_cb(const struct qurt_rpc_msg *msg, void *p) { auto *driver = (UARTDriver_MAVLinkUDP *)p; - driver->_readbuf.write(msg->mav_msg, msg->data_length); + driver->_readbuf.write(msg->data, msg->data_length); } /* @@ -177,17 +177,15 @@ bool UARTDriver_MAVLinkUDP::_write_pending_bytes(void) return false; } - struct qurt_mavlink_msg msg; - if (n > sizeof(msg.mav_msg)) { + struct qurt_rpc_msg msg; + if (n > sizeof(msg.data)) { return false; } + msg.msg_id = QURT_MSG_ID_MAVLINK_MSG; msg.seq = seq++; - msg.data_length = n; - n = _writebuf.read(msg.mav_msg, n); - if (n > 0) { - (void) sl_client_send_data((const uint8_t*)&msg, n + QURT_MAVLINK_MSG_HEADER_LEN); - } - return n > 0; + msg.data_length = _writebuf.read(msg.data, n); + + return qurt_rpc_send(msg); } /* diff --git a/libraries/AP_HAL_QURT/UARTDriver.h b/libraries/AP_HAL_QURT/UARTDriver.h index 61bfeefc0b..c1692808c2 100644 --- a/libraries/AP_HAL_QURT/UARTDriver.h +++ b/libraries/AP_HAL_QURT/UARTDriver.h @@ -92,7 +92,7 @@ public: } private: - static void _mavlink_data_cb(const struct qurt_mavlink_msg *msg, void *p); + static void _mavlink_data_cb(const struct qurt_rpc_msg *msg, void *p); uint32_t seq; }; diff --git a/libraries/AP_HAL_QURT/ap_host/service/README.md b/libraries/AP_HAL_QURT/ap_host/service/README.md new file mode 100644 index 0000000000..63c9d4f83d --- /dev/null +++ b/libraries/AP_HAL_QURT/ap_host/service/README.md @@ -0,0 +1,25 @@ +# ArduPilot on Voxl-2 + +This directory provides a systemd service file for ArduPilot on Voxl2 +by ModalAI + +To build use: + + - ./waf configure --board QURT + - ./waf copter + +To install copy files as follows: + + - voxl-ardupilot.service to /etc/systemd/system/ + - voxl-ardupilot to /usr/bin/ + - build/QURT/ardupilot to /usr/bin/ + - build/QURT/bin/arducopter to /usr/lib/rfsa/adsp/ArduPilot.so + - copy the right parameter file from Tools/Frame_params/ModalAI/ to /data/APM/defaults.parm + +You can then use: + + - systemctl enable voxl-ardupilot.service + - systemctl start voxl-ardupilot + + + diff --git a/libraries/AP_HAL_QURT/ap_host/service/voxl-ardupilot b/libraries/AP_HAL_QURT/ap_host/service/voxl-ardupilot new file mode 100755 index 0000000000..441a53a413 --- /dev/null +++ b/libraries/AP_HAL_QURT/ap_host/service/voxl-ardupilot @@ -0,0 +1,8 @@ +#!/bin/bash + +print_usage() { + echo -e "\nUsage: voxl-ardupilot" + exit 1 +} + +/usr/bin/ardupilot diff --git a/libraries/AP_HAL_QURT/ap_host/service/voxl-ardupilot.service b/libraries/AP_HAL_QURT/ap_host/service/voxl-ardupilot.service new file mode 100644 index 0000000000..c1326183ac --- /dev/null +++ b/libraries/AP_HAL_QURT/ap_host/service/voxl-ardupilot.service @@ -0,0 +1,13 @@ +[Unit] +Description=ArduPilot +After=sscrpcd.service +Requires=sscrpcd.service + +[Service] +Restart=always +RestartSec=1s +ExecStartPre=/bin/sleep 1 +ExecStart=/usr/bin/voxl-ardupilot + +[Install] +WantedBy=multi-user.target diff --git a/libraries/AP_HAL_QURT/ap_host/src/main.cpp b/libraries/AP_HAL_QURT/ap_host/src/main.cpp index cd57c3af9b..7fa3f2d837 100644 --- a/libraries/AP_HAL_QURT/ap_host/src/main.cpp +++ b/libraries/AP_HAL_QURT/ap_host/src/main.cpp @@ -27,9 +27,14 @@ static int socket_fd; static bool connected; static struct sockaddr_in remote_addr; -#define UDP_OUT_PORT 14551 #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 + + // directory for logs, parameters, terrain etc #define DATA_DIRECTORY "/data/APM" @@ -55,6 +60,8 @@ static void shutdown_signal_handler(int signo) return; } +static void slpi_init(void); + static uint32_t num_params = 0; static uint32_t expected_seq = 0; @@ -63,12 +70,12 @@ static void receive_callback(const uint8_t *data, uint32_t length_in_bytes) if (enable_debug) { printf("Got %u bytes in receive callback\n", length_in_bytes); } - const auto *msg = (const struct qurt_mavlink_msg *)data; - if (length_in_bytes < QURT_MAVLINK_MSG_HEADER_LEN) { + const auto *msg = (const struct qurt_rpc_msg *)data; + if (length_in_bytes < QURT_RPC_MSG_HEADER_LEN) { printf("Invalid length_in_bytes %d", length_in_bytes); return; } - if (msg->data_length + QURT_MAVLINK_MSG_HEADER_LEN != length_in_bytes) { + if (msg->data_length + QURT_RPC_MSG_HEADER_LEN != length_in_bytes) { printf("Invalid lengths %d %d\n", msg->data_length, length_in_bytes); return; } @@ -80,19 +87,33 @@ static void receive_callback(const uint8_t *data, uint32_t length_in_bytes) switch (msg->msg_id) { case QURT_MSG_ID_MAVLINK_MSG: { if (_running) { - const auto bytes_sent = sendto(socket_fd, msg->mav_msg, msg->data_length, 0, (struct sockaddr *)&remote_addr, sizeof(remote_addr)); + 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"); } } break; } + case QURT_MSG_ID_REBOOT: { + printf("Rebooting\n"); + exit(0); + break; + } default: fprintf(stderr, "Got unknown message id %d\n", msg->msg_id); break; } } +static void slpi_init(void) +{ + int r; + while ((r=slpi_link_init(enable_remote_debug, &receive_callback, SO_NAME)) != 0) { + fprintf(stderr, "slpi_link_init error %d %s, retrying\n", r, strerror(errno)); + sleep(1); + } +} + /* setup directories for the hexagon code to use */ @@ -135,14 +156,7 @@ int main() return -1; } - if (slpi_link_init(enable_remote_debug, &receive_callback, SO_NAME) != 0) { - fprintf(stderr, "Error with init\n"); - sleep(1); - slpi_link_reset(); - return -1; - } else if (enable_debug) { - printf("slpi_link_initialize succeeded\n"); - } + slpi_init(); //initialize socket and structure socket_fd = socket(AF_INET, SOCK_DGRAM, 0); @@ -151,6 +165,24 @@ int main() 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); + + struct sockaddr_in local {}; + local.sin_addr.s_addr = INADDR_ANY; + local.sin_family = AF_INET; + local.sin_port = htons(MAVLINK_UDP_PORT_LOCAL); + + if (bind(socket_fd, (struct sockaddr *)&local, sizeof(local)) == 0) { + printf("Bind localhost OK\n"); + } else { + printf("Bind 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); @@ -166,7 +198,10 @@ int main() if (bind(socket_fd, (struct sockaddr *)&any, sizeof(any)) == 0) { printf("Bind OK\n"); + } else { + printf("Bind failed: %s", strerror(errno)); } +#endif printf("Waiting for receive\n"); @@ -175,10 +210,10 @@ int main() uint32_t next_seq = 0; while (_running) { - struct qurt_mavlink_msg msg {}; + struct qurt_rpc_msg msg {}; struct sockaddr_in from; socklen_t fromlen = sizeof(from); - uint32_t bytes_received = recvfrom(socket_fd, msg.mav_msg, sizeof(msg.mav_msg), 0, + uint32_t bytes_received = recvfrom(socket_fd, msg.data, sizeof(msg.data), 0, (struct sockaddr*)&from, &fromlen); if (bytes_received > 0 && !connected) { remote_addr = from; @@ -189,14 +224,15 @@ int main() fprintf(stderr, "Received failed"); continue; } - if (bytes_received > sizeof(msg.mav_msg)) { + if (bytes_received > sizeof(msg.data)) { printf("Invalid bytes_received %d\n", bytes_received); continue; } + msg.msg_id = QURT_MSG_ID_MAVLINK_MSG; 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_MAVLINK_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"); } } diff --git a/libraries/AP_HAL_QURT/ap_host/src/protocol.h b/libraries/AP_HAL_QURT/ap_host/src/protocol.h index f3b3d3eb4a..3b10ba3bf1 100644 --- a/libraries/AP_HAL_QURT/ap_host/src/protocol.h +++ b/libraries/AP_HAL_QURT/ap_host/src/protocol.h @@ -1,18 +1,17 @@ #include -// #include -#define MAVLINK_MAX_PAYLOAD_LEN 255 - #pragma once #define QURT_MSG_ID_MAVLINK_MSG 1 +#define QURT_MSG_ID_REBOOT 2 -struct __attribute__((__packed__)) qurt_mavlink_msg { - uint8_t msg_id{QURT_MSG_ID_MAVLINK_MSG}; +struct __attribute__((__packed__)) qurt_rpc_msg { + uint8_t msg_id; uint16_t data_length; uint32_t seq; - uint8_t mav_msg[MAVLINK_MAX_PAYLOAD_LEN]; + uint8_t data[300]; }; -#define QURT_MAVLINK_MSG_HEADER_LEN 7 +#define QURT_RPC_MSG_HEADER_LEN 7 + diff --git a/libraries/AP_HAL_QURT/replace.cpp b/libraries/AP_HAL_QURT/replace.cpp index 0425079b89..9da9f6428a 100644 --- a/libraries/AP_HAL_QURT/replace.cpp +++ b/libraries/AP_HAL_QURT/replace.cpp @@ -147,7 +147,7 @@ int slpi_link_client_init(void) return 0; } -typedef void (*mavlink_data_callback_t)(const struct qurt_mavlink_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 void *mav_cb_ptr; @@ -161,11 +161,11 @@ void register_mavlink_data_callback(mavlink_data_callback_t func, void *p) int slpi_link_client_receive(const uint8_t *data, int data_len_in_bytes) { - if (data_len_in_bytes < QURT_MAVLINK_MSG_HEADER_LEN) { + if (data_len_in_bytes < QURT_RPC_MSG_HEADER_LEN) { return 0; } - const auto *msg = (struct qurt_mavlink_msg *)data; - if (msg->data_length + QURT_MAVLINK_MSG_HEADER_LEN != data_len_in_bytes) { + const auto *msg = (struct qurt_rpc_msg *)data; + if (msg->data_length + QURT_RPC_MSG_HEADER_LEN != data_len_in_bytes) { return 0; } if (msg->seq != expected_seq) { @@ -208,3 +208,14 @@ char *__wrap_strdup(const char *s) { return strndup(s, strlen(s)); } + +/* + send a RPC message to the host + */ +bool qurt_rpc_send(struct qurt_rpc_msg &msg) +{ + if (msg.data_length > sizeof(msg.data)) { + return false; + } + return sl_client_send_data((const uint8_t*)&msg, msg.data_length + QURT_RPC_MSG_HEADER_LEN) >= 0; +} diff --git a/libraries/AP_HAL_QURT/replace.h b/libraries/AP_HAL_QURT/replace.h index ca9b2a035b..0fb943e6ee 100644 --- a/libraries/AP_HAL_QURT/replace.h +++ b/libraries/AP_HAL_QURT/replace.h @@ -9,6 +9,7 @@ #include #include +#include "ap_host/src/protocol.h" #ifdef __cplusplus extern "C" { @@ -47,3 +48,9 @@ extern volatile uint32_t _last_counter; // missing defines from math.h #define M_SQRT1_2 0.70710678118654752440 + +#ifdef __cplusplus +// send a message to the host +bool qurt_rpc_send(struct qurt_rpc_msg &msg); +#endif +