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

3 files changed, 14 insertions, 3 deletions

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