Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-03 22:48:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

HAL_PX4: moved to common DeviceBus class for thread management in I2C and SPI

Browse Source
This commit is contained in:
Andrew Tridgell 2016-11-04 09:48:36 +11:00
parent 972f85c490
commit 1bb450c722
7 changed files with 209 additions and 261 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

116
libraries/AP_HAL_PX4/Device.cpp Normal file
View File

@ -0,0 +1,116 @@
/*
* 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 "Device.h"
#include <arch/board/board.h>
#include "board_config.h"
#include <stdio.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_HAL/utility/OwnPtr.h>
#include "Scheduler.h"
#include "Semaphores.h"
extern bool _px4_thread_should_exit;
namespace PX4 {
static const AP_HAL::HAL &hal = AP_HAL::get_HAL();
/*
per-bus callback thread
*/
void *DeviceBus::bus_thread(void *arg)
{
struct DeviceBus *binfo = (struct DeviceBus *)arg;
while (!_px4_thread_should_exit) {
uint64_t now = AP_HAL::micros64();
DeviceBus::callback_info *callback;
// find a callback to run
for (callback = binfo->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
if (binfo->semaphore.take(0)) {
callback->cb();
binfo->semaphore.give();
}
}
}
// work out when next loop is needed
uint64_t next_needed = 0;
now = AP_HAL::micros64();
for (callback = binfo->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 400usec, so one thread doesn't
// completely dominate the CPU
if (delay < 400) {
delay = 400;
}
hal.scheduler->delay_microseconds(delay);
}
return nullptr;
}
AP_HAL::Device::PeriodicHandle DeviceBus::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb)
{
if (!thread_started) {
thread_started = true;
pthread_attr_t thread_attr;
struct sched_param param;
pthread_attr_init(&thread_attr);
pthread_attr_setstacksize(&thread_attr, 1024);
param.sched_priority = thread_priority;
(void)pthread_attr_setschedparam(&thread_attr, &param);
pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO);
pthread_create(&thread_ctx, &thread_attr, &DeviceBus::bus_thread, this);
}
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;
}
}

47
libraries/AP_HAL_PX4/Device.h Normal file
View File

@ -0,0 +1,47 @@
/*
* 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 "Scheduler.h"
namespace PX4 {
class DeviceBus {
public:
DeviceBus(uint8_t _thread_priority = APM_I2C_PRIORITY) :
thread_priority(_thread_priority) {}
struct DeviceBus *next;
Semaphore semaphore;
AP_HAL::Device::PeriodicHandle register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb);
static void *bus_thread(void *arg);
private:
struct callback_info {
struct callback_info *next;
AP_HAL::Device::PeriodicCb cb;
uint32_t period_usec;
uint64_t next_usec;
} *callbacks;
uint8_t thread_priority;
pthread_t thread_ctx;
bool thread_started;
};
}

104
libraries/AP_HAL_PX4/I2CDevice.cpp
View File

@ -19,8 +19,6 @@
#include "Util.h"
#include "Scheduler.h"
extern bool _px4_thread_should_exit;
namespace PX4 {
uint8_t PX4::PX4_I2C::instance;
@ -82,8 +80,6 @@ bool PX4_I2C::do_transfer(uint8_t address, const uint8_t *send, uint32_t send_le
return ret;
}
PX4::I2CDevice::bus_info PX4::I2CDevice::businfo[num_buses];
I2CDevice::I2CDevice(uint8_t bus, uint8_t address) :
_busnum(bus),
_px4dev(_busnum),
@ -107,100 +103,17 @@ bool I2CDevice::read_registers_multiple(uint8_t first_reg, uint8_t *recv,
return false;
}
/*
per-bus i2c callback thread
*/
void *I2CDevice::i2c_thread(void *arg)
{
struct bus_info *binfo = (struct bus_info *)arg;
while (!_px4_thread_should_exit) {
uint64_t now = AP_HAL::micros64();
uint64_t next_needed = 0;
struct callback_info *callback;
// find a callback to run
for (callback = binfo->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
if (binfo->semaphore.take(0)) {
callback->cb();
binfo->semaphore.give();
}
}
if (next_needed == 0 ||
callback->next_usec < next_needed) {
next_needed = callback->next_usec;
}
}
// delay for at most 50ms, to handle newly added callbacks
now = AP_HAL::micros64();
uint32_t delay = 50000;
if (next_needed > now && next_needed - now < delay) {
delay = next_needed - now;
}
hal.scheduler->delay_microseconds(delay);
}
return nullptr;
}
/*
find or create thread for this bus
*/
I2CDevice::bus_info *I2CDevice::find_thread(void)
{
if (_busnum >= num_buses) {
return nullptr;
}
struct bus_info &binfo = businfo[_busnum];
if (binfo.thread_started) {
return &binfo;
}
binfo.bus = _busnum;
binfo.thread_started = true;
pthread_attr_t thread_attr;
struct sched_param param;
pthread_attr_init(&thread_attr);
pthread_attr_setstacksize(&thread_attr, 1024);
param.sched_priority = APM_I2C_PRIORITY;
(void)pthread_attr_setschedparam(&thread_attr, &param);
pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO);
pthread_create(&binfo.thread_ctx, &thread_attr, &I2CDevice::i2c_thread, &binfo);
return &binfo;
}
/*
register a periodic callback
*/
AP_HAL::Device::PeriodicHandle I2CDevice::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb)
{
struct bus_info *binfo = find_thread();
if (binfo == nullptr) {
if (_busnum >= num_buses) {
return nullptr;
}
struct callback_info *callback = new 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 = binfo->callbacks;
binfo->callbacks = callback;
return callback;
struct DeviceBus &binfo = businfo[_busnum];
return binfo.register_periodic_callback(period_usec, cb);
}
@ -209,17 +122,6 @@ AP_HAL::Device::PeriodicHandle I2CDevice::register_periodic_callback(uint32_t pe
*/
bool I2CDevice::adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec)
{
struct bus_info *binfo = find_thread();
if (binfo == nullptr) {
return nullptr;
}
for (struct callback_info *callback = binfo->callbacks; callback; callback = callback->next) {
if (h == (AP_HAL::Device::PeriodicHandle)callback) {
callback->period_usec = period_usec;
return true;
}
}
return false;
}

23
libraries/AP_HAL_PX4/I2CDevice.h
View File

@ -23,6 +23,7 @@
#include <AP_HAL/utility/OwnPtr.h>
#include "Semaphores.h"
#include "I2CWrapper.h"
#include "Device.h"
namespace PX4 {
@ -64,29 +65,9 @@ public:
return &businfo[_busnum<num_buses?_busnum:0].semaphore;
}
struct callback_info {
struct callback_info *next;
AP_HAL::Device::PeriodicCb cb;
uint32_t period_usec;
uint64_t next_usec;
};
struct bus_info {
struct callback_info *callbacks;
Semaphore semaphore;
pthread_t thread_ctx;
bool thread_started;
uint8_t bus;
};
private:
static const uint8_t num_buses = 2;
static struct bus_info businfo[num_buses];
// find or create thread for this bus
struct bus_info *find_thread(void);
static void *i2c_thread(void *arg);
DeviceBus businfo[num_buses];
uint8_t _busnum;
PX4_I2C _px4dev;

109
libraries/AP_HAL_PX4/SPIDevice.cpp
View File

@ -23,8 +23,6 @@
#include "Scheduler.h"
#include "Semaphores.h"
extern bool _px4_thread_should_exit;
namespace PX4 {
static const AP_HAL::HAL &hal = AP_HAL::get_HAL();
@ -50,7 +48,6 @@ SPIDesc SPIDeviceManager::device_table[] = {
SPIDevice::SPIDevice(SPIBus &_bus, SPIDesc &_device_desc)
: bus(_bus)
, device_desc(_device_desc)
, px4dev(device_desc.bus, device_desc.device, device_desc.mode, device_desc.lowspeed)
{
}
@ -62,15 +59,30 @@ bool SPIDevice::set_speed(AP_HAL::Device::Speed speed)
{
switch (speed) {
case AP_HAL::Device::SPEED_HIGH:
px4dev.set_speed(device_desc.highspeed);
frequency = device_desc.highspeed;
break;
case AP_HAL::Device::SPEED_LOW:
px4dev.set_speed(device_desc.lowspeed);
frequency = device_desc.lowspeed;
break;
}
return true;
}
/*
low level transfer function
*/
void SPIDevice::do_transfer(uint8_t *send, uint8_t *recv, uint32_t len)
{
irqstate_t state = irqsave();
SPI_SETFREQUENCY(bus.dev, frequency);
SPI_SETMODE(bus.dev, device_desc.mode);
SPI_SETBITS(bus.dev, 8);
SPI_SELECT(bus.dev, device_desc.device, true);
SPI_EXCHANGE(bus.dev, send, recv, len);
SPI_SELECT(bus.dev, device_desc.device, false);
irqrestore(state);
}
bool SPIDevice::transfer(const uint8_t *send, uint32_t send_len,
uint8_t *recv, uint32_t recv_len)
{
@ -81,22 +93,20 @@ bool SPIDevice::transfer(const uint8_t *send, uint32_t send_len,
if (recv_len > 0) {
memset(&buf[send_len], 0, recv_len);
}
bool ret = px4dev.do_transfer(buf, buf, send_len+recv_len);
if (recv_len > 0 && ret) {
do_transfer(buf, buf, send_len+recv_len);
if (recv_len > 0) {
memcpy(recv, &buf[send_len], recv_len);
}
return ret;
return true;
}
bool SPIDevice::transfer_fullduplex(const uint8_t *send, uint8_t *recv, uint32_t len)
{
uint8_t buf[len];
memcpy(buf, send, len);
bool ret = px4dev.do_transfer(buf, buf, len);
if (ret) {
memcpy(recv, buf, len);
}
return ret;
do_transfer(buf, buf, len);
memcpy(recv, buf, len);
return true;
}
AP_HAL::Semaphore *SPIDevice::get_semaphore()
@ -104,76 +114,10 @@ AP_HAL::Semaphore *SPIDevice::get_semaphore()
return &bus.semaphore;
}
/*
per-bus spi callback thread
*/
void *SPIDevice::spi_thread(void *arg)
{
struct SPIBus *binfo = (struct SPIBus *)arg;
while (!_px4_thread_should_exit) {
uint64_t now = AP_HAL::micros64();
uint64_t next_needed = 0;
SPIBus::callback_info *callback;
// find a callback to run
for (callback = binfo->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
if (binfo->semaphore.take(0)) {
callback->cb();
binfo->semaphore.give();
}
}
if (next_needed == 0 ||
callback->next_usec < next_needed) {
next_needed = callback->next_usec;
}
}
// delay for at most 50ms, to handle newly added callbacks
now = AP_HAL::micros64();
uint32_t delay = 50000;
if (next_needed > now && next_needed - now < delay) {
delay = next_needed - now;
}
hal.scheduler->delay_microseconds(delay);
}
return nullptr;
}
AP_HAL::Device::PeriodicHandle SPIDevice::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb)
{
if (!bus.thread_started) {
bus.thread_started = true;
pthread_attr_t thread_attr;
struct sched_param param;
pthread_attr_init(&thread_attr);
pthread_attr_setstacksize(&thread_attr, 1024);
param.sched_priority = APM_SPI_PRIORITY;
(void)pthread_attr_setschedparam(&thread_attr, &param);
pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO);
pthread_create(&bus.thread_ctx, &thread_attr, &SPIDevice::spi_thread, &bus);
}
SPIBus::callback_info *callback = new SPIBus::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 = bus.callbacks;
bus.callbacks = callback;
return callback;
return bus.register_periodic_callback(period_usec, cb);
}
bool SPIDevice::adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec)
@ -202,8 +146,8 @@ SPIDeviceManager::get_device(const char *name)
SPIDesc &desc = device_table[i];
// find the bus
struct SPIBus *busp;
for (busp = buses; busp; busp = busp->next) {
SPIBus *busp;
for (busp = buses; busp; busp = (SPIBus *)busp->next) {
if (busp->bus == desc.bus) {
break;
}
@ -216,6 +160,7 @@ SPIDeviceManager::get_device(const char *name)
}
busp->next = buses;
busp->bus = desc.bus;
busp->dev = up_spiinitialize(desc.bus);
buses = busp;
}

30
libraries/AP_HAL_PX4/SPIDevice.h
View File

@ -19,23 +19,18 @@
#include <AP_HAL/SPIDevice.h>
#include <drivers/device/spi.h>
#include "Semaphores.h"
#include "SPIWrapper.h"
#include "Device.h"
#include "Scheduler.h"
namespace PX4 {
class SPIDesc;
struct SPIBus {
struct SPIBus *next;
struct callback_info {
struct callback_info *next;
AP_HAL::Device::PeriodicCb cb;
uint32_t period_usec;
uint64_t next_usec;
} *callbacks;
Semaphore semaphore;
pthread_t thread_ctx;
bool thread_started;
class SPIBus : public DeviceBus {
public:
SPIBus(void) :
DeviceBus(APM_SPI_PRIORITY) {}
struct spi_dev_s *dev;
uint8_t bus;
};
@ -68,6 +63,9 @@ public:
/* See AP_HAL::Device::set_speed() */
bool set_speed(AP_HAL::Device::Speed speed) override;
// low level transfer function
void do_transfer(uint8_t *send, uint8_t *recv, uint32_t len);
/* See AP_HAL::Device::transfer() */
bool transfer(const uint8_t *send, uint32_t send_len,
uint8_t *recv, uint32_t recv_len) override;
@ -89,8 +87,8 @@ public:
private:
SPIBus &bus;
SPIDesc &device_desc;
PX4_SPI px4dev;
uint32_t frequency;
static void *spi_thread(void *arg);
};
@ -107,7 +105,7 @@ public:
private:
static SPIDesc device_table[];
struct SPIBus *buses;
SPIBus *buses;
};
}

41
libraries/AP_HAL_PX4/SPIWrapper.h
View File

@ -1,41 +0,0 @@
#include <AP_HAL/AP_HAL.h>
#include <arch/board/board.h>
#include "board_config.h"
#include <drivers/device/spi.h>
#include "AP_HAL_PX4.h"
extern const AP_HAL::HAL& hal;
/*
wrapper class for SPI to expose protected functions from PX4NuttX
*/
class PX4::PX4_SPI {
public:
PX4_SPI(uint8_t bus, enum spi_dev_e device, enum spi_mode_e mode, uint32_t frequency) :
_bus(bus), _device(device), _mode(mode), _frequency(frequency) {
_dev = up_spiinitialize(_bus);
SPI_SELECT(_dev, _device, false);
}
bool do_transfer(uint8_t *send, uint8_t *recv, uint32_t len) {
irqstate_t state = irqsave();
SPI_SETFREQUENCY(_dev, _frequency);
SPI_SETMODE(_dev, _mode);
SPI_SETBITS(_dev, 8);
SPI_SELECT(_dev, _device, true);
SPI_EXCHANGE(_dev, send, recv, len);
SPI_SELECT(_dev, _device, false);
irqrestore(state);
return true;
}
void set_speed(uint32_t speed_hz) {
_frequency = speed_hz;
}
private:
struct spi_dev_s *_dev;
uint8_t _bus;
enum spi_dev_e _device;
enum spi_mode_e _mode;
uint32_t _frequency;
};