Merged master into driver_framework

src/drivers/boards/aerocore/board_config.h

@@ -82,6 +82,9 @@ __BEGIN_DECLS
 #define GPIO_SPI_CS_GYRO	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3)
 #define GPIO_SPI_CS_BARO	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN4)
 
+/* SPI4--Ramtron */
+#define PX4_SPI_BUS_RAMTRON	4
+
 /* Nominal chip selects for devices on SPI bus #3 */
 #define PX4_SPIDEV_ACCEL_MAG	0
 #define PX4_SPIDEV_GYRO		1

src/drivers/boards/px4fmu-v2/board_config.h

@@ -113,6 +113,7 @@ __BEGIN_DECLS
 #define GPIO_SPI_CS_EXT3	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
 
 #define PX4_SPI_BUS_SENSORS	1
+#define PX4_SPI_BUS_RAMTRON	2
 #define PX4_SPI_BUS_EXT		4
 
 /* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */

src/drivers/pwm_out_sim/pwm_out_sim.cpp

@@ -548,7 +548,7 @@ PWMSim::control_callback(uintptr_t handle,
 	const actuator_controls_s *controls = (actuator_controls_s *)handle;
 
 	if (_armed) {
-		input = controls->control[control_index];
+		input = controls[control_group].control[control_index];
 
 	} else {
 		/* clamp actuator to zero if not armed */

src/drivers/px4fmu/fmu.cpp

@@ -1469,6 +1469,10 @@ PX4FMU::sensor_reset(int ms)
 	stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
 	stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
 
+	stm32_configgpio(GPIO_SPI1_SCK);
+	stm32_configgpio(GPIO_SPI1_MISO);
+	stm32_configgpio(GPIO_SPI1_MOSI);
+
 	// // XXX bring up the EXTI pins again
 	// stm32_configgpio(GPIO_GYRO_DRDY);
 	// stm32_configgpio(GPIO_MAG_DRDY);

src/drivers/px4io/px4io.cpp

@@ -1780,8 +1780,8 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
 
 	/* get RSSI from input channel */
 	if (_rssi_pwm_chan > 0 && _rssi_pwm_chan <= input_rc_s::RC_INPUT_MAX_CHANNELS && _rssi_pwm_max - _rssi_pwm_min != 0) {
-		int rssi = (input_rc.values[_rssi_pwm_chan - 1] - _rssi_pwm_min) /
-			   ((_rssi_pwm_max - _rssi_pwm_min) / 100);
+		int rssi = ((input_rc.values[_rssi_pwm_chan - 1] - _rssi_pwm_min) * 100) /
+			   (_rssi_pwm_max - _rssi_pwm_min);
 		rssi = rssi > 100 ? 100 : rssi;
 		rssi = rssi < 0 ? 0 : rssi;
 		input_rc.rssi = rssi;

src/firmware/posix/CMakeLists.txt

@@ -36,7 +36,7 @@ add_dependencies(run_config mainapp)
 
 foreach(viewer none jmavsim gazebo)
 	foreach(debugger none gdb lldb ddd valgrind)
-		foreach(model none iris vtol)
+		foreach(model none iris tailsitter)
 			if (debugger STREQUAL "none")
 				if (model STREQUAL "none")
 					set(_targ_name "${viewer}")

src/include/mavlink/mavlink_log.h

@@ -106,10 +106,10 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...);
  * @param _fd		A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
  * @param _text		The text to log;
  */
-#define mavlink_and_console_log_emergency(_fd, _text, ...)		mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, _text, ##__VA_ARGS__); \
-	fprintf(stderr, "telem> "); \
-	fprintf(stderr, _text, ##__VA_ARGS__); \
-	fprintf(stderr, "\n");
+#define mavlink_and_console_log_emergency(_fd, _text, ...)		do { mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, _text, ##__VA_ARGS__); \
+		fprintf(stderr, "telem> "); \
+		fprintf(stderr, _text, ##__VA_ARGS__); \
+		fprintf(stderr, "\n"); } while(0);
 
 /**
  * Send a mavlink critical message and print to console.
@@ -117,10 +117,10 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...);
  * @param _fd		A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
  * @param _text		The text to log;
  */
-#define mavlink_and_console_log_critical(_fd, _text, ...)		mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, _text, ##__VA_ARGS__); \
-	fprintf(stderr, "telem> "); \
-	fprintf(stderr, _text, ##__VA_ARGS__); \
-	fprintf(stderr, "\n");
+#define mavlink_and_console_log_critical(_fd, _text, ...)		do { mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, _text, ##__VA_ARGS__); \
+		fprintf(stderr, "telem> "); \
+		fprintf(stderr, _text, ##__VA_ARGS__); \
+		fprintf(stderr, "\n");  } while(0);
 
 /**
  * Send a mavlink emergency message and print to console.
@@ -128,10 +128,10 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...);
  * @param _fd		A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
  * @param _text		The text to log;
  */
-#define mavlink_and_console_log_info(_fd, _text, ...)			mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_INFO, _text, ##__VA_ARGS__); \
-	fprintf(stderr, "telem> "); \
-	fprintf(stderr, _text, ##__VA_ARGS__); \
-	fprintf(stderr, "\n");
+#define mavlink_and_console_log_info(_fd, _text, ...)			do { mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_INFO, _text, ##__VA_ARGS__); \
+		fprintf(stderr, "telem> "); \
+		fprintf(stderr, _text, ##__VA_ARGS__); \
+		fprintf(stderr, "\n"); } while(0);
 
 struct mavlink_logmessage {
 	char text[MAVLINK_LOG_MAXLEN + 1];

src/lib/ecl/CMakeLists.txt

@@ -39,6 +39,7 @@ px4_add_module(
 		attitude_fw/ecl_pitch_controller.cpp
 		attitude_fw/ecl_roll_controller.cpp
 		attitude_fw/ecl_yaw_controller.cpp
+		attitude_fw/ecl_wheel_controller.cpp
 		l1/ecl_l1_pos_controller.cpp
 		validation/data_validator.cpp
 		validation/data_validator_group.cpp

src/lib/ecl/attitude_fw/ecl_controller.h

@@ -75,6 +75,8 @@ struct ECL_ControlData {
 	float airspeed_max;
 	float airspeed;
 	float scaler;
+	float groundspeed;
+	float groundspeed_scaler;
 	bool lock_integrator;
 };
 

src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp

@@ -0,0 +1,153 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name ECL 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 ecl_wheel_controller.cpp
+ * Implementation of a simple PID wheel controller for heading tracking.
+ *
+ * Authors and acknowledgements in header.
+ */
+
+#include "ecl_wheel_controller.h"
+#include <stdint.h>
+#include <float.h>
+#include <geo/geo.h>
+#include <ecl/ecl.h>
+#include <mathlib/mathlib.h>
+#include <systemlib/err.h>
+#include <ecl/ecl.h>
+
+ECL_WheelController::ECL_WheelController() :
+    ECL_Controller("wheel")
+{
+}
+
+ECL_WheelController::~ECL_WheelController()
+{
+}
+
+float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_data)
+{
+    /* Do not calculate control signal with bad inputs */
+    if (!(PX4_ISFINITE(ctl_data.yaw_rate) &&
+          PX4_ISFINITE(ctl_data.groundspeed) &&
+          PX4_ISFINITE(ctl_data.groundspeed_scaler))) {
+        perf_count(_nonfinite_input_perf);
+        return math::constrain(_last_output, -1.0f, 1.0f);
+    }
+
+    /* get the usual dt estimate */
+    uint64_t dt_micros = ecl_elapsed_time(&_last_run);
+    _last_run = ecl_absolute_time();
+    float dt = (float)dt_micros * 1e-6f;
+
+    /* lock integral for long intervals */
+    bool lock_integrator = ctl_data.lock_integrator;
+
+    if (dt_micros > 500000) {
+        lock_integrator = true;
+    }
+
+    /* input conditioning */
+    float min_speed = 1.0f;
+
+    /* Calculate body angular rate error */
+    _rate_error = _rate_setpoint - ctl_data.yaw_rate; //body angular rate error
+
+    if (!lock_integrator && _k_i > 0.0f && ctl_data.groundspeed > min_speed) {
+
+        float id = _rate_error * dt * ctl_data.groundspeed_scaler;
+
+        /*
+         * anti-windup: do not allow integrator to increase if actuator is at limit
+         */
+        if (_last_output < -1.0f) {
+            /* only allow motion to center: increase value */
+            id = math::max(id, 0.0f);
+
+        } else if (_last_output > 1.0f) {
+            /* only allow motion to center: decrease value */
+            id = math::min(id, 0.0f);
+        }
+
+        _integrator += id;
+    }
+
+    /* integrator limit */
+    //xxx: until start detection is available: integral part in control signal is limited here
+    float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
+
+    /* Apply PI rate controller and store non-limited output */
+    _last_output = _rate_setpoint * _k_ff * ctl_data.groundspeed_scaler +
+            _rate_error * _k_p * ctl_data.groundspeed_scaler * ctl_data.groundspeed_scaler +
+            integrator_constrained;
+    /*warnx("wheel: _last_output: %.4f, _integrator: %.4f, scaler %.4f",
+            (double)_last_output, (double)_integrator, (double)ctl_data.groundspeed_scaler);*/
+
+
+    return math::constrain(_last_output, -1.0f, 1.0f);
+}
+
+
+float ECL_WheelController::control_attitude(const struct ECL_ControlData &ctl_data)
+{
+    /* Do not calculate control signal with bad inputs */
+    if (!(PX4_ISFINITE(ctl_data.yaw_setpoint) &&
+          PX4_ISFINITE(ctl_data.yaw))) {
+        perf_count(_nonfinite_input_perf);
+        warnx("not controlling wheel");
+        return _rate_setpoint;
+    }
+
+    /* Calculate the error */
+    float yaw_error = ctl_data.yaw_setpoint - ctl_data.yaw;
+    /* shortest angle (wrap around) */
+    yaw_error = (float)fmod((float)fmod((yaw_error + M_PI_F), M_TWOPI_F) + M_TWOPI_F, M_TWOPI_F) - M_PI_F;
+    /*warnx("yaw_error: %.4f", (double)yaw_error);*/
+
+    /*  Apply P controller: rate setpoint from current error and time constant */
+    _rate_setpoint =  yaw_error / _tc;
+
+    /* limit the rate */
+    if (_max_rate > 0.01f) {
+        if (_rate_setpoint > 0.0f) {
+            _rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
+
+        } else {
+            _rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;
+        }
+
+    }
+
+    return _rate_setpoint;
+}

src/lib/ecl/attitude_fw/ecl_wheel_controller.h

@@ -0,0 +1,70 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name ECL 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 ecl_wheel_controller.h
+ * Definition of a simple orthogonal coordinated turn yaw PID controller.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ * @author Andreas Antener <andreas@uaventure.com>
+ *
+ * Acknowledgements:
+ *
+ *   The control design is based on a design
+ *   by Paul Riseborough and Andrew Tridgell, 2013,
+ *   which in turn is based on initial work of
+ *   Jonathan Challinger, 2012.
+ */
+#ifndef ECL_HEADING_CONTROLLER_H
+#define ECL_HEADING_CONTROLLER_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "ecl_controller.h"
+
+class __EXPORT ECL_WheelController :
+    public ECL_Controller
+{
+public:
+    ECL_WheelController();
+
+    ~ECL_WheelController();
+
+    float control_attitude(const struct ECL_ControlData &ctl_data);
+
+    float control_bodyrate(const struct ECL_ControlData &ctl_data);
+};
+
+#endif // ECL_HEADING_CONTROLLER_H

src/lib/ecl/attitude_fw/ecl_yaw_controller.h

@@ -76,14 +76,15 @@ public:
 		_coordinated_method = coordinated_method;
 	}
 
-protected:
-	float _coordinated_min_speed;
-
 	enum {
 		COORD_METHOD_OPEN = 0,
-		COORD_METHOD_CLOSEACC = 1,
+		COORD_METHOD_CLOSEACC = 1
 	};
 
+protected:
+	float _coordinated_min_speed;
+	float _max_rate;
+
 	int32_t _coordinated_method;
 
 	float control_bodyrate_impl(const struct ECL_ControlData &ctl_data);

src/lib/ecl/validation/data_validator.cpp

@@ -43,6 +43,7 @@
 #include <ecl/ecl.h>
 
 DataValidator::DataValidator(DataValidator *prev_sibling) :
+	_error_mask(ERROR_FLAG_NO_ERROR),
 	_time_last(0),
 	_timeout_interval(20000),
 	_event_count(0),
@@ -111,39 +112,45 @@ DataValidator::put(uint64_t timestamp, float val[3], uint64_t error_count_in, in
 float
 DataValidator::confidence(uint64_t timestamp)
 {
+	float ret = 1.0f;
+	
 	/* check if we have any data */
 	if (_time_last == 0) {
-		return 0.0f;
+		_error_mask |= ERROR_FLAG_NO_DATA;
+		ret = 0.0f;
 	}
-
-	/* check error count limit */
-	if (_error_count > NORETURN_ERRCOUNT) {
-		return 0.0f;
+	
+	/* timed out - that's it */
+	if (timestamp - _time_last > _timeout_interval) {
+		_error_mask |= ERROR_FLAG_TIMEOUT;
+		ret = 0.0f;
 	}
 
 	/* we got the exact same sensor value N times in a row */
 	if (_value_equal_count > VALUE_EQUAL_COUNT_MAX) {
-		return 0.0f;
+		_error_mask |= ERROR_FLAG_STALE_DATA;
+		ret = 0.0f;
 	}
-
-	/* timed out - that's it */
-	if (timestamp - _time_last > _timeout_interval) {
-		return 0.0f;
+	
+	/* check error count limit */
+	if (_error_count > NORETURN_ERRCOUNT) {
+		_error_mask |= ERROR_FLAG_HIGH_ERRCOUNT;
+		ret = 0.0f;
 	}
 
 	/* cap error density counter at window size */
 	if (_error_density > ERROR_DENSITY_WINDOW) {
+		_error_mask |= ERROR_FLAG_HIGH_ERRDENSITY;
 		_error_density = ERROR_DENSITY_WINDOW;
 	}
-
-	/* return local error density for last N measurements */
-	return 1.0f - (_error_density / ERROR_DENSITY_WINDOW);
-}
-
-int
-DataValidator::priority()
-{
-	return _priority;
+	
+	/* no critical errors */
+	if(ret > 0.0f) {
+		/* return local error density for last N measurements */
+		ret = 1.0f - (_error_density / ERROR_DENSITY_WINDOW);
+	}
+	
+	return ret;
 }
 
 void

src/lib/ecl/validation/data_validator.h

@@ -91,7 +91,18 @@ public:
 	 * Get the priority of this validator
 	 * @return		the stored priority
 	 */
-	int			priority();
+	int			priority() { return (_priority); }
+	
+	/**
+	 * Get the error state of this validator
+	 * @return		the bitmask with the error status
+	 */
+	uint32_t		state() { return (_error_mask); }
+	
+	/**
+	 * Reset the error state of this validator
+	 */
+	void			reset_state() { _error_mask = ERROR_FLAG_NO_ERROR; }
 
 	/**
 	 * Get the RMS values of this validator
@@ -111,9 +122,20 @@ public:
 	 * @param timeout_interval_us The timeout interval in microseconds
 	 */
 	void			set_timeout(uint64_t timeout_interval_us) { _timeout_interval = timeout_interval_us; }
+	
+	/**
+	 * Data validator error states 
+	 */
+	static constexpr uint32_t ERROR_FLAG_NO_ERROR      	= (0x00000000U);
+	static constexpr uint32_t ERROR_FLAG_NO_DATA       	= (0x00000001U);
+	static constexpr uint32_t ERROR_FLAG_STALE_DATA    	= (0x00000001U << 1);
+	static constexpr uint32_t ERROR_FLAG_TIMEOUT 	   	= (0x00000001U << 2);
+	static constexpr uint32_t ERROR_FLAG_HIGH_ERRCOUNT 	= (0x00000001U << 3);
+	static constexpr uint32_t ERROR_FLAG_HIGH_ERRDENSITY 	= (0x00000001U << 4);
 
 private:
 	static const unsigned _dimensions = 3;
+	uint32_t _error_mask;			/**< sensor error state */
 	uint64_t _time_last;			/**< last timestamp */
 	uint64_t _timeout_interval;		/**< interval in which the datastream times out in us */
 	uint64_t _event_count;			/**< total data counter */

src/lib/ecl/validation/data_validator_group.cpp

@@ -138,11 +138,13 @@ DataValidatorGroup::get_best(uint64_t timestamp, int *index)
 
 		bool true_failsafe = true;
 
-		/* check wether the switch was a failsafe or preferring a higher priority sensor */
+		/* check whether the switch was a failsafe or preferring a higher priority sensor */
 		if (pre_check_prio != -1 && pre_check_prio < max_priority &&
 			fabsf(pre_check_confidence - max_confidence) < 0.1f) {
 			/* this is not a failover */
 			true_failsafe = false;
+			/* reset error flags, this is likely a hotplug sensor coming online late */
+			best->reset_state();
 		}
 
 		/* if we're no initialized, initialize the bookkeeping but do not count a failsafe */
@@ -199,17 +201,25 @@ void
 DataValidatorGroup::print()
 {
 	/* print the group's state */
-	ECL_INFO("validator: best: %d, prev best: %d, failsafe: %s (# %u)",
+	ECL_INFO("validator: best: %d, prev best: %d, failsafe: %s (%u events)",
 		_curr_best, _prev_best, (_toggle_count > 0) ? "YES" : "NO",
 		_toggle_count);
 
-
 	DataValidator *next = _first;
 	unsigned i = 0;
 
 	while (next != nullptr) {
 		if (next->used()) {
-			ECL_INFO("sensor #%u, prio: %d", i, next->priority());
+			uint32_t flags = next->state();
+			
+			ECL_INFO("sensor #%u, prio: %d, state:%s%s%s%s%s%s", i, next->priority(),
+			((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " NO_DATA" : ""),
+			((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " STALE_DATA" : ""),
+			((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " DATA_TIMEOUT" : ""),
+			((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " HIGH_ERRCOUNT" : ""),
+			((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " HIGH_ERRDENSITY" : ""),
+			((flags == DataValidator::ERROR_FLAG_NO_ERROR) ? " OK" : ""));
+			
 			next->print();
 		}
 		next = next->sibling();
@@ -222,3 +232,35 @@ DataValidatorGroup::failover_count()
 {
 	return _toggle_count;
 }
+
+int
+DataValidatorGroup::failover_index()
+{
+	DataValidator *next = _first;
+	unsigned i = 0;
+	
+	while (next != nullptr) {
+		if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && (i == (unsigned)_prev_best)) {
+			return i;
+		}
+		next = next->sibling();
+		i++;
+	}
+	return -1;
+}
+
+uint32_t
+DataValidatorGroup::failover_state()
+{	
+	DataValidator *next = _first;
+	unsigned i = 0;
+	
+	while (next != nullptr) {
+		if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && (i == (unsigned)_prev_best)) {
+			return next->state();
+		}
+		next = next->sibling();
+		i++;
+	}
+	return DataValidator::ERROR_FLAG_NO_ERROR;
+}

src/lib/ecl/validation/data_validator_group.h

@@ -80,6 +80,20 @@ public:
 	 * @return		the number of failovers
 	 */
 	unsigned		failover_count();
+	
+	/**
+	 * Get the index of the failed sensor in the group
+	 *
+	 * @return		index of the failed sensor
+	 */
+	int			failover_index();
+	
+	/**
+	 * Get the error state of the failed sensor in the group
+	 *
+	 * @return		bitmask with erro states of the failed sensor
+	 */
+	uint32_t		failover_state();
 
 	/**
 	 * Print the validator value

src/lib/geo/geo.c

@@ -298,6 +298,41 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou
 	return CONSTANTS_RADIUS_OF_EARTH * c;
 }
 
+__EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist,
+		double *lat_target, double *lon_target)
+{
+	if (fabsf(dist) < FLT_EPSILON) {
+		*lat_target = lat_A;
+		*lon_target = lon_A;
+
+	} else if (dist >= FLT_EPSILON) {
+		float heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B);
+		waypoint_from_heading_and_distance(lat_A, lon_A, heading, dist, lat_target, lon_target);
+
+	} else {
+		float heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B);
+		heading = _wrap_2pi(heading + M_PI_F);
+		waypoint_from_heading_and_distance(lat_A, lon_A, heading, dist, lat_target, lon_target);
+	}
+}
+
+__EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist,
+		double *lat_target, double *lon_target)
+{
+	bearing = _wrap_2pi(bearing);
+	double radius_ratio = (double)(fabs(dist) / CONSTANTS_RADIUS_OF_EARTH);
+
+	double lat_start_rad = lat_start * M_DEG_TO_RAD;
+	double lon_start_rad = lon_start * M_DEG_TO_RAD;
+
+	*lat_target = asin(sin(lat_start_rad) * cos(radius_ratio) + cos(lat_start_rad) * sin(radius_ratio) * cos((
+				   double)bearing));
+	*lon_target = lon_start_rad + atan2(sin((double)bearing) * sin(radius_ratio) * cos(lat_start_rad),
+					    cos(radius_ratio) - sin(lat_start_rad) * sin(*lat_target));
+
+	*lat_target *= M_RAD_TO_DEG;
+	*lon_target *= M_RAD_TO_DEG;
+}
 __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
 {
 	double lat_now_rad = lat_now * M_DEG_TO_RAD;

src/lib/geo/geo.h

@@ -236,6 +236,36 @@ __EXPORT int globallocalconverter_getref(double *lat_0, double *lon_0, float *al
  */
 __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
 
+
+/**
+ * Creates a new waypoint C on the line of two given waypoints (A, B) at certain distance
+ * from waypoint A
+ *
+ * @param lat_A waypoint A latitude in degrees (47.1234567°, not 471234567°)
+ * @param lon_A waypoint A longitude in degrees (8.1234567°, not 81234567°)
+ * @param lat_B waypoint B latitude in degrees (47.1234567°, not 471234567°)
+ * @param lon_B waypoint B longitude in degrees (8.1234567°, not 81234567°)
+ * @param dist distance of target waypoint from waypoint A in meters (can be negative)
+ * @param lat_target latitude of target waypoint C in degrees (47.1234567°, not 471234567°)
+ * @param lon_target longitude of target waypoint C in degrees (47.1234567°, not 471234567°)
+ */
+__EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist,
+		double *lat_target, double *lon_target);
+
+/**
+ * Creates a waypoint from given waypoint, distance and bearing
+ * see http://www.movable-type.co.uk/scripts/latlong.html
+ *
+ * @param lat_start latitude of starting waypoint in degrees (47.1234567°, not 471234567°)
+ * @param lon_start longitude of starting waypoint in degrees (8.1234567°, not 81234567°)
+ * @param bearing in rad
+ * @param distance in meters
+ * @param lat_target latitude of target waypoint in degrees (47.1234567°, not 471234567°)
+ * @param lon_target longitude of target waypoint in degrees (47.1234567°, not 471234567°)
+ */
+__EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist,
+		double *lat_target, double *lon_target);
+
 /**
  * Returns the bearing to the next waypoint in radians.
  *

src/lib/runway_takeoff/CMakeLists.txt

@@ -0,0 +1,43 @@
+############################################################################
+#
+#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+px4_add_module(
+	MODULE lib__runway_takeoff
+	COMPILE_FLAGS
+		-Os
+	SRCS
+		RunwayTakeoff.cpp
+		runway_takeoff_params.c
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 

src/lib/runway_takeoff/RunwayTakeoff.cpp

@@ -0,0 +1,286 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @file RunwayTakeoff.cpp
+ * Runway takeoff handling for fixed-wing UAVs with steerable wheels.
+ *
+ * @author Roman Bapst <roman@px4.io>
+ * @author Andreas Antener <andreas@uaventure.com>
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <math.h>
+
+#include "RunwayTakeoff.h"
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+#include <mavlink/mavlink_log.h>
+#include <mathlib/mathlib.h>
+
+namespace runwaytakeoff
+{
+
+RunwayTakeoff::RunwayTakeoff() :
+	SuperBlock(NULL, "RWTO"),
+	_state(),
+	_initialized(false),
+	_initialized_time(0),
+	_init_yaw(0),
+	_climbout(false),
+	_throttle_ramp_time(2 * 1e6),
+	_start_wp(),
+	_runway_takeoff_enabled(this, "TKOFF"),
+	_heading_mode(this, "HDG"),
+	_nav_alt(this, "NAV_ALT"),
+	_takeoff_throttle(this, "MAX_THR"),
+	_runway_pitch_sp(this, "PSP"),
+	_max_takeoff_pitch(this, "MAX_PITCH"),
+	_max_takeoff_roll(this, "MAX_ROLL"),
+	_min_airspeed_scaling(this, "AIRSPD_SCL"),
+	_airspeed_min(this, "FW_AIRSPD_MIN", false),
+	_climbout_diff(this, "FW_CLMBOUT_DIFF", false)
+{
+
+	updateParams();
+}
+
+RunwayTakeoff::~RunwayTakeoff()
+{
+
+}
+
+void RunwayTakeoff::init(float yaw, double current_lat, double current_lon)
+{
+	_init_yaw = yaw;
+	_initialized = true;
+	_state = RunwayTakeoffState::THROTTLE_RAMP;
+	_initialized_time = hrt_absolute_time();
+	_climbout = true; // this is true until climbout is finished
+	_start_wp(0) = (float)current_lat;
+	_start_wp(1) = (float)current_lon;
+}
+
+void RunwayTakeoff::update(float airspeed, float alt_agl,
+			   double current_lat, double current_lon, int mavlink_fd)
+{
+
+	switch (_state) {
+	case RunwayTakeoffState::THROTTLE_RAMP:
+		if (hrt_elapsed_time(&_initialized_time) > _throttle_ramp_time) {
+			_state = RunwayTakeoffState::CLAMPED_TO_RUNWAY;
+		}
+
+		break;
+
+	case RunwayTakeoffState::CLAMPED_TO_RUNWAY:
+		if (airspeed > _airspeed_min.get() * _min_airspeed_scaling.get()) {
+			_state = RunwayTakeoffState::TAKEOFF;
+			mavlink_log_info(mavlink_fd, "#Takeoff airspeed reached");
+		}
+
+		break;
+
+	case RunwayTakeoffState::TAKEOFF:
+		if (alt_agl > _nav_alt.get()) {
+			_state = RunwayTakeoffState::CLIMBOUT;
+
+			/*
+			 * If we started in heading hold mode, move the navigation start WP to the current location now.
+			 * The navigator will take this as starting point to navigate towards the takeoff WP.
+			 */
+			if (_heading_mode.get() == 0) {
+				_start_wp(0) = (float)current_lat;
+				_start_wp(1) = (float)current_lon;
+			}
+
+			mavlink_log_info(mavlink_fd, "#Climbout");
+		}
+
+		break;
+
+	case RunwayTakeoffState::CLIMBOUT:
+		if (alt_agl > _climbout_diff.get()) {
+			_climbout = false;
+			_state = RunwayTakeoffState::FLY;
+			mavlink_log_info(mavlink_fd, "#Navigating to waypoint");
+		}
+
+		break;
+
+	default:
+		break;
+	}
+}
+
+/*
+ * Returns true as long as we're below navigation altitude
+ */
+bool RunwayTakeoff::controlYaw()
+{
+	// keep controlling yaw directly until we start navigation
+	return _state < RunwayTakeoffState::CLIMBOUT;
+}
+
+/*
+ * Returns pitch setpoint to use.
+ *
+ * Limited (parameter) as long as the plane is on runway. Otherwise
+ * use the one from TECS
+ */
+float RunwayTakeoff::getPitch(float tecsPitch)
+{
+	if (_state <= RunwayTakeoffState::CLAMPED_TO_RUNWAY) {
+		return math::radians(_runway_pitch_sp.get());
+	}
+
+	return tecsPitch;
+}
+
+/*
+ * Returns the roll setpoint to use.
+ */
+float RunwayTakeoff::getRoll(float navigatorRoll)
+{
+	// until we have enough ground clearance, set roll to 0
+	if (_state < RunwayTakeoffState::CLIMBOUT) {
+		return 0.0f;
+	}
+
+	// allow some roll during climbout
+	else if (_state < RunwayTakeoffState::FLY) {
+		return math::constrain(navigatorRoll,
+				       math::radians(-_max_takeoff_roll.get()),
+				       math::radians(_max_takeoff_roll.get()));
+	}
+
+	return navigatorRoll;
+}
+
+/*
+ * Returns the yaw setpoint to use.
+ *
+ * In heading hold mode (_heading_mode == 0), it returns initial yaw as long as it's on the
+ * runway. When it has enough ground clearance we start navigation towards WP.
+ */
+float RunwayTakeoff::getYaw(float navigatorYaw)
+{
+	if (_heading_mode.get() == 0 && _state < RunwayTakeoffState::CLIMBOUT) {
+		return _init_yaw;
+
+	} else {
+		return navigatorYaw;
+	}
+}
+
+/*
+ * Returns the throttle setpoint to use.
+ *
+ * Ramps up in the beginning, until it lifts off the runway it is set to
+ * parameter value, then it returns the TECS throttle.
+ */
+float RunwayTakeoff::getThrottle(float tecsThrottle)
+{
+	switch (_state) {
+	case RunwayTakeoffState::THROTTLE_RAMP: {
+			float throttle = hrt_elapsed_time(&_initialized_time) / (float)_throttle_ramp_time *
+					 _takeoff_throttle.get();
+			return throttle < _takeoff_throttle.get() ?
+			       throttle :
+			       _takeoff_throttle.get();
+		}
+
+	case RunwayTakeoffState::CLAMPED_TO_RUNWAY:
+		return _takeoff_throttle.get();
+
+	default:
+		return tecsThrottle;
+	}
+}
+
+bool RunwayTakeoff::resetIntegrators()
+{
+	// reset integrators if we're still on runway
+	return _state < RunwayTakeoffState::TAKEOFF;
+}
+
+/*
+ * Returns the minimum pitch for TECS to use.
+ *
+ * In climbout we either want what was set on the waypoint (sp_min) but at least
+ * the climbtout minimum pitch (parameter).
+ * Otherwise use the minimum that is enforced generally (parameter).
+ */
+float RunwayTakeoff::getMinPitch(float sp_min, float climbout_min, float min)
+{
+	if (_state < RunwayTakeoffState::FLY) {
+		return math::max(sp_min, climbout_min);
+	}
+
+	else {
+		return min;
+	}
+}
+
+/*
+ * Returns the maximum pitch for TECS to use.
+ *
+ * Limited by parameter (if set) until climbout is done.
+ */
+float RunwayTakeoff::getMaxPitch(float max)
+{
+	// use max pitch from parameter if set (> 0.1)
+	if (_state < RunwayTakeoffState::FLY && _max_takeoff_pitch.get() > 0.1f) {
+		return _max_takeoff_pitch.get();
+	}
+
+	else {
+		return max;
+	}
+}
+
+/*
+ * Returns the "previous" (start) WP for navigation.
+ */
+math::Vector<2> RunwayTakeoff::getStartWP()
+{
+	return _start_wp;
+}
+
+void RunwayTakeoff::reset()
+{
+	_initialized = false;
+	_state = RunwayTakeoffState::THROTTLE_RAMP;
+}
+
+}

src/lib/runway_takeoff/RunwayTakeoff.h

@@ -0,0 +1,121 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @file RunwayTakeoff.h
+ * Runway takeoff handling for fixed-wing UAVs with steerable wheels.
+ *
+ * @author Roman Bapst <roman@px4.io>
+ * @author Andreas Antener <andreas@uaventure.com>
+ */
+
+#ifndef RUNWAYTAKEOFF_H
+#define RUNWAYTAKEOFF_H
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <math.h>
+
+#include <drivers/drv_hrt.h>
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+#include <mavlink/mavlink_log.h>
+#include <mathlib/mathlib.h>
+
+namespace runwaytakeoff
+{
+
+enum RunwayTakeoffState {
+	THROTTLE_RAMP = 0, /**< ramping up throttle */
+	CLAMPED_TO_RUNWAY = 1, /**< clamped to runway, controlling yaw directly (wheel or rudder) */
+	TAKEOFF = 2, /**< taking off, get ground clearance, roll 0 */
+	CLIMBOUT = 3, /**< climbout to safe height before navigation, roll limited */
+	FLY = 4 /**< fly towards takeoff waypoint */
+};
+
+class __EXPORT RunwayTakeoff : public control::SuperBlock
+{
+public:
+	RunwayTakeoff();
+	~RunwayTakeoff();
+
+	void init(float yaw, double current_lat, double current_lon);
+	void update(float airspeed, float alt_agl, double current_lat, double current_lon, int mavlink_fd);
+
+	RunwayTakeoffState getState() { return _state; };
+	bool isInitialized() { return _initialized; };
+
+	bool runwayTakeoffEnabled() { return (bool)_runway_takeoff_enabled.get(); };
+	float getMinAirspeedScaling() { return _min_airspeed_scaling.get(); };
+	float getInitYaw() { return _init_yaw; };
+
+	bool controlYaw();
+	bool climbout() { return _climbout; };
+	float getPitch(float tecsPitch);
+	float getRoll(float navigatorRoll);
+	float getYaw(float navigatorYaw);
+	float getThrottle(float tecsThrottle);
+	bool resetIntegrators();
+	float getMinPitch(float sp_min, float climbout_min, float min);
+	float getMaxPitch(float max);
+	math::Vector<2> getStartWP();
+
+	void reset();
+
+protected:
+private:
+	/** state variables **/
+	RunwayTakeoffState _state;
+	bool _initialized;
+	hrt_abstime _initialized_time;
+	float _init_yaw;
+	bool _climbout;
+	unsigned _throttle_ramp_time;
+	math::Vector<2> _start_wp;
+
+	/** parameters **/
+	control::BlockParamInt _runway_takeoff_enabled;
+	control::BlockParamInt _heading_mode;
+	control::BlockParamFloat _nav_alt;
+	control::BlockParamFloat _takeoff_throttle;
+	control::BlockParamFloat _runway_pitch_sp;
+	control::BlockParamFloat _max_takeoff_pitch;
+	control::BlockParamFloat _max_takeoff_roll;
+	control::BlockParamFloat _min_airspeed_scaling;
+	control::BlockParamFloat _airspeed_min;
+	control::BlockParamFloat _climbout_diff;
+
+};
+
+}
+
+#endif // RUNWAYTAKEOFF_H

src/lib/runway_takeoff/runway_takeoff_params.c

@@ -0,0 +1,137 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file runway_takeoff_params.c
+ *
+ * Parameters for runway takeoff
+ *
+ * @author Andreas Antener <andreas@uaventure.com>
+ */
+
+#include <px4_config.h>
+
+#include <systemlib/param/param.h>
+
+/**
+ * Enable or disable runway takeoff with landing gear
+ *
+ * 0: disabled, 1: enabled
+ *
+ * @min 0
+ * @max 1
+ * @group Runway Takeoff
+ */
+PARAM_DEFINE_INT32(RWTO_TKOFF, 0);
+
+/**
+ * Specifies which heading should be held during runnway takeoff.
+ *
+ * 0: airframe heading, 1: heading towards takeoff waypoint
+ *
+ * @min 0
+ * @max 1
+ * @group Runway Takeoff
+ */
+PARAM_DEFINE_INT32(RWTO_HDG, 0);
+
+/**
+ * Altitude AGL at which we have enough ground clearance to allow some roll.
+ * Until RWTO_NAV_ALT is reached the plane is held level and only
+ * rudder is used to keep the heading (see RWTO_HDG). This should be below
+ * FW_CLMBOUT_DIFF if FW_CLMBOUT_DIFF > 0.
+ *
+ * @unit m
+ * @min 0.0
+ * @max 100.0
+ * @group Runway Takeoff
+ */
+PARAM_DEFINE_FLOAT(RWTO_NAV_ALT, 5.0);
+
+/**
+ * Max throttle during runway takeoff.
+ * (Can be used to test taxi on runway)
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group Runway Takeoff
+ */
+PARAM_DEFINE_FLOAT(RWTO_MAX_THR, 1.0);
+
+/**
+ * Pitch setpoint during taxi / before takeoff airspeed is reached.
+ * A taildragger with stearable wheel might need to pitch up
+ * a little to keep it's wheel on the ground before airspeed
+ * to takeoff is reached.
+ *
+ * @unit deg
+ * @min 0.0
+ * @max 20.0
+ * @group Runway Takeoff
+ */
+PARAM_DEFINE_FLOAT(RWTO_PSP, 0.0);
+
+/**
+ * Max pitch during takeoff.
+ * Fixed-wing settings are used if set to 0. Note that there is also a minimum
+ * pitch of 10 degrees during takeoff, so this must be larger if set.
+ *
+ * @unit deg
+ * @min 0.0
+ * @max 60.0
+ * @group Runway Takeoff
+ */
+PARAM_DEFINE_FLOAT(RWTO_MAX_PITCH, 20.0);
+
+/**
+ * Max roll during climbout.
+ * Roll is limited during climbout to ensure enough lift and prevents aggressive
+ * navigation before we're on a safe height.
+ *
+ * @unit deg
+ * @min 0.0
+ * @max 60.0
+ * @group Runway Takeoff
+ */
+PARAM_DEFINE_FLOAT(RWTO_MAX_ROLL, 25.0);
+
+/**
+ * Min. airspeed scaling factor for takeoff.
+ * Pitch up will be commanded when the following airspeed is reached:
+ * FW_AIRSPD_MIN * RWTO_AIRSPD_SCL
+ *
+ * @min 0.0
+ * @max 2.0
+ * @group Runway Takeoff
+ */
+PARAM_DEFINE_FLOAT(RWTO_AIRSPD_SCL, 1.3);

src/lib/terrain_estimation/CMakeLists.txt

@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+px4_add_module(
+	MODULE lib__terrain_estimation
+	COMPILE_FLAGS
+		-Os
+	SRCS
+		terrain_estimator.cpp
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 

src/lib/terrain_estimation/terrain_estimator.cpp

@@ -0,0 +1,201 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 Roman Bapst. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file terrain_estimator.cpp
+ * A terrain estimation kalman filter.
+ */
+
+#include "terrain_estimator.h"
+
+#define DISTANCE_TIMEOUT 100000		// time in usec after which laser is considered dead
+
+TerrainEstimator::TerrainEstimator() :
+	_distance_last(0.0f),
+	_terrain_valid(false),
+	_time_last_distance(0),
+	_time_last_gps(0)
+{
+	memset(&_x._data[0], 0, sizeof(_x._data));
+	_u_z = 0.0f;
+	_P.setIdentity();
+}
+
+bool TerrainEstimator::is_distance_valid(float distance)
+{
+	if (distance > 40.0f || distance < 0.00001f) {
+		return false;
+
+	} else {
+		return true;
+	}
+}
+
+void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitude,
+			       const struct sensor_combined_s *sensor,
+			       const struct distance_sensor_s *distance)
+{
+	if (attitude->R_valid) {
+		matrix::Matrix<float, 3, 3> R_att(attitude->R);
+		matrix::Vector<float, 3> a(&sensor->accelerometer_m_s2[0]);
+		matrix::Vector<float, 3> u;
+		u = R_att * a;
+		_u_z = u(2) + 9.81f; // compensate for gravity
+
+	} else {
+		_u_z = 0.0f;
+	}
+
+	// dynamics matrix
+	matrix::Matrix<float, n_x, n_x> A;
+	A.setZero();
+	A(0, 1) = 1;
+	A(1, 2) = 1;
+
+	// input matrix
+	matrix::Matrix<float, n_x, 1>  B;
+	B.setZero();
+	B(1, 0) = 1;
+
+	// input noise variance
+	float R = 0.135f;
+
+	// process noise convariance
+	matrix::Matrix<float, n_x, n_x>  Q;
+	Q(0, 0) = 0;
+	Q(1, 1) = 0;
+
+	// do prediction
+	matrix::Vector<float, n_x>  dx = (A * _x) * dt;
+	dx(1) += B(1, 0) * _u_z * dt;
+
+	// propagate state and covariance matrix
+	_x += dx;
+	_P += (A * _P + _P * A.transpose() +
+	       B * R * B.transpose() + Q) * dt;
+}
+
+void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicle_gps_position_s *gps,
+		const struct distance_sensor_s *distance,
+		const struct vehicle_attitude_s *attitude)
+{
+	// terrain estimate is invalid if we have range sensor timeout
+	if (time_ref - distance->timestamp > DISTANCE_TIMEOUT) {
+		_terrain_valid = false;
+	}
+
+	if (distance->timestamp > _time_last_distance) {
+
+		float d = distance->current_distance;
+
+		matrix::Matrix<float, 1, n_x> C;
+		C(0, 0) = -1; // measured altitude,
+
+		float R = 0.009f;
+
+		matrix::Vector<float, 1> y;
+		y(0) = d * cosf(attitude->roll) * cosf(attitude->pitch);
+
+		// residual
+		matrix::Matrix<float, 1, 1> S_I = (C * _P * C.transpose());
+		S_I(0, 0) += R;
+		S_I = matrix::inv<float, 1> (S_I);
+		matrix::Vector<float, 1> r = y - C * _x;
+
+		matrix::Matrix<float, n_x, 1> K = _P * C.transpose() * S_I;
+
+		// some sort of outlayer rejection
+		if (fabsf(distance->current_distance - _distance_last) < 1.0f) {
+			_x += K * r;
+			_P -= K * C * _P;
+		}
+
+		// if the current and the last range measurement are bad then we consider the terrain
+		// estimate to be invalid
+		if (!is_distance_valid(distance->current_distance) && !is_distance_valid(_distance_last)) {
+			_terrain_valid = false;
+
+		} else {
+			_terrain_valid = true;
+		}
+
+		_time_last_distance = distance->timestamp;
+		_distance_last = distance->current_distance;
+	}
+
+	if (gps->timestamp_position > _time_last_gps && gps->fix_type >= 3) {
+		matrix::Matrix<float, 1, n_x> C;
+		C(0, 1) = 1;
+
+		float R = 0.056f;
+
+		matrix::Vector<float, 1> y;
+		y(0) = gps->vel_d_m_s;
+
+		// residual
+		matrix::Matrix<float, 1, 1> S_I = (C * _P * C.transpose());
+		S_I(0, 0) += R;
+		S_I = matrix::inv<float, 1>(S_I);
+		matrix::Vector<float, 1> r = y - C * _x;
+
+		matrix::Matrix<float, n_x, 1> K = _P * C.transpose() * S_I;
+		_x += K * r;
+		_P -= K * C * _P;
+
+		_time_last_gps = gps->timestamp_position;
+	}
+
+	// reinitialise filter if we find bad data
+	bool reinit = false;
+
+	for (int i = 0; i < n_x; i++) {
+		if (!PX4_ISFINITE(_x(i))) {
+			reinit = true;
+		}
+	}
+
+	for (int i = 0; i < n_x; i++) {
+		for (int j = 0; j < n_x; j++) {
+			if (!PX4_ISFINITE(_P(i, j))) {
+				reinit = true;
+			}
+		}
+	}
+
+	if (reinit) {
+		memset(&_x._data[0], 0, sizeof(_x._data));
+		_P.setZero();
+		_P(0, 0) = _P(1, 1) = _P(2, 2) = 0.1f;
+	}
+
+}

src/lib/terrain_estimation/terrain_estimator.h

@@ -0,0 +1,100 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 Roman Bapst. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file terrain_estimator.h
+ */
+
+#include <lib/mathlib/mathlib.h>
+#include "matrix/Matrix.hpp"
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/distance_sensor.h>
+
+
+/*
+* This class can be used to estimate distance to the ground using a laser range finder.
+* It's assumed that the laser points down vertically if the vehicle is in it's neutral pose.
+* The predict(...) function will do a state prediciton based on accelerometer inputs. It also
+* considers accelerometer bias.
+* The measurement_update(...) function does a measurement update based on range finder and gps
+* velocity measurements. Both functions should always be called together when there is new
+* acceleration data available.
+* The is_valid() function provides information whether the estimate is valid.
+*/
+
+class __EXPORT TerrainEstimator
+{
+public:
+	TerrainEstimator();
+	~TerrainEstimator() {};
+
+	bool is_valid() {return _terrain_valid;}
+	float get_distance_to_ground() {return -_x(0);}
+	float get_velocity() {return _x(1);};
+
+	void predict(float dt, const struct vehicle_attitude_s *attitude, const struct sensor_combined_s *sensor,
+		     const struct distance_sensor_s *distance);
+	void measurement_update(uint64_t time_ref, const struct vehicle_gps_position_s *gps,
+				const struct distance_sensor_s *distance,
+				const struct vehicle_attitude_s *attitude);
+
+private:
+	enum {n_x = 3};
+
+	float _distance_last;
+	bool _terrain_valid;
+
+	// kalman filter variables
+	matrix::Vector<float, n_x> _x;		// state: ground distance, velocity, accel bias in z direction
+	float  _u_z;			// acceleration in earth z direction
+	matrix::Matrix<float, 3, 3> _P;	// covariance matrix
+
+	// timestamps
+	uint64_t _time_last_distance;
+	uint64_t _time_last_gps;
+
+	/*
+	struct {
+		float var_acc_z;
+		float var_p_z;
+		float var_p_vz;
+		float var_lidar;
+		float var_gps_vz;
+	} _params;
+	*/
+
+	bool is_distance_valid(float distance);
+
+};

src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp

@@ -348,10 +348,17 @@ void AttitudeEstimatorQ::task_main()
 					_voter_gyro.put(i, sensors.gyro_timestamp[i], &gyro[0], sensors.gyro_errcount[i], sensors.gyro_priority[i]);
 				}
 
-				_voter_accel.put(i, sensors.accelerometer_timestamp[i], &sensors.accelerometer_m_s2[i * 3],
-						 sensors.accelerometer_errcount[i], sensors.accelerometer_priority[i]);
-				_voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3],
-					       sensors.magnetometer_errcount[i], sensors.magnetometer_priority[i]);
+				/* ignore empty fields */
+				if (sensors.accelerometer_timestamp[i] > 0) {
+					_voter_accel.put(i, sensors.accelerometer_timestamp[i], &sensors.accelerometer_m_s2[i * 3],
+							 sensors.accelerometer_errcount[i], sensors.accelerometer_priority[i]);
+				}
+
+				/* ignore empty fields */
+				if (sensors.magnetometer_timestamp[i] > 0) {
+					_voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3],
+						       sensors.magnetometer_errcount[i], sensors.magnetometer_priority[i]);
+				}
 			}
 
 			int best_gyro, best_accel, best_mag;
@@ -369,12 +376,48 @@ void AttitudeEstimatorQ::task_main()
 
 			_data_good = true;
 
-			if (!_failsafe && (_voter_gyro.failover_count() > 0 ||
-					   _voter_accel.failover_count() > 0 ||
-					   _voter_mag.failover_count() > 0)) {
+			if (!_failsafe) {
+				uint32_t flags = DataValidator::ERROR_FLAG_NO_ERROR;
 
-				_failsafe = true;
-				mavlink_and_console_log_emergency(_mavlink_fd, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY");
+				if (_voter_gyro.failover_count() > 0) {
+					_failsafe = true;
+					flags = _voter_gyro.failover_state();
+					mavlink_and_console_log_emergency(_mavlink_fd, "Gyro #%i failure :%s%s%s%s%s!",
+									  _voter_gyro.failover_index(),
+									  ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
+									  ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""),
+									  ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""),
+									  ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""),
+									  ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : ""));
+				}
+
+				if (_voter_accel.failover_count() > 0) {
+					_failsafe = true;
+					flags = _voter_accel.failover_state();
+					mavlink_and_console_log_emergency(_mavlink_fd, "Accel #%i failure :%s%s%s%s%s!",
+									  _voter_accel.failover_index(),
+									  ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
+									  ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""),
+									  ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""),
+									  ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""),
+									  ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : ""));
+				}
+
+				if (_voter_mag.failover_count() > 0) {
+					_failsafe = true;
+					flags = _voter_mag.failover_state();
+					mavlink_and_console_log_emergency(_mavlink_fd, "Mag #%i failure :%s%s%s%s%s!",
+									  _voter_mag.failover_index(),
+									  ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
+									  ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""),
+									  ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""),
+									  ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""),
+									  ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : ""));
+				}
+
+				if (_failsafe) {
+					mavlink_and_console_log_emergency(_mavlink_fd, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY");
+				}
 			}
 
 			if (!_vibration_warning && (_voter_gyro.get_vibration_factor(curr_time) > _vibration_warning_threshold ||

src/modules/commander/CMakeLists.txt

@@ -37,7 +37,7 @@ endif()
 px4_add_module(
 	MODULE modules__commander
 	MAIN commander
-	STACK 1200
+	STACK 4096
 	COMPILE_FLAGS
 		${MODULE_CFLAGS}
 		-Os

src/modules/commander/PreflightCheck.cpp

@@ -116,7 +116,7 @@ static int check_calibration(DevHandle &h, const char* param_template, int &devi
 	return !calibration_found;
 }
 
-static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id)
+static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id, bool report_fail)
 {
 	bool success = true;
 
@@ -127,8 +127,10 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in
 
 	if (!h.isValid()) {
 		if (!optional) {
-			mavlink_and_console_log_critical(mavlink_fd,
+			if (report_fail) {
+				mavlink_and_console_log_critical(mavlink_fd,
 							 "PREFLIGHT FAIL: NO MAG SENSOR #%u", instance);
+			}
 		}
 
 		return false;
@@ -137,8 +139,10 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in
 	int ret = check_calibration(h, "CAL_MAG%u_ID", device_id);
 
 	if (ret) {
-		mavlink_and_console_log_critical(mavlink_fd,
+		if (report_fail) {
+			mavlink_and_console_log_critical(mavlink_fd,
 						 "PREFLIGHT FAIL: MAG #%u UNCALIBRATED", instance);
+		}
 		success = false;
 		goto out;
 	}
@@ -146,8 +150,10 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in
 	ret = h.ioctl(MAGIOCSELFTEST, 0);
 
 	if (ret != OK) {
-		mavlink_and_console_log_critical(mavlink_fd,
+		if (report_fail) {
+			mavlink_and_console_log_critical(mavlink_fd,
 						 "PREFLIGHT FAIL: MAG #%u SELFTEST FAILED", instance);
+		}
 		success = false;
 		goto out;
 	}
@@ -157,7 +163,7 @@ out:
 	return success;
 }
 
-static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic, int &device_id)
+static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic, int &device_id, bool report_fail)
 {
 	bool success = true;
 
@@ -168,8 +174,10 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
 
 	if (!h.isValid()) {
 		if (!optional) {
-			mavlink_and_console_log_critical(mavlink_fd,
+			if (report_fail) {
+				mavlink_and_console_log_critical(mavlink_fd,
 							 "PREFLIGHT FAIL: NO ACCEL SENSOR #%u", instance);
+			}
 		}
 
 		return false;
@@ -178,8 +186,10 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
 	int ret = check_calibration(h, "CAL_ACC%u_ID", device_id);
 
 	if (ret) {
-		mavlink_and_console_log_critical(mavlink_fd,
+		if (report_fail) {
+			mavlink_and_console_log_critical(mavlink_fd,
 						 "PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED", instance);
+		}
 		success = false;
 		goto out;
 	}
@@ -187,8 +197,10 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
 	ret = h.ioctl(ACCELIOCSELFTEST, 0);
 
 	if (ret != OK) {
-		mavlink_and_console_log_critical(mavlink_fd,
+		if (report_fail) {
+			mavlink_and_console_log_critical(mavlink_fd,
 						 "PREFLIGHT FAIL: ACCEL #%u SELFTEST FAILED", instance);
+		}
 		success = false;
 		goto out;
 	}
@@ -204,13 +216,17 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
 			float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
 
 			if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
-				mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL RANGE, hold still on arming");
+				if (report_fail) {
+					mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL RANGE, hold still on arming");
+				}
 				/* this is frickin' fatal */
 				success = false;
 				goto out;
 			}
 		} else {
-			mavlink_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL READ");
+			if (report_fail) {
+				mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL READ");
+			}
 			/* this is frickin' fatal */
 			success = false;
 			goto out;
@@ -223,7 +239,7 @@ out:
 	return success;
 }
 
-static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id)
+static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id, bool report_fail)
 {
 	bool success = true;
 
@@ -234,8 +250,10 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
 
 	if (!h.isValid()) {
 		if (!optional) {
-			mavlink_and_console_log_critical(mavlink_fd,
+			if (report_fail) {
+				mavlink_and_console_log_critical(mavlink_fd,
 							 "PREFLIGHT FAIL: NO GYRO SENSOR #%u", instance);
+			}
 		}
 
 		return false;
@@ -244,8 +262,10 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
 	int ret = check_calibration(h, "CAL_GYRO%u_ID", device_id);
 
 	if (ret) {
-		mavlink_and_console_log_critical(mavlink_fd,
+		if (report_fail) {
+			mavlink_and_console_log_critical(mavlink_fd,
 						 "PREFLIGHT FAIL: GYRO #%u UNCALIBRATED", instance);
+		}
 		success = false;
 		goto out;
 	}
@@ -253,8 +273,10 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
 	ret = h.ioctl(GYROIOCSELFTEST, 0);
 
 	if (ret != OK) {
-		mavlink_and_console_log_critical(mavlink_fd,
+		if (report_fail) {
+			mavlink_and_console_log_critical(mavlink_fd,
 						 "PREFLIGHT FAIL: GYRO #%u SELFTEST FAILED", instance);
+		}
 		success = false;
 		goto out;
 	}
@@ -264,7 +286,7 @@ out:
 	return success;
 }
 
-static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id)
+static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id, bool report_fail)
 {
 	bool success = true;
 
@@ -275,8 +297,10 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
 
 	if (!h.isValid()) {
 		if (!optional) {
-			mavlink_and_console_log_critical(mavlink_fd,
+			if (report_fail) {
+				mavlink_and_console_log_critical(mavlink_fd,
 							 "PREFLIGHT FAIL: NO BARO SENSOR #%u", instance);
+			}
 		}
 
 		return false;
@@ -300,7 +324,7 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
 	return success;
 }
 
-static bool airspeedCheck(int mavlink_fd, bool optional)
+static bool airspeedCheck(int mavlink_fd, bool optional, bool report_fail)
 {
 	bool success = true;
 	int ret;
@@ -310,13 +334,17 @@ static bool airspeedCheck(int mavlink_fd, bool optional)
 
 	if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) ||
 	    (hrt_elapsed_time(&airspeed.timestamp) > (500 * 1000))) {
-		mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING");
+		if (report_fail) {
+			mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING");
+		}
 		success = false;
 		goto out;
 	}
 
 	if (fabsf(airspeed.indicated_airspeed_m_s) > 6.0f) {
-		mavlink_and_console_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE");
+		if (report_fail) {
+			mavlink_and_console_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE");
+		}
 		// XXX do not make this fatal yet
 	}
 
@@ -325,7 +353,7 @@ out:
 	return success;
 }
 
-static bool gnssCheck(int mavlink_fd)
+static bool gnssCheck(int mavlink_fd, bool report_fail)
 {
 	bool success = true;
 
@@ -347,8 +375,10 @@ static bool gnssCheck(int mavlink_fd)
 	}
 
 	//Report failure to detect module
-	if(!success) {
-		mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GPS RECEIVER MISSING");
+	if (!success) {
+		if (report_fail) {
+			mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GPS RECEIVER MISSING");
+		}
 	}
 
 	px4_close(gpsSub);
@@ -356,7 +386,7 @@ static bool gnssCheck(int mavlink_fd)
 }
 
 bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro,
-		    bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic)
+		    bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic, bool reportFailures)
 {
 	bool failed = false;
 
@@ -371,7 +401,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
 			bool required = (i < max_mandatory_mag_count);
 			int device_id = -1;
 
-			if (!magnometerCheck(mavlink_fd, i, !required, device_id) && required) {
+			if (!magnometerCheck(mavlink_fd, i, !required, device_id, reportFailures) && required) {
 				failed = true;
 			}
 
@@ -382,7 +412,9 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
 
 		/* check if the primary device is present */
 		if (!prime_found && prime_id != 0) {
-			mavlink_log_critical(mavlink_fd, "warning: primary compass not operational");
+			if (reportFailures) {
+				mavlink_and_console_log_critical(mavlink_fd, "Warning: Primary compass not found");
+			}
 			failed = true;
 		}
 	}
@@ -398,7 +430,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
 			bool required = (i < max_mandatory_accel_count);
 			int device_id = -1;
 
-			if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic, device_id) && required) {
+			if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic, device_id, reportFailures) && required) {
 				failed = true;
 			}
 
@@ -409,7 +441,9 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
 
 		/* check if the primary device is present */
 		if (!prime_found && prime_id != 0) {
-			mavlink_log_critical(mavlink_fd, "warning: primary accelerometer not operational");
+			if (reportFailures) {
+				mavlink_and_console_log_critical(mavlink_fd, "Warning: Primary accelerometer not found");
+			}
 			failed = true;
 		}
 	}
@@ -425,7 +459,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
 			bool required = (i < max_mandatory_gyro_count);
 			int device_id = -1;
 
-			if (!gyroCheck(mavlink_fd, i, !required, device_id) && required) {
+			if (!gyroCheck(mavlink_fd, i, !required, device_id, reportFailures) && required) {
 				failed = true;
 			}
 
@@ -436,7 +470,9 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
 
 		/* check if the primary device is present */
 		if (!prime_found && prime_id != 0) {
-			mavlink_log_critical(mavlink_fd, "warning: primary gyro not operational");
+			if (reportFailures) {
+				mavlink_and_console_log_critical(mavlink_fd, "Warning: Primary gyro not found");
+			}
 			failed = true;
 		}
 	}
@@ -452,7 +488,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
 			bool required = (i < max_mandatory_baro_count);
 			int device_id = -1;
 
-			if (!baroCheck(mavlink_fd, i, !required, device_id) && required) {
+			if (!baroCheck(mavlink_fd, i, !required, device_id, reportFailures) && required) {
 				failed = true;
 			}
 
@@ -464,29 +500,33 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
 		// TODO there is no logic in place to calibrate the primary baro yet
 		// // check if the primary device is present 
 		if (!prime_found && prime_id != 0) {
-			mavlink_log_critical(mavlink_fd, "warning: primary barometer not operational");
+			if (reportFailures) {
+				mavlink_and_console_log_critical(mavlink_fd, "warning: primary barometer not operational");
+			}
 			failed = true;
 		}
 	}
 
 	/* ---- AIRSPEED ---- */
 	if (checkAirspeed) {
-		if (!airspeedCheck(mavlink_fd, true)) {
+		if (!airspeedCheck(mavlink_fd, true, reportFailures)) {
 			failed = true;
 		}
 	}
 
 	/* ---- RC CALIBRATION ---- */
 	if (checkRC) {
-		if (rc_calibration_check(mavlink_fd) != OK) {
-			mavlink_log_critical(mavlink_fd, "RC calibration check failed");
+		if (rc_calibration_check(mavlink_fd, reportFailures) != OK) {
+			if (reportFailures) {
+				mavlink_and_console_log_critical(mavlink_fd, "RC calibration check failed");
+			}
 			failed = true;
 		}
 	}
 
 	/* ---- Global Navigation Satellite System receiver ---- */
 	if (checkGNSS) {
-		if(!gnssCheck(mavlink_fd)) {
+		if (!gnssCheck(mavlink_fd, reportFailures)) {
 			failed = true;
 		}
 	}

src/modules/commander/PreflightCheck.h

@@ -67,7 +67,7 @@ namespace Commander
 *   true if the GNSS receiver should be checked
 **/
 bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc,
-    bool checkGyro, bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic = false);
+    bool checkGyro, bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic, bool reportFailures = false);
 
 const unsigned max_mandatory_gyro_count = 1;
 const unsigned max_optional_gyro_count = 3;

src/modules/commander/airspeed_calibration.cpp

@@ -68,7 +68,7 @@ static const char *sensor_name = "dpress";
 static void feedback_calibration_failed(int mavlink_fd)
 {
 	sleep(5);
-	mavlink_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name);
+	mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name);
 }
 
 int do_airspeed_calibration(int mavlink_fd)
@@ -78,7 +78,7 @@ int do_airspeed_calibration(int mavlink_fd)
 	const unsigned maxcount = 2400;
 
 	/* give directions */
-	mavlink_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
+	mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
 
 	const unsigned calibration_count = (maxcount * 2) / 3;
 
@@ -101,7 +101,7 @@ int do_airspeed_calibration(int mavlink_fd)
 			paramreset_successful = true;
 
 		} else {
-			mavlink_log_critical(mavlink_fd, "[cal] airspeed offset zero failed");
+			mavlink_and_console_log_critical(mavlink_fd, "[cal] airspeed offset zero failed");
 		}
 
 		px4_close(fd);
@@ -115,18 +115,18 @@ int do_airspeed_calibration(int mavlink_fd)
 		float analog_scaling = 0.0f;
 		param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling));
 		if (fabsf(analog_scaling) < 0.1f) {
-			mavlink_log_critical(mavlink_fd, "[cal] No airspeed sensor, see http://px4.io/help/aspd");
+			mavlink_and_console_log_critical(mavlink_fd, "[cal] No airspeed sensor, see http://px4.io/help/aspd");
 			goto error_return;
 		}
 
 		/* set scaling offset parameter */
 		if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
-			mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG);
+			mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, 1);
 			goto error_return;
 		}
 	}
 
-	mavlink_log_critical(mavlink_fd, "[cal] Ensure sensor is not measuring wind");
+	mavlink_and_console_log_critical(mavlink_fd, "[cal] Ensure sensor is not measuring wind");
 	usleep(500 * 1000);
 
 	while (calibration_counter < calibration_count) {
@@ -149,7 +149,7 @@ int do_airspeed_calibration(int mavlink_fd)
 			calibration_counter++;
 
 			if (calibration_counter % (calibration_count / 20) == 0) {
-				mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, (calibration_counter * 80) / calibration_count);
+				mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, (calibration_counter * 80) / calibration_count);
 			}
 
 		} else if (poll_ret == 0) {
@@ -167,14 +167,14 @@ int do_airspeed_calibration(int mavlink_fd)
 		airscale.offset_pa = diff_pres_offset;
 		if (fd_scale > 0) {
 			if (OK != px4_ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
-				mavlink_log_critical(mavlink_fd, "[cal] airspeed offset update failed");
+				mavlink_and_console_log_critical(mavlink_fd, "[cal] airspeed offset update failed");
 			}
 
 			px4_close(fd_scale);
 		}
 
 		if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
-			mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG);
+			mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, 1);
 			goto error_return;
 		}
 
@@ -183,7 +183,7 @@ int do_airspeed_calibration(int mavlink_fd)
 
 		if (save_ret != 0) {
 			warn("WARNING: auto-save of params to storage failed");
-			mavlink_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG);
+			mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG);
 			goto error_return;
 		}
 
@@ -192,12 +192,12 @@ int do_airspeed_calibration(int mavlink_fd)
 		goto error_return;
 	}
 
-	mavlink_log_critical(mavlink_fd, "[cal] Offset of %d Pascal", (int)diff_pres_offset);
+	mavlink_and_console_log_critical(mavlink_fd, "[cal] Offset of %d Pascal", (int)diff_pres_offset);
 
 	/* wait 500 ms to ensure parameter propagated through the system */
 	usleep(500 * 1000);
 
-	mavlink_log_critical(mavlink_fd, "[cal] Create airflow now");
+	mavlink_and_console_log_critical(mavlink_fd, "[cal] Create airflow now");
 
 	calibration_counter = 0;
 
@@ -222,7 +222,7 @@ int do_airspeed_calibration(int mavlink_fd)
 
 			if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) {
 				if (calibration_counter % 500 == 0) {
-					mavlink_log_info(mavlink_fd, "[cal] Create air pressure! (got %d, wanted: 50 Pa)",
+					mavlink_and_console_log_info(mavlink_fd, "[cal] Create air pressure! (got %d, wanted: 50 Pa)",
 						(int)diff_pres.differential_pressure_raw_pa);
 				}
 				continue;
@@ -230,26 +230,26 @@ int do_airspeed_calibration(int mavlink_fd)
 
 			/* do not allow negative values */
 			if (diff_pres.differential_pressure_raw_pa < 0.0f) {
-				mavlink_log_info(mavlink_fd, "[cal] Negative pressure difference detected (%d Pa)",
+				mavlink_and_console_log_info(mavlink_fd, "[cal] Negative pressure difference detected (%d Pa)",
 						(int)diff_pres.differential_pressure_raw_pa);
-				mavlink_log_info(mavlink_fd, "[cal] Swap static and dynamic ports!");
+				mavlink_and_console_log_info(mavlink_fd, "[cal] Swap static and dynamic ports!");
 
 				/* the user setup is wrong, wipe the calibration to force a proper re-calibration */
 
 				diff_pres_offset = 0.0f;
 				if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
-					mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG);
+					mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, 1);
 					goto error_return;
 				}
 
 				/* save */
-				mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 0);
+				mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 0);
 				(void)param_save_default();
 
 				feedback_calibration_failed(mavlink_fd);
 				goto error_return;
 			} else {
-				mavlink_log_info(mavlink_fd, "[cal] Positive pressure: OK (%d Pa)",
+				mavlink_and_console_log_info(mavlink_fd, "[cal] Positive pressure: OK (%d Pa)",
 					(int)diff_pres.differential_pressure_raw_pa);
 				break;
 			}
@@ -266,9 +266,9 @@ int do_airspeed_calibration(int mavlink_fd)
 		goto error_return;
 	}
 
-	mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100);
+	mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100);
 
-	mavlink_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name);
+	mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name);
 	tune_neutral(true);
 
 normal_return:

src/modules/commander/commander.cpp

@@ -148,6 +148,8 @@ static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60;		/**< Maximum percentage
 #define OFFBOARD_TIMEOUT		500000
 #define DIFFPRESS_TIMEOUT		2000000
 
+#define HOTPLUG_SENS_TIMEOUT		(8 * 1000 * 1000)	/**< wait for hotplug sensors to come online for upto 10 seconds */
+
 #define PRINT_INTERVAL	5000000
 #define PRINT_MODE_REJECT_INTERVAL	10000000
 
@@ -299,7 +301,7 @@ int commander_main(int argc, char *argv[])
 		daemon_task = px4_task_spawn_cmd("commander",
 					     SCHED_DEFAULT,
 					     SCHED_PRIORITY_MAX - 40,
-					     3500,
+					     3600,
 					     commander_thread_main,
 					     (char * const *)&argv[0]);
 
@@ -360,6 +362,8 @@ int commander_main(int argc, char *argv[])
 				calib_ret = do_level_calibration(mavlink_fd);
 			} else if (!strcmp(argv[2], "esc")) {
 				calib_ret = do_esc_calibration(mavlink_fd, &armed);
+			} else if (!strcmp(argv[2], "airspeed")) {
+				calib_ret = do_airspeed_calibration(mavlink_fd);
 			} else {
 				warnx("argument %s unsupported.", argv[2]);
 			}
@@ -377,23 +381,31 @@ int commander_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "check")) {
 		int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0);
-		int checkres = prearm_check(&status, mavlink_fd_local);
+		int checkres = 0;
+		checkres = preflight_check(&status, mavlink_fd_local, false, true);
+		warnx("Preflight check: %s", (checkres == 0) ? "OK" : "FAILED");
+		checkres = preflight_check(&status, mavlink_fd_local, true, true);
+		warnx("Prearm check: %s", (checkres == 0) ? "OK" : "FAILED");
 		px4_close(mavlink_fd_local);
-		warnx("FINAL RESULT: %s", (checkres == 0) ? "OK" : "FAILED");
 		return 0;
 	}
 
 	if (!strcmp(argv[1], "arm")) {
 		int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0);
-		arm_disarm(true, mavlink_fd_local, "command line");
-		warnx("note: not updating home position on commandline arming!");
+		if (TRANSITION_CHANGED == arm_disarm(true, mavlink_fd_local, "command line")) {
+			warnx("note: not updating home position on commandline arming!");
+		} else {
+			warnx("arming failed");
+		}
 		px4_close(mavlink_fd_local);
 		return 0;
 	}
 
 	if (!strcmp(argv[1], "disarm")) {
 		int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0);
-		arm_disarm(false, mavlink_fd_local, "command line");
+		if (TRANSITION_DENIED == arm_disarm(false, mavlink_fd_local, "command line")) {
+			warnx("rejected disarm");
+		}
 		px4_close(mavlink_fd_local);
 		return 0;
 	}
@@ -891,6 +903,7 @@ int commander_thread_main(int argc, char *argv[])
 	/* not yet initialized */
 	commander_initialized = false;
 
+	bool sensor_fail_tune_played = false;
 	bool arm_tune_played = false;
 	bool was_landed = true;
 	bool was_armed = false;
@@ -1020,6 +1033,9 @@ int commander_thread_main(int argc, char *argv[])
 
 	// XXX for now just set sensors as initialized
 	status.condition_system_sensors_initialized = true;
+	
+	status.condition_system_prearm_error_reported = false;
+	status.condition_system_hotplug_timeout = false;
 
 	status.counter++;
 	status.timestamp = hrt_absolute_time();
@@ -1101,7 +1117,7 @@ int commander_thread_main(int argc, char *argv[])
 
 	bool updated = false;
 
-	rc_calibration_check(mavlink_fd);
+	rc_calibration_check(mavlink_fd, true);
 
 	/* Subscribe to safety topic */
 	int safety_sub = orb_subscribe(ORB_ID(safety));
@@ -1243,6 +1259,7 @@ int commander_thread_main(int argc, char *argv[])
 
 	// Run preflight check
 	int32_t rc_in_off = 0;
+	bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT;
 	param_get(_param_autostart_id, &autostart_id);
 	param_get(_param_rc_in_off, &rc_in_off);
 	status.rc_input_mode = rc_in_off;
@@ -1251,14 +1268,10 @@ int commander_thread_main(int argc, char *argv[])
 		status.condition_system_sensors_initialized = false;
 		set_tune_override(TONE_STARTUP_TUNE); //normal boot tune
 	} else {
-		status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true,
-			checkAirspeed, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !status.circuit_breaker_engaged_gpsfailure_check);
-		if (!status.condition_system_sensors_initialized) {
-			set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune
-		}
-		else {
+			// sensor diagnostics done continiously, not just at boot so don't warn about any issues just yet
+			status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true,
+			checkAirspeed, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !status.circuit_breaker_engaged_gpsfailure_check, false);
 			set_tune_override(TONE_STARTUP_TUNE); //normal boot tune
-		}
 	}
 
 	commander_boot_timestamp = hrt_absolute_time();
@@ -1291,7 +1304,7 @@ int commander_thread_main(int argc, char *argv[])
 	/* initialize low priority thread */
 	pthread_attr_t commander_low_prio_attr;
 	pthread_attr_init(&commander_low_prio_attr);
-	pthread_attr_setstacksize(&commander_low_prio_attr, 2600);
+	pthread_attr_setstacksize(&commander_low_prio_attr, 2880);
 
 	struct sched_param param;
 	(void)pthread_attr_getschedparam(&commander_low_prio_attr, &param);
@@ -1354,11 +1367,11 @@ int commander_thread_main(int argc, char *argv[])
 
 				if (map_mode_sw == 0 && map_mode_sw != map_mode_sw_new && map_mode_sw_new < input_rc_s::RC_INPUT_MAX_CHANNELS && map_mode_sw_new > 0) {
 					status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed,
-							!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check);
+							!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout);
 				}
 
 				/* re-check RC calibration */
-				rc_calibration_check(mavlink_fd);
+				rc_calibration_check(mavlink_fd, true);
 			}
 
 			/* Safety parameters */
@@ -1446,6 +1459,8 @@ int commander_thread_main(int argc, char *argv[])
 				    !armed.armed) {
 
 				    	bool chAirspeed = false;
+				    	bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT;
+				    	
 					/* Perform airspeed check only if circuit breaker is not
 					 * engaged and it's not a rotary wing */
 					if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) {
@@ -1456,11 +1471,11 @@ int commander_thread_main(int argc, char *argv[])
 					if (is_hil_setup(autostart_id)) {
 						/* HIL configuration: check only RC input */
 						(void)Commander::preflightCheck(mavlink_fd, false, false, false, false, false,
-								!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), false);
+								!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), false, true);
 					} else {
 						/* check sensors also */
 						(void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed,
-								!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check);
+								!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout);
 					}
 				}
 
@@ -1686,10 +1701,10 @@ int commander_thread_main(int argc, char *argv[])
 				status_changed = true;
 
 				if (status.condition_landed) {
-					mavlink_log_critical(mavlink_fd, "LANDING DETECTED");
+					mavlink_and_console_log_info(mavlink_fd, "LANDING DETECTED");
 
 				} else {
-					mavlink_log_critical(mavlink_fd, "TAKEOFF DETECTED");
+					mavlink_and_console_log_info(mavlink_fd, "TAKEOFF DETECTED");
 				}
 			}
 
@@ -2177,11 +2192,17 @@ int commander_thread_main(int argc, char *argv[])
 				if (telemetry_lost[i] &&
 				    hrt_elapsed_time(&telemetry_last_dl_loss[i]) > datalink_regain_timeout * 1e6) {
 
-					/* only report a regain */
+					/* report a regain */
 					if (telemetry_last_dl_loss[i] > 0) {
 						mavlink_and_console_log_info(mavlink_fd, "data link #%i regained", i);
+					} else if (telemetry_last_dl_loss[i] == 0) {
+						/* new link */
 					}
 
+					/* got link again or new */
+					status.condition_system_prearm_error_reported = false;
+					status_changed = true;
+
 					telemetry_lost[i] = false;
 					have_link = true;
 
@@ -2414,7 +2435,7 @@ int commander_thread_main(int argc, char *argv[])
 		} else {
 			set_tune(TONE_STOP_TUNE);
 		}
-
+		
 		/* reset arm_tune_played when disarmed */
 		if (!armed.armed || (safety.safety_switch_available && !safety.safety_off)) {
 
@@ -2425,6 +2446,21 @@ int commander_thread_main(int argc, char *argv[])
 
 			arm_tune_played = false;
 		}
+		
+		/* play sensor failure tunes if we already waited for hotplug sensors to come up and failed */
+		hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT;
+		
+		if (!sensor_fail_tune_played && (!status.condition_system_sensors_initialized && hotplug_timeout)) {
+			set_tune_override(TONE_GPS_WARNING_TUNE);
+			sensor_fail_tune_played = true;
+			status_changed = true;
+		}
+		
+		/* update timeout flag */
+		if(!(hotplug_timeout == status.condition_system_hotplug_timeout)) {
+			status.condition_system_hotplug_timeout = hotplug_timeout;
+			status_changed = true;
+		}
 
 		counter++;
 
@@ -2482,6 +2518,8 @@ get_circuit_breaker_params()
 {
 	status.circuit_breaker_engaged_power_check =
 		circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY);
+	status.cb_usb =
+		circuit_breaker_enabled("CBRK_USB_CHK", CBRK_USB_CHK_KEY);
 	status.circuit_breaker_engaged_airspd_check =
 		circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY);
 	status.circuit_breaker_engaged_enginefailure_check =
@@ -2508,19 +2546,24 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
 	/* driving rgbled */
 	if (changed) {
 		bool set_normal_color = false;
-
+		bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT;
+		
 		/* set mode */
 		if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
 			rgbled_set_mode(RGBLED_MODE_ON);
 			set_normal_color = true;
 
-		} else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR || !status.condition_system_sensors_initialized) {
+		} else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR || (!status.condition_system_sensors_initialized && hotplug_timeout)) {
 			rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
 			rgbled_set_color(RGBLED_COLOR_RED);
 
 		} else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
 			rgbled_set_mode(RGBLED_MODE_BREATHE);
 			set_normal_color = true;
+		
+		} else if (!status.condition_system_sensors_initialized && !hotplug_timeout) {
+			rgbled_set_mode(RGBLED_MODE_BREATHE);
+			set_normal_color = true;
 
 		} else {	// STANDBY_ERROR and other states
 			rgbled_set_mode(RGBLED_MODE_BLINK_NORMAL);
@@ -2536,7 +2579,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
 			} else if (status_local->battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) {
 				rgbled_set_color(RGBLED_COLOR_RED);
 			} else {
-				if (status_local->condition_home_position_valid) {
+				if (status_local->condition_home_position_valid && status_local->condition_global_position_valid) {
 					rgbled_set_color(RGBLED_COLOR_GREEN);
 
 				} else {
@@ -3197,6 +3240,7 @@ void *commander_low_prio_loop(void *arg)
 						// so this would be prone to false negatives.
 
 						bool checkAirspeed = false;
+						bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT;
 						/* Perform airspeed check only if circuit breaker is not
 						 * engaged and it's not a rotary wing */
 						if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) {
@@ -3204,7 +3248,7 @@ void *commander_low_prio_loop(void *arg)
 						}
 
 						status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed,
-							!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check);
+							!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout);
 
 						arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, false /* fRunPreArmChecks */, mavlink_fd);
 

src/modules/commander/commander_params.c

@@ -55,6 +55,7 @@
  * @group Radio Calibration
  * @min -0.25
  * @max 0.25
+ * @decimal 2
  */
 PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f);
 
@@ -69,6 +70,7 @@ PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f);
  * @group Radio Calibration
  * @min -0.25
  * @max 0.25
+ * @decimal 2
  */
 PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f);
 
@@ -83,6 +85,7 @@ PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f);
  * @group Radio Calibration
  * @min -0.25
  * @max 0.25
+ * @decimal 2
  */
 PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f);
 
@@ -93,6 +96,7 @@ PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f);
  *
  * @group Battery Calibration
  * @unit V
+ * @decimal 2
  */
 PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f);
 
@@ -103,6 +107,7 @@ PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f);
  *
  * @group Battery Calibration
  * @unit V
+ * @decimal 2
  */
 PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.2f);
 
@@ -115,6 +120,8 @@ PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.2f);
  * @group Battery Calibration
  * @unit V
  * @min 0.0
+ * @max 1.5
+ * @decimal 2
  */
 PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.07f);
 
@@ -137,6 +144,7 @@ PARAM_DEFINE_INT32(BAT_N_CELLS, 3);
  *
  * @group Battery Calibration
  * @unit mA
+ * @decimal 0
  */
 PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f);
 
@@ -184,6 +192,7 @@ PARAM_DEFINE_INT32(COM_DL_REG_T, 0);
  * @group Commander
  * @min 0.0
  * @max 1.0
+ * @decimal 1
  */
 PARAM_DEFINE_FLOAT(COM_EF_THROT, 0.5f);
 
@@ -196,6 +205,7 @@ PARAM_DEFINE_FLOAT(COM_EF_THROT, 0.5f);
  * @min 0.0
  * @max 30.0
  * @unit ampere
+ * @decimal 2
  */
 PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f);
 
@@ -209,6 +219,7 @@ PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f);
  * @unit second
  * @min 0.0
  * @max 60.0
+ * @decimal 1
  */
 PARAM_DEFINE_FLOAT(COM_EF_TIME, 10.0f);
 
@@ -221,6 +232,7 @@ PARAM_DEFINE_FLOAT(COM_EF_TIME, 10.0f);
  * @unit second
  * @min 0
  * @max 35
+ * @decimal 1
  */
 PARAM_DEFINE_FLOAT(COM_RC_LOSS_T, 0.5f);
 
@@ -233,6 +245,7 @@ PARAM_DEFINE_FLOAT(COM_RC_LOSS_T, 0.5f);
  * @unit meter
  * @min 2
  * @max 15
+ * @decimal 2
  */
 PARAM_DEFINE_FLOAT(COM_HOME_H_T, 5.0f);
 
@@ -245,6 +258,7 @@ PARAM_DEFINE_FLOAT(COM_HOME_H_T, 5.0f);
  * @unit meter
  * @min 5
  * @max 25
+ * @decimal 2
  */
 PARAM_DEFINE_FLOAT(COM_HOME_V_T, 10.0f);
 

src/modules/commander/gyro_calibration.cpp

@@ -122,7 +122,7 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data)
 				}
 				
 				if (s == 0 && calibration_counter[0] % (calibration_count / 20) == 0) {
-					mavlink_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, (calibration_counter[0] * 100) / calibration_count);
+					mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, (calibration_counter[0] * 100) / calibration_count);
 				}
 			}
 			
@@ -131,14 +131,14 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data)
 		}
 		
 		if (poll_errcount > 1000) {
-			mavlink_log_critical(worker_data->mavlink_fd, CAL_ERROR_SENSOR_MSG);
+			mavlink_and_console_log_critical(worker_data->mavlink_fd, CAL_ERROR_SENSOR_MSG);
 			return calibrate_return_error;
 		}
 	}
 	
 	for (unsigned s = 0; s < max_gyros; s++) {
 		if (worker_data->device_id[s] != 0 && calibration_counter[s] < calibration_count / 2) {
-			mavlink_log_critical(worker_data->mavlink_fd, "[cal] ERROR: missing data, sensor %d", s)
+			mavlink_and_console_log_critical(worker_data->mavlink_fd, "[cal] ERROR: missing data, sensor %d", s)
 			return calibrate_return_error;
 		}
 
@@ -155,7 +155,7 @@ int do_gyro_calibration(int mavlink_fd)
 	int			res = OK;
 	gyro_worker_data_t	worker_data = {};
 
-	mavlink_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
+	mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
 
 	worker_data.mavlink_fd = mavlink_fd;
 	
@@ -179,7 +179,7 @@ int do_gyro_calibration(int mavlink_fd)
 		(void)sprintf(str, "CAL_GYRO%u_ID", s);
 		res = param_set_no_notification(param_find(str), &(worker_data.device_id[s]));
 		if (res != OK) {
-			mavlink_log_critical(mavlink_fd, "[cal] Unable to reset CAL_GYRO%u_ID", s);
+			mavlink_and_console_log_critical(mavlink_fd, "[cal] Unable to reset CAL_GYRO%u_ID", s);
 			return ERROR;
 		}
 		
@@ -193,7 +193,7 @@ int do_gyro_calibration(int mavlink_fd)
 			px4_close(fd);
 
 			if (res != OK) {
-				mavlink_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, s);
+				mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, s);
 				return ERROR;
 			}
 		}
@@ -238,7 +238,7 @@ int do_gyro_calibration(int mavlink_fd)
 			float zdiff = worker_data.gyro_report_0.z - worker_data.gyro_scale[0].z_offset;
 
 			/* maximum allowable calibration error in radians */
-			const float maxoff = 0.0055f;
+			const float maxoff = 0.01f;
 
 			if (!PX4_ISFINITE(worker_data.gyro_scale[0].x_offset) ||
 			    !PX4_ISFINITE(worker_data.gyro_scale[0].y_offset) ||
@@ -302,7 +302,7 @@ int do_gyro_calibration(int mavlink_fd)
 				px4_close(fd);
 
 				if (res != OK) {
-					mavlink_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG);
+					mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG, 1);
 				}
 			}
 		}
@@ -325,7 +325,7 @@ int do_gyro_calibration(int mavlink_fd)
 		res = param_save_default();
 
 		if (res != OK) {
-			mavlink_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG);
+			mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG);
 		}
 	}
 
@@ -333,9 +333,9 @@ int do_gyro_calibration(int mavlink_fd)
 	usleep(200000);
 
 	if (res == OK) {
-		mavlink_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name);
+		mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name);
 	} else {
-		mavlink_log_info(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name);
+		mavlink_and_console_log_info(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name);
 	}
 
 	/* give this message enough time to propagate */

src/modules/commander/state_machine_helper.cpp

@@ -97,8 +97,6 @@ static const char * const state_names[vehicle_status_s::ARMING_STATE_MAX] = {
 	"ARMING_STATE_IN_AIR_RESTORE",
 };
 
-static bool sensor_feedback_provided = false;
-
 transition_result_t
 arming_state_transition(struct vehicle_status_s *status,		///< current vehicle status
 			const struct safety_s   *safety,		///< current safety settings
@@ -126,10 +124,20 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 		 */
 		int prearm_ret = OK;
 
-		/* only perform the check if we have to */
+		/* only perform the pre-arm check if we have to */
 		if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED
 				&& status->hil_state == vehicle_status_s::HIL_STATE_OFF) {
-			prearm_ret = prearm_check(status, mavlink_fd);
+
+			prearm_ret = preflight_check(status, mavlink_fd, true /* pre-arm */ );
+		}
+		/* re-run the pre-flight check as long as sensors are failing */
+		if (!status->condition_system_sensors_initialized 
+				&& (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED ||
+				    new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY)
+             			&& status->hil_state == vehicle_status_s::HIL_STATE_OFF) {
+
+            		prearm_ret = preflight_check(status, mavlink_fd, false /* pre-flight */);
+			status->condition_system_sensors_initialized = !prearm_ret;
 		}
 
 		/*
@@ -175,7 +183,7 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 					// Fail transition if we need safety switch press
 					} else if (safety->safety_switch_available && !safety->safety_off) {
 
-						mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first!");
+						mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first!");
 						feedback_provided = true;
 						valid_transition = false;
 					}
@@ -200,10 +208,10 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 								feedback_provided = true;
 								valid_transition = false;
 							} else if (status->avionics_power_rail_voltage < 4.9f) {
-								mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage);
+								mavlink_and_console_log_critical(mavlink_fd, "CAUTION: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage);
 								feedback_provided = true;
 							} else if (status->avionics_power_rail_voltage > 5.4f) {
-								mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power high: %6.2f Volt", (double)status->avionics_power_rail_voltage);
+								mavlink_and_console_log_critical(mavlink_fd, "CAUTION: Avionics power high: %6.2f Volt", (double)status->avionics_power_rail_voltage);
 								feedback_provided = true;
 							}
 						}
@@ -247,9 +255,11 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 			(new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) &&
 			(status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR) &&
 			(!status->condition_system_sensors_initialized)) {
-			if (!sensor_feedback_provided || (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)) {
+			if ((!status->condition_system_prearm_error_reported && 
+			      status->condition_system_hotplug_timeout) || 
+			     (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)) {
 				mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors need inspection");
-				sensor_feedback_provided = true;
+				status->condition_system_prearm_error_reported = true;
 			}
 			feedback_provided = true;
 			valid_transition = false;
@@ -265,8 +275,9 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 
 		/* reset feedback state */
 		if (status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR &&
+			status->arming_state != vehicle_status_s::ARMING_STATE_INIT &&
 			valid_transition) {
-			sensor_feedback_provided = false;
+			status->condition_system_prearm_error_reported = false;
 		}
 
 		/* end of atomic state update */
@@ -384,7 +395,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
 		switch (new_state) {
 		case vehicle_status_s::HIL_STATE_OFF:
 			/* we're in HIL and unexpected things can happen if we disable HIL now */
-			mavlink_log_critical(mavlink_fd, "Not switching off HIL (safety)");
+			mavlink_and_console_log_critical(mavlink_fd, "Not switching off HIL (safety)");
 			ret = TRANSITION_DENIED;
 			break;
 
@@ -457,7 +468,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
 					closedir(d);
 
 					ret = TRANSITION_CHANGED;
-					mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
+					mavlink_and_console_log_critical(mavlink_fd, "Switched to ON hil state");
 
 				} else {
 					/* failed opening dir */
@@ -518,11 +529,11 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
 				}
 
 				ret = TRANSITION_CHANGED;
-				mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
+				mavlink_and_console_log_critical(mavlink_fd, "Switched to ON hil state");
 #endif
 
 			} else {
-				mavlink_log_critical(mavlink_fd, "Not switching to HIL when armed");
+				mavlink_and_console_log_critical(mavlink_fd, "Not switching to HIL when armed");
 				ret = TRANSITION_DENIED;
 			}
 			break;
@@ -759,24 +770,35 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 	return status->nav_state != nav_state_old;
 }
 
-int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
+int preflight_check(struct vehicle_status_s *status, const int mavlink_fd, bool prearm, bool force_report)
 {
-	/* at this point this equals the preflight check, but might add additional
-	 * quantities later.
+	/* 
 	 */
+	bool reportFailures = force_report || (!status->condition_system_prearm_error_reported &&
+		status->condition_system_hotplug_timeout);
+	
 	bool checkAirspeed = false;
 	/* Perform airspeed check only if circuit breaker is not
 	 * engaged and it's not a rotary wing */
-	if (!status->circuit_breaker_engaged_airspd_check && !status->is_rotary_wing) {
+	if (!status->circuit_breaker_engaged_airspd_check && (!status->is_rotary_wing || status->is_vtol)) {
 		checkAirspeed = true;
 	}
 
-	bool prearm_ok = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !(status->rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status->circuit_breaker_engaged_gpsfailure_check, true);
-
-	if (status->usb_connected) {
-		prearm_ok = false;
-		mavlink_log_critical(mavlink_fd, "NOT ARMING: Flying with USB connected prohibited");
+	bool preflight_ok = Commander::preflightCheck(mavlink_fd, true, true, true, true,
+				checkAirspeed, !(status->rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF),
+				!status->circuit_breaker_engaged_gpsfailure_check, true, reportFailures);
+		
+	if (!status->cb_usb && status->usb_connected && prearm) {
+		preflight_ok = false;
+		if (reportFailures) {
+			mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Flying with USB connected prohibited");
+		}
 	}
 
-	return !prearm_ok;
+	/* report once, then set the flag */
+	if (mavlink_fd >= 0 && reportFailures && !preflight_ok) {
+		status->condition_system_prearm_error_reported = true;
+	}
+
+	return !preflight_ok;
 }

src/modules/commander/state_machine_helper.h

@@ -65,6 +65,6 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
 
 bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished, const bool stay_in_failsafe);
 
-int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd);
+int preflight_check(struct vehicle_status_s *status, const int mavlink_fd, bool prearm, bool force_report=false);
 
 #endif /* STATE_MACHINE_HELPER_H_ */

src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h

@@ -69,6 +69,7 @@
 #include <mathlib/math/filter/LowPassFilter2p.hpp>
 
 #include <geo/geo.h>
+#include <terrain_estimation/terrain_estimator.h>
 #include <systemlib/perf_counter.h>
 #include <lib/ecl/validation/data_validator_group.h>
 #include "estimator_22states.h"
@@ -278,6 +279,8 @@ private:
 
     AttPosEKF                   *_ekf;
 
+    TerrainEstimator            *_terrain_estimator;
+
     /* Low pass filter for attitude rates */
     math::LowPassFilter2p _LP_att_P;
     math::LowPassFilter2p _LP_att_Q;

src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp

@@ -212,6 +212,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
 	_parameters{},
 	_parameter_handles{},
 	_ekf(nullptr),
+	_terrain_estimator(nullptr),
 
 	_LP_att_P(250.0f, 20.0f),
 	_LP_att_Q(250.0f, 20.0f),
@@ -219,6 +220,8 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
 {
 	_voter_mag.set_timeout(200000);
 
+	_terrain_estimator = new TerrainEstimator();
+
 	_parameter_handles.vel_delay_ms = param_find("PE_VEL_DELAY_MS");
 	_parameter_handles.pos_delay_ms = param_find("PE_POS_DELAY_MS");
 	_parameter_handles.height_delay_ms = param_find("PE_HGT_DELAY_MS");
@@ -267,7 +270,7 @@ AttitudePositionEstimatorEKF::~AttitudePositionEstimatorEKF()
 			}
 		} while (_estimator_task != -1);
 	}
-
+	delete _terrain_estimator;
 	delete _ekf;
 
 	estimator::g_estimator = nullptr;
@@ -697,6 +700,10 @@ void AttitudePositionEstimatorEKF::task_main()
 					// Run EKF data fusion steps
 					updateSensorFusion(_gpsIsGood, _newDataMag, _newRangeData, _newHgtData, _newAdsData);
 
+					// Run separate terrain estimator
+					_terrain_estimator->predict(_ekf->dtIMU, &_att, &_sensor_combined, &_distance);
+					_terrain_estimator->measurement_update(hrt_absolute_time(), &_gps, &_distance, &_att);
+
 					// Publish attitude estimations
 					publishAttitude();
 
@@ -997,9 +1004,12 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition()
 	}
 
 	/* terrain altitude */
-	_global_pos.terrain_alt = _ekf->hgtRef - _ekf->flowStates[1];
-	_global_pos.terrain_alt_valid = (_distance_last_valid > 0) &&
-					(hrt_elapsed_time(&_distance_last_valid) < 20 * 1000 * 1000);
+	if (_terrain_estimator->is_valid()) {
+		_global_pos.terrain_alt = _global_pos.alt - _terrain_estimator->get_distance_to_ground();
+		_global_pos.terrain_alt_valid = true;
+	} else {
+		_global_pos.terrain_alt_valid = false;
+	}
 
 	_global_pos.yaw = _local_pos.yaw;
 

src/modules/fw_att_control/fw_att_control_main.cpp

@@ -82,6 +82,7 @@
 #include <ecl/attitude_fw/ecl_pitch_controller.h>
 #include <ecl/attitude_fw/ecl_roll_controller.h>
 #include <ecl/attitude_fw/ecl_yaw_controller.h>
+#include <ecl/attitude_fw/ecl_wheel_controller.h>
 #include <platforms/px4_defines.h>
 
 /**
@@ -160,6 +161,10 @@ private:
 	bool		_setpoint_valid;		/**< flag if the position control setpoint is valid */
 	bool		_debug;				/**< if set to true, print debug output */
 
+	float _flaps_cmd_last;
+	float _flaperons_cmd_last;
+
+
 	struct {
 		float tconst;
 		float p_p;
@@ -181,6 +186,11 @@ private:
 		float y_coordinated_min_speed;
 		int32_t y_coordinated_method;
 		float y_rmax;
+		float w_p;
+		float w_i;
+		float w_ff;
+		float w_integrator_max;
+		float w_rmax;
 
 		float airspeed_min;
 		float airspeed_trim;
@@ -196,6 +206,9 @@ private:
 		float man_roll_max;				/**< Max Roll in rad */
 		float man_pitch_max;			/**< Max Pitch in rad */
 
+		float flaps_scale;				/**< Scale factor for flaps */
+		float flaperon_scale;			/**< Scale factor for flaperons */
+
 		int vtol_type;					/**< VTOL type: 0 = tailsitter, 1 = tiltrotor */
 
 	}		_parameters;			/**< local copies of interesting parameters */
@@ -222,6 +235,11 @@ private:
 		param_t y_coordinated_min_speed;
 		param_t y_coordinated_method;
 		param_t y_rmax;
+		param_t w_p;
+		param_t w_i;
+		param_t w_ff;
+		param_t w_integrator_max;
+		param_t w_rmax;
 
 		param_t airspeed_min;
 		param_t airspeed_trim;
@@ -235,6 +253,9 @@ private:
 		param_t man_roll_max;
 		param_t man_pitch_max;
 
+		param_t flaps_scale;
+		param_t flaperon_scale;
+
 		param_t vtol_type;
 
 	}		_parameter_handles;		/**< handles for interesting parameters */
@@ -248,6 +269,7 @@ private:
 	ECL_RollController				_roll_ctrl;
 	ECL_PitchController				_pitch_ctrl;
 	ECL_YawController				_yaw_ctrl;
+	ECL_WheelController			_wheel_ctrl;
 
 
 	/**
@@ -345,7 +367,9 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_nonfinite_output_perf(perf_alloc(PC_COUNT, "fw att control nonfinite output")),
 /* states */
 	_setpoint_valid(false),
-	_debug(false)
+	_debug(false),
+	_flaps_cmd_last(0),
+	_flaperons_cmd_last(0)
 {
 	/* safely initialize structs */
 	_ctrl_state = {};
@@ -380,6 +404,12 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_parameter_handles.y_integrator_max = param_find("FW_YR_IMAX");
 	_parameter_handles.y_rmax = param_find("FW_Y_RMAX");
 
+	_parameter_handles.w_p = param_find("FW_WR_P");
+	_parameter_handles.w_i = param_find("FW_WR_I");
+	_parameter_handles.w_ff = param_find("FW_WR_FF");
+	_parameter_handles.w_integrator_max = param_find("FW_WR_IMAX");
+	_parameter_handles.w_rmax = param_find("FW_W_RMAX");
+
 	_parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN");
 	_parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM");
 	_parameter_handles.airspeed_max = param_find("FW_AIRSPD_MAX");
@@ -396,6 +426,9 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_parameter_handles.man_roll_max = param_find("FW_MAN_R_MAX");
 	_parameter_handles.man_pitch_max = param_find("FW_MAN_P_MAX");
 
+	_parameter_handles.flaps_scale = param_find("FW_FLAPS_SCL");
+	_parameter_handles.flaperon_scale = param_find("FW_FLAPERON_SCL");
+
 	_parameter_handles.vtol_type = param_find("VT_TYPE");
 
 	/* fetch initial parameter values */
@@ -458,6 +491,12 @@ FixedwingAttitudeControl::parameters_update()
 	param_get(_parameter_handles.y_coordinated_method, &(_parameters.y_coordinated_method));
 	param_get(_parameter_handles.y_rmax, &(_parameters.y_rmax));
 
+	param_get(_parameter_handles.w_p, &(_parameters.w_p));
+	param_get(_parameter_handles.w_i, &(_parameters.w_i));
+	param_get(_parameter_handles.w_ff, &(_parameters.w_ff));
+	param_get(_parameter_handles.w_integrator_max, &(_parameters.w_integrator_max));
+	param_get(_parameter_handles.w_rmax, &(_parameters.w_rmax));
+
 	param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min));
 	param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim));
 	param_get(_parameter_handles.airspeed_max, &(_parameters.airspeed_max));
@@ -474,6 +513,9 @@ FixedwingAttitudeControl::parameters_update()
 	_parameters.man_roll_max = math::radians(_parameters.man_roll_max);
 	_parameters.man_pitch_max = math::radians(_parameters.man_pitch_max);
 
+	param_get(_parameter_handles.flaps_scale, &_parameters.flaps_scale);
+	param_get(_parameter_handles.flaperon_scale, &_parameters.flaperon_scale);
+
 	param_get(_parameter_handles.vtol_type, &_parameters.vtol_type);
 
 	/* pitch control parameters */
@@ -502,6 +544,13 @@ FixedwingAttitudeControl::parameters_update()
 	_yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method);
 	_yaw_ctrl.set_max_rate(math::radians(_parameters.y_rmax));
 
+	/* wheel control parameters */
+	_wheel_ctrl.set_k_p(_parameters.w_p);
+	_wheel_ctrl.set_k_i(_parameters.w_i);
+	_wheel_ctrl.set_k_ff(_parameters.w_ff);
+	_wheel_ctrl.set_integrator_max(_parameters.w_integrator_max);
+	_wheel_ctrl.set_max_rate(math::radians(_parameters.w_rmax));
+
 	return OK;
 }
 
@@ -751,6 +800,10 @@ FixedwingAttitudeControl::task_main()
 
 			vehicle_status_poll();
 
+			// the position controller will not emit attitude setpoints in some modes
+			// we need to make sure that this flag is reset
+			_att_sp.fw_control_yaw = _att_sp.fw_control_yaw && _vcontrol_mode.flag_control_auto_enabled;
+
 			/* lock integrator until control is started */
 			bool lock_integrator;
 
@@ -778,9 +831,53 @@ FixedwingAttitudeControl::task_main()
 			/* default flaps to center */
 			float flaps_control = 0.0f;
 
+			static float delta_flaps = 0;
+
 			/* map flaps by default to manual if valid */
-			if (PX4_ISFINITE(_manual.flaps)) {
-				flaps_control = _manual.flaps;
+			if (PX4_ISFINITE(_manual.flaps) && _vcontrol_mode.flag_control_manual_enabled) {
+				flaps_control = 0.5f * (_manual.flaps + 1.0f ) * _parameters.flaps_scale;
+			} else if (_vcontrol_mode.flag_control_auto_enabled) {
+				flaps_control = _att_sp.apply_flaps ? 1.0f * _parameters.flaps_scale : 0.0f;
+			}
+
+			// move the actual control value continuous with time
+			static hrt_abstime t_flaps_changed = 0;
+			if (fabsf(flaps_control - _flaps_cmd_last) > 0.01f) {
+				t_flaps_changed = hrt_absolute_time();
+				delta_flaps = flaps_control - _flaps_cmd_last;
+				_flaps_cmd_last = flaps_control;
+			}
+
+			static float flaps_applied = 0.0f;
+
+			if (fabsf(flaps_applied - flaps_control) > 0.01f) {
+				flaps_applied = (flaps_control - delta_flaps) + (float)hrt_elapsed_time(&t_flaps_changed) * (delta_flaps) / 1000000;
+			}
+
+			/* default flaperon to center */
+			float flaperon = 0.0f;
+
+			static float delta_flaperon = 0.0f;
+
+			/* map flaperons by default to manual if valid */
+			if (PX4_ISFINITE(_manual.aux2) && _vcontrol_mode.flag_control_manual_enabled) {
+				flaperon = 0.5f * (_manual.aux2 + 1.0f) * _parameters.flaperon_scale;
+			} else if (_vcontrol_mode.flag_control_auto_enabled) {
+				flaperon = _att_sp.apply_flaps ? 1.0f * _parameters.flaperon_scale : 0.0f;
+			}
+
+			// move the actual control value continuous with time
+			static hrt_abstime t_flaperons_changed = 0;
+			if (fabsf(flaperon - _flaperons_cmd_last) > 0.01f) {
+				t_flaperons_changed = hrt_absolute_time();
+				delta_flaperon = flaperon - _flaperons_cmd_last;
+				_flaperons_cmd_last = flaperon;
+			}
+
+			static float flaperon_applied = 0.0f;
+
+			if (fabsf(flaperon_applied - flaperon) > 0.01f) {
+				flaperon_applied = (flaperon - delta_flaperon) + (float)hrt_elapsed_time(&t_flaperons_changed) * (delta_flaperon) / 1000000;
 			}
 
 			/* decide if in stabilized or full manual control */
@@ -806,11 +903,19 @@ FixedwingAttitudeControl::task_main()
 				 *
 				 * Forcing the scaling to this value allows reasonable handheld tests.
 				 */
-
 				float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min : airspeed);
 
+				/* Use min airspeed to calculate ground speed scaling region.
+				 * Don't scale below gspd_scaling_trim
+				 */
+				float groundspeed = sqrtf(_global_pos.vel_n * _global_pos.vel_n +
+						_global_pos.vel_e * _global_pos.vel_e);
+				float gspd_scaling_trim = (_parameters.airspeed_min * 0.6f);
+				float groundspeed_scaler = gspd_scaling_trim / ((groundspeed < gspd_scaling_trim) ? gspd_scaling_trim : groundspeed);
+
 				float roll_sp = _parameters.rollsp_offset_rad;
 				float pitch_sp = _parameters.pitchsp_offset_rad;
+				float yaw_sp = 0.0f;
 				float yaw_manual = 0.0f;
 				float throttle_sp = 0.0f;
 
@@ -824,6 +929,7 @@ FixedwingAttitudeControl::task_main()
 					/* read in attitude setpoint from attitude setpoint uorb topic */
 					roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
 					pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
+					yaw_sp = _att_sp.yaw_body;
 					throttle_sp = _att_sp.thrust;
 
 					/* reset integrals where needed */
@@ -835,6 +941,7 @@ FixedwingAttitudeControl::task_main()
 					}
 					if (_att_sp.yaw_reset_integral) {
 						_yaw_ctrl.reset_integrator();
+						_wheel_ctrl.reset_integrator();
 					}
 				} else if (_vcontrol_mode.flag_control_velocity_enabled) {
 
@@ -860,6 +967,7 @@ FixedwingAttitudeControl::task_main()
 					}
 					if (_att_sp.yaw_reset_integral) {
 						_yaw_ctrl.reset_integrator();
+						_wheel_ctrl.reset_integrator();
 					}
 
 				} else if (_vcontrol_mode.flag_control_altitude_enabled) {
@@ -879,6 +987,7 @@ FixedwingAttitudeControl::task_main()
 					}
 					if (_att_sp.yaw_reset_integral) {
 						_yaw_ctrl.reset_integrator();
+						_wheel_ctrl.reset_integrator();
 					}
 				} else {
 					/*
@@ -929,6 +1038,7 @@ FixedwingAttitudeControl::task_main()
 					_roll_ctrl.reset_integrator();
 					_pitch_ctrl.reset_integrator();
 					_yaw_ctrl.reset_integrator();
+					_wheel_ctrl.reset_integrator();
 				}
 
 				/* Prepare speed_body_u and speed_body_w */
@@ -952,17 +1062,23 @@ FixedwingAttitudeControl::task_main()
 				control_input.acc_body_z = _accel.z;
 				control_input.roll_setpoint = roll_sp;
 				control_input.pitch_setpoint = pitch_sp;
+				control_input.yaw_setpoint = yaw_sp;
 				control_input.airspeed_min = _parameters.airspeed_min;
 				control_input.airspeed_max = _parameters.airspeed_max;
 				control_input.airspeed = airspeed;
 				control_input.scaler = airspeed_scaling;
 				control_input.lock_integrator = lock_integrator;
+				control_input.groundspeed = groundspeed;
+				control_input.groundspeed_scaler = groundspeed_scaler;
+
+				_yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method);
 
 				/* Run attitude controllers */
 				if (PX4_ISFINITE(roll_sp) && PX4_ISFINITE(pitch_sp)) {
 					_roll_ctrl.control_attitude(control_input);
 					_pitch_ctrl.control_attitude(control_input);
 					_yaw_ctrl.control_attitude(control_input); //runs last, because is depending on output of roll and pitch attitude
+					_wheel_ctrl.control_attitude(control_input);
 
 					/* Update input data for rate controllers */
 					control_input.roll_rate_setpoint = _roll_ctrl.get_desired_rate();
@@ -1002,13 +1118,21 @@ FixedwingAttitudeControl::task_main()
 						}
 					}
 
-					float yaw_u = _yaw_ctrl.control_bodyrate(control_input);
+					float yaw_u = 0.0f;
+					if (_att_sp.fw_control_yaw == true) {
+						yaw_u = _wheel_ctrl.control_bodyrate(control_input);
+					}
+
+					else {
+						yaw_u = _yaw_ctrl.control_bodyrate(control_input);
+					}
 					_actuators.control[2] = (PX4_ISFINITE(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw;
 
 					/* add in manual rudder control */
 					_actuators.control[2] += yaw_manual;
 					if (!PX4_ISFINITE(yaw_u)) {
 						_yaw_ctrl.reset_integrator();
+						_wheel_ctrl.reset_integrator();
 						perf_count(_nonfinite_output_perf);
 						if (_debug && loop_counter % 10 == 0) {
 							warnx("yaw_u %.4f", (double)yaw_u);
@@ -1059,9 +1183,9 @@ FixedwingAttitudeControl::task_main()
 				_actuators.control[actuator_controls_s::INDEX_THROTTLE] = _manual.z;
 			}
 
-			_actuators.control[actuator_controls_s::INDEX_FLAPS] = flaps_control;
+			_actuators.control[actuator_controls_s::INDEX_FLAPS] = flaps_applied;
 			_actuators.control[5] = _manual.aux1;
-			_actuators.control[6] = _manual.aux2;
+			_actuators.control[actuator_controls_s::INDEX_AIRBRAKES] = flaperon_applied;
 			_actuators.control[7] = _manual.aux3;
 
 			/* lazily publish the setpoint only once available */

src/modules/fw_att_control/fw_att_control_params.c

@@ -220,6 +220,55 @@ PARAM_DEFINE_FLOAT(FW_YR_IMAX, 0.2f);
  */
 PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0.0f);
 
+/**
+ * Wheel steering rate proportional gain
+ *
+ * This defines how much the wheel steering input will be commanded depending on the
+ * current body angular rate error.
+ *
+ * @min 0.005
+ * @max 1.0
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_WR_P, 0.5f);
+
+/**
+ * Wheel steering 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_WR_I, 0.1f);
+
+/**
+ * Wheel steering 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_WR_IMAX, 1.0f);
+
+/**
+ * Maximum wheel steering rate
+ *
+ * This limits the maximum wheel steering 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_W_RMAX, 0.0f);
+
 /**
  * Roll rate feed forward
  *
@@ -255,6 +304,17 @@ PARAM_DEFINE_FLOAT(FW_PR_FF, 0.5f);
  */
 PARAM_DEFINE_FLOAT(FW_YR_FF, 0.3f);
 
+/**
+ * Wheel steering 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_WR_FF, 0.2f);
+
 /**
  * Minimal speed for yaw coordination
  *
@@ -374,3 +434,21 @@ PARAM_DEFINE_FLOAT(FW_MAN_R_MAX, 45.0f);
  * @group FW Attitude Control
  */
 PARAM_DEFINE_FLOAT(FW_MAN_P_MAX, 45.0f);
+
+/**
+ * Scale factor for flaps
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_FLAPS_SCL, 1.0f);
+
+/**
+ * Scale factor for flaperons
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_FLAPERON_SCL, 0.0f);

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -49,6 +49,7 @@
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
  * @author Thomas Gubler <thomasgubler@gmail.com>
+ * @author Andreas Antener <andreas@uaventure.com>
  */
 
 #include <px4_config.h>
@@ -95,6 +96,7 @@
 #include "landingslope.h"
 #include "mtecs/mTecs.h"
 #include <platforms/px4_defines.h>
+#include <runway_takeoff/RunwayTakeoff.h>
 
 static int	_control_task = -1;			/**< task handle for sensor task */
 #define HDG_HOLD_DIST_NEXT 			3000.0f 	// initial distance of waypoint in front of plane in heading hold mode
@@ -103,6 +105,7 @@ static int	_control_task = -1;			/**< task handle for sensor task */
 #define HDG_HOLD_YAWRATE_THRESH 	0.15f 		// max yawrate at which plane locks yaw for heading hold mode
 #define HDG_HOLD_MAN_INPUT_THRESH 	0.01f 		// max manual roll input from user which does not change the locked heading
 #define TAKEOFF_IDLE				0.2f 		// idle speed for POSCTRL/ATTCTRL (when landed and throttle stick > 0)
+#define T_ALT_TIMEOUT 				1 			// time after which we abort landing if terrain estimate is not valid
 
 static constexpr float THROTTLE_THRESH = 0.05f; 	///< max throttle from user which will not lead to motors spinning up in altitude controlled modes
 static constexpr float MANUAL_THROTTLE_CLIMBOUT_THRESH = 0.85f;	///< a throttle / pitch input above this value leads to the system switching to climbout mode
@@ -194,9 +197,17 @@ private:
 	bool land_onslope;
 	bool land_useterrain;
 
+	// landing relevant states
+	float _t_alt_prev_valid;	//**< last terrain estimate which was valid */
+	hrt_abstime _time_last_t_alt; //*< time at which we had last valid terrain alt */
+	hrt_abstime _time_started_landing;	//*< time at which landing started */
+	float height_flare;					//*< estimated height to ground at which flare started */
+
 	bool _was_in_air;	/**< indicated wether the plane was in the air in the previous interation*/
 	hrt_abstime _time_went_in_air;	/**< time at which the plane went in the air */
 
+	runwaytakeoff::RunwayTakeoff _runway_takeoff;
+
 	/* takeoff/launch states */
 	LaunchDetectionResult launch_detection_state;
 
@@ -277,6 +288,7 @@ private:
 		float land_flare_pitch_min_deg;
 		float land_flare_pitch_max_deg;
 		int land_use_terrain_estimate;
+		float land_airspeed_scale;
 
 	}		_parameters;			/**< local copies of interesting parameters */
 
@@ -325,6 +337,7 @@ private:
 		param_t land_flare_pitch_min_deg;
 		param_t land_flare_pitch_max_deg;
 		param_t land_use_terrain_estimate;
+		param_t land_airspeed_scale;
 
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
@@ -391,6 +404,11 @@ private:
 	 */
 	float		get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos);
 
+	/**
+	 * Return the terrain estimate during takeoff or takeoff_alt if terrain estimate is not available
+	 */
+	float		get_terrain_altitude_takeoff(float takeoff_alt, const struct vehicle_global_position_s &global_pos);
+
 	/**
 	 * Check if we are in a takeoff situation
 	 */
@@ -519,8 +537,13 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	land_motor_lim(false),
 	land_onslope(false),
 	land_useterrain(false),
+	_t_alt_prev_valid(0),
+	_time_last_t_alt(0),
+	_time_started_landing(0),
+	height_flare(0.0f),
 	_was_in_air(false),
 	_time_went_in_air(0),
+	_runway_takeoff(),
 	launch_detection_state(LAUNCHDETECTION_RES_NONE),
 	last_manual(false),
 	landingslope(),
@@ -562,6 +585,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_parameter_handles.land_flare_pitch_min_deg = param_find("FW_FLARE_PMIN");
 	_parameter_handles.land_flare_pitch_max_deg = param_find("FW_FLARE_PMAX");
 	_parameter_handles.land_use_terrain_estimate= param_find("FW_LND_USETER");
+	_parameter_handles.land_airspeed_scale = param_find("FW_AIRSPD_SCALE");
 
 	_parameter_handles.time_const = 			param_find("FW_T_TIME_CONST");
 	_parameter_handles.time_const_throt = 			param_find("FW_T_THRO_CONST");
@@ -665,6 +689,7 @@ FixedwingPositionControl::parameters_update()
 	param_get(_parameter_handles.land_flare_pitch_min_deg, &(_parameters.land_flare_pitch_min_deg));
 	param_get(_parameter_handles.land_flare_pitch_max_deg, &(_parameters.land_flare_pitch_max_deg));
 	param_get(_parameter_handles.land_use_terrain_estimate, &(_parameters.land_use_terrain_estimate));
+	param_get(_parameter_handles.land_airspeed_scale, &(_parameters.land_airspeed_scale));
 
 	_l1_control.set_l1_damping(_parameters.l1_damping);
 	_l1_control.set_l1_period(_parameters.l1_period);
@@ -715,6 +740,8 @@ FixedwingPositionControl::parameters_update()
 	/* Update the mTecs */
 	_mTecs.updateParams();
 
+	_runway_takeoff.updateParams();
+
 	return OK;
 }
 
@@ -947,9 +974,9 @@ float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint
 
 	/* Decide if the terrain estimation can be used, once we switched to using the terrain we stick with it
 	 * for the whole landing */
-	if (_parameters.land_use_terrain_estimate && (global_pos.terrain_alt_valid || land_useterrain)) {
+	if (_parameters.land_use_terrain_estimate && global_pos.terrain_alt_valid) {
 		if(!land_useterrain) {
-			mavlink_log_info(_mavlink_fd, "#audio: Landing, using terrain estimate");
+			mavlink_log_info(_mavlink_fd, "Landing, using terrain estimate");
 			land_useterrain = true;
 		}
 		return global_pos.terrain_alt;
@@ -958,6 +985,15 @@ float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint
 	}
 }
 
+float FixedwingPositionControl::get_terrain_altitude_takeoff(float takeoff_alt, const struct vehicle_global_position_s &global_pos)
+{
+	if (PX4_ISFINITE(global_pos.terrain_alt) && global_pos.terrain_alt_valid) {
+		return global_pos.terrain_alt;
+	}
+
+	return takeoff_alt;
+}
+
 bool FixedwingPositionControl::update_desired_altitude(float dt)
 {
 	/*
@@ -1054,6 +1090,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 	bool setpoint = true;
 
+	_att_sp.fw_control_yaw = false;		// by default we don't want yaw to be contoller directly with rudder
+	_att_sp.apply_flaps = false;		// by default we don't use flaps
 	float eas2tas = 1.0f; // XXX calculate actual number based on current measurements
 
 	/* filter speed and altitude for controller */
@@ -1171,19 +1209,39 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			_att_sp.roll_body = _l1_control.nav_roll();
 			_att_sp.yaw_body = _l1_control.nav_bearing();
 
-			if (in_takeoff_situation()) {
+			float alt_sp;
+			if (_nav_capabilities.abort_landing == true) {
+				// if we entered loiter due to an aborted landing, demand
+				// altitude setpoint well above landing waypoint
+				alt_sp = _pos_sp_triplet.current.alt + 2.0f * _parameters.climbout_diff;
+			} else {
+				// use altitude given by wapoint
+				alt_sp = _pos_sp_triplet.current.alt;
+			}
+
+			if (in_takeoff_situation() ||
+				((_global_pos.alt < _pos_sp_triplet.current.alt + _parameters.climbout_diff ) && _nav_capabilities.abort_landing == true)) {
 					/* limit roll motion to ensure enough lift */
 					_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f),
 							math::radians(15.0f));
 			}
 
-			tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas,
+			tecs_update_pitch_throttle(alt_sp, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas,
 						math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
 						_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
 						false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed);
 
 		} else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
 
+			// apply full flaps for landings. this flag will also trigger the use of flaperons
+			// if they have been enabled using the corresponding parameter
+			_att_sp.apply_flaps = true;
+
+			// save time at which we started landing
+			if (_time_started_landing == 0) {
+				_time_started_landing = hrt_absolute_time();
+			}
+
 			float bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1));
 			float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
 
@@ -1196,7 +1254,20 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				wp_distance_save = 0.0f;
 			}
 
-			//warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO");
+			// create virtual waypoint which is on the desired flight path but
+			// some distance behind landing waypoint. This will make sure that the plane
+			// will always follow the desired flight path even if we get close or past
+			// the landing waypoint
+			math::Vector<2> curr_wp_shifted;
+			double lat;
+			double lon;
+			create_waypoint_from_line_and_dist(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon, pos_sp_triplet.previous.lat,pos_sp_triplet.previous.lon, -1000.0f, &lat, &lon);
+			curr_wp_shifted(0) = (float)lat;
+			curr_wp_shifted(1) = (float)lon;
+
+			// we want the plane to keep tracking the desired flight path until we start flaring
+			// if we go into heading hold mode earlier then we risk to be pushed away from the runway by cross winds
+			//if (land_noreturn_vertical) {
 			if (wp_distance < _parameters.land_heading_hold_horizontal_distance || land_noreturn_horizontal) {
 
 				/* heading hold, along the line connecting this and the last waypoint */
@@ -1207,7 +1278,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 					} else {
 						target_bearing = _yaw;
 					}
-					mavlink_log_info(_mavlink_fd, "#audio: Landing, heading hold");
+					mavlink_log_info(_mavlink_fd, "#Landing, heading hold");
 				}
 
 //					warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_yaw));
@@ -1236,12 +1307,45 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			// XXX this could make a great param
 
 			float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f;
-			float airspeed_land = 1.3f * _parameters.airspeed_min;
-			float airspeed_approach = 1.3f * _parameters.airspeed_min;
+			float airspeed_land = _parameters.land_airspeed_scale * _parameters.airspeed_min;
+			float airspeed_approach = _parameters.land_airspeed_scale * _parameters.airspeed_min;
 
 			/* Get an estimate of the terrain altitude if available, otherwise terrain_alt will be
 			 * equal to _pos_sp_triplet.current.alt */
-			float terrain_alt = get_terrain_altitude_landing(_pos_sp_triplet.current.alt, _global_pos);
+			float terrain_alt;
+			if (_parameters.land_use_terrain_estimate) {
+				if (_global_pos.terrain_alt_valid) {
+					// all good, have valid terrain altitude
+					terrain_alt = _global_pos.terrain_alt;
+					_t_alt_prev_valid = terrain_alt;
+					_time_last_t_alt = hrt_absolute_time();
+				} else if (_time_last_t_alt == 0) {
+					// we have started landing phase but don't have valid terrain
+					// wait for some time, maybe we will soon get a valid estimate
+					// until then just use the altitude of the landing waypoint
+					if (hrt_elapsed_time(&_time_started_landing) < 10 * 1000 * 1000) {
+						terrain_alt = _pos_sp_triplet.current.alt;
+					} else {
+						// still no valid terrain, abort landing
+						terrain_alt = _pos_sp_triplet.current.alt;
+						_nav_capabilities.abort_landing = true;
+					}
+				} else if ((!_global_pos.terrain_alt_valid && hrt_elapsed_time(&_time_last_t_alt) < T_ALT_TIMEOUT * 1000 * 1000)
+					|| land_noreturn_vertical) {
+					// use previous terrain estimate for some time and hope to recover
+					// if we are already flaring (land_noreturn_vertical) then just
+					//  go with the old estimate
+					terrain_alt = _t_alt_prev_valid;
+				} else {
+					// terrain alt was not valid for long time, abort landing
+					terrain_alt = _t_alt_prev_valid;
+					_nav_capabilities.abort_landing = true;
+				}
+			} else {
+				// no terrain estimation, just use landing waypoint altitude
+				terrain_alt = _pos_sp_triplet.current.alt;
+			}
+			
 
 			/* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */
 			float L_altitude_rel = _pos_sp_triplet.previous.valid ?
@@ -1264,11 +1368,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				/* kill the throttle if param requests it */
 				throttle_max = _parameters.throttle_max;
 
+				/* enable direct yaw control using rudder/wheel */
+				_att_sp.yaw_body = target_bearing;
+				_att_sp.fw_control_yaw = true;
+
 				 if (_global_pos.alt < terrain_alt + landingslope.motor_lim_relative_alt() || land_motor_lim) {
 					throttle_max = math::min(throttle_max, _parameters.throttle_land_max);
 					if (!land_motor_lim) {
 						land_motor_lim  = true;
-						mavlink_log_info(_mavlink_fd, "#audio: Landing, limiting throttle");
+						mavlink_log_info(_mavlink_fd, "#Landing, limiting throttle");
 					}
 
 				 }
@@ -1292,10 +1400,19 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 						land_motor_lim ? tecs_status_s::TECS_MODE_LAND_THROTTLELIM : tecs_status_s::TECS_MODE_LAND);
 
 				if (!land_noreturn_vertical) {
-					mavlink_log_info(_mavlink_fd, "#audio: Landing, flaring");
+					// just started with the flaring phase
+					_att_sp.pitch_body = 0.0f;
+					height_flare = _global_pos.alt - terrain_alt;
+					mavlink_log_info(_mavlink_fd, "#Landing, flaring");
 					land_noreturn_vertical = true;
+				} else {
+					if (_global_pos.vel_d > 0.1f) {
+						_att_sp.pitch_body = math::radians(_parameters.land_flare_pitch_min_deg) *
+								math::constrain((height_flare - (_global_pos.alt - terrain_alt)) / height_flare, 0.0f, 1.0f);
+					} else {
+						_att_sp.pitch_body = _att_sp.pitch_body;
+					}
 				}
-				//warnx("Landing:  flare, _global_pos.alt  %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance);
 
 				flare_curve_alt_rel_last = flare_curve_alt_rel;
 			} else {
@@ -1314,7 +1431,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 					/* stay on slope */
 					altitude_desired_rel = landing_slope_alt_rel_desired;
 					if (!land_onslope) {
-						mavlink_log_info(_mavlink_fd, "#audio: Landing, on slope");
+						mavlink_log_info(_mavlink_fd, "#Landing, on slope");
 						land_onslope = true;
 					}
 				} else {
@@ -1338,94 +1455,160 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 		} else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
 
-			/* Perform launch detection */
-			if (launchDetector.launchDetectionEnabled() &&
-					launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
-				/* Inform user that launchdetection is running */
-				static hrt_abstime last_sent = 0;
-				if(hrt_absolute_time() - last_sent > 4e6) {
-					mavlink_log_critical(_mavlink_fd, "Launchdetection running");
-					last_sent = hrt_absolute_time();
+			if (_runway_takeoff.runwayTakeoffEnabled()) {
+				if (!_runway_takeoff.isInitialized()) {
+					math::Quaternion q(&_ctrl_state.q[0]);
+					math::Vector<3> euler = q.to_euler();
+					_runway_takeoff.init(euler(2), _global_pos.lat, _global_pos.lon);
+
+					/* need this already before takeoff is detected
+					 * doesn't matter if it gets reset when takeoff is detected eventually */
+					_takeoff_ground_alt = _global_pos.alt;
+
+					mavlink_log_info(_mavlink_fd, "#Takeoff on runway");
 				}
 
-				/* Detect launch */
-				launchDetector.update(_sensor_combined.accelerometer_m_s2[0]);
+				float terrain_alt = get_terrain_altitude_takeoff(_takeoff_ground_alt, _global_pos);
 
-				/* update our copy of the launch detection state */
-				launch_detection_state = launchDetector.getLaunchDetected();
-			} else	{
-				/* no takeoff detection --> fly */
-				launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS;
-			}
+				// update runway takeoff helper
+				_runway_takeoff.update(
+					_ctrl_state.airspeed,
+					_global_pos.alt - terrain_alt,
+					_global_pos.lat,
+					_global_pos.lon,
+					_mavlink_fd);
 
-			/* Set control values depending on the detection state */
-			if (launch_detection_state != LAUNCHDETECTION_RES_NONE) {
-				/* Launch has been detected, hence we have to control the plane. */
+				/*
+				 * Update navigation: _runway_takeoff returns the start WP according to mode and phase.
+				 * If we use the navigator heading or not is decided later.
+				 */
+				_l1_control.navigate_waypoints(_runway_takeoff.getStartWP(), curr_wp, current_position, ground_speed_2d);
 
-				_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
-				_att_sp.roll_body = _l1_control.nav_roll();
-				_att_sp.yaw_body = _l1_control.nav_bearing();
-
-				/* Select throttle: only in LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS we want to use
-				 * full throttle, otherwise we use the preTakeOff Throttle */
-				float takeoff_throttle = launch_detection_state !=
-					LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS ?
-					launchDetector.getThrottlePreTakeoff() : _parameters.throttle_max;
-
-				/* select maximum pitch: the launchdetector may impose another limit for the pitch
-				 * depending on the state of the launch */
-				float takeoff_pitch_max_deg = launchDetector.getPitchMax(_parameters.pitch_limit_max);
+				// update tecs
+				float takeoff_pitch_max_deg = _runway_takeoff.getMaxPitch(_parameters.pitch_limit_max);
 				float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg);
 
-				/* apply minimum pitch and limit roll if target altitude is not within climbout_diff
-				 * meters */
-				if (_parameters.climbout_diff > 0.001f && altitude_error > _parameters.climbout_diff) {
+				tecs_update_pitch_throttle(_pos_sp_triplet.current.alt,
+						calculate_target_airspeed(
+							_runway_takeoff.getMinAirspeedScaling() * _parameters.airspeed_min),
+						eas2tas,
+						math::radians(_parameters.pitch_limit_min),
+						takeoff_pitch_max_rad,
+						_parameters.throttle_min,
+						_parameters.throttle_max, // XXX should we also set runway_takeoff_throttle here?
+						_parameters.throttle_cruise,
+						_runway_takeoff.climbout(),
+						math::radians(_runway_takeoff.getMinPitch(
+							_pos_sp_triplet.current.pitch_min,
+							10.0f,
+							_parameters.pitch_limit_min)),
+						_global_pos.alt,
+						ground_speed,
+						tecs_status_s::TECS_MODE_TAKEOFF,
+						takeoff_pitch_max_deg != _parameters.pitch_limit_max);
 
-					/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
-					tecs_update_pitch_throttle(_pos_sp_triplet.current.alt,
-							calculate_target_airspeed(1.3f * _parameters.airspeed_min),
-							eas2tas,
-							math::radians(_parameters.pitch_limit_min),
-							takeoff_pitch_max_rad,
-							_parameters.throttle_min, takeoff_throttle,
-							_parameters.throttle_cruise,
-							true,
-							math::max(math::radians(pos_sp_triplet.current.pitch_min),
-							math::radians(10.0f)),
-							_global_pos.alt,
-							ground_speed,
-							tecs_status_s::TECS_MODE_TAKEOFF,
-							takeoff_pitch_max_deg != _parameters.pitch_limit_max);
+				// assign values
+				_att_sp.roll_body = _runway_takeoff.getRoll(_l1_control.nav_roll());
+				_att_sp.yaw_body = _runway_takeoff.getYaw(_l1_control.nav_bearing());
+				_att_sp.fw_control_yaw = _runway_takeoff.controlYaw();
+				_att_sp.pitch_body = _runway_takeoff.getPitch(_tecs.get_pitch_demand());
 
-					/* limit roll motion to ensure enough lift */
-					_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f),
-							math::radians(15.0f));
+				// reset integrals except yaw (which also counts for the wheel controller)
+				_att_sp.roll_reset_integral = _runway_takeoff.resetIntegrators();
+				_att_sp.pitch_reset_integral = _runway_takeoff.resetIntegrators();
+
+				/*warnx("yaw: %.4f, roll: %.4f, pitch: %.4f", (double)_att_sp.yaw_body,
+					(double)_att_sp.roll_body, (double)_att_sp.pitch_body);*/
 
-				} else {
-					tecs_update_pitch_throttle(_pos_sp_triplet.current.alt,
-							calculate_target_airspeed(_parameters.airspeed_trim),
-							eas2tas,
-							math::radians(_parameters.pitch_limit_min),
-							math::radians(_parameters.pitch_limit_max),
-							_parameters.throttle_min,
-							takeoff_throttle,
-							_parameters.throttle_cruise,
-							false,
-							math::radians(_parameters.pitch_limit_min),
-							_global_pos.alt,
-							ground_speed);
-				}
 			} else {
-				/* Tell the attitude controller to stop integrating while we are waiting
-				 * for the launch */
-				_att_sp.roll_reset_integral = true;
-				_att_sp.pitch_reset_integral = true;
-				_att_sp.yaw_reset_integral = true;
+				/* Perform launch detection */
+				if (launchDetector.launchDetectionEnabled() &&
+					launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
+					/* Inform user that launchdetection is running */
+					static hrt_abstime last_sent = 0;
+					if(hrt_absolute_time() - last_sent > 4e6) {
+						mavlink_log_critical(_mavlink_fd, "#Launchdetection running");
+						last_sent = hrt_absolute_time();
+					}
 
-				/* Set default roll and pitch setpoints during detection phase */
-				_att_sp.roll_body = 0.0f;
-				_att_sp.pitch_body = math::max(math::radians(pos_sp_triplet.current.pitch_min),
-						math::radians(10.0f));
+					/* Detect launch */
+					launchDetector.update(_sensor_combined.accelerometer_m_s2[0]);
+
+					/* update our copy of the launch detection state */
+					launch_detection_state = launchDetector.getLaunchDetected();
+				} else	{
+					/* no takeoff detection --> fly */
+					launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS;
+				}
+
+				/* Set control values depending on the detection state */
+				if (launch_detection_state != LAUNCHDETECTION_RES_NONE) {
+					/* Launch has been detected, hence we have to control the plane. */
+
+					_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
+					_att_sp.roll_body = _l1_control.nav_roll();
+					_att_sp.yaw_body = _l1_control.nav_bearing();
+
+					/* Select throttle: only in LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS we want to use
+					 * full throttle, otherwise we use the preTakeOff Throttle */
+					float takeoff_throttle = launch_detection_state !=
+								LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS ?
+								launchDetector.getThrottlePreTakeoff() : _parameters.throttle_max;
+
+					/* select maximum pitch: the launchdetector may impose another limit for the pitch
+					 * depending on the state of the launch */
+					float takeoff_pitch_max_deg = launchDetector.getPitchMax(_parameters.pitch_limit_max);
+					float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg);
+
+					/* apply minimum pitch and limit roll if target altitude is not within climbout_diff
+					 * meters */
+					if (_parameters.climbout_diff > 0.001f && altitude_error > _parameters.climbout_diff) {
+						/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
+						tecs_update_pitch_throttle(_pos_sp_triplet.current.alt,
+								calculate_target_airspeed(1.3f * _parameters.airspeed_min),
+								eas2tas,
+								math::radians(_parameters.pitch_limit_min),
+								takeoff_pitch_max_rad,
+								_parameters.throttle_min, takeoff_throttle,
+								_parameters.throttle_cruise,
+								true,
+								math::max(math::radians(_pos_sp_triplet.current.pitch_min),
+								math::radians(10.0f)),
+								_global_pos.alt,
+								ground_speed,
+								tecs_status_s::TECS_MODE_TAKEOFF,
+								takeoff_pitch_max_deg != _parameters.pitch_limit_max);
+
+						/* limit roll motion to ensure enough lift */
+						_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f),
+								math::radians(15.0f));
+
+					} else {
+						tecs_update_pitch_throttle(_pos_sp_triplet.current.alt,
+								calculate_target_airspeed(_parameters.airspeed_trim),
+								eas2tas,
+								math::radians(_parameters.pitch_limit_min),
+								math::radians(_parameters.pitch_limit_max),
+								_parameters.throttle_min,
+								takeoff_throttle,
+								_parameters.throttle_cruise,
+								false,
+								math::radians(_parameters.pitch_limit_min),
+								_global_pos.alt,
+								ground_speed);
+					}
+				} else {
+					/* Tell the attitude controller to stop integrating while we are waiting
+					 * for the launch */
+					_att_sp.roll_reset_integral = true;
+					_att_sp.pitch_reset_integral = true;
+					_att_sp.yaw_reset_integral = true;
+
+					/* Set default roll and pitch setpoints during detection phase */
+					_att_sp.roll_body = 0.0f;
+					_att_sp.pitch_body = math::max(math::radians(pos_sp_triplet.current.pitch_min),
+							math::radians(10.0f));
+				}
 			}
 
 		}
@@ -1620,9 +1803,16 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 		/** MANUAL FLIGHT **/
 
-		/* reset hold altitude */
+		// reset hold altitude
 		_hold_alt = _global_pos.alt;
 
+		// reset terrain estimation relevant values
+		_time_started_landing = 0;
+		_time_last_t_alt = 0;
+
+		// reset lading abort state
+		_nav_capabilities.abort_landing = false;
+
 		/* no flight mode applies, do not publish an attitude setpoint */
 		setpoint = false;
 
@@ -1637,15 +1827,26 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 	if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) {
 		/* Set thrust to 0 to minimize damage */
 		_att_sp.thrust = 0.0f;
+
 	} else if (_control_mode_current ==  FW_POSCTRL_MODE_AUTO && // launchdetector only available in auto
 			pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
-			launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
+			launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS &&
+			!_runway_takeoff.runwayTakeoffEnabled()) {
 		 /* making sure again that the correct thrust is used,
 		 * without depending on library calls for safety reasons */
 		_att_sp.thrust = launchDetector.getThrottlePreTakeoff();
+
+	} else if (_control_mode_current ==  FW_POSCTRL_MODE_AUTO &&
+			pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
+			_runway_takeoff.runwayTakeoffEnabled()) {
+		_att_sp.thrust = _runway_takeoff.getThrottle(
+				math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() :
+				_tecs.get_throttle_demand(), throttle_max));
+
 	} else if (_control_mode_current ==  FW_POSCTRL_MODE_AUTO &&
 			pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
 		_att_sp.thrust = 0.0f;
+
 	} else {
 		/* Copy thrust and pitch values from tecs */
 		if (_vehicle_status.condition_landed &&
@@ -1663,9 +1864,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 	/* During a takeoff waypoint while waiting for launch the pitch sp is set
 	 * already (not by tecs) */
-	if (!(_control_mode_current ==  FW_POSCTRL_MODE_AUTO &&
-				pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
-				launch_detection_state == LAUNCHDETECTION_RES_NONE)) {
+	if (!(_control_mode_current ==  FW_POSCTRL_MODE_AUTO && (
+				(pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
+					(launch_detection_state == LAUNCHDETECTION_RES_NONE ||
+					_runway_takeoff.runwayTakeoffEnabled())) ||
+				(pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND &&
+					land_noreturn_vertical))
+	)) {
 		_att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand();
 	}
 
@@ -1825,6 +2030,7 @@ void FixedwingPositionControl::reset_takeoff_state()
 {
 	launch_detection_state = LAUNCHDETECTION_RES_NONE;
 	launchDetector.reset();
+	_runway_takeoff.reset();
 }
 
 void FixedwingPositionControl::reset_landing_state()

src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c

@@ -443,3 +443,15 @@ PARAM_DEFINE_FLOAT(FW_FLARE_PMIN, 2.5f);
  *
  */
 PARAM_DEFINE_FLOAT(FW_FLARE_PMAX, 15.0f);
+
+/**
+ * Takeoff and landing airspeed scale factor
+ *
+ * Multiplying this factor with the minimum airspeed of the plane
+ * gives the target airspeed for takeoff and landing approach.
+ *
+ * @min 1.0
+ * @max 1.5
+ * @group L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_AIRSPD_SCALE, 1.3f);

src/modules/mavlink/mavlink_main.cpp

@@ -1523,6 +1523,9 @@ Mavlink::task_main(int argc, char *argv[])
 			} else if (strcmp(myoptarg, "osd") == 0) {
 				_mode = MAVLINK_MODE_OSD;
 
+			} else if (strcmp(myoptarg, "magic") == 0) {
+				_mode = MAVLINK_MODE_MAGIC;
+
 			} else if (strcmp(myoptarg, "config") == 0) {
 				_mode = MAVLINK_MODE_CONFIG;
 			}
@@ -1709,7 +1712,7 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("SYSTEM_TIME", 1.0f);
 		configure_stream("TIMESYNC", 10.0f);
 		configure_stream("ACTUATOR_CONTROL_TARGET0", 10.0f);
-		/* camera trigger is rate limited at the source, do not limit here */
+		//camera trigger is rate limited at the source, do not limit here
 		configure_stream("CAMERA_TRIGGER", 500.0f);
 		configure_stream("EXTENDED_SYS_STATE", 2.0f);
 		break;
@@ -1729,6 +1732,10 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("EXTENDED_SYS_STATE", 1.0f);
 		break;
 
+	case MAVLINK_MODE_MAGIC:
+		//stream nothing
+		break;
+
 	case MAVLINK_MODE_CONFIG:
 		// Enable a number of interesting streams we want via USB
 		configure_stream("SYS_STATUS", 1.0f);

src/modules/mavlink/mavlink_main.h

@@ -149,6 +149,7 @@ public:
 		MAVLINK_MODE_CUSTOM,
 		MAVLINK_MODE_ONBOARD,
 		MAVLINK_MODE_OSD,
+		MAVLINK_MODE_MAGIC,
 		MAVLINK_MODE_CONFIG
 	};
 

src/modules/navigator/mission_params.c

@@ -52,7 +52,7 @@
  * @unit meters
  * @group Mission
  */
-PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 10.0f);
+PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 2.5f);
 
 /**
  * Enable persistent onboard mission storage

src/modules/navigator/navigator.h

@@ -135,7 +135,7 @@ public:
 	struct vehicle_gps_position_s*	    get_gps_position() { return &_gps_pos; }
 	struct sensor_combined_s*	    get_sensor_combined() { return &_sensor_combined; }
 	struct home_position_s*		    get_home_position() { return &_home_pos; }
-	bool				    home_position_valid() { return _home_position_set; }
+	bool				    home_position_valid() { return (_home_pos.timestamp > 0); }
 	struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; }
 	struct mission_result_s*	    get_mission_result() { return &_mission_result; }
 	struct geofence_result_s*		    get_geofence_result() { return &_geofence_result; }
@@ -205,8 +205,6 @@ private:
 	geofence_result_s				_geofence_result;
 	vehicle_attitude_setpoint_s			_att_sp;
 
-	bool 		_home_position_set;
-
 	bool 		_mission_item_valid;		/**< flags if the current mission item is valid */
 	int		_mission_instance_count;	/**< instance count for the current mission */
 
@@ -257,7 +255,7 @@ private:
 	/**
 	 * Retrieve home position
 	 */
-	void		home_position_update();
+	void		home_position_update(bool force=false);
 
 	/**
 	 * Retreive navigation capabilities

src/modules/navigator/navigator_main.cpp

@@ -126,7 +126,6 @@ Navigator::Navigator() :
 	_pos_sp_triplet{},
 	_mission_result{},
 	_att_sp{},
-	_home_position_set(false),
 	_mission_item_valid(false),
 	_mission_instance_count(0),
 	_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
@@ -206,17 +205,13 @@ Navigator::sensor_combined_update()
 }
 
 void
-Navigator::home_position_update()
+Navigator::home_position_update(bool force)
 {
 	bool updated = false;
 	orb_check(_home_pos_sub, &updated);
 
-	if (updated) {
+	if (updated || force) {
 		orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos);
-
-		if (_home_pos.timestamp > 0) {
-			_home_position_set = true;
-		}
 	}
 }
 
@@ -298,7 +293,7 @@ Navigator::task_main()
 	global_position_update();
 	gps_position_update();
 	sensor_combined_update();
-	home_position_update();
+	home_position_update(true);
 	navigation_capabilities_update();
 	params_update();
 
@@ -408,7 +403,7 @@ Navigator::task_main()
 		if (have_geofence_position_data &&
 			(_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) &&
 			(hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL)) {
-			bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter[0], _home_pos, _home_position_set);
+			bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter[0], _home_pos, home_position_valid());
 			last_geofence_check = hrt_absolute_time();
 			have_geofence_position_data = false;
 
@@ -445,8 +440,15 @@ Navigator::task_main()
 				_can_loiter_at_sp = false;
 				break;
 			case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
-				_pos_sp_triplet_published_invalid_once = false;
-				_navigation_mode = &_mission;
+				if (_nav_caps.abort_landing) {
+					// pos controller aborted landing, requests loiter
+					// above landing waypoint
+					_navigation_mode = &_loiter;
+					_pos_sp_triplet_published_invalid_once = false;
+				} else {
+					_pos_sp_triplet_published_invalid_once = false;
+					_navigation_mode = &_mission;
+				}
 				break;
 			case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
 				_pos_sp_triplet_published_invalid_once = false;
@@ -489,7 +491,7 @@ Navigator::task_main()
 		}
 
 		/* iterate through navigation modes and set active/inactive for each */
-		for(unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) {
+		for (unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) {
 			_navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]);
 		}
 

src/modules/position_estimator_inav/CMakeLists.txt

@@ -31,7 +31,7 @@
 #
 ############################################################################
 if(${OS} STREQUAL "nuttx")
-	list(APPEND MODULE_CFLAGS -Wframe-larger-than=3890)
+	list(APPEND MODULE_CFLAGS -Wframe-larger-than=4000)
 endif()
 px4_add_module(
 	MODULE modules__position_estimator_inav

src/modules/position_estimator_inav/position_estimator_inav_main.c

@@ -586,8 +586,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 					/* set this flag if flow should be accurate according to current velocity and attitude rate estimate */
 					flow_accurate = fabsf(body_v_est[1] / flow_dist - att.rollspeed) < max_flow &&
 							fabsf(body_v_est[0] / flow_dist + att.pitchspeed) < max_flow;
-					//this flag is not working -->
-					flow_accurate = true; //already checked if flow_q > 0.3
 
 
 					/*calculate offset of flow-gyro using already calibrated gyro from autopilot*/
@@ -840,7 +838,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 						} else if (t > ref_init_start + ref_init_delay) {
 							ref_inited = true;
-
+							
 							/* set position estimate to (0, 0, 0), use GPS velocity for XY */
 							x_est[0] = 0.0f;
 							x_est[1] = gps.vel_n_m_s;
@@ -953,9 +951,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		t_prev = t;
 
 		/* increase EPH/EPV on each step */
+		if (eph < 0.000001f) { //get case where eph is 0 -> would stay 0
+			eph = 0.001;
+		}
 		if (eph < max_eph_epv) {
 			eph *= 1.0f + dt;
 		}
+		if (epv < 0.000001f) { //get case where epv is 0 -> would stay 0
+			epv = 0.001;
+		}
 		if (epv < max_eph_epv) {
 			epv += 0.005f * dt;	// add 1m to EPV each 200s (baro drift)
 		}
@@ -968,9 +972,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		bool use_vision_z = vision_valid && params.w_z_vision_p > MIN_VALID_W;
 		/* use MOCAP if it's valid and has a valid weight parameter */
 		bool use_mocap = mocap_valid && params.w_mocap_p > MIN_VALID_W;
+		if(params.disable_mocap) { //disable mocap if fake gps is used
+			use_mocap = false;
+		}
 		/* use flow if it's valid and (accurate or no GPS available) */
 		bool use_flow = flow_valid && (flow_accurate || !use_gps_xy);
 
+		
 		bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || use_vision_xy || use_mocap;
 
 		bool dist_bottom_valid = (t < lidar_valid_time + lidar_valid_timeout);
@@ -1166,7 +1174,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 				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);
-				//mavlink_log_info(mavlink_fd, "w_flow = %2.4f\t w_xy_flow = %2.4f\n", (double)w_flow, (double)params.w_xy_flow);
 			}
 
 			if (use_gps_xy) {
@@ -1264,6 +1271,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				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

@@ -158,10 +158,10 @@ PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_V, 0.0f);
  * Weight (cutoff frequency) for optical flow (velocity) measurements.
  *
  * @min 0.0
- * @max 30.0
+ * @max 10.0
  * @group Position Estimator INAV
  */
-PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 10.0f);
+PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 9.0f);
 
 /**
  * XY axis weight for resetting velocity
@@ -313,6 +313,17 @@ PARAM_DEFINE_FLOAT(INAV_FLOW_DIST_X, 0.0f);
  */
 PARAM_DEFINE_FLOAT(INAV_FLOW_DIST_Y, 0.0f);
 
+/**
+ * Disable mocap (set 0 if using fake gps)
+ *
+ * Disable mocap
+ *
+ * @min 0
+ * @max 1
+ * @group Position Estimator INAV
+ */
+PARAM_DEFINE_FLOAT(INAV_DISAB_MOCAP, 0);
+
 /**
  * Disable vision input
  *
@@ -364,6 +375,7 @@ int inav_parameters_init(struct position_estimator_inav_param_handles *h)
 	h->delay_gps = param_find("INAV_DELAY_GPS");
 	h->flow_module_offset_x = param_find("INAV_FLOW_DIST_X");
 	h->flow_module_offset_y = param_find("INAV_FLOW_DIST_Y");
+	h->disable_mocap = param_find("INAV_DISAB_MOCAP");
 
 	return 0;
 }
@@ -394,6 +406,7 @@ int inav_parameters_update(const struct position_estimator_inav_param_handles *h
 	param_get(h->delay_gps, &(p->delay_gps));
 	param_get(h->flow_module_offset_x, &(p->flow_module_offset_x));
 	param_get(h->flow_module_offset_y, &(p->flow_module_offset_y));
+	param_get(h->disable_mocap, &(p->disable_mocap));
 
 	return 0;
 }

src/modules/position_estimator_inav/position_estimator_inav_params.h

@@ -67,6 +67,7 @@ struct position_estimator_inav_params {
 	float delay_gps;
 	float flow_module_offset_x;
 	float flow_module_offset_y;
+	int32_t disable_mocap;
 };
 
 struct position_estimator_inav_param_handles {
@@ -95,6 +96,7 @@ struct position_estimator_inav_param_handles {
 	param_t delay_gps;
 	param_t flow_module_offset_x;
 	param_t flow_module_offset_y;
+	param_t disable_mocap;
 };
 
 #define CBRK_NO_VISION_KEY	328754

src/modules/sdlog2/sdlog2.c

@@ -425,7 +425,6 @@ int create_log_dir()
 			mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO);
 
 			if (mkdir_ret == 0) {
-				warnx("log dir created: %s", log_dir);
 				break;
 
 			} else if (errno != EEXIST) {
@@ -1677,6 +1676,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_msg.msg_type = LOG_RC_MSG;
 			/* Copy only the first 8 channels of 14 */
 			memcpy(log_msg.body.log_RC.channel, buf.rc.channels, sizeof(log_msg.body.log_RC.channel));
+			log_msg.body.log_RC.rssi = buf.rc.rssi;
 			log_msg.body.log_RC.channel_count = buf.rc.channel_count;
 			log_msg.body.log_RC.signal_lost = buf.rc.signal_lost;
 			LOGBUFFER_WRITE_AND_COUNT(RC);

src/modules/sdlog2/sdlog2_messages.h

@@ -189,6 +189,7 @@ struct log_STAT_s {
 #define LOG_RC_MSG 11
 struct log_RC_s {
 	float channel[8];
+	uint8_t rssi;
 	uint8_t channel_count;
 	uint8_t signal_lost;
 };
@@ -532,7 +533,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(STAT, "BBBfBBf",		"MainState,ArmS,Failsafe,BatRem,BatWarn,Landed,Load"),
 	LOG_FORMAT(VTOL, "f",		"Arsp"),
 	LOG_FORMAT(CTS, "fffffff", "Vx_b,Vy_b,Vz_b,Vinf,P,Q,R"),
-	LOG_FORMAT(RC, "ffffffffBB",		"Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count,SignalLost"),
+	LOG_FORMAT(RC, "ffffffffBBB",		"Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,RSSI,Count,SignalLost"),
 	LOG_FORMAT(OUT0, "ffffffff",		"Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
 	LOG_FORMAT(AIRS, "fff",			"IndSpeed,TrueSpeed,AirTemp"),
 	LOG_FORMAT(ARSP, "fff",			"RollRateSP,PitchRateSP,YawRateSP"),

src/modules/simulator/simulator_mavlink.cpp

@@ -80,7 +80,7 @@ void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg)
 	// for now we only support quadrotors
 	unsigned n = 4;
 
-	if (_vehicle_status.is_rotary_wing) {
+	if (_vehicle_status.is_rotary_wing || _vehicle_status.is_vtol) {
 		for (unsigned i = 0; i < 8; i++) {
 			if (_actuators.output[i] > PWM_LOWEST_MIN / 2) {
 				if (i < n) {
@@ -113,7 +113,7 @@ void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg)
 
 	actuator_msg.time_usec = hrt_absolute_time();
 	actuator_msg.roll_ailerons = out[0];
-	actuator_msg.pitch_elevator = _vehicle_status.is_rotary_wing ? out[1] : -out[1];
+	actuator_msg.pitch_elevator = (_vehicle_status.is_rotary_wing || _vehicle_status.is_vtol)  ? out[1] : -out[1];
 	actuator_msg.yaw_rudder = out[2];
 	actuator_msg.throttle = out[3];
 	actuator_msg.aux1 = out[4];

src/modules/systemlib/circuit_breaker.h

@@ -56,6 +56,7 @@
 #define CBRK_FLIGHTTERM_KEY	121212
 #define CBRK_ENGINEFAIL_KEY	284953
 #define CBRK_GPSFAIL_KEY	240024
+#define CBRK_USB_CHK_KEY	197848
 
 #include <stdbool.h>
 

src/modules/systemlib/circuit_breaker_params.c

@@ -147,3 +147,16 @@ PARAM_DEFINE_INT32(CBRK_GPSFAIL, 240024);
  * @group Circuit Breaker
  */
 PARAM_DEFINE_INT32(CBRK_BUZZER, 0);
+
+/**
+ * Circuit breaker for USB link check
+ *
+ * Setting this parameter to 197848 will disable the USB connected
+ * checks in the commander.
+ * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK
+ *
+ * @min 0
+ * @max 197848
+ * @group Circuit Breaker
+ */
+PARAM_DEFINE_INT32(CBRK_USB_CHK, 0);

src/modules/systemlib/rc_check.c

@@ -52,7 +52,7 @@
 
 #define RC_INPUT_MAP_UNMAPPED 0
 
-int rc_calibration_check(int mavlink_fd)
+int rc_calibration_check(int mavlink_fd, bool report_fail)
 {
 
 	char nbuf[20];
@@ -74,7 +74,8 @@ int rc_calibration_check(int mavlink_fd)
 		param_t map_parm = param_find(rc_map_mandatory[j]);
 
 		if (map_parm == PARAM_INVALID) {
-			mavlink_log_critical(mavlink_fd, "RC ERR: PARAM %s MISSING", rc_map_mandatory[j]);
+			if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: PARAM %s MISSING", rc_map_mandatory[j]); }
+
 			/* give system time to flush error message in case there are more */
 			usleep(100000);
 			map_fail_count++;
@@ -86,14 +87,16 @@ int rc_calibration_check(int mavlink_fd)
 		param_get(map_parm, &mapping);
 
 		if (mapping > RC_INPUT_MAX_CHANNELS) {
-			mavlink_log_critical(mavlink_fd, "RC ERR: %s >= # CHANS", rc_map_mandatory[j]);
+			if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: %s >= # CHANS", rc_map_mandatory[j]); }
+
 			/* give system time to flush error message in case there are more */
 			usleep(100000);
 			map_fail_count++;
 		}
 
 		if (mapping == 0) {
-			mavlink_log_critical(mavlink_fd, "RC ERR: Mandatory %s is unmapped", rc_map_mandatory[j]);
+			if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: Mandatory %s is unmapped", rc_map_mandatory[j]); }
+
 			/* give system time to flush error message in case there are more */
 			usleep(100000);
 			map_fail_count++;
@@ -144,35 +147,44 @@ int rc_calibration_check(int mavlink_fd)
 		/* assert min..center..max ordering */
 		if (param_min < RC_INPUT_LOWEST_MIN_US) {
 			count++;
-			mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_MIN < %u", i + 1, RC_INPUT_LOWEST_MIN_US);
+
+			if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: RC_%d_MIN < %u", i + 1, RC_INPUT_LOWEST_MIN_US); }
+
 			/* give system time to flush error message in case there are more */
 			usleep(100000);
 		}
 
 		if (param_max > RC_INPUT_HIGHEST_MAX_US) {
 			count++;
-			mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_MAX > %u", i + 1, RC_INPUT_HIGHEST_MAX_US);
+
+			if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: RC_%d_MAX > %u", i + 1, RC_INPUT_HIGHEST_MAX_US); }
+
 			/* give system time to flush error message in case there are more */
 			usleep(100000);
 		}
 
 		if (param_trim < param_min) {
 			count++;
-			mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM < MIN (%d/%d)", i + 1, (int)param_trim, (int)param_min);
+
+			if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM < MIN (%d/%d)", i + 1, (int)param_trim, (int)param_min); }
+
 			/* give system time to flush error message in case there are more */
 			usleep(100000);
 		}
 
 		if (param_trim > param_max) {
 			count++;
-			mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM > MAX (%d/%d)", i + 1, (int)param_trim, (int)param_max);
+
+			if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM > MAX (%d/%d)", i + 1, (int)param_trim, (int)param_max); }
+
 			/* give system time to flush error message in case there are more */
 			usleep(100000);
 		}
 
 		/* assert deadzone is sane */
 		if (param_dz > RC_INPUT_MAX_DEADZONE_US) {
-			mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_DZ > %u", i + 1, RC_INPUT_MAX_DEADZONE_US);
+			if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: RC_%d_DZ > %u", i + 1, RC_INPUT_MAX_DEADZONE_US); }
+
 			/* give system time to flush error message in case there are more */
 			usleep(100000);
 			count++;
@@ -187,8 +199,13 @@ int rc_calibration_check(int mavlink_fd)
 
 	if (channels_failed) {
 		sleep(2);
-		mavlink_and_console_log_critical(mavlink_fd, "%d config error%s for %d RC channel%s.", total_fail_count,
-						 (total_fail_count > 1) ? "s" : "", channels_failed, (channels_failed > 1) ? "s" : "");
+
+		if (report_fail) {
+			mavlink_and_console_log_critical(mavlink_fd, "%d config error%s for %d RC channel%s.",
+							 total_fail_count,
+							 (total_fail_count > 1) ? "s" : "", channels_failed, (channels_failed > 1) ? "s" : "");
+		}
+
 		usleep(100000);
 	}
 

src/modules/systemlib/rc_check.h

@@ -47,6 +47,6 @@ __BEGIN_DECLS
  * @return			0 / OK if RC calibration is ok, index + 1 of the first
  *				channel that failed else (so 1 == first channel failed)
  */
-__EXPORT int	rc_calibration_check(int mavlink_fd);
+__EXPORT int	rc_calibration_check(int mavlink_fd, bool report_fail);
 
 __END_DECLS

src/modules/uavcan/CMakeLists.txt

@@ -37,15 +37,12 @@ set(UAVCAN_PLATFORM stm32 CACHE STRING "uavcan platform")
 string(TOUPPER "${OS}" OS_UPPER)
 add_definitions(
 		-DUAVCAN_CPP_VERSION=UAVCAN_CPP03
-		-DUAVCAN_MAX_NETWORK_SIZE_HINT=16
 		-DUAVCAN_MEM_POOL_BLOCK_SIZE=48
 		-DUAVCAN_NO_ASSERTIONS
 		-DUAVCAN_PLATFORM=stm32
 		-DUAVCAN_STM32_${OS_UPPER}=1
 		-DUAVCAN_STM32_NUM_IFACES=2
 		-DUAVCAN_STM32_TIMER_NUMBER=5
-		-DUAVCAN_USE_CPP03=ON
-		-DUAVCAN_USE_EXTERNAL_SNPRINT
 		)
 
 add_subdirectory(libuavcan EXCLUDE_FROM_ALL)

src/modules/uavcan/allocator.hpp

@@ -0,0 +1,73 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @author Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <systemlib/err.h>
+#include <uavcan/uavcan.hpp>
+#include <uavcan/helpers/heap_based_pool_allocator.hpp>
+
+// TODO: Entire UAVCAN application should be moved into a namespace later; this is the first step.
+namespace uavcan_node
+{
+
+struct AllocatorSynchronizer
+{
+	const ::irqstate_t state = ::irqsave();
+	~AllocatorSynchronizer() { ::irqrestore(state); }
+};
+
+struct Allocator : public uavcan::HeapBasedPoolAllocator<uavcan::MemPoolBlockSize, AllocatorSynchronizer>
+{
+	static constexpr unsigned CapacitySoftLimit = 250;
+	static constexpr unsigned CapacityHardLimit = 500;
+
+	Allocator() :
+		uavcan::HeapBasedPoolAllocator<uavcan::MemPoolBlockSize, AllocatorSynchronizer>(CapacitySoftLimit, CapacityHardLimit)
+	{ }
+
+	~Allocator()
+	{
+	        if (getNumAllocatedBlocks() > 0)
+	        {
+	        	warnx("UAVCAN LEAKS MEMORY: %u BLOCKS (%u BYTES) LOST",
+	        		getNumAllocatedBlocks(), getNumAllocatedBlocks() * uavcan::MemPoolBlockSize);
+	        }
+	}
+};
+
+}

src/modules/uavcan/sensors/baro.cpp

@@ -44,7 +44,7 @@ UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode &node) :
 	UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_BASE_DEVICE_PATH, ORB_ID(sensor_baro)),
 	_sub_air_pressure_data(node),
 	_sub_air_temperature_data(node),
-	_reports(nullptr)
+	_reports(2, sizeof(baro_report))
 { }
 
 int UavcanBarometerBridge::init()
@@ -55,13 +55,6 @@ int UavcanBarometerBridge::init()
 		return res;
 	}
 
-	/* allocate basic report buffers */
-	_reports = new ringbuffer::RingBuffer(2, sizeof(baro_report));
-
-	if (_reports == nullptr) {
-		return -1;
-	}
-
 	res = _sub_air_pressure_data.start(AirPressureCbBinder(this, &UavcanBarometerBridge::air_pressure_sub_cb));
 
 	if (res < 0) {
@@ -91,7 +84,7 @@ ssize_t UavcanBarometerBridge::read(struct file *filp, char *buffer, size_t bufl
 	}
 
 	while (count--) {
-		if (_reports->get(baro_buf)) {
+		if (_reports.get(baro_buf)) {
 			ret += sizeof(*baro_buf);
 			baro_buf++;
 		}
@@ -132,7 +125,7 @@ int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg)
 
 			irqstate_t flags = irqsave();
 
-			if (!_reports->resize(arg)) {
+			if (!_reports.resize(arg)) {
 				irqrestore(flags);
 				return -ENOMEM;
 			}
@@ -186,7 +179,7 @@ void UavcanBarometerBridge::air_pressure_sub_cb(const
 	report.altitude = (((std::pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
 
 	// add to the ring buffer
-	_reports->force(&report);
+	_reports.force(&report);
 
 	publish(msg.getSrcNodeID().get(), &report);
 }

src/modules/uavcan/sensors/baro.hpp

@@ -76,8 +76,10 @@ private:
 
 	uavcan::Subscriber<uavcan::equipment::air_data::StaticPressure, AirPressureCbBinder> _sub_air_pressure_data;
 	uavcan::Subscriber<uavcan::equipment::air_data::StaticTemperature, AirTemperatureCbBinder> _sub_air_temperature_data;
+
+	ringbuffer::RingBuffer _reports;
+
 	unsigned _msl_pressure = 101325;
-	ringbuffer::RingBuffer	*_reports;
 	float last_temperature_kelvin = 0.0f;
 
 };

src/modules/uavcan/uavcan_main.cpp

@@ -77,7 +77,7 @@
 UavcanNode *UavcanNode::_instance;
 UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) :
 	CDev("uavcan", UAVCAN_DEVICE_PATH),
-	_node(can_driver, system_clock),
+	_node(can_driver, system_clock, _pool_allocator),
 	_node_mutex(),
 	_esc_controller(_node),
 	_time_sync_master(_node),
@@ -144,10 +144,9 @@ UavcanNode::~UavcanNode()
 		} while (_task != -1);
 	}
 
-	/* clean up the alternate device node */
-	// unregister_driver(PWM_OUTPUT_DEVICE_PATH);
-
-	::close(_armed_sub);
+	(void)::close(_armed_sub);
+	(void)::close(_test_motor_sub);
+	(void)::close(_actuator_direct_sub);
 
 	// Removing the sensor bridges
 	auto br = _sensor_bridges.getHead();
@@ -166,6 +165,10 @@ UavcanNode::~UavcanNode()
 	pthread_mutex_destroy(&_node_mutex);
 	px4_sem_destroy(&_server_command_sem);
 
+	// Is it allowed to delete it like that?
+	if (_mixers != nullptr) {
+		delete _mixers;
+	}
 }
 
 int UavcanNode::getHardwareVersion(uavcan::protocol::HardwareVersion &hwver)
@@ -965,6 +968,8 @@ int UavcanNode::run()
 		}
 	}
 
+	(void)::close(busevent_fd);
+
 	teardown();
 	warnx("exiting.");
 
@@ -1119,6 +1124,33 @@ UavcanNode::print_info()
 
 	(void)pthread_mutex_lock(&_node_mutex);
 
+	// Memory status
+	printf("Pool allocator status:\n");
+	printf("\tCapacity hard/soft: %u/%u blocks\n",
+		_pool_allocator.getBlockCapacityHardLimit(), _pool_allocator.getBlockCapacity());
+	printf("\tReserved:  %u blocks\n", _pool_allocator.getNumReservedBlocks());
+	printf("\tAllocated: %u blocks\n", _pool_allocator.getNumAllocatedBlocks());
+
+	// UAVCAN node perfcounters
+	printf("UAVCAN node status:\n");
+	printf("\tInternal failures: %llu\n", _node.getInternalFailureCount());
+	printf("\tTransfer errors:   %llu\n", _node.getDispatcher().getTransferPerfCounter().getErrorCount());
+	printf("\tRX transfers:      %llu\n", _node.getDispatcher().getTransferPerfCounter().getRxTransferCount());
+	printf("\tTX transfers:      %llu\n", _node.getDispatcher().getTransferPerfCounter().getTxTransferCount());
+
+	// CAN driver status
+	for (unsigned i = 0; i < _node.getDispatcher().getCanIOManager().getCanDriver().getNumIfaces(); i++) {
+		printf("CAN%u status:\n", unsigned(i + 1));
+
+		auto iface = _node.getDispatcher().getCanIOManager().getCanDriver().getIface(i);
+		printf("\tHW errors: %llu\n", iface->getErrorCount());
+
+		auto iface_perf_cnt = _node.getDispatcher().getCanIOManager().getIfacePerfCounters(i);
+		printf("\tIO errors: %llu\n", iface_perf_cnt.errors);
+		printf("\tRX frames: %llu\n", iface_perf_cnt.frames_rx);
+		printf("\tTX frames: %llu\n", iface_perf_cnt.frames_tx);
+	}
+
 	// ESC mixer status
 	printf("ESC actuators control groups: sub: %u / req: %u / fds: %u\n",
 	       (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
@@ -1169,13 +1201,20 @@ UavcanNode::print_info()
 	(void)pthread_mutex_unlock(&_node_mutex);
 }
 
+void UavcanNode::shrink()
+{
+	(void)pthread_mutex_lock(&_node_mutex);
+	_pool_allocator.shrink();
+	(void)pthread_mutex_unlock(&_node_mutex);
+}
+
 /*
  * App entry point
  */
 static void print_usage()
 {
 	warnx("usage: \n"
-	      "\tuavcan {start [fw]|status|stop [all|fw]|arm|disarm|update fw|param [set|get|list|save] nodeid [name] [value]|reset nodeid}");
+	      "\tuavcan {start [fw]|status|stop [all|fw]|shrink|arm|disarm|update fw|param [set|get|list|save] nodeid [name] [value]|reset nodeid}");
 }
 
 extern "C" __EXPORT int uavcan_main(int argc, char *argv[]);
@@ -1251,6 +1290,11 @@ int uavcan_main(int argc, char *argv[])
 		::exit(0);
 	}
 
+	if (!std::strcmp(argv[1], "shrink")) {
+		inst->shrink();
+		::exit(0);
+	}
+
 	if (!std::strcmp(argv[1], "arm")) {
 		inst->arm_actuators(true);
 		::exit(0);

src/modules/uavcan/uavcan_main.hpp

@@ -36,6 +36,7 @@
 #include <px4_config.h>
 
 #include <uavcan_stm32/uavcan_stm32.hpp>
+#include <uavcan/helpers/heap_based_pool_allocator.hpp>
 #include <uavcan/protocol/global_time_sync_master.hpp>
 #include <uavcan/protocol/global_time_sync_slave.hpp>
 #include <uavcan/protocol/param/GetSet.hpp>
@@ -55,6 +56,7 @@
 #include "sensors/sensor_bridge.hpp"
 
 #include "uavcan_servers.hpp"
+#include "allocator.hpp"
 
 /**
  * @file uavcan_main.hpp
@@ -78,9 +80,8 @@ class UavcanNode : public device::CDev
 	static constexpr unsigned FramePerSecond     = MaxBitRatePerSec / bitPerFrame;
 	static constexpr unsigned FramePerMSecond    = ((FramePerSecond / 1000) + 1);
 
-	static constexpr unsigned PollTimeoutMs      = 10;
+	static constexpr unsigned PollTimeoutMs      = 3;
 
-	static constexpr unsigned MemPoolSize = 64 * uavcan::MemPoolBlockSize;
 	/*
 	 * This memory is reserved for uavcan to use for queuing CAN frames.
 	 * At 1Mbit there is approximately one CAN frame every 145 uS.
@@ -97,7 +98,6 @@ class UavcanNode : public device::CDev
 	static constexpr unsigned StackSize          = 1800;
 
 public:
-	typedef uavcan::Node<MemPoolSize> Node;
 	typedef uavcan_stm32::CanInitHelper<RxQueueLenPerIface> CanInitHelper;
 	enum eServerAction {None, Start, Stop, CheckFW , Busy};
 
@@ -109,7 +109,7 @@ public:
 
 	static int	start(uavcan::NodeID node_id, uint32_t bitrate);
 
-	Node		&get_node() { return _node; }
+	uavcan::Node<>	&get_node() { return _node; }
 
 	// TODO: move the actuator mixing stuff into the ESC controller class
 	static int	control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input);
@@ -121,6 +121,8 @@ public:
 
 	void		print_info();
 
+	void		shrink();
+
 	static UavcanNode *instance() { return _instance; }
 	static int         getHardwareVersion(uavcan::protocol::HardwareVersion &hwver);
 	int             fw_server(eServerAction action);
@@ -130,8 +132,8 @@ public:
 	int             set_param(int remote_node_id, const char *name, char *value);
 	int             get_param(int remote_node_id, const char *name);
 	int             reset_node(int remote_node_id);
-private:
 
+private:
 	void		fill_node_info();
 	int		init(uavcan::NodeID node_id);
 	void		node_spin_once();
@@ -167,7 +169,9 @@ private:
 
 	static UavcanNode	*_instance;			///< singleton pointer
 
-	Node			_node;				///< library instance
+	uavcan_node::Allocator _pool_allocator;
+
+	uavcan::Node<>		_node;				///< library instance
 	pthread_mutex_t		_node_mutex;
 	px4_sem_t                   _server_command_sem;
 	UavcanEscController	_esc_controller;

src/modules/uavcan/uavcan_servers.cpp

@@ -85,8 +85,8 @@
 UavcanServers *UavcanServers::_instance;
 UavcanServers::UavcanServers(uavcan::INode &main_node) :
 	_subnode_thread(-1),
-	_vdriver(NumIfaces, uavcan_stm32::SystemClock::instance()),
-	_subnode(_vdriver, uavcan_stm32::SystemClock::instance()),
+	_vdriver(NumIfaces, uavcan_stm32::SystemClock::instance(), main_node.getAllocator(), VirtualIfaceBlockAllocationQuota),
+	_subnode(_vdriver, uavcan_stm32::SystemClock::instance(), main_node.getAllocator()),
 	_main_node(main_node),
 	_tracer(),
 	_storage_backend(),
@@ -141,13 +141,14 @@ int UavcanServers::stop()
 		return -1;
 	}
 
-	_instance = nullptr;
-
-	if (server->_subnode_thread != -1) {
-		pthread_cancel(server->_subnode_thread);
-		pthread_join(server->_subnode_thread, NULL);
+	if (server->_subnode_thread) {
+		warnx("stopping fw srv thread...");
+		server->_subnode_thread_should_exit = true;
+		(void)pthread_join(server->_subnode_thread, NULL);
 	}
 
+	_instance = nullptr;
+
 	server->_main_node.getDispatcher().removeRxFrameListener();
 
 	delete server;
@@ -334,7 +335,7 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
 	memset(_esc_enumeration_ids, 0, sizeof(_esc_enumeration_ids));
 	_esc_enumeration_index = 0;
 
-	while (1) {
+	while (!_subnode_thread_should_exit) {
 
 		if (_check_fw == true) {
 			_check_fw = false;
@@ -554,7 +555,9 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
 		}
 	}
 
-	warnx("exiting.");
+	_subnode_thread_should_exit = false;
+
+	warnx("exiting");
 	return (pthread_addr_t) 0;
 }
 

src/modules/uavcan/uavcan_servers.hpp

@@ -81,24 +81,10 @@ class UavcanServers
 {
 	static constexpr unsigned NumIfaces = 1;  // UAVCAN_STM32_NUM_IFACES
 
-	static constexpr unsigned MemPoolSize = 64 * uavcan::MemPoolBlockSize;
-
-	static constexpr unsigned MaxCanFramesPerTransfer   =  63;
-
-	/**
-	 * This number is based on the worst case max number of frames per interface. With
-	 * MemPoolBlockSize set at 48 this is 6048 Bytes.
-	 *
-	 * The servers can be forced to use the primary interface only, this can be achieved simply by passing
-	 * 1 instead of UAVCAN_STM32_NUM_IFACES into the constructor of the virtual CAN driver.
-	 */
-	static constexpr unsigned QueuePoolSize =
-		(NumIfaces * uavcan::MemPoolBlockSize * MaxCanFramesPerTransfer);
-
 	static constexpr unsigned StackSize  = 6000;
 	static constexpr unsigned Priority  =  120;
 
-	typedef uavcan::SubNode<MemPoolSize> SubNode;
+	static constexpr unsigned VirtualIfaceBlockAllocationQuota =  80;
 
 public:
 	UavcanServers(uavcan::INode &main_node);
@@ -108,7 +94,7 @@ public:
 	static int      start(uavcan::INode &main_node);
 	static int      stop();
 
-	SubNode         &get_node() { return _subnode; }
+	uavcan::SubNode<> &get_node() { return _subnode; }
 
 	static UavcanServers *instance() { return _instance; }
 
@@ -124,6 +110,7 @@ public:
 private:
 	pthread_t         _subnode_thread;
 	pthread_mutex_t   _subnode_mutex;
+	volatile bool     _subnode_thread_should_exit = false;
 
 	int		init();
 
@@ -131,12 +118,10 @@ private:
 
 	static UavcanServers	*_instance;            ///< singleton pointer
 
-	typedef VirtualCanDriver<QueuePoolSize> vCanDriver;
+	VirtualCanDriver _vdriver;
 
-	vCanDriver    _vdriver;
-
-	uavcan::SubNode<MemPoolSize>  _subnode;   ///< library instance
-	uavcan::INode                &_main_node; ///< library instance
+	uavcan::SubNode<>  _subnode;
+	uavcan::INode      &_main_node;
 
 	uavcan_posix::dynamic_node_id_server::FileEventTracer _tracer;
 	uavcan_posix::dynamic_node_id_server::FileStorageBackend _storage_backend;

src/modules/uavcan/uavcan_virtual_can_driver.hpp

@@ -114,6 +114,14 @@ public:
 		uavcan::IsDynamicallyAllocatable<Item>::check();
 	}
 
+	~Queue()
+	{
+		while (!isEmpty())
+		{
+			pop();
+		}
+	}
+
 	bool isEmpty() const { return list_.isEmpty(); }
 
 	/**
@@ -329,11 +337,7 @@ public:
 /**
  * Objects of this class are owned by the sub-node thread.
  * This class does not use heap memory.
- * @tparam SharedMemoryPoolSize         Amount of memory, in bytes, that will be statically allocated for the
- *                                      memory pool that will be shared across all interfaces for RX/TX queues.
- *                                      Typically this value should be no less than 4K per interface.
  */
-template <unsigned SharedMemoryPoolSize>
 class VirtualCanDriver : public uavcan::ICanDriver,
 	public uavcan::IRxFrameListener,
 	public ITxQueueInjector,
@@ -400,9 +404,8 @@ class VirtualCanDriver : public uavcan::ICanDriver,
 		}
 	};
 
-	Event event_;               ///< Used to unblock the select() call when IO happens.
+	Event event_;                     ///< Used to unblock the select() call when IO happens.
 	pthread_mutex_t driver_mutex_;    ///< Shared across all ifaces
-	uavcan::PoolAllocator<SharedMemoryPoolSize, uavcan::MemPoolBlockSize> allocator_;   ///< Shared across all ifaces
 	uavcan::LazyConstructor<VirtualCanIface> ifaces_[uavcan::MaxCanIfaces];
 	const unsigned num_ifaces_;
 	uavcan::ISystemClock &clock_;
@@ -476,7 +479,10 @@ class VirtualCanDriver : public uavcan::ICanDriver,
 	}
 
 public:
-	VirtualCanDriver(unsigned arg_num_ifaces, uavcan::ISystemClock &system_clock) :
+	VirtualCanDriver(unsigned arg_num_ifaces,
+		         uavcan::ISystemClock &system_clock,
+		         uavcan::IPoolAllocator& allocator,
+		         unsigned virtual_iface_block_allocation_quota) :
 		num_ifaces_(arg_num_ifaces),
 		clock_(system_clock)
 	{
@@ -485,15 +491,11 @@ public:
 
 		assert(num_ifaces_ > 0 && num_ifaces_ <= uavcan::MaxCanIfaces);
 
-		const unsigned quota_per_iface = allocator_.getNumBlocks() / num_ifaces_;
-		const unsigned quota_per_queue = quota_per_iface;             // 2x overcommit
-
-		UAVCAN_TRACE("VirtualCanDriver", "Total blocks: %u, quota per queue: %u",
-			     unsigned(allocator_.getNumBlocks()), unsigned(quota_per_queue));
+		const unsigned quota_per_queue = virtual_iface_block_allocation_quota;             // 2x overcommit
 
 		for (unsigned i = 0; i < num_ifaces_; i++) {
-			ifaces_[i].template construct<uavcan::IPoolAllocator &, uavcan::ISystemClock &,
-				pthread_mutex_t &, unsigned>(allocator_, clock_, driver_mutex_, quota_per_queue);
+			ifaces_[i].construct<uavcan::IPoolAllocator&, uavcan::ISystemClock&, pthread_mutex_t&,
+					     unsigned>(allocator, clock_, driver_mutex_, quota_per_queue);
 		}
 	}
 

src/modules/vtol_att_control/standard.cpp

@@ -154,7 +154,7 @@ void Standard::update_vtol_state()
 
 		} else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) {
 			// continue the transition to fw mode while monitoring airspeed for a final switch to fw mode
-			if (_airspeed->true_airspeed_m_s >= _params_standard.airspeed_trans) {
+			if (_airspeed->true_airspeed_m_s >= _params_standard.airspeed_trans || !_armed->armed) {
 				_vtol_schedule.flight_mode = FW_MODE;
 				// we can turn off the multirotor motors now
 				_flag_enable_mc_motors = false;

src/modules/vtol_att_control/tiltrotor.cpp

@@ -205,7 +205,8 @@ void Tiltrotor::update_vtol_state()
 		case TRANSITION_FRONT_P1:
 
 			// check if we have reached airspeed to switch to fw mode
-			if (_airspeed->true_airspeed_m_s >= _params_tiltrotor.airspeed_trans) {
+			// also allow switch if we are not armed for the sake of bench testing
+			if (_airspeed->true_airspeed_m_s >= _params_tiltrotor.airspeed_trans || !_armed->armed) {
 				_vtol_schedule.flight_mode = TRANSITION_FRONT_P2;
 				_vtol_schedule.transition_start = hrt_absolute_time();
 			}

src/modules/vtol_att_control/vtol_att_control_main.cpp

@@ -690,6 +690,7 @@ int vtol_att_control_main(int argc, char *argv[])
 			return 1;
 		}
 
+		return 0;
 	}
 
 	if (!strcmp(argv[1], "stop")) {

src/platforms/posix/drivers/gyrosim/gyrosim.cpp

@@ -99,9 +99,9 @@ using namespace DriverFramework;
 // Product Name	Product Revision
 #define GYROSIMES_REV_C4		0x14
 
-#define GYROSIM_ACCEL_DEFAULT_RATE	1000
+#define GYROSIM_ACCEL_DEFAULT_RATE	400
 
-#define GYROSIM_GYRO_DEFAULT_RATE	1000
+#define GYROSIM_GYRO_DEFAULT_RATE	400
 
 #define GYROSIM_ONE_G			9.80665f
 

src/platforms/posix/main.cpp

@@ -35,6 +35,7 @@
  * Basic shell to execute builtin "apps"
  *
  * @author Mark Charlebois <charlebm@gmail.com>
+ * @auther Roman Bapst <bapstroman@gmail.com>
  */
 
 #include <iostream>
@@ -46,6 +47,7 @@
 #include "apps.h"
 #include "px4_middleware.h"
 #include "DriverFramework.hpp"
+#include <termios.h>
 
 namespace px4
 {
@@ -56,6 +58,8 @@ using namespace std;
 
 typedef int (*px4_main_t)(int argc, char *argv[]);
 
+#define CMD_BUFF_SIZE	100
+
 static bool _ExitFlag = false;
 extern "C" {
 	void _SigIntHandler(int sig_num);
@@ -204,22 +208,82 @@ int main(int argc, char **argv)
 		}
 
 		if (!daemon_mode) {
-			string mystr;
+			string mystr = "";
+			string string_buffer[CMD_BUFF_SIZE];
+			int buf_ptr_write = 0;
+			int buf_ptr_read = 0;
 
 			print_prompt();
 
+			// change input mode so that we can manage shell
+			struct termios term;
+			tcgetattr(0, &term);
+			term.c_lflag &= ~ICANON;
+			term.c_lflag &= ~ECHO;
+			tcsetattr(0, TCSANOW, &term);
+			setbuf(stdin, NULL);
+
 			while (!_ExitFlag) {
 
-				struct pollfd fds;
-				int ret;
-				fds.fd = 0; /* stdin */
-				fds.events = POLLIN;
-				ret = poll(&fds, 1, 100);
+				char c = getchar();
 
-				if (ret > 0) {
-					getline(cin, mystr);
+				switch (c) {
+				case 127:	// backslash
+					if (mystr.length() > 0) {
+						mystr.pop_back();
+						printf("%c[2K", 27);	// clear line
+						cout << (char)13;
+						print_prompt();
+						cout << mystr;
+					}
+
+					break;
+
+				case'\n':	// user hit enter
+					if (buf_ptr_write == CMD_BUFF_SIZE) {
+						buf_ptr_write = 0;
+					}
+
+					string_buffer[buf_ptr_write] = mystr;
+					buf_ptr_write++;
 					process_line(mystr, !daemon_mode);
 					mystr = "";
+					buf_ptr_read = buf_ptr_write;
+					break;
+
+				case '\033': {	// arrow keys
+						c = getchar();	// skip first one, does not have the info
+						c = getchar();
+
+						if (c == 'A') {
+							buf_ptr_read--;
+
+						} else if (c == 'B') {
+							if (buf_ptr_read < buf_ptr_write) {
+								buf_ptr_read++;
+							}
+
+						} else {
+							// TODO: Support editing current line
+						}
+
+						if (buf_ptr_read < 0) {
+							buf_ptr_read = 0;
+						}
+
+						string saved_cmd = string_buffer[buf_ptr_read];
+						printf("%c[2K", 27);
+						cout << (char)13;
+						mystr = saved_cmd;
+						print_prompt();
+						cout << mystr;
+						break;
+					}
+
+				default:	// any other input
+					cout << c;
+					mystr += c;
+					break;
 				}
 			}
 

src/systemcmds/mtd/mtd.c

@@ -176,12 +176,9 @@ struct mtd_dev_s *mtd_partition(FAR struct mtd_dev_s *mtd,
 static void
 ramtron_attach(void)
 {
-	/* find the right spi */
-#ifdef CONFIG_ARCH_BOARD_AEROCORE
-	struct spi_dev_s *spi = up_spiinitialize(4);
-#else
-	struct spi_dev_s *spi = up_spiinitialize(2);
-#endif
+	/* initialize the right spi */
+	struct spi_dev_s *spi = up_spiinitialize(PX4_SPI_BUS_RAMTRON);
+
 	/* this resets the spi bus, set correct bus speed again */
 	SPI_SETFREQUENCY(spi, 10 * 1000 * 1000);
 	SPI_SETBITS(spi, 8);
@@ -269,10 +266,10 @@ mtd_start(char *partition_names[], unsigned n_partitions)
 	}
 
 	if (!attached) {
-#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
-		at24xxx_attach();
-#else
+#ifdef CONFIG_MTD_RAMTRON
 		ramtron_attach();
+#else
+		at24xxx_attach();
 #endif
 	}