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 File Reference

INS for rotorcrafts combining vertical and horizontal filters. More...

#include "subsystems/ins/ins_int.h"
#include "subsystems/imu.h"
#include "subsystems/sensors/baro.h"
#include "subsystems/gps.h"
#include "generated/airframe.h"
#include "math/pprz_algebra_int.h"
#include "math/pprz_algebra_float.h"
#include "state.h"
#include "subsystems/ins/vf_extended_float.h"
#include "math/pprz_geodetic_int.h"
#include "generated/flight_plan.h"
#include "filters/median_filter.h"
+ Include dependency graph for ins_int_extended.c:

Go to the source code of this file.

Macros

#define USE_INS_NAV_INIT   TRUE
 
#define INS_SONAR_OFFSET   0
 
#define VFF_R_SONAR_0   0.1
 
#define VFF_R_SONAR_OF_M   0.2
 

Functions

void ins_init ()
 INS initialization. More...
 
void ins_periodic (void)
 INS periodic call. More...
 
void ins_realign_h (struct FloatVect2 pos, struct FloatVect2 speed)
 INS horizontal realign. More...
 
void ins_realign_v (float z)
 INS vertical realign. More...
 
void ins_propagate ()
 Propagation. More...
 
void ins_update_baro ()
 Update INS state with barometer measurements. More...
 
void ins_update_gps (void)
 Update INS state with GPS measurements. More...
 
void ins_update_sonar ()
 Update INS state with sonar measurements. More...
 

Variables

struct LtpDef_i ins_ltp_def
 Ins implementation state (fixed point) More...
 
bool_t ins_ltp_initialised
 
struct NedCoor_i ins_gps_pos_cm_ned
 
struct NedCoor_i ins_gps_speed_cm_s_ned
 
int32_t ins_qfe
 
bool_t ins_baro_initialised
 
int32_t ins_baro_alt
 
struct MedianFilterInt baro_median
 
bool_t ins_update_on_agl
 
int32_t ins_sonar_alt
 
int32_t ins_sonar_offset
 
struct MedianFilterInt sonar_median
 
struct NedCoor_i ins_ltp_pos
 
struct NedCoor_i ins_ltp_speed
 
struct NedCoor_i ins_ltp_accel
 

Detailed Description

INS for rotorcrafts combining vertical and horizontal filters.

Definition in file ins_int_extended.c.

Macro Definition Documentation

#define INS_SONAR_OFFSET   0

Definition at line 91 of file ins_int_extended.c.

Referenced by ins_init().

#define USE_INS_NAV_INIT   TRUE

Definition at line 62 of file ins_int_extended.c.

#define VFF_R_SONAR_0   0.1

Definition at line 93 of file ins_int_extended.c.

Referenced by ins_update_sonar().

#define VFF_R_SONAR_OF_M   0.2

Definition at line 94 of file ins_int_extended.c.

Referenced by ins_update_sonar().

Function Documentation

void ins_periodic ( void  )

INS periodic call.

Needs to be implemented by each INS algorithm.

Definition at line 139 of file ins_int_extended.c.

void ins_propagate ( void  )

Propagation.

Usually integrates the gyro rates to angles. Reads the global imu data struct. Needs to be implemented by each INS algorithm.

Definition at line 155 of file ins_int_extended.c.

References Imu::accel, ACCEL_BFP_OF_REAL, ACCEL_FLOAT_OF_BFP, b2_hff_propagate(), baro, Imu::body_to_imu_rmat, BS_RUNNING, imu, ins_ltp_accel, ins_ltp_pos, ins_ltp_speed, INS_NED_TO_STATE, INT32_RMAT_TRANSP_VMULT, POS_BFP_OF_REAL, SPEED_BFP_OF_REAL, stateGetNedToBodyRMat_i(), Baro::status, vff_propagate(), vff_z, vff_zdot, vff_zdotdot, NedCoor_i::x, Int32Vect3::x, NedCoor_i::y, Int32Vect3::y, NedCoor_i::z, and Int32Vect3::z.

+ Here is the call graph for this function:

void ins_realign_h ( struct FloatVect2  pos,
struct FloatVect2  speed 
)

INS horizontal realign.

Parameters
posnew horizontal position to set
speednew horizontal speed to set Needs to be implemented by each INS algorithm.

Definition at line 147 of file ins_int_extended.c.

void ins_realign_v ( float  z)

INS vertical realign.

Parameters
znew altitude to set Needs to be implemented by each INS algorithm.

Definition at line 151 of file ins_int_extended.c.

References vff_realign().

+ Here is the call graph for this function:

void ins_update_sonar ( void  )

Update INS state with sonar measurements.

Reads the global #sonar data struct. Needs to be implemented by each INS algorithm.

Definition at line 264 of file ins_int_extended.c.

References baro, BS_RUNNING, DefaultChannel, DefaultDevice, ins_baro_alt, ins_sonar_offset, POS_BFP_OF_REAL, POS_FLOAT_OF_BFP, sonar_meas, sonar_median, stabilization_cmd, Baro::status, update_median_filter(), variance_float(), vff_offset, VFF_R_SONAR_0, VFF_R_SONAR_OF_M, vff_update_alt_conf(), and vff_update_offset().

+ Here is the call graph for this function:

Variable Documentation

struct MedianFilterInt baro_median

Definition at line 83 of file ins_int_extended.c.

Referenced by ins_init(), and ins_update_baro().

int32_t ins_baro_alt

Definition at line 81 of file ins_int_extended.c.

Referenced by ins_update_sonar().

bool_t ins_baro_initialised

Definition at line 80 of file ins_int_extended.c.

struct NedCoor_i ins_gps_pos_cm_ned

Definition at line 70 of file ins_int_extended.c.

Referenced by ins_update_gps().

struct NedCoor_i ins_gps_speed_cm_s_ned

Definition at line 71 of file ins_int_extended.c.

Referenced by ins_update_gps().

struct NedCoor_i ins_ltp_accel

Definition at line 99 of file ins_int_extended.c.

Referenced by ins_init(), ins_propagate(), and ins_update_baro().

struct LtpDef_i ins_ltp_def

Ins implementation state (fixed point)

global INS state

Definition at line 68 of file ins_int_extended.c.

Referenced by ins_init(), and ins_update_gps().

bool_t ins_ltp_initialised

Definition at line 69 of file ins_int_extended.c.

struct NedCoor_i ins_ltp_pos

Definition at line 97 of file ins_int_extended.c.

Referenced by ins_init(), ins_propagate(), ins_update_baro(), and ins_update_gps().

struct NedCoor_i ins_ltp_speed

Definition at line 98 of file ins_int_extended.c.

Referenced by ins_init(), ins_propagate(), ins_update_baro(), and ins_update_gps().

int32_t ins_qfe

Definition at line 79 of file ins_int_extended.c.

Referenced by ins_update_baro().

int32_t ins_sonar_alt

Definition at line 87 of file ins_int_extended.c.

int32_t ins_sonar_offset

Definition at line 88 of file ins_int_extended.c.

Referenced by ins_update_sonar().

bool_t ins_update_on_agl

Definition at line 86 of file ins_int_extended.c.

Referenced by track_init().

struct MedianFilterInt sonar_median

Definition at line 89 of file ins_int_extended.c.

Referenced by ins_init(), and ins_update_sonar().