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

ControlMath: replace attitude generation

Browse Source 
with simple quaternion calculation.
Difference: The body yaw axis when projected
onto the horizontal plane is not stying in line
with the wold frame heading. This alignment
was generating additional yaw control effort
for any combined roll and pitch rotation.
This commit is contained in:
Matthias Grob 2019-11-07 13:42:38 +01:00
parent 75695787fd
commit e21b43f5e2
2 changed files with 19 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -44,9 +44,20 @@ using namespace matrix;
namespace ControlMath
{
void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp)
{
bodyzToAttitude(-thr_sp, yaw_sp, att_sp);
const Quatf q = bodyzToQuaternion(thr_sp, yaw_sp);
// copy quaternion setpoint to attitude setpoint topic
q.copyTo(att_sp.q_d);
// calculate euler angles, for logging only, must not be used for control
const Eulerf euler{q};
att_sp.roll_body = euler.phi();
att_sp.pitch_body = euler.theta();
att_sp.yaw_body = euler.psi();
att_sp.thrust_body[2] = -thr_sp.length();
}
@ -67,56 +78,16 @@ void limitTilt(Vector3f &body_unit, const Vector3f &world_unit, const float max_
body_unit = cosf(angle) * world_unit + sinf(angle) * rejection.unit();
}
void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp)
Quatf bodyzToQuaternion(Vector3f body_z, const float yaw_sp)
{
// zero vector, no direction, set safe level value
if (body_z.norm_squared() < FLT_EPSILON) {
body_z(2) = 1.f;
body_z(2) = -1.f;
}
body_z.normalize();
// vector of desired yaw direction in XY plane, rotated by PI/2
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;
// keep nose to front while inverted upside down
if (body_z(2) < 0.f) {
body_x = -body_x;
}
if (fabsf(body_z(2)) < 0.000001f) {
// desired thrust is in XY plane, set X downside to construct correct matrix,
// but yaw component will not be used actually
body_x.zero();
body_x(2) = 1.f;
}
body_x.normalize();
// desired body_y axis
const Vector3f body_y = body_z % body_x;
Dcmf R_sp;
// fill rotation matrix
for (int i = 0; i < 3; i++) {
R_sp(i, 0) = body_x(i);
R_sp(i, 1) = body_y(i);
R_sp(i, 2) = body_z(i);
}
// copy quaternion setpoint to attitude setpoint topic
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
const Eulerf euler{R_sp};
att_sp.roll_body = euler.phi();
att_sp.pitch_body = euler.theta();
att_sp.yaw_body = euler.psi();
const Quatf q{Vector3f(0.f, 0.f, -1.f), body_z};
const Quatf q_yaw{cosf(yaw_sp / 2.f), 0.f, 0.f, sinf(yaw_sp / 2.f)};
return q * q_yaw;
}
Vector2f constrainXY(const Vector2f &v0, const Vector2f &v1, const float &max)

2
src/modules/mc_pos_control/PositionControl/ControlMath.hpp
View File

@ -69,6 +69,8 @@ void limitTilt(matrix::Vector3f &body_unit, const matrix::Vector3f &world_unit,
*/
void bodyzToAttitude(matrix::Vector3f body_z, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp);
matrix::Quatf bodyzToQuaternion(matrix::Vector3f body_z, const float yaw_sp);
/**
* Outputs the sum of two vectors but respecting the limits and priority.
* The sum of two vectors are constraint such that v0 has priority over v1.