diff --git a/conf/airframes/Poine/booz2_a1p.xml b/conf/airframes/Poine/booz2_a1p.xml index dddb01b4d0..8f09786046 100644 --- a/conf/airframes/Poine/booz2_a1p.xml +++ b/conf/airframes/Poine/booz2_a1p.xml @@ -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 diff --git a/conf/autopilot/mercury.makefile b/conf/autopilot/mercury.makefile index 67cd7aed8e..a54e693d49 100644 --- a/conf/autopilot/mercury.makefile +++ b/conf/autopilot/mercury.makefile @@ -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 diff --git a/conf/autopilot/subsystems/rotorcraft/ahrs_cmpl.makefile b/conf/autopilot/subsystems/rotorcraft/ahrs_cmpl.makefile index eaee1d012b..26f8660fd3 100644 --- a/conf/autopilot/subsystems/rotorcraft/ahrs_cmpl.makefile +++ b/conf/autopilot/subsystems/rotorcraft/ahrs_cmpl.makefile @@ -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 diff --git a/conf/autopilot/subsystems/rotorcraft/ahrs_lkf.makefile b/conf/autopilot/subsystems/rotorcraft/ahrs_lkf.makefile index c3261b994f..143786c247 100644 --- a/conf/autopilot/subsystems/rotorcraft/ahrs_lkf.makefile +++ b/conf/autopilot/subsystems/rotorcraft/ahrs_lkf.makefile @@ -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 diff --git a/conf/autopilot/subsystems/rotorcraft/ahrs_mlkf.makefile b/conf/autopilot/subsystems/rotorcraft/ahrs_mlkf.makefile index 2989e4baa6..884b48747f 100644 --- a/conf/autopilot/subsystems/rotorcraft/ahrs_mlkf.makefile +++ b/conf/autopilot/subsystems/rotorcraft/ahrs_mlkf.makefile @@ -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 diff --git a/conf/control_panel.xml.adelaide b/conf/control_panel.xml.adelaide index 834a15fc5d..ee3a5064a6 100644 --- a/conf/control_panel.xml.adelaide +++ b/conf/control_panel.xml.adelaide @@ -111,7 +111,7 @@ - + @@ -119,13 +119,13 @@ - + - + diff --git a/conf/control_panel.xml.example b/conf/control_panel.xml.example index 67af868193..e97471def0 100644 --- a/conf/control_panel.xml.example +++ b/conf/control_panel.xml.example @@ -140,7 +140,7 @@ - + @@ -149,7 +149,7 @@ - + @@ -158,7 +158,7 @@ - + diff --git a/conf/messages.xml b/conf/messages.xml index c11cadc352..3a628ee355 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -1404,7 +1404,7 @@ - + @@ -1423,7 +1423,7 @@ - + @@ -1441,7 +1441,7 @@ - + @@ -1451,7 +1451,7 @@ - + diff --git a/conf/settings/settings_booz2_ahrs_cmpl.xml b/conf/settings/settings_booz2_ahrs_cmpl.xml index 3a56ee73f9..274683aa23 100644 --- a/conf/settings/settings_booz2_ahrs_cmpl.xml +++ b/conf/settings/settings_booz2_ahrs_cmpl.xml @@ -4,7 +4,7 @@ - + diff --git a/conf/settings/settings_booz2_ahrs_lkf.xml b/conf/settings/settings_booz2_ahrs_lkf.xml index bdb41c849b..5bbb3da1f6 100644 --- a/conf/settings/settings_booz2_ahrs_lkf.xml +++ b/conf/settings/settings_booz2_ahrs_lkf.xml @@ -4,10 +4,10 @@ - - - - + + + + diff --git a/sw/airborne/booz/booz2_ins.c b/sw/airborne/booz/booz2_ins.c index 66d6e4daae..dd7a78c182 100644 --- a/sw/airborne/booz/booz2_ins.c +++ b/sw/airborne/booz/booz2_ins.c @@ -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 diff --git a/sw/airborne/booz/booz2_telemetry.h b/sw/airborne/booz/booz2_telemetry.h index 0d7ddb3ff3..a4fc112f32 100644 --- a/sw/airborne/booz/booz2_telemetry.h +++ b/sw/airborne/booz/booz2_telemetry.h @@ -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 \ ); \ } diff --git a/sw/airborne/booz/booz_fms.c b/sw/airborne/booz/booz_fms.c index 8d90d79d5f..7e92c4c2e1 100644 --- a/sw/airborne/booz/booz_fms.c +++ b/sw/airborne/booz/booz_fms.c @@ -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) } diff --git a/sw/airborne/booz/guidance/booz2_guidance_h.c b/sw/airborne/booz/guidance/booz2_guidance_h.c index ecbbbd19d1..88fac59cbf 100644 --- a/sw/airborne/booz/guidance/booz2_guidance_h.c +++ b/sw/airborne/booz/guidance/booz2_guidance_h.c @@ -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 = diff --git a/sw/airborne/booz/guidance/booz2_guidance_v.c b/sw/airborne/booz/guidance/booz2_guidance_v.c index 43ea0918fa..7029d354eb 100644 --- a/sw/airborne/booz/guidance/booz2_guidance_v.c +++ b/sw/airborne/booz/guidance/booz2_guidance_v.c @@ -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; diff --git a/sw/airborne/booz/ins/booz2_hf_float-old.c b/sw/airborne/booz/ins/booz2_hf_float-old.c index 10855052a9..9c3b1d073b 100644 --- a/sw/airborne/booz/ins/booz2_hf_float-old.c +++ b/sw/airborne/booz/ins/booz2_hf_float-old.c @@ -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 */ diff --git a/sw/airborne/booz/ins/booz2_hf_float.c b/sw/airborne/booz/ins/booz2_hf_float.c index 7506e3d2d2..bbe57074f5 100644 --- a/sw/airborne/booz/ins/booz2_hf_float.c +++ b/sw/airborne/booz/ins/booz2_hf_float.c @@ -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 @@ -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 diff --git a/sw/airborne/booz/stabilization/booz_stabilization_attitude_euler_float.c b/sw/airborne/booz/stabilization/booz_stabilization_attitude_euler_float.c index 7e84176e8f..ae98112264 100644 --- a/sw/airborne/booz/stabilization/booz_stabilization_attitude_euler_float.c +++ b/sw/airborne/booz/stabilization/booz_stabilization_attitude_euler_float.c @@ -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); diff --git a/sw/airborne/booz/stabilization/booz_stabilization_attitude_euler_int.c b/sw/airborne/booz/stabilization/booz_stabilization_attitude_euler_int.c index 974d50ad3e..9edfde87af 100644 --- a/sw/airborne/booz/stabilization/booz_stabilization_attitude_euler_int.c +++ b/sw/airborne/booz/stabilization/booz_stabilization_attitude_euler_int.c @@ -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] = diff --git a/sw/airborne/booz/stabilization/booz_stabilization_attitude_quat_float.c b/sw/airborne/booz/stabilization/booz_stabilization_attitude_quat_float.c index 7d1515cab5..01d57c7c9f 100644 --- a/sw/airborne/booz/stabilization/booz_stabilization_attitude_quat_float.c +++ b/sw/airborne/booz/stabilization/booz_stabilization_attitude_quat_float.c @@ -30,7 +30,7 @@ #include #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]; diff --git a/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_float.h b/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_float.h index b808b3c920..72bb4d05d3 100644 --- a/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_float.h +++ b/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_float.h @@ -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); \ } \ } diff --git a/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_int.h b/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_int.h index 58ea1f8cc7..e208c9b5f5 100644 --- a/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_int.h +++ b/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_int.h @@ -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; \ } diff --git a/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_quat_float.c b/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_quat_float.c index 0a06c91a76..2c19341ee9 100644 --- a/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_quat_float.c +++ b/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_quat_float.c @@ -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; } diff --git a/sw/airborne/booz/stabilization/booz_stabilization_rate.c b/sw/airborne/booz/stabilization/booz_stabilization_rate.c index b0592d99d8..1668375109 100644 --- a/sw/airborne/booz/stabilization/booz_stabilization_rate.c +++ b/sw/airborne/booz/stabilization/booz_stabilization_rate.c @@ -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); diff --git a/sw/airborne/booz/test/test_mlkf.c b/sw/airborne/booz/test/test_mlkf.c index c12f945c15..593cc42867 100644 --- a/sw/airborne/booz/test/test_mlkf.c +++ b/sw/airborne/booz/test/test_mlkf.c @@ -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 PPRZ_MODE_MOTORS_OFF && booz_ahrs_aligner.status == BOOZ_AHRS_ALIGNER_LOCKED)); + !(pprz_mode > PPRZ_MODE_MOTORS_OFF && ahrs_aligner.status == AHRS_ALIGNER_LOCKED)); } diff --git a/sw/airborne/csc/mercury_xsens.c b/sw/airborne/csc/mercury_xsens.c index 48dafeb207..cc60f304f0 100644 --- a/sw/airborne/csc/mercury_xsens.c +++ b/sw/airborne/csc/mercury_xsens.c @@ -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(); } } diff --git a/sw/airborne/csc/mercury_xsens.h b/sw/airborne/csc/mercury_xsens.h index d5e501ce4a..01a9c5ca91 100644 --- a/sw/airborne/csc/mercury_xsens.h +++ b/sw/airborne/csc/mercury_xsens.h @@ -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); \ } diff --git a/sw/airborne/firmwares/rotorcraft/ahrs.c b/sw/airborne/firmwares/rotorcraft/ahrs.c index 15b228abb0..3cffc6e3d3 100644 --- a/sw/airborne/firmwares/rotorcraft/ahrs.c +++ b/sw/airborne/firmwares/rotorcraft/ahrs.c @@ -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; diff --git a/sw/airborne/firmwares/rotorcraft/ahrs.h b/sw/airborne/firmwares/rotorcraft/ahrs.h index 198d366daa..0c31e516c2 100644 --- a/sw/airborne/firmwares/rotorcraft/ahrs.h +++ b/sw/airborne/firmwares/rotorcraft/ahrs.h @@ -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 */ diff --git a/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_aligner.c b/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_aligner.c index bd629b80da..fc0dd5fa8d 100644 --- a/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_aligner.c +++ b/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_aligner.c @@ -21,13 +21,13 @@ * Boston, MA 02111-1307, USA. */ -#include "booz_ahrs_aligner.h" +#include "ahrs_aligner.h" #include /* 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 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 diff --git a/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_aligner.h b/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_aligner.h index a2b7c96fbd..1ef1c549bf 100644 --- a/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_aligner.h +++ b/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_aligner.h @@ -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 */ diff --git a/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_cmpl_euler.c b/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_cmpl_euler.c index edf0408434..80d3fe0241 100644 --- a/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_cmpl_euler.c +++ b/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_cmpl_euler.c @@ -1,6 +1,6 @@ /* * $Id: $ - * + * * Copyright (C) 2008-2009 Antoine Drouin * * 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); } - diff --git a/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_cmpl_euler.h b/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_cmpl_euler.h index 05fe0b0310..1e247cf233 100644 --- a/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_cmpl_euler.h +++ b/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_cmpl_euler.h @@ -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 */ diff --git a/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_float_ekf.c b/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_float_ekf.c index ddec19b360..098ae7304d 100644 --- a/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_float_ekf.c +++ b/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_float_ekf.c @@ -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 */ diff --git a/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_float_ekf.h b/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_float_ekf.h index b9dbd73df7..438e46bf95 100644 --- a/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_float_ekf.h +++ b/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_float_ekf.h @@ -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 */ diff --git a/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_float_lkf.c b/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_float_lkf.c index e36aacd68a..394ff1cffd 100644 --- a/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_float_lkf.c +++ b/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_float_lkf.c @@ -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(); } diff --git a/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_float_lkf.h b/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_float_lkf.h index 4184d31f31..16f82bd3e0 100644 --- a/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_float_lkf.h +++ b/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_float_lkf.h @@ -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 */ diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c index f871aaede9..ed28927fbf 100644 --- a/sw/airborne/firmwares/rotorcraft/main.c +++ b/sw/airborne/firmwares/rotorcraft/main.c @@ -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(); } diff --git a/sw/airborne/modules/cam_control/booz_cam.c b/sw/airborne/modules/cam_control/booz_cam.c index 44bfc3a612..07258fbccc 100644 --- a/sw/airborne/modules/cam_control/booz_cam.c +++ b/sw/airborne/modules/cam_control/booz_cam.c @@ -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: diff --git a/sw/airborne/modules/cam_control/cam_track.c b/sw/airborne/modules/cam_control/cam_track.c index d60d1b9547..87718ebfa4 100644 --- a/sw/airborne/modules/cam_control/cam_track.c +++ b/sw/airborne/modules/cam_control/cam_track.c @@ -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); diff --git a/sw/airborne/modules/ins/ins_vn100.c b/sw/airborne/modules/ins/ins_vn100.c index 016c9ad4d8..8053d9838d 100644 --- a/sw/airborne/modules/ins/ins_vn100.c +++ b/sw/airborne/modules/ins/ins_vn100.c @@ -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, diff --git a/sw/airborne/modules/ins/ins_vn100.h b/sw/airborne/modules/ins/ins_vn100.h index b1274be958..1319a0a070 100644 --- a/sw/airborne/modules/ins/ins_vn100.h +++ b/sw/airborne/modules/ins/ins_vn100.h @@ -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, \ diff --git a/sw/airborne/modules/vehicle_interface/vi.c b/sw/airborne/modules/vehicle_interface/vi.c index 647cb4e89c..208068075c 100644 --- a/sw/airborne/modules/vehicle_interface/vi.c +++ b/sw/airborne/modules/vehicle_interface/vi.c @@ -25,7 +25,7 @@ #include "imu.h" #include "booz/booz_gps.h" -#include "booz/booz_ahrs.h" +#include "booz/ahrs.h" #include "airframe.h" diff --git a/sw/simulator/nps/nps_autopilot_booz.c b/sw/simulator/nps/nps_autopilot_booz.c index 47ea91e855..ab77e941cd 100644 --- a/sw/simulator/nps/nps_autopilot_booz.c +++ b/sw/simulator/nps/nps_autopilot_booz.c @@ -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); } diff --git a/sw/simulator/old_booz/booz2_sim_main.c b/sw/simulator/old_booz/booz2_sim_main.c index d1d33b9117..7329ef1dba 100644 --- a/sw/simulator/old_booz/booz2_sim_main.c +++ b/sw/simulator/old_booz/booz2_sim_main.c @@ -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 */