Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-19 14:23:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

HAL_Linux: added qflight board subtype

Browse Source
This commit is contained in:
Andrew Tridgell 2015-12-14 14:19:54 +11:00
parent cfd54c5683
commit dc8a3bbf92
20 changed files with 1098 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h
View File

@ -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;

2
libraries/AP_HAL_Linux/AP_HAL_Linux_Private.h
View File

@ -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"

2
libraries/AP_HAL_Linux/GPIO_Sysfs.cpp
View File

@ -2,7 +2,7 @@
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX && CONFIG_HAL_BOARD_SUBTYPE != HAL_BOARD_SUBTYPE_LINUX_QFLIGHT
#include <fcntl.h>
#include <linux/limits.h>

32
libraries/AP_HAL_Linux/HAL_Linux_Class.cpp
View File

@ -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;

62
libraries/AP_HAL_Linux/RCInput.cpp
View File

@ -16,7 +16,7 @@
#include "RCInput.h"
#include "sbus.h"
#include "dsm.h"
#include <AP_HAL/utility/dsm.h>
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_values; i++) {
if (values[i] != 0) {
_pwm_values[i] = values[i];
}
}
/*
the apparent number of channels can change on DSM,
as they are spread across multiple frames. We just
use the max num_values we get
*/
if (num_values > _num_channels) {
_num_channels = num_values;
}
new_rc_input = true;
#if 0
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

11
libraries/AP_HAL_Linux/RCInput.h
View File

@ -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"

74
libraries/AP_HAL_Linux/RCInput_DSM.cpp Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
/*
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 <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT
#include "RCInput_DSM.h"
#include <AP_HAL_Linux/qflight/qflight_util.h>
#include <AP_HAL_Linux/qflight/qflight_dsp.h>
#include <stdio.h>
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

36
libraries/AP_HAL_Linux/RCInput_DSM.h Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
#pragma once
#include <AP_HAL/AP_HAL.h>
#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

176
libraries/AP_HAL_Linux/RCOutput_qflight.cpp Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
/*
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 <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT
#include "RCOutput_qflight.h"
#include <AP_HAL_Linux/qflight/qflight_util.h>
#include <AP_HAL_Linux/qflight/qflight_dsp.h>
#include <GCS_MAVLink/include/mavlink/v1.0/checksum.h>
#include <stdio.h>
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<<ch;
}
void RCOutput_QFLIGHT::disable_ch(uint8_t ch)
{
if (ch >= channel_count) {
return;
}
enable_mask &= ~1U<<ch;
}
void RCOutput_QFLIGHT::write(uint8_t ch, uint16_t period_us)
{
if (ch >= 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; i<nrcin_bytes; i++) {
if (rcu.bytes[i] == magic) {
memmove(&rcu.bytes[0], &rcu.bytes[i], nrcin_bytes-i);
nrcin_bytes = nrcin_bytes - i;
return;
}
}
nrcin_bytes = 0;
return;
}
}
if (nrcin_bytes == sizeof(rcu.bytes)) {
if (rcu.rcin.magic == 0xf6 &&
crc_calculate((uint8_t*)rcu.rcin.rcin, sizeof(rcu.rcin.rcin)) == rcu.rcin.crc) {
bool have_data = false;
for (uint8_t i=0; i<8; i++) {
if (rcu.rcin.rcin[i] != 0) {
have_data = true;
break;
}
}
if (have_data) {
hal.rcin->set_overrides((int16_t*)rcu.rcin.rcin, 8);
}
}
nrcin_bytes = 0;
}
}
#endif // CONFIG_HAL_BOARD_SUBTYPE

47
libraries/AP_HAL_Linux/RCOutput_qflight.h Normal file
View File

@ -0,0 +1,47 @@
#pragma once
#include <AP_HAL/AP_HAL.h>
#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

62
libraries/AP_HAL_Linux/Scheduler.cpp
View File

@ -16,6 +16,14 @@
#include <errno.h>
#include <sys/mman.h>
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT
#include <rpcmem.h>
#include <AP_HAL_Linux/qflight/qflight_util.h>
#include <AP_HAL_Linux/qflight/qflight_dsp.h>
#include <AP_HAL_Linux/qflight/qflight_buffer.h>
#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;
}

1
libraries/AP_HAL_Linux/Scheduler.h
View File

@ -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);

25
libraries/AP_HAL_Linux/UARTDriver.cpp
View File

@ -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
*/

4
libraries/AP_HAL_Linux/UARTDriver.h
View File

@ -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
};

104
libraries/AP_HAL_Linux/UARTQFlight.cpp Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
/*
This is a UART driver for the QFLIGHT port. Actual UART output
happens via RPC calls. See the qflight/ subdirectory for details
*/
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT
#include <stdio.h>
#include <unistd.h>
#include "UARTQFlight.h"
#include <AP_HAL_Linux/qflight/qflight_util.h>
#include <AP_HAL_Linux/qflight/qflight_dsp.h>
#include <stdio.h>
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

25
libraries/AP_HAL_Linux/UARTQFlight.h Normal file
View File

@ -0,0 +1,25 @@
#pragma once
#include <AP_HAL/AP_HAL.h>
#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

395
libraries/AP_HAL_Linux/qflight/dsp_functions.cpp Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
/*
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 <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include "qflight_dsp.h"
extern "C" {
#include "bmp280_api.h"
#include "mpu9x50.h"
}
#include <types.h>
#include <fcntl.h>
#include <unistd.h>
#include <stdbool.h>
#include <stdint.h>
#include <stdarg.h>
#include <sys/timespec.h>
#include <errno.h>
#include <string.h>
#include <time.h>
#include <dspal_time.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <dirent.h>
#include <stdlib.h>
#include <dev_fs_lib_serial.h>
#include "qflight_buffer.h"
#include <AP_HAL/utility/RingBuffer.h>
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<DSPBuffer::IMU::BUF> imu_buffer(30);
static ObjectBuffer<DSPBuffer::MAG::BUF> mag_buffer(10);
static ObjectBuffer<DSPBuffer::BARO::BUF> 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; i<num_open_uarts; i++) {
if (fd == uarts[i].fd) break;
}
if (i == num_open_uarts) {
return -1;
}
close(fd);
delete uarts[i].readbuffer;
if (i < num_open_uarts-1) {
memmove(&uarts[i], &uarts[i+1], ((num_open_uarts-1)-i)*sizeof(uarts[0]));
}
num_open_uarts--;
return 0;
}
/*
read from a UART
*/
int qflight_UART_read(int32_t fd, uint8_t *buf, int size, int32_t *nread)
{
uint8_t i;
for (i=0; i<num_open_uarts; i++) {
if (fd == uarts[i].fd) break;
}
if (i == num_open_uarts) {
return -1;
}
*nread = uarts[i].readbuffer->read(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<sizeof(baudrate_table)/sizeof(baudrate_table[0]); i++) {
if (baudrate <= baudrate_table[i].baudrate) {
struct dspal_serial_ioctl_data_rate rate {};
rate.bit_rate = baudrate_table[i].arg;
int ret = ioctl(fd, SERIAL_IOCTL_SET_DATA_RATE, (void *)&rate);
HAP_PRINTF("set_rate -> %d\n", ret);
return 0;
}
}
return -1;
}

38
libraries/AP_HAL_Linux/qflight/qflight_buffer.h Normal file
View File

@ -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;
};

15
libraries/AP_HAL_Linux/qflight/qflight_dsp.idl Normal file
View File

@ -0,0 +1,15 @@
#include "AEEStdDef.idl"
interface qflight {
// sensor calls
long get_imu_data(rout sequence<uint8> outdata);
long get_mag_data(rout sequence<uint8> outdata);
long get_baro_data(rout sequence<uint8> 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<uint8> buf, rout int32 nread);
long UART_write(in int32 fd, in sequence<uint8> buf, rout int32 nwritten);
long UART_close(in int32 fd);
};

3
libraries/AP_HAL_Linux/qflight/qflight_util.h Normal file
View File

@ -0,0 +1,3 @@
#include <rpcmem.h>
#define QFLIGHT_RPC_ALLOCATE(type) (type *)rpcmem_alloc_def(sizeof(type))