Merge pull request #277 from PX4/preflight_check

Preflight check improvements

7 files changed, 158 insertions, 33 deletions

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index a40142792..8b2fae12b 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -1273,6 +1273,9 @@ PX4IO::print_status()
 		((alarms & PX4IO_P_STATUS_ALARMS_FMU_LOST)      ? " FMU_LOST" : ""),
 		((alarms & PX4IO_P_STATUS_ALARMS_RC_LOST)       ? " RC_LOST" : ""),
 		((alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR)     ? " PWM_ERROR" : ""));
+	/* now clear alarms */
+	io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, 0xFFFF);
+
 	printf("vbatt %u ibatt %u vbatt scale %u\n",
 	       io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT),
 	       io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT),
diff --git a/src/include/mavlink/mavlink_log.h b/src/include/mavlink/mavlink_log.h
index 233a76cb3..a28ff3a68 100644
--- a/src/include/mavlink/mavlink_log.h
+++ b/src/include/mavlink/mavlink_log.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
  *   Author: Lorenz Meier <lm@inf.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
@@ -47,27 +47,42 @@
  */
 #include <sys/ioctl.h>
 
-/*
+/**
  * The mavlink log device node; must be opened before messages
  * can be logged.
  */
 #define MAVLINK_LOG_DEVICE			"/dev/mavlink"
+/**
+ * The maximum string length supported.
+ */
+#define MAVLINK_LOG_MAXLEN			50
 
 #define MAVLINK_IOC_SEND_TEXT_INFO		_IOC(0x1100, 1)
 #define MAVLINK_IOC_SEND_TEXT_CRITICAL		_IOC(0x1100, 2)
 #define MAVLINK_IOC_SEND_TEXT_EMERGENCY		_IOC(0x1100, 3)
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+__EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...);
+#ifdef __cplusplus
+}
+#endif
+
+/*
+ * The va_args implementation here is not beautiful, but obviously we run into the same issues
+ * the GCC devs saw, and are using their solution:
+ *
+ * http://gcc.gnu.org/onlinedocs/cpp/Variadic-Macros.html
+ */
+
 /**
  * Send a mavlink emergency message.
  *
  * @param _fd		A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
  * @param _text		The text to log;
  */
-#ifdef __cplusplus
-#define mavlink_log_emergency(_fd, _text)	::ioctl(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, (unsigned long)_text);
-#else
-#define mavlink_log_emergency(_fd, _text)	ioctl(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, (unsigned long)_text);
-#endif
+#define mavlink_log_emergency(_fd, _text, ...)		mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, _text, ##__VA_ARGS__);
 
 /**
  * Send a mavlink critical message.
@@ -75,11 +90,7 @@
  * @param _fd		A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
  * @param _text		The text to log;
  */
-#ifdef __cplusplus
-#define mavlink_log_critical(_fd, _text)	::ioctl(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, (unsigned long)_text);
-#else
-#define mavlink_log_critical(_fd, _text)	ioctl(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, (unsigned long)_text);
-#endif
+#define mavlink_log_critical(_fd, _text, ...)		mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, _text, ##__VA_ARGS__);
 
 /**
  * Send a mavlink info message.
@@ -87,14 +98,10 @@
  * @param _fd		A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
  * @param _text		The text to log;
  */
-#ifdef __cplusplus
-#define mavlink_log_info(_fd, _text)		::ioctl(_fd, MAVLINK_IOC_SEND_TEXT_INFO, (unsigned long)_text);
-#else
-#define mavlink_log_info(_fd, _text)		ioctl(_fd, MAVLINK_IOC_SEND_TEXT_INFO, (unsigned long)_text);
-#endif
+#define mavlink_log_info(_fd, _text, ...)		mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_INFO, _text, ##__VA_ARGS__);
 
 struct mavlink_logmessage {
-	char text[51];
+	char text[MAVLINK_LOG_MAXLEN + 1];
 	unsigned char severity;
 };
 
@@ -116,5 +123,7 @@ void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_
 
 int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem);
 
+void mavlink_logbuffer_vasprintf(struct mavlink_logbuffer *lb, int severity, const char *fmt, ...);
+
 #endif
 
diff --git a/src/modules/mavlink/mavlink_log.c b/src/modules/mavlink/mavlink_log.c
index fa974dc0b..d9416a08b 100644
--- a/src/modules/mavlink/mavlink_log.c
+++ b/src/modules/mavlink/mavlink_log.c
@@ -41,6 +41,7 @@
 
 #include <string.h>
 #include <stdlib.h>
+#include <stdarg.h>
 
 #include <mavlink/mavlink_log.h>
 
@@ -87,3 +88,31 @@ int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessa
 		return 1;
 	}
 }
+
+void mavlink_logbuffer_vasprintf(struct mavlink_logbuffer *lb, int severity, const char *fmt, ...)
+{
+	va_list ap;
+	va_start(ap, fmt);
+	int end = (lb->start + lb->count) % lb->size;
+	lb->elems[end].severity = severity;
+	vsnprintf(lb->elems[end].text, sizeof(lb->elems[0].text), fmt, ap);
+	va_end(ap);
+
+	/* increase count */
+	if (mavlink_logbuffer_is_full(lb)) {
+		lb->start = (lb->start + 1) % lb->size; /* full, overwrite */
+
+	} else {
+		++lb->count;
+	}
+}
+
+__EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...)
+{
+	va_list ap;
+	va_start(ap, fmt);
+	char text[MAVLINK_LOG_MAXLEN + 1];
+	vsnprintf(text, sizeof(text), fmt, ap);
+	va_end(ap);
+	ioctl(_fd, severity, (unsigned long)&text[0]);
+}
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index dc36f6c93..3cf9ca149 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -217,9 +217,8 @@ controls_tick() {
 		if (assigned_channels == 0) {
 			rc_input_lost = true;
 		} else {
-			/* set RC OK flag and clear RC lost alarm */
+			/* set RC OK flag */
 			r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK;
-			r_status_alarms &= ~PX4IO_P_STATUS_ALARMS_RC_LOST;
 		}
 
 		/*
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index 5ada1b220..1234e2eea 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -100,7 +100,6 @@ mixer_tick(void)
 
 	} else {
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK;
-		r_status_alarms &= ~PX4IO_P_STATUS_ALARMS_FMU_LOST;
 	}
 
 	source = MIX_FAILSAFE;
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 61049c8b6..9698a0f2f 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -203,7 +203,6 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
 
 		system_state.fmu_data_received_time = hrt_absolute_time();
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK;
-        	r_status_alarms &= ~PX4IO_P_STATUS_ALARMS_FMU_LOST;
 		r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RAW_PWM;
 		
 		break;
@@ -413,18 +412,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 				if (conf[PX4IO_P_RC_CONFIG_CENTER] > conf[PX4IO_P_RC_CONFIG_MAX]) {
 					count++;
 				}
-
 				/* assert deadzone is sane */
 				if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) {
 					count++;
 				}
-				// The following check isn't needed as constraint checks in controls.c will catch this.
-				//if (conf[PX4IO_P_RC_CONFIG_MIN] > (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
-				//	count++;
-				//}
-				//if (conf[PX4IO_P_RC_CONFIG_MAX] < (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
-				//	count++;
-				//}
 				if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS) {
 					count++;
 				}
diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c
index 42256be61..4bcce18fb 100644
--- a/src/systemcmds/preflight_check/preflight_check.c
+++ b/src/systemcmds/preflight_check/preflight_check.c
@@ -41,10 +41,12 @@
 #include <unistd.h>
 #include <stdlib.h>
 #include <stdio.h>
+#include <string.h>
 #include <fcntl.h>
 #include <errno.h>
 
 #include <systemlib/err.h>
+#include <systemlib/param/param.h>
 
 #include <drivers/drv_led.h>
 #include <drivers/drv_hrt.h>
@@ -98,7 +100,7 @@ int preflight_check_main(int argc, char *argv[])
 	
 	if (ret != OK) {
 		warnx("magnetometer calibration missing or bad - calibrate magnetometer first");
-		mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CALIBRATION");
+		mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CHECK/CAL");
 		system_ok = false;
 		goto system_eval;
 	}
@@ -111,7 +113,7 @@ int preflight_check_main(int argc, char *argv[])
 	
 	if (ret != OK) {
 		warnx("accel self test failed");
-		mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL CHECK");
+		mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL CHECK/CAL");
 		system_ok = false;
 		goto system_eval;
 	}
@@ -124,7 +126,7 @@ int preflight_check_main(int argc, char *argv[])
 	
 	if (ret != OK) {
 		warnx("gyro self test failed");
-		mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CHECK");
+		mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CHECK/CAL");
 		system_ok = false;
 		goto system_eval;
 	}
@@ -134,6 +136,99 @@ int preflight_check_main(int argc, char *argv[])
 	close(fd);
 	fd = open(BARO_DEVICE_PATH, 0);
 
+	/* ---- RC CALIBRATION ---- */
+
+	param_t _parameter_handles_min, _parameter_handles_trim, _parameter_handles_max,
+	_parameter_handles_rev, _parameter_handles_dz;
+
+	float param_min, param_max, param_trim, param_rev, param_dz;
+
+	bool rc_ok = true;
+	char nbuf[20];
+
+	for (int i = 0; i < 12; i++) {
+		/* should the channel be enabled? */
+		uint8_t count = 0;
+
+		/* min values */
+		sprintf(nbuf, "RC%d_MIN", i + 1);
+		_parameter_handles_min = param_find(nbuf);
+		param_get(_parameter_handles_min, &param_min);
+
+		/* trim values */
+		sprintf(nbuf, "RC%d_TRIM", i + 1);
+		_parameter_handles_trim = param_find(nbuf);
+		param_get(_parameter_handles_trim, &param_trim);
+
+		/* max values */
+		sprintf(nbuf, "RC%d_MAX", i + 1);
+		_parameter_handles_max = param_find(nbuf);
+		param_get(_parameter_handles_max, &param_max);
+
+		/* channel reverse */
+		sprintf(nbuf, "RC%d_REV", i + 1);
+		_parameter_handles_rev = param_find(nbuf);
+		param_get(_parameter_handles_rev, &param_rev);
+
+		/* channel deadzone */
+		sprintf(nbuf, "RC%d_DZ", i + 1);
+		_parameter_handles_dz = param_find(nbuf);
+		param_get(_parameter_handles_dz, &param_dz);
+
+		/* assert min..center..max ordering */
+		if (param_min < 500) {
+			count++;
+			mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MIN < 500", i+1);
+			/* give system time to flush error message in case there are more */
+			usleep(100000);
+		}
+		if (param_max > 2500) {
+			count++;
+			mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAX > 2500", i+1);
+			/* give system time to flush error message in case there are more */
+			usleep(100000);
+		}
+		if (param_trim < param_min) {
+			count++;
+			mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM < MIN", i+1);
+			/* give system time to flush error message in case there are more */
+			usleep(100000);
+		}
+		if (param_trim > param_max) {
+			count++;
+			mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM > MAX", i+1);
+			/* give system time to flush error message in case there are more */
+			usleep(100000);
+		}
+
+		/* assert deadzone is sane */
+		if (param_dz > 500) {
+			mavlink_log_critical(mavlink_fd, "ERR: RC_%d_DZ > 500", i+1);
+			/* give system time to flush error message in case there are more */
+			usleep(100000);
+			count++;
+		}
+
+		/* XXX needs inspection of all the _MAP params */
+		// if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS) {
+		// 	mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1);
+		// 	/* give system time to flush error message in case there are more */
+		// 	usleep(100000);
+		// 	count++;
+		// }
+
+		/* sanity checks pass, enable channel */
+		if (count) {
+			mavlink_log_critical(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1));
+			usleep(100000);
+			rc_ok = false;
+		}
+	}
+
+	/* require RC ok to keep system_ok */
+	system_ok &= rc_ok;
+
+
 		
 
 system_eval: