2015-08-18 00:29:58 -03:00
|
|
|
|
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
|
|
|
|
|
|
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
|
|
|
|
#include "RCOutput_Raspilot.h"
|
2016-05-17 23:26:57 -03:00
|
|
|
|
|
|
|
#include <cmath>
|
2015-08-18 00:29:58 -03:00
|
|
|
#include <dirent.h>
|
2016-05-17 23:26:57 -03:00
|
|
|
#include <fcntl.h>
|
2015-08-18 00:29:58 -03:00
|
|
|
#include <stdint.h>
|
2016-05-17 23:26:57 -03:00
|
|
|
#include <stdio.h>
|
|
|
|
#include <stdlib.h>
|
|
|
|
#include <sys/stat.h>
|
|
|
|
#include <sys/types.h>
|
|
|
|
#include <unistd.h>
|
|
|
|
|
2015-08-18 00:29:58 -03:00
|
|
|
#include "px4io_protocol.h"
|
2016-05-17 23:26:57 -03:00
|
|
|
#include "GPIO.h"
|
2015-08-18 00:29:58 -03:00
|
|
|
|
|
|
|
using namespace Linux;
|
|
|
|
|
|
|
|
#define PWM_CHAN_COUNT 8
|
|
|
|
|
2015-10-16 17:22:11 -03:00
|
|
|
static const AP_HAL::HAL& hal = AP_HAL::get_HAL();
|
2015-08-18 00:29:58 -03:00
|
|
|
|
2015-12-02 11:14:20 -04:00
|
|
|
void RCOutput_Raspilot::init()
|
2015-08-18 00:29:58 -03:00
|
|
|
{
|
2016-04-26 14:34:25 -03:00
|
|
|
_dev = hal.spi->get_device("raspio");
|
2016-05-17 23:26:57 -03:00
|
|
|
|
2015-10-20 18:13:25 -03:00
|
|
|
hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&RCOutput_Raspilot::_update, void));
|
2015-08-18 00:29:58 -03:00
|
|
|
}
|
|
|
|
|
2015-10-20 18:13:25 -03:00
|
|
|
void RCOutput_Raspilot::set_freq(uint32_t chmask, uint16_t freq_hz)
|
2016-05-17 23:26:57 -03:00
|
|
|
{
|
2016-04-26 14:34:25 -03:00
|
|
|
if (!_dev->get_semaphore()->take(10)) {
|
2015-08-18 00:29:58 -03:00
|
|
|
return;
|
|
|
|
}
|
2016-05-17 23:26:57 -03:00
|
|
|
|
2015-08-18 00:29:58 -03:00
|
|
|
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] = freq_hz;
|
|
|
|
_dma_packet_tx.crc = 0;
|
|
|
|
_dma_packet_tx.crc = crc_packet(&_dma_packet_tx);
|
2016-04-26 14:34:25 -03:00
|
|
|
_dev->transfer((uint8_t *)&_dma_packet_tx, sizeof(_dma_packet_tx),
|
|
|
|
(uint8_t *)&_dma_packet_rx, sizeof(_dma_packet_rx));
|
2016-05-17 23:26:57 -03:00
|
|
|
|
2015-08-18 00:29:58 -03:00
|
|
|
_frequency = freq_hz;
|
2016-05-17 23:26:57 -03:00
|
|
|
|
2016-04-26 14:34:25 -03:00
|
|
|
_dev->get_semaphore()->give();
|
2015-08-18 00:29:58 -03:00
|
|
|
}
|
|
|
|
|
2015-10-20 18:13:25 -03:00
|
|
|
uint16_t RCOutput_Raspilot::get_freq(uint8_t ch)
|
2015-08-18 00:29:58 -03:00
|
|
|
{
|
|
|
|
return _frequency;
|
|
|
|
}
|
|
|
|
|
2015-10-20 18:13:25 -03:00
|
|
|
void RCOutput_Raspilot::enable_ch(uint8_t ch)
|
2015-08-18 00:29:58 -03:00
|
|
|
{
|
2016-05-17 23:26:57 -03:00
|
|
|
|
2015-08-18 00:29:58 -03:00
|
|
|
}
|
|
|
|
|
2015-10-20 18:13:25 -03:00
|
|
|
void RCOutput_Raspilot::disable_ch(uint8_t ch)
|
2015-08-18 00:29:58 -03:00
|
|
|
{
|
|
|
|
write(ch, 0);
|
|
|
|
}
|
|
|
|
|
2015-10-20 18:13:25 -03:00
|
|
|
void RCOutput_Raspilot::write(uint8_t ch, uint16_t period_us)
|
2016-05-17 23:26:57 -03:00
|
|
|
{
|
2015-08-18 00:29:58 -03:00
|
|
|
if(ch >= PWM_CHAN_COUNT){
|
|
|
|
return;
|
|
|
|
}
|
2016-05-17 23:26:57 -03:00
|
|
|
|
2015-08-18 00:29:58 -03:00
|
|
|
_period_us[ch] = period_us;
|
|
|
|
}
|
|
|
|
|
2015-10-20 18:13:25 -03:00
|
|
|
uint16_t RCOutput_Raspilot::read(uint8_t ch)
|
2015-08-18 00:29:58 -03:00
|
|
|
{
|
|
|
|
if(ch >= PWM_CHAN_COUNT){
|
|
|
|
return 0;
|
|
|
|
}
|
2016-05-17 23:26:57 -03:00
|
|
|
|
2015-08-18 00:29:58 -03:00
|
|
|
return _period_us[ch];
|
|
|
|
}
|
|
|
|
|
2015-10-20 18:13:25 -03:00
|
|
|
void RCOutput_Raspilot::read(uint16_t* period_us, uint8_t len)
|
2015-08-18 00:29:58 -03:00
|
|
|
{
|
2016-05-17 23:26:57 -03:00
|
|
|
for (int i = 0; i < len; i++)
|
2015-08-18 00:29:58 -03:00
|
|
|
period_us[i] = read(0 + i);
|
|
|
|
}
|
|
|
|
|
2015-10-20 18:13:25 -03:00
|
|
|
void RCOutput_Raspilot::_update(void)
|
2015-08-18 00:29:58 -03:00
|
|
|
{
|
|
|
|
int i;
|
2016-05-17 23:26:57 -03:00
|
|
|
|
2015-11-19 23:10:58 -04:00
|
|
|
if (AP_HAL::micros() - _last_update_timestamp < 10000) {
|
2015-08-18 00:29:58 -03:00
|
|
|
return;
|
|
|
|
}
|
2016-05-17 23:26:57 -03:00
|
|
|
|
2015-11-19 23:10:58 -04:00
|
|
|
_last_update_timestamp = AP_HAL::micros();
|
2016-05-17 23:26:57 -03:00
|
|
|
|
2016-04-26 14:34:25 -03:00
|
|
|
if (!_dev->get_semaphore()->take_nonblocking()) {
|
2015-08-18 00:29:58 -03:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
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);
|
2016-04-26 14:34:25 -03:00
|
|
|
_dev->transfer((uint8_t *)&_dma_packet_tx, sizeof(_dma_packet_tx),
|
|
|
|
(uint8_t *)&_dma_packet_rx, sizeof(_dma_packet_rx));
|
2016-05-17 23:26:57 -03:00
|
|
|
|
2015-08-18 00:29:58 -03:00
|
|
|
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);
|
2016-04-26 14:34:25 -03:00
|
|
|
_dev->transfer((uint8_t *)&_dma_packet_tx, sizeof(_dma_packet_tx),
|
|
|
|
(uint8_t *)&_dma_packet_rx, sizeof(_dma_packet_rx));
|
2016-05-17 23:26:57 -03:00
|
|
|
|
2015-08-18 00:29:58 -03:00
|
|
|
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);
|
2016-04-26 14:34:25 -03:00
|
|
|
_dev->transfer((uint8_t *)&_dma_packet_tx, sizeof(_dma_packet_tx),
|
|
|
|
(uint8_t *)&_dma_packet_rx, sizeof(_dma_packet_rx));
|
2016-05-17 23:26:57 -03:00
|
|
|
|
2016-04-26 14:34:25 -03:00
|
|
|
_dev->get_semaphore()->give();
|
2015-08-18 00:29:58 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
#endif // CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
|