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); }