/*
* Compute body orientation and rates from imu orientation and rates
*/
static inline void compute_body_orientation_and_rates(void) {
FLOAT_QUAT_COMP_INV(ahrs_float.ltp_to_body_quat,
ahrs_float.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat);
FLOAT_RMAT_COMP_INV(ahrs_float.ltp_to_body_rmat,
ahrs_float.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat);
FLOAT_EULERS_OF_RMAT(ahrs_float.ltp_to_body_euler, ahrs_float.ltp_to_body_rmat);
FLOAT_RMAT_TRANSP_RATEMULT(ahrs_float.body_rate, ahrs_impl.body_to_imu_rmat, ahrs_float.imu_rate);
}
void stabilization_attitude_read_rc_setpoint_quat_f(struct FloatQuat* q_sp, bool_t in_flight) {
// 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);
#else
stabilization_attitude_read_rc_setpoint_eulers_f(&stab_att_sp_euler, in_flight);
#endif
struct FloatQuat q_rp_cmd;
#if USE_EARTH_BOUND_RC_SETPOINT
stabilization_attitude_read_rc_roll_pitch_earth_quat_f(&q_rp_cmd);
#else
stabilization_attitude_read_rc_roll_pitch_quat_f(&q_rp_cmd);
#endif
/* get current heading */
const struct FloatVect3 zaxis = {0., 0., 1.};
struct FloatQuat q_yaw;
//Care Free mode
if (guidance_h_mode == GUIDANCE_H_MODE_CARE_FREE) {
//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;
FLOAT_QUAT_COMP_INV(ltp_to_body_quat, ahrs_impl.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat);
/* Set state */
#ifdef AHRS_UPDATE_FW_ESTIMATOR
struct FloatEulers neutrals_to_body_eulers = { ins_roll_neutral, ins_pitch_neutral, 0. };
struct FloatQuat neutrals_to_body_quat, ltp_to_neutrals_quat;
FLOAT_QUAT_OF_EULERS(neutrals_to_body_quat, neutrals_to_body_eulers);
FLOAT_QUAT_NORMALIZE(neutrals_to_body_quat);
FLOAT_QUAT_COMP_INV(ltp_to_neutrals_quat, ltp_to_body_quat, neutrals_to_body_quat);
stateSetNedToBodyQuat_f(<p_to_neutrals_quat);
#else
stateSetNedToBodyQuat_f(<p_to_body_quat);
#endif
/* compute body rates */
struct FloatRates body_rate;
FLOAT_RMAT_TRANSP_RATEMULT(body_rate, ahrs_impl.body_to_imu_rmat, ahrs_impl.imu_rate);
stateSetBodyRates_f(&body_rate);
}
/**
* Compute body orientation and rates from imu orientation and rates
*/
static inline void set_body_state_from_quat(void) {
/* Compute LTP to BODY quaternion */
struct FloatQuat ltp_to_body_quat;
FLOAT_QUAT_COMP_INV(ltp_to_body_quat, ahrs_impl.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat);
/* Set in state interface */
stateSetNedToBodyQuat_f(<p_to_body_quat);
/* compute body rates */
struct FloatRates body_rate;
FLOAT_RMAT_TRANSP_RATEMULT(body_rate, ahrs_impl.body_to_imu_rmat, ahrs_impl.imu_rate);
/* Set state */
stateSetBodyRates_f(&body_rate);
}
void stabilization_attitude_read_rc(bool_t in_flight) {
#if USE_SETPOINTS_WITH_TRANSITIONS
stabilization_attitude_read_rc_absolute(in_flight);
#else
//FIXME: remove me, do in quaternion directly
stabilization_attitude_read_rc_setpoint_eulers(&stab_att_sp_euler, in_flight);
struct FloatQuat q_rp_cmd;
#if USE_EARTH_BOUND_RC_SETPOINT
stabilization_attitude_read_rc_roll_pitch_earth_quat(&q_rp_cmd);
#else
stabilization_attitude_read_rc_roll_pitch_quat(&q_rp_cmd);
#endif
/* get current heading */
const struct FloatVect3 zaxis = {0., 0., 1.};
struct FloatQuat q_yaw;
FLOAT_QUAT_OF_AXIS_ANGLE(q_yaw, zaxis, ANGLE_FLOAT_OF_BFP(stateGetNedToBodyEulers_i()->psi));
/* apply roll and pitch commands with respect to current heading */
struct FloatQuat q_sp;
FLOAT_QUAT_COMP(q_sp, q_yaw, q_rp_cmd);
FLOAT_QUAT_NORMALIZE(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(stab_att_sp_euler.psi));
/* rotation between current yaw and yaw setpoint */
struct FloatQuat q_yaw_diff;
FLOAT_QUAT_COMP_INV(q_yaw_diff, q_yaw_sp, q_yaw);
/* temporary copy with roll/pitch command and current heading */
struct FloatQuat q_rp_sp;
QUAT_COPY(q_rp_sp, q_sp);
/* compute final setpoint with yaw */
FLOAT_QUAT_COMP_NORM_SHORTEST(q_sp, q_rp_sp, q_yaw_diff);
}
QUAT_BFP_OF_REAL(stab_att_sp_quat, q_sp);
#endif
}
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;
}
