Cleaned up calibration, added text messages ring buffer

5 files changed, 163 insertions, 60 deletions

diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index 285b11a45..dafd32ec2 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -291,23 +291,28 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
 	state_machine_publish(status_pub, status, mavlink_fd);
 
 	int sub_mag = orb_subscribe(ORB_ID(sensor_mag));
-	orb_set_interval(sub_mag, 22);
 	struct mag_report mag;
 
-	/* 30 seconds */
+	/* 45 seconds */
 	uint64_t calibration_interval = 45 * 1000 * 1000;
+
+	/* maximum 2000 values */
+	const unsigned int calibration_maxcount = 2000;
 	unsigned int calibration_counter = 0;
 
-	/*
-	 * FLT_MIN is not the most negative float number,
-	 * but the smallest number by magnitude float can
-	 * represent. Use -FLT_MAX to initialize the most
-	 * negative number
-	 */
-	float mag_max[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX};
-	float mag_min[3] = {FLT_MAX, FLT_MAX, FLT_MAX};
+	/* limit update rate to get equally spaced measurements over time (in ms) */
+	orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount);
+
+	// XXX old cal
+	//  * FLT_MIN is not the most negative float number,
+	//  * but the smallest number by magnitude float can
+	//  * represent. Use -FLT_MAX to initialize the most
+	//  * negative number
+	 
+	// float mag_max[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX};
+	// float mag_min[3] = {FLT_MAX, FLT_MAX, FLT_MAX};
 
-	int fd = open(MAG_DEVICE_PATH, 0);
+	int fd = open(MAG_DEVICE_PATH, O_RDONLY);
 
 	/* erase old calibration */
 	struct mag_scale mscale_null = {
@@ -332,7 +337,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
 
 	/* calibrate offsets */
 
-	uint64_t calibration_start = hrt_absolute_time();
+	// uint64_t calibration_start = hrt_absolute_time();
 
 	uint64_t axis_deadline = hrt_absolute_time();
 	uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
@@ -340,7 +345,6 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
 	const char axislabels[3] = { 'Z', 'X', 'Y'};
 	int axis_index = -1;
 
-	const int calibration_maxcount = 2000;
 	float *x = malloc(sizeof(float) * calibration_maxcount);
 	float *y = malloc(sizeof(float) * calibration_maxcount);
 	float *z = malloc(sizeof(float) * calibration_maxcount);
@@ -426,27 +430,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
 	free(y);
 	free(z);
 
-	float mag_offset[3] = {sphere_x, sphere_y, sphere_z};
-
-	// *
-	//  * The offset is subtracted from the sensor values, so the result is the
-	//  * POSITIVE number that has to be subtracted from the sensor data
-	//  * to shift the center to zero
-	//  *
-	//  * offset = max - ((max - min) / 2.0f)
-	//  *          max - max/2 + min/2
-	//  *          max/2 + min/2
-	//  *
-	//  * which reduces to
-	//  *
-	//  * offset = (max + min) / 2.0f
-	 
-
-	// mag_offset[0] = (mag_max[0] + mag_min[0]) / 2.0f;
-	// mag_offset[1] = (mag_max[1] + mag_min[1]) / 2.0f;
-	// mag_offset[2] = (mag_max[2] + mag_min[2]) / 2.0f;
-
-	if (isfinite(mag_offset[0]) && isfinite(mag_offset[1]) && isfinite(mag_offset[2])) {
+	if (isfinite(sphere_x) && isfinite(sphere_y) && isfinite(sphere_z)) {
 
 		fd = open(MAG_DEVICE_PATH, 0);
 
@@ -455,9 +439,9 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
 		if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale))
 			warn("WARNING: failed to get scale / offsets for mag");
 
-		mscale.x_offset = mag_offset[0];
-		mscale.y_offset = mag_offset[1];
-		mscale.z_offset = mag_offset[2];
+		mscale.x_offset = sphere_x;
+		mscale.y_offset = sphere_y;
+		mscale.z_offset = sphere_z;
 
 		if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale))
 			warn("WARNING: failed to set scale / offsets for mag");
@@ -495,13 +479,19 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
 			warn("WARNING: auto-save of params to EEPROM failed");
 		}
 
-		printf("[mag cal] scale: %.6f %.6f %.6f\n\t\toffset: %.6f %.6f %.6f\nradius: %.6f GA\n",
-			mscale.x_scale, mscale.y_scale, mscale.z_scale,
-			mscale.x_offset, mscale.y_offset, mscale.z_offset, sphere_radius);
+		printf("[mag cal]\tscale: %.6f %.6f %.6f\n         \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n",
+			(double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale,
+			(double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset, (double)sphere_radius);
 		
-		// char buf[50];
-		// sprintf(buf, "[commander] mag cal: x:%d y:%d z:%d mGa", (int)(mag_offset[0]*1000), (int)(mag_offset[1]*1000), (int)(mag_offset[2]*1000));
-		// mavlink_log_info(mavlink_fd, buf);
+		char buf[52];
+		sprintf(buf, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset,
+			(double)mscale.y_offset, (double)mscale.z_offset);
+		mavlink_log_info(mavlink_fd, buf);
+
+		sprintf(buf, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale,
+			(double)mscale.y_scale, (double)mscale.z_scale);
+		mavlink_log_info(mavlink_fd, buf);
+
 		mavlink_log_info(mavlink_fd, "[commander] mag calibration done");
 	} else {
 		mavlink_log_info(mavlink_fd, "[commander] mag calibration FAILED (NaN)");
diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp
index fc095bff8..8e78825c3 100644
--- a/apps/drivers/hmc5883/hmc5883.cpp
+++ b/apps/drivers/hmc5883/hmc5883.cpp
@@ -284,9 +284,9 @@ HMC5883::HMC5883(int bus) :
 	_next_report(0),
 	_oldest_report(0),
 	_reports(nullptr),
-	_mag_topic(-1),
 	_range_scale(0), /* default range scale from counts to gauss */
 	_range_ga(1.3f),
+	_mag_topic(-1),
 	_sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")),
 	_comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")),
 	_buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows"))
@@ -950,7 +950,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 		goto out;
 	}
 
-	if (OK != ioctl(filp, MAGIOCEXSTRAP, 0)) {
+	if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) {
 		warnx("failed to disable sensor calibration mode");
 		goto out;
 	}
@@ -969,9 +969,9 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 
 	out:
 		if (ret == OK) {
-			warnx("calibration successfully finished.");
+			warnx("mag scale calibration successfully finished.");
 		} else {
-			warnx("calibration failed.");
+			warnx("mag scale calibration failed.");
 		}
 	return ret;
 }
@@ -1200,7 +1200,6 @@ test()
  */
 int calibrate()
 {
-	ssize_t sz;
 	int ret;
 
 	int fd = open(MAG_DEVICE_PATH, O_RDONLY);
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