static void test_10(void) { struct FloatEulers euler; EULERS_ASSIGN(euler , RadOfDeg(0.), RadOfDeg(10.), RadOfDeg(0.)); DISPLAY_FLOAT_EULERS_DEG("euler", euler); struct FloatQuat quat; FLOAT_QUAT_OF_EULERS(quat, euler); DISPLAY_FLOAT_QUAT("####quat", quat); struct Int32Eulers euleri; EULERS_BFP_OF_REAL(euleri, euler); DISPLAY_INT32_EULERS("euleri", euleri); struct Int32Quat quati; INT32_QUAT_OF_EULERS(quati, euleri); DISPLAY_INT32_QUAT("####quat", quati); struct Int32RMat rmati; INT32_RMAT_OF_EULERS(rmati, euleri); DISPLAY_INT32_RMAT("####rmat", rmati); struct Int32Quat quat_ltp_to_body; struct Int32Quat body_to_imu_quat; INT32_QUAT_ZERO( body_to_imu_quat); INT32_QUAT_COMP_INV(quat_ltp_to_body, body_to_imu_quat, quati); DISPLAY_INT32_QUAT("####quat_ltp_to_body", quat_ltp_to_body); }
__attribute__ ((always_inline)) static inline void compute_body_orientation(void) { /* Compute LTP to BODY quaternion */ INT32_QUAT_COMP_INV(ahrs.ltp_to_body_quat, ahrs.ltp_to_imu_quat, imu.body_to_imu_quat); /* Compute LTP to BODY rotation matrix */ INT32_RMAT_COMP_INV(ahrs.ltp_to_body_rmat, ahrs.ltp_to_imu_rmat, imu.body_to_imu_rmat); /* compute LTP to BODY eulers */ INT32_EULERS_OF_RMAT(ahrs.ltp_to_body_euler, ahrs.ltp_to_body_rmat); /* compute body rates */ INT32_RMAT_TRANSP_RATEMULT(ahrs.body_rate, imu.body_to_imu_rmat, ahrs.imu_rate); }
void stabilization_attitude_run(bool_t enable_integrator) { /* * Update reference */ stabilization_attitude_ref_update(); /* * Compute errors for feedback */ /* attitude error */ struct Int32Quat att_err; INT32_QUAT_INV_COMP(att_err, ahrs.ltp_to_body_quat, stab_att_ref_quat); /* wrap it in the shortest direction */ INT32_QUAT_WRAP_SHORTEST(att_err); INT32_QUAT_NORMALIZE(att_err); /* rate error */ struct Int32Rates rate_err; RATES_DIFF(rate_err, ahrs.body_rate, stab_att_ref_rate); /* integrated error */ if (enable_integrator) { struct Int32Quat 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; INT32_QUAT_COMP_INV(new_sum_err, stabilization_att_sum_err_quat, scaled_att_err); INT32_QUAT_NORMALIZE(new_sum_err); QUAT_COPY(stabilization_att_sum_err_quat, new_sum_err); INT32_EULERS_OF_QUAT(stabilization_att_sum_err, stabilization_att_sum_err_quat); } else { /* reset accumulator */ INT32_QUAT_ZERO( stabilization_att_sum_err_quat ); INT_EULERS_ZERO( stabilization_att_sum_err ); } attitude_run_ff(stabilization_att_ff_cmd, current_stabilization_gains, &stab_att_ref_accel); attitude_run_fb(stabilization_att_fb_cmd, current_stabilization_gains, &att_err, &rate_err, &stabilization_att_sum_err_quat); for (int i = COMMAND_ROLL; i <= COMMAND_YAW; i++) { stabilization_cmd[i] = stabilization_att_fb_cmd[i]+stabilization_att_ff_cmd[i]; Bound(stabilization_cmd[i], -200, 200); } }
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 test_3(void) { /* Compute BODY to IMU eulers */ struct Int32Eulers b2i_e; EULERS_ASSIGN(b2i_e, ANGLE_BFP_OF_REAL(RadOfDeg(10.66)), ANGLE_BFP_OF_REAL(RadOfDeg(-0.7)), ANGLE_BFP_OF_REAL(RadOfDeg(0.))); DISPLAY_INT32_EULERS_AS_FLOAT_DEG("b2i_e", b2i_e); /* Compute BODY to IMU quaternion */ struct Int32Quat b2i_q; INT32_QUAT_OF_EULERS(b2i_q, b2i_e); DISPLAY_INT32_QUAT_AS_EULERS_DEG("b2i_q", b2i_q); // INT32_QUAT_NORMALIZE(b2i_q); // DISPLAY_INT32_QUAT_AS_EULERS_DEG("b2i_q_n", b2i_q); /* Compute BODY to IMU rotation matrix */ struct Int32RMat b2i_r; INT32_RMAT_OF_EULERS(b2i_r, b2i_e); // DISPLAY_INT32_RMAT("b2i_r", b2i_r); DISPLAY_INT32_RMAT_AS_EULERS_DEG("b2i_r", b2i_r); /* Compute LTP to IMU eulers */ struct Int32Eulers l2i_e; EULERS_ASSIGN(l2i_e, ANGLE_BFP_OF_REAL(RadOfDeg(0.)), ANGLE_BFP_OF_REAL(RadOfDeg(20.)), ANGLE_BFP_OF_REAL(RadOfDeg(0.))); DISPLAY_INT32_EULERS_AS_FLOAT_DEG("l2i_e", l2i_e); /* Compute LTP to IMU quaternion */ struct Int32Quat l2i_q; INT32_QUAT_OF_EULERS(l2i_q, l2i_e); DISPLAY_INT32_QUAT_AS_EULERS_DEG("l2i_q", l2i_q); /* Compute LTP to IMU rotation matrix */ struct Int32RMat l2i_r; INT32_RMAT_OF_EULERS(l2i_r, l2i_e); // DISPLAY_INT32_RMAT("l2i_r", l2i_r); DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2i_r", l2i_r); /* again but from quaternion */ struct Int32RMat l2i_r2; INT32_RMAT_OF_QUAT(l2i_r2, l2i_q); // DISPLAY_INT32_RMAT("l2i_r2", l2i_r2); DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2i_r2", l2i_r2); /* Compute LTP to BODY quaternion */ struct Int32Quat l2b_q; INT32_QUAT_COMP_INV(l2b_q, b2i_q, l2i_q); DISPLAY_INT32_QUAT_AS_EULERS_DEG("l2b_q", l2b_q); /* Compute LTP to BODY rotation matrix */ struct Int32RMat l2b_r; INT32_RMAT_COMP_INV(l2b_r, l2i_r, b2i_r); // DISPLAY_INT32_RMAT("l2b_r", l2b_r); DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2b_r2", l2b_r); /* again but from quaternion */ struct Int32RMat l2b_r2; INT32_RMAT_OF_QUAT(l2b_r2, l2b_q); // DISPLAY_INT32_RMAT("l2b_r2", l2b_r2); DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2b_r2", l2b_r2); /* compute LTP to BODY eulers */ struct Int32Eulers l2b_e; INT32_EULERS_OF_RMAT(l2b_e, l2b_r); DISPLAY_INT32_EULERS_AS_FLOAT_DEG("l2b_e", l2b_e); /* again but from quaternion */ struct Int32Eulers l2b_e2; INT32_EULERS_OF_QUAT(l2b_e2, l2b_q); DISPLAY_INT32_EULERS_AS_FLOAT_DEG("l2b_e2", l2b_e2); }