Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
ins_ekf2.cpp File Reference
#include "subsystems/ins/ins_ekf2.h"
#include "subsystems/abi.h"
#include "stabilization/stabilization_attitude.h"
#include "generated/airframe.h"
#include "EKF/ekf.h"
#include "math/pprz_isa.h"
#include "mcu_periph/sys_time.h"
#include "autopilot.h"
#include "subsystems/datalink/telemetry.h"
+ Include dependency graph for ins_ekf2.cpp:

Go to the source code of this file.

Data Structures

struct  ekf2_t
 

Macros

#define INS_EKF2_AGL_ID   ABI_BROADCAST
 For SITL and NPS we need special includes. More...
 
#define INS_SONAR_MIN_RANGE   0.001
 Default AGL sensor minimum range. More...
 
#define INS_SONAR_MAX_RANGE   4.0
 Default AGL sensor maximum range. More...
 
#define INS_EKF2_BARO_ID   ABI_BROADCAST
 default barometer to use in INS More...
 
#define INS_EKF2_GYRO_ID   ABI_BROADCAST
 
#define INS_EKF2_ACCEL_ID   ABI_BROADCAST
 
#define INS_EKF2_MAG_ID   ABI_BROADCAST
 
#define INS_EKF2_GPS_ID   GPS_MULTI_ID
 

Functions

static void agl_cb (uint8_t sender_id, uint32_t stamp, float distance)
 
static void baro_cb (uint8_t sender_id, uint32_t stamp, float pressure)
 
static void gyro_cb (uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
 
static void accel_cb (uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
 
static void mag_cb (uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag)
 
static void gps_cb (uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
 
static void body_to_imu_cb (uint8_t sender_id, struct FloatQuat *q_b2i_f)
 
static void ins_ekf2_publish_attitude (uint32_t stamp)
 Publish the attitude and get the new state Directly called after a succeslfull gyro+accel reading. More...
 
static void send_ins_ref (struct transport_tx *trans, struct link_device *dev)
 
static void send_ins_ekf2 (struct transport_tx *trans, struct link_device *dev)
 
static void send_ins_ekf2_ext (struct transport_tx *trans, struct link_device *dev)
 
static void send_filter_status (struct transport_tx *trans, struct link_device *dev)
 
static void send_wind_info_ret (struct transport_tx *trans, struct link_device *dev)
 
static void send_ahrs_bias (struct transport_tx *trans, struct link_device *dev)
 
void ins_ekf2_init (void)
 
void ins_ekf2_update (void)
 
void ins_ekf2_change_param (int32_t unk)
 

Variables

static abi_event agl_ev
 
static abi_event baro_ev
 
static abi_event gyro_ev
 
static abi_event accel_ev
 
static abi_event mag_ev
 
static abi_event gps_ev
 
static abi_event body_to_imu_ev
 
struct gps_message gps_msg = {}
 
static Ekf ekf
 EKF class itself. More...
 
static parameters * ekf_params
 The EKF parameters. More...
 
struct ekf2_t ekf2
 Local EKF2 status structure. More...
 
static uint8_t ahrs_ekf2_id = AHRS_COMP_ID_EKF2
 Component ID for EKF. More...
 
struct ekf2_parameters_t ekf2_params
 

Detailed Description

INS based in the EKF2 of PX4

Definition in file ins_ekf2.cpp.


Data Structure Documentation

◆ ekf2_t

struct ekf2_t

Definition at line 121 of file ins_ekf2.cpp.

+ Collaboration diagram for ekf2_t:
Data Fields
FloatVect3 accel
uint32_t accel_dt
uint32_t accel_stamp
bool accel_valid
struct OrientationReps body_to_imu
bool got_imu_data
FloatRates gyro
uint32_t gyro_dt
uint32_t gyro_stamp
bool gyro_valid
struct LtpDef_i ltp_def
uint64_t ltp_stamp
uint8_t quat_reset_counter

Macro Definition Documentation

◆ INS_EKF2_ACCEL_ID

#define INS_EKF2_ACCEL_ID   ABI_BROADCAST

Definition at line 85 of file ins_ekf2.cpp.

◆ INS_EKF2_AGL_ID

#define INS_EKF2_AGL_ID   ABI_BROADCAST

For SITL and NPS we need special includes.

Prevent setting INS reference from flight plan default AGL sensor to use in INS

Definition at line 51 of file ins_ekf2.cpp.

◆ INS_EKF2_BARO_ID

#define INS_EKF2_BARO_ID   ABI_BROADCAST

default barometer to use in INS

Definition at line 72 of file ins_ekf2.cpp.

◆ INS_EKF2_GPS_ID

#define INS_EKF2_GPS_ID   GPS_MULTI_ID

Definition at line 97 of file ins_ekf2.cpp.

◆ INS_EKF2_GYRO_ID

#define INS_EKF2_GYRO_ID   ABI_BROADCAST

Definition at line 79 of file ins_ekf2.cpp.

◆ INS_EKF2_MAG_ID

#define INS_EKF2_MAG_ID   ABI_BROADCAST

Definition at line 91 of file ins_ekf2.cpp.

◆ INS_SONAR_MAX_RANGE

#define INS_SONAR_MAX_RANGE   4.0

Default AGL sensor maximum range.

Definition at line 63 of file ins_ekf2.cpp.

◆ INS_SONAR_MIN_RANGE

#define INS_SONAR_MIN_RANGE   0.001

Default AGL sensor minimum range.

Definition at line 57 of file ins_ekf2.cpp.

Function Documentation

◆ accel_cb()

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

Definition at line 491 of file ins_ekf2.cpp.

References ekf2_t::accel, ekf2_t::accel_dt, ekf2_t::accel_stamp, ekf2_t::accel_valid, ACCELS_FLOAT_OF_BFP, ekf2_t::body_to_imu, ekf2, float_rmat_transp_vmult(), ekf2_t::gyro_valid, ins_ekf2_publish_attitude(), and orientationGetRMat_f().

Referenced by ins_ekf2_init().

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

◆ agl_cb()

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

Definition at line 459 of file ins_ekf2.cpp.

References ekf.

Referenced by ins_ekf2_init().

+ Here is the caller graph for this function:

◆ baro_cb()

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

Definition at line 445 of file ins_ekf2.cpp.

References ekf, f, pprz_isa_density_of_pressure(), and pprz_isa_height_of_pressure_full().

Referenced by ins_ekf2_init().

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

◆ body_to_imu_cb()

static void body_to_imu_cb ( uint8_t  sender_id,
struct FloatQuat q_b2i_f 
)
static

Definition at line 570 of file ins_ekf2.cpp.

References ekf2_t::body_to_imu, ekf2, and orientationSetQuat_f().

Referenced by ins_ekf2_init().

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

◆ gps_cb()

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

◆ gyro_cb()

static void gyro_cb ( uint8_t  sender_id,
uint32_t  stamp,
struct Int32Rates gyro 
)
static

Definition at line 465 of file ins_ekf2.cpp.

References ekf2_t::accel_valid, ekf2_t::body_to_imu, ekf2, float_rmat_transp_ratemult(), ekf2_t::gyro, ekf2_t::gyro_dt, ekf2_t::gyro_stamp, ekf2_t::gyro_valid, ins_ekf2_publish_attitude(), orientationGetRMat_f(), and RATES_FLOAT_OF_BFP.

Referenced by ins_ekf2_init().

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

◆ ins_ekf2_change_param()

void ins_ekf2_change_param ( int32_t  unk)

Definition at line 364 of file ins_ekf2.cpp.

References ekf2_params, ekf_params, and ekf2_parameters_t::mag_fusion_type.

◆ ins_ekf2_init()

◆ ins_ekf2_publish_attitude()

◆ ins_ekf2_update()

◆ mag_cb()

static void mag_cb ( uint8_t  sender_id,
uint32_t  stamp,
struct Int32Vect3 mag 
)
static

Definition at line 517 of file ins_ekf2.cpp.

References ekf2_t::body_to_imu, ekf, ekf2, float_rmat_transp_vmult(), ekf2_t::got_imu_data, MAGS_FLOAT_OF_BFP, orientationGetRMat_f(), FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by ins_ekf2_init().

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

◆ send_ahrs_bias()

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

Definition at line 235 of file ins_ekf2.cpp.

References dev, ekf, and states.

Referenced by ins_ekf2_init().

+ Here is the caller graph for this function:

◆ send_filter_status()

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

Definition at line 198 of file ins_ekf2.cpp.

References ahrs_ekf2_id, dev, and ekf.

Referenced by ins_ekf2_init().

+ Here is the caller graph for this function:

◆ send_ins_ekf2()

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

Definition at line 165 of file ins_ekf2.cpp.

References dev, and ekf.

Referenced by ins_ekf2_init().

+ Here is the caller graph for this function:

◆ send_ins_ekf2_ext()

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

Definition at line 184 of file ins_ekf2.cpp.

References dev, and ekf.

Referenced by ins_ekf2_init().

+ Here is the caller graph for this function:

◆ send_ins_ref()

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

Definition at line 155 of file ins_ekf2.cpp.

References LlaCoor_i::alt, dev, LtpDef_i::ecef, ekf2, LtpDef_i::hmsl, LlaCoor_i::lat, LtpDef_i::lla, LlaCoor_i::lon, ekf2_t::ltp_def, ekf2_t::ltp_stamp, EcefCoor_i::x, EcefCoor_i::y, and EcefCoor_i::z.

Referenced by ins_ekf2_init().

+ Here is the caller graph for this function:

◆ send_wind_info_ret()

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

Definition at line 223 of file ins_ekf2.cpp.

References dev, and ekf.

Referenced by ins_ekf2_init().

+ Here is the caller graph for this function:

Variable Documentation

◆ accel_ev

abi_event accel_ev
static

Definition at line 105 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().

◆ agl_ev

abi_event agl_ev
static

Definition at line 102 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().

◆ ahrs_ekf2_id

uint8_t ahrs_ekf2_id = AHRS_COMP_ID_EKF2
static

Component ID for EKF.

Definition at line 147 of file ins_ekf2.cpp.

Referenced by send_filter_status().

◆ baro_ev

abi_event baro_ev
static

Definition at line 103 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().

◆ body_to_imu_ev

abi_event body_to_imu_ev
static

Definition at line 108 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().

◆ ekf

◆ ekf2

struct ekf2_t ekf2

Local EKF2 status structure.

Definition at line 146 of file ins_ekf2.cpp.

Referenced by accel_cb(), body_to_imu_cb(), gyro_cb(), ins_ekf2_init(), ins_ekf2_publish_attitude(), ins_ekf2_update(), mag_cb(), and send_ins_ref().

◆ ekf2_params

struct ekf2_parameters_t ekf2_params

Definition at line 150 of file ins_ekf2.cpp.

Referenced by ins_ekf2_change_param().

◆ ekf_params

parameters* ekf_params
static

The EKF parameters.

Definition at line 145 of file ins_ekf2.cpp.

Referenced by ins_ekf2_change_param(), and ins_ekf2_init().

◆ gps_ev

abi_event gps_ev
static

Definition at line 107 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().

◆ gps_msg

struct gps_message gps_msg = {}

Definition at line 109 of file ins_ekf2.cpp.

Referenced by gps_cb().

◆ gyro_ev

abi_event gyro_ev
static

Definition at line 104 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().

◆ mag_ev

abi_event mag_ev
static

Definition at line 106 of file ins_ekf2.cpp.

Referenced by ins_ekf2_init().