37#include "generated/flight_plan.h"
40#ifndef INS_EXT_POSE_P0
41#define INS_EXT_POSE_P0 {1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1.}
44#ifndef INS_EXT_POSE_Q_NOISE
45#define INS_EXT_POSE_Q_NOISE {1.0, 1.0, 1.0, 0.0173, 4.878e-4, 3.547e-4}
48#ifndef INS_EXT_POSE_R_NOISE
49#define INS_EXT_POSE_R_NOISE {8.372e-6, 3.832e-6, 4.761e-6, 2.830e-4, 8.684e-6, 7.013e-6}
52#if INS_EXT_POSE_DEBUG_PRINT
54#define DEBUG_PRINT(...) printf(__VA_ARGS__)
56#define DEBUG_PRINT(...) {}
59#ifdef INS_EXT_VISION_ROTATION
116#if PERIODIC_TELEMETRY
193#ifndef INS_EXT_POSE_IMU_ID
194#define INS_EXT_POSE_IMU_ID ABI_BROADCAST
253#ifdef INS_EXT_VISION_ROTATION
296#if PERIODIC_TELEMETRY
348 {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
349 {0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
350 {0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
351 {0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0},
352 {0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0},
353 {0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0}};
362 for (i = 0 ; i < n; i++) {
363 for (
j = 0 ;
j < n;
j++) {
379 float X0[
EKF_NUM_STATES] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
404 const float x3 = x1 * x2;
411 const float x10 = x0 *
x9;
414 const float x13 = x0 *
x5;
420 const float x19 = 1.0f / x2;
444 const float x3 = x1 * x2;
452 const float x11 = x2 *
x5;
453 const float x12 = x1 *
x6;
459 const float x18 = x0 *
x17;
461 const float x20 =
x17 * x2;
467 const float x26 = x1 *
x17;
473 const float x32 = x1 *
x29;
475 const float x34 = x1 *
x28;
476 const float x35 = 1.0f /
x17;
478 const float x37 = x1 *
x35;
523 const float x4 = x2 *
x3;
526 const float x7 = x1 *
x6;
527 const float x8 = x2 *
x6;
528 const float x9 = x1 *
x3;
530 const float x11 = 1.f / x0;
552#if INS_EXT_POSE_USE_RK4
609#if INS_EXT_POSE_USE_RK4
862 DEBUG_PRINT(
"ekf prediction step U = %f, %f, %f, %f, %f, %f dt = %f \n",
889 DEBUG_PRINT(
"ekf measurement step Z = %f, %f, %f, %f \n",
951 "ekf_X1,ekf_X2,ekf_X3,ekf_X4,ekf_X5,ekf_X6,ekf_X7,ekf_X8,ekf_X9,ekf_X10,ekf_X11,ekf_X12,ekf_X13,ekf_X14,ekf_X15,");
952 fprintf(file,
"ekf_U1,ekf_U2,ekf_U3,ekf_U4,ekf_U5,ekf_U6,");
953 fprintf(file,
"ekf_Z1,ekf_Z2,ekf_Z3,ekf_Z4,");
958 fprintf(file,
"%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,",
962 fprintf(file,
"%f,%f,%f,%f,%f,%f,",
Main include for ABI (AirBorneInterface).
Event structure to store callbacks in a linked list.
#define AHRS_COMP_ID_GENERIC
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
static void float_vect_add(float *a, const float *b, const int n)
a += b
static void float_vect_sum(float *o, const float *a, const float *b, const int n)
o = a + b
static void float_vect_smul(float *o, const float *a, const float s, const int n)
o = a * s
#define float_rmat_of_eulers
static void float_mat_vect_mul(float *o, float **a, float *b, int m, int n)
o = a * b
static void float_vect_scale(float *a, const float s, const int n)
a *= s
static void float_mat_sum_scaled(float **a, float **b, float k, int m, int n)
a += k*b, where k is a scalar value
static void float_mat_mul_copy(float **o, float **a, float **b, int m, int n, int l)
o = a * b
static void float_mat_copy(float **a, float **b, int m, int n)
a = b
static void float_vect_copy(float *a, const float *b, const int n)
a = b
static void float_mat_mul(float **o, float **a, float **b, int m, int n, int l)
o = a * b
static void float_mat_transpose(float **o, float **a, int n, int m)
transpose non-square matrix
#define MAKE_MATRIX_PTR(_ptr, _mat, _rows)
Make a pointer to a matrix of _rows lines.
void float_mat_invert(float **o, float **mat, int n)
Calculate inverse of any n x n matrix (passed as C array) o = mat^-1 Algorithm verified with Matlab.
void float_rmat_transp_vmult(struct FloatVect3 *vb, struct FloatRMat *m_b2a, struct FloatVect3 *va)
rotate 3D vector by transposed rotation matrix.
void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q)
euler rotation 'ZYX'
static void float_mat_transpose_square(float **a, int n)
transpose square matrix
void float_quat_comp(struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c)
Composition (multiplication) of two quaternions.
static void float_mat_diagonal_scal(float **o, float v, int n)
Make an n x n identity matrix (for matrix passed as array)
static void float_mat_scale(float **a, float k, int m, int n)
a *= k, where k is a scalar value
#define POSITIONS_BFP_OF_REAL(_ef, _ei)
#define EULERS_COPY(_a, _b)
#define QUAT_COPY(_qo, _qi)
#define SPEEDS_BFP_OF_REAL(_ef, _ei)
#define VECT3_COPY(_a, _b)
#define ACCELS_BFP_OF_REAL(_ef, _ei)
#define RATES_FLOAT_OF_BFP(_rf, _ri)
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
#define ENU_OF_TO_NED(_po, _pi)
int32_t lat
in degrees*1e7
int32_t hmsl
Height above mean sea level in mm.
int32_t alt
in millimeters above WGS84 reference ellipsoid
struct LlaCoor_i lla
Reference point in lla.
struct EcefCoor_i ecef
Reference point in ecef.
int32_t lon
in degrees*1e7
void ecef_of_lla_i(struct EcefCoor_i *out, struct LlaCoor_i *in)
Convert a LLA to ECEF.
void ltp_def_from_ecef_i(struct LtpDef_i *def, struct EcefCoor_i *ecef)
vector in EarthCenteredEarthFixed coordinates
vector in Latitude, Longitude and Altitude
definition of the local (flat earth) coordinate system
vector in North East Down coordinates
static void stateSetAccelNed_f(uint16_t id, struct NedCoor_f *ned_accel)
Set acceleration in NED coordinates (float).
static void stateSetAccelBody_i(uint16_t id, struct Int32Vect3 *body_accel)
Set acceleration in Body coordinates (int).
static void stateSetNedToBodyEulers_f(uint16_t id, struct FloatEulers *ned_to_body_eulers)
Set vehicle body attitude from euler angles (float).
static void stateSetPositionNed_f(uint16_t id, struct NedCoor_f *ned_pos)
Set position from local NED coordinates (float).
static void stateSetLocalOrigin_i(uint16_t id, struct LtpDef_i *ltp_def)
Set the local (flat earth) coordinate frame origin (int).
static void stateSetBodyRates_f(uint16_t id, struct FloatRates *body_rate)
Set vehicle body angular rate (float).
static void stateSetSpeedNed_f(uint16_t id, struct NedCoor_f *ned_speed)
Set ground speed in local NED coordinates (float).
Inertial Measurement Unit interface.
Integrated Navigation System interface.
static void ekf_L(const float X[EKF_NUM_STATES], const float U[EKF_NUM_INPUTS], float out[EKF_NUM_STATES][EKF_NUM_INPUTS])
struct FloatRates gyros_f
struct FloatEulers ev_att
static void send_ins(struct transport_tx *trans, struct link_device *dev)
Provide telemetry.
void ins_ext_pose_msg_update(uint8_t *buf)
Import External Pose Message.
static void ekf_F(const float X[EKF_NUM_STATES], const float U[EKF_NUM_INPUTS], float out[EKF_NUM_STATES][EKF_NUM_STATES])
struct InsExtPose ins_ext_pose
static float ekf_P[EKF_NUM_STATES][EKF_NUM_STATES]
static float ekf_R[EKF_NUM_OUTPUTS][EKF_NUM_OUTPUTS]
#define INS_EXT_POSE_R_NOISE
static abi_event accel_ev
static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
void ins_ext_pose_log_data(FILE *file)
static void ekf_init(void)
EKF protos.
struct FloatVect3 accels_f
static void ins_ext_pose_init_from_flightplan(void)
static float ekf_U[EKF_NUM_INPUTS]
void ins_ext_pose_init(void)
Module.
struct NedCoor_i ltp_speed
static void ekf_measurement_step(const float Z[EKF_NUM_OUTPUTS])
static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
struct NedCoor_i ltp_accel
static void send_ins_z(struct transport_tx *trans, struct link_device *dev)
static float ekf_Q[EKF_NUM_INPUTS][EKF_NUM_INPUTS]
#define INS_EXT_POSE_Q_NOISE
void ins_ext_pose_log_header(FILE *file)
Logging.
static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
void ins_ext_pose_run(void)
#define INS_EXT_POSE_IMU_ID
Import Gyro and Acc from ABI.
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
static void ekf_set_diag(float **a, float *b, int n)
static float ekf_Z[EKF_NUM_OUTPUTS]
static void ekf_prediction_step(const float U[EKF_NUM_INPUTS], const float dt)
static void send_external_pose_down(struct transport_tx *trans, struct link_device *dev)
static void ekf_f(const float X[EKF_NUM_STATES], const float U[EKF_NUM_INPUTS], float out[EKF_NUM_STATES])
static void ekf_run(void)
float ekf_X[EKF_NUM_STATES]
static float ekf_H[EKF_NUM_OUTPUTS][EKF_NUM_STATES]
static void send_ahrs_bias(struct transport_tx *trans, struct link_device *dev)
Data for telemetry and LTP origin.
Integrated Navigation System interface.
void waypoints_localize_all(void)
update local ENU coordinates of global waypoints
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
Paparazzi generic algebra macros.
Paparazzi floating point algebra.
vector in East North Up coordinates Units: meters
vector in North East Down coordinates Units: meters
API to get/set the generic vehicle states.
static const struct usb_device_descriptor dev
static float get_sys_time_float(void)
Get the time in seconds since startup.
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.