2013-01-04 07:25:36 -04:00
|
|
|
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
|
|
|
2015-08-11 03:28:43 -03:00
|
|
|
#include <AP_HAL/AP_HAL.h>
|
2013-01-04 07:25:36 -04:00
|
|
|
|
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
|
|
|
|
#include "RCOutput.h"
|
|
|
|
|
|
|
|
#include <sys/types.h>
|
|
|
|
#include <sys/stat.h>
|
|
|
|
#include <fcntl.h>
|
|
|
|
#include <unistd.h>
|
|
|
|
|
|
|
|
#include <drivers/drv_pwm_output.h>
|
2014-11-20 03:31:41 -04:00
|
|
|
#include <uORB/topics/actuator_direct.h>
|
|
|
|
#include <drivers/drv_hrt.h>
|
2016-04-15 03:38:58 -03:00
|
|
|
#include <drivers/drv_pwm_output.h>
|
|
|
|
#include <drivers/drv_sbus.h>
|
2013-01-04 07:25:36 -04:00
|
|
|
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
|
|
|
|
using namespace PX4;
|
|
|
|
|
2016-04-13 03:28:50 -03:00
|
|
|
/*
|
|
|
|
enable RCOUT_DEBUG_LATENCY to measure output latency using a logic
|
|
|
|
analyser. AUX6 will go high during the cork/push output.
|
|
|
|
*/
|
|
|
|
#define RCOUT_DEBUG_LATENCY 0
|
|
|
|
|
2015-12-02 11:14:20 -04:00
|
|
|
void PX4RCOutput::init()
|
2013-01-04 07:25:36 -04:00
|
|
|
{
|
2013-01-25 04:16:28 -04:00
|
|
|
_perf_rcout = perf_alloc(PC_ELAPSED, "APM_rcout");
|
2015-02-13 05:21:03 -04:00
|
|
|
_pwm_fd = open(PWM_OUTPUT0_DEVICE_PATH, O_RDWR);
|
2013-01-04 07:25:36 -04:00
|
|
|
if (_pwm_fd == -1) {
|
2015-11-19 23:11:07 -04:00
|
|
|
AP_HAL::panic("Unable to open " PWM_OUTPUT0_DEVICE_PATH);
|
2013-01-04 07:25:36 -04:00
|
|
|
}
|
2013-01-27 00:05:28 -04:00
|
|
|
if (ioctl(_pwm_fd, PWM_SERVO_ARM, 0) != 0) {
|
2013-01-26 21:52:03 -04:00
|
|
|
hal.console->printf("RCOutput: Unable to setup IO arming\n");
|
|
|
|
}
|
2013-05-16 03:26:57 -03:00
|
|
|
if (ioctl(_pwm_fd, PWM_SERVO_SET_ARM_OK, 0) != 0) {
|
|
|
|
hal.console->printf("RCOutput: Unable to setup IO arming OK\n");
|
|
|
|
}
|
2016-04-15 03:38:58 -03:00
|
|
|
|
2013-04-25 06:59:30 -03:00
|
|
|
_rate_mask = 0;
|
|
|
|
_alt_fd = -1;
|
|
|
|
_servo_count = 0;
|
|
|
|
_alt_servo_count = 0;
|
|
|
|
|
|
|
|
if (ioctl(_pwm_fd, PWM_SERVO_GET_COUNT, (unsigned long)&_servo_count) != 0) {
|
|
|
|
hal.console->printf("RCOutput: Unable to get servo count\n");
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2015-06-03 22:00:34 -03:00
|
|
|
for (uint8_t i=0; i<ORB_MULTI_MAX_INSTANCES; i++) {
|
|
|
|
_outputs[i].pwm_sub = orb_subscribe_multi(ORB_ID(actuator_outputs), i);
|
|
|
|
}
|
2014-11-05 06:38:12 -04:00
|
|
|
|
2015-11-25 03:10:30 -04:00
|
|
|
#if !defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
|
2013-04-25 06:59:30 -03:00
|
|
|
_alt_fd = open("/dev/px4fmu", O_RDWR);
|
|
|
|
if (_alt_fd == -1) {
|
|
|
|
hal.console->printf("RCOutput: failed to open /dev/px4fmu");
|
|
|
|
return;
|
|
|
|
}
|
2015-11-25 03:10:30 -04:00
|
|
|
#endif
|
2014-08-27 06:52:07 -03:00
|
|
|
|
|
|
|
// ensure not to write zeros to disabled channels
|
|
|
|
_enabled_channels = 0;
|
2016-04-15 03:38:58 -03:00
|
|
|
for (uint8_t i=0; i < PX4_NUM_OUTPUT_CHANNELS; i++) {
|
2014-08-27 06:52:07 -03:00
|
|
|
_period[i] = PWM_IGNORE_THIS_CHANNEL;
|
|
|
|
}
|
2014-11-20 03:31:41 -04:00
|
|
|
|
|
|
|
// publish actuator vaules on demand
|
2015-05-31 01:49:27 -03:00
|
|
|
_actuator_direct_pub = NULL;
|
2014-11-20 03:31:41 -04:00
|
|
|
|
|
|
|
// and armed state
|
2015-05-31 01:49:27 -03:00
|
|
|
_actuator_armed_pub = NULL;
|
2014-01-19 21:56:30 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
void PX4RCOutput::_init_alt_channels(void)
|
|
|
|
{
|
|
|
|
if (_alt_fd == -1) {
|
|
|
|
return;
|
|
|
|
}
|
2013-04-25 06:59:30 -03:00
|
|
|
if (ioctl(_alt_fd, PWM_SERVO_ARM, 0) != 0) {
|
|
|
|
hal.console->printf("RCOutput: Unable to setup alt IO arming\n");
|
2014-01-19 21:56:30 -04:00
|
|
|
return;
|
2013-04-25 06:59:30 -03:00
|
|
|
}
|
2013-05-16 03:26:57 -03:00
|
|
|
if (ioctl(_alt_fd, PWM_SERVO_SET_ARM_OK, 0) != 0) {
|
2014-01-15 07:25:50 -04:00
|
|
|
hal.console->printf("RCOutput: Unable to setup alt IO arming OK\n");
|
2014-01-19 21:56:30 -04:00
|
|
|
return;
|
2013-05-16 03:26:57 -03:00
|
|
|
}
|
2013-04-25 06:59:30 -03:00
|
|
|
if (ioctl(_alt_fd, PWM_SERVO_GET_COUNT, (unsigned long)&_alt_servo_count) != 0) {
|
|
|
|
hal.console->printf("RCOutput: Unable to get servo count\n");
|
|
|
|
}
|
2013-01-04 07:25:36 -04:00
|
|
|
}
|
|
|
|
|
2016-01-04 16:17:44 -04:00
|
|
|
/*
|
|
|
|
set output frequency on outputs associated with fd
|
|
|
|
*/
|
|
|
|
void PX4RCOutput::set_freq_fd(int fd, uint32_t chmask, uint16_t freq_hz)
|
2013-01-04 07:25:36 -04:00
|
|
|
{
|
2016-01-04 16:17:44 -04:00
|
|
|
// we can't set this per channel
|
2016-04-22 00:24:24 -03:00
|
|
|
if (freq_hz > 50 || freq_hz == 1) {
|
2013-04-25 06:59:30 -03:00
|
|
|
// we're being asked to set the alt rate
|
2016-04-15 18:23:49 -03:00
|
|
|
if (_output_mode == MODE_PWM_ONESHOT) {
|
|
|
|
/*
|
|
|
|
set a 1Hz update for oneshot. This periodic output will
|
|
|
|
never actually trigger, instead we will directly trigger
|
|
|
|
the pulse after each push()
|
|
|
|
*/
|
|
|
|
freq_hz = 1;
|
|
|
|
}
|
2016-04-22 00:24:24 -03:00
|
|
|
//::printf("SET_UPDATE_RATE %u\n", (unsigned)freq_hz);
|
2016-01-04 16:17:44 -04:00
|
|
|
if (ioctl(fd, PWM_SERVO_SET_UPDATE_RATE, (unsigned long)freq_hz) != 0) {
|
2013-04-25 06:59:30 -03:00
|
|
|
hal.console->printf("RCOutput: Unable to set alt rate to %uHz\n", (unsigned)freq_hz);
|
|
|
|
return;
|
|
|
|
}
|
2013-01-05 07:45:17 -04:00
|
|
|
_freq_hz = freq_hz;
|
|
|
|
}
|
2013-04-25 06:59:30 -03:00
|
|
|
|
2016-01-04 16:17:44 -04:00
|
|
|
/* work out the new rate mask. The outputs have 3 groups of servos.
|
2013-04-25 06:59:30 -03:00
|
|
|
|
|
|
|
Group 0: channels 0 1
|
|
|
|
Group 1: channels 4 5 6 7
|
|
|
|
Group 2: channels 2 3
|
|
|
|
|
|
|
|
Channels within a group must be set to the same rate.
|
|
|
|
|
|
|
|
For the moment we never set the channels above 8 to more than
|
|
|
|
50Hz
|
|
|
|
*/
|
2016-04-22 00:24:24 -03:00
|
|
|
if (freq_hz > 50 || freq_hz == 1) {
|
2013-04-25 06:59:30 -03:00
|
|
|
// we are setting high rates on the given channels
|
|
|
|
_rate_mask |= chmask & 0xFF;
|
|
|
|
if (_rate_mask & 0x3) {
|
|
|
|
_rate_mask |= 0x3;
|
|
|
|
}
|
|
|
|
if (_rate_mask & 0xc) {
|
|
|
|
_rate_mask |= 0xc;
|
|
|
|
}
|
|
|
|
if (_rate_mask & 0xF0) {
|
|
|
|
_rate_mask |= 0xF0;
|
|
|
|
}
|
|
|
|
} else {
|
|
|
|
// we are setting low rates on the given channels
|
|
|
|
if (chmask & 0x3) {
|
|
|
|
_rate_mask &= ~0x3;
|
|
|
|
}
|
|
|
|
if (chmask & 0xc) {
|
|
|
|
_rate_mask &= ~0xc;
|
|
|
|
}
|
|
|
|
if (chmask & 0xf0) {
|
|
|
|
_rate_mask &= ~0xf0;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2016-04-22 00:24:24 -03:00
|
|
|
//::printf("SELECT_UPDATE_RATE 0x%02x\n", (unsigned)_rate_mask);
|
2016-01-04 16:17:44 -04:00
|
|
|
if (ioctl(fd, PWM_SERVO_SET_SELECT_UPDATE_RATE, _rate_mask) != 0) {
|
2013-04-25 06:59:30 -03:00
|
|
|
hal.console->printf("RCOutput: Unable to set alt rate mask to 0x%x\n", (unsigned)_rate_mask);
|
|
|
|
}
|
2013-01-04 07:25:36 -04:00
|
|
|
}
|
|
|
|
|
2016-01-04 16:17:44 -04:00
|
|
|
/*
|
|
|
|
set output frequency
|
|
|
|
*/
|
|
|
|
void PX4RCOutput::set_freq(uint32_t chmask, uint16_t freq_hz)
|
|
|
|
{
|
2016-05-25 23:18:40 -03:00
|
|
|
if (freq_hz > 50 && _output_mode == MODE_PWM_ONESHOT) {
|
|
|
|
// rate is irrelevent in oneshot
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2016-05-11 00:01:15 -03:00
|
|
|
// re-fetch servo count as it might have changed due to a change in BRD_PWM_COUNT
|
|
|
|
if (_pwm_fd != -1 && ioctl(_pwm_fd, PWM_SERVO_GET_COUNT, (unsigned long)&_servo_count) != 0) {
|
|
|
|
hal.console->printf("RCOutput: Unable to get servo count\n");
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
if (_alt_fd != -1 && ioctl(_alt_fd, PWM_SERVO_GET_COUNT, (unsigned long)&_alt_servo_count) != 0) {
|
|
|
|
hal.console->printf("RCOutput: Unable to get alt servo count\n");
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2016-01-04 16:17:44 -04:00
|
|
|
// greater than 400 doesn't give enough room at higher periods for
|
|
|
|
// the down pulse
|
|
|
|
if (freq_hz > 400) {
|
|
|
|
freq_hz = 400;
|
|
|
|
}
|
|
|
|
uint32_t primary_mask = chmask & ((1UL<<_servo_count)-1);
|
|
|
|
uint32_t alt_mask = chmask >> _servo_count;
|
|
|
|
if (primary_mask && _pwm_fd != -1) {
|
|
|
|
set_freq_fd(_pwm_fd, primary_mask, freq_hz);
|
|
|
|
}
|
|
|
|
if (alt_mask && _alt_fd != -1) {
|
|
|
|
set_freq_fd(_alt_fd, alt_mask, freq_hz);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2013-01-04 07:25:36 -04:00
|
|
|
uint16_t PX4RCOutput::get_freq(uint8_t ch)
|
|
|
|
{
|
2014-01-19 21:56:30 -04:00
|
|
|
if (_rate_mask & (1U<<ch)) {
|
2013-04-25 06:59:30 -03:00
|
|
|
return _freq_hz;
|
|
|
|
}
|
|
|
|
return 50;
|
2013-01-04 07:25:36 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
void PX4RCOutput::enable_ch(uint8_t ch)
|
|
|
|
{
|
2014-08-27 06:52:07 -03:00
|
|
|
if (ch >= PX4_NUM_OUTPUT_CHANNELS) {
|
|
|
|
return;
|
|
|
|
}
|
2014-11-13 20:58:09 -04:00
|
|
|
if (ch >= 8 && !(_enabled_channels & (1U<<ch))) {
|
2016-05-12 14:01:14 -03:00
|
|
|
// this is the first enable of an auxiliary channel - setup
|
2014-01-19 21:56:30 -04:00
|
|
|
// aux channels now. This delayed setup makes it possible to
|
|
|
|
// use BRD_PWM_COUNT to setup the number of PWM channels.
|
|
|
|
_init_alt_channels();
|
|
|
|
}
|
|
|
|
_enabled_channels |= (1U<<ch);
|
2014-08-27 13:28:30 -03:00
|
|
|
if (_period[ch] == PWM_IGNORE_THIS_CHANNEL) {
|
|
|
|
_period[ch] = 0;
|
|
|
|
}
|
2013-01-04 07:25:36 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
void PX4RCOutput::disable_ch(uint8_t ch)
|
|
|
|
{
|
2014-08-27 06:52:07 -03:00
|
|
|
if (ch >= PX4_NUM_OUTPUT_CHANNELS) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2014-01-19 21:56:30 -04:00
|
|
|
_enabled_channels &= ~(1U<<ch);
|
2014-08-27 06:52:07 -03:00
|
|
|
_period[ch] = PWM_IGNORE_THIS_CHANNEL;
|
2013-01-04 07:25:36 -04:00
|
|
|
}
|
|
|
|
|
2014-01-15 07:25:50 -04:00
|
|
|
void PX4RCOutput::set_safety_pwm(uint32_t chmask, uint16_t period_us)
|
|
|
|
{
|
|
|
|
struct pwm_output_values pwm_values;
|
|
|
|
memset(&pwm_values, 0, sizeof(pwm_values));
|
|
|
|
for (uint8_t i=0; i<_servo_count; i++) {
|
|
|
|
if ((1UL<<i) & chmask) {
|
|
|
|
pwm_values.values[i] = period_us;
|
|
|
|
}
|
|
|
|
pwm_values.channel_count++;
|
|
|
|
}
|
|
|
|
int ret = ioctl(_pwm_fd, PWM_SERVO_SET_DISARMED_PWM, (long unsigned int)&pwm_values);
|
|
|
|
if (ret != OK) {
|
|
|
|
hal.console->printf("Failed to setup disarmed PWM for 0x%08x to %u\n", (unsigned)chmask, period_us);
|
2014-04-20 19:36:52 -03:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
void PX4RCOutput::set_failsafe_pwm(uint32_t chmask, uint16_t period_us)
|
|
|
|
{
|
|
|
|
struct pwm_output_values pwm_values;
|
|
|
|
memset(&pwm_values, 0, sizeof(pwm_values));
|
|
|
|
for (uint8_t i=0; i<_servo_count; i++) {
|
|
|
|
if ((1UL<<i) & chmask) {
|
|
|
|
pwm_values.values[i] = period_us;
|
|
|
|
}
|
|
|
|
pwm_values.channel_count++;
|
|
|
|
}
|
|
|
|
int ret = ioctl(_pwm_fd, PWM_SERVO_SET_FAILSAFE_PWM, (long unsigned int)&pwm_values);
|
|
|
|
if (ret != OK) {
|
|
|
|
hal.console->printf("Failed to setup failsafe PWM for 0x%08x to %u\n", (unsigned)chmask, period_us);
|
2014-01-15 07:25:50 -04:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2014-09-14 05:23:58 -03:00
|
|
|
bool PX4RCOutput::force_safety_on(void)
|
|
|
|
{
|
|
|
|
int ret = ioctl(_pwm_fd, PWM_SERVO_SET_FORCE_SAFETY_ON, 0);
|
|
|
|
return (ret == OK);
|
|
|
|
}
|
|
|
|
|
2014-02-11 00:56:44 -04:00
|
|
|
void PX4RCOutput::force_safety_off(void)
|
|
|
|
{
|
|
|
|
int ret = ioctl(_pwm_fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0);
|
|
|
|
if (ret != OK) {
|
|
|
|
hal.console->printf("Failed to force safety off\n");
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2013-01-04 07:25:36 -04:00
|
|
|
void PX4RCOutput::write(uint8_t ch, uint16_t period_us)
|
|
|
|
{
|
2016-04-13 05:24:31 -03:00
|
|
|
if (ch >= PX4_NUM_OUTPUT_CHANNELS) {
|
2013-01-22 16:29:59 -04:00
|
|
|
return;
|
|
|
|
}
|
2014-01-19 21:56:30 -04:00
|
|
|
if (!(_enabled_channels & (1U<<ch))) {
|
|
|
|
// not enabled
|
|
|
|
return;
|
|
|
|
}
|
2013-02-23 19:04:17 -04:00
|
|
|
if (ch >= _max_channel) {
|
|
|
|
_max_channel = ch + 1;
|
2013-01-25 04:16:28 -04:00
|
|
|
}
|
2016-04-15 18:23:49 -03:00
|
|
|
/*
|
|
|
|
only mark an update is needed if there has been a change, or we
|
|
|
|
are in oneshot mode. In oneshot mode we always need to send the
|
|
|
|
output
|
|
|
|
*/
|
2016-04-15 03:38:58 -03:00
|
|
|
if (period_us != _period[ch] ||
|
|
|
|
_output_mode == MODE_PWM_ONESHOT) {
|
2013-01-22 16:29:59 -04:00
|
|
|
_period[ch] = period_us;
|
2013-01-25 04:16:28 -04:00
|
|
|
_need_update = true;
|
2013-01-22 16:29:59 -04:00
|
|
|
}
|
2013-01-04 07:25:36 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
uint16_t PX4RCOutput::read(uint8_t ch)
|
|
|
|
{
|
2013-01-22 16:29:59 -04:00
|
|
|
if (ch >= PX4_NUM_OUTPUT_CHANNELS) {
|
|
|
|
return 0;
|
2013-01-04 07:25:36 -04:00
|
|
|
}
|
2014-11-05 06:38:12 -04:00
|
|
|
// if px4io has given us a value for this channel use that,
|
|
|
|
// otherwise use the value we last sent. This makes it easier to
|
|
|
|
// observe the behaviour of failsafe in px4io
|
2015-06-03 22:00:34 -03:00
|
|
|
for (uint8_t i=0; i<ORB_MULTI_MAX_INSTANCES; i++) {
|
|
|
|
if (_outputs[i].pwm_sub >= 0 &&
|
|
|
|
ch < _outputs[i].outputs.noutputs &&
|
|
|
|
_outputs[i].outputs.output[ch] > 0) {
|
|
|
|
return _outputs[i].outputs.output[ch];
|
|
|
|
}
|
2014-11-05 06:38:12 -04:00
|
|
|
}
|
2013-01-22 16:29:59 -04:00
|
|
|
return _period[ch];
|
2013-01-04 07:25:36 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
void PX4RCOutput::read(uint16_t* period_us, uint8_t len)
|
|
|
|
{
|
|
|
|
for (uint8_t i=0; i<len; i++) {
|
|
|
|
period_us[i] = read(i);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2016-03-10 17:55:58 -04:00
|
|
|
uint16_t PX4RCOutput::read_last_sent(uint8_t ch)
|
|
|
|
{
|
|
|
|
if (ch >= PX4_NUM_OUTPUT_CHANNELS) {
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
return _period[ch];
|
|
|
|
}
|
|
|
|
|
|
|
|
void PX4RCOutput::read_last_sent(uint16_t* period_us, uint8_t len)
|
|
|
|
{
|
|
|
|
for (uint8_t i=0; i<len; i++) {
|
|
|
|
period_us[i] = read_last_sent(i);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2014-11-20 03:31:41 -04:00
|
|
|
/*
|
|
|
|
update actuator armed state
|
|
|
|
*/
|
|
|
|
void PX4RCOutput::_arm_actuators(bool arm)
|
|
|
|
{
|
|
|
|
if (_armed.armed == arm) {
|
|
|
|
// already armed;
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
_armed.timestamp = hrt_absolute_time();
|
|
|
|
_armed.armed = arm;
|
|
|
|
_armed.ready_to_arm = arm;
|
|
|
|
_armed.lockdown = false;
|
|
|
|
_armed.force_failsafe = false;
|
|
|
|
|
2015-05-31 01:49:27 -03:00
|
|
|
if (_actuator_armed_pub == NULL) {
|
2014-11-20 03:31:41 -04:00
|
|
|
_actuator_armed_pub = orb_advertise(ORB_ID(actuator_armed), &_armed);
|
|
|
|
} else {
|
|
|
|
orb_publish(ORB_ID(actuator_armed), _actuator_armed_pub, &_armed);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
publish new outputs to the actuator_direct topic
|
|
|
|
*/
|
|
|
|
void PX4RCOutput::_publish_actuators(void)
|
|
|
|
{
|
|
|
|
struct actuator_direct_s actuators;
|
|
|
|
|
2015-06-06 20:07:47 -03:00
|
|
|
if (_esc_pwm_min == 0 ||
|
|
|
|
_esc_pwm_max == 0) {
|
|
|
|
// not initialised yet
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2014-11-20 03:31:41 -04:00
|
|
|
actuators.nvalues = _max_channel;
|
2015-05-31 01:49:27 -03:00
|
|
|
if (actuators.nvalues > actuators.NUM_ACTUATORS_DIRECT) {
|
|
|
|
actuators.nvalues = actuators.NUM_ACTUATORS_DIRECT;
|
2014-11-20 03:31:41 -04:00
|
|
|
}
|
|
|
|
// don't publish more than 8 actuators for now, as the uavcan ESC
|
|
|
|
// driver refuses to update any motors if you try to publish more
|
|
|
|
// than 8
|
|
|
|
if (actuators.nvalues > 8) {
|
|
|
|
actuators.nvalues = 8;
|
|
|
|
}
|
2015-06-06 20:07:47 -03:00
|
|
|
bool armed = hal.util->get_soft_armed();
|
2014-11-20 03:31:41 -04:00
|
|
|
actuators.timestamp = hrt_absolute_time();
|
|
|
|
for (uint8_t i=0; i<actuators.nvalues; i++) {
|
2015-06-06 20:07:47 -03:00
|
|
|
if (!armed) {
|
|
|
|
actuators.values[i] = 0;
|
|
|
|
} else {
|
|
|
|
actuators.values[i] = (_period[i] - _esc_pwm_min) / (float)(_esc_pwm_max - _esc_pwm_min);
|
|
|
|
}
|
2014-11-20 03:31:41 -04:00
|
|
|
// actuator values are from -1 to 1
|
|
|
|
actuators.values[i] = actuators.values[i]*2 - 1;
|
|
|
|
}
|
|
|
|
|
2015-05-31 01:49:27 -03:00
|
|
|
if (_actuator_direct_pub == NULL) {
|
2014-11-20 03:31:41 -04:00
|
|
|
_actuator_direct_pub = orb_advertise(ORB_ID(actuator_direct), &actuators);
|
|
|
|
} else {
|
|
|
|
orb_publish(ORB_ID(actuator_direct), _actuator_direct_pub, &actuators);
|
|
|
|
}
|
|
|
|
if (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED) {
|
|
|
|
_arm_actuators(true);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2016-04-13 03:28:50 -03:00
|
|
|
void PX4RCOutput::_send_outputs(void)
|
2013-01-25 04:16:28 -04:00
|
|
|
{
|
2015-11-19 23:11:07 -04:00
|
|
|
uint32_t now = AP_HAL::micros();
|
2013-01-25 04:16:28 -04:00
|
|
|
|
2014-11-06 02:29:14 -04:00
|
|
|
if ((_enabled_channels & ((1U<<_servo_count)-1)) == 0) {
|
|
|
|
// no channels enabled
|
2014-11-20 03:31:41 -04:00
|
|
|
_arm_actuators(false);
|
2014-11-06 02:29:14 -04:00
|
|
|
goto update_pwm;
|
|
|
|
}
|
|
|
|
|
2013-01-25 04:16:28 -04:00
|
|
|
// always send at least at 20Hz, otherwise the IO board may think
|
|
|
|
// we are dead
|
|
|
|
if (now - _last_output > 50000) {
|
|
|
|
_need_update = true;
|
|
|
|
}
|
|
|
|
|
2015-12-05 17:31:45 -04:00
|
|
|
// check for PWM count changing. This can happen then the user changes BRD_PWM_COUNT
|
|
|
|
if (now - _last_config_us > 1000000) {
|
|
|
|
if (_pwm_fd != -1) {
|
|
|
|
ioctl(_pwm_fd, PWM_SERVO_GET_COUNT, (unsigned long)&_servo_count);
|
|
|
|
}
|
|
|
|
if (_alt_fd != -1) {
|
|
|
|
ioctl(_alt_fd, PWM_SERVO_GET_COUNT, (unsigned long)&_alt_servo_count);
|
|
|
|
}
|
|
|
|
_last_config_us = now;
|
|
|
|
}
|
|
|
|
|
2013-01-25 04:16:28 -04:00
|
|
|
if (_need_update && _pwm_fd != -1) {
|
|
|
|
_need_update = false;
|
|
|
|
perf_begin(_perf_rcout);
|
2016-04-15 03:38:58 -03:00
|
|
|
uint8_t to_send = _max_channel<_servo_count?_max_channel:_servo_count;
|
|
|
|
if (_sbus_enabled) {
|
|
|
|
to_send = _max_channel;
|
|
|
|
}
|
|
|
|
if (to_send > 0) {
|
|
|
|
for (int i=to_send-1; i >= 0; i--) {
|
2016-04-15 19:32:04 -03:00
|
|
|
if (_period[i] == 0 || _period[i] == PWM_IGNORE_THIS_CHANNEL) {
|
2016-04-15 03:38:58 -03:00
|
|
|
to_send = i;
|
2016-04-21 22:51:08 -03:00
|
|
|
} else {
|
|
|
|
break;
|
2016-04-15 03:38:58 -03:00
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
if (to_send > 0) {
|
|
|
|
::write(_pwm_fd, _period, to_send*sizeof(_period[0]));
|
|
|
|
}
|
2016-03-12 19:03:56 -04:00
|
|
|
if (_max_channel > _servo_count) {
|
|
|
|
// maybe send updates to alt_fd
|
2013-04-25 06:59:30 -03:00
|
|
|
if (_alt_fd != -1 && _alt_servo_count > 0) {
|
|
|
|
uint8_t n = _max_channel - _servo_count;
|
|
|
|
if (n > _alt_servo_count) {
|
|
|
|
n = _alt_servo_count;
|
|
|
|
}
|
2016-04-13 05:24:31 -03:00
|
|
|
if (n > 0) {
|
|
|
|
::write(_alt_fd, &_period[_servo_count], n*sizeof(_period[0]));
|
|
|
|
}
|
2013-04-25 06:59:30 -03:00
|
|
|
}
|
|
|
|
}
|
2014-11-20 03:31:41 -04:00
|
|
|
|
|
|
|
// also publish to actuator_direct
|
|
|
|
_publish_actuators();
|
|
|
|
|
2013-01-25 04:16:28 -04:00
|
|
|
perf_end(_perf_rcout);
|
|
|
|
_last_output = now;
|
|
|
|
}
|
2014-11-05 06:38:12 -04:00
|
|
|
|
|
|
|
update_pwm:
|
2015-06-03 22:00:34 -03:00
|
|
|
for (uint8_t i=0; i<ORB_MULTI_MAX_INSTANCES; i++) {
|
|
|
|
bool rc_updated = false;
|
|
|
|
if (_outputs[i].pwm_sub >= 0 &&
|
|
|
|
orb_check(_outputs[i].pwm_sub, &rc_updated) == 0 &&
|
|
|
|
rc_updated) {
|
|
|
|
orb_copy(ORB_ID(actuator_outputs), _outputs[i].pwm_sub, &_outputs[i].outputs);
|
|
|
|
}
|
|
|
|
}
|
2014-11-05 06:38:12 -04:00
|
|
|
|
2013-01-25 04:16:28 -04:00
|
|
|
}
|
|
|
|
|
2016-04-13 03:28:50 -03:00
|
|
|
void PX4RCOutput::cork()
|
|
|
|
{
|
|
|
|
#if RCOUT_DEBUG_LATENCY
|
|
|
|
hal.gpio->pinMode(55, HAL_GPIO_OUTPUT);
|
|
|
|
hal.gpio->write(55, 1);
|
|
|
|
#endif
|
|
|
|
_corking = true;
|
|
|
|
}
|
|
|
|
|
|
|
|
void PX4RCOutput::push()
|
|
|
|
{
|
|
|
|
#if RCOUT_DEBUG_LATENCY
|
|
|
|
hal.gpio->pinMode(55, HAL_GPIO_OUTPUT);
|
|
|
|
hal.gpio->write(55, 0);
|
|
|
|
#endif
|
|
|
|
_corking = false;
|
|
|
|
if (_output_mode == MODE_PWM_ONESHOT) {
|
|
|
|
// run timer immediately in oneshot mode
|
|
|
|
_send_outputs();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
void PX4RCOutput::_timer_tick(void)
|
|
|
|
{
|
|
|
|
if (_output_mode != MODE_PWM_ONESHOT) {
|
2016-04-15 18:23:49 -03:00
|
|
|
/* in oneshot mode the timer does nothing as the outputs are
|
|
|
|
* sent from push() */
|
2016-04-13 03:28:50 -03:00
|
|
|
_send_outputs();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2016-04-15 03:38:58 -03:00
|
|
|
/*
|
|
|
|
enable sbus output
|
|
|
|
*/
|
|
|
|
bool PX4RCOutput::enable_sbus_out(uint16_t rate_hz)
|
|
|
|
{
|
|
|
|
int fd = open("/dev/px4io", 0);
|
|
|
|
if (fd == -1) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
for (uint8_t tries=0; tries<10; tries++) {
|
|
|
|
if (ioctl(fd, SBUS_SET_PROTO_VERSION, 1) != 0) {
|
|
|
|
continue;
|
|
|
|
}
|
|
|
|
if (ioctl(fd, PWM_SERVO_SET_SBUS_RATE, rate_hz) != 0) {
|
|
|
|
continue;
|
|
|
|
}
|
|
|
|
close(fd);
|
|
|
|
_sbus_enabled = true;
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
close(fd);
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
setup output mode
|
|
|
|
*/
|
|
|
|
void PX4RCOutput::set_output_mode(enum output_mode mode)
|
|
|
|
{
|
2016-04-16 05:52:31 -03:00
|
|
|
if (_output_mode == mode) {
|
|
|
|
// no change
|
|
|
|
return;
|
|
|
|
}
|
2016-04-22 00:24:24 -03:00
|
|
|
if (mode == MODE_PWM_ONESHOT) {
|
2016-04-22 00:50:05 -03:00
|
|
|
// when using oneshot we don't want the regular pulses. The
|
|
|
|
// best we can do with the current PX4Firmware code is ask for
|
|
|
|
// 1Hz. This does still produce pulses, but the trigger calls
|
|
|
|
// mean the timer is constantly reset, so no pulses are
|
|
|
|
// produced except when triggered by push() when the main loop
|
|
|
|
// is running
|
2016-04-22 00:24:24 -03:00
|
|
|
set_freq(_rate_mask, 1);
|
|
|
|
}
|
2016-04-15 03:38:58 -03:00
|
|
|
_output_mode = mode;
|
|
|
|
if (_output_mode == MODE_PWM_ONESHOT) {
|
2016-04-22 00:24:24 -03:00
|
|
|
//::printf("enable oneshot\n");
|
2016-04-15 03:38:58 -03:00
|
|
|
ioctl(_pwm_fd, PWM_SERVO_SET_ONESHOT, 1);
|
2016-04-15 18:23:49 -03:00
|
|
|
if (_alt_fd != -1) {
|
|
|
|
ioctl(_alt_fd, PWM_SERVO_SET_ONESHOT, 1);
|
|
|
|
}
|
2016-04-15 03:38:58 -03:00
|
|
|
} else {
|
|
|
|
ioctl(_pwm_fd, PWM_SERVO_SET_ONESHOT, 0);
|
2016-04-15 18:23:49 -03:00
|
|
|
if (_alt_fd != -1) {
|
|
|
|
ioctl(_alt_fd, PWM_SERVO_SET_ONESHOT, 0);
|
|
|
|
}
|
2016-04-15 03:38:58 -03:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
2013-01-04 07:25:36 -04:00
|
|
|
#endif // CONFIG_HAL_BOARD
|