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

[imu] publish BODY_TO_IMU_QUAT from imu set functions

Browse Source 
and remove body_to_imu from imuf
This commit is contained in:
Felix Ruess
2015-01-19 20:24:28 +01:00
parent ac34d5581d
commit b071a69ab0
2 changed files with 6 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/subsystems/imu.c
+6 -18
View File
@@ -30,6 +30,7 @@
#include "subsystems/imu.h"
#include "state.h"
#include "subsystems/abi.h"
#ifdef IMU_POWER_GPIO
#include "mcu_periph/gpio.h"
@@ -149,9 +150,6 @@ void imu_init(void)
struct FloatEulers body_to_imu_eulers =
{IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers);
#if USE_IMU_FLOAT
orientationSetEulers_f(&imuf.body_to_imu, &body_to_imu_eulers);
#endif
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL", send_accel);
@@ -179,9 +177,7 @@ void imu_SetBodyToImuPhi(float phi)
memcpy(&body_to_imu_eulers, orientationGetEulers_f(&imu.body_to_imu), sizeof(struct FloatEulers));
body_to_imu_eulers.phi = phi;
orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers);
#if USE_IMU_FLOAT
orientationSetEulers_f(&imuf.body_to_imu, &body_to_imu_eulers);
#endif
AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
}
void imu_SetBodyToImuTheta(float theta)
@@ -190,9 +186,7 @@ void imu_SetBodyToImuTheta(float theta)
memcpy(&body_to_imu_eulers, orientationGetEulers_f(&imu.body_to_imu), sizeof(struct FloatEulers));
body_to_imu_eulers.theta = theta;
orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers);
#if USE_IMU_FLOAT
orientationSetEulers_f(&imuf.body_to_imu, &body_to_imu_eulers);
#endif
AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
}
void imu_SetBodyToImuPsi(float psi)
@@ -201,9 +195,7 @@ void imu_SetBodyToImuPsi(float psi)
memcpy(&body_to_imu_eulers, orientationGetEulers_f(&imu.body_to_imu), sizeof(struct FloatEulers));
body_to_imu_eulers.psi = psi;
orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers);
#if USE_IMU_FLOAT
orientationSetEulers_f(&imuf.body_to_imu, &body_to_imu_eulers);
#endif
AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
}
void imu_SetBodyToImuCurrent(float set)
@@ -219,9 +211,7 @@ void imu_SetBodyToImuCurrent(float set)
body_to_imu_eulers.phi += stateGetNedToBodyEulers_f()->phi;
body_to_imu_eulers.theta += stateGetNedToBodyEulers_f()->theta;
orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers);
#if USE_IMU_FLOAT
orientationSetEulers_f(&imuf.body_to_imu, &body_to_imu_eulers);
#endif
AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
} else {
// indicate that we couldn't set to current roll/pitch
imu.b2i_set_current = FALSE;
@@ -231,9 +221,7 @@ void imu_SetBodyToImuCurrent(float set)
struct FloatEulers body_to_imu_eulers =
{IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers);
#if USE_IMU_FLOAT
orientationSetEulers_f(&imuf.body_to_imu, &body_to_imu_eulers);
#endif
AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
}
}
sw/airborne/subsystems/imu.h
-1
View File
@@ -64,7 +64,6 @@ struct ImuFloat {
struct FloatVect3 accel;
struct FloatVect3 mag;
struct FloatRates gyro_prev;
struct OrientationReps body_to_imu; ///< rotation from body to imu frame
uint32_t sample_count;
};