Pos Ctrl: Logging more tilt information for debugging

src/modules/mc_pos_control/PositionControl/ControlMath.cpp

@@ -89,6 +89,19 @@ void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, const matrix::
 
 	// Estimate the optimal tilt angle and direction to counteract the wind
 
+	// Calculate the setpoint z axis
+	Vector3f cmd_z;
+	matrix::Dcmf R_cmd = matrix::Quatf(att_sp.q_d);
+
+	for (int i = 0; i < 3; i++) {
+		cmd_z(i) = R_cmd(i, 2);
+	}
+
+	omni_status.tilt_angle_est = asinf(Vector2f(cmd_z(0), cmd_z(1)).norm() / cmd_z.norm());;
+	omni_status.tilt_direction_est = wrap_2pi(atan2f(-cmd_z(1), -cmd_z(0)));
+	omni_status.tilt_roll_est = att_sp.roll_body;
+	omni_status.tilt_pitch_est = att_sp.pitch_body;
+
 	// Calculate the current z axis
 	Vector3f curr_z;
 	matrix::Dcmf R_body = att;

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -698,10 +698,6 @@ MulticopterPositionControl::Run()
 						     attitude_setpoint, omni_status);
 
 			omni_status.att_mode = _param_omni_att_mode.get();
-			omni_status.tilt_angle_est = _tilt_angle;
-			omni_status.tilt_direction_est = _tilt_dir;
-			omni_status.tilt_roll_est = _tilt_roll;
-			omni_status.tilt_pitch_est = _tilt_pitch;
 
 			_omni_attitude_status_pub.publish(omni_status);