From 500f32443c19df9dd2e8334ef670457736584071 Mon Sep 17 00:00:00 2001 From: Dirk Dokter Date: Sun, 23 Sep 2012 12:54:40 +0200 Subject: [PATCH] [math] moved automatic orientation calculations to generic pprz_orientation_conversion --- conf/firmwares/rotorcraft.makefile | 2 +- .../subsystems/fixedwing/autopilot.makefile | 2 +- doc/manual/math.dox | 11 + .../math/pprz_orientation_conversion.c | 211 +++++++++++++++++ .../math/pprz_orientation_conversion.h | 223 ++++++++++++++++++ sw/airborne/state.c | 176 +------------- sw/airborne/state.h | 147 ++---------- 7 files changed, 470 insertions(+), 302 deletions(-) create mode 100644 doc/manual/math.dox create mode 100644 sw/airborne/math/pprz_orientation_conversion.c create mode 100644 sw/airborne/math/pprz_orientation_conversion.h diff --git a/conf/firmwares/rotorcraft.makefile b/conf/firmwares/rotorcraft.makefile index a394a3d966..d25efa24d1 100644 --- a/conf/firmwares/rotorcraft.makefile +++ b/conf/firmwares/rotorcraft.makefile @@ -52,7 +52,7 @@ ap.srcs += $(SRC_ARCH)/mcu_arch.c # # Math functions # -ap.srcs += math/pprz_geodetic_int.c math/pprz_geodetic_float.c math/pprz_geodetic_double.c math/pprz_trig_int.c +ap.srcs += math/pprz_geodetic_int.c math/pprz_geodetic_float.c math/pprz_geodetic_double.c math/pprz_trig_int.c math/pprz_orientation_conversion.c ifeq ($(ARCH), stm32) ap.srcs += lisa/plug_sys.c diff --git a/conf/firmwares/subsystems/fixedwing/autopilot.makefile b/conf/firmwares/subsystems/fixedwing/autopilot.makefile index cc49e50191..c3db971434 100644 --- a/conf/firmwares/subsystems/fixedwing/autopilot.makefile +++ b/conf/firmwares/subsystems/fixedwing/autopilot.makefile @@ -87,7 +87,7 @@ $(TARGET).srcs += $(SRC_FIXEDWING)/inter_mcu.c # # Math functions # -$(TARGET).srcs += math/pprz_geodetic_int.c math/pprz_geodetic_float.c math/pprz_geodetic_double.c math/pprz_trig_int.c +$(TARGET).srcs += math/pprz_geodetic_int.c math/pprz_geodetic_float.c math/pprz_geodetic_double.c math/pprz_trig_int.c math/pprz_orientation_conversion.c # # I2C diff --git a/doc/manual/math.dox b/doc/manual/math.dox new file mode 100644 index 0000000000..ea6b31796a --- /dev/null +++ b/doc/manual/math.dox @@ -0,0 +1,11 @@ +/** @page math Math Functions +Add links to specific math docs here... +*/ + +/** + @defgroup math Math Functions +*/ + +/** @file +This file contains the @ref math pages. +*/ diff --git a/sw/airborne/math/pprz_orientation_conversion.c b/sw/airborne/math/pprz_orientation_conversion.c new file mode 100644 index 0000000000..76c37b64a0 --- /dev/null +++ b/sw/airborne/math/pprz_orientation_conversion.c @@ -0,0 +1,211 @@ +/* + * Copyright (C) 2011-2012 The Paparazzi Team + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file pprz_orientation_conversion.h + * + * Generic orientation representation and conversion. + * + * This is for example used in the @ref state_interface "state interface". + * + * @author Felix Ruess + */ + +/** + * @addtogroup math + * @{ + */ +/** + * @addtogroup math_orientation_representation Generic Orientation Representations + * @{ + */ +#include "pprz_orientation_conversion.h" + + +/****************************************************************************** + * * + * Transformation functions for the ORIENTATION representations * + * * + *****************************************************************************/ + +void orientationCalcQuat_i(struct OrientationReps* orientation) { + if (bit_is_set(orientation->status, ORREP_QUAT_I)) + return; + + if (bit_is_set(orientation->status, ORREP_QUAT_F)) { + QUAT_BFP_OF_REAL(orientation->quat_i, orientation->quat_f); + } + else if (bit_is_set(orientation->status, ORREP_RMAT_I)) { + INT32_QUAT_OF_RMAT(orientation->quat_i, orientation->rmat_i); + } + else if (bit_is_set(orientation->status, ORREP_EULER_I)) { + INT32_QUAT_OF_EULERS(orientation->quat_i, orientation->eulers_i); + } + else if (bit_is_set(orientation->status, ORREP_RMAT_F)) { + RMAT_BFP_OF_REAL(orientation->rmat_i, orientation->rmat_f); + SetBit(orientation->status, ORREP_RMAT_I); + INT32_QUAT_OF_RMAT(orientation->quat_i, orientation->rmat_i); + } + else if (bit_is_set(orientation->status, ORREP_EULER_F)) { + EULERS_BFP_OF_REAL(orientation->eulers_i, orientation->eulers_f); + SetBit(orientation->status, ORREP_EULER_I); + INT32_QUAT_OF_EULERS(orientation->quat_i, orientation->eulers_i); + } + /* set bit to indicate this representation is computed */ + SetBit(orientation->status, ORREP_QUAT_I); +} + +void orientationCalcRMat_i(struct OrientationReps* orientation) { + if (bit_is_set(orientation->status, ORREP_RMAT_I)) + return; + + if (bit_is_set(orientation->status, ORREP_RMAT_F)) { + RMAT_BFP_OF_REAL(orientation->rmat_i, orientation->rmat_f); + } + else if (bit_is_set(orientation->status, ORREP_QUAT_I)) { + INT32_RMAT_OF_QUAT(orientation->rmat_i, orientation->quat_i); + } + else if (bit_is_set(orientation->status, ORREP_EULER_I)) { + INT32_RMAT_OF_EULERS(orientation->rmat_i, orientation->eulers_i); + } + else if (bit_is_set(orientation->status, ORREP_QUAT_F)) { + QUAT_BFP_OF_REAL(orientation->quat_i, orientation->quat_f); + SetBit(orientation->status, ORREP_QUAT_I); + INT32_RMAT_OF_QUAT(orientation->rmat_i, orientation->quat_i); + } + else if (bit_is_set(orientation->status, ORREP_EULER_F)) { + EULERS_BFP_OF_REAL(orientation->eulers_i, orientation->eulers_f); + SetBit(orientation->status, ORREP_EULER_I); + INT32_RMAT_OF_EULERS(orientation->rmat_i, orientation->eulers_i); + } + /* set bit to indicate this representation is computed */ + SetBit(orientation->status, ORREP_RMAT_I); +} + +void orientationCalcEulers_i(struct OrientationReps* orientation) { + if (bit_is_set(orientation->status, ORREP_EULER_I)) + return; + + if (bit_is_set(orientation->status, ORREP_EULER_F)) { + EULERS_BFP_OF_REAL(orientation->eulers_i, orientation->eulers_f); + } + else if (bit_is_set(orientation->status, ORREP_RMAT_I)) { + INT32_EULERS_OF_RMAT(orientation->eulers_i, orientation->rmat_i); + } + else if (bit_is_set(orientation->status, ORREP_QUAT_I)) { + INT32_EULERS_OF_QUAT(orientation->eulers_i, orientation->quat_i); + } + else if (bit_is_set(orientation->status, ORREP_RMAT_F)) { + RMAT_BFP_OF_REAL(orientation->rmat_i, orientation->rmat_f); + SetBit(orientation->status, ORREP_RMAT_I); + INT32_EULERS_OF_RMAT(orientation->eulers_i, orientation->rmat_i); + } + else if (bit_is_set(orientation->status, ORREP_QUAT_F)) { + QUAT_BFP_OF_REAL(orientation->quat_i, orientation->quat_f); + SetBit(orientation->status, ORREP_QUAT_I); + INT32_EULERS_OF_QUAT(orientation->eulers_i, orientation->quat_i); + } + /* set bit to indicate this representation is computed */ + SetBit(orientation->status, ORREP_EULER_I); +} + +void orientationCalcQuat_f(struct OrientationReps* orientation) { + if (bit_is_set(orientation->status, ORREP_QUAT_F)) + return; + + if (bit_is_set(orientation->status, ORREP_QUAT_I)) { + QUAT_FLOAT_OF_BFP(orientation->quat_f, orientation->quat_i); + } + else if (bit_is_set(orientation->status, ORREP_RMAT_F)) { + FLOAT_QUAT_OF_RMAT(orientation->quat_f, orientation->rmat_f); + } + else if (bit_is_set(orientation->status, ORREP_EULER_F)) { + FLOAT_QUAT_OF_EULERS(orientation->quat_f, orientation->eulers_f); + } + else if (bit_is_set(orientation->status, ORREP_RMAT_I)) { + RMAT_FLOAT_OF_BFP(orientation->rmat_f, orientation->rmat_i); + SetBit(orientation->status, ORREP_RMAT_F); + FLOAT_QUAT_OF_RMAT(orientation->quat_f, orientation->rmat_f); + } + else if (bit_is_set(orientation->status, ORREP_EULER_I)) { + EULERS_FLOAT_OF_BFP(orientation->eulers_f, orientation->eulers_i); + SetBit(orientation->status, ORREP_EULER_F); + FLOAT_QUAT_OF_EULERS(orientation->quat_f, orientation->eulers_f); + } + /* set bit to indicate this representation is computed */ + SetBit(orientation->status, ORREP_QUAT_F); +} + +void orientationCalcRMat_f(struct OrientationReps* orientation) { + if (bit_is_set(orientation->status, ORREP_RMAT_F)) + return; + + if (bit_is_set(orientation->status, ORREP_RMAT_I)) { + RMAT_FLOAT_OF_BFP(orientation->rmat_f, orientation->rmat_i); + } + else if (bit_is_set(orientation->status, ORREP_QUAT_F)) { + FLOAT_RMAT_OF_QUAT(orientation->rmat_i, orientation->quat_i); + } + else if (bit_is_set(orientation->status, ORREP_EULER_F)) { + FLOAT_RMAT_OF_EULERS(orientation->rmat_i, orientation->eulers_i); + } + else if (bit_is_set(orientation->status, ORREP_QUAT_I)) { + QUAT_FLOAT_OF_BFP(orientation->quat_f, orientation->quat_i); + SetBit(orientation->status, ORREP_QUAT_F); + FLOAT_RMAT_OF_QUAT(orientation->rmat_f, orientation->quat_f); + } + else if (bit_is_set(orientation->status, ORREP_EULER_I)) { + EULERS_FLOAT_OF_BFP(orientation->eulers_f, orientation->eulers_i); + SetBit(orientation->status, ORREP_EULER_F); + FLOAT_RMAT_OF_EULERS(orientation->rmat_f, orientation->eulers_f); + } + /* set bit to indicate this representation is computed */ + SetBit(orientation->status, ORREP_RMAT_F); +} + +void orientationCalcEulers_f(struct OrientationReps* orientation) { + if (bit_is_set(orientation->status, ORREP_EULER_F)) + return; + + if (bit_is_set(orientation->status, ORREP_EULER_I)) { + EULERS_FLOAT_OF_BFP(orientation->eulers_f, orientation->eulers_i); + } + else if (bit_is_set(orientation->status, ORREP_RMAT_F)) { + FLOAT_EULERS_OF_RMAT(orientation->eulers_f, orientation->rmat_f); + } + else if (bit_is_set(orientation->status, ORREP_QUAT_F)) { + FLOAT_EULERS_OF_QUAT(orientation->eulers_f, orientation->quat_f); + } + else if (bit_is_set(orientation->status, ORREP_RMAT_I)) { + RMAT_FLOAT_OF_BFP(orientation->rmat_f, orientation->rmat_i); + SetBit(orientation->status, ORREP_RMAT_F); + FLOAT_EULERS_OF_RMAT(orientation->eulers_i, orientation->rmat_i); + } + else if (bit_is_set(orientation->status, ORREP_QUAT_I)) { + QUAT_FLOAT_OF_BFP(orientation->quat_f, orientation->quat_i); + SetBit(orientation->status, ORREP_QUAT_F); + FLOAT_EULERS_OF_QUAT(orientation->eulers_f, orientation->quat_f); + } + /* set bit to indicate this representation is computed */ + SetBit(orientation->status, ORREP_EULER_F); +} +/** @}*/ +/** @}*/ diff --git a/sw/airborne/math/pprz_orientation_conversion.h b/sw/airborne/math/pprz_orientation_conversion.h new file mode 100644 index 0000000000..18677a1737 --- /dev/null +++ b/sw/airborne/math/pprz_orientation_conversion.h @@ -0,0 +1,223 @@ +/* + * Copyright (C) 2011-2012 The Paparazzi Team + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + * + */ + +/** + * @file pprz_orienation_conversion.h + * Generic orientation representation and conversions. + * + * This file contains the functions to automatically convert between + * the different representations. They should normally not be used + * directly and instead the stateGet/Set interfaces used. + * Also see the @ref math_orientation_representation "Generic Orientation Representation" page. + * + * @author Felix Ruess + */ + +/** + * @addtogroup math + * @{ + */ + +/** + * This generic orientation representation consists of a struct, containing the 6 orientation + * representations, and a status variable. The bits in the status variable indicate which + * representations of the orientation are up-to-date. + * + * When a getter is used to get a certain representation, the status bit is checked to see if + * the current value is already available in the desired orientation representation. + * If the desired representation is not available, it will be calculated. + * + * When a setter is used to set a representation, all status bits are cleared, and only the + * status bit for the set representation is set to one. + */ + +/** + * @defgroup math_orientation_representation Generic Orientation Representations + * @{ + */ + +#ifndef PPRZ_ORIENTATION_CONVERSION_H +#define PPRZ_ORIENTATION_CONVERSION_H + +#include "math/pprz_algebra_int.h" +#include "math/pprz_algebra_float.h" + +#include "std.h" + + +#define ORREP_QUAT_I 0 ///< Quaternion (BFP int) +#define ORREP_EULER_I 1 ///< zyx Euler (BFP int) +#define ORREP_RMAT_I 2 ///< Rotation Matrix (BFP int) +#define ORREP_QUAT_F 3 ///< Quaternion (float) +#define ORREP_EULER_F 4 ///< zyx Euler (float) +#define ORREP_RMAT_F 5 ///< Rotation Matrix (float) + +/* + * @brief Struct with euler/rmat/quaternion orientation representations in BFP int and float + */ +struct OrientationReps { + /** + * Holds the status bits for all orientation representations. + * When the corresponding bit is set, the representation + * is already computed. + */ + uint8_t status; + + /** + * Orientation quaternion. + * Units: #INT32_QUAT_FRAC + */ + struct Int32Quat quat_i; + + /** + * Orientation in zyx euler angles. + * Units: rad in BFP with #INT32_ANGLE_FRAC + */ + struct Int32Eulers eulers_i; + + /** + * Orientation rotation matrix. + * Units: rad in BFP with #INT32_TRIG_FRAC + */ + struct Int32RMat rmat_i; + + /** + * Orientation as quaternion. + * Units: unit length quaternion + */ + struct FloatQuat quat_f; + + /** + * Orienation in zyx euler angles. + * Units: rad + */ + struct FloatEulers eulers_f; + + /** + * Orientation rotation matrix. + * Units: rad + */ + struct FloatRMat rmat_f; +}; + +/************* declaration of transformation functions ************/ +extern void orientationCalcQuat_i(struct OrientationReps* orientation); +extern void orientationCalcRMat_i(struct OrientationReps* orientation); +extern void orientationCalcEulers_i(struct OrientationReps* orientation); +extern void orientationCalcQuat_f(struct OrientationReps* orientation); +extern void orientationCalcRMat_f(struct OrientationReps* orientation); +extern void orientationCalcEulers_f(struct OrientationReps* orientation); + + +/*********************** validity test functions ******************/ +/// Test if orientations are valid. +static inline bool_t orienationCheckValid(struct OrientationReps* orientation) { + return (orientation->status); +} + +/// Set vehicle body attitude from quaternion (int). +static inline void orientationSetQuat_i(struct OrientationReps* orientation, struct Int32Quat* quat) { + QUAT_COPY(orientation->quat_i, *quat); + /* clear bits for all attitude representations and only set the new one */ + orientation->status = (1 << ORREP_QUAT_I); +} + +/// Set vehicle body attitude from rotation matrix (int). +static inline void orientationSetRMat_i(struct OrientationReps* orientation, struct Int32RMat* rmat) { + RMAT_COPY(orientation->rmat_i, *rmat); + /* clear bits for all attitude representations and only set the new one */ + orientation->status = (1 << ORREP_RMAT_I); +} + +/// Set vehicle body attitude from euler angles (int). +static inline void orientationSetEulers_i(struct OrientationReps* orientation, struct Int32Eulers* eulers) { + EULERS_COPY(orientation->eulers_i, *eulers); + /* clear bits for all attitude representations and only set the new one */ + orientation->status = (1 << ORREP_EULER_I); +} + +/// Set vehicle body attitude from quaternion (float). +static inline void orientationSetQuat_f(struct OrientationReps* orientation, struct FloatQuat* quat) { + QUAT_COPY(orientation->quat_f, *quat); + /* clear bits for all attitude representations and only set the new one */ + orientation->status = (1 << ORREP_QUAT_F); +} + +/// Set vehicle body attitude from rotation matrix (float). +static inline void orientationSetRMat_f(struct OrientationReps* orientation, struct FloatRMat* rmat) { + RMAT_COPY(orientation->rmat_f, *rmat); + /* clear bits for all attitude representations and only set the new one */ + orientation->status = (1 << ORREP_RMAT_F); +} + +/// Set vehicle body attitude from euler angles (float). +static inline void orientationSetEulers_f(struct OrientationReps* orientation, struct FloatEulers* eulers) { + EULERS_COPY(orientation->eulers_f, *eulers); + /* clear bits for all attitude representations and only set the new one */ + orientation->status = (1 << ORREP_EULER_F); +} + + +/// Get vehicle body attitude quaternion (int). +static inline struct Int32Quat* orientationGetQuat_i(struct OrientationReps* orientation) { + if (!bit_is_set(orientation->status, ORREP_QUAT_I)) + orientationCalcQuat_i(orientation); + return &orientation->quat_i; +} + +/// Get vehicle body attitude rotation matrix (int). +static inline struct Int32RMat* orientationGetRMat_i(struct OrientationReps* orientation) { + if (!bit_is_set(orientation->status, ORREP_RMAT_I)) + orientationCalcRMat_i(orientation); + return &orientation->rmat_i; +} + +/// Get vehicle body attitude euler angles (int). +static inline struct Int32Eulers* orientationGetEulers_i(struct OrientationReps* orientation) { + if (!bit_is_set(orientation->status, ORREP_EULER_I)) + orientationCalcEulers_i(orientation); + return &orientation->eulers_i; +} + +/// Get vehicle body attitude quaternion (float). +static inline struct FloatQuat* orientationGetQuat_f(struct OrientationReps* orientation) { + if (!bit_is_set(orientation->status, ORREP_QUAT_F)) + orientationCalcQuat_f(orientation); + return &orientation->quat_f; +} + +/// Get vehicle body attitude rotation matrix (float). +static inline struct FloatRMat* orientationGetRMat_f(struct OrientationReps* orientation) { + if (!bit_is_set(orientation->status, ORREP_RMAT_F)) + orientationCalcRMat_f(orientation); + return &orientation->rmat_f; +} + +/// Get vehicle body attitude euler angles (float). +static inline struct FloatEulers* orientationGetEulers_f(struct OrientationReps* orientation) { + if (!bit_is_set(orientation->status, ORREP_EULER_F)) + orientationCalcEulers_f(orientation); + return &orientation->eulers_f; +} + +#endif /* PPRZ_ORIENTATION_CONVERSION_H */ +/** @}*/ +/** @}*/ diff --git a/sw/airborne/state.c b/sw/airborne/state.c index 7a197fa23f..0b92f2fe3a 100644 --- a/sw/airborne/state.c +++ b/sw/airborne/state.c @@ -44,7 +44,7 @@ void stateInit(void) { state.pos_status = 0; state.speed_status = 0; state.accel_status = 0; - state.att_status = 0; + state.ned_to_body_orientation.status = 0; state.rate_status = 0; state.wind_air_status = 0; state.ned_initialized_i = FALSE; @@ -1135,180 +1135,6 @@ void stateCalcAccelEcef_f(void) { } /** @}*/ - - -/****************************************************************************** - * * - * Transformation functions for the ATTITUDE representations * - * * - *****************************************************************************/ -/** @addtogroup state_attitude - * @{ */ - -void stateCalcNedToBodyQuat_i(void) { - if (bit_is_set(state.att_status, ATT_QUAT_I)) - return; - - if (bit_is_set(state.att_status, ATT_QUAT_F)) { - QUAT_BFP_OF_REAL(state.ned_to_body_quat_i, state.ned_to_body_quat_f); - } - else 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); - } - else if (bit_is_set(state.att_status, ATT_EULER_I)) { - INT32_QUAT_OF_EULERS(state.ned_to_body_quat_i, state.ned_to_body_eulers_i); - } - else if (bit_is_set(state.att_status, ATT_RMAT_F)) { - RMAT_BFP_OF_REAL(state.ned_to_body_rmat_i, state.ned_to_body_rmat_f); - SetBit(state.att_status, ATT_RMAT_I); - INT32_QUAT_OF_RMAT(state.ned_to_body_quat_i, state.ned_to_body_rmat_i); - } - else if (bit_is_set(state.att_status, ATT_EULER_F)) { - EULERS_BFP_OF_REAL(state.ned_to_body_eulers_i, state.ned_to_body_eulers_f); - SetBit(state.att_status, ATT_EULER_I); - INT32_QUAT_OF_EULERS(state.ned_to_body_quat_i, state.ned_to_body_eulers_i); - } - /* set bit to indicate this representation is computed */ - SetBit(state.att_status, ATT_QUAT_I); -} - -void stateCalcNedToBodyRMat_i(void) { - if (bit_is_set(state.att_status, ATT_RMAT_I)) - return; - - if (bit_is_set(state.att_status, ATT_RMAT_F)) { - RMAT_BFP_OF_REAL(state.ned_to_body_rmat_i, state.ned_to_body_rmat_f); - } - else 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); - } - else if (bit_is_set(state.att_status, ATT_EULER_I)) { - INT32_RMAT_OF_EULERS(state.ned_to_body_rmat_i, state.ned_to_body_eulers_i); - } - else if (bit_is_set(state.att_status, ATT_QUAT_F)) { - QUAT_BFP_OF_REAL(state.ned_to_body_quat_i, state.ned_to_body_quat_f); - SetBit(state.att_status, ATT_QUAT_I); - INT32_RMAT_OF_QUAT(state.ned_to_body_rmat_i, state.ned_to_body_quat_i); - } - else if (bit_is_set(state.att_status, ATT_EULER_F)) { - EULERS_BFP_OF_REAL(state.ned_to_body_eulers_i, state.ned_to_body_eulers_f); - SetBit(state.att_status, ATT_EULER_I); - INT32_RMAT_OF_EULERS(state.ned_to_body_rmat_i, state.ned_to_body_eulers_i); - } - /* set bit to indicate this representation is computed */ - SetBit(state.att_status, ATT_RMAT_I); -} - -void stateCalcNedToBodyEulers_i(void) { - if (bit_is_set(state.att_status, ATT_EULER_I)) - return; - - if (bit_is_set(state.att_status, ATT_EULER_F)) { - EULERS_BFP_OF_REAL(state.ned_to_body_eulers_i, state.ned_to_body_eulers_f); - } - else if (bit_is_set(state.att_status, ATT_RMAT_I)) { - INT32_EULERS_OF_RMAT(state.ned_to_body_eulers_i, state.ned_to_body_rmat_i); - } - else 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); - } - else if (bit_is_set(state.att_status, ATT_RMAT_F)) { - RMAT_BFP_OF_REAL(state.ned_to_body_rmat_i, state.ned_to_body_rmat_f); - SetBit(state.att_status, ATT_RMAT_I); - INT32_EULERS_OF_RMAT(state.ned_to_body_eulers_i, state.ned_to_body_rmat_i); - } - else if (bit_is_set(state.att_status, ATT_QUAT_F)) { - QUAT_BFP_OF_REAL(state.ned_to_body_quat_i, state.ned_to_body_quat_f); - SetBit(state.att_status, ATT_QUAT_I); - INT32_EULERS_OF_QUAT(state.ned_to_body_eulers_i, state.ned_to_body_quat_i); - } - /* set bit to indicate this representation is computed */ - SetBit(state.att_status, ATT_EULER_I); -} - -void stateCalcNedToBodyQuat_f(void) { - if (bit_is_set(state.att_status, ATT_QUAT_F)) - return; - - if (bit_is_set(state.att_status, ATT_QUAT_I)) { - QUAT_FLOAT_OF_BFP(state.ned_to_body_quat_f, state.ned_to_body_quat_i); - } - else 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); - } - else if (bit_is_set(state.att_status, ATT_EULER_F)) { - FLOAT_QUAT_OF_EULERS(state.ned_to_body_quat_f, state.ned_to_body_eulers_f); - } - else if (bit_is_set(state.att_status, ATT_RMAT_I)) { - RMAT_FLOAT_OF_BFP(state.ned_to_body_rmat_f, state.ned_to_body_rmat_i); - SetBit(state.att_status, ATT_RMAT_F); - FLOAT_QUAT_OF_RMAT(state.ned_to_body_quat_f, state.ned_to_body_rmat_f); - } - else if (bit_is_set(state.att_status, ATT_EULER_I)) { - EULERS_FLOAT_OF_BFP(state.ned_to_body_eulers_f, state.ned_to_body_eulers_i); - SetBit(state.att_status, ATT_EULER_F); - FLOAT_QUAT_OF_EULERS(state.ned_to_body_quat_f, state.ned_to_body_eulers_f); - } - /* set bit to indicate this representation is computed */ - SetBit(state.att_status, ATT_QUAT_F); -} - -void stateCalcNedToBodyRMat_f(void) { - if (bit_is_set(state.att_status, ATT_RMAT_F)) - return; - - if (bit_is_set(state.att_status, ATT_RMAT_I)) { - RMAT_FLOAT_OF_BFP(state.ned_to_body_rmat_f, state.ned_to_body_rmat_i); - } - else if (bit_is_set(state.att_status, ATT_QUAT_F)) { - FLOAT_RMAT_OF_QUAT(state.ned_to_body_rmat_i, state.ned_to_body_quat_i); - } - else if (bit_is_set(state.att_status, ATT_EULER_F)) { - FLOAT_RMAT_OF_EULERS(state.ned_to_body_rmat_i, state.ned_to_body_eulers_i); - } - else if (bit_is_set(state.att_status, ATT_QUAT_I)) { - QUAT_FLOAT_OF_BFP(state.ned_to_body_quat_f, state.ned_to_body_quat_i); - SetBit(state.att_status, ATT_QUAT_F); - FLOAT_RMAT_OF_QUAT(state.ned_to_body_rmat_f, state.ned_to_body_quat_f); - } - else if (bit_is_set(state.att_status, ATT_EULER_I)) { - EULERS_FLOAT_OF_BFP(state.ned_to_body_eulers_f, state.ned_to_body_eulers_i); - SetBit(state.att_status, ATT_EULER_F); - FLOAT_RMAT_OF_EULERS(state.ned_to_body_rmat_f, state.ned_to_body_eulers_f); - } - /* set bit to indicate this representation is computed */ - SetBit(state.att_status, ATT_RMAT_F); -} - -void stateCalcNedToBodyEulers_f(void) { - if (bit_is_set(state.att_status, ATT_EULER_F)) - return; - - if (bit_is_set(state.att_status, ATT_EULER_I)) { - EULERS_FLOAT_OF_BFP(state.ned_to_body_eulers_f, state.ned_to_body_eulers_i); - } - else if (bit_is_set(state.att_status, ATT_RMAT_F)) { - FLOAT_EULERS_OF_RMAT(state.ned_to_body_eulers_f, state.ned_to_body_rmat_f); - } - else 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); - } - else if (bit_is_set(state.att_status, ATT_RMAT_I)) { - RMAT_FLOAT_OF_BFP(state.ned_to_body_rmat_f, state.ned_to_body_rmat_i); - SetBit(state.att_status, ATT_RMAT_F); - FLOAT_EULERS_OF_RMAT(state.ned_to_body_eulers_i, state.ned_to_body_rmat_i); - } - else if (bit_is_set(state.att_status, ATT_QUAT_I)) { - QUAT_FLOAT_OF_BFP(state.ned_to_body_quat_f, state.ned_to_body_quat_i); - SetBit(state.att_status, ATT_QUAT_F); - FLOAT_EULERS_OF_QUAT(state.ned_to_body_eulers_f, state.ned_to_body_quat_f); - } - /* set bit to indicate this representation is computed */ - SetBit(state.att_status, ATT_EULER_F); -} -/** @}*/ - - /****************************************************************************** * * * Transformation functions for the ANGULAR RATE representations * diff --git a/sw/airborne/state.h b/sw/airborne/state.h index 5f87a40213..6ea61e33e0 100644 --- a/sw/airborne/state.h +++ b/sw/airborne/state.h @@ -35,6 +35,7 @@ #include "math/pprz_algebra_float.h" #include "math/pprz_geodetic_int.h" #include "math/pprz_geodetic_float.h" +#include "math/pprz_orientation_conversion.h" #include "std.h" #include @@ -101,18 +102,6 @@ #define ACCEL_NED_F 3 /**@}*/ -/** - * @defgroup state_attitude Attitude representations - * @{ - */ -#define ATT_QUAT_I 0 -#define ATT_EULER_I 1 -#define ATT_RMAT_I 2 -#define ATT_QUAT_F 3 -#define ATT_EULER_F 4 -#define ATT_RMAT_F 5 -/**@}*/ - /** * @defgroup state_rate Angular rate representations * @{ @@ -362,66 +351,9 @@ struct State { /** @}*/ - /** @addtogroup state_attitude + /** @defgroup state_attitude * @{ */ - /** - * Holds the status bits for all attitude representations. - * When the corresponding bit is set, the representation - * is already computed. - */ - uint8_t att_status; - - /** - * Attitude quaternion. - * Specifies rotation from local NED frame to body frame. - * Units: #INT32_QUAT_FRAC - * - * @code - * struct Int32Vect3 body_accel; - * INT32_QUAT_VMULT(body_accel, *stateGetNedToBodyQuat_i(), *stateGetAccelNed_i()); - * @endcode - */ - struct Int32Quat ned_to_body_quat_i; - - /** - * Attitude in zyx euler angles. - * Specifies rotation from local NED frame to body frame. - * Units: rad in BFP with #INT32_ANGLE_FRAC - */ - struct Int32Eulers ned_to_body_eulers_i; - - /** - * Attitude rotation matrix. - * Specifies rotation from local NED frame to body frame. - * Units: rad in BFP with #INT32_TRIG_FRAC - */ - struct Int32RMat ned_to_body_rmat_i; - - /** - * Attitude as quaternion. - * Specifies rotation from local NED frame to body frame. - * Units: unit length quaternion - * - * @code - * struct FloatVect3 body_accel; - * FLOAT_QUAT_VMULT(body_accel, *stateGetNedToBodyQuat_f(), *stateGetAccelNed_f()); - * @endcode - */ - struct FloatQuat ned_to_body_quat_f; - - /** - * Attitude in zyx euler angles. - * Specifies rotation from local NED frame to body frame. - * Units: rad - */ - struct FloatEulers ned_to_body_eulers_f; - - /** - * Attitude rotation matrix. - * Specifies rotation from local NED frame to body frame. - * Units: rad - */ - struct FloatRMat ned_to_body_rmat_f; + struct OrientationReps ned_to_body_orientation; /** @}*/ @@ -1026,122 +958,87 @@ static inline struct EcefCoor_f* stateGetAccelEcef_f(void) { } /** @}*/ - - /****************************************************************************** - * * - * Set and Get functions for the ATTITUDE representations * - * * - *****************************************************************************/ +* * +* Set and Get functions for the ATTITUDE representations * +* (Calls the functions in math/pprz_orientation_conversion) * +* * +*****************************************************************************/ /** @addtogroup state_attitude - * @{ */ - -/************* 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); - +* @{ */ /*********************** validity test functions ******************/ /// Test if attitudes are valid. static inline bool_t stateIsAttitudeValid(void) { - return (state.att_status); + return (orienationCheckValid(&state.ned_to_body_orientation)); } /************************ Set functions ****************************/ /// Set vehicle body attitude from quaternion (int). 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); + orientationSetQuat_i(&state.ned_to_body_orientation,ned_to_body_quat); } /// Set vehicle body attitude from rotation matrix (int). 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); + orientationSetRMat_i(&state.ned_to_body_orientation,ned_to_body_rmat); } /// Set vehicle body attitude from euler angles (int). 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); + orientationSetEulers_i(&state.ned_to_body_orientation,ned_to_body_eulers); } /// Set vehicle body attitude from quaternion (float). 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); + orientationSetQuat_f(&state.ned_to_body_orientation,ned_to_body_quat); } /// Set vehicle body attitude from rotation matrix (float). 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); + orientationSetRMat_f(&state.ned_to_body_orientation,ned_to_body_rmat); } /// Set vehicle body attitude from euler angles (float). 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); + orientationSetEulers_f(&state.ned_to_body_orientation,ned_to_body_eulers); } /************************ Get functions ****************************/ /// Get vehicle body attitude quaternion (int). 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; + return orientationGetQuat_i(&state.ned_to_body_orientation); } /// Get vehicle body attitude rotation matrix (int). 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; + return orientationGetRMat_i(&state.ned_to_body_orientation); } /// Get vehicle body attitude euler angles (int). 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; + return orientationGetEulers_i(&state.ned_to_body_orientation); } /// Get vehicle body attitude quaternion (float). 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; + return orientationGetQuat_f(&state.ned_to_body_orientation); } /// Get vehicle body attitude rotation matrix (float). 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; + return orientationGetRMat_f(&state.ned_to_body_orientation); } /// Get vehicle body attitude euler angles (float). 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; + return orientationGetEulers_f(&state.ned_to_body_orientation); } /** @}*/ - /****************************************************************************** * * * Set and Get functions for the ANGULAR RATE representations *