35#include "generated/flight_plan.h"
40#define DEBUG_PRINT(...) printf(__VA_ARGS__)
42#define DEBUG_PRINT(...) {}
45#ifdef INS_EXT_VISION_ROTATION
163#ifndef INS_EXT_POSE_IMU_ID
164#define INS_EXT_POSE_IMU_ID ABI_BROADCAST
223#ifdef INS_EXT_VISION_ROTATION
259static inline void ekf_run(
void);
277#if PERIODIC_TELEMETRY
330float 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}};
340 for (i = 0 ; i < n; i++) {
341 for (
j = 0 ;
j < n;
j++) {
357 float X0[
EKF_NUM_STATES] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
361 float Pdiag[
EKF_NUM_STATES] = {1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1.};
380 float x0 =
cos(X[8]);
381 float x1 =
U[0] - X[9];
382 float x2 =
cos(X[7]);
384 float x4 =
U[2] - X[11];
385 float x5 =
sin(X[6]);
386 float x6 =
sin(X[8]);
388 float x8 =
sin(X[7]);
389 float x9 =
cos(X[6]);
391 float x11 =
U[1] - X[10];
395 float x15 =
U[4] - X[13];
397 float x17 =
U[5] - X[14];
399 float x19 = 1.0 / x2;
405 out[5] = -x1 *
x8 +
x11 * x2 *
x5 + x2 *
x4 *
x9 + 9.8100000000000005;
420 float x0 =
U[1] - X[10];
421 float x1 =
sin(X[6]);
422 float x2 =
sin(X[8]);
424 float x4 =
sin(X[7]);
425 float x5 =
cos(X[6]);
426 float x6 =
cos(X[8]);
430 float x10 =
U[2] - X[11];
435 float x15 =
U[0] - X[9];
448 float x28 =
U[4] - X[13];
451 float x31 =
U[5] - X[14];
515 out[3][11] = -
x3 -
x8;
541 out[5][7] = -x0 * x1 *
x4 -
x10 *
x27 +
x17 * (-
U[0] + X[9]);
570 out[7][6] = -
x34 +
x5 * (-
U[5] + X[14]);
689 float x0 =
cos(X[7]);
690 float x1 =
cos(X[8]);
691 float x2 =
sin(X[8]);
692 float x3 =
cos(X[6]);
694 float x5 =
sin(X[7]);
695 float x6 =
sin(X[6]);
700 float x11 = 1.0 / x0;
719 out[3][0] = -x0 * x1;
721 out[3][2] = -
x5 *
x9 -
x8;
725 out[4][0] = -x0 * x2;
726 out[4][1] = -
x5 *
x8 -
x9;
727 out[4][2] = -
x4 *
x5 +
x7;
732 out[5][1] = -x0 *
x6;
733 out[5][2] = -x0 *
x3;
741 out[6][4] = -
x10 *
x6;
742 out[6][5] = -
x10 *
x3;
753 out[8][4] = -
x11 *
x6;
754 out[8][5] = -
x11 *
x3;
1167 static bool start =
false;
1212 DEBUG_PRINT(
"ekf prediction step U = %f, %f, %f, %f, %f, %f dt = %f \n",
ekf_U[0],
ekf_U[1],
ekf_U[2],
ekf_U[3],
1295 "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,");
1296 fprintf(file,
"ekf_U1,ekf_U2,ekf_U3,ekf_U4,ekf_U5,ekf_U6,");
1297 fprintf(file,
"ekf_Z1,ekf_Z2,ekf_Z3,ekf_Z4,");
1302 fprintf(file,
"%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,",
ekf_X[0],
ekf_X[1],
ekf_X[2],
ekf_X[3],
ekf_X[4],
1304 fprintf(file,
"%f,%f,%f,%f,%f,%f,",
ekf_U[0],
ekf_U[1],
ekf_U[2],
ekf_U[3],
ekf_U[4],
ekf_U[5]);
Main include for ABI (AirBorneInterface).
Event structure to store callbacks in a linked list.
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
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 ACCELS_BFP_OF_REAL(_ef, _ei)
#define RATES_FLOAT_OF_BFP(_rf, _ri)
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
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 struct FloatRMat * stateGetNedToBodyRMat_f(void)
Get vehicle body attitude rotation matrix (float).
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 InsExtPose ins_ext_pos
struct FloatRates gyros_f
struct FloatEulers ev_att
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 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])
void ins_ext_pos_log_data(FILE *file)
float ekf_P[EKF_NUM_STATES][EKF_NUM_STATES]
float ekf_R[EKF_NUM_OUTPUTS][EKF_NUM_OUTPUTS]
static abi_event accel_ev
static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
static void ekf_init(void)
EKF protos.
struct FloatVect3 accels_f
static void ins_ext_pose_init_from_flightplan(void)
float ekf_U[EKF_NUM_INPUTS]
void ekf_set_diag(float **a, float *b, int n)
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)
float ekf_Q[EKF_NUM_INPUTS][EKF_NUM_INPUTS]
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_step(const float U[EKF_NUM_INPUTS], const float Z[EKF_NUM_OUTPUTS], const float dt)
void ins_ext_pos_log_header(FILE *file)
Logging.
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]
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 floating point algebra.
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.
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_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 int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.