mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-06 16:03:58 -04:00
AP_HAL_Linux: remove Qualcomm board support
This commit is contained in:
Francisco Ferreira
Andrew Tridgell
parent
695d92ddbf
commit
5e821428a1
@ -2,8 +2,6 @@
|
|||||||
|
|
||||||
#include <AP_HAL/AP_HAL.h>
|
#include <AP_HAL/AP_HAL.h>
|
||||||
|
|
||||||
#if CONFIG_HAL_BOARD_SUBTYPE != HAL_BOARD_SUBTYPE_LINUX_QFLIGHT
|
|
||||||
|
|
||||||
#include <fcntl.h>
|
#include <fcntl.h>
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
#include <sys/stat.h>
|
#include <sys/stat.h>
|
||||||
@ -244,5 +242,3 @@ fail_snprintf:
|
|||||||
hal.console->printf("GPIO_Sysfs: Unable to export pin %u.\n", pin);
|
hal.console->printf("GPIO_Sysfs: Unable to export pin %u.\n", pin);
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
|
||||||
|
|||||||
@ -21,7 +21,6 @@
|
|||||||
#include "OpticalFlow_Onboard.h"
|
#include "OpticalFlow_Onboard.h"
|
||||||
#include "RCInput.h"
|
#include "RCInput.h"
|
||||||
#include "RCInput_AioPRU.h"
|
#include "RCInput_AioPRU.h"
|
||||||
#include "RCInput_DSM.h"
|
|
||||||
#include "RCInput_Navio2.h"
|
#include "RCInput_Navio2.h"
|
||||||
#include "RCInput_PRU.h"
|
#include "RCInput_PRU.h"
|
||||||
#include "RCInput_RPI.h"
|
#include "RCInput_RPI.h"
|
||||||
@ -153,8 +152,6 @@ static RCInput_ZYNQ rcinDriver;
|
|||||||
static RCInput_UDP rcinDriver;
|
static RCInput_UDP rcinDriver;
|
||||||
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_MINLURE
|
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_MINLURE
|
||||||
static RCInput_UART rcinDriver("/dev/ttyS5");
|
static RCInput_UART rcinDriver("/dev/ttyS5");
|
||||||
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT
|
|
||||||
static RCInput_DSM rcinDriver;
|
|
||||||
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO
|
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO
|
||||||
static RCInput_Multi rcinDriver{2, new RCInput_SBUS, new RCInput_115200("/dev/uart-sumd")};
|
static RCInput_Multi rcinDriver{2, new RCInput_SBUS, new RCInput_115200("/dev/uart-sumd")};
|
||||||
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_AERO
|
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_AERO
|
||||||
@ -201,8 +198,6 @@ static RCOutput_PCA9685 rcoutDriver(i2c_mgr_instance.get_device(
|
|||||||
/* UEFI with lpss set to PCI */
|
/* UEFI with lpss set to PCI */
|
||||||
"pci0000:00/0000:00:18.6" },
|
"pci0000:00/0000:00:18.6" },
|
||||||
PCA9685_PRIMARY_ADDRESS), false, 0, MINNOW_GPIO_S5_1);
|
PCA9685_PRIMARY_ADDRESS), false, 0, MINNOW_GPIO_S5_1);
|
||||||
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT
|
|
||||||
static RCOutput_QFLIGHT rcoutDriver;
|
|
||||||
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO
|
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO
|
||||||
static RCOutput_Disco rcoutDriver(i2c_mgr_instance.get_device(HAL_RCOUT_DISCO_BLDC_I2C_BUS, HAL_RCOUT_DISCO_BLDC_I2C_ADDR));
|
static RCOutput_Disco rcoutDriver(i2c_mgr_instance.get_device(HAL_RCOUT_DISCO_BLDC_I2C_BUS, HAL_RCOUT_DISCO_BLDC_I2C_ADDR));
|
||||||
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO2
|
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO2
|
||||||
@ -291,10 +286,6 @@ void HAL_Linux::run(int argc, char* const argv[], Callbacks* callbacks) const
|
|||||||
{"uartD", true, 0, 'D'},
|
{"uartD", true, 0, 'D'},
|
||||||
{"uartE", true, 0, 'E'},
|
{"uartE", true, 0, 'E'},
|
||||||
{"uartF", true, 0, 'F'},
|
{"uartF", true, 0, 'F'},
|
||||||
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT
|
|
||||||
{"dsm", true, 0, 'S'},
|
|
||||||
{"ESC", true, 0, 'e'},
|
|
||||||
#endif
|
|
||||||
{"log-directory", true, 0, 'l'},
|
{"log-directory", true, 0, 'l'},
|
||||||
{"terrain-directory", true, 0, 't'},
|
{"terrain-directory", true, 0, 't'},
|
||||||
{"module-directory", true, 0, 'M'},
|
{"module-directory", true, 0, 'M'},
|
||||||
@ -328,14 +319,6 @@ void HAL_Linux::run(int argc, char* const argv[], Callbacks* callbacks) const
|
|||||||
case 'F':
|
case 'F':
|
||||||
uartFDriver.set_device_path(gopt.optarg);
|
uartFDriver.set_device_path(gopt.optarg);
|
||||||
break;
|
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':
|
case 'l':
|
||||||
utilInstance.set_custom_log_directory(gopt.optarg);
|
utilInstance.set_custom_log_directory(gopt.optarg);
|
||||||
break;
|
break;
|
||||||
|
|||||||
@ -1,73 +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 <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
|
|
||||||
|
|
||||||
@ -1,41 +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 <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"
|
|
||||||
|
|
||||||
namespace Linux {
|
|
||||||
|
|
||||||
class RCInput_DSM : public 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
|
|
||||||
@ -1,198 +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 <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>
|
|
||||||
|
|
||||||
#include <RC_Channel/RC_Channel.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/v2.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;
|
|
||||||
if (!corked) {
|
|
||||||
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) {
|
|
||||||
// FIXME: This is an incredibly dirty hack as this probhibits the usage of
|
|
||||||
// overrides if an RC reciever is connected, as the next RC input will
|
|
||||||
// stomp over the GCS set overrides. This results in incredibly confusing,
|
|
||||||
// undocumented behaviour, that cannot be reported to the user.
|
|
||||||
for (uint8_t i = 0; i < 8; i++) {
|
|
||||||
RC_Channels::set_override(i, rcu.rcin.rcin[i]);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
nrcin_bytes = 0;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void RCOutput_QFLIGHT::cork(void)
|
|
||||||
{
|
|
||||||
corked = true;
|
|
||||||
}
|
|
||||||
|
|
||||||
void RCOutput_QFLIGHT::push(void)
|
|
||||||
{
|
|
||||||
if (corked) {
|
|
||||||
corked = false;
|
|
||||||
need_write = true;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
#endif // CONFIG_HAL_BOARD_SUBTYPE
|
|
||||||
|
|
||||||
@ -1,54 +0,0 @@
|
|||||||
#pragma once
|
|
||||||
|
|
||||||
#include <AP_HAL/AP_HAL.h>
|
|
||||||
#include "AP_HAL_Linux.h"
|
|
||||||
|
|
||||||
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT
|
|
||||||
|
|
||||||
namespace Linux {
|
|
||||||
|
|
||||||
class 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);
|
|
||||||
void cork(void) override;
|
|
||||||
void push(void) override;
|
|
||||||
|
|
||||||
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;
|
|
||||||
bool corked;
|
|
||||||
};
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
#endif // CONFIG_HAL_BOARD_SUBTYPE
|
|
||||||
@ -18,13 +18,6 @@
|
|||||||
#include "UARTDriver.h"
|
#include "UARTDriver.h"
|
||||||
#include "Util.h"
|
#include "Util.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
|
|
||||||
|
|
||||||
#if HAL_WITH_UAVCAN
|
#if HAL_WITH_UAVCAN
|
||||||
#include "CAN.h"
|
#include "CAN.h"
|
||||||
#endif
|
#endif
|
||||||
@ -257,16 +250,6 @@ void Scheduler::_timer_task()
|
|||||||
|
|
||||||
_in_timer_proc = false;
|
_in_timer_proc = false;
|
||||||
|
|
||||||
#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
|
|
||||||
|
|
||||||
#if HAL_WITH_UAVCAN
|
#if HAL_WITH_UAVCAN
|
||||||
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
|
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
|
||||||
for (i = 0; i < MAX_NUMBER_OF_CAN_INTERFACES; i++) {
|
for (i = 0; i < MAX_NUMBER_OF_CAN_INTERFACES; i++) {
|
||||||
@ -310,16 +293,12 @@ void Scheduler::_run_uarts()
|
|||||||
|
|
||||||
void Scheduler::_rcin_task()
|
void Scheduler::_rcin_task()
|
||||||
{
|
{
|
||||||
#if !HAL_LINUX_UARTS_ON_TIMER_THREAD
|
|
||||||
RCInput::from(hal.rcin)->_timer_tick();
|
RCInput::from(hal.rcin)->_timer_tick();
|
||||||
#endif
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void Scheduler::_uart_task()
|
void Scheduler::_uart_task()
|
||||||
{
|
{
|
||||||
#if !HAL_LINUX_UARTS_ON_TIMER_THREAD
|
|
||||||
_run_uarts();
|
_run_uarts();
|
||||||
#endif
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void Scheduler::_tonealarm_task()
|
void Scheduler::_tonealarm_task()
|
||||||
@ -376,14 +355,6 @@ void Scheduler::stop_clock(uint64_t time_usec)
|
|||||||
|
|
||||||
bool Scheduler::SchedulerThread::_run()
|
bool Scheduler::SchedulerThread::_run()
|
||||||
{
|
{
|
||||||
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT
|
|
||||||
if (_sched._timer_thread.is_current_thread()) {
|
|
||||||
/* make rpcmem initialization on timer thread */
|
|
||||||
printf("Initialising rpcmem\n");
|
|
||||||
rpcmem_init();
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
_sched._wait_all_threads();
|
_sched._wait_all_threads();
|
||||||
|
|
||||||
return PeriodicThread::_run();
|
return PeriodicThread::_run();
|
||||||
|
|||||||
@ -22,7 +22,6 @@
|
|||||||
#include "ConsoleDevice.h"
|
#include "ConsoleDevice.h"
|
||||||
#include "TCPServerDevice.h"
|
#include "TCPServerDevice.h"
|
||||||
#include "UARTDevice.h"
|
#include "UARTDevice.h"
|
||||||
#include "UARTQFlight.h"
|
|
||||||
#include "UDPDevice.h"
|
#include "UDPDevice.h"
|
||||||
|
|
||||||
#include <GCS_MAVLink/GCS.h>
|
#include <GCS_MAVLink/GCS.h>
|
||||||
@ -129,10 +128,6 @@ AP_HAL::OwnPtr<SerialDevice> UARTDriver::_parseDevicePath(const char *arg)
|
|||||||
|
|
||||||
if (stat(arg, &st) == 0 && S_ISCHR(st.st_mode)) {
|
if (stat(arg, &st) == 0 && S_ISCHR(st.st_mode)) {
|
||||||
return AP_HAL::OwnPtr<SerialDevice>(new UARTDevice(arg));
|
return AP_HAL::OwnPtr<SerialDevice>(new UARTDevice(arg));
|
||||||
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT
|
|
||||||
} else if (strncmp(arg, "qflight:", 8) == 0) {
|
|
||||||
return AP_HAL::OwnPtr<SerialDevice>(new QFLIGHTDevice(device_path));
|
|
||||||
#endif
|
|
||||||
} else if (strncmp(arg, "tcp:", 4) != 0 &&
|
} else if (strncmp(arg, "tcp:", 4) != 0 &&
|
||||||
strncmp(arg, "udp:", 4) != 0 &&
|
strncmp(arg, "udp:", 4) != 0 &&
|
||||||
strncmp(arg, "udpin:", 6)) {
|
strncmp(arg, "udpin:", 6)) {
|
||||||
|
|||||||
@ -1,103 +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 <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
|
|
||||||
|
|
||||||
@ -1,25 +0,0 @@
|
|||||||
#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
|
|
||||||
@ -1,118 +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 QFLIGHT port. See the AP_HAL_QURT directory
|
|
||||||
for information on the QURT port.
|
|
||||||
|
|
||||||
# Building ArduPilot for 'QFLIGHT'
|
|
||||||
|
|
||||||
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 qflight -j4
|
|
||||||
```
|
|
||||||
|
|
||||||
you can then upload the firmware to your board by joining to the WiFi
|
|
||||||
network of the board and doing this
|
|
||||||
|
|
||||||
```
|
|
||||||
make qflight_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/libqflight_skel.so
|
|
||||||
```
|
|
||||||
|
|
||||||
To start ArduPilot just run the elf file as root on the flight
|
|
||||||
board. You can control UART output with command line options. A
|
|
||||||
typical startup command would be:
|
|
||||||
|
|
||||||
```
|
|
||||||
/root/ArduCopter.elf -A udp:192.168.1.255:14550:bcast -e /dev/tty-3 -B qflight:/dev/tty-2 --dsm /dev/tty-4
|
|
||||||
```
|
|
||||||
|
|
||||||
That will start ArduPilot with telemetry over UDP on port 14550, GPS
|
|
||||||
on tty-2 on the DSPs, Skektrum satellite RC input on tty-4 and
|
|
||||||
ESC output on tty-3.
|
|
||||||
|
|
||||||
Then you can open your favourite MAVLink compatible GCS and connect
|
|
||||||
with UDP.
|
|
||||||
|
|
||||||
# Logging
|
|
||||||
|
|
||||||
Logs will appear in /var/APM/logs as usual for Linux ArduPilot
|
|
||||||
ports. You can download logs over MAVLink or transfer over WiFi.
|
|
||||||
|
|
||||||
# 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.
|
|
||||||
|
|
||||||
# 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.
|
|
||||||
|
|
||||||
Note that you can also use RC input from that attached board, allowing
|
|
||||||
you to use any ArduPilot compatible RC receiver.
|
|
||||||
@ -1,393 +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 <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 <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 nullptr;
|
|
||||||
}
|
|
||||||
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 nullptr;
|
|
||||||
}
|
|
||||||
|
|
||||||
static void mpu9250_startup(void)
|
|
||||||
{
|
|
||||||
if (!mpu9250_started) {
|
|
||||||
if (init_mpu9250() != 0) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
mpu9x50_register_interrupt(65, mpu_data_ready, nullptr);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/*
|
|
||||||
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|O_CLOEXEC);
|
|
||||||
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;
|
|
||||||
}
|
|
||||||
@ -1,38 +0,0 @@
|
|||||||
#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;
|
|
||||||
};
|
|
||||||
@ -1,15 +0,0 @@
|
|||||||
#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);
|
|
||||||
};
|
|
||||||
@ -1,3 +0,0 @@
|
|||||||
#include <rpcmem.h>
|
|
||||||
|
|
||||||
#define QFLIGHT_RPC_ALLOCATE(type) (type *)rpcmem_alloc_def(sizeof(type))
|
|
||||||
Loading…
Reference in New Issue
Block a user