Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
ins_int.c File Reference

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

#include "modules/ins/ins_int.h"
#include "modules/core/abi.h"
#include "modules/imu/imu.h"
#include "modules/gps/gps.h"
#include "generated/airframe.h"
#include "modules/ins/vf_float.h"
#include "math/pprz_geodetic_int.h"
#include "math/pprz_isa.h"
#include "math/pprz_stat.h"
#include "modules/datalink/telemetry.h"
Include dependency graph for ins_int.c:

Go to the source code of this file.

Macros

#define VFF_R_AGL   0.2
#define INS_VFF_R_GPS   2.0
#define INS_VFF_VZ_R_GPS   2.0
#define INS_MAX_PROPAGATION_STEPS   200
 maximum number of propagation steps without any updates in between
#define USE_INS_NAV_INIT   TRUE
#define INS_BARO_MAX_INIT_VAR   1.f
 default barometer to use in INS
#define INS_INT_BARO_ID   AIR_DATA_SENDER_ID
#define INS_INT_IMU_ID   ABI_BROADCAST
 ABI binding for IMU data.
#define INS_INT_GPS_ID   GPS_MULTI_ID
#define INS_INT_VEL_ID   ABI_BROADCAST
 ABI binding for VELOCITY_ESTIMATE.
#define INS_INT_POS_ID   ABI_BROADCAST
#define INS_INT_AGL_ID   ABI_BROADCAST
 ABI binding for AGL.
#define press_hist_len   10

Functions

static void baro_cb (uint8_t sender_id, uint32_t stamp, float pressure)
static void accel_cb (uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
static void gps_cb (uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
static void vel_est_cb (uint8_t sender_id, uint32_t stamp, float x, float y, float z, float noise_x, float noise_y, float noise_z)
static void pos_est_cb (uint8_t sender_id, uint32_t stamp, float x, float y, float z, float noise_x, float noise_y, float noise_z)
static void agl_cb (uint8_t sender_id, uint32_t stamp, float distance)
 agl_cb This callback handles all estimates of the height of the vehicle above the ground under it This is only used with the extended version of the vertical float filter
static void reset_cb (uint8_t sender_id, uint8_t flag)
static void send_ins (struct transport_tx *trans, struct link_device *dev)
static void send_ins_z (struct transport_tx *trans, struct link_device *dev)
static void send_ins_ref (struct transport_tx *trans, struct link_device *dev)
static void ins_ned_to_state (void)
 copy position and speed to state interface
static void ins_update_from_vff (void)
 update ins state from vertical filter
void ins_int_init (void)
static void reset_ref (void)
static void reset_vertical_ref (void)
static void reset_vertical_pos (void)
void ins_int_propagate (struct Int32Vect3 *accel, float dt)
void ins_int_update_gps (struct GpsState *gps_s)

Variables

abi_event baro_ev
static abi_event accel_ev
static abi_event gps_ev
static abi_event vel_est_ev
static abi_event pos_est_ev
static abi_event agl_ev
 The agl ABI event.
static abi_event reset_ev
struct InsInt ins_int
 global INS state

Detailed Description

INS for rotorcrafts combining vertical and horizontal filters.

Definition in file ins_int.c.

Macro Definition Documentation

◆ INS_BARO_MAX_INIT_VAR

#define INS_BARO_MAX_INIT_VAR   1.f

default barometer to use in INS

Definition at line 105 of file ins_int.c.

Referenced by baro_cb().

◆ INS_INT_AGL_ID

#define INS_INT_AGL_ID   ABI_BROADCAST

ABI binding for AGL.

Usually this is comes from sonar or gps.

Definition at line 156 of file ins_int.c.

Referenced by ins_int_init().

◆ INS_INT_BARO_ID

#define INS_INT_BARO_ID   AIR_DATA_SENDER_ID

Definition at line 107 of file ins_int.c.

Referenced by ins_int_init().

◆ INS_INT_GPS_ID

#define INS_INT_GPS_ID   GPS_MULTI_ID

Definition at line 124 of file ins_int.c.

Referenced by ins_int_init().

◆ INS_INT_IMU_ID

#define INS_INT_IMU_ID   ABI_BROADCAST

ABI binding for IMU data.

Used accel ABI messages.

Definition at line 117 of file ins_int.c.

Referenced by ins_int_init().

◆ INS_INT_POS_ID

#define INS_INT_POS_ID   ABI_BROADCAST

Definition at line 143 of file ins_int.c.

Referenced by ins_int_init().

◆ INS_INT_VEL_ID

#define INS_INT_VEL_ID   ABI_BROADCAST

ABI binding for VELOCITY_ESTIMATE.

Usually this is coming from opticflow.

Definition at line 134 of file ins_int.c.

Referenced by ins_int_init().

◆ INS_MAX_PROPAGATION_STEPS

#define INS_MAX_PROPAGATION_STEPS   200

maximum number of propagation steps without any updates in between

Definition at line 96 of file ins_int.c.

Referenced by ins_int_init(), and ins_int_propagate().

◆ INS_VFF_R_GPS

#define INS_VFF_R_GPS   2.0

Definition at line 86 of file ins_int.c.

Referenced by ins_int_update_gps().

◆ INS_VFF_VZ_R_GPS

#define INS_VFF_VZ_R_GPS   2.0

Definition at line 90 of file ins_int.c.

Referenced by ins_int_update_gps().

◆ press_hist_len

#define press_hist_len   10

Referenced by baro_cb().

◆ USE_INS_NAV_INIT

#define USE_INS_NAV_INIT   TRUE

Definition at line 100 of file ins_int.c.

◆ VFF_R_AGL

#define VFF_R_AGL   0.2

Definition at line 59 of file ins_int.c.

Function Documentation

◆ accel_cb()

void accel_cb ( uint8_t sender_id,
uint32_t stamp,
struct Int32Vect3 * accel )
static

Definition at line 573 of file ins_int.c.

References ins_int_propagate(), and PRINT_CONFIG_MSG().

Here is the call graph for this function:

◆ agl_cb()

void agl_cb ( uint8_t sender_id,
uint32_t stamp,
float distance )
static

agl_cb This callback handles all estimates of the height of the vehicle above the ground under it This is only used with the extended version of the vertical float filter

Definition at line 534 of file ins_int.c.

◆ baro_cb()

void baro_cb ( uint8_t sender_id,
uint32_t stamp,
float pressure )
static

◆ gps_cb()

void gps_cb ( uint8_t sender_id,
uint32_t stamp ,
struct GpsState * gps_s )
static

Definition at line 587 of file ins_int.c.

References ins_int_update_gps().

Here is the call graph for this function:

◆ ins_int_init()

◆ ins_int_propagate()

void ins_int_propagate ( struct Int32Vect3 * accel,
float dt )

◆ ins_int_update_gps()

◆ ins_ned_to_state()

void ins_ned_to_state ( void )
static

copy position and speed to state interface

Definition at line 538 of file ins_int.c.

References ins_int, nps_bypass_ins, sim_overwrite_ins(), stateSetAccelNed_i(), stateSetPositionNed_i(), and stateSetSpeedNed_i().

Referenced by ins_int_propagate(), ins_int_update_gps(), ins_ned_to_state(), pos_est_cb(), and vel_est_cb().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ ins_update_from_vff()

void ins_update_from_vff ( void )
static

update ins state from vertical filter

Definition at line 552 of file ins_int.c.

References ACCEL_BFP_OF_REAL, ins_int, POS_BFP_OF_REAL, SPEED_BFP_OF_REAL, and vff.

Referenced by baro_cb(), and ins_int_propagate().

Here is the caller graph for this function:

◆ pos_est_cb()

void pos_est_cb ( uint8_t sender_id,
uint32_t stamp,
float x,
float y,
float z,
float noise_x,
float noise_y,
float noise_z )
static

Definition at line 659 of file ins_int.c.

References hff_update_pos(), ins_int, ins_ned_to_state(), POS_BFP_OF_REAL, vff_update_z_conf(), FloatVect2::x, and FloatVect2::y.

Referenced by ins_int_init().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ reset_cb()

void reset_cb ( uint8_t sender_id UNUSED,
uint8_t flag )
static

Definition at line 297 of file ins_int.c.

References INS_RESET_REF, INS_RESET_VERTICAL_POS, INS_RESET_VERTICAL_REF, reset_ref(), reset_vertical_pos(), reset_vertical_ref(), and UNUSED.

Here is the call graph for this function:

◆ reset_ref()

void reset_ref ( void )
static

Definition at line 250 of file ins_int.c.

References LlaCoor_i::alt, ecef_int_from_gps(), gps, GpsFixValid, ins_int, lla_int_from_gps(), ltp_def_from_ecef_i(), and stateSetLocalOrigin_i().

Referenced by ins_int_update_gps(), and reset_cb().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ reset_vertical_pos()

void reset_vertical_pos ( void )
static

Definition at line 292 of file ins_int.c.

References ins_int.

Referenced by reset_cb().

Here is the caller graph for this function:

◆ reset_vertical_ref()

void reset_vertical_ref ( void )
static

Definition at line 274 of file ins_int.c.

References LlaCoor_i::alt, gps, GpsFixValid, ins_int, LlaCoor_i::lat, lla_int_from_gps(), LlaCoor_i::lon, ltp_def_from_lla_i(), stateGetLlaOrigin_i(), and stateSetLocalOrigin_i().

Referenced by reset_cb().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ send_ins()

void send_ins ( struct transport_tx * trans,
struct link_device * dev )
static

Definition at line 171 of file ins_int.c.

References dev, and ins_int.

◆ send_ins_ref()

void send_ins_ref ( struct transport_tx * trans,
struct link_device * dev )
static

Definition at line 185 of file ins_int.c.

References dev, and ins_int.

◆ send_ins_z()

void send_ins_z ( struct transport_tx * trans,
struct link_device * dev )
static

Definition at line 179 of file ins_int.c.

References dev, and ins_int.

◆ vel_est_cb()

void vel_est_cb ( uint8_t sender_id,
uint32_t stamp,
float x,
float y,
float z,
float noise_x,
float noise_y,
float noise_z )
static

Definition at line 597 of file ins_int.c.

References float_quat_vmult(), hff_update_vel(), ins_int, ins_ned_to_state(), POS_BFP_OF_REAL, QUAT_INVERT, SPEED_BFP_OF_REAL, stateGetNedToBodyQuat_f(), vff_update_vz_conf(), FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by ins_int_init().

Here is the call graph for this function:
Here is the caller graph for this function:

Variable Documentation

◆ accel_ev

abi_event accel_ev
static

Definition at line 120 of file ins_int.c.

◆ agl_ev

abi_event agl_ev
static

The agl ABI event.

Definition at line 159 of file ins_int.c.

◆ baro_ev

abi_event baro_ev

Definition at line 110 of file ins_int.c.

◆ gps_ev

abi_event gps_ev
static

Definition at line 127 of file ins_int.c.

◆ ins_int

◆ pos_est_ev

abi_event pos_est_ev
static

Definition at line 146 of file ins_int.c.

Referenced by ins_int_init().

◆ reset_ev

abi_event reset_ev
static

Definition at line 163 of file ins_int.c.

◆ vel_est_ev

abi_event vel_est_ev
static

Definition at line 137 of file ins_int.c.

Referenced by ins_int_init().