![]() |
Paparazzi UAS
v6.3_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 "stabilization/stabilization_attitude.h"
#include "generated/airframe.h"
#include "generated/flight_plan.h"
#include "EKF/ekf.h"
#include "math/pprz_isa.h"
#include "math/pprz_geodetic_wgs84.h"
#include "mcu_periph/sys_time.h"
#include "autopilot.h"
#include "modules/datalink/telemetry.h"
Go to the source code of this file.
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 | 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 | ins_ekf2_publish_attitude (uint32_t stamp) |
Publish the attitude and get the new state Directly called after a succeslfull gyro+accel reading. More... | |
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 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) |
void | ins_ekf2_init (void) |
void | ins_ekf2_update (void) |
void | ins_ekf2_change_param (int32_t unk) |
void | ins_ekf2_remove_gps (int32_t mode) |
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 | optical_flow_ev |
static Ekf | ekf |
EKF class itself. More... | |
static parameters * | ekf_params |
The EKF parameters. More... | |
struct ekf2_t | ekf2 |
Local EKF2 status structure. More... | |
INS based in the EKF2 of PX4.
Definition in file ins_ekf2.cpp.
#define INS_EKF2_ACCEL_ID ABI_BROADCAST |
Definition at line 128 of file ins_ekf2.cpp.
#define INS_EKF2_AGL_ID ABI_BROADCAST |
default AGL sensor to use in INS
Definition at line 116 of file ins_ekf2.cpp.
#define INS_EKF2_BARO_ID ABI_BROADCAST |
default barometer to use in INS
Definition at line 103 of file ins_ekf2.cpp.
#define INS_EKF2_BARO_NOISE 3.5f |
Definition at line 272 of file ins_ekf2.cpp.
#define INS_EKF2_EVA_NOISE 0.05f |
Definition at line 254 of file ins_ekf2.cpp.
#define INS_EKF2_EVP_NOISE 0.02f |
Definition at line 242 of file ins_ekf2.cpp.
#define INS_EKF2_EVV_NOISE 0.1f |
Definition at line 248 of file ins_ekf2.cpp.
#define INS_EKF2_FLOW_INNOV_GATE 4 |
Definition at line 236 of file ins_ekf2.cpp.
#define INS_EKF2_FLOW_NOISE 0.03 |
Definition at line 224 of file ins_ekf2.cpp.
#define INS_EKF2_FLOW_NOISE_QMIN 0.05 |
Definition at line 230 of file ins_ekf2.cpp.
#define INS_EKF2_FLOW_POS_X 0 |
Definition at line 206 of file ins_ekf2.cpp.
#define INS_EKF2_FLOW_POS_Y 0 |
Definition at line 212 of file ins_ekf2.cpp.
#define INS_EKF2_FLOW_POS_Z 0 |
Definition at line 218 of file ins_ekf2.cpp.
#define INS_EKF2_FLOW_SENSOR_DELAY 15 |
Definition at line 188 of file ins_ekf2.cpp.
#define INS_EKF2_FUSION_MODE (MASK_USE_GPS) |
Special configuration for Optitrack.
The EKF2 fusion mode setting
Definition at line 64 of file ins_ekf2.cpp.
#define INS_EKF2_GPS_CHECK_MASK 21 |
The EKF2 GPS checks before initialization.
Definition at line 76 of file ins_ekf2.cpp.
#define INS_EKF2_GPS_ID GPS_MULTI_ID |
Definition at line 140 of file ins_ekf2.cpp.
#define INS_EKF2_GPS_P_NOISE 0.5f |
Definition at line 266 of file ins_ekf2.cpp.
#define INS_EKF2_GPS_POS_X 0 |
Definition at line 170 of file ins_ekf2.cpp.
#define INS_EKF2_GPS_POS_Y 0 |
Definition at line 176 of file ins_ekf2.cpp.
#define INS_EKF2_GPS_POS_Z 0 |
Definition at line 182 of file ins_ekf2.cpp.
#define INS_EKF2_GPS_V_NOISE 0.3f |
Definition at line 260 of file ins_ekf2.cpp.
#define INS_EKF2_GYRO_ID ABI_BROADCAST |
Definition at line 122 of file ins_ekf2.cpp.
#define INS_EKF2_IMU_POS_X 0 |
Definition at line 152 of file ins_ekf2.cpp.
#define INS_EKF2_IMU_POS_Y 0 |
Definition at line 158 of file ins_ekf2.cpp.
#define INS_EKF2_IMU_POS_Z 0 |
Definition at line 164 of file ins_ekf2.cpp.
#define INS_EKF2_MAG_ID ABI_BROADCAST |
Definition at line 134 of file ins_ekf2.cpp.
#define INS_EKF2_MAX_FLOW_RATE 200 |
Definition at line 200 of file ins_ekf2.cpp.
#define INS_EKF2_MIN_FLOW_QUALITY 100 |
Definition at line 194 of file ins_ekf2.cpp.
#define INS_EKF2_OF_ID ABI_BROADCAST |
Definition at line 146 of file ins_ekf2.cpp.
#define INS_EKF2_RANGE_MAIN_AGL 1 |
If enabled uses radar sensor as primary AGL source, if possible.
Definition at line 94 of file ins_ekf2.cpp.
#define INS_EKF2_SONAR_MAX_RANGE 4 |
Default AGL sensor maximum range.
Definition at line 88 of file ins_ekf2.cpp.
#define INS_EKF2_SONAR_MIN_RANGE 0.001 |
Default AGL sensor minimum range.
Definition at line 82 of file ins_ekf2.cpp.
#define INS_EKF2_TEMPERATURE_ID ABI_BROADCAST |
default temperature sensor to use in INS
Definition at line 110 of file ins_ekf2.cpp.
#define INS_EKF2_VDIST_SENSOR_TYPE VDIST_SENSOR_BARO |
The EKF2 primary vertical distance sensor type.
Definition at line 70 of file ins_ekf2.cpp.
#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 49 of file ins_ekf2.cpp.
|
static |
Definition at line 809 of file ins_ekf2.cpp.
References ekf2_t::accel_dt, ekf2_t::accel_valid, ekf2_t::delta_accel, ekf2, ekf2_t::gyro_valid, ins_ekf2_publish_attitude(), and VECT3_COPY.
Referenced by ins_ekf2_init().
Definition at line 783 of file ins_ekf2.cpp.
References ekf.
Referenced by ins_ekf2_init().
Definition at line 762 of file ins_ekf2.cpp.
References ekf, ekf2, pprz_isa_density_of_pressure(), pprz_isa_height_of_pressure_full(), ekf2_t::qnh, and ekf2_t::temp.
Referenced by ins_ekf2_init().
Definition at line 848 of file ins_ekf2.cpp.
References GpsState::course, ekf, GpsState::fix, GPS_VALID_VEL_NED_BIT, GpsState::gspeed, GpsState::hacc, GpsState::hmsl, 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.
Referenced by ins_ekf2_init().
|
static |
Definition at line 794 of file ins_ekf2.cpp.
References ekf2_t::accel_valid, ekf2_t::delta_gyro, ekf2, ekf2_t::gyro_dt, ekf2_t::gyro_valid, ins_ekf2_publish_attitude(), and RATES_COPY.
Referenced by ins_ekf2_init().
void ins_ekf2_change_param | ( | int32_t | unk | ) |
Definition at line 646 of file ins_ekf2.cpp.
References ekf2, ekf_params, and ekf2_t::mag_fusion_type.
void ins_ekf2_init | ( | void | ) |
Definition at line 461 of file ins_ekf2.cpp.
References accel_int_cb(), accel_int_ev, ekf2_t::accel_valid, agl_cb(), agl_ev, LlaCoor_i::alt, baro_cb(), baro_ev, DefaultPeriodic, ekf, ekf2, ekf_params, ekf2_t::flow_stamp, ekf2_t::got_imu_data, gps_cb(), gps_ev, gyro_int_cb(), gyro_int_ev, ekf2_t::gyro_valid, LtpDef_i::hmsl, INS_EKF2_ACCEL_ID, INS_EKF2_AGL_ID, INS_EKF2_BARO_ID, INS_EKF2_BARO_NOISE, INS_EKF2_FLOW_INNOV_GATE, INS_EKF2_FLOW_NOISE, INS_EKF2_FLOW_NOISE_QMIN, INS_EKF2_FLOW_POS_X, INS_EKF2_FLOW_POS_Y, INS_EKF2_FLOW_POS_Z, INS_EKF2_FLOW_SENSOR_DELAY, INS_EKF2_FUSION_MODE, INS_EKF2_GPS_CHECK_MASK, INS_EKF2_GPS_ID, INS_EKF2_GPS_P_NOISE, INS_EKF2_GPS_POS_X, INS_EKF2_GPS_POS_Y, INS_EKF2_GPS_POS_Z, INS_EKF2_GPS_V_NOISE, INS_EKF2_GYRO_ID, INS_EKF2_IMU_POS_X, INS_EKF2_IMU_POS_Y, INS_EKF2_IMU_POS_Z, INS_EKF2_MAG_ID, INS_EKF2_MAX_FLOW_RATE, INS_EKF2_MIN_FLOW_QUALITY, INS_EKF2_OF_ID, INS_EKF2_RANGE_MAIN_AGL, INS_EKF2_SONAR_MAX_RANGE, INS_EKF2_SONAR_MIN_RANGE, INS_EKF2_TEMPERATURE_ID, INS_EKF2_VDIST_SENSOR_TYPE, LlaCoor_i::lat, LlaCoor_i::lon, ekf2_t::ltp_def, ltp_def_from_lla_i(), ekf2_t::ltp_stamp, mag_cb(), mag_ev, optical_flow_cb(), optical_flow_ev, ekf2_t::qnh, ekf2_t::quat_reset_counter, register_periodic_telemetry(), send_ahrs_bias(), send_filter_status(), send_ins(), send_ins_ekf2(), send_ins_ekf2_ext(), send_ins_ref(), send_ins_z(), send_wind_info_ret(), stateSetLocalOrigin_i(), ekf2_t::temp, temperature_cb(), temperature_ev, and waypoints_localize_all().
void ins_ekf2_parse_EXTERNAL_POSE | ( | uint8_t * | buf | ) |
Definition at line 660 of file ins_ekf2.cpp.
References ekf, get_sys_time_usec(), INS_EKF2_EVA_NOISE, INS_EKF2_EVP_NOISE, and INS_EKF2_EVV_NOISE.
void ins_ekf2_parse_EXTERNAL_POSE_SMALL | ( | uint8_t * | buf | ) |
Definition at line 683 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 690 of file ins_ekf2.cpp.
References ACCEL_BFP_OF_REAL, ekf2_t::accel_dt, ekf2_t::accel_valid, ANGLE_BFP_OF_REAL, autopilot_in_flight(), ekf2_t::delta_accel, ekf2_t::delta_gyro, ekf, ekf2, ekf2_t::got_imu_data, guidance_h, guidance_h_read_rc(), ekf2_t::gyro_dt, ekf2_t::gyro_valid, HorizontalGuidanceSetpoint::heading, RotorcraftNavigation::heading, nav, FloatRates::p, FloatEulers::psi, FloatRates::q, FloatQuat::qi, ekf2_t::quat_reset_counter, FloatQuat::qx, FloatQuat::qy, FloatQuat::qz, FloatRates::r, HorizontalGuidance::rc_sp, HorizontalGuidance::sp, stab_att_sp_euler, stabilization_attitude_enter(), stateSetAccelBody_i(), stateSetBodyRates_f(), stateSetNedToBodyQuat_f(), FloatVect3::x, Int32Vect3::x, FloatVect3::y, Int32Vect3::y, FloatVect3::z, and Int32Vect3::z.
Referenced by accel_int_cb(), and gyro_int_cb().
void ins_ekf2_remove_gps | ( | int32_t | mode | ) |
Definition at line 651 of file ins_ekf2.cpp.
References ekf2, ekf_params, ekf2_t::fusion_mode, INS_EKF2_FUSION_MODE, and mode.
void ins_ekf2_update | ( | void | ) |
Definition at line 569 of file ins_ekf2.cpp.
References LlaCoor_i::alt, autopilot_in_flight(), ekf, ekf2, ekf2_t::got_imu_data, LtpDef_i::hmsl, LlaCoor_i::lat, LlaCoor_i::lon, ekf2_t::ltp_def, ltp_def_from_lla_i(), ekf2_t::ltp_stamp, 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 |
Definition at line 824 of file ins_ekf2.cpp.
References ekf, ekf2, ekf2_t::got_imu_data, MAGS_FLOAT_OF_BFP, FloatVect3::x, FloatVect3::y, and FloatVect3::z.
Referenced by ins_ekf2_init().
|
static |
Definition at line 882 of file ins_ekf2.cpp.
References ekf, ekf2, and ekf2_t::flow_stamp.
Referenced by ins_ekf2_init().
|
static |
Definition at line 449 of file ins_ekf2.cpp.
Referenced by ins_ekf2_init().
|
static |
Definition at line 412 of file ins_ekf2.cpp.
References AHRS_COMP_ID_EKF2, dev, and ekf.
Referenced by ins_ekf2_init().
|
static |
Definition at line 308 of file ins_ekf2.cpp.
References ACCEL_BFP_OF_REAL, dev, ekf, POS_BFP_OF_REAL, SPEED_BFP_OF_REAL, NedCoor_i::x, NedCoor_i::y, and NedCoor_i::z.
Referenced by ins_ekf2_init().
|
static |
Definition at line 365 of file ins_ekf2.cpp.
Referenced by ins_ekf2_init().
|
static |
Definition at line 398 of file ins_ekf2.cpp.
Referenced by ins_ekf2_init().
|
static |
Definition at line 355 of file ins_ekf2.cpp.
References LlaCoor_i::alt, dev, LtpDef_i::ecef, ekf2, LtpDef_i::hmsl, LlaCoor_i::lat, LtpDef_i::lla, LlaCoor_i::lon, ekf2_t::ltp_def, ekf2_t::ltp_stamp, EcefCoor_i::x, EcefCoor_i::y, and EcefCoor_i::z.
Referenced by ins_ekf2_init().
|
static |
Definition at line 335 of file ins_ekf2.cpp.
References ACCEL_BFP_OF_REAL, dev, ekf, POS_BFP_OF_REAL, pos_z, and SPEED_BFP_OF_REAL.
Referenced by ins_ekf2_init().
|
static |
Definition at line 437 of file ins_ekf2.cpp.
Referenced by ins_ekf2_init().
|
static |
Definition at line 777 of file ins_ekf2.cpp.
References ekf2, and ekf2_t::temp.
Referenced by ins_ekf2_init().
|
static |
Definition at line 281 of file ins_ekf2.cpp.
Referenced by ins_ekf2_init().
|
static |
Definition at line 279 of file ins_ekf2.cpp.
Referenced by ins_ekf2_init().
|
static |
Definition at line 277 of file ins_ekf2.cpp.
Referenced by ins_ekf2_init().
|
static |
EKF class itself.
Definition at line 301 of file ins_ekf2.cpp.
Referenced by agl_cb(), baro_cb(), gps_cb(), ins_ekf2_init(), ins_ekf2_parse_EXTERNAL_POSE(), ins_ekf2_publish_attitude(), ins_ekf2_update(), mag_cb(), optical_flow_cb(), send_ahrs_bias(), 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 302 of file ins_ekf2.cpp.
Referenced by accel_int_cb(), baro_cb(), gyro_int_cb(), ins_ekf2_change_param(), ins_ekf2_init(), ins_ekf2_publish_attitude(), ins_ekf2_remove_gps(), ins_ekf2_update(), mag_cb(), optical_flow_cb(), send_ins_ref(), and temperature_cb().
|
static |
The EKF parameters.
Definition at line 302 of file ins_ekf2.cpp.
Referenced by ins_ekf2_change_param(), ins_ekf2_init(), and ins_ekf2_remove_gps().
|
static |
Definition at line 283 of file ins_ekf2.cpp.
Referenced by ins_ekf2_init().
|
static |
Definition at line 280 of file ins_ekf2.cpp.
Referenced by ins_ekf2_init().
|
static |
Definition at line 282 of file ins_ekf2.cpp.
Referenced by ins_ekf2_init().
|
static |
Definition at line 284 of file ins_ekf2.cpp.
Referenced by ins_ekf2_init().
|
static |
Definition at line 278 of file ins_ekf2.cpp.
Referenced by ins_ekf2_init().