Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
ins_ext_pose.c File Reference

Integrated Navigation System interface. More...

#include <time.h>
#include "ins_ext_pose.h"
#include "state.h"
#include "math/pprz_algebra_float.h"
#include "modules/imu/imu.h"
#include "modules/ins/ins.h"
#include "generated/flight_plan.h"
#include "modules/core/abi.h"
#include "modules/datalink/telemetry.h"
+ Include dependency graph for ins_ext_pose.c:

Go to the source code of this file.

Data Structures

struct  InsExtPose
 Data for telemetry and LTP origin. More...
 

Macros

#define DEBUG_PRINT(...)   {}
 
#define INS_EXT_POSE_IMU_ID   ABI_BROADCAST
 Import Gyro and Acc from ABI. More...
 

Functions

static void ins_ext_pose_init_from_flightplan (void)
 
static void send_ins (struct transport_tx *trans, struct link_device *dev)
 Provide telemetry. More...
 
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 send_external_pose_down (struct transport_tx *trans, struct link_device *dev)
 
static void send_ahrs_bias (struct transport_tx *trans, struct link_device *dev)
 
static void accel_cb (uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
 
static void gyro_cb (uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
 
void ins_ext_pose_msg_update (uint8_t *buf)
 Import External Pose Message. More...
 
void ins_reset_local_origin (void)
 INS local origin reset. More...
 
void ins_reset_altitude_ref (void)
 INS altitude reference reset. More...
 
static void ekf_init (void)
 EKF protos. More...
 
static void ekf_run (void)
 
void ins_ext_pose_init (void)
 Module. More...
 
void ins_ext_pose_run (void)
 
static void ekf_f (const float X[EKF_NUM_STATES], const float U[EKF_NUM_INPUTS], float out[EKF_NUM_STATES])
 
static void ekf_F (const float X[EKF_NUM_STATES], const float U[EKF_NUM_INPUTS], float out[EKF_NUM_STATES][EKF_NUM_STATES])
 
static void ekf_L (const float X[EKF_NUM_STATES], const float U[EKF_NUM_INPUTS], float out[EKF_NUM_STATES][EKF_NUM_INPUTS])
 
static void ekf_f_rk4 (const float X[EKF_NUM_STATES], const float U[EKF_NUM_INPUTS], const float dt, float out[EKF_NUM_STATES])
 
static void ekf_step (const float U[EKF_NUM_INPUTS], const float Z[EKF_NUM_OUTPUTS], const float dt)
 
static void ekf_prediction_step (const float U[EKF_NUM_INPUTS], const float dt)
 
static void ekf_measurement_step (const float Z[EKF_NUM_OUTPUTS])
 
void ekf_set_diag (float **a, float *b, int n)
 
void ins_ext_pos_log_header (FILE *file)
 Logging. More...
 
void ins_ext_pos_log_data (FILE *file)
 

Variables

struct InsExtPose ins_ext_pos
 
static abi_event accel_ev
 
static abi_event gyro_ev
 
float ekf_X [EKF_NUM_STATES]
 
float ekf_U [EKF_NUM_INPUTS]
 
float ekf_Z [EKF_NUM_OUTPUTS]
 
float ekf_P [EKF_NUM_STATES][EKF_NUM_STATES]
 
float ekf_Q [EKF_NUM_INPUTS][EKF_NUM_INPUTS]
 
float ekf_R [EKF_NUM_OUTPUTS][EKF_NUM_OUTPUTS]
 
float ekf_H [EKF_NUM_OUTPUTS][EKF_NUM_STATES] = {{1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0}}
 
float t0
 
float t1
 

Detailed Description

Integrated Navigation System interface.

Definition in file ins_ext_pose.c.


Data Structure Documentation

◆ InsExtPose

struct InsExtPose

Data for telemetry and LTP origin.

Definition at line 51 of file ins_ext_pose.c.

+ Collaboration diagram for InsExtPose:
Data Fields
struct FloatVect3 accels_f
struct FloatEulers ev_att
struct FloatVect3 ev_pos
struct FloatQuat ev_quat
float ev_time
struct FloatVect3 ev_vel
struct FloatRates gyros_f
bool has_new_acc
bool has_new_ext_pose
bool has_new_gyro
struct NedCoor_i ltp_accel
struct LtpDef_i ltp_def
struct NedCoor_i ltp_pos
struct NedCoor_i ltp_speed

Macro Definition Documentation

◆ DEBUG_PRINT

#define DEBUG_PRINT (   ...)    {}

Definition at line 42 of file ins_ext_pose.c.

◆ INS_EXT_POSE_IMU_ID

#define INS_EXT_POSE_IMU_ID   ABI_BROADCAST

Import Gyro and Acc from ABI.

Definition at line 164 of file ins_ext_pose.c.

Function Documentation

◆ accel_cb()

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

Definition at line 184 of file ins_ext_pose.c.

References InsExtPose::accels_f, ACCELS_FLOAT_OF_BFP, InsExtPose::has_new_acc, and ins_ext_pos.

Referenced by ins_ext_pose_init().

+ Here is the caller graph for this function:

◆ ekf_f()

static void ekf_f ( const float  X[EKF_NUM_STATES],
const float  U[EKF_NUM_INPUTS],
float  out[EKF_NUM_STATES] 
)
inlinestatic

Definition at line 389 of file ins_ext_pose.c.

References mesonh.mesonh_atmosphere::X, and x12.

Referenced by ekf_f_rk4(), and ekf_prediction_step().

+ Here is the caller graph for this function:

◆ ekf_F()

static void ekf_F ( const float  X[EKF_NUM_STATES],
const float  U[EKF_NUM_INPUTS],
float  out[EKF_NUM_STATES][EKF_NUM_STATES] 
)
inlinestatic

Definition at line 428 of file ins_ext_pose.c.

References mesonh.mesonh_atmosphere::X, and x12.

Referenced by ekf_prediction_step(), and ekf_step().

+ Here is the caller graph for this function:

◆ ekf_f_rk4()

static void ekf_f_rk4 ( const float  X[EKF_NUM_STATES],
const float  U[EKF_NUM_INPUTS],
const float  dt,
float  out[EKF_NUM_STATES] 
)
inlinestatic

Definition at line 806 of file ins_ext_pose.c.

References ekf_f(), EKF_NUM_STATES, float_vect_add(), float_vect_scale(), float_vect_smul(), float_vect_sum(), and mesonh.mesonh_atmosphere::X.

Referenced by ekf_step().

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

◆ ekf_init()

static void ekf_init ( void  )
inlinestatic

EKF protos.

Definition at line 364 of file ins_ext_pose.c.

References DEBUG_PRINT, EKF_NUM_INPUTS, EKF_NUM_OUTPUTS, EKF_NUM_STATES, ekf_P, ekf_Q, ekf_R, ekf_set_diag(), ekf_U, ekf_X, ekf_Z, float_vect_copy(), and MAKE_MATRIX_PTR.

Referenced by ins_ext_pose_init().

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

◆ ekf_L()

static void ekf_L ( const float  X[EKF_NUM_STATES],
const float  U[EKF_NUM_INPUTS],
float  out[EKF_NUM_STATES][EKF_NUM_INPUTS] 
)
inlinestatic

Definition at line 697 of file ins_ext_pose.c.

References mesonh.mesonh_atmosphere::X.

Referenced by ekf_prediction_step(), and ekf_step().

+ Here is the caller graph for this function:

◆ ekf_measurement_step()

static void ekf_measurement_step ( const float  Z[EKF_NUM_OUTPUTS])
inlinestatic

◆ ekf_prediction_step()

static void ekf_prediction_step ( const float  U[EKF_NUM_INPUTS],
const float  dt 
)
inlinestatic

◆ ekf_run()

◆ ekf_set_diag()

void ekf_set_diag ( float **  a,
float *  b,
int  n 
)

Definition at line 348 of file ins_ext_pose.c.

References b.

Referenced by ekf_init().

+ Here is the caller graph for this function:

◆ ekf_step()

static void ekf_step ( const float  U[EKF_NUM_INPUTS],
const float  Z[EKF_NUM_OUTPUTS],
const float  dt 
)
inlinestatic

◆ gyro_cb()

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

Definition at line 176 of file ins_ext_pose.c.

References InsExtPose::gyros_f, InsExtPose::has_new_gyro, ins_ext_pos, and RATES_FLOAT_OF_BFP.

Referenced by ins_ext_pose_init().

+ Here is the caller graph for this function:

◆ ins_ext_pos_log_data()

void ins_ext_pos_log_data ( FILE *  file)

Definition at line 1311 of file ins_ext_pose.c.

Referenced by logger_file_write_row().

+ Here is the caller graph for this function:

◆ ins_ext_pos_log_header()

void ins_ext_pos_log_header ( FILE *  file)

Logging.

Definition at line 1303 of file ins_ext_pose.c.

Referenced by logger_file_write_header().

+ Here is the caller graph for this function:

◆ ins_ext_pose_init()

◆ ins_ext_pose_init_from_flightplan()

static void ins_ext_pose_init_from_flightplan ( void  )
static

Definition at line 76 of file ins_ext_pose.c.

References LlaCoor_i::alt, ecef_of_lla_i(), LtpDef_i::hmsl, ins_ext_pos, LlaCoor_i::lat, LlaCoor_i::lon, InsExtPose::ltp_def, ltp_def_from_ecef_i(), stateSetLocalOrigin_i(), and waypoints_localize_all().

Referenced by ins_ext_pose_init().

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

◆ ins_ext_pose_msg_update()

◆ ins_ext_pose_run()

void ins_ext_pose_run ( void  )

Definition at line 305 of file ins_ext_pose.c.

References ekf_run().

+ Here is the call graph for this function:

◆ ins_reset_altitude_ref()

void ins_reset_altitude_ref ( void  )

INS altitude reference reset.

Reset only vertical reference to the current altitude. Does nothing if not implemented by specific INS algorithm.

Definition at line 260 of file ins_ext_pose.c.

◆ ins_reset_local_origin()

void ins_reset_local_origin ( void  )

INS local origin reset.

Reset horizontal and vertical reference to the current position. Does nothing if not implemented by specific INS algorithm.

INS local origin reset.

Definition at line 255 of file ins_ext_pose.c.

◆ send_ahrs_bias()

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

Definition at line 142 of file ins_ext_pose.c.

References dev, and ekf_X.

Referenced by ins_ext_pose_init().

+ Here is the caller graph for this function:

◆ send_external_pose_down()

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

◆ send_ins()

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

Provide telemetry.

Definition at line 102 of file ins_ext_pose.c.

References dev, ins_ext_pos, InsExtPose::ltp_accel, InsExtPose::ltp_pos, InsExtPose::ltp_speed, NedCoor_i::x, NedCoor_i::y, and NedCoor_i::z.

Referenced by ins_ext_pose_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 118 of file ins_ext_pose.c.

References LlaCoor_i::alt, dev, LtpDef_i::ecef, LtpDef_i::hmsl, ins_ext_pos, LlaCoor_i::lat, LtpDef_i::lla, LlaCoor_i::lon, InsExtPose::ltp_def, EcefCoor_i::x, EcefCoor_i::y, and EcefCoor_i::z.

Referenced by ins_ext_pose_init().

+ Here is the caller graph for this function:

◆ send_ins_z()

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

Definition at line 110 of file ins_ext_pose.c.

References dev, ins_ext_pos, InsExtPose::ltp_accel, InsExtPose::ltp_pos, InsExtPose::ltp_speed, and NedCoor_i::z.

Referenced by ins_ext_pose_init().

+ Here is the caller graph for this function:

Variable Documentation

◆ accel_ev

abi_event accel_ev
static

Definition at line 168 of file ins_ext_pose.c.

Referenced by ins_ext_pose_init().

◆ ekf_H

float ekf_H[EKF_NUM_OUTPUTS][EKF_NUM_STATES] = {{1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0}}

Definition at line 341 of file ins_ext_pose.c.

Referenced by ekf_measurement_step(), and ekf_step().

◆ ekf_P

Definition at line 337 of file ins_ext_pose.c.

Referenced by ekf_init(), ekf_measurement_step(), ekf_prediction_step(), and ekf_step().

◆ ekf_Q

Definition at line 338 of file ins_ext_pose.c.

Referenced by ekf_init(), ekf_prediction_step(), and ekf_step().

◆ ekf_R

Definition at line 339 of file ins_ext_pose.c.

Referenced by ekf_init(), ekf_measurement_step(), and ekf_step().

◆ ekf_U

float ekf_U[EKF_NUM_INPUTS]

Definition at line 335 of file ins_ext_pose.c.

Referenced by ekf_init(), and ekf_run().

◆ ekf_X

◆ ekf_Z

float ekf_Z[EKF_NUM_OUTPUTS]

Definition at line 336 of file ins_ext_pose.c.

Referenced by ekf_init(), and ekf_run().

◆ gyro_ev

abi_event gyro_ev
static

Definition at line 169 of file ins_ext_pose.c.

Referenced by ins_ext_pose_init().

◆ ins_ext_pos

◆ t0

float t0

Definition at line 344 of file ins_ext_pose.c.

Referenced by ekf_run().

◆ t1

float t1