39 #include "generated/airframe.h"
40 #include "generated/flight_plan.h"
42 #if FIXEDWING_FIRMWARE
46 #ifndef INS_MEKF_WIND_FILTER_ID
47 #define INS_MEKF_WIND_FILTER_ID 3
63 #define PrintLog sdLogWriteLog
64 #define LogFileIsOpen() (pprzLogFile != -1)
67 #define PrintLog fprintf
68 #define LogFileIsOpen() (pprzLogFile != NULL)
74 #if PERIODIC_TELEMETRY
78 static void send_euler(
struct transport_tx *trans,
struct link_device *
dev)
84 pprz_msg_send_AHRS_EULER(trans,
dev, AC_ID,
85 <p_to_imu_euler.
phi,
86 <p_to_imu_euler.
theta,
87 <p_to_imu_euler.
psi,
91 static void send_wind(
struct transport_tx *trans,
struct link_device *
dev)
98 pprz_msg_send_WIND_INFO_RET(trans,
dev, AC_ID, &flags,
99 &wind_enu.
x, &wind_enu.
y, &wind_enu.
z, &va);
113 if (t_diff > 50000) {
117 pprz_msg_send_STATE_FILTER_STATUS(trans,
dev, AC_ID, &
id, &mde, &
val);
132 pprz_msg_send_INV_FILTER(trans,
dev,
159 #ifndef INS_MEKF_WIND_AIRSPEED_ID
160 #define INS_MEKF_WIND_AIRSPEED_ID ABI_BROADCAST
165 #ifndef INS_MEKF_WIND_INCIDENCE_ID
166 #define INS_MEKF_WIND_INCIDENCE_ID ABI_BROADCAST
171 #ifndef INS_MEKF_WIND_BARO_ID
173 #define INS_MEKF_WIND_BARO_ID BARO_BOARD_SENDER_ID
175 #define INS_MEKF_WIND_BARO_ID ABI_BROADCAST
181 #ifndef INS_MEKF_WIND_IMU_ID
182 #define INS_MEKF_WIND_IMU_ID ABI_BROADCAST
187 #ifndef INS_MEKF_WIND_MAG_ID
188 #define INS_MEKF_WIND_MAG_ID ABI_BROADCAST
195 #ifndef INS_MEKF_WIND_GPS_ID
196 #define INS_MEKF_WIND_GPS_ID GPS_MULTI_ID
215 static float alpha = 10.0f;
217 static float baro_moy = 0.0f;
218 static float baro_prev = 0.0f;
225 baro_prev = pressure;
227 baro_moy = (baro_moy * (i - 1) + pressure) / i;
228 alpha = (10.*
alpha + (baro_moy - baro_prev)) / (11.0f);
229 baro_prev = baro_moy;
231 if (fabs(
alpha) < 0.1f) {
246 if (LogFileIsOpen()) {
261 if (LogFileIsOpen()) {
274 if (LogFileIsOpen()) {
287 if (LogFileIsOpen()) {
306 #if USE_AUTO_INS_FREQ || !defined(INS_PROPAGATE_FREQUENCY)
318 PRINT_CONFIG_MSG(
"Using fixed INS_PROPAGATE_FREQUENCY for INS MEKF_WIND propagation.")
319 PRINT_CONFIG_VAR(INS_PROPAGATE_FREQUENCY)
320 const float dt = 1. / (INS_PROPAGATE_FREQUENCY);
332 if (LogFileIsOpen()) {
336 "%.3f gyro_accel %.3f %.3f %.3f %.3f %.3f %.3f \n",
338 gyro_body.
p, gyro_body.
q, gyro_body.
r,
356 "%.3f output %.4f %.4f %.4f %.3f %.3f %.3f ",
359 rates.
p, rates.
q, rates.
r);
361 "%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f ",
363 speed.
x, speed.
y, speed.
z,
364 accel.
x, accel.
y, accel.
z);
366 "%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n",
367 ab.
x, ab.
y, ab.
z, rb.
p, rb.
q, rb.
r, bb,
368 wind.
x, wind.
y, wind.
z, airspeed);
378 uint32_t stamp __attribute__((unused)),
385 uint32_t stamp __attribute__((unused)),
396 if (LogFileIsOpen()) {
398 "%.3f magneto %.3f %.3f %.3f\n",
400 mag_body.
x, mag_body.
y, mag_body.
z);
407 uint32_t stamp __attribute__((unused)),
438 uint32_t stamp __attribute__((unused)),
443 #if FIXEDWING_FIRMWARE
461 if (LogFileIsOpen()) {
463 "%.3f gps %.3f %.3f %.3f %.3f %.3f %.3f \n",
465 pos.
x, pos.
y, pos.
z, speed.
x, speed.
y, speed.
z);
473 struct NedCoor_i gps_pos_cm_ned, ned_pos;
483 if (LogFileIsOpen()) {
485 "%.3f gps %.3f %.3f %.3f %.3f %.3f %.3f \n",
487 pos.
x, pos.
y, pos.
z, speed.
x, speed.
y, speed.
z);
522 #if FIXEDWING_FIRMWARE
526 utm0.
alt = GROUND_ALT;
532 llh_nav0.
lat = NAV_LAT0;
533 llh_nav0.
lon = NAV_LON0;
534 llh_nav0.
alt = NAV_ALT0 + NAV_MSL0;
556 const struct FloatVect3 mag_h = { INS_H_X, INS_H_Y, INS_H_Z };
571 #if PERIODIC_TELEMETRY
579 #if LOG_MEKF_WIND && SITL
584 snprintf(filename, 512,
"%s/mekf_wind_%05d.csv", STRINGIFY(MEKF_WIND_LOG_PATH),
counter);
588 snprintf(filename, 512,
"%s/mekf_wind_%05d.csv", STRINGIFY(MEKF_WIND_LOG_PATH), ++
counter);
592 printf(
"Failed to open WE log file '%s'\n",filename);
594 printf(
"Opening WE log file '%s'\n",filename);
606 #if FIXEDWING_FIRMWARE
625 #if FIXEDWING_FIRMWARE
static void h(const real32_T x[7], const real32_T q[4], real32_T y[6])
Main include for ABI (AirBorneInterface).
#define ABI_BROADCAST
Broadcast address.
Event structure to store callbacks in a linked list.
void ahrs_aligner_init(void)
Interface to align the AHRS via low-passed measurements at startup.
Utility functions for floating point AHRS implementations.
static void ahrs_float_get_quat_from_accel_mag(struct FloatQuat *q, struct FloatVect3 *accel, struct FloatVect3 *mag)
float tas_from_dynamic_pressure(float q)
Calculate true airspeed from dynamic pressure.
float tas_from_eas(float eas)
Calculate true airspeed from equivalent airspeed.
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
struct UtmCoor_f utm_float_from_gps(struct GpsState *gps_s, uint8_t zone)
Convenience functions to get utm position from GPS state.
struct NedCoor_f ned_vel_float_from_gps(struct GpsState *gps_s)
Get GPS ned velocity (float) Converted on the fly if not available.
struct EcefCoor_f ecef_vel_float_from_gps(struct GpsState *gps_s)
Get GPS ecef velocity (float) Converted on the fly if not available.
struct GpsState gps
global GPS state
struct LlaCoor_i lla_int_from_gps(struct GpsState *gps_s)
Get GPS lla (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.
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)
#define GPS_FIX_3D
3D GPS fix
data structure for GPS information
static struct LtpDef_i ltp_def
void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q)
euler rotation 'ZYX'
#define MAGS_FLOAT_OF_BFP(_ef, _ei)
#define RATES_FLOAT_OF_BFP(_rf, _ri)
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
#define INT32_POS_OF_CM_DEN
#define INT32_POS_OF_CM_NUM
#define INT32_VECT3_SCALE_2(_a, _b, _num, _den)
#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.
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)
#define NED_FLOAT_OF_BFP(_o, _i)
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 ltp_def_from_lla_i(struct LtpDef_i *def, struct LlaCoor_i *lla)
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 float pprz_isa_height_of_pressure(float pressure, float ref_p)
Get relative altitude from pressure (using simplified equation).
#define PPRZ_ISA_SEA_LEVEL_PRESSURE
ISA sea level standard atmospheric pressure in Pascal.
static void stateSetAccelNed_f(struct NedCoor_f *ned_accel)
Set acceleration in NED coordinates (float).
static void stateSetNedToBodyQuat_f(struct FloatQuat *ned_to_body_quat)
Set vehicle body attitude from quaternion (float).
struct LtpDef_f ned_origin_f
Definition of the local (flat earth) coordinate system.
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
bool ned_initialized_f
True if local float coordinate frame is initialsed.
bool utm_initialized_f
True if utm origin (float) coordinate frame is initialsed.
static void stateSetPositionNed_f(struct NedCoor_f *ned_pos)
Set position from local NED coordinates (float).
struct UtmCoor_f utm_origin_f
Definition of the origin of Utm coordinate system.
static void stateSetPositionUtm_f(struct UtmCoor_f *utm_pos)
Set position from UTM coordinates (float).
static void stateSetLocalOrigin_i(struct LtpDef_i *ltp_def)
Set the local (flat earth) coordinate frame origin (int).
static void stateSetLocalUtmOrigin_f(struct UtmCoor_f *utm_def)
Set the local (flat earth) coordinate frame origin from UTM (float).
static void stateSetBodyRates_f(struct FloatRates *body_rate)
Set vehicle body angular rate (float).
static void stateSetSpeedNed_f(struct NedCoor_f *ned_speed)
Set ground speed in local NED coordinates (float).
Integrated Navigation System interface.
float ins_mekf_wind_get_baro_bias(void)
struct NedCoor_f ins_mekf_wind_get_wind_ned(void)
void ins_mekf_wind_update_incidence(float aoa, float aos)
void ins_mekf_wind_set_pos_ned(struct NedCoor_f *p)
float ins_mekf_wind_get_airspeed_norm(void)
void ins_mekf_wind_update_baro(float baro_alt)
struct NedCoor_f ins_mekf_wind_get_accel_ned(void)
void ins_mekf_wind_init(void)
Init function.
void ins_mekf_wind_set_mag_h(const struct FloatVect3 *mag_h)
struct NedCoor_f ins_mekf_wind_get_pos_ned(void)
Getter/Setter functions.
struct FloatRates ins_mekf_wind_get_body_rates(void)
void ins_mekf_wind_set_speed_ned(struct NedCoor_f *s)
void ins_mekf_wind_update_mag(struct FloatVect3 *mag, bool attitude_only)
struct FloatVect3 ins_mekf_wind_get_accel_bias(void)
struct FloatQuat ins_mekf_wind_get_quat(void)
void ins_mekf_wind_propagate(struct FloatRates *gyro, struct FloatVect3 *acc, float dt)
Full INS propagation.
void ins_mekf_wind_align(struct FloatRates *gyro_bias, struct FloatQuat *quat)
void ins_mekf_wind_propagate_ahrs(struct FloatRates *gyro, struct FloatVect3 *acc, float dt)
AHRS-only propagation + accel correction.
struct FloatRates ins_mekf_wind_get_rates_bias(void)
struct NedCoor_f ins_mekf_wind_get_speed_ned(void)
void ins_mekf_wind_update_airspeed(float airspeed)
void ins_mekf_wind_update_pos_speed(struct FloatVect3 *pos, struct FloatVect3 *speed)
Multiplicative Extended Kalman Filter in rotation matrix formulation.
static void airspeed_cb(uint8_t sender_id, float airspeed)
static void send_inv_filter(struct transport_tx *trans, struct link_device *dev)
void ins_reset_local_origin(void)
local implemetation of the ins_reset functions
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
static abi_event airspeed_ev
#define INS_MEKF_WIND_IMU_ID
IMU (gyro, accel)
static void send_wind(struct transport_tx *trans, struct link_device *dev)
static abi_event accel_ev
static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
Call ins_mekf_wind_propagate on new gyro measurements.
static void mag_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag)
#define INS_MEKF_WIND_INCIDENCE_ID
incidence angles
static abi_event pressure_diff_ev
#define INS_MEKF_WIND_FILTER_ID
#define INS_MEKF_WIND_AIRSPEED_ID
airspeed (Pitot tube)
static abi_event aligner_ev
static uint32_t last_imu_stamp
void ins_mekf_wind_wrapper_init(void)
Init function.
static abi_event geo_mag_ev
static void set_state_from_ins(void)
update state interface
static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
static void incidence_cb(uint8_t sender_id, uint8_t flag, float aoa, float sideslip)
static void baro_cb(uint8_t sender_id, uint32_t stamp, float pressure)
static struct FloatVect3 ins_mekf_wind_accel
last accel measurement
void ins_reset_altitude_ref(void)
INS altitude reference reset.
static abi_event incidence_ev
#define INS_MEKF_WIND_BARO_ID
baro
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
struct InsMekfWind ins_mekf_wind
static void geo_mag_cb(uint8_t sender_id, struct FloatVect3 *h)
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)
logging functions
#define INS_MEKF_WIND_GPS_ID
ABI binding for gps data.
static void pressure_diff_cb(uint8_t sender_id, float pdyn)
#define INS_MEKF_WIND_MAG_ID
magnetometer
Paparazzi specific wrapper to run MEKF-Wind INS filter.
#define ins_mekf_wind_wrapper_Reset(_v)
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
Fixedwing Navigation library.
void ned_of_ecef_vect_f(struct NedCoor_f *ned, struct LtpDef_f *def, struct EcefCoor_f *ecef)
float alt
in meters (above WGS84 reference ellipsoid or above MSL)
uint8_t zone
UTM zone number.
vector in EarthCenteredEarthFixed coordinates
vector in East North Up coordinates Units: meters
vector in North East Down coordinates Units: meters
position in UTM coordinates Units: meters
Paparazzi atmospheric pressure conversion utilities.
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.