float float_rmat_reorthogonalize(struct FloatRMat *rm)
{
const struct FloatVect3 r0 = {RMAT_ELMT(*rm, 0, 0),
RMAT_ELMT(*rm, 0, 1),
RMAT_ELMT(*rm, 0, 2)
};
const struct FloatVect3 r1 = {RMAT_ELMT(*rm, 1, 0),
RMAT_ELMT(*rm, 1, 1),
RMAT_ELMT(*rm, 1, 2)
};
float _err = -0.5 * VECT3_DOT_PRODUCT(r0, r1);
struct FloatVect3 r0_t;
VECT3_SUM_SCALED(r0_t, r0, r1, _err);
struct FloatVect3 r1_t;
VECT3_SUM_SCALED(r1_t, r1, r0, _err);
struct FloatVect3 r2_t;
VECT3_CROSS_PRODUCT(r2_t, r0_t, r1_t);
float s = renorm_factor(VECT3_NORM2(r0_t));
MAT33_ROW_VECT3_SMUL(*rm, 0, r0_t, s);
s = renorm_factor(VECT3_NORM2(r1_t));
MAT33_ROW_VECT3_SMUL(*rm, 1, r1_t, s);
s = renorm_factor(VECT3_NORM2(r2_t));
MAT33_ROW_VECT3_SMUL(*rm, 2, r2_t, s);
return _err;
}
void quat_from_earth_cmd_f(struct FloatQuat *quat, struct FloatVect2 *cmd, float heading) {
/* cmd_x is positive to north = negative pitch
* cmd_y is positive to east = positive roll
*
* orientation vector describing simultaneous rotation of roll/pitch
*/
const struct FloatVect3 ov = {cmd->y, -cmd->x, 0.0};
/* quaternion from that orientation vector */
struct FloatQuat q_rp;
float_quat_of_orientation_vect(&q_rp, &ov);
/* as rotation matrix */
struct FloatRMat R_rp;
float_rmat_of_quat(&R_rp, &q_rp);
/* body x-axis (before heading command) is first column */
struct FloatVect3 b_x;
VECT3_ASSIGN(b_x, R_rp.m[0], R_rp.m[3], R_rp.m[6]);
/* body z-axis (thrust vect) is last column */
struct FloatVect3 thrust_vect;
VECT3_ASSIGN(thrust_vect, R_rp.m[2], R_rp.m[5], R_rp.m[8]);
/// @todo optimize yaw angle calculation
/*
* Instead of using the psi setpoint angle to rotate around the body z-axis,
* calculate the real angle needed to align the projection of the body x-axis
* onto the horizontal plane with the psi setpoint.
*
* angle between two vectors a and b:
* angle = atan2(norm(cross(a,b)), dot(a,b)) * sign(dot(cross(a,b), n))
* where the normal n is the thrust vector (i.e. both a and b lie in that plane)
*/
// desired heading vect in earth x-y plane
const struct FloatVect3 psi_vect = {cosf(heading), sinf(heading), 0.0};
/* projection of desired heading onto body x-y plane
* b = v - dot(v,n)*n
*/
float dot = VECT3_DOT_PRODUCT(psi_vect, thrust_vect);
struct FloatVect3 dotn;
VECT3_SMUL(dotn, thrust_vect, dot);
// b = v - dot(v,n)*n
struct FloatVect3 b;
VECT3_DIFF(b, psi_vect, dotn);
dot = VECT3_DOT_PRODUCT(b_x, b);
struct FloatVect3 cross;
VECT3_CROSS_PRODUCT(cross, b_x, 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 = VECT3_DOT_PRODUCT(cross, thrust_vect);
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(quat, &q_rp, &q_yaw);
float_quat_normalize(quat);
float_quat_wrap_shortest(quat);
}