From 56457d080107948517157e5e82918a6d56e9af56 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 1 Aug 2014 20:49:17 +0200 Subject: [PATCH] [airborne] use math functions instead of macros sed to the rescue: find sw -path sw/airborne/math -prune -o -name "*.[ch]" -exec sed -i 's/\([ ]*[A-Z0-9_]*QUAT_OF[A-Z0-9_]*(\)\([a-zA-Z0-9_]*,[ ]*\)\([a-zA-Z0-9_]*)\)/\L\1\E\&\2\&\3/g' {} + and so on... --- .../fixedwing/guidance/energy_ctrl.c | 2 +- .../rotorcraft/guidance/guidance_v.c | 2 +- sw/airborne/firmwares/rotorcraft/navigation.c | 2 +- .../stabilization_attitude_quat_float.c | 12 +- .../stabilization_attitude_quat_int.c | 10 +- ...bilization_attitude_quat_transformations.c | 12 +- .../stabilization_attitude_rc_setpoint.c | 32 ++-- .../stabilization_attitude_ref_quat_float.c | 20 +-- .../stabilization_attitude_ref_quat_int.c | 14 +- .../modules/cam_control/rotorcraft_cam.c | 4 +- sw/airborne/modules/ins/ins_vn100.c | 14 +- sw/airborne/modules/nav/nav_catapult.c | 2 +- sw/airborne/state.c | 8 +- sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c | 42 ++--- sw/airborne/subsystems/ahrs/ahrs_float_dcm.c | 10 +- sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c | 12 +- .../subsystems/ahrs/ahrs_float_utils.h | 8 +- sw/airborne/subsystems/ahrs/ahrs_gx3.c | 4 +- .../subsystems/ahrs/ahrs_int_cmpl_euler.c | 14 +- .../subsystems/ahrs/ahrs_int_cmpl_quat.c | 48 ++--- sw/airborne/subsystems/imu.c | 1 - sw/airborne/subsystems/ins/hf_float.c | 4 +- .../subsystems/ins/ins_float_invariant.c | 6 +- sw/airborne/subsystems/ins/ins_int.c | 4 +- sw/airborne/test/ahrs/run_ahrs_on_synth.c | 2 +- sw/airborne/test/pprz_algebra_print.h | 8 +- sw/airborne/test/test_algebra.c | 164 +++++++++--------- sw/airborne/test/test_bla.c | 12 +- sw/ground_segment/misc/natnet2ivy.c | 2 +- 29 files changed, 237 insertions(+), 238 deletions(-) diff --git a/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c b/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c index 99c860b6fe7..6ac3d6fe443 100644 --- a/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c +++ b/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c @@ -347,7 +347,7 @@ void v_ctl_climb_loop( void ) #ifndef SITL struct Int32Vect3 accel_meas_body; struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu); - INT32_RMAT_TRANSP_VMULT(accel_meas_body, *body_to_imu_rmat, imu.accel); + int32_rmat_transp_vmult(&accel_meas_body, body_to_imu_rmat, &imu.accel); float vdot = ACCEL_FLOAT_OF_BFP(accel_meas_body.x) / 9.81f - sinf(stateGetNedToBodyEulers_f()->theta); #else float vdot = 0; diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c index c460b96dec5..861606e2902 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c @@ -340,7 +340,7 @@ static int32_t get_vertical_thrust_coeff(void) { struct Int32RMat* att = stateGetNedToBodyRMat_i(); /* thrust vector: - * INT32_RMAT_VMULT(thrust_vect, att, zaxis) + * int32_rmat_vmult(&thrust_vect, &att, &zaxis) * same as last colum of rmat with INT32_TRIG_FRAC * struct Int32Vect thrust_vect = {att.m[2], att.m[5], att.m[8]}; * diff --git a/sw/airborne/firmwares/rotorcraft/navigation.c b/sw/airborne/firmwares/rotorcraft/navigation.c index 706928e2259..e4da641faef 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.c +++ b/sw/airborne/firmwares/rotorcraft/navigation.c @@ -227,7 +227,7 @@ void nav_circle(struct EnuCoor_i * wp_center, int32_t radius) { // store last qdr int32_t last_qdr = nav_circle_qdr; // compute qdr - INT32_ATAN2(nav_circle_qdr, pos_diff.y, pos_diff.x); + nav_circle_qdr = int32_atan2(pos_diff.y, pos_diff.x); // increment circle radians if (nav_circle_radians != 0) { int32_t angle_diff = nav_circle_qdr - last_qdr; diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c index c4fa5987c34..e1c445d675a 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c @@ -293,9 +293,9 @@ void stabilization_attitude_run(bool_t enable_integrator) { /* attitude error */ struct FloatQuat att_err; struct FloatQuat* att_quat = stateGetNedToBodyQuat_f(); - FLOAT_QUAT_INV_COMP(att_err, *att_quat, stab_att_ref_quat); + float_quat_inv_comp(&att_err, att_quat, &stab_att_ref_quat); /* wrap it in the shortest direction */ - FLOAT_QUAT_WRAP_SHORTEST(att_err); + float_quat_wrap_shortest(&att_err); /* rate error */ struct FloatRates rate_err; @@ -313,10 +313,10 @@ void stabilization_attitude_run(bool_t enable_integrator) { 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_comp(&new_sum_err, &stabilization_att_sum_err_quat, &scaled_att_err); + float_quat_normalize(&new_sum_err); QUAT_COPY(stabilization_att_sum_err_quat, new_sum_err); - FLOAT_EULERS_OF_QUAT(stabilization_att_sum_err, stabilization_att_sum_err_quat); + float_eulers_of_quat(&stabilization_att_sum_err, &stabilization_att_sum_err_quat); } else { /* reset accumulator */ float_quat_identity(&stabilization_att_sum_err_quat); @@ -346,5 +346,5 @@ void stabilization_attitude_run(bool_t enable_integrator) { void stabilization_attitude_read_rc(bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn) { stabilization_attitude_read_rc_setpoint_quat_f(&stab_att_sp_quat, in_flight, in_carefree, coordinated_turn); - //FLOAT_QUAT_WRAP_SHORTEST(stab_att_sp_quat); + //float_quat_wrap_shortest(&stab_att_sp_quat); } diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c index 09f5f1e8c10..e826cc21f9b 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c @@ -230,8 +230,8 @@ void stabilization_attitude_run(bool_t enable_integrator) { struct Int32Quat* att_quat = stateGetNedToBodyQuat_i(); INT32_QUAT_INV_COMP(att_err, *att_quat, stab_att_ref_quat); /* wrap it in the shortest direction */ - INT32_QUAT_WRAP_SHORTEST(att_err); - INT32_QUAT_NORMALIZE(att_err); + int32_quat_wrap_shortest(&att_err); + int32_quat_normalize(&att_err); /* rate error */ const struct Int32Rates rate_ref_scaled = { @@ -250,10 +250,10 @@ void stabilization_attitude_run(bool_t enable_integrator) { 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(new_sum_err, stabilization_att_sum_err_quat, scaled_att_err); - INT32_QUAT_NORMALIZE(new_sum_err); + int32_quat_comp(&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); + int32_eulers_of_quat(&stabilization_att_sum_err, &stabilization_att_sum_err_quat); } else { /* reset accumulator */ int32_quat_identity(&stabilization_att_sum_err_quat); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.c index abd507564ca..83d3032a6f8 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.c @@ -39,7 +39,7 @@ void quat_from_rpy_cmd_f(struct FloatQuat *quat, struct FloatEulers *rpy) { /* orientation vector describing simultaneous rotation of roll/pitch/yaw */ const struct FloatVect3 ov = {rpy->phi, rpy->theta, rpy->psi}; /* quaternion from that orientation vector */ - FLOAT_QUAT_OF_ORIENTATION_VECT(*quat, ov); + float_quat_of_orientation_vect(quat, &ov); } @@ -67,11 +67,11 @@ void quat_from_earth_cmd_f(struct FloatQuat *quat, struct FloatVect2 *cmd, float 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); + float_quat_of_orientation_vect(&q_rp, &ov); /* as rotation matrix */ struct FloatRMat R_rp; - FLOAT_RMAT_OF_QUAT(R_rp, q_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]); @@ -124,8 +124,8 @@ void quat_from_earth_cmd_f(struct FloatQuat *quat, struct FloatVect2 *cmd, float 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); + float_quat_comp(quat, &q_rp, &q_yaw); + float_quat_normalize(quat); + float_quat_wrap_shortest(quat); } diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c index 3fc8fe145e4..3500b65f905 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c @@ -298,7 +298,7 @@ void stabilization_attitude_read_rc_roll_pitch_quat_f(struct FloatQuat* q) { ov.z = 0.0; /* quaternion from that orientation vector */ - FLOAT_QUAT_OF_ORIENTATION_VECT(*q, ov); + float_quat_of_orientation_vect(q, &ov); } /** Read roll/pitch command from RC as quaternion. @@ -317,7 +317,7 @@ void stabilization_attitude_read_rc_roll_pitch_earth_quat_f(struct FloatQuat* q) float qy_pitch = sinf(pitch2); float qi_pitch = cosf(pitch2); - /* only multiply non-zero entries of FLOAT_QUAT_COMP(*q, q_roll, q_pitch) */ + /* only multiply non-zero entries of float_quat_comp(q, &q_roll, &q_pitch) */ q->qi = qi_roll * qi_pitch; q->qx = qx_roll * qi_pitch; q->qy = qi_roll * qy_pitch; @@ -351,33 +351,33 @@ void stabilization_attitude_read_rc_setpoint_quat_f(struct FloatQuat* q_sp, bool //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); + float_quat_of_axis_angle(&q_yaw, &zaxis, care_free_heading); } else { - FLOAT_QUAT_OF_AXIS_ANGLE(q_yaw, zaxis, stateGetNedToBodyEulers_f()->psi); + 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); + 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)); + 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); + 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); + 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); + float_quat_comp_norm_shortest(q_sp, &q_rp_sp, &q_yaw_diff); } else { QUAT_COPY(*q_sp, q_rp_sp); } @@ -403,21 +403,21 @@ void stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(struct FloatQuat 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)); + 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); + 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); + 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); + 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); + float_quat_comp(&q_rp_sp, &q_yaw, &q_rp_cmd); + float_quat_normalize(&q_rp_sp); QUAT_COPY(*q_sp, q_rp_sp); } diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c index 8413b074f26..17c93cd2995 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c @@ -69,9 +69,9 @@ static inline void reset_psi_ref_from_body(void) { static inline void update_ref_quat_from_eulers(void) { struct FloatRMat ref_rmat; - FLOAT_RMAT_OF_EULERS(ref_rmat, stab_att_ref_euler); - FLOAT_QUAT_OF_RMAT(stab_att_ref_quat, ref_rmat); - FLOAT_QUAT_WRAP_SHORTEST(stab_att_ref_quat); + float_rmat_of_eulers(&ref_rmat, &stab_att_ref_euler); + float_quat_of_rmat(&stab_att_ref_quat, &ref_rmat); + float_quat_wrap_shortest(&stab_att_ref_quat); } void stabilization_attitude_ref_idx_set_omega_p(uint8_t idx, float omega) { @@ -143,18 +143,18 @@ 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); + 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); + 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, stab_att_ref_quat, delta_q); + float_quat_comp(&new_ref_quat, &stab_att_ref_quat, &delta_q); QUAT_COPY(stab_att_ref_quat, new_ref_quat); #endif - FLOAT_QUAT_NORMALIZE(stab_att_ref_quat); + float_quat_normalize(&stab_att_ref_quat); /* integrate reference rotational speeds */ struct FloatRates delta_rate; @@ -164,9 +164,9 @@ void stabilization_attitude_ref_update(void) { /* compute reference angular accelerations */ struct FloatQuat err; /* compute reference attitude error */ - FLOAT_QUAT_INV_COMP(err, stab_att_sp_quat, stab_att_ref_quat); + float_quat_inv_comp(&err, &stab_att_sp_quat, &stab_att_ref_quat); /* wrap it in the shortest direction */ - FLOAT_QUAT_WRAP_SHORTEST(err); + float_quat_wrap_shortest(&err); /* propagate the 2nd order linear model: xdotdot = -2*zeta*omega*xdot - omega^2*x */ /* since error quaternion contains the half-angles we get 2*omega^2*err */ 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 @@ -185,5 +185,5 @@ void stabilization_attitude_ref_update(void) { SATURATE_SPEED_TRIM_ACCEL(); /* compute ref_euler */ - FLOAT_EULERS_OF_QUAT(stab_att_ref_euler, stab_att_ref_quat); + float_eulers_of_quat(&stab_att_ref_euler, &stab_att_ref_quat); } diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c index 00d9da774c3..cb6b07a9a63 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c @@ -149,8 +149,8 @@ void stabilization_attitude_ref_enter(void) /* convert reference attitude with REF_ANGLE_FRAC to eulers with normal INT32_ANGLE_FRAC */ struct Int32Eulers ref_eul; INT32_EULERS_RSHIFT(ref_eul, stab_att_ref_euler, (REF_ANGLE_FRAC - INT32_ANGLE_FRAC)); - INT32_QUAT_OF_EULERS(stab_att_ref_quat, ref_eul); - INT32_QUAT_WRAP_SHORTEST(stab_att_ref_quat); + int32_quat_of_eulers(&stab_att_ref_quat, &ref_eul); + int32_quat_wrap_shortest(&stab_att_ref_quat); /* set reference rate and acceleration to zero */ memset(&stab_att_ref_accel, 0, sizeof(struct Int32Rates)); @@ -176,14 +176,14 @@ void stabilization_attitude_ref_update(void) { OFFSET_AND_ROUND(stab_att_ref_rate.q, (REF_RATE_FRAC - INT32_RATE_FRAC)), OFFSET_AND_ROUND(stab_att_ref_rate.r, (REF_RATE_FRAC - INT32_RATE_FRAC)) }; struct Int32Quat qdot; - INT32_QUAT_DERIVATIVE(qdot, rate_ref_scaled, stab_att_ref_quat); + int32_quat_derivative(&qdot, &rate_ref_scaled, &stab_att_ref_quat); //QUAT_SMUL(qdot, qdot, DT_UPDATE); qdot.qi = qdot.qi >> F_UPDATE_RES; qdot.qx = qdot.qx >> F_UPDATE_RES; qdot.qy = qdot.qy >> F_UPDATE_RES; qdot.qz = qdot.qz >> F_UPDATE_RES; QUAT_ADD(stab_att_ref_quat, qdot); - INT32_QUAT_NORMALIZE(stab_att_ref_quat); + int32_quat_normalize(&stab_att_ref_quat); /* integrate reference rotational speeds * delta rate = ref_accel * dt @@ -198,9 +198,9 @@ void stabilization_attitude_ref_update(void) { /* compute reference angular accelerations */ struct Int32Quat err; /* compute reference attitude error */ - INT32_QUAT_INV_COMP(err, stab_att_sp_quat, stab_att_ref_quat); + int32_quat_inv_comp(&err, &stab_att_sp_quat, &stab_att_ref_quat); /* wrap it in the shortest direction */ - INT32_QUAT_WRAP_SHORTEST(err); + int32_quat_wrap_shortest(&err); /* propagate the 2nd order linear model : accel = -2*zeta*omega * rate - omega^2 * angle */ @@ -229,6 +229,6 @@ void stabilization_attitude_ref_update(void) { /* compute ref_euler for debugging and telemetry */ struct Int32Eulers ref_eul; - INT32_EULERS_OF_QUAT(ref_eul, stab_att_ref_quat); + int32_eulers_of_quat(&ref_eul, &stab_att_ref_quat); INT32_EULERS_LSHIFT(stab_att_ref_euler, ref_eul, (REF_ANGLE_FRAC - INT32_ANGLE_FRAC)); } diff --git a/sw/airborne/modules/cam_control/rotorcraft_cam.c b/sw/airborne/modules/cam_control/rotorcraft_cam.c index 41102128520..7adb4e491f1 100644 --- a/sw/airborne/modules/cam_control/rotorcraft_cam.c +++ b/sw/airborne/modules/cam_control/rotorcraft_cam.c @@ -152,13 +152,13 @@ void rotorcraft_cam_periodic(void) { struct Int32Vect2 diff; VECT2_DIFF(diff, waypoints[ROTORCRAFT_CAM_TRACK_WP], *stateGetPositionEnu_i()); INT32_VECT2_RSHIFT(diff,diff,INT32_POS_FRAC); - INT32_ATAN2(rotorcraft_cam_pan,diff.x,diff.y); + rotorcraft_cam_pan = int32_atan2(diff.x, diff.y); nav_heading = rotorcraft_cam_pan; #if ROTORCRAFT_CAM_USE_TILT_ANGLES int32_t dist, height; INT32_VECT2_NORM(dist, diff); height = (waypoints[ROTORCRAFT_CAM_TRACK_WP].z - stateGetPositionEnu_i()->z) >> INT32_POS_FRAC; - INT32_ATAN2(rotorcraft_cam_tilt, height, dist); + rotorcraft_cam_tilt = int32_atan2(height, dist); Bound(rotorcraft_cam_tilt, CAM_TA_MIN, CAM_TA_MAX); rotorcraft_cam_tilt_pwm = ROTORCRAFT_CAM_TILT_MIN + D_TILT * (rotorcraft_cam_tilt - CAM_TA_MIN) / (CAM_TA_MAX - CAM_TA_MIN); #endif diff --git a/sw/airborne/modules/ins/ins_vn100.c b/sw/airborne/modules/ins/ins_vn100.c index 7b1f2b092b8..001f4e759e7 100644 --- a/sw/airborne/modules/ins/ins_vn100.c +++ b/sw/airborne/modules/ins/ins_vn100.c @@ -205,14 +205,14 @@ static inline void parse_ins_msg( void ) { ins_quat.qx = last_received_packet.Data[1].Float; ins_quat.qy = last_received_packet.Data[2].Float; ins_quat.qz = last_received_packet.Data[3].Float; - FLOAT_EULERS_OF_QUAT(ins_eulers, ins_quat); + float_eulers_of_quat(&ins_eulers, &ins_quat); break; case VN100_REG_QTM : ins_quat.qi = last_received_packet.Data[0].Float; ins_quat.qx = last_received_packet.Data[1].Float; ins_quat.qy = last_received_packet.Data[2].Float; ins_quat.qz = last_received_packet.Data[3].Float; - FLOAT_EULERS_OF_QUAT(ins_eulers, ins_quat); + float_eulers_of_quat(&ins_eulers, &ins_quat); ins_mag.x = last_received_packet.Data[4].Float; ins_mag.y = last_received_packet.Data[5].Float; ins_mag.z = last_received_packet.Data[6].Float; @@ -222,7 +222,7 @@ static inline void parse_ins_msg( void ) { ins_quat.qx = last_received_packet.Data[1].Float; ins_quat.qy = last_received_packet.Data[2].Float; ins_quat.qz = last_received_packet.Data[3].Float; - FLOAT_EULERS_OF_QUAT(ins_eulers, ins_quat); + float_eulers_of_quat(&ins_eulers, &ins_quat); ins_accel.x = last_received_packet.Data[4].Float; ins_accel.y = last_received_packet.Data[5].Float; ins_accel.z = last_received_packet.Data[6].Float; @@ -232,7 +232,7 @@ static inline void parse_ins_msg( void ) { ins_quat.qx = last_received_packet.Data[1].Float; ins_quat.qy = last_received_packet.Data[2].Float; ins_quat.qz = last_received_packet.Data[3].Float; - FLOAT_EULERS_OF_QUAT(ins_eulers, ins_quat); + float_eulers_of_quat(&ins_eulers, &ins_quat); ins_rates.p = last_received_packet.Data[4].Float; ins_rates.q = last_received_packet.Data[5].Float; ins_rates.r = last_received_packet.Data[6].Float; @@ -242,7 +242,7 @@ static inline void parse_ins_msg( void ) { ins_quat.qx = last_received_packet.Data[1].Float; ins_quat.qy = last_received_packet.Data[2].Float; ins_quat.qz = last_received_packet.Data[3].Float; - FLOAT_EULERS_OF_QUAT(ins_eulers, ins_quat); + float_eulers_of_quat(&ins_eulers, &ins_quat); ins_mag.x = last_received_packet.Data[4].Float; ins_mag.y = last_received_packet.Data[5].Float; ins_mag.z = last_received_packet.Data[6].Float; @@ -255,7 +255,7 @@ static inline void parse_ins_msg( void ) { ins_quat.qx = last_received_packet.Data[1].Float; ins_quat.qy = last_received_packet.Data[2].Float; ins_quat.qz = last_received_packet.Data[3].Float; - FLOAT_EULERS_OF_QUAT(ins_eulers, ins_quat); + float_eulers_of_quat(&ins_eulers, &ins_quat); ins_accel.x = last_received_packet.Data[4].Float; ins_accel.y = last_received_packet.Data[5].Float; ins_accel.z = last_received_packet.Data[6].Float; @@ -268,7 +268,7 @@ static inline void parse_ins_msg( void ) { ins_quat.qx = last_received_packet.Data[1].Float; ins_quat.qy = last_received_packet.Data[2].Float; ins_quat.qz = last_received_packet.Data[3].Float; - FLOAT_EULERS_OF_QUAT(ins_eulers, ins_quat); + float_eulers_of_quat(&ins_eulers, &ins_quat); ins_mag.x = last_received_packet.Data[4].Float; ins_mag.y = last_received_packet.Data[5].Float; ins_mag.z = last_received_packet.Data[6].Float; diff --git a/sw/airborne/modules/nav/nav_catapult.c b/sw/airborne/modules/nav/nav_catapult.c index a147ca2c675..e29c88c8a47 100644 --- a/sw/airborne/modules/nav/nav_catapult.c +++ b/sw/airborne/modules/nav/nav_catapult.c @@ -110,7 +110,7 @@ void nav_catapult_highrate_module(void) #ifndef SITL struct Int32Vect3 accel_meas_body; struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu); - INT32_RMAT_TRANSP_VMULT(accel_meas_body, *body_to_imu_rmat, imu.accel); + int32_rmat_transp_vmult(&accel_meas_body, body_to_imu_rmat, &imu.accel); if (ACCEL_FLOAT_OF_BFP(accel_meas_body.x) < (nav_catapult_acceleration_threshold * 9.81)) #else if (launch != 1) diff --git a/sw/airborne/state.c b/sw/airborne/state.c index 6d4845a38c1..9a32002cb69 100644 --- a/sw/airborne/state.c +++ b/sw/airborne/state.c @@ -812,23 +812,23 @@ void stateCalcHorizontalSpeedDir_i(void) { state.h_speed_dir_i = SPEED_BFP_OF_REAL(state.h_speed_dir_f); } else if (bit_is_set(state.speed_status, SPEED_NED_I)) { - INT32_ATAN2(state.h_speed_dir_i, state.ned_speed_i.y, state.ned_speed_i.x); + state.h_speed_dir_i = int32_atan2(state.ned_speed_i.y, state.ned_speed_i.x); INT32_COURSE_NORMALIZE(state.h_speed_dir_i); } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) { - INT32_ATAN2(state.h_speed_dir_i, state.enu_speed_i.x, state.enu_speed_i.y); + state.h_speed_dir_i = int32_atan2(state.enu_speed_i.x, state.enu_speed_i.y); INT32_COURSE_NORMALIZE(state.h_speed_dir_i); } else if (bit_is_set(state.speed_status, SPEED_NED_F)) { SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f); SetBit(state.speed_status, SPEED_NED_I); - INT32_ATAN2(state.h_speed_dir_i, state.ned_speed_i.y, state.ned_speed_i.x); + state.h_speed_dir_i = int32_atan2(state.ned_speed_i.y, state.ned_speed_i.x); INT32_COURSE_NORMALIZE(state.h_speed_dir_i); } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) { SPEEDS_BFP_OF_REAL(state.enu_speed_i, state.enu_speed_f); SetBit(state.speed_status, SPEED_ENU_I); - INT32_ATAN2(state.h_speed_dir_i, state.enu_speed_i.x, state.enu_speed_i.y); + state.h_speed_dir_i = int32_atan2(state.enu_speed_i.x, state.enu_speed_i.y); INT32_COURSE_NORMALIZE(state.h_speed_dir_i); } /* set bit to indicate this representation is computed */ diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c index ff2e05f0274..9621a862c26 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c @@ -121,7 +121,7 @@ struct AhrsFloatCmpl ahrs_impl; static void send_att(void) { struct FloatEulers ltp_to_imu_euler; - FLOAT_EULERS_OF_QUAT(ltp_to_imu_euler, ahrs_impl.ltp_to_imu_quat); + float_eulers_of_quat(<p_to_imu_euler, &ahrs_impl.ltp_to_imu_quat); struct Int32Eulers euler_i; EULERS_BFP_OF_REAL(euler_i, ltp_to_imu_euler); struct Int32Eulers* eulers_body = stateGetNedToBodyEulers_i(); @@ -214,7 +214,7 @@ void ahrs_align(void) { #endif /* Convert initial orientation from quat to rotation matrix representations. */ - FLOAT_RMAT_OF_QUAT(ahrs_impl.ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat); + float_rmat_of_quat(&ahrs_impl.ltp_to_imu_rmat, &ahrs_impl.ltp_to_imu_quat); /* Compute initial body orientation */ compute_body_orientation_and_rates(); @@ -250,14 +250,14 @@ void ahrs_propagate(void) { const float dt = 1./AHRS_PROPAGATE_FREQUENCY; #if AHRS_PROPAGATE_RMAT - FLOAT_RMAT_INTEGRATE_FI(ahrs_impl.ltp_to_imu_rmat, omega, dt); + float_rmat_integrate_fi(&ahrs_impl.ltp_to_imu_rmat, &omega, dt); float_rmat_reorthogonalize(&ahrs_impl.ltp_to_imu_rmat); - FLOAT_QUAT_OF_RMAT(ahrs_impl.ltp_to_imu_quat, ahrs_impl.ltp_to_imu_rmat); + float_quat_of_rmat(&ahrs_impl.ltp_to_imu_quat, &ahrs_impl.ltp_to_imu_rmat); #endif #if AHRS_PROPAGATE_QUAT - FLOAT_QUAT_INTEGRATE(ahrs_impl.ltp_to_imu_quat, omega, dt); - FLOAT_QUAT_NORMALIZE(ahrs_impl.ltp_to_imu_quat); - FLOAT_RMAT_OF_QUAT(ahrs_impl.ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat); + float_quat_integrate(&ahrs_impl.ltp_to_imu_quat, &omega, dt); + float_quat_normalize(&ahrs_impl.ltp_to_imu_quat); + float_rmat_of_quat(&ahrs_impl.ltp_to_imu_rmat, &ahrs_impl.ltp_to_imu_quat); #endif compute_body_orientation_and_rates(); @@ -317,7 +317,7 @@ void ahrs_update_accel(void) { * <0.66G = 0, 1G = 1.0, >1.33G = 0 */ - const float g_meas_norm = FLOAT_VECT3_NORM(filtered_gravity_measurement) / 9.81; + const float g_meas_norm = float_vect3_norm(&filtered_gravity_measurement) / 9.81; ahrs_impl.weight = 1.0 - ahrs_impl.gravity_heuristic_factor * fabs(1.0 - g_meas_norm) / 10.0; Bound(ahrs_impl.weight, 0.15, 1.0); } @@ -390,7 +390,7 @@ void ahrs_update_mag_2d(void) { struct FloatVect2 expected_ltp; VECT2_COPY(expected_ltp, ahrs_impl.mag_h); // normalize expected ltp in 2D (x,y) - FLOAT_VECT2_NORMALIZE(expected_ltp); + float_vect2_normalize(&expected_ltp); struct FloatVect3 measured_imu; MAGS_FLOAT_OF_BFP(measured_imu, imu.mag); @@ -400,7 +400,7 @@ void ahrs_update_mag_2d(void) { struct FloatVect2 measured_ltp_2d={measured_ltp.x, measured_ltp.y}; // normalize measured ltp in 2D (x,y) - FLOAT_VECT2_NORMALIZE(measured_ltp_2d); + float_vect2_normalize(&measured_ltp_2d); const struct FloatVect3 residual_ltp = { 0, @@ -434,7 +434,7 @@ void ahrs_update_mag_2d_dumb(void) { /* project mag on local tangeant plane */ struct FloatEulers ltp_to_imu_euler; - FLOAT_EULERS_OF_RMAT(ltp_to_imu_euler, ahrs_impl.ltp_to_imu_rmat); + float_eulers_of_rmat(<p_to_imu_euler, &ahrs_impl.ltp_to_imu_rmat); struct FloatVect3 magf; MAGS_FLOAT_OF_BFP(magf, imu.mag); const float cphi = cosf(ltp_to_imu_euler.phi); @@ -542,20 +542,20 @@ void ahrs_realign_heading(float heading) { QUAT_COPY(q_h, *ltp_to_body_quat); q_h.qx = 0.; q_h.qy = 0.; - FLOAT_QUAT_NORMALIZE(q_h); + 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); + 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); + float_quat_comp_norm_shortest(&q, &q_c, ltp_to_body_quat); /* compute ltp to imu rotation quaternion and matrix */ struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&imu.body_to_imu); - FLOAT_QUAT_COMP(ahrs_impl.ltp_to_imu_quat, q, *body_to_imu_quat); - FLOAT_RMAT_OF_QUAT(ahrs_impl.ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat); + float_quat_comp(&ahrs_impl.ltp_to_imu_quat, &q, 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); @@ -571,14 +571,14 @@ 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(ltp_to_body_quat, ahrs_impl.ltp_to_imu_quat, *body_to_imu_quat); + float_quat_comp_inv(<p_to_body_quat, &ahrs_impl.ltp_to_imu_quat, 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); + float_quat_of_eulers(&neutrals_to_body_quat, &neutrals_to_body_eulers); + float_quat_normalize(&neutrals_to_body_quat); + float_quat_comp_inv(<p_to_neutrals_quat, <p_to_body_quat, &neutrals_to_body_quat); stateSetNedToBodyQuat_f(<p_to_neutrals_quat); #else stateSetNedToBodyQuat_f(<p_to_body_quat); @@ -587,6 +587,6 @@ static inline void compute_body_orientation_and_rates(void) { /* 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); + float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_impl.imu_rate); stateSetBodyRates_f(&body_rate); } diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c index f1a8b86f03d..28d8a2afdfb 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c @@ -147,7 +147,7 @@ void ahrs_align(void) /* Convert initial orientation in quaternion and rotation matrice representations. */ struct FloatRMat ltp_to_imu_rmat; - FLOAT_RMAT_OF_EULERS(ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_euler); + float_rmat_of_eulers(<p_to_imu_rmat, &ahrs_impl.ltp_to_imu_euler); /* set filter dcm */ set_dcm_matrix_from_rmat(<p_to_imu_rmat); @@ -531,16 +531,16 @@ static inline void set_body_orientation_and_rates(void) { struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&imu.body_to_imu); struct FloatRates body_rate; - FLOAT_RMAT_TRANSP_RATEMULT(body_rate, *body_to_imu_rmat, ahrs_impl.imu_rate); + float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_impl.imu_rate); stateSetBodyRates_f(&body_rate); struct FloatRMat ltp_to_imu_rmat, ltp_to_body_rmat; - FLOAT_RMAT_OF_EULERS(ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_euler); - FLOAT_RMAT_COMP_INV(ltp_to_body_rmat, ltp_to_imu_rmat, *body_to_imu_rmat); + float_rmat_of_eulers(<p_to_imu_rmat, &ahrs_impl.ltp_to_imu_euler); + float_rmat_comp_inv(<p_to_body_rmat, <p_to_imu_rmat, body_to_imu_rmat); // Some stupid lines of code for neutrals struct FloatEulers ltp_to_body_euler; - FLOAT_EULERS_OF_RMAT(ltp_to_body_euler, ltp_to_body_rmat); + float_eulers_of_rmat(<p_to_body_euler, <p_to_body_rmat); ltp_to_body_euler.phi -= ins_roll_neutral; ltp_to_body_euler.theta -= ins_pitch_neutral; stateSetNedToBodyEulers_f(<p_to_body_euler); diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c index 926c65c3170..8d0d42e8adf 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c @@ -133,7 +133,7 @@ void ahrs_update_accel(void) { ACCELS_FLOAT_OF_BFP(imu_g, imu.accel); const float alpha = 0.92; ahrs_impl.lp_accel = alpha * ahrs_impl.lp_accel + - (1. - alpha) *(FLOAT_VECT3_NORM(imu_g) - 9.81); + (1. - alpha) *(float_vect3_norm(&imu_g) - 9.81); const struct FloatVect3 earth_g = {0., 0., -9.81 }; const float dn = 250*fabs( ahrs_impl.lp_accel ); struct FloatVect3 g_noise = {1.+dn, 1.+dn, 1.+dn}; @@ -169,7 +169,7 @@ static inline void propagate_ref(void) { /* propagate reference quaternion */ const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY); - FLOAT_QUAT_INTEGRATE(ahrs_impl.ltp_to_imu_quat, ahrs_impl.imu_rate, dt); + float_quat_integrate(&ahrs_impl.ltp_to_imu_quat, &ahrs_impl.imu_rate, dt); } @@ -270,8 +270,8 @@ 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); + 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_identity(&ahrs_impl.gibbs_cor); @@ -286,13 +286,13 @@ 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, *body_to_imu_quat); + float_quat_comp_inv(<p_to_body_quat, &ahrs_impl.ltp_to_imu_quat, 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, *body_to_imu_rmat, ahrs_impl.imu_rate); + float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_impl.imu_rate); /* Set state */ stateSetBodyRates_f(&body_rate); diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_utils.h b/sw/airborne/subsystems/ahrs/ahrs_float_utils.h index 721ae83e4f8..04f837f6346 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_utils.h +++ b/sw/airborne/subsystems/ahrs/ahrs_float_utils.h @@ -80,7 +80,7 @@ static inline void ahrs_float_get_quat_from_accel(struct FloatQuat* q, struct In q->qy = acc_normalized.x; q->qz = 0.0; q->qi = 1.0 - acc_normalized.z; - FLOAT_QUAT_NORMALIZE(*q); + float_quat_normalize(q); } } @@ -97,7 +97,7 @@ static inline void ahrs_float_get_quat_from_accel_mag(struct FloatQuat* q, struc /* and rotate to horizontal plane using the quat from above */ struct FloatRMat rmat_phi_theta; - FLOAT_RMAT_OF_QUAT(rmat_phi_theta, q_a); + float_rmat_of_quat(&rmat_phi_theta, &q_a); struct FloatVect3 mag_ltp; float_rmat_transp_vmult(&mag_ltp, &rmat_phi_theta, &mag_float); @@ -120,12 +120,12 @@ static inline void ahrs_float_get_quat_from_accel_mag(struct FloatQuat* q, struc q_m.qy = 0.0; q_m.qz = mag_ltp.x * AHRS_H_Y - mag_ltp.y * AHRS_H_X; q_m.qi = norm2 + dot; - FLOAT_QUAT_NORMALIZE(q_m); + float_quat_normalize(&q_m); } // q_ltp2imu = q_a * q_m // and wrap and normalize - FLOAT_QUAT_COMP_NORM_SHORTEST(*q, q_m, q_a); + float_quat_comp_norm_shortest(q, &q_m, &q_a); } #endif /* AHRS_FLOAT_UTILS_H */ diff --git a/sw/airborne/subsystems/ahrs/ahrs_gx3.c b/sw/airborne/subsystems/ahrs/ahrs_gx3.c index 275e1dcec62..917d458f1ef 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_gx3.c +++ b/sw/airborne/subsystems/ahrs/ahrs_gx3.c @@ -268,7 +268,7 @@ void gx3_packet_read_message(void) { FLOAT_RMAT_COMP(ltp_to_body_rmat, ahrs_impl.gx3_rmat, *body_to_imu_rmat); #ifdef AHRS_UPDATE_FW_ESTIMATOR // fixedwing struct FloatEulers ltp_to_body_eulers; - FLOAT_EULERS_OF_RMAT(ltp_to_body_eulers, ltp_to_body_rmat); + float_eulers_of_rmat(<p_to_body_eulers, <p_to_body_rmat); ltp_to_body_eulers.phi -= ins_roll_neutral; ltp_to_body_eulers.theta -= ins_pitch_neutral; #if AHRS_USE_GPS_HEADING && USE_GPS @@ -282,7 +282,7 @@ void gx3_packet_read_message(void) { #else #ifdef IMU_MAG_OFFSET //rotorcraft struct FloatEulers ltp_to_body_eulers; - FLOAT_EULERS_OF_RMAT(ltp_to_body_eulers, ltp_to_body_rmat); + float_eulers_of_rmat(<p_to_body_eulers, <p_to_body_rmat); ltp_to_body_eulers.psi -= ahrs_impl.mag_offset; stateSetNedToBodyEulers_f(<p_to_body_eulers); #else diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c index b486ed31614..b3ded1f74d5 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c @@ -226,7 +226,7 @@ void ahrs_propagate(void) { /* integrate eulers */ struct Int32Eulers euler_dot; - INT32_EULERS_DOT_OF_RATES(euler_dot, ahrs_impl.ltp_to_imu_euler, ahrs_impl.imu_rate); + int32_eulers_dot_of_rates(&euler_dot, &ahrs_impl.ltp_to_imu_euler, &ahrs_impl.imu_rate); EULERS_ADD(ahrs_impl.hi_res_euler, euler_dot); /* low pass measurement */ @@ -282,11 +282,11 @@ void ahrs_update_mag(void) { /* measures phi and theta assuming no dynamic acceleration ?!! */ __attribute__ ((always_inline)) static inline void get_phi_theta_measurement_fom_accel(int32_t* phi_meas, int32_t* theta_meas, struct Int32Vect3 accel) { - INT32_ATAN2(*phi_meas, -accel.y, -accel.z); + *phi_meas = int32_atan2(-accel.y, -accel.z); int32_t cphi; PPRZ_ITRIG_COS(cphi, *phi_meas); int32_t cphi_ax = -INT_MULT_RSHIFT(cphi, accel.x, INT32_TRIG_FRAC); - INT32_ATAN2(*theta_meas, -cphi_ax, -accel.z); + *theta_meas = int32_atan2(-cphi_ax, -accel.z); *phi_meas *= F_UPDATE; *theta_meas *= F_UPDATE; @@ -325,13 +325,13 @@ static void set_body_state_from_euler(void) { struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu); struct Int32RMat ltp_to_imu_rmat, ltp_to_body_rmat; /* Compute LTP to IMU rotation matrix */ - INT32_RMAT_OF_EULERS(ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_euler); + int32_rmat_of_eulers(<p_to_imu_rmat, &ahrs_impl.ltp_to_imu_euler); /* Compute LTP to BODY rotation matrix */ - INT32_RMAT_COMP_INV(ltp_to_body_rmat, ltp_to_imu_rmat, *body_to_imu_rmat); + int32_rmat_comp_inv(<p_to_body_rmat, <p_to_imu_rmat, body_to_imu_rmat); /* Set state */ #ifdef AHRS_UPDATE_FW_ESTIMATOR struct Int32Eulers ltp_to_body_euler; - INT32_EULERS_OF_RMAT(ltp_to_body_euler, ltp_to_body_rmat); + int32_eulers_of_rmat(<p_to_body_euler, <p_to_body_rmat); ltp_to_body_euler.phi -= ANGLE_BFP_OF_REAL(ins_roll_neutral); ltp_to_body_euler.theta -= ANGLE_BFP_OF_REAL(ins_pitch_neutral); stateSetNedToBodyEulers_i(<p_to_body_euler); @@ -341,7 +341,7 @@ static void set_body_state_from_euler(void) { struct Int32Rates body_rate; /* compute body rates */ - INT32_RMAT_TRANSP_RATEMULT(body_rate, *body_to_imu_rmat, ahrs_impl.imu_rate); + int32_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_impl.imu_rate); /* Set state */ stateSetBodyRates_i(&body_rate); diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c index 6795886ca3b..cbf5d993f2a 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c @@ -164,7 +164,7 @@ static void send_quat(void) { static void send_euler(void) { struct Int32Eulers ltp_to_imu_euler; - INT32_EULERS_OF_QUAT(ltp_to_imu_euler, ahrs_impl.ltp_to_imu_quat); + int32_eulers_of_quat(<p_to_imu_euler, &ahrs_impl.ltp_to_imu_quat); struct Int32Eulers* eulers = stateGetNedToBodyEulers_i(); DOWNLINK_SEND_AHRS_EULER_INT(DefaultChannel, DefaultDevice, <p_to_imu_euler.phi, @@ -280,9 +280,9 @@ void ahrs_propagate(void) { INT_RATES_ZERO(ahrs_impl.rate_correction); /* integrate quaternion */ - INT32_QUAT_INTEGRATE_FI(ahrs_impl.ltp_to_imu_quat, ahrs_impl.high_rez_quat, - omega, AHRS_PROPAGATE_FREQUENCY); - INT32_QUAT_NORMALIZE(ahrs_impl.ltp_to_imu_quat); + int32_quat_integrate_fi(&ahrs_impl.ltp_to_imu_quat, &ahrs_impl.high_rez_quat, + &omega, AHRS_PROPAGATE_FREQUENCY); + int32_quat_normalize(&ahrs_impl.ltp_to_imu_quat); set_body_state_from_quat(); @@ -315,7 +315,7 @@ void ahrs_update_accel(void) { // c2 = ltp z-axis in imu-frame struct Int32RMat ltp_to_imu_rmat; - INT32_RMAT_OF_QUAT(ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat); + int32_rmat_of_quat(<p_to_imu_rmat, &ahrs_impl.ltp_to_imu_quat); struct Int32Vect3 c2 = { RMAT_ELMT(ltp_to_imu_rmat, 0,2), RMAT_ELMT(ltp_to_imu_rmat, 1,2), RMAT_ELMT(ltp_to_imu_rmat, 2,2)}; @@ -345,7 +345,7 @@ void ahrs_update_accel(void) { /* convert centrifucal acceleration from body to imu frame */ struct Int32Vect3 acc_c_imu; struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu); - INT32_RMAT_VMULT(acc_c_imu, *body_to_imu_rmat, acc_c_body); + int32_rmat_vmult(&acc_c_imu, body_to_imu_rmat, &acc_c_body); /* and subtract it from imu measurement to get a corrected measurement * of the gravity vector */ @@ -454,10 +454,10 @@ void ahrs_set_mag_gains(void) { static inline void ahrs_update_mag_full(void) { struct Int32RMat ltp_to_imu_rmat; - INT32_RMAT_OF_QUAT(ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat); + int32_rmat_of_quat(<p_to_imu_rmat, &ahrs_impl.ltp_to_imu_quat); struct Int32Vect3 expected_imu; - INT32_RMAT_VMULT(expected_imu, ltp_to_imu_rmat, ahrs_impl.mag_h); + int32_rmat_vmult(&expected_imu, <p_to_imu_rmat, &ahrs_impl.mag_h); struct Int32Vect3 residual; VECT3_CROSS_PRODUCT(residual, imu.mag, expected_imu); @@ -505,17 +505,17 @@ static inline void ahrs_update_mag_2d(void) { struct Int32Vect2 expected_ltp = {ahrs_impl.mag_h.x, ahrs_impl.mag_h.y}; /* normalize expected ltp in 2D (x,y) */ - INT32_VECT2_NORMALIZE(expected_ltp, INT32_MAG_FRAC); + int32_vect2_normalize(&expected_ltp, INT32_MAG_FRAC); struct Int32RMat ltp_to_imu_rmat; - INT32_RMAT_OF_QUAT(ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat); + int32_rmat_of_quat(<p_to_imu_rmat, &ahrs_impl.ltp_to_imu_quat); struct Int32Vect3 measured_ltp; - INT32_RMAT_TRANSP_VMULT(measured_ltp, ltp_to_imu_rmat, imu.mag); + int32_rmat_transp_vmult(&measured_ltp, <p_to_imu_rmat, &imu.mag); /* normalize measured ltp in 2D (x,y) */ struct Int32Vect2 measured_ltp_2d = {measured_ltp.x, measured_ltp.y}; - INT32_VECT2_NORMALIZE(measured_ltp_2d, INT32_MAG_FRAC); + int32_vect2_normalize(&measured_ltp_2d, INT32_MAG_FRAC); /* residual_ltp FRAC: 2 * MAG_FRAC - 5 = 17 */ struct Int32Vect3 residual_ltp = @@ -525,7 +525,7 @@ static inline void ahrs_update_mag_2d(void) { struct Int32Vect3 residual_imu; - INT32_RMAT_VMULT(residual_imu, ltp_to_imu_rmat, residual_ltp); + int32_rmat_vmult(&residual_imu, <p_to_imu_rmat, &residual_ltp); /* Complementary filter proportionnal gain. * Kp = 2 * mag_zeta * mag_omega * AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY @@ -620,8 +620,8 @@ void ahrs_update_heading(int32_t heading) { struct Int32Vect3 residual_imu; struct Int32RMat ltp_to_imu_rmat; - INT32_RMAT_OF_QUAT(ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat); - INT32_RMAT_VMULT(residual_imu, ltp_to_imu_rmat, residual_ltp); + int32_rmat_of_quat(<p_to_imu_rmat, &ahrs_impl.ltp_to_imu_quat); + int32_rmat_vmult(&residual_imu, <p_to_imu_rmat, &residual_ltp); // residual FRAC = TRIG_FRAC + TRIG_FRAC = 14 + 14 = 28 // rate_correction FRAC = RATE_FRAC = 12 @@ -669,20 +669,20 @@ void ahrs_realign_heading(int32_t heading) { QUAT_COPY(q_h, ltp_to_body_quat); q_h.qx = 0; q_h.qy = 0; - INT32_QUAT_NORMALIZE(q_h); + int32_quat_normalize(&q_h); /* quaternion representing rotation from current to new heading */ struct Int32Quat q_c; - INT32_QUAT_INV_COMP_NORM_SHORTEST(q_c, q_h, q_h_new); + int32_quat_inv_comp_norm_shortest(&q_c, &q_h, &q_h_new); /* correct current heading in body frame */ struct Int32Quat q; - INT32_QUAT_COMP_NORM_SHORTEST(q, q_c, ltp_to_body_quat); + int32_quat_comp_norm_shortest(&q, &q_c, <p_to_body_quat); QUAT_COPY(ltp_to_body_quat, q); /* compute ltp to imu rotations */ struct Int32Quat *body_to_imu_quat = orientationGetQuat_i(&imu.body_to_imu); - INT32_QUAT_COMP(ahrs_impl.ltp_to_imu_quat, ltp_to_body_quat, *body_to_imu_quat); + int32_quat_comp(&ahrs_impl.ltp_to_imu_quat, <p_to_body_quat, body_to_imu_quat); /* Set state */ stateSetNedToBodyQuat_i(<p_to_body_quat); @@ -696,7 +696,7 @@ static inline void set_body_state_from_quat(void) { /* Compute LTP to BODY quaternion */ struct Int32Quat ltp_to_body_quat; struct Int32Quat *body_to_imu_quat = orientationGetQuat_i(&imu.body_to_imu); - INT32_QUAT_COMP_INV(ltp_to_body_quat, ahrs_impl.ltp_to_imu_quat, *body_to_imu_quat); + int32_quat_comp_inv(<p_to_body_quat, &ahrs_impl.ltp_to_imu_quat, body_to_imu_quat); /* Set state */ #ifdef AHRS_UPDATE_FW_ESTIMATOR struct Int32Eulers neutrals_to_body_eulers = { @@ -704,9 +704,9 @@ static inline void set_body_state_from_quat(void) { ANGLE_BFP_OF_REAL(ins_pitch_neutral), 0 }; struct Int32Quat neutrals_to_body_quat, ltp_to_neutrals_quat; - INT32_QUAT_OF_EULERS(neutrals_to_body_quat, neutrals_to_body_eulers); - INT32_QUAT_NORMALIZE(neutrals_to_body_quat); - INT32_QUAT_COMP_INV(ltp_to_neutrals_quat, ltp_to_body_quat, neutrals_to_body_quat); + int32_quat_of_eulers(&neutrals_to_body_quat, &neutrals_to_body_eulers); + int32_quat_normalize(&neutrals_to_body_quat); + int32_quat_comp_inv(<p_to_neutrals_quat, <p_to_body_quat, &neutrals_to_body_quat); stateSetNedToBodyQuat_i(<p_to_neutrals_quat); #else stateSetNedToBodyQuat_i(<p_to_body_quat); @@ -715,7 +715,7 @@ static inline void set_body_state_from_quat(void) { /* compute body rates */ struct Int32Rates body_rate; struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu); - INT32_RMAT_TRANSP_RATEMULT(body_rate, *body_to_imu_rmat, ahrs_impl.imu_rate); + int32_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_impl.imu_rate); /* Set state */ stateSetBodyRates_i(&body_rate); } diff --git a/sw/airborne/subsystems/imu.c b/sw/airborne/subsystems/imu.c index c331c4eb94f..64dfbfb55e1 100644 --- a/sw/airborne/subsystems/imu.c +++ b/sw/airborne/subsystems/imu.c @@ -207,4 +207,3 @@ void imu_SetBodyToImuCurrent(float set) { orientationSetEulers_f(&imu.body_to_imu, &imu_to_body_eulers); } } - diff --git a/sw/airborne/subsystems/ins/hf_float.c b/sw/airborne/subsystems/ins/hf_float.c index ff70fef84a4..c08ef3af248 100644 --- a/sw/airborne/subsystems/ins/hf_float.c +++ b/sw/airborne/subsystems/ins/hf_float.c @@ -464,7 +464,7 @@ void b2_hff_propagate(void) { /* rotate imu accel measurement to body frame and filter */ struct Int32Vect3 acc_meas_body; struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu); - INT32_RMAT_TRANSP_VMULT(acc_meas_body, *body_to_imu_rmat, imu.accel); + int32_rmat_transp_vmult(&acc_meas_body, body_to_imu_rmat, &imu.accel); struct Int32Vect3 acc_body_filtered; acc_body_filtered.x = update_butterworth_2_low_pass_int(&filter_x, acc_meas_body.x); @@ -477,7 +477,7 @@ void b2_hff_propagate(void) { if (b2_hff_lost_counter < b2_hff_lost_limit) { struct Int32Vect3 filtered_accel_ltp; struct Int32RMat* ltp_to_body_rmat = stateGetNedToBodyRMat_i(); - INT32_RMAT_TRANSP_VMULT(filtered_accel_ltp, (*ltp_to_body_rmat), acc_body_filtered); + int32_rmat_transp_vmult(&filtered_accel_ltp, ltp_to_body_rmat, &acc_body_filtered); b2_hff_xdd_meas = ACCEL_FLOAT_OF_BFP(filtered_accel_ltp.x); b2_hff_ydd_meas = ACCEL_FLOAT_OF_BFP(filtered_accel_ltp.y); #ifdef GPS_LAG diff --git a/sw/airborne/subsystems/ins/ins_float_invariant.c b/sw/airborne/subsystems/ins/ins_float_invariant.c index 36bd680426b..49b32ae074f 100644 --- a/sw/airborne/subsystems/ins/ins_float_invariant.c +++ b/sw/airborne/subsystems/ins/ins_float_invariant.c @@ -359,10 +359,10 @@ void ahrs_propagate(void) { // fill command vector struct Int32Rates gyro_meas_body; struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu); - INT32_RMAT_TRANSP_RATEMULT(gyro_meas_body, *body_to_imu_rmat, imu.gyro); + int32_rmat_transp_ratemult(&gyro_meas_body, body_to_imu_rmat, &imu.gyro); RATES_FLOAT_OF_BFP(ins_impl.cmd.rates, gyro_meas_body); struct Int32Vect3 accel_meas_body; - INT32_RMAT_TRANSP_VMULT(accel_meas_body, *body_to_imu_rmat, imu.accel); + int32_rmat_transp_vmult(&accel_meas_body, body_to_imu_rmat, &imu.accel); ACCELS_FLOAT_OF_BFP(ins_impl.cmd.accel, accel_meas_body); // update correction gains @@ -558,7 +558,7 @@ void ahrs_update_mag(void) { struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu); struct Int32Vect3 mag_meas_body; // new values in body frame - INT32_RMAT_TRANSP_VMULT(mag_meas_body, *body_to_imu_rmat, imu.mag); + int32_rmat_transp_vmult(&mag_meas_body, body_to_imu_rmat, &imu.mag); MAGS_FLOAT_OF_BFP(ins_impl.meas.mag, mag_meas_body); // reset counter mag_frozen_count = MAG_FROZEN_COUNT; diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c index 25d8fd3eef8..bd7a68dd69c 100644 --- a/sw/airborne/subsystems/ins/ins_int.c +++ b/sw/airborne/subsystems/ins/ins_int.c @@ -221,9 +221,9 @@ void ins_propagate(void) { /* untilt accels */ struct Int32Vect3 accel_meas_body; struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu); - INT32_RMAT_TRANSP_VMULT(accel_meas_body, *body_to_imu_rmat, imu.accel); + int32_rmat_transp_vmult(&accel_meas_body, body_to_imu_rmat, &imu.accel); struct Int32Vect3 accel_meas_ltp; - INT32_RMAT_TRANSP_VMULT(accel_meas_ltp, (*stateGetNedToBodyRMat_i()), accel_meas_body); + int32_rmat_transp_vmult(&accel_meas_ltp, stateGetNedToBodyRMat_i(), &accel_meas_body); float z_accel_meas_float = ACCEL_FLOAT_OF_BFP(accel_meas_ltp.z); if (ins_impl.baro_initialized) { diff --git a/sw/airborne/test/ahrs/run_ahrs_on_synth.c b/sw/airborne/test/ahrs/run_ahrs_on_synth.c index 5f951047e8e..6e8e8d5c4d5 100644 --- a/sw/airborne/test/ahrs/run_ahrs_on_synth.c +++ b/sw/airborne/test/ahrs/run_ahrs_on_synth.c @@ -53,7 +53,7 @@ static void report(void) { #if AHRS_TYPE == AHRS_TYPE_ICQ struct Int32Eulers ltp_to_imu_euler_i; - INT32_EULERS_OF_QUAT(ltp_to_imu_euler_i, ahrs_impl.ltp_to_imu_quat); + int32_eulers_of_quat(<p_to_imu_euler_i, &ahrs_impl.ltp_to_imu_quat); struct FloatEulers ltp_to_imu_euler_f; EULERS_FLOAT_OF_BFP(ltp_to_imu_euler_f, ltp_to_imu_euler_i); printf("%f %f %f ", DegOfRad(ltp_to_imu_euler_f.phi), diff --git a/sw/airborne/test/pprz_algebra_print.h b/sw/airborne/test/pprz_algebra_print.h index 5a8c0e6bb36..744a431e834 100644 --- a/sw/airborne/test/pprz_algebra_print.h +++ b/sw/airborne/test/pprz_algebra_print.h @@ -47,7 +47,7 @@ #define DISPLAY_FLOAT_QUAT_AS_EULERS_DEG(text, quat) { \ struct FloatEulers _fe; \ - FLOAT_EULERS_OF_QUAT(_fe, quat); \ + float_eulers_of_quat(&_fe, &quat); \ DISPLAY_FLOAT_EULERS_DEG(text, _fe); \ } @@ -55,7 +55,7 @@ #define DISPLAY_FLOAT_RMAT_AS_EULERS_DEG(text, _mat) { \ struct FloatEulers _fe; \ - FLOAT_EULERS_OF_RMAT(_fe, (_mat)); \ + float_eulers_of_rmat(&_fe, &(_mat)); \ DISPLAY_FLOAT_EULERS_DEG(text, _fe); \ } @@ -128,7 +128,7 @@ struct FloatQuat _fq; \ QUAT_FLOAT_OF_BFP(_fq, _quat); \ struct FloatEulers _fe; \ - FLOAT_EULERS_OF_QUAT(_fe, _fq); \ + float_eulers_of_quat(&_fe, &_fq); \ DISPLAY_FLOAT_EULERS_DEG(text, _fe); \ } @@ -154,7 +154,7 @@ struct FloatRMat _frm; \ RMAT_FLOAT_OF_BFP(_frm, (_mat)); \ struct FloatEulers _fe; \ - FLOAT_EULERS_OF_RMAT(_fe, _frm); \ + float_eulers_of_rmat(&_fe, &_frm); \ DISPLAY_FLOAT_EULERS_DEG(text, _fe); \ } diff --git a/sw/airborne/test/test_algebra.c b/sw/airborne/test/test_algebra.c index c465c26f9aa..c9825649556 100644 --- a/sw/airborne/test/test_algebra.c +++ b/sw/airborne/test/test_algebra.c @@ -79,15 +79,15 @@ static void test_1(void) { DISPLAY_INT32_EULERS("euler_i", euler_i); struct FloatQuat quat_f; - FLOAT_QUAT_OF_EULERS(quat_f, euler_f); + float_quat_of_eulers(&quat_f, &euler_f); DISPLAY_FLOAT_QUAT("quat_f", quat_f); struct Int32Quat quat_i; - INT32_QUAT_OF_EULERS(quat_i, euler_i); + int32_quat_of_eulers(&quat_i, &euler_i); DISPLAY_INT32_QUAT("quat_i", quat_i); struct Int32RMat rmat_i; - INT32_RMAT_OF_QUAT(rmat_i, quat_i); + int32_rmat_of_quat(&rmat_i, &quat_i); DISPLAY_INT32_RMAT("rmat_i", rmat_i); } @@ -106,17 +106,17 @@ static void test_2(void) { DISPLAY_INT32_EULERS("euler_i", euler_i); struct Int32Quat quat_i; - INT32_QUAT_OF_EULERS(quat_i, euler_i); + int32_quat_of_eulers(&quat_i, &euler_i); DISPLAY_INT32_QUAT("quat_i", quat_i); - INT32_QUAT_NORMALIZE(quat_i); + int32_quat_normalize(&quat_i); DISPLAY_INT32_QUAT("quat_i_n", quat_i); struct Int32Vect3 v2; - INT32_QUAT_VMULT(v2, quat_i, v1); + int32_quat_vmult(&v2, &quat_i, &v1); DISPLAY_INT32_VECT3("v2", v2); struct Int32RMat rmat_i; - INT32_RMAT_OF_QUAT(rmat_i, quat_i); + int32_rmat_of_quat(&rmat_i, &quat_i); DISPLAY_INT32_RMAT("rmat_i", rmat_i); struct Int32Vect3 v3; @@ -124,7 +124,7 @@ static void test_2(void) { DISPLAY_INT32_VECT3("v3", v3); struct Int32RMat rmat_i2; - INT32_RMAT_OF_EULERS(rmat_i2, euler_i); + int32_rmat_of_eulers(&rmat_i2, &euler_i); DISPLAY_INT32_RMAT("rmat_i2", rmat_i2); struct Int32Vect3 v4; @@ -132,14 +132,14 @@ static void test_2(void) { DISPLAY_INT32_VECT3("v4", v4); struct FloatQuat quat_f; - FLOAT_QUAT_OF_EULERS(quat_f, euler_f); + float_quat_of_eulers(&quat_f, &euler_f); DISPLAY_FLOAT_QUAT("quat_f", quat_f); struct FloatVect3 v5; VECT3_COPY(v5, v1); DISPLAY_FLOAT_VECT3("v5", v5); struct FloatVect3 v6; - FLOAT_QUAT_VMULT(v6, quat_f, v5); + float_quat_vmult(&v6, &quat_f, &v5); DISPLAY_FLOAT_VECT3("v6", v6); } @@ -153,14 +153,14 @@ static void test_3(void) { /* Compute BODY to IMU quaternion */ struct Int32Quat b2i_q; - INT32_QUAT_OF_EULERS(b2i_q, b2i_e); + int32_quat_of_eulers(&b2i_q, &b2i_e); DISPLAY_INT32_QUAT_AS_EULERS_DEG("b2i_q", b2i_q); - // INT32_QUAT_NORMALIZE(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); + 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); @@ -171,48 +171,48 @@ static void test_3(void) { /* Compute LTP to IMU quaternion */ struct Int32Quat l2i_q; - INT32_QUAT_OF_EULERS(l2i_q, l2i_e); + 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); + 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); + 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); + 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); + 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); + 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); + 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); + int32_eulers_of_quat(&l2b_e2, &l2b_q); DISPLAY_INT32_EULERS_AS_FLOAT_DEG("l2b_e2", l2b_e2); } @@ -230,14 +230,14 @@ static void test_4_int(void) { /* transform to quaternion */ struct Int32Quat _q; - INT32_QUAT_OF_EULERS(_q, _e); + int32_quat_of_eulers(&_q, &_e); DISPLAY_INT32_QUAT_AS_EULERS_DEG("quat1 ", _q); - // INT32_QUAT_NORMALIZE(_q); + // int32_quat_normalize(&_q); // DISPLAY_INT32_QUAT_2("_q_n", _q); /* back to eulers */ struct Int32Eulers _e2; - INT32_EULERS_OF_QUAT(_e2, _q); + int32_eulers_of_quat(&_e2, &_q); DISPLAY_INT32_EULERS_AS_FLOAT_DEG("back to euler ", _e2); @@ -255,14 +255,14 @@ static void test_4_float(void) { /* transform to quaternion */ struct FloatQuat q; - FLOAT_QUAT_OF_EULERS(q, e); + float_quat_of_eulers(&q, &e); // DISPLAY_FLOAT_QUAT("q", q); - FLOAT_QUAT_NORMALIZE(q); + float_quat_normalize(&q); DISPLAY_FLOAT_QUAT("q_n", q); DISPLAY_FLOAT_QUAT_AS_INT("q_n as int", q); /* back to eulers */ struct FloatEulers e2; - FLOAT_EULERS_OF_QUAT(e2, q); + float_eulers_of_quat(&e2, &q); DISPLAY_FLOAT_EULERS_DEG("e2", e2); @@ -275,11 +275,11 @@ static void test_5(void) { DISPLAY_FLOAT_EULERS_DEG("fe", fe); printf("\n"); struct FloatQuat fq; - FLOAT_QUAT_OF_EULERS(fq, fe); + float_quat_of_eulers(&fq, &fe); test_eulers_of_quat(fq, 1); printf("\n"); struct FloatRMat frm; - FLOAT_RMAT_OF_EULERS(frm, fe); + float_rmat_of_eulers(&frm, &fe); DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("frm", frm); test_eulers_of_rmat(frm, 1); printf("\n"); @@ -290,11 +290,11 @@ static void test_5(void) { float test_eulers_of_quat(struct FloatQuat fq, int display) { struct FloatEulers fe; - FLOAT_EULERS_OF_QUAT(fe, fq); + float_eulers_of_quat(&fe, &fq); struct Int32Quat iq; QUAT_BFP_OF_REAL(iq, fq); struct Int32Eulers ie; - INT32_EULERS_OF_QUAT(ie, iq); + int32_eulers_of_quat(&ie, &iq); struct FloatEulers fe2; EULERS_FLOAT_OF_BFP(fe2, ie); EULERS_SUB(fe2, ie); @@ -315,11 +315,11 @@ float test_eulers_of_quat(struct FloatQuat fq, int display) { float test_eulers_of_rmat(struct FloatRMat frm, int display) { struct FloatEulers fe; - FLOAT_EULERS_OF_RMAT(fe, frm); + float_eulers_of_rmat(&fe, &frm); struct Int32RMat irm; RMAT_BFP_OF_REAL(irm, frm); struct Int32Eulers ie; - INT32_EULERS_OF_RMAT(ie, irm); + int32_eulers_of_rmat(&ie, &irm); struct FloatEulers fe2; EULERS_FLOAT_OF_BFP(fe2, ie); EULERS_SUB(fe2, ie); @@ -350,17 +350,17 @@ static void test_6(void) { EULERS_ASSIGN(eb2c, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(90.)); DISPLAY_FLOAT_EULERS_DEG("eb2c", eb2c); struct FloatRMat fa2b; - FLOAT_RMAT_OF_EULERS(fa2b, ea2b); + float_rmat_of_eulers(&fa2b, &ea2b); struct FloatRMat fb2c; - FLOAT_RMAT_OF_EULERS(fb2c, eb2c); + float_rmat_of_eulers(&fb2c, &eb2c); printf("\n"); test_rmat_comp(fa2b, fb2c, 1); struct FloatQuat qa2b; - FLOAT_QUAT_OF_EULERS(qa2b, ea2b); + float_quat_of_eulers(&qa2b, &ea2b); struct FloatQuat qb2c; - FLOAT_QUAT_OF_EULERS(qb2c, eb2c); + float_quat_of_eulers(&qb2c, &eb2c); printf("\n"); test_quat_comp(qa2b, qb2c, 1); @@ -373,13 +373,13 @@ static void test_6(void) { float test_rmat_comp(struct FloatRMat ma2b_f, struct FloatRMat mb2c_f, int display) { struct FloatRMat ma2c_f; - FLOAT_RMAT_COMP(ma2c_f, ma2b_f, mb2c_f); + float_rmat_comp(&ma2c_f, &ma2b_f, &mb2c_f); struct Int32RMat ma2b_i; RMAT_BFP_OF_REAL(ma2b_i, ma2b_f); struct Int32RMat mb2c_i; RMAT_BFP_OF_REAL(mb2c_i, mb2c_f); struct Int32RMat ma2c_i; - INT32_RMAT_COMP(ma2c_i, ma2b_i, mb2c_i); + int32_rmat_comp(&ma2c_i, &ma2b_i, &mb2c_i); struct FloatRMat err; RMAT_DIFF(err, ma2c_f, ma2c_i); @@ -399,13 +399,13 @@ float test_rmat_comp(struct FloatRMat ma2b_f, struct FloatRMat mb2c_f, int displ 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); + 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); + int32_quat_comp(&qa2c_i, &qa2b_i, &qb2c_i); struct FloatQuat err; QUAT_DIFF(err, qa2c_f, qa2c_i); @@ -434,17 +434,17 @@ static void test_7(void) { EULERS_ASSIGN(eb2c, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(90.)); DISPLAY_FLOAT_EULERS_DEG("eb2c", eb2c); struct FloatRMat fa2c; - FLOAT_RMAT_OF_EULERS(fa2c, ea2c); + float_rmat_of_eulers(&fa2c, &ea2c); struct FloatRMat fb2c; - FLOAT_RMAT_OF_EULERS(fb2c, eb2c); + float_rmat_of_eulers(&fb2c, &eb2c); printf("\n"); test_rmat_comp_inv(fa2c, fb2c, 1); struct FloatQuat qa2c; - FLOAT_QUAT_OF_EULERS(qa2c, ea2c); + float_quat_of_eulers(&qa2c, &ea2c); struct FloatQuat qb2c; - FLOAT_QUAT_OF_EULERS(qb2c, eb2c); + float_quat_of_eulers(&qb2c, &eb2c); printf("\n"); test_quat_comp_inv(qa2c, qb2c, 1); @@ -502,17 +502,17 @@ static void test_10(void) { EULERS_ASSIGN(euler , RadOfDeg(0.), RadOfDeg(10.), RadOfDeg(0.)); DISPLAY_FLOAT_EULERS_DEG("euler", euler); struct FloatQuat quat; - FLOAT_QUAT_OF_EULERS(quat, euler); + 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); + int32_quat_of_eulers(&quati, &euleri); DISPLAY_INT32_QUAT("####quat", quati); struct Int32RMat rmati; - INT32_RMAT_OF_EULERS(rmati, euleri); + int32_rmat_of_eulers(&rmati, &euleri); DISPLAY_INT32_RMAT("####rmat", rmati); struct Int32Quat quat_ltp_to_body; @@ -520,7 +520,7 @@ static void test_10(void) { int32_quat_identity(&body_to_imu_quat); - INT32_QUAT_COMP_INV(quat_ltp_to_body, body_to_imu_quat, quati); + int32_quat_comp_inv(&quat_ltp_to_body, &body_to_imu_quat, &quati); DISPLAY_INT32_QUAT("####quat_ltp_to_body", quat_ltp_to_body); } @@ -528,13 +528,13 @@ static void test_10(void) { float test_rmat_comp_inv(struct FloatRMat ma2c_f, struct FloatRMat mb2c_f, int display) { struct FloatRMat ma2b_f; - FLOAT_RMAT_COMP_INV(ma2b_f, ma2c_f, mb2c_f); + float_rmat_comp_inv(&ma2b_f, &ma2c_f, &mb2c_f); struct Int32RMat ma2c_i; RMAT_BFP_OF_REAL(ma2c_i, ma2c_f); struct Int32RMat mb2c_i; RMAT_BFP_OF_REAL(mb2c_i, mb2c_f); struct Int32RMat ma2b_i; - INT32_RMAT_COMP_INV(ma2b_i, ma2c_i, mb2c_i); + int32_rmat_comp_inv(&ma2b_i, &ma2c_i, &mb2c_i); struct FloatRMat err; RMAT_DIFF(err, ma2b_f, ma2b_i); @@ -553,13 +553,13 @@ float test_rmat_comp_inv(struct FloatRMat ma2c_f, struct FloatRMat mb2c_f, int d 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); + 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); + int32_quat_comp_inv(&qa2b_i, &qa2c_i, &qb2c_i); struct FloatQuat err; QUAT_DIFF(err, qa2b_f, qa2b_i); @@ -590,7 +590,7 @@ void test_of_axis_angle(void) { DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("quat", my_q); struct FloatRMat my_r1; - FLOAT_RMAT_OF_QUAT(my_r1, my_q); + float_rmat_of_quat(&my_r1, &my_q); DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat1", my_r1); DISPLAY_FLOAT_RMAT("rmat1", my_r1); @@ -616,17 +616,17 @@ void test_of_axis_angle(void) { FLOAT_RMAT_OF_AXIS_ANGLE(r_roll, ux, eul.phi); struct FloatRMat r_yaw_pitch; - FLOAT_RMAT_COMP(r_yaw_pitch, r_yaw, r_pitch); + float_rmat_comp(&r_yaw_pitch, &r_yaw, &r_pitch); struct FloatRMat r_yaw_pitch_roll; - FLOAT_RMAT_COMP(r_yaw_pitch_roll, r_yaw_pitch, r_roll); + float_rmat_comp(&r_yaw_pitch_roll, &r_yaw_pitch, &r_roll); DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat", r_yaw_pitch_roll); DISPLAY_FLOAT_RMAT("rmat", r_yaw_pitch_roll); DISPLAY_FLOAT_EULERS_DEG("eul", eul); struct FloatRMat rmat1; - FLOAT_RMAT_OF_EULERS(rmat1, eul); + float_rmat_of_eulers(&rmat1, &eul); DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat1", rmat1); DISPLAY_FLOAT_RMAT("rmat1", rmat1); @@ -639,16 +639,16 @@ float test_quat_of_rmat(void) { // struct FloatEulers eul = {RadOfDeg(0.13), RadOfDeg(180.), RadOfDeg(-61.)}; struct FloatRMat rm; - FLOAT_RMAT_OF_EULERS(rm, eul); + float_rmat_of_eulers(&rm, &eul); DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat", rm); struct FloatQuat q; - FLOAT_QUAT_OF_RMAT(q, rm); + float_quat_of_rmat(&q, &rm); DISPLAY_FLOAT_QUAT("q_of_rm ", q); DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("q_of_rm ", q); struct FloatQuat qref; - FLOAT_QUAT_OF_EULERS(qref, eul); + float_quat_of_eulers(&qref, &eul); DISPLAY_FLOAT_QUAT("q_of_euler", qref); DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("q_of_euler", qref); @@ -656,18 +656,18 @@ float test_quat_of_rmat(void) { struct FloatRMat r_att; struct FloatEulers e312 = { eul.phi, eul.theta, eul.psi }; - FLOAT_RMAT_OF_EULERS_312(r_att, e312); + float_rmat_of_eulers_312(&r_att, &e312); DISPLAY_FLOAT_RMAT("r_att ", r_att); DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("r_att ", r_att); struct FloatQuat q_att; - FLOAT_QUAT_OF_RMAT(q_att, r_att); + float_quat_of_rmat(&q_att, &r_att); struct FloatEulers e_att; - FLOAT_EULERS_OF_RMAT(e_att, r_att); + float_eulers_of_rmat(&e_att, &r_att); DISPLAY_FLOAT_EULERS_DEG("of rmat", e_att); - FLOAT_EULERS_OF_QUAT(e_att, q_att); + float_eulers_of_quat(&e_att, &q_att); DISPLAY_FLOAT_EULERS_DEG("of quat", e_att); return 0.; @@ -684,11 +684,11 @@ float test_rmat_of_eulers_312(void) { DISPLAY_INT32_EULERS("eul312_i", eul312_i); struct FloatRMat rmat_f; - FLOAT_RMAT_OF_EULERS_312(rmat_f, eul312_f); + float_rmat_of_eulers_312(&rmat_f, &eul312_f); DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat float", rmat_f); struct Int32RMat rmat_i; - INT32_RMAT_OF_EULERS_312(rmat_i, eul312_i); + int32_rmat_of_eulers_312(&rmat_i, &eul312_i); DISPLAY_INT32_RMAT_AS_EULERS_DEG("rmat int", rmat_i); return 0; @@ -712,13 +712,13 @@ void test1234(void) { FLOAT_RMAT_OF_AXIS_ANGLE(r_roll, ux, eul.phi); struct FloatRMat r_tmp; - FLOAT_RMAT_COMP(r_tmp, r_yaw, r_roll); + float_rmat_comp(&r_tmp, &r_yaw, &r_roll); struct FloatRMat r_att; - FLOAT_RMAT_COMP(r_att, r_tmp, r_pitch); + float_rmat_comp(&r_att, &r_tmp, &r_pitch); DISPLAY_FLOAT_RMAT("r_att_ref ", r_att); - FLOAT_RMAT_OF_EULERS_312(r_att, eul); + float_rmat_of_eulers_312(&r_att, &eul); DISPLAY_FLOAT_RMAT("r_att312 ", r_att); } @@ -738,11 +738,11 @@ float test_quat(void) { DISPLAY_FLOAT_QUAT("iq", iq); struct FloatQuat q1; - FLOAT_QUAT_COMP(q1, q, iq); + float_quat_comp(&q1, &q, &iq); DISPLAY_FLOAT_QUAT("q1", q1); struct FloatQuat q2; - FLOAT_QUAT_COMP(q2, q, iq); + float_quat_comp(&q2, &q, &iq); DISPLAY_FLOAT_QUAT("q2", q2); struct FloatQuat qe; @@ -752,11 +752,11 @@ float test_quat(void) { struct FloatVect3 a = { 2., 1., 3.}; DISPLAY_FLOAT_VECT3("a ", a); struct FloatVect3 a1; - FLOAT_QUAT_VMULT(a1, q, a); + float_quat_vmult(&a1, &q, &a); DISPLAY_FLOAT_VECT3("a1", a1); struct FloatVect3 a2; - FLOAT_QUAT_VMULT(a2, qe, a); + float_quat_vmult(&a2, &qe, &a); DISPLAY_FLOAT_VECT3("a2", a2); return 0.; @@ -770,13 +770,13 @@ float test_quat2(void) { // DISPLAY_FLOAT_EULERS_DEG("eula2b", eula2b); struct FloatQuat qa2b; - FLOAT_QUAT_OF_EULERS(qa2b, eula2b); + 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); + double_quat_of_eulers(&qa2b_d, &eula2b_d); DISPLAY_FLOAT_QUAT("qa2b_d", qa2b_d); struct FloatVect3 u = { 1., 0., 0.}; @@ -792,7 +792,7 @@ float test_quat2(void) { // DISPLAY_FLOAT_EULERS_DEG("eula2c", eula2c); struct FloatQuat qa2c; - FLOAT_QUAT_OF_EULERS(qa2c, eula2c); + float_quat_of_eulers(&qa2c, &eula2c); DISPLAY_FLOAT_QUAT("qa2c", qa2c); @@ -802,11 +802,11 @@ float test_quat2(void) { struct FloatQuat qb2c1; - FLOAT_QUAT_COMP(qb2c1, qb2a, qa2c); + float_quat_comp(&qb2c1, &qb2a, &qa2c); DISPLAY_FLOAT_QUAT("qb2c1", qb2c1); struct FloatQuat qb2c2; - FLOAT_QUAT_INV_COMP(qb2c2, qa2b, qa2c); + float_quat_inv_comp(&qb2c2, &qa2b, &qa2c); DISPLAY_FLOAT_QUAT("qb2c2", qb2c2); return 0.; @@ -828,19 +828,19 @@ float test_INT32_QUAT_OF_RMAT(struct FloatEulers* eul_f, bool_t display) { if (display) DISPLAY_FLOAT_RMAT("rmat float", rmat_f); struct Int32RMat rmat_i; - INT32_RMAT_OF_EULERS_321(rmat_i, eul321_i); + int32_rmat_of_eulers_321(&rmat_i, &eul321_i); if (display) DISPLAY_INT32_RMAT_AS_EULERS_DEG("rmat int", rmat_i); if (display) DISPLAY_INT32_RMAT("rmat int", rmat_i); if (display) DISPLAY_INT32_RMAT_AS_FLOAT("rmat int", rmat_i); struct FloatQuat qf; - FLOAT_QUAT_OF_RMAT(qf, rmat_f); + float_quat_of_rmat(&qf, &rmat_f); //FLOAT_QUAT_WRAP_SHORTEST(qf); if (display) DISPLAY_FLOAT_QUAT("qf", qf); struct Int32Quat qi; - INT32_QUAT_OF_RMAT(qi, rmat_i); - //INT32_QUAT_WRAP_SHORTEST(qi); + int32_quat_of_rmat(&qi, &rmat_i); + //int32_quat_wrap_shortest(&qi); if (display) DISPLAY_INT32_QUAT("qi", qi); if (display) DISPLAY_INT32_QUAT_2("qi", qi); diff --git a/sw/airborne/test/test_bla.c b/sw/airborne/test/test_bla.c index e7bb9da462c..e1869b068a5 100644 --- a/sw/airborne/test/test_bla.c +++ b/sw/airborne/test/test_bla.c @@ -22,20 +22,20 @@ int main(int argc, char** argv) { DISPLAY_FLOAT_EULERS("eb", eb); struct FloatRMat ra; - FLOAT_RMAT_OF_EULERS(ra, ea); + float_rmat_of_eulers(&ra, &ea); struct FloatRMat rb; - FLOAT_RMAT_OF_EULERS(rb, eb); + float_rmat_of_eulers(&rb, &eb); struct FloatRMat rc; - FLOAT_RMAT_COMP(rc, ra, rb); + float_rmat_comp(&rc, &ra, &rb); DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rc ", rc); struct FloatQuat qa; - FLOAT_QUAT_OF_EULERS(qa, ea); + float_quat_of_eulers(&qa, &ea); struct FloatQuat qb; - FLOAT_QUAT_OF_EULERS(qb, eb); + float_quat_of_eulers(&qb, &eb); struct FloatQuat qc; - FLOAT_QUAT_COMP(qc, qa, qb); + float_quat_comp(&qc, &qa, &qb); DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("qc ", qc); diff --git a/sw/ground_segment/misc/natnet2ivy.c b/sw/ground_segment/misc/natnet2ivy.c index d1e80953247..e2bb6140938 100644 --- a/sw/ground_segment/misc/natnet2ivy.c +++ b/sw/ground_segment/misc/natnet2ivy.c @@ -484,7 +484,7 @@ gboolean timeout_transmit_callback(gpointer data) { orient.qx = rigidBodies[i].qx; orient.qy = rigidBodies[i].qy; orient.qz = rigidBodies[i].qz; - DOUBLE_EULERS_OF_QUAT(orient_eulers, orient); + double_eulers_of_quat(&orient_eulers, &orient); // Calculate the heading by adding the Natnet offset angle and normalizing it double heading = -orient_eulers.psi-tracking_offset_angle;