Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
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/abi.h"
32 
33 #include "subsystems/imu.h"
34 #include "subsystems/gps.h"
35 
36 #include "generated/airframe.h"
37 
38 #if USE_VFF_EXTENDED
40 #else
42 #endif
43 
44 #if USE_HFF
46 #endif
47 
48 #if defined SITL && USE_NPS
49 //#include "nps_fdm.h"
50 #include "nps_autopilot.h"
51 #include <stdio.h>
52 #endif
53 
54 #include "math/pprz_geodetic_int.h"
55 #include "math/pprz_isa.h"
56 
57 #include "generated/flight_plan.h"
58 
59 
60 #if USE_SONAR
61 #if !USE_VFF_EXTENDED
62 #error USE_SONAR needs USE_VFF_EXTENDED
63 #endif
64 
66 #ifndef INS_SONAR_ID
67 #define INS_SONAR_ID ABI_BROADCAST
68 #endif
69 abi_event sonar_ev;
70 static void sonar_cb(uint8_t sender_id, const float *distance);
71 
72 #ifdef INS_SONAR_THROTTLE_THRESHOLD
74 #endif
75 
76 #ifndef INS_SONAR_OFFSET
77 #define INS_SONAR_OFFSET 0.
78 #endif
79 #ifndef INS_SONAR_MIN_RANGE
80 #define INS_SONAR_MIN_RANGE 0.001
81 #endif
82 #define VFF_R_SONAR_0 0.1
83 #ifndef VFF_R_SONAR_OF_M
84 #define VFF_R_SONAR_OF_M 0.2
85 #endif
86 
87 #ifndef INS_SONAR_UPDATE_ON_AGL
88 #define INS_SONAR_UPDATE_ON_AGL FALSE
89 PRINT_CONFIG_MSG("INS_SONAR_UPDATE_ON_AGL defaulting to FALSE")
90 #endif
91 
92 #endif // USE_SONAR
93 
94 #ifndef INS_VFF_R_GPS
95 #define INS_VFF_R_GPS 2.0
96 #endif
97 
98 #ifndef USE_INS_NAV_INIT
99 #define USE_INS_NAV_INIT TRUE
100 PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
101 #endif
102 
103 #ifdef INS_BARO_SENS
104 #warning INS_BARO_SENS is obsolete, please remove it from your airframe file.
105 #endif
106 
108 #ifndef INS_BARO_ID
109 #if USE_BARO_BOARD
110 #define INS_BARO_ID BARO_BOARD_SENDER_ID
111 #else
112 #define INS_BARO_ID ABI_BROADCAST
113 #endif
114 #endif
117 static void baro_cb(uint8_t sender_id, const float *pressure);
118 
120 
121 #if PERIODIC_TELEMETRY
123 
124 static void send_ins(void) {
125  DOWNLINK_SEND_INS(DefaultChannel, DefaultDevice,
126  &ins_impl.ltp_pos.x, &ins_impl.ltp_pos.y, &ins_impl.ltp_pos.z,
127  &ins_impl.ltp_speed.x, &ins_impl.ltp_speed.y, &ins_impl.ltp_speed.z,
128  &ins_impl.ltp_accel.x, &ins_impl.ltp_accel.y, &ins_impl.ltp_accel.z);
129 }
130 
131 static void send_ins_z(void) {
132  DOWNLINK_SEND_INS_Z(DefaultChannel, DefaultDevice,
133  &ins_impl.baro_z, &ins_impl.ltp_pos.z, &ins_impl.ltp_speed.z, &ins_impl.ltp_accel.z);
134 }
135 
136 static void send_ins_ref(void) {
137  if (ins_impl.ltp_initialized) {
138  DOWNLINK_SEND_INS_REF(DefaultChannel, DefaultDevice,
139  &ins_impl.ltp_def.ecef.x, &ins_impl.ltp_def.ecef.y, &ins_impl.ltp_def.ecef.z,
140  &ins_impl.ltp_def.lla.lat, &ins_impl.ltp_def.lla.lon, &ins_impl.ltp_def.lla.alt,
141  &ins_impl.ltp_def.hmsl, &ins_impl.qfe);
142  }
143 }
144 #endif
145 
146 static void ins_init_origin_from_flightplan(void);
147 static void ins_ned_to_state(void);
148 static void ins_update_from_vff(void);
149 #if USE_HFF
150 static void ins_update_from_hff(void);
151 #endif
152 
153 
154 void ins_init(void) {
155 
156 #if USE_INS_NAV_INIT
158  ins_impl.ltp_initialized = TRUE;
159 #else
160  ins_impl.ltp_initialized = FALSE;
161 #endif
162 
163  // Bind to BARO_ABS message
164  AbiBindMsgBARO_ABS(INS_BARO_ID, &baro_ev, baro_cb);
165  ins_impl.baro_initialized = FALSE;
166 
167 #if USE_SONAR
168  ins_impl.update_on_agl = INS_SONAR_UPDATE_ON_AGL;
169  // Bind to AGL message
170  AbiBindMsgAGL(INS_SONAR_ID, &sonar_ev, sonar_cb);
171 #endif
172 
173  ins_impl.vf_reset = FALSE;
174  ins_impl.hf_realign = FALSE;
175 
176  /* init vertical and horizontal filters */
177  vff_init_zero();
178 #if USE_HFF
179  b2_hff_init(0., 0., 0., 0.);
180 #endif
181 
182  INT32_VECT3_ZERO(ins_impl.ltp_pos);
183  INT32_VECT3_ZERO(ins_impl.ltp_speed);
184  INT32_VECT3_ZERO(ins_impl.ltp_accel);
185 
186 #if PERIODIC_TELEMETRY
187  register_periodic_telemetry(DefaultPeriodic, "INS", send_ins);
188  register_periodic_telemetry(DefaultPeriodic, "INS_Z", send_ins_z);
189  register_periodic_telemetry(DefaultPeriodic, "INS_REF", send_ins_ref);
190 #endif
191 }
192 
193 void ins_periodic(void) {
194  if (ins_impl.ltp_initialized)
196 }
197 
199  ins_impl.ltp_initialized = FALSE;
200 #if USE_HFF
201  ins_impl.hf_realign = TRUE;
202 #endif
203  ins_impl.vf_reset = TRUE;
204 }
205 
207 #if USE_GPS
208  struct LlaCoor_i lla = {
211  gps.lla_pos.alt
212  };
213  ltp_def_from_lla_i(&ins_impl.ltp_def, &lla),
214  ins_impl.ltp_def.hmsl = gps.hmsl;
215  stateSetLocalOrigin_i(&ins_impl.ltp_def);
216 #endif
217  ins_impl.vf_reset = TRUE;
218 }
219 
220 void ins_propagate(void) {
221  /* untilt accels */
222  struct Int32Vect3 accel_meas_body;
223  struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu);
224  INT32_RMAT_TRANSP_VMULT(accel_meas_body, *body_to_imu_rmat, imu.accel);
225  struct Int32Vect3 accel_meas_ltp;
226  INT32_RMAT_TRANSP_VMULT(accel_meas_ltp, (*stateGetNedToBodyRMat_i()), accel_meas_body);
227 
228  float z_accel_meas_float = ACCEL_FLOAT_OF_BFP(accel_meas_ltp.z);
229  if (ins_impl.baro_initialized) {
230  vff_propagate(z_accel_meas_float);
232  }
233  else { // feed accel from the sensors
234  // subtract -9.81m/s2 (acceleration measured due to gravity,
235  // but vehicle not accelerating in ltp)
236  ins_impl.ltp_accel.z = accel_meas_ltp.z + ACCEL_BFP_OF_REAL(9.81);
237  }
238 
239 #if USE_HFF
240  /* propagate horizontal filter */
242  /* convert and copy result to ins_impl */
243  ins_update_from_hff();
244 #else
245  ins_impl.ltp_accel.x = accel_meas_ltp.x;
246  ins_impl.ltp_accel.y = accel_meas_ltp.y;
247 #endif /* USE_HFF */
248 
250 }
251 
252 static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pressure) {
253  if (!ins_impl.baro_initialized && *pressure > 1e-7) {
254  // wait for a first positive value
255  ins_impl.qfe = *pressure;
256  ins_impl.baro_initialized = TRUE;
257  }
258 
259  if (ins_impl.baro_initialized) {
260  if (ins_impl.vf_reset) {
261  ins_impl.vf_reset = FALSE;
262  ins_impl.qfe = *pressure;
263  vff_realign(0.);
265  }
266  else {
267  ins_impl.baro_z = -pprz_isa_height_of_pressure(*pressure, ins_impl.qfe);
268 #if USE_VFF_EXTENDED
269  vff_update_baro(ins_impl.baro_z);
270 #else
271  vff_update(ins_impl.baro_z);
272 #endif
273  }
275  }
276 }
277 
278 #if USE_GPS
279 void ins_update_gps(void) {
280  if (gps.fix == GPS_FIX_3D) {
281  if (!ins_impl.ltp_initialized) {
283  ins_impl.ltp_def.lla.alt = gps.lla_pos.alt;
284  ins_impl.ltp_def.hmsl = gps.hmsl;
285  ins_impl.ltp_initialized = TRUE;
286  stateSetLocalOrigin_i(&ins_impl.ltp_def);
287  }
288 
289  struct NedCoor_i gps_pos_cm_ned;
290  ned_of_ecef_point_i(&gps_pos_cm_ned, &ins_impl.ltp_def, &gps.ecef_pos);
292  struct NedCoor_i gps_speed_cm_s_ned;
293  ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &ins_impl.ltp_def, &gps.ecef_vel);
294 
295 #if INS_USE_GPS_ALT
296  vff_update_z_conf((float)gps_pos_cm_ned.z / 100.0, INS_VFF_R_GPS);
297 #endif
298 
299 #if USE_HFF
300  /* horizontal gps transformed to NED in meters as float */
301  struct FloatVect2 gps_pos_m_ned;
302  VECT2_ASSIGN(gps_pos_m_ned, gps_pos_cm_ned.x, gps_pos_cm_ned.y);
303  VECT2_SDIV(gps_pos_m_ned, gps_pos_m_ned, 100.0f);
304 
305  struct FloatVect2 gps_speed_m_s_ned;
306  VECT2_ASSIGN(gps_speed_m_s_ned, gps_speed_cm_s_ned.x, gps_speed_cm_s_ned.y);
307  VECT2_SDIV(gps_speed_m_s_ned, gps_speed_m_s_ned, 100.);
308 
309  if (ins_impl.hf_realign) {
310  ins_impl.hf_realign = FALSE;
311  const struct FloatVect2 zero = {0.0f, 0.0f};
312  b2_hff_realign(gps_pos_m_ned, zero);
313  }
314  // run horizontal filter
315  b2_hff_update_gps(&gps_pos_m_ned, &gps_speed_m_s_ned);
316  // convert and copy result to ins_impl
317  ins_update_from_hff();
318 
319 #else /* hff not used */
320  /* simply copy horizontal pos/speed from gps */
321  INT32_VECT2_SCALE_2(ins_impl.ltp_pos, gps_pos_cm_ned,
323  INT32_VECT2_SCALE_2(ins_impl.ltp_speed, gps_speed_cm_s_ned,
325 #endif /* USE_HFF */
326 
328  }
329 }
330 #endif /* USE_GPS */
331 
332 
333 #if USE_SONAR
334 static void sonar_cb(uint8_t __attribute__((unused)) sender_id, const float *distance) {
335  static float last_offset = 0.;
336 
337  /* update filter assuming a flat ground */
338  if (*distance < INS_SONAR_MAX_RANGE && *distance > INS_SONAR_MIN_RANGE
339 #ifdef INS_SONAR_THROTTLE_THRESHOLD
340  && stabilization_cmd[COMMAND_THRUST] < INS_SONAR_THROTTLE_THRESHOLD
341 #endif
342 #ifdef INS_SONAR_BARO_THRESHOLD
343  && ins_impl.baro_z > -INS_SONAR_BARO_THRESHOLD /* z down */
344 #endif
345  && ins_impl.update_on_agl
346  && ins_impl.baro_initialized) {
347  vff_update_z_conf(-(*distance), VFF_R_SONAR_0 + VFF_R_SONAR_OF_M * fabsf(*distance));
348  last_offset = vff.offset;
349  }
350  else {
351  /* update offset with last value to avoid divergence */
352  vff_update_offset(last_offset);
353  }
354 }
355 #endif // USE_SONAR
356 
357 
360 
361  struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
362  llh_nav0.lat = NAV_LAT0;
363  llh_nav0.lon = NAV_LON0;
364  /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
365  llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
366 
367  struct EcefCoor_i ecef_nav0;
368  ecef_of_lla_i(&ecef_nav0, &llh_nav0);
369 
370  ltp_def_from_ecef_i(&ins_impl.ltp_def, &ecef_nav0);
371  ins_impl.ltp_def.hmsl = NAV_ALT0;
372  stateSetLocalOrigin_i(&ins_impl.ltp_def);
373 
374 }
375 
377 static void ins_ned_to_state(void) {
378  stateSetPositionNed_i(&ins_impl.ltp_pos);
379  stateSetSpeedNed_i(&ins_impl.ltp_speed);
380  stateSetAccelNed_i(&ins_impl.ltp_accel);
381 
382 #if defined SITL && USE_NPS
383  if (nps_bypass_ins)
384  sim_overwrite_ins();
385 #endif
386 }
387 
389 static void ins_update_from_vff(void) {
391  ins_impl.ltp_speed.z = SPEED_BFP_OF_REAL(vff.zdot);
392  ins_impl.ltp_pos.z = POS_BFP_OF_REAL(vff.z);
393 }
394 
395 #if USE_HFF
396 
397 static void ins_update_from_hff(void) {
402  ins_impl.ltp_pos.x = POS_BFP_OF_REAL(b2_hff_state.x);
403  ins_impl.ltp_pos.y = POS_BFP_OF_REAL(b2_hff_state.y);
404 }
405 #endif
struct Ins ins
global INS state
Definition: ins.c:36
static void stateSetPositionNed_i(struct NedCoor_i *ned_pos)
Set position from local NED coordinates (int).
Definition: state.h:508
int32_t y
in centimeters
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:65
int32_t lat
in degrees*1e7
struct EcefCoor_i ecef_vel
speed ECEF in cm/s
Definition: gps.h:68
static struct Int32RMat * orientationGetRMat_i(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (int).
void vff_update_z_conf(float z_meas, float conf)
float baro_z
z-position calculated from baro in meters (z-down)
Definition: ins_int.h:58
Interface for extended vertical filter (in float).
Horizontal filter (x,y) to estimate position and velocity.
#define INT32_VECT2_SCALE_2(_a, _b, _num, _den)
float xdot
Definition: hf_float.h:41
vector in EarthCenteredEarthFixed coordinates
void b2_hff_realign(struct FloatVect2 pos, struct FloatVect2 vel)
Definition: hf_float.c:570
Periodic telemetry system header (includes downlink utility and generated code).
static struct Int32RMat * stateGetNedToBodyRMat_i(void)
Get vehicle body attitude rotation matrix (int).
Definition: state.h:1007
bool_t baro_initialized
Definition: ins_int.h:60
#define ACCEL_BFP_OF_REAL(_af)
enum InsStatus status
status of the INS
Definition: ins.h:47
#define INS_BARO_ID
default barometer to use in INS
Definition: ins_int.c:112
static void ins_update_from_vff(void)
update ins state from vertical filter
Definition: ins_int.c:389
Main include for ABI (AirBorneInterface).
float zdot
z-velocity estimate in m/s (NED, z-down)
float x
Definition: hf_float.h:39
struct Int32Vect3 accel
accelerometer measurements in m/s^2 in BFP with INT32_ACCEL_FRAC
Definition: imu.h:42
int32_t x
North.
#define INT32_RMAT_TRANSP_VMULT(_vb, _m_b2a, _va)
struct OrientationReps body_to_imu
rotation from body to imu frame
Definition: imu.h:52
void ins_reset_local_origin(void)
INS local origin reset.
Definition: ins_int.c:198
static float pprz_isa_height_of_pressure(float pressure, float ref)
Get relative altitude from pressure (using simplified equation).
Definition: pprz_isa.h:73
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
struct LtpDef_i ltp_def
Definition: ins_int.h:39
float xdotdot
Definition: hf_float.h:42
static void stateSetSpeedNed_i(struct NedCoor_i *ned_speed)
Set ground speed in local NED coordinates (int).
Definition: state.h:712
#define GPS_FIX_3D
Definition: gps.h:43
int32_t z
in centimeters
bool_t register_periodic_telemetry(struct pprz_telemetry *_pt, const char *_msg, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:38
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition: gps.h:64
void ins_update_gps(void)
Update INS state with GPS measurements.
Definition: ins_int.c:279
void ltp_def_from_lla_i(struct LtpDef_i *def, struct LlaCoor_i *lla)
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.
abi_event baro_ev
Definition: ins_int.c:116
vector in North East Down coordinates
static void stateSetAccelNed_i(struct NedCoor_i *ned_accel)
Set acceleration in NED coordinates (int).
Definition: state.h:894
struct InsInt ins_impl
global INS state
Definition: ins_int.c:119
float ydot
Definition: hf_float.h:45
int32_t z
Down.
float qfe
Definition: ins_int.h:59
#define INT32_SPEED_OF_CM_S_DEN
#define INT32_VECT3_ZERO(_v)
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)
Paparazzi atmospheric pressure convertion utilities.
struct Imu imu
global IMU state
Definition: imu_aspirin2.c:47
void vff_update_offset(float offset)
void ned_of_ecef_point_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct EcefCoor_i *ecef)
Convert a point from ECEF to local NED.
static void ins_ned_to_state(void)
copy position and speed to state interface
Definition: ins_int.c:377
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:265
static void baro_cb(uint8_t sender_id, const float *pressure)
Definition: ins_int.c:252
float zdotdot
z-acceleration in m/s^2 (NED, z-down)
int32_t y
East.
Paparazzi fixed point math for geodetic calculations.
void vff_init_zero(void)
#define ACCEL_FLOAT_OF_BFP(_ai)
int32_t x
in centimeters
Inertial Measurement Unit interface.
#define INT32_POS_OF_CM_DEN
struct HfilterFloat b2_hff_state
Definition: hf_float.c:120
bool_t ltp_initialized
Definition: ins_int.h:40
#define TRUE
Definition: imu_chimu.h:144
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
Definition: state.h:162
void ltp_def_from_ecef_i(struct LtpDef_i *def, struct EcefCoor_i *ecef)
struct NedCoor_i ltp_accel
Definition: ins_int.h:55
void vff_realign(float z_meas)
float ydotdot
Definition: hf_float.h:46
unsigned char uint8_t
Definition: types.h:14
float y
Definition: hf_float.h:43
struct VffExtended vff
#define INT32_SPEED_OF_CM_S_NUM
int32_t lon
in degrees*1e7
int32_t hmsl
Height above mean sea level in mm.
void b2_hff_propagate(void)
Definition: hf_float.c:453
General stabilization interface for rotorcrafts.
struct EcefCoor_i ecef
Reference point in ecef.
void ned_of_ecef_vect_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct EcefCoor_i *ecef)
Rotate a vector from ECEF to NED.
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
Definition: stabilization.c:28
void b2_hff_update_gps(struct FloatVect2 *pos_ned, struct FloatVect2 *speed_ned)
Definition: hf_float.c:512
struct NedCoor_i ltp_speed
Definition: ins_int.h:54
void vff_update(float z_meas)
Definition: vf_float.c:199
rotation matrix
void vff_update_baro(float z_meas)
void ins_init(void)
INS initialization.
Definition: ins_int.c:154
struct NedCoor_i ltp_pos
Definition: ins_int.h:53
#define INT32_POS_OF_CM_NUM
void ins_periodic(void)
INS periodic call.
Definition: ins_int.c:193
bool_t hf_realign
request to realign horizontal filter.
Definition: ins_int.h:45
void ins_propagate(void)
Propagation.
Definition: ins_int.c:220
static void ins_init_origin_from_flightplan(void)
initialize the local origin (ltp_def) from flight plan position
Definition: ins_int.c:359
#define VECT2_ASSIGN(_a, _x, _y)
Definition: pprz_algebra.h:44
#define VECT2_SDIV(_vo, _vi, _s)
Definition: pprz_algebra.h:86
#define INS_VFF_R_GPS
Definition: ins_int.c:95
Ins implementation state (fixed point)
Definition: ins_int.h:38
float z
z-position estimate in m (NED, z-down)
#define SPEED_BFP_OF_REAL(_af)
void vff_propagate(float accel)
struct GpsState gps
global GPS state
Definition: gps.c:41
float offset
baro offset estimate
struct State state
Definition: state.c:36
void ins_reset_altitude_ref(void)
INS altitude reference reset.
Definition: ins_int.c:206
bool_t vf_reset
request to reset vertical filter.
Definition: ins_int.h:50
INS for rotorcrafts combining vertical and horizontal filters.
#define POS_BFP_OF_REAL(_af)
Event structure to store callbacks in a linked list.
Definition: abi_common.h:58