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

added an initialisation function for imu_float

Browse Source
This commit is contained in:
Antoine Drouin
2010-11-04 15:50:58 +00:00
parent 6ce33a0dd1
commit 8e2513845b
2 changed files with 28 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/subsystems/imu.c
+26 -2
View File
@@ -1,7 +1,7 @@
/*
* $Id$
*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
* Copyright (C) 2008-2010 The Paparazzi Team
*
* This file is part of paparazzi.
*
@@ -38,7 +38,7 @@ void imu_init(void) {
Compute quaternion and rotation matrix
for conversions between body and imu frame
*/
#ifdef IMU_BODY_TO_IMU_PHI
#if defined IMU_BODY_TO_IMU_PHI && defined IMU_BODY_TO_IMU_THETA & defined IMU_BODY_TO_IMU_PSI
struct Int32Eulers body_to_imu_eulers =
{ ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_PHI),
ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_THETA),
@@ -46,7 +46,31 @@ void imu_init(void) {
INT32_QUAT_OF_EULERS(imu.body_to_imu_quat, body_to_imu_eulers);
INT32_QUAT_NORMALISE(imu.body_to_imu_quat);
INT32_RMAT_OF_EULERS(imu.body_to_imu_rmat, body_to_imu_eulers);
#else
INT32_QUAT_ZERO(imu.body_to_imu_quat);
INT32_RMAT_ZERO(imu.body_to_imu_rmat);
#endif
imu_impl_init();
}
void imu_float_init(struct ImuFloat* imuf) {
/*
Compute quaternion and rotation matrix
for conversions between body and imu frame
*/
#if defined IMU_BODY_TO_IMU_PHI && defined IMU_BODY_TO_IMU_THETA & defined IMU_BODY_TO_IMU_PSI
EULERS_ASSIGN(imuf->body_to_imu_eulers,
IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI);
FLOAT_QUAT_OF_EULERS(imuf->body_to_imu_quat, imuf->body_to_imu_eulers);
FLOAT_QUAT_NORMALISE(imuf->body_to_imu_quat);
FLOAT_RMAT_OF_EULERS(imuf->body_to_imu_rmat, imuf->body_to_imu_eulers);
#else
EULERS_ASSIGN(imuf->body_to_imu_eulers, 0., 0., 0.);
FLOAT_QUAT_ZERO(imuf->body_to_imu_quat);
FLOAT_RMAT_ZERO(imuf->body_to_imu_rmat);
#endif
}
sw/airborne/subsystems/imu.h
+2
View File
@@ -59,6 +59,8 @@ struct ImuFloat {
uint32_t sample_count;
};
extern void imu_float_init(struct ImuFloat* imuf);
/* underlying hardware */
#ifdef IMU_TYPE_H
#include IMU_TYPE_H