moved commander to C++, preparation for better gyro scale calibration respecting the current attitude for more accurate results

13 files changed, 156 insertions, 86 deletions

diff --git a/src/modules/commander/accelerometer_calibration.c b/src/modules/commander/accelerometer_calibration.cpp
index bc9d24956..df92d51d2 100644
--- a/src/modules/commander/accelerometer_calibration.c
+++ b/src/modules/commander/accelerometer_calibration.cpp
@@ -33,7 +33,7 @@
  ****************************************************************************/
 
 /**
- * @file accelerometer_calibration.c
+ * @file accelerometer_calibration.cpp
  *
  * Implementation of accelerometer calibration.
  *
@@ -113,8 +113,6 @@
 #include <sys/prctl.h>
 #include <math.h>
 #include <string.h>
-
-
 #include <drivers/drv_hrt.h>
 #include <uORB/topics/sensor_combined.h>
 #include <drivers/drv_accel.h>
@@ -123,6 +121,12 @@
 #include <systemlib/err.h>
 #include <mavlink/mavlink_log.h>
 
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
 int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]);
 int detect_orientation(int mavlink_fd, int sub_sensor_combined);
 int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num);
@@ -229,7 +233,10 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
 		sprintf(str, "meas started: %s", orientation_strs[orient]);
 		mavlink_log_info(mavlink_fd, str);
 		read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num);
-		str_ptr = sprintf(str, "meas result for %s: [ %.2f %.2f %.2f ]", orientation_strs[orient], accel_ref[orient][0], accel_ref[orient][1], accel_ref[orient][2]);
+		str_ptr = sprintf(str, "meas result for %s: [ %.2f %.2f %.2f ]", orientation_strs[orient],
+			(double)accel_ref[orient][0],
+			(double)accel_ref[orient][1],
+			(double)accel_ref[orient][2]);
 		mavlink_log_info(mavlink_fd, str);
 		data_collected[orient] = true;
 		tune_neutral();
@@ -274,7 +281,9 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
 	float accel_err_thr = 5.0f;
 	/* still time required in us */
 	int64_t still_time = 2000000;
-	struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } };
+	struct pollfd fds[1];
+	fds[0].fd = sub_sensor_combined;
+	fds[0].events = POLLIN;
 
 	hrt_abstime t_start = hrt_absolute_time();
 	/* set timeout to 30s */
@@ -342,29 +351,29 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
 		}
 	}
 
-	if (  fabs(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr &&
-		  fabs(accel_ema[1]) < accel_err_thr &&
-		  fabs(accel_ema[2]) < accel_err_thr  )
+	if (  fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr &&
+		  fabsf(accel_ema[1]) < accel_err_thr &&
+		  fabsf(accel_ema[2]) < accel_err_thr  )
 		return 0;	// [ g, 0, 0 ]
-	if (  fabs(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr &&
-		  fabs(accel_ema[1]) < accel_err_thr &&
-		  fabs(accel_ema[2]) < accel_err_thr  )
+	if (  fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr &&
+		  fabsf(accel_ema[1]) < accel_err_thr &&
+		  fabsf(accel_ema[2]) < accel_err_thr  )
 		return 1;	// [ -g, 0, 0 ]
-	if (  fabs(accel_ema[0]) < accel_err_thr &&
-		  fabs(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr &&
-		  fabs(accel_ema[2]) < accel_err_thr  )
+	if (  fabsf(accel_ema[0]) < accel_err_thr &&
+		  fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr &&
+		  fabsf(accel_ema[2]) < accel_err_thr  )
 		return 2;	// [ 0, g, 0 ]
-	if (  fabs(accel_ema[0]) < accel_err_thr &&
-		  fabs(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr &&
-		  fabs(accel_ema[2]) < accel_err_thr  )
+	if (  fabsf(accel_ema[0]) < accel_err_thr &&
+		  fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr &&
+		  fabsf(accel_ema[2]) < accel_err_thr  )
 		return 3;	// [ 0, -g, 0 ]
-	if (  fabs(accel_ema[0]) < accel_err_thr &&
-		  fabs(accel_ema[1]) < accel_err_thr &&
-		  fabs(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr  )
+	if (  fabsf(accel_ema[0]) < accel_err_thr &&
+		  fabsf(accel_ema[1]) < accel_err_thr &&
+		  fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr  )
 		return 4;	// [ 0, 0, g ]
-	if (  fabs(accel_ema[0]) < accel_err_thr &&
-		  fabs(accel_ema[1]) < accel_err_thr &&
-		  fabs(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr  )
+	if (  fabsf(accel_ema[0]) < accel_err_thr &&
+		  fabsf(accel_ema[1]) < accel_err_thr &&
+		  fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr  )
 		return 5;	// [ 0, 0, -g ]
 
 	mavlink_log_info(mavlink_fd, "ERROR: invalid orientation");
@@ -376,7 +385,9 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
  * Read specified number of accelerometer samples, calculate average and dispersion.
  */
 int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num) {
-	struct pollfd fds[1] = { { .fd = sensor_combined_sub, .events = POLLIN } };
+	struct pollfd fds[1];
+	fds[0].fd = sensor_combined_sub;
+	fds[0].events = POLLIN;
 	int count = 0;
 	float accel_sum[3] = { 0.0f, 0.0f, 0.0f };
 
diff --git a/src/modules/commander/airspeed_calibration.c b/src/modules/commander/airspeed_calibration.cpp
index feaf11aee..df08292e3 100644
--- a/src/modules/commander/airspeed_calibration.c
+++ b/src/modules/commander/airspeed_calibration.cpp
@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file airspeed_calibration.c
+ * @file airspeed_calibration.cpp
  * Airspeed sensor calibration routine
  */
 
@@ -65,7 +65,9 @@ void do_airspeed_calibration(int mavlink_fd)
 	while (calibration_counter < calibration_count) {
 
 		/* wait blocking for new data */
-		struct pollfd fds[1] = { { .fd = diff_pres_sub, .events = POLLIN } };
+		struct pollfd fds[1];
+		fds[0].fd = diff_pres_sub;
+		fds[0].events = POLLIN;
 
 		int poll_ret = poll(fds, 1, 1000);
 
diff --git a/src/modules/commander/baro_calibration.c b/src/modules/commander/baro_calibration.cpp
index a70594794..d7515b3d9 100644
--- a/src/modules/commander/baro_calibration.c
+++ b/src/modules/commander/baro_calibration.cpp
@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file baro_calibration.c
+ * @file baro_calibration.cpp
  * Barometer calibration routine
  */
 
diff --git a/src/modules/commander/calibration_routines.c b/src/modules/commander/calibration_routines.cpp
index 799cd4269..be38ea104 100644
--- a/src/modules/commander/calibration_routines.c
+++ b/src/modules/commander/calibration_routines.cpp
@@ -33,7 +33,7 @@
  ****************************************************************************/
 
 /**
- * @file calibration_routines.c
+ * @file calibration_routines.cpp
  * Calibration routines implementations.
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.cpp
index c4dc495f6..253b6295f 100644
--- a/src/modules/commander/commander.c
+++ b/src/modules/commander/commander.cpp
@@ -36,13 +36,11 @@
  ****************************************************************************/
 
 /**
- * @file commander.c
+ * @file commander.cpp
  * Main system state machine implementation.
  *
  */
 
-#include "commander.h"
-
 #include <nuttx/config.h>
 #include <pthread.h>
 #include <stdio.h>
@@ -97,12 +95,11 @@
 #include "rc_calibration.h"
 #include "airspeed_calibration.h"
 
-PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0);	/**< Go into low-level failsafe after 0 ms */
-//PARAM_DEFINE_INT32(SYS_FAILSAVE_HL, 0);	/**< Go into high-level failsafe after 0 ms */
-PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f);
-PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f);
-PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f);
-
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
 
 extern struct system_load_s system_load;
 
@@ -160,8 +157,10 @@ enum {
  *
  * The actual stack size should be set in the call
  * to task_create().
+ *
+ * @ingroup apps
  */
-__EXPORT int commander_main(int argc, char *argv[]);
+extern "C" __EXPORT int commander_main(int argc, char *argv[]);
 
 /**
  * Print the correct usage.
diff --git a/src/modules/commander/commander_helper.c b/src/modules/commander/commander_helper.cpp
index 199f73e6c..9427bd892 100644
--- a/src/modules/commander/commander_helper.c
+++ b/src/modules/commander/commander_helper.cpp
@@ -34,7 +34,7 @@
  ****************************************************************************/
 
 /**
- * @file commander_helper.c
+ * @file commander_helper.cpp
  * Commander helper functions implementations
  */
 
@@ -56,6 +56,12 @@
 
 #include "commander_helper.h"
 
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
 bool is_multirotor(const struct vehicle_status_s *current_status)
 {
 	return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) ||
@@ -175,11 +181,6 @@ int led_off(int led)
 	return ioctl(leds, LED_OFF, led);
 }
 
-
-PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f);
-PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f);
-PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3);
-
 float battery_remaining_estimate_voltage(float voltage)
 {
 	float ret = 0;
diff --git a/src/modules/commander/commander.h b/src/modules/commander/commander_params.c
index 6e57c0ba5..6832fc5ce 100644
--- a/src/modules/commander/commander.h
+++ b/src/modules/commander/commander_params.c
@@ -1,10 +1,7 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
- *           Lorenz Meier <lm@inf.ethz.ch>
- *           Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
+ *   Copyright (c) 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
  * modification, are permitted provided that the following conditions
@@ -36,19 +33,22 @@
  ****************************************************************************/
 
 /**
- * @file commander.h
- * Main system state machine definition.
+ * @file commander_params.c
+ *
+ * Parameters defined by the sensors task.
  *
- * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
  * @author Lorenz Meier <lm@inf.ethz.ch>
  * @author Thomas Gubler <thomasgubler@student.ethz.ch>
  * @author Julian Oes <joes@student.ethz.ch>
- *
  */
 
-#ifndef COMMANDER_H_
-#define COMMANDER_H_
-
-
+#include <nuttx/config.h>
+#include <systemlib/param/param.h>
 
-#endif /* COMMANDER_H_ */
+PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0);	/**< Go into low-level failsafe after 0 ms */
+PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f);
+PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f);
+PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f);
+PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f);
+PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f);
+PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3);
diff --git a/src/modules/commander/gyro_calibration.c b/src/modules/commander/gyro_calibration.cpp
index 865f74ab4..9e6909db0 100644
--- a/src/modules/commander/gyro_calibration.c
+++ b/src/modules/commander/gyro_calibration.cpp
@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file gyro_calibration.c
+ * @file gyro_calibration.cpp
  * Gyroscope calibration routine
  */
 
@@ -82,7 +82,9 @@ void do_gyro_calibration(int mavlink_fd)
 	while (calibration_counter < calibration_count) {
 
 		/* wait blocking for new data */
-		struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } };
+		struct pollfd fds[1];
+		fds[0].fd = sub_sensor_combined;
+		fds[0].events = POLLIN;
 
 		int poll_ret = poll(fds, 1, 1000);
 
@@ -105,6 +107,49 @@ void do_gyro_calibration(int mavlink_fd)
 	gyro_offset[2] = gyro_offset[2] / calibration_count;
 
 
+	if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) {
+
+		if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0]))
+			|| param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1]))
+			|| param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) {
+			mavlink_log_critical(mavlink_fd, "Setting gyro offsets failed!");
+		}
+
+		/* set offsets to actual value */
+		fd = open(GYRO_DEVICE_PATH, 0);
+		struct gyro_scale gscale = {
+			gyro_offset[0],
+			1.0f,
+			gyro_offset[1],
+			1.0f,
+			gyro_offset[2],
+			1.0f,
+		};
+
+		if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale))
+			warn("WARNING: failed to set scale / offsets for gyro");
+
+		close(fd);
+
+		/* auto-save to EEPROM */
+		int save_ret = param_save_default();
+
+		if (save_ret != 0) {
+			warnx("WARNING: auto-save of params to storage failed");
+			mavlink_log_critical(mavlink_fd, "gyro store failed");
+			// XXX negative tune
+			return;
+		}
+
+		mavlink_log_info(mavlink_fd, "gyro calibration done");
+
+		tune_positive();
+		/* third beep by cal end routine */
+
+	} else {
+		mavlink_log_info(mavlink_fd, "offset cal FAILED (NaN)");
+		return;
+	}
 
 
 	/*** --- SCALING --- ***/
@@ -136,7 +181,9 @@ void do_gyro_calibration(int mavlink_fd)
 			break;
 
 		/* wait blocking for new data */
-		struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } };
+		struct pollfd fds[1];
+		fds[0].fd = sub_sensor_combined;
+		fds[0].events = POLLIN;
 
 		int poll_ret = poll(fds, 1, 1000);
 
@@ -180,27 +227,28 @@ void do_gyro_calibration(int mavlink_fd)
 	}
 
 	float gyro_scale = baseline_integral / gyro_integral;
+	float gyro_scales[] = { gyro_scale, gyro_scale, gyro_scale };
 	warnx("gyro scale: yaw (z): %6.4f", gyro_scale);
 	mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", gyro_scale);
 
 
-	if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) {
+	if (isfinite(gyro_scales[0]) && isfinite(gyro_scales[1]) && isfinite(gyro_scales[2])) {
 
-		if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0]))
-			|| param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1]))
-			|| param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) {
-			mavlink_log_critical(mavlink_fd, "Setting gyro offsets failed!");
+		if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scales[0]))
+			|| param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scales[1]))
+			|| param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scales[2]))) {
+			mavlink_log_critical(mavlink_fd, "Setting gyro scale failed!");
 		}
 
 		/* set offsets to actual value */
 		fd = open(GYRO_DEVICE_PATH, 0);
 		struct gyro_scale gscale = {
 			gyro_offset[0],
-			1.0f,
+			gyro_scales[0],
 			gyro_offset[1],
-			1.0f,
+			gyro_scales[1],
 			gyro_offset[2],
-			1.0f,
+			gyro_scales[2],
 		};
 
 		if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale))
diff --git a/src/modules/commander/mag_calibration.c b/src/modules/commander/mag_calibration.cpp
index dbd31a7e7..9a25103f8 100644
--- a/src/modules/commander/mag_calibration.c
+++ b/src/modules/commander/mag_calibration.cpp
@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file mag_calibration.c
+ * @file mag_calibration.cpp
  * Magnetometer calibration routine
  */
 
@@ -120,7 +120,9 @@ void do_mag_calibration(int mavlink_fd)
 	       calibration_counter < calibration_maxcount) {
 
 		/* wait blocking for new data */
-		struct pollfd fds[1] = { { .fd = sub_mag, .events = POLLIN } };
+		struct pollfd fds[1];
+		fds[0].fd = sub_mag;
+		fds[0].events = POLLIN;
 
 		/* user guidance */
 		if (hrt_absolute_time() >= axis_deadline &&
diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk
index fef8e366b..91d75121e 100644
--- a/src/modules/commander/module.mk
+++ b/src/modules/commander/module.mk
@@ -36,14 +36,15 @@
 #
 
 MODULE_COMMAND	 	= commander
-SRCS		 	= commander.c \
-			state_machine_helper.c \
-			commander_helper.c \
-			calibration_routines.c \
-			accelerometer_calibration.c \
-			gyro_calibration.c \
-			mag_calibration.c \
-			baro_calibration.c \
-			rc_calibration.c \
-			airspeed_calibration.c
+SRCS		 	= commander.cpp \
+			commander_params.c \
+			state_machine_helper.cpp \
+			commander_helper.cpp \
+			calibration_routines.cpp \
+			accelerometer_calibration.cpp \
+			gyro_calibration.cpp \
+			mag_calibration.cpp \
+			baro_calibration.cpp \
+			rc_calibration.cpp \
+			airspeed_calibration.cpp
 
diff --git a/src/modules/commander/rc_calibration.c b/src/modules/commander/rc_calibration.cpp
index a21d3f558..0de411713 100644
--- a/src/modules/commander/rc_calibration.c
+++ b/src/modules/commander/rc_calibration.cpp
@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file rc_calibration.c
+ * @file rc_calibration.cpp
  * Remote Control calibration routine
  */
 
diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.cpp
index 792ead8f3..3cf60a99a 100644
--- a/src/modules/commander/state_machine_helper.c
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -34,7 +34,7 @@
  ****************************************************************************/
 
 /**
- * @file state_machine_helper.c
+ * @file state_machine_helper.cpp
  * State machine helper functions implementations
  */
 
@@ -56,6 +56,11 @@
 #include "state_machine_helper.h"
 #include "commander_helper.h"
 
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
 
 int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int armed_pub, struct actuator_armed_s *armed, const int mavlink_fd) {
 
diff --git a/src/modules/systemlib/systemlib.h b/src/modules/systemlib/systemlib.h
index 0194b5e52..356215b0e 100644
--- a/src/modules/systemlib/systemlib.h
+++ b/src/modules/systemlib/systemlib.h
@@ -42,11 +42,12 @@
 #include <float.h>
 #include <stdint.h>
 
-/** Reboots the board */
-extern void up_systemreset(void) noreturn_function;
-
 __BEGIN_DECLS
 
+/** Reboots the board */
+//extern void up_systemreset(void) noreturn_function;
+#include <../arch/common/up_internal.h>
+
 /** Sends SIGUSR1 to all processes */
 __EXPORT void killall(void);