From e1e69408f058694a66a2d1c0c791859a7c1ca31e Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 21 Feb 2011 23:59:14 +0100 Subject: [PATCH] put get and set as static inline functions in state.h, transformation calculations as normal functions in state.c --- sw/airborne/state.c | 433 +++++++++++++---------------------------- sw/airborne/state.h | 460 ++++++++++++++++++++++++++++++++++++-------- 2 files changed, 516 insertions(+), 377 deletions(-) diff --git a/sw/airborne/state.c b/sw/airborne/state.c index fd8ee5aa428..a768a58d8b7 100644 --- a/sw/airborne/state.c +++ b/sw/airborne/state.c @@ -36,61 +36,15 @@ struct State state; /******************************************************************************* * * - * Set and Get functions for the POSITION representations * + * transformation functions for the POSITION representations * * * ******************************************************************************/ /** @addtogroup PosGroup * @{ */ -/************************ Set functions ****************************/ -inline void StateSetPositionEcef_i(struct EcefCoor_i* ecef_pos) { - INT32_VECT3_COPY(state.ecef_pos_i, *ecef_pos); - /* clear bits for all position representations and only set the new one */ - state.pos_status = (1 << POS_ECEF_I); -} - -inline void StateSetPositionNed_i(struct NedCoor_i* ned_pos) { - INT32_VECT3_COPY(state.ned_pos_i, *ned_pos); - /* clear bits for all position representations and only set the new one */ - state.pos_status = (1 << POS_NED_I); -} - -inline void StateSetPositionLla_i(struct LlaCoor_i* lla_pos) { - LLA_COPY(state.lla_pos_i, *lla_pos); - /* clear bits for all position representations and only set the new one */ - state.pos_status = (1 << POS_LLA_I); -} - -//TODO utm in fixedpoint as well? - -inline void StateSetPositionUtm_f(struct FloatVect3* utm_pos) { - //TODO utm zone?? - VECT3_COPY(state.utm_pos_f, *utm_pos); - /* clear bits for all position representations and only set the new one */ - state.pos_status = (uint8_t)(1 << POS_UTM_F); -} - -inline void StateSetPositionEcef_f(struct EcefCoor_f* ecef_pos) { - VECT3_COPY(state.ecef_pos_f, *ecef_pos); - /* clear bits for all position representations and only set the new one */ - state.pos_status = (1 << POS_ECEF_F); -} - -inline void StateSetPositionNed_f(struct NedCoor_f* ned_pos) { - VECT3_COPY(state.ned_pos_f, *ned_pos); - /* clear bits for all position representations and only set the new one */ - state.pos_status = (1 << POS_NED_F); -} - -inline void StateSetPositionLla_f(struct LlaCoor_f* lla_pos) { - LLA_COPY(state.lla_pos_f, *lla_pos); - /* clear bits for all position representations and only set the new one */ - state.pos_status = (1 << POS_LLA_F); -} -/************************ Get functions ****************************/ -inline struct EcefCoor_i StateGetPositionEcef_i(void) { +void StateCalcPositionEcef_i(void) { if (bit_is_set(state.pos_status, POS_ECEF_I)) - return state.ecef_pos_i; + return; if (bit_is_set(state.pos_status, POS_ECEF_F)) { ECEF_BFP_OF_REAL(state.ecef_pos_i, state.ecef_pos_f); @@ -116,18 +70,18 @@ inline struct EcefCoor_i StateGetPositionEcef_i(void) { } else { /* could not get this representation, set errno */ - struct EcefCoor_i _ecef_zero = {0}; - return _ecef_zero; + //struct EcefCoor_i _ecef_zero = {0}; + //return _ecef_zero; } /* set bit to indicate this representation is computed */ SetBit(state.pos_status, POS_ECEF_I); - return state.ecef_pos_i; + //return state.ecef_pos_i; } -inline struct NedCoor_i StateGetPositionNed_i(void) { +void StateCalcPositionNed_i(void) { if (bit_is_set(state.pos_status, POS_NED_I)) - return state.ned_pos_i; + return; int errno = 0; if (state.ned_initialised_i) { @@ -161,18 +115,18 @@ inline struct NedCoor_i StateGetPositionNed_i(void) { errno = 2; } if (errno) { - struct NedCoor_i _ned_zero = {0}; - return _ned_zero; + //struct NedCoor_i _ned_zero = {0}; + //return _ned_zero; } /* set bit to indicate this representation is computed */ SetBit(state.pos_status, POS_NED_I); - return state.ned_pos_i; + //return state.ned_pos_i; } -inline struct LlaCoor_i StateGetPositionLla_i(void) { +void StateCalcPositionLla_i(void) { if (bit_is_set(state.pos_status, POS_LLA_I)) - return state.lla_pos_i; + return; if (bit_is_set(state.pos_status, POS_LLA_F)) { LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f); @@ -203,18 +157,18 @@ inline struct LlaCoor_i StateGetPositionLla_i(void) { } else { /* could not get this representation, set errno */ - struct LlaCoor_i _lla_zero = {0}; - return _lla_zero; + //struct LlaCoor_i _lla_zero = {0}; + //return _lla_zero; } /* set bit to indicate this representation is computed */ SetBit(state.pos_status, POS_LLA_I); - return state.lla_pos_i; + //return state.lla_pos_i; } -inline struct EcefCoor_f StateGetPositionEcef_f(void) { +void StateCalcPositionEcef_f(void) { if (bit_is_set(state.pos_status, POS_ECEF_F)) - return state.ecef_pos_f; + return; if (bit_is_set(state.pos_status, POS_ECEF_I)) { ECEF_FLOAT_OF_BFP(state.ecef_pos_f, state.ecef_pos_i); @@ -233,24 +187,24 @@ inline struct EcefCoor_f StateGetPositionEcef_f(void) { ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f); } else if (bit_is_set(state.pos_status, POS_LLA_I)) { - struct EcefCoor_i _ecef_i2; - ecef_of_lla_i(&_ecef_i2, &state.lla_pos_i); - ECEF_FLOAT_OF_BFP(state.ecef_pos_f, _ecef_i2); + LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i); + SetBit(state.pos_status, POS_LLA_F); + ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f); } else { /* could not get this representation, set errno */ - struct EcefCoor_f _ecef_zero = {0.0f}; - return _ecef_zero; + //struct EcefCoor_f _ecef_zero = {0.0f}; + //return _ecef_zero; } /* set bit to indicate this representation is computed */ SetBit(state.pos_status, POS_ECEF_F); - return state.ecef_pos_f; + //return state.ecef_pos_f; } -inline struct NedCoor_f StateGetPositionNed_f(void) { +void StateCalcPositionNed_f(void) { if (bit_is_set(state.pos_status, POS_NED_F)) - return state.ned_pos_f; + return; int errno = 0; if (state.ned_initialised_f) { @@ -284,18 +238,18 @@ inline struct NedCoor_f StateGetPositionNed_f(void) { errno = 2; } if (errno) { - struct NedCoor_f _ned_zero = {0.0f}; - return _ned_zero; + //struct NedCoor_f _ned_zero = {0.0f}; + //return _ned_zero; } /* set bit to indicate this representation is computed */ SetBit(state.pos_status, POS_NED_F); - return state.ned_pos_f; + //return state.ned_pos_f; } -inline struct LlaCoor_f StateGetPositionLla_f(void) { +void StateCalcPositionLla_f(void) { if (bit_is_set(state.pos_status, POS_LLA_F)) - return state.lla_pos_f; + return; if (bit_is_set(state.pos_status, POS_LLA_I)) { LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_f); @@ -325,13 +279,13 @@ inline struct LlaCoor_f StateGetPositionLla_f(void) { } else { /* could not get this representation, set errno */ - struct LlaCoor_f _lla_zero = {0.0}; - return _lla_zero; + //struct LlaCoor_f _lla_zero = {0.0}; + //return _lla_zero; } /* set bit to indicate this representation is computed */ SetBit(state.pos_status, POS_LLA_F); - return state.lla_pos_f; + //return state.lla_pos_f; } /** @}*/ @@ -341,40 +295,16 @@ inline struct LlaCoor_f StateGetPositionLla_f(void) { /****************************************************************************** * * - * Set and Get functions for the SPEED representations * + * Transformation functions for the SPEED representations * * * *****************************************************************************/ /** @addtogroup SpeedGroup * @{ */ /************************ Set functions ****************************/ -inline void StateSetSpeedNed_i(struct NedCoor_i* ned_speed) { - INT32_VECT3_COPY(state.ned_speed_i, *ned_speed); - /* clear bits for all speed representations and only set the new one */ - state.speed_status = (1 << SPEED_NED_I); -} - -inline void StateSetSpeedEcef_i(struct EcefCoor_i* ecef_speed) { - INT32_VECT3_COPY(state.ecef_speed_i, *ecef_speed); - /* clear bits for all speed representations and only set the new one */ - state.speed_status = (1 << SPEED_ECEF_I); -} - -inline void StateSetSpeedNed_f(struct NedCoor_f* ned_speed) { - VECT3_COPY(state.ned_speed_f, *ned_speed); - /* clear bits for all speed representations and only set the new one */ - state.speed_status = (1 << SPEED_NED_F); -} - -inline void StateSetSpeedEcef_f(struct EcefCoor_f* ecef_speed) { - VECT3_COPY(state.ecef_speed_f, *ecef_speed); - /* clear bits for all speed representations and only set the new one */ - state.speed_status = (1 << SPEED_ECEF_F); -} -/************************ Get functions ****************************/ -inline struct NedCoor_i StateGetSpeedNed_i(void) { +void StateCalcSpeedNed_i(void) { if (bit_is_set(state.speed_status, SPEED_NED_I)) - return state.ned_speed_i; + return; int errno = 0; if (state.ned_initialised_i) { @@ -397,18 +327,18 @@ inline struct NedCoor_i StateGetSpeedNed_i(void) { errno = 2; } if (errno) { - struct NedCoor_i _ned_zero = {0}; - return _ned_zero; + //struct NedCoor_i _ned_zero = {0}; + //return _ned_zero; } /* set bit to indicate this representation is computed */ SetBit(state.speed_status, SPEED_NED_I); - return state.ned_speed_i; + //return state.ned_speed_i; } -inline struct EcefCoor_i StateGetSpeedEcef_i(void) { +void StateCalcSpeedEcef_i(void) { if (bit_is_set(state.speed_status, SPEED_ECEF_I)) - return state.ecef_speed_i; + return; if (bit_is_set(state.speed_status, SPEED_ECEF_F)) { SPEEDS_BFP_OF_REAL(state.ecef_speed_i, state.ecef_speed_f); @@ -424,18 +354,18 @@ inline struct EcefCoor_i StateGetSpeedEcef_i(void) { } else { /* could not get this representation, set errno */ - struct EcefCoor_i _ecef_zero = {0}; - return _ecef_zero; + //struct EcefCoor_i _ecef_zero = {0}; + //return _ecef_zero; } /* set bit to indicate this representation is computed */ SetBit(state.speed_status, SPEED_ECEF_I); - return state.ecef_speed_i; + //return state.ecef_speed_i; } -inline int32_t StateGetHorizontalSpeedNorm_i(void) { //TODO +void StateCalcHorizontalSpeedNorm_i(void) { //TODO if (bit_is_set(state.speed_status, SPEED_HNORM_I)) - return state.h_speed_norm_i; + return; if (bit_is_set(state.speed_status, SPEED_HNORM_F)){ state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f); @@ -445,9 +375,9 @@ inline int32_t StateGetHorizontalSpeedNorm_i(void) { //TODO //INT32_VECT2_NORM(state.h_speed_norm_i, state.ned_speed_i); } else if (bit_is_set(state.speed_status, SPEED_NED_F)) { - float _norm_f; - FLOAT_VECT2_NORM(_norm_f, state.ned_speed_f); - state.h_speed_norm_i = SPEED_BFP_OF_REAL(_norm_f); + FLOAT_VECT2_NORM(state.h_speed_norm_f, state.ned_speed_f); + SetBit(state.speed_status, SPEED_HNORM_F); + state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f); } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) { /* transform ecef speed to ned, set status bit, then compute norm */ @@ -456,23 +386,25 @@ inline int32_t StateGetHorizontalSpeedNorm_i(void) { //TODO //INT32_VECT2_NORM(state.h_speed_norm_i, state.ned_speed_i); } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) { - float _norm_f; - FLOAT_VECT2_NORM(_norm_f, state.ned_speed_f); - state.h_speed_norm_i = SPEED_BFP_OF_REAL(_norm_f); + ned_of_ecef_vect_f(&state.ned_speed_f, &state.ned_origin_f, &state.ecef_speed_f); + SetBit(state.speed_status, SPEED_NED_F); + FLOAT_VECT2_NORM(state.h_speed_norm_f, state.ned_speed_f); + SetBit(state.speed_status, SPEED_HNORM_F); + state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f); } else { - int32_t _norm_zero = 0; - return _norm_zero; + //int32_t _norm_zero = 0; + //return _norm_zero; } /* set bit to indicate this representation is computed */ SetBit(state.speed_status, SPEED_HNORM_I); - return state.h_speed_norm_i; + //return state.h_speed_norm_i; } -inline int32_t StateGetHorizontalSpeedDir_i(void) { //TODO +void StateCalcHorizontalSpeedDir_i(void) { //TODO if (bit_is_set(state.speed_status, SPEED_HDIR_I)) - return state.h_speed_dir_i; + return; if (bit_is_set(state.speed_status, SPEED_HDIR_F)){ state.h_speed_dir_i = SPEED_BFP_OF_REAL(state.h_speed_dir_f); @@ -483,12 +415,12 @@ inline int32_t StateGetHorizontalSpeedDir_i(void) { //TODO /* set bit to indicate this representation is computed */ SetBit(state.speed_status, SPEED_HDIR_I); - return state.h_speed_dir_i; + //return state.h_speed_dir_i; } -inline struct NedCoor_f StateGetSpeedNed_f(void) { +void StateCalcSpeedNed_f(void) { if (bit_is_set(state.speed_status, SPEED_NED_F)) - return state.ned_speed_f; + return; int errno = 0; if (state.ned_initialised_f) { @@ -511,18 +443,18 @@ inline struct NedCoor_f StateGetSpeedNed_f(void) { errno = 2; } if (errno) { - struct NedCoor_f _ned_zero = {0.0f}; - return _ned_zero; + //struct NedCoor_f _ned_zero = {0.0f}; + //return _ned_zero; } /* set bit to indicate this representation is computed */ SetBit(state.speed_status, SPEED_NED_F); - return state.ned_speed_f; + //return state.ned_speed_f; } -inline struct EcefCoor_f StateGetSpeedEcef_f(void) { +void StateCalcSpeedEcef_f(void) { if (bit_is_set(state.speed_status, SPEED_ECEF_F)) - return state.ecef_speed_f; + return; if (bit_is_set(state.speed_status, SPEED_ECEF_I)) { SPEEDS_FLOAT_OF_BFP(state.ecef_speed_f, state.ned_speed_i); @@ -538,18 +470,18 @@ inline struct EcefCoor_f StateGetSpeedEcef_f(void) { } else { /* could not get this representation, set errno */ - struct EcefCoor_f _ecef_zero = {0.0f}; - return _ecef_zero; + //struct EcefCoor_f _ecef_zero = {0.0f}; + //return _ecef_zero; } /* set bit to indicate this representation is computed */ SetBit(state.speed_status, SPEED_ECEF_F); - return state.ecef_speed_f; + //return state.ecef_speed_f; } -inline float StateGetHorizontalSpeedNorm_f(void) { //TODO - if (!bit_is_set(state.speed_status, SPEED_HNORM_F)) - return state.h_speed_norm_f;return state.h_speed_norm_f; +void StateCalcHorizontalSpeedNorm_f(void) { //TODO + if (bit_is_set(state.speed_status, SPEED_HNORM_F)) + return; if (bit_is_set(state.speed_status, SPEED_HNORM_I)){ state.h_speed_norm_f = SPEED_FLOAT_OF_BFP(state.h_speed_norm_i); @@ -559,12 +491,12 @@ inline float StateGetHorizontalSpeedNorm_f(void) { //TODO /* set bit to indicate this representation is computed */ SetBit(state.speed_status, SPEED_HNORM_F); - return state.h_speed_norm_f; + //return state.h_speed_norm_f; } -inline float StateGetHorizontalSpeedDir_f(void) { //TODO +void StateCalcHorizontalSpeedDir_f(void) { //TODO if (bit_is_set(state.speed_status, SPEED_HDIR_F)) - return state.h_speed_dir_f; + return; if (bit_is_set(state.speed_status, SPEED_HDIR_I)){ state.h_speed_dir_f = SPEED_FLOAT_OF_BFP(state.h_speed_dir_i); @@ -574,7 +506,7 @@ inline float StateGetHorizontalSpeedDir_f(void) { //TODO /* set bit to indicate this representation is computed */ SetBit(state.speed_status, SPEED_HDIR_F); - return state.h_speed_dir_f; + //return state.h_speed_dir_f; } /** @}*/ @@ -582,40 +514,15 @@ inline float StateGetHorizontalSpeedDir_f(void) { //TODO /****************************************************************************** * * - * Set and Get functions for the ACCELERATION representations * + * Transformation functions for the ACCELERATION representations * * * *****************************************************************************/ /** @addtogroup AccelGroup * @{ */ -/************************ Set functions ****************************/ -inline void StateSetAccelNed_i(struct NedCoor_i* ned_accel) { - INT32_VECT3_COPY(state.ned_accel_i, *ned_accel); - /* clear bits for all accel representations and only set the new one */ - state.accel_status = (1 << ACCEL_NED_I); -} - -inline void StateSetAccelEcef_i(struct EcefCoor_i* ecef_accel) { - INT32_VECT3_COPY(state.ecef_accel_i, *ecef_accel); - /* clear bits for all accel representations and only set the new one */ - state.accel_status = (1 << ACCEL_ECEF_I); -} - -inline void StateSetAccelNed_f(struct NedCoor_f* ned_accel) { - VECT3_COPY(state.ned_accel_f, *ned_accel); - /* clear bits for all accel representations and only set the new one */ - state.accel_status = (1 << ACCEL_NED_F); -} - -inline void StateSetAccelEcef_f(struct EcefCoor_f* ecef_accel) { - VECT3_COPY(state.ecef_accel_f, *ecef_accel); - /* clear bits for all accel representations and only set the new one */ - state.accel_status = (1 << ACCEL_ECEF_F); -} -/************************ Get functions ****************************/ -inline struct NedCoor_i StateGetAccelNed_i(void) { +void StateCalcAccelNed_i(void) { if (bit_is_set(state.accel_status, ACCEL_NED_I)) - return state.ned_accel_i; + return; if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) { if (state.ned_initialised_i) { @@ -627,12 +534,12 @@ inline struct NedCoor_i StateGetAccelNed_i(void) { /* set bit to indicate this representation is computed */ SetBit(state.accel_status, ACCEL_NED_I); - return state.ned_accel_i; + //return state.ned_accel_i; } -inline struct EcefCoor_i StateGetAccelEcef_i(void) { +void StateCalcAccelEcef_i(void) { if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) - return state.ecef_accel_i; + return; if (bit_is_set(state.accel_status, ACCEL_NED_I)) { if (state.ned_initialised_i) { @@ -644,12 +551,12 @@ inline struct EcefCoor_i StateGetAccelEcef_i(void) { /* set bit to indicate this representation is computed */ SetBit(state.accel_status, ACCEL_ECEF_I); - return state.ecef_accel_i; + //return state.ecef_accel_i; } -inline struct NedCoor_f StateGetAccelNed_f(void) { +void StateCalcAccelNed_f(void) { if (bit_is_set(state.accel_status, ACCEL_NED_F)) - return state.ned_accel_f; + return; if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) { if (state.ned_initialised_f) { @@ -661,12 +568,12 @@ inline struct NedCoor_f StateGetAccelNed_f(void) { /* set bit to indicate this representation is computed */ SetBit(state.accel_status, ACCEL_NED_F); - return state.ned_accel_f; + //return state.ned_accel_f; } -inline struct EcefCoor_f StateGetAccelEcef_f(void) { +void StateCalcAccelEcef_f(void) { if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) - return state.ecef_accel_f; + return; if (bit_is_set(state.accel_status, ACCEL_NED_F)) { //ecef_of_ned_vect_f(&state.ecef_accel_f, &state.ned_origin_f, &state.ned_accel_f); @@ -676,7 +583,7 @@ inline struct EcefCoor_f StateGetAccelEcef_f(void) { /* set bit to indicate this representation is computed */ SetBit(state.accel_status, ACCEL_ECEF_F); - return state.ecef_accel_f; + //return state.ecef_accel_f; } /** @}*/ @@ -684,50 +591,16 @@ inline struct EcefCoor_f StateGetAccelEcef_f(void) { /****************************************************************************** * * - * Set and Get functions for the ATTITUDE representations * + * Transformation functions for the ATTITUDE representations * * * *****************************************************************************/ /** @addtogroup AttGroup * @{ */ -/************************ Set functions ****************************/ -inline void StateSetNedToBodyQuat_i(struct Int32Quat* ned_to_body_quat) { - QUAT_COPY(state.ned_to_body_quat_i, *ned_to_body_quat); - /* clear bits for all attitude representations and only set the new one */ - state.att_status = (1 << ATT_QUAT_I); -} - -inline void StateSetNedToBodyRMat_i(struct Int32RMat* ned_to_body_rmat) { - RMAT_COPY(state.ned_to_body_rmat_i, *ned_to_body_rmat); - /* clear bits for all attitude representations and only set the new one */ - state.att_status = (1 << ATT_RMAT_I); -} - -inline void StateSetNedToBodyEulers_i(struct Int32Eulers* ned_to_body_eulers) { - EULERS_COPY(state.ned_to_body_eulers_i, *ned_to_body_eulers); - /* clear bits for all attitude representations and only set the new one */ - state.att_status = (1 << ATT_EULER_I); -} - -inline void StateSetNedToBodyQuat_f(struct FloatQuat* ned_to_body_quat) { - QUAT_COPY(state.ned_to_body_quat_f, *ned_to_body_quat); - /* clear bits for all attitude representations and only set the new one */ - state.att_status = (1 << ATT_QUAT_F); -} - -inline void StateSetNedToBodyRMat_f(struct FloatRMat* ned_to_body_rmat) { - RMAT_COPY(state.ned_to_body_rmat_f, *ned_to_body_rmat); - /* clear bits for all attitude representations and only set the new one */ - state.att_status = (1 << ATT_RMAT_F); -} -inline void StateSetNedToBodyEulers_f(struct FloatEulers* ned_to_body_eulers) { - EULERS_COPY(state.ned_to_body_eulers_f, *ned_to_body_eulers); - /* clear bits for all attitude representations and only set the new one */ - state.att_status = (1 << ATT_EULER_F); -} +void StateCalcNedToBodyQuat_i(void) { + if (bit_is_set(state.att_status, ATT_QUAT_I)) + return; -/************************ Get functions ****************************/ -inline struct Int32Quat StateGetNedToBodyQuat_i(void) { if (!bit_is_set(state.att_status, ATT_QUAT_I)) { if (bit_is_set(state.att_status, ATT_RMAT_I)) { INT32_QUAT_OF_RMAT(state.ned_to_body_quat_i, state.ned_to_body_rmat_i); @@ -739,12 +612,12 @@ inline struct Int32Quat StateGetNedToBodyQuat_i(void) { /* set bit to indicate this representation is computed */ SetBit(state.att_status, ATT_QUAT_I); } - return state.ned_to_body_quat_i; + //return state.ned_to_body_quat_i; } -inline struct Int32RMat StateGetNedToBodyRMat_i(void) { +void StateCalcNedToBodyRMat_i(void) { if (bit_is_set(state.att_status, ATT_RMAT_I)) - return state.ned_to_body_rmat_i; + return; if (bit_is_set(state.att_status, ATT_QUAT_I)) { INT32_RMAT_OF_QUAT(state.ned_to_body_rmat_i, state.ned_to_body_quat_i); @@ -756,12 +629,12 @@ inline struct Int32RMat StateGetNedToBodyRMat_i(void) { /* set bit to indicate this representation is computed */ SetBit(state.att_status, ATT_RMAT_I); - return state.ned_to_body_rmat_i; + //return state.ned_to_body_rmat_i; } -inline struct Int32Eulers StateGetNedToBodyEulers_i(void) { +void StateCalcNedToBodyEulers_i(void) { if (bit_is_set(state.att_status, ATT_EULER_I)) - return state.ned_to_body_eulers_i; + return; if (bit_is_set(state.att_status, ATT_QUAT_I)) { INT32_EULERS_OF_QUAT(state.ned_to_body_eulers_i, state.ned_to_body_quat_i); @@ -773,12 +646,12 @@ inline struct Int32Eulers StateGetNedToBodyEulers_i(void) { /* set bit to indicate this representation is computed */ SetBit(state.att_status, ATT_EULER_I); - return state.ned_to_body_eulers_i; + //return state.ned_to_body_eulers_i; } -inline struct FloatQuat StateGetNedToBodyQuat_f(void) { +void StateCalcNedToBodyQuat_f(void) { if (bit_is_set(state.att_status, ATT_QUAT_F)) - return state.ned_to_body_quat_f; + return; if (bit_is_set(state.att_status, ATT_RMAT_F)) { FLOAT_QUAT_OF_RMAT(state.ned_to_body_quat_f, state.ned_to_body_rmat_f); @@ -790,12 +663,12 @@ inline struct FloatQuat StateGetNedToBodyQuat_f(void) { /* set bit to indicate this representation is computed */ SetBit(state.att_status, ATT_QUAT_F); - return state.ned_to_body_quat_f; + //return state.ned_to_body_quat_f; } -inline struct FloatRMat StateGetNedToBodyRMat_f(void) { +void StateCalcNedToBodyRMat_f(void) { if (bit_is_set(state.att_status, ATT_RMAT_F)) - return state.ned_to_body_rmat_f; + return; if (bit_is_set(state.att_status, ATT_QUAT_F)) { FLOAT_RMAT_OF_QUAT(state.ned_to_body_rmat_f, state.ned_to_body_quat_f); @@ -807,12 +680,12 @@ inline struct FloatRMat StateGetNedToBodyRMat_f(void) { /* set bit to indicate this representation is computed */ SetBit(state.att_status, ATT_RMAT_F); - return state.ned_to_body_rmat_f; + //return state.ned_to_body_rmat_f; } -inline struct FloatEulers StateGetNedToBodyEulers_f(void) { +void StateCalcNedToBodyEulers_f(void) { if (bit_is_set(state.att_status, ATT_EULER_F)) - return state.ned_to_body_eulers_f; + return; if (bit_is_set(state.att_status, ATT_QUAT_F)) { FLOAT_EULERS_OF_QUAT(state.ned_to_body_eulers_f, state.ned_to_body_quat_f); @@ -824,36 +697,22 @@ inline struct FloatEulers StateGetNedToBodyEulers_f(void) { /* set bit to indicate this representation is computed */ SetBit(state.att_status, ATT_EULER_F); - return state.ned_to_body_eulers_f; + //return state.ned_to_body_eulers_f; } /** @}*/ /****************************************************************************** * * - * Set and Get functions for the ANGULAR RATE representations * + * Transformation functions for the ANGULAR RATE representations * * * *****************************************************************************/ /** @addtogroup RateGroup * @{ */ -/************************ Set functions ****************************/ -inline void StateSetBodyRates_i(struct Int32Rates* body_rate) { - RATES_COPY(state.body_rates_i, *body_rate); - /* clear bits for all attitude representations and only set the new one */ - state.rate_status = (1 << RATE_I); -} - -inline void StateSetBodyRates_f(struct FloatRates* body_rate) { - RATES_COPY(state.body_rates_f, *body_rate); - /* clear bits for all attitude representations and only set the new one */ - state.rate_status = (1 << RATE_F); -} - -/************************ Get functions ****************************/ -inline struct Int32Rates StateGetBodyRates_i(void) { +void StateCalcBodyRates_i(void) { if (bit_is_set(state.rate_status, RATE_I)) - return state.body_rates_i; + return; if (bit_is_set(state.rate_status, RATE_F)) { RATES_BFP_OF_REAL(state.body_rates_i, state.body_rates_f); @@ -861,12 +720,12 @@ inline struct Int32Rates StateGetBodyRates_i(void) { /* set bit to indicate this representation is computed */ SetBit(state.rate_status, RATE_I); - return state.body_rates_i; + //return state.body_rates_i; } -inline struct FloatRates StateGetBodyRates_f(void) { +void StateCalcBodyRates_f(void) { if (bit_is_set(state.rate_status, RATE_F)) - return state.body_rates_f; + return; if (bit_is_set(state.rate_status, RATE_I)) { RATES_FLOAT_OF_BFP(state.body_rates_f, state.body_rates_i); @@ -874,51 +733,23 @@ inline struct FloatRates StateGetBodyRates_f(void) { /* set bit to indicate this representation is computed */ SetBit(state.rate_status, RATE_F); - return state.body_rates_f; + //return state.body_rates_f; } + /** @}*/ /****************************************************************************** * * - * Set and Get functions for the WIND- AND AIRSPEED representations * + * Transformation functions for the WIND- AND AIRSPEED representations * * * *****************************************************************************/ /** @addtogroup WindAirGroup * @{ */ -/************************ Set functions ****************************/ -inline void StateSetHorizontalWindspeed_i(struct Int32Vect2* h_windspeed) { - VECT2_COPY(state.h_windspeed_i, *h_windspeed); - /* clear bits for all windspeed representations and only set the new one */ - ClearBit(state.wind_air_status, WINDSPEED_F); - SetBit(state.wind_air_status, WINDSPEED_I); -} - -inline void StateSetAirspeed_i(int32_t* airspeed) { - state.airspeed_i = *airspeed; - /* clear bits for all windspeed representations and only set the new one */ - ClearBit(state.wind_air_status, AIRSPEED_F); - SetBit(state.wind_air_status, AIRSPEED_I); -} - -inline void StateSetHorizontalWindspeed_f(struct FloatVect2* h_windspeed) { - VECT2_COPY(state.h_windspeed_f, *h_windspeed); - /* clear bits for all windspeed representations and only set the new one */ - ClearBit(state.wind_air_status, WINDSPEED_I); - SetBit(state.wind_air_status, WINDSPEED_F); -} - -inline void StateSetAirspeed_f(float* airspeed) { - state.airspeed_f = *airspeed; - /* clear bits for all windspeed representations and only set the new one */ - ClearBit(state.wind_air_status, AIRSPEED_I); - SetBit(state.wind_air_status, AIRSPEED_F); -} -/************************ Get functions ****************************/ -inline struct Int32Vect2 StateGetHorizontalWindspeed_i(void) { +void StateCalcHorizontalWindspeed_i(void) { if (bit_is_set(state.wind_air_status, WINDSPEED_I)) - return state.h_windspeed_i; + return; if (bit_is_set(state.wind_air_status, WINDSPEED_F)) { state.h_windspeed_i.x = SPEED_BFP_OF_REAL(state.h_windspeed_f.x); @@ -927,12 +758,12 @@ inline struct Int32Vect2 StateGetHorizontalWindspeed_i(void) { /* set bit to indicate this representation is computed */ SetBit(state.rate_status, WINDSPEED_I); - return state.h_windspeed_i; + //return state.h_windspeed_i; } -inline int32_t StateGetAirspeed_i(void) { +void StateCalcAirspeed_i(void) { if (bit_is_set(state.wind_air_status, AIRSPEED_I)) - return state.airspeed_i; + return; if (bit_is_set(state.wind_air_status, AIRSPEED_F)) { state.airspeed_i = SPEED_BFP_OF_REAL(state.airspeed_f); @@ -940,12 +771,12 @@ inline int32_t StateGetAirspeed_i(void) { /* set bit to indicate this representation is computed */ SetBit(state.wind_air_status, AIRSPEED_I); - return state.airspeed_i; + //return state.airspeed_i; } -inline struct FloatVect2 StateGetHorizontalWindspeed_f(void) { +void StateCalcHorizontalWindspeed_f(void) { if (bit_is_set(state.wind_air_status, WINDSPEED_F)) - return state.h_windspeed_f; + return; if (bit_is_set(state.wind_air_status, WINDSPEED_I)) { state.h_windspeed_f.x = SPEED_FLOAT_OF_BFP(state.h_windspeed_i.x); @@ -954,12 +785,12 @@ inline struct FloatVect2 StateGetHorizontalWindspeed_f(void) { /* set bit to indicate this representation is computed */ SetBit(state.rate_status, WINDSPEED_F); - return state.h_windspeed_f; + //return state.h_windspeed_f; } -inline float StateGetAirspeed_f(void) { +void StateCalcAirspeed_f(void) { if (bit_is_set(state.wind_air_status, AIRSPEED_F)) - return state.airspeed_f; + return; if (bit_is_set(state.wind_air_status, AIRSPEED_I)) { state.airspeed_f = SPEED_FLOAT_OF_BFP(state.airspeed_i); @@ -967,6 +798,6 @@ inline float StateGetAirspeed_f(void) { /* set bit to indicate this representation is computed */ SetBit(state.wind_air_status, AIRSPEED_F); - return state.airspeed_f; + //return state.airspeed_f; } /** @}*/ diff --git a/sw/airborne/state.h b/sw/airborne/state.h index 9e55aaff4a7..8775ec4cd7f 100644 --- a/sw/airborne/state.h +++ b/sw/airborne/state.h @@ -389,236 +389,544 @@ extern struct State state; -/******************************************************************* - * Set and Get functions for the POSITION representations - *******************************************************************/ +/******************************************************************************* + * * + * Set and Get functions for the POSITION representations * + * * + ******************************************************************************/ /** @addtogroup PosGroup * @{ */ + +/************* declaration of transformation functions ************/ +extern void StateCalcPositionEcef_i(void); +extern void StateCalcPositionNed_i(void); +extern void StateCalcPositionLla_i(void); +extern void StateCalcPositionEcef_f(void); +extern void StateCalcPositionNed_f(void); +extern void StateCalcPositionLla_f(void); + /************************ Set functions ****************************/ + /** @brief Set position from ECEF coordinates (int). */ -inline void StateSetPositionEcef_i(struct EcefCoor_i* ecef_pos); +static inline void StateSetPositionEcef_i(struct EcefCoor_i* ecef_pos) { + INT32_VECT3_COPY(state.ecef_pos_i, *ecef_pos); + /* clear bits for all position representations and only set the new one */ + state.pos_status = (1 << POS_ECEF_I); +} /** @brief Set position from local NED coordinates (int). */ -inline void StateSetPositionNed_i(struct NedCoor_i* ned_pos); +static inline void StateSetPositionNed_i(struct NedCoor_i* ned_pos) { + INT32_VECT3_COPY(state.ned_pos_i, *ned_pos); + /* clear bits for all position representations and only set the new one */ + state.pos_status = (1 << POS_NED_I); +} /** @brief Set position from LLA coordinates (int). */ -inline void StateSetPositionLla_i(struct LlaCoor_i* lla_pos); +static inline void StateSetPositionLla_i(struct LlaCoor_i* lla_pos) { + LLA_COPY(state.lla_pos_i, *lla_pos); + /* clear bits for all position representations and only set the new one */ + state.pos_status = (1 << POS_LLA_I); +} /** @brief Set position from UTM coordinates (float). */ -inline void StateSetPositionUtm_f(struct FloatVect3* utm_pos); +static inline void StateSetPositionUtm_f(struct FloatVect3* utm_pos) { + //TODO utm zone?? + VECT3_COPY(state.utm_pos_f, *utm_pos); + /* clear bits for all position representations and only set the new one */ + state.pos_status = (uint8_t)(1 << POS_UTM_F); +} /** @brief Set position from ECEF coordinates (float). */ -inline void StateSetPositionEcef_f(struct EcefCoor_f* ecef_pos); +static inline void StateSetPositionEcef_f(struct EcefCoor_f* ecef_pos) { + VECT3_COPY(state.ecef_pos_f, *ecef_pos); + /* clear bits for all position representations and only set the new one */ + state.pos_status = (1 << POS_ECEF_F); +} /** @brief Set position from local NED coordinates (float). */ -inline void StateSetPositionNed_f(struct NedCoor_f* ned_pos); +static inline void StateSetPositionNed_f(struct NedCoor_f* ned_pos) { + VECT3_COPY(state.ned_pos_f, *ned_pos); + /* clear bits for all position representations and only set the new one */ + state.pos_status = (1 << POS_NED_F); +} /** @brief Set position from LLA coordinates (float). */ -inline void StateSetPositionLla_f(struct LlaCoor_f* lla_pos); +static inline void StateSetPositionLla_f(struct LlaCoor_f* lla_pos) { + LLA_COPY(state.lla_pos_f, *lla_pos); + /* clear bits for all position representations and only set the new one */ + state.pos_status = (1 << POS_LLA_F); +} /************************ Get functions ****************************/ + /** @brief Get position in ECEF coordinates (int). */ -inline struct EcefCoor_i StateGetPositionEcef_i(void); +static inline struct EcefCoor_i StateGetPositionEcef_i(void) { + if (!bit_is_set(state.pos_status, POS_ECEF_I)) + StateCalcPositionEcef_i(); + return state.ecef_pos_i; +} /** @brief Get position in local NED coordinates (int). */ -inline struct NedCoor_i StateGetPositionNed_i(void); +static inline struct NedCoor_i StateGetPositionNed_i(void) { + if (!bit_is_set(state.pos_status, POS_NED_I)) + StateCalcPositionNed_i(); + return state.ned_pos_i; +} /** @brief Get position in LLA coordinates (int). */ -inline struct LlaCoor_i StateGetPositionLla_i(void); +static inline struct LlaCoor_i StateGetPositionLla_i(void) { + if (!bit_is_set(state.pos_status, POS_LLA_I)) + StateCalcPositionLla_i(); + return state.lla_pos_i; +} /** @brief Get position in UTM coordinates (float). */ -inline struct FloatVect3 StateGetPositionUtm_f(void); +//static inline struct FloatVect3 StateGetPositionUtm_f(void); /** @brief Get position in ECEF coordinates (float). */ -inline struct EcefCoor_f StateGetPositionEcef_f(void); +static inline struct EcefCoor_f StateGetPositionEcef_f(void) { + if (!bit_is_set(state.pos_status, POS_ECEF_F)) + StateCalcPositionEcef_f(); + return state.ecef_pos_f; +} /** @brief Get position in local NED coordinates (float). */ -inline struct NedCoor_f StateGetPositionNed_f(void); +static inline struct NedCoor_f StateGetPositionNed_f(void) { + if (!bit_is_set(state.pos_status, POS_NED_F)) + StateCalcPositionNed_f(); + return state.ned_pos_f; +} /** @brief Get position in LLA coordinates (float). */ -inline struct LlaCoor_f StateGetPositionLla_f(void); +static inline struct LlaCoor_f StateGetPositionLla_f(void) { + if (!bit_is_set(state.pos_status, POS_LLA_F)) + StateCalcPositionLla_f(); + return state.lla_pos_f; +} + /** @}*/ -/******************************************************************* - * Set and Get functions for the SPEED representations - *******************************************************************/ +/****************************************************************************** + * * + * Set and Get functions for the SPEED representations * + * * + *****************************************************************************/ /** @addtogroup SpeedGroup * @{ */ + +/************* declaration of transformation functions ************/ +extern void StateCalcSpeedNed_i(void); +extern void StateCalcSpeedEcef_i(void); +extern void StateCalcHorizontalSpeedNorm_i(void); +extern void StateCalcHorizontalSpeedDir_i(void); +extern void StateCalcSpeedNed_f(void); +extern void StateCalcSpeedEcef_f(void); +extern void StateCalcHorizontalSpeedNorm_f(void); +extern void StateCalcHorizontalSpeedDir_f(void); + /************************ Set functions ****************************/ + /** @brief Set ground speed in local NED coordinates (int). */ -inline void StateSetSpeedNed_i(struct NedCoor_i* ned_speed); +static inline void StateSetSpeedNed_i(struct NedCoor_i* ned_speed) { + INT32_VECT3_COPY(state.ned_speed_i, *ned_speed); + /* clear bits for all speed representations and only set the new one */ + state.speed_status = (1 << SPEED_NED_I); +} /** @brief Set ground speed in ECEF coordinates (int). */ -inline void StateSetSpeedEcef_i(struct EcefCoor_i* ecef_speed); +static inline void StateSetSpeedEcef_i(struct EcefCoor_i* ecef_speed) { + INT32_VECT3_COPY(state.ecef_speed_i, *ecef_speed); + /* clear bits for all speed representations and only set the new one */ + state.speed_status = (1 << SPEED_ECEF_I); +} /** @brief Set ground speed in local NED coordinates (float). */ -inline void StateSetSpeedNed_f(struct NedCoor_f* ned_speed); +static inline void StateSetSpeedNed_f(struct NedCoor_f* ned_speed) { + VECT3_COPY(state.ned_speed_f, *ned_speed); + /* clear bits for all speed representations and only set the new one */ + state.speed_status = (1 << SPEED_NED_F); +} /** @brief Set ground speed in ECEF coordinates (float). */ -inline void StateSetSpeedEcef_f(struct EcefCoor_f* ecef_speed); +static inline void StateSetSpeedEcef_f(struct EcefCoor_f* ecef_speed) { + VECT3_COPY(state.ecef_speed_f, *ecef_speed); + /* clear bits for all speed representations and only set the new one */ + state.speed_status = (1 << SPEED_ECEF_F); +} /************************ Get functions ****************************/ + /** @brief Get ground speed in local NED coordinates (int). */ -inline struct NedCoor_i StateGetSpeedNed_i(void); +static inline struct NedCoor_i StateGetSpeedNed_i(void) { + if (!bit_is_set(state.speed_status, SPEED_NED_I)) + StateCalcSpeedNed_i(); + return state.ned_speed_i; +} /** @brief Get ground speed in ECEF coordinates (int). */ -inline struct EcefCoor_i StateGetSpeedEcef_i(void); +static inline struct EcefCoor_i StateGetSpeedEcef_i(void) { + if (!bit_is_set(state.speed_status, SPEED_ECEF_I)) + StateCalcSpeedEcef_i(); + return state.ecef_speed_i; +} /** @brief Get norm of horizontal ground speed (int). */ -inline int32_t StateGetHorizontalSpeedNorm_i(void); +static inline int32_t StateGetHorizontalSpeedNorm_i(void) { + if (!bit_is_set(state.speed_status, SPEED_HNORM_I)) + StateCalcHorizontalSpeedNorm_i(); + return state.h_speed_norm_i; +} /** @brief Get dir of horizontal ground speed (int). */ -inline int32_t StateGetHorizontalSpeedDir_i(void); +static inline int32_t StateGetHorizontalSpeedDir_i(void) { + if (!bit_is_set(state.speed_status, SPEED_HDIR_I)) + StateCalcHorizontalSpeedDir_i(); + return state.h_speed_dir_i; +} /** @brief Get ground speed in local NED coordinates (float). */ -inline struct NedCoor_f StateGetSpeedNed_f(void); +static inline struct NedCoor_f StateGetSpeedNed_f(void) { + if (!bit_is_set(state.speed_status, SPEED_NED_F)) + StateCalcSpeedNed_f(); + return state.ned_speed_f; +} /** @brief Get ground speed in ECEF coordinates (float). */ -inline struct EcefCoor_f StateGetSpeedEcef_f(void); +static inline struct EcefCoor_f StateGetSpeedEcef_f(void) { + if (!bit_is_set(state.speed_status, SPEED_ECEF_F)) + StateCalcSpeedEcef_f(); + return state.ecef_speed_f; +} /** @brief Get norm of horizontal ground speed (float). */ -inline float StateGetHorizontalSpeedNorm_f(void); +static inline float StateGetHorizontalSpeedNorm_f(void) { + if (!bit_is_set(state.speed_status, SPEED_HNORM_F)) + StateCalcHorizontalSpeedNorm_f(); + return state.h_speed_norm_f; +} /** @brief Get dir of horizontal ground speed (float). */ -inline float StateGetHorizontalSpeedDir_f(void); +static inline float StateGetHorizontalSpeedDir_f(void) { + if (!bit_is_set(state.speed_status, SPEED_HDIR_F)) + StateCalcHorizontalSpeedDir_f(); + return state.h_speed_dir_f; +} /** @}*/ -/******************************************************************* - * Set and Get functions for the ACCELERATION representations - *******************************************************************/ +/****************************************************************************** + * * + * Set and Get functions for the ACCELERATION representations * + * * + *****************************************************************************/ /** @addtogroup AccelGroup * @{ */ + +/************* declaration of transformation functions ************/ +extern void StateCalcAccelNed_i(void); +extern void StateCalcAccelEcef_i(void); +extern void StateCalcAccelNed_f(void); +extern void StateCalcAccelEcef_f(void); + /************************ Set functions ****************************/ + /** @brief Set acceleration in NED coordinates (int). */ -inline void StateSetAccelNed_i(struct NedCoor_i* ned_accel); +static inline void StateSetAccelNed_i(struct NedCoor_i* ned_accel) { + INT32_VECT3_COPY(state.ned_accel_i, *ned_accel); + /* clear bits for all accel representations and only set the new one */ + state.accel_status = (1 << ACCEL_NED_I); +} /** @brief Set acceleration in ECEF coordinates (int). */ -inline void StateSetAccelEcef_i(struct EcefCoor_i* ecef_accel); +static inline void StateSetAccelEcef_i(struct EcefCoor_i* ecef_accel) { + INT32_VECT3_COPY(state.ecef_accel_i, *ecef_accel); + /* clear bits for all accel representations and only set the new one */ + state.accel_status = (1 << ACCEL_ECEF_I); +} /** @brief Set acceleration in NED coordinates (float). */ -inline void StateSetAccelNed_f(struct NedCoor_f* ned_accel); +static inline void StateSetAccelNed_f(struct NedCoor_f* ned_accel) { + VECT3_COPY(state.ned_accel_f, *ned_accel); + /* clear bits for all accel representations and only set the new one */ + state.accel_status = (1 << ACCEL_NED_F); +} /** @brief Set acceleration in ECEF coordinates (float). */ -inline void StateSetAccelEcef_f(struct EcefCoor_f* ecef_accel); +static inline void StateSetAccelEcef_f(struct EcefCoor_f* ecef_accel) { + VECT3_COPY(state.ecef_accel_f, *ecef_accel); + /* clear bits for all accel representations and only set the new one */ + state.accel_status = (1 << ACCEL_ECEF_F); +} /************************ Get functions ****************************/ + /** @brief Get acceleration in NED coordinates (int). */ -inline struct NedCoor_i StateGetAccelNed_i(void); +static inline struct NedCoor_i StateGetAccelNed_i(void) { + if (!bit_is_set(state.accel_status, ACCEL_NED_I)) + StateCalcAccelNed_i(); + return state.ned_accel_i; +} /** @brief Get acceleration in ECEF coordinates (int). */ -inline struct EcefCoor_i StateGetAccelEcef_i(void); +static inline struct EcefCoor_i StateGetAccelEcef_i(void) { + if (!bit_is_set(state.accel_status, ACCEL_ECEF_I)) + StateCalcAccelEcef_i(); + return state.ecef_accel_i; +} /** @brief Get acceleration in NED coordinates (float). */ -inline struct NedCoor_f StateGetAccelNed_f(void); +static inline struct NedCoor_f StateGetAccelNed_f(void) { + if (bit_is_set(state.accel_status, ACCEL_NED_F)) + StateCalcAccelNed_f(); + return state.ned_accel_f; +} /** @brief Get acceleration in ECEF coordinates (float). */ -inline struct EcefCoor_f StateGetAccelEcef_f(void); +static inline struct EcefCoor_f StateGetAccelEcef_f(void) { + if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) + StateCalcAccelEcef_f(); + return state.ecef_accel_f; +} /** @}*/ -/******************************************************************* - * Set and Get functions for the ATTITUDE representations - *******************************************************************/ +/****************************************************************************** + * * + * Set and Get functions for the ATTITUDE representations * + * * + *****************************************************************************/ /** @addtogroup AttGroup * @{ */ + +/************* declaration of transformation functions ************/ +extern void StateCalcNedToBodyQuat_i(void); +extern void StateCalcNedToBodyRMat_i(void); +extern void StateCalcNedToBodyEulers_i(void); +extern void StateCalcNedToBodyQuat_f(void); +extern void StateCalcNedToBodyRMat_f(void); +extern void StateCalcNedToBodyEulers_f(void); + /************************ Set functions ****************************/ + /** @brief Set vehicle body attitude from quaternion (int). */ -inline void StateSetNedToBodyQuat_i(struct Int32Quat* ned_to_body_quat); +static inline void StateSetNedToBodyQuat_i(struct Int32Quat* ned_to_body_quat) { + QUAT_COPY(state.ned_to_body_quat_i, *ned_to_body_quat); + /* clear bits for all attitude representations and only set the new one */ + state.att_status = (1 << ATT_QUAT_I); +} /** @brief Set vehicle body attitude from rotation matrix (int). */ -inline void StateSetNedToBodyRMat_i(struct Int32RMat* ned_to_body_rmat); +static inline void StateSetNedToBodyRMat_i(struct Int32RMat* ned_to_body_rmat) { + RMAT_COPY(state.ned_to_body_rmat_i, *ned_to_body_rmat); + /* clear bits for all attitude representations and only set the new one */ + state.att_status = (1 << ATT_RMAT_I); +} /** @brief Set vehicle body attitude from euler angles (int). */ -inline void StateSetNedToBodyEulers_i(struct Int32Eulers* ned_to_body_eulers); +static inline void StateSetNedToBodyEulers_i(struct Int32Eulers* ned_to_body_eulers) { + EULERS_COPY(state.ned_to_body_eulers_i, *ned_to_body_eulers); + /* clear bits for all attitude representations and only set the new one */ + state.att_status = (1 << ATT_EULER_I); +} /** @brief Set vehicle body attitude from quaternion (float). */ -inline void StateSetNedToBodyQuat_f(struct FloatQuat* ned_to_body_quat); +static inline void StateSetNedToBodyQuat_f(struct FloatQuat* ned_to_body_quat) { + QUAT_COPY(state.ned_to_body_quat_f, *ned_to_body_quat); + /* clear bits for all attitude representations and only set the new one */ + state.att_status = (1 << ATT_QUAT_F); +} /** @brief Set vehicle body attitude from rotation matrix (float). */ -inline void StateSetNedToBodyRMat_f(struct FloatRMat* ned_to_body_rmat); +static inline void StateSetNedToBodyRMat_f(struct FloatRMat* ned_to_body_rmat) { + RMAT_COPY(state.ned_to_body_rmat_f, *ned_to_body_rmat); + /* clear bits for all attitude representations and only set the new one */ + state.att_status = (1 << ATT_RMAT_F); +} /** @brief Set vehicle body attitude from euler angles (float). */ -inline void StateSetNedToBodyEulers_f(struct FloatEulers* ned_to_body_eulers); +static inline void StateSetNedToBodyEulers_f(struct FloatEulers* ned_to_body_eulers) { + EULERS_COPY(state.ned_to_body_eulers_f, *ned_to_body_eulers); + /* clear bits for all attitude representations and only set the new one */ + state.att_status = (1 << ATT_EULER_F); +} /************************ Get functions ****************************/ + /** @brief Get vehicle body attitude quaternion (int). */ -inline struct Int32Quat StateGetNedToBodyQuat_i(void); +static inline struct Int32Quat StateGetNedToBodyQuat_i(void) { + if (!bit_is_set(state.att_status, ATT_QUAT_I)) + StateCalcNedToBodyQuat_i(); + return state.ned_to_body_quat_i; +} /** @brief Get vehicle body attitude rotation matrix (int). */ -inline struct Int32RMat StateGetNedToBodyRMat_i(void); +static inline struct Int32RMat StateGetNedToBodyRMat_i(void) { + if (!bit_is_set(state.att_status, ATT_RMAT_I)) + StateCalcNedToBodyRMat_i(); + return state.ned_to_body_rmat_i; +} /** @brief Get vehicle body attitude euler angles (int). */ -inline struct Int32Eulers StateGetNedToBodyEulers_i(void); +static inline struct Int32Eulers StateGetNedToBodyEulers_i(void) { + if (!bit_is_set(state.att_status, ATT_EULER_I)) + StateCalcNedToBodyEulers_i(); + return state.ned_to_body_eulers_i; +} /** @brief Get vehicle body attitude quaternion (float). */ -inline struct FloatQuat StateGetNedToBodyQuat_f(void); +static inline struct FloatQuat StateGetNedToBodyQuat_f(void) { + if (!bit_is_set(state.att_status, ATT_QUAT_F)) + StateCalcNedToBodyQuat_f(); + return state.ned_to_body_quat_f; +} /** @brief Get vehicle body attitude rotation matrix (float). */ -inline struct FloatRMat StateGetNedToBodyRMat_f(void); +static inline struct FloatRMat StateGetNedToBodyRMat_f(void) { + if (!bit_is_set(state.att_status, ATT_RMAT_F)) + StateCalcNedToBodyRMat_f(); + return state.ned_to_body_rmat_f; +} /** @brief Get vehicle body attitude euler angles (float). */ -inline struct FloatEulers StateGetNedToBodyEulers_f(void); +static inline struct FloatEulers StateGetNedToBodyEulers_f(void) { + if (!bit_is_set(state.att_status, ATT_EULER_F)) + StateCalcNedToBodyEulers_f(); + return state.ned_to_body_eulers_f; +} /** @}*/ -/******************************************************************* - * Set and Get functions for the ANGULAR RATE representations - *******************************************************************/ +/****************************************************************************** + * * + * Set and Get functions for the ANGULAR RATE representations * + * * + *****************************************************************************/ /** @addtogroup RateGroup * @{ */ + +/************* declaration of transformation functions ************/ +extern void StateCalcBodyRates_i(void); +extern void StateCalcBodyRates_f(void); + /************************ Set functions ****************************/ + /** @brief Set vehicle body angular rate (int). */ -inline void StateSetBodyRates_i(struct Int32Rates* body_rate); +static inline void StateSetBodyRates_i(struct Int32Rates* body_rate) { + RATES_COPY(state.body_rates_i, *body_rate); + /* clear bits for all attitude representations and only set the new one */ + state.rate_status = (1 << RATE_I); +} /** @brief Set vehicle body angular rate (float). */ -inline void StateSetBodyRates_f(struct FloatRates* body_rate); +static inline void StateSetBodyRates_f(struct FloatRates* body_rate) { + RATES_COPY(state.body_rates_f, *body_rate); + /* clear bits for all attitude representations and only set the new one */ + state.rate_status = (1 << RATE_F); +} /************************ Get functions ****************************/ + /** @brief Get vehicle body angular rate (int). */ -inline struct Int32Rates StateGetBodyRates_i(void); +static inline struct Int32Rates StateGetBodyRates_i(void) { + if (!bit_is_set(state.rate_status, RATE_I)) + StateCalcBodyRates_i(); + return state.body_rates_i; +} /** @brief Get vehicle body angular rate (float). */ -inline struct FloatRates StateGetBodyRates_f(void); +static inline struct FloatRates StateGetBodyRates_f(void) { + if (!bit_is_set(state.rate_status, RATE_F)) + StateCalcBodyRates_f(); + return state.body_rates_f; +} + /** @}*/ -/******************************************************************* - * Set and Get functions for the WIND- AND AIRSPEED representations - *******************************************************************/ +/****************************************************************************** + * * + * Set and Get functions for the WIND- AND AIRSPEED representations * + * * + *****************************************************************************/ /** @addtogroup WindAirGroup * @{ */ + +/************* declaration of transformation functions ************/ +extern void StateCalcHorizontalWindspeed_i(void); +extern void StateCalcAirspeed_i(void); +extern void StateCalcHorizontalWindspeed_f(void); +extern void StateCalcAirspeed_f(void); + /************************ Set functions ****************************/ + /** @brief Set horizontal windspeed (int). */ -inline void StateSetHorizontalWindspeed_i(struct Int32Vect2* h_windspeed); +static inline void StateSetHorizontalWindspeed_i(struct Int32Vect2* h_windspeed) { + VECT2_COPY(state.h_windspeed_i, *h_windspeed); + /* clear bits for all windspeed representations and only set the new one */ + ClearBit(state.wind_air_status, WINDSPEED_F); + SetBit(state.wind_air_status, WINDSPEED_I); +} /** @brief Set airspeed (int). */ -inline void StateSetAirspeed_i(int32_t* airspeed); +static inline void StateSetAirspeed_i(int32_t* airspeed) { + state.airspeed_i = *airspeed; + /* clear bits for all windspeed representations and only set the new one */ + ClearBit(state.wind_air_status, AIRSPEED_F); + SetBit(state.wind_air_status, AIRSPEED_I); +} /** @brief Set horizontal windspeed (float). */ -inline void StateSetHorizontalWindspeed_f(struct FloatVect2* h_windspeed); +static inline void StateSetHorizontalWindspeed_f(struct FloatVect2* h_windspeed) { + VECT2_COPY(state.h_windspeed_f, *h_windspeed); + /* clear bits for all windspeed representations and only set the new one */ + ClearBit(state.wind_air_status, WINDSPEED_I); + SetBit(state.wind_air_status, WINDSPEED_F); +} /** @brief Set airspeed (float). */ -inline void StateSetAirspeed_f(float* airspeed); +static inline void StateSetAirspeed_f(float* airspeed) { + state.airspeed_f = *airspeed; + /* clear bits for all windspeed representations and only set the new one */ + ClearBit(state.wind_air_status, AIRSPEED_I); + SetBit(state.wind_air_status, AIRSPEED_F); +} /************************ Get functions ****************************/ + /** @brief Get horizontal windspeed (int). */ -inline struct Int32Vect2 StateGetHorizontalWindspeed_i(void); +static inline struct Int32Vect2 StateGetHorizontalWindspeed_i(void) { + if (!bit_is_set(state.wind_air_status, WINDSPEED_I)) + StateCalcHorizontalWindspeed_i(); + return state.h_windspeed_i; +} /** @brief Get airspeed (int). */ -inline int32_t StateGetAirspeed_i(void); +static inline int32_t StateGetAirspeed_i(void) { + if (!bit_is_set(state.wind_air_status, AIRSPEED_I)) + StateCalcAirspeed_i(); + return state.airspeed_i; +} /** @brief Get horizontal windspeed (float). */ -inline struct FloatVect2 StateGetHorizontalWindspeed_f(void); +static inline struct FloatVect2 StateGetHorizontalWindspeed_f(void) { + if (!bit_is_set(state.wind_air_status, WINDSPEED_F)) + StateCalcHorizontalWindspeed_f(); + return state.h_windspeed_f; +} /** @brief Get airspeed (float). */ -inline float StateGetAirspeed_f(void); +static inline float StateGetAirspeed_f(void) { + if (!bit_is_set(state.wind_air_status, AIRSPEED_F)) + StateCalcAirspeed_f(); + return state.airspeed_f; +} + /** @}*/