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

Merge branch '#61-px4io-spektrum-decoder' into fixedwing_outdoor

Browse Source
This commit is contained in:
Lorenz Meier
2012-12-01 10:51:25 +01:00
parent 00b79764d7 de88732e8e
commit 121a9fc490
19 changed files with 1085 additions and 515 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/drivers/px4io/px4io.cpp
+47 -69
View File
@@ -53,6 +53,7 @@
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <termios.h>
#include <fcntl.h>
#include <arch/board/board.h>
@@ -88,15 +89,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
@@ -125,8 +124,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
*/
@@ -195,8 +192,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;
@@ -206,33 +202,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;
}
@@ -300,6 +280,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) {
@@ -358,7 +348,6 @@ PX4IO::task_main()
/* this would be bad... */
if (ret < 0) {
log("poll error %d", errno);
usleep(1000000);
continue;
}
@@ -418,8 +407,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;
@@ -480,15 +477,17 @@ PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received)
}
_connected = true;
/* publish raw rc channel values from IO */
_input_rc.timestamp = hrt_absolute_time();
_input_rc.channel_count = rep->channel_count;
for (int i = 0; i < rep->channel_count; i++)
{
_input_rc.values[i] = rep->rc_channel[i];
}
/* publish raw rc channel values from IO if valid channels are present */
if (rep->channel_count > 0) {
_input_rc.timestamp = hrt_absolute_time();
_input_rc.channel_count = rep->channel_count;
for (int i = 0; i < rep->channel_count; i++)
{
_input_rc.values[i] = rep->rc_channel[i];
}
orb_publish(ORB_ID(input_rc), _to_input_rc, &_input_rc);
orb_publish(ORB_ID(input_rc), _to_input_rc, &_input_rc);
}
/* remember the latched arming switch state */
_switch_armed = rep->armed;
@@ -531,7 +530,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)
@@ -651,15 +649,6 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
return ret;
}
void
PX4IO::set_rx_mode(unsigned mode)
{
if (mode != _rx_mode) {
_rx_mode = mode;
_config_needed = true;
}
}
extern "C" __EXPORT int px4io_main(int argc, char *argv[]);
namespace
@@ -761,25 +750,14 @@ px4io_main(int argc, char *argv[])
return ret;
}
if (!strcmp(argv[1], "rx_dsm_10bit")) {
if (g_dev == nullptr)
errx(1, "not started");
g_dev->set_rx_mode(RX_MODE_DSM_10BIT);
}
if (!strcmp(argv[1], "rx_dsm_11bit")) {
if (g_dev == nullptr)
errx(1, "not started");
g_dev->set_rx_mode(RX_MODE_DSM_11BIT);
}
if (!strcmp(argv[1], "rx_sbus")) {
if (g_dev == nullptr)
errx(1, "not started");
g_dev->set_rx_mode(RX_MODE_FUTABA_SBUS);
}
if (!strcmp(argv[1], "rx_dsm") ||
!strcmp(argv[1], "rx_dsm_10bit") ||
!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();
errx(1, "need a command, try 'start', 'test', 'rx_dsm_10bit', 'rx_dsm_11bit', 'rx_sbus' or 'update'");
errx(1, "need a command, try 'start', 'test', 'rx_ppm', 'rx_dsm', 'rx_sbus' or 'update'");
}
apps/drivers/px4io/uploader.cpp
+4 -2
View File
@@ -189,8 +189,10 @@ PX4IO_Uploader::drain()
int ret;
do {
ret = recv(c, 10);
//log("discard 0x%02x", c);
ret = recv(c, 250);
if (ret == OK) {
//log("discard 0x%02x", c);
}
} while (ret == OK);
}