Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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 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)

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

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 ( ...)
Value:
{}

Definition at line 107 of file ins_ext_pose.c.

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

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

Referenced by ins_ext_pose_check_age().

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

Referenced by ins_ext_pose_check_age().

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

Referenced by ins_ext_pose_check_age().

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

Referenced by ekf_run().

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

Referenced by ins_ext_pose_init().

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

Referenced by ekf_init(), and ekf_reset_on_pose().

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

Referenced by ekf_init().

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

Referenced by ekf_init().

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

Referenced by send_filter_status().

Function Documentation

◆ accel_cb()

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 ACCELS_FLOAT_OF_BFP, and ins_ext_pose.

◆ ekf_F()

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

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

◆ ekf_init()

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

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, and EKF_Z_THETA.

Referenced by ekf_prediction_step().

Here is the caller graph for this function:

◆ ekf_measurement_step()

void ekf_measurement_step ( const float Z[EKF_NUM_OUTPUTS])
static

◆ ekf_prediction_step()

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

◆ ekf_psi_meas()

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, FLOAT_ANGLE_NORMALIZE, and ins_ext_pose.

Referenced by ekf_reset_on_pose(), and ekf_run().

Here is the caller graph for this function:

◆ ekf_reset_on_pose()

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, ins_ext_pose, INS_EXT_POSE_P0, 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()

◆ ekf_set_diag()

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

Definition at line 488 of file ins_ext_pose.c.

References b.

Referenced by ekf_init(), and ekf_reset_on_pose().

Here is the caller graph for this function:

◆ gyro_cb()

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 gyro_cb(), ins_ext_pose, and RATES_FLOAT_OF_BFP.

Here is the call graph for this function:

◆ ins_ext_pose_check_age()

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
tscapture 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 delay, 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, and Min.

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

◆ ins_ext_pose_init_from_flightplan()

void ins_ext_pose_init_from_flightplan ( void )
static

Definition at line 158 of file ins_ext_pose.c.

References LlaCoor_i::alt, ecef_of_lla_i(), ins_ext_pose, LlaCoor_i::lat, LlaCoor_i::lon, 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, and ekf_Z.

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.

Referenced by logger_file_write_header().

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

References ekf_run().

Here is the call graph for this function:

◆ send_ahrs_bias()

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, and ekf_X.

◆ send_external_pose_down()

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, and ins_ext_pose.

◆ send_filter_status()

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, get_sys_time_float(), ins_ext_pose, and INS_EXT_POSE_TIMEOUT.

Here is the call graph for this function:

◆ send_ins()

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, and ins_ext_pose.

◆ send_ins_ref()

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

Definition at line 199 of file ins_ext_pose.c.

References dev, and ins_ext_pose.

◆ send_ins_z()

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, and ins_ext_pose.

Variable Documentation

◆ accel_ev

abi_event accel_ev
static

Definition at line 266 of file ins_ext_pose.c.

◆ 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

◆ 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

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

◆ ins_ext_pose

◆ 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