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

vehicle_local_position: rename yaw -> heading and add reset logic

Browse Source 
- vehicle_global_position yaw removed (redundant)
This commit is contained in:
Daniel Agar
2020-08-07 14:06:38 -04:00
committed by Mathieu Bresciani
parent 33fb9d0c62
commit 97fc1db768
24 changed files with 75 additions and 86 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/PositionControl/ControlMath.cpp
+7 -7
View File
@@ -77,7 +77,7 @@ void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpo
body_z.normalize();
// vector of desired yaw direction in XY plane, rotated by PI/2
Vector3f y_C(-sinf(yaw_sp), cosf(yaw_sp), 0.0f);
const Vector3f y_C{-sinf(yaw_sp), cosf(yaw_sp), 0.f};
// desired body_x axis, orthogonal to body_z
Vector3f body_x = y_C % body_z;
@@ -97,7 +97,7 @@ void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpo
body_x.normalize();
// desired body_y axis
Vector3f body_y = body_z % body_x;
const Vector3f body_y = body_z % body_x;
Dcmf R_sp;
@@ -109,14 +109,14 @@ void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpo
}
// copy quaternion setpoint to attitude setpoint topic
Quatf q_sp = R_sp;
const Quatf q_sp{R_sp};
q_sp.copyTo(att_sp.q_d);
// calculate euler angles, for logging only, must not be used for control
Eulerf euler = R_sp;
att_sp.roll_body = euler(0);
att_sp.pitch_body = euler(1);
att_sp.yaw_body = euler(2);
const Eulerf euler{R_sp};
att_sp.roll_body = euler.phi();
att_sp.pitch_body = euler.theta();
att_sp.yaw_body = euler.psi();
}
Vector2f constrainXY(const Vector2f &v0, const Vector2f &v1, const float &max)