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

ahrs cmpl: directly compute initial quaternion from accel and mag instead of using euler angles

Browse Source 
closes #132
This commit is contained in:
Felix Ruess
2012-02-20 20:27:50 +01:00
parent f1656ef01e
commit b422aa492f
5 changed files with 98 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/fraser_lisa_m_rotorcraft.xml
+4 -1
View File
@@ -25,6 +25,9 @@
<subsystem name="gps" type="ublox"/>
<subsystem name="stabilization" type="euler"/>
<subsystem name="ahrs" type="int_cmpl_quat"/>
<!--subsystem name="ahrs" type="float_cmpl">
<define name="AHRS_PROPAGATE_QUAT"/>
</subsystem-->
</firmware>
<firmware name="setup">
@@ -55,7 +58,7 @@
</firmware>
<modules main_freq="512">
<!--load name="sys_mon.xml"/-->
<load name="sys_mon.xml"/>
</modules>
<commands>
sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c
+14 -1
View File
@@ -85,14 +85,27 @@ void ahrs_init(void) {
}
#define AHRS_ALIGN_QUAT 1
void ahrs_align(void) {
#if AHRS_ALIGN_QUAT
/* Compute an initial orientation from accel and mag directly as quaternion */
ahrs_float_get_quat_from_accel_mag(&ahrs_float.ltp_to_imu_quat, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag);
/* Convert initial orientation from quat to euler and rotation matrix representations. */
compute_imu_rmat_and_euler_from_quat();
#else
/* Compute an initial orientation using euler angles */
ahrs_float_get_euler_from_accel_mag(&ahrs_float.ltp_to_imu_euler, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag);
/* Convert initial orientation in quaternion and rotation matrice representations. */
/* Convert initial orientation in quaternion and rotation matrix representations. */
FLOAT_QUAT_OF_EULERS(ahrs_float.ltp_to_imu_quat, ahrs_float.ltp_to_imu_euler);
FLOAT_RMAT_OF_QUAT(ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_quat);
#endif
/* Compute initial body orientation */
compute_body_orientation_and_rates();
sw/airborne/subsystems/ahrs/ahrs_float_utils.h
+67
View File
@@ -3,6 +3,8 @@
#include "subsystems/ahrs/ahrs_magnetic_field_model.h"
#define ABS(_x) ((_x) < 0 ? -(_x) : (_x))
static inline void ahrs_float_get_euler_from_accel_mag(struct FloatEulers* e, struct Int32Vect3* accel, struct Int32Vect3* mag) {
/* get phi and theta from accelerometer */
struct FloatVect3 accelf;
@@ -26,4 +28,69 @@ static inline void ahrs_float_get_euler_from_accel_mag(struct FloatEulers* e, st
}
static inline void ahrs_float_get_quat_from_accel_mag(struct FloatQuat* q, struct Int32Vect3* accel, struct Int32Vect3* mag) {
/* normalized accel measurement in floating point */
struct FloatVect3 acc_normalized;
ACCELS_FLOAT_OF_BFP(acc_normalized, *accel);
FLOAT_VECT3_NORMALIZE(acc_normalized);
/* the quaternion representing roll and pitch from acc measurement */
struct FloatQuat q_a;
/*
* axis we want to rotate around is cross product of accel and reference [0,0,-g]
* normalized: cross(acc_normalized, [0,0,-1])
* vector part of quaternion is the axis
* scalar part (angle): 1.0 + dot(acc_normalized, [0,0,-1])
*/
q_a.qx = - acc_normalized.y;
q_a.qy = acc_normalized.x;
q_a.qz = 0.0;
q_a.qi = 1.0 - acc_normalized.z;
FLOAT_QUAT_NORMALIZE(q_a);
/* handle 180deg case */
if ( ABS(acc_normalized.z - 1.0) < 5*FLT_MIN ) {
QUAT_ASSIGN(q_a, 0.0, 1.0, 0.0, 0.0);
}
/* convert mag measurement to float */
struct FloatVect3 mag_float;
MAGS_FLOAT_OF_BFP(mag_float, *mag);
/* and rotate to horizontal plane using the quat from above */
struct FloatRMat rmat_phi_theta;
FLOAT_RMAT_OF_QUAT(rmat_phi_theta, q_a);
struct FloatVect3 mag_ltp;
FLOAT_RMAT_VECT3_TRANSP_MUL(mag_ltp, rmat_phi_theta, mag_float);
/* heading from mag -> make quaternion to rotate around ltp z axis*/
struct FloatQuat q_m;
/* dot([mag_n.x, mag_n.x, 0], [AHRS_H_X, AHRS_H_Y, 0]) */
float dot = mag_ltp.x * AHRS_H_X + mag_ltp.y * AHRS_H_Y;
/* |v1||v2| */
float norm2 = sqrtf(SQUARE(mag_ltp.x) + SQUARE(mag_ltp.y))
* sqrtf(SQUARE(AHRS_H_X) + SQUARE(AHRS_H_Y));
// catch 180deg case
if (ABS(norm2 + dot) < 5*FLT_MIN) {
QUAT_ASSIGN(q_m, 0.0, 0.0, 0.0, 1.0);
} else {
/* q_xyz = cross([mag_n.x, mag_n.y, 0], [AHRS_H_X, AHRS_H_Y, 0]) */
q_m.qx = 0.0;
q_m.qy = 0.0;
q_m.qz = mag_ltp.x * AHRS_H_Y - mag_ltp.y * AHRS_H_X;
q_m.qi = norm2 + dot;
FLOAT_QUAT_NORMALIZE(q_m);
}
// q_ltp2imu = q_a * q_m
// and wrap and normalize
FLOAT_QUAT_COMP_NORM_SHORTEST(*q, q_m, q_a);
}
#endif /* AHRS_FLOAT_UTILS_H */
sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c
+4 -14
View File
@@ -112,10 +112,10 @@ void ahrs_init(void) {
void ahrs_align(void) {
/* Compute an initial orientation using euler angles */
ahrs_int_get_euler_from_accel_mag(&ahrs.ltp_to_imu_euler, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag);
/* Convert initial orientation in quaternion and rotation matrice representations. */
compute_imu_quat_and_rmat_from_euler();
/* Compute an initial orientation from accel and mag directly as quaternion */
ahrs_int_get_quat_from_accel_mag(&ahrs.ltp_to_imu_quat, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag);
/* Convert initial orientation from quat to euler and rotation matrix representations. */
compute_imu_euler_and_rmat_from_quat();
compute_body_orientation();
@@ -376,16 +376,6 @@ void ahrs_update_heading(int32_t heading) {
INT_RATES_RSHIFT(ahrs_impl.gyro_bias, ahrs_impl.high_rez_bias, 28);
}
/* Compute ltp to imu rotation in quaternion and rotation matrice representation
from the euler angle representation */
__attribute__ ((always_inline)) static inline void compute_imu_quat_and_rmat_from_euler(void) {
/* Compute LTP to IMU quaternion */
INT32_QUAT_OF_EULERS(ahrs.ltp_to_imu_quat, ahrs.ltp_to_imu_euler);
/* Compute LTP to IMU rotation matrix */
INT32_RMAT_OF_EULERS(ahrs.ltp_to_imu_rmat, ahrs.ltp_to_imu_euler);
}
/* Compute ltp to imu rotation in euler angles and rotation matrice representation
from the quaternion representation */
sw/airborne/subsystems/ahrs/ahrs_int_utils.h
+9
View File
@@ -6,6 +6,8 @@
#include "subsystems/ahrs/ahrs_magnetic_field_model.h"
#include "subsystems/ahrs/ahrs_float_utils.h"
static inline void ahrs_int_get_euler_from_accel_mag(struct Int32Eulers* e, struct Int32Vect3* accel, struct Int32Vect3* mag) {
// DISPLAY_INT32_VECT3("# accel", (*accel));
const float fphi = atan2f(-accel->y, -accel->z);
@@ -44,4 +46,11 @@ static inline void ahrs_int_get_euler_from_accel_mag(struct Int32Eulers* e, stru
}
static inline void ahrs_int_get_quat_from_accel_mag(struct Int32Quat* q, struct Int32Vect3* accel, struct Int32Vect3* mag) {
struct FloatQuat q_f;
ahrs_float_get_quat_from_accel_mag(&q_f, accel, mag);
QUAT_BFP_OF_REAL(*q, q_f);
}
#endif /* AHRS_INT_UTILS_H */