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

Simulator: UART changes 

Some changes were needed to use the simulator and the UART for rc control.

Signed-off-by: Mark Charlebois <charlebm@gmail.com>
This commit is contained in:
Mark Charlebois 2015-06-03 14:30:35 -07:00
parent 5c013af574
commit fd1effa4fe
1 changed files with 109 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -30,7 +30,7 @@
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include <termios.h>
#include <px4_log.h>
#include <px4_time.h>
#include "simulator.h"
@ -46,6 +46,8 @@ using namespace simulator;
static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS;
static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS;
static int openUart(const char *uart_name, int baud);
void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg) {
float out[8];
@ -393,7 +395,7 @@ void Simulator::updateSamples()
(void)pthread_attr_setschedparam(&sender_thread_attr, &param);
// setup serial connection to autopilot (used to get manual controls)
int serial_fd = open(PIXHAWK_DEVICE, O_RDWR);
int serial_fd = openUart(PIXHAWK_DEVICE, 115200);
if (serial_fd < 0) {
PX4_WARN("failed to open %s", PIXHAWK_DEVICE);
@ -417,7 +419,8 @@ void Simulator::updateSamples()
int len = 0;
// wait for first data from simulator
// wait for first data from simulator and respond with first controls
// this is important for the UDP communication to work
int pret = -1;
while (pret <= 0) {
pret = ::poll(&fds[0], (sizeof(fds[0])/sizeof(fds[0])), 100);
@ -425,6 +428,7 @@ void Simulator::updateSamples()
if (fds[0].revents & POLLIN) {
len = recvfrom(_fd, _buf, sizeof(_buf), 0, (struct sockaddr *)&_srcaddr, &_addrlen);
send_data();
}
// got data from simulator, now activate the sending thread
@ -483,3 +487,105 @@ void Simulator::updateSamples()
}
}
}
int openUart(const char *uart_name, int baud)
{
/* process baud rate */
int speed;
switch (baud) {
case 0: speed = B0; break;
case 50: speed = B50; break;
case 75: speed = B75; break;
case 110: speed = B110; break;
case 134: speed = B134; break;
case 150: speed = B150; break;
case 200: speed = B200; break;
case 300: speed = B300; break;
case 600: speed = B600; break;
case 1200: speed = B1200; break;
case 1800: speed = B1800; break;
case 2400: speed = B2400; break;
case 4800: speed = B4800; break;
case 9600: speed = B9600; break;
case 19200: speed = B19200; break;
case 38400: speed = B38400; break;
case 57600: speed = B57600; break;
case 115200: speed = B115200; break;
case 230400: speed = B230400; break;
case 460800: speed = B460800; break;
case 921600: speed = B921600; break;
default:
warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600\t\n115200\n230400\n460800\n921600\n",
baud);
return -EINVAL;
}
/* open uart */
int uart_fd = ::open(uart_name, O_RDWR | O_NOCTTY);
if (uart_fd < 0) {
return uart_fd;
}
/* Try to set baud rate */
struct termios uart_config;
memset(&uart_config, 0, sizeof(uart_config));
int termios_state;
/* Back up the original uart configuration to restore it after exit */
if ((termios_state = tcgetattr(uart_fd, &uart_config)) < 0) {
warnx("ERR GET CONF %s: %d\n", uart_name, termios_state);
::close(uart_fd);
return -1;
}
/* Fill the struct for the new configuration */
tcgetattr(uart_fd, &uart_config);
/* USB serial is indicated by /dev/ttyACM0*/
if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/ttyACM1") != OK) {
/* Set baud rate */
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
warnx("ERR SET BAUD %s: %d\n", uart_name, termios_state);
::close(uart_fd);
return -1;
}
}
// Make raw
cfmakeraw(&uart_config);
if ((termios_state = tcsetattr(uart_fd, TCSANOW, &uart_config)) < 0) {
warnx("ERR SET CONF %s\n", uart_name);
::close(uart_fd);
return -1;
}
return uart_fd;
}