Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-06 16:58:48 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

[ahrs] float_cmpl: use OrientationReps for ltp_to_body

Browse Source
This commit is contained in:
Felix Ruess
2015-11-24 21:01:19 +01:00
parent 4ce9a4c875
commit 3eb3efd5c6
4 changed files with 42 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/math/pprz_orientation_conversion.h
+8
View File
@@ -137,6 +137,14 @@ static inline bool_t orienationCheckValid(struct OrientationReps *orientation)
return (orientation->status);
}
/// Set to identity orientation.
static inline void orientationSetIdentity(struct OrientationReps *orientation)
{
int32_quat_identity(&orientation->quat_i);
/* clear bits for all attitude representations and only set the new one */
orientation->status = (1 << ORREP_QUAT_I);
}
/// Set vehicle body attitude from quaternion (int).
static inline void orientationSetQuat_i(struct OrientationReps *orientation, struct Int32Quat *quat)
{
sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c
+22 -13
View File
@@ -94,6 +94,9 @@ void ahrs_fc_init(void)
/* init ltp_to_imu quaternion as zero/identity rotation */
float_quat_identity(&ahrs_fc.ltp_to_imu_quat);
orientationSetIdentity(&ahrs_fc.body_to_imu);
orientationSetIdentity(&ahrs_fc.ltp_to_body);
FLOAT_RATES_ZERO(ahrs_fc.imu_rate);
/* set default filter cut-off frequency and damping */
@@ -434,16 +437,14 @@ void ahrs_fc_update_heading(float heading)
FLOAT_ANGLE_NORMALIZE(heading);
struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_fc.body_to_imu);
struct FloatQuat ltp_to_body_quat;
float_quat_comp_inv(&ltp_to_body_quat, &ahrs_fc.ltp_to_imu_quat, body_to_imu_quat);
struct FloatRMat ltp_to_body_rmat;
float_rmat_of_quat(&ltp_to_body_rmat, &ltp_to_body_quat);
ahrs_fc_recompute_ltp_to_body();
struct FloatRMat *ltp_to_body_rmat = orientationGetRMat_f(&ahrs_fc.ltp_to_body);
// row 0 of ltp_to_body_rmat = body x-axis in ltp frame
// we only consider x and y
struct FloatVect2 expected_ltp = {
RMAT_ELMT(ltp_to_body_rmat, 0, 0),
RMAT_ELMT(ltp_to_body_rmat, 0, 1)
RMAT_ELMT(*ltp_to_body_rmat, 0, 0),
RMAT_ELMT(*ltp_to_body_rmat, 0, 1)
};
// expected_heading cross measured_heading
@@ -486,12 +487,11 @@ void ahrs_fc_realign_heading(float heading)
q_h_new.qz = sinf(heading / 2.0);
q_h_new.qi = cosf(heading / 2.0);
struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_fc.body_to_imu);
struct FloatQuat ltp_to_body_quat;
float_quat_comp_inv(&ltp_to_body_quat, &ahrs_fc.ltp_to_imu_quat, body_to_imu_quat);
ahrs_fc_recompute_ltp_to_body();
struct FloatQuat *ltp_to_body_quat = orientationGetQuat_f(&ahrs_fc.ltp_to_body);
/* quaternion representing current heading only */
struct FloatQuat q_h;
QUAT_COPY(q_h, ltp_to_body_quat);
struct FloatQuat q_h = *ltp_to_body_quat;
q_h.qx = 0.;
q_h.qy = 0.;
float_quat_normalize(&q_h);
@@ -502,9 +502,10 @@ void ahrs_fc_realign_heading(float heading)
/* correct current heading in body frame */
struct FloatQuat q;
float_quat_comp_norm_shortest(&q, &q_c, &ltp_to_body_quat);
float_quat_comp_norm_shortest(&q, &q_c, ltp_to_body_quat);
/* compute ltp to imu rotation quaternion and matrix */
struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_fc.body_to_imu);
float_quat_comp(&ahrs_fc.ltp_to_imu_quat, &q, body_to_imu_quat);
float_rmat_of_quat(&ahrs_fc.ltp_to_imu_rmat, &ahrs_fc.ltp_to_imu_quat);
@@ -526,3 +527,11 @@ void ahrs_fc_set_body_to_imu_quat(struct FloatQuat *q_b2i)
ahrs_fc.ltp_to_imu_rmat = *orientationGetRMat_f(&ahrs_fc.body_to_imu);
}
}
void ahrs_fc_recompute_ltp_to_body(void)
{
struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_fc.body_to_imu);
struct FloatQuat ltp_to_body_quat;
float_quat_comp_inv(&ltp_to_body_quat, &ahrs_fc.ltp_to_imu_quat, body_to_imu_quat);
orientationSetQuat_f(&ahrs_fc.ltp_to_body, &ltp_to_body_quat);
}
sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h
+2
View File
@@ -70,6 +70,7 @@ struct AhrsFloatCmpl {
uint16_t mag_cnt; ///< number of propagations since last mag update
struct OrientationReps body_to_imu;
struct OrientationReps ltp_to_body;
enum AhrsFCStatus status;
bool_t is_aligned;
@@ -80,6 +81,7 @@ extern struct AhrsFloatCmpl ahrs_fc;
extern void ahrs_fc_init(void);
extern void ahrs_fc_set_body_to_imu(struct OrientationReps *body_to_imu);
extern void ahrs_fc_set_body_to_imu_quat(struct FloatQuat *q_b2i);
extern void ahrs_fc_recompute_ltp_to_body(void);
extern bool_t ahrs_fc_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
struct Int32Vect3 *lp_mag);
extern void ahrs_fc_propagate(struct Int32Rates *gyro, float dt);
sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.c
+10 -16
View File
@@ -73,22 +73,17 @@ static void send_euler_int(struct transport_tx *trans, struct link_device *dev)
struct Int32Eulers eulers_imu;
EULERS_BFP_OF_REAL(eulers_imu, ltp_to_imu_euler);
/* compute Eulers in int (body frame) */
struct FloatQuat ltp_to_body_quat;
struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_fc.body_to_imu);
float_quat_comp_inv(&ltp_to_body_quat, &ahrs_fc.ltp_to_imu_quat, body_to_imu_quat);
struct FloatEulers ltp_to_body_euler;
float_eulers_of_quat(&ltp_to_body_euler, &ltp_to_body_quat);
struct Int32Eulers eulers_body;
EULERS_BFP_OF_REAL(eulers_body, ltp_to_body_euler);
/* get Eulers in int (body frame) */
ahrs_fc_recompute_ltp_to_body();
struct Int32Eulers *eulers_body = orientationGetEulers_i(&ahrs_fc.ltp_to_body);
pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID,
&eulers_imu.phi,
&eulers_imu.theta,
&eulers_imu.psi,
&eulers_body.phi,
&eulers_body.theta,
&eulers_body.psi,
&eulers_body->phi,
&eulers_body->theta,
&eulers_body->psi,
&ahrs_fc_id);
}
@@ -247,12 +242,11 @@ static bool_t ahrs_fc_enable_output(bool_t enable)
static void compute_body_orientation_and_rates(void)
{
if (ahrs_fc_output_enabled) {
/* Compute LTP to BODY quaternion */
struct FloatQuat ltp_to_body_quat;
struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_fc.body_to_imu);
float_quat_comp_inv(&ltp_to_body_quat, &ahrs_fc.ltp_to_imu_quat, body_to_imu_quat);
/* recompute LTP to BODY quaternion */
ahrs_fc_recompute_ltp_to_body();
struct FloatQuat *ltp_to_body_quat = orientationGetQuat_f(&ahrs_fc.ltp_to_body);
/* Set state */
stateSetNedToBodyQuat_f(&ltp_to_body_quat);
stateSetNedToBodyQuat_f(ltp_to_body_quat);
/* compute body rates */
struct FloatRates body_rate;