diff --git a/libraries/AP_HAL_QURT/AP_HAL_QURT.h b/libraries/AP_HAL_QURT/AP_HAL_QURT.h new file mode 100644 index 0000000000..2c753c203d --- /dev/null +++ b/libraries/AP_HAL_QURT/AP_HAL_QURT.h @@ -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 . + */ +#pragma once + +/* Your layer exports should depend on AP_HAL.h ONLY. */ +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_QURT + +#include "HAL_QURT_Class.h" +#include "AP_HAL_QURT_Main.h" + +#endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_HAL_QURT/AP_HAL_QURT_Main.h b/libraries/AP_HAL_QURT/AP_HAL_QURT_Main.h new file mode 100644 index 0000000000..5bb282c3f0 --- /dev/null +++ b/libraries/AP_HAL_QURT/AP_HAL_QURT_Main.h @@ -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 . + */ +#pragma once + +#define AP_MAIN qurt_ardupilot_main + diff --git a/libraries/AP_HAL_QURT/AP_HAL_QURT_Namespace.h b/libraries/AP_HAL_QURT/AP_HAL_QURT_Namespace.h new file mode 100644 index 0000000000..6ddfdb7142 --- /dev/null +++ b/libraries/AP_HAL_QURT/AP_HAL_QURT_Namespace.h @@ -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 . + */ + +#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; +} + diff --git a/libraries/AP_HAL_QURT/AP_HAL_QURT_Private.h b/libraries/AP_HAL_QURT/AP_HAL_QURT_Private.h new file mode 100644 index 0000000000..95a6b97003 --- /dev/null +++ b/libraries/AP_HAL_QURT/AP_HAL_QURT_Private.h @@ -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 . + */ +#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" diff --git a/libraries/AP_HAL_QURT/AnalogIn.cpp b/libraries/AP_HAL_QURT/AnalogIn.cpp new file mode 100644 index 0000000000..88e051a52d --- /dev/null +++ b/libraries/AP_HAL_QURT/AnalogIn.cpp @@ -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; +} diff --git a/libraries/AP_HAL_QURT/AnalogIn.h b/libraries/AP_HAL_QURT/AnalogIn.h new file mode 100644 index 0000000000..cecd6fb735 --- /dev/null +++ b/libraries/AP_HAL_QURT/AnalogIn.h @@ -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; +}; diff --git a/libraries/AP_HAL_QURT/DeviceBus.cpp b/libraries/AP_HAL_QURT/DeviceBus.cpp new file mode 100644 index 0000000000..c361fdaf84 --- /dev/null +++ b/libraries/AP_HAL_QURT/DeviceBus.cpp @@ -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 . + */ + +#include "DeviceBus.h" + +#include +#include +#include + +#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; +} diff --git a/libraries/AP_HAL_QURT/DeviceBus.h b/libraries/AP_HAL_QURT/DeviceBus.h new file mode 100644 index 0000000000..d62db9ccf2 --- /dev/null +++ b/libraries/AP_HAL_QURT/DeviceBus.h @@ -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 . + */ + +#pragma once + +#include +#include +#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; +}; + +} + + diff --git a/libraries/AP_HAL_QURT/HAL_QURT_Class.cpp b/libraries/AP_HAL_QURT/HAL_QURT_Class.cpp new file mode 100644 index 0000000000..60904b7e39 --- /dev/null +++ b/libraries/AP_HAL_QURT/HAL_QURT_Class.cpp @@ -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 . + */ + +#include + +#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 +#include +#include +#include +#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(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; +} diff --git a/libraries/AP_HAL_QURT/HAL_QURT_Class.h b/libraries/AP_HAL_QURT/HAL_QURT_Class.h new file mode 100644 index 0000000000..9c23f889af --- /dev/null +++ b/libraries/AP_HAL_QURT/HAL_QURT_Class.h @@ -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 . + */ +#pragma once + +#include + +#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); +}; diff --git a/libraries/AP_HAL_QURT/I2CDevice.cpp b/libraries/AP_HAL_QURT/I2CDevice.cpp new file mode 100644 index 0000000000..312c8defb7 --- /dev/null +++ b/libraries/AP_HAL_QURT/I2CDevice.cpp @@ -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 . + */ +#include "I2CDevice.h" + +#include +#include +#include +#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 +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(nullptr); + } + auto dev = AP_HAL::OwnPtr(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(); +} diff --git a/libraries/AP_HAL_QURT/I2CDevice.h b/libraries/AP_HAL_QURT/I2CDevice.h new file mode 100644 index 0000000000..4e1c44e8f0 --- /dev/null +++ b/libraries/AP_HAL_QURT/I2CDevice.h @@ -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 . + */ +#pragma once + +#include + +#include +#include +#include + +#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(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(i2c_mgr); + } + + AP_HAL::OwnPtr 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; +}; +} diff --git a/libraries/AP_HAL_QURT/RCInput.cpp b/libraries/AP_HAL_QURT/RCInput.cpp new file mode 100644 index 0000000000..a52f95bf51 --- /dev/null +++ b/libraries/AP_HAL_QURT/RCInput.cpp @@ -0,0 +1,58 @@ +#include "RCInput.h" +#include + +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; + } +} diff --git a/libraries/AP_HAL_QURT/RCInput.h b/libraries/AP_HAL_QURT/RCInput.h new file mode 100644 index 0000000000..96befb4076 --- /dev/null +++ b/libraries/AP_HAL_QURT/RCInput.h @@ -0,0 +1,28 @@ +#pragma once + +#include "AP_HAL_QURT.h" +#include + +#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; +}; diff --git a/libraries/AP_HAL_QURT/RCOutput.cpp b/libraries/AP_HAL_QURT/RCOutput.cpp new file mode 100644 index 0000000000..65187c329a --- /dev/null +++ b/libraries/AP_HAL_QURT/RCOutput.cpp @@ -0,0 +1,225 @@ +#include + +#include "RCOutput.h" +#include + +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<= ARRAY_SIZE(period)) { + return; + } + enable_mask &= ~1U<= 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(); + } +} diff --git a/libraries/AP_HAL_QURT/RCOutput.h b/libraries/AP_HAL_QURT/RCOutput.h new file mode 100644 index 0000000000..2333c39655 --- /dev/null +++ b/libraries/AP_HAL_QURT/RCOutput.h @@ -0,0 +1,75 @@ +#pragma once + +#include +#include "AP_HAL_QURT.h" +#include + +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; +}; diff --git a/libraries/AP_HAL_QURT/SPIDevice.cpp b/libraries/AP_HAL_QURT/SPIDevice.cpp new file mode 100644 index 0000000000..616cf6a76f --- /dev/null +++ b/libraries/AP_HAL_QURT/SPIDevice.cpp @@ -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 . + */ + +#include "SPIDevice.h" + +#include +#include +#include +#include "Scheduler.h" +#include "Semaphores.h" +#include "interface.h" +#include + +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 +SPIDeviceManager::get_device(const char *name) +{ + uint8_t i; + for (i = 0; i(nullptr); + } + + if (spi_bus == nullptr) { + spi_bus = new SPIBus(); + } + + return AP_HAL::OwnPtr(new SPIDevice(name, *spi_bus)); +} + diff --git a/libraries/AP_HAL_QURT/SPIDevice.h b/libraries/AP_HAL_QURT/SPIDevice.h new file mode 100644 index 0000000000..65a5585820 --- /dev/null +++ b/libraries/AP_HAL_QURT/SPIDevice.h @@ -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 . + */ + +#pragma once + +#include +#include +#include +#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 get_device(const char *name) override; +}; +} + diff --git a/libraries/AP_HAL_QURT/Scheduler.cpp b/libraries/AP_HAL_QURT/Scheduler.cpp new file mode 100644 index 0000000000..f4fa39aaff --- /dev/null +++ b/libraries/AP_HAL_QURT/Scheduler.cpp @@ -0,0 +1,344 @@ +#include + +#include "AP_HAL_QURT.h" +#include "Scheduler.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "UARTDriver.h" +#include "Storage.h" +#include "RCOutput.h" +#include "RCInput.h" +#include +#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, ¶m); + + 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, ¶m); + + 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, ¶m); + + 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; +} diff --git a/libraries/AP_HAL_QURT/Scheduler.h b/libraries/AP_HAL_QURT/Scheduler.h new file mode 100644 index 0000000000..8c4fd863a9 --- /dev/null +++ b/libraries/AP_HAL_QURT/Scheduler.h @@ -0,0 +1,84 @@ +#pragma once + +#include +#if CONFIG_HAL_BOARD == HAL_BOARD_QURT +#include "AP_HAL_QURT_Namespace.h" +#include +#include +#include + +#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 + + + diff --git a/libraries/AP_HAL_QURT/Semaphores.cpp b/libraries/AP_HAL_QURT/Semaphores.cpp new file mode 100644 index 0000000000..1c7c280e0a --- /dev/null +++ b/libraries/AP_HAL_QURT/Semaphores.cpp @@ -0,0 +1,111 @@ +#include + +#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); + } +} diff --git a/libraries/AP_HAL_QURT/Semaphores.h b/libraries/AP_HAL_QURT/Semaphores.h new file mode 100644 index 0000000000..61a36795e0 --- /dev/null +++ b/libraries/AP_HAL_QURT/Semaphores.h @@ -0,0 +1,42 @@ +#pragma once + +#include +#include +#include +#include + +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; +}; + +} diff --git a/libraries/AP_HAL_QURT/Storage.cpp b/libraries/AP_HAL_QURT/Storage.cpp new file mode 100644 index 0000000000..a4bf15acd1 --- /dev/null +++ b/libraries/AP_HAL_QURT/Storage.cpp @@ -0,0 +1,195 @@ +#include "Storage.h" + +#include +#include +#include +#include +#include + +#include + +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; + 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_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< + +#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); + } + + + 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]; +}; + +} diff --git a/libraries/AP_HAL_QURT/Thread.cpp b/libraries/AP_HAL_QURT/Thread.cpp new file mode 100644 index 0000000000..246e7bc903 --- /dev/null +++ b/libraries/AP_HAL_QURT/Thread.cpp @@ -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 . + */ +#include "Thread.h" + +#include +#include +#include +#include +#include + +#include +#include +#include "Scheduler.h" + +extern const AP_HAL::HAL &hal; + +namespace QURT +{ + + +void *Thread::_run_trampoline(void *arg) +{ + Thread *thread = static_cast(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, ¶m)) != 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; +} + +} diff --git a/libraries/AP_HAL_QURT/Thread.h b/libraries/AP_HAL_QURT/Thread.h new file mode 100644 index 0000000000..69985a04d1 --- /dev/null +++ b/libraries/AP_HAL_QURT/Thread.h @@ -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 . + */ +#pragma once + +#include +#include +#include + +#include + +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; +}; + +} diff --git a/libraries/AP_HAL_QURT/UARTDriver.cpp b/libraries/AP_HAL_QURT/UARTDriver.cpp new file mode 100644 index 0000000000..9bb56957f4 --- /dev/null +++ b/libraries/AP_HAL_QURT/UARTDriver.cpp @@ -0,0 +1,278 @@ + +#include "interface.h" +#include "UARTDriver.h" +#include + +#if HAL_GCS_ENABLED +#include +#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; +} diff --git a/libraries/AP_HAL_QURT/UARTDriver.h b/libraries/AP_HAL_QURT/UARTDriver.h new file mode 100644 index 0000000000..61bfeefc0b --- /dev/null +++ b/libraries/AP_HAL_QURT/UARTDriver.h @@ -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 . + */ + +#pragma once + +#include "AP_HAL_QURT.h" +#include "Semaphores.h" +#include +#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; +}; diff --git a/libraries/AP_HAL_QURT/Util.cpp b/libraries/AP_HAL_QURT/Util.cpp new file mode 100644 index 0000000000..164d301b54 --- /dev/null +++ b/libraries/AP_HAL_QURT/Util.cpp @@ -0,0 +1,80 @@ +#include +#include "Semaphores.h" +#include + +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 diff --git a/libraries/AP_HAL_QURT/Util.h b/libraries/AP_HAL_QURT/Util.h new file mode 100644 index 0000000000..7c389703da --- /dev/null +++ b/libraries/AP_HAL_QURT/Util.h @@ -0,0 +1,38 @@ +#pragma once + +#include +#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 +}; diff --git a/libraries/AP_HAL_QURT/ap_host/libslpi-link-api/inc/slpi_link.h b/libraries/AP_HAL_QURT/ap_host/libslpi-link-api/inc/slpi_link.h new file mode 100644 index 0000000000..863ad969de --- /dev/null +++ b/libraries/AP_HAL_QURT/ap_host/libslpi-link-api/inc/slpi_link.h @@ -0,0 +1,22 @@ +#ifndef SLPI_LINK_H +#define SLPI_LINK_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +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 diff --git a/libraries/AP_HAL_QURT/ap_host/libslpi-link-api/src/slpi_link_stub.c b/libraries/AP_HAL_QURT/ap_host/libslpi-link-api/src/slpi_link_stub.c new file mode 100644 index 0000000000..e82fe7a190 --- /dev/null +++ b/libraries/AP_HAL_QURT/ap_host/libslpi-link-api/src/slpi_link_stub.c @@ -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; +} + diff --git a/libraries/AP_HAL_QURT/ap_host/src/getifaddrs.cpp b/libraries/AP_HAL_QURT/ap_host/src/getifaddrs.cpp new file mode 100644 index 0000000000..54ba54e184 --- /dev/null +++ b/libraries/AP_HAL_QURT/ap_host/src/getifaddrs.cpp @@ -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 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 . +*/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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 diff --git a/libraries/AP_HAL_QURT/ap_host/src/getifaddrs.h b/libraries/AP_HAL_QURT/ap_host/src/getifaddrs.h new file mode 100644 index 0000000000..6fe4105195 --- /dev/null +++ b/libraries/AP_HAL_QURT/ap_host/src/getifaddrs.h @@ -0,0 +1,2 @@ +const char *get_ipv4_broadcast(void); + diff --git a/libraries/AP_HAL_QURT/ap_host/src/main.cpp b/libraries/AP_HAL_QURT/ap_host/src/main.cpp new file mode 100644 index 0000000000..cd57c3af9b --- /dev/null +++ b/libraries/AP_HAL_QURT/ap_host/src/main.cpp @@ -0,0 +1,213 @@ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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; +} + diff --git a/libraries/AP_HAL_QURT/ap_host/src/protocol.h b/libraries/AP_HAL_QURT/ap_host/src/protocol.h new file mode 100644 index 0000000000..f3b3d3eb4a --- /dev/null +++ b/libraries/AP_HAL_QURT/ap_host/src/protocol.h @@ -0,0 +1,18 @@ + +#include + +// #include +#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 diff --git a/libraries/AP_HAL_QURT/interface.h b/libraries/AP_HAL_QURT/interface.h new file mode 100644 index 0000000000..fb3019f127 --- /dev/null +++ b/libraries/AP_HAL_QURT/interface.h @@ -0,0 +1,49 @@ +#define __EXPORT __attribute__ ((visibility ("default"))) + +#ifndef __cplusplus +#error "C++ should be defined!!!" +#endif + +#include + +// 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 diff --git a/libraries/AP_HAL_QURT/malloc.cpp b/libraries/AP_HAL_QURT/malloc.cpp new file mode 100644 index 0000000000..5a3c40ed2f --- /dev/null +++ b/libraries/AP_HAL_QURT/malloc.cpp @@ -0,0 +1,84 @@ +/* + wrappers around core memory allocation functions from libc +*/ + +#include +#include +#include + +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; +} + diff --git a/libraries/AP_HAL_QURT/replace.cpp b/libraries/AP_HAL_QURT/replace.cpp new file mode 100644 index 0000000000..0425079b89 --- /dev/null +++ b/libraries/AP_HAL_QURT/replace.cpp @@ -0,0 +1,210 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#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(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)); +} diff --git a/libraries/AP_HAL_QURT/replace.h b/libraries/AP_HAL_QURT/replace.h new file mode 100644 index 0000000000..ca9b2a035b --- /dev/null +++ b/libraries/AP_HAL_QURT/replace.h @@ -0,0 +1,49 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include +#include + +#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 diff --git a/libraries/AP_HAL_QURT/system.cpp b/libraries/AP_HAL_QURT/system.cpp new file mode 100644 index 0000000000..f6d8b8fa5d --- /dev/null +++ b/libraries/AP_HAL_QURT/system.cpp @@ -0,0 +1,65 @@ +#include +#include +#include +#include + +#include +#include + +#include "replace.h" +#include +#include + +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