diff --git a/libraries/AP_HAL_Linux/AP_HAL_Linux_Private.h b/libraries/AP_HAL_Linux/AP_HAL_Linux_Private.h index b639079911..d989f3f04d 100644 --- a/libraries/AP_HAL_Linux/AP_HAL_Linux_Private.h +++ b/libraries/AP_HAL_Linux/AP_HAL_Linux_Private.h @@ -13,8 +13,8 @@ #include "SPIDriver.h" #include "AnalogIn.h" #include "AnalogIn_ADS1115.h" -#include "RaspilotAnalogIn.h" #include "AnalogIn_IIO.h" +#include "AnalogIn_Raspilot.h" #include "Storage.h" #include "GPIO.h" #include "RCInput.h" @@ -27,6 +27,7 @@ #include "RCOutput_PRU.h" #include "RCOutput_AioPRU.h" #include "RCOutput_PCA9685.h" +#include "RCOutput_Raspilot.h" #include "RCOutput_ZYNQ.h" #include "RCOutput_Bebop.h" #include "RCOutput_Raspilot.h" @@ -46,4 +47,3 @@ #include "Flow_PX4.h" #endif // __AP_HAL_LINUX_PRIVATE_H__ - diff --git a/libraries/AP_HAL_Linux/RaspilotAnalogIn.cpp b/libraries/AP_HAL_Linux/AnalogIn_Raspilot.cpp similarity index 79% rename from libraries/AP_HAL_Linux/RaspilotAnalogIn.cpp rename to libraries/AP_HAL_Linux/AnalogIn_Raspilot.cpp index 3fc209c0a4..7d0bbc7523 100644 --- a/libraries/AP_HAL_Linux/RaspilotAnalogIn.cpp +++ b/libraries/AP_HAL_Linux/AnalogIn_Raspilot.cpp @@ -2,8 +2,7 @@ #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX -#include -#include "RaspilotAnalogIn.h" +#include "AnalogIn_Raspilot.h" #include "px4io_protocol.h" #define RASPILOT_ANALOGIN_DEBUG 0 @@ -12,17 +11,17 @@ #define debug(fmt, args ...) do {hal.console->printf("%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 ...) +#define debug(fmt, args ...) +#define error(fmt, args ...) #endif -RaspilotAnalogSource::RaspilotAnalogSource(int16_t pin): +AnalogSource_Raspilot::AnalogSource_Raspilot(int16_t pin): _pin(pin), _value(0.0f) { } -void RaspilotAnalogSource::set_pin(uint8_t pin) +void AnalogSource_Raspilot::set_pin(uint8_t pin) { if (_pin == pin) { return; @@ -30,27 +29,27 @@ void RaspilotAnalogSource::set_pin(uint8_t pin) _pin = pin; } -float RaspilotAnalogSource::read_average() -{ +float AnalogSource_Raspilot::read_average() +{ return read_latest(); } -float RaspilotAnalogSource::read_latest() +float AnalogSource_Raspilot::read_latest() { return _value; } -float RaspilotAnalogSource::voltage_average() +float AnalogSource_Raspilot::voltage_average() { return _value; } -float RaspilotAnalogSource::voltage_latest() +float AnalogSource_Raspilot::voltage_latest() { return _value; } -float RaspilotAnalogSource::voltage_average_ratiometric() +float AnalogSource_Raspilot::voltage_average_ratiometric() { return _value; } @@ -65,7 +64,7 @@ RaspilotAnalogIn::RaspilotAnalogIn() float RaspilotAnalogIn::board_voltage(void) { _vcc_pin_analog_source->set_pin(4); - + return 5.0; //return _vcc_pin_analog_source->voltage_average() * 2.0; } @@ -74,7 +73,7 @@ AP_HAL::AnalogSource* RaspilotAnalogIn::channel(int16_t pin) { for (uint8_t j = 0; j < _channels_number; j++) { if (_channels[j] == NULL) { - _channels[j] = new RaspilotAnalogSource(pin); + _channels[j] = new AnalogSource_Raspilot(pin); return _channels[j]; } } @@ -86,16 +85,16 @@ AP_HAL::AnalogSource* RaspilotAnalogIn::channel(int16_t pin) void RaspilotAnalogIn::init() { _vcc_pin_analog_source = channel(4); - + _spi = hal.spi->device(AP_HAL::SPIDevice_RASPIO); _spi_sem = _spi->get_semaphore(); - + if (_spi_sem == NULL) { AP_HAL::panic("PANIC: RCIutput_Raspilot did not get " "valid SPI semaphore!"); return; // never reached } - + hal.scheduler->suspend_timer_procs(); hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&RaspilotAnalogIn::_update, void)); hal.scheduler->resume_timer_procs(); @@ -106,11 +105,11 @@ void RaspilotAnalogIn::_update() if (AP_HAL::micros() - _last_update_timestamp < 100000) { return; } - + if (!_spi_sem->take_nonblocking()) { return; } - + struct IOPacket _dma_packet_tx, _dma_packet_rx; uint16_t count = RASPILOT_ADC_MAX_CHANNELS; _dma_packet_tx.count_code = count | PKT_CODE_READ; @@ -120,17 +119,23 @@ void RaspilotAnalogIn::_update() _dma_packet_tx.crc = crc_packet(&_dma_packet_tx); /* set raspilotio to read reg4 */ _spi->transaction((uint8_t *)&_dma_packet_tx, (uint8_t *)&_dma_packet_rx, sizeof(_dma_packet_tx)); - + hal.scheduler->delay_microseconds(200); - + + count = 0; + _dma_packet_tx.count_code = count | PKT_CODE_READ; + _dma_packet_tx.page = 0; + _dma_packet_tx.offset = 0; + _dma_packet_tx.crc = 0; + _dma_packet_tx.crc = crc_packet(&_dma_packet_tx); /* get reg4 data from raspilotio */ _spi->transaction((uint8_t *)&_dma_packet_tx, (uint8_t *)&_dma_packet_rx, sizeof(_dma_packet_tx)); - + _spi_sem->give(); for (int16_t i = 0; i < RASPILOT_ADC_MAX_CHANNELS; i++) { for (int16_t j=0; j < RASPILOT_ADC_MAX_CHANNELS; j++) { - RaspilotAnalogSource *source = _channels[j]; + AnalogSource_Raspilot *source = _channels[j]; if (source != NULL && i == source->_pin) { source->_value = _dma_packet_rx.regs[i] * 3.3 / 4096.0; diff --git a/libraries/AP_HAL_Linux/RaspilotAnalogIn.h b/libraries/AP_HAL_Linux/AnalogIn_Raspilot.h similarity index 78% rename from libraries/AP_HAL_Linux/RaspilotAnalogIn.h rename to libraries/AP_HAL_Linux/AnalogIn_Raspilot.h index 79c79fca59..d62f150c3f 100644 --- a/libraries/AP_HAL_Linux/RaspilotAnalogIn.h +++ b/libraries/AP_HAL_Linux/AnalogIn_Raspilot.h @@ -1,15 +1,15 @@ -#ifndef __RaspilotAnalogIn_H__ -#define __RaspilotAnalogIn_H__ +#pragma once -#include "AP_HAL_Linux.h" #include +#include "AP_HAL_Linux.h" + #define RASPILOT_ADC_MAX_CHANNELS 8 -class RaspilotAnalogSource: public AP_HAL::AnalogSource { +class AnalogSource_Raspilot: public AP_HAL::AnalogSource { public: friend class RaspilotAnalogIn; - RaspilotAnalogSource(int16_t pin); + AnalogSource_Raspilot(int16_t pin); float read_average(); float read_latest(); void set_pin(uint8_t p); @@ -31,15 +31,15 @@ public: /* Board voltage is not available */ float board_voltage(void); - + protected: AP_HAL::AnalogSource *_vcc_pin_analog_source; - + private: AP_HAL::SPIDeviceDriver *_spi; AP_HAL::Semaphore *_spi_sem; - - RaspilotAnalogSource *_channels[RASPILOT_ADC_MAX_CHANNELS]; + + AnalogSource_Raspilot *_channels[RASPILOT_ADC_MAX_CHANNELS]; uint8_t _channels_number; @@ -47,5 +47,3 @@ private: uint32_t _last_update_timestamp; }; - -#endif