Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
ins_alt_float.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2004-2012 The Paparazzi Team
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
27 #include "subsystems/ins.h"
28 
29 #include <inttypes.h>
30 #include <math.h>
31 
32 #include "state.h"
33 #include "subsystems/gps.h"
34 #include "subsystems/nav.h"
35 
36 #include "generated/airframe.h"
37 
38 #ifdef DEBUG_ALT_KALMAN
39 #include "mcu_periph/uart.h"
40 #include "ap_downlink.h"
41 #endif
42 
43 /* vertical position and speed in meters (z-up)*/
44 float ins_alt;
46 
47 #if USE_BAROMETER
48 #include "generated/modules.h"
53 #endif
54 
55 void ins_init() {
56 
59 
60  stateSetPositionUtm_f(&utm0);
61 
63 
64 #if USE_BAROMETER
65  ins_qfe = 0;;
67  ins_baro_alt = 0.;
68 #endif
70 
71  EstimatorSetAlt(0.);
72 
73 }
74 
75 void ins_periodic( void ) {
76 }
77 
78 void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), struct FloatVect2 speed __attribute__ ((unused))) {
79 }
80 
81 void ins_realign_v(float z __attribute__ ((unused))) {
82 }
83 
84 void ins_propagate() {
85 }
86 
88 #if USE_BAROMETER
89  // TODO update kalman filter with baro struct
90  if (baro.status == BS_RUNNING) {
91  if (!ins_baro_initialised) {
94  }
95  if (ins.vf_realign) {
98  }
99  else { /* not realigning, so normal update with baro measurement */
100  /* altitude decreases with increasing baro.absolute pressure */
101  ins_baro_alt = ground_alt - (baro.absolute - ins_qfe) * INS_BARO_SENS;
102  /* run the filter */
104  /* set new altitude, just copy old horizontal position */
105  struct UtmCoor_f utm;
107  utm.alt = ins_alt;
108  stateSetPositionUtm_f(&utm);
109  struct NedCoor_f ned_vel;
110  memcpy(&ned_vel, stateGetSpeedNed_f(), sizeof(struct NedCoor_f));
111  ned_vel.z = -ins_alt_dot;
112  stateSetSpeedNed_f(&ned_vel);
113  }
114  }
115 #endif
116 }
117 
118 
119 void ins_update_gps(void) {
120 #if USE_GPS
121  struct UtmCoor_f utm;
122  utm.east = gps.utm_pos.east / 100.;
123  utm.north = gps.utm_pos.north / 100.;
124  utm.zone = nav_utm_zone0;
125 
126 #if !USE_BAROMETER
127  float falt = gps.hmsl / 1000.;
128  EstimatorSetAlt(falt);
129 #endif
130  utm.alt = ins_alt;
131  // set position
132  stateSetPositionUtm_f(&utm);
133 
134  struct NedCoor_f ned_vel = {
135  gps.ned_vel.x / 100.,
136  gps.ned_vel.y / 100.,
137  -ins_alt_dot
138  };
139  // set velocity
140  stateSetSpeedNed_f(&ned_vel);
141 
142 #endif
143 }
144 
146 }
147 
149 
150 #ifndef ALT_KALMAN_ENABLED
151 #define ALT_KALMAN_ENABLED FALSE
152 #endif
153 
154 #ifndef GPS_DT
155 #define GPS_DT 0.25
156 #endif
157 #define GPS_SIGMA2 1.
158 #define GPS_R 2.
159 
160 #define BARO_DT 0.1
161 
162 static float p[2][2];
163 
164 void alt_kalman_reset( void ) {
165  p[0][0] = 1.;
166  p[0][1] = 0.;
167  p[1][0] = 0.;
168  p[1][1] = 1.;
169 }
170 
171 void alt_kalman_init( void ) {
172  alt_kalman_enabled = ALT_KALMAN_ENABLED;
174 }
175 
176 void alt_kalman(float z_meas) {
177  float DT;
178  float R;
179  float SIGMA2;
180 
181 #if USE_BAROMETER
182 #if USE_BARO_MS5534A
183  if (alt_baro_enabled) {
184  DT = BARO_DT;
185  R = baro_MS5534A_r;
186  SIGMA2 = baro_MS5534A_sigma2;
187  } else
188 #elif USE_BARO_ETS
189  if (baro_ets_enabled) {
190  DT = BARO_ETS_DT;
191  R = baro_ets_r;
192  SIGMA2 = baro_ets_sigma2;
193  } else
194 #elif USE_BARO_MS5611
195  if (baro_ms5611_enabled) {
196  DT = BARO_MS5611_DT;
197  R = baro_ms5611_r;
198  SIGMA2 = baro_ms5611_sigma2;
199  } else
200 #elif USE_BARO_BMP
201  if (baro_bmp_enabled) {
202  DT = BARO_BMP_DT;
203  R = baro_bmp_r;
204  SIGMA2 = baro_bmp_sigma2;
205  } else
206 #endif
207 #endif // USE_BAROMETER
208  {
209  DT = GPS_DT;
210  R = GPS_R;
211  SIGMA2 = GPS_SIGMA2;
212  }
213 
214  float q[2][2];
215  q[0][0] = DT*DT*DT*DT/4.;
216  q[0][1] = DT*DT*DT/2.;
217  q[1][0] = DT*DT*DT/2.;
218  q[1][1] = DT*DT;
219 
220 
221  /* predict */
222  ins_alt += ins_alt_dot * DT;
223  p[0][0] = p[0][0]+p[1][0]*DT+DT*(p[0][1]+p[1][1]*DT) + SIGMA2*q[0][0];
224  p[0][1] = p[0][1]+p[1][1]*DT + SIGMA2*q[0][1];
225  p[1][0] = p[1][0]+p[1][1]*DT + SIGMA2*q[1][0];
226  p[1][1] = p[1][1] + SIGMA2*q[1][1];
227 
228  /* error estimate */
229  float e = p[0][0] + R;
230 
231  if (fabs(e) > 1e-5) {
232  float k_0 = p[0][0] / e;
233  float k_1 = p[1][0] / e;
234  e = z_meas - ins_alt;
235 
236  /* correction */
237  ins_alt += k_0 * e;
238  ins_alt_dot += k_1 * e;
239 
240  p[1][0] = -p[0][0]*k_1+p[1][0];
241  p[1][1] = -p[0][1]*k_1+p[1][1];
242  p[0][0] = p[0][0] * (1-k_0);
243  p[0][1] = p[0][1] * (1-k_0);
244  }
245 
246 #ifdef DEBUG_ALT_KALMAN
247  DOWNLINK_SEND_ALT_KALMAN(DefaultChannel,DefaultDevice,&(p[0][0]),&(p[0][1]),&(p[1][0]), &(p[1][1]));
248 #endif
249 }
250 
struct Ins ins
global INS state
Definition: ins.c:30
#define UTM_COPY(_u1, _u2)
Definition: pprz_geodetic.h:30
struct NedCoor_i ned_vel
speed NED in cm/s
Definition: gps.h:69
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
float ins_alt_dot
estimated vertical speed in m/s (positive-up)
Definition: ins_alt_float.c:45
#define BARO_DT
Common barometric sensor implementation.
void ins_update_gps(void)
Update INS state with GPS measurements.
int32_t ins_qfe
Definition: ins_alt_float.c:50
bool_t baro_ms5611_enabled
void ins_realign_v(float z)
INS vertical realign.
Definition: ins_alt_float.c:81
#define GPS_R
int32_t x
North.
uint8_t nav_utm_zone0
Definition: common_nav.c:44
Integrated Navigation System interface.
int32_t hmsl
height above mean sea level in mm
Definition: gps.h:67
float baro_ets_r
Definition: baro_ets.c:85
bool_t baro_bmp_enabled
Definition: baro_bmp.c:68
#define FALSE
Definition: imu_chimu.h:141
#define BARO_ETS_DT
new measurement every baro_ets_read_periodic
Definition: baro_ets.h:49
static void stateSetPositionUtm_f(struct UtmCoor_f *utm_pos)
Set position from UTM coordinates (float).
Definition: state.h:555
int32_t absolute
Definition: baro.h:41
static void stateSetSpeedNed_f(struct NedCoor_f *ned_speed)
Set ground speed in local NED coordinates (float).
Definition: state.h:754
uint8_t zone
UTM zone number.
float baro_ms5611_r
int32_t nav_utm_north0
Definition: common_nav.c:43
bool_t ins_baro_initialised
Definition: ins_alt_float.c:51
vector in North East Down coordinates Units: meters
static struct NedCoor_f * stateGetSpeedNed_f(void)
Get ground speed in local NED coordinates (float).
Definition: state.h:833
float north
in meters
Device independent GPS code (interface)
void alt_kalman_reset(void)
bool_t alt_kalman_enabled
float ins_baro_alt
Definition: ins_alt_float.c:52
float z
in meters
position in UTM coordinates Units: meters
bool_t alt_baro_enabled
Definition: sim_baro.c:7
float baro_MS5534A_r
Definition: sim_baro.c:13
int32_t y
East.
float baro_bmp_sigma2
Definition: baro_bmp.c:70
int32_t north
in centimeters
float baro_bmp_r
Definition: baro_bmp.c:69
#define BARO_BMP_DT
new measurement every 3rd baro_bmp_periodic
Definition: baro_bmp.h:39
signed long int32_t
Definition: types.h:19
#define TRUE
Definition: imu_chimu.h:144
bool_t baro_ets_enabled
Definition: baro_ets.c:84
#define GPS_SIGMA2
struct Baro baro
Definition: baro_board.c:36
#define BARO_MS5611_DT
new measurement every baro_ms5611_periodic
int32_t east
in centimeters
void ins_propagate()
Propagation.
Definition: ins_alt_float.c:84
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition: state.h:651
float ins_alt
estimated altitude above MSL in meters
Definition: ins_alt_float.c:44
API to get/set the generic vehicle states.
float baro_ets_sigma2
Definition: baro_ets.c:86
#define EstimatorSetAlt(z)
Definition: ins_alt_float.h:71
void ins_update_baro()
Update INS state with barometer measurements.
Definition: ins_alt_float.c:87
void ins_update_sonar()
Update INS state with sonar measurements.
#define R
Definition: agl_vfilter.c:22
int32_t nav_utm_east0
Definition: common_nav.c:42
enum BaroStatus status
Definition: baro.h:43
void alt_kalman(float z_meas)
float baro_ms5611_sigma2
static float p[2][2]
void alt_kalman_init(void)
struct UtmCoor_i utm_pos
position in UTM (north,east: cm; alt: mm over ellipsoid)
Definition: gps.h:66
bool_t vf_realign
realign vertically if true
Definition: ins.h:47
#define ALT_KALMAN_ENABLED
void ins_periodic(void)
INS periodic call.
Definition: ins_alt_float.c:75
float east
in meters
float baro_MS5534A_sigma2
Definition: sim_baro.c:14
float alt
in meters above WGS84 reference ellipsoid
static void stateSetLocalUtmOrigin_f(struct UtmCoor_f *utm_def)
Set the local (flat earth) coordinate frame origin from UTM (float).
Definition: state.h:459
struct GpsState gps
global GPS state
Definition: gps.c:33
#define GPS_DT
void ins_init()
INS initialization.
Definition: ins_alt_float.c:55
void ins_realign_h(struct FloatVect2 pos, struct FloatVect2 speed)
INS horizontal realign.
Definition: ins_alt_float.c:78