float test_quat_comp(struct FloatQuat qa2b_f, struct FloatQuat qb2c_f, int display)
{
struct FloatQuat qa2c_f;
float_quat_comp(&qa2c_f, &qa2b_f, &qb2c_f);
struct Int32Quat qa2b_i;
QUAT_BFP_OF_REAL(qa2b_i, qa2b_f);
struct Int32Quat qb2c_i;
QUAT_BFP_OF_REAL(qb2c_i, qb2c_f);
struct Int32Quat qa2c_i;
int32_quat_comp(&qa2c_i, &qa2b_i, &qb2c_i);
struct FloatQuat err;
QUAT_DIFF(err, qa2c_f, qa2c_i);
float norm_err = FLOAT_QUAT_NORM(err);
if (display) {
printf("quat comp\n");
DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("a2cf", qa2c_f);
DISPLAY_INT32_QUAT_AS_EULERS_DEG("a2ci", qa2c_i);
}
return norm_err;
}
void ahrs_fc_realign_heading(float heading)
{
FLOAT_ANGLE_NORMALIZE(heading);
/* quaternion representing only the heading rotation from ltp to body */
struct FloatQuat q_h_new;
q_h_new.qx = 0.0;
q_h_new.qy = 0.0;
q_h_new.qz = sinf(heading / 2.0);
q_h_new.qi = cosf(heading / 2.0);
struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_fc.body_to_imu);
struct FloatQuat ltp_to_body_quat;
float_quat_comp_inv(<p_to_body_quat, &ahrs_fc.ltp_to_imu_quat, body_to_imu_quat);
/* quaternion representing current heading only */
struct FloatQuat q_h;
QUAT_COPY(q_h, ltp_to_body_quat);
q_h.qx = 0.;
q_h.qy = 0.;
float_quat_normalize(&q_h);
/* quaternion representing rotation from current to new heading */
struct FloatQuat q_c;
float_quat_inv_comp_norm_shortest(&q_c, &q_h, &q_h_new);
/* correct current heading in body frame */
struct FloatQuat q;
float_quat_comp_norm_shortest(&q, &q_c, <p_to_body_quat);
/* compute ltp to imu rotation quaternion and matrix */
float_quat_comp(&ahrs_fc.ltp_to_imu_quat, &q, body_to_imu_quat);
float_rmat_of_quat(&ahrs_fc.ltp_to_imu_rmat, &ahrs_fc.ltp_to_imu_quat);
ahrs_fc.heading_aligned = TRUE;
}
//Function that reads the rc setpoint in an earth bound frame
void stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(struct FloatQuat *q_sp, bool_t in_flight,
bool_t in_carefree, bool_t coordinated_turn)
{
// FIXME: remove me, do in quaternion directly
// is currently still needed, since the yaw setpoint integration is done in eulers
#if defined STABILIZATION_ATTITUDE_TYPE_INT
stabilization_attitude_read_rc_setpoint_eulers(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn);
#else
stabilization_attitude_read_rc_setpoint_eulers_f(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn);
#endif
const struct FloatVect3 zaxis = {0., 0., 1.};
struct FloatQuat q_rp_cmd;
stabilization_attitude_read_rc_roll_pitch_earth_quat_f(&q_rp_cmd);
if (in_flight) {
/* get current heading setpoint */
struct FloatQuat q_yaw_sp;
#if defined STABILIZATION_ATTITUDE_TYPE_INT
float_quat_of_axis_angle(&q_yaw_sp, &zaxis, ANGLE_FLOAT_OF_BFP(stab_att_sp_euler.psi));
#else
float_quat_of_axis_angle(&q_yaw_sp, &zaxis, stab_att_sp_euler.psi);
#endif
float_quat_comp(q_sp, &q_yaw_sp, &q_rp_cmd);
} else {
struct FloatQuat q_yaw;
float_quat_of_axis_angle(&q_yaw, &zaxis, stateGetNedToBodyEulers_f()->psi);
/* roll/pitch commands applied to to current heading */
struct FloatQuat q_rp_sp;
float_quat_comp(&q_rp_sp, &q_yaw, &q_rp_cmd);
float_quat_normalize(&q_rp_sp);
QUAT_COPY(*q_sp, q_rp_sp);
}
}
float test_quat(void)
{
struct FloatVect3 u = { 1., 2., 3.};
FLOAT_VECT3_NORMALIZE(u);
float angle = RadOfDeg(30.);
struct FloatQuat q;
FLOAT_QUAT_OF_AXIS_ANGLE(q, u, angle);
DISPLAY_FLOAT_QUAT("q ", q);
struct FloatQuat iq;
FLOAT_QUAT_INVERT(iq, q);
DISPLAY_FLOAT_QUAT("iq", iq);
struct FloatQuat q1;
float_quat_comp(&q1, &q, &iq);
DISPLAY_FLOAT_QUAT("q1", q1);
struct FloatQuat q2;
float_quat_comp(&q2, &q, &iq);
DISPLAY_FLOAT_QUAT("q2", q2);
struct FloatQuat qe;
QUAT_EXPLEMENTARY(qe, q);
DISPLAY_FLOAT_QUAT("qe", qe);
struct FloatVect3 a = { 2., 1., 3.};
DISPLAY_FLOAT_VECT3("a ", a);
struct FloatVect3 a1;
float_quat_vmult(&a1, &q, &a);
DISPLAY_FLOAT_VECT3("a1", a1);
struct FloatVect3 a2;
float_quat_vmult(&a2, &qe, &a);
DISPLAY_FLOAT_VECT3("a2", a2);
return 0.;
}
/** Read attitude setpoint from RC as quaternion
* Interprets the stick positions as axes.
* @param[in] coordinated_turn true if in horizontal mode forward
* @param[in] in_carefree true if in carefree mode
* @param[in] in_flight true if in flight
* @param[out] q_sp attitude setpoint as quaternion
*/
void stabilization_attitude_read_rc_setpoint_quat_f(struct FloatQuat *q_sp, bool_t in_flight, bool_t in_carefree,
bool_t coordinated_turn)
{
// FIXME: remove me, do in quaternion directly
// is currently still needed, since the yaw setpoint integration is done in eulers
#if defined STABILIZATION_ATTITUDE_TYPE_INT
stabilization_attitude_read_rc_setpoint_eulers(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn);
#else
stabilization_attitude_read_rc_setpoint_eulers_f(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn);
#endif
struct FloatQuat q_rp_cmd;
stabilization_attitude_read_rc_roll_pitch_quat_f(&q_rp_cmd);
/* get current heading */
const struct FloatVect3 zaxis = {0., 0., 1.};
struct FloatQuat q_yaw;
//Care Free mode
if (in_carefree) {
//care_free_heading has been set to current psi when entering care free mode.
float_quat_of_axis_angle(&q_yaw, &zaxis, care_free_heading);
} else {
float_quat_of_axis_angle(&q_yaw, &zaxis, stateGetNedToBodyEulers_f()->psi);
}
/* roll/pitch commands applied to to current heading */
struct FloatQuat q_rp_sp;
float_quat_comp(&q_rp_sp, &q_yaw, &q_rp_cmd);
float_quat_normalize(&q_rp_sp);
if (in_flight) {
/* get current heading setpoint */
struct FloatQuat q_yaw_sp;
#if defined STABILIZATION_ATTITUDE_TYPE_INT
float_quat_of_axis_angle(&q_yaw_sp, &zaxis, ANGLE_FLOAT_OF_BFP(stab_att_sp_euler.psi));
#else
float_quat_of_axis_angle(&q_yaw_sp, &zaxis, stab_att_sp_euler.psi);
#endif
/* rotation between current yaw and yaw setpoint */
struct FloatQuat q_yaw_diff;
float_quat_comp_inv(&q_yaw_diff, &q_yaw_sp, &q_yaw);
/* compute final setpoint with yaw */
float_quat_comp_norm_shortest(q_sp, &q_rp_sp, &q_yaw_diff);
} else {
QUAT_COPY(*q_sp, q_rp_sp);
}
}
float test_quat2(void)
{
struct FloatEulers eula2b;
EULERS_ASSIGN(eula2b, RadOfDeg(70.), RadOfDeg(0.), RadOfDeg(0.));
// DISPLAY_FLOAT_EULERS_DEG("eula2b", eula2b);
struct FloatQuat qa2b;
float_quat_of_eulers(&qa2b, &eula2b);
DISPLAY_FLOAT_QUAT("qa2b", qa2b);
struct DoubleEulers eula2b_d;
EULERS_ASSIGN(eula2b_d, RadOfDeg(70.), RadOfDeg(0.), RadOfDeg(0.));
struct DoubleQuat qa2b_d;
double_quat_of_eulers(&qa2b_d, &eula2b_d);
DISPLAY_FLOAT_QUAT("qa2b_d", qa2b_d);
struct FloatVect3 u = { 1., 0., 0.};
float angle = RadOfDeg(70.);
struct FloatQuat q;
FLOAT_QUAT_OF_AXIS_ANGLE(q, u, angle);
DISPLAY_FLOAT_QUAT("q ", q);
struct FloatEulers eula2c;
EULERS_ASSIGN(eula2c, RadOfDeg(80.), RadOfDeg(0.), RadOfDeg(0.));
// DISPLAY_FLOAT_EULERS_DEG("eula2c", eula2c);
struct FloatQuat qa2c;
float_quat_of_eulers(&qa2c, &eula2c);
DISPLAY_FLOAT_QUAT("qa2c", qa2c);
struct FloatQuat qb2a;
FLOAT_QUAT_INVERT(qb2a, qa2b);
DISPLAY_FLOAT_QUAT("qb2a", qb2a);
struct FloatQuat qb2c1;
float_quat_comp(&qb2c1, &qb2a, &qa2c);
DISPLAY_FLOAT_QUAT("qb2c1", qb2c1);
struct FloatQuat qb2c2;
float_quat_inv_comp(&qb2c2, &qa2b, &qa2c);
DISPLAY_FLOAT_QUAT("qb2c2", qb2c2);
return 0.;
}
void stabilization_attitude_set_setpoint_rp_quat_f(bool in_flight, int32_t heading)
{
struct FloatQuat q_rp_cmd;
float_quat_of_eulers(&q_rp_cmd, &guidance_euler_cmd); //TODO this is a quaternion without yaw! add the desired yaw before you use it!
/* get current heading */
const struct FloatVect3 zaxis = {0., 0., 1.};
struct FloatQuat q_yaw;
float_quat_of_axis_angle(&q_yaw, &zaxis, stateGetNedToBodyEulers_f()->psi);
/* roll/pitch commands applied to to current heading */
struct FloatQuat q_rp_sp;
float_quat_comp(&q_rp_sp, &q_yaw, &q_rp_cmd);
float_quat_normalize(&q_rp_sp);
struct FloatQuat q_sp;
if (in_flight) {
/* get current heading setpoint */
struct FloatQuat q_yaw_sp;
float_quat_of_axis_angle(&q_yaw_sp, &zaxis, ANGLE_FLOAT_OF_BFP(heading));
/* rotation between current yaw and yaw setpoint */
struct FloatQuat q_yaw_diff;
float_quat_comp_inv(&q_yaw_diff, &q_yaw_sp, &q_yaw);
/* compute final setpoint with yaw */
float_quat_comp_norm_shortest(&q_sp, &q_rp_sp, &q_yaw_diff);
} else {
QUAT_COPY(q_sp, q_rp_sp);
}
QUAT_BFP_OF_REAL(stab_att_sp_quat,q_sp);
}
void float_quat_comp_norm_shortest(struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c)
{
float_quat_comp(a2c, a2b, b2c);
float_quat_wrap_shortest(a2c);
float_quat_normalize(a2c);
}
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);
}