![]() |
Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
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"
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. | |
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 |
Integrated Navigation System interface.
Definition in file ins_ext_pose.c.
struct InsExtPose |
Data for telemetry and LTP origin.
Definition at line 51 of file ins_ext_pose.c.
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 |
#define DEBUG_PRINT | ( | ... | ) | {} |
Definition at line 42 of file ins_ext_pose.c.
#define INS_EXT_POSE_IMU_ID ABI_BROADCAST |
Import Gyro and Acc from ABI.
Definition at line 164 of file ins_ext_pose.c.
|
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().
|
inlinestatic |
Definition at line 378 of file ins_ext_pose.c.
Referenced by ekf_f_rk4(), and ekf_prediction_step().
|
inlinestatic |
Definition at line 417 of file ins_ext_pose.c.
Referenced by ekf_prediction_step(), and ekf_step().
|
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 foo.
Referenced by ekf_step().
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(), foo, and MAKE_MATRIX_PTR.
Referenced by ins_ext_pose_init().
|
inlinestatic |
Definition at line 686 of file ins_ext_pose.c.
References foo.
Referenced by ekf_prediction_step(), and ekf_step().
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(), foo, K, and MAKE_MATRIX_PTR.
Referenced by ekf_run().
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(), foo, and MAKE_MATRIX_PTR.
Referenced by ekf_run().
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(), foo, 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, and FloatVect3::z.
Referenced by ins_ext_pose_run().
Definition at line 337 of file ins_ext_pose.c.
Referenced by ekf_init().
|
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(), foo, K, and MAKE_MATRIX_PTR.
|
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().
Definition at line 1300 of file ins_ext_pose.c.
References ekf_U, ekf_X, ekf_Z, and foo.
Referenced by logger_file_write_row().
Logging.
Definition at line 1292 of file ins_ext_pose.c.
References foo.
Referenced by logger_file_write_header().
Module.
Definition at line 265 of file ins_ext_pose.c.
References accel_cb(), accel_ev, DefaultPeriodic, ekf_init(), foo, 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().
Definition at line 76 of file ins_ext_pose.c.
References ecef_of_lla_i(), foo, LtpDef_i::hmsl, ins_ext_pos, LlaCoor_i::lat, InsExtPose::ltp_def, ltp_def_from_ecef_i(), stateSetLocalOrigin_i(), and waypoints_localize_all().
Referenced by ins_ext_pose_init().
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(), foo, 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.
Definition at line 294 of file ins_ext_pose.c.
References ekf_run().
|
static |
Definition at line 142 of file ins_ext_pose.c.
References dev, ekf_X, and foo.
Referenced by ins_ext_pose_init().
|
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, foo, ins_ext_pos, FloatQuat::qi, FloatQuat::qx, FloatQuat::qy, FloatQuat::qz, FloatVect3::x, FloatVect3::y, and FloatVect3::z.
Referenced by ins_ext_pose_init().
|
static |
Provide telemetry.
Definition at line 102 of file ins_ext_pose.c.
References dev, foo, 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().
|
static |
Definition at line 118 of file ins_ext_pose.c.
References LlaCoor_i::alt, dev, LtpDef_i::ecef, foo, 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().
|
static |
Definition at line 110 of file ins_ext_pose.c.
References dev, foo, ins_ext_pos, InsExtPose::ltp_accel, InsExtPose::ltp_pos, InsExtPose::ltp_speed, and NedCoor_i::z.
Referenced by ins_ext_pose_init().
|
static |
Definition at line 168 of file ins_ext_pose.c.
Referenced by ins_ext_pose_init().
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().
float ekf_P[EKF_NUM_STATES][EKF_NUM_STATES] |
Definition at line 326 of file ins_ext_pose.c.
Referenced by ekf_init(), ekf_measurement_step(), ekf_prediction_step(), and ekf_step().
float ekf_Q[EKF_NUM_INPUTS][EKF_NUM_INPUTS] |
Definition at line 327 of file ins_ext_pose.c.
Referenced by ekf_init(), ekf_prediction_step(), and ekf_step().
float ekf_R[EKF_NUM_OUTPUTS][EKF_NUM_OUTPUTS] |
Definition at line 328 of file ins_ext_pose.c.
Referenced by ekf_init(), ekf_measurement_step(), and ekf_step().
float ekf_U[EKF_NUM_INPUTS] |
Definition at line 324 of file ins_ext_pose.c.
Referenced by ekf_init(), ekf_run(), and ins_ext_pos_log_data().
float ekf_X[EKF_NUM_STATES] |
Definition at line 323 of file ins_ext_pose.c.
Referenced by ekf_init(), ekf_measurement_step(), ekf_prediction_step(), ekf_run(), ekf_step(), ins_ext_pos_log_data(), and send_ahrs_bias().
float ekf_Z[EKF_NUM_OUTPUTS] |
Definition at line 325 of file ins_ext_pose.c.
Referenced by ekf_init(), ekf_run(), and ins_ext_pos_log_data().
|
static |
Definition at line 169 of file ins_ext_pose.c.
Referenced by ins_ext_pose_init().
struct InsExtPose ins_ext_pos |
Definition at line 73 of file ins_ext_pose.c.
Referenced by accel_cb(), ekf_run(), gyro_cb(), ins_ext_pose_init(), ins_ext_pose_init_from_flightplan(), ins_ext_pose_msg_update(), send_external_pose_down(), send_ins(), send_ins_ref(), and send_ins_z().
float t0 |
Definition at line 333 of file ins_ext_pose.c.
Referenced by ekf_run().
float t1 |
Definition at line 334 of file ins_ext_pose.c.
Referenced by distributed_circular(), ekf_run(), mtostk_apply_polynomial_temp(), and nps_main_loop().