Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-06 16:58:48 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

[stabilization] attitude quat: fix psi angle

Browse Source
This commit is contained in:
Felix Ruess
2013-09-02 18:13:57 +02:00
parent d5322dd73e
commit ec3218cdb5
2 changed files with 29 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c
+14 -1
View File
@@ -162,7 +162,8 @@ void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) {
* onto the horizontal plane with the psi setpoint.
*
* angle between two vectors a and b:
* angle = atan2(norm(cross(a,b)), dot(a,b))
* angle = atan2(norm(cross(a,b)), dot(a,b)) * sign(dot(cross(a,b), n))
* where n is the thrust vector (i.e. both a and b lie in that plane)
*/
const struct FloatVect3 xaxis = {1.0, 0.0, 0.0};
const struct FloatVect3 zaxis = {0.0, 0.0, 1.0};
@@ -174,14 +175,17 @@ void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) {
psi_vect.x = cosf(stab_att_sp_euler.psi);
psi_vect.y = sinf(stab_att_sp_euler.psi);
psi_vect.z = 0.0;
// normal is the direction of the thrust vector
struct FloatVect3 normal;
FLOAT_QUAT_VMULT(normal, q_rp, zaxis);
// projection of desired heading onto body x-y plane
// b = v - dot(v,n)*n
float dot = FLOAT_VECT3_DOT_PRODUCT(psi_vect, normal);
struct FloatVect3 dotn;
FLOAT_VECT3_SMUL(dotn, normal, dot);
// b = v - dot(v,n)*n
struct FloatVect3 b;
FLOAT_VECT3_DIFF(b, psi_vect, dotn);
dot = FLOAT_VECT3_DOT_PRODUCT(a, b);
@@ -189,14 +193,23 @@ void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) {
VECT3_CROSS_PRODUCT(cross, a, b);
// norm of the cross product
float nc = FLOAT_VECT3_NORM(cross);
// angle = atan2(norm(cross(a,b)), dot(a,b))
float yaw2 = atan2(nc, dot) / 2.0;
// negative angle if needed
// sign(dot(cross(a,b), n)
float dot_cross_ab = FLOAT_VECT3_DOT_PRODUCT(cross, normal);
if (dot_cross_ab < 0) {
yaw2 = -yaw2;
}
/* quaternion with yaw command */
struct FloatQuat q_yaw;
QUAT_ASSIGN(q_yaw, cosf(yaw2), 0.0, 0.0, sinf(yaw2));
/* final setpoint: apply roll/pitch, then yaw around resulting body z-axis */
FLOAT_QUAT_COMP(stab_att_sp_quat, q_yaw, q_rp);
FLOAT_QUAT_NORMALIZE(stab_att_sp_quat);
FLOAT_QUAT_WRAP_SHORTEST(stab_att_sp_quat);
}
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c
+15 -1
View File
@@ -117,7 +117,8 @@ void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) {
* onto the horizontal plane with the psi setpoint.
*
* angle between two vectors a and b:
* angle = atan2(norm(cross(a,b)), dot(a,b))
* angle = atan2(norm(cross(a,b)), dot(a,b)) * sign(dot(cross(a,b), n))
* where n is the thrust vector (i.e. both a and b lie in that plane)
*/
const struct FloatVect3 xaxis = {1.0, 0.0, 0.0};
const struct FloatVect3 zaxis = {0.0, 0.0, 1.0};
@@ -129,23 +130,35 @@ void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) {
psi_vect.x = cosf(psi_sp);
psi_vect.y = sinf(psi_sp);
psi_vect.z = 0.0;
// normal is the direction of the thrust vector
struct FloatVect3 normal;
FLOAT_QUAT_VMULT(normal, q_rp, zaxis);
// projection of desired heading onto body x-y plane
// b = v - dot(v,n)*n
float dot = FLOAT_VECT3_DOT_PRODUCT(psi_vect, normal);
struct FloatVect3 dotn;
FLOAT_VECT3_SMUL(dotn, normal, dot);
// b = v - dot(v,n)*n
struct FloatVect3 b;
FLOAT_VECT3_DIFF(b, psi_vect, dotn);
dot = FLOAT_VECT3_DOT_PRODUCT(a, b);
struct FloatVect3 cross;
VECT3_CROSS_PRODUCT(cross, a, b);
// norm of the cross product
float nc = FLOAT_VECT3_NORM(cross);
// angle = atan2(norm(cross(a,b)), dot(a,b))
float yaw2 = atan2(nc, dot) / 2.0;
// negative angle if needed
// sign(dot(cross(a,b), n)
float dot_cross_ab = FLOAT_VECT3_DOT_PRODUCT(cross, normal);
if (dot_cross_ab < 0) {
yaw2 = -yaw2;
}
/* quaternion with yaw command */
struct FloatQuat q_yaw;
QUAT_ASSIGN(q_yaw, cosf(yaw2), 0.0, 0.0, sinf(yaw2));
@@ -153,6 +166,7 @@ void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) {
/* final setpoint: apply roll/pitch, then yaw around resulting body z-axis */
struct FloatQuat q_sp;
FLOAT_QUAT_COMP(q_sp, q_yaw, q_rp);
FLOAT_QUAT_NORMALIZE(q_sp);
FLOAT_QUAT_WRAP_SHORTEST(q_sp);
/* convert to fixed point */