From 3f4d0f42406fb11c1370b99d9bef36d69bc19f5c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 18 May 2024 21:43:58 +1000 Subject: [PATCH 01/73] HAL_QURT: revive QURT HAL --- libraries/AP_HAL_QURT/AP_HAL_QURT.h | 25 ++ libraries/AP_HAL_QURT/AP_HAL_QURT_Main.h | 15 + libraries/AP_HAL_QURT/AP_HAL_QURT_Namespace.h | 29 ++ libraries/AP_HAL_QURT/AP_HAL_QURT_Private.h | 23 ++ libraries/AP_HAL_QURT/HAL_QURT_Class.cpp | 144 +++++++++ libraries/AP_HAL_QURT/HAL_QURT_Class.h | 25 ++ libraries/AP_HAL_QURT/RCInput.cpp | 184 +++++++++++ libraries/AP_HAL_QURT/RCInput.h | 52 +++ libraries/AP_HAL_QURT/RCOutput.cpp | 126 ++++++++ libraries/AP_HAL_QURT/RCOutput.h | 44 +++ libraries/AP_HAL_QURT/Scheduler.cpp | 274 ++++++++++++++++ libraries/AP_HAL_QURT/Scheduler.h | 73 +++++ libraries/AP_HAL_QURT/Semaphores.cpp | 39 +++ libraries/AP_HAL_QURT/Semaphores.h | 34 ++ libraries/AP_HAL_QURT/Storage.cpp | 49 +++ libraries/AP_HAL_QURT/Storage.h | 27 ++ libraries/AP_HAL_QURT/UARTDriver.cpp | 300 ++++++++++++++++++ libraries/AP_HAL_QURT/UARTDriver.h | 68 ++++ libraries/AP_HAL_QURT/UDPDriver.cpp | 254 +++++++++++++++ libraries/AP_HAL_QURT/UDPDriver.h | 55 ++++ libraries/AP_HAL_QURT/Util.cpp | 27 ++ libraries/AP_HAL_QURT/Util.h | 31 ++ libraries/AP_HAL_QURT/dsp_main.cpp | 107 +++++++ libraries/AP_HAL_QURT/replace.cpp | 76 +++++ libraries/AP_HAL_QURT/replace.h | 39 +++ libraries/AP_HAL_QURT/system.cpp | 66 ++++ 26 files changed, 2186 insertions(+) create mode 100644 libraries/AP_HAL_QURT/AP_HAL_QURT.h create mode 100644 libraries/AP_HAL_QURT/AP_HAL_QURT_Main.h create mode 100644 libraries/AP_HAL_QURT/AP_HAL_QURT_Namespace.h create mode 100644 libraries/AP_HAL_QURT/AP_HAL_QURT_Private.h create mode 100644 libraries/AP_HAL_QURT/HAL_QURT_Class.cpp create mode 100644 libraries/AP_HAL_QURT/HAL_QURT_Class.h create mode 100644 libraries/AP_HAL_QURT/RCInput.cpp create mode 100644 libraries/AP_HAL_QURT/RCInput.h create mode 100644 libraries/AP_HAL_QURT/RCOutput.cpp create mode 100644 libraries/AP_HAL_QURT/RCOutput.h create mode 100644 libraries/AP_HAL_QURT/Scheduler.cpp create mode 100644 libraries/AP_HAL_QURT/Scheduler.h create mode 100644 libraries/AP_HAL_QURT/Semaphores.cpp create mode 100644 libraries/AP_HAL_QURT/Semaphores.h create mode 100644 libraries/AP_HAL_QURT/Storage.cpp create mode 100644 libraries/AP_HAL_QURT/Storage.h create mode 100644 libraries/AP_HAL_QURT/UARTDriver.cpp create mode 100644 libraries/AP_HAL_QURT/UARTDriver.h create mode 100644 libraries/AP_HAL_QURT/UDPDriver.cpp create mode 100644 libraries/AP_HAL_QURT/UDPDriver.h create mode 100644 libraries/AP_HAL_QURT/Util.cpp create mode 100644 libraries/AP_HAL_QURT/Util.h create mode 100644 libraries/AP_HAL_QURT/dsp_main.cpp create mode 100644 libraries/AP_HAL_QURT/replace.cpp create mode 100644 libraries/AP_HAL_QURT/replace.h create mode 100644 libraries/AP_HAL_QURT/system.cpp 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 0000000000000..2c753c203d973 --- /dev/null +++ b/libraries/AP_HAL_QURT/AP_HAL_QURT.h @@ -0,0 +1,25 @@ +/* + 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 + +/* 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 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 0000000000000..a38bfd959c5af --- /dev/null +++ b/libraries/AP_HAL_QURT/AP_HAL_QURT_Main.h @@ -0,0 +1,15 @@ +/* + 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 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 0000000000000..279c588e041fb --- /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 0000000000000..67e7413b247e2 --- /dev/null +++ b/libraries/AP_HAL_QURT/AP_HAL_QURT_Private.h @@ -0,0 +1,23 @@ +/* + 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 + +/* 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" 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 0000000000000..ee34b1d33c018 --- /dev/null +++ b/libraries/AP_HAL_QURT/HAL_QURT_Class.cpp @@ -0,0 +1,144 @@ +/* + 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 +#include + +using namespace QURT; + +static UDPDriver uartADriver; +static UARTDriver uartBDriver("/dev/tty-4"); +static UARTDriver uartCDriver("/dev/tty-2"); +static UARTDriver uartDDriver(nullptr); +static UARTDriver uartEDriver(nullptr); + +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; +static Empty::I2CDeviceManager i2c_mgr_instance; + +bool qurt_ran_overtime; + +HAL_QURT::HAL_QURT() : + AP_HAL::HAL( + &uartADriver, + &uartBDriver, + &uartCDriver, + &uartDDriver, + &uartEDriver, + nullptr, // uartF + &i2c_mgr_instance, + &spiDeviceManager, + &analogIn, + &storageDriver, + &uartADriver, + &gpioDriver, + &rcinDriver, + &rcoutDriver, + &schedulerInstance, + &utilInstance, + nullptr) +{ +} + +void HAL_QURT::run(int argc, char* const argv[], Callbacks* callbacks) const +{ + assert(callbacks); + + int opt; + const struct GetOptLong::option options[] = { + {"uartB", true, 0, 'B'}, + {"uartC", true, 0, 'C'}, + {"uartD", true, 0, 'D'}, + {"uartE", true, 0, 'E'}, + {"dsm", true, 0, 'S'}, + {"ESC", true, 0, 'e'}, + {0, false, 0, 0} + }; + + GetOptLong gopt(argc, argv, "B:C:D:E:e:S", + options); + + /* + parse command line options + */ + while ((opt = gopt.getoption()) != -1) { + switch (opt) { + case 'B': + uartBDriver.set_device_path(gopt.optarg); + break; + case 'C': + uartCDriver.set_device_path(gopt.optarg); + break; + case 'D': + uartDDriver.set_device_path(gopt.optarg); + break; + case 'E': + uartEDriver.set_device_path(gopt.optarg); + break; + case 'e': + rcoutDriver.set_device_path(gopt.optarg); + break; + case 'S': + rcinDriver.set_device_path(gopt.optarg); + break; + default: + printf("Unknown option '%c'\n", (char)opt); + exit(1); + } + } + + /* 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 0000000000000..7c1a1d42b7622 --- /dev/null +++ b/libraries/AP_HAL_QURT/HAL_QURT_Class.h @@ -0,0 +1,25 @@ +/* + 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 HAL_QURT : public AP_HAL::HAL { +public: + HAL_QURT(); + void run(int argc, char* const* argv, Callbacks* callbacks) const override; +}; diff --git a/libraries/AP_HAL_QURT/RCInput.cpp b/libraries/AP_HAL_QURT/RCInput.cpp new file mode 100644 index 0000000000000..bcf7d8ac4676f --- /dev/null +++ b/libraries/AP_HAL_QURT/RCInput.cpp @@ -0,0 +1,184 @@ +#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() +{ + bool ret = new_rc_input; + if (ret) { + new_rc_input = false; + } + return ret; +} + +uint8_t RCInput::num_channels() +{ + return _num_channels; +} + +uint16_t RCInput::read(uint8_t ch) +{ + 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 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 0000000000000..7ebddd79e5a2d --- /dev/null +++ b/libraries/AP_HAL_QURT/RCInput.h @@ -0,0 +1,52 @@ +#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 set_device_path(const char *path) { + device_path = path; + } + + 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_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 0000000000000..d7876a555999c --- /dev/null +++ b/libraries/AP_HAL_QURT/RCOutput.cpp @@ -0,0 +1,126 @@ +#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; + if (!corked) { + 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)); +} + +void RCOutput::cork(void) +{ + corked = true; +} + +void RCOutput::push(void) +{ + if (corked) { + need_write = true; + corked = false; + } +} + +#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 0000000000000..bbef8eb7e3ce2 --- /dev/null +++ b/libraries/AP_HAL_QURT/RCOutput.h @@ -0,0 +1,44 @@ +#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 set_device_path(const char *path) { + device_path = 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 cork(void) override; + void push(void) override; + + 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; + bool corked; +}; + +#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 0000000000000..214fcb1993e8c --- /dev/null +++ b/libraries/AP_HAL_QURT/Scheduler.cpp @@ -0,0 +1,274 @@ +#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 "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_main_thread()) { + ::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) { + call_delay_cb(); + } + } +} + +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 != nullptr) { + _failsafe(); + } + + _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 nullptr; +} + +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(); + hal.uartB->timer_tick(); + hal.uartC->timer_tick(); + hal.uartD->timer_tick(); + hal.uartE->timer_tick(); + hal.uartF->timer_tick(); + } + return nullptr; +} + +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 nullptr; +} + +bool Scheduler::in_main_thread() const +{ + return getpid() == _main_task_pid; +} + +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 0000000000000..ce1bdb24d2723 --- /dev/null +++ b/libraries/AP_HAL_QURT/Scheduler.h @@ -0,0 +1,73 @@ +#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 + +/* 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_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_main_thread() const override; + 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_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 0000000000000..6d5dc6a62ea79 --- /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 == HAL_SEMAPHORE_BLOCK_FOREVER) { + 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 0000000000000..ca7b9ee3d226e --- /dev/null +++ b/libraries/AP_HAL_QURT/Semaphores.h @@ -0,0 +1,34 @@ +#pragma once + +#include +#include + +namespace QURT { + +class Semaphore : public AP_HAL::Semaphore { +public: + friend class BinarySemaphore; + Semaphore(); + bool give() override; + bool take(uint32_t timeout_ms) override; + bool take_nonblocking() override; + +protected: + int dummy_lock; +}; + + +class BinarySemaphore : public AP_HAL::BinarySemaphore { +public: + BinarySemaphore(bool initial_state=false); + + bool wait(uint32_t timeout_us) override; + bool wait_blocking(void) override; + void signal(void) override; + +protected: + Semaphore mtx; + bool pending; +}; + +} diff --git a/libraries/AP_HAL_QURT/Storage.cpp b/libraries/AP_HAL_QURT/Storage.cpp new file mode 100644 index 0000000000000..d9d1f74fd8353 --- /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(HAL_SEMAPHORE_BLOCK_FOREVER); + 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 0000000000000..6f5771150b00c --- /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 0000000000000..f61655cf17181 --- /dev/null +++ b/libraries/AP_HAL_QURT/UARTDriver.cpp @@ -0,0 +1,300 @@ +/* + 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 (devname == nullptr) { + return; + } + 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(HAL_SEMAPHORE_BLOCK_FOREVER)) { + 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(HAL_SEMAPHORE_BLOCK_FOREVER)) { + 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(HAL_SEMAPHORE_BLOCK_FOREVER)) { + 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 0000000000000..c6e22e74ad91a --- /dev/null +++ b/libraries/AP_HAL_QURT/UARTDriver.h @@ -0,0 +1,68 @@ +/* + 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 set_device_path(const char *path) { + devname = path; + } + + 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) override; + +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 0000000000000..90434fe44151b --- /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(HAL_SEMAPHORE_BLOCK_FOREVER)) { + 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(HAL_SEMAPHORE_BLOCK_FOREVER)) { + 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(HAL_SEMAPHORE_BLOCK_FOREVER)) { + 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 0000000000000..3f724eedad1c0 --- /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 0000000000000..08e635c73e87e --- /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 0000000000000..bdf2d054f1298 --- /dev/null +++ b/libraries/AP_HAL_QURT/Util.h @@ -0,0 +1,31 @@ +/* + 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) {} + 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 0000000000000..8349dfa03b650 --- /dev/null +++ b/libraries/AP_HAL_QURT/dsp_main.cpp @@ -0,0 +1,107 @@ +#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 *cmdptr) +{ + char *cmdline = (char *)cmdptr; + HAP_PRINTF("Ardupilot main_thread started"); + + // break cmdline into argc/argv + int argc = 0; + for (int i=0; cmdline[i]; i++) { + if (cmdline[i] == '\n') argc++; + } + const char **argv = (const char **)calloc(argc+2, sizeof(char *)); + argv[0] = &cmdline[0]; + argc = 0; + for (int i=0; cmdline[i]; i++) { + if (cmdline[i] == '\n') { + cmdline[i] = 0; + argv[argc+1] = &cmdline[i+1]; + argc++; + } + } + argv[argc] = nullptr; + + ArduPilot_main(argc, argv); + return nullptr; +} + + +uint32_t ardupilot_start(const char *cmdline, int len) +{ + 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, (void *)strdup((char*)cmdline)); + 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(HAL_SEMAPHORE_BLOCK_FOREVER); + 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(HAL_SEMAPHORE_BLOCK_FOREVER); + 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/replace.cpp b/libraries/AP_HAL_QURT/replace.cpp new file mode 100644 index 0000000000000..050d2c9b77710 --- /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(nullptr, 0, format, ap2); + va_end(ap2); + if (ret < 0) return ret; + + (*ptr) = (char *)malloc(ret+1); + if (!*ptr) return -1; + + va_copy(ap2, ap); + ret = vsnprintf(*ptr, ret+1, format, ap2); + va_end(ap2); + + return ret; +} + +int asprintf(char **ptr, const char *format, ...) +{ + va_list ap; + int ret; + + *ptr = nullptr; + va_start(ap, format); + ret = vasprintf(ptr, format, ap); + va_end(ap); + + return ret; +} + +} diff --git a/libraries/AP_HAL_QURT/replace.h b/libraries/AP_HAL_QURT/replace.h new file mode 100644 index 0000000000000..8f085c2af3c0c --- /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 0000000000000..64f3a57b83cd0 --- /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 From 2623e72e1712ef903b6affc9f2686ca3c912fd01 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 18 May 2024 21:44:17 +1000 Subject: [PATCH 02/73] AP_Filesystem: added QURT support --- libraries/AP_Filesystem/AP_Filesystem.h | 4 ++++ libraries/AP_Filesystem/AP_Filesystem_QURT.h | 12 ++++++++++++ 2 files changed, 16 insertions(+) create mode 100644 libraries/AP_Filesystem/AP_Filesystem_QURT.h diff --git a/libraries/AP_Filesystem/AP_Filesystem.h b/libraries/AP_Filesystem/AP_Filesystem.h index c2bd20e765563..5b44e985d90ef 100644 --- a/libraries/AP_Filesystem/AP_Filesystem.h +++ b/libraries/AP_Filesystem/AP_Filesystem.h @@ -62,6 +62,10 @@ struct dirent { #include "AP_Filesystem_ESP32.h" #endif +#if CONFIG_HAL_BOARD == HAL_BOARD_QURT +#include "AP_Filesystem_QURT.h" +#endif + #include "AP_Filesystem_backend.h" class AP_Filesystem { diff --git a/libraries/AP_Filesystem/AP_Filesystem_QURT.h b/libraries/AP_Filesystem/AP_Filesystem_QURT.h new file mode 100644 index 0000000000000..b1e618e41d96a --- /dev/null +++ b/libraries/AP_Filesystem/AP_Filesystem_QURT.h @@ -0,0 +1,12 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "AP_Filesystem_backend.h" From d9595de0cdb51b7fc194237aa5b173aa69c1370c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 18 May 2024 21:44:32 +1000 Subject: [PATCH 03/73] AP_HAL: added QURT HAL --- libraries/AP_HAL/AP_HAL_Boards.h | 3 +++ libraries/AP_HAL/board/qurt.h | 40 ++++++++++++++++++++++++++++++++ 2 files changed, 43 insertions(+) create mode 100644 libraries/AP_HAL/board/qurt.h diff --git a/libraries/AP_HAL/AP_HAL_Boards.h b/libraries/AP_HAL/AP_HAL_Boards.h index 24b6a66d3b5c6..0f75a557f9b17 100644 --- a/libraries/AP_HAL/AP_HAL_Boards.h +++ b/libraries/AP_HAL/AP_HAL_Boards.h @@ -14,6 +14,7 @@ #define HAL_BOARD_CHIBIOS 10 #define HAL_BOARD_F4LIGHT 11 // reserved #define HAL_BOARD_ESP32 12 +#define HAL_BOARD_QURT 13 #define HAL_BOARD_EMPTY 99 /* Default board subtype is -1 */ @@ -139,6 +140,8 @@ #include #elif CONFIG_HAL_BOARD == HAL_BOARD_ESP32 #include +#elif CONFIG_HAL_BOARD == HAL_BOARD_QURT + #include #else #error "Unknown CONFIG_HAL_BOARD type" #endif diff --git a/libraries/AP_HAL/board/qurt.h b/libraries/AP_HAL/board/qurt.h new file mode 100644 index 0000000000000..7ceab4d3e7717 --- /dev/null +++ b/libraries/AP_HAL/board/qurt.h @@ -0,0 +1,40 @@ +#pragma once + +#define HAL_BOARD_NAME "QURT" +#define HAL_CPU_CLASS HAL_CPU_CLASS_1000 +#define HAL_MEM_CLASS HAL_MEM_CLASS_1000 +#define HAL_STORAGE_SIZE 32768 +#define HAL_STORAGE_SIZE_AVAILABLE HAL_STORAGE_SIZE + +// only include if compiling C++ code +#ifdef __cplusplus +#include +#define HAL_Semaphore QURT::Semaphore +#define HAL_BinarySemaphore QURT::BinarySemaphore +#endif + +#ifndef HAL_HAVE_HARDWARE_DOUBLE +#define HAL_HAVE_HARDWARE_DOUBLE 1 +#endif + +#ifndef HAL_WITH_EKF_DOUBLE +#define HAL_WITH_EKF_DOUBLE HAL_HAVE_HARDWARE_DOUBLE +#endif + +#ifndef HAL_GYROFFT_ENABLED +#define HAL_GYROFFT_ENABLED 0 +#endif + +/* + disable features for initial port + */ +#define AP_SCRIPTING_ENABLED 0 +#define HAL_HAVE_BOARD_VOLTAGE 0 +#define HAL_HAVE_SERVO_VOLTAGE 0 +#define HAL_HAVE_SAFETY_SWITCH 0 + +/* + macros to ease the port + */ +#define strnlen(s,n) strnlen_s(s,n) + From e1208456025128e8967bed69a728e3b9c7d3d641 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 18 May 2024 21:44:44 +1000 Subject: [PATCH 04/73] AP_Common: fixed QURT build --- libraries/AP_Common/AP_Common.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/AP_Common/AP_Common.cpp b/libraries/AP_Common/AP_Common.cpp index 19f2002ac4121..ad1910b72e9d8 100644 --- a/libraries/AP_Common/AP_Common.cpp +++ b/libraries/AP_Common/AP_Common.cpp @@ -19,6 +19,7 @@ #include #include "AP_Common.h" +#include extern const AP_HAL::HAL& hal; From 11b62d7c4238e6f2ca4f476b9d1a8475f26356f6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 18 May 2024 21:44:53 +1000 Subject: [PATCH 05/73] AP_Baro: fixed QURT build --- libraries/AP_Baro/AP_Baro_SPL06.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/AP_Baro/AP_Baro_SPL06.cpp b/libraries/AP_Baro/AP_Baro_SPL06.cpp index baca15dee2d20..591426b8a3f58 100644 --- a/libraries/AP_Baro/AP_Baro_SPL06.cpp +++ b/libraries/AP_Baro/AP_Baro_SPL06.cpp @@ -17,6 +17,7 @@ #if AP_BARO_SPL06_ENABLED #include +#include #include extern const AP_HAL::HAL &hal; From 439819c54a260838ab076e55183fc3a40dd0ac6f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 18 May 2024 21:45:13 +1000 Subject: [PATCH 06/73] Tools: added QURT build --- Tools/ardupilotwaf/boards.py | 60 ++++++++++++++++++++++++++++++++- Tools/ardupilotwaf/toolchain.py | 2 ++ 2 files changed, 61 insertions(+), 1 deletion(-) diff --git a/Tools/ardupilotwaf/boards.py b/Tools/ardupilotwaf/boards.py index b25591523afaf..3997c3d7970c8 100644 --- a/Tools/ardupilotwaf/boards.py +++ b/Tools/ardupilotwaf/boards.py @@ -38,7 +38,10 @@ def __init__(self): def configure(self, cfg): cfg.env.TOOLCHAIN = cfg.options.toolchain or self.toolchain cfg.env.ROMFS_FILES = [] - cfg.load('toolchain') + if hasattr(self,'configure_toolchain'): + self.configure_toolchain(cfg) + else: + cfg.load('toolchain') cfg.load('cxx_checks') env = waflib.ConfigSet.ConfigSet() @@ -190,6 +193,8 @@ def srcpath(path): cfg.msg("Configured VSCode Intellisense:", 'no', color='YELLOW') def cc_version_gte(self, cfg, want_major, want_minor): + if cfg.env.TOOLCHAIN == "custom": + return True (major, minor, patchlevel) = cfg.env.CC_VERSION return (int(major) > want_major or (int(major) == want_major and int(minor) >= want_minor)) @@ -1618,3 +1623,56 @@ class SITL_x86_64_linux_gnu(SITL_static): class SITL_arm_linux_gnueabihf(SITL_static): toolchain = 'arm-linux-gnueabihf' + +class QURT(Board): + '''support for QURT based boards''' + toolchain = 'custom' + + def __init__(self): + self.with_can = False + + def configure_toolchain(self, cfg): + cfg.env.CXX_NAME = 'gcc' + cfg.env.HEXAGON_SDK_DIR = "/opt/hexagon-sdk/4.1.0.4-lite" + cfg.env.CC_VERSION = ('4','1','0') + cfg.env.TOOLCHAIN_DIR = cfg.env.HEXAGON_SDK_DIR + "/tools/HEXAGON_Tools/8.4.05/Tools" + cfg.env.COMPILER_CC = cfg.env.TOOLCHAIN_DIR + "/bin/hexagon-clang" + cfg.env.COMPILER_CXX = cfg.env.TOOLCHAIN_DIR + "/bin/hexagon-clang++" + cfg.env.CXX = ["ccache", cfg.env.COMPILER_CXX] + cfg.env.CXX_TGT_F = ['-c', '-o'] + cfg.env.CPPPATH_ST = '-I%s' + cfg.env.DEFINES_ST = '-D%s' + + + def configure_env(self, cfg, env): + super(QURT, self).configure_env(cfg, env) + + env.BOARD_CLASS = "QURT" + env.HEXAGON_LINK = cfg.env.HEXAGON_SDK_DIR + "/tools/HEXAGON_Tools/8.4.05/Tools/bin/hexagon-link" + env.HEAXGON_APP = "libardupilot.so" + env.HEXAGON_LINKFLAGS = f"-march=hexagon -mcpu=hexagonv66 -shared -call_shared -G0 {cfg.env.TOOLCHAIN_DIR}/target/hexagon/lib/v66/G0/pic/initS.o -L{cfg.env.TOOLCHAIN_DIR}/target/hexagon/lib/v66/G0/pic -Bsymbolic {cfg.env.TOOLCHAIN_DIR}/target/hexagon/lib/v66/G0/pic/libgcc.a --wrap=malloc --wrap=calloc --wrap=free --wrap=realloc --wrap=memalign --wrap=__stack_chk_fail -lc -soname={env.HEXAGON_APP} --start-group --whole-archive OBJECT_LIST --end-group --start-group -lgcc --end-group {cfg.env.TOOLCHAIN_DIR}/target/hexagon/lib/v66/G0/pic/finiS.o" + env.INCLUDES += [cfg.env.HEXAGON_SDK_DIR + "/rtos/qurt/computev66/include/qurt"] + env.CXXFLAGS += ["-fPIC", "-MD"] + + env.DEFINES.update( + CONFIG_HAL_BOARD = 'HAL_BOARD_QURT', + CONFIG_HAL_BOARD_SUBTYPE = 'HAL_BOARD_SUBTYPE_NONE', + AP_SIM_ENABLED = 0, + ) + + if not cfg.env.DEBUG: + env.CXXFLAGS += [ + '-O2', + ] + + env.AP_LIBRARIES += [ + 'AP_HAL_QURT', + ] + + def build(self, bld): + super(QURT, self).build(bld) + + def get_name(self): + # get name of class + return self.__class__.__name__ + diff --git a/Tools/ardupilotwaf/toolchain.py b/Tools/ardupilotwaf/toolchain.py index eb86084307cb2..5fec76f829552 100644 --- a/Tools/ardupilotwaf/toolchain.py +++ b/Tools/ardupilotwaf/toolchain.py @@ -133,6 +133,8 @@ def configure(cfg): _filter_supported_cxx_compilers('g++', 'clang++') cfg.msg('Using toolchain', cfg.env.TOOLCHAIN) + if cfg.env.TOOLCHAIN == "custom": + return if cfg.env.TOOLCHAIN == 'native': cfg.load('compiler_cxx compiler_c') From 6a46d505e40f25b74ef4406c83d71c20840e6174 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 19 May 2024 09:08:41 +1000 Subject: [PATCH 07/73] AP_Compass: cast to unsigned for QURT --- libraries/AP_Compass/AP_Compass_IST8308.cpp | 2 +- libraries/AP_Compass/AP_Compass_IST8310.cpp | 2 +- libraries/AP_Compass/AP_Compass_LIS3MDL.cpp | 2 +- libraries/AP_Compass/AP_Compass_MMC3416.cpp | 2 +- libraries/AP_Compass/AP_Compass_MMC5xx3.cpp | 2 +- libraries/AP_Compass/AP_Compass_QMC5883L.cpp | 2 +- libraries/AP_Compass/AP_Compass_QMC5883P.cpp | 2 +- 7 files changed, 7 insertions(+), 7 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass_IST8308.cpp b/libraries/AP_Compass/AP_Compass_IST8308.cpp index c4c3ea499feec..256a106a80877 100644 --- a/libraries/AP_Compass/AP_Compass_IST8308.cpp +++ b/libraries/AP_Compass/AP_Compass_IST8308.cpp @@ -168,7 +168,7 @@ bool AP_Compass_IST8308::init() set_dev_id(_instance, _dev->get_bus_id()); printf("%s found on bus %u id %u address 0x%02x\n", name, - _dev->bus_num(), _dev->get_bus_id(), _dev->get_bus_address()); + _dev->bus_num(), unsigned(_dev->get_bus_id()), _dev->get_bus_address()); set_rotation(_instance, _rotation); diff --git a/libraries/AP_Compass/AP_Compass_IST8310.cpp b/libraries/AP_Compass/AP_Compass_IST8310.cpp index c2f368ce58a1e..3fb9baf2e7bbe 100644 --- a/libraries/AP_Compass/AP_Compass_IST8310.cpp +++ b/libraries/AP_Compass/AP_Compass_IST8310.cpp @@ -178,7 +178,7 @@ bool AP_Compass_IST8310::init() set_dev_id(_instance, _dev->get_bus_id()); printf("%s found on bus %u id %u address 0x%02x\n", name, - _dev->bus_num(), _dev->get_bus_id(), _dev->get_bus_address()); + _dev->bus_num(), unsigned(_dev->get_bus_id()), _dev->get_bus_address()); set_rotation(_instance, _rotation); diff --git a/libraries/AP_Compass/AP_Compass_LIS3MDL.cpp b/libraries/AP_Compass/AP_Compass_LIS3MDL.cpp index 713ff00befe2f..828c9cf632ec1 100644 --- a/libraries/AP_Compass/AP_Compass_LIS3MDL.cpp +++ b/libraries/AP_Compass/AP_Compass_LIS3MDL.cpp @@ -113,7 +113,7 @@ bool AP_Compass_LIS3MDL::init() } set_dev_id(compass_instance, dev->get_bus_id()); - printf("Found a LIS3MDL on 0x%x as compass %u\n", dev->get_bus_id(), compass_instance); + printf("Found a LIS3MDL on 0x%x as compass %u\n", unsigned(dev->get_bus_id()), compass_instance); set_rotation(compass_instance, rotation); diff --git a/libraries/AP_Compass/AP_Compass_MMC3416.cpp b/libraries/AP_Compass/AP_Compass_MMC3416.cpp index b8003869c599d..f069a822f5ea1 100644 --- a/libraries/AP_Compass/AP_Compass_MMC3416.cpp +++ b/libraries/AP_Compass/AP_Compass_MMC3416.cpp @@ -99,7 +99,7 @@ bool AP_Compass_MMC3416::init() set_dev_id(compass_instance, dev->get_bus_id()); - printf("Found a MMC3416 on 0x%x as compass %u\n", dev->get_bus_id(), compass_instance); + printf("Found a MMC3416 on 0x%x as compass %u\n", unsigned(dev->get_bus_id()), compass_instance); set_rotation(compass_instance, rotation); diff --git a/libraries/AP_Compass/AP_Compass_MMC5xx3.cpp b/libraries/AP_Compass/AP_Compass_MMC5xx3.cpp index d671f3deb5b79..79db90cf8faae 100644 --- a/libraries/AP_Compass/AP_Compass_MMC5xx3.cpp +++ b/libraries/AP_Compass/AP_Compass_MMC5xx3.cpp @@ -115,7 +115,7 @@ bool AP_Compass_MMC5XX3::init() set_dev_id(compass_instance, dev->get_bus_id()); - printf("Found a MMC5983 on 0x%x as compass %u\n", dev->get_bus_id(), compass_instance); + printf("Found a MMC5983 on 0x%x as compass %u\n", unsigned(dev->get_bus_id()), compass_instance); set_rotation(compass_instance, rotation); diff --git a/libraries/AP_Compass/AP_Compass_QMC5883L.cpp b/libraries/AP_Compass/AP_Compass_QMC5883L.cpp index ad1bea49892a3..d5b81ec4a4640 100644 --- a/libraries/AP_Compass/AP_Compass_QMC5883L.cpp +++ b/libraries/AP_Compass/AP_Compass_QMC5883L.cpp @@ -123,7 +123,7 @@ bool AP_Compass_QMC5883L::init() set_dev_id(_instance, _dev->get_bus_id()); printf("%s found on bus %u id %u address 0x%02x\n", name, - _dev->bus_num(), _dev->get_bus_id(), _dev->get_bus_address()); + _dev->bus_num(), unsigned(_dev->get_bus_id()), _dev->get_bus_address()); set_rotation(_instance, _rotation); diff --git a/libraries/AP_Compass/AP_Compass_QMC5883P.cpp b/libraries/AP_Compass/AP_Compass_QMC5883P.cpp index 80d6e46f1c699..7bc8293820a5f 100644 --- a/libraries/AP_Compass/AP_Compass_QMC5883P.cpp +++ b/libraries/AP_Compass/AP_Compass_QMC5883P.cpp @@ -134,7 +134,7 @@ bool AP_Compass_QMC5883P::init() set_dev_id(_instance, _dev->get_bus_id()); printf("%s found on bus %u id %u address 0x%02x\n", name, - _dev->bus_num(), _dev->get_bus_id(), _dev->get_bus_address()); + _dev->bus_num(), unsigned(_dev->get_bus_id()), _dev->get_bus_address()); set_rotation(_instance, _rotation); From 43cd984b0d655e8c6bfdec1dc613cf5b8d98cf36 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 19 May 2024 09:08:58 +1000 Subject: [PATCH 08/73] AP_Filesystem: support no filesystem reading --- libraries/AP_Filesystem/AP_Filesystem.cpp | 4 ++++ libraries/AP_Filesystem/AP_Filesystem_Sys.cpp | 2 ++ 2 files changed, 6 insertions(+) diff --git a/libraries/AP_Filesystem/AP_Filesystem.cpp b/libraries/AP_Filesystem/AP_Filesystem.cpp index 63f575bb5be2d..4a97061d7b916 100644 --- a/libraries/AP_Filesystem/AP_Filesystem.cpp +++ b/libraries/AP_Filesystem/AP_Filesystem.cpp @@ -16,6 +16,9 @@ #include "AP_Filesystem.h" #include "AP_Filesystem_config.h" + +#if AP_FILESYSTEM_FILE_READING_ENABLED + #include #include #include @@ -418,3 +421,4 @@ AP_Filesystem &FS() } } +#endif // AP_FILESYSTEM_FILE_READING_ENABLED diff --git a/libraries/AP_Filesystem/AP_Filesystem_Sys.cpp b/libraries/AP_Filesystem/AP_Filesystem_Sys.cpp index 9eadd7f607b40..d673ee71d0f01 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_Sys.cpp +++ b/libraries/AP_Filesystem/AP_Filesystem_Sys.cpp @@ -244,7 +244,9 @@ struct dirent *AP_Filesystem_Sys::readdir(void *dirp) // we have reached end of list return nullptr; } +#if AP_FILESYSTEM_HAVE_DIRENT_DTYPE dtracker->curr_file.d_type = DT_REG; +#endif size_t max_length = ARRAY_SIZE(dtracker->curr_file.d_name); strncpy_noterm(dtracker->curr_file.d_name, sysfs_file_list[dtracker->file_offset].name, max_length); dtracker->file_offset++; From 148a9cddff03f780a81529844da875fc8b03c1b0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 19 May 2024 09:09:17 +1000 Subject: [PATCH 09/73] AP_HAL: added more replacement functions for QURT --- libraries/AP_HAL/AP_HAL_Boards.h | 4 ++++ libraries/AP_HAL/board/qurt.h | 22 ++++++++++++++++++---- libraries/AP_HAL_QURT/replace.h | 5 +++++ 3 files changed, 27 insertions(+), 4 deletions(-) diff --git a/libraries/AP_HAL/AP_HAL_Boards.h b/libraries/AP_HAL/AP_HAL_Boards.h index 0f75a557f9b17..7a49997c7317d 100644 --- a/libraries/AP_HAL/AP_HAL_Boards.h +++ b/libraries/AP_HAL/AP_HAL_Boards.h @@ -385,3 +385,7 @@ #endif #define HAL_GPIO_LED_OFF (!HAL_GPIO_LED_ON) + +#ifndef AP_FILESYSTEM_HAVE_DIRENT_DTYPE +#define AP_FILESYSTEM_HAVE_DIRENT_DTYPE 1 +#endif diff --git a/libraries/AP_HAL/board/qurt.h b/libraries/AP_HAL/board/qurt.h index 7ceab4d3e7717..6ff502e9b7dff 100644 --- a/libraries/AP_HAL/board/qurt.h +++ b/libraries/AP_HAL/board/qurt.h @@ -14,7 +14,7 @@ #endif #ifndef HAL_HAVE_HARDWARE_DOUBLE -#define HAL_HAVE_HARDWARE_DOUBLE 1 +#define HAL_HAVE_HARDWARE_DOUBLE 0 #endif #ifndef HAL_WITH_EKF_DOUBLE @@ -32,9 +32,23 @@ #define HAL_HAVE_BOARD_VOLTAGE 0 #define HAL_HAVE_SERVO_VOLTAGE 0 #define HAL_HAVE_SAFETY_SWITCH 0 +#define HAL_WITH_MCU_MONITORING 0 +#define HAL_USE_QUADSPI 0 +#define HAL_LOGGING_ENABLED 0 +#define HAL_WITH_DSP 0 +#define HAL_CANFD_SUPPORTED 0 +#define HAL_NUM_CAN_IFACES 0 +#define AP_CRASHDUMP_ENABLED 0 +#define HAL_ENABLE_DFU_BOOT 0 +#define HAL_PARAM_DEFAULTS_PATH nullptr +#define AP_FILESYSTEM_HAVE_DIRENT_DTYPE 0 +#define AP_FILESYSTEM_PARAM_ENABLED 0 +#define AP_FILESYSTEM_FILE_READING_ENABLED 0 + +#define USE_LIBC_REALLOC 1 + /* - macros to ease the port + bring in missing standard library functions */ -#define strnlen(s,n) strnlen_s(s,n) - +#include diff --git a/libraries/AP_HAL_QURT/replace.h b/libraries/AP_HAL_QURT/replace.h index 8f085c2af3c0c..c208403ac8860 100644 --- a/libraries/AP_HAL_QURT/replace.h +++ b/libraries/AP_HAL_QURT/replace.h @@ -21,6 +21,8 @@ extern "C" { off_t lseek(int, off_t, int); DIR *opendir (const char *); int unlink(const char *pathname); + void *memmem(const void *haystack, size_t haystacklen, + const void *needle, size_t needlelen); //typedef int32_t pid_t; pid_t getpid (void); @@ -36,4 +38,7 @@ extern volatile uint32_t _last_counter; #define HAP_LINE() do { _last_dsp_line = __LINE__; _last_dsp_file = __FILE__; _last_counter++; } while (0) +// missing defines from math.h +#define M_SQRT1_2 0.70710678118654752440 + #endif // CONFIG_HAL_BOARD From dee2f70b250259b0e53894beecbedc82849ac907 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 19 May 2024 09:09:42 +1000 Subject: [PATCH 10/73] AP_OpticalFlow: cast to unsigned for QURT --- libraries/AP_OpticalFlow/AP_OpticalFlow_PX4Flow.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4Flow.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4Flow.cpp index 0f72760d02c30..6c2130615302c 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4Flow.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4Flow.cpp @@ -78,7 +78,7 @@ bool AP_OpticalFlow_PX4Flow::scan_buses(void) struct i2c_integral_frame frame; success = tdev->read_registers(REG_INTEGRAL_FRAME, (uint8_t *)&frame, sizeof(frame)); if (success) { - printf("Found PX4Flow on bus %u\n", bus); + printf("Found PX4Flow on bus %u\n", unsigned(bus)); dev = std::move(tdev); break; } From bf81eb712e68af984fbdc90ff19ef7cd7d3e5281 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 19 May 2024 09:09:42 +1000 Subject: [PATCH 11/73] AP_RangeFinder: cast to unsigned for QURT --- libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp | 2 +- libraries/AP_RangeFinder/AP_RangeFinder_VL53L0X.cpp | 2 +- libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp index 608f2492efa3b..37cf958647ccb 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp @@ -207,7 +207,7 @@ bool AP_RangeFinder_PulsedLightLRF::init(void) } } - printf("Found LidarLite device=0x%x v2=%d v3hp=%d\n", _dev->get_bus_id(), (int)v2_hardware, (int)v3hp_hardware); + printf("Found LidarLite device=0x%x v2=%d v3hp=%d\n", unsigned(_dev->get_bus_id()), (int)v2_hardware, (int)v3hp_hardware); _dev->get_semaphore()->give(); diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_VL53L0X.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_VL53L0X.cpp index 51c10dec8cd00..e83a0994437ed 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_VL53L0X.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_VL53L0X.cpp @@ -259,7 +259,7 @@ bool AP_RangeFinder_VL53L0X::check_id(void) v2 != 0xAA) { return false; } - printf("Detected VL53L0X on bus 0x%x\n", dev->get_bus_id()); + printf("Detected VL53L0X on bus 0x%x\n", unsigned(dev->get_bus_id())); return true; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.cpp index e0eba359eee8f..ffced316d2276 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.cpp @@ -81,7 +81,7 @@ bool AP_RangeFinder_VL53L1X::check_id(void) (v2 != 0xCC)) { return false; } - printf("Detected VL53L1X on bus 0x%x\n", dev->get_bus_id()); + printf("Detected VL53L1X on bus 0x%x\n", unsigned(dev->get_bus_id())); return true; } From 6d64063a93eaefb94ed7632be8fc80770b843f8e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 19 May 2024 09:10:17 +1000 Subject: [PATCH 12/73] GCS_MAVLink: support no filesystem reading --- libraries/GCS_MAVLink/GCS_FTP.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/libraries/GCS_MAVLink/GCS_FTP.cpp b/libraries/GCS_MAVLink/GCS_FTP.cpp index 720142a839ef5..920bd6a406991 100644 --- a/libraries/GCS_MAVLink/GCS_FTP.cpp +++ b/libraries/GCS_MAVLink/GCS_FTP.cpp @@ -588,6 +588,7 @@ void GCS_MAVLINK::ftp_worker(void) { // calculates how much string length is needed to fit this in a list response int GCS_MAVLINK::gen_dir_entry(char *dest, size_t space, const char *path, const struct dirent * entry) { +#if AP_FILESYSTEM_FILE_READING_ENABLED const bool is_file = entry->d_type == DT_REG || entry->d_type == DT_LNK; if (space < 3) { @@ -616,6 +617,9 @@ int GCS_MAVLINK::gen_dir_entry(char *dest, size_t space, const char *path, const } else { return hal.util->snprintf(dest, space, "D%s%c", entry->d_name, (char)0); } +#else + return -1; +#endif // AP_FILESYSTEM_FILE_READING_ENABLED } // list the contents of a directory, skip the offset number of entries before providing data From 6fd1fba41ef5934dbf42eb452613ac6de95465f8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 19 May 2024 09:39:00 +1000 Subject: [PATCH 13/73] HAL_QURT: more hackery --- libraries/AP_HAL_QURT/AP_HAL_QURT_Private.h | 1 - libraries/AP_HAL_QURT/RCInput.cpp | 183 ++------------ libraries/AP_HAL_QURT/RCInput.h | 52 +--- libraries/AP_HAL_QURT/RCOutput.cpp | 123 ++-------- libraries/AP_HAL_QURT/RCOutput.h | 48 +--- libraries/AP_HAL_QURT/UARTDriver.h | 56 ++--- libraries/AP_HAL_QURT/UDPDriver.cpp | 254 -------------------- libraries/AP_HAL_QURT/UDPDriver.h | 55 ----- libraries/AP_HAL_QURT/dsp_main.cpp | 1 - libraries/AP_HAL_QURT/replace.h | 4 - libraries/AP_HAL_QURT/system.cpp | 4 - 11 files changed, 66 insertions(+), 715 deletions(-) delete mode 100644 libraries/AP_HAL_QURT/UDPDriver.cpp delete mode 100644 libraries/AP_HAL_QURT/UDPDriver.h diff --git a/libraries/AP_HAL_QURT/AP_HAL_QURT_Private.h b/libraries/AP_HAL_QURT/AP_HAL_QURT_Private.h index 67e7413b247e2..95a6b970037a2 100644 --- a/libraries/AP_HAL_QURT/AP_HAL_QURT_Private.h +++ b/libraries/AP_HAL_QURT/AP_HAL_QURT_Private.h @@ -19,5 +19,4 @@ */ #include "UARTDriver.h" -#include "UDPDriver.h" #include "Util.h" diff --git a/libraries/AP_HAL_QURT/RCInput.cpp b/libraries/AP_HAL_QURT/RCInput.cpp index bcf7d8ac4676f..90fb90f1c0078 100644 --- a/libraries/AP_HAL_QURT/RCInput.cpp +++ b/libraries/AP_HAL_QURT/RCInput.cpp @@ -1,184 +1,31 @@ -#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 ); -} +RCInput::RCInput() +{} 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() -{ - bool ret = new_rc_input; - if (ret) { - new_rc_input = false; - } - return ret; -} - -uint8_t RCInput::num_channels() -{ - return _num_channels; -} +{} -uint16_t RCInput::read(uint8_t ch) -{ - 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 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 - } - } +uint8_t RCInput::read(uint16_t* periods, uint8_t len) { + for (uint8_t i = 0; i < len; i++){ + if (i == 2) periods[i] = 900; + else periods[i] = 1500; } + return len; } -#endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_HAL_QURT/RCInput.h b/libraries/AP_HAL_QURT/RCInput.h index 7ebddd79e5a2d..d9918ad2c736d 100644 --- a/libraries/AP_HAL_QURT/RCInput.h +++ b/libraries/AP_HAL_QURT/RCInput.h @@ -2,51 +2,13 @@ #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 set_device_path(const char *path) { - device_path = path; - } - - 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_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; + RCInput(); + void init() override; + bool new_input() override; + uint8_t num_channels() override; + uint16_t read(uint8_t ch) override; + uint8_t read(uint16_t* periods, uint8_t len) override; + virtual const char *protocol() const override { return "Empty"; } }; - diff --git a/libraries/AP_HAL_QURT/RCOutput.cpp b/libraries/AP_HAL_QURT/RCOutput.cpp index d7876a555999c..07fb94aa8477c 100644 --- a/libraries/AP_HAL_QURT/RCOutput.cpp +++ b/libraries/AP_HAL_QURT/RCOutput.cpp @@ -1,126 +1,41 @@ -#include - -#if CONFIG_HAL_BOARD == HAL_BOARD_QURT #include "RCOutput.h" -#include -#include -#include -#include -#include - -extern const AP_HAL::HAL& hal; +#include using namespace QURT; -void RCOutput::init() -{ -} +void RCOutput::init() {} -void RCOutput::set_freq(uint32_t chmask, uint16_t freq_hz) -{ - // no support for changing frequency yet -} +void RCOutput::set_freq(uint32_t chmask, uint16_t freq_hz) {} -uint16_t RCOutput::get_freq(uint8_t ch) -{ - // return fixed fake value - return 490; +uint16_t RCOutput::get_freq(uint8_t chan) { + return 50; } -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; - if (!corked) { - need_write = true; + if (chan < ARRAY_SIZE(value)) { + value[chan] = period_us; } } -uint16_t RCOutput::read(uint8_t ch) +uint16_t RCOutput::read(uint8_t chan) { - if (ch >= channel_count) { - return 0; + if (chan < ARRAY_SIZE(value)) { + return value[chan]; } - return period[ch]; + return 900; } -void RCOutput::read(uint16_t *period_us, uint8_t len) +void RCOutput::read(uint16_t* period_us, uint8_t len) { - for (int i = 0; i < len; i++) { - period_us[i] = read(i); - } + len = MIN(len, ARRAY_SIZE(value)); + memcpy(period_us, value, len*sizeof(value[0])); } -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)); -} - -void RCOutput::cork(void) -{ - corked = true; -} - -void RCOutput::push(void) -{ - if (corked) { - need_write = true; - corked = false; - } -} - -#endif // CONFIG_HAL_BOARD_SUBTYPE - diff --git a/libraries/AP_HAL_QURT/RCOutput.h b/libraries/AP_HAL_QURT/RCOutput.h index bbef8eb7e3ce2..1c78839b50311 100644 --- a/libraries/AP_HAL_QURT/RCOutput.h +++ b/libraries/AP_HAL_QURT/RCOutput.h @@ -1,44 +1,18 @@ #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 set_device_path(const char *path) { - device_path = 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 cork(void) override; - void push(void) override; - - void timer_update(void); - + void init() override; + void set_freq(uint32_t chmask, uint16_t freq_hz) override; + uint16_t get_freq(uint8_t ch) override; + void enable_ch(uint8_t ch) override; + void disable_ch(uint8_t ch) override; + void write(uint8_t ch, uint16_t period_us) override; + uint16_t read(uint8_t ch) override; + void read(uint16_t* period_us, uint8_t len) override; + void cork(void) override {} + void push(void) override {} 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; - bool corked; + uint16_t value[16]; }; - -#endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_HAL_QURT/UARTDriver.h b/libraries/AP_HAL_QURT/UARTDriver.h index c6e22e74ad91a..1a9fb5b6e363f 100644 --- a/libraries/AP_HAL_QURT/UARTDriver.h +++ b/libraries/AP_HAL_QURT/UARTDriver.h @@ -23,46 +23,18 @@ class QURT::UARTDriver : public AP_HAL::UARTDriver { public: UARTDriver(const char *name); - /* QURT implementations of UARTDriver virtual methods */ - - void set_device_path(const char *path) { - devname = path; - } - - 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) override; - -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); + bool is_initialized() override; + bool tx_pending() override; + + /* Empty implementations of Stream virtual methods */ + uint32_t txspace() override; + +protected: + void _begin(uint32_t b, uint16_t rxS, uint16_t txS) override; + size_t _write(const uint8_t *buffer, size_t size) override; + ssize_t _read(uint8_t *buffer, uint16_t size) override WARN_IF_UNUSED; + void _end() override; + void _flush() override; + uint32_t _available() override; + bool _discard_input() override; }; diff --git a/libraries/AP_HAL_QURT/UDPDriver.cpp b/libraries/AP_HAL_QURT/UDPDriver.cpp deleted file mode 100644 index 90434fe44151b..0000000000000 --- a/libraries/AP_HAL_QURT/UDPDriver.cpp +++ /dev/null @@ -1,254 +0,0 @@ -/* - 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(HAL_SEMAPHORE_BLOCK_FOREVER)) { - 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(HAL_SEMAPHORE_BLOCK_FOREVER)) { - 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(HAL_SEMAPHORE_BLOCK_FOREVER)) { - 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 deleted file mode 100644 index 3f724eedad1c0..0000000000000 --- a/libraries/AP_HAL_QURT/UDPDriver.h +++ /dev/null @@ -1,55 +0,0 @@ -/* - 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/dsp_main.cpp b/libraries/AP_HAL_QURT/dsp_main.cpp index 8349dfa03b650..cc0125cadf66a 100644 --- a/libraries/AP_HAL_QURT/dsp_main.cpp +++ b/libraries/AP_HAL_QURT/dsp_main.cpp @@ -1,5 +1,4 @@ #include -#include "UDPDriver.h" #include #include #include diff --git a/libraries/AP_HAL_QURT/replace.h b/libraries/AP_HAL_QURT/replace.h index c208403ac8860..cc1513ea34009 100644 --- a/libraries/AP_HAL_QURT/replace.h +++ b/libraries/AP_HAL_QURT/replace.h @@ -1,7 +1,5 @@ #pragma once -#include -#if CONFIG_HAL_BOARD == HAL_BOARD_QURT #include #include #include @@ -40,5 +38,3 @@ extern volatile uint32_t _last_counter; // missing defines from math.h #define M_SQRT1_2 0.70710678118654752440 - -#endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_HAL_QURT/system.cpp b/libraries/AP_HAL_QURT/system.cpp index 64f3a57b83cd0..65b8b919c7728 100644 --- a/libraries/AP_HAL_QURT/system.cpp +++ b/libraries/AP_HAL_QURT/system.cpp @@ -6,8 +6,6 @@ #include #include -#include -#include #include "replace.h" #include @@ -34,8 +32,6 @@ void panic(const char *errormsg, ...) vsnprintf(buf, sizeof(buf), errormsg, ap); va_end(ap); HAP_PRINTF(buf); - usleep(2000000); - hal.rcin->deinit(); exit(1); } From c32942d8e7662c3ff37d53ee35b190c131540720 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 19 May 2024 09:39:17 +1000 Subject: [PATCH 14/73] AP_HAL: cope with lua longjmp for C++ --- libraries/AP_HAL/board/qurt.h | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/AP_HAL/board/qurt.h b/libraries/AP_HAL/board/qurt.h index 6ff502e9b7dff..732c4145722f1 100644 --- a/libraries/AP_HAL/board/qurt.h +++ b/libraries/AP_HAL/board/qurt.h @@ -44,6 +44,7 @@ #define AP_FILESYSTEM_HAVE_DIRENT_DTYPE 0 #define AP_FILESYSTEM_PARAM_ENABLED 0 #define AP_FILESYSTEM_FILE_READING_ENABLED 0 +#define LUA_USE_LONGJMP 1 #define USE_LIBC_REALLOC 1 From 3f4c630f5039fc334efadc1c196cc6228dc5a9f1 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 19 May 2024 10:29:56 +1000 Subject: [PATCH 15/73] AP_HAL_ChibiOS: removed Util::run_debug_shell() unused function --- libraries/AP_HAL_ChibiOS/Util.h | 1 - 1 file changed, 1 deletion(-) diff --git a/libraries/AP_HAL_ChibiOS/Util.h b/libraries/AP_HAL_ChibiOS/Util.h index 8e431484a096d..1df3e32c767de 100644 --- a/libraries/AP_HAL_ChibiOS/Util.h +++ b/libraries/AP_HAL_ChibiOS/Util.h @@ -37,7 +37,6 @@ class ChibiOS::Util : public AP_HAL::Util { return static_cast(util); } - bool run_debug_shell(AP_HAL::BetterStream *stream) override { return false; } uint32_t available_memory() override; // get path to custom defaults file for AP_Param From ce9bb1bf67894e8113857c4bbc18cf36a8cd4e51 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 19 May 2024 10:29:56 +1000 Subject: [PATCH 16/73] AP_HAL_Empty: removed Util::run_debug_shell() unused function --- libraries/AP_HAL_Empty/Util.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/libraries/AP_HAL_Empty/Util.h b/libraries/AP_HAL_Empty/Util.h index 3dae90c2f82e5..55e590a20e37b 100644 --- a/libraries/AP_HAL_Empty/Util.h +++ b/libraries/AP_HAL_Empty/Util.h @@ -4,6 +4,4 @@ #include "AP_HAL_Empty_Namespace.h" class Empty::Util : public AP_HAL::Util { -public: - bool run_debug_shell(AP_HAL::BetterStream *stream) override { return false; } }; From 9aea969cca12b28e299ac256a4dd4cc2b0e8233e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 19 May 2024 10:29:56 +1000 Subject: [PATCH 17/73] AP_HAL_ESP32: removed Util::run_debug_shell() unused function --- libraries/AP_HAL_ESP32/Util.h | 4 ---- 1 file changed, 4 deletions(-) diff --git a/libraries/AP_HAL_ESP32/Util.h b/libraries/AP_HAL_ESP32/Util.h index 3360e1ad5a197..fcf83ec3b613a 100644 --- a/libraries/AP_HAL_ESP32/Util.h +++ b/libraries/AP_HAL_ESP32/Util.h @@ -28,10 +28,6 @@ class ESP32::Util : public AP_HAL::Util return static_cast(util); } - bool run_debug_shell(AP_HAL::BetterStream *stream) override - { - return false; - } uint32_t available_memory() override; // Special Allocation Routines From 8e7746a91d40e08cb8237049bf25b75e7ef4f136 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 19 May 2024 10:29:56 +1000 Subject: [PATCH 18/73] AP_HAL: removed Util::run_debug_shell() unused function --- libraries/AP_HAL/Util.h | 5 ----- 1 file changed, 5 deletions(-) diff --git a/libraries/AP_HAL/Util.h b/libraries/AP_HAL/Util.h index da22f1a8be1d9..1d2fcdafce737 100644 --- a/libraries/AP_HAL/Util.h +++ b/libraries/AP_HAL/Util.h @@ -44,11 +44,6 @@ class AP_HAL::Util { // set command line parameters to the eeprom on start virtual void set_cmdline_parameters() {}; - // run a debug shall on the given stream if possible. This is used - // to support dropping into a debug shell to run firmware upgrade - // commands - virtual bool run_debug_shell(AP_HAL::BetterStream *stream) = 0; - enum safety_state : uint8_t { SAFETY_NONE, SAFETY_DISARMED, From 56eda48a3a07ca9725b39fc176ca11a0722fd852 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 19 May 2024 10:29:56 +1000 Subject: [PATCH 19/73] AP_HAL_Linux: removed Util::run_debug_shell() unused function --- libraries/AP_HAL_Linux/Util.h | 1 - 1 file changed, 1 deletion(-) diff --git a/libraries/AP_HAL_Linux/Util.h b/libraries/AP_HAL_Linux/Util.h index 9aa0a82314678..70faada85b601 100644 --- a/libraries/AP_HAL_Linux/Util.h +++ b/libraries/AP_HAL_Linux/Util.h @@ -29,7 +29,6 @@ class Util : public AP_HAL::Util { } void init(int argc, char *const *argv); - bool run_debug_shell(AP_HAL::BetterStream *stream) override { return false; } /** return commandline arguments, if available From 5997bbbb61a9feea952a164179235e76b43f020c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 19 May 2024 10:29:56 +1000 Subject: [PATCH 20/73] AP_HAL_SITL: removed Util::run_debug_shell() unused function --- libraries/AP_HAL_SITL/Util.h | 4 ---- 1 file changed, 4 deletions(-) diff --git a/libraries/AP_HAL_SITL/Util.h b/libraries/AP_HAL_SITL/Util.h index 588fc2a25a311..cbfbeda556670 100644 --- a/libraries/AP_HAL_SITL/Util.h +++ b/libraries/AP_HAL_SITL/Util.h @@ -17,10 +17,6 @@ class HALSITL::Util : public AP_HAL::Util { Util(SITL_State *_sitlState) : sitlState(_sitlState) {} - bool run_debug_shell(AP_HAL::BetterStream *stream) override { - return false; - } - /** how much free memory do we have in bytes. */ From 7a62c63544a0a55c077c46cb8d6579821011c7d8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 19 May 2024 10:54:54 +1000 Subject: [PATCH 21/73] Tools: added posix includes for QURT --- Tools/ardupilotwaf/boards.py | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/ardupilotwaf/boards.py b/Tools/ardupilotwaf/boards.py index 3997c3d7970c8..1392bdf07cbbe 100644 --- a/Tools/ardupilotwaf/boards.py +++ b/Tools/ardupilotwaf/boards.py @@ -1652,6 +1652,7 @@ def configure_env(self, cfg, env): env.HEAXGON_APP = "libardupilot.so" env.HEXAGON_LINKFLAGS = f"-march=hexagon -mcpu=hexagonv66 -shared -call_shared -G0 {cfg.env.TOOLCHAIN_DIR}/target/hexagon/lib/v66/G0/pic/initS.o -L{cfg.env.TOOLCHAIN_DIR}/target/hexagon/lib/v66/G0/pic -Bsymbolic {cfg.env.TOOLCHAIN_DIR}/target/hexagon/lib/v66/G0/pic/libgcc.a --wrap=malloc --wrap=calloc --wrap=free --wrap=realloc --wrap=memalign --wrap=__stack_chk_fail -lc -soname={env.HEXAGON_APP} --start-group --whole-archive OBJECT_LIST --end-group --start-group -lgcc --end-group {cfg.env.TOOLCHAIN_DIR}/target/hexagon/lib/v66/G0/pic/finiS.o" env.INCLUDES += [cfg.env.HEXAGON_SDK_DIR + "/rtos/qurt/computev66/include/qurt"] + env.INCLUDES += [cfg.env.HEXAGON_SDK_DIR + "/rtos/qurt/computev66/include/posix"] env.CXXFLAGS += ["-fPIC", "-MD"] env.DEFINES.update( From 74f462db6fdc686b640d9bfa066a6a328ef761fc Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 19 May 2024 10:55:57 +1000 Subject: [PATCH 22/73] AP_HAL_ChibiOS: use if for ENABLE_HEAP allows for HAL with no heap functions --- libraries/AP_HAL_ChibiOS/Util.cpp | 2 +- libraries/AP_HAL_ChibiOS/Util.h | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/Util.cpp b/libraries/AP_HAL_ChibiOS/Util.cpp index 03a8b3e745027..29d5e31d05a57 100644 --- a/libraries/AP_HAL_ChibiOS/Util.cpp +++ b/libraries/AP_HAL_ChibiOS/Util.cpp @@ -96,7 +96,7 @@ void Util::free_type(void *ptr, size_t size, AP_HAL::Util::Memory_Type mem_type) } -#ifdef ENABLE_HEAP +#if ENABLE_HEAP void *Util::allocate_heap_memory(size_t size) { diff --git a/libraries/AP_HAL_ChibiOS/Util.h b/libraries/AP_HAL_ChibiOS/Util.h index 1df3e32c767de..f11dbf7239aad 100644 --- a/libraries/AP_HAL_ChibiOS/Util.h +++ b/libraries/AP_HAL_ChibiOS/Util.h @@ -48,7 +48,7 @@ class ChibiOS::Util : public AP_HAL::Util { void *malloc_type(size_t size, AP_HAL::Util::Memory_Type mem_type) override; void free_type(void *ptr, size_t size, AP_HAL::Util::Memory_Type mem_type) override; -#ifdef ENABLE_HEAP +#if ENABLE_HEAP // heap functions, note that a heap once alloc'd cannot be dealloc'd virtual void *allocate_heap_memory(size_t size) override; virtual void *heap_realloc(void *heap, void *ptr, size_t old_size, size_t new_size) override; @@ -142,7 +142,7 @@ class ChibiOS::Util : public AP_HAL::Util { FlashBootloader flash_bootloader() override; #endif -#ifdef ENABLE_HEAP +#if ENABLE_HEAP static memory_heap_t scripting_heap; #endif // ENABLE_HEAP From 105484d334582e8ff4d466ee691a413ea131c958 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 19 May 2024 10:55:57 +1000 Subject: [PATCH 23/73] AP_HAL_ESP32: use if for ENABLE_HEAP allows for HAL with no heap functions --- libraries/AP_HAL_ESP32/Util.cpp | 2 +- libraries/AP_HAL_ESP32/Util.h | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AP_HAL_ESP32/Util.cpp b/libraries/AP_HAL_ESP32/Util.cpp index d232ff890765e..2eab971532339 100644 --- a/libraries/AP_HAL_ESP32/Util.cpp +++ b/libraries/AP_HAL_ESP32/Util.cpp @@ -90,7 +90,7 @@ void Util::free_type(void *ptr, size_t size, AP_HAL::Util::Memory_Type mem_type) } -#ifdef ENABLE_HEAP +#if ENABLE_HEAP void *Util::allocate_heap_memory(size_t size) { diff --git a/libraries/AP_HAL_ESP32/Util.h b/libraries/AP_HAL_ESP32/Util.h index fcf83ec3b613a..8e9ba9dc94221 100644 --- a/libraries/AP_HAL_ESP32/Util.h +++ b/libraries/AP_HAL_ESP32/Util.h @@ -34,7 +34,7 @@ class ESP32::Util : public AP_HAL::Util void *malloc_type(size_t size, AP_HAL::Util::Memory_Type mem_type) override; void free_type(void *ptr, size_t size, AP_HAL::Util::Memory_Type mem_type) override; -#ifdef ENABLE_HEAP +#if ENABLE_HEAP // heap functions, note that a heap once alloc'd cannot be dealloc'd virtual void *allocate_heap_memory(size_t size) override; virtual void *heap_realloc(void *heap, void *ptr, size_t old_size, size_t new_size) override; @@ -85,7 +85,7 @@ class ESP32::Util : public AP_HAL::Util FlashBootloader flash_bootloader() override; #endif -#ifdef ENABLE_HEAP +#if ENABLE_HEAP // static memory_heap_t scripting_heap; #endif // ENABLE_HEAP From a6f4a4447fc5cbd26ac17e63eda733a9feb60e67 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 19 May 2024 10:55:58 +1000 Subject: [PATCH 24/73] AP_HAL: use if for ENABLE_HEAP allows for HAL with no heap functions --- libraries/AP_HAL/Util.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_HAL/Util.h b/libraries/AP_HAL/Util.h index 1d2fcdafce737..9d447e25e980d 100644 --- a/libraries/AP_HAL/Util.h +++ b/libraries/AP_HAL/Util.h @@ -149,7 +149,7 @@ class AP_HAL::Util { virtual void *malloc_type(size_t size, Memory_Type mem_type) { return calloc(1, size); } virtual void free_type(void *ptr, size_t size, Memory_Type mem_type) { return free(ptr); } -#ifdef ENABLE_HEAP +#if ENABLE_HEAP // heap functions, note that a heap once alloc'd cannot be dealloc'd virtual void *allocate_heap_memory(size_t size) = 0; virtual void *heap_realloc(void *heap, void *ptr, size_t old_size, size_t new_size) = 0; From 1cef95003e731691667d3ecc7f07bf7534cf7517 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 19 May 2024 10:55:58 +1000 Subject: [PATCH 25/73] AP_HAL_Linux: use if for ENABLE_HEAP allows for HAL with no heap functions --- libraries/AP_HAL_Linux/Util.cpp | 2 +- libraries/AP_HAL_Linux/Util.h | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AP_HAL_Linux/Util.cpp b/libraries/AP_HAL_Linux/Util.cpp index dc9d8083a17b1..1638476e090cc 100644 --- a/libraries/AP_HAL_Linux/Util.cpp +++ b/libraries/AP_HAL_Linux/Util.cpp @@ -242,7 +242,7 @@ int Util::get_hw_arm32() return -ENOENT; } -#ifdef ENABLE_HEAP +#if ENABLE_HEAP void *Util::allocate_heap_memory(size_t size) { struct heap *new_heap = (struct heap*)malloc(sizeof(struct heap)); diff --git a/libraries/AP_HAL_Linux/Util.h b/libraries/AP_HAL_Linux/Util.h index 70faada85b601..11a29dacb31c5 100644 --- a/libraries/AP_HAL_Linux/Util.h +++ b/libraries/AP_HAL_Linux/Util.h @@ -71,7 +71,7 @@ class Util : public AP_HAL::Util { bool get_system_id(char buf[50]) override; bool get_system_id_unformatted(uint8_t buf[], uint8_t &len) override; -#ifdef ENABLE_HEAP +#if ENABLE_HEAP // heap functions, note that a heap once alloc'd cannot be dealloc'd virtual void *allocate_heap_memory(size_t size) override; virtual void *heap_realloc(void *h, void *ptr, size_t old_size, size_t new_size) override; @@ -116,7 +116,7 @@ class Util : public AP_HAL::Util { const char *custom_defaults = HAL_PARAM_DEFAULTS_PATH; static const char *_hw_names[UTIL_NUM_HARDWARES]; -#ifdef ENABLE_HEAP +#if ENABLE_HEAP struct heap_allocation_header { size_t allocation_size; // size of allocated block, not including this header }; From 012694ba24445a417ada177ac369e88ea9596927 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 19 May 2024 10:55:58 +1000 Subject: [PATCH 26/73] AP_HAL_SITL: use if for ENABLE_HEAP allows for HAL with no heap functions --- libraries/AP_HAL_SITL/Util.cpp | 2 +- libraries/AP_HAL_SITL/Util.h | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AP_HAL_SITL/Util.cpp b/libraries/AP_HAL_SITL/Util.cpp index f5c6bbeeaaef9..727456e5157a0 100644 --- a/libraries/AP_HAL_SITL/Util.cpp +++ b/libraries/AP_HAL_SITL/Util.cpp @@ -85,7 +85,7 @@ bool HALSITL::Util::get_system_id(char buf[50]) return get_system_id_unformatted((uint8_t *)buf, len); } -#ifdef ENABLE_HEAP +#if ENABLE_HEAP void *HALSITL::Util::allocate_heap_memory(size_t size) { struct heap *new_heap = (struct heap*)malloc(sizeof(struct heap)); diff --git a/libraries/AP_HAL_SITL/Util.h b/libraries/AP_HAL_SITL/Util.h index cbfbeda556670..ac73c6a365a23 100644 --- a/libraries/AP_HAL_SITL/Util.h +++ b/libraries/AP_HAL_SITL/Util.h @@ -43,7 +43,7 @@ class HALSITL::Util : public AP_HAL::Util { bool get_system_id_unformatted(uint8_t buf[], uint8_t &len) override; void dump_stack_trace(); -#ifdef ENABLE_HEAP +#if ENABLE_HEAP // heap functions, note that a heap once alloc'd cannot be dealloc'd void *allocate_heap_memory(size_t size) override; void *heap_realloc(void *heap, void *ptr, size_t old_size, size_t new_size) override; @@ -90,7 +90,7 @@ class HALSITL::Util : public AP_HAL::Util { static ToneAlarm_SF _toneAlarm; #endif -#ifdef ENABLE_HEAP +#if ENABLE_HEAP struct heap_allocation_header { size_t allocation_size; // size of allocated block, not including this header }; From 302e30cb07d2f91e149562cd79df1e7fc9608bd9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 19 May 2024 10:56:23 +1000 Subject: [PATCH 27/73] HAL_QURT: got to link stage --- libraries/AP_HAL_QURT/HAL_QURT_Class.cpp | 80 ++---- libraries/AP_HAL_QURT/Scheduler.cpp | 29 +-- libraries/AP_HAL_QURT/Scheduler.h | 19 +- libraries/AP_HAL_QURT/Semaphores.cpp | 72 +++++- libraries/AP_HAL_QURT/Semaphores.h | 6 +- libraries/AP_HAL_QURT/UARTDriver.cpp | 305 ++--------------------- libraries/AP_HAL_QURT/Util.cpp | 62 ++++- libraries/AP_HAL_QURT/Util.h | 49 ++-- libraries/AP_HAL_QURT/dsp_main.cpp | 5 +- 9 files changed, 207 insertions(+), 420 deletions(-) diff --git a/libraries/AP_HAL_QURT/HAL_QURT_Class.cpp b/libraries/AP_HAL_QURT/HAL_QURT_Class.cpp index ee34b1d33c018..7fedc94d9f724 100644 --- a/libraries/AP_HAL_QURT/HAL_QURT_Class.cpp +++ b/libraries/AP_HAL_QURT/HAL_QURT_Class.cpp @@ -30,18 +30,16 @@ using namespace QURT; -static UDPDriver uartADriver; -static UARTDriver uartBDriver("/dev/tty-4"); -static UARTDriver uartCDriver("/dev/tty-2"); -static UARTDriver uartDDriver(nullptr); -static UARTDriver uartEDriver(nullptr); +static Empty::UARTDriver serial0Driver; +static UARTDriver serial1Driver("/dev/tty-4"); +static UARTDriver serial2Driver("/dev/tty-2"); 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 Empty::RCInput rcinDriver; +static RCOutput rcoutDriver; static Util utilInstance; static Scheduler schedulerInstance; static Empty::I2CDeviceManager i2c_mgr_instance; @@ -50,22 +48,29 @@ bool qurt_ran_overtime; HAL_QURT::HAL_QURT() : AP_HAL::HAL( - &uartADriver, - &uartBDriver, - &uartCDriver, - &uartDDriver, - &uartEDriver, - nullptr, // uartF + &serial0Driver, + &serial1Driver, + &serial2Driver, + nullptr, + nullptr, + nullptr, + nullptr, + nullptr, + nullptr, + nullptr, &i2c_mgr_instance, &spiDeviceManager, + nullptr, &analogIn, &storageDriver, - &uartADriver, + &serial0Driver, &gpioDriver, &rcinDriver, &rcoutDriver, &schedulerInstance, &utilInstance, + nullptr, + nullptr, nullptr) { } @@ -74,58 +79,15 @@ void HAL_QURT::run(int argc, char* const argv[], Callbacks* callbacks) const { assert(callbacks); - int opt; - const struct GetOptLong::option options[] = { - {"uartB", true, 0, 'B'}, - {"uartC", true, 0, 'C'}, - {"uartD", true, 0, 'D'}, - {"uartE", true, 0, 'E'}, - {"dsm", true, 0, 'S'}, - {"ESC", true, 0, 'e'}, - {0, false, 0, 0} - }; - - GetOptLong gopt(argc, argv, "B:C:D:E:e:S", - options); - - /* - parse command line options - */ - while ((opt = gopt.getoption()) != -1) { - switch (opt) { - case 'B': - uartBDriver.set_device_path(gopt.optarg); - break; - case 'C': - uartCDriver.set_device_path(gopt.optarg); - break; - case 'D': - uartDDriver.set_device_path(gopt.optarg); - break; - case 'E': - uartEDriver.set_device_path(gopt.optarg); - break; - case 'e': - rcoutDriver.set_device_path(gopt.optarg); - break; - case 'S': - rcinDriver.set_device_path(gopt.optarg); - break; - default: - printf("Unknown option '%c'\n", (char)opt); - exit(1); - } - } - /* 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); + serial0Driver.begin(115200); rcinDriver.init(); callbacks->setup(); - scheduler->system_initialized(); + scheduler->set_system_initialized(); for (;;) { callbacks->loop(); diff --git a/libraries/AP_HAL_QURT/Scheduler.cpp b/libraries/AP_HAL_QURT/Scheduler.cpp index 214fcb1993e8c..57b2fe198516d 100644 --- a/libraries/AP_HAL_QURT/Scheduler.cpp +++ b/libraries/AP_HAL_QURT/Scheduler.cpp @@ -11,8 +11,7 @@ #include #include #include -#include -#include +#include #include "UARTDriver.h" #include "Storage.h" @@ -64,8 +63,7 @@ void Scheduler::init() void Scheduler::delay_microseconds(uint16_t usec) { - //pthread_yield(); - usleep(usec); + qurt_sleep(usec); } void Scheduler::delay(uint16_t ms) @@ -139,7 +137,7 @@ void Scheduler::resume_timer_procs() void Scheduler::reboot(bool hold_in_bootloader) { HAP_PRINTF("**** REBOOT REQUESTED ****"); - usleep(2000000); + qurt_sleep(2000000); exit(1); } @@ -174,7 +172,6 @@ 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); @@ -184,15 +181,6 @@ void *Scheduler::_timer_thread(void *arg) // 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 nullptr; } @@ -228,11 +216,12 @@ void *Scheduler::_uart_thread(void *arg) // process any pending serial bytes //((UARTDriver *)hal.uartA)->timer_tick(); - hal.uartB->timer_tick(); - hal.uartC->timer_tick(); - hal.uartD->timer_tick(); - hal.uartE->timer_tick(); - hal.uartF->timer_tick(); + for (uint8_t i = 0; i < hal.num_serial; i++) { + auto *p = hal.serial(i); + if (p != nullptr) { + p->_timer_tick(); + } + } } return nullptr; } diff --git a/libraries/AP_HAL_QURT/Scheduler.h b/libraries/AP_HAL_QURT/Scheduler.h index ce1bdb24d2723..6171693d64988 100644 --- a/libraries/AP_HAL_QURT/Scheduler.h +++ b/libraries/AP_HAL_QURT/Scheduler.h @@ -20,20 +20,23 @@ class QURT::Scheduler : public AP_HAL::Scheduler { Scheduler(); /* AP_HAL::Scheduler methods */ - void init(); - void delay(uint16_t ms); - void delay_microseconds(uint16_t us); - 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 init() override; + void delay(uint16_t ms) override; + void delay_microseconds(uint16_t us) override; + void register_timer_process(AP_HAL::MemberProc) override; + void register_io_process(AP_HAL::MemberProc) override; + void register_timer_failsafe(AP_HAL::Proc, uint32_t period_us) override; void suspend_timer_procs(); void resume_timer_procs(); - void reboot(bool hold_in_bootloader); + void reboot(bool hold_in_bootloader) override; bool in_main_thread() const override; void system_initialized(); void hal_initialized(); - + + void set_system_initialized() override; + bool is_system_initialized() override; + private: bool _initialized; volatile bool _hal_initialized; diff --git a/libraries/AP_HAL_QURT/Semaphores.cpp b/libraries/AP_HAL_QURT/Semaphores.cpp index 6d5dc6a62ea79..b9d454b7cf542 100644 --- a/libraries/AP_HAL_QURT/Semaphores.cpp +++ b/libraries/AP_HAL_QURT/Semaphores.cpp @@ -1,19 +1,27 @@ #include -#if CONFIG_HAL_BOARD == HAL_BOARD_QURT - #include "Semaphores.h" extern const AP_HAL::HAL& hal; using namespace QURT; -bool Semaphore::give() +// construct a semaphore +Semaphore::Semaphore() +{ + pthread_mutexattr_t attr; + pthread_mutexattr_init(&attr); + pthread_mutexattr_setprotocol(&attr, PTHREAD_PRIO_INHERIT); + pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_RECURSIVE); + pthread_mutex_init(&_lock, &attr); +} + +bool Semaphore::give() { return pthread_mutex_unlock(&_lock) == 0; } -bool Semaphore::take(uint32_t timeout_ms) +bool Semaphore::take(uint32_t timeout_ms) { if (timeout_ms == HAL_SEMAPHORE_BLOCK_FOREVER) { return pthread_mutex_lock(&_lock) == 0; @@ -31,9 +39,61 @@ bool Semaphore::take(uint32_t timeout_ms) return false; } -bool Semaphore::take_nonblocking() +bool Semaphore::take_nonblocking() { return pthread_mutex_trylock(&_lock) == 0; } -#endif // CONFIG_HAL_BOARD +/* + binary semaphore using pthread condition variables + */ + +BinarySemaphore::BinarySemaphore(bool initial_state) : + AP_HAL::BinarySemaphore(initial_state) +{ + pthread_cond_init(&cond, NULL); + pending = initial_state; +} + +bool BinarySemaphore::wait(uint32_t timeout_us) +{ + WITH_SEMAPHORE(mtx); + if (!pending) { + struct timespec ts; + if (clock_gettime(CLOCK_REALTIME, &ts) != 0) { + return false; + } + ts.tv_sec += timeout_us/1000000UL; + ts.tv_nsec += (timeout_us % 1000000U) * 1000UL; + if (ts.tv_nsec >= 1000000000L) { + ts.tv_sec++; + ts.tv_nsec -= 1000000000L; + } + if (pthread_cond_timedwait(&cond, &mtx._lock, &ts) != 0) { + return false; + } + } + pending = false; + return true; +} + +bool BinarySemaphore::wait_blocking(void) +{ + WITH_SEMAPHORE(mtx); + if (!pending) { + if (pthread_cond_wait(&cond, &mtx._lock) != 0) { + return false; + } + } + pending = false; + return true; +} + +void BinarySemaphore::signal(void) +{ + WITH_SEMAPHORE(mtx); + if (!pending) { + pending = true; + pthread_cond_signal(&cond); + } +} diff --git a/libraries/AP_HAL_QURT/Semaphores.h b/libraries/AP_HAL_QURT/Semaphores.h index ca7b9ee3d226e..3e29abb723d38 100644 --- a/libraries/AP_HAL_QURT/Semaphores.h +++ b/libraries/AP_HAL_QURT/Semaphores.h @@ -1,7 +1,9 @@ #pragma once #include +#include #include +#include namespace QURT { @@ -12,9 +14,8 @@ class Semaphore : public AP_HAL::Semaphore { bool give() override; bool take(uint32_t timeout_ms) override; bool take_nonblocking() override; - protected: - int dummy_lock; + pthread_mutex_t _lock; }; @@ -28,6 +29,7 @@ class BinarySemaphore : public AP_HAL::BinarySemaphore { protected: Semaphore mtx; + pthread_cond_t cond; bool pending; }; diff --git a/libraries/AP_HAL_QURT/UARTDriver.cpp b/libraries/AP_HAL_QURT/UARTDriver.cpp index f61655cf17181..cb364d09f1c5b 100644 --- a/libraries/AP_HAL_QURT/UARTDriver.cpp +++ b/libraries/AP_HAL_QURT/UARTDriver.cpp @@ -1,300 +1,25 @@ -/* - 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 (devname == nullptr) { - return; - } - 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 -void UARTDriver::end() -{ - initialised = false; - if (fd != -1) { - ::close(fd); - fd = -1; - } - if (readbuf) { - delete readbuf; - readbuf = nullptr; - } - if (writebuf) { - delete writebuf; - writebuf = nullptr; - } - -} +QURT::UARTDriver::UARTDriver(const char *name) {} -void UARTDriver::flush() -{ -} - -bool UARTDriver::is_initialized() -{ - return fd != -1 && initialised; -} +/* QURT implementations of virtual methods */ +void QURT::UARTDriver::_begin(uint32_t b, uint16_t rxS, uint16_t txS) {} +void QURT::UARTDriver::_end() {} +void QURT::UARTDriver::_flush() {} +bool QURT::UARTDriver::is_initialized() { return false; } +bool QURT::UARTDriver::tx_pending() { return false; } -void UARTDriver::set_blocking_writes(bool blocking) +uint32_t QURT::UARTDriver::_available() { return 0; } +uint32_t QURT::UARTDriver::txspace() { return 1; } +bool QURT::UARTDriver::_discard_input() { return false; } +size_t QURT::UARTDriver::_write(const uint8_t *buffer, size_t size) { - nonblocking_writes = !blocking; + return size; } - -bool UARTDriver::tx_pending() +ssize_t QURT::UARTDriver::_read(uint8_t *buffer, uint16_t size) { - 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(HAL_SEMAPHORE_BLOCK_FOREVER)) { - 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(HAL_SEMAPHORE_BLOCK_FOREVER)) { - 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(HAL_SEMAPHORE_BLOCK_FOREVER)) { - 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; + return 0; } -#endif diff --git a/libraries/AP_HAL_QURT/Util.cpp b/libraries/AP_HAL_QURT/Util.cpp index 08e635c73e87e..391a8cb3cf1db 100644 --- a/libraries/AP_HAL_QURT/Util.cpp +++ b/libraries/AP_HAL_QURT/Util.cpp @@ -9,19 +9,63 @@ extern const AP_HAL::HAL& hal; 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) +#if ENABLE_HEAP +void *Util::allocate_heap_memory(size_t size) { - return 256*1024; + struct heap *new_heap = (struct heap*)malloc(sizeof(struct heap)); + if (new_heap != nullptr) { + new_heap->max_heap_size = size; + new_heap->current_heap_usage = 0; + } + return (void *)new_heap; } -// create a new semaphore -Semaphore *Util::new_semaphore(void) +void *Util::heap_realloc(void *h, void *ptr, size_t old_size, size_t new_size) { - return new Semaphore; + if (h == nullptr) { + return nullptr; + } + + struct heap *heapp = (struct heap*)h; + + // extract appropriate headers. We use the old_size from the + // header not from the caller. We use SITL to catch cases they + // don't match (which would be a lua bug) + old_size = 0; + heap_allocation_header *old_header = nullptr; + if (ptr != nullptr) { + old_header = ((heap_allocation_header *)ptr) - 1; + old_size = old_header->allocation_size; + } + + if ((heapp->current_heap_usage + new_size - old_size) > heapp->max_heap_size) { + // fail the allocation as we don't have the memory. Note that we don't simulate fragmentation + return nullptr; + } + + heapp->current_heap_usage -= old_size; + if (new_size == 0) { + free(old_header); + return nullptr; + } + + heap_allocation_header *new_header = (heap_allocation_header *)malloc(new_size + sizeof(heap_allocation_header)); + if (new_header == nullptr) { + // total failure to allocate, this is very surprising in SITL + return nullptr; + } + heapp->current_heap_usage += new_size; + new_header->allocation_size = new_size; + void *new_mem = new_header + 1; + + if (ptr == nullptr) { + return new_mem; + } + memcpy(new_mem, ptr, old_size > new_size ? new_size : old_size); + free(old_header); + return new_mem; } +#endif // ENABLE_HEAP + #endif // CONFIG_HAL_BOARD == HAL_BOARD_QURT diff --git a/libraries/AP_HAL_QURT/Util.h b/libraries/AP_HAL_QURT/Util.h index bdf2d054f1298..9f42cf7bf7e3d 100644 --- a/libraries/AP_HAL_QURT/Util.h +++ b/libraries/AP_HAL_QURT/Util.h @@ -1,18 +1,3 @@ -/* - 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 @@ -20,12 +5,30 @@ class QURT::Util : public AP_HAL::Util { public: - Util(void) {} - 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; + /* + set HW RTC in UTC microseconds + */ + void set_hw_rtc(uint64_t time_utc_usec) override {} + + /* + get system clock in UTC microseconds + */ + uint64_t get_hw_rtc() const override { return 0; } + +#if ENABLE_HEAP + // heap functions, note that a heap once alloc'd cannot be dealloc'd + virtual void *allocate_heap_memory(size_t size) override; + virtual void *heap_realloc(void *h, void *ptr, size_t old_size, size_t new_size) override; +#endif // ENABLE_HEAP + +#if ENABLE_HEAP + struct heap_allocation_header { + size_t allocation_size; // size of allocated block, not including this header + }; + + struct heap { + size_t max_heap_size; + size_t current_heap_usage; + }; +#endif // ENABLE_HEAP }; - diff --git a/libraries/AP_HAL_QURT/dsp_main.cpp b/libraries/AP_HAL_QURT/dsp_main.cpp index cc0125cadf66a..cfffd08b4104c 100644 --- a/libraries/AP_HAL_QURT/dsp_main.cpp +++ b/libraries/AP_HAL_QURT/dsp_main.cpp @@ -5,7 +5,6 @@ #include "Scheduler.h" #include "Storage.h" #include "replace.h" -#include extern const AP_HAL::HAL& hal; @@ -97,10 +96,10 @@ uint32_t ardupilot_get_storage(uint8_t *buf, int len) uint32_t ardupilot_socket_check(uint8_t *buf, int len, uint32_t *nbytes) { - return QURT::UDPDriver::from(hal.uartA)->socket_check(buf, len, nbytes); + return 0; } 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); + return 0; } From 91ef39bdf0fd35875307570fd3778f9f69b17004 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 19 May 2024 12:09:12 +1000 Subject: [PATCH 28/73] Tools: more QURT hackery --- Tools/ardupilotwaf/boards.py | 44 ++++++++++++++++++++++++++++++++++-- 1 file changed, 42 insertions(+), 2 deletions(-) diff --git a/Tools/ardupilotwaf/boards.py b/Tools/ardupilotwaf/boards.py index 1392bdf07cbbe..b10d1905d0e24 100644 --- a/Tools/ardupilotwaf/boards.py +++ b/Tools/ardupilotwaf/boards.py @@ -1638,17 +1638,30 @@ def configure_toolchain(self, cfg): cfg.env.TOOLCHAIN_DIR = cfg.env.HEXAGON_SDK_DIR + "/tools/HEXAGON_Tools/8.4.05/Tools" cfg.env.COMPILER_CC = cfg.env.TOOLCHAIN_DIR + "/bin/hexagon-clang" cfg.env.COMPILER_CXX = cfg.env.TOOLCHAIN_DIR + "/bin/hexagon-clang++" + cfg.env.LINK_CXX = cfg.env.HEXAGON_SDK_DIR + "/tools/HEXAGON_Tools/8.4.05/Tools/bin/hexagon-link" cfg.env.CXX = ["ccache", cfg.env.COMPILER_CXX] cfg.env.CXX_TGT_F = ['-c', '-o'] + cfg.env.CCLNK_SRC_F = [] + cfg.env.CXXLNK_SRC_F = [] + cfg.env.CXXLNK_TGT_F = ['-o'] + cfg.env.CCLNK_TGT_F = ['-o'] cfg.env.CPPPATH_ST = '-I%s' cfg.env.DEFINES_ST = '-D%s' - + cfg.env.AR = cfg.env.HEXAGON_SDK_DIR + "/tools/HEXAGON_Tools/8.4.05/Tools/bin/hexagon-ar" + cfg.env.ARFLAGS = 'rcs' + cfg.env.cxxstlib_PATTERN = 'lib%s.a' + cfg.env.cstlib_PATTERN = 'lib%s.a' + cfg.env.LIBPATH_ST = '-L%s' + cfg.env.LIB_ST = '-l%s' + cfg.env.SHLIB_MARKER = '' + cfg.env.STLIBPATH_ST = '-L%s' + cfg.env.STLIB_MARKER = '' + cfg.env.STLIB_ST = '-l%s' def configure_env(self, cfg, env): super(QURT, self).configure_env(cfg, env) env.BOARD_CLASS = "QURT" - env.HEXAGON_LINK = cfg.env.HEXAGON_SDK_DIR + "/tools/HEXAGON_Tools/8.4.05/Tools/bin/hexagon-link" env.HEAXGON_APP = "libardupilot.so" env.HEXAGON_LINKFLAGS = f"-march=hexagon -mcpu=hexagonv66 -shared -call_shared -G0 {cfg.env.TOOLCHAIN_DIR}/target/hexagon/lib/v66/G0/pic/initS.o -L{cfg.env.TOOLCHAIN_DIR}/target/hexagon/lib/v66/G0/pic -Bsymbolic {cfg.env.TOOLCHAIN_DIR}/target/hexagon/lib/v66/G0/pic/libgcc.a --wrap=malloc --wrap=calloc --wrap=free --wrap=realloc --wrap=memalign --wrap=__stack_chk_fail -lc -soname={env.HEXAGON_APP} --start-group --whole-archive OBJECT_LIST --end-group --start-group -lgcc --end-group {cfg.env.TOOLCHAIN_DIR}/target/hexagon/lib/v66/G0/pic/finiS.o" env.INCLUDES += [cfg.env.HEXAGON_SDK_DIR + "/rtos/qurt/computev66/include/qurt"] @@ -1661,6 +1674,33 @@ def configure_env(self, cfg, env): AP_SIM_ENABLED = 0, ) + env.LINKFLAGS = [ + "-march=hexagon", + "-mcpu=hexagonv66", + "-shared", + "-call_shared", + "-G0", + cfg.env.TOOLCHAIN_DIR + "/target/hexagon/lib/v66/G0/pic/initS.o", + f"-L{cfg.env.TOOLCHAIN_DIR}/target/hexagon/lib/v66/G0/pic", + "-Bsymbolic", + cfg.env.TOOLCHAIN_DIR + "/target/hexagon/lib/v66/G0/pic/libgcc.a", + "--wrap=malloc", + "--wrap=calloc", + "--wrap=free", + "--wrap=realloc", + "--wrap=memalign", + "--wrap=__stack_chk_fail", + "-lc", + f"-soname={cfg.env.HEXAGON_APP}", + "--start-group", + "--whole-archive", + "--end-group", + "--start-group", + "-lgcc", + "--end-group", + cfg.env.TOOLCHAIN_DIR + "/target/hexagon/lib/v66/G0/pic/finiS.o" + ] + if not cfg.env.DEBUG: env.CXXFLAGS += [ '-O2', From 53faa501874dd80b59593d01f4ef64c9e68b527b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 19 May 2024 12:09:47 +1000 Subject: [PATCH 29/73] AP_Scripting: don't build luac.c --- libraries/AP_Scripting/lua/src/luac.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/libraries/AP_Scripting/lua/src/luac.c b/libraries/AP_Scripting/lua/src/luac.c index 549ad3950047d..03b101d40fa02 100644 --- a/libraries/AP_Scripting/lua/src/luac.c +++ b/libraries/AP_Scripting/lua/src/luac.c @@ -1,3 +1,4 @@ +#if 0 /* ** $Id: luac.c,v 1.76 2018/06/19 01:32:02 lhf Exp $ ** Lua compiler (saves bytecodes to files; also lists bytecodes) @@ -448,3 +449,5 @@ static void PrintFunction(const Proto* f, int full) if (full) PrintDebug(f); for (i=0; ip[i],full); } + +#endif From 60b1bfb17f8300b36ff85de86b38dc50c3613f85 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 19 May 2024 16:38:37 +1000 Subject: [PATCH 30/73] Tools: break out CAN libraries --- Tools/ardupilotwaf/ardupilotwaf.py | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/Tools/ardupilotwaf/ardupilotwaf.py b/Tools/ardupilotwaf/ardupilotwaf.py index d20eef7e50c60..cc81bc33a14c3 100644 --- a/Tools/ardupilotwaf/ardupilotwaf.py +++ b/Tools/ardupilotwaf/ardupilotwaf.py @@ -16,6 +16,13 @@ '*.cpp', ] +COMMON_VEHICLE_DEPENDENT_CAN_LIBRARIES = [ + 'AP_CANManager', + 'AP_KDECAN', + 'AP_PiccoloCAN', + 'AP_PiccoloCAN/piccolo_protocol', +] + COMMON_VEHICLE_DEPENDENT_LIBRARIES = [ 'AP_Airspeed', 'AP_AccelCal', @@ -26,7 +33,6 @@ 'AP_BattMonitor', 'AP_BoardConfig', 'AP_Camera', - 'AP_CANManager', 'AP_Common', 'AP_Compass', 'AP_Declination', @@ -34,7 +40,6 @@ 'AP_HAL', 'AP_HAL_Empty', 'AP_InertialSensor', - 'AP_KDECAN', 'AP_Math', 'AP_Mission', 'AP_DAL', @@ -73,8 +78,6 @@ 'AP_SBusOut', 'AP_IOMCU', 'AP_Parachute', - 'AP_PiccoloCAN', - 'AP_PiccoloCAN/piccolo_protocol', 'AP_RAMTRON', 'AP_RCProtocol', 'AP_Radio', @@ -248,11 +251,8 @@ def ap_get_all_libraries(bld): def ap_common_vehicle_libraries(bld): libraries = COMMON_VEHICLE_DEPENDENT_LIBRARIES - if bld.env.DEST_BINFMT == 'pe': - libraries += [ - 'AC_Fence', - 'AC_AttitudeControl', - ] + if bld.env.with_can: + libraries.extend(COMMON_VEHICLE_DEPENDENT_CAN_LIBRARIES) return libraries From 2a5bcac69d85830b62192725b424629dfe33c121 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 19 May 2024 16:40:10 +1000 Subject: [PATCH 31/73] Tools: QURT progress --- Tools/ardupilotwaf/boards.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/Tools/ardupilotwaf/boards.py b/Tools/ardupilotwaf/boards.py index b10d1905d0e24..2c102a89f157d 100644 --- a/Tools/ardupilotwaf/boards.py +++ b/Tools/ardupilotwaf/boards.py @@ -204,6 +204,8 @@ def configure_env(self, cfg, env): # make easy to override them. Convert back to list before consumption. env.DEFINES = {} + env.with_can = self.with_can + # potentially set extra defines from an environment variable: if cfg.options.define is not None: for (n, v) in [d.split("=") for d in cfg.options.define]: @@ -1640,7 +1642,9 @@ def configure_toolchain(self, cfg): cfg.env.COMPILER_CXX = cfg.env.TOOLCHAIN_DIR + "/bin/hexagon-clang++" cfg.env.LINK_CXX = cfg.env.HEXAGON_SDK_DIR + "/tools/HEXAGON_Tools/8.4.05/Tools/bin/hexagon-link" cfg.env.CXX = ["ccache", cfg.env.COMPILER_CXX] + cfg.env.CC = ["ccache", cfg.env.COMPILER_CC] cfg.env.CXX_TGT_F = ['-c', '-o'] + cfg.env.CC_TGT_F = ['-c', '-o'] cfg.env.CCLNK_SRC_F = [] cfg.env.CXXLNK_SRC_F = [] cfg.env.CXXLNK_TGT_F = ['-o'] @@ -1663,10 +1667,11 @@ def configure_env(self, cfg, env): env.BOARD_CLASS = "QURT" env.HEAXGON_APP = "libardupilot.so" - env.HEXAGON_LINKFLAGS = f"-march=hexagon -mcpu=hexagonv66 -shared -call_shared -G0 {cfg.env.TOOLCHAIN_DIR}/target/hexagon/lib/v66/G0/pic/initS.o -L{cfg.env.TOOLCHAIN_DIR}/target/hexagon/lib/v66/G0/pic -Bsymbolic {cfg.env.TOOLCHAIN_DIR}/target/hexagon/lib/v66/G0/pic/libgcc.a --wrap=malloc --wrap=calloc --wrap=free --wrap=realloc --wrap=memalign --wrap=__stack_chk_fail -lc -soname={env.HEXAGON_APP} --start-group --whole-archive OBJECT_LIST --end-group --start-group -lgcc --end-group {cfg.env.TOOLCHAIN_DIR}/target/hexagon/lib/v66/G0/pic/finiS.o" + env.HEXAGON_LINKFLAGS = f"-march=hexagon -mcpu=hexagonv66 -fpic -shared -call_shared -mG0lib -G0 {cfg.env.TOOLCHAIN_DIR}/target/hexagon/lib/v66/G0/pic/initS.o -L{cfg.env.TOOLCHAIN_DIR}/target/hexagon/lib/v66/G0/pic -Bsymbolic {cfg.env.TOOLCHAIN_DIR}/target/hexagon/lib/v66/G0/pic/libgcc.a --wrap=malloc --wrap=calloc --wrap=free --wrap=realloc --wrap=memalign --wrap=__stack_chk_fail -lc -soname={env.HEXAGON_APP} --start-group --whole-archive OBJECT_LIST --end-group --start-group -lgcc --end-group {cfg.env.TOOLCHAIN_DIR}/target/hexagon/lib/v66/G0/pic/finiS.o" env.INCLUDES += [cfg.env.HEXAGON_SDK_DIR + "/rtos/qurt/computev66/include/qurt"] env.INCLUDES += [cfg.env.HEXAGON_SDK_DIR + "/rtos/qurt/computev66/include/posix"] env.CXXFLAGS += ["-fPIC", "-MD"] + env.CFLAGS += ["-fPIC", "-MD"] env.DEFINES.update( CONFIG_HAL_BOARD = 'HAL_BOARD_QURT', From 2bce578eede8ca0bac8c1b1e921bb32e2ae43b06 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 19 May 2024 16:40:29 +1000 Subject: [PATCH 32/73] AP_Baro: don't build sim specific fns without sim --- libraries/AP_Baro/AP_Baro_atmosphere.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/libraries/AP_Baro/AP_Baro_atmosphere.cpp b/libraries/AP_Baro/AP_Baro_atmosphere.cpp index 63667432e8f45..da81e1033d280 100644 --- a/libraries/AP_Baro/AP_Baro_atmosphere.cpp +++ b/libraries/AP_Baro/AP_Baro_atmosphere.cpp @@ -314,6 +314,7 @@ float AP_Baro::_get_EAS2TAS(void) const #endif } +#if AP_SIM_ENABLED // lookup expected temperature in degrees C for a given altitude. Used for SITL backend float AP_Baro::get_temperatureC_for_alt_amsl(const float alt_amsl) { @@ -329,6 +330,7 @@ float AP_Baro::get_pressure_for_alt_amsl(const float alt_amsl) get_pressure_temperature_for_alt_amsl(alt_amsl, pressure, temp_K); return pressure; } +#endif // AP_SIM_ENABLED /* return sea level pressure given a current altitude and pressure reading From 76382b6cead5c2e1f39502d9af3b482500290e93 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 19 May 2024 16:40:59 +1000 Subject: [PATCH 33/73] AP_Filesystem: support QURT posix --- libraries/AP_Filesystem/AP_Filesystem.cpp | 2 ++ libraries/AP_Filesystem/AP_Filesystem_config.h | 2 +- libraries/AP_Filesystem/AP_Filesystem_posix.cpp | 9 ++++++++- libraries/AP_Filesystem/AP_Filesystem_posix.h | 4 ++++ 4 files changed, 15 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Filesystem/AP_Filesystem.cpp b/libraries/AP_Filesystem/AP_Filesystem.cpp index 4a97061d7b916..2fabb7b3a6cb3 100644 --- a/libraries/AP_Filesystem/AP_Filesystem.cpp +++ b/libraries/AP_Filesystem/AP_Filesystem.cpp @@ -205,7 +205,9 @@ AP_Filesystem::DirHandle *AP_Filesystem::opendir(const char *pathname) (strlen(pathname) == 1 && pathname[0] == '/')) { virtual_dirent.backend_ofs = 0; virtual_dirent.d_off = 0; +#if AP_FILESYSTEM_HAVE_DIRENT_DTYPE virtual_dirent.de.d_type = DT_DIR; +#endif } else { virtual_dirent.backend_ofs = 255; } diff --git a/libraries/AP_Filesystem/AP_Filesystem_config.h b/libraries/AP_Filesystem/AP_Filesystem_config.h index a184051610cc4..f5a97fb4fad56 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_config.h +++ b/libraries/AP_Filesystem/AP_Filesystem_config.h @@ -23,7 +23,7 @@ #endif #ifndef AP_FILESYSTEM_POSIX_ENABLED -#define AP_FILESYSTEM_POSIX_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX) +#define AP_FILESYSTEM_POSIX_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX || CONFIG_HAL_BOARD == HAL_BOARD_QURT) #endif #ifndef AP_FILESYSTEM_ROMFS_ENABLED diff --git a/libraries/AP_Filesystem/AP_Filesystem_posix.cpp b/libraries/AP_Filesystem/AP_Filesystem_posix.cpp index 5a6169053472d..81e87fa3661e0 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_posix.cpp +++ b/libraries/AP_Filesystem/AP_Filesystem_posix.cpp @@ -26,10 +26,13 @@ #if defined(__APPLE__) #include -#else +#elif CONFIG_HAL_BOARD != HAL_BOARD_QURT #include #endif + +#if AP_FILESYSTEM_POSIX_HAVE_UTIME #include +#endif extern const AP_HAL::HAL& hal; @@ -182,6 +185,7 @@ int64_t AP_Filesystem_Posix::disk_space(const char *path) */ bool AP_Filesystem_Posix::set_mtime(const char *filename, const uint32_t mtime_sec) { +#if AP_FILESYSTEM_POSIX_HAVE_UTIME FS_CHECK_ALLOWED(false); filename = map_filename(filename); struct utimbuf times {}; @@ -189,6 +193,9 @@ bool AP_Filesystem_Posix::set_mtime(const char *filename, const uint32_t mtime_s times.modtime = mtime_sec; return utime(filename, ×) == 0; +#else + return false; +#endif } #endif // AP_FILESYSTEM_POSIX_ENABLED diff --git a/libraries/AP_Filesystem/AP_Filesystem_posix.h b/libraries/AP_Filesystem/AP_Filesystem_posix.h index e44764213aee4..69103ab297e0b 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_posix.h +++ b/libraries/AP_Filesystem/AP_Filesystem_posix.h @@ -29,6 +29,10 @@ #include #include "AP_Filesystem_backend.h" +#ifndef AP_FILESYSTEM_POSIX_HAVE_UTIME +#define AP_FILESYSTEM_POSIX_HAVE_UTIME 1 +#endif + class AP_Filesystem_Posix : public AP_Filesystem_Backend { public: From 84d10db6ec3a4ec8e9db1f16a721be1671bae71a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 19 May 2024 16:41:14 +1000 Subject: [PATCH 34/73] AP_HAL: qurt fixups --- libraries/AP_HAL/board/qurt.h | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/libraries/AP_HAL/board/qurt.h b/libraries/AP_HAL/board/qurt.h index 732c4145722f1..02c04a52d627d 100644 --- a/libraries/AP_HAL/board/qurt.h +++ b/libraries/AP_HAL/board/qurt.h @@ -34,17 +34,21 @@ #define HAL_HAVE_SAFETY_SWITCH 0 #define HAL_WITH_MCU_MONITORING 0 #define HAL_USE_QUADSPI 0 -#define HAL_LOGGING_ENABLED 0 #define HAL_WITH_DSP 0 #define HAL_CANFD_SUPPORTED 0 #define HAL_NUM_CAN_IFACES 0 #define AP_CRASHDUMP_ENABLED 0 #define HAL_ENABLE_DFU_BOOT 0 -#define HAL_PARAM_DEFAULTS_PATH nullptr #define AP_FILESYSTEM_HAVE_DIRENT_DTYPE 0 -#define AP_FILESYSTEM_PARAM_ENABLED 0 -#define AP_FILESYSTEM_FILE_READING_ENABLED 0 +#define HAL_PICCOLO_CAN_ENABLE 0 #define LUA_USE_LONGJMP 1 +#define AP_MAVLINK_FTP_ENABLED 0 +#define AP_FILESYSTEM_POSIX_HAVE_UTIME 0 + +#define HAL_PARAM_DEFAULTS_PATH nullptr +#define HAL_BOARD_STORAGE_DIRECTORY "APM" +#define HAL_BOARD_LOG_DIRECTORY "APM/logs" +#define HAL_BOARD_TERRAIN_DIRECTORY "APM/terrain" #define USE_LIBC_REALLOC 1 From 6cc9e245bbfa8a31808a802ed110b3f1a3f2e745 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 19 May 2024 16:41:28 +1000 Subject: [PATCH 35/73] HAL_QURT: fixups --- libraries/AP_HAL_QURT/Scheduler.cpp | 2 +- libraries/AP_HAL_QURT/Scheduler.h | 3 +-- libraries/AP_HAL_QURT/replace.h | 7 +++++++ 3 files changed, 9 insertions(+), 3 deletions(-) diff --git a/libraries/AP_HAL_QURT/Scheduler.cpp b/libraries/AP_HAL_QURT/Scheduler.cpp index 57b2fe198516d..87d7a794e189a 100644 --- a/libraries/AP_HAL_QURT/Scheduler.cpp +++ b/libraries/AP_HAL_QURT/Scheduler.cpp @@ -247,7 +247,7 @@ bool Scheduler::in_main_thread() const return getpid() == _main_task_pid; } -void Scheduler::system_initialized() { +void Scheduler::set_system_initialized() { if (_initialized) { AP_HAL::panic("PANIC: scheduler::system_initialized called" "more than once"); diff --git a/libraries/AP_HAL_QURT/Scheduler.h b/libraries/AP_HAL_QURT/Scheduler.h index 6171693d64988..d2f5e0dcb9def 100644 --- a/libraries/AP_HAL_QURT/Scheduler.h +++ b/libraries/AP_HAL_QURT/Scheduler.h @@ -31,11 +31,10 @@ class QURT::Scheduler : public AP_HAL::Scheduler { void reboot(bool hold_in_bootloader) override; bool in_main_thread() const override; - void system_initialized(); void hal_initialized(); void set_system_initialized() override; - bool is_system_initialized() override; + bool is_system_initialized() override { return _initialized; } private: bool _initialized; diff --git a/libraries/AP_HAL_QURT/replace.h b/libraries/AP_HAL_QURT/replace.h index cc1513ea34009..6d6f2d6ccb7f1 100644 --- a/libraries/AP_HAL_QURT/replace.h +++ b/libraries/AP_HAL_QURT/replace.h @@ -9,12 +9,16 @@ #include #include + +#ifdef __cplusplus extern "C" { +#endif /* work around broken headers */ size_t strnlen(const char *s, size_t maxlen); + char *strndup(const char *s, size_t n); int asprintf(char **, const char *, ...); off_t lseek(int, off_t, int); DIR *opendir (const char *); @@ -26,7 +30,10 @@ extern "C" { pid_t getpid (void); void HAP_printf(const char *file, int line, const char *fmt, ...); + +#ifdef __cplusplus } +#endif #define HAP_PRINTF(...) HAP_printf(__FILE__, __LINE__, __VA_ARGS__) From bdfe1f0e68bf216dda758e83fbeb4ca3576c5dca Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 19 May 2024 16:41:48 +1000 Subject: [PATCH 36/73] GCS_MAVLink: prevent all_stream_entries duplication --- libraries/GCS_MAVLink/GCS_Dummy.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/libraries/GCS_MAVLink/GCS_Dummy.cpp b/libraries/GCS_MAVLink/GCS_Dummy.cpp index f55212d8564e9..e10d3991a9a82 100644 --- a/libraries/GCS_MAVLink/GCS_Dummy.cpp +++ b/libraries/GCS_MAVLink/GCS_Dummy.cpp @@ -5,6 +5,9 @@ #include "GCS_Dummy.h" #include +#include + +#if APM_BUILD_TYPE(APM_BUILD_UNKNOWN) #define FORCE_VERSION_H_INCLUDE #include @@ -12,6 +15,7 @@ #undef FORCE_VERSION_H_INCLUDE const struct GCS_MAVLINK::stream_entries GCS_MAVLINK::all_stream_entries[] {}; +#endif /* send_text implementation for dummy GCS From 7d9058cf50da54da041d4c0489595bac4ec7e073 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 19 May 2024 19:30:25 +1000 Subject: [PATCH 37/73] AP_HAL: qurt update --- libraries/AP_HAL/board/qurt.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/libraries/AP_HAL/board/qurt.h b/libraries/AP_HAL/board/qurt.h index 02c04a52d627d..880eaa7154edf 100644 --- a/libraries/AP_HAL/board/qurt.h +++ b/libraries/AP_HAL/board/qurt.h @@ -44,6 +44,8 @@ #define LUA_USE_LONGJMP 1 #define AP_MAVLINK_FTP_ENABLED 0 #define AP_FILESYSTEM_POSIX_HAVE_UTIME 0 +#define AP_FILESYSTEM_POSIX_HAVE_FSYNC 0 +#define AP_FILESYSTEM_POSIX_HAVE_STATFS 0 #define HAL_PARAM_DEFAULTS_PATH nullptr #define HAL_BOARD_STORAGE_DIRECTORY "APM" From b919dc272552d447ec4337dfbab3034badfa8aac Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 19 May 2024 19:30:43 +1000 Subject: [PATCH 38/73] AP_Filesystem: QURT compat for posix filesystem --- libraries/AP_Filesystem/AP_Filesystem.h | 6 +----- libraries/AP_Filesystem/AP_Filesystem_posix.cpp | 16 ++++++++++++++-- libraries/AP_Filesystem/AP_Filesystem_posix.h | 12 ++++++++++++ libraries/AP_Filesystem/posix_compat.h | 6 ++++++ 4 files changed, 33 insertions(+), 7 deletions(-) diff --git a/libraries/AP_Filesystem/AP_Filesystem.h b/libraries/AP_Filesystem/AP_Filesystem.h index 5b44e985d90ef..cc8619ca52152 100644 --- a/libraries/AP_Filesystem/AP_Filesystem.h +++ b/libraries/AP_Filesystem/AP_Filesystem.h @@ -54,7 +54,7 @@ struct dirent { #define AP_FILESYSTEM_FORMAT_ENABLED 1 #endif -#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX || CONFIG_HAL_BOARD == HAL_BOARD_SITL +#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX || CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_QURT #include "AP_Filesystem_posix.h" #endif @@ -62,10 +62,6 @@ struct dirent { #include "AP_Filesystem_ESP32.h" #endif -#if CONFIG_HAL_BOARD == HAL_BOARD_QURT -#include "AP_Filesystem_QURT.h" -#endif - #include "AP_Filesystem_backend.h" class AP_Filesystem { diff --git a/libraries/AP_Filesystem/AP_Filesystem_posix.cpp b/libraries/AP_Filesystem/AP_Filesystem_posix.cpp index 81e87fa3661e0..2e0a9ed1b501c 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_posix.cpp +++ b/libraries/AP_Filesystem/AP_Filesystem_posix.cpp @@ -91,8 +91,12 @@ int32_t AP_Filesystem_Posix::write(int fd, const void *buf, uint32_t count) int AP_Filesystem_Posix::fsync(int fd) { +#if AP_FILESYSTEM_POSIX_HAVE_FSYNC FS_CHECK_ALLOWED(-1); return ::fsync(fd); +#else + return 0; +#endif } int32_t AP_Filesystem_Posix::lseek(int fd, int32_t offset, int seek_from) @@ -114,7 +118,7 @@ int AP_Filesystem_Posix::unlink(const char *pathname) pathname = map_filename(pathname); // we match the FATFS interface and use unlink // for both files and directories - int ret = ::rmdir(pathname); + int ret = ::rmdir(const_cast(pathname)); if (ret == -1) { ret = ::unlink(pathname); } @@ -125,7 +129,7 @@ int AP_Filesystem_Posix::mkdir(const char *pathname) { FS_CHECK_ALLOWED(-1); pathname = map_filename(pathname); - return ::mkdir(pathname, 0775); + return ::mkdir(const_cast(pathname), 0775); } void *AP_Filesystem_Posix::opendir(const char *pathname) @@ -158,6 +162,7 @@ int AP_Filesystem_Posix::rename(const char *oldpath, const char *newpath) // return free disk space in bytes int64_t AP_Filesystem_Posix::disk_free(const char *path) { +#if AP_FILESYSTEM_POSIX_HAVE_STATFS FS_CHECK_ALLOWED(-1); path = map_filename(path); struct statfs stats; @@ -165,11 +170,15 @@ int64_t AP_Filesystem_Posix::disk_free(const char *path) return -1; } return (((int64_t)stats.f_bavail) * stats.f_bsize); +#else + return -1; +#endif } // return total disk space in bytes int64_t AP_Filesystem_Posix::disk_space(const char *path) { +#if AP_FILESYSTEM_POSIX_HAVE_STATFS FS_CHECK_ALLOWED(-1); path = map_filename(path); struct statfs stats; @@ -177,6 +186,9 @@ int64_t AP_Filesystem_Posix::disk_space(const char *path) return -1; } return (((int64_t)stats.f_blocks) * stats.f_bsize); +#else + return -1; +#endif } diff --git a/libraries/AP_Filesystem/AP_Filesystem_posix.h b/libraries/AP_Filesystem/AP_Filesystem_posix.h index 69103ab297e0b..2e291db70f77d 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_posix.h +++ b/libraries/AP_Filesystem/AP_Filesystem_posix.h @@ -33,6 +33,18 @@ #define AP_FILESYSTEM_POSIX_HAVE_UTIME 1 #endif +#ifndef AP_FILESYSTEM_POSIX_HAVE_FSYNC +#define AP_FILESYSTEM_POSIX_HAVE_FSYNC 1 +#endif + +#ifndef AP_FILESYSTEM_POSIX_HAVE_STATFS +#define AP_FILESYSTEM_POSIX_HAVE_STATFS 1 +#endif + +#ifndef O_CLOEXEC +#define O_CLOEXEC 0 +#endif + class AP_Filesystem_Posix : public AP_Filesystem_Backend { public: diff --git a/libraries/AP_Filesystem/posix_compat.h b/libraries/AP_Filesystem/posix_compat.h index 4ccf56b5c5f43..46b81223c1c42 100644 --- a/libraries/AP_Filesystem/posix_compat.h +++ b/libraries/AP_Filesystem/posix_compat.h @@ -77,6 +77,11 @@ char *tmpnam(char s[L_tmpnam]); #define SEEK_END 2 #endif +#ifndef __cplusplus +/* + only redefine posix functions for C code (eg. lua). + for C++ use the AP_Filsystem APIs +*/ #define FILE APFS_FILE #define fopen(p,m) apfs_fopen(p,m) #define fprintf(stream, format, args...) apfs_fprintf(stream, format, ##args) @@ -101,6 +106,7 @@ char *tmpnam(char s[L_tmpnam]); #define remove(pathname) apfs_remove(pathname) int sprintf(char *str, const char *format, ...); #endif +#endif #ifdef __cplusplus } From b808b856bbbdc442a7c258723aab055d079505c8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 19 May 2024 19:39:09 +1000 Subject: [PATCH 39/73] waf: added -lpthread for QURT --- Tools/ardupilotwaf/boards.py | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/ardupilotwaf/boards.py b/Tools/ardupilotwaf/boards.py index 2c102a89f157d..2e50d7559d5b3 100644 --- a/Tools/ardupilotwaf/boards.py +++ b/Tools/ardupilotwaf/boards.py @@ -1696,6 +1696,7 @@ def configure_env(self, cfg, env): "--wrap=memalign", "--wrap=__stack_chk_fail", "-lc", + '-lpthread', f"-soname={cfg.env.HEXAGON_APP}", "--start-group", "--whole-archive", From c45f821004331011cbe8ccefc10e21e28c908e3f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 20 May 2024 05:50:35 +1000 Subject: [PATCH 40/73] HAL_QURT: templates for replacement fns --- libraries/AP_HAL_QURT/Scheduler.cpp | 4 +- libraries/AP_HAL_QURT/replace.cpp | 72 +++++++++++++++++++++++++++++ 2 files changed, 74 insertions(+), 2 deletions(-) diff --git a/libraries/AP_HAL_QURT/Scheduler.cpp b/libraries/AP_HAL_QURT/Scheduler.cpp index 87d7a794e189a..119465c4b3dbd 100644 --- a/libraries/AP_HAL_QURT/Scheduler.cpp +++ b/libraries/AP_HAL_QURT/Scheduler.cpp @@ -63,7 +63,7 @@ void Scheduler::init() void Scheduler::delay_microseconds(uint16_t usec) { - qurt_sleep(usec); + qurt_timer_sleep(usec); } void Scheduler::delay(uint16_t ms) @@ -137,7 +137,7 @@ void Scheduler::resume_timer_procs() void Scheduler::reboot(bool hold_in_bootloader) { HAP_PRINTF("**** REBOOT REQUESTED ****"); - qurt_sleep(2000000); + qurt_timer_sleep(2000000); exit(1); } diff --git a/libraries/AP_HAL_QURT/replace.cpp b/libraries/AP_HAL_QURT/replace.cpp index 050d2c9b77710..c8ba0f34003a3 100644 --- a/libraries/AP_HAL_QURT/replace.cpp +++ b/libraries/AP_HAL_QURT/replace.cpp @@ -6,7 +6,9 @@ #include #include #include +#include #include "replace.h" +#include "interface.h" extern "C" { @@ -73,4 +75,74 @@ int asprintf(char **ptr, const char *format, ...) return ret; } +void *memmem(const void *haystack, size_t haystacklen, + const void *needle, size_t needlelen) +{ + // TODO: IMPLEMENT THIS!!! + return NULL; +} + +char *strndup(const char *str, size_t size) +{ + // TODO: IMPLEMENT THIS!!! + return NULL; +} + +// INVESTIGATE: Why are these next two pthread functions not in the SLPI base image? +int pthread_mutexattr_setprotocol(pthread_mutexattr_t *attr, int protocol) +{ + return 0; +} +int pthread_cond_init(pthread_cond_t *cond, pthread_condattr_t *attr) +{ + return 0; +} + +// INVESTIGATE: What is this needed on QURT? +int apfs_rename(const char *oldpath, const char *newpath) +{ + return 0; +} + +// INVESTIGATE: How to enable +void lua_abort() {} +const char* lua_get_modules_path() {return NULL;} +int lua_get_current_ref() {return 0;} + +// INVESTIGATE: Seems important :-) +int ArduPilot_main(int argc, const char *argv[]) +{ + return 0; +} + +} + +int px4muorb_orb_initialize(fc_func_ptrs *func_ptrs, int32_t clock_offset_us) +{ + return 0; +} + +int px4muorb_topic_advertised(const char *name) +{ + return 0; +} + +int px4muorb_add_subscriber(const char *name) +{ + return 0; +} + +int px4muorb_remove_subscriber(const char *name) +{ + return 0; +} + +int px4muorb_send_topic_data(const char *name, const uint8_t *data, int data_len_in_bytes) +{ + return 0; +} + +float px4muorb_get_cpu_load(void) +{ + return 0.0f; } From 13253848d7f5515c0391d48c713204e7b4db9865 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 20 May 2024 05:53:54 +1000 Subject: [PATCH 41/73] AP_JSON: only build with simulator enabled --- libraries/AP_JSON/AP_JSON.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/libraries/AP_JSON/AP_JSON.cpp b/libraries/AP_JSON/AP_JSON.cpp index 32ee85fe2beef..5d2e4c3dae0db 100644 --- a/libraries/AP_JSON/AP_JSON.cpp +++ b/libraries/AP_JSON/AP_JSON.cpp @@ -41,6 +41,9 @@ #include #include +// only needed for simulation environments for now +#if AP_SIM_ENABLED + /* load JSON file, returning a value object or nullptr on failure */ @@ -716,3 +719,5 @@ std::string AP_JSON::parse(value &out, const std::string &s) ::parse(out, s.begin(), s.end(), &err); return err; } + +#endif // AP_SIM_ENABLED From 701da7ee09eb070c8bcb5ed67b6089925404d8a4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 20 May 2024 05:57:35 +1000 Subject: [PATCH 42/73] waf: pthread link not needed for QURT --- Tools/ardupilotwaf/boards.py | 1 - 1 file changed, 1 deletion(-) diff --git a/Tools/ardupilotwaf/boards.py b/Tools/ardupilotwaf/boards.py index 2e50d7559d5b3..2c102a89f157d 100644 --- a/Tools/ardupilotwaf/boards.py +++ b/Tools/ardupilotwaf/boards.py @@ -1696,7 +1696,6 @@ def configure_env(self, cfg, env): "--wrap=memalign", "--wrap=__stack_chk_fail", "-lc", - '-lpthread', f"-soname={cfg.env.HEXAGON_APP}", "--start-group", "--whole-archive", From d6f8f869c5649673374b02a4a233ed360ace8e47 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 20 May 2024 09:39:35 +1000 Subject: [PATCH 43/73] HAL_QURT: fixed semaphores --- libraries/AP_HAL_QURT/Semaphores.cpp | 55 +++++++--------------------- libraries/AP_HAL_QURT/Semaphores.h | 9 +++-- libraries/AP_HAL_QURT/replace.cpp | 40 ++++++++++++++------ 3 files changed, 48 insertions(+), 56 deletions(-) diff --git a/libraries/AP_HAL_QURT/Semaphores.cpp b/libraries/AP_HAL_QURT/Semaphores.cpp index b9d454b7cf542..21837764c8f2d 100644 --- a/libraries/AP_HAL_QURT/Semaphores.cpp +++ b/libraries/AP_HAL_QURT/Semaphores.cpp @@ -9,71 +9,46 @@ using namespace QURT; // construct a semaphore Semaphore::Semaphore() { - pthread_mutexattr_t attr; - pthread_mutexattr_init(&attr); - pthread_mutexattr_setprotocol(&attr, PTHREAD_PRIO_INHERIT); - pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_RECURSIVE); - pthread_mutex_init(&_lock, &attr); + qurt_rmutex_init(&_lock); } bool Semaphore::give() { - return pthread_mutex_unlock(&_lock) == 0; + qurt_rmutex_unlock(&_lock); + return true; } bool Semaphore::take(uint32_t timeout_ms) { if (timeout_ms == HAL_SEMAPHORE_BLOCK_FOREVER) { - return pthread_mutex_lock(&_lock) == 0; - } - if (take_nonblocking()) { + qurt_rmutex_lock(&_lock); 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; + return qurt_rmutex_lock_timed(&_lock, timeout_ms*1000UL); } bool Semaphore::take_nonblocking() { - return pthread_mutex_trylock(&_lock) == 0; + return qurt_rmutex_try_lock(&_lock) == 0; } /* - binary semaphore using pthread condition variables + binary semaphore using condition variables */ BinarySemaphore::BinarySemaphore(bool initial_state) : AP_HAL::BinarySemaphore(initial_state) { - pthread_cond_init(&cond, NULL); + qurt_cond_init(&cond); pending = initial_state; } bool BinarySemaphore::wait(uint32_t timeout_us) { - WITH_SEMAPHORE(mtx); - if (!pending) { - struct timespec ts; - if (clock_gettime(CLOCK_REALTIME, &ts) != 0) { - return false; - } - ts.tv_sec += timeout_us/1000000UL; - ts.tv_nsec += (timeout_us % 1000000U) * 1000UL; - if (ts.tv_nsec >= 1000000000L) { - ts.tv_sec++; - ts.tv_nsec -= 1000000000L; - } - if (pthread_cond_timedwait(&cond, &mtx._lock, &ts) != 0) { - return false; - } - } - pending = false; + /* + not available! + */ + AP_HAL::panic("no timed BinarySemaphore"); return true; } @@ -81,9 +56,7 @@ bool BinarySemaphore::wait_blocking(void) { WITH_SEMAPHORE(mtx); if (!pending) { - if (pthread_cond_wait(&cond, &mtx._lock) != 0) { - return false; - } + qurt_cond_wait(&cond, &mtx._lock); } pending = false; return true; @@ -94,6 +67,6 @@ void BinarySemaphore::signal(void) WITH_SEMAPHORE(mtx); if (!pending) { pending = true; - pthread_cond_signal(&cond); + qurt_cond_signal(&cond); } } diff --git a/libraries/AP_HAL_QURT/Semaphores.h b/libraries/AP_HAL_QURT/Semaphores.h index 3e29abb723d38..84a81eb0b7546 100644 --- a/libraries/AP_HAL_QURT/Semaphores.h +++ b/libraries/AP_HAL_QURT/Semaphores.h @@ -3,7 +3,10 @@ #include #include #include -#include +extern "C" { +#include +#include +} namespace QURT { @@ -15,7 +18,7 @@ class Semaphore : public AP_HAL::Semaphore { bool take(uint32_t timeout_ms) override; bool take_nonblocking() override; protected: - pthread_mutex_t _lock; + qurt_mutex_t _lock; }; @@ -29,7 +32,7 @@ class BinarySemaphore : public AP_HAL::BinarySemaphore { protected: Semaphore mtx; - pthread_cond_t cond; + qurt_cond_t cond; bool pending; }; diff --git a/libraries/AP_HAL_QURT/replace.cpp b/libraries/AP_HAL_QURT/replace.cpp index c8ba0f34003a3..eeb440e9db2e6 100644 --- a/libraries/AP_HAL_QURT/replace.cpp +++ b/libraries/AP_HAL_QURT/replace.cpp @@ -78,21 +78,37 @@ int asprintf(char **ptr, const char *format, ...) void *memmem(const void *haystack, size_t haystacklen, const void *needle, size_t needlelen) { - // TODO: IMPLEMENT THIS!!! - return NULL; -} + if (needlelen == 0) { + return const_cast(haystack); + } + while (haystacklen >= needlelen) { + char *p = (char *)memchr(haystack, *(const char *)needle, + haystacklen-(needlelen-1)); + if (!p) return NULL; + if (memcmp(p, needle, needlelen) == 0) { + return p; + } + haystack = p+1; + haystacklen -= (p - (const char *)haystack) + 1; + } + return NULL; +} + +char *strndup(const char *s, size_t n) +{ + char *ret; + + n = strnlen(s, n); + ret = (char*)malloc(n+1); + if (!ret) { + return NULL; + } + memcpy(ret, s, n); + ret[n] = 0; -char *strndup(const char *str, size_t size) -{ - // TODO: IMPLEMENT THIS!!! - return NULL; + return ret; } -// INVESTIGATE: Why are these next two pthread functions not in the SLPI base image? -int pthread_mutexattr_setprotocol(pthread_mutexattr_t *attr, int protocol) -{ - return 0; -} int pthread_cond_init(pthread_cond_t *cond, pthread_condattr_t *attr) { return 0; From 15427a55f55de837ba10d980de3f6b7713513d83 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 20 May 2024 14:37:54 +1000 Subject: [PATCH 44/73] HAL_QURT: added interface.h --- libraries/AP_HAL_QURT/interface.h | 50 +++++++++++++++++++++++++++++++ 1 file changed, 50 insertions(+) create mode 100644 libraries/AP_HAL_QURT/interface.h diff --git a/libraries/AP_HAL_QURT/interface.h b/libraries/AP_HAL_QURT/interface.h new file mode 100644 index 0000000000000..cb557543ac309 --- /dev/null +++ b/libraries/AP_HAL_QURT/interface.h @@ -0,0 +1,50 @@ +#define __EXPORT __attribute__ ((visibility ("default"))) + +// Should be in termios.h +typedef unsigned int speed_t; + +// TODO: This has to be defined in the slpi_proc build and in the PX4 build. +// Make it accessible from one file to both builds. +typedef struct { + int (*advertise_func_ptr)(const char *topic_name); + int (*subscribe_func_ptr)(const char *topic_name); + int (*unsubscribe_func_ptr)(const char *topic_name); + int (*topic_data_func_ptr)(const char *name, const uint8_t *data, int data_len_in_bytes); + // device::SPI::_config_spi_bus_func_t config_spi_bus; + // device::SPI::_spi_transfer_func_t spi_transfer; + int (*_config_spi_bus_func_t)(); + int (*_spi_transfer_func_t)(int, const uint8_t *, uint8_t *, const unsigned); + // device::I2C::_config_i2c_bus_func_t config_i2c_bus; + // device::I2C::_set_i2c_address_func_t set_i2c_address; + // device::I2C::_i2c_transfer_func_t i2c_transfer; + int (*_config_i2c_bus_func_t)(uint8_t, uint8_t, uint32_t); + int (*_set_i2c_address_func_t)(int, uint8_t); + int (*_i2c_transfer_func_t)(int, const uint8_t *, const unsigned, uint8_t *, const unsigned); + // open_uart_func_t open_uart_func; + // write_uart_func_t write_uart_func; + // read_uart_func_t read_uart_func; + int (*open_uart_func_t)(uint8_t, speed_t); + int (*write_uart_func_t)(int, const void *, size_t); + int (*read_uart_func_t)(int, void *, size_t); + int (*register_interrupt_callback)(int (*)(int, void *, void *), void *arg); +} fc_func_ptrs; + +#ifndef __cplusplus +#error "C++ should be defined!!!" +#endif + +extern "C" { + + int px4muorb_orb_initialize(fc_func_ptrs *func_ptrs, int32_t clock_offset_us) __EXPORT; + + int px4muorb_topic_advertised(const char *name) __EXPORT; + + int px4muorb_add_subscriber(const char *name) __EXPORT; + + int px4muorb_remove_subscriber(const char *name) __EXPORT; + + int px4muorb_send_topic_data(const char *name, const uint8_t *data, int data_len_in_bytes) __EXPORT; + + float px4muorb_get_cpu_load(void) __EXPORT; + +} \ No newline at end of file From 3f934625a1e42d5f5588a564f0597b17f13ff9fc Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 19 May 2024 07:35:27 +1000 Subject: [PATCH 45/73] Tools: specify pymonocypher version in more places and confirm version when running tools --- Tools/ardupilotwaf/chibios.py | 7 ++++++- Tools/scripts/signing/README.md | 10 ++++++++-- Tools/scripts/signing/generate_keys.py | 5 ++++- Tools/scripts/signing/make_secure_bl.py | 6 ------ Tools/scripts/signing/make_secure_fw.py | 6 +++++- 5 files changed, 23 insertions(+), 11 deletions(-) diff --git a/Tools/ardupilotwaf/chibios.py b/Tools/ardupilotwaf/chibios.py index 26b8c0e313fd4..4f1e7a8626d0c 100644 --- a/Tools/ardupilotwaf/chibios.py +++ b/Tools/ardupilotwaf/chibios.py @@ -261,8 +261,13 @@ def sign_firmware(image, private_keyfile): try: import monocypher except ImportError: - Logs.error("Please install monocypher with: python3 -m pip install pymonocypher") + Logs.error("Please install monocypher with: python3 -m pip install pymonocypher==3.1.3.2") return None + + if monocypher.__version__ != "3.1.3.2": + Logs.error("must use monocypher 3.1.3.2, please run: python3 -m pip install pymonocypher==3.1.3.2") + return None + try: key = open(private_keyfile, 'r').read() except Exception as ex: diff --git a/Tools/scripts/signing/README.md b/Tools/scripts/signing/README.md index dc4032c00987f..06fe1d6527b29 100644 --- a/Tools/scripts/signing/README.md +++ b/Tools/scripts/signing/README.md @@ -12,7 +12,7 @@ firmware doesn't match any of the public keys in the bootloader. To generate a public/private key pair, run the following command: ``` - python3 -m pip install pymonocypher + python3 -m pip install pymonocypher==3.1.3.2 Tools/scripts/signing/generate_keys.py NAME ``` @@ -131,7 +131,13 @@ This opens a secure command session using your private_key.dat file to allow the securecommand getpublickeys will return the number of public keys...you will need this next securecommand removepublickeys 0 X where X is the number of public keys...this removes them ``` -Re-run the 'getpublickeys' command again to verify that all keys have been removed. + +For example, if you have a standard firmware with the 3 ArduPilot +public keys and one of your own public keys then X will be 4 in the +above command. + +Re-run the 'getpublickeys' command again to verify that all keys have +been removed. Now exit MAVProxy and build a firmware using the normal bootloader but still using the --signed-fw option: diff --git a/Tools/scripts/signing/generate_keys.py b/Tools/scripts/signing/generate_keys.py index 5da4388c308f9..38b7d38650452 100755 --- a/Tools/scripts/signing/generate_keys.py +++ b/Tools/scripts/signing/generate_keys.py @@ -9,9 +9,12 @@ try: import monocypher except ImportError: - print("Please install monocypher with: python3 -m pip install pymonocypher") + print("Please install monocypher with: python3 -m pip install pymonocypher==3.1.3.2") sys.exit(1) +if monocypher.__version__ != "3.1.3.2": + Logs.error("must use monocypher 3.1.3.2, please run: python3 -m pip install pymonocypher==3.1.3.2") + sys.exit(1) if len(sys.argv) != 2: print("Usage: generate_keys.py BASENAME") diff --git a/Tools/scripts/signing/make_secure_bl.py b/Tools/scripts/signing/make_secure_bl.py index bd60bee3c7af0..5187b0f15d3b1 100755 --- a/Tools/scripts/signing/make_secure_bl.py +++ b/Tools/scripts/signing/make_secure_bl.py @@ -7,12 +7,6 @@ import os import base64 -try: - import monocypher -except ImportError: - print("Please install monocypher with: python3 -m pip install pymonocypher") - sys.exit(1) - # get command line arguments from argparse import ArgumentParser parser = ArgumentParser(description='make_secure_bl') diff --git a/Tools/scripts/signing/make_secure_fw.py b/Tools/scripts/signing/make_secure_fw.py index 4e916090fc227..8c8862cb62f8c 100755 --- a/Tools/scripts/signing/make_secure_fw.py +++ b/Tools/scripts/signing/make_secure_fw.py @@ -10,9 +10,13 @@ try: import monocypher except ImportError: - print("Please install monocypher with: python3 -m pip install pymonocypher") + print("Please install monocypher with: python3 -m pip install pymonocypher==3.1.3.2") sys.exit(1) +if monocypher.__version__ != "3.1.3.2": + Logs.error("must use monocypher 3.1.3.2, please run: python3 -m pip install pymonocypher==3.1.3.2") + return None + key_len = 32 sig_len = 64 sig_version = 30437 From d9c540337d62c50e799f4664ffaa04b71bc94212 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 17 May 2024 14:08:00 +1000 Subject: [PATCH 46/73] autotest: add support for minimum_duration to wait_ekf_flags .... by re-implementing in terms of a WaitAndMaintain class --- Tools/autotest/vehicle_test_suite.py | 70 +++++++++++++++++----------- 1 file changed, 43 insertions(+), 27 deletions(-) diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py index 07a0e13b43654..b788843a13a8b 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -465,6 +465,47 @@ def progress_text(self, current_value): return (f"Want=({self.target.lat},{self.target.lng}) distance={self.horizontal_error(current_value)}") +class WaitAndMaintainEKFFlags(WaitAndMaintain): + '''Waits for EKF status flags to include required_flags and have + error_bits *not* set.''' + def __init__(self, test_suite, required_flags, error_bits, **kwargs): + super(WaitAndMaintainEKFFlags, self).__init__(test_suite, **kwargs) + self.required_flags = required_flags + self.error_bits = error_bits + self.last_EKF_STATUS_REPORT = None + + def announce_start_text(self): + return f"Waiting for EKF value {self.required_flags}" + + def get_current_value(self): + self.last_EKF_STATUS_REPORT = self.test_suite.assert_receive_message('EKF_STATUS_REPORT', timeout=10) + return self.last_EKF_STATUS_REPORT.flags + + def validate_value(self, value): + if value & self.error_bits: + return False + + if (value & self.required_flags) != self.required_flags: + return False + + return True + + def success_text(self): + return "EKF Flags OK" + + def timeoutexception(self): + self.progress("Last EKF status report:") + self.progress(self.test_suite.dump_message_verbose(self.last_EKF_STATUS_REPORT)) + + return AutoTestTimeoutException(f"Failed to get EKF.flags={self.required_flags}") + + def progress_text(self, current_value): + error_bits_str = "" + if current_value & self.error_bits: + error_bits_str = " (error bits present)" + return (f"Want=({self.required_flags}) got={current_value}{error_bits_str}") + + class WaitAndMaintainArmed(WaitAndMaintain): def get_current_value(self): return self.test_suite.armed() @@ -8017,33 +8058,8 @@ def wait_ekf_happy(self, timeout=45, require_absolute=True): error_bits |= mavutil.mavlink.ESTIMATOR_GPS_GLITCH self.wait_ekf_flags(required_value, error_bits, timeout=timeout) - def wait_ekf_flags(self, required_value, error_bits, timeout=30): - self.progress("Waiting for EKF value %u" % required_value) - last_print_time = 0 - tstart = self.get_sim_time() - m = None - while timeout is None or self.get_sim_time_cached() < tstart + timeout: - m = self.mav.recv_match(type='EKF_STATUS_REPORT', blocking=True, timeout=timeout) - if m is None: - continue - current = m.flags - errors = current & error_bits - everything_ok = (errors == 0 and - current & required_value == required_value) - if everything_ok or self.get_sim_time_cached() - last_print_time > 1: - self.progress("Wait EKF.flags: required:%u current:%u errors=%u" % - (required_value, current, errors)) - last_print_time = self.get_sim_time_cached() - if everything_ok: - self.progress("EKF Flags OK") - return True - m_str = str(m) - if m is not None: - m_str = self.dump_message_verbose(m) - self.progress("Last EKF_STATUS_REPORT message:") - self.progress(m_str) - raise AutoTestTimeoutException("Failed to get EKF.flags=%u" % - required_value) + def wait_ekf_flags(self, required_value, error_bits, **kwargs): + WaitAndMaintainEKFFlags(self, required_value, error_bits, **kwargs).run() def wait_gps_disable(self, position_horizontal=True, position_vertical=False, timeout=30): """Disable GPS and wait for EKF to report the end of assistance from GPS.""" From 0d625d36d5405b5f7a2d113d5659d53cfddd67ff Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 17 May 2024 14:13:20 +1000 Subject: [PATCH 47/73] autotest: pass kwargs straight through in wait_ekf_args this allows minimum_duration to be passed through to the underlying methods, for example --- Tools/autotest/vehicle_test_suite.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py index b788843a13a8b..ebc16cf3b2f53 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -8035,8 +8035,10 @@ def wait_heartbeat(self, drain_mav=True, quiet=False, *args, **x): if m.get_srcSystem() == self.sysid_thismav(): return m - def wait_ekf_happy(self, timeout=45, require_absolute=True): + def wait_ekf_happy(self, require_absolute=True, **kwargs): """Wait for EKF to be happy""" + if "timeout" not in kwargs: + kwargs["timeout"] = 45 """ if using SITL estimates directly """ if (int(self.get_parameter('AHRS_EKF_TYPE')) == 10): @@ -8056,7 +8058,7 @@ def wait_ekf_happy(self, timeout=45, require_absolute=True): mavutil.mavlink.ESTIMATOR_POS_VERT_ABS | mavutil.mavlink.ESTIMATOR_PRED_POS_HORIZ_ABS) error_bits |= mavutil.mavlink.ESTIMATOR_GPS_GLITCH - self.wait_ekf_flags(required_value, error_bits, timeout=timeout) + WaitAndMaintainEKFFlags(self, required_value, error_bits, **kwargs).run() def wait_ekf_flags(self, required_value, error_bits, **kwargs): WaitAndMaintainEKFFlags(self, required_value, error_bits, **kwargs).run() From b810bf2a9b5d6889085481619e65242b0b204bbc Mon Sep 17 00:00:00 2001 From: Eric Katzfey Date: Mon, 20 May 2024 11:22:37 -0700 Subject: [PATCH 48/73] AP_HAL_QURT: Moved Semaphore implementation to the Linux implementation --- libraries/AP_HAL_QURT/Semaphores.cpp | 52 +++++++++++++++++++++------- libraries/AP_HAL_QURT/Semaphores.h | 10 +++--- 2 files changed, 43 insertions(+), 19 deletions(-) diff --git a/libraries/AP_HAL_QURT/Semaphores.cpp b/libraries/AP_HAL_QURT/Semaphores.cpp index 21837764c8f2d..f6e3154f73611 100644 --- a/libraries/AP_HAL_QURT/Semaphores.cpp +++ b/libraries/AP_HAL_QURT/Semaphores.cpp @@ -9,27 +9,38 @@ using namespace QURT; // construct a semaphore Semaphore::Semaphore() { - qurt_rmutex_init(&_lock); + pthread_mutexattr_t attr; + pthread_mutexattr_init(&attr); + pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_RECURSIVE); + pthread_mutex_init(&_lock, &attr); } bool Semaphore::give() { - qurt_rmutex_unlock(&_lock); - return true; + return pthread_mutex_unlock(&_lock) == 0; } bool Semaphore::take(uint32_t timeout_ms) { if (timeout_ms == HAL_SEMAPHORE_BLOCK_FOREVER) { - qurt_rmutex_lock(&_lock); + return pthread_mutex_lock(&_lock) == 0; + } + if (take_nonblocking()) { return true; } - return qurt_rmutex_lock_timed(&_lock, timeout_ms*1000UL); + 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 qurt_rmutex_try_lock(&_lock) == 0; + return pthread_mutex_trylock(&_lock) == 0; } /* @@ -39,16 +50,29 @@ bool Semaphore::take_nonblocking() BinarySemaphore::BinarySemaphore(bool initial_state) : AP_HAL::BinarySemaphore(initial_state) { - qurt_cond_init(&cond); + pthread_cond_init(&cond, NULL); pending = initial_state; } bool BinarySemaphore::wait(uint32_t timeout_us) { - /* - not available! - */ - AP_HAL::panic("no timed BinarySemaphore"); + WITH_SEMAPHORE(mtx); + if (!pending) { + struct timespec ts; + if (clock_gettime(CLOCK_REALTIME, &ts) != 0) { + return false; + } + ts.tv_sec += timeout_us/1000000UL; + ts.tv_nsec += (timeout_us % 1000000U) * 1000UL; + if (ts.tv_nsec >= 1000000000L) { + ts.tv_sec++; + ts.tv_nsec -= 1000000000L; + } + if (pthread_cond_timedwait(&cond, &mtx._lock, &ts) != 0) { + return false; + } + } + pending = false; return true; } @@ -56,7 +80,9 @@ bool BinarySemaphore::wait_blocking(void) { WITH_SEMAPHORE(mtx); if (!pending) { - qurt_cond_wait(&cond, &mtx._lock); + if (pthread_cond_wait(&cond, &mtx._lock) != 0) { + return false; + } } pending = false; return true; @@ -67,6 +93,6 @@ void BinarySemaphore::signal(void) WITH_SEMAPHORE(mtx); if (!pending) { pending = true; - qurt_cond_signal(&cond); + pthread_cond_signal(&cond); } } diff --git a/libraries/AP_HAL_QURT/Semaphores.h b/libraries/AP_HAL_QURT/Semaphores.h index 84a81eb0b7546..1b64bd0734540 100644 --- a/libraries/AP_HAL_QURT/Semaphores.h +++ b/libraries/AP_HAL_QURT/Semaphores.h @@ -3,10 +3,7 @@ #include #include #include -extern "C" { -#include -#include -} +#include namespace QURT { @@ -18,7 +15,8 @@ class Semaphore : public AP_HAL::Semaphore { bool take(uint32_t timeout_ms) override; bool take_nonblocking() override; protected: - qurt_mutex_t _lock; + // qurt_mutex_t _lock; + pthread_mutex_t _lock; }; @@ -32,7 +30,7 @@ class BinarySemaphore : public AP_HAL::BinarySemaphore { protected: Semaphore mtx; - qurt_cond_t cond; + pthread_cond_t cond; bool pending; }; From be872d65a6bf7007ccc0c209bfc32be742476b47 Mon Sep 17 00:00:00 2001 From: Eric Katzfey Date: Mon, 20 May 2024 17:25:27 -0700 Subject: [PATCH 49/73] Removed unnecessary dsp_main.cpp file --- libraries/AP_HAL_QURT/dsp_main.cpp | 105 ----------------------------- 1 file changed, 105 deletions(-) delete mode 100644 libraries/AP_HAL_QURT/dsp_main.cpp diff --git a/libraries/AP_HAL_QURT/dsp_main.cpp b/libraries/AP_HAL_QURT/dsp_main.cpp deleted file mode 100644 index cfffd08b4104c..0000000000000 --- a/libraries/AP_HAL_QURT/dsp_main.cpp +++ /dev/null @@ -1,105 +0,0 @@ -#include -#include -#include -#include -#include "Scheduler.h" -#include "Storage.h" -#include "replace.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 *cmdptr) -{ - char *cmdline = (char *)cmdptr; - HAP_PRINTF("Ardupilot main_thread started"); - - // break cmdline into argc/argv - int argc = 0; - for (int i=0; cmdline[i]; i++) { - if (cmdline[i] == '\n') argc++; - } - const char **argv = (const char **)calloc(argc+2, sizeof(char *)); - argv[0] = &cmdline[0]; - argc = 0; - for (int i=0; cmdline[i]; i++) { - if (cmdline[i] == '\n') { - cmdline[i] = 0; - argv[argc+1] = &cmdline[i+1]; - argc++; - } - } - argv[argc] = nullptr; - - ArduPilot_main(argc, argv); - return nullptr; -} - - -uint32_t ardupilot_start(const char *cmdline, int len) -{ - 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, (void *)strdup((char*)cmdline)); - 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(HAL_SEMAPHORE_BLOCK_FOREVER); - 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(HAL_SEMAPHORE_BLOCK_FOREVER); - 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 0; -} - -uint32_t ardupilot_socket_input(const uint8_t *buf, int len, uint32_t *nbytes) -{ - return 0; -} From e5e7a2b3d29bde4bd1849e1273847b99cf2f7192 Mon Sep 17 00:00:00 2001 From: Eric Katzfey Date: Tue, 21 May 2024 16:38:25 -0700 Subject: [PATCH 50/73] AP_HAL_QURT: Bunch of changes to link everything properly and call the run method --- Tools/ardupilotwaf/boards.py | 24 +++++++++------ libraries/AP_HAL/board/qurt.h | 2 ++ libraries/AP_HAL_QURT/AP_HAL_QURT_Main.h | 3 ++ libraries/AP_HAL_QURT/HAL_QURT_Class.cpp | 13 ++++---- libraries/AP_HAL_QURT/replace.cpp | 39 +++++++++++++++++++++++- 5 files changed, 64 insertions(+), 17 deletions(-) diff --git a/Tools/ardupilotwaf/boards.py b/Tools/ardupilotwaf/boards.py index 2c102a89f157d..a5b6a65a923ff 100644 --- a/Tools/ardupilotwaf/boards.py +++ b/Tools/ardupilotwaf/boards.py @@ -1666,8 +1666,7 @@ def configure_env(self, cfg, env): super(QURT, self).configure_env(cfg, env) env.BOARD_CLASS = "QURT" - env.HEAXGON_APP = "libardupilot.so" - env.HEXAGON_LINKFLAGS = f"-march=hexagon -mcpu=hexagonv66 -fpic -shared -call_shared -mG0lib -G0 {cfg.env.TOOLCHAIN_DIR}/target/hexagon/lib/v66/G0/pic/initS.o -L{cfg.env.TOOLCHAIN_DIR}/target/hexagon/lib/v66/G0/pic -Bsymbolic {cfg.env.TOOLCHAIN_DIR}/target/hexagon/lib/v66/G0/pic/libgcc.a --wrap=malloc --wrap=calloc --wrap=free --wrap=realloc --wrap=memalign --wrap=__stack_chk_fail -lc -soname={env.HEXAGON_APP} --start-group --whole-archive OBJECT_LIST --end-group --start-group -lgcc --end-group {cfg.env.TOOLCHAIN_DIR}/target/hexagon/lib/v66/G0/pic/finiS.o" + env.HEXAGON_APP = "libardupilot.so" env.INCLUDES += [cfg.env.HEXAGON_SDK_DIR + "/rtos/qurt/computev66/include/qurt"] env.INCLUDES += [cfg.env.HEXAGON_SDK_DIR + "/rtos/qurt/computev66/include/posix"] env.CXXFLAGS += ["-fPIC", "-MD"] @@ -1679,6 +1678,18 @@ def configure_env(self, cfg, env): AP_SIM_ENABLED = 0, ) +# The linking has to be done very precisely for QURT. Below is an example of a +# linker run_str that works correctly. This needs to be modified in the waflib/Tools/cxx.py +# file in the waf submodule + +# class cxxprogram(link_task): +# "Links object files into c++ programs" +# # run_str = '${LINK_CXX} ${LINKFLAGS} ${CXXLNK_SRC_F}${SRC} ${CXXLNK_TGT_F}${TGT[0].relpath()} ${RPATH_ST:RPATH} ${FRAMEWORKPATH_ST:FRAMEWORKPATH} ${FRAMEWORK_ST:FRAMEWORK} ${ARCH_ST:ARCH} ${STLIB_MARKER} ${STLIBPATH_ST:STLIBPATH} ${STLIB_ST:STLIB} ${SHLIB_MARKER} ${LIBPATH_ST:LIBPATH} ${LIB_ST:LIB} ${LDFLAGS}' +# run_str = '${LINK_CXX} ${CXXLNK_TGT_F} ${TGT[0].relpath()} ${LINKFLAGS} ${STLIBPATH_ST:STLIBPATH} --start-group --whole-archive ${CXXLNK_SRC_F}${SRC} ${STLIB_ST:STLIB} --end-group --start-group -lgcc --end-group /opt/hexagon-sdk/4.1.0.4-lite/tools/HEXAGON_Tools/8.4.05/Tools/target/hexagon/lib/v66/G0/pic/finiS.o' +# vars = ['LINKDEPS'] +# ext_out = ['.bin'] +# inst_to = '${BINDIR}' + env.LINKFLAGS = [ "-march=hexagon", "-mcpu=hexagonv66", @@ -1696,14 +1707,7 @@ def configure_env(self, cfg, env): "--wrap=memalign", "--wrap=__stack_chk_fail", "-lc", - f"-soname={cfg.env.HEXAGON_APP}", - "--start-group", - "--whole-archive", - "--end-group", - "--start-group", - "-lgcc", - "--end-group", - cfg.env.TOOLCHAIN_DIR + "/target/hexagon/lib/v66/G0/pic/finiS.o" + "-lc++" ] if not cfg.env.DEBUG: diff --git a/libraries/AP_HAL/board/qurt.h b/libraries/AP_HAL/board/qurt.h index 880eaa7154edf..4e4baced78195 100644 --- a/libraries/AP_HAL/board/qurt.h +++ b/libraries/AP_HAL/board/qurt.h @@ -1,5 +1,7 @@ #pragma once +#include + #define HAL_BOARD_NAME "QURT" #define HAL_CPU_CLASS HAL_CPU_CLASS_1000 #define HAL_MEM_CLASS HAL_MEM_CLASS_1000 diff --git a/libraries/AP_HAL_QURT/AP_HAL_QURT_Main.h b/libraries/AP_HAL_QURT/AP_HAL_QURT_Main.h index a38bfd959c5af..e0b57992c2513 100644 --- a/libraries/AP_HAL_QURT/AP_HAL_QURT_Main.h +++ b/libraries/AP_HAL_QURT/AP_HAL_QURT_Main.h @@ -13,3 +13,6 @@ along with this program. If not, see . */ #pragma once + +#define AP_MAIN qurt_arducopter_main + diff --git a/libraries/AP_HAL_QURT/HAL_QURT_Class.cpp b/libraries/AP_HAL_QURT/HAL_QURT_Class.cpp index 7fedc94d9f724..278b738c14cce 100644 --- a/libraries/AP_HAL_QURT/HAL_QURT_Class.cpp +++ b/libraries/AP_HAL_QURT/HAL_QURT_Class.cpp @@ -86,12 +86,13 @@ void HAL_QURT::run(int argc, char* const argv[], Callbacks* callbacks) const schedulerInstance.hal_initialized(); serial0Driver.begin(115200); rcinDriver.init(); - callbacks->setup(); - scheduler->set_system_initialized(); - - for (;;) { - callbacks->loop(); - } + // Runs okay up to here. Next line causes a crash! + // callbacks->setup(); + // scheduler->set_system_initialized(); + // + // for (;;) { + // callbacks->loop(); + // } } const AP_HAL::HAL& AP_HAL::get_HAL() { diff --git a/libraries/AP_HAL_QURT/replace.cpp b/libraries/AP_HAL_QURT/replace.cpp index eeb440e9db2e6..4e9e2c6ef0003 100644 --- a/libraries/AP_HAL_QURT/replace.cpp +++ b/libraries/AP_HAL_QURT/replace.cpp @@ -133,9 +133,15 @@ int ArduPilot_main(int argc, const char *argv[]) } +extern "C" int qurt_arducopter_main(int argc, char* const argv[]); + int px4muorb_orb_initialize(fc_func_ptrs *func_ptrs, int32_t clock_offset_us) { - return 0; + HAP_PRINTF("About to call qurt_arducopter_main %p", &qurt_arducopter_main); + + qurt_arducopter_main(0, NULL); + + return 0; } int px4muorb_topic_advertised(const char *name) @@ -162,3 +168,34 @@ float px4muorb_get_cpu_load(void) { return 0.0f; } + +extern "C" void free(void *ptr) { + return; +} + +// malloc +extern "C" void *malloc(size_t size) { + return NULL; +} + +// posix_memalign +extern "C" int posix_memalign(void **memptr, size_t alignment, size_t size) { + return 0; +} + +// calloc +extern "C" void *calloc(size_t nmemb, size_t size) { + return NULL; +} + + +// realloc +extern "C" void *realloc(void *ptr, size_t size) { + return NULL; +} + +// nanosleep +#include +extern "C" int nanosleep(const struct timespec *req, struct timespec *_Nullable rem) { + return 0; +} From 358d249c20876877dd2fb3d2fc76f491673271b0 Mon Sep 17 00:00:00 2001 From: Eric Katzfey Date: Tue, 21 May 2024 17:46:52 -0700 Subject: [PATCH 51/73] Cleaned up a little more after hackfest to get link to work --- Tools/ardupilotwaf/boards.py | 3 +-- libraries/AP_HAL_QURT/replace.cpp | 30 ------------------------------ 2 files changed, 1 insertion(+), 32 deletions(-) diff --git a/Tools/ardupilotwaf/boards.py b/Tools/ardupilotwaf/boards.py index a5b6a65a923ff..b3971c568755e 100644 --- a/Tools/ardupilotwaf/boards.py +++ b/Tools/ardupilotwaf/boards.py @@ -1706,8 +1706,7 @@ def configure_env(self, cfg, env): "--wrap=realloc", "--wrap=memalign", "--wrap=__stack_chk_fail", - "-lc", - "-lc++" + "-lc" ] if not cfg.env.DEBUG: diff --git a/libraries/AP_HAL_QURT/replace.cpp b/libraries/AP_HAL_QURT/replace.cpp index 4e9e2c6ef0003..f62258ffa33f3 100644 --- a/libraries/AP_HAL_QURT/replace.cpp +++ b/libraries/AP_HAL_QURT/replace.cpp @@ -169,33 +169,3 @@ float px4muorb_get_cpu_load(void) return 0.0f; } -extern "C" void free(void *ptr) { - return; -} - -// malloc -extern "C" void *malloc(size_t size) { - return NULL; -} - -// posix_memalign -extern "C" int posix_memalign(void **memptr, size_t alignment, size_t size) { - return 0; -} - -// calloc -extern "C" void *calloc(size_t nmemb, size_t size) { - return NULL; -} - - -// realloc -extern "C" void *realloc(void *ptr, size_t size) { - return NULL; -} - -// nanosleep -#include -extern "C" int nanosleep(const struct timespec *req, struct timespec *_Nullable rem) { - return 0; -} From 571e5bb13e0d2a25f6410ee1c229d7d1a2d33d1f Mon Sep 17 00:00:00 2001 From: Eric Katzfey Date: Thu, 23 May 2024 11:07:48 -0700 Subject: [PATCH 52/73] Added support for console printf --- libraries/AP_HAL_QURT/HAL_QURT_Class.cpp | 8 ++++---- libraries/AP_HAL_QURT/UARTDriver.cpp | 22 +++++++++++++++++++++- libraries/AP_HAL_QURT/UARTDriver.h | 6 ++++++ libraries/AP_Vehicle/AP_Vehicle.cpp | 3 +++ 4 files changed, 34 insertions(+), 5 deletions(-) diff --git a/libraries/AP_HAL_QURT/HAL_QURT_Class.cpp b/libraries/AP_HAL_QURT/HAL_QURT_Class.cpp index 278b738c14cce..47ec4d5b3ba6b 100644 --- a/libraries/AP_HAL_QURT/HAL_QURT_Class.cpp +++ b/libraries/AP_HAL_QURT/HAL_QURT_Class.cpp @@ -30,7 +30,7 @@ using namespace QURT; -static Empty::UARTDriver serial0Driver; +static UARTDriver serial0Driver("/dev/console"); static UARTDriver serial1Driver("/dev/tty-4"); static UARTDriver serial2Driver("/dev/tty-2"); @@ -87,9 +87,9 @@ void HAL_QURT::run(int argc, char* const argv[], Callbacks* callbacks) const serial0Driver.begin(115200); rcinDriver.init(); // Runs okay up to here. Next line causes a crash! - // callbacks->setup(); - // scheduler->set_system_initialized(); - // + callbacks->setup(); + scheduler->set_system_initialized(); + // for (;;) { // callbacks->loop(); // } diff --git a/libraries/AP_HAL_QURT/UARTDriver.cpp b/libraries/AP_HAL_QURT/UARTDriver.cpp index cb364d09f1c5b..a6c5457caa18a 100644 --- a/libraries/AP_HAL_QURT/UARTDriver.cpp +++ b/libraries/AP_HAL_QURT/UARTDriver.cpp @@ -2,7 +2,27 @@ #include "UARTDriver.h" #include -QURT::UARTDriver::UARTDriver(const char *name) {} +extern const AP_HAL::HAL& hal; + +QURT::UARTDriver::UARTDriver(const char *name) +{ + if (strcmp(name, "/dev/console") == 0) { + _is_console = true; + HAP_PRINTF("UART console created"); + } +} + +void QURT::UARTDriver::printf(const char *fmt, ...) +{ + if (_is_console) { + va_list ap; + char buf[300]; + va_start(ap, fmt); + vsnprintf(buf, sizeof(buf), fmt, ap); + va_end(ap); + HAP_PRINTF(buf); + } +} /* QURT implementations of virtual methods */ void QURT::UARTDriver::_begin(uint32_t b, uint16_t rxS, uint16_t txS) {} diff --git a/libraries/AP_HAL_QURT/UARTDriver.h b/libraries/AP_HAL_QURT/UARTDriver.h index 1a9fb5b6e363f..438540f67518f 100644 --- a/libraries/AP_HAL_QURT/UARTDriver.h +++ b/libraries/AP_HAL_QURT/UARTDriver.h @@ -19,6 +19,8 @@ #include "Semaphores.h" #include +#define CONSOLE_BUFFER_SIZE 64 + class QURT::UARTDriver : public AP_HAL::UARTDriver { public: UARTDriver(const char *name); @@ -29,6 +31,8 @@ class QURT::UARTDriver : public AP_HAL::UARTDriver { /* Empty implementations of Stream virtual methods */ uint32_t txspace() override; + void printf(const char *fmt, ...) override; + protected: void _begin(uint32_t b, uint16_t rxS, uint16_t txS) override; size_t _write(const uint8_t *buffer, size_t size) override; @@ -37,4 +41,6 @@ class QURT::UARTDriver : public AP_HAL::UARTDriver { void _flush() override; uint32_t _available() override; bool _discard_input() override; + bool _is_console{ false }; + char _console_buffer[CONSOLE_BUFFER_SIZE]; }; diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index b05851bdfe7f8..25a3d81b013fb 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.cpp +++ b/libraries/AP_Vehicle/AP_Vehicle.cpp @@ -398,6 +398,9 @@ void AP_Vehicle::setup() can_mgr.init(); #endif +// Crashes if we go farther! +return; + #if HAL_LOGGING_ENABLED logger.init(get_log_bitmask(), get_log_structures(), get_num_log_structures()); #endif From 3a91db91eb295384f614f67a8d287facc71c2ef7 Mon Sep 17 00:00:00 2001 From: Eric Katzfey Date: Thu, 23 May 2024 13:07:24 -0700 Subject: [PATCH 53/73] Added some commented out definitions for SIM --- libraries/AP_HAL/board/qurt.h | 26 ++++++++++++++++++++++++++ 1 file changed, 26 insertions(+) diff --git a/libraries/AP_HAL/board/qurt.h b/libraries/AP_HAL/board/qurt.h index 4e4baced78195..36d30ebbe9898 100644 --- a/libraries/AP_HAL/board/qurt.h +++ b/libraries/AP_HAL/board/qurt.h @@ -49,6 +49,32 @@ #define AP_FILESYSTEM_POSIX_HAVE_FSYNC 0 #define AP_FILESYSTEM_POSIX_HAVE_STATFS 0 +// SITL on Hardware definitions +// env SIM_ENABLED 1 +// +// #define INS_MAX_INSTANCES 2 +// #define HAL_COMPASS_MAX_SENSORS 2 +// +// #define AP_GPS_BACKEND_DEFAULT_ENABLED 0 +// #define AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED 0 +// +// #define HAL_NAVEKF2_AVAILABLE 0 +// #define EK3_FEATURE_BODY_ODOM 0 +// #define EK3_FEATURE_EXTERNAL_NAV 0 +// #define EK3_FEATURE_DRAG_FUSION 0 +// #define HAL_ADSB_ENABLED 0 +// #define HAL_PROXIMITY_ENABLED 0 +// #define HAL_VISUALODOM_ENABLED 0 +// #define HAL_GENERATOR_ENABLED 0 +// +// #define HAL_MSP_OPTICALFLOW_ENABLED 0 +// #define HAL_SUPPORT_RCOUT_SERIAL 0 +// #define HAL_HOTT_TELEM_ENABLED 0 +// #define HAL_HIGH_LATENCY2 0 +// +// #define AP_SIM_INS_FILE_ENABLED 0 +// End of SITL on Hardware definitions + #define HAL_PARAM_DEFAULTS_PATH nullptr #define HAL_BOARD_STORAGE_DIRECTORY "APM" #define HAL_BOARD_LOG_DIRECTORY "APM/logs" From 53311cbf79f1626d31576363cfd7863ad9684533 Mon Sep 17 00:00:00 2001 From: Eric Katzfey Date: Thu, 23 May 2024 16:15:21 -0700 Subject: [PATCH 54/73] Implemented thread_create in scheduler based on Linux implementation --- libraries/AP_HAL_QURT/Scheduler.cpp | 58 +++++++++++++++++++++++++++++ libraries/AP_HAL_QURT/Scheduler.h | 5 +++ 2 files changed, 63 insertions(+) diff --git a/libraries/AP_HAL_QURT/Scheduler.cpp b/libraries/AP_HAL_QURT/Scheduler.cpp index 119465c4b3dbd..e3c7372fccd0c 100644 --- a/libraries/AP_HAL_QURT/Scheduler.cpp +++ b/libraries/AP_HAL_QURT/Scheduler.cpp @@ -61,6 +61,64 @@ void Scheduler::init() pthread_create(&_io_thread_ctx, &thread_attr, &Scheduler::_io_thread, this); } +#define APM_LINUX_MAX_PRIORITY 20 +#define APM_LINUX_TIMER_PRIORITY 15 +#define APM_LINUX_UART_PRIORITY 14 +#define APM_LINUX_NET_PRIORITY 14 +#define APM_LINUX_RCIN_PRIORITY 13 +#define APM_LINUX_MAIN_PRIORITY 12 +#define APM_LINUX_IO_PRIORITY 10 +#define APM_LINUX_SCRIPTING_PRIORITY 1 +#define AP_LINUX_SENSORS_SCHED_PRIO 12 + +uint8_t Scheduler::calculate_thread_priority(priority_base base, int8_t priority) const +{ + uint8_t thread_priority = APM_LINUX_IO_PRIORITY; + static const struct { + priority_base base; + uint8_t p; + } priority_map[] = { + { PRIORITY_BOOST, APM_LINUX_MAIN_PRIORITY}, + { PRIORITY_MAIN, APM_LINUX_MAIN_PRIORITY}, + { PRIORITY_SPI, AP_LINUX_SENSORS_SCHED_PRIO}, + { PRIORITY_I2C, AP_LINUX_SENSORS_SCHED_PRIO}, + { PRIORITY_CAN, APM_LINUX_TIMER_PRIORITY}, + { PRIORITY_TIMER, APM_LINUX_TIMER_PRIORITY}, + { PRIORITY_RCIN, APM_LINUX_RCIN_PRIORITY}, + { PRIORITY_IO, APM_LINUX_IO_PRIORITY}, + { PRIORITY_UART, APM_LINUX_UART_PRIORITY}, + { PRIORITY_STORAGE, APM_LINUX_IO_PRIORITY}, + { PRIORITY_SCRIPTING, APM_LINUX_SCRIPTING_PRIORITY}, + { PRIORITY_NET, APM_LINUX_NET_PRIORITY}, + }; + for (uint8_t i=0; i Date: Sat, 25 May 2024 06:33:03 +1000 Subject: [PATCH 55/73] AP_HAL: more progress on getting through setup and into loop --- libraries/AP_HAL/board/qurt.h | 30 ++++++++++++++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/libraries/AP_HAL/board/qurt.h b/libraries/AP_HAL/board/qurt.h index 36d30ebbe9898..9c929c8956abe 100644 --- a/libraries/AP_HAL/board/qurt.h +++ b/libraries/AP_HAL/board/qurt.h @@ -45,9 +45,39 @@ #define HAL_PICCOLO_CAN_ENABLE 0 #define LUA_USE_LONGJMP 1 #define AP_MAVLINK_FTP_ENABLED 0 +#define HAL_LOGGING_MAVLINK_ENABLED 0 +#define HAL_LOGGING_FILESYSTEM_ENABLED 0 #define AP_FILESYSTEM_POSIX_HAVE_UTIME 0 #define AP_FILESYSTEM_POSIX_HAVE_FSYNC 0 #define AP_FILESYSTEM_POSIX_HAVE_STATFS 0 +// Get undefined symbols if this isn't enabled: +// _ZN7NavEKF39Log_WriteEv +// _ZNK7AP_AHRS11Write_AHRS2Ev +// _ZNK7AP_AHRS9Write_POSEv +// #define HAL_LOGGING_ENABLED 0 +// #define AP_GRIPPER_ENABLED 0 +// #define AP_AIRSPEED_ENABLED 0 +// #define AP_SRV_CHANNELS_ENABLED 0 +// #define HAL_RUNCAM_ENABLED 0 +// #define HAL_HOTT_TELEM_ENABLED 0 +// #define HAL_VISUALODOM_ENABLED 0 +// #define AP_VIDEOTX_ENABLED 0 +// #define AP_SMARTAUDIO_ENABLED 0 +// #define AP_TRAMP_ENABLED 0 +// #define AP_OPENDRONEID_ENABLED 0 +// #define HAL_EFI_ENABLED 0 +// #define AP_TEMPERATURE_SENSOR_ENABLED 0 +// #define AP_KDECAN_ENABLED 0 +// #define AP_AIS_ENABLED 0 +// #define HAL_NMEA_OUTPUT_ENABLED 0 +// #define AP_FENCE_ENABLED 0 +// #define AP_CUSTOMROTATIONS_ENABLED 0 +// #define AP_DDS_ENABLED 0 +// #define AP_CHECK_FIRMWARE_ENABLED 0 +// #define HAL_MSP_ENABLED 0 +// #define HAL_EXTERNAL_AHRS_ENABLED 0 +// #define HAL_GENERATOR_ENABLED 0 +// #define HAL_CANMANAGER_ENABLED 0 // SITL on Hardware definitions // env SIM_ENABLED 1 From dcac5ed02d6a075ea7c5c6bd48ccec7895dd84c4 Mon Sep 17 00:00:00 2001 From: Eric Katzfey Date: Sat, 25 May 2024 06:33:03 +1000 Subject: [PATCH 56/73] AP_HAL_QURT: more progress on getting through setup and into loop --- libraries/AP_HAL_QURT/HAL_QURT_Class.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_HAL_QURT/HAL_QURT_Class.cpp b/libraries/AP_HAL_QURT/HAL_QURT_Class.cpp index 47ec4d5b3ba6b..2479f9caafc8d 100644 --- a/libraries/AP_HAL_QURT/HAL_QURT_Class.cpp +++ b/libraries/AP_HAL_QURT/HAL_QURT_Class.cpp @@ -86,10 +86,10 @@ void HAL_QURT::run(int argc, char* const argv[], Callbacks* callbacks) const schedulerInstance.hal_initialized(); serial0Driver.begin(115200); rcinDriver.init(); - // Runs okay up to here. Next line causes a crash! callbacks->setup(); scheduler->set_system_initialized(); + // Crashes on the loop // for (;;) { // callbacks->loop(); // } From 933ac3349c07b1daf46db6861e1dfb57a9158380 Mon Sep 17 00:00:00 2001 From: Eric Katzfey Date: Sat, 25 May 2024 06:33:03 +1000 Subject: [PATCH 57/73] AP_Vehicle: more progress on getting through setup and into loop --- libraries/AP_Vehicle/AP_Vehicle.cpp | 37 ++++++++++++++++++++++++++--- 1 file changed, 34 insertions(+), 3 deletions(-) diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index 25a3d81b013fb..959c607adae13 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.cpp +++ b/libraries/AP_Vehicle/AP_Vehicle.cpp @@ -294,6 +294,10 @@ AP_Vehicle& vehicle = *AP_Vehicle::get_singleton(); extern AP_Vehicle& vehicle; #endif +#include +// #define GOT_HERE() do { DEV_PRINTF("got to %s:%d\n", __FILE__, __LINE__); qurt_timer_sleep(5000); } while(false); +#define GOT_HERE() + /* setup is called when the sketch starts */ @@ -398,9 +402,6 @@ void AP_Vehicle::setup() can_mgr.init(); #endif -// Crashes if we go farther! -return; - #if HAL_LOGGING_ENABLED logger.init(get_log_bitmask(), get_log_structures(), get_num_log_structures()); #endif @@ -413,27 +414,36 @@ return; // init_ardupilot is where the vehicle does most of its initialisation. init_ardupilot(); +GOT_HERE(); + #if AP_SCRIPTING_ENABLED scripting.init(); #endif // AP_SCRIPTING_ENABLED +GOT_HERE(); + #if AP_AIRSPEED_ENABLED airspeed.init(); +GOT_HERE(); if (airspeed.enabled()) { airspeed.calibrate(true); } +GOT_HERE(); #if APM_BUILD_TYPE(APM_BUILD_ArduPlane) else { +GOT_HERE(); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "No airspeed sensor"); } #endif #endif // AP_AIRSPEED_ENABLED +GOT_HERE(); #if AP_SRV_CHANNELS_ENABLED SRV_Channels::init(); #endif +GOT_HERE(); // gyro FFT needs to be initialized really late #if HAL_GYROFFT_ENABLED #if AP_SCHEDULER_ENABLED @@ -442,84 +452,105 @@ return; gyro_fft.init(1000); #endif #endif +GOT_HERE(); #if HAL_RUNCAM_ENABLED runcam.init(); #endif +GOT_HERE(); #if HAL_HOTT_TELEM_ENABLED hott_telem.init(); #endif +GOT_HERE(); #if HAL_VISUALODOM_ENABLED // init library used for visual position estimation visual_odom.init(); #endif +GOT_HERE(); #if AP_VIDEOTX_ENABLED vtx.init(); #endif +GOT_HERE(); #if AP_SMARTAUDIO_ENABLED smartaudio.init(); #endif +GOT_HERE(); #if AP_TRAMP_ENABLED tramp.init(); #endif +GOT_HERE(); #if AP_PARAM_KEY_DUMP AP_Param::show_all(hal.console, true); #endif +GOT_HERE(); send_watchdog_reset_statustext(); +GOT_HERE(); #if AP_OPENDRONEID_ENABLED opendroneid.init(); #endif // init EFI monitoring +GOT_HERE(); #if HAL_EFI_ENABLED efi.init(); #endif +GOT_HERE(); #if AP_TEMPERATURE_SENSOR_ENABLED temperature_sensor.init(); #endif +GOT_HERE(); #if AP_KDECAN_ENABLED kdecan.init(); #endif +GOT_HERE(); #if AP_AIS_ENABLED ais.init(); #endif +GOT_HERE(); #if HAL_NMEA_OUTPUT_ENABLED nmea.init(); #endif +GOT_HERE(); #if AP_FENCE_ENABLED fence.init(); #endif +GOT_HERE(); #if AP_CUSTOMROTATIONS_ENABLED custom_rotations.init(); #endif +GOT_HERE(); #if AP_FILTER_ENABLED filters.init(); #endif +GOT_HERE(); #if HAL_WITH_ESC_TELEM && HAL_GYROFFT_ENABLED for (uint8_t i = 0; i Date: Sat, 25 May 2024 06:33:03 +1000 Subject: [PATCH 58/73] ArduCopter: more progress on getting through setup and into loop --- ArduCopter/system.cpp | 95 ++++++++++++++++++++++++++++++++++++++++--- 1 file changed, 90 insertions(+), 5 deletions(-) diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index bb2d3bc2f6787..36a21808bece3 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -13,6 +13,10 @@ static void failsafe_check_static() copter.failsafe_check(); } +#include +// #define GOT_HERE() do { DEV_PRINTF("got to %s:%d\n", __FILE__, __LINE__); qurt_timer_sleep(5000); } while(false); +#define GOT_HERE() + void Copter::init_ardupilot() { // init winch @@ -29,46 +33,72 @@ void Copter::init_ardupilot() // Init RSSI rssi.init(); - - barometer.init(); + + // Any use of barometer crashes the DSP + // barometer.init(); // setup telem slots with serial ports gcs().setup_uarts(); +GOT_HERE(); + #if OSD_ENABLED == ENABLED osd.init(); #endif +GOT_HERE(); + // update motor interlock state update_using_interlock(); +GOT_HERE(); + #if FRAME_CONFIG == HELI_FRAME // trad heli specific initialisation heli_init(); #endif + +GOT_HERE(); + #if FRAME_CONFIG == HELI_FRAME input_manager.set_loop_rate(scheduler.get_loop_rate_hz()); #endif +GOT_HERE(); + init_rc_in(); // sets up rc channels from radio +GOT_HERE(); + // initialise surface to be tracked in SurfaceTracking // must be before rc init to not override initial switch position surface_tracking.init((SurfaceTracking::Surface)copter.g2.surftrak_mode.get()); +GOT_HERE(); + // allocate the motors class allocate_motors(); +GOT_HERE(); + // initialise rc channels including setting mode rc().convert_options(RC_Channel::AUX_FUNC::ARMDISARM_UNUSED, RC_Channel::AUX_FUNC::ARMDISARM_AIRMODE); +GOT_HERE(); + rc().init(); +GOT_HERE(); + // sets up motors and output to escs init_rc_out(); +GOT_HERE(); + // check if we should enter esc calibration mode esc_calibration_startup_check(); +GOT_HERE(); + // motors initialised so parameters can be sent ap.initialised_params = true; @@ -76,51 +106,77 @@ void Copter::init_ardupilot() relay.init(); #endif +GOT_HERE(); + /* * setup the 'main loop is dead' check. Note that this relies on * the RC library being initialised. */ hal.scheduler->register_timer_failsafe(failsafe_check_static, 1000); +GOT_HERE(); + // Do GPS init gps.set_log_gps_bit(MASK_LOG_GPS); +GOT_HERE(); + gps.init(); +GOT_HERE(); + AP::compass().set_log_bit(MASK_LOG_COMPASS); +GOT_HERE(); + AP::compass().init(); #if AP_AIRSPEED_ENABLED +GOT_HERE(); + airspeed.set_log_bit(MASK_LOG_IMU); #endif #if AP_OAPATHPLANNER_ENABLED +GOT_HERE(); + g2.oa.init(); #endif +GOT_HERE(); + attitude_control->parameter_sanity_check(); #if AP_OPTICALFLOW_ENABLED // initialise optical flow sensor +GOT_HERE(); + optflow.init(MASK_LOG_OPTFLOW); #endif // AP_OPTICALFLOW_ENABLED #if HAL_MOUNT_ENABLED // initialise camera mount +GOT_HERE(); + camera_mount.init(); #endif #if AP_CAMERA_ENABLED // initialise camera +GOT_HERE(); + camera.init(); #endif #if AC_PRECLAND_ENABLED // initialise precision landing +GOT_HERE(); + init_precland(); #endif #if AP_LANDINGGEAR_ENABLED // initialise landing gear position +GOT_HERE(); + landinggear.init(); #endif @@ -130,67 +186,96 @@ void Copter::init_ardupilot() // read Baro pressure at ground //----------------------------- - barometer.set_log_baro_bit(MASK_LOG_IMU); - barometer.calibrate(); +GOT_HERE(); + + // Any use of barometer crashes the DSP + // barometer.set_log_baro_bit(MASK_LOG_IMU); + // barometer.calibrate(); #if RANGEFINDER_ENABLED == ENABLED // initialise rangefinder +GOT_HERE(); + init_rangefinder(); #endif #if HAL_PROXIMITY_ENABLED // init proximity sensor +GOT_HERE(); + g2.proximity.init(); #endif #if AP_BEACON_ENABLED // init beacons used for non-gps position estimation +GOT_HERE(); + g2.beacon.init(); #endif #if AP_RPM_ENABLED +GOT_HERE(); + // initialise AP_RPM library rpm_sensor.init(); #endif #if MODE_AUTO_ENABLED == ENABLED +GOT_HERE(); + // initialise mission library mode_auto.mission.init(); #endif #if MODE_SMARTRTL_ENABLED == ENABLED +GOT_HERE(); + // initialize SmartRTL g2.smart_rtl.init(); #endif #if HAL_LOGGING_ENABLED +GOT_HERE(); + // initialise AP_Logger library logger.setVehicle_Startup_Writer(FUNCTOR_BIND(&copter, &Copter::Log_Write_Vehicle_Startup_Messages, void)); #endif - startup_INS_ground(); +GOT_HERE(); + + // Get an error message here and then crashes + // startup_INS_ground(); #if AC_CUSTOMCONTROL_MULTI_ENABLED == ENABLED +GOT_HERE(); + custom_control.init(); #endif // set landed flags set_land_complete(true); +GOT_HERE(); + set_land_complete_maybe(true); +GOT_HERE(); // enable CPU failsafe failsafe_enable(); +GOT_HERE(); ins.set_log_raw_bit(MASK_LOG_IMU_RAW); +GOT_HERE(); motors->output_min(); // output lowest possible value to motors // attempt to set the initial_mode, else set to STABILIZE +GOT_HERE(); if (!set_mode((enum Mode::Number)g.initial_mode.get(), ModeReason::INITIALISED)) { // set mode to STABILIZE will trigger mode change notification to pilot set_mode(Mode::Number::STABILIZE, ModeReason::UNAVAILABLE); } +GOT_HERE(); pos_variance_filt.set_cutoff_frequency(g2.fs_ekf_filt_hz); vel_variance_filt.set_cutoff_frequency(g2.fs_ekf_filt_hz); hgt_variance_filt.set_cutoff_frequency(g2.fs_ekf_filt_hz); From 1e032052ee2a93a26f69391d878b944baa87366e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 25 May 2024 08:32:23 +1000 Subject: [PATCH 59/73] Copter: start baro --- ArduCopter/system.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 36a21808bece3..900d48974357f 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -35,7 +35,7 @@ void Copter::init_ardupilot() rssi.init(); // Any use of barometer crashes the DSP - // barometer.init(); + barometer.init(); // setup telem slots with serial ports gcs().setup_uarts(); From ffedf8c375d100f2fbb8db338a792d73177f2812 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 25 May 2024 08:32:44 +1000 Subject: [PATCH 60/73] HAL_QURT: fixed thread create --- libraries/AP_HAL_QURT/HAL_QURT_Class.cpp | 38 ++++- libraries/AP_HAL_QURT/HAL_QURT_Class.h | 2 + libraries/AP_HAL_QURT/Scheduler.cpp | 32 ++-- libraries/AP_HAL_QURT/Thread.cpp | 184 +++++++++++++++++++++++ libraries/AP_HAL_QURT/Thread.h | 96 ++++++++++++ libraries/AP_HAL_QURT/replace.cpp | 2 + 6 files changed, 336 insertions(+), 18 deletions(-) create mode 100644 libraries/AP_HAL_QURT/Thread.cpp create mode 100644 libraries/AP_HAL_QURT/Thread.h diff --git a/libraries/AP_HAL_QURT/HAL_QURT_Class.cpp b/libraries/AP_HAL_QURT/HAL_QURT_Class.cpp index 2479f9caafc8d..2de71998c1077 100644 --- a/libraries/AP_HAL_QURT/HAL_QURT_Class.cpp +++ b/libraries/AP_HAL_QURT/HAL_QURT_Class.cpp @@ -75,6 +75,32 @@ HAL_QURT::HAL_QURT() : { } + +static HAL_QURT::Callbacks *_callbacks; + +void HAL_QURT::main_thread(void) +{ + HAP_PRINTF("In main_thread!"); + rcinDriver.init(); + _callbacks->setup(); + scheduler->set_system_initialized(); + + HAP_PRINTF("starting loop"); + + for (;;) { + _callbacks->loop(); + } +} + +void HAL_QURT::start_main_thread(Callbacks* callbacks) +{ + _callbacks = callbacks; + scheduler->thread_create(FUNCTOR_BIND_MEMBER(&HAL_QURT::main_thread, void), "main_thread", + 32768, + AP_HAL::Scheduler::PRIORITY_MAIN, + 0); +} + void HAL_QURT::run(int argc, char* const argv[], Callbacks* callbacks) const { assert(callbacks); @@ -85,14 +111,10 @@ void HAL_QURT::run(int argc, char* const argv[], Callbacks* callbacks) const scheduler->init(); schedulerInstance.hal_initialized(); serial0Driver.begin(115200); - rcinDriver.init(); - callbacks->setup(); - scheduler->set_system_initialized(); - - // Crashes on the loop - // for (;;) { - // callbacks->loop(); - // } + + HAP_PRINTF("Creating thread!"); + + const_cast(this)->start_main_thread(callbacks); } const AP_HAL::HAL& AP_HAL::get_HAL() { diff --git a/libraries/AP_HAL_QURT/HAL_QURT_Class.h b/libraries/AP_HAL_QURT/HAL_QURT_Class.h index 7c1a1d42b7622..4980f03f25c37 100644 --- a/libraries/AP_HAL_QURT/HAL_QURT_Class.h +++ b/libraries/AP_HAL_QURT/HAL_QURT_Class.h @@ -22,4 +22,6 @@ class HAL_QURT : public AP_HAL::HAL { public: HAL_QURT(); void run(int argc, char* const* argv, Callbacks* callbacks) const override; + void start_main_thread(Callbacks* callbacks); + void main_thread(void); }; diff --git a/libraries/AP_HAL_QURT/Scheduler.cpp b/libraries/AP_HAL_QURT/Scheduler.cpp index e3c7372fccd0c..30a93d0b9efeb 100644 --- a/libraries/AP_HAL_QURT/Scheduler.cpp +++ b/libraries/AP_HAL_QURT/Scheduler.cpp @@ -17,6 +17,7 @@ #include "Storage.h" #include "RCOutput.h" #include +#include "Thread.h" using namespace QURT; @@ -101,22 +102,33 @@ uint8_t Scheduler::calculate_thread_priority(priority_base base, int8_t priority return thread_priority; } -bool Scheduler::thread_create(AP_HAL::MemberProc proc, const char *name, - uint32_t stack_size, priority_base base, int8_t priority) +/* + create a new thread +*/ +bool Scheduler::thread_create(AP_HAL::MemberProc proc, const char *name, uint32_t stack_size, priority_base base, int8_t priority) { - pthread_attr_t thread_attr; - struct sched_param param; + Thread *thread = new Thread{(Thread::task_t)proc}; + if (!thread) { + return false; + } - pthread_attr_init(&thread_attr); - pthread_attr_setstacksize(&thread_attr, (1024 * 256) + stack_size); + const uint8_t thread_priority = calculate_thread_priority(base, priority); - param.sched_priority = calculate_thread_priority(base, priority); + // Add 10k to HAL-independent requested stack size + thread->set_stack_size(10 * 1024 + stack_size); - (void)pthread_attr_setschedparam(&thread_attr, ¶m); + /* + * We should probably store the thread handlers and join() when exiting, + * but let's the thread manage itself for now. + */ + thread->set_auto_free(true); - pthread_create(&_timer_thread_ctx, &thread_attr, &Scheduler::_timer_thread, this); + if (!thread->start(name, SCHED_FIFO, thread_priority)) { + delete thread; + return false; + } - return true; + return true; } void Scheduler::delay_microseconds(uint16_t usec) diff --git a/libraries/AP_HAL_QURT/Thread.cpp b/libraries/AP_HAL_QURT/Thread.cpp new file mode 100644 index 0000000000000..79aeddc868af9 --- /dev/null +++ b/libraries/AP_HAL_QURT/Thread.cpp @@ -0,0 +1,184 @@ +/* + * Copyright (C) 2016 Intel Corporation. All rights reserved. + * + * This file 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 file 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 "Thread.h" + +#include +#include +#include +#include +#include + +#include +#include +#include "Scheduler.h" + +extern const AP_HAL::HAL &hal; + +namespace QURT { + + +void *Thread::_run_trampoline(void *arg) +{ + Thread *thread = static_cast(arg); + thread->_run(); + + if (thread->_auto_free) { + delete thread; + } + + return nullptr; +} + +bool Thread::_run() +{ + if (!_task) { + return false; + } + + _task(); + + return true; +} + +size_t Thread::get_stack_usage() +{ + return 0; +} + +bool Thread::start(const char *name, int policy, int prio) +{ + if (_started) { + return false; + } + + struct sched_param param = { .sched_priority = prio }; + pthread_attr_t attr; + int r; + + pthread_attr_init(&attr); + + /* + we need to run as root to get realtime scheduling. Allow it to + run as non-root for debugging purposes, plus to allow the Replay + tool to run + */ + if ((r = pthread_attr_setschedparam(&attr, ¶m)) != 0) { + AP_HAL::panic("Failed to set attributes for thread '%s': %s", + name, strerror(r)); + } + + if (_stack_size) { + if (pthread_attr_setstacksize(&attr, _stack_size) != 0) { + return false; + } + } + + r = pthread_create(&_ctx, &attr, &Thread::_run_trampoline, this); + if (r != 0) { + AP_HAL::panic("Failed to create thread '%s': %s", + name, strerror(r)); + } + pthread_attr_destroy(&attr); + + _started = true; + + return true; +} + +bool Thread::is_current_thread() +{ + return pthread_equal(pthread_self(), _ctx); +} + +bool Thread::join() +{ + void *ret; + + if (_ctx == 0) { + return false; + } + + if (pthread_join(_ctx, &ret) != 0 || + (intptr_t)ret != 0) { + return false; + } + + return true; +} + + +bool PeriodicThread::set_rate(uint32_t rate_hz) +{ + if (_started || rate_hz == 0) { + return false; + } + + _period_usec = hz_to_usec(rate_hz); + + return true; +} + +bool Thread::set_stack_size(size_t stack_size) +{ + if (_started) { + return false; + } + + _stack_size = MAX(stack_size, (size_t) PTHREAD_STACK_MIN); + + return true; +} + +bool PeriodicThread::_run() +{ + if (_period_usec == 0) { + return false; + } + + uint64_t next_run_usec = AP_HAL::micros64() + _period_usec; + + while (!_should_exit) { + uint64_t dt = next_run_usec - AP_HAL::micros64(); + if (dt > _period_usec) { + // we've lost sync - restart + next_run_usec = AP_HAL::micros64(); + } else { + qurt_timer_sleep(dt); + } + next_run_usec += _period_usec; + + _task(); + } + + _started = false; + _should_exit = false; + + return true; +} + +bool PeriodicThread::stop() +{ + if (!is_started()) { + return false; + } + + _should_exit = true; + + return true; +} + +} diff --git a/libraries/AP_HAL_QURT/Thread.h b/libraries/AP_HAL_QURT/Thread.h new file mode 100644 index 0000000000000..9d351bdab3539 --- /dev/null +++ b/libraries/AP_HAL_QURT/Thread.h @@ -0,0 +1,96 @@ +/* + * Copyright (C) 2016 Intel Corporation. All rights reserved. + * + * This file 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 file 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 +#include + +#include + +namespace QURT { + +/* + * Interface abstracting threads + */ +class Thread { +public: + FUNCTOR_TYPEDEF(task_t, void); + + Thread(task_t t) : _task(t) { } + + virtual ~Thread() { } + + bool start(const char *name, int policy, int prio); + + bool is_current_thread(); + + bool is_started() const { return _started; } + + size_t get_stack_usage(); + + bool set_stack_size(size_t stack_size); + + void set_auto_free(bool auto_free) { _auto_free = auto_free; } + + virtual bool stop() { return false; } + + bool join(); + +protected: + static void *_run_trampoline(void *arg); + + /* + * Run the task assigned in the constructor. May be overriden in case it's + * preferred to use Thread as an interface or when user wants to aggregate + * some initialization or teardown for the thread. + */ + virtual bool _run(); + + void _poison_stack(); + + task_t _task; + bool _started = false; + bool _should_exit = false; + bool _auto_free = false; + pthread_t _ctx = 0; + + struct stack_debug { + uint32_t *start; + uint32_t *end; + } _stack_debug; + + size_t _stack_size = 0; +}; + +class PeriodicThread : public Thread { +public: + PeriodicThread(Thread::task_t t) + : Thread(t) + { } + + bool set_rate(uint32_t rate_hz); + + bool stop() override; + +protected: + bool _run() override; + + uint64_t _period_usec = 0; +}; + +} diff --git a/libraries/AP_HAL_QURT/replace.cpp b/libraries/AP_HAL_QURT/replace.cpp index f62258ffa33f3..f5ca818f83be6 100644 --- a/libraries/AP_HAL_QURT/replace.cpp +++ b/libraries/AP_HAL_QURT/replace.cpp @@ -141,6 +141,8 @@ int px4muorb_orb_initialize(fc_func_ptrs *func_ptrs, int32_t clock_offset_us) qurt_arducopter_main(0, NULL); + HAP_PRINTF("qurt_arducopter_main RETURNED"); + return 0; } From 3bc67f42a86522fa0f84b8627481dcea66ddf86d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 25 May 2024 10:40:06 +1000 Subject: [PATCH 61/73] HAL_QURT: fixed interface fns --- libraries/AP_HAL_QURT/interface.h | 59 ++++++++++++++++++------------- libraries/AP_HAL_QURT/replace.cpp | 9 +++-- 2 files changed, 41 insertions(+), 27 deletions(-) diff --git a/libraries/AP_HAL_QURT/interface.h b/libraries/AP_HAL_QURT/interface.h index cb557543ac309..be24146de1d63 100644 --- a/libraries/AP_HAL_QURT/interface.h +++ b/libraries/AP_HAL_QURT/interface.h @@ -6,36 +6,45 @@ typedef unsigned int speed_t; // TODO: This has to be defined in the slpi_proc build and in the PX4 build. // Make it accessible from one file to both builds. typedef struct { - int (*advertise_func_ptr)(const char *topic_name); - int (*subscribe_func_ptr)(const char *topic_name); - int (*unsubscribe_func_ptr)(const char *topic_name); - int (*topic_data_func_ptr)(const char *name, const uint8_t *data, int data_len_in_bytes); - // device::SPI::_config_spi_bus_func_t config_spi_bus; - // device::SPI::_spi_transfer_func_t spi_transfer; - int (*_config_spi_bus_func_t)(); - int (*_spi_transfer_func_t)(int, const uint8_t *, uint8_t *, const unsigned); - // device::I2C::_config_i2c_bus_func_t config_i2c_bus; - // device::I2C::_set_i2c_address_func_t set_i2c_address; - // device::I2C::_i2c_transfer_func_t i2c_transfer; - int (*_config_i2c_bus_func_t)(uint8_t, uint8_t, uint32_t); - int (*_set_i2c_address_func_t)(int, uint8_t); - int (*_i2c_transfer_func_t)(int, const uint8_t *, const unsigned, uint8_t *, const unsigned); - // open_uart_func_t open_uart_func; - // write_uart_func_t write_uart_func; - // read_uart_func_t read_uart_func; - int (*open_uart_func_t)(uint8_t, speed_t); - int (*write_uart_func_t)(int, const void *, size_t); - int (*read_uart_func_t)(int, void *, size_t); - int (*register_interrupt_callback)(int (*)(int, void *, void *), void *arg); -} fc_func_ptrs; + int (*advertise_func_ptr)(const char *topic_name); + int (*subscribe_func_ptr)(const char *topic_name); + int (*unsubscribe_func_ptr)(const char *topic_name); + int (*topic_data_func_ptr)(const char *name, const uint8_t *data, int data_len_in_bytes); + // device::SPI::_config_spi_bus_func_t config_spi_bus; + // device::SPI::_spi_transfer_func_t spi_transfer; + int (*_config_spi_bus_func_t)(); + int (*_spi_transfer_func_t)(int, const uint8_t *, uint8_t *, const unsigned); + + /* + configure an I2C device, returns a file descriptor + */ + int (*_config_i2c_bus_func_t)(uint8_t busnum, uint8_t i2c_address, uint32_t frequency); + + /* + change I2C address + */ + int (*_set_i2c_address_func_t)(int fd, uint8_t i2c_address); + + /* + I2C transfer, returns 0 on success + */ + int (*_i2c_transfer_func_t)(int fd, const uint8_t *send_data, const unsigned send_len, uint8_t *recv_data, const unsigned recv_len); + + // open_uart_func_t open_uart_func; + // write_uart_func_t write_uart_func; + // read_uart_func_t read_uart_func; + int (*open_uart_func_t)(uint8_t, speed_t); + int (*write_uart_func_t)(int, const void *, size_t); + int (*read_uart_func_t)(int, void *, size_t); + int (*register_interrupt_callback)(int (*)(int, void *, void *), void *arg); +} qurt_func_ptrs_t; #ifndef __cplusplus #error "C++ should be defined!!!" #endif extern "C" { - - int px4muorb_orb_initialize(fc_func_ptrs *func_ptrs, int32_t clock_offset_us) __EXPORT; + int px4muorb_orb_initialize(qurt_func_ptrs_t *func_ptrs, int32_t clock_offset_us) __EXPORT; int px4muorb_topic_advertised(const char *name) __EXPORT; @@ -47,4 +56,4 @@ extern "C" { float px4muorb_get_cpu_load(void) __EXPORT; -} \ No newline at end of file +} diff --git a/libraries/AP_HAL_QURT/replace.cpp b/libraries/AP_HAL_QURT/replace.cpp index f5ca818f83be6..72db6e65c9ce1 100644 --- a/libraries/AP_HAL_QURT/replace.cpp +++ b/libraries/AP_HAL_QURT/replace.cpp @@ -10,6 +10,8 @@ #include "replace.h" #include "interface.h" +extern qurt_func_ptrs_t qurt_func_ptrs; + extern "C" { // this is not declared in qurt headers @@ -135,10 +137,13 @@ int ArduPilot_main(int argc, const char *argv[]) extern "C" int qurt_arducopter_main(int argc, char* const argv[]); -int px4muorb_orb_initialize(fc_func_ptrs *func_ptrs, int32_t clock_offset_us) +int px4muorb_orb_initialize(qurt_func_ptrs_t *func_ptrs, int32_t clock_offset_us) { HAP_PRINTF("About to call qurt_arducopter_main %p", &qurt_arducopter_main); - + + qurt_func_ptrs = *func_ptrs; + HAP_PRINTF("qurt_func_ptrs->_config_i2c_bus_func_t=%p", qurt_func_ptrs._config_i2c_bus_func_t); + qurt_arducopter_main(0, NULL); HAP_PRINTF("qurt_arducopter_main RETURNED"); From 285d8b17ed7c4fb914256e223e9dc5147c5448fc Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 25 May 2024 11:23:52 +1000 Subject: [PATCH 62/73] waf: wrap printf on QURT --- Tools/ardupilotwaf/boards.py | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/ardupilotwaf/boards.py b/Tools/ardupilotwaf/boards.py index b3971c568755e..4c4b0c8dff098 100644 --- a/Tools/ardupilotwaf/boards.py +++ b/Tools/ardupilotwaf/boards.py @@ -1705,6 +1705,7 @@ def configure_env(self, cfg, env): "--wrap=free", "--wrap=realloc", "--wrap=memalign", + "--wrap=printf", "--wrap=__stack_chk_fail", "-lc" ] From a7ae56ec9c2a6221e37be78ea8fe79db3a0ba6e5 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 25 May 2024 11:24:11 +1000 Subject: [PATCH 63/73] AP_HAL: added baro for QURT --- libraries/AP_HAL/board/qurt.h | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/libraries/AP_HAL/board/qurt.h b/libraries/AP_HAL/board/qurt.h index 9c929c8956abe..4bb98514bdb1a 100644 --- a/libraries/AP_HAL/board/qurt.h +++ b/libraries/AP_HAL/board/qurt.h @@ -112,6 +112,12 @@ #define USE_LIBC_REALLOC 1 +/* + barometer list + */ +#define PROBE_BARO_I2C(driver, bus, addr, args ...) ADD_BACKEND(AP_Baro_ ## driver::probe(*this,std::move(GET_I2C_DEVICE(bus, addr)),##args)) +#define HAL_BARO_PROBE_LIST PROBE_BARO_I2C(ICP101XX, 3, 0x63) + /* bring in missing standard library functions From ec9b25a0f10158c3c1ea45fe526efc0aa706298d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 25 May 2024 11:24:38 +1000 Subject: [PATCH 64/73] HAL_QURT: initial I2C implementation --- libraries/AP_HAL_QURT/DeviceBus.cpp | 135 +++++++++++++++++++++ libraries/AP_HAL_QURT/DeviceBus.h | 55 +++++++++ libraries/AP_HAL_QURT/HAL_QURT_Class.cpp | 10 +- libraries/AP_HAL_QURT/HAL_QURT_Class.h | 1 + libraries/AP_HAL_QURT/I2CDevice.cpp | 143 +++++++++++++++++++++++ libraries/AP_HAL_QURT/I2CDevice.h | 134 +++++++++++++++++++++ libraries/AP_HAL_QURT/Semaphores.cpp | 17 ++- libraries/AP_HAL_QURT/Semaphores.h | 2 + libraries/AP_HAL_QURT/Thread.cpp | 1 + libraries/AP_HAL_QURT/UARTDriver.cpp | 3 +- libraries/AP_HAL_QURT/replace.cpp | 13 +++ libraries/AP_HAL_QURT/replace.h | 2 + libraries/AP_HAL_QURT/system.cpp | 1 + 13 files changed, 511 insertions(+), 6 deletions(-) create mode 100644 libraries/AP_HAL_QURT/DeviceBus.cpp create mode 100644 libraries/AP_HAL_QURT/DeviceBus.h create mode 100644 libraries/AP_HAL_QURT/I2CDevice.cpp create mode 100644 libraries/AP_HAL_QURT/I2CDevice.h diff --git a/libraries/AP_HAL_QURT/DeviceBus.cpp b/libraries/AP_HAL_QURT/DeviceBus.cpp new file mode 100644 index 0000000000000..e53c2380ebff0 --- /dev/null +++ b/libraries/AP_HAL_QURT/DeviceBus.cpp @@ -0,0 +1,135 @@ +/* + * This file 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 file 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 "DeviceBus.h" + +#include +#include +#include + +#include "Scheduler.h" +#include "Semaphores.h" + +using namespace QURT; + +extern const AP_HAL::HAL& hal; + +DeviceBus::DeviceBus(AP_HAL::Scheduler::priority_base _thread_priority) : + thread_priority(_thread_priority), semaphore() +{ +} + +/* + per-bus callback thread +*/ +void DeviceBus::bus_thread(void) +{ + while (true) { + uint64_t now = AP_HAL::micros64(); + DeviceBus::callback_info *callback; + + // find a callback to run + for (callback = callbacks; callback; callback = callback->next) { + if (now >= callback->next_usec) { + while (now >= callback->next_usec) { + callback->next_usec += callback->period_usec; + } + // call it with semaphore held + if (semaphore.take(HAL_SEMAPHORE_BLOCK_FOREVER)) { + callback->cb(); + semaphore.give(); + } + } + } + + // work out when next loop is needed + uint64_t next_needed = 0; + now = AP_HAL::micros64(); + + for (callback = callbacks; callback; callback = callback->next) { + if (next_needed == 0 || + callback->next_usec < next_needed) { + next_needed = callback->next_usec; + if (next_needed < now) { + next_needed = now; + } + } + } + + // delay for at most 50ms, to handle newly added callbacks + uint32_t delay = 50000; + if (next_needed >= now && next_needed - now < delay) { + delay = next_needed - now; + } + // don't delay for less than 100usec, so one thread doesn't + // completely dominate the CPU + if (delay < 100) { + delay = 100; + } + hal.scheduler->delay_microseconds(delay); + } + return; +} + +AP_HAL::Device::PeriodicHandle DeviceBus::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb, AP_HAL::Device *_hal_device) +{ + if (!thread_started) { + thread_started = true; + hal_device = _hal_device; + // setup a name for the thread + char name[30]; + switch (hal_device->bus_type()) { + case AP_HAL::Device::BUS_TYPE_I2C: + snprintf(name, sizeof(name), "APM_I2C:%u", + hal_device->bus_num()); + break; + + case AP_HAL::Device::BUS_TYPE_SPI: + snprintf(name, sizeof(name), "APM_SPI:%u", + hal_device->bus_num()); + break; + default: + break; + } +#ifdef BUSDEBUG + printf("%s:%d Thread Start\n", __PRETTY_FUNCTION__, __LINE__); +#endif + hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&DeviceBus::bus_thread, void), + name, HAL_QURT_DEVICE_STACK_SIZE, thread_priority, 0); + } + DeviceBus::callback_info *callback = new DeviceBus::callback_info; + if (callback == nullptr) { + return nullptr; + } + callback->cb = cb; + callback->period_usec = period_usec; + callback->next_usec = AP_HAL::micros64() + period_usec; + + // add to linked list of callbacks on thread + callback->next = callbacks; + callbacks = callback; + + return callback; +} + +/* + * Adjust the timer for the next call: it needs to be called from the bus + * thread, otherwise it will race with it + */ +bool DeviceBus::adjust_timer(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec) +{ + // TODO: add timer adjustment + return true; +} diff --git a/libraries/AP_HAL_QURT/DeviceBus.h b/libraries/AP_HAL_QURT/DeviceBus.h new file mode 100644 index 0000000000000..59be3fb68d268 --- /dev/null +++ b/libraries/AP_HAL_QURT/DeviceBus.h @@ -0,0 +1,55 @@ +/* + * This file 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 file 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 +#include "Semaphores.h" +#include "AP_HAL_QURT.h" +#include "Scheduler.h" + +#define HAL_QURT_DEVICE_STACK_SIZE 32768 + +namespace QURT +{ + +class DeviceBus +{ +public: + DeviceBus(AP_HAL::Scheduler::priority_base _thread_priority); + + struct DeviceBus *next; + HAL_Semaphore semaphore; + + AP_HAL::Device::PeriodicHandle register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb, AP_HAL::Device *hal_device); + bool adjust_timer(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec); + void bus_thread(void); + +private: + struct callback_info { + struct callback_info *next; + AP_HAL::Device::PeriodicCb cb; + uint32_t period_usec; + uint64_t next_usec; + } *callbacks; + AP_HAL::Scheduler::priority_base thread_priority; + bool thread_started; + AP_HAL::Device *hal_device; +}; + +} + + diff --git a/libraries/AP_HAL_QURT/HAL_QURT_Class.cpp b/libraries/AP_HAL_QURT/HAL_QURT_Class.cpp index 2de71998c1077..b462cb449f034 100644 --- a/libraries/AP_HAL_QURT/HAL_QURT_Class.cpp +++ b/libraries/AP_HAL_QURT/HAL_QURT_Class.cpp @@ -23,6 +23,7 @@ #include "Semaphores.h" #include "RCInput.h" #include "RCOutput.h" +#include "I2CDevice.h" #include #include #include @@ -36,13 +37,13 @@ static UARTDriver serial2Driver("/dev/tty-2"); static Empty::SPIDeviceManager spiDeviceManager; static Empty::AnalogIn analogIn; -static Storage storageDriver; +static Empty::Storage storageDriver; static Empty::GPIO gpioDriver; static Empty::RCInput rcinDriver; static RCOutput rcoutDriver; static Util utilInstance; static Scheduler schedulerInstance; -static Empty::I2CDeviceManager i2c_mgr_instance; +static I2CDeviceManager i2c_mgr_instance; bool qurt_ran_overtime; @@ -76,6 +77,9 @@ HAL_QURT::HAL_QURT() : } +// QURT interface pointers +qurt_func_ptrs_t qurt_func_ptrs {}; + static HAL_QURT::Callbacks *_callbacks; void HAL_QURT::main_thread(void) @@ -96,7 +100,7 @@ void HAL_QURT::start_main_thread(Callbacks* callbacks) { _callbacks = callbacks; scheduler->thread_create(FUNCTOR_BIND_MEMBER(&HAL_QURT::main_thread, void), "main_thread", - 32768, + 500000, AP_HAL::Scheduler::PRIORITY_MAIN, 0); } diff --git a/libraries/AP_HAL_QURT/HAL_QURT_Class.h b/libraries/AP_HAL_QURT/HAL_QURT_Class.h index 4980f03f25c37..28c8868d5c297 100644 --- a/libraries/AP_HAL_QURT/HAL_QURT_Class.h +++ b/libraries/AP_HAL_QURT/HAL_QURT_Class.h @@ -17,6 +17,7 @@ #include #include "AP_HAL_QURT_Namespace.h" +#include "interface.h" class HAL_QURT : public AP_HAL::HAL { public: diff --git a/libraries/AP_HAL_QURT/I2CDevice.cpp b/libraries/AP_HAL_QURT/I2CDevice.cpp new file mode 100644 index 0000000000000..2e9cdc1410ba2 --- /dev/null +++ b/libraries/AP_HAL_QURT/I2CDevice.cpp @@ -0,0 +1,143 @@ +/* + * This file 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 file 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 "I2CDevice.h" + +#include +#include +#include +#include "Scheduler.h" + +extern qurt_func_ptrs_t qurt_func_ptrs; + +using namespace QURT; + +/* + 4 I2C buses + + bus1: mag + bus2: power manager + bus4: external spare bus + bus5: barometer (internal) +*/ +static uint8_t i2c_bus_ids[] = { 1, 2, 4, 5 }; + +static uint32_t i2c_internal_mask = (1U<<3); + +I2CBus I2CDeviceManager::businfo[ARRAY_SIZE(i2c_bus_ids)]; + +I2CDeviceManager::I2CDeviceManager(void) +{ + /* + open all the buses with a default speed + */ + HAP_PRINTF("In I2CDeviceManager constructor"); +} + +I2CDevice::I2CDevice(uint8_t busnum, uint8_t address, uint32_t bus_clock, bool use_smbus, uint32_t timeout_ms) : + _retries(10), + _address(address), + bus(I2CDeviceManager::businfo[busnum]) +{ + HAP_PRINTF("Constructing I2CDevice %u 0x%02x %u", unsigned(busnum), unsigned(address), unsigned(bus_clock)); + qurt_timer_sleep(5000); + + if (bus.fd == -2) { + HAP_PRINTF("Calling _config_i2c_bus_func_t %u", + unsigned(i2c_bus_ids[busnum])); qurt_timer_sleep(5000); + bus.fd = qurt_func_ptrs._config_i2c_bus_func_t(i2c_bus_ids[busnum], address, bus_clock/1000); + HAP_PRINTF("Opened I2C bus %u -> %d", unsigned(busnum), bus.fd); + } + set_device_bus(busnum); + set_device_address(address); + asprintf(&pname, "I2C:%u:%02x", + (unsigned)busnum, (unsigned)address); +} + +I2CDevice::~I2CDevice() +{ + free(pname); +} + +bool I2CDevice::transfer(const uint8_t *send, uint32_t send_len, + uint8_t *recv, uint32_t recv_len) +{ + if (bus.fd <= 0) { + return false; + } + if (!bus.semaphore.check_owner()) { + HAP_PRINTF("I2C: not owner of 0x%x", (unsigned)get_bus_id()); + return false; + } + if (bus.last_address != _address) { + HAP_PRINTF("I2C: changing address for fd %d to 0x%02x", bus.fd, _address); + qurt_func_ptrs._set_i2c_address_func_t(bus.fd, _address); + bus.last_address = _address; + } + return qurt_func_ptrs._i2c_transfer_func_t(bus.fd, send, send_len, recv, recv_len) == 0; +} + +/* + register a periodic callback +*/ +AP_HAL::Device::PeriodicHandle I2CDevice::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb) +{ + return bus.register_periodic_callback(period_usec, cb, this); +} + + +/* + adjust a periodic callback +*/ +bool I2CDevice::adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec) +{ + return bus.adjust_timer(h, period_usec); +} + +AP_HAL::OwnPtr +I2CDeviceManager::get_device(uint8_t bus, uint8_t address, + uint32_t bus_clock, + bool use_smbus, + uint32_t timeout_ms) +{ + if (bus >= ARRAY_SIZE(i2c_bus_ids)) { + return AP_HAL::OwnPtr(nullptr); + } + auto dev = AP_HAL::OwnPtr(new I2CDevice(bus, address, bus_clock, use_smbus, timeout_ms)); + return dev; +} + +/* + get mask of bus numbers for all configured I2C buses +*/ +uint32_t I2CDeviceManager::get_bus_mask(void) const +{ + return ((1U << ARRAY_SIZE(i2c_bus_ids)) - 1); +} + +/* + get mask of bus numbers for all configured internal I2C buses +*/ +uint32_t I2CDeviceManager::get_bus_mask_internal(void) const +{ + return i2c_internal_mask; +} + +/* + get mask of bus numbers for all configured external I2C buses +*/ +uint32_t I2CDeviceManager::get_bus_mask_external(void) const +{ + return get_bus_mask() & ~get_bus_mask_internal(); +} diff --git a/libraries/AP_HAL_QURT/I2CDevice.h b/libraries/AP_HAL_QURT/I2CDevice.h new file mode 100644 index 0000000000000..b72200926304c --- /dev/null +++ b/libraries/AP_HAL_QURT/I2CDevice.h @@ -0,0 +1,134 @@ +/* + * This file 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 file 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 +#include +#include + +#include "Semaphores.h" +#include "Scheduler.h" +#include "DeviceBus.h" + +namespace QURT +{ + +class I2CBus : public DeviceBus +{ +public: + I2CBus():DeviceBus(AP_HAL::Scheduler::PRIORITY_I2C) {}; + uint32_t bus_clock; + int fd = -2; + uint8_t last_address; +}; + +class I2CDevice : public AP_HAL::I2CDevice +{ +public: + static I2CDevice *from(AP_HAL::I2CDevice *dev) + { + return static_cast(dev); + } + + I2CDevice(uint8_t bus, uint8_t address, uint32_t bus_clock, bool use_smbus, uint32_t timeout_ms); + ~I2CDevice(); + + /* See AP_HAL::I2CDevice::set_address() */ + void set_address(uint8_t address) override + { + _address = address; + } + + /* See AP_HAL::I2CDevice::set_retries() */ + void set_retries(uint8_t retries) override + { + _retries = retries; + } + + /* See AP_HAL::Device::set_speed(): Empty implementation, not supported. */ + bool set_speed(enum Device::Speed speed) override + { + return true; + } + + /* See AP_HAL::Device::transfer() */ + bool transfer(const uint8_t *send, uint32_t send_len, + uint8_t *recv, uint32_t recv_len) override; + + bool read_registers_multiple(uint8_t first_reg, uint8_t *recv, + uint32_t recv_len, uint8_t times) override + { + return false; + }; + + /* See AP_HAL::Device::register_periodic_callback() */ + AP_HAL::Device::PeriodicHandle register_periodic_callback( + uint32_t period_usec, AP_HAL::Device::PeriodicCb) override; + + /* See AP_HAL::Device::adjust_periodic_callback() */ + bool adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec) override; + + AP_HAL::Semaphore* get_semaphore() override //TODO check all + { + // if asking for invalid bus number use bus 0 semaphore + return &bus.semaphore; + } + +protected: + I2CBus &bus; + uint8_t _retries; + uint8_t _address; + char *pname; + +}; + +class I2CDeviceManager : public AP_HAL::I2CDeviceManager +{ +public: + friend class I2CDevice; + + static I2CBus businfo[]; + + // constructor + I2CDeviceManager(); + + static I2CDeviceManager *from(AP_HAL::I2CDeviceManager *i2c_mgr) + { + return static_cast(i2c_mgr); + } + + AP_HAL::OwnPtr get_device(uint8_t bus, uint8_t address, + uint32_t bus_clock=100000, + bool use_smbus = false, + uint32_t timeout_ms=4) override; + + /* + get mask of bus numbers for all configured I2C buses + */ + uint32_t get_bus_mask(void) const override; + + /* + get mask of bus numbers for all configured external I2C buses + */ + uint32_t get_bus_mask_external(void) const override; + + /* + get mask of bus numbers for all configured internal I2C buses + */ + uint32_t get_bus_mask_internal(void) const override; +}; +} diff --git a/libraries/AP_HAL_QURT/Semaphores.cpp b/libraries/AP_HAL_QURT/Semaphores.cpp index f6e3154f73611..1c7c280e0a0bb 100644 --- a/libraries/AP_HAL_QURT/Semaphores.cpp +++ b/libraries/AP_HAL_QURT/Semaphores.cpp @@ -20,10 +20,19 @@ bool Semaphore::give() return pthread_mutex_unlock(&_lock) == 0; } +bool Semaphore::check_owner() +{ + return owner == pthread_self(); +} + bool Semaphore::take(uint32_t timeout_ms) { if (timeout_ms == HAL_SEMAPHORE_BLOCK_FOREVER) { - return pthread_mutex_lock(&_lock) == 0; + auto ok = pthread_mutex_lock(&_lock) == 0; + if (ok) { + owner = pthread_self(); + } + return ok; } if (take_nonblocking()) { return true; @@ -40,7 +49,11 @@ bool Semaphore::take(uint32_t timeout_ms) bool Semaphore::take_nonblocking() { - return pthread_mutex_trylock(&_lock) == 0; + const auto ok = pthread_mutex_trylock(&_lock) == 0; + if (ok) { + owner = pthread_self(); + } + return ok; } /* diff --git a/libraries/AP_HAL_QURT/Semaphores.h b/libraries/AP_HAL_QURT/Semaphores.h index 1b64bd0734540..e017641d89672 100644 --- a/libraries/AP_HAL_QURT/Semaphores.h +++ b/libraries/AP_HAL_QURT/Semaphores.h @@ -14,9 +14,11 @@ class Semaphore : public AP_HAL::Semaphore { bool give() override; bool take(uint32_t timeout_ms) override; bool take_nonblocking() override; + bool check_owner(void); protected: // qurt_mutex_t _lock; pthread_mutex_t _lock; + pthread_t owner; }; diff --git a/libraries/AP_HAL_QURT/Thread.cpp b/libraries/AP_HAL_QURT/Thread.cpp index 79aeddc868af9..1e4af72e1fc9a 100644 --- a/libraries/AP_HAL_QURT/Thread.cpp +++ b/libraries/AP_HAL_QURT/Thread.cpp @@ -82,6 +82,7 @@ bool Thread::start(const char *name, int policy, int prio) } if (_stack_size) { + HAP_PRINTF("Using stack_size=%u", unsigned(_stack_size)); if (pthread_attr_setstacksize(&attr, _stack_size) != 0) { return false; } diff --git a/libraries/AP_HAL_QURT/UARTDriver.cpp b/libraries/AP_HAL_QURT/UARTDriver.cpp index a6c5457caa18a..d3845ccc1fcbb 100644 --- a/libraries/AP_HAL_QURT/UARTDriver.cpp +++ b/libraries/AP_HAL_QURT/UARTDriver.cpp @@ -20,7 +20,8 @@ void QURT::UARTDriver::printf(const char *fmt, ...) va_start(ap, fmt); vsnprintf(buf, sizeof(buf), fmt, ap); va_end(ap); - HAP_PRINTF(buf); + HAP_PRINTF(buf); + qurt_timer_sleep(5000); } } diff --git a/libraries/AP_HAL_QURT/replace.cpp b/libraries/AP_HAL_QURT/replace.cpp index 72db6e65c9ce1..343a8ae9388cf 100644 --- a/libraries/AP_HAL_QURT/replace.cpp +++ b/libraries/AP_HAL_QURT/replace.cpp @@ -176,3 +176,16 @@ float px4muorb_get_cpu_load(void) return 0.0f; } +int __wrap_printf(const char *fmt, ...) +{ + va_list ap; + + char buf[300]; + va_start(ap, fmt); + vsnprintf(buf, sizeof(buf), fmt, ap); + va_end(ap); + HAP_PRINTF(buf); + qurt_timer_sleep(5000); + + return 0; +} diff --git a/libraries/AP_HAL_QURT/replace.h b/libraries/AP_HAL_QURT/replace.h index 6d6f2d6ccb7f1..33d6c83b4b5b3 100644 --- a/libraries/AP_HAL_QURT/replace.h +++ b/libraries/AP_HAL_QURT/replace.h @@ -31,6 +31,8 @@ extern "C" { void HAP_printf(const char *file, int line, const char *fmt, ...); + int __wrap_printf(const char *fmt, ...); + #ifdef __cplusplus } #endif diff --git a/libraries/AP_HAL_QURT/system.cpp b/libraries/AP_HAL_QURT/system.cpp index 65b8b919c7728..bff78c8c0b756 100644 --- a/libraries/AP_HAL_QURT/system.cpp +++ b/libraries/AP_HAL_QURT/system.cpp @@ -32,6 +32,7 @@ void panic(const char *errormsg, ...) vsnprintf(buf, sizeof(buf), errormsg, ap); va_end(ap); HAP_PRINTF(buf); + qurt_timer_sleep(100000); exit(1); } From 88191f60a583bf9bcf3217b783622f99b3957452 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 25 May 2024 13:40:35 +1000 Subject: [PATCH 65/73] HAL_QURT: added SPI support --- libraries/AP_HAL_QURT/DeviceBus.h | 2 +- libraries/AP_HAL_QURT/HAL_QURT_Class.cpp | 3 +- libraries/AP_HAL_QURT/SPIDevice.cpp | 139 +++++++++++++++++++++++ libraries/AP_HAL_QURT/SPIDevice.h | 65 +++++++++++ libraries/AP_HAL_QURT/Scheduler.cpp | 52 ++++----- libraries/AP_HAL_QURT/Thread.cpp | 3 +- libraries/AP_HAL_QURT/interface.h | 15 ++- 7 files changed, 244 insertions(+), 35 deletions(-) create mode 100644 libraries/AP_HAL_QURT/SPIDevice.cpp create mode 100644 libraries/AP_HAL_QURT/SPIDevice.h diff --git a/libraries/AP_HAL_QURT/DeviceBus.h b/libraries/AP_HAL_QURT/DeviceBus.h index 59be3fb68d268..d62db9ccf2ded 100644 --- a/libraries/AP_HAL_QURT/DeviceBus.h +++ b/libraries/AP_HAL_QURT/DeviceBus.h @@ -21,7 +21,7 @@ #include "AP_HAL_QURT.h" #include "Scheduler.h" -#define HAL_QURT_DEVICE_STACK_SIZE 32768 +#define HAL_QURT_DEVICE_STACK_SIZE 8192 namespace QURT { diff --git a/libraries/AP_HAL_QURT/HAL_QURT_Class.cpp b/libraries/AP_HAL_QURT/HAL_QURT_Class.cpp index b462cb449f034..d01e194fcb47f 100644 --- a/libraries/AP_HAL_QURT/HAL_QURT_Class.cpp +++ b/libraries/AP_HAL_QURT/HAL_QURT_Class.cpp @@ -24,6 +24,7 @@ #include "RCInput.h" #include "RCOutput.h" #include "I2CDevice.h" +#include "SPIDevice.h" #include #include #include @@ -35,7 +36,7 @@ static UARTDriver serial0Driver("/dev/console"); static UARTDriver serial1Driver("/dev/tty-4"); static UARTDriver serial2Driver("/dev/tty-2"); -static Empty::SPIDeviceManager spiDeviceManager; +static SPIDeviceManager spiDeviceManager; static Empty::AnalogIn analogIn; static Empty::Storage storageDriver; static Empty::GPIO gpioDriver; diff --git a/libraries/AP_HAL_QURT/SPIDevice.cpp b/libraries/AP_HAL_QURT/SPIDevice.cpp new file mode 100644 index 0000000000000..2ba6ebaee025e --- /dev/null +++ b/libraries/AP_HAL_QURT/SPIDevice.cpp @@ -0,0 +1,139 @@ +/* + * This file 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 file 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 "SPIDevice.h" + +#include +#include +#include +#include "Scheduler.h" +#include "Semaphores.h" +#include + +using namespace QURT; + +#define MHZ (1000U*1000U) +#define KHZ (1000U) + +const char *device_names[] = {"INV1", "INV2", "INV3"}; + +extern qurt_func_ptrs_t qurt_func_ptrs; + +static SPIBus *spi_bus; + +SPIBus::SPIBus(void): + DeviceBus(Scheduler::PRIORITY_SPI) +{ + fd = qurt_func_ptrs._config_spi_bus_func_t(); + HAP_PRINTF("Created SPI bus -> %d", fd); +} + +SPIDevice::SPIDevice(const char *name, SPIBus &_bus) + : bus(_bus) +{ + set_device_bus(0); + set_device_address(1); + set_speed(AP_HAL::Device::SPEED_LOW); + + pname = name; +} + +SPIDevice::~SPIDevice() +{ +} + +bool SPIDevice::set_speed(AP_HAL::Device::Speed _speed) +{ + return true; +} + +bool SPIDevice::transfer(const uint8_t *send, uint32_t send_len, + uint8_t *recv, uint32_t recv_len) +{ + //HAP_PRINTF("SPI transfer %p %u %p %u [%02x %02x]", send, send_len, recv, recv_len, send?send[0]:1, recv?recv[0]:1); + if (!send || !recv) { + // simplest cases + return transfer_fullduplex(send, recv, recv_len?recv_len:send_len); + } + uint8_t buf[send_len+recv_len]; + if (send_len > 0) { + memcpy(buf, send, send_len); + } + if (recv_len > 0) { + memset(&buf[send_len], 0, recv_len); + } + transfer_fullduplex(buf, buf, send_len+recv_len); + if (recv_len > 0) { + memcpy(recv, &buf[send_len], recv_len); + } + return true; +} + +bool SPIDevice::transfer_fullduplex(const uint8_t *send, uint8_t *recv, uint32_t len) +{ + uint8_t dummy[len]; + memset(dummy, 0xFF, sizeof(dummy)); + if (send == nullptr) { + send = dummy; + } + if (recv == nullptr) { + recv = dummy; + } + int ret = qurt_func_ptrs._spi_transfer_func_t(bus.fd, send, recv, len); + return ret == 0; +} + +void SPIDevice::acquire_bus(bool accuire) +{ +} + +AP_HAL::Semaphore *SPIDevice::get_semaphore() +{ + return &bus.semaphore; +} + +AP_HAL::Device::PeriodicHandle SPIDevice::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb) +{ + return bus.register_periodic_callback(period_usec, cb, this); +} + +bool SPIDevice::adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec) +{ + return bus.adjust_timer(h, period_usec); +} + +AP_HAL::OwnPtr +SPIDeviceManager::get_device(const char *name) +{ + uint8_t i; + for (i = 0; i(nullptr); + } + + if (spi_bus == nullptr) { + spi_bus = new SPIBus(); + } + + HAP_PRINTF("*&*&*&*&* got SPI name %s", name); + + return AP_HAL::OwnPtr(new SPIDevice(name, *spi_bus)); +} + diff --git a/libraries/AP_HAL_QURT/SPIDevice.h b/libraries/AP_HAL_QURT/SPIDevice.h new file mode 100644 index 0000000000000..65a55858204e0 --- /dev/null +++ b/libraries/AP_HAL_QURT/SPIDevice.h @@ -0,0 +1,65 @@ +/* + * This file 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 file 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 +#include +#include "AP_HAL_QURT.h" + +#include "Semaphores.h" +#include "Scheduler.h" +#include "DeviceBus.h" + +namespace QURT +{ + +class SPIBus : public DeviceBus +{ +public: + SPIBus(); + int fd = -1; +}; + +class SPIDevice : public AP_HAL::SPIDevice +{ +public: + SPIDevice(const char *name, SPIBus &bus); + virtual ~SPIDevice(); + + bool set_speed(AP_HAL::Device::Speed speed) override; + bool transfer(const uint8_t *send, uint32_t send_len, + uint8_t *recv, uint32_t recv_len) override; + bool transfer_fullduplex(const uint8_t *send, uint8_t *recv, uint32_t len) override; + AP_HAL::Semaphore *get_semaphore() override; + AP_HAL::Device::PeriodicHandle register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb) override; + bool adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec) override; + +private: + SPIBus &bus; + const char *pname; + void acquire_bus(bool accuire); +}; + +class SPIDeviceManager : public AP_HAL::SPIDeviceManager +{ +public: + friend class SPIDevice; + + AP_HAL::OwnPtr get_device(const char *name) override; +}; +} + diff --git a/libraries/AP_HAL_QURT/Scheduler.cpp b/libraries/AP_HAL_QURT/Scheduler.cpp index 30a93d0b9efeb..8a5ef4ec1334c 100644 --- a/libraries/AP_HAL_QURT/Scheduler.cpp +++ b/libraries/AP_HAL_QURT/Scheduler.cpp @@ -36,7 +36,7 @@ void Scheduler::init() struct sched_param param; pthread_attr_init(&thread_attr); - pthread_attr_setstacksize(&thread_attr, 40960); + pthread_attr_setstacksize(&thread_attr, 16000); param.sched_priority = APM_TIMER_PRIORITY; (void)pthread_attr_setschedparam(&thread_attr, ¶m); @@ -45,7 +45,7 @@ void Scheduler::init() // the UART thread runs at a medium priority pthread_attr_init(&thread_attr); - pthread_attr_setstacksize(&thread_attr, 40960); + pthread_attr_setstacksize(&thread_attr, 16000); param.sched_priority = APM_UART_PRIORITY; (void)pthread_attr_setschedparam(&thread_attr, ¶m); @@ -54,7 +54,7 @@ void Scheduler::init() // the IO thread runs at lower priority pthread_attr_init(&thread_attr); - pthread_attr_setstacksize(&thread_attr, 40960); + pthread_attr_setstacksize(&thread_attr, 16000); param.sched_priority = APM_IO_PRIORITY; (void)pthread_attr_setschedparam(&thread_attr, ¶m); @@ -62,39 +62,39 @@ void Scheduler::init() pthread_create(&_io_thread_ctx, &thread_attr, &Scheduler::_io_thread, this); } -#define APM_LINUX_MAX_PRIORITY 20 -#define APM_LINUX_TIMER_PRIORITY 15 -#define APM_LINUX_UART_PRIORITY 14 -#define APM_LINUX_NET_PRIORITY 14 -#define APM_LINUX_RCIN_PRIORITY 13 -#define APM_LINUX_MAIN_PRIORITY 12 -#define APM_LINUX_IO_PRIORITY 10 -#define APM_LINUX_SCRIPTING_PRIORITY 1 -#define AP_LINUX_SENSORS_SCHED_PRIO 12 +#define APM_QURT_MAX_PRIORITY 20 +#define APM_QURT_TIMER_PRIORITY 15 +#define APM_QURT_UART_PRIORITY 14 +#define APM_QURT_NET_PRIORITY 14 +#define APM_QURT_RCIN_PRIORITY 13 +#define APM_QURT_MAIN_PRIORITY 12 +#define APM_QURT_IO_PRIORITY 10 +#define APM_QURT_SCRIPTING_PRIORITY 1 +#define AP_QURT_SENSORS_SCHED_PRIO 12 uint8_t Scheduler::calculate_thread_priority(priority_base base, int8_t priority) const { - uint8_t thread_priority = APM_LINUX_IO_PRIORITY; + uint8_t thread_priority = APM_QURT_IO_PRIORITY; static const struct { priority_base base; uint8_t p; } priority_map[] = { - { PRIORITY_BOOST, APM_LINUX_MAIN_PRIORITY}, - { PRIORITY_MAIN, APM_LINUX_MAIN_PRIORITY}, - { PRIORITY_SPI, AP_LINUX_SENSORS_SCHED_PRIO}, - { PRIORITY_I2C, AP_LINUX_SENSORS_SCHED_PRIO}, - { PRIORITY_CAN, APM_LINUX_TIMER_PRIORITY}, - { PRIORITY_TIMER, APM_LINUX_TIMER_PRIORITY}, - { PRIORITY_RCIN, APM_LINUX_RCIN_PRIORITY}, - { PRIORITY_IO, APM_LINUX_IO_PRIORITY}, - { PRIORITY_UART, APM_LINUX_UART_PRIORITY}, - { PRIORITY_STORAGE, APM_LINUX_IO_PRIORITY}, - { PRIORITY_SCRIPTING, APM_LINUX_SCRIPTING_PRIORITY}, - { PRIORITY_NET, APM_LINUX_NET_PRIORITY}, + { PRIORITY_BOOST, APM_QURT_MAIN_PRIORITY}, + { PRIORITY_MAIN, APM_QURT_MAIN_PRIORITY}, + { PRIORITY_SPI, AP_QURT_SENSORS_SCHED_PRIO+1}, + { PRIORITY_I2C, AP_QURT_SENSORS_SCHED_PRIO}, + { PRIORITY_CAN, APM_QURT_TIMER_PRIORITY}, + { PRIORITY_TIMER, APM_QURT_TIMER_PRIORITY}, + { PRIORITY_RCIN, APM_QURT_RCIN_PRIORITY}, + { PRIORITY_IO, APM_QURT_IO_PRIORITY}, + { PRIORITY_UART, APM_QURT_UART_PRIORITY}, + { PRIORITY_STORAGE, APM_QURT_IO_PRIORITY}, + { PRIORITY_SCRIPTING, APM_QURT_SCRIPTING_PRIORITY}, + { PRIORITY_NET, APM_QURT_NET_PRIORITY}, }; for (uint8_t i=0; i Date: Sat, 25 May 2024 13:41:13 +1000 Subject: [PATCH 66/73] AP_HAL: added SPI probe list for QURT --- libraries/AP_HAL/board/qurt.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/libraries/AP_HAL/board/qurt.h b/libraries/AP_HAL/board/qurt.h index 4bb98514bdb1a..a2adebee0c18b 100644 --- a/libraries/AP_HAL/board/qurt.h +++ b/libraries/AP_HAL/board/qurt.h @@ -118,6 +118,11 @@ #define PROBE_BARO_I2C(driver, bus, addr, args ...) ADD_BACKEND(AP_Baro_ ## driver::probe(*this,std::move(GET_I2C_DEVICE(bus, addr)),##args)) #define HAL_BARO_PROBE_LIST PROBE_BARO_I2C(ICP101XX, 3, 0x63) +/* + IMU list + */ +#define PROBE_IMU_SPI(driver, devname, args ...) ADD_BACKEND(AP_InertialSensor_ ## driver::probe(*this,hal.spi->get_device(devname),##args)) +#define HAL_INS_PROBE_LIST PROBE_IMU_SPI(Invensensev3, "INV3", ROTATION_NONE) /* bring in missing standard library functions From a12ef9c665f17de9d52784d622e0f2317579c349 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 25 May 2024 13:42:44 +1000 Subject: [PATCH 67/73] Copter: QURT hacks --- ArduCopter/system.cpp | 34 +++++++++++++++++++++++++++------- 1 file changed, 27 insertions(+), 7 deletions(-) diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 900d48974357f..7532fa6108496 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -15,28 +15,44 @@ static void failsafe_check_static() #include // #define GOT_HERE() do { DEV_PRINTF("got to %s:%d\n", __FILE__, __LINE__); qurt_timer_sleep(5000); } while(false); -#define GOT_HERE() +#define GOT_HERE() do {} while(0) + void Copter::init_ardupilot() { + GOT_HERE(); + // init winch #if AP_WINCH_ENABLED g2.winch.init(); #endif + GOT_HERE(); + // initialise notify system notify.init(); + + GOT_HERE(); + notify_flight_mode(); + GOT_HERE(); + // initialise battery monitor battery.init(); + GOT_HERE(); + // Init RSSI rssi.init(); + GOT_HERE(); + // Any use of barometer crashes the DSP barometer.init(); + GOT_HERE(); + // setup telem slots with serial ports gcs().setup_uarts(); @@ -189,8 +205,8 @@ GOT_HERE(); GOT_HERE(); // Any use of barometer crashes the DSP - // barometer.set_log_baro_bit(MASK_LOG_IMU); - // barometer.calibrate(); + barometer.set_log_baro_bit(MASK_LOG_IMU); + barometer.calibrate(); #if RANGEFINDER_ENABLED == ENABLED // initialise rangefinder @@ -241,13 +257,12 @@ GOT_HERE(); logger.setVehicle_Startup_Writer(FUNCTOR_BIND(&copter, &Copter::Log_Write_Vehicle_Startup_Messages, void)); #endif -GOT_HERE(); + GOT_HERE(); - // Get an error message here and then crashes - // startup_INS_ground(); + startup_INS_ground(); #if AC_CUSTOMCONTROL_MULTI_ENABLED == ENABLED -GOT_HERE(); + GOT_HERE(); custom_control.init(); #endif @@ -294,11 +309,16 @@ void Copter::startup_INS_ground() ahrs.init(); ahrs.set_vehicle_class(AP_AHRS::VehicleClass::COPTER); + GOT_HERE(); + // Warm up and calibrate gyro offsets ins.init(scheduler.get_loop_rate_hz()); + GOT_HERE(); + // reset ahrs including gyro bias ahrs.reset(); + GOT_HERE(); } // position_ok - returns true if the horizontal absolute position is ok and home position is set From 2e17e917a8ec053b2409c55315edcac99c66399c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 25 May 2024 13:42:56 +1000 Subject: [PATCH 68/73] AP_AHRS: QURT hacks --- libraries/AP_AHRS/AP_AHRS_DCM.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index b26bdeb472714..74e3543297cbd 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -34,6 +34,9 @@ #include #include +#include +#define GOT_HERE() do { DEV_PRINTF("got to %s:%d\n", __FILE__, __LINE__); qurt_timer_sleep(5000); } while(false); + extern const AP_HAL::HAL& hal; // this is the speed in m/s above which we first get a yaw lock with @@ -177,12 +180,15 @@ void AP_AHRS_DCM::matrix_update(void) void AP_AHRS_DCM::reset(bool recover_eulers) { + GOT_HERE(); + // reset the integration terms _omega_I.zero(); _omega_P.zero(); _omega_yaw_P.zero(); _omega.zero(); + GOT_HERE(); // if the caller wants us to try to recover to the current // attitude then calculate the dcm matrix from the current // roll/pitch/yaw values @@ -205,14 +211,6 @@ AP_AHRS_DCM::reset(bool recover_eulers) // Get body frame accel vector Vector3f initAccVec = _ins.get_accel(); - uint8_t counter = 0; - - // the first vector may be invalid as the filter starts up - while ((initAccVec.length() < 9.0f || initAccVec.length() > 11) && counter++ < 20) { - _ins.wait_for_sample(); - _ins.update(); - initAccVec = _ins.get_accel(); - } // normalise the acceleration vector if (initAccVec.length() > 5.0f) { @@ -228,12 +226,14 @@ AP_AHRS_DCM::reset(bool recover_eulers) _dcm_matrix.from_euler(roll, pitch, 0); } + GOT_HERE(); // pre-calculate some trig for CPU purposes: _cos_yaw = cosf(yaw); _sin_yaw = sinf(yaw); _last_startup_ms = AP_HAL::millis(); + GOT_HERE(); } /* From ea1f908f87dea19caae378b1ff2d41906c3a367c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 25 May 2024 13:43:07 +1000 Subject: [PATCH 69/73] AP_BoardConfig: QURT hacks --- libraries/AP_BoardConfig/AP_BoardConfig.cpp | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/libraries/AP_BoardConfig/AP_BoardConfig.cpp b/libraries/AP_BoardConfig/AP_BoardConfig.cpp index e60b8e04b1479..a7632e2a0ab27 100644 --- a/libraries/AP_BoardConfig/AP_BoardConfig.cpp +++ b/libraries/AP_BoardConfig/AP_BoardConfig.cpp @@ -439,6 +439,23 @@ bool AP_BoardConfig::_in_error_loop; void AP_BoardConfig::throw_error(const char *err_type, const char *fmt, va_list arg) { + HAP_PRINTF("throw_error: %s", err_type); + qurt_timer_sleep(5000); + uint32_t last_ms = AP_HAL::millis(); + uint32_t last_us = AP_HAL::micros(); + while (true) { + qurt_timer_sleep(1000000); + uint32_t now_ms = AP_HAL::millis(); + uint32_t now_us = AP_HAL::micros(); + uint32_t dt_ms = now_ms - last_ms; + uint32_t dt_us = now_us - last_us; + HAP_PRINTF("tick! %u %u dt_ms=%u dt_us=%u\n", + unsigned(now_ms), unsigned(now_us), + unsigned(dt_ms), unsigned(dt_us)); + last_ms = now_ms; + last_us = now_us; + } + _in_error_loop = true; /* to give the user the opportunity to connect to USB we keep From 098b5815710825e87441d208c1e71ca5dedba237 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 25 May 2024 13:43:46 +1000 Subject: [PATCH 70/73] AP_InertialSensor: disable gyro cal HACK --- libraries/AP_InertialSensor/AP_InertialSensor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index bf62d4cdfced3..816fbeff5164d 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -392,7 +392,7 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = { // @Description: Conrols when automatic gyro calibration is performed // @Values: 0:Never, 1:Start-up only // @User: Advanced - AP_GROUPINFO("_GYR_CAL", 24, AP_InertialSensor, _gyro_cal_timing, 1), + AP_GROUPINFO("_GYR_CAL", 24, AP_InertialSensor, _gyro_cal_timing, 0), // @Param: _TRIM_OPTION // @DisplayName: Accel cal trim option From dae73e68b8ed1f81c6d5a2d6e25549b2f114a720 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 25 May 2024 14:16:32 +1000 Subject: [PATCH 71/73] waf: fixed QURT link --- Tools/ardupilotwaf/boards.py | 16 ++++------------ 1 file changed, 4 insertions(+), 12 deletions(-) diff --git a/Tools/ardupilotwaf/boards.py b/Tools/ardupilotwaf/boards.py index 4c4b0c8dff098..32b3818f124d5 100644 --- a/Tools/ardupilotwaf/boards.py +++ b/Tools/ardupilotwaf/boards.py @@ -1661,6 +1661,10 @@ def configure_toolchain(self, cfg): cfg.env.STLIBPATH_ST = '-L%s' cfg.env.STLIB_MARKER = '' cfg.env.STLIB_ST = '-l%s' + cfg.env.LDFLAGS = [ + '-lgcc', + cfg.env.TOOLCHAIN_DIR + '/target/hexagon/lib/v66/G0/pic/finiS.o' + ] def configure_env(self, cfg, env): super(QURT, self).configure_env(cfg, env) @@ -1678,18 +1682,6 @@ def configure_env(self, cfg, env): AP_SIM_ENABLED = 0, ) -# The linking has to be done very precisely for QURT. Below is an example of a -# linker run_str that works correctly. This needs to be modified in the waflib/Tools/cxx.py -# file in the waf submodule - -# class cxxprogram(link_task): -# "Links object files into c++ programs" -# # run_str = '${LINK_CXX} ${LINKFLAGS} ${CXXLNK_SRC_F}${SRC} ${CXXLNK_TGT_F}${TGT[0].relpath()} ${RPATH_ST:RPATH} ${FRAMEWORKPATH_ST:FRAMEWORKPATH} ${FRAMEWORK_ST:FRAMEWORK} ${ARCH_ST:ARCH} ${STLIB_MARKER} ${STLIBPATH_ST:STLIBPATH} ${STLIB_ST:STLIB} ${SHLIB_MARKER} ${LIBPATH_ST:LIBPATH} ${LIB_ST:LIB} ${LDFLAGS}' -# run_str = '${LINK_CXX} ${CXXLNK_TGT_F} ${TGT[0].relpath()} ${LINKFLAGS} ${STLIBPATH_ST:STLIBPATH} --start-group --whole-archive ${CXXLNK_SRC_F}${SRC} ${STLIB_ST:STLIB} --end-group --start-group -lgcc --end-group /opt/hexagon-sdk/4.1.0.4-lite/tools/HEXAGON_Tools/8.4.05/Tools/target/hexagon/lib/v66/G0/pic/finiS.o' -# vars = ['LINKDEPS'] -# ext_out = ['.bin'] -# inst_to = '${BINDIR}' - env.LINKFLAGS = [ "-march=hexagon", "-mcpu=hexagonv66", From 04e5c08f758cb660593df0b6283652adde3c56f8 Mon Sep 17 00:00:00 2001 From: Eric Katzfey Date: Tue, 4 Jun 2024 09:58:30 -0700 Subject: [PATCH 72/73] AP_Common: Added overridden new and delete for QURT HAL to allow use of larger heap --- libraries/AP_Common/c++.cpp | 78 +++++++++++++++++++++++++++++++++++++ 1 file changed, 78 insertions(+) diff --git a/libraries/AP_Common/c++.cpp b/libraries/AP_Common/c++.cpp index b9804818e85cb..7c34976e2722a 100644 --- a/libraries/AP_Common/c++.cpp +++ b/libraries/AP_Common/c++.cpp @@ -7,6 +7,82 @@ #include #include + +#if CONFIG_HAL_BOARD == HAL_BOARD_QURT + +#include + +/* + These are the heap access function exported by the SLPI DSP image for Qurt OS. +*/ +extern "C" { + void *fc_heap_alloc(size_t size); + void fc_heap_free(void* ptr); + size_t fc_heap_size(void); + size_t fc_heap_usage(void); +} + +void *operator new (size_t size, std::nothrow_t const ¬hrow) noexcept +{ + if (size < 1) { + size = 1; + } + + return (fc_heap_alloc(size)); +} + +void *operator new[](size_t size, std::nothrow_t const ¬hrow) noexcept +{ + if (size < 1) { + size = 1; + } + + return (fc_heap_alloc(size)); +} + +/* + These variants are for new without std::nothrow. We don't want to ever + use these from ardupilot code + */ +void *operator new (size_t size) +{ + if (size < 1) { + size = 1; + } + + return (fc_heap_alloc(size)); +} + + +void *operator new[](size_t size) +{ + if (size < 1) { + size = 1; + } + + return (fc_heap_alloc(size)); +} + +/* + Override delete to free up memory to correct heap +*/ + +// extern const AP_HAL::HAL& hal; + +void operator delete (void *p) noexcept +{ + if (p) { fc_heap_free(p); } + + // DEV_PRINTF("Heap size: %u, heap usage: %u", fc_heap_size(), fc_heap_usage()); +} + +void operator delete[](void *ptr) noexcept +{ + if (ptr) { fc_heap_free(ptr); } +} + +#else + /* globally override new and delete to ensure that we always start with zero memory. This ensures consistent behaviour. @@ -36,3 +112,5 @@ void operator delete[](void * ptr) { if (ptr) free(ptr); } + +#endif From 1b00e03994b774cf524bae93fc884aa57c82f612 Mon Sep 17 00:00:00 2001 From: Eric Katzfey Date: Tue, 4 Jun 2024 10:12:46 -0700 Subject: [PATCH 73/73] AP_Common: Fixed formatting in c++.cpp --- libraries/AP_Common/c++.cpp | 56 +++++++++++++++++++++---------------- 1 file changed, 32 insertions(+), 24 deletions(-) diff --git a/libraries/AP_Common/c++.cpp b/libraries/AP_Common/c++.cpp index 7c34976e2722a..9e282504992ee 100644 --- a/libraries/AP_Common/c++.cpp +++ b/libraries/AP_Common/c++.cpp @@ -24,20 +24,20 @@ extern "C" { void *operator new (size_t size, std::nothrow_t const ¬hrow) noexcept { - if (size < 1) { - size = 1; - } + if (size < 1) { + size = 1; + } - return (fc_heap_alloc(size)); + return (fc_heap_alloc(size)); } void *operator new[](size_t size, std::nothrow_t const ¬hrow) noexcept { - if (size < 1) { - size = 1; - } + if (size < 1) { + size = 1; + } - return (fc_heap_alloc(size)); + return (fc_heap_alloc(size)); } /* @@ -46,21 +46,21 @@ void *operator new[](size_t size, std::nothrow_t const ¬hrow) noexcept */ void *operator new (size_t size) { - if (size < 1) { - size = 1; - } + if (size < 1) { + size = 1; + } - return (fc_heap_alloc(size)); + return (fc_heap_alloc(size)); } void *operator new[](size_t size) { - if (size < 1) { - size = 1; - } + if (size < 1) { + size = 1; + } - return (fc_heap_alloc(size)); + return (fc_heap_alloc(size)); } /* @@ -71,14 +71,18 @@ void *operator new[](size_t size) void operator delete (void *p) noexcept { - if (p) { fc_heap_free(p); } + if (p) { + fc_heap_free(p); + } // DEV_PRINTF("Heap size: %u, heap usage: %u", fc_heap_size(), fc_heap_usage()); } void operator delete[](void *ptr) noexcept { - if (ptr) { fc_heap_free(ptr); } + if (ptr) { + fc_heap_free(ptr); + } } #else @@ -87,17 +91,19 @@ void operator delete[](void *ptr) noexcept globally override new and delete to ensure that we always start with zero memory. This ensures consistent behaviour. */ -void * operator new(size_t size) +void * operator new (size_t size) { if (size < 1) { size = 1; } - return(calloc(size, 1)); + return (calloc(size, 1)); } -void operator delete(void *p) +void operator delete (void *p) { - if (p) free(p); + if (p) { + free(p); + } } void * operator new[](size_t size) @@ -105,12 +111,14 @@ void * operator new[](size_t size) if (size < 1) { size = 1; } - return(calloc(size, 1)); + return (calloc(size, 1)); } void operator delete[](void * ptr) { - if (ptr) free(ptr); + if (ptr) { + free(ptr); + } } #endif