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

AP_HAL_Linux: SPIUARTDriver: add some cosmetic changes

Browse Source 
Mainly changes to follow the coding style and reduce the complexity a
little bit.
This commit is contained in:
Lucas De Marchi 2016-07-22 02:06:03 -03:00
parent e3c0476b8a
commit 065592e7df
2 changed files with 71 additions and 96 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

160
libraries/AP_HAL_Linux/SPIUARTDriver.cpp
View File

@ -1,18 +1,16 @@
#include "SPIUARTDriver.h" #include "SPIUARTDriver.h"
#include <assert.h>
#include <stdlib.h> #include <stdlib.h>
#include <cstdio> #include <cstdio>
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include <AP_HAL/utility/RingBuffer.h> #include <AP_HAL/utility/RingBuffer.h>
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL &hal;
#define SPIUART_DEBUG 0 #ifdef SPIUART_DEBUG
#include <stdio.h>
#include <cassert>
#if SPIUART_DEBUG
#define debug(fmt, args ...) do {hal.console->printf("[SPIUARTDriver]: %s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0) #define debug(fmt, args ...) do {hal.console->printf("[SPIUARTDriver]: %s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)
#define error(fmt, args ...) do {fprintf(stderr,"%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0) #define error(fmt, args ...) do {fprintf(stderr,"%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)
#else #else
@ -22,43 +20,21 @@ extern const AP_HAL::HAL& hal;
using namespace Linux; using namespace Linux;
SPIUARTDriver::SPIUARTDriver() : SPIUARTDriver::SPIUARTDriver()
UARTDriver(false), : UARTDriver(false)
_dev(nullptr),
_last_update_timestamp(0),
_buffer(NULL),
_external(false)
{ {
_readbuf = NULL;
_writebuf = NULL;
}
bool SPIUARTDriver::sem_take_nonblocking()
{
return _dev->get_semaphore()->take_nonblocking();
}
void SPIUARTDriver::sem_give()
{
_dev->get_semaphore()->give();
} }
void SPIUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) void SPIUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
{ {
if (device_path != NULL) { if (device_path != NULL) {
UARTDriver::begin(b,rxS,txS); UARTDriver::begin(b, rxS, txS);
if ( is_initialized()) { if (is_initialized()) {
_external = true; _external = true;
return; return;
} }
} }
if (rxS < 1024) {
rxS = 2048;
}
if (txS < 1024) {
txS = 2048;
}
if (!is_initialized()) { if (!is_initialized()) {
_dev = hal.spi->get_device("ublox"); _dev = hal.spi->get_device("ublox");
if (!_dev) { if (!_dev) {
@ -66,65 +42,70 @@ void SPIUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
} }
} }
/* if (rxS < 1024) {
rxS = 2048;
}
if (txS < 1024) {
txS = 2048;
}
/*
allocate the read buffer allocate the read buffer
*/ */
if (rxS != 0 && rxS != _readbuf_size) { if (rxS != 0 && rxS != _readbuf_size) {
_readbuf_size = rxS; _readbuf_size = rxS;
if (_readbuf != NULL) { if (_readbuf != NULL) {
free(_readbuf); free(_readbuf);
} }
_readbuf = (uint8_t *)malloc(_readbuf_size); _readbuf = (uint8_t *)malloc(_readbuf_size);
_readbuf_head = 0; _readbuf_head = 0;
_readbuf_tail = 0; _readbuf_tail = 0;
} }
/* /*
allocate the write buffer allocate the write buffer
*/ */
if (txS != 0 && txS != _writebuf_size) { if (txS != 0 && txS != _writebuf_size) {
_writebuf_size = txS; _writebuf_size = txS;
if (_writebuf != NULL) { if (_writebuf != NULL) {
free(_writebuf); free(_writebuf);
} }
_writebuf = (uint8_t *)malloc(_writebuf_size); _writebuf = (uint8_t *)malloc(_writebuf_size);
_writebuf_head = 0; _writebuf_head = 0;
_writebuf_tail = 0; _writebuf_tail = 0;
} }
if (_buffer == NULL) { if (_buffer == NULL) {
/* Do not allocate new buffer, if we're just changing speed */ /* Do not allocate new buffer, if we're just changing speed */
_buffer = new uint8_t[rxS]; _buffer = new uint8_t[rxS];
if (_buffer == NULL) { if (_buffer == NULL) {
hal.console->printf("Not enough memory\n"); hal.console->printf("Not enough memory\n");
AP_HAL::panic("Not enough memory\n"); AP_HAL::panic("Not enough memory\n");
} }
} }
_dev = hal.spi->get_device("ublox"); switch (b) {
case 4000000U:
switch (b) { if (is_initialized()) {
case 4000000U: /* Do not allow speed changes before device is initialized, because
if (is_initialized()) {
/* Do not allow speed changes before device is initialized, because
* it can lead to misconfiguraration. Once the device is initialized, * it can lead to misconfiguraration. Once the device is initialized,
* it's sage to update speed * it's sage to update speed
*/ */
_dev->set_speed(AP_HAL::Device::SPEED_HIGH); _dev->set_speed(AP_HAL::Device::SPEED_HIGH);
debug("Set higher SPI-frequency"); debug("Set higher SPI-frequency");
} else { } else {
_dev->set_speed(AP_HAL::Device::SPEED_LOW); _dev->set_speed(AP_HAL::Device::SPEED_LOW);
debug("Set lower SPI-frequency"); debug("Set lower SPI-frequency");
} }
break; break;
default: default:
_dev->set_speed(AP_HAL::Device::SPEED_LOW); _dev->set_speed(AP_HAL::Device::SPEED_LOW);
debug("Set lower SPI-frequency"); debug("Set lower SPI-frequency");
debug("%s: wrong baudrate (%u) for SPI-driven device. setting default speed", __func__, b); debug("%s: wrong baudrate (%u) for SPI-driven device. setting default speed", __func__, b);
break; break;
} }
_initialised = true; _initialised = true;
} }
int SPIUARTDriver::_write_fd(const uint8_t *buf, uint16_t size) int SPIUARTDriver::_write_fd(const uint8_t *buf, uint16_t size)
@ -133,13 +114,13 @@ int SPIUARTDriver::_write_fd(const uint8_t *buf, uint16_t size)
return UARTDriver::_write_fd(buf,size); return UARTDriver::_write_fd(buf,size);
} }
if (!sem_take_nonblocking()) { if (!_dev->get_semaphore()->take_nonblocking()) {
return 0; return 0;
} }
_dev->transfer_fullduplex(buf, _buffer, size); _dev->transfer_fullduplex(buf, _buffer, size);
sem_give(); _dev->get_semaphore()->give();
BUF_ADVANCEHEAD(_writebuf, size); BUF_ADVANCEHEAD(_writebuf, size);
@ -186,9 +167,7 @@ int SPIUARTDriver::_write_fd(const uint8_t *buf, uint16_t size)
BUF_ADVANCETAIL(_readbuf, n); BUF_ADVANCETAIL(_readbuf, n);
} }
return ret; return ret;
} }
static uint8_t ff_stub[300] = {0xff}; static uint8_t ff_stub[300] = {0xff};
@ -199,23 +178,21 @@ int SPIUARTDriver::_read_fd(uint8_t *buf, uint16_t n)
return UARTDriver::_read_fd(buf, n); return UARTDriver::_read_fd(buf, n);
} }
if (!sem_take_nonblocking()) {
return 0;
}
/* Make SPI transactions shorter. It can save SPI bus from keeping too /* Make SPI transactions shorter. It can save SPI bus from keeping too
* long. It's essential for NavIO as MPU9250 is on the same bus and * long. It's essential for NavIO as MPU9250 is on the same bus and
* doesn't like to be waiting. Making transactions more frequent but shorter * doesn't like to be waiting. Making transactions more frequent but shorter
* is a win. * is a win.
*/ */
if (n > 100) { if (n > 100) {
n = 100; n = 100;
} }
_dev->transfer_fullduplex(ff_stub, buf, n); if (!_dev->get_semaphore()->take_nonblocking()) {
return 0;
}
sem_give(); _dev->transfer_fullduplex(ff_stub, buf, n);
_dev->get_semaphore()->give();
BUF_ADVANCETAIL(_readbuf, n); BUF_ADVANCETAIL(_readbuf, n);
@ -228,6 +205,7 @@ void SPIUARTDriver::_timer_tick(void)
UARTDriver::_timer_tick(); UARTDriver::_timer_tick();
return; return;
} }
/* lower the update rate */ /* lower the update rate */
if (AP_HAL::micros() - _last_update_timestamp < 10000) { if (AP_HAL::micros() - _last_update_timestamp < 10000) {
return; return;

7
libraries/AP_HAL_Linux/SPIUARTDriver.h
View File

@ -15,14 +15,11 @@ protected:
int _write_fd(const uint8_t *buf, uint16_t n); int _write_fd(const uint8_t *buf, uint16_t n);
int _read_fd(uint8_t *buf, uint16_t n); int _read_fd(uint8_t *buf, uint16_t n);
private:
bool sem_take_nonblocking();
void sem_give();
AP_HAL::OwnPtr<AP_HAL::SPIDevice> _dev; AP_HAL::OwnPtr<AP_HAL::SPIDevice> _dev;
uint8_t *_buffer;
uint32_t _last_update_timestamp; uint32_t _last_update_timestamp;
uint8_t *_buffer;
bool _external; bool _external;
}; };