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

Signed-off-by: Mark Charlebois <charlebm@gmail.com>

Conflicts:
	src/modules/commander/gyro_calibration.cpp
	src/modules/mavlink/mavlink_ftp.cpp

src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp

@@ -397,16 +397,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 
 					hrt_abstime vel_t = 0;
 					bool vel_valid = false;
-					if (ekf_params.acc_comp == 1 && gps.fix_type >= 3 && gps.eph < 10.0f && gps.vel_ned_valid && hrt_absolute_time() < gps.timestamp_velocity + 500000) {
-						vel_valid = true;
-						if (gps_updated) {
-							vel_t = gps.timestamp_velocity;
-							vel(0) = gps.vel_n_m_s;
-							vel(1) = gps.vel_e_m_s;
-							vel(2) = gps.vel_d_m_s;
-						}
-
-					} else if (ekf_params.acc_comp == 2 && gps.eph < 5.0f && global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) {
+					if (gps.eph < 5.0f && global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) {
 						vel_valid = true;
 						if (global_pos_updated) {
 							vel_t = global_pos.timestamp;
@@ -487,8 +478,6 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 						if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) {
 							mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f));
 
-						} else {
-							mag_decl = ekf_params.mag_decl;
 						}
 
 						/* update mag declination rotation matrix */

src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c

@@ -108,11 +108,6 @@ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R2, 100.0f);
  */
 PARAM_DEFINE_INT32(EKF_ATT_ENABLED, 1);
 
-/* magnetic declination, in degrees */
-PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f);
-
-PARAM_DEFINE_INT32(ATT_ACC_COMP, 2);
-
 /**
  * Moment of inertia matrix diagonal entry (1, 1)
  *
@@ -160,10 +155,6 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h)
 	h->r1 	=	param_find("EKF_ATT_V4_R1");
 	h->r2 	=	param_find("EKF_ATT_V4_R2");
 
-	h->mag_decl   =	param_find("ATT_MAG_DECL");
-
-	h->acc_comp   =	param_find("ATT_ACC_COMP");
-
 	h->moment_inertia_J[0]  =   param_find("ATT_J11");
 	h->moment_inertia_J[1]  =   param_find("ATT_J22");
 	h->moment_inertia_J[2]  =   param_find("ATT_J33");
@@ -183,11 +174,6 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru
 	param_get(h->r1, &(p->r[1]));
 	param_get(h->r2, &(p->r[2]));
 
-	param_get(h->mag_decl, &(p->mag_decl));
-	p->mag_decl *= M_PI_F / 180.0f;
-
-	param_get(h->acc_comp, &(p->acc_comp));
-
 	for (int i = 0; i < 3; i++) {
 		param_get(h->moment_inertia_J[i], &(p->moment_inertia_J[3 * i + i]));
 	}

src/modules/attitude_estimator_q/attitude_estimator_q_params.c

@@ -41,10 +41,70 @@
 
 #include <systemlib/param/param.h>
 
+/**
+ * Complimentary filter accelerometer weight
+ *
+ * @group Attitude Q estimator
+ * @min 0
+ * @max 1
+ */
 PARAM_DEFINE_FLOAT(ATT_W_ACC, 0.2f);
+
+/**
+ * Complimentary filter magnetometer weight
+ *
+ * @group Attitude Q estimator
+ * @min 0
+ * @max 1
+ */
 PARAM_DEFINE_FLOAT(ATT_W_MAG, 0.1f);
+
+/**
+ * Complimentary filter gyroscope bias weight
+ *
+ * @group Attitude Q estimator
+ * @min 0
+ * @max 1
+ */
 PARAM_DEFINE_FLOAT(ATT_W_GYRO_BIAS, 0.1f);
-PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f);		///< magnetic declination, in degrees
-PARAM_DEFINE_INT32(ATT_MAG_DECL_A, 1);		///< automatic GPS based magnetic declination
-PARAM_DEFINE_INT32(ATT_ACC_COMP, 2);		///< acceleration compensation
-PARAM_DEFINE_FLOAT(ATT_BIAS_MAX, 0.05f);		///< gyro bias limit, rad/s
+
+/**
+ * Magnetic declination, in degrees
+ *
+ * This parameter is not used in normal operation,
+ * as the declination is looked up based on the
+ * GPS coordinates of the vehicle.
+ *
+ * @group Attitude Q estimator
+ * @unit degrees
+ */
+PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f);
+
+/**
+ * Enable automatic GPS based declination compensation
+ *
+ * @group Attitude Q estimator
+ * @min 0
+ * @max 1
+ */
+PARAM_DEFINE_INT32(ATT_MAG_DECL_A, 1);
+
+/**
+ * Enable acceleration compensation based on GPS
+ * velocity.
+ *
+ * @group Attitude Q estimator
+ * @min 1
+ * @max 2
+ */
+PARAM_DEFINE_INT32(ATT_ACC_COMP, 2);
+
+/**
+ * Gyro bias limit
+ *
+ * @group Attitude Q estimator
+ * @min 0
+ * @max 2
+ * @unit rad/s
+ */
+PARAM_DEFINE_FLOAT(ATT_BIAS_MAX, 0.05f);

src/modules/commander/commander.cpp

@@ -2164,10 +2164,12 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
 
 		if (set_normal_color) {
 			/* set color */
-			if (status_local->battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW || status_local->failsafe) {
+			if (status_local->failsafe) {
+				rgbled_set_color(RGBLED_COLOR_PURPLE);
+			} else if (status_local->battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW) {
 				rgbled_set_color(RGBLED_COLOR_AMBER);
-				/* vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL handled as vehicle_status_s::ARMING_STATE_ARMED_ERROR / vehicle_status_s::ARMING_STATE_STANDBY_ERROR */
-
+			} else if (status_local->battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) {
+				rgbled_set_color(RGBLED_COLOR_RED);
 			} else {
 				if (status_local->condition_global_position_valid) {
 					rgbled_set_color(RGBLED_COLOR_GREEN);
@@ -2801,7 +2803,7 @@ void *commander_low_prio_loop(void *arg)
 								need_param_autosave_timeout = 0;
 							}
 
-							mavlink_log_info(mavlink_fd, "settings saved");
+							/* do not spam MAVLink, but provide the answer / green led mechanism */
 							answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
 
 						} else {

src/modules/commander/gyro_calibration.cpp

@@ -77,7 +77,7 @@ typedef struct  {
 	struct gyro_report	gyro_report_0;
 } gyro_worker_data_t;
 
-static calibrate_return gyro_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data)
+static calibrate_return gyro_calibration_worker(int cancel_sub, void* data)
 {
 	gyro_worker_data_t*	worker_data = (gyro_worker_data_t*)(data);
 	unsigned		calibration_counter[max_gyros] = { 0 };
@@ -92,6 +92,7 @@ static calibrate_return gyro_calibration_worker(detect_orientation_return orient
 	}
 	
 	memset(&worker_data->gyro_report_0, 0, sizeof(worker_data->gyro_report_0));
+	memset(&worker_data->gyro_scale, 0, sizeof(worker_data->gyro_scale));
 	
 	/* use first gyro to pace, but count correctly per-gyro for statistics */
 	while (calibration_counter[0] < calibration_count) {
@@ -152,7 +153,7 @@ static calibrate_return gyro_calibration_worker(detect_orientation_return orient
 int do_gyro_calibration(int mavlink_fd)
 {
 	int			res = OK;
-	gyro_worker_data_t	worker_data;
+	gyro_worker_data_t	worker_data = {};
 
 	mavlink_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
 
@@ -199,51 +200,63 @@ int do_gyro_calibration(int mavlink_fd)
 	for (unsigned s = 0; s < max_gyros; s++) {
 		worker_data.gyro_sensor_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s);
 	}
+
+	int cancel_sub  = calibrate_cancel_subscribe();
+
+	unsigned try_count = 0;
+	unsigned max_tries = 20;
+	res = ERROR;
 	
-	// Calibrate right-side up
-	
-	bool side_collected[detect_orientation_side_count] = { true, true, true, true, true, false };
-	
-	int cancel_sub  = calibrate_cancel_subscribe();	
-	calibrate_return cal_return = calibrate_from_orientation(mavlink_fd,                 // Mavlink fd to write output
-								cancel_sub,                 // Subscription to vehicle_command for cancel support
-								side_collected,		// Sides to calibrate
-								gyro_calibration_worker,     // Calibration worker
-								&worker_data,		// Opaque data for calibration worked
-								true);			// true: lenient still detection
+	do {
+		// Calibrate gyro and ensure user didn't move
+		calibrate_return cal_return = gyro_calibration_worker(cancel_sub, &worker_data);
+
+		if (cal_return == calibrate_return_cancelled) {
+			// Cancel message already sent, we are done here
+			res = ERROR;
+			break;
+
+		} else if (cal_return == calibrate_return_error) {
+			res = ERROR;
+
+		} else {
+			/* check offsets */
+			float xdiff = worker_data.gyro_report_0.x - worker_data.gyro_scale[0].x_offset;
+			float ydiff = worker_data.gyro_report_0.y - worker_data.gyro_scale[0].y_offset;
+			float zdiff = worker_data.gyro_report_0.z - worker_data.gyro_scale[0].z_offset;
+
+			/* maximum allowable calibration error in radians */
+			const float maxoff = 0.0055f;
+
+			if (!isfinite(worker_data.gyro_scale[0].x_offset) ||
+			    !isfinite(worker_data.gyro_scale[0].y_offset) ||
+			    !isfinite(worker_data.gyro_scale[0].z_offset) ||
+			    fabsf(xdiff) > maxoff ||
+			    fabsf(ydiff) > maxoff ||
+			    fabsf(zdiff) > maxoff) {
+
+				mavlink_and_console_log_critical(mavlink_fd, "[cal] motion, retrying..");
+				res = ERROR;
+
+			} else {
+				res = OK;
+			}
+		}
+		try_count++;
+
+	} while (res == ERROR && try_count <= max_tries);
+
+	if (try_count >= max_tries) {
+		mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: Motion during calibration");
+		res = ERROR;
+	}
+
 	calibrate_cancel_unsubscribe(cancel_sub);
 	
 	for (unsigned s = 0; s < max_gyros; s++) {
 		px4_close(worker_data.gyro_sensor_sub[s]);
 	}
 
-	if (cal_return == calibrate_return_cancelled) {
-		// Cancel message already sent, we are done here
-		return ERROR;
-	} else if (cal_return == calibrate_return_error) {
-		res = ERROR;
-	}
-		
-	if (res == OK) {
-		/* check offsets */
-		float xdiff = worker_data.gyro_report_0.x - worker_data.gyro_scale[0].x_offset;
-		float ydiff = worker_data.gyro_report_0.y - worker_data.gyro_scale[0].y_offset;
-		float zdiff = worker_data.gyro_report_0.z - worker_data.gyro_scale[0].z_offset;
-
-		/* maximum allowable calibration error in radians */
-		const float maxoff = 0.0055f;
-
-		if (!PX4_ISFINITE(worker_data.gyro_scale[0].x_offset) ||
-		    !PX4_ISFINITE(worker_data.gyro_scale[0].y_offset) ||
-		    !PX4_ISFINITE(worker_data.gyro_scale[0].z_offset) ||
-		    fabsf(xdiff) > maxoff ||
-		    fabsf(ydiff) > maxoff ||
-		    fabsf(zdiff) > maxoff) {
-			mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: Motion during calibration");
-			res = ERROR;
-		}
-	}
-
 	if (res == OK) {
 		/* set offset parameters to new values */
 		bool failed = false;

src/modules/commander/mag_calibration.cpp

@@ -53,6 +53,7 @@
 #include <fcntl.h>
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_accel.h>
+#include <drivers/drv_gyro.h>
 #include <uORB/topics/sensor_combined.h>
 #include <drivers/drv_mag.h>
 #include <mavlink/mavlink_log.h>
@@ -196,7 +197,71 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
 	
 	mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] Rotate vehicle around the detected orientation");
 	mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] Continue rotation for %u seconds", worker_data->calibration_interval_perside_seconds);
-	sleep(2);
+
+	/*
+	 * Detect if the system is rotating.
+	 *
+	 * We're detecting this as a general rotation on any axis, not necessary on the one we
+	 * asked the user for. This is because we really just need two roughly orthogonal axes
+	 * 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 last_gyro = 0;
+	float gyro_x_integral = 0.0f;
+	float gyro_y_integral = 0.0f;
+	float gyro_z_integral = 0.0f;
+
+	const float gyro_int_thresh_rad = 0.5f;
+
+	int sub_gyro = orb_subscribe(ORB_ID(sensor_gyro));
+
+	while (fabsf(gyro_x_integral) < gyro_int_thresh_rad &&
+		fabsf(gyro_y_integral) < gyro_int_thresh_rad &&
+		fabsf(gyro_z_integral) < gyro_int_thresh_rad) {
+
+		/* abort on request */
+		if (calibrate_cancel_check(worker_data->mavlink_fd, cancel_sub)) {
+			result = calibrate_return_cancelled;
+			close(sub_gyro);
+			return result;
+		}
+
+		/* abort with timeout */
+		if (hrt_absolute_time() > detection_deadline) {
+			result = calibrate_return_error;
+			warnx("int: %8.4f, %8.4f, %8.4f", (double)gyro_x_integral, (double)gyro_y_integral, (double)gyro_z_integral);
+			mavlink_and_console_log_critical(worker_data->mavlink_fd, "Failed: This calibration requires rotation.");
+			break;
+		}
+
+		/* Wait clocking for new data on all gyro */
+		struct pollfd fds[1];
+		fds[0].fd = sub_gyro;
+		fds[0].events = POLLIN;
+		size_t fd_count = 1;
+
+		int poll_ret = poll(fds, fd_count, 1000);
+
+		if (poll_ret > 0) {
+			struct gyro_report gyro;
+			orb_copy(ORB_ID(sensor_gyro), sub_gyro, &gyro);
+
+			/* ensure we have a valid first timestamp */
+			if (last_gyro > 0) {
+
+				/* integrate */
+				float delta_t = (gyro.timestamp - last_gyro) / 1e6f;
+				gyro_x_integral += gyro.x * delta_t;
+				gyro_y_integral += gyro.y * delta_t;
+				gyro_z_integral += gyro.z * delta_t;
+			}
+
+			last_gyro = gyro.timestamp;
+		}
+	}
+
+	close(sub_gyro);
 	
 	uint64_t calibration_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds;
 	unsigned poll_errcount = 0;

src/modules/mavlink/mavlink_ftp.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014, 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
@@ -301,23 +301,32 @@ MavlinkFTP::_reply(mavlink_file_transfer_protocol_t* ftp_req)
 MavlinkFTP::ErrorCode
 MavlinkFTP::_workList(PayloadHeader* payload)
 {
-    char dirPath[kMaxDataLength];
-    strncpy(dirPath, _data_as_cstring(payload), kMaxDataLength);
-    
+	char dirPath[kMaxDataLength];
+	strncpy(dirPath, _data_as_cstring(payload), kMaxDataLength);
+
+	ErrorCode errorCode = kErrNone;
+	unsigned offset = 0;
+
 	DIR *dp = opendir(dirPath);
 
 	if (dp == nullptr) {
-		warnx("FTP: can't open path '%s'", dirPath);
-		return kErrFailErrno;
+		_mavlink->send_statustext_critical("FTP: can't open path (file system corrupted?)");
+		_mavlink->send_statustext_critical(dirPath);
+		// this is not an FTP error, abort directory read and continue
+
+		payload->data[offset++] = kDirentSkip;
+		*((char *)&payload->data[offset]) = '\0';
+		offset++;
+		payload->size = offset;
+
+		return errorCode;
 	}
-    
+
 #ifdef MAVLINK_FTP_DEBUG
 	warnx("FTP: list %s offset %d", dirPath, payload->offset);
 #endif
 
-	ErrorCode errorCode = kErrNone;
 	struct dirent entry, *result = nullptr;
-	unsigned offset = 0;
 
 	// move to the requested offset
 	seekdir(dp, payload->offset);
@@ -325,9 +334,16 @@ MavlinkFTP::_workList(PayloadHeader* payload)
 	for (;;) {
 		// read the directory entry
 		if (readdir_r(dp, &entry, &result)) {
-			warnx("FTP: list %s readdir_r failure\n", dirPath);
-			errorCode = kErrFailErrno;
-			break;
+			_mavlink->send_statustext_critical("FTP: list readdir_r failure");
+			_mavlink->send_statustext_critical(dirPath);
+
+			payload->data[offset++] = kDirentSkip;
+			*((char *)&payload->data[offset]) = '\0';
+			offset++;
+			payload->size = offset;
+			closedir(dp);
+
+			return errorCode;
 		}
 
 		// no more entries?
@@ -365,7 +381,8 @@ MavlinkFTP::_workList(PayloadHeader* payload)
 #else
 		case DT_DIR:
 #endif
-			if (strcmp(entry.d_name, ".") == 0 || strcmp(entry.d_name, "..") == 0) {
+			// XXX @DonLakeFlyer: Remove the first condition for the test setup
+			if ((entry.d_name[0] == '.') || strcmp(entry.d_name, ".") == 0 || strcmp(entry.d_name, "..") == 0) {
 				// Don't bother sending these back
 				direntType = kDirentSkip;
 			} else {
@@ -499,6 +516,7 @@ MavlinkFTP::_workBurst(PayloadHeader* payload, uint8_t target_system_id)
 	// Setup for streaming sends
 	_session_info.stream_download = true;
 	_session_info.stream_offset = payload->offset;
+	_session_info.stream_chunk_transmitted = 0;
 	_session_info.stream_seq_number = payload->seq_number + 1;
 	_session_info.stream_target_system_id = target_system_id;
 
@@ -885,6 +903,7 @@ void MavlinkFTP::send(const hrt_abstime t)
 			} else {
 				payload->size = bytes_read;
 				_session_info.stream_offset += bytes_read;
+				_session_info.stream_chunk_transmitted += bytes_read;
 			}
 		}
 		
@@ -903,8 +922,12 @@ void MavlinkFTP::send(const hrt_abstime t)
 #ifndef MAVLINK_FTP_UNIT_TEST
 			if (max_bytes_to_send < (get_size()*2)) {
 				more_data = false;
-				payload->burst_complete = true;
-				_session_info.stream_download = false;
+				/* perform transfers in 35K chunks - this is determined empirical */
+				if (_session_info.stream_chunk_transmitted > 35000) {
+					payload->burst_complete = true;
+					_session_info.stream_download = false;
+					_session_info.stream_chunk_transmitted = 0;
+				}
 			} else {
 #endif
 				more_data = true;

src/modules/mavlink/mavlink_ftp.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014, 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
@@ -165,6 +165,7 @@ private:
 		uint32_t	stream_offset;
 		uint16_t	stream_seq_number;
 		uint8_t		stream_target_system_id;
+		unsigned	stream_chunk_transmitted;
 	};
 	struct SessionInfo _session_info;	///< Session info, fd=-1 for no active session
 	

src/modules/mavlink/mavlink_main.cpp

@@ -93,7 +93,7 @@
 static const int ERROR = -1;
 
 #define DEFAULT_DEVICE_NAME			"/dev/ttyS1"
-#define MAX_DATA_RATE				60000	///< max data rate in bytes/s
+#define MAX_DATA_RATE				1000000	///< max data rate in bytes/s
 #define MAIN_LOOP_DELAY 			10000	///< 100 Hz @ 1000 bytes/s data rate
 #define FLOW_CONTROL_DISABLE_THRESHOLD		40	///< picked so that some messages still would fit it.
 
@@ -262,7 +262,9 @@ Mavlink::~Mavlink()
 		} while (_task_running);
 	}
 
-	LL_DELETE(_mavlink_instances, this);
+	if (_mavlink_instances) {
+		LL_DELETE(_mavlink_instances, this);
+	}
 }
 
 void
@@ -1025,7 +1027,7 @@ void
 Mavlink::adjust_stream_rates(const float multiplier)
 {
 	/* do not allow to push us to zero */
-	if (multiplier < 0.01f) {
+	if (multiplier < 0.0005f) {
 		return;
 	}
 
@@ -1036,9 +1038,9 @@ Mavlink::adjust_stream_rates(const float multiplier)
 		unsigned interval = stream->get_interval();
 		interval /= multiplier;
 
-		/* allow max ~600 Hz */
+		/* allow max ~2000 Hz */
 		if (interval < 1600) {
-			interval = 1600;
+			interval = 500;
 		}
 
 		/* set new interval */
@@ -1421,7 +1423,7 @@ Mavlink::task_main(int argc, char *argv[])
 
 	/* MAVLINK_FTP stream */
 	_mavlink_ftp = (MavlinkFTP *) MavlinkFTP::new_instance(this);
-	_mavlink_ftp->set_interval(interval_from_rate(120.0f));
+	_mavlink_ftp->set_interval(interval_from_rate(80.0f));
 	LL_APPEND(_streams, _mavlink_ftp);
 	
 	/* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on
@@ -1471,7 +1473,7 @@ Mavlink::task_main(int argc, char *argv[])
 	}
 
 	/* set main loop delay depending on data rate to minimize CPU overhead */
-	_main_loop_delay = MAIN_LOOP_DELAY * 1000 / _datarate;
+	_main_loop_delay = (MAIN_LOOP_DELAY * 1000) / _datarate;
 
 	/* now the instance is fully initialized and we can bump the instance count */
 	LL_APPEND(_mavlink_instances, this);

src/modules/mavlink/mavlink_parameters.cpp

@@ -181,8 +181,8 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 void
 MavlinkParametersManager::send(const hrt_abstime t)
 {
-	/* send all parameters if requested */
-	if (_send_all_index >= 0) {
+	/* send all parameters if requested, but only after the system has booted */
+	if (_send_all_index >= 0 && t > 4 * 1000 * 1000) {
 
 		/* skip if no space is available */
 		if (_mavlink->get_free_tx_buf() < get_size()) {

src/modules/mavlink/mavlink_receiver.cpp

@@ -131,6 +131,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_hil_local_alt0(0.0f),
 	_hil_local_proj_ref{},
 	_offboard_control_mode{},
+	_att_sp{},
 	_rates_sp{},
 	_time_offset_avg_alpha(0.6),
 	_time_offset(0)
@@ -780,16 +781,16 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
 		 * throttle and has the ignore bits set for attitude and rates don't change the flags for attitude and
 		 * body rates to keep the controllers running
 		 */
-		bool ignore_bodyrate = (bool)(set_attitude_target.type_mask & 0x7);
-		bool ignore_attitude = (bool)(set_attitude_target.type_mask & (1 << 7));
+		bool ignore_bodyrate_msg = (bool)(set_attitude_target.type_mask & 0x7);
+		bool ignore_attitude_msg = (bool)(set_attitude_target.type_mask & (1 << 7));
 
-		if (ignore_bodyrate && ignore_attitude && !_offboard_control_mode.ignore_thrust) {
+		if (ignore_bodyrate_msg && ignore_attitude_msg && !_offboard_control_mode.ignore_thrust) {
 			/* Message want's us to ignore everything except thrust: only ignore if previously ignored */
-			_offboard_control_mode.ignore_bodyrate = ignore_bodyrate && _offboard_control_mode.ignore_bodyrate;
-			_offboard_control_mode.ignore_attitude = ignore_attitude && _offboard_control_mode.ignore_attitude;
+			_offboard_control_mode.ignore_bodyrate = ignore_bodyrate_msg && _offboard_control_mode.ignore_bodyrate;
+			_offboard_control_mode.ignore_attitude = ignore_attitude_msg && _offboard_control_mode.ignore_attitude;
 		} else {
-			_offboard_control_mode.ignore_bodyrate = ignore_bodyrate;
-			_offboard_control_mode.ignore_attitude = ignore_attitude;
+			_offboard_control_mode.ignore_bodyrate = ignore_bodyrate_msg;
+			_offboard_control_mode.ignore_attitude = ignore_attitude_msg;
 		}
 
 		_offboard_control_mode.ignore_position = true;
@@ -818,22 +819,25 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
 
 				/* Publish attitude setpoint if attitude and thrust ignore bits are not set */
 				if (!(_offboard_control_mode.ignore_attitude)) {
-					struct vehicle_attitude_setpoint_s att_sp = {};
-					att_sp.timestamp = hrt_absolute_time();
-					mavlink_quaternion_to_euler(set_attitude_target.q,
-							&att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body);
-					mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])att_sp.R_body);
-					att_sp.R_valid = true;
-					if (!_offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid
-						att_sp.thrust = set_attitude_target.thrust;
+					_att_sp.timestamp = hrt_absolute_time();
+					if (!ignore_attitude_msg) { // only copy att sp if message contained new data
+						mavlink_quaternion_to_euler(set_attitude_target.q,
+								&_att_sp.roll_body, &_att_sp.pitch_body, &_att_sp.yaw_body);
+						mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])_att_sp.R_body);
+						_att_sp.R_valid = true;
+						_att_sp.yaw_sp_move_rate = 0.0;
+						memcpy(_att_sp.q_d, set_attitude_target.q, sizeof(_att_sp.q_d));
+						_att_sp.q_d_valid = true;
 					}
-					att_sp.yaw_sp_move_rate = 0.0;
-					memcpy(att_sp.q_d, set_attitude_target.q, sizeof(att_sp.q_d));
-					att_sp.q_d_valid = true;
+
+					if (!_offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid
+						_att_sp.thrust = set_attitude_target.thrust;
+					}
+
 					if (_att_sp_pub < 0) {
-						_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
+						_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
 					} else {
-						orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &att_sp);
+						orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
 					}
 				}
 
@@ -841,9 +845,11 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
 				///XXX add support for ignoring individual axes
 				if (!(_offboard_control_mode.ignore_bodyrate)) {
 					_rates_sp.timestamp = hrt_absolute_time();
-					_rates_sp.roll = set_attitude_target.body_roll_rate;
-					_rates_sp.pitch = set_attitude_target.body_pitch_rate;
-					_rates_sp.yaw = set_attitude_target.body_yaw_rate;
+					if (!ignore_bodyrate_msg) { // only copy att rates sp if message contained new data
+						_rates_sp.roll = set_attitude_target.body_roll_rate;
+						_rates_sp.pitch = set_attitude_target.body_pitch_rate;
+						_rates_sp.yaw = set_attitude_target.body_yaw_rate;
+					}
 					if (!_offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid
 						_rates_sp.thrust = set_attitude_target.thrust;
 					}
@@ -1356,8 +1362,8 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		memset(&hil_global_pos, 0, sizeof(hil_global_pos));
 
 		hil_global_pos.timestamp = timestamp;
-		hil_global_pos.lat = hil_state.lat;
-		hil_global_pos.lon = hil_state.lon;
+		hil_global_pos.lat = hil_state.lat / ((double)1e7);
+		hil_global_pos.lon = hil_state.lon / ((double)1e7);
 		hil_global_pos.alt = hil_state.alt / 1000.0f;
 		hil_global_pos.vel_n = hil_state.vx / 100.0f;
 		hil_global_pos.vel_e = hil_state.vy / 100.0f;
@@ -1581,7 +1587,7 @@ MavlinkReceiver::receive_start(Mavlink *parent)
 	param.sched_priority = SCHED_PRIORITY_MAX - 80;
 	(void)pthread_attr_setschedparam(&receiveloop_attr, &param);
 
-	pthread_attr_setstacksize(&receiveloop_attr, 1800);
+	pthread_attr_setstacksize(&receiveloop_attr, 2100);
 	pthread_t thread;
 	pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void *)parent);
 

src/modules/mavlink/mavlink_receiver.h

@@ -187,6 +187,7 @@ private:
 	float _hil_local_alt0;
 	struct map_projection_reference_s _hil_local_proj_ref;
 	struct offboard_control_mode_s _offboard_control_mode;
+	struct vehicle_attitude_setpoint_s _att_sp;
 	struct vehicle_rates_setpoint_s _rates_sp;
 	double _time_offset_avg_alpha;
 	uint64_t _time_offset;

src/modules/mc_att_control_multiplatform/mc_att_control.cpp

@@ -75,23 +75,23 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 
 	/* parameters */
 	_params_handles({
-			.roll_p		= px4::ParameterFloat("MC_ROLL_P", PARAM_MC_ROLL_P_DEFAULT),
-			.roll_rate_p	= px4::ParameterFloat("MC_ROLLRATE_P", PARAM_MC_ROLLRATE_P_DEFAULT),
-			.roll_rate_i	= px4::ParameterFloat("MC_ROLLRATE_I", PARAM_MC_ROLLRATE_I_DEFAULT),
-			.roll_rate_d	= px4::ParameterFloat("MC_ROLLRATE_D", PARAM_MC_ROLLRATE_D_DEFAULT),
-			.pitch_p	= px4::ParameterFloat("MC_PITCH_P", PARAM_MC_PITCH_P_DEFAULT),
-			.pitch_rate_p	= px4::ParameterFloat("MC_PITCHRATE_P", PARAM_MC_PITCHRATE_P_DEFAULT),
-			.pitch_rate_i	= px4::ParameterFloat("MC_PITCHRATE_I", PARAM_MC_PITCHRATE_I_DEFAULT),
-			.pitch_rate_d	= px4::ParameterFloat("MC_PITCHRATE_D", PARAM_MC_PITCHRATE_D_DEFAULT),
-			.yaw_p		= px4::ParameterFloat("MC_YAW_P", PARAM_MC_YAW_P_DEFAULT),
-			.yaw_rate_p	= px4::ParameterFloat("MC_YAWRATE_P", PARAM_MC_YAWRATE_P_DEFAULT),
-			.yaw_rate_i	= px4::ParameterFloat("MC_YAWRATE_I", PARAM_MC_YAWRATE_I_DEFAULT),
-			.yaw_rate_d	= px4::ParameterFloat("MC_YAWRATE_D", PARAM_MC_YAWRATE_D_DEFAULT),
-			.yaw_ff		= px4::ParameterFloat("MC_YAW_FF", PARAM_MC_YAW_FF_DEFAULT),
-			.yaw_rate_max	= px4::ParameterFloat("MC_YAWRATE_MAX", PARAM_MC_YAWRATE_MAX_DEFAULT),
-			.acro_roll_max	= px4::ParameterFloat("MC_ACRO_R_MAX", PARAM_MC_ACRO_R_MAX_DEFAULT),
-			.acro_pitch_max	= px4::ParameterFloat("MC_ACRO_P_MAX", PARAM_MC_ACRO_P_MAX_DEFAULT),
-			.acro_yaw_max	= px4::ParameterFloat("MC_ACRO_Y_MAX", PARAM_MC_ACRO_Y_MAX_DEFAULT)
+			.roll_p		= px4::ParameterFloat("MP_ROLL_P", PARAM_MP_ROLL_P_DEFAULT),
+			.roll_rate_p	= px4::ParameterFloat("MP_ROLLRATE_P", PARAM_MP_ROLLRATE_P_DEFAULT),
+			.roll_rate_i	= px4::ParameterFloat("MP_ROLLRATE_I", PARAM_MP_ROLLRATE_I_DEFAULT),
+			.roll_rate_d	= px4::ParameterFloat("MP_ROLLRATE_D", PARAM_MP_ROLLRATE_D_DEFAULT),
+			.pitch_p	= px4::ParameterFloat("MP_PITCH_P", PARAM_MP_PITCH_P_DEFAULT),
+			.pitch_rate_p	= px4::ParameterFloat("MP_PITCHRATE_P", PARAM_MP_PITCHRATE_P_DEFAULT),
+			.pitch_rate_i	= px4::ParameterFloat("MP_PITCHRATE_I", PARAM_MP_PITCHRATE_I_DEFAULT),
+			.pitch_rate_d	= px4::ParameterFloat("MP_PITCHRATE_D", PARAM_MP_PITCHRATE_D_DEFAULT),
+			.yaw_p		= px4::ParameterFloat("MP_YAW_P", PARAM_MP_YAW_P_DEFAULT),
+			.yaw_rate_p	= px4::ParameterFloat("MP_YAWRATE_P", PARAM_MP_YAWRATE_P_DEFAULT),
+			.yaw_rate_i	= px4::ParameterFloat("MP_YAWRATE_I", PARAM_MP_YAWRATE_I_DEFAULT),
+			.yaw_rate_d	= px4::ParameterFloat("MP_YAWRATE_D", PARAM_MP_YAWRATE_D_DEFAULT),
+			.yaw_ff		= px4::ParameterFloat("MP_YAW_FF", PARAM_MP_YAW_FF_DEFAULT),
+			.yaw_rate_max	= px4::ParameterFloat("MP_YAWRATE_MAX", PARAM_MP_YAWRATE_MAX_DEFAULT),
+			.acro_roll_max	= px4::ParameterFloat("MP_ACRO_R_MAX", PARAM_MP_ACRO_R_MAX_DEFAULT),
+			.acro_pitch_max	= px4::ParameterFloat("MP_ACRO_P_MAX", PARAM_MP_ACRO_P_MAX_DEFAULT),
+			.acro_yaw_max	= px4::ParameterFloat("MP_ACRO_Y_MAX", PARAM_MP_ACRO_Y_MAX_DEFAULT)
 	}),
 
 	/* performance counters */

src/modules/mc_att_control_multiplatform/mc_att_control_params.c

@@ -52,7 +52,7 @@
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_ROLL_P);
+PX4_PARAM_DEFINE_FLOAT(MP_ROLL_P);
 
 /**
  * Roll rate P gain
@@ -62,7 +62,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_ROLL_P);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_P);
+PX4_PARAM_DEFINE_FLOAT(MP_ROLLRATE_P);
 
 /**
  * Roll rate I gain
@@ -72,7 +72,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_P);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_I);
+PX4_PARAM_DEFINE_FLOAT(MP_ROLLRATE_I);
 
 /**
  * Roll rate D gain
@@ -82,7 +82,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_I);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_D);
+PX4_PARAM_DEFINE_FLOAT(MP_ROLLRATE_D);
 
 /**
  * Pitch P gain
@@ -93,7 +93,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_D);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_PITCH_P);
+PX4_PARAM_DEFINE_FLOAT(MP_PITCH_P);
 
 /**
  * Pitch rate P gain
@@ -103,7 +103,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_PITCH_P);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_P);
+PX4_PARAM_DEFINE_FLOAT(MP_PITCHRATE_P);
 
 /**
  * Pitch rate I gain
@@ -113,7 +113,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_P);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_I);
+PX4_PARAM_DEFINE_FLOAT(MP_PITCHRATE_I);
 
 /**
  * Pitch rate D gain
@@ -123,7 +123,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_I);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_D);
+PX4_PARAM_DEFINE_FLOAT(MP_PITCHRATE_D);
 
 /**
  * Yaw P gain
@@ -134,7 +134,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_D);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_YAW_P);
+PX4_PARAM_DEFINE_FLOAT(MP_YAW_P);
 
 /**
  * Yaw rate P gain
@@ -144,7 +144,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_YAW_P);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_P);
+PX4_PARAM_DEFINE_FLOAT(MP_YAWRATE_P);
 
 /**
  * Yaw rate I gain
@@ -154,7 +154,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_P);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_I);
+PX4_PARAM_DEFINE_FLOAT(MP_YAWRATE_I);
 
 /**
  * Yaw rate D gain
@@ -164,7 +164,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_I);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_D);
+PX4_PARAM_DEFINE_FLOAT(MP_YAWRATE_D);
 
 /**
  * Yaw feed forward
@@ -175,7 +175,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_D);
  * @max 1.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_YAW_FF);
+PX4_PARAM_DEFINE_FLOAT(MP_YAW_FF);
 
 /**
  * Max yaw rate
@@ -187,7 +187,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_YAW_FF);
  * @max 360.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX);
+PX4_PARAM_DEFINE_FLOAT(MP_YAWRATE_MAX);
 
 /**
  * Max acro roll rate
@@ -197,7 +197,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX);
  * @max 360.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX);
+PX4_PARAM_DEFINE_FLOAT(MP_ACRO_R_MAX);
 
 /**
  * Max acro pitch rate
@@ -207,7 +207,7 @@ PX4_PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX);
  * @max 360.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX);
+PX4_PARAM_DEFINE_FLOAT(MP_ACRO_P_MAX);
 
 /**
  * Max acro yaw rate
@@ -216,4 +216,4 @@ PX4_PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX);
+PX4_PARAM_DEFINE_FLOAT(MP_ACRO_Y_MAX);

src/modules/mc_att_control_multiplatform/mc_att_control_params.h

@@ -33,7 +33,7 @@
  ****************************************************************************/
 
 /**
- * @file mc_att_control_params.h
+ * @file MP_att_control_params.h
  * Parameters for multicopter attitude controller.
  *
  * @author Tobias Naegeli <naegelit@student.ethz.ch>
@@ -43,20 +43,20 @@
  */
 #pragma once
 
-#define PARAM_MC_ROLL_P_DEFAULT 6.0f
-#define PARAM_MC_ROLLRATE_P_DEFAULT 0.1f
-#define PARAM_MC_ROLLRATE_I_DEFAULT 0.0f
-#define PARAM_MC_ROLLRATE_D_DEFAULT 0.002f
-#define PARAM_MC_PITCH_P_DEFAULT 6.0f
-#define PARAM_MC_PITCHRATE_P_DEFAULT 0.1f
-#define PARAM_MC_PITCHRATE_I_DEFAULT 0.0f
-#define PARAM_MC_PITCHRATE_D_DEFAULT 0.002f
-#define PARAM_MC_YAW_P_DEFAULT 2.0f
-#define PARAM_MC_YAWRATE_P_DEFAULT 0.3f
-#define PARAM_MC_YAWRATE_I_DEFAULT 0.0f
-#define PARAM_MC_YAWRATE_D_DEFAULT 0.0f
-#define PARAM_MC_YAW_FF_DEFAULT 0.5f
-#define PARAM_MC_YAWRATE_MAX_DEFAULT 120.0f
-#define PARAM_MC_ACRO_R_MAX_DEFAULT 35.0f
-#define PARAM_MC_ACRO_P_MAX_DEFAULT 35.0f
-#define PARAM_MC_ACRO_Y_MAX_DEFAULT 120.0f
+#define PARAM_MP_ROLL_P_DEFAULT 6.0f
+#define PARAM_MP_ROLLRATE_P_DEFAULT 0.1f
+#define PARAM_MP_ROLLRATE_I_DEFAULT 0.0f
+#define PARAM_MP_ROLLRATE_D_DEFAULT 0.002f
+#define PARAM_MP_PITCH_P_DEFAULT 6.0f
+#define PARAM_MP_PITCHRATE_P_DEFAULT 0.1f
+#define PARAM_MP_PITCHRATE_I_DEFAULT 0.0f
+#define PARAM_MP_PITCHRATE_D_DEFAULT 0.002f
+#define PARAM_MP_YAW_P_DEFAULT 2.0f
+#define PARAM_MP_YAWRATE_P_DEFAULT 0.3f
+#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_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

@@ -1105,7 +1105,6 @@ MulticopterPositionControl::task_main()
 
 					/* derivative of velocity error, not includes setpoint acceleration */
 					math::Vector<3> vel_err_d = (_sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt;
-					_vel_prev = _vel;
 
 					/* thrust vector in NED frame */
 					math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + vel_err_d.emult(_params.vel_d) + thrust_int;
@@ -1288,6 +1287,11 @@ MulticopterPositionControl::task_main()
 						memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
 						_att_sp.R_valid = true;
 
+						/* copy quaternion setpoint to attitude setpoint topic */
+						math::Quaternion q_sp;
+						q_sp.from_dcm(R);
+						memcpy(&_att_sp.q_d[0], &q_sp.data[0], sizeof(_att_sp.q_d));
+
 						/* calculate euler angles, for logging only, must not be used for control */
 						math::Vector<3> euler = R.to_euler();
 						_att_sp.roll_body = euler(0);
@@ -1303,6 +1307,11 @@ MulticopterPositionControl::task_main()
 						memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
 						_att_sp.R_valid = true;
 
+						/* copy quaternion setpoint to attitude setpoint topic */
+						math::Quaternion q_sp;
+						q_sp.from_dcm(R);
+						memcpy(&_att_sp.q_d[0], &q_sp.data[0], sizeof(_att_sp.q_d));
+
 						_att_sp.roll_body = 0.0f;
 						_att_sp.pitch_body = 0.0f;
 					}
@@ -1388,12 +1397,20 @@ MulticopterPositionControl::task_main()
 			math::Matrix<3,3> R_sp;
 			R_sp.from_euler(_att_sp.roll_body,_att_sp.pitch_body,_att_sp.yaw_body);
 			memcpy(&_att_sp.R_body[0], R_sp.data, sizeof(_att_sp.R_body));
+
+			/* copy quaternion setpoint to attitude setpoint topic */
+			math::Quaternion q_sp;
+			q_sp.from_dcm(R_sp);
+			memcpy(&_att_sp.q_d[0], &q_sp.data[0], sizeof(_att_sp.q_d));
 			_att_sp.timestamp = hrt_absolute_time();
 		}
 		else {
 			reset_yaw_sp = true;
 		}
 
+		/* update previous velocity for velocity controller D part */
+		_vel_prev = _vel;
+
 		/* publish attitude setpoint
 		 * Do not publish if offboard is enabled but position/velocity control is disabled,
 		 * in this case the attitude setpoint is published by the mavlink app

src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp

@@ -69,27 +69,27 @@ MulticopterPositionControl::MulticopterPositionControl() :
 
 	/* parameters */
 	_params_handles({
-		.thr_min	    = px4::ParameterFloat("MPC_THR_MIN", PARAM_MPC_THR_MIN_DEFAULT),
-		.thr_max	    = px4::ParameterFloat("MPC_THR_MAX", PARAM_MPC_THR_MAX_DEFAULT),
-		.z_p		    = px4::ParameterFloat("MPC_Z_P", PARAM_MPC_Z_P_DEFAULT),
-		.z_vel_p	    = px4::ParameterFloat("MPC_Z_VEL_P", PARAM_MPC_Z_VEL_P_DEFAULT),
-		.z_vel_i	    = px4::ParameterFloat("MPC_Z_VEL_I", PARAM_MPC_Z_VEL_I_DEFAULT),
-		.z_vel_d	    = px4::ParameterFloat("MPC_Z_VEL_D", PARAM_MPC_Z_VEL_D_DEFAULT),
-		.z_vel_max	    = px4::ParameterFloat("MPC_Z_VEL_MAX", PARAM_MPC_Z_VEL_MAX_DEFAULT),
-		.z_ff		    = px4::ParameterFloat("MPC_Z_FF", PARAM_MPC_Z_FF_DEFAULT),
-		.xy_p		    = px4::ParameterFloat("MPC_XY_P", PARAM_MPC_XY_P_DEFAULT),
-		.xy_vel_p	    = px4::ParameterFloat("MPC_XY_VEL_P", PARAM_MPC_XY_VEL_P_DEFAULT),
-		.xy_vel_i	    = px4::ParameterFloat("MPC_XY_VEL_I", PARAM_MPC_XY_VEL_I_DEFAULT),
-		.xy_vel_d	    = px4::ParameterFloat("MPC_XY_VEL_D", PARAM_MPC_XY_VEL_D_DEFAULT),
-		.xy_vel_max	    = px4::ParameterFloat("MPC_XY_VEL_MAX", PARAM_MPC_XY_VEL_MAX_DEFAULT),
-		.xy_ff		    = px4::ParameterFloat("MPC_XY_FF", PARAM_MPC_XY_FF_DEFAULT),
-		.tilt_max_air	    = px4::ParameterFloat("MPC_TILTMAX_AIR", PARAM_MPC_TILTMAX_AIR_DEFAULT),
-		.land_speed	    = px4::ParameterFloat("MPC_LAND_SPEED", PARAM_MPC_LAND_SPEED_DEFAULT),
-		.tilt_max_land	    = px4::ParameterFloat("MPC_TILTMAX_LND", PARAM_MPC_TILTMAX_LND_DEFAULT),
-		.man_roll_max	    = px4::ParameterFloat("MPC_MAN_R_MAX", PARAM_MPC_MAN_R_MAX_DEFAULT),
-		.man_pitch_max	    = px4::ParameterFloat("MPC_MAN_P_MAX", PARAM_MPC_MAN_P_MAX_DEFAULT),
-		.man_yaw_max	    = px4::ParameterFloat("MPC_MAN_Y_MAX", PARAM_MPC_MAN_Y_MAX_DEFAULT),
-		.mc_att_yaw_p	    = px4::ParameterFloat("MC_YAW_P", PARAM_MC_YAW_P_DEFAULT)
+		.thr_min	    = px4::ParameterFloat("MPP_THR_MIN", PARAM_MPP_THR_MIN_DEFAULT),
+		.thr_max	    = px4::ParameterFloat("MPP_THR_MAX", PARAM_MPP_THR_MAX_DEFAULT),
+		.z_p		    = px4::ParameterFloat("MPP_Z_P", PARAM_MPP_Z_P_DEFAULT),
+		.z_vel_p	    = px4::ParameterFloat("MPP_Z_VEL_P", PARAM_MPP_Z_VEL_P_DEFAULT),
+		.z_vel_i	    = px4::ParameterFloat("MPP_Z_VEL_I", PARAM_MPP_Z_VEL_I_DEFAULT),
+		.z_vel_d	    = px4::ParameterFloat("MPP_Z_VEL_D", PARAM_MPP_Z_VEL_D_DEFAULT),
+		.z_vel_max	    = px4::ParameterFloat("MPP_Z_VEL_MAX", PARAM_MPP_Z_VEL_MAX_DEFAULT),
+		.z_ff		    = px4::ParameterFloat("MPP_Z_FF", PARAM_MPP_Z_FF_DEFAULT),
+		.xy_p		    = px4::ParameterFloat("MPP_XY_P", PARAM_MPP_XY_P_DEFAULT),
+		.xy_vel_p	    = px4::ParameterFloat("MPP_XY_VEL_P", PARAM_MPP_XY_VEL_P_DEFAULT),
+		.xy_vel_i	    = px4::ParameterFloat("MPP_XY_VEL_I", PARAM_MPP_XY_VEL_I_DEFAULT),
+		.xy_vel_d	    = px4::ParameterFloat("MPP_XY_VEL_D", PARAM_MPP_XY_VEL_D_DEFAULT),
+		.xy_vel_max	    = px4::ParameterFloat("MPP_XY_VEL_MAX", PARAM_MPP_XY_VEL_MAX_DEFAULT),
+		.xy_ff		    = px4::ParameterFloat("MPP_XY_FF", PARAM_MPP_XY_FF_DEFAULT),
+		.tilt_max_air	    = px4::ParameterFloat("MPP_TILTMAX_AIR", PARAM_MPP_TILTMAX_AIR_DEFAULT),
+		.land_speed	    = px4::ParameterFloat("MPP_LAND_SPEED", PARAM_MPP_LAND_SPEED_DEFAULT),
+		.tilt_max_land	    = px4::ParameterFloat("MPP_TILTMAX_LND", PARAM_MPP_TILTMAX_LND_DEFAULT),
+		.man_roll_max	    = px4::ParameterFloat("MPP_MAN_R_MAX", PARAM_MPP_MAN_R_MAX_DEFAULT),
+		.man_pitch_max	    = px4::ParameterFloat("MPP_MAN_P_MAX", PARAM_MPP_MAN_P_MAX_DEFAULT),
+		.man_yaw_max	    = px4::ParameterFloat("MPP_MAN_Y_MAX", PARAM_MPP_MAN_Y_MAX_DEFAULT),
+		.mc_att_yaw_p	    = px4::ParameterFloat("MP_YAW_P", PARAM_MP_YAW_P_DEFAULT)
 	}),
 	_ref_alt(0.0f),
 	_ref_timestamp(0),

src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c

@@ -52,7 +52,7 @@
  * @max 1.0
  * @group Multicopter Position Control
  */
-PX4_PARAM_DEFINE_FLOAT(MPC_THR_MIN);
+PX4_PARAM_DEFINE_FLOAT(MPP_THR_MIN);
 
 /**
  * Maximum thrust
@@ -63,7 +63,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_THR_MIN);
  * @max 1.0
  * @group Multicopter Position Control
  */
-PX4_PARAM_DEFINE_FLOAT(MPC_THR_MAX);
+PX4_PARAM_DEFINE_FLOAT(MPP_THR_MAX);
 
 /**
  * Proportional gain for vertical position error
@@ -71,7 +71,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_THR_MAX);
  * @min 0.0
  * @group Multicopter Position Control
  */
-PX4_PARAM_DEFINE_FLOAT(MPC_Z_P);
+PX4_PARAM_DEFINE_FLOAT(MPP_Z_P);
 
 /**
  * Proportional gain for vertical velocity error
@@ -79,7 +79,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_Z_P);
  * @min 0.0
  * @group Multicopter Position Control
  */
-PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_P);
+PX4_PARAM_DEFINE_FLOAT(MPP_Z_VEL_P);
 
 /**
  * Integral gain for vertical velocity error
@@ -89,7 +89,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_P);
  * @min 0.0
  * @group Multicopter Position Control
  */
-PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_I);
+PX4_PARAM_DEFINE_FLOAT(MPP_Z_VEL_I);
 
 /**
  * Differential gain for vertical velocity error
@@ -97,7 +97,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_I);
  * @min 0.0
  * @group Multicopter Position Control
  */
-PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_D);
+PX4_PARAM_DEFINE_FLOAT(MPP_Z_VEL_D);
 
 /**
  * Maximum vertical velocity
@@ -108,7 +108,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_D);
  * @min 0.0
  * @group Multicopter Position Control
  */
-PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX);
+PX4_PARAM_DEFINE_FLOAT(MPP_Z_VEL_MAX);
 
 /**
  * Vertical velocity feed forward
@@ -119,7 +119,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX);
  * @max 1.0
  * @group Multicopter Position Control
  */
-PX4_PARAM_DEFINE_FLOAT(MPC_Z_FF);
+PX4_PARAM_DEFINE_FLOAT(MPP_Z_FF);
 
 /**
  * Proportional gain for horizontal position error
@@ -127,7 +127,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_Z_FF);
  * @min 0.0
  * @group Multicopter Position Control
  */
-PX4_PARAM_DEFINE_FLOAT(MPC_XY_P);
+PX4_PARAM_DEFINE_FLOAT(MPP_XY_P);
 
 /**
  * Proportional gain for horizontal velocity error
@@ -135,7 +135,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_XY_P);
  * @min 0.0
  * @group Multicopter Position Control
  */
-PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_P);
+PX4_PARAM_DEFINE_FLOAT(MPP_XY_VEL_P);
 
 /**
  * Integral gain for horizontal velocity error
@@ -145,7 +145,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_P);
  * @min 0.0
  * @group Multicopter Position Control
  */
-PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_I);
+PX4_PARAM_DEFINE_FLOAT(MPP_XY_VEL_I);
 
 /**
  * Differential gain for horizontal velocity error. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
@@ -153,7 +153,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_I);
  * @min 0.0
  * @group Multicopter Position Control
  */
-PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_D);
+PX4_PARAM_DEFINE_FLOAT(MPP_XY_VEL_D);
 
 /**
  * Maximum horizontal velocity
@@ -164,7 +164,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_D);
  * @min 0.0
  * @group Multicopter Position Control
  */
-PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX);
+PX4_PARAM_DEFINE_FLOAT(MPP_XY_VEL_MAX);
 
 /**
  * Horizontal velocity feed forward
@@ -175,7 +175,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX);
  * @max 1.0
  * @group Multicopter Position Control
  */
-PX4_PARAM_DEFINE_FLOAT(MPC_XY_FF);
+PX4_PARAM_DEFINE_FLOAT(MPP_XY_FF);
 
 /**
  * Maximum tilt angle in air
@@ -187,7 +187,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_XY_FF);
  * @max 90.0
  * @group Multicopter Position Control
  */
-PX4_PARAM_DEFINE_FLOAT(MPC_TILTMAX_AIR);
+PX4_PARAM_DEFINE_FLOAT(MPP_TILTMAX_AIR);
 
 /**
  * Maximum tilt during landing
@@ -199,7 +199,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_TILTMAX_AIR);
  * @max 90.0
  * @group Multicopter Position Control
  */
-PX4_PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND);
+PX4_PARAM_DEFINE_FLOAT(MPP_TILTMAX_LND);
 
 /**
  * Landing descend rate
@@ -208,7 +208,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND);
  * @min 0.0
  * @group Multicopter Position Control
  */
-PX4_PARAM_DEFINE_FLOAT(MPC_LAND_SPEED);
+PX4_PARAM_DEFINE_FLOAT(MPP_LAND_SPEED);
 
 /**
  * Max manual roll
@@ -218,7 +218,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_LAND_SPEED);
  * @max 90.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MPC_MAN_R_MAX);
+PX4_PARAM_DEFINE_FLOAT(MPP_MAN_R_MAX);
 
 /**
  * Max manual pitch
@@ -228,7 +228,7 @@ PX4_PARAM_DEFINE_FLOAT(MPC_MAN_R_MAX);
  * @max 90.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MPC_MAN_P_MAX);
+PX4_PARAM_DEFINE_FLOAT(MPP_MAN_P_MAX);
 
 /**
  * Max manual yaw rate
@@ -237,5 +237,5 @@ PX4_PARAM_DEFINE_FLOAT(MPC_MAN_P_MAX);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PX4_PARAM_DEFINE_FLOAT(MPC_MAN_Y_MAX);
+PX4_PARAM_DEFINE_FLOAT(MPP_MAN_Y_MAX);
 

src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h

@@ -41,24 +41,24 @@
 
 #pragma once
 
-#define PARAM_MPC_THR_MIN_DEFAULT 0.1f
-#define PARAM_MPC_THR_MAX_DEFAULT 1.0f
-#define PARAM_MPC_Z_P_DEFAULT 1.0f
-#define PARAM_MPC_Z_VEL_P_DEFAULT 0.1f
-#define PARAM_MPC_Z_VEL_I_DEFAULT 0.02f
-#define PARAM_MPC_Z_VEL_D_DEFAULT 0.0f
-#define PARAM_MPC_Z_VEL_MAX_DEFAULT 5.0f
-#define PARAM_MPC_Z_FF_DEFAULT 0.5f
-#define PARAM_MPC_XY_P_DEFAULT 1.0f
-#define PARAM_MPC_XY_VEL_P_DEFAULT 0.1f
-#define PARAM_MPC_XY_VEL_I_DEFAULT 0.02f
-#define PARAM_MPC_XY_VEL_D_DEFAULT 0.01f
-#define PARAM_MPC_XY_VEL_MAX_DEFAULT 5.0f
-#define PARAM_MPC_XY_FF_DEFAULT 0.5f
-#define PARAM_MPC_TILTMAX_AIR_DEFAULT 45.0f
-#define PARAM_MPC_TILTMAX_LND_DEFAULT 15.0f
-#define PARAM_MPC_LAND_SPEED_DEFAULT 1.0f
-#define PARAM_MPC_MAN_R_MAX_DEFAULT 35.0f
-#define PARAM_MPC_MAN_P_MAX_DEFAULT 35.0f
-#define PARAM_MPC_MAN_Y_MAX_DEFAULT 120.0f
+#define PARAM_MPP_THR_MIN_DEFAULT 0.1f
+#define PARAM_MPP_THR_MAX_DEFAULT 1.0f
+#define PARAM_MPP_Z_P_DEFAULT 1.0f
+#define PARAM_MPP_Z_VEL_P_DEFAULT 0.1f
+#define PARAM_MPP_Z_VEL_I_DEFAULT 0.02f
+#define PARAM_MPP_Z_VEL_D_DEFAULT 0.0f
+#define PARAM_MPP_Z_VEL_MAX_DEFAULT 5.0f
+#define PARAM_MPP_Z_FF_DEFAULT 0.5f
+#define PARAM_MPP_XY_P_DEFAULT 1.0f
+#define PARAM_MPP_XY_VEL_P_DEFAULT 0.1f
+#define PARAM_MPP_XY_VEL_I_DEFAULT 0.02f
+#define PARAM_MPP_XY_VEL_D_DEFAULT 0.01f
+#define PARAM_MPP_XY_VEL_MAX_DEFAULT 5.0f
+#define PARAM_MPP_XY_FF_DEFAULT 0.5f
+#define PARAM_MPP_TILTMAX_AIR_DEFAULT 45.0f
+#define PARAM_MPP_TILTMAX_LND_DEFAULT 15.0f
+#define PARAM_MPP_LAND_SPEED_DEFAULT 1.0f
+#define PARAM_MPP_MAN_R_MAX_DEFAULT 35.0f
+#define PARAM_MPP_MAN_P_MAX_DEFAULT 35.0f
+#define PARAM_MPP_MAN_Y_MAX_DEFAULT 120.0f
 

src/modules/navigator/rtl.cpp

@@ -103,6 +103,7 @@ RTL::on_activation()
 			_mission_item.altitude_is_relative = false;
 			_mission_item.altitude = _navigator->get_global_position()->alt;
 		}
+
 	}
 
 	set_rtl_item();
@@ -146,7 +147,8 @@ RTL::set_rtl_item()
 		_mission_item.autocontinue = true;
 		_mission_item.origin = ORIGIN_ONBOARD;
 
-		mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: climb to %d meters above home",
+		mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: climb to %d m (%d m above home)",
+			(int)(climb_alt),
 			(int)(climb_alt - _navigator->get_home_position()->alt));
 		break;
 	}
@@ -177,7 +179,8 @@ RTL::set_rtl_item()
 		_mission_item.autocontinue = true;
 		_mission_item.origin = ORIGIN_ONBOARD;
 
-		mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: return at %d meters above home",
+		mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: return at %d m (%d m above home)",
+			(int)(_mission_item.altitude),
 			(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
 		break;
 	}
@@ -197,7 +200,8 @@ RTL::set_rtl_item()
 		_mission_item.autocontinue = false;
 		_mission_item.origin = ORIGIN_ONBOARD;
 
-		mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: descend to %d meters above home",
+		mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: descend to %d m (%d m above home)",
+			(int)(_mission_item.altitude),
 			(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
 		break;
 	}

src/modules/position_estimator_inav/position_estimator_inav_params.c

@@ -83,7 +83,7 @@ PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_V, 0.0f);
  * @max 10.0
  * @group Position Estimator INAV
  */
-PARAM_DEFINE_FLOAT(INAV_W_Z_VIS_P, 0.5f);
+PARAM_DEFINE_FLOAT(INAV_W_Z_VIS_P, 5.0f);
 
 /**
  * Z axis weight for sonar

src/modules/sdlog2/sdlog2.c

@@ -1982,10 +1982,10 @@ void handle_command(struct vehicle_command_s *cmd)
 		if (param == 1)	{
 			sdlog2_start_log();
 
-		} else if (param == 0)	{
+		} else if (param == -1)	{
 			sdlog2_stop_log();
 		} else {
-			warnx("unknown storage cmd");
+			// Silently ignore non-matching command values, as they could be for params.
 		}
 
 		break;

src/modules/sensors/sensors.cpp

@@ -637,6 +637,7 @@ Sensors::Sensors() :
 	(void)param_find("CAL_MAG1_ROT");
 	(void)param_find("CAL_MAG2_ROT");
 	(void)param_find("SYS_PARAM_VER");
+	(void)param_find("SYS_AUTOSTART");
 	
 	/* fetch initial parameter values */
 	parameters_update();

src/modules/systemlib/mixer/mixer_multirotor.cpp

@@ -200,18 +200,31 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
 unsigned
 MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
 {
+	/* Summary of mixing strategy:
+	1) mix roll, pitch and thrust without yaw.
+	2) if some outputs violate range [0,1] then try to shift all outputs to minimize violation ->
+		increase or decrease total thrust (boost). The total increase or decrease of thrust is limited
+		(max_thrust_diff). If after the shift some outputs still violate the bounds then scale roll & pitch.
+		In case there is violation at the lower and upper bound then try to shift such that violation is equal
+		on both sides.
+	3) mix in yaw and scale if it leads to limit violation.
+	4) scale all outputs to range [idle_speed,1]
+	*/
+
 	float		roll    = constrain(get_control(0, 0) * _roll_scale, -1.0f, 1.0f);
-	//lowsyslog("roll: %d, get_control0: %d, %d\n", (int)(roll), (int)(get_control(0, 0)), (int)(_roll_scale));
 	float		pitch   = constrain(get_control(0, 1) * _pitch_scale, -1.0f, 1.0f);
 	float		yaw     = constrain(get_control(0, 2) * _yaw_scale, -1.0f, 1.0f);
 	float		thrust  = constrain(get_control(0, 3), 0.0f, 1.0f);
-	//lowsyslog("thrust: %d, get_control3: %d\n", (int)(thrust), (int)(get_control(0, 3)));
 	float		min_out = 0.0f;
 	float		max_out = 0.0f;
 
+	// clean register for saturation status flags
 	if (status_reg != NULL) {
 		(*status_reg) = 0;
 	}
+	// thrust boost parameters
+	float thrust_increase_factor = 1.5f;
+	float thrust_decrease_factor = 0.6f;
 
 	/* perform initial mix pass yielding unbounded outputs, ignore yaw */
 	for (unsigned i = 0; i < _rotor_count; i++) {
@@ -221,14 +234,6 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
 
 		out *= _rotors[i].out_scale;
 
-		/* limit yaw if it causes outputs clipping */
-		if (out >= 0.0f && out < -yaw * _rotors[i].yaw_scale) {
-			yaw = -out / _rotors[i].yaw_scale;
-			if(status_reg != NULL) {
-				(*status_reg) |= PX4IO_P_STATUS_MIXER_YAW_LIMIT;
-			}
-		}
-
 		/* calculate min and max output values */
 		if (out < min_out) {
 			min_out = out;
@@ -240,51 +245,89 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
 		outputs[i] = out;
 	}
 
-	/* scale down roll/pitch controls if some outputs are negative, don't add yaw, keep total thrust */
-	if (min_out < 0.0f) {
-		float scale_in = thrust / (thrust - min_out);
+	float boost = 0.0f;				// value added to demanded thrust (can also be negative)
+	float roll_pitch_scale = 1.0f;	// scale for demanded roll and pitch
 
-		max_out = 0.0f;
-
-		/* mix again with adjusted controls */
-		for (unsigned i = 0; i < _rotor_count; i++) {
-			float out = scale_in * (roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) + thrust;
-
-			/* update max output value */
-			if (out > max_out) {
-				max_out = out;
-			}
-
-			outputs[i] = out;
+	if(min_out < 0.0f && max_out < 1.0f && -min_out <= 1.0f - max_out) {
+		float max_thrust_diff = thrust * thrust_increase_factor - thrust;
+		if(max_thrust_diff >= -min_out) {
+			boost = -min_out;
 		}
+		else {
+			boost = max_thrust_diff;
+			roll_pitch_scale = (thrust + boost)/(thrust - min_out);
+		}
+	}
+	else if (max_out > 1.0f && min_out > 0.0f && min_out >= max_out - 1.0f) {
+		float max_thrust_diff = thrust - thrust_decrease_factor*thrust;
+		if(max_thrust_diff >= max_out - 1.0f) {
+			boost = -(max_out - 1.0f);
+		} else {
+			boost = -max_thrust_diff;
+			roll_pitch_scale = (1 - (thrust + boost))/(max_out - thrust);
+		}
+	}
+	else if (min_out < 0.0f && max_out < 1.0f && -min_out > 1.0f - max_out) {
+		float max_thrust_diff = thrust * thrust_increase_factor - thrust;
+		boost = constrain(-min_out - (1.0f - max_out)/2.0f,0.0f, max_thrust_diff);
+		roll_pitch_scale = (thrust + boost)/(thrust - min_out);
+	}
+	else if (max_out > 1.0f && min_out > 0.0f && min_out < max_out - 1.0f ) {
+		float max_thrust_diff = thrust - thrust_decrease_factor*thrust;
+		boost = constrain(-(max_out - 1.0f - min_out)/2.0f, -max_thrust_diff, 0.0f);
+		roll_pitch_scale = (1 - (thrust + boost))/(max_out - thrust);
+	}
+	else if (min_out < 0.0f && max_out > 1.0f) {
+		boost = constrain(-(max_out - 1.0f + min_out)/2.0f, thrust_decrease_factor*thrust - thrust, thrust_increase_factor*thrust - thrust);
+		roll_pitch_scale = (thrust + boost)/(thrust - min_out);
+	}
 
+	// notify if saturation has occurred
+	if(min_out < 0.0f) {
 		if(status_reg != NULL) {
 			(*status_reg) |= PX4IO_P_STATUS_MIXER_LOWER_LIMIT;
 		}
-
-	} else {
-		/* roll/pitch mixed without lower side limiting, add yaw control */
-		for (unsigned i = 0; i < _rotor_count; i++) {
-			outputs[i] += yaw * _rotors[i].yaw_scale;
-		}
 	}
-
-	/* scale down all outputs if some outputs are too large, reduce total thrust */
-	float scale_out;
-	if (max_out > 1.0f) {
-		scale_out = 1.0f / max_out;
-
+	if(max_out > 0.0f) {
 		if(status_reg != NULL) {
 			(*status_reg) |= PX4IO_P_STATUS_MIXER_UPPER_LIMIT;
 		}
-
-	} else {
-		scale_out = 1.0f;
 	}
 
-	/* scale outputs to range _idle_speed..1, and do final limiting */
+	// mix again but now with thrust boost, scale roll/pitch and also add yaw
+	for(unsigned i = 0; i < _rotor_count; i++) {
+		float out = (roll * _rotors[i].roll_scale +
+			    pitch * _rotors[i].pitch_scale) * roll_pitch_scale +
+				yaw * _rotors[i].yaw_scale +
+			    thrust + boost;
+
+		out *= _rotors[i].out_scale;
+
+		// scale yaw if it violates limits. inform about yaw limit reached
+		if(out < 0.0f) {
+			yaw = -((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) *
+				roll_pitch_scale + thrust + boost)/_rotors[i].yaw_scale;
+			if(status_reg != NULL) {
+				(*status_reg) |= PX4IO_P_STATUS_MIXER_YAW_LIMIT;
+			}
+		}
+		else if(out > 1.0f) {
+			yaw = (1.0f - ((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) *
+				roll_pitch_scale + thrust + boost))/_rotors[i].yaw_scale;
+			if(status_reg != NULL) {
+				(*status_reg) |= PX4IO_P_STATUS_MIXER_YAW_LIMIT;
+			}
+		}
+	}
+
+	/* last mix, add yaw and scale outputs to range idle_speed...1 */
 	for (unsigned i = 0; i < _rotor_count; i++) {
-		outputs[i] = constrain(_idle_speed + (outputs[i] * (1.0f - _idle_speed) * scale_out), _idle_speed, 1.0f);
+		outputs[i] = (roll * _rotors[i].roll_scale +
+			    pitch * _rotors[i].pitch_scale) * roll_pitch_scale +
+				yaw * _rotors[i].yaw_scale +
+			    thrust + boost;
+
+		outputs[i] = constrain(_idle_speed + (outputs[i] * (1.0f - _idle_speed)), _idle_speed, 1.0f);
 	}
 
 	return _rotor_count;