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

HAL_QURT: initial implementation 

this provides RC onput, RC output, scheduling, storage, UARTs and all
necessary support routines to fly ArduPilot on QURT
This commit is contained in:
Andrew Tridgell 2015-03-24 16:28:09 -07:00
parent 3b546bb242
commit 0816937ab1
28 changed files with 2286 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
libraries/AP_HAL_QURT/AP_HAL_QURT.h Normal file
View File

@ -0,0 +1,30 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef __AP_HAL_QURT_H__
#define __AP_HAL_QURT_H__
/* Your layer exports should depend on AP_HAL.h ONLY. */
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_QURT
#include "HAL_QURT_Class.h"
#include "AP_HAL_QURT_Main.h"
#endif // CONFIG_HAL_BOARD
#endif //__AP_HAL_QURT_H__

19
libraries/AP_HAL_QURT/AP_HAL_QURT_Main.h Normal file
View File

@ -0,0 +1,19 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef __AP_HAL_QURT_MAIN_H__
#define __AP_HAL_QURT_MAIN_H__
#endif // __AP_HAL_QURT_MAIN_H__

29
libraries/AP_HAL_QURT/AP_HAL_QURT_Namespace.h Normal file
View File

@ -0,0 +1,29 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
namespace QURT {
class UARTDriver;
class UDPDriver;
class Util;
class Scheduler;
class Storage;
class Util;
class Semaphore;
class RCInput;
class RCOutput;
}

28
libraries/AP_HAL_QURT/AP_HAL_QURT_Private.h Normal file
View File

@ -0,0 +1,28 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef __AP_HAL_QURT_PRIVATE_H__
#define __AP_HAL_QURT_PRIVATE_H__
/* Umbrella header for all private headers of the AP_HAL_QURT module.
* Only import this header from inside AP_HAL_QURT
*/
#include "UARTDriver.h"
#include "UDPDriver.h"
#include "Util.h"
#endif // __AP_HAL_QURT_PRIVATE_H__

102
libraries/AP_HAL_QURT/HAL_QURT_Class.cpp Normal file
View File

@ -0,0 +1,102 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_QURT
#include "HAL_QURT_Class.h"
#include "AP_HAL_QURT_Private.h"
#include "Scheduler.h"
#include "Storage.h"
#include "Semaphores.h"
#include "RCInput.h"
#include "RCOutput.h"
#include <AP_HAL_Empty/AP_HAL_Empty.h>
#include <AP_HAL_Empty/AP_HAL_Empty_Private.h>
#include <assert.h>
using namespace QURT;
static UDPDriver uartADriver;
static UARTDriver uartBDriver("/dev/tty-4");
static UARTDriver uartCDriver("/dev/tty-2");
static Empty::UARTDriver uartDDriver;
static Semaphore i2cSemaphore;
static Empty::I2CDriver i2cDriver(&i2cSemaphore);
static Empty::SPIDeviceManager spiDeviceManager;
static Empty::AnalogIn analogIn;
static Storage storageDriver;
static Empty::GPIO gpioDriver;
static RCInput rcinDriver("/dev/tty-1");
static RCOutput rcoutDriver("/dev/tty-3");
static Util utilInstance;
static Scheduler schedulerInstance;
bool qurt_ran_overtime;
HAL_QURT::HAL_QURT() :
AP_HAL::HAL(
&uartADriver,
&uartBDriver,
&uartCDriver,
&uartDDriver,
NULL, /* no uartE */
&i2cDriver,
NULL, /* only one i2c */
NULL, /* only one i2c */
&spiDeviceManager,
&analogIn,
&storageDriver,
&uartADriver,
&gpioDriver,
&rcinDriver,
&rcoutDriver,
&schedulerInstance,
&utilInstance,
NULL)
{
}
void HAL_QURT::run(int argc, char* const argv[], Callbacks* callbacks) const
{
assert(callbacks);
/* initialize all drivers and private members here.
* up to the programmer to do this in the correct order.
* Scheduler should likely come first. */
scheduler->init();
schedulerInstance.hal_initialized();
uartA->begin(115200);
rcinDriver.init();
callbacks->setup();
scheduler->system_initialized();
for (;;) {
callbacks->loop();
}
}
const AP_HAL::HAL& AP_HAL::get_HAL() {
static const HAL_QURT *hal;
if (hal == nullptr) {
hal = new HAL_QURT;
HAP_PRINTF("allocated HAL_QURT of size %u", sizeof(*hal));
}
return *hal;
}
#endif

30
libraries/AP_HAL_QURT/HAL_QURT_Class.h Normal file
View File

@ -0,0 +1,30 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef __AP_HAL_QURT_CLASS_H__
#define __AP_HAL_QURT_CLASS_H__
#include <AP_HAL/AP_HAL.h>
#include "AP_HAL_QURT_Namespace.h"
class HAL_QURT : public AP_HAL::HAL {
public:
HAL_QURT();
void run(int argc, char* const* argv, Callbacks* callbacks) const override;
};
#endif // __AP_HAL_QURT_CLASS_H__

193
libraries/AP_HAL_QURT/RCInput.cpp Normal file
View File

@ -0,0 +1,193 @@
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_QURT
#include <stdio.h>
#include <sys/time.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <errno.h>
#include <unistd.h>
#include <fcntl.h>
#include <stdint.h>
#include <dev_fs_lib_serial.h>
#include "RCInput.h"
#include <AP_HAL/utility/dsm.h>
extern const AP_HAL::HAL& hal;
using namespace QURT;
RCInput::RCInput(const char *_device_path) :
device_path(_device_path),
new_rc_input(false)
{
}
extern "C" {
static void read_callback_trampoline(void *, char *, size_t );
}
void RCInput::init()
{
if (device_path == nullptr) {
return;
}
fd = open(device_path, O_RDONLY|O_NONBLOCK);
if (fd == -1) {
AP_HAL::panic("Unable to open RC input %s", device_path);
}
struct dspal_serial_ioctl_data_rate rate;
rate.bit_rate = DSPAL_SIO_BITRATE_115200;
int ret = ioctl(fd, SERIAL_IOCTL_SET_DATA_RATE, (void *)&rate);
struct dspal_serial_ioctl_receive_data_callback callback;
callback.context = this;
callback.rx_data_callback_func_ptr = read_callback_trampoline;
ret = ioctl(fd, SERIAL_IOCTL_SET_RECEIVE_DATA_CALLBACK, (void *)&callback);
}
static void read_callback_trampoline(void *ctx, char *buf, size_t size)
{
((RCInput *)ctx)->read_callback(buf, size);
}
/*
callback for incoming data
*/
void RCInput::read_callback(char *buf, size_t size)
{
add_dsm_input((const uint8_t *)buf, size);
}
bool RCInput::new_input()
{
return new_rc_input;
}
uint8_t RCInput::num_channels()
{
return _num_channels;
}
uint16_t RCInput::read(uint8_t ch)
{
new_rc_input = false;
if (_override[ch]) {
return _override[ch];
}
if (ch >= _num_channels) {
return 0;
}
return _pwm_values[ch];
}
uint8_t RCInput::read(uint16_t* periods, uint8_t len)
{
uint8_t i;
for (i=0; i<len; i++) {
if((periods[i] = read(i))){
continue;
}
else{
break;
}
}
return (i+1);
}
bool RCInput::set_overrides(int16_t *overrides, uint8_t len)
{
bool res = false;
if(len > QURT_RC_INPUT_NUM_CHANNELS){
len = QURT_RC_INPUT_NUM_CHANNELS;
}
for (uint8_t i = 0; i < len; i++) {
res |= set_override(i, overrides[i]);
}
return res;
}
bool RCInput::set_override(uint8_t channel, int16_t override)
{
if (override < 0) return false; /* -1: no change. */
if (channel < QURT_RC_INPUT_NUM_CHANNELS) {
_override[channel] = override;
if (override != 0) {
new_rc_input = true;
return true;
}
}
return false;
}
void RCInput::clear_overrides()
{
for (uint8_t i = 0; i < QURT_RC_INPUT_NUM_CHANNELS; i++) {
_override[i] = 0;
}
}
/*
add some bytes of input in DSM serial stream format, coping with partial packets
*/
void RCInput::add_dsm_input(const uint8_t *bytes, size_t nbytes)
{
if (nbytes == 0) {
return;
}
const uint8_t dsm_frame_size = sizeof(dsm.frame);
uint32_t now = AP_HAL::millis();
if (now - dsm.last_input_ms > 5) {
// resync based on time
dsm.partial_frame_count = 0;
}
dsm.last_input_ms = now;
while (nbytes > 0) {
size_t n = nbytes;
if (dsm.partial_frame_count + n > dsm_frame_size) {
n = dsm_frame_size - dsm.partial_frame_count;
}
if (n > 0) {
memcpy(&dsm.frame[dsm.partial_frame_count], bytes, n);
dsm.partial_frame_count += n;
nbytes -= n;
bytes += n;
}
if (dsm.partial_frame_count == dsm_frame_size) {
dsm.partial_frame_count = 0;
uint16_t values[16] {};
uint16_t num_values=0;
if (dsm_decode(AP_HAL::micros64(), dsm.frame, values, &num_values, 16) &&
num_values >= 5) {
for (uint8_t i=0; i<num_values; i++) {
if (values[i] != 0) {
_pwm_values[i] = values[i];
}
}
/*
the apparent number of channels can change on DSM,
as they are spread across multiple frames. We just
use the max num_values we get
*/
if (num_values > _num_channels) {
_num_channels = num_values;
}
new_rc_input = true;
#if 0
HAP_PRINTF("Decoded DSM %u channels %u %u %u %u %u %u %u %u\n",
(unsigned)num_values,
values[0], values[1], values[2], values[3], values[4], values[5], values[6], values[7]);
#endif
}
}
}
}
#endif // CONFIG_HAL_BOARD

49
libraries/AP_HAL_QURT/RCInput.h Normal file
View File

@ -0,0 +1,49 @@
#pragma once
#include "AP_HAL_QURT.h"
#define QURT_RC_INPUT_NUM_CHANNELS 16
class QURT::RCInput : public AP_HAL::RCInput {
public:
RCInput(const char *device_path);
static RCInput *from(AP_HAL::RCInput *rcinput) {
return static_cast<RCInput*>(rcinput);
}
void init();
bool new_input();
uint8_t num_channels();
uint16_t read(uint8_t ch);
uint8_t read(uint16_t* periods, uint8_t len);
bool set_overrides(int16_t *overrides, uint8_t len);
bool set_override(uint8_t channel, int16_t override);
void clear_overrides();
void read_callback(char *buf, size_t size);
private:
volatile bool new_rc_input;
uint16_t _pwm_values[QURT_RC_INPUT_NUM_CHANNELS];
uint8_t _num_channels;
/* override state */
uint16_t _override[QURT_RC_INPUT_NUM_CHANNELS];
// add some DSM input bytes, for RCInput over a serial port
void add_dsm_input(const uint8_t *bytes, size_t nbytes);
const char *device_path;
int32_t fd = -1;
// state of add_dsm_input
struct {
uint8_t frame[16];
uint8_t partial_frame_count;
uint32_t last_input_ms;
} dsm;
};

113
libraries/AP_HAL_QURT/RCOutput.cpp Normal file
View File

@ -0,0 +1,113 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_QURT
#include "RCOutput.h"
#include <GCS_MAVLink/include/mavlink/v1.0/checksum.h>
#include <stdio.h>
#include <fcntl.h>
#include <unistd.h>
#include <dev_fs_lib_serial.h>
extern const AP_HAL::HAL& hal;
using namespace QURT;
void RCOutput::init()
{
}
void RCOutput::set_freq(uint32_t chmask, uint16_t freq_hz)
{
// no support for changing frequency yet
}
uint16_t RCOutput::get_freq(uint8_t ch)
{
// return fixed fake value
return 490;
}
void RCOutput::enable_ch(uint8_t ch)
{
if (ch >= channel_count) {
return;
}
enable_mask |= 1U<<ch;
}
void RCOutput::disable_ch(uint8_t ch)
{
if (ch >= channel_count) {
return;
}
enable_mask &= ~1U<<ch;
}
void RCOutput::write(uint8_t ch, uint16_t period_us)
{
if (ch >= channel_count) {
return;
}
period[ch] = period_us;
need_write = true;
}
uint16_t RCOutput::read(uint8_t ch)
{
if (ch >= channel_count) {
return 0;
}
return period[ch];
}
void RCOutput::read(uint16_t *period_us, uint8_t len)
{
for (int i = 0; i < len; i++) {
period_us[i] = read(i);
}
}
extern "C" {
// discard incoming data
static void read_callback_trampoline(void *, char *, size_t ) {}
}
void RCOutput::timer_update(void)
{
if (fd == -1 && device_path != nullptr) {
HAP_PRINTF("Opening RCOutput %s", device_path);
fd = open(device_path, O_RDWR|O_NONBLOCK);
if (fd == -1) {
AP_HAL::panic("Unable to open %s", device_path);
}
HAP_PRINTF("Opened ESC UART %s fd=%d\n", device_path, fd);
if (fd != -1) {
struct dspal_serial_ioctl_data_rate rate;
rate.bit_rate = DSPAL_SIO_BITRATE_115200;
ioctl(fd, SERIAL_IOCTL_SET_DATA_RATE, (void *)&rate);
struct dspal_serial_ioctl_receive_data_callback callback;
callback.context = this;
callback.rx_data_callback_func_ptr = read_callback_trampoline;
ioctl(fd, SERIAL_IOCTL_SET_RECEIVE_DATA_CALLBACK, (void *)&callback);
}
}
if (!need_write || fd == -1) {
return;
}
struct PACKED {
uint8_t magic = 0xF7;
uint16_t period[channel_count];
uint16_t crc;
} frame;
memcpy(frame.period, period, sizeof(period));
frame.crc = crc_calculate((uint8_t*)frame.period, channel_count*2);
need_write = false;
::write(fd, (uint8_t *)&frame, sizeof(frame));
}
#endif // CONFIG_HAL_BOARD_SUBTYPE

36
libraries/AP_HAL_QURT/RCOutput.h Normal file
View File

@ -0,0 +1,36 @@
#pragma once
#include <AP_HAL/AP_HAL.h>
#include "AP_HAL_QURT.h"
#if CONFIG_HAL_BOARD == HAL_BOARD_QURT
class QURT::RCOutput : public AP_HAL::RCOutput {
public:
RCOutput(const char *_device_path) {
device_path = _device_path;
}
void init();
void set_freq(uint32_t chmask, uint16_t freq_hz);
uint16_t get_freq(uint8_t ch);
void enable_ch(uint8_t ch);
void disable_ch(uint8_t ch);
void write(uint8_t ch, uint16_t period_us);
uint16_t read(uint8_t ch);
void read(uint16_t *period_us, uint8_t len);
void timer_update(void);
private:
const char *device_path;
const uint32_t baudrate = 115200;
static const uint8_t channel_count = 4;
int fd = -1;
uint16_t enable_mask;
uint16_t period[channel_count];
volatile bool need_write;
};
#endif // CONFIG_HAL_BOARD

291
libraries/AP_HAL_QURT/Scheduler.cpp Normal file
View File

@ -0,0 +1,291 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_QURT
#include "AP_HAL_QURT.h"
#include "Scheduler.h"
#include <sys/stat.h>
#include <sys/types.h>
#include <unistd.h>
#include <stdlib.h>
#include <sched.h>
#include <errno.h>
#include <stdio.h>
#include <dspal/include/pthread.h>
#include <dspal_types.h>
#include "UARTDriver.h"
//#include "AnalogIn.h"
#include "Storage.h"
#include "RCOutput.h"
#include <AP_Scheduler/AP_Scheduler.h>
using namespace QURT;
extern const AP_HAL::HAL& hal;
Scheduler::Scheduler()
{
}
void Scheduler::init()
{
_main_task_pid = getpid();
// setup the timer thread - this will call tasks at 1kHz
pthread_attr_t thread_attr;
struct sched_param param;
pthread_attr_init(&thread_attr);
pthread_attr_setstacksize(&thread_attr, 40960);
param.sched_priority = APM_TIMER_PRIORITY;
(void)pthread_attr_setschedparam(&thread_attr, &param);
pthread_create(&_timer_thread_ctx, &thread_attr, &Scheduler::_timer_thread, this);
// the UART thread runs at a medium priority
pthread_attr_init(&thread_attr);
pthread_attr_setstacksize(&thread_attr, 40960);
param.sched_priority = APM_UART_PRIORITY;
(void)pthread_attr_setschedparam(&thread_attr, &param);
pthread_create(&_uart_thread_ctx, &thread_attr, &Scheduler::_uart_thread, this);
// the IO thread runs at lower priority
pthread_attr_init(&thread_attr);
pthread_attr_setstacksize(&thread_attr, 40960);
param.sched_priority = APM_IO_PRIORITY;
(void)pthread_attr_setschedparam(&thread_attr, &param);
pthread_create(&_io_thread_ctx, &thread_attr, &Scheduler::_io_thread, this);
}
void Scheduler::delay_microseconds(uint16_t usec)
{
//pthread_yield();
usleep(usec);
}
void Scheduler::delay(uint16_t ms)
{
if (in_timerprocess()) {
::printf("ERROR: delay() from timer process\n");
return;
}
uint64_t start = AP_HAL::micros64();
uint64_t now;
while (((now=AP_HAL::micros64()) - start)/1000 < ms) {
delay_microseconds(1000);
if (_min_delay_cb_ms <= ms) {
if (_delay_cb) {
_delay_cb();
}
}
}
}
void Scheduler::register_delay_callback(AP_HAL::Proc proc,
uint16_t min_time_ms)
{
_delay_cb = proc;
_min_delay_cb_ms = min_time_ms;
}
void Scheduler::register_timer_process(AP_HAL::MemberProc proc)
{
for (uint8_t i = 0; i < _num_timer_procs; i++) {
if (_timer_proc[i] == proc) {
return;
}
}
if (_num_timer_procs < QURT_SCHEDULER_MAX_TIMER_PROCS) {
_timer_proc[_num_timer_procs] = proc;
_num_timer_procs++;
} else {
hal.console->printf("Out of timer processes\n");
}
}
void Scheduler::register_io_process(AP_HAL::MemberProc proc)
{
for (uint8_t i = 0; i < _num_io_procs; i++) {
if (_io_proc[i] == proc) {
return;
}
}
if (_num_io_procs < QURT_SCHEDULER_MAX_TIMER_PROCS) {
_io_proc[_num_io_procs] = proc;
_num_io_procs++;
} else {
hal.console->printf("Out of IO processes\n");
}
}
void Scheduler::register_timer_failsafe(AP_HAL::Proc failsafe, uint32_t period_us)
{
_failsafe = failsafe;
}
void Scheduler::suspend_timer_procs()
{
_timer_suspended = true;
}
void Scheduler::resume_timer_procs()
{
_timer_suspended = false;
if (_timer_event_missed == true) {
_run_timers(false);
_timer_event_missed = false;
}
}
void Scheduler::reboot(bool hold_in_bootloader)
{
HAP_PRINTF("**** REBOOT REQUESTED ****");
usleep(2000000);
exit(1);
}
void Scheduler::_run_timers(bool called_from_timer_thread)
{
if (_in_timer_proc) {
return;
}
_in_timer_proc = true;
if (!_timer_suspended) {
// now call the timer based drivers
for (int i = 0; i < _num_timer_procs; i++) {
if (_timer_proc[i]) {
_timer_proc[i]();
}
}
} else if (called_from_timer_thread) {
_timer_event_missed = true;
}
// and the failsafe, if one is setup
if (_failsafe != NULL) {
_failsafe();
}
// process analog input
// ((QURTAnalogIn *)hal.analogin)->_timer_tick();
_in_timer_proc = false;
}
extern bool qurt_ran_overtime;
void *Scheduler::_timer_thread(void *arg)
{
Scheduler *sched = (Scheduler *)arg;
uint32_t last_ran_overtime = 0;
while (!sched->_hal_initialized) {
sched->delay_microseconds(1000);
}
while (true) {
sched->delay_microseconds(1000);
// run registered timers
sched->_run_timers(true);
// process any pending RC output requests
((RCOutput *)hal.rcout)->timer_update();
if (qurt_ran_overtime && AP_HAL::millis() - last_ran_overtime > 2000) {
last_ran_overtime = AP_HAL::millis();
printf("Overtime in task %d\n", (int)AP_Scheduler::current_task);
hal.console->printf("Overtime in task %d\n", (int)AP_Scheduler::current_task);
}
}
return NULL;
}
void Scheduler::_run_io(void)
{
if (_in_io_proc) {
return;
}
_in_io_proc = true;
if (!_timer_suspended) {
// now call the IO based drivers
for (int i = 0; i < _num_io_procs; i++) {
if (_io_proc[i]) {
_io_proc[i]();
}
}
}
_in_io_proc = false;
}
void *Scheduler::_uart_thread(void *arg)
{
Scheduler *sched = (Scheduler *)arg;
while (!sched->_hal_initialized) {
sched->delay_microseconds(1000);
}
while (true) {
sched->delay_microseconds(1000);
// process any pending serial bytes
//((UARTDriver *)hal.uartA)->timer_tick();
((UARTDriver *)hal.uartB)->timer_tick();
((UARTDriver *)hal.uartC)->timer_tick();
//((UARTDriver *)hal.uartD)->timer_tick();
}
return NULL;
}
void *Scheduler::_io_thread(void *arg)
{
Scheduler *sched = (Scheduler *)arg;
while (!sched->_hal_initialized) {
sched->delay_microseconds(1000);
}
while (true) {
sched->delay_microseconds(1000);
// run registered IO processes
sched->_run_io();
}
return NULL;
}
bool Scheduler::in_timerprocess()
{
return getpid() != _main_task_pid;
}
bool Scheduler::system_initializing() {
return !_initialized;
}
void Scheduler::system_initialized() {
if (_initialized) {
AP_HAL::panic("PANIC: scheduler::system_initialized called"
"more than once");
}
_initialized = true;
}
void Scheduler::hal_initialized(void)
{
HAP_PRINTF("HAL is initialised");
_hal_initialized = true;
}
#endif

81
libraries/AP_HAL_QURT/Scheduler.h Normal file
View File

@ -0,0 +1,81 @@
#pragma once
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_QURT
#include "AP_HAL_QURT_Namespace.h"
#include <sys/time.h>
#include <signal.h>
#include <pthread.h>
#define QURT_SCHEDULER_MAX_TIMER_PROCS 8
#define APM_MAIN_PRIORITY 180
#define APM_TIMER_PRIORITY 181
#define APM_UART_PRIORITY 60
#define APM_IO_PRIORITY 58
#define APM_SHELL_PRIORITY 57
#define APM_OVERTIME_PRIORITY 10
#define APM_STARTUP_PRIORITY 10
#define APM_MAIN_THREAD_STACK_SIZE 16384
/* Scheduler implementation: */
class QURT::Scheduler : public AP_HAL::Scheduler {
public:
Scheduler();
/* AP_HAL::Scheduler methods */
void init();
void delay(uint16_t ms);
void delay_microseconds(uint16_t us);
void register_delay_callback(AP_HAL::Proc, uint16_t min_time_ms);
void register_timer_process(AP_HAL::MemberProc);
void register_io_process(AP_HAL::MemberProc);
void register_timer_failsafe(AP_HAL::Proc, uint32_t period_us);
void suspend_timer_procs();
void resume_timer_procs();
void reboot(bool hold_in_bootloader);
bool in_timerprocess();
bool system_initializing();
void system_initialized();
void hal_initialized();
private:
bool _initialized;
volatile bool _hal_initialized;
AP_HAL::Proc _delay_cb;
uint16_t _min_delay_cb_ms;
AP_HAL::Proc _failsafe;
volatile bool _timer_pending;
volatile bool _timer_suspended;
AP_HAL::MemberProc _timer_proc[QURT_SCHEDULER_MAX_TIMER_PROCS];
uint8_t _num_timer_procs;
volatile bool _in_timer_proc;
AP_HAL::MemberProc _io_proc[QURT_SCHEDULER_MAX_TIMER_PROCS];
uint8_t _num_io_procs;
volatile bool _in_io_proc;
volatile bool _timer_event_missed;
pid_t _main_task_pid;
pthread_t _timer_thread_ctx;
pthread_t _io_thread_ctx;
pthread_t _storage_thread_ctx;
pthread_t _uart_thread_ctx;
static void *_timer_thread(void *arg);
static void *_io_thread(void *arg);
static void *_storage_thread(void *arg);
static void *_uart_thread(void *arg);
void _run_timers(bool called_from_timer_thread);
void _run_io(void);
};
#endif

39
libraries/AP_HAL_QURT/Semaphores.cpp Normal file
View File

@ -0,0 +1,39 @@
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_QURT
#include "Semaphores.h"
extern const AP_HAL::HAL& hal;
using namespace QURT;
bool Semaphore::give()
{
return pthread_mutex_unlock(&_lock) == 0;
}
bool Semaphore::take(uint32_t timeout_ms)
{
if (timeout_ms == 0) {
return pthread_mutex_lock(&_lock) == 0;
}
if (take_nonblocking()) {
return true;
}
uint64_t start = AP_HAL::micros64();
do {
hal.scheduler->delay_microseconds(200);
if (take_nonblocking()) {
return true;
}
} while ((AP_HAL::micros64() - start) < timeout_ms*1000);
return false;
}
bool Semaphore::take_nonblocking()
{
return pthread_mutex_trylock(&_lock) == 0;
}
#endif // CONFIG_HAL_BOARD

22
libraries/AP_HAL_QURT/Semaphores.h Normal file
View File

@ -0,0 +1,22 @@
#pragma once
#include <AP_HAL/AP_HAL_Boards.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_QURT
#include "AP_HAL_QURT.h"
#include <pthread.h>
class QURT::Semaphore : public AP_HAL::Semaphore {
public:
Semaphore() {
HAP_PRINTF("%s constructor", __FUNCTION__);
pthread_mutex_init(&_lock, NULL);
HAP_PRINTF("%s constructor2", __FUNCTION__);
}
bool give();
bool take(uint32_t timeout_ms);
bool take_nonblocking();
private:
pthread_mutex_t _lock;
};
#endif // CONFIG_HAL_BOARD

49
libraries/AP_HAL_QURT/Storage.cpp Normal file
View File

@ -0,0 +1,49 @@
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_QURT
#include <assert.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <unistd.h>
#include <errno.h>
#include <stdio.h>
#include "Storage.h"
using namespace QURT;
/*
This stores 'eeprom' data on the filesystem, with a 16k size
Data is written on the ARM frontend via a RPC call
*/
extern const AP_HAL::HAL& hal;
volatile bool Storage::dirty;
uint8_t Storage::buffer[QURT_STORAGE_SIZE];
Semaphore Storage::lock;
void Storage::read_block(void *dst, uint16_t loc, size_t n)
{
if (loc >= sizeof(buffer)-(n-1)) {
return;
}
memcpy(dst, &buffer[loc], n);
}
void Storage::write_block(uint16_t loc, const void *src, size_t n)
{
if (loc >= sizeof(buffer)-(n-1)) {
return;
}
if (memcmp(src, &buffer[loc], n) != 0) {
lock.take(0);
memcpy(&buffer[loc], src, n);
dirty = true;
lock.give();
}
}
#endif // CONFIG_HAL_BOARD

27
libraries/AP_HAL_QURT/Storage.h Normal file
View File

@ -0,0 +1,27 @@
#pragma once
#include <AP_HAL/AP_HAL.h>
#include "AP_HAL_QURT_Namespace.h"
#include "Semaphores.h"
#include <stdio.h>
#define QURT_STORAGE_SIZE HAL_STORAGE_SIZE
class QURT::Storage : public AP_HAL::Storage
{
public:
Storage() {}
static Storage *from(AP_HAL::Storage *storage) {
return static_cast<Storage*>(storage);
}
void init() {}
void read_block(void *dst, uint16_t src, size_t n);
void write_block(uint16_t dst, const void* src, size_t n);
static volatile bool dirty;
static uint8_t buffer[QURT_STORAGE_SIZE];
static Semaphore lock;
};

297
libraries/AP_HAL_QURT/UARTDriver.cpp Normal file
View File

@ -0,0 +1,297 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_QURT
#include <stdlib.h>
#include <unistd.h>
#include "UARTDriver.h"
#include <sys/types.h>
#include <sys/stat.h>
#include <stdio.h>
#include <fcntl.h>
#include <dev_fs_lib_serial.h>
#include <AP_HAL/utility/RingBuffer.h>
using namespace QURT;
extern const AP_HAL::HAL& hal;
UARTDriver::UARTDriver(const char *name) :
devname(name)
{
}
void UARTDriver::begin(uint32_t b)
{
begin(b, 16384, 16384);
}
static const struct {
uint32_t baudrate;
enum DSPAL_SERIAL_BITRATES arg;
} baudrate_table[] = {
{ 9600, DSPAL_SIO_BITRATE_9600 },
{ 14400, DSPAL_SIO_BITRATE_14400 },
{ 19200, DSPAL_SIO_BITRATE_19200 },
{ 38400, DSPAL_SIO_BITRATE_38400 },
{ 57600, DSPAL_SIO_BITRATE_57600 },
{ 76800, DSPAL_SIO_BITRATE_76800 },
{ 115200, DSPAL_SIO_BITRATE_115200 },
{ 230400, DSPAL_SIO_BITRATE_230400 },
{ 250000, DSPAL_SIO_BITRATE_250000 },
{ 460800, DSPAL_SIO_BITRATE_460800 },
{ 921600, DSPAL_SIO_BITRATE_921600 },
{ 2000000, DSPAL_SIO_BITRATE_2000000 },
};
extern "C" {
static void read_callback_trampoline(void *, char *, size_t );
}
static void read_callback_trampoline(void *ctx, char *buf, size_t size)
{
((UARTDriver *)ctx)->_read_callback(buf, size);
}
/*
callback for incoming data
*/
void UARTDriver::_read_callback(char *buf, size_t size)
{
if (readbuf == nullptr) {
return;
}
uint32_t n = readbuf->write((const uint8_t *)buf, size);
if (n != size) {
HAP_PRINTF("read_callback lost %u %u", n, size);
}
}
void UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
{
if (rxS < 4096) {
rxS = 4096;
}
if (txS < 4096) {
txS = 4096;
}
if (fd == -1) {
fd = open(devname, O_RDWR | O_NONBLOCK);
if (fd == -1) {
AP_HAL::panic("Unable to open %s", devname);
}
}
/*
allocate the read buffer
we allocate buffers before we successfully open the device as we
want to allocate in the early stages of boot, and cause minimum
thrashing of the heap once we are up. The ttyACM0 driver may not
connect for some time after boot
*/
if (rxS != 0 && (readbuf == nullptr || rxS != readbuf->get_size())) {
initialised = false;
if (readbuf != nullptr) {
delete readbuf;
}
readbuf = new ByteBuffer(rxS);
}
/*
allocate the write buffer
*/
if (txS != 0 && (writebuf == nullptr || txS != writebuf->get_size())) {
initialised = false;
if (writebuf != nullptr) {
delete writebuf;
}
writebuf = new ByteBuffer(txS);
}
struct dspal_serial_ioctl_receive_data_callback callback;
callback.context = this;
callback.rx_data_callback_func_ptr = read_callback_trampoline;
int ret = ioctl(fd, SERIAL_IOCTL_SET_RECEIVE_DATA_CALLBACK, (void *)&callback);
if (b != 0) {
for (uint8_t i=0; i<ARRAY_SIZE(baudrate_table); i++) {
if (b <= baudrate_table[i].baudrate) {
struct dspal_serial_ioctl_data_rate rate;
rate.bit_rate = baudrate_table[i].arg;
ret = ioctl(fd, SERIAL_IOCTL_SET_DATA_RATE, (void *)&rate);
break;
}
}
}
if (readbuf && writebuf && fd != -1) {
initialised = true;
}
}
void UARTDriver::end()
{
initialised = false;
if (fd != -1) {
::close(fd);
fd = -1;
}
if (readbuf) {
delete readbuf;
readbuf = nullptr;
}
if (writebuf) {
delete writebuf;
writebuf = nullptr;
}
}
void UARTDriver::flush()
{
}
bool UARTDriver::is_initialized()
{
return fd != -1 && initialised;
}
void UARTDriver::set_blocking_writes(bool blocking)
{
nonblocking_writes = !blocking;
}
bool UARTDriver::tx_pending()
{
return false;
}
/* QURT implementations of Stream virtual methods */
int16_t UARTDriver::available()
{
if (!initialised) {
return 0;
}
return readbuf->available();
}
int16_t UARTDriver::txspace()
{
if (!initialised) {
return 0;
}
return writebuf->space();
}
int16_t UARTDriver::read()
{
uint8_t c;
if (!initialised) {
return -1;
}
if (!lock.take(0)) {
return 0;
}
if (readbuf->empty()) {
lock.give();
return -1;
}
readbuf->read(&c, 1);
lock.give();
return c;
}
/* QURT implementations of Print virtual methods */
size_t UARTDriver::write(uint8_t c)
{
if (!initialised) {
return 0;
}
if (!lock.take(0)) {
return 0;
}
while (writebuf->space() == 0) {
if (nonblocking_writes) {
lock.give();
return 0;
}
hal.scheduler->delay_microseconds(1000);
}
writebuf->write(&c, 1);
lock.give();
return 1;
}
size_t UARTDriver::write(const uint8_t *buffer, size_t size)
{
if (!initialised) {
return 0;
}
if (!nonblocking_writes) {
/*
use the per-byte delay loop in write() above for blocking writes
*/
size_t ret = 0;
while (size--) {
if (write(*buffer++) != 1) break;
ret++;
}
return ret;
}
if (!lock.take(0)) {
return 0;
}
size_t ret = writebuf->write(buffer, size);
lock.give();
return ret;
}
/*
push any pending bytes to/from the serial port. This is called at
1kHz in the timer thread
*/
void UARTDriver::timer_tick(void)
{
uint16_t n;
if (fd == -1 || !initialised || !lock.take_nonblocking()) {
return;
}
in_timer = true;
// write any pending bytes
n = writebuf->available();
if (n == 0) {
in_timer = false;
lock.give();
return;
}
if (n > 64) {
n = 64;
}
uint8_t buf[n];
writebuf->read(buf, n);
::write(fd, buf, n);
lock.give();
in_timer = false;
}
#endif

63
libraries/AP_HAL_QURT/UARTDriver.h Normal file
View File

@ -0,0 +1,63 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include "AP_HAL_QURT.h"
#include "Semaphores.h"
#include <AP_HAL/utility/RingBuffer.h>
class QURT::UARTDriver : public AP_HAL::UARTDriver {
public:
UARTDriver(const char *name);
/* QURT implementations of UARTDriver virtual methods */
void begin(uint32_t b);
void begin(uint32_t b, uint16_t rxS, uint16_t txS);
void end();
void flush();
bool is_initialized();
void set_blocking_writes(bool blocking);
bool tx_pending();
/* QURT implementations of Stream virtual methods */
int16_t available();
int16_t txspace();
int16_t read();
/* QURT implementations of Print virtual methods */
size_t write(uint8_t c);
size_t write(const uint8_t *buffer, size_t size);
void _read_callback(char *buf, size_t size);
void timer_tick(void);
private:
const char *devname;
int fd = -1;
Semaphore lock;
ByteBuffer *readbuf;
ByteBuffer *writebuf;
bool nonblocking_writes = false;
volatile bool in_timer = false;
volatile bool initialised = false;
uint64_t last_write_time_us;
int write_fd(const uint8_t *buf, uint16_t n);
};

254
libraries/AP_HAL_QURT/UDPDriver.cpp Normal file
View File

@ -0,0 +1,254 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_QURT
#include <stdlib.h>
#include <unistd.h>
#include "UDPDriver.h"
#include <stdio.h>
#include <AP_HAL/utility/RingBuffer.h>
#include <AP_Math/AP_Math.h>
using namespace QURT;
extern const AP_HAL::HAL& hal;
void UDPDriver::begin(uint32_t b)
{
begin(b, 16384, 16384);
}
void UDPDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
{
rxS = constrain_int32(rxS, 16384, 30000);
txS = constrain_int32(txS, 16384, 30000);
/*
allocate the read buffer
*/
if (rxS != 0 && (readbuf == nullptr || rxS != readbuf->get_size())) {
initialised = false;
if (readbuf != nullptr) {
delete readbuf;
}
readbuf = new ByteBuffer(rxS);
}
/*
allocate the write buffer
*/
if (txS != 0 && (writebuf == nullptr || txS != writebuf->get_size())) {
initialised = false;
if (writebuf != nullptr) {
delete writebuf;
}
writebuf = new ByteBuffer(txS);
}
if (readbuf && writebuf) {
initialised = true;
}
}
void UDPDriver::end()
{
initialised = false;
if (readbuf) {
delete readbuf;
readbuf = nullptr;
}
if (writebuf) {
delete writebuf;
writebuf = nullptr;
}
}
void UDPDriver::flush()
{
}
bool UDPDriver::is_initialized()
{
return initialised;
}
void UDPDriver::set_blocking_writes(bool blocking)
{
nonblocking_writes = !blocking;
}
bool UDPDriver::tx_pending()
{
return false;
}
/* QURT implementations of Stream virtual methods */
int16_t UDPDriver::available()
{
if (!initialised) {
return 0;
}
return readbuf->available();
}
int16_t UDPDriver::txspace()
{
if (!initialised) {
return 0;
}
return writebuf->space();
}
int16_t UDPDriver::read()
{
uint8_t c;
if (!initialised) {
return -1;
}
if (!lock.take(0)) {
return 0;
}
if (readbuf->empty()) {
lock.give();
return -1;
}
readbuf->read(&c, 1);
lock.give();
return c;
}
/* QURT implementations of Print virtual methods */
size_t UDPDriver::write(uint8_t c)
{
if (!initialised) {
return 0;
}
if (!lock.take(0)) {
return 0;
}
while (writebuf->space() == 0) {
if (nonblocking_writes) {
lock.give();
return 0;
}
hal.scheduler->delay_microseconds(1000);
}
writebuf->write(&c, 1);
lock.give();
return 1;
}
size_t UDPDriver::write(const uint8_t *buffer, size_t size)
{
if (!initialised) {
return 0;
}
if (!nonblocking_writes) {
/*
use the per-byte delay loop in write() above for blocking writes
*/
size_t ret = 0;
while (size--) {
if (write(*buffer++) != 1) break;
ret++;
}
return ret;
}
if (!lock.take(0)) {
return 0;
}
size_t ret = writebuf->write(buffer, size);
lock.give();
return ret;
}
uint32_t UDPDriver::socket_check(uint8_t *buf, int len, uint32_t *nbytes)
{
if (!initialised) {
return 1;
}
if (!writebuf) {
return 1;
}
*nbytes = writebuf->available();
if (*nbytes == 0) {
return 1;
}
if (*nbytes > len) {
*nbytes = len;
}
uint32_t n = *nbytes;
if (writebuf->peek(0) != 254) {
/*
we have a non-mavlink packet at the start of the
buffer. Look ahead for a MAVLink start byte, up to 256 bytes
ahead
*/
uint16_t limit = n>256?256:n;
uint16_t i;
for (i=0; i<limit; i++) {
if (writebuf->peek(i) == 254) {
n = i;
break;
}
}
// if we didn't find a MAVLink marker then limit the send size to 256
if (i == limit) {
n = limit;
}
} else {
// this looks like a MAVLink packet - try to write on
// packet boundaries when possible
if (n < 8) {
n = 0;
} else {
// the length of the packet is the 2nd byte, and mavlink
// packets have a 6 byte header plus 2 byte checksum,
// giving len+8 bytes
uint8_t len = writebuf->peek(1);
if (n < len+8) {
// we don't have a full packet yet
n = 0;
} else if (n > len+8) {
// send just 1 packet at a time (so MAVLink packets
// are aligned on UDP boundaries)
n = len+8;
}
}
}
*nbytes = n;
writebuf->read(buf, *nbytes);
return 0;
}
uint32_t UDPDriver::socket_input(const uint8_t *buf, int len, uint32_t *nbytes)
{
if (!initialised) {
return 1;
}
if (!readbuf) {
return 1;
}
*nbytes = readbuf->write(buf, len);
return 0;
}
#endif // CONFIG_HAL_BOARD

55
libraries/AP_HAL_QURT/UDPDriver.h Normal file
View File

@ -0,0 +1,55 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include "AP_HAL_QURT.h"
#include "Semaphores.h"
#include <AP_HAL/utility/RingBuffer.h>
class QURT::UDPDriver : public AP_HAL::UARTDriver {
public:
static UDPDriver *from(AP_HAL::UARTDriver *d) {
return static_cast<UDPDriver*>(d);
}
void begin(uint32_t b);
void begin(uint32_t b, uint16_t rxS, uint16_t txS);
void end();
void flush();
bool is_initialized();
void set_blocking_writes(bool blocking);
bool tx_pending();
int16_t available();
int16_t txspace();
int16_t read();
size_t write(uint8_t c);
size_t write(const uint8_t *buffer, size_t size);
uint32_t socket_check(uint8_t *buf, int len, uint32_t *nbytes);
uint32_t socket_input(const uint8_t *buf, int len, uint32_t *nbytes);
enum flow_control get_flow_control(void) { return FLOW_CONTROL_ENABLE; };
private:
Semaphore lock;
bool initialised;
bool nonblocking_writes = true;
ByteBuffer *readbuf;
ByteBuffer *writebuf;
};

27
libraries/AP_HAL_QURT/Util.cpp Normal file
View File

@ -0,0 +1,27 @@
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_QURT
#include "Semaphores.h"
#include <stdio.h>
extern const AP_HAL::HAL& hal;
#include "Util.h"
using namespace QURT;
/*
always report 256k of free memory. I don't know how to query
available memory on QURT
*/
uint32_t Util::available_memory(void)
{
return 256*1024;
}
// create a new semaphore
Semaphore *Util::new_semaphore(void)
{
return new Semaphore;
}
#endif // CONFIG_HAL_BOARD == HAL_BOARD_QURT

33
libraries/AP_HAL_QURT/Util.h Normal file
View File

@ -0,0 +1,33 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include <AP_HAL/AP_HAL.h>
#include "AP_HAL_QURT_Namespace.h"
class QURT::Util : public AP_HAL::Util {
public:
Util(void) {
HAP_PRINTF("%s constructor", __FUNCTION__);
}
bool run_debug_shell(AP_HAL::BetterStream *stream) override { return false; }
uint32_t available_memory(void) override;
// create a new semaphore
Semaphore *new_semaphore(void) override;
};

88
libraries/AP_HAL_QURT/dsp_main.cpp Normal file
View File

@ -0,0 +1,88 @@
#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, &param);
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);
}

133
libraries/AP_HAL_QURT/mainapp/mainapp.cpp Normal file
View File

@ -0,0 +1,133 @@
/*
main program for HAL_QURT port
*/
#include <stdio.h>
#include <stdint.h>
#include <sys/types.h>
#include <fcntl.h>
#include <unistd.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include <time.h>
#include <errno.h>
#include <AP_HAL/utility/Socket.h>
#include <qurt_dsp.h>
static SocketAPM sock{true};
static bool connected;
static uint32_t last_get_storage_us;
static uint64_t start_time;
// location of virtual eeprom in Linux filesystem
#define STORAGE_DIR "/var/APM"
#define STORAGE_FILE STORAGE_DIR "/" SKETCHNAME ".stg"
// time since startup in microseconds
static uint64_t micros64()
{
struct timespec ts;
clock_gettime(CLOCK_MONOTONIC, &ts);
uint64_t ret = ts.tv_sec*1000*1000ULL + ts.tv_nsec/1000U;
if (start_time == 0) {
start_time = ret;
}
ret -= start_time;
return ret;
}
/*
send storage file to DSPs
*/
static void send_storage(void)
{
int fd = open(STORAGE_FILE, O_RDWR|O_CREAT, 0644);
if (fd == -1) {
printf("Unable to open %s", STORAGE_FILE);
exit(1);
}
uint8_t buf[16384];
memset(buf, 0, sizeof(buf));
read(fd, buf, sizeof(buf));
if (ardupilot_set_storage(buf, sizeof(buf)) != 0) {
printf("Failed to send initial storage");
exit(1);
}
close(fd);
}
/*
get updated storage file from DSPs
*/
static void get_storage(void)
{
uint8_t buf[16384];
if (ardupilot_get_storage(buf, sizeof(buf)) != 0) {
return;
}
int fd = open(STORAGE_FILE ".new", O_WRONLY);
if (fd == -1) {
printf("Unable to open %s - %s\n", STORAGE_FILE ".new", strerror(errno));
}
write(fd, buf, sizeof(buf));
close(fd);
// atomic rename
if (rename(STORAGE_FILE ".new", STORAGE_FILE) != 0) {
printf("Unable to rename to %s - %s\n", STORAGE_FILE, strerror(errno));
}
}
/*
handle any incoming or outgoing UDP socket data on behalf of the DSPs
*/
static void socket_check(void)
{
uint8_t buf[300];
ssize_t ret = sock.recv(buf, sizeof(buf), 0);
if (ret > 0) {
uint32_t nbytes;
ardupilot_socket_input(buf, ret, &nbytes);
if (!connected) {
const char *ip;
uint16_t port;
sock.last_recv_address(ip, port);
connected = sock.connect(ip, port);
if (connected) {
printf("Connected to UDP %s:%u\n", ip, (unsigned)port);
}
sock.set_blocking(false);
}
}
uint32_t nbytes;
if (ardupilot_socket_check(buf, sizeof(buf), &nbytes) == 0) {
if (!connected) {
sock.sendto(buf, nbytes, "255.255.255.255", 14550);
} else {
sock.send(buf, nbytes);
}
}
}
/*
main program
*/
int main(int argc, const char *argv[])
{
sock.set_broadcast();
printf("Starting DSP code\n");
send_storage();
ardupilot_start();
while (true) {
uint64_t now = micros64();
if (now - last_get_storage_us > 1000*1000) {
printf("tick t=%.6f\n", now*1.0e-6f);
ardupilot_heartbeat();
get_storage();
last_get_storage_us = now;
}
socket_check();
usleep(5000);
}
}

17
libraries/AP_HAL_QURT/qurt_dsp.idl Normal file
View File

@ -0,0 +1,17 @@
#include "AEEStdDef.idl"
interface ardupilot {
// start main thread
uint32 start();
// a heartbeat for debugging
uint32 heartbeat();
// get eeprom updates
uint32 set_storage(in sequence<uint8> eeprom);
uint32 get_storage(rout sequence<uint8> eeprom);
// handle socket data
uint32 socket_check(rout sequence<uint8> buf, rout uint32 nbytes);
uint32 socket_input(in sequence<uint8> buf, rout uint32 nbytes);
};

76
libraries/AP_HAL_QURT/replace.cpp Normal file
View File

@ -0,0 +1,76 @@
#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 "replace.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(NULL, 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 = NULL;
va_start(ap, format);
ret = vasprintf(ptr, format, ap);
va_end(ap);
return ret;
}
}

39
libraries/AP_HAL_QURT/replace.h Normal file
View File

@ -0,0 +1,39 @@
#pragma once
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_QURT
#include <stdlib.h>
#include <stdint.h>
#include <types.h>
#include <sys/types.h>
#include <unistd.h>
#include <dirent.h>
#include <types.h>
#include <dirent.h>
extern "C" {
/*
work around broken headers
*/
size_t strnlen(const char *s, size_t maxlen);
int asprintf(char **, const char *, ...);
off_t lseek(int, off_t, int);
DIR *opendir (const char *);
int unlink(const char *pathname);
//typedef int32_t pid_t;
pid_t getpid (void);
void HAP_printf(const char *file, int line, const char *fmt, ...);
}
#define HAP_PRINTF(...) HAP_printf(__FILE__, __LINE__, __VA_ARGS__)
extern volatile int _last_dsp_line;
extern volatile const char *_last_dsp_file;
extern volatile uint32_t _last_counter;
#define HAP_LINE() do { _last_dsp_line = __LINE__; _last_dsp_file = __FILE__; _last_counter++; } while (0)
#endif // CONFIG_HAL_BOARD

66
libraries/AP_HAL_QURT/system.cpp Normal file
View File

@ -0,0 +1,66 @@
#include <stdarg.h>
#include <stdio.h>
#include <sys/time.h>
#include <unistd.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_HAL/system.h>
#include <sys/timespec.h>
#include <dspal_time.h>
#include "replace.h"
#include <fenv.h>
extern const AP_HAL::HAL& hal;
namespace AP_HAL {
static struct {
uint64_t start_time;
} state;
void init()
{
state.start_time = micros64();
// we don't want exceptions in flight code. That is the job of SITL
feclearexcept(FE_OVERFLOW | FE_DIVBYZERO | FE_INVALID);
}
void panic(const char *errormsg, ...)
{
char buf[200];
va_list ap;
va_start(ap, errormsg);
vsnprintf(buf, sizeof(buf), errormsg, ap);
va_end(ap);
HAP_PRINTF(buf);
usleep(2000000);
hal.rcin->deinit();
exit(1);
}
uint32_t micros()
{
return micros64() & 0xFFFFFFFF;
}
uint32_t millis()
{
return millis64() & 0xFFFFFFFF;
}
uint64_t micros64()
{
struct timespec ts;
clock_gettime(CLOCK_MONOTONIC, &ts);
uint64_t ret = ts.tv_sec*1000*1000ULL + ts.tv_nsec/1000U;
ret -= state.start_time;
return ret;
}
uint64_t millis64()
{
return micros64() / 1000;
}
} // namespace AP_HAL