void ahrs_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* ltp_to_body_quat = stateGetNedToBodyQuat_f();
/* 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, *ltp_to_body_quat);
/* compute ltp to imu rotation quaternion and matrix */
FLOAT_QUAT_COMP(ahrs_impl.ltp_to_imu_quat, q, ahrs_impl.body_to_imu_quat);
FLOAT_RMAT_OF_QUAT(ahrs_impl.ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat);
/* set state */
stateSetNedToBodyQuat_f(&q);
ahrs_impl.heading_aligned = TRUE;
}
/**
* Incorporate errors to reference and zeros state
*/
static inline void reset_state(void) {
ahrs_impl.gibbs_cor.qi = 2.;
struct FloatQuat q_tmp;
FLOAT_QUAT_COMP(q_tmp, ahrs_impl.ltp_to_imu_quat, ahrs_impl.gibbs_cor);
FLOAT_QUAT_NORMALIZE(q_tmp);
memcpy(&ahrs_impl.ltp_to_imu_quat, &q_tmp, sizeof(ahrs_impl.ltp_to_imu_quat));
FLOAT_QUAT_ZERO(ahrs_impl.gibbs_cor);
}
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);
}
}
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.;
}
void stabilization_attitude_run(bool_t enable_integrator) {
/*
* Update reference
*/
stabilization_attitude_ref_update();
/*
* Compute errors for feedback
*/
/* attitude error */
struct FloatQuat att_err;
FLOAT_QUAT_INV_COMP(att_err, ahrs_float.ltp_to_body_quat, stab_att_ref_quat);
/* wrap it in the shortest direction */
FLOAT_QUAT_WRAP_SHORTEST(att_err);
/* rate error */
struct FloatRates rate_err;
RATES_DIFF(rate_err, stab_att_ref_rate, ahrs_float.body_rate);
/* integrated error */
if (enable_integrator) {
struct FloatQuat new_sum_err, scaled_att_err;
/* update accumulator */
scaled_att_err.qi = att_err.qi;
scaled_att_err.qx = att_err.qx / IERROR_SCALE;
scaled_att_err.qy = att_err.qy / IERROR_SCALE;
scaled_att_err.qz = att_err.qz / IERROR_SCALE;
FLOAT_QUAT_COMP(new_sum_err, stabilization_att_sum_err_quat, scaled_att_err);
FLOAT_QUAT_NORMALIZE(new_sum_err);
FLOAT_QUAT_COPY(stabilization_att_sum_err_quat, new_sum_err);
FLOAT_EULERS_OF_QUAT(stabilization_att_sum_err_eulers, stabilization_att_sum_err_quat);
} else {
/* reset accumulator */
FLOAT_QUAT_ZERO( stabilization_att_sum_err_quat );
FLOAT_EULERS_ZERO( stabilization_att_sum_err_eulers );
}
attitude_run_ff(stabilization_att_ff_cmd, &stabilization_gains[gain_idx], &stab_att_ref_accel);
attitude_run_fb(stabilization_att_fb_cmd, &stabilization_gains[gain_idx], &att_err, &rate_err, &ahrs_float.body_rate_d, &stabilization_att_sum_err_quat);
// FIXME: this is very dangerous! only works if this really includes all commands
for (int i = COMMAND_ROLL; i <= COMMAND_YAW_SURFACE; i++) {
stabilization_cmd[i] = stabilization_att_fb_cmd[i]+stabilization_att_ff_cmd[i];
}
/* bound the result */
BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ);
BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ);
BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ);
}
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.;
}
//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);
}
}
void stabilization_attitude_ref_update(void) {
/* integrate reference attitude */
#if STABILIZATION_ATTITUDE_REF_QUAT_INFINITESIMAL_STEP
struct FloatQuat qdot;
FLOAT_QUAT_DERIVATIVE(qdot, stab_att_ref_rate, stab_att_ref_quat);
QUAT_SMUL(qdot, qdot, DT_UPDATE);
QUAT_ADD(stab_att_ref_quat, qdot);
#else // use finite step (involves trig)
struct FloatQuat delta_q;
FLOAT_QUAT_DIFFERENTIAL(delta_q, stab_att_ref_rate, DT_UPDATE);
/* compose new ref_quat by quaternion multiplication of delta rotation and current ref_quat */
struct FloatQuat new_ref_quat;
FLOAT_QUAT_COMP(new_ref_quat, delta_q, stab_att_ref_quat);
QUAT_COPY(stab_att_ref_quat, new_ref_quat);
#endif
FLOAT_QUAT_NORMALIZE(stab_att_ref_quat);
/* integrate reference rotational speeds */
struct FloatRates delta_rate;
RATES_SMUL(delta_rate, stab_att_ref_accel, DT_UPDATE);
RATES_ADD(stab_att_ref_rate, delta_rate);
/* compute reference angular accelerations */
struct FloatQuat err;
/* compute reference attitude error */
FLOAT_QUAT_INV_COMP(err, stab_att_sp_quat, stab_att_ref_quat);
/* wrap it in the shortest direction */
FLOAT_QUAT_WRAP_SHORTEST(err);
/* propagate the 2nd order linear model */
stab_att_ref_accel.p = -2.*stab_att_ref_model[ref_idx].zeta.p*stab_att_ref_model[ref_idx].omega.p*stab_att_ref_rate.p
- stab_att_ref_model[ref_idx].omega.p*stab_att_ref_model[ref_idx].omega.p*err.qx;
stab_att_ref_accel.q = -2.*stab_att_ref_model[ref_idx].zeta.q*stab_att_ref_model[ref_idx].omega.q*stab_att_ref_rate.q
- stab_att_ref_model[ref_idx].omega.q*stab_att_ref_model[ref_idx].omega.q*err.qy;
stab_att_ref_accel.r = -2.*stab_att_ref_model[ref_idx].zeta.r*stab_att_ref_model[ref_idx].omega.r*stab_att_ref_rate.r
- stab_att_ref_model[ref_idx].omega.r*stab_att_ref_model[ref_idx].omega.r*err.qz;
/* saturate acceleration */
const struct FloatRates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q, -REF_ACCEL_MAX_R };
const struct FloatRates MAX_ACCEL = { REF_ACCEL_MAX_P, REF_ACCEL_MAX_Q, REF_ACCEL_MAX_R };
RATES_BOUND_BOX(stab_att_ref_accel, MIN_ACCEL, MAX_ACCEL);
/* saturate angular speed and trim accel accordingly */
SATURATE_SPEED_TRIM_ACCEL();
/* compute ref_euler */
FLOAT_EULERS_OF_QUAT(stab_att_ref_euler, stab_att_ref_quat);
}
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(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;
}
static void print_estimator_state(double time) {
#if FILTER_OUTPUT_IN_NED
struct EcefCoor_d pos_ecef,
cur_pos_ecef,
cur_vel_ecef;
struct NedCoor_d pos_ned,
vel_ned;
struct DoubleQuat q_ecef2body,
q_ecef2enu,
q_enu2body,
q_ned2enu,
q_ned2body;
VECTOR_AS_VECT3(pos_ecef,pos_0_ecef);
VECTOR_AS_VECT3(cur_pos_ecef,ins.avg_state.position);
VECTOR_AS_VECT3(cur_vel_ecef,ins.avg_state.velocity);
ned_of_ecef_point_d(&pos_ned, ¤t_ltp, &cur_pos_ecef);
ned_of_ecef_vect_d(&vel_ned, ¤t_ltp, &cur_vel_ecef);
int32_t xdd = 0;
int32_t ydd = 0;
int32_t zdd = 0;
int32_t xd = vel_ned.x/0.0000019073;
int32_t yd = vel_ned.y/0.0000019073;
int32_t zd = vel_ned.z/0.0000019073;
int32_t x = pos_ned.x/0.0039;
int32_t y = pos_ned.y/0.0039;
int32_t z = pos_ned.z/0.0039;
fprintf(ins_logfile, "%f %d BOOZ2_INS2 %d %d %d %d %d %d %d %d %d\n", time, AC_ID, xdd, ydd, zdd, xd, yd, zd, x, y, z);
#if 0
QUAT_ASSIGN(q_ecef2body, ins.avg_state.orientation.w(), -ins.avg_state.orientation.x(),
-ins.avg_state.orientation.y(), -ins.avg_state.orientation.z());
QUAT_ASSIGN(q_ned2enu, 0, M_SQRT1_2, M_SQRT1_2, 0);
FLOAT_QUAT_OF_RMAT(q_ecef2enu, current_ltp.ltp_of_ecef);
FLOAT_QUAT_INV_COMP(q_enu2body, q_ecef2enu, q_ecef2body); // q_enu2body = q_ecef2body * (q_ecef2enu)^*
FLOAT_QUAT_COMP(q_ned2body, q_ned2enu, q_enu2body); // q_ned2body = q_enu2body * q_ned2enu
#else /* if 0 */
QUATERNIOND_AS_DOUBLEQUAT(q_ecef2body, ins.avg_state.orientation);
DOUBLE_QUAT_OF_RMAT(q_ecef2enu, current_ltp.ltp_of_ecef);
FLOAT_QUAT_INV_COMP(q_enu2body, q_ecef2enu, q_ecef2body);
QUAT_ENU_FROM_TO_NED(q_enu2body, q_ned2body);
#endif /* if 0 */
struct FloatEulers e;
FLOAT_EULERS_OF_QUAT(e, q_ned2body);
#if PRINT_EULER_NED
printf("EULER % 6.1f % 6.1f % 6.1f\n", e.phi*180*M_1_PI, e.theta*180*M_1_PI, e.psi*180*M_1_PI);
#endif /* PRINT_EULER_NED */
fprintf(ins_logfile, "%f %d AHRS_EULER %f %f %f\n", time, AC_ID, e.phi, e.theta, e.psi);
fprintf(ins_logfile, "%f %d DEBUG_COVARIANCE %f %f %f %f %f %f %f %f %f %f %f %f\n", time, AC_ID,
sqrt(ins.cov( 0, 0)), sqrt(ins.cov( 1, 1)), sqrt(ins.cov( 2, 2)),
sqrt(ins.cov( 3, 3)), sqrt(ins.cov( 4, 4)), sqrt(ins.cov( 5, 5)),
sqrt(ins.cov( 6, 6)), sqrt(ins.cov( 7, 7)), sqrt(ins.cov( 8, 8)),
sqrt(ins.cov( 9, 9)), sqrt(ins.cov(10,10)), sqrt(ins.cov(11,11)));
fprintf(ins_logfile, "%f %d BOOZ_SIM_GYRO_BIAS %f %f %f\n", time, AC_ID, ins.avg_state.gyro_bias(0), ins.avg_state.gyro_bias(1), ins.avg_state.gyro_bias(2));
#else /* FILTER_OUTPUT_IN_ECEF */
int32_t xdd = 0;
int32_t ydd = 0;
int32_t zdd = 0;
int32_t xd = ins.avg_state.velocity(0)/0.0000019073;
int32_t yd = ins.avg_state.velocity(1)/0.0000019073;
int32_t zd = ins.avg_state.velocity(2)/0.0000019073;
int32_t x = ins.avg_state.position(0)/0.0039;
int32_t y = ins.avg_state.position(1)/0.0039;
int32_t z = ins.avg_state.position(2)/0.0039;
fprintf(ins_logfile, "%f %d BOOZ2_INS2 %d %d %d %d %d %d %d %d %d\n", time, AC_ID, xdd, ydd, zdd, xd, yd, zd, x, y, z);
struct FloatQuat q_ecef2body;
QUAT_ASSIGN(q_ecef2body, ins.avg_state.orientation.w(), ins.avg_state.orientation.x(),
ins.avg_state.orientation.y(), ins.avg_state.orientation.z());
struct FloatEulers e_ecef2body;
FLOAT_EULERS_OF_QUAT(e_ecef2body, q_ecef2body);
fprintf(ins_logfile, "%f %d AHRS_EULER %f %f %f\n", time, AC_ID, e_ecef2body.phi, e_ecef2body.theta, e_ecef2body.psi);
fprintf(ins_logfile, "%f %d DEBUG_COVARIANCE %f %f %f %f %f %f %f %f %f %f %f %f\n", time, AC_ID,
sqrt(ins.cov( 0, 0)), sqrt(ins.cov( 1, 1)), sqrt(ins.cov( 2, 2)),
sqrt(ins.cov( 3, 3)), sqrt(ins.cov( 4, 4)), sqrt(ins.cov( 5, 5)),
sqrt(ins.cov( 6, 6)), sqrt(ins.cov( 7, 7)), sqrt(ins.cov( 8, 8)),
sqrt(ins.cov( 9, 9)), sqrt(ins.cov(10,10)), sqrt(ins.cov(11,11)));
fprintf(ins_logfile, "%f %d BOOZ_SIM_GYRO_BIAS %f %f %f\n", time, AC_ID, ins.avg_state.gyro_bias(0), ins.avg_state.gyro_bias(1), ins.avg_state.gyro_bias(2));
#endif /* FILTER_OUTPUT_IN_NED / ECEF */
}
static void ahrs_do_update_mag(void) {
int i, j, k;
#ifdef BAFL_DEBUG2
printf("Mag update.\n");
#endif
MAGS_FLOAT_OF_BFP(bafl_mag, imu.mag);
/* P_prio = P */
for (i = 0; i < BAFL_SSIZE; i++) {
for (j = 0; j < BAFL_SSIZE; j++) {
bafl_Pprio[i][j] = bafl_P[i][j];
}
}
/*
* set up measurement matrix
*
* h = [236.; -2.; 396.];
* hx = [ h(2); -h(1); 0]; //only psi (phi and theta = 0)
* Hm = Cnb * hx;
* H = [ 0 0 0 0 0
* 0 0 Cnb*hx 0 0 0
* 0 0 0 0 0 ];
* */
/*bafl_H[0][2] = RMAT_ELMT(bafl_dcm, 0, 0) * bafl_h.y - RMAT_ELMT(bafl_dcm, 0, 1) * bafl_h.x;
bafl_H[1][2] = RMAT_ELMT(bafl_dcm, 1, 0) * bafl_h.y - RMAT_ELMT(bafl_dcm, 1, 1) * bafl_h.x;
bafl_H[2][2] = RMAT_ELMT(bafl_dcm, 2, 0) * bafl_h.y - RMAT_ELMT(bafl_dcm, 2, 1) * bafl_h.x;*/
bafl_H[0][2] = -RMAT_ELMT(bafl_dcm, 0, 1);
bafl_H[1][2] = -RMAT_ELMT(bafl_dcm, 1, 1);
bafl_H[2][2] = -RMAT_ELMT(bafl_dcm, 2, 1);
//rest is zero
/**********************************************
* compute Kalman gain K
*
* K = P_prio * H_T * inv(S)
* S = H * P_prio * HT + R
*
* K = P_prio * HT * inv(H * P_prio * HT + R)
*
**********************************************/
/* covariance residual S = H * P_prio * HT + R */
/* temp_S(3x6) = H(3x6) * P_prio(6x6)
*
* only third column of H is non-zero
*/
for (i = 0; i < 3; i++) {
for (j = 0; j < BAFL_SSIZE; j++) {
bafl_tempS[i][j] = bafl_H[i][2] * bafl_Pprio[2][j];
}
}
/* S(3x3) = temp_S(3x6) * HT(6x3) + R(3x3)
*
* only third row of HT is non-zero
*/
for (i = 0; i < 3; i++) {
for (j = 0; j < 3; j++) {
bafl_S[i][j] = bafl_tempS[i][2] * bafl_H[j][2]; /* H[j][2] = HT[2][j] */
}
bafl_S[i][i] += bafl_R_mag;
}
/* invert S
*/
FLOAT_MAT33_INVERT(bafl_invS, bafl_S);
/* temp_K(6x3) = P_prio(6x6) * HT(6x3)
*
* only third row of HT is non-zero
*/
for (i = 0; i < BAFL_SSIZE; i++) {
for (j = 0; j < 3; j++) {
/* not computing zero elements */
bafl_tempK[i][j] = bafl_Pprio[i][2] * bafl_H[j][2]; /* H[j][2] = HT[2][j] */
}
}
/* K(6x3) = temp_K(6x3) * invS(3x3)
*/
for (i = 0; i < BAFL_SSIZE; i++) {
for (j = 0; j < 3; j++) {
bafl_K[i][j] = bafl_tempK[i][0] * bafl_invS[0][j];
bafl_K[i][j] += bafl_tempK[i][1] * bafl_invS[1][j];
bafl_K[i][j] += bafl_tempK[i][2] * bafl_invS[2][j];
}
}
/**********************************************
* Update filter state.
*
* The a priori filter state is zero since the errors are corrected after each update.
*
* X = X_prio + K (y - H * X_prio)
* X = K * y
**********************************************/
/* innovation
* y = Cnb * [hx; hy; hz] - mag
*/
FLOAT_RMAT_VECT3_MUL(bafl_ym, bafl_dcm, bafl_h); //can be optimized
FLOAT_VECT3_SUB(bafl_ym, bafl_mag);
/* X(6) = K(6x3) * y(3)
*/
for (i = 0; i < BAFL_SSIZE; i++) {
bafl_X[i] = bafl_K[i][0] * bafl_ym.x;
bafl_X[i] += bafl_K[i][1] * bafl_ym.y;
bafl_X[i] += bafl_K[i][2] * bafl_ym.z;
}
/**********************************************
* Update the filter covariance.
*
* P = P_prio - K * H * P_prio
* P = ( I - K * H ) * P_prio
*
**********************************************/
/* temp(6x6) = I(6x6) - K(6x3)*H(3x6)
*
* last 3 columns of H are zero
*/
for (i = 0; i < 6; i++) {
for (j = 0; j < 6; j++) {
if (i == j) {
bafl_tempP[i][j] = 1.;
} else {
bafl_tempP[i][j] = 0.;
}
if (j == 2) { /* omit the parts where H is zero */
for (k = 0; k < 3; k++) {
bafl_tempP[i][j] -= bafl_K[i][k] * bafl_H[k][j];
}
}
}
}
/* P(6x6) = temp(6x6) * P_prio(6x6)
*/
for (i = 0; i < BAFL_SSIZE; i++) {
for (j = 0; j < BAFL_SSIZE; j++) {
bafl_P[i][j] = bafl_tempP[i][0] * bafl_Pprio[0][j];
for (k = 1; k < BAFL_SSIZE; k++) {
bafl_P[i][j] += bafl_tempP[i][k] * bafl_Pprio[k][j];
}
}
}
#ifdef LKF_PRINT_P
printf("Pum=");
for (i = 0; i < 6; i++) {
printf("[");
for (j = 0; j < 6; j++) {
printf("%f\t", bafl_P[i][j]);
}
printf("]\n ");
}
printf("\n");
#endif
/**********************************************
* Correct errors.
*
*
**********************************************/
/* Error quaternion.
*/
QUAT_ASSIGN(bafl_q_m_err, 1.0, bafl_X[0]/2, bafl_X[1]/2, bafl_X[2]/2);
FLOAT_QUAT_INVERT(bafl_q_m_err, bafl_q_m_err);
/* normalize */
float q_sq;
q_sq = bafl_q_m_err.qx * bafl_q_m_err.qx + bafl_q_m_err.qy * bafl_q_m_err.qy + bafl_q_m_err.qz * bafl_q_m_err.qz;
if (q_sq > 1) { /* this should actually never happen */
FLOAT_QUAT_SMUL(bafl_q_m_err, bafl_q_m_err, 1 / sqrtf(1 + q_sq));
printf("mag error quaternion q_sq > 1!!\n");
} else {
bafl_q_m_err.qi = sqrtf(1 - q_sq);
}
/* correct attitude
*/
FLOAT_QUAT_COMP(bafl_qtemp, bafl_q_m_err, bafl_quat);
FLOAT_QUAT_COPY(bafl_quat, bafl_qtemp);
/* correct gyro bias
*/
RATES_ASSIGN(bafl_b_m_err, bafl_X[3], bafl_X[4], bafl_X[5]);
RATES_SUB(bafl_bias, bafl_b_m_err);
/*
* compute all representations
*/
/* maintain rotation matrix representation */
FLOAT_RMAT_OF_QUAT(bafl_dcm, bafl_quat);
/* maintain euler representation */
FLOAT_EULERS_OF_RMAT(bafl_eulers, bafl_dcm);
AHRS_TO_BFP();
AHRS_LTP_TO_BODY();
}
static void ahrs_do_update_accel(void) {
int i, j, k;
#ifdef BAFL_DEBUG2
printf("Accel update.\n");
#endif
/* P_prio = P */
for (i = 0; i < BAFL_SSIZE; i++) {
for (j = 0; j < BAFL_SSIZE; j++) {
bafl_Pprio[i][j] = bafl_P[i][j];
}
}
/*
* set up measurement matrix
*
* g = 9.81
* gx = [ 0 -g 0
* g 0 0
* 0 0 0 ]
* H = [ 0 0 0 ]
* [ -Cnb*gx 0 0 0 ]
* [ 0 0 0 ]
*
* */
bafl_H[0][0] = -RMAT_ELMT(bafl_dcm, 0, 1) * BAFL_g;
bafl_H[0][1] = RMAT_ELMT(bafl_dcm, 0, 0) * BAFL_g;
bafl_H[1][0] = -RMAT_ELMT(bafl_dcm, 1, 1) * BAFL_g;
bafl_H[1][1] = RMAT_ELMT(bafl_dcm, 1, 0) * BAFL_g;
bafl_H[2][0] = -RMAT_ELMT(bafl_dcm, 2, 1) * BAFL_g;
bafl_H[2][1] = RMAT_ELMT(bafl_dcm, 2, 0) * BAFL_g;
/* rest is zero
bafl_H[0][2] = 0.;
bafl_H[1][2] = 0.;
bafl_H[2][2] = 0.;*/
/**********************************************
* compute Kalman gain K
*
* K = P_prio * H_T * inv(S)
* S = H * P_prio * HT + R
*
* K = P_prio * HT * inv(H * P_prio * HT + R)
*
**********************************************/
/* covariance residual S = H * P_prio * HT + R */
/* temp_S(3x6) = H(3x6) * P_prio(6x6)
* last 4 columns of H are zero
*/
for (i = 0; i < 3; i++) {
for (j = 0; j < BAFL_SSIZE; j++) {
bafl_tempS[i][j] = bafl_H[i][0] * bafl_Pprio[0][j];
bafl_tempS[i][j] += bafl_H[i][1] * bafl_Pprio[1][j];
}
}
/* S(3x3) = temp_S(3x6) * HT(6x3) + R(3x3)
*
* bottom 4 rows of HT are zero
*/
for (i = 0; i < 3; i++) {
for (j = 0; j < 3; j++) {
bafl_S[i][j] = bafl_tempS[i][0] * bafl_H[j][0]; /* H[j][0] = HT[0][j] */
bafl_S[i][j] += bafl_tempS[i][1] * bafl_H[j][1]; /* H[j][0] = HT[0][j] */
}
bafl_S[i][i] += bafl_R_accel;
}
/* invert S
*/
FLOAT_MAT33_INVERT(bafl_invS, bafl_S);
/* temp_K(6x3) = P_prio(6x6) * HT(6x3)
*
* bottom 4 rows of HT are zero
*/
for (i = 0; i < BAFL_SSIZE; i++) {
for (j = 0; j < 3; j++) {
/* not computing zero elements */
bafl_tempK[i][j] = bafl_Pprio[i][0] * bafl_H[j][0]; /* H[j][0] = HT[0][j] */
bafl_tempK[i][j] += bafl_Pprio[i][1] * bafl_H[j][1]; /* H[j][1] = HT[1][j] */
}
}
/* K(6x3) = temp_K(6x3) * invS(3x3)
*/
for (i = 0; i < BAFL_SSIZE; i++) {
for (j = 0; j < 3; j++) {
bafl_K[i][j] = bafl_tempK[i][0] * bafl_invS[0][j];
bafl_K[i][j] += bafl_tempK[i][1] * bafl_invS[1][j];
bafl_K[i][j] += bafl_tempK[i][2] * bafl_invS[2][j];
}
}
/**********************************************
* Update filter state.
*
* The a priori filter state is zero since the errors are corrected after each update.
*
* X = X_prio + K (y - H * X_prio)
* X = K * y
**********************************************/
/*printf("R = ");
for (i = 0; i < 3; i++) {
printf("[");
for (j = 0; j < 3; j++) {
printf("%f\t", RMAT_ELMT(bafl_dcm, i, j));
}
printf("]\n ");
}
printf("\n");*/
/* innovation
* y = Cnb * -[0; 0; g] - accel
*/
bafl_ya.x = -RMAT_ELMT(bafl_dcm, 0, 2) * BAFL_g - bafl_accel_measure.x;
bafl_ya.y = -RMAT_ELMT(bafl_dcm, 1, 2) * BAFL_g - bafl_accel_measure.y;
bafl_ya.z = -RMAT_ELMT(bafl_dcm, 2, 2) * BAFL_g - bafl_accel_measure.z;
/* X(6) = K(6x3) * y(3)
*/
for (i = 0; i < BAFL_SSIZE; i++) {
bafl_X[i] = bafl_K[i][0] * bafl_ya.x;
bafl_X[i] += bafl_K[i][1] * bafl_ya.y;
bafl_X[i] += bafl_K[i][2] * bafl_ya.z;
}
/**********************************************
* Update the filter covariance.
*
* P = P_prio - K * H * P_prio
* P = ( I - K * H ) * P_prio
*
**********************************************/
/* temp(6x6) = I(6x6) - K(6x3)*H(3x6)
*
* last 4 columns of H are zero
*/
for (i = 0; i < BAFL_SSIZE; i++) {
for (j = 0; j < BAFL_SSIZE; j++) {
if (i == j) {
bafl_tempP[i][j] = 1.;
} else {
bafl_tempP[i][j] = 0.;
}
if (j < 2) { /* omit the parts where H is zero */
for (k = 0; k < 3; k++) {
bafl_tempP[i][j] -= bafl_K[i][k] * bafl_H[k][j];
}
}
}
}
/* P(6x6) = temp(6x6) * P_prio(6x6)
*/
for (i = 0; i < BAFL_SSIZE; i++) {
for (j = 0; j < BAFL_SSIZE; j++) {
bafl_P[i][j] = bafl_tempP[i][0] * bafl_Pprio[0][j];
for (k = 1; k < BAFL_SSIZE; k++) {
bafl_P[i][j] += bafl_tempP[i][k] * bafl_Pprio[k][j];
}
}
}
#ifdef LKF_PRINT_P
printf("Pua=");
for (i = 0; i < 6; i++) {
printf("[");
for (j = 0; j < 6; j++) {
printf("%f\t", bafl_P[i][j]);
}
printf("]\n ");
}
printf("\n");
#endif
/**********************************************
* Correct errors.
*
*
**********************************************/
/* Error quaternion.
*/
QUAT_ASSIGN(bafl_q_a_err, 1.0, bafl_X[0]/2, bafl_X[1]/2, bafl_X[2]/2);
FLOAT_QUAT_INVERT(bafl_q_a_err, bafl_q_a_err);
/* normalize
* Is this needed? Or is the approximation good enough?*/
float q_sq;
q_sq = bafl_q_a_err.qx * bafl_q_a_err.qx + bafl_q_a_err.qy * bafl_q_a_err.qy + bafl_q_a_err.qz * bafl_q_a_err.qz;
if (q_sq > 1) { /* this should actually never happen */
FLOAT_QUAT_SMUL(bafl_q_a_err, bafl_q_a_err, 1 / sqrtf(1 + q_sq));
printf("Accel error quaternion q_sq > 1!!\n");
} else {
bafl_q_a_err.qi = sqrtf(1 - q_sq);
}
/* correct attitude
*/
FLOAT_QUAT_COMP(bafl_qtemp, bafl_q_a_err, bafl_quat);
FLOAT_QUAT_COPY(bafl_quat, bafl_qtemp);
/* correct gyro bias
*/
RATES_ASSIGN(bafl_b_a_err, bafl_X[3], bafl_X[4], bafl_X[5]);
RATES_SUB(bafl_bias, bafl_b_a_err);
/*
* compute all representations
*/
/* maintain rotation matrix representation */
FLOAT_RMAT_OF_QUAT(bafl_dcm, bafl_quat);
/* maintain euler representation */
FLOAT_EULERS_OF_RMAT(bafl_eulers, bafl_dcm);
AHRS_TO_BFP();
AHRS_LTP_TO_BODY();
}
/*
* Propagate our dynamic system in time
*
* Run the strapdown computation and the predict step of the filter
*
*
* quat_dot = Wxq(pqr) * quat
* bias_dot = 0
*
* Wxq is the quaternion omega matrix:
*
* [ 0, -p, -q, -r ]
* 1/2 * [ p, 0, r, -q ]
* [ q, -r, 0, p ]
* [ r, q, -p, 0 ]
*
*/
void ahrs_propagate(void) {
int i, j, k;
ahrs_lowpass_accel();
/* compute unbiased rates */
RATES_FLOAT_OF_BFP(bafl_rates, imu.gyro);
RATES_SUB(bafl_rates, bafl_bias);
/* run strapdown computation
*
*/
/* multiplicative quaternion update */
/* compute correction quaternion */
QUAT_ASSIGN(bafl_qr, 1., bafl_rates.p * BAFL_DT / 2, bafl_rates.q * BAFL_DT / 2, bafl_rates.r * BAFL_DT / 2);
/* normalize it. maybe not necessary?? */
float q_sq;
q_sq = (bafl_qr.qx * bafl_qr.qx +bafl_qr.qy * bafl_qr.qy + bafl_qr.qz * bafl_qr.qz) / 4;
if (q_sq > 1) { /* this should actually never happen */
FLOAT_QUAT_SMUL(bafl_qr, bafl_qr, 1 / sqrtf(1 + q_sq));
} else {
bafl_qr.qi = sqrtf(1 - q_sq);
}
/* propagate correction quaternion */
FLOAT_QUAT_COMP(bafl_qtemp, bafl_quat, bafl_qr);
FLOAT_QUAT_COPY(bafl_quat, bafl_qtemp);
bafl_qnorm = FLOAT_QUAT_NORM(bafl_quat);
//TODO check if broot force normalization is good, use lagrange normalization?
FLOAT_QUAT_NORMALISE(bafl_quat);
/*
* compute all representations
*/
/* maintain rotation matrix representation */
FLOAT_RMAT_OF_QUAT(bafl_dcm, bafl_quat);
/* maintain euler representation */
FLOAT_EULERS_OF_RMAT(bafl_eulers, bafl_dcm);
AHRS_TO_BFP();
AHRS_LTP_TO_BODY();
/* error KF-Filter
* propagate only the error covariance matrix since error is corrected after each measurement
*
* F = [ 0 0 0 ]
* [ 0 0 0 -Cbn ]
* [ 0 0 0 ]
* [ 0 0 0 0 0 0 ]
* [ 0 0 0 0 0 0 ]
* [ 0 0 0 0 0 0 ]
*
* T = e^(dt * F)
*
* T = [ 1 0 0 ]
* [ 0 1 0 -Cbn*dt ]
* [ 0 0 1 ]
* [ 0 0 0 1 0 0 ]
* [ 0 0 0 0 1 0 ]
* [ 0 0 0 0 0 1 ]
*
* P_prio = T * P * T_T + Q
*
*/
/*
* compute state transition matrix T
*
* diagonal elements of T are always one
*/
for (i = 0; i < 3; i++) {
for (j = 0; j < 3; j++) {
bafl_T[i][j + 3] = -RMAT_ELMT(bafl_dcm, j, i); /* inverted bafl_dcm */
}
}
/*
* estimate the a priori error covariance matrix P_k = T * P_k-1 * T_T + Q
*/
/* Temporary multiplication temp(6x6) = T(6x6) * P(6x6) */
for (i = 0; i < BAFL_SSIZE; i++) {
for (j = 0; j < BAFL_SSIZE; j++) {
bafl_tempP[i][j] = 0.;
for (k = 0; k < BAFL_SSIZE; k++) {
bafl_tempP[i][j] += bafl_T[i][k] * bafl_P[k][j];
}
}
}
/* P_k(6x6) = temp(6x6) * T_T(6x6) + Q */
for (i = 0; i < BAFL_SSIZE; i++) {
for (j = 0; j < BAFL_SSIZE; j++) {
bafl_P[i][j] = 0.;
for (k = 0; k < BAFL_SSIZE; k++) {
bafl_P[i][j] += bafl_tempP[i][k] * bafl_T[j][k]; //T[j][k] = T_T[k][j]
}
}
if (i<3) {
bafl_P[i][i] += bafl_Q_att;
} else {
bafl_P[i][i] += bafl_Q_gyro;
}
}
#ifdef LKF_PRINT_P
printf("Pp =");
for (i = 0; i < 6; i++) {
printf("[");
for (j = 0; j < 6; j++) {
printf("%f\t", bafl_P[i][j]);
}
printf("]\n ");
}
printf("\n");
#endif
}
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 = FLOAT_VECT3_DOT_PRODUCT(psi_vect, thrust_vect);
struct FloatVect3 dotn;
FLOAT_VECT3_SMUL(dotn, thrust_vect, dot);
// b = v - dot(v,n)*n
struct FloatVect3 b;
FLOAT_VECT3_DIFF(b, psi_vect, dotn);
dot = FLOAT_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 = FLOAT_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);
}
void stabilization_attitude_run(bool_t enable_integrator) {
/*
* Update reference
*/
stabilization_attitude_ref_update();
/*
* Compute errors for feedback
*/
/* attitude error */
struct FloatQuat att_err;
struct FloatQuat* att_quat = stateGetNedToBodyQuat_f();
FLOAT_QUAT_INV_COMP(att_err, *att_quat, stab_att_ref_quat);
/* wrap it in the shortest direction */
FLOAT_QUAT_WRAP_SHORTEST(att_err);
/* rate error */
struct FloatRates rate_err;
struct FloatRates* body_rate = stateGetBodyRates_f();
RATES_DIFF(rate_err, stab_att_ref_rate, *body_rate);
/* rate_d error */
struct FloatRates body_rate_d;
RATES_DIFF(body_rate_d, *body_rate, last_body_rate);
RATES_COPY(last_body_rate, *body_rate);
/* integrated error */
if (enable_integrator) {
struct FloatQuat new_sum_err, scaled_att_err;
/* update accumulator */
scaled_att_err.qi = att_err.qi;
scaled_att_err.qx = att_err.qx / IERROR_SCALE;
scaled_att_err.qy = att_err.qy / IERROR_SCALE;
scaled_att_err.qz = att_err.qz / IERROR_SCALE;
FLOAT_QUAT_COMP(new_sum_err, stabilization_att_sum_err_quat, scaled_att_err);
FLOAT_QUAT_NORMALIZE(new_sum_err);
FLOAT_QUAT_COPY(stabilization_att_sum_err_quat, new_sum_err);
FLOAT_EULERS_OF_QUAT(stabilization_att_sum_err_eulers, stabilization_att_sum_err_quat);
} else {
/* reset accumulator */
FLOAT_QUAT_ZERO( stabilization_att_sum_err_quat );
FLOAT_EULERS_ZERO( stabilization_att_sum_err_eulers );
}
attitude_run_ff(stabilization_att_ff_cmd, &stabilization_gains[gain_idx], &stab_att_ref_accel);
attitude_run_fb(stabilization_att_fb_cmd, &stabilization_gains[gain_idx], &att_err, &rate_err, &body_rate_d, &stabilization_att_sum_err_quat);
stabilization_cmd[COMMAND_ROLL] = stabilization_att_fb_cmd[COMMAND_ROLL] + stabilization_att_ff_cmd[COMMAND_ROLL];
stabilization_cmd[COMMAND_PITCH] = stabilization_att_fb_cmd[COMMAND_PITCH] + stabilization_att_ff_cmd[COMMAND_PITCH];
stabilization_cmd[COMMAND_YAW] = stabilization_att_fb_cmd[COMMAND_YAW] + stabilization_att_ff_cmd[COMMAND_YAW];
#ifdef HAS_SURFACE_COMMANDS
stabilization_cmd[COMMAND_ROLL_SURFACE] = stabilization_att_fb_cmd[COMMAND_ROLL_SURFACE] + stabilization_att_ff_cmd[COMMAND_ROLL_SURFACE];
stabilization_cmd[COMMAND_PITCH_SURFACE] = stabilization_att_fb_cmd[COMMAND_PITCH_SURFACE] + stabilization_att_ff_cmd[COMMAND_PITCH_SURFACE];
stabilization_cmd[COMMAND_YAW_SURFACE] = stabilization_att_fb_cmd[COMMAND_YAW_SURFACE] + stabilization_att_ff_cmd[COMMAND_YAW_SURFACE];
#endif
/* bound the result */
BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ);
BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ);
BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ);
}
