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

HAL_QURT: adjustments to support different frames 

added docs and service files, and support reboot
This commit is contained in:
Andrew Tridgell 2024-07-13 10:45:16 +10:00
parent 42737116bf
commit cd8b93e1c5
13 changed files with 157 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_HAL/board/qurt.h
View File

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

11
libraries/AP_HAL_QURT/I2CDevice.cpp
View File

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

3
libraries/AP_HAL_QURT/RCOutput.cpp
View File

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

8
libraries/AP_HAL_QURT/Scheduler.cpp
View File

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

20
libraries/AP_HAL_QURT/UARTDriver.cpp
View File

@ -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);
}
/*

2
libraries/AP_HAL_QURT/UARTDriver.h
View File

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

25
libraries/AP_HAL_QURT/ap_host/service/README.md Normal file
View File

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

8
libraries/AP_HAL_QURT/ap_host/service/voxl-ardupilot Executable file
View File

@ -0,0 +1,8 @@
#!/bin/bash
print_usage() {
echo -e "\nUsage: voxl-ardupilot"
exit 1
}
/usr/bin/ardupilot

13
libraries/AP_HAL_QURT/ap_host/service/voxl-ardupilot.service Normal file
View File

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

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

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

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

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

19
libraries/AP_HAL_QURT/replace.cpp
View File

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

7
libraries/AP_HAL_QURT/replace.h
View File

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