diff --git a/libraries/AP_HAL_QURT/AP_HAL_QURT.h b/libraries/AP_HAL_QURT/AP_HAL_QURT.h new file mode 100644 index 0000000000..26d8e0b476 --- /dev/null +++ b/libraries/AP_HAL_QURT/AP_HAL_QURT.h @@ -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 . + */ + +#ifndef __AP_HAL_QURT_H__ +#define __AP_HAL_QURT_H__ + +/* Your layer exports should depend on AP_HAL.h ONLY. */ +#include + +#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__ + diff --git a/libraries/AP_HAL_QURT/AP_HAL_QURT_Main.h b/libraries/AP_HAL_QURT/AP_HAL_QURT_Main.h new file mode 100644 index 0000000000..039800a993 --- /dev/null +++ b/libraries/AP_HAL_QURT/AP_HAL_QURT_Main.h @@ -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 . + */ + +#ifndef __AP_HAL_QURT_MAIN_H__ +#define __AP_HAL_QURT_MAIN_H__ + +#endif // __AP_HAL_QURT_MAIN_H__ diff --git a/libraries/AP_HAL_QURT/AP_HAL_QURT_Namespace.h b/libraries/AP_HAL_QURT/AP_HAL_QURT_Namespace.h new file mode 100644 index 0000000000..279c588e04 --- /dev/null +++ b/libraries/AP_HAL_QURT/AP_HAL_QURT_Namespace.h @@ -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 . + */ + +#pragma once + +namespace QURT { + class UARTDriver; + class UDPDriver; + class Util; + class Scheduler; + class Storage; + class Util; + class Semaphore; + class RCInput; + class RCOutput; +} + diff --git a/libraries/AP_HAL_QURT/AP_HAL_QURT_Private.h b/libraries/AP_HAL_QURT/AP_HAL_QURT_Private.h new file mode 100644 index 0000000000..71d29f280c --- /dev/null +++ b/libraries/AP_HAL_QURT/AP_HAL_QURT_Private.h @@ -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 . + */ + +#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__ + diff --git a/libraries/AP_HAL_QURT/HAL_QURT_Class.cpp b/libraries/AP_HAL_QURT/HAL_QURT_Class.cpp new file mode 100644 index 0000000000..569014d9e7 --- /dev/null +++ b/libraries/AP_HAL_QURT/HAL_QURT_Class.cpp @@ -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 . + */ + +#include +#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 +#include +#include + +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 diff --git a/libraries/AP_HAL_QURT/HAL_QURT_Class.h b/libraries/AP_HAL_QURT/HAL_QURT_Class.h new file mode 100644 index 0000000000..0e9811ceba --- /dev/null +++ b/libraries/AP_HAL_QURT/HAL_QURT_Class.h @@ -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 . + */ + +#ifndef __AP_HAL_QURT_CLASS_H__ +#define __AP_HAL_QURT_CLASS_H__ + +#include + +#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__ + diff --git a/libraries/AP_HAL_QURT/RCInput.cpp b/libraries/AP_HAL_QURT/RCInput.cpp new file mode 100644 index 0000000000..cd10521499 --- /dev/null +++ b/libraries/AP_HAL_QURT/RCInput.cpp @@ -0,0 +1,193 @@ +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_QURT +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "RCInput.h" +#include + +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 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_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 diff --git a/libraries/AP_HAL_QURT/RCInput.h b/libraries/AP_HAL_QURT/RCInput.h new file mode 100644 index 0000000000..8653ddf7c0 --- /dev/null +++ b/libraries/AP_HAL_QURT/RCInput.h @@ -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); + } + + 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; +}; + diff --git a/libraries/AP_HAL_QURT/RCOutput.cpp b/libraries/AP_HAL_QURT/RCOutput.cpp new file mode 100644 index 0000000000..0fc917a3d1 --- /dev/null +++ b/libraries/AP_HAL_QURT/RCOutput.cpp @@ -0,0 +1,113 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_QURT + +#include "RCOutput.h" +#include +#include +#include +#include +#include + +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<= channel_count) { + return; + } + enable_mask &= ~1U<= 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 + diff --git a/libraries/AP_HAL_QURT/RCOutput.h b/libraries/AP_HAL_QURT/RCOutput.h new file mode 100644 index 0000000000..fefa138448 --- /dev/null +++ b/libraries/AP_HAL_QURT/RCOutput.h @@ -0,0 +1,36 @@ +#pragma once + +#include +#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 diff --git a/libraries/AP_HAL_QURT/Scheduler.cpp b/libraries/AP_HAL_QURT/Scheduler.cpp new file mode 100644 index 0000000000..9a7d0490c3 --- /dev/null +++ b/libraries/AP_HAL_QURT/Scheduler.cpp @@ -0,0 +1,291 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include +#if CONFIG_HAL_BOARD == HAL_BOARD_QURT + +#include "AP_HAL_QURT.h" +#include "Scheduler.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "UARTDriver.h" +//#include "AnalogIn.h" +#include "Storage.h" +#include "RCOutput.h" +#include + +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, ¶m); + + 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, ¶m); + + 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, ¶m); + + 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 diff --git a/libraries/AP_HAL_QURT/Scheduler.h b/libraries/AP_HAL_QURT/Scheduler.h new file mode 100644 index 0000000000..1c5389fe37 --- /dev/null +++ b/libraries/AP_HAL_QURT/Scheduler.h @@ -0,0 +1,81 @@ +#pragma once + +#include +#if CONFIG_HAL_BOARD == HAL_BOARD_QURT +#include "AP_HAL_QURT_Namespace.h" +#include +#include +#include + +#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 + + + diff --git a/libraries/AP_HAL_QURT/Semaphores.cpp b/libraries/AP_HAL_QURT/Semaphores.cpp new file mode 100644 index 0000000000..6bc728c23c --- /dev/null +++ b/libraries/AP_HAL_QURT/Semaphores.cpp @@ -0,0 +1,39 @@ +#include + +#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 diff --git a/libraries/AP_HAL_QURT/Semaphores.h b/libraries/AP_HAL_QURT/Semaphores.h new file mode 100644 index 0000000000..ba7029d588 --- /dev/null +++ b/libraries/AP_HAL_QURT/Semaphores.h @@ -0,0 +1,22 @@ +#pragma once + +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_QURT +#include "AP_HAL_QURT.h" +#include + +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 diff --git a/libraries/AP_HAL_QURT/Storage.cpp b/libraries/AP_HAL_QURT/Storage.cpp new file mode 100644 index 0000000000..11bdc9bd3f --- /dev/null +++ b/libraries/AP_HAL_QURT/Storage.cpp @@ -0,0 +1,49 @@ +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_QURT + +#include +#include +#include +#include +#include +#include +#include +#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 diff --git a/libraries/AP_HAL_QURT/Storage.h b/libraries/AP_HAL_QURT/Storage.h new file mode 100644 index 0000000000..6f5771150b --- /dev/null +++ b/libraries/AP_HAL_QURT/Storage.h @@ -0,0 +1,27 @@ +#pragma once + +#include +#include "AP_HAL_QURT_Namespace.h" +#include "Semaphores.h" +#include + +#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); + } + + 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; +}; + diff --git a/libraries/AP_HAL_QURT/UARTDriver.cpp b/libraries/AP_HAL_QURT/UARTDriver.cpp new file mode 100644 index 0000000000..656be4184f --- /dev/null +++ b/libraries/AP_HAL_QURT/UARTDriver.cpp @@ -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 . + */ + +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_QURT +#include +#include +#include "UARTDriver.h" +#include +#include +#include +#include +#include +#include + +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; iavailable(); +} + +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 diff --git a/libraries/AP_HAL_QURT/UARTDriver.h b/libraries/AP_HAL_QURT/UARTDriver.h new file mode 100644 index 0000000000..ce5d9da9bb --- /dev/null +++ b/libraries/AP_HAL_QURT/UARTDriver.h @@ -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 . + */ + +#pragma once + +#include "AP_HAL_QURT.h" +#include "Semaphores.h" +#include + +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); +}; diff --git a/libraries/AP_HAL_QURT/UDPDriver.cpp b/libraries/AP_HAL_QURT/UDPDriver.cpp new file mode 100644 index 0000000000..bd1241273a --- /dev/null +++ b/libraries/AP_HAL_QURT/UDPDriver.cpp @@ -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 . + */ + +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_QURT +#include +#include +#include "UDPDriver.h" +#include +#include +#include + +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; ipeek(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 + diff --git a/libraries/AP_HAL_QURT/UDPDriver.h b/libraries/AP_HAL_QURT/UDPDriver.h new file mode 100644 index 0000000000..4eb34c094a --- /dev/null +++ b/libraries/AP_HAL_QURT/UDPDriver.h @@ -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 . + */ + +#pragma once + +#include "AP_HAL_QURT.h" +#include "Semaphores.h" +#include + +class QURT::UDPDriver : public AP_HAL::UARTDriver { +public: + static UDPDriver *from(AP_HAL::UARTDriver *d) { + return static_cast(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; +}; diff --git a/libraries/AP_HAL_QURT/Util.cpp b/libraries/AP_HAL_QURT/Util.cpp new file mode 100644 index 0000000000..08e635c73e --- /dev/null +++ b/libraries/AP_HAL_QURT/Util.cpp @@ -0,0 +1,27 @@ +#include +#if CONFIG_HAL_BOARD == HAL_BOARD_QURT +#include "Semaphores.h" +#include + +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 diff --git a/libraries/AP_HAL_QURT/Util.h b/libraries/AP_HAL_QURT/Util.h new file mode 100644 index 0000000000..8927f6b88e --- /dev/null +++ b/libraries/AP_HAL_QURT/Util.h @@ -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 . + */ + +#pragma once + +#include +#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; +}; + diff --git a/libraries/AP_HAL_QURT/dsp_main.cpp b/libraries/AP_HAL_QURT/dsp_main.cpp new file mode 100644 index 0000000000..d94906487f --- /dev/null +++ b/libraries/AP_HAL_QURT/dsp_main.cpp @@ -0,0 +1,88 @@ +#include +#include "UDPDriver.h" +#include +#include +#include +#include "Scheduler.h" +#include "Storage.h" +#include "replace.h" +#include + +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); +} diff --git a/libraries/AP_HAL_QURT/mainapp/mainapp.cpp b/libraries/AP_HAL_QURT/mainapp/mainapp.cpp new file mode 100644 index 0000000000..01ad16e8d0 --- /dev/null +++ b/libraries/AP_HAL_QURT/mainapp/mainapp.cpp @@ -0,0 +1,133 @@ +/* + main program for HAL_QURT port + */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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); + } +} diff --git a/libraries/AP_HAL_QURT/qurt_dsp.idl b/libraries/AP_HAL_QURT/qurt_dsp.idl new file mode 100644 index 0000000000..727ecf1fd9 --- /dev/null +++ b/libraries/AP_HAL_QURT/qurt_dsp.idl @@ -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 eeprom); + uint32 get_storage(rout sequence eeprom); + + // handle socket data + uint32 socket_check(rout sequence buf, rout uint32 nbytes); + uint32 socket_input(in sequence buf, rout uint32 nbytes); +}; diff --git a/libraries/AP_HAL_QURT/replace.cpp b/libraries/AP_HAL_QURT/replace.cpp new file mode 100644 index 0000000000..6d89ba1a74 --- /dev/null +++ b/libraries/AP_HAL_QURT/replace.cpp @@ -0,0 +1,76 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#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; +} + +} diff --git a/libraries/AP_HAL_QURT/replace.h b/libraries/AP_HAL_QURT/replace.h new file mode 100644 index 0000000000..8f085c2af3 --- /dev/null +++ b/libraries/AP_HAL_QURT/replace.h @@ -0,0 +1,39 @@ +#pragma once +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_QURT +#include +#include +#include +#include +#include +#include + +#include +#include +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 diff --git a/libraries/AP_HAL_QURT/system.cpp b/libraries/AP_HAL_QURT/system.cpp new file mode 100644 index 0000000000..64f3a57b83 --- /dev/null +++ b/libraries/AP_HAL_QURT/system.cpp @@ -0,0 +1,66 @@ +#include +#include +#include +#include + +#include +#include + +#include +#include +#include "replace.h" +#include + +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