Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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.h"
#include "math/pprz_algebra_float.h"
#include "modules/imu/imu.h"
#include "modules/ahrs/ahrs.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 INS_EXT_POSE_P0   {1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1.}
 
#define INS_EXT_POSE_Q_NOISE   {1.0, 1.0, 1.0, 0.0173, 4.878e-4, 3.547e-4}
 
#define INS_EXT_POSE_R_NOISE   {8.372e-6, 3.832e-6, 4.761e-6, 2.830e-4, 8.684e-6, 7.013e-6}
 
#define DEBUG_PRINT(...)   {}
 
#define INS_EXT_POSE_IMU_ID   ABI_BROADCAST
 Import Gyro and Acc from ABI.
 

Functions

static void ins_ext_pose_init_from_flightplan (void)
 
static void send_ins (struct transport_tx *trans, struct link_device *dev)
 Provide telemetry.
 
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_filter_status (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.
 
static void ekf_init (void)
 EKF protos.
 
static void ekf_run (void)
 
void ins_ext_pose_init (void)
 Module.
 
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_prediction_step (const float U[EKF_NUM_INPUTS], const float dt)
 
static void ekf_measurement_step (const float Z[EKF_NUM_OUTPUTS])
 
static void ekf_set_diag (float **a, float *b, int n)
 
void ins_ext_pose_log_header (FILE *file)
 Logging.
 
void ins_ext_pose_log_data (FILE *file)
 

Variables

struct InsExtPose ins_ext_pose
 
static abi_event accel_ev
 
static abi_event gyro_ev
 
float ekf_X [EKF_NUM_STATES]
 
static float ekf_U [EKF_NUM_INPUTS]
 
static float ekf_Z [EKF_NUM_OUTPUTS]
 
static float ekf_P [EKF_NUM_STATES][EKF_NUM_STATES]
 
static float ekf_Q [EKF_NUM_INPUTS][EKF_NUM_INPUTS]
 
static float ekf_R [EKF_NUM_OUTPUTS][EKF_NUM_OUTPUTS]
 
static float ekf_H [EKF_NUM_OUTPUTS][EKF_NUM_STATES]
 
static float t0
 
static 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 65 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
bool started

Macro Definition Documentation

◆ DEBUG_PRINT

#define DEBUG_PRINT (   ...)    {}

Definition at line 56 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 194 of file ins_ext_pose.c.

◆ INS_EXT_POSE_P0

#define INS_EXT_POSE_P0   {1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1.}

Definition at line 41 of file ins_ext_pose.c.

◆ INS_EXT_POSE_Q_NOISE

#define INS_EXT_POSE_Q_NOISE   {1.0, 1.0, 1.0, 0.0173, 4.878e-4, 3.547e-4}

Definition at line 45 of file ins_ext_pose.c.

◆ INS_EXT_POSE_R_NOISE

#define INS_EXT_POSE_R_NOISE   {8.372e-6, 3.832e-6, 4.761e-6, 2.830e-4, 8.684e-6, 7.013e-6}

Definition at line 49 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 213 of file ins_ext_pose.c.

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

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] 
)
static

◆ 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] 
)
static

◆ ekf_init()

static void ekf_init ( void  )
static

EKF protos.

Definition at line 375 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(), foo, INS_EXT_POSE_P0, INS_EXT_POSE_Q_NOISE, INS_EXT_POSE_R_NOISE, 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] 
)
static

Definition at line 516 of file ins_ext_pose.c.

References EKF_NUM_INPUTS, EKF_NUM_STATES, EKF_X_PHI, EKF_X_PSI, EKF_X_THETA, EKF_X_VEL_X, EKF_X_VEL_Y, EKF_X_VEL_Z, EKF_Z_PHI, EKF_Z_POS_X, EKF_Z_POS_Y, EKF_Z_POS_Z, EKF_Z_PSI, EKF_Z_THETA, and foo.

Referenced by ekf_prediction_step().

+ Here is the caller graph for this function:

◆ ekf_measurement_step()

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

◆ ekf_prediction_step()

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

◆ ekf_run()

◆ ekf_set_diag()

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

Definition at line 359 of file ins_ext_pose.c.

References b, and foo.

Referenced by ekf_init().

+ Here is the caller graph for this function:

◆ gyro_cb()

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

Definition at line 205 of file ins_ext_pose.c.

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

Referenced by ins_ext_pose_init().

+ 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 94 of file ins_ext_pose.c.

References ecef_of_lla_i(), foo, LtpDef_i::hmsl, ins_ext_pose, LlaCoor_i::lat, 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_log_data()

void ins_ext_pose_log_data ( FILE file)

Definition at line 956 of file ins_ext_pose.c.

References ekf_U, ekf_X, ekf_Z, and foo.

◆ ins_ext_pose_log_header()

void ins_ext_pose_log_header ( FILE file)

Logging.

Definition at line 948 of file ins_ext_pose.c.

References foo.

◆ ins_ext_pose_msg_update()

◆ ins_ext_pose_run()

void ins_ext_pose_run ( void  )

Definition at line 314 of file ins_ext_pose.c.

References ekf_run().

+ Here is the call graph for this function:

◆ send_ahrs_bias()

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

Definition at line 172 of file ins_ext_pose.c.

References dev, ekf_X, and foo.

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_filter_status()

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

Definition at line 144 of file ins_ext_pose.c.

References AHRS_COMP_ID_GENERIC, dev, foo, ins_ext_pose, and InsExtPose::started.

Referenced by ins_ext_pose_init().

+ Here is the caller graph for this function:

◆ send_ins()

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

Provide telemetry.

Definition at line 119 of file ins_ext_pose.c.

References dev, foo, ins_ext_pose, 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 135 of file ins_ext_pose.c.

References LlaCoor_i::alt, dev, LtpDef_i::ecef, foo, LtpDef_i::hmsl, ins_ext_pose, 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 127 of file ins_ext_pose.c.

References dev, foo, ins_ext_pose, 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 198 of file ins_ext_pose.c.

Referenced by ins_ext_pose_init().

◆ ekf_H

Initial value:
= {
{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 347 of file ins_ext_pose.c.

Referenced by ekf_measurement_step().

◆ ekf_P

Definition at line 343 of file ins_ext_pose.c.

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

◆ ekf_Q

Definition at line 344 of file ins_ext_pose.c.

Referenced by ekf_init(), and ekf_prediction_step().

◆ ekf_R

Definition at line 345 of file ins_ext_pose.c.

Referenced by ekf_init(), and ekf_measurement_step().

◆ ekf_U

float ekf_U[EKF_NUM_INPUTS]
static

Definition at line 341 of file ins_ext_pose.c.

Referenced by ekf_init(), ekf_run(), and ins_ext_pose_log_data().

◆ ekf_X

◆ ekf_Z

float ekf_Z[EKF_NUM_OUTPUTS]
static

Definition at line 342 of file ins_ext_pose.c.

Referenced by ekf_init(), ekf_run(), and ins_ext_pose_log_data().

◆ gyro_ev

abi_event gyro_ev
static

Definition at line 199 of file ins_ext_pose.c.

Referenced by ins_ext_pose_init().

◆ ins_ext_pose

◆ t0

float t0
static

Definition at line 356 of file ins_ext_pose.c.

Referenced by amt22_read(), and ekf_run().

◆ t1