Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 22:49:53 +08:00
Code Issues Actions 31 Packages Projects Releases Wiki Activity

[math] moved automatic orientation calculations to generic pprz_orientation_conversion

Browse Source
This commit is contained in:
Dirk Dokter
2012-09-23 12:54:40 +02:00
committed by Felix Ruess
parent 18dbffb6a3
commit 500f32443c
7 changed files with 470 additions and 302 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/firmwares/rotorcraft.makefile
+1 -1
View File
@@ -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
conf/firmwares/subsystems/fixedwing/autopilot.makefile
+1 -1
View File
@@ -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
doc/manual/math.dox
+11
View File
@@ -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.
*/
sw/airborne/math/pprz_orientation_conversion.c
+211
View File
@@ -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 <felix.ruess@gmail.com>
*/
/**
* @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);
}
/** @}*/
/** @}*/
sw/airborne/math/pprz_orientation_conversion.h
+223
View File
@@ -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
* <http://www.gnu.org/licenses/>.
*
*/
/**
* @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 <felix.ruess@gmail.com>
*/
/**
* @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 */
/** @}*/
/** @}*/
sw/airborne/state.c
+1 -175
View File
@@ -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 *
sw/airborne/state.h
+22 -125
View File
@@ -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 <string.h>
@@ -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 *