5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 22:48:29 -04:00

HAL_QURT: revived QURT HAL

based around new SDK for ModalAI Voxl2 vehicles
This commit is contained in:
Andrew Tridgell 2024-07-06 09:07:40 +10:00
parent 354a40651d
commit 2fe08b6177
41 changed files with 3984 additions and 0 deletions

View File

@ -0,0 +1,25 @@
/*
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

View File

@ -0,0 +1,18 @@
/*
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
#define AP_MAIN qurt_ardupilot_main

View File

@ -0,0 +1,35 @@
/*
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 UARTDriver_Console;
class UARTDriver_MAVLinkUDP;
class UARTDriver_Local;
class UDPDriver;
class Util;
class Scheduler;
class Storage;
class Util;
class Semaphore;
class RCInput;
class RCOutput;
class AnalogSource;
class AnalogIn;
}

View File

@ -0,0 +1,22 @@
/*
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 "Util.h"

View File

@ -0,0 +1,80 @@
#include "AnalogIn.h"
#include "RCOutput.h"
using namespace QURT;
#define ANALOG_PIN_VOLTAGE 1
#define ANALOG_PIN_CURRENT 2
#define NUM_CHANNELS 10
extern const AP_HAL::HAL &hal;
// static table of voltage sources
static AnalogSource sources[NUM_CHANNELS];
AnalogIn::AnalogIn()
{
}
float AnalogSource::read_average()
{
return read_latest();
}
float AnalogSource::voltage_average()
{
return read_latest();
}
float AnalogSource::voltage_latest()
{
return read_latest();
}
float AnalogSource::read_latest()
{
const auto *rcout = (QURT::RCOutput *)hal.rcout;
switch (pin) {
case ANALOG_PIN_VOLTAGE:
return rcout->get_voltage();
case ANALOG_PIN_CURRENT:
return rcout->get_current();
default:
break;
}
return 0;
}
bool AnalogSource::set_pin(uint8_t p)
{
switch (p) {
case ANALOG_PIN_VOLTAGE:
case ANALOG_PIN_CURRENT:
pin = p;
return true;
default:
break;
}
return false;
}
void AnalogIn::init()
{
}
AP_HAL::AnalogSource* AnalogIn::channel(int16_t n)
{
if (next_chan >= ARRAY_SIZE(sources)) {
return nullptr;
}
return &sources[next_chan++];
}
/*
not available, report 5v to keep GCS happy
*/
float AnalogIn::board_voltage(void)
{
return 5.0f;
}

View File

@ -0,0 +1,29 @@
#pragma once
#include "AP_HAL_QURT.h"
class QURT::AnalogSource : public AP_HAL::AnalogSource
{
public:
float read_average() override;
float read_latest() override;
bool set_pin(uint8_t p) override;
float voltage_average() override;
float voltage_latest() override;
float voltage_average_ratiometric() override
{
return voltage_average();
}
uint8_t pin;
};
class QURT::AnalogIn : public AP_HAL::AnalogIn
{
public:
AnalogIn();
void init() override;
AP_HAL::AnalogSource* channel(int16_t n) override;
float board_voltage(void) override;
private:
uint8_t next_chan;
};

View File

@ -0,0 +1,133 @@
/*
* This file 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 file 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 "DeviceBus.h"
#include <AP_HAL/AP_HAL.h>
#include <AP_HAL/utility/OwnPtr.h>
#include <stdio.h>
#include "Scheduler.h"
#include "Semaphores.h"
using namespace QURT;
extern const AP_HAL::HAL& hal;
DeviceBus::DeviceBus(AP_HAL::Scheduler::priority_base _thread_priority) :
thread_priority(_thread_priority), semaphore()
{
}
/*
per-bus callback thread
*/
void DeviceBus::bus_thread(void)
{
while (true) {
uint64_t now = AP_HAL::micros64();
DeviceBus::callback_info *callback;
// find a callback to run
for (callback = callbacks; callback; callback = callback->next) {
if (now >= callback->next_usec) {
while (now >= callback->next_usec) {
callback->next_usec += callback->period_usec;
}
// call it with semaphore held
WITH_SEMAPHORE(semaphore);
callback->cb();
}
}
// work out when next loop is needed
uint64_t next_needed = 0;
now = AP_HAL::micros64();
for (callback = callbacks; callback; callback = callback->next) {
if (next_needed == 0 ||
callback->next_usec < next_needed) {
next_needed = callback->next_usec;
if (next_needed < now) {
next_needed = now;
}
}
}
// delay for at most 50ms, to handle newly added callbacks
uint32_t delay = 50000;
if (next_needed >= now && next_needed - now < delay) {
delay = next_needed - now;
}
// don't delay for less than 100usec, so one thread doesn't
// completely dominate the CPU
if (delay < 100) {
delay = 100;
}
hal.scheduler->delay_microseconds(delay);
}
return;
}
AP_HAL::Device::PeriodicHandle DeviceBus::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb, AP_HAL::Device *_hal_device)
{
if (!thread_started) {
thread_started = true;
hal_device = _hal_device;
// setup a name for the thread
char name[30];
switch (hal_device->bus_type()) {
case AP_HAL::Device::BUS_TYPE_I2C:
snprintf(name, sizeof(name), "APM_I2C:%u",
hal_device->bus_num());
break;
case AP_HAL::Device::BUS_TYPE_SPI:
snprintf(name, sizeof(name), "APM_SPI:%u",
hal_device->bus_num());
break;
default:
break;
}
#ifdef BUSDEBUG
printf("%s:%d Thread Start\n", __PRETTY_FUNCTION__, __LINE__);
#endif
hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&DeviceBus::bus_thread, void),
name, HAL_QURT_DEVICE_STACK_SIZE, thread_priority, 0);
}
DeviceBus::callback_info *callback = new DeviceBus::callback_info;
if (callback == nullptr) {
return nullptr;
}
callback->cb = cb;
callback->period_usec = period_usec;
callback->next_usec = AP_HAL::micros64() + period_usec;
// add to linked list of callbacks on thread
callback->next = callbacks;
callbacks = callback;
return callback;
}
/*
* Adjust the timer for the next call: it needs to be called from the bus
* thread, otherwise it will race with it
*/
bool DeviceBus::adjust_timer(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec)
{
// TODO: add timer adjustment
return true;
}

View File

@ -0,0 +1,55 @@
/*
* This file 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 file 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 <inttypes.h>
#include <AP_HAL/HAL.h>
#include "Semaphores.h"
#include "AP_HAL_QURT.h"
#include "Scheduler.h"
#define HAL_QURT_DEVICE_STACK_SIZE 8192
namespace QURT
{
class DeviceBus
{
public:
DeviceBus(AP_HAL::Scheduler::priority_base _thread_priority);
struct DeviceBus *next;
HAL_Semaphore semaphore;
AP_HAL::Device::PeriodicHandle register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb, AP_HAL::Device *hal_device);
bool adjust_timer(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec);
void bus_thread(void);
private:
struct callback_info {
struct callback_info *next;
AP_HAL::Device::PeriodicCb cb;
uint32_t period_usec;
uint64_t next_usec;
} *callbacks;
AP_HAL::Scheduler::priority_base thread_priority;
bool thread_started;
AP_HAL::Device *hal_device;
};
}

View File

@ -0,0 +1,152 @@
/*
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>
#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 "AnalogIn.h"
#include "I2CDevice.h"
#include "SPIDevice.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>
#include "interface.h"
#include "ap_host/src/protocol.h"
extern "C" {
typedef void (*external_error_handler_t)(void);
};
static void crash_error_handler(void)
{
HAP_PRINTF("CRASH_ERROR_HANDLER: at %p", &crash_error_handler);
}
using namespace QURT;
static UARTDriver_Console consoleDriver;
static UARTDriver_MAVLinkUDP serial0Driver;
static UARTDriver_Local serial3Driver(QURT_UART_GPS);
static UARTDriver_Local serial4Driver(QURT_UART_RCIN);
static SPIDeviceManager spiDeviceManager;
static AnalogIn analogIn;
static Storage storageDriver;
static Empty::GPIO gpioDriver;
static RCInput rcinDriver;
static RCOutput rcoutDriver;
static Util utilInstance;
static Scheduler schedulerInstance;
static I2CDeviceManager i2c_mgr_instance;
bool qurt_ran_overtime;
HAL_QURT::HAL_QURT() :
AP_HAL::HAL(
&serial0Driver,
nullptr,
nullptr,
&serial3Driver,
&serial4Driver,
nullptr,
nullptr,
nullptr,
nullptr,
nullptr,
&i2c_mgr_instance,
&spiDeviceManager,
nullptr,
&analogIn,
&storageDriver,
&consoleDriver,
&gpioDriver,
&rcinDriver,
&rcoutDriver,
&schedulerInstance,
&utilInstance,
nullptr,
nullptr,
nullptr)
{
}
static HAL_QURT::Callbacks *_callbacks;
void HAL_QURT::main_thread(void)
{
sl_client_register_fatal_error_cb(crash_error_handler);
// Let SLPI image send out it's initialization response before we
// try to send anything out.
qurt_timer_sleep(1000000);
rcinDriver.init();
analogIn.init();
rcoutDriver.init();
_callbacks->setup();
scheduler->set_system_initialized();
HAP_PRINTF("starting loop");
for (;;) {
// ensure other threads get some time
qurt_timer_sleep(200);
// call main loop
_callbacks->loop();
}
}
void HAL_QURT::start_main_thread(Callbacks* callbacks)
{
_callbacks = callbacks;
scheduler->thread_create(FUNCTOR_BIND_MEMBER(&HAL_QURT::main_thread, void), "main_thread",
(20 * 1024),
AP_HAL::Scheduler::PRIORITY_MAIN,
0);
}
void HAL_QURT::run(int argc, char* const argv[], Callbacks* callbacks) const
{
assert(callbacks);
/* 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();
serial0Driver.begin(115200);
HAP_PRINTF("Creating main thread");
const_cast<HAL_QURT *>(this)->start_main_thread(callbacks);
}
const AP_HAL::HAL& AP_HAL::get_HAL()
{
static const HAL_QURT *hal;
if (hal == nullptr) {
hal = new HAL_QURT;
}
return *hal;
}

View File

@ -0,0 +1,29 @@
/*
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"
#include "interface.h"
class HAL_QURT : public AP_HAL::HAL
{
public:
HAL_QURT();
void run(int argc, char* const* argv, Callbacks* callbacks) const override;
void start_main_thread(Callbacks* callbacks);
void main_thread(void);
};

View File

@ -0,0 +1,131 @@
/*
* This file 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 file 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 "I2CDevice.h"
#include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h>
#include <AP_HAL_QURT/Semaphores.h>
#include "Scheduler.h"
#include "interface.h"
using namespace QURT;
/*
4 I2C buses
bus1: mag
bus2: power manager
bus4: external spare bus
bus5: barometer (internal)
*/
static uint8_t i2c_bus_ids[] = { 1, 2, 4, 5 };
static uint32_t i2c_internal_mask = (1U<<3);
I2CBus I2CDeviceManager::businfo[ARRAY_SIZE(i2c_bus_ids)];
I2CDeviceManager::I2CDeviceManager(void)
{
}
I2CDevice::I2CDevice(uint8_t busnum, uint8_t address, uint32_t bus_clock, bool use_smbus, uint32_t timeout_ms) :
_address(address),
bus(I2CDeviceManager::businfo[busnum])
{
HAP_PRINTF("Constructing I2CDevice %u 0x%02x %u", unsigned(busnum), unsigned(address), unsigned(bus_clock));
if (bus.fd == -2) {
bus.fd = sl_client_config_i2c_bus(i2c_bus_ids[busnum], address, bus_clock/1000);
}
set_device_bus(busnum);
set_device_address(address);
asprintf(&pname, "I2C:%u:%02x",
(unsigned)busnum, (unsigned)address);
}
I2CDevice::~I2CDevice()
{
free(pname);
}
bool I2CDevice::transfer(const uint8_t *send, uint32_t send_len,
uint8_t *recv, uint32_t recv_len)
{
if (bus.fd < 0) {
return false;
}
if (!bus.semaphore.check_owner()) {
return false;
}
if (bus.last_address != _address) {
sl_client_set_address_i2c_bus(bus.fd, _address);
bus.last_address = _address;
}
return sl_client_i2c_transfer(bus.fd, send, send_len, recv, recv_len) == 0;
}
/*
register a periodic callback
*/
AP_HAL::Device::PeriodicHandle I2CDevice::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb)
{
return bus.register_periodic_callback(period_usec, cb, this);
}
/*
adjust a periodic callback
*/
bool I2CDevice::adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec)
{
return bus.adjust_timer(h, period_usec);
}
AP_HAL::OwnPtr<AP_HAL::I2CDevice>
I2CDeviceManager::get_device(uint8_t bus, uint8_t address,
uint32_t bus_clock,
bool use_smbus,
uint32_t timeout_ms)
{
if (bus >= ARRAY_SIZE(i2c_bus_ids)) {
return AP_HAL::OwnPtr<AP_HAL::I2CDevice>(nullptr);
}
auto dev = AP_HAL::OwnPtr<AP_HAL::I2CDevice>(new I2CDevice(bus, address, bus_clock, use_smbus, timeout_ms));
return dev;
}
/*
get mask of bus numbers for all configured I2C buses
*/
uint32_t I2CDeviceManager::get_bus_mask(void) const
{
return ((1U << ARRAY_SIZE(i2c_bus_ids)) - 1);
}
/*
get mask of bus numbers for all configured internal I2C buses
*/
uint32_t I2CDeviceManager::get_bus_mask_internal(void) const
{
return i2c_internal_mask;
}
/*
get mask of bus numbers for all configured external I2C buses
*/
uint32_t I2CDeviceManager::get_bus_mask_external(void) const
{
return get_bus_mask() & ~get_bus_mask_internal();
}

View File

@ -0,0 +1,134 @@
/*
* This file 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 file 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 <inttypes.h>
#include <AP_HAL/HAL.h>
#include <AP_HAL/I2CDevice.h>
#include <AP_HAL/utility/OwnPtr.h>
#include "Semaphores.h"
#include "Scheduler.h"
#include "DeviceBus.h"
namespace QURT
{
class I2CBus : public DeviceBus
{
public:
I2CBus():DeviceBus(AP_HAL::Scheduler::PRIORITY_I2C) {};
uint32_t bus_clock;
int fd = -2;
uint8_t last_address;
};
class I2CDevice : public AP_HAL::I2CDevice
{
public:
static I2CDevice *from(AP_HAL::I2CDevice *dev)
{
return static_cast<I2CDevice*>(dev);
}
I2CDevice(uint8_t bus, uint8_t address, uint32_t bus_clock, bool use_smbus, uint32_t timeout_ms);
~I2CDevice();
/* See AP_HAL::I2CDevice::set_address() */
void set_address(uint8_t address) override
{
_address = address;
}
/* See AP_HAL::I2CDevice::set_retries() */
void set_retries(uint8_t retries) override
{
_retries = retries;
}
/* See AP_HAL::Device::set_speed(): Empty implementation, not supported. */
bool set_speed(enum Device::Speed speed) override
{
return true;
}
/* See AP_HAL::Device::transfer() */
bool transfer(const uint8_t *send, uint32_t send_len,
uint8_t *recv, uint32_t recv_len) override;
bool read_registers_multiple(uint8_t first_reg, uint8_t *recv,
uint32_t recv_len, uint8_t times) override
{
return false;
};
/* See AP_HAL::Device::register_periodic_callback() */
AP_HAL::Device::PeriodicHandle register_periodic_callback(
uint32_t period_usec, AP_HAL::Device::PeriodicCb) override;
/* See AP_HAL::Device::adjust_periodic_callback() */
bool adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec) override;
AP_HAL::Semaphore* get_semaphore() override //TODO check all
{
// if asking for invalid bus number use bus 0 semaphore
return &bus.semaphore;
}
protected:
I2CBus &bus;
uint8_t _retries;
uint8_t _address;
char *pname;
};
class I2CDeviceManager : public AP_HAL::I2CDeviceManager
{
public:
friend class I2CDevice;
static I2CBus businfo[];
// constructor
I2CDeviceManager();
static I2CDeviceManager *from(AP_HAL::I2CDeviceManager *i2c_mgr)
{
return static_cast<I2CDeviceManager*>(i2c_mgr);
}
AP_HAL::OwnPtr<AP_HAL::I2CDevice> get_device(uint8_t bus, uint8_t address,
uint32_t bus_clock=100000,
bool use_smbus = false,
uint32_t timeout_ms=4) override;
/*
get mask of bus numbers for all configured I2C buses
*/
uint32_t get_bus_mask(void) const override;
/*
get mask of bus numbers for all configured external I2C buses
*/
uint32_t get_bus_mask_external(void) const override;
/*
get mask of bus numbers for all configured internal I2C buses
*/
uint32_t get_bus_mask_internal(void) const override;
};
}

View File

@ -0,0 +1,58 @@
#include "RCInput.h"
#include <AP_Math/AP_Math.h>
using namespace QURT;
void RCInput::init()
{
AP::RC().init();
}
const char *RCInput::protocol() const
{
return AP::RC().protocol_name();
}
bool RCInput::new_input()
{
bool ret = updated;
updated = false;
return ret;
}
uint8_t RCInput::num_channels()
{
return num_chan;
}
uint16_t RCInput::read(uint8_t channel)
{
if (channel >= MIN(RC_INPUT_MAX_CHANNELS, num_chan)) {
return 0;
}
return values[channel];
}
uint8_t RCInput::read(uint16_t* periods, uint8_t len)
{
WITH_SEMAPHORE(mutex);
len = MIN(len, num_chan);
memcpy(periods, values, len*sizeof(periods[0]));
return len;
}
void RCInput::_timer_tick(void)
{
auto &rcprot = AP::RC();
WITH_SEMAPHORE(mutex);
rcprot.update();
if (rcprot.new_input()) {
num_chan = rcprot.num_channels();
num_chan = MIN(num_chan, RC_INPUT_MAX_CHANNELS);
rcprot.read(values, num_chan);
updated = true;
}
}

View File

@ -0,0 +1,28 @@
#pragma once
#include "AP_HAL_QURT.h"
#include <AP_RCProtocol/AP_RCProtocol.h>
#ifndef RC_INPUT_MAX_CHANNELS
#define RC_INPUT_MAX_CHANNELS 18
#endif
class QURT::RCInput : public AP_HAL::RCInput
{
public:
void init() override;
bool new_input() override;
uint8_t num_channels() override;
uint16_t read(uint8_t ch) override;
uint8_t read(uint16_t* periods, uint8_t len) override;
const char *protocol() const override;
void _timer_tick(void);
private:
HAL_Semaphore mutex;
uint16_t values[RC_INPUT_MAX_CHANNELS];
uint8_t num_chan;
bool updated;
};

View File

@ -0,0 +1,225 @@
#include <AP_HAL/AP_HAL.h>
#include "RCOutput.h"
#include <AP_Math/AP_Math.h>
extern const AP_HAL::HAL& hal;
using namespace QURT;
#define ESC_PACKET_TYPE_PWM_CMD 1
#define ESC_PACKET_TYPE_FB_RESPONSE 128
#define ESC_PACKET_TYPE_FB_POWER_STATUS 132
#define ESC_PKT_HEADER 0xAF
void RCOutput::init()
{
fd = sl_client_config_uart(QURT_UART_ESC, baudrate);
if (fd == -1) {
HAP_PRINTF("Failed to open ESC UART");
}
HAP_PRINTF("ESC UART: %d", fd);
}
void RCOutput::set_freq(uint32_t chmask, uint16_t freq_hz)
{
// no support for changing frequency
}
uint16_t RCOutput::get_freq(uint8_t ch)
{
// return fixed fake value
return 490;
}
void RCOutput::enable_ch(uint8_t ch)
{
if (ch >= ARRAY_SIZE(period)) {
return;
}
enable_mask |= 1U<<ch;
}
void RCOutput::disable_ch(uint8_t ch)
{
if (ch >= ARRAY_SIZE(period)) {
return;
}
enable_mask &= ~1U<<ch;
}
void RCOutput::write(uint8_t ch, uint16_t period_us)
{
if (ch >= ARRAY_SIZE(period)) {
return;
}
period[ch] = period_us;
if (!corked) {
need_write = true;
}
}
uint16_t RCOutput::read(uint8_t ch)
{
if (ch >= ARRAY_SIZE(period)) {
return 0;
}
return period[ch];
}
void RCOutput::read(uint16_t *period_us, uint8_t len)
{
for (auto i = 0; i < len; i++) {
period_us[i] = read(i);
}
}
/*
send a packet with CRC to the ESC
*/
void RCOutput::send_esc_packet(uint8_t type, uint8_t *data, uint16_t size)
{
uint16_t packet_size = size + 5;
uint8_t out[packet_size];
out[0] = ESC_PKT_HEADER;
out[1] = packet_size;
out[2] = type;
memcpy(&out[3], data, size);
uint16_t crc = calc_crc_modbus(&out[1], packet_size - 3);
memcpy(&out[packet_size - 2], &crc, sizeof(uint16_t));
sl_client_uart_write(fd, (const char *)out, packet_size);
}
/*
convert 1000 to 2000 PWM to -800 to 800 for QURT ESCs
*/
static int16_t pwm_to_esc(uint16_t pwm)
{
const float p = constrain_float((pwm-1000)*0.001, 0, 1);
return int16_t(800*p);
}
/*
send current commands to ESCs
*/
void RCOutput::send_receive(void)
{
if (fd == -1) {
return;
}
int16_t data[5] {};
for (uint8_t i=0; i<4; i++) {
data[esc_map[i]] = pwm_to_esc(period[i]);
}
need_write = false;
const uint32_t now_ms = AP_HAL::millis();
if (now_ms - last_fb_req_ms > 5) {
last_fb_req_ms = now_ms;
// request feedback from one ESC
last_fb_idx = (last_fb_idx+1) % 4;
data[last_fb_idx] |= 1;
}
send_esc_packet(ESC_PACKET_TYPE_PWM_CMD, (uint8_t *)data, sizeof(data));
check_response();
}
/*
handle a telem feedback packet
*/
void RCOutput::handle_esc_feedback(const struct esc_response_v2 &pkt)
{
const uint8_t idx = esc_map_rev[pkt.id_state>>4];
if (idx >= ARRAY_SIZE(period)) {
return;
}
update_rpm(idx, pkt.rpm);
AP_ESC_Telem_Backend::TelemetryData tdata {};
tdata.voltage = pkt.voltage*0.001;
tdata.current = pkt.current*0.008;
tdata.temperature_cdeg = pkt.temperature;
update_telem_data(idx, tdata,
AP_ESC_Telem_Backend::TelemetryType::CURRENT |
AP_ESC_Telem_Backend::TelemetryType::VOLTAGE |
AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE);
}
/*
handle a power status packet, making it available to AnalogIn
*/
void RCOutput::handle_power_status(const struct esc_power_status &pkt)
{
esc_voltage = pkt.voltage * 0.001;
const int16_t current_offset = 2048; // why this offset?
esc_current = (pkt.current+current_offset) * 0.008;
}
// check for responses
void RCOutput::check_response(void)
{
uint8_t buf[256];
struct PACKED esc_packet {
uint8_t header;
uint8_t length;
uint8_t type;
union {
struct esc_response_v2 resp_v2;
struct esc_power_status power_status;
} u;
};
auto n = sl_client_uart_read(fd, (char *)buf, sizeof(buf));
while (n >= 3) {
const auto *pkt = (struct esc_packet *)buf;
if (pkt->header != ESC_PKT_HEADER || pkt->length > n) {
return;
}
const uint16_t crc = calc_crc_modbus(&pkt->length, pkt->length-3);
const uint16_t crc2 = buf[pkt->length-2] | buf[pkt->length-1]<<8;
if (crc != crc2) {
return;
}
switch (pkt->type) {
case ESC_PACKET_TYPE_FB_RESPONSE:
handle_esc_feedback(pkt->u.resp_v2);
break;
case ESC_PACKET_TYPE_FB_POWER_STATUS:
handle_power_status(pkt->u.power_status);
break;
default:
HAP_PRINTF("Unknown pkt %u", pkt->type);
break;
}
if (n == pkt->length) {
break;
}
memmove(&buf[0], &buf[pkt->length], n - pkt->length);
n -= pkt->length;
}
}
void RCOutput::cork(void)
{
corked = true;
}
void RCOutput::push(void)
{
if (corked) {
corked = false;
need_write = true;
send_receive();
}
}

View File

@ -0,0 +1,75 @@
#pragma once
#include <AP_HAL/AP_HAL.h>
#include "AP_HAL_QURT.h"
#include <AP_ESC_Telem/AP_ESC_Telem.h>
class QURT::RCOutput : public AP_HAL::RCOutput, AP_ESC_Telem_Backend
{
public:
void init() override;
void set_freq(uint32_t chmask, uint16_t freq_hz) override;
uint16_t get_freq(uint8_t ch) override;
void enable_ch(uint8_t ch) override;
void disable_ch(uint8_t ch) override;
void write(uint8_t ch, uint16_t period_us) override;
uint16_t read(uint8_t ch) override;
void read(uint16_t *period_us, uint8_t len) override;
void cork(void) override;
void push(void) override;
float get_voltage(void) const
{
return esc_voltage;
}
float get_current(void) const
{
return esc_current;
}
private:
const uint32_t baudrate = 2000000;
void send_receive(void);
void check_response(void);
void send_esc_packet(uint8_t type, uint8_t *data, uint16_t size);
struct PACKED esc_response_v2 {
uint8_t id_state; // bits 0:3 = state, bits 4:7 = ID
uint16_t rpm; // Current RPM of the motor
uint8_t cmd_counter; // Number of commands received by the ESC
uint8_t power; // Applied power [0..100]
uint16_t voltage; // Voltage measured by the ESC in mV
int16_t current; // Current measured by the ESC in 8mA resolution
int16_t temperature; // Temperature measured by the ESC in 0.01 degC resolution
};
struct PACKED esc_power_status {
uint8_t id; //ESC Id (could be used as system ID elsewhere)
uint16_t voltage; //Input voltage (Millivolts)
int16_t current; //Total Current (8mA resolution)
};
void handle_esc_feedback(const struct esc_response_v2 &pkt);
void handle_power_status(const struct esc_power_status &pkt);
// order is RL, FL, FR, RR, map to X frame
const uint8_t esc_map[4] {2, 0, 1, 3};
const uint8_t esc_map_rev[4] {1, 2, 0, 3};
int fd = -1;
uint16_t enable_mask;
static const uint8_t channel_count = 4;
uint16_t period[channel_count];
volatile bool need_write;
bool corked;
HAL_Semaphore mutex;
uint8_t last_fb_idx;
uint32_t last_fb_req_ms;
float esc_voltage;
float esc_current;
};

View File

@ -0,0 +1,128 @@
/*
* This file 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 file 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 "SPIDevice.h"
#include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h>
#include <AP_HAL/utility/OwnPtr.h>
#include "Scheduler.h"
#include "Semaphores.h"
#include "interface.h"
#include <stdio.h>
using namespace QURT;
#define MHZ (1000U*1000U)
#define KHZ (1000U)
const char *device_names[] = {"INV1", "INV2", "INV3"};
static SPIBus *spi_bus;
SPIBus::SPIBus(void):
DeviceBus(Scheduler::PRIORITY_SPI)
{
fd = sl_client_config_spi_bus();
HAP_PRINTF("Created SPI bus -> %d", fd);
}
SPIDevice::SPIDevice(const char *name, SPIBus &_bus)
: bus(_bus)
{
set_device_bus(0);
set_device_address(1);
set_speed(AP_HAL::Device::SPEED_LOW);
pname = name;
}
SPIDevice::~SPIDevice()
{
}
bool SPIDevice::set_speed(AP_HAL::Device::Speed _speed)
{
return true;
}
bool SPIDevice::transfer(const uint8_t *send, uint32_t send_len,
uint8_t *recv, uint32_t recv_len)
{
// If there is no receive buffer then this is a write transaction
// and the data to write is in the send buffer
if (!recv) {
return transfer_fullduplex(send, (uint8_t*) send, send_len);
}
// This is a read transaction
uint8_t buf[send_len+recv_len];
if (send_len > 0) {
memcpy(buf, send, send_len);
}
if (recv_len > 0) {
memset(&buf[send_len], 0, recv_len);
}
transfer_fullduplex(buf, buf, send_len+recv_len);
if (recv_len > 0) {
memcpy(recv, &buf[send_len], recv_len);
}
return true;
}
bool SPIDevice::transfer_fullduplex(const uint8_t *send, uint8_t *recv, uint32_t len)
{
return (sl_client_spi_transfer(bus.fd, send, recv, len) == 0);
}
void SPIDevice::acquire_bus(bool accuire)
{
}
AP_HAL::Semaphore *SPIDevice::get_semaphore()
{
return &bus.semaphore;
}
AP_HAL::Device::PeriodicHandle SPIDevice::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb)
{
return bus.register_periodic_callback(period_usec, cb, this);
}
bool SPIDevice::adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec)
{
return bus.adjust_timer(h, period_usec);
}
AP_HAL::OwnPtr<AP_HAL::SPIDevice>
SPIDeviceManager::get_device(const char *name)
{
uint8_t i;
for (i = 0; i<ARRAY_SIZE(device_names); i++) {
if (strcmp(device_names[i], name) == 0) {
break;
}
}
if (i == ARRAY_SIZE(device_names)) {
return AP_HAL::OwnPtr<AP_HAL::SPIDevice>(nullptr);
}
if (spi_bus == nullptr) {
spi_bus = new SPIBus();
}
return AP_HAL::OwnPtr<AP_HAL::SPIDevice>(new SPIDevice(name, *spi_bus));
}

View File

@ -0,0 +1,65 @@
/*
* This file 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 file 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 <inttypes.h>
#include <AP_HAL/HAL.h>
#include <AP_HAL/SPIDevice.h>
#include "AP_HAL_QURT.h"
#include "Semaphores.h"
#include "Scheduler.h"
#include "DeviceBus.h"
namespace QURT
{
class SPIBus : public DeviceBus
{
public:
SPIBus();
int fd = -1;
};
class SPIDevice : public AP_HAL::SPIDevice
{
public:
SPIDevice(const char *name, SPIBus &bus);
virtual ~SPIDevice();
bool set_speed(AP_HAL::Device::Speed speed) override;
bool transfer(const uint8_t *send, uint32_t send_len,
uint8_t *recv, uint32_t recv_len) override;
bool transfer_fullduplex(const uint8_t *send, uint8_t *recv, uint32_t len) override;
AP_HAL::Semaphore *get_semaphore() override;
AP_HAL::Device::PeriodicHandle register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb) override;
bool adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec) override;
private:
SPIBus &bus;
const char *pname;
void acquire_bus(bool accuire);
};
class SPIDeviceManager : public AP_HAL::SPIDeviceManager
{
public:
friend class SPIDevice;
AP_HAL::OwnPtr<AP_HAL::SPIDevice> get_device(const char *name) override;
};
}

View File

@ -0,0 +1,344 @@
#include <AP_HAL/AP_HAL.h>
#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 <pthread.h>
#include "UARTDriver.h"
#include "Storage.h"
#include "RCOutput.h"
#include "RCInput.h"
#include <AP_Scheduler/AP_Scheduler.h>
#include "Thread.h"
using namespace QURT;
extern const AP_HAL::HAL& hal;
Scheduler::Scheduler()
{
}
void Scheduler::init()
{
_main_thread_ctx = pthread_self();
// 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, 16000);
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, 16000);
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, 16000);
param.sched_priority = APM_IO_PRIORITY;
(void)pthread_attr_setschedparam(&thread_attr, &param);
pthread_create(&_io_thread_ctx, &thread_attr, &Scheduler::_io_thread, this);
}
#define APM_QURT_MAX_PRIORITY (200 + 20)
#define APM_QURT_TIMER_PRIORITY (200 + 15)
#define APM_QURT_UART_PRIORITY (200 + 14)
#define APM_QURT_NET_PRIORITY (200 + 14)
#define APM_QURT_RCIN_PRIORITY (200 + 13)
#define APM_QURT_MAIN_PRIORITY (200 + 12)
#define APM_QURT_IO_PRIORITY (200 + 10)
#define APM_QURT_SCRIPTING_PRIORITY (200 + 1)
#define AP_QURT_SENSORS_SCHED_PRIO (200 + 12)
uint8_t Scheduler::calculate_thread_priority(priority_base base, int8_t priority) const
{
uint8_t thread_priority = APM_QURT_IO_PRIORITY;
static const struct {
priority_base base;
uint8_t p;
} priority_map[] = {
{ PRIORITY_BOOST, APM_QURT_MAIN_PRIORITY},
{ PRIORITY_MAIN, APM_QURT_MAIN_PRIORITY},
{ PRIORITY_SPI, AP_QURT_SENSORS_SCHED_PRIO+1},
{ PRIORITY_I2C, AP_QURT_SENSORS_SCHED_PRIO},
{ PRIORITY_CAN, APM_QURT_TIMER_PRIORITY},
{ PRIORITY_TIMER, APM_QURT_TIMER_PRIORITY},
{ PRIORITY_RCIN, APM_QURT_RCIN_PRIORITY},
{ PRIORITY_IO, APM_QURT_IO_PRIORITY},
{ PRIORITY_UART, APM_QURT_UART_PRIORITY},
{ PRIORITY_STORAGE, APM_QURT_IO_PRIORITY},
{ PRIORITY_SCRIPTING, APM_QURT_SCRIPTING_PRIORITY},
{ PRIORITY_NET, APM_QURT_NET_PRIORITY},
};
for (const auto &m : priority_map) {
if (m.base == base) {
thread_priority = constrain_int16(m.p + priority, 1, APM_QURT_MAX_PRIORITY);
break;
}
}
return thread_priority;
}
/*
create a new thread
*/
bool Scheduler::thread_create(AP_HAL::MemberProc proc, const char *name, uint32_t stack_size, priority_base base, int8_t priority)
{
Thread *thread = new Thread{(Thread::task_t)proc};
if (thread == nullptr) {
return false;
}
const uint8_t thread_priority = calculate_thread_priority(base, priority);
stack_size = MAX(stack_size, 8192U);
// Setting the stack size too large can cause odd behavior!!!
thread->set_stack_size(stack_size);
/*
* We should probably store the thread handlers and join() when exiting,
* but let's the thread manage itself for now.
*/
thread->set_auto_free(true);
DEV_PRINTF("Starting thread %s: Priority %u", name, thread_priority);
if (!thread->start(name, SCHED_FIFO, thread_priority)) {
delete thread;
return false;
}
return true;
}
void Scheduler::delay_microseconds(uint16_t usec)
{
qurt_timer_sleep(usec);
}
void Scheduler::delay(uint16_t ms)
{
uint64_t start = AP_HAL::micros64();
while ((AP_HAL::micros64() - start)/1000 < ms) {
delay_microseconds(1000);
if (_min_delay_cb_ms <= ms) {
if (in_main_thread()) {
const auto old_task = hal.util->persistent_data.scheduler_task;
hal.util->persistent_data.scheduler_task = -4;
call_delay_cb();
hal.util->persistent_data.scheduler_task = old_task;
}
}
}
}
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 ****");
// delay for printf to appear on USB monitor
qurt_timer_sleep(10000);
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;
while (!sched->_hal_initialized) {
sched->delay_microseconds(1000);
}
while (true) {
sched->delay_microseconds(1000);
// run registered timers
sched->_run_timers(true);
}
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(200);
// process any pending serial bytes
for (uint8_t i = 0; i < hal.num_serial; i++) {
auto *p = hal.serial(i);
if (p != nullptr) {
p->_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();
// update storage
hal.storage->_timer_tick();
// update RC input
((QURT::RCInput *)hal.rcin)->_timer_tick();
}
return nullptr;
}
bool Scheduler::in_main_thread() const
{
return pthread_equal(pthread_self(), _main_thread_ctx);
}
void Scheduler::set_system_initialized()
{
_main_thread_ctx = pthread_self();
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;
}

View File

@ -0,0 +1,84 @@
#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() override;
void delay(uint16_t ms) override;
void delay_microseconds(uint16_t us) override;
void register_timer_process(AP_HAL::MemberProc) override;
void register_io_process(AP_HAL::MemberProc) override;
void register_timer_failsafe(AP_HAL::Proc, uint32_t period_us) override;
void suspend_timer_procs();
void resume_timer_procs();
void reboot(bool hold_in_bootloader) override;
bool in_main_thread() const override;
void hal_initialized();
void set_system_initialized() override;
bool is_system_initialized() override
{
return _initialized;
}
bool thread_create(AP_HAL::MemberProc proc, const char *name,
uint32_t stack_size, priority_base base, int8_t priority) override;
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;
pthread_t _main_thread_ctx;
pthread_t _timer_thread_ctx;
pthread_t _io_thread_ctx;
pthread_t _storage_thread_ctx;
pthread_t _uart_thread_ctx;
uint8_t calculate_thread_priority(priority_base base, int8_t priority) const;
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

View File

@ -0,0 +1,111 @@
#include <AP_HAL/AP_HAL.h>
#include "Semaphores.h"
extern const AP_HAL::HAL& hal;
using namespace QURT;
// construct a semaphore
Semaphore::Semaphore()
{
pthread_mutexattr_t attr;
pthread_mutexattr_init(&attr);
pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_RECURSIVE);
pthread_mutex_init(&_lock, &attr);
}
bool Semaphore::give()
{
return pthread_mutex_unlock(&_lock) == 0;
}
bool Semaphore::check_owner()
{
return owner == pthread_self();
}
bool Semaphore::take(uint32_t timeout_ms)
{
if (timeout_ms == HAL_SEMAPHORE_BLOCK_FOREVER) {
auto ok = pthread_mutex_lock(&_lock) == 0;
if (ok) {
owner = pthread_self();
}
return ok;
}
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()
{
const auto ok = pthread_mutex_trylock(&_lock) == 0;
if (ok) {
owner = pthread_self();
}
return ok;
}
/*
binary semaphore using condition variables
*/
BinarySemaphore::BinarySemaphore(bool initial_state) :
AP_HAL::BinarySemaphore(initial_state)
{
pthread_cond_init(&cond, NULL);
pending = initial_state;
}
bool BinarySemaphore::wait(uint32_t timeout_us)
{
WITH_SEMAPHORE(mtx);
if (!pending) {
struct timespec ts;
if (clock_gettime(CLOCK_REALTIME, &ts) != 0) {
return false;
}
ts.tv_sec += timeout_us/1000000UL;
ts.tv_nsec += (timeout_us % 1000000U) * 1000UL;
if (ts.tv_nsec >= 1000000000L) {
ts.tv_sec++;
ts.tv_nsec -= 1000000000L;
}
if (pthread_cond_timedwait(&cond, &mtx._lock, &ts) != 0) {
return false;
}
}
pending = false;
return true;
}
bool BinarySemaphore::wait_blocking(void)
{
WITH_SEMAPHORE(mtx);
if (!pending) {
if (pthread_cond_wait(&cond, &mtx._lock) != 0) {
return false;
}
}
pending = false;
return true;
}
void BinarySemaphore::signal(void)
{
WITH_SEMAPHORE(mtx);
if (!pending) {
pending = true;
pthread_cond_signal(&cond);
}
}

View File

@ -0,0 +1,42 @@
#pragma once
#include <AP_HAL/AP_HAL_Boards.h>
#include <stdint.h>
#include <AP_HAL/Semaphores.h>
#include <pthread.h>
namespace QURT
{
class Semaphore : public AP_HAL::Semaphore
{
public:
friend class BinarySemaphore;
Semaphore();
bool give() override;
bool take(uint32_t timeout_ms) override;
bool take_nonblocking() override;
bool check_owner(void);
protected:
// qurt_mutex_t _lock;
pthread_mutex_t _lock;
pthread_t owner;
};
class BinarySemaphore : public AP_HAL::BinarySemaphore
{
public:
BinarySemaphore(bool initial_state=false);
bool wait(uint32_t timeout_us) override;
bool wait_blocking(void) override;
void signal(void) override;
protected:
Semaphore mtx;
pthread_cond_t cond;
bool pending;
};
}

View File

@ -0,0 +1,195 @@
#include "Storage.h"
#include <fcntl.h>
#include <stdio.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <unistd.h>
#include <AP_HAL/AP_HAL.h>
using namespace QURT;
#define QURT_STORAGE_MAX_WRITE 512
#define QURT_STORAGE_LINE_SHIFT 9
#define QURT_STORAGE_LINE_SIZE (1<<QURT_STORAGE_LINE_SHIFT)
#define QURT_STORAGE_NUM_LINES (QURT_STORAGE_SIZE/QURT_STORAGE_LINE_SIZE)
/*
This stores 'eeprom' data on the SD card, with a 4k size, and a
in-memory buffer. This keeps the latency down.
*/
// name the storage file after the sketch so you can use the same board
// card for ArduCopter and ArduPlane
#define STORAGE_FILE AP_FILESYSTEM_POSIX_MAP_FILENAME_BASEDIR "/APM/" AP_BUILD_TARGET_NAME ".stg"
extern const AP_HAL::HAL& hal;
bool Storage::_storage_create(void)
{
int fd = -1;
fd = open(STORAGE_FILE, O_WRONLY|O_CREAT|O_TRUNC, 0755);
if (fd == -1) {
AP_HAL::panic("Failed to open storage: %s", STORAGE_FILE);
return false;
}
// take up all needed space
if (write(fd, _buffer, sizeof(_buffer)) != sizeof(_buffer)) {
AP_HAL::panic("Failed to init %s", STORAGE_FILE);
goto fail;
}
// ensure the directory is updated with the new size
close(fd);
return true;
fail:
if (fd != -1) {
close(fd);
}
return false;
}
void Storage::init()
{
if (_initialised) {
return;
}
_dirty_mask = 0;
int fd = open(STORAGE_FILE, O_RDWR);
if (fd == -1) {
_storage_create();
fd = open(STORAGE_FILE, O_RDWR);
if (fd == -1) {
AP_HAL::panic("Failed to create %s", STORAGE_FILE);
}
}
ssize_t ret = read(fd, _buffer, sizeof(_buffer));
if (ret != sizeof(_buffer)) {
close(fd);
_storage_create();
fd = open(STORAGE_FILE, O_RDWR);
if (fd == -1) {
AP_HAL::panic("Failed to open %s", STORAGE_FILE);
}
if (read(fd, _buffer, sizeof(_buffer)) != sizeof(_buffer)) {
AP_HAL::panic("Failed to read %s", STORAGE_FILE);
}
}
_fd = fd;
_initialised = true;
}
/*
mark some lines as dirty. Note that there is no attempt to avoid
the race condition between this code and the _timer_tick() code
below, which both update _dirty_mask. If we lose the race then the
result is that a line is written more than once, but it won't result
in a line not being written.
*/
void Storage::_mark_dirty(uint16_t loc, uint16_t length)
{
if (length == 0) {
return;
}
uint16_t end = loc + length - 1;
for (uint8_t line=loc>>QURT_STORAGE_LINE_SHIFT;
line <= end>>QURT_STORAGE_LINE_SHIFT;
line++) {
_dirty_mask |= 1U << line;
}
}
void Storage::read_block(void *dst, uint16_t loc, size_t n)
{
if (loc >= sizeof(_buffer)-(n-1)) {
return;
}
init();
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) {
init();
memcpy(&_buffer[loc], src, n);
_mark_dirty(loc, n);
}
}
void Storage::_timer_tick(void)
{
if (!_initialised || _dirty_mask == 0 || _fd == -1) {
return;
}
// write out the first dirty set of lines. We don't write more
// than one to keep the latency of this call to a minimum
uint8_t i, n;
for (i=0; i<QURT_STORAGE_NUM_LINES; i++) {
if (_dirty_mask & (1<<i)) {
break;
}
}
if (i == QURT_STORAGE_NUM_LINES) {
// this shouldn't be possible
return;
}
uint32_t write_mask = (1U<<i);
// see how many lines to write
for (n=1; (i+n) < QURT_STORAGE_NUM_LINES &&
n < (QURT_STORAGE_MAX_WRITE>>QURT_STORAGE_LINE_SHIFT); n++) {
if (!(_dirty_mask & (1<<(n+i)))) {
break;
}
// mark that line clean
write_mask |= (1<<(n+i));
}
/*
write the lines. This also updates _dirty_mask. Note that
because this is a SCHED_FIFO thread it will not be preempted
by the main task except during blocking calls. This means we
don't need a semaphore around the _dirty_mask updates.
*/
if (lseek(_fd, i<<QURT_STORAGE_LINE_SHIFT, SEEK_SET) == (i<<QURT_STORAGE_LINE_SHIFT)) {
_dirty_mask &= ~write_mask;
if (write(_fd, &_buffer[i<<QURT_STORAGE_LINE_SHIFT], n<<QURT_STORAGE_LINE_SHIFT) != n<<QURT_STORAGE_LINE_SHIFT) {
// write error - likely EINTR
_dirty_mask |= write_mask;
close(_fd);
_fd = -1;
}
if (_dirty_mask == 0) {
// close and re-open to force storage to sync, QURT has no fsync() call
close(_fd);
_fd = open(STORAGE_FILE, O_RDWR);
}
}
}
/*
get storage size and ptr
*/
bool Storage::get_storage_ptr(void *&ptr, size_t &size)
{
if (!_initialised) {
return false;
}
ptr = _buffer;
size = sizeof(_buffer);
return true;
}

View File

@ -0,0 +1,45 @@
#pragma once
#include <AP_HAL/AP_HAL.h>
#define QURT_STORAGE_SIZE HAL_STORAGE_SIZE
namespace QURT
{
class Storage : public AP_HAL::Storage
{
public:
static Storage *from(AP_HAL::Storage *storage)
{
return static_cast<Storage*>(storage);
}
void init() override;
uint8_t read_byte(uint16_t loc);
uint16_t read_word(uint16_t loc);
uint32_t read_dword(uint16_t loc);
void read_block(void *dst, uint16_t src, size_t n) override;
void write_byte(uint16_t loc, uint8_t value);
void write_word(uint16_t loc, uint16_t value);
void write_dword(uint16_t loc, uint32_t value);
void write_block(uint16_t dst, const void* src, size_t n) override;
bool get_storage_ptr(void *&ptr, size_t &size) override;
virtual void _timer_tick(void) override;
protected:
void _mark_dirty(uint16_t loc, uint16_t length);
bool _storage_create(void);
int _fd = -1;
volatile bool _initialised;
volatile uint32_t _dirty_mask;
uint8_t _buffer[QURT_STORAGE_SIZE];
};
}

View File

@ -0,0 +1,182 @@
/*
* Copyright (C) 2016 Intel Corporation. All rights reserved.
*
* This file 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 file 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 "Thread.h"
#include <limits.h>
#include <sys/types.h>
#include <stdio.h>
#include <unistd.h>
#include <utility>
#include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h>
#include "Scheduler.h"
extern const AP_HAL::HAL &hal;
namespace QURT
{
void *Thread::_run_trampoline(void *arg)
{
Thread *thread = static_cast<Thread *>(arg);
thread->_run();
if (thread->_auto_free) {
delete thread;
}
return nullptr;
}
bool Thread::_run()
{
if (!_task) {
return false;
}
_task();
return true;
}
size_t Thread::get_stack_usage()
{
return 0;
}
bool Thread::start(const char *name, int policy, int prio)
{
if (_started) {
return false;
}
struct sched_param param = { .sched_priority = prio };
pthread_attr_t attr;
int r;
pthread_attr_init(&attr);
if ((r = pthread_attr_setschedparam(&attr, &param)) != 0) {
AP_HAL::panic("Failed to set attributes for thread '%s': %s",
name, strerror(r));
}
const uint32_t stack_alignment = 2048U;
_stack_size = (_stack_size+stack_alignment) & ~stack_alignment;
if (_stack_size) {
if (pthread_attr_setstacksize(&attr, _stack_size) != 0) {
return false;
}
}
r = pthread_create(&_ctx, &attr, &Thread::_run_trampoline, this);
if (r != 0) {
AP_HAL::panic("Failed to create thread '%s': %s",
name, strerror(r));
}
pthread_attr_destroy(&attr);
_started = true;
return true;
}
bool Thread::is_current_thread()
{
return pthread_equal(pthread_self(), _ctx);
}
bool Thread::join()
{
void *ret;
if (_ctx == 0) {
return false;
}
if (pthread_join(_ctx, &ret) != 0 ||
(intptr_t)ret != 0) {
return false;
}
return true;
}
bool PeriodicThread::set_rate(uint32_t rate_hz)
{
if (_started || rate_hz == 0) {
return false;
}
_period_usec = hz_to_usec(rate_hz);
return true;
}
bool Thread::set_stack_size(size_t stack_size)
{
if (_started) {
return false;
}
_stack_size = MAX(stack_size, (size_t) PTHREAD_STACK_MIN);
return true;
}
bool PeriodicThread::_run()
{
if (_period_usec == 0) {
return false;
}
uint64_t next_run_usec = AP_HAL::micros64() + _period_usec;
while (!_should_exit) {
uint64_t dt = next_run_usec - AP_HAL::micros64();
if (dt > _period_usec) {
// we've lost sync - restart
next_run_usec = AP_HAL::micros64();
} else {
qurt_timer_sleep(dt);
}
next_run_usec += _period_usec;
_task();
}
_started = false;
_should_exit = false;
return true;
}
bool PeriodicThread::stop()
{
if (!is_started()) {
return false;
}
_should_exit = true;
return true;
}
}

View File

@ -0,0 +1,108 @@
/*
* Copyright (C) 2016 Intel Corporation. All rights reserved.
*
* This file 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 file 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 <pthread.h>
#include <inttypes.h>
#include <stdlib.h>
#include <AP_HAL/utility/functor.h>
namespace QURT
{
/*
* Interface abstracting threads
*/
class Thread
{
public:
FUNCTOR_TYPEDEF(task_t, void);
Thread(task_t t) : _task(t) { }
virtual ~Thread() { }
bool start(const char *name, int policy, int prio);
bool is_current_thread();
bool is_started() const
{
return _started;
}
size_t get_stack_usage();
bool set_stack_size(size_t stack_size);
void set_auto_free(bool auto_free)
{
_auto_free = auto_free;
}
virtual bool stop()
{
return false;
}
bool join();
protected:
static void *_run_trampoline(void *arg);
/*
* Run the task assigned in the constructor. May be overriden in case it's
* preferred to use Thread as an interface or when user wants to aggregate
* some initialization or teardown for the thread.
*/
virtual bool _run();
void _poison_stack();
task_t _task;
bool _started = false;
bool _should_exit = false;
bool _auto_free = false;
pthread_t _ctx = 0;
struct stack_debug {
uint32_t *start;
uint32_t *end;
} _stack_debug;
size_t _stack_size = 0;
};
class PeriodicThread : public Thread
{
public:
PeriodicThread(Thread::task_t t)
: Thread(t)
{ }
bool set_rate(uint32_t rate_hz);
bool stop() override;
protected:
bool _run() override;
uint64_t _period_usec = 0;
};
}

View File

@ -0,0 +1,278 @@
#include "interface.h"
#include "UARTDriver.h"
#include <AP_Common/ExpandingString.h>
#if HAL_GCS_ENABLED
#include <AP_HAL/utility/packetise.h>
#endif
extern const AP_HAL::HAL& hal;
using namespace QURT;
/* QURT implementations of virtual methods */
void UARTDriver::_begin(uint32_t b, uint16_t rxS, uint16_t txS)
{
if (_initialised) {
return;
}
/* we have enough memory to have a larger transmit buffer for
* all ports. This means we don't get delays while waiting to
* write GPS config packets
*/
if (rxS < 4096) {
rxS = 4096;
}
if (txS < 4096) {
txS = 4096;
}
WITH_SEMAPHORE(_write_mutex);
if (_writebuf.set_size(txS) && _readbuf.set_size(rxS)) {
_initialised = true;
}
}
void UARTDriver::_end()
{
}
void UARTDriver::_flush()
{
}
bool UARTDriver::is_initialized()
{
return _initialised;
}
bool UARTDriver::tx_pending()
{
return (_writebuf.available() > 0);
}
uint32_t UARTDriver::_available()
{
if (!_initialised) {
return 0;
}
WITH_SEMAPHORE(_read_mutex);
return _readbuf.available();
}
uint32_t UARTDriver::txspace()
{
if (!_initialised) {
return 0;
}
return _writebuf.space();
}
bool UARTDriver::_discard_input()
{
if (!_initialised) {
return false;
}
WITH_SEMAPHORE(_read_mutex);
_readbuf.clear();
return true;
}
size_t UARTDriver::_write(const uint8_t *buffer, size_t size)
{
if (!_initialised) {
return 0;
}
WITH_SEMAPHORE(_write_mutex);
return _writebuf.write(buffer, size);
}
ssize_t UARTDriver::_read(uint8_t *buffer, uint16_t size)
{
if (!_initialised) {
return 0;
}
WITH_SEMAPHORE(_read_mutex);
return _readbuf.read(buffer, size);
}
/*
push any pending bytes to/from the serial port. This is called at
1kHz in the timer thread. Doing it this way reduces the system call
overhead in the main task enormously.
*/
void UARTDriver::_timer_tick(void)
{
if (!_initialised) {
return;
}
for (auto i=0; i<10; i++) {
if (!_write_pending_bytes()) {
break;
}
}
_fill_read_buffer();
}
/*
methods for UARTDriver_Console
*/
void UARTDriver_Console::printf(const char *fmt, ...)
{
va_list ap;
char buf[300];
va_start(ap, fmt);
vsnprintf(buf, sizeof(buf), fmt, ap);
va_end(ap);
HAP_PRINTF(buf);
}
/*
methods for UARTDriver_MAVLinkUDP
*/
typedef void (*mavlink_data_callback_t)(const struct qurt_mavlink_msg *msg, void* p);
extern void register_mavlink_data_callback(mavlink_data_callback_t func, void *p);
UARTDriver_MAVLinkUDP::UARTDriver_MAVLinkUDP(void)
{
register_mavlink_data_callback(_mavlink_data_cb, (void *) this);
}
void UARTDriver_MAVLinkUDP::_mavlink_data_cb(const struct qurt_mavlink_msg *msg, void *p)
{
auto *driver = (UARTDriver_MAVLinkUDP *)p;
driver->_readbuf.write(msg->mav_msg, msg->data_length);
}
/*
try to push out one lump of pending bytes
return true if progress is made
*/
bool UARTDriver_MAVLinkUDP::_write_pending_bytes(void)
{
WITH_SEMAPHORE(_write_mutex);
// write any pending bytes
const uint32_t available_bytes = _writebuf.available();
uint16_t n = available_bytes;
if (n > 0) {
// send on MAVLink packet boundaries if possible
n = mavlink_packetise(_writebuf, n);
}
if (n <= 0) {
return false;
}
struct qurt_mavlink_msg msg;
if (n > sizeof(msg.mav_msg)) {
return false;
}
msg.seq = seq++;
msg.data_length = n;
n = _writebuf.read(msg.mav_msg, n);
if (n > 0) {
(void) sl_client_send_data((const uint8_t*)&msg, n + QURT_MAVLINK_MSG_HEADER_LEN);
}
return n > 0;
}
/*
setup baudrate for this local UART
*/
void UARTDriver_Local::_begin(uint32_t b, uint16_t rxS, uint16_t txS)
{
if (b == 0) {
// re-open not needed
return;
}
// QURT wants 420000 for CRSF, ArduPilot driver wants 416666
if (b == 416666) {
b = 420000;
}
UARTDriver::_begin(b, rxS, txS);
if (baudrate != b || fd == -1) {
int fd2 = sl_client_config_uart(port_id, b);
if (fd2 == -1 && fd != -1) {
// baudrate rejected, revert to last baudrate
sl_client_config_uart(port_id, baudrate);
}
if (fd2 != -1) {
baudrate = b;
fd = fd2;
}
}
}
/*
push out pending bytes
*/
bool UARTDriver_Local::_write_pending_bytes(void)
{
WITH_SEMAPHORE(_write_mutex);
if (fd == -1) {
return false;
}
uint32_t available;
const uint8_t *ptr = _writebuf.readptr(available);
if (ptr != nullptr) {
auto n = sl_client_uart_write(fd, (const char *)ptr, available);
if (n > 0) {
_writebuf.advance(n);
return true;
}
}
return false;
}
/*
read from the UART into _readbuf
*/
void UARTDriver_Local::_fill_read_buffer(void)
{
WITH_SEMAPHORE(_read_mutex);
if (fd == -1) {
return;
}
uint32_t n = _readbuf.space();
if (n > 512) {
n = 512;
}
char buf[n];
auto nread = sl_client_uart_read(fd, buf, sizeof(buf));
if (nread > 0) {
_readbuf.write((const uint8_t *)buf, nread);
receive_timestamp_us = AP_HAL::micros64();
}
}
/*
return timestamp estimate in microseconds for when the start of a
nbytes packet arrived on the uart.
*/
uint64_t UARTDriver_Local::receive_time_constraint_us(uint16_t nbytes)
{
uint64_t last_receive_us = receive_timestamp_us;
if (baudrate > 0) {
// assume 10 bits per byte
uint32_t transport_time_us = (1000000UL * 10UL / baudrate) * (nbytes + available());
last_receive_us -= transport_time_us;
}
return last_receive_us;
}

View File

@ -0,0 +1,132 @@
/*
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>
#include "ap_host/src/protocol.h"
class QURT::UARTDriver : public AP_HAL::UARTDriver
{
public:
bool is_initialized() override;
bool tx_pending() override;
/* Empty implementations of Stream virtual methods */
uint32_t txspace() override;
virtual bool _write_pending_bytes(void)
{
return false;
}
virtual void _timer_tick(void) override;
virtual void _fill_read_buffer(void) {}
virtual uint32_t bw_in_bytes_per_second() const override
{
return 5760;
}
virtual enum AP_HAL::UARTDriver::flow_control get_flow_control(void) override
{
return AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE;
}
protected:
virtual void _begin(uint32_t b, uint16_t rxS, uint16_t txS) override;
size_t _write(const uint8_t *buffer, size_t size) override;
ssize_t _read(uint8_t *buffer, uint16_t size) override WARN_IF_UNUSED;
void _end() override;
void _flush() override;
uint32_t _available() override;
bool _discard_input() override;
volatile bool _initialised;
ByteBuffer _readbuf{0};
ByteBuffer _writebuf{0};
QURT::Semaphore _read_mutex;
QURT::Semaphore _write_mutex;
};
/*
subclass for console output, maps to HAP_PRINTF
*/
class QURT::UARTDriver_Console : public QURT::UARTDriver
{
public:
using UARTDriver::UARTDriver;
virtual void printf(const char *fmt, ...) override;
};
/*
subclass for MAVLink UDP communications
*/
class QURT::UARTDriver_MAVLinkUDP : public QURT::UARTDriver
{
public:
UARTDriver_MAVLinkUDP(void);
bool _write_pending_bytes(void) override;
uint32_t bw_in_bytes_per_second() const override
{
return 250000 * 3;
}
enum AP_HAL::UARTDriver::flow_control get_flow_control(void) override
{
return AP_HAL::UARTDriver::FLOW_CONTROL_ENABLE;
}
private:
static void _mavlink_data_cb(const struct qurt_mavlink_msg *msg, void *p);
uint32_t seq;
};
/*
subclass for local UART communications
*/
class QURT::UARTDriver_Local : public QURT::UARTDriver
{
public:
UARTDriver_Local(uint8_t _port_id) : port_id(_port_id) {}
uint32_t bw_in_bytes_per_second() const override
{
return baudrate?baudrate/10:5760;
}
bool _write_pending_bytes(void) override;
void _begin(uint32_t b, uint16_t rxS, uint16_t txS) override;
void _fill_read_buffer(void) override;
uint32_t get_baud_rate() const override
{
return baudrate;
}
/*
return timestamp estimate in microseconds for when the start of
a nbytes packet arrived on the uart.
*/
uint64_t receive_time_constraint_us(uint16_t nbytes) override;
private:
const uint8_t port_id;
int fd = -1;
uint32_t baudrate;
uint64_t receive_timestamp_us;
};

View File

@ -0,0 +1,80 @@
#include <AP_HAL/AP_HAL.h>
#include "Semaphores.h"
#include <stdio.h>
extern const AP_HAL::HAL& hal;
#include "Util.h"
using namespace QURT;
extern "C" {
void *fc_heap_alloc(size_t size);
void fc_heap_free(void* ptr);
size_t fc_heap_size(void);
size_t fc_heap_usage(void);
}
uint32_t Util::available_memory(void)
{
return fc_heap_size() - fc_heap_usage();
}
#if ENABLE_HEAP
void *Util::allocate_heap_memory(size_t size)
{
struct heap *new_heap = (struct heap*)malloc(sizeof(struct heap));
if (new_heap != nullptr) {
new_heap->max_heap_size = size;
new_heap->current_heap_usage = 0;
}
return (void *)new_heap;
}
void *Util::heap_realloc(void *h, void *ptr, size_t old_size, size_t new_size)
{
if (h == nullptr) {
return nullptr;
}
struct heap *heapp = (struct heap*)h;
// extract appropriate headers. We use the old_size from the
// header not from the caller. We use SITL to catch cases they
// don't match (which would be a lua bug)
old_size = 0;
heap_allocation_header *old_header = nullptr;
if (ptr != nullptr) {
old_header = ((heap_allocation_header *)ptr) - 1;
old_size = old_header->allocation_size;
}
if ((heapp->current_heap_usage + new_size - old_size) > heapp->max_heap_size) {
// fail the allocation as we don't have the memory. Note that we don't simulate fragmentation
return nullptr;
}
heapp->current_heap_usage -= old_size;
if (new_size == 0) {
free(old_header);
return nullptr;
}
heap_allocation_header *new_header = (heap_allocation_header *)malloc(new_size + sizeof(heap_allocation_header));
if (new_header == nullptr) {
// can't get the new memory, old memory is kept
return nullptr;
}
heapp->current_heap_usage += new_size;
new_header->allocation_size = new_size;
void *new_mem = new_header + 1;
if (ptr == nullptr) {
return new_mem;
}
memcpy(new_mem, ptr, old_size > new_size ? new_size : old_size);
free(old_header);
return new_mem;
}
#endif // ENABLE_HEAP

View File

@ -0,0 +1,38 @@
#pragma once
#include <AP_HAL/AP_HAL.h>
#include "AP_HAL_QURT_Namespace.h"
class QURT::Util : public AP_HAL::Util
{
public:
/*
set HW RTC in UTC microseconds
*/
void set_hw_rtc(uint64_t time_utc_usec) override {}
/*
get system clock in UTC microseconds
*/
uint64_t get_hw_rtc() const override
{
return 0;
}
uint32_t available_memory(void) override;
#if ENABLE_HEAP
// heap functions, note that a heap once alloc'd cannot be dealloc'd
virtual void *allocate_heap_memory(size_t size) override;
virtual void *heap_realloc(void *h, void *ptr, size_t old_size, size_t new_size) override;
struct heap_allocation_header {
uint64_t allocation_size; // size of allocated block, not including this header
};
struct heap {
size_t max_heap_size;
size_t current_heap_usage;
};
#endif // ENABLE_HEAP
};

View File

@ -0,0 +1,22 @@
#ifndef SLPI_LINK_H
#define SLPI_LINK_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include <stdbool.h>
typedef void (*slpi_link_cb)(const uint8_t *data, uint32_t length_in_bytes);
int slpi_link_init(bool enable_debug_messages, slpi_link_cb callback, const char *library_name);
int slpi_link_get_time_offset(void);
int slpi_link_send(const uint8_t *data, uint32_t length_in_bytes);
void slpi_link_reset(void);
#ifdef __cplusplus
}
#endif
#endif // SLPI_LINK_H

View File

@ -0,0 +1,23 @@
#include "../inc/slpi_link.h"
int slpi_link_init(bool enable_debug_messages, slpi_link_cb callback, const char *library_name)
{
return 0;
}
int slpi_link_get_time_offset(void)
{
return 0;
}
int slpi_link_send(const uint8_t *data, uint32_t length_in_bytes)
{
return 0;
}
void slpi_link_reset(void)
{
return;
}

View File

@ -0,0 +1,188 @@
/*
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 ones that are broadcast capable */
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

View File

@ -0,0 +1,2 @@
const char *get_ipv4_broadcast(void);

View File

@ -0,0 +1,213 @@
#include <stdio.h>
#include <stdint.h>
#include <stdbool.h>
#include <string.h>
#include <unistd.h>
#include <pthread.h>
#include <signal.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <string.h>
#include <errno.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <unistd.h>
#include <stdlib.h>
#include "slpi_link.h"
#include "protocol.h"
#include "getifaddrs.h"
volatile bool _running = false;
static bool enable_debug = false;
static bool enable_remote_debug = false;
static int socket_fd;
static bool connected;
static struct sockaddr_in remote_addr;
#define UDP_OUT_PORT 14551
#define SO_NAME "ArduPilot.so"
// directory for logs, parameters, terrain etc
#define DATA_DIRECTORY "/data/APM"
static void shutdown_signal_handler(int signo)
{
switch (signo) {
case SIGINT: // normal ctrl-c shutdown interrupt
_running = false;
fprintf(stderr, "\nreceived SIGINT Ctrl-C\n");
break;
case SIGTERM: // catchable terminate signal
_running = false;
fprintf(stderr, "\nreceived SIGTERM\n");
break;
case SIGHUP:
// terminal closed or disconnected, carry on anyway
fprintf(stderr, "\nreceived SIGHUP, continuing anyway\n");
break;
default:
fprintf(stderr, "\nreceived signal %d\n", signo);
break;
}
return;
}
static uint32_t num_params = 0;
static uint32_t expected_seq = 0;
static void receive_callback(const uint8_t *data, uint32_t length_in_bytes)
{
if (enable_debug) {
printf("Got %u bytes in receive callback\n", length_in_bytes);
}
const auto *msg = (const struct qurt_mavlink_msg *)data;
if (length_in_bytes < QURT_MAVLINK_MSG_HEADER_LEN) {
printf("Invalid length_in_bytes %d", length_in_bytes);
return;
}
if (msg->data_length + QURT_MAVLINK_MSG_HEADER_LEN != length_in_bytes) {
printf("Invalid lengths %d %d\n", msg->data_length, length_in_bytes);
return;
}
if (msg->seq != expected_seq) {
printf("Invalid seq %u %u\n", msg->seq, expected_seq);
}
expected_seq = msg->seq + 1;
switch (msg->msg_id) {
case QURT_MSG_ID_MAVLINK_MSG: {
if (_running) {
const auto bytes_sent = sendto(socket_fd, msg->mav_msg, msg->data_length, 0, (struct sockaddr *)&remote_addr, sizeof(remote_addr));
if (bytes_sent <= 0) {
fprintf(stderr, "Send to GCS failed\n");
}
}
break;
}
default:
fprintf(stderr, "Got unknown message id %d\n", msg->msg_id);
break;
}
}
/*
setup directories for the hexagon code to use
*/
static void setup_directores(void)
{
struct stat st;
mkdir(DATA_DIRECTORY, 0777);
chmod(DATA_DIRECTORY, 0777);
if (stat(DATA_DIRECTORY, &st) != 0 || !S_ISDIR(st.st_mode)) {
printf("Unable to create %s", DATA_DIRECTORY);
exit(1);
}
}
int main()
{
printf("Starting up\n");
setup_directores();
// make the sigaction struct for shutdown signals
// sa_handler and sa_sigaction is a union, only set one
struct sigaction action;
action.sa_handler = shutdown_signal_handler;
sigemptyset(&action.sa_mask);
action.sa_flags = 0;
// set actions
if (sigaction(SIGINT, &action, NULL) < 0) {
fprintf(stderr, "ERROR: failed to set sigaction\n");
return -1;
}
if (sigaction(SIGTERM, &action, NULL) < 0) {
fprintf(stderr, "ERROR: failed to set sigaction\n");
return -1;
}
if (sigaction(SIGHUP, &action, NULL) < 0) {
fprintf(stderr, "ERROR: failed to set sigaction\n");
return -1;
}
if (slpi_link_init(enable_remote_debug, &receive_callback, SO_NAME) != 0) {
fprintf(stderr, "Error with init\n");
sleep(1);
slpi_link_reset();
return -1;
} else if (enable_debug) {
printf("slpi_link_initialize succeeded\n");
}
//initialize socket and structure
socket_fd = socket(AF_INET, SOCK_DGRAM, 0);
if (socket_fd == -1) {
fprintf(stderr, "Could not create socket");
return -1;
}
const char *bcast_address = get_ipv4_broadcast();
printf("Broadcast address=%s\n", bcast_address);
inet_aton(bcast_address, &remote_addr.sin_addr);
remote_addr.sin_family = AF_INET;
remote_addr.sin_port = htons(UDP_OUT_PORT);
int one = 1;
setsockopt(socket_fd,SOL_SOCKET,SO_BROADCAST,(char *)&one,sizeof(one));
struct sockaddr_in any {};
any.sin_addr.s_addr = INADDR_ANY;
any.sin_port = htons(15550);
if (bind(socket_fd, (struct sockaddr *)&any, sizeof(any)) == 0) {
printf("Bind OK\n");
}
printf("Waiting for receive\n");
printf("Enter ctrl-c to exit\n");
_running = true;
uint32_t next_seq = 0;
while (_running) {
struct qurt_mavlink_msg msg {};
struct sockaddr_in from;
socklen_t fromlen = sizeof(from);
uint32_t bytes_received = recvfrom(socket_fd, msg.mav_msg, sizeof(msg.mav_msg), 0,
(struct sockaddr*)&from, &fromlen);
if (bytes_received > 0 && !connected) {
remote_addr = from;
connected = true;
printf("Connnected to %s\n", inet_ntoa(from.sin_addr));
}
if (bytes_received < 0) {
fprintf(stderr, "Received failed");
continue;
}
if (bytes_received > sizeof(msg.mav_msg)) {
printf("Invalid bytes_received %d\n", bytes_received);
continue;
}
msg.data_length = bytes_received;
msg.seq = next_seq++;
// printf("Message received. %d bytes\n", bytes_received);
if (slpi_link_send((const uint8_t*) &msg, bytes_received + QURT_MAVLINK_MSG_HEADER_LEN)) {
fprintf(stderr, "slpi_link_send_data failed\n");
}
}
printf("Reseting SLPI\n");
// Send reset to SLPI
slpi_link_reset();
sleep(1);
printf("Exiting\n");
return 0;
}

View File

@ -0,0 +1,18 @@
#include <stdint.h>
// #include <GCS_MAVLink/GCS_MAVLink.h>
#define MAVLINK_MAX_PAYLOAD_LEN 255
#pragma once
#define QURT_MSG_ID_MAVLINK_MSG 1
struct __attribute__((__packed__)) qurt_mavlink_msg {
uint8_t msg_id{QURT_MSG_ID_MAVLINK_MSG};
uint16_t data_length;
uint32_t seq;
uint8_t mav_msg[MAVLINK_MAX_PAYLOAD_LEN];
};
#define QURT_MAVLINK_MSG_HEADER_LEN 7

View File

@ -0,0 +1,49 @@
#define __EXPORT __attribute__ ((visibility ("default")))
#ifndef __cplusplus
#error "C++ should be defined!!!"
#endif
#include <stdint.h>
// Functions that are called by the SLPI LINK server into AP client
extern "C" {
// Called by the SLPI LINK server to initialize and start AP
int slpi_link_client_init(void) __EXPORT;
// Called by the SLPI LINK server when there is a new message for AP
int slpi_link_client_receive(const uint8_t *data, int data_len_in_bytes) __EXPORT;
}
// Functions in SLPI LINK server that are called by AP client into DSP
extern "C" {
// Send a message to the applications processor
int sl_client_send_data(const uint8_t *data, int data_len_in_bytes);
void sl_client_register_fatal_error_cb(void (*func)(void));
// Interrupt callback registration
int sl_client_register_interrupt_callback(int (*func)(int, void*, void*), void* arg);
// Get DSP CPU utilization (0 - 100)
int sl_client_get_cpu_utilization(void);
// I2C interface API
int sl_client_config_i2c_bus(uint8_t bus_number, uint8_t address, uint32_t frequency);
void sl_client_set_address_i2c_bus(int fd, uint8_t address);
int sl_client_i2c_transfer(int fd, const uint8_t *send, const unsigned send_len, uint8_t *recv, const unsigned recv_len);
// SPI interface API
int sl_client_spi_transfer(int fd, const uint8_t *send, uint8_t *recv, const unsigned len);
int sl_client_config_spi_bus(void);
// UART interface API
int sl_client_config_uart(uint8_t port_number, uint32_t speed);
int sl_client_uart_write(int fd, const char *data, const unsigned data_len);
int sl_client_uart_read(int fd, char *buffer, const unsigned buffer_len);
}
// IDs for serial ports
#define QURT_UART_GPS 6
#define QURT_UART_RCIN 7
#define QURT_UART_ESC 2

View File

@ -0,0 +1,84 @@
/*
wrappers around core memory allocation functions from libc
*/
#include <stdlib.h>
#include <AP_HAL/AP_HAL.h>
#include <new>
const std::nothrow_t std::nothrow;
extern "C" {
void *fc_heap_alloc(size_t size);
void fc_heap_free(void* ptr);
void *__wrap_malloc(size_t size);
void __wrap_free(void *ptr);
void *__wrap_calloc(size_t nmemb, size_t size);
void *__wrap_realloc(void *ptr, size_t size);
}
#define HEAP_HEADER_MAGIC 0x1763247F
typedef struct {
size_t size;
uint32_t magic;
} heap_header_t;
void *__wrap_malloc(size_t size)
{
// fc_heap_alloc guarantees zero filled memory
auto *ret = (heap_header_t *)fc_heap_alloc(size+sizeof(heap_header_t));
if (ret == nullptr) {
return nullptr;
}
ret->size = size;
ret->magic = HEAP_HEADER_MAGIC;
return (void*)(ret+1);
}
void __wrap_free(void *ptr)
{
if (ptr == nullptr) {
return;
}
auto *h = ((heap_header_t *)ptr)-1;
if (h->magic != HEAP_HEADER_MAGIC) {
AP_HAL::panic("free: bad memory header 0x%x", unsigned(h->magic));
}
fc_heap_free((void*)h);
}
void *__wrap_calloc(size_t nmemb, size_t size)
{
return __wrap_malloc(nmemb*size);
}
void *__wrap_realloc(void *ptr, size_t size)
{
if (size == 0) {
// a free
__wrap_free(ptr);
return nullptr;
}
if (ptr == nullptr) {
// new allocation
return __wrap_malloc(size);
}
auto *h = ((heap_header_t *)ptr)-1;
if (h->magic != HEAP_HEADER_MAGIC) {
AP_HAL::panic("realloc: bad memory header 0x%x", unsigned(h->magic));
}
void *ret = __wrap_malloc(size);
if (ret == nullptr) {
return nullptr;
}
const size_t copy_size = size > h->size? h->size : size;
memcpy(ret, ptr, copy_size);
__wrap_free(ptr);
return ret;
}

View File

@ -0,0 +1,210 @@
#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 <pthread.h>
#include "replace.h"
#include "interface.h"
#include "ap_host/src/protocol.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;
}
void *memmem(const void *haystack, size_t haystacklen,
const void *needle, size_t needlelen)
{
if (needlelen == 0) {
return const_cast<void*>(haystack);
}
while (haystacklen >= needlelen) {
char *p = (char *)memchr(haystack, *(const char *)needle,
haystacklen-(needlelen-1));
if (!p) {
return NULL;
}
if (memcmp(p, needle, needlelen) == 0) {
return p;
}
haystack = p+1;
haystacklen -= (p - (const char *)haystack) + 1;
}
return NULL;
}
char *strndup(const char *s, size_t n)
{
char *ret;
n = strnlen(s, n);
ret = (char*)malloc(n+1);
if (!ret) {
return NULL;
}
memcpy(ret, s, n);
ret[n] = 0;
return ret;
}
int pthread_cond_init(pthread_cond_t *cond, pthread_condattr_t *attr)
{
return 0;
}
// INVESTIGATE: What is this needed on QURT?
int apfs_rename(const char *oldpath, const char *newpath)
{
return 0;
}
// INVESTIGATE: Seems important :-)
int ArduPilot_main(int argc, const char *argv[])
{
return 0;
}
char *__wrap_strdup(const char *s);
}
extern "C" int qurt_ardupilot_main(int argc, char* const argv[]);
int slpi_link_client_init(void)
{
HAP_PRINTF("About to call qurt_ardupilot_main %p", &qurt_ardupilot_main);
qurt_ardupilot_main(0, NULL);
return 0;
}
typedef void (*mavlink_data_callback_t)(const struct qurt_mavlink_msg *msg, void* p);
static mavlink_data_callback_t mav_cb;
static void *mav_cb_ptr;
static uint32_t expected_seq;
void register_mavlink_data_callback(mavlink_data_callback_t func, void *p)
{
mav_cb = func;
mav_cb_ptr = p;
}
int slpi_link_client_receive(const uint8_t *data, int data_len_in_bytes)
{
if (data_len_in_bytes < QURT_MAVLINK_MSG_HEADER_LEN) {
return 0;
}
const auto *msg = (struct qurt_mavlink_msg *)data;
if (msg->data_length + QURT_MAVLINK_MSG_HEADER_LEN != data_len_in_bytes) {
return 0;
}
if (msg->seq != expected_seq) {
HAP_PRINTF("Bad sequence %u %u", msg->seq, expected_seq);
}
expected_seq = msg->seq + 1;
switch (msg->msg_id) {
case QURT_MSG_ID_MAVLINK_MSG: {
if (mav_cb) {
mav_cb(msg, mav_cb_ptr);
}
break;
}
default:
HAP_PRINTF("Got unknown message id %d", msg->msg_id);
break;
}
return 0;
}
int __wrap_printf(const char *fmt, ...)
{
va_list ap;
char buf[300];
va_start(ap, fmt);
vsnprintf(buf, sizeof(buf), fmt, ap);
va_end(ap);
HAP_PRINTF(buf);
return 0;
}
/*
free() on strdup return on QURT causes a memory fault
*/
char *__wrap_strdup(const char *s)
{
return strndup(s, strlen(s));
}

View File

@ -0,0 +1,49 @@
#pragma once
#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>
#ifdef __cplusplus
extern "C" {
#endif
/*
work around broken headers
*/
size_t strnlen(const char *s, size_t maxlen);
char *strndup(const char *s, size_t n);
int asprintf(char **, const char *, ...);
off_t lseek(int, off_t, int);
DIR *opendir (const char *);
int unlink(const char *pathname);
void *memmem(const void *haystack, size_t haystacklen,
const void *needle, size_t needlelen);
//typedef int32_t pid_t;
pid_t getpid (void);
void HAP_printf(const char *file, int line, const char *fmt, ...);
int __wrap_printf(const char *fmt, ...);
#ifdef __cplusplus
}
#endif
#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)
// missing defines from math.h
#define M_SQRT1_2 0.70710678118654752440

View File

@ -0,0 +1,65 @@
#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 "replace.h"
#include <fenv.h>
#include <AP_Math/div1000.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("PANIC: %s", buf);
qurt_timer_sleep(100000);
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 + uint64_div1000(ts.tv_nsec);
ret -= state.start_time;
return ret;
}
uint64_t millis64()
{
return uint64_div1000(micros64());
}
} // namespace AP_HAL