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.master
Lucas De Marchi
8 years ago
18 changed files with 3 additions and 915 deletions
@ -1,123 +0,0 @@
@@ -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; |
||||
} |
||||
} |
||||
} |
||||
} |
||||
@ -1,46 +0,0 @@
@@ -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,57 +0,0 @@
@@ -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
|
||||
@ -1,21 +0,0 @@
@@ -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); |
||||
}; |
||||
|
||||
} |
||||
@ -1,144 +0,0 @@
@@ -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
|
||||
@ -1,34 +0,0 @@
@@ -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; |
||||
}; |
||||
|
||||
} |
||||
@ -1,175 +0,0 @@
@@ -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; |
||||
} |
||||
@ -1,40 +0,0 @@
@@ -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; |
||||
}; |
||||
|
||||
} |
||||
@ -1,196 +0,0 @@
@@ -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 |
||||
@ -1,24 +0,0 @@
@@ -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; |
||||
}; |
||||
|
||||
} |
||||
Loading…
Reference in new issue