dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf2: split out header and rename

This commit is contained in:
Daniel Agar 2020-08-24 14:02:42 -04:00
parent 4879a4e2ef
commit 33a7fed240
4 changed files with 573 additions and 538 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
src/modules/ekf2/CMakeLists.txt
View File

@ -1,6 +1,6 @@
############################################################################ ############################################################################
# #
# Copyright (c) 2015 PX4 Development Team. All rights reserved. # Copyright (c) 2015-2020 PX4 Development Team. All rights reserved.
# #
# Redistribution and use in source and binary forms, with or without # Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions # modification, are permitted provided that the following conditions
@ -39,11 +39,12 @@ px4_add_module(
COMPILE_FLAGS COMPILE_FLAGS
STACK_MAX 2400 STACK_MAX 2400
SRCS SRCS
ekf2_main.cpp EKF2.cpp
EKF2.hpp
DEPENDS DEPENDS
git_ecl git_ecl
ecl_EKF ecl_EKF
ecl_geo ecl_geo
perf perf
Ekf2Utility EKF2Utility
) )

558
src/modules/ekf2/ekf2_main.cpp → src/modules/ekf2/EKF2.cpp
View File

@ -1,6 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (c) 2015-2019 PX4 Development Team. All rights reserved. * Copyright (c) 2015-2020 PX4 Development Team. All rights reserved.
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@ -31,513 +31,9 @@
* *
****************************************************************************/ ****************************************************************************/
/** #include "EKF2.hpp"
* @file ekf2_main.cpp
* Implementation of the attitude and position estimator.
*
* @author Roman Bapst
*/
#include <float.h> EKF2::EKF2(bool replay_mode):
#include <drivers/drv_hrt.h>
#include <lib/ecl/EKF/ekf.h>
#include <lib/mathlib/mathlib.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <px4_platform_common/time.h>
#include <uORB/Publication.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/SubscriptionMultiArray.hpp>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/ekf2_timestamps.h>
#include <uORB/topics/ekf_gps_drift.h>
#include <uORB/topics/ekf_gps_position.h>
#include <uORB/topics/estimator_innovations.h>
#include <uORB/topics/estimator_sensor_bias.h>
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/landing_target_pose.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/sensor_selection.h>
#include <uORB/topics/vehicle_air_data.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_imu.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_magnetometer.h>
#include <uORB/topics/vehicle_odometry.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/wind_estimate.h>
#include <uORB/topics/yaw_estimator_status.h>
#include "Utility/PreFlightChecker.hpp"
// defines used to specify the mask position for use of different accuracy metrics in the GPS blending algorithm
#define BLEND_MASK_USE_SPD_ACC 1
#define BLEND_MASK_USE_HPOS_ACC 2
#define BLEND_MASK_USE_VPOS_ACC 4
// define max number of GPS receivers supported and 0 base instance used to access virtual 'blended' GPS solution
#define GPS_MAX_RECEIVERS 2
#define GPS_BLENDED_INSTANCE 2
using math::constrain;
using namespace time_literals;
class Ekf2 final : public ModuleBase<Ekf2>, public ModuleParams, public px4::ScheduledWorkItem
{
public:
explicit Ekf2(bool replay_mode = false);
~Ekf2() override;
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
bool init();
int print_status() override;
private:
void Run() override;
int getRangeSubIndex(); ///< get subscription index of first downward-facing range sensor
void fillGpsMsgWithVehicleGpsPosData(gps_message &msg, const vehicle_gps_position_s &data);
PreFlightChecker _preflt_checker;
void runPreFlightChecks(float dt, const filter_control_status_u &control_status,
const vehicle_status_s &vehicle_status,
const estimator_innovations_s &innov);
void resetPreFlightChecks();
template<typename Param>
void update_mag_bias(Param &mag_bias_param, int axis_index);
template<typename Param>
bool update_mag_decl(Param &mag_decl_param);
void publish_attitude(const hrt_abstime &timestamp);
void publish_wind_estimate(const hrt_abstime &timestamp);
void publish_yaw_estimator_status(const hrt_abstime &timestamp);
/*
* Update the internal state estimate for a blended GPS solution that is a weighted average of the phsyical
* receiver solutions. This internal state cannot be used directly by estimators because if physical receivers
* have significant position differences, variation in receiver estimated accuracy will cause undesirable
* variation in the position solution.
*/
bool blend_gps_data();
/*
* Calculate internal states used to blend GPS data from multiple receivers using weightings calculated
* by calc_blend_weights()
* States are written to _gps_state and _gps_blended_state class variables
*/
void update_gps_blend_states();
/*
* The location in _gps_blended_state will move around as the relative accuracy changes.
* To mitigate this effect a low-pass filtered offset from each GPS location to the blended location is
* calculated.
*/
void update_gps_offsets();
/*
* Apply the steady state physical receiver offsets calculated by update_gps_offsets().
*/
void apply_gps_offsets();
/*
Calculate GPS output that is a blend of the offset corrected physical receiver data
*/
void calc_gps_blend_output();
/*
* Calculate filtered WGS84 height from estimated AMSL height
*/
float filter_altitude_ellipsoid(float amsl_hgt);
inline float sq(float x) { return x * x; };
const bool _replay_mode; ///< true when we use replay data from a log
// time slip monitoring
uint64_t _integrated_time_us = 0; ///< integral of gyro delta time from start (uSec)
uint64_t _start_time_us = 0; ///< system time at EKF start (uSec)
int64_t _last_time_slip_us = 0; ///< Last time slip (uSec)
perf_counter_t _ekf_update_perf;
// Initialise time stamps used to send sensor data to the EKF and for logging
uint8_t _invalid_mag_id_count = 0; ///< number of times an invalid magnetomer device ID has been detected
// Used to check, save and use learned magnetometer biases
hrt_abstime _last_magcal_us = 0; ///< last time the EKF was operating a mode that estimates magnetomer biases (uSec)
hrt_abstime _total_cal_time_us = 0; ///< accumulated calibration time since the last save
float _last_valid_mag_cal[3] = {}; ///< last valid XYZ magnetometer bias estimates (mGauss)
bool _valid_cal_available[3] = {}; ///< true when an unsaved valid calibration for the XYZ magnetometer bias is available
float _last_valid_variance[3] = {}; ///< variances for the last valid magnetometer XYZ bias estimates (mGauss**2)
// Used to control saving of mag declination to be used on next startup
bool _mag_decl_saved = false; ///< true when the magnetic declination has been saved
// set pose/velocity as invalid if standard deviation is bigger than max_std_dev
// TODO: the user should be allowed to set these values by a parameter
static constexpr float ep_max_std_dev = 100.0f; ///< Maximum permissible standard deviation for estimated position
static constexpr float eo_max_std_dev = 100.0f; ///< Maximum permissible standard deviation for estimated orientation
//static constexpr float ev_max_std_dev = 100.0f; ///< Maximum permissible standard deviation for estimated velocity
// GPS blending and switching
gps_message _gps_state[GPS_MAX_RECEIVERS] {}; ///< internal state data for the physical GPS
gps_message _gps_blended_state{}; ///< internal state data for the blended GPS
gps_message _gps_output[GPS_MAX_RECEIVERS + 1] {}; ///< output state data for the physical and blended GPS
Vector2f _NE_pos_offset_m[GPS_MAX_RECEIVERS] = {}; ///< Filtered North,East position offset from GPS instance to blended solution in _output_state.location (m)
float _hgt_offset_mm[GPS_MAX_RECEIVERS] = {}; ///< Filtered height offset from GPS instance relative to blended solution in _output_state.location (mm)
Vector3f _blended_antenna_offset = {}; ///< blended antenna offset
float _blend_weights[GPS_MAX_RECEIVERS] = {}; ///< blend weight for each GPS. The blend weights must sum to 1.0 across all instances.
uint64_t _time_prev_us[GPS_MAX_RECEIVERS] = {}; ///< the previous value of time_us for that GPS instance - used to detect new data.
uint8_t _gps_best_index = 0; ///< index of the physical receiver with the lowest reported error
uint8_t _gps_select_index = 0; ///< 0 = GPS1, 1 = GPS2, 2 = blended
uint8_t _gps_time_ref_index =
0; ///< index of the receiver that is used as the timing reference for the blending update
uint8_t _gps_oldest_index = 0; ///< index of the physical receiver with the oldest data
uint8_t _gps_newest_index = 0; ///< index of the physical receiver with the newest data
uint8_t _gps_slowest_index = 0; ///< index of the physical receiver with the slowest update rate
float _gps_dt[GPS_MAX_RECEIVERS] = {}; ///< average time step in seconds.
bool _gps_new_output_data = false; ///< true if there is new output data for the EKF
bool _had_valid_terrain = false; ///< true if at any time there was a valid terrain estimate
int32_t _gps_alttitude_ellipsoid[GPS_MAX_RECEIVERS] {}; ///< altitude in 1E-3 meters (millimeters) above ellipsoid
uint64_t _gps_alttitude_ellipsoid_previous_timestamp[GPS_MAX_RECEIVERS] {}; ///< storage for previous timestamp to compute dt
float _wgs84_hgt_offset = 0; ///< height offset between AMSL and WGS84
bool _imu_bias_reset_request{false};
// republished aligned external visual odometry
bool new_ev_data_received = false;
vehicle_odometry_s _ev_odom{};
uORB::Subscription _airdata_sub{ORB_ID(vehicle_air_data)};
uORB::Subscription _airspeed_sub{ORB_ID(airspeed)};
uORB::Subscription _ev_odom_sub{ORB_ID(vehicle_visual_odometry)};
uORB::Subscription _landing_target_pose_sub{ORB_ID(landing_target_pose)};
uORB::Subscription _magnetometer_sub{ORB_ID(vehicle_magnetometer)};
uORB::Subscription _optical_flow_sub{ORB_ID(optical_flow)};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)};
uORB::Subscription _status_sub{ORB_ID(vehicle_status)};
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
uORB::SubscriptionCallbackWorkItem _sensor_combined_sub{this, ORB_ID(sensor_combined)};
static constexpr int MAX_SENSOR_COUNT = 3;
uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub{this, ORB_ID(vehicle_imu)};
int _imu_sub_index{-1};
bool _callback_registered{false};
int _lockstep_component{-1};
// because we can have several distance sensor instances with different orientations
uORB::SubscriptionMultiArray<distance_sensor_s> _distance_sensor_subs{ORB_ID::distance_sensor};
int _range_finder_sub_index = -1; // index for downward-facing range finder subscription
// because we can have multiple GPS instances
uORB::Subscription _gps_subs[GPS_MAX_RECEIVERS] {{ORB_ID(vehicle_gps_position), 0}, {ORB_ID(vehicle_gps_position), 1}};
sensor_selection_s _sensor_selection{};
vehicle_land_detected_s _vehicle_land_detected{};
vehicle_status_s _vehicle_status{};
uORB::Publication<ekf2_timestamps_s> _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)};
uORB::Publication<ekf_gps_drift_s> _ekf_gps_drift_pub{ORB_ID(ekf_gps_drift)};
uORB::Publication<ekf_gps_position_s> _blended_gps_pub{ORB_ID(ekf_gps_position)};
uORB::Publication<estimator_innovations_s> _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)};
uORB::Publication<estimator_innovations_s> _estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)};
uORB::Publication<estimator_innovations_s> _estimator_innovations_pub{ORB_ID(estimator_innovations)};
uORB::Publication<estimator_sensor_bias_s> _estimator_sensor_bias_pub{ORB_ID(estimator_sensor_bias)};
uORB::Publication<estimator_status_s> _estimator_status_pub{ORB_ID(estimator_status)};
uORB::Publication<vehicle_attitude_s> _att_pub{ORB_ID(vehicle_attitude)};
uORB::Publication<vehicle_odometry_s> _vehicle_odometry_pub{ORB_ID(vehicle_odometry)};
uORB::Publication<yaw_estimator_status_s> _yaw_est_pub{ORB_ID(yaw_estimator_status)};
uORB::PublicationData<vehicle_global_position_s> _vehicle_global_position_pub{ORB_ID(vehicle_global_position)};
uORB::PublicationData<vehicle_local_position_s> _vehicle_local_position_pub{ORB_ID(vehicle_local_position)};
uORB::PublicationData<vehicle_odometry_s> _vehicle_visual_odometry_aligned_pub{ORB_ID(vehicle_visual_odometry_aligned)};
uORB::PublicationMulti<wind_estimate_s> _wind_pub{ORB_ID(wind_estimate)};
Ekf _ekf;
parameters *_params; ///< pointer to ekf parameter struct (located in _ekf class instance)
DEFINE_PARAMETERS(
(ParamExtInt<px4::params::EKF2_MIN_OBS_DT>)
_param_ekf2_min_obs_dt, ///< Maximum time delay of any sensor used to increase buffer length to handle large timing jitter (mSec)
(ParamExtFloat<px4::params::EKF2_MAG_DELAY>)
_param_ekf2_mag_delay, ///< magnetometer measurement delay relative to the IMU (mSec)
(ParamExtFloat<px4::params::EKF2_BARO_DELAY>)
_param_ekf2_baro_delay, ///< barometer height measurement delay relative to the IMU (mSec)
(ParamExtFloat<px4::params::EKF2_GPS_DELAY>)
_param_ekf2_gps_delay, ///< GPS measurement delay relative to the IMU (mSec)
(ParamExtFloat<px4::params::EKF2_OF_DELAY>)
_param_ekf2_of_delay, ///< optical flow measurement delay relative to the IMU (mSec) - this is to the middle of the optical flow integration interval
(ParamExtFloat<px4::params::EKF2_RNG_DELAY>)
_param_ekf2_rng_delay, ///< range finder measurement delay relative to the IMU (mSec)
(ParamExtFloat<px4::params::EKF2_ASP_DELAY>)
_param_ekf2_asp_delay, ///< airspeed measurement delay relative to the IMU (mSec)
(ParamExtFloat<px4::params::EKF2_EV_DELAY>)
_param_ekf2_ev_delay, ///< off-board vision measurement delay relative to the IMU (mSec)
(ParamExtFloat<px4::params::EKF2_AVEL_DELAY>)
_param_ekf2_avel_delay, ///< auxillary velocity measurement delay relative to the IMU (mSec)
(ParamExtFloat<px4::params::EKF2_GYR_NOISE>)
_param_ekf2_gyr_noise, ///< IMU angular rate noise used for covariance prediction (rad/sec)
(ParamExtFloat<px4::params::EKF2_ACC_NOISE>)
_param_ekf2_acc_noise, ///< IMU acceleration noise use for covariance prediction (m/sec**2)
// process noise
(ParamExtFloat<px4::params::EKF2_GYR_B_NOISE>)
_param_ekf2_gyr_b_noise, ///< process noise for IMU rate gyro bias prediction (rad/sec**2)
(ParamExtFloat<px4::params::EKF2_ACC_B_NOISE>)
_param_ekf2_acc_b_noise,///< process noise for IMU accelerometer bias prediction (m/sec**3)
(ParamExtFloat<px4::params::EKF2_MAG_E_NOISE>)
_param_ekf2_mag_e_noise, ///< process noise for earth magnetic field prediction (Gauss/sec)
(ParamExtFloat<px4::params::EKF2_MAG_B_NOISE>)
_param_ekf2_mag_b_noise, ///< process noise for body magnetic field prediction (Gauss/sec)
(ParamExtFloat<px4::params::EKF2_WIND_NOISE>)
_param_ekf2_wind_noise, ///< process noise for wind velocity prediction (m/sec**2)
(ParamExtFloat<px4::params::EKF2_TERR_NOISE>) _param_ekf2_terr_noise, ///< process noise for terrain offset (m/sec)
(ParamExtFloat<px4::params::EKF2_TERR_GRAD>)
_param_ekf2_terr_grad, ///< gradient of terrain used to estimate process noise due to changing position (m/m)
(ParamExtFloat<px4::params::EKF2_GPS_V_NOISE>)
_param_ekf2_gps_v_noise, ///< minimum allowed observation noise for gps velocity fusion (m/sec)
(ParamExtFloat<px4::params::EKF2_GPS_P_NOISE>)
_param_ekf2_gps_p_noise, ///< minimum allowed observation noise for gps position fusion (m)
(ParamExtFloat<px4::params::EKF2_NOAID_NOISE>)
_param_ekf2_noaid_noise, ///< observation noise for non-aiding position fusion (m)
(ParamExtFloat<px4::params::EKF2_BARO_NOISE>)
_param_ekf2_baro_noise, ///< observation noise for barometric height fusion (m)
(ParamExtFloat<px4::params::EKF2_BARO_GATE>)
_param_ekf2_baro_gate, ///< barometric height innovation consistency gate size (STD)
(ParamExtFloat<px4::params::EKF2_GND_EFF_DZ>)
_param_ekf2_gnd_eff_dz, ///< barometric deadzone range for negative innovations (m)
(ParamExtFloat<px4::params::EKF2_GND_MAX_HGT>)
_param_ekf2_gnd_max_hgt, ///< maximum height above the ground level for expected negative baro innovations (m)
(ParamExtFloat<px4::params::EKF2_GPS_P_GATE>)
_param_ekf2_gps_p_gate, ///< GPS horizontal position innovation consistency gate size (STD)
(ParamExtFloat<px4::params::EKF2_GPS_V_GATE>)
_param_ekf2_gps_v_gate, ///< GPS velocity innovation consistency gate size (STD)
(ParamExtFloat<px4::params::EKF2_TAS_GATE>)
_param_ekf2_tas_gate, ///< True Airspeed innovation consistency gate size (STD)
// control of magnetometer fusion
(ParamExtFloat<px4::params::EKF2_HEAD_NOISE>)
_param_ekf2_head_noise, ///< measurement noise used for simple heading fusion (rad)
(ParamExtFloat<px4::params::EKF2_MAG_NOISE>)
_param_ekf2_mag_noise, ///< measurement noise used for 3-axis magnetoemeter fusion (Gauss)
(ParamExtFloat<px4::params::EKF2_EAS_NOISE>)
_param_ekf2_eas_noise, ///< measurement noise used for airspeed fusion (m/sec)
(ParamExtFloat<px4::params::EKF2_BETA_GATE>)
_param_ekf2_beta_gate, ///< synthetic sideslip innovation consistency gate size (STD)
(ParamExtFloat<px4::params::EKF2_BETA_NOISE>) _param_ekf2_beta_noise, ///< synthetic sideslip noise (rad)
(ParamExtFloat<px4::params::EKF2_MAG_DECL>) _param_ekf2_mag_decl,///< magnetic declination (degrees)
(ParamExtFloat<px4::params::EKF2_HDG_GATE>)
_param_ekf2_hdg_gate,///< heading fusion innovation consistency gate size (STD)
(ParamExtFloat<px4::params::EKF2_MAG_GATE>)
_param_ekf2_mag_gate, ///< magnetometer fusion innovation consistency gate size (STD)
(ParamExtInt<px4::params::EKF2_DECL_TYPE>)
_param_ekf2_decl_type, ///< bitmask used to control the handling of declination data
(ParamExtInt<px4::params::EKF2_MAG_TYPE>)
_param_ekf2_mag_type, ///< integer used to specify the type of magnetometer fusion used
(ParamExtFloat<px4::params::EKF2_MAG_ACCLIM>)
_param_ekf2_mag_acclim, ///< integer used to specify the type of magnetometer fusion used
(ParamExtFloat<px4::params::EKF2_MAG_YAWLIM>)
_param_ekf2_mag_yawlim, ///< yaw rate threshold used by mode select logic (rad/sec)
(ParamExtInt<px4::params::EKF2_GPS_CHECK>)
_param_ekf2_gps_check, ///< bitmask used to control which GPS quality checks are used
(ParamExtFloat<px4::params::EKF2_REQ_EPH>) _param_ekf2_req_eph, ///< maximum acceptable horiz position error (m)
(ParamExtFloat<px4::params::EKF2_REQ_EPV>) _param_ekf2_req_epv, ///< maximum acceptable vert position error (m)
(ParamExtFloat<px4::params::EKF2_REQ_SACC>) _param_ekf2_req_sacc, ///< maximum acceptable speed error (m/s)
(ParamExtInt<px4::params::EKF2_REQ_NSATS>) _param_ekf2_req_nsats, ///< minimum acceptable satellite count
(ParamExtFloat<px4::params::EKF2_REQ_PDOP>)
_param_ekf2_req_pdop, ///< maximum acceptable position dilution of precision
(ParamExtFloat<px4::params::EKF2_REQ_HDRIFT>)
_param_ekf2_req_hdrift, ///< maximum acceptable horizontal drift speed (m/s)
(ParamExtFloat<px4::params::EKF2_REQ_VDRIFT>) _param_ekf2_req_vdrift, ///< maximum acceptable vertical drift speed (m/s)
// measurement source control
(ParamExtInt<px4::params::EKF2_AID_MASK>)
_param_ekf2_aid_mask, ///< bitmasked integer that selects which of the GPS and optical flow aiding sources will be used
(ParamExtInt<px4::params::EKF2_HGT_MODE>) _param_ekf2_hgt_mode, ///< selects the primary source for height data
(ParamExtInt<px4::params::EKF2_TERR_MASK>)
_param_ekf2_terr_mask, ///< bitmasked integer that selects which of range finder and optical flow aiding sources will be used for terrain estimation
(ParamExtInt<px4::params::EKF2_NOAID_TOUT>)
_param_ekf2_noaid_tout, ///< maximum lapsed time from last fusion of measurements that constrain drift before the EKF will report the horizontal nav solution invalid (uSec)
// range finder fusion
(ParamExtFloat<px4::params::EKF2_RNG_NOISE>)
_param_ekf2_rng_noise, ///< observation noise for range finder measurements (m)
(ParamExtFloat<px4::params::EKF2_RNG_SFE>) _param_ekf2_rng_sfe, ///< scale factor from range to range noise (m/m)
(ParamExtFloat<px4::params::EKF2_RNG_GATE>)
_param_ekf2_rng_gate, ///< range finder fusion innovation consistency gate size (STD)
(ParamExtFloat<px4::params::EKF2_MIN_RNG>) _param_ekf2_min_rng, ///< minimum valid value for range when on ground (m)
(ParamExtFloat<px4::params::EKF2_RNG_PITCH>) _param_ekf2_rng_pitch, ///< range sensor pitch offset (rad)
(ParamExtInt<px4::params::EKF2_RNG_AID>)
_param_ekf2_rng_aid, ///< enables use of a range finder even if primary height source is not range finder
(ParamExtFloat<px4::params::EKF2_RNG_A_VMAX>)
_param_ekf2_rng_a_vmax, ///< maximum allowed horizontal velocity for range aid (m/s)
(ParamExtFloat<px4::params::EKF2_RNG_A_HMAX>)
_param_ekf2_rng_a_hmax, ///< maximum allowed absolute altitude (AGL) for range aid (m)
(ParamExtFloat<px4::params::EKF2_RNG_A_IGATE>)
_param_ekf2_rng_a_igate, ///< gate size used for innovation consistency checks for range aid fusion (STD)
// vision estimate fusion
(ParamInt<px4::params::EKF2_EV_NOISE_MD>)
_param_ekf2_ev_noise_md, ///< determine source of vision observation noise
(ParamFloat<px4::params::EKF2_EVP_NOISE>)
_param_ekf2_evp_noise, ///< default position observation noise for exernal vision measurements (m)
(ParamFloat<px4::params::EKF2_EVV_NOISE>)
_param_ekf2_evv_noise, ///< default velocity observation noise for exernal vision measurements (m/s)
(ParamFloat<px4::params::EKF2_EVA_NOISE>)
_param_ekf2_eva_noise, ///< default angular observation noise for exernal vision measurements (rad)
(ParamExtFloat<px4::params::EKF2_EVV_GATE>)
_param_ekf2_evv_gate, ///< external vision velocity innovation consistency gate size (STD)
(ParamExtFloat<px4::params::EKF2_EVP_GATE>)
_param_ekf2_evp_gate, ///< external vision position innovation consistency gate size (STD)
// optical flow fusion
(ParamExtFloat<px4::params::EKF2_OF_N_MIN>)
_param_ekf2_of_n_min, ///< best quality observation noise for optical flow LOS rate measurements (rad/sec)
(ParamExtFloat<px4::params::EKF2_OF_N_MAX>)
_param_ekf2_of_n_max, ///< worst quality observation noise for optical flow LOS rate measurements (rad/sec)
(ParamExtInt<px4::params::EKF2_OF_QMIN>)
_param_ekf2_of_qmin, ///< minimum acceptable quality integer from the flow sensor
(ParamExtFloat<px4::params::EKF2_OF_GATE>)
_param_ekf2_of_gate, ///< optical flow fusion innovation consistency gate size (STD)
(ParamInt<px4::params::EKF2_IMU_ID>) _param_ekf2_imu_id,
// sensor positions in body frame
(ParamExtFloat<px4::params::EKF2_IMU_POS_X>) _param_ekf2_imu_pos_x, ///< X position of IMU in body frame (m)
(ParamExtFloat<px4::params::EKF2_IMU_POS_Y>) _param_ekf2_imu_pos_y, ///< Y position of IMU in body frame (m)
(ParamExtFloat<px4::params::EKF2_IMU_POS_Z>) _param_ekf2_imu_pos_z, ///< Z position of IMU in body frame (m)
(ParamExtFloat<px4::params::EKF2_GPS_POS_X>) _param_ekf2_gps_pos_x, ///< X position of GPS antenna in body frame (m)
(ParamExtFloat<px4::params::EKF2_GPS_POS_Y>) _param_ekf2_gps_pos_y, ///< Y position of GPS antenna in body frame (m)
(ParamExtFloat<px4::params::EKF2_GPS_POS_Z>) _param_ekf2_gps_pos_z, ///< Z position of GPS antenna in body frame (m)
(ParamExtFloat<px4::params::EKF2_RNG_POS_X>) _param_ekf2_rng_pos_x, ///< X position of range finder in body frame (m)
(ParamExtFloat<px4::params::EKF2_RNG_POS_Y>) _param_ekf2_rng_pos_y, ///< Y position of range finder in body frame (m)
(ParamExtFloat<px4::params::EKF2_RNG_POS_Z>) _param_ekf2_rng_pos_z, ///< Z position of range finder in body frame (m)
(ParamExtFloat<px4::params::EKF2_OF_POS_X>)
_param_ekf2_of_pos_x, ///< X position of optical flow sensor focal point in body frame (m)
(ParamExtFloat<px4::params::EKF2_OF_POS_Y>)
_param_ekf2_of_pos_y, ///< Y position of optical flow sensor focal point in body frame (m)
(ParamExtFloat<px4::params::EKF2_OF_POS_Z>)
_param_ekf2_of_pos_z, ///< Z position of optical flow sensor focal point in body frame (m)
(ParamExtFloat<px4::params::EKF2_EV_POS_X>)
_param_ekf2_ev_pos_x, ///< X position of VI sensor focal point in body frame (m)
(ParamExtFloat<px4::params::EKF2_EV_POS_Y>)
_param_ekf2_ev_pos_y, ///< Y position of VI sensor focal point in body frame (m)
(ParamExtFloat<px4::params::EKF2_EV_POS_Z>)
_param_ekf2_ev_pos_z, ///< Z position of VI sensor focal point in body frame (m)
// control of airspeed and sideslip fusion
(ParamFloat<px4::params::EKF2_ARSP_THR>)
_param_ekf2_arsp_thr, ///< A value of zero will disabled airspeed fusion. Any positive value sets the minimum airspeed which will be used (m/sec)
(ParamInt<px4::params::EKF2_FUSE_BETA>)
_param_ekf2_fuse_beta, ///< Controls synthetic sideslip fusion, 0 disables, 1 enables
// output predictor filter time constants
(ParamExtFloat<px4::params::EKF2_TAU_VEL>)
_param_ekf2_tau_vel, ///< time constant used by the output velocity complementary filter (sec)
(ParamExtFloat<px4::params::EKF2_TAU_POS>)
_param_ekf2_tau_pos, ///< time constant used by the output position complementary filter (sec)
// IMU switch on bias parameters
(ParamExtFloat<px4::params::EKF2_GBIAS_INIT>)
_param_ekf2_gbias_init, ///< 1-sigma gyro bias uncertainty at switch on (rad/sec)
(ParamExtFloat<px4::params::EKF2_ABIAS_INIT>)
_param_ekf2_abias_init, ///< 1-sigma accelerometer bias uncertainty at switch on (m/sec**2)
(ParamExtFloat<px4::params::EKF2_ANGERR_INIT>)
_param_ekf2_angerr_init, ///< 1-sigma tilt error after initial alignment using gravity vector (rad)
// EKF saved XYZ magnetometer bias values
(ParamFloat<px4::params::EKF2_MAGBIAS_X>) _param_ekf2_magbias_x, ///< X magnetometer bias (mGauss)
(ParamFloat<px4::params::EKF2_MAGBIAS_Y>) _param_ekf2_magbias_y, ///< Y magnetometer bias (mGauss)
(ParamFloat<px4::params::EKF2_MAGBIAS_Z>) _param_ekf2_magbias_z, ///< Z magnetometer bias (mGauss)
(ParamInt<px4::params::EKF2_MAGBIAS_ID>)
_param_ekf2_magbias_id, ///< ID of the magnetometer sensor used to learn the bias values
(ParamFloat<px4::params::EKF2_MAGB_VREF>)
_param_ekf2_magb_vref, ///< Assumed error variance of previously saved magnetometer bias estimates (mGauss**2)
(ParamFloat<px4::params::EKF2_MAGB_K>)
_param_ekf2_magb_k, ///< maximum fraction of the learned magnetometer bias that is saved at each disarm
// EKF accel bias learning control
(ParamExtFloat<px4::params::EKF2_ABL_LIM>) _param_ekf2_abl_lim, ///< Accelerometer bias learning limit (m/s**2)
(ParamExtFloat<px4::params::EKF2_ABL_ACCLIM>)
_param_ekf2_abl_acclim, ///< Maximum IMU accel magnitude that allows IMU bias learning (m/s**2)
(ParamExtFloat<px4::params::EKF2_ABL_GYRLIM>)
_param_ekf2_abl_gyrlim, ///< Maximum IMU gyro angular rate magnitude that allows IMU bias learning (m/s**2)
(ParamExtFloat<px4::params::EKF2_ABL_TAU>)
_param_ekf2_abl_tau, ///< Time constant used to inhibit IMU delta velocity bias learning (sec)
// Multi-rotor drag specific force fusion
(ParamExtFloat<px4::params::EKF2_DRAG_NOISE>)
_param_ekf2_drag_noise, ///< observation noise variance for drag specific force measurements (m/sec**2)**2
(ParamExtFloat<px4::params::EKF2_BCOEF_X>) _param_ekf2_bcoef_x, ///< ballistic coefficient along the X-axis (kg/m**2)
(ParamExtFloat<px4::params::EKF2_BCOEF_Y>) _param_ekf2_bcoef_y, ///< ballistic coefficient along the Y-axis (kg/m**2)
// Corrections for static pressure position error where Ps_error = Ps_meas - Ps_truth
// Coef = Ps_error / Pdynamic, where Pdynamic = 1/2 * density * TAS**2
(ParamExtFloat<px4::params::EKF2_ASPD_MAX>)
_param_ekf2_aspd_max, ///< upper limit on airspeed used for correction (m/s**2)
(ParamExtFloat<px4::params::EKF2_PCOEF_XP>)
_param_ekf2_pcoef_xp, ///< static pressure position error coefficient along the positive X body axis
(ParamExtFloat<px4::params::EKF2_PCOEF_XN>)
_param_ekf2_pcoef_xn, ///< static pressure position error coefficient along the negative X body axis
(ParamExtFloat<px4::params::EKF2_PCOEF_YP>)
_param_ekf2_pcoef_yp, ///< static pressure position error coefficient along the positive Y body axis
(ParamExtFloat<px4::params::EKF2_PCOEF_YN>)
_param_ekf2_pcoef_yn, ///< static pressure position error coefficient along the negative Y body axis
(ParamExtFloat<px4::params::EKF2_PCOEF_Z>)
_param_ekf2_pcoef_z, ///< static pressure position error coefficient along the Z body axis
// GPS blending
(ParamInt<px4::params::EKF2_GPS_MASK>)
_param_ekf2_gps_mask, ///< mask defining when GPS accuracy metrics are used to calculate the blend ratio
(ParamFloat<px4::params::EKF2_GPS_TAU>)
_param_ekf2_gps_tau, ///< time constant controlling how rapidly the offset used to bring GPS solutions together is allowed to change (sec)
// Test used to determine if the vehicle is static or moving
(ParamExtFloat<px4::params::EKF2_MOVE_TEST>)
_param_ekf2_move_test, ///< scaling applied to IMU data thresholds used to determine if the vehicle is static or moving.
(ParamFloat<px4::params::EKF2_REQ_GPS_H>) _param_ekf2_req_gps_h, ///< Required GPS health time
(ParamExtInt<px4::params::EKF2_MAG_CHECK>) _param_ekf2_mag_check, ///< Mag field strength check
// Used by EKF-GSF experimental yaw estimator
(ParamExtFloat<px4::params::EKF2_GSF_TAS>)
_param_ekf2_gsf_tas_default ///< default value of true airspeed assumed during fixed wing operation
)
};
Ekf2::Ekf2(bool replay_mode):
ModuleParams(nullptr), ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
_replay_mode(replay_mode), _replay_mode(replay_mode),
@ -653,13 +149,13 @@ Ekf2::Ekf2(bool replay_mode):
_ekf.set_min_required_gps_health_time(_param_ekf2_req_gps_h.get() * 1_s); _ekf.set_min_required_gps_health_time(_param_ekf2_req_gps_h.get() * 1_s);
} }
Ekf2::~Ekf2() EKF2::~EKF2()
{ {
px4_lockstep_unregister_component(_lockstep_component); px4_lockstep_unregister_component(_lockstep_component);
perf_free(_ekf_update_perf); perf_free(_ekf_update_perf);
} }
bool Ekf2::init() bool EKF2::init()
{ {
const uint32_t device_id = _param_ekf2_imu_id.get(); const uint32_t device_id = _param_ekf2_imu_id.get();
@ -695,7 +191,7 @@ bool Ekf2::init()
return true; return true;
} }
int Ekf2::print_status() int EKF2::print_status()
{ {
PX4_INFO("local position: %s", (_ekf.local_position_is_valid()) ? "valid" : "invalid"); PX4_INFO("local position: %s", (_ekf.local_position_is_valid()) ? "valid" : "invalid");
PX4_INFO("global position: %s", (_ekf.global_position_is_valid()) ? "valid" : "invalid"); PX4_INFO("global position: %s", (_ekf.global_position_is_valid()) ? "valid" : "invalid");
@ -708,7 +204,7 @@ int Ekf2::print_status()
} }
template<typename Param> template<typename Param>
void Ekf2::update_mag_bias(Param &mag_bias_param, int axis_index) void EKF2::update_mag_bias(Param &mag_bias_param, int axis_index)
{ {
if (_valid_cal_available[axis_index]) { if (_valid_cal_available[axis_index]) {
@ -727,7 +223,7 @@ void Ekf2::update_mag_bias(Param &mag_bias_param, int axis_index)
} }
template<typename Param> template<typename Param>
bool Ekf2::update_mag_decl(Param &mag_decl_param) bool EKF2::update_mag_decl(Param &mag_decl_param)
{ {
// update stored declination value // update stored declination value
float declination_deg; float declination_deg;
@ -741,7 +237,7 @@ bool Ekf2::update_mag_decl(Param &mag_decl_param)
return false; return false;
} }
void Ekf2::Run() void EKF2::Run()
{ {
if (should_exit()) { if (should_exit()) {
_sensor_combined_sub.unregisterCallback(); _sensor_combined_sub.unregisterCallback();
@ -1738,7 +1234,7 @@ void Ekf2::Run()
} }
} }
void Ekf2::fillGpsMsgWithVehicleGpsPosData(gps_message &msg, const vehicle_gps_position_s &data) void EKF2::fillGpsMsgWithVehicleGpsPosData(gps_message &msg, const vehicle_gps_position_s &data)
{ {
msg.time_usec = data.timestamp; msg.time_usec = data.timestamp;
msg.lat = data.lat; msg.lat = data.lat;
@ -1759,7 +1255,7 @@ void Ekf2::fillGpsMsgWithVehicleGpsPosData(gps_message &msg, const vehicle_gps_p
msg.pdop = sqrtf(data.hdop * data.hdop + data.vdop * data.vdop); msg.pdop = sqrtf(data.hdop * data.hdop + data.vdop * data.vdop);
} }
void Ekf2::runPreFlightChecks(const float dt, void EKF2::runPreFlightChecks(const float dt,
const filter_control_status_u &control_status, const filter_control_status_u &control_status,
const vehicle_status_s &vehicle_status, const vehicle_status_s &vehicle_status,
const estimator_innovations_s &innov) const estimator_innovations_s &innov)
@ -1775,12 +1271,12 @@ void Ekf2::runPreFlightChecks(const float dt,
_preflt_checker.update(dt, innov); _preflt_checker.update(dt, innov);
} }
void Ekf2::resetPreFlightChecks() void EKF2::resetPreFlightChecks()
{ {
_preflt_checker.reset(); _preflt_checker.reset();
} }
int Ekf2::getRangeSubIndex() int EKF2::getRangeSubIndex()
{ {
for (unsigned i = 0; i < _distance_sensor_subs.size(); i++) { for (unsigned i = 0; i < _distance_sensor_subs.size(); i++) {
distance_sensor_s report; distance_sensor_s report;
@ -1797,7 +1293,7 @@ int Ekf2::getRangeSubIndex()
return -1; return -1;
} }
void Ekf2::publish_attitude(const hrt_abstime &timestamp) void EKF2::publish_attitude(const hrt_abstime &timestamp)
{ {
if (_ekf.attitude_valid()) { if (_ekf.attitude_valid()) {
// generate vehicle attitude quaternion data // generate vehicle attitude quaternion data
@ -1819,7 +1315,7 @@ void Ekf2::publish_attitude(const hrt_abstime &timestamp)
} }
} }
void Ekf2::publish_yaw_estimator_status(const hrt_abstime &timestamp) void EKF2::publish_yaw_estimator_status(const hrt_abstime &timestamp)
{ {
yaw_estimator_status_s yaw_est_test_data{}; yaw_estimator_status_s yaw_est_test_data{};
@ -1837,7 +1333,7 @@ void Ekf2::publish_yaw_estimator_status(const hrt_abstime &timestamp)
} }
} }
void Ekf2::publish_wind_estimate(const hrt_abstime &timestamp) void EKF2::publish_wind_estimate(const hrt_abstime &timestamp)
{ {
if (_ekf.get_wind_status()) { if (_ekf.get_wind_status()) {
// Publish wind estimate only if ekf declares them valid // Publish wind estimate only if ekf declares them valid
@ -1859,7 +1355,7 @@ void Ekf2::publish_wind_estimate(const hrt_abstime &timestamp)
} }
} }
bool Ekf2::blend_gps_data() bool EKF2::blend_gps_data()
{ {
// zero the blend weights // zero the blend weights
memset(&_blend_weights, 0, sizeof(_blend_weights)); memset(&_blend_weights, 0, sizeof(_blend_weights));
@ -2115,7 +1611,7 @@ bool Ekf2::blend_gps_data()
* because if physical receivers have significant position differences, variation in receiver estimated accuracy will * because if physical receivers have significant position differences, variation in receiver estimated accuracy will
* cause undesirable variation in the position solution. * cause undesirable variation in the position solution.
*/ */
void Ekf2::update_gps_blend_states() void EKF2::update_gps_blend_states()
{ {
// initialise the blended states so we can accumulate the results using the weightings for each GPS receiver. // initialise the blended states so we can accumulate the results using the weightings for each GPS receiver.
_gps_blended_state.time_usec = 0; _gps_blended_state.time_usec = 0;
@ -2262,7 +1758,7 @@ void Ekf2::update_gps_blend_states()
* To mitigate this effect a low-pass filtered offset from each GPS location to the blended location is * To mitigate this effect a low-pass filtered offset from each GPS location to the blended location is
* calculated. * calculated.
*/ */
void Ekf2::update_gps_offsets() void EKF2::update_gps_offsets()
{ {
// Calculate filter coefficients to be applied to the offsets for each GPS position and height offset // Calculate filter coefficients to be applied to the offsets for each GPS position and height offset
@ -2320,7 +1816,7 @@ void Ekf2::update_gps_offsets()
/* /*
* Apply the steady state physical receiver offsets calculated by update_gps_offsets(). * Apply the steady state physical receiver offsets calculated by update_gps_offsets().
*/ */
void Ekf2::apply_gps_offsets() void EKF2::apply_gps_offsets()
{ {
// calculate offset corrected output for each physical GPS. // calculate offset corrected output for each physical GPS.
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
@ -2354,7 +1850,7 @@ void Ekf2::apply_gps_offsets()
/* /*
Calculate GPS output that is a blend of the offset corrected physical receiver data Calculate GPS output that is a blend of the offset corrected physical receiver data
*/ */
void Ekf2::calc_gps_blend_output() void EKF2::calc_gps_blend_output()
{ {
// Convert each GPS position to a local NEU offset relative to the reference position // Convert each GPS position to a local NEU offset relative to the reference position
// which is defined as the positon of the blended solution calculated from non offset corrected data // which is defined as the positon of the blended solution calculated from non offset corrected data
@ -2410,7 +1906,7 @@ void Ekf2::calc_gps_blend_output()
} }
float Ekf2::filter_altitude_ellipsoid(float amsl_hgt) float EKF2::filter_altitude_ellipsoid(float amsl_hgt)
{ {
float height_diff = static_cast<float>(_gps_alttitude_ellipsoid[0]) * 1e-3f - amsl_hgt; float height_diff = static_cast<float>(_gps_alttitude_ellipsoid[0]) * 1e-3f - amsl_hgt;
@ -2432,12 +1928,12 @@ float Ekf2::filter_altitude_ellipsoid(float amsl_hgt)
return amsl_hgt + _wgs84_hgt_offset; return amsl_hgt + _wgs84_hgt_offset;
} }
int Ekf2::custom_command(int argc, char *argv[]) int EKF2::custom_command(int argc, char *argv[])
{ {
return print_usage("unknown command"); return print_usage("unknown command");
} }
int Ekf2::task_spawn(int argc, char *argv[]) int EKF2::task_spawn(int argc, char *argv[])
{ {
bool replay_mode = false; bool replay_mode = false;
@ -2446,7 +1942,7 @@ int Ekf2::task_spawn(int argc, char *argv[])
replay_mode = true; replay_mode = true;
} }
Ekf2 *instance = new Ekf2(replay_mode); EKF2 *instance = new EKF2(replay_mode);
if (instance) { if (instance) {
_object.store(instance); _object.store(instance);
@ -2467,7 +1963,7 @@ int Ekf2::task_spawn(int argc, char *argv[])
return PX4_ERROR; return PX4_ERROR;
} }
int Ekf2::print_usage(const char *reason) int EKF2::print_usage(const char *reason)
{ {
if (reason) { if (reason) {
PX4_WARN("%s\n", reason); PX4_WARN("%s\n", reason);
@ -2495,5 +1991,5 @@ timestamps from the sensor topics.
extern "C" __EXPORT int ekf2_main(int argc, char *argv[]) extern "C" __EXPORT int ekf2_main(int argc, char *argv[])
{ {
return Ekf2::main(argc, argv); return EKF2::main(argc, argv);
} }

538
src/modules/ekf2/EKF2.hpp Normal file
View File

@ -0,0 +1,538 @@
/****************************************************************************
*
* Copyright (c) 2015-2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file EKF2.cpp
* Implementation of the attitude and position estimator.
*
* @author Roman Bapst
*/
#pragma once
#include <float.h>
#include <drivers/drv_hrt.h>
#include <lib/ecl/EKF/ekf.h>
#include <lib/mathlib/mathlib.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <px4_platform_common/time.h>
#include <uORB/Publication.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/SubscriptionMultiArray.hpp>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/ekf2_timestamps.h>
#include <uORB/topics/ekf_gps_drift.h>
#include <uORB/topics/ekf_gps_position.h>
#include <uORB/topics/estimator_innovations.h>
#include <uORB/topics/estimator_sensor_bias.h>
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/landing_target_pose.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/sensor_selection.h>
#include <uORB/topics/vehicle_air_data.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_imu.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_magnetometer.h>
#include <uORB/topics/vehicle_odometry.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/wind_estimate.h>
#include <uORB/topics/yaw_estimator_status.h>
#include "Utility/PreFlightChecker.hpp"
// defines used to specify the mask position for use of different accuracy metrics in the GPS blending algorithm
#define BLEND_MASK_USE_SPD_ACC 1
#define BLEND_MASK_USE_HPOS_ACC 2
#define BLEND_MASK_USE_VPOS_ACC 4
// define max number of GPS receivers supported and 0 base instance used to access virtual 'blended' GPS solution
#define GPS_MAX_RECEIVERS 2
#define GPS_BLENDED_INSTANCE 2
using math::constrain;
using namespace time_literals;
class EKF2 final : public ModuleBase<EKF2>, public ModuleParams, public px4::ScheduledWorkItem
{
public:
explicit EKF2(bool replay_mode = false);
~EKF2() override;
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
bool init();
int print_status() override;
private:
void Run() override;
int getRangeSubIndex(); ///< get subscription index of first downward-facing range sensor
void fillGpsMsgWithVehicleGpsPosData(gps_message &msg, const vehicle_gps_position_s &data);
PreFlightChecker _preflt_checker;
void runPreFlightChecks(float dt, const filter_control_status_u &control_status,
const vehicle_status_s &vehicle_status,
const estimator_innovations_s &innov);
void resetPreFlightChecks();
template<typename Param>
void update_mag_bias(Param &mag_bias_param, int axis_index);
template<typename Param>
bool update_mag_decl(Param &mag_decl_param);
void publish_attitude(const hrt_abstime &timestamp);
void publish_wind_estimate(const hrt_abstime &timestamp);
void publish_yaw_estimator_status(const hrt_abstime &timestamp);
/*
* Update the internal state estimate for a blended GPS solution that is a weighted average of the phsyical
* receiver solutions. This internal state cannot be used directly by estimators because if physical receivers
* have significant position differences, variation in receiver estimated accuracy will cause undesirable
* variation in the position solution.
*/
bool blend_gps_data();
/*
* Calculate internal states used to blend GPS data from multiple receivers using weightings calculated
* by calc_blend_weights()
* States are written to _gps_state and _gps_blended_state class variables
*/
void update_gps_blend_states();
/*
* The location in _gps_blended_state will move around as the relative accuracy changes.
* To mitigate this effect a low-pass filtered offset from each GPS location to the blended location is
* calculated.
*/
void update_gps_offsets();
/*
* Apply the steady state physical receiver offsets calculated by update_gps_offsets().
*/
void apply_gps_offsets();
/*
Calculate GPS output that is a blend of the offset corrected physical receiver data
*/
void calc_gps_blend_output();
/*
* Calculate filtered WGS84 height from estimated AMSL height
*/
float filter_altitude_ellipsoid(float amsl_hgt);
inline float sq(float x) { return x * x; };
const bool _replay_mode; ///< true when we use replay data from a log
// time slip monitoring
uint64_t _integrated_time_us = 0; ///< integral of gyro delta time from start (uSec)
uint64_t _start_time_us = 0; ///< system time at EKF start (uSec)
int64_t _last_time_slip_us = 0; ///< Last time slip (uSec)
perf_counter_t _ekf_update_perf;
// Initialise time stamps used to send sensor data to the EKF and for logging
uint8_t _invalid_mag_id_count = 0; ///< number of times an invalid magnetomer device ID has been detected
// Used to check, save and use learned magnetometer biases
hrt_abstime _last_magcal_us = 0; ///< last time the EKF was operating a mode that estimates magnetomer biases (uSec)
hrt_abstime _total_cal_time_us = 0; ///< accumulated calibration time since the last save
float _last_valid_mag_cal[3] = {}; ///< last valid XYZ magnetometer bias estimates (mGauss)
bool _valid_cal_available[3] = {}; ///< true when an unsaved valid calibration for the XYZ magnetometer bias is available
float _last_valid_variance[3] = {}; ///< variances for the last valid magnetometer XYZ bias estimates (mGauss**2)
// Used to control saving of mag declination to be used on next startup
bool _mag_decl_saved = false; ///< true when the magnetic declination has been saved
// set pose/velocity as invalid if standard deviation is bigger than max_std_dev
// TODO: the user should be allowed to set these values by a parameter
static constexpr float ep_max_std_dev = 100.0f; ///< Maximum permissible standard deviation for estimated position
static constexpr float eo_max_std_dev = 100.0f; ///< Maximum permissible standard deviation for estimated orientation
//static constexpr float ev_max_std_dev = 100.0f; ///< Maximum permissible standard deviation for estimated velocity
// GPS blending and switching
gps_message _gps_state[GPS_MAX_RECEIVERS] {}; ///< internal state data for the physical GPS
gps_message _gps_blended_state{}; ///< internal state data for the blended GPS
gps_message _gps_output[GPS_MAX_RECEIVERS + 1] {}; ///< output state data for the physical and blended GPS
Vector2f _NE_pos_offset_m[GPS_MAX_RECEIVERS] = {}; ///< Filtered North,East position offset from GPS instance to blended solution in _output_state.location (m)
float _hgt_offset_mm[GPS_MAX_RECEIVERS] = {}; ///< Filtered height offset from GPS instance relative to blended solution in _output_state.location (mm)
Vector3f _blended_antenna_offset = {}; ///< blended antenna offset
float _blend_weights[GPS_MAX_RECEIVERS] = {}; ///< blend weight for each GPS. The blend weights must sum to 1.0 across all instances.
uint64_t _time_prev_us[GPS_MAX_RECEIVERS] = {}; ///< the previous value of time_us for that GPS instance - used to detect new data.
uint8_t _gps_best_index = 0; ///< index of the physical receiver with the lowest reported error
uint8_t _gps_select_index = 0; ///< 0 = GPS1, 1 = GPS2, 2 = blended
uint8_t _gps_time_ref_index =
0; ///< index of the receiver that is used as the timing reference for the blending update
uint8_t _gps_oldest_index = 0; ///< index of the physical receiver with the oldest data
uint8_t _gps_newest_index = 0; ///< index of the physical receiver with the newest data
uint8_t _gps_slowest_index = 0; ///< index of the physical receiver with the slowest update rate
float _gps_dt[GPS_MAX_RECEIVERS] = {}; ///< average time step in seconds.
bool _gps_new_output_data = false; ///< true if there is new output data for the EKF
bool _had_valid_terrain = false; ///< true if at any time there was a valid terrain estimate
int32_t _gps_alttitude_ellipsoid[GPS_MAX_RECEIVERS] {}; ///< altitude in 1E-3 meters (millimeters) above ellipsoid
uint64_t _gps_alttitude_ellipsoid_previous_timestamp[GPS_MAX_RECEIVERS] {}; ///< storage for previous timestamp to compute dt
float _wgs84_hgt_offset = 0; ///< height offset between AMSL and WGS84
bool _imu_bias_reset_request{false};
// republished aligned external visual odometry
bool new_ev_data_received = false;
vehicle_odometry_s _ev_odom{};
uORB::Subscription _airdata_sub{ORB_ID(vehicle_air_data)};
uORB::Subscription _airspeed_sub{ORB_ID(airspeed)};
uORB::Subscription _ev_odom_sub{ORB_ID(vehicle_visual_odometry)};
uORB::Subscription _landing_target_pose_sub{ORB_ID(landing_target_pose)};
uORB::Subscription _magnetometer_sub{ORB_ID(vehicle_magnetometer)};
uORB::Subscription _optical_flow_sub{ORB_ID(optical_flow)};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)};
uORB::Subscription _status_sub{ORB_ID(vehicle_status)};
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
uORB::SubscriptionCallbackWorkItem _sensor_combined_sub{this, ORB_ID(sensor_combined)};
static constexpr int MAX_SENSOR_COUNT = 3;
uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub{this, ORB_ID(vehicle_imu)};
int _imu_sub_index{-1};
bool _callback_registered{false};
int _lockstep_component{-1};
// because we can have several distance sensor instances with different orientations
uORB::SubscriptionMultiArray<distance_sensor_s> _distance_sensor_subs{ORB_ID::distance_sensor};
int _range_finder_sub_index = -1; // index for downward-facing range finder subscription
// because we can have multiple GPS instances
uORB::Subscription _gps_subs[GPS_MAX_RECEIVERS] {{ORB_ID(vehicle_gps_position), 0}, {ORB_ID(vehicle_gps_position), 1}};
sensor_selection_s _sensor_selection{};
vehicle_land_detected_s _vehicle_land_detected{};
vehicle_status_s _vehicle_status{};
uORB::Publication<ekf2_timestamps_s> _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)};
uORB::Publication<ekf_gps_drift_s> _ekf_gps_drift_pub{ORB_ID(ekf_gps_drift)};
uORB::Publication<ekf_gps_position_s> _blended_gps_pub{ORB_ID(ekf_gps_position)};
uORB::Publication<estimator_innovations_s> _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)};
uORB::Publication<estimator_innovations_s> _estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)};
uORB::Publication<estimator_innovations_s> _estimator_innovations_pub{ORB_ID(estimator_innovations)};
uORB::Publication<estimator_sensor_bias_s> _estimator_sensor_bias_pub{ORB_ID(estimator_sensor_bias)};
uORB::Publication<estimator_status_s> _estimator_status_pub{ORB_ID(estimator_status)};
uORB::Publication<vehicle_attitude_s> _att_pub{ORB_ID(vehicle_attitude)};
uORB::Publication<vehicle_odometry_s> _vehicle_odometry_pub{ORB_ID(vehicle_odometry)};
uORB::Publication<yaw_estimator_status_s> _yaw_est_pub{ORB_ID(yaw_estimator_status)};
uORB::PublicationData<vehicle_global_position_s> _vehicle_global_position_pub{ORB_ID(vehicle_global_position)};
uORB::PublicationData<vehicle_local_position_s> _vehicle_local_position_pub{ORB_ID(vehicle_local_position)};
uORB::PublicationData<vehicle_odometry_s> _vehicle_visual_odometry_aligned_pub{ORB_ID(vehicle_visual_odometry_aligned)};
uORB::PublicationMulti<wind_estimate_s> _wind_pub{ORB_ID(wind_estimate)};
Ekf _ekf;
parameters *_params; ///< pointer to ekf parameter struct (located in _ekf class instance)
DEFINE_PARAMETERS(
(ParamExtInt<px4::params::EKF2_MIN_OBS_DT>)
_param_ekf2_min_obs_dt, ///< Maximum time delay of any sensor used to increase buffer length to handle large timing jitter (mSec)
(ParamExtFloat<px4::params::EKF2_MAG_DELAY>)
_param_ekf2_mag_delay, ///< magnetometer measurement delay relative to the IMU (mSec)
(ParamExtFloat<px4::params::EKF2_BARO_DELAY>)
_param_ekf2_baro_delay, ///< barometer height measurement delay relative to the IMU (mSec)
(ParamExtFloat<px4::params::EKF2_GPS_DELAY>)
_param_ekf2_gps_delay, ///< GPS measurement delay relative to the IMU (mSec)
(ParamExtFloat<px4::params::EKF2_OF_DELAY>)
_param_ekf2_of_delay, ///< optical flow measurement delay relative to the IMU (mSec) - this is to the middle of the optical flow integration interval
(ParamExtFloat<px4::params::EKF2_RNG_DELAY>)
_param_ekf2_rng_delay, ///< range finder measurement delay relative to the IMU (mSec)
(ParamExtFloat<px4::params::EKF2_ASP_DELAY>)
_param_ekf2_asp_delay, ///< airspeed measurement delay relative to the IMU (mSec)
(ParamExtFloat<px4::params::EKF2_EV_DELAY>)
_param_ekf2_ev_delay, ///< off-board vision measurement delay relative to the IMU (mSec)
(ParamExtFloat<px4::params::EKF2_AVEL_DELAY>)
_param_ekf2_avel_delay, ///< auxillary velocity measurement delay relative to the IMU (mSec)
(ParamExtFloat<px4::params::EKF2_GYR_NOISE>)
_param_ekf2_gyr_noise, ///< IMU angular rate noise used for covariance prediction (rad/sec)
(ParamExtFloat<px4::params::EKF2_ACC_NOISE>)
_param_ekf2_acc_noise, ///< IMU acceleration noise use for covariance prediction (m/sec**2)
// process noise
(ParamExtFloat<px4::params::EKF2_GYR_B_NOISE>)
_param_ekf2_gyr_b_noise, ///< process noise for IMU rate gyro bias prediction (rad/sec**2)
(ParamExtFloat<px4::params::EKF2_ACC_B_NOISE>)
_param_ekf2_acc_b_noise,///< process noise for IMU accelerometer bias prediction (m/sec**3)
(ParamExtFloat<px4::params::EKF2_MAG_E_NOISE>)
_param_ekf2_mag_e_noise, ///< process noise for earth magnetic field prediction (Gauss/sec)
(ParamExtFloat<px4::params::EKF2_MAG_B_NOISE>)
_param_ekf2_mag_b_noise, ///< process noise for body magnetic field prediction (Gauss/sec)
(ParamExtFloat<px4::params::EKF2_WIND_NOISE>)
_param_ekf2_wind_noise, ///< process noise for wind velocity prediction (m/sec**2)
(ParamExtFloat<px4::params::EKF2_TERR_NOISE>) _param_ekf2_terr_noise, ///< process noise for terrain offset (m/sec)
(ParamExtFloat<px4::params::EKF2_TERR_GRAD>)
_param_ekf2_terr_grad, ///< gradient of terrain used to estimate process noise due to changing position (m/m)
(ParamExtFloat<px4::params::EKF2_GPS_V_NOISE>)
_param_ekf2_gps_v_noise, ///< minimum allowed observation noise for gps velocity fusion (m/sec)
(ParamExtFloat<px4::params::EKF2_GPS_P_NOISE>)
_param_ekf2_gps_p_noise, ///< minimum allowed observation noise for gps position fusion (m)
(ParamExtFloat<px4::params::EKF2_NOAID_NOISE>)
_param_ekf2_noaid_noise, ///< observation noise for non-aiding position fusion (m)
(ParamExtFloat<px4::params::EKF2_BARO_NOISE>)
_param_ekf2_baro_noise, ///< observation noise for barometric height fusion (m)
(ParamExtFloat<px4::params::EKF2_BARO_GATE>)
_param_ekf2_baro_gate, ///< barometric height innovation consistency gate size (STD)
(ParamExtFloat<px4::params::EKF2_GND_EFF_DZ>)
_param_ekf2_gnd_eff_dz, ///< barometric deadzone range for negative innovations (m)
(ParamExtFloat<px4::params::EKF2_GND_MAX_HGT>)
_param_ekf2_gnd_max_hgt, ///< maximum height above the ground level for expected negative baro innovations (m)
(ParamExtFloat<px4::params::EKF2_GPS_P_GATE>)
_param_ekf2_gps_p_gate, ///< GPS horizontal position innovation consistency gate size (STD)
(ParamExtFloat<px4::params::EKF2_GPS_V_GATE>)
_param_ekf2_gps_v_gate, ///< GPS velocity innovation consistency gate size (STD)
(ParamExtFloat<px4::params::EKF2_TAS_GATE>)
_param_ekf2_tas_gate, ///< True Airspeed innovation consistency gate size (STD)
// control of magnetometer fusion
(ParamExtFloat<px4::params::EKF2_HEAD_NOISE>)
_param_ekf2_head_noise, ///< measurement noise used for simple heading fusion (rad)
(ParamExtFloat<px4::params::EKF2_MAG_NOISE>)
_param_ekf2_mag_noise, ///< measurement noise used for 3-axis magnetoemeter fusion (Gauss)
(ParamExtFloat<px4::params::EKF2_EAS_NOISE>)
_param_ekf2_eas_noise, ///< measurement noise used for airspeed fusion (m/sec)
(ParamExtFloat<px4::params::EKF2_BETA_GATE>)
_param_ekf2_beta_gate, ///< synthetic sideslip innovation consistency gate size (STD)
(ParamExtFloat<px4::params::EKF2_BETA_NOISE>) _param_ekf2_beta_noise, ///< synthetic sideslip noise (rad)
(ParamExtFloat<px4::params::EKF2_MAG_DECL>) _param_ekf2_mag_decl,///< magnetic declination (degrees)
(ParamExtFloat<px4::params::EKF2_HDG_GATE>)
_param_ekf2_hdg_gate,///< heading fusion innovation consistency gate size (STD)
(ParamExtFloat<px4::params::EKF2_MAG_GATE>)
_param_ekf2_mag_gate, ///< magnetometer fusion innovation consistency gate size (STD)
(ParamExtInt<px4::params::EKF2_DECL_TYPE>)
_param_ekf2_decl_type, ///< bitmask used to control the handling of declination data
(ParamExtInt<px4::params::EKF2_MAG_TYPE>)
_param_ekf2_mag_type, ///< integer used to specify the type of magnetometer fusion used
(ParamExtFloat<px4::params::EKF2_MAG_ACCLIM>)
_param_ekf2_mag_acclim, ///< integer used to specify the type of magnetometer fusion used
(ParamExtFloat<px4::params::EKF2_MAG_YAWLIM>)
_param_ekf2_mag_yawlim, ///< yaw rate threshold used by mode select logic (rad/sec)
(ParamExtInt<px4::params::EKF2_GPS_CHECK>)
_param_ekf2_gps_check, ///< bitmask used to control which GPS quality checks are used
(ParamExtFloat<px4::params::EKF2_REQ_EPH>) _param_ekf2_req_eph, ///< maximum acceptable horiz position error (m)
(ParamExtFloat<px4::params::EKF2_REQ_EPV>) _param_ekf2_req_epv, ///< maximum acceptable vert position error (m)
(ParamExtFloat<px4::params::EKF2_REQ_SACC>) _param_ekf2_req_sacc, ///< maximum acceptable speed error (m/s)
(ParamExtInt<px4::params::EKF2_REQ_NSATS>) _param_ekf2_req_nsats, ///< minimum acceptable satellite count
(ParamExtFloat<px4::params::EKF2_REQ_PDOP>)
_param_ekf2_req_pdop, ///< maximum acceptable position dilution of precision
(ParamExtFloat<px4::params::EKF2_REQ_HDRIFT>)
_param_ekf2_req_hdrift, ///< maximum acceptable horizontal drift speed (m/s)
(ParamExtFloat<px4::params::EKF2_REQ_VDRIFT>) _param_ekf2_req_vdrift, ///< maximum acceptable vertical drift speed (m/s)
// measurement source control
(ParamExtInt<px4::params::EKF2_AID_MASK>)
_param_ekf2_aid_mask, ///< bitmasked integer that selects which of the GPS and optical flow aiding sources will be used
(ParamExtInt<px4::params::EKF2_HGT_MODE>) _param_ekf2_hgt_mode, ///< selects the primary source for height data
(ParamExtInt<px4::params::EKF2_TERR_MASK>)
_param_ekf2_terr_mask, ///< bitmasked integer that selects which of range finder and optical flow aiding sources will be used for terrain estimation
(ParamExtInt<px4::params::EKF2_NOAID_TOUT>)
_param_ekf2_noaid_tout, ///< maximum lapsed time from last fusion of measurements that constrain drift before the EKF will report the horizontal nav solution invalid (uSec)
// range finder fusion
(ParamExtFloat<px4::params::EKF2_RNG_NOISE>)
_param_ekf2_rng_noise, ///< observation noise for range finder measurements (m)
(ParamExtFloat<px4::params::EKF2_RNG_SFE>) _param_ekf2_rng_sfe, ///< scale factor from range to range noise (m/m)
(ParamExtFloat<px4::params::EKF2_RNG_GATE>)
_param_ekf2_rng_gate, ///< range finder fusion innovation consistency gate size (STD)
(ParamExtFloat<px4::params::EKF2_MIN_RNG>) _param_ekf2_min_rng, ///< minimum valid value for range when on ground (m)
(ParamExtFloat<px4::params::EKF2_RNG_PITCH>) _param_ekf2_rng_pitch, ///< range sensor pitch offset (rad)
(ParamExtInt<px4::params::EKF2_RNG_AID>)
_param_ekf2_rng_aid, ///< enables use of a range finder even if primary height source is not range finder
(ParamExtFloat<px4::params::EKF2_RNG_A_VMAX>)
_param_ekf2_rng_a_vmax, ///< maximum allowed horizontal velocity for range aid (m/s)
(ParamExtFloat<px4::params::EKF2_RNG_A_HMAX>)
_param_ekf2_rng_a_hmax, ///< maximum allowed absolute altitude (AGL) for range aid (m)
(ParamExtFloat<px4::params::EKF2_RNG_A_IGATE>)
_param_ekf2_rng_a_igate, ///< gate size used for innovation consistency checks for range aid fusion (STD)
// vision estimate fusion
(ParamInt<px4::params::EKF2_EV_NOISE_MD>)
_param_ekf2_ev_noise_md, ///< determine source of vision observation noise
(ParamFloat<px4::params::EKF2_EVP_NOISE>)
_param_ekf2_evp_noise, ///< default position observation noise for exernal vision measurements (m)
(ParamFloat<px4::params::EKF2_EVV_NOISE>)
_param_ekf2_evv_noise, ///< default velocity observation noise for exernal vision measurements (m/s)
(ParamFloat<px4::params::EKF2_EVA_NOISE>)
_param_ekf2_eva_noise, ///< default angular observation noise for exernal vision measurements (rad)
(ParamExtFloat<px4::params::EKF2_EVV_GATE>)
_param_ekf2_evv_gate, ///< external vision velocity innovation consistency gate size (STD)
(ParamExtFloat<px4::params::EKF2_EVP_GATE>)
_param_ekf2_evp_gate, ///< external vision position innovation consistency gate size (STD)
// optical flow fusion
(ParamExtFloat<px4::params::EKF2_OF_N_MIN>)
_param_ekf2_of_n_min, ///< best quality observation noise for optical flow LOS rate measurements (rad/sec)
(ParamExtFloat<px4::params::EKF2_OF_N_MAX>)
_param_ekf2_of_n_max, ///< worst quality observation noise for optical flow LOS rate measurements (rad/sec)
(ParamExtInt<px4::params::EKF2_OF_QMIN>)
_param_ekf2_of_qmin, ///< minimum acceptable quality integer from the flow sensor
(ParamExtFloat<px4::params::EKF2_OF_GATE>)
_param_ekf2_of_gate, ///< optical flow fusion innovation consistency gate size (STD)
(ParamInt<px4::params::EKF2_IMU_ID>) _param_ekf2_imu_id,
// sensor positions in body frame
(ParamExtFloat<px4::params::EKF2_IMU_POS_X>) _param_ekf2_imu_pos_x, ///< X position of IMU in body frame (m)
(ParamExtFloat<px4::params::EKF2_IMU_POS_Y>) _param_ekf2_imu_pos_y, ///< Y position of IMU in body frame (m)
(ParamExtFloat<px4::params::EKF2_IMU_POS_Z>) _param_ekf2_imu_pos_z, ///< Z position of IMU in body frame (m)
(ParamExtFloat<px4::params::EKF2_GPS_POS_X>) _param_ekf2_gps_pos_x, ///< X position of GPS antenna in body frame (m)
(ParamExtFloat<px4::params::EKF2_GPS_POS_Y>) _param_ekf2_gps_pos_y, ///< Y position of GPS antenna in body frame (m)
(ParamExtFloat<px4::params::EKF2_GPS_POS_Z>) _param_ekf2_gps_pos_z, ///< Z position of GPS antenna in body frame (m)
(ParamExtFloat<px4::params::EKF2_RNG_POS_X>) _param_ekf2_rng_pos_x, ///< X position of range finder in body frame (m)
(ParamExtFloat<px4::params::EKF2_RNG_POS_Y>) _param_ekf2_rng_pos_y, ///< Y position of range finder in body frame (m)
(ParamExtFloat<px4::params::EKF2_RNG_POS_Z>) _param_ekf2_rng_pos_z, ///< Z position of range finder in body frame (m)
(ParamExtFloat<px4::params::EKF2_OF_POS_X>)
_param_ekf2_of_pos_x, ///< X position of optical flow sensor focal point in body frame (m)
(ParamExtFloat<px4::params::EKF2_OF_POS_Y>)
_param_ekf2_of_pos_y, ///< Y position of optical flow sensor focal point in body frame (m)
(ParamExtFloat<px4::params::EKF2_OF_POS_Z>)
_param_ekf2_of_pos_z, ///< Z position of optical flow sensor focal point in body frame (m)
(ParamExtFloat<px4::params::EKF2_EV_POS_X>)
_param_ekf2_ev_pos_x, ///< X position of VI sensor focal point in body frame (m)
(ParamExtFloat<px4::params::EKF2_EV_POS_Y>)
_param_ekf2_ev_pos_y, ///< Y position of VI sensor focal point in body frame (m)
(ParamExtFloat<px4::params::EKF2_EV_POS_Z>)
_param_ekf2_ev_pos_z, ///< Z position of VI sensor focal point in body frame (m)
// control of airspeed and sideslip fusion
(ParamFloat<px4::params::EKF2_ARSP_THR>)
_param_ekf2_arsp_thr, ///< A value of zero will disabled airspeed fusion. Any positive value sets the minimum airspeed which will be used (m/sec)
(ParamInt<px4::params::EKF2_FUSE_BETA>)
_param_ekf2_fuse_beta, ///< Controls synthetic sideslip fusion, 0 disables, 1 enables
// output predictor filter time constants
(ParamExtFloat<px4::params::EKF2_TAU_VEL>)
_param_ekf2_tau_vel, ///< time constant used by the output velocity complementary filter (sec)
(ParamExtFloat<px4::params::EKF2_TAU_POS>)
_param_ekf2_tau_pos, ///< time constant used by the output position complementary filter (sec)
// IMU switch on bias parameters
(ParamExtFloat<px4::params::EKF2_GBIAS_INIT>)
_param_ekf2_gbias_init, ///< 1-sigma gyro bias uncertainty at switch on (rad/sec)
(ParamExtFloat<px4::params::EKF2_ABIAS_INIT>)
_param_ekf2_abias_init, ///< 1-sigma accelerometer bias uncertainty at switch on (m/sec**2)
(ParamExtFloat<px4::params::EKF2_ANGERR_INIT>)
_param_ekf2_angerr_init, ///< 1-sigma tilt error after initial alignment using gravity vector (rad)
// EKF saved XYZ magnetometer bias values
(ParamFloat<px4::params::EKF2_MAGBIAS_X>) _param_ekf2_magbias_x, ///< X magnetometer bias (mGauss)
(ParamFloat<px4::params::EKF2_MAGBIAS_Y>) _param_ekf2_magbias_y, ///< Y magnetometer bias (mGauss)
(ParamFloat<px4::params::EKF2_MAGBIAS_Z>) _param_ekf2_magbias_z, ///< Z magnetometer bias (mGauss)
(ParamInt<px4::params::EKF2_MAGBIAS_ID>)
_param_ekf2_magbias_id, ///< ID of the magnetometer sensor used to learn the bias values
(ParamFloat<px4::params::EKF2_MAGB_VREF>)
_param_ekf2_magb_vref, ///< Assumed error variance of previously saved magnetometer bias estimates (mGauss**2)
(ParamFloat<px4::params::EKF2_MAGB_K>)
_param_ekf2_magb_k, ///< maximum fraction of the learned magnetometer bias that is saved at each disarm
// EKF accel bias learning control
(ParamExtFloat<px4::params::EKF2_ABL_LIM>) _param_ekf2_abl_lim, ///< Accelerometer bias learning limit (m/s**2)
(ParamExtFloat<px4::params::EKF2_ABL_ACCLIM>)
_param_ekf2_abl_acclim, ///< Maximum IMU accel magnitude that allows IMU bias learning (m/s**2)
(ParamExtFloat<px4::params::EKF2_ABL_GYRLIM>)
_param_ekf2_abl_gyrlim, ///< Maximum IMU gyro angular rate magnitude that allows IMU bias learning (m/s**2)
(ParamExtFloat<px4::params::EKF2_ABL_TAU>)
_param_ekf2_abl_tau, ///< Time constant used to inhibit IMU delta velocity bias learning (sec)
// Multi-rotor drag specific force fusion
(ParamExtFloat<px4::params::EKF2_DRAG_NOISE>)
_param_ekf2_drag_noise, ///< observation noise variance for drag specific force measurements (m/sec**2)**2
(ParamExtFloat<px4::params::EKF2_BCOEF_X>) _param_ekf2_bcoef_x, ///< ballistic coefficient along the X-axis (kg/m**2)
(ParamExtFloat<px4::params::EKF2_BCOEF_Y>) _param_ekf2_bcoef_y, ///< ballistic coefficient along the Y-axis (kg/m**2)
// Corrections for static pressure position error where Ps_error = Ps_meas - Ps_truth
// Coef = Ps_error / Pdynamic, where Pdynamic = 1/2 * density * TAS**2
(ParamExtFloat<px4::params::EKF2_ASPD_MAX>)
_param_ekf2_aspd_max, ///< upper limit on airspeed used for correction (m/s**2)
(ParamExtFloat<px4::params::EKF2_PCOEF_XP>)
_param_ekf2_pcoef_xp, ///< static pressure position error coefficient along the positive X body axis
(ParamExtFloat<px4::params::EKF2_PCOEF_XN>)
_param_ekf2_pcoef_xn, ///< static pressure position error coefficient along the negative X body axis
(ParamExtFloat<px4::params::EKF2_PCOEF_YP>)
_param_ekf2_pcoef_yp, ///< static pressure position error coefficient along the positive Y body axis
(ParamExtFloat<px4::params::EKF2_PCOEF_YN>)
_param_ekf2_pcoef_yn, ///< static pressure position error coefficient along the negative Y body axis
(ParamExtFloat<px4::params::EKF2_PCOEF_Z>)
_param_ekf2_pcoef_z, ///< static pressure position error coefficient along the Z body axis
// GPS blending
(ParamInt<px4::params::EKF2_GPS_MASK>)
_param_ekf2_gps_mask, ///< mask defining when GPS accuracy metrics are used to calculate the blend ratio
(ParamFloat<px4::params::EKF2_GPS_TAU>)
_param_ekf2_gps_tau, ///< time constant controlling how rapidly the offset used to bring GPS solutions together is allowed to change (sec)
// Test used to determine if the vehicle is static or moving
(ParamExtFloat<px4::params::EKF2_MOVE_TEST>)
_param_ekf2_move_test, ///< scaling applied to IMU data thresholds used to determine if the vehicle is static or moving.
(ParamFloat<px4::params::EKF2_REQ_GPS_H>) _param_ekf2_req_gps_h, ///< Required GPS health time
(ParamExtInt<px4::params::EKF2_MAG_CHECK>) _param_ekf2_mag_check, ///< Mag field strength check
// Used by EKF-GSF experimental yaw estimator
(ParamExtFloat<px4::params::EKF2_GSF_TAS>)
_param_ekf2_gsf_tas_default ///< default value of true airspeed assumed during fixed wing operation
)
};

8
src/modules/ekf2/Utility/CMakeLists.txt
View File

@ -31,15 +31,15 @@
# #
############################################################################# #############################################################################
px4_add_library(Ekf2Utility px4_add_library(EKF2Utility
PreFlightChecker.cpp PreFlightChecker.cpp
) )
target_include_directories(Ekf2Utility target_include_directories(EKF2Utility
PUBLIC PUBLIC
${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_SOURCE_DIR}
) )
target_link_libraries(Ekf2Utility PRIVATE mathlib) target_link_libraries(EKF2Utility PRIVATE mathlib)
px4_add_unit_gtest(SRC PreFlightCheckerTest.cpp LINKLIBS Ekf2Utility) px4_add_unit_gtest(SRC PreFlightCheckerTest.cpp LINKLIBS EKF2Utility)