dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge pull request #8 from tumbili/serial_device 

read serial device to obtain manual control setpoint
This commit is contained in:
mcharleb 2015-05-12 14:36:18 -07:00
parent 3db5f3bb3b 3a79679e2d
commit 21d7e4f24f
1 changed files with 46 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
src/modules/simulator/simulator_mavlink.cpp
View File

@ -40,6 +40,7 @@ using namespace simulator;
#define SEND_INTERVAL 1000
#define UDP_PORT 14550
#define PIXHAWK_DEVICE "/dev/ttyACM0"
static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS;
static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS;
@ -308,29 +309,49 @@ void Simulator::updateSamples()
pthread_create(&sender_thread, &sender_thread_attr, Simulator::sending_trampoline, NULL);
pthread_attr_destroy(&sender_thread_attr);
struct pollfd socket_fds;
socket_fds.fd = _fd;
socket_fds.events = POLLIN;
// setup serial connection to autopilot (used to get manual controls)
int serial_fd = open(PIXHAWK_DEVICE, O_RDWR);
if (serial_fd < 0) {
PX4_WARN("failed to open %s\n", PIXHAWK_DEVICE);
}
// tell the device to stream some messages
char command[] = "\nsh /etc/init.d/rc.usb\n";
int w = ::write(serial_fd, command, sizeof(command));
if (w <= 0) {
PX4_WARN("failed to send streaming command to %s\n", PIXHAWK_DEVICE);
}
char serial_buf[1024];
struct pollfd fds[2];
fds[0].fd = _fd;
fds[0].events = POLLIN;
fds[1].fd = serial_fd;
fds[1].events = POLLIN;
int len = 0;
// wait for new mavlink messages to arrive
while (true) {
int socket_pret = ::poll(&socket_fds, (size_t)1, 100);
int pret = ::poll(&fds[0], (sizeof(fds)/sizeof(fds[0])), 100);
//timed out
if (socket_pret == 0)
if (pret == 0)
continue;
// this is undesirable but not much we can do
if (socket_pret < 0) {
PX4_WARN("poll error %d, %d", socket_pret, errno);
if (pret < 0) {
PX4_WARN("poll error %d, %d", pret, errno);
// sleep a bit before next try
usleep(100000);
continue;
}
if (socket_fds.revents & POLLIN) {
// got data from simulator
if (fds[0].revents & POLLIN) {
len = recvfrom(_fd, _buf, sizeof(_buf), 0, (struct sockaddr *)&_srcaddr, &_addrlen);
if (len > 0) {
mavlink_message_t msg;
@ -346,6 +367,23 @@ void Simulator::updateSamples()
}
}
// got data from PIXHAWK
if (fds[1].revents & POLLIN) {
len = ::read(serial_fd, serial_buf, sizeof(serial_buf));
if (len > 0) {
mavlink_message_t msg;
mavlink_status_t status;
for (int i = 0; i < len; ++i)
{
if (mavlink_parse_char(MAVLINK_COMM_0, serial_buf[i], &msg, &status))
{
// have a message, handle it
handle_message(&msg);
}
}
}
}
// publish these messages so that attitude estimator does not complain
hrt_abstime time_last = hrt_absolute_time();
baro.timestamp = time_last;