aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_HAL_Linux: remove raspilot

Browse Source 
It's not being sold, there are just a few (different) engineering
samples built and there are no plans for this to go forward for people
that were pushing it.
This commit is contained in:
Lucas De Marchi 2017-09-07 22:39:35 -07:00
parent 0062084c40
commit 7ba82ff23a
18 changed files with 3 additions and 915 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

123
libraries/AP_HAL_Linux/AnalogIn_Raspilot.cpp
View File

@ -1,123 +0,0 @@
#include "AnalogIn_Raspilot.h"
#include <algorithm>
#include <AP_HAL/AP_HAL.h>
#include "px4io_protocol.h"
AnalogSource_Raspilot::AnalogSource_Raspilot(int16_t pin)
: _pin(pin)
{
}
void AnalogSource_Raspilot::set_pin(uint8_t pin)
{
if (_pin == pin) {
return;
}
_pin = pin;
}
float AnalogSource_Raspilot::read_average()
{
return read_latest();
}
float AnalogSource_Raspilot::read_latest()
{
return _value;
}
float AnalogSource_Raspilot::voltage_average()
{
return _value;
}
float AnalogSource_Raspilot::voltage_latest()
{
return _value;
}
float AnalogSource_Raspilot::voltage_average_ratiometric()
{
return _value;
}
extern const AP_HAL::HAL& hal;
AnalogIn_Raspilot::AnalogIn_Raspilot()
{
_channels_number = RASPILOT_ADC_MAX_CHANNELS;
}
float AnalogIn_Raspilot::board_voltage(void)
{
_vcc_pin_analog_source->set_pin(4);
return 5.0;
}
AP_HAL::AnalogSource* AnalogIn_Raspilot::channel(int16_t pin)
{
for (uint8_t j = 0; j < _channels_number; j++) {
if (_channels[j] == nullptr) {
_channels[j] = new AnalogSource_Raspilot(pin);
return _channels[j];
}
}
hal.console->printf("Out of analog channels\n");
return nullptr;
}
void AnalogIn_Raspilot::init()
{
_vcc_pin_analog_source = channel(4);
_dev = std::move(hal.spi->get_device("raspio"));
if (!_dev) {
AP_HAL::panic("Bus for AnalogIn_Raspilot not found");
return;
}
_dev->register_periodic_callback(100000, FUNCTOR_BIND_MEMBER(&AnalogIn_Raspilot::_update, void));
}
void AnalogIn_Raspilot::_update()
{
struct IOPacket tx = { }, rx = { };
uint16_t count = RASPILOT_ADC_MAX_CHANNELS;
tx.count_code = count | PKT_CODE_READ;
tx.page = PX4IO_PAGE_RAW_ADC_INPUT;
tx.offset = 0;
tx.crc = 0;
tx.crc = crc_packet(&tx);
/* set raspilotio to read reg4 */
_dev->transfer((uint8_t *)&tx, sizeof(tx), (uint8_t *)&rx, sizeof(rx));
// TODO: should not delay for such huge values: converting this to a
// state-machine like driver would be better, adjusting the callback timer
hal.scheduler->delay_microseconds(200);
count = 0;
tx.count_code = count | PKT_CODE_READ;
tx.page = 0;
tx.offset = 0;
tx.crc = 0;
tx.crc = crc_packet(&tx);
/* get reg4 data from raspilotio */
_dev->transfer((uint8_t *)&tx, sizeof(tx), (uint8_t *)&rx, sizeof(rx));
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];
if (source != nullptr && i == source->_pin) {
source->_value = rx.regs[i] * 3.3 / 4096.0;
}
}
}
}

46
libraries/AP_HAL_Linux/AnalogIn_Raspilot.h
View File

@ -1,46 +0,0 @@
#pragma once
#include <AP_ADC/AP_ADC.h>
#include <AP_HAL/SPIDevice.h>
#include "AP_HAL_Linux.h"
#define RASPILOT_ADC_MAX_CHANNELS 8
class AnalogSource_Raspilot: public AP_HAL::AnalogSource {
public:
friend class AnalogIn_Raspilot;
AnalogSource_Raspilot(int16_t pin);
float read_average();
float read_latest();
void set_pin(uint8_t p);
void set_stop_pin(uint8_t p) { }
void set_settle_time(uint16_t settle_time_ms) { }
float voltage_average();
float voltage_latest();
float voltage_average_ratiometric();
private:
int16_t _pin;
float _value;
};
class AnalogIn_Raspilot: public AP_HAL::AnalogIn {
public:
AnalogIn_Raspilot();
void init();
AP_HAL::AnalogSource* channel(int16_t n);
/* Board voltage is not available */
float board_voltage(void);
protected:
void _update();
AP_HAL::AnalogSource *_vcc_pin_analog_source;
AnalogSource_Raspilot *_channels[RASPILOT_ADC_MAX_CHANNELS];
AP_HAL::OwnPtr<AP_HAL::SPIDevice> _dev;
uint8_t _channels_number;
};

1
libraries/AP_HAL_Linux/GPIO.h
View File

@ -25,7 +25,6 @@ private:
#include "GPIO_BBB.h"
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO2 || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2 || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DARK || \

1
libraries/AP_HAL_Linux/GPIO_RPI.cpp
View File

@ -1,7 +1,6 @@
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2 || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DARK || \

19
libraries/AP_HAL_Linux/HAL_Linux_Class.cpp
View File

@ -15,7 +15,6 @@
#include "AnalogIn_ADS1115.h"
#include "AnalogIn_IIO.h"
#include "AnalogIn_Navio2.h"
#include "AnalogIn_Raspilot.h"
#include "GPIO.h"
#include "I2CDevice.h"
#include "OpticalFlow_Onboard.h"
@ -25,7 +24,6 @@
#include "RCInput_Navio2.h"
#include "RCInput_PRU.h"
#include "RCInput_RPI.h"
#include "RCInput_Raspilot.h"
#include "RCInput_SBUS.h"
#include "RCInput_SoloLink.h"
#include "RCInput_UART.h"
@ -39,10 +37,8 @@
#include "RCOutput_Disco.h"
#include "RCOutput_PCA9685.h"
#include "RCOutput_PRU.h"
#include "RCOutput_Raspilot.h"
#include "RCOutput_Sysfs.h"
#include "RCOutput_ZYNQ.h"
#include "RPIOUARTDriver.h"
#include "SPIDevice.h"
#include "SPIUARTDriver.h"
#include "Scheduler.h"
@ -55,7 +51,6 @@ using namespace Linux;
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO2 || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2 || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DARK || \
@ -67,11 +62,7 @@ static Util utilInstance;
// 3 serial ports on Linux for now
static UARTDriver uartADriver(true);
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
static RPIOUARTDriver uartCDriver;
#else
static UARTDriver uartCDriver(false);
#endif
static UARTDriver uartDDriver(false);
static UARTDriver uartEDriver(false);
static UARTDriver uartFDriver(false);
@ -92,8 +83,6 @@ static UARTDriver uartBDriver(false);
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXFMINI
static AnalogIn_ADS1115 analogIn;
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
static AnalogIn_Raspilot analogIn;
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBOARD || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI || \
@ -121,7 +110,6 @@ static GPIO_BBB gpioDriver;
*/
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO2 || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2 || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DARK || \
@ -150,8 +138,6 @@ static RCInput_AioPRU rcinDriver;
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DARK || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXFMINI
static RCInput_RPI rcinDriver;
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
static RCInput_Raspilot rcinDriver;
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ZYNQ
static RCInput_ZYNQ rcinDriver;
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_OCPOC_ZYNQ
@ -194,11 +180,6 @@ static RCOutput_PCA9685 rcoutDriver(i2c_mgr_instance.get_device(1, PCA9685_PRIMA
static RCOutput_PCA9685 rcoutDriver(i2c_mgr_instance.get_device(1, PCA9685_QUATENARY_ADDRESS), false, 0, RPI_GPIO_4);
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DARK
static RCOutput_PCA9685 rcoutDriver(i2c_mgr_instance.get_device(1, PCA9685_QUINARY_ADDRESS), false, 0, RPI_GPIO_27);
/*
use the STM32 based RCOutput driver on Raspilot
*/
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
static RCOutput_Raspilot rcoutDriver;
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ZYNQ || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_OCPOC_ZYNQ
static RCOutput_ZYNQ rcoutDriver;

57
libraries/AP_HAL_Linux/RCInput_Raspilot.cpp
View File

@ -1,57 +0,0 @@
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
#include <assert.h>
#include <errno.h>
#include <fcntl.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <unistd.h>
#include "RCInput_Raspilot.h"
#include "px4io_protocol.h"
static const AP_HAL::HAL& hal = AP_HAL::get_HAL();
using namespace Linux;
void RCInput_Raspilot::init()
{
_dev = hal.spi->get_device("raspio");
// start the timer process to read samples
_dev->register_periodic_callback(10000, FUNCTOR_BIND_MEMBER(&RCInput_Raspilot::_poll_data, void));
}
void RCInput_Raspilot::_poll_data(void)
{
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;
_dma_packet_tx.page = 4;
_dma_packet_tx.offset = 0;
_dma_packet_tx.crc = 0;
_dma_packet_tx.crc = crc_packet(&_dma_packet_tx);
/* set raspilotio to read reg4 */
_dev->transfer((uint8_t *)&_dma_packet_tx, sizeof(_dma_packet_tx),
(uint8_t *)&_dma_packet_rx, sizeof(_dma_packet_rx));
/* get reg4 data from raspilotio */
_dev->transfer((uint8_t *)&_dma_packet_tx, sizeof(_dma_packet_tx),
(uint8_t *)&_dma_packet_rx, sizeof(_dma_packet_rx));
uint16_t num_values = _dma_packet_rx.regs[0];
uint16_t rc_ok = _dma_packet_rx.regs[1] & (1 << 4);
uint8_t rx_crc = _dma_packet_rx.crc;
_dma_packet_rx.crc = 0;
if ( rc_ok && (rx_crc == crc_packet(&_dma_packet_rx)) ) {
_update_periods(&_dma_packet_rx.regs[6], (uint8_t)num_values);
}
}
#endif // CONFIG_HAL_BOARD_SUBTYPE

21
libraries/AP_HAL_Linux/RCInput_Raspilot.h
View File

@ -1,21 +0,0 @@
#pragma once
#include "AP_HAL_Linux.h"
#include "RCInput.h"
#include <AP_HAL/SPIDevice.h>
namespace Linux {
class RCInput_Raspilot : public RCInput
{
public:
void init();
private:
AP_HAL::OwnPtr<AP_HAL::SPIDevice> _dev;
void _poll_data(void);
};
}

144
libraries/AP_HAL_Linux/RCOutput_Raspilot.cpp
View File

@ -1,144 +0,0 @@
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
#include "RCOutput_Raspilot.h"
#include <cmath>
#include <dirent.h>
#include <fcntl.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <unistd.h>
#include "px4io_protocol.h"
#include "GPIO.h"
using namespace Linux;
#define PWM_CHAN_COUNT 8
static const AP_HAL::HAL& hal = AP_HAL::get_HAL();
void RCOutput_Raspilot::init()
{
_dev = hal.spi->get_device("raspio");
_dev->register_periodic_callback(10000, FUNCTOR_BIND_MEMBER(&RCOutput_Raspilot::_update, void));
}
void RCOutput_Raspilot::set_freq(uint32_t chmask, uint16_t freq_hz)
{
_new_frequency = freq_hz;
}
uint16_t RCOutput_Raspilot::get_freq(uint8_t ch)
{
return _frequency;
}
void RCOutput_Raspilot::enable_ch(uint8_t ch)
{
}
void RCOutput_Raspilot::disable_ch(uint8_t ch)
{
write(ch, 0);
}
void RCOutput_Raspilot::write(uint8_t ch, uint16_t period_us)
{
if(ch >= PWM_CHAN_COUNT){
return;
}
_period_us[ch] = period_us;
}
uint16_t RCOutput_Raspilot::read(uint8_t ch)
{
if(ch >= PWM_CHAN_COUNT){
return 0;
}
return _period_us[ch];
}
void RCOutput_Raspilot::read(uint16_t* period_us, uint8_t len)
{
for (int i = 0; i < len; i++)
period_us[i] = read(0 + i);
}
void RCOutput_Raspilot::_update(void)
{
int i;
if (_corked) {
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;
uint16_t count = 1;
_dma_packet_tx.count_code = count | PKT_CODE_WRITE;
_dma_packet_tx.page = 50;
_dma_packet_tx.offset = 1;
_dma_packet_tx.regs[0] = 75;
_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));
count = 1;
_dma_packet_tx.count_code = count | PKT_CODE_WRITE;
_dma_packet_tx.page = 50;
_dma_packet_tx.offset = 12;
_dma_packet_tx.regs[0] = 0x560B;
_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));
count = PWM_CHAN_COUNT;
_dma_packet_tx.count_code = count | PKT_CODE_WRITE;
_dma_packet_tx.page = 54;
_dma_packet_tx.offset = 0;
for (i=0; i<PWM_CHAN_COUNT; i++) {
_dma_packet_tx.regs[i] = _period_us[i];
}
_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));
}
void RCOutput_Raspilot::cork(void)
{
_corked = true;
}
void RCOutput_Raspilot::push(void)
{
_corked = false;
}
#endif // CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT

34
libraries/AP_HAL_Linux/RCOutput_Raspilot.h
View File

@ -1,34 +0,0 @@
#pragma once
#include "AP_HAL_Linux.h"
#include <AP_HAL/SPIDevice.h>
namespace Linux {
class RCOutput_Raspilot : public AP_HAL::RCOutput {
public:
void init();
void set_freq(uint32_t chmask, uint16_t freq_hz);
uint16_t get_freq(uint8_t ch);
void enable_ch(uint8_t ch);
void disable_ch(uint8_t ch);
void write(uint8_t ch, uint16_t period_us);
uint16_t read(uint8_t ch);
void read(uint16_t* period_us, uint8_t len);
void cork(void) override;
void push(void) override;
private:
void reset();
void _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;
};
}

175
libraries/AP_HAL_Linux/RPIOUARTDriver.cpp
View File

@ -1,175 +0,0 @@
#include "RPIOUARTDriver.h"
#include <stdlib.h>
#include <cstdio>
#include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h>
#include "px4io_protocol.h"
extern const AP_HAL::HAL& hal;
#define RPIOUART_DEBUG 0
#include <cassert>
#if RPIOUART_DEBUG
#define debug(fmt, args ...) do {hal.console->printf("[RPIOUARTDriver]: %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
#define debug(fmt, args ...)
#define error(fmt, args ...)
#endif
using namespace Linux;
RPIOUARTDriver::RPIOUARTDriver() :
UARTDriver(false),
_dev(nullptr),
_external(false),
_need_set_baud(false),
_baudrate(0)
{
}
bool RPIOUARTDriver::isExternal()
{
return _external;
}
void RPIOUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
{
//hal.console->printf("[RPIOUARTDriver]: begin \n");
if (device_path != nullptr) {
UARTDriver::begin(b,rxS,txS);
if ( is_initialized()) {
_external = true;
return;
}
}
if (rxS < 1024) {
rxS = 2048;
}
if (txS < 1024) {
txS = 2048;
}
_initialised = false;
while (_in_timer) hal.scheduler->delay(1);
_readbuf.set_size(rxS);
_writebuf.set_size(txS);
if (!_registered_callback) {
_dev = hal.spi->get_device("raspio");
_registered_callback = true;
_dev->register_periodic_callback(100000, FUNCTOR_BIND_MEMBER(&RPIOUARTDriver::_bus_timer, void));
}
/* set baudrate */
_baudrate = b;
_need_set_baud = true;
if (_writebuf.get_size() && _readbuf.get_size()) {
_initialised = true;
}
}
int RPIOUARTDriver::_write_fd(const uint8_t *buf, uint16_t n)
{
if (_external) {
return UARTDriver::_write_fd(buf, n);
}
return -1;
}
int RPIOUARTDriver::_read_fd(uint8_t *buf, uint16_t n)
{
if (_external) {
return UARTDriver::_read_fd(buf, n);
}
return -1;
}
void RPIOUARTDriver::_timer_tick(void)
{
if (_external) {
UARTDriver::_timer_tick();
return;
}
}
void RPIOUARTDriver::_bus_timer(void)
{
/* set the baudrate of raspilotio */
if (_need_set_baud) {
if (_baudrate != 0) {
struct IOPacket _packet_tx = {0}, _packet_rx = {0};
_packet_tx.count_code = 2 | PKT_CODE_WRITE;
_packet_tx.page = PX4IO_PAGE_UART_BUFFER;
_packet_tx.regs[0] = _baudrate & 0xffff;
_packet_tx.regs[1] = _baudrate >> 16;
_packet_tx.crc = crc_packet(&_packet_tx);
_dev->transfer((uint8_t *)&_packet_tx, sizeof(_packet_tx),
(uint8_t *)&_packet_rx, sizeof(_packet_rx));
hal.scheduler->delay(1);
}
_need_set_baud = false;
}
/* finish set */
if (!_initialised) {
return;
}
_in_timer = true;
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 / 100000));
uint32_t n = MIN(_writebuf.available(), max_size);
_writebuf.read(&((uint8_t *)_packet_tx.regs)[0], n);
_packet_tx.count_code = PKT_MAX_REGS | PKT_CODE_SPIUART;
_packet_tx.page = PX4IO_PAGE_UART_BUFFER;
_packet_tx.offset = n;
_packet_tx.crc = crc_packet(&_packet_tx);
/* end get write_buf bytes */
/* set raspilotio to read uart data */
_dev->transfer((uint8_t *)&_packet_tx, sizeof(_packet_tx),
(uint8_t *)&_packet_rx, sizeof(_packet_rx));
hal.scheduler->delay_microseconds(100);
/* get uart data from raspilotio */
memset(&_packet_tx, 0, sizeof(_packet_tx));
_packet_tx.count_code = PKT_CODE_READ;
_packet_tx.crc = crc_packet(&_packet_tx);
_dev->transfer((uint8_t *)&_packet_tx, sizeof(_packet_tx),
(uint8_t *)&_packet_rx, sizeof(_packet_rx));
hal.scheduler->delay_microseconds(100);
if (_packet_rx.page == PX4IO_PAGE_UART_BUFFER) {
/* add bytes to read buf */
max_size = MIN(_packet_rx.offset, PKT_MAX_REGS * 2);
n = MIN(_readbuf.space(), max_size);
_readbuf.write(&((uint8_t *)_packet_rx.regs)[0], n);
}
_in_timer = false;
}

40
libraries/AP_HAL_Linux/RPIOUARTDriver.h
View File

@ -1,40 +0,0 @@
#pragma once
#include "AP_HAL_Linux.h"
#include "UARTDriver.h"
#include <AP_HAL/SPIDevice.h>
namespace Linux {
class RPIOUARTDriver : public UARTDriver {
public:
RPIOUARTDriver();
static RPIOUARTDriver *from(AP_HAL::UARTDriver *uart) {
return static_cast<RPIOUARTDriver*>(uart);
}
void begin(uint32_t b, uint16_t rxS, uint16_t txS);
void _timer_tick(void);
bool isExternal(void);
protected:
int _write_fd(const uint8_t *buf, uint16_t n);
int _read_fd(uint8_t *buf, uint16_t n);
private:
bool _in_timer;
void _bus_timer(void);
AP_HAL::OwnPtr<AP_HAL::SPIDevice> _dev;
bool _external;
bool _registered_callback;
bool _need_set_baud;
uint32_t _baudrate;
};
}

8
libraries/AP_HAL_Linux/SPIDevice.cpp
View File

@ -99,14 +99,6 @@ SPIDesc SPIDeviceManager::_device[] = {
SPIDesc("mpu9250ext", 1, 0, SPI_MODE_3, 8, SPI_CS_KERNEL, 1*MHZ, 11*MHZ),
SPIDesc("ms5611", 2, 1, SPI_MODE_3, 8, SPI_CS_KERNEL, 10*MHZ,10*MHZ),
};
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
SPIDesc SPIDeviceManager::_device[] = {
SPIDesc("mpu6000", 0, 0, SPI_MODE_3, 8, RPI_GPIO_25, 1*MHZ, 11*MHZ),
SPIDesc("ms5611", 0, 0, SPI_MODE_3, 8, RPI_GPIO_23, 10*MHZ, 10*MHZ),
SPIDesc("lsm9ds0_am", 0, 0, SPI_MODE_3, 8, RPI_GPIO_22, 10*MHZ, 10*MHZ),
SPIDesc("lsm9ds0_g", 0, 0, SPI_MODE_3, 8, RPI_GPIO_12, 10*MHZ, 10*MHZ),
SPIDesc("raspio", 0, 0, SPI_MODE_3, 8, RPI_GPIO_7, 10*MHZ, 10*MHZ),
};
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_OCPOC_ZYNQ
SPIDesc SPIDeviceManager::_device[] = {
/* MPU9250 is restricted to 1MHz for non-data and interrupt registers */

15
libraries/AP_HAL_Linux/Scheduler.cpp
View File

@ -13,7 +13,6 @@
#include <AP_Vehicle/AP_Vehicle_Type.h>
#include "RCInput.h"
#include "RPIOUARTDriver.h"
#include "SPIUARTDriver.h"
#include "Storage.h"
#include "UARTDriver.h"
@ -254,13 +253,6 @@ void Scheduler::_timer_task()
}
}
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
//SPI UART use SPI
if (!((RPIOUARTDriver *)hal.uartC)->isExternal()) {
((RPIOUARTDriver *)hal.uartC)->_timer_tick();
}
#endif
_timer_semaphore.give();
// and the failsafe, if one is setup
@ -305,14 +297,7 @@ void Scheduler::_run_uarts()
// process any pending serial bytes
UARTDriver::from(hal.uartA)->_timer_tick();
UARTDriver::from(hal.uartB)->_timer_tick();
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
//SPI UART not use SPI
if (RPIOUARTDriver::from(hal.uartC)->isExternal()) {
RPIOUARTDriver::from(hal.uartC)->_timer_tick();
}
#else
UARTDriver::from(hal.uartC)->_timer_tick();
#endif
UARTDriver::from(hal.uartD)->_timer_tick();
UARTDriver::from(hal.uartE)->_timer_tick();
UARTDriver::from(hal.uartF)->_timer_tick();

196
libraries/AP_HAL_Linux/ToneAlarm_Raspilot.cpp
View File

@ -1,196 +0,0 @@
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
#include "ToneAlarm_Raspilot.h"
#include <errno.h>
#include <fcntl.h>
#include <iostream>
#include <poll.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/mman.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <unistd.h>
#include <AP_Math/AP_Math.h>
#include "GPIO.h"
#include "Util_RPI.h"
#define RASPILOT_TONE_PIN RPI_GPIO_18
#define RASPILOT_TONE_PIN_ALT 5
#define RPI1_PWM_BASE 0x2020C000
#define RPI1_CLK_BASE 0x20101000
#define RPI2_PWM_BASE 0x3F20C000
#define RPI2_CLK_BASE 0x3F101000
#define RPI_PWM_CTL 0
#define RPI_PWM_RNG1 4
#define RPI_PWM_DAT1 5
#define RPI_PWMCLK_CNTL 40
#define RPI_PWMCLK_DIV 41
using namespace Linux;
extern const AP_HAL::HAL &hal;
ToneAlarm_Raspilot::ToneAlarm_Raspilot()
{
// initialy no tune to play
tune_num = -1;
tune_pos = 0;
}
bool ToneAlarm_Raspilot::init()
{
uint32_t pwm_address, clk_address;
int mem_fd;
// play startup tune
tune_num = 0;
int rpi_version = UtilRPI::from(hal.util)->get_rpi_version();
if (rpi_version == 1) {
pwm_address = RPI1_PWM_BASE;
clk_address = RPI1_CLK_BASE;
} else {
pwm_address = RPI2_PWM_BASE;
clk_address = RPI2_CLK_BASE;
}
// open /dev/mem
if ((mem_fd = open("/dev/mem", O_RDWR|O_SYNC|O_CLOEXEC) ) < 0) {
AP_HAL::panic("Can't open /dev/mem");
}
// mmap GPIO
void *pwm_map = mmap(
nullptr, // Any adddress in our space will do
BLOCK_SIZE, // Map length
PROT_READ|PROT_WRITE, // Enable reading & writting to mapped memory
MAP_SHARED, // Shared with other processes
mem_fd, // File to map
pwm_address // Offset to GPIO peripheral
);
void *clk_map = mmap(
nullptr, // Any adddress in our space will do
BLOCK_SIZE, // Map length
PROT_READ|PROT_WRITE, // Enable reading & writting to mapped memory
MAP_SHARED, // Shared with other processes
mem_fd, // File to map
clk_address // Offset to GPIO peripheral
);
// No need to keep mem_fd open after mmap
close(mem_fd);
if (pwm_map == MAP_FAILED || clk_map == MAP_FAILED) {
AP_HAL::panic("ToneAlarm: Error!! Can't open /dev/mem");
}
_pwm = (volatile uint32_t *)pwm_map;
_clk = (volatile uint32_t *)clk_map;
hal.gpio->pinMode(RASPILOT_TONE_PIN, HAL_GPIO_ALT, 5);
return true;
}
void ToneAlarm_Raspilot::stop()
{
_set_pwm0_duty(0);
}
bool ToneAlarm_Raspilot::play()
{
uint32_t cur_time = AP_HAL::millis();
if (tune_num != prev_tune_num){
tune_changed = true;
return true;
}
if (cur_note != 0){
_set_pwm0_period(1000000/cur_note);
_set_pwm0_duty(50);
cur_note =0;
prev_time = cur_time;
}
if ((cur_time - prev_time) > duration){
stop();
if (tune[tune_num][tune_pos] == '\0'){
if (!tune_repeat[tune_num]){
tune_num = -1;
}
tune_pos = 0;
tune_comp = true;
return false;
}
return true;
}
return false;
}
void ToneAlarm_Raspilot::_set_pwm0_period(uint32_t time_us)
{
// stop clock and waiting for busy flag doesn't work, so kill clock
*(_clk + RPI_PWMCLK_CNTL) = 0x5A000000 | (1 << 5);
usleep(10);
// set frequency
// DIVI is the integer part of the divisor
// the fractional part (DIVF) drops clock cycles to get the output frequency, bad for servo motors
// 320 bits for one cycle of 20 milliseconds = 62.5 us per bit = 16 kHz
int idiv = (int) (19200000.0f / (320000000.0f / time_us));
if (idiv < 1 || idiv > 0x1000) {
return;
}
*(_clk + RPI_PWMCLK_DIV) = 0x5A000000 | (idiv<<12);
// source=osc and enable clock
*(_clk + RPI_PWMCLK_CNTL) = 0x5A000011;
// disable PWM
*(_pwm + RPI_PWM_CTL) = 0;
// needs some time until the PWM module gets disabled, without the delay the PWM module crashs
usleep(10);
// filled with 0 for 20 milliseconds = 320 bits
*(_pwm + RPI_PWM_RNG1) = 320;
// init with 0%
_set_pwm0_duty(0);
// start PWM1 in serializer mode
*(_pwm + RPI_PWM_CTL) = 3;
}
void ToneAlarm_Raspilot::_set_pwm0_duty(uint8_t percent)
{
int bit_count = constrain_int32(320 * percent / 100, 320, 0);
unsigned int bits = 0;
// FIXME: bits overflows for any bit_count > 32
while (bit_count) {
bits <<= 1;
bits |= 1;
bit_count--;
}
*(_pwm + RPI_PWM_DAT1) = bits;
}
#endif

24
libraries/AP_HAL_Linux/ToneAlarm_Raspilot.h
View File

@ -1,24 +0,0 @@
#pragma once
#include "AP_HAL_Linux.h"
#include "ToneAlarm.h"
namespace Linux {
class ToneAlarm_Raspilot : public ToneAlarm {
public:
ToneAlarm_Raspilot();
bool init() override;
void stop() override;
bool play() override;
private:
void _set_pwm0_period(uint32_t time_us);
void _set_pwm0_duty(uint8_t percent);
volatile uint32_t *_pwm;
volatile uint32_t *_clk;
};
}

5
libraries/AP_HAL_Linux/Util.cpp
View File

@ -10,7 +10,6 @@
#include <AP_HAL/AP_HAL.h>
#include "Heat_Pwm.h"
#include "ToneAlarm_Raspilot.h"
#include "ToneAlarm_Disco.h"
#include "Util.h"
@ -19,9 +18,7 @@ using namespace Linux;
extern const AP_HAL::HAL& hal;
static int state;
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
ToneAlarm_Raspilot Util::_toneAlarm;
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO
ToneAlarm_Disco Util::_toneAlarm;
#else
ToneAlarm Util::_toneAlarm;

8
libraries/AP_HAL_Linux/Util.h
View File

@ -5,9 +5,7 @@
#include "Heat.h"
#include "Perf.h"
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
#include "ToneAlarm_Raspilot.h"
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO
#include "ToneAlarm_Disco.h"
#endif
#include "ToneAlarm.h"
@ -99,9 +97,7 @@ public:
int get_hw_arm32();
private:
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
static ToneAlarm_Raspilot _toneAlarm;
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO
static ToneAlarm_Disco _toneAlarm;
#else
static ToneAlarm _toneAlarm;

1
libraries/AP_HAL_Linux/Util_RPI.cpp
View File

@ -2,7 +2,6 @@
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO2 || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2 || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DARK || \