attitude_estimator_ekf: acc comp bug fixed, estimated gravity vector logging

src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp

@@ -410,7 +410,7 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 							vel(2) = gps.vel_d_m_s;
 						}
 
-					} else if (ekf_params.acc_comp == 2 && global_pos_updated && global_pos.valid && hrt_absolute_time() < global_pos.timestamp + 500000) {
+					} else if (ekf_params.acc_comp == 2 && global_pos.valid && hrt_absolute_time() < global_pos.timestamp + 500000) {
 						vel_valid = true;
 						if (global_pos_updated) {
 							vel_t = global_pos.timestamp;
@@ -532,6 +532,10 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					att.pitchacc = x_aposteriori[4];
 					att.yawacc = x_aposteriori[5];
 
+					att.g_comp[0] = raw.accelerometer_m_s2[0] - acc(0);
+					att.g_comp[1] = raw.accelerometer_m_s2[1] - acc(1);
+					att.g_comp[2] = raw.accelerometer_m_s2[2] - acc(2);
+
 					/* copy offsets */
 					memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets));
 

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -798,6 +798,7 @@ MulticopterPositionControl::task_main()
 					}
 				}
 
+				if (_control_mode.flag_control_velocity_enabled) {
 					if (tilt > tilt_max && thr_min >= 0.0f) {
 						/* crop horizontal component */
 						float k = tanf(tilt_max) / tanf(tilt);
@@ -805,6 +806,22 @@ MulticopterPositionControl::task_main()
 						thrust_sp(1) *= k;
 						saturation_xy = true;
 					}
+				} else {
+					/* thrust compensation for altitude only control mode */
+					float att_comp;
+
+					if (_att.R[2][2] > TILT_COS_MAX) {
+						att_comp = 1.0f / _att.R[2][2];
+					} else if (_att.R[2][2] > 0.0f) {
+						att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _att.R[2][2] + 1.0f;
+						saturation_z = true;
+					} else {
+						att_comp = 1.0f;
+						saturation_z = true;
+					}
+
+					thrust_sp(2) *= att_comp;
+				}
 
 				/* limit max thrust */
 				float thrust_abs = thrust_sp.length();
@@ -854,7 +871,7 @@ MulticopterPositionControl::task_main()
 						thrust_int(2) = 0.0f;
 				}
 
-				/* calculate attitude and thrust from thrust vector */
+				/* calculate attitude setpoint from thrust vector */
 				if (_control_mode.flag_control_velocity_enabled) {
 					/* desired body_z axis = -normalize(thrust_vector) */
 					math::Vector<3> body_x;
@@ -910,19 +927,6 @@ MulticopterPositionControl::task_main()
 					_att_sp.roll_body = euler(0);
 					_att_sp.pitch_body = euler(1);
 					/* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */
-
-				} else {
-					/* thrust compensation for altitude only control mode */
-					float att_comp;
-
-					if (_att.R[2][2] > TILT_COS_MAX)
-						att_comp = 1.0f / _att.R[2][2];
-					else if (_att.R[2][2] > 0.0f)
-						att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _att.R[2][2] + 1.0f;
-					else
-						att_comp = 1.0f;
-
-					thrust_abs *= att_comp;
 				}
 
 				_att_sp.thrust = thrust_abs;

src/modules/sdlog2/sdlog2.c

@@ -1161,6 +1161,9 @@ int sdlog2_thread_main(int argc, char *argv[])
 				log_msg.body.log_ATT.roll_rate = buf.att.rollspeed;
 				log_msg.body.log_ATT.pitch_rate = buf.att.pitchspeed;
 				log_msg.body.log_ATT.yaw_rate = buf.att.yawspeed;
+				log_msg.body.log_ATT.gx = buf.att.g_comp[0];
+				log_msg.body.log_ATT.gy = buf.att.g_comp[1];
+				log_msg.body.log_ATT.gz = buf.att.g_comp[2];
 				LOGBUFFER_WRITE_AND_COUNT(ATT);
 			}
 

src/modules/sdlog2/sdlog2_messages.h

@@ -57,6 +57,9 @@ struct log_ATT_s {
 	float roll_rate;
 	float pitch_rate;
 	float yaw_rate;
+	float gx;
+	float gy;
+	float gz;
 };
 
 /* --- ATSP - ATTITUDE SET POINT --- */
@@ -289,7 +292,7 @@ struct log_PARM_s {
 /* construct list of all message formats */
 static const struct log_format_s log_formats[] = {
 	/* business-level messages, ID < 0x80 */
-	LOG_FORMAT(ATT, "ffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate"),
+	LOG_FORMAT(ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"),
 	LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"),
 	LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
 	LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"),

src/modules/uORB/topics/vehicle_attitude.h

@@ -76,6 +76,7 @@ struct vehicle_attitude_s {
 	float rate_offsets[3];	/**< Offsets of the body angular rates from zero		*/
 	float R[3][3];		/**< Rotation matrix body to world, (Tait-Bryan, NED)		*/
 	float q[4];		/**< Quaternion (NED)						*/
+	float g_comp[3];	/**< Compensated gravity vector				*/
 	bool R_valid;		/**< Rotation matrix valid					*/
 	bool q_valid;		/**< Quaternion valid						*/