fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-24 03:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Rework the PX4IO software architecture:

Browse Source 
- 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
2012-11-30 00:02:47 -08:00
parent e153476950
commit 9fa794a8fa
10 changed files with 288 additions and 308 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/px4io/comms.c
+38 -113
View File
@@ -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++;
}