Merge branch 'beta' into beta_uavacan

src/drivers/gps/gps.cpp

@@ -310,7 +310,7 @@ GPS::task_main()
 			_report_gps_pos.cog_rad = 0.0f;
 			_report_gps_pos.vel_ned_valid = true;				
 
-			//no time and satellite information simulated
+			/* no time and satellite information simulated */
 
 
 			if (!(_pub_blocked)) {
@@ -351,28 +351,46 @@ GPS::task_main()
 
 			unlock();
 
+			/* the Ashtech driver lies about successful configuration and the
+			 * MTK driver is not well tested, so we really only trust the UBX
+			 * driver for an advance publication
+			 */
 			if (_Helper->configure(_baudrate) == 0) {
 				unlock();
 
-				//Publish initial report that we have access to a GPS
-				//Make sure to clear any stale data in case driver is reset
+				/* reset report */
 				memset(&_report_gps_pos, 0, sizeof(_report_gps_pos));
-				_report_gps_pos.timestamp_position = hrt_absolute_time();
-				_report_gps_pos.timestamp_variance = hrt_absolute_time();
-				_report_gps_pos.timestamp_velocity = hrt_absolute_time();
-				_report_gps_pos.timestamp_time = hrt_absolute_time();
 
-				if (!(_pub_blocked)) {
-					if (_report_gps_pos_pub > 0) {
-						orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
+				if (_mode == GPS_DRIVER_MODE_UBX) {
+					/* Publish initial report that we have access to a GPS,
+					 * but set all critical state fields to indicate we have
+					 * no valid position lock
+					 */
 
-					} else {
-						_report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos);
+					_report_gps_pos.timestamp_time = hrt_absolute_time();
+
+					/* reset the timestamps for data, because we have no data yet */
+					_report_gps_pos.timestamp_position = 0;
+					_report_gps_pos.timestamp_variance = 0;
+					_report_gps_pos.timestamp_velocity = 0;
+
+					/* set a massive variance */
+					_report_gps_pos.eph = 10000.0f;
+					_report_gps_pos.epv = 10000.0f;
+					_report_gps_pos.fix_type = 0;
+
+					if (!(_pub_blocked)) {
+						if (_report_gps_pos_pub > 0) {
+							orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
+
+						} else {
+							_report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos);
+						}
 					}
-				}
 
-				// GPS is obviously detected successfully, reset statistics
-				_Helper->reset_update_rates();
+					/* GPS is obviously detected successfully, reset statistics */
+					_Helper->reset_update_rates();
+				}
 
 				int helper_ret;
 				while ((helper_ret = _Helper->receive(TIMEOUT_5HZ)) > 0 && !_task_should_exit) {

src/drivers/gps/ubx.cpp

@@ -83,7 +83,7 @@
 
 /**** Warning macros, disable to save memory */
 #define UBX_WARN(s, ...)		{warnx(s, ## __VA_ARGS__);}
-
+#define UBX_DEBUG(s, ...)		{/*warnx(s, ## __VA_ARGS__);*/}
 
 UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info) :
 	_fd(fd),
@@ -212,7 +212,7 @@ UBX::configure(unsigned &baudrate)
 	} else {
 		_use_nav_pvt = true;
 	}
-	UBX_WARN("%susing NAV-PVT", _use_nav_pvt ? "" : "not ");
+	UBX_DEBUG("%susing NAV-PVT", _use_nav_pvt ? "" : "not ");
 
 	if (!_use_nav_pvt) {
 		configure_message_rate(UBX_MSG_NAV_TIMEUTC, 5);
@@ -271,9 +271,9 @@ UBX::wait_for_ack(const uint16_t msg, const unsigned timeout, const bool report)
 		ret = 0;	// ACK received ok
 	} else if (report) {
 		if (_ack_state == UBX_ACK_GOT_NAK) {
-			UBX_WARN("ubx msg 0x%04x NAK", SWAP16((unsigned)msg));
+			UBX_DEBUG("ubx msg 0x%04x NAK", SWAP16((unsigned)msg));
 		} else {
-			UBX_WARN("ubx msg 0x%04x ACK timeout", SWAP16((unsigned)msg));
+			UBX_DEBUG("ubx msg 0x%04x ACK timeout", SWAP16((unsigned)msg));
 		}
 	}
 
@@ -566,7 +566,7 @@ UBX::payload_rx_init()
 		break;
 
 	case UBX_RXMSG_DISABLE:	// disable unexpected messages
-		UBX_WARN("ubx msg 0x%04x len %u unexpected", SWAP16((unsigned)_rx_msg), (unsigned)_rx_payload_length);
+		UBX_DEBUG("ubx msg 0x%04x len %u unexpected", SWAP16((unsigned)_rx_msg), (unsigned)_rx_payload_length);
 
 		{
 			hrt_abstime t = hrt_absolute_time();
@@ -574,7 +574,7 @@ UBX::payload_rx_init()
 			if (t > _disable_cmd_last + DISABLE_MSG_INTERVAL) {
 				/* don't attempt for every message to disable, some might not be disabled */
 				_disable_cmd_last = t;
-				UBX_WARN("ubx disabling msg 0x%04x", SWAP16((unsigned)_rx_msg));
+				UBX_DEBUG("ubx disabling msg 0x%04x", SWAP16((unsigned)_rx_msg));
 				configure_message_rate(_rx_msg, 0);
 			}
 		}
@@ -677,16 +677,16 @@ UBX::payload_rx_add_mon_ver(const uint8_t b)
 			// Part 1 complete: decode Part 1 buffer and calculate hash for SW&HW version strings
 			_ubx_version = fnv1_32_str(_buf.payload_rx_mon_ver_part1.swVersion, FNV1_32_INIT);
 			_ubx_version = fnv1_32_str(_buf.payload_rx_mon_ver_part1.hwVersion, _ubx_version);
-			UBX_WARN("VER hash 0x%08x", _ubx_version);
-			UBX_WARN("VER hw  \"%10s\"", _buf.payload_rx_mon_ver_part1.hwVersion);
-			UBX_WARN("VER sw  \"%30s\"", _buf.payload_rx_mon_ver_part1.swVersion);
+			UBX_DEBUG("VER hash 0x%08x", _ubx_version);
+			UBX_DEBUG("VER hw  \"%10s\"", _buf.payload_rx_mon_ver_part1.hwVersion);
+			UBX_DEBUG("VER sw  \"%30s\"", _buf.payload_rx_mon_ver_part1.swVersion);
 		}
 		// fill Part 2 buffer
 		unsigned buf_index = (_rx_payload_index - sizeof(ubx_payload_rx_mon_ver_part1_t)) % sizeof(ubx_payload_rx_mon_ver_part2_t);
 		_buf.raw[buf_index] = b;
 		if (buf_index == sizeof(ubx_payload_rx_mon_ver_part2_t) - 1) {
 			// Part 2 complete: decode Part 2 buffer
-			UBX_WARN("VER ext \" %30s\"", _buf.payload_rx_mon_ver_part2.extension);
+			UBX_DEBUG("VER ext \" %30s\"", _buf.payload_rx_mon_ver_part2.extension);
 		}
 	}
 

src/drivers/mpu6000/mpu6000.cpp

@@ -913,10 +913,14 @@ MPU6000::gyro_self_test()
 
 	/*
 	 * Maximum deviation of 20 degrees, according to
-	 * http://www.invensense.com/mems/gyro/documents/PS-MPU-6000A-00v3.4.pdf
+	 * http://www.farnell.com/datasheets/1788002.pdf
 	 * Section 6.1, initial ZRO tolerance
+	 *
+	 * 20 dps (0.34 rad/s) initial offset
+	 * and 20 dps temperature drift, so 0.34 rad/s * 2
 	 */
-	const float max_offset = 0.34f;
+	const float max_offset = 2.0f * 0.34f;
+
 	/* 30% scale error is chosen to catch completely faulty units but
 	 * to let some slight scale error pass. Requires a rate table or correlation
 	 * with mag rotations + data fit to

src/drivers/px4flow/module.mk

@@ -41,4 +41,6 @@ SRCS		= px4flow.cpp
 
 MAXOPTIMIZATION	 = -Os
 
+MODULE_STACKSIZE = 1200
+
 EXTRACXXFLAGS = -Wno-attributes

src/drivers/px4flow/px4flow.cpp

@@ -602,7 +602,7 @@ bool start_in_progress = false;
 const int START_RETRY_COUNT = 5;
 const int START_RETRY_TIMEOUT = 1000;
 
-void	start();
+int	start();
 void	stop();
 void	test();
 void	reset();
@@ -611,22 +611,26 @@ void	info();
 /**
  * Start the driver.
  */
-void
+int
 start()
 {
 	int fd;
 	
 	/* entry check: */
 	if (start_in_progress) {
-		errx(1, "start in progress");
+		warnx("start already in progress");
+		return 1;
 	}
 	start_in_progress = true;
 
 	if (g_dev != nullptr) {
 		start_in_progress = false;
-		errx(1, "already started");
+		warnx("already started");
+		return 1;
 	}
 
+	warnx("scanning I2C buses for device..");
+
 	int retry_nr = 0;
 	while (1) {
 		const int busses_to_try[] = {
@@ -684,11 +688,12 @@ start()
 
 			/* success! */
 			start_in_progress = false;
-			exit(0);
+			return 0;
 		}
 
 		if (retry_nr < START_RETRY_COUNT) {
-			warnx("PX4FLOW not found on I2C busses. Retrying in %d ms. Giving up in %d retries.", START_RETRY_TIMEOUT, START_RETRY_COUNT - retry_nr);
+			/* lets not be too verbose */
+			// warnx("PX4FLOW not found on I2C busses. Retrying in %d ms. Giving up in %d retries.", START_RETRY_TIMEOUT, START_RETRY_COUNT - retry_nr);
 			usleep(START_RETRY_TIMEOUT * 1000);
 			retry_nr++;
 		} else {
@@ -702,7 +707,7 @@ start()
 	}
 	
 	start_in_progress = false;
-	errx(1, "PX4FLOW could not be started over I2C");
+	return 1;
 }
 
 /**
@@ -852,7 +857,7 @@ px4flow_main(int argc, char *argv[])
 	 * Start/load the driver.
 	 */
 	if (!strcmp(argv[1], "start")) {
-		px4flow::start();
+		return px4flow::start();
 	}
 
 	/*

src/drivers/px4fmu/fmu.cpp

@@ -165,7 +165,7 @@ private:
 	unsigned	_num_failsafe_set;
 	unsigned	_num_disarmed_set;
 
-	static bool	arm_nothrottle() { return (_armed.ready_to_arm && !_armed.armed); }
+	static bool	arm_nothrottle() { return (_armed.prearmed && !_armed.armed); }
 
 	static void	task_main_trampoline(int argc, char *argv[]);
 	void		task_main();
@@ -295,7 +295,8 @@ PX4FMU::PX4FMU() :
 	memset(_controls, 0, sizeof(_controls));
 	memset(_poll_fds, 0, sizeof(_poll_fds));
 
-	_debug_enabled = true;
+	/* only enable this during development */
+	_debug_enabled = false;
 }
 
 PX4FMU::~PX4FMU()
@@ -342,9 +343,9 @@ PX4FMU::init()
 	_class_instance = register_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH);
 
 	if (_class_instance == CLASS_DEVICE_PRIMARY) {
-		log("default PWM output device");
+		/* lets not be too verbose */
 	} else if (_class_instance < 0) {
-		log("FAILED registering class device");
+		warnx("FAILED registering class device");
 	}
 
 	/* reset GPIOs */
@@ -536,11 +537,11 @@ PX4FMU::subscribe()
 	_poll_fds_num = 0;
 	for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
 		if (sub_groups & (1 << i)) {
-			warnx("subscribe to actuator_controls_%d", i);
+			debug("subscribe to actuator_controls_%d", i);
 			_control_subs[i] = orb_subscribe(_control_topics[i]);
 		}
 		if (unsub_groups & (1 << i)) {
-			warnx("unsubscribe from actuator_controls_%d", i);
+			debug("unsubscribe from actuator_controls_%d", i);
 			::close(_control_subs[i]);
 			_control_subs[i] = -1;
 		}
@@ -733,7 +734,7 @@ PX4FMU::task_main()
 			orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
 
 			/* update the armed status and check that we're not locked down */
-			bool set_armed = (_armed.armed || _armed.ready_to_arm) && !_armed.lockdown;
+			bool set_armed = (_armed.armed || _armed.prearmed) && !_armed.lockdown;
 
 			if (_servo_armed != set_armed) {
 				_servo_armed = set_armed;

src/drivers/px4io/px4io.cpp

@@ -1116,25 +1116,42 @@ PX4IO::task_main()
 				(void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_REVERSE, pwm_invert_mask);
 
 				float trim_val;
-				param_t trim_parm;
+				param_t parm_handle;
 
-				trim_parm = param_find("TRIM_ROLL");
-				if (trim_parm != PARAM_INVALID) {
-					param_get(trim_parm, &trim_val);
+				parm_handle = param_find("TRIM_ROLL");
+				if (parm_handle != PARAM_INVALID) {
+					param_get(parm_handle, &trim_val);
 					(void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_ROLL, FLOAT_TO_REG(trim_val));
 				}
 
-				trim_parm = param_find("TRIM_PITCH");
-				if (trim_parm != PARAM_INVALID) {
-					param_get(trim_parm, &trim_val);
+				parm_handle = param_find("TRIM_PITCH");
+				if (parm_handle != PARAM_INVALID) {
+					param_get(parm_handle, &trim_val);
 					(void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_PITCH, FLOAT_TO_REG(trim_val));
 				}
 
-				trim_parm = param_find("TRIM_YAW");
-				if (trim_parm != PARAM_INVALID) {
-					param_get(trim_parm, &trim_val);
+				parm_handle = param_find("TRIM_YAW");
+				if (parm_handle != PARAM_INVALID) {
+					param_get(parm_handle, &trim_val);
 					(void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_YAW, FLOAT_TO_REG(trim_val));
 				}
+
+				/* S.BUS output */
+				int sbus_mode;
+				parm_handle = param_find("PWM_SBUS_MODE");
+				if (parm_handle != PARAM_INVALID) {
+					param_get(parm_handle, &sbus_mode);
+					if (sbus_mode == 1) {
+						/* enable S.BUS 1 */
+						(void)io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS1_OUT);
+					} else if (sbus_mode == 2) {
+						/* enable S.BUS 2 */
+						(void)io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
+					} else {
+						/* disable S.BUS */
+						(void)io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT), 0);
+					}
+				}
 			}
 
 		}

src/drivers/px4io/px4io_params.c

@@ -129,3 +129,14 @@ PARAM_DEFINE_INT32(PWM_MAIN_REV7, 0);
  * @group PWM Outputs
  */
 PARAM_DEFINE_INT32(PWM_MAIN_REV8, 0);
+
+/**
+ * Enable S.BUS out
+ *
+ * Set to 1 to enable S.BUS version 1 output instead of RSSI.
+ *
+ * @min 0
+ * @max 1
+ * @group PWM Outputs
+ */
+PARAM_DEFINE_INT32(PWM_SBUS_MODE, 0);

src/lib/external_lgpl/tecs/tecs.cpp

@@ -27,7 +27,8 @@ using namespace math;
  *
  */
 
-void TECS::update_50hz(float baro_altitude, float airspeed, const math::Matrix<3,3> &rotMat, const math::Vector<3> &accel_body, const math::Vector<3> &accel_earth)
+void TECS::update_state(float baro_altitude, float airspeed, const math::Matrix<3,3> &rotMat,
+	const math::Vector<3> &accel_body, const math::Vector<3> &accel_earth, bool altitude_lock, bool in_air)
 {
 	// Implement third order complementary filter for height and height rate
 	// estimted height rate = _integ2_state
@@ -45,17 +46,29 @@ void TECS::update_50hz(float baro_altitude, float airspeed, const math::Matrix<3
 	// 	DT, baro_altitude, airspeed, rotMat(0, 0), rotMat(1, 1), accel_body(0), accel_body(1), accel_body(2),
 	// 	accel_earth(0), accel_earth(1), accel_earth(2));
 
-	if (DT > 1.0f) {
+	bool reset_altitude = false;
+
+	if (_update_50hz_last_usec == 0 || DT > DT_MAX) {
+		DT            = 0.02f; // when first starting TECS, use a
+		// small time constant
+		reset_altitude = true;
+	}
+
+	if (!altitude_lock || !in_air) {
+		reset_altitude = true;
+	}
+
+	if (reset_altitude) {
 		_integ3_state = baro_altitude;
 		_integ2_state = 0.0f;
 		_integ1_state = 0.0f;
-		DT            = 0.02f; // when first starting TECS, use a
-		// small time constant
 	}
 
 	_update_50hz_last_usec = now;
 	_EAS = airspeed;
 
+	_in_air = in_air;
+
 	// Get height acceleration
 	float hgt_ddot_mea = -(accel_earth(2) + CONSTANTS_ONE_G);
 	// Perform filter calculation using backwards Euler integration
@@ -70,7 +83,7 @@ void TECS::update_50hz(float baro_altitude, float airspeed, const math::Matrix<3
 
 	// If more than 1 second has elapsed since last update then reset the integrator state
 	// to the measured height
-	if (DT > 1.0f) {
+	if (reset_altitude) {
 		_integ3_state = baro_altitude;
 
 	} else {
@@ -95,6 +108,10 @@ void TECS::update_50hz(float baro_altitude, float airspeed, const math::Matrix<3
 		_vel_dot = 0.0f;
 	}
 
+	if (!_in_air) {
+		_states_initalized = false;
+	}
+
 }
 
 void TECS::_update_speed(float airspeed_demand, float indicated_airspeed,
@@ -103,7 +120,6 @@ void TECS::_update_speed(float airspeed_demand, float indicated_airspeed,
 	// Calculate time in seconds since last update
 	uint64_t now = ecl_absolute_time();
 	float DT = max((now - _update_speed_last_usec), 0ULL) * 1.0e-6f;
-	_update_speed_last_usec = now;
 
 	// Convert equivalent airspeeds to true airspeeds
 
@@ -113,7 +129,7 @@ void TECS::_update_speed(float airspeed_demand, float indicated_airspeed,
 	_TASmin   = indicated_airspeed_min * EAS2TAS;
 
 	// Reset states of time since last update is too large
-	if (DT > 1.0f) {
+	if (_update_speed_last_usec == 0 || DT > 1.0f || !_in_air) {
 		_integ5_state = (_EAS * EAS2TAS);
 		_integ4_state = 0.0f;
 		DT            = 0.1f; // when first starting TECS, use a
@@ -146,7 +162,7 @@ void TECS::_update_speed(float airspeed_demand, float indicated_airspeed,
 	_integ5_state = _integ5_state + integ5_input * DT;
 	// limit the airspeed to a minimum of 3 m/s
 	_integ5_state = max(_integ5_state, 3.0f);
-
+	_update_speed_last_usec = now;
 }
 
 void TECS::_update_speed_demand(void)
@@ -165,40 +181,38 @@ void TECS::_update_speed_demand(void)
 
 	// calculate velocity rate limits based on physical performance limits
 	// provision to use a different rate limit if bad descent or underspeed condition exists
-	// Use 50% of maximum energy rate to allow margin for total energy contgroller
-//	float velRateMax;
-//	float velRateMin;
-//
-//	if ((_badDescent) || (_underspeed)) {
-//		velRateMax = 0.5f * _STEdot_max / _integ5_state;
-//		velRateMin = 0.5f * _STEdot_min / _integ5_state;
-//
-//	} else {
-//		velRateMax = 0.5f * _STEdot_max / _integ5_state;
-//		velRateMin = 0.5f * _STEdot_min / _integ5_state;
-//	}
-//
+	// Use 50% of maximum energy rate to allow margin for total energy controller
+	float velRateMax;
+	float velRateMin;
+
+	if ((_badDescent) || (_underspeed)) {
+		velRateMax = 0.5f * _STEdot_max / _integ5_state;
+		velRateMin = 0.5f * _STEdot_min / _integ5_state;
+
+	} else {
+		velRateMax = 0.5f * _STEdot_max / _integ5_state;
+		velRateMin = 0.5f * _STEdot_min / _integ5_state;
+	}
+
 //	// Apply rate limit
-//	if ((_TAS_dem - _TAS_dem_adj) > (velRateMax * 0.1f)) {
-//		_TAS_dem_adj = _TAS_dem_adj + velRateMax * 0.1f;
+//	if ((_TAS_dem - _TAS_dem_adj) > (velRateMax * _DT)) {
+//		_TAS_dem_adj = _TAS_dem_adj + velRateMax * _DT;
 //		_TAS_rate_dem = velRateMax;
 //
-//	} else if ((_TAS_dem - _TAS_dem_adj) < (velRateMin * 0.1f)) {
-//		_TAS_dem_adj = _TAS_dem_adj + velRateMin * 0.1f;
+//	} else if ((_TAS_dem - _TAS_dem_adj) < (velRateMin * _DT)) {
+//		_TAS_dem_adj = _TAS_dem_adj + velRateMin * _DT;
 //		_TAS_rate_dem = velRateMin;
 //
 //	} else {
 //		_TAS_dem_adj = _TAS_dem;
 //
 //
-//	_TAS_rate_dem = (_TAS_dem - _TAS_dem_last) / 0.1f;
+//	_TAS_rate_dem = (_TAS_dem - _TAS_dem_last) / _DT;
 //	}
 
-	_TAS_dem_adj = _TAS_dem;
-	_TAS_rate_dem = (_TAS_dem_adj-_integ5_state)*_speedrate_p; //xxx: using a p loop for now
+	_TAS_dem_adj = constrain(_TAS_dem, _TASmin, _TASmax);;
+	_TAS_rate_dem = constrain((_TAS_dem_adj - _integ5_state) * _speedrate_p, velRateMin, velRateMax); //xxx: using a p loop for now
 
-	// Constrain speed demand again to protect against bad values on initialisation.
-	_TAS_dem_adj = constrain(_TAS_dem_adj, _TASmin, _TASmax);
 //	_TAS_dem_last = _TAS_dem;
 
 //	warnx("_TAS_rate_dem: %.1f, _TAS_dem_adj %.1f, _integ5_state %.1f, _badDescent %u , _underspeed %u, velRateMin %.1f",
@@ -209,23 +223,29 @@ void TECS::_update_speed_demand(void)
 
 void TECS::_update_height_demand(float demand, float state)
 {
-//	// Apply 2 point moving average to demanded height
-//	// This is required because height demand is only updated at 5Hz
-//	_hgt_dem = 0.5f * (demand + _hgt_dem_in_old);
-//	_hgt_dem_in_old = _hgt_dem;
-//
-//	// printf("hgt_dem: %6.2f hgt_dem_last: %6.2f max_climb_rate: %6.2f\n", _hgt_dem, _hgt_dem_prev,
-//	// 	_maxClimbRate);
-//
-//	// Limit height rate of change
-//	if ((_hgt_dem - _hgt_dem_prev) > (_maxClimbRate * 0.1f)) {
-//		_hgt_dem = _hgt_dem_prev + _maxClimbRate * 0.1f;
-//
-//	} else if ((_hgt_dem - _hgt_dem_prev) < (-_maxSinkRate * 0.1f)) {
-//		_hgt_dem = _hgt_dem_prev - _maxSinkRate * 0.1f;
-//	}
-//
-//	_hgt_dem_prev = _hgt_dem;
+	// Handle initialization
+	if (isfinite(demand) && fabsf(_hgt_dem_in_old) < 0.1f) {
+		_hgt_dem_in_old = demand;
+	}
+	// Apply 2 point moving average to demanded height
+	// This is required because height demand is updated in steps
+	if (isfinite(demand)) {
+		_hgt_dem = 0.5f * (demand + _hgt_dem_in_old);
+	} else {
+		_hgt_dem = _hgt_dem_in_old;
+	}
+	_hgt_dem_in_old = _hgt_dem;
+
+	// Limit height demand
+	// this is important to avoid a windup
+	if ((_hgt_dem - _hgt_dem_prev) > (_maxClimbRate * _DT)) {
+		_hgt_dem = _hgt_dem_prev + _maxClimbRate * _DT;
+
+	} else if ((_hgt_dem - _hgt_dem_prev) < (-_maxSinkRate * _DT)) {
+		_hgt_dem = _hgt_dem_prev - _maxSinkRate * _DT;
+	}
+
+	_hgt_dem_prev = _hgt_dem;
 //
 //	// Apply first order lag to height demand
 //	_hgt_dem_adj = 0.05f * _hgt_dem + 0.95f * _hgt_dem_adj_last;
@@ -235,9 +255,9 @@ void TECS::_update_height_demand(float demand, float state)
 //	// printf("hgt_dem: %6.2f hgt_dem_adj: %6.2f hgt_dem_last: %6.2f hgt_rate_dem: %6.2f\n", _hgt_dem, _hgt_dem_adj, _hgt_dem_adj_last,
 //	// 	_hgt_rate_dem);
 
-	_hgt_dem_adj = demand;//0.025f * demand + 0.975f * _hgt_dem_adj_last;
-	_hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p + _heightrate_ff * (_hgt_dem_adj - _hgt_dem_adj_last)/_DT;
+	_hgt_dem_adj = 0.1f * _hgt_dem + 0.9f * _hgt_dem_adj_last;
 	_hgt_dem_adj_last = _hgt_dem_adj;
+	_hgt_rate_dem = (_hgt_dem_adj - state) * _heightrate_p + _heightrate_ff * (_hgt_dem_adj - _hgt_dem_adj_last) / _DT;
 
 	// Limit height rate of change
 	if (_hgt_rate_dem > _maxClimbRate) {
@@ -471,7 +491,7 @@ void TECS::_update_pitch(void)
 void TECS::_initialise_states(float pitch, float throttle_cruise, float baro_altitude, float ptchMinCO_rad)
 {
 	// Initialise states and variables if DT > 1 second or in climbout
-	if (_DT > 1.0f) {
+	if (_update_pitch_throttle_last_usec == 0 || _DT > 1.0f || !_in_air || !_states_initalized) {
 		_integ6_state      = 0.0f;
 		_integ7_state      = 0.0f;
 		_last_throttle_dem = throttle_cruise;
@@ -484,8 +504,10 @@ void TECS::_initialise_states(float pitch, float throttle_cruise, float baro_alt
 		_TAS_dem_adj       = _TAS_dem;
 		_underspeed        = false;
 		_badDescent        = false;
-		_DT                = 0.1f; // when first starting TECS, use a
-		// small time constant
+
+		if (_DT > 1.0f || _DT < 0.001f) {
+			_DT                = DT_MIN;
+		}
 
 	} else if (_climbOutDem) {
 		_PITCHminf          = ptchMinCO_rad;
@@ -498,6 +520,8 @@ void TECS::_initialise_states(float pitch, float throttle_cruise, float baro_alt
 		_underspeed        = false;
 		_badDescent 	   = false;
 	}
+
+	_states_initalized = true;
 }
 
 void TECS::_update_STE_rate_lim(void)
@@ -512,17 +536,14 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f
 				 float throttle_min, float throttle_max, float throttle_cruise,
 				 float pitch_limit_min, float pitch_limit_max)
 {
+
 	// Calculate time in seconds since last update
 	uint64_t now = ecl_absolute_time();
 	_DT = max((now - _update_pitch_throttle_last_usec), 0ULL) * 1.0e-6f;
-	_update_pitch_throttle_last_usec = now;
 
 	// printf("tecs in: dt:%10.6f pitch: %6.2f baro_alt: %6.2f alt sp: %6.2f\neas sp: %6.2f eas: %6.2f, eas2tas: %6.2f\n %s pitch min C0: %6.2f thr min: %6.2f, thr max: %6.2f thr cruis: %6.2f pt min: %6.2f, pt max: %6.2f\n",
 	// 	_DT, pitch, baro_altitude, hgt_dem, EAS_dem, indicated_airspeed, EAS2TAS, (climbOutDem) ? "climb" : "level", ptchMinCO, throttle_min, throttle_max, throttle_cruise, pitch_limit_min, pitch_limit_max);
 
-	// Update the speed estimate using a 2nd order complementary filter
-	_update_speed(EAS_dem, indicated_airspeed, _indicated_airspeed_min, _indicated_airspeed_max, EAS2TAS);
-
 	// Convert inputs
 	_THRmaxf  = throttle_max;
 	_THRminf  = throttle_min;
@@ -533,6 +554,13 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f
 	// initialise selected states and variables if DT > 1 second or in climbout
 	_initialise_states(pitch, throttle_cruise, baro_altitude, ptchMinCO);
 
+	if (!_in_air) {
+		return;
+	}
+
+	// Update the speed estimate using a 2nd order complementary filter
+	_update_speed(EAS_dem, indicated_airspeed, _indicated_airspeed_min, _indicated_airspeed_max, EAS2TAS);
+
 	// Calculate Specific Total Energy Rate Limits
 	_update_STE_rate_lim();
 
@@ -583,4 +611,6 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f
 	_tecs_state.ptch     = _pitch_dem;
 	_tecs_state.dspd_dem = _TAS_rate_dem;
 
+	_update_pitch_throttle_last_usec = now;
+
 }

src/lib/external_lgpl/tecs/tecs.h

@@ -80,6 +80,8 @@ public:
 		_SPEdot(0.0f),
 		_SKEdot(0.0f),
 		_airspeed_enabled(false),
+		_states_initalized(false),
+		_in_air(false),
 		_throttle_slewrate(0.0f)
 	{
 	}
@@ -95,7 +97,8 @@ public:
 	// Update of the estimated height and height rate internal state
 	// Update of the inertial speed rate internal state
 	// Should be called at 50Hz or greater
-	void update_50hz(float baro_altitude, float airspeed, const math::Matrix<3,3> &rotMat, const math::Vector<3> &accel_body, const math::Vector<3> &accel_earth);
+	void update_state(float baro_altitude, float airspeed, const math::Matrix<3,3> &rotMat,
+		const math::Vector<3> &accel_body, const math::Vector<3> &accel_earth, bool altitude_lock, bool in_air);
 
 	// Update the control loop calculations
 	void update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO,
@@ -376,7 +379,12 @@ private:
 	// Time since last update of main TECS loop (seconds)
 	float _DT;
 
+	static constexpr float DT_MIN = 0.1f;
+	static constexpr float DT_MAX = 1.0f;
+
 	bool _airspeed_enabled;
+	bool _states_initalized;
+	bool _in_air;
 	float _throttle_slewrate;
 	float _indicated_airspeed_min;
 	float _indicated_airspeed_max;

src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp

@@ -205,7 +205,7 @@ int AttitudeEstimatorQ::start() {
 	_control_task = task_spawn_cmd("attitude_estimator_q",
 				       SCHED_DEFAULT,
 				       SCHED_PRIORITY_MAX - 5,
-				       2500,
+				       2000,
 				       (main_t)&AttitudeEstimatorQ::task_main_trampoline,
 				       nullptr);
 

src/modules/commander/accelerometer_calibration.cpp

@@ -559,6 +559,7 @@ int do_level_calibration(int mavlink_fd) {
 	const unsigned cal_time = 5;
 	const unsigned cal_hz = 100;
 	const unsigned settle_time = 30;
+	bool success = false;
 	int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
 	struct vehicle_attitude_s att;
 	memset(&att, 0, sizeof(att));
@@ -597,7 +598,15 @@ int do_level_calibration(int mavlink_fd) {
 	start = hrt_absolute_time();
 	// average attitude for 5 seconds
 	while(hrt_elapsed_time(&start) < cal_time * 1000000) {
-		poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
+		int pollret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
+
+		if (pollret <= 0) {
+			// attitude estimator is not running
+			mavlink_and_console_log_critical(mavlink_fd, "attitude estimator not running - check system boot");
+			mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "level");
+			goto out;
+		}
+
 		orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
 		roll_mean += att.roll;
 		pitch_mean += att.pitch;
@@ -606,7 +615,6 @@ int do_level_calibration(int mavlink_fd) {
 
 	mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100);
 
-	bool success = false;
 	if (counter > (cal_time * cal_hz / 2 )) {
 		roll_mean /= counter;
 		pitch_mean /= counter;

src/modules/commander/airspeed_calibration.cpp

@@ -72,12 +72,12 @@ int do_airspeed_calibration(int mavlink_fd)
 {
 	int result = OK;
 	unsigned calibration_counter = 0;
-	const unsigned maxcount = 3000;
+	const unsigned maxcount = 2400;
 
 	/* give directions */
 	mavlink_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
 
-	const unsigned calibration_count = 2000;
+	const unsigned calibration_count = (maxcount * 2) / 3;
 
 	int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
 	struct differential_pressure_s diff_pres;
@@ -201,10 +201,10 @@ int do_airspeed_calibration(int mavlink_fd)
 	/* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */
 	while (calibration_counter < maxcount) {
 
-        if (calibrate_cancel_check(mavlink_fd, cancel_sub)) {
-		goto error_return;
-        }
-        
+		if (calibrate_cancel_check(mavlink_fd, cancel_sub)) {
+			goto error_return;
+		}
+
 		/* wait blocking for new data */
 		struct pollfd fds[1];
 		fds[0].fd = diff_pres_sub;

src/modules/commander/calibration_routines.cpp

@@ -469,8 +469,8 @@ calibrate_return calibrate_from_orientation(int		mavlink_fd,
 		/* inform user about already handled side */
 		if (side_data_collected[orient]) {
 			orientation_failures++;
-			mavlink_and_console_log_info(mavlink_fd, "[cal] %s side completed or not needed", detect_orientation_str(orient));
-			mavlink_and_console_log_info(mavlink_fd, "[cal] rotate to a pending side");
+			mavlink_and_console_log_critical(mavlink_fd, "%s side already completed", detect_orientation_str(orient));
+			mavlink_and_console_log_critical(mavlink_fd, "rotate to a pending side");
 			continue;
 		}
 		

src/modules/commander/commander.cpp

@@ -275,7 +275,7 @@ int commander_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "start")) {
 
 		if (thread_running) {
-			warnx("commander already running");
+			warnx("already running");
 			/* this is not an error */
 			exit(0);
 		}
@@ -288,11 +288,18 @@ int commander_main(int argc, char *argv[])
 					     commander_thread_main,
 					     (argv) ? (char * const *)&argv[2] : (char * const *)NULL);
 
-		while (!thread_running) {
-			usleep(200);
+		unsigned constexpr max_wait_us = 1000000;
+		unsigned constexpr max_wait_steps = 2000;
+
+		unsigned i;
+		for (i = 0; i < max_wait_steps; i++) {
+			usleep(max_wait_us / max_wait_steps);
+			if (thread_running) {
+				break;
+			}
 		}
 
-		exit(0);
+		exit(!(i < max_wait_steps));
 	}
 
 	if (!strcmp(argv[1], "stop")) {
@@ -870,6 +877,8 @@ int commander_thread_main(int argc, char *argv[])
 	param_t _param_autostart_id = param_find("SYS_AUTOSTART");
 	param_t _param_autosave_params = param_find("COM_AUTOS_PAR");
 	param_t _param_rc_in_off = param_find("COM_RC_IN_MODE");
+	param_t _param_eph = param_find("COM_HOME_H_T");
+	param_t _param_epv = param_find("COM_HOME_V_T");
 
 	const char *main_states_str[vehicle_status_s::MAIN_STATE_MAX];
 	main_states_str[vehicle_status_s::MAIN_STATE_MANUAL]			= "MANUAL";
@@ -1293,6 +1302,10 @@ int commander_thread_main(int argc, char *argv[])
 
 			/* Parameter autosave setting */
 			param_get(_param_autosave_params, &autosave_params);
+
+			/* EPH / EPV */
+			param_get(_param_eph, &eph_threshold);
+			param_get(_param_epv, &epv_threshold);
 		}
 
 		/* Set flag to autosave parameters if necessary */
@@ -1342,10 +1355,15 @@ int commander_thread_main(int argc, char *argv[])
 				orb_copy(telemetry_status_orb_id[i], telemetry_subs[i], &telemetry);
 
 				/* perform system checks when new telemetry link connected */
-				if (mavlink_fd &&
-				    telemetry_last_heartbeat[i] == 0 &&
+				if (
+				    /* we actually have a way to talk to the user */
+				    mavlink_fd &&
+				    /* we first connect a link or re-connect a link after loosing it */
+				    (telemetry_last_heartbeat[i] == 0 || (hrt_elapsed_time(&telemetry_last_heartbeat[i]) > 3 * 1000 * 1000)) &&
+				    /* and this link has a communication partner */
 				    telemetry.heartbeat_time > 0 &&
-				    hrt_elapsed_time(&telemetry.heartbeat_time) < datalink_loss_timeout * 1e6) {
+				    /* and the system is not already armed (and potentially flying) */
+				    !armed.armed) {
 
 				    	bool chAirspeed = false;
 					/* Perform airspeed check only if circuit breaker is not
@@ -1817,7 +1835,7 @@ int commander_thread_main(int argc, char *argv[])
 
 			} else {
 				if (status.rc_signal_lost) {
-					mavlink_log_critical(mavlink_fd, "RC SIGNAL REGAINED after %llums",
+					mavlink_log_info(mavlink_fd, "RC SIGNAL REGAINED after %llums",
 							     (hrt_absolute_time() - status.rc_signal_lost_timestamp) / 1000);
 					status_changed = true;
 				}
@@ -1944,7 +1962,7 @@ int commander_thread_main(int argc, char *argv[])
 
 					/* only report a regain */
 					if (telemetry_last_dl_loss[i] > 0) {
-						mavlink_and_console_log_critical(mavlink_fd, "data link #%i regained", i);
+						mavlink_and_console_log_info(mavlink_fd, "data link #%i regained", i);
 					}
 
 					telemetry_lost[i] = false;
@@ -1962,7 +1980,7 @@ int commander_thread_main(int argc, char *argv[])
 					/* only reset the timestamp to a different time on state change */
 					telemetry_last_dl_loss[i]  = hrt_absolute_time();
 
-					mavlink_and_console_log_critical(mavlink_fd, "data link #%i lost", i);
+					mavlink_and_console_log_info(mavlink_fd, "data link #%i lost", i);
 					telemetry_lost[i] = true;
 				}
 			}
@@ -1977,7 +1995,9 @@ int commander_thread_main(int argc, char *argv[])
 
 		} else {
 			if (!status.data_link_lost) {
-				mavlink_and_console_log_critical(mavlink_fd, "ALL DATA LINKS LOST");
+				if (armed.armed) {
+					mavlink_and_console_log_critical(mavlink_fd, "ALL DATA LINKS LOST");
+				}
 				status.data_link_lost = true;
 				status.data_link_lost_counter++;
 				status_changed = true;
@@ -2138,6 +2158,19 @@ int commander_thread_main(int argc, char *argv[])
 			orb_publish(ORB_ID(vehicle_status), status_pub, &status);
 
 			armed.timestamp = now;
+
+			/* set prearmed state if safety is off, or safety is not present and 5 seconds passed */
+			if (safety.safety_switch_available) {
+
+				/* safety is off, go into prearmed */
+				armed.prearmed = safety.safety_off;
+			} else {
+				/* safety is not present, go into prearmed
+				 * (all output drivers should be started / unlocked last in the boot process
+				 * when the rest of the system is fully initialized)
+				 */
+				armed.prearmed = (hrt_elapsed_time(&commander_boot_timestamp) > 5 * 1000 * 1000);
+			}
 			orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
 		}
 
@@ -2959,6 +2992,21 @@ void *commander_low_prio_loop(void *arg)
 								mavlink_log_critical(mavlink_fd, "ERROR: %s", strerror(ret));
 							}
 
+							answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED);
+						}
+					} else if (((int)(cmd.param1)) == 2) {
+
+						/* reset parameters and save empty file */
+						param_reset_all();
+						int ret = param_save_default();
+
+						if (ret == OK) {
+							/* do not spam MAVLink, but provide the answer / green led mechanism */
+							mavlink_log_critical(mavlink_fd, "onboard parameters reset");
+							answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
+
+						} else {
+							mavlink_log_critical(mavlink_fd, "param reset error");
 							answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED);
 						}
 					}

src/modules/commander/commander_params.c

@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (c) 2013-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
@@ -37,16 +36,54 @@
  *
  * Parameters defined by the sensors task.
  *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Thomas Gubler <thomasgubler@student.ethz.ch>
- * @author Julian Oes <julian@oes.ch>
+ * @author Lorenz Meier <lorenz@px4.io>
+ * @author Thomas Gubler <thomas@px4.io>
+ * @author Julian Oes <julian@px4.io>
  */
 
 #include <nuttx/config.h>
 #include <systemlib/param/param.h>
 
+/**
+ * Roll trim
+ *
+ * The trim value is the actuator control value the system needs
+ * for straight and level flight. It can be calibrated by
+ * flying manually straight and level using the RC trims and
+ * copying them using the GCS.
+ *
+ * @group Radio Calibration
+ * @min -0.25
+ * @max 0.25
+ */
 PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f);
+
+/**
+ * Pitch trim
+ *
+ * The trim value is the actuator control value the system needs
+ * for straight and level flight. It can be calibrated by
+ * flying manually straight and level using the RC trims and
+ * copying them using the GCS.
+ *
+ * @group Radio Calibration
+ * @min -0.25
+ * @max 0.25
+ */
 PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f);
+
+/**
+ * Yaw trim
+ *
+ * The trim value is the actuator control value the system needs
+ * for straight and level flight. It can be calibrated by
+ * flying manually straight and level using the RC trims and
+ * copying them using the GCS.
+ *
+ * @group Radio Calibration
+ * @min -0.25
+ * @max 0.25
+ */
 PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f);
 
 /**
@@ -77,6 +114,7 @@ PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.2f);
  *
  * @group Battery Calibration
  * @unit V
+ * @min 0.0f
  */
 PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.07f);
 
@@ -87,6 +125,8 @@ PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.07f);
  *
  * @group Battery Calibration
  * @unit S
+ * @min 1
+ * @max 10
  */
 PARAM_DEFINE_INT32(BAT_N_CELLS, 3);
 
@@ -182,7 +222,31 @@ PARAM_DEFINE_FLOAT(COM_EF_TIME, 10.0f);
  * @min 0
  * @max 35
  */
-PARAM_DEFINE_FLOAT(COM_RC_LOSS_T, 0.5);
+PARAM_DEFINE_FLOAT(COM_RC_LOSS_T, 0.5f);
+
+/**
+ * Home set horizontal threshold
+ *
+ * The home position will be set if the estimated positioning accuracy is below the threshold.
+ *
+ * @group Commander
+ * @unit meter
+ * @min 2
+ * @max 15
+ */
+PARAM_DEFINE_FLOAT(COM_HOME_H_T, 5.0f);
+
+/**
+ * Home set vertical threshold
+ *
+ * The home position will be set if the estimated positioning accuracy is below the threshold.
+ *
+ * @group Commander
+ * @unit meter
+ * @min 5
+ * @max 25
+ */
+PARAM_DEFINE_FLOAT(COM_HOME_V_T, 10.0f);
 
 /**
  * Autosaving of params

src/modules/commander/mag_calibration.cpp

@@ -64,9 +64,11 @@
 static const int ERROR = -1;
 
 static const char *sensor_name = "mag";
-static const unsigned max_mags = 3;
+static constexpr unsigned max_mags = 3;
 static constexpr float mag_sphere_radius = 0.2f;
-static const unsigned int calibration_sides = 6;
+static constexpr unsigned int calibration_sides = 6;			///< The total number of sides
+static constexpr unsigned int calibration_total_points = 240;		///< The total points per magnetometer
+static constexpr unsigned int calibraton_duration_seconds = 42; 	///< The total duration the routine is allowed to take
 
 calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]);
 
@@ -204,6 +206,10 @@ static bool reject_sample(float sx, float sy, float sz, float x[], float y[], fl
 	return false;
 }
 
+static unsigned progress_percentage(mag_worker_data_t* worker_data) {
+	return 100 * ((float)worker_data->done_count) / calibration_sides;
+}
+
 static calibrate_return mag_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data)
 {
 	calibrate_return result = calibrate_return_ok;
@@ -223,7 +229,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
 	 * for a good result, so we're not constraining the user more than we have to.
 	 */
 
-	hrt_abstime detection_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds;
+	hrt_abstime detection_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds * 5;
 	hrt_abstime last_gyro = 0;
 	float gyro_x_integral = 0.0f;
 	float gyro_y_integral = 0.0f;
@@ -344,8 +350,8 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
 				// Progress indicator for side
 				mavlink_and_console_log_info(worker_data->mavlink_fd,
 							     "[cal] %s side calibration: progress <%u>",
-							     detect_orientation_str(orientation),
-							     (unsigned)(100 * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside)));
+							     detect_orientation_str(orientation), progress_percentage(worker_data) +
+							     (unsigned)((100 / calibration_sides) * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside)));
 			}
 		} else {
 			poll_errcount++;
@@ -362,7 +368,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
 		mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] %s side done, rotate to a different side", detect_orientation_str(orientation));
 		
 		worker_data->done_count++;
-		mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, 34 * worker_data->done_count);
+		mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, progress_percentage(worker_data));
 	}
 	
 	return result;
@@ -376,8 +382,8 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
 	
 	worker_data.mavlink_fd = mavlink_fd;
 	worker_data.done_count = 0;
-	worker_data.calibration_points_perside = 40;
-	worker_data.calibration_interval_perside_seconds = 20;
+	worker_data.calibration_points_perside = calibration_total_points / calibration_sides;
+	worker_data.calibration_interval_perside_seconds = calibraton_duration_seconds / calibration_sides;
 	worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000;
 
 	// Collect: Right-side up, Left Side, Nose down
@@ -496,6 +502,10 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
 		printf("RAW DATA:\n--------------------\n");
 		for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {
 
+			if (worker_data.calibration_counter_total[cur_mag] == 0) {
+				continue;
+			}
+
 			printf("RAW: MAG %u with %u samples:\n", cur_mag, worker_data.calibration_counter_total[cur_mag]);
 
 			for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) {
@@ -511,6 +521,10 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
 		printf("CALIBRATED DATA:\n--------------------\n");
 		for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {
 
+			if (worker_data.calibration_counter_total[cur_mag] == 0) {
+				continue;
+			}
+
 			printf("Calibrated: MAG %u with %u samples:\n", cur_mag, worker_data.calibration_counter_total[cur_mag]);
 
 			for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) {

src/modules/fw_att_control/fw_att_control_main.cpp

@@ -163,7 +163,6 @@ private:
 	struct {
 		float tconst;
 		float p_p;
-		float p_d;
 		float p_i;
 		float p_ff;
 		float p_rmax_pos;
@@ -171,7 +170,6 @@ private:
 		float p_integrator_max;
 		float p_roll_feedforward;
 		float r_p;
-		float r_d;
 		float r_i;
 		float r_ff;
 		float r_integrator_max;
@@ -208,7 +206,6 @@ private:
 
 		param_t tconst;
 		param_t p_p;
-		param_t p_d;
 		param_t p_i;
 		param_t p_ff;
 		param_t p_rmax_pos;
@@ -216,7 +213,6 @@ private:
 		param_t p_integrator_max;
 		param_t p_roll_feedforward;
 		param_t r_p;
-		param_t r_d;
 		param_t r_i;
 		param_t r_ff;
 		param_t r_integrator_max;
@@ -699,7 +695,6 @@ FixedwingAttitudeControl::task_main()
 		/* only run controller if attitude changed */
 		if (fds[1].revents & POLLIN) {
 
-
 			static uint64_t last_run = 0;
 			float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
 			last_run = hrt_absolute_time();
@@ -803,6 +798,11 @@ FixedwingAttitudeControl::task_main()
 				//warnx("_actuators_airframe.control[1] = -1.0f;");
 			}
 
+			/* if we are in rotary wing mode, do nothing */
+			if (_vehicle_status.is_rotary_wing && !_vehicle_status.is_vtol) {
+				continue;
+			}
+
 			/* default flaps to center */
 			float flaps_control = 0.0f;
 
@@ -812,11 +812,8 @@ FixedwingAttitudeControl::task_main()
 			}
 
 			/* decide if in stabilized or full manual control */
-
 			if (_vcontrol_mode.flag_control_attitude_enabled) {
-
 				/* scale around tuning airspeed */
-
 				float airspeed;
 
 				/* if airspeed is not updating, we assume the normal average speed */

src/modules/fw_att_control/fw_att_control_params.c

@@ -84,11 +84,11 @@ PARAM_DEFINE_FLOAT(FW_PR_P, 0.08f);
  * 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
+ * @min 0.005
+ * @max 0.5
  * @group FW Attitude Control
  */
-PARAM_DEFINE_FLOAT(FW_PR_I, 0.005f);
+PARAM_DEFINE_FLOAT(FW_PR_I, 0.02f);
 
 /**
  * Maximum positive / up pitch rate.
@@ -101,7 +101,7 @@ PARAM_DEFINE_FLOAT(FW_PR_I, 0.005f);
  * @max 90.0
  * @group FW Attitude Control
  */
-PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 0.0f);
+PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 60.0f);
 
 /**
  * Maximum negative / down pitch rate.
@@ -114,7 +114,7 @@ PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 0.0f);
  * @max 90.0
  * @group FW Attitude Control
  */
-PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 0.0f);
+PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 60.0f);
 
 /**
  * Pitch rate integrator limit
@@ -157,11 +157,11 @@ PARAM_DEFINE_FLOAT(FW_RR_P, 0.05f);
  * This gain defines how much control response will result out of a steady
  * state error. It trims any constant error.
  *
- * @min 0.0
- * @max 100.0
+ * @min 0.005
+ * @max 0.2
  * @group FW Attitude Control
  */
-PARAM_DEFINE_FLOAT(FW_RR_I, 0.005f);
+PARAM_DEFINE_FLOAT(FW_RR_I, 0.01f);
 
 /**
  * Roll Integrator Anti-Windup
@@ -185,7 +185,7 @@ PARAM_DEFINE_FLOAT(FW_RR_IMAX, 0.2f);
  * @max 90.0
  * @group FW Attitude Control
  */
-PARAM_DEFINE_FLOAT(FW_R_RMAX, 0.0f);
+PARAM_DEFINE_FLOAT(FW_R_RMAX, 70.0f);
 
 /**
  * Yaw rate proportional gain
@@ -258,7 +258,7 @@ PARAM_DEFINE_FLOAT(FW_RR_FF, 0.5f);
  * @max 10.0
  * @group FW Attitude Control
  */
-PARAM_DEFINE_FLOAT(FW_PR_FF, 0.4f);
+PARAM_DEFINE_FLOAT(FW_PR_FF, 0.5f);
 
 /**
  * Yaw rate feed forward

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -179,6 +179,8 @@ private:
 	float	_hdg_hold_yaw;				/**< hold heading for velocity mode */
 	bool	_hdg_hold_enabled;			/**< heading hold enabled */
 	bool	_yaw_lock_engaged;			/**< yaw is locked for heading hold */
+	float	_althold_epv;				/**< the position estimate accuracy when engaging alt hold */
+	bool	_was_in_deadband;				/**< wether the last stick input was in althold deadband */
 	struct position_setpoint_s _hdg_hold_prev_wp;	/**< position where heading hold started */
 	struct position_setpoint_s _hdg_hold_curr_wp;	/**< position to which heading hold flies */
 	hrt_abstime _control_position_last_called; /**<last call of control_position  */
@@ -508,6 +510,8 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_hdg_hold_yaw(0.0f),
 	_hdg_hold_enabled(false),
 	_yaw_lock_engaged(false),
+	_althold_epv(0.0f),
+	_was_in_deadband(false),
 	_hdg_hold_prev_wp{},
 	_hdg_hold_curr_wp{},
 	_control_position_last_called(0),
@@ -968,12 +972,20 @@ float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint
 
 bool FixedwingPositionControl::update_desired_altitude(float dt)
 {
-	const float deadBand = (60.0f/1000.0f);
+	/*
+	 * The complete range is -1..+1, so this is 6%
+	 * of the up or down range or 3% of the total range.
+	 */
+	const float deadBand = 0.06f;
+
+	/*
+	 * The correct scaling of the complete range needs
+	 * to account for the missing part of the slope
+	 * due to the deadband
+	 */
 	const float factor = 1.0f - deadBand;
-	// XXX this should go into a manual stick mapper
-	// class
-	static float _althold_epv = 0.0f;
-	static bool was_in_deadband = false;
+
+	/* Climbout mode sets maximum throttle and pitch up */
 	bool climbout_mode = false;
 
 	/*
@@ -988,24 +1000,29 @@ bool FixedwingPositionControl::update_desired_altitude(float dt)
 		_althold_epv = _global_pos.epv;
 	}
 
-	// XXX the sign magic in this function needs to be fixed
-
+	/*
+	 * Manual control has as convention the rotation around
+	 * an axis. Positive X means to rotate positively around
+	 * the X axis in NED frame, which is pitching down
+	 */
 	if (_manual.x > deadBand) {
-		float pitch = (_manual.x - deadBand) / factor;
-		_hold_alt -= (_parameters.max_climb_rate * dt) * pitch;
-		was_in_deadband = false;
-		climbout_mode = (fabsf(_manual.x) > MANUAL_THROTTLE_CLIMBOUT_THRESH);
+		/* pitching down */
+		float pitch = -(_manual.x - deadBand) / factor;
+		_hold_alt += (_parameters.max_sink_rate * dt) * pitch;
+		_was_in_deadband = false;
 	} else if (_manual.x < - deadBand) {
-		float pitch = (_manual.x + deadBand) / factor;
-		_hold_alt -= (_parameters.max_sink_rate * dt) * pitch;
-		was_in_deadband = false;
-	} else if (!was_in_deadband) {
+		/* pitching up */
+		float pitch = -(_manual.x + deadBand) / factor;
+		_hold_alt += (_parameters.max_climb_rate * dt) * pitch;
+		_was_in_deadband = false;
+		climbout_mode = (pitch > MANUAL_THROTTLE_CLIMBOUT_THRESH);
+	} else if (!_was_in_deadband) {
 		 /* store altitude at which manual.x was inside deadBand
 		  * The aircraft should immediately try to fly at this altitude
 		  * as this is what the pilot expects when he moves the stick to the center */
 		_hold_alt = _global_pos.alt;
 		_althold_epv = _global_pos.epv;
-		was_in_deadband = true;
+		_was_in_deadband = true;
 	}
 
 	return climbout_mode;
@@ -1037,7 +1054,7 @@ bool
 FixedwingPositionControl::control_position(const math::Vector<2> &current_position, const math::Vector<3> &ground_speed,
 		const struct position_setpoint_triplet_s &pos_sp_triplet)
 {
-	float dt = FLT_MIN; // Using non zero value to a avoid division by zero
+	float dt = 0.01; // Using non zero value to a avoid division by zero
 	if (_control_position_last_called > 0) {
 		dt = (float)hrt_elapsed_time(&_control_position_last_called) * 1e-6f;
 	}
@@ -1045,19 +1062,27 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 	bool setpoint = true;
 
-	math::Vector<2> ground_speed_2d = {ground_speed(0), ground_speed(1)};
-	calculate_gndspeed_undershoot(current_position, ground_speed_2d, pos_sp_triplet);
-
 	float eas2tas = 1.0f; // XXX calculate actual number based on current measurements
 
 	/* filter speed and altitude for controller */
 	math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2);
 	math::Vector<3> accel_earth = _R_nb * accel_body;
 
-	if (!_mTecs.getEnabled() && !_vehicle_status.condition_landed) {
-		_tecs.update_50hz(_global_pos.alt /* XXX might switch to alt err here */, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth);
+	/* tell TECS to update its state, but let it know when it cannot actually control the plane */
+	bool in_air_alt_control = (!_vehicle_status.condition_landed &&
+		(_control_mode.flag_control_auto_enabled ||
+		 _control_mode.flag_control_velocity_enabled ||
+		 _control_mode.flag_control_altitude_enabled));
+
+	/* update TECS filters */
+	if (!_mTecs.getEnabled()) {
+		_tecs.update_state(_global_pos.alt, _airspeed.indicated_airspeed_m_s, _R_nb,
+			accel_body, accel_earth, (_global_pos.timestamp > 0), in_air_alt_control);
 	}
 
+	math::Vector<2> ground_speed_2d = {ground_speed(0), ground_speed(1)};
+	calculate_gndspeed_undershoot(current_position, ground_speed_2d, pos_sp_triplet);
+
 	/* define altitude error */
 	float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt;
 
@@ -1477,7 +1502,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				throttle_max,
 				_parameters.throttle_cruise,
 				climbout_requested,
-				pitch_limit_min,
+				((climbout_requested) ? math::radians(10.0f) : pitch_limit_min),
 				_global_pos.alt,
 				ground_speed,
 				tecs_status_s::TECS_MODE_NORMAL);
@@ -1587,7 +1612,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				throttle_max,
 				_parameters.throttle_cruise,
 				climbout_requested,
-				pitch_limit_min,
+				((climbout_requested) ? math::radians(10.0f) : pitch_limit_min),
 				_global_pos.alt,
 				ground_speed,
 				tecs_status_s::TECS_MODE_NORMAL);

src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c

@@ -327,7 +327,7 @@ PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f);
  *
  * @group Fixed Wing TECS
  */
-PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f);
+PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 15.0f);
 
 /**
  * Speed <--> Altitude priority
@@ -378,7 +378,7 @@ PARAM_DEFINE_FLOAT(FW_T_HRATE_FF, 0.0f);
  *
  * @group Fixed Wing TECS
  */
-PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f);
+PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.02f);
 
 /**
  * Landing slope angle

src/modules/fw_pos_control_l1/mtecs/mTecs.cpp

@@ -204,7 +204,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 		mode = tecs_status_s::TECS_MODE_UNDERSPEED;
 	}
 
-	/* Set special ouput limiters if we are not in TECS_MODE_NORMAL */
+	/* Set special output limiters if we are not in TECS_MODE_NORMAL */
 	BlockOutputLimiter *outputLimiterThrottle = &_controlTotalEnergy.getOutputLimiter();
 	BlockOutputLimiter *outputLimiterPitch = &_controlEnergyDistribution.getOutputLimiter();
 	if (mode == tecs_status_s::TECS_MODE_TAKEOFF) {
@@ -221,7 +221,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 		outputLimiterPitch = &_BlockOutputLimiterUnderspeedPitch;
 	}
 
-	/* Apply overrride given by the limitOverride argument (this is used for limits which are not given by
+	/* Apply override given by the limitOverride argument (this is used for limits which are not given by
 	 * parameters such as pitch limits with takeoff waypoints or throttle limits when the launchdetector
 	 * is running) */
 	limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch);
@@ -253,7 +253,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 				(double)accelerationLongitudinalSp, (double)airspeedDerivative);
 	}
 
-	/* publish status messge */
+	/* publish status message */
 	_status.update();
 
 	/* clean up */

src/modules/land_detector/FixedwingLandDetector.cpp

@@ -49,7 +49,11 @@ FixedwingLandDetector::FixedwingLandDetector() : LandDetector(),
 	_vehicleLocalPositionSub(-1),
 	_vehicleLocalPosition({}),
 	_airspeedSub(-1),
-	_airspeed({}),
+	_vehicleStatusSub(-1),
+	_armingSub(-1),
+	_airspeed{},
+	_vehicleStatus{},
+	_arming{},
 	_parameterSub(-1),
 	_velocity_xy_filtered(0.0f),
 	_velocity_z_filtered(0.0f),
@@ -66,6 +70,8 @@ void FixedwingLandDetector::initialize()
 	// Subscribe to local position and airspeed data
 	_vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position));
 	_airspeedSub = orb_subscribe(ORB_ID(airspeed));
+	_vehicleStatusSub = orb_subscribe(ORB_ID(vehicle_status));
+	_armingSub = orb_subscribe(ORB_ID(actuator_armed));
 
 	updateParameterCache(true);
 }
@@ -74,6 +80,8 @@ void FixedwingLandDetector::updateSubscriptions()
 {
 	orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition);
 	orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed);
+	orb_update(ORB_ID(vehicle_status), _vehicleStatusSub, &_vehicleStatus);
+	orb_update(ORB_ID(actuator_armed), _armingSub, &_arming);
 }
 
 bool FixedwingLandDetector::update()
@@ -81,6 +89,11 @@ bool FixedwingLandDetector::update()
 	// First poll for new data from our subscriptions
 	updateSubscriptions();
 
+	// only trigger flight conditions if we are armed
+	if (!_arming.armed) {
+		return true;
+	}
+
 	const uint64_t now = hrt_absolute_time();
 	bool landDetected = false;
 

src/modules/land_detector/FixedwingLandDetector.h

@@ -44,7 +44,9 @@
 #include "LandDetector.h"
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/airspeed.h>
+#include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_status.h>
 #include <systemlib/param/param.h>
 
 class FixedwingLandDetector : public LandDetector
@@ -90,11 +92,15 @@ private:
 	} _params;
 
 private:
-	int                                     _vehicleLocalPositionSub;   /**< notification of local position */
-	struct vehicle_local_position_s         _vehicleLocalPosition;      /**< the result from local position subscription */
-	int                                     _airspeedSub;
-	struct airspeed_s                       _airspeed;
-	int 									_parameterSub;
+	int					_vehicleLocalPositionSub;	/**< notification of local position */
+	struct vehicle_local_position_s		_vehicleLocalPosition;		/**< the result from local position subscription */
+	int					_airspeedSub;
+	int					_vehicleStatusSub;
+	int					_armingSub;
+	struct airspeed_s			_airspeed;
+	struct vehicle_status_s 		_vehicleStatus;
+	struct actuator_armed_s			_arming;
+	int 					_parameterSub;
 
 	float _velocity_xy_filtered;
 	float _velocity_z_filtered;

src/modules/land_detector/LandDetector.cpp

@@ -46,8 +46,9 @@
 LandDetector::LandDetector() :
 	_landDetectedPub(-1),
 	_landDetected({0, false}),
-	      _taskShouldExit(false),
-	      _taskIsRunning(false)
+	_arming_time(0),
+	_taskShouldExit(false),
+	_taskIsRunning(false)
 {
 	// ctor
 }

src/modules/land_detector/LandDetector.h

@@ -96,10 +96,11 @@ protected:
 
 	static constexpr uint64_t LAND_DETECTOR_TRIGGER_TIME = 2000000;  /**< usec that landing conditions have to hold
                                                                           before triggering a land */
+	static constexpr uint64_t LAND_DETECTOR_ARM_PHASE_TIME = 1000000;	/**< time interval in which wider acceptance thresholds are used after arming */
 
-protected:
-	orb_advert_t                            _landDetectedPub;           /**< publisher for position in local frame */
-	struct vehicle_land_detected_s          _landDetected;              /**< local vehicle position */
+	orb_advert_t				_landDetectedPub;		/**< publisher for position in local frame */
+	struct vehicle_land_detected_s		_landDetected;			/**< local vehicle position */
+	uint64_t				_arming_time;			/**< timestamp of arming time */
 
 private:
 	bool _taskShouldExit;                                               /**< true if it is requested that this task should exit */

src/modules/land_detector/MulticopterLandDetector.cpp

@@ -53,12 +53,12 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(),
 	_actuatorsSub(-1),
 	_armingSub(-1),
 	_parameterSub(-1),
-    _attitudeSub(-1),
-	_vehicleGlobalPosition({}),
-	_vehicleStatus({}),
-	_actuators({}),
-	_arming({}),
-    _vehicleAttitude({}),
+	_attitudeSub(-1),
+	_vehicleGlobalPosition{},
+	_vehicleStatus{},
+	_actuators{},
+	_arming{},
+	_vehicleAttitude{},
 	_landTimer(0)
 {
 	_paramHandle.maxRotation = param_find("LNDMC_ROT_MAX");
@@ -97,7 +97,10 @@ bool MulticopterLandDetector::update()
 
 	// only trigger flight conditions if we are armed
 	if (!_arming.armed) {
+		_arming_time = 0;
 		return true;
+	} else if (_arming_time == 0) {
+		_arming_time = hrt_absolute_time();
 	}
 
 	// return status based on armed state if no position lock is available
@@ -110,8 +113,18 @@ bool MulticopterLandDetector::update()
 
 	const uint64_t now = hrt_absolute_time();
 
-	// check if we are moving vertically
-	bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > _params.maxClimbRate;
+	float armThresholdFactor = 1.0f;
+
+	// Widen acceptance thresholds for landed state right after arming
+	// so that motor spool-up and other effects do not trigger false negatives
+	if (hrt_elapsed_time(&_arming_time) < LAND_DETECTOR_ARM_PHASE_TIME) {
+		armThresholdFactor = 2.5f;
+	}
+
+	// check if we are moving vertically - this might see a spike after arming due to
+	// throttle-up vibration. If accelerating fast the throttle thresholds will still give
+	// an accurate in-air indication
+	bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > _params.maxClimbRate * armThresholdFactor;
 
 	// check if we are moving horizontally
 	bool horizontalMovement = sqrtf(_vehicleGlobalPosition.vel_n * _vehicleGlobalPosition.vel_n
@@ -119,9 +132,11 @@ bool MulticopterLandDetector::update()
 					&& _vehicleStatus.condition_global_position_valid;
 
 	// next look if all rotation angles are not moving
-	bool rotating = fabsf(_vehicleAttitude.rollspeed)  > _params.maxRotation ||
-				    fabsf(_vehicleAttitude.pitchspeed) > _params.maxRotation ||
-        			fabsf(_vehicleAttitude.yawspeed)   > _params.maxRotation;
+	float maxRotationScaled = _params.maxRotation * armThresholdFactor;
+
+	bool rotating = (fabsf(_vehicleAttitude.rollspeed)  > maxRotationScaled) ||
+				    (fabsf(_vehicleAttitude.pitchspeed) > maxRotationScaled) ||
+				(fabsf(_vehicleAttitude.yawspeed) > maxRotationScaled);
 
 	// check if thrust output is minimal (about half of default)
 	bool minimalThrust = _actuators.control[3] <= _params.maxThrottle;

src/modules/land_detector/MulticopterLandDetector.h

@@ -97,20 +97,20 @@ private:
 	} _params;
 
 private:
-	int _vehicleGlobalPositionSub;                                      /**< notification of global position */
+	int _vehicleGlobalPositionSub;						/**< notification of global position */
 	int _vehicleStatusSub;
 	int _actuatorsSub;
 	int _armingSub;
 	int _parameterSub;
 	int _attitudeSub;
 
-	struct vehicle_global_position_s          _vehicleGlobalPosition;   /**< the result from global position subscription */
-	struct vehicle_status_s 			      _vehicleStatus;
-	struct actuator_controls_s                _actuators;
-	struct actuator_armed_s                   _arming;
-	struct vehicle_attitude_s                 _vehicleAttitude;
+	struct vehicle_global_position_s	_vehicleGlobalPosition;		/**< the result from global position subscription */
+	struct vehicle_status_s 		_vehicleStatus;
+	struct actuator_controls_s		_actuators;
+	struct actuator_armed_s			_arming;
+	struct vehicle_attitude_s		_vehicleAttitude;
 
-	uint64_t _landTimer;                                                /**< timestamp in microseconds since a possible land was detected*/
+	uint64_t _landTimer;							/**< timestamp in microseconds since a possible land was detected*/
 };
 
 #endif //__MULTICOPTER_LAND_DETECTOR_H__

src/modules/land_detector/land_detector_params.c

@@ -49,7 +49,7 @@
  *
  * @group Land Detector
  */
-PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 0.60f);
+PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 0.70f);
 
 /**
  * Multicopter max horizontal velocity

src/modules/mavlink/mavlink_main.cpp

@@ -115,6 +115,8 @@ extern mavlink_system_t mavlink_system;
 
 static void usage(void);
 
+bool Mavlink::_boot_complete = false;
+
 Mavlink::Mavlink() :
 	_device_name(DEFAULT_DEVICE_NAME),
 	_task_should_exit(false),
@@ -150,6 +152,7 @@ Mavlink::Mavlink() :
 	_datarate(1000),
 	_datarate_events(500),
 	_rate_mult(1.0f),
+	_last_hw_rate_timestamp(0),
 	_mavlink_param_queue_index(0),
 	mavlink_link_termination_allowed(false),
 	_subscribe_to_stream(nullptr),
@@ -1230,6 +1233,7 @@ Mavlink::update_rate_mult()
 	float const_rate = 0.0f;
 	float rate = 0.0f;
 
+	/* scale down rates if their theoretical bandwidth is exceeding the link bandwidth */
 	MavlinkStream *stream;
 	LL_FOREACH(_streams, stream) {
 		if (stream->const_rate()) {
@@ -1241,7 +1245,55 @@ Mavlink::update_rate_mult()
 	}
 
 	/* don't scale up rates, only scale down if needed */
-	_rate_mult = fminf(1.0f, ((float)_datarate - const_rate) / rate);
+	float bandwidth_mult = fminf(1.0f, ((float)_datarate - const_rate) / rate);
+
+	/* check if we have radio feedback */
+	struct telemetry_status_s &tstatus = get_rx_status();
+
+	bool radio_critical = false;
+	bool radio_found = false;
+
+	/* 2nd pass: Now check hardware limits */
+	if (tstatus.type == TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO) {
+
+		radio_found = true;
+
+		if (tstatus.txbuf < RADIO_BUFFER_LOW_PERCENTAGE) {
+			radio_critical = true;
+		}
+	}
+
+	float hardware_mult = _rate_mult;
+
+	/* scale down if we have a TX err rate suggesting link congestion */
+	if (_rate_txerr > 0.0f && !radio_critical) {
+		hardware_mult = (_rate_tx) / (_rate_tx + _rate_txerr);
+	} else if (radio_found && tstatus.timestamp != _last_hw_rate_timestamp) {
+
+		if (tstatus.txbuf < RADIO_BUFFER_CRITICAL_LOW_PERCENTAGE) {
+			/* this indicates link congestion, reduce rate by 20% */
+			hardware_mult *= 0.80f;
+		} else if (tstatus.txbuf < RADIO_BUFFER_LOW_PERCENTAGE) {
+			/* this indicates link congestion, reduce rate by 2.5% */
+			hardware_mult *= 0.975f;
+		} else if (tstatus.txbuf > RADIO_BUFFER_HALF_PERCENTAGE) {
+			/* this indicates spare bandwidth, increase by 2.5% */
+			hardware_mult *= 1.025f;
+			/* limit to a max multiplier of 1 */
+			hardware_mult = fminf(1.0f, hardware_mult);
+		}
+	} else {
+		/* no limitation, set hardware to 1 */
+		hardware_mult = 1.0f;
+	}
+
+	_last_hw_rate_timestamp = tstatus.timestamp;
+
+	/* pick the minimum from bandwidth mult and hardware mult as limit */
+	_rate_mult = fminf(bandwidth_mult, hardware_mult);
+
+	/* ensure the rate multiplier never drops below 5% so that something is always sent */
+	_rate_mult = fmaxf(0.05f, _rate_mult);
 }
 
 int
@@ -1440,22 +1492,22 @@ Mavlink::task_main(int argc, char *argv[])
 	case MAVLINK_MODE_NORMAL:
 		configure_stream("SYS_STATUS", 1.0f);
 		configure_stream("GPS_GLOBAL_ORIGIN", 0.5f);
-		configure_stream("HIGHRES_IMU", 1.0f);
-		configure_stream("ATTITUDE", 10.0f);
+		configure_stream("HIGHRES_IMU", 2.0f);
+		configure_stream("ATTITUDE", 20.0f);
 		configure_stream("VFR_HUD", 8.0f);
 		configure_stream("GPS_RAW_INT", 1.0f);
 		configure_stream("GLOBAL_POSITION_INT", 3.0f);
 		configure_stream("LOCAL_POSITION_NED", 3.0f);
-		configure_stream("RC_CHANNELS_RAW", 1.0f);
+		configure_stream("RC_CHANNELS_RAW", 4.0f);
 		configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f);
-		configure_stream("ATTITUDE_TARGET", 3.0f);
+		configure_stream("ATTITUDE_TARGET", 8.0f);
 		configure_stream("DISTANCE_SENSOR", 0.5f);
 		configure_stream("OPTICAL_FLOW_RAD", 5.0f);
 		break;
 
 	case MAVLINK_MODE_ONBOARD:
 		configure_stream("SYS_STATUS", 1.0f);
-		configure_stream("ATTITUDE", 50.0f);
+		configure_stream("ATTITUDE", 250.0f);
 		configure_stream("HIGHRES_IMU", 50.0f);
 		configure_stream("GPS_RAW_INT", 5.0f);
 		configure_stream("GLOBAL_POSITION_INT", 50.0f);
@@ -1737,6 +1789,12 @@ Mavlink::start(int argc, char *argv[])
 	// before returning to the shell
 	int ic = Mavlink::instance_count();
 
+	if (ic == MAVLINK_COMM_NUM_BUFFERS) {
+		warnx("Maximum MAVLink instance count of %d reached.",
+			(int)MAVLINK_COMM_NUM_BUFFERS);
+		return 1;
+	}
+
 	// Instantiate thread
 	char buf[24];
 	sprintf(buf, "mavlink_if%d", ic);
@@ -1909,6 +1967,10 @@ int mavlink_main(int argc, char *argv[])
 	} else if (!strcmp(argv[1], "stream")) {
 		return Mavlink::stream_command(argc, argv);
 
+	} else if (!strcmp(argv[1], "boot_complete")) {
+		Mavlink::set_boot_complete();
+		return 0;
+
 	} else {
 		usage();
 		exit(1);

src/modules/mavlink/mavlink_main.h

@@ -145,6 +145,14 @@ public:
 
 	bool			get_forwarding_on() { return _forwarding_on; }
 
+	/**
+	 * Set the boot complete flag on all instances
+	 *
+	 * Setting the flag unblocks parameter transmissions, which are gated
+	 * beforehand to ensure that the system is fully initialized.
+	 */
+	static void		set_boot_complete() { _boot_complete = true; }
+
 	/**
 	 * Get the free space in the transmit buffer
 	 *
@@ -294,6 +302,8 @@ public:
 
 	unsigned		get_system_type() { return _system_type; }
 
+	static bool		boot_complete() { return _boot_complete; }
+
 protected:
 	Mavlink			*next;
 
@@ -302,6 +312,7 @@ private:
 
 	int			_mavlink_fd;
 	bool			_task_running;
+	static bool		_boot_complete;
 
 	/* states */
 	bool			_hil_enabled;		/**< Hardware In the Loop mode */
@@ -340,6 +351,7 @@ private:
 	int			_datarate;		///< data rate for normal streams (attitude, position, etc.)
 	int			_datarate_events;	///< data rate for params, waypoints, text messages
 	float			_rate_mult;
+	hrt_abstime		_last_hw_rate_timestamp;
 
 	/**
 	 * If the queue index is not at 0, the queue sending
@@ -398,6 +410,10 @@ private:
 
 	static unsigned int	interval_from_rate(float rate);
 
+	static constexpr unsigned RADIO_BUFFER_CRITICAL_LOW_PERCENTAGE = 25;
+	static constexpr unsigned RADIO_BUFFER_LOW_PERCENTAGE = 35;
+	static constexpr unsigned RADIO_BUFFER_HALF_PERCENTAGE = 50;
+
 	int configure_stream(const char *stream_name, const float rate);
 
 	/**

src/modules/mavlink/mavlink_messages.cpp

@@ -537,7 +537,7 @@ protected:
 			msg.onboard_control_sensors_health = status.onboard_control_sensors_health;
 			msg.load = status.load * 1000.0f;
 			msg.voltage_battery = status.battery_voltage * 1000.0f;
-			msg.current_battery = status.battery_current / 10.0f;
+			msg.current_battery = status.battery_current * 100.0f;
 			msg.drop_rate_comm = status.drop_rate_comm;
 			msg.errors_comm = status.errors_comm;
 			msg.errors_count1 = status.errors_count1;
@@ -562,7 +562,7 @@ protected:
 					bat_msg.voltages[i] = 0;
 				}
 			}
-			bat_msg.current_battery = status.battery_current / 10.0f;
+			bat_msg.current_battery = status.battery_current * 100.0f;
 			bat_msg.current_consumed = status.battery_discharged_mah;
 			bat_msg.energy_consumed = -1.0f;
 			bat_msg.battery_remaining = (status.battery_voltage > 0) ?

src/modules/mavlink/mavlink_parameters.cpp

@@ -193,7 +193,7 @@ void
 MavlinkParametersManager::send(const hrt_abstime t)
 {
 	/* send all parameters if requested, but only after the system has booted */
-	if (_send_all_index >= 0 && t > 4 * 1000 * 1000) {
+	if (_send_all_index >= 0 && _mavlink->boot_complete()) {
 
 		/* skip if no space is available */
 		if (_mavlink->get_free_tx_buf() < get_size()) {
@@ -215,6 +215,10 @@ MavlinkParametersManager::send(const hrt_abstime t)
 		if ((p == PARAM_INVALID) || (_send_all_index >= (int) param_count())) {
 			_send_all_index = -1;
 		}
+	} else if (_send_all_index == 0 && hrt_absolute_time() > 20 * 1000 * 1000) {
+		/* the boot did not seem to ever complete, warn user and set boot complete */
+		_mavlink->send_statustext_critical("WARNING: SYSTEM BOOT INCOMPLETE. CHECK CONFIG.");
+		_mavlink->set_boot_complete();
 	}
 }
 

src/modules/mavlink/mavlink_receiver.cpp

@@ -35,9 +35,9 @@
  * @file mavlink_receiver.cpp
  * MAVLink protocol message receive and dispatch
  *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Anton Babushkin <anton.babushkin@me.com>
- * @author Thomas Gubler <thomasgubler@gmail.com>
+ * @author Lorenz Meier <lorenz@px4.io>
+ * @author Anton Babushkin <anton@px4.io>
+ * @author Thomas Gubler <thomas@px4.io>
  */
 
 /* XXX trim includes */
@@ -133,7 +133,9 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_att_sp{},
 	_rates_sp{},
 	_time_offset_avg_alpha(0.6),
-	_time_offset(0)
+	_time_offset(0),
+	_mom_switch_pos{},
+	_mom_switch_state(0)
 {
 
 }
@@ -904,15 +906,45 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
 	}
 }
 
-static switch_pos_t decode_switch_pos(uint16_t buttons, int sw) {
+switch_pos_t
+MavlinkReceiver::decode_switch_pos(uint16_t buttons, unsigned sw)
+{
+	// XXX non-standard 3 pos switch decoding
 	return (buttons >> (sw * 2)) & 3;
 }
 
-static int decode_switch_pos_n(uint16_t buttons, int sw) {
-	if (buttons & (1 << sw)) {
-		return 1;
+int
+MavlinkReceiver::decode_switch_pos_n(uint16_t buttons, unsigned sw)
+{
+
+	bool on = (buttons & (1 << sw));
+
+	if (sw < MOM_SWITCH_COUNT) {
+
+		bool last_on = (_mom_switch_state & (1 << sw));
+
+		/* first switch is 2-pos, rest is 2 pos */
+		unsigned state_count = (sw == 0) ? 3 : 2;
+
+		/* only transition on low state */
+		if (!on && (on != last_on)) {
+
+			_mom_switch_pos[sw]++;
+			if (_mom_switch_pos[sw] == state_count) {
+				_mom_switch_pos[sw] = 0;
+			}
+		}
+
+		/* state_count - 1 is the number of intervals and 1000 is the range,
+		 * with 2 states 0 becomes 0, 1 becomes 1000. With
+		 * 3 states 0 becomes 0, 1 becomes 500, 2 becomes 1000,
+		 * and so on for more states.
+		 */
+		return (_mom_switch_pos[sw] * 1000) / (state_count - 1) + 1000;
+
 	} else {
-		return 0;
+		/* return the current state */
+		return on * 1000 + 1000;
 	}
 }
 
@@ -1009,12 +1041,18 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
 			rc.values[3] = 1000;
 		}
 
-		rc.values[4] = decode_switch_pos_n(man.buttons, 0) * 1000 + 1000;
-		rc.values[5] = decode_switch_pos_n(man.buttons, 1) * 1000 + 1000;
-		rc.values[6] = decode_switch_pos_n(man.buttons, 2) * 1000 + 1000;
-		rc.values[7] = decode_switch_pos_n(man.buttons, 3) * 1000 + 1000;
-		rc.values[8] = decode_switch_pos_n(man.buttons, 4) * 1000 + 1000;
-		rc.values[9] = decode_switch_pos_n(man.buttons, 5) * 1000 + 1000;
+		/* decode all switches which fit into the channel mask */
+		unsigned max_switch = (sizeof(man.buttons) * 8);
+		unsigned max_channels = (sizeof(rc.values) / sizeof(rc.values[0]));
+		if (max_switch > (max_channels - 4)) {
+			max_switch = (max_channels - 4);
+		}
+
+		/* fill all channels */
+		for (unsigned i = 0; i < max_switch; i++) {
+			rc.values[i + 4] = decode_switch_pos_n(man.buttons, i);
+		}
+		_mom_switch_state = man.buttons;
 
 		if (_rc_pub <= 0) {
 			_rc_pub = orb_advertise(ORB_ID(input_rc), &rc);

src/modules/mavlink/mavlink_receiver.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-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
@@ -35,8 +35,8 @@
  * @file mavlink_orb_listener.h
  * MAVLink 1.0 uORB listener definition
  *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Lorenz Meier <lorenz@px4.io>
+ * @author Anton Babushkin <anton@px4.io>
  */
 
 #pragma once
@@ -139,15 +139,26 @@ private:
 	void *receive_thread(void *arg);
 
 	/**
-	* Convert remote timestamp to local hrt time (usec)
-	* Use timesync if available, monotonic boot time otherwise
-	*/
+	 * Convert remote timestamp to local hrt time (usec)
+	 * Use timesync if available, monotonic boot time otherwise
+	 */
 	uint64_t sync_stamp(uint64_t usec);
+
 	/**
-	* Exponential moving average filter to smooth time offset
-	*/
+	 * Exponential moving average filter to smooth time offset
+	 */
 	void smooth_time_offset(uint64_t offset_ns);
 
+	/**
+	 * Decode a switch position from a bitfield
+	 */
+	switch_pos_t decode_switch_pos(uint16_t buttons, unsigned sw);
+
+	/**
+	 * Decode a switch position from a bitfield and state
+	 */
+	int decode_switch_pos_n(uint16_t buttons, unsigned sw);
+
 	mavlink_status_t status;
 	struct vehicle_local_position_s hil_local_pos;
 	struct vehicle_land_detected_s hil_land_detector;
@@ -192,6 +203,11 @@ private:
 	double _time_offset_avg_alpha;
 	uint64_t _time_offset;
 
+	static constexpr unsigned MOM_SWITCH_COUNT = 8;
+
+	uint8_t _mom_switch_pos[MOM_SWITCH_COUNT];
+	uint16_t _mom_switch_state;
+
 	/* do not allow copying this class */
 	MavlinkReceiver(const MavlinkReceiver &);
 	MavlinkReceiver operator=(const MavlinkReceiver &);

src/modules/mavlink/module.mk

@@ -53,6 +53,6 @@ MAXOPTIMIZATION	 = -Os
 
 MODULE_STACKSIZE = 1200
 
-EXTRACXXFLAGS	= -Weffc++ -Wno-attributes -Wno-packed
+EXTRACXXFLAGS	= -Weffc++ -Wno-attributes -Wno-packed -DMAVLINK_COMM_NUM_BUFFERS=3
 
-EXTRACFLAGS	= -Wno-packed
+EXTRACFLAGS	= -Wno-packed -DMAVLINK_COMM_NUM_BUFFERS=3

src/modules/mc_att_control/mc_att_control_params.c

@@ -232,14 +232,16 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_MAX, 360.0f);
 /**
  * Max yaw rate
  *
- * Limit for yaw rate, has effect for large rotations in autonomous mode, to avoid large control output and mixer saturation.
+ * Limit for yaw rate, has effect for large rotations in autonomous mode,
+ * to avoid large control output and mixer saturation. A value of significantly
+ * over 60 degrees per second can already lead to mixer saturation.
  *
  * @unit deg/s
  * @min 0.0
  * @max 360.0
  * @group Multicopter Attitude Control
  */
-PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 120.0f);
+PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 60.0f);
 
 /**
  * Max acro roll rate

src/modules/mc_att_control_multiplatform/mc_att_control_params.h

@@ -56,7 +56,7 @@
 #define PARAM_MP_YAWRATE_I_DEFAULT 0.0f
 #define PARAM_MP_YAWRATE_D_DEFAULT 0.0f
 #define PARAM_MP_YAW_FF_DEFAULT 0.5f
-#define PARAM_MP_YAWRATE_MAX_DEFAULT 120.0f
+#define PARAM_MP_YAWRATE_MAX_DEFAULT 60.0f
 #define PARAM_MP_ACRO_R_MAX_DEFAULT 35.0f
 #define PARAM_MP_ACRO_P_MAX_DEFAULT 35.0f
 #define PARAM_MP_ACRO_Y_MAX_DEFAULT 120.0f

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -80,6 +80,9 @@
 #include <mavlink/mavlink_log.h>
 #include <platforms/px4_defines.h>
 
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+
 #define TILT_COS_MAX	0.7f
 #define SIGMA			0.000001f
 #define MIN_DIST		0.01f
@@ -92,7 +95,7 @@
  */
 extern "C" __EXPORT int mc_pos_control_main(int argc, char *argv[]);
 
-class MulticopterPositionControl
+class MulticopterPositionControl : public control::SuperBlock
 {
 public:
 	/**
@@ -119,15 +122,15 @@ private:
 	int		_control_task;			/**< task handle for task */
 	int		_mavlink_fd;			/**< mavlink fd */
 
-	int 	_vehicle_status_sub;	/**< vehicle status subscription */
-	int		_att_sub;				/**< vehicle attitude subscription */
+	int		_vehicle_status_sub;		/**< vehicle status subscription */
+	int		_att_sub;			/**< vehicle attitude subscription */
 	int		_att_sp_sub;			/**< vehicle attitude setpoint */
 	int		_control_mode_sub;		/**< vehicle control mode subscription */
 	int		_params_sub;			/**< notification of parameter updates */
 	int		_manual_sub;			/**< notification of manual control updates */
 	int		_arming_sub;			/**< arming status of outputs */
 	int		_local_pos_sub;			/**< vehicle local position */
-	int		_pos_sp_triplet_sub;	/**< position setpoint triplet */
+	int		_pos_sp_triplet_sub;		/**< position setpoint triplet */
 	int		_local_pos_sp_sub;		/**< offboard local position setpoint */
 	int		_global_vel_sp_sub;		/**< offboard global velocity setpoint */
 
@@ -135,17 +138,19 @@ private:
 	orb_advert_t	_local_pos_sp_pub;		/**< vehicle local position setpoint publication */
 	orb_advert_t	_global_vel_sp_pub;		/**< vehicle global velocity setpoint publication */
 
-	struct vehicle_status_s 			_vehicle_status; /**< vehicle status */
+	struct vehicle_status_s 			_vehicle_status; 	/**< vehicle status */
 	struct vehicle_attitude_s			_att;			/**< vehicle attitude */
 	struct vehicle_attitude_setpoint_s		_att_sp;		/**< vehicle attitude setpoint */
 	struct manual_control_setpoint_s		_manual;		/**< r/c channel data */
-	struct vehicle_control_mode_s			_control_mode;	/**< vehicle control mode */
+	struct vehicle_control_mode_s			_control_mode;		/**< vehicle control mode */
 	struct actuator_armed_s				_arming;		/**< actuator arming status */
 	struct vehicle_local_position_s			_local_pos;		/**< vehicle local position */
 	struct position_setpoint_triplet_s		_pos_sp_triplet;	/**< vehicle global position setpoint triplet */
 	struct vehicle_local_position_setpoint_s	_local_pos_sp;		/**< vehicle local position setpoint */
-	struct vehicle_global_velocity_setpoint_s	_global_vel_sp;	/**< vehicle global velocity setpoint */
+	struct vehicle_global_velocity_setpoint_s	_global_vel_sp;		/**< vehicle global velocity setpoint */
 
+	control::BlockParamFloat _manual_thr_min;
+	control::BlockParamFloat _manual_thr_max;
 
 	struct {
 		param_t thr_min;
@@ -210,7 +215,7 @@ private:
 	/**
 	 * Update our local parameter cache.
 	 */
-	int			parameters_update(bool force);
+	int		parameters_update(bool force);
 
 	/**
 	 * Update control outputs
@@ -290,7 +295,7 @@ MulticopterPositionControl	*g_control;
 }
 
 MulticopterPositionControl::MulticopterPositionControl() :
-
+	SuperBlock(NULL, "MPC"),
 	_task_should_exit(false),
 	_control_task(-1),
 	_mavlink_fd(-1),
@@ -310,7 +315,8 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	_att_sp_pub(-1),
 	_local_pos_sp_pub(-1),
 	_global_vel_sp_pub(-1),
-
+	_manual_thr_min(this, "MANTHR_MIN"),
+	_manual_thr_max(this, "MANTHR_MAX"),
 	_ref_alt(0.0f),
 	_ref_timestamp(0),
 
@@ -410,6 +416,10 @@ MulticopterPositionControl::parameters_update(bool force)
 	}
 
 	if (updated || force) {
+		/* update C++ param system */
+		updateParams();
+
+		/* update legacy C interface params */
 		param_get(_params_handles.thr_min, &_params.thr_min);
 		param_get(_params_handles.thr_max, &_params.thr_max);
 		param_get(_params_handles.tilt_max_air, &_params.tilt_max_air);
@@ -1413,11 +1423,11 @@ MulticopterPositionControl::task_main()
 
 			/* control throttle directly if no climb rate controller is active */
 			if (!_control_mode.flag_control_climb_rate_enabled) {
-				_att_sp.thrust = math::min(_manual.z, _params.thr_max);
+				_att_sp.thrust = math::min(_manual.z, _manual_thr_max.get());
 
 				/* enforce minimum throttle if not landed */
 				if (!_vehicle_status.condition_landed) {
-					_att_sp.thrust = math::max(_att_sp.thrust, _params.thr_min);
+					_att_sp.thrust = math::max(_att_sp.thrust, _manual_thr_min.get());
 				}
 			}
 

src/modules/mc_pos_control/mc_pos_control_params.c

@@ -41,7 +41,32 @@
 #include <systemlib/param/param.h>
 
 /**
- * Minimum thrust
+ * Minimum thrust in auto thrust control
+ *
+ * It's recommended to set it > 0 to avoid free fall with zero thrust.
+ *
+ * @min 0.05
+ * @max 1.0
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.12f);
+
+/**
+ * Maximum thrust in auto thrust control
+ *
+ * Limit max allowed thrust. Setting a value of one can put
+ * the system into actuator saturation as no spread between
+ * the motors is possible any more. A value of 0.8 - 0.9
+ * is recommended.
+ *
+ * @min 0.0
+ * @max 0.95
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.9f);
+
+/**
+ * Minimum manual thrust
  *
  * Minimum vertical thrust. It's recommended to set it > 0 to avoid free fall with zero thrust.
  *
@@ -49,10 +74,10 @@
  * @max 1.0
  * @group Multicopter Position Control
  */
-PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.12f);
+PARAM_DEFINE_FLOAT(MPC_MANTHR_MIN, 0.12f);
 
 /**
- * Maximum thrust
+ * Maximum manual thrust
  *
  * Limit max allowed thrust. Setting a value of one can put
  * the system into actuator saturation as no spread between
@@ -63,7 +88,7 @@ PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.12f);
  * @max 1.0
  * @group Multicopter Position Control
  */
-PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.9f);
+PARAM_DEFINE_FLOAT(MPC_MANTHR_MAX, 0.9f);
 
 /**
  * Proportional gain for vertical position error

src/modules/navigator/mission.cpp

@@ -70,8 +70,8 @@ Mission::Mission(Navigator *navigator, const char *name) :
 	_param_dist_1wp(this, "MIS_DIST_1WP", false),
 	_param_altmode(this, "MIS_ALTMODE", false),
 	_param_yawmode(this, "MIS_YAWMODE", false),
-	_onboard_mission({0}),
-	_offboard_mission({0}),
+	_onboard_mission{},
+	_offboard_mission{},
 	_current_onboard_mission_index(-1),
 	_current_offboard_mission_index(-1),
 	_need_takeoff(true),
@@ -110,32 +110,29 @@ Mission::on_inactive()
 			update_offboard_mission();
 		}
 
-		/* check if the home position became valid in the meantime */
-		if ((_mission_type == MISSION_TYPE_NONE || _mission_type == MISSION_TYPE_OFFBOARD) &&
-			!_home_inited && _navigator->home_position_valid()) {
+	} else {
 
-			dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
+		/* load missions from storage */
+		mission_s mission_state;
 
-			_navigator->get_mission_result()->valid = _missionFeasiblityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), _navigator->get_vstatus()->is_rotary_wing,
-					dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
-					_navigator->get_home_position()->alt, _navigator->home_position_valid(),
-					_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
-					_param_dist_1wp.get(), _navigator->get_mission_result()->warning);
+		dm_lock(DM_KEY_MISSION_STATE);
 
-			_navigator->increment_mission_instance_count();
-			_navigator->set_mission_result_updated();
+		/* read current state */
+		int read_res = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s));
 
-			_home_inited = true;
+		dm_unlock(DM_KEY_MISSION_STATE);
+
+		if (read_res == sizeof(mission_s)) {
+			_offboard_mission.dataman_id = mission_state.dataman_id;
+			_offboard_mission.count = mission_state.count;
+			_current_offboard_mission_index = mission_state.current_seq;
 		}
 
-	} else {
-		/* read mission topics on initialization */
 		_inited = true;
-
-		update_onboard_mission();
-		update_offboard_mission();
 	}
 
+	check_mission_valid();
+
 	/* require takeoff after non-loiter or landing */
 	if (!_navigator->get_can_loiter_at_sp() || _navigator->get_vstatus()->condition_landed) {
 		_need_takeoff = true;
@@ -151,6 +148,8 @@ Mission::on_activation()
 void
 Mission::on_active()
 {
+	check_mission_valid();
+
 	/* check if anything has changed */
 	bool onboard_updated = false;
 	orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);
@@ -272,6 +271,8 @@ Mission::update_offboard_mission()
 		_offboard_mission.count = 0;
 		_offboard_mission.current_seq = 0;
 		_current_offboard_mission_index = 0;
+
+		warnx("mission check failed");
 	}
 
 	set_current_offboard_mission_item();
@@ -652,6 +653,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
 
 		if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) {
 			/* mission item index out of bounds */
+			mavlink_log_critical(_navigator->get_mavlink_fd(), "[wpm] err: index: %d, max: %d", *mission_index_ptr, (int)mission->count);
 			return false;
 		}
 
@@ -795,3 +797,26 @@ Mission::set_mission_finished()
 	_navigator->get_mission_result()->finished = true;
 	_navigator->set_mission_result_updated();
 }
+
+bool
+Mission::check_mission_valid()
+{
+	/* check if the home position became valid in the meantime */
+	if (!_home_inited && _navigator->home_position_valid()) {
+
+		dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
+
+		_navigator->get_mission_result()->valid = _missionFeasiblityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), _navigator->get_vstatus()->is_rotary_wing,
+				dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
+				_navigator->get_home_position()->alt, _navigator->home_position_valid(),
+				_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
+				_param_dist_1wp.get(), _navigator->get_mission_result()->warning);
+
+		_navigator->increment_mission_instance_count();
+		_navigator->set_mission_result_updated();
+
+		_home_inited = true;
+	}
+
+	return _navigator->get_mission_result()->valid;
+}

src/modules/navigator/mission.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-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
@@ -39,6 +39,7 @@
  * @author Thomas Gubler <thomasgubler@gmail.com>
  * @author Anton Babushkin <anton.babushkin@me.com>
  * @author Ban Siesta <bansiesta@gmail.com>
+ * @author Lorenz Meier <lorenz@px4.io>
  */
 
 #ifndef NAVIGATOR_MISSION_H
@@ -165,6 +166,11 @@ private:
 	 */
 	void set_mission_finished();
 
+	/**
+	 * Check wether a mission is ready to go
+	 */
+	bool check_mission_valid();
+
 	control::BlockParamInt _param_onboard_enabled;
 	control::BlockParamFloat _param_takeoff_alt;
 	control::BlockParamFloat _param_dist_1wp;

src/modules/sdlog2/sdlog2.c

@@ -156,7 +156,7 @@ PARAM_DEFINE_INT32(SDLOG_EXT, -1);
  * @max  1
  * @group SD Logging
  */
-PARAM_DEFINE_INT32(SDLOG_GPSTIME, 0);
+PARAM_DEFINE_INT32(SDLOG_GPSTIME, 1);
 
 #define LOGBUFFER_WRITE_AND_COUNT(_msg) if (logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(_msg))) { \
 		log_msgs_written++; \
@@ -344,7 +344,20 @@ int sdlog2_main(int argc, char *argv[])
 						 3000,
 						 sdlog2_thread_main,
 						 (char * const *)argv);
-		exit(0);
+
+		/* wait for the task to launch */
+		unsigned const max_wait_us = 1000000;
+		unsigned const max_wait_steps = 2000;
+
+		unsigned i;
+		for (i = 0; i < max_wait_steps; i++) {
+			usleep(max_wait_us / max_wait_steps);
+			if (thread_running) {
+				break;
+			}
+		}
+
+		exit(!(i < max_wait_steps));
 	}
 
 	if (!strcmp(argv[1], "stop")) {
@@ -1228,8 +1241,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 	/* close stdout */
 	close(1);
 
-	thread_running = true;
-
 	/* initialize thread synchronization */
 	pthread_mutex_init(&logbuffer_mutex, NULL);
 	pthread_cond_init(&logbuffer_cond, NULL);
@@ -1263,6 +1274,9 @@ int sdlog2_thread_main(int argc, char *argv[])
 		sdlog2_start_log();
 	}
 
+	/* running, report */
+	thread_running = true;
+
 	while (!main_thread_should_exit) {
 		usleep(sleep_delay);
 

src/modules/sensors/sensor_params.c

@@ -680,7 +680,7 @@ PARAM_DEFINE_INT32(SENS_FLOW_ROT, 0);
  * This parameter defines a rotational offset in degrees around the Y (Pitch) axis. It allows the user
  * to fine tune the board offset in the event of misalignment.
  *
- * @unit radians
+ * @unit degrees
  * @group Sensor Calibration
  */
 PARAM_DEFINE_FLOAT(SENS_BOARD_Y_OFF, 0.0f);
@@ -691,7 +691,7 @@ PARAM_DEFINE_FLOAT(SENS_BOARD_Y_OFF, 0.0f);
  * This parameter defines a rotational offset in degrees around the X (Roll) axis It allows the user
  * to fine tune the board offset in the event of misalignment.
  *
- * @unit radians
+ * @unit degrees
  * @group Sensor Calibration
  */
 PARAM_DEFINE_FLOAT(SENS_BOARD_X_OFF, 0.0f);
@@ -702,7 +702,7 @@ PARAM_DEFINE_FLOAT(SENS_BOARD_X_OFF, 0.0f);
  * This parameter defines a rotational offset in degrees around the Z (Yaw) axis. It allows the user
  * to fine tune the board offset in the event of misalignment.
  *
- * @unit radians
+ * @unit degrees
  * @group Sensor Calibration
  */
 PARAM_DEFINE_FLOAT(SENS_BOARD_Z_OFF, 0.0f);
@@ -2273,7 +2273,7 @@ PARAM_DEFINE_INT32(SENS_EN_LL40LS, 0);
  * REBOOT IN ORDER TO APPLY THE CHANGES. COMPLETELY POWER-CYCLE
  * THE SYSTEM TO PUT CHANGES INTO EFFECT.
  *
- * Set to 1000 for default or 900 to increase servo travel
+ * Set to 1000 for industry default or 900 to increase servo travel.
  *
  * @min 800
  * @max 1400
@@ -2289,7 +2289,7 @@ PARAM_DEFINE_INT32(PWM_MIN, 1000);
  * REBOOT IN ORDER TO APPLY THE CHANGES. COMPLETELY POWER-CYCLE
  * THE SYSTEM TO PUT CHANGES INTO EFFECT.
  *
- * Set to 2000 for default or 2100 to increase servo travel
+ * Set to 2000 for industry default or 2100 to increase servo travel.
  *
  * @min 1600
  * @max 2200

src/modules/systemlib/cpuload.c

@@ -78,8 +78,6 @@ void cpuload_initialize_once()
 		system_load.tasks[i].valid = false;
 	}
 
-	uint64_t now = hrt_absolute_time();
-
 	int static_tasks_count = 2;	// there are at least 2 threads that should be initialized statically - "idle" and "init"
 
 #ifdef CONFIG_PAGING
@@ -94,7 +92,6 @@ void cpuload_initialize_once()
 
 	// perform static initialization of "system" threads
 	for (system_load.total_count = 0; system_load.total_count < static_tasks_count; system_load.total_count++) {
-		system_load.tasks[system_load.total_count].start_time = now;
 		system_load.tasks[system_load.total_count].total_runtime = 0;
 		system_load.tasks[system_load.total_count].curr_start_time = 0;
 		system_load.tasks[system_load.total_count].tcb = sched_gettcb(
@@ -111,7 +108,6 @@ void sched_note_start(FAR struct tcb_s *tcb)
 	for (i = 1; i < CONFIG_MAX_TASKS; i++) {
 		if (!system_load.tasks[i].valid) {
 			/* slot is available */
-			system_load.tasks[i].start_time = hrt_absolute_time();
 			system_load.tasks[i].total_runtime = 0;
 			system_load.tasks[i].curr_start_time = 0;
 			system_load.tasks[i].tcb = tcb;

src/modules/systemlib/cpuload.h

@@ -42,7 +42,6 @@ __BEGIN_DECLS
 struct system_load_taskinfo_s {
 	uint64_t total_runtime;			///< Runtime since start (start_time - total_runtime)/(start_time - current_time) = load
 	uint64_t curr_start_time;		///< Start time of the current scheduling slot
-	uint64_t start_time;			///< FIRST start time of task
 	FAR struct tcb_s *tcb;			///<
 	bool valid;						///< Task is currently active / valid
 };

src/modules/vtol_att_control/module.mk

@@ -42,6 +42,8 @@ SRCS		= vtol_att_control_main.cpp \
 			  tiltrotor_params.c \
 			  tiltrotor.cpp \
 			  vtol_type.cpp \
-			  tailsitter.cpp
+			  tailsitter.cpp \
+			  standard_params.c \
+			  standard.cpp
 
 EXTRACXXFLAGS = -Wno-write-strings

src/modules/vtol_att_control/standard.cpp

@@ -0,0 +1,296 @@
+/****************************************************************************
+ *
+ *   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 standard.cpp
+ *
+ * @author Simon Wilks		<simon@uaventure.com>
+ * @author Roman Bapst 		<bapstroman@gmail.com>
+ *
+*/
+
+#include "standard.h"
+#include "vtol_att_control_main.h"
+
+Standard::Standard(VtolAttitudeControl *attc) :
+	VtolType(attc),
+	_flag_enable_mc_motors(true),
+	_pusher_throttle(0.0f),
+	_mc_att_ctl_weight(1.0f),
+	_airspeed_trans_blend_margin(0.0f)
+{
+	_vtol_schedule.flight_mode = MC_MODE;
+	_vtol_schedule.transition_start = 0;
+
+	_params_handles_standard.front_trans_dur = param_find("VT_F_TRANS_DUR");
+	_params_handles_standard.back_trans_dur = param_find("VT_B_TRANS_DUR");
+	_params_handles_standard.pusher_trans = param_find("VT_TRANS_THR");
+	_params_handles_standard.airspeed_blend = param_find("VT_ARSP_BLEND");
+	_params_handles_standard.airspeed_trans = param_find("VT_ARSP_TRANS");
+ }
+
+Standard::~Standard()
+{
+}
+
+int
+Standard::parameters_update()
+{
+	float v;
+
+	/* duration of a forwards transition to fw mode */
+	param_get(_params_handles_standard.front_trans_dur, &v);
+	_params_standard.front_trans_dur = math::constrain(v, 0.0f, 5.0f);
+
+	/* duration of a back transition to mc mode */
+	param_get(_params_handles_standard.back_trans_dur, &v);
+	_params_standard.back_trans_dur = math::constrain(v, 0.0f, 5.0f);
+
+	/* target throttle value for pusher motor during the transition to fw mode */
+	param_get(_params_handles_standard.pusher_trans, &v);
+	_params_standard.pusher_trans = math::constrain(v, 0.0f, 5.0f);
+
+	/* airspeed at which it we should switch to fw mode */
+	param_get(_params_handles_standard.airspeed_trans, &v);
+	_params_standard.airspeed_trans = math::constrain(v, 1.0f, 20.0f);
+
+	/* airspeed at which we start blending mc/fw controls */
+	param_get(_params_handles_standard.airspeed_blend, &v);
+	_params_standard.airspeed_blend = math::constrain(v, 0.0f, 20.0f);
+
+	_airspeed_trans_blend_margin = _params_standard.airspeed_trans - _params_standard.airspeed_blend;
+
+	return OK;
+}
+
+void Standard::update_vtol_state()
+{
+ 	parameters_update();
+
+	/* After flipping the switch the vehicle will start the pusher (or tractor) motor, picking up
+	 * forward speed. After the vehicle has picked up enough speed the rotors shutdown. 
+	 * For the back transition the pusher motor is immediately stopped and rotors reactivated.
+ 	 */
+
+	if (_manual_control_sp->aux1 < 0.0f) {
+		// the transition to fw mode switch is off
+		if (_vtol_schedule.flight_mode == MC_MODE) {
+			// in mc mode
+			_vtol_schedule.flight_mode = MC_MODE;
+			_mc_att_ctl_weight = 1.0f;
+
+		} else if (_vtol_schedule.flight_mode == FW_MODE) {
+			// transition to mc mode
+			_vtol_schedule.flight_mode = TRANSITION_TO_MC;
+			_flag_enable_mc_motors = true;
+			_vtol_schedule.transition_start = hrt_absolute_time();
+
+		} else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) {
+			// failsafe back to mc mode
+			_vtol_schedule.flight_mode = MC_MODE;
+			_mc_att_ctl_weight = 1.0f;
+
+		} else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) {
+			// keep transitioning to mc mode
+			_vtol_schedule.flight_mode = MC_MODE;
+		}
+
+		// the pusher motor should never be powered when in or transitioning to mc mode
+		_pusher_throttle = 0.0f;
+
+	} else {
+		// the transition to fw mode switch is on
+		if (_vtol_schedule.flight_mode == MC_MODE) {
+			// start transition to fw mode
+			_vtol_schedule.flight_mode = TRANSITION_TO_FW;
+			_vtol_schedule.transition_start = hrt_absolute_time();
+
+		} else if (_vtol_schedule.flight_mode == FW_MODE) {
+			// in fw mode
+			_vtol_schedule.flight_mode = FW_MODE;
+			_mc_att_ctl_weight = 0.0f;
+
+		} 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) {
+				_vtol_schedule.flight_mode = FW_MODE;
+				// we can turn off the multirotor motors now
+				_flag_enable_mc_motors = false;
+				// don't set pusher throttle here as it's being ramped up elsewhere
+			}
+
+		} else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) {
+			// transitioning to mc mode & transition switch on - failsafe back into fw mode
+			_vtol_schedule.flight_mode = FW_MODE;
+		}
+	}
+
+	// map specific control phases to simple control modes
+	switch(_vtol_schedule.flight_mode) {
+		case MC_MODE:
+			_vtol_mode = ROTARY_WING;
+			break;
+		case FW_MODE:
+			_vtol_mode = FIXED_WING;
+			break;
+		case TRANSITION_TO_FW:
+		case TRANSITION_TO_MC:
+			_vtol_mode = TRANSITION;
+			break;
+	}
+}
+
+void Standard::update_transition_state()
+{
+	if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) {
+		if (_params_standard.front_trans_dur <= 0.0f) {
+			// just set the final target throttle value
+			_pusher_throttle = _params_standard.pusher_trans;
+		} else if (_pusher_throttle <= _params_standard.pusher_trans) {
+			// ramp up throttle to the target throttle value
+			_pusher_throttle = _params_standard.pusher_trans * 
+						(float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.front_trans_dur * 1000000.0f);
+		}
+
+		// do blending of mc and fw controls if a blending airspeed has been provided
+		if (_airspeed_trans_blend_margin > 0.0f && _airspeed->true_airspeed_m_s >= _params_standard.airspeed_blend) {
+			_mc_att_ctl_weight = 1.0f - fabsf(_airspeed->true_airspeed_m_s - _params_standard.airspeed_blend) / _airspeed_trans_blend_margin;
+		} else {
+			// at low speeds give full weight to mc
+			_mc_att_ctl_weight = 1.0f;
+		}
+
+	} else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) {
+		// continually increase mc attitude control as we transition back to mc mode
+		if (_params_standard.back_trans_dur > 0.0f) {
+			_mc_att_ctl_weight = (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.back_trans_dur * 1000000.0f);
+		} else {
+			_mc_att_ctl_weight = 1.0f;	
+		}
+
+		// in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again	
+		if (_flag_enable_mc_motors) {
+			set_max_mc(2000);
+			set_idle_mc();
+			_flag_enable_mc_motors = false;
+		}
+	}
+
+	_mc_att_ctl_weight = math::constrain(_mc_att_ctl_weight, 0.0f, 1.0f);
+}
+
+void Standard::update_mc_state()
+{
+	// do nothing
+}
+
+void Standard::process_mc_data()
+{
+	fill_att_control_output();
+}
+
+void Standard::process_fw_data()
+{
+	fill_att_control_output();
+}
+
+ void Standard::update_fw_state()
+{
+	// in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again	
+	if (!_flag_enable_mc_motors) {
+		set_max_mc(950);
+		set_idle_fw();  // force them to stop, not just idle
+		_flag_enable_mc_motors = true;
+	}
+ }
+
+void Standard::update_external_state()
+{
+}
+
+/**
+ * Prepare message to acutators with data from mc and fw attitude controllers. An mc attitude weighting will determine
+ * what proportion of control should be applied to each of the control groups (mc and fw).
+ */
+void Standard::fill_att_control_output()
+{
+	/* multirotor controls */
+	_actuators_out_0->control[0] = _actuators_mc_in->control[0] * _mc_att_ctl_weight;	// roll
+	_actuators_out_0->control[1] = _actuators_mc_in->control[1] * _mc_att_ctl_weight;	// pitch
+	_actuators_out_0->control[2] = _actuators_mc_in->control[2] * _mc_att_ctl_weight;	// yaw
+	_actuators_out_0->control[3] = _actuators_mc_in->control[3];	// throttle
+
+	/* fixed wing controls */
+	const float fw_att_ctl_weight = 1.0f - _mc_att_ctl_weight;
+	_actuators_out_1->control[0] = -_actuators_fw_in->control[0] * fw_att_ctl_weight;	//roll
+	_actuators_out_1->control[1] = (_actuators_fw_in->control[1] + _params->fw_pitch_trim) * fw_att_ctl_weight;	//pitch
+	_actuators_out_1->control[2] = _actuators_fw_in->control[2] * fw_att_ctl_weight;	// yaw
+
+	// set the fixed wing throttle control
+	if (_vtol_schedule.flight_mode == FW_MODE) {
+		// take the throttle value commanded by the fw controller
+		_actuators_out_1->control[3] = _actuators_fw_in->control[3];
+	} else {
+		// otherwise we may be ramping up the throttle during the transition to fw mode 
+		_actuators_out_1->control[3] = _pusher_throttle;
+	}
+}
+
+/**
+* Disable all multirotor motors when in fw mode.
+*/
+void
+Standard::set_max_mc(unsigned pwm_value)
+{
+	int ret;
+	unsigned servo_count;
+	char *dev = PWM_OUTPUT0_DEVICE_PATH;
+	int fd = open(dev, 0);
+
+	if (fd < 0) {err(1, "can't open %s", dev);}
+
+	ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
+	struct pwm_output_values pwm_values;
+	memset(&pwm_values, 0, sizeof(pwm_values));
+
+	for (int i = 0; i < _params->vtol_motor_count; i++) {
+		pwm_values.values[i] = pwm_value;
+		pwm_values.channel_count = _params->vtol_motor_count;
+	}
+
+	ret = ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values);
+
+	if (ret != OK) {errx(ret, "failed setting max values");}
+
+	close(fd);
+}

src/modules/vtol_att_control/standard.h

@@ -0,0 +1,107 @@
+/****************************************************************************
+ *
+ *   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 standard.h
+ * VTOL with fixed multirotor motor configurations (such as quad) and a pusher
+ * (or puller aka tractor) motor for forward flight.
+ *
+ * @author Simon Wilks 		<simon@uaventure.com>
+ * @author Roman Bapst 		<bapstroman@gmail.com>
+ *
+ */
+
+#ifndef STANDARD_H
+#define STANDARD_H
+#include "vtol_type.h"
+#include <systemlib/param/param.h>
+#include <drivers/drv_hrt.h>
+
+class Standard : public VtolType
+{
+
+public:
+
+	Standard(VtolAttitudeControl * _att_controller);
+	~Standard();
+
+	void update_vtol_state();
+	void update_mc_state();
+	void process_mc_data();
+	void update_fw_state();
+	void process_fw_data();
+	void update_transition_state();
+	void update_external_state();
+
+private:
+
+	struct {
+		float front_trans_dur;
+		float back_trans_dur;
+		float pusher_trans;
+		float airspeed_blend;
+		float airspeed_trans;
+	} _params_standard;
+
+	struct {
+		param_t front_trans_dur;
+		param_t back_trans_dur;
+		param_t pusher_trans;
+		param_t airspeed_blend;
+		param_t airspeed_trans;
+	} _params_handles_standard;
+
+	enum vtol_mode {
+		MC_MODE = 0,
+		TRANSITION_TO_FW,
+		TRANSITION_TO_MC,
+		FW_MODE
+	};
+
+	struct {
+		vtol_mode flight_mode;			// indicates in which mode the vehicle is in
+		hrt_abstime transition_start;	// at what time did we start a transition (front- or backtransition)
+	}_vtol_schedule;
+
+	bool _flag_enable_mc_motors;
+	float _pusher_throttle;
+	float _mc_att_ctl_weight;	// the amount of multicopter attitude control that should be applied in fixed wing mode while transitioning 
+	float _airspeed_trans_blend_margin;
+
+	void fill_att_control_output();
+	void set_max_mc(unsigned pwm_value);
+
+	int parameters_update();
+
+};
+#endif

src/modules/vtol_att_control/standard_params.c

@@ -0,0 +1,51 @@
+/****************************************************************************
+ *
+ *   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 standard_params.c
+ * Parameters for the standard VTOL controller.
+ *
+ * @author Simon Wilks	<simon@uaventure.com>
+ * @author Roman Bapst	<bapstroman@gmail.com>
+ */
+
+#include <systemlib/param/param.h>
+
+/**
+ * Target throttle value for pusher/puller motor during the transition to fw mode
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_TRANS_THR, 0.6f);

src/modules/vtol_att_control/tiltrotor.cpp

@@ -194,7 +194,7 @@ void Tiltrotor::update_mc_state()
 
 void Tiltrotor::process_mc_data()
 {
-	fill_mc_att_control_output();
+	fill_att_control_output();
 }
 
  void Tiltrotor::update_fw_state()
@@ -222,19 +222,22 @@ void Tiltrotor::process_mc_data()
 
 void Tiltrotor::process_fw_data()
 {
-	fill_fw_att_control_output();
+	fill_att_control_output();
 }
 
 void Tiltrotor::update_transition_state()
 {
 	if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) {
 		// tilt rotors forward up to certain angle
-		if (_tilt_control <= _params_tiltrotor.tilt_transition) {
-			_tilt_control = _params_tiltrotor.tilt_mc +  fabsf(_params_tiltrotor.tilt_transition - _params_tiltrotor.tilt_mc)*(float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.front_trans_dur*1000000.0f);
+		if (_params_tiltrotor.front_trans_dur <= 0.0f) {
+			_tilt_control = _params_tiltrotor.tilt_transition;
+		} else if (_tilt_control <= _params_tiltrotor.tilt_transition) {
+			_tilt_control = _params_tiltrotor.tilt_mc + fabsf(_params_tiltrotor.tilt_transition - _params_tiltrotor.tilt_mc) * 
+						(float) hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tiltrotor.front_trans_dur * 1000000.0f);
 		}
 
 		// do blending of mc and fw controls
-		if (_airspeed->true_airspeed_m_s >= ARSP_BLEND_START) {
+		if (_airspeed->true_airspeed_m_s >= ARSP_BLEND_START && _params_tiltrotor.airspeed_trans - ARSP_BLEND_START > 0.0f) {
 			_roll_weight_mc = 1.0f - (_airspeed->true_airspeed_m_s - ARSP_BLEND_START) / (_params_tiltrotor.airspeed_trans - ARSP_BLEND_START);
 		} else {
 			// at low speeds give full weight to mc
@@ -244,13 +247,14 @@ void Tiltrotor::update_transition_state()
 		_roll_weight_mc = math::constrain(_roll_weight_mc, 0.0f, 1.0f);
 
 	} else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2) {
-		_tilt_control = _params_tiltrotor.tilt_transition +  fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_transition)*(float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(0.5f*1000000.0f);
+		_tilt_control = _params_tiltrotor.tilt_transition +  fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_transition) *
+					(float) hrt_elapsed_time(&_vtol_schedule.transition_start) / (0.5f * 1000000.0f);
 		_roll_weight_mc = 0.0f;
 	} else if (_vtol_schedule.flight_mode == TRANSITION_BACK) {
 		// tilt rotors forward up to certain angle
-		float progress = (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.back_trans_dur*1000000.0f);
+		float progress = (float) hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tiltrotor.back_trans_dur * 1000000.0f);
 		if (_tilt_control > _params_tiltrotor.tilt_mc) {
-			_tilt_control = _params_tiltrotor.tilt_fw -  fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_mc)*progress;
+			_tilt_control = _params_tiltrotor.tilt_fw -  fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_mc) * progress;
 		}
 
 		_roll_weight_mc = progress;
@@ -263,38 +267,26 @@ void Tiltrotor::update_external_state()
 }
 
  /**
-* Prepare message to acutators with data from mc attitude controller.
+* Prepare message to acutators with data from the attitude controllers.
 */
-void Tiltrotor::fill_mc_att_control_output()
+void Tiltrotor::fill_att_control_output()
 {
-	_actuators_out_0->control[0] = _actuators_mc_in->control[0];
-	_actuators_out_0->control[1] = _actuators_mc_in->control[1];
-	_actuators_out_0->control[2] = _actuators_mc_in->control[2];
-	_actuators_out_0->control[3] = _actuators_mc_in->control[3];
+	_actuators_out_0->control[0] = _actuators_mc_in->control[0] * _roll_weight_mc;	// roll
+	_actuators_out_0->control[1] = _actuators_mc_in->control[1] * _roll_weight_mc;	// pitch
+	_actuators_out_0->control[2] = _actuators_mc_in->control[2] * _roll_weight_mc;	// yaw
+
+	if (_vtol_schedule.flight_mode == FW_MODE) {
+		_actuators_out_1->control[3] = _actuators_fw_in->control[3];	// fw throttle
+	} else {
+		_actuators_out_0->control[3] = _actuators_mc_in->control[3];	// mc throttle
+	}
 
 	_actuators_out_1->control[0] = -_actuators_fw_in->control[0] * (1.0f - _roll_weight_mc);	//roll elevon
-	_actuators_out_1->control[1] = (_actuators_fw_in->control[1] + _params->fw_pitch_trim)* (1.0f -_roll_weight_mc);	//pitch elevon
-
+	_actuators_out_1->control[1] = (_actuators_fw_in->control[1] + _params->fw_pitch_trim)* (1.0f -_roll_weight_mc);	//pitch elevon	
 	_actuators_out_1->control[4] = _tilt_control;	// for tilt-rotor control
-}
 
-/**
-* Prepare message to acutators with data from fw attitude controller.
-*/
-void Tiltrotor::fill_fw_att_control_output()
-{
-	/*For the first test in fw mode, only use engines for thrust!!!*/
-	_actuators_out_0->control[0] = _actuators_mc_in->control[0] * _roll_weight_mc;
-	_actuators_out_0->control[1] = _actuators_mc_in->control[1] * _roll_weight_mc;
-	_actuators_out_0->control[2] = _actuators_mc_in->control[2] * _roll_weight_mc;
-	_actuators_out_0->control[3] = _actuators_fw_in->control[3];
-	/*controls for the elevons */
-	_actuators_out_1->control[0] = -_actuators_fw_in->control[0];	// roll elevon
-	_actuators_out_1->control[1] = _actuators_fw_in->control[1] + _params->fw_pitch_trim;	// pitch elevon
 	// unused now but still logged
-	_actuators_out_1->control[2] = _actuators_fw_in->control[2];	// yaw
-	_actuators_out_1->control[3] = _actuators_fw_in->control[3];	// throttle
-	_actuators_out_1->control[4] = _tilt_control;
+	_actuators_out_1->control[2] = _actuators_fw_in->control[2];	// fw yaw
 }
 
 /**

src/modules/vtol_att_control/tiltrotor.h

@@ -99,8 +99,7 @@ private:
 	float _tilt_control;
 	float _roll_weight_mc;
 
-	void fill_mc_att_control_output();
-	void fill_fw_att_control_output();
+	void fill_att_control_output();
 	void set_max_mc();
 	void set_max_fw(unsigned pwm_value);
 

src/modules/vtol_att_control/tiltrotor_params.c

@@ -39,28 +39,6 @@
  */
 
 #include <systemlib/param/param.h>
- 
-/**
- * Duration of a front transition
- *
- * Time in seconds used for a transition
- *
- * @min 0.0
- * @max 5
- * @group VTOL Attitude Control
- */
-PARAM_DEFINE_FLOAT(VT_F_TRANS_DUR,3.0f);
-
-/**
- * Duration of a back transition
- *
- * Time in seconds used for a back transition
- *
- * @min 0.0
- * @max 5
- * @group VTOL Attitude Control
- */
-PARAM_DEFINE_FLOAT(VT_B_TRANS_DUR,2.0f);
 
 /**
  * Position of tilt servo in mc mode
@@ -71,7 +49,7 @@ PARAM_DEFINE_FLOAT(VT_B_TRANS_DUR,2.0f);
  * @max 1
  * @group VTOL Attitude Control
  */
-PARAM_DEFINE_FLOAT(VT_TILT_MC,0.0f);
+PARAM_DEFINE_FLOAT(VT_TILT_MC, 0.0f);
 
 /**
  * Position of tilt servo in transition mode
@@ -82,7 +60,7 @@ PARAM_DEFINE_FLOAT(VT_TILT_MC,0.0f);
  * @max 1
  * @group VTOL Attitude Control
  */
-PARAM_DEFINE_FLOAT(VT_TILT_TRANS,0.3f);
+PARAM_DEFINE_FLOAT(VT_TILT_TRANS, 0.3f);
 
 /**
  * Position of tilt servo in fw mode
@@ -93,15 +71,4 @@ PARAM_DEFINE_FLOAT(VT_TILT_TRANS,0.3f);
  * @max 1
  * @group VTOL Attitude Control
  */
-PARAM_DEFINE_FLOAT(VT_TILT_FW,1.0f);
-
-/**
- * Transition airspeed
- *
- * Airspeed at which we can switch to fw mode
- *
- * @min 0.0
- * @max 20
- * @group VTOL Attitude Control
- */
-PARAM_DEFINE_FLOAT(VT_ARSP_TRANS,10.0f);
+PARAM_DEFINE_FLOAT(VT_TILT_FW, 1.0f);

src/modules/vtol_att_control/vtol_att_control_main.cpp

@@ -122,6 +122,9 @@ VtolAttitudeControl::VtolAttitudeControl() :
 	} else if (_params.vtol_type == 1) {
 		_tiltrotor = new Tiltrotor(this);
 		_vtol_type = _tiltrotor;
+	} else if (_params.vtol_type == 2) {
+		_standard = new Standard(this);
+		_vtol_type = _standard;
 	} else {
 		_task_should_exit = true;
 	}
@@ -530,6 +533,9 @@ void VtolAttitudeControl::task_main()
 			// update transition state if got any new data
 			if (got_new_data) {
 				_vtol_type->update_transition_state();
+
+				_vtol_type->process_mc_data();
+				fill_mc_att_rates_sp();
 			}
 
 		} else if (_vtol_type->get_mode() == EXTERNAL) {

src/modules/vtol_att_control/vtol_att_control_main.h

@@ -84,6 +84,7 @@
 
 #include "tiltrotor.h"
 #include "tailsitter.h"
+#include "standard.h"
 
 
 extern "C" __EXPORT int vtol_att_control_main(int argc, char *argv[]);
@@ -188,6 +189,7 @@ private:
 	VtolType * _vtol_type;	// base class for different vtol types
 	Tiltrotor * _tiltrotor;	// tailsitter vtol type
 	Tailsitter * _tailsitter;	// tiltrotor vtol type
+	Standard * _standard;	// standard vtol type
 
 //*****************Member functions***********************************************************************
 

src/modules/vtol_att_control/vtol_att_control_params.c

@@ -143,10 +143,10 @@ PARAM_DEFINE_FLOAT(VT_PROP_EFF, 0.0f);
 PARAM_DEFINE_FLOAT(VT_ARSP_LP_GAIN, 0.3f);
 
 /**
- * VTOL Type (Tailsitter=0, Tiltrotor=1)
+ * VTOL Type (Tailsitter=0, Tiltrotor=1, Standard=2)
  *
  * @min 0
- * @max 1
+ * @max 2
  * @group VTOL Attitude Control
  */
 PARAM_DEFINE_INT32(VT_TYPE, 0);
@@ -161,3 +161,47 @@ PARAM_DEFINE_INT32(VT_TYPE, 0);
  * @group VTOL Attitude Control
  */
 PARAM_DEFINE_INT32(VT_ELEV_MC_LOCK, 0);
+
+/**
+ * Duration of a front transition
+ *
+ * Time in seconds used for a transition
+ *
+ * @min 0.0
+ * @max 5
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_F_TRANS_DUR, 3.0f);
+
+/**
+ * Duration of a back transition
+ *
+ * Time in seconds used for a back transition
+ *
+ * @min 0.0
+ * @max 5
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_B_TRANS_DUR, 2.0f);
+
+/**
+ * Transition blending airspeed
+ *
+ * Airspeed at which we can start blending both fw and mc controls. Set to 0 to disable.
+ *
+ * @min 0.0
+ * @max 20.0
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_ARSP_BLEND, 8.0f);
+
+/**
+ * Transition airspeed
+ *
+ * Airspeed at which we can switch to fw mode
+ *
+ * @min 1.0
+ * @max 20
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_ARSP_TRANS, 10.0f);

src/modules/vtol_att_control/vtol_type.h

@@ -38,8 +38,8 @@
  *
  */
 
-#ifndef VTOL_YYPE_H
-#define VTOL_YYPE_H
+#ifndef VTOL_TYPE_H
+#define VTOL_TYPE_H
 
 struct Params {
 	int idle_pwm_mc;			// pwm value for idle in mc mode

src/systemcmds/nshterm/nshterm.c

@@ -68,8 +68,8 @@ nshterm_main(int argc, char *argv[])
     int armed_fd = orb_subscribe(ORB_ID(actuator_armed));
     struct actuator_armed_s armed;
 
-    /* back off 800 ms to avoid running into the USB setup timing */
-    while (hrt_absolute_time() < 800U * 1000U) {
+    /* back off 1500 ms to avoid running into the USB setup timing */
+    while (hrt_absolute_time() < 1500U * 1000U) {
         usleep(50000);
     }