mirror of https://github.com/ArduPilot/ardupilot
211 lines
4.5 KiB
C++
211 lines
4.5 KiB
C++
#include <stdio.h>
|
|
#include <stdint.h>
|
|
#include <stdlib.h>
|
|
#include <unistd.h>
|
|
#include <sys/types.h>
|
|
#include <sys/stat.h>
|
|
#include <fcntl.h>
|
|
#include <dirent.h>
|
|
#include <pthread.h>
|
|
#include "replace.h"
|
|
#include "interface.h"
|
|
#include "ap_host/src/protocol.h"
|
|
|
|
extern "C" {
|
|
|
|
// this is not declared in qurt headers
|
|
void HAP_debug(const char *msg, int level, const char *filename, int line);
|
|
|
|
void HAP_printf(const char *file, int line, const char *format, ...)
|
|
{
|
|
va_list ap;
|
|
char buf[300];
|
|
|
|
va_start(ap, format);
|
|
vsnprintf(buf, sizeof(buf), format, ap);
|
|
va_end(ap);
|
|
HAP_debug(buf, 0, file, line);
|
|
//usleep(20000);
|
|
}
|
|
|
|
/**
|
|
QURT doesn't have strnlen()
|
|
**/
|
|
size_t strnlen(const char *s, size_t max)
|
|
{
|
|
size_t len;
|
|
|
|
for (len = 0; len < max; len++) {
|
|
if (s[len] == '\0') {
|
|
break;
|
|
}
|
|
}
|
|
return len;
|
|
}
|
|
|
|
int vasprintf(char **ptr, const char *format, va_list ap)
|
|
{
|
|
int ret;
|
|
va_list ap2;
|
|
|
|
va_copy(ap2, ap);
|
|
ret = vsnprintf(nullptr, 0, format, ap2);
|
|
va_end(ap2);
|
|
if (ret < 0) {
|
|
return ret;
|
|
}
|
|
|
|
(*ptr) = (char *)malloc(ret+1);
|
|
if (!*ptr) {
|
|
return -1;
|
|
}
|
|
|
|
va_copy(ap2, ap);
|
|
ret = vsnprintf(*ptr, ret+1, format, ap2);
|
|
va_end(ap2);
|
|
|
|
return ret;
|
|
}
|
|
|
|
int asprintf(char **ptr, const char *format, ...)
|
|
{
|
|
va_list ap;
|
|
int ret;
|
|
|
|
*ptr = nullptr;
|
|
va_start(ap, format);
|
|
ret = vasprintf(ptr, format, ap);
|
|
va_end(ap);
|
|
|
|
return ret;
|
|
}
|
|
|
|
void *memmem(const void *haystack, size_t haystacklen,
|
|
const void *needle, size_t needlelen)
|
|
{
|
|
if (needlelen == 0) {
|
|
return const_cast<void*>(haystack);
|
|
}
|
|
while (haystacklen >= needlelen) {
|
|
char *p = (char *)memchr(haystack, *(const char *)needle,
|
|
haystacklen-(needlelen-1));
|
|
if (!p) {
|
|
return NULL;
|
|
}
|
|
if (memcmp(p, needle, needlelen) == 0) {
|
|
return p;
|
|
}
|
|
haystack = p+1;
|
|
haystacklen -= (p - (const char *)haystack) + 1;
|
|
}
|
|
return NULL;
|
|
}
|
|
|
|
char *strndup(const char *s, size_t n)
|
|
{
|
|
char *ret;
|
|
|
|
n = strnlen(s, n);
|
|
ret = (char*)malloc(n+1);
|
|
if (!ret) {
|
|
return NULL;
|
|
}
|
|
memcpy(ret, s, n);
|
|
ret[n] = 0;
|
|
|
|
return ret;
|
|
}
|
|
|
|
int pthread_cond_init(pthread_cond_t *cond, pthread_condattr_t *attr)
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
// INVESTIGATE: What is this needed on QURT?
|
|
int apfs_rename(const char *oldpath, const char *newpath)
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
// INVESTIGATE: Seems important :-)
|
|
int ArduPilot_main(int argc, const char *argv[])
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
char *__wrap_strdup(const char *s);
|
|
}
|
|
|
|
extern "C" int qurt_ardupilot_main(int argc, char* const argv[]);
|
|
|
|
int slpi_link_client_init(void)
|
|
{
|
|
HAP_PRINTF("About to call qurt_ardupilot_main %p", &qurt_ardupilot_main);
|
|
|
|
qurt_ardupilot_main(0, NULL);
|
|
|
|
return 0;
|
|
}
|
|
|
|
typedef void (*mavlink_data_callback_t)(const struct qurt_mavlink_msg *msg, void* p);
|
|
|
|
static mavlink_data_callback_t mav_cb;
|
|
static void *mav_cb_ptr;
|
|
static uint32_t expected_seq;
|
|
|
|
void register_mavlink_data_callback(mavlink_data_callback_t func, void *p)
|
|
{
|
|
mav_cb = func;
|
|
mav_cb_ptr = 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) {
|
|
return 0;
|
|
}
|
|
const auto *msg = (struct qurt_mavlink_msg *)data;
|
|
if (msg->data_length + QURT_MAVLINK_MSG_HEADER_LEN != data_len_in_bytes) {
|
|
return 0;
|
|
}
|
|
if (msg->seq != expected_seq) {
|
|
HAP_PRINTF("Bad sequence %u %u", msg->seq, expected_seq);
|
|
}
|
|
expected_seq = msg->seq + 1;
|
|
|
|
switch (msg->msg_id) {
|
|
case QURT_MSG_ID_MAVLINK_MSG: {
|
|
if (mav_cb) {
|
|
mav_cb(msg, mav_cb_ptr);
|
|
}
|
|
break;
|
|
}
|
|
default:
|
|
HAP_PRINTF("Got unknown message id %d", msg->msg_id);
|
|
break;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
int __wrap_printf(const char *fmt, ...)
|
|
{
|
|
va_list ap;
|
|
|
|
char buf[300];
|
|
va_start(ap, fmt);
|
|
vsnprintf(buf, sizeof(buf), fmt, ap);
|
|
va_end(ap);
|
|
HAP_PRINTF(buf);
|
|
|
|
return 0;
|
|
}
|
|
|
|
/*
|
|
free() on strdup return on QURT causes a memory fault
|
|
*/
|
|
char *__wrap_strdup(const char *s)
|
|
{
|
|
return strndup(s, strlen(s));
|
|
}
|