|
Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
INS based in the EKF2 of PX4. More...
#include "modules/ins/ins_ekf2.h"#include "modules/nav/waypoints.h"#include "modules/core/abi.h"#include "modules/imu/imu.h"#include "stabilization/stabilization_attitude.h"#include "generated/airframe.h"#include "generated/flight_plan.h"#include "EKF/ekf.h"#include "lib/gnss/gnss_checks.hpp"#include "lib/world_magnetic_model/geo_mag_declination.h"#include "math/pprz_isa.h"#include "math/pprz_geodetic_wgs84.h"#include "mcu_periph/sys_time.h"#include "autopilot.h"#include "generated/ekf2_params.h"#include "modules/datalink/telemetry.h"Go to the source code of this file.
Macros | |
| #define | USE_INS_NAV_INIT TRUE |
| For SITL and NPS we need special includes. | |
| #define | GNSS_CHECK_NSATS (1<<0) |
| GNSS checks (INS_EKF2_GNSS_CHECK), bits of the PX4 GNSS_CHECK parameter. | |
| #define | GNSS_CHECK_PDOP (1<<1) |
| position dilution of precision (INS_EKF2_GNSS_REQ_PDOP) | |
| #define | GNSS_CHECK_EPH (1<<2) |
| horizontal position accuracy (INS_EKF2_GNSS_REQ_EPH) | |
| #define | GNSS_CHECK_EPV (1<<3) |
| vertical position accuracy (INS_EKF2_GNSS_REQ_EPV) | |
| #define | GNSS_CHECK_SACC (1<<4) |
| speed accuracy (INS_EKF2_GNSS_REQ_SACC) | |
| #define | GNSS_CHECK_HDRIFT (1<<5) |
| horizontal position drift at rest on the ground (INS_EKF2_GNSS_REQ_HDRIFT) | |
| #define | GNSS_CHECK_VDRIFT (1<<6) |
| vertical position drift at rest on the ground (INS_EKF2_GNSS_REQ_VDRIFT) | |
| #define | GNSS_CHECK_HSPEED (1<<7) |
| horizontal speed at rest on the ground (INS_EKF2_GNSS_REQ_HDRIFT) | |
| #define | GNSS_CHECK_VSPEED (1<<8) |
| vertical speed at rest on the ground (INS_EKF2_GNSS_REQ_VDRIFT) | |
| #define | GNSS_CHECK_SPOOFED (1<<9) |
| receiver reports spoofing | |
| #define | GNSS_CHECK_FIX (1<<10) |
| fix type (INS_EKF2_GNSS_REQ_FIX) | |
| #define | GNSS_CHECK_JAMMED (1<<11) |
| receiver reports jamming | |
| #define | INS_EKF2_GNSS_CHECK (GNSS_CHECK_NSATS | GNSS_CHECK_EPH | GNSS_CHECK_SACC | GNSS_CHECK_FIX) |
| GNSS checks used, PX4 enables all except jamming (2047). | |
| #define | EKF2_GPS_CTRL_HPOS 1 |
| Values of INS_EKF2_GPS_CTRL (bitmask). | |
| #define | EKF2_GPS_CTRL_VPOS 2 |
| vertical position | |
| #define | EKF2_GPS_CTRL_VEL 4 |
| 3D velocity | |
| #define | EKF2_GPS_CTRL_YAW 8 |
| dual antenna heading | |
| #define | EKF2_EV_CTRL_HPOS 1 |
| Values of INS_EKF2_EV_CTRL (bitmask). | |
| #define | EKF2_EV_CTRL_VPOS 2 |
| vertical position | |
| #define | EKF2_EV_CTRL_VEL 4 |
| 3D velocity | |
| #define | EKF2_EV_CTRL_YAW 8 |
| yaw | |
| #define | EKF2_HGT_REF_BARO 0 |
| Values of INS_EKF2_HGT_REF. | |
| #define | EKF2_HGT_REF_GNSS 1 |
| #define | EKF2_HGT_REF_RANGE 2 |
| #define | EKF2_HGT_REF_EV 3 |
| #define | EKF2_MAG_TYPE_AUTO 0 |
| Values of INS_EKF2_MAG_TYPE. | |
| #define | EKF2_MAG_TYPE_HEADING 1 |
| heading only | |
| #define | EKF2_MAG_TYPE_NONE 5 |
| not used | |
| #define | EKF2_MAG_TYPE_INIT 6 |
| only to initialize the heading | |
| #define | INS_EKF2_GNSS_REQ_EPH 5.0f |
| Indoor configuration with only external vision (e.g. | |
| #define | INS_EKF2_GNSS_REQ_SACC 1.0f |
| #define | INS_EKF2_HGT_REF EKF2_HGT_REF_BARO |
| Barometer as height reference (PX4 uses GNSS), as the previous version: the heights don't depend on the ground altitude of the flight plan. | |
| #define | INS_EKF2_AT_REST_DETECTION TRUE |
| At rest detection, as the PX4 land detector (at rest when landed and not moving for 1 s): the EKF then runs the GNSS drift checks on the ground, fuses zero velocity without other velocity aiding (so a drifting GNSS, e.g. | |
| #define | INS_EKF2_AT_REST_GYRO 0.3f |
| gyro threshold (deg/s), noise at rest about 0.15 | |
| #define | INS_EKF2_AT_REST_ACCEL 0.1f |
| accelerometer threshold (m/s2), noise at rest about 0.04 | |
| #define | INS_EKF2_AT_REST_RATE 1.f |
| average rotation rate threshold (deg/s), above the gyro bias before it is estimated | |
| #define | INS_EKF2_AT_REST_TIME 1.f |
| time below the thresholds before at rest (s) | |
| #define | INS_EKF2_RNG_FOG 1.f |
| #define | INS_EKF2_GPS_DELAY 110 |
| GNSS measurement delay (ms), as the PX4 SENS_GNSS0_DELAY parameter. | |
| #define | INS_EKF2_RNG_MIN_DIST 0.001f |
| Minimum distance the range finder can measure (m). | |
| #define | INS_EKF2_RNG_MAX_DIST 4.f |
| Maximum distance the range finder can measure (m). | |
| #define | INS_EKF2_OF_MAX_RATE 200.f |
| Maximum flow rate the optical flow sensor can measure (rad/s). | |
| #define | INS_EKF2_AIRSPEED_ID AIR_DATA_SENDER_ID |
| default airspeed sensor to use in INS (equivalent airspeed, only fused above INS_EKF2_ARSP_THR): any sensor, or the airspeed computed by air_data from a differential pressure sensor. | |
| #define | INS_EKF2_BARO_ID AIR_DATA_SENDER_ID |
| default barometer to use in INS | |
| #define | INS_EKF2_TEMPERATURE_ID AIR_DATA_SENDER_ID |
| default temperature sensor to use in INS | |
| #define | INS_EKF2_AGL_ID ABI_BROADCAST |
| default AGL sensor to use in INS | |
| #define | INS_EKF2_GYRO_ID ABI_BROADCAST |
| #define | INS_EKF2_ACCEL_ID ABI_BROADCAST |
| #define | INS_EKF2_MAG_ID ABI_BROADCAST |
| #define | INS_EKF2_GPS_ID GPS_MULTI_ID |
| #define | INS_EKF2_RELPOS_ID ABI_BROADCAST |
| #define | INS_EKF2_OF_ID ABI_BROADCAST |
| #define | INS_EKF2_GPS_POS_X 0 |
| #define | INS_EKF2_GPS_POS_Y 0 |
| #define | INS_EKF2_GPS_POS_Z 0 |
| #define | INS_EKF2_GPS_YAW_NOISE 0.1f |
| #define | ekf2_log(...) |
| #define | ekf2_log_param_changed(...) |
| #define | ekf2_log_origin_set(...) |
| #define | ekf2_log_check_start() |
| #define | ekf2_log_output(...) |
| #define | INS_EKF2_EVENT_MSGS TRUE |
| #define | INS_EKF2_EVENTS_ALL FALSE |
| Send all the EKF events to the GCS, not only the important ones (alignment, dead reckoning, faults). | |
| #define | INS_EKF2_EVENT_HOLDOFF 5.f |
| Minimum time between two messages about the same status, so a flickering status can't flood the link [s]. | |
| #define | ins_ekf2_check_events() |
| #define | ins_ekf2_preflight_filter() |
Enumerations | |
| enum | ekf2_input_type { EKF2_IN_IMU , EKF2_IN_UPDATE , EKF2_IN_MAG , EKF2_IN_BARO , EKF2_IN_GNSS , EKF2_IN_GNSS_YAW , EKF2_IN_AIRSPEED , EKF2_IN_RANGE , EKF2_IN_FLOW , EKF2_IN_EV , EKF2_IN_FLAGS , EKF2_IN_ORIGIN , EKF2_IN_PARAMS , EKF2_IN_CONFIG , EKF2_IN_OUTPUT } |
Functions | |
| static void | baro_cb (uint8_t sender_id, uint32_t stamp, float pressure) |
| static void | temperature_cb (uint8_t sender_id, float temp) |
| static void | agl_cb (uint8_t sender_id, uint32_t stamp, float distance) |
| static void | gyro_int_cb (uint8_t sender_id, uint32_t stamp, struct FloatRates *delta_gyro, uint16_t dt) |
| static void | accel_int_cb (uint8_t sender_id, uint32_t stamp, struct FloatVect3 *delta_accel, uint16_t dt) |
| static void | mag_cb (uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag) |
| static void | gps_cb (uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s) |
| static void | relpos_cb (uint8_t sender_id, uint32_t stamp, struct RelPosNED *relpos) |
| 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) |
| static void | airspeed_cb (uint8_t sender_id, float eas) |
| static void | reset_cb (uint8_t sender_id, uint8_t flag) |
| 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. | |
| 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 keeps increasing when the 32 bit time wraps (after 71 minutes). | |
| 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. | |
| static bool | ekf2_input_enabled (void) |
| static void | send_ins (struct transport_tx *trans, struct link_device *dev) |
| static void | send_ins_z (struct transport_tx *trans, struct link_device *dev) |
| static void | send_ins_ref (struct transport_tx *trans, struct link_device *dev) |
| static float | test_ratio (float ratio) |
| Innovation test ratio, 0 when not available. | |
| static void | send_ins_ekf2 (struct transport_tx *trans, struct link_device *dev) |
| static void | send_ins_ekf2_ext (struct transport_tx *trans, struct link_device *dev) |
| static void | send_filter_status (struct transport_tx *trans, struct link_device *dev) |
| static void | send_wind_info_ret (struct transport_tx *trans, struct link_device *dev) |
| static void | send_ahrs_bias (struct transport_tx *trans, struct link_device *dev) |
| static void | send_ahrs_quat (struct transport_tx *trans, struct link_device *dev) |
| static void | send_external_pose_down (struct transport_tx *trans, struct link_device *dev) |
| void | ins_ekf2_init (void) |
| static void | reset_ref (void) |
| static void | reset_vertical_ref (void) |
| static void | ins_ekf2_at_rest_update (const imuSample &imu, const Vector3f &gyro_bias) |
| 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 ignores them until it is initialised by the first IMU data). | |
| static void | ins_ekf2_airspeed_update (void) |
| Give the last airspeed to the EKF, in the thread of the EKF. | |
| void | ins_ekf2_update (void) |
| void | ins_ekf2_parse_EXTERNAL_POSE (uint8_t *buf) |
| void | ins_ekf2_parse_EXTERNAL_POSE_SMALL (uint8_t *buf) |
Variables | |
| static abi_event | baro_ev |
| static abi_event | temperature_ev |
| static abi_event | agl_ev |
| static abi_event | gyro_int_ev |
| static abi_event | accel_int_ev |
| static abi_event | mag_ev |
| static abi_event | gps_ev |
| static abi_event | relpos_ev |
| static abi_event | optical_flow_ev |
| static abi_event | airspeed_ev |
| static abi_event | reset_ev |
| static Ekf | ekf |
| EKF class itself. | |
| static parameters * | ekf_params |
| The EKF parameters. | |
| static GnssChecks | gnss_checks |
| GNSS checks before the GNSS data is used. | |
| struct ekf2_t | ekf2 |
| Local EKF2 status structure. | |
| static struct extVisionSample | sample_ev |
| External vision sample. | |
| bool | ins_ekf2_events_all = INS_EKF2_EVENTS_ALL |
| struct { | |
| Vector3f | gyro_avg |
| 2 s average of the gyro (rad/s) | |
| Vector3f | accel_avg |
| 2 s average of the accelerometer (m/s2) | |
| float | gyro_metric |
| Low-passed deviation of the gyro from its average (rad/s). | |
| float | accel_metric |
| Low-passed deviation of the accelerometer from its average (m/s2). | |
| float | still_time |
| Time below the thresholds (s). | |
| bool | init |
| volatile bool | still |
| Below the thresholds for INS_EKF2_AT_REST_TIME. | |
| bool | at_rest |
| Last at rest flag given to the EKF. | |
| } | ekf2_rest |
| At rest detection from the IMU samples given to the EKF. | |
| struct { | |
| float | eas |
| Equivalent airspeed (m/s). | |
| uint32_t | stamp |
| Time of the measurement (us). | |
| volatile bool | new_data |
| Set last by the callback, cleared once given to the EKF. | |
| } | ekf2_airspeed |
| Airspeed (equivalent airspeed) to the EKF as true airspeed. | |
INS based in the EKF2 of PX4.
The EKF2 and the PX4 libraries it needs are copied in sw/ext/px4_ekf2 (see sync_px4_ekf2.py there). The GNSS checks, which PX4 runs in its sensors module, are done here before the GNSS data is given to the EKF.
Definition in file ins_ekf2.cpp.
| #define EKF2_EV_CTRL_HPOS 1 |
Values of INS_EKF2_EV_CTRL (bitmask).
horizontal position
Definition at line 116 of file ins_ekf2.cpp.
| #define EKF2_EV_CTRL_VEL 4 |
3D velocity
Definition at line 118 of file ins_ekf2.cpp.
| #define EKF2_EV_CTRL_VPOS 2 |
vertical position
Definition at line 117 of file ins_ekf2.cpp.
| #define EKF2_EV_CTRL_YAW 8 |
yaw
Definition at line 119 of file ins_ekf2.cpp.
| #define EKF2_GPS_CTRL_HPOS 1 |
Values of INS_EKF2_GPS_CTRL (bitmask).
horizontal position
Definition at line 110 of file ins_ekf2.cpp.
| #define EKF2_GPS_CTRL_VEL 4 |
| #define EKF2_GPS_CTRL_VPOS 2 |
vertical position
Definition at line 111 of file ins_ekf2.cpp.
| #define EKF2_GPS_CTRL_YAW 8 |
dual antenna heading
Definition at line 113 of file ins_ekf2.cpp.
| #define EKF2_HGT_REF_BARO 0 |
Values of INS_EKF2_HGT_REF.
Definition at line 122 of file ins_ekf2.cpp.
| #define EKF2_HGT_REF_EV 3 |
Definition at line 125 of file ins_ekf2.cpp.
| #define EKF2_HGT_REF_GNSS 1 |
Definition at line 123 of file ins_ekf2.cpp.
| #define EKF2_HGT_REF_RANGE 2 |
Definition at line 124 of file ins_ekf2.cpp.
| #define ekf2_log | ( | ... | ) |
Definition at line 552 of file ins_ekf2.cpp.
Referenced by agl_cb(), baro_cb(), gps_cb(), ins_ekf2_airspeed_update(), ins_ekf2_parse_EXTERNAL_POSE(), ins_ekf2_publish_attitude(), ins_ekf2_update(), ins_ekf2_update_system_flags(), mag_cb(), optical_flow_cb(), and relpos_cb().
| #define ekf2_log_check_start | ( | ) |
| #define ekf2_log_origin_set | ( | ... | ) |
Definition at line 554 of file ins_ekf2.cpp.
Referenced by ins_ekf2_init(), ins_ekf2_update(), reset_ref(), and reset_vertical_ref().
| #define ekf2_log_output | ( | ... | ) |
| #define ekf2_log_param_changed | ( | ... | ) |
| #define EKF2_MAG_TYPE_AUTO 0 |
Values of INS_EKF2_MAG_TYPE.
heading on the ground, 3D in the air
Definition at line 128 of file ins_ekf2.cpp.
| #define EKF2_MAG_TYPE_HEADING 1 |
heading only
Definition at line 129 of file ins_ekf2.cpp.
| #define EKF2_MAG_TYPE_INIT 6 |
only to initialize the heading
Definition at line 131 of file ins_ekf2.cpp.
| #define EKF2_MAG_TYPE_NONE 5 |
not used
Definition at line 130 of file ins_ekf2.cpp.
| #define GNSS_CHECK_EPH (1<<2) |
horizontal position accuracy (INS_EKF2_GNSS_REQ_EPH)
Definition at line 92 of file ins_ekf2.cpp.
| #define GNSS_CHECK_EPV (1<<3) |
vertical position accuracy (INS_EKF2_GNSS_REQ_EPV)
Definition at line 93 of file ins_ekf2.cpp.
| #define GNSS_CHECK_FIX (1<<10) |
fix type (INS_EKF2_GNSS_REQ_FIX)
Definition at line 100 of file ins_ekf2.cpp.
| #define GNSS_CHECK_HDRIFT (1<<5) |
horizontal position drift at rest on the ground (INS_EKF2_GNSS_REQ_HDRIFT)
Definition at line 95 of file ins_ekf2.cpp.
| #define GNSS_CHECK_HSPEED (1<<7) |
horizontal speed at rest on the ground (INS_EKF2_GNSS_REQ_HDRIFT)
Definition at line 97 of file ins_ekf2.cpp.
| #define GNSS_CHECK_JAMMED (1<<11) |
receiver reports jamming
Definition at line 101 of file ins_ekf2.cpp.
| #define GNSS_CHECK_NSATS (1<<0) |
GNSS checks (INS_EKF2_GNSS_CHECK), bits of the PX4 GNSS_CHECK parameter.
number of satellites (INS_EKF2_GNSS_REQ_NSATS)
Definition at line 90 of file ins_ekf2.cpp.
| #define GNSS_CHECK_PDOP (1<<1) |
position dilution of precision (INS_EKF2_GNSS_REQ_PDOP)
Definition at line 91 of file ins_ekf2.cpp.
| #define GNSS_CHECK_SACC (1<<4) |
speed accuracy (INS_EKF2_GNSS_REQ_SACC)
Definition at line 94 of file ins_ekf2.cpp.
| #define GNSS_CHECK_SPOOFED (1<<9) |
receiver reports spoofing
Definition at line 99 of file ins_ekf2.cpp.
| #define GNSS_CHECK_VDRIFT (1<<6) |
vertical position drift at rest on the ground (INS_EKF2_GNSS_REQ_VDRIFT)
Definition at line 96 of file ins_ekf2.cpp.
| #define GNSS_CHECK_VSPEED (1<<8) |
vertical speed at rest on the ground (INS_EKF2_GNSS_REQ_VDRIFT)
Definition at line 98 of file ins_ekf2.cpp.
| #define INS_EKF2_ACCEL_ID ABI_BROADCAST |
Definition at line 292 of file ins_ekf2.cpp.
Referenced by ins_ekf2_init().
| #define INS_EKF2_AGL_ID ABI_BROADCAST |
default AGL sensor to use in INS
Definition at line 280 of file ins_ekf2.cpp.
Referenced by ins_ekf2_init().
| #define INS_EKF2_AIRSPEED_ID AIR_DATA_SENDER_ID |
default airspeed sensor to use in INS (equivalent airspeed, only fused above INS_EKF2_ARSP_THR): any sensor, or the airspeed computed by air_data from a differential pressure sensor.
The airspeed estimated from the wind (wind_estimation_quadrotor) is never used.
Definition at line 262 of file ins_ekf2.cpp.
Referenced by ins_ekf2_init().
| #define INS_EKF2_AT_REST_ACCEL 0.1f |
accelerometer threshold (m/s2), noise at rest about 0.04
Definition at line 194 of file ins_ekf2.cpp.
Referenced by ins_ekf2_at_rest_update().
| #define INS_EKF2_AT_REST_DETECTION TRUE |
At rest detection, as the PX4 land detector (at rest when landed and not moving for 1 s): the EKF then runs the GNSS drift checks on the ground, fuses zero velocity without other velocity aiding (so a drifting GNSS, e.g.
indoors, doesn't tilt it) and fuses the gyro as its bias. At rest when disarmed, on the ground, the gyro and accelerometer deviation from their 2 s average (low-passed over 0.5 s) stay below the thresholds and the average rotation rate (without the gyro bias) below INS_EKF2_AT_REST_RATE, for INS_EKF2_AT_REST_TIME. Stricter than the PX4 vibration metric, which doesn't see the vehicle being turned by hand (and would then fuse that rotation as gyro bias). Disable it (FALSE) on a moving platform (e.g. a ship or car deck), where the IMU can look still while moving
Definition at line 188 of file ins_ekf2.cpp.
Referenced by ins_ekf2_update_system_flags().
| #define INS_EKF2_AT_REST_GYRO 0.3f |
gyro threshold (deg/s), noise at rest about 0.15
Definition at line 191 of file ins_ekf2.cpp.
Referenced by ins_ekf2_at_rest_update().
| #define INS_EKF2_AT_REST_RATE 1.f |
average rotation rate threshold (deg/s), above the gyro bias before it is estimated
Definition at line 197 of file ins_ekf2.cpp.
Referenced by ins_ekf2_at_rest_update().
| #define INS_EKF2_AT_REST_TIME 1.f |
time below the thresholds before at rest (s)
Definition at line 200 of file ins_ekf2.cpp.
Referenced by ins_ekf2_at_rest_update().
| #define INS_EKF2_BARO_ID AIR_DATA_SENDER_ID |
default barometer to use in INS
Definition at line 268 of file ins_ekf2.cpp.
Referenced by ins_ekf2_init().
| #define ins_ekf2_check_events | ( | ) |
| #define INS_EKF2_EVENT_HOLDOFF 5.f |
Minimum time between two messages about the same status, so a flickering status can't flood the link [s].
Definition at line 827 of file ins_ekf2.cpp.
| #define INS_EKF2_EVENT_MSGS TRUE |
Definition at line 815 of file ins_ekf2.cpp.
| #define INS_EKF2_EVENTS_ALL FALSE |
Send all the EKF events to the GCS, not only the important ones (alignment, dead reckoning, faults).
All events are always written to the flight recorder. Can be changed from the GCS.
Definition at line 821 of file ins_ekf2.cpp.
| #define INS_EKF2_GNSS_CHECK (GNSS_CHECK_NSATS | GNSS_CHECK_EPH | GNSS_CHECK_SACC | GNSS_CHECK_FIX) |
GNSS checks used, PX4 enables all except jamming (2047).
Definition at line 105 of file ins_ekf2.cpp.
Referenced by ins_ekf2_init().
| #define INS_EKF2_GNSS_REQ_EPH 5.0f |
Indoor configuration with only external vision (e.g.
Optitrack): position, velocity, yaw and height from external vision, no GNSS, magnetometer and barometer, and the external vision noise and delay of the previous EKF2 version. Each value can still be changed in the airframe. GNSS accuracy thresholds of the previous version (PX4: EPH 3 m, speed accuracy 0.5 m/s), reached by the internal receivers of e.g. the Bebop2
Definition at line 166 of file ins_ekf2.cpp.
Referenced by ins_ekf2_init().
| #define INS_EKF2_GNSS_REQ_SACC 1.0f |
Definition at line 169 of file ins_ekf2.cpp.
Referenced by ins_ekf2_init().
| #define INS_EKF2_GPS_DELAY 110 |
GNSS measurement delay (ms), as the PX4 SENS_GNSS0_DELAY parameter.
Definition at line 236 of file ins_ekf2.cpp.
Referenced by gps_cb(), ins_ekf2_init(), and relpos_cb().
| #define INS_EKF2_GPS_ID GPS_MULTI_ID |
Definition at line 304 of file ins_ekf2.cpp.
Referenced by ins_ekf2_init().
| #define INS_EKF2_GPS_POS_X 0 |
Definition at line 322 of file ins_ekf2.cpp.
Referenced by gps_cb().
| #define INS_EKF2_GPS_POS_Y 0 |
Definition at line 327 of file ins_ekf2.cpp.
Referenced by gps_cb().
| #define INS_EKF2_GPS_POS_Z 0 |
Definition at line 332 of file ins_ekf2.cpp.
Referenced by gps_cb().
| #define INS_EKF2_GPS_YAW_NOISE 0.1f |
Definition at line 338 of file ins_ekf2.cpp.
| #define INS_EKF2_GYRO_ID ABI_BROADCAST |
Definition at line 286 of file ins_ekf2.cpp.
Referenced by ins_ekf2_init().
| #define INS_EKF2_HGT_REF EKF2_HGT_REF_BARO |
Barometer as height reference (PX4 uses GNSS), as the previous version: the heights don't depend on the ground altitude of the flight plan.
Definition at line 175 of file ins_ekf2.cpp.
| #define INS_EKF2_MAG_ID ABI_BROADCAST |
Definition at line 298 of file ins_ekf2.cpp.
Referenced by ins_ekf2_init().
| #define INS_EKF2_OF_ID ABI_BROADCAST |
Definition at line 316 of file ins_ekf2.cpp.
Referenced by ins_ekf2_init().
| #define INS_EKF2_OF_MAX_RATE 200.f |
Maximum flow rate the optical flow sensor can measure (rad/s).
Definition at line 254 of file ins_ekf2.cpp.
Referenced by ins_ekf2_init().
| #define ins_ekf2_preflight_filter | ( | ) |
| #define INS_EKF2_RELPOS_ID ABI_BROADCAST |
Definition at line 310 of file ins_ekf2.cpp.
Referenced by ins_ekf2_init().
| #define INS_EKF2_RNG_FOG 1.f |
Definition at line 219 of file ins_ekf2.cpp.
| #define INS_EKF2_RNG_MAX_DIST 4.f |
Maximum distance the range finder can measure (m).
Definition at line 248 of file ins_ekf2.cpp.
Referenced by ins_ekf2_init().
| #define INS_EKF2_RNG_MIN_DIST 0.001f |
Minimum distance the range finder can measure (m).
Definition at line 242 of file ins_ekf2.cpp.
Referenced by ins_ekf2_init().
| #define INS_EKF2_TEMPERATURE_ID AIR_DATA_SENDER_ID |
default temperature sensor to use in INS
Definition at line 274 of file ins_ekf2.cpp.
Referenced by ins_ekf2_init().
| #define USE_INS_NAV_INIT TRUE |
For SITL and NPS we need special includes.
INS reference from flight plan, true by default
Definition at line 77 of file ins_ekf2.cpp.
| enum ekf2_input_type |
Definition at line 420 of file ins_ekf2.cpp.
|
static |
Definition at line 1739 of file ins_ekf2.cpp.
References ekf2, ekf2_time_us(), ins_ekf2_publish_attitude(), and VECT3_COPY.
Referenced by ins_ekf2_init().
Definition at line 1706 of file ins_ekf2.cpp.
References ekf, EKF2_IN_RANGE, ekf2_input_enabled(), ekf2_log, and ekf2_time_us().
Referenced by agl_dist_init(), ins_ekf2_init(), ins_int_init(), and opticflow_pmw3901_init().
|
static |
Definition at line 1399 of file ins_ekf2.cpp.
References AIRSPEED_WE_QUAD_ID, ekf2_airspeed, and get_sys_time_usec().
Referenced by ins_ekf2_init(), and ins_mekf_wind_wrapper_init().
Definition at line 1678 of file ins_ekf2.cpp.
References ekf, ekf2, EKF2_IN_BARO, ekf2_input_enabled(), ekf2_log, ekf2_time_us(), pprz_isa_density_of_pressure(), pprz_isa_height_of_pressure_full(), and PPRZ_ISA_SEA_LEVEL_PRESSURE.
Time of a measurement that was taken delay_ms before it arrived.
Definition at line 407 of file ins_ekf2.cpp.
References ekf2_time_us().
Referenced by gps_cb(), optical_flow_cb(), and relpos_cb().
|
inlinestatic |
Definition at line 557 of file ins_ekf2.cpp.
Referenced by agl_cb(), baro_cb(), gps_cb(), ins_ekf2_airspeed_update(), ins_ekf2_parse_EXTERNAL_POSE(), ins_ekf2_publish_attitude(), ins_ekf2_update(), mag_cb(), optical_flow_cb(), and relpos_cb().
Convert a 32 bit timestamp (us) of an ABI message to the 64 bit time of the EKF, so the EKF time keeps increasing when the 32 bit time wraps (after 71 minutes).
Only a step back of more than half the range is a wrap: a time read just before a later one stored by another caller is a little older, and taking it for a wrap would put the EKF time 71 minutes ahead.
Definition at line 388 of file ins_ekf2.cpp.
References get_sys_time_usec().
Referenced by accel_int_cb(), agl_cb(), baro_cb(), ekf2_delayed_time_us(), gps_cb(), gyro_int_cb(), ins_ekf2_airspeed_update(), ins_ekf2_init(), ins_ekf2_parse_EXTERNAL_POSE(), ins_ekf2_update(), ins_ekf2_update_system_flags(), mag_cb(), reset_ref(), and reset_vertical_ref().
Definition at line 1788 of file ins_ekf2.cpp.
References ekf, ekf2, ekf2_delayed_time_us(), EKF2_GPS_CTRL_VEL, EKF2_IN_GNSS, ekf2_input_enabled(), ekf2_log, ekf2_log_param_changed, ekf2_time_us(), ekf_params, GpsState::fix, gnss_checks, GPS_VALID_VEL_ECEF_BIT, GPS_VALID_VEL_NED_BIT, GpsState::hacc, GpsState::hmsl, INS_EKF2_GPS_DELAY, INS_EKF2_GPS_POS_X, INS_EKF2_GPS_POS_Y, INS_EKF2_GPS_POS_Z, LlaCoor_i::lat, lla_int_from_gps(), LlaCoor_i::lon, ned_vel_float_from_gps(), GpsState::num_sv, GpsState::pdop, GpsState::sacc, GpsState::vacc, GpsState::valid_fields, NedCoor_f::x, NedCoor_f::y, and NedCoor_f::z.
|
static |
Definition at line 1724 of file ins_ekf2.cpp.
References ekf2, ekf2_time_us(), ins_ekf2_publish_attitude(), and RATES_COPY.
Referenced by ins_ekf2_init().
|
static |
Give the last airspeed to the EKF, in the thread of the EKF.
Definition at line 1410 of file ins_ekf2.cpp.
References ekf, ekf2, ekf2_airspeed, EKF2_IN_AIRSPEED, ekf2_input_enabled(), ekf2_log, ekf2_time_us(), and PPRZ_ISA_AIR_DENSITY.
Referenced by ins_ekf2_update().
|
static |
Definition at line 1319 of file ins_ekf2.cpp.
References alpha, ekf2_rest, imu, INS_EKF2_AT_REST_ACCEL, INS_EKF2_AT_REST_GYRO, INS_EKF2_AT_REST_RATE, and INS_EKF2_AT_REST_TIME.
Referenced by ins_ekf2_publish_attitude().
| void ins_ekf2_init | ( | void | ) |
Definition at line 1127 of file ins_ekf2.cpp.
References ABI_BROADCAST, accel_int_cb(), accel_int_ev, agl_cb(), agl_ev, airspeed_cb(), airspeed_ev, LlaCoor_i::alt, baro_cb(), baro_ev, DefaultPeriodic, ekf, ekf2, ekf2_log_origin_set, ekf2_time_us(), ekf_params, FLOAT_VECT3_ZERO, get_sys_time_usec(), gnss_checks, gps_cb(), gps_ev, gyro_int_cb(), gyro_int_ev, INS_EKF2_ACCEL_ID, INS_EKF2_AGL_ID, INS_EKF2_AIRSPEED_ID, INS_EKF2_BARO_ID, INS_EKF2_GNSS_CHECK, INS_EKF2_GNSS_REQ_EPH, INS_EKF2_GNSS_REQ_SACC, INS_EKF2_GPS_DELAY, INS_EKF2_GPS_ID, INS_EKF2_GYRO_ID, INS_EKF2_MAG_ID, INS_EKF2_OF_ID, INS_EKF2_OF_MAX_RATE, INS_EKF2_RELPOS_ID, INS_EKF2_RNG_MAX_DIST, INS_EKF2_RNG_MIN_DIST, INS_EKF2_TEMPERATURE_ID, LlaCoor_i::lat, LlaCoor_i::lon, ltp_def_from_lla_i(), mag_cb(), mag_ev, optical_flow_cb(), optical_flow_ev, PPRZ_ISA_AIR_DENSITY, preflight_check_register(), register_periodic_telemetry(), relpos_cb(), relpos_ev, reset_cb(), reset_ev, sample_ev, send_ahrs_bias(), send_ahrs_quat(), send_external_pose_down(), send_filter_status(), send_ins(), send_ins_ekf2(), send_ins_ekf2_ext(), send_ins_ref(), send_ins_z(), send_wind_info_ret(), stateSetLocalOrigin_i(), temperature_cb(), temperature_ev, and waypoints_localize_all().
| void ins_ekf2_parse_EXTERNAL_POSE | ( | uint8_t * | buf | ) |
Definition at line 1543 of file ins_ekf2.cpp.
References ekf, EKF2_IN_EV, ekf2_input_enabled(), ekf2_log, ekf2_time_us(), float_quat_comp(), get_sys_time_usec(), FloatQuat::qi, FloatQuat::qx, FloatQuat::qy, FloatQuat::qz, and sample_ev.
| void ins_ekf2_parse_EXTERNAL_POSE_SMALL | ( | uint8_t * | buf | ) |
Definition at line 1593 of file ins_ekf2.cpp.
|
static |
Publish the attitude and get the new state Directly called after a succeslfull gyro+accel reading.
Definition at line 1601 of file ins_ekf2.cpp.
References ACCEL_BFP_OF_REAL, ekf, ekf2, EKF2_IN_IMU, ekf2_input_enabled(), ekf2_log, guidance_h, ins_ekf2_at_rest_update(), nav, FloatRates::p, FloatRates::q, FloatQuat::qi, FloatQuat::qx, FloatQuat::qy, FloatQuat::qz, FloatRates::r, stabilization_attitude_enter(), stateSetAccelBody_i(), stateSetBodyRates_f(), stateSetNedToBodyQuat_f(), Int32Vect3::x, Int32Vect3::y, and Int32Vect3::z.
Referenced by accel_int_cb(), and gyro_int_cb().
| void ins_ekf2_update | ( | void | ) |
Definition at line 1436 of file ins_ekf2.cpp.
References LlaCoor_i::alt, ekf, ekf2, EKF2_IN_UPDATE, ekf2_input_enabled(), ekf2_log, ekf2_log_check_start, ekf2_log_origin_set, ekf2_log_output, ekf2_time_us(), get_sys_time_usec(), ins_ekf2_airspeed_update(), ins_ekf2_check_events, ins_ekf2_preflight_filter, ins_ekf2_update_system_flags(), LlaCoor_i::lat, LlaCoor_i::lon, ltp_def_from_lla_i(), nps_bypass_ins, sim_overwrite_ins(), stateSetAccelNed_f(), stateSetLocalOrigin_i(), stateSetPositionNed_f(), stateSetSpeedNed_f(), waypoints_localize_all(), wgs84_ellipsoid_to_geoid_i(), NedCoor_f::x, NedCoor_f::y, and NedCoor_f::z.
|
static |
Give the arming, in-air and at rest state to the EKF when it changes, and every second (the EKF ignores them until it is initialised by the first IMU data).
Definition at line 1354 of file ins_ekf2.cpp.
References autopilot_get_motors_on(), autopilot_in_flight(), ekf, ekf2, EKF2_IN_FLAGS, ekf2_log, ekf2_rest, ekf2_time_us(), get_sys_time_usec(), INS_EKF2_AT_REST_DETECTION, and stateIsFixedWingFlight().
Referenced by ins_ekf2_update().
|
static |
Definition at line 1754 of file ins_ekf2.cpp.
References ekf, ekf2, EKF2_IN_MAG, ekf2_input_enabled(), ekf2_log, ekf2_time_us(), imu_get_mag(), MAGS_FLOAT_OF_BFP, scale, imu_mag_t::strength, FloatVect3::x, FloatVect3::y, and FloatVect3::z.
|
static |
Definition at line 1892 of file ins_ekf2.cpp.
References ekf, ekf2, ekf2_delayed_time_us(), EKF2_IN_FLOW, ekf2_input_enabled(), and ekf2_log.
Referenced by ins_ekf2_init().
Definition at line 1857 of file ins_ekf2.cpp.
References ekf, ekf2_delayed_time_us(), EKF2_IN_GNSS_YAW, ekf2_input_enabled(), ekf2_log, RelPosNED::heading, RelPosNED::heading_acc, INS_EKF2_GPS_DELAY, and RelPosNED::reference_id.
Referenced by ins_ekf2_init().
Definition at line 1292 of file ins_ekf2.cpp.
References INS_RESET_REF, INS_RESET_VERTICAL_REF, reset_ref(), reset_vertical_ref(), and UNUSED.
|
static |
Definition at line 1257 of file ins_ekf2.cpp.
References ekf, ekf2, ekf2_log_origin_set, ekf2_time_us(), get_sys_time_usec(), gps, GpsFixValid, LlaCoor_i::lat, lla_int_from_gps(), LlaCoor_i::lon, ltp_def_from_lla_i(), and stateSetLocalOrigin_i().
Referenced by reset_cb().
|
static |
Definition at line 1272 of file ins_ekf2.cpp.
References LlaCoor_i::alt, ekf, ekf2, ekf2_log_origin_set, ekf2_time_us(), get_sys_time_usec(), gps, GpsFixValid, LlaCoor_i::lat, lla_int_from_gps(), LlaCoor_i::lon, ltp_def_from_lla_i(), stateGetLlaOrigin_i(), and stateSetLocalOrigin_i().
Referenced by reset_cb().
|
static |
Definition at line 744 of file ins_ekf2.cpp.
Referenced by ins_ekf2_init(), and ins_ext_pose_init().
|
static |
Definition at line 754 of file ins_ekf2.cpp.
References dev, ekf, foo, Int32Quat::qi, QUAT1_BFP_OF_REAL, Int32Quat::qx, Int32Quat::qy, Int32Quat::qz, and stateGetNedToBodyQuat_i().
Referenced by ins_ekf2_init().
|
static |
Definition at line 779 of file ins_ekf2.cpp.
References dev, and sample_ev.
Referenced by ins_ekf2_init(), and ins_ext_pose_init().
|
static |
Definition at line 707 of file ins_ekf2.cpp.
References AHRS_COMP_ID_EKF2, dev, and ekf.
|
static |
Definition at line 563 of file ins_ekf2.cpp.
References ACCEL_BFP_OF_REAL, dev, ekf, ekf2, POS_BFP_OF_REAL, SPEED_BFP_OF_REAL, NedCoor_i::x, NedCoor_i::y, and NedCoor_i::z.
Referenced by ins_ekf2_init(), ins_ext_pose_init(), ins_flow_init(), ins_gps_passthrough_init(), ins_int_init(), and ins_vectornav_init().
|
static |
Definition at line 624 of file ins_ekf2.cpp.
References dev, ekf, gnss_checks, and test_ratio().
Referenced by ins_ekf2_init().
|
static |
Definition at line 662 of file ins_ekf2.cpp.
References dev, ekf, and gnss_checks.
Referenced by ins_ekf2_init().
|
static |
Definition at line 608 of file ins_ekf2.cpp.
Referenced by ins_ekf2_init(), ins_ext_pose_init(), ins_float_invariant_wrapper_init(), ins_flow_init(), ins_gps_passthrough_init(), ins_int_init(), and ins_vectornav_init().
|
static |
Definition at line 589 of file ins_ekf2.cpp.
References ACCEL_BFP_OF_REAL, dev, ekf, ekf2, POS_BFP_OF_REAL, pos_z, and SPEED_BFP_OF_REAL.
Referenced by ins_ekf2_init(), ins_ext_pose_init(), ins_gps_passthrough_init(), ins_int_init(), and ins_vectornav_init().
|
static |
Definition at line 732 of file ins_ekf2.cpp.
Referenced by ins_ekf2_init().
|
static |
Definition at line 1700 of file ins_ekf2.cpp.
References ekf2.
Referenced by ins_ekf2_init().
|
static |
Innovation test ratio, 0 when not available.
Definition at line 619 of file ins_ekf2.cpp.
Referenced by send_ins_ekf2().
|
static |
Definition at line 351 of file ins_ekf2.cpp.
Referenced by ins_ekf2_init().
|
static |
Definition at line 349 of file ins_ekf2.cpp.
|
static |
Definition at line 356 of file ins_ekf2.cpp.
Referenced by ins_ekf2_init(), and ins_mekf_wind_wrapper_init().
|
static |
Definition at line 347 of file ins_ekf2.cpp.
|
static |
EKF class itself.
Definition at line 376 of file ins_ekf2.cpp.
Referenced by agl_cb(), baro_cb(), gps_cb(), ins_ekf2_airspeed_update(), ins_ekf2_init(), ins_ekf2_parse_EXTERNAL_POSE(), ins_ekf2_publish_attitude(), ins_ekf2_update(), ins_ekf2_update_system_flags(), mag_cb(), optical_flow_cb(), relpos_cb(), reset_ref(), reset_vertical_ref(), send_ahrs_bias(), send_ahrs_quat(), send_filter_status(), send_ins(), send_ins_ekf2(), send_ins_ekf2_ext(), send_ins_z(), and send_wind_info_ret().
| struct ekf2_t ekf2 |
Local EKF2 status structure.
Definition at line 379 of file ins_ekf2.cpp.
Referenced by accel_int_cb(), baro_cb(), gps_cb(), gyro_int_cb(), ins_ekf2_airspeed_update(), ins_ekf2_init(), ins_ekf2_publish_attitude(), ins_ekf2_update(), ins_ekf2_update_system_flags(), mag_cb(), optical_flow_cb(), reset_ref(), reset_vertical_ref(), send_ins(), send_ins_ref(), send_ins_z(), and temperature_cb().
| struct { ... } ekf2_airspeed |
Airspeed (equivalent airspeed) to the EKF as true airspeed.
The airspeed can come from another thread (e.g. a UAVCAN sensor through air_data, in the CAN receive thread): only keep it here, it is given to the EKF from ins_ekf2_update(), as the other samples.
Referenced by airspeed_cb(), and ins_ekf2_airspeed_update().
| struct { ... } ekf2_rest |
At rest detection from the IMU samples given to the EKF.
Referenced by ins_ekf2_at_rest_update(), and ins_ekf2_update_system_flags().
|
static |
The EKF parameters.
Definition at line 377 of file ins_ekf2.cpp.
Referenced by ekf_aw_wrapper_init(), ekf_aw_wrapper_periodic(), gps_cb(), and ins_ekf2_init().
|
static |
GNSS checks before the GNSS data is used.
Definition at line 378 of file ins_ekf2.cpp.
Referenced by gps_cb(), ins_ekf2_init(), send_ins_ekf2(), and send_ins_ekf2_ext().
|
static |
Definition at line 353 of file ins_ekf2.cpp.
|
static |
Definition at line 350 of file ins_ekf2.cpp.
Referenced by ins_ekf2_init().
| bool ins_ekf2_events_all = INS_EKF2_EVENTS_ALL |
Definition at line 823 of file ins_ekf2.cpp.
|
static |
Definition at line 352 of file ins_ekf2.cpp.
|
static |
Definition at line 355 of file ins_ekf2.cpp.
|
static |
Definition at line 354 of file ins_ekf2.cpp.
Referenced by ins_ekf2_init().
|
static |
Definition at line 357 of file ins_ekf2.cpp.
|
static |
External vision sample.
Definition at line 380 of file ins_ekf2.cpp.
Referenced by ins_ekf2_init(), ins_ekf2_parse_EXTERNAL_POSE(), and send_external_pose_down().
|
static |
Definition at line 348 of file ins_ekf2.cpp.
Referenced by ins_ekf2_init().