Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_HAL_QURT: remove Qualcomm board support

This commit is contained in:
Francisco Ferreira 2018-05-11 13:26:52 +01:00 committed by Andrew Tridgell
parent 5e821428a1
commit 6f210131ed
30 changed files with 0 additions and 2679 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
libraries/AP_HAL_QURT/AP_HAL_QURT.h
View File

@ -1,25 +0,0 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
/* Your layer exports should depend on AP_HAL.h ONLY. */
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_QURT
#include "HAL_QURT_Class.h"
#include "AP_HAL_QURT_Main.h"
#endif // CONFIG_HAL_BOARD

15
libraries/AP_HAL_QURT/AP_HAL_QURT_Main.h
View File

@ -1,15 +0,0 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once

29
libraries/AP_HAL_QURT/AP_HAL_QURT_Namespace.h
View File

@ -1,29 +0,0 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
namespace QURT {
class UARTDriver;
class UDPDriver;
class Util;
class Scheduler;
class Storage;
class Util;
class Semaphore;
class RCInput;
class RCOutput;
}

23
libraries/AP_HAL_QURT/AP_HAL_QURT_Private.h
View File

@ -1,23 +0,0 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
/* Umbrella header for all private headers of the AP_HAL_QURT module.
* Only import this header from inside AP_HAL_QURT
*/
#include "UARTDriver.h"
#include "UDPDriver.h"
#include "Util.h"

144
libraries/AP_HAL_QURT/HAL_QURT_Class.cpp
View File

@ -1,144 +0,0 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_QURT
#include "HAL_QURT_Class.h"
#include "AP_HAL_QURT_Private.h"
#include "Scheduler.h"
#include "Storage.h"
#include "Semaphores.h"
#include "RCInput.h"
#include "RCOutput.h"
#include <AP_HAL_Empty/AP_HAL_Empty.h>
#include <AP_HAL_Empty/AP_HAL_Empty_Private.h>
#include <AP_HAL/utility/getopt_cpp.h>
#include <assert.h>
using namespace QURT;
static UDPDriver uartADriver;
static UARTDriver uartBDriver("/dev/tty-4");
static UARTDriver uartCDriver("/dev/tty-2");
static UARTDriver uartDDriver(nullptr);
static UARTDriver uartEDriver(nullptr);
static Empty::SPIDeviceManager spiDeviceManager;
static Empty::AnalogIn analogIn;
static Storage storageDriver;
static Empty::GPIO gpioDriver;
static RCInput rcinDriver("/dev/tty-1");
static RCOutput rcoutDriver("/dev/tty-3");
static Util utilInstance;
static Scheduler schedulerInstance;
static Empty::I2CDeviceManager i2c_mgr_instance;
bool qurt_ran_overtime;
HAL_QURT::HAL_QURT() :
AP_HAL::HAL(
&uartADriver,
&uartBDriver,
&uartCDriver,
&uartDDriver,
&uartEDriver,
nullptr, // uartF
&i2c_mgr_instance,
&spiDeviceManager,
&analogIn,
&storageDriver,
&uartADriver,
&gpioDriver,
&rcinDriver,
&rcoutDriver,
&schedulerInstance,
&utilInstance,
nullptr)
{
}
void HAL_QURT::run(int argc, char* const argv[], Callbacks* callbacks) const
{
assert(callbacks);
int opt;
const struct GetOptLong::option options[] = {
{"uartB", true, 0, 'B'},
{"uartC", true, 0, 'C'},
{"uartD", true, 0, 'D'},
{"uartE", true, 0, 'E'},
{"dsm", true, 0, 'S'},
{"ESC", true, 0, 'e'},
{0, false, 0, 0}
};
GetOptLong gopt(argc, argv, "B:C:D:E:e:S",
options);
/*
parse command line options
*/
while ((opt = gopt.getoption()) != -1) {
switch (opt) {
case 'B':
uartBDriver.set_device_path(gopt.optarg);
break;
case 'C':
uartCDriver.set_device_path(gopt.optarg);
break;
case 'D':
uartDDriver.set_device_path(gopt.optarg);
break;
case 'E':
uartEDriver.set_device_path(gopt.optarg);
break;
case 'e':
rcoutDriver.set_device_path(gopt.optarg);
break;
case 'S':
rcinDriver.set_device_path(gopt.optarg);
break;
default:
printf("Unknown option '%c'\n", (char)opt);
exit(1);
}
}
/* initialize all drivers and private members here.
* up to the programmer to do this in the correct order.
* Scheduler should likely come first. */
scheduler->init();
schedulerInstance.hal_initialized();
uartA->begin(115200);
rcinDriver.init();
callbacks->setup();
scheduler->system_initialized();
for (;;) {
callbacks->loop();
}
}
const AP_HAL::HAL& AP_HAL::get_HAL() {
static const HAL_QURT *hal;
if (hal == nullptr) {
hal = new HAL_QURT;
HAP_PRINTF("allocated HAL_QURT of size %u", sizeof(*hal));
}
return *hal;
}
#endif

25
libraries/AP_HAL_QURT/HAL_QURT_Class.h
View File

@ -1,25 +0,0 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include <AP_HAL/AP_HAL.h>
#include "AP_HAL_QURT_Namespace.h"
class HAL_QURT : public AP_HAL::HAL {
public:
HAL_QURT();
void run(int argc, char* const* argv, Callbacks* callbacks) const override;
};

184
libraries/AP_HAL_QURT/RCInput.cpp
View File

@ -1,184 +0,0 @@
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_QURT
#include <stdio.h>
#include <sys/time.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <errno.h>
#include <unistd.h>
#include <fcntl.h>
#include <stdint.h>
#include <dev_fs_lib_serial.h>
#include "RCInput.h"
#include <AP_HAL/utility/dsm.h>
extern const AP_HAL::HAL& hal;
using namespace QURT;
RCInput::RCInput(const char *_device_path) :
device_path(_device_path),
new_rc_input(false)
{
}
extern "C" {
static void read_callback_trampoline(void *, char *, size_t );
}
void RCInput::init()
{
if (device_path == nullptr) {
return;
}
fd = open(device_path, O_RDONLY|O_NONBLOCK);
if (fd == -1) {
AP_HAL::panic("Unable to open RC input %s", device_path);
}
struct dspal_serial_ioctl_data_rate rate;
rate.bit_rate = DSPAL_SIO_BITRATE_115200;
int ret = ioctl(fd, SERIAL_IOCTL_SET_DATA_RATE, (void *)&rate);
struct dspal_serial_ioctl_receive_data_callback callback;
callback.context = this;
callback.rx_data_callback_func_ptr = read_callback_trampoline;
ret = ioctl(fd, SERIAL_IOCTL_SET_RECEIVE_DATA_CALLBACK, (void *)&callback);
}
static void read_callback_trampoline(void *ctx, char *buf, size_t size)
{
((RCInput *)ctx)->read_callback(buf, size);
}
/*
callback for incoming data
*/
void RCInput::read_callback(char *buf, size_t size)
{
add_dsm_input((const uint8_t *)buf, size);
}
bool RCInput::new_input()
{
bool ret = new_rc_input;
if (ret) {
new_rc_input = false;
}
return ret;
}
uint8_t RCInput::num_channels()
{
return _num_channels;
}
uint16_t RCInput::read(uint8_t ch)
{
if (_override[ch]) {
return _override[ch];
}
if (ch >= _num_channels) {
return 0;
}
return _pwm_values[ch];
}
uint8_t RCInput::read(uint16_t* periods, uint8_t len)
{
uint8_t i;
for (i=0; i<len; i++) {
if((periods[i] = read(i))){
continue;
}
else{
break;
}
}
return (i+1);
}
bool RCInput::set_override(uint8_t channel, int16_t override)
{
if (override < 0) return false; /* -1: no change. */
if (channel < QURT_RC_INPUT_NUM_CHANNELS) {
_override[channel] = override;
if (override != 0) {
new_rc_input = true;
return true;
}
}
return false;
}
void RCInput::clear_overrides()
{
for (uint8_t i = 0; i < QURT_RC_INPUT_NUM_CHANNELS; i++) {
_override[i] = 0;
}
}
/*
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
HAP_PRINTF("Decoded DSM %u channels %u %u %u %u %u %u %u %u\n",
(unsigned)num_values,
values[0], values[1], values[2], values[3], values[4], values[5], values[6], values[7]);
#endif
}
}
}
}
#endif // CONFIG_HAL_BOARD

52
libraries/AP_HAL_QURT/RCInput.h
View File

@ -1,52 +0,0 @@
#pragma once
#include "AP_HAL_QURT.h"
#define QURT_RC_INPUT_NUM_CHANNELS 16
class QURT::RCInput : public AP_HAL::RCInput {
public:
RCInput(const char *device_path);
static RCInput *from(AP_HAL::RCInput *rcinput) {
return static_cast<RCInput*>(rcinput);
}
void set_device_path(const char *path) {
device_path = path;
}
void init();
bool new_input();
uint8_t num_channels();
uint16_t read(uint8_t ch);
uint8_t read(uint16_t* periods, uint8_t len);
bool set_override(uint8_t channel, int16_t override);
void clear_overrides();
void read_callback(char *buf, size_t size);
private:
volatile bool new_rc_input;
uint16_t _pwm_values[QURT_RC_INPUT_NUM_CHANNELS];
uint8_t _num_channels;
/* override state */
uint16_t _override[QURT_RC_INPUT_NUM_CHANNELS];
// add some DSM input bytes, for RCInput over a serial port
void add_dsm_input(const uint8_t *bytes, size_t nbytes);
const char *device_path;
int32_t fd = -1;
// state of add_dsm_input
struct {
uint8_t frame[16];
uint8_t partial_frame_count;
uint32_t last_input_ms;
} dsm;
};

126
libraries/AP_HAL_QURT/RCOutput.cpp
View File

@ -1,126 +0,0 @@
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_QURT
#include "RCOutput.h"
#include <GCS_MAVLink/include/mavlink/v2.0/checksum.h>
#include <stdio.h>
#include <fcntl.h>
#include <unistd.h>
#include <dev_fs_lib_serial.h>
extern const AP_HAL::HAL& hal;
using namespace QURT;
void RCOutput::init()
{
}
void RCOutput::set_freq(uint32_t chmask, uint16_t freq_hz)
{
// no support for changing frequency yet
}
uint16_t RCOutput::get_freq(uint8_t ch)
{
// return fixed fake value
return 490;
}
void RCOutput::enable_ch(uint8_t ch)
{
if (ch >= channel_count) {
return;
}
enable_mask |= 1U<<ch;
}
void RCOutput::disable_ch(uint8_t ch)
{
if (ch >= channel_count) {
return;
}
enable_mask &= ~1U<<ch;
}
void RCOutput::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::read(uint8_t ch)
{
if (ch >= channel_count) {
return 0;
}
return period[ch];
}
void RCOutput::read(uint16_t *period_us, uint8_t len)
{
for (int i = 0; i < len; i++) {
period_us[i] = read(i);
}
}
extern "C" {
// discard incoming data
static void read_callback_trampoline(void *, char *, size_t ) {}
}
void RCOutput::timer_update(void)
{
if (fd == -1 && device_path != nullptr) {
HAP_PRINTF("Opening RCOutput %s", device_path);
fd = open(device_path, O_RDWR|O_NONBLOCK);
if (fd == -1) {
AP_HAL::panic("Unable to open %s", device_path);
}
HAP_PRINTF("Opened ESC UART %s fd=%d\n", device_path, fd);
if (fd != -1) {
struct dspal_serial_ioctl_data_rate rate;
rate.bit_rate = DSPAL_SIO_BITRATE_115200;
ioctl(fd, SERIAL_IOCTL_SET_DATA_RATE, (void *)&rate);
struct dspal_serial_ioctl_receive_data_callback callback;
callback.context = this;
callback.rx_data_callback_func_ptr = read_callback_trampoline;
ioctl(fd, SERIAL_IOCTL_SET_RECEIVE_DATA_CALLBACK, (void *)&callback);
}
}
if (!need_write || fd == -1) {
return;
}
struct PACKED {
uint8_t magic = 0xF7;
uint16_t period[channel_count];
uint16_t crc;
} frame;
memcpy(frame.period, period, sizeof(period));
frame.crc = crc_calculate((uint8_t*)frame.period, channel_count*2);
need_write = false;
::write(fd, (uint8_t *)&frame, sizeof(frame));
}
void RCOutput::cork(void)
{
corked = true;
}
void RCOutput::push(void)
{
if (corked) {
need_write = true;
corked = false;
}
}
#endif // CONFIG_HAL_BOARD_SUBTYPE

44
libraries/AP_HAL_QURT/RCOutput.h
View File

@ -1,44 +0,0 @@
#pragma once
#include <AP_HAL/AP_HAL.h>
#include "AP_HAL_QURT.h"
#if CONFIG_HAL_BOARD == HAL_BOARD_QURT
class QURT::RCOutput : public AP_HAL::RCOutput {
public:
RCOutput(const char *_device_path) {
device_path = _device_path;
}
void set_device_path(const char *path) {
device_path = path;
}
void init();
void set_freq(uint32_t chmask, uint16_t freq_hz);
uint16_t get_freq(uint8_t ch);
void enable_ch(uint8_t ch);
void disable_ch(uint8_t ch);
void write(uint8_t ch, uint16_t period_us);
uint16_t read(uint8_t ch);
void read(uint16_t *period_us, uint8_t len);
void cork(void) override;
void push(void) override;
void timer_update(void);
private:
const char *device_path;
const uint32_t baudrate = 115200;
static const uint8_t channel_count = 4;
int fd = -1;
uint16_t enable_mask;
uint16_t period[channel_count];
volatile bool need_write;
bool corked;
};
#endif // CONFIG_HAL_BOARD

138
libraries/AP_HAL_QURT/README.md
View File

@ -1,138 +0,0 @@
# ArduPilot on Qualcomm Flight
This is a port of ArduPilot to the Qualcomm Flight development board:
http://shop.intrinsyc.com/products/snapdragon-flight-dev-kit
This board is interesting because it is small but offers a lot of CPU
power and two on-board cameras.
The board has 4 'Krait' ARM cores which run Linux (by default Ubuntu
14.04 Trusty), plus 3 'Hexagon' DSP cores which run the QURT RTOS.
There are two ports of ArduPilot to this board. One is called
'HAL_QURT' and runs primarily on the DSPs, with just a small shim on
the ARM cores. The other is a HAL_Linux subtype called 'QFLIGHT' which
runs mostly on the ARM cores, with just sensor and UARTs on the DSPs.
This is the readme for the QURT port. See the AP_HAL_Linux/qflight
directory for information on the QFLIGHT port.
# Building ArduPilot
Due to some rather unusual licensing terms from Intrinsyc we cannot
distribute binaries of ArduPilot (or any program built with the
Qualcomm libraries). So you will have to build the firmware yourself.
To build ArduPilot you will need 3 library packages from
Intrinsyc. They are:
* the HEXAGON_Tools package, tested with version 7.2.11
* the Hexagon_SDK packet, version 2.0
* the HexagonFCAddon package, tested with Flight_BSP_1.1_ES3_003.2
These packages should all be unpacked in a $HOME/Qualcomm directory.
To build APM:Copter you then do:
```
cd ArduCopter
make qurt -j4
```
you can then upload the firmware to your board by joining to the WiFi
network of the board and doing this
```
make qurt_send FLIGHT_BOARD=myboard
```
where "myboard" is the hostname or IP address of your board.
This will install two files:
```
/root/ArduCopter.elf
/usr/share/data/adsp/libardupilot_skel.so
```
To start ArduPilot just run the elf file as root on the flight board:
```
./ArduCopter.elf
```
By default ArduPilot will send telemetry on UDP 14550 to the local
WiFi network. Just open your favourite MAVLink compatible GCS and
connect with UDP.
You can optionally give command line arguments for the device paths:
```
-B GPS device (default /dev/tty-4)
-C telemetry1 (default /dev/tty-2)
-D telemetry2 (no default)
-E GPS2 (no default)
-e ESC output device (default /dev/tty-3)
-S spektrum input device (default /dev/tty-1)
```
# Logging
To get DataFlash logs you need to create the logs directory like this:
```
mkdir /usr/share/data/adsp/logs
```
Normal ArduPilot dataflash logs will appear in that directory. You
will need to transfer them off your board using scp, ftp, rsync or
Samba.
# UART connections
The Qualcomm Flight board has 4 DF13 6 pin UART connectors. Be careful
though as they do not have the same pinout as the same connectors on a
Pixhawk.
The pinout of them all is:
* pin1: power
* pin2: TX
* pin3: RX
* pin5: GND
3 of the 4 ports provide 3.3V power on pin1, while the 4th port
provides 5V power. Note that pin6 is not ground, unlike on a Pixhawk.
The 4 ports are called /dev/tty-1, /dev/tty-2, /dev/tty-3 and
/dev/tty-4. The first port is the one closest to the USB3
connector. The ports proceed counter-clockwise from there. So tty-2 is
the one closest to the power connector.
Only tty-2 provides 5V power. The others provide 3.3V power. You will
need to check whether your GPS can be powered off 3.3V.
The default assignment of the ports is:
* /dev/tty-1: RC input (Spektrum satellite only)
* /dev/tty-2: telemetry at 57600
* /dev/tty-3: RC output (see below)
* /dev/tty-4: GPS
You can control which device is used for what purpose with the command
line options given above.
This assumes a GPS that can be powered off 3.3V. A uBlox GPS is
recommended, although any ArduPilot compatible serial GPS can be used.
# ESC PWM Output
To get signals to ESCs or servos you need to use a UART. The default
setup is to send 4 PWM signals as serial data on /dev/tty-3. This is
designed to work with this firmware for any ArduPilot compatible
board:
https://github.com/tridge/ardupilot/tree/hal-qurt/libraries/RC_Channel/examples/RC_UART
that firmware will read the UART serial stream and output to the PWM
output of the board you use. For example, you could use a Pixracer or
Pixhawk board. This is an interim solution until Qualcomm/Intrinsyc
release an ESC add-on control board for the Qualcomm Flight.

274
libraries/AP_HAL_QURT/Scheduler.cpp
View File

@ -1,274 +0,0 @@
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_QURT
#include "AP_HAL_QURT.h"
#include "Scheduler.h"
#include <sys/stat.h>
#include <sys/types.h>
#include <unistd.h>
#include <stdlib.h>
#include <sched.h>
#include <errno.h>
#include <stdio.h>
#include <dspal/include/pthread.h>
#include <dspal_types.h>
#include "UARTDriver.h"
#include "Storage.h"
#include "RCOutput.h"
#include <AP_Scheduler/AP_Scheduler.h>
using namespace QURT;
extern const AP_HAL::HAL& hal;
Scheduler::Scheduler()
{
}
void Scheduler::init()
{
_main_task_pid = getpid();
// setup the timer thread - this will call tasks at 1kHz
pthread_attr_t thread_attr;
struct sched_param param;
pthread_attr_init(&thread_attr);
pthread_attr_setstacksize(&thread_attr, 40960);
param.sched_priority = APM_TIMER_PRIORITY;
(void)pthread_attr_setschedparam(&thread_attr, &param);
pthread_create(&_timer_thread_ctx, &thread_attr, &Scheduler::_timer_thread, this);
// the UART thread runs at a medium priority
pthread_attr_init(&thread_attr);
pthread_attr_setstacksize(&thread_attr, 40960);
param.sched_priority = APM_UART_PRIORITY;
(void)pthread_attr_setschedparam(&thread_attr, &param);
pthread_create(&_uart_thread_ctx, &thread_attr, &Scheduler::_uart_thread, this);
// the IO thread runs at lower priority
pthread_attr_init(&thread_attr);
pthread_attr_setstacksize(&thread_attr, 40960);
param.sched_priority = APM_IO_PRIORITY;
(void)pthread_attr_setschedparam(&thread_attr, &param);
pthread_create(&_io_thread_ctx, &thread_attr, &Scheduler::_io_thread, this);
}
void Scheduler::delay_microseconds(uint16_t usec)
{
//pthread_yield();
usleep(usec);
}
void Scheduler::delay(uint16_t ms)
{
if (!in_main_thread()) {
::printf("ERROR: delay() from timer process\n");
return;
}
uint64_t start = AP_HAL::micros64();
uint64_t now;
while (((now=AP_HAL::micros64()) - start)/1000 < ms) {
delay_microseconds(1000);
if (_min_delay_cb_ms <= ms) {
call_delay_cb();
}
}
}
void Scheduler::register_timer_process(AP_HAL::MemberProc proc)
{
for (uint8_t i = 0; i < _num_timer_procs; i++) {
if (_timer_proc[i] == proc) {
return;
}
}
if (_num_timer_procs < QURT_SCHEDULER_MAX_TIMER_PROCS) {
_timer_proc[_num_timer_procs] = proc;
_num_timer_procs++;
} else {
hal.console->printf("Out of timer processes\n");
}
}
void Scheduler::register_io_process(AP_HAL::MemberProc proc)
{
for (uint8_t i = 0; i < _num_io_procs; i++) {
if (_io_proc[i] == proc) {
return;
}
}
if (_num_io_procs < QURT_SCHEDULER_MAX_TIMER_PROCS) {
_io_proc[_num_io_procs] = proc;
_num_io_procs++;
} else {
hal.console->printf("Out of IO processes\n");
}
}
void Scheduler::register_timer_failsafe(AP_HAL::Proc failsafe, uint32_t period_us)
{
_failsafe = failsafe;
}
void Scheduler::suspend_timer_procs()
{
_timer_suspended = true;
}
void Scheduler::resume_timer_procs()
{
_timer_suspended = false;
if (_timer_event_missed == true) {
_run_timers(false);
_timer_event_missed = false;
}
}
void Scheduler::reboot(bool hold_in_bootloader)
{
HAP_PRINTF("**** REBOOT REQUESTED ****");
usleep(2000000);
exit(1);
}
void Scheduler::_run_timers(bool called_from_timer_thread)
{
if (_in_timer_proc) {
return;
}
_in_timer_proc = true;
if (!_timer_suspended) {
// now call the timer based drivers
for (int i = 0; i < _num_timer_procs; i++) {
if (_timer_proc[i]) {
_timer_proc[i]();
}
}
} else if (called_from_timer_thread) {
_timer_event_missed = true;
}
// and the failsafe, if one is setup
if (_failsafe != nullptr) {
_failsafe();
}
_in_timer_proc = false;
}
extern bool qurt_ran_overtime;
void *Scheduler::_timer_thread(void *arg)
{
Scheduler *sched = (Scheduler *)arg;
uint32_t last_ran_overtime = 0;
while (!sched->_hal_initialized) {
sched->delay_microseconds(1000);
}
while (true) {
sched->delay_microseconds(1000);
// run registered timers
sched->_run_timers(true);
// process any pending RC output requests
((RCOutput *)hal.rcout)->timer_update();
if (qurt_ran_overtime && AP_HAL::millis() - last_ran_overtime > 2000) {
last_ran_overtime = AP_HAL::millis();
printf("Overtime in task %d\n", (int)AP_Scheduler::current_task);
hal.console->printf("Overtime in task %d\n", (int)AP_Scheduler::current_task);
}
}
return nullptr;
}
void Scheduler::_run_io(void)
{
if (_in_io_proc) {
return;
}
_in_io_proc = true;
if (!_timer_suspended) {
// now call the IO based drivers
for (int i = 0; i < _num_io_procs; i++) {
if (_io_proc[i]) {
_io_proc[i]();
}
}
}
_in_io_proc = false;
}
void *Scheduler::_uart_thread(void *arg)
{
Scheduler *sched = (Scheduler *)arg;
while (!sched->_hal_initialized) {
sched->delay_microseconds(1000);
}
while (true) {
sched->delay_microseconds(1000);
// process any pending serial bytes
//((UARTDriver *)hal.uartA)->timer_tick();
hal.uartB->timer_tick();
hal.uartC->timer_tick();
hal.uartD->timer_tick();
hal.uartE->timer_tick();
hal.uartF->timer_tick();
}
return nullptr;
}
void *Scheduler::_io_thread(void *arg)
{
Scheduler *sched = (Scheduler *)arg;
while (!sched->_hal_initialized) {
sched->delay_microseconds(1000);
}
while (true) {
sched->delay_microseconds(1000);
// run registered IO processes
sched->_run_io();
}
return nullptr;
}
bool Scheduler::in_main_thread() const
{
return getpid() == _main_task_pid;
}
void Scheduler::system_initialized() {
if (_initialized) {
AP_HAL::panic("PANIC: scheduler::system_initialized called"
"more than once");
}
_initialized = true;
}
void Scheduler::hal_initialized(void)
{
HAP_PRINTF("HAL is initialised");
_hal_initialized = true;
}
#endif

73
libraries/AP_HAL_QURT/Scheduler.h
View File

@ -1,73 +0,0 @@
#pragma once
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_QURT
#include "AP_HAL_QURT_Namespace.h"
#include <sys/time.h>
#include <signal.h>
#include <pthread.h>
#define QURT_SCHEDULER_MAX_TIMER_PROCS 8
#define APM_MAIN_PRIORITY 180
#define APM_TIMER_PRIORITY 181
#define APM_UART_PRIORITY 60
#define APM_IO_PRIORITY 58
/* Scheduler implementation: */
class QURT::Scheduler : public AP_HAL::Scheduler {
public:
Scheduler();
/* AP_HAL::Scheduler methods */
void init();
void delay(uint16_t ms);
void delay_microseconds(uint16_t us);
void register_timer_process(AP_HAL::MemberProc);
void register_io_process(AP_HAL::MemberProc);
void register_timer_failsafe(AP_HAL::Proc, uint32_t period_us);
void suspend_timer_procs();
void resume_timer_procs();
void reboot(bool hold_in_bootloader);
bool in_main_thread() const override;
void system_initialized();
void hal_initialized();
private:
bool _initialized;
volatile bool _hal_initialized;
AP_HAL::Proc _delay_cb;
uint16_t _min_delay_cb_ms;
AP_HAL::Proc _failsafe;
volatile bool _timer_suspended;
AP_HAL::MemberProc _timer_proc[QURT_SCHEDULER_MAX_TIMER_PROCS];
uint8_t _num_timer_procs;
volatile bool _in_timer_proc;
AP_HAL::MemberProc _io_proc[QURT_SCHEDULER_MAX_TIMER_PROCS];
uint8_t _num_io_procs;
volatile bool _in_io_proc;
volatile bool _timer_event_missed;
pid_t _main_task_pid;
pthread_t _timer_thread_ctx;
pthread_t _io_thread_ctx;
pthread_t _storage_thread_ctx;
pthread_t _uart_thread_ctx;
static void *_timer_thread(void *arg);
static void *_io_thread(void *arg);
static void *_storage_thread(void *arg);
static void *_uart_thread(void *arg);
void _run_timers(bool called_from_timer_thread);
void _run_io(void);
};
#endif

39
libraries/AP_HAL_QURT/Semaphores.cpp
View File

@ -1,39 +0,0 @@
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_QURT
#include "Semaphores.h"
extern const AP_HAL::HAL& hal;
using namespace QURT;
bool Semaphore::give()
{
return pthread_mutex_unlock(&_lock) == 0;
}
bool Semaphore::take(uint32_t timeout_ms)
{
if (timeout_ms == HAL_SEMAPHORE_BLOCK_FOREVER) {
return pthread_mutex_lock(&_lock) == 0;
}
if (take_nonblocking()) {
return true;
}
uint64_t start = AP_HAL::micros64();
do {
hal.scheduler->delay_microseconds(200);
if (take_nonblocking()) {
return true;
}
} while ((AP_HAL::micros64() - start) < timeout_ms*1000);
return false;
}
bool Semaphore::take_nonblocking()
{
return pthread_mutex_trylock(&_lock) == 0;
}
#endif // CONFIG_HAL_BOARD

20
libraries/AP_HAL_QURT/Semaphores.h
View File

@ -1,20 +0,0 @@
#pragma once
#include <AP_HAL/AP_HAL_Boards.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_QURT
#include "AP_HAL_QURT.h"
#include <pthread.h>
class QURT::Semaphore : public AP_HAL::Semaphore {
public:
Semaphore() {
pthread_mutex_init(&_lock, nullptr);
}
bool give();
bool take(uint32_t timeout_ms);
bool take_nonblocking();
private:
pthread_mutex_t _lock;
};
#endif // CONFIG_HAL_BOARD

49
libraries/AP_HAL_QURT/Storage.cpp
View File

@ -1,49 +0,0 @@
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_QURT
#include <assert.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <unistd.h>
#include <errno.h>
#include <stdio.h>
#include "Storage.h"
using namespace QURT;
/*
This stores 'eeprom' data on the filesystem, with a 16k size
Data is written on the ARM frontend via a RPC call
*/
extern const AP_HAL::HAL& hal;
volatile bool Storage::dirty;
uint8_t Storage::buffer[QURT_STORAGE_SIZE];
Semaphore Storage::lock;
void Storage::read_block(void *dst, uint16_t loc, size_t n)
{
if (loc >= sizeof(buffer)-(n-1)) {
return;
}
memcpy(dst, &buffer[loc], n);
}
void Storage::write_block(uint16_t loc, const void *src, size_t n)
{
if (loc >= sizeof(buffer)-(n-1)) {
return;
}
if (memcmp(src, &buffer[loc], n) != 0) {
lock.take(HAL_SEMAPHORE_BLOCK_FOREVER);
memcpy(&buffer[loc], src, n);
dirty = true;
lock.give();
}
}
#endif // CONFIG_HAL_BOARD

27
libraries/AP_HAL_QURT/Storage.h
View File

@ -1,27 +0,0 @@
#pragma once
#include <AP_HAL/AP_HAL.h>
#include "AP_HAL_QURT_Namespace.h"
#include "Semaphores.h"
#include <stdio.h>
#define QURT_STORAGE_SIZE HAL_STORAGE_SIZE
class QURT::Storage : public AP_HAL::Storage
{
public:
Storage() {}
static Storage *from(AP_HAL::Storage *storage) {
return static_cast<Storage*>(storage);
}
void init() {}
void read_block(void *dst, uint16_t src, size_t n);
void write_block(uint16_t dst, const void* src, size_t n);
static volatile bool dirty;
static uint8_t buffer[QURT_STORAGE_SIZE];
static Semaphore lock;
};

300
libraries/AP_HAL_QURT/UARTDriver.cpp
View File

@ -1,300 +0,0 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_QURT
#include <stdlib.h>
#include <unistd.h>
#include "UARTDriver.h"
#include <sys/types.h>
#include <sys/stat.h>
#include <stdio.h>
#include <fcntl.h>
#include <dev_fs_lib_serial.h>
#include <AP_HAL/utility/RingBuffer.h>
using namespace QURT;
extern const AP_HAL::HAL& hal;
UARTDriver::UARTDriver(const char *name) :
devname(name)
{
}
void UARTDriver::begin(uint32_t b)
{
begin(b, 16384, 16384);
}
static const struct {
uint32_t baudrate;
enum DSPAL_SERIAL_BITRATES arg;
} baudrate_table[] = {
{ 9600, DSPAL_SIO_BITRATE_9600 },
{ 14400, DSPAL_SIO_BITRATE_14400 },
{ 19200, DSPAL_SIO_BITRATE_19200 },
{ 38400, DSPAL_SIO_BITRATE_38400 },
{ 57600, DSPAL_SIO_BITRATE_57600 },
{ 76800, DSPAL_SIO_BITRATE_76800 },
{ 115200, DSPAL_SIO_BITRATE_115200 },
{ 230400, DSPAL_SIO_BITRATE_230400 },
{ 250000, DSPAL_SIO_BITRATE_250000 },
{ 460800, DSPAL_SIO_BITRATE_460800 },
{ 921600, DSPAL_SIO_BITRATE_921600 },
{ 2000000, DSPAL_SIO_BITRATE_2000000 },
};
extern "C" {
static void read_callback_trampoline(void *, char *, size_t );
}
static void read_callback_trampoline(void *ctx, char *buf, size_t size)
{
((UARTDriver *)ctx)->_read_callback(buf, size);
}
/*
callback for incoming data
*/
void UARTDriver::_read_callback(char *buf, size_t size)
{
if (readbuf == nullptr) {
return;
}
uint32_t n = readbuf->write((const uint8_t *)buf, size);
if (n != size) {
HAP_PRINTF("read_callback lost %u %u", n, size);
}
}
void UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
{
if (devname == nullptr) {
return;
}
if (rxS < 4096) {
rxS = 4096;
}
if (txS < 4096) {
txS = 4096;
}
if (fd == -1) {
fd = open(devname, O_RDWR | O_NONBLOCK);
if (fd == -1) {
AP_HAL::panic("Unable to open %s", devname);
}
}
/*
allocate the read buffer
we allocate buffers before we successfully open the device as we
want to allocate in the early stages of boot, and cause minimum
thrashing of the heap once we are up. The ttyACM0 driver may not
connect for some time after boot
*/
if (rxS != 0 && (readbuf == nullptr || rxS != readbuf->get_size())) {
initialised = false;
if (readbuf != nullptr) {
delete readbuf;
}
readbuf = new ByteBuffer(rxS);
}
/*
allocate the write buffer
*/
if (txS != 0 && (writebuf == nullptr || txS != writebuf->get_size())) {
initialised = false;
if (writebuf != nullptr) {
delete writebuf;
}
writebuf = new ByteBuffer(txS);
}
struct dspal_serial_ioctl_receive_data_callback callback;
callback.context = this;
callback.rx_data_callback_func_ptr = read_callback_trampoline;
int ret = ioctl(fd, SERIAL_IOCTL_SET_RECEIVE_DATA_CALLBACK, (void *)&callback);
if (b != 0) {
for (uint8_t i=0; i<ARRAY_SIZE(baudrate_table); i++) {
if (b <= baudrate_table[i].baudrate) {
struct dspal_serial_ioctl_data_rate rate;
rate.bit_rate = baudrate_table[i].arg;
ret = ioctl(fd, SERIAL_IOCTL_SET_DATA_RATE, (void *)&rate);
break;
}
}
}
if (readbuf && writebuf && fd != -1) {
initialised = true;
}
}
void UARTDriver::end()
{
initialised = false;
if (fd != -1) {
::close(fd);
fd = -1;
}
if (readbuf) {
delete readbuf;
readbuf = nullptr;
}
if (writebuf) {
delete writebuf;
writebuf = nullptr;
}
}
void UARTDriver::flush()
{
}
bool UARTDriver::is_initialized()
{
return fd != -1 && initialised;
}
void UARTDriver::set_blocking_writes(bool blocking)
{
nonblocking_writes = !blocking;
}
bool UARTDriver::tx_pending()
{
return false;
}
/* QURT implementations of Stream virtual methods */
int16_t UARTDriver::available()
{
if (!initialised) {
return 0;
}
return readbuf->available();
}
int16_t UARTDriver::txspace()
{
if (!initialised) {
return 0;
}
return writebuf->space();
}
int16_t UARTDriver::read()
{
uint8_t c;
if (!initialised) {
return -1;
}
if (!lock.take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
return 0;
}
if (readbuf->empty()) {
lock.give();
return -1;
}
readbuf->read(&c, 1);
lock.give();
return c;
}
/* QURT implementations of Print virtual methods */
size_t UARTDriver::write(uint8_t c)
{
if (!initialised) {
return 0;
}
if (!lock.take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
return 0;
}
while (writebuf->space() == 0) {
if (nonblocking_writes) {
lock.give();
return 0;
}
hal.scheduler->delay_microseconds(1000);
}
writebuf->write(&c, 1);
lock.give();
return 1;
}
size_t UARTDriver::write(const uint8_t *buffer, size_t size)
{
if (!initialised) {
return 0;
}
if (!nonblocking_writes) {
/*
use the per-byte delay loop in write() above for blocking writes
*/
size_t ret = 0;
while (size--) {
if (write(*buffer++) != 1) break;
ret++;
}
return ret;
}
if (!lock.take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
return 0;
}
size_t ret = writebuf->write(buffer, size);
lock.give();
return ret;
}
/*
push any pending bytes to/from the serial port. This is called at
1kHz in the timer thread
*/
void UARTDriver::_timer_tick(void)
{
uint16_t n;
if (fd == -1 || !initialised || !lock.take_nonblocking()) {
return;
}
in_timer = true;
// write any pending bytes
n = writebuf->available();
if (n == 0) {
in_timer = false;
lock.give();
return;
}
if (n > 64) {
n = 64;
}
uint8_t buf[n];
writebuf->read(buf, n);
::write(fd, buf, n);
lock.give();
in_timer = false;
}
#endif

68
libraries/AP_HAL_QURT/UARTDriver.h
View File

@ -1,68 +0,0 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include "AP_HAL_QURT.h"
#include "Semaphores.h"
#include <AP_HAL/utility/RingBuffer.h>
class QURT::UARTDriver : public AP_HAL::UARTDriver {
public:
UARTDriver(const char *name);
/* QURT implementations of UARTDriver virtual methods */
void set_device_path(const char *path) {
devname = path;
}
void begin(uint32_t b);
void begin(uint32_t b, uint16_t rxS, uint16_t txS);
void end();
void flush();
bool is_initialized();
void set_blocking_writes(bool blocking);
bool tx_pending();
/* QURT implementations of Stream virtual methods */
int16_t available();
int16_t txspace();
int16_t read();
/* QURT implementations of Print virtual methods */
size_t write(uint8_t c);
size_t write(const uint8_t *buffer, size_t size);
void _read_callback(char *buf, size_t size);
void _timer_tick(void) override;
private:
const char *devname;
int fd = -1;
Semaphore lock;
ByteBuffer *readbuf;
ByteBuffer *writebuf;
bool nonblocking_writes = false;
volatile bool in_timer = false;
volatile bool initialised = false;
uint64_t last_write_time_us;
int write_fd(const uint8_t *buf, uint16_t n);
};

254
libraries/AP_HAL_QURT/UDPDriver.cpp
View File

@ -1,254 +0,0 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_QURT
#include <stdlib.h>
#include <unistd.h>
#include "UDPDriver.h"
#include <stdio.h>
#include <AP_HAL/utility/RingBuffer.h>
#include <AP_Math/AP_Math.h>
using namespace QURT;
extern const AP_HAL::HAL& hal;
void UDPDriver::begin(uint32_t b)
{
begin(b, 16384, 16384);
}
void UDPDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
{
rxS = constrain_int32(rxS, 16384, 30000);
txS = constrain_int32(txS, 16384, 30000);
/*
allocate the read buffer
*/
if (rxS != 0 && (readbuf == nullptr || rxS != readbuf->get_size())) {
initialised = false;
if (readbuf != nullptr) {
delete readbuf;
}
readbuf = new ByteBuffer(rxS);
}
/*
allocate the write buffer
*/
if (txS != 0 && (writebuf == nullptr || txS != writebuf->get_size())) {
initialised = false;
if (writebuf != nullptr) {
delete writebuf;
}
writebuf = new ByteBuffer(txS);
}
if (readbuf && writebuf) {
initialised = true;
}
}
void UDPDriver::end()
{
initialised = false;
if (readbuf) {
delete readbuf;
readbuf = nullptr;
}
if (writebuf) {
delete writebuf;
writebuf = nullptr;
}
}
void UDPDriver::flush()
{
}
bool UDPDriver::is_initialized()
{
return initialised;
}
void UDPDriver::set_blocking_writes(bool blocking)
{
nonblocking_writes = !blocking;
}
bool UDPDriver::tx_pending()
{
return false;
}
/* QURT implementations of Stream virtual methods */
int16_t UDPDriver::available()
{
if (!initialised) {
return 0;
}
return readbuf->available();
}
int16_t UDPDriver::txspace()
{
if (!initialised) {
return 0;
}
return writebuf->space();
}
int16_t UDPDriver::read()
{
uint8_t c;
if (!initialised) {
return -1;
}
if (!lock.take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
return 0;
}
if (readbuf->empty()) {
lock.give();
return -1;
}
readbuf->read(&c, 1);
lock.give();
return c;
}
/* QURT implementations of Print virtual methods */
size_t UDPDriver::write(uint8_t c)
{
if (!initialised) {
return 0;
}
if (!lock.take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
return 0;
}
while (writebuf->space() == 0) {
if (nonblocking_writes) {
lock.give();
return 0;
}
hal.scheduler->delay_microseconds(1000);
}
writebuf->write(&c, 1);
lock.give();
return 1;
}
size_t UDPDriver::write(const uint8_t *buffer, size_t size)
{
if (!initialised) {
return 0;
}
if (!nonblocking_writes) {
/*
use the per-byte delay loop in write() above for blocking writes
*/
size_t ret = 0;
while (size--) {
if (write(*buffer++) != 1) break;
ret++;
}
return ret;
}
if (!lock.take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
return 0;
}
size_t ret = writebuf->write(buffer, size);
lock.give();
return ret;
}
uint32_t UDPDriver::socket_check(uint8_t *buf, int len, uint32_t *nbytes)
{
if (!initialised) {
return 1;
}
if (!writebuf) {
return 1;
}
*nbytes = writebuf->available();
if (*nbytes == 0) {
return 1;
}
if (*nbytes > len) {
*nbytes = len;
}
uint32_t n = *nbytes;
if (writebuf->peek(0) != 254) {
/*
we have a non-mavlink packet at the start of the
buffer. Look ahead for a MAVLink start byte, up to 256 bytes
ahead
*/
uint16_t limit = n>256?256:n;
uint16_t i;
for (i=0; i<limit; i++) {
if (writebuf->peek(i) == 254) {
n = i;
break;
}
}
// if we didn't find a MAVLink marker then limit the send size to 256
if (i == limit) {
n = limit;
}
} else {
// this looks like a MAVLink packet - try to write on
// packet boundaries when possible
if (n < 8) {
n = 0;
} else {
// the length of the packet is the 2nd byte, and mavlink
// packets have a 6 byte header plus 2 byte checksum,
// giving len+8 bytes
uint8_t len = writebuf->peek(1);
if (n < len+8) {
// we don't have a full packet yet
n = 0;
} else if (n > len+8) {
// send just 1 packet at a time (so MAVLink packets
// are aligned on UDP boundaries)
n = len+8;
}
}
}
*nbytes = n;
writebuf->read(buf, *nbytes);
return 0;
}
uint32_t UDPDriver::socket_input(const uint8_t *buf, int len, uint32_t *nbytes)
{
if (!initialised) {
return 1;
}
if (!readbuf) {
return 1;
}
*nbytes = readbuf->write(buf, len);
return 0;
}
#endif // CONFIG_HAL_BOARD

55
libraries/AP_HAL_QURT/UDPDriver.h
View File

@ -1,55 +0,0 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include "AP_HAL_QURT.h"
#include "Semaphores.h"
#include <AP_HAL/utility/RingBuffer.h>
class QURT::UDPDriver : public AP_HAL::UARTDriver {
public:
static UDPDriver *from(AP_HAL::UARTDriver *d) {
return static_cast<UDPDriver*>(d);
}
void begin(uint32_t b);
void begin(uint32_t b, uint16_t rxS, uint16_t txS);
void end();
void flush();
bool is_initialized();
void set_blocking_writes(bool blocking);
bool tx_pending();
int16_t available();
int16_t txspace();
int16_t read();
size_t write(uint8_t c);
size_t write(const uint8_t *buffer, size_t size);
uint32_t socket_check(uint8_t *buf, int len, uint32_t *nbytes);
uint32_t socket_input(const uint8_t *buf, int len, uint32_t *nbytes);
enum flow_control get_flow_control(void) { return FLOW_CONTROL_ENABLE; }
private:
Semaphore lock;
bool initialised;
bool nonblocking_writes = true;
ByteBuffer *readbuf;
ByteBuffer *writebuf;
};

27
libraries/AP_HAL_QURT/Util.cpp
View File

@ -1,27 +0,0 @@
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_QURT
#include "Semaphores.h"
#include <stdio.h>
extern const AP_HAL::HAL& hal;
#include "Util.h"
using namespace QURT;
/*
always report 256k of free memory. I don't know how to query
available memory on QURT
*/
uint32_t Util::available_memory(void)
{
return 256*1024;
}
// create a new semaphore
Semaphore *Util::new_semaphore(void)
{
return new Semaphore;
}
#endif // CONFIG_HAL_BOARD == HAL_BOARD_QURT

31
libraries/AP_HAL_QURT/Util.h
View File

@ -1,31 +0,0 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include <AP_HAL/AP_HAL.h>
#include "AP_HAL_QURT_Namespace.h"
class QURT::Util : public AP_HAL::Util {
public:
Util(void) {}
bool run_debug_shell(AP_HAL::BetterStream *stream) override { return false; }
uint32_t available_memory(void) override;
// create a new semaphore
Semaphore *new_semaphore(void) override;
};

107
libraries/AP_HAL_QURT/dsp_main.cpp
View File

@ -1,107 +0,0 @@
#include <AP_HAL/AP_HAL.h>
#include "UDPDriver.h"
#include <stdio.h>
#include <stdint.h>
#include <unistd.h>
#include "Scheduler.h"
#include "Storage.h"
#include "replace.h"
#include <qurt_dsp.h>
extern const AP_HAL::HAL& hal;
extern "C" {
int ArduPilot_main(int argc, const char *argv[]);
}
volatile int _last_dsp_line = __LINE__;
volatile const char *_last_dsp_file = __FILE__;
volatile uint32_t _last_counter;
static void *main_thread(void *cmdptr)
{
char *cmdline = (char *)cmdptr;
HAP_PRINTF("Ardupilot main_thread started");
// break cmdline into argc/argv
int argc = 0;
for (int i=0; cmdline[i]; i++) {
if (cmdline[i] == '\n') argc++;
}
const char **argv = (const char **)calloc(argc+2, sizeof(char *));
argv[0] = &cmdline[0];
argc = 0;
for (int i=0; cmdline[i]; i++) {
if (cmdline[i] == '\n') {
cmdline[i] = 0;
argv[argc+1] = &cmdline[i+1];
argc++;
}
}
argv[argc] = nullptr;
ArduPilot_main(argc, argv);
return nullptr;
}
uint32_t ardupilot_start(const char *cmdline, int len)
{
HAP_PRINTF("Starting Ardupilot");
pthread_attr_t thread_attr;
struct sched_param param;
pthread_t thread_ctx;
pthread_attr_init(&thread_attr);
pthread_attr_setstacksize(&thread_attr, 80000);
param.sched_priority = APM_MAIN_PRIORITY;
(void)pthread_attr_setschedparam(&thread_attr, &param);
pthread_create(&thread_ctx, &thread_attr, main_thread, (void *)strdup((char*)cmdline));
return 0;
}
uint32_t ardupilot_heartbeat()
{
HAP_PRINTF("tick %u %s:%d", (unsigned)_last_counter, _last_dsp_file, _last_dsp_line);
return 0;
}
uint32_t ardupilot_set_storage(const uint8_t *buf, int len)
{
if (len != sizeof(QURT::Storage::buffer)) {
HAP_PRINTF("Incorrect storage size %u", len);
return 1;
}
QURT::Storage::lock.take(HAL_SEMAPHORE_BLOCK_FOREVER);
memcpy(QURT::Storage::buffer, buf, len);
QURT::Storage::lock.give();
return 0;
}
uint32_t ardupilot_get_storage(uint8_t *buf, int len)
{
if (len != sizeof(QURT::Storage::buffer)) {
HAP_PRINTF("Incorrect storage size %u", len);
return 1;
}
if (!QURT::Storage::dirty) {
return 1;
}
QURT::Storage::lock.take(HAL_SEMAPHORE_BLOCK_FOREVER);
memcpy(buf, QURT::Storage::buffer, len);
QURT::Storage::lock.give();
return 0;
}
uint32_t ardupilot_socket_check(uint8_t *buf, int len, uint32_t *nbytes)
{
return QURT::UDPDriver::from(hal.uartA)->socket_check(buf, len, nbytes);
}
uint32_t ardupilot_socket_input(const uint8_t *buf, int len, uint32_t *nbytes)
{
return QURT::UDPDriver::from(hal.uartA)->socket_input(buf, len, nbytes);
}

187
libraries/AP_HAL_QURT/mainapp/getifaddrs.cpp
View File

@ -1,187 +0,0 @@
/*
get system network addresses
based on code from Samba
Copyright (C) Andrew Tridgell 1998
Copyright (C) Jeremy Allison 2007
Copyright (C) Jelmer Vernooij <jelmer@samba.org> 2007
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>
#include <string.h>
#include <sys/types.h>
#include <sys/time.h>
#include <ifaddrs.h>
#include <net/if.h>
#include <sys/ioctl.h>
#include <netinet/in.h>
#include <arpa/inet.h>
void freeifaddrs(struct ifaddrs *ifp)
{
if (ifp != nullptr) {
free(ifp->ifa_name);
free(ifp->ifa_addr);
free(ifp->ifa_netmask);
free(ifp->ifa_dstaddr);
freeifaddrs(ifp->ifa_next);
free(ifp);
}
}
static struct sockaddr *sockaddr_dup(struct sockaddr *sa)
{
struct sockaddr *ret;
socklen_t socklen;
socklen = sizeof(struct sockaddr_storage);
ret = (struct sockaddr *)calloc(1, socklen);
if (ret == nullptr)
return nullptr;
memcpy(ret, sa, socklen);
return ret;
}
/* this works for Linux 2.2, Solaris 2.5, SunOS4, HPUX 10.20, OSF1
V4.0, Ultrix 4.4, SCO Unix 3.2, IRIX 6.4 and FreeBSD 3.2.
It probably also works on any BSD style system. */
int getifaddrs(struct ifaddrs **ifap)
{
struct ifconf ifc;
char buff[8192];
int fd, i, n;
struct ifreq *ifr=nullptr;
struct ifaddrs *curif;
struct ifaddrs *lastif = nullptr;
*ifap = nullptr;
if ((fd = socket(AF_INET, SOCK_DGRAM, 0)) == -1) {
return -1;
}
ifc.ifc_len = sizeof(buff);
ifc.ifc_buf = buff;
if (ioctl(fd, SIOCGIFCONF, &ifc) != 0) {
close(fd);
return -1;
}
ifr = ifc.ifc_req;
n = ifc.ifc_len / sizeof(struct ifreq);
/* Loop through interfaces, looking for given IP address */
for (i=n-1; i>=0; i--) {
if (ioctl(fd, SIOCGIFFLAGS, &ifr[i]) == -1) {
freeifaddrs(*ifap);
close(fd);
return -1;
}
curif = (struct ifaddrs *)calloc(1, sizeof(struct ifaddrs));
if (curif == nullptr) {
freeifaddrs(*ifap);
close(fd);
return -1;
}
curif->ifa_name = strdup(ifr[i].ifr_name);
if (curif->ifa_name == nullptr) {
free(curif);
freeifaddrs(*ifap);
close(fd);
return -1;
}
curif->ifa_flags = ifr[i].ifr_flags;
curif->ifa_dstaddr = nullptr;
curif->ifa_data = nullptr;
curif->ifa_next = nullptr;
curif->ifa_addr = nullptr;
if (ioctl(fd, SIOCGIFADDR, &ifr[i]) != -1) {
curif->ifa_addr = sockaddr_dup(&ifr[i].ifr_addr);
if (curif->ifa_addr == nullptr) {
free(curif->ifa_name);
free(curif);
freeifaddrs(*ifap);
close(fd);
return -1;
}
}
curif->ifa_netmask = nullptr;
if (ioctl(fd, SIOCGIFNETMASK, &ifr[i]) != -1) {
curif->ifa_netmask = sockaddr_dup(&ifr[i].ifr_addr);
if (curif->ifa_netmask == nullptr) {
if (curif->ifa_addr != nullptr) {
free(curif->ifa_addr);
}
free(curif->ifa_name);
free(curif);
freeifaddrs(*ifap);
close(fd);
return -1;
}
}
if (lastif == nullptr) {
*ifap = curif;
} else {
lastif->ifa_next = curif;
}
lastif = curif;
}
close(fd);
return 0;
}
const char *get_ipv4_broadcast(void)
{
struct ifaddrs *ifap = nullptr;
if (getifaddrs(&ifap) != 0) {
return nullptr;
}
struct ifaddrs *ia;
for (ia=ifap; ia; ia=ia->ifa_next) {
struct sockaddr_in *sin = (struct sockaddr_in *)ia->ifa_addr;
struct sockaddr_in *nmask = (struct sockaddr_in *)ia->ifa_netmask;
struct in_addr bcast;
if (strcmp(ia->ifa_name, "lo") == 0) {
continue;
}
bcast.s_addr = sin->sin_addr.s_addr | ~nmask->sin_addr.s_addr;
const char *ret = inet_ntoa(bcast);
freeifaddrs(ifap);
return ret;
}
freeifaddrs(ifap);
return nullptr;
}
#ifdef MAIN_PROGRAM
int main(void)
{
printf("%s\n", get_ipv4_broadcast());
return 0;
}
#endif

165
libraries/AP_HAL_QURT/mainapp/mainapp.cpp
View File

@ -1,165 +0,0 @@
/*
main program for HAL_QURT port
*/
#include <stdio.h>
#include <stdint.h>
#include <sys/types.h>
#include <fcntl.h>
#include <unistd.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <errno.h>
#include <AP_HAL/utility/Socket.h>
#include <qurt_dsp.h>
static SocketAPM sock{true};
static bool connected;
static uint32_t last_get_storage_us;
static uint64_t start_time;
// location of virtual eeprom in Linux filesystem
#define STORAGE_DIR "/var/APM"
#define STORAGE_FILE STORAGE_DIR "/" SKETCHNAME ".stg"
extern const char *get_ipv4_broadcast(void);
// time since startup in microseconds
static uint64_t micros64()
{
struct timespec ts;
clock_gettime(CLOCK_MONOTONIC, &ts);
uint64_t ret = ts.tv_sec*1000*1000ULL + ts.tv_nsec/1000U;
if (start_time == 0) {
start_time = ret;
}
ret -= start_time;
return ret;
}
/*
send storage file to DSPs
*/
static void send_storage(void)
{
int fd = open(STORAGE_FILE, O_RDWR|O_CREAT, 0644);
if (fd == -1) {
printf("Unable to open %s", STORAGE_FILE);
exit(1);
}
uint8_t buf[16384];
memset(buf, 0, sizeof(buf));
read(fd, buf, sizeof(buf));
if (ardupilot_set_storage(buf, sizeof(buf)) != 0) {
printf("Failed to send initial storage");
exit(1);
}
close(fd);
}
/*
get updated storage file from DSPs
*/
static void get_storage(void)
{
uint8_t buf[16384];
if (ardupilot_get_storage(buf, sizeof(buf)) != 0) {
return;
}
int fd = open(STORAGE_FILE ".new", O_WRONLY|O_CREAT|O_TRUNC, 0644);
if (fd == -1) {
printf("Unable to open %s - %s\n", STORAGE_FILE ".new", strerror(errno));
}
write(fd, buf, sizeof(buf));
close(fd);
// atomic rename
if (rename(STORAGE_FILE ".new", STORAGE_FILE) != 0) {
printf("Unable to rename to %s - %s\n", STORAGE_FILE, strerror(errno));
}
}
/*
handle any incoming or outgoing UDP socket data on behalf of the DSPs
*/
static void socket_check(void)
{
static const char *bcast = nullptr;
uint8_t buf[300];
ssize_t ret = sock.recv(buf, sizeof(buf), 0);
if (ret > 0) {
uint32_t nbytes;
ardupilot_socket_input(buf, ret, &nbytes);
if (!connected) {
const char *ip;
uint16_t port;
sock.last_recv_address(ip, port);
connected = sock.connect(ip, port);
if (connected) {
printf("Connected to UDP %s:%u\n", ip, (unsigned)port);
}
sock.set_blocking(false);
}
}
uint32_t nbytes;
if (bcast == nullptr) {
bcast = get_ipv4_broadcast();
if (bcast == nullptr) {
bcast = "255.255.255.255";
}
printf("Broadcasting to %s\n", bcast);
}
if (ardupilot_socket_check(buf, sizeof(buf), &nbytes) == 0) {
if (!connected) {
sock.sendto(buf, nbytes, bcast, 14550);
} else {
sock.send(buf, nbytes);
}
}
}
/*
encode argv/argv as a sequence separated by \n
*/
static char *encode_argv(int argc, const char *argv[])
{
uint32_t len = 0;
for (int i=0; i<argc; i++) {
len += strlen(argv[i]) + 1;
}
char *ret = (char *)malloc(len+1);
char *p = ret;
for (int i=0; i<argc; i++) {
size_t slen = strlen(argv[i]);
strcpy(p, argv[i]);
p[slen] = '\n';
p += slen + 1;
}
*p = 0;
return ret;
}
/*
main program
*/
int main(int argc, const char *argv[])
{
sock.set_broadcast();
printf("Starting DSP code\n");
send_storage();
char *cmdline = encode_argv(argc, argv);
ardupilot_start(cmdline, strlen(cmdline));
free(cmdline);
while (true) {
uint64_t now = micros64();
if (now - last_get_storage_us > 1000*1000) {
printf("tick t=%.6f\n", now*1.0e-6f);
ardupilot_heartbeat();
get_storage();
last_get_storage_us = now;
}
socket_check();
usleep(5000);
}
}

17
libraries/AP_HAL_QURT/qurt_dsp.idl
View File

@ -1,17 +0,0 @@
#include "AEEStdDef.idl"
interface ardupilot {
// start main thread
uint32 start(in sequence<char> cmdline);
// a heartbeat for debugging
uint32 heartbeat();
// get eeprom updates
uint32 set_storage(in sequence<uint8> eeprom);
uint32 get_storage(rout sequence<uint8> eeprom);
// handle socket data
uint32 socket_check(rout sequence<uint8> buf, rout uint32 nbytes);
uint32 socket_input(in sequence<uint8> buf, rout uint32 nbytes);
};

76
libraries/AP_HAL_QURT/replace.cpp
View File

@ -1,76 +0,0 @@
#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <dirent.h>
#include "replace.h"
extern "C" {
// this is not declared in qurt headers
void HAP_debug(const char *msg, int level, const char *filename, int line);
void HAP_printf(const char *file, int line, const char *format, ...)
{
va_list ap;
char buf[300];
va_start(ap, format);
vsnprintf(buf, sizeof(buf), format, ap);
va_end(ap);
HAP_debug(buf, 0, file, line);
//usleep(20000);
}
/**
QURT doesn't have strnlen()
**/
size_t strnlen(const char *s, size_t max)
{
size_t len;
for (len = 0; len < max; len++) {
if (s[len] == '\0') {
break;
}
}
return len;
}
int vasprintf(char **ptr, const char *format, va_list ap)
{
int ret;
va_list ap2;
va_copy(ap2, ap);
ret = vsnprintf(nullptr, 0, format, ap2);
va_end(ap2);
if (ret < 0) return ret;
(*ptr) = (char *)malloc(ret+1);
if (!*ptr) return -1;
va_copy(ap2, ap);
ret = vsnprintf(*ptr, ret+1, format, ap2);
va_end(ap2);
return ret;
}
int asprintf(char **ptr, const char *format, ...)
{
va_list ap;
int ret;
*ptr = nullptr;
va_start(ap, format);
ret = vasprintf(ptr, format, ap);
va_end(ap);
return ret;
}
}

39
libraries/AP_HAL_QURT/replace.h
View File

@ -1,39 +0,0 @@
#pragma once
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_QURT
#include <stdlib.h>
#include <stdint.h>
#include <types.h>
#include <sys/types.h>
#include <unistd.h>
#include <dirent.h>
#include <types.h>
#include <dirent.h>
extern "C" {
/*
work around broken headers
*/
size_t strnlen(const char *s, size_t maxlen);
int asprintf(char **, const char *, ...);
off_t lseek(int, off_t, int);
DIR *opendir (const char *);
int unlink(const char *pathname);
//typedef int32_t pid_t;
pid_t getpid (void);
void HAP_printf(const char *file, int line, const char *fmt, ...);
}
#define HAP_PRINTF(...) HAP_printf(__FILE__, __LINE__, __VA_ARGS__)
extern volatile int _last_dsp_line;
extern volatile const char *_last_dsp_file;
extern volatile uint32_t _last_counter;
#define HAP_LINE() do { _last_dsp_line = __LINE__; _last_dsp_file = __FILE__; _last_counter++; } while (0)
#endif // CONFIG_HAL_BOARD

66
libraries/AP_HAL_QURT/system.cpp
View File

@ -1,66 +0,0 @@
#include <stdarg.h>
#include <stdio.h>
#include <sys/time.h>
#include <unistd.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_HAL/system.h>
#include <sys/timespec.h>
#include <dspal_time.h>
#include "replace.h"
#include <fenv.h>
extern const AP_HAL::HAL& hal;
namespace AP_HAL {
static struct {
uint64_t start_time;
} state;
void init()
{
state.start_time = micros64();
// we don't want exceptions in flight code. That is the job of SITL
feclearexcept(FE_OVERFLOW | FE_DIVBYZERO | FE_INVALID);
}
void panic(const char *errormsg, ...)
{
char buf[200];
va_list ap;
va_start(ap, errormsg);
vsnprintf(buf, sizeof(buf), errormsg, ap);
va_end(ap);
HAP_PRINTF(buf);
usleep(2000000);
hal.rcin->deinit();
exit(1);
}
uint32_t micros()
{
return micros64() & 0xFFFFFFFF;
}
uint32_t millis()
{
return millis64() & 0xFFFFFFFF;
}
uint64_t micros64()
{
struct timespec ts;
clock_gettime(CLOCK_MONOTONIC, &ts);
uint64_t ret = ts.tv_sec*1000*1000ULL + ts.tv_nsec/1000U;
ret -= state.start_time;
return ret;
}
uint64_t millis64()
{
return micros64() / 1000;
}
} // namespace AP_HAL