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; }
float test_quat_comp_inv(struct FloatQuat qa2c_f, struct FloatQuat qb2c_f, int display) { struct FloatQuat qa2b_f; float_quat_comp_inv(&qa2b_f, &qa2c_f, &qb2c_f); struct Int32Quat qa2c_i; QUAT_BFP_OF_REAL(qa2c_i, qa2c_f); struct Int32Quat qb2c_i; QUAT_BFP_OF_REAL(qb2c_i, qb2c_f); struct Int32Quat qa2b_i; int32_quat_comp_inv(&qa2b_i, &qa2c_i, &qb2c_i); struct FloatQuat err; QUAT_DIFF(err, qa2b_f, qa2b_i); float norm_err = FLOAT_QUAT_NORM(err); if (display) { printf("quat comp_inv\n"); DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("a2bf", qa2b_f); DISPLAY_INT32_QUAT_AS_EULERS_DEG("a2bi", qa2b_i); } return norm_err; }
static void send_att(struct transport_tx *trans, struct link_device *dev) { /* compute eulers in int (IMU frame) */ struct FloatEulers ltp_to_imu_euler; float_eulers_of_quat(<p_to_imu_euler, &ahrs_float_inv.state.quat); struct Int32Eulers eulers_imu; EULERS_BFP_OF_REAL(eulers_imu, ltp_to_imu_euler); /* compute Eulers in int (body frame) */ struct FloatQuat ltp_to_body_quat; struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_float_inv.body_to_imu); float_quat_comp_inv(<p_to_body_quat, &ahrs_float_inv.state.quat, body_to_imu_quat); struct FloatEulers ltp_to_body_euler; float_eulers_of_quat(<p_to_body_euler, <p_to_body_quat); struct Int32Eulers eulers_body; EULERS_BFP_OF_REAL(eulers_body, ltp_to_body_euler); pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID, &eulers_imu.phi, &eulers_imu.theta, &eulers_imu.psi, &eulers_body.phi, &eulers_body.theta, &eulers_body.psi, &ahrs_finv_id); }
/** 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); } }
/** * Compute body orientation and rates from imu orientation and rates */ static inline void compute_body_orientation_and_rates(void) { /* Compute LTP to BODY quaternion */ struct FloatQuat ltp_to_body_quat; struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&imu.body_to_imu); float_quat_comp_inv(<p_to_body_quat, &ahrs_impl.ltp_to_imu_quat, body_to_imu_quat); /* Set state */ stateSetNedToBodyQuat_f(<p_to_body_quat); /* compute body rates */ struct FloatRates body_rate; struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&imu.body_to_imu); float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_impl.imu_rate); stateSetBodyRates_f(&body_rate); }
void ahrs_fc_update_heading(float heading) { FLOAT_ANGLE_NORMALIZE(heading); 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); struct FloatRMat ltp_to_body_rmat; float_rmat_of_quat(<p_to_body_rmat, <p_to_body_quat); // row 0 of ltp_to_body_rmat = body x-axis in ltp frame // we only consider x and y struct FloatVect2 expected_ltp = { RMAT_ELMT(ltp_to_body_rmat, 0, 0), RMAT_ELMT(ltp_to_body_rmat, 0, 1) }; // expected_heading cross measured_heading struct FloatVect3 residual_ltp = { 0, 0, expected_ltp.x * sinf(heading) - expected_ltp.y * cosf(heading) }; struct FloatVect3 residual_imu; float_rmat_vmult(&residual_imu, &ahrs_fc.ltp_to_imu_rmat, &residual_ltp); const float heading_rate_update_gain = 2.5; RATES_ADD_SCALED_VECT(ahrs_fc.rate_correction, residual_imu, heading_rate_update_gain); float heading_bias_update_gain; /* crude attempt to only update bias if deviation is small * e.g. needed when you only have gps providing heading * and the inital heading is totally different from * the gps course information you get once you have a gps fix. * Otherwise the bias will be falsely "corrected". */ if (fabs(residual_ltp.z) < sinf(RadOfDeg(5.))) { heading_bias_update_gain = -2.5e-4; } else { heading_bias_update_gain = 0.0; } RATES_ADD_SCALED_VECT(ahrs_fc.gyro_bias, residual_imu, heading_bias_update_gain); }
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_inv_norm_shortest(struct FloatQuat *a2b, struct FloatQuat *a2c, struct FloatQuat *b2c) { float_quat_comp_inv(a2b, a2c, b2c); float_quat_wrap_shortest(a2b); float_quat_normalize(a2b); }