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...

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 378 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 417 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 795 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 353 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 686 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

Definition at line 1077 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(), K, MAKE_MATRIX_PTR, and mesonh.mesonh_atmosphere::Z.

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
	)

inlinestatic

Definition at line 996 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(), 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_run()

static void ekf_run ( void )

inlinestatic

Definition at line 1165 of file ins_ext_pose.c.

References ACCELS_BFP_OF_REAL, InsExtPose::accels_f, DEBUG_PRINT, ekf_measurement_step(), ekf_prediction_step(), ekf_U, ekf_X, ekf_Z, InsExtPose::ev_att, InsExtPose::ev_pos, float_rmat_transp_vmult(), get_sys_time_float(), InsExtPose::gyros_f, InsExtPose::has_new_acc, InsExtPose::has_new_ext_pose, InsExtPose::has_new_gyro, ins_ext_pos, FloatRates::p, FloatEulers::phi, FloatEulers::psi, FloatRates::q, FloatRates::r, stateGetNedToBodyRMat_f(), stateSetAccelBody_i(), stateSetAccelNed_f(), stateSetBodyRates_f(), stateSetNedToBodyEulers_f(), stateSetPositionNed_f(), stateSetSpeedNed_f(), t0, t1, FloatEulers::theta, FloatVect3::x, NedCoor_f::x, FloatVect3::y, NedCoor_f::y, FloatVect3::z, and NedCoor_f::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()

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

Definition at line 337 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

Definition at line 844 of file ins_ext_pose.c.

References ekf_F(), ekf_f_rk4(), ekf_H, ekf_L(), EKF_NUM_INPUTS, EKF_NUM_OUTPUTS, EKF_NUM_STATES, ekf_P, ekf_Q, ekf_R, ekf_X, float_mat_diagonal_scal(), float_mat_invert(), float_mat_mul(), float_mat_mul_copy(), float_mat_scale(), float_mat_sum_scaled(), float_mat_transpose(), float_mat_transpose_square(), float_mat_vect_mul(), float_vect_add(), float_vect_scale(), K, MAKE_MATRIX_PTR, and mesonh.mesonh_atmosphere::Z.

Here is the call graph for this function:

◆ 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 1300 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 1292 of file ins_ext_pose.c.

Referenced by logger_file_write_header().

Here is the caller graph for this function:

◆ ins_ext_pose_init()

void ins_ext_pose_init ( void )

Module.

Definition at line 265 of file ins_ext_pose.c.

References accel_cb(), accel_ev, DefaultPeriodic, ekf_init(), gyro_cb(), gyro_ev, InsExtPose::has_new_acc, InsExtPose::has_new_ext_pose, InsExtPose::has_new_gyro, ins_ext_pos, INS_EXT_POSE_IMU_ID, ins_ext_pose_init_from_flightplan(), register_periodic_telemetry(), send_ahrs_bias(), send_external_pose_down(), send_ins(), send_ins_ref(), and send_ins_z().

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

void ins_ext_pose_msg_update ( uint8_t * buf )

Import External Pose Message.

Definition at line 197 of file ins_ext_pose.c.

References DEBUG_PRINT, InsExtPose::ev_att, InsExtPose::ev_pos, InsExtPose::ev_quat, InsExtPose::ev_time, InsExtPose::ev_vel, float_eulers_of_quat(), float_quat_comp(), get_sys_time_usec(), InsExtPose::has_new_ext_pose, ins_ext_pos, FloatEulers::phi, FloatEulers::psi, FloatQuat::qi, FloatQuat::qx, FloatQuat::qy, FloatQuat::qz, FloatEulers::theta, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Here is the call graph for this function:

◆ ins_ext_pose_run()

void ins_ext_pose_run ( void )

Definition at line 294 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 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

Definition at line 127 of file ins_ext_pose.c.

References dev, InsExtPose::ev_pos, InsExtPose::ev_quat, InsExtPose::ev_time, InsExtPose::ev_vel, ins_ext_pos, 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_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 330 of file ins_ext_pose.c.

Referenced by ekf_measurement_step(), and ekf_step().