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

4 files changed, 48 insertions, 162 deletions

diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
index d18106541..a582ece17 100644
--- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp
+++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
@@ -235,7 +235,7 @@ public:
 	}
 
 private:
-	static const unsigned	_tune_max = 1024; // be reasonable about user tunes
+	static const unsigned	_tune_max = 1024 * 8; // be reasonable about user tunes
 	const char		* _default_tunes[TONE_NUMBER_OF_TUNES];
 	const char		* _tune_names[TONE_NUMBER_OF_TUNES];
 	static const uint8_t	_note_tab[];
@@ -330,154 +330,6 @@ ToneAlarm::ToneAlarm() :
 	_default_tunes[TONE_NOTIFY_POSITIVE_TUNE] = "MFT200e8a8a";					// Notify Positive tone
 	_default_tunes[TONE_NOTIFY_NEUTRAL_TUNE] = "MFT200e8e";						// Notify Neutral tone
 	_default_tunes[TONE_NOTIFY_NEGATIVE_TUNE] = "MFT200e8c8e8c8e8c8";				// Notify Negative tone
-#if 0 // don't include unused tunes... but keep them for nostalgic reason
-	_default_tunes[TONE_CHARGE_TUNE] =
-		"MFT90O3C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C4"; // charge!
-	_default_tunes[TONE_DIXIE_TUNE] = "MFT60O3C32O2A32F16F16F32G32A32A+32O3C16C16C16O2A16";		// dixie
-	_default_tunes[TONE_CUCURACHA_TUNE] = "MFT90O2C16C16C16F8.A8C16C16C16F8.A4P16P8";		// cucuracha 
-	_default_tunes[TONE_YANKEE_TUNE] = "MNT150L8O2GGABGBADGGABL4GL8F+";				// yankee
-	_default_tunes[TONE_DAISY_TUNE] =
-		"MFT200O3C4.O2A4.G4.F4.D8E8F8D4F8C2.O2G4.O3C4.O2A4.F4.D8E8F8G4A8G2P8";			// daisy
-	_default_tunes[TONE_WILLIAM_TELL_TUNE] =
-		"T200O2B4P8B16B16B4P8B16B16B8G+8E8G+8B8G+8B8O3E8"					// william tell
-		"O2B8G+8E8G+8B8G+8B8O3E8O2B4P8B16B16B4P8B16"
-		"O2B16B4P8B16B16B4P8B16B16B8B16B16B8B8B8B16"
-		"O2B16B8B8B8B16B16B8B8B8B16B16B8B8B2B2B8P8"
-		"P4P4P8O1B16B16B8B16B16B8B16B16O2E8F+8G+8"
-		"O1B16B16B8B16B16O2E8G+16G+16F+8D+8O1B8B16"
-		"O1B16B8B16B16B8B16B16O2E8F+8G+8E16G+16B4"
-		"O2B16A16G+16F+16E8G+8E8O3B16B16B8B16B16B8"
-		"O3B16B16O4E8F+8G+8O3B16B16B8B16B16O4E8G+16"
-		"O4G+16F+8D+8O3B8B16B16B8B16B16B8B16B16O4E8"
-		"O4F+8G+8E16G+16B4B16A16G+16F+16E8G+8E8O3G+16"
-		"O3G+16G+8G+16G+16G+8G+16G+16G+8O4C+8O3G+8"
-		"O4C+8O3G+8O4C+8O3G+8F+8E8D+8C+8G+16G+16G+8"
-		"O3G+16G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8O3G+8"
-		"O4C+8O3B8A+8B8A+8B8G+16G+16G+8G+16G+16G+8"
-		"O3G+16G+16G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3G+8"
-		"O3F+8E8D+8C+8G+16G+16G+8G+16G+16G+8G+16G+16"
-		"O3G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3B8A+8B8O2B16"
-		"O2B16B8F+16F+16F+8F+16F+16F+8G+8A8F+4A8G+8"
-		"O2E4G+8F+8F+8F+8O3F+16F+16F+8F+16F+16F+8"
-		"O3G+8A8F+4A8G+8E4G+8F+8O2B16B16B8O1B16B16"
-		"O1B8B16B16B8B16B16O2E8F+8G+8O1B16B16B8B16"
-		"O1B16O2E8G+16G+16F+8D+8O1B8B16B16B8B16B16"
-		"O1B8B16B16O2E8F+8G+8E16G+16B4B16A16G+16F+16"
-		"O2E8G+8E8O3B16B16B8B16B16B8B16B16O4E8F+8"
-		"O4G+8O3B16B16B8B16B16O4E8G+16G+16F+8D+8O3B8"
-		"O3B16B16B8B16B16B8B16B16O4E8F+8G+8E16G+16"
-		"O4B4B16A16G+16F+16E8G+8E8O3E64F64G64A64B64"
-		"O4C64D64E8E16E16E8E8G+4.F+8E8D+8E8C+8O3B16"
-		"O4C+16O3B16O4C+16O3B16O4C+16D+16E16O3A16"
-		"O3B16A16B16A16B16O4C+16D+16O3G+16A16G+16"
-		"O3A16G+16A16B16O4C+16O3F+16G+16F+16G+16F+16"
-		"O3G+16F+16G+16F+16G+16F+16D+16O2B16O3B16"
-		"O4C+16D+16E8D+8E8C+8O3B16O4C+16O3B16O4C+16"
-		"O3B16O4C+16D+16E16O3A16B16A16B16A16B16O4C+16"
-		"O4D+16O3G+16A16G+16A16G+16A16B16O4C+16O3F+16"
-		"O3G+16F+16G+16F+16A16F+16E16E8P8C+4C+16O2C16"
-		"O3C+16O2C16O3D+16C+16O2B16A16A16G+16E16C+16"
-		"O2C+16C+16C+16C+16E16D+16O1C16G+16G+16G+16"
-		"O1G+16G+16G+16O2C+16E16G+16O3C+16C+16C+16"
-		"O3C+16C+16O2C16O3C+16O2C16O3D+16C+16O2B16"
-		"O2A16A16G+16E16C+16C+16C+16C+16C+16E16D+16"
-		"O1C16G+16G+16G+16G+16G+16G+16O2C+16E16G+16"
-		"O3C+16E16D+16C+16D+16O2C16G+16G+16G+16O3G+16"
-		"O3E16C+16D+16O2C16G+16G+16G+16O3G+16E16C+16"
-		"O3D+16O2B16G+16G+16A+16G16D+16D+16G+16G16"
-		"O2G+16G16G+16A16G+16F+16E16O1B16A+16B16O2E16"
-		"O1B16O2F+16O1B16O2G+16E16D+16E16G+16E16A16"
-		"O2F+16B16O3G+16F+16E16D+16F+16E16C+16O2B16"
-		"O3C+16O2B16O3C+16D+16E16F+16G+16O2A16B16"
-		"O2A16B16O3C+16D+16E16F+16O2G+16A16G+16A16"
-		"O2C16O3C+16D+16E16O2F+16G+16F+16G+16F+16"
-		"O2G+16F+16G+16F+16G+16F+16D+16O1B16C16O2C+16"
-		"O2D+16E16O1B16A+16B16O2E16O1B16O2F+16O1B16"
-		"O2G+16E16D+16E16G+16E16A16F+16B16O3G+16F+16"
-		"O3E16D+16F+16E16C+16O2B16O3C+16O2B16O3C+16"
-		"O3D+16E16F+16G+16O2A16B16A16B16O3C+16D+16"
-		"O3E16F+16O2G+16A16G+16A16B16O3C+16D+16E16"
-		"O2F+16O3C+16O2C16O3C+16D+16C+16O2A16F+16"
-		"O2E16O3E16F+16G+16A16B16O4C+16D+16E8E16E16"
-		"O4E8E8G+4.F8E8D+8E8C+8O3B16O4C+16O3B16O4C+16"
-		"O3B16O4C+16D+16E16O3A16B16A16B16A16B16O4C+16"
-		"O4D+16O3G+16A16G+16A16G+16A16B16O4C+16O3F+16"
-		"O3G+16F+16G+16F+16G+16F+16G+16F+16G+16F+16"
-		"O3D+16O2B16O3B16O4C+16D+16E8E16E16E8E8G+4."
-		"O4F+8E8D+8E8C+8O3B16O4C+16O3B16O4C+16O3B16"
-		"O4C+16D+16E16O3A16B16A16B16A16B16O4C+16D+16"
-		"O3G+16A16G+16A16G+16A16B16O4C+16O3F+16G+16"
-		"O3F+16G+16F+16A16G+16F+16E8O2B8O3E8G+16G+16"
-		"O3G+8G+16G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8"
-		"O3G+8O4C+8O3G+8F+8E8D+8C+8G+16G+16G+8G+16"
-		"O3G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8O3G+8"
-		"O4C+8O3B8A+8B8A+8B8G+16G+16G+8G+16G+16G+8"
-		"O3G+16G+16G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3G+8"
-		"O3F+8E8D+8C+8G+16G+16G+8G+16G+16G+8G+16G+16"
-		"O3G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3B8A+8B8A+8"
-		"O3B8O2F+16F+16F+8F+16F+16F+8G+8A8F+4A8G+8"
-		"O2E4G+8F+8B8O1B8O2F+16F+16F+8F+16F+16F+8"
-		"O2G+8A8F+4A8G+8E4G+8F+8B16B16B8O1B16B16B8"
-		"O1B16B16B8B16B16O2E8F+8G+8O1B16B16B8B16B16"
-		"O2E8G+16G+16F+8D+8O1B8B16B16B8B16B16B8B16"
-		"O1B16O2E8F+8G+8E16G+16B4B16A16G+16F+16E8"
-		"O1B8O2E8O3B16B16B8B16B16B8B16B16O4E8F+8G+8"
-		"O3B16B16B8B16B16O4E8G+16G+16F+8D+8O3B8B16"
-		"O3B16B8B16B16B8B16B16O4E8F+8G+8O3E16G+16"
-		"O3B4B16A16G+16F+16E16F+16G+16A16G+16A16B16"
-		"O4C+16O3B16O4C+16D+16E16D+16E16F+16G+16A16"
-		"O3B16O4A16O3B16O4A16O3B16O4A16O3B16O4A16"
-		"O3B16O4A16O3B16O4A16O3B16O4A16O3B16E16F+16"
-		"O3G+16A16G+16A16B16O4C+16O3B16O4C+16D+16"
-		"O4E16D+16E16F+16G+16A16O3B16O4A16O3B16O4A16"
-		"O3B16O4A16O3B16O4A16O3B16O4A16O3B16O4A16"
-		"O3B16O4A16O3B16P16G+16O4G+16O3G+16P16D+16"
-		"O4D+16O3D+16P16E16O4E16O3E16P16A16O4A16O3A16"
-		"P16O3G+16O4G+16O3G+16P16D+16O4D+16O3D+16"
-		"P16O3E16O4E16O3E16P16A16O4A16O3A16O4G16O3G16"
-		"O4G16O3G16O4G16O3G16O4G16O3G16O4G8E8C8E8"
-		"O4G+16O3G+16O4G+16O3G+16O4G+16O3G+16O4G+16"
-		"O3G+16O4G+8E8O3B8O4E8G+16O3G+16O4G+16O3G+16"
-		"O4G+16O3G+16O4G+16O3G+16O4G+8F8C+8F8A+16"
-		"O3A+16O4A+16O3A+16O4A+16O3A+16O4A+16O3A+16"
-		"O4A+8G8E8G8B8P16A+16P16A16P16G+16P16F+16"
-		"P16O4E16P16D+16P16C+16P16O3B16P16A+16P16"
-		"O3A16P16G+16P16F+16P16E16P16D+16P16F+16E16"
-		"O3F+16G+16A16G+16A16B16O4C+16O3B16O4C+16"
-		"O4D+16E16D+16E16F+16G+16A16O3B16O4A16O3B16"
-		"O4A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16"
-		"O4A16O3B16O4A16O3B16E16F+16G+16A16G+16A16"
-		"O3B16O4C+16O3B16O4C+16D+16E16D+16E16F+16"
-		"O4G+16A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16"
-		"O4A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16"
-		"P16O3G+16O4G+16O3G+16P16D+16O4D+16O3D+16"
-		"P16O3E16O4E16O3E16P16A16O4A16O3A16P16G+16"
-		"O4G+16O3G+16P16D+16O4D+16O3D+16P16E16O4E16"
-		"O3E16P16A16O4A16O3A16O4G16O3G16O4G16O3G16"
-		"O4G16O3G16O4G16O3G16O4G8E8C8E8G+16O3G+16"
-		"O4G+16O3G+16O4G+16O3G+16O4G+16O3G+16O4G+8"
-		"O4E8O3B8O4E8G+16O3G+16O4G+16O3G+16O4G+16"
-		"O3G+16O4G+16O3G+16O4G+8F8C+8F8A+16O3A+16"
-		"O4A+16O3A+16O4A+16O3A+16O4A+16O3A+16O4A+8"
-		"O4G8E8G8B8P16A+16P16A16P16G+16P16F+16P16"
-		"O4E16P16D+16P16C+16P16O3B16P16A+16P16A16"
-		"P16O3G+16P16F+16P16E16P16D+16P16F16E16D+16"
-		"O3E16D+16E8B16B16B8B16B16B8B16B16O4E8F+8"
-		"O4G+8O3B16B16B8B16B16B8B16B16O4G+8A8B8P8"
-		"O4E8F+8G+8P8O3G+8A8B8P8P2O2B16C16O3C+16D16"
-		"O3D+16E16F16F+16G16G+16A16A+16B16C16O4C+16"
-		"O4D+16E16D+16F+16D+16E16D+16F+16D+16E16D+16"
-		"O4F+16D+16E16D+16F+16D+16E16D+16F+16D+16"
-		"O4E16D+16F+16D+16E16D+16F+16D+16E16D+16F+16"
-		"O4D+16E8E16O3E16O4E16O3E16O4E16O3E16O4E8"
-		"O3B16O2B16O3B16O2B16O3B16O2B16O3B8G+16O2G+16"
-		"O3G+16O2G+16O3G+16O2G+16O3G8E16O2E16O3E16"
-		"O2E16O3E16O2E16O3E8E16E16E8E8E8O2B16B16B8"
-		"O2B8B8G+16G+16G+8G+8G+8E16E16E8E8E8O1B8O2E8"
-		"O1B8O2G+8E8B8G+8O3E8O2B8O3E8O2B8O3G+8E8B8"
-		"O3G+8O4E4P8E16E16E8E8E8E8E4P8E16E4P8O2E16"
-		"O2E2P64";
-#endif
 	_default_tunes[TONE_ARMING_WARNING_TUNE] = "MNT75L1O2G";					//arming warning
 	_default_tunes[TONE_BATTERY_WARNING_SLOW_TUNE] = "MBNT100a8";					//battery warning slow
 	_default_tunes[TONE_BATTERY_WARNING_FAST_TUNE] = "MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8";	//battery warning fast
@@ -983,7 +835,7 @@ play_tune(unsigned tune)
 }
 
 int
-play_string(const char *str)
+play_string(const char *str, bool free_buffer)
 {
 	int	fd, ret;
 
@@ -995,6 +847,9 @@ play_string(const char *str)
 	ret = write(fd, str, strlen(str) + 1);
 	close(fd);
 
+	if (free_buffer)
+		free((void *)str);
+
 	if (ret < 0)
 		err(1, "play tune");
 	exit(0);
@@ -1021,29 +876,49 @@ tone_alarm_main(int argc, char *argv[])
 	}
 
 	if (argc > 1) {
-		if (!strcmp(argv[1], "start"))
+		const char *argv1 = argv[1];
+
+		if (!strcmp(argv1, "start"))
 			play_tune(TONE_STARTUP_TUNE);
 
-		if (!strcmp(argv[1], "stop"))
+		if (!strcmp(argv1, "stop"))
 			play_tune(TONE_STOP_TUNE);
 
-		if ((tune = strtol(argv[1], nullptr, 10)) != 0)
+		if ((tune = strtol(argv1, nullptr, 10)) != 0)
 			play_tune(tune);
 
 		/* It might be a tune name */
 		for (tune = 1; tune < TONE_NUMBER_OF_TUNES; tune++)
-			if (!strcmp(g_dev->name(tune), argv[1]))
+			if (!strcmp(g_dev->name(tune), argv1))
 				play_tune(tune);
 
+		/* If it is a file name then load and play it as a string */
+		if (*argv1 == '/') {
+			FILE *fd = fopen(argv1, "r");
+			int sz;
+			char *buffer;
+			if (fd == nullptr)
+				errx(1, "couldn't open '%s'", argv1);
+			fseek(fd, 0, SEEK_END);
+			sz = ftell(fd);
+			fseek(fd, 0, SEEK_SET);
+			buffer = (char *)malloc(sz + 1);
+			if (buffer == nullptr)
+				errx(1, "not enough memory memory");
+			fread(buffer, sz, 1, fd);
+			/* terminate the string */
+			buffer[sz] = 0;
+			play_string(buffer, true);
+		}
+
 		/* if it looks like a PLAY string... */
-		if (strlen(argv[1]) > 2) {
-			const char *str = argv[1];
-			if (str[0] == 'M') {
-				play_string(str);
+		if (strlen(argv1) > 2) {
+			if (*argv1 == 'M') {
+				play_string(argv1, false);
 			}
 		}
 
 	}
 
-	errx(1, "unrecognised command, try 'start', 'stop' or an alarm number or name");
+	errx(1, "unrecognized command, try 'start', 'stop', an alarm number or name, or a file name starting with a '/'");
 }
diff --git a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp
index 236052b56..86bda3c75 100755
--- a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp
+++ b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp
@@ -730,7 +730,7 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					// Because proper mount of PX4 will give you a reversed accelerometer readings.
 					NonlinearSO3AHRSupdate(gyro[0],gyro[1],gyro[2],-acc[0],-acc[1],-acc[2],mag[0],mag[1],mag[2],so3_comp_params.Kp,so3_comp_params.Ki, dt);
 
-					// Convert q->R.
+					// Convert q->R, This R converts inertial frame to body frame.
 					Rot_matrix[0] = q0q0 + q1q1 - q2q2 - q3q3;// 11
         				Rot_matrix[1] = 2.0 * (q1*q2 + q0*q3);	// 12
         				Rot_matrix[2] = 2.0 * (q1*q3 - q0*q2);	// 13
@@ -794,7 +794,7 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					memcpy(&att.rate_offsets, &(gyro_bias), sizeof(att.rate_offsets));
 
 					/* copy rotation matrix */
-					memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix));
+					memcpy(&att.R, Rot_matrix, sizeof(float)*9);
 					att.R_valid = true;
 
 					if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) {
diff --git a/src/modules/position_estimator_mc/position_estimator_mc_main.c b/src/modules/position_estimator_mc/position_estimator_mc_main.c
index 984bd1329..363961819 100755
--- a/src/modules/position_estimator_mc/position_estimator_mc_main.c
+++ b/src/modules/position_estimator_mc/position_estimator_mc_main.c
@@ -497,7 +497,12 @@ int position_estimator_mc_thread_main(int argc, char *argv[])
 				local_pos_est.vz = x_z_aposteriori_k[1];
 				local_pos_est.timestamp = hrt_absolute_time();
 				if ((isfinite(x_x_aposteriori_k[0])) && (isfinite(x_x_aposteriori_k[1])) && (isfinite(x_y_aposteriori_k[0])) && (isfinite(x_y_aposteriori_k[1])) && (isfinite(x_z_aposteriori_k[0])) && (isfinite(x_z_aposteriori_k[1]))){
-					orb_publish(ORB_ID(vehicle_local_position), local_pos_est_pub, &local_pos_est);
+					if(local_pos_est_pub > 0)
+						orb_publish(ORB_ID(vehicle_local_position), local_pos_est_pub, &local_pos_est);
+					else
+						local_pos_est_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos_est);
+					//char buf[0xff]; sprintf(buf,"[pos_est_mc] x:%f, y:%f, z:%f",x_x_aposteriori_k[0],x_y_aposteriori_k[0],x_z_aposteriori_k[0]);
+					//mavlink_log_info(mavlink_fd, buf);
 				}
 			}
 		} /* end of poll return value check */
diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h
index a7cda34a8..686fd765c 100644
--- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h
+++ b/src/modules/uORB/topics/vehicle_attitude_setpoint.h
@@ -64,6 +64,12 @@ struct vehicle_attitude_setpoint_s
 	float R_body[9];				/**< Rotation matrix describing the setpoint as rotation from the current body frame */
 	bool R_valid;					/**< Set to true if rotation matrix is valid */
 
+	//! For quaternion-based attitude control
+	float q_d[4];				/** Desired quaternion for quaternion control */
+	bool q_d_valid;					/**< Set to true if quaternion vector is valid */
+	float q_e[4];				/** Attitude error in quaternion */
+	bool q_e_valid;					/**< Set to true if quaternion error vector is valid */
+
 	float thrust;					/**< Thrust in Newton the power system should generate */
 
 };