[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [6229] moved ahrs to general subsystems dir
|
From: |
Felix Ruess |
|
Subject: |
[paparazzi-commits] [6229] moved ahrs to general subsystems dir |
|
Date: |
Mon, 25 Oct 2010 14:10:08 +0000 |
Revision: 6229
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=6229
Author: flixr
Date: 2010-10-25 14:10:07 +0000 (Mon, 25 Oct 2010)
Log Message:
-----------
moved ahrs to general subsystems dir
Modified Paths:
--------------
paparazzi3/trunk/conf/autopilot/fixedwing.makefile
paparazzi3/trunk/conf/autopilot/rotorcraft.makefile
paparazzi3/trunk/conf/autopilot/subsystems/lisa_passthrough/ahrs_cmpl.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/settings/settings_booz2_ahrs_cmpl.xml
paparazzi3/trunk/conf/settings/settings_booz2_ahrs_lkf.xml
paparazzi3/trunk/sw/airborne/booz/test/test_mlkf.c
paparazzi3/trunk/sw/airborne/csc/mercury_xsens.h
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/hf_float.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/main.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/telemetry.h
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/vehicle_interface/vi.c
paparazzi3/trunk/sw/simulator/nps/nps_autopilot_booz.c
paparazzi3/trunk/sw/simulator/old_booz/booz2_sim_main.c
Added Paths:
-----------
paparazzi3/trunk/sw/airborne/subsystems/ahrs/
paparazzi3/trunk/sw/airborne/subsystems/ahrs/ahrs_aligner.c
paparazzi3/trunk/sw/airborne/subsystems/ahrs/ahrs_aligner.h
paparazzi3/trunk/sw/airborne/subsystems/ahrs/ahrs_cmpl_euler.c
paparazzi3/trunk/sw/airborne/subsystems/ahrs/ahrs_cmpl_euler.h
paparazzi3/trunk/sw/airborne/subsystems/ahrs/ahrs_float_ekf.c
paparazzi3/trunk/sw/airborne/subsystems/ahrs/ahrs_float_ekf.h
paparazzi3/trunk/sw/airborne/subsystems/ahrs/ahrs_float_lkf.c
paparazzi3/trunk/sw/airborne/subsystems/ahrs/ahrs_float_lkf.h
paparazzi3/trunk/sw/airborne/subsystems/ahrs.c
paparazzi3/trunk/sw/airborne/subsystems/ahrs.h
Removed Paths:
-------------
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs/
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs.h
Modified: paparazzi3/trunk/conf/autopilot/fixedwing.makefile
===================================================================
--- paparazzi3/trunk/conf/autopilot/fixedwing.makefile 2010-10-25 12:46:09 UTC
(rev 6228)
+++ paparazzi3/trunk/conf/autopilot/fixedwing.makefile 2010-10-25 14:10:07 UTC
(rev 6229)
@@ -14,6 +14,7 @@
SRC_FIXEDWING_TEST=$(SRC_FIXEDWING)/
SRC_FIRMWARE=firmwares/fixedwing
+SRC_SUBSYSTEMS=subsystems
FIXEDWING_INC = -I$(SRC_FIRMWARE) -I$(SRC_FIXEDWING) -I$(SRC_FIXEDWING_ARCH)
Modified: paparazzi3/trunk/conf/autopilot/rotorcraft.makefile
===================================================================
--- paparazzi3/trunk/conf/autopilot/rotorcraft.makefile 2010-10-25 12:46:09 UTC
(rev 6228)
+++ paparazzi3/trunk/conf/autopilot/rotorcraft.makefile 2010-10-25 14:10:07 UTC
(rev 6229)
@@ -45,13 +45,13 @@
CFG_BOOZ=$(PAPARAZZI_SRC)/conf/autopilot/
-BOOZ_INC = -I$(SRC_FIRMWARE) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) -I$(SRC_BOARD)
+ROTORCRAFT_INC = -I$(SRC_FIRMWARE) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH)
-I$(SRC_BOARD)
ap.ARCHDIR = $(ARCH)
-ap.CFLAGS += $(BOOZ_INC)
+ap.CFLAGS += $(ROTORCRAFT_INC)
ap.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) -DPERIPHERALS_AUTO_INIT
ap.srcs = $(SRC_FIRMWARE)/main.c
Modified:
paparazzi3/trunk/conf/autopilot/subsystems/lisa_passthrough/ahrs_cmpl.makefile
===================================================================
---
paparazzi3/trunk/conf/autopilot/subsystems/lisa_passthrough/ahrs_cmpl.makefile
2010-10-25 12:46:09 UTC (rev 6228)
+++
paparazzi3/trunk/conf/autopilot/subsystems/lisa_passthrough/ahrs_cmpl.makefile
2010-10-25 14:10:07 UTC (rev 6229)
@@ -3,6 +3,6 @@
#
ap.CFLAGS += -DUSE_AHRS_CMPL -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
-DAHRS_FIXED_POINT
-stm_passthrough.srcs += $(SRC_FIRMWARE)/ahrs.c
-stm_passthrough.srcs += $(SRC_FIRMWARE)/ahrs/ahrs_aligner.c
-stm_passthrough.srcs += $(SRC_FIRMWARE)/ahrs/ahrs_cmpl_euler.c
+stm_passthrough.srcs += $(SRC_SUBSYSTEMS)/ahrs.c
+stm_passthrough.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c
+stm_passthrough.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_cmpl_euler.c
Modified:
paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/ahrs_cmpl.makefile
===================================================================
--- paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/ahrs_cmpl.makefile
2010-10-25 12:46:09 UTC (rev 6228)
+++ paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/ahrs_cmpl.makefile
2010-10-25 14:10:07 UTC (rev 6229)
@@ -3,11 +3,11 @@
#
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
+ap.srcs += $(SRC_SUBSYSTEMS)/ahrs.c
+ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c
+ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/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
+sim.srcs += $(SRC_SUBSYSTEMS)/ahrs.c
+sim.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c
+sim.srcs += $(SRC_SUBSYSTEMS)/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-10-25 12:46:09 UTC (rev 6228)
+++ paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/ahrs_lkf.makefile
2010-10-25 14:10:07 UTC (rev 6229)
@@ -3,11 +3,11 @@
#
ap.CFLAGS += -DUSE_AHRS_LKF -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
-ap.srcs += $(SRC_FIRMWARE)/ahrs.c
-ap.srcs += $(SRC_FIRMWARE)/ahrs/ahrs_aligner.c
-ap.srcs += $(SRC_FIRMWARE)/ahrs/ahrs_float_lkf.c
+ap.srcs += $(SRC_SUBSYSTEMS)/ahrs.c
+ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c
+ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_float_lkf.c
sim.CFLAGS += -DUSE_AHRS_LKF -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
-sim.srcs += $(SRC_FIRMWARE)/ahrs.c
-sim.srcs += $(SRC_FIRMWARE)/ahrs/ahrs_aligner.c
-sim.srcs += $(SRC_FIRMWARE)/ahrs/ahrs_float_lkf.c
+sim.srcs += $(SRC_SUBSYSTEMS)/ahrs.c
+sim.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c
+sim.srcs += $(SRC_SUBSYSTEMS)/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-10-25 12:46:09 UTC (rev 6228)
+++ paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/ahrs_mlkf.makefile
2010-10-25 14:10:07 UTC (rev 6229)
@@ -3,14 +3,14 @@
#
ap.CFLAGS += -DAHRS_ALIGNER_LED=3
-ap.srcs += $(SRC_FIRMWARE)/ahrs.c
-ap.srcs += $(SRC_FIRMWARE)/ahrs/ahrs_aligner.c
+ap.srcs += $(SRC_SUBSYSTEMS)/ahrs.c
+ap.srcs += $(SRC_SUBSYSTEMS)/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_FIRMWARE)/ahrs.c
-sim.srcs += $(SRC_FIRMWARE)/ahrs/ahrs_aligner.c
+sim.srcs += $(SRC_SUBSYSTEMS)/ahrs.c
+sim.srcs += $(SRC_SUBSYSTEMS)/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/settings/settings_booz2_ahrs_cmpl.xml
===================================================================
--- paparazzi3/trunk/conf/settings/settings_booz2_ahrs_cmpl.xml 2010-10-25
12:46:09 UTC (rev 6228)
+++ paparazzi3/trunk/conf/settings/settings_booz2_ahrs_cmpl.xml 2010-10-25
14:10:07 UTC (rev 6229)
@@ -4,7 +4,7 @@
<dl_settings>
<dl_settings NAME="Filter">
- <dl_setting var="face_reinj_1" min="512" step="512" max="262144"
module="ahrs/ahrs_cmpl_euler" shortname="reinj_1"/>
+ <dl_setting var="face_reinj_1" min="512" step="512" max="262144"
module="subsystems/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-10-25
12:46:09 UTC (rev 6228)
+++ paparazzi3/trunk/conf/settings/settings_booz2_ahrs_lkf.xml 2010-10-25
14:10:07 UTC (rev 6229)
@@ -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/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_setting var="bafl_sigma_accel" min="0.1" step="1." max="1000.0"
module="subsystems/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="subsystems/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="subsystems/ahrs/ahrs_float_lkf" shortname="Q att"/>
+ <dl_setting var="bafl_Q_gyro" min="0.0" step="0.000001" max="0.1"
module="subsystems/ahrs/ahrs_float_lkf" shortname="Q gyro"/>
</dl_settings>
</dl_settings>
Modified: paparazzi3/trunk/sw/airborne/booz/test/test_mlkf.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/test/test_mlkf.c 2010-10-25 12:46:09 UTC
(rev 6228)
+++ paparazzi3/trunk/sw/airborne/booz/test/test_mlkf.c 2010-10-25 14:10:07 UTC
(rev 6229)
@@ -3,7 +3,7 @@
#include "math/pprz_algebra_double.h"
#include <subsystems/imu.h>
-#include <firmwares/rotorcraft/ahrs.h>
+#include <subsystems/ahrs.h>
#include "ahrs/ahrs_mlkf.h"
static void read_data(const char* filename);
Modified: paparazzi3/trunk/sw/airborne/csc/mercury_xsens.h
===================================================================
--- paparazzi3/trunk/sw/airborne/csc/mercury_xsens.h 2010-10-25 12:46:09 UTC
(rev 6228)
+++ paparazzi3/trunk/sw/airborne/csc/mercury_xsens.h 2010-10-25 14:10:07 UTC
(rev 6229)
@@ -62,7 +62,7 @@
extern int xsens_setzero;
-#include <firmwares/rotorcraft/ahrs.h>
+#include <subsystems/ahrs.h>
#define PERIODIC_SEND_BOOZ2_GYRO() { \
DOWNLINK_SEND_BOOZ2_GYRO(&imu.gyro.p, \
Deleted: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs.c
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs.c 2010-10-25
12:46:09 UTC (rev 6228)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs.c 2010-10-25
14:10:07 UTC (rev 6229)
@@ -1,31 +0,0 @@
-/*
- * $Id$
- *
- * Copyright (C) 2009 Felix Ruess <address@hidden>
- *
- * 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.
- */
-
-#include <firmwares/rotorcraft/ahrs.h>
-#include <subsystems/imu.h>
-
-struct Ahrs ahrs;
-struct AhrsFloat ahrs_float;
-
-float ahrs_mag_offset;
-
Deleted: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs.h
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs.h 2010-10-25
12:46:09 UTC (rev 6228)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs.h 2010-10-25
14:10:07 UTC (rev 6229)
@@ -1,91 +0,0 @@
-/*
- * $Id$
- *
- * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
- *
- * 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.
- */
-
-#ifndef AHRS_H
-#define AHRS_H
-
-#include "std.h"
-#include "math/pprz_algebra_int.h"
-#include "math/pprz_algebra_float.h"
-#include <firmwares/rotorcraft/ahrs/ahrs_aligner.h>
-
-#define AHRS_UNINIT 0
-#define AHRS_RUNNING 1
-
-struct Ahrs {
-
- struct Int32Quat ltp_to_imu_quat;
- struct Int32Eulers ltp_to_imu_euler;
- struct Int32RMat ltp_to_imu_rmat;
- struct Int32Rates imu_rate;
-
- struct Int32Quat ltp_to_body_quat;
- struct Int32Eulers ltp_to_body_euler;
- struct Int32RMat ltp_to_body_rmat;
- struct Int32Rates body_rate;
-
- uint8_t status;
-};
-
-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;
-
- uint8_t status;
-};
-
-extern struct Ahrs ahrs;
-extern struct AhrsFloat ahrs_float;
-
-extern float ahrs_mag_offset;
-
-#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 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 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 /* AHRS_H */
Modified:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
2010-10-25 12:46:09 UTC (rev 6228)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
2010-10-25 14:10:07 UTC (rev 6229)
@@ -25,7 +25,7 @@
//#define GUIDANCE_H_USE_REF
#include <firmwares/rotorcraft/guidance/guidance_h.h>
-#include <firmwares/rotorcraft/ahrs.h>
+#include <subsystems/ahrs.h>
#include <firmwares/rotorcraft/stabilization.h>
// #include "booz_fms.h" FIXME
#include <firmwares/rotorcraft/ins.h>
Modified:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
2010-10-25 12:46:09 UTC (rev 6228)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
2010-10-25 14:10:07 UTC (rev 6229)
@@ -28,7 +28,7 @@
#include "booz_radio_control.h"
#include <firmwares/rotorcraft/stabilization.h>
-#include <firmwares/rotorcraft/ahrs.h>
+#include <subsystems/ahrs.h>
// #include "booz_fms.h" FIXME
#include <firmwares/rotorcraft/navigation.h>
Modified: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/hf_float.c
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/hf_float.c
2010-10-25 12:46:09 UTC (rev 6228)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/hf_float.c
2010-10-25 14:10:07 UTC (rev 6229)
@@ -25,7 +25,7 @@
#include "hf_float.h"
#include <firmwares/rotorcraft/ins.h>
#include <subsystems/imu.h>
-#include <firmwares/rotorcraft/ahrs.h>
+#include <subsystems/ahrs.h>
#include "booz_gps.h"
#include <stdlib.h>
Modified: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins.c
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins.c 2010-10-25
12:46:09 UTC (rev 6228)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins.c 2010-10-25
14:10:07 UTC (rev 6229)
@@ -32,7 +32,7 @@
#include "math/pprz_algebra_int.h"
#include "math/pprz_algebra_float.h"
-#include <firmwares/rotorcraft/ahrs.h>
+#include <subsystems/ahrs.h>
#ifdef USE_VFF
#include "ins/vf_float.h"
Modified: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/main.c
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/main.c 2010-10-25
12:46:09 UTC (rev 6228)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/main.c 2010-10-25
14:10:07 UTC (rev 6229)
@@ -52,7 +52,7 @@
#include <firmwares/rotorcraft/stabilization.h>
#include <firmwares/rotorcraft/guidance.h>
-#include <firmwares/rotorcraft/ahrs.h>
+#include <subsystems/ahrs.h>
#include <firmwares/rotorcraft/ins.h>
#if defined USE_CAM || USE_DROP
Modified:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c
===================================================================
---
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c
2010-10-25 12:46:09 UTC (rev 6228)
+++
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c
2010-10-25 14:10:07 UTC (rev 6229)
@@ -24,7 +24,7 @@
#include <firmwares/rotorcraft/stabilization.h>
#include "math/pprz_algebra_float.h"
-#include <firmwares/rotorcraft/ahrs.h>
+#include <subsystems/ahrs.h>
#include "booz_radio_control.h"
#include "airframe.h"
Modified:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c
===================================================================
---
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c
2010-10-25 12:46:09 UTC (rev 6228)
+++
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c
2010-10-25 14:10:07 UTC (rev 6229)
@@ -22,7 +22,7 @@
*/
#include <firmwares/rotorcraft/stabilization.h>
-#include <firmwares/rotorcraft/ahrs.h>
+#include <subsystems/ahrs.h>
#include "booz_radio_control.h"
Modified:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c
===================================================================
---
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c
2010-10-25 12:46:09 UTC (rev 6228)
+++
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c
2010-10-25 14:10:07 UTC (rev 6229)
@@ -30,7 +30,7 @@
#include <stdio.h>
#include "math/pprz_algebra_float.h"
#include "math/pprz_algebra_int.h"
-#include <firmwares/rotorcraft/ahrs.h>
+#include <subsystems/ahrs.h>
#include "airframe.h"
struct FloatAttitudeGains
stabilization_gains[STABILIZATION_ATTITUDE_FLOAT_GAIN_NB];
Modified:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c
===================================================================
---
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c
2010-10-25 12:46:09 UTC (rev 6228)
+++
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c
2010-10-25 14:10:07 UTC (rev 6229)
@@ -28,7 +28,7 @@
#include "airframe.h"
#include <firmwares/rotorcraft/stabilization.h>
-#include <firmwares/rotorcraft/ahrs.h>
+#include <subsystems/ahrs.h>
#include "stabilization_attitude_ref_float.h"
#include "quat_setpoint.h"
Modified:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c
===================================================================
---
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c
2010-10-25 12:46:09 UTC (rev 6228)
+++
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c
2010-10-25 14:10:07 UTC (rev 6229)
@@ -24,7 +24,7 @@
#include <firmwares/rotorcraft/stabilization.h>
-#include <firmwares/rotorcraft/ahrs.h>
+#include <subsystems/ahrs.h>
#include <subsystems/imu.h>
#include "booz_radio_control.h"
Modified: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/telemetry.h
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/telemetry.h
2010-10-25 12:46:09 UTC (rev 6228)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/telemetry.h
2010-10-25 14:10:07 UTC (rev 6229)
@@ -49,7 +49,7 @@
#include <subsystems/imu.h>
#include "booz_gps.h"
#include <firmwares/rotorcraft/ins.h>
-#include <firmwares/rotorcraft/ahrs.h>
+#include <subsystems/ahrs.h>
#include "i2c_hw.h"
@@ -301,7 +301,7 @@
#endif /* STABILISATION_ATTITUDE_TYPE_FLOAT */
-#include <firmwares/rotorcraft/ahrs/ahrs_aligner.h>
+#include <subsystems/ahrs/ahrs_aligner.h>
#define PERIODIC_SEND_FILTER_ALIGNER(_chan) { \
DOWNLINK_SEND_FILTER_ALIGNER(_chan, \
&ahrs_aligner.lp_gyro.p, \
@@ -325,7 +325,7 @@
#ifdef USE_AHRS_CMPL
-#include <firmwares/rotorcraft/ahrs/ahrs_cmpl_euler.h>
+#include <subsystems/ahrs/ahrs_cmpl_euler.h>
#define PERIODIC_SEND_FILTER(_chan) { \
DOWNLINK_SEND_FILTER(_chan, \
&ahrs.ltp_to_imu_euler.phi, \
@@ -349,7 +349,7 @@
#endif
#ifdef USE_AHRS_LKF
-#include <firmwares/rotorcraft/ahrs.h>
+#include <subsystems/ahrs.h>
#include "ahrs/ahrs_float_lkf.h"
#define PERIODIC_SEND_AHRS_LKF(_chan) { \
DOWNLINK_SEND_AHRS_LKF(&bafl_eulers.phi, \
Modified: paparazzi3/trunk/sw/airborne/modules/cam_control/booz_cam.c
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/cam_control/booz_cam.c 2010-10-25
12:46:09 UTC (rev 6228)
+++ paparazzi3/trunk/sw/airborne/modules/cam_control/booz_cam.c 2010-10-25
14:10:07 UTC (rev 6229)
@@ -24,7 +24,7 @@
#include "cam_control/booz_cam.h"
#include "booz2_pwm_hw.h"
-#include <firmwares/rotorcraft/ahrs.h>
+#include <subsystems/ahrs.h>
#include <firmwares/rotorcraft/navigation.h>
#include <firmwares/rotorcraft/ins.h>
#include "flight_plan.h"
Modified: paparazzi3/trunk/sw/airborne/modules/cam_control/cam_track.c
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/cam_control/cam_track.c
2010-10-25 12:46:09 UTC (rev 6228)
+++ paparazzi3/trunk/sw/airborne/modules/cam_control/cam_track.c
2010-10-25 14:10:07 UTC (rev 6229)
@@ -25,7 +25,7 @@
#include "cam_track.h"
#include <firmwares/rotorcraft/ins.h>
-#include <firmwares/rotorcraft/ahrs.h>
+#include <subsystems/ahrs.h>
#ifdef USE_HFF
#include "ins/hf_float.h"
Modified: paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi.c
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi.c 2010-10-25
12:46:09 UTC (rev 6228)
+++ paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi.c 2010-10-25
14:10:07 UTC (rev 6229)
@@ -24,7 +24,7 @@
#include "vehicle_interface/vi.h"
#include <subsystems/imu.h>
-#include <firmwares/rotorcraft/ahrs.h>
+#include <subsystems/ahrs.h>
#include "booz/booz_gps.h"
#include "airframe.h"
Copied: paparazzi3/trunk/sw/airborne/subsystems/ahrs/ahrs_aligner.c (from rev
6228, paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_aligner.c)
===================================================================
--- paparazzi3/trunk/sw/airborne/subsystems/ahrs/ahrs_aligner.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/subsystems/ahrs/ahrs_aligner.c 2010-10-25
14:10:07 UTC (rev 6229)
@@ -0,0 +1,104 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
+ *
+ * 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.
+ */
+
+#include "ahrs_aligner.h"
+
+#include <stdlib.h> /* for abs() */
+#include <subsystems/imu.h>
+#include "led.h"
+
+struct AhrsAligner ahrs_aligner;
+
+#define SAMPLES_NB 512
+static struct Int32Rates gyro_sum;
+static struct Int32Vect3 accel_sum;
+static struct Int32Vect3 mag_sum;
+static int32_t ref_sensor_samples[SAMPLES_NB];
+static uint32_t samples_idx;
+
+void ahrs_aligner_init(void) {
+
+ ahrs_aligner.status = AHRS_ALIGNER_RUNNING;
+ INT_RATES_ZERO(gyro_sum);
+ INT_VECT3_ZERO(accel_sum);
+ INT_VECT3_ZERO(mag_sum);
+ samples_idx = 0;
+ ahrs_aligner.noise = 0;
+ ahrs_aligner.low_noise_cnt = 0;
+}
+
+#define LOW_NOISE_THRESHOLD 90000
+#define LOW_NOISE_TIME 5
+
+void ahrs_aligner_run(void) {
+
+ RATES_ADD(gyro_sum, imu.gyro);
+ VECT3_ADD(accel_sum, imu.accel);
+ VECT3_ADD(mag_sum, imu.mag);
+
+ ref_sensor_samples[samples_idx] = imu.accel.z;
+ samples_idx++;
+
+#ifdef AHRS_ALIGNER_LED
+ RunOnceEvery(50, {LED_TOGGLE(AHRS_ALIGNER_LED);});
+#endif
+
+ if (samples_idx >= SAMPLES_NB) {
+ int32_t avg_ref_sensor = accel_sum.z;
+ if ( avg_ref_sensor >= 0)
+ avg_ref_sensor += SAMPLES_NB / 2;
+ 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++) {
+ int32_t diff = ref_sensor_samples[i] - avg_ref_sensor;
+ ahrs_aligner.noise += abs(diff);
+ }
+
+ 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 (ahrs_aligner.noise < LOW_NOISE_THRESHOLD)
+ 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
+ }
+ }
+
+}
Copied: paparazzi3/trunk/sw/airborne/subsystems/ahrs/ahrs_aligner.h (from rev
6228, paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_aligner.h)
===================================================================
--- paparazzi3/trunk/sw/airborne/subsystems/ahrs/ahrs_aligner.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/subsystems/ahrs/ahrs_aligner.h 2010-10-25
14:10:07 UTC (rev 6229)
@@ -0,0 +1,48 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
+ *
+ * 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.
+ */
+
+#ifndef AHRS_ALIGNER_H
+#define AHRS_ALIGNER_H
+
+#include "std.h"
+#include "math/pprz_algebra_int.h"
+
+#define AHRS_ALIGNER_UNINIT 0
+#define AHRS_ALIGNER_RUNNING 1
+#define AHRS_ALIGNER_LOCKED 2
+
+struct AhrsAligner {
+ struct Int32Rates lp_gyro;
+ struct Int32Vect3 lp_accel;
+ struct Int32Vect3 lp_mag;
+ int32_t noise;
+ int32_t low_noise_cnt;
+ uint8_t status;
+};
+
+extern struct AhrsAligner ahrs_aligner;
+
+extern void ahrs_aligner_init(void);
+extern void ahrs_aligner_run(void);
+
+#endif /* AHRS_ALIGNER_H */
Copied: paparazzi3/trunk/sw/airborne/subsystems/ahrs/ahrs_cmpl_euler.c (from
rev 6228,
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_cmpl_euler.c)
===================================================================
--- paparazzi3/trunk/sw/airborne/subsystems/ahrs/ahrs_cmpl_euler.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/subsystems/ahrs/ahrs_cmpl_euler.c
2010-10-25 14:10:07 UTC (rev 6229)
@@ -0,0 +1,188 @@
+/*
+ * $Id: $
+ *
+ * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
+ *
+ * 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.
+ */
+
+#include "ahrs_cmpl_euler.h"
+
+#include <subsystems/imu.h>
+#include <subsystems/ahrs/ahrs_aligner.h>
+
+#include "airframe.h"
+#include "math/pprz_trig_int.h"
+#include "math/pprz_algebra_int.h"
+
+
+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 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 = FACE_REINJ_1;
+
+ INT_EULERS_ZERO(face_uncorrected);
+
+#ifdef IMU_MAG_OFFSET
+ ahrs_mag_offset = IMU_MAG_OFFSET;
+#else
+ ahrs_mag_offset = 0.;
+#endif
+}
+
+void ahrs_align(void) {
+
+ RATES_COPY( face_gyro_bias, ahrs_aligner.lp_gyro);
+ ahrs.status = AHRS_RUNNING;
+
+}
+
+
+#define F_UPDATE 512
+
+#define PI_INTEG_EULER (INT32_ANGLE_PI * F_UPDATE)
+#define TWO_PI_INTEG_EULER (INT32_ANGLE_2_PI * F_UPDATE)
+#define INTEG_EULER_NORMALIZE(_a) { \
+ while (_a > PI_INTEG_EULER) _a -= TWO_PI_INTEG_EULER; \
+ while (_a < -PI_INTEG_EULER) _a += TWO_PI_INTEG_EULER; \
+ }
+
+
+/*
+ *
+ * fc = 1/(2*pi*tau)
+ *
+ * alpha = dt / ( tau + dt )
+ *
+ *
+ * y(i) = alpha x(i) + (1-alpha) y(i-1)
+ * or
+ * y(i) = y(i-1) + alpha * (x(i) - y(i-1))
+ *
+ *
+ */
+
+void ahrs_propagate(void) {
+
+ /* unbias gyro */
+ struct Int32Rates uf_rate;
+ 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, ahrs.ltp_to_imu_euler, ahrs.imu_rate);
+ EULERS_ADD(face_corrected, euler_dot);
+
+ /* low pass measurement */
+ EULERS_ADD(face_measure, measurement);
+ EULERS_SDIV(face_measure, face_measure, 2);
+ /* compute residual */
+ EULERS_DIFF(face_residual, face_measure, face_corrected);
+ INTEG_EULER_NORMALIZE(face_residual.psi);
+
+ struct Int32Eulers correction;
+ /* compute a correction */
+ EULERS_SDIV(correction, face_residual, face_reinj_1);
+ /* correct estimation */
+ EULERS_ADD(face_corrected, correction);
+ INTEG_EULER_NORMALIZE(face_corrected.psi);
+
+
+ /* Compute LTP to IMU eulers */
+ EULERS_SDIV(ahrs.ltp_to_imu_euler, face_corrected, F_UPDATE);
+ /* Compute LTP to IMU quaternion */
+ INT32_QUAT_OF_EULERS(ahrs.ltp_to_imu_quat, ahrs.ltp_to_imu_euler);
+ /* Compute LTP to IMU rotation matrix */
+ INT32_RMAT_OF_EULERS(ahrs.ltp_to_imu_rmat, ahrs.ltp_to_imu_euler);
+
+ /* Compute LTP to BODY quaternion */
+ 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(ahrs.ltp_to_body_rmat, ahrs.ltp_to_imu_rmat,
imu.body_to_imu_rmat);
+ /* compute LTP to BODY eulers */
+ INT32_EULERS_OF_RMAT(ahrs.ltp_to_body_euler, ahrs.ltp_to_body_rmat);
+ /* compute body rates */
+ INT32_RMAT_TRANSP_RATEMULT(ahrs.body_rate, imu.body_to_imu_rmat,
ahrs.imu_rate);
+
+}
+
+void ahrs_update_accel(void) {
+
+ /* build a measurement assuming constant acceleration ?!! */
+ INT32_ATAN2(measurement.phi, -imu.accel.y, -imu.accel.z);
+ int32_t cphi;
+ PPRZ_ITRIG_COS(cphi, measurement.phi);
+ int32_t cphi_ax = -INT_MULT_RSHIFT(cphi, imu.accel.x, INT32_TRIG_FRAC);
+ INT32_ATAN2(measurement.theta, -cphi_ax, -imu.accel.z);
+ measurement.phi *= F_UPDATE;
+ measurement.theta *= F_UPDATE;
+
+}
+
+/* measure psi assuming magnetic vector is in earth plan (md = 0) */
+void ahrs_update_mag(void) {
+
+ int32_t sphi;
+ PPRZ_ITRIG_SIN(sphi, ahrs.ltp_to_imu_euler.phi);
+ int32_t cphi;
+ PPRZ_ITRIG_COS(cphi, ahrs.ltp_to_imu_euler.phi);
+ int32_t stheta;
+ PPRZ_ITRIG_SIN(stheta, ahrs.ltp_to_imu_euler.theta);
+ int32_t ctheta;
+ 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;
+ //int32_t sphi_ctheta = (sphi*ctheta)>>INT32_TRIG_FRAC;
+ //int32_t cphi_ctheta = (cphi*ctheta)>>INT32_TRIG_FRAC;
+
+ const int32_t mn =
+ ctheta * imu.mag.x +
+ sphi_stheta * imu.mag.y +
+ cphi_stheta * imu.mag.z;
+ const int32_t me =
+ 0 * imu.mag.x +
+ cphi * imu.mag.y +
+ -sphi * imu.mag.z;
+ //const int32_t md =
+ // -stheta * imu.mag.x +
+ // sphi_ctheta * imu.mag.y +
+ // cphi_ctheta * imu.mag.z;
+ float m_psi = -atan2(me, mn);
+ measurement.psi = ((m_psi -
RadOfDeg(ahrs_mag_offset))*(float)(1<<(INT32_ANGLE_FRAC))*F_UPDATE);
+
+}
Copied: paparazzi3/trunk/sw/airborne/subsystems/ahrs/ahrs_cmpl_euler.h (from
rev 6228,
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_cmpl_euler.h)
===================================================================
--- paparazzi3/trunk/sw/airborne/subsystems/ahrs/ahrs_cmpl_euler.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/subsystems/ahrs/ahrs_cmpl_euler.h
2010-10-25 14:10:07 UTC (rev 6229)
@@ -0,0 +1,40 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
+ *
+ * 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.
+ */
+
+#ifndef AHRS_CMPL_EULER_H
+#define AHRS_CMPL_EULER_H
+
+#include <subsystems/ahrs.h>
+#include "std.h"
+#include "math/pprz_algebra_int.h"
+
+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 face_reinj_1;
+
+
+#endif /* AHRS_CMPL_EULER_H */
Copied: paparazzi3/trunk/sw/airborne/subsystems/ahrs/ahrs_float_ekf.c (from rev
6228, paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_float_ekf.c)
===================================================================
--- paparazzi3/trunk/sw/airborne/subsystems/ahrs/ahrs_float_ekf.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/subsystems/ahrs/ahrs_float_ekf.c
2010-10-25 14:10:07 UTC (rev 6229)
@@ -0,0 +1,171 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
+ *
+ * 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.
+ */
+
+#include "math/pprz_algebra_float.h"
+
+/* our estimated attitude */
+struct FloatQuat bafe_quat;
+/* our estimated gyro biases */
+struct FloatRates bafe_bias;
+/* we get unbiased body rates as byproduct */
+struct FloatRates bafe_rates;
+/* maintain a euler angles representation */
+struct FloatEulers bafe_eulers;
+/* maintains a rotation matrix representation */
+struct FloatEulers bafe_dcm;
+/* time derivative of our quaternion */
+struct FloatQuat bafe_qdot;
+
+#define BAFE_SSIZE 7
+/* covariance matrix and its time derivative */
+float bafe_P[BAFE_SSIZE][BAFE_SSIZE];
+float bafe_Pdot[BAFE_SSIZE][BAFE_SSIZE];
+
+/*
+ * F represents the Jacobian of the derivative of the system with respect
+ * its states. We do not allocate the bottom three rows since we know that
+ * the derivatives of bias_dot are all zero.
+ */
+float bafe_F[4][7];
+
+/*
+ * Kalman filter variables.
+ */
+float bafe_PHt[7];
+float bafe_K[7];
+float bafe_E;
+/*
+ * H represents the Jacobian of the measurements of the attitude
+ * with respect to the states of the filter. We do not allocate the bottom
+ * three rows since we know that the attitude measurements have no
+ * relationship to gyro bias.
+ */
+float bafe_H[4];
+/* temporary variable used during state covariance update */
+float bafe_FP[4][7];
+
+/*
+ * Q is our estimate noise variance. It is supposed to be an NxN
+ * matrix, but with elements only on the diagonals. Additionally,
+ * since the quaternion has no expected noise (we can't directly measure
+ * it), those are zero. For the gyro, we expect around 5 deg/sec noise,
+ * which is 0.08 rad/sec. The variance is then 0.08^2 ~= 0.0075.
+ */
+/* I measured about 0.009 rad/s noise */
+#define bafe_Q_gyro 8e-03
+
+/*
+ * R is our measurement noise estimate. Like Q, it is supposed to be
+ * an NxN matrix with elements on the diagonals. However, since we can
+ * not directly measure the gyro bias, we have no estimate for it.
+ * We only have an expected noise in the pitch and roll accelerometers
+ * and in the compass.
+ */
+#define BAFE_R_PHI 1.3 * 1.3
+#define BAFE_R_THETA 1.3 * 1.3
+#define BAFE_R_PSI 2.5 * 2.5
+
+#ifndef BAFE_DT
+#define BAFE_DT (1./512.)
+#endif
+
+extern void ahrs_init(void);
+extern void ahrs_align(void);
+
+
+/*
+ * Propagate our dynamic system
+ *
+ * quat_dot = Wxq(pqr) * quat
+ * bias_dot = 0
+ *
+ * Wxq is the quaternion omega matrix:
+ *
+ * [ 0, -p, -q, -r ]
+ * 1/2 * [ p, 0, r, -q ]
+ * [ q, -r, 0, p ]
+ * [ r, q, -p, 0 ]
+ *
+ *
+ *
+ *
+ * [ 0 -p -q -r q1 q2 q3]
+ * F = 1/2 * [ p 0 r -q -q0 q3 -q2]
+ * [ q -r 0 p -q3 -q0 q1]
+ * [ r q -p 0 q2 -q1 -q0]
+ * [ 0 0 0 0 0 0 0]
+ * [ 0 0 0 0 0 0 0]
+ * [ 0 0 0 0 0 0 0]
+ *
+ */
+
+void ahrs_propagate(void) {
+ /* compute unbiased rates */
+ RATES_FLOAT_OF_BFP(bafe_rates, imu.gyro);
+ RATES_SUB(bafe_rates, bafe_bias);
+
+ /* compute F
+ F is only needed later on to update the state covariance P.
+ However, its [0:3][0:3] region is actually the Wxq(pqr) which is needed to
+ compute the time derivative of the quaternion, so we compute F now
+ */
+
+ /* Fill in Wxq(pqr) into F */
+ bafe_F[0][0] = bafe_F[1][1] = bafe_F[2][2] = bafe_F[3][3] = 0;
+ bafe_F[1][0] = bafe_F[2][3] = bafe_rates.p * 0.5;
+ bafe_F[2][0] = bafe_F[3][1] = bafe_rates.q * 0.5;
+ bafe_F[3][0] = bafe_F[1][2] = bafe_rates.r * 0.5;
+
+ bafe_F[0][1] = bafe_F[3][2] = -bafe_F[1][0];
+ bafe_F[0][2] = bafe_F[1][3] = -bafe_F[2][0];
+ bafe_F[0][3] = bafe_F[2][1] = -bafe_F[3][0];
+ /* Fill in [4:6][0:3] region */
+ bafe_F[0][4] = bafe_F[2][6] = bafe_quat.qx * 0.5;
+ bafe_F[0][5] = bafe_F[3][4] = bafe_quat.qy * 0.5;
+ bafe_F[0][6] = bafe_F[1][5] = bafe_quat.qz * 0.5;
+
+ bafe_F[1][4] = bafe_F[2][5] = bafe_F[3][6] = bafe_quat.qi * -0.5;
+ bafe_F[3][5] = -bafe_F[0][4];
+ bafe_F[1][6] = -bafe_F[0][5];
+ bafe_F[2][4] = -bafe_F[0][6];
+ /* quat_dot = Wxq(pqr) * quat */
+ bafe_qdot.qi=
bafe_F[0][1]*bafe_quat.qx+bafe_F[0][2]*bafe_quat.qy+bafe_F[0][3] * bafe_quat.qz;
+ bafe_qdot.qx= bafe_F[1][0]*bafe_quat.qi
+bafe_F[1][2]*bafe_quat.qy+bafe_F[1][3] * bafe_quat.qz;
+ bafe_qdot.qy= bafe_F[2][0]*bafe_quat.qi+bafe_F[2][1]*bafe_quat.qx
+bafe_F[2][3] * bafe_quat.qz;
+ bafe_qdot.qz=
bafe_F[3][0]*bafe_quat.qi+bafe_F[3][1]*bafe_quat.qx+bafe_F[3][2]*bafe_quat.qy
;
+ /* propagate quaternion */
+ bafe_quat.qi += bafe_qdot.qi * BAFE_DT;
+ bafe_quat.qx += bafe_qdot.qx * BAFE_DT;
+ bafe_quat.qy += bafe_qdot.qy * BAFE_DT;
+ bafe_quat.qz += bafe_qdot.qz * BAFE_DT;
+
+
+}
+
+
+extern void ahrs_update(void);
+
+
+
+
+#endif /* AHRS_FLOAT_EKF_H */
Copied: paparazzi3/trunk/sw/airborne/subsystems/ahrs/ahrs_float_ekf.h (from rev
6228, paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_float_ekf.h)
===================================================================
--- paparazzi3/trunk/sw/airborne/subsystems/ahrs/ahrs_float_ekf.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/subsystems/ahrs/ahrs_float_ekf.h
2010-10-25 14:10:07 UTC (rev 6229)
@@ -0,0 +1,37 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
+ *
+ * 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.
+ */
+
+#ifndef AHRS_FLOAT_EKF_H
+#define AHRS_FLOAT_EKF_H
+
+
+
+extern void ahrs_init(void);
+extern void ahrs_align(void);
+extern void ahrs_propagate(void);
+extern void ahrs_update(void);
+
+
+
+
+#endif /* AHRS_FLOAT_EKF_H */
Copied: paparazzi3/trunk/sw/airborne/subsystems/ahrs/ahrs_float_lkf.c (from rev
6228, paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_float_lkf.c)
===================================================================
--- paparazzi3/trunk/sw/airborne/subsystems/ahrs/ahrs_float_lkf.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/subsystems/ahrs/ahrs_float_lkf.c
2010-10-25 14:10:07 UTC (rev 6229)
@@ -0,0 +1,852 @@
+/*
+ * $Id: $
+ *
+ * Copyright (C) 2009 Felix Ruess <address@hidden>
+ * Copyright (C) 2009 Antoine Drouin <address@hidden>
+ *
+ * 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.
+ */
+
+#include "ahrs_float_lkf.h"
+
+#include <subsystems/imu.h>
+#include <subsystems/ahrs/ahrs_aligner.h>
+
+#include "airframe.h"
+#include "math/pprz_algebra_float.h"
+
+#include <stdio.h>
+
+
+#define BAFL_DEBUG
+
+static void ahrs_do_update_accel(void);
+static void ahrs_do_update_mag(void);
+
+
+/* our estimated attitude (ltp <-> imu) */
+struct FloatQuat bafl_quat;
+/* our estimated gyro biases */
+struct FloatRates bafl_bias;
+/* we get unbiased body rates as byproduct */
+struct FloatRates bafl_rates;
+/* maintain a euler angles representation */
+struct FloatEulers bafl_eulers;
+/* C n->b rotation matrix representation */
+struct FloatRMat bafl_dcm;
+
+/* estimated attitude errors from accel update */
+struct FloatQuat bafl_q_a_err;
+/* estimated attitude errors from mag update */
+struct FloatQuat bafl_q_m_err;
+/* our estimated gyro bias errors from accel update */
+struct FloatRates bafl_b_a_err;
+/* our estimated gyro bias errors from mag update */
+struct FloatRates bafl_b_m_err;
+/* temporary quaternion */
+struct FloatQuat bafl_qtemp;
+/* correction quaternion of strapdown computation */
+struct FloatQuat bafl_qr;
+/* norm of attitude quaternion */
+float bafl_qnorm;
+
+/* pseude measured angles for debug */
+float bafl_phi_accel;
+float bafl_theta_accel;
+
+#ifndef BAFL_SSIZE
+#define BAFL_SSIZE 6
+#endif
+/* error covariance matrix */
+float bafl_P[BAFL_SSIZE][BAFL_SSIZE];
+/* filter state */
+float bafl_X[BAFL_SSIZE];
+
+/*
+ * F represents the Jacobian of the derivative of the system with respect
+ * its states. We do not allocate the bottom three rows since we know that
+ * the derivatives of bias_dot are all zero.
+ */
+float bafl_F[3][3];
+/*
+ * T represents the discrete state transition matrix
+ * T = e^(F * dt)
+ */
+float bafl_T[6][6];
+
+/*
+ * Kalman filter variables.
+ */
+float bafl_Pprio[BAFL_SSIZE][BAFL_SSIZE];
+/* temporary variable used during state covariance update */
+float bafl_tempP[BAFL_SSIZE][BAFL_SSIZE];
+float bafl_K[6][3];
+float bafl_tempK[6][3];
+float bafl_S[3][3];
+float bafl_tempS[3][6];
+float bafl_invS[3][3];
+struct FloatVect3 bafl_ya;
+struct FloatVect3 bafl_ym;
+
+/*
+ * H represents the Jacobian of the measurements of the attitude
+ * with respect to the states of the filter. We do not allocate the bottom
+ * three rows since we know that the attitude measurements have no
+ * relationship to gyro bias.
+ * The last three columns always stay zero.
+ */
+float bafl_H[3][3];
+
+/* low pass of accel measurements */
+struct FloatVect3 bafl_accel_measure;
+struct FloatVect3 bafl_accel_last;
+
+struct FloatVect3 bafl_mag;
+
+/* temporary variables for the strapdown computation */
+float bafl_qom[4][4];
+
+#define BAFL_g 9.81
+
+#define BAFL_hx 1.0
+#define BAFL_hy 0.0
+#define BAFL_hz 1.0
+struct FloatVect3 bafl_h;
+
+/*
+ * Q is our estimate noise variance. It is supposed to be an NxN
+ * matrix, but with elements only on the diagonals. Additionally,
+ * since the quaternion has no expected noise (we can't directly measure
+ * it), those are zero. For the gyro, we expect around 5 deg/sec noise,
+ * which is 0.08 rad/sec. The variance is then 0.08^2 ~= 0.0075.
+ */
+/* I measured about 0.009 rad/s noise */
+#define BAFL_Q_GYRO 1e-04
+#define BAFL_Q_ATT 0
+float bafl_Q_gyro;
+float bafl_Q_att;
+
+/*
+ * R is our measurement noise estimate. Like Q, it is supposed to be
+ * an NxN matrix with elements on the diagonals. However, since we can
+ * not directly measure the gyro bias, we have no estimate for it.
+ * We only have an expected noise in the pitch and roll accelerometers
+ * and in the compass.
+ */
+#define BAFL_SIGMA_ACCEL 1000.0
+#define BAFL_SIGMA_MAG 20.
+float bafl_sigma_accel;
+float bafl_sigma_mag;
+float bafl_R_accel;
+float bafl_R_mag;
+
+#ifndef BAFL_DT
+#define BAFL_DT (1./512.)
+#endif
+
+
+#define FLOAT_MAT33_INVERT(_mo, _mi) {
\
+ float _det = 0.;
\
+ _det = _mi[0][0] * (_mi[2][2] * _mi[1][1] - _mi[2][1] * _mi[1][2])
\
+ - _mi[1][0] * (_mi[2][2] * _mi[0][1] - _mi[2][1] * _mi[0][2])
\
+ + _mi[2][0] * (_mi[1][2] * _mi[0][1] - _mi[1][1] * _mi[0][2]);
\
+ if (_det != 0.) { /* ? test for zero? */
\
+ _mo[0][0] = (_mi[1][1] * _mi[2][2] - _mi[1][2] * _mi[2][1]) /
_det; \
+ _mo[0][1] = (_mi[0][2] * _mi[2][1] - _mi[0][1] * _mi[2][2]) /
_det; \
+ _mo[0][2] = (_mi[0][1] * _mi[1][2] - _mi[0][2] * _mi[1][1]) /
_det; \
+
\
+ _mo[1][0] = (_mi[1][2] * _mi[2][0] - _mi[1][0] * _mi[2][2]) /
_det; \
+ _mo[1][1] = (_mi[0][0] * _mi[2][2] - _mi[0][2] * _mi[2][0]) /
_det; \
+ _mo[1][2] = (_mi[0][2] * _mi[1][0] - _mi[0][0] * _mi[1][2]) /
_det; \
+
\
+ _mo[2][0] = (_mi[1][0] * _mi[2][1] - _mi[1][1] * _mi[2][0]) /
_det; \
+ _mo[2][1] = (_mi[0][1] * _mi[2][0] - _mi[0][0] * _mi[2][1]) /
_det; \
+ _mo[2][2] = (_mi[0][0] * _mi[1][1] - _mi[0][1] * _mi[1][0]) /
_det; \
+ } else {
\
+ printf("ERROR! Not invertible!\n");
\
+ for (int _i=0; _i<3; _i++) {
\
+ for (int _j=0; _j<3; _j++) {
\
+ _mo[_i][_j] = 0.0;
\
+ }
\
+ }
\
+ }
\
+}
+
+#define AHRS_TO_BFP() {
\
+ /* IMU rate */
\
+ RATES_BFP_OF_REAL(ahrs.imu_rate, bafl_rates);
\
+ /* LTP to IMU eulers */
\
+ EULERS_BFP_OF_REAL(ahrs.ltp_to_imu_euler, bafl_eulers); \
+ /* LTP to IMU quaternion */
\
+ QUAT_BFP_OF_REAL(ahrs.ltp_to_imu_quat, bafl_quat);
\
+ /* LTP to IMU rotation matrix */
\
+ 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(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(ahrs.ltp_to_body_rmat, ahrs.ltp_to_imu_rmat,
imu.body_to_imu_rmat); \
+ /* compute LTP to BODY eulers */
\
+ INT32_EULERS_OF_RMAT(ahrs.ltp_to_body_euler, ahrs.ltp_to_body_rmat);
\
+ /* compute body rates */
\
+ INT32_RMAT_TRANSP_RATEMULT(ahrs.body_rate, imu.body_to_imu_rmat,
ahrs.imu_rate); \
+}
+
+
+void ahrs_init(void) {
+ int i, j;
+
+ for (i = 0; i < BAFL_SSIZE; i++) {
+ for (j = 0; j < BAFL_SSIZE; j++) {
+ bafl_T[i][j] = 0.;
+ bafl_P[i][j] = 0.;
+ }
+ /* Insert the diagonal elements of the T-Matrix. These will
never change. */
+ bafl_T[i][i] = 1.0;
+ /* initial covariance values */
+ if (i<3) {
+ bafl_P[i][i] = 1.0;
+ } else {
+ bafl_P[i][i] = 0.1;
+ }
+ }
+
+ FLOAT_QUAT_ZERO(bafl_quat);
+
+ 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);
+
+ 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;
+
+ FLOAT_VECT3_ASSIGN(bafl_h, BAFL_hx,BAFL_hy, BAFL_hz);
+
+}
+
+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 ahrs_lowpass_accel(void) {
+ // get latest measurement
+ ACCELS_FLOAT_OF_BFP(bafl_accel_last, imu.accel);
+ // low pass it
+ VECT3_ADD(bafl_accel_measure, bafl_accel_last);
+ VECT3_SDIV(bafl_accel_measure, bafl_accel_measure, 2);
+
+#ifdef BAFL_DEBUG
+ bafl_phi_accel = atan2f( -bafl_accel_measure.y, -bafl_accel_measure.z);
+ bafl_theta_accel = atan2f(bafl_accel_measure.x,
sqrtf(bafl_accel_measure.y*bafl_accel_measure.y +
bafl_accel_measure.z*bafl_accel_measure.z));
+#endif
+}
+
+/*
+ * Propagate our dynamic system in time
+ *
+ * Run the strapdown computation and the predict step of the filter
+ *
+ *
+ * quat_dot = Wxq(pqr) * quat
+ * bias_dot = 0
+ *
+ * Wxq is the quaternion omega matrix:
+ *
+ * [ 0, -p, -q, -r ]
+ * 1/2 * [ p, 0, r, -q ]
+ * [ q, -r, 0, p ]
+ * [ r, q, -p, 0 ]
+ *
+ */
+void ahrs_propagate(void) {
+ int i, j, k;
+
+ ahrs_lowpass_accel();
+
+ /* compute unbiased rates */
+ RATES_FLOAT_OF_BFP(bafl_rates, imu.gyro);
+ RATES_SUB(bafl_rates, bafl_bias);
+
+
+ /* run strapdown computation
+ *
+ */
+
+ /* multiplicative quaternion update */
+ /* compute correction quaternion */
+ QUAT_ASSIGN(bafl_qr, 1., bafl_rates.p * BAFL_DT / 2, bafl_rates.q *
BAFL_DT / 2, bafl_rates.r * BAFL_DT / 2);
+ /* normalize it. maybe not necessary?? */
+ float q_sq;
+ q_sq = (bafl_qr.qx * bafl_qr.qx +bafl_qr.qy * bafl_qr.qy + bafl_qr.qz *
bafl_qr.qz) / 4;
+ if (q_sq > 1) { /* this should actually never happen */
+ FLOAT_QUAT_SMUL(bafl_qr, bafl_qr, 1 / sqrtf(1 + q_sq));
+ } else {
+ bafl_qr.qi = sqrtf(1 - q_sq);
+ }
+ /* propagate correction quaternion */
+ FLOAT_QUAT_COMP(bafl_qtemp, bafl_quat, bafl_qr);
+ FLOAT_QUAT_COPY(bafl_quat, bafl_qtemp);
+
+ bafl_qnorm = FLOAT_QUAT_NORM(bafl_quat);
+ //TODO check if broot force normalization is good, use lagrange
normalization?
+ FLOAT_QUAT_NORMALISE(bafl_quat);
+
+
+ /*
+ * compute all representations
+ */
+ /* maintain rotation matrix representation */
+ FLOAT_RMAT_OF_QUAT(bafl_dcm, bafl_quat);
+ /* maintain euler representation */
+ FLOAT_EULERS_OF_RMAT(bafl_eulers, bafl_dcm);
+ AHRS_TO_BFP();
+ AHRS_LTP_TO_BODY();
+
+
+ /* error KF-Filter
+ * propagate only the error covariance matrix since error is corrected
after each measurement
+ *
+ * F = [ 0 0 0 ]
+ * [ 0 0 0 -Cbn ]
+ * [ 0 0 0 ]
+ * [ 0 0 0 0 0 0 ]
+ * [ 0 0 0 0 0 0 ]
+ * [ 0 0 0 0 0 0 ]
+ *
+ * T = e^(dt * F)
+ *
+ * T = [ 1 0 0 ]
+ * [ 0 1 0 -Cbn*dt ]
+ * [ 0 0 1 ]
+ * [ 0 0 0 1 0 0 ]
+ * [ 0 0 0 0 1 0 ]
+ * [ 0 0 0 0 0 1 ]
+ *
+ * P_prio = T * P * T_T + Q
+ *
+ */
+
+ /*
+ * compute state transition matrix T
+ *
+ * diagonal elements of T are always one
+ */
+ for (i = 0; i < 3; i++) {
+ for (j = 0; j < 3; j++) {
+ bafl_T[i][j + 3] = -RMAT_ELMT(bafl_dcm, j, i); /*
inverted bafl_dcm */
+ }
+ }
+
+
+ /*
+ * estimate the a priori error covariance matrix P_k = T * P_k-1 * T_T
+ Q
+ */
+ /* Temporary multiplication temp(6x6) = T(6x6) * P(6x6) */
+ for (i = 0; i < BAFL_SSIZE; i++) {
+ for (j = 0; j < BAFL_SSIZE; j++) {
+ bafl_tempP[i][j] = 0.;
+ for (k = 0; k < BAFL_SSIZE; k++) {
+ bafl_tempP[i][j] += bafl_T[i][k] * bafl_P[k][j];
+ }
+ }
+ }
+
+ /* P_k(6x6) = temp(6x6) * T_T(6x6) + Q */
+ for (i = 0; i < BAFL_SSIZE; i++) {
+ for (j = 0; j < BAFL_SSIZE; j++) {
+ bafl_P[i][j] = 0.;
+ for (k = 0; k < BAFL_SSIZE; k++) {
+ bafl_P[i][j] += bafl_tempP[i][k] *
bafl_T[j][k]; //T[j][k] = T_T[k][j]
+ }
+ }
+ if (i<3) {
+ bafl_P[i][i] += bafl_Q_att;
+ } else {
+ bafl_P[i][i] += bafl_Q_gyro;
+ }
+ }
+
+#ifdef LKF_PRINT_P
+ printf("Pp =");
+ for (i = 0; i < 6; i++) {
+ printf("[");
+ for (j = 0; j < 6; j++) {
+ printf("%f\t", bafl_P[i][j]);
+ }
+ printf("]\n ");
+ }
+ printf("\n");
+#endif
+}
+
+void ahrs_update_accel(void) {
+ RunOnceEvery(50, ahrs_do_update_accel());
+}
+
+static void ahrs_do_update_accel(void) {
+ int i, j, k;
+
+#ifdef BAFL_DEBUG2
+ printf("Accel update.\n");
+#endif
+
+ /* P_prio = P */
+ for (i = 0; i < BAFL_SSIZE; i++) {
+ for (j = 0; j < BAFL_SSIZE; j++) {
+ bafl_Pprio[i][j] = bafl_P[i][j];
+ }
+ }
+
+ /*
+ * set up measurement matrix
+ *
+ * g = 9.81
+ * gx = [ 0 -g 0
+ * g 0 0
+ * 0 0 0 ]
+ * H = [ 0 0 0 ]
+ * [ -Cnb*gx 0 0 0 ]
+ * [ 0 0 0 ]
+ *
+ * */
+ bafl_H[0][0] = -RMAT_ELMT(bafl_dcm, 0, 1) * BAFL_g;
+ bafl_H[0][1] = RMAT_ELMT(bafl_dcm, 0, 0) * BAFL_g;
+ bafl_H[1][0] = -RMAT_ELMT(bafl_dcm, 1, 1) * BAFL_g;
+ bafl_H[1][1] = RMAT_ELMT(bafl_dcm, 1, 0) * BAFL_g;
+ bafl_H[2][0] = -RMAT_ELMT(bafl_dcm, 2, 1) * BAFL_g;
+ bafl_H[2][1] = RMAT_ELMT(bafl_dcm, 2, 0) * BAFL_g;
+ /* rest is zero
+ bafl_H[0][2] = 0.;
+ bafl_H[1][2] = 0.;
+ bafl_H[2][2] = 0.;*/
+
+ /**********************************************
+ * compute Kalman gain K
+ *
+ * K = P_prio * H_T * inv(S)
+ * S = H * P_prio * HT + R
+ *
+ * K = P_prio * HT * inv(H * P_prio * HT + R)
+ *
+ **********************************************/
+
+ /* covariance residual S = H * P_prio * HT + R */
+
+ /* temp_S(3x6) = H(3x6) * P_prio(6x6)
+ * last 4 columns of H are zero
+ */
+ for (i = 0; i < 3; i++) {
+ for (j = 0; j < BAFL_SSIZE; j++) {
+ bafl_tempS[i][j] = bafl_H[i][0] * bafl_Pprio[0][j];
+ bafl_tempS[i][j] += bafl_H[i][1] * bafl_Pprio[1][j];
+ }
+ }
+
+ /* S(3x3) = temp_S(3x6) * HT(6x3) + R(3x3)
+ *
+ * bottom 4 rows of HT are zero
+ */
+ for (i = 0; i < 3; i++) {
+ for (j = 0; j < 3; j++) {
+ bafl_S[i][j] = bafl_tempS[i][0] * bafl_H[j][0]; /*
H[j][0] = HT[0][j] */
+ bafl_S[i][j] += bafl_tempS[i][1] * bafl_H[j][1]; /*
H[j][0] = HT[0][j] */
+ }
+ bafl_S[i][i] += bafl_R_accel;
+ }
+
+ /* invert S
+ */
+ FLOAT_MAT33_INVERT(bafl_invS, bafl_S);
+
+
+ /* temp_K(6x3) = P_prio(6x6) * HT(6x3)
+ *
+ * bottom 4 rows of HT are zero
+ */
+ for (i = 0; i < BAFL_SSIZE; i++) {
+ for (j = 0; j < 3; j++) {
+ /* not computing zero elements */
+ bafl_tempK[i][j] = bafl_Pprio[i][0] * bafl_H[j][0]; /*
H[j][0] = HT[0][j] */
+ bafl_tempK[i][j] += bafl_Pprio[i][1] * bafl_H[j][1]; /*
H[j][1] = HT[1][j] */
+ }
+ }
+
+ /* K(6x3) = temp_K(6x3) * invS(3x3)
+ */
+ for (i = 0; i < BAFL_SSIZE; i++) {
+ for (j = 0; j < 3; j++) {
+ bafl_K[i][j] = bafl_tempK[i][0] * bafl_invS[0][j];
+ bafl_K[i][j] += bafl_tempK[i][1] * bafl_invS[1][j];
+ bafl_K[i][j] += bafl_tempK[i][2] * bafl_invS[2][j];
+ }
+ }
+
+ /**********************************************
+ * Update filter state.
+ *
+ * The a priori filter state is zero since the errors are corrected
after each update.
+ *
+ * X = X_prio + K (y - H * X_prio)
+ * X = K * y
+ **********************************************/
+
+ /*printf("R = ");
+ for (i = 0; i < 3; i++) {
+ printf("[");
+ for (j = 0; j < 3; j++) {
+ printf("%f\t", RMAT_ELMT(bafl_dcm, i, j));
+ }
+ printf("]\n ");
+ }
+ printf("\n");*/
+
+ /* innovation
+ * y = Cnb * -[0; 0; g] - accel
+ */
+ bafl_ya.x = -RMAT_ELMT(bafl_dcm, 0, 2) * BAFL_g - bafl_accel_measure.x;
+ bafl_ya.y = -RMAT_ELMT(bafl_dcm, 1, 2) * BAFL_g - bafl_accel_measure.y;
+ bafl_ya.z = -RMAT_ELMT(bafl_dcm, 2, 2) * BAFL_g - bafl_accel_measure.z;
+
+ /* X(6) = K(6x3) * y(3)
+ */
+ for (i = 0; i < BAFL_SSIZE; i++) {
+ bafl_X[i] = bafl_K[i][0] * bafl_ya.x;
+ bafl_X[i] += bafl_K[i][1] * bafl_ya.y;
+ bafl_X[i] += bafl_K[i][2] * bafl_ya.z;
+ }
+
+ /**********************************************
+ * Update the filter covariance.
+ *
+ * P = P_prio - K * H * P_prio
+ * P = ( I - K * H ) * P_prio
+ *
+ **********************************************/
+
+ /* temp(6x6) = I(6x6) - K(6x3)*H(3x6)
+ *
+ * last 4 columns of H are zero
+ */
+ for (i = 0; i < BAFL_SSIZE; i++) {
+ for (j = 0; j < BAFL_SSIZE; j++) {
+ if (i == j) {
+ bafl_tempP[i][j] = 1.;
+ } else {
+ bafl_tempP[i][j] = 0.;
+ }
+ if (j < 2) { /* omit the parts where H is zero */
+ for (k = 0; k < 3; k++) {
+ bafl_tempP[i][j] -= bafl_K[i][k] *
bafl_H[k][j];
+ }
+ }
+ }
+ }
+
+ /* P(6x6) = temp(6x6) * P_prio(6x6)
+ */
+ for (i = 0; i < BAFL_SSIZE; i++) {
+ for (j = 0; j < BAFL_SSIZE; j++) {
+ bafl_P[i][j] = bafl_tempP[i][0] * bafl_Pprio[0][j];
+ for (k = 1; k < BAFL_SSIZE; k++) {
+ bafl_P[i][j] += bafl_tempP[i][k] *
bafl_Pprio[k][j];
+ }
+ }
+ }
+
+#ifdef LKF_PRINT_P
+ printf("Pua=");
+ for (i = 0; i < 6; i++) {
+ printf("[");
+ for (j = 0; j < 6; j++) {
+ printf("%f\t", bafl_P[i][j]);
+ }
+ printf("]\n ");
+ }
+ printf("\n");
+#endif
+
+ /**********************************************
+ * Correct errors.
+ *
+ *
+ **********************************************/
+
+ /* Error quaternion.
+ */
+ QUAT_ASSIGN(bafl_q_a_err, 1.0, bafl_X[0]/2, bafl_X[1]/2, bafl_X[2]/2);
+ FLOAT_QUAT_INVERT(bafl_q_a_err, bafl_q_a_err);
+
+ /* normalize
+ * Is this needed? Or is the approximation good enough?*/
+ float q_sq;
+ q_sq = bafl_q_a_err.qx * bafl_q_a_err.qx + bafl_q_a_err.qy *
bafl_q_a_err.qy + bafl_q_a_err.qz * bafl_q_a_err.qz;
+ if (q_sq > 1) { /* this should actually never happen */
+ FLOAT_QUAT_SMUL(bafl_q_a_err, bafl_q_a_err, 1 / sqrtf(1 +
q_sq));
+ printf("Accel error quaternion q_sq > 1!!\n");
+ } else {
+ bafl_q_a_err.qi = sqrtf(1 - q_sq);
+ }
+
+ /* correct attitude
+ */
+ FLOAT_QUAT_COMP(bafl_qtemp, bafl_q_a_err, bafl_quat);
+ FLOAT_QUAT_COPY(bafl_quat, bafl_qtemp);
+
+
+ /* correct gyro bias
+ */
+ RATES_ASSIGN(bafl_b_a_err, bafl_X[3], bafl_X[4], bafl_X[5]);
+ RATES_SUB(bafl_bias, bafl_b_a_err);
+
+ /*
+ * compute all representations
+ */
+ /* maintain rotation matrix representation */
+ FLOAT_RMAT_OF_QUAT(bafl_dcm, bafl_quat);
+ /* maintain euler representation */
+ FLOAT_EULERS_OF_RMAT(bafl_eulers, bafl_dcm);
+ AHRS_TO_BFP();
+ AHRS_LTP_TO_BODY();
+}
+
+
+void ahrs_update_mag(void) {
+ RunOnceEvery(10, ahrs_do_update_mag());
+}
+
+static void ahrs_do_update_mag(void) {
+ int i, j, k;
+
+#ifdef BAFL_DEBUG2
+ printf("Mag update.\n");
+#endif
+
+ MAGS_FLOAT_OF_BFP(bafl_mag, imu.mag);
+
+ /* P_prio = P */
+ for (i = 0; i < BAFL_SSIZE; i++) {
+ for (j = 0; j < BAFL_SSIZE; j++) {
+ bafl_Pprio[i][j] = bafl_P[i][j];
+ }
+ }
+
+ /*
+ * set up measurement matrix
+ *
+ * h = [236.; -2.; 396.];
+ * hx = [ h(2); -h(1); 0]; //only psi (phi and theta = 0)
+ * Hm = Cnb * hx;
+ * H = [ 0 0 0 0 0
+ * 0 0 Cnb*hx 0 0 0
+ * 0 0 0 0 0 ];
+ * */
+ /*bafl_H[0][2] = RMAT_ELMT(bafl_dcm, 0, 0) * bafl_h.y -
RMAT_ELMT(bafl_dcm, 0, 1) * bafl_h.x;
+ bafl_H[1][2] = RMAT_ELMT(bafl_dcm, 1, 0) * bafl_h.y -
RMAT_ELMT(bafl_dcm, 1, 1) * bafl_h.x;
+ bafl_H[2][2] = RMAT_ELMT(bafl_dcm, 2, 0) * bafl_h.y -
RMAT_ELMT(bafl_dcm, 2, 1) * bafl_h.x;*/
+ bafl_H[0][2] = -RMAT_ELMT(bafl_dcm, 0, 1);
+ bafl_H[1][2] = -RMAT_ELMT(bafl_dcm, 1, 1);
+ bafl_H[2][2] = -RMAT_ELMT(bafl_dcm, 2, 1);
+ //rest is zero
+
+ /**********************************************
+ * compute Kalman gain K
+ *
+ * K = P_prio * H_T * inv(S)
+ * S = H * P_prio * HT + R
+ *
+ * K = P_prio * HT * inv(H * P_prio * HT + R)
+ *
+ **********************************************/
+
+ /* covariance residual S = H * P_prio * HT + R */
+
+ /* temp_S(3x6) = H(3x6) * P_prio(6x6)
+ *
+ * only third column of H is non-zero
+ */
+ for (i = 0; i < 3; i++) {
+ for (j = 0; j < BAFL_SSIZE; j++) {
+ bafl_tempS[i][j] = bafl_H[i][2] * bafl_Pprio[2][j];
+ }
+ }
+
+ /* S(3x3) = temp_S(3x6) * HT(6x3) + R(3x3)
+ *
+ * only third row of HT is non-zero
+ */
+ for (i = 0; i < 3; i++) {
+ for (j = 0; j < 3; j++) {
+ bafl_S[i][j] = bafl_tempS[i][2] * bafl_H[j][2]; /*
H[j][2] = HT[2][j] */
+ }
+ bafl_S[i][i] += bafl_R_mag;
+ }
+
+ /* invert S
+ */
+ FLOAT_MAT33_INVERT(bafl_invS, bafl_S);
+
+ /* temp_K(6x3) = P_prio(6x6) * HT(6x3)
+ *
+ * only third row of HT is non-zero
+ */
+ for (i = 0; i < BAFL_SSIZE; i++) {
+ for (j = 0; j < 3; j++) {
+ /* not computing zero elements */
+ bafl_tempK[i][j] = bafl_Pprio[i][2] * bafl_H[j][2]; /*
H[j][2] = HT[2][j] */
+ }
+ }
+
+ /* K(6x3) = temp_K(6x3) * invS(3x3)
+ */
+ for (i = 0; i < BAFL_SSIZE; i++) {
+ for (j = 0; j < 3; j++) {
+ bafl_K[i][j] = bafl_tempK[i][0] * bafl_invS[0][j];
+ bafl_K[i][j] += bafl_tempK[i][1] * bafl_invS[1][j];
+ bafl_K[i][j] += bafl_tempK[i][2] * bafl_invS[2][j];
+ }
+ }
+
+ /**********************************************
+ * Update filter state.
+ *
+ * The a priori filter state is zero since the errors are corrected
after each update.
+ *
+ * X = X_prio + K (y - H * X_prio)
+ * X = K * y
+ **********************************************/
+
+ /* innovation
+ * y = Cnb * [hx; hy; hz] - mag
+ */
+ FLOAT_RMAT_VECT3_MUL(bafl_ym, bafl_dcm, bafl_h); //can be optimized
+ FLOAT_VECT3_SUB(bafl_ym, bafl_mag);
+
+ /* X(6) = K(6x3) * y(3)
+ */
+ for (i = 0; i < BAFL_SSIZE; i++) {
+ bafl_X[i] = bafl_K[i][0] * bafl_ym.x;
+ bafl_X[i] += bafl_K[i][1] * bafl_ym.y;
+ bafl_X[i] += bafl_K[i][2] * bafl_ym.z;
+ }
+
+ /**********************************************
+ * Update the filter covariance.
+ *
+ * P = P_prio - K * H * P_prio
+ * P = ( I - K * H ) * P_prio
+ *
+ **********************************************/
+
+ /* temp(6x6) = I(6x6) - K(6x3)*H(3x6)
+ *
+ * last 3 columns of H are zero
+ */
+ for (i = 0; i < 6; i++) {
+ for (j = 0; j < 6; j++) {
+ if (i == j) {
+ bafl_tempP[i][j] = 1.;
+ } else {
+ bafl_tempP[i][j] = 0.;
+ }
+ if (j == 2) { /* omit the parts where H is zero */
+ for (k = 0; k < 3; k++) {
+ bafl_tempP[i][j] -= bafl_K[i][k] *
bafl_H[k][j];
+ }
+ }
+ }
+ }
+
+ /* P(6x6) = temp(6x6) * P_prio(6x6)
+ */
+ for (i = 0; i < BAFL_SSIZE; i++) {
+ for (j = 0; j < BAFL_SSIZE; j++) {
+ bafl_P[i][j] = bafl_tempP[i][0] * bafl_Pprio[0][j];
+ for (k = 1; k < BAFL_SSIZE; k++) {
+ bafl_P[i][j] += bafl_tempP[i][k] *
bafl_Pprio[k][j];
+ }
+ }
+ }
+
+#ifdef LKF_PRINT_P
+ printf("Pum=");
+ for (i = 0; i < 6; i++) {
+ printf("[");
+ for (j = 0; j < 6; j++) {
+ printf("%f\t", bafl_P[i][j]);
+ }
+ printf("]\n ");
+ }
+ printf("\n");
+#endif
+
+ /**********************************************
+ * Correct errors.
+ *
+ *
+ **********************************************/
+
+ /* Error quaternion.
+ */
+ QUAT_ASSIGN(bafl_q_m_err, 1.0, bafl_X[0]/2, bafl_X[1]/2, bafl_X[2]/2);
+ FLOAT_QUAT_INVERT(bafl_q_m_err, bafl_q_m_err);
+ /* normalize */
+ float q_sq;
+ q_sq = bafl_q_m_err.qx * bafl_q_m_err.qx + bafl_q_m_err.qy *
bafl_q_m_err.qy + bafl_q_m_err.qz * bafl_q_m_err.qz;
+ if (q_sq > 1) { /* this should actually never happen */
+ FLOAT_QUAT_SMUL(bafl_q_m_err, bafl_q_m_err, 1 / sqrtf(1 +
q_sq));
+ printf("mag error quaternion q_sq > 1!!\n");
+ } else {
+ bafl_q_m_err.qi = sqrtf(1 - q_sq);
+ }
+
+ /* correct attitude
+ */
+ FLOAT_QUAT_COMP(bafl_qtemp, bafl_q_m_err, bafl_quat);
+ FLOAT_QUAT_COPY(bafl_quat, bafl_qtemp);
+
+ /* correct gyro bias
+ */
+ RATES_ASSIGN(bafl_b_m_err, bafl_X[3], bafl_X[4], bafl_X[5]);
+ RATES_SUB(bafl_bias, bafl_b_m_err);
+
+ /*
+ * compute all representations
+ */
+ /* maintain rotation matrix representation */
+ FLOAT_RMAT_OF_QUAT(bafl_dcm, bafl_quat);
+ /* maintain euler representation */
+ FLOAT_EULERS_OF_RMAT(bafl_eulers, bafl_dcm);
+ AHRS_TO_BFP();
+ AHRS_LTP_TO_BODY();
+}
+
+void ahrs_update(void) {
+ ahrs_update_accel();
+ ahrs_update_mag();
+}
Copied: paparazzi3/trunk/sw/airborne/subsystems/ahrs/ahrs_float_lkf.h (from rev
6228, paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs/ahrs_float_lkf.h)
===================================================================
--- paparazzi3/trunk/sw/airborne/subsystems/ahrs/ahrs_float_lkf.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/subsystems/ahrs/ahrs_float_lkf.h
2010-10-25 14:10:07 UTC (rev 6229)
@@ -0,0 +1,71 @@
+/*
+ * $Id: $
+ *
+ * Copyright (C) 2009 Felix Ruess <address@hidden>
+ * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
+ *
+ * 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.
+ */
+
+#ifndef AHRS_FLOAT_LKF_H
+#define AHRS_FLOAT_LKF_H
+
+#include <subsystems/ahrs.h>
+#include "std.h"
+#include "math/pprz_algebra_int.h"
+
+extern struct FloatQuat bafl_quat;
+extern struct FloatRates bafl_bias;
+extern struct FloatRates bafl_rates;
+extern struct FloatEulers bafl_eulers;
+extern struct FloatRMat bafl_dcm;
+
+extern struct FloatQuat bafl_q_a_err;
+extern struct FloatQuat bafl_q_m_err;
+extern struct FloatRates bafl_b_a_err;
+extern struct FloatRates bafl_b_m_err;
+extern float bafl_qnorm;
+extern float bafl_phi_accel;
+extern float bafl_theta_accel;
+extern struct FloatVect3 bafl_accel_measure;
+extern struct FloatVect3 bafl_mag;
+
+#define BAFL_SSIZE 6
+extern float bafl_P[BAFL_SSIZE][BAFL_SSIZE];
+extern float bafl_X[BAFL_SSIZE];
+
+extern float bafl_sigma_accel;
+extern float bafl_sigma_mag;
+extern float bafl_R_accel;
+extern float bafl_R_mag;
+
+extern float bafl_Q_att;
+extern float bafl_Q_gyro;
+
+
+#define ahrs_float_lkf_SetRaccel(_v) { \
+ bafl_sigma_accel = _v; \
+ bafl_R_accel = _v * _v; \
+}
+#define ahrs_float_lkf_SetRmag(_v) { \
+ bafl_sigma_mag = _v; \
+ bafl_R_mag = _v * _v; \
+}
+
+#endif /* AHRS_FLOAT_LKF_H */
+
Copied: paparazzi3/trunk/sw/airborne/subsystems/ahrs.c (from rev 6228,
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs.c)
===================================================================
--- paparazzi3/trunk/sw/airborne/subsystems/ahrs.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/subsystems/ahrs.c 2010-10-25 14:10:07 UTC
(rev 6229)
@@ -0,0 +1,31 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2009 Felix Ruess <address@hidden>
+ *
+ * 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.
+ */
+
+#include <subsystems/ahrs.h>
+#include <subsystems/imu.h>
+
+struct Ahrs ahrs;
+struct AhrsFloat ahrs_float;
+
+float ahrs_mag_offset;
+
Copied: paparazzi3/trunk/sw/airborne/subsystems/ahrs.h (from rev 6228,
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ahrs.h)
===================================================================
--- paparazzi3/trunk/sw/airborne/subsystems/ahrs.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/subsystems/ahrs.h 2010-10-25 14:10:07 UTC
(rev 6229)
@@ -0,0 +1,91 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
+ *
+ * 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.
+ */
+
+#ifndef AHRS_H
+#define AHRS_H
+
+#include "std.h"
+#include "math/pprz_algebra_int.h"
+#include "math/pprz_algebra_float.h"
+#include <subsystems/ahrs/ahrs_aligner.h>
+
+#define AHRS_UNINIT 0
+#define AHRS_RUNNING 1
+
+struct Ahrs {
+
+ struct Int32Quat ltp_to_imu_quat;
+ struct Int32Eulers ltp_to_imu_euler;
+ struct Int32RMat ltp_to_imu_rmat;
+ struct Int32Rates imu_rate;
+
+ struct Int32Quat ltp_to_body_quat;
+ struct Int32Eulers ltp_to_body_euler;
+ struct Int32RMat ltp_to_body_rmat;
+ struct Int32Rates body_rate;
+
+ uint8_t status;
+};
+
+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;
+
+ uint8_t status;
+};
+
+extern struct Ahrs ahrs;
+extern struct AhrsFloat ahrs_float;
+
+extern float ahrs_mag_offset;
+
+#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 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 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 /* AHRS_H */
Modified: paparazzi3/trunk/sw/simulator/nps/nps_autopilot_booz.c
===================================================================
--- paparazzi3/trunk/sw/simulator/nps/nps_autopilot_booz.c 2010-10-25
12:46:09 UTC (rev 6228)
+++ paparazzi3/trunk/sw/simulator/nps/nps_autopilot_booz.c 2010-10-25
14:10:07 UTC (rev 6229)
@@ -81,7 +81,7 @@
}
#include "nps_fdm.h"
-#include <firmwares/rotorcraft/ahrs.h>
+#include <subsystems/ahrs.h>
#include "math/pprz_algebra.h"
void sim_overwrite_ahrs(void) {
Modified: paparazzi3/trunk/sw/simulator/old_booz/booz2_sim_main.c
===================================================================
--- paparazzi3/trunk/sw/simulator/old_booz/booz2_sim_main.c 2010-10-25
12:46:09 UTC (rev 6228)
+++ paparazzi3/trunk/sw/simulator/old_booz/booz2_sim_main.c 2010-10-25
14:10:07 UTC (rev 6229)
@@ -125,7 +125,7 @@
}
-#include <firmwares/rotorcraft/ahrs.h>
+#include <subsystems/ahrs.h>
static void sim_run_one_step(void) {
@@ -195,7 +195,7 @@
#ifdef BYPASS_AHRS
#include "booz_geometry_mixed.h"
-#include <firmwares/rotorcraft/ahrs.h>
+#include <subsystems/ahrs.h>
static void sim_overwrite_ahrs(void) {
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]);
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [6229] moved ahrs to general subsystems dir,
Felix Ruess <=