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
126 uint32_t stamp __attribute__((unused)),
142 bool ltp_initialized;
173 #define MOMENT_DELAY 20
182 #define OF_N_ROTORS 4
199 #define USE_STANDARD_PARAMS 0
201 #if USE_STANDARD_PARAMS
207 float parameters[20] = {0.0018244, 0.400, 0.085, 0.152163; 0.170734; 0.103436; 0.122109,
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
212 float parameters[20] = {0.0018244, 0.400, 0.085, 0.108068 0.115448 0.201207 0.208834,
213 0.1, 0.1, 0.1, 1.0f * M_PI / 180.0f, 100.0f * M_PI / 180.0f, 0.1f, 3.0f,
214 1.0f, 10.0f * M_PI / 180.0f, 10.0f * M_PI / 180.0f, 1.0f, 1.0f, 0.5f
220 #if N_MEAS_OF_KF == 3
222 float 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};
225 #if CONSTANT_ALT_FILTER
227 float 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};
229 float 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};
232 float 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};
237 #if CONSTANT_ALT_FILTER
240 float 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};
243 #if N_MEAS_OF_KF == 3
244 #if PREDICT_GYROS == 0
246 float 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};
249 float 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};
254 float 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};
282 #define PAR_PRED_ROLL_1 22
283 #define PAR_PRED_ROLL_2 23
284 #define PAR_PRED_ROLL_3 24
298 #if PERIODIC_TELEMETRY
304 static void send_quat(
struct transport_tx *trans,
struct link_device *
dev)
307 pprz_msg_send_AHRS_QUAT_INT(trans,
dev, AC_ID,
325 pprz_msg_send_AHRS_EULER_INT(trans,
dev, AC_ID,
326 <p_to_imu_euler.
phi,
327 <p_to_imu_euler.
theta,
328 <p_to_imu_euler.
psi,
336 static void send_bias(
struct transport_tx *trans,
struct link_device *
dev)
338 pprz_msg_send_AHRS_GYRO_BIAS_INT(trans,
dev, AC_ID,
349 pprz_msg_send_GEO_MAG(trans,
dev, AC_ID,
360 if (t_diff > 50000) { mde = 5; }
365 static void send_ins(
struct transport_tx *trans,
struct link_device *
dev)
367 pprz_msg_send_INS(trans,
dev, AC_ID,
383 static float fake_qfe = 0.0;
385 pprz_msg_send_INS_REF(trans,
dev, AC_ID,
409 theta = (180.0 / M_PI) * eulers->
theta;
412 float phi_dot = 0.0f;
420 struct FloatVect3 NED_velocities, body_velocities;
421 NED_velocities.
x = velocities->
x;
422 NED_velocities.
y = velocities->
y;
423 NED_velocities.
z = velocities->
z;
426 float vy_GT = body_velocities.
y;
430 phi_GT = (180.0 / M_PI) * eulers->
phi;
432 phi_GT = (180.0 / M_PI) *
GT_phi;
434 float vx_GT = body_velocities.
x;
437 theta_GT = (180.0 / M_PI) * eulers->
theta;
439 theta_GT = (180.0 / M_PI) *
GT_theta;
441 float p_GT = rates->
p;
442 float q_GT = rates->
q;
443 float z_GT = -position->
z;
444 float vz_GT = -velocities->
z;
482 thrust -= thrust_bias;
485 float actual_lp_thrust = mass *
g;
486 thrust_bias = thrust - actual_lp_thrust;
488 pprz_msg_send_INS_FLOW_INFO(trans,
dev, AC_ID,
489 &vy, &phi, &
p, &vx, &theta, &q, &
z, &z_dot,
490 &vy_GT, &phi_GT, &p_GT, &vx_GT, &theta_GT, &q_GT,
545 llh_nav0.
lat = NAV_LAT0;
546 llh_nav0.
lon = NAV_LON0;
548 llh_nav0.
alt = NAV_ALT0 + NAV_MSL0;
634 #if PERIODIC_TELEMETRY
681 float subpixel_factor = 10.0f;
682 float focal_x = 347.22;
683 float new_time = ((float)stamp) / 1e6;
698 printf(
"v = %f, angle = %f, angle_dot = %f, z = %f.\n",
701 printf(
"v = %f, angle = %f, angle_dot = %f, z = %f, vx = %f, theta = %f.\n",
707 printf(
"v = %f, angle = %f, angle_dot = %f, z = %f, z_dot = %f.\n",
710 printf(
"v = %f, angle = %f, angle_dot = %f, z = %f, z_dot = %f, thrust bias = %f.\n",
728 printf(
"True: v = %f, angle = %f, angle_dot = %f, z = %f, z_dot = %f.\n",
729 velocities->
y, eulers->
phi, rates->
p, -position->
z, -velocities->
z);
790 float actual_lp_thrust = mass *
g;
831 DEBUG_PRINT(
"Thrust acceleration = %f, g = %f\n", thrust / mass,
g);
905 (M_PI / 180.0f) / 74.0f;
908 DEBUG_PRINT(
"Rate p = %f, gyro p = %f\n", rates->p,
1227 DEBUG_PRINT(
"Expected flow filter: %f, Expected flow ground truth = %f, Real flow x: %f, Real flow y: %f.\n",
1233 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",
1238 float gyro_meas_roll;
1275 OF_X[i] += KI[i][0];
1331 float trace_P = 0.0f;
1350 #if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
1356 float dt = (float)(stamp - last_stamp) * 1e-6;
1362 float current_rate = ((float)gyro->
p);
1364 current_rate = ((float)gyro->
q);
1369 PRINT_CONFIG_MSG(
"Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS_ICQ propagation.")
1380 uint32_t __attribute__((unused)) stamp,
1383 #if USE_AUTO_AHRS_FREQ || !defined(AHRS_CORRECT_FREQUENCY)
1387 float dt = (float)(stamp - last_stamp) * 1e-6;
1393 PRINT_CONFIG_MSG(
"Using fixed AHRS_CORRECT_FREQUENCY for AHRS int_cmpl_quat accel update.")
1394 PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY)
1396 const float dt = 1. / (AHRS_CORRECT_FREQUENCY);
1451 orient.
quat_i = ltp_to_body_quat;
1484 for (
int i = 0; i < num_act; i++) {
1485 if (feedback[i].set.
rpm) {
1537 struct FloatVect3 NED_velocities, body_velocities;
1538 body_velocities.
x = 0.0f;
1540 body_velocities.
z = 0.0f;
1566 uint32_t stamp __attribute__((unused)),
Main include for ABI (AirBorneInterface).
Event structure to store callbacks in a linked list.
#define AHRS_COMP_ID_FLOW
void ahrs_aligner_init(void)
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)
struct Int32Rates gyro_bias
struct Int32Rates body_rate
struct Int32Quat ltp_to_body_quat
enum AhrsICQStatus status
status of the AHRS, AHRS_ICQ_UNINIT or AHRS_ICQ_RUNNING
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.
if(GpsFixValid() &&e_identification_started)
struct GpsState gps
global GPS state
int32_t hmsl
height above mean sea level (MSL) in mm
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
struct EcefCoor_i ecef_pos
position in ECEF in cm
struct EcefCoor_i ecef_vel
speed ECEF in cm/s
#define GPS_FIX_3D
3D GPS fix
data structure for GPS information
static struct LtpDef_i ltp_def
static void float_mat_diff(float **o, float **a, float **b, int m, int n)
o = a - b
static void float_mat_sum(float **o, float **a, float **b, int m, int n)
o = a + b
void float_rmat_inv(struct FloatRMat *m_b2a, struct FloatRMat *m_a2b)
Inverse/transpose of a rotation matrix.
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.
static void float_mat_diagonal_scal(float **o, float v, int n)
Make an n x n identity matrix (for matrix passed as array)
void float_rmat_vmult(struct FloatVect3 *vb, struct FloatRMat *m_a2b, struct FloatVect3 *va)
rotate 3D vector by rotation matrix.
int32_t p
in rad/s with INT32_RATE_FRAC
int32_t r
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 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)
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 void stateSetNedToBodyQuat_i(struct Int32Quat *ned_to_body_quat)
Set vehicle body attitude from quaternion (int).
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
static struct FloatRMat * stateGetNedToBodyRMat_f(void)
Get vehicle body attitude rotation matrix (float).
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
static void stateSetPositionNed_i(struct NedCoor_i *ned_pos)
Set position from local NED coordinates (int).
static struct NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
static void stateSetLocalOrigin_i(struct LtpDef_i *ltp_def)
Set the local (flat earth) coordinate frame origin (int).
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
static void stateSetBodyRates_i(struct Int32Rates *body_rate)
Set vehicle body angular rate (int).
static struct NedCoor_f * stateGetSpeedNed_f(void)
Get ground speed in local NED coordinates (float).
static void stateSetSpeedNed_i(struct NedCoor_i *ned_speed)
Set ground speed in local NED coordinates (int).
#define AHRS_PROPAGATE_FREQUENCY
void ins_reset_local_origin(void)
INS local origin reset.
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 abi_event accel_ev
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
float RPM_FACTORS[OF_N_ROTORS]
static void send_geo_mag(struct transport_tx *trans, struct link_device *dev)
static abi_event aligner_ev
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 uint32_t ahrs_icq_last_stamp
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
Paparazzi floating point algebra.
vector in North East Down coordinates Units: meters
Simple matrix helper macros.
struct Stabilization stabilization
General stabilization interface for rotorcrafts.
int32_t cmd[COMMANDS_NB]
output command vector, range from [-MAX_PPRZ:MAX_PPRZ] (store for messages)
API to get/set the generic vehicle states.
static const struct usb_device_descriptor dev
Architecture independent timing functions.
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 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.