HAL_QURT: adjustments to support different frames

added docs and service files, and support reboot

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

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) {

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 * 0.008;
-    esc_current = (pkt.current+current_offset) * 0.008;
 }
 
 // check for responses

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);
 }
 

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;
+    struct qurt_rpc_msg msg;
-    if (n > sizeof(msg.mav_msg)) {
+    if (n > sizeof(msg.data)) {
         return false;
     }
+    msg.msg_id = QURT_MSG_ID_MAVLINK_MSG;
     msg.seq = seq++;
-    msg.data_length = n;
+    msg.data_length = _writebuf.read(msg.data, n);
-    n = _writebuf.read(msg.mav_msg, n);
+
-    if (n > 0) {
+    return qurt_rpc_send(msg);
-        (void) sl_client_send_data((const uint8_t*)&msg, n + QURT_MAVLINK_MSG_HEADER_LEN);
-    }
-    return n > 0;
 }
 
 /*

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;
 };
 

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
+
+
+

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

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

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;
+    const auto *msg = (const struct qurt_rpc_msg *)data;
-    if (length_in_bytes < QURT_MAVLINK_MSG_HEADER_LEN) {
+    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) {
+    slpi_init();
-        fprintf(stderr, "Error with init\n");
-        sleep(1);
-        slpi_link_reset();
-        return -1;
-    } else if (enable_debug) {
-        printf("slpi_link_initialize succeeded\n");
-    }
 
     //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");
         }
     }

libraries/AP_HAL_QURT/ap_host/src/protocol.h

@@ -1,18 +1,17 @@
 
 #include <stdint.h>
 
-// #include <GCS_MAVLink/GCS_MAVLink.h>
-#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 {
+struct __attribute__((__packed__)) qurt_rpc_msg {
-    uint8_t msg_id{QURT_MSG_ID_MAVLINK_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
+

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;
+    const auto *msg = (struct qurt_rpc_msg *)data;
-    if (msg->data_length + QURT_MAVLINK_MSG_HEADER_LEN != data_len_in_bytes) {
+    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;
+}

libraries/AP_HAL_QURT/replace.h

@@ -9,6 +9,7 @@
 
 #include <types.h>
 #include <dirent.h>
+#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
+