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

HAL_Linux: convert RASPilot drivers to thread per bus

This commit is contained in:
Andrew Tridgell 2016-11-18 12:05:30 +11:00 committed by Lucas De Marchi
parent 6129b1abb6
commit 6f82ec0642
8 changed files with 44 additions and 106 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/AP_HAL_Linux/AnalogIn_Raspilot.cpp
View File

@ -81,21 +81,11 @@ void AnalogIn_Raspilot::init()
return;
}
hal.scheduler->suspend_timer_procs();
hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AnalogIn_Raspilot::_update, void));
hal.scheduler->resume_timer_procs();
_dev->register_periodic_callback(100000, FUNCTOR_BIND_MEMBER(&AnalogIn_Raspilot::_update, bool));
}
void AnalogIn_Raspilot::_update()
bool AnalogIn_Raspilot::_update()
{
if (AP_HAL::micros() - _last_update_timestamp < 100000) {
return;
}
if (!_dev->get_semaphore()->take_nonblocking()) {
return;
}
struct IOPacket tx = { }, rx = { };
uint16_t count = RASPILOT_ADC_MAX_CHANNELS;
tx.count_code = count | PKT_CODE_READ;
@ -119,8 +109,6 @@ void AnalogIn_Raspilot::_update()
/* get reg4 data from raspilotio */
_dev->transfer((uint8_t *)&tx, sizeof(tx), (uint8_t *)&rx, sizeof(rx));
_dev->get_semaphore()->give();
for (int16_t i = 0; i < RASPILOT_ADC_MAX_CHANNELS; i++) {
for (int16_t j=0; j < RASPILOT_ADC_MAX_CHANNELS; j++) {
AnalogSource_Raspilot *source = _channels[j];
@ -130,6 +118,5 @@ void AnalogIn_Raspilot::_update()
}
}
}
_last_update_timestamp = AP_HAL::micros();
return true;
}

3
libraries/AP_HAL_Linux/AnalogIn_Raspilot.h
View File

@ -35,13 +35,12 @@ public:
float board_voltage(void);
protected:
void _update();
bool _update();
AP_HAL::AnalogSource *_vcc_pin_analog_source;
AnalogSource_Raspilot *_channels[RASPILOT_ADC_MAX_CHANNELS];
AP_HAL::OwnPtr<AP_HAL::SPIDevice> _dev;
uint32_t _last_update_timestamp;
uint8_t _channels_number;
};

17
libraries/AP_HAL_Linux/RCInput_Raspilot.cpp
View File

@ -24,22 +24,11 @@ void RCInput_Raspilot::init()
_dev = hal.spi->get_device("raspio");
// start the timer process to read samples
hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&RCInput_Raspilot::_poll_data, void));
_dev->register_periodic_callback(10000, FUNCTOR_BIND_MEMBER(&RCInput_Raspilot::_poll_data, bool));
}
void RCInput_Raspilot::_poll_data(void)
bool RCInput_Raspilot::_poll_data(void)
{
// Throttle read rate to 100hz maximum.
if (AP_HAL::micros() - _last_timer < 10000) {
return;
}
_last_timer = AP_HAL::micros();
if (!_dev->get_semaphore()->take_nonblocking()) {
return;
}
struct IOPacket _dma_packet_tx, _dma_packet_rx;
uint16_t count = LINUX_RC_INPUT_NUM_CHANNELS;
_dma_packet_tx.count_code = count | PKT_CODE_READ;
@ -64,7 +53,7 @@ void RCInput_Raspilot::_poll_data(void)
_update_periods(&_dma_packet_rx.regs[6], (uint8_t)num_values);
}
_dev->get_semaphore()->give();
return true;
}
#endif // CONFIG_HAL_BOARD_SUBTYPE

4
libraries/AP_HAL_Linux/RCInput_Raspilot.h
View File

@ -13,11 +13,9 @@ public:
void init();
private:
uint32_t _last_timer;
AP_HAL::OwnPtr<AP_HAL::SPIDevice> _dev;
void _poll_data(void);
bool _poll_data(void);
};
}

47
libraries/AP_HAL_Linux/RCOutput_Raspilot.cpp
View File

@ -27,29 +27,12 @@ void RCOutput_Raspilot::init()
{
_dev = hal.spi->get_device("raspio");
hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&RCOutput_Raspilot::_update, void));
_dev->register_periodic_callback(10000, FUNCTOR_BIND_MEMBER(&RCOutput_Raspilot::_update, bool));
}
void RCOutput_Raspilot::set_freq(uint32_t chmask, uint16_t freq_hz)
{
if (!_dev->get_semaphore()->take(10)) {
return;
}
struct IOPacket _dma_packet_tx, _dma_packet_rx;
uint16_t count = 1;
_dma_packet_tx.count_code = count | PKT_CODE_WRITE;
_dma_packet_tx.page = 50;
_dma_packet_tx.offset = 3;
_dma_packet_tx.regs[0] = freq_hz;
_dma_packet_tx.crc = 0;
_dma_packet_tx.crc = crc_packet(&_dma_packet_tx);
_dev->transfer((uint8_t *)&_dma_packet_tx, sizeof(_dma_packet_tx),
(uint8_t *)&_dma_packet_rx, sizeof(_dma_packet_rx));
_frequency = freq_hz;
_dev->get_semaphore()->give();
_new_frequency = freq_hz;
}
uint16_t RCOutput_Raspilot::get_freq(uint8_t ch)
@ -91,18 +74,27 @@ void RCOutput_Raspilot::read(uint16_t* period_us, uint8_t len)
period_us[i] = read(0 + i);
}
void RCOutput_Raspilot::_update(void)
bool RCOutput_Raspilot::_update(void)
{
int i;
if (_corked || AP_HAL::micros() - _last_update_timestamp < 10000) {
return;
if (_corked) {
return true;
}
_last_update_timestamp = AP_HAL::micros();
if (!_dev->get_semaphore()->take_nonblocking()) {
return;
if (_new_frequency) {
_frequency = _new_frequency;
_new_frequency = 0;
struct IOPacket _dma_packet_tx, _dma_packet_rx;
uint16_t count = 1;
_dma_packet_tx.count_code = count | PKT_CODE_WRITE;
_dma_packet_tx.page = 50;
_dma_packet_tx.offset = 3;
_dma_packet_tx.regs[0] = _frequency;
_dma_packet_tx.crc = 0;
_dma_packet_tx.crc = crc_packet(&_dma_packet_tx);
_dev->transfer((uint8_t *)&_dma_packet_tx, sizeof(_dma_packet_tx),
(uint8_t *)&_dma_packet_rx, sizeof(_dma_packet_rx));
}
struct IOPacket _dma_packet_tx, _dma_packet_rx;
@ -137,8 +129,7 @@ void RCOutput_Raspilot::_update(void)
_dma_packet_tx.crc = crc_packet(&_dma_packet_tx);
_dev->transfer((uint8_t *)&_dma_packet_tx, sizeof(_dma_packet_tx),
(uint8_t *)&_dma_packet_rx, sizeof(_dma_packet_rx));
_dev->get_semaphore()->give();
return true;
}
void RCOutput_Raspilot::cork(void)

3
libraries/AP_HAL_Linux/RCOutput_Raspilot.h
View File

@ -20,12 +20,13 @@ public:
private:
void reset();
void _update(void);
bool _update(void);
AP_HAL::OwnPtr<AP_HAL::SPIDevice> _dev;
uint32_t _last_update_timestamp;
uint16_t _frequency;
uint16_t _new_frequency;
uint16_t _period_us[8];
bool _corked;
};

51
libraries/AP_HAL_Linux/RPIOUARTDriver.cpp
View File

@ -8,8 +8,6 @@
#include "px4io_protocol.h"
#define RPIOUART_POLL_TIME_INTERVAL 10000
extern const AP_HAL::HAL& hal;
#define RPIOUART_DEBUG 0
@ -29,23 +27,12 @@ using namespace Linux;
RPIOUARTDriver::RPIOUARTDriver() :
UARTDriver(false),
_dev(nullptr),
_last_update_timestamp(0),
_external(false),
_need_set_baud(false),
_baudrate(0)
{
}
bool RPIOUARTDriver::sem_take_nonblocking()
{
return _dev->get_semaphore()->take_nonblocking();
}
void RPIOUARTDriver::sem_give()
{
_dev->get_semaphore()->give();
}
bool RPIOUARTDriver::isExternal()
{
return _external;
@ -76,18 +63,20 @@ void RPIOUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
_readbuf.set_size(rxS);
_writebuf.set_size(txS);
_dev = hal.spi->get_device("raspio");
if (!_registered_callback) {
_dev = hal.spi->get_device("raspio");
_registered_callback = true;
_dev->register_periodic_callback(100000, FUNCTOR_BIND_MEMBER(&RPIOUARTDriver::_bus_timer, bool));
}
/* set baudrate */
_baudrate = b;
_need_set_baud = true;
while (_need_set_baud) {
hal.scheduler->delay(1);
}
if (_writebuf.get_size() && _readbuf.get_size()) {
_initialised = true;
}
}
int RPIOUARTDriver::_write_fd(const uint8_t *buf, uint16_t n)
@ -114,14 +103,13 @@ void RPIOUARTDriver::_timer_tick(void)
UARTDriver::_timer_tick();
return;
}
}
bool RPIOUARTDriver::_bus_timer(void)
{
/* set the baudrate of raspilotio */
if (_need_set_baud) {
if (_baudrate != 0) {
if (!_dev->get_semaphore()->take_nonblocking()) {
return;
}
struct IOPacket _packet_tx = {0}, _packet_rx = {0};
_packet_tx.count_code = 2 | PKT_CODE_WRITE;
@ -134,33 +122,23 @@ void RPIOUARTDriver::_timer_tick(void)
(uint8_t *)&_packet_rx, sizeof(_packet_rx));
hal.scheduler->delay(1);
_dev->get_semaphore()->give();
}
_need_set_baud = false;
}
/* finish set */
if (!_initialised) return;
/* lower the update rate */
if (AP_HAL::micros() - _last_update_timestamp < RPIOUART_POLL_TIME_INTERVAL) {
return;
if (!_initialised) {
return true;
}
_in_timer = true;
if (!_dev->get_semaphore()->take_nonblocking()) {
return;
}
struct IOPacket _packet_tx = {0}, _packet_rx = {0};
/* get write_buf bytes */
uint32_t max_size = MIN((uint32_t) PKT_MAX_REGS * 2,
_baudrate / 10 / (1000000 / RPIOUART_POLL_TIME_INTERVAL));
_baudrate / 10 / (1000000 / 100000));
uint32_t n = MIN(_writebuf.available(), max_size);
_writebuf.read(&((uint8_t *)_packet_tx.regs)[0], n);
@ -185,9 +163,6 @@ void RPIOUARTDriver::_timer_tick(void)
hal.scheduler->delay_microseconds(100);
/* release sem */
_dev->get_semaphore()->give();
if (_packet_rx.page == PX4IO_PAGE_UART_BUFFER) {
/* add bytes to read buf */
max_size = MIN(_packet_rx.offset, PKT_MAX_REGS * 2);
@ -198,5 +173,5 @@ void RPIOUARTDriver::_timer_tick(void)
_in_timer = false;
_last_update_timestamp = AP_HAL::micros();
return true;
}

6
libraries/AP_HAL_Linux/RPIOUARTDriver.h
View File

@ -26,14 +26,12 @@ protected:
private:
bool _in_timer;
bool sem_take_nonblocking();
void sem_give();
bool _bus_timer(void);
AP_HAL::OwnPtr<AP_HAL::SPIDevice> _dev;
uint32_t _last_update_timestamp;
bool _external;
bool _registered_callback;
bool _need_set_baud;
uint32_t _baudrate;