diff --git a/libraries/AP_HAL_Linux/AnalogIn_Raspilot.cpp b/libraries/AP_HAL_Linux/AnalogIn_Raspilot.cpp deleted file mode 100644 index 7d4d5eb349..0000000000 --- a/libraries/AP_HAL_Linux/AnalogIn_Raspilot.cpp +++ /dev/null @@ -1,123 +0,0 @@ -#include "AnalogIn_Raspilot.h" - -#include - -#include - -#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; - } - } - } -} diff --git a/libraries/AP_HAL_Linux/AnalogIn_Raspilot.h b/libraries/AP_HAL_Linux/AnalogIn_Raspilot.h deleted file mode 100644 index 5ee582fdf7..0000000000 --- a/libraries/AP_HAL_Linux/AnalogIn_Raspilot.h +++ /dev/null @@ -1,46 +0,0 @@ -#pragma once - -#include -#include - -#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 _dev; - - uint8_t _channels_number; -}; diff --git a/libraries/AP_HAL_Linux/GPIO.h b/libraries/AP_HAL_Linux/GPIO.h index d9a27ae12c..4995b358a5 100644 --- a/libraries/AP_HAL_Linux/GPIO.h +++ b/libraries/AP_HAL_Linux/GPIO.h @@ -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 || \ diff --git a/libraries/AP_HAL_Linux/GPIO_RPI.cpp b/libraries/AP_HAL_Linux/GPIO_RPI.cpp index 385f243d55..f03fd60c52 100644 --- a/libraries/AP_HAL_Linux/GPIO_RPI.cpp +++ b/libraries/AP_HAL_Linux/GPIO_RPI.cpp @@ -1,7 +1,6 @@ #include #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 || \ diff --git a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp b/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp index caecb3f9af..d8c6b0853e 100644 --- a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp +++ b/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp @@ -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; diff --git a/libraries/AP_HAL_Linux/RCInput_Raspilot.cpp b/libraries/AP_HAL_Linux/RCInput_Raspilot.cpp deleted file mode 100644 index 1d5ab68f0b..0000000000 --- a/libraries/AP_HAL_Linux/RCInput_Raspilot.cpp +++ /dev/null @@ -1,57 +0,0 @@ -#include - -#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT - -#include -#include -#include -#include -#include -#include -#include -#include - -#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 diff --git a/libraries/AP_HAL_Linux/RCInput_Raspilot.h b/libraries/AP_HAL_Linux/RCInput_Raspilot.h deleted file mode 100644 index fc9a8670ea..0000000000 --- a/libraries/AP_HAL_Linux/RCInput_Raspilot.h +++ /dev/null @@ -1,21 +0,0 @@ -#pragma once - -#include "AP_HAL_Linux.h" -#include "RCInput.h" -#include - - -namespace Linux { - -class RCInput_Raspilot : public RCInput -{ -public: - void init(); - -private: - AP_HAL::OwnPtr _dev; - - void _poll_data(void); -}; - -} diff --git a/libraries/AP_HAL_Linux/RCOutput_Raspilot.cpp b/libraries/AP_HAL_Linux/RCOutput_Raspilot.cpp deleted file mode 100644 index 223217ba78..0000000000 --- a/libraries/AP_HAL_Linux/RCOutput_Raspilot.cpp +++ /dev/null @@ -1,144 +0,0 @@ - -#include - -#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT -#include "RCOutput_Raspilot.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#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; itransfer((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 diff --git a/libraries/AP_HAL_Linux/RCOutput_Raspilot.h b/libraries/AP_HAL_Linux/RCOutput_Raspilot.h deleted file mode 100644 index e8529ea918..0000000000 --- a/libraries/AP_HAL_Linux/RCOutput_Raspilot.h +++ /dev/null @@ -1,34 +0,0 @@ -#pragma once - -#include "AP_HAL_Linux.h" -#include - -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 _dev; - - uint32_t _last_update_timestamp; - uint16_t _frequency; - uint16_t _new_frequency; - uint16_t _period_us[8]; - bool _corked; -}; - -} diff --git a/libraries/AP_HAL_Linux/RPIOUARTDriver.cpp b/libraries/AP_HAL_Linux/RPIOUARTDriver.cpp deleted file mode 100644 index d80b566558..0000000000 --- a/libraries/AP_HAL_Linux/RPIOUARTDriver.cpp +++ /dev/null @@ -1,175 +0,0 @@ -#include "RPIOUARTDriver.h" - -#include -#include - -#include -#include - -#include "px4io_protocol.h" - -extern const AP_HAL::HAL& hal; - -#define RPIOUART_DEBUG 0 - -#include - -#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; -} diff --git a/libraries/AP_HAL_Linux/RPIOUARTDriver.h b/libraries/AP_HAL_Linux/RPIOUARTDriver.h deleted file mode 100644 index e1b119bf7b..0000000000 --- a/libraries/AP_HAL_Linux/RPIOUARTDriver.h +++ /dev/null @@ -1,40 +0,0 @@ -#pragma once - -#include "AP_HAL_Linux.h" - -#include "UARTDriver.h" -#include - -namespace Linux { - -class RPIOUARTDriver : public UARTDriver { -public: - RPIOUARTDriver(); - - static RPIOUARTDriver *from(AP_HAL::UARTDriver *uart) { - return static_cast(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 _dev; - - bool _external; - bool _registered_callback; - - bool _need_set_baud; - uint32_t _baudrate; -}; - -} diff --git a/libraries/AP_HAL_Linux/SPIDevice.cpp b/libraries/AP_HAL_Linux/SPIDevice.cpp index f8eeb8dae1..d7d2cf5145 100644 --- a/libraries/AP_HAL_Linux/SPIDevice.cpp +++ b/libraries/AP_HAL_Linux/SPIDevice.cpp @@ -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 */ diff --git a/libraries/AP_HAL_Linux/Scheduler.cpp b/libraries/AP_HAL_Linux/Scheduler.cpp index 1991a15eff..c11f89d83e 100644 --- a/libraries/AP_HAL_Linux/Scheduler.cpp +++ b/libraries/AP_HAL_Linux/Scheduler.cpp @@ -13,7 +13,6 @@ #include #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(); diff --git a/libraries/AP_HAL_Linux/ToneAlarm_Raspilot.cpp b/libraries/AP_HAL_Linux/ToneAlarm_Raspilot.cpp deleted file mode 100644 index a02132cced..0000000000 --- a/libraries/AP_HAL_Linux/ToneAlarm_Raspilot.cpp +++ /dev/null @@ -1,196 +0,0 @@ -#include - -#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT - -#include "ToneAlarm_Raspilot.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#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 diff --git a/libraries/AP_HAL_Linux/ToneAlarm_Raspilot.h b/libraries/AP_HAL_Linux/ToneAlarm_Raspilot.h deleted file mode 100644 index 33be2581a8..0000000000 --- a/libraries/AP_HAL_Linux/ToneAlarm_Raspilot.h +++ /dev/null @@ -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; -}; - -} diff --git a/libraries/AP_HAL_Linux/Util.cpp b/libraries/AP_HAL_Linux/Util.cpp index 5b66d2e055..965896af10 100644 --- a/libraries/AP_HAL_Linux/Util.cpp +++ b/libraries/AP_HAL_Linux/Util.cpp @@ -10,7 +10,6 @@ #include #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; diff --git a/libraries/AP_HAL_Linux/Util.h b/libraries/AP_HAL_Linux/Util.h index 22736b3147..cf28954f33 100644 --- a/libraries/AP_HAL_Linux/Util.h +++ b/libraries/AP_HAL_Linux/Util.h @@ -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; diff --git a/libraries/AP_HAL_Linux/Util_RPI.cpp b/libraries/AP_HAL_Linux/Util_RPI.cpp index f031083028..00865a05ba 100644 --- a/libraries/AP_HAL_Linux/Util_RPI.cpp +++ b/libraries/AP_HAL_Linux/Util_RPI.cpp @@ -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 || \