fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

Pos Ctrl: Logging more tilt information for debugging

Browse Source
This commit is contained in:
Azarakhsh Keipour 2020-09-22 11:53:14 -04:00
parent b825e2f140
commit 96a496b07d
2 changed files with 13 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
src/modules/mc_pos_control/PositionControl/ControlMath.cpp
View File

@ -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;

4
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -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);