Merge branch 'master' of git://github.com/PX4/Firmware into uORB_tiny

ROMFS/px4fmu_common/init.d/3031_phantom

@@ -37,3 +37,7 @@ then
 fi
 
 set MIXER FMU_Q
+
+# Provide ESC a constant 1000 us pulse
+set PWM_OUTPUTS 4
+set PWM_DISARMED 1000

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

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();
@@ -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,6 +490,7 @@ 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;
 				}
@@ -557,6 +566,7 @@ UBX::handle_message()
 
 					_rate_count_vel++;
 
+					_got_velned = true;
 					ret = 1;
 					break;
 				}

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;

src/drivers/px4io/px4io.cpp

@@ -197,8 +197,10 @@ public:
 	 * Print IO status.
 	 *
 	 * Print all relevant IO status information
+	 *
+	 * @param extended_status Shows more verbose information (in particular RC config)
 	 */
-	void			print_status();
+	void			print_status(bool extended_status);
 
 	/**
 	 * Fetch and print debug console output.
@@ -1850,7 +1852,7 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries)
 }
 
 void
-PX4IO::print_status()
+PX4IO::print_status(bool extended_status)
 {
 	/* basic configuration */
 	printf("protocol %u hardware %u bootloader %u buffer %uB crc 0x%04x%04x\n",
@@ -2013,19 +2015,21 @@ PX4IO::print_status()
 		printf("\n");
 	}
 
-	for (unsigned i = 0; i < _max_rc_input; i++) {
-		unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i;
-		uint16_t options = io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_OPTIONS);
-		printf("input %u min %u center %u max %u deadzone %u assigned %u options 0x%04x%s%s\n",
-		       i,
-		       io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MIN),
-		       io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_CENTER),
-		       io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MAX),
-		       io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_DEADZONE),
-		       io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_ASSIGNMENT),
-		       options,
-		       ((options & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) ? " ENABLED" : ""),
-		       ((options & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) ? " REVERSED" : ""));
+	if (extended_status) {
+		for (unsigned i = 0; i < _max_rc_input; i++) {
+			unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i;
+			uint16_t options = io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_OPTIONS);
+			printf("input %u min %u center %u max %u deadzone %u assigned %u options 0x%04x%s%s\n",
+			       i,
+			       io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MIN),
+			       io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_CENTER),
+			       io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MAX),
+			       io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_DEADZONE),
+			       io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_ASSIGNMENT),
+			       options,
+			       ((options & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) ? " ENABLED" : ""),
+			       ((options & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) ? " REVERSED" : ""));
+		}
 	}
 
 	printf("failsafe");
@@ -2853,7 +2857,7 @@ monitor(void)
 		if (g_dev != nullptr) {
 
 			printf("\033[2J\033[H"); /* move cursor home and clear screen */
-			(void)g_dev->print_status();
+			(void)g_dev->print_status(false);
 			(void)g_dev->print_debug();
 			printf("\n\n\n[ Use 'px4io debug <N>' for more output. Hit <enter> three times to exit monitor mode ]\n");
 
@@ -3119,7 +3123,7 @@ px4io_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "status")) {
 
 		printf("[px4io] loaded\n");
-		g_dev->print_status();
+		g_dev->print_status(true);
 
 		exit(0);
 	}

src/drivers/px4io/px4io_uploader.cpp

@@ -240,9 +240,9 @@ PX4IO_Uploader::upload(const char *filenames[])
 	close(_io_fd);
 	_io_fd = -1;
 
-        // sleep for enough time for the IO chip to boot. This makes
-        // forceupdate more reliably startup IO again after update
-        up_udelay(100*1000);
+	// sleep for enough time for the IO chip to boot. This makes
+	// forceupdate more reliably startup IO again after update
+	up_udelay(100*1000);
 
 	return ret;
 }

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()

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()

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()

src/lib/geo/geo.h

@@ -50,7 +50,7 @@
 
 __BEGIN_DECLS
 
-#include "geo/geo_mag_declination.h"
+#include "geo_mag_declination.h"
 
 #include <stdbool.h>
 

src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp

@@ -65,6 +65,7 @@
 #include <drivers/drv_hrt.h>
 
 #include <lib/mathlib/mathlib.h>
+#include <lib/geo/geo.h>
 
 #include <systemlib/systemlib.h>
 #include <systemlib/perf_counter.h>
@@ -287,6 +288,9 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 
 	float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f };
 
+	/* magnetic declination, in radians */
+	float mag_decl = 0.0f;
+
 	/* rotation matrix for magnetic declination */
 	math::Matrix<3, 3> R_decl;
 	R_decl.identity();
@@ -325,9 +329,6 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 
 				/* update parameters */
 				parameters_update(&ekf_param_handles, &ekf_params);
-
-				/* update mag declination rotation matrix */
-				R_decl.from_euler(0.0f, 0.0f, ekf_params.mag_decl);
 			}
 
 			/* only run filter if sensor values changed */
@@ -340,6 +341,13 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 				orb_check(sub_gps, &gps_updated);
 				if (gps_updated) {
 					orb_copy(ORB_ID(vehicle_gps_position), sub_gps, &gps);
+
+					if (gps.eph_m < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) {
+						mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f));
+
+						/* update mag declination rotation matrix */
+						R_decl.from_euler(0.0f, 0.0f, mag_decl);
+					}
 				}
 
 				bool global_pos_updated;
@@ -469,7 +477,15 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 						parameters_update(&ekf_param_handles, &ekf_params);
 
 						/* update mag declination rotation matrix */
-						R_decl.from_euler(0.0f, 0.0f, ekf_params.mag_decl);
+						if (gps.eph_m < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) {
+							mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f));
+
+						} else {
+							mag_decl = ekf_params.mag_decl;
+						}
+
+						/* update mag declination rotation matrix */
+						R_decl.from_euler(0.0f, 0.0f, mag_decl);
 
 						x_aposteriori_k[0] = z_k[0];
 						x_aposteriori_k[1] = z_k[1];
@@ -515,7 +531,7 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 
 					att.roll = euler[0];
 					att.pitch = euler[1];
-					att.yaw = euler[2] + ekf_params.mag_decl;
+					att.yaw = euler[2] + mag_decl;
 
 					att.rollspeed = x_aposteriori[0];
 					att.pitchspeed = x_aposteriori[1];

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);

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -232,8 +232,6 @@ private:
 
 		float throttle_land_max;
 
-		float loiter_hold_radius;
-
 		float heightrate_p;
 		float speedrate_p;
 
@@ -277,8 +275,6 @@ private:
 
 		param_t throttle_land_max;
 
-		param_t loiter_hold_radius;
-
 		param_t heightrate_p;
 		param_t speedrate_p;
 
@@ -441,7 +437,6 @@ FixedwingPositionControl::FixedwingPositionControl() :
 
 	_parameter_handles.l1_period = param_find("FW_L1_PERIOD");
 	_parameter_handles.l1_damping = param_find("FW_L1_DAMPING");
-	_parameter_handles.loiter_hold_radius = param_find("FW_LOITER_R");
 
 	_parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN");
 	_parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM");
@@ -513,7 +508,6 @@ FixedwingPositionControl::parameters_update()
 	/* L1 control parameters */
 	param_get(_parameter_handles.l1_damping, &(_parameters.l1_damping));
 	param_get(_parameter_handles.l1_period, &(_parameters.l1_period));
-	param_get(_parameter_handles.loiter_hold_radius, &(_parameters.loiter_hold_radius));
 
 	param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min));
 	param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim));

src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c

@@ -71,17 +71,6 @@ PARAM_DEFINE_FLOAT(FW_L1_PERIOD, 25.0f);
  */
 PARAM_DEFINE_FLOAT(FW_L1_DAMPING, 0.75f);
 
-/**
- * Default Loiter Radius
- *
- * This radius is used when no other loiter radius is set.
- *
- * @min 10.0
- * @max 100.0
- * @group L1 Control
- */
-PARAM_DEFINE_FLOAT(FW_LOITER_R, 50.0f);
-
 /**
  * Cruise throttle
  *

src/modules/mavlink/mavlink_commands.cpp

@@ -40,34 +40,31 @@
 
 #include "mavlink_commands.h"
 
-MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) : _channel(channel)
+MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) : _channel(channel), _cmd_time(0)
 {
 	_cmd_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_command));
-	_cmd = (struct vehicle_command_s *)_cmd_sub->get_data();
-}
-
-MavlinkCommandsStream::~MavlinkCommandsStream()
-{
 }
 
 void
 MavlinkCommandsStream::update(const hrt_abstime t)
 {
-	if (_cmd_sub->update(t)) {
+	struct vehicle_command_s cmd;
+
+	if (_cmd_sub->update(&_cmd_time, &cmd)) {
 		/* only send commands for other systems/components */
-		if (_cmd->target_system != mavlink_system.sysid || _cmd->target_component != mavlink_system.compid) {
+		if (cmd.target_system != mavlink_system.sysid || cmd.target_component != mavlink_system.compid) {
 			mavlink_msg_command_long_send(_channel,
-						      _cmd->target_system,
-						      _cmd->target_component,
-						      _cmd->command,
-						      _cmd->confirmation,
-						      _cmd->param1,
-						      _cmd->param2,
-						      _cmd->param3,
-						      _cmd->param4,
-						      _cmd->param5,
-						      _cmd->param6,
-						      _cmd->param7);
+						      cmd.target_system,
+						      cmd.target_component,
+						      cmd.command,
+						      cmd.confirmation,
+						      cmd.param1,
+						      cmd.param2,
+						      cmd.param3,
+						      cmd.param4,
+						      cmd.param5,
+						      cmd.param6,
+						      cmd.param7);
 		}
 	}
 }

src/modules/mavlink/mavlink_commands.h

@@ -55,10 +55,10 @@ private:
 	MavlinkOrbSubscription *_cmd_sub;
 	struct vehicle_command_s *_cmd;
 	mavlink_channel_t _channel;
+	uint64_t _cmd_time;
 
 public:
 	MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel);
-	~MavlinkCommandsStream();
 	void update(const hrt_abstime t);
 };
 

src/modules/mavlink/mavlink_main.cpp

@@ -236,6 +236,12 @@ Mavlink::Mavlink() :
 	_subscribe_to_stream_rate(0.0f),
 	_flow_control_enabled(true),
 	_message_buffer({}),
+	_param_initialized(false),
+	_param_system_id(0),
+	_param_component_id(0),
+	_param_system_type(0),
+	_param_use_hil_gps(0),
+
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "mavlink"))
 {
@@ -494,44 +500,39 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg)
 
 void Mavlink::mavlink_update_system(void)
 {
-	static bool initialized = false;
-	static param_t param_system_id;
-	static param_t param_component_id;
-	static param_t param_system_type;
-	static param_t param_use_hil_gps;
 
-	if (!initialized) {
-		param_system_id = param_find("MAV_SYS_ID");
-		param_component_id = param_find("MAV_COMP_ID");
-		param_system_type = param_find("MAV_TYPE");
-		param_use_hil_gps = param_find("MAV_USEHILGPS");
-		initialized = true;
+	if (!_param_initialized) {
+		_param_system_id = param_find("MAV_SYS_ID");
+		_param_component_id = param_find("MAV_COMP_ID");
+		_param_system_type = param_find("MAV_TYPE");
+		_param_use_hil_gps = param_find("MAV_USEHILGPS");
+		_param_initialized = true;
 	}
 
 	/* update system and component id */
 	int32_t system_id;
-	param_get(param_system_id, &system_id);
+	param_get(_param_system_id, &system_id);
 
 	if (system_id > 0 && system_id < 255) {
 		mavlink_system.sysid = system_id;
 	}
 
 	int32_t component_id;
-	param_get(param_component_id, &component_id);
+	param_get(_param_component_id, &component_id);
 
 	if (component_id > 0 && component_id < 255) {
 		mavlink_system.compid = component_id;
 	}
 
 	int32_t system_type;
-	param_get(param_system_type, &system_type);
+	param_get(_param_system_type, &system_type);
 
 	if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) {
 		mavlink_system.type = system_type;
 	}
 
 	int32_t use_hil_gps;
-	param_get(param_use_hil_gps, &use_hil_gps);
+	param_get(_param_use_hil_gps, &use_hil_gps);
 
 	_use_hil_gps = (bool)use_hil_gps;
 }
@@ -790,9 +791,14 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav
 {
 	switch (msg->msgid) {
 	case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
-			/* Start sending parameters */
-			mavlink_pm_start_queued_send();
-			mavlink_missionlib_send_gcs_string("[mavlink pm] sending list");
+			mavlink_param_request_list_t req;
+			mavlink_msg_param_request_list_decode(msg, &req);
+			if (req.target_system == mavlink_system.sysid &&
+					(req.target_component == mavlink_system.compid || req.target_component == MAV_COMP_ID_ALL)) {
+				/* Start sending parameters */
+				mavlink_pm_start_queued_send();
+				mavlink_missionlib_send_gcs_string("[mavlink pm] sending list");
+			}
 		} break;
 
 	case MAVLINK_MSG_ID_PARAM_SET: {
@@ -814,7 +820,7 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav
 
 					if (param == PARAM_INVALID) {
 						char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
-						sprintf(buf, "[mavlink pm] unknown: %s", name);
+						sprintf(buf, "[pm] unknown: %s", name);
 						mavlink_missionlib_send_gcs_string(buf);
 
 					} else {
@@ -954,6 +960,7 @@ void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state)
 	state->current_partner_compid = 0;
 	state->timestamp_lastaction = 0;
 	state->timestamp_last_send_setpoint = 0;
+	state->timestamp_last_send_request = 0;
 	state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT;
 	state->current_dataman_id = 0;
 }
@@ -1002,8 +1009,6 @@ void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq)
 
 	} else {
 		mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds");
-
-		if (_verbose) { warnx("ERROR: index out of bounds"); }
 	}
 }
 
@@ -1070,13 +1075,12 @@ void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, u
 		wpr.seq = seq;
 		mavlink_msg_mission_request_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpr);
 		mavlink_missionlib_send_message(&msg);
+		_wpm->timestamp_last_send_request = hrt_absolute_time();
 
 		if (_verbose) { warnx("Sent waypoint request %u to ID %u", wpr.seq, wpr.target_system); }
 
 	} else {
 		mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity");
-
-		if (_verbose) { warnx("ERROR: Waypoint index exceeds list capacity"); }
 	}
 }
 
@@ -1107,11 +1111,13 @@ void Mavlink::mavlink_waypoint_eventloop(uint64_t now)
 
 		mavlink_missionlib_send_gcs_string("Operation timeout");
 
-		if (_verbose) { warnx("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", _wpm->current_state); }
-
 		_wpm->current_state = MAVLINK_WPM_STATE_IDLE;
 		_wpm->current_partner_sysid = 0;
 		_wpm->current_partner_compid = 0;
+
+	} else if (now - _wpm->timestamp_last_send_request > 500000 && _wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) {
+		/* try to get WP again after short timeout */
+		mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id);
 	}
 }
 
@@ -1139,8 +1145,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 
 			} else {
 				mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch");
-
-				if (_verbose) { warnx("REJ. WP CMD: curr partner id mismatch"); }
 			}
 
 			break;
@@ -1164,21 +1168,11 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 
 					} else {
 						mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list");
-
-						if (_verbose) { warnx("IGN WP CURR CMD: Not in list"); }
 					}
 
 				} else {
 					mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy");
-
-					if (_verbose) { warnx("IGN WP CURR CMD: Busy"); }
-
 				}
-
-			} else {
-				mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
-
-				if (_verbose) { warnx("REJ. WP CMD: target id mismatch"); }
 			}
 
 			break;
@@ -1208,14 +1202,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 
 				} else {
 					mavlink_missionlib_send_gcs_string("IGN REQUEST LIST: Busy");
-
-					if (_verbose) { warnx("IGN REQUEST LIST: Busy"); }
 				}
-
-			} else {
-				mavlink_missionlib_send_gcs_string("REJ. REQUEST LIST: target id mismatch");
-
-				if (_verbose) { warnx("REJ. REQUEST LIST: target id mismatch"); }
 			}
 
 			break;
@@ -1232,8 +1219,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 
 					mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list");
 
-					if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.", wpr.seq); }
-
 					break;
 				}
 
@@ -1244,15 +1229,13 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 				if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) {
 
 					if (wpr.seq == 0) {
-						if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
+						if (_verbose) { warnx("Got ITEM_REQUEST of waypoint %u from %u changing to STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
 
 						_wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS;
 
 					} else {
 						mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0");
 
-						if (_verbose) { warnx("REJ. WP CMD: First id != 0"); }
-
 						break;
 					}
 
@@ -1260,17 +1243,15 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 
 					if (wpr.seq == _wpm->current_wp_id) {
 
-						if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
+						if (_verbose) { warnx("Got ITEM_REQUEST of waypoint %u (again) from %u staying in STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
 
 					} else if (wpr.seq == _wpm->current_wp_id + 1) {
 
-						if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
+						if (_verbose) { warnx("Got ITEM_REQUEST of waypoint %u from %u staying in STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
 
 					} else {
 						mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected");
 
-						if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).", wpr.seq, _wpm->current_wp_id, _wpm->current_wp_id + 1); }
-
 						break;
 					}
 
@@ -1278,8 +1259,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 
 					mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
 
-					if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).", _wpm->current_state); }
-
 					break;
 				}
 
@@ -1293,7 +1272,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 				} else {
 					mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR);
 
-					if (_verbose) { warnx("ERROR: Waypoint %u out of bounds", wpr.seq); }
+					mavlink_missionlib_send_gcs_string("ERROR: Waypoint out of bounds");
 				}
 
 
@@ -1304,12 +1283,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 					mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
 
 					if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.", msg->sysid, _wpm->current_partner_sysid); }
-
-				} else {
-
-					mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
-
-					if (_verbose) { warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); }
 				}
 			}
 
@@ -1333,15 +1306,11 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 					}
 
 					if (wpc.count == 0) {
-						mavlink_missionlib_send_gcs_string("COUNT 0");
-
-						if (_verbose) { warnx("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE"); }
+						mavlink_missionlib_send_gcs_string("WP COUNT 0");
 
 						break;
 					}
 
-					if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); }
-
 					_wpm->current_state = MAVLINK_WPM_STATE_GETLIST;
 					_wpm->current_wp_id = 0;
 					_wpm->current_partner_sysid = msg->sysid;
@@ -1355,25 +1324,13 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 					if (_wpm->current_wp_id == 0) {
 						mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN");
 
-						if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u", wpc.count, msg->sysid); }
-
 					} else {
-						mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
-
-						if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.", _wpm->current_wp_id); }
+						mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy with WP");
 					}
 
 				} else {
 					mavlink_missionlib_send_gcs_string("IGN MISSION_COUNT CMD: Busy");
-
-					if (_verbose) { warnx("IGN MISSION_COUNT CMD: Busy"); }
 				}
-
-			} else {
-
-				mavlink_missionlib_send_gcs_string("REJ. WP COUNT CMD: target id mismatch");
-
-				if (_verbose) { warnx("IGNORED WAYPOINT COUNT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); }
 			}
 		}
 		break;
@@ -1395,7 +1352,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 
 					if (wp.seq != 0) {
 						mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP not 0");
-						warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.", wp.seq);
 						break;
 					}
 
@@ -1403,12 +1359,11 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 
 					if (wp.seq >= _wpm->current_count) {
 						mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP out of bounds");
-						warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was out of bounds.", wp.seq);
 						break;
 					}
 
 					if (wp.seq != _wpm->current_wp_id) {
-						warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.", wp.seq, _wpm->current_wp_id);
+						mavlink_missionlib_send_gcs_string("IGN: waypoint ID mismatch");
 						mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id);
 						break;
 					}
@@ -1473,11 +1428,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 				} else {
 					mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id);
 				}
-
-			} else {
-				mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
-
-				if (_verbose) { warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); }
 			}
 
 			break;
@@ -1513,13 +1463,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 
 					if (_verbose) { warnx("IGN WP CLEAR CMD: Busy"); }
 				}
-
-
-			} else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && _wpm->current_state != MAVLINK_WPM_STATE_IDLE) {
-
-				mavlink_missionlib_send_gcs_string("REJ. WP CLERR CMD: target id mismatch");
-
-				if (_verbose) { warnx("IGNORED WAYPOINT CLEAR COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); }
 			}
 
 			break;
@@ -1535,11 +1478,10 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 void
 Mavlink::mavlink_missionlib_send_message(mavlink_message_t *msg)
 {
-	uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN];
+	uint8_t buf[MAVLINK_MAX_PACKET_LEN];
 
-	uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg);
-
-	mavlink_send_uart_bytes(_channel, missionlib_msg_buf, len);
+	uint16_t len = mavlink_msg_to_send_buffer(buf, msg);
+	mavlink_send_uart_bytes(_channel, buf, len);
 }
 
 
@@ -1613,31 +1555,36 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
 				/* delete stream */
 				LL_DELETE(_streams, stream);
 				delete stream;
+				warnx("deleted stream %s", stream->get_name());
 			}
 
 			return OK;
 		}
 	}
 
-	if (interval > 0) {
-		/* search for stream with specified name in supported streams list */
-		for (unsigned int i = 0; streams_list[i] != nullptr; i++) {
-			if (strcmp(stream_name, streams_list[i]->get_name()) == 0) {
-				/* create new instance */
-				stream = streams_list[i]->new_instance();
-				stream->set_channel(get_channel());
-				stream->set_interval(interval);
-				stream->subscribe(this);
-				LL_APPEND(_streams, stream);
-				return OK;
-			}
-		}
-
-	} else {
-		/* stream not found, nothing to disable */
+	if (interval == 0) {
+		/* stream was not active and is requested to be disabled, do nothing */
 		return OK;
 	}
 
+	/* search for stream with specified name in supported streams list */
+	for (unsigned int i = 0; streams_list[i] != nullptr; i++) {
+
+		if (strcmp(stream_name, streams_list[i]->get_name()) == 0) {
+			/* create new instance */
+			stream = streams_list[i]->new_instance();
+			stream->set_channel(get_channel());
+			stream->set_interval(interval);
+			stream->subscribe(this);
+			LL_APPEND(_streams, stream);
+
+			return OK;
+		}
+	}
+
+	/* if we reach here, the stream list does not contain the stream */
+	warnx("stream %s not found", stream_name);
+
 	return ERROR;
 }
 
@@ -1927,7 +1874,7 @@ Mavlink::task_main(int argc, char *argv[])
 	/* if we are passing on mavlink messages, we need to prepare a buffer for this instance */
 	if (_passing_on) {
 		/* initialize message buffer if multiplexing is on */
-		if (OK != message_buffer_init(500)) {
+		if (OK != message_buffer_init(300)) {
 			errx(1, "can't allocate message buffer, exiting");
 		}
 
@@ -1957,9 +1904,12 @@ Mavlink::task_main(int argc, char *argv[])
 	_task_running = true;
 
 	MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update));
+	uint64_t param_time = 0;
 	MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status));
+	uint64_t status_time = 0;
 
-	struct vehicle_status_s *status = (struct vehicle_status_s *) status_sub->get_data();
+	struct vehicle_status_s status;
+	status_sub->update(&status_time, &status);
 
 	MavlinkCommandsStream commands_stream(this, _channel);
 
@@ -2016,14 +1966,14 @@ Mavlink::task_main(int argc, char *argv[])
 
 		hrt_abstime t = hrt_absolute_time();
 
-		if (param_sub->update(t)) {
+		if (param_sub->update(&param_time, nullptr)) {
 			/* parameters updated */
 			mavlink_update_system();
 		}
 
-		if (status_sub->update(t)) {
+		if (status_sub->update(&status_time, &status)) {
 			/* switch HIL mode if required */
-			set_hil_enabled(status->hil_state == HIL_STATE_ON);
+			set_hil_enabled(status.hil_state == HIL_STATE_ON);
 		}
 
 		/* update commands stream */

src/modules/mavlink/mavlink_main.h

@@ -93,6 +93,7 @@ struct mavlink_wpm_storage {
 	uint8_t current_partner_compid;
 	uint64_t timestamp_lastaction;
 	uint64_t timestamp_last_send_setpoint;
+	uint64_t timestamp_last_send_request;
 	uint32_t timeout;
 	int current_dataman_id;
 };
@@ -276,11 +277,16 @@ private:
 		int size;
 		char *data;
 	};
-	mavlink_message_buffer _message_buffer;
+	mavlink_message_buffer	_message_buffer;
 
-	pthread_mutex_t _message_buffer_mutex;
+	pthread_mutex_t		_message_buffer_mutex;
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
+	bool			_param_initialized;
+	param_t			_param_system_id;
+	param_t			_param_component_id;
+	param_t			_param_system_type;
+	param_t			_param_use_hil_gps;
 
 	/**
 	 * Send one parameter.

src/modules/mavlink/mavlink_messages.h

@@ -43,6 +43,19 @@
 
 #include "mavlink_stream.h"
 
-extern MavlinkStream *streams_list[];
+class StreamListItem {
+
+public:
+	MavlinkStream* (*new_instance)();
+	const char* (*get_name)();
+
+	StreamListItem(MavlinkStream* (*inst)(), const char* (*name)()) :
+		new_instance(inst),
+		get_name(name) {};
+
+	~StreamListItem() {};
+};
+
+extern StreamListItem *streams_list[];
 
 #endif /* MAVLINK_MESSAGES_H_ */

src/modules/mavlink/mavlink_orb_subscription.cpp

@@ -44,56 +44,65 @@
 #include <uORB/uORB.h>
 #include <stdio.h>
 
+#include <systemlib/err.h>
+
 #include "mavlink_orb_subscription.h"
 
 MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) :
 	_fd(orb_subscribe(_topic)),
 	_published(false),
 	_topic(topic),
-	_last_check(0),
 	next(nullptr)
 {
-	_data = malloc(topic->o_size);
-	memset(_data, 0, topic->o_size);
 }
 
 MavlinkOrbSubscription::~MavlinkOrbSubscription()
 {
 	close(_fd);
-	free(_data);
 }
 
 orb_id_t
-MavlinkOrbSubscription::get_topic()
+MavlinkOrbSubscription::get_topic() const
 {
 	return _topic;
 }
 
-void *
-MavlinkOrbSubscription::get_data()
+bool
+MavlinkOrbSubscription::update(uint64_t *time, void* data)
 {
-	return _data;
+	// TODO this is NOT atomic operation, we can get data newer than time
+	// if topic was published between orb_stat and orb_copy calls.
+
+	uint64_t time_topic;
+	if (orb_stat(_fd, &time_topic)) {
+		/* error getting last topic publication time */
+		time_topic = 0;
+	}
+
+	if (time_topic != *time) {
+
+		if (orb_copy(_topic, _fd, data)) {
+			/* error copying topic data */
+			memset(data, 0, _topic->o_size);
+			//warnx("err copy, fd: %d, obj: %s, size: %d", _fd, _topic->o_name, _topic->o_size);
+			return false;
+
+		} else {
+			/* data copied successfully */
+			_published = true;
+			*time = time_topic;
+			return true;
+		}
+
+	} else {
+		return false;
+	}
 }
 
 bool
-MavlinkOrbSubscription::update(const hrt_abstime t)
+MavlinkOrbSubscription::update(void* data)
 {
-	if (_last_check == t) {
-		/* already checked right now, return result of the check */
-		return _updated;
-
-	} else {
-		_last_check = t;
-		orb_check(_fd, &_updated);
-
-		if (_updated) {
-			orb_copy(_topic, _fd, _data);
-			_published = true;
-			return true;
-		}
-	}
-
-	return false;
+	return !orb_copy(_topic, _fd, data);
 }
 
 bool

src/modules/mavlink/mavlink_orb_subscription.h

@@ -48,12 +48,26 @@
 class MavlinkOrbSubscription
 {
 public:
-	MavlinkOrbSubscription *next;	/*< pointer to next subscription in list */
+	MavlinkOrbSubscription *next;	///< pointer to next subscription in list
 
 	MavlinkOrbSubscription(const orb_id_t topic);
 	~MavlinkOrbSubscription();
 
-	bool update(const hrt_abstime t);
+	/**
+	 * Check if subscription updated and get data.
+	 *
+	 * @return true only if topic was updated and data copied to buffer successfully.
+	 * If topic was not updated since last check it will return false but still copy the data.
+	 * If no data available data buffer will be filled with zeroes.
+	 */
+	bool update(uint64_t *time, void* data);
+
+	/**
+	 * Copy topic data to given buffer.
+	 *
+	 * @return true only if topic data copied successfully.
+	 */
+	bool update(void* data);
 
 	/**
 	 * Check if the topic has been published.
@@ -62,16 +76,12 @@ public:
 	 * @return true if the topic has been published at least once.
 	 */
 	bool is_published();
-	void *get_data();
-	orb_id_t get_topic();
+	orb_id_t get_topic() const;
 
 private:
-	const orb_id_t _topic;		/*< topic metadata */
-	int _fd;					/*< subscription handle */
-	bool _published;			/*< topic was ever published */
-	void *_data;				/*< pointer to data buffer */
-	hrt_abstime _last_check;	/*< time of last check */
-	bool _updated;				/*< updated on last check */
+	const orb_id_t _topic;		///< topic metadata
+	int _fd;			///< subscription handle
+	bool _published;		///< topic was ever published
 };
 
 

src/modules/mavlink/mavlink_receiver.cpp

@@ -952,7 +952,6 @@ MavlinkReceiver::receive_start(Mavlink *parent)
 	(void)pthread_attr_setschedparam(&receiveloop_attr, &param);
 
 	pthread_attr_setstacksize(&receiveloop_attr, 2900);
-
 	pthread_t thread;
 	pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void *)parent);
 

src/modules/mavlink/mavlink_stream.h

@@ -50,14 +50,6 @@ class MavlinkStream;
 
 class MavlinkStream
 {
-private:
-	hrt_abstime _last_sent;
-
-protected:
-	mavlink_channel_t _channel;
-	unsigned int _interval;
-
-	virtual void send(const hrt_abstime t) = 0;
 
 public:
 	MavlinkStream *next;
@@ -71,9 +63,19 @@ public:
 	 * @return 0 if updated / sent, -1 if unchanged
 	 */
 	int update(const hrt_abstime t);
-	virtual MavlinkStream *new_instance() = 0;
+	static MavlinkStream *new_instance();
+	static const char *get_name_static();
 	virtual void subscribe(Mavlink *mavlink) = 0;
-	virtual const char *get_name() = 0;
+	virtual const char *get_name() const = 0;
+
+protected:
+	mavlink_channel_t _channel;
+	unsigned int _interval;
+
+	virtual void send(const hrt_abstime t) = 0;
+
+private:
+	hrt_abstime _last_sent;
 };
 
 

src/modules/position_estimator_inav/position_estimator_inav_main.c

@@ -71,19 +71,20 @@
 #include "inertial_filter.h"
 
 #define MIN_VALID_W 0.00001f
+#define PUB_INTERVAL 10000	// limit publish rate to 100 Hz
+#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 */
 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
 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.
  */
@@ -135,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);
@@ -199,8 +210,21 @@ 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 eph = 1.0;
-	float epv = 1.0;
+	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));
+	memset(R_buf, 0, sizeof(R_buf));
+	memset(R_gps, 0, sizeof(R_gps));
+	int buf_ptr = 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));
@@ -257,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)
@@ -536,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;
@@ -597,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");
@@ -657,16 +679,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 = 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;
@@ -674,8 +702,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 							corr_gps[2][1] = 0.0f;
 						}
 
-						eph = fminf(eph, gps.eph_m);
-						epv = fminf(epv, gps.epv_m);
+						/* save rotation matrix at this moment */
+						memcpy(R_gps, R_buf[est_i], sizeof(R_gps));
 
 						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);
@@ -717,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;
@@ -731,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);
 
@@ -763,7 +795,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) {
@@ -777,6 +809,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;
@@ -807,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);
@@ -832,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);
 
@@ -910,8 +969,25 @@ 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 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 */
 			local_pos.xy_valid = can_estimate_xy;
 			local_pos.v_xy_valid = can_estimate_xy;

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.2f);
 
 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;
 }

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;
 };
 
 /**

src/modules/systemlib/perf_counter.c

@@ -301,7 +301,7 @@ perf_print_counter_fd(int fd, perf_counter_t handle)
 	case PC_ELAPSED: {
 		struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
 
-		dprintf(fd, "%s: %llu events, %lluus elapsed, %llu avg, min %lluus max %lluus\n",
+		dprintf(fd, "%s: %llu events, %lluus elapsed, %lluus avg, min %lluus max %lluus\n",
 		       handle->name,
 		       pce->event_count,
 		       pce->time_total,
@@ -314,7 +314,7 @@ perf_print_counter_fd(int fd, perf_counter_t handle)
 	case PC_INTERVAL: {
 		struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle;
 
-		dprintf(fd, "%s: %llu events, %llu avg, min %lluus max %lluus\n",
+		dprintf(fd, "%s: %llu events, %lluus avg, min %lluus max %lluus\n",
 		       handle->name,
 		       pci->event_count,
 		       (pci->time_last - pci->time_first) / pci->event_count,