[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [4045] use gps accuracy information for measurement
From: |
Felix Ruess |
Subject: |
[paparazzi-commits] [4045] use gps accuracy information for measurement noise in hfilter. |
Date: |
Wed, 02 Sep 2009 12:09:39 +0000 |
Revision: 4045
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4045
Author: flixr
Date: 2009-09-02 12:09:38 +0000 (Wed, 02 Sep 2009)
Log Message:
-----------
use gps accuracy information for measurement noise in hfilter.
define USE_GPS4R to use it
Modified Paths:
--------------
paparazzi3/trunk/conf/airframes/booz2_flixr.xml
paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.c
Modified: paparazzi3/trunk/conf/airframes/booz2_flixr.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/booz2_flixr.xml 2009-09-02 07:15:08 UTC
(rev 4044)
+++ paparazzi3/trunk/conf/airframes/booz2_flixr.xml 2009-09-02 12:09:38 UTC
(rev 4045)
@@ -205,7 +205,7 @@
include $(CFG_BOOZ)/subsystems/booz2_ins_hff.makefile
#set GPS lag for horizontal filter
- ap.CFLAGS += -DGPS_LAG=0.8
+ ap.CFLAGS += -DGPS_LAG=0.8 -DUSE_GPS_ACC4R
</makefile>
Modified: paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.c 2009-09-02
07:15:08 UTC (rev 4044)
+++ paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.c 2009-09-02
12:09:38 UTC (rev 4045)
@@ -26,6 +26,7 @@
#include "booz2_ins.h"
#include "booz_imu.h"
#include "booz_ahrs.h"
+#include "booz2_gps.h"
#include <stdlib.h>
#ifdef SITL
@@ -47,9 +48,15 @@
#define Q ACCEL_NOISE*DT_HFILTER*DT_HFILTER/2.
#define Qdotdot ACCEL_NOISE*DT_HFILTER
//TODO: proper measurement noise
-#define Rpos 5.
-#define Rspeed 1.
+#define R_POS 7.
+#define R_POS_MAX 20.
+#define R_POS_MIN 2.
+#define R_SPEED 2.
+#define R_SPEED_MAX 10.
+#define R_SPEED_MIN 0.2
+float Rpos, Rspeed;
+
/*
X_x = [ x xdot]
@@ -162,6 +169,8 @@
void b2_hff_init(float init_x, float init_xdot, float init_y, float init_ydot)
{
+ Rpos = R_POS;
+ Rspeed = R_SPEED;
b2_hff_init_x(init_x, init_xdot);
b2_hff_init_y(init_y, init_ydot);
#ifdef GPS_LAG
@@ -390,6 +399,19 @@
void b2_hff_update_gps(void) {
+#ifdef USE_GPS_ACC4R
+ Rpos = (float) booz_gps_state.pacc / 100.;
+ if (Rpos > R_POS_MAX)
+ Rpos = R_POS_MAX;
+ else if (Rpos < R_POS_MIN)
+ Rpos = R_POS_MIN;
+ Rspeed = (float) booz_gps_state.sacc / 100.;
+ if (Rspeed > R_SPEED_MAX)
+ Rspeed = R_SPEED_MAX;
+ else if (Rspeed < R_SPEED_MIN)
+ Rspeed = R_SPEED_MIN;
+#endif
+
#ifdef GPS_LAG
if (GPS_LAG_N == 0) {
#endif
[Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [4045] use gps accuracy information for measurement noise in hfilter.,
Felix Ruess <=