Cleaned up calibration, added text messages ring buffer

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