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