Merge remote-tracking branch 'upstream/master' into attitudeekf

14 files changed, 400 insertions, 44 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 4f976546e..6c2c03070 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -59,6 +59,7 @@
 #include <string.h>
 #include <math.h>
 #include <poll.h>
+#include <float.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/sensor_combined.h>
@@ -92,6 +93,7 @@
 #include <systemlib/err.h>
 #include <systemlib/cpuload.h>
 #include <systemlib/rc_check.h>
+#include <geo/geo.h>
 #include <systemlib/state_table.h>
 #include <dataman/dataman.h>
 
@@ -875,6 +877,8 @@ int commander_thread_main(int argc, char *argv[])
 	int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
 	struct vehicle_gps_position_s gps_position;
 	memset(&gps_position, 0, sizeof(gps_position));
+	gps_position.eph = FLT_MAX;
+	gps_position.epv = FLT_MAX;
 
 	/* Subscribe to sensor topic */
 	int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
@@ -1335,6 +1339,16 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position);
 		}
 
+		/* Initialize map projection if gps is valid */
+		if (!map_projection_global_initialized()
+				&& (gps_position.eph < eph_threshold)
+				&& (gps_position.epv < epv_threshold)
+				&& hrt_elapsed_time((hrt_abstime*)&gps_position.timestamp_position) < 1e6) {
+			/* set reference for global coordinates <--> local coordiantes conversion and map_projection */
+			globallocalconverter_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, (float)gps_position.alt * 1.0e-3f, hrt_absolute_time());
+		}
+
+		/* start mission result check */
 		orb_check(mission_result_sub, &updated);
 
 		if (updated) {
diff --git a/src/modules/gpio_led/module.mk b/src/modules/gpio_led/module.mk
index 3b8553489..70c75b10c 100644
--- a/src/modules/gpio_led/module.mk
+++ b/src/modules/gpio_led/module.mk
@@ -37,3 +37,5 @@
 
 MODULE_COMMAND	= gpio_led
 SRCS			= gpio_led.c
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp
index 17d1babff..00c8df18c 100644
--- a/src/modules/mavlink/mavlink_ftp.cpp
+++ b/src/modules/mavlink/mavlink_ftp.cpp
@@ -39,6 +39,9 @@
 
 #include "mavlink_ftp.h"
 
+// Uncomment the line below to get better debug output. Never commit with this left on.
+//#define MAVLINK_FTP_DEBUG
+
 MavlinkFTP *MavlinkFTP::_server;
 
 MavlinkFTP *
@@ -125,7 +128,9 @@ MavlinkFTP::_worker(Request *req)
 		warnx("ftp: bad crc");
 	}
 
-	//printf("ftp: channel %u opc %u size %u offset %u\n", req->channel(), hdr->opcode, hdr->size, hdr->offset);
+#ifdef MAVLINK_FTP_DEBUG
+	printf("ftp: channel %u opc %u size %u offset %u\n", req->channel(), hdr->opcode, hdr->size, hdr->offset);
+#endif
 
 	switch (hdr->opcode) {
 	case kCmdNone:
@@ -172,7 +177,9 @@ out:
 	// handle success vs. error
 	if (errorCode == kErrNone) {
 		hdr->opcode = kRspAck;
-		//warnx("FTP: ack\n");
+#ifdef MAVLINK_FTP_DEBUG
+		warnx("FTP: ack\n");
+#endif
 	} else {
 		warnx("FTP: nak %u", errorCode);
 		hdr->opcode = kRspNak;
@@ -217,7 +224,9 @@ MavlinkFTP::_workList(Request *req)
 		return kErrNotDir;
 	}
     
-	//warnx("FTP: list %s offset %d", dirPath, hdr->offset);
+#ifdef MAVLINK_FTP_DEBUG
+	warnx("FTP: list %s offset %d", dirPath, hdr->offset);
+#endif
 
 	ErrorCode errorCode = kErrNone;
 	struct dirent entry, *result = nullptr;
@@ -247,11 +256,13 @@ MavlinkFTP::_workList(Request *req)
 
 		uint32_t fileSize = 0;
 		char buf[256];
+		char direntType;
 
-		// store the type marker
+		// Determine the directory entry type
 		switch (entry.d_type) {
 		case DTYPE_FILE:
-			hdr->data[offset++] = kDirentFile;
+			// For files we get the file size as well
+			direntType = kDirentFile;
 			snprintf(buf, sizeof(buf), "%s/%s", dirPath, entry.d_name);
 			struct stat st;
 			if (stat(buf, &st) == 0) {
@@ -259,29 +270,34 @@ MavlinkFTP::_workList(Request *req)
 			}
 			break;
 		case DTYPE_DIRECTORY:
-			hdr->data[offset++] = kDirentDir;
+			direntType = kDirentDir;
 			break;
 		default:
-			hdr->data[offset++] = kDirentUnknown;
+			direntType = kDirentUnknown;
 			break;
 		}
 		
 		if (entry.d_type == DTYPE_FILE) {
+			// Files send filename and file length
 			snprintf(buf, sizeof(buf), "%s\t%d", entry.d_name, fileSize);
 		} else {
+			// Everything else just sends name
 			strncpy(buf, entry.d_name, sizeof(buf));
 			buf[sizeof(buf)-1] = 0;
 		}
 		size_t nameLen = strlen(buf);
 
-		// name too big to fit?
-		if ((nameLen + offset + 2) > kMaxDataLength) {
+		// Do we have room for the name, the one char directory identifier and the null terminator?
+		if ((offset + nameLen + 2) > kMaxDataLength) {
 			break;
 		}
 		
-		// copy the name, which we know will fit
+		// Move the data into the buffer
+		hdr->data[offset++] = direntType;
 		strcpy((char *)&hdr->data[offset], buf);
-		//printf("FTP: list %s %s\n", dirPath, (char *)&hdr->data[offset-1]);
+#ifdef MAVLINK_FTP_DEBUG
+		printf("FTP: list %s %s\n", dirPath, (char *)&hdr->data[offset-1]);
+#endif
 		offset += nameLen + 1;
 	}
 
@@ -342,7 +358,9 @@ MavlinkFTP::_workRead(Request *req)
 	}
 
 	// Seek to the specified position
-	//warnx("seek %d", hdr->offset);
+#ifdef MAVLINK_FTP_DEBUG
+	warnx("seek %d", hdr->offset);
+#endif
 	if (lseek(_session_fds[session_index], hdr->offset, SEEK_SET) < 0) {
 		// Unable to see to the specified location
 		warnx("seek fail");
diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h
index 800c98b69..43de89de9 100644
--- a/src/modules/mavlink/mavlink_ftp.h
+++ b/src/modules/mavlink/mavlink_ftp.h
@@ -154,15 +154,17 @@ private:
 
 			if (!success) {
 				warnx("FTP TX ERR");
-			} 
-			// else {
-				// warnx("wrote: sys: %d, comp: %d, chan: %d, len: %d, checksum: %d",
-				// 	_mavlink->get_system_id(),
-				// 	_mavlink->get_component_id(),
-				// 	_mavlink->get_channel(),
-				// 	len,
-				// 	msg.checksum);
-			// }
+			}
+#ifdef MAVLINK_FTP_DEBUG
+			else {
+				warnx("wrote: sys: %d, comp: %d, chan: %d, len: %d, checksum: %d",
+				      _mavlink->get_system_id(),
+				      _mavlink->get_component_id(),
+				      _mavlink->get_channel(),
+				      len,
+				      msg.checksum);
+			}
+#endif
 		}
 
 		uint8_t		*rawData() { return &_message.data[0]; }
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 9a39d3bed..408605939 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -1388,8 +1388,8 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("GLOBAL_POSITION_INT", 3.0f);
 		configure_stream("LOCAL_POSITION_NED", 3.0f);
 		configure_stream("RC_CHANNELS_RAW", 1.0f);
-		configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f);
-		configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f);
+		configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f);
+		configure_stream("ATTITUDE_TARGET", 3.0f);
 		configure_stream("DISTANCE_SENSOR", 0.5f);
 		break;
 
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index c10be77b5..af4d46a36 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -1333,7 +1333,7 @@ protected:
 				}
 
 				for (unsigned i = 0; i < 8; i++) {
-					if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
+					if (act.output[i] > PWM_LOWEST_MIN / 2) {
 						if (i < n) {
 							/* scale PWM out 900..2100 us to 0..1 for rotors */
 							out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
@@ -1344,7 +1344,7 @@ protected:
 						}
 
 					} else {
-						/* send 0 when disarmed */
+						/* send 0 when disarmed and for disabled channels */
 						out[i] = 0.0f;
 					}
 				}
@@ -1353,15 +1353,20 @@ protected:
 				/* fixed wing: scale throttle to 0..1 and other channels to -1..1 */
 
 				for (unsigned i = 0; i < 8; i++) {
-					if (i != 3) {
-						/* scale PWM out 900..2100 us to -1..1 for normal channels */
-						out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2);
+					if (act.output[i] > PWM_LOWEST_MIN / 2) {
+						if (i != 3) {
+							/* scale PWM out 900..2100 us to -1..1 for normal channels */
+							out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2);
+
+						} else {
+							/* scale PWM out 900..2100 us to 0..1 for throttle */
+							out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
+						}
 
 					} else {
-						/* scale PWM out 900..2100 us to 0..1 for throttle */
-						out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
+						/* set 0 for disabled channels */
+						out[i] = 0.0f;
 					}
-
 				}
 			}
 
@@ -1637,10 +1642,10 @@ protected:
 				msg.chan2_raw = (rc.channel_count > (i * port_width) + 1) ? rc.values[(i * port_width) + 1] : UINT16_MAX;
 				msg.chan3_raw = (rc.channel_count > (i * port_width) + 2) ? rc.values[(i * port_width) + 2] : UINT16_MAX;
 				msg.chan4_raw = (rc.channel_count > (i * port_width) + 3) ? rc.values[(i * port_width) + 3] : UINT16_MAX;
-				msg.chan4_raw = (rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX;
-				msg.chan4_raw = (rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX;
-				msg.chan4_raw = (rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX;
-				msg.chan4_raw = (rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX;
+				msg.chan5_raw = (rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX;
+				msg.chan6_raw = (rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX;
+				msg.chan7_raw = (rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX;
+				msg.chan8_raw = (rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX;
 				msg.rssi = rc.rssi;
 
 				_mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS_RAW, &msg);
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 63bac45c0..c0fae0a2f 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -108,6 +108,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_att_sp_pub(-1),
 	_rates_sp_pub(-1),
 	_vicon_position_pub(-1),
+	_vision_position_pub(-1),
 	_telemetry_status_pub(-1),
 	_rc_pub(-1),
 	_manual_pub(-1),
@@ -148,6 +149,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_vicon_position_estimate(msg);
 		break;
 
+	case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE:
+		handle_message_vision_position_estimate(msg);
+		break;
+
 	case MAVLINK_MSG_ID_RADIO_STATUS:
 		handle_message_radio_status(msg);
 		break;
@@ -359,6 +364,45 @@ MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg)
 }
 
 void
+MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
+{
+	mavlink_vision_position_estimate_t pos;
+	mavlink_msg_vision_position_estimate_decode(msg, &pos);
+
+	struct vision_position_estimate vision_position;
+	memset(&vision_position, 0, sizeof(vision_position));
+
+	// Use the component ID to identify the vision sensor
+	vision_position.id = msg->compid;
+
+	vision_position.timestamp_boot = hrt_absolute_time();
+	vision_position.timestamp_computer = pos.usec;
+	vision_position.x = pos.x;
+	vision_position.y = pos.y;
+	vision_position.z = pos.z;
+
+	// XXX fix this
+	vision_position.vx = 0.0f;
+	vision_position.vy = 0.0f;
+	vision_position.vz = 0.0f;
+	
+	math::Quaternion q;
+	q.from_euler(pos.roll, pos.pitch, pos.yaw);
+
+	vision_position.q[0] = q(0);
+	vision_position.q[1] = q(1);
+	vision_position.q[2] = q(2);
+	vision_position.q[3] = q(3);
+
+	if (_vision_position_pub < 0) {
+		_vision_position_pub = orb_advertise(ORB_ID(vision_position_estimate), &vision_position);
+
+	} else {
+		orb_publish(ORB_ID(vision_position_estimate), _vision_position_pub, &vision_position);
+	}
+}
+
+void
 MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
 {
 	/* telemetry status supported only on first TELEMETRY_STATUS_ORB_ID_NUM mavlink channels */
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index f4d0c52d8..014193100 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -58,6 +58,7 @@
 #include <uORB/topics/vehicle_global_velocity_setpoint.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_vicon_position.h>
+#include <uORB/topics/vision_position_estimate.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/optical_flow.h>
@@ -112,6 +113,7 @@ private:
 	void handle_message_optical_flow(mavlink_message_t *msg);
 	void handle_message_set_mode(mavlink_message_t *msg);
 	void handle_message_vicon_position_estimate(mavlink_message_t *msg);
+	void handle_message_vision_position_estimate(mavlink_message_t *msg);
 	void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg);
 	void handle_message_radio_status(mavlink_message_t *msg);
 	void handle_message_manual_control(mavlink_message_t *msg);
@@ -145,6 +147,7 @@ private:
 	orb_advert_t _att_sp_pub;
 	orb_advert_t _rates_sp_pub;
 	orb_advert_t _vicon_position_pub;
+	orb_advert_t _vision_position_pub;
 	orb_advert_t _telemetry_status_pub;
 	orb_advert_t _rc_pub;
 	orb_advert_t _manual_pub;
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
index 05eae047c..81bbaa658 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -59,6 +59,7 @@
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/vision_position_estimate.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/optical_flow.h>
 #include <mavlink/mavlink_log.h>
@@ -80,6 +81,7 @@ static bool thread_running = false; /**< Deamon status flag */
 static int position_estimator_inav_task; /**< Handle of deamon task / thread */
 static bool verbose_mode = false;
 
+static const hrt_abstime vision_topic_timeout = 500000;	// Vision topic timeout = 0.5s
 static const hrt_abstime gps_topic_timeout = 500000;		// GPS topic timeout = 0.5s
 static const hrt_abstime flow_topic_timeout = 1000000;	// optical flow topic timeout = 1s
 static const hrt_abstime sonar_timeout = 150000;	// sonar timeout = 150ms
@@ -233,6 +235,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 	float eph_flow = 1.0f;
 
+	float eph_vision = 0.5f;
+	float epv_vision = 0.5f;
+
 	float x_est_prev[2], y_est_prev[2], z_est_prev[2];
 	memset(x_est_prev, 0, sizeof(x_est_prev));
 	memset(y_est_prev, 0, sizeof(y_est_prev));
@@ -279,6 +284,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	};
 	float w_gps_xy = 1.0f;
 	float w_gps_z = 1.0f;
+	
+	float corr_vision[3][2] = {
+		{ 0.0f, 0.0f },		// N (pos, vel)
+		{ 0.0f, 0.0f },		// E (pos, vel)
+		{ 0.0f, 0.0f },		// D (pos, vel)
+	};
+	
 	float corr_sonar = 0.0f;
 	float corr_sonar_filtered = 0.0f;
 
@@ -294,6 +306,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	bool sonar_valid = false;		// sonar is valid
 	bool flow_valid = false;		// flow is valid
 	bool flow_accurate = false;		// flow should be accurate (this flag not updated if flow_valid == false)
+	bool vision_valid = false;
 
 	/* declare and safely initialize all structs */
 	struct actuator_controls_s actuator;
@@ -312,6 +325,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	memset(&local_pos, 0, sizeof(local_pos));
 	struct optical_flow_s flow;
 	memset(&flow, 0, sizeof(flow));
+	struct vision_position_estimate vision;
+	memset(&vision, 0, sizeof(vision));
 	struct vehicle_global_position_s global_pos;
 	memset(&global_pos, 0, sizeof(global_pos));
 
@@ -323,6 +338,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
 	int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
 	int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
+	int vision_position_estimate_sub = orb_subscribe(ORB_ID(vision_position_estimate));
 	int home_position_sub = orb_subscribe(ORB_ID(home_position));
 
 	/* advertise */
@@ -616,6 +632,46 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				}
 			}
 
+
+			/* check no vision circuit breaker is set */
+			if (params.no_vision != CBRK_NO_VISION_KEY) {
+				/* vehicle vision position */
+				orb_check(vision_position_estimate_sub, &updated);
+
+				if (updated) {
+					orb_copy(ORB_ID(vision_position_estimate), vision_position_estimate_sub, &vision);
+
+					/* reset position estimate on first vision update */
+					if (!vision_valid) {
+						x_est[0] = vision.x;
+						x_est[1] = vision.vx;
+						y_est[0] = vision.y;
+						y_est[1] = vision.vy;
+						/* only reset the z estimate if the z weight parameter is not zero */ 
+						if (params.w_z_vision_p > MIN_VALID_W)
+						{
+							z_est[0] = vision.z;
+							z_est[1] = vision.vz;
+						}
+						
+						vision_valid = true;
+						warnx("VISION estimate valid");
+						mavlink_log_info(mavlink_fd, "[inav] VISION estimate valid");
+					}
+
+					/* calculate correction for position */
+					corr_vision[0][0] = vision.x - x_est[0];
+					corr_vision[1][0] = vision.y - y_est[0];
+					corr_vision[2][0] = vision.z - z_est[0];
+
+					/* calculate correction for velocity */
+					corr_vision[0][1] = vision.vx - x_est[1];
+					corr_vision[1][1] = vision.vy - y_est[1];
+					corr_vision[2][1] = vision.vz - z_est[1];
+
+				}
+			}
+
 			/* vehicle GPS position */
 			orb_check(vehicle_gps_position_sub, &updated);
 
@@ -732,14 +788,21 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		}
 
 		/* check for timeout on GPS topic */
-		if (gps_valid && t > gps.timestamp_position + gps_topic_timeout) {
+		if (gps_valid && (t > (gps.timestamp_position + gps_topic_timeout))) {
 			gps_valid = false;
 			warnx("GPS timeout");
 			mavlink_log_info(mavlink_fd, "[inav] GPS timeout");
 		}
 
+		/* check for timeout on vision topic */
+		if (vision_valid && (t > (vision.timestamp_boot + vision_topic_timeout))) {
+			vision_valid = false;
+			warnx("VISION timeout");
+			mavlink_log_info(mavlink_fd, "[inav] VISION timeout");
+		}
+
 		/* check for sonar measurement timeout */
-		if (sonar_valid && t > sonar_time + sonar_timeout) {
+		if (sonar_valid && (t > (sonar_time + sonar_timeout))) {
 			corr_sonar = 0.0f;
 			sonar_valid = false;
 		}
@@ -759,10 +822,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		/* use GPS if it's valid and reference position initialized */
 		bool use_gps_xy = ref_inited && gps_valid && params.w_xy_gps_p > MIN_VALID_W;
 		bool use_gps_z = ref_inited && gps_valid && params.w_z_gps_p > MIN_VALID_W;
+		/* use VISION if it's valid and has a valid weight parameter */
+		bool use_vision_xy = vision_valid && params.w_xy_vision_p > MIN_VALID_W;
+		bool use_vision_z = vision_valid && params.w_z_vision_p > MIN_VALID_W;
 		/* use flow if it's valid and (accurate or no GPS available) */
 		bool use_flow = flow_valid && (flow_accurate || !use_gps_xy);
 
-		bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow;
+		bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || use_vision_xy;
 
 		bool dist_bottom_valid = (t < sonar_valid_time + sonar_valid_timeout);
 
@@ -781,6 +847,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		float w_xy_gps_v = params.w_xy_gps_v * w_gps_xy;
 		float w_z_gps_p = params.w_z_gps_p * w_gps_z;
 
+		float w_xy_vision_p = params.w_xy_vision_p;
+		float w_xy_vision_v = params.w_xy_vision_v;
+		float w_z_vision_p = params.w_z_vision_p;
+
 		/* reduce GPS weight if optical flow is good */
 		if (use_flow && flow_accurate) {
 			w_xy_gps_p *= params.w_gps_flow;
@@ -821,6 +891,35 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			}
 		}
 
+		/* accelerometer bias correction for VISION (use buffered rotation matrix) */
+		accel_bias_corr[0] = 0.0f;
+		accel_bias_corr[1] = 0.0f;
+		accel_bias_corr[2] = 0.0f;
+
+		if (use_vision_xy) {
+			accel_bias_corr[0] -= corr_vision[0][0] * w_xy_vision_p * w_xy_vision_p;
+			accel_bias_corr[0] -= corr_vision[0][1] * w_xy_vision_v;
+			accel_bias_corr[1] -= corr_vision[1][0] * w_xy_vision_p * w_xy_vision_p;
+			accel_bias_corr[1] -= corr_vision[1][1] * w_xy_vision_v;
+		}
+
+		if (use_vision_z) {
+			accel_bias_corr[2] -= corr_vision[2][0] * w_z_vision_p * w_z_vision_p;
+		}
+
+		/* transform error vector from NED frame to body frame */
+		for (int i = 0; i < 3; i++) {
+			float c = 0.0f;
+
+			for (int j = 0; j < 3; j++) {
+				c += att.R[j][i] * accel_bias_corr[j];
+			}
+
+			if (isfinite(c)) {
+				acc_bias[i] += c * params.w_acc_bias * dt;
+			}
+		}
+
 		/* accelerometer bias correction for flow and baro (assume that there is no delay) */
 		accel_bias_corr[0] = 0.0f;
 		accel_bias_corr[1] = 0.0f;
@@ -863,10 +962,16 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p);
 		}
 
+		if (use_vision_z) {
+			epv = fminf(epv, epv_vision);
+			inertial_filter_correct(corr_vision[2][0], dt, z_est, 0, w_z_vision_p);
+		}
+
 		if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) {
 			write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
 			memcpy(z_est, z_est_prev, sizeof(z_est));
 			memset(corr_gps, 0, sizeof(corr_gps));
+			memset(corr_vision, 0, sizeof(corr_vision));
 			corr_baro = 0;
 
 		} else {
@@ -904,11 +1009,24 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				}
 			}
 
+			if (use_vision_xy) {
+				eph = fminf(eph, eph_vision);
+
+				inertial_filter_correct(corr_vision[0][0], dt, x_est, 0, w_xy_vision_p);
+				inertial_filter_correct(corr_vision[1][0], dt, y_est, 0, w_xy_vision_p);
+
+				if (w_xy_vision_v > MIN_VALID_W) {
+					inertial_filter_correct(corr_vision[0][1], dt, x_est, 1, w_xy_vision_v);
+					inertial_filter_correct(corr_vision[1][1], dt, y_est, 1, w_xy_vision_v);
+				}
+			}
+
 			if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(y_est[0]) && isfinite(y_est[1]))) {
 				write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
 				memcpy(x_est, x_est_prev, sizeof(x_est));
 				memcpy(y_est, y_est_prev, sizeof(y_est));
 				memset(corr_gps, 0, sizeof(corr_gps));
+				memset(corr_vision, 0, sizeof(corr_vision));
 				memset(corr_flow, 0, sizeof(corr_flow));
 
 			} else {
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c
index 98b31d17b..cc0fb4155 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_params.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 Anton Babushkin. All rights reserved.
- *   Author: 	Anton Babushkin	<rk3dov@gmail.com>
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -35,6 +34,8 @@
 /*
  * @file position_estimator_inav_params.c
  *
+ * @author Anton Babushkin <rk3dov@gmail.com>
+ *
  * Parameters for position_estimator_inav
  */
 
@@ -63,6 +64,17 @@ PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 0.5f);
 PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f);
 
 /**
+ * Z axis weight for vision
+ *
+ * Weight (cutoff frequency) for vision altitude measurements. vision altitude data is very noisy and should be used only as slow correction for baro offset.
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group Position Estimator INAV
+ */
+PARAM_DEFINE_FLOAT(INAV_W_Z_VIS_P, 0.5f);
+
+/**
  * Z axis weight for sonar
  *
  * Weight (cutoff frequency) for sonar measurements.
@@ -96,6 +108,28 @@ PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f);
 PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f);
 
 /**
+ * XY axis weight for vision position
+ *
+ * Weight (cutoff frequency) for vision position measurements.
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group Position Estimator INAV
+ */
+PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_P, 5.0f);
+
+/**
+ * XY axis weight for vision velocity
+ *
+ * Weight (cutoff frequency) for vision velocity measurements.
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group Position Estimator INAV
+ */
+PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_V, 0.0f);
+
+/**
  * XY axis weight for optical flow
  *
  * Weight (cutoff frequency) for optical flow (velocity) measurements.
@@ -232,13 +266,27 @@ PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.2f);
  */
 PARAM_DEFINE_FLOAT(INAV_DELAY_GPS, 0.2f);
 
+/**
+ * Disable vision input
+ *
+ * Set to the appropriate key (328754) to disable vision input.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group Position Estimator INAV
+ */
+PARAM_DEFINE_INT32(CBRK_NO_VISION, 0);
+
 int parameters_init(struct position_estimator_inav_param_handles *h)
 {
 	h->w_z_baro = param_find("INAV_W_Z_BARO");
 	h->w_z_gps_p = param_find("INAV_W_Z_GPS_P");
+	h->w_z_vision_p = param_find("INAV_W_Z_VIS_P");
 	h->w_z_sonar = param_find("INAV_W_Z_SONAR");
 	h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P");
 	h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V");
+	h->w_xy_vision_p = param_find("INAV_W_XY_VIS_P");
+	h->w_xy_vision_v = param_find("INAV_W_XY_VIS_V");
 	h->w_xy_flow = param_find("INAV_W_XY_FLOW");
 	h->w_xy_res_v = param_find("INAV_W_XY_RES_V");
 	h->w_gps_flow = param_find("INAV_W_GPS_FLOW");
@@ -250,6 +298,7 @@ int parameters_init(struct position_estimator_inav_param_handles *h)
 	h->land_t = param_find("INAV_LAND_T");
 	h->land_disp = param_find("INAV_LAND_DISP");
 	h->land_thr = param_find("INAV_LAND_THR");
+	h->no_vision = param_find("CBRK_NO_VISION");
 	h->delay_gps = param_find("INAV_DELAY_GPS");
 
 	return OK;
@@ -259,9 +308,12 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str
 {
 	param_get(h->w_z_baro, &(p->w_z_baro));
 	param_get(h->w_z_gps_p, &(p->w_z_gps_p));
+	param_get(h->w_z_vision_p, &(p->w_z_vision_p));
 	param_get(h->w_z_sonar, &(p->w_z_sonar));
 	param_get(h->w_xy_gps_p, &(p->w_xy_gps_p));
 	param_get(h->w_xy_gps_v, &(p->w_xy_gps_v));
+	param_get(h->w_xy_vision_p, &(p->w_xy_vision_p));
+	param_get(h->w_xy_vision_v, &(p->w_xy_vision_v));
 	param_get(h->w_xy_flow, &(p->w_xy_flow));
 	param_get(h->w_xy_res_v, &(p->w_xy_res_v));
 	param_get(h->w_gps_flow, &(p->w_gps_flow));
@@ -273,6 +325,7 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str
 	param_get(h->land_t, &(p->land_t));
 	param_get(h->land_disp, &(p->land_disp));
 	param_get(h->land_thr, &(p->land_thr));
+	param_get(h->no_vision, &(p->no_vision));
 	param_get(h->delay_gps, &(p->delay_gps));
 
 	return OK;
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h
index 423f0d879..d0a65e42e 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_params.h
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 Anton Babushkin. All rights reserved.
- *   Author: 	Anton Babushkin	<rk3dov@gmail.com>
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -33,9 +32,11 @@
  ****************************************************************************/
 
 /*
- * @file position_estimator_inav_params.h
+ * @file position_estimator_inav_params.c
  *
- * Parameters for Position Estimator
+ * @author Anton Babushkin <rk3dov@gmail.com>
+ *
+ * Parameters definition for position_estimator_inav
  */
 
 #include <systemlib/param/param.h>
@@ -43,9 +44,12 @@
 struct position_estimator_inav_params {
 	float w_z_baro;
 	float w_z_gps_p;
+	float w_z_vision_p;
 	float w_z_sonar;
 	float w_xy_gps_p;
 	float w_xy_gps_v;
+	float w_xy_vision_p;
+	float w_xy_vision_v;
 	float w_xy_flow;
 	float w_xy_res_v;
 	float w_gps_flow;
@@ -57,15 +61,19 @@ struct position_estimator_inav_params {
 	float land_t;
 	float land_disp;
 	float land_thr;
+	int32_t no_vision;
 	float delay_gps;
 };
 
 struct position_estimator_inav_param_handles {
 	param_t w_z_baro;
 	param_t w_z_gps_p;
+	param_t w_z_vision_p;
 	param_t w_z_sonar;
 	param_t w_xy_gps_p;
 	param_t w_xy_gps_v;
+	param_t w_xy_vision_p;
+	param_t w_xy_vision_v;
 	param_t w_xy_flow;
 	param_t w_xy_res_v;
 	param_t w_gps_flow;
@@ -77,9 +85,12 @@ struct position_estimator_inav_param_handles {
 	param_t land_t;
 	param_t land_disp;
 	param_t land_thr;
+	param_t no_vision;
 	param_t delay_gps;
 };
 
+#define CBRK_NO_VISION_KEY	328754
+
 /**
  * Initialize all parameter handles and values
  *
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index 08c3ce1ac..f7d5f8737 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -213,6 +213,9 @@ ORB_DEFINE(encoders, struct encoders_s);
 #include "topics/estimator_status.h"
 ORB_DEFINE(estimator_status, struct estimator_status_report);
 
+#include "topics/vision_position_estimate.h"
+ORB_DEFINE(vision_position_estimate, struct vision_position_estimate);
+
 #include "topics/vehicle_force_setpoint.h"
 ORB_DEFINE(vehicle_force_setpoint, struct vehicle_force_setpoint_s);
 
diff --git a/src/modules/uORB/topics/vision_position_estimate.h b/src/modules/uORB/topics/vision_position_estimate.h
new file mode 100644
index 000000000..74c96cf2e
--- /dev/null
+++ b/src/modules/uORB/topics/vision_position_estimate.h
@@ -0,0 +1,82 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * 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 vision_position_estimate.h
+ * Vision based position estimate
+ */
+
+#ifndef TOPIC_VISION_POSITION_ESTIMATE_H_
+#define TOPIC_VISION_POSITION_ESTIMATE_H_
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+/**
+ * Vision based position estimate in NED frame
+ */
+struct vision_position_estimate {
+
+	unsigned id;				/**< ID of the estimator, commonly the component ID of the incoming message */
+
+	uint64_t timestamp_boot;		/**< time of this estimate, in microseconds since system start */
+	uint64_t timestamp_computer;		/**< timestamp provided by the companion computer, in us */
+
+	float x;				/**< X position in meters in NED earth-fixed frame */
+	float y;				/**< Y position in meters in NED earth-fixed frame */
+	float z;				/**< Z position in meters in NED earth-fixed frame (negative altitude) */
+
+	float vx;				/**< X velocity in meters per second in NED earth-fixed frame */
+	float vy;				/**< Y velocity in meters per second in NED earth-fixed frame */
+	float vz;				/**< Z velocity in meters per second in NED earth-fixed frame */
+
+	float q[4];				/**< Estimated attitude as quaternion */
+
+	// XXX Add covariances here
+
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(vision_position_estimate);
+
+#endif /* TOPIC_VISION_POSITION_ESTIMATE_H_ */
diff --git a/src/modules/unit_test/module.mk b/src/modules/unit_test/module.mk
index f00b0f592..5000790a5 100644
--- a/src/modules/unit_test/module.mk
+++ b/src/modules/unit_test/module.mk
@@ -37,3 +37,4 @@
 
 SRCS		 = unit_test.cpp
 
+MAXOPTIMIZATION	 = -Os