36#include "stabilization/stabilization_attitude.h"
37#include "generated/airframe.h"
38#include "generated/flight_plan.h"
40#include "lib/gnss/gnss_checks.hpp"
41#include "lib/world_magnetic_model/geo_mag_declination.h"
47using namespace estimator;
50#if defined SITL && USE_NPS
56#if defined(INS_EKF2_FUSION_MODE) || defined(INS_EKF2_VDIST_SENSOR_TYPE) || defined(INS_EKF2_MAG_FUSION_TYPE)
57#error "INS_EKF2_FUSION_MODE, VDIST_SENSOR_TYPE and MAG_FUSION_TYPE are replaced by INS_EKF2_GPS_CTRL, EV_CTRL, OF_CTRL, DRAG_CTRL, HGT_REF and MAG_TYPE"
59#if defined(INS_EKF2_GPS_CHECK_MASK) || defined(INS_EKF2_GPS_REQ_EPH) || defined(INS_EKF2_GPS_REQ_EPV) || defined(INS_EKF2_GPS_REQ_SACC) || defined(INS_EKF2_GPS_REQ_NSATS) || defined(INS_EKF2_GPS_REQ_PDOP)
60#error "INS_EKF2_GPS_CHECK_MASK and GPS_REQ_* are replaced by INS_EKF2_GNSS_CHECK and GNSS_REQ_*"
62#if defined(INS_EKF2_FLOW_SENSOR_DELAY) || defined(INS_EKF2_MIN_FLOW_QUALITY) || defined(INS_EKF2_MAX_FLOW_RATE) || defined(INS_EKF2_FLOW_NOISE) || defined(INS_EKF2_FLOW_NOISE_QMIN) || defined(INS_EKF2_FLOW_INNOV_GATE) || defined(INS_EKF2_FLOW_POS_X) || defined(INS_EKF2_FLOW_POS_Y) || defined(INS_EKF2_FLOW_POS_Z)
63#error "INS_EKF2_FLOW_* are replaced by INS_EKF2_OF_DELAY, OF_QMIN, OF_MAX_RATE, OF_N_MIN, OF_N_MAX, OF_GATE and OF_POS_*"
65#if defined(INS_EKF2_SONAR_MIN_RANGE) || defined(INS_EKF2_SONAR_MAX_RANGE) || defined(INS_EKF2_RANGE_MAIN_AGL)
66#error "INS_EKF2_SONAR_* and RANGE_MAIN_AGL are replaced by INS_EKF2_RNG_MIN_DIST, RNG_MAX_DIST and RNG_CTRL"
68#if defined(INS_EKF2_RELHEADING_DISTANCE) || defined(INS_EKF2_RELHEADING_ERR) || defined(INS_EKF2_GPS_COURSE_YAW)
69#error "INS_EKF2_RELHEADING_DISTANCE/_ERR and GPS_COURSE_YAW are replaced by GPS_UBX_HEADING_BASELINE/_TOL (gps_ublox) and the EKF-GSF yaw estimator (yaw from the GNSS velocity)"
71#if defined(INS_EKF2_GPS_ANTENNA_DISTANCE) || defined(INS_EKF2_GPS_ANTENNA_DISTANCE_ERR)
72#error "INS_EKF2_GPS_ANTENNA_DISTANCE/_ERR are replaced by GPS_UBX_HEADING_BASELINE/_TOL in the gps_ublox module, which drops the heading itself"
76#ifndef USE_INS_NAV_INIT
77#define USE_INS_NAV_INIT TRUE
90#define GNSS_CHECK_NSATS (1<<0)
91#define GNSS_CHECK_PDOP (1<<1)
92#define GNSS_CHECK_EPH (1<<2)
93#define GNSS_CHECK_EPV (1<<3)
94#define GNSS_CHECK_SACC (1<<4)
95#define GNSS_CHECK_HDRIFT (1<<5)
96#define GNSS_CHECK_VDRIFT (1<<6)
97#define GNSS_CHECK_HSPEED (1<<7)
98#define GNSS_CHECK_VSPEED (1<<8)
99#define GNSS_CHECK_SPOOFED (1<<9)
100#define GNSS_CHECK_FIX (1<<10)
101#define GNSS_CHECK_JAMMED (1<<11)
104#ifndef INS_EKF2_GNSS_CHECK
105#define INS_EKF2_GNSS_CHECK (GNSS_CHECK_NSATS | GNSS_CHECK_EPH | GNSS_CHECK_SACC | GNSS_CHECK_FIX)
110#define EKF2_GPS_CTRL_HPOS 1
111#define EKF2_GPS_CTRL_VPOS 2
112#define EKF2_GPS_CTRL_VEL 4
113#define EKF2_GPS_CTRL_YAW 8
116#define EKF2_EV_CTRL_HPOS 1
117#define EKF2_EV_CTRL_VPOS 2
118#define EKF2_EV_CTRL_VEL 4
119#define EKF2_EV_CTRL_YAW 8
122#define EKF2_HGT_REF_BARO 0
123#define EKF2_HGT_REF_GNSS 1
124#define EKF2_HGT_REF_RANGE 2
125#define EKF2_HGT_REF_EV 3
128#define EKF2_MAG_TYPE_AUTO 0
129#define EKF2_MAG_TYPE_HEADING 1
130#define EKF2_MAG_TYPE_NONE 5
131#define EKF2_MAG_TYPE_INIT 6
136#if INS_EKF2_OPTITRACK
137#ifndef INS_EKF2_GPS_CTRL
138#define INS_EKF2_GPS_CTRL 0
140#ifndef INS_EKF2_EV_CTRL
141#define INS_EKF2_EV_CTRL (EKF2_EV_CTRL_HPOS | EKF2_EV_CTRL_VPOS | EKF2_EV_CTRL_VEL | EKF2_EV_CTRL_YAW)
143#ifndef INS_EKF2_HGT_REF
144#define INS_EKF2_HGT_REF EKF2_HGT_REF_EV
146#ifndef INS_EKF2_MAG_TYPE
147#define INS_EKF2_MAG_TYPE EKF2_MAG_TYPE_NONE
149#ifndef INS_EKF2_BARO_CTRL
150#define INS_EKF2_BARO_CTRL 0
152#ifndef INS_EKF2_EVP_NOISE
153#define INS_EKF2_EVP_NOISE 0.02f
155#ifndef INS_EKF2_EVA_NOISE
156#define INS_EKF2_EVA_NOISE 0.05f
158#ifndef INS_EKF2_EV_DELAY
159#define INS_EKF2_EV_DELAY 175.f
165#ifndef INS_EKF2_GNSS_REQ_EPH
166#define INS_EKF2_GNSS_REQ_EPH 5.0f
168#ifndef INS_EKF2_GNSS_REQ_SACC
169#define INS_EKF2_GNSS_REQ_SACC 1.0f
174#ifndef INS_EKF2_HGT_REF
175#define INS_EKF2_HGT_REF EKF2_HGT_REF_BARO
187#ifndef INS_EKF2_AT_REST_DETECTION
188#define INS_EKF2_AT_REST_DETECTION TRUE
190#ifndef INS_EKF2_AT_REST_GYRO
191#define INS_EKF2_AT_REST_GYRO 0.3f
193#ifndef INS_EKF2_AT_REST_ACCEL
194#define INS_EKF2_AT_REST_ACCEL 0.1f
196#ifndef INS_EKF2_AT_REST_RATE
197#define INS_EKF2_AT_REST_RATE 1.f
199#ifndef INS_EKF2_AT_REST_TIME
200#define INS_EKF2_AT_REST_TIME 1.f
204#if defined(FIXEDWING_FIRMWARE)
205#ifndef INS_EKF2_ARSP_THR
206#define INS_EKF2_ARSP_THR 8.f
208#ifndef INS_EKF2_FUSE_BETA
209#define INS_EKF2_FUSE_BETA 1
211#ifndef INS_EKF2_MAG_ACCLIM
212#define INS_EKF2_MAG_ACCLIM 0.f
214#ifndef INS_EKF2_RNG_QLTY_T
215#define INS_EKF2_RNG_QLTY_T 3.f
218#ifndef INS_EKF2_RNG_FOG
219#define INS_EKF2_RNG_FOG 1.f
224#if INS_EKF2_LOG_INPUTS
225#define INS_EKF2_PARAM_TABLE
227#include "generated/ekf2_params.h"
235#ifndef INS_EKF2_GPS_DELAY
236#define INS_EKF2_GPS_DELAY 110
241#ifndef INS_EKF2_RNG_MIN_DIST
242#define INS_EKF2_RNG_MIN_DIST 0.001f
247#ifndef INS_EKF2_RNG_MAX_DIST
248#define INS_EKF2_RNG_MAX_DIST 4.f
253#ifndef INS_EKF2_OF_MAX_RATE
254#define INS_EKF2_OF_MAX_RATE 200.f
261#ifndef INS_EKF2_AIRSPEED_ID
262#define INS_EKF2_AIRSPEED_ID AIR_DATA_SENDER_ID
267#ifndef INS_EKF2_BARO_ID
268#define INS_EKF2_BARO_ID AIR_DATA_SENDER_ID
273#ifndef INS_EKF2_TEMPERATURE_ID
274#define INS_EKF2_TEMPERATURE_ID AIR_DATA_SENDER_ID
279#ifndef INS_EKF2_AGL_ID
280#define INS_EKF2_AGL_ID ABI_BROADCAST
285#ifndef INS_EKF2_GYRO_ID
286#define INS_EKF2_GYRO_ID ABI_BROADCAST
291#ifndef INS_EKF2_ACCEL_ID
292#define INS_EKF2_ACCEL_ID ABI_BROADCAST
297#ifndef INS_EKF2_MAG_ID
298#define INS_EKF2_MAG_ID ABI_BROADCAST
303#ifndef INS_EKF2_GPS_ID
304#define INS_EKF2_GPS_ID GPS_MULTI_ID
309#ifndef INS_EKF2_RELPOS_ID
310#define INS_EKF2_RELPOS_ID ABI_BROADCAST
315#ifndef INS_EKF2_OF_ID
316#define INS_EKF2_OF_ID ABI_BROADCAST
321#ifndef INS_EKF2_GPS_POS_X
322#define INS_EKF2_GPS_POS_X 0
326#ifndef INS_EKF2_GPS_POS_Y
327#define INS_EKF2_GPS_POS_Y 0
331#ifndef INS_EKF2_GPS_POS_Z
332#define INS_EKF2_GPS_POS_Z 0
337#ifndef INS_EKF2_GPS_YAW_NOISE
338#define INS_EKF2_GPS_YAW_NOISE 0.1f
342#ifdef INS_EXT_VISION_ROTATION
393 if (now < last_now && (last_now - now) > (1UL << 31)) {
394 wraps += (1ULL << 32);
400 if (stamp > now && (stamp - now) > (1UL << 31) && time_us >= (1ULL << 32)) {
401 time_us -= (1ULL << 32);
410 return (time_us > delay_ms * 1000ULL) ? time_us - delay_ms * 1000ULL : 0;
426#if INS_EKF2_LOG_INPUTS
427#include "pprzlink/messages.h"
430#ifndef INS_EKF2_LOG_WAIT
431#define INS_EKF2_LOG_WAIT 60
435#ifndef INS_EKF2_LOG_OUTPUT_DIV
436#define INS_EKF2_LOG_OUTPUT_DIV 10
443#ifndef INS_EKF2_LOG_DEVICE
444#define INS_EKF2_LOG_DEVICE flightrecorder_sdlog
445#if !FLIGHTRECORDER_SDLOG
446#error "INS_EKF2_LOG_INPUTS needs the flight_recorder module (also in the nps target) or INS_EKF2_LOG_DEVICE"
449#define EKF2_LOG_TP (&pprzlog_tp.trans_tx)
450#define EKF2_LOG_DEV (&(INS_EKF2_LOG_DEVICE).device)
451static bool ekf2_log_ready(
void) {
return (INS_EKF2_LOG_DEVICE).file != NULL && *(INS_EKF2_LOG_DEVICE).file != -1; }
454static enum { EKF2_LOG_WAITING, EKF2_LOG_ON, EKF2_LOG_OFF } ekf2_log_state = EKF2_LOG_WAITING;
455static struct {
double lat, lon;
float alt;
bool set; } ekf2_log_origin;
459 if (ekf2_log_state != EKF2_LOG_ON) {
465 pprz_msg_send_EKF2_INPUT(EKF2_LOG_TP, EKF2_LOG_DEV, AC_ID, &t, &type,
466 nb_ints, nb_ints ? ints : &no_int, nb_values, nb_values ? values : &no_value);
470static void ekf2_log_params(
uint64_t time_us)
472 for (
int i = 0; i < ins_ekf2_params_nb; i += 32) {
475 for (
int j = i; j < ins_ekf2_params_nb && n < 32; j++) {
476 values[n++] = ins_ekf2_params[j].get(*
ekf_params);
486 for (
int i = 0; i < ins_ekf2_params_nb; i++) {
487 if (!strcmp(ins_ekf2_params[i].name, name)) {
489 float value = ins_ekf2_params[i].get(*
ekf_params);
498 if (ekf2_log_state == EKF2_LOG_WAITING) {
499 ekf2_log_origin.lat = lat;
500 ekf2_log_origin.lon = lon;
501 ekf2_log_origin.alt = alt;
502 ekf2_log_origin.set =
true;
512 if (ekf2_log_state != EKF2_LOG_WAITING) {
515 if (ekf2_log_ready()) {
516 ekf2_log_state = EKF2_LOG_ON;
518 ekf2_log_params(now);
519 float config[6] = {INS_EKF2_TAU_POS, INS_EKF2_TAU_VEL, INS_EKF2_GPS_YAW_RST,
522 if (ekf2_log_origin.set) {
526 ekf2_log_state = EKF2_LOG_OFF;
531static inline bool ekf2_input_enabled(
void) {
return ekf2_log_state != EKF2_LOG_WAITING; }
535#if INS_EKF2_LOG_OUTPUT_DIV
537 if (++count < INS_EKF2_LOG_OUTPUT_DIV) {
541 const Vector3f pos{
ekf.getPosition()};
542 const Vector3f vel{
ekf.getVelocity()};
543 const Quatf q{
ekf.getQuaternion()};
544 float values[10] = {pos(0), pos(1), pos(2), vel(0), vel(1), vel(2), q(0), q(1), q(2), q(3)};
552#define ekf2_log(...) {}
553#define ekf2_log_param_changed(...) {}
554#define ekf2_log_origin_set(...) {}
555#define ekf2_log_check_start() {}
556#define ekf2_log_output(...) {}
560#if PERIODIC_TELEMETRY
563static void send_ins(
struct transport_tx *trans,
struct link_device *
dev)
568 const Vector3f pos_f{
ekf.getPosition()};
569 const Vector3f speed_f{
ekf.getVelocity()};
583 pprz_msg_send_INS(trans,
dev, AC_ID,
584 &pos.
x, &pos.
y, &pos.
z,
585 &speed.
x, &speed.
y, &speed.
z,
586 &accel.
x, &accel.
y, &accel.
z);
595 const Vector3f pos_f{
ekf.getPosition()};
596 const Vector3f speed_f{
ekf.getVelocity()};
604 pprz_msg_send_INS_Z(trans,
dev, AC_ID,
605 &baro_z, &
pos_z, &speed_z, &accel_z);
610 float qfe = 101325.0;
611 if (
ekf2.ltp_stamp > 0)
612 pprz_msg_send_INS_REF(trans,
dev, AC_ID,
613 &
ekf2.ltp_def.ecef.x, &
ekf2.ltp_def.ecef.y, &
ekf2.ltp_def.ecef.z,
614 &
ekf2.ltp_def.lla.lat, &
ekf2.ltp_def.lla.lon, &
ekf2.ltp_def.lla.alt,
615 &
ekf2.ltp_def.hmsl, &qfe);
621 return PX4_ISFINITE(ratio) ? ratio : 0.f;
626 uint32_t control_mode =
ekf.control_status().value;
627 uint16_t filter_fault_status =
ekf.fault_status().value;
635 float pos =
test_ratio(
ekf.getHorizontalPositionInnovationTestRatio());
636 float hgt =
test_ratio(
ekf.getVerticalPositionInnovationTestRatio());
637 float tas =
test_ratio(
ekf.getAirspeedInnovationTestRatio());
638 float hagl =
test_ratio(
ekf.getHeightAboveGroundInnovationTestRatio());
639 float beta =
test_ratio(
ekf.getSyntheticSideslipInnovationTestRatio());
642 const float ratios[] = {mag, vel, pos, hgt, tas, hagl, flow, beta};
644 for (
uint8_t i = 0; i <
sizeof(ratios) /
sizeof(ratios[0]); i++) {
645 if (ratios[i] > 1.f) {
646 innov_test_status |= (1 << i);
650 float mag_decl = 0.0f;
651 ekf.get_mag_decl_deg(mag_decl);
653 uint8_t terrain_valid =
ekf.isTerrainEstimateValid() ? 1 : 0;
654 uint8_t dead_reckoning =
ekf.control_status_flags().inertial_dead_reckoning ? 1 : 0;
656 pprz_msg_send_INS_EKF2(trans,
dev, AC_ID,
657 &control_mode, &filter_fault_status, &gps_check_status, &soln_status,
658 &innov_test_status, &mag, &vel, &pos, &hgt, &tas, &hagl, &flow, &beta,
659 &mag_decl, &terrain_valid, &dead_reckoning);
665 float gps_drift[3] = {
670 uint8_t gps_blocked = !PX4_ISFINITE(gps_drift[0]);
676 float pos_acc_h, pos_acc_v, vel_acc_h, vel_acc_v;
677 ekf.get_ekf_gpos_accuracy(&pos_acc_h, &pos_acc_v);
678 ekf.get_ekf_vel_accuracy(&vel_acc_h, &vel_acc_v);
681 float hagl =
ekf.isTerrainEstimateValid() ?
ekf.getHagl() : NAN;
682 float baro_bias =
ekf.getBaroBiasEstimatorStatus().bias;
683 float gnss_hgt_bias =
ekf.getGpsHgtBiasEstimatorStatus().bias;
686 float yaw_gsf = NAN, yaw_gsf_var = NAN;
687 float gsf_yaw[N_MODELS_EKFGSF], gsf_innov_vn[N_MODELS_EKFGSF], gsf_innov_ve[N_MODELS_EKFGSF], gsf_weight[N_MODELS_EKFGSF];
688 if (!
ekf.getDataEKFGSF(&yaw_gsf, &yaw_gsf_var, gsf_yaw, gsf_innov_vn, gsf_innov_ve, gsf_weight)) {
692 float yaw_gsf_std = (PX4_ISFINITE(yaw_gsf_var) && yaw_gsf_var >= 0.f) ? sqrtf(yaw_gsf_var) : NAN;
695 float mag_inc, mag_inc_ref, mag_strength, mag_strength_ref;
696 ekf.get_mag_checks(mag_inc, mag_inc_ref, mag_strength, mag_strength_ref);
698 pprz_msg_send_INS_EKF2_EXT(trans,
dev, AC_ID,
699 &gps_drift[0], &gps_drift[1], &gps_drift[2], &gps_blocked,
701 &pos_acc_h, &pos_acc_v, &vel_acc_h, &vel_acc_v,
702 &hagl, &baro_bias, &gnss_hgt_bias,
703 &yaw_gsf, &yaw_gsf_std,
704 &mag_inc, &mag_inc_ref, &mag_strength, &mag_strength_ref);
710 const auto &flags =
ekf.control_status_flags();
711 uint32_t filter_fault_status =
ekf.fault_status().value;
712 uint16_t filter_fault_status_16 = filter_fault_status;
716 if (flags.tilt_align && flags.yaw_align && (flags.gnss_pos || flags.ev_pos)) {
718 }
else if (flags.tilt_align && flags.yaw_align) {
725 if (filter_fault_status) {
729 pprz_msg_send_STATE_FILTER_STATUS(trans,
dev, AC_ID, &ahrs_ekf2_id, &mde, &filter_fault_status_16);
734 Vector2f wind =
ekf.getWindVelocity();
735 const Vector3f vel =
ekf.getVelocity();
737 float tas = Vector3f(vel(0) - wind(0), vel(1) - wind(1), vel(2)).norm();
741 pprz_msg_send_WIND_INFO_RET(trans,
dev, AC_ID, &flags, &wind(1), &wind(0), &f_zero, &tas);
746 Vector3f accel_bias =
ekf.getAccelBias();
747 Vector3f gyro_bias =
ekf.getGyroBias();
748 Vector3f mag_bias =
ekf.getMagBias();
750 pprz_msg_send_AHRS_BIAS(trans,
dev, AC_ID, &accel_bias(0), &accel_bias(1), &accel_bias(2),
751 &gyro_bias(0), &gyro_bias(1), &gyro_bias(2), &mag_bias(0), &mag_bias(1), &mag_bias(2));
757 const Quatf att_q{
ekf.getQuaternion()};
765 pprz_msg_send_AHRS_QUAT_INT(trans,
dev, AC_ID,
767 <p_to_body_quat.
qi,
768 <p_to_body_quat.
qx,
769 <p_to_body_quat.
qy,
770 <p_to_body_quat.
qz,
784 float sample_temp_ev[11];
785 sample_temp_ev[0] = (float)
sample_ev.time_us;
796 pprz_msg_send_EXTERNAL_POSE_DOWN(trans,
dev, AC_ID,
807 &sample_temp_ev[10]);
814#ifndef INS_EKF2_EVENT_MSGS
815#define INS_EKF2_EVENT_MSGS TRUE
820#ifndef INS_EKF2_EVENTS_ALL
821#define INS_EKF2_EVENTS_ALL FALSE
826#ifndef INS_EKF2_EVENT_HOLDOFF
827#define INS_EKF2_EVENT_HOLDOFF 5.f
830#if INS_EKF2_EVENT_MSGS
832#if FLIGHTRECORDER_SDLOG
838struct ekf2_status_text {
846static const struct ekf2_status_text ekf2_control_texts[] = {
847 {0,
"tilt aligned", NULL,
true},
848 {1,
"yaw aligned",
"yaw alignment lost",
true},
849 {2,
"GNSS pos fusion on",
"GNSS pos fusion off",
false},
850 {44,
"GNSS vel fusion on",
"GNSS vel fusion off",
false},
851 {11,
"GNSS hgt fusion on",
"GNSS hgt fusion off",
false},
852 {22,
"GNSS yaw fusion on",
"GNSS yaw fusion off",
false},
853 {9,
"baro hgt fusion on",
"baro hgt fusion off",
false},
854 {10,
"range hgt fusion on",
"range hgt fusion off",
false},
855 {3,
"flow fusion on",
"flow fusion off",
false},
856 {12,
"EV pos fusion on",
"EV pos fusion off",
false},
857 {24,
"EV vel fusion on",
"EV vel fusion off",
false},
858 {13,
"EV yaw fusion on",
"EV yaw fusion off",
false},
859 {14,
"EV hgt fusion on",
"EV hgt fusion off",
false},
860 {4,
"mag hdg fusion on",
"mag hdg fusion off",
false},
861 {5,
"mag 3D fusion on",
"mag 3D fusion off",
false},
862 {19,
"airspeed fusion on",
"airspeed fusion off",
false},
863 {15,
"sideslip fusion on",
"sideslip fusion off",
false},
864 {16,
"mag field disturbed",
"mag field ok",
false},
865 {18,
"mag fault",
"mag fault cleared",
true},
866 {45,
"GNSS fault",
"GNSS fault cleared",
true},
867 {47,
"GNSS hgt fault",
"GNSS hgt fault cleared",
true},
868 {27,
"GNSS yaw fault",
"GNSS yaw fault cleared",
true},
869 {43,
"baro fault",
"baro fault cleared",
true},
870 {28,
"range fault",
"range fault cleared",
true},
871 {36,
"EV yaw fault",
"EV yaw fault cleared",
true},
872 {29,
"dead reckoning",
"dead reckoning ended",
true},
873 {30,
"wind dead reckoning",
"wind dead reckoning ended",
true},
877static const struct ekf2_status_text ekf2_fault_texts[] = {
878 {0,
"mag X fusion error", NULL,
true},
879 {1,
"mag Y fusion error", NULL,
true},
880 {2,
"mag Z fusion error", NULL,
true},
881 {3,
"heading fusion error", NULL,
true},
882 {4,
"mag decl fusion error", NULL,
true},
883 {5,
"airspeed fusion error", NULL,
true},
884 {6,
"sideslip fusion error", NULL,
true},
885 {7,
"flow X fusion error", NULL,
true},
886 {8,
"flow Y fusion error", NULL,
true},
887 {10,
"bad vertical accel", NULL,
true},
888 {11,
"accel clipping", NULL,
true},
893static const char *
const ekf2_info_texts[] = {
894 "GNSS checks passed",
"vel reset to GNSS",
"vel reset to flow",
"vel reset to EV",
"vel reset to zero",
895 "pos reset to last known",
"pos reset to GNSS",
"pos reset to EV", NULL,
896 NULL, NULL, NULL,
"yaw aligned to IMU/GNSS",
897 "hgt reset to baro",
"hgt reset to GNSS",
"hgt reset to range",
"hgt reset to EV",
898 "pos reset to ext obs",
"wind reset to ext obs",
902#define EKF2_INFO_IMPORTANT ((1UL << 0) | (1UL << 12))
904#define EKF2_CONTROL_TEXTS_NB (sizeof(ekf2_control_texts) / sizeof(ekf2_control_texts[0]))
905#define EKF2_FAULT_TEXTS_NB (sizeof(ekf2_fault_texts) / sizeof(ekf2_fault_texts[0]))
906#define EKF2_INFO_TEXTS_NB (sizeof(ekf2_info_texts) / sizeof(ekf2_info_texts[0]))
911 float control_time[EKF2_CONTROL_TEXTS_NB];
912 float fault_time[EKF2_FAULT_TEXTS_NB];
913 float info_time[EKF2_INFO_TEXTS_NB];
921static void ekf2_event_send(
void)
924 if (ekf2_events.log.len > 0) {
925 printf(
"%s\n", ekf2_events.log.text);
929 if (ekf2_events.gcs.len > 0) {
933#if FLIGHTRECORDER_SDLOG
934 if (ekf2_events.log.len > 0 && flightRecorderLogFile != -1) {
935 pprz_msg_send_INFO_MSG(&
pprzlog_tp.trans_tx, &flightrecorder_sdlog.device, AC_ID, ekf2_events.log.len, ekf2_events.log.text);
938 ekf2_events.gcs.len = 0;
939 ekf2_events.log.len = 0;
943static void ekf2_event_append(
char *msg,
uint8_t *len,
size_t size,
const char *text)
945 int n = snprintf(msg + *len, size - *len, *len == 0 ?
"EKF2: %s" :
", %s", text);
947 *len =
Min(*len + n, (
int)size - 1);
953static void ekf2_event_add(
const char *text,
bool important)
955 size_t tlen = strlen(text);
956 if ((ekf2_events.log.len > 0 && ekf2_events.log.len + 2 + tlen >=
sizeof(ekf2_events.log.text)) ||
957 (ekf2_events.gcs.len > 0 && ekf2_events.gcs.len + 2 + tlen >=
sizeof(ekf2_events.gcs.text))) {
960 ekf2_event_append(ekf2_events.log.text, &ekf2_events.log.len,
sizeof(ekf2_events.log.text), text);
962 ekf2_event_append(ekf2_events.gcs.text, &ekf2_events.gcs.len,
sizeof(ekf2_events.gcs.text), text);
968static void ekf2_event_bits(
const struct ekf2_status_text *texts,
uint8_t nb,
float *times, T &reported, T current,
float now)
970 for (
uint8_t i = 0; i < nb; i++) {
971 T mask = ((
T)1) << texts[i].bit;
972 if ((current & mask) == (reported & mask) || (times[i] > 0.f && now - times[i] <
INS_EKF2_EVENT_HOLDOFF)) {
975 const char *text = (current & mask) ? texts[i].on : texts[i].off;
977 ekf2_event_add(text, texts[i].important);
980 reported = (reported & ~mask) | (current & mask);
988 if (now - ekf2_events.last_check < 0.5f) {
991 ekf2_events.last_check = now;
994 uint32_t info =
ekf.information_event_status().value;
995 ekf.clear_information_events();
996 for (
uint8_t i = 0; i < EKF2_INFO_TEXTS_NB; i++) {
997 float *time = &ekf2_events.info_time[i];
998 if ((info & (1UL << i)) && ekf2_info_texts[i] != NULL && (*time <= 0.f || now - *time >=
INS_EKF2_EVENT_HOLDOFF)) {
999 ekf2_event_add(ekf2_info_texts[i], (EKF2_INFO_IMPORTANT & (1UL << i)) != 0);
1003 ekf2_event_bits<uint64_t>(ekf2_control_texts, EKF2_CONTROL_TEXTS_NB, ekf2_events.control_time,
1004 ekf2_events.control,
ekf.control_status().value, now);
1005 ekf2_event_bits<uint32_t>(ekf2_fault_texts, EKF2_FAULT_TEXTS_NB, ekf2_events.fault_time,
1006 ekf2_events.fault,
ekf.fault_status().value, now);
1010#define ins_ekf2_check_events() {}
1020#ifndef INS_EKF2_PREFLIGHT_POS
1021#define INS_EKF2_PREFLIGHT_POS TRUE
1026#ifndef INS_EKF2_PREFLIGHT_INNOV_TAU
1027#define INS_EKF2_PREFLIGHT_INNOV_TAU 1.f
1033#define EKF2_PREFLIGHT_INNOV_NB 4
1034static const char *
const ekf2_preflight_innov_names[EKF2_PREFLIGHT_INNOV_NB] = {
"heading",
"velocity",
"position",
"height"};
1036 float ratio[EKF2_PREFLIGHT_INNOV_NB];
1044 float dt = now - ekf2_preflight.last_time;
1045 ekf2_preflight.last_time = now;
1046 const float ratio[EKF2_PREFLIGHT_INNOV_NB] = {
1047 ekf.getHeadingInnovationTestRatio(),
1048 math::max(
ekf.getHorizontalVelocityInnovationTestRatio(),
ekf.getVerticalVelocityInnovationTestRatio()),
1049 ekf.getHorizontalPositionInnovationTestRatio(),
1050 ekf.getVerticalPositionInnovationTestRatio()
1052 float alpha = math::constrain(dt / (INS_EKF2_PREFLIGHT_INNOV_TAU + dt), 0.f, 1.f);
1053 for (
uint8_t i = 0; i < EKF2_PREFLIGHT_INNOV_NB; i++) {
1054 if (!PX4_ISFINITE(ratio[i])) {
1055 ekf2_preflight.ratio[i] = NAN;
1056 }
else if (!PX4_ISFINITE(ekf2_preflight.ratio[i])) {
1057 ekf2_preflight.ratio[i] = ratio[i];
1059 ekf2_preflight.ratio[i] +=
alpha * (ratio[i] - ekf2_preflight.ratio[i]);
1066 const auto &flags =
ekf.control_status_flags();
1069 if (!flags.tilt_align) {
1072 if (!flags.yaw_align) {
1075 if (
ekf.fault_status().value != 0) {
1076 preflight_error(result,
"EKF2 numerical fault 0x%x", (
unsigned int)
ekf.fault_status().value);
1078 if (flags.mag_fault || flags.gnss_fault || flags.baro_fault || flags.gnss_hgt_fault || flags.gnss_yaw_fault) {
1079 preflight_error(result,
"EKF2 sensor fault (mag %d, GNSS %d, baro %d, GNSS hgt %d, GNSS yaw %d)",
1080 flags.mag_fault, flags.gnss_fault, flags.baro_fault, flags.gnss_hgt_fault, flags.gnss_yaw_fault);
1082 if (flags.inertial_dead_reckoning) {
1086#if INS_EKF2_PREFLIGHT_POS
1087 const int32_t gnss_pos =
static_cast<int32_t>(GnssCtrl::HPOS) |
static_cast<int32_t>(GnssCtrl::VEL);
1088 if ((
ekf_params->ekf2_gps_ctrl & gnss_pos) && !(flags.gnss_pos || flags.gnss_vel)) {
1089 static const char *
const names[] = {
"nsats",
"pdop",
"hacc",
"vacc",
"sacc",
"hdrift",
"vdrift",
"hspeed",
1090 "vspeed",
"spoofed",
"fix",
"jammed"
1092 char failed[80] =
"";
1094 for (
uint8_t i = 0; i <
sizeof(names) /
sizeof(names[0]); i++) {
1095 if (flags_fail & (1 << i)) {
1096 size_t l = strlen(failed);
1097 snprintf(failed + l,
sizeof(failed) - l,
"%s%s", l ?
"," :
"", names[i]);
1100 preflight_error(result,
"EKF2 not using GNSS (failed: %s)", failed[0] ? failed :
"no data");
1103 if ((
ekf_params->ekf2_ev_ctrl & ev_pos) && !(flags.ev_pos || flags.ev_vel)) {
1109 for (
uint8_t i = 0; i < EKF2_PREFLIGHT_INNOV_NB; i++) {
1110 if (PX4_ISFINITE(ekf2_preflight.ratio[i]) && ekf2_preflight.ratio[i] > 1.f) {
1111 preflight_error(result,
"EKF2 %s innovation too high (%.1f)", ekf2_preflight_innov_names[i], ekf2_preflight.ratio[i]);
1115 if (flags.mag_field_disturbed) {
1118 if (result->
fail_cnt == fail_cnt) {
1123#define ins_ekf2_preflight_filter() {}
1137 ekf.output_predictor().set_imu_offset(
ekf_params->imu_pos_body);
1138 ekf.output_predictor().set_pos_correction_tc(INS_EKF2_TAU_POS);
1139 ekf.output_predictor().set_vel_correction_tc(INS_EKF2_TAU_VEL);
1140 ekf.set_gnss_yaw_reset_wait(INS_EKF2_GPS_YAW_RST * 1000000);
1143 FusionControl *fc =
ekf.getFusionControlHandle();
1144 fc->gps.enabled =
true;
1145 fc->of.enabled =
true;
1146 fc->ev.enabled =
true;
1147 fc->baro.enabled =
true;
1148 fc->rng.enabled =
true;
1149 fc->mag.enabled =
true;
1150 fc->aspd.enabled =
true;
1151 ekf.updateParameters();
1154 GnssChecks::Params checks_params{};
1156 checks_params.req_nsats = INS_EKF2_GNSS_REQ_NSATS;
1157 checks_params.req_pdop = INS_EKF2_GNSS_REQ_PDOP;
1159 checks_params.req_epv = INS_EKF2_GNSS_REQ_EPV;
1161 checks_params.req_hdrift = INS_EKF2_GNSS_REQ_HDRIFT;
1162 checks_params.req_vdrift = INS_EKF2_GNSS_REQ_VDRIFT;
1163 checks_params.req_fix = INS_EKF2_GNSS_REQ_FIX;
1164 checks_params.min_health_time_us = INS_EKF2_GNSS_REQ_TIME * 1000000;
1169 ekf2.flow_stamp = 0;
1170 ekf2.gyro_valid =
false;
1171 ekf2.accel_valid =
false;
1172 ekf2.got_imu_data =
false;
1173 ekf2.quat_reset_counter = 0;
1175 ekf2.in_air =
false;
1177 ekf2.fixed_wing =
false;
1181 ekf2.mag_scale = 0.4f;
1183 float strength = get_mag_strength_gauss(NAV_LAT0 * 1e-7f, NAV_LON0 * 1e-7f);
1184 if (PX4_ISFINITE(strength) && strength > 0.1f && strength < 1.f) {
1185 ekf2.mag_scale = strength;
1188 ekf2.system_flags_time = 0;
1203 if (
ekf.setEkfGlobalOrigin(NAV_LAT0 * 1e-7, NAV_LON0 * 1e-7, (NAV_ALT0) * 1e-3)) {
1205 llh_nav0.
lat = NAV_LAT0;
1206 llh_nav0.
lon = NAV_LON0;
1208 llh_nav0.
alt = NAV_ALT0 + NAV_MSL0;
1211 ekf2.ltp_def.hmsl = NAV_ALT0;
1221#if PERIODIC_TELEMETRY
1250 for (
uint8_t i = 0; i < EKF2_PREFLIGHT_INNOV_NB; i++) {
1251 ekf2_preflight.ratio[i] = NAN;
1263 if (
ekf.setEkfGlobalOrigin(lla_pos.
lat * 1e-7, lla_pos.
lon * 1e-7,
gps.hmsl * 1e-3)) {
1283 if (
ekf.setEkfGlobalOrigin(lla.
lat * 1e-7, lla.
lon * 1e-7,
gps.hmsl * 1e-3)) {
1315 volatile bool still;
1321 if (!(
imu.delta_ang_dt > 0.f) || !(
imu.delta_vel_dt > 0.f)) {
1324 const float dt =
imu.delta_ang_dt;
1325 const Vector3f gyro =
imu.delta_ang /
imu.delta_ang_dt;
1326 const Vector3f accel =
imu.delta_vel /
imu.delta_vel_dt;
1331 ekf2_rest.gyro_metric = 10.f * gyro_thr;
1336 const float alpha_avg = fminf(dt / 2.f, 1.f);
1337 const float alpha = fminf(dt / 0.5f, 1.f);
1342 const float rate = (
ekf2_rest.gyro_avg - gyro_bias).norm();
1363 if (in_air ==
ekf2.in_air && armed ==
ekf2.armed && fixed_wing ==
ekf2.fixed_wing && at_rest ==
ekf2_rest.at_rest
1364 && now <
ekf2.system_flags_time + 1000000) {
1368 systemFlagUpdate flags{};
1369 flags.time_us = now;
1370 flags.armed = armed;
1371 flags.in_air = in_air;
1372 flags.at_rest = at_rest;
1373 flags.is_fixed_wing = fixed_wing;
1375 int32_t bits = (flags.armed << 0) | (flags.at_rest << 1) | (flags.in_air << 2) | (flags.is_fixed_wing << 3)
1376 | (flags.gnd_effect << 4) | (flags.constant_pos << 5) | (flags.in_transition << 6);
1379 ekf.setSystemFlagData(flags);
1381 ekf2.in_air = in_air;
1383 ekf2.fixed_wing = fixed_wing;
1385 ekf2.system_flags_time = now;
1396 volatile bool new_data;
1418 if (
ekf2.air_density <= 0.f) {
1421 airspeedSample sample{};
1424 sample.true_airspeed = eas * sample.eas2tas;
1429 float values[2] = {sample.true_airspeed, sample.eas2tas};
1432 ekf.setAirspeedData(sample);
1446 if (
ekf2.got_imu_data) {
1456 const Vector3f vel_deriv_f{
ekf.getVelocityDerivative()};
1457 ekf.resetVelocityDerivativeAccumulation();
1460 if (
ekf.control_status_flags().tilt_align) {
1462 const Vector3f pos_f{
ekf.getPosition()};
1472 const Vector3f vel_f{
ekf.getVelocity()};
1481 ekf2.accel_ned.x = vel_deriv_f(0);
1482 ekf2.accel_ned.y = vel_deriv_f(1);
1483 ekf2.accel_ned.z = vel_deriv_f(2);
1487 accel.
x =
ekf2.accel_ned.x;
1488 accel.
y =
ekf2.accel_ned.y;
1489 accel.
z =
ekf2.accel_ned.z;
1494 double ekf_origin_lat, ekf_origin_lon;
1500 if (
ekf.global_origin_valid()) {
1501 ekf.getEkfGlobalOrigin(origin_time, ekf_origin_lat, ekf_origin_lon, ref_alt);
1505 if (origin_time >
ekf2.ltp_stamp) {
1506#if INS_EKF2_FORCE_NAV_REF && USE_INS_NAV_INIT
1508 if (
ekf.setEkfGlobalOrigin(NAV_LAT0 * 1e-7, NAV_LON0 * 1e-7, (NAV_ALT0) * 1e-3)) {
1510 llh_nav0.
lat = NAV_LAT0;
1511 llh_nav0.
lon = NAV_LON0;
1512 llh_nav0.
alt = NAV_ALT0 + NAV_MSL0;
1515 ekf2.ltp_def.hmsl = NAV_ALT0;
1519 lla_ref.
lat = ekf_origin_lat * 1e7;
1520 lla_ref.
lon = ekf_origin_lon * 1e7;
1523 ekf2.ltp_def.hmsl = ref_alt * 1e3;
1529 ekf2.ltp_stamp = origin_time;
1534#if defined SITL && USE_NPS
1540 ekf2.got_imu_data =
false;
1545 if (DL_EXTERNAL_POSE_ac_id(buf) != AC_ID) {
return; }
1548 sample_ev.pos(0) = DL_EXTERNAL_POSE_enu_y(buf);
1549 sample_ev.pos(1) = DL_EXTERNAL_POSE_enu_x(buf);
1550 sample_ev.pos(2) = -DL_EXTERNAL_POSE_enu_z(buf);
1551 sample_ev.vel(0) = DL_EXTERNAL_POSE_enu_yd(buf);
1552 sample_ev.vel(1) = DL_EXTERNAL_POSE_enu_xd(buf);
1553 sample_ev.vel(2) = -DL_EXTERNAL_POSE_enu_zd(buf);
1554 sample_ev.quat(0) = DL_EXTERNAL_POSE_body_qi(buf);
1555 sample_ev.quat(1) = DL_EXTERNAL_POSE_body_qy(buf);
1556 sample_ev.quat(2) = DL_EXTERNAL_POSE_body_qx(buf);
1557 sample_ev.quat(3) = -DL_EXTERNAL_POSE_body_qz(buf);
1559#ifdef INS_EXT_VISION_ROTATION
1571 sample_ev.position_var.setAll(INS_EKF2_EVP_NOISE * INS_EKF2_EVP_NOISE);
1572 sample_ev.velocity_var.setAll(INS_EKF2_EVV_NOISE * INS_EKF2_EVV_NOISE);
1573 sample_ev.orientation_var.setAll(INS_EKF2_EVA_NOISE * INS_EKF2_EVA_NOISE);
1574 sample_ev.pos_frame = PositionFrame::LOCAL_FRAME_NED;
1575 sample_ev.vel_frame = VelocityFrame::LOCAL_FRAME_NED;
1603 imuSample imu_sample = {};
1604 imu_sample.time_us = time_us;
1605 imu_sample.delta_ang_dt =
ekf2.gyro_dt * 1.e-6f;
1606 imu_sample.delta_ang = Vector3f{
ekf2.delta_gyro.p,
ekf2.delta_gyro.q,
ekf2.delta_gyro.r};
1607 imu_sample.delta_vel_dt =
ekf2.accel_dt * 1.e-6f;
1608 imu_sample.delta_vel = Vector3f{
ekf2.delta_accel.x,
ekf2.delta_accel.y,
ekf2.delta_accel.z};
1614 float values[6] = {imu_sample.delta_ang(0), imu_sample.delta_ang(1), imu_sample.delta_ang(2),
1615 imu_sample.delta_vel(0), imu_sample.delta_vel(1), imu_sample.delta_vel(2)};
1619 ekf.setIMUData(imu_sample);
1621 if (
ekf.attitude_valid()) {
1624 const Quatf att_q{
ekf.getQuaternion()};
1625 ltp_to_body_quat.
qi = att_q(0);
1626 ltp_to_body_quat.
qx = att_q(1);
1627 ltp_to_body_quat.
qy = att_q(2);
1628 ltp_to_body_quat.
qz = att_q(3);
1634 float delta_q_reset[4];
1636 ekf.get_quat_reset(delta_q_reset, &quat_reset_counter);
1638#ifndef NO_RESET_UPDATE_SETPOINT_HEADING
1640 if (
ekf2.quat_reset_counter < quat_reset_counter) {
1641 float psi = matrix::Eulerf(matrix::Quatf(delta_q_reset)).psi();
1647 ekf2.quat_reset_counter = quat_reset_counter;
1653 Vector3f gyro_bias{
ekf.getGyroBias()};
1654 body_rates.
p = (
ekf2.delta_gyro.p / (
ekf2.gyro_dt * 1.e-6f)) - gyro_bias(0);
1655 body_rates.
q = (
ekf2.delta_gyro.q / (
ekf2.gyro_dt * 1.e-6f)) - gyro_bias(1);
1656 body_rates.
r = (
ekf2.delta_gyro.r / (
ekf2.gyro_dt * 1.e-6f)) - gyro_bias(2);
1663 Vector3f accel_bias{
ekf.getAccelBias()};
1672 ekf2.gyro_valid =
false;
1673 ekf2.accel_valid =
false;
1674 ekf2.got_imu_data =
true;
1685 ekf.set_air_density(rho);
1686 ekf2.air_density = rho;
1689 baroSample sample{};
1693 float values[2] = {sample.hgt, rho};
1696 ekf.setBaroData(sample);
1711 estimator::sensor::rangeSample sample{};
1713 sample.rng = distance;
1714 sample.quality = -1;
1717 int32_t quality = sample.quality;
1720 ekf.setRangeData(sample);
1730 ekf2.gyro_valid =
true;
1733 if (
ekf2.gyro_valid &&
ekf2.accel_valid) {
1745 ekf2.accel_valid =
true;
1748 if (
ekf2.gyro_valid &&
ekf2.accel_valid) {
1777 sample.mag = Vector3f{mag_gauss.
x, mag_gauss.
y, mag_gauss.
z};
1779 float values[3] = {mag_gauss.
x, mag_gauss.
y, mag_gauss.
z};
1783 ekf.setMagData(sample);
1784 ekf2.got_imu_data =
true;
1809 gnssChecksSample checks_sample{};
1811 checks_sample.lat = lla_pos.
lat * 1e-7;
1812 checks_sample.lon = lla_pos.
lon * 1e-7;
1813 checks_sample.alt = gps_s->
hmsl * 1e-3f;
1814 checks_sample.vel = Vector3f{ned_vel.
x, ned_vel.
y, ned_vel.
z};
1815 checks_sample.hacc = gps_s->
hacc / 100.0f;
1816 checks_sample.vacc = gps_s->
vacc / 100.0f;
1817 checks_sample.sacc = gps_s->
sacc / 100.0f;
1818 checks_sample.fix_type = gps_s->
fix;
1819 checks_sample.nsats = gps_s->
num_sv;
1820 checks_sample.pdop = gps_s->
pdop / 100.0f;
1821 const bool at_rest =
ekf.control_status_flags().vehicle_at_rest;
1825 gnssSample sample{};
1827 sample.lat = checks_sample.lat;
1828 sample.lon = checks_sample.lon;
1829 sample.alt = checks_sample.alt;
1830 sample.vel = checks_sample.vel;
1831 sample.hacc = checks_sample.hacc;
1832 sample.vacc = checks_sample.vacc;
1833 sample.sacc = checks_sample.sacc;
1834 sample.fix_type = checks_sample.fix_type;
1835 sample.nsats = checks_sample.nsats;
1836 sample.pdop = checks_sample.pdop;
1839 sample.sacc_passes_strict = !(
gnss_checks.getStrictFailFlags() & vehicle_gnss_s::CHECK_SACC);
1843 int32_t flags = (sample.usable << 0) | (sample.sacc_passes_strict << 1) | (sample.spoofed << 2) | (sample.jammed << 3)
1845 | (at_rest << 6) | (
ekf2.armed << 7) | (
ekf2.in_air << 8);
1846 int32_t ints[7] = {lla_pos.
lat, lla_pos.
lon, sample.fix_type, sample.nsats, flags,
1848 float values[11] = {sample.alt, sample.vel(0), sample.vel(1), sample.vel(2), sample.hacc, sample.vacc, sample.sacc,
1849 sample.pdop, sample.pos_body(0), sample.pos_body(1), sample.pos_body(2)};
1852 ekf.setGpsData(sample);
1864#ifdef INS_EKF2_RELHEADING_REF_ID
1867 !PX4_ISFINITE(relpos->
heading)
1872#ifdef INS_EKF2_GPS_YAW_OFFSET
1874 gnssYawSample yaw_sample{};
1876 yaw_sample.yaw = wrap_pi(relpos->
heading - RadOfDeg(INS_EKF2_GPS_YAW_OFFSET));
1878 yaw_sample.yaw_offset = RadOfDeg(INS_EKF2_GPS_YAW_OFFSET);
1879 yaw_sample.usable =
true;
1881 int32_t usable = yaw_sample.usable;
1882 float values[3] = {yaw_sample.yaw, yaw_sample.yaw_acc, yaw_sample.yaw_offset};
1885 ekf.setGnssYawData(yaw_sample);
1896 int32_t flow_der_x __attribute__((unused)),
1897 int32_t flow_der_y __attribute__((unused)),
1899 float size_divergence __attribute__((unused)))
1905 if (
ekf2.flow_stamp <= 0) {
1906 ekf2.flow_stamp = stamp;
1910 ekf2.flow_stamp = stamp;
1914 flowSample sample{};
1916 sample.flow_rate = Vector2f{(float)RadOfDeg(flow_y), -(float)RadOfDeg(flow_x)};
1917 sample.gyro_rate = Vector3f{NAN, NAN, NAN};
1918 sample.quality = quality;
1921 int32_t quality = sample.quality;
1922 float values[5] = {sample.flow_rate(0), sample.flow_rate(1), sample.gyro_rate(0), sample.gyro_rate(1), sample.gyro_rate(2)};
1926 ekf.setOpticalFlowData(sample);
Main include for ABI (AirBorneInterface).
struct abi_struct abi_event
#define ABI_BROADCAST
Broadcast address.
#define AIRSPEED_WE_QUAD_ID
#define AHRS_COMP_ID_EKF2
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
static void mag_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag)
static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
bool autopilot_get_motors_on(void)
get motors status
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.
Common code for AP and FBW telemetry.
static struct uart_periph * dev
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 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.
int32_t hmsl
height above mean sea level (MSL) in mm
float heading
Relative heading to the reference station in radians.
uint32_t sacc
speed accuracy in cm/s
#define GPS_VALID_VEL_ECEF_BIT
#define GPS_VALID_VEL_NED_BIT
float heading_acc
Heading accuracy in radians.
uint32_t hacc
horizontal accuracy in cm
uint16_t pdop
position dilution of precision scaled by 100
uint8_t valid_fields
bitfield indicating valid fields (GPS_VALID_x_BIT)
uint32_t vacc
vertical accuracy in cm
uint16_t reference_id
Reference station identification.
uint8_t num_sv
number of sat in fix
data structure for GPS information
#define FLOAT_VECT3_ZERO(_v)
void float_quat_comp(struct FloatQuat *a2c, const struct FloatQuat *a2b, const struct FloatQuat *b2c)
Composition (multiplication) of two quaternions.
#define MAGS_FLOAT_OF_BFP(_ef, _ei)
#define RATES_COPY(_a, _b)
#define VECT3_COPY(_a, _b)
#define QUAT1_BFP_OF_REAL(_qf)
#define ACCEL_BFP_OF_REAL(_af)
#define POS_BFP_OF_REAL(_af)
#define SPEED_BFP_OF_REAL(_af)
int32_t lat
in degrees*1e7
int32_t alt
in millimeters above WGS84 reference ellipsoid
int32_t lon
in degrees*1e7
void ltp_def_from_lla_i(struct LtpDef_i *def, struct LlaCoor_i *lla)
vector in Latitude, Longitude and Altitude
vector in North East Down coordinates
static int32_t wgs84_ellipsoid_to_geoid_i(int32_t lat, int32_t lon)
Get WGS84 ellipsoid/geoid separation.
static float pprz_isa_density_of_pressure(float pressure, float temp)
Get the air density (rho) from a given pressure and temperature.
#define PPRZ_ISA_AIR_DENSITY
standard air density in kg/m^3
#define PPRZ_ISA_SEA_LEVEL_PRESSURE
ISA sea level standard atmospheric pressure in Pascal.
static float pprz_isa_height_of_pressure_full(float pressure, float ref_p)
Get relative altitude from pressure (using full equation).
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 Int32Quat * stateGetNedToBodyQuat_i(void)
Get vehicle body attitude quaternion (int).
static void stateSetNedToBodyQuat_f(uint16_t id, struct FloatQuat *ned_to_body_quat)
Set vehicle body attitude from quaternion (float).
static bool stateIsFixedWingFlight(void)
Flying as a fixed wing (always for the fixedwing firmware, in forward flight for hybrids).
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).
struct LlaCoor_i stateGetLlaOrigin_i(void)
Get the LLA position of the 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).
struct Imu imu
global IMU state
struct imu_mag_t * imu_get_mag(uint8_t sender_id, bool create)
Find or create the mag in the imu structure.
Inertial Measurement Unit interface.
float strength
Earth field strength where it was calibrated (gauss), the scaled values are 1 there; 0 if unknown.
#define INS_RESET_VERTICAL_REF
#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 baro_cb(uint8_t sender_id, uint32_t stamp, float pressure)
#define ekf2_log_check_start()
static void reset_vertical_ref(void)
#define EKF2_GPS_CTRL_VEL
3D velocity
#define INS_EKF2_GNSS_REQ_EPH
Indoor configuration with only external vision (e.g.
static void relpos_cb(uint8_t sender_id, uint32_t stamp, struct RelPosNED *relpos)
static GnssChecks gnss_checks
GNSS checks before the GNSS data is used.
static void ins_ekf2_at_rest_update(const imuSample &imu, const Vector3f &gyro_bias)
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
#define INS_EKF2_RNG_MAX_DIST
Maximum distance the range finder can measure (m).
static abi_event airspeed_ev
#define INS_EKF2_EVENT_HOLDOFF
Minimum time between two messages about the same status, so a flickering status can't flood the link ...
void ins_ekf2_update(void)
#define INS_EKF2_AT_REST_ACCEL
accelerometer threshold (m/s2), noise at rest about 0.04
static void reset_cb(uint8_t sender_id, uint8_t flag)
static void ins_ekf2_airspeed_update(void)
Give the last airspeed to the EKF, in the thread of the EKF.
static void send_ins(struct transport_tx *trans, struct link_device *dev)
#define ekf2_log_output(...)
#define INS_EKF2_GPS_POS_Y
static void send_ins_ekf2(struct transport_tx *trans, struct link_device *dev)
#define INS_EKF2_GPS_DELAY
GNSS measurement delay (ms), as the PX4 SENS_GNSS0_DELAY parameter.
static void send_ins_ekf2_ext(struct transport_tx *trans, struct link_device *dev)
static abi_event temperature_ev
static void 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)
#define INS_EKF2_TEMPERATURE_ID
default temperature sensor to use in INS
static void reset_ref(void)
#define INS_EKF2_RELPOS_ID
static void mag_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag)
static void accel_int_cb(uint8_t sender_id, uint32_t stamp, struct FloatVect3 *delta_accel, uint16_t dt)
#define INS_EKF2_AIRSPEED_ID
default airspeed sensor to use in INS (equivalent airspeed, only fused above INS_EKF2_ARSP_THR): any ...
#define INS_EKF2_GPS_POS_Z
static abi_event gyro_int_ev
static abi_event relpos_ev
static struct @251215145125312176322261332011232027323240311102 ekf2_rest
At rest detection from the IMU samples given to the EKF.
#define INS_EKF2_ACCEL_ID
static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
static void temperature_cb(uint8_t sender_id, float temp)
static void agl_cb(uint8_t sender_id, uint32_t stamp, float distance)
#define INS_EKF2_GPS_YAW_NOISE
#define INS_EKF2_GNSS_REQ_SACC
static void send_ins_z(struct transport_tx *trans, struct link_device *dev)
static parameters * ekf_params
The EKF parameters.
static void ins_ekf2_update_system_flags(void)
Give the arming, in-air and at rest state to the EKF when it changes, and every second (the EKF ignor...
static bool ekf2_input_enabled(void)
static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
#define ins_ekf2_check_events()
#define ekf2_log_param_changed(...)
static float test_ratio(float ratio)
Innovation test ratio, 0 when not available.
static struct @056174147002310323301350044105370077147041341005 ekf2_airspeed
Airspeed (equivalent airspeed) to the EKF as true airspeed.
static void baro_cb(uint8_t sender_id, uint32_t stamp, float pressure)
static uint64_t ekf2_time_us(uint32_t stamp)
Convert a 32 bit timestamp (us) of an ABI message to the 64 bit time of the EKF, so the EKF time keep...
static void ins_ekf2_publish_attitude(uint64_t time_us)
Publish the attitude and get the new state Directly called after a succeslfull gyro+accel reading.
#define ekf2_log_origin_set(...)
static void send_wind_info_ret(struct transport_tx *trans, struct link_device *dev)
#define INS_EKF2_GNSS_CHECK
GNSS checks used, PX4 enables all except jamming (2047).
static Ekf ekf
EKF class itself.
static uint64_t ekf2_delayed_time_us(uint32_t stamp, uint32_t delay_ms)
Time of a measurement that was taken delay_ms before it arrived.
#define INS_EKF2_AT_REST_TIME
time below the thresholds before at rest (s)
struct ekf2_t ekf2
Local EKF2 status structure.
void ins_ekf2_parse_EXTERNAL_POSE(uint8_t *buf)
#define INS_EKF2_AT_REST_RATE
average rotation rate threshold (deg/s), above the gyro bias before it is estimated
void ins_ekf2_parse_EXTERNAL_POSE_SMALL(uint8_t *buf)
static void send_ahrs_quat(struct transport_tx *trans, struct link_device *dev)
#define INS_EKF2_HGT_REF
Barometer as height reference (PX4 uses GNSS), as the previous version: the heights don't depend on t...
#define INS_EKF2_AGL_ID
default AGL sensor to use in INS
static void gyro_int_cb(uint8_t sender_id, uint32_t stamp, struct FloatRates *delta_gyro, uint16_t dt)
#define INS_EKF2_AT_REST_GYRO
gyro threshold (deg/s), noise at rest about 0.15
static abi_event accel_int_ev
#define INS_EKF2_EVENTS_ALL
Send all the EKF events to the GCS, not only the important ones (alignment, dead reckoning,...
static struct extVisionSample sample_ev
External vision sample.
#define INS_EKF2_BARO_ID
default barometer to use in INS
static void send_external_pose_down(struct transport_tx *trans, struct link_device *dev)
#define INS_EKF2_GPS_POS_X
static void airspeed_cb(uint8_t sender_id, float eas)
#define INS_EKF2_RNG_MIN_DIST
Minimum distance the range finder can measure (m).
#define INS_EKF2_AT_REST_DETECTION
At rest detection, as the PX4 land detector (at rest when landed and not moving for 1 s): the EKF the...
static void send_ahrs_bias(struct transport_tx *trans, struct link_device *dev)
#define ins_ekf2_preflight_filter()
#define INS_EKF2_OF_MAX_RATE
Maximum flow rate the optical flow sensor can measure (rad/s).
INS based in the EKF2 of PX4.
void waypoints_localize_all(void)
update local ENU coordinates of global waypoints
void sim_overwrite_ins(void)
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
static abi_event optical_flow_ev
static abi_event agl_ev
The altitude ABI event.
vector in North East Down coordinates Units: meters
Paparazzi atmospheric pressure conversion utilities.
struct pprzlog_transport pprzlog_tp
PPRZLOG transport structure.
Initialize pprzlog transport.
void preflight_error(struct preflight_result_t *result, const char *fmt,...)
Register a preflight error used inside the preflight checking functions.
void preflight_success(struct preflight_result_t *result, const char *fmt,...)
Register a preflight success used inside the preflight checking functions.
void preflight_warning(struct preflight_result_t *result, const char *fmt,...)
Register a preflight warning used inside the preflight checking functions.
void preflight_check_register(struct preflight_check_t *check, preflight_check_f func)
Register a preflight check and add it to the linked list.
struct HorizontalGuidance guidance_h
struct RotorcraftNavigation nav
void stabilization_attitude_enter(void)
Attitude control enter function.
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.
static const float pos_z[]
static const float scale[]
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 long long uint64_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.