Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-24 05:45:59 +08:00
Code Issues Actions 24 Packages Projects Releases Wiki Activity

[imu] possibily to set imu_to_body to current roll/pitch and reset to airframe values

Browse Source
This commit is contained in:
Felix Ruess
2014-08-02 20:58:53 +02:00
parent c78a0c5eaa
commit 45002df9f7
2 changed files with 26 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/settings/estimation/imu_to_body.xml
+2
View File
@@ -6,6 +6,8 @@
<dl_setting MAX="90" MIN="-90" STEP="0.5" VAR="imu.body_to_imu.eulers_f.phi" shortname="imu2body phi" module="subsystems/imu" param="IMU_BODY_TO_IMU_PHI" unit="rad" alt_unit="deg" handler="SetBodyToImuPhi"/>
<dl_setting MAX="90" MIN="-90" STEP="0.5" VAR="imu.body_to_imu.eulers_f.theta" shortname="imu2body theta" module="subsystems/imu" param="IMU_BODY_TO_IMU_THETA" unit="rad" alt_unit="deg" handler="SetBodyToImuTheta"/>
<dl_setting MAX="180" MIN="-180" STEP="0.5" VAR="imu.body_to_imu.eulers_f.psi" shortname="imu2body psi" module="subsystems/imu" param="IMU_BODY_TO_IMU_PSI" unit="rad" alt_unit="deg" handler="SetBodyToImuPsi"/>
<dl_setting MAX="1" MIN="0" STEP="1" VAR="imu.b2i_set_current" shortname="imu2body cur roll/pitch" module="subsystems/imu" handler="SetBodyToImuCurrent"/>
<dl_setting MAX="1" MIN="0" STEP="1" VAR="imu.b2i_reset" shortname="imu2body reset" module="subsystems/imu" handler="ResetBodyToImu"/>
</dl_settings>
</dl_settings>
</settings>
sw/airborne/subsystems/imu.h
+24
View File
@@ -30,6 +30,7 @@
#include "math/pprz_algebra_int.h"
#include "math/pprz_algebra_float.h"
#include "math/pprz_orientation_conversion.h"
#include "state.h"
#include "generated/airframe.h"
/* must be defined by underlying hardware */
@@ -50,6 +51,8 @@ struct Imu {
struct Int32Vect3 accel_unscaled; ///< unscaled accelerometer measurements
struct Int32Vect3 mag_unscaled; ///< unscaled magnetometer measurements
struct OrientationReps body_to_imu; ///< rotation from body to imu frame
bool_t b2i_set_current; ///< flag for settings in order to set current roll and pitch as body_to_imu
bool_t b2i_reset; ///< flag for settings in order to reset body_to_imu to airframe defines
};
/** abstract IMU interface providing floating point interface */
@@ -164,5 +167,26 @@ static inline void imu_SetBodyToImuPsi(float psi) {
orientationSetEulers_f(&imu.body_to_imu, &imu_to_body_eulers);
}
static inline void imu_SetBodyToImuCurrent(float set) {
imu.b2i_set_current = set;
if (imu.b2i_set_current) {
struct FloatEulers imu_to_body_eulers;
memcpy(&imu_to_body_eulers, orientationGetEulers_f(&imu.body_to_imu), sizeof(struct FloatEulers));
// set to current roll and pitch
imu_to_body_eulers.phi = stateGetNedToBodyEulers_f()->phi;
imu_to_body_eulers.theta = stateGetNedToBodyEulers_f()->theta;
orientationSetEulers_f(&imu.body_to_imu, &imu_to_body_eulers);
}
}
static inline void imu_ResetBodyToImu(float reset) {
imu.b2i_reset = reset;
if (imu.b2i_reset) {
struct FloatEulers imu_to_body_eulers =
{IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
orientationSetEulers_f(&imu.body_to_imu, &imu_to_body_eulers);
}
}
#endif /* IMU_H */