diff --git a/libraries/AP_HAL_QURT/AP_HAL_QURT.h b/libraries/AP_HAL_QURT/AP_HAL_QURT.h deleted file mode 100644 index 2c753c203d..0000000000 --- a/libraries/AP_HAL_QURT/AP_HAL_QURT.h +++ /dev/null @@ -1,25 +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 - -/* 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 deleted file mode 100644 index a38bfd959c..0000000000 --- a/libraries/AP_HAL_QURT/AP_HAL_QURT_Main.h +++ /dev/null @@ -1,15 +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 diff --git a/libraries/AP_HAL_QURT/AP_HAL_QURT_Namespace.h b/libraries/AP_HAL_QURT/AP_HAL_QURT_Namespace.h deleted file mode 100644 index 279c588e04..0000000000 --- a/libraries/AP_HAL_QURT/AP_HAL_QURT_Namespace.h +++ /dev/null @@ -1,29 +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 - -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 deleted file mode 100644 index 67e7413b24..0000000000 --- a/libraries/AP_HAL_QURT/AP_HAL_QURT_Private.h +++ /dev/null @@ -1,23 +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 - -/* 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 deleted file mode 100644 index ee34b1d33c..0000000000 --- a/libraries/AP_HAL_QURT/HAL_QURT_Class.cpp +++ /dev/null @@ -1,144 +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 "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 deleted file mode 100644 index 7c1a1d42b7..0000000000 --- a/libraries/AP_HAL_QURT/HAL_QURT_Class.h +++ /dev/null @@ -1,25 +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 - -#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 deleted file mode 100644 index bcf7d8ac46..0000000000 --- a/libraries/AP_HAL_QURT/RCInput.cpp +++ /dev/null @@ -1,184 +0,0 @@ -#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 deleted file mode 100644 index 7ebddd79e5..0000000000 --- a/libraries/AP_HAL_QURT/RCInput.h +++ /dev/null @@ -1,52 +0,0 @@ -#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 deleted file mode 100644 index d7876a5559..0000000000 --- a/libraries/AP_HAL_QURT/RCOutput.cpp +++ /dev/null @@ -1,126 +0,0 @@ -#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 deleted file mode 100644 index bbef8eb7e3..0000000000 --- a/libraries/AP_HAL_QURT/RCOutput.h +++ /dev/null @@ -1,44 +0,0 @@ -#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/README.md b/libraries/AP_HAL_QURT/README.md deleted file mode 100644 index 34b8a07122..0000000000 --- a/libraries/AP_HAL_QURT/README.md +++ /dev/null @@ -1,138 +0,0 @@ -# ArduPilot on Qualcomm Flight - -This is a port of ArduPilot to the Qualcomm Flight development board: - - http://shop.intrinsyc.com/products/snapdragon-flight-dev-kit - -This board is interesting because it is small but offers a lot of CPU -power and two on-board cameras. - -The board has 4 'Krait' ARM cores which run Linux (by default Ubuntu -14.04 Trusty), plus 3 'Hexagon' DSP cores which run the QURT RTOS. - -There are two ports of ArduPilot to this board. One is called -'HAL_QURT' and runs primarily on the DSPs, with just a small shim on -the ARM cores. The other is a HAL_Linux subtype called 'QFLIGHT' which -runs mostly on the ARM cores, with just sensor and UARTs on the DSPs. - -This is the readme for the QURT port. See the AP_HAL_Linux/qflight -directory for information on the QFLIGHT port. - -# Building ArduPilot - -Due to some rather unusual licensing terms from Intrinsyc we cannot -distribute binaries of ArduPilot (or any program built with the -Qualcomm libraries). So you will have to build the firmware yourself. - -To build ArduPilot you will need 3 library packages from -Intrinsyc. They are: - - * the HEXAGON_Tools package, tested with version 7.2.11 - * the Hexagon_SDK packet, version 2.0 - * the HexagonFCAddon package, tested with Flight_BSP_1.1_ES3_003.2 - -These packages should all be unpacked in a $HOME/Qualcomm directory. - -To build APM:Copter you then do: - -``` - cd ArduCopter - make qurt -j4 -``` - -you can then upload the firmware to your board by joining to the WiFi -network of the board and doing this - -``` - make qurt_send FLIGHT_BOARD=myboard -``` - -where "myboard" is the hostname or IP address of your board. - -This will install two files: - -``` - /root/ArduCopter.elf - /usr/share/data/adsp/libardupilot_skel.so -``` - -To start ArduPilot just run the elf file as root on the flight board: -``` - ./ArduCopter.elf -``` -By default ArduPilot will send telemetry on UDP 14550 to the local -WiFi network. Just open your favourite MAVLink compatible GCS and -connect with UDP. - -You can optionally give command line arguments for the device paths: -``` - -B GPS device (default /dev/tty-4) - -C telemetry1 (default /dev/tty-2) - -D telemetry2 (no default) - -E GPS2 (no default) - -e ESC output device (default /dev/tty-3) - -S spektrum input device (default /dev/tty-1) -``` -# Logging - -To get DataFlash logs you need to create the logs directory like this: - -``` - mkdir /usr/share/data/adsp/logs -``` - -Normal ArduPilot dataflash logs will appear in that directory. You -will need to transfer them off your board using scp, ftp, rsync or -Samba. - -# UART connections - -The Qualcomm Flight board has 4 DF13 6 pin UART connectors. Be careful -though as they do not have the same pinout as the same connectors on a -Pixhawk. - -The pinout of them all is: - - * pin1: power - * pin2: TX - * pin3: RX - * pin5: GND - -3 of the 4 ports provide 3.3V power on pin1, while the 4th port -provides 5V power. Note that pin6 is not ground, unlike on a Pixhawk. - -The 4 ports are called /dev/tty-1, /dev/tty-2, /dev/tty-3 and -/dev/tty-4. The first port is the one closest to the USB3 -connector. The ports proceed counter-clockwise from there. So tty-2 is -the one closest to the power connector. - -Only tty-2 provides 5V power. The others provide 3.3V power. You will -need to check whether your GPS can be powered off 3.3V. - -The default assignment of the ports is: - - * /dev/tty-1: RC input (Spektrum satellite only) - * /dev/tty-2: telemetry at 57600 - * /dev/tty-3: RC output (see below) - * /dev/tty-4: GPS - -You can control which device is used for what purpose with the command -line options given above. - -This assumes a GPS that can be powered off 3.3V. A uBlox GPS is -recommended, although any ArduPilot compatible serial GPS can be used. - -# ESC PWM Output - -To get signals to ESCs or servos you need to use a UART. The default -setup is to send 4 PWM signals as serial data on /dev/tty-3. This is -designed to work with this firmware for any ArduPilot compatible -board: - - https://github.com/tridge/ardupilot/tree/hal-qurt/libraries/RC_Channel/examples/RC_UART - -that firmware will read the UART serial stream and output to the PWM -output of the board you use. For example, you could use a Pixracer or -Pixhawk board. This is an interim solution until Qualcomm/Intrinsyc -release an ESC add-on control board for the Qualcomm Flight. - diff --git a/libraries/AP_HAL_QURT/Scheduler.cpp b/libraries/AP_HAL_QURT/Scheduler.cpp deleted file mode 100644 index 214fcb1993..0000000000 --- a/libraries/AP_HAL_QURT/Scheduler.cpp +++ /dev/null @@ -1,274 +0,0 @@ -#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 deleted file mode 100644 index ce1bdb24d2..0000000000 --- a/libraries/AP_HAL_QURT/Scheduler.h +++ /dev/null @@ -1,73 +0,0 @@ -#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 deleted file mode 100644 index 6d5dc6a62e..0000000000 --- a/libraries/AP_HAL_QURT/Semaphores.cpp +++ /dev/null @@ -1,39 +0,0 @@ -#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 deleted file mode 100644 index a3fe08807c..0000000000 --- a/libraries/AP_HAL_QURT/Semaphores.h +++ /dev/null @@ -1,20 +0,0 @@ -#pragma once - -#include - -#if CONFIG_HAL_BOARD == HAL_BOARD_QURT -#include "AP_HAL_QURT.h" -#include - -class QURT::Semaphore : public AP_HAL::Semaphore { -public: - Semaphore() { - pthread_mutex_init(&_lock, nullptr); - } - bool give(); - bool take(uint32_t timeout_ms); - bool take_nonblocking(); -private: - pthread_mutex_t _lock; -}; -#endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_HAL_QURT/Storage.cpp b/libraries/AP_HAL_QURT/Storage.cpp deleted file mode 100644 index d9d1f74fd8..0000000000 --- a/libraries/AP_HAL_QURT/Storage.cpp +++ /dev/null @@ -1,49 +0,0 @@ -#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 deleted file mode 100644 index 6f5771150b..0000000000 --- a/libraries/AP_HAL_QURT/Storage.h +++ /dev/null @@ -1,27 +0,0 @@ -#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 deleted file mode 100644 index f61655cf17..0000000000 --- a/libraries/AP_HAL_QURT/UARTDriver.cpp +++ /dev/null @@ -1,300 +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 "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 deleted file mode 100644 index c6e22e74ad..0000000000 --- a/libraries/AP_HAL_QURT/UARTDriver.h +++ /dev/null @@ -1,68 +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::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 deleted file mode 100644 index 90434fe441..0000000000 --- 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 3f724eedad..0000000000 --- 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/Util.cpp b/libraries/AP_HAL_QURT/Util.cpp deleted file mode 100644 index 08e635c73e..0000000000 --- a/libraries/AP_HAL_QURT/Util.cpp +++ /dev/null @@ -1,27 +0,0 @@ -#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 deleted file mode 100644 index bdf2d054f1..0000000000 --- a/libraries/AP_HAL_QURT/Util.h +++ /dev/null @@ -1,31 +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 -#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 deleted file mode 100644 index 8349dfa03b..0000000000 --- a/libraries/AP_HAL_QURT/dsp_main.cpp +++ /dev/null @@ -1,107 +0,0 @@ -#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/mainapp/getifaddrs.cpp b/libraries/AP_HAL_QURT/mainapp/getifaddrs.cpp deleted file mode 100644 index e1d73d09dd..0000000000 --- a/libraries/AP_HAL_QURT/mainapp/getifaddrs.cpp +++ /dev/null @@ -1,187 +0,0 @@ -/* - get system network addresses - - based on code from Samba - - Copyright (C) Andrew Tridgell 1998 - Copyright (C) Jeremy Allison 2007 - Copyright (C) Jelmer Vernooij 2007 - - 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 -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -void freeifaddrs(struct ifaddrs *ifp) -{ - if (ifp != nullptr) { - free(ifp->ifa_name); - free(ifp->ifa_addr); - free(ifp->ifa_netmask); - free(ifp->ifa_dstaddr); - freeifaddrs(ifp->ifa_next); - free(ifp); - } -} - -static struct sockaddr *sockaddr_dup(struct sockaddr *sa) -{ - struct sockaddr *ret; - socklen_t socklen; - socklen = sizeof(struct sockaddr_storage); - ret = (struct sockaddr *)calloc(1, socklen); - if (ret == nullptr) - return nullptr; - memcpy(ret, sa, socklen); - return ret; -} - -/* this works for Linux 2.2, Solaris 2.5, SunOS4, HPUX 10.20, OSF1 - V4.0, Ultrix 4.4, SCO Unix 3.2, IRIX 6.4 and FreeBSD 3.2. - - It probably also works on any BSD style system. */ - -int getifaddrs(struct ifaddrs **ifap) -{ - struct ifconf ifc; - char buff[8192]; - int fd, i, n; - struct ifreq *ifr=nullptr; - struct ifaddrs *curif; - struct ifaddrs *lastif = nullptr; - - *ifap = nullptr; - - if ((fd = socket(AF_INET, SOCK_DGRAM, 0)) == -1) { - return -1; - } - - ifc.ifc_len = sizeof(buff); - ifc.ifc_buf = buff; - - if (ioctl(fd, SIOCGIFCONF, &ifc) != 0) { - close(fd); - return -1; - } - - ifr = ifc.ifc_req; - - n = ifc.ifc_len / sizeof(struct ifreq); - - /* Loop through interfaces, looking for given IP address */ - for (i=n-1; i>=0; i--) { - if (ioctl(fd, SIOCGIFFLAGS, &ifr[i]) == -1) { - freeifaddrs(*ifap); - close(fd); - return -1; - } - - curif = (struct ifaddrs *)calloc(1, sizeof(struct ifaddrs)); - if (curif == nullptr) { - freeifaddrs(*ifap); - close(fd); - return -1; - } - curif->ifa_name = strdup(ifr[i].ifr_name); - if (curif->ifa_name == nullptr) { - free(curif); - freeifaddrs(*ifap); - close(fd); - return -1; - } - curif->ifa_flags = ifr[i].ifr_flags; - curif->ifa_dstaddr = nullptr; - curif->ifa_data = nullptr; - curif->ifa_next = nullptr; - - curif->ifa_addr = nullptr; - if (ioctl(fd, SIOCGIFADDR, &ifr[i]) != -1) { - curif->ifa_addr = sockaddr_dup(&ifr[i].ifr_addr); - if (curif->ifa_addr == nullptr) { - free(curif->ifa_name); - free(curif); - freeifaddrs(*ifap); - close(fd); - return -1; - } - } - - curif->ifa_netmask = nullptr; - if (ioctl(fd, SIOCGIFNETMASK, &ifr[i]) != -1) { - curif->ifa_netmask = sockaddr_dup(&ifr[i].ifr_addr); - if (curif->ifa_netmask == nullptr) { - if (curif->ifa_addr != nullptr) { - free(curif->ifa_addr); - } - free(curif->ifa_name); - free(curif); - freeifaddrs(*ifap); - close(fd); - return -1; - } - } - - if (lastif == nullptr) { - *ifap = curif; - } else { - lastif->ifa_next = curif; - } - lastif = curif; - } - - close(fd); - - return 0; -} - -const char *get_ipv4_broadcast(void) -{ - struct ifaddrs *ifap = nullptr; - if (getifaddrs(&ifap) != 0) { - return nullptr; - } - struct ifaddrs *ia; - for (ia=ifap; ia; ia=ia->ifa_next) { - struct sockaddr_in *sin = (struct sockaddr_in *)ia->ifa_addr; - struct sockaddr_in *nmask = (struct sockaddr_in *)ia->ifa_netmask; - struct in_addr bcast; - if (strcmp(ia->ifa_name, "lo") == 0) { - continue; - } - bcast.s_addr = sin->sin_addr.s_addr | ~nmask->sin_addr.s_addr; - const char *ret = inet_ntoa(bcast); - freeifaddrs(ifap); - return ret; - } - freeifaddrs(ifap); - return nullptr; -} - -#ifdef MAIN_PROGRAM -int main(void) -{ - printf("%s\n", get_ipv4_broadcast()); - return 0; -} -#endif diff --git a/libraries/AP_HAL_QURT/mainapp/mainapp.cpp b/libraries/AP_HAL_QURT/mainapp/mainapp.cpp deleted file mode 100644 index eefd1863fd..0000000000 --- a/libraries/AP_HAL_QURT/mainapp/mainapp.cpp +++ /dev/null @@ -1,165 +0,0 @@ -/* - main program for HAL_QURT port - */ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -static SocketAPM sock{true}; -static bool connected; -static uint32_t last_get_storage_us; -static uint64_t start_time; - -// location of virtual eeprom in Linux filesystem -#define STORAGE_DIR "/var/APM" -#define STORAGE_FILE STORAGE_DIR "/" SKETCHNAME ".stg" - -extern const char *get_ipv4_broadcast(void); - -// time since startup in microseconds -static uint64_t micros64() -{ - struct timespec ts; - clock_gettime(CLOCK_MONOTONIC, &ts); - uint64_t ret = ts.tv_sec*1000*1000ULL + ts.tv_nsec/1000U; - if (start_time == 0) { - start_time = ret; - } - ret -= start_time; - return ret; -} - -/* - send storage file to DSPs - */ -static void send_storage(void) -{ - int fd = open(STORAGE_FILE, O_RDWR|O_CREAT, 0644); - if (fd == -1) { - printf("Unable to open %s", STORAGE_FILE); - exit(1); - } - uint8_t buf[16384]; - memset(buf, 0, sizeof(buf)); - read(fd, buf, sizeof(buf)); - if (ardupilot_set_storage(buf, sizeof(buf)) != 0) { - printf("Failed to send initial storage"); - exit(1); - } - close(fd); -} - -/* - get updated storage file from DSPs - */ -static void get_storage(void) -{ - uint8_t buf[16384]; - if (ardupilot_get_storage(buf, sizeof(buf)) != 0) { - return; - } - int fd = open(STORAGE_FILE ".new", O_WRONLY|O_CREAT|O_TRUNC, 0644); - if (fd == -1) { - printf("Unable to open %s - %s\n", STORAGE_FILE ".new", strerror(errno)); - } - write(fd, buf, sizeof(buf)); - close(fd); - // atomic rename - if (rename(STORAGE_FILE ".new", STORAGE_FILE) != 0) { - printf("Unable to rename to %s - %s\n", STORAGE_FILE, strerror(errno)); - } -} - -/* - handle any incoming or outgoing UDP socket data on behalf of the DSPs - */ -static void socket_check(void) -{ - static const char *bcast = nullptr; - uint8_t buf[300]; - ssize_t ret = sock.recv(buf, sizeof(buf), 0); - if (ret > 0) { - uint32_t nbytes; - ardupilot_socket_input(buf, ret, &nbytes); - if (!connected) { - const char *ip; - uint16_t port; - sock.last_recv_address(ip, port); - connected = sock.connect(ip, port); - if (connected) { - printf("Connected to UDP %s:%u\n", ip, (unsigned)port); - } - sock.set_blocking(false); - } - } - uint32_t nbytes; - if (bcast == nullptr) { - bcast = get_ipv4_broadcast(); - if (bcast == nullptr) { - bcast = "255.255.255.255"; - } - printf("Broadcasting to %s\n", bcast); - } - if (ardupilot_socket_check(buf, sizeof(buf), &nbytes) == 0) { - if (!connected) { - sock.sendto(buf, nbytes, bcast, 14550); - } else { - sock.send(buf, nbytes); - } - } -} - -/* - encode argv/argv as a sequence separated by \n - */ -static char *encode_argv(int argc, const char *argv[]) -{ - uint32_t len = 0; - for (int i=0; i 1000*1000) { - printf("tick t=%.6f\n", now*1.0e-6f); - ardupilot_heartbeat(); - get_storage(); - last_get_storage_us = now; - } - socket_check(); - usleep(5000); - } -} diff --git a/libraries/AP_HAL_QURT/qurt_dsp.idl b/libraries/AP_HAL_QURT/qurt_dsp.idl deleted file mode 100644 index d813e29d50..0000000000 --- a/libraries/AP_HAL_QURT/qurt_dsp.idl +++ /dev/null @@ -1,17 +0,0 @@ -#include "AEEStdDef.idl" - -interface ardupilot { - // start main thread - uint32 start(in sequence cmdline); - - // a heartbeat for debugging - uint32 heartbeat(); - - // get eeprom updates - uint32 set_storage(in sequence eeprom); - uint32 get_storage(rout sequence eeprom); - - // handle socket data - uint32 socket_check(rout sequence buf, rout uint32 nbytes); - uint32 socket_input(in sequence buf, rout uint32 nbytes); -}; diff --git a/libraries/AP_HAL_QURT/replace.cpp b/libraries/AP_HAL_QURT/replace.cpp deleted file mode 100644 index 050d2c9b77..0000000000 --- a/libraries/AP_HAL_QURT/replace.cpp +++ /dev/null @@ -1,76 +0,0 @@ -#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 deleted file mode 100644 index 8f085c2af3..0000000000 --- a/libraries/AP_HAL_QURT/replace.h +++ /dev/null @@ -1,39 +0,0 @@ -#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 deleted file mode 100644 index 64f3a57b83..0000000000 --- a/libraries/AP_HAL_QURT/system.cpp +++ /dev/null @@ -1,66 +0,0 @@ -#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