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	INS_EXT_POSE_MAX_AGE 30.f
	Maximum age of an external pose [ms], 0 to disable the check.

#define	INS_EXT_POSE_AGE_DRIFT 0.01f
	Rate at which the lowest delay may rise [ms per received pose], to follow clock drift.

#define	INS_EXT_POSE_AGE_RESYNC 1000
	Delay jump above which the sender clock is considered restarted [ms].

#define	INS_EXT_POSE_AGE_RESET_TIME 300
	Time with all poses too old after which the lowest delay is reset [ms].

#define	INS_EXT_POSE_GATE_PSI RadOfDeg(40.f)
	Innovation gates [rad], 0 to disable.

#define	INS_EXT_POSE_GATE_TILT RadOfDeg(15.f)

#define	INS_EXT_POSE_GATE_RESET_NB 10
	Number of consecutive gated poses after which the filter is reset on the pose.

#define	INS_EXT_POSE_TIMEOUT 0.5f
	Time without fused pose after which the filter status is reported as lost [s].

#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)

static bool	ins_ext_pose_check_age (uint32_t ts)
	Import External Pose Message.

void	ins_ext_pose_msg_update (uint8_t *buf)

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)

static float	ekf_psi_meas (void)
	Heading measurement close to the (unwrapped) heading state.

static void	ekf_reset_on_pose (void)
	Set position and attitude to the external pose and restore the initial covariance.

void	ins_ext_pose_log_header (FILE *file)
	Logging.

void	ins_ext_pose_log_data (FILE *file)

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

Collaboration diagram for InsExtPose:

Data Fields
struct FloatVect3	accels_f
uint32_t	age_ok_time	receive time of the last pose with a valid age [ms]
float	delay_min	lowest recent receive delay, relative to delay_ref [ms]
uint32_t	delay_ref	receive delay reference (includes clock offset) [ms]
float	ev_age	age of the last pose [ms]
struct FloatEulers	ev_att
struct FloatVect3	ev_pos
struct FloatQuat	ev_quat
float	ev_time
struct FloatVect3	ev_vel
uint8_t	gate_cnt	consecutive gated poses
struct FloatRates	gyros_f
bool	has_new_acc
bool	has_new_ext_pose
bool	has_new_gyro
float	last_fused	time of the last fused pose [s]
struct NedCoor_i	ltp_accel
struct LtpDef_i	ltp_def
struct NedCoor_i	ltp_pos
struct NedCoor_i	ltp_speed
uint16_t	nb_gated	poses dropped by the innovation gates
uint16_t	nb_reset	filter resets after repeated gating
uint16_t	nb_stale	poses dropped for age or order
bool	started
uint32_t	ts_last	last accepted capture time [ms]
bool	ts_valid	capture time reference initialized

Macro Definition Documentation

◆ DEBUG_PRINT

#define DEBUG_PRINT ( ... ) {}

Definition at line 107 of file ins_ext_pose.c.

◆ INS_EXT_POSE_AGE_DRIFT

#define INS_EXT_POSE_AGE_DRIFT 0.01f

Rate at which the lowest delay may rise [ms per received pose], to follow clock drift.

Definition at line 63 of file ins_ext_pose.c.

◆ INS_EXT_POSE_AGE_RESET_TIME

#define INS_EXT_POSE_AGE_RESET_TIME 300

Time with all poses too old after which the lowest delay is reset [ms].

Follows a lasting increase of the link delay; the innovation gates still apply.

Definition at line 75 of file ins_ext_pose.c.

◆ INS_EXT_POSE_AGE_RESYNC

#define INS_EXT_POSE_AGE_RESYNC 1000

Delay jump above which the sender clock is considered restarted [ms].

Definition at line 68 of file ins_ext_pose.c.

◆ INS_EXT_POSE_GATE_PSI

#define INS_EXT_POSE_GATE_PSI RadOfDeg(40.f)

Innovation gates [rad], 0 to disable.

A pose whose heading or roll/pitch differs more than this from the prediction is not fused.

Definition at line 82 of file ins_ext_pose.c.

◆ INS_EXT_POSE_GATE_RESET_NB

#define INS_EXT_POSE_GATE_RESET_NB 10

Number of consecutive gated poses after which the filter is reset on the pose.

Definition at line 91 of file ins_ext_pose.c.

◆ INS_EXT_POSE_GATE_TILT

#define INS_EXT_POSE_GATE_TILT RadOfDeg(15.f)

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

◆ INS_EXT_POSE_MAX_AGE

#define INS_EXT_POSE_MAX_AGE 30.f

Maximum age of an external pose [ms], 0 to disable the check.

The age is the receive delay above the lowest delay seen recently, computed from the capture time in the timestamp field of EXTERNAL_POSE (ms, sender clock). Not used when the sender leaves the timestamp at 0.

Definition at line 58 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.

◆ INS_EXT_POSE_TIMEOUT

#define INS_EXT_POSE_TIMEOUT 0.5f

Time without fused pose after which the filter status is reported as lost [s].

Definition at line 96 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 281 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

Definition at line 528 of file ins_ext_pose.c.

References EKF_U_ACC_X, EKF_U_ACC_Y, EKF_U_ACC_Z, EKF_U_GYRO_P, EKF_U_GYRO_Q, EKF_U_GYRO_R, EKF_X_A_BIAS_X, EKF_X_A_BIAS_Y, EKF_X_A_BIAS_Z, EKF_X_G_BIAS_P, EKF_X_G_BIAS_Q, EKF_X_G_BIAS_R, EKF_X_PHI, EKF_X_POS_X, EKF_X_POS_Y, EKF_X_POS_Z, EKF_X_PSI, EKF_X_THETA, EKF_X_VEL_X, EKF_X_VEL_Y, EKF_X_VEL_Z, foo, and x12.

Referenced by 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]
	)

static

Definition at line 567 of file ins_ext_pose.c.

References EKF_NUM_STATES, EKF_U_ACC_X, EKF_U_ACC_Y, EKF_U_ACC_Z, EKF_U_GYRO_Q, EKF_U_GYRO_R, EKF_X_A_BIAS_X, EKF_X_A_BIAS_Y, EKF_X_A_BIAS_Z, EKF_X_G_BIAS_P, EKF_X_G_BIAS_Q, EKF_X_G_BIAS_R, EKF_X_PHI, EKF_X_POS_X, EKF_X_POS_Y, EKF_X_POS_Z, EKF_X_PSI, EKF_X_THETA, EKF_X_VEL_X, EKF_X_VEL_Y, EKF_X_VEL_Z, foo, and x12.

Referenced by ekf_prediction_step().

Here is the caller graph for this function:

◆ ekf_init()

static void ekf_init ( void )

static

EKF protos.

Definition at line 504 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 645 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

Definition at line 860 of file ins_ext_pose.c.

References ekf_H, EKF_NUM_OUTPUTS, EKF_NUM_STATES, ekf_P, ekf_R, ekf_X, float_mat_copy(), float_mat_invert(), float_mat_mul(), float_mat_mul_copy(), float_mat_scale(), float_mat_sum_scaled(), float_mat_transpose(), float_mat_vect_mul(), float_vect_add(), float_vect_copy(), float_vect_scale(), foo, K, and MAKE_MATRIX_PTR.

Referenced by ekf_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ ekf_prediction_step()

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

static

Definition at line 733 of file ins_ext_pose.c.

References ekf_f(), ekf_F(), ekf_L(), EKF_NUM_INPUTS, EKF_NUM_STATES, ekf_P, ekf_Q, ekf_X, float_mat_copy(), float_mat_diagonal_scal(), float_mat_mul(), float_mat_scale(), float_mat_sum_scaled(), float_mat_transpose(), float_mat_transpose_square(), float_vect_add(), float_vect_copy(), float_vect_scale(), foo, and MAKE_MATRIX_PTR.

Referenced by ekf_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ ekf_psi_meas()

static float ekf_psi_meas ( void )

static

Heading measurement close to the (unwrapped) heading state.

Definition at line 946 of file ins_ext_pose.c.

References ekf_X, EKF_X_PSI, InsExtPose::ev_att, FLOAT_ANGLE_NORMALIZE, foo, ins_ext_pose, and FloatEulers::psi.

Referenced by ekf_reset_on_pose(), and ekf_run().

Here is the caller graph for this function:

◆ ekf_reset_on_pose()

static void ekf_reset_on_pose ( void )

static

Set position and attitude to the external pose and restore the initial covariance.

Definition at line 954 of file ins_ext_pose.c.

References EKF_NUM_STATES, ekf_P, ekf_psi_meas(), ekf_set_diag(), ekf_X, EKF_X_PHI, EKF_X_POS_X, EKF_X_POS_Y, EKF_X_POS_Z, EKF_X_PSI, EKF_X_THETA, InsExtPose::ev_att, InsExtPose::ev_pos, foo, ins_ext_pose, INS_EXT_POSE_P0, MAKE_MATRIX_PTR, FloatEulers::phi, FloatEulers::theta, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by ekf_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ ekf_run()

static void ekf_run ( void )

static

Definition at line 968 of file ins_ext_pose.c.

References ACCELS_BFP_OF_REAL, InsExtPose::accels_f, DEBUG_PRINT, ekf_measurement_step(), ekf_prediction_step(), ekf_psi_meas(), ekf_reset_on_pose(), ekf_U, EKF_U_ACC_X, EKF_U_ACC_Y, EKF_U_ACC_Z, EKF_U_GYRO_P, EKF_U_GYRO_Q, EKF_U_GYRO_R, ekf_X, EKF_X_A_BIAS_X, EKF_X_A_BIAS_Y, EKF_X_A_BIAS_Z, EKF_X_G_BIAS_P, EKF_X_G_BIAS_Q, EKF_X_G_BIAS_R, EKF_X_PHI, EKF_X_POS_X, EKF_X_POS_Y, EKF_X_POS_Z, EKF_X_PSI, EKF_X_THETA, EKF_X_VEL_X, EKF_X_VEL_Y, EKF_X_VEL_Z, ekf_Z, EKF_Z_PHI, EKF_Z_POS_X, EKF_Z_POS_Y, EKF_Z_POS_Z, EKF_Z_PSI, EKF_Z_THETA, InsExtPose::ev_att, InsExtPose::ev_pos, FLOAT_ANGLE_NORMALIZE, float_rmat_of_eulers, float_rmat_transp_vmult(), foo, InsExtPose::gate_cnt, get_sys_time_float(), InsExtPose::gyros_f, InsExtPose::has_new_acc, InsExtPose::has_new_ext_pose, InsExtPose::has_new_gyro, ins_ext_pose, ins_ext_pose_gate_psi, INS_EXT_POSE_GATE_RESET_NB, ins_ext_pose_gate_tilt, InsExtPose::last_fused, InsExtPose::ltp_accel, InsExtPose::ltp_pos, InsExtPose::ltp_speed, InsExtPose::nb_gated, InsExtPose::nb_reset, FloatRates::p, FloatEulers::phi, POSITIONS_BFP_OF_REAL, FloatRates::q, FloatRates::r, SPEEDS_BFP_OF_REAL, InsExtPose::started, stateSetAccelBody_i(), stateSetAccelNed_f(), stateSetBodyRates_f(), stateSetNedToBodyEulers_f(), stateSetPositionNed_f(), stateSetSpeedNed_f(), t0, t1, FloatEulers::theta, VECT3_COPY, FloatVect3::x, NedCoor_f::x, FloatVect3::y, and FloatVect3::z.

Referenced by ins_ext_pose_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ ekf_set_diag()

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

static

Definition at line 488 of file ins_ext_pose.c.

References b, and foo.

Referenced by ekf_init(), and ekf_reset_on_pose().

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 273 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_check_age()

static bool ins_ext_pose_check_age ( uint32_t ts )

static

Import External Pose Message.

Check the age of a pose from its capture time. The receive delay (receive time - capture time) includes an unknown clock offset, so the age is taken as the delay above the lowest delay seen recently.

Parameters

ts	capture time [ms], 0 if not provided

Returns: true if the pose is recent enough and newer than the previous one

Definition at line 300 of file ins_ext_pose.c.

References InsExtPose::age_ok_time, delay, InsExtPose::delay_min, InsExtPose::delay_ref, InsExtPose::ev_age, foo, get_sys_time_msec(), ins_ext_pose, INS_EXT_POSE_AGE_DRIFT, INS_EXT_POSE_AGE_RESET_TIME, INS_EXT_POSE_AGE_RESYNC, ins_ext_pose_max_age, Min, InsExtPose::ts_last, and InsExtPose::ts_valid.

Referenced by ins_ext_pose_msg_update().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ ins_ext_pose_init()

void ins_ext_pose_init ( void )

Module.

Definition at line 405 of file ins_ext_pose.c.

References accel_cb(), accel_ev, DefaultPeriodic, ekf_init(), InsExtPose::ev_age, foo, InsExtPose::gate_cnt, gyro_cb(), gyro_ev, InsExtPose::has_new_acc, InsExtPose::has_new_ext_pose, InsExtPose::has_new_gyro, ins_ext_pose, INS_EXT_POSE_IMU_ID, ins_ext_pose_init_from_flightplan(), InsExtPose::last_fused, InsExtPose::nb_gated, InsExtPose::nb_reset, InsExtPose::nb_stale, register_periodic_telemetry(), send_ahrs_bias(), send_external_pose_down(), send_filter_status(), send_ins(), send_ins_ref(), send_ins_z(), InsExtPose::started, and InsExtPose::ts_valid.

Here is the call graph for this function:

◆ ins_ext_pose_init_from_flightplan()

static void ins_ext_pose_init_from_flightplan ( void )

static

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

References ekf_U, ekf_X, ekf_Z, and foo.

Referenced by logger_file_write_row().

Here is the caller graph for this function:

◆ ins_ext_pose_log_header()

void ins_ext_pose_log_header ( FILE * file )

Logging.

Definition at line 1115 of file ins_ext_pose.c.

References foo.

Referenced by logger_file_write_header().

Here is the caller graph for this function:

◆ ins_ext_pose_msg_update()

void ins_ext_pose_msg_update ( uint8_t * buf )

Definition at line 343 of file ins_ext_pose.c.

References DEBUG_PRINT, ENU_OF_TO_NED, EULERS_COPY, InsExtPose::ev_att, InsExtPose::ev_pos, InsExtPose::ev_quat, InsExtPose::ev_time, InsExtPose::ev_vel, float_eulers_of_quat(), float_quat_comp(), foo, get_sys_time_usec(), InsExtPose::has_new_ext_pose, ins_ext_pose, ins_ext_pose_check_age(), InsExtPose::nb_stale, FloatEulers::phi, FloatEulers::psi, FloatQuat::qi, QUAT_COPY, FloatQuat::qx, FloatQuat::qy, FloatQuat::qz, FloatEulers::theta, and EnuCoor_f::x.

Here is the call graph for this function:

◆ ins_ext_pose_run()

void ins_ext_pose_run ( void )

Definition at line 443 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 240 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

Definition at line 224 of file ins_ext_pose.c.

References dev, InsExtPose::ev_pos, InsExtPose::ev_quat, InsExtPose::ev_time, InsExtPose::ev_vel, foo, ins_ext_pose, FloatQuat::qi, FloatQuat::qx, FloatQuat::qy, FloatQuat::qz, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

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

References AHRS_COMP_ID_GENERIC, dev, foo, get_sys_time_float(), ins_ext_pose, INS_EXT_POSE_TIMEOUT, InsExtPose::last_fused, InsExtPose::nb_gated, InsExtPose::nb_stale, and InsExtPose::started.

Referenced by ins_ext_pose_init().

Here is the call graph for this function:

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 183 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 199 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 191 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 266 of file ins_ext_pose.c.

Referenced by ins_ext_pose_init().

◆ ekf_H

float ekf_H[EKF_NUM_OUTPUTS][EKF_NUM_STATES]

static

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

Referenced by ekf_measurement_step().

◆ ekf_P

float ekf_P[EKF_NUM_STATES][EKF_NUM_STATES]

static

Definition at line 472 of file ins_ext_pose.c.

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

◆ ekf_Q

float ekf_Q[EKF_NUM_INPUTS][EKF_NUM_INPUTS]

static

Definition at line 473 of file ins_ext_pose.c.

Referenced by ekf_init(), and ekf_prediction_step().

◆ ekf_R

float ekf_R[EKF_NUM_OUTPUTS][EKF_NUM_OUTPUTS]

static

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

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

◆ ekf_X

float ekf_X[EKF_NUM_STATES]

Definition at line 469 of file ins_ext_pose.c.

Referenced by ekf_init(), ekf_measurement_step(), ekf_prediction_step(), ekf_psi_meas(), ekf_reset_on_pose(), ekf_run(), ins_ext_pose_log_data(), and send_ahrs_bias().

◆ ekf_Z

float ekf_Z[EKF_NUM_OUTPUTS]

static

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

Referenced by ins_ext_pose_init().

◆ ins_ext_pose

struct InsExtPose ins_ext_pose

Definition at line 155 of file ins_ext_pose.c.

Referenced by accel_cb(), ekf_psi_meas(), ekf_reset_on_pose(), ekf_run(), gyro_cb(), ins_ext_pose_check_age(), ins_ext_pose_init(), ins_ext_pose_init_from_flightplan(), ins_ext_pose_msg_update(), send_external_pose_down(), send_filter_status(), send_ins(), send_ins_ref(), and send_ins_z().

◆ ins_ext_pose_gate_psi

float ins_ext_pose_gate_psi = INS_EXT_POSE_GATE_PSI

Definition at line 100 of file ins_ext_pose.c.

Referenced by ekf_run().

◆ ins_ext_pose_gate_tilt

float ins_ext_pose_gate_tilt = INS_EXT_POSE_GATE_TILT

Definition at line 101 of file ins_ext_pose.c.

Referenced by ekf_run().

◆ ins_ext_pose_max_age

float ins_ext_pose_max_age = INS_EXT_POSE_MAX_AGE

Definition at line 99 of file ins_ext_pose.c.

Referenced by ins_ext_pose_check_age().

◆ t0

float t0

static

Definition at line 485 of file ins_ext_pose.c.

Referenced by amt22_read(), and ekf_run().

◆ t1

float t1

static

Definition at line 486 of file ins_ext_pose.c.

Referenced by amt22_read(), distributed_circular(), ekf_run(), mtostk_apply_polynomial_temp(), nps_main_loop(), and oneloop_nB_calc_nB_states().

Variables
float	ins_ext_pose_max_age = INS_EXT_POSE_MAX_AGE

float	ins_ext_pose_gate_psi = INS_EXT_POSE_GATE_PSI

float	ins_ext_pose_gate_tilt = INS_EXT_POSE_GATE_TILT

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

Data Structures

Macros

Functions

Variables

Detailed Description

Data Structure Documentation

◆ InsExtPose

Macro Definition Documentation

◆ DEBUG_PRINT

◆ INS_EXT_POSE_AGE_DRIFT

◆ INS_EXT_POSE_AGE_RESET_TIME

◆ INS_EXT_POSE_AGE_RESYNC

◆ INS_EXT_POSE_GATE_PSI

◆ INS_EXT_POSE_GATE_RESET_NB

◆ INS_EXT_POSE_GATE_TILT

◆ INS_EXT_POSE_IMU_ID

◆ INS_EXT_POSE_MAX_AGE

◆ INS_EXT_POSE_P0

◆ INS_EXT_POSE_Q_NOISE

◆ INS_EXT_POSE_R_NOISE

◆ INS_EXT_POSE_TIMEOUT

Function Documentation

◆ accel_cb()

◆ ekf_f()

◆ ekf_F()

◆ ekf_init()

◆ ekf_L()

◆ ekf_measurement_step()

◆ ekf_prediction_step()

◆ ekf_psi_meas()

◆ ekf_reset_on_pose()

◆ ekf_run()

◆ ekf_set_diag()

◆ gyro_cb()

◆ ins_ext_pose_check_age()

◆ ins_ext_pose_init()

◆ ins_ext_pose_init_from_flightplan()

◆ ins_ext_pose_log_data()

◆ ins_ext_pose_log_header()

◆ ins_ext_pose_msg_update()

◆ ins_ext_pose_run()

◆ send_ahrs_bias()

◆ send_external_pose_down()

◆ send_filter_status()

◆ send_ins()

◆ send_ins_ref()

◆ send_ins_z()

Variable Documentation

◆ accel_ev

◆ ekf_H

◆ ekf_P

◆ ekf_Q

◆ ekf_R

◆ ekf_U

◆ ekf_X

◆ ekf_Z

◆ gyro_ev

◆ ins_ext_pose

◆ ins_ext_pose_gate_psi

◆ ins_ext_pose_gate_tilt

◆ ins_ext_pose_max_age

◆ t0

◆ t1