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 | |
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')
-rw-r--r-- | apps/mavlink/mavlink.c | 29 | ||||
-rw-r--r-- | apps/mavlink/mavlink_log.c | 80 | ||||
-rw-r--r-- | apps/mavlink/mavlink_log.h | 29 |
3 files changed, 126 insertions, 12 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); } diff --git a/apps/mavlink/mavlink_log.c b/apps/mavlink/mavlink_log.c new file mode 100644 index 000000000..660e282f8 --- /dev/null +++ b/apps/mavlink/mavlink_log.c @@ -0,0 +1,80 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mavlink_log.c + * MAVLink text logging. + * + * @author Lorenz Meier <lm@inf.ethz.ch> + */ + +#include <string.h> + +#include "mavlink_log.h" + +void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size) { + lb->size = size; + lb->start = 0; + lb->count = 0; + lb->elems = (struct mavlink_logmessage *)calloc(lb->size, sizeof(struct mavlink_logmessage)); +} + +int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb) { + return lb->count == lb->size; +} + +int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb) { + return lb->count == 0; +} + +void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_logmessage *elem) { + int end = (lb->start + lb->count) % lb->size; + memcpy(&(lb->elems[end]), elem, sizeof(struct mavlink_logmessage)); + if (mavlink_logbuffer_is_full(lb)) { + lb->start = (lb->start + 1) % lb->size; /* full, overwrite */ + } else { + ++lb->count; + } +} + +int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem) { + if (!mavlink_logbuffer_is_empty(lb)) { + memcpy(elem, &(lb->elems[lb->start]), sizeof(struct mavlink_logmessage)); + lb->start = (lb->start + 1) % lb->size; + --lb->count; + return 0; + } else { + return 1; + } +}
\ No newline at end of file diff --git a/apps/mavlink/mavlink_log.h b/apps/mavlink/mavlink_log.h index 1ad1364be..cb6fd9d98 100644 --- a/apps/mavlink/mavlink_log.h +++ b/apps/mavlink/mavlink_log.h @@ -1,7 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * 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_log.h * MAVLink text logging. + * + * @author Lorenz Meier <lm@inf.ethz.ch> */ #ifndef MAVLINK_LOG @@ -79,5 +81,28 @@ */ #define mavlink_log_info(_fd, _text) ioctl(_fd, MAVLINK_IOC_SEND_TEXT_INFO, (unsigned long)_text); +struct mavlink_logmessage { + char text[51]; + unsigned char severity; +}; + +struct mavlink_logbuffer { + unsigned int start; + // unsigned int end; + unsigned int size; + int count; + struct mavlink_logmessage *elems; +}; + +void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size); + +int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb); + +int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb); + +void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_logmessage *elem); + +int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem); + #endif |