diff --git a/doc/pprz_algebra/quaternion.tex b/doc/pprz_algebra/quaternion.tex index 7977222283..3cb937575f 100644 --- a/doc/pprz_algebra/quaternion.tex +++ b/doc/pprz_algebra/quaternion.tex @@ -162,8 +162,8 @@ It is also possible to directly normalise the quaternion \begin{equation} \quat{} := \frac{\quat{}}{\norm{\quat{}}} \end{equation} -\inHfile{INT32\_QUAT\_NORMALISE(q)}{pprz\_algebra\_int} -\inHfile{FLOAT\_QUAT\_NORMALISE(q)}{pprz\_algebra\_float} +\inHfile{INT32\_QUAT\_NORMALIZE(q)}{pprz\_algebra\_int} +\inHfile{FLOAT\_QUAT\_NORMALIZE(q)}{pprz\_algebra\_float} \subsection*{Making the real value positive} It is possible to invert the quaternion if its real value is negative 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 2680feef43..36f6dd9df7 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c @@ -200,7 +200,7 @@ void stabilization_attitude_run(bool_t enable_integrator) { scaled_att_err.qy = att_err.qy / IERROR_SCALE; scaled_att_err.qz = att_err.qz / IERROR_SCALE; FLOAT_QUAT_COMP_INV(new_sum_err, stabilization_att_sum_err_quat, scaled_att_err); - FLOAT_QUAT_NORMALISE(new_sum_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 { 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 7799d9995b..c6478d5b40 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 @@ -117,7 +117,7 @@ void stabilization_attitude_ref_update() { 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); - FLOAT_QUAT_NORMALISE(stab_att_ref_quat); + FLOAT_QUAT_NORMALIZE(stab_att_ref_quat); /* integrate reference rotational speeds */ struct FloatRates delta_rate; diff --git a/sw/airborne/fms/fms_spi_autopilot_msg.c b/sw/airborne/fms/fms_spi_autopilot_msg.c index 33c0a53ee1..8df37b8ba3 100644 --- a/sw/airborne/fms/fms_spi_autopilot_msg.c +++ b/sw/airborne/fms/fms_spi_autopilot_msg.c @@ -106,7 +106,7 @@ int spi_ap_link_init() FLOAT_QUAT_OF_AXIS_ANGLE(imuFloat.body_to_imu_quat, x_axis, QUAT_SETPOINT_HOVER_PITCH); #endif - FLOAT_QUAT_NORMALISE(imuFloat.body_to_imu_quat); + FLOAT_QUAT_NORMALIZE(imuFloat.body_to_imu_quat); FLOAT_EULERS_OF_QUAT(imuFloat.body_to_imu_eulers, imuFloat.body_to_imu_quat); FLOAT_RMAT_OF_QUAT(imuFloat.body_to_imu_rmat, imuFloat.body_to_imu_quat); diff --git a/sw/airborne/math/pprz_algebra_float.h b/sw/airborne/math/pprz_algebra_float.h index 4e07955bc2..5a31345f6c 100644 --- a/sw/airborne/math/pprz_algebra_float.h +++ b/sw/airborne/math/pprz_algebra_float.h @@ -494,7 +494,7 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) { #define FLOAT_QUAT_NORM(_q) (sqrtf(SQUARE(_q.qi) + SQUARE(_q.qx)+ \ SQUARE(_q.qy) + SQUARE(_q.qz))) \ -#define FLOAT_QUAT_NORMALISE(q) { \ +#define FLOAT_QUAT_NORMALIZE(q) { \ float norm = FLOAT_QUAT_NORM(q); \ if (norm > FLT_MIN) { \ q.qi = q.qi / norm; \ @@ -515,7 +515,7 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) { #define FLOAT_QUAT_COMP_NORM_SHORTEST(_a2c, _a2b, _b2c) { \ FLOAT_QUAT_COMP(_a2c, _a2b, _b2c); \ FLOAT_QUAT_WRAP_SHORTEST(_a2c); \ - FLOAT_QUAT_NORMALISE(_a2c); \ + FLOAT_QUAT_NORMALIZE(_a2c); \ } /* _a2c = _a2b comp _b2c , aka _a2c = _a2b * _b2c */ @@ -532,7 +532,7 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) { #define FLOAT_QUAT_COMP_INV_NORM_SHORTEST(_a2b, _a2c, _b2c) { \ FLOAT_QUAT_COMP_INV(_a2b, _a2c, _b2c); \ FLOAT_QUAT_WRAP_SHORTEST(_a2b); \ - FLOAT_QUAT_NORMALISE(_a2b); \ + FLOAT_QUAT_NORMALIZE(_a2b); \ } /* _a2b = _a2c comp_inv _b2c , aka _a2b = _a2c * inv(_b2c) */ @@ -547,7 +547,7 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) { #define FLOAT_QUAT_INV_COMP_NORM_SHORTEST(_b2c, _a2b, _a2c) { \ FLOAT_QUAT_INV_COMP(_b2c, _a2b, _a2c); \ FLOAT_QUAT_WRAP_SHORTEST(_b2c); \ - FLOAT_QUAT_NORMALISE(_b2c); \ + FLOAT_QUAT_NORMALIZE(_b2c); \ } /* _b2c = _a2b inv_comp _a2c , aka _b2c = inv(_a2b) * _a2c */ diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_rmat.c b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_rmat.c index 8e91105add..6eed92bc4d 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_rmat.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_rmat.c @@ -119,7 +119,7 @@ void ahrs_propagate(void) { #endif #ifdef AHRS_PROPAGATE_QUAT FLOAT_QUAT_INTEGRATE(ahrs_float.ltp_to_imu_quat, omega, dt); - FLOAT_QUAT_NORMALISE(ahrs_float.ltp_to_imu_quat); + FLOAT_QUAT_NORMALIZE(ahrs_float.ltp_to_imu_quat); compute_imu_rmat_and_euler_from_quat(); #endif compute_body_orientation_and_rates(); diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_lkf.c b/sw/airborne/subsystems/ahrs/ahrs_float_lkf.c index 50e2c6f6df..f1e781a36a 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_lkf.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_lkf.c @@ -315,7 +315,7 @@ void ahrs_propagate(void) { bafl_qnorm = FLOAT_QUAT_NORM(bafl_quat); //TODO check if broot force normalization is good, use lagrange normalization? - FLOAT_QUAT_NORMALISE(bafl_quat); + FLOAT_QUAT_NORMALIZE(bafl_quat); /* diff --git a/sw/airborne/subsystems/imu.c b/sw/airborne/subsystems/imu.c index d5412ff5f8..515adc6b38 100644 --- a/sw/airborne/subsystems/imu.c +++ b/sw/airborne/subsystems/imu.c @@ -64,7 +64,7 @@ void imu_float_init(struct ImuFloat* imuf) { EULERS_ASSIGN(imuf->body_to_imu_eulers, IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI); FLOAT_QUAT_OF_EULERS(imuf->body_to_imu_quat, imuf->body_to_imu_eulers); - FLOAT_QUAT_NORMALISE(imuf->body_to_imu_quat); + FLOAT_QUAT_NORMALIZE(imuf->body_to_imu_quat); FLOAT_RMAT_OF_EULERS(imuf->body_to_imu_rmat, imuf->body_to_imu_eulers); #else EULERS_ASSIGN(imuf->body_to_imu_eulers, 0., 0., 0.); diff --git a/sw/airborne/test/test_algebra.c b/sw/airborne/test/test_algebra.c index b8b7ce4d8b..ee67f3dffe 100644 --- a/sw/airborne/test/test_algebra.c +++ b/sw/airborne/test/test_algebra.c @@ -108,7 +108,7 @@ static void test_2(void) { struct Int32Quat quat_i; INT32_QUAT_OF_EULERS(quat_i, euler_i); DISPLAY_INT32_QUAT("quat_i", quat_i); - INT32_QUAT_NORMALISE(quat_i); + INT32_QUAT_NORMALIZE(quat_i); DISPLAY_INT32_QUAT("quat_i_n", quat_i); struct Int32Vect3 v2; @@ -155,7 +155,7 @@ static void test_3(void) { struct Int32Quat b2i_q; INT32_QUAT_OF_EULERS(b2i_q, b2i_e); DISPLAY_INT32_QUAT_AS_EULERS_DEG("b2i_q", b2i_q); - // INT32_QUAT_NORMALISE(b2i_q); + // INT32_QUAT_NORMALIZE(b2i_q); // DISPLAY_INT32_QUAT_AS_EULERS_DEG("b2i_q_n", b2i_q); /* Compute BODY to IMU rotation matrix */ @@ -232,7 +232,7 @@ static void test_4_int(void) { struct Int32Quat _q; INT32_QUAT_OF_EULERS(_q, _e); DISPLAY_INT32_QUAT_AS_EULERS_DEG("quat1 ", _q); - // INT32_QUAT_NORMALISE(_q); + // INT32_QUAT_NORMALIZE(_q); // DISPLAY_INT32_QUAT_2("_q_n", _q); /* back to eulers */ @@ -257,7 +257,7 @@ static void test_4_float(void) { struct FloatQuat q; FLOAT_QUAT_OF_EULERS(q, e); // DISPLAY_FLOAT_QUAT("q", q); - FLOAT_QUAT_NORMALISE(q); + FLOAT_QUAT_NORMALIZE(q); DISPLAY_FLOAT_QUAT("q_n", q); DISPLAY_FLOAT_QUAT_AS_INT("q_n as int", q); /* back to eulers */