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_int.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2010 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 
29 #include "subsystems/ins/ins_int.h"
30 
31 #include "subsystems/imu.h"
33 #include "subsystems/gps.h"
34 
35 #include "generated/airframe.h"
36 
37 #if USE_VFF
39 #endif
40 
41 #if USE_HFF
43 #endif
44 
45 #ifdef SITL
46 #include "nps_fdm.h"
47 #include <stdio.h>
48 #endif
49 
50 
51 #include "math/pprz_geodetic_int.h"
52 
53 #include "generated/flight_plan.h"
54 
55 #ifndef USE_INS_NAV_INIT
56 #define USE_INS_NAV_INIT TRUE
57 PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
58 #endif
59 
60 /* gps transformed to LTP-NED */
65 #if USE_HFF
66 /* horizontal gps transformed to NED in meters as float */
67 struct FloatVect2 ins_gps_pos_m_ned;
68 struct FloatVect2 ins_gps_speed_m_s_ned;
69 #endif
70 
71 /* barometer */
72 #if USE_VFF
76 #if USE_SONAR
77 bool_t ins_update_on_agl;
79 #endif
80 #endif
81 
82 /* output */
86 
87 
88 void ins_init() {
89 #if USE_INS_NAV_INIT
90  ins_ltp_initialised = TRUE;
91 
93  struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
94  llh_nav0.lat = INT32_RAD_OF_DEG(NAV_LAT0);
95  llh_nav0.lon = INT32_RAD_OF_DEG(NAV_LON0);
96  /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
97  llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
98 
99  struct EcefCoor_i ecef_nav0;
100  ecef_of_lla_i(&ecef_nav0, &llh_nav0);
101 
102  ltp_def_from_ecef_i(&ins_ltp_def, &ecef_nav0);
103  ins_ltp_def.hmsl = NAV_ALT0;
105 #else
106  ins_ltp_initialised = FALSE;
107 #endif
108 #if USE_VFF
109  ins_baro_initialised = FALSE;
110 #if USE_SONAR
111  ins_update_on_agl = FALSE;
112 #endif
113  vff_init(0., 0., 0.);
114 #endif
115  ins.vf_realign = FALSE;
116  ins.hf_realign = FALSE;
117 #if USE_HFF
118  b2_hff_init(0., 0., 0., 0.);
119 #endif
123 
124  // TODO correct init
126 
127 }
128 
129 void ins_periodic( void ) {
130 }
131 
132 void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), struct FloatVect2 speed __attribute__ ((unused))) {
133 #if USE_HFF
134  b2_hff_realign(pos, speed);
135 #endif /* USE_HFF */
136 }
137 
138 void ins_realign_v(float z __attribute__ ((unused))) {
139 #if USE_VFF
140  vff_realign(z);
141 #endif
142 }
143 
145  /* untilt accels */
146  struct Int32Vect3 accel_meas_body;
148  struct Int32Vect3 accel_meas_ltp;
149  INT32_RMAT_TRANSP_VMULT(accel_meas_ltp, (*stateGetNedToBodyRMat_i()), accel_meas_body);
150 
151 #if USE_VFF
152  float z_accel_meas_float = ACCEL_FLOAT_OF_BFP(accel_meas_ltp.z);
153  if (baro.status == BS_RUNNING && ins_baro_initialised) {
154  vff_propagate(z_accel_meas_float);
158  }
159  else { // feed accel from the sensors
160  // subtract -9.81m/s2 (acceleration measured due to gravity, but vehivle not accelerating in ltp)
161  ins_ltp_accel.z = accel_meas_ltp.z + ACCEL_BFP_OF_REAL(9.81);
162  }
163 #else
164  ins_ltp_accel.z = accel_meas_ltp.z + ACCEL_BFP_OF_REAL(9.81);
165 #endif /* USE_VFF */
166 
167 #if USE_HFF
168  /* propagate horizontal filter */
170 #else
171  ins_ltp_accel.x = accel_meas_ltp.x;
172  ins_ltp_accel.y = accel_meas_ltp.y;
173 #endif /* USE_HFF */
174 
176 }
177 
179 #if USE_VFF
180  if (baro.status == BS_RUNNING) {
181  if (!ins_baro_initialised) {
182  ins_qfe = baro.absolute;
183  ins_baro_initialised = TRUE;
184  }
185  if (ins.vf_realign) {
186  ins.vf_realign = FALSE;
187  ins_qfe = baro.absolute;
188 #if USE_SONAR
189  ins_sonar_offset = sonar_meas;
190 #endif
191  vff_realign(0.);
195  }
196  else { /* not realigning, so normal update with baro measurement */
197  ins_baro_alt = ((baro.absolute - ins_qfe) * INS_BARO_SENS_NUM)/INS_BARO_SENS_DEN;
198  float alt_float = POS_FLOAT_OF_BFP(ins_baro_alt);
199  vff_update(alt_float);
200  }
201  }
203 #endif
204 }
205 
206 
207 void ins_update_gps(void) {
208 #if USE_GPS
209  if (gps.fix == GPS_FIX_3D) {
210  if (!ins_ltp_initialised) {
214  ins_ltp_initialised = TRUE;
216  }
219 #if USE_HFF
221  VECT2_SDIV(ins_gps_pos_m_ned, ins_gps_pos_m_ned, 100.);
223  VECT2_SDIV(ins_gps_speed_m_s_ned, ins_gps_speed_m_s_ned, 100.);
224  if (ins.hf_realign) {
225  ins.hf_realign = FALSE;
226 #ifdef SITL
227  struct FloatVect2 true_pos, true_speed;
228  VECT2_COPY(true_pos, fdm.ltpprz_pos);
229  VECT2_COPY(true_speed, fdm.ltpprz_ecef_vel);
230  b2_hff_realign(true_pos, true_speed);
231 #else
232  const struct FloatVect2 zero = {0.0, 0.0};
233  b2_hff_realign(ins_gps_pos_m_ned, zero);
234 #endif
235  }
237 #if !USE_VFF /* vff not used */
240 #endif /* vff not used */
241 #endif /* hff used */
242 
243 
244 #if !USE_HFF /* hff not used */
245 #if !USE_VFF /* neither hf nor vf used */
248 #else /* only vff used */
251 #endif
252 
253 #if USE_GPS_LAG_HACK
254  VECT2_COPY(d_pos, ins_ltp_speed);
255  INT32_VECT2_RSHIFT(d_pos, d_pos, 11);
256  VECT2_ADD(ins_ltp_pos, d_pos);
257 #endif
258 #endif /* hff not used */
259 
261  }
262 #endif /* USE_GPS */
263 }
264 
266 #if USE_SONAR && USE_VFF
267  static int32_t sonar_filtered = 0;
268  sonar_filtered = (sonar_meas + 2*sonar_filtered) / 3;
269  /* update baro_qfe assuming a flat ground */
270  if (ins_update_on_agl && baro.status == BS_RUNNING) {
271  int32_t d_sonar = (((int32_t)sonar_filtered - ins_sonar_offset) * INS_SONAR_SENS_NUM) / INS_SONAR_SENS_DEN;
272  ins_qfe = baro.absolute + (d_sonar * (INS_BARO_SENS_DEN))/INS_BARO_SENS_NUM;
273  }
274 #endif
275 }
void ins_init()
INS initialization.
Definition: ins_int.c:88
struct Ins ins
global INS state
Definition: ins.c:30
struct LlaCoor_i lla_pos
position in LLA (lat,lon: rad*1e7; alt: mm over ellipsoid)
Definition: gps.h:65
#define INT32_VECT2_RSHIFT(_o, _i, _r)
int32_t lat
in radians*1e7
struct EcefCoor_i ecef_vel
speed ECEF in cm/s
Definition: gps.h:68
uint16_t sonar_meas
Raw ADC value.
Definition: sonar_adc.c:52
Common barometric sensor implementation.
Horizontal filter (x,y) to estimate position and velocity.
#define INT32_VECT2_SCALE_2(_a, _b, _num, _den)
vector in EarthCenteredEarthFixed coordinates
void b2_hff_realign(struct FloatVect2 pos, struct FloatVect2 vel)
Definition: hf_float.c:558
static struct Int32RMat * stateGetNedToBodyRMat_i(void)
Get vehicle body attitude rotation matrix (int).
Definition: state.h:1007
#define INS_RUNNING
Definition: ins.h:36
int32_t ins_qfe
Definition: ins_alt_float.c:50
struct NedCoor_i ins_ltp_speed
Definition: ins_int.c:84
#define ACCEL_BFP_OF_REAL(_af)
struct Int32RMat body_to_imu_rmat
rotation from body to imu frame as a rotation matrix
Definition: imu.h:52
float vff_z
int32_t ins_sonar_offset
struct Int32Vect3 accel
accelerometer measurements
Definition: imu.h:41
void ins_realign_v(float z)
INS vertical realign.
Definition: ins_int.c:138
int32_t x
North.
#define INT32_RMAT_TRANSP_VMULT(_vb, _m_b2a, _va)
struct LlaCoor_i lla
Reference point in lla.
int32_t hmsl
height above mean sea level in mm
Definition: gps.h:67
vector in Latitude, Longitude and Altitude
uint8_t fix
status of fix
Definition: gps.h:78
#define GPS_FIX_3D
Definition: gps.h:43
int32_t z
in centimeters
struct NedCoor_i ins_gps_pos_cm_ned
Definition: ins_int.c:63
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition: gps.h:64
bool_t hf_realign
realign horizontally if true
Definition: ins.h:46
void ins_update_gps(void)
Update INS state with GPS measurements.
Definition: ins_int.c:207
float vff_zdotdot
void ecef_of_lla_i(struct EcefCoor_i *out, struct LlaCoor_i *in)
#define FALSE
Definition: imu_chimu.h:141
Vertical filter (in float) estimating altitude, velocity and accel bias.
void ins_update_baro()
Update INS state with barometer measurements.
Definition: ins_int.c:178
vector in North East Down coordinates
int32_t z
Down.
int32_t absolute
Definition: baro.h:41
struct NedCoor_i ins_gps_speed_cm_s_ned
Definition: ins_int.c:64
uint8_t status
status of the INS
Definition: ins.h:45
definition of the local (flat earth) coordinate system
struct NedCoor_i ins_ltp_accel
Definition: ins_int.c:85
#define INS_NED_TO_STATE()
Definition: ins_int.h:73
#define INT32_RAD_OF_DEG(_deg)
#define INT32_SPEED_OF_CM_S_DEN
bool_t ins_baro_initialised
Definition: ins_alt_float.c:51
#define INT32_VECT3_ZERO(_v)
#define POS_FLOAT_OF_BFP(_ai)
static void stateSetLocalOrigin_i(struct LtpDef_i *ltp_def)
Set the local (flat earth) coordinate frame origin (int).
Definition: state.h:440
Device independent GPS code (interface)
struct Imu imu
global IMU state
Definition: imu_aspirin2.c:50
float ins_baro_alt
Definition: ins_alt_float.c:52
void ned_of_ecef_point_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct EcefCoor_i *ecef)
int32_t alt
in millimeters above WGS84 reference ellipsoid
void b2_hff_init(float init_x, float init_xdot, float init_y, float init_ydot)
Definition: hf_float.c:242
int32_t y
East.
void b2_hff_update_gps(void)
Definition: hf_float.c:493
Paparazzi fixed point math for geodetic calculations.
#define ACCEL_FLOAT_OF_BFP(_ai)
Inertial Measurement Unit interface.
#define INT32_POS_OF_CM_DEN
struct LtpDef_i ins_ltp_def
Ins implementation state (fixed point)
Definition: ins_int.c:61
signed long int32_t
Definition: types.h:19
void ins_update_sonar()
Update INS state with sonar measurements.
Definition: ins_int.c:265
#define TRUE
Definition: imu_chimu.h:144
void ltp_def_from_ecef_i(struct LtpDef_i *def, struct EcefCoor_i *ecef)
void vff_realign(float z_meas)
bool_t ins_update_on_agl
struct Baro baro
Definition: baro_board.c:36
#define VECT2_ADD(_a, _b)
Definition: pprz_algebra.h:49
#define INT32_SPEED_OF_CM_S_NUM
int32_t lon
in radians*1e7
int32_t hmsl
Height above mean sea level in mm.
void b2_hff_propagate(void)
Definition: hf_float.c:426
enum BaroStatus status
Definition: baro.h:43
void ned_of_ecef_vect_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct EcefCoor_i *ecef)
void vff_update(float z_meas)
Definition: vf_float.c:176
struct NedCoor_i ins_ltp_pos
Definition: ins_int.c:83
#define INT32_POS_OF_CM_NUM
void ins_propagate()
Propagation.
Definition: ins_int.c:144
void ins_periodic(void)
INS periodic call.
Definition: ins_int.c:129
bool_t vf_realign
realign vertically if true
Definition: ins.h:47
float vff_zdot
void vff_init(float init_z, float init_zdot, float init_accel_bias, float init_baro_offset)
#define VECT2_ASSIGN(_a, _x, _y)
Definition: pprz_algebra.h:37
#define VECT2_SDIV(_vo, _vi, _s)
Definition: pprz_algebra.h:79
bool_t ins_ltp_initialised
Definition: ins_int.c:62
void ins_realign_h(struct FloatVect2 pos, struct FloatVect2 speed)
INS horizontal realign.
Definition: ins_int.c:132
#define SPEED_BFP_OF_REAL(_af)
#define VECT2_COPY(_a, _b)
Definition: pprz_algebra.h:43
void vff_propagate(float accel)
struct GpsState gps
global GPS state
Definition: gps.c:33
INS for rotorcrafts combining vertical and horizontal filters.
#define POS_BFP_OF_REAL(_af)