38#include "generated/airframe.h"
42#include "generated/airframe.h"
43#include "generated/flight_plan.h"
49#define DEBUG_INS_FLOW 0
53#define DEBUG_PRINT printf
54#define DEBUG_MAT_PRINT MAT_PRINT
57#define DEBUG_PRINT(...)
58#define DEBUG_MAT_PRINT(...)
61#ifndef AHRS_ICQ_OUTPUT_ENABLED
62#define AHRS_ICQ_OUTPUT_ENABLED TRUE
67#ifndef INS_FLOW_GYRO_ID
68#define INS_FLOW_GYRO_ID ABI_BROADCAST
73#ifndef INS_FLOW_ACCEL_ID
74#define INS_FLOW_ACCEL_ID ABI_BROADCAST
79#ifndef INS_FLOW_IMU_ID
80#define INS_FLOW_IMU_ID ABI_BROADCAST
86#ifndef INS_FLOW_GPS_ID
87#define INS_FLOW_GPS_ID GPS_MULTI_ID
92#ifndef INS_OPTICAL_FLOW_ID
93#define INS_OPTICAL_FLOW_ID ABI_BROADCAST
99#define INS_RPM_ID ABI_BROADCAST
120 uint32_t stamp __attribute__((unused)),
168#define MOMENT_DELAY 20
194#define USE_STANDARD_PARAMS 0
196#if USE_STANDARD_PARAMS
202float parameters[20] = {0.0018244, 0.400, 0.085, 0.152163; 0.170734; 0.103436; 0.122109,
203 0.1, 0.1, 0.1, 1.0f * M_PI / 180.0f, 100.0f * M_PI / 180.0f, 0.1f, 3.0f,
204 1.0f, 10.0f * M_PI / 180.0f, 10.0f * M_PI / 180.0f, 1.0f, 1.0f, 0.5f
207float parameters[20] = {0.0018244, 0.400, 0.085, 0.108068 0.115448 0.201207 0.208834,
208 0.1, 0.1, 0.1, 1.0f * M_PI / 180.0f, 100.0f * M_PI / 180.0f, 0.1f, 3.0f,
209 1.0f, 10.0f * M_PI / 180.0f, 10.0f * M_PI / 180.0f, 1.0f, 1.0f, 0.5f
217float parameters[20] = {1.234994e-01, 3.603662e-01, 8.751691e-02, 1.636867e-01, 1.561769e-01, 1.856140e-01, 1.601066e-02, 1.187989e-01, 1.507075e-01, 2.471644e-01, 7.934140e-02, 1.770048e+00, 1.345862e-01, 2.881410e+00, 1.003584e+00, 1.280523e-01, 7.549402e-02, 9.640423e-01, 1.078312e+00, 3.468849e-01};
220#if CONSTANT_ALT_FILTER
222float parameters[20] = {1.396428e-01, 2.517970e-01, 3.575834e-02, 2.626194e-01, 1.078661e-01, 3.126137e-01, 4.621823e-02, 3.258048e-01, 8.456147e-02, 2.275105e-01, 2.820394e-02, 1.937395e+00, -4.259889e-02, 2.755648e+00, 1.000810e+00, -3.474577e-03, 3.146387e-01, 8.809383e-01, 9.878757e-01, 6.741976e-01};
224float parameters[20] = {3.363769e-01, 4.917425e-01, 1.903805e-01, 2.945672e-01, 1.258647e-01, 1.513736e-01, 5.894541e-01, 2.162745e-01, 5.527361e-01, 1.385623e-01, 8.307731e-01, 1.488212e+00, 2.439721e-01, 3.052758e+00, 8.246426e-01, 9.988101e-02, 1.247046e-01, 8.834364e-01, 7.971876e-01, 1.112319e+00};
227float parameters[20] = {4.370754e-02, 3.770587e-01, 1.187542e-01, 1.174995e-01, 1.419432e-01, 6.950201e-02, 2.251078e-01, 9.113943e-02, 2.230198e-01, 5.767389e-02, 1.855676e-02, 1.676359e+00, 5.822681e-02, 2.869468e+00, 1.140625e+00, 6.831383e-02, 1.600776e-01, 9.853843e-01, 1.000381e+00, 5.081224e-01};
232#if CONSTANT_ALT_FILTER
235float parameters[20] = {8.407886e-03, 4.056093e-01, 1.555919e-01, 1.291584e-01, 2.594766e-01, 1.927331e-01, 9.599609e-02, 1.688265e-01, 5.589618e-02, 1.605665e-01, 1.195912e-01, 1.809532e+00, 4.268251e-02, 3.003060e+00, 1.098473e+00, 1.944433e-01, 2.363352e-01, 1.110390e+00, 1.190994e+00, 6.211962e-01};
239#if PREDICT_GYROS == 0
241float parameters[22] = {0.041001, 1.015066, -0.058495, 0.498353, -0.156362, 0.383511, 0.924635, 0.681918, 0.318947, 0.298235, 0.224906, 1.371037, 0.008888, 3.045428, 0.893953, 0.529789, 0.295028, 1.297515, 0.767550, 0.334040, 0.192238, 0.301966};
244float parameters[26] = {1.065019, 0.270407, 0.164520, 0.008321, 0.083628, 0.261853, 0.210707, 0.204501, 0.164267, 0.097979, 0.053705, 1.640180, 0.151171, 3.086366, 1.025684, 0.011813, 0.177164, 0.995710, 1.050374, 0.617920, 0.028360, 0.447258, 0.077277, 0.360559, 0.555940, 0.133979};
249float parameters[22] = {0.219033, 0.376572, 0.184002, 0.096388, 0.240843, 0.172390, 0.133111, 0.495885, 0.357086, 0.233624, 0.125611, 1.661682, 0.136735, 2.812652, 0.715887, 0.166932, 0.371409, 1.043920, 0.840683, 0.567703, 0.192238, 0.301966};
277#define PAR_PRED_ROLL_1 22
278#define PAR_PRED_ROLL_2 23
279#define PAR_PRED_ROLL_3 24
293#if PERIODIC_TELEMETRY
299static void send_quat(
struct transport_tx *trans,
struct link_device *
dev)
302 pprz_msg_send_AHRS_QUAT_INT(trans,
dev, AC_ID,
320 pprz_msg_send_AHRS_EULER_INT(trans,
dev, AC_ID,
321 <p_to_imu_euler.
phi,
322 <p_to_imu_euler.
theta,
323 <p_to_imu_euler.
psi,
331static void send_bias(
struct transport_tx *trans,
struct link_device *
dev)
333 pprz_msg_send_AHRS_GYRO_BIAS_INT(trans,
dev, AC_ID,
344 pprz_msg_send_GEO_MAG(trans,
dev, AC_ID,
352 if (!
ahrs_icq.is_aligned) { mde = 2; }
355 if (t_diff > 50000) { mde = 5; }
360static void send_ins(
struct transport_tx *trans,
struct link_device *
dev)
362 pprz_msg_send_INS(trans,
dev, AC_ID,
378 static float fake_qfe = 0.0;
380 pprz_msg_send_INS_REF(trans,
dev, AC_ID,
383 &
ins_flow.ltp_def.hmsl, (
float *)&fake_qfe);
404 theta = (180.0 / M_PI) * eulers->
theta;
407 float phi_dot = 0.0f;
415 struct FloatVect3 NED_velocities, body_velocities;
416 NED_velocities.
x = velocities->
x;
417 NED_velocities.
y = velocities->
y;
418 NED_velocities.
z = velocities->
z;
421 float vy_GT = body_velocities.
y;
425 phi_GT = (180.0 / M_PI) * eulers->
phi;
427 phi_GT = (180.0 / M_PI) *
GT_phi;
429 float vx_GT = body_velocities.
x;
432 theta_GT = (180.0 / M_PI) * eulers->
theta;
434 theta_GT = (180.0 / M_PI) *
GT_theta;
436 float p_GT = rates->
p;
437 float q_GT = rates->
q;
438 float z_GT = -position->
z;
439 float vz_GT = -velocities->
z;
477 thrust -= thrust_bias;
480 float actual_lp_thrust = mass *
g;
481 thrust_bias = thrust - actual_lp_thrust;
483 pprz_msg_send_INS_FLOW_INFO(trans,
dev, AC_ID,
484 &vy, &phi, &
p, &vx, &theta, &q, &
z, &z_dot,
485 &vy_GT, &phi_GT, &p_GT, &vx_GT, &theta_GT, &q_GT,
538 llh_nav0.
lat = NAV_LAT0;
539 llh_nav0.
lon = NAV_LON0;
541 llh_nav0.
alt = NAV_ALT0 + NAV_MSL0;
551 ins_flow.new_flow_measurement =
false;
627#if PERIODIC_TELEMETRY
675 float subpixel_factor = 10.0f;
676 float focal_x = 347.22;
677 float new_time = ((float)stamp) / 1e6;
678 float fps = 1.0f / (new_time -
ins_flow.vision_time);
679 ins_flow.optical_flow_x = (((float)flow_x) * fps) / (subpixel_factor * focal_x);
680 ins_flow.optical_flow_y = (((float)flow_y) * fps) / (subpixel_factor * focal_x);
681 ins_flow.divergence = 1.27 * size_divergence * fps;
684 ins_flow.new_flow_measurement =
true;
692 printf(
"v = %f, angle = %f, angle_dot = %f, z = %f.\n",
695 printf(
"v = %f, angle = %f, angle_dot = %f, z = %f, vx = %f, theta = %f.\n",
701 printf(
"v = %f, angle = %f, angle_dot = %f, z = %f, z_dot = %f.\n",
704 printf(
"v = %f, angle = %f, angle_dot = %f, z = %f, z_dot = %f, thrust bias = %f.\n",
722 printf(
"True: v = %f, angle = %f, angle_dot = %f, z = %f, z_dot = %f.\n",
723 velocities->
y, eulers->
phi, rates->
p, -position->
z, -velocities->
z);
784 float actual_lp_thrust = mass *
g;
825 DEBUG_PRINT(
"Thrust acceleration = %f, g = %f\n", thrust / mass,
g);
899 (M_PI / 180.0f) / 74.0f;
902 DEBUG_PRINT(
"Rate p = %f, gyro p = %f\n", rates->p,
903 (
ins_flow.lp_gyro_roll -
ins_flow.lp_gyro_bias_roll) * (M_PI / 180.0f) / 74.0f);
1095 if (
ins_flow.new_flow_measurement) {
1221 DEBUG_PRINT(
"Expected flow filter: %f, Expected flow ground truth = %f, Real flow x: %f, Real flow y: %f.\n",
1227 DEBUG_PRINT(
"Expected flow in body X direction filter: %f, Real flow in corresponding y direction: %f, gyro = %f, expected velocity = %f, real velocity = %f, expected theta = %f, real theta = %f.\n",
1232 float gyro_meas_roll;
1234 gyro_meas_roll = (
ins_flow.lp_gyro_roll -
ins_flow.lp_gyro_bias_roll) * (M_PI / 180.0f) / 74.0f;
1269 OF_X[i] += KI[i][0];
1325 float trace_P = 0.0f;
1332 ins_flow.new_flow_measurement =
false;
1344#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
1349 if (last_stamp > 0 &&
ahrs_icq.is_aligned) {
1350 float dt = (float)(stamp - last_stamp) * 1e-6;
1356 float current_rate = ((float)gyro->
p);
1358 current_rate = ((float)gyro->
q);
1363 PRINT_CONFIG_MSG(
"Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS_ICQ propagation.")
1374 uint32_t __attribute__((unused)) stamp,
1377#if USE_AUTO_AHRS_FREQ || !defined(AHRS_CORRECT_FREQUENCY)
1380 if (last_stamp > 0 &&
ahrs_icq.is_aligned) {
1381 float dt = (float)(stamp - last_stamp) * 1e-6;
1387 PRINT_CONFIG_MSG(
"Using fixed AHRS_CORRECT_FREQUENCY for AHRS int_cmpl_quat accel update.")
1390 const float dt = 1. / (AHRS_CORRECT_FREQUENCY);
1414 orient.
quat_i = ltp_to_body_quat;
1447 for (
int i = 0; i < num_act; i++) {
1448 if (feedback[i].set.
rpm) {
1481 ins_flow.ltp_pos.z = z_Ned_i_filter;
1502 struct FloatVect3 NED_velocities, body_velocities;
1503 body_velocities.
x = 0.0f;
1505 body_velocities.
z = 0.0f;
1531 uint32_t stamp __attribute__((unused)),
Main include for ABI (AirBorneInterface).
struct abi_struct abi_event
#define ABI_BROADCAST
Broadcast address.
#define AHRS_COMP_ID_FLOW
void ahrs_aligner_init(void)
static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
static void send_geo_mag(struct transport_tx *trans, struct link_device *dev)
static abi_event aligner_ev
static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
static void aligner_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, struct Int32Vect3 *lp_mag)
static void send_euler(struct transport_tx *trans, struct link_device *dev)
static void send_bias(struct transport_tx *trans, struct link_device *dev)
static void set_body_state_from_quat(void)
Compute body orientation and rates from imu orientation and rates.
bool ahrs_icq_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, struct Int32Vect3 *lp_mag)
void ahrs_icq_update_accel(struct Int32Vect3 *accel, float dt)
struct AhrsIntCmplQuat ahrs_icq
Default Rate filter Low pass.
void ahrs_icq_update_gps(struct GpsState *gps_s)
void ahrs_icq_propagate(struct Int32Rates *gyro, float dt)
static void send_quat(struct transport_tx *trans, struct link_device *dev)
static uint32_t ahrs_icq_last_stamp
bool autopilot_in_flight(void)
get in_flight flag
Core autopilot interface common to all firmwares.
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
static struct uart_periph * dev
static abi_event accel_ev
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
struct GpsState gps
global GPS state
struct EcefCoor_i ecef_vel_int_from_gps(struct GpsState *gps_s)
Get GPS ecef velocity (integer) Converted on the fly if not available.
struct EcefCoor_i ecef_int_from_gps(struct GpsState *gps_s)
Get GPS ecef pos (integer) Converted on the fly if not available.
#define GPS_FIX_3D
3D GPS fix
data structure for GPS information
static struct LtpDef_i ltp_def
void float_mat_invert(float **o, float **mat, const int n)
Calculate inverse of any n x n matrix (passed as C array) o = mat^-1 Algorithm verified with Matlab.
static void float_mat_transpose(float **o, float **a, const int n, const int m)
transpose non-square matrix
static void float_mat_mul(float **o, float **a, float **b, const int m, const int n, const int l)
o = a * b
static void float_mat_diagonal_scal(float **o, float v, const int n)
Make an n x n identity matrix (for matrix passed as array).
void float_rmat_vmult(struct FloatVect3 *vb, const struct FloatRMat *m_a2b, const struct FloatVect3 *va)
rotate 3D vector by rotation matrix.
static void float_mat_sum(float **o, float **a, float **b, const int m, const int n)
o = a + b
#define MAKE_MATRIX_PTR(_ptr, _mat, _rows)
Make a pointer to a matrix of _rows lines.
void float_rmat_inv(struct FloatRMat *m_b2a, const struct FloatRMat *m_a2b)
Inverse/transpose of a rotation matrix.
static void float_mat_diff(float **o, float **a, float **b, const int m, const int n)
o = a - b
int32_t p
in rad/s with INT32_RATE_FRAC
int32_t phi
in rad with INT32_ANGLE_FRAC
int32_t q
in rad/s with INT32_RATE_FRAC
int32_t psi
in rad with INT32_ANGLE_FRAC
int32_t theta
in rad with INT32_ANGLE_FRAC
void int32_eulers_of_quat(struct Int32Eulers *e, struct Int32Quat *q)
#define INT32_POS_OF_CM_DEN
#define INT32_SPEED_OF_CM_S_NUM
#define INT32_SPEED_OF_CM_S_DEN
#define INT32_POS_OF_CM_NUM
#define INT32_VECT3_SCALE_2(_a, _b, _num, _den)
#define MAG_FLOAT_OF_BFP(_ai)
int32_t lat
in degrees*1e7
int32_t alt
in millimeters above WGS84 reference ellipsoid
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)
void ned_of_ecef_point_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct EcefCoor_i *ecef)
Convert a point from ECEF to local NED.
void ned_of_ecef_vect_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct EcefCoor_i *ecef)
Rotate a vector from ECEF to NED.
vector in EarthCenteredEarthFixed coordinates
vector in Latitude, Longitude and Altitude
definition of the local (flat earth) coordinate system
vector in North East Down coordinates
struct FloatEulers eulers_f
Orienation in zyx euler angles.
struct Int32Quat quat_i
Orientation quaternion.
uint8_t status
Holds the status bits for all orientation representations.
#define ORREP_QUAT_I
Quaternion (BFP int).
#define ORREP_EULER_F
zyx Euler (float)
static struct Int32Quat * orientationGetQuat_i(struct OrientationReps *orientation)
Get vehicle body attitude quaternion (int).
static struct FloatEulers * orientationGetEulers_f(struct OrientationReps *orientation)
Get vehicle body attitude euler angles (float).
static struct Int32Quat * stateGetNedToBodyQuat_i(void)
Get vehicle body attitude quaternion (int).
static struct FloatRMat * stateGetNedToBodyRMat_f(void)
Get vehicle body attitude rotation matrix (float).
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
static void stateSetNedToBodyQuat_i(uint16_t id, struct Int32Quat *ned_to_body_quat)
Set vehicle body attitude from quaternion (int).
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
static void stateSetLocalOrigin_i(uint16_t id, struct LtpDef_i *ltp_def)
Set the local (flat earth) coordinate frame origin (int).
static struct NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
static void stateSetPositionNed_i(uint16_t id, struct NedCoor_i *ned_pos)
Set position from local NED coordinates (int).
static void stateSetBodyRates_i(uint16_t id, struct Int32Rates *body_rate)
Set vehicle body angular rate (int).
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
static struct NedCoor_f * stateGetSpeedNed_f(void)
Get ground speed in local NED coordinates (float).
static void stateSetSpeedNed_i(uint16_t id, struct NedCoor_i *ned_speed)
Set ground speed in local NED coordinates (int).
#define AHRS_PROPAGATE_FREQUENCY
#define INS_RESET_REF
flags for INS reset
static void reset_cb(uint8_t sender_id, uint8_t flag)
static abi_event reset_ev
static void send_ins(struct transport_tx *trans, struct link_device *dev)
static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
float moments[MOMENT_DELAY]
static void print_ins_flow_state(void)
static void send_ins(struct transport_tx *trans, struct link_device *dev)
struct NedCoor_i ltp_speed
static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
float OF_P[N_STATES_OF_KF][N_STATES_OF_KF]
static uint8_t ahrs_flow_id
Component ID for FLOW.
#define INS_OPTICAL_FLOW_ID
static void print_true_state(void)
float OF_X[N_STATES_OF_KF]
static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
struct NedCoor_i ltp_accel
static void reset_cb(uint8_t sender_id UNUSED, uint8_t flag)
float RPM_FACTORS[OF_N_ROTORS]
static void send_geo_mag(struct transport_tx *trans, struct link_device *dev)
static void ins_act_feedback_cb(uint8_t sender_id, struct act_feedback_t *feedback, uint8_t num_act)
static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
static void send_quat(struct transport_tx *trans, struct link_device *dev)
static abi_event ins_act_feedback_ev
float OF_Q[N_STATES_OF_KF][N_STATES_OF_KF]
static void ins_reset_filter(void)
void ins_optical_flow_cb(uint8_t sender_id, uint32_t stamp, int32_t flow_x, int32_t flow_y, int32_t flow_der_x, int32_t flow_der_y, float quality, float size_divergence)
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
void ins_flow_update(void)
static void aligner_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, struct Int32Vect3 *lp_mag)
static void send_euler(struct transport_tx *trans, struct link_device *dev)
static void set_body_state_from_quat(void)
Rotate angles and rates from imu to body frame and set state.
static void send_bias(struct transport_tx *trans, struct link_device *dev)
float OF_R[N_MEAS_OF_KF][N_MEAS_OF_KF]
static abi_event ins_optical_flow_ev
#define DEBUG_MAT_PRINT(...)
#define AHRS_ICQ_OUTPUT_ENABLED
#define INS_FLOW_ACCEL_ID
bool new_flow_measurement
static void send_ins_flow(struct transport_tx *trans, struct link_device *dev)
#define CONSTANT_ALT_FILTER
#define OF_LAT_FLOW_X_IND
#define OF_THRUST_BIAS_IND
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
static struct FloatVect3 H
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
Paparazzi floating point algebra.
vector in North East Down coordinates Units: meters
Simple matrix helper macros.
struct Stabilization stabilization
General stabilization interface for rotorcrafts.
API to get/set the generic vehicle states.
Architecture independent timing functions.
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.
int int32_t
Typedef defining 32 bit int type.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.