Merge branch 'FW_control' of https://github.com/dougweibel/Firmware into fw_control

9 files changed, 276 insertions, 14 deletions

diff --git a/ROMFS/scripts/rcS b/ROMFS/scripts/rcS
index 409bde33b..b5fbfe0f5 100755
--- a/ROMFS/scripts/rcS
+++ b/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
 
 #
diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index a7550b54b..d93a48e31 100644
--- a/apps/commander/commander.c
+++ b/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 */
diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp
index f9079c3ff..6fefbfafc 100644
--- a/apps/drivers/hmc5883/hmc5883.cpp
+++ b/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;
diff --git a/apps/fixedwing_pos_control/fixedwing_pos_control.h b/apps/fixedwing_pos_control/fixedwing_pos_control.h
new file mode 100644
index 000000000..f631c90a1
--- /dev/null
+++ b/apps/fixedwing_pos_control/fixedwing_pos_control.h
@@ -0,0 +1,73 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Doug Weibel <douglas.weibel@colorado.edu>
+ *           @author Lorenz Meier <lm@inf.ethz.ch>
+ 			 @author Thomas Gubler <thomasgubler@student.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 fixedwing_pos_control.h
+ * Position control for fixed wing airframes.
+ */
+
+#ifndef FIXEDWING_POS_CONTROL_H_
+#define FIXEDWING_POS_CONTROL_H_
+
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/actuator_controls.h>
+
+#endif 
+
+
+struct planned_path_segments_s {
+	bool segment_type;
+	double start_lat;			// Start of line or center of arc
+	double start_lon;
+	double end_lat;
+	double end_lon;
+	float radius;				// Radius of arc
+	float arc_start_bearing;		// Bearing from center to start of arc
+	float arc_sweep;				// Angle (radians) swept out by arc around center.  
+								// Positive for clockwise, negative for counter-clockwise
+};
+
+
+float _wrap180(float bearing);
+float _wrap360(float bearing);
+float _wrapPI(float bearing);
+float _wrap2PI(float bearing);
+
+/* FIXEDWING_CONTROL_H_ */
+
diff --git a/apps/fixedwing_pos_control/fixedwing_pos_control_main.c b/apps/fixedwing_pos_control/fixedwing_pos_control_main.c
index eb4945464..9eb34ae44 100644
--- a/apps/fixedwing_pos_control/fixedwing_pos_control_main.c
+++ b/apps/fixedwing_pos_control/fixedwing_pos_control_main.c
@@ -67,6 +67,7 @@
  *
  */
 PARAM_DEFINE_FLOAT(FW_HEADING_P, 0.1f);
+PARAM_DEFINE_FLOAT(FW_XTRACK_P, 0.01745f);		// Radians per meter off track
 PARAM_DEFINE_FLOAT(FW_ALT_P, 0.1f);
 PARAM_DEFINE_FLOAT(FW_ROLL_LIM, 0.7f);	// Roll angle limit in radians
 PARAM_DEFINE_FLOAT(FW_PITCH_LIM, 0.35f);	// Pitch angle limit in radians
@@ -74,6 +75,7 @@ PARAM_DEFINE_FLOAT(FW_PITCH_LIM, 0.35f);	// Pitch angle limit in radians
 
 struct fw_pos_control_params {
 	float heading_p;
+	float xtrack_p;
 	float altitude_p;
 	float roll_lim;
 	float pitch_lim;
@@ -81,6 +83,7 @@ struct fw_pos_control_params {
 
 struct fw_pos_control_param_handles {
 	param_t heading_p;
+	param_t xtrack_p;
 	param_t altitude_p;
 	param_t roll_lim;
 	param_t pitch_lim;
@@ -121,6 +124,7 @@ static int parameters_init(struct fw_pos_control_param_handles *h)
 {
 	/* PID parameters */
 	h->heading_p 	=	param_find("FW_HEADING_P");
+	h->xtrack_p 	=	param_find("FW_XTRACK_P");
 	h->altitude_p 	=	param_find("FW_ALT_P");
 	h->roll_lim 	=	param_find("FW_ROLL_LIM");
 	h->pitch_lim 	=	param_find("FW_PITCH_LIM");
@@ -132,6 +136,7 @@ static int parameters_init(struct fw_pos_control_param_handles *h)
 static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_pos_control_params *p)
 {
 	param_get(h->heading_p, &(p->heading_p));
+	param_get(h->xtrack_p, &(p->xtrack_p));
 	param_get(h->altitude_p, &(p->altitude_p));
 	param_get(h->roll_lim, &(p->roll_lim));
 	param_get(h->pitch_lim, &(p->pitch_lim));
@@ -158,11 +163,15 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
 		/* declare and safely initialize all structs */
 		struct vehicle_global_position_s global_pos;
 		memset(&global_pos, 0, sizeof(global_pos));
+		struct vehicle_global_position_s start_pos;		// Temporary variable, replace with
+		memset(&start_pos, 0, sizeof(start_pos));		// previous waypoint when available
 		struct vehicle_global_position_setpoint_s global_setpoint;
 		memset(&global_setpoint, 0, sizeof(global_setpoint));
 		struct vehicle_attitude_s att;
 		memset(&att, 0, sizeof(att));
-
+		crosstrack_error_s xtrack_err;
+		//memset(&xtrack_err, 0, sizeof(xtrack_err));
+		
 		/* output structs */
 		struct vehicle_attitude_setpoint_s attitude_setpoint;
 		memset(&attitude_setpoint, 0, sizeof(attitude_setpoint));
@@ -181,6 +190,8 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
 		/* Setup of loop */
 		struct pollfd fds = { .fd = att_sub, .events = POLLIN };
 		bool global_sp_updated_set_once = false;
+		bool start_point_set = false;		// This is a temporary flag till the
+											// previous waypoint is available for computations
 
 		while(!thread_should_exit)
 		{
@@ -221,7 +232,10 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
 			orb_check(global_setpoint_sub, &global_sp_updated);
 			if(global_sp_updated)
 				global_sp_updated_set_once = true;
-
+			if(global_sp_updated_set_once && !start_point_set) {
+				start_pos = global_pos;
+				start_point_set = true;
+			}
 
 			/* Load local copies */
 			orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
@@ -239,17 +253,18 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
 				/* calculate bearing error */
 				float target_bearing = get_bearing_to_next_waypoint(global_pos.lat / (double)1e7d, global_pos.lon / (double)1e7d,
 					global_setpoint.lat / (double)1e7d, global_setpoint.lon / (double)1e7d);
+				
+				/* calculate crosstrack error */
+				// Only the case of a straight line track following handled so far
+				xtrack_err = get_distance_to_line(global_pos.lat / (double)1e7d, global_pos.lon / (double)1e7d,
+									start_pos.lat / (double)1e7d, start_pos.lon / (double)1e7d,
+									global_setpoint.lat / (double)1e7d, global_setpoint.lon / (double)1e7d);
 
 				/* shift error to prevent wrapping issues */
 				float bearing_error = target_bearing - att.yaw;
-
-				if (bearing_error < M_PI_F) {
-					bearing_error += 2.0f * M_PI_F;
-				}
-
-				if (bearing_error > M_PI_F) {
-					bearing_error -= 2.0f * M_PI_F;
-				}
+				if(!(xtrack_err.error || xtrack_err.past_end))
+						bearing_error -= p.xtrack_p * xtrack_err.distance;
+				bearing_error = _wrapPI(bearing_error);
 
 				/* calculate roll setpoint, do this artificially around zero */
 				attitude_setpoint.roll_tait_bryan = pid_calculate(&heading_controller, bearing_error, 0.0f, 0.0f, 0.0f);
@@ -344,4 +359,62 @@ int fixedwing_pos_control_main(int argc, char *argv[])
 }
 
 
+float _wrapPI(float bearing)
+{
+
+	while (bearing > M_PI_F) {
+		bearing = bearing - M_TWOPI_F;
+	}
+
+	while (bearing <=  -M_PI_F) {
+		bearing = bearing + M_TWOPI_F;
+	}
+
+	return bearing;
+}
+
+float _wrap2PI(float bearing)
+{
+
+	while (bearing >= M_TWOPI_F) {
+		bearing = bearing - M_TWOPI_F;
+	}
+
+	while (bearing <  0.0f) {
+		bearing = bearing + M_TWOPI_F;
+	}
+
+	return bearing;
+}
+
+float _wrap180(float bearing)
+{
+
+	while (bearing > 180.0f) {
+		bearing = bearing - 360.0f;
+	}
+
+	while (bearing <=  -180.0f) {
+		bearing = bearing + 360.0f;
+	}
+
+	return bearing;
+}
+
+float _wrap360(float bearing)
+{
+
+	while (bearing >= 360.0f) {
+		bearing = bearing - 360.0f;
+	}
+
+	while (bearing <  0.0f) {
+		bearing = bearing + 360.0f;
+	}
+
+	return bearing;
+}
+
+
+
 
diff --git a/apps/sensors/sensor_params.c b/apps/sensors/sensor_params.c
index 5e8c5746c..892ec975a 100644
--- a/apps/sensors/sensor_params.c
+++ b/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);
diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp
index 54d2f6a0b..9522bd31a 100644
--- a/apps/sensors/sensors.cpp
+++ b/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);
 	}
diff --git a/apps/systemcmds/eeprom/24xxxx_mtd.c b/apps/systemcmds/eeprom/24xxxx_mtd.c
index 79149caa0..781b01065 100644
--- a/apps/systemcmds/eeprom/24xxxx_mtd.c
+++ b/apps/systemcmds/eeprom/24xxxx_mtd.c
@@ -502,6 +502,33 @@ FAR struct mtd_dev_s *at24c_initialize(FAR struct i2c_dev_s *dev) {
 		priv->perf_write_errors = perf_alloc(PC_COUNT, "EEPROM write errors");
 	}
 
+	/* attempt to read to validate device is present */
+	unsigned char buf[5];
+	uint8_t addrbuf[2] = {0, 0};
+
+	struct i2c_msg_s msgv[2] = {
+		{
+			.addr = priv->addr,
+			.flags = 0,
+			.buffer = &addrbuf[0],
+			.length = sizeof(addrbuf),
+		},
+		{
+			.addr = priv->addr,
+			.flags = I2C_M_READ,
+			.buffer = &buf[0],
+			.length = sizeof(buf),
+		}
+	};
+
+	perf_begin(priv->perf_reads);
+	int ret = I2C_TRANSFER(priv->dev, &msgv[0], 2);
+	perf_end(priv->perf_reads);
+
+	if (ret < 0) {
+		return NULL;
+	}
+
 	/* Return the implementation-specific state structure as the MTD device */
 
 	fvdbg("Return %p\n", priv);
diff --git a/apps/systemcmds/eeprom/eeprom.c b/apps/systemcmds/eeprom/eeprom.c
index 19a14aa02..b4257cda9 100644
--- a/apps/systemcmds/eeprom/eeprom.c
+++ b/apps/systemcmds/eeprom/eeprom.c
@@ -118,9 +118,19 @@ eeprom_attach(void)
 	if (i2c == NULL)
 		errx(1, "failed to locate I2C bus");
 
-	/* start the MTD driver */
-	eeprom_mtd = at24c_initialize(i2c);
+	/* start the MTD driver, attempt 5 times */
+	for (int i = 0; i < 5; i++) {
+		eeprom_mtd = at24c_initialize(i2c);
+		if (eeprom_mtd) {
+			/* abort on first valid result */
+			if (i > 0) {
+				warnx("warning: EEPROM needed %d attempts to attach", i+1);
+			}
+			break;
+		}
+	}
 
+	/* if last attempt is still unsuccessful, abort */
 	if (eeprom_mtd == NULL)
 		errx(1, "failed to initialize EEPROM driver");