89 lines
2.1 KiB
C++
89 lines
2.1 KiB
C++
|
#include <AP_HAL/AP_HAL.h>
|
||
|
|
#include "UDPDriver.h"
|
||
|
|
#include <stdio.h>
|
||
|
|
#include <stdint.h>
|
||
|
|
#include <unistd.h>
|
||
|
|
#include "Scheduler.h"
|
||
|
|
#include "Storage.h"
|
||
|
|
#include "replace.h"
|
||
|
|
#include <qurt_dsp.h>
|
||
|
|
|
||
|
|
extern const AP_HAL::HAL& hal;
|
||
|
|
|
||
|
|
extern "C" {
|
||
|
|
int ArduPilot_main(int argc, const char *argv[]);
|
||
|
|
}
|
||
|
|
|
||
|
|
volatile int _last_dsp_line = __LINE__;
|
||
|
|
volatile const char *_last_dsp_file = __FILE__;
|
||
|
|
volatile uint32_t _last_counter;
|
||
|
|
|
||
|
|
static void *main_thread(void *)
|
||
|
|
{
|
||
|
|
HAP_PRINTF("Ardupilot main_thread started");
|
||
|
|
ArduPilot_main(0, NULL);
|
||
|
|
return NULL;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
uint32_t ardupilot_start()
|
||
|
|
{
|
||
|
|
HAP_PRINTF("Starting Ardupilot");
|
||
|
|
pthread_attr_t thread_attr;
|
||
|
|
struct sched_param param;
|
||
|
|
pthread_t thread_ctx;
|
||
|
|
|
||
|
|
pthread_attr_init(&thread_attr);
|
||
|
|
pthread_attr_setstacksize(&thread_attr, 80000);
|
||
|
|
|
||
|
|
param.sched_priority = APM_MAIN_PRIORITY;
|
||
|
|
(void)pthread_attr_setschedparam(&thread_attr, ¶m);
|
||
|
|
|
||
|
|
pthread_create(&thread_ctx, &thread_attr, main_thread, NULL);
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
uint32_t ardupilot_heartbeat()
|
||
|
|
{
|
||
|
|
HAP_PRINTF("tick %u %s:%d", (unsigned)_last_counter, _last_dsp_file, _last_dsp_line);
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
uint32_t ardupilot_set_storage(const uint8_t *buf, int len)
|
||
|
|
{
|
||
|
|
if (len != sizeof(QURT::Storage::buffer)) {
|
||
|
|
HAP_PRINTF("Incorrect storage size %u", len);
|
||
|
|
return 1;
|
||
|
|
}
|
||
|
|
QURT::Storage::lock.take(0);
|
||
|
|
memcpy(QURT::Storage::buffer, buf, len);
|
||
|
|
QURT::Storage::lock.give();
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
uint32_t ardupilot_get_storage(uint8_t *buf, int len)
|
||
|
|
{
|
||
|
|
if (len != sizeof(QURT::Storage::buffer)) {
|
||
|
|
HAP_PRINTF("Incorrect storage size %u", len);
|
||
|
|
return 1;
|
||
|
|
}
|
||
|
|
if (!QURT::Storage::dirty) {
|
||
|
|
return 1;
|
||
|
|
}
|
||
|
|
QURT::Storage::lock.take(0);
|
||
|
|
memcpy(buf, QURT::Storage::buffer, len);
|
||
|
|
QURT::Storage::lock.give();
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
uint32_t ardupilot_socket_check(uint8_t *buf, int len, uint32_t *nbytes)
|
||
|
|
{
|
||
|
|
return QURT::UDPDriver::from(hal.uartA)->socket_check(buf, len, nbytes);
|
||
|
|
}
|
||
|
|
|
||
|
|
uint32_t ardupilot_socket_input(const uint8_t *buf, int len, uint32_t *nbytes)
|
||
|
|
{
|
||
|
|
return QURT::UDPDriver::from(hal.uartA)->socket_input(buf, len, nbytes);
|
||
|
|
}
|