Paparazzi UAS  v4.0.4_stable-3-gf39211a
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
ins.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 
22 #include "subsystems/ins.h"
23 
24 #include "subsystems/imu.h"
26 #include "subsystems/gps.h"
27 
28 #include "generated/airframe.h"
29 #include "math/pprz_algebra_int.h"
31 
32 #include "subsystems/ahrs.h"
33 
34 #if USE_VFF
36 #endif
37 
38 #if USE_HFF
40 #endif
41 
42 #ifdef SITL
43 #include "nps_fdm.h"
44 #include <stdio.h>
45 #endif
46 
47 
48 #include "math/pprz_geodetic_int.h"
49 
50 #include "generated/flight_plan.h"
51 
52 /* gps transformed to LTP-NED */
57 #if USE_HFF
58 /* horizontal gps transformed to NED in meters as float */
59 struct FloatVect2 ins_gps_pos_m_ned;
60 struct FloatVect2 ins_gps_speed_m_s_ned;
61 #endif
63 
64 /* barometer */
65 #if USE_VFF
66 int32_t ins_qfe;
67 bool_t ins_baro_initialised;
68 int32_t ins_baro_alt;
69 #if USE_SONAR
70 bool_t ins_update_on_agl;
71 int32_t ins_sonar_offset;
72 #endif
73 #endif
75 
76 /* output */
83 
84 
85 void ins_init() {
86 #if USE_INS_NAV_INIT
88 
90  struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
91  llh_nav0.lat = INT32_RAD_OF_DEG(NAV_LAT0);
92  llh_nav0.lon = INT32_RAD_OF_DEG(NAV_LON0);
93  /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
94  llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
95 
96  struct EcefCoor_i ecef_nav0;
97  ecef_of_lla_i(&ecef_nav0, &llh_nav0);
98 
99  ltp_def_from_ecef_i(&ins_ltp_def, &ecef_nav0);
100  ins_ltp_def.hmsl = NAV_ALT0;
101 #else
103 #endif
104 #if USE_VFF
105  ins_baro_initialised = FALSE;
106 #if USE_SONAR
107  ins_update_on_agl = FALSE;
108 #endif
109  vff_init(0., 0., 0.);
110 #endif
113 #if USE_HFF
114  b2_hff_init(0., 0., 0., 0.);
115 #endif
122 }
123 
124 void ins_periodic( void ) {
125 }
126 
127 #if USE_HFF
128 void ins_realign_h(struct FloatVect2 pos, struct FloatVect2 speed) {
129  b2_hff_realign(pos, speed);
130 }
131 #else
132 void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), struct FloatVect2 speed __attribute__ ((unused))) {}
133 #endif /* USE_HFF */
134 
135 
136 void ins_realign_v(float z) {
137 #if USE_VFF
138  vff_realign(z);
139 #endif
140 }
141 
143  /* untilt accels */
144  struct Int32Vect3 accel_meas_body;
146  struct Int32Vect3 accel_meas_ltp;
147  INT32_RMAT_TRANSP_VMULT(accel_meas_ltp, ahrs.ltp_to_body_rmat, accel_meas_body);
148 
149 #if USE_VFF
150  float z_accel_meas_float = ACCEL_FLOAT_OF_BFP(accel_meas_ltp.z);
151  if (baro.status == BS_RUNNING && ins_baro_initialised) {
152  vff_propagate(z_accel_meas_float);
156  }
157  else { // feed accel from the sensors
158  // subtract -9.81m/s2 (acceleration measured due to gravity, but vehivle not accelerating in ltp)
159  ins_ltp_accel.z = accel_meas_ltp.z + ACCEL_BFP_OF_REAL(9.81);
160  }
161 #else
162  ins_ltp_accel.z = accel_meas_ltp.z + ACCEL_BFP_OF_REAL(9.81);
163 #endif /* USE_VFF */
164 
165 #if USE_HFF
166  /* propagate horizontal filter */
168 #else
169  ins_ltp_accel.x = accel_meas_ltp.x;
170  ins_ltp_accel.y = accel_meas_ltp.y;
171 #endif /* USE_HFF */
172 
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) {
187  ins_qfe = baro.absolute;
188 #if USE_SONAR
189  ins_sonar_offset = sonar_meas;
190 #endif
191  vff_realign(0.);
198  }
199  else { /* not realigning, so normal update with baro measurement */
200  ins_baro_alt = ((baro.absolute - ins_qfe) * INS_BARO_SENS_NUM)/INS_BARO_SENS_DEN;
201  float alt_float = POS_FLOAT_OF_BFP(ins_baro_alt);
202  vff_update(alt_float);
203  }
204  }
205 #endif
206 }
207 
208 
209 void ins_update_gps(void) {
210 #if USE_GPS
211  if (gps.fix == GPS_FIX_3D) {
212  if (!ins_ltp_initialised) {
217  }
220 #if USE_HFF
222  VECT2_SDIV(ins_gps_pos_m_ned, ins_gps_pos_m_ned, 100.);
224  VECT2_SDIV(ins_gps_speed_m_s_ned, ins_gps_speed_m_s_ned, 100.);
225  if (ins_hf_realign) {
227 #ifdef SITL
228  struct FloatVect2 true_pos, true_speed;
229  VECT2_COPY(true_pos, fdm.ltpprz_pos);
230  VECT2_COPY(true_speed, fdm.ltpprz_ecef_vel);
231  b2_hff_realign(true_pos, true_speed);
232 #else
233  const struct FloatVect2 zero = {0.0, 0.0};
234  b2_hff_realign(ins_gps_pos_m_ned, zero);
235 #endif
236  }
238 #if !USE_VFF /* vff not used */
241 #endif /* vff not used */
242 #endif /* hff used */
243 
244 
245 #if !USE_HFF /* hff not used */
246 #if !USE_VFF /* neither hf nor vf used */
249 #else /* only vff used */
252 #endif
253 
254 #if USE_GPS_LAG_HACK
255  VECT2_COPY(d_pos, ins_ltp_speed);
256  INT32_VECT2_RSHIFT(d_pos, d_pos, 11);
257  VECT2_ADD(ins_ltp_pos, d_pos);
258 #endif
259 #endif /* hff not used */
260 
264  }
265 #endif /* USE_GPS */
266 }
267 
269 #if USE_SONAR && USE_VFF
270  static int32_t sonar_filtered = 0;
271  sonar_filtered = (sonar_meas + 2*sonar_filtered) / 3;
272  /* update baro_qfe assuming a flat ground */
273  if (ins_update_on_agl && baro.status == BS_RUNNING) {
274  int32_t d_sonar = (((int32_t)sonar_filtered - ins_sonar_offset) * INS_SONAR_SENS_NUM) / INS_SONAR_SENS_DEN;
275  ins_qfe = baro.absolute + (d_sonar * (INS_BARO_SENS_DEN))/INS_BARO_SENS_NUM;
276  }
277 #endif
278 }
struct LlaCoor_i lla_pos
position in LLA (lat,lon: rad*1e7; alt: mm over ellipsoid)
Definition: gps.h:64
#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:67
void vff_propagate(float accel)
Definition: vf_float.c:84
Attitude and Heading Reference System interface.
void ins_realign_v(float z)
Definition: ins.c:136
#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:552
struct EnuCoor_i ins_enu_speed
Definition: ins.c:81
struct LtpDef_i ins_ltp_def
Definition: ins.c:53
#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
struct EnuCoor_i ins_enu_pos
Definition: ins.c:80
struct Ahrs ahrs
global AHRS state (fixed point version)
Definition: ahrs.c:24
struct NedCoor_i ins_ltp_accel
Definition: ins.c:79
bool_t ins_vf_realign
Definition: ins.c:74
void vff_realign(float z_meas)
Definition: vf_float.c:219
struct Int32Vect3 accel
accelerometer measurements
Definition: imu.h:41
#define INT32_RMAT_TRANSP_VMULT(_vb, _m_b2a, _va)
void ins_update_sonar()
Definition: ins.c:268
Device independent INS code.
struct LlaCoor_i lla
Reference point in lla.
int32_t hmsl
height above mean sea level in mm
Definition: gps.h:66
vector in Latitude, Longitude and Altitude
uint8_t fix
status of fix
Definition: gps.h:77
#define GPS_FIX_3D
Definition: gps.h:42
int32_t z
in centimeters
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition: gps.h:63
struct NedCoor_i ins_ltp_pos
Definition: ins.c:77
void ecef_of_lla_i(struct EcefCoor_i *out, struct LlaCoor_i *in)
#define FALSE
Definition: imu_chimu.h:141
struct Int32RMat ltp_to_body_rmat
Rotation from LocalTangentPlane to body frame as Rotation Matrix.
Definition: ahrs.h:51
vector in North East Down coordinates
float vff_zdot
Definition: vf_float.c:46
void ins_update_baro()
Definition: ins.c:178
struct EnuCoor_i ins_enu_accel
Definition: ins.c:82
int32_t absolute
Definition: baro.h:42
definition of the local (flat earth) coordinate system
#define INT32_RAD_OF_DEG(_deg)
#define INT32_SPEED_OF_CM_S_DEN
Paparazzi floating point algebra.
struct Baro baro
Definition: baro_board.c:34
#define INT32_VECT3_ZERO(_v)
#define POS_FLOAT_OF_BFP(_ai)
void ins_init()
Definition: ins.c:85
Device independent GPS code (interface)
void ins_propagate()
Definition: ins.c:142
void ned_of_ecef_point_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct EcefCoor_i *ecef)
bool_t ins_hf_realign
Definition: ins.c:62
struct NedCoor_i ins_ltp_speed
Definition: ins.c:78
int32_t alt
in millimeters above WGS84 reference ellipsoid
void vff_init(float init_z, float init_zdot, float init_bias)
Definition: vf_float.c:53
void ins_realign_h(struct FloatVect2 pos __attribute__((unused)), struct FloatVect2 speed __attribute__((unused)))
Definition: ins.c:132
void b2_hff_init(float init_x, float init_xdot, float init_y, float init_ydot)
Definition: hf_float.c:237
void ins_periodic(void)
Definition: ins.c:124
void b2_hff_update_gps(void)
Definition: hf_float.c:487
Paparazzi fixed point math for geodetic calculations.
#define ACCEL_FLOAT_OF_BFP(_ai)
Inertial Measurement Unit interface.
#define INT32_POS_OF_CM_DEN
signed long int32_t
Definition: types.h:19
#define TRUE
Definition: imu_chimu.h:144
bool_t ins_ltp_initialised
Definition: ins.c:54
float vff_zdotdot
Definition: vf_float.c:47
void ltp_def_from_ecef_i(struct LtpDef_i *def, struct EcefCoor_i *ecef)
#define VECT2_ADD(_a, _b)
Definition: pprz_algebra.h:51
void ins_update_gps(void)
Definition: ins.c:209
#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:421
enum BaroStatus status
Definition: baro.h:44
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:160
int32_t speed
struct Imu imu
global IMU state
Definition: imu_aspirin2.c:50
#define INT32_POS_OF_CM_NUM
struct NedCoor_i ins_gps_pos_cm_ned
Definition: ins.c:55
#define VECT2_ASSIGN(_a, _x, _y)
Definition: pprz_algebra.h:39
#define VECT2_SDIV(_vo, _vi, _s)
Definition: pprz_algebra.h:81
vector in East North Up coordinates
#define SPEED_BFP_OF_REAL(_af)
#define VECT2_COPY(_a, _b)
Definition: pprz_algebra.h:45
__attribute__((always_inline))
Definition: i2c_arch.c:35
uint16_t sonar_meas
struct GpsState gps
global GPS state
Definition: gps.c:31
struct NedCoor_i ins_gps_speed_cm_s_ned
Definition: ins.c:56
#define INT32_VECT3_ENU_OF_NED(_o, _i)
Paparazzi fixed point algebra.
#define POS_BFP_OF_REAL(_af)
float vff_z
Definition: vf_float.c:44