diff --git a/libraries/AP_HAL_Linux/RCOutput_AeroIO.cpp b/libraries/AP_HAL_Linux/RCOutput_AeroIO.cpp new file mode 100644 index 0000000000..e36ada977b --- /dev/null +++ b/libraries/AP_HAL_Linux/RCOutput_AeroIO.cpp @@ -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 . + */ +#include "RCOutput_AeroIO.h" + +#include + +#include +#include +#include + +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); +} diff --git a/libraries/AP_HAL_Linux/RCOutput_AeroIO.h b/libraries/AP_HAL_Linux/RCOutput_AeroIO.h new file mode 100644 index 0000000000..1592591fa9 --- /dev/null +++ b/libraries/AP_HAL_Linux/RCOutput_AeroIO.h @@ -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 . + */ + +#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 _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 true the push must be called to perform the writing, + * otherwise will be not necessary. + */ + bool _corking = false; +}; + +}