Paparazzi UAS
v7.0_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) |
static void | send_ahrs_quat (struct transport_tx *trans, struct link_device *dev) |
void | ins_ekf2_init (void) |
void | ins_reset_local_origin (void) |
INS local origin reset. More... | |
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 849 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 823 of file ins_ekf2.cpp.
References ekf.
Referenced by ins_ekf2_init().
Definition at line 802 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 888 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 834 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 686 of file ins_ekf2.cpp.
References ekf2, ekf_params, and ekf2_t::mag_fusion_type.
void ins_ekf2_init | ( | void | ) |
Definition at line 486 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_ahrs_quat(), 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 700 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 723 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 730 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 691 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 609 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.
void ins_reset_local_origin | ( | void | ) |
INS local origin reset.
Reset horizontal and vertical reference to the current position. Does nothing if not implemented by specific INS algorithm.
INS local origin reset.
Definition at line 594 of file ins_ekf2.cpp.
References ekf, ekf2, gps, GpsFixValid, LtpDef_i::hmsl, GpsState::hmsl, LlaCoor_i::lat, lla_int_from_gps(), LlaCoor_i::lon, ekf2_t::ltp_def, ltp_def_from_lla_i(), and stateSetLocalOrigin_i().
|
static |
Definition at line 864 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 922 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 459 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 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 817 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(), ins_reset_local_origin(), mag_cb(), optical_flow_cb(), 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 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(), ins_reset_local_origin(), 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().