From ead91f325989cda312424ec3b2cc7fd11913f5c8 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Thu, 29 May 2014 22:42:33 +0200
Subject: [PATCH 01/13] position_estimator_inav: GPS delay compensation

---
 .../position_estimator_inav_main.c            | 50 ++++++++++++++++---
 .../position_estimator_inav_params.c          |  3 ++
 .../position_estimator_inav_params.h          |  2 +
 3 files changed, 47 insertions(+), 8 deletions(-)

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 d7503e42da..d6a44304d2 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -71,6 +71,8 @@
 #include "inertial_filter.h"
 
 #define MIN_VALID_W 0.00001f
+#define PUB_INTERVAL 10000	// limit publish rate to 100 Hz
+#define EST_BUF_SIZE 500000 / PUB_INTERVAL		// buffer size is 0.5s
 
 static bool thread_should_exit = false; /**< Deamon exit flag */
 static bool thread_running = false; /**< Deamon status flag */
@@ -83,7 +85,6 @@ static const hrt_abstime sonar_timeout = 150000;	// sonar timeout = 150ms
 static const hrt_abstime sonar_valid_timeout = 1000000;	// estimate sonar distance during this time after sonar loss
 static const hrt_abstime xy_src_timeout = 2000000;	// estimate position during this time after position sources loss
 static const uint32_t updates_counter_len = 1000000;
-static const uint32_t pub_interval = 10000;	// limit publish rate to 100 Hz
 static const float max_flow = 1.0f;	// max flow value that can be used, rad/s
 
 __EXPORT int position_estimator_inav_main(int argc, char *argv[]);
@@ -92,6 +93,16 @@ int position_estimator_inav_thread_main(int argc, char *argv[]);
 
 static void usage(const char *reason);
 
+static inline int min(int val1, int val2)
+{
+	return (val1 < val2) ? val1 : val2;
+}
+
+static inline int max(int val1, int val2)
+{
+	return (val1 > val2) ? val1 : val2;
+}
+
 /**
  * Print the correct usage.
  */
@@ -199,6 +210,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	float y_est[2] = { 0.0f, 0.0f };	// pos, vel
 	float z_est[2] = { 0.0f, 0.0f };	// pos, vel
 
+	float est_buf[EST_BUF_SIZE][3][2];
+	memset(est_buf, 0, sizeof(est_buf));
+	int est_buf_ptr = 0;
+
 	float eph = 1.0;
 	float epv = 1.0;
 
@@ -657,16 +672,22 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 							y_est[1] = gps.vel_e_m_s;
 						}
 
+						/* calculate index of estimated values in buffer */
+						int est_i = est_buf_ptr - 1 - min(EST_BUF_SIZE - 1, max(0, (int)(params.delay_gps * 1000000.0f / PUB_INTERVAL)));
+						if (est_i < 0) {
+							est_i += EST_BUF_SIZE;
+						}
+
 						/* calculate correction for position */
-						corr_gps[0][0] = gps_proj[0] - x_est[0];
-						corr_gps[1][0] = gps_proj[1] - y_est[0];
-						corr_gps[2][0] = local_pos.ref_alt - alt - z_est[0];
+						corr_gps[0][0] = gps_proj[0] - est_buf[est_i][0][0];
+						corr_gps[1][0] = gps_proj[1] - est_buf[est_i][1][0];
+						corr_gps[2][0] = local_pos.ref_alt - alt - est_buf[est_i][2][0];
 
 						/* calculate correction for velocity */
 						if (gps.vel_ned_valid) {
-							corr_gps[0][1] = gps.vel_n_m_s - x_est[1];
-							corr_gps[1][1] = gps.vel_e_m_s - y_est[1];
-							corr_gps[2][1] = gps.vel_d_m_s - z_est[1];
+							corr_gps[0][1] = gps.vel_n_m_s - est_buf[est_i][0][1];
+							corr_gps[1][1] = gps.vel_e_m_s - est_buf[est_i][1][1];
+							corr_gps[2][1] = gps.vel_d_m_s - est_buf[est_i][2][1];
 
 						} else {
 							corr_gps[0][1] = 0.0f;
@@ -910,8 +931,21 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			}
 		}
 
-		if (t > pub_last + pub_interval) {
+		if (t > pub_last + PUB_INTERVAL) {
 			pub_last = t;
+
+			/* push current estimate to FIFO buffer */
+			est_buf[est_buf_ptr][0][0] = x_est[0];
+			est_buf[est_buf_ptr][0][1] = x_est[1];
+			est_buf[est_buf_ptr][1][0] = y_est[0];
+			est_buf[est_buf_ptr][1][1] = y_est[1];
+			est_buf[est_buf_ptr][2][0] = z_est[0];
+			est_buf[est_buf_ptr][2][1] = z_est[1];
+			est_buf_ptr++;
+			if (est_buf_ptr >= EST_BUF_SIZE) {
+				est_buf_ptr = 0;
+			}
+
 			/* publish local position */
 			local_pos.xy_valid = can_estimate_xy;
 			local_pos.v_xy_valid = can_estimate_xy;
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 8aa08b6f2c..8509d15cba 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_params.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c
@@ -55,6 +55,7 @@ PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f);
 PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f);
 PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f);
 PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.2f);
+PARAM_DEFINE_FLOAT(INAV_DELAY_GPS, 0.1f);
 
 int parameters_init(struct position_estimator_inav_param_handles *h)
 {
@@ -73,6 +74,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->delay_gps = param_find("INAV_DELAY_GPS");
 
 	return OK;
 }
@@ -94,6 +96,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->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 08ec996a13..df1cfaa710 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_params.h
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h
@@ -56,6 +56,7 @@ struct position_estimator_inav_params {
 	float land_t;
 	float land_disp;
 	float land_thr;
+	float delay_gps;
 };
 
 struct position_estimator_inav_param_handles {
@@ -74,6 +75,7 @@ struct position_estimator_inav_param_handles {
 	param_t land_t;
 	param_t land_disp;
 	param_t land_thr;
+	param_t delay_gps;
 };
 
 /**

From fdd5d7b8b8835dd3e2655cd104f7440c9f56ba27 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Fri, 30 May 2014 11:03:06 +0200
Subject: [PATCH 02/13] position_estimator_inav: add buffer for rotation matrix
 to do accel bias correction properly

---
 .../position_estimator_inav_main.c            | 61 ++++++++++++++-----
 1 file changed, 45 insertions(+), 16 deletions(-)

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 d6a44304d2..aa533c777f 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -72,7 +72,7 @@
 
 #define MIN_VALID_W 0.00001f
 #define PUB_INTERVAL 10000	// limit publish rate to 100 Hz
-#define EST_BUF_SIZE 500000 / PUB_INTERVAL		// buffer size is 0.5s
+#define EST_BUF_SIZE 250000 / PUB_INTERVAL		// buffer size is 0.5s
 
 static bool thread_should_exit = false; /**< Deamon exit flag */
 static bool thread_running = false; /**< Deamon status flag */
@@ -146,7 +146,7 @@ int position_estimator_inav_main(int argc, char *argv[])
 
 		thread_should_exit = false;
 		position_estimator_inav_task = task_spawn_cmd("position_estimator_inav",
-					       SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 4000,
+					       SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 5000,
 					       position_estimator_inav_thread_main,
 					       (argv) ? (const char **) &argv[2] : (const char **) NULL);
 		exit(0);
@@ -210,9 +210,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	float y_est[2] = { 0.0f, 0.0f };	// pos, vel
 	float z_est[2] = { 0.0f, 0.0f };	// pos, vel
 
-	float est_buf[EST_BUF_SIZE][3][2];
+	float est_buf[EST_BUF_SIZE][3][2];	// estimated position buffer
+	float R_buf[EST_BUF_SIZE][3][3];	// rotation matrix buffer
+	float R_gps[3][3];					// rotation matrix for GPS correction moment
 	memset(est_buf, 0, sizeof(est_buf));
-	int est_buf_ptr = 0;
+	memset(R_buf, 0, sizeof(R_buf));
+	memset(R_gps, 0, sizeof(R_gps));
+	int buf_ptr = 0;
 
 	float eph = 1.0;
 	float epv = 1.0;
@@ -673,7 +677,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 						}
 
 						/* calculate index of estimated values in buffer */
-						int est_i = est_buf_ptr - 1 - min(EST_BUF_SIZE - 1, max(0, (int)(params.delay_gps * 1000000.0f / PUB_INTERVAL)));
+						int est_i = buf_ptr - 1 - min(EST_BUF_SIZE - 1, max(0, (int)(params.delay_gps * 1000000.0f / PUB_INTERVAL)));
 						if (est_i < 0) {
 							est_i += EST_BUF_SIZE;
 						}
@@ -695,6 +699,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 							corr_gps[2][1] = 0.0f;
 						}
 
+						/* save rotation matrix at this moment */
+						memcpy(R_gps, R_buf[est_i], sizeof(R_gps));
+
 						eph = fminf(eph, gps.eph_m);
 						epv = fminf(epv, gps.epv_m);
 
@@ -784,7 +791,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			corr_baro += offs_corr;
 		}
 
-		/* accelerometer bias correction */
+		/* accelerometer bias correction for GPS (use buffered rotation matrix) */
 		float accel_bias_corr[3] = { 0.0f, 0.0f, 0.0f };
 
 		if (use_gps_xy) {
@@ -798,6 +805,24 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			accel_bias_corr[2] -= corr_gps[2][0] * w_z_gps_p * w_z_gps_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 += R_gps[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;
+		accel_bias_corr[2] = 0.0f;
+
 		if (use_flow) {
 			accel_bias_corr[0] -= corr_flow[0] * params.w_xy_flow;
 			accel_bias_corr[1] -= corr_flow[1] * params.w_xy_flow;
@@ -934,16 +959,20 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		if (t > pub_last + PUB_INTERVAL) {
 			pub_last = t;
 
-			/* push current estimate to FIFO buffer */
-			est_buf[est_buf_ptr][0][0] = x_est[0];
-			est_buf[est_buf_ptr][0][1] = x_est[1];
-			est_buf[est_buf_ptr][1][0] = y_est[0];
-			est_buf[est_buf_ptr][1][1] = y_est[1];
-			est_buf[est_buf_ptr][2][0] = z_est[0];
-			est_buf[est_buf_ptr][2][1] = z_est[1];
-			est_buf_ptr++;
-			if (est_buf_ptr >= EST_BUF_SIZE) {
-				est_buf_ptr = 0;
+			/* push current estimate to buffer */
+			est_buf[buf_ptr][0][0] = x_est[0];
+			est_buf[buf_ptr][0][1] = x_est[1];
+			est_buf[buf_ptr][1][0] = y_est[0];
+			est_buf[buf_ptr][1][1] = y_est[1];
+			est_buf[buf_ptr][2][0] = z_est[0];
+			est_buf[buf_ptr][2][1] = z_est[1];
+
+			/* push current rotation matrix to buffer */
+			memcpy(R_buf[buf_ptr], att.R, sizeof(att.R));
+
+			buf_ptr++;
+			if (buf_ptr >= EST_BUF_SIZE) {
+				buf_ptr = 0;
 			}
 
 			/* publish local position */

From 0bdde089243bbf7e3651f9535f5c847f894e0ac7 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Fri, 30 May 2014 21:18:03 +0200
Subject: [PATCH 03/13] position_estimator_inav: default GPS delay changed to
 0.2s

---
 .../position_estimator_inav/position_estimator_inav_params.c    | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

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 8509d15cba..d88419c258 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_params.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c
@@ -55,7 +55,7 @@ PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f);
 PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f);
 PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f);
 PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.2f);
-PARAM_DEFINE_FLOAT(INAV_DELAY_GPS, 0.1f);
+PARAM_DEFINE_FLOAT(INAV_DELAY_GPS, 0.2f);
 
 int parameters_init(struct position_estimator_inav_param_handles *h)
 {

From 47c9d326209034dc373ab54647d29df4e63b5285 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Fri, 30 May 2014 21:23:26 +0200
Subject: [PATCH 04/13] ubx: disable all debug messages

---
 src/drivers/gps/ubx.cpp | 170 ++++++++++++++++++++--------------------
 1 file changed, 85 insertions(+), 85 deletions(-)

diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp
index 7502809bd7..cbfb61d00a 100644
--- a/src/drivers/gps/ubx.cpp
+++ b/src/drivers/gps/ubx.cpp
@@ -219,19 +219,19 @@ UBX::configure(unsigned &baudrate)
 		return 1;
 	}
 
-	configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5);
-
-	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
-		warnx("MSG CFG FAIL: NAV SVINFO");
-		return 1;
-	}
-
-	configure_message_rate(UBX_CLASS_MON, UBX_MESSAGE_MON_HW, 1);
-
-	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
-		warnx("MSG CFG FAIL: MON HW");
-		return 1;
-	}
+//	configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5);
+//
+//	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
+//		warnx("MSG CFG FAIL: NAV SVINFO");
+//		return 1;
+//	}
+//
+//	configure_message_rate(UBX_CLASS_MON, UBX_MESSAGE_MON_HW, 1);
+//
+//	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
+//		warnx("MSG CFG FAIL: MON HW");
+//		return 1;
+//	}
 
 	_configured = true;
 	return 0;
@@ -486,61 +486,61 @@ UBX::handle_message()
 					break;
 				}
 
-			case UBX_MESSAGE_NAV_SVINFO: {
-					//printf("GOT NAV_SVINFO\n");
-					const int length_part1 = 8;
-					gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer;
-					const int length_part2 = 12;
-					gps_bin_nav_svinfo_part2_packet_t *packet_part2;
-
-					uint8_t satellites_used = 0;
-					int i;
-
-					//printf("Number of Channels: %d\n", packet_part1->numCh);
-					for (i = 0; i < packet_part1->numCh; i++) {
-						/* set pointer to sattelite_i information */
-						packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) & (_rx_buffer[length_part1 + i * length_part2]);
-
-						/* write satellite information to global storage */
-						uint8_t sv_used = packet_part2->flags & 0x01;
-
-						if (sv_used) {
-							/* count SVs used for NAV */
-							satellites_used++;
-						}
-
-						/* record info for all channels, whether or not the SV is used for NAV */
-						_gps_position->satellite_used[i] = sv_used;
-						_gps_position->satellite_snr[i] = packet_part2->cno;
-						_gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev);
-						_gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f);
-						_gps_position->satellite_prn[i] = packet_part2->svid;
-						//printf("SAT %d: %d %d %d %d\n", i, (int)sv_used, (int)packet_part2->cno, (int)(uint8_t)(packet_part2->elev), (int)packet_part2->svid);
-					}
-
-					for (i = packet_part1->numCh; i < 20; i++) {
-						/* unused channels have to be set to zero for e.g. MAVLink */
-						_gps_position->satellite_prn[i] = 0;
-						_gps_position->satellite_used[i] = 0;
-						_gps_position->satellite_snr[i] = 0;
-						_gps_position->satellite_elevation[i] = 0;
-						_gps_position->satellite_azimuth[i] = 0;
-					}
-
-					_gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones
-
-					if (packet_part1->numCh > 0) {
-						_gps_position->satellite_info_available = true;
-
-					} else {
-						_gps_position->satellite_info_available = false;
-					}
-
-					_gps_position->timestamp_satellites = hrt_absolute_time();
-
-					ret = 1;
-					break;
-				}
+//			case UBX_MESSAGE_NAV_SVINFO: {
+//					//printf("GOT NAV_SVINFO\n");
+//					const int length_part1 = 8;
+//					gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer;
+//					const int length_part2 = 12;
+//					gps_bin_nav_svinfo_part2_packet_t *packet_part2;
+//
+//					uint8_t satellites_used = 0;
+//					int i;
+//
+//					//printf("Number of Channels: %d\n", packet_part1->numCh);
+//					for (i = 0; i < packet_part1->numCh; i++) {
+//						/* set pointer to sattelite_i information */
+//						packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) & (_rx_buffer[length_part1 + i * length_part2]);
+//
+//						/* write satellite information to global storage */
+//						uint8_t sv_used = packet_part2->flags & 0x01;
+//
+//						if (sv_used) {
+//							/* count SVs used for NAV */
+//							satellites_used++;
+//						}
+//
+//						/* record info for all channels, whether or not the SV is used for NAV */
+//						_gps_position->satellite_used[i] = sv_used;
+//						_gps_position->satellite_snr[i] = packet_part2->cno;
+//						_gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev);
+//						_gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f);
+//						_gps_position->satellite_prn[i] = packet_part2->svid;
+//						//printf("SAT %d: %d %d %d %d\n", i, (int)sv_used, (int)packet_part2->cno, (int)(uint8_t)(packet_part2->elev), (int)packet_part2->svid);
+//					}
+//
+//					for (i = packet_part1->numCh; i < 20; i++) {
+//						/* unused channels have to be set to zero for e.g. MAVLink */
+//						_gps_position->satellite_prn[i] = 0;
+//						_gps_position->satellite_used[i] = 0;
+//						_gps_position->satellite_snr[i] = 0;
+//						_gps_position->satellite_elevation[i] = 0;
+//						_gps_position->satellite_azimuth[i] = 0;
+//					}
+//
+//					_gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones
+//
+//					if (packet_part1->numCh > 0) {
+//						_gps_position->satellite_info_available = true;
+//
+//					} else {
+//						_gps_position->satellite_info_available = false;
+//					}
+//
+//					_gps_position->timestamp_satellites = hrt_absolute_time();
+//
+//					ret = 1;
+//					break;
+//				}
 
 			case UBX_MESSAGE_NAV_VELNED: {
 					// printf("GOT NAV_VELNED\n");
@@ -573,23 +573,23 @@ UBX::handle_message()
 				break;
 			}
 
-		case UBX_CLASS_MON: {
-			switch (_message_id) {
-			case UBX_MESSAGE_MON_HW: {
-
-					struct gps_bin_mon_hw_packet *p = (struct gps_bin_mon_hw_packet*) _rx_buffer;
-
-					_gps_position->noise_per_ms = p->noisePerMS;
-					_gps_position->jamming_indicator = p->jamInd;
-
-					ret = 1;
-					break;
-				}
-
-			default:
-				break;
-			}
-		}
+//		case UBX_CLASS_MON: {
+//			switch (_message_id) {
+//			case UBX_MESSAGE_MON_HW: {
+//
+//					struct gps_bin_mon_hw_packet *p = (struct gps_bin_mon_hw_packet*) _rx_buffer;
+//
+//					_gps_position->noise_per_ms = p->noisePerMS;
+//					_gps_position->jamming_indicator = p->jamInd;
+//
+//					ret = 1;
+//					break;
+//				}
+//
+//			default:
+//				break;
+//			}
+//		}
 
 		default:
 			break;

From efb44969d584e1b2613d96a795fc94a3ef728d9a Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Fri, 30 May 2014 21:57:48 +0200
Subject: [PATCH 05/13] ubx: send update only if got POSLLH & VELNED & TIMEUTC

---
 src/drivers/gps/ubx.cpp | 184 +++++++++++++++++++++-------------------
 src/drivers/gps/ubx.h   |   3 +
 2 files changed, 100 insertions(+), 87 deletions(-)

diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp
index cbfb61d00a..c143eeb0c2 100644
--- a/src/drivers/gps/ubx.cpp
+++ b/src/drivers/gps/ubx.cpp
@@ -69,6 +69,9 @@ UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) :
 	_gps_position(gps_position),
 	_configured(false),
 	_waiting_for_ack(false),
+	_got_posllh(false),
+	_got_velned(false),
+	_got_timeutc(false),
 	_disable_cmd_last(0)
 {
 	decode_init();
@@ -219,19 +222,19 @@ UBX::configure(unsigned &baudrate)
 		return 1;
 	}
 
-//	configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5);
-//
-//	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
-//		warnx("MSG CFG FAIL: NAV SVINFO");
-//		return 1;
-//	}
-//
-//	configure_message_rate(UBX_CLASS_MON, UBX_MESSAGE_MON_HW, 1);
-//
-//	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
-//		warnx("MSG CFG FAIL: MON HW");
-//		return 1;
-//	}
+	configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5);
+
+	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
+		warnx("MSG CFG FAIL: NAV SVINFO");
+		return 1;
+	}
+
+	configure_message_rate(UBX_CLASS_MON, UBX_MESSAGE_MON_HW, 1);
+
+	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
+		warnx("MSG CFG FAIL: MON HW");
+		return 1;
+	}
 
 	_configured = true;
 	return 0;
@@ -275,9 +278,10 @@ UBX::receive(unsigned timeout)
 	bool handled = false;
 
 	while (true) {
+		bool ready_to_return = _configured ? (_got_posllh && _got_velned && _got_timeutc) : handled;
 
 		/* poll for new data, wait for only UBX_PACKET_TIMEOUT (2ms) if something already received */
-		int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), handled ? UBX_PACKET_TIMEOUT : timeout);
+		int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), ready_to_return ? UBX_PACKET_TIMEOUT : timeout);
 
 		if (ret < 0) {
 			/* something went wrong when polling */
@@ -286,7 +290,10 @@ UBX::receive(unsigned timeout)
 
 		} else if (ret == 0) {
 			/* return success after short delay after receiving a packet or timeout after long delay */
-			if (handled) {
+			if (ready_to_return) {
+				_got_posllh = false;
+				_got_velned = false;
+				_got_timeutc = false;
 				return 1;
 
 			} else {
@@ -438,6 +445,7 @@ UBX::handle_message()
 
 					_rate_count_lat_lon++;
 
+					_got_posllh = true;
 					ret = 1;
 					break;
 				}
@@ -482,65 +490,66 @@ UBX::handle_message()
 					_gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f);
 					_gps_position->timestamp_time = hrt_absolute_time();
 
+					_got_timeutc = true;
 					ret = 1;
 					break;
 				}
 
-//			case UBX_MESSAGE_NAV_SVINFO: {
-//					//printf("GOT NAV_SVINFO\n");
-//					const int length_part1 = 8;
-//					gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer;
-//					const int length_part2 = 12;
-//					gps_bin_nav_svinfo_part2_packet_t *packet_part2;
-//
-//					uint8_t satellites_used = 0;
-//					int i;
-//
-//					//printf("Number of Channels: %d\n", packet_part1->numCh);
-//					for (i = 0; i < packet_part1->numCh; i++) {
-//						/* set pointer to sattelite_i information */
-//						packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) & (_rx_buffer[length_part1 + i * length_part2]);
-//
-//						/* write satellite information to global storage */
-//						uint8_t sv_used = packet_part2->flags & 0x01;
-//
-//						if (sv_used) {
-//							/* count SVs used for NAV */
-//							satellites_used++;
-//						}
-//
-//						/* record info for all channels, whether or not the SV is used for NAV */
-//						_gps_position->satellite_used[i] = sv_used;
-//						_gps_position->satellite_snr[i] = packet_part2->cno;
-//						_gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev);
-//						_gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f);
-//						_gps_position->satellite_prn[i] = packet_part2->svid;
-//						//printf("SAT %d: %d %d %d %d\n", i, (int)sv_used, (int)packet_part2->cno, (int)(uint8_t)(packet_part2->elev), (int)packet_part2->svid);
-//					}
-//
-//					for (i = packet_part1->numCh; i < 20; i++) {
-//						/* unused channels have to be set to zero for e.g. MAVLink */
-//						_gps_position->satellite_prn[i] = 0;
-//						_gps_position->satellite_used[i] = 0;
-//						_gps_position->satellite_snr[i] = 0;
-//						_gps_position->satellite_elevation[i] = 0;
-//						_gps_position->satellite_azimuth[i] = 0;
-//					}
-//
-//					_gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones
-//
-//					if (packet_part1->numCh > 0) {
-//						_gps_position->satellite_info_available = true;
-//
-//					} else {
-//						_gps_position->satellite_info_available = false;
-//					}
-//
-//					_gps_position->timestamp_satellites = hrt_absolute_time();
-//
-//					ret = 1;
-//					break;
-//				}
+			case UBX_MESSAGE_NAV_SVINFO: {
+					//printf("GOT NAV_SVINFO\n");
+					const int length_part1 = 8;
+					gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer;
+					const int length_part2 = 12;
+					gps_bin_nav_svinfo_part2_packet_t *packet_part2;
+
+					uint8_t satellites_used = 0;
+					int i;
+
+					//printf("Number of Channels: %d\n", packet_part1->numCh);
+					for (i = 0; i < packet_part1->numCh; i++) {
+						/* set pointer to sattelite_i information */
+						packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) & (_rx_buffer[length_part1 + i * length_part2]);
+
+						/* write satellite information to global storage */
+						uint8_t sv_used = packet_part2->flags & 0x01;
+
+						if (sv_used) {
+							/* count SVs used for NAV */
+							satellites_used++;
+						}
+
+						/* record info for all channels, whether or not the SV is used for NAV */
+						_gps_position->satellite_used[i] = sv_used;
+						_gps_position->satellite_snr[i] = packet_part2->cno;
+						_gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev);
+						_gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f);
+						_gps_position->satellite_prn[i] = packet_part2->svid;
+						//printf("SAT %d: %d %d %d %d\n", i, (int)sv_used, (int)packet_part2->cno, (int)(uint8_t)(packet_part2->elev), (int)packet_part2->svid);
+					}
+
+					for (i = packet_part1->numCh; i < 20; i++) {
+						/* unused channels have to be set to zero for e.g. MAVLink */
+						_gps_position->satellite_prn[i] = 0;
+						_gps_position->satellite_used[i] = 0;
+						_gps_position->satellite_snr[i] = 0;
+						_gps_position->satellite_elevation[i] = 0;
+						_gps_position->satellite_azimuth[i] = 0;
+					}
+
+					_gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones
+
+					if (packet_part1->numCh > 0) {
+						_gps_position->satellite_info_available = true;
+
+					} else {
+						_gps_position->satellite_info_available = false;
+					}
+
+					_gps_position->timestamp_satellites = hrt_absolute_time();
+
+					ret = 1;
+					break;
+				}
 
 			case UBX_MESSAGE_NAV_VELNED: {
 					// printf("GOT NAV_VELNED\n");
@@ -557,6 +566,7 @@ UBX::handle_message()
 
 					_rate_count_vel++;
 
+					_got_velned = true;
 					ret = 1;
 					break;
 				}
@@ -573,23 +583,23 @@ UBX::handle_message()
 				break;
 			}
 
-//		case UBX_CLASS_MON: {
-//			switch (_message_id) {
-//			case UBX_MESSAGE_MON_HW: {
-//
-//					struct gps_bin_mon_hw_packet *p = (struct gps_bin_mon_hw_packet*) _rx_buffer;
-//
-//					_gps_position->noise_per_ms = p->noisePerMS;
-//					_gps_position->jamming_indicator = p->jamInd;
-//
-//					ret = 1;
-//					break;
-//				}
-//
-//			default:
-//				break;
-//			}
-//		}
+		case UBX_CLASS_MON: {
+			switch (_message_id) {
+			case UBX_MESSAGE_MON_HW: {
+
+					struct gps_bin_mon_hw_packet *p = (struct gps_bin_mon_hw_packet*) _rx_buffer;
+
+					_gps_position->noise_per_ms = p->noisePerMS;
+					_gps_position->jamming_indicator = p->jamInd;
+
+					ret = 1;
+					break;
+				}
+
+			default:
+				break;
+			}
+		}
 
 		default:
 			break;
diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h
index 5cf47b60b5..43d6888936 100644
--- a/src/drivers/gps/ubx.h
+++ b/src/drivers/gps/ubx.h
@@ -397,6 +397,9 @@ private:
 	struct vehicle_gps_position_s *_gps_position;
 	bool			_configured;
 	bool			_waiting_for_ack;
+	bool			_got_posllh;
+	bool			_got_velned;
+	bool			_got_timeutc;
 	uint8_t			_message_class_needed;
 	uint8_t			_message_id_needed;
 	ubx_decode_state_t	_decode_state;

From 6b8248ee29f38ae0f2ff10ebc23b1816e1fc6829 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Sat, 31 May 2014 16:19:38 +0200
Subject: [PATCH 06/13] position_estimator_inav: more safe EPH/EPV estimation,
 minor changes

---
 .../position_estimator_inav_main.c            | 47 ++++++++++++-------
 1 file changed, 30 insertions(+), 17 deletions(-)

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 aa533c777f..f908d7a3be 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -79,7 +79,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 gps_topic_timeout = 1000000;		// GPS topic timeout = 1s
+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
 static const hrt_abstime sonar_valid_timeout = 1000000;	// estimate sonar distance during this time after sonar loss
@@ -218,8 +218,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	memset(R_gps, 0, sizeof(R_gps));
 	int buf_ptr = 0;
 
-	float eph = 1.0;
-	float epv = 1.0;
+	static const float min_eph_epv = 2.0f;	// min EPH/EPV, used for weight calculation
+	static const float max_eph_epv = 20.0f;	// max EPH/EPV acceptable for estimation
+
+	float eph = max_eph_epv;
+	float epv = 1.0f;
+
+	float eph_flow = 1.0f;
 
 	float x_est_prev[2], y_est_prev[2], z_est_prev[2];
 	memset(x_est_prev, 0, sizeof(x_est_prev));
@@ -276,9 +281,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	float corr_flow[] = { 0.0f, 0.0f };	// N E
 	float w_flow = 0.0f;
 
-	static float min_eph_epv = 2.0f;	// min EPH/EPV, used for weight calculation
-	static float max_eph_epv = 10.0f;	// max EPH/EPV acceptable for estimation
-
 	float sonar_prev = 0.0f;
 	hrt_abstime flow_prev = 0;			// time of last flow measurement
 	hrt_abstime sonar_time = 0;			// time of last sonar measurement (not filtered)
@@ -555,9 +557,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 						w_flow *= 0.05f;
 					}
 
-					flow_valid = true;
+					/* under ideal conditions, on 1m distance assume EPH = 10cm */
+					eph_flow = 0.1 / w_flow;
 
-					eph = fminf(eph, 0.1 / att.R[2][2] / flow_q * fmaxf(1.0f, flow_dist));	// under ideal conditions, on 1m distance assume EPH = 10cm
+					flow_valid = true;
 
 				} else {
 					w_flow = 0.0f;
@@ -616,13 +619,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 				/* hysteresis for GPS quality */
 				if (gps_valid) {
-					if (gps.eph_m > max_eph_epv * 1.5f || gps.epv_m > max_eph_epv * 1.5f || gps.fix_type < 3) {
+					if (gps.eph_m > max_eph_epv || gps.epv_m > max_eph_epv || gps.fix_type < 3) {
 						gps_valid = false;
 						mavlink_log_info(mavlink_fd, "[inav] GPS signal lost");
 					}
 
 				} else {
-					if (gps.eph_m < max_eph_epv && gps.epv_m < max_eph_epv && gps.fix_type >= 3) {
+					if (gps.eph_m < max_eph_epv * 0.7f && gps.epv_m < max_eph_epv * 0.7f && gps.fix_type >= 3) {
 						gps_valid = true;
 						reset_est = true;
 						mavlink_log_info(mavlink_fd, "[inav] GPS signal found");
@@ -702,9 +705,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 						/* save rotation matrix at this moment */
 						memcpy(R_gps, R_buf[est_i], sizeof(R_gps));
 
-						eph = fminf(eph, gps.eph_m);
-						epv = fminf(epv, gps.epv_m);
-
 						w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph_m);
 						w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv_m);
 					}
@@ -745,8 +745,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		t_prev = t;
 
 		/* increase EPH/EPV on each step */
-		eph *= 1.0 + dt;
-		epv += 0.005 * dt;	// add 1m to EPV each 200s (baro drift)
+		if (eph < max_eph_epv) {
+			eph *= 1.0 + dt;
+		}
+		if (epv < max_eph_epv) {
+			epv += 0.005 * dt;	// add 1m to EPV each 200s (baro drift)
+		}
 
 		/* 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;
@@ -759,7 +763,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			xy_src_time = t;
 		}
 
-		bool can_estimate_xy = eph < max_eph_epv * 1.5;
+		bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow;
 
 		bool dist_bottom_valid = (t < sonar_valid_time + sonar_valid_timeout);
 
@@ -853,7 +857,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 		/* inertial filter correction for altitude */
 		inertial_filter_correct(corr_baro, dt, z_est, 0, params.w_z_baro);
-		inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p);
+
+		if (use_gps_z) {
+			epv = fminf(epv, gps.epv_m);
+
+			inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_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);
@@ -878,11 +887,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 			/* inertial filter correction for position */
 			if (use_flow) {
+				eph = fminf(eph, eph_flow);
+
 				inertial_filter_correct(corr_flow[0], dt, x_est, 1, params.w_xy_flow * w_flow);
 				inertial_filter_correct(corr_flow[1], dt, y_est, 1, params.w_xy_flow * w_flow);
 			}
 
 			if (use_gps_xy) {
+				eph = fminf(eph, gps.eph_m);
+
 				inertial_filter_correct(corr_gps[0][0], dt, x_est, 0, w_xy_gps_p);
 				inertial_filter_correct(corr_gps[1][0], dt, y_est, 0, w_xy_gps_p);
 

From 85b2dfa0c662298d49f584e6bd774954b04cec35 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Sun, 8 Jun 2014 12:30:27 +0200
Subject: [PATCH 07/13] fix initialization of perfcounters in fw att
 controllers

---
 src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 4 ++--
 src/lib/ecl/attitude_fw/ecl_roll_controller.cpp  | 4 ++--
 src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp   | 4 ++--
 3 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
index 0a909d02f5..a0a18bc2e4 100644
--- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
@@ -61,9 +61,9 @@ ECL_PitchController::ECL_PitchController() :
 	_integrator(0.0f),
 	_rate_error(0.0f),
 	_rate_setpoint(0.0f),
-	_bodyrate_setpoint(0.0f)
+	_bodyrate_setpoint(0.0f),
+	_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control pitch nonfinite input"))
 {
-	perf_alloc(PC_COUNT, "fw att control pitch nonfinite input");
 }
 
 ECL_PitchController::~ECL_PitchController()
diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
index 82903ef5aa..d2a231694d 100644
--- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
@@ -59,9 +59,9 @@ ECL_RollController::ECL_RollController() :
 	_integrator(0.0f),
 	_rate_error(0.0f),
 	_rate_setpoint(0.0f),
-	_bodyrate_setpoint(0.0f)
+	_bodyrate_setpoint(0.0f),
+	_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control roll nonfinite input"))
 {
-	perf_alloc(PC_COUNT, "fw att control roll nonfinite input");
 }
 
 ECL_RollController::~ECL_RollController()
diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
index e53ffc6448..79184e2cdd 100644
--- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
@@ -58,9 +58,9 @@ ECL_YawController::ECL_YawController() :
 	_rate_error(0.0f),
 	_rate_setpoint(0.0f),
 	_bodyrate_setpoint(0.0f),
-	_coordinated_min_speed(1.0f)
+	_coordinated_min_speed(1.0f),
+	_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control yaw nonfinite input"))
 {
-	perf_alloc(PC_COUNT, "fw att control yaw nonfinite input");
 }
 
 ECL_YawController::~ECL_YawController()

From 128389d3b14a43d48712b2bb5e32cac85b5dafcc Mon Sep 17 00:00:00 2001
From: Andrew Chambers <achamber@gmail.com>
Date: Mon, 9 Jun 2014 16:01:44 -0700
Subject: [PATCH 08/13] Converted style to work with wiki. Cleaned up bad
 fields.

---
 .../fw_att_control/fw_att_control_params.c    | 367 +++++++++++++-----
 1 file changed, 270 insertions(+), 97 deletions(-)

diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c
index aa637e2074..7cae846785 100644
--- a/src/modules/fw_att_control/fw_att_control_params.c
+++ b/src/modules/fw_att_control/fw_att_control_params.c
@@ -50,149 +50,322 @@ f *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
  * Controller parameters, accessible via MAVLink
  *
  */
-// @DisplayName		Attitude Time Constant
-// @Description		This defines the latency between a step input and the achieved setpoint (inverse to a P gain). Half a second is a good start value and fits for most average systems. Smaller systems may require smaller values, but as this will wear out servos faster, the value should only be decreased as needed.
-// @Range		0.4 to 1.0 seconds, in tens of seconds
+
+/**
+ * Attitude Time Constant
+ *
+ * This defines the latency between a step input and the achieved setpoint
+ * (inverse to a P gain). Half a second is a good start value and fits for
+ * most average systems. Smaller systems may require smaller values, but as
+ * this will wear out servos faster, the value should only be decreased as
+ * needed.
+ *
+ * @unit seconds
+ * @min 0.4
+ * @max 1.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_ATT_TC, 0.5f);
 
-// @DisplayName		Pitch rate proportional gain.
-// @Description		This defines how much the elevator input will be commanded depending on the current body angular rate error.
-// @Range		10 to 200, 1 increments
+/**
+ * Pitch rate proportional gain.
+ *
+ * This defines how much the elevator input will be commanded depending on the
+ * current body angular rate error.
+ *
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_PR_P, 0.05f);
 
-// @DisplayName		Pitch rate integrator gain.
-// @Description		This gain defines how much control response will result out of a steady state error. It trims any constant error.
-// @Range		0 to 50.0
+/**
+ * Pitch rate integrator gain.
+ *
+ * This gain defines how much control response will result out of a steady
+ * state error. It trims any constant error.
+ *
+ * @min 0.0
+ * @max 50.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_PR_I, 0.0f);
 
-// @DisplayName		Maximum positive / up pitch rate.
-// @Description		This limits the maximum pitch up angular rate the controller will output (in degrees per second). Setting a value of zero disables the limit.
-// @Range		0 to 90.0 degrees per seconds, in 1 increments
+/**
+ * Maximum positive / up pitch rate.
+ *
+ * This limits the maximum pitch up angular rate the controller will output (in
+ * degrees per second). Setting a value of zero disables the limit.
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @max 90.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 0.0f);
 
-// @DisplayName		Maximum negative / down pitch rate.
-// @Description		This limits the maximum pitch down up angular rate the controller will output (in degrees per second). Setting a value of zero disables the limit.
-// @Range		0 to 90.0 degrees per seconds, in 1 increments
+/**
+ * Maximum negative / down pitch rate.
+ *
+ * This limits the maximum pitch down up angular rate the controller will
+ * output (in degrees per second). Setting a value of zero disables the limit.
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @max 90.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 0.0f);
 
-// @DisplayName		Pitch rate integrator limit
-// @Description		The portion of the integrator part in the control surface deflection is limited to this value
-// @Range		0.0 to 1
-// @Increment		0.1
+/**
+ * Pitch rate integrator limit
+ *
+ * The portion of the integrator part in the control surface deflection is
+ * limited to this value
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.2f);
 
-// @DisplayName		Roll to Pitch feedforward gain.
-// @Description		This compensates during turns and ensures the nose stays level.
-// @Range		0.5 2.0
-// @Increment		0.05
-// @User		User
+/**
+ * Roll to Pitch feedforward gain.
+ *
+ * This compensates during turns and ensures the nose stays level.
+ *
+ * @min 0.0
+ * @max 2.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_P_ROLLFF, 0.0f); //xxx: set to 0 as default, see comment in ECL_PitchController::control_attitude (float turn_offset = ...)
 
-// @DisplayName		Roll rate proportional Gain.
-// @Description		This defines how much the aileron input will be commanded depending on the current body angular rate error.
-// @Range		10.0 200.0
-// @Increment		10.0
-// @User		User
+/**
+ * Roll rate proportional Gain
+ *
+ * This defines how much the aileron input will be commanded depending on the
+ * current body angular rate error.
+ *
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_RR_P, 0.05f);
 
-// @DisplayName		Roll rate integrator Gain
-// @Description		This gain defines how much control response will result out of a steady state error. It trims any constant error.
-// @Range		0.0 100.0
-// @Increment		5.0
-// @User		User
+/**
+ * Roll rate integrator Gain
+ *
+ * This gain defines how much control response will result out of a steady
+ * state error. It trims any constant error.
+ *
+ * @min 0.0
+ * @max 100.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_RR_I, 0.0f);
 
-// @DisplayName		Roll Integrator Anti-Windup
-// @Description		The portion of the integrator part in the control surface deflection is limited to this value.
-// @Range		0.0 to 1.0
-// @Increment		0.1
+/**
+ * Roll Integrator Anti-Windup
+ *
+ * The portion of the integrator part in the control surface deflection is limited to this value.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_RR_IMAX, 0.2f);
 
-// @DisplayName		Maximum Roll Rate
-// @Description		This limits the maximum roll rate the controller will output (in degrees per second). Setting a value of zero disables the limit.
-// @Range		0 to 90.0 degrees per seconds
-// @Increment		1.0
-PARAM_DEFINE_FLOAT(FW_R_RMAX, 0);
+/**
+ * Maximum Roll Rate
+ *
+ * This limits the maximum roll rate the controller will output (in degrees per
+ * second). Setting a value of zero disables the limit.
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @max 90.0
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_R_RMAX, 0.0f);
 
-// @DisplayName		Yaw rate proportional gain.
-// @Description		This defines how much the rudder input will be commanded depending on the current body angular rate error.
-// @Range		10 to 200, 1 increments
-PARAM_DEFINE_FLOAT(FW_YR_P, 0.05);
+/**
+ * Yaw rate proportional gain
+ *
+ * This defines how much the rudder input will be commanded depending on the
+ * current body angular rate error.
+ *
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_YR_P, 0.05f);
 
-// @DisplayName		Yaw rate integrator gain.
-// @Description		This gain defines how much control response will result out of a steady state error. It trims any constant error.
-// @Range		0 to 50.0
+/**
+ * Yaw rate integrator gain
+ *
+ * This gain defines how much control response will result out of a steady
+ * state error. It trims any constant error.
+ *
+ * @min 0.0
+ * @max 50.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_YR_I, 0.0f);
 
-// @DisplayName		Yaw rate integrator limit
-// @Description		The portion of the integrator part in the control surface deflection is limited to this value
-// @Range		0.0 to 1
-// @Increment		0.1
+/**
+ * Yaw rate integrator limit
+ *
+ * The portion of the integrator part in the control surface deflection is
+ * limited to this value
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_YR_IMAX, 0.2f);
 
-// @DisplayName		Maximum Yaw Rate
-// @Description		This limits the maximum yaw rate the controller will output (in degrees per second). Setting a value of zero disables the limit.
-// @Range		0 to 90.0 degrees per seconds
-// @Increment		1.0
-PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0);
+/**
+ * Maximum Yaw Rate
+ *
+ * This limits the maximum yaw rate the controller will output (in degrees per
+ * second). Setting a value of zero disables the limit.
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @max 90.0
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0.0f);
 
-// @DisplayName		Roll rate feed forward
-// @Description		Direct feed forward from rate setpoint to control surface output
-// @Range		0 to 10
-// @Increment		0.1
+/**
+ * Roll rate feed forward
+ *
+ * Direct feed forward from rate setpoint to control surface output
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_RR_FF, 0.3f);
 
-// @DisplayName		Pitch rate feed forward
-// @Description		Direct feed forward from rate setpoint to control surface output
-// @Range		0 to 10
-// @Increment		0.1
+/**
+ * Pitch rate feed forward
+ *
+ * Direct feed forward from rate setpoint to control surface output
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_PR_FF, 0.4f);
 
-// @DisplayName		Yaw rate feed forward
-// @Description		Direct feed forward from rate setpoint to control surface output
-// @Range		0 to 10
-// @Increment		0.1
+/**
+ * Yaw rate feed forward
+ *
+ * Direct feed forward from rate setpoint to control surface output
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_YR_FF, 0.3f);
 
-// @DisplayName		Minimal speed for yaw coordination
-// @Description		For airspeeds above this value the yaw rate is calculated for a coordinated turn. Set to a very high value to disable.
-// @Range		0 to 90.0 degrees per seconds
-// @Increment		1.0
+/**
+ * Minimal speed for yaw coordination
+ *
+ * For airspeeds above this value, the yaw rate is calculated for a coordinated
+ * turn. Set to a very high value to disable.
+ *
+ * @unit m/s
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_YCO_VMIN, 1000.0f);
 
-/* Airspeed parameters: the following parameters about airspeed are used by the attitude and the positon controller */
+/* Airspeed parameters:
+ * The following parameters about airspeed are used by the attitude and the
+ * position controller.
+ * */
 
-// @DisplayName		Minimum Airspeed
-// @Description		If the airspeed falls below this value the TECS controller will try to increase airspeed more aggressively
-// @Range		0.0 to 30
+/**
+ * Minimum Airspeed
+ *
+ * If the airspeed falls below this value, the TECS controller will try to
+ * increase airspeed more aggressively.
+ *
+ * @unit m/s
+ * @min 0.0
+ * @max 30.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 13.0f);
 
-// @DisplayName		Trim Airspeed
-// @Description		The TECS controller tries to fly at this airspeed
-// @Range		0.0 to 30
+/**
+ * Trim Airspeed
+ *
+ * The TECS controller tries to fly at this airspeed.
+ *
+ * @unit m/s
+ * @min 0.0
+ * @max 30.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 20.0f);
 
-// @DisplayName		Maximum Airspeed
-// @Description		If the airspeed is above this value the TECS controller will try to decrease airspeed more aggressively
-// @Range		0.0 to 30
+/**
+ * Maximum Airspeed
+ *
+ * If the airspeed is above this value, the TECS controller will try to decrease
+ * airspeed more aggressively.
+ *
+ * @unit m/s
+ * @min 0.0
+ * @max 30.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 50.0f);
 
-// @DisplayName		Roll Setpoint Offset
-// @Description		An airframe specific offset of the roll setpoint in degrees, the value is added to the roll setpoint and should correspond to the typical cruise speed of the airframe
-// @Range		-90.0 to 90.0
+/**
+ * Roll Setpoint Offset
+ *
+ * An airframe specific offset of the roll setpoint in degrees, the value is
+ * added to the roll setpoint and should correspond to the typical cruise speed
+ * of the airframe.
+ *
+ * @unit deg
+ * @min -90.0
+ * @max 90.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_RSP_OFF, 0.0f);
 
-// @DisplayName		Pitch Setpoint Offset
-// @Description		An airframe specific offset of the pitch setpoint in degrees, the value is added to the pitch setpoint and should correspond to the typical cruise speed of the airframe
-// @Range		-90.0 to 90.0
+/**
+ * Pitch Setpoint Offset
+ *
+ * An airframe specific offset of the pitch setpoint in degrees, the value is
+ * added to the pitch setpoint and should correspond to the typical cruise
+ * speed of the airframe.
+ *
+ * @unit deg
+ * @min -90.0
+ * @max 90.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_PSP_OFF, 0.0f);
 
-// @DisplayName		Max Manual Roll
-// @Description		Max roll for manual control in attitude stabilized mode
-// @Range		0.0 to 90.0
+/**
+ * Max Manual Roll
+ *
+ * Max roll for manual control in attitude stabilized mode
+ *
+ * @unit deg
+ * @min 0.0
+ * @max 90.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_MAN_R_MAX, 45.0f);
 
-// @DisplayName		Max Manual Pitch
-// @Description		Max pitch for manual control in attitude stabilized mode
-// @Range		0.0 to 90.0
+/**
+ * Max Manual Pitch
+ *
+ * Max pitch for manual control in attitude stabilized mode
+ *
+ * @unit deg
+ * @min 0.0
+ * @max 90.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_MAN_P_MAX, 45.0f);

From d5c0933d6516741f432a8f259149384fa2a2f95b Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Tue, 10 Jun 2014 14:29:17 +0200
Subject: [PATCH 09/13] mavlink: report global position setpoint and do this
 always no just when updated, otherwise the values are not visible in QGC

---
 ROMFS/px4fmu_common/init.d/rc.usb        |  2 ++
 src/modules/mavlink/mavlink_messages.cpp | 14 ++++++--------
 2 files changed, 8 insertions(+), 8 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb
index 76593881db..d6e638c0ab 100644
--- a/ROMFS/px4fmu_common/init.d/rc.usb
+++ b/ROMFS/px4fmu_common/init.d/rc.usb
@@ -17,6 +17,8 @@ mavlink stream -d /dev/ttyACM0 -s ATTITUDE_CONTROLS -r 30
 usleep 100000
 mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_0 -r 20
 usleep 100000
+mavlink stream -d /dev/ttyACM0 -s GLOBAL_POSITION_SETPOINT_INT -r 20
+usleep 100000
 
 # Exit shell to make it available to MAVLink
 exit
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index fd41b723ab..c44bdc53dd 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -938,14 +938,12 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		if (pos_sp_triplet_sub->update(t)) {
-			mavlink_msg_global_position_setpoint_int_send(_channel,
-					MAV_FRAME_GLOBAL,
-					(int32_t)(pos_sp_triplet->current.lat * 1e7),
-					(int32_t)(pos_sp_triplet->current.lon * 1e7),
-					(int32_t)(pos_sp_triplet->current.alt * 1000),
-					(int16_t)(pos_sp_triplet->current.yaw * M_RAD_TO_DEG_F * 100.0f));
-		}
+		mavlink_msg_global_position_setpoint_int_send(_channel,
+			MAV_FRAME_GLOBAL,
+			(int32_t)(pos_sp_triplet->current.lat * 1e7),
+			(int32_t)(pos_sp_triplet->current.lon * 1e7),
+			(int32_t)(pos_sp_triplet->current.alt * 1000),
+			(int16_t)(pos_sp_triplet->current.yaw * M_RAD_TO_DEG_F * 100.0f));
 	}
 };
 

From 064a75a3c289a32fa4d5234e3874712e7981f238 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Tue, 10 Jun 2014 15:02:20 +0200
Subject: [PATCH 10/13] mavlink: put update call back in

---
 src/modules/mavlink/mavlink_messages.cpp | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index c44bdc53dd..d94e7fc7e5 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -938,6 +938,8 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
+		/* always send this message, even if it has not been updated */
+		pos_sp_triplet_sub->update(t);
 		mavlink_msg_global_position_setpoint_int_send(_channel,
 			MAV_FRAME_GLOBAL,
 			(int32_t)(pos_sp_triplet->current.lat * 1e7),

From 7fa5458bc619df427fc29283ff5ff32b933f2906 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 11 Jun 2014 19:51:33 +0200
Subject: [PATCH 11/13] mtecs: add D gain for speed outer loop

---
 src/modules/fw_pos_control_l1/mtecs/mTecs.h     |  4 ++--
 .../fw_pos_control_l1/mtecs/mTecs_params.c      | 17 +++++++++++++++++
 2 files changed, 19 insertions(+), 2 deletions(-)

diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h
index ddd6583e86..346acfaf38 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h
@@ -160,8 +160,8 @@ protected:
 	/* control blocks */
 	BlockFFPILimitedCustom _controlTotalEnergy;		/**< FFPI controller for total energy control: output is throttle */
 	BlockFFPILimitedCustom _controlEnergyDistribution;	/**< FFPI controller for energy distribution control: output is pitch */
-	BlockPDLimited	_controlAltitude;		/**< P controller for altitude: output is the flight path angle setpoint */
-	BlockPLimited	_controlAirSpeed;			/**< P controller for airspeed: output is acceleration setpoint */
+	BlockPDLimited	_controlAltitude;		/**< PD controller for altitude: output is the flight path angle setpoint */
+	BlockPDLimited	_controlAirSpeed;			/**< PD controller for airspeed: output is acceleration setpoint */
 
 	/* Other calculation Blocks */
 	control::BlockDerivative _airspeedDerivative;
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
index 39514b2238..6165611b98 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
@@ -233,6 +233,23 @@ PARAM_DEFINE_FLOAT(MT_FPA_MAX, 30.0f);
  */
 PARAM_DEFINE_FLOAT(MT_ACC_P, 1.5f);
 
+/**
+ * D gain for the airspeed control
+ * Maps the change of airspeed error to the acceleration setpoint
+ *
+ * @min 0.0f
+ * @max 10.0f
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_ACC_D, 0.0f);
+
+/**
+ * Lowpass for ACC error derivative calculation (see MT_ACC_D)
+ *
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_ACC_D_LP, 1.0f);
+
 /**
  * Minimal acceleration (air)
  *

From a1bcd5d3136fb5320996c2df90eea91d230d55ac Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 11 Jun 2014 20:22:37 +0200
Subject: [PATCH 12/13] mtecs: small cleanup, move subclass to own file

---
 .../fw_pos_control_l1_main.cpp                |   2 +-
 src/modules/fw_pos_control_l1/module.mk       |   1 +
 .../fw_pos_control_l1/mtecs/limitoverride.cpp |  71 ++++++++++++
 .../fw_pos_control_l1/mtecs/limitoverride.h   | 107 ++++++++++++++++++
 src/modules/fw_pos_control_l1/mtecs/mTecs.cpp |  25 ----
 src/modules/fw_pos_control_l1/mtecs/mTecs.h   |  57 +---------
 6 files changed, 181 insertions(+), 82 deletions(-)
 create mode 100644 src/modules/fw_pos_control_l1/mtecs/limitoverride.cpp
 create mode 100644 src/modules/fw_pos_control_l1/mtecs/limitoverride.h

diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index fe4eb66a92..5df17e2ecf 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -1456,7 +1456,7 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
 		if (ground_speed_length > FLT_EPSILON) {
 			flightPathAngle = -asinf(ground_speed(2)/ground_speed_length);
 		}
-		fwPosctrl::mTecs::LimitOverride limitOverride;
+		fwPosctrl::LimitOverride limitOverride;
 		if (climbout_mode) {
 			limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad);
 		} else {
diff --git a/src/modules/fw_pos_control_l1/module.mk b/src/modules/fw_pos_control_l1/module.mk
index c887223f4a..af155fe089 100644
--- a/src/modules/fw_pos_control_l1/module.mk
+++ b/src/modules/fw_pos_control_l1/module.mk
@@ -41,4 +41,5 @@ SRCS		= fw_pos_control_l1_main.cpp \
 		  fw_pos_control_l1_params.c \
 		  landingslope.cpp \
 		  mtecs/mTecs.cpp \
+		  mtecs/limitoverride.cpp \
 		  mtecs/mTecs_params.c
diff --git a/src/modules/fw_pos_control_l1/mtecs/limitoverride.cpp b/src/modules/fw_pos_control_l1/mtecs/limitoverride.cpp
new file mode 100644
index 0000000000..58795edb65
--- /dev/null
+++ b/src/modules/fw_pos_control_l1/mtecs/limitoverride.cpp
@@ -0,0 +1,71 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: 	@author Thomas Gubler <thomasgubler@gmail.com>
+ *
+ * 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 limitoverride.cpp
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#include "limitoverride.h"
+
+namespace fwPosctrl {
+
+bool LimitOverride::applyOverride(BlockOutputLimiter &outputLimiterThrottle,
+		BlockOutputLimiter &outputLimiterPitch)
+{
+	bool ret = false;
+
+	if (overrideThrottleMinEnabled)	{
+		outputLimiterThrottle.setMin(overrideThrottleMin);
+		ret = true;
+	}
+	if (overrideThrottleMaxEnabled)	{
+		outputLimiterThrottle.setMax(overrideThrottleMax);
+		ret = true;
+	}
+	if (overridePitchMinEnabled)	{
+		outputLimiterPitch.setMin(overridePitchMin);
+		ret = true;
+	}
+	if (overridePitchMaxEnabled)	{
+		outputLimiterPitch.setMax(overridePitchMax);
+		ret = true;
+	}
+
+	return ret;
+}
+
+} /* namespace fwPosctrl */
diff --git a/src/modules/fw_pos_control_l1/mtecs/limitoverride.h b/src/modules/fw_pos_control_l1/mtecs/limitoverride.h
new file mode 100644
index 0000000000..64c2e7bbde
--- /dev/null
+++ b/src/modules/fw_pos_control_l1/mtecs/limitoverride.h
@@ -0,0 +1,107 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: 	@author Thomas Gubler <thomasgubler@gmail.com>
+ *
+ * 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 limitoverride.h
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+
+#ifndef LIMITOVERRIDE_H_
+#define LIMITOVERRIDE_H_
+
+#include "mTecs_blocks.h"
+
+namespace fwPosctrl
+{
+
+/* A small class which provides helper functions to override control output limits which are usually set by
+* parameters in special cases
+*/
+class LimitOverride
+{
+public:
+	LimitOverride() :
+		overrideThrottleMinEnabled(false),
+		overrideThrottleMaxEnabled(false),
+		overridePitchMinEnabled(false),
+		overridePitchMaxEnabled(false)
+	{};
+
+	~LimitOverride() {};
+
+	/*
+	* Override the limits of the outputlimiter instances given by the arguments with the limits saved in
+	* this class (if enabled)
+	* @return true if the limit was applied
+	*/
+	bool applyOverride(BlockOutputLimiter &outputLimiterThrottle,
+			BlockOutputLimiter &outputLimiterPitch);
+
+	/* Functions to enable or disable the override */
+	void enableThrottleMinOverride(float value) { enable(&overrideThrottleMinEnabled,
+			&overrideThrottleMin, value); }
+	void disableThrottleMinOverride() { disable(&overrideThrottleMinEnabled); }
+	void enableThrottleMaxOverride(float value) { enable(&overrideThrottleMaxEnabled,
+			&overrideThrottleMax, value); }
+	void disableThrottleMaxOverride() { disable(&overrideThrottleMaxEnabled); }
+	void enablePitchMinOverride(float value) { enable(&overridePitchMinEnabled,
+			&overridePitchMin, value); }
+	void disablePitchMinOverride() { disable(&overridePitchMinEnabled); }
+	void enablePitchMaxOverride(float value) { enable(&overridePitchMaxEnabled,
+			&overridePitchMax, value); }
+	void disablePitchMaxOverride() { disable(&overridePitchMaxEnabled); }
+
+protected:
+	bool overrideThrottleMinEnabled;
+	float overrideThrottleMin;
+	bool overrideThrottleMaxEnabled;
+	float overrideThrottleMax;
+	bool overridePitchMinEnabled;
+	float overridePitchMin; //in degrees (replaces param values)
+	bool overridePitchMaxEnabled;
+	float overridePitchMax; //in degrees (replaces param values)
+
+	/* Enable a specific limit override */
+	void enable(bool *flag, float *limit, float value) { *flag = true; *limit = value; };
+
+	/* Disable a specific limit override */
+	void disable(bool *flag) { *flag = false; };
+};
+
+} /* namespace fwPosctrl */
+
+#endif /* LIMITOVERRIDE_H_ */
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
index 32f9f19cac..5894333f3c 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
@@ -302,29 +302,4 @@ void mTecs::debug(const char *fmt, ...) {
 	debug_print(fmt, args);
 }
 
-bool mTecs::LimitOverride::applyOverride(BlockOutputLimiter &outputLimiterThrottle,
-		BlockOutputLimiter &outputLimiterPitch)
-{
-	bool ret = false;
-
-	if (overrideThrottleMinEnabled)	{
-		outputLimiterThrottle.setMin(overrideThrottleMin);
-		ret = true;
-	}
-	if (overrideThrottleMaxEnabled)	{
-		outputLimiterThrottle.setMax(overrideThrottleMax);
-		ret = true;
-	}
-	if (overridePitchMinEnabled)	{
-		outputLimiterPitch.setMin(overridePitchMin);
-		ret = true;
-	}
-	if (overridePitchMaxEnabled)	{
-		outputLimiterPitch.setMax(overridePitchMax);
-		ret = true;
-	}
-
-	return ret;
-}
-
 } /* namespace fwPosctrl */
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h
index 346acfaf38..0369640f28 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h
@@ -44,6 +44,7 @@
 #define MTECS_H_
 
 #include "mTecs_blocks.h"
+#include "limitoverride.h"
 
 #include <controllib/block/BlockParam.hpp>
 #include <drivers/drv_hrt.h>
@@ -60,62 +61,6 @@ public:
 	mTecs();
 	virtual ~mTecs();
 
-	/* A small class which provides helper fucntions to override control output limits which are usually set by
-	 * parameters in special cases
-	 */
-	class LimitOverride
-	{
-	public:
-		LimitOverride() :
-			overrideThrottleMinEnabled(false),
-			overrideThrottleMaxEnabled(false),
-			overridePitchMinEnabled(false),
-			overridePitchMaxEnabled(false)
-		{};
-
-		~LimitOverride() {};
-
-		/*
-		 * Override the limits of the outputlimiter instances given by the arguments with the limits saved in
-		 * this class (if enabled)
-		 * @return true if the limit was applied
-		 */
-		bool applyOverride(BlockOutputLimiter &outputLimiterThrottle,
-				BlockOutputLimiter &outputLimiterPitch);
-
-		/* Functions to enable or disable the override */
-		void enableThrottleMinOverride(float value) { enable(&overrideThrottleMinEnabled,
-				&overrideThrottleMin, value); }
-		void disableThrottleMinOverride() { disable(&overrideThrottleMinEnabled); }
-		void enableThrottleMaxOverride(float value) { enable(&overrideThrottleMaxEnabled,
-				&overrideThrottleMax, value); }
-		void disableThrottleMaxOverride() { disable(&overrideThrottleMaxEnabled); }
-		void enablePitchMinOverride(float value) { enable(&overridePitchMinEnabled,
-				&overridePitchMin, value); }
-		void disablePitchMinOverride() { disable(&overridePitchMinEnabled); }
-		void enablePitchMaxOverride(float value) { enable(&overridePitchMaxEnabled,
-				&overridePitchMax, value); }
-		void disablePitchMaxOverride() { disable(&overridePitchMaxEnabled); }
-
-	protected:
-		bool overrideThrottleMinEnabled;
-		float overrideThrottleMin;
-		bool overrideThrottleMaxEnabled;
-		float overrideThrottleMax;
-		bool overridePitchMinEnabled;
-		float overridePitchMin; //in degrees (replaces param values)
-		bool overridePitchMaxEnabled;
-		float overridePitchMax; //in degrees (replaces param values)
-
-		/* Enable a specific limit override */
-		void enable(bool *flag, float *limit, float value) { *flag = true; *limit = value;
-		};
-		/* Disable a specific limit override */
-		void disable(bool *flag) { *flag = false; };
-
-
-	};
-
 	/*
 	 * Control in altitude setpoint and speed mode
 	 */

From f9946c98a809d18e0a037ee45f39195fd92c62fd Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 11 Jun 2014 21:03:27 +0200
Subject: [PATCH 13/13] mtecs: filter airspeed

---
 src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 22 +++++++++++++------
 src/modules/fw_pos_control_l1/mtecs/mTecs.h   | 12 +++++-----
 .../fw_pos_control_l1/mtecs/mTecs_params.c    |  6 +++++
 src/modules/sdlog2/sdlog2.c                   |  1 +
 src/modules/sdlog2/sdlog2_messages.h          |  3 ++-
 src/modules/uORB/topics/tecs_status.h         |  1 +
 6 files changed, 32 insertions(+), 13 deletions(-)

diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
index 5894333f3c..03353cbc1a 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
@@ -58,6 +58,7 @@ mTecs::mTecs() :
 	_controlEnergyDistribution(this, "PIT", true),
 	_controlAltitude(this, "FPA", true),
 	_controlAirSpeed(this, "ACC"),
+	_airspeedLowpass(this, "A_LP"),
 	_airspeedDerivative(this, "AD"),
 	_throttleSp(0.0f),
 	_pitchSp(0.0f),
@@ -122,12 +123,18 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng
 	/* time measurement */
 	updateTimeMeasurement();
 
+	/* Filter arispeed */
+	float airspeedFiltered = _airspeedLowpass.update(airspeed);
+
 	/* calculate longitudinal acceleration setpoint from airspeed setpoint*/
-	float accelerationLongitudinalSp = _controlAirSpeed.update(airspeedSp - airspeed);
+	float accelerationLongitudinalSp = _controlAirSpeed.update(airspeedSp - airspeedFiltered);
 
 	/* Debug output */
 	if (_counter % 10 == 0) {
-		debug("updateFlightPathAngleSpeed airspeedSp %.4f, airspeed %.4f, accelerationLongitudinalSp%.4f", (double)airspeedSp, (double)airspeed, (double)accelerationLongitudinalSp);
+		debug("updateFlightPathAngleSpeed airspeedSp %.4f, airspeed %.4f airspeedFiltered %.4f,"
+				"accelerationLongitudinalSp%.4f",
+				(double)airspeedSp, (double)airspeed,
+				(double)airspeedFiltered, (double)accelerationLongitudinalSp);
 	}
 
 	/* Write part of the status message */
@@ -135,19 +142,20 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng
 	_status.flightPathAngle = flightPathAngle;
 	_status.airspeedSp = airspeedSp;
 	_status.airspeed = airspeed;
+	_status.airspeedFiltered = airspeedFiltered;
 
 
 	/* use longitudinal acceleration setpoint for total energy control */
-	return updateFlightPathAngleAcceleration(flightPathAngle, flightPathAngleSp, airspeed,
+	return updateFlightPathAngleAcceleration(flightPathAngle, flightPathAngleSp, airspeedFiltered,
 			accelerationLongitudinalSp, mode, limitOverride);
 }
 
-int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed,
+int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeedFiltered,
 		float accelerationLongitudinalSp, tecs_mode mode, LimitOverride limitOverride)
 {
 	/* check if all input arguments are numbers and abort if not so */
 	if (!isfinite(flightPathAngle) || !isfinite(flightPathAngleSp) ||
-			!isfinite(airspeed) || !isfinite(accelerationLongitudinalSp) || !isfinite(mode)) {
+			!isfinite(airspeedFiltered) || !isfinite(accelerationLongitudinalSp) || !isfinite(mode)) {
 		return -1;
 	}
 	/* time measurement */
@@ -160,7 +168,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 	float flightPathAngleError = flightPathAngleSp - flightPathAngle;
 	float airspeedDerivative = 0.0f;
 	if(_airspeedDerivative.getDt() > 0.0f) {
-		airspeedDerivative = _airspeedDerivative.update(airspeed);
+		airspeedDerivative = _airspeedDerivative.update(airspeedFiltered);
 	}
 	float airspeedDerivativeNorm = airspeedDerivative / CONSTANTS_ONE_G;
 	float airspeedDerivativeSp = accelerationLongitudinalSp;
@@ -186,7 +194,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 	}
 
 	/* Check airspeed: if below safe value switch to underspeed mode (if not in land or takeoff mode) */
-	if (mode != TECS_MODE_LAND && mode != TECS_MODE_TAKEOFF && airspeed < _airspeedMin.get()) {
+	if (mode != TECS_MODE_LAND && mode != TECS_MODE_TAKEOFF && airspeedFiltered < _airspeedMin.get()) {
 		mode = TECS_MODE_UNDERSPEED;
 	}
 
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h
index 0369640f28..c102f5dee5 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h
@@ -76,7 +76,7 @@ public:
 	/*
 	 * Control in flightPathAngle setpoint (flollow a slope etc.) and acceleration mode (base case)
 	 */
-	int updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed,
+	int updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeedFiltered,
 			float accelerationLongitudinalSp, tecs_mode mode, LimitOverride limitOverride);
 
 	/*
@@ -90,9 +90,10 @@ public:
 	void resetDerivatives(float airspeed);
 
 	/* Accessors */
-	bool getEnabled() {return _mTecsEnabled.get() > 0;}
-	float getThrottleSetpoint() {return _throttleSp;}
-	float getPitchSetpoint() {return _pitchSp;}
+	bool getEnabled() { return _mTecsEnabled.get() > 0; }
+	float getThrottleSetpoint() { return _throttleSp; }
+	float getPitchSetpoint() { return _pitchSp; }
+	float airspeedLowpassUpdate(float input) { return _airspeedLowpass.update(input); }
 
 protected:
 	/* parameters */
@@ -109,7 +110,8 @@ protected:
 	BlockPDLimited	_controlAirSpeed;			/**< PD controller for airspeed: output is acceleration setpoint */
 
 	/* Other calculation Blocks */
-	control::BlockDerivative _airspeedDerivative;
+	control::BlockLowPass _airspeedLowpass;		/**< low pass filter for airspeed */
+	control::BlockDerivative _airspeedDerivative;	/**< airspeed derivative calulation */
 
 	/* Output setpoints */
 	float _throttleSp;				/**< Throttle Setpoint from 0 to 1 */
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
index 6165611b98..c95bf1dc9d 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
@@ -222,6 +222,12 @@ PARAM_DEFINE_FLOAT(MT_FPA_MIN, -10.0f);
  */
 PARAM_DEFINE_FLOAT(MT_FPA_MAX, 30.0f);
 
+/**
+ * Lowpass (cutoff freq.) for airspeed
+ *
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_A_LP, 1.0f);
 
 /**
  * P gain for the airspeed control
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index c19579f0f9..0813bf7b0b 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1509,6 +1509,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle;
 			log_msg.body.log_TECS.airspeedSp = buf.tecs_status.airspeedSp;
 			log_msg.body.log_TECS.airspeed = buf.tecs_status.airspeed;
+			log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeedFiltered;
 			log_msg.body.log_TECS.airspeedDerivativeSp = buf.tecs_status.airspeedDerivativeSp;
 			log_msg.body.log_TECS.airspeedDerivative = buf.tecs_status.airspeedDerivative;
 			log_msg.body.log_TECS.totalEnergyRateSp = buf.tecs_status.totalEnergyRateSp;
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index a874351b30..c42ff0afec 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -355,6 +355,7 @@ struct log_TECS_s {
 	float flightPathAngle;
 	float airspeedSp;
 	float airspeed;
+	float airspeedFiltered;
 	float airspeedDerivativeSp;
 	float airspeedDerivative;
 
@@ -430,7 +431,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
 	LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
 	LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
-	LOG_FORMAT(TECS, "ffffffffffffB",	"AltSP,Alt,FpaSP,Fpa,AsSP,As,AsDSP,AsD,TERSP,TER,EDRSP,EDR,Mod"),
+	LOG_FORMAT(TECS, "fffffffffffffB",	"AltSP,Alt,FpaSP,Fpa,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,Mod"),
 	LOG_FORMAT(WIND, "ffff",	"X,Y,CovX,CovY"),
 
 	/* system-level messages, ID >= 0x80 */
diff --git a/src/modules/uORB/topics/tecs_status.h b/src/modules/uORB/topics/tecs_status.h
index fc530b2955..05310e9062 100644
--- a/src/modules/uORB/topics/tecs_status.h
+++ b/src/modules/uORB/topics/tecs_status.h
@@ -70,6 +70,7 @@ struct tecs_status_s {
 	float flightPathAngle;
 	float airspeedSp;
 	float airspeed;
+	float airspeedFiltered;
 	float airspeedDerivativeSp;
 	float airspeedDerivative;