ardupilot/libraries/AP_HAL_Linux/RCOutput_Sysfs.cpp

153 lines
3.6 KiB
C++

/*
* Copyright (C) 2015 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_Sysfs.h"
#include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h>
namespace Linux {
RCOutput_Sysfs::RCOutput_Sysfs(uint8_t chip, uint8_t channel_base, uint8_t channel_count)
: _chip(chip)
, _channel_base(channel_base)
, _channel_count(channel_count)
, _pwm_channels(new PWM_Sysfs_Base *[_channel_count])
, _pending(new uint16_t[_channel_count])
{
}
RCOutput_Sysfs::~RCOutput_Sysfs()
{
for (uint8_t i = 0; i < _channel_count; i++) {
delete _pwm_channels[i];
}
delete [] _pwm_channels;
}
void RCOutput_Sysfs::init()
{
for (uint8_t i = 0; i < _channel_count; i++) {
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO
_pwm_channels[i] = new PWM_Sysfs_Bebop(_channel_base+i);
#else
_pwm_channels[i] = new PWM_Sysfs(_chip, _channel_base+i);
#endif
if (!_pwm_channels[i]) {
AP_HAL::panic("RCOutput_Sysfs_PWM: Unable to setup PWM pin.");
}
_pwm_channels[i]->init();
_pwm_channels[i]->enable(false);
/* Set the initial frequency */
_pwm_channels[i]->set_freq(50);
_pwm_channels[i]->set_duty_cycle(0);
_pwm_channels[i]->set_polarity(PWM_Sysfs::Polarity::NORMAL);
}
}
void RCOutput_Sysfs::set_freq(uint32_t chmask, uint16_t freq_hz)
{
for (uint8_t i = 0; i < _channel_count; i++) {
if (chmask & 1 << i) {
_pwm_channels[i]->set_freq(freq_hz);
}
}
}
uint16_t RCOutput_Sysfs::get_freq(uint8_t ch)
{
if (ch >= _channel_count) {
return 0;
}
return _pwm_channels[ch]->get_freq();
}
void RCOutput_Sysfs::enable_ch(uint8_t ch)
{
if (ch >= _channel_count) {
return;
}
_pwm_channels[ch]->enable(true);
}
void RCOutput_Sysfs::disable_ch(uint8_t ch)
{
if (ch >= _channel_count) {
return;
}
_pwm_channels[ch]->enable(false);
}
void RCOutput_Sysfs::write(uint8_t ch, uint16_t period_us)
{
if (ch >= _channel_count) {
return;
}
if (_corked) {
_pending[ch] = period_us;
_pending_mask |= (1U<<ch);
} else {
_pwm_channels[ch]->set_duty_cycle(usec_to_nsec(period_us));
}
}
uint16_t RCOutput_Sysfs::read(uint8_t ch)
{
if (ch >= _channel_count) {
return 1000;
}
return nsec_to_usec(_pwm_channels[ch]->get_duty_cycle());
}
void RCOutput_Sysfs::read(uint16_t *period_us, uint8_t len)
{
for (int i = 0; i < MIN(len, _channel_count); i++) {
period_us[i] = read(i);
}
for (int i = _channel_count; i < len; i++) {
period_us[i] = 1000;
}
}
void RCOutput_Sysfs::cork(void)
{
_corked = true;
}
void RCOutput_Sysfs::push(void)
{
if (!_corked) {
return;
}
for (uint8_t i=0; i<_channel_count; i++) {
if ((1U<<i) & _pending_mask) {
_pwm_channels[i]->set_duty_cycle(usec_to_nsec(_pending[i]));
}
}
_pending_mask = 0;
_corked = false;
}
}