forked from Archive/PX4-Autopilot
Rework the PX4IO software architecture:
- Use a separate thread for handing R/C inputs and outputs. - Remove all PX4IO R/C receiver configuration; it's all automatic now. - Rework the main loop, dedicate it to PX4FMU communications after startup. - Fix several issues in the px4io driver that would cause a crash if PX4IO was not responding.
This commit is contained in:
px4dev
parent
e153476950
commit
9fa794a8fa
|
|
@ -87,15 +87,13 @@ public:
|
|||
|
||||
virtual int ioctl(file *filp, int cmd, unsigned long arg);
|
||||
|
||||
void set_rx_mode(unsigned mode);
|
||||
|
||||
private:
|
||||
static const unsigned _max_actuators = PX4IO_OUTPUT_CHANNELS;
|
||||
|
||||
int _serial_fd; ///< serial interface to PX4IO
|
||||
hx_stream_t _io_stream; ///< HX protocol stream
|
||||
|
||||
int _task; ///< worker task
|
||||
volatile int _task; ///< worker task
|
||||
volatile bool _task_should_exit;
|
||||
volatile bool _connected; ///< true once we have received a valid frame
|
||||
|
||||
|
|
@ -121,8 +119,6 @@ private:
|
|||
bool _send_needed; ///< If true, we need to send a packet to IO
|
||||
bool _config_needed; ///< if true, we need to set a config update to IO
|
||||
|
||||
uint8_t _rx_mode; ///< the current RX mode on IO
|
||||
|
||||
/**
|
||||
* Trampoline to the worker task
|
||||
*/
|
||||
|
|
@ -190,8 +186,7 @@ PX4IO::PX4IO() :
|
|||
_primary_pwm_device(false),
|
||||
_switch_armed(false),
|
||||
_send_needed(false),
|
||||
_config_needed(false),
|
||||
_rx_mode(RX_MODE_PPM_ONLY)
|
||||
_config_needed(false)
|
||||
{
|
||||
/* we need this potentially before it could be set in task_main */
|
||||
g_dev = this;
|
||||
|
|
@ -201,33 +196,17 @@ PX4IO::PX4IO() :
|
|||
|
||||
PX4IO::~PX4IO()
|
||||
{
|
||||
if (_task != -1) {
|
||||
/* tell the task we want it to go away */
|
||||
_task_should_exit = true;
|
||||
/* tell the task we want it to go away */
|
||||
_task_should_exit = true;
|
||||
|
||||
/* spin waiting for the thread to stop */
|
||||
unsigned i = 10;
|
||||
|
||||
do {
|
||||
/* wait 50ms - it should wake every 100ms or so worst-case */
|
||||
usleep(50000);
|
||||
|
||||
/* if we have given up, kill it */
|
||||
if (--i == 0) {
|
||||
task_delete(_task);
|
||||
break;
|
||||
}
|
||||
|
||||
} while (_task != -1);
|
||||
/* spin waiting for the task to stop */
|
||||
for (unsigned i = 0; (i < 10) && (_task != -1); i++) {
|
||||
/* give it another 100ms */
|
||||
usleep(100000);
|
||||
}
|
||||
|
||||
/* clean up the alternate device node */
|
||||
if (_primary_pwm_device)
|
||||
unregister_driver(PWM_OUTPUT_DEVICE_PATH);
|
||||
|
||||
/* kill the HX stream */
|
||||
if (_io_stream != nullptr)
|
||||
hx_stream_free(_io_stream);
|
||||
/* well, kill it anyway, though this will probably crash */
|
||||
if (_task != -1)
|
||||
task_delete(_task);
|
||||
|
||||
g_dev = nullptr;
|
||||
}
|
||||
|
|
@ -295,6 +274,16 @@ PX4IO::task_main()
|
|||
goto out;
|
||||
}
|
||||
|
||||
/* 115200bps, no parity, one stop bit */
|
||||
{
|
||||
struct termios t;
|
||||
|
||||
tcgetattr(_serial_fd, &t);
|
||||
cfsetspeed(&t, 115200);
|
||||
t.c_cflag &= ~(CSTOPB | PARENB);
|
||||
tcsetattr(_serial_fd, TCSANOW, &t);
|
||||
}
|
||||
|
||||
/* protocol stream */
|
||||
_io_stream = hx_stream_init(_serial_fd, &PX4IO::rx_callback_trampoline, this);
|
||||
if (_io_stream == nullptr) {
|
||||
|
|
@ -402,8 +391,16 @@ PX4IO::task_main()
|
|||
}
|
||||
|
||||
out:
|
||||
debug("exiting");
|
||||
|
||||
/* kill the HX stream */
|
||||
if (_io_stream != nullptr)
|
||||
hx_stream_free(_io_stream);
|
||||
::close(_serial_fd);
|
||||
|
||||
/* clean up the alternate device node */
|
||||
if (_primary_pwm_device)
|
||||
unregister_driver(PWM_OUTPUT_DEVICE_PATH);
|
||||
|
||||
/* tell the dtor that we are exiting */
|
||||
_task = -1;
|
||||
|
|
@ -514,7 +511,6 @@ PX4IO::config_send()
|
|||
int ret;
|
||||
|
||||
cfg.f2i_config_magic = F2I_CONFIG_MAGIC;
|
||||
cfg.serial_rx_mode = _rx_mode;
|
||||
|
||||
ret = hx_stream_send(_io_stream, &cfg, sizeof(cfg));
|
||||
if (ret)
|
||||
|
|
@ -634,17 +630,6 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
|
|||
return ret;
|
||||
}
|
||||
|
||||
void
|
||||
PX4IO::set_rx_mode(unsigned mode)
|
||||
{
|
||||
/*
|
||||
* Always (re)set the rx mode; makes testing
|
||||
* easier after PX4IO has been restarted.
|
||||
*/
|
||||
_rx_mode = mode;
|
||||
_config_needed = true;
|
||||
}
|
||||
|
||||
extern "C" __EXPORT int px4io_main(int argc, char *argv[]);
|
||||
|
||||
namespace
|
||||
|
|
@ -748,25 +733,10 @@ px4io_main(int argc, char *argv[])
|
|||
|
||||
if (!strcmp(argv[1], "rx_dsm") ||
|
||||
!strcmp(argv[1], "rx_dsm_10bit") ||
|
||||
!strcmp(argv[1], "rx_dsm_11bit")) {
|
||||
if (g_dev == nullptr)
|
||||
errx(1, "not started");
|
||||
g_dev->set_rx_mode(RX_MODE_DSM);
|
||||
exit(0);
|
||||
}
|
||||
if (!strcmp(argv[1], "rx_sbus")) {
|
||||
if (g_dev == nullptr)
|
||||
errx(1, "not started");
|
||||
g_dev->set_rx_mode(RX_MODE_FUTABA_SBUS);
|
||||
exit(0);
|
||||
}
|
||||
if (!strcmp(argv[1], "rx_ppm")) {
|
||||
if (g_dev == nullptr)
|
||||
errx(1, "not started");
|
||||
g_dev->set_rx_mode(RX_MODE_PPM_ONLY);
|
||||
exit(0);
|
||||
}
|
||||
|
||||
!strcmp(argv[1], "rx_dsm_11bit") ||
|
||||
!strcmp(argv[1], "rx_sbus") ||
|
||||
!strcmp(argv[1], "rx_ppm"))
|
||||
errx(0, "receiver type is automatically detected, option '%s' is deprecated", argv[1]);
|
||||
if (!strcmp(argv[1], "test"))
|
||||
test();
|
||||
|
||||
|
|
|
|||
|
|
@ -32,10 +32,11 @@
|
|||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file FMU communication for the PX4IO module.
|
||||
* @file comms.c
|
||||
*
|
||||
* FMU communication for the PX4IO module.
|
||||
*/
|
||||
|
||||
|
||||
#include <nuttx/config.h>
|
||||
#include <stdio.h>
|
||||
#include <stdbool.h>
|
||||
|
|
@ -44,7 +45,6 @@
|
|||
#include <debug.h>
|
||||
#include <stdlib.h>
|
||||
#include <errno.h>
|
||||
#include <termios.h>
|
||||
#include <string.h>
|
||||
#include <poll.h>
|
||||
|
||||
|
|
@ -54,101 +54,50 @@
|
|||
#include <systemlib/hx_stream.h>
|
||||
#include <systemlib/perf_counter.h>
|
||||
|
||||
#define DEBUG
|
||||
#include "px4io.h"
|
||||
|
||||
#define FMU_MIN_REPORT_INTERVAL 5000 /* 5ms */
|
||||
#define FMU_MAX_REPORT_INTERVAL 100000 /* 100ms */
|
||||
|
||||
int frame_rx;
|
||||
int frame_bad;
|
||||
|
||||
static int fmu_fd;
|
||||
static hx_stream_t stream;
|
||||
static int rx_fd;
|
||||
static struct px4io_report report;
|
||||
|
||||
static void comms_handle_frame(void *arg, const void *buffer, size_t length);
|
||||
|
||||
static struct pollfd pollfds[2];
|
||||
static int pollcount;
|
||||
|
||||
void
|
||||
comms_init(void)
|
||||
{
|
||||
/* initialise the FMU interface */
|
||||
fmu_fd = open("/dev/ttyS1", O_RDWR | O_NONBLOCK);
|
||||
if (fmu_fd < 0)
|
||||
lib_lowprintf("COMMS: fmu open failed %d\n", errno);
|
||||
stream = hx_stream_init(fmu_fd, comms_handle_frame, NULL);
|
||||
pollfds[0].fd = fmu_fd;
|
||||
pollfds[0].events = POLLIN;
|
||||
pollcount = 1;
|
||||
|
||||
/* default state in the report to FMU */
|
||||
report.i2f_magic = I2F_MAGIC;
|
||||
|
||||
}
|
||||
|
||||
static void
|
||||
serial_rx_init(unsigned serial_mode)
|
||||
{
|
||||
if (serial_mode == system_state.serial_rx_mode)
|
||||
return;
|
||||
system_state.serial_rx_mode = serial_mode;
|
||||
|
||||
if (serial_mode == RX_MODE_PPM_ONLY) {
|
||||
if (rx_fd != -1) {
|
||||
pollcount = 1;
|
||||
close(rx_fd);
|
||||
rx_fd = -1;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
/* open the serial port used for DSM and S.bus communication */
|
||||
rx_fd = open("/dev/ttyS0", O_RDONLY | O_NONBLOCK);
|
||||
pollfds[1].fd = rx_fd;
|
||||
pollfds[1].events = POLLIN;
|
||||
pollcount = 2;
|
||||
|
||||
struct termios t;
|
||||
tcgetattr(rx_fd, &t);
|
||||
|
||||
switch (serial_mode) {
|
||||
case RX_MODE_DSM:
|
||||
/* 115200, no parity, one stop bit */
|
||||
cfsetspeed(&t, 115200);
|
||||
t.c_cflag &= ~(CSTOPB | PARENB);
|
||||
|
||||
dsm_init(serial_mode);
|
||||
break;
|
||||
|
||||
case RX_MODE_FUTABA_SBUS:
|
||||
/* 100000, even parity, two stop bits */
|
||||
cfsetspeed(&t, 100000);
|
||||
t.c_cflag |= (CSTOPB | PARENB);
|
||||
|
||||
sbus_init(serial_mode);
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
tcsetattr(rx_fd, TCSANOW, &t);
|
||||
}
|
||||
|
||||
void
|
||||
comms_check(void)
|
||||
comms_main(void)
|
||||
{
|
||||
/*
|
||||
* Check for serial data
|
||||
*/
|
||||
int ret = poll(pollfds, pollcount, 0);
|
||||
|
||||
if (ret > 0) {
|
||||
struct pollfd fds;
|
||||
fds.fd = fmu_fd;
|
||||
fds.events = POLLIN;
|
||||
debug("FMU: ready");
|
||||
|
||||
for (;;) {
|
||||
/* wait for serial data, but no more than 100ms */
|
||||
poll(&fds, 1, 100);
|
||||
|
||||
/*
|
||||
* Pull bytes from FMU and feed them to the HX engine.
|
||||
* Limit the number of bytes we actually process on any one iteration.
|
||||
*/
|
||||
if (pollfds[0].revents & POLLIN) {
|
||||
if (fds.revents & POLLIN) {
|
||||
char buf[32];
|
||||
ssize_t count = read(fmu_fd, buf, sizeof(buf));
|
||||
for (int i = 0; i < count; i++)
|
||||
|
|
@ -156,54 +105,32 @@ comms_check(void)
|
|||
}
|
||||
|
||||
/*
|
||||
* Pull bytes from the serial RX port and feed them to the decoder
|
||||
* if we care about serial RX data.
|
||||
* Decide if it's time to send an update to the FMU.
|
||||
*/
|
||||
if ((pollcount > 1) && (pollfds[1].revents & POLLIN)) {
|
||||
switch (system_state.serial_rx_mode) {
|
||||
case RX_MODE_DSM:
|
||||
dsm_input(rx_fd);
|
||||
break;
|
||||
static hrt_abstime last_report_time;
|
||||
hrt_abstime now, delta;
|
||||
|
||||
case RX_MODE_FUTABA_SBUS:
|
||||
sbus_input(rx_fd);
|
||||
break;
|
||||
/* should we send a report to the FMU? */
|
||||
now = hrt_absolute_time();
|
||||
delta = now - last_report_time;
|
||||
if ((delta > FMU_MIN_REPORT_INTERVAL) &&
|
||||
(system_state.fmu_report_due || (delta > FMU_MAX_REPORT_INTERVAL))) {
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
system_state.fmu_report_due = false;
|
||||
last_report_time = now;
|
||||
|
||||
/* populate the report */
|
||||
for (int i = 0; i < system_state.rc_channels; i++)
|
||||
report.rc_channel[i] = system_state.rc_channel_data[i];
|
||||
report.channel_count = system_state.rc_channels;
|
||||
report.armed = system_state.armed;
|
||||
|
||||
/* and send it */
|
||||
hx_stream_send(stream, &report, sizeof(report));
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Decide if it's time to send an update to the FMU.
|
||||
*/
|
||||
static hrt_abstime last_report_time;
|
||||
hrt_abstime now, delta;
|
||||
|
||||
/* should we send a report to the FMU? */
|
||||
now = hrt_absolute_time();
|
||||
delta = now - last_report_time;
|
||||
if ((delta > FMU_MIN_REPORT_INTERVAL) &&
|
||||
(system_state.fmu_report_due || (delta > FMU_MAX_REPORT_INTERVAL))) {
|
||||
|
||||
system_state.fmu_report_due = false;
|
||||
last_report_time = now;
|
||||
|
||||
/* populate the report */
|
||||
for (int i = 0; i < system_state.rc_channels; i++)
|
||||
report.rc_channel[i] = system_state.rc_channel_data[i];
|
||||
report.channel_count = system_state.rc_channels;
|
||||
report.armed = system_state.armed;
|
||||
|
||||
/* and send it */
|
||||
hx_stream_send(stream, &report, sizeof(report));
|
||||
}
|
||||
}
|
||||
|
||||
int frame_rx;
|
||||
int frame_bad;
|
||||
|
||||
static void
|
||||
comms_handle_config(const void *buffer, size_t length)
|
||||
{
|
||||
|
|
@ -215,8 +142,6 @@ comms_handle_config(const void *buffer, size_t length)
|
|||
}
|
||||
|
||||
frame_rx++;
|
||||
|
||||
serial_rx_init(cfg->serial_rx_mode);
|
||||
}
|
||||
|
||||
static void
|
||||
|
|
@ -274,9 +199,9 @@ comms_handle_frame(void *arg, const void *buffer, size_t length)
|
|||
comms_handle_config(buffer, length);
|
||||
break;
|
||||
default:
|
||||
frame_bad++;
|
||||
break;
|
||||
}
|
||||
}
|
||||
frame_bad++;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,88 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file controls.c
|
||||
*
|
||||
* R/C inputs and servo outputs.
|
||||
*/
|
||||
|
||||
|
||||
#include <nuttx/config.h>
|
||||
#include <stdio.h>
|
||||
#include <stdbool.h>
|
||||
#include <fcntl.h>
|
||||
#include <unistd.h>
|
||||
#include <debug.h>
|
||||
#include <stdlib.h>
|
||||
#include <errno.h>
|
||||
#include <termios.h>
|
||||
#include <string.h>
|
||||
#include <poll.h>
|
||||
|
||||
#include <nuttx/clock.h>
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <systemlib/hx_stream.h>
|
||||
#include <systemlib/perf_counter.h>
|
||||
|
||||
#define DEBUG
|
||||
#include "px4io.h"
|
||||
|
||||
void
|
||||
controls_main(void)
|
||||
{
|
||||
struct pollfd fds[2];
|
||||
|
||||
fds[0].fd = dsm_init("/dev/ttyS0");
|
||||
fds[0].events = POLLIN;
|
||||
|
||||
|
||||
fds[1].fd = sbus_init("/dev/ttyS2");
|
||||
fds[1].events = POLLIN;
|
||||
|
||||
for (;;) {
|
||||
/* run this loop at ~100Hz */
|
||||
poll(fds, 2, 10);
|
||||
|
||||
if (fds[0].revents & POLLIN)
|
||||
dsm_input();
|
||||
if (fds[1].revents & POLLIN)
|
||||
sbus_input();
|
||||
|
||||
/* XXX do ppm processing, bypass mode, etc. here */
|
||||
|
||||
/* do PWM output updates */
|
||||
mixer_tick();
|
||||
}
|
||||
}
|
||||
|
|
@ -44,6 +44,7 @@
|
|||
#include <fcntl.h>
|
||||
#include <unistd.h>
|
||||
#include <string.h>
|
||||
#include <termios.h>
|
||||
|
||||
#include <systemlib/ppm_decode.h>
|
||||
|
||||
|
|
@ -57,6 +58,8 @@
|
|||
#define DSM_FRAME_SIZE 16
|
||||
#define DSM_FRAME_CHANNELS 7
|
||||
|
||||
static int dsm_fd = -1;
|
||||
|
||||
static hrt_abstime last_rx_time;
|
||||
static hrt_abstime last_frame_time;
|
||||
|
||||
|
|
@ -66,27 +69,46 @@ static unsigned partial_frame_count;
|
|||
static bool insync;
|
||||
static unsigned channel_shift;
|
||||
|
||||
unsigned dsm_frame_drops;
|
||||
|
||||
static bool dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value);
|
||||
static void dsm_guess_format(bool reset);
|
||||
static void dsm_decode(hrt_abstime now);
|
||||
|
||||
void
|
||||
dsm_init(unsigned mode)
|
||||
int
|
||||
dsm_init(const char *device)
|
||||
{
|
||||
insync = false;
|
||||
partial_frame_count = 0;
|
||||
last_rx_time = hrt_absolute_time();
|
||||
if (dsm_fd < 0)
|
||||
dsm_fd = open(device, O_RDONLY);
|
||||
|
||||
/* reset the format detector */
|
||||
dsm_guess_format(true);
|
||||
if (dsm_fd >= 0) {
|
||||
struct termios t;
|
||||
|
||||
debug("DSM: enabled and waiting\n");
|
||||
/* 115200bps, no parity, one stop bit */
|
||||
tcgetattr(dsm_fd, &t);
|
||||
cfsetspeed(&t, 115200);
|
||||
t.c_cflag &= ~(CSTOPB | PARENB);
|
||||
tcsetattr(dsm_fd, TCSANOW, &t);
|
||||
|
||||
/* initialise the decoder */
|
||||
insync = false;
|
||||
partial_frame_count = 0;
|
||||
last_rx_time = hrt_absolute_time();
|
||||
|
||||
/* reset the format detector */
|
||||
dsm_guess_format(true);
|
||||
|
||||
debug("DSM: ready");
|
||||
} else {
|
||||
debug("DSM: open failed");
|
||||
}
|
||||
|
||||
return dsm_fd;
|
||||
}
|
||||
|
||||
void
|
||||
dsm_input(int fd)
|
||||
dsm_input(void)
|
||||
{
|
||||
uint8_t buf[DSM_FRAME_SIZE];
|
||||
ssize_t ret;
|
||||
hrt_abstime now;
|
||||
|
||||
|
|
@ -107,16 +129,17 @@ dsm_input(int fd)
|
|||
*/
|
||||
now = hrt_absolute_time();
|
||||
if ((now - last_rx_time) > 5000) {
|
||||
if (partial_frame_count > 0)
|
||||
debug("DSM: reset @ %d", partial_frame_count);
|
||||
partial_frame_count = 0;
|
||||
if (partial_frame_count > 0) {
|
||||
dsm_frame_drops++;
|
||||
partial_frame_count = 0;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Fetch bytes, but no more than we would need to complete
|
||||
* the current frame.
|
||||
*/
|
||||
ret = read(fd, buf, DSM_FRAME_SIZE - partial_frame_count);
|
||||
ret = read(dsm_fd, &frame[partial_frame_count], DSM_FRAME_SIZE - partial_frame_count);
|
||||
|
||||
/* if the read failed for any reason, just give up here */
|
||||
if (ret < 1)
|
||||
|
|
@ -126,7 +149,6 @@ dsm_input(int fd)
|
|||
/*
|
||||
* Add bytes to the current frame
|
||||
*/
|
||||
memcpy(&frame[partial_frame_count], buf, ret);
|
||||
partial_frame_count += ret;
|
||||
|
||||
/*
|
||||
|
|
@ -292,8 +314,12 @@ dsm_decode(hrt_abstime frame_time)
|
|||
/* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
|
||||
if (channel_shift == 11)
|
||||
value /= 2;
|
||||
|
||||
/* stuff the decoded channel into the PPM input buffer */
|
||||
ppm_buffer[channel] = 988 + value;
|
||||
}
|
||||
|
||||
/* and note that we have received data from the R/C controller */
|
||||
/* XXX failsafe will cause problems here - need a strategy for detecting it */
|
||||
ppm_last_valid_decode = hrt_absolute_time();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -60,17 +60,6 @@
|
|||
*/
|
||||
static unsigned fmu_input_drops;
|
||||
#define FMU_INPUT_DROP_LIMIT 20
|
||||
|
||||
/*
|
||||
* HRT periodic call used to check for control input data.
|
||||
*/
|
||||
static struct hrt_call mixer_input_call;
|
||||
|
||||
/*
|
||||
* Mixer periodic tick.
|
||||
*/
|
||||
static void mixer_tick(void *arg);
|
||||
|
||||
/*
|
||||
* Collect RC input data from the controller source(s).
|
||||
*/
|
||||
|
|
@ -92,20 +81,8 @@ struct mixer {
|
|||
/* XXX more config here */
|
||||
} mixers[IO_SERVO_COUNT];
|
||||
|
||||
int
|
||||
mixer_init(void)
|
||||
{
|
||||
|
||||
|
||||
/* look for control data at 50Hz */
|
||||
hrt_call_every(&mixer_input_call, 1000, 20000, mixer_tick, NULL);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
static void
|
||||
mixer_tick(void *arg)
|
||||
void
|
||||
mixer_tick(void)
|
||||
{
|
||||
uint16_t *control_values;
|
||||
int control_count;
|
||||
|
|
|
|||
|
|
@ -62,10 +62,7 @@ struct px4io_config {
|
|||
uint16_t f2i_config_magic;
|
||||
#define F2I_CONFIG_MAGIC 0x6366
|
||||
|
||||
uint8_t serial_rx_mode;
|
||||
#define RX_MODE_PPM_ONLY 0
|
||||
#define RX_MODE_DSM 1
|
||||
#define RX_MODE_FUTABA_SBUS 2
|
||||
/* XXX currently nothing here */
|
||||
};
|
||||
|
||||
/* report from IO to FMU */
|
||||
|
|
|
|||
|
|
@ -55,37 +55,15 @@
|
|||
__EXPORT int user_start(int argc, char *argv[]);
|
||||
|
||||
struct sys_state_s system_state;
|
||||
int gpio_fd;
|
||||
|
||||
static const char cursor[] = {'|', '/', '-', '\\'};
|
||||
|
||||
static struct hrt_call timer_tick_call;
|
||||
volatile int timers[TIMER_NUM_TIMERS];
|
||||
static void timer_tick(void *arg);
|
||||
|
||||
int user_start(int argc, char *argv[])
|
||||
{
|
||||
int cycle = 0;
|
||||
bool heartbeat = false;
|
||||
bool failsafe = false;
|
||||
|
||||
/* configure the high-resolution time/callout interface */
|
||||
hrt_init();
|
||||
|
||||
/* init the FMU and receiver links */
|
||||
comms_init();
|
||||
|
||||
/* configure the first 8 PWM outputs (i.e. all of them) */
|
||||
/* note, must do this after comms init to steal back PA0, which is CTS otherwise */
|
||||
up_pwm_servo_init(0xff);
|
||||
|
||||
/* print some startup info */
|
||||
lib_lowprintf("\nPX4IO: starting\n");
|
||||
struct mallinfo minfo = mallinfo();
|
||||
lib_lowprintf("free %u largest %u\n", minfo.mxordblk, minfo.fordblks);
|
||||
|
||||
/* start the software timer service */
|
||||
hrt_call_every(&timer_tick_call, 1000, 1000, timer_tick, NULL);
|
||||
|
||||
/* default all the LEDs to off while we start */
|
||||
LED_AMBER(false);
|
||||
|
|
@ -95,66 +73,27 @@ int user_start(int argc, char *argv[])
|
|||
/* turn on servo power */
|
||||
POWER_SERVO(true);
|
||||
|
||||
/* start the mixer */
|
||||
mixer_init();
|
||||
|
||||
/* start the safety switch handler */
|
||||
safety_init();
|
||||
|
||||
/* set up some timers for the main loop */
|
||||
timers[TIMER_BLINK_BLUE] = 250; /* heartbeat blink @ 2Hz */
|
||||
timers[TIMER_STATUS_PRINT] = 1000; /* print status message @ 1Hz */
|
||||
/* start the flight control signal handler */
|
||||
task_create("FCon",
|
||||
SCHED_PRIORITY_DEFAULT,
|
||||
1024,
|
||||
(main_t)controls_main,
|
||||
NULL);
|
||||
|
||||
/*
|
||||
* Main loop servicing communication with FMU
|
||||
*/
|
||||
while(true) {
|
||||
|
||||
/* check for communication from FMU, send updates */
|
||||
comms_check();
|
||||
/* initialise the FMU communications interface */
|
||||
comms_init();
|
||||
|
||||
/* blink the heartbeat LED */
|
||||
if (timers[TIMER_BLINK_BLUE] == 0) {
|
||||
timers[TIMER_BLINK_BLUE] = 250;
|
||||
LED_BLUE(heartbeat = !heartbeat);
|
||||
}
|
||||
/* configure the first 8 PWM outputs (i.e. all of them) */
|
||||
/* note, must do this after comms init to steal back PA0, which is CTS otherwise */
|
||||
up_pwm_servo_init(0xff);
|
||||
|
||||
/* blink the failsafe LED if we don't have FMU input */
|
||||
if (!system_state.mixer_use_fmu) {
|
||||
if (timers[TIMER_BLINK_AMBER] == 0) {
|
||||
timers[TIMER_BLINK_AMBER] = 125;
|
||||
failsafe = !failsafe;
|
||||
}
|
||||
} else {
|
||||
failsafe = false;
|
||||
}
|
||||
LED_AMBER(failsafe);
|
||||
|
||||
/* print some simple status */
|
||||
#if 0
|
||||
if (timers[TIMER_STATUS_PRINT] == 0) {
|
||||
timers[TIMER_STATUS_PRINT] = 1000;
|
||||
lib_lowprintf("%c %s | %s | %s | %s | C=%d F=%d B=%d \r",
|
||||
cursor[cycle++ & 3],
|
||||
(system_state.arm_ok ? "FMU_ARMED" : "FMU_SAFE"),
|
||||
(system_state.armed ? "ARMED" : "SAFE"),
|
||||
(system_state.rc_channels ? "RC OK" : "NO RC"),
|
||||
(system_state.mixer_use_fmu ? "FMU OK" : "NO FMU"),
|
||||
system_state.rc_channels,
|
||||
frame_rx, frame_bad
|
||||
);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
struct mallinfo minfo = mallinfo();
|
||||
lib_lowprintf("free %u largest %u\n", minfo.mxordblk, minfo.fordblks);
|
||||
|
||||
/* Should never reach here */
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
static void
|
||||
timer_tick(void *arg)
|
||||
{
|
||||
for (unsigned i = 0; i < TIMER_NUM_TIMERS; i++)
|
||||
if (timers[i] > 0)
|
||||
timers[i]--;
|
||||
}
|
||||
/* we're done here, go run the communications loop */
|
||||
comms_main();
|
||||
}
|
||||
|
|
@ -147,7 +147,7 @@ extern volatile int timers[TIMER_NUM_TIMERS];
|
|||
/*
|
||||
* Mixer
|
||||
*/
|
||||
extern int mixer_init(void);
|
||||
extern void mixer_tick(void);
|
||||
|
||||
/*
|
||||
* Safety switch/LED.
|
||||
|
|
@ -158,15 +158,16 @@ extern void safety_init(void);
|
|||
* FMU communications
|
||||
*/
|
||||
extern void comms_init(void);
|
||||
extern void comms_check(void);
|
||||
extern void comms_main(void) __attribute__((noreturn));
|
||||
|
||||
/*
|
||||
* Serial receiver decoders.
|
||||
* R/C receiver handling.
|
||||
*/
|
||||
extern void dsm_init(unsigned mode);
|
||||
extern void dsm_input(int fd);
|
||||
extern void sbus_init(unsigned mode);
|
||||
extern void sbus_input(int fd);
|
||||
extern void controls_main(void);
|
||||
extern int dsm_init(const char *device);
|
||||
extern void dsm_input(void);
|
||||
extern int sbus_init(const char *device);
|
||||
extern void sbus_input(void);
|
||||
|
||||
/*
|
||||
* Assertion codes
|
||||
|
|
|
|||
|
|
@ -51,6 +51,8 @@
|
|||
#include "px4io.h"
|
||||
|
||||
static struct hrt_call arming_call;
|
||||
static struct hrt_call heartbeat_call;
|
||||
static struct hrt_call failsafe_call;
|
||||
|
||||
/*
|
||||
* Count the number of times in a row that we see the arming button
|
||||
|
|
@ -63,13 +65,22 @@ static unsigned counter;
|
|||
|
||||
static bool safety_led_state;
|
||||
static bool safety_button_pressed;
|
||||
|
||||
static void safety_check_button(void *arg);
|
||||
static void heartbeat_blink(void *arg);
|
||||
static void failsafe_blink(void *arg);
|
||||
|
||||
void
|
||||
safety_init(void)
|
||||
{
|
||||
/* arrange for the button handler to be called at 10Hz */
|
||||
hrt_call_every(&arming_call, 1000, 100000, safety_check_button, NULL);
|
||||
|
||||
/* arrange for the heartbeat handler to be called at 4Hz */
|
||||
hrt_call_every(&heartbeat_call, 1000, 250000, heartbeat_blink, NULL);
|
||||
|
||||
/* arrange for the failsafe blinker to be called at 8Hz */
|
||||
hrt_call_every(&failsafe_call, 1000, 125000, failsafe_blink, NULL);
|
||||
}
|
||||
|
||||
static void
|
||||
|
|
@ -117,3 +128,28 @@ safety_check_button(void *arg)
|
|||
}
|
||||
LED_SAFETY(safety_led_state);
|
||||
}
|
||||
|
||||
|
||||
static void
|
||||
heartbeat_blink(void *arg)
|
||||
{
|
||||
static bool heartbeat = false;
|
||||
|
||||
/* XXX add flags here that need to be frobbed by various loops */
|
||||
|
||||
LED_BLUE(heartbeat = !heartbeat);
|
||||
}
|
||||
|
||||
static void
|
||||
failsafe_blink(void *arg)
|
||||
{
|
||||
static bool failsafe = false;
|
||||
|
||||
/* blink the failsafe LED if we don't have FMU input */
|
||||
if (!system_state.mixer_use_fmu) {
|
||||
failsafe = !failsafe;
|
||||
} else {
|
||||
failsafe = false;
|
||||
}
|
||||
LED_AMBER(failsafe);
|
||||
}
|
||||
|
|
@ -41,18 +41,39 @@
|
|||
|
||||
#include <fcntl.h>
|
||||
#include <unistd.h>
|
||||
#include <termios.h>
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#define DEBUG
|
||||
#include "px4io.h"
|
||||
#include "protocol.h"
|
||||
|
||||
void
|
||||
sbus_init(unsigned mode)
|
||||
static int sbus_fd = -1;
|
||||
|
||||
int
|
||||
sbus_init(const char *device)
|
||||
{
|
||||
if (sbus_fd < 0)
|
||||
sbus_fd = open(device, O_RDONLY);
|
||||
|
||||
if (sbus_fd >= 0) {
|
||||
struct termios t;
|
||||
|
||||
/* 100000bps, even parity, two stop bits */
|
||||
tcgetattr(sbus_fd, &t);
|
||||
cfsetspeed(&t, 100000);
|
||||
t.c_cflag |= (CSTOPB | PARENB);
|
||||
tcsetattr(sbus_fd, TCSANOW, &t);
|
||||
|
||||
debug("Sbus: ready");
|
||||
} else {
|
||||
debug("Sbus: open failed");
|
||||
}
|
||||
|
||||
return sbus_fd;
|
||||
}
|
||||
|
||||
void
|
||||
sbus_input(int fd)
|
||||
sbus_input(void)
|
||||
{
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue