2 changed files with 437 additions and 0 deletions
@ -0,0 +1,281 @@ |
|||||||
|
/// -- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil --
|
||||||
|
/*
|
||||||
|
* Copyright (C) 2016 Intel Corporation. All rights reserved. |
||||||
|
* |
||||||
|
* This file is free software: you can redistribute it and/or modify it |
||||||
|
* under the terms of the GNU General Public License as published by the |
||||||
|
* Free Software Foundation, either version 3 of the License, or |
||||||
|
* (at your option) any later version. |
||||||
|
* |
||||||
|
* This file is distributed in the hope that it will be useful, but |
||||||
|
* WITHOUT ANY WARRANTY; without even the implied warranty of |
||||||
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. |
||||||
|
* See the GNU General Public License for more details. |
||||||
|
* |
||||||
|
* You should have received a copy of the GNU General Public License along |
||||||
|
* with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||||
|
*/ |
||||||
|
#include "RCOutput_AeroIO.h" |
||||||
|
|
||||||
|
#include <utility> |
||||||
|
|
||||||
|
#include <AP_HAL/AP_HAL.h> |
||||||
|
#include <AP_HAL/utility/sparse-endian.h> |
||||||
|
#include <AP_Math/AP_Math.h> |
||||||
|
|
||||||
|
using namespace Linux; |
||||||
|
|
||||||
|
// Device name in @SPIDeviceDriver#_device
|
||||||
|
#define DEVICE_NAME "aeroio" |
||||||
|
|
||||||
|
// Number of channels
|
||||||
|
#define PWM_CHAN_COUNT 16 |
||||||
|
|
||||||
|
// Set all channels
|
||||||
|
#define ALL_CHAN_MASK ((1 << PWM_CHAN_COUNT) - 1) |
||||||
|
|
||||||
|
// Default PWM frequency
|
||||||
|
#define DEFAULT_FREQ 400 |
||||||
|
|
||||||
|
// Default PWM duty cycle
|
||||||
|
#define DEFAULT_DUTY 0 |
||||||
|
|
||||||
|
// Set or Clear MSb of BYTE
|
||||||
|
#define WADDRESS(x) ((x) | 0x8000) |
||||||
|
#define RADDRESS(x) ((x) & 0x7FFF) |
||||||
|
|
||||||
|
// Variables to perform ongoing tests
|
||||||
|
#define READ_PREFIX 0x80 |
||||||
|
#define WRITE_PREFIX 0x40 |
||||||
|
|
||||||
|
/**
|
||||||
|
* The data_array uses 3 elements to perform the data transaction. |
||||||
|
* The first element is a data byte that provides to FPGA's hardware |
||||||
|
* the transaction type that will be realized inside the SPI module. |
||||||
|
* Where: |
||||||
|
* |
||||||
|
* ╔═════════╦═════════╦══════════╦══════════╦══════════╦══════════╦══════════╦═══════════╗ |
||||||
|
* ║ MSB ║ ║ ║ ║ ║ ║ ║ LSB ║ |
||||||
|
* ╠═════════╬═════════╬══════════╬══════════╬══════════╬══════════╬══════════╬═══════════╣ |
||||||
|
* ║ wr_addr ║ rd_addr ║ reserved ║ reserved ║ reserved ║ reserved ║ reserved ║ reserved ║ |
||||||
|
* ╚═════════╩═════════╩══════════╩══════════╩══════════╩══════════╩══════════╩═══════════╝ |
||||||
|
* |
||||||
|
* ╔═══════════╦═════════╦═════════╗ |
||||||
|
* ║ Register ║ wr_addr ║ rd_addr ║ |
||||||
|
* ╠═══════════╬═════════╬═════════╣ |
||||||
|
* ║ write ║ 0 ║ X ║ |
||||||
|
* ╠═══════════╬═════════╬═════════╣ |
||||||
|
* ║ read ║ X ║ 0 ║ |
||||||
|
* ╠═══════════╬═════════╬═════════╣ |
||||||
|
* ║ status ║ 1 ║ 1 ║ |
||||||
|
* ╚═══════════╩═════════╩═════════╝ |
||||||
|
* |
||||||
|
* So, to perform a write transaction in the SPI module it's necessary to send. E.g: |
||||||
|
* 0b 01xx xxxx |
||||||
|
* And to a read transaction.. |
||||||
|
* 0b 10xx xxxx |
||||||
|
* |
||||||
|
* The PWM frequency is always even and the duty cycle percentage odd. E.g: |
||||||
|
* pwm_01: Address 0x0000 frequency |
||||||
|
* : Address 0x0001 duty cycle |
||||||
|
* pwm_02: Address 0x0002 frequency |
||||||
|
* . |
||||||
|
* . |
||||||
|
* . |
||||||
|
* |
||||||
|
* Eg of allowed values: |
||||||
|
* // PWM channel in 100Hz
|
||||||
|
* uint16_t freq = 100; |
||||||
|
* |
||||||
|
* // duty cycle in (1823/65535) that's 2.78% of 100Hz:
|
||||||
|
* // the signal will hold high until 278 usec
|
||||||
|
* uint16_t duty = 1823; |
||||||
|
*/ |
||||||
|
|
||||||
|
static const AP_HAL::HAL &hal = AP_HAL::get_HAL(); |
||||||
|
|
||||||
|
RCOutput_AeroIO::RCOutput_AeroIO() |
||||||
|
: _freq_buffer(new uint16_t[PWM_CHAN_COUNT]) |
||||||
|
, _duty_buffer(new uint16_t[PWM_CHAN_COUNT]) |
||||||
|
{ |
||||||
|
} |
||||||
|
|
||||||
|
RCOutput_AeroIO::~RCOutput_AeroIO() |
||||||
|
{ |
||||||
|
delete _freq_buffer; |
||||||
|
delete _duty_buffer; |
||||||
|
} |
||||||
|
|
||||||
|
void RCOutput_AeroIO::init() |
||||||
|
{ |
||||||
|
_spi = std::move(hal.spi->get_device(DEVICE_NAME)); |
||||||
|
if (!_spi) { |
||||||
|
AP_HAL::panic("Could not initialize AeroIO"); |
||||||
|
} |
||||||
|
|
||||||
|
// Reset all channels to default value
|
||||||
|
cork(); |
||||||
|
set_freq(ALL_CHAN_MASK, DEFAULT_FREQ); |
||||||
|
for (uint8_t i = 0; i < PWM_CHAN_COUNT; i++) { |
||||||
|
write(i, DEFAULT_DUTY); |
||||||
|
} |
||||||
|
push(); |
||||||
|
} |
||||||
|
|
||||||
|
void RCOutput_AeroIO::set_freq(uint32_t chmask, uint16_t freq_hz) |
||||||
|
{ |
||||||
|
_pending_freq_write_mask |= chmask; |
||||||
|
|
||||||
|
for (uint8_t i = 0; i < PWM_CHAN_COUNT; i++) { |
||||||
|
if ((chmask >> i) & 0x01) { |
||||||
|
_freq_buffer[i] = freq_hz; |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
if (!_corking) { |
||||||
|
push(); |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
uint16_t RCOutput_AeroIO::get_freq(uint8_t ch) |
||||||
|
{ |
||||||
|
if (ch >= PWM_CHAN_COUNT) { |
||||||
|
return 0; |
||||||
|
} |
||||||
|
return _freq_buffer[ch]; |
||||||
|
} |
||||||
|
|
||||||
|
void RCOutput_AeroIO::enable_ch(uint8_t ch) |
||||||
|
{ |
||||||
|
if (ch >= PWM_CHAN_COUNT) { |
||||||
|
return; |
||||||
|
} |
||||||
|
_pending_duty_write_mask |= (1U << ch); |
||||||
|
push(); |
||||||
|
} |
||||||
|
|
||||||
|
void RCOutput_AeroIO::disable_ch(uint8_t ch) |
||||||
|
{ |
||||||
|
if (ch >= PWM_CHAN_COUNT) { |
||||||
|
return; |
||||||
|
} |
||||||
|
_duty_buffer[ch] = 0; |
||||||
|
_pending_duty_write_mask |= (1U << ch); |
||||||
|
push(); |
||||||
|
} |
||||||
|
|
||||||
|
void RCOutput_AeroIO::write(uint8_t ch, uint16_t period_us) |
||||||
|
{ |
||||||
|
_pending_duty_write_mask |= (1U << ch); |
||||||
|
_duty_buffer[ch] = period_us; |
||||||
|
|
||||||
|
if (!_corking) { |
||||||
|
push(); |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
void RCOutput_AeroIO::cork() |
||||||
|
{ |
||||||
|
_corking = true; |
||||||
|
} |
||||||
|
|
||||||
|
void RCOutput_AeroIO::push() |
||||||
|
{ |
||||||
|
_corking = false; |
||||||
|
|
||||||
|
for (uint8_t i = 0; i < PWM_CHAN_COUNT; i++) { |
||||||
|
if ((_pending_freq_write_mask >> i) & 0x01) { |
||||||
|
_hw_write(2 * i + 1, _freq_buffer[i]); |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
for (uint8_t i = 0; i < PWM_CHAN_COUNT; i++) { |
||||||
|
if ((_pending_duty_write_mask >> i) & 0x01) { |
||||||
|
_hw_write(2 * i, _usec_to_hw(_freq_buffer[i], _duty_buffer[i])); |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
_pending_freq_write_mask = _pending_duty_write_mask = 0; |
||||||
|
} |
||||||
|
|
||||||
|
uint16_t RCOutput_AeroIO::read(uint8_t ch) |
||||||
|
{ |
||||||
|
if (ch >= PWM_CHAN_COUNT) { |
||||||
|
return 0; |
||||||
|
} |
||||||
|
#ifndef AEROIO_DEBUG |
||||||
|
return _duty_buffer[ch]; |
||||||
|
#else |
||||||
|
return _hw_to_usec(_freq_buffer[ch], _hw_read(2 * ch)); |
||||||
|
#endif |
||||||
|
} |
||||||
|
|
||||||
|
void RCOutput_AeroIO::read(uint16_t *period_us, uint8_t len) |
||||||
|
{ |
||||||
|
for (uint8_t i = 0; i < len; i++) { |
||||||
|
period_us[i] = read(i); |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
bool RCOutput_AeroIO::_hw_write(uint16_t address, uint16_t data) |
||||||
|
{ |
||||||
|
struct PACKED { |
||||||
|
uint8_t prefix; |
||||||
|
be16_t addr; |
||||||
|
be16_t val; |
||||||
|
} tx; |
||||||
|
|
||||||
|
address = WADDRESS(address); |
||||||
|
|
||||||
|
tx.prefix = WRITE_PREFIX; |
||||||
|
tx.addr = htobe16(address); |
||||||
|
tx.val = htobe16(data); |
||||||
|
|
||||||
|
return _spi->transfer((uint8_t *)&tx, sizeof(tx), nullptr, 0); |
||||||
|
} |
||||||
|
|
||||||
|
uint16_t RCOutput_AeroIO::_hw_read(uint16_t address) |
||||||
|
{ |
||||||
|
struct PACKED { |
||||||
|
uint8_t prefix; |
||||||
|
be16_t addr; |
||||||
|
} tx; |
||||||
|
struct PACKED { |
||||||
|
uint8_t ignored[2]; |
||||||
|
be16_t val; |
||||||
|
} rx; |
||||||
|
|
||||||
|
address = RADDRESS(address); |
||||||
|
|
||||||
|
// Write in the SPI buffer the address value
|
||||||
|
tx.prefix = WRITE_PREFIX; |
||||||
|
tx.addr = htobe16(address); |
||||||
|
if (!_spi->transfer((uint8_t *)&tx, sizeof(tx), nullptr, 0)) { |
||||||
|
return 0; |
||||||
|
} |
||||||
|
|
||||||
|
/*
|
||||||
|
* Read the SPI buffer, sending only the prefix as tx |
||||||
|
* The hardware will fill in 32 bits after the request |
||||||
|
*/ |
||||||
|
tx.prefix = READ_PREFIX; |
||||||
|
if (!_spi->transfer((uint8_t *)&tx, 1, (uint8_t *)&rx, sizeof(rx))) { |
||||||
|
return 0; |
||||||
|
} |
||||||
|
|
||||||
|
return be16toh(rx.val); |
||||||
|
} |
||||||
|
|
||||||
|
uint16_t RCOutput_AeroIO::_usec_to_hw(uint16_t freq, uint16_t usec) |
||||||
|
{ |
||||||
|
float f = freq; |
||||||
|
float u = usec; |
||||||
|
return 0xFFFF * u * f / USEC_PER_SEC; |
||||||
|
} |
||||||
|
|
||||||
|
uint16_t RCOutput_AeroIO::_hw_to_usec(uint16_t freq, uint16_t hw_val) |
||||||
|
{ |
||||||
|
float p = hw_val; |
||||||
|
float f = freq; |
||||||
|
return p * USEC_PER_SEC / (0xFFFF * f); |
||||||
|
} |
@ -0,0 +1,156 @@ |
|||||||
|
/// -- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil --
|
||||||
|
/*
|
||||||
|
* Copyright (C) 2016 Intel Corporation. All rights reserved. |
||||||
|
* |
||||||
|
* This file is free software: you can redistribute it and/or modify it |
||||||
|
* under the terms of the GNU General Public License as published by the |
||||||
|
* Free Software Foundation, either version 3 of the License, or |
||||||
|
* (at your option) any later version. |
||||||
|
* |
||||||
|
* This file is distributed in the hope that it will be useful, but |
||||||
|
* WITHOUT ANY WARRANTY; without even the implied warranty of |
||||||
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. |
||||||
|
* See the GNU General Public License for more details. |
||||||
|
* |
||||||
|
* You should have received a copy of the GNU General Public License along |
||||||
|
* with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||||
|
*/ |
||||||
|
|
||||||
|
#pragma once |
||||||
|
|
||||||
|
#include "AP_HAL_Linux.h" |
||||||
|
|
||||||
|
namespace Linux { |
||||||
|
|
||||||
|
class RCOutput_AeroIO : public AP_HAL::RCOutput { |
||||||
|
public: |
||||||
|
RCOutput_AeroIO(); |
||||||
|
~RCOutput_AeroIO(); |
||||||
|
|
||||||
|
void init() override; |
||||||
|
|
||||||
|
/**
|
||||||
|
* Enable channel |
||||||
|
*/ |
||||||
|
void enable_ch(uint8_t ch) override; |
||||||
|
|
||||||
|
/**
|
||||||
|
* Disable channel (0 of duty cycle) |
||||||
|
*/ |
||||||
|
void disable_ch(uint8_t ch) override; |
||||||
|
|
||||||
|
/**
|
||||||
|
* Set all channels in the same frequency |
||||||
|
* |
||||||
|
* @chmask Bitmask |
||||||
|
* @freq_hz Frequency |
||||||
|
*/ |
||||||
|
void set_freq(uint32_t chmask, uint16_t freq_hz) override; |
||||||
|
|
||||||
|
/**
|
||||||
|
* Get frequency of channel |
||||||
|
* |
||||||
|
* @ch channel |
||||||
|
* |
||||||
|
* Return: frequency of this channel |
||||||
|
*/ |
||||||
|
uint16_t get_freq(uint8_t ch) override; |
||||||
|
|
||||||
|
/**
|
||||||
|
* Set period in µs |
||||||
|
* |
||||||
|
* @ch channel |
||||||
|
* @period_us time in µs |
||||||
|
*/ |
||||||
|
void write(uint8_t ch, uint16_t period_us) override; |
||||||
|
|
||||||
|
void cork() override; |
||||||
|
|
||||||
|
void push() override; |
||||||
|
|
||||||
|
/**
|
||||||
|
* Get period of the duty cycle in µs |
||||||
|
*/ |
||||||
|
uint16_t read(uint8_t ch) override; |
||||||
|
|
||||||
|
/**
|
||||||
|
* Set @period_us with the values in µs of each channel |
||||||
|
* |
||||||
|
* @period_us vector that will be filled with duty cycle periods of |
||||||
|
* each channel |
||||||
|
* @len size of period_us vector |
||||||
|
*/ |
||||||
|
void read(uint16_t *period_us, uint8_t len) override; |
||||||
|
|
||||||
|
private: |
||||||
|
/**
|
||||||
|
* Convert from µs to hw units, 16bits percentage of |
||||||
|
* the frequency, where 0xFFFF is 1/Freq seconds in high |
||||||
|
* |
||||||
|
* @freq Frequency |
||||||
|
* @usec Time in µseconds |
||||||
|
* |
||||||
|
* Return: conversion from µs in a specific frequency to 16bits |
||||||
|
*/ |
||||||
|
static uint16_t _usec_to_hw(uint16_t freq, uint16_t usec); |
||||||
|
|
||||||
|
/**
|
||||||
|
* Convert from hw units, 16bits percentage of the frequency, to |
||||||
|
* time in µs |
||||||
|
* |
||||||
|
* @freq Frequency |
||||||
|
* @hw_val 16b percentage |
||||||
|
*/ |
||||||
|
static uint16_t _hw_to_usec(uint16_t freq, uint16_t hw_val); |
||||||
|
|
||||||
|
/**
|
||||||
|
* Low-level spi write |
||||||
|
* |
||||||
|
* @address register address |
||||||
|
* @data value to be written |
||||||
|
* |
||||||
|
* Return: true on success, false otherwise |
||||||
|
*/ |
||||||
|
bool _hw_write(uint16_t address, uint16_t data); |
||||||
|
|
||||||
|
/**
|
||||||
|
* Low-level spi read |
||||||
|
* |
||||||
|
* @address register address |
||||||
|
* |
||||||
|
* Return: value read from @address |
||||||
|
*/ |
||||||
|
uint16_t _hw_read(uint16_t address); |
||||||
|
|
||||||
|
AP_HAL::OwnPtr<AP_HAL::Device> _spi; |
||||||
|
|
||||||
|
/**
|
||||||
|
* Frequency buffer of last written values |
||||||
|
*/ |
||||||
|
uint16_t *_freq_buffer; |
||||||
|
|
||||||
|
/**
|
||||||
|
* Duty cycle buffer of last written values |
||||||
|
*/ |
||||||
|
uint16_t *_duty_buffer; |
||||||
|
|
||||||
|
/**
|
||||||
|
* Save information about the channel that will be write in the |
||||||
|
* next @RCOutput_AeroIO#push call. |
||||||
|
* |
||||||
|
* 0b...101 |
||||||
|
* ││└── 1st channel (Pending operation) |
||||||
|
* │└─── 2nd Channel (No pending operation) |
||||||
|
* └──── 3rd Channel (Pending operation) |
||||||
|
*/ |
||||||
|
uint32_t _pending_duty_write_mask = 0; |
||||||
|
uint32_t _pending_freq_write_mask = 0; |
||||||
|
|
||||||
|
/**
|
||||||
|
* If <B>true</B> the push must be called to perform the writing, |
||||||
|
* otherwise will be not necessary. |
||||||
|
*/ |
||||||
|
bool _corking = false; |
||||||
|
}; |
||||||
|
|
||||||
|
} |
Loading…
Reference in new issue