Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_HAL_Linux: RCOutput_Raspilot: use SPIDevice interface

This commit is contained in:
Luiz Ywata 2016-04-26 14:34:25 -03:00 committed by Lucas De Marchi
parent 86ac9bc367
commit bfd840c5ca
2 changed files with 15 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
libraries/AP_HAL_Linux/RCOutput_Raspilot.cpp
View File

@ -25,21 +25,14 @@ static const AP_HAL::HAL& hal = AP_HAL::get_HAL();
void RCOutput_Raspilot::init()
{
_spi = hal.spi->device(AP_HAL::SPIDevice_RASPIO);
_spi_sem = _spi->get_semaphore();
if (_spi_sem == NULL) {
AP_HAL::panic("PANIC: RCOutput_Raspilot did not get "
"valid SPI semaphore!");
return; // never reached
}
_dev = hal.spi->get_device("raspio");
hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&RCOutput_Raspilot::_update, void));
}
void RCOutput_Raspilot::set_freq(uint32_t chmask, uint16_t freq_hz)
{
if (!_spi_sem->take(10)) {
if (!_dev->get_semaphore()->take(10)) {
return;
}
@ -51,11 +44,12 @@ void RCOutput_Raspilot::set_freq(uint32_t chmask, uint16_t freq_hz)
_dma_packet_tx.regs[0] = freq_hz;
_dma_packet_tx.crc = 0;
_dma_packet_tx.crc = crc_packet(&_dma_packet_tx);
_spi->transaction((uint8_t *)&_dma_packet_tx, (uint8_t *)&_dma_packet_rx, sizeof(_dma_packet_tx));
_dev->transfer((uint8_t *)&_dma_packet_tx, sizeof(_dma_packet_tx),
(uint8_t *)&_dma_packet_rx, sizeof(_dma_packet_rx));
_frequency = freq_hz;
_spi_sem->give();
_dev->get_semaphore()->give();
}
uint16_t RCOutput_Raspilot::get_freq(uint8_t ch)
@ -107,7 +101,7 @@ void RCOutput_Raspilot::_update(void)
_last_update_timestamp = AP_HAL::micros();
if (!_spi_sem->take_nonblocking()) {
if (!_dev->get_semaphore()->take_nonblocking()) {
return;
}
@ -119,7 +113,8 @@ void RCOutput_Raspilot::_update(void)
_dma_packet_tx.regs[0] = 75;
_dma_packet_tx.crc = 0;
_dma_packet_tx.crc = crc_packet(&_dma_packet_tx);
_spi->transaction((uint8_t *)&_dma_packet_tx, (uint8_t *)&_dma_packet_rx, sizeof(_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;
@ -128,7 +123,8 @@ void RCOutput_Raspilot::_update(void)
_dma_packet_tx.regs[0] = 0x560B;
_dma_packet_tx.crc = 0;
_dma_packet_tx.crc = crc_packet(&_dma_packet_tx);
_spi->transaction((uint8_t *)&_dma_packet_tx, (uint8_t *)&_dma_packet_rx, sizeof(_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;
@ -139,9 +135,10 @@ void RCOutput_Raspilot::_update(void)
}
_dma_packet_tx.crc = 0;
_dma_packet_tx.crc = crc_packet(&_dma_packet_tx);
_spi->transaction((uint8_t *)&_dma_packet_tx, (uint8_t *)&_dma_packet_rx, sizeof(_dma_packet_tx));
_dev->transfer((uint8_t *)&_dma_packet_tx, sizeof(_dma_packet_tx),
(uint8_t *)&_dma_packet_rx, sizeof(_dma_packet_rx));
_spi_sem->give();
_dev->get_semaphore()->give();
}
#endif // CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT

4
libraries/AP_HAL_Linux/RCOutput_Raspilot.h
View File

@ -1,6 +1,7 @@
#pragma once
#include "AP_HAL_Linux.h"
#include <AP_HAL/SPIDevice.h>
class Linux::RCOutput_Raspilot : public AP_HAL::RCOutput {
void init();
@ -16,8 +17,7 @@ private:
void reset();
void _update(void);
AP_HAL::SPIDeviceDriver *_spi;
AP_HAL::Semaphore *_spi_sem;
AP_HAL::OwnPtr<AP_HAL::SPIDevice> _dev;
uint32_t _last_update_timestamp;
uint16_t _frequency;