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_extended.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2010 The Paparazzi Team
3  * Copyright (C) 2012 Gautier Hattenberger
4  *
5  * This file is part of paparazzi.
6  *
7  * paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with paparazzi; see the file COPYING. If not, write to
19  * the Free Software Foundation, 59 Temple Place - Suite 330,
20  * Boston, MA 02111-1307, USA.
21  */
22 
30 #include "subsystems/ins/ins_int.h"
31 
32 #include "subsystems/imu.h"
34 #include "subsystems/gps.h"
35 
36 #include "generated/airframe.h"
37 #include "math/pprz_algebra_int.h"
39 
40 #include "state.h"
41 
43 
44 #if USE_HFF
46 #endif
47 
48 #ifdef SITL
49 #include "nps_fdm.h"
50 #include <stdio.h>
51 #endif
52 
53 #ifdef INS_SONAR_THROTTLE_THRESHOLD
55 #endif
56 
57 #include "math/pprz_geodetic_int.h"
58 
59 #include "generated/flight_plan.h"
60 
61 #ifndef USE_INS_NAV_INIT
62 #define USE_INS_NAV_INIT TRUE
63 PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
64 #endif
65 
66 
67 /* gps transformed to LTP-NED */
72 #if USE_HFF
73 /* horizontal gps transformed to NED in meters as float */
74 struct FloatVect2 ins_gps_pos_m_ned;
75 struct FloatVect2 ins_gps_speed_m_s_ned;
76 #endif
77 
78 /* barometer */
82 #include "filters/median_filter.h"
84 
85 /* sonar */
90 #ifndef INS_SONAR_OFFSET
91 #define INS_SONAR_OFFSET 0
92 #endif
93 #define VFF_R_SONAR_0 0.1
94 #define VFF_R_SONAR_OF_M 0.2
95 
96 /* output */
100 
101 
102 void ins_init() {
103 #if USE_INS_NAV_INIT
104  ins_ltp_initialised = TRUE;
105 
107  struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
108  llh_nav0.lat = INT32_RAD_OF_DEG(NAV_LAT0);
109  llh_nav0.lon = INT32_RAD_OF_DEG(NAV_LON0);
110  /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
111  llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
112 
113  struct EcefCoor_i ecef_nav0;
114  ecef_of_lla_i(&ecef_nav0, &llh_nav0);
115 
116  ltp_def_from_ecef_i(&ins_ltp_def, &ecef_nav0);
117  ins_ltp_def.hmsl = NAV_ALT0;
119 #else
120  ins_ltp_initialised = FALSE;
121 #endif
122 
123  ins_baro_initialised = FALSE;
125  ins_update_on_agl = FALSE;
127  ins_sonar_offset = INS_SONAR_OFFSET;
128  vff_init(0., 0., 0., 0.);
129  ins.vf_realign = FALSE;
130  ins.hf_realign = FALSE;
131 #if USE_HFF
132  b2_hff_init(0., 0., 0., 0.);
133 #endif
137 }
138 
139 void ins_periodic( void ) {
140 }
141 
142 #if USE_HFF
143 void ins_realign_h(struct FloatVect2 pos, struct FloatVect2 speed) {
144  b2_hff_realign(pos, speed);
145 }
146 #else
147 void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), struct FloatVect2 speed __attribute__ ((unused))) {}
148 #endif /* USE_HFF */
149 
150 
151 void ins_realign_v(float z) {
152  vff_realign(z);
153 }
154 
156  /* untilt accels */
157  struct Int32Vect3 accel_meas_body;
159  struct Int32Vect3 accel_meas_ltp;
160  INT32_RMAT_TRANSP_VMULT(accel_meas_ltp, *stateGetNedToBodyRMat_i(), accel_meas_body);
161 
162  float z_accel_meas_float = ACCEL_FLOAT_OF_BFP(accel_meas_ltp.z);
163  if (baro.status == BS_RUNNING && ins_baro_initialised) {
164  vff_propagate(z_accel_meas_float);
168  }
169  else { // feed accel from the sensors
170  // subtract -9.81m/s2 (acceleration measured due to gravity, but vehivle not accelerating in ltp)
171  ins_ltp_accel.z = accel_meas_ltp.z + ACCEL_BFP_OF_REAL(9.81);
172  }
173 
174 #if USE_HFF
175  /* propagate horizontal filter */
177 #else
178  ins_ltp_accel.x = accel_meas_ltp.x;
179  ins_ltp_accel.y = accel_meas_ltp.y;
180 #endif /* USE_HFF */
181 
183 }
184 
187  if (baro.status == BS_RUNNING) {
188  if (!ins_baro_initialised) {
189  ins_qfe = baro_pressure;
190  ins_baro_initialised = TRUE;
191  }
192  if (ins.vf_realign) {
193  ins.vf_realign = FALSE;
194  ins_qfe = baro_pressure;
195  vff_realign(0.);
199  }
200  else { /* not realigning, so normal update with baro measurement */
201  ins_baro_alt = ((baro_pressure - ins_qfe) * INS_BARO_SENS_NUM)/INS_BARO_SENS_DEN;
202  float alt_float = POS_FLOAT_OF_BFP(ins_baro_alt);
203  vff_update_baro(alt_float);
204  }
205  }
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) {
216  ins_ltp_initialised = TRUE;
218  }
221 #if USE_HFF
223  VECT2_SDIV(ins_gps_pos_m_ned, ins_gps_pos_m_ned, 100.);
225  VECT2_SDIV(ins_gps_speed_m_s_ned, ins_gps_speed_m_s_ned, 100.);
226  if (ins.hf_realign) {
227  ins.hf_realign = FALSE;
228 #ifdef SITL
229  struct FloatVect2 true_pos, true_speed;
230  VECT2_COPY(true_pos, fdm.ltpprz_pos);
231  VECT2_COPY(true_speed, fdm.ltpprz_ecef_vel);
232  b2_hff_realign(true_pos, true_speed);
233 #else
234  const struct FloatVect2 zero = {0.0, 0.0};
235  b2_hff_realign(ins_gps_pos_m_ned, zero);
236 #endif
237  }
239 #endif /* hff used */
240 
241 #if !USE_HFF /* hff not used */
244 #endif /* hff not used */
245 
246  }
247 #endif /* USE_GPS */
248 }
249 
250 //#define INS_SONAR_VARIANCE_THRESHOLD 0.01
251 
252 #ifdef INS_SONAR_VARIANCE_THRESHOLD
253 
254 #include "messages.h"
255 #include "mcu_periph/uart.h"
257 
258 #include "math/pprz_stat.h"
259 #define VAR_ERR_MAX 10
260 float var_err[VAR_ERR_MAX];
261 uint8_t var_idx = 0;
262 #endif
263 
265  static float last_offset = 0.;
266  // new value filtered with median_filter
267  ins_sonar_alt = update_median_filter(&sonar_median, sonar_meas);
268  float sonar = (ins_sonar_alt - ins_sonar_offset) * INS_SONAR_SENS;
269 
270 #ifdef INS_SONAR_VARIANCE_THRESHOLD
271  /* compute variance of error between sonar and baro alt */
272  int32_t err = POS_BFP_OF_REAL(sonar) + ins_baro_alt; // sonar positive up, baro positive down !!!!
273  var_err[var_idx] = POS_FLOAT_OF_BFP(err);
274  var_idx = (var_idx + 1) % VAR_ERR_MAX;
275  float var = variance_float(var_err, VAR_ERR_MAX);
276  DOWNLINK_SEND_INS_SONAR(DefaultChannel,DefaultDevice,&err, &sonar, &var);
277  //DOWNLINK_SEND_INS_SONAR(DefaultChannel,DefaultDevice,&ins_sonar_alt, &sonar, &var);
278 #endif
279 
280  /* update filter assuming a flat ground */
281  if (sonar < INS_SONAR_MAX_RANGE
282 #ifdef INS_SONAR_MIN_RANGE
283  && sonar > INS_SONAR_MIN_RANGE
284 #endif
285 #ifdef INS_SONAR_THROTTLE_THRESHOLD
286  && stabilization_cmd[COMMAND_THRUST] < INS_SONAR_THROTTLE_THRESHOLD
287 #endif
288 #ifdef INS_SONAR_STAB_THRESHOLD
289  && stabilization_cmd[COMMAND_ROLL] < INS_SONAR_STAB_THRESHOLD
290  && stabilization_cmd[COMMAND_ROLL] > -INS_SONAR_STAB_THRESHOLD
291  && stabilization_cmd[COMMAND_PITCH] < INS_SONAR_STAB_THRESHOLD
292  && stabilization_cmd[COMMAND_PITCH] > -INS_SONAR_STAB_THRESHOLD
293  && stabilization_cmd[COMMAND_YAW] < INS_SONAR_STAB_THRESHOLD
294  && stabilization_cmd[COMMAND_YAW] > -INS_SONAR_STAB_THRESHOLD
295 #endif
296 #ifdef INS_SONAR_BARO_THRESHOLD
297  && ins_baro_alt > -POS_BFP_OF_REAL(INS_SONAR_BARO_THRESHOLD) /* z down */
298 #endif
299 #ifdef INS_SONAR_VARIANCE_THRESHOLD
300  && var < INS_SONAR_VARIANCE_THRESHOLD
301 #endif
302  && ins_update_on_agl
303  && baro.status == BS_RUNNING) {
304  vff_update_alt_conf(-sonar, VFF_R_SONAR_0 + VFF_R_SONAR_OF_M * fabs(sonar));
305  last_offset = vff_offset;
306  }
307  else {
308  /* update offset with last value to avoid divergence */
309  vff_update_offset(last_offset);
310  }
311 }
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
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
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.
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)
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 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
struct MedianFilterInt sonar_median
int32_t ins_sonar_offset
#define INS_SONAR_OFFSET
struct Int32Vect3 accel
accelerometer measurements
Definition: imu.h:41
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
struct NedCoor_i ins_ltp_accel
#define GPS_FIX_3D
Definition: gps.h:43
int32_t z
in centimeters
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition: gps.h:64
struct NedCoor_i ins_ltp_pos
bool_t hf_realign
realign horizontally if true
Definition: ins.h:46
void ins_init()
INS initialization.
float vff_zdotdot
bool_t ins_ltp_initialised
void ecef_of_lla_i(struct EcefCoor_i *out, struct LlaCoor_i *in)
#define FALSE
Definition: imu_chimu.h:141
int32_t ins_baro_alt
void vff_update_alt_conf(float z_meas, float conf)
vector in North East Down coordinates
int32_t z
Down.
int32_t absolute
Definition: baro.h:41
definition of the local (flat earth) coordinate system
#define VFF_R_SONAR_0
#define INS_NED_TO_STATE()
Definition: ins_int.h:73
#define INT32_RAD_OF_DEG(_deg)
void ins_periodic(void)
INS periodic call.
#define INT32_SPEED_OF_CM_S_DEN
int32_t ins_qfe
Paparazzi floating point algebra.
#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
int32_t update_median_filter(struct MedianFilterInt *filter, int32_t new_data)
Definition: median_filter.h:50
void vff_update_offset(float offset)
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
static float variance_float(float *array, int nb)
Compute the variance of an array of values (float).
Definition: pprz_stat.h:37
int32_t y
East.
void b2_hff_update_gps(void)
Definition: hf_float.c:493
Paparazzi fixed point math for geodetic calculations.
struct NedCoor_i ins_gps_pos_cm_ned
int32_t ins_sonar_alt
#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)
signed long int32_t
Definition: types.h:19
#define TRUE
Definition: imu_chimu.h:144
#define VFF_R_SONAR_OF_M
void ltp_def_from_ecef_i(struct LtpDef_i *def, struct EcefCoor_i *ecef)
bool_t ins_baro_initialised
void vff_realign(float z_meas)
bool_t ins_update_on_agl
struct Baro baro
Definition: baro_board.c:36
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
#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
General stabilization interface for rotorcrafts.
struct MedianFilterInt baro_median
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)
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
Definition: stabilization.c:28
float vff_offset
void vff_update_baro(float z_meas)
void ins_update_baro()
Update INS state with barometer measurements.
#define INT32_POS_OF_CM_NUM
bool_t vf_realign
realign vertically if true
Definition: ins.h:47
void ins_update_sonar()
Update INS state with sonar measurements.
void ins_realign_v(float z)
INS vertical realign.
float vff_zdot
void vff_init(float init_z, float init_zdot, float init_accel_bias, float init_baro_offset)
void ins_realign_h(struct FloatVect2 pos, struct FloatVect2 speed)
INS horizontal realign.
#define VECT2_ASSIGN(_a, _x, _y)
Definition: pprz_algebra.h:37
#define VECT2_SDIV(_vo, _vi, _s)
Definition: pprz_algebra.h:79
void init_median_filter(struct MedianFilterInt *filter)
Definition: median_filter.h:41
#define SPEED_BFP_OF_REAL(_af)
struct NedCoor_i ins_ltp_speed
#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
Paparazzi fixed point algebra.
INS for rotorcrafts combining vertical and horizontal filters.
#define POS_BFP_OF_REAL(_af)
struct NedCoor_i ins_gps_speed_cm_s_ned
void ins_propagate()
Propagation.
void ins_update_gps(void)
Update INS state with GPS measurements.