Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 22:49:53 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

rename booz_ahrs to ahrs, fix makefiles

Browse Source
This commit is contained in:
Felix Ruess
2010-09-27 22:56:39 +00:00
parent 9a893dcdf2
commit c2a645a7ee
46 changed files with 407 additions and 408 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/Poine/booz2_a1p.xml
+1 -1
View File
@@ -221,7 +221,7 @@ include $(CFG_BOOZ)/subsystems/booz2_actuators_mkk.makefile
include $(CFG_BOOZ)/subsystems/booz2_imu_b2v1.makefile
include $(CFG_BOOZ)/subsystems/booz2_gps.makefile
include $(CFG_BOOZ)/subsystems/booz_ahrs_mlkf.makefile
include $(CFG_BOOZ)/subsystems/ahrs_mlkf.makefile
include $(CFG_BOOZ)/subsystems/booz2_fms_test_signal.makefile
conf/autopilot/mercury.makefile
+1 -1
View File
@@ -73,7 +73,7 @@ ap.CFLAGS += -DPPRZ_TRIG_CONST=const
ap.srcs += $(SRC_CSC)/mercury_xsens.c $(SRC_BOOZ)/booz_imu.c math/pprz_trig_int.c
ap.CFLAGS += -DXSENS1_LINK=Uart0 -DBOOZ_IMU_TYPE_H=\"mercury_xsens.h\"
ap.srcs += $(SRC_BOOZ)/ahrs/booz_ahrs_cmpl_euler.c $(SRC_BOOZ)/ahrs/booz_ahrs_aligner.c
ap.srcs += $(SRC_BOOZ)/ahrs/ahrs_cmpl_euler.c $(SRC_BOOZ)/ahrs/ahrs_aligner.c
ap.CFLAGS += -DFLOAT_T=float
ap.srcs += $(SRC_BOOZ)/booz_stabilization.c
conf/autopilot/subsystems/rotorcraft/ahrs_cmpl.makefile
+8 -8
View File
@@ -2,12 +2,12 @@
# Complementary filter for attitude estimation
#
ap.CFLAGS += -DUSE_AHRS_CMPL -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) -DBOOZ_AHRS_FIXED_POINT
ap.srcs += $(SRC_BOOZ)/booz_ahrs.c
ap.srcs += $(SRC_BOOZ)/ahrs/booz_ahrs_aligner.c
ap.srcs += $(SRC_BOOZ)/ahrs/booz_ahrs_cmpl_euler.c
ap.CFLAGS += -DUSE_AHRS_CMPL -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) -DAHRS_FIXED_POINT
ap.srcs += $(SRC_FIRMWARE)/ahrs.c
ap.srcs += $(SRC_FIRMWARE)/ahrs/ahrs_aligner.c
ap.srcs += $(SRC_FIRMWARE)/ahrs/ahrs_cmpl_euler.c
sim.CFLAGS += -DUSE_AHRS_CMPL -DAHRS_ALIGNER_LED=3 -DBOOZ_AHRS_FIXED_POINT
sim.srcs += $(SRC_BOOZ)/booz_ahrs.c
sim.srcs += $(SRC_BOOZ)/ahrs/booz_ahrs_aligner.c
sim.srcs += $(SRC_BOOZ)/ahrs/booz_ahrs_cmpl_euler.c
sim.CFLAGS += -DUSE_AHRS_CMPL -DAHRS_ALIGNER_LED=3 -DAHRS_FIXED_POINT
sim.srcs += $(SRC_FIRMWARE)/ahrs.c
sim.srcs += $(SRC_FIRMWARE)/ahrs/ahrs_aligner.c
sim.srcs += $(SRC_FIRMWARE)/ahrs/ahrs_cmpl_euler.c
conf/autopilot/subsystems/rotorcraft/ahrs_lkf.makefile
+6 -6
View File
@@ -3,11 +3,11 @@
#
ap.CFLAGS += -DUSE_AHRS_LKF -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
ap.srcs += $(SRC_BOOZ)/booz_ahrs.c
ap.srcs += $(SRC_BOOZ)/ahrs/booz_ahrs_aligner.c
ap.srcs += $(SRC_BOOZ)/ahrs/booz_ahrs_float_lkf.c
ap.srcs += $(SRC_FIRMWARE)/ahrs.c
ap.srcs += $(SRC_FIRMWARE)/ahrs/ahrs_aligner.c
ap.srcs += $(SRC_FIRMWARE)/ahrs/ahrs_float_lkf.c
sim.CFLAGS += -DUSE_AHRS_LKF -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
sim.srcs += $(SRC_BOOZ)/booz_ahrs.c
sim.srcs += $(SRC_BOOZ)/ahrs/booz_ahrs_aligner.c
sim.srcs += $(SRC_BOOZ)/ahrs/booz_ahrs_float_lkf.c
sim.srcs += $(SRC_FIRMWARE)/ahrs.c
sim.srcs += $(SRC_FIRMWARE)/ahrs/ahrs_aligner.c
sim.srcs += $(SRC_FIRMWARE)/ahrs/ahrs_float_lkf.c
conf/autopilot/subsystems/rotorcraft/ahrs_mlkf.makefile
+4 -4
View File
@@ -3,14 +3,14 @@
#
ap.CFLAGS += -DAHRS_ALIGNER_LED=3
ap.srcs += $(SRC_BOOZ)/booz_ahrs.c
ap.srcs += $(SRC_BOOZ)/ahrs/booz_ahrs_aligner.c
ap.srcs += $(SRC_FIRMWARE)/ahrs.c
ap.srcs += $(SRC_FIRMWARE)/ahrs/ahrs_aligner.c
ap.srcs += $(SRC_BOOZ_PRIV)/ahrs/booz_ahrs_mlkf.c
ap.srcs += $(SRC_BOOZ_PRIV)/ahrs/booz_ahrs_opt.c
sim.CFLAGS += -I$(SRC_BOOZ_PRIV)
sim.CFLAGS += -DAHRS_ALIGNER_LED=3
sim.srcs += $(SRC_BOOZ)/booz_ahrs.c
sim.srcs += $(SRC_BOOZ)/ahrs/booz_ahrs_aligner.c
sim.srcs += $(SRC_FIRMWARE)/ahrs.c
sim.srcs += $(SRC_FIRMWARE)/ahrs/ahrs_aligner.c
sim.srcs += $(SRC_BOOZ_PRIV)/ahrs/booz_ahrs_mlkf.c
sim.srcs += $(SRC_BOOZ_PRIV)/ahrs/booz_ahrs_mlkf_opt.c
conf/control_panel.xml.adelaide
+3 -3
View File
@@ -111,7 +111,7 @@
<arg flag="-c" constant="'*:telemetry:BOOZ_SIM_RATE_ATTITUDE:phi'"/>
<arg flag="-c" constant="'*:telemetry:BOOZ_ATT_LOOP:est_phi:57.3'"/>
<arg flag="-c" constant="'*:telemetry:BOOZ_ATT_LOOP:sp_phi:57.3'"/>
<arg flag="-c" constant="'*:telemetry:BOOZ_AHRS_MEASURE:phi:57.3'"/>
<arg flag="-c" constant="'*:telemetry:AHRS_MEASURE:phi:57.3'"/>
</program>
<program name="Real-time Plotter">
<arg flag="-t" constant="attitude : theta"/>
@@ -119,13 +119,13 @@
<arg flag="-c" constant="'*:telemetry:BOOZ_SIM_RATE_ATTITUDE:theta'"/>
<arg flag="-c" constant="'*:telemetry:BOOZ_ATT_LOOP:est_theta:57.3'"/>
<arg flag="-c" constant="'*:telemetry:BOOZ_ATT_LOOP:sp_theta:57.3'"/>
<arg flag="-c" constant="'*:telemetry:BOOZ_AHRS_MEASURE:theta:57.3'"/>
<arg flag="-c" constant="'*:telemetry:AHRS_MEASURE:theta:57.3'"/>
</program>
<program name="Real-time Plotter">
<arg flag="-t" constant="attitude : psi"/>
<arg flag="-u" constant="0.1"/>
<arg flag="-c" constant="'*:telemetry:BOOZ_SIM_RATE_ATTITUDE:psi'"/>
<arg flag="-c" constant="'*:telemetry:BOOZ_AHRS_MEASURE:psi:57.3'"/>
<arg flag="-c" constant="'*:telemetry:AHRS_MEASURE:psi:57.3'"/>
</program>
<program name="Real-time Plotter">
<arg flag="-t" constant="speed : u v w"/>
conf/control_panel.xml.example
+3 -3
View File
@@ -140,7 +140,7 @@
<arg flag="-c" constant="'*:telemetry:BOOZ_SIM_RATE_ATTITUDE:phi'"/>
<arg flag="-c" constant="'*:telemetry:BOOZ_ATT_LOOP:est_phi:57.3'"/>
<arg flag="-c" constant="'*:telemetry:BOOZ_ATT_LOOP:sp_phi:57.3'"/>
<arg flag="-c" constant="'*:telemetry:BOOZ_AHRS_MEASURE:phi:57.3'"/>
<arg flag="-c" constant="'*:telemetry:AHRS_MEASURE:phi:57.3'"/>
</program>
<program name="Real-time Plotter">
@@ -149,7 +149,7 @@
<arg flag="-c" constant="'*:telemetry:BOOZ_SIM_RATE_ATTITUDE:theta'"/>
<arg flag="-c" constant="'*:telemetry:BOOZ_ATT_LOOP:est_theta:57.3'"/>
<arg flag="-c" constant="'*:telemetry:BOOZ_ATT_LOOP:sp_theta:57.3'"/>
<arg flag="-c" constant="'*:telemetry:BOOZ_AHRS_MEASURE:theta:57.3'"/>
<arg flag="-c" constant="'*:telemetry:AHRS_MEASURE:theta:57.3'"/>
</program>
<program name="Real-time Plotter">
@@ -158,7 +158,7 @@
<arg flag="-c" constant="'*:telemetry:BOOZ_SIM_RATE_ATTITUDE:psi'"/>
<!-- <arg flag="-c" constant="'*:telemetry:BOOZ_ATT_LOOP:est_psi:57.3'"/> -->
<!-- <arg flag="-c" constant="'*:telemetry:BOOZ_ATT_LOOP:sp_psi:57.3'"/> -->
<arg flag="-c" constant="'*:telemetry:BOOZ_AHRS_MEASURE:psi:57.3'"/>
<arg flag="-c" constant="'*:telemetry:AHRS_MEASURE:psi:57.3'"/>
</program>
<program name="Real-time Plotter">
conf/messages.xml
+4 -4
View File
@@ -1404,7 +1404,7 @@
<field name="yaw_gamma_i" type="float" />
</message>
<message name="BOOZ_AHRS_LKF" id="193">
<message name="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" />
@@ -1423,7 +1423,7 @@
<field name="mz" type="float" />
</message>
<message name="BOOZ_AHRS_LKF_DEBUG" id="194">
<message name="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" />
@@ -1441,7 +1441,7 @@
<field name="br_cov" type="float" />
</message>
<message name="BOOZ_AHRS_LKF_ACC_DBG" id="195">
<message name="AHRS_LKF_ACC_DBG" id="195">
<field name="qi_err" type="float" />
<field name="qx_err" type="float" />
<field name="qy_err" type="float" />
@@ -1451,7 +1451,7 @@
<field name="br_err" type="float" unit="rad/s" alt_unit="degres/s" alt_unit_coef="57.29578" />
</message>
<message name="BOOZ_AHRS_LKF_MAG_DBG" id="196">
<message name="AHRS_LKF_MAG_DBG" id="196">
<field name="qi_err" type="float" />
<field name="qx_err" type="float" />
<field name="qy_err" type="float" />
conf/settings/settings_booz2_ahrs_cmpl.xml
+1 -1
View File
@@ -4,7 +4,7 @@
<dl_settings>
<dl_settings NAME="Filter">
<dl_setting var="booz2_face_reinj_1" min="512" step="512" max="262144" module="ahrs/booz_ahrs_cmpl_euler" shortname="reinj_1"/>
<dl_setting var="face_reinj_1" min="512" step="512" max="262144" module="ahrs/ahrs_cmpl_euler" shortname="reinj_1"/>
</dl_settings>
</dl_settings>
conf/settings/settings_booz2_ahrs_lkf.xml
+4 -4
View File
@@ -4,10 +4,10 @@
<dl_settings>
<dl_settings NAME="Filter">
<dl_setting var="bafl_sigma_accel" min="0.1" step="1." max="1000.0" module="ahrs/booz_ahrs_float_lkf" shortname="sigma_accel" handler="SetRaccel"/>
<dl_setting var="bafl_sigma_mag" min="1.0" step="1.0" max="1000.0" module="ahrs/booz_ahrs_float_lkf" shortname="sigma_mag" handler="SetRmag"/>
<dl_setting var="bafl_Q_att" min="0.0" step="0.000001" max="0.1" module="ahrs/booz_ahrs_float_lkf" shortname="Q att"/>
<dl_setting var="bafl_Q_gyro" min="0.0" step="0.000001" max="0.1" module="ahrs/booz_ahrs_float_lkf" shortname="Q gyro"/>
<dl_setting var="bafl_sigma_accel" min="0.1" step="1." max="1000.0" module="ahrs/ahrs_float_lkf" shortname="sigma_accel" handler="SetRaccel"/>
<dl_setting var="bafl_sigma_mag" min="1.0" step="1.0" max="1000.0" module="ahrs/ahrs_float_lkf" shortname="sigma_mag" handler="SetRmag"/>
<dl_setting var="bafl_Q_att" min="0.0" step="0.000001" max="0.1" module="ahrs/ahrs_float_lkf" shortname="Q att"/>
<dl_setting var="bafl_Q_gyro" min="0.0" step="0.000001" max="0.1" module="ahrs/ahrs_float_lkf" shortname="Q gyro"/>
</dl_settings>
</dl_settings>
sw/airborne/booz/booz2_ins.c
+2 -2
View File
@@ -32,7 +32,7 @@
#include "math/pprz_algebra_int.h"
#include "math/pprz_algebra_float.h"
#include "booz_ahrs.h"
#include "ahrs.h"
#ifdef USE_VFF
#include "ins/booz2_vf_float.h"
@@ -151,7 +151,7 @@ void booz_ins_propagate() {
struct Int32Vect3 accel_body;
INT32_RMAT_TRANSP_VMULT(accel_body, imu.body_to_imu_rmat, imu.accel);
struct Int32Vect3 accel_ltp;
INT32_RMAT_TRANSP_VMULT(accel_ltp, booz_ahrs.ltp_to_body_rmat, accel_body);
INT32_RMAT_TRANSP_VMULT(accel_ltp, ahrs.ltp_to_body_rmat, accel_body);
float z_accel_float = ACCEL_FLOAT_OF_BFP(accel_ltp.z);
#ifdef USE_VFF
sw/airborne/booz/booz2_telemetry.h
+90 -90
View File
@@ -49,7 +49,7 @@
#include "imu.h"
#include "booz_gps.h"
#include "booz2_ins.h"
#include "booz_ahrs.h"
#include "ahrs.h"
#include "i2c_hw.h"
@@ -215,12 +215,12 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
#ifdef STABILISATION_ATTITUDE_TYPE_INT
#define PERIODIC_SEND_BOOZ2_STAB_ATTITUDE(_chan) { \
DOWNLINK_SEND_BOOZ2_STAB_ATTITUDE_INT(_chan, \
&booz_ahrs.body_rate.p, \
&booz_ahrs.body_rate.q, \
&booz_ahrs.body_rate.r, \
&booz_ahrs.ltp_to_body_euler.phi, \
&booz_ahrs.ltp_to_body_euler.theta, \
&booz_ahrs.ltp_to_body_euler.psi, \
&ahrs.body_rate.p, \
&ahrs.body_rate.q, \
&ahrs.body_rate.r, \
&ahrs.ltp_to_body_euler.phi, \
&ahrs.ltp_to_body_euler.theta, \
&ahrs.ltp_to_body_euler.psi, \
&booz_stab_att_sp_euler.phi, \
&booz_stab_att_sp_euler.theta, \
&booz_stab_att_sp_euler.psi, \
@@ -259,12 +259,12 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
#ifdef STABILISATION_ATTITUDE_TYPE_FLOAT
#define PERIODIC_SEND_BOOZ2_STAB_ATTITUDE(_chan) { \
DOWNLINK_SEND_BOOZ2_STAB_ATTITUDE_FLOAT(_chan, \
&booz_ahrs_float.body_rate.p, \
&booz_ahrs_float.body_rate.q, \
&booz_ahrs_float.body_rate.r, \
&booz_ahrs_float.ltp_to_body_euler.phi, \
&booz_ahrs_float.ltp_to_body_euler.theta, \
&booz_ahrs_float.ltp_to_body_euler.psi, \
&ahrs_float.body_rate.p, \
&ahrs_float.body_rate.q, \
&ahrs_float.body_rate.r, \
&ahrs_float.ltp_to_body_euler.phi, \
&ahrs_float.ltp_to_body_euler.theta, \
&ahrs_float.ltp_to_body_euler.psi, \
&booz_stab_att_ref_euler.phi, \
&booz_stab_att_ref_euler.theta, \
&booz_stab_att_ref_euler.psi, \
@@ -301,17 +301,17 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
#endif /* STABILISATION_ATTITUDE_TYPE_FLOAT */
#include "ahrs/booz_ahrs_aligner.h"
#include "ahrs/ahrs_aligner.h"
#define PERIODIC_SEND_BOOZ2_FILTER_ALIGNER(_chan) { \
DOWNLINK_SEND_BOOZ2_FILTER_ALIGNER(_chan, \
&booz_ahrs_aligner.lp_gyro.p, \
&booz_ahrs_aligner.lp_gyro.q, \
&booz_ahrs_aligner.lp_gyro.r, \
&ahrs_aligner.lp_gyro.p, \
&ahrs_aligner.lp_gyro.q, \
&ahrs_aligner.lp_gyro.r, \
&imu.gyro.p, \
&imu.gyro.q, \
&imu.gyro.r, \
&booz_ahrs_aligner.noise, \
&booz_ahrs_aligner.low_noise_cnt); \
&ahrs_aligner.noise, \
&ahrs_aligner.low_noise_cnt); \
}
@@ -325,34 +325,34 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
#ifdef USE_AHRS_CMPL
#include "ahrs/booz_ahrs_cmpl_euler.h"
#include "ahrs/ahrs_cmpl_euler.h"
#define PERIODIC_SEND_BOOZ2_FILTER(_chan) { \
DOWNLINK_SEND_BOOZ2_FILTER(_chan, \
&booz_ahrs.ltp_to_imu_euler.phi, \
&booz_ahrs.ltp_to_imu_euler.theta, \
&booz_ahrs.ltp_to_imu_euler.psi, \
&booz2_face_measure.phi, \
&booz2_face_measure.theta, \
&booz2_face_measure.psi, \
&booz2_face_corrected.phi, \
&booz2_face_corrected.theta, \
&booz2_face_corrected.psi, \
&booz2_face_residual.phi, \
&booz2_face_residual.theta, \
&booz2_face_residual.psi, \
&booz2_face_gyro_bias.p, \
&booz2_face_gyro_bias.q, \
&booz2_face_gyro_bias.r); \
&ahrs.ltp_to_imu_euler.phi, \
&ahrs.ltp_to_imu_euler.theta, \
&ahrs.ltp_to_imu_euler.psi, \
&face_measure.phi, \
&face_measure.theta, \
&face_measure.psi, \
&face_corrected.phi, \
&face_corrected.theta, \
&face_corrected.psi, \
&face_residual.phi, \
&face_residual.theta, \
&face_residual.psi, \
&face_gyro_bias.p, \
&face_gyro_bias.q, \
&face_gyro_bias.r); \
}
#else
#define PERIODIC_SEND_BOOZ2_FILTER(_chan) {}
#endif
#ifdef USE_AHRS_LKF
#include "booz_ahrs.h"
#include "ahrs/booz_ahrs_float_lkf.h"
#define PERIODIC_SEND_BOOZ_AHRS_LKF(_chan) { \
DOWNLINK_SEND_BOOZ_AHRS_LKF(&bafl_eulers.phi, \
#include "ahrs.h"
#include "ahrs/ahrs_float_lkf.h"
#define PERIODIC_SEND_AHRS_LKF(_chan) { \
DOWNLINK_SEND_AHRS_LKF(&bafl_eulers.phi, \
_chan, \
&bafl_eulers.theta, \
&bafl_eulers.psi, \
@@ -370,8 +370,8 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
&bafl_mag.y, \
&bafl_mag.z); \
}
#define PERIODIC_SEND_BOOZ_AHRS_LKF_DEBUG(_chan) { \
DOWNLINK_SEND_BOOZ_AHRS_LKF_DEBUG(_chan, \
#define PERIODIC_SEND_AHRS_LKF_DEBUG(_chan) { \
DOWNLINK_SEND_AHRS_LKF_DEBUG(_chan, \
&bafl_X[0], \
&bafl_X[1], \
&bafl_X[2], \
@@ -388,8 +388,8 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
&bafl_P[4][4], \
&bafl_P[5][5]); \
}
#define PERIODIC_SEND_BOOZ_AHRS_LKF_ACC_DBG(_chan) { \
DOWNLINK_SEND_BOOZ_AHRS_LKF_ACC_DBG(_chan, \
#define PERIODIC_SEND_AHRS_LKF_ACC_DBG(_chan) { \
DOWNLINK_SEND_AHRS_LKF_ACC_DBG(_chan, \
&bafl_q_a_err.qi, \
&bafl_q_a_err.qx, \
&bafl_q_a_err.qy, \
@@ -398,8 +398,8 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
&bafl_b_a_err.q, \
&bafl_b_a_err.r); \
}
#define PERIODIC_SEND_BOOZ_AHRS_LKF_MAG_DBG(_chan) { \
DOWNLINK_SEND_BOOZ_AHRS_LKF_MAG_DBG(_chan, \
#define PERIODIC_SEND_AHRS_LKF_MAG_DBG(_chan) { \
DOWNLINK_SEND_AHRS_LKF_MAG_DBG(_chan, \
&bafl_q_m_err.qi, \
&bafl_q_m_err.qx, \
&bafl_q_m_err.qy, \
@@ -409,55 +409,55 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
&bafl_b_m_err.r); \
}
#else
#define PERIODIC_SEND_BOOZ_AHRS_LKF(_chan) {}
#define PERIODIC_SEND_BOOZ_AHRS_LKF_DEBUG(_chan) {}
#define PERIODIC_SEND_BOOZ_AHRS_LKF_MAG_DBG(_chan) {}
#define PERIODIC_SEND_BOOZ_AHRS_LKF_ACC_DBG(_chan) {}
#define PERIODIC_SEND_AHRS_LKF(_chan) {}
#define PERIODIC_SEND_AHRS_LKF_DEBUG(_chan) {}
#define PERIODIC_SEND_AHRS_LKF_MAG_DBG(_chan) {}
#define PERIODIC_SEND_AHRS_LKF_ACC_DBG(_chan) {}
#endif
#define PERIODIC_SEND_BOOZ2_AHRS_QUAT(_chan) { \
DOWNLINK_SEND_BOOZ2_AHRS_QUAT(_chan, \
&booz_ahrs.ltp_to_imu_quat.qi, \
&booz_ahrs.ltp_to_imu_quat.qx, \
&booz_ahrs.ltp_to_imu_quat.qy, \
&booz_ahrs.ltp_to_imu_quat.qz, \
&booz_ahrs.ltp_to_body_quat.qi, \
&booz_ahrs.ltp_to_body_quat.qx, \
&booz_ahrs.ltp_to_body_quat.qy, \
&booz_ahrs.ltp_to_body_quat.qz); \
&ahrs.ltp_to_imu_quat.qi, \
&ahrs.ltp_to_imu_quat.qx, \
&ahrs.ltp_to_imu_quat.qy, \
&ahrs.ltp_to_imu_quat.qz, \
&ahrs.ltp_to_body_quat.qi, \
&ahrs.ltp_to_body_quat.qx, \
&ahrs.ltp_to_body_quat.qy, \
&ahrs.ltp_to_body_quat.qz); \
}
#define PERIODIC_SEND_BOOZ2_AHRS_EULER(_chan) { \
DOWNLINK_SEND_BOOZ2_AHRS_EULER(_chan, \
&booz_ahrs.ltp_to_imu_euler.phi, \
&booz_ahrs.ltp_to_imu_euler.theta, \
&booz_ahrs.ltp_to_imu_euler.psi, \
&booz_ahrs.ltp_to_body_euler.phi, \
&booz_ahrs.ltp_to_body_euler.theta, \
&booz_ahrs.ltp_to_body_euler.psi); \
&ahrs.ltp_to_imu_euler.phi, \
&ahrs.ltp_to_imu_euler.theta, \
&ahrs.ltp_to_imu_euler.psi, \
&ahrs.ltp_to_body_euler.phi, \
&ahrs.ltp_to_body_euler.theta, \
&ahrs.ltp_to_body_euler.psi); \
}
#define PERIODIC_SEND_BOOZ2_AHRS_RMAT(_chan) { \
DOWNLINK_SEND_BOOZ2_AHRS_RMAT(_chan, \
&booz_ahrs.ltp_to_imu_rmat.m[0], \
&booz_ahrs.ltp_to_imu_rmat.m[1], \
&booz_ahrs.ltp_to_imu_rmat.m[2], \
&booz_ahrs.ltp_to_imu_rmat.m[3], \
&booz_ahrs.ltp_to_imu_rmat.m[4], \
&booz_ahrs.ltp_to_imu_rmat.m[5], \
&booz_ahrs.ltp_to_imu_rmat.m[6], \
&booz_ahrs.ltp_to_imu_rmat.m[7], \
&booz_ahrs.ltp_to_imu_rmat.m[8], \
&booz_ahrs.ltp_to_body_rmat.m[0], \
&booz_ahrs.ltp_to_body_rmat.m[1], \
&booz_ahrs.ltp_to_body_rmat.m[2], \
&booz_ahrs.ltp_to_body_rmat.m[3], \
&booz_ahrs.ltp_to_body_rmat.m[4], \
&booz_ahrs.ltp_to_body_rmat.m[5], \
&booz_ahrs.ltp_to_body_rmat.m[6], \
&booz_ahrs.ltp_to_body_rmat.m[7], \
&booz_ahrs.ltp_to_body_rmat.m[8]); \
&ahrs.ltp_to_imu_rmat.m[0], \
&ahrs.ltp_to_imu_rmat.m[1], \
&ahrs.ltp_to_imu_rmat.m[2], \
&ahrs.ltp_to_imu_rmat.m[3], \
&ahrs.ltp_to_imu_rmat.m[4], \
&ahrs.ltp_to_imu_rmat.m[5], \
&ahrs.ltp_to_imu_rmat.m[6], \
&ahrs.ltp_to_imu_rmat.m[7], \
&ahrs.ltp_to_imu_rmat.m[8], \
&ahrs.ltp_to_body_rmat.m[0], \
&ahrs.ltp_to_body_rmat.m[1], \
&ahrs.ltp_to_body_rmat.m[2], \
&ahrs.ltp_to_body_rmat.m[3], \
&ahrs.ltp_to_body_rmat.m[4], \
&ahrs.ltp_to_body_rmat.m[5], \
&ahrs.ltp_to_body_rmat.m[6], \
&ahrs.ltp_to_body_rmat.m[7], \
&ahrs.ltp_to_body_rmat.m[8]); \
}
@@ -670,9 +670,9 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
&booz_ins_enu_speed.x, \
&booz_ins_enu_speed.y, \
&booz_ins_enu_speed.z, \
&booz_ahrs.ltp_to_body_euler.phi, \
&booz_ahrs.ltp_to_body_euler.theta, \
&booz_ahrs.ltp_to_body_euler.psi, \
&ahrs.ltp_to_body_euler.phi, \
&ahrs.ltp_to_body_euler.theta, \
&ahrs.ltp_to_body_euler.psi, \
&booz2_guidance_h_pos_sp.y, \
&booz2_guidance_h_pos_sp.x, \
&carrot_up, \
@@ -752,12 +752,12 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
&booz_stabilization_cmd[COMMAND_PITCH], \
&booz_stabilization_cmd[COMMAND_YAW], \
&booz_stabilization_cmd[COMMAND_THRUST], \
&booz_ahrs.ltp_to_imu_euler.phi, \
&booz_ahrs.ltp_to_imu_euler.theta, \
&booz_ahrs.ltp_to_imu_euler.psi, \
&booz_ahrs.ltp_to_body_euler.phi, \
&booz_ahrs.ltp_to_body_euler.theta, \
&booz_ahrs.ltp_to_body_euler.psi \
&ahrs.ltp_to_imu_euler.phi, \
&ahrs.ltp_to_imu_euler.theta, \
&ahrs.ltp_to_imu_euler.psi, \
&ahrs.ltp_to_body_euler.phi, \
&ahrs.ltp_to_body_euler.theta, \
&ahrs.ltp_to_body_euler.psi \
); \
}
sw/airborne/booz/booz_fms.c
+3 -3
View File
@@ -25,7 +25,7 @@
#include "imu.h"
#include "booz_gps.h"
#include "booz_ahrs.h"
#include "ahrs.h"
#include "airframe.h"
@@ -81,7 +81,7 @@ void booz_fms_update_info(void) {
//booz_fms_info.gps.num_sv = booz_gps_state.num_sv;
//booz_fms_info.gps.fix = booz_gps_state.fix;
// PPRZ_INT16_OF_INT32_EULER(booz_fms_info.ahrs.euler, booz_ahrs_state.euler)
// PPRZ_INT16_OF_INT32_RATE (booz_fms_info.ahrs.rate, booz_ahrs_state.rate)
// PPRZ_INT16_OF_INT32_EULER(booz_fms_info.ahrs.euler, ahrs_state.euler)
// PPRZ_INT16_OF_INT32_RATE (booz_fms_info.ahrs.rate, ahrs_state.rate)
}
sw/airborne/booz/guidance/booz2_guidance_h.c
+5 -5
View File
@@ -25,7 +25,7 @@
//#define B2_GUIDANCE_H_USE_REF
#include "booz2_guidance_h.h"
#include "booz_ahrs.h"
#include "ahrs.h"
#include "booz_stabilization.h"
#include "booz_fms.h"
#include "booz2_ins.h"
@@ -251,8 +251,8 @@ static inline void booz2_guidance_h_hover_run(void) {
/* Rotate to body frame */
int32_t s_psi, c_psi;
PPRZ_ITRIG_SIN(s_psi, booz_ahrs.ltp_to_body_euler.psi);
PPRZ_ITRIG_COS(c_psi, booz_ahrs.ltp_to_body_euler.psi);
PPRZ_ITRIG_SIN(s_psi, ahrs.ltp_to_body_euler.psi);
PPRZ_ITRIG_COS(c_psi, ahrs.ltp_to_body_euler.psi);
// INT32_TRIG_FRAC - 2: 100mm erreur, gain 100 -> 10000 command | 2 degres = 36000, so multiply by 4
@@ -348,8 +348,8 @@ static inline void booz2_guidance_h_nav_run(bool_t in_flight) {
/* Rotate to body frame */
int32_t s_psi, c_psi;
PPRZ_ITRIG_SIN(s_psi, booz_ahrs.ltp_to_body_euler.psi);
PPRZ_ITRIG_COS(c_psi, booz_ahrs.ltp_to_body_euler.psi);
PPRZ_ITRIG_SIN(s_psi, ahrs.ltp_to_body_euler.psi);
PPRZ_ITRIG_COS(c_psi, ahrs.ltp_to_body_euler.psi);
// Restore angle ref resolution after rotation
booz2_guidance_h_command_body.phi =
sw/airborne/booz/guidance/booz2_guidance_v.c
+3 -3
View File
@@ -28,7 +28,7 @@
#include "booz_radio_control.h"
#include "booz_stabilization.h"
#include "booz_ahrs.h"
#include "ahrs.h"
#include "booz_fms.h"
#include "booz2_navigation.h"
@@ -264,8 +264,8 @@ static inline void run_hover_loop(bool_t in_flight) {
#else
booz2_guidance_v_ff_cmd = g_m_zdd / inv_m;
int32_t cphi,ctheta,cphitheta;
PPRZ_ITRIG_COS(cphi, booz_ahrs.ltp_to_body_euler.phi);
PPRZ_ITRIG_COS(ctheta, booz_ahrs.ltp_to_body_euler.theta);
PPRZ_ITRIG_COS(cphi, ahrs.ltp_to_body_euler.phi);
PPRZ_ITRIG_COS(ctheta, ahrs.ltp_to_body_euler.theta);
cphitheta = (cphi * ctheta) >> INT32_TRIG_FRAC;
if (cphitheta < BOOZ2_MAX_BANK_COEF) cphitheta = BOOZ2_MAX_BANK_COEF;
booz2_guidance_v_ff_cmd = (booz2_guidance_v_ff_cmd << INT32_TRIG_FRAC) / cphitheta;
sw/airborne/booz/ins/booz2_hf_float-old.c
+4 -4
View File
@@ -25,7 +25,7 @@
#include "booz2_ins.h"
#include "imu.h"
#include "booz_ahrs.h"
#include "ahrs.h"
#include "math/pprz_algebra_int.h"
@@ -56,8 +56,8 @@ void b2ins_propagate(void) {
/* unbias accelerometers */
VECT3_DIFF(accel_imu, imu.accel, scaled_biases);
/* convert to LTP */
// BOOZ_IQUAT_VDIV(b2ins_accel_ltp, booz_ahrs.ltp_to_imu_quat, accel_imu);
INT32_RMAT_TRANSP_VMULT(b2ins_accel_ltp, booz_ahrs.ltp_to_imu_rmat, accel_imu);
// BOOZ_IQUAT_VDIV(b2ins_accel_ltp, ahrs.ltp_to_imu_quat, accel_imu);
INT32_RMAT_TRANSP_VMULT(b2ins_accel_ltp, ahrs.ltp_to_imu_rmat, accel_imu);
/* correct for gravity */
b2ins_accel_ltp.z += ACCEL_BFP_OF_REAL(9.81);
/* propagate position */
@@ -111,7 +111,7 @@ void b2ins_update_gps(void) {
VECT2_SDIV(speed_residual3, speed_residual, (1<<9));
speed_residual3.z = 0;
struct Int32Vect3 bias_cor_s;
INT32_RMAT_VMULT( bias_cor_s, booz_ahrs.ltp_to_imu_rmat, speed_residual3);
INT32_RMAT_VMULT( bias_cor_s, ahrs.ltp_to_imu_rmat, speed_residual3);
VECT3_ADD(b2ins_accel_bias, bias_cor_s);
#endif /* UPDATE_FROM_SPEED */
sw/airborne/booz/ins/booz2_hf_float.c
+2 -2
View File
@@ -25,7 +25,7 @@
#include "booz2_hf_float.h"
#include "booz2_ins.h"
#include "imu.h"
#include "booz_ahrs.h"
#include "ahrs.h"
#include "booz_gps.h"
#include <stdlib.h>
@@ -440,7 +440,7 @@ void b2_hff_propagate(void) {
/* compute float ltp mean acceleration */
b2_hff_compute_accel_body_mean(HFF_PRESCALER);
struct Int32Vect3 mean_accel_ltp;
INT32_RMAT_TRANSP_VMULT(mean_accel_ltp, booz_ahrs.ltp_to_body_rmat, acc_body_mean);
INT32_RMAT_TRANSP_VMULT(mean_accel_ltp, ahrs.ltp_to_body_rmat, acc_body_mean);
b2_hff_xdd_meas = ACCEL_FLOAT_OF_BFP(mean_accel_ltp.x);
b2_hff_ydd_meas = ACCEL_FLOAT_OF_BFP(mean_accel_ltp.y);
#ifdef GPS_LAG
sw/airborne/booz/stabilization/booz_stabilization_attitude_euler_float.c
+3 -3
View File
@@ -24,7 +24,7 @@
#include "booz_stabilization.h"
#include "math/pprz_algebra_float.h"
#include "booz_ahrs.h"
#include "ahrs.h"
#include "booz_radio_control.h"
#include "airframe.h"
@@ -98,7 +98,7 @@ void booz_stabilization_attitude_run(bool_t in_flight) {
/* Compute feedback */
/* attitude error */
struct FloatEulers att_float;
EULERS_FLOAT_OF_BFP(att_float, booz_ahrs.ltp_to_body_euler);
EULERS_FLOAT_OF_BFP(att_float, ahrs.ltp_to_body_euler);
struct FloatEulers att_err;
EULERS_DIFF(att_err, att_float, booz_stab_att_ref_euler);
FLOAT_ANGLE_NORMALIZE(att_err.psi);
@@ -114,7 +114,7 @@ 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);
RATES_FLOAT_OF_BFP(rate_float, ahrs.body_rate);
struct FloatRates rate_err;
RATES_DIFF(rate_err, rate_float, booz_stab_att_ref_rate);
sw/airborne/booz/stabilization/booz_stabilization_attitude_euler_int.c
+3 -3
View File
@@ -22,7 +22,7 @@
*/
#include "booz_stabilization.h"
#include "booz_ahrs.h"
#include "ahrs.h"
#include "booz_radio_control.h"
@@ -108,7 +108,7 @@ void booz_stabilization_attitude_run(bool_t in_flight) {
OFFSET_AND_ROUND(booz_stab_att_ref_euler.theta, (REF_ANGLE_FRAC - INT32_ANGLE_FRAC)),
OFFSET_AND_ROUND(booz_stab_att_ref_euler.psi, (REF_ANGLE_FRAC - INT32_ANGLE_FRAC)) };
struct Int32Eulers att_err;
EULERS_DIFF(att_err, booz_ahrs.ltp_to_body_euler, att_ref_scaled);
EULERS_DIFF(att_err, ahrs.ltp_to_body_euler, att_ref_scaled);
INT32_ANGLE_NORMALIZE(att_err.psi);
if (in_flight) {
@@ -126,7 +126,7 @@ void booz_stabilization_attitude_run(bool_t in_flight) {
OFFSET_AND_ROUND(booz_stab_att_ref_rate.q, (REF_RATE_FRAC - INT32_RATE_FRAC)),
OFFSET_AND_ROUND(booz_stab_att_ref_rate.r, (REF_RATE_FRAC - INT32_RATE_FRAC)) };
struct Int32Rates rate_err;
RATES_DIFF(rate_err, booz_ahrs.body_rate, rate_ref_scaled);
RATES_DIFF(rate_err, ahrs.body_rate, rate_ref_scaled);
/* PID */
booz_stabilization_att_fb_cmd[COMMAND_ROLL] =
sw/airborne/booz/stabilization/booz_stabilization_attitude_quat_float.c
+4 -4
View File
@@ -30,7 +30,7 @@
#include <stdio.h>
#include "math/pprz_algebra_float.h"
#include "math/pprz_algebra_int.h"
#include "booz_ahrs.h"
#include "ahrs.h"
#include "airframe.h"
struct FloatAttitudeGains booz_stabilization_gains[BOOZ_STABILIZATION_ATTITUDE_GAIN_NB];
@@ -183,13 +183,13 @@ void booz_stabilization_attitude_run(bool_t enable_integrator) {
/* attitude error */
struct FloatQuat att_err;
FLOAT_QUAT_INV_COMP(att_err, booz_ahrs_float.ltp_to_body_quat, booz_stab_att_ref_quat);
FLOAT_QUAT_INV_COMP(att_err, ahrs_float.ltp_to_body_quat, booz_stab_att_ref_quat);
/* wrap it in the shortest direction */
FLOAT_QUAT_WRAP_SHORTEST(att_err);
/* rate error */
struct FloatRates rate_err;
RATES_DIFF(rate_err, booz_ahrs_float.body_rate, booz_stab_att_ref_rate);
RATES_DIFF(rate_err, ahrs_float.body_rate, booz_stab_att_ref_rate);
/* integrated error */
if (enable_integrator) {
@@ -211,7 +211,7 @@ void booz_stabilization_attitude_run(bool_t enable_integrator) {
attitude_run_ff(booz_stabilization_att_ff_cmd, &booz_stabilization_gains[gain_idx], &booz_stab_att_ref_accel);
attitude_run_fb(booz_stabilization_att_fb_cmd, &booz_stabilization_gains[gain_idx], &att_err, &rate_err, &booz_ahrs_float.body_rate_d, &booz_stabilization_att_sum_err_quat);
attitude_run_fb(booz_stabilization_att_fb_cmd, &booz_stabilization_gains[gain_idx], &att_err, &rate_err, &ahrs_float.body_rate_d, &booz_stabilization_att_sum_err_quat);
for (int i = COMMAND_ROLL; i <= COMMAND_YAW_SURFACE; i++) {
booz_stabilization_cmd[i] = booz_stabilization_att_fb_cmd[i]+booz_stabilization_att_ff_cmd[i];
sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_float.h
+1 -1
View File
@@ -61,7 +61,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 = ANGLE_FLOAT_OF_BFP(ahrs.ltp_to_body_euler.psi); \
} \
}
sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_int.h
+2 -2
View File
@@ -75,7 +75,7 @@
} \
} \
else { /* if not flying, use current yaw as setpoint */ \
_sp.psi = (booz_ahrs.ltp_to_body_euler.psi << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC)); \
_sp.psi = (ahrs.ltp_to_body_euler.psi << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC)); \
} \
}
@@ -85,7 +85,7 @@
}
#define BOOZ_STABILIZATION_ATTITUDE_RESET_PSI_REF(_sp) { \
_sp.psi = booz_ahrs.ltp_to_body_euler.psi << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC); \
_sp.psi = ahrs.ltp_to_body_euler.psi << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC); \
booz_stab_att_ref_euler.psi = _sp.psi; \
booz_stab_att_ref_rate.r = 0; \
}
sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_quat_float.c
+2 -2
View File
@@ -28,7 +28,7 @@
#include "airframe.h"
#include "booz_stabilization.h"
#include "booz_ahrs.h"
#include "ahrs.h"
#include "booz_stabilization_attitude_ref_float.h"
#include "quat_setpoint.h"
@@ -56,7 +56,7 @@ static const float omega_r[] = BOOZ_STABILIZATION_ATTITUDE_REF_OMEGA_R;
static const float zeta_r[] = BOOZ_STABILIZATION_ATTITUDE_REF_ZETA_R;
static void reset_psi_ref_from_body(void) {
booz_stab_att_ref_euler.psi = booz_ahrs_float.ltp_to_body_euler.psi;
booz_stab_att_ref_euler.psi = ahrs_float.ltp_to_body_euler.psi;
booz_stab_att_ref_rate.r = 0;
booz_stab_att_ref_accel.r = 0;
}
sw/airborne/booz/stabilization/booz_stabilization_rate.c
+2 -2
View File
@@ -24,7 +24,7 @@
#include "booz_stabilization.h"
#include "booz_ahrs.h"
#include "ahrs.h"
#include "imu.h"
#include "booz_radio_control.h"
@@ -166,7 +166,7 @@ void booz_stabilization_rate_run(bool_t in_flight) {
OFFSET_AND_ROUND(booz_stabilization_rate_ref.q, (REF_FRAC - INT32_RATE_FRAC)),
OFFSET_AND_ROUND(booz_stabilization_rate_ref.r, (REF_FRAC - INT32_RATE_FRAC)) };
struct Int32Rates _error;
RATES_DIFF(_error, booz_ahrs.body_rate, _ref_scaled);
RATES_DIFF(_error, ahrs.body_rate, _ref_scaled);
if (in_flight) {
/* update integrator */
RATES_ADD(booz_stabilization_rate_sum_err, _error);
sw/airborne/booz/test/test_mlkf.c
+10 -10
View File
@@ -3,8 +3,8 @@
#include "math/pprz_algebra_double.h"
#include "imu.h"
#include "booz_ahrs.h"
#include "ahrs/booz_ahrs_mlkf.h"
#include "ahrs.h"
#include "ahrs/ahrs_mlkf.h"
static void read_data(const char* filename);
static void feed_imu(int i);
@@ -40,13 +40,13 @@ int main(int argc, char** argv) {
read_data(IN_FILE);
imu_init();
booz_ahrs_init();
ahrs_init();
for (int i=0; i<nb_samples; i++) {
feed_imu(i);
booz_ahrs_propagate();
booz_ahrs_update_accel();
booz_ahrs_update_mag();
ahrs_propagate();
ahrs_update_accel();
ahrs_update_mag();
store_filter_output(i);
}
dump_output(OUT_FILE);
@@ -92,10 +92,10 @@ static void feed_imu(int i) {
static void store_filter_output(int i) {
QUAT_COPY(output[i].quat_est, booz_ahrs_float.ltp_to_imu_quat);
RATES_COPY(output[i].bias_est, booz_ahrs_mlkf.gyro_bias);
RATES_COPY(output[i].rate_est, booz_ahrs_float.imu_rate);
memcpy(output[i].P, booz_ahrs_mlkf.P, sizeof(booz_ahrs_mlkf.P));
QUAT_COPY(output[i].quat_est, ahrs_float.ltp_to_imu_quat);
RATES_COPY(output[i].bias_est, ahrs_mlkf.gyro_bias);
RATES_COPY(output[i].rate_est, ahrs_float.imu_rate);
memcpy(output[i].P, ahrs_mlkf.P, sizeof(ahrs_mlkf.P));
}
sw/airborne/csc/mercury_main.c
+5 -5
View File
@@ -41,8 +41,8 @@
#include "csc_msg_def.h"
#include ACTUATORS
#include "imu.h"
#include "booz/ahrs/booz_ahrs_aligner.h"
#include "booz/booz_ahrs.h"
#include "booz/ahrs/ahrs_aligner.h"
#include "booz/ahrs.h"
#include "mercury_xsens.h"
#include "csc_telemetry.h"
#include "csc_adc.h"
@@ -114,8 +114,8 @@ static inline void csc_main_init( void ) {
imu_init();
booz_ahrs_aligner_init();
booz_ahrs_init();
ahrs_aligner_init();
ahrs_init();
xsens_init();
@@ -159,7 +159,7 @@ static inline void csc_main_periodic( void )
baro_scp_periodic();
csc_ap_periodic(pprz_mode == PPRZ_MODE_IN_FLIGHT,
!(pprz_mode > PPRZ_MODE_MOTORS_OFF && booz_ahrs_aligner.status == BOOZ_AHRS_ALIGNER_LOCKED));
!(pprz_mode > PPRZ_MODE_MOTORS_OFF && ahrs_aligner.status == AHRS_ALIGNER_LOCKED));
}
sw/airborne/csc/mercury_xsens.c
+7 -7
View File
@@ -28,8 +28,8 @@
#include "mercury_xsens.h"
#include "imu.h"
#include "booz/booz_ahrs.h"
#include "booz/ahrs/booz_ahrs_aligner.h"
#include "booz/ahrs.h"
#include "booz/ahrs/ahrs_aligner.h"
#include "imu.h"
#include "csc_booz2_ins.h"
@@ -316,14 +316,14 @@ void xsens_parse_msg( uint8_t xsens_id ) {
// Copied from booz2_main -- 5143134f060fcc57ce657e17d8b7fc2e72119fd7
// mmt 6/15/09
if (booz_ahrs.status == BOOZ_AHRS_UNINIT) {
if (ahrs.status == AHRS_UNINIT) {
// 150
booz_ahrs_aligner_run();
if (booz_ahrs_aligner.status == BOOZ_AHRS_ALIGNER_LOCKED)
booz_ahrs_align();
ahrs_aligner_run();
if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED)
ahrs_align();
}
else {
booz_ahrs_propagate();
ahrs_propagate();
booz_ins_propagate();
}
}
sw/airborne/csc/mercury_xsens.h
+7 -7
View File
@@ -62,7 +62,7 @@ extern uint16_t xsens_time_stamp[XSENS_COUNT];
extern int xsens_setzero;
#include "booz_ahrs.h"
#include "ahrs.h"
#define PERIODIC_SEND_BOOZ2_GYRO() { \
DOWNLINK_SEND_BOOZ2_GYRO(&imu.gyro.p, \
@@ -83,12 +83,12 @@ extern int xsens_setzero;
}
#define PERIODIC_SEND_BOOZ2_AHRS_EULER() { \
DOWNLINK_SEND_BOOZ2_AHRS_EULER(&booz_ahrs.ltp_to_imu_euler.phi, \
&booz_ahrs.ltp_to_imu_euler.theta, \
&booz_ahrs.ltp_to_imu_euler.psi, \
&booz_ahrs.ltp_to_body_euler.phi, \
&booz_ahrs.ltp_to_body_euler.theta, \
&booz_ahrs.ltp_to_body_euler.psi); \
DOWNLINK_SEND_BOOZ2_AHRS_EULER(&ahrs.ltp_to_imu_euler.phi, \
&ahrs.ltp_to_imu_euler.theta, \
&ahrs.ltp_to_imu_euler.psi, \
&ahrs.ltp_to_body_euler.phi, \
&ahrs.ltp_to_body_euler.theta, \
&ahrs.ltp_to_body_euler.psi); \
}
sw/airborne/firmwares/rotorcraft/ahrs.c
+4 -4
View File
@@ -21,11 +21,11 @@
* Boston, MA 02111-1307, USA.
*/
#include "booz_ahrs.h"
#include "ahrs.h"
#include "imu.h"
struct BoozAhrs booz_ahrs;
struct BoozAhrsFloat booz_ahrs_float;
struct Ahrs ahrs;
struct AhrsFloat ahrs_float;
float booz_ahrs_mag_offset;
float ahrs_mag_offset;
sw/airborne/firmwares/rotorcraft/ahrs.h
+28 -28
View File
@@ -21,18 +21,18 @@
* Boston, MA 02111-1307, USA.
*/
#ifndef BOOZ_AHRS_H
#define BOOZ_AHRS_H
#ifndef AHRS_H
#define AHRS_H
#include "std.h"
#include "math/pprz_algebra_int.h"
#include "math/pprz_algebra_float.h"
#include "ahrs/booz_ahrs_aligner.h"
#include "ahrs/ahrs_aligner.h"
#define BOOZ_AHRS_UNINIT 0
#define BOOZ_AHRS_RUNNING 1
#define AHRS_UNINIT 0
#define AHRS_RUNNING 1
struct BoozAhrs {
struct Ahrs {
struct Int32Quat ltp_to_imu_quat;
struct Int32Eulers ltp_to_imu_euler;
@@ -47,45 +47,45 @@ struct BoozAhrs {
uint8_t status;
};
struct BoozAhrsFloat {
struct AhrsFloat {
struct FloatQuat ltp_to_imu_quat;
struct FloatEulers ltp_to_imu_euler;
struct FloatRMat ltp_to_imu_rmat;
struct FloatRates imu_rate;
struct FloatRates imu_rate_previous;
struct FloatRates imu_rate_d;
struct FloatQuat ltp_to_body_quat;
struct FloatEulers ltp_to_body_euler;
struct FloatRMat ltp_to_body_rmat;
struct FloatRates body_rate;
struct FloatRates body_rate_d;
struct FloatRates body_rate;
struct FloatRates body_rate_d;
uint8_t status;
};
extern struct BoozAhrs booz_ahrs;
extern struct BoozAhrsFloat booz_ahrs_float;
extern struct Ahrs ahrs;
extern struct AhrsFloat ahrs_float;
extern float booz_ahrs_mag_offset;
extern float ahrs_mag_offset;
#define BOOZ_AHRS_FLOAT_OF_INT32() { \
QUAT_FLOAT_OF_BFP(booz_ahrs_float.ltp_to_body_quat, booz_ahrs.ltp_to_body_quat); \
EULERS_FLOAT_OF_BFP(booz_ahrs_float.ltp_to_body_euler, booz_ahrs.ltp_to_body_euler); \
RATES_FLOAT_OF_BFP(booz_ahrs_float.body_rate, booz_ahrs.body_rate); \
#define AHRS_FLOAT_OF_INT32() { \
QUAT_FLOAT_OF_BFP(ahrs_float.ltp_to_body_quat, ahrs.ltp_to_body_quat); \
EULERS_FLOAT_OF_BFP(ahrs_float.ltp_to_body_euler, ahrs.ltp_to_body_euler); \
RATES_FLOAT_OF_BFP(ahrs_float.body_rate, ahrs.body_rate); \
}
#define BOOZ_AHRS_INT_OF_FLOAT() { \
QUAT_BFP_OF_REAL(booz_ahrs.ltp_to_body_quat, booz_ahrs_float.ltp_to_body_quat); \
EULERS_BFP_OF_REAL(booz_ahrs.ltp_to_body_euler, booz_ahrs_float.ltp_to_body_euler); \
RMAT_BFP_OF_REAL(booz_ahrs.ltp_to_body_rmat, booz_ahrs_float.ltp_to_body_rmat); \
RATES_BFP_OF_REAL(booz_ahrs.body_rate, booz_ahrs_float.body_rate); \
#define AHRS_INT_OF_FLOAT() { \
QUAT_BFP_OF_REAL(ahrs.ltp_to_body_quat, ahrs_float.ltp_to_body_quat); \
EULERS_BFP_OF_REAL(ahrs.ltp_to_body_euler, ahrs_float.ltp_to_body_euler); \
RMAT_BFP_OF_REAL(ahrs.ltp_to_body_rmat, ahrs_float.ltp_to_body_rmat); \
RATES_BFP_OF_REAL(ahrs.body_rate, ahrs_float.body_rate); \
}
extern void booz_ahrs_init(void);
extern void booz_ahrs_align(void);
extern void booz_ahrs_propagate(void);
extern void booz_ahrs_update_accel(void);
extern void booz_ahrs_update_mag(void);
extern void ahrs_init(void);
extern void ahrs_align(void);
extern void ahrs_propagate(void);
extern void ahrs_update_accel(void);
extern void ahrs_update_mag(void);
#endif /* BOOZ_AHRS_H */
#endif /* AHRS_H */
sw/airborne/firmwares/rotorcraft/ahrs/ahrs_aligner.c
+18 -18
View File
@@ -21,13 +21,13 @@
* Boston, MA 02111-1307, USA.
*/
#include "booz_ahrs_aligner.h"
#include "ahrs_aligner.h"
#include <stdlib.h> /* for abs() */
#include "imu.h"
#include "led.h"
struct BoozAhrsAligner booz_ahrs_aligner;
struct AhrsAligner ahrs_aligner;
#define SAMPLES_NB 512
static struct Int32Rates gyro_sum;
@@ -36,21 +36,21 @@ static struct Int32Vect3 mag_sum;
static int32_t ref_sensor_samples[SAMPLES_NB];
static uint32_t samples_idx;
void booz_ahrs_aligner_init(void) {
void ahrs_aligner_init(void) {
booz_ahrs_aligner.status = BOOZ_AHRS_ALIGNER_RUNNING;
ahrs_aligner.status = AHRS_ALIGNER_RUNNING;
INT_RATES_ZERO(gyro_sum);
INT_VECT3_ZERO(accel_sum);
INT_VECT3_ZERO(mag_sum);
samples_idx = 0;
booz_ahrs_aligner.noise = 0;
booz_ahrs_aligner.low_noise_cnt = 0;
ahrs_aligner.noise = 0;
ahrs_aligner.low_noise_cnt = 0;
}
#define LOW_NOISE_THRESHOLD 90000
#define LOW_NOISE_TIME 5
void booz_ahrs_aligner_run(void) {
void ahrs_aligner_run(void) {
RATES_ADD(gyro_sum, imu.gyro);
VECT3_ADD(accel_sum, imu.accel);
@@ -71,30 +71,30 @@ void booz_ahrs_aligner_run(void) {
avg_ref_sensor -= SAMPLES_NB / 2;
avg_ref_sensor /= SAMPLES_NB;
booz_ahrs_aligner.noise = 0;
ahrs_aligner.noise = 0;
int i;
for (i=0; i<SAMPLES_NB; i++) {
int32_t diff = ref_sensor_samples[i] - avg_ref_sensor;
booz_ahrs_aligner.noise += abs(diff);
ahrs_aligner.noise += abs(diff);
}
RATES_SDIV(booz_ahrs_aligner.lp_gyro, gyro_sum, SAMPLES_NB);
VECT3_SDIV(booz_ahrs_aligner.lp_accel, accel_sum, SAMPLES_NB);
VECT3_SDIV(booz_ahrs_aligner.lp_mag, mag_sum, SAMPLES_NB);
RATES_SDIV(ahrs_aligner.lp_gyro, gyro_sum, SAMPLES_NB);
VECT3_SDIV(ahrs_aligner.lp_accel, accel_sum, SAMPLES_NB);
VECT3_SDIV(ahrs_aligner.lp_mag, mag_sum, SAMPLES_NB);
INT_RATES_ZERO(gyro_sum);
INT_VECT3_ZERO(accel_sum);
INT_VECT3_ZERO(mag_sum);
samples_idx = 0;
if (booz_ahrs_aligner.noise < LOW_NOISE_THRESHOLD)
booz_ahrs_aligner.low_noise_cnt++;
if (ahrs_aligner.noise < LOW_NOISE_THRESHOLD)
ahrs_aligner.low_noise_cnt++;
else
if ( booz_ahrs_aligner.low_noise_cnt > 0)
booz_ahrs_aligner.low_noise_cnt--;
if ( ahrs_aligner.low_noise_cnt > 0)
ahrs_aligner.low_noise_cnt--;
if (booz_ahrs_aligner.low_noise_cnt > LOW_NOISE_TIME) {
booz_ahrs_aligner.status = BOOZ_AHRS_ALIGNER_LOCKED;
if (ahrs_aligner.low_noise_cnt > LOW_NOISE_TIME) {
ahrs_aligner.status = AHRS_ALIGNER_LOCKED;
#ifdef AHRS_ALIGNER_LED
LED_ON(AHRS_ALIGNER_LED);
#endif
sw/airborne/firmwares/rotorcraft/ahrs/ahrs_aligner.h
+10 -10
View File
@@ -21,17 +21,17 @@
* Boston, MA 02111-1307, USA.
*/
#ifndef BOOZ_AHRS_ALIGNER_H
#define BOOZ_AHRS_ALIGNER_H
#ifndef AHRS_ALIGNER_H
#define AHRS_ALIGNER_H
#include "std.h"
#include "math/pprz_algebra_int.h"
#define BOOZ_AHRS_ALIGNER_UNINIT 0
#define BOOZ_AHRS_ALIGNER_RUNNING 1
#define BOOZ_AHRS_ALIGNER_LOCKED 2
#define AHRS_ALIGNER_UNINIT 0
#define AHRS_ALIGNER_RUNNING 1
#define AHRS_ALIGNER_LOCKED 2
struct BoozAhrsAligner {
struct AhrsAligner {
struct Int32Rates lp_gyro;
struct Int32Vect3 lp_accel;
struct Int32Vect3 lp_mag;
@@ -40,9 +40,9 @@ struct BoozAhrsAligner {
uint8_t status;
};
extern struct BoozAhrsAligner booz_ahrs_aligner;
extern struct AhrsAligner ahrs_aligner;
extern void booz_ahrs_aligner_init(void);
extern void booz_ahrs_aligner_run(void);
extern void ahrs_aligner_init(void);
extern void ahrs_aligner_run(void);
#endif /* BOOZ_AHRS_ALIGNER_H */
#endif /* AHRS_ALIGNER_H */
sw/airborne/firmwares/rotorcraft/ahrs/ahrs_cmpl_euler.c
+54 -55
View File
@@ -1,6 +1,6 @@
/*
* $Id: $
*
*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* This file is part of paparazzi.
@@ -18,53 +18,53 @@
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
* Boston, MA 02111-1307, USA.
*/
#include "booz_ahrs_cmpl_euler.h"
#include "ahrs_cmpl_euler.h"
#include "imu.h"
#include "booz_ahrs_aligner.h"
#include "ahrs_aligner.h"
#include "airframe.h"
#include "math/pprz_trig_int.h"
#include "math/pprz_algebra_int.h"
struct Int32Rates booz2_face_gyro_bias;
struct Int32Eulers booz2_face_measure;
struct Int32Eulers booz2_face_residual;
struct Int32Eulers booz2_face_uncorrected;
struct Int32Eulers booz2_face_corrected;
struct Int32Rates face_gyro_bias;
struct Int32Eulers face_measure;
struct Int32Eulers face_residual;
struct Int32Eulers face_uncorrected;
struct Int32Eulers face_corrected;
struct Int32Eulers measurement;
int32_t booz2_face_reinj_1;
int32_t face_reinj_1;
void booz_ahrs_init(void) {
booz_ahrs.status = BOOZ_AHRS_UNINIT;
INT_EULERS_ZERO(booz_ahrs.ltp_to_body_euler);
INT_EULERS_ZERO(booz_ahrs.ltp_to_imu_euler);
INT32_QUAT_ZERO(booz_ahrs.ltp_to_body_quat);
INT32_QUAT_ZERO(booz_ahrs.ltp_to_imu_quat);
INT_RATES_ZERO(booz_ahrs.body_rate);
INT_RATES_ZERO(booz_ahrs.imu_rate);
INT_RATES_ZERO(booz2_face_gyro_bias);
booz2_face_reinj_1 = BOOZ2_FACE_REINJ_1;
void ahrs_init(void) {
ahrs.status = AHRS_UNINIT;
INT_EULERS_ZERO(ahrs.ltp_to_body_euler);
INT_EULERS_ZERO(ahrs.ltp_to_imu_euler);
INT32_QUAT_ZERO(ahrs.ltp_to_body_quat);
INT32_QUAT_ZERO(ahrs.ltp_to_imu_quat);
INT_RATES_ZERO(ahrs.body_rate);
INT_RATES_ZERO(ahrs.imu_rate);
INT_RATES_ZERO(face_gyro_bias);
face_reinj_1 = BOOZ2_FACE_REINJ_1;
INT_EULERS_ZERO(booz2_face_uncorrected);
INT_EULERS_ZERO(face_uncorrected);
#ifdef IMU_MAG_OFFSET
booz_ahrs_mag_offset = IMU_MAG_OFFSET;
ahrs_mag_offset = IMU_MAG_OFFSET;
#else
booz_ahrs_mag_offset = 0.;
ahrs_mag_offset = 0.;
#endif
}
void booz_ahrs_align(void) {
void ahrs_align(void) {
RATES_COPY( booz2_face_gyro_bias, booz_ahrs_aligner.lp_gyro);
booz_ahrs.status = BOOZ_AHRS_RUNNING;
RATES_COPY( face_gyro_bias, ahrs_aligner.lp_gyro);
ahrs.status = AHRS_RUNNING;
}
@@ -93,54 +93,54 @@ void booz_ahrs_align(void) {
*
*/
void booz_ahrs_propagate(void) {
void ahrs_propagate(void) {
/* unbias gyro */
struct Int32Rates uf_rate;
RATES_DIFF(uf_rate, imu.gyro, booz2_face_gyro_bias);
/* low pass rate */
RATES_ADD(booz_ahrs.imu_rate, uf_rate);
RATES_SDIV(booz_ahrs.imu_rate, booz_ahrs.imu_rate, 2);
RATES_DIFF(uf_rate, imu.gyro, face_gyro_bias);
/* low pass rate */
RATES_ADD(ahrs.imu_rate, uf_rate);
RATES_SDIV(ahrs.imu_rate, ahrs.imu_rate, 2);
/* integrate eulers */
struct Int32Eulers euler_dot;
INT32_EULERS_DOT_OF_RATES(euler_dot, booz_ahrs.ltp_to_imu_euler, booz_ahrs.imu_rate);
EULERS_ADD(booz2_face_corrected, euler_dot);
INT32_EULERS_DOT_OF_RATES(euler_dot, ahrs.ltp_to_imu_euler, ahrs.imu_rate);
EULERS_ADD(face_corrected, euler_dot);
/* low pass measurement */
EULERS_ADD(booz2_face_measure, measurement);
EULERS_SDIV(booz2_face_measure, booz2_face_measure, 2);
EULERS_ADD(face_measure, measurement);
EULERS_SDIV(face_measure, face_measure, 2);
/* compute residual */
EULERS_DIFF(booz2_face_residual, booz2_face_measure, booz2_face_corrected);
INTEG_EULER_NORMALIZE(booz2_face_residual.psi);
EULERS_DIFF(face_residual, face_measure, face_corrected);
INTEG_EULER_NORMALIZE(face_residual.psi);
struct Int32Eulers correction;
/* compute a correction */
EULERS_SDIV(correction, booz2_face_residual, booz2_face_reinj_1);
EULERS_SDIV(correction, face_residual, face_reinj_1);
/* correct estimation */
EULERS_ADD(booz2_face_corrected, correction);
INTEG_EULER_NORMALIZE(booz2_face_corrected.psi);
EULERS_ADD(face_corrected, correction);
INTEG_EULER_NORMALIZE(face_corrected.psi);
/* Compute LTP to IMU eulers */
EULERS_SDIV(booz_ahrs.ltp_to_imu_euler, booz2_face_corrected, F_UPDATE);
EULERS_SDIV(ahrs.ltp_to_imu_euler, face_corrected, F_UPDATE);
/* Compute LTP to IMU quaternion */
INT32_QUAT_OF_EULERS(booz_ahrs.ltp_to_imu_quat, booz_ahrs.ltp_to_imu_euler);
INT32_QUAT_OF_EULERS(ahrs.ltp_to_imu_quat, ahrs.ltp_to_imu_euler);
/* Compute LTP to IMU rotation matrix */
INT32_RMAT_OF_EULERS(booz_ahrs.ltp_to_imu_rmat, booz_ahrs.ltp_to_imu_euler);
INT32_RMAT_OF_EULERS(ahrs.ltp_to_imu_rmat, ahrs.ltp_to_imu_euler);
/* Compute LTP to BODY quaternion */
INT32_QUAT_COMP_INV(booz_ahrs.ltp_to_body_quat, booz_ahrs.ltp_to_imu_quat, imu.body_to_imu_quat);
INT32_QUAT_COMP_INV(ahrs.ltp_to_body_quat, ahrs.ltp_to_imu_quat, imu.body_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, imu.body_to_imu_rmat);
INT32_RMAT_COMP_INV(ahrs.ltp_to_body_rmat, ahrs.ltp_to_imu_rmat, 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);
INT32_EULERS_OF_RMAT(ahrs.ltp_to_body_euler, ahrs.ltp_to_body_rmat);
/* compute body rates */
INT32_RMAT_TRANSP_RATEMULT(booz_ahrs.body_rate, imu.body_to_imu_rmat, booz_ahrs.imu_rate);
INT32_RMAT_TRANSP_RATEMULT(ahrs.body_rate, imu.body_to_imu_rmat, ahrs.imu_rate);
}
void booz_ahrs_update_accel(void) {
void ahrs_update_accel(void) {
/* build a measurement assuming constant acceleration ?!! */
INT32_ATAN2(measurement.phi, -imu.accel.y, -imu.accel.z);
@@ -154,16 +154,16 @@ void booz_ahrs_update_accel(void) {
}
/* measure psi assuming magnetic vector is in earth plan (md = 0) */
void booz_ahrs_update_mag(void) {
void ahrs_update_mag(void) {
int32_t sphi;
PPRZ_ITRIG_SIN(sphi, booz_ahrs.ltp_to_imu_euler.phi);
PPRZ_ITRIG_SIN(sphi, ahrs.ltp_to_imu_euler.phi);
int32_t cphi;
PPRZ_ITRIG_COS(cphi, booz_ahrs.ltp_to_imu_euler.phi);
PPRZ_ITRIG_COS(cphi, ahrs.ltp_to_imu_euler.phi);
int32_t stheta;
PPRZ_ITRIG_SIN(stheta, booz_ahrs.ltp_to_imu_euler.theta);
PPRZ_ITRIG_SIN(stheta, ahrs.ltp_to_imu_euler.theta);
int32_t ctheta;
PPRZ_ITRIG_COS(ctheta, booz_ahrs.ltp_to_imu_euler.theta);
PPRZ_ITRIG_COS(ctheta, ahrs.ltp_to_imu_euler.theta);
int32_t sphi_stheta = (sphi*stheta)>>INT32_TRIG_FRAC;
int32_t cphi_stheta = (cphi*stheta)>>INT32_TRIG_FRAC;
@@ -183,7 +183,6 @@ void booz_ahrs_update_mag(void) {
// sphi_ctheta * imu.mag.y +
// cphi_ctheta * imu.mag.z;
float m_psi = -atan2(me, mn);
measurement.psi = ((m_psi - RadOfDeg(booz_ahrs_mag_offset))*(float)(1<<(INT32_ANGLE_FRAC))*F_UPDATE);
measurement.psi = ((m_psi - RadOfDeg(ahrs_mag_offset))*(float)(1<<(INT32_ANGLE_FRAC))*F_UPDATE);
}
sw/airborne/firmwares/rotorcraft/ahrs/ahrs_cmpl_euler.h
+10 -10
View File
@@ -21,20 +21,20 @@
* Boston, MA 02111-1307, USA.
*/
#ifndef BOOZ_AHRS_CMPL_EULER_H
#define BOOZ_AHRS_CMPL_EULER_H
#ifndef AHRS_CMPL_EULER_H
#define AHRS_CMPL_EULER_H
#include "booz_ahrs.h"
#include "ahrs.h"
#include "std.h"
#include "math/pprz_algebra_int.h"
extern struct Int32Rates booz2_face_gyro_bias;
extern struct Int32Eulers booz2_face_measure;
extern struct Int32Eulers booz2_face_residual;
extern struct Int32Eulers booz2_face_uncorrected;
extern struct Int32Eulers booz2_face_corrected;
extern struct Int32Rates face_gyro_bias;
extern struct Int32Eulers face_measure;
extern struct Int32Eulers face_residual;
extern struct Int32Eulers face_uncorrected;
extern struct Int32Eulers face_corrected;
extern int32_t booz2_face_reinj_1;
extern int32_t face_reinj_1;
#endif /* BOOZ_AHRS_CMPL_EULER_H */
#endif /* AHRS_CMPL_EULER_H */
sw/airborne/firmwares/rotorcraft/ahrs/ahrs_float_ekf.c
+5 -5
View File
@@ -89,8 +89,8 @@ float bafe_FP[4][7];
#define BAFE_DT (1./512.)
#endif
extern void booz_ahrs_init(void);
extern void booz_ahrs_align(void);
extern void ahrs_init(void);
extern void ahrs_align(void);
/*
@@ -119,7 +119,7 @@ extern void booz_ahrs_align(void);
*
*/
void booz_ahrs_propagate(void) {
void ahrs_propagate(void) {
/* compute unbiased rates */
RATES_FLOAT_OF_BFP(bafe_rates, imu.gyro);
RATES_SUB(bafe_rates, bafe_bias);
@@ -163,10 +163,10 @@ void booz_ahrs_propagate(void) {
}
extern void booz_ahrs_update(void);
extern void ahrs_update(void);
#endif /* BOOZ_AHRS_FLOAT_EKF_H */
#endif /* AHRS_FLOAT_EKF_H */
sw/airborne/firmwares/rotorcraft/ahrs/ahrs_float_ekf.h
+7 -7
View File
@@ -21,18 +21,18 @@
* Boston, MA 02111-1307, USA.
*/
#ifndef BOOZ_AHRS_FLOAT_EKF_H
#define BOOZ_AHRS_FLOAT_EKF_H
#ifndef AHRS_FLOAT_EKF_H
#define AHRS_FLOAT_EKF_H
extern void booz_ahrs_init(void);
extern void booz_ahrs_align(void);
extern void booz_ahrs_propagate(void);
extern void booz_ahrs_update(void);
extern void ahrs_init(void);
extern void ahrs_align(void);
extern void ahrs_propagate(void);
extern void ahrs_update(void);
#endif /* BOOZ_AHRS_FLOAT_EKF_H */
#endif /* AHRS_FLOAT_EKF_H */
sw/airborne/firmwares/rotorcraft/ahrs/ahrs_float_lkf.c
+38 -38
View File
@@ -22,10 +22,10 @@
* Boston, MA 02111-1307, USA.
*/
#include "booz_ahrs_float_lkf.h"
#include "ahrs_float_lkf.h"
#include "imu.h"
#include "booz_ahrs_aligner.h"
#include "ahrs_aligner.h"
#include "airframe.h"
#include "math/pprz_algebra_float.h"
@@ -35,8 +35,8 @@
#define BAFL_DEBUG
static void booz_ahrs_do_update_accel(void);
static void booz_ahrs_do_update_mag(void);
static void ahrs_do_update_accel(void);
static void ahrs_do_update_mag(void);
/* our estimated attitude (ltp <-> imu) */
@@ -189,28 +189,28 @@ float bafl_R_mag;
#define AHRS_TO_BFP() { \
/* IMU rate */ \
RATES_BFP_OF_REAL(booz_ahrs.imu_rate, bafl_rates); \
RATES_BFP_OF_REAL(ahrs.imu_rate, bafl_rates); \
/* LTP to IMU eulers */ \
EULERS_BFP_OF_REAL(booz_ahrs.ltp_to_imu_euler, bafl_eulers); \
EULERS_BFP_OF_REAL(ahrs.ltp_to_imu_euler, bafl_eulers); \
/* LTP to IMU quaternion */ \
QUAT_BFP_OF_REAL(booz_ahrs.ltp_to_imu_quat, bafl_quat); \
QUAT_BFP_OF_REAL(ahrs.ltp_to_imu_quat, bafl_quat); \
/* LTP to IMU rotation matrix */ \
RMAT_BFP_OF_REAL(booz_ahrs.ltp_to_imu_rmat, bafl_dcm); \
RMAT_BFP_OF_REAL(ahrs.ltp_to_imu_rmat, bafl_dcm); \
}
#define AHRS_LTP_TO_BODY() { \
/* Compute LTP to BODY quaternion */ \
INT32_QUAT_COMP_INV(booz_ahrs.ltp_to_body_quat, booz_ahrs.ltp_to_imu_quat, imu.body_to_imu_quat); \
INT32_QUAT_COMP_INV(ahrs.ltp_to_body_quat, ahrs.ltp_to_imu_quat, imu.body_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, imu.body_to_imu_rmat); \
INT32_RMAT_COMP_INV(ahrs.ltp_to_body_rmat, ahrs.ltp_to_imu_rmat, 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); \
INT32_EULERS_OF_RMAT(ahrs.ltp_to_body_euler, ahrs.ltp_to_body_rmat); \
/* compute body rates */ \
INT32_RMAT_TRANSP_RATEMULT(booz_ahrs.body_rate, imu.body_to_imu_rmat, booz_ahrs.imu_rate); \
INT32_RMAT_TRANSP_RATEMULT(ahrs.body_rate, imu.body_to_imu_rmat, ahrs.imu_rate); \
}
void booz_ahrs_init(void) {
void ahrs_init(void) {
int i, j;
for (i = 0; i < BAFL_SSIZE; i++) {
@@ -230,16 +230,16 @@ void booz_ahrs_init(void) {
FLOAT_QUAT_ZERO(bafl_quat);
booz_ahrs.status = BOOZ_AHRS_UNINIT;
INT_EULERS_ZERO(booz_ahrs.ltp_to_body_euler);
INT_EULERS_ZERO(booz_ahrs.ltp_to_imu_euler);
INT32_QUAT_ZERO(booz_ahrs.ltp_to_body_quat);
INT32_QUAT_ZERO(booz_ahrs.ltp_to_imu_quat);
INT_RATES_ZERO(booz_ahrs.body_rate);
INT_RATES_ZERO(booz_ahrs.imu_rate);
ahrs.status = AHRS_UNINIT;
INT_EULERS_ZERO(ahrs.ltp_to_body_euler);
INT_EULERS_ZERO(ahrs.ltp_to_imu_euler);
INT32_QUAT_ZERO(ahrs.ltp_to_body_quat);
INT32_QUAT_ZERO(ahrs.ltp_to_imu_quat);
INT_RATES_ZERO(ahrs.body_rate);
INT_RATES_ZERO(ahrs.imu_rate);
booz_ahrs_float_lkf_SetRaccel(BAFL_SIGMA_ACCEL);
booz_ahrs_float_lkf_SetRmag(BAFL_SIGMA_MAG);
ahrs_float_lkf_SetRaccel(BAFL_SIGMA_ACCEL);
ahrs_float_lkf_SetRmag(BAFL_SIGMA_MAG);
bafl_Q_att = BAFL_Q_ATT;
bafl_Q_gyro = BAFL_Q_GYRO;
@@ -248,13 +248,13 @@ void booz_ahrs_init(void) {
}
void booz_ahrs_align(void) {
RATES_FLOAT_OF_BFP(bafl_bias, booz_ahrs_aligner.lp_gyro);
ACCELS_FLOAT_OF_BFP(bafl_accel_measure, booz_ahrs_aligner.lp_accel);
booz_ahrs.status = BOOZ_AHRS_RUNNING;
void ahrs_align(void) {
RATES_FLOAT_OF_BFP(bafl_bias, ahrs_aligner.lp_gyro);
ACCELS_FLOAT_OF_BFP(bafl_accel_measure, ahrs_aligner.lp_accel);
ahrs.status = AHRS_RUNNING;
}
static inline void booz_ahrs_lowpass_accel(void) {
static inline void ahrs_lowpass_accel(void) {
// get latest measurement
ACCELS_FLOAT_OF_BFP(bafl_accel_last, imu.accel);
// low pass it
@@ -284,10 +284,10 @@ static inline void booz_ahrs_lowpass_accel(void) {
* [ r, q, -p, 0 ]
*
*/
void booz_ahrs_propagate(void) {
void ahrs_propagate(void) {
int i, j, k;
booz_ahrs_lowpass_accel();
ahrs_lowpass_accel();
/* compute unbiased rates */
RATES_FLOAT_OF_BFP(bafl_rates, imu.gyro);
@@ -405,11 +405,11 @@ void booz_ahrs_propagate(void) {
#endif
}
void booz_ahrs_update_accel(void) {
RunOnceEvery(50, booz_ahrs_do_update_accel());
void ahrs_update_accel(void) {
RunOnceEvery(50, ahrs_do_update_accel());
}
static void booz_ahrs_do_update_accel(void) {
static void ahrs_do_update_accel(void) {
int i, j, k;
#ifdef BAFL_DEBUG2
@@ -636,11 +636,11 @@ static void booz_ahrs_do_update_accel(void) {
}
void booz_ahrs_update_mag(void) {
RunOnceEvery(10, booz_ahrs_do_update_mag());
void ahrs_update_mag(void) {
RunOnceEvery(10, ahrs_do_update_mag());
}
static void booz_ahrs_do_update_mag(void) {
static void ahrs_do_update_mag(void) {
int i, j, k;
#ifdef BAFL_DEBUG2
@@ -846,7 +846,7 @@ static void booz_ahrs_do_update_mag(void) {
AHRS_LTP_TO_BODY();
}
void booz_ahrs_update(void) {
booz_ahrs_update_accel();
booz_ahrs_update_mag();
void ahrs_update(void) {
ahrs_update_accel();
ahrs_update_mag();
}
sw/airborne/firmwares/rotorcraft/ahrs/ahrs_float_lkf.h
+6 -6
View File
@@ -22,10 +22,10 @@
* Boston, MA 02111-1307, USA.
*/
#ifndef BOOZ_AHRS_FLOAT_LKF_H
#define BOOZ_AHRS_FLOAT_LKF_H
#ifndef AHRS_FLOAT_LKF_H
#define AHRS_FLOAT_LKF_H
#include "booz_ahrs.h"
#include "ahrs.h"
#include "std.h"
#include "math/pprz_algebra_int.h"
@@ -58,14 +58,14 @@ extern float bafl_Q_att;
extern float bafl_Q_gyro;
#define booz_ahrs_float_lkf_SetRaccel(_v) { \
#define ahrs_float_lkf_SetRaccel(_v) { \
bafl_sigma_accel = _v; \
bafl_R_accel = _v * _v; \
}
#define booz_ahrs_float_lkf_SetRmag(_v) { \
#define ahrs_float_lkf_SetRmag(_v) { \
bafl_sigma_mag = _v; \
bafl_R_mag = _v * _v; \
}
#endif /* BOOZ_AHRS_FLOAT_LKF_H */
#endif /* AHRS_FLOAT_LKF_H */
sw/airborne/firmwares/rotorcraft/main.c
+11 -11
View File
@@ -52,7 +52,7 @@
#include "booz_stabilization.h"
#include "booz_guidance.h"
#include "booz_ahrs.h"
#include "ahrs.h"
#include "booz2_ins.h"
#if defined USE_CAM || USE_DROP
@@ -119,8 +119,8 @@ STATIC_INLINE void booz2_main_init( void ) {
booz2_guidance_v_init();
booz_stabilization_init();
booz_ahrs_aligner_init();
booz_ahrs_init();
ahrs_aligner_init();
ahrs_init();
booz_ins_init();
@@ -220,14 +220,14 @@ static inline void on_gyro_accel_event( void ) {
ImuScaleGyro(imu);
ImuScaleAccel(imu);
if (booz_ahrs.status == BOOZ_AHRS_UNINIT) {
booz_ahrs_aligner_run();
if (booz_ahrs_aligner.status == BOOZ_AHRS_ALIGNER_LOCKED)
booz_ahrs_align();
if (ahrs.status == AHRS_UNINIT) {
ahrs_aligner_run();
if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED)
ahrs_align();
}
else {
booz_ahrs_propagate();
booz_ahrs_update_accel();
ahrs_propagate();
ahrs_update_accel();
#ifdef SITL
if (nps_bypass_ahrs) sim_overwrite_ahrs();
#endif
@@ -249,6 +249,6 @@ static inline void on_gps_event(void) {
static inline void on_mag_event(void) {
ImuScaleMag(imu);
if (booz_ahrs.status == BOOZ_AHRS_RUNNING)
booz_ahrs_update_mag();
if (ahrs.status == AHRS_RUNNING)
ahrs_update_mag();
}
sw/airborne/modules/cam_control/booz_cam.c
+2 -2
View File
@@ -24,7 +24,7 @@
#include "cam_control/booz_cam.h"
#include "booz2_pwm_hw.h"
#include "booz_ahrs.h"
#include "ahrs.h"
#include "booz2_navigation.h"
#include "booz2_ins.h"
#include "flight_plan.h"
@@ -93,7 +93,7 @@ void booz_cam_periodic(void) {
booz_cam_tilt_pwm = BOOZ_CAM_TILT_NEUTRAL;
#endif
#ifdef BOOZ_CAM_USE_PAN
booz_cam_pan = booz_ahrs.ltp_to_body_euler.psi;
booz_cam_pan = ahrs.ltp_to_body_euler.psi;
#endif
break;
case BOOZ_CAM_MODE_MANUAL:
sw/airborne/modules/cam_control/cam_track.c
+4 -4
View File
@@ -25,7 +25,7 @@
#include "cam_track.h"
#include "booz2_ins.h"
#include "booz_ahrs.h"
#include "ahrs.h"
#ifdef USE_HFF
#include "ins/booz2_hf_float.h"
@@ -72,7 +72,7 @@ void track_periodic_task(void) {
cmd_msg[c++] = 'A';
cmd_msg[c++] = ' ';
float phi = ANGLE_FLOAT_OF_BFP(booz_ahrs.ltp_to_body_euler.phi);
float phi = ANGLE_FLOAT_OF_BFP(ahrs.ltp_to_body_euler.phi);
if (phi > 0) cmd_msg[c++] = ' ';
else { cmd_msg[c++] = '-'; phi = -phi; }
cmd_msg[c++] = '0' + ((unsigned int) phi % 10);
@@ -81,7 +81,7 @@ void track_periodic_task(void) {
cmd_msg[c++] = '0' + ((unsigned int) (1000*phi) % 10);
cmd_msg[c++] = '0' + ((unsigned int) (10000*phi) % 10);
cmd_msg[c++] = ' ';
float theta = ANGLE_FLOAT_OF_BFP(booz_ahrs.ltp_to_body_euler.theta);
float theta = ANGLE_FLOAT_OF_BFP(ahrs.ltp_to_body_euler.theta);
if (theta > 0) cmd_msg[c++] = ' ';
else { cmd_msg[c++] = '-'; theta = -theta; }
cmd_msg[c++] = '0' + ((unsigned int) theta % 10);
@@ -90,7 +90,7 @@ void track_periodic_task(void) {
cmd_msg[c++] = '0' + ((unsigned int) (1000*theta) % 10);
cmd_msg[c++] = '0' + ((unsigned int) (10000*theta) % 10);
cmd_msg[c++] = ' ';
float psi = ANGLE_FLOAT_OF_BFP(booz_ahrs.ltp_to_body_euler.psi);
float psi = ANGLE_FLOAT_OF_BFP(ahrs.ltp_to_body_euler.psi);
if (psi > 0) cmd_msg[c++] = ' ';
else { cmd_msg[c++] = '-'; psi = -psi; }
cmd_msg[c++] = '0' + ((unsigned int) psi % 10);
sw/airborne/modules/ins/ins_vn100.c
+1 -1
View File
@@ -181,7 +181,7 @@ void parse_ins_msg( void ) {
#include "downlink.h"
extern void ins_report_task( void ) {
DOWNLINK_SEND_BOOZ_AHRS_LKF(DefaultChannel,
DOWNLINK_SEND_AHRS_LKF(DefaultChannel,
&ins_eulers.phi, &ins_eulers.theta, &ins_eulers.psi,
&ins_quat.qi, &ins_quat.qx, &ins_quat.qy, &ins_quat.qz,
&ins_rates.p, &ins_rates.q, &ins_rates.r,
sw/airborne/modules/ins/ins_vn100.h
+1 -1
View File
@@ -79,7 +79,7 @@ extern uint8_t ins_init_status;
#define INS_VN100_READY 3
/* Telemetry */
#define PERIODIC_SEND_AHRS(_chan) DOWNLINK_SEND_BOOZ_AHRS_LKF(_chan, \
#define PERIODIC_SEND_AHRS(_chan) DOWNLINK_SEND_AHRS_LKF(_chan, \
&ins_eulers.phi, &ins_eulers.theta, &ins_eulers.psi, \
&ins_quat.qi, &ins_quat.qx, &ins_quat.qy, &ins_quat.qz, \
&ins_rates.p, &ins_rates.q, &ins_rates.r, \
sw/airborne/modules/vehicle_interface/vi.c
+1 -1
View File
@@ -25,7 +25,7 @@
#include "imu.h"
#include "booz/booz_gps.h"
#include "booz/booz_ahrs.h"
#include "booz/ahrs.h"
#include "airframe.h"
sw/simulator/nps/nps_autopilot_booz.c
+5 -5
View File
@@ -74,16 +74,16 @@ void nps_autopilot_run_step(double time __attribute__ ((unused))) {
}
#include "nps_fdm.h"
#include "booz_ahrs.h"
#include "ahrs.h"
#include "math/pprz_algebra.h"
void sim_overwrite_ahrs(void) {
EULERS_BFP_OF_REAL(booz_ahrs.ltp_to_body_euler, fdm.ltp_to_body_eulers);
EULERS_BFP_OF_REAL(ahrs.ltp_to_body_euler, fdm.ltp_to_body_eulers);
QUAT_BFP_OF_REAL(booz_ahrs.ltp_to_body_quat, fdm.ltp_to_body_quat);
QUAT_BFP_OF_REAL(ahrs.ltp_to_body_quat, fdm.ltp_to_body_quat);
RATES_BFP_OF_REAL(booz_ahrs.body_rate, fdm.body_ecef_rotvel);
RATES_BFP_OF_REAL(ahrs.body_rate, fdm.body_ecef_rotvel);
INT32_RMAT_OF_QUAT(booz_ahrs.ltp_to_body_rmat, booz_ahrs.ltp_to_body_quat);
INT32_RMAT_OF_QUAT(ahrs.ltp_to_body_rmat, ahrs.ltp_to_body_quat);
}
sw/simulator/old_booz/booz2_sim_main.c
+12 -12
View File
@@ -125,7 +125,7 @@ static gboolean booz2_sim_periodic(gpointer data __attribute__ ((unused))) {
}
#include "booz_ahrs.h"
#include "ahrs.h"
static void sim_run_one_step(void) {
@@ -195,20 +195,20 @@ static void sim_run_one_step(void) {
#ifdef BYPASS_AHRS
#include "booz_geometry_mixed.h"
#include "booz_ahrs.h"
#include "ahrs.h"
static void sim_overwrite_ahrs(void) {
booz_ahrs.ltp_to_body_euler.phi = BOOZ_ANGLE_I_OF_F(bfm.eulers->ve[AXIS_X]);
booz_ahrs.ltp_to_body_euler.theta = BOOZ_ANGLE_I_OF_F(bfm.eulers->ve[AXIS_Y]);
booz_ahrs.ltp_to_body_euler.psi = BOOZ_ANGLE_I_OF_F(bfm.eulers->ve[AXIS_Z]);
ahrs.ltp_to_body_euler.phi = BOOZ_ANGLE_I_OF_F(bfm.eulers->ve[AXIS_X]);
ahrs.ltp_to_body_euler.theta = BOOZ_ANGLE_I_OF_F(bfm.eulers->ve[AXIS_Y]);
ahrs.ltp_to_body_euler.psi = BOOZ_ANGLE_I_OF_F(bfm.eulers->ve[AXIS_Z]);
booz_ahrs.ltp_to_body_quat.qi = BOOZ_INT_OF_FLOAT(bfm.quat->ve[QUAT_QI], IQUAT_RES);
booz_ahrs.ltp_to_body_quat.qx = BOOZ_INT_OF_FLOAT(bfm.quat->ve[QUAT_QX], IQUAT_RES);
booz_ahrs.ltp_to_body_quat.qy = BOOZ_INT_OF_FLOAT(bfm.quat->ve[QUAT_QY], IQUAT_RES);
booz_ahrs.ltp_to_body_quat.qz = BOOZ_INT_OF_FLOAT(bfm.quat->ve[QUAT_QZ], IQUAT_RES);
ahrs.ltp_to_body_quat.qi = BOOZ_INT_OF_FLOAT(bfm.quat->ve[QUAT_QI], IQUAT_RES);
ahrs.ltp_to_body_quat.qx = BOOZ_INT_OF_FLOAT(bfm.quat->ve[QUAT_QX], IQUAT_RES);
ahrs.ltp_to_body_quat.qy = BOOZ_INT_OF_FLOAT(bfm.quat->ve[QUAT_QY], IQUAT_RES);
ahrs.ltp_to_body_quat.qz = BOOZ_INT_OF_FLOAT(bfm.quat->ve[QUAT_QZ], IQUAT_RES);
booz_ahrs.body_rate.p = BOOZ_RATE_I_OF_F(bfm.ang_rate_body->ve[AXIS_X]);
booz_ahrs.body_rate.q = BOOZ_RATE_I_OF_F(bfm.ang_rate_body->ve[AXIS_Y]);
booz_ahrs.body_rate.r = BOOZ_RATE_I_OF_F(bfm.ang_rate_body->ve[AXIS_Z]);
ahrs.body_rate.p = BOOZ_RATE_I_OF_F(bfm.ang_rate_body->ve[AXIS_X]);
ahrs.body_rate.q = BOOZ_RATE_I_OF_F(bfm.ang_rate_body->ve[AXIS_Y]);
ahrs.body_rate.r = BOOZ_RATE_I_OF_F(bfm.ang_rate_body->ve[AXIS_Z]);
}
#endif /* BYPASS_AHRS */