[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [5976] rename booz_ahrs to ahrs, fix makefiles
|
From: |
Felix Ruess |
|
Subject: |
[paparazzi-commits] [5976] rename booz_ahrs to ahrs, fix makefiles |
|
Date: |
Mon, 27 Sep 2010 22:56:40 +0000 |
Revision: 5976
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5976
Author: flixr
Date: 2010-09-27 22:56:39 +0000 (Mon, 27 Sep 2010)
Log Message:
-----------
rename booz_ahrs to ahrs, fix makefiles
Modified Paths:
--------------
paparazzi3/trunk/conf/airframes/Poine/booz2_a1p.xml
paparazzi3/trunk/conf/autopilot/mercury.makefile
paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/ahrs_cmpl.makefile
paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/ahrs_lkf.makefile
paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/ahrs_mlkf.makefile
paparazzi3/trunk/conf/control_panel.xml.adelaide
paparazzi3/trunk/conf/control_panel.xml.example
paparazzi3/trunk/conf/messages.xml
paparazzi3/trunk/conf/settings/settings_booz2_ahrs_cmpl.xml
paparazzi3/trunk/conf/settings/settings_booz2_ahrs_lkf.xml
paparazzi3/trunk/sw/airborne/booz/booz2_ins.c
paparazzi3/trunk/sw/airborne/booz/booz2_telemetry.h
paparazzi3/trunk/sw/airborne/booz/booz_fms.c
paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_h.c
paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_v.c
paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float-old.c
paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.c
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_euler_float.c
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_euler_int.c
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_quat_float.c
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_float.h
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_int.h
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_quat_float.c
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_rate.c
paparazzi3/trunk/sw/airborne/booz/test/test_mlkf.c
paparazzi3/trunk/sw/airborne/csc/mercury_main.c
paparazzi3/trunk/sw/airborne/csc/mercury_xsens.c
paparazzi3/trunk/sw/airborne/csc/mercury_xsens.h
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_aligner.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_aligner.h
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_cmpl_euler.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_cmpl_euler.h
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_float_ekf.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_float_ekf.h
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_float_lkf.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_float_lkf.h
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs.h
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/main.c
paparazzi3/trunk/sw/airborne/modules/cam_control/booz_cam.c
paparazzi3/trunk/sw/airborne/modules/cam_control/cam_track.c
paparazzi3/trunk/sw/airborne/modules/ins/ins_vn100.c
paparazzi3/trunk/sw/airborne/modules/ins/ins_vn100.h
paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi.c
paparazzi3/trunk/sw/simulator/nps/nps_autopilot_booz.c
paparazzi3/trunk/sw/simulator/old_booz/booz2_sim_main.c
Modified: paparazzi3/trunk/conf/airframes/Poine/booz2_a1p.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/Poine/booz2_a1p.xml 2010-09-27 22:56:30 UTC
(rev 5975)
+++ paparazzi3/trunk/conf/airframes/Poine/booz2_a1p.xml 2010-09-27 22:56:39 UTC
(rev 5976)
@@ -221,7 +221,7 @@
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
Modified: paparazzi3/trunk/conf/autopilot/mercury.makefile
===================================================================
--- paparazzi3/trunk/conf/autopilot/mercury.makefile 2010-09-27 22:56:30 UTC
(rev 5975)
+++ paparazzi3/trunk/conf/autopilot/mercury.makefile 2010-09-27 22:56:39 UTC
(rev 5976)
@@ -73,7 +73,7 @@
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
Modified:
paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/ahrs_cmpl.makefile
===================================================================
--- paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/ahrs_cmpl.makefile
2010-09-27 22:56:30 UTC (rev 5975)
+++ paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/ahrs_cmpl.makefile
2010-09-27 22:56:39 UTC (rev 5976)
@@ -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
Modified:
paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/ahrs_lkf.makefile
===================================================================
--- paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/ahrs_lkf.makefile
2010-09-27 22:56:30 UTC (rev 5975)
+++ paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/ahrs_lkf.makefile
2010-09-27 22:56:39 UTC (rev 5976)
@@ -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
Modified:
paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/ahrs_mlkf.makefile
===================================================================
--- paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/ahrs_mlkf.makefile
2010-09-27 22:56:30 UTC (rev 5975)
+++ paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/ahrs_mlkf.makefile
2010-09-27 22:56:39 UTC (rev 5976)
@@ -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
Modified: paparazzi3/trunk/conf/control_panel.xml.adelaide
===================================================================
--- paparazzi3/trunk/conf/control_panel.xml.adelaide 2010-09-27 22:56:30 UTC
(rev 5975)
+++ paparazzi3/trunk/conf/control_panel.xml.adelaide 2010-09-27 22:56:39 UTC
(rev 5976)
@@ -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"/>
Modified: paparazzi3/trunk/conf/control_panel.xml.example
===================================================================
--- paparazzi3/trunk/conf/control_panel.xml.example 2010-09-27 22:56:30 UTC
(rev 5975)
+++ paparazzi3/trunk/conf/control_panel.xml.example 2010-09-27 22:56:39 UTC
(rev 5976)
@@ -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">
Modified: paparazzi3/trunk/conf/messages.xml
===================================================================
--- paparazzi3/trunk/conf/messages.xml 2010-09-27 22:56:30 UTC (rev 5975)
+++ paparazzi3/trunk/conf/messages.xml 2010-09-27 22:56:39 UTC (rev 5976)
@@ -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" />
Modified: paparazzi3/trunk/conf/settings/settings_booz2_ahrs_cmpl.xml
===================================================================
--- paparazzi3/trunk/conf/settings/settings_booz2_ahrs_cmpl.xml 2010-09-27
22:56:30 UTC (rev 5975)
+++ paparazzi3/trunk/conf/settings/settings_booz2_ahrs_cmpl.xml 2010-09-27
22:56:39 UTC (rev 5976)
@@ -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>
Modified: paparazzi3/trunk/conf/settings/settings_booz2_ahrs_lkf.xml
===================================================================
--- paparazzi3/trunk/conf/settings/settings_booz2_ahrs_lkf.xml 2010-09-27
22:56:30 UTC (rev 5975)
+++ paparazzi3/trunk/conf/settings/settings_booz2_ahrs_lkf.xml 2010-09-27
22:56:39 UTC (rev 5976)
@@ -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>
Modified: paparazzi3/trunk/sw/airborne/booz/booz2_ins.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_ins.c 2010-09-27 22:56:30 UTC
(rev 5975)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_ins.c 2010-09-27 22:56:39 UTC
(rev 5976)
@@ -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 @@
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
Modified: paparazzi3/trunk/sw/airborne/booz/booz2_telemetry.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_telemetry.h 2010-09-27 22:56:30 UTC
(rev 5975)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_telemetry.h 2010-09-27 22:56:39 UTC
(rev 5976)
@@ -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 @@
#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 @@
#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 @@
#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 @@
#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 @@
&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 @@
&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 @@
&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 @@
&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 @@
&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 @@
&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 \
); \
}
Modified: paparazzi3/trunk/sw/airborne/booz/booz_fms.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz_fms.c 2010-09-27 22:56:30 UTC
(rev 5975)
+++ paparazzi3/trunk/sw/airborne/booz/booz_fms.c 2010-09-27 22:56:39 UTC
(rev 5976)
@@ -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 @@
//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)
}
Modified: paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_h.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_h.c
2010-09-27 22:56:30 UTC (rev 5975)
+++ paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_h.c
2010-09-27 22:56:39 UTC (rev 5976)
@@ -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 @@
/* 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 @@
/* 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 =
Modified: paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_v.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_v.c
2010-09-27 22:56:30 UTC (rev 5975)
+++ paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_v.c
2010-09-27 22:56:39 UTC (rev 5976)
@@ -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 @@
#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;
Modified: paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float-old.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float-old.c 2010-09-27
22:56:30 UTC (rev 5975)
+++ paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float-old.c 2010-09-27
22:56:39 UTC (rev 5976)
@@ -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 @@
/* 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 @@
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 */
Modified: paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.c 2010-09-27
22:56:30 UTC (rev 5975)
+++ paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.c 2010-09-27
22:56:39 UTC (rev 5976)
@@ -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 @@
/* 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
Modified:
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_euler_float.c
===================================================================
---
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_euler_float.c
2010-09-27 22:56:30 UTC (rev 5975)
+++
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_euler_float.c
2010-09-27 22:56:39 UTC (rev 5976)
@@ -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 @@
/* 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 @@
/* 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);
Modified:
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_euler_int.c
===================================================================
---
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_euler_int.c
2010-09-27 22:56:30 UTC (rev 5975)
+++
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_euler_int.c
2010-09-27 22:56:39 UTC (rev 5976)
@@ -22,7 +22,7 @@
*/
#include "booz_stabilization.h"
-#include "booz_ahrs.h"
+#include "ahrs.h"
#include "booz_radio_control.h"
@@ -108,7 +108,7 @@
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 @@
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] =
Modified:
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_quat_float.c
===================================================================
---
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_quat_float.c
2010-09-27 22:56:30 UTC (rev 5975)
+++
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_quat_float.c
2010-09-27 22:56:39 UTC (rev 5976)
@@ -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 @@
/* 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 @@
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];
Modified:
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_float.h
===================================================================
---
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_float.h
2010-09-27 22:56:30 UTC (rev 5975)
+++
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_float.h
2010-09-27 22:56:39 UTC (rev 5976)
@@ -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); \
} \
}
Modified:
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_int.h
===================================================================
---
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_int.h
2010-09-27 22:56:30 UTC (rev 5975)
+++
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_euler_int.h
2010-09-27 22:56:39 UTC (rev 5976)
@@ -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; \
}
Modified:
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_quat_float.c
===================================================================
---
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_quat_float.c
2010-09-27 22:56:30 UTC (rev 5975)
+++
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_attitude_ref_quat_float.c
2010-09-27 22:56:39 UTC (rev 5976)
@@ -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 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;
}
Modified:
paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_rate.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_rate.c
2010-09-27 22:56:30 UTC (rev 5975)
+++ paparazzi3/trunk/sw/airborne/booz/stabilization/booz_stabilization_rate.c
2010-09-27 22:56:39 UTC (rev 5976)
@@ -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 @@
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);
Modified: paparazzi3/trunk/sw/airborne/booz/test/test_mlkf.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/test/test_mlkf.c 2010-09-27 22:56:30 UTC
(rev 5975)
+++ paparazzi3/trunk/sw/airborne/booz/test/test_mlkf.c 2010-09-27 22:56:39 UTC
(rev 5976)
@@ -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 @@
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 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));
}
Modified: paparazzi3/trunk/sw/airborne/csc/mercury_main.c
===================================================================
--- paparazzi3/trunk/sw/airborne/csc/mercury_main.c 2010-09-27 22:56:30 UTC
(rev 5975)
+++ paparazzi3/trunk/sw/airborne/csc/mercury_main.c 2010-09-27 22:56:39 UTC
(rev 5976)
@@ -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 @@
imu_init();
- booz_ahrs_aligner_init();
- booz_ahrs_init();
+ ahrs_aligner_init();
+ ahrs_init();
xsens_init();
@@ -159,7 +159,7 @@
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));
}
Modified: paparazzi3/trunk/sw/airborne/csc/mercury_xsens.c
===================================================================
--- paparazzi3/trunk/sw/airborne/csc/mercury_xsens.c 2010-09-27 22:56:30 UTC
(rev 5975)
+++ paparazzi3/trunk/sw/airborne/csc/mercury_xsens.c 2010-09-27 22:56:39 UTC
(rev 5976)
@@ -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 @@
// 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();
}
}
Modified: paparazzi3/trunk/sw/airborne/csc/mercury_xsens.h
===================================================================
--- paparazzi3/trunk/sw/airborne/csc/mercury_xsens.h 2010-09-27 22:56:30 UTC
(rev 5975)
+++ paparazzi3/trunk/sw/airborne/csc/mercury_xsens.h 2010-09-27 22:56:39 UTC
(rev 5976)
@@ -62,7 +62,7 @@
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 @@
}
#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); \
}
Modified: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_aligner.c
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_aligner.c
2010-09-27 22:56:30 UTC (rev 5975)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_aligner.c
2010-09-27 22:56:39 UTC (rev 5976)
@@ -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 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 @@
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
Modified: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_aligner.h
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_aligner.h
2010-09-27 22:56:30 UTC (rev 5975)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_aligner.h
2010-09-27 22:56:39 UTC (rev 5976)
@@ -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 @@
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 */
Modified:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_cmpl_euler.c
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_cmpl_euler.c
2010-09-27 22:56:30 UTC (rev 5975)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_cmpl_euler.c
2010-09-27 22:56:39 UTC (rev 5976)
@@ -1,6 +1,6 @@
/*
* $Id: $
- *
+ *
* Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
*
* 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_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 @@
}
/* 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 @@
// 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);
}
-
Modified:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_cmpl_euler.h
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_cmpl_euler.h
2010-09-27 22:56:30 UTC (rev 5975)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_cmpl_euler.h
2010-09-27 22:56:39 UTC (rev 5976)
@@ -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 */
Modified:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_float_ekf.c
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_float_ekf.c
2010-09-27 22:56:30 UTC (rev 5975)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_float_ekf.c
2010-09-27 22:56:39 UTC (rev 5976)
@@ -89,8 +89,8 @@
#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 @@
*
*/
-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 @@
}
-extern void booz_ahrs_update(void);
+extern void ahrs_update(void);
-#endif /* BOOZ_AHRS_FLOAT_EKF_H */
+#endif /* AHRS_FLOAT_EKF_H */
Modified:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_float_ekf.h
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_float_ekf.h
2010-09-27 22:56:30 UTC (rev 5975)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_float_ekf.h
2010-09-27 22:56:39 UTC (rev 5976)
@@ -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 */
Modified:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_float_lkf.c
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_float_lkf.c
2010-09-27 22:56:30 UTC (rev 5975)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_float_lkf.c
2010-09-27 22:56:39 UTC (rev 5976)
@@ -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 @@
#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 @@
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_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 @@
* [ 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 @@
#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 @@
}
-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 @@
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();
}
Modified:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_float_lkf.h
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_float_lkf.h
2010-09-27 22:56:30 UTC (rev 5975)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_float_lkf.h
2010-09-27 22:56:39 UTC (rev 5976)
@@ -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_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 */
Modified: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs.c
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs.c 2010-09-27
22:56:30 UTC (rev 5975)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs.c 2010-09-27
22:56:39 UTC (rev 5976)
@@ -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;
Modified: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs.h
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs.h 2010-09-27
22:56:30 UTC (rev 5975)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs.h 2010-09-27
22:56:39 UTC (rev 5976)
@@ -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 @@
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 */
Modified: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/main.c
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/main.c 2010-09-27
22:56:30 UTC (rev 5975)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/main.c 2010-09-27
22:56:39 UTC (rev 5976)
@@ -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 @@
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 @@
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_mag_event(void) {
ImuScaleMag(imu);
- if (booz_ahrs.status == BOOZ_AHRS_RUNNING)
- booz_ahrs_update_mag();
+ if (ahrs.status == AHRS_RUNNING)
+ ahrs_update_mag();
}
Modified: paparazzi3/trunk/sw/airborne/modules/cam_control/booz_cam.c
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/cam_control/booz_cam.c 2010-09-27
22:56:30 UTC (rev 5975)
+++ paparazzi3/trunk/sw/airborne/modules/cam_control/booz_cam.c 2010-09-27
22:56:39 UTC (rev 5976)
@@ -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 @@
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:
Modified: paparazzi3/trunk/sw/airborne/modules/cam_control/cam_track.c
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/cam_control/cam_track.c
2010-09-27 22:56:30 UTC (rev 5975)
+++ paparazzi3/trunk/sw/airborne/modules/cam_control/cam_track.c
2010-09-27 22:56:39 UTC (rev 5976)
@@ -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 @@
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 @@
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 @@
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);
Modified: paparazzi3/trunk/sw/airborne/modules/ins/ins_vn100.c
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/ins/ins_vn100.c 2010-09-27
22:56:30 UTC (rev 5975)
+++ paparazzi3/trunk/sw/airborne/modules/ins/ins_vn100.c 2010-09-27
22:56:39 UTC (rev 5976)
@@ -181,7 +181,7 @@
#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,
Modified: paparazzi3/trunk/sw/airborne/modules/ins/ins_vn100.h
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/ins/ins_vn100.h 2010-09-27
22:56:30 UTC (rev 5975)
+++ paparazzi3/trunk/sw/airborne/modules/ins/ins_vn100.h 2010-09-27
22:56:39 UTC (rev 5976)
@@ -79,7 +79,7 @@
#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, \
Modified: paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi.c
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi.c 2010-09-27
22:56:30 UTC (rev 5975)
+++ paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi.c 2010-09-27
22:56:39 UTC (rev 5976)
@@ -25,7 +25,7 @@
#include "imu.h"
#include "booz/booz_gps.h"
-#include "booz/booz_ahrs.h"
+#include "booz/ahrs.h"
#include "airframe.h"
Modified: paparazzi3/trunk/sw/simulator/nps/nps_autopilot_booz.c
===================================================================
--- paparazzi3/trunk/sw/simulator/nps/nps_autopilot_booz.c 2010-09-27
22:56:30 UTC (rev 5975)
+++ paparazzi3/trunk/sw/simulator/nps/nps_autopilot_booz.c 2010-09-27
22:56:39 UTC (rev 5976)
@@ -74,16 +74,16 @@
}
#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);
}
Modified: paparazzi3/trunk/sw/simulator/old_booz/booz2_sim_main.c
===================================================================
--- paparazzi3/trunk/sw/simulator/old_booz/booz2_sim_main.c 2010-09-27
22:56:30 UTC (rev 5975)
+++ paparazzi3/trunk/sw/simulator/old_booz/booz2_sim_main.c 2010-09-27
22:56:39 UTC (rev 5976)
@@ -125,7 +125,7 @@
}
-#include "booz_ahrs.h"
+#include "ahrs.h"
static void sim_run_one_step(void) {
@@ -195,20 +195,20 @@
#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 */
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [5976] rename booz_ahrs to ahrs, fix makefiles,
Felix Ruess <=