diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-10-22 08:14:43 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-10-22 08:14:43 +0200 |
commit | df8148033a1f60400693e80c3732a43cc26e0ee2 (patch) | |
tree | 5d2cbc85bbe40ad41ff8e0cb2854e17c3d4149d8 /apps/mavlink/mavlink.c | |
parent | 096bf2dc93fe8360fa83bee409452f8db7bc3593 (diff) | |
download | px4-firmware-df8148033a1f60400693e80c3732a43cc26e0ee2.tar.gz px4-firmware-df8148033a1f60400693e80c3732a43cc26e0ee2.tar.bz2 px4-firmware-df8148033a1f60400693e80c3732a43cc26e0ee2.zip |
Cleaned up calibration, added text messages ring buffer
Diffstat (limited to 'apps/mavlink/mavlink.c')
-rw-r--r-- | apps/mavlink/mavlink.c | 29 |
1 files changed, 19 insertions, 10 deletions
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 293bbe00c..fd02eb363 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -1,7 +1,7 @@ - /**************************************************************************** +/**************************************************************************** * * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * Author: Lorenz Meier <lm@inf.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,6 +35,8 @@ /** * @file mavlink.c * MAVLink 1.0 protocol implementation. + * + * @author Lorenz Meier <lm@inf.ethz.ch> */ #include <nuttx/config.h> @@ -114,9 +116,6 @@ mavlink_wpm_storage *wpm = &wpm_s; bool mavlink_hil_enabled = false; -/* buffer for message strings */ -static char mavlink_message_string[51] = {0}; - /* protocol interface */ static int uart; static int baudrate; @@ -127,6 +126,8 @@ static enum { MAVLINK_INTERFACE_MODE_ONBOARD } mavlink_link_mode = MAVLINK_INTERFACE_MODE_OFFBOARD; +static struct mavlink_logbuffer lb; + static void mavlink_update_system(void); static int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); static void usage(void); @@ -332,7 +333,9 @@ mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) case (int)MAVLINK_IOC_SEND_TEXT_CRITICAL: case (int)MAVLINK_IOC_SEND_TEXT_EMERGENCY: { const char *txt = (const char *)arg; - strncpy(mavlink_message_string, txt, 51); + struct mavlink_logmessage msg; + strncpy(msg.text, txt, sizeof(msg.text)); + mavlink_logbuffer_write(&lb, &msg); total_counter++; return OK; } @@ -489,6 +492,9 @@ void mavlink_update_system(void) */ int mavlink_thread_main(int argc, char *argv[]) { + /* initialize mavlink text message buffering */ + mavlink_logbuffer_init(&lb, 5); + int ch; char *device_name = "/dev/ttyS1"; baudrate = 57600; @@ -662,9 +668,12 @@ int mavlink_thread_main(int argc, char *argv[]) usleep(10000); /* send one string at 10 Hz */ - if (mavlink_message_string[0] != '\0') { - mavlink_missionlib_send_gcs_string(mavlink_message_string); - mavlink_message_string[0] = '\0'; + if (!mavlink_logbuffer_is_empty(&lb)) { + struct mavlink_logmessage msg; + int lb_ret = mavlink_logbuffer_read(&lb, &msg); + if (lb_ret == OK) { + mavlink_missionlib_send_gcs_string(msg.text); + } } /* sleep 15 ms */ @@ -711,7 +720,7 @@ int mavlink_main(int argc, char *argv[]) SCHED_PRIORITY_DEFAULT, 6000, mavlink_thread_main, - argv); + (const char**)argv); exit(0); } |