Francisco Ferreira
7 years ago
committed by
Andrew Tridgell
15 changed files with 0 additions and 1116 deletions
@ -1,73 +0,0 @@
@@ -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 @@
@@ -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 @@
@@ -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 @@
@@ -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
|
||||
@ -1,103 +0,0 @@
@@ -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 @@
@@ -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 @@
@@ -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 @@
@@ -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 @@
@@ -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 @@
@@ -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); |
||||
}; |
||||
Loading…
Reference in new issue