mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 22:48:29 -04:00
HAL_QURT: revived QURT HAL
based around new SDK for ModalAI Voxl2 vehicles
This commit is contained in:
parent
354a40651d
commit
2fe08b6177
libraries/AP_HAL_QURT
AP_HAL_QURT.hAP_HAL_QURT_Main.hAP_HAL_QURT_Namespace.hAP_HAL_QURT_Private.hAnalogIn.cppAnalogIn.hDeviceBus.cppDeviceBus.hHAL_QURT_Class.cppHAL_QURT_Class.hI2CDevice.cppI2CDevice.hRCInput.cppRCInput.hRCOutput.cppRCOutput.hSPIDevice.cppSPIDevice.hScheduler.cppScheduler.hSemaphores.cppSemaphores.hStorage.cppStorage.hThread.cppThread.hUARTDriver.cppUARTDriver.hUtil.cppUtil.h
ap_host
interface.hmalloc.cppreplace.cppreplace.hsystem.cpp
25
libraries/AP_HAL_QURT/AP_HAL_QURT.h
Normal file
25
libraries/AP_HAL_QURT/AP_HAL_QURT.h
Normal file
@ -0,0 +1,25 @@
|
||||
/*
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
/* Your layer exports should depend on AP_HAL.h ONLY. */
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_QURT
|
||||
|
||||
#include "HAL_QURT_Class.h"
|
||||
#include "AP_HAL_QURT_Main.h"
|
||||
|
||||
#endif // CONFIG_HAL_BOARD
|
18
libraries/AP_HAL_QURT/AP_HAL_QURT_Main.h
Normal file
18
libraries/AP_HAL_QURT/AP_HAL_QURT_Main.h
Normal file
@ -0,0 +1,18 @@
|
||||
/*
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#define AP_MAIN qurt_ardupilot_main
|
||||
|
35
libraries/AP_HAL_QURT/AP_HAL_QURT_Namespace.h
Normal file
35
libraries/AP_HAL_QURT/AP_HAL_QURT_Namespace.h
Normal file
@ -0,0 +1,35 @@
|
||||
/*
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
namespace QURT
|
||||
{
|
||||
class UARTDriver;
|
||||
class UARTDriver_Console;
|
||||
class UARTDriver_MAVLinkUDP;
|
||||
class UARTDriver_Local;
|
||||
class UDPDriver;
|
||||
class Util;
|
||||
class Scheduler;
|
||||
class Storage;
|
||||
class Util;
|
||||
class Semaphore;
|
||||
class RCInput;
|
||||
class RCOutput;
|
||||
class AnalogSource;
|
||||
class AnalogIn;
|
||||
}
|
||||
|
22
libraries/AP_HAL_QURT/AP_HAL_QURT_Private.h
Normal file
22
libraries/AP_HAL_QURT/AP_HAL_QURT_Private.h
Normal file
@ -0,0 +1,22 @@
|
||||
/*
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
/* Umbrella header for all private headers of the AP_HAL_QURT module.
|
||||
* Only import this header from inside AP_HAL_QURT
|
||||
*/
|
||||
|
||||
#include "UARTDriver.h"
|
||||
#include "Util.h"
|
80
libraries/AP_HAL_QURT/AnalogIn.cpp
Normal file
80
libraries/AP_HAL_QURT/AnalogIn.cpp
Normal file
@ -0,0 +1,80 @@
|
||||
#include "AnalogIn.h"
|
||||
#include "RCOutput.h"
|
||||
|
||||
using namespace QURT;
|
||||
|
||||
#define ANALOG_PIN_VOLTAGE 1
|
||||
#define ANALOG_PIN_CURRENT 2
|
||||
|
||||
#define NUM_CHANNELS 10
|
||||
|
||||
extern const AP_HAL::HAL &hal;
|
||||
|
||||
// static table of voltage sources
|
||||
static AnalogSource sources[NUM_CHANNELS];
|
||||
|
||||
AnalogIn::AnalogIn()
|
||||
{
|
||||
}
|
||||
|
||||
float AnalogSource::read_average()
|
||||
{
|
||||
return read_latest();
|
||||
}
|
||||
|
||||
float AnalogSource::voltage_average()
|
||||
{
|
||||
return read_latest();
|
||||
}
|
||||
|
||||
float AnalogSource::voltage_latest()
|
||||
{
|
||||
return read_latest();
|
||||
}
|
||||
|
||||
float AnalogSource::read_latest()
|
||||
{
|
||||
const auto *rcout = (QURT::RCOutput *)hal.rcout;
|
||||
switch (pin) {
|
||||
case ANALOG_PIN_VOLTAGE:
|
||||
return rcout->get_voltage();
|
||||
case ANALOG_PIN_CURRENT:
|
||||
return rcout->get_current();
|
||||
default:
|
||||
break;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
bool AnalogSource::set_pin(uint8_t p)
|
||||
{
|
||||
switch (p) {
|
||||
case ANALOG_PIN_VOLTAGE:
|
||||
case ANALOG_PIN_CURRENT:
|
||||
pin = p;
|
||||
return true;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
void AnalogIn::init()
|
||||
{
|
||||
}
|
||||
|
||||
AP_HAL::AnalogSource* AnalogIn::channel(int16_t n)
|
||||
{
|
||||
if (next_chan >= ARRAY_SIZE(sources)) {
|
||||
return nullptr;
|
||||
}
|
||||
return &sources[next_chan++];
|
||||
}
|
||||
|
||||
/*
|
||||
not available, report 5v to keep GCS happy
|
||||
*/
|
||||
float AnalogIn::board_voltage(void)
|
||||
{
|
||||
return 5.0f;
|
||||
}
|
29
libraries/AP_HAL_QURT/AnalogIn.h
Normal file
29
libraries/AP_HAL_QURT/AnalogIn.h
Normal file
@ -0,0 +1,29 @@
|
||||
#pragma once
|
||||
|
||||
#include "AP_HAL_QURT.h"
|
||||
|
||||
class QURT::AnalogSource : public AP_HAL::AnalogSource
|
||||
{
|
||||
public:
|
||||
float read_average() override;
|
||||
float read_latest() override;
|
||||
bool set_pin(uint8_t p) override;
|
||||
float voltage_average() override;
|
||||
float voltage_latest() override;
|
||||
float voltage_average_ratiometric() override
|
||||
{
|
||||
return voltage_average();
|
||||
}
|
||||
uint8_t pin;
|
||||
};
|
||||
|
||||
class QURT::AnalogIn : public AP_HAL::AnalogIn
|
||||
{
|
||||
public:
|
||||
AnalogIn();
|
||||
void init() override;
|
||||
AP_HAL::AnalogSource* channel(int16_t n) override;
|
||||
float board_voltage(void) override;
|
||||
private:
|
||||
uint8_t next_chan;
|
||||
};
|
133
libraries/AP_HAL_QURT/DeviceBus.cpp
Normal file
133
libraries/AP_HAL_QURT/DeviceBus.cpp
Normal file
@ -0,0 +1,133 @@
|
||||
/*
|
||||
* This file is free software: you can redistribute it and/or modify it
|
||||
* under the terms of the GNU General Public License as published by the
|
||||
* Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This file is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
||||
* See the GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include "DeviceBus.h"
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <AP_HAL/utility/OwnPtr.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#include "Scheduler.h"
|
||||
#include "Semaphores.h"
|
||||
|
||||
using namespace QURT;
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
DeviceBus::DeviceBus(AP_HAL::Scheduler::priority_base _thread_priority) :
|
||||
thread_priority(_thread_priority), semaphore()
|
||||
{
|
||||
}
|
||||
|
||||
/*
|
||||
per-bus callback thread
|
||||
*/
|
||||
void DeviceBus::bus_thread(void)
|
||||
{
|
||||
while (true) {
|
||||
uint64_t now = AP_HAL::micros64();
|
||||
DeviceBus::callback_info *callback;
|
||||
|
||||
// find a callback to run
|
||||
for (callback = callbacks; callback; callback = callback->next) {
|
||||
if (now >= callback->next_usec) {
|
||||
while (now >= callback->next_usec) {
|
||||
callback->next_usec += callback->period_usec;
|
||||
}
|
||||
// call it with semaphore held
|
||||
WITH_SEMAPHORE(semaphore);
|
||||
callback->cb();
|
||||
}
|
||||
}
|
||||
|
||||
// work out when next loop is needed
|
||||
uint64_t next_needed = 0;
|
||||
now = AP_HAL::micros64();
|
||||
|
||||
for (callback = callbacks; callback; callback = callback->next) {
|
||||
if (next_needed == 0 ||
|
||||
callback->next_usec < next_needed) {
|
||||
next_needed = callback->next_usec;
|
||||
if (next_needed < now) {
|
||||
next_needed = now;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// delay for at most 50ms, to handle newly added callbacks
|
||||
uint32_t delay = 50000;
|
||||
if (next_needed >= now && next_needed - now < delay) {
|
||||
delay = next_needed - now;
|
||||
}
|
||||
// don't delay for less than 100usec, so one thread doesn't
|
||||
// completely dominate the CPU
|
||||
if (delay < 100) {
|
||||
delay = 100;
|
||||
}
|
||||
hal.scheduler->delay_microseconds(delay);
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
AP_HAL::Device::PeriodicHandle DeviceBus::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb, AP_HAL::Device *_hal_device)
|
||||
{
|
||||
if (!thread_started) {
|
||||
thread_started = true;
|
||||
hal_device = _hal_device;
|
||||
// setup a name for the thread
|
||||
char name[30];
|
||||
switch (hal_device->bus_type()) {
|
||||
case AP_HAL::Device::BUS_TYPE_I2C:
|
||||
snprintf(name, sizeof(name), "APM_I2C:%u",
|
||||
hal_device->bus_num());
|
||||
break;
|
||||
|
||||
case AP_HAL::Device::BUS_TYPE_SPI:
|
||||
snprintf(name, sizeof(name), "APM_SPI:%u",
|
||||
hal_device->bus_num());
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
#ifdef BUSDEBUG
|
||||
printf("%s:%d Thread Start\n", __PRETTY_FUNCTION__, __LINE__);
|
||||
#endif
|
||||
hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&DeviceBus::bus_thread, void),
|
||||
name, HAL_QURT_DEVICE_STACK_SIZE, thread_priority, 0);
|
||||
}
|
||||
DeviceBus::callback_info *callback = new DeviceBus::callback_info;
|
||||
if (callback == nullptr) {
|
||||
return nullptr;
|
||||
}
|
||||
callback->cb = cb;
|
||||
callback->period_usec = period_usec;
|
||||
callback->next_usec = AP_HAL::micros64() + period_usec;
|
||||
|
||||
// add to linked list of callbacks on thread
|
||||
callback->next = callbacks;
|
||||
callbacks = callback;
|
||||
|
||||
return callback;
|
||||
}
|
||||
|
||||
/*
|
||||
* Adjust the timer for the next call: it needs to be called from the bus
|
||||
* thread, otherwise it will race with it
|
||||
*/
|
||||
bool DeviceBus::adjust_timer(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec)
|
||||
{
|
||||
// TODO: add timer adjustment
|
||||
return true;
|
||||
}
|
55
libraries/AP_HAL_QURT/DeviceBus.h
Normal file
55
libraries/AP_HAL_QURT/DeviceBus.h
Normal file
@ -0,0 +1,55 @@
|
||||
/*
|
||||
* This file is free software: you can redistribute it and/or modify it
|
||||
* under the terms of the GNU General Public License as published by the
|
||||
* Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This file is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
||||
* See the GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <inttypes.h>
|
||||
#include <AP_HAL/HAL.h>
|
||||
#include "Semaphores.h"
|
||||
#include "AP_HAL_QURT.h"
|
||||
#include "Scheduler.h"
|
||||
|
||||
#define HAL_QURT_DEVICE_STACK_SIZE 8192
|
||||
|
||||
namespace QURT
|
||||
{
|
||||
|
||||
class DeviceBus
|
||||
{
|
||||
public:
|
||||
DeviceBus(AP_HAL::Scheduler::priority_base _thread_priority);
|
||||
|
||||
struct DeviceBus *next;
|
||||
HAL_Semaphore semaphore;
|
||||
|
||||
AP_HAL::Device::PeriodicHandle register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb, AP_HAL::Device *hal_device);
|
||||
bool adjust_timer(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec);
|
||||
void bus_thread(void);
|
||||
|
||||
private:
|
||||
struct callback_info {
|
||||
struct callback_info *next;
|
||||
AP_HAL::Device::PeriodicCb cb;
|
||||
uint32_t period_usec;
|
||||
uint64_t next_usec;
|
||||
} *callbacks;
|
||||
AP_HAL::Scheduler::priority_base thread_priority;
|
||||
bool thread_started;
|
||||
AP_HAL::Device *hal_device;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
|
152
libraries/AP_HAL_QURT/HAL_QURT_Class.cpp
Normal file
152
libraries/AP_HAL_QURT/HAL_QURT_Class.cpp
Normal file
@ -0,0 +1,152 @@
|
||||
/*
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
#include "HAL_QURT_Class.h"
|
||||
#include "AP_HAL_QURT_Private.h"
|
||||
#include "Scheduler.h"
|
||||
#include "Storage.h"
|
||||
#include "Semaphores.h"
|
||||
#include "RCInput.h"
|
||||
#include "RCOutput.h"
|
||||
#include "AnalogIn.h"
|
||||
#include "I2CDevice.h"
|
||||
#include "SPIDevice.h"
|
||||
#include <AP_HAL_Empty/AP_HAL_Empty.h>
|
||||
#include <AP_HAL_Empty/AP_HAL_Empty_Private.h>
|
||||
#include <AP_HAL/utility/getopt_cpp.h>
|
||||
#include <assert.h>
|
||||
#include "interface.h"
|
||||
#include "ap_host/src/protocol.h"
|
||||
|
||||
|
||||
extern "C" {
|
||||
typedef void (*external_error_handler_t)(void);
|
||||
};
|
||||
|
||||
static void crash_error_handler(void)
|
||||
{
|
||||
HAP_PRINTF("CRASH_ERROR_HANDLER: at %p", &crash_error_handler);
|
||||
}
|
||||
|
||||
using namespace QURT;
|
||||
|
||||
static UARTDriver_Console consoleDriver;
|
||||
static UARTDriver_MAVLinkUDP serial0Driver;
|
||||
static UARTDriver_Local serial3Driver(QURT_UART_GPS);
|
||||
static UARTDriver_Local serial4Driver(QURT_UART_RCIN);
|
||||
|
||||
static SPIDeviceManager spiDeviceManager;
|
||||
static AnalogIn analogIn;
|
||||
static Storage storageDriver;
|
||||
static Empty::GPIO gpioDriver;
|
||||
static RCInput rcinDriver;
|
||||
static RCOutput rcoutDriver;
|
||||
static Util utilInstance;
|
||||
static Scheduler schedulerInstance;
|
||||
static I2CDeviceManager i2c_mgr_instance;
|
||||
|
||||
bool qurt_ran_overtime;
|
||||
|
||||
HAL_QURT::HAL_QURT() :
|
||||
AP_HAL::HAL(
|
||||
&serial0Driver,
|
||||
nullptr,
|
||||
nullptr,
|
||||
&serial3Driver,
|
||||
&serial4Driver,
|
||||
nullptr,
|
||||
nullptr,
|
||||
nullptr,
|
||||
nullptr,
|
||||
nullptr,
|
||||
&i2c_mgr_instance,
|
||||
&spiDeviceManager,
|
||||
nullptr,
|
||||
&analogIn,
|
||||
&storageDriver,
|
||||
&consoleDriver,
|
||||
&gpioDriver,
|
||||
&rcinDriver,
|
||||
&rcoutDriver,
|
||||
&schedulerInstance,
|
||||
&utilInstance,
|
||||
nullptr,
|
||||
nullptr,
|
||||
nullptr)
|
||||
{
|
||||
}
|
||||
|
||||
static HAL_QURT::Callbacks *_callbacks;
|
||||
|
||||
void HAL_QURT::main_thread(void)
|
||||
{
|
||||
sl_client_register_fatal_error_cb(crash_error_handler);
|
||||
|
||||
// Let SLPI image send out it's initialization response before we
|
||||
// try to send anything out.
|
||||
qurt_timer_sleep(1000000);
|
||||
|
||||
rcinDriver.init();
|
||||
analogIn.init();
|
||||
rcoutDriver.init();
|
||||
_callbacks->setup();
|
||||
scheduler->set_system_initialized();
|
||||
|
||||
HAP_PRINTF("starting loop");
|
||||
|
||||
for (;;) {
|
||||
// ensure other threads get some time
|
||||
qurt_timer_sleep(200);
|
||||
|
||||
// call main loop
|
||||
_callbacks->loop();
|
||||
}
|
||||
}
|
||||
|
||||
void HAL_QURT::start_main_thread(Callbacks* callbacks)
|
||||
{
|
||||
_callbacks = callbacks;
|
||||
scheduler->thread_create(FUNCTOR_BIND_MEMBER(&HAL_QURT::main_thread, void), "main_thread",
|
||||
(20 * 1024),
|
||||
AP_HAL::Scheduler::PRIORITY_MAIN,
|
||||
0);
|
||||
}
|
||||
|
||||
void HAL_QURT::run(int argc, char* const argv[], Callbacks* callbacks) const
|
||||
{
|
||||
assert(callbacks);
|
||||
|
||||
/* initialize all drivers and private members here.
|
||||
* up to the programmer to do this in the correct order.
|
||||
* Scheduler should likely come first. */
|
||||
scheduler->init();
|
||||
schedulerInstance.hal_initialized();
|
||||
serial0Driver.begin(115200);
|
||||
|
||||
HAP_PRINTF("Creating main thread");
|
||||
|
||||
const_cast<HAL_QURT *>(this)->start_main_thread(callbacks);
|
||||
}
|
||||
|
||||
const AP_HAL::HAL& AP_HAL::get_HAL()
|
||||
{
|
||||
static const HAL_QURT *hal;
|
||||
if (hal == nullptr) {
|
||||
hal = new HAL_QURT;
|
||||
}
|
||||
return *hal;
|
||||
}
|
29
libraries/AP_HAL_QURT/HAL_QURT_Class.h
Normal file
29
libraries/AP_HAL_QURT/HAL_QURT_Class.h
Normal file
@ -0,0 +1,29 @@
|
||||
/*
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
#include "AP_HAL_QURT_Namespace.h"
|
||||
#include "interface.h"
|
||||
|
||||
class HAL_QURT : public AP_HAL::HAL
|
||||
{
|
||||
public:
|
||||
HAL_QURT();
|
||||
void run(int argc, char* const* argv, Callbacks* callbacks) const override;
|
||||
void start_main_thread(Callbacks* callbacks);
|
||||
void main_thread(void);
|
||||
};
|
131
libraries/AP_HAL_QURT/I2CDevice.cpp
Normal file
131
libraries/AP_HAL_QURT/I2CDevice.cpp
Normal file
@ -0,0 +1,131 @@
|
||||
/*
|
||||
* This file is free software: you can redistribute it and/or modify it
|
||||
* under the terms of the GNU General Public License as published by the
|
||||
* Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This file is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
||||
* See the GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#include "I2CDevice.h"
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <AP_Math/AP_Math.h>
|
||||
#include <AP_HAL_QURT/Semaphores.h>
|
||||
#include "Scheduler.h"
|
||||
#include "interface.h"
|
||||
|
||||
using namespace QURT;
|
||||
|
||||
/*
|
||||
4 I2C buses
|
||||
|
||||
bus1: mag
|
||||
bus2: power manager
|
||||
bus4: external spare bus
|
||||
bus5: barometer (internal)
|
||||
*/
|
||||
static uint8_t i2c_bus_ids[] = { 1, 2, 4, 5 };
|
||||
|
||||
static uint32_t i2c_internal_mask = (1U<<3);
|
||||
|
||||
I2CBus I2CDeviceManager::businfo[ARRAY_SIZE(i2c_bus_ids)];
|
||||
|
||||
I2CDeviceManager::I2CDeviceManager(void)
|
||||
{
|
||||
}
|
||||
|
||||
I2CDevice::I2CDevice(uint8_t busnum, uint8_t address, uint32_t bus_clock, bool use_smbus, uint32_t timeout_ms) :
|
||||
_address(address),
|
||||
bus(I2CDeviceManager::businfo[busnum])
|
||||
{
|
||||
HAP_PRINTF("Constructing I2CDevice %u 0x%02x %u", unsigned(busnum), unsigned(address), unsigned(bus_clock));
|
||||
|
||||
if (bus.fd == -2) {
|
||||
bus.fd = sl_client_config_i2c_bus(i2c_bus_ids[busnum], address, bus_clock/1000);
|
||||
}
|
||||
set_device_bus(busnum);
|
||||
set_device_address(address);
|
||||
asprintf(&pname, "I2C:%u:%02x",
|
||||
(unsigned)busnum, (unsigned)address);
|
||||
}
|
||||
|
||||
I2CDevice::~I2CDevice()
|
||||
{
|
||||
free(pname);
|
||||
}
|
||||
|
||||
bool I2CDevice::transfer(const uint8_t *send, uint32_t send_len,
|
||||
uint8_t *recv, uint32_t recv_len)
|
||||
{
|
||||
if (bus.fd < 0) {
|
||||
return false;
|
||||
}
|
||||
if (!bus.semaphore.check_owner()) {
|
||||
return false;
|
||||
}
|
||||
if (bus.last_address != _address) {
|
||||
sl_client_set_address_i2c_bus(bus.fd, _address);
|
||||
bus.last_address = _address;
|
||||
}
|
||||
return sl_client_i2c_transfer(bus.fd, send, send_len, recv, recv_len) == 0;
|
||||
}
|
||||
|
||||
/*
|
||||
register a periodic callback
|
||||
*/
|
||||
AP_HAL::Device::PeriodicHandle I2CDevice::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb)
|
||||
{
|
||||
return bus.register_periodic_callback(period_usec, cb, this);
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
adjust a periodic callback
|
||||
*/
|
||||
bool I2CDevice::adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec)
|
||||
{
|
||||
return bus.adjust_timer(h, period_usec);
|
||||
}
|
||||
|
||||
AP_HAL::OwnPtr<AP_HAL::I2CDevice>
|
||||
I2CDeviceManager::get_device(uint8_t bus, uint8_t address,
|
||||
uint32_t bus_clock,
|
||||
bool use_smbus,
|
||||
uint32_t timeout_ms)
|
||||
{
|
||||
if (bus >= ARRAY_SIZE(i2c_bus_ids)) {
|
||||
return AP_HAL::OwnPtr<AP_HAL::I2CDevice>(nullptr);
|
||||
}
|
||||
auto dev = AP_HAL::OwnPtr<AP_HAL::I2CDevice>(new I2CDevice(bus, address, bus_clock, use_smbus, timeout_ms));
|
||||
return dev;
|
||||
}
|
||||
|
||||
/*
|
||||
get mask of bus numbers for all configured I2C buses
|
||||
*/
|
||||
uint32_t I2CDeviceManager::get_bus_mask(void) const
|
||||
{
|
||||
return ((1U << ARRAY_SIZE(i2c_bus_ids)) - 1);
|
||||
}
|
||||
|
||||
/*
|
||||
get mask of bus numbers for all configured internal I2C buses
|
||||
*/
|
||||
uint32_t I2CDeviceManager::get_bus_mask_internal(void) const
|
||||
{
|
||||
return i2c_internal_mask;
|
||||
}
|
||||
|
||||
/*
|
||||
get mask of bus numbers for all configured external I2C buses
|
||||
*/
|
||||
uint32_t I2CDeviceManager::get_bus_mask_external(void) const
|
||||
{
|
||||
return get_bus_mask() & ~get_bus_mask_internal();
|
||||
}
|
134
libraries/AP_HAL_QURT/I2CDevice.h
Normal file
134
libraries/AP_HAL_QURT/I2CDevice.h
Normal file
@ -0,0 +1,134 @@
|
||||
/*
|
||||
* This file is free software: you can redistribute it and/or modify it
|
||||
* under the terms of the GNU General Public License as published by the
|
||||
* Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This file is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
||||
* See the GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include <inttypes.h>
|
||||
|
||||
#include <AP_HAL/HAL.h>
|
||||
#include <AP_HAL/I2CDevice.h>
|
||||
#include <AP_HAL/utility/OwnPtr.h>
|
||||
|
||||
#include "Semaphores.h"
|
||||
#include "Scheduler.h"
|
||||
#include "DeviceBus.h"
|
||||
|
||||
namespace QURT
|
||||
{
|
||||
|
||||
class I2CBus : public DeviceBus
|
||||
{
|
||||
public:
|
||||
I2CBus():DeviceBus(AP_HAL::Scheduler::PRIORITY_I2C) {};
|
||||
uint32_t bus_clock;
|
||||
int fd = -2;
|
||||
uint8_t last_address;
|
||||
};
|
||||
|
||||
class I2CDevice : public AP_HAL::I2CDevice
|
||||
{
|
||||
public:
|
||||
static I2CDevice *from(AP_HAL::I2CDevice *dev)
|
||||
{
|
||||
return static_cast<I2CDevice*>(dev);
|
||||
}
|
||||
|
||||
I2CDevice(uint8_t bus, uint8_t address, uint32_t bus_clock, bool use_smbus, uint32_t timeout_ms);
|
||||
~I2CDevice();
|
||||
|
||||
/* See AP_HAL::I2CDevice::set_address() */
|
||||
void set_address(uint8_t address) override
|
||||
{
|
||||
_address = address;
|
||||
}
|
||||
|
||||
/* See AP_HAL::I2CDevice::set_retries() */
|
||||
void set_retries(uint8_t retries) override
|
||||
{
|
||||
_retries = retries;
|
||||
}
|
||||
|
||||
/* See AP_HAL::Device::set_speed(): Empty implementation, not supported. */
|
||||
bool set_speed(enum Device::Speed speed) override
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
/* See AP_HAL::Device::transfer() */
|
||||
bool transfer(const uint8_t *send, uint32_t send_len,
|
||||
uint8_t *recv, uint32_t recv_len) override;
|
||||
|
||||
bool read_registers_multiple(uint8_t first_reg, uint8_t *recv,
|
||||
uint32_t recv_len, uint8_t times) override
|
||||
{
|
||||
return false;
|
||||
};
|
||||
|
||||
/* See AP_HAL::Device::register_periodic_callback() */
|
||||
AP_HAL::Device::PeriodicHandle register_periodic_callback(
|
||||
uint32_t period_usec, AP_HAL::Device::PeriodicCb) override;
|
||||
|
||||
/* See AP_HAL::Device::adjust_periodic_callback() */
|
||||
bool adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec) override;
|
||||
|
||||
AP_HAL::Semaphore* get_semaphore() override //TODO check all
|
||||
{
|
||||
// if asking for invalid bus number use bus 0 semaphore
|
||||
return &bus.semaphore;
|
||||
}
|
||||
|
||||
protected:
|
||||
I2CBus &bus;
|
||||
uint8_t _retries;
|
||||
uint8_t _address;
|
||||
char *pname;
|
||||
|
||||
};
|
||||
|
||||
class I2CDeviceManager : public AP_HAL::I2CDeviceManager
|
||||
{
|
||||
public:
|
||||
friend class I2CDevice;
|
||||
|
||||
static I2CBus businfo[];
|
||||
|
||||
// constructor
|
||||
I2CDeviceManager();
|
||||
|
||||
static I2CDeviceManager *from(AP_HAL::I2CDeviceManager *i2c_mgr)
|
||||
{
|
||||
return static_cast<I2CDeviceManager*>(i2c_mgr);
|
||||
}
|
||||
|
||||
AP_HAL::OwnPtr<AP_HAL::I2CDevice> get_device(uint8_t bus, uint8_t address,
|
||||
uint32_t bus_clock=100000,
|
||||
bool use_smbus = false,
|
||||
uint32_t timeout_ms=4) override;
|
||||
|
||||
/*
|
||||
get mask of bus numbers for all configured I2C buses
|
||||
*/
|
||||
uint32_t get_bus_mask(void) const override;
|
||||
|
||||
/*
|
||||
get mask of bus numbers for all configured external I2C buses
|
||||
*/
|
||||
uint32_t get_bus_mask_external(void) const override;
|
||||
|
||||
/*
|
||||
get mask of bus numbers for all configured internal I2C buses
|
||||
*/
|
||||
uint32_t get_bus_mask_internal(void) const override;
|
||||
};
|
||||
}
|
58
libraries/AP_HAL_QURT/RCInput.cpp
Normal file
58
libraries/AP_HAL_QURT/RCInput.cpp
Normal file
@ -0,0 +1,58 @@
|
||||
#include "RCInput.h"
|
||||
#include <AP_Math/AP_Math.h>
|
||||
|
||||
using namespace QURT;
|
||||
|
||||
void RCInput::init()
|
||||
{
|
||||
AP::RC().init();
|
||||
}
|
||||
|
||||
const char *RCInput::protocol() const
|
||||
{
|
||||
return AP::RC().protocol_name();
|
||||
}
|
||||
|
||||
bool RCInput::new_input()
|
||||
{
|
||||
bool ret = updated;
|
||||
updated = false;
|
||||
return ret;
|
||||
}
|
||||
|
||||
uint8_t RCInput::num_channels()
|
||||
{
|
||||
return num_chan;
|
||||
}
|
||||
|
||||
uint16_t RCInput::read(uint8_t channel)
|
||||
{
|
||||
if (channel >= MIN(RC_INPUT_MAX_CHANNELS, num_chan)) {
|
||||
return 0;
|
||||
}
|
||||
return values[channel];
|
||||
}
|
||||
|
||||
uint8_t RCInput::read(uint16_t* periods, uint8_t len)
|
||||
{
|
||||
WITH_SEMAPHORE(mutex);
|
||||
len = MIN(len, num_chan);
|
||||
memcpy(periods, values, len*sizeof(periods[0]));
|
||||
return len;
|
||||
}
|
||||
|
||||
void RCInput::_timer_tick(void)
|
||||
{
|
||||
auto &rcprot = AP::RC();
|
||||
|
||||
WITH_SEMAPHORE(mutex);
|
||||
|
||||
rcprot.update();
|
||||
|
||||
if (rcprot.new_input()) {
|
||||
num_chan = rcprot.num_channels();
|
||||
num_chan = MIN(num_chan, RC_INPUT_MAX_CHANNELS);
|
||||
rcprot.read(values, num_chan);
|
||||
updated = true;
|
||||
}
|
||||
}
|
28
libraries/AP_HAL_QURT/RCInput.h
Normal file
28
libraries/AP_HAL_QURT/RCInput.h
Normal file
@ -0,0 +1,28 @@
|
||||
#pragma once
|
||||
|
||||
#include "AP_HAL_QURT.h"
|
||||
#include <AP_RCProtocol/AP_RCProtocol.h>
|
||||
|
||||
#ifndef RC_INPUT_MAX_CHANNELS
|
||||
#define RC_INPUT_MAX_CHANNELS 18
|
||||
#endif
|
||||
|
||||
class QURT::RCInput : public AP_HAL::RCInput
|
||||
{
|
||||
public:
|
||||
void init() override;
|
||||
bool new_input() override;
|
||||
uint8_t num_channels() override;
|
||||
uint16_t read(uint8_t ch) override;
|
||||
uint8_t read(uint16_t* periods, uint8_t len) override;
|
||||
|
||||
const char *protocol() const override;
|
||||
|
||||
void _timer_tick(void);
|
||||
|
||||
private:
|
||||
HAL_Semaphore mutex;
|
||||
uint16_t values[RC_INPUT_MAX_CHANNELS];
|
||||
uint8_t num_chan;
|
||||
bool updated;
|
||||
};
|
225
libraries/AP_HAL_QURT/RCOutput.cpp
Normal file
225
libraries/AP_HAL_QURT/RCOutput.cpp
Normal file
@ -0,0 +1,225 @@
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
#include "RCOutput.h"
|
||||
#include <AP_Math/AP_Math.h>
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
using namespace QURT;
|
||||
|
||||
#define ESC_PACKET_TYPE_PWM_CMD 1
|
||||
#define ESC_PACKET_TYPE_FB_RESPONSE 128
|
||||
#define ESC_PACKET_TYPE_FB_POWER_STATUS 132
|
||||
|
||||
#define ESC_PKT_HEADER 0xAF
|
||||
|
||||
void RCOutput::init()
|
||||
{
|
||||
fd = sl_client_config_uart(QURT_UART_ESC, baudrate);
|
||||
if (fd == -1) {
|
||||
HAP_PRINTF("Failed to open ESC UART");
|
||||
}
|
||||
HAP_PRINTF("ESC UART: %d", fd);
|
||||
}
|
||||
|
||||
void RCOutput::set_freq(uint32_t chmask, uint16_t freq_hz)
|
||||
{
|
||||
// no support for changing frequency
|
||||
}
|
||||
|
||||
uint16_t RCOutput::get_freq(uint8_t ch)
|
||||
{
|
||||
// return fixed fake value
|
||||
return 490;
|
||||
}
|
||||
|
||||
void RCOutput::enable_ch(uint8_t ch)
|
||||
{
|
||||
if (ch >= ARRAY_SIZE(period)) {
|
||||
return;
|
||||
}
|
||||
enable_mask |= 1U<<ch;
|
||||
}
|
||||
|
||||
void RCOutput::disable_ch(uint8_t ch)
|
||||
{
|
||||
if (ch >= ARRAY_SIZE(period)) {
|
||||
return;
|
||||
}
|
||||
enable_mask &= ~1U<<ch;
|
||||
}
|
||||
|
||||
void RCOutput::write(uint8_t ch, uint16_t period_us)
|
||||
{
|
||||
if (ch >= ARRAY_SIZE(period)) {
|
||||
return;
|
||||
}
|
||||
period[ch] = period_us;
|
||||
if (!corked) {
|
||||
need_write = true;
|
||||
}
|
||||
}
|
||||
|
||||
uint16_t RCOutput::read(uint8_t ch)
|
||||
{
|
||||
if (ch >= ARRAY_SIZE(period)) {
|
||||
return 0;
|
||||
}
|
||||
return period[ch];
|
||||
}
|
||||
|
||||
void RCOutput::read(uint16_t *period_us, uint8_t len)
|
||||
{
|
||||
for (auto i = 0; i < len; i++) {
|
||||
period_us[i] = read(i);
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
send a packet with CRC to the ESC
|
||||
*/
|
||||
void RCOutput::send_esc_packet(uint8_t type, uint8_t *data, uint16_t size)
|
||||
{
|
||||
uint16_t packet_size = size + 5;
|
||||
uint8_t out[packet_size];
|
||||
|
||||
out[0] = ESC_PKT_HEADER;
|
||||
out[1] = packet_size;
|
||||
out[2] = type;
|
||||
|
||||
memcpy(&out[3], data, size);
|
||||
|
||||
uint16_t crc = calc_crc_modbus(&out[1], packet_size - 3);
|
||||
|
||||
memcpy(&out[packet_size - 2], &crc, sizeof(uint16_t));
|
||||
|
||||
sl_client_uart_write(fd, (const char *)out, packet_size);
|
||||
}
|
||||
|
||||
/*
|
||||
convert 1000 to 2000 PWM to -800 to 800 for QURT ESCs
|
||||
*/
|
||||
static int16_t pwm_to_esc(uint16_t pwm)
|
||||
{
|
||||
const float p = constrain_float((pwm-1000)*0.001, 0, 1);
|
||||
return int16_t(800*p);
|
||||
}
|
||||
|
||||
/*
|
||||
send current commands to ESCs
|
||||
*/
|
||||
void RCOutput::send_receive(void)
|
||||
{
|
||||
if (fd == -1) {
|
||||
return;
|
||||
}
|
||||
|
||||
int16_t data[5] {};
|
||||
|
||||
for (uint8_t i=0; i<4; i++) {
|
||||
data[esc_map[i]] = pwm_to_esc(period[i]);
|
||||
}
|
||||
|
||||
need_write = false;
|
||||
|
||||
const uint32_t now_ms = AP_HAL::millis();
|
||||
if (now_ms - last_fb_req_ms > 5) {
|
||||
last_fb_req_ms = now_ms;
|
||||
// request feedback from one ESC
|
||||
last_fb_idx = (last_fb_idx+1) % 4;
|
||||
data[last_fb_idx] |= 1;
|
||||
}
|
||||
|
||||
send_esc_packet(ESC_PACKET_TYPE_PWM_CMD, (uint8_t *)data, sizeof(data));
|
||||
|
||||
check_response();
|
||||
}
|
||||
|
||||
/*
|
||||
handle a telem feedback packet
|
||||
*/
|
||||
void RCOutput::handle_esc_feedback(const struct esc_response_v2 &pkt)
|
||||
{
|
||||
const uint8_t idx = esc_map_rev[pkt.id_state>>4];
|
||||
if (idx >= ARRAY_SIZE(period)) {
|
||||
return;
|
||||
}
|
||||
update_rpm(idx, pkt.rpm);
|
||||
|
||||
AP_ESC_Telem_Backend::TelemetryData tdata {};
|
||||
tdata.voltage = pkt.voltage*0.001;
|
||||
tdata.current = pkt.current*0.008;
|
||||
tdata.temperature_cdeg = pkt.temperature;
|
||||
|
||||
update_telem_data(idx, tdata,
|
||||
AP_ESC_Telem_Backend::TelemetryType::CURRENT |
|
||||
AP_ESC_Telem_Backend::TelemetryType::VOLTAGE |
|
||||
AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE);
|
||||
}
|
||||
|
||||
/*
|
||||
handle a power status packet, making it available to AnalogIn
|
||||
*/
|
||||
void RCOutput::handle_power_status(const struct esc_power_status &pkt)
|
||||
{
|
||||
esc_voltage = pkt.voltage * 0.001;
|
||||
const int16_t current_offset = 2048; // why this offset?
|
||||
esc_current = (pkt.current+current_offset) * 0.008;
|
||||
}
|
||||
|
||||
// check for responses
|
||||
void RCOutput::check_response(void)
|
||||
{
|
||||
uint8_t buf[256];
|
||||
struct PACKED esc_packet {
|
||||
uint8_t header;
|
||||
uint8_t length;
|
||||
uint8_t type;
|
||||
union {
|
||||
struct esc_response_v2 resp_v2;
|
||||
struct esc_power_status power_status;
|
||||
} u;
|
||||
};
|
||||
auto n = sl_client_uart_read(fd, (char *)buf, sizeof(buf));
|
||||
while (n >= 3) {
|
||||
const auto *pkt = (struct esc_packet *)buf;
|
||||
if (pkt->header != ESC_PKT_HEADER || pkt->length > n) {
|
||||
return;
|
||||
}
|
||||
const uint16_t crc = calc_crc_modbus(&pkt->length, pkt->length-3);
|
||||
const uint16_t crc2 = buf[pkt->length-2] | buf[pkt->length-1]<<8;
|
||||
if (crc != crc2) {
|
||||
return;
|
||||
}
|
||||
switch (pkt->type) {
|
||||
case ESC_PACKET_TYPE_FB_RESPONSE:
|
||||
handle_esc_feedback(pkt->u.resp_v2);
|
||||
break;
|
||||
case ESC_PACKET_TYPE_FB_POWER_STATUS:
|
||||
handle_power_status(pkt->u.power_status);
|
||||
break;
|
||||
default:
|
||||
HAP_PRINTF("Unknown pkt %u", pkt->type);
|
||||
break;
|
||||
}
|
||||
if (n == pkt->length) {
|
||||
break;
|
||||
}
|
||||
memmove(&buf[0], &buf[pkt->length], n - pkt->length);
|
||||
n -= pkt->length;
|
||||
}
|
||||
}
|
||||
|
||||
void RCOutput::cork(void)
|
||||
{
|
||||
corked = true;
|
||||
}
|
||||
|
||||
void RCOutput::push(void)
|
||||
{
|
||||
if (corked) {
|
||||
corked = false;
|
||||
need_write = true;
|
||||
send_receive();
|
||||
}
|
||||
}
|
75
libraries/AP_HAL_QURT/RCOutput.h
Normal file
75
libraries/AP_HAL_QURT/RCOutput.h
Normal file
@ -0,0 +1,75 @@
|
||||
#pragma once
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include "AP_HAL_QURT.h"
|
||||
#include <AP_ESC_Telem/AP_ESC_Telem.h>
|
||||
|
||||
class QURT::RCOutput : public AP_HAL::RCOutput, AP_ESC_Telem_Backend
|
||||
{
|
||||
public:
|
||||
void init() override;
|
||||
void set_freq(uint32_t chmask, uint16_t freq_hz) override;
|
||||
uint16_t get_freq(uint8_t ch) override;
|
||||
void enable_ch(uint8_t ch) override;
|
||||
void disable_ch(uint8_t ch) override;
|
||||
void write(uint8_t ch, uint16_t period_us) override;
|
||||
uint16_t read(uint8_t ch) override;
|
||||
void read(uint16_t *period_us, uint8_t len) override;
|
||||
void cork(void) override;
|
||||
void push(void) override;
|
||||
|
||||
float get_voltage(void) const
|
||||
{
|
||||
return esc_voltage;
|
||||
}
|
||||
float get_current(void) const
|
||||
{
|
||||
return esc_current;
|
||||
}
|
||||
|
||||
|
||||
private:
|
||||
const uint32_t baudrate = 2000000;
|
||||
|
||||
void send_receive(void);
|
||||
void check_response(void);
|
||||
void send_esc_packet(uint8_t type, uint8_t *data, uint16_t size);
|
||||
|
||||
struct PACKED esc_response_v2 {
|
||||
uint8_t id_state; // bits 0:3 = state, bits 4:7 = ID
|
||||
|
||||
uint16_t rpm; // Current RPM of the motor
|
||||
uint8_t cmd_counter; // Number of commands received by the ESC
|
||||
uint8_t power; // Applied power [0..100]
|
||||
|
||||
uint16_t voltage; // Voltage measured by the ESC in mV
|
||||
int16_t current; // Current measured by the ESC in 8mA resolution
|
||||
int16_t temperature; // Temperature measured by the ESC in 0.01 degC resolution
|
||||
};
|
||||
|
||||
struct PACKED esc_power_status {
|
||||
uint8_t id; //ESC Id (could be used as system ID elsewhere)
|
||||
uint16_t voltage; //Input voltage (Millivolts)
|
||||
int16_t current; //Total Current (8mA resolution)
|
||||
};
|
||||
|
||||
void handle_esc_feedback(const struct esc_response_v2 &pkt);
|
||||
void handle_power_status(const struct esc_power_status &pkt);
|
||||
|
||||
// order is RL, FL, FR, RR, map to X frame
|
||||
const uint8_t esc_map[4] {2, 0, 1, 3};
|
||||
const uint8_t esc_map_rev[4] {1, 2, 0, 3};
|
||||
|
||||
int fd = -1;
|
||||
uint16_t enable_mask;
|
||||
static const uint8_t channel_count = 4;
|
||||
uint16_t period[channel_count];
|
||||
volatile bool need_write;
|
||||
bool corked;
|
||||
HAL_Semaphore mutex;
|
||||
uint8_t last_fb_idx;
|
||||
uint32_t last_fb_req_ms;
|
||||
|
||||
float esc_voltage;
|
||||
float esc_current;
|
||||
};
|
128
libraries/AP_HAL_QURT/SPIDevice.cpp
Normal file
128
libraries/AP_HAL_QURT/SPIDevice.cpp
Normal file
@ -0,0 +1,128 @@
|
||||
/*
|
||||
* This file is free software: you can redistribute it and/or modify it
|
||||
* under the terms of the GNU General Public License as published by the
|
||||
* Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This file is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
||||
* See the GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include "SPIDevice.h"
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <AP_Math/AP_Math.h>
|
||||
#include <AP_HAL/utility/OwnPtr.h>
|
||||
#include "Scheduler.h"
|
||||
#include "Semaphores.h"
|
||||
#include "interface.h"
|
||||
#include <stdio.h>
|
||||
|
||||
using namespace QURT;
|
||||
|
||||
#define MHZ (1000U*1000U)
|
||||
#define KHZ (1000U)
|
||||
|
||||
const char *device_names[] = {"INV1", "INV2", "INV3"};
|
||||
|
||||
static SPIBus *spi_bus;
|
||||
|
||||
SPIBus::SPIBus(void):
|
||||
DeviceBus(Scheduler::PRIORITY_SPI)
|
||||
{
|
||||
fd = sl_client_config_spi_bus();
|
||||
HAP_PRINTF("Created SPI bus -> %d", fd);
|
||||
}
|
||||
|
||||
SPIDevice::SPIDevice(const char *name, SPIBus &_bus)
|
||||
: bus(_bus)
|
||||
{
|
||||
set_device_bus(0);
|
||||
set_device_address(1);
|
||||
set_speed(AP_HAL::Device::SPEED_LOW);
|
||||
|
||||
pname = name;
|
||||
}
|
||||
|
||||
SPIDevice::~SPIDevice()
|
||||
{
|
||||
}
|
||||
|
||||
bool SPIDevice::set_speed(AP_HAL::Device::Speed _speed)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
bool SPIDevice::transfer(const uint8_t *send, uint32_t send_len,
|
||||
uint8_t *recv, uint32_t recv_len)
|
||||
{
|
||||
// If there is no receive buffer then this is a write transaction
|
||||
// and the data to write is in the send buffer
|
||||
if (!recv) {
|
||||
return transfer_fullduplex(send, (uint8_t*) send, send_len);
|
||||
}
|
||||
|
||||
// This is a read transaction
|
||||
uint8_t buf[send_len+recv_len];
|
||||
if (send_len > 0) {
|
||||
memcpy(buf, send, send_len);
|
||||
}
|
||||
if (recv_len > 0) {
|
||||
memset(&buf[send_len], 0, recv_len);
|
||||
}
|
||||
transfer_fullduplex(buf, buf, send_len+recv_len);
|
||||
if (recv_len > 0) {
|
||||
memcpy(recv, &buf[send_len], recv_len);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool SPIDevice::transfer_fullduplex(const uint8_t *send, uint8_t *recv, uint32_t len)
|
||||
{
|
||||
return (sl_client_spi_transfer(bus.fd, send, recv, len) == 0);
|
||||
}
|
||||
|
||||
void SPIDevice::acquire_bus(bool accuire)
|
||||
{
|
||||
}
|
||||
|
||||
AP_HAL::Semaphore *SPIDevice::get_semaphore()
|
||||
{
|
||||
return &bus.semaphore;
|
||||
}
|
||||
|
||||
AP_HAL::Device::PeriodicHandle SPIDevice::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb)
|
||||
{
|
||||
return bus.register_periodic_callback(period_usec, cb, this);
|
||||
}
|
||||
|
||||
bool SPIDevice::adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec)
|
||||
{
|
||||
return bus.adjust_timer(h, period_usec);
|
||||
}
|
||||
|
||||
AP_HAL::OwnPtr<AP_HAL::SPIDevice>
|
||||
SPIDeviceManager::get_device(const char *name)
|
||||
{
|
||||
uint8_t i;
|
||||
for (i = 0; i<ARRAY_SIZE(device_names); i++) {
|
||||
if (strcmp(device_names[i], name) == 0) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (i == ARRAY_SIZE(device_names)) {
|
||||
return AP_HAL::OwnPtr<AP_HAL::SPIDevice>(nullptr);
|
||||
}
|
||||
|
||||
if (spi_bus == nullptr) {
|
||||
spi_bus = new SPIBus();
|
||||
}
|
||||
|
||||
return AP_HAL::OwnPtr<AP_HAL::SPIDevice>(new SPIDevice(name, *spi_bus));
|
||||
}
|
||||
|
65
libraries/AP_HAL_QURT/SPIDevice.h
Normal file
65
libraries/AP_HAL_QURT/SPIDevice.h
Normal file
@ -0,0 +1,65 @@
|
||||
/*
|
||||
* This file is free software: you can redistribute it and/or modify it
|
||||
* under the terms of the GNU General Public License as published by the
|
||||
* Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This file is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
||||
* See the GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <inttypes.h>
|
||||
#include <AP_HAL/HAL.h>
|
||||
#include <AP_HAL/SPIDevice.h>
|
||||
#include "AP_HAL_QURT.h"
|
||||
|
||||
#include "Semaphores.h"
|
||||
#include "Scheduler.h"
|
||||
#include "DeviceBus.h"
|
||||
|
||||
namespace QURT
|
||||
{
|
||||
|
||||
class SPIBus : public DeviceBus
|
||||
{
|
||||
public:
|
||||
SPIBus();
|
||||
int fd = -1;
|
||||
};
|
||||
|
||||
class SPIDevice : public AP_HAL::SPIDevice
|
||||
{
|
||||
public:
|
||||
SPIDevice(const char *name, SPIBus &bus);
|
||||
virtual ~SPIDevice();
|
||||
|
||||
bool set_speed(AP_HAL::Device::Speed speed) override;
|
||||
bool transfer(const uint8_t *send, uint32_t send_len,
|
||||
uint8_t *recv, uint32_t recv_len) override;
|
||||
bool transfer_fullduplex(const uint8_t *send, uint8_t *recv, uint32_t len) override;
|
||||
AP_HAL::Semaphore *get_semaphore() override;
|
||||
AP_HAL::Device::PeriodicHandle register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb) override;
|
||||
bool adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec) override;
|
||||
|
||||
private:
|
||||
SPIBus &bus;
|
||||
const char *pname;
|
||||
void acquire_bus(bool accuire);
|
||||
};
|
||||
|
||||
class SPIDeviceManager : public AP_HAL::SPIDeviceManager
|
||||
{
|
||||
public:
|
||||
friend class SPIDevice;
|
||||
|
||||
AP_HAL::OwnPtr<AP_HAL::SPIDevice> get_device(const char *name) override;
|
||||
};
|
||||
}
|
||||
|
344
libraries/AP_HAL_QURT/Scheduler.cpp
Normal file
344
libraries/AP_HAL_QURT/Scheduler.cpp
Normal file
@ -0,0 +1,344 @@
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
#include "AP_HAL_QURT.h"
|
||||
#include "Scheduler.h"
|
||||
|
||||
#include <sys/stat.h>
|
||||
#include <sys/types.h>
|
||||
#include <unistd.h>
|
||||
#include <stdlib.h>
|
||||
#include <sched.h>
|
||||
#include <errno.h>
|
||||
#include <stdio.h>
|
||||
#include <pthread.h>
|
||||
|
||||
#include "UARTDriver.h"
|
||||
#include "Storage.h"
|
||||
#include "RCOutput.h"
|
||||
#include "RCInput.h"
|
||||
#include <AP_Scheduler/AP_Scheduler.h>
|
||||
#include "Thread.h"
|
||||
|
||||
using namespace QURT;
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
Scheduler::Scheduler()
|
||||
{
|
||||
}
|
||||
|
||||
void Scheduler::init()
|
||||
{
|
||||
_main_thread_ctx = pthread_self();
|
||||
|
||||
// setup the timer thread - this will call tasks at 1kHz
|
||||
pthread_attr_t thread_attr;
|
||||
struct sched_param param;
|
||||
|
||||
pthread_attr_init(&thread_attr);
|
||||
pthread_attr_setstacksize(&thread_attr, 16000);
|
||||
|
||||
param.sched_priority = APM_TIMER_PRIORITY;
|
||||
(void)pthread_attr_setschedparam(&thread_attr, ¶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;
|
||||
}
|
84
libraries/AP_HAL_QURT/Scheduler.h
Normal file
84
libraries/AP_HAL_QURT/Scheduler.h
Normal file
@ -0,0 +1,84 @@
|
||||
#pragma once
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_QURT
|
||||
#include "AP_HAL_QURT_Namespace.h"
|
||||
#include <sys/time.h>
|
||||
#include <signal.h>
|
||||
#include <pthread.h>
|
||||
|
||||
#define QURT_SCHEDULER_MAX_TIMER_PROCS 8
|
||||
|
||||
#define APM_MAIN_PRIORITY 180
|
||||
#define APM_TIMER_PRIORITY 181
|
||||
#define APM_UART_PRIORITY 60
|
||||
#define APM_IO_PRIORITY 58
|
||||
|
||||
/* Scheduler implementation: */
|
||||
class QURT::Scheduler : public AP_HAL::Scheduler
|
||||
{
|
||||
public:
|
||||
Scheduler();
|
||||
/* AP_HAL::Scheduler methods */
|
||||
|
||||
void init() override;
|
||||
void delay(uint16_t ms) override;
|
||||
void delay_microseconds(uint16_t us) override;
|
||||
void register_timer_process(AP_HAL::MemberProc) override;
|
||||
void register_io_process(AP_HAL::MemberProc) override;
|
||||
void register_timer_failsafe(AP_HAL::Proc, uint32_t period_us) override;
|
||||
void suspend_timer_procs();
|
||||
void resume_timer_procs();
|
||||
void reboot(bool hold_in_bootloader) override;
|
||||
|
||||
bool in_main_thread() const override;
|
||||
void hal_initialized();
|
||||
|
||||
void set_system_initialized() override;
|
||||
bool is_system_initialized() override
|
||||
{
|
||||
return _initialized;
|
||||
}
|
||||
|
||||
bool thread_create(AP_HAL::MemberProc proc, const char *name,
|
||||
uint32_t stack_size, priority_base base, int8_t priority) override;
|
||||
|
||||
private:
|
||||
bool _initialized;
|
||||
volatile bool _hal_initialized;
|
||||
AP_HAL::Proc _delay_cb;
|
||||
uint16_t _min_delay_cb_ms;
|
||||
AP_HAL::Proc _failsafe;
|
||||
|
||||
volatile bool _timer_suspended;
|
||||
|
||||
AP_HAL::MemberProc _timer_proc[QURT_SCHEDULER_MAX_TIMER_PROCS];
|
||||
uint8_t _num_timer_procs;
|
||||
volatile bool _in_timer_proc;
|
||||
|
||||
AP_HAL::MemberProc _io_proc[QURT_SCHEDULER_MAX_TIMER_PROCS];
|
||||
uint8_t _num_io_procs;
|
||||
volatile bool _in_io_proc;
|
||||
|
||||
volatile bool _timer_event_missed;
|
||||
|
||||
pthread_t _main_thread_ctx;
|
||||
pthread_t _timer_thread_ctx;
|
||||
pthread_t _io_thread_ctx;
|
||||
pthread_t _storage_thread_ctx;
|
||||
pthread_t _uart_thread_ctx;
|
||||
|
||||
uint8_t calculate_thread_priority(priority_base base, int8_t priority) const;
|
||||
|
||||
static void *_timer_thread(void *arg);
|
||||
static void *_io_thread(void *arg);
|
||||
static void *_storage_thread(void *arg);
|
||||
static void *_uart_thread(void *arg);
|
||||
|
||||
void _run_timers(bool called_from_timer_thread);
|
||||
void _run_io(void);
|
||||
};
|
||||
#endif
|
||||
|
||||
|
||||
|
111
libraries/AP_HAL_QURT/Semaphores.cpp
Normal file
111
libraries/AP_HAL_QURT/Semaphores.cpp
Normal file
@ -0,0 +1,111 @@
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
#include "Semaphores.h"
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
using namespace QURT;
|
||||
|
||||
// construct a semaphore
|
||||
Semaphore::Semaphore()
|
||||
{
|
||||
pthread_mutexattr_t attr;
|
||||
pthread_mutexattr_init(&attr);
|
||||
pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_RECURSIVE);
|
||||
pthread_mutex_init(&_lock, &attr);
|
||||
}
|
||||
|
||||
bool Semaphore::give()
|
||||
{
|
||||
return pthread_mutex_unlock(&_lock) == 0;
|
||||
}
|
||||
|
||||
bool Semaphore::check_owner()
|
||||
{
|
||||
return owner == pthread_self();
|
||||
}
|
||||
|
||||
bool Semaphore::take(uint32_t timeout_ms)
|
||||
{
|
||||
if (timeout_ms == HAL_SEMAPHORE_BLOCK_FOREVER) {
|
||||
auto ok = pthread_mutex_lock(&_lock) == 0;
|
||||
if (ok) {
|
||||
owner = pthread_self();
|
||||
}
|
||||
return ok;
|
||||
}
|
||||
if (take_nonblocking()) {
|
||||
return true;
|
||||
}
|
||||
uint64_t start = AP_HAL::micros64();
|
||||
do {
|
||||
hal.scheduler->delay_microseconds(200);
|
||||
if (take_nonblocking()) {
|
||||
return true;
|
||||
}
|
||||
} while ((AP_HAL::micros64() - start) < timeout_ms*1000);
|
||||
return false;
|
||||
}
|
||||
|
||||
bool Semaphore::take_nonblocking()
|
||||
{
|
||||
const auto ok = pthread_mutex_trylock(&_lock) == 0;
|
||||
if (ok) {
|
||||
owner = pthread_self();
|
||||
}
|
||||
return ok;
|
||||
}
|
||||
|
||||
/*
|
||||
binary semaphore using condition variables
|
||||
*/
|
||||
|
||||
BinarySemaphore::BinarySemaphore(bool initial_state) :
|
||||
AP_HAL::BinarySemaphore(initial_state)
|
||||
{
|
||||
pthread_cond_init(&cond, NULL);
|
||||
pending = initial_state;
|
||||
}
|
||||
|
||||
bool BinarySemaphore::wait(uint32_t timeout_us)
|
||||
{
|
||||
WITH_SEMAPHORE(mtx);
|
||||
if (!pending) {
|
||||
struct timespec ts;
|
||||
if (clock_gettime(CLOCK_REALTIME, &ts) != 0) {
|
||||
return false;
|
||||
}
|
||||
ts.tv_sec += timeout_us/1000000UL;
|
||||
ts.tv_nsec += (timeout_us % 1000000U) * 1000UL;
|
||||
if (ts.tv_nsec >= 1000000000L) {
|
||||
ts.tv_sec++;
|
||||
ts.tv_nsec -= 1000000000L;
|
||||
}
|
||||
if (pthread_cond_timedwait(&cond, &mtx._lock, &ts) != 0) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
pending = false;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool BinarySemaphore::wait_blocking(void)
|
||||
{
|
||||
WITH_SEMAPHORE(mtx);
|
||||
if (!pending) {
|
||||
if (pthread_cond_wait(&cond, &mtx._lock) != 0) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
pending = false;
|
||||
return true;
|
||||
}
|
||||
|
||||
void BinarySemaphore::signal(void)
|
||||
{
|
||||
WITH_SEMAPHORE(mtx);
|
||||
if (!pending) {
|
||||
pending = true;
|
||||
pthread_cond_signal(&cond);
|
||||
}
|
||||
}
|
42
libraries/AP_HAL_QURT/Semaphores.h
Normal file
42
libraries/AP_HAL_QURT/Semaphores.h
Normal file
@ -0,0 +1,42 @@
|
||||
#pragma once
|
||||
|
||||
#include <AP_HAL/AP_HAL_Boards.h>
|
||||
#include <stdint.h>
|
||||
#include <AP_HAL/Semaphores.h>
|
||||
#include <pthread.h>
|
||||
|
||||
namespace QURT
|
||||
{
|
||||
|
||||
class Semaphore : public AP_HAL::Semaphore
|
||||
{
|
||||
public:
|
||||
friend class BinarySemaphore;
|
||||
Semaphore();
|
||||
bool give() override;
|
||||
bool take(uint32_t timeout_ms) override;
|
||||
bool take_nonblocking() override;
|
||||
bool check_owner(void);
|
||||
protected:
|
||||
// qurt_mutex_t _lock;
|
||||
pthread_mutex_t _lock;
|
||||
pthread_t owner;
|
||||
};
|
||||
|
||||
|
||||
class BinarySemaphore : public AP_HAL::BinarySemaphore
|
||||
{
|
||||
public:
|
||||
BinarySemaphore(bool initial_state=false);
|
||||
|
||||
bool wait(uint32_t timeout_us) override;
|
||||
bool wait_blocking(void) override;
|
||||
void signal(void) override;
|
||||
|
||||
protected:
|
||||
Semaphore mtx;
|
||||
pthread_cond_t cond;
|
||||
bool pending;
|
||||
};
|
||||
|
||||
}
|
195
libraries/AP_HAL_QURT/Storage.cpp
Normal file
195
libraries/AP_HAL_QURT/Storage.cpp
Normal file
@ -0,0 +1,195 @@
|
||||
#include "Storage.h"
|
||||
|
||||
#include <fcntl.h>
|
||||
#include <stdio.h>
|
||||
#include <sys/stat.h>
|
||||
#include <sys/types.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
using namespace QURT;
|
||||
|
||||
#define QURT_STORAGE_MAX_WRITE 512
|
||||
#define QURT_STORAGE_LINE_SHIFT 9
|
||||
#define QURT_STORAGE_LINE_SIZE (1<<QURT_STORAGE_LINE_SHIFT)
|
||||
#define QURT_STORAGE_NUM_LINES (QURT_STORAGE_SIZE/QURT_STORAGE_LINE_SIZE)
|
||||
|
||||
/*
|
||||
This stores 'eeprom' data on the SD card, with a 4k size, and a
|
||||
in-memory buffer. This keeps the latency down.
|
||||
*/
|
||||
|
||||
// name the storage file after the sketch so you can use the same board
|
||||
// card for ArduCopter and ArduPlane
|
||||
#define STORAGE_FILE AP_FILESYSTEM_POSIX_MAP_FILENAME_BASEDIR "/APM/" AP_BUILD_TARGET_NAME ".stg"
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
bool Storage::_storage_create(void)
|
||||
{
|
||||
int fd = -1;
|
||||
|
||||
fd = open(STORAGE_FILE, O_WRONLY|O_CREAT|O_TRUNC, 0755);
|
||||
if (fd == -1) {
|
||||
AP_HAL::panic("Failed to open storage: %s", STORAGE_FILE);
|
||||
return false;
|
||||
}
|
||||
|
||||
// take up all needed space
|
||||
if (write(fd, _buffer, sizeof(_buffer)) != sizeof(_buffer)) {
|
||||
AP_HAL::panic("Failed to init %s", STORAGE_FILE);
|
||||
goto fail;
|
||||
}
|
||||
|
||||
// ensure the directory is updated with the new size
|
||||
close(fd);
|
||||
|
||||
return true;
|
||||
|
||||
fail:
|
||||
if (fd != -1) {
|
||||
close(fd);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
void Storage::init()
|
||||
{
|
||||
if (_initialised) {
|
||||
return;
|
||||
}
|
||||
|
||||
_dirty_mask = 0;
|
||||
|
||||
int fd = open(STORAGE_FILE, O_RDWR);
|
||||
if (fd == -1) {
|
||||
_storage_create();
|
||||
fd = open(STORAGE_FILE, O_RDWR);
|
||||
if (fd == -1) {
|
||||
AP_HAL::panic("Failed to create %s", STORAGE_FILE);
|
||||
}
|
||||
}
|
||||
|
||||
ssize_t ret = read(fd, _buffer, sizeof(_buffer));
|
||||
|
||||
if (ret != sizeof(_buffer)) {
|
||||
close(fd);
|
||||
_storage_create();
|
||||
fd = open(STORAGE_FILE, O_RDWR);
|
||||
if (fd == -1) {
|
||||
AP_HAL::panic("Failed to open %s", STORAGE_FILE);
|
||||
}
|
||||
if (read(fd, _buffer, sizeof(_buffer)) != sizeof(_buffer)) {
|
||||
AP_HAL::panic("Failed to read %s", STORAGE_FILE);
|
||||
}
|
||||
}
|
||||
|
||||
_fd = fd;
|
||||
_initialised = true;
|
||||
}
|
||||
|
||||
/*
|
||||
mark some lines as dirty. Note that there is no attempt to avoid
|
||||
the race condition between this code and the _timer_tick() code
|
||||
below, which both update _dirty_mask. If we lose the race then the
|
||||
result is that a line is written more than once, but it won't result
|
||||
in a line not being written.
|
||||
*/
|
||||
void Storage::_mark_dirty(uint16_t loc, uint16_t length)
|
||||
{
|
||||
if (length == 0) {
|
||||
return;
|
||||
}
|
||||
uint16_t end = loc + length - 1;
|
||||
for (uint8_t line=loc>>QURT_STORAGE_LINE_SHIFT;
|
||||
line <= end>>QURT_STORAGE_LINE_SHIFT;
|
||||
line++) {
|
||||
_dirty_mask |= 1U << line;
|
||||
}
|
||||
}
|
||||
|
||||
void Storage::read_block(void *dst, uint16_t loc, size_t n)
|
||||
{
|
||||
if (loc >= sizeof(_buffer)-(n-1)) {
|
||||
return;
|
||||
}
|
||||
init();
|
||||
memcpy(dst, &_buffer[loc], n);
|
||||
}
|
||||
|
||||
void Storage::write_block(uint16_t loc, const void *src, size_t n)
|
||||
{
|
||||
if (loc >= sizeof(_buffer)-(n-1)) {
|
||||
return;
|
||||
}
|
||||
if (memcmp(src, &_buffer[loc], n) != 0) {
|
||||
init();
|
||||
memcpy(&_buffer[loc], src, n);
|
||||
_mark_dirty(loc, n);
|
||||
}
|
||||
}
|
||||
|
||||
void Storage::_timer_tick(void)
|
||||
{
|
||||
if (!_initialised || _dirty_mask == 0 || _fd == -1) {
|
||||
return;
|
||||
}
|
||||
|
||||
// write out the first dirty set of lines. We don't write more
|
||||
// than one to keep the latency of this call to a minimum
|
||||
uint8_t i, n;
|
||||
for (i=0; i<QURT_STORAGE_NUM_LINES; i++) {
|
||||
if (_dirty_mask & (1<<i)) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (i == QURT_STORAGE_NUM_LINES) {
|
||||
// this shouldn't be possible
|
||||
return;
|
||||
}
|
||||
uint32_t write_mask = (1U<<i);
|
||||
// see how many lines to write
|
||||
for (n=1; (i+n) < QURT_STORAGE_NUM_LINES &&
|
||||
n < (QURT_STORAGE_MAX_WRITE>>QURT_STORAGE_LINE_SHIFT); n++) {
|
||||
if (!(_dirty_mask & (1<<(n+i)))) {
|
||||
break;
|
||||
}
|
||||
// mark that line clean
|
||||
write_mask |= (1<<(n+i));
|
||||
}
|
||||
|
||||
/*
|
||||
write the lines. This also updates _dirty_mask. Note that
|
||||
because this is a SCHED_FIFO thread it will not be preempted
|
||||
by the main task except during blocking calls. This means we
|
||||
don't need a semaphore around the _dirty_mask updates.
|
||||
*/
|
||||
if (lseek(_fd, i<<QURT_STORAGE_LINE_SHIFT, SEEK_SET) == (i<<QURT_STORAGE_LINE_SHIFT)) {
|
||||
_dirty_mask &= ~write_mask;
|
||||
if (write(_fd, &_buffer[i<<QURT_STORAGE_LINE_SHIFT], n<<QURT_STORAGE_LINE_SHIFT) != n<<QURT_STORAGE_LINE_SHIFT) {
|
||||
// write error - likely EINTR
|
||||
_dirty_mask |= write_mask;
|
||||
close(_fd);
|
||||
_fd = -1;
|
||||
}
|
||||
if (_dirty_mask == 0) {
|
||||
// close and re-open to force storage to sync, QURT has no fsync() call
|
||||
close(_fd);
|
||||
_fd = open(STORAGE_FILE, O_RDWR);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
get storage size and ptr
|
||||
*/
|
||||
bool Storage::get_storage_ptr(void *&ptr, size_t &size)
|
||||
{
|
||||
if (!_initialised) {
|
||||
return false;
|
||||
}
|
||||
ptr = _buffer;
|
||||
size = sizeof(_buffer);
|
||||
return true;
|
||||
}
|
45
libraries/AP_HAL_QURT/Storage.h
Normal file
45
libraries/AP_HAL_QURT/Storage.h
Normal file
@ -0,0 +1,45 @@
|
||||
#pragma once
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
#define QURT_STORAGE_SIZE HAL_STORAGE_SIZE
|
||||
|
||||
namespace QURT
|
||||
{
|
||||
|
||||
class Storage : public AP_HAL::Storage
|
||||
{
|
||||
public:
|
||||
static Storage *from(AP_HAL::Storage *storage)
|
||||
{
|
||||
return static_cast<Storage*>(storage);
|
||||
}
|
||||
|
||||
|
||||
void init() override;
|
||||
|
||||
uint8_t read_byte(uint16_t loc);
|
||||
uint16_t read_word(uint16_t loc);
|
||||
uint32_t read_dword(uint16_t loc);
|
||||
void read_block(void *dst, uint16_t src, size_t n) override;
|
||||
|
||||
void write_byte(uint16_t loc, uint8_t value);
|
||||
void write_word(uint16_t loc, uint16_t value);
|
||||
void write_dword(uint16_t loc, uint32_t value);
|
||||
void write_block(uint16_t dst, const void* src, size_t n) override;
|
||||
|
||||
bool get_storage_ptr(void *&ptr, size_t &size) override;
|
||||
|
||||
virtual void _timer_tick(void) override;
|
||||
|
||||
protected:
|
||||
void _mark_dirty(uint16_t loc, uint16_t length);
|
||||
bool _storage_create(void);
|
||||
|
||||
int _fd = -1;
|
||||
volatile bool _initialised;
|
||||
volatile uint32_t _dirty_mask;
|
||||
uint8_t _buffer[QURT_STORAGE_SIZE];
|
||||
};
|
||||
|
||||
}
|
182
libraries/AP_HAL_QURT/Thread.cpp
Normal file
182
libraries/AP_HAL_QURT/Thread.cpp
Normal file
@ -0,0 +1,182 @@
|
||||
/*
|
||||
* Copyright (C) 2016 Intel Corporation. All rights reserved.
|
||||
*
|
||||
* This file is free software: you can redistribute it and/or modify it
|
||||
* under the terms of the GNU General Public License as published by the
|
||||
* Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This file is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
||||
* See the GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#include "Thread.h"
|
||||
|
||||
#include <limits.h>
|
||||
#include <sys/types.h>
|
||||
#include <stdio.h>
|
||||
#include <unistd.h>
|
||||
#include <utility>
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <AP_Math/AP_Math.h>
|
||||
#include "Scheduler.h"
|
||||
|
||||
extern const AP_HAL::HAL &hal;
|
||||
|
||||
namespace QURT
|
||||
{
|
||||
|
||||
|
||||
void *Thread::_run_trampoline(void *arg)
|
||||
{
|
||||
Thread *thread = static_cast<Thread *>(arg);
|
||||
thread->_run();
|
||||
|
||||
if (thread->_auto_free) {
|
||||
delete thread;
|
||||
}
|
||||
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
bool Thread::_run()
|
||||
{
|
||||
if (!_task) {
|
||||
return false;
|
||||
}
|
||||
|
||||
_task();
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
size_t Thread::get_stack_usage()
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
bool Thread::start(const char *name, int policy, int prio)
|
||||
{
|
||||
if (_started) {
|
||||
return false;
|
||||
}
|
||||
|
||||
struct sched_param param = { .sched_priority = prio };
|
||||
pthread_attr_t attr;
|
||||
int r;
|
||||
|
||||
pthread_attr_init(&attr);
|
||||
|
||||
if ((r = pthread_attr_setschedparam(&attr, ¶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;
|
||||
}
|
||||
|
||||
}
|
108
libraries/AP_HAL_QURT/Thread.h
Normal file
108
libraries/AP_HAL_QURT/Thread.h
Normal file
@ -0,0 +1,108 @@
|
||||
/*
|
||||
* Copyright (C) 2016 Intel Corporation. All rights reserved.
|
||||
*
|
||||
* This file is free software: you can redistribute it and/or modify it
|
||||
* under the terms of the GNU General Public License as published by the
|
||||
* Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This file is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
||||
* See the GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include <pthread.h>
|
||||
#include <inttypes.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#include <AP_HAL/utility/functor.h>
|
||||
|
||||
namespace QURT
|
||||
{
|
||||
|
||||
/*
|
||||
* Interface abstracting threads
|
||||
*/
|
||||
class Thread
|
||||
{
|
||||
public:
|
||||
FUNCTOR_TYPEDEF(task_t, void);
|
||||
|
||||
Thread(task_t t) : _task(t) { }
|
||||
|
||||
virtual ~Thread() { }
|
||||
|
||||
bool start(const char *name, int policy, int prio);
|
||||
|
||||
bool is_current_thread();
|
||||
|
||||
bool is_started() const
|
||||
{
|
||||
return _started;
|
||||
}
|
||||
|
||||
size_t get_stack_usage();
|
||||
|
||||
bool set_stack_size(size_t stack_size);
|
||||
|
||||
void set_auto_free(bool auto_free)
|
||||
{
|
||||
_auto_free = auto_free;
|
||||
}
|
||||
|
||||
virtual bool stop()
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
bool join();
|
||||
|
||||
protected:
|
||||
static void *_run_trampoline(void *arg);
|
||||
|
||||
/*
|
||||
* Run the task assigned in the constructor. May be overriden in case it's
|
||||
* preferred to use Thread as an interface or when user wants to aggregate
|
||||
* some initialization or teardown for the thread.
|
||||
*/
|
||||
virtual bool _run();
|
||||
|
||||
void _poison_stack();
|
||||
|
||||
task_t _task;
|
||||
bool _started = false;
|
||||
bool _should_exit = false;
|
||||
bool _auto_free = false;
|
||||
pthread_t _ctx = 0;
|
||||
|
||||
struct stack_debug {
|
||||
uint32_t *start;
|
||||
uint32_t *end;
|
||||
} _stack_debug;
|
||||
|
||||
size_t _stack_size = 0;
|
||||
};
|
||||
|
||||
class PeriodicThread : public Thread
|
||||
{
|
||||
public:
|
||||
PeriodicThread(Thread::task_t t)
|
||||
: Thread(t)
|
||||
{ }
|
||||
|
||||
bool set_rate(uint32_t rate_hz);
|
||||
|
||||
bool stop() override;
|
||||
|
||||
protected:
|
||||
bool _run() override;
|
||||
|
||||
uint64_t _period_usec = 0;
|
||||
};
|
||||
|
||||
}
|
278
libraries/AP_HAL_QURT/UARTDriver.cpp
Normal file
278
libraries/AP_HAL_QURT/UARTDriver.cpp
Normal file
@ -0,0 +1,278 @@
|
||||
|
||||
#include "interface.h"
|
||||
#include "UARTDriver.h"
|
||||
#include <AP_Common/ExpandingString.h>
|
||||
|
||||
#if HAL_GCS_ENABLED
|
||||
#include <AP_HAL/utility/packetise.h>
|
||||
#endif
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
using namespace QURT;
|
||||
|
||||
/* QURT implementations of virtual methods */
|
||||
void UARTDriver::_begin(uint32_t b, uint16_t rxS, uint16_t txS)
|
||||
{
|
||||
if (_initialised) {
|
||||
return;
|
||||
}
|
||||
|
||||
/* we have enough memory to have a larger transmit buffer for
|
||||
* all ports. This means we don't get delays while waiting to
|
||||
* write GPS config packets
|
||||
*/
|
||||
if (rxS < 4096) {
|
||||
rxS = 4096;
|
||||
}
|
||||
if (txS < 4096) {
|
||||
txS = 4096;
|
||||
}
|
||||
|
||||
WITH_SEMAPHORE(_write_mutex);
|
||||
|
||||
if (_writebuf.set_size(txS) && _readbuf.set_size(rxS)) {
|
||||
_initialised = true;
|
||||
}
|
||||
}
|
||||
|
||||
void UARTDriver::_end()
|
||||
{
|
||||
}
|
||||
|
||||
void UARTDriver::_flush()
|
||||
{
|
||||
}
|
||||
|
||||
bool UARTDriver::is_initialized()
|
||||
{
|
||||
return _initialised;
|
||||
}
|
||||
|
||||
bool UARTDriver::tx_pending()
|
||||
{
|
||||
return (_writebuf.available() > 0);
|
||||
}
|
||||
|
||||
uint32_t UARTDriver::_available()
|
||||
{
|
||||
if (!_initialised) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
WITH_SEMAPHORE(_read_mutex);
|
||||
|
||||
return _readbuf.available();
|
||||
}
|
||||
|
||||
uint32_t UARTDriver::txspace()
|
||||
{
|
||||
if (!_initialised) {
|
||||
return 0;
|
||||
}
|
||||
return _writebuf.space();
|
||||
}
|
||||
|
||||
bool UARTDriver::_discard_input()
|
||||
{
|
||||
if (!_initialised) {
|
||||
return false;
|
||||
}
|
||||
|
||||
WITH_SEMAPHORE(_read_mutex);
|
||||
|
||||
_readbuf.clear();
|
||||
return true;
|
||||
}
|
||||
|
||||
size_t UARTDriver::_write(const uint8_t *buffer, size_t size)
|
||||
{
|
||||
if (!_initialised) {
|
||||
return 0;
|
||||
}
|
||||
WITH_SEMAPHORE(_write_mutex);
|
||||
|
||||
return _writebuf.write(buffer, size);
|
||||
}
|
||||
|
||||
ssize_t UARTDriver::_read(uint8_t *buffer, uint16_t size)
|
||||
{
|
||||
if (!_initialised) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
WITH_SEMAPHORE(_read_mutex);
|
||||
|
||||
return _readbuf.read(buffer, size);
|
||||
}
|
||||
|
||||
/*
|
||||
push any pending bytes to/from the serial port. This is called at
|
||||
1kHz in the timer thread. Doing it this way reduces the system call
|
||||
overhead in the main task enormously.
|
||||
*/
|
||||
void UARTDriver::_timer_tick(void)
|
||||
{
|
||||
if (!_initialised) {
|
||||
return;
|
||||
}
|
||||
|
||||
for (auto i=0; i<10; i++) {
|
||||
if (!_write_pending_bytes()) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
_fill_read_buffer();
|
||||
}
|
||||
|
||||
/*
|
||||
methods for UARTDriver_Console
|
||||
*/
|
||||
void UARTDriver_Console::printf(const char *fmt, ...)
|
||||
{
|
||||
va_list ap;
|
||||
char buf[300];
|
||||
va_start(ap, fmt);
|
||||
vsnprintf(buf, sizeof(buf), fmt, ap);
|
||||
va_end(ap);
|
||||
HAP_PRINTF(buf);
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
methods for UARTDriver_MAVLinkUDP
|
||||
*/
|
||||
typedef void (*mavlink_data_callback_t)(const struct qurt_mavlink_msg *msg, void* p);
|
||||
extern void register_mavlink_data_callback(mavlink_data_callback_t func, void *p);
|
||||
|
||||
UARTDriver_MAVLinkUDP::UARTDriver_MAVLinkUDP(void)
|
||||
{
|
||||
register_mavlink_data_callback(_mavlink_data_cb, (void *) this);
|
||||
}
|
||||
|
||||
void UARTDriver_MAVLinkUDP::_mavlink_data_cb(const struct qurt_mavlink_msg *msg, void *p)
|
||||
{
|
||||
auto *driver = (UARTDriver_MAVLinkUDP *)p;
|
||||
driver->_readbuf.write(msg->mav_msg, msg->data_length);
|
||||
}
|
||||
|
||||
/*
|
||||
try to push out one lump of pending bytes
|
||||
return true if progress is made
|
||||
*/
|
||||
bool UARTDriver_MAVLinkUDP::_write_pending_bytes(void)
|
||||
{
|
||||
WITH_SEMAPHORE(_write_mutex);
|
||||
|
||||
// write any pending bytes
|
||||
const uint32_t available_bytes = _writebuf.available();
|
||||
uint16_t n = available_bytes;
|
||||
|
||||
if (n > 0) {
|
||||
// send on MAVLink packet boundaries if possible
|
||||
n = mavlink_packetise(_writebuf, n);
|
||||
}
|
||||
|
||||
if (n <= 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
struct qurt_mavlink_msg msg;
|
||||
if (n > sizeof(msg.mav_msg)) {
|
||||
return false;
|
||||
}
|
||||
msg.seq = seq++;
|
||||
msg.data_length = n;
|
||||
n = _writebuf.read(msg.mav_msg, n);
|
||||
if (n > 0) {
|
||||
(void) sl_client_send_data((const uint8_t*)&msg, n + QURT_MAVLINK_MSG_HEADER_LEN);
|
||||
}
|
||||
return n > 0;
|
||||
}
|
||||
|
||||
/*
|
||||
setup baudrate for this local UART
|
||||
*/
|
||||
void UARTDriver_Local::_begin(uint32_t b, uint16_t rxS, uint16_t txS)
|
||||
{
|
||||
if (b == 0) {
|
||||
// re-open not needed
|
||||
return;
|
||||
}
|
||||
|
||||
// QURT wants 420000 for CRSF, ArduPilot driver wants 416666
|
||||
if (b == 416666) {
|
||||
b = 420000;
|
||||
}
|
||||
|
||||
UARTDriver::_begin(b, rxS, txS);
|
||||
|
||||
if (baudrate != b || fd == -1) {
|
||||
int fd2 = sl_client_config_uart(port_id, b);
|
||||
if (fd2 == -1 && fd != -1) {
|
||||
// baudrate rejected, revert to last baudrate
|
||||
sl_client_config_uart(port_id, baudrate);
|
||||
}
|
||||
if (fd2 != -1) {
|
||||
baudrate = b;
|
||||
fd = fd2;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
push out pending bytes
|
||||
*/
|
||||
bool UARTDriver_Local::_write_pending_bytes(void)
|
||||
{
|
||||
WITH_SEMAPHORE(_write_mutex);
|
||||
if (fd == -1) {
|
||||
return false;
|
||||
}
|
||||
uint32_t available;
|
||||
const uint8_t *ptr = _writebuf.readptr(available);
|
||||
if (ptr != nullptr) {
|
||||
auto n = sl_client_uart_write(fd, (const char *)ptr, available);
|
||||
if (n > 0) {
|
||||
_writebuf.advance(n);
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
/*
|
||||
read from the UART into _readbuf
|
||||
*/
|
||||
void UARTDriver_Local::_fill_read_buffer(void)
|
||||
{
|
||||
WITH_SEMAPHORE(_read_mutex);
|
||||
if (fd == -1) {
|
||||
return;
|
||||
}
|
||||
uint32_t n = _readbuf.space();
|
||||
if (n > 512) {
|
||||
n = 512;
|
||||
}
|
||||
char buf[n];
|
||||
auto nread = sl_client_uart_read(fd, buf, sizeof(buf));
|
||||
if (nread > 0) {
|
||||
_readbuf.write((const uint8_t *)buf, nread);
|
||||
receive_timestamp_us = AP_HAL::micros64();
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
return timestamp estimate in microseconds for when the start of a
|
||||
nbytes packet arrived on the uart.
|
||||
*/
|
||||
uint64_t UARTDriver_Local::receive_time_constraint_us(uint16_t nbytes)
|
||||
{
|
||||
uint64_t last_receive_us = receive_timestamp_us;
|
||||
if (baudrate > 0) {
|
||||
// assume 10 bits per byte
|
||||
uint32_t transport_time_us = (1000000UL * 10UL / baudrate) * (nbytes + available());
|
||||
last_receive_us -= transport_time_us;
|
||||
}
|
||||
return last_receive_us;
|
||||
}
|
132
libraries/AP_HAL_QURT/UARTDriver.h
Normal file
132
libraries/AP_HAL_QURT/UARTDriver.h
Normal file
@ -0,0 +1,132 @@
|
||||
/*
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "AP_HAL_QURT.h"
|
||||
#include "Semaphores.h"
|
||||
#include <AP_HAL/utility/RingBuffer.h>
|
||||
#include "ap_host/src/protocol.h"
|
||||
|
||||
class QURT::UARTDriver : public AP_HAL::UARTDriver
|
||||
{
|
||||
public:
|
||||
bool is_initialized() override;
|
||||
bool tx_pending() override;
|
||||
|
||||
/* Empty implementations of Stream virtual methods */
|
||||
uint32_t txspace() override;
|
||||
|
||||
virtual bool _write_pending_bytes(void)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
virtual void _timer_tick(void) override;
|
||||
virtual void _fill_read_buffer(void) {}
|
||||
|
||||
virtual uint32_t bw_in_bytes_per_second() const override
|
||||
{
|
||||
return 5760;
|
||||
}
|
||||
virtual enum AP_HAL::UARTDriver::flow_control get_flow_control(void) override
|
||||
{
|
||||
return AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE;
|
||||
}
|
||||
|
||||
protected:
|
||||
virtual void _begin(uint32_t b, uint16_t rxS, uint16_t txS) override;
|
||||
size_t _write(const uint8_t *buffer, size_t size) override;
|
||||
ssize_t _read(uint8_t *buffer, uint16_t size) override WARN_IF_UNUSED;
|
||||
void _end() override;
|
||||
void _flush() override;
|
||||
uint32_t _available() override;
|
||||
bool _discard_input() override;
|
||||
volatile bool _initialised;
|
||||
|
||||
ByteBuffer _readbuf{0};
|
||||
ByteBuffer _writebuf{0};
|
||||
|
||||
QURT::Semaphore _read_mutex;
|
||||
QURT::Semaphore _write_mutex;
|
||||
};
|
||||
|
||||
/*
|
||||
subclass for console output, maps to HAP_PRINTF
|
||||
*/
|
||||
class QURT::UARTDriver_Console : public QURT::UARTDriver
|
||||
{
|
||||
public:
|
||||
using UARTDriver::UARTDriver;
|
||||
virtual void printf(const char *fmt, ...) override;
|
||||
};
|
||||
|
||||
/*
|
||||
subclass for MAVLink UDP communications
|
||||
*/
|
||||
class QURT::UARTDriver_MAVLinkUDP : public QURT::UARTDriver
|
||||
{
|
||||
public:
|
||||
UARTDriver_MAVLinkUDP(void);
|
||||
|
||||
bool _write_pending_bytes(void) override;
|
||||
|
||||
uint32_t bw_in_bytes_per_second() const override
|
||||
{
|
||||
return 250000 * 3;
|
||||
}
|
||||
enum AP_HAL::UARTDriver::flow_control get_flow_control(void) override
|
||||
{
|
||||
return AP_HAL::UARTDriver::FLOW_CONTROL_ENABLE;
|
||||
}
|
||||
|
||||
private:
|
||||
static void _mavlink_data_cb(const struct qurt_mavlink_msg *msg, void *p);
|
||||
uint32_t seq;
|
||||
};
|
||||
|
||||
/*
|
||||
subclass for local UART communications
|
||||
*/
|
||||
class QURT::UARTDriver_Local : public QURT::UARTDriver
|
||||
{
|
||||
public:
|
||||
UARTDriver_Local(uint8_t _port_id) : port_id(_port_id) {}
|
||||
|
||||
uint32_t bw_in_bytes_per_second() const override
|
||||
{
|
||||
return baudrate?baudrate/10:5760;
|
||||
}
|
||||
|
||||
bool _write_pending_bytes(void) override;
|
||||
void _begin(uint32_t b, uint16_t rxS, uint16_t txS) override;
|
||||
void _fill_read_buffer(void) override;
|
||||
|
||||
uint32_t get_baud_rate() const override
|
||||
{
|
||||
return baudrate;
|
||||
}
|
||||
|
||||
/*
|
||||
return timestamp estimate in microseconds for when the start of
|
||||
a nbytes packet arrived on the uart.
|
||||
*/
|
||||
uint64_t receive_time_constraint_us(uint16_t nbytes) override;
|
||||
|
||||
private:
|
||||
const uint8_t port_id;
|
||||
int fd = -1;
|
||||
uint32_t baudrate;
|
||||
uint64_t receive_timestamp_us;
|
||||
};
|
80
libraries/AP_HAL_QURT/Util.cpp
Normal file
80
libraries/AP_HAL_QURT/Util.cpp
Normal file
@ -0,0 +1,80 @@
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include "Semaphores.h"
|
||||
#include <stdio.h>
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
#include "Util.h"
|
||||
|
||||
using namespace QURT;
|
||||
|
||||
extern "C" {
|
||||
void *fc_heap_alloc(size_t size);
|
||||
void fc_heap_free(void* ptr);
|
||||
size_t fc_heap_size(void);
|
||||
size_t fc_heap_usage(void);
|
||||
}
|
||||
|
||||
uint32_t Util::available_memory(void)
|
||||
{
|
||||
return fc_heap_size() - fc_heap_usage();
|
||||
}
|
||||
|
||||
#if ENABLE_HEAP
|
||||
void *Util::allocate_heap_memory(size_t size)
|
||||
{
|
||||
struct heap *new_heap = (struct heap*)malloc(sizeof(struct heap));
|
||||
if (new_heap != nullptr) {
|
||||
new_heap->max_heap_size = size;
|
||||
new_heap->current_heap_usage = 0;
|
||||
}
|
||||
return (void *)new_heap;
|
||||
}
|
||||
|
||||
void *Util::heap_realloc(void *h, void *ptr, size_t old_size, size_t new_size)
|
||||
{
|
||||
if (h == nullptr) {
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
struct heap *heapp = (struct heap*)h;
|
||||
|
||||
// extract appropriate headers. We use the old_size from the
|
||||
// header not from the caller. We use SITL to catch cases they
|
||||
// don't match (which would be a lua bug)
|
||||
old_size = 0;
|
||||
heap_allocation_header *old_header = nullptr;
|
||||
if (ptr != nullptr) {
|
||||
old_header = ((heap_allocation_header *)ptr) - 1;
|
||||
old_size = old_header->allocation_size;
|
||||
}
|
||||
|
||||
if ((heapp->current_heap_usage + new_size - old_size) > heapp->max_heap_size) {
|
||||
// fail the allocation as we don't have the memory. Note that we don't simulate fragmentation
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
heapp->current_heap_usage -= old_size;
|
||||
if (new_size == 0) {
|
||||
free(old_header);
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
heap_allocation_header *new_header = (heap_allocation_header *)malloc(new_size + sizeof(heap_allocation_header));
|
||||
if (new_header == nullptr) {
|
||||
// can't get the new memory, old memory is kept
|
||||
return nullptr;
|
||||
}
|
||||
heapp->current_heap_usage += new_size;
|
||||
new_header->allocation_size = new_size;
|
||||
void *new_mem = new_header + 1;
|
||||
|
||||
if (ptr == nullptr) {
|
||||
return new_mem;
|
||||
}
|
||||
memcpy(new_mem, ptr, old_size > new_size ? new_size : old_size);
|
||||
free(old_header);
|
||||
return new_mem;
|
||||
}
|
||||
|
||||
#endif // ENABLE_HEAP
|
38
libraries/AP_HAL_QURT/Util.h
Normal file
38
libraries/AP_HAL_QURT/Util.h
Normal file
@ -0,0 +1,38 @@
|
||||
#pragma once
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include "AP_HAL_QURT_Namespace.h"
|
||||
|
||||
class QURT::Util : public AP_HAL::Util
|
||||
{
|
||||
public:
|
||||
/*
|
||||
set HW RTC in UTC microseconds
|
||||
*/
|
||||
void set_hw_rtc(uint64_t time_utc_usec) override {}
|
||||
|
||||
/*
|
||||
get system clock in UTC microseconds
|
||||
*/
|
||||
uint64_t get_hw_rtc() const override
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
uint32_t available_memory(void) override;
|
||||
|
||||
#if ENABLE_HEAP
|
||||
// heap functions, note that a heap once alloc'd cannot be dealloc'd
|
||||
virtual void *allocate_heap_memory(size_t size) override;
|
||||
virtual void *heap_realloc(void *h, void *ptr, size_t old_size, size_t new_size) override;
|
||||
|
||||
struct heap_allocation_header {
|
||||
uint64_t allocation_size; // size of allocated block, not including this header
|
||||
};
|
||||
|
||||
struct heap {
|
||||
size_t max_heap_size;
|
||||
size_t current_heap_usage;
|
||||
};
|
||||
#endif // ENABLE_HEAP
|
||||
};
|
@ -0,0 +1,22 @@
|
||||
#ifndef SLPI_LINK_H
|
||||
#define SLPI_LINK_H
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
typedef void (*slpi_link_cb)(const uint8_t *data, uint32_t length_in_bytes);
|
||||
|
||||
int slpi_link_init(bool enable_debug_messages, slpi_link_cb callback, const char *library_name);
|
||||
int slpi_link_get_time_offset(void);
|
||||
int slpi_link_send(const uint8_t *data, uint32_t length_in_bytes);
|
||||
void slpi_link_reset(void);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // SLPI_LINK_H
|
@ -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;
|
||||
}
|
||||
|
188
libraries/AP_HAL_QURT/ap_host/src/getifaddrs.cpp
Normal file
188
libraries/AP_HAL_QURT/ap_host/src/getifaddrs.cpp
Normal file
@ -0,0 +1,188 @@
|
||||
/*
|
||||
get system network addresses
|
||||
|
||||
based on code from Samba
|
||||
|
||||
Copyright (C) Andrew Tridgell 1998
|
||||
Copyright (C) Jeremy Allison 2007
|
||||
Copyright (C) Jelmer Vernooij <jelmer@samba.org> 2007
|
||||
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdio.h>
|
||||
#include <unistd.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <sys/types.h>
|
||||
#include <sys/time.h>
|
||||
#include <ifaddrs.h>
|
||||
#include <net/if.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <netinet/in.h>
|
||||
#include <arpa/inet.h>
|
||||
|
||||
void freeifaddrs(struct ifaddrs *ifp)
|
||||
{
|
||||
if (ifp != nullptr) {
|
||||
free(ifp->ifa_name);
|
||||
free(ifp->ifa_addr);
|
||||
free(ifp->ifa_netmask);
|
||||
free(ifp->ifa_dstaddr);
|
||||
freeifaddrs(ifp->ifa_next);
|
||||
free(ifp);
|
||||
}
|
||||
}
|
||||
|
||||
static struct sockaddr *sockaddr_dup(struct sockaddr *sa)
|
||||
{
|
||||
struct sockaddr *ret;
|
||||
socklen_t socklen;
|
||||
socklen = sizeof(struct sockaddr_storage);
|
||||
ret = (struct sockaddr *)calloc(1, socklen);
|
||||
if (ret == nullptr) {
|
||||
return nullptr;
|
||||
}
|
||||
memcpy(ret, sa, socklen);
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* this works for Linux 2.2, Solaris 2.5, SunOS4, HPUX 10.20, OSF1
|
||||
V4.0, Ultrix 4.4, SCO Unix 3.2, IRIX 6.4 and FreeBSD 3.2.
|
||||
|
||||
It probably also works on any BSD style system. */
|
||||
|
||||
int getifaddrs(struct ifaddrs **ifap)
|
||||
{
|
||||
struct ifconf ifc;
|
||||
char buff[8192];
|
||||
int fd, i, n;
|
||||
struct ifreq *ifr=nullptr;
|
||||
struct ifaddrs *curif;
|
||||
struct ifaddrs *lastif = nullptr;
|
||||
|
||||
*ifap = nullptr;
|
||||
|
||||
if ((fd = socket(AF_INET, SOCK_DGRAM, 0)) == -1) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
ifc.ifc_len = sizeof(buff);
|
||||
ifc.ifc_buf = buff;
|
||||
|
||||
if (ioctl(fd, SIOCGIFCONF, &ifc) != 0) {
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
|
||||
ifr = ifc.ifc_req;
|
||||
|
||||
n = ifc.ifc_len / sizeof(struct ifreq);
|
||||
|
||||
/* Loop through interfaces, looking for ones that are broadcast capable */
|
||||
for (i=n-1; i>=0; i--) {
|
||||
if (ioctl(fd, SIOCGIFFLAGS, &ifr[i]) == -1) {
|
||||
freeifaddrs(*ifap);
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
|
||||
curif = (struct ifaddrs *)calloc(1, sizeof(struct ifaddrs));
|
||||
if (curif == nullptr) {
|
||||
freeifaddrs(*ifap);
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
curif->ifa_name = strdup(ifr[i].ifr_name);
|
||||
if (curif->ifa_name == nullptr) {
|
||||
free(curif);
|
||||
freeifaddrs(*ifap);
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
curif->ifa_flags = ifr[i].ifr_flags;
|
||||
curif->ifa_dstaddr = nullptr;
|
||||
curif->ifa_data = nullptr;
|
||||
curif->ifa_next = nullptr;
|
||||
|
||||
curif->ifa_addr = nullptr;
|
||||
if (ioctl(fd, SIOCGIFADDR, &ifr[i]) != -1) {
|
||||
curif->ifa_addr = sockaddr_dup(&ifr[i].ifr_addr);
|
||||
if (curif->ifa_addr == nullptr) {
|
||||
free(curif->ifa_name);
|
||||
free(curif);
|
||||
freeifaddrs(*ifap);
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
curif->ifa_netmask = nullptr;
|
||||
if (ioctl(fd, SIOCGIFNETMASK, &ifr[i]) != -1) {
|
||||
curif->ifa_netmask = sockaddr_dup(&ifr[i].ifr_addr);
|
||||
if (curif->ifa_netmask == nullptr) {
|
||||
if (curif->ifa_addr != nullptr) {
|
||||
free(curif->ifa_addr);
|
||||
}
|
||||
free(curif->ifa_name);
|
||||
free(curif);
|
||||
freeifaddrs(*ifap);
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
if (lastif == nullptr) {
|
||||
*ifap = curif;
|
||||
} else {
|
||||
lastif->ifa_next = curif;
|
||||
}
|
||||
lastif = curif;
|
||||
}
|
||||
|
||||
close(fd);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
const char *get_ipv4_broadcast(void)
|
||||
{
|
||||
struct ifaddrs *ifap = nullptr;
|
||||
if (getifaddrs(&ifap) != 0) {
|
||||
return nullptr;
|
||||
}
|
||||
struct ifaddrs *ia;
|
||||
for (ia=ifap; ia; ia=ia->ifa_next) {
|
||||
struct sockaddr_in *sin = (struct sockaddr_in *)ia->ifa_addr;
|
||||
struct sockaddr_in *nmask = (struct sockaddr_in *)ia->ifa_netmask;
|
||||
struct in_addr bcast;
|
||||
if (strcmp(ia->ifa_name, "lo") == 0) {
|
||||
continue;
|
||||
}
|
||||
bcast.s_addr = sin->sin_addr.s_addr | ~nmask->sin_addr.s_addr;
|
||||
const char *ret = inet_ntoa(bcast);
|
||||
freeifaddrs(ifap);
|
||||
return ret;
|
||||
}
|
||||
freeifaddrs(ifap);
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
#ifdef MAIN_PROGRAM
|
||||
int main(void)
|
||||
{
|
||||
printf("%s\n", get_ipv4_broadcast());
|
||||
return 0;
|
||||
}
|
||||
#endif
|
2
libraries/AP_HAL_QURT/ap_host/src/getifaddrs.h
Normal file
2
libraries/AP_HAL_QURT/ap_host/src/getifaddrs.h
Normal file
@ -0,0 +1,2 @@
|
||||
const char *get_ipv4_broadcast(void);
|
||||
|
213
libraries/AP_HAL_QURT/ap_host/src/main.cpp
Normal file
213
libraries/AP_HAL_QURT/ap_host/src/main.cpp
Normal file
@ -0,0 +1,213 @@
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <string.h>
|
||||
#include <unistd.h>
|
||||
#include <pthread.h>
|
||||
#include <signal.h>
|
||||
#include <netinet/in.h>
|
||||
#include <arpa/inet.h>
|
||||
#include <string.h>
|
||||
#include <errno.h>
|
||||
#include <sys/stat.h>
|
||||
#include <sys/types.h>
|
||||
#include <unistd.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#include "slpi_link.h"
|
||||
#include "protocol.h"
|
||||
#include "getifaddrs.h"
|
||||
|
||||
volatile bool _running = false;
|
||||
static bool enable_debug = false;
|
||||
static bool enable_remote_debug = false;
|
||||
|
||||
static int socket_fd;
|
||||
static bool connected;
|
||||
static struct sockaddr_in remote_addr;
|
||||
|
||||
#define UDP_OUT_PORT 14551
|
||||
#define SO_NAME "ArduPilot.so"
|
||||
|
||||
// directory for logs, parameters, terrain etc
|
||||
#define DATA_DIRECTORY "/data/APM"
|
||||
|
||||
static void shutdown_signal_handler(int signo)
|
||||
{
|
||||
switch (signo) {
|
||||
case SIGINT: // normal ctrl-c shutdown interrupt
|
||||
_running = false;
|
||||
fprintf(stderr, "\nreceived SIGINT Ctrl-C\n");
|
||||
break;
|
||||
case SIGTERM: // catchable terminate signal
|
||||
_running = false;
|
||||
fprintf(stderr, "\nreceived SIGTERM\n");
|
||||
break;
|
||||
case SIGHUP:
|
||||
// terminal closed or disconnected, carry on anyway
|
||||
fprintf(stderr, "\nreceived SIGHUP, continuing anyway\n");
|
||||
break;
|
||||
default:
|
||||
fprintf(stderr, "\nreceived signal %d\n", signo);
|
||||
break;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
static uint32_t num_params = 0;
|
||||
static uint32_t expected_seq = 0;
|
||||
|
||||
static void receive_callback(const uint8_t *data, uint32_t length_in_bytes)
|
||||
{
|
||||
if (enable_debug) {
|
||||
printf("Got %u bytes in receive callback\n", length_in_bytes);
|
||||
}
|
||||
const auto *msg = (const struct qurt_mavlink_msg *)data;
|
||||
if (length_in_bytes < QURT_MAVLINK_MSG_HEADER_LEN) {
|
||||
printf("Invalid length_in_bytes %d", length_in_bytes);
|
||||
return;
|
||||
}
|
||||
if (msg->data_length + QURT_MAVLINK_MSG_HEADER_LEN != length_in_bytes) {
|
||||
printf("Invalid lengths %d %d\n", msg->data_length, length_in_bytes);
|
||||
return;
|
||||
}
|
||||
if (msg->seq != expected_seq) {
|
||||
printf("Invalid seq %u %u\n", msg->seq, expected_seq);
|
||||
}
|
||||
expected_seq = msg->seq + 1;
|
||||
|
||||
switch (msg->msg_id) {
|
||||
case QURT_MSG_ID_MAVLINK_MSG: {
|
||||
if (_running) {
|
||||
const auto bytes_sent = sendto(socket_fd, msg->mav_msg, msg->data_length, 0, (struct sockaddr *)&remote_addr, sizeof(remote_addr));
|
||||
if (bytes_sent <= 0) {
|
||||
fprintf(stderr, "Send to GCS failed\n");
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
default:
|
||||
fprintf(stderr, "Got unknown message id %d\n", msg->msg_id);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
setup directories for the hexagon code to use
|
||||
*/
|
||||
static void setup_directores(void)
|
||||
{
|
||||
struct stat st;
|
||||
|
||||
mkdir(DATA_DIRECTORY, 0777);
|
||||
chmod(DATA_DIRECTORY, 0777);
|
||||
if (stat(DATA_DIRECTORY, &st) != 0 || !S_ISDIR(st.st_mode)) {
|
||||
printf("Unable to create %s", DATA_DIRECTORY);
|
||||
exit(1);
|
||||
}
|
||||
}
|
||||
|
||||
int main()
|
||||
{
|
||||
printf("Starting up\n");
|
||||
|
||||
setup_directores();
|
||||
|
||||
// make the sigaction struct for shutdown signals
|
||||
// sa_handler and sa_sigaction is a union, only set one
|
||||
struct sigaction action;
|
||||
action.sa_handler = shutdown_signal_handler;
|
||||
sigemptyset(&action.sa_mask);
|
||||
action.sa_flags = 0;
|
||||
|
||||
// set actions
|
||||
if (sigaction(SIGINT, &action, NULL) < 0) {
|
||||
fprintf(stderr, "ERROR: failed to set sigaction\n");
|
||||
return -1;
|
||||
}
|
||||
if (sigaction(SIGTERM, &action, NULL) < 0) {
|
||||
fprintf(stderr, "ERROR: failed to set sigaction\n");
|
||||
return -1;
|
||||
}
|
||||
if (sigaction(SIGHUP, &action, NULL) < 0) {
|
||||
fprintf(stderr, "ERROR: failed to set sigaction\n");
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (slpi_link_init(enable_remote_debug, &receive_callback, SO_NAME) != 0) {
|
||||
fprintf(stderr, "Error with init\n");
|
||||
sleep(1);
|
||||
slpi_link_reset();
|
||||
return -1;
|
||||
} else if (enable_debug) {
|
||||
printf("slpi_link_initialize succeeded\n");
|
||||
}
|
||||
|
||||
//initialize socket and structure
|
||||
socket_fd = socket(AF_INET, SOCK_DGRAM, 0);
|
||||
if (socket_fd == -1) {
|
||||
fprintf(stderr, "Could not create socket");
|
||||
return -1;
|
||||
}
|
||||
|
||||
const char *bcast_address = get_ipv4_broadcast();
|
||||
printf("Broadcast address=%s\n", bcast_address);
|
||||
inet_aton(bcast_address, &remote_addr.sin_addr);
|
||||
remote_addr.sin_family = AF_INET;
|
||||
remote_addr.sin_port = htons(UDP_OUT_PORT);
|
||||
|
||||
int one = 1;
|
||||
setsockopt(socket_fd,SOL_SOCKET,SO_BROADCAST,(char *)&one,sizeof(one));
|
||||
|
||||
struct sockaddr_in any {};
|
||||
any.sin_addr.s_addr = INADDR_ANY;
|
||||
any.sin_port = htons(15550);
|
||||
|
||||
if (bind(socket_fd, (struct sockaddr *)&any, sizeof(any)) == 0) {
|
||||
printf("Bind OK\n");
|
||||
}
|
||||
|
||||
printf("Waiting for receive\n");
|
||||
|
||||
printf("Enter ctrl-c to exit\n");
|
||||
_running = true;
|
||||
uint32_t next_seq = 0;
|
||||
|
||||
while (_running) {
|
||||
struct qurt_mavlink_msg msg {};
|
||||
struct sockaddr_in from;
|
||||
socklen_t fromlen = sizeof(from);
|
||||
uint32_t bytes_received = recvfrom(socket_fd, msg.mav_msg, sizeof(msg.mav_msg), 0,
|
||||
(struct sockaddr*)&from, &fromlen);
|
||||
if (bytes_received > 0 && !connected) {
|
||||
remote_addr = from;
|
||||
connected = true;
|
||||
printf("Connnected to %s\n", inet_ntoa(from.sin_addr));
|
||||
}
|
||||
if (bytes_received < 0) {
|
||||
fprintf(stderr, "Received failed");
|
||||
continue;
|
||||
}
|
||||
if (bytes_received > sizeof(msg.mav_msg)) {
|
||||
printf("Invalid bytes_received %d\n", bytes_received);
|
||||
continue;
|
||||
}
|
||||
msg.data_length = bytes_received;
|
||||
msg.seq = next_seq++;
|
||||
// printf("Message received. %d bytes\n", bytes_received);
|
||||
if (slpi_link_send((const uint8_t*) &msg, bytes_received + QURT_MAVLINK_MSG_HEADER_LEN)) {
|
||||
fprintf(stderr, "slpi_link_send_data failed\n");
|
||||
}
|
||||
}
|
||||
|
||||
printf("Reseting SLPI\n");
|
||||
// Send reset to SLPI
|
||||
slpi_link_reset();
|
||||
sleep(1);
|
||||
|
||||
printf("Exiting\n");
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
18
libraries/AP_HAL_QURT/ap_host/src/protocol.h
Normal file
18
libraries/AP_HAL_QURT/ap_host/src/protocol.h
Normal file
@ -0,0 +1,18 @@
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
// #include <GCS_MAVLink/GCS_MAVLink.h>
|
||||
#define MAVLINK_MAX_PAYLOAD_LEN 255
|
||||
|
||||
#pragma once
|
||||
|
||||
#define QURT_MSG_ID_MAVLINK_MSG 1
|
||||
|
||||
struct __attribute__((__packed__)) qurt_mavlink_msg {
|
||||
uint8_t msg_id{QURT_MSG_ID_MAVLINK_MSG};
|
||||
uint16_t data_length;
|
||||
uint32_t seq;
|
||||
uint8_t mav_msg[MAVLINK_MAX_PAYLOAD_LEN];
|
||||
};
|
||||
|
||||
#define QURT_MAVLINK_MSG_HEADER_LEN 7
|
49
libraries/AP_HAL_QURT/interface.h
Normal file
49
libraries/AP_HAL_QURT/interface.h
Normal file
@ -0,0 +1,49 @@
|
||||
#define __EXPORT __attribute__ ((visibility ("default")))
|
||||
|
||||
#ifndef __cplusplus
|
||||
#error "C++ should be defined!!!"
|
||||
#endif
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
// Functions that are called by the SLPI LINK server into AP client
|
||||
extern "C" {
|
||||
// Called by the SLPI LINK server to initialize and start AP
|
||||
int slpi_link_client_init(void) __EXPORT;
|
||||
|
||||
// Called by the SLPI LINK server when there is a new message for AP
|
||||
int slpi_link_client_receive(const uint8_t *data, int data_len_in_bytes) __EXPORT;
|
||||
}
|
||||
|
||||
// Functions in SLPI LINK server that are called by AP client into DSP
|
||||
extern "C" {
|
||||
// Send a message to the applications processor
|
||||
int sl_client_send_data(const uint8_t *data, int data_len_in_bytes);
|
||||
|
||||
void sl_client_register_fatal_error_cb(void (*func)(void));
|
||||
|
||||
// Interrupt callback registration
|
||||
int sl_client_register_interrupt_callback(int (*func)(int, void*, void*), void* arg);
|
||||
|
||||
// Get DSP CPU utilization (0 - 100)
|
||||
int sl_client_get_cpu_utilization(void);
|
||||
|
||||
// I2C interface API
|
||||
int sl_client_config_i2c_bus(uint8_t bus_number, uint8_t address, uint32_t frequency);
|
||||
void sl_client_set_address_i2c_bus(int fd, uint8_t address);
|
||||
int sl_client_i2c_transfer(int fd, const uint8_t *send, const unsigned send_len, uint8_t *recv, const unsigned recv_len);
|
||||
|
||||
// SPI interface API
|
||||
int sl_client_spi_transfer(int fd, const uint8_t *send, uint8_t *recv, const unsigned len);
|
||||
int sl_client_config_spi_bus(void);
|
||||
|
||||
// UART interface API
|
||||
int sl_client_config_uart(uint8_t port_number, uint32_t speed);
|
||||
int sl_client_uart_write(int fd, const char *data, const unsigned data_len);
|
||||
int sl_client_uart_read(int fd, char *buffer, const unsigned buffer_len);
|
||||
}
|
||||
|
||||
// IDs for serial ports
|
||||
#define QURT_UART_GPS 6
|
||||
#define QURT_UART_RCIN 7
|
||||
#define QURT_UART_ESC 2
|
84
libraries/AP_HAL_QURT/malloc.cpp
Normal file
84
libraries/AP_HAL_QURT/malloc.cpp
Normal file
@ -0,0 +1,84 @@
|
||||
/*
|
||||
wrappers around core memory allocation functions from libc
|
||||
*/
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <new>
|
||||
|
||||
const std::nothrow_t std::nothrow;
|
||||
|
||||
extern "C" {
|
||||
void *fc_heap_alloc(size_t size);
|
||||
void fc_heap_free(void* ptr);
|
||||
|
||||
void *__wrap_malloc(size_t size);
|
||||
void __wrap_free(void *ptr);
|
||||
void *__wrap_calloc(size_t nmemb, size_t size);
|
||||
void *__wrap_realloc(void *ptr, size_t size);
|
||||
}
|
||||
|
||||
#define HEAP_HEADER_MAGIC 0x1763247F
|
||||
|
||||
typedef struct {
|
||||
size_t size;
|
||||
uint32_t magic;
|
||||
} heap_header_t;
|
||||
|
||||
void *__wrap_malloc(size_t size)
|
||||
{
|
||||
// fc_heap_alloc guarantees zero filled memory
|
||||
auto *ret = (heap_header_t *)fc_heap_alloc(size+sizeof(heap_header_t));
|
||||
if (ret == nullptr) {
|
||||
return nullptr;
|
||||
}
|
||||
ret->size = size;
|
||||
ret->magic = HEAP_HEADER_MAGIC;
|
||||
return (void*)(ret+1);
|
||||
}
|
||||
|
||||
void __wrap_free(void *ptr)
|
||||
{
|
||||
if (ptr == nullptr) {
|
||||
return;
|
||||
}
|
||||
auto *h = ((heap_header_t *)ptr)-1;
|
||||
if (h->magic != HEAP_HEADER_MAGIC) {
|
||||
AP_HAL::panic("free: bad memory header 0x%x", unsigned(h->magic));
|
||||
}
|
||||
fc_heap_free((void*)h);
|
||||
}
|
||||
|
||||
void *__wrap_calloc(size_t nmemb, size_t size)
|
||||
{
|
||||
return __wrap_malloc(nmemb*size);
|
||||
}
|
||||
|
||||
void *__wrap_realloc(void *ptr, size_t size)
|
||||
{
|
||||
if (size == 0) {
|
||||
// a free
|
||||
__wrap_free(ptr);
|
||||
return nullptr;
|
||||
}
|
||||
if (ptr == nullptr) {
|
||||
// new allocation
|
||||
return __wrap_malloc(size);
|
||||
}
|
||||
|
||||
auto *h = ((heap_header_t *)ptr)-1;
|
||||
if (h->magic != HEAP_HEADER_MAGIC) {
|
||||
AP_HAL::panic("realloc: bad memory header 0x%x", unsigned(h->magic));
|
||||
}
|
||||
|
||||
void *ret = __wrap_malloc(size);
|
||||
if (ret == nullptr) {
|
||||
return nullptr;
|
||||
}
|
||||
const size_t copy_size = size > h->size? h->size : size;
|
||||
memcpy(ret, ptr, copy_size);
|
||||
__wrap_free(ptr);
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
210
libraries/AP_HAL_QURT/replace.cpp
Normal file
210
libraries/AP_HAL_QURT/replace.cpp
Normal file
@ -0,0 +1,210 @@
|
||||
#include <stdio.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <unistd.h>
|
||||
#include <sys/types.h>
|
||||
#include <sys/stat.h>
|
||||
#include <fcntl.h>
|
||||
#include <dirent.h>
|
||||
#include <pthread.h>
|
||||
#include "replace.h"
|
||||
#include "interface.h"
|
||||
#include "ap_host/src/protocol.h"
|
||||
|
||||
extern "C" {
|
||||
|
||||
// this is not declared in qurt headers
|
||||
void HAP_debug(const char *msg, int level, const char *filename, int line);
|
||||
|
||||
void HAP_printf(const char *file, int line, const char *format, ...)
|
||||
{
|
||||
va_list ap;
|
||||
char buf[300];
|
||||
|
||||
va_start(ap, format);
|
||||
vsnprintf(buf, sizeof(buf), format, ap);
|
||||
va_end(ap);
|
||||
HAP_debug(buf, 0, file, line);
|
||||
//usleep(20000);
|
||||
}
|
||||
|
||||
/**
|
||||
QURT doesn't have strnlen()
|
||||
**/
|
||||
size_t strnlen(const char *s, size_t max)
|
||||
{
|
||||
size_t len;
|
||||
|
||||
for (len = 0; len < max; len++) {
|
||||
if (s[len] == '\0') {
|
||||
break;
|
||||
}
|
||||
}
|
||||
return len;
|
||||
}
|
||||
|
||||
int vasprintf(char **ptr, const char *format, va_list ap)
|
||||
{
|
||||
int ret;
|
||||
va_list ap2;
|
||||
|
||||
va_copy(ap2, ap);
|
||||
ret = vsnprintf(nullptr, 0, format, ap2);
|
||||
va_end(ap2);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
(*ptr) = (char *)malloc(ret+1);
|
||||
if (!*ptr) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
va_copy(ap2, ap);
|
||||
ret = vsnprintf(*ptr, ret+1, format, ap2);
|
||||
va_end(ap2);
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
int asprintf(char **ptr, const char *format, ...)
|
||||
{
|
||||
va_list ap;
|
||||
int ret;
|
||||
|
||||
*ptr = nullptr;
|
||||
va_start(ap, format);
|
||||
ret = vasprintf(ptr, format, ap);
|
||||
va_end(ap);
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
void *memmem(const void *haystack, size_t haystacklen,
|
||||
const void *needle, size_t needlelen)
|
||||
{
|
||||
if (needlelen == 0) {
|
||||
return const_cast<void*>(haystack);
|
||||
}
|
||||
while (haystacklen >= needlelen) {
|
||||
char *p = (char *)memchr(haystack, *(const char *)needle,
|
||||
haystacklen-(needlelen-1));
|
||||
if (!p) {
|
||||
return NULL;
|
||||
}
|
||||
if (memcmp(p, needle, needlelen) == 0) {
|
||||
return p;
|
||||
}
|
||||
haystack = p+1;
|
||||
haystacklen -= (p - (const char *)haystack) + 1;
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
|
||||
char *strndup(const char *s, size_t n)
|
||||
{
|
||||
char *ret;
|
||||
|
||||
n = strnlen(s, n);
|
||||
ret = (char*)malloc(n+1);
|
||||
if (!ret) {
|
||||
return NULL;
|
||||
}
|
||||
memcpy(ret, s, n);
|
||||
ret[n] = 0;
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
int pthread_cond_init(pthread_cond_t *cond, pthread_condattr_t *attr)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
// INVESTIGATE: What is this needed on QURT?
|
||||
int apfs_rename(const char *oldpath, const char *newpath)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
// INVESTIGATE: Seems important :-)
|
||||
int ArduPilot_main(int argc, const char *argv[])
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
char *__wrap_strdup(const char *s);
|
||||
}
|
||||
|
||||
extern "C" int qurt_ardupilot_main(int argc, char* const argv[]);
|
||||
|
||||
int slpi_link_client_init(void)
|
||||
{
|
||||
HAP_PRINTF("About to call qurt_ardupilot_main %p", &qurt_ardupilot_main);
|
||||
|
||||
qurt_ardupilot_main(0, NULL);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
typedef void (*mavlink_data_callback_t)(const struct qurt_mavlink_msg *msg, void* p);
|
||||
|
||||
static mavlink_data_callback_t mav_cb;
|
||||
static void *mav_cb_ptr;
|
||||
static uint32_t expected_seq;
|
||||
|
||||
void register_mavlink_data_callback(mavlink_data_callback_t func, void *p)
|
||||
{
|
||||
mav_cb = func;
|
||||
mav_cb_ptr = p;
|
||||
}
|
||||
|
||||
int slpi_link_client_receive(const uint8_t *data, int data_len_in_bytes)
|
||||
{
|
||||
if (data_len_in_bytes < QURT_MAVLINK_MSG_HEADER_LEN) {
|
||||
return 0;
|
||||
}
|
||||
const auto *msg = (struct qurt_mavlink_msg *)data;
|
||||
if (msg->data_length + QURT_MAVLINK_MSG_HEADER_LEN != data_len_in_bytes) {
|
||||
return 0;
|
||||
}
|
||||
if (msg->seq != expected_seq) {
|
||||
HAP_PRINTF("Bad sequence %u %u", msg->seq, expected_seq);
|
||||
}
|
||||
expected_seq = msg->seq + 1;
|
||||
|
||||
switch (msg->msg_id) {
|
||||
case QURT_MSG_ID_MAVLINK_MSG: {
|
||||
if (mav_cb) {
|
||||
mav_cb(msg, mav_cb_ptr);
|
||||
}
|
||||
break;
|
||||
}
|
||||
default:
|
||||
HAP_PRINTF("Got unknown message id %d", msg->msg_id);
|
||||
break;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int __wrap_printf(const char *fmt, ...)
|
||||
{
|
||||
va_list ap;
|
||||
|
||||
char buf[300];
|
||||
va_start(ap, fmt);
|
||||
vsnprintf(buf, sizeof(buf), fmt, ap);
|
||||
va_end(ap);
|
||||
HAP_PRINTF(buf);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*
|
||||
free() on strdup return on QURT causes a memory fault
|
||||
*/
|
||||
char *__wrap_strdup(const char *s)
|
||||
{
|
||||
return strndup(s, strlen(s));
|
||||
}
|
49
libraries/AP_HAL_QURT/replace.h
Normal file
49
libraries/AP_HAL_QURT/replace.h
Normal file
@ -0,0 +1,49 @@
|
||||
#pragma once
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <stdint.h>
|
||||
#include <types.h>
|
||||
#include <sys/types.h>
|
||||
#include <unistd.h>
|
||||
#include <dirent.h>
|
||||
|
||||
#include <types.h>
|
||||
#include <dirent.h>
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/*
|
||||
work around broken headers
|
||||
*/
|
||||
size_t strnlen(const char *s, size_t maxlen);
|
||||
char *strndup(const char *s, size_t n);
|
||||
int asprintf(char **, const char *, ...);
|
||||
off_t lseek(int, off_t, int);
|
||||
DIR *opendir (const char *);
|
||||
int unlink(const char *pathname);
|
||||
void *memmem(const void *haystack, size_t haystacklen,
|
||||
const void *needle, size_t needlelen);
|
||||
|
||||
//typedef int32_t pid_t;
|
||||
pid_t getpid (void);
|
||||
|
||||
void HAP_printf(const char *file, int line, const char *fmt, ...);
|
||||
|
||||
int __wrap_printf(const char *fmt, ...);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#define HAP_PRINTF(...) HAP_printf(__FILE__, __LINE__, __VA_ARGS__)
|
||||
|
||||
extern volatile int _last_dsp_line;
|
||||
extern volatile const char *_last_dsp_file;
|
||||
extern volatile uint32_t _last_counter;
|
||||
|
||||
#define HAP_LINE() do { _last_dsp_line = __LINE__; _last_dsp_file = __FILE__; _last_counter++; } while (0)
|
||||
|
||||
// missing defines from math.h
|
||||
#define M_SQRT1_2 0.70710678118654752440
|
65
libraries/AP_HAL_QURT/system.cpp
Normal file
65
libraries/AP_HAL_QURT/system.cpp
Normal file
@ -0,0 +1,65 @@
|
||||
#include <stdarg.h>
|
||||
#include <stdio.h>
|
||||
#include <sys/time.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <AP_HAL/system.h>
|
||||
|
||||
#include "replace.h"
|
||||
#include <fenv.h>
|
||||
#include <AP_Math/div1000.h>
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
namespace AP_HAL
|
||||
{
|
||||
|
||||
static struct {
|
||||
uint64_t start_time;
|
||||
} state;
|
||||
|
||||
void init()
|
||||
{
|
||||
state.start_time = micros64();
|
||||
// we don't want exceptions in flight code. That is the job of SITL
|
||||
feclearexcept(FE_OVERFLOW | FE_DIVBYZERO | FE_INVALID);
|
||||
}
|
||||
|
||||
void panic(const char *errormsg, ...)
|
||||
{
|
||||
char buf[200];
|
||||
va_list ap;
|
||||
va_start(ap, errormsg);
|
||||
vsnprintf(buf, sizeof(buf), errormsg, ap);
|
||||
va_end(ap);
|
||||
HAP_PRINTF("PANIC: %s", buf);
|
||||
qurt_timer_sleep(100000);
|
||||
exit(1);
|
||||
}
|
||||
|
||||
uint32_t micros()
|
||||
{
|
||||
return micros64() & 0xFFFFFFFF;
|
||||
}
|
||||
|
||||
uint32_t millis()
|
||||
{
|
||||
return millis64() & 0xFFFFFFFF;
|
||||
}
|
||||
|
||||
uint64_t micros64()
|
||||
{
|
||||
struct timespec ts;
|
||||
clock_gettime(CLOCK_MONOTONIC, &ts);
|
||||
uint64_t ret = ts.tv_sec*1000*1000ULL + uint64_div1000(ts.tv_nsec);
|
||||
ret -= state.start_time;
|
||||
return ret;
|
||||
}
|
||||
|
||||
uint64_t millis64()
|
||||
{
|
||||
return uint64_div1000(micros64());
|
||||
}
|
||||
|
||||
} // namespace AP_HAL
|
Loading…
Reference in New Issue
Block a user