Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-23 04:45:37 +08:00
Code Issues Actions 24 Packages Projects Releases Wiki Activity

Merge pull request #856 from paparazzi/ahrs_refactor

Browse Source 
AHRS API/Interface refactor

- each AHRS implementation has it's own function names, to basically make it possible to run multiple at the same time
- input data passed as pointer arguments
- wrappers that subscribe to ABI messages (gyro, accel, mag) which call the actual implementation
  - makes it easier to run the actual algorithm e.g. for testing without ABI
- body_to_imu quaternion is also set via ABI
- IMU measurements are published via ABI from the gyro/accel/mag callbacks from main for now
- each ahrs implementation has an `is_aligned` bool for now...
- MLKF: function to only update heading from mag (default now, define AHRS_MAG_UPDATE_ALL_AXES to update all axes as previously)
    
Merging now as it is already useful.
Still TODO:
- publish GPS via ABI as well (and get rid of global ahrs_update_gps)
- update all IMU implementations to publish ABI messages directly
- pimp or get rid of ahrs_register_impl (to actually handle multiple AHRS at the same time)
This commit is contained in:
Felix Ruess
2015-03-01 22:07:23 +01:00
parent e7f2c979d0 bb5ba6e716
commit 2dd069e80a
72 changed files with 2783 additions and 1697 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/abi.xml
+26
View File
@@ -20,6 +20,32 @@
<field name="temp" type="float" unit="deg Celcius"/>
</message>
<message name="IMU_GYRO_INT32" id="4">
<field name="stamp" type="uint32_t" unit="us"/>
<field name="gyro" type="struct Int32Rates *"/>
</message>
<message name="IMU_ACCEL_INT32" id="5">
<field name="stamp" type="uint32_t" unit="us"/>
<field name="accel" type="struct Int32Vect3 *"/>
</message>
<message name="IMU_MAG_INT32" id="6">
<field name="stamp" type="uint32_t" unit="us"/>
<field name="mag" type="struct Int32Vect3 *"/>
</message>
<message name="IMU_LOWPASSED" id="7">
<field name="stamp" type="uint32_t" unit="us"/>
<field name="gyro" type="struct Int32Rates *"/>
<field name="accel" type="struct Int32Vect3 *"/>
<field name="mag" type="struct Int32Vect3 *"/>
</message>
<message name="BODY_TO_IMU_QUAT" id="8">
<field name="q_b2i_f" type="struct FloatQuat *"/>
</message>
</msg_class>
conf/airframes/examples/quadrotor_lisa_mx.xml
+6 -1
View File
@@ -33,7 +33,12 @@
<subsystem name="imu" type="lisa_mx_v2.1"/>
<subsystem name="gps" type="ublox"/>
<subsystem name="stabilization" type="int_quat"/>
<subsystem name="ahrs" type="float_mlkf"/>
<subsystem name="ahrs" type="float_mlkf">
<!-- Uncomment to enable all axis update from the Magnetometer. Do
it only if you have a very well calibrated and tested
magnetometer otherwise your attitude will drift. -->
<!--define name="AHRS_MAG_UPDATE_ALL_AXES" value="1"/-->
</subsystem>
<subsystem name="ins" type="hff"/>
<!--define name="KILL_ON_GROUND_DETECT" value="TRUE"/-->
conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_quat.makefile
+2 -1
View File
@@ -21,10 +21,11 @@ ifneq ($(AHRS_ALIGNER_LED),none)
AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
endif
AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl.h\"
AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_wrapper.h\"
AHRS_CFLAGS += -DAHRS_PROPAGATE_QUAT
AHRS_SRCS += subsystems/ahrs.c
AHRS_SRCS += subsystems/ahrs/ahrs_float_cmpl.c
AHRS_SRCS += subsystems/ahrs/ahrs_float_cmpl_wrapper.c
AHRS_SRCS += subsystems/ahrs/ahrs_aligner.c
ifdef AHRS_PROPAGATE_FREQUENCY
conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_rmat.makefile
+2 -1
View File
@@ -21,10 +21,11 @@ ifneq ($(AHRS_ALIGNER_LED),none)
AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
endif
AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl.h\"
AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_wrapper.h\"
AHRS_CFLAGS += -DAHRS_PROPAGATE_RMAT
AHRS_SRCS += subsystems/ahrs.c
AHRS_SRCS += subsystems/ahrs/ahrs_float_cmpl.c
AHRS_SRCS += subsystems/ahrs/ahrs_float_cmpl_wrapper.c
AHRS_SRCS += subsystems/ahrs/ahrs_aligner.c
ifdef AHRS_PROPAGATE_FREQUENCY
conf/firmwares/subsystems/fixedwing/ahrs_float_dcm.makefile
+2 -1
View File
@@ -5,7 +5,7 @@
USE_MAGNETOMETER ?= 0
AHRS_ALIGNER_LED ?= none
AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_dcm.h\"
AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_dcm_wrapper.h\"
AHRS_CFLAGS += -DUSE_AHRS_ALIGNER
AHRS_CFLAGS += -DUSE_AHRS
@@ -16,6 +16,7 @@ endif
AHRS_SRCS += $(SRC_SUBSYSTEMS)/ahrs.c
AHRS_SRCS += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c
AHRS_SRCS += $(SRC_SUBSYSTEMS)/ahrs/ahrs_float_dcm.c
AHRS_SRCS += $(SRC_SUBSYSTEMS)/ahrs/ahrs_float_dcm_wrapper.c
ifneq ($(AHRS_ALIGNER_LED),none)
conf/firmwares/subsystems/fixedwing/ahrs_gx3.makefile
-9
View File
@@ -4,17 +4,8 @@
GX3_PORT ?= UART3
GX3_BAUD ?= B921600
AHRS_ALIGNER_LED ?= none
AHRS_CFLAGS = -DUSE_AHRS
AHRS_CFLAGS += -DUSE_IMU
AHRS_CFLAGS += -DUSE_IMU_FLOAT
AHRS_CFLAGS += -DUSE_AHRS_ALIGNER
ifneq ($(AHRS_ALIGNER_LED),none)
AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
endif
AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_gx3.h\"
AHRS_SRCS += $(SRC_SUBSYSTEMS)/ahrs.c
conf/firmwares/subsystems/fixedwing/ahrs_int_cmpl_quat.makefile
+2 -1
View File
@@ -22,9 +22,10 @@ ifneq ($(AHRS_ALIGNER_LED),none)
AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
endif
AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_quat.h\"
AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.h\"
AHRS_SRCS += subsystems/ahrs.c
AHRS_SRCS += subsystems/ahrs/ahrs_int_cmpl_quat.c
AHRS_SRCS += subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.c
AHRS_SRCS += subsystems/ahrs/ahrs_aligner.c
conf/firmwares/subsystems/shared/ahrs_float_cmpl_quat.makefile
+2 -1
View File
@@ -19,10 +19,11 @@ ifneq ($(AHRS_ALIGNER_LED),none)
AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
endif
AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl.h\"
AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_wrapper.h\"
AHRS_CFLAGS += -DAHRS_PROPAGATE_QUAT
AHRS_SRCS += subsystems/ahrs.c
AHRS_SRCS += subsystems/ahrs/ahrs_float_cmpl.c
AHRS_SRCS += subsystems/ahrs/ahrs_float_cmpl_wrapper.c
AHRS_SRCS += subsystems/ahrs/ahrs_aligner.c
ifdef AHRS_PROPAGATE_FREQUENCY
conf/firmwares/subsystems/shared/ahrs_float_cmpl_rmat.makefile
+2 -1
View File
@@ -18,10 +18,11 @@ ifneq ($(AHRS_ALIGNER_LED),none)
AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
endif
AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl.h\"
AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_wrapper.h\"
AHRS_CFLAGS += -DAHRS_PROPAGATE_RMAT
AHRS_SRCS += subsystems/ahrs.c
AHRS_SRCS += subsystems/ahrs/ahrs_float_cmpl.c
AHRS_SRCS += subsystems/ahrs/ahrs_float_cmpl_wrapper.c
AHRS_SRCS += subsystems/ahrs/ahrs_aligner.c
ifdef AHRS_PROPAGATE_FREQUENCY
conf/firmwares/subsystems/shared/ahrs_float_mlkf.makefile
+2 -1
View File
@@ -18,9 +18,10 @@ ifneq ($(AHRS_ALIGNER_LED),none)
AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
endif
AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_mlkf.h\"
AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_mlkf_wrapper.h\"
AHRS_SRCS += subsystems/ahrs.c
AHRS_SRCS += subsystems/ahrs/ahrs_float_mlkf.c
AHRS_SRCS += subsystems/ahrs/ahrs_float_mlkf_wrapper.c
AHRS_SRCS += subsystems/ahrs/ahrs_aligner.c
ifdef AHRS_PROPAGATE_FREQUENCY
conf/firmwares/subsystems/rotorcraft/ahrs_gx3.makefile → conf/firmwares/subsystems/shared/ahrs_gx3.makefile
+1 -8
View File
@@ -1,18 +1,11 @@
# Rotorcraft AHRS module for GX3
# AHRS subsystem for GX3
# 2013, Utah State University, http://aggieair.usu.edu/
GX3_PORT ?= UART3
GX3_BAUD ?= B921600
AHRS_ALIGNER_LED ?= none
AHRS_CFLAGS = -DUSE_AHRS
AHRS_CFLAGS += -DAHRS_FLOAT
AHRS_CFLAGS += -DUSE_IMU
AHRS_CFLAGS += -DUSE_IMU_FLOAT
ifneq ($(AHRS_ALIGNER_LED),none)
AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
endif
AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_gx3.h\"
AHRS_SRCS += $(SRC_SUBSYSTEMS)/ahrs.c
conf/firmwares/subsystems/shared/ahrs_int_cmpl_euler.makefile
+2 -1
View File
@@ -17,9 +17,10 @@ ifneq ($(AHRS_ALIGNER_LED),none)
AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
endif
AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_euler.h\"
AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.h\"
AHRS_SRCS += subsystems/ahrs.c
AHRS_SRCS += subsystems/ahrs/ahrs_int_cmpl_euler.c
AHRS_SRCS += subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.c
AHRS_SRCS += subsystems/ahrs/ahrs_aligner.c
ap.CFLAGS += $(AHRS_CFLAGS)
conf/firmwares/subsystems/shared/ahrs_int_cmpl_quat.makefile
+2 -1
View File
@@ -19,9 +19,10 @@ ifneq ($(AHRS_ALIGNER_LED),none)
AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
endif
AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_quat.h\"
AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.h\"
AHRS_SRCS += subsystems/ahrs.c
AHRS_SRCS += subsystems/ahrs/ahrs_int_cmpl_quat.c
AHRS_SRCS += subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.c
AHRS_SRCS += subsystems/ahrs/ahrs_aligner.c
ifdef AHRS_PROPAGATE_FREQUENCY
conf/modules/ahrs_chimu_spi.xml
+1 -1
View File
@@ -19,7 +19,7 @@
<file name="imu_chimu.c"/>
<file name="ahrs.c" dir="subsystems"/>
<raw>
ap.CFLAGS += -DAHRS_TYPE_H=\"modules/ins/ins_module.h\"
ap.CFLAGS += -DAHRS_TYPE_H=\"modules/ins/ahrs_chimu.h\"
</raw>
</makefile>
</module>
conf/modules/ahrs_chimu_uart.xml
+1 -1
View File
@@ -23,7 +23,7 @@
<file name="imu_chimu.c"/>
<file name="ahrs.c" dir="subsystems"/>
<raw>
ap.CFLAGS += -DAHRS_TYPE_H=\"modules/ins/ins_module.h\"
ap.CFLAGS += -DAHRS_TYPE_H=\"modules/ins/ahrs_chimu.h\"
</raw>
</makefile>
</module>
conf/settings/estimation/ahrs_float_cmpl.xml
+5 -5
View File
@@ -4,11 +4,11 @@
<dl_settings>
<dl_settings NAME="AHRS">
<dl_setting var="ahrs_impl.gravity_heuristic_factor" min="0" step="1" max="50" module="subsystems/ahrs/ahrs_float_cmpl" shortname="g_heuristic" param="AHRS_GRAVITY_HEURISTIC_FACTOR"/>
<dl_setting var="ahrs_impl.accel_omega" min="0.02" step="0.02" max="0.2" module="subsystems/ahrs/ahrs_float_cmpl" shortname="acc_omega" param="AHRS_ACCEL_OMEGA" unit="rad/s"/>
<dl_setting var="ahrs_impl.accel_zeta" min="0.7" step="0.05" max="1.5" module="subsystems/ahrs/ahrs_float_cmpl" shortname="acc_zeta" param="AHRS_ACCEL_ZETA"/>
<dl_setting var="ahrs_impl.mag_omega" min="0.02" step="0.01" max="0.1" module="subsystems/ahrs/ahrs_float_cmpl" shortname="mag_omega" param="AHRS_MAG_OMEGA" unit="rad/s"/>
<dl_setting var="ahrs_impl.mag_zeta" min="0.7" step="0.05" max="1.5" module="subsystems/ahrs/ahrs_float_cmpl" shortname="mag_zeta" param="AHRS_MAG_ZETA"/>
<dl_setting var="ahrs_fc.gravity_heuristic_factor" min="0" step="1" max="50" module="subsystems/ahrs/ahrs_float_cmpl" shortname="g_heuristic" param="AHRS_GRAVITY_HEURISTIC_FACTOR"/>
<dl_setting var="ahrs_fc.accel_omega" min="0.02" step="0.02" max="0.2" module="subsystems/ahrs/ahrs_float_cmpl" shortname="acc_omega" param="AHRS_ACCEL_OMEGA" unit="rad/s"/>
<dl_setting var="ahrs_fc.accel_zeta" min="0.7" step="0.05" max="1.5" module="subsystems/ahrs/ahrs_float_cmpl" shortname="acc_zeta" param="AHRS_ACCEL_ZETA"/>
<dl_setting var="ahrs_fc.mag_omega" min="0.02" step="0.01" max="0.1" module="subsystems/ahrs/ahrs_float_cmpl" shortname="mag_omega" param="AHRS_MAG_OMEGA" unit="rad/s"/>
<dl_setting var="ahrs_fc.mag_zeta" min="0.7" step="0.05" max="1.5" module="subsystems/ahrs/ahrs_float_cmpl" shortname="mag_zeta" param="AHRS_MAG_ZETA"/>
</dl_settings>
</dl_settings>
conf/settings/estimation/ahrs_float_mlkf.xml
+3 -3
View File
@@ -4,9 +4,9 @@
<dl_settings>
<dl_settings NAME="AHRS">
<dl_setting var="ahrs_impl.mag_noise.x" min="0.0" step="0.02" max="1.0" module="subsystems/ahrs/ahrs_float_mlkf" shortname="mag_noise_x" param="AHRS_MAG_NOISE_X"/>
<dl_setting var="ahrs_impl.mag_noise.y" min="0.0" step="0.02" max="1.0" module="subsystems/ahrs/ahrs_float_mlkf" shortname="mag_noise_y" param="AHRS_MAG_NOISE_Y"/>
<dl_setting var="ahrs_impl.mag_noise.z" min="0.0" step="0.02" max="1.0" module="subsystems/ahrs/ahrs_float_mlkf" shortname="mag_noise_z" param="AHRS_MAG_NOISE_Z"/>
<dl_setting var="ahrs_mlkf.mag_noise.x" min="0.0" step="0.02" max="1.0" module="subsystems/ahrs/ahrs_float_mlkf" shortname="mag_noise_x" param="AHRS_MAG_NOISE_X"/>
<dl_setting var="ahrs_mlkf.mag_noise.y" min="0.0" step="0.02" max="1.0" module="subsystems/ahrs/ahrs_float_mlkf" shortname="mag_noise_y" param="AHRS_MAG_NOISE_Y"/>
<dl_setting var="ahrs_mlkf.mag_noise.z" min="0.0" step="0.02" max="1.0" module="subsystems/ahrs/ahrs_float_mlkf" shortname="mag_noise_z" param="AHRS_MAG_NOISE_Z"/>
</dl_settings>
</dl_settings>
conf/settings/estimation/ahrs_int_cmpl_euler.xml
+1 -1
View File
@@ -4,7 +4,7 @@
<dl_settings>
<dl_settings NAME="Filter">
<dl_setting var="ahrs_impl.reinj_1" min="512" step="512" max="262144" module="subsystems/ahrs/ahrs_int_cmpl_euler" shortname="reinj_1"/>
<dl_setting var="ahrs_ice.reinj_1" min="512" step="512" max="262144" module="subsystems/ahrs/ahrs_int_cmpl_euler" shortname="reinj_1"/>
</dl_settings>
</dl_settings>
conf/settings/estimation/ahrs_int_cmpl_quat.xml
+5 -5
View File
@@ -4,11 +4,11 @@
<dl_settings>
<dl_settings NAME="AHRS">
<dl_setting var="ahrs_impl.gravity_heuristic_factor" min="0" step="1" max="50" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="g_heuristic" param="AHRS_GRAVITY_HEURISTIC_FACTOR"/>
<dl_setting var="ahrs_impl.accel_omega" min="0.02" step="0.02" max="0.2" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="acc_omega" param="AHRS_ACCEL_OMEGA" unit="rad/s" handler="SetAccelOmega"/>
<dl_setting var="ahrs_impl.accel_zeta" min="0.7" step="0.05" max="1.5" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="acc_zeta" param="AHRS_ACCEL_ZETA" handler="SetAccelZeta"/>
<dl_setting var="ahrs_impl.mag_omega" min="0.02" step="0.01" max="0.1" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="mag_omega" param="AHRS_MAG_OMEGA" unit="rad/s" handler="SetMagOmega"/>
<dl_setting var="ahrs_impl.mag_zeta" min="0.7" step="0.05" max="1.5" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="mag_zeta" param="AHRS_MAG_ZETA" handler="SetMagZeta"/>
<dl_setting var="ahrs_icq.gravity_heuristic_factor" min="0" step="1" max="50" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="g_heuristic" param="AHRS_GRAVITY_HEURISTIC_FACTOR"/>
<dl_setting var="ahrs_icq.accel_omega" min="0.02" step="0.02" max="0.2" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="acc_omega" param="AHRS_ACCEL_OMEGA" unit="rad/s" handler="SetAccelOmega"/>
<dl_setting var="ahrs_icq.accel_zeta" min="0.7" step="0.05" max="1.5" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="acc_zeta" param="AHRS_ACCEL_ZETA" handler="SetAccelZeta"/>
<dl_setting var="ahrs_icq.mag_omega" min="0.02" step="0.01" max="0.1" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="mag_omega" param="AHRS_MAG_OMEGA" unit="rad/s" handler="SetMagOmega"/>
<dl_setting var="ahrs_icq.mag_zeta" min="0.7" step="0.05" max="1.5" module="subsystems/ahrs/ahrs_int_cmpl_quat" shortname="mag_zeta" param="AHRS_MAG_ZETA" handler="SetMagZeta"/>
</dl_settings>
</dl_settings>
sw/airborne/arch/sim/sim_ahrs.c
-5
View File
@@ -15,7 +15,6 @@ float sim_psi; ///< in radians
float sim_p; ///< in radians/s
float sim_q; ///< in radians/s
float sim_r; ///< in radians/s
bool_t ahrs_sim_available;
// Updates from Ocaml sim
@@ -25,8 +24,6 @@ value provide_attitude(value phi, value theta, value psi)
sim_theta = Double_val(theta);
sim_psi = - Double_val(psi) + M_PI / 2.;
ahrs_sim_available = TRUE;
return Val_unit;
}
@@ -36,8 +33,6 @@ value provide_rates(value p, value q, value r)
sim_q = Double_val(q);
sim_r = Double_val(r);
ahrs_sim_available = TRUE;
return Val_unit;
}
sw/airborne/boards/ardrone/actuators_at.c
+9 -9
View File
@@ -43,28 +43,28 @@ void actuators_set(pprz_t commands[])
float yaw = ((float)commands[COMMAND_YAW] / (float)MAX_PPRZ);
//Starting engine
if (thrust > 0 && (ahrs_impl.control_state == CTRL_DEFAULT || ahrs_impl.control_state == CTRL_INIT
|| ahrs_impl.control_state == CTRL_LANDED)) {
if(thrust > 0 && (ahrs_ardrone2.control_state == CTRL_DEFAULT || ahrs_ardrone2.control_state == CTRL_INIT
|| ahrs_ardrone2.control_state == CTRL_LANDED)) {
at_com_send_ref(REF_TAKEOFF);
}
//Check emergency or stop engine
if ((ahrs_impl.state & ARDRONE_EMERGENCY_MASK) != 0) {
if ((ahrs_ardrone2.state & ARDRONE_EMERGENCY_MASK) != 0) {
at_com_send_ref(REF_EMERGENCY);
} else if (thrust < -0.9 && !(ahrs_impl.control_state == CTRL_DEFAULT || ahrs_impl.control_state == CTRL_INIT
|| ahrs_impl.control_state == CTRL_LANDED)) {
} else if(thrust < -0.9 && !(ahrs_ardrone2.control_state == CTRL_DEFAULT ||
ahrs_ardrone2.control_state == CTRL_INIT || ahrs_ardrone2.control_state == CTRL_LANDED)) {
at_com_send_ref(0);
}
//Calibration
if ((ahrs_impl.state & ARDRONE_MAGNETO_NEEDS_CALIB) != 0 && (ahrs_impl.control_state == CTRL_FLYING
|| ahrs_impl.control_state == CTRL_HOVERING)) {
if ((ahrs_ardrone2.state & ARDRONE_MAGNETO_NEEDS_CALIB) != 0 &&
(ahrs_ardrone2.control_state == CTRL_FLYING || ahrs_ardrone2.control_state == CTRL_HOVERING)) {
at_com_send_calib(0);
}
//Moving
if ((ahrs_impl.state & ARDRONE_MAGNETO_NEEDS_CALIB) == 0 && (ahrs_impl.control_state == CTRL_FLYING
|| ahrs_impl.control_state == CTRL_HOVERING)) {
if ((ahrs_ardrone2.state & ARDRONE_MAGNETO_NEEDS_CALIB) == 0 &&
(ahrs_ardrone2.control_state == CTRL_FLYING || ahrs_ardrone2.control_state == CTRL_HOVERING)) {
at_com_send_pcmd(1, thrust, roll, pitch, yaw);
}
sw/airborne/boards/ardrone/navdata.c
+1 -1
View File
@@ -157,7 +157,7 @@ static void send_navdata(struct transport_tx *trans, struct link_device *dev)
static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
{
uint8_t mde = 3;
if (ahrs.status == AHRS_UNINIT) { mde = 2; }
if (!DefaultAhrsImpl.is_aligned) { mde = 2; }
if (imu_lost) { mde = 5; }
uint16_t val = imu_lost_counter;
pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &mde, &val);
sw/airborne/firmwares/fixedwing/main_ap.c
+41 -59
View File
@@ -126,6 +126,11 @@ PRINT_CONFIG_VAR(MODULES_FREQUENCY)
PRINT_CONFIG_VAR(BARO_PERIODIC_FREQUENCY)
#endif
#define __DefaultAhrsRegister(_x) _x ## _register()
#define _DefaultAhrsRegister(_x) __DefaultAhrsRegister(_x)
#define DefaultAhrsRegister() _DefaultAhrsRegister(DefaultAhrsImpl)
#if USE_AHRS && USE_IMU
#ifdef AHRS_PROPAGATE_FREQUENCY
@@ -144,7 +149,7 @@ volatile uint8_t ahrs_timeout_counter = 0;
static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
{
uint8_t mde = 3;
if (ahrs.status == AHRS_UNINIT) { mde = 2; }
if (!DefaultAhrsImpl.is_aligned) { mde = 2; }
if (ahrs_timeout_counter > 10) { mde = 5; }
uint16_t val = 0;
pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &mde, &val);
@@ -195,8 +200,15 @@ void init_ap(void)
#endif
#if USE_AHRS
#if defined SITL && !USE_NPS && !USE_INFRARED
ahrs_sim_init();
#else
ahrs_init();
DefaultAhrsRegister();
#endif
#endif
ins_init();
#if USE_AHRS && USE_IMU
register_periodic_telemetry(DefaultPeriodic, "STATE_FILTER_STATUS", send_filter_status);
@@ -206,8 +218,6 @@ void init_ap(void)
baro_init();
#endif
ins_init();
/************* Links initialization ***************/
#if defined MCU_SPI_LINK || defined MCU_UART_LINK || defined MCU_CAN_LINK
link_mcu_init();
@@ -261,6 +271,11 @@ void init_ap(void)
ap_state->command_roll_trim = COMMAND_ROLL_TRIM;
ap_state->command_pitch_trim = COMMAND_PITCH_TRIM;
ap_state->command_yaw_trim = COMMAND_YAW_TRIM;
#if USE_IMU
// send body_to_imu from here for now
AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
#endif
}
@@ -615,9 +630,8 @@ void sensors_task(void)
#endif // USE_IMU
//FIXME: this is just a kludge
#if USE_AHRS && defined SITL && !USE_NPS
// dt is not really used in ahrs_sim
ahrs_propagate(1. / PERIODIC_FREQUENCY);
#if USE_AHRS && defined SITL && !USE_NPS && !USE_INFRARED
update_ahrs_from_sim();
#endif
#if USE_GPS
@@ -738,10 +752,10 @@ void event_task_ap(void)
#if USE_GPS
static inline void on_gps_solution(void)
{
ins_update_gps();
#if USE_AHRS
ahrs_update_gps();
#endif
ins_update_gps();
#ifdef GPS_TRIGGERED_FUNCTION
GPS_TRIGGERED_FUNCTION();
#endif
@@ -749,69 +763,53 @@ static inline void on_gps_solution(void)
#endif
#if USE_AHRS
#if USE_IMU
static inline void on_accel_event(void)
{
#if USE_AUTO_AHRS_FREQ || !defined(AHRS_CORRECT_FREQUENCY)
PRINT_CONFIG_MSG("Calculating dt for AHRS accel update.")
// timestamp in usec when last callback was received
static uint32_t last_ts = 0;
// current timestamp
uint32_t now_ts = get_sys_time_usec();
// dt between this and last callback in seconds
float dt = (float)(now_ts - last_ts) / 1e6;
last_ts = now_ts;
#else
PRINT_CONFIG_MSG("Using fixed AHRS_CORRECT_FREQUENCY for AHRS accel update.")
PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY)
const float dt = 1. / AHRS_CORRECT_FREQUENCY;
#endif
imu_scale_accel(&imu);
if (ahrs.status != AHRS_UNINIT) {
ahrs_update_accel(dt);
}
AbiSendMsgIMU_ACCEL_INT32(1, now_ts, &imu.accel);
}
static inline void on_gyro_event(void)
{
#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
PRINT_CONFIG_MSG("Calculating dt for AHRS/INS propagation.")
// timestamp in usec when last callback was received
static uint32_t last_ts = 0;
// current timestamp
uint32_t now_ts = get_sys_time_usec();
// dt between this and last callback in seconds
float dt = (float)(now_ts - last_ts) / 1e6;
last_ts = now_ts;
#else
PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS/INS propagation.")
PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
#endif
ahrs_timeout_counter = 0;
imu_scale_gyro(&imu);
AbiSendMsgIMU_GYRO_INT32(1, now_ts, &imu.gyro_prev);
#if USE_AHRS_ALIGNER
// Run aligner on raw data as it also makes averages.
if (ahrs.status == AHRS_UNINIT) {
if (ahrs_aligner.status != AHRS_ALIGNER_LOCKED) {
ahrs_aligner_run();
if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED) {
ahrs_align();
}
return;
}
#endif
ahrs_propagate(dt);
#if defined SITL && USE_NPS
if (nps_bypass_ahrs) { sim_overwrite_ahrs(); }
#endif
#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
PRINT_CONFIG_MSG("Calculating dt for INS propagation.")
// timestamp in usec when last callback was received
static uint32_t last_ts = 0;
// dt between this and last callback in seconds
float dt = (float)(now_ts - last_ts) / 1e6;
last_ts = now_ts;
#else
PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for INS propagation.")
PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
#endif
ins_propagate(dt);
#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
new_ins_attitude = 1;
#endif
@@ -821,28 +819,12 @@ static inline void on_gyro_event(void)
static inline void on_mag_event(void)
{
#if USE_MAGNETOMETER
#if USE_AUTO_AHRS_FREQ || !defined(AHRS_MAG_CORRECT_FREQUENCY)
PRINT_CONFIG_MSG("Calculating dt for AHRS mag update.")
// timestamp in usec when last callback was received
static uint32_t last_ts = 0;
// current timestamp
uint32_t now_ts = get_sys_time_usec();
// dt between this and last callback in seconds
float dt = (float)(now_ts - last_ts) / 1e6;
last_ts = now_ts;
#else
PRINT_CONFIG_MSG("Using fixed AHRS_MAG_CORRECT_FREQUENCY for AHRS mag update.")
PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY)
const float dt = 1. / (AHRS_MAG_CORRECT_FREQUENCY);
#endif
imu_scale_mag(&imu);
if (ahrs.status == AHRS_RUNNING) {
ahrs_update_mag(dt);
}
AbiSendMsgIMU_MAG_INT32(1, now_ts, &imu.mag);
#endif
}
#endif // USE_IMU
#endif // USE_AHRS
sw/airborne/firmwares/rotorcraft/autopilot.c
+1 -1
View File
@@ -92,7 +92,7 @@ bool_t autopilot_detect_ground_once;
#include "subsystems/ahrs.h"
static inline int ahrs_is_aligned(void)
{
return (ahrs.status == AHRS_RUNNING);
return DefaultAhrsImpl.is_aligned;
}
#else
PRINT_CONFIG_MSG("Using AUTOPILOT_DISABLE_AHRS_KILL")
sw/airborne/firmwares/rotorcraft/main.c
+40 -57
View File
@@ -69,7 +69,9 @@ PRINT_CONFIG_MSG_VALUE("USE_BARO_BOARD is TRUE, reading onboard baro: ", BARO_BO
#include "firmwares/rotorcraft/guidance.h"
#include "subsystems/ahrs.h"
#if USE_AHRS_ALIGNER
#include "subsystems/ahrs/ahrs_aligner.h"
#endif
#include "subsystems/ins.h"
#include "state.h"
@@ -112,6 +114,10 @@ INFO_VALUE("it is recommended to configure in your airframe PERIODIC_FREQUENCY t
#endif
#endif
#define __DefaultAhrsRegister(_x) _x ## _register()
#define _DefaultAhrsRegister(_x) __DefaultAhrsRegister(_x)
#define DefaultAhrsRegister() _DefaultAhrsRegister(DefaultAhrsImpl)
static inline void on_gyro_event(void);
static inline void on_accel_event(void);
static inline void on_gps_event(void);
@@ -160,10 +166,14 @@ STATIC_INLINE void main_init(void)
baro_init();
#endif
imu_init();
#if USE_AHRS_ALIGNER
ahrs_aligner_init();
#endif
ahrs_init();
ins_init();
DefaultAhrsRegister();
#if USE_GPS
gps_init();
#endif
@@ -190,6 +200,9 @@ STATIC_INLINE void main_init(void)
#if USE_BARO_BOARD
baro_tid = sys_time_register_timer(1. / BARO_PERIODIC_FREQUENCY, NULL);
#endif
// send body_to_imu from here for now
AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
}
STATIC_INLINE void handle_periodic_tasks(void)
@@ -343,61 +356,48 @@ STATIC_INLINE void main_event(void)
}
static inline void on_accel_event(void)
{
#if USE_AUTO_AHRS_FREQ || !defined(AHRS_CORRECT_FREQUENCY)
PRINT_CONFIG_MSG("Calculating dt for AHRS accel update.")
// timestamp in usec when last callback was received
static uint32_t last_ts = 0;
static inline void on_accel_event( void ) {
// current timestamp
uint32_t now_ts = get_sys_time_usec();
// dt between this and last callback
float dt = (float)(now_ts - last_ts) / 1e6;
last_ts = now_ts;
#else
PRINT_CONFIG_MSG("Using fixed AHRS_CORRECT_FREQUENCY for AHRS accel update.")
PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY)
const float dt = 1. / (AHRS_CORRECT_FREQUENCY);
#endif
imu_scale_accel(&imu);
if (ahrs.status != AHRS_UNINIT) {
ahrs_update_accel(dt);
}
AbiSendMsgIMU_ACCEL_INT32(1, now_ts, &imu.accel);
}
static inline void on_gyro_event(void)
{
#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
PRINT_CONFIG_MSG("Calculating dt for AHRS/INS propagation.")
// timestamp in usec when last callback was received
static uint32_t last_ts = 0;
static inline void on_gyro_event( void ) {
// current timestamp
uint32_t now_ts = get_sys_time_usec();
imu_scale_gyro(&imu);
AbiSendMsgIMU_GYRO_INT32(1, now_ts, &imu.gyro_prev);
#if USE_AHRS_ALIGNER
if (ahrs_aligner.status != AHRS_ALIGNER_LOCKED) {
ahrs_aligner_run();
return;
}
#endif
#ifdef SITL
if (nps_bypass_ahrs) sim_overwrite_ahrs();
#endif
#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
PRINT_CONFIG_MSG("Calculating dt for INS propagation.")
// timestamp in usec when last callback was received
static uint32_t last_ts = 0;
// dt between this and last callback in seconds
float dt = (float)(now_ts - last_ts) / 1e6;
last_ts = now_ts;
#else
PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS/INS propagation.")
PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for INS propagation.")
PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
#endif
ins_propagate(dt);
imu_scale_gyro(&imu);
if (ahrs.status == AHRS_UNINIT) {
ahrs_aligner_run();
if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED) {
ahrs_align();
}
} else {
ahrs_propagate(dt);
#ifdef SITL
if (nps_bypass_ahrs) { sim_overwrite_ahrs(); }
#endif
ins_propagate(dt);
}
#ifdef USE_VEHICLE_INTERFACE
vi_notify_imu_available();
#endif
@@ -417,27 +417,10 @@ static inline void on_gps_event(void)
static inline void on_mag_event(void)
{
imu_scale_mag(&imu);
#if USE_MAGNETOMETER
#if USE_AUTO_AHRS_FREQ || !defined(AHRS_MAG_CORRECT_FREQUENCY)
PRINT_CONFIG_MSG("Calculating dt for AHRS mag update.")
// timestamp in usec when last callback was received
static uint32_t last_ts = 0;
// current timestamp
uint32_t now_ts = get_sys_time_usec();
// dt between this and last callback in seconds
float dt = (float)(now_ts - last_ts) / 1e6;
last_ts = now_ts;
#else
PRINT_CONFIG_MSG("Using fixed AHRS_MAG_CORRECT_FREQUENCY for AHRS mag update.")
PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY)
const float dt = 1. / (AHRS_MAG_CORRECT_FREQUENCY);
#endif
if (ahrs.status == AHRS_RUNNING) {
ahrs_update_mag(dt);
}
#endif // USE_MAGNETOMETER
AbiSendMsgIMU_MAG_INT32(1, now_ts, &imu.mag);
#ifdef USE_VEHICLE_INTERFACE
vi_notify_mag_available();
sw/airborne/modules/datalink/mavlink.c
+1 -1
View File
@@ -296,7 +296,7 @@ static inline void mavlink_send_heartbeat(void)
#else
uint8_t mav_type = MAV_TYPE_QUADROTOR;
#endif
if (ahrs.status == AHRS_RUNNING) {
if (DefaultAhrsImpl.is_aligned) {
if (kill_throttle) {
mav_state = MAV_STATE_STANDBY;
}
sw/airborne/modules/geo_mag/geo_mag.c
+3 -3
View File
@@ -76,11 +76,11 @@ void geo_mag_event(void)
// copy to ahrs
#ifdef AHRS_FLOAT
VECT3_COPY(ahrs_impl.mag_h, geo_mag.vect);
VECT3_COPY(DefaultAhrsImpl.mag_h, geo_mag.vect);
#else
// convert to MAG_BFP and copy to ahrs
VECT3_ASSIGN(ahrs_impl.mag_h, MAG_BFP_OF_REAL(geo_mag.vect.x), MAG_BFP_OF_REAL(geo_mag.vect.y),
MAG_BFP_OF_REAL(geo_mag.vect.z));
VECT3_ASSIGN(DefaultAhrsImpl.mag_h, MAG_BFP_OF_REAL(geo_mag.vect.x),
MAG_BFP_OF_REAL(geo_mag.vect.y), MAG_BFP_OF_REAL(geo_mag.vect.z));
#endif
geo_mag.ready = TRUE;
sw/airborne/modules/ins/ahrs_chimu.h
+43
View File
@@ -0,0 +1,43 @@
/*
* Copyright (C) 2014 Felix Ruess
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* 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.
*/
/**
* @file modules/ins/ahrs_chimu.h
*/
#ifndef AHRS_CHIMU_H
#define AHRS_CHIMU_H
#include "modules/ins/ins_module.h"
#include "subsystems/ahrs.h"
struct AhrsChimu {
bool_t is_aligned;
};
extern struct AhrsChimu ahrs_chimu;
#define DefaultAhrsImpl ahrs_chimu
extern void ahrs_chimu_register(void);
extern void ahrs_chimu_init(void);
#endif
sw/airborne/modules/ins/ahrs_chimu_spi.c
+15 -15
View File
@@ -26,6 +26,7 @@
#include "ins_module.h"
#include "imu_chimu.h"
#include "ahrs_chimu.h"
#include "led.h"
@@ -34,9 +35,18 @@ CHIMU_PARSER_DATA CHIMU_DATA;
INS_FORMAT ins_roll_neutral;
INS_FORMAT ins_pitch_neutral;
void ahrs_init(void)
struct AhrsChimu ahrs_chimu;
void ahrs_chimu_update_gps(void);
void ahrs_chimu_register(void)
{
ahrs.status = AHRS_UNINIT;
ahrs_register_impl(ahrs_chimu_init, ahrs_chimu_update_gps);
}
void ahrs_chimu_init(void)
{
ahrs_chimu.is_aligned = FALSE;
// uint8_t ping[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG00_PING, 0x00, 0xE6 };
uint8_t rate[12] = {CHIMU_STX, CHIMU_STX, 0x06, CHIMU_BROADCAST, MSG10_UARTSETTINGS, 0x05, 0xff, 0x79, 0x00, 0x00, 0x01, 0x76 }; // 50Hz attitude only + SPI
@@ -68,10 +78,6 @@ void ahrs_init(void)
InsSend(rate, 12);
}
void ahrs_align(void)
{
ahrs.status = AHRS_RUNNING;
}
void parse_ins_msg(void)
{
@@ -98,6 +104,8 @@ void parse_ins_msg(void)
0.
}; // FIXME rate r
stateSetBodyRates_f(&rates);
//FIXME
ahrs_chimu.is_aligned = TRUE;
} else if (CHIMU_DATA.m_MsgID == 0x02) {
#if CHIMU_DOWNLINK_IMMEDIATE
RunOnceEvery(25, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &CHIMU_DATA.m_sensor.rate[0],
@@ -109,7 +117,7 @@ void parse_ins_msg(void)
}
void ahrs_update_gps(void)
void ahrs_chimu_update_gps(void)
{
// Send SW Centripetal Corrections
uint8_t centripedal[19] = {0xae, 0xae, 0x0d, 0xaa, 0x0b, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc2 };
@@ -130,11 +138,3 @@ void ahrs_update_gps(void)
// Downlink Send
}
void ahrs_propagate(float dt __attribute__((unused)))
{
}
void ahrs_update_accel(float dt __attribute__((unused)))
{
}
sw/airborne/modules/ins/ahrs_chimu_uart.c
+10 -10
View File
@@ -30,9 +30,16 @@ CHIMU_PARSER_DATA CHIMU_DATA;
INS_FORMAT ins_roll_neutral;
INS_FORMAT ins_pitch_neutral;
void ahrs_init(void)
struct AhrsChimu ahrs_chimu;
void ahrs_chimu_register(void)
{
ahrs.status = AHRS_UNINIT;
ahrs_register_impl(ahrs_chimu_init, NULL);
}
void ahrs_chimu_init(void)
{
ahrs_chimu.is_aligned = FALSE;
uint8_t ping[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG00_PING, 0x00, 0xE6 };
uint8_t rate[12] = {CHIMU_STX, CHIMU_STX, 0x06, CHIMU_BROADCAST, MSG10_UARTSETTINGS, 0x05, 0xff, 0x79, 0x00, 0x00, 0x01, 0x76 }; // 50Hz attitude only + SPI
@@ -61,10 +68,6 @@ void ahrs_init(void)
CHIMU_Checksum(rate, 12);
InsSend(rate, 12);
}
void ahrs_align(void)
{
ahrs.status = AHRS_RUNNING;
}
void parse_ins_msg(void)
@@ -86,6 +89,7 @@ void parse_ins_msg(void)
CHIMU_DATA.m_attitude.euler.psi
};
stateSetNedToBodyEulers_f(&att);
ahrs_chimu.is_aligned = TRUE;
#if CHIMU_DOWNLINK_IMMEDIATE
DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &CHIMU_DATA.m_attitude.euler.phi,
&CHIMU_DATA.m_attitude.euler.theta, &CHIMU_DATA.m_attitude.euler.psi);
@@ -95,7 +99,3 @@ void parse_ins_msg(void)
}
}
}
void ahrs_update_gps(void)
{
}
sw/airborne/modules/sensors/mag_hmc58xx.c
+11 -28
View File
@@ -32,7 +32,7 @@
#if MODULE_HMC58XX_UPDATE_AHRS
#include "subsystems/imu.h"
#include "subsystems/ahrs.h"
#include "subsystems/abi.h"
#ifndef HMC58XX_CHAN_X
#define HMC58XX_CHAN_X 0
@@ -60,20 +60,13 @@ void mag_hmc58xx_module_periodic(void)
void mag_hmc58xx_module_event(void)
{
#if USE_AUTO_AHRS_FREQ || !defined(AHRS_MAG_CORRECT_FREQUENCY)
PRINT_CONFIG_MSG("Calculating dt for AHRS mag update.")
// timestamp in usec when last callback was received
static uint32_t last_ts = 0;
#else
PRINT_CONFIG_MSG("Using fixed AHRS_MAG_CORRECT_FREQUENCY for AHRS mag update.")
PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY)
const float dt = 1. / (AHRS_MAG_CORRECT_FREQUENCY);
#endif
hmc58xx_event(&mag_hmc58xx);
#if MODULE_HMC58XX_UPDATE_AHRS
if (mag_hmc58xx.data_available) {
#if MODULE_HMC58XX_UPDATE_AHRS
// current timestamp
uint32_t now_ts = get_sys_time_usec();
// set channel order
struct Int32Vect3 mag = {
(int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_X]),
@@ -84,26 +77,16 @@ void mag_hmc58xx_module_event(void)
VECT3_COPY(imu.mag_unscaled, mag);
// scale vector
imu_scale_mag(&imu);
// update ahrs
if (ahrs.status == AHRS_RUNNING) {
#if USE_AUTO_AHRS_FREQ || !defined(AHRS_MAG_CORRECT_FREQUENCY)
// current timestamp
uint32_t now_ts = get_sys_time_usec();
// dt between this and last callback in seconds
float dt = (float)(now_ts - last_ts) / 1e6;
last_ts = now_ts;
#endif
ahrs_update_mag(dt);
}
mag_hmc58xx.data_available = FALSE;
}
AbiSendMsgIMU_MAG_INT32(MAG_HMC58XX_SENDER_ID, now_ts, &imu.mag);
#endif
#if MODULE_HMC58XX_SYNC_SEND
if (mag_hmc58xx.data_available) {
mag_hmc58xx_report();
mag_hmc58xx.data_available = FALSE;
}
#endif
#if MODULE_HMC58XX_UPDATE_AHRS || MODULE_HMC58XX_SYNC_SEND
mag_hmc58xx.data_available = FALSE;
#endif
}
}
void mag_hmc58xx_report(void)
sw/airborne/subsystems/abi_sender_ids.h
+7
View File
@@ -103,5 +103,12 @@
#define AGL_SONAR_NPS_ID 3
#endif
/*
* IDs of magnetometer sensors (including IMUs with mag)
*/
#ifndef MAG_HMC58XX_SENDER_ID
#define MAG_HMC58XX_SENDER_ID 2
#endif
#endif /* ABI_SENDER_IDS_H */
sw/airborne/subsystems/ahrs.c
+17 -6
View File
@@ -29,12 +29,23 @@
struct Ahrs ahrs;
// weak functions, used if not explicitly provided by implementation
void ahrs_register_impl(AhrsInit init, AhrsUpdateGps update_gps)
{
ahrs.init = init;
ahrs.update_gps = update_gps;
void WEAK ahrs_propagate(float dt __attribute__((unused))) {}
ahrs.init();
}
void WEAK ahrs_update_accel(float dt __attribute__((unused))) {}
void ahrs_init(void)
{
ahrs.init = NULL;
ahrs.update_gps = NULL;
}
void WEAK ahrs_update_mag(float dt __attribute__((unused))) {}
void WEAK ahrs_update_gps(void) {}
void ahrs_update_gps(void)
{
if (ahrs.update_gps != NULL) {
ahrs.update_gps();
}
}
sw/airborne/subsystems/ahrs.h
+10 -36
View File
@@ -28,61 +28,35 @@
#define AHRS_H
#include "std.h"
#include "math/pprz_algebra_int.h"
#include "math/pprz_algebra_float.h"
#include "state.h"
#define AHRS_UNINIT 0
#define AHRS_RUNNING 1
/* underlying includes (needed for parameters) */
#ifdef AHRS_TYPE_H
#include AHRS_TYPE_H
#endif
typedef void (*AhrsInit)(void);
typedef void (*AhrsUpdateGps)(void);
/** Attitude and Heading Reference System state */
struct Ahrs {
uint8_t status; ///< status of the AHRS, AHRS_UNINIT or AHRS_RUNNING
/* function pointers to actual implementation, set by ahrs_register_impl */
AhrsInit init;
AhrsUpdateGps update_gps;
};
/** global AHRS state */
extern struct Ahrs ahrs;
extern void ahrs_register_impl(AhrsInit init, AhrsUpdateGps update_gps);
/** AHRS initialization. Called at startup.
* Needs to be implemented by each AHRS algorithm.
* Initialized the global AHRS struct.
*/
extern void ahrs_init(void);
/** Aligns the AHRS. Called after ahrs_aligner has run to set initial attitude and biases.
* Must set the ahrs status to AHRS_RUNNING.
* Needs to be implemented by each AHRS algorithm.
*/
extern void ahrs_align(void);
/** Propagation. Usually integrates the gyro rates to angles.
* Reads the global #imu data struct.
* Does nothing if not implemented by specific AHRS algorithm.
* @param dt time difference since last propagation in seconds
*/
extern void ahrs_propagate(float dt);
/** Update AHRS state with accerleration measurements.
* Reads the global #imu data struct.
* Does nothing if not implemented by specific AHRS algorithm.
* @param dt time difference since last update in seconds
*/
extern void ahrs_update_accel(float dt);
/** Update AHRS state with magnetometer measurements.
* Reads the global #imu data struct.
* Does nothing if not implemented by specific AHRS algorithm.
* @param dt time difference since last update in seconds
*/
extern void ahrs_update_mag(float dt);
/** Update AHRS state with GPS measurements.
* Calls implementation if registered.
* Reads the global #gps data struct.
* Does nothing if not implemented by specific AHRS algorithm.
*/
extern void ahrs_update_gps(void);
sw/airborne/subsystems/ahrs/ahrs_aligner.c
+28 -19
View File
@@ -31,6 +31,8 @@
#include <stdlib.h> /* for abs() */
#include "subsystems/imu.h"
#include "led.h"
#include "subsystems/abi.h"
#include "mcu_periph/sys_time.h"
struct AhrsAligner ahrs_aligner;
@@ -45,21 +47,23 @@ static uint32_t samples_idx;
#if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h"
static void send_aligner(struct transport_tx *trans, struct link_device *dev) {
static void send_aligner(struct transport_tx *trans, struct link_device *dev)
{
pprz_msg_send_FILTER_ALIGNER(trans, dev, AC_ID,
&ahrs_aligner.lp_gyro.p,
&ahrs_aligner.lp_gyro.q,
&ahrs_aligner.lp_gyro.r,
&imu.gyro.p,
&imu.gyro.q,
&imu.gyro.r,
&ahrs_aligner.noise,
&ahrs_aligner.low_noise_cnt,
&ahrs_aligner.status);
&ahrs_aligner.lp_gyro.p,
&ahrs_aligner.lp_gyro.q,
&ahrs_aligner.lp_gyro.r,
&imu.gyro.p,
&imu.gyro.q,
&imu.gyro.r,
&ahrs_aligner.noise,
&ahrs_aligner.low_noise_cnt,
&ahrs_aligner.status);
}
#endif
void ahrs_aligner_init(void) {
void ahrs_aligner_init(void)
{
ahrs_aligner.status = AHRS_ALIGNER_RUNNING;
INT_RATES_ZERO(gyro_sum);
@@ -81,7 +85,8 @@ void ahrs_aligner_init(void) {
#define LOW_NOISE_TIME 5
#endif
void ahrs_aligner_run(void) {
void ahrs_aligner_run(void)
{
RATES_ADD(gyro_sum, imu.gyro);
VECT3_ADD(accel_sum, imu.accel);
@@ -96,15 +101,16 @@ void ahrs_aligner_run(void) {
if (samples_idx >= SAMPLES_NB) {
int32_t avg_ref_sensor = accel_sum.z;
if ( avg_ref_sensor >= 0)
if (avg_ref_sensor >= 0) {
avg_ref_sensor += SAMPLES_NB / 2;
else
} else {
avg_ref_sensor -= SAMPLES_NB / 2;
}
avg_ref_sensor /= SAMPLES_NB;
ahrs_aligner.noise = 0;
int i;
for (i=0; i<SAMPLES_NB; i++) {
for (i = 0; i < SAMPLES_NB; i++) {
int32_t diff = ref_sensor_samples[i] - avg_ref_sensor;
ahrs_aligner.noise += abs(diff);
}
@@ -118,17 +124,20 @@ void ahrs_aligner_run(void) {
INT_VECT3_ZERO(mag_sum);
samples_idx = 0;
if (ahrs_aligner.noise < LOW_NOISE_THRESHOLD)
if (ahrs_aligner.noise < LOW_NOISE_THRESHOLD) {
ahrs_aligner.low_noise_cnt++;
else
if ( ahrs_aligner.low_noise_cnt > 0)
ahrs_aligner.low_noise_cnt--;
} else if (ahrs_aligner.low_noise_cnt > 0) {
ahrs_aligner.low_noise_cnt--;
}
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
uint32_t now_ts = get_sys_time_usec();
AbiSendMsgIMU_LOWPASSED(ABI_BROADCAST, now_ts, &ahrs_aligner.lp_gyro,
&ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag);
}
}
sw/airborne/subsystems/ahrs/ahrs_ardrone2.c
+87 -84
View File
@@ -32,6 +32,7 @@
# include <stdio.h>
#endif
#include "subsystems/ahrs.h"
#include "ahrs_ardrone2.h"
#include "state.h"
#include "math/pprz_algebra_float.h"
@@ -42,79 +43,87 @@
#include "subsystems/gps/gps_ardrone2.h"
#endif
struct AhrsARDrone ahrs_impl;
struct AhrsAligner ahrs_aligner;
struct AhrsARDrone ahrs_ardrone2;
unsigned char buffer[4096]; //Packet buffer
#if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h"
static void send_ahrs_ad2(struct transport_tx *trans, struct link_device *dev) {
static void send_ahrs_ad2(struct transport_tx *trans, struct link_device *dev)
{
pprz_msg_send_AHRS_ARDRONE2(trans, dev, AC_ID,
&ahrs_impl.state,
&ahrs_impl.control_state,
&ahrs_impl.eulers.phi,
&ahrs_impl.eulers.theta,
&ahrs_impl.eulers.psi,
&ahrs_impl.speed.x,
&ahrs_impl.speed.y,
&ahrs_impl.speed.z,
&ahrs_impl.accel.x,
&ahrs_impl.accel.y,
&ahrs_impl.accel.z,
&ahrs_impl.altitude,
&ahrs_impl.battery);
&ahrs_ardrone2.state,
&ahrs_ardrone2.control_state,
&ahrs_ardrone2.eulers.phi,
&ahrs_ardrone2.eulers.theta,
&ahrs_ardrone2.eulers.psi,
&ahrs_ardrone2.speed.x,
&ahrs_ardrone2.speed.y,
&ahrs_ardrone2.speed.z,
&ahrs_ardrone2.accel.x,
&ahrs_ardrone2.accel.y,
&ahrs_ardrone2.accel.z,
&ahrs_ardrone2.altitude,
&ahrs_ardrone2.battery);
}
#endif
void ahrs_init(void) {
void ahrs_ardrone2_register(void)
{
ahrs_register_impl(ahrs_ardrone2_init, NULL);
}
void ahrs_ardrone2_init(void)
{
init_at_com();
//Set navdata_demo to FALSE and flat trim the ar drone
at_com_send_config("general:navdata_demo", "FALSE");
at_com_send_ftrim();
ahrs.status = AHRS_RUNNING;
ahrs_ardrone2.is_aligned = TRUE;
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "AHRS_ARDRONE2", send_ahrs_ad2);
#endif
}
void ahrs_align(void) {
}
#ifdef ARDRONE2_DEBUG
static void dump(const void *_b, size_t s) {
static void dump(const void *_b, size_t s)
{
const unsigned char *b = _b;
size_t n;
for(n = 0; n < s; ++n) {
for (n = 0; n < s; ++n) {
printf("%02x ", b[n]);
if (n%16 == 15)
if (n % 16 == 15) {
printf("\n");
}
}
if (n%16 != 0)
if (n % 16 != 0) {
printf("\n");
}
}
#endif
void ahrs_propagate(float dt __attribute__((unused))) {
void ahrs_ardrone2_propagate(void)
{
int l;
//Recieve the main packet
l = at_com_recieve_navdata(buffer);
navdata_t* main_packet = (navdata_t*) &buffer;
navdata_t *main_packet = (navdata_t *) &buffer;
#ifdef ARDRONE2_DEBUG
if (l < 0)
if (l < 0) {
printf("errno = %d\n", errno);
}
#endif
//When this isn't a valid packet return
if(l < 0 || main_packet->header != NAVDATA_HEADER)
if (l < 0 || main_packet->header != NAVDATA_HEADER) {
return;
}
#ifdef ARDRONE2_DEBUG
printf("Read %d\n", l);
@@ -122,81 +131,75 @@ void ahrs_propagate(float dt __attribute__((unused))) {
#endif
//Set the state
ahrs_impl.state = main_packet->ardrone_state;
ahrs_ardrone2.state = main_packet->ardrone_state;
//Init the option
navdata_option_t* navdata_option = (navdata_option_t*)&(main_packet->options[0]);
navdata_option_t *navdata_option = (navdata_option_t *) & (main_packet->options[0]);
bool_t full_read = FALSE;
//The possible packets
navdata_demo_t* navdata_demo;
navdata_gps_t* navdata_gps;
navdata_phys_measures_t* navdata_phys_measures;
navdata_demo_t *navdata_demo;
navdata_gps_t *navdata_gps;
navdata_phys_measures_t *navdata_phys_measures;
//Read the navdata until packet is fully readed
while(!full_read && navdata_option->size > 0) {
while (!full_read && navdata_option->size > 0) {
#ifdef ARDRONE2_DEBUG
printf ("tag = %d\n", navdata_option->tag);
printf("tag = %d\n", navdata_option->tag);
#endif
//Check the tag for the right option
switch(navdata_option->tag) {
case 0: //NAVDATA_DEMO
navdata_demo = (navdata_demo_t*) navdata_option;
switch (navdata_option->tag) {
case 0: //NAVDATA_DEMO
navdata_demo = (navdata_demo_t *) navdata_option;
//Set the AHRS state
ahrs_impl.control_state = navdata_demo->ctrl_state >> 16;
ahrs_impl.eulers.phi = navdata_demo->phi;
ahrs_impl.eulers.theta = navdata_demo->theta;
ahrs_impl.eulers.psi = navdata_demo->psi;
ahrs_impl.speed.x = navdata_demo->vx / 1000;
ahrs_impl.speed.y = navdata_demo->vy / 1000;
ahrs_impl.speed.z = navdata_demo->vz / 1000;
ahrs_impl.altitude = navdata_demo->altitude / 10;
ahrs_impl.battery = navdata_demo->vbat_flying_percentage;
//Set the AHRS state
ahrs_ardrone2.control_state = navdata_demo->ctrl_state >> 16;
ahrs_ardrone2.eulers.phi = navdata_demo->phi;
ahrs_ardrone2.eulers.theta = navdata_demo->theta;
ahrs_ardrone2.eulers.psi = navdata_demo->psi;
ahrs_ardrone2.speed.x = navdata_demo->vx / 1000;
ahrs_ardrone2.speed.y = navdata_demo->vy / 1000;
ahrs_ardrone2.speed.z = navdata_demo->vz / 1000;
ahrs_ardrone2.altitude = navdata_demo->altitude / 10;
ahrs_ardrone2.battery = navdata_demo->vbat_flying_percentage;
//Set the ned to body eulers
struct FloatEulers angles;
angles.theta = navdata_demo->theta/180000.*M_PI;
angles.psi = navdata_demo->psi/180000.*M_PI;
angles.phi = navdata_demo->phi/180000.*M_PI;
stateSetNedToBodyEulers_f(&angles);
//Set the ned to body eulers
struct FloatEulers angles;
angles.theta = navdata_demo->theta / 180000.*M_PI;
angles.psi = navdata_demo->psi / 180000.*M_PI;
angles.phi = navdata_demo->phi / 180000.*M_PI;
stateSetNedToBodyEulers_f(&angles);
//Update the electrical supply
electrical.vsupply = navdata_demo->vbat_flying_percentage;
break;
case 3: //NAVDATA_PHYS_MEASURES
navdata_phys_measures = (navdata_phys_measures_t*) navdata_option;
//Update the electrical supply
electrical.vsupply = navdata_demo->vbat_flying_percentage;
break;
case 3: //NAVDATA_PHYS_MEASURES
navdata_phys_measures = (navdata_phys_measures_t *) navdata_option;
//Set the AHRS accel state
INT32_VECT3_SCALE_2(ahrs_impl.accel, navdata_phys_measures->phys_accs, 9.81, 1000)
break;
//Set the AHRS accel state
INT32_VECT3_SCALE_2(ahrs_ardrone2.accel, navdata_phys_measures->phys_accs, 9.81, 1000)
break;
#ifdef USE_GPS_ARDRONE2
case 27: //NAVDATA_GPS
case 27: //NAVDATA_GPS
# ifdef ARDRONE2_DEBUG
dump(navdata_option, navdata_option->size);
dump(navdata_option, navdata_option->size);
# endif
navdata_gps = (navdata_gps_t*) navdata_option;
navdata_gps = (navdata_gps_t *) navdata_option;
// Send the data to the gps parser
gps_ardrone2_parse(navdata_gps);
break;
// Send the data to the gps parser
gps_ardrone2_parse(navdata_gps);
break;
#endif
case 0xFFFF: //CHECKSUM
//TODO: Check the checksum
full_read = TRUE;
break;
default:
case 0xFFFF: //CHECKSUM
//TODO: Check the checksum
full_read = TRUE;
break;
default:
#ifdef ARDRONE2_DEBUG
printf("NAVDATA UNKNOWN TAG: %d %d\n", navdata_option->tag, navdata_option->size);
printf("NAVDATA UNKNOWN TAG: %d %d\n", navdata_option->tag, navdata_option->size);
#endif
break;
break;
}
navdata_option = (navdata_option_t*) ((uint32_t)navdata_option + navdata_option->size);
navdata_option = (navdata_option_t *)((uint32_t)navdata_option + navdata_option->size);
}
}
void ahrs_aligner_init(void) {
}
void ahrs_aligner_run(void) {
}
sw/airborne/subsystems/ahrs/ahrs_ardrone2.h
+9 -3
View File
@@ -30,10 +30,9 @@
#ifndef AHRS_ARDRONE2_H
#define AHRS_ARDRONE2_H
#include "subsystems/ahrs.h"
#include "subsystems/ahrs/ahrs_aligner.h"
#include "std.h"
#include "math/pprz_algebra_int.h"
#include "math/pprz_geodetic_float.h"
struct AhrsARDrone {
uint32_t state; // ARDRONE_STATES
@@ -44,7 +43,14 @@ struct AhrsARDrone {
int32_t altitude; // in cm above ground
uint32_t battery; // in percentage
struct Int32Quat ltp_to_imu_quat;
bool_t is_aligned;
};
extern struct AhrsARDrone ahrs_impl;
extern struct AhrsARDrone ahrs_ardrone2;
#define DefaultAhrsImpl ahrs_ardrone2
extern void ahrs_ardrone2_register(void);
extern void ahrs_ardrone2_init(void);
extern void ahrs_ardrone2_propagate(void);
#endif /* AHRS_ARDRONE2_H */
sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c
+195 -212
View File
File diff suppressed because it is too large Load Diff
sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h
+28 -7
View File
@@ -27,10 +27,17 @@
* Propagation can be done in rotation matrix or quaternion representation.
*/
#ifndef AHRS_FLOAT_CMPL
#define AHRS_FLOAT_CMPL
#ifndef AHRS_FLOAT_CMPL_H
#define AHRS_FLOAT_CMPL_H
#include "std.h"
#include "math/pprz_algebra_float.h"
#include "math/pprz_orientation_conversion.h"
enum AhrsFCStatus {
AHRS_FC_UNINIT,
AHRS_FC_RUNNING
};
struct AhrsFloatCmpl {
struct FloatRates gyro_bias;
@@ -60,23 +67,37 @@ struct AhrsFloatCmpl {
/* internal counters for the gains */
uint16_t accel_cnt; ///< number of propagations since last accel update
uint16_t mag_cnt; ///< number of propagations since last mag update
struct OrientationReps body_to_imu;
enum AhrsFCStatus status;
bool_t is_aligned;
};
extern struct AhrsFloatCmpl ahrs_impl;
extern struct AhrsFloatCmpl ahrs_fc;
extern void ahrs_fc_init(void);
extern void ahrs_fc_set_body_to_imu(struct OrientationReps *body_to_imu);
extern void ahrs_fc_set_body_to_imu_quat(struct FloatQuat *q_b2i);
extern bool_t ahrs_fc_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
struct Int32Vect3 *lp_mag);
extern void ahrs_fc_propagate(struct Int32Rates *gyro, float dt);
extern void ahrs_fc_update_accel(struct Int32Vect3 *accel, float dt);
extern void ahrs_fc_update_mag(struct Int32Vect3 *mag, float dt);
extern void ahrs_fc_update_gps(void);
/** Update yaw based on a heading measurement.
* e.g. from GPS course
* @param heading Heading in body frame, radians (CW/north)
*/
void ahrs_update_heading(float heading);
void ahrs_fc_update_heading(float heading);
/** Hard reset yaw to a heading.
* Doesn't affect the bias.
* Sets ahrs_impl.heading_aligned to TRUE.
* Sets ahrs_fc.heading_aligned to TRUE.
* @param heading Heading in body frame, radians (CW/north)
*/
void ahrs_realign_heading(float heading);
void ahrs_fc_realign_heading(float heading);
#endif /* AHRS_FLOAT_CMPL_RMAT */
#endif /* AHRS_FLOAT_CMPL_H */
sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.c
+173
View File
@@ -0,0 +1,173 @@
/*
* Copyright (C) 2015 Felix Ruess <felix.ruess@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file subsystems/ahrs/ahrs_float_cmpl_wrapper.c
*
* Paparazzi specific wrapper to run floating point complementary filter.
*/
#include "subsystems/ahrs/ahrs_float_cmpl_wrapper.h"
#include "subsystems/ahrs.h"
#include "subsystems/abi.h"
#include "state.h"
#if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h"
static void send_att(struct transport_tx *trans, struct link_device *dev)
{
struct FloatEulers ltp_to_imu_euler;
float_eulers_of_quat(&ltp_to_imu_euler, &ahrs_fc.ltp_to_imu_quat);
struct Int32Eulers euler_i;
EULERS_BFP_OF_REAL(euler_i, ltp_to_imu_euler);
struct Int32Eulers *eulers_body = stateGetNedToBodyEulers_i();
pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID,
&euler_i.phi,
&euler_i.theta,
&euler_i.psi,
&(eulers_body->phi),
&(eulers_body->theta),
&(eulers_body->psi));
}
static void send_geo_mag(struct transport_tx *trans, struct link_device *dev)
{
pprz_msg_send_GEO_MAG(trans, dev, AC_ID,
&ahrs_fc.mag_h.x, &ahrs_fc.mag_h.y, &ahrs_fc.mag_h.z);
}
#endif
/** ABI binding for IMU data.
* Used for gyro, accel and mag ABI messages.
*/
#ifndef AHRS_FC_IMU_ID
#define AHRS_FC_IMU_ID ABI_BROADCAST
#endif
static abi_event gyro_ev;
static abi_event accel_ev;
static abi_event mag_ev;
static abi_event aligner_ev;
static abi_event body_to_imu_ev;
static void gyro_cb(uint8_t __attribute__((unused)) sender_id,
uint32_t __attribute__((unused)) stamp,
struct Int32Rates *gyro)
{
#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
PRINT_CONFIG_MSG("Calculating dt for AHRS_FC propagation.")
/* timestamp in usec when last callback was received */
static uint32_t last_stamp = 0;
if (last_stamp > 0 && ahrs_fc.is_aligned) {
float dt = (float)(stamp - last_stamp) * 1e-6;
ahrs_fc_propagate(gyro, dt);
}
last_stamp = stamp;
#else
PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS_FC propagation.")
PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
if (ahrs_fc.status == AHRS_FC_RUNNING) {
const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
ahrs_fc_propagate(gyro, dt);
}
#endif
}
static void accel_cb(uint8_t __attribute__((unused)) sender_id,
uint32_t __attribute__((unused)) stamp,
struct Int32Vect3 *accel)
{
#if USE_AUTO_AHRS_FREQ || !defined(AHRS_CORRECT_FREQUENCY)
PRINT_CONFIG_MSG("Calculating dt for AHRS float_cmpl accel update.")
static uint32_t last_stamp = 0;
if (last_stamp > 0 && ahrs_fc.is_aligned) {
float dt = (float)(stamp - last_stamp) * 1e-6;
ahrs_fc_update_accel((struct Int32Vect3 *)accel, dt);
}
last_stamp = stamp;
#else
PRINT_CONFIG_MSG("Using fixed AHRS_CORRECT_FREQUENCY for AHRS float_cmpl accel update.")
PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY)
if (ahrs_fc.is_aligned) {
const float dt = 1. / (AHRS_CORRECT_FREQUENCY);
ahrs_fc_update_accel((struct Int32Vect3 *)accel, dt);
}
#endif
}
static void mag_cb(uint8_t __attribute__((unused)) sender_id,
uint32_t __attribute__((unused)) stamp,
struct Int32Vect3 *mag)
{
#if USE_AUTO_AHRS_FREQ || !defined(AHRS_MAG_CORRECT_FREQUENCY)
PRINT_CONFIG_MSG("Calculating dt for AHRS float_cmpl mag update.")
static uint32_t last_stamp = 0;
if (last_stamp > 0 && ahrs_fc.is_aligned) {
float dt = (float)(stamp - last_stamp) * 1e-6;
ahrs_fc_update_mag(mag, dt);
}
last_stamp = stamp;
#else
PRINT_CONFIG_MSG("Using fixed AHRS_MAG_CORRECT_FREQUENCY for AHRS float_cmpl mag update.")
PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY)
if (ahrs_fc.is_aligned) {
const float dt = 1. / (AHRS_MAG_CORRECT_FREQUENCY);
ahrs_fc_update_mag(mag, dt);
}
#endif
}
static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
uint32_t stamp __attribute__((unused)),
struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
struct Int32Vect3 *lp_mag)
{
if (!ahrs_fc.is_aligned) {
ahrs_fc_align(lp_gyro, lp_accel, lp_mag);
}
}
static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
struct FloatQuat *q_b2i_f)
{
ahrs_fc_set_body_to_imu_quat(q_b2i_f);
}
void ahrs_fc_register(void)
{
ahrs_register_impl(ahrs_fc_init, ahrs_fc_update_gps);
/*
* Subscribe to scaled IMU measurements and attach callbacks
*/
AbiBindMsgIMU_GYRO_INT32(AHRS_FC_IMU_ID, &gyro_ev, gyro_cb);
AbiBindMsgIMU_ACCEL_INT32(AHRS_FC_IMU_ID, &accel_ev, accel_cb);
AbiBindMsgIMU_MAG_INT32(AHRS_FC_IMU_ID, &mag_ev, mag_cb);
AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb);
AbiBindMsgBODY_TO_IMU_QUAT(ABI_BROADCAST, &body_to_imu_ev, body_to_imu_cb);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "AHRS_EULER_INT", send_att);
register_periodic_telemetry(DefaultPeriodic, "GEO_MAG", send_geo_mag);
#endif
}
sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.h
+36
View File
@@ -0,0 +1,36 @@
/*
* Copyright (C) 2015 Felix Ruess <felix.ruess@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file subsystems/ahrs/ahrs_float_cmpl_wrapper.h
*
* Paparazzi specific wrapper to run floating point complementary filter.
*/
#ifndef AHRS_FLOAT_CMPL_WRAPPER_H
#define AHRS_FLOAT_CMPL_WRAPPER_H
#include "subsystems/ahrs/ahrs_float_cmpl.h"
#define DefaultAhrsImpl ahrs_fc
extern void ahrs_fc_register(void);
#endif /* AHRS_FLOAT_CMPL_WRAPPER_H */
sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
+212 -189
View File
File diff suppressed because it is too large Load Diff
sw/airborne/subsystems/ahrs/ahrs_float_dcm.h
+22 -2
View File
@@ -26,6 +26,12 @@
#include <inttypes.h>
#include "math/pprz_algebra_float.h"
#include "math/pprz_orientation_conversion.h"
enum AhrsDCMStatus {
AHRS_DCM_UNINIT,
AHRS_DCM_RUNNING
};
struct AhrsFloatDCM {
struct FloatRates gyro_bias;
@@ -39,9 +45,13 @@ struct AhrsFloatDCM {
float gps_course;
bool_t gps_course_valid;
uint8_t gps_age;
};
extern struct AhrsFloatDCM ahrs_impl;
struct OrientationReps body_to_imu;
enum AhrsDCMStatus status;
bool_t is_aligned;
};
extern struct AhrsFloatDCM ahrs_dcm;
// DCM Parameters
@@ -69,4 +79,14 @@ extern int renorm_blowup_count;
extern float imu_health;
#endif
extern void ahrs_dcm_init(void);
extern void ahrs_dcm_set_body_to_imu(struct OrientationReps *body_to_imu);
extern void ahrs_dcm_set_body_to_imu_quat(struct FloatQuat *q_b2i);
extern bool_t ahrs_dcm_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
struct Int32Vect3 *lp_mag);
extern void ahrs_dcm_propagate(struct Int32Rates *gyro, float dt);
extern void ahrs_dcm_update_accel(struct Int32Vect3 *accel);
extern void ahrs_dcm_update_mag(struct Int32Vect3 *mag);
extern void ahrs_dcm_update_gps(void);
#endif // AHRS_FLOAT_DCM_H
sw/airborne/subsystems/ahrs/ahrs_float_dcm_algebra.h
+23 -26
View File
@@ -21,34 +21,34 @@
#define AHRS_FLOAT_DCM_ALGEBRA_H
static inline float Vector_Dot_Product(float vector1[3],float vector2[3])
static inline float Vector_Dot_Product(float vector1[3], float vector2[3])
{
return vector1[0]*vector2[0] + vector1[1]*vector2[1] + vector1[2]*vector2[2];
return vector1[0] * vector2[0] + vector1[1] * vector2[1] + vector1[2] * vector2[2];
}
static inline void Vector_Cross_Product(float vectorOut[3], float v1[3],float v2[3])
static inline void Vector_Cross_Product(float vectorOut[3], float v1[3], float v2[3])
{
vectorOut[0]= (v1[1]*v2[2]) - (v1[2]*v2[1]);
vectorOut[1]= (v1[2]*v2[0]) - (v1[0]*v2[2]);
vectorOut[2]= (v1[0]*v2[1]) - (v1[1]*v2[0]);
vectorOut[0] = (v1[1] * v2[2]) - (v1[2] * v2[1]);
vectorOut[1] = (v1[2] * v2[0]) - (v1[0] * v2[2]);
vectorOut[2] = (v1[0] * v2[1]) - (v1[1] * v2[0]);
}
static inline void Vector_Scale(float vectorOut[3],float vectorIn[3], float scale2)
static inline void Vector_Scale(float vectorOut[3], float vectorIn[3], float scale2)
{
vectorOut[0]=vectorIn[0]*scale2;
vectorOut[1]=vectorIn[1]*scale2;
vectorOut[2]=vectorIn[2]*scale2;
vectorOut[0] = vectorIn[0] * scale2;
vectorOut[1] = vectorIn[1] * scale2;
vectorOut[2] = vectorIn[2] * scale2;
}
static inline void Vector_Add(float vectorOut[3],float vectorIn1[3], float vectorIn2[3])
static inline void Vector_Add(float vectorOut[3], float vectorIn1[3], float vectorIn2[3])
{
vectorOut[0]=vectorIn1[0]+vectorIn2[0];
vectorOut[1]=vectorIn1[1]+vectorIn2[1];
vectorOut[2]=vectorIn1[2]+vectorIn2[2];
vectorOut[0] = vectorIn1[0] + vectorIn2[0];
vectorOut[1] = vectorIn1[1] + vectorIn2[1];
vectorOut[2] = vectorIn1[2] + vectorIn2[2];
}
/*
#define Matrix_Multiply( _m_a2b, _m_b2c, _m_a2c) { \
#define Matrix_Multiply( _m_a2b, _m_b2c, _m_a2c) { \
_m_a2c[0] = (_m_b2c[0]*_m_a2b[0] + _m_b2c[1]*_m_a2b[3] + _m_b2c[2]*_m_a2b[6]); \
_m_a2c[1] = (_m_b2c[0]*_m_a2b[1] + _m_b2c[1]*_m_a2b[4] + _m_b2c[2]*_m_a2b[7]); \
_m_a2c[2] = (_m_b2c[0]*_m_a2b[2] + _m_b2c[1]*_m_a2b[5] + _m_b2c[2]*_m_a2b[8]); \
@@ -61,20 +61,17 @@ static inline void Vector_Add(float vectorOut[3],float vectorIn1[3], float vecto
}
*/
static inline void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3])
static inline void Matrix_Multiply(float a[3][3], float b[3][3], float mat[3][3])
{
float op[3];
for(int x=0; x<3; x++)
{
for(int y=0; y<3; y++)
{
for(int w=0; w<3; w++)
{
op[w]=a[x][w]*b[w][y];
}
mat[x][y]=op[0]+op[1]+op[2];
}
for (int x = 0; x < 3; x++) {
for (int y = 0; y < 3; y++) {
for (int w = 0; w < 3; w++) {
op[w] = a[x][w] * b[w][y];
}
mat[x][y] = op[0] + op[1] + op[2];
}
}
}
#endif // AHRS_FLOAT_DCM_ALGEBRA_H
sw/airborne/subsystems/ahrs/ahrs_float_dcm_wrapper.c
+114
View File
@@ -0,0 +1,114 @@
/*
* Copyright (C) 2015 Felix Ruess <felix.ruess@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file subsystems/ahrs/ahrs_float_dcm_wrapper.c
*
* Paparazzi specific wrapper to run floating point complementary filter.
*/
#include "subsystems/ahrs/ahrs_float_dcm_wrapper.h"
#include "subsystems/ahrs.h"
#include "subsystems/abi.h"
#include "state.h"
/** ABI binding for IMU data.
* Used for gyro, accel and mag ABI messages.
*/
#ifndef AHRS_DCM_IMU_ID
#define AHRS_DCM_IMU_ID ABI_BROADCAST
#endif
static abi_event gyro_ev;
static abi_event accel_ev;
static abi_event mag_ev;
static abi_event aligner_ev;
static abi_event body_to_imu_ev;
static void gyro_cb(uint8_t __attribute__((unused)) sender_id,
uint32_t __attribute__((unused)) stamp,
struct Int32Rates *gyro)
{
#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
PRINT_CONFIG_MSG("Calculating dt for AHRS dcm propagation.")
/* timestamp in usec when last callback was received */
static uint32_t last_stamp = 0;
if (last_stamp > 0 && ahrs_dcm.is_aligned) {
float dt = (float)(stamp - last_stamp) * 1e-6;
ahrs_dcm_propagate(gyro, dt);
}
last_stamp = stamp;
#else
PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS dcm propagation.")
PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
if (ahrs_dcm.is_aligned) {
const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
ahrs_dcm_propagate(gyro, dt);
}
#endif
}
static void accel_cb(uint8_t sender_id __attribute__((unused)),
uint32_t stamp __attribute__((unused)),
struct Int32Vect3 *accel)
{
if (ahrs_dcm.is_aligned) {
ahrs_dcm_update_accel(accel);
}
}
static void mag_cb(uint8_t sender_id __attribute__((unused)),
uint32_t stamp __attribute__((unused)),
struct Int32Vect3 *mag)
{
if (ahrs_dcm.is_aligned) {
ahrs_dcm_update_mag(mag);
}
}
static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
uint32_t stamp __attribute__((unused)),
struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
struct Int32Vect3 *lp_mag)
{
if (!ahrs_dcm.is_aligned) {
ahrs_dcm_align(lp_gyro, lp_accel, lp_mag);
}
}
static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
struct FloatQuat *q_b2i_f)
{
ahrs_dcm_set_body_to_imu_quat(q_b2i_f);
}
void ahrs_dcm_register(void)
{
ahrs_register_impl(ahrs_dcm_init, ahrs_dcm_update_gps);
/*
* Subscribe to scaled IMU measurements and attach callbacks
*/
AbiBindMsgIMU_GYRO_INT32(AHRS_DCM_IMU_ID, &gyro_ev, gyro_cb);
AbiBindMsgIMU_ACCEL_INT32(AHRS_DCM_IMU_ID, &accel_ev, accel_cb);
AbiBindMsgIMU_MAG_INT32(AHRS_DCM_IMU_ID, &mag_ev, mag_cb);
AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb);
AbiBindMsgBODY_TO_IMU_QUAT(ABI_BROADCAST, &body_to_imu_ev, body_to_imu_cb);
}
sw/airborne/subsystems/ahrs/ahrs_float_dcm_wrapper.h
+36
View File
@@ -0,0 +1,36 @@
/*
* Copyright (C) 2015 Felix Ruess <felix.ruess@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file subsystems/ahrs/ahrs_float_dcm_wrapper.h
*
* Paparazzi specific wrapper to run floating point DCM filter.
*/
#ifndef AHRS_FLOAT_DCM_WRAPPER_H
#define AHRS_FLOAT_DCM_WRAPPER_H
#include "subsystems/ahrs/ahrs_float_dcm.h"
#define DefaultAhrsImpl ahrs_dcm
extern void ahrs_dcm_register(void);
#endif /* AHRS_FLOAT_DCM_WRAPPER_H */
sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c
+218 -105
View File
@@ -29,16 +29,12 @@
*/
#include "subsystems/ahrs/ahrs_float_mlkf.h"
#include "subsystems/ahrs/ahrs_aligner.h"
#include "subsystems/ahrs/ahrs_float_utils.h"
#include <float.h> /* for FLT_MIN */
#include <string.h> /* for memcpy */
#include <math.h> /* for M_PI */
#include "state.h"
#include "subsystems/imu.h"
#include "math/pprz_algebra_float.h"
#include "math/pprz_algebra_int.h"
#include "math/pprz_simple_matrix.h"
@@ -52,119 +48,153 @@
#define AHRS_MAG_NOISE_Z 0.2
#endif
static inline void propagate_ref(float dt);
static inline void propagate_ref(struct Int32Rates *gyro, float dt);
static inline void propagate_state(float dt);
static inline void update_state(const struct FloatVect3 *i_expected, struct FloatVect3* b_measured, struct FloatVect3* noise);
static inline void update_state(const struct FloatVect3 *i_expected,
struct FloatVect3 *b_measured,
struct FloatVect3 *noise);
static inline void update_state_heading(const struct FloatVect3 *i_expected,
struct FloatVect3 *b_measured,
struct FloatVect3 *noise);
static inline void reset_state(void);
static inline void set_body_state_from_quat(void);
struct AhrsMlkf ahrs_impl;
struct AhrsMlkf ahrs_mlkf;
#if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h"
static void send_geo_mag(struct transport_tx *trans, struct link_device *dev) {
pprz_msg_send_GEO_MAG(trans, dev, AC_ID,
&ahrs_impl.mag_h.x, &ahrs_impl.mag_h.y, &ahrs_impl.mag_h.z);
}
#endif
void ahrs_mlkf_init(void)
{
void ahrs_init(void) {
ahrs_mlkf.is_aligned = FALSE;
ahrs.status = AHRS_UNINIT;
/* init ltp_to_imu quaternion as zero/identity rotation */
float_quat_identity(&ahrs_mlkf.ltp_to_imu_quat);
FLOAT_RATES_ZERO(ahrs_mlkf.imu_rate);
/* Set ltp_to_imu so that body is zero */
memcpy(&ahrs_impl.ltp_to_imu_quat, orientationGetQuat_f(&imu.body_to_imu),
sizeof(struct FloatQuat));
FLOAT_RATES_ZERO(ahrs_impl.imu_rate);
VECT3_ASSIGN(ahrs_impl.mag_h, AHRS_H_X, AHRS_H_Y, AHRS_H_Z);
VECT3_ASSIGN(ahrs_mlkf.mag_h, AHRS_H_X, AHRS_H_Y, AHRS_H_Z);
/*
* Initialises our state
*/
FLOAT_RATES_ZERO(ahrs_impl.gyro_bias);
FLOAT_RATES_ZERO(ahrs_mlkf.gyro_bias);
const float P0_a = 1.;
const float P0_b = 1e-4;
float P0[6][6] = {{ P0_a, 0., 0., 0., 0., 0. },
{ 0., P0_a, 0., 0., 0., 0. },
{ 0., 0., P0_a, 0., 0., 0. },
{ 0., 0., 0., P0_b, 0., 0. },
{ 0., 0., 0., 0., P0_b, 0. },
{ 0., 0., 0., 0., 0., P0_b}};
memcpy(ahrs_impl.P, P0, sizeof(P0));
{ 0., P0_a, 0., 0., 0., 0. },
{ 0., 0., P0_a, 0., 0., 0. },
{ 0., 0., 0., P0_b, 0., 0. },
{ 0., 0., 0., 0., P0_b, 0. },
{ 0., 0., 0., 0., 0., P0_b}
};
memcpy(ahrs_mlkf.P, P0, sizeof(P0));
VECT3_ASSIGN(ahrs_impl.mag_noise, AHRS_MAG_NOISE_X, AHRS_MAG_NOISE_Y, AHRS_MAG_NOISE_Z);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "GEO_MAG", send_geo_mag);
#endif
VECT3_ASSIGN(ahrs_mlkf.mag_noise, AHRS_MAG_NOISE_X, AHRS_MAG_NOISE_Y, AHRS_MAG_NOISE_Z);
}
void ahrs_align(void) {
void ahrs_mlkf_set_body_to_imu(struct OrientationReps *body_to_imu)
{
ahrs_mlkf_set_body_to_imu_quat(orientationGetQuat_f(body_to_imu));
}
void ahrs_mlkf_set_body_to_imu_quat(struct FloatQuat *q_b2i)
{
orientationSetQuat_f(&ahrs_mlkf.body_to_imu, q_b2i);
if (!ahrs_mlkf.is_aligned) {
/* Set ltp_to_imu so that body is zero */
memcpy(&ahrs_mlkf.ltp_to_imu_quat, orientationGetQuat_f(&ahrs_mlkf.body_to_imu),
sizeof(struct FloatQuat));
}
}
bool_t ahrs_mlkf_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
struct Int32Vect3 *lp_mag)
{
/* Compute an initial orientation from accel and mag directly as quaternion */
ahrs_float_get_quat_from_accel_mag(&ahrs_impl.ltp_to_imu_quat, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag);
ahrs_float_get_quat_from_accel_mag(&ahrs_mlkf.ltp_to_imu_quat, lp_accel, lp_mag);
/* set initial body orientation */
set_body_state_from_quat();
/* used averaged gyro as initial value for bias */
struct Int32Rates bias0;
RATES_COPY(bias0, ahrs_aligner.lp_gyro);
RATES_FLOAT_OF_BFP(ahrs_impl.gyro_bias, bias0);
RATES_COPY(bias0, *lp_gyro);
RATES_FLOAT_OF_BFP(ahrs_mlkf.gyro_bias, bias0);
ahrs.status = AHRS_RUNNING;
ahrs_mlkf.is_aligned = TRUE;
return TRUE;
}
void ahrs_propagate(float dt) {
propagate_ref(dt);
void ahrs_mlkf_propagate(struct Int32Rates *gyro, float dt)
{
propagate_ref(gyro, dt);
propagate_state(dt);
set_body_state_from_quat();
}
void ahrs_update_accel(float dt __attribute__((unused))) {
void ahrs_mlkf_update_accel(struct Int32Vect3 *accel)
{
struct FloatVect3 imu_g;
ACCELS_FLOAT_OF_BFP(imu_g, imu.accel);
ACCELS_FLOAT_OF_BFP(imu_g, *accel);
const float alpha = 0.92;
ahrs_impl.lp_accel = alpha * ahrs_impl.lp_accel +
(1. - alpha) *(float_vect3_norm(&imu_g) - 9.81);
ahrs_mlkf.lp_accel = alpha * ahrs_mlkf.lp_accel +
(1. - alpha) * (float_vect3_norm(&imu_g) - 9.81);
const struct FloatVect3 earth_g = {0., 0., -9.81 };
const float dn = 250*fabs( ahrs_impl.lp_accel );
struct FloatVect3 g_noise = {1.+dn, 1.+dn, 1.+dn};
const float dn = 250 * fabs(ahrs_mlkf.lp_accel);
struct FloatVect3 g_noise = {1. + dn, 1. + dn, 1. + dn};
update_state(&earth_g, &imu_g, &g_noise);
reset_state();
}
void ahrs_mlkf_update_mag(struct Int32Vect3 *mag)
{
#if AHRS_MAG_UPDATE_ALL_AXES
ahrs_mlkf_update_mag_full(mag);
#else
ahrs_mlkf_update_mag_2d(mag);
#endif
}
void ahrs_update_mag(float dt __attribute__((unused))) {
void ahrs_mlkf_update_mag_2d(struct Int32Vect3 *mag)
{
struct FloatVect3 imu_h;
MAGS_FLOAT_OF_BFP(imu_h, imu.mag);
update_state(&ahrs_impl.mag_h, &imu_h, &ahrs_impl.mag_noise);
MAGS_FLOAT_OF_BFP(imu_h, *mag);
update_state_heading(&ahrs_mlkf.mag_h, &imu_h, &ahrs_mlkf.mag_noise);
reset_state();
}
void ahrs_mlkf_update_mag_full(struct Int32Vect3 *mag)
{
struct FloatVect3 imu_h;
MAGS_FLOAT_OF_BFP(imu_h, *mag);
update_state(&ahrs_mlkf.mag_h, &imu_h, &ahrs_mlkf.mag_noise);
reset_state();
}
static inline void propagate_ref(float dt) {
static inline void propagate_ref(struct Int32Rates *gyro, float dt)
{
/* converts gyro to floating point */
struct FloatRates gyro_float;
RATES_FLOAT_OF_BFP(gyro_float, imu.gyro_prev);
RATES_FLOAT_OF_BFP(gyro_float, *gyro);
/* unbias measurement */
RATES_SUB(gyro_float, ahrs_impl.gyro_bias);
RATES_SUB(gyro_float, ahrs_mlkf.gyro_bias);
#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
/* lowpass angular rates */
const float alpha = 0.1;
FLOAT_RATES_LIN_CMB(ahrs_impl.imu_rate, ahrs_impl.imu_rate,
(1.-alpha), gyro_float, alpha);
FLOAT_RATES_LIN_CMB(ahrs_mlkf.imu_rate, ahrs_mlkf.imu_rate,
(1. - alpha), gyro_float, alpha);
#else
RATES_COPY(ahrs_impl.imu_rate, gyro_float);
RATES_COPY(ahrs_mlkf.imu_rate, gyro_float);
#endif
/* propagate reference quaternion */
float_quat_integrate(&ahrs_impl.ltp_to_imu_quat, &ahrs_impl.imu_rate, dt);
float_quat_integrate(&ahrs_mlkf.ltp_to_imu_quat, &ahrs_mlkf.imu_rate, dt);
}
@@ -172,48 +202,57 @@ static inline void propagate_ref(float dt) {
* Progagate filter's covariance
* We don't propagate state as we assume to have reseted
*/
static inline void propagate_state(float dt) {
static inline void propagate_state(float dt)
{
/* predict covariance */
const float dp = ahrs_impl.imu_rate.p*dt;
const float dq = ahrs_impl.imu_rate.q*dt;
const float dr = ahrs_impl.imu_rate.r*dt;
const float dp = ahrs_mlkf.imu_rate.p * dt;
const float dq = ahrs_mlkf.imu_rate.q * dt;
const float dr = ahrs_mlkf.imu_rate.r * dt;
float F[6][6] = {{ 1., dr, -dq, -dt, 0., 0. },
{ -dr, 1., dp, 0., -dt, 0. },
{ dq, -dp, 1., 0., 0., -dt },
{ 0., 0., 0., 1., 0., 0. },
{ 0., 0., 0., 0., 1., 0. },
{ 0., 0., 0., 0., 0., 1. }};
{ -dr, 1., dp, 0., -dt, 0. },
{ dq, -dp, 1., 0., 0., -dt },
{ 0., 0., 0., 1., 0., 0. },
{ 0., 0., 0., 0., 1., 0. },
{ 0., 0., 0., 0., 0., 1. }
};
// P = FPF' + GQG
float tmp[6][6];
MAT_MUL(6,6,6, tmp, F, ahrs_impl.P);
MAT_MUL_T(6,6,6, ahrs_impl.P, tmp, F);
MAT_MUL(6, 6, 6, tmp, F, ahrs_mlkf.P);
MAT_MUL_T(6, 6, 6, ahrs_mlkf.P, tmp, F);
const float dt2 = dt * dt;
const float GQG[6] = {dt2*10e-3, dt2*10e-3, dt2*10e-3, dt2*9e-6, dt2*9e-6, dt2*9e-6 };
for (int i=0;i<6;i++)
ahrs_impl.P[i][i] += GQG[i];
const float GQG[6] = {dt2 * 10e-3, dt2 * 10e-3, dt2 * 10e-3, dt2 * 9e-6, dt2 * 9e-6, dt2 * 9e-6 };
for (int i = 0; i < 6; i++) {
ahrs_mlkf.P[i][i] += GQG[i];
}
}
/**
* Incorporate one 3D vector measurement
* Incorporate one 3D vector measurement.
* @param i_expected expected 3d vector in inertial frame
* @param b_measured measured 3d vector in body/imu frame
* @param noise measurement noise vector (diagonal of covariance)
*/
static inline void update_state(const struct FloatVect3 *i_expected, struct FloatVect3* b_measured, struct FloatVect3* noise) {
static inline void update_state(const struct FloatVect3 *i_expected, struct FloatVect3 *b_measured,
struct FloatVect3 *noise)
{
/* converted expected measurement from inertial to body frame */
struct FloatVect3 b_expected;
float_quat_vmult(&b_expected, &ahrs_impl.ltp_to_imu_quat, (struct FloatVect3*)i_expected);
float_quat_vmult(&b_expected, &ahrs_mlkf.ltp_to_imu_quat, (struct FloatVect3 *)i_expected);
// S = HPH' + JRJ
float H[3][6] = {{ 0., -b_expected.z, b_expected.y, 0., 0., 0.},
{ b_expected.z, 0., -b_expected.x, 0., 0., 0.},
{-b_expected.y, b_expected.x, 0., 0., 0., 0.}};
{ -b_expected.y, b_expected.x, 0., 0., 0., 0.}
};
float tmp[3][6];
MAT_MUL(3,6,6, tmp, H, ahrs_impl.P);
MAT_MUL(3, 6, 6, tmp, H, ahrs_mlkf.P);
float S[3][3];
MAT_MUL_T(3,6,3, S, tmp, H);
MAT_MUL_T(3, 6, 3, S, tmp, H);
/* add the measurement noise */
S[0][0] += noise->x;
@@ -225,68 +264,142 @@ static inline void update_state(const struct FloatVect3 *i_expected, struct Floa
// K = PH'invS
float tmp2[6][3];
MAT_MUL_T(6,6,3, tmp2, ahrs_impl.P, H);
MAT_MUL_T(6, 6, 3, tmp2, ahrs_mlkf.P, H);
float K[6][3];
MAT_MUL(6,3,3, K, tmp2, invS);
MAT_MUL(6, 3, 3, K, tmp2, invS);
// P = (I-KH)P
float tmp3[6][6];
MAT_MUL(6,3,6, tmp3, K, H);
MAT_MUL(6, 3, 6, tmp3, K, H);
float I6[6][6] = {{ 1., 0., 0., 0., 0., 0. },
{ 0., 1., 0., 0., 0., 0. },
{ 0., 0., 1., 0., 0., 0. },
{ 0., 0., 0., 1., 0., 0. },
{ 0., 0., 0., 0., 1., 0. },
{ 0., 0., 0., 0., 0., 1. }};
{ 0., 1., 0., 0., 0., 0. },
{ 0., 0., 1., 0., 0., 0. },
{ 0., 0., 0., 1., 0., 0. },
{ 0., 0., 0., 0., 1., 0. },
{ 0., 0., 0., 0., 0., 1. }
};
float tmp4[6][6];
MAT_SUB(6,6, tmp4, I6, tmp3);
MAT_SUB(6, 6, tmp4, I6, tmp3);
float tmp5[6][6];
MAT_MUL(6,6,6, tmp5, tmp4, ahrs_impl.P);
memcpy(ahrs_impl.P, tmp5, sizeof(ahrs_impl.P));
MAT_MUL(6, 6, 6, tmp5, tmp4, ahrs_mlkf.P);
memcpy(ahrs_mlkf.P, tmp5, sizeof(ahrs_mlkf.P));
// X = X + Ke
struct FloatVect3 e;
VECT3_DIFF(e, *b_measured, b_expected);
ahrs_impl.gibbs_cor.qx += K[0][0]*e.x + K[0][1]*e.y + K[0][2]*e.z;
ahrs_impl.gibbs_cor.qy += K[1][0]*e.x + K[1][1]*e.y + K[1][2]*e.z;
ahrs_impl.gibbs_cor.qz += K[2][0]*e.x + K[2][1]*e.y + K[2][2]*e.z;
ahrs_impl.gyro_bias.p += K[3][0]*e.x + K[3][1]*e.y + K[3][2]*e.z;
ahrs_impl.gyro_bias.q += K[4][0]*e.x + K[4][1]*e.y + K[4][2]*e.z;
ahrs_impl.gyro_bias.r += K[5][0]*e.x + K[5][1]*e.y + K[5][2]*e.z;
ahrs_mlkf.gibbs_cor.qx += K[0][0] * e.x + K[0][1] * e.y + K[0][2] * e.z;
ahrs_mlkf.gibbs_cor.qy += K[1][0] * e.x + K[1][1] * e.y + K[1][2] * e.z;
ahrs_mlkf.gibbs_cor.qz += K[2][0] * e.x + K[2][1] * e.y + K[2][2] * e.z;
ahrs_mlkf.gyro_bias.p += K[3][0] * e.x + K[3][1] * e.y + K[3][2] * e.z;
ahrs_mlkf.gyro_bias.q += K[4][0] * e.x + K[4][1] * e.y + K[4][2] * e.z;
ahrs_mlkf.gyro_bias.r += K[5][0] * e.x + K[5][1] * e.y + K[5][2] * e.z;
}
/**
* Incorporate one 3D vector measurement, only correcting heading.
* @param i_expected expected 3d vector in inertial frame
* @param b_measured measured 3d vector in body/imu frame
* @param noise measurement noise vector (diagonal of covariance)
* TODO: optimize
*/
static inline void update_state_heading(const struct FloatVect3 *i_expected,
struct FloatVect3 *b_measured,
struct FloatVect3 *noise)
{
/* converted expected measurement from inertial to body frame */
struct FloatVect3 b_expected;
float_quat_vmult(&b_expected, &ahrs_mlkf.ltp_to_imu_quat, (struct FloatVect3 *)i_expected);
/* set roll/pitch errors to zero to only correct heading */
struct FloatVect3 i_h_2d = {i_expected->y, -i_expected->x, 0.f};
struct FloatVect3 b_yaw;
float_quat_vmult(&b_yaw, &ahrs_mlkf.ltp_to_imu_quat, &i_h_2d);
// S = HPH' + JRJ
float H[3][6] = {{ 0., 0., b_yaw.x, 0., 0., 0.},
{ 0., 0., b_yaw.y, 0., 0., 0.},
{ 0., 0., b_yaw.z, 0., 0., 0.}
};
float tmp[3][6];
MAT_MUL(3, 6, 6, tmp, H, ahrs_mlkf.P);
float S[3][3];
MAT_MUL_T(3, 6, 3, S, tmp, H);
/* add the measurement noise */
S[0][0] += noise->x;
S[1][1] += noise->y;
S[2][2] += noise->z;
float invS[3][3];
MAT_INV33(invS, S);
// K = PH'invS
float tmp2[6][3];
MAT_MUL_T(6, 6, 3, tmp2, ahrs_mlkf.P, H);
float K[6][3];
MAT_MUL(6, 3, 3, K, tmp2, invS);
// P = (I-KH)P
float tmp3[6][6];
MAT_MUL(6, 3, 6, tmp3, K, H);
float I6[6][6] = {{ 1., 0., 0., 0., 0., 0. },
{ 0., 1., 0., 0., 0., 0. },
{ 0., 0., 1., 0., 0., 0. },
{ 0., 0., 0., 1., 0., 0. },
{ 0., 0., 0., 0., 1., 0. },
{ 0., 0., 0., 0., 0., 1. }
};
float tmp4[6][6];
MAT_SUB(6, 6, tmp4, I6, tmp3);
float tmp5[6][6];
MAT_MUL(6, 6, 6, tmp5, tmp4, ahrs_mlkf.P);
memcpy(ahrs_mlkf.P, tmp5, sizeof(ahrs_mlkf.P));
// X = X + Ke
struct FloatVect3 e;
VECT3_DIFF(e, *b_measured, b_expected);
ahrs_mlkf.gibbs_cor.qx += K[0][0] * e.x + K[0][1] * e.y + K[0][2] * e.z;
ahrs_mlkf.gibbs_cor.qy += K[1][0] * e.x + K[1][1] * e.y + K[1][2] * e.z;
ahrs_mlkf.gibbs_cor.qz += K[2][0] * e.x + K[2][1] * e.y + K[2][2] * e.z;
ahrs_mlkf.gyro_bias.p += K[3][0] * e.x + K[3][1] * e.y + K[3][2] * e.z;
ahrs_mlkf.gyro_bias.q += K[4][0] * e.x + K[4][1] * e.y + K[4][2] * e.z;
ahrs_mlkf.gyro_bias.r += K[5][0] * e.x + K[5][1] * e.y + K[5][2] * e.z;
}
/**
* Incorporate errors to reference and zeros state
*/
static inline void reset_state(void) {
static inline void reset_state(void)
{
ahrs_impl.gibbs_cor.qi = 2.;
ahrs_mlkf.gibbs_cor.qi = 2.;
struct FloatQuat q_tmp;
float_quat_comp(&q_tmp, &ahrs_impl.ltp_to_imu_quat, &ahrs_impl.gibbs_cor);
float_quat_comp(&q_tmp, &ahrs_mlkf.ltp_to_imu_quat, &ahrs_mlkf.gibbs_cor);
float_quat_normalize(&q_tmp);
memcpy(&ahrs_impl.ltp_to_imu_quat, &q_tmp, sizeof(ahrs_impl.ltp_to_imu_quat));
float_quat_identity(&ahrs_impl.gibbs_cor);
memcpy(&ahrs_mlkf.ltp_to_imu_quat, &q_tmp, sizeof(ahrs_mlkf.ltp_to_imu_quat));
float_quat_identity(&ahrs_mlkf.gibbs_cor);
}
/**
* Compute body orientation and rates from imu orientation and rates
*/
static inline void set_body_state_from_quat(void) {
struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&imu.body_to_imu);
struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&imu.body_to_imu);
static inline void set_body_state_from_quat(void)
{
struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_mlkf.body_to_imu);
struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_mlkf.body_to_imu);
/* Compute LTP to BODY quaternion */
struct FloatQuat ltp_to_body_quat;
float_quat_comp_inv(&ltp_to_body_quat, &ahrs_impl.ltp_to_imu_quat, body_to_imu_quat);
float_quat_comp_inv(&ltp_to_body_quat, &ahrs_mlkf.ltp_to_imu_quat, body_to_imu_quat);
/* Set in state interface */
stateSetNedToBodyQuat_f(&ltp_to_body_quat);
/* compute body rates */
struct FloatRates body_rate;
float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_impl.imu_rate);
float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_mlkf.imu_rate);
/* Set state */
stateSetBodyRates_f(&body_rate);
sw/airborne/subsystems/ahrs/ahrs_float_mlkf.h
+24 -2
View File
@@ -31,9 +31,14 @@
#ifndef AHRS_FLOAT_MLKF_H
#define AHRS_FLOAT_MLKF_H
#include "subsystems/ahrs.h"
#include "std.h"
#include "math/pprz_algebra_float.h"
#include "math/pprz_orientation_conversion.h"
enum AhrsMlkfStatus {
AHRS_MLKF_UNINIT,
AHRS_MLKF_RUNNING
};
struct AhrsMlkf {
struct FloatQuat ltp_to_imu_quat; ///< Rotation from LocalTangentPlane to IMU frame as unit quaternion
@@ -48,8 +53,25 @@ struct AhrsMlkf {
struct FloatQuat gibbs_cor;
float P[6][6];
float lp_accel;
/** body_to_imu rotation */
struct OrientationReps body_to_imu;
enum AhrsMlkfStatus status;
bool_t is_aligned;
};
extern struct AhrsMlkf ahrs_impl;
extern struct AhrsMlkf ahrs_mlkf;
extern void ahrs_mlkf_init(void);
extern void ahrs_mlkf_set_body_to_imu(struct OrientationReps *body_to_imu);
extern void ahrs_mlkf_set_body_to_imu_quat(struct FloatQuat *q_b2i);
extern bool_t ahrs_mlkf_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
struct Int32Vect3 *lp_mag);
extern void ahrs_mlkf_propagate(struct Int32Rates *gyro, float dt);
extern void ahrs_mlkf_update_accel(struct Int32Vect3 *accel);
extern void ahrs_mlkf_update_mag(struct Int32Vect3 *mag);
extern void ahrs_mlkf_update_mag_2d(struct Int32Vect3 *mag);
extern void ahrs_mlkf_update_mag_full(struct Int32Vect3 *mag);
#endif /* AHRS_FLOAT_MLKF_H */
sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.c
+130
View File
@@ -0,0 +1,130 @@
/*
* Copyright (C) 2014 Felix Ruess <felix.ruess@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* 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.
*/
/**
* @file subsystems/ahrs/ahrs_float_mlkf_wrapper.c
*
* Paparazzi specific wrapper to run MLKF filter.
*/
#include "subsystems/ahrs/ahrs_float_mlkf_wrapper.h"
#include "subsystems/ahrs.h"
#include "subsystems/abi.h"
#if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h"
static void send_geo_mag(struct transport_tx *trans, struct link_device *dev)
{
pprz_msg_send_GEO_MAG(trans, dev, AC_ID,
&ahrs_mlkf.mag_h.x, &ahrs_mlkf.mag_h.y, &ahrs_mlkf.mag_h.z);
}
#endif
/** ABI binding for IMU data.
* Used for gyro, accel and mag ABI messages.
*/
#ifndef AHRS_MLKF_IMU_ID
#define AHRS_MLKF_IMU_ID ABI_BROADCAST
#endif
static abi_event gyro_ev;
static abi_event accel_ev;
static abi_event mag_ev;
static abi_event aligner_ev;
static abi_event body_to_imu_ev;
static void gyro_cb(uint8_t __attribute__((unused)) sender_id,
uint32_t __attribute__((unused)) stamp,
struct Int32Rates *gyro)
{
#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
PRINT_CONFIG_MSG("Calculating dt for AHRS_MLKF propagation.")
/* timestamp in usec when last callback was received */
static uint32_t last_stamp = 0;
if (last_stamp > 0 && ahrs_mlkf.is_aligned) {
float dt = (float)(stamp - last_stamp) * 1e-6;
ahrs_mlkf_propagate(gyro, dt);
}
last_stamp = stamp;
#else
PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS_MLKF propagation.")
PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
if (ahrs_mlkf.status == AHRS_MLKF_RUNNING) {
const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
ahrs_mlkf_propagate(gyro, dt);
}
#endif
}
static void accel_cb(uint8_t sender_id __attribute__((unused)),
uint32_t stamp __attribute__((unused)),
struct Int32Vect3 *accel)
{
if (ahrs_mlkf.is_aligned) {
ahrs_mlkf_update_accel(accel);
}
}
static void mag_cb(uint8_t sender_id __attribute__((unused)),
uint32_t stamp __attribute__((unused)),
struct Int32Vect3 *mag)
{
if (ahrs_mlkf.is_aligned) {
ahrs_mlkf_update_mag(mag);
}
}
static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
uint32_t stamp __attribute__((unused)),
struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
struct Int32Vect3 *lp_mag)
{
if (!ahrs_mlkf.is_aligned) {
ahrs_mlkf_align(lp_accel, lp_accel, lp_mag);
}
}
static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
struct FloatQuat *q_b2i_f)
{
ahrs_mlkf_set_body_to_imu_quat(q_b2i_f);
}
void ahrs_mlkf_register(void)
{
ahrs_register_impl(ahrs_mlkf_init, NULL);
/*
* Subscribe to scaled IMU measurements and attach callbacks
*/
AbiBindMsgIMU_GYRO_INT32(AHRS_MLKF_IMU_ID, &gyro_ev, gyro_cb);
AbiBindMsgIMU_ACCEL_INT32(AHRS_MLKF_IMU_ID, &accel_ev, accel_cb);
AbiBindMsgIMU_MAG_INT32(AHRS_MLKF_IMU_ID, &mag_ev, mag_cb);
AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb);
AbiBindMsgBODY_TO_IMU_QUAT(ABI_BROADCAST, &body_to_imu_ev, body_to_imu_cb);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "GEO_MAG", send_geo_mag);
#endif
}
sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.h
+36
View File
@@ -0,0 +1,36 @@
/*
* Copyright (C) 2014 Felix Ruess <felix.ruess@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file subsystems/ahrs/ahrs_float_mlkf_wrapper.h
*
* Paparazzi specific wrapper to run MLKF filter.
*/
#ifndef AHRS_FLOAT_MLKF_WRAPPER_H
#define AHRS_FLOAT_MLKF_WRAPPER_H
#include "subsystems/ahrs/ahrs_float_mlkf.h"
#define DefaultAhrsImpl ahrs_mlkf
extern void ahrs_mlkf_register(void);
#endif /* AHRS_FLOAT_MLKF_WRAPPER_H */

Some files were not shown because too many files have changed in this diff Show More