Calibration improvement

ROMFS/scripts/rcS

@@ -46,6 +46,8 @@ if [ -f /fs/microsd/etc/rc ]
 then
 	echo "[init] reading /fs/microsd/etc/rc"
 	sh /fs/microsd/etc/rc
+else
+	echo "[init] script /fs/microsd/etc/rc not present"
 fi
 
 #

apps/commander/commander.c

@@ -360,6 +360,10 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
 	float *y = malloc(sizeof(float) * calibration_maxcount);
 	float *z = malloc(sizeof(float) * calibration_maxcount);
 
+	tune_confirm();
+	sleep(2);
+	tune_confirm();
+
 	while (hrt_absolute_time() < calibration_deadline &&
 		calibration_counter < calibration_maxcount) {
 
@@ -504,6 +508,13 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
 		mavlink_log_info(mavlink_fd, buf);
 
 		mavlink_log_info(mavlink_fd, "[commander] mag calibration done");
+
+		tune_confirm();
+		sleep(2);
+		tune_confirm();
+		sleep(2);
+		/* third beep by cal end routine */
+
 	} else {
 		mavlink_log_info(mavlink_fd, "[commander] mag calibration FAILED (NaN)");
 	}
@@ -607,6 +618,12 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status)
 		// sprintf(buf, "cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]);
 		// mavlink_log_info(mavlink_fd, buf);
 		mavlink_log_info(mavlink_fd, "[commander] gyro calibration done");
+
+		tune_confirm();
+		sleep(2);
+		tune_confirm();
+		sleep(2);
+		/* third beep by cal end routine */
 	} else {
 		mavlink_log_info(mavlink_fd, "[commander] gyro calibration FAILED (NaN)");
 	}
@@ -721,6 +738,12 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status)
 		//sprintf(buf, "[commander] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]);
 		//mavlink_log_info(mavlink_fd, buf);
 		mavlink_log_info(mavlink_fd, "[commander] accel calibration done");
+
+		tune_confirm();
+		sleep(2);
+		tune_confirm();
+		sleep(2);
+		/* third beep by cal end routine */
 	} else {
 		mavlink_log_info(mavlink_fd, "[commander] accel calibration FAILED (NaN)");
 	}
@@ -740,7 +763,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 	uint8_t result = MAV_RESULT_UNSUPPORTED;
 
 	/* announce command handling */
-	ioctl(buzzer, TONE_SET_ALARM, 1);
+	tune_confirm();
 
 
 	/* supported command handling start */

apps/drivers/hmc5883/hmc5883.cpp

@@ -66,6 +66,8 @@
 #include <drivers/drv_mag.h>
 #include <drivers/drv_hrt.h>
 
+#include <float.h>
+
 /*
  * HMC5883 internal constants and data structures.
  */
@@ -159,6 +161,10 @@ private:
 	perf_counter_t		_comms_errors;
 	perf_counter_t		_buffer_overflows;
 
+	/* status reporting */
+	bool			_sensor_ok;		/**< sensor was found and reports ok */
+	bool			_calibrated;		/**< the calibration is valid */
+
 	/**
 	 * Test whether the device supported by the driver is present at a
 	 * specific address.
@@ -272,6 +278,13 @@ private:
 	 */
 	float			meas_to_float(uint8_t in[2]);
 
+	/**
+	 * Check the current calibration and update device status
+	 *
+	 * @return 0 if calibration is ok, 1 else
+	 */
+	 int 			check_calibration();
+
 };
 
 /* helper macro for handling report buffer indices */
@@ -295,7 +308,9 @@ HMC5883::HMC5883(int bus) :
 	_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"))
+	_buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")),
+	_sensor_ok(false),
+	_calibrated(false)
 {
 	// enable debug() calls
 	_debug_enabled = true;
@@ -351,6 +366,8 @@ HMC5883::init()
 	set_range(_range_ga);
 
 	ret = OK;
+	/* sensor is ok, but not calibrated */
+	_sensor_ok = true;
 out:
 	return ret;
 }
@@ -1000,6 +1017,36 @@ out:
 	return ret;
 }
 
+int HMC5883::check_calibration()
+{
+	bool scale_valid, offset_valid;
+
+	if ((-2.0f * FLT_EPSILON + 1.0f < _scale.x_scale && _scale.x_scale < 2.0f * FLT_EPSILON + 1.0f) &&
+		(-2.0f * FLT_EPSILON + 1.0f < _scale.y_scale && _scale.y_scale < 2.0f * FLT_EPSILON + 1.0f) &&
+		(-2.0f * FLT_EPSILON + 1.0f < _scale.z_scale && _scale.z_scale < 2.0f * FLT_EPSILON + 1.0f)) {
+		/* scale is different from one */
+		scale_valid = true;
+	} else {
+		scale_valid = false;
+	}
+
+	if ((-2.0f * FLT_EPSILON < _scale.x_offset && _scale.x_offset < 2.0f * FLT_EPSILON) &&
+		(-2.0f * FLT_EPSILON < _scale.y_offset && _scale.y_offset < 2.0f * FLT_EPSILON) &&
+		(-2.0f * FLT_EPSILON < _scale.z_offset && _scale.z_offset < 2.0f * FLT_EPSILON)) {
+		/* offset is different from zero */
+		offset_valid = true;
+	} else {
+		offset_valid = false;
+	}
+
+	if (_calibrated && !(offset_valid && scale_valid)) {
+		warnx("warning: mag %s%s", (scale_valid) ? "" : "scale invalid. ",
+					  (offset_valid) ? "" : "offset invalid.");
+		_calibrated = false;
+		// XXX Notify system via uORB
+	}
+}
+
 int HMC5883::set_excitement(unsigned enable)
 {
 	int ret;

apps/sensors/sensor_params.c

@@ -130,6 +130,9 @@ PARAM_DEFINE_INT32(RC_MAP_PITCH, 2);
 PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3);
 PARAM_DEFINE_INT32(RC_MAP_YAW, 4);
 PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5);
+PARAM_DEFINE_INT32(RC_MAP_AUX1, 6);
+PARAM_DEFINE_INT32(RC_MAP_AUX2, 7);
+PARAM_DEFINE_INT32(RC_MAP_AUX3, 8);
 
 PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.4f);
 PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.4f);

apps/sensors/sensors.cpp

@@ -1030,6 +1030,10 @@ Sensors::task_main()
 		manual_control.pitch = 0.0f;
 		manual_control.yaw = 0.0f;
 		manual_control.throttle = 0.0f;
+		manual_control.aux1_cam_pan_flaps = 0.0f;
+		manual_control.aux2_cam_tilt = 0.0f;
+		manual_control.aux3_cam_zoom = 0.0f;
+		manual_control.aux4_cam_roll = 0.0f;
 
 		_manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control);
 	}