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

booz updates from mark

Browse Source
This commit is contained in:
Allen Ibara
2009-08-12 18:30:22 +00:00
parent a88d4ba2ca
commit 549db8693b
4 changed files with 28 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/booz/ahrs/booz2_filter_attitude_cmpl_euler.c
-2
View File
@@ -29,8 +29,6 @@
#include "airframe.h"
#include "math/pprz_trig_int.h"
struct BoozAhrs booz_ahrs;
struct Int32Rates booz2_face_gyro_bias;
struct Int32Eulers booz2_face_measure;
sw/airborne/booz/booz_ahrs.h
+17
View File
@@ -26,6 +26,7 @@
#include "std.h"
#include "math/pprz_algebra_int.h"
#include "math/pprz_algebra_float.h"
#include "ahrs/booz_ahrs_aligner.h"
#define BOOZ_AHRS_UNINIT 0
@@ -43,7 +44,23 @@ struct BoozAhrs {
uint8_t status;
};
struct BoozAhrsFloat {
struct FloatQuat ltp_to_body_quat;
struct FloatEulers ltp_to_body_euler;
struct FloatRates body_rate;
uint8_t status;
};
extern struct BoozAhrs booz_ahrs;
extern struct BoozAhrsFloat booz_ahrs_float;
#define BOOZ_AHRS_FLOAT_OF_INT32() { \
QUAT_FLOAT_OF_BFP(booz_ahrs_float.ltp_to_body_quat, booz_ahrs.ltp_to_body_quat); \
RATES_FLOAT_OF_BFP(booz_ahrs_float.body_rate, booz_ahrs.body_rate); \
booz_ahrs_float.ltp_to_body_euler.phi = ANGLE_FLOAT_OF_BFP(booz_ahrs.ltp_to_body_euler.phi); \
booz_ahrs_float.ltp_to_body_euler.theta = ANGLE_FLOAT_OF_BFP(booz_ahrs.ltp_to_body_euler.theta); \
booz_ahrs_float.ltp_to_body_euler.psi = ANGLE_FLOAT_OF_BFP(booz_ahrs.ltp_to_body_euler.psi); \
}
extern void booz_ahrs_init(void);
extern void booz_ahrs_align(void);
sw/airborne/booz/stabilization/booz_stabilization_attitude_quat_float.c
+9 -6
View File
@@ -87,7 +87,11 @@ void booz_stabilization_attitude_enter(void) {
#define MAX_SUM_ERR RadOfDeg(56000)
void booz_stabilization_attitude_run(bool_t in_flight) {
#ifdef BOOZ_AHRS_FIXED_POINT
BOOZ_AHRS_FLOAT_OF_INT32();
#endif
/*
* Update reference
*/
@@ -109,10 +113,10 @@ void booz_stabilization_attitude_run(bool_t in_flight) {
*/
/* attitude error */
struct FloatQuat att_quat_float;
QUAT_FLOAT_OF_BFP(att_quat_float, booz_ahrs.ltp_to_body_quat);
struct FloatQuat att_err;
FLOAT_QUAT_INV_COMP(att_err, att_quat_float, booz_stab_att_ref_quat);
FLOAT_QUAT_INV_COMP(att_err, booz_ahrs_float.ltp_to_body_quat, booz_stab_att_ref_quat);
/* wrap it in the shortest direction */
FLOAT_QUAT_WRAP_SHORTEST(att_err);
@@ -127,10 +131,9 @@ void booz_stabilization_attitude_run(bool_t in_flight) {
}
/* rate error */
struct FloatRates rate_float;
RATES_FLOAT_OF_BFP(rate_float, booz_ahrs.body_rate);
struct FloatRates rate_err;
RATES_DIFF(rate_err, rate_float, booz_stab_att_ref_rate);
RATES_DIFF(rate_err, booz_ahrs_float.body_rate, booz_stab_att_ref_rate);
/* PID */
sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_quat_float.h
+2 -2
View File
@@ -50,7 +50,7 @@
} \
} \
else { /* if not flying, use current yaw as setpoint */ \
_sp.psi = ANGLE_FLOAT_OF_BFP(booz_ahrs.ltp_to_body_euler.psi); \
_sp.psi = booz_ahrs_float.ltp_to_body_euler.psi; \
} \
\
struct FloatRMat sp_rmat; \
@@ -65,7 +65,7 @@
booz_stab_att_ref_euler.psi = _sp.psi; \
booz_stab_att_ref_rate.r = 0; \
struct FloatRMat sp_rmat; \
/* FLOAT_RMAT_OF_EULERS_312(sp_rmat, _sp); */ \
/*FLOAT_RMAT_OF_EULERS_312(sp_rmat, _sp);*/ \
FLOAT_RMAT_OF_EULERS_321(sp_rmat, _sp); \
FLOAT_QUAT_OF_RMAT(booz_stab_att_sp_quat, sp_rmat); \
/*FLOAT_EULERS_OF_QUAT(sp_euler321, sp_quat);*/ \