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

booz ahrs lkf and related sim stuff

Browse Source
This commit is contained in:
Felix Ruess
2009-07-30 23:59:04 +00:00
parent 42f4d4e91d
commit 33ef262d7f
10 changed files with 207 additions and 72 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/messages.xml
+31
View File
@@ -1224,6 +1224,37 @@
<field name="yaw_gamma_d" type="float" />
<field name="yaw_gamma_i" type="float" />
</message>
<message name="BOOZ_AHRS_LKF" id="193">
<field name="phi" type="float" unit="rad" alt_unit="degres" alt_unit_coef="57.29578" />
<field name="theta" type="float" unit="rad" alt_unit="degres" alt_unit_coef="57.29578" />
<field name="psi" type="float" unit="rad" alt_unit="degres" alt_unit_coef="57.29578" />
<field name="qi" type="float" />
<field name="qx" type="float" />
<field name="qy" type="float" />
<field name="qz" type="float" />
<field name="p" type="float" unit="rad/s" alt_unit="degres/s" alt_unit_coef="57.29578" />
<field name="q" type="float" unit="rad/s" alt_unit="degres/s" alt_unit_coef="57.29578" />
<field name="r" type="float" unit="rad/s" alt_unit="degres/s" alt_unit_coef="57.29578" />
<field name="bp" type="float" unit="rad/s" alt_unit="degres/s" alt_unit_coef="57.29578" />
<field name="bq" type="float" unit="rad/s" alt_unit="degres/s" alt_unit_coef="57.29578" />
<field name="br" type="float" unit="rad/s" alt_unit="degres/s" alt_unit_coef="57.29578" />
</message>
<message name="BOOZ_AHRS_LKF_DEBUG" id="194">
<field name="phi_err" type="float" unit="rad" alt_unit="degres" alt_unit_coef="57.29578" />
<field name="theta_err" type="float" unit="rad" alt_unit="degres" alt_unit_coef="57.29578" />
<field name="psi_err" type="float" unit="rad" alt_unit="degres" alt_unit_coef="57.29578" />
<field name="bp_err" type="float" unit="rad/s" alt_unit="degres/s" alt_unit_coef="57.29578" />
<field name="bq_err" type="float" unit="rad/s" alt_unit="degres/s" alt_unit_coef="57.29578" />
<field name="br_err" type="float" unit="rad/s" alt_unit="degres/s" alt_unit_coef="57.29578" />
<field name="phi_cov" type="float" />
<field name="theta_cov" type="float" />
<field name="psi_cov" type="float" />
<field name="bp_cov" type="float" />
<field name="bq_cov" type="float" />
<field name="br_cov" type="float" />
</message>
</class>
conf/settings/settings_booz2.xml
+1 -1
View File
@@ -4,7 +4,7 @@
<dl_settings>
<dl_settings NAME="Misc">
<dl_setting var="telemetry_mode_Main" min="0" step="1" max="11" module="booz2_telemetry" shortname="telemetry" values="Default|PPM|Raw sensors|Scaled sensors|AHRS|Rate loop|Att loop|Vert loop|H loop|Aligner|HS_att_roll|Tune_hover"/>
<dl_setting var="telemetry_mode_Main" min="0" step="1" max="12" module="booz2_telemetry" shortname="telemetry" values="Default|PPM|Raw sensors|Scaled sensors|AHRS|AHRS LKF|Rate loop|Att loop|Vert loop|H loop|Aligner|HS_att_roll|Tune_hover"/>
<dl_setting var="booz2_autopilot_mode_auto2" min="0" step="1" max="12" module="booz2_autopilot" shortname="auto2" values="Fail|Kill|Rate|Att|Rate_rcC|Att_rcC|Att_C|Rate_Z|Att_Z|Hover|Hover_C|Hover_Z|Nav"/>
<dl_setting var="booz2_autopilot_tol" min="0" step="1" max="2" module="booz2_autopilot" shortname="tol" values="RIEN|TO|LA" handler="SetTol"/>
</dl_settings>
conf/telemetry/telemetry_booz2_flixr.xml
+42 -32
View File
@@ -3,30 +3,6 @@
<telemetry>
<process name="Imu">
<mode name="default">
<message name="IMU_ACCEL_RAW" period=".21"/>
<message name="IMU_GYRO_RAW" period=".22"/>
<message name="IMU_MAG_RAW" period=".23"/>
<!-- <message name="IMU_DEBUG" period=".24"/>-->
</mode>
<mode name="raw_sensors">
</mode>
<mode name="scaled_sensors">
</mode>
<mode name="simulation">
</mode>
</process>
<process name="Main">
<mode name="default">
@@ -34,14 +10,19 @@
<message name="BOOZ_STATUS" period="1.2"/>
<message name="BOOZ2_FP" period="0.25"/>
<message name="ALIVE" period="2.1"/>
<message name="BOOZ2_NAV_REF" period="5."/>
<message name="BOOZ2_NAV_STATUS" period="1.6"/>
<message name="WP_MOVED" period="1.3"/>
<message name="BOOZ2_GPS" period=".25"/>
</mode>
<mode name="ppm">
<message name="PPM" period="0.5"/>
<message name="RC" period="0.5"/>
<message name="PPM" period="0.5"/>
<message name="RC" period="0.5"/>
<message name="BOOZ2_RADIO_CONTROL" period="0.5"/>
<message name="BOOZ_STATUS" period="1"/>
</mode>
<mode name="raw_sensors">
<message name="BOOZ_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
@@ -49,6 +30,7 @@
<message name="IMU_ACCEL_RAW" period=".05"/>
<message name="IMU_GYRO_RAW" period=".05"/>
<message name="IMU_MAG_RAW" period=".05"/>
<message name="BOOZ2_BARO_RAW" period=".1"/>
</mode>
<mode name="scaled_sensors">
@@ -60,12 +42,24 @@
<message name="BOOZ2_MAG" period=".1"/>
</mode>
<mode name="filter">
<mode name="ahrs">
<message name="BOOZ_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<!-- <message name="BOOZ2_ALIGNER" period=".1"/> -->
<message name="BOOZ2_FILTER" period=".02"/>
<message name="BOOZ2_FILTER" period=".5"/>
<!-- <message name="BOOZ2_AHRS_QUAT" period=".25"/> -->
<message name="BOOZ2_AHRS_EULER" period=".1"/>
<!-- <message name="BOOZ2_AHRS_RMAT" period=".5"/> -->
</mode>
<mode name="ahrs_lkf">
<message name="BOOZ_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<message name="BOOZ_AHRS_LKF" period=".02"/>
<message name="BOOZ_AHRS_LKF_DEBUG" period=".1" />
<message name="BOOZ2_GYRO" period=".02"/>
</mode>
<mode name="rate_loop">
@@ -80,14 +74,18 @@
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="0.9"/>
<message name="BOOZ2_STAB_ATTITUDE" period=".03"/>
<message name="BOOZ2_STAB_ATTITUDE_REF" period=".2"/>
<message name="BOOZ2_STAB_ATTITUDE_REF" period=".03"/>
</mode>
<mode name="vert_loop">
<message name="BOOZ_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="0.9"/>
<message name="BOOZ2_VFF" period=".05"/>
<message name="BOOZ2_VERT_LOOP" period=".05"/>
<!-- <message name="BOOZ2_CMD" period=".05"/> -->
<message name="BOOZ2_INS" period=".05"/>
<message name="BOOZ2_INS_REF" period="5.1"/>
</mode>
<mode name="h_loop">
@@ -95,6 +93,9 @@
<message name="BOOZ2_HOVER_LOOP" period="0.25"/>
<message name="BOOZ2_STAB_ATTITUDE" period=".2"/>
<message name="BOOZ2_STAB_ATTITUDE_REF" period=".2"/>
<message name="BOOZ2_FP" period="0.25"/>
<message name="BOOZ_STATUS" period="1.2"/>
<message name="BOOZ2_NAV_REF" period="5."/>
</mode>
<mode name="aligner">
@@ -106,10 +107,19 @@
<message name="BOOZ_STATUS" period="1.2"/>
<message name="ALIVE" period="0.9"/>
<message name="DL_VALUE" period="0.5"/>
<message name="BOOZ2_STAB_ATTITUDE_HS_ROLL" period="0.02"/>
<!-- <message name="BOOZ2_STAB_ATTITUDE_HS_ROLL" period="0.02"/> -->
</mode>
<mode name="tune_hover">
<message name="BOOZ2_TUNE_HOVER" period=".05"/>
<message name="BOOZ2_GPS" period=".25"/>
<!--
<message name="BOOZ2_INS2" period=".05"/>
<message name="BOOZ2_INS3" period=".20"/>
<message name="BOOZ2_INS_REF" period="5.1"/> -->
</mode>
</process>
</telemetry>
sw/airborne/booz/ahrs/booz_ahrs_float_lkf.c
+48 -14
View File
@@ -33,24 +33,29 @@
struct BoozAhrs booz_ahrs;
/* our estimated attitude */
/* our estimated attitude (ltp <-> imu) */
struct FloatQuat bafl_quat;
/* our estimated gyro biases */
struct FloatRates bafl_bias;
/* our estimated attitude errors */
struct FloatQuat bafl_quat_err;
/* our estimated gyro bias errors */
struct FloatRates bafl_bias_err;
/* we get unbiased body rates as byproduct */
struct FloatRates bafl_rates;
/* maintain a euler angles representation */
struct FloatEulers bafl_eulers;
/* maintains a rotation matrix representation */
struct FloatRMat bafl_dcm;
/* our estimated attitude errors */
struct FloatQuat bafl_quat_err;
/* our estimated gyro bias errors */
struct FloatRates bafl_bias_err;
/* time derivative of our quaternion */
struct FloatQuat bafl_qdot;
/* correction quaternion of strapdown computatino */
struct FloatQuat bafl_qr;
#ifndef BAFL_SSIZE
#define BAFL_SSIZE 6
#endif
/* error covariance matrix */
float bafl_P[BAFL_SSIZE][BAFL_SSIZE];
/* filter state */
@@ -121,10 +126,6 @@ struct FloatVect3 bafl_h;
* We only have an expected noise in the pitch and roll accelerometers
* and in the compass.
*/
#define BAFL_R_PHI 1.3 * 1.3
#define BAFL_R_THETA 1.3 * 1.3
#define BAFL_R_PSI 2.5 * 2.5
#define BAFL_SIGMA_ACCEL 5.0
#define BAFL_SIGMA_MAG 300.
float bafl_sigma_accel;
@@ -201,14 +202,47 @@ void booz_ahrs_propagate(void) {
*/
/* compute qdot and normalize it */
FLOAT_QUAT_DERIVATIVE_LAGRANGE(bafl_qdot, bafl_rates, bafl_quat);
//FLOAT_QUAT_DERIVATIVE(bafl_qdot, bafl_rates, bafl_quat);
/* multiply qdot with dt */
QUAT_SMUL(bafl_qdot, bafl_qdot, BAFL_DT);
//QUAT_SMUL(bafl_qdot, bafl_qdot, BAFL_DT);
/* propagate quaternion */
QUAT_ADD(bafl_quat, bafl_qdot);
//QUAT_ADD(bafl_quat, bafl_qdot);
/* multiplicative quaternion update */
QUAT_ASSIGN(bafl_qr, 1., bafl_rates.p * BAFL_DT/2, bafl_rates.q * BAFL_DT/2, bafl_rates.r * BAFL_DT/2);
FLOAT_QUAT_COMP(bafl_quat, bafl_qr, bafl_quat);
FLOAT_QUAT_NORMALISE(bafl_quat);
/* maintain rotation matrix representation */
FLOAT_RMAT_OF_QUAT(bafl_dcm, bafl_quat);
/* maintain euler representation */
FLOAT_EULERS_OF_RMAT(bafl_eulers, bafl_dcm);
/*
* convert to binary floating point
*/
/* IMU rate */
RATES_BFP_OF_REAL(booz_ahrs.imu_rate, bafl_rates);
/* LTP to IMU eulers */
EULERS_BFP_OF_REAL(booz_ahrs.ltp_to_imu_euler, bafl_eulers);
/* LTP to IMU quaternion */
QUAT_BFP_OF_REAL(booz_ahrs.ltp_to_imu_quat, bafl_quat);
/* LTP to IMU rotation matrix */
RMAT_BFP_OF_REAL(booz_ahrs.ltp_to_imu_rmat, bafl_dcm);
/* Compute LTP to BODY quaternion */
INT32_QUAT_COMP_INV(booz_ahrs.ltp_to_body_quat, booz_imu.body_to_imu_quat, booz_ahrs.ltp_to_imu_quat);
/* Compute LTP to BODY rotation matrix */
INT32_RMAT_COMP_INV(booz_ahrs.ltp_to_body_rmat, booz_ahrs.ltp_to_imu_rmat, booz_imu.body_to_imu_rmat);
/* compute LTP to BODY eulers */
INT32_EULERS_OF_RMAT(booz_ahrs.ltp_to_body_euler, booz_ahrs.ltp_to_body_rmat);
/* compute body rates */
INT32_RMAT_TRANSP_RATEMULT(booz_ahrs.body_rate, booz_imu.body_to_imu_rmat, booz_ahrs.imu_rate);
//TODO check if normalization is good
@@ -479,7 +513,7 @@ void booz_ahrs_update_accel(void) {
/* correct attitude
*/
FLOAT_QUAT_COMP(bafl_quat, bafl_quat, bafl_quat_err);
FLOAT_QUAT_COMP(bafl_quat, bafl_quat_err, bafl_quat);
/* correct gyro bias
*/
@@ -719,7 +753,7 @@ void booz_ahrs_update_mag(void) {
/* correct attitude
*/
FLOAT_QUAT_COMP(bafl_quat, bafl_quat, bafl_quat_err);
FLOAT_QUAT_COMP(bafl_quat, bafl_quat_err, bafl_quat);
/* correct gyro bias
*/
sw/airborne/booz/ahrs/booz_ahrs_float_lkf.h
+13
View File
@@ -29,6 +29,19 @@
#include "std.h"
#include "math/pprz_algebra_int.h"
extern struct FloatQuat bafl_quat;
extern struct FloatRates bafl_bias;
extern struct FloatRates bafl_rates;
extern struct FloatEulers bafl_eulers;
extern struct FloatRMat bafl_dcm;
extern struct FloatQuat bafl_quat_err;
extern struct FloatRates bafl_bias_err;
#define BAFL_SSIZE 6
extern float bafl_P[BAFL_SSIZE][BAFL_SSIZE];
extern float bafl_X[BAFL_SSIZE];
extern float bafl_sigma_accel;
extern float bafl_sigma_mag;
extern float bafl_R_accel;
sw/airborne/booz/booz2_main.c
+2 -2
View File
@@ -183,7 +183,7 @@ static inline void on_gyro_accel_event( void ) {
else {
booz_ahrs_propagate();
#ifdef USE_AHRS_LKF
booz_ahrs_update_accel();
//booz_ahrs_update_accel();
#endif
// booz2_filter_attitude_update();
booz_ins_propagate();
@@ -202,6 +202,6 @@ static inline void on_gps_event(void) {
static inline void on_mag_event(void) {
BoozImuScaleMag();
#ifdef USE_AHRS_LKF
booz_ahrs_update_mag();
//booz_ahrs_update_mag();
#endif
}
sw/airborne/booz/booz2_telemetry.h
+37
View File
@@ -288,6 +288,43 @@
#define PERIODIC_SEND_BOOZ2_FILTER() {}
#endif
#ifdef USE_AHRS_LKF
#include "booz_ahrs.h"
#include "ahrs/booz_ahrs_float_lkf.h"
#define PERIODIC_SEND_BOOZ_AHRS_LKF() { \
DOWNLINK_SEND_BOOZ_AHRS_LKF(&bafl_eulers.phi, \
&bafl_eulers.theta, \
&bafl_eulers.psi, \
&bafl_quat.qi, \
&bafl_quat.qx, \
&bafl_quat.qy, \
&bafl_quat.qz, \
&bafl_rates.p, \
&bafl_rates.q, \
&bafl_rates.r, \
&bafl_bias.p, \
&bafl_bias.q, \
&bafl_bias.r); \
}
#define PERIODIC_SEND_BOOZ_AHRS_LKF_DEBUG() { \
DOWNLINK_SEND_BOOZ_AHRS_LKF_DEBUG(&bafl_X[0], \
&bafl_X[1], \
&bafl_X[2], \
&bafl_bias_err.p, \
&bafl_bias_err.q, \
&bafl_bias_err.r, \
&bafl_P[0][0], \
&bafl_P[1][1], \
&bafl_P[2][2], \
&bafl_P[3][3], \
&bafl_P[4][4], \
&bafl_P[5][5]); \
}
#else
#define PERIODIC_SEND_BOOZ_AHRS_LKF() {}
#define PERIODIC_SEND_BOOZ_AHRS_LKF_DEBUG() {}
#endif
#define PERIODIC_SEND_BOOZ2_AHRS_QUAT() { \
DOWNLINK_SEND_BOOZ2_AHRS_QUAT(&booz_ahrs.ltp_to_imu_quat.qi, \
sw/airborne/math/pprz_algebra.h
+8
View File
@@ -376,6 +376,14 @@
// Quaternions
//
//
#define QUAT_ASSIGN(_q, _i, _x, _y, _z) { \
(_q).qi = (_i); \
(_q).qx = (_x); \
(_q).qy = (_y); \
(_q).qz = (_z); \
}
#define QUAT_DIFF(_qc, _qa, _qb) { \
(_qc).qi = (_qa).qi - (_qb).qi; \
sw/airborne/math/pprz_algebra_float.h
+1 -6
View File
@@ -299,12 +299,7 @@ struct FloatRates {
* Quaternions
*/
#define FLOAT_QUAT_ZERO(_q) { \
(_q).qi = 1.; \
(_q).qx = 0.; \
(_q).qy = 0.; \
(_q).qz = 0.; \
}
#define FLOAT_QUAT_ZERO(_q) QUAT_ASSIGN(_q, 1., 0., 0., 0.)
/* _q += _qa */
#define FLOAT_QUAT_ADD(_qo, _qi) QUAT_ADD(_qo, _qi)
sw/simulator/nps/nps_ivy.c
+24 -17
View File
@@ -9,8 +9,8 @@
#include "nps_autopilot.h"
#include "nps_fdm.h"
static void on_DL_SETTING(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
static void on_DL_SETTING(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]);
void nps_ivy_init(void) {
@@ -26,8 +26,8 @@ void nps_ivy_init(void) {
#include "settings.h"
#include "dl_protocol.h"
#include "downlink.h"
static void on_DL_SETTING(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
static void on_DL_SETTING(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]) {
uint8_t index = atoi(argv[2]);
float value = atof(argv[3]);
@@ -40,27 +40,34 @@ void nps_ivy_display(void) {
/*
IvySendMsg("%d COMMANDS %f %f %f %f",
IvySendMsg("%d COMMANDS %f %f %f %f",
AC_ID,
autopilot.commands[SERVO_FRONT],
autopilot.commands[SERVO_BACK],
autopilot.commands[SERVO_BACK],
autopilot.commands[SERVO_RIGHT],
autopilot.commands[SERVO_LEFT]);
autopilot.commands[SERVO_LEFT]);
*/
IvySendMsg("%d BOOZ_SIM_RATE_ATTITUDE %f %f %f %f %f %f",
IvySendMsg("%d BOOZ_SIM_RATE_ATTITUDE %f %f %f %f %f %f",
AC_ID,
DegOfRad(fdm.body_ecef_rotvel.p),
DegOfRad(fdm.body_ecef_rotvel.q),
DegOfRad(fdm.body_ecef_rotvel.p),
DegOfRad(fdm.body_ecef_rotvel.q),
DegOfRad(fdm.body_ecef_rotvel.r),
DegOfRad(fdm.ltp_to_body_eulers.phi),
DegOfRad(fdm.ltp_to_body_eulers.theta),
DegOfRad(fdm.ltp_to_body_eulers.phi),
DegOfRad(fdm.ltp_to_body_eulers.theta),
DegOfRad(fdm.ltp_to_body_eulers.psi));
IvySendMsg("%d BOOZ_SIM_SPEED_POS %f %f %f %f %f %f",
IvySendMsg("%d BOOZ_SIM_SPEED_POS %f %f %f %f %f %f",
AC_ID,
(fdm.ltp_ecef_vel.x),
(fdm.ltp_ecef_vel.y),
(fdm.ltp_ecef_vel.x),
(fdm.ltp_ecef_vel.y),
(fdm.ltp_ecef_vel.z),
(fdm.ltpprz_pos.x),
(fdm.ltpprz_pos.y),
(fdm.ltpprz_pos.x),
(fdm.ltpprz_pos.y),
(fdm.ltpprz_pos.z));
#if 0
IvySendMsg("%d BOOZ_SIM_GYRO_BIAS %f %f %f",
AC_ID,
(sensors.gyro.bias_random_walk_value.p),
(sensors.gyro.bias_random_walk_value.q),
(sensors.gyro.bias_random_walk_value.r));
#endif
}