From dc8a3bbf92badfdfef63d55b0be7c5b2ea7d4ace Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 14 Dec 2015 14:19:54 +1100 Subject: [PATCH] HAL_Linux: added qflight board subtype --- .../AP_HAL_Linux/AP_HAL_Linux_Namespace.h | 2 + libraries/AP_HAL_Linux/AP_HAL_Linux_Private.h | 2 + libraries/AP_HAL_Linux/GPIO_Sysfs.cpp | 2 +- libraries/AP_HAL_Linux/HAL_Linux_Class.cpp | 32 +- libraries/AP_HAL_Linux/RCInput.cpp | 62 ++- libraries/AP_HAL_Linux/RCInput.h | 11 + libraries/AP_HAL_Linux/RCInput_DSM.cpp | 74 ++++ libraries/AP_HAL_Linux/RCInput_DSM.h | 36 ++ libraries/AP_HAL_Linux/RCOutput_qflight.cpp | 176 ++++++++ libraries/AP_HAL_Linux/RCOutput_qflight.h | 47 +++ libraries/AP_HAL_Linux/Scheduler.cpp | 62 ++- libraries/AP_HAL_Linux/Scheduler.h | 1 + libraries/AP_HAL_Linux/UARTDriver.cpp | 25 ++ libraries/AP_HAL_Linux/UARTDriver.h | 4 + libraries/AP_HAL_Linux/UARTQFlight.cpp | 104 +++++ libraries/AP_HAL_Linux/UARTQFlight.h | 25 ++ .../AP_HAL_Linux/qflight/dsp_functions.cpp | 395 ++++++++++++++++++ .../AP_HAL_Linux/qflight/qflight_buffer.h | 38 ++ .../AP_HAL_Linux/qflight/qflight_dsp.idl | 15 + libraries/AP_HAL_Linux/qflight/qflight_util.h | 3 + 20 files changed, 1098 insertions(+), 18 deletions(-) create mode 100644 libraries/AP_HAL_Linux/RCInput_DSM.cpp create mode 100644 libraries/AP_HAL_Linux/RCInput_DSM.h create mode 100644 libraries/AP_HAL_Linux/RCOutput_qflight.cpp create mode 100644 libraries/AP_HAL_Linux/RCOutput_qflight.h create mode 100644 libraries/AP_HAL_Linux/UARTQFlight.cpp create mode 100644 libraries/AP_HAL_Linux/UARTQFlight.h create mode 100644 libraries/AP_HAL_Linux/qflight/dsp_functions.cpp create mode 100644 libraries/AP_HAL_Linux/qflight/qflight_buffer.h create mode 100644 libraries/AP_HAL_Linux/qflight/qflight_dsp.idl create mode 100644 libraries/AP_HAL_Linux/qflight/qflight_util.h diff --git a/libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h b/libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h index d059e93c2d..aa34099a0a 100644 --- a/libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h +++ b/libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h @@ -30,6 +30,7 @@ namespace Linux { class RCInput_ZYNQ; class RCInput_UART; class RCInput_UDP; + class RCInput_DSM; class RCOutput_PRU; class RCOutput_AioPRU; class RCOutput_PCA9685; @@ -37,6 +38,7 @@ namespace Linux { class RCOutput_ZYNQ; class RCOutput_Bebop; class RCOutput_Sysfs; + class RCOutput_QFLIGHT; class Semaphore; class Scheduler; class Util; diff --git a/libraries/AP_HAL_Linux/AP_HAL_Linux_Private.h b/libraries/AP_HAL_Linux/AP_HAL_Linux_Private.h index 3e03c45e2c..54070af485 100644 --- a/libraries/AP_HAL_Linux/AP_HAL_Linux_Private.h +++ b/libraries/AP_HAL_Linux/AP_HAL_Linux_Private.h @@ -22,6 +22,7 @@ #include "RCInput_UART.h" #include "RCInput_UDP.h" #include "RCInput_Raspilot.h" +#include "RCInput_DSM.h" #include "RCOutput_PRU.h" #include "RCOutput_AioPRU.h" #include "RCOutput_PCA9685.h" @@ -29,6 +30,7 @@ #include "RCOutput_Bebop.h" #include "RCOutput_Raspilot.h" #include "RCOutput_Sysfs.h" +#include "RCOutput_qflight.h" #include "Semaphores.h" #include "Scheduler.h" #include "ToneAlarmDriver.h" diff --git a/libraries/AP_HAL_Linux/GPIO_Sysfs.cpp b/libraries/AP_HAL_Linux/GPIO_Sysfs.cpp index 44ef43561a..86d7ad1ad9 100644 --- a/libraries/AP_HAL_Linux/GPIO_Sysfs.cpp +++ b/libraries/AP_HAL_Linux/GPIO_Sysfs.cpp @@ -2,7 +2,7 @@ #include -#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX +#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX && CONFIG_HAL_BOARD_SUBTYPE != HAL_BOARD_SUBTYPE_LINUX_QFLIGHT #include #include diff --git a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp b/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp index 4188ce17cb..5a9385ecf7 100644 --- a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp +++ b/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp @@ -38,6 +38,7 @@ static RPIOUARTDriver uartCDriver; #else static UARTDriver uartCDriver(false); #endif +static UARTDriver uartDDriver(false); static UARTDriver uartEDriver(false); #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP @@ -63,6 +64,9 @@ static I2CDriver i2cDriver0(&i2cSemaphore0, i2c_devpaths); /* One additional emulated bus */ static Semaphore i2cSemaphore1; static I2CDriver i2cDriver1(&i2cSemaphore1, "/dev/i2c-10"); +#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT +static Semaphore i2cSemaphore0; +static Empty::I2CDriver i2cDriver0(&i2cSemaphore0); #else static Semaphore i2cSemaphore0; static I2CDriver i2cDriver0(&i2cSemaphore0, "/dev/i2c-1"); @@ -74,6 +78,8 @@ static SPIDeviceManager spiDeviceManager; static ADS1115AnalogIn analogIn; #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT static RaspilotAnalogIn analogIn; +#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT +static Empty::AnalogIn analogIn; #else static AnalogIn analogIn; #endif @@ -125,6 +131,8 @@ static RCInput_ZYNQ rcinDriver; static RCInput_UDP rcinDriver; #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_MINLURE static RCInput_UART rcinDriver("/dev/ttyS2"); +#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT +static RCInput_DSM rcinDriver; #else static RCInput rcinDriver; #endif @@ -154,6 +162,8 @@ static RCOutput_ZYNQ rcoutDriver; static RCOutput_Bebop rcoutDriver; #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_MINLURE static RCOutput_PCA9685 rcoutDriver(PCA9685_PRIMARY_ADDRESS, false, 0, MINNOW_GPIO_S5_1); +#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT +static RCOutput_QFLIGHT rcoutDriver; #else static Empty::RCOutput rcoutDriver; #endif @@ -171,7 +181,7 @@ HAL_Linux::HAL_Linux() : &uartADriver, &uartBDriver, &uartCDriver, - NULL, /* no uartD */ + &uartDDriver, &uartEDriver, #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP &i2cDriver0, @@ -200,7 +210,7 @@ HAL_Linux::HAL_Linux() : void _usage(void) { - printf("Usage: -A uartAPath -B uartBPath -C uartCPath\n"); + printf("Usage: -A uartAPath -B uartBPath -C uartCPath -D uartDPath -E uartEPath\n"); printf("Options:\n"); printf("\t-serial: -A /dev/ttyO4\n"); printf("\t -B /dev/ttyS1\n"); @@ -224,14 +234,19 @@ void HAL_Linux::run(int argc, char* const argv[], Callbacks* callbacks) const {"uartA", true, 0, 'A'}, {"uartB", true, 0, 'B'}, {"uartC", true, 0, 'C'}, + {"uartD", true, 0, 'D'}, {"uartE", true, 0, 'E'}, +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT + {"dsm", true, 0, 'S'}, + {"ESC", true, 0, 'e'}, +#endif {"log-directory", true, 0, 'l'}, {"terrain-directory", true, 0, 't'}, {"help", false, 0, 'h'}, {0, false, 0, 0} }; - GetOptLong gopt(argc, argv, "A:B:C:E:l:t:h", + GetOptLong gopt(argc, argv, "A:B:C:D:E:l:t:he:S", options); /* @@ -248,9 +263,20 @@ void HAL_Linux::run(int argc, char* const argv[], Callbacks* callbacks) const 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; +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT + case 'e': + rcoutDriver.set_device_path(gopt.optarg); + break; + case 'S': + rcinDriver.set_device_path(gopt.optarg); + break; +#endif // CONFIG_HAL_BOARD_SUBTYPE case 'l': utilInstance.set_custom_log_directory(gopt.optarg); break; diff --git a/libraries/AP_HAL_Linux/RCInput.cpp b/libraries/AP_HAL_Linux/RCInput.cpp index 1b9dabda02..2bba7ad948 100644 --- a/libraries/AP_HAL_Linux/RCInput.cpp +++ b/libraries/AP_HAL_Linux/RCInput.cpp @@ -16,7 +16,7 @@ #include "RCInput.h" #include "sbus.h" -#include "dsm.h" +#include extern const AP_HAL::HAL& hal; @@ -374,4 +374,64 @@ void RCInput::_update_periods(uint16_t *periods, uint8_t len) new_rc_input = true; } + +/* + add some bytes of input in DSM serial stream format, coping with partial packets + */ +void RCInput::add_dsm_input(const uint8_t *bytes, size_t nbytes) +{ + if (nbytes == 0) { + return; + } + const uint8_t dsm_frame_size = sizeof(dsm.frame); + + uint32_t now = AP_HAL::millis(); + if (now - dsm.last_input_ms > 5) { + // resync based on time + dsm.partial_frame_count = 0; + } + dsm.last_input_ms = now; + + while (nbytes > 0) { + size_t n = nbytes; + if (dsm.partial_frame_count + n > dsm_frame_size) { + n = dsm_frame_size - dsm.partial_frame_count; + } + if (n > 0) { + memcpy(&dsm.frame[dsm.partial_frame_count], bytes, n); + dsm.partial_frame_count += n; + nbytes -= n; + bytes += n; + } + + if (dsm.partial_frame_count == dsm_frame_size) { + dsm.partial_frame_count = 0; + uint16_t values[16] {}; + uint16_t num_values=0; + if (dsm_decode(AP_HAL::micros64(), dsm.frame, values, &num_values, 16) && + num_values >= 5) { + for (uint8_t i=0; i _num_channels) { + _num_channels = num_values; + } + new_rc_input = true; +#if 0 + 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_Linux/RCInput.h b/libraries/AP_HAL_Linux/RCInput.h index 11ceb57dfa..b3f4eff47e 100644 --- a/libraries/AP_HAL_Linux/RCInput.h +++ b/libraries/AP_HAL_Linux/RCInput.h @@ -31,6 +31,10 @@ public: // specific implementations virtual void _timer_tick() {} + // add some DSM input bytes, for RCInput over a serial port + void add_dsm_input(const uint8_t *bytes, size_t nbytes); + + protected: void _process_rc_pulse(uint16_t width_s0, uint16_t width_s1, uint16_t channel = LINUX_RC_INPUT_CHANNEL_INVALID); @@ -67,6 +71,13 @@ public: uint16_t bytes[16]; // including start bit and stop bit uint16_t bit_ofs; } dsm_state; + + // state of add_dsm_input + struct { + uint8_t frame[16]; + uint8_t partial_frame_count; + uint32_t last_input_ms; + } dsm; }; #include "RCInput_PRU.h" diff --git a/libraries/AP_HAL_Linux/RCInput_DSM.cpp b/libraries/AP_HAL_Linux/RCInput_DSM.cpp new file mode 100644 index 0000000000..c10f4e1d14 --- /dev/null +++ b/libraries/AP_HAL_Linux/RCInput_DSM.cpp @@ -0,0 +1,74 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + 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 . + */ +/* + this is a driver for DSM input in the QFLIGHT board. It could be + extended to other boards in future by providing an open/read/write + abstraction + */ + +#include + +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT +#include "RCInput_DSM.h" +#include +#include +#include + +extern const AP_HAL::HAL& hal; + +using namespace Linux; + +void RCInput_DSM::init() +{ +} + +void RCInput_DSM::set_device_path(const char *path) +{ + device_path = path; + printf("Set DSM device path %s\n", path); +} + +void RCInput_DSM::_timer_tick(void) +{ + if (device_path == nullptr) { + return; + } + int ret; + /* + we defer the open to the timer tick to ensure all RPC calls are + made in the same thread + */ + if (fd == -1) { + ret = qflight_UART_open(device_path, &fd); + if (ret == 0) { + printf("Opened DSM input %s fd=%d\n", device_path, (int)fd); + fflush(stdout); + qflight_UART_set_baudrate(fd, 115200); + } + } + if (fd != -1) { + uint8_t bytes[16]; + int32_t nread; + ret = qflight_UART_read(fd, bytes, sizeof(bytes), &nread); + if (ret == 0 && nread > 0) { + // printf("Read %u DSM bytes at %u\n", (unsigned)nread, AP_HAL::millis()); + fflush(stdout); + add_dsm_input(bytes, nread); + } + } +} +#endif // CONFIG_HAL_BOARD_SUBTYPE + diff --git a/libraries/AP_HAL_Linux/RCInput_DSM.h b/libraries/AP_HAL_Linux/RCInput_DSM.h new file mode 100644 index 0000000000..3aa0de9a3b --- /dev/null +++ b/libraries/AP_HAL_Linux/RCInput_DSM.h @@ -0,0 +1,36 @@ +/* + 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 +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT + +#include "RCInput.h" +#include "RCInput_DSM.h" + +class Linux::RCInput_DSM : public Linux::RCInput +{ +public: + void init() override; + void _timer_tick(void) override; + void set_device_path(const char *path); + +private: + const char *device_path; + int32_t fd = -1; +}; +#endif // CONFIG_HAL_BOARD_SUBTYPE + diff --git a/libraries/AP_HAL_Linux/RCOutput_qflight.cpp b/libraries/AP_HAL_Linux/RCOutput_qflight.cpp new file mode 100644 index 0000000000..b5f4f5dbf8 --- /dev/null +++ b/libraries/AP_HAL_Linux/RCOutput_qflight.cpp @@ -0,0 +1,176 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + 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 . + */ +/* + this is a driver for RC output in the QFLIGHT board. Output goes via + a UART with a CRC. See libraries/RC_Channel/examples/RC_UART for an + example of the other end of this protocol + */ +#include + +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT + +#include "RCOutput_qflight.h" +#include +#include +#include +#include + +extern const AP_HAL::HAL& hal; + +using namespace Linux; + +void RCOutput_QFLIGHT::init() +{ + hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&RCOutput_QFLIGHT::timer_update, void)); +} + +void RCOutput_QFLIGHT::set_device_path(const char *_device) +{ + device = _device; +} + + +void RCOutput_QFLIGHT::set_freq(uint32_t chmask, uint16_t freq_hz) +{ + // no support for changing frequency yet +} + +uint16_t RCOutput_QFLIGHT::get_freq(uint8_t ch) +{ + // return fixed fake value - no control of frequency over the UART + return 490; +} + +void RCOutput_QFLIGHT::enable_ch(uint8_t ch) +{ + if (ch >= channel_count) { + return; + } + enable_mask |= 1U<= channel_count) { + return; + } + enable_mask &= ~1U<= channel_count) { + return; + } + period[ch] = period_us; + need_write = true; +} + +uint16_t RCOutput_QFLIGHT::read(uint8_t ch) +{ + if (ch >= channel_count) { + return 0; + } + return period[ch]; +} + +void RCOutput_QFLIGHT::read(uint16_t *period_us, uint8_t len) +{ + for (int i = 0; i < len; i++) { + period_us[i] = read(i); + } +} + +void RCOutput_QFLIGHT::timer_update(void) +{ + /* + we defer the open to the time to ensure all RPC calls are made + from the same thread + */ + if (fd == -1 && device != nullptr) { + int ret = qflight_UART_open(device, &fd); + printf("Opened ESC UART %s ret=%d fd=%d\n", + device, ret, (int)fd); + if (fd != -1) { + qflight_UART_set_baudrate(fd, baudrate); + } + } + if (!need_write || fd == -1) { + return; + } + /* + this implements the PWM over UART prototocol. + */ + 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); + int32_t nwritten; + qflight_UART_write(fd, (uint8_t *)&frame, sizeof(frame), &nwritten); + need_write = false; + + check_rc_in(); +} + +/* + we accept RC input from the UART and treat it as RC overrides. This + is an lazy way to allow an RCOutput driver to do RCInput. See the + RC_UART example for the other end of this protocol + */ +void RCOutput_QFLIGHT::check_rc_in(void) +{ + const uint8_t magic = 0xf6; + while (nrcin_bytes != sizeof(rcu.bytes)) { + int32_t nread; + if (qflight_UART_read(fd, rcu.bytes, sizeof(rcu.bytes)-nrcin_bytes, &nread) != 0 || nread <= 0) { + return; + } + nrcin_bytes += nread; + if (rcu.rcin.magic != magic) { + for (uint8_t i=1; iset_overrides((int16_t*)rcu.rcin.rcin, 8); + } + } + nrcin_bytes = 0; + } +} + +#endif // CONFIG_HAL_BOARD_SUBTYPE + diff --git a/libraries/AP_HAL_Linux/RCOutput_qflight.h b/libraries/AP_HAL_Linux/RCOutput_qflight.h new file mode 100644 index 0000000000..89e832acf4 --- /dev/null +++ b/libraries/AP_HAL_Linux/RCOutput_qflight.h @@ -0,0 +1,47 @@ +#pragma once + +#include +#include "AP_HAL_Linux.h" + +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT + +class Linux::RCOutput_QFLIGHT : public AP_HAL::RCOutput { +public: + 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 set_device_path(const char *device); + +private: + const char *device = nullptr; + const uint32_t baudrate = 115200; + static const uint8_t channel_count = 4; + + int32_t fd = -1; + uint16_t enable_mask; + uint16_t period[channel_count]; + volatile bool need_write; + + void timer_update(void); + + void check_rc_in(void); + + uint32_t last_read_check_ms; + struct PACKED rcin_frame { + uint8_t magic; + uint16_t rcin[8]; + uint16_t crc; + }; + union { + struct rcin_frame rcin; + uint8_t bytes[19]; + } rcu; + uint8_t nrcin_bytes; +}; + +#endif // CONFIG_HAL_BOARD_SUBTYPE diff --git a/libraries/AP_HAL_Linux/Scheduler.cpp b/libraries/AP_HAL_Linux/Scheduler.cpp index 2422fd7dbf..6422c08511 100644 --- a/libraries/AP_HAL_Linux/Scheduler.cpp +++ b/libraries/AP_HAL_Linux/Scheduler.cpp @@ -16,6 +16,14 @@ #include #include +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT +#include +#include +#include +#include +#endif + + using namespace Linux; extern const AP_HAL::HAL& hal; @@ -261,7 +269,13 @@ void *Scheduler::_timer_thread(void* arg) while (sched->system_initializing()) { poll(NULL, 0, 1); } - /* + +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT + printf("Initialising rpcmem\n"); + rpcmem_init(); +#endif + +/* this aims to run at an average of 1kHz, so that it can be used to drive 1kHz processes without drift */ @@ -277,6 +291,16 @@ void *Scheduler::_timer_thread(void* arg) next_run_usec += 1000; // run registered timers sched->_run_timers(true); + +#if HAL_LINUX_UARTS_ON_TIMER_THREAD + /* + some boards require that UART calls happen on the same + thread as other calls of the same time. This impacts the + QFLIGHT calls where UART output is an RPC call to the DSPs + */ + _run_uarts(); + RCInput::from(hal.rcin)->_timer_tick(); +#endif } return NULL; } @@ -306,11 +330,33 @@ void *Scheduler::_rcin_thread(void *arg) } while (true) { sched->_microsleep(APM_LINUX_RCIN_PERIOD); +#if !HAL_LINUX_UARTS_ON_TIMER_THREAD RCInput::from(hal.rcin)->_timer_tick(); +#endif } return NULL; } + +/* + run timers for all UARTs + */ +void Scheduler::_run_uarts(void) +{ + // process any pending serial bytes + UARTDriver::from(hal.uartA)->_timer_tick(); + UARTDriver::from(hal.uartB)->_timer_tick(); +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT + //SPI UART not use SPI + if (RPIOUARTDriver::from(hal.uartC)->isExternal()) { + RPIOUARTDriver::from(hal.uartC)->_timer_tick(); + } +#else + UARTDriver::from(hal.uartC)->_timer_tick(); +#endif + UARTDriver::from(hal.uartE)->_timer_tick(); +} + void *Scheduler::_uart_thread(void* arg) { Scheduler* sched = (Scheduler *)arg; @@ -320,19 +366,9 @@ void *Scheduler::_uart_thread(void* arg) } while (true) { sched->_microsleep(APM_LINUX_UART_PERIOD); - - // process any pending serial bytes - UARTDriver::from(hal.uartA)->_timer_tick(); - UARTDriver::from(hal.uartB)->_timer_tick(); -#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT - //SPI UART not use SPI - if (RPIOUARTDriver::from(hal.uartC)->isExternal()) { - RPIOUARTDriver::from(hal.uartC)->_timer_tick(); - } -#else - UARTDriver::from(hal.uartC)->_timer_tick(); +#if !HAL_LINUX_UARTS_ON_TIMER_THREAD + _run_uarts(); #endif - UARTDriver::from(hal.uartE)->_timer_tick(); } return NULL; } diff --git a/libraries/AP_HAL_Linux/Scheduler.h b/libraries/AP_HAL_Linux/Scheduler.h index bede9f6f0f..53df4ed7af 100644 --- a/libraries/AP_HAL_Linux/Scheduler.h +++ b/libraries/AP_HAL_Linux/Scheduler.h @@ -82,6 +82,7 @@ private: static void *_io_thread(void* arg); static void *_rcin_thread(void* arg); static void *_uart_thread(void* arg); + static void _run_uarts(void); static void *_tonealarm_thread(void* arg); void _run_timers(bool called_from_timer_thread); diff --git a/libraries/AP_HAL_Linux/UARTDriver.cpp b/libraries/AP_HAL_Linux/UARTDriver.cpp index 8c79ad5093..f0dcc4ef61 100644 --- a/libraries/AP_HAL_Linux/UARTDriver.cpp +++ b/libraries/AP_HAL_Linux/UARTDriver.cpp @@ -28,6 +28,7 @@ #include "UDPDevice.h" #include "ConsoleDevice.h" #include "TCPServerDevice.h" +#include "UARTQFlight.h" extern const AP_HAL::HAL& hal; @@ -87,6 +88,15 @@ void UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) break; } +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT + case DEVICE_QFLIGHT: + { + _qflight_start_connection(); + _flow_control = FLOW_CONTROL_DISABLE; + break; + } +#endif + case DEVICE_SERIAL: { if (!_serial_start_connection()) { @@ -192,6 +202,10 @@ UARTDriver::device_type UARTDriver::_parseDevicePath(const char *arg) if (stat(arg, &st) == 0 && S_ISCHR(st.st_mode)) { return DEVICE_SERIAL; +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT + } else if (strncmp(arg, "qflight:", 8) == 0) { + return DEVICE_QFLIGHT; +#endif } else if (strncmp(arg, "tcp:", 4) != 0 && strncmp(arg, "udp:", 4) != 0) { return DEVICE_UNKNOWN; @@ -259,6 +273,17 @@ bool UARTDriver::_serial_start_connection() return true; } +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT +bool UARTDriver::_qflight_start_connection() +{ + _device = new QFLIGHTDevice(device_path); + _connected = _device->open(); + _flow_control = FLOW_CONTROL_DISABLE; + + return true; +} +#endif + /* start a UDP connection for the serial port */ diff --git a/libraries/AP_HAL_Linux/UARTDriver.h b/libraries/AP_HAL_Linux/UARTDriver.h index e9f70a050c..dfce309eab 100644 --- a/libraries/AP_HAL_Linux/UARTDriver.h +++ b/libraries/AP_HAL_Linux/UARTDriver.h @@ -56,11 +56,15 @@ private: void _udp_start_connection(void); void _tcp_start_connection(void); bool _serial_start_connection(void); + bool _qflight_start_connection(void); enum device_type { DEVICE_TCP, DEVICE_UDP, DEVICE_SERIAL, +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT + DEVICE_QFLIGHT, +#endif DEVICE_UNKNOWN }; diff --git a/libraries/AP_HAL_Linux/UARTQFlight.cpp b/libraries/AP_HAL_Linux/UARTQFlight.cpp new file mode 100644 index 0000000000..c7dad17088 --- /dev/null +++ b/libraries/AP_HAL_Linux/UARTQFlight.cpp @@ -0,0 +1,104 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + 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 . + */ +/* + This is a UART driver for the QFLIGHT port. Actual UART output + happens via RPC calls. See the qflight/ subdirectory for details + */ + +#include + +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT + +#include +#include + +#include "UARTQFlight.h" + +#include +#include +#include + +QFLIGHTDevice::QFLIGHTDevice(const char *_device_path) +{ + device_path = _device_path; + if (strncmp(device_path, "qflight:", 8) == 0) { + device_path += 8; + } +} + +QFLIGHTDevice::~QFLIGHTDevice() +{ + close(); +} + +bool QFLIGHTDevice::close() +{ + if (fd != -1) { + if (qflight_UART_close(fd) != 0) { + return false; + } + } + + fd = -1; + return true; +} + +bool QFLIGHTDevice::open() +{ + int ret = qflight_UART_open(device_path, &fd); + + if (ret != 0 || fd == -1) { + printf("Failed to open UART device %s ret=%d fd=%d\n", + device_path, ret, (int)fd); + return false; + } + printf("opened QFLIGHT UART device %s ret=%d fd=%d\n", + device_path, ret, (int)fd); + return true; +} + +ssize_t QFLIGHTDevice::read(uint8_t *buf, uint16_t n) +{ + int32_t nread = 0; + int ret = qflight_UART_read(fd, buf, n, &nread); + if (ret != 0) { + return 0; + } + return nread; +} + +ssize_t QFLIGHTDevice::write(const uint8_t *buf, uint16_t n) +{ + int32_t nwritten = 0; + int ret = qflight_UART_write(fd, buf, n, &nwritten); + if (ret != 0) { + return 0; + } + return nwritten; +} + +void QFLIGHTDevice::set_blocking(bool blocking) +{ + // no implementation yet +} + +void QFLIGHTDevice::set_speed(uint32_t baudrate) +{ + qflight_UART_set_baudrate(fd, baudrate); +} + +#endif // CONFIG_HAL_BOARD_SUBTYPE + diff --git a/libraries/AP_HAL_Linux/UARTQFlight.h b/libraries/AP_HAL_Linux/UARTQFlight.h new file mode 100644 index 0000000000..c90c2368f8 --- /dev/null +++ b/libraries/AP_HAL_Linux/UARTQFlight.h @@ -0,0 +1,25 @@ +#pragma once + +#include +#include "SerialDevice.h" + +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT + +class QFLIGHTDevice: public SerialDevice { +public: + QFLIGHTDevice(const char *device_path); + virtual ~QFLIGHTDevice(); + + virtual bool open() override; + virtual bool close() override; + virtual ssize_t write(const uint8_t *buf, uint16_t n) override; + virtual ssize_t read(uint8_t *buf, uint16_t n) override; + virtual void set_blocking(bool blocking) override; + virtual void set_speed(uint32_t speed) override; + +private: + int32_t fd = -1; + const char *device_path; +}; + +#endif diff --git a/libraries/AP_HAL_Linux/qflight/dsp_functions.cpp b/libraries/AP_HAL_Linux/qflight/dsp_functions.cpp new file mode 100644 index 0000000000..c486f9b122 --- /dev/null +++ b/libraries/AP_HAL_Linux/qflight/dsp_functions.cpp @@ -0,0 +1,395 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + 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 . + */ +/* + This is an implementation of all of the code for the QFLIGHT board + that runs on the DSPs. See qflight_dsp.idl for the interface + definition for the RPC calls + */ +#include +#include +#include +#include "qflight_dsp.h" +extern "C" { +#include "bmp280_api.h" +#include "mpu9x50.h" +} + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "qflight_buffer.h" +#include + +const float GRAVITY_MSS = 9.80665; +const float ACCEL_SCALE_1G = GRAVITY_MSS / 2048.0; +const float GYRO_SCALE = 0.0174532 / 16.4; +const float RAD_TO_DEG = 57.295779513082320876798154814105; + +static ObjectBuffer imu_buffer(30); +static ObjectBuffer mag_buffer(10); +static ObjectBuffer baro_buffer(10); +static bool mpu9250_started; +static uint32_t bmp280_handle; +static uint32_t baro_counter; + +/* + read buffering for UARTs + */ +static const uint8_t max_uarts = 8; +static uint8_t num_open_uarts; +static struct uartbuf { + int fd; + ByteBuffer *readbuffer; +} uarts[max_uarts]; + +extern "C" { +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); +} + +void HAP_printf(const char *file, int line, const char *format, ...); +#define HAP_PRINTF(...) HAP_printf(__FILE__, __LINE__, __VA_ARGS__) + +static int init_barometer(void) +{ + int ret = bmp280_open("/dev/i2c-2", &bmp280_handle); + HAP_PRINTF("**** bmp280: ret=%d handle=0x%x\n", ret, (unsigned)bmp280_handle); + return ret; +} + +static int init_mpu9250(void) +{ + struct mpu9x50_config config; + + config.gyro_lpf = MPU9X50_GYRO_LPF_184HZ; + config.acc_lpf = MPU9X50_ACC_LPF_184HZ; + config.gyro_fsr = MPU9X50_GYRO_FSR_2000DPS; + config.acc_fsr = MPU9X50_ACC_FSR_16G; + config.gyro_sample_rate = MPU9x50_SAMPLE_RATE_1000HZ; + config.compass_enabled = true; + config.compass_sample_rate = MPU9x50_COMPASS_SAMPLE_RATE_100HZ; + config.spi_dev_path = "/dev/spi-1"; + + int ret; + ret = mpu9x50_validate_configuration(&config); + HAP_PRINTF("***** mpu9250 validate ret=%d\n", ret); + if (ret != 0) { + return ret; + } + ret = mpu9x50_initialize(&config); + HAP_PRINTF("***** mpu9250 initialise ret=%d\n", ret); + + mpu9250_started = true; + + return ret; +} + +/* + thread gathering sensor data from mpu9250 + */ +static void *mpu_data_ready(void *ctx) +{ + struct mpu9x50_data data; + memset(&data, 0, sizeof(data)); + + int ret = mpu9x50_get_data(&data); + if (ret != 0) { + return NULL; + } + DSPBuffer::IMU::BUF b; + b.timestamp = data.timestamp; + b.accel[0] = data.accel_raw[0]*ACCEL_SCALE_1G; + b.accel[1] = data.accel_raw[1]*ACCEL_SCALE_1G; + b.accel[2] = data.accel_raw[2]*ACCEL_SCALE_1G; + b.gyro[0] = data.gyro_raw[0]*GYRO_SCALE; + b.gyro[1] = data.gyro_raw[1]*GYRO_SCALE; + b.gyro[2] = data.gyro_raw[2]*GYRO_SCALE; + imu_buffer.push(b); + + if (data.mag_data_ready) { + DSPBuffer::MAG::BUF m; + m.mag_raw[0] = data.mag_raw[0]; + m.mag_raw[1] = data.mag_raw[1]; + m.mag_raw[2] = data.mag_raw[2]; + m.timestamp = data.timestamp; + mag_buffer.push(m); + } + + if (bmp280_handle != 0 && baro_counter++ % 10 == 0) { + struct bmp280_sensor_data data; + memset(&data, 0, sizeof(data)); + int ret = bmp280_get_sensor_data(bmp280_handle, &data, false); + if (ret == 0) { + DSPBuffer::BARO::BUF b; + b.pressure_pa = data.pressure_in_pa; + b.temperature_C = data.temperature_in_c; + b.timestamp = data.last_read_time_in_usecs; + baro_buffer.push(b); + } + } + + return NULL; +} + +static void mpu9250_startup(void) +{ + if (!mpu9250_started) { + if (init_mpu9250() != 0) { + return; + } + mpu9x50_register_interrupt(65, mpu_data_ready, NULL); + } +} + +/* + get any available IMU data + */ +int qflight_get_imu_data(uint8_t *buf, int len) +{ + DSPBuffer::IMU &imu = *(DSPBuffer::IMU *)buf; + + if (len != sizeof(imu)) { + HAP_PRINTF("incorrect size for imu data %d should be %d\n", + len, sizeof(imu)); + return 1; + } + + mpu9250_startup(); + + imu.num_samples = 0; + while (imu.num_samples < imu.max_samples && + imu_buffer.pop(imu.buf[imu.num_samples])) { + imu.num_samples++; + } + + return 0; +} + +/* + get any available mag data + */ +int qflight_get_mag_data(uint8_t *buf, int len) +{ + DSPBuffer::MAG &mag = *(DSPBuffer::MAG *)buf; + + if (len != sizeof(mag)) { + HAP_PRINTF("incorrect size for mag data %d should be %d\n", + len, sizeof(mag)); + return 1; + } + + mpu9250_startup(); + + mag.num_samples = 0; + while (mag.num_samples < mag.max_samples && + mag_buffer.pop(mag.buf[mag.num_samples])) { + mag.num_samples++; + } + + return 0; +} + + +/* + get any available baro data + */ +int qflight_get_baro_data(uint8_t *buf, int len) +{ + DSPBuffer::BARO &baro = *(DSPBuffer::BARO *)buf; + + if (len != sizeof(baro)) { + HAP_PRINTF("incorrect size for baro data %d should be %d\n", + len, sizeof(baro)); + return 1; + } + + mpu9250_startup(); + + if (bmp280_handle == 0) { + if (init_barometer() != 0) { + return 1; + } + } + + baro.num_samples = 0; + while (baro.num_samples < baro.max_samples && + baro_buffer.pop(baro.buf[baro.num_samples])) { + baro.num_samples++; + } + + return 0; +} + +extern "C" { +static void read_callback_trampoline(void *, char *, size_t ); +} + +static void read_callback_trampoline(void *ctx, char *buf, size_t size) +{ + if (size > 0) { + ((ByteBuffer *)ctx)->write((const uint8_t *)buf, size); + } +} + +/* + open a UART + */ +int qflight_UART_open(const char *device, int32_t *_fd) +{ + if (num_open_uarts == max_uarts) { + return -1; + } + struct uartbuf &b = uarts[num_open_uarts]; + int fd = open(device, O_RDWR | O_NONBLOCK); + if (fd == -1) { + return -1; + } + b.fd = fd; + b.readbuffer = new ByteBuffer(16384); + + struct dspal_serial_open_options options; + options.bit_rate = DSPAL_SIO_BITRATE_57600; + options.tx_flow = DSPAL_SIO_FCTL_OFF; + options.rx_flow = DSPAL_SIO_FCTL_OFF; + options.rx_data_callback = nullptr; + options.tx_data_callback = nullptr; + options.is_tx_data_synchronous = false; + int ret = ioctl(fd, SERIAL_IOCTL_OPEN_OPTIONS, (void *)&options); + if (ret != 0) { + HAP_PRINTF("Failed to setup UART flow control options"); + } + + struct dspal_serial_ioctl_receive_data_callback callback {}; + callback.context = b.readbuffer; + callback.rx_data_callback_func_ptr = read_callback_trampoline; + ret = ioctl(fd, SERIAL_IOCTL_SET_RECEIVE_DATA_CALLBACK, (void *)&callback); + if (ret != 0) { + HAP_PRINTF("Failed to setup UART read trampoline"); + delete b.readbuffer; + close(fd); + return -1; + } + + HAP_PRINTF("UART open %s fd=%d num_open=%u", + device, fd, num_open_uarts); + num_open_uarts++; + *_fd = fd; + return 0; +} + +/* + close a UART + */ +int qflight_UART_close(int32_t fd) +{ + uint8_t i; + for (i=0; iread(buf, size); + return 0; +} + +/* + write to a UART + */ +int qflight_UART_write(int32_t fd, const uint8_t *buf, int size, int32_t *nwritten) +{ + *nwritten = write(fd, buf, size); + return 0; +} + +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 }, +}; + +/* + set UART baudrate + */ +int qflight_UART_set_baudrate(int32_t fd, uint32_t baudrate) +{ + for (uint8_t i=0; i %d\n", ret); + return 0; + } + } + return -1; +} diff --git a/libraries/AP_HAL_Linux/qflight/qflight_buffer.h b/libraries/AP_HAL_Linux/qflight/qflight_buffer.h new file mode 100644 index 0000000000..effa27bcf8 --- /dev/null +++ b/libraries/AP_HAL_Linux/qflight/qflight_buffer.h @@ -0,0 +1,38 @@ +#pragma once + +/* + shared memory structures for sensor data and peripheral control on Qualcomm flight board + */ +struct DSPBuffer { + // IMU data + struct IMU { + static const uint32_t max_samples = 10; + uint32_t num_samples; + struct BUF { + uint64_t timestamp; + float accel[3]; + float gyro[3]; + } buf[max_samples]; + } imu; + + // MAG data + struct MAG { + static const uint64_t max_samples = 10; + uint32_t num_samples; + struct BUF { + uint64_t timestamp; + int16_t mag_raw[3]; + } buf[max_samples]; + } mag; + + // baro data + struct BARO { + static const uint32_t max_samples = 10; + uint32_t num_samples; + struct BUF { + uint64_t timestamp; + float pressure_pa; + float temperature_C; + } buf[max_samples]; + } baro; +}; diff --git a/libraries/AP_HAL_Linux/qflight/qflight_dsp.idl b/libraries/AP_HAL_Linux/qflight/qflight_dsp.idl new file mode 100644 index 0000000000..340c2e072d --- /dev/null +++ b/libraries/AP_HAL_Linux/qflight/qflight_dsp.idl @@ -0,0 +1,15 @@ +#include "AEEStdDef.idl" + +interface qflight { + // sensor calls + long get_imu_data(rout sequence outdata); + long get_mag_data(rout sequence outdata); + long get_baro_data(rout sequence outdata); + + // UART control + long UART_open(in string device, rout int32 fd); + long UART_set_baudrate(in int32 fd, in uint32 baudrate); + long UART_read(in int32 fd, rout sequence buf, rout int32 nread); + long UART_write(in int32 fd, in sequence buf, rout int32 nwritten); + long UART_close(in int32 fd); +}; diff --git a/libraries/AP_HAL_Linux/qflight/qflight_util.h b/libraries/AP_HAL_Linux/qflight/qflight_util.h new file mode 100644 index 0000000000..53a24a9524 --- /dev/null +++ b/libraries/AP_HAL_Linux/qflight/qflight_util.h @@ -0,0 +1,3 @@ +#include + +#define QFLIGHT_RPC_ALLOCATE(type) (type *)rpcmem_alloc_def(sizeof(type))