PX4 Firmware
PX4 Autopilot Software http://px4.io
|
Public Member Functions | |
Ekf2 (bool replay_mode=false) | |
~Ekf2 () override | |
void | Run () override |
bool | init () |
int | print_status () override |
Static Public Member Functions | |
static int | task_spawn (int argc, char *argv[]) |
static int | custom_command (int argc, char *argv[]) |
static int | print_usage (const char *reason=nullptr) |
Private Member Functions | |
int | getRangeSubIndex () |
get subscription index of first downward-facing range sensor More... | |
void | fillGpsMsgWithVehicleGpsPosData (gps_message &msg, const vehicle_gps_position_s &data) |
void | runPreFlightChecks (float dt, const filter_control_status_u &control_status, const vehicle_status_s &vehicle_status, const ekf2_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) |
bool | publish_attitude (const sensor_combined_s &sensors, const hrt_abstime &now) |
bool | publish_wind_estimate (const hrt_abstime ×tamp) |
const Vector3f | get_vel_body_wind () |
bool | blend_gps_data () |
void | update_gps_blend_states () |
void | update_gps_offsets () |
void | apply_gps_offsets () |
void | calc_gps_blend_output () |
float | filter_altitude_ellipsoid (float amsl_hgt) |
Private Attributes | |
PreFlightChecker | _preflt_checker |
const bool | _replay_mode |
true when we use replay data from a log More... | |
uint64_t | _integrated_time_us = 0 |
integral of gyro delta time from start (uSec) More... | |
uint64_t | _start_time_us = 0 |
system time at EKF start (uSec) More... | |
int64_t | _last_time_slip_us = 0 |
Last time slip (uSec) More... | |
perf_counter_t | _ekf_update_perf |
uint8_t | _invalid_mag_id_count = 0 |
number of times an invalid magnetomer device ID has been detected More... | |
float | _mag_data_sum [3] = {} |
summed magnetometer readings (Gauss) More... | |
uint64_t | _mag_time_sum_ms = 0 |
summed magnetoemter time stamps (mSec) More... | |
uint8_t | _mag_sample_count = 0 |
number of magnetometer measurements summed during downsampling More... | |
int32_t | _mag_time_ms_last_used = 0 |
time stamp of the last averaged magnetometer measurement sent to the EKF (mSec) More... | |
float | _balt_data_sum = 0.0f |
summed pressure altitude readings (m) More... | |
uint64_t | _balt_time_sum_ms = 0 |
summed pressure altitude time stamps (mSec) More... | |
uint8_t | _balt_sample_count = 0 |
number of barometric altitude measurements summed More... | |
uint32_t | _balt_time_ms_last_used |
time stamp of the last averaged barometric altitude measurement sent to the EKF (mSec) More... | |
hrt_abstime | _last_magcal_us = 0 |
last time the EKF was operating a mode that estimates magnetomer biases (uSec) More... | |
hrt_abstime | _total_cal_time_us = 0 |
accumulated calibration time since the last save More... | |
float | _last_valid_mag_cal [3] = {} |
last valid XYZ magnetometer bias estimates (mGauss) More... | |
bool | _valid_cal_available [3] = {} |
true when an unsaved valid calibration for the XYZ magnetometer bias is available More... | |
float | _last_valid_variance [3] = {} |
variances for the last valid magnetometer XYZ bias estimates (mGauss**2) More... | |
bool | _mag_decl_saved = false |
true when the magnetic declination has been saved More... | |
gps_message | _gps_state [GPS_MAX_RECEIVERS] {} |
internal state data for the physical GPS More... | |
gps_message | _gps_blended_state {} |
internal state data for the blended GPS More... | |
gps_message | _gps_output [GPS_MAX_RECEIVERS+1] {} |
output state data for the physical and blended GPS More... | |
Vector2f | _NE_pos_offset_m [GPS_MAX_RECEIVERS] = {} |
Filtered North,East position offset from GPS instance to blended solution in _output_state.location (m) More... | |
float | _hgt_offset_mm [GPS_MAX_RECEIVERS] = {} |
Filtered height offset from GPS instance relative to blended solution in _output_state.location (mm) More... | |
Vector3f | _blended_antenna_offset = {} |
blended antenna offset More... | |
float | _blend_weights [GPS_MAX_RECEIVERS] = {} |
blend weight for each GPS. The blend weights must sum to 1.0 across all instances. More... | |
uint64_t | _time_prev_us [GPS_MAX_RECEIVERS] = {} |
the previous value of time_us for that GPS instance - used to detect new data. More... | |
uint8_t | _gps_best_index = 0 |
index of the physical receiver with the lowest reported error More... | |
uint8_t | _gps_select_index = 0 |
0 = GPS1, 1 = GPS2, 2 = blended More... | |
uint8_t | _gps_time_ref_index |
index of the receiver that is used as the timing reference for the blending update More... | |
uint8_t | _gps_oldest_index = 0 |
index of the physical receiver with the oldest data More... | |
uint8_t | _gps_newest_index = 0 |
index of the physical receiver with the newest data More... | |
uint8_t | _gps_slowest_index = 0 |
index of the physical receiver with the slowest update rate More... | |
float | _gps_dt [GPS_MAX_RECEIVERS] = {} |
average time step in seconds. More... | |
bool | _gps_new_output_data = false |
true if there is new output data for the EKF More... | |
bool | _had_valid_terrain = false |
true if at any time there was a valid terrain estimate More... | |
int32_t | _gps_alttitude_ellipsoid [GPS_MAX_RECEIVERS] {} |
altitude in 1E-3 meters (millimeters) above ellipsoid More... | |
uint64_t | _gps_alttitude_ellipsoid_previous_timestamp [GPS_MAX_RECEIVERS] {} |
storage for previous timestamp to compute dt More... | |
float | _wgs84_hgt_offset = 0 |
height offset between AMSL and WGS84 More... | |
bool | _imu_bias_reset_request {false} |
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 | _sensors_sub {this, ORB_ID(sensor_combined)} |
uORB::Subscription | _range_finder_subs [ORB_MULTI_MAX_INSTANCES] {{ORB_ID(distance_sensor), 0}, {ORB_ID(distance_sensor), 1}, {ORB_ID(distance_sensor), 2}, {ORB_ID(distance_sensor), 3}} |
int | _range_finder_sub_index = -1 |
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_innovations_s > | _estimator_innovations_pub {ORB_ID(ekf2_innovations)} |
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_status_s > | _estimator_status_pub {ORB_ID(estimator_status)} |
uORB::Publication< sensor_bias_s > | _sensor_bias_pub {ORB_ID(sensor_bias)} |
uORB::Publication< vehicle_attitude_s > | _att_pub {ORB_ID(vehicle_attitude)} |
uORB::Publication< vehicle_odometry_s > | _vehicle_odometry_pub {ORB_ID(vehicle_odometry)} |
uORB::PublicationMulti< wind_estimate_s > | _wind_pub {ORB_ID(wind_estimate)} |
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)} |
Ekf | _ekf |
parameters * | _params |
pointer to ekf parameter struct (located in _ekf class instance) More... | |
Static Private Attributes | |
static constexpr float | ep_max_std_dev = 100.0f |
Maximum permissible standard deviation for estimated position. More... | |
static constexpr float | eo_max_std_dev = 100.0f |
Maximum permissible standard deviation for estimated orientation. More... | |
Definition at line 95 of file ekf2_main.cpp.
|
explicit |
Definition at line 533 of file ekf2_main.cpp.
References Ekf::set_min_required_gps_health_time().
Referenced by task_spawn().
|
override |
Definition at line 641 of file ekf2_main.cpp.
References perf_free().
|
private |
Definition at line 2235 of file ekf2_main.cpp.
References _gps_output, _gps_state, add_vector_to_global_position(), estimator::gps_message::alt, estimator::gps_message::eph, estimator::gps_message::epv, estimator::gps_message::fix_type, GPS_MAX_RECEIVERS, estimator::gps_message::lat, estimator::gps_message::lon, estimator::gps_message::nsats, estimator::gps_message::pdop, estimator::gps_message::sacc, estimator::gps_message::time_usec, estimator::gps_message::vel_m_s, estimator::gps_message::vel_ned, estimator::gps_message::vel_ned_valid, estimator::gps_message::yaw, and estimator::gps_message::yaw_offset.
Referenced by Run().
|
private |
Definition at line 1778 of file ekf2_main.cpp.
References _gps_blended_state, _gps_newest_index, _gps_slowest_index, _gps_state, _gps_time_ref_index, BLEND_MASK_USE_HPOS_ACC, BLEND_MASK_USE_SPD_ACC, BLEND_MASK_USE_VPOS_ACC, estimator::gps_message::eph, estimator::gps_message::epv, f(), GPS_MAX_RECEIVERS, estimator::gps_message::sacc, estimator::gps_message::time_usec, update_gps_blend_states(), and update_gps_offsets().
Referenced by Run().
|
private |
Definition at line 2271 of file ekf2_main.cpp.
References _gps_blended_state, _gps_output, add_vector_to_global_position(), estimator::gps_message::alt, estimator::gps_message::eph, estimator::gps_message::epv, f(), estimator::gps_message::fix_type, get_vector_to_next_waypoint(), GPS_BLENDED_INSTANCE, GPS_MAX_RECEIVERS, estimator::gps_message::lat, estimator::gps_message::lon, estimator::gps_message::nsats, estimator::gps_message::pdop, estimator::gps_message::sacc, estimator::gps_message::time_usec, estimator::gps_message::vel_m_s, estimator::gps_message::vel_ned, estimator::gps_message::vel_ned_valid, estimator::gps_message::yaw, and estimator::gps_message::yaw_offset.
Referenced by Run().
|
static |
Definition at line 2351 of file ekf2_main.cpp.
References print_usage().
|
private |
Definition at line 1646 of file ekf2_main.cpp.
References vehicle_gps_position_s::alt, estimator::gps_message::alt, vehicle_gps_position_s::eph, estimator::gps_message::eph, vehicle_gps_position_s::epv, estimator::gps_message::epv, estimator::gps_message::fix_type, vehicle_gps_position_s::fix_type, vehicle_gps_position_s::hdop, vehicle_gps_position_s::heading, vehicle_gps_position_s::heading_offset, vehicle_gps_position_s::lat, estimator::gps_message::lat, vehicle_gps_position_s::lon, estimator::gps_message::lon, estimator::gps_message::nsats, estimator::gps_message::pdop, vehicle_gps_position_s::s_variance_m_s, estimator::gps_message::sacc, vehicle_gps_position_s::satellites_used, estimator::gps_message::time_usec, vehicle_gps_position_s::timestamp, vehicle_gps_position_s::vdop, vehicle_gps_position_s::vel_d_m_s, vehicle_gps_position_s::vel_e_m_s, vehicle_gps_position_s::vel_m_s, estimator::gps_message::vel_m_s, vehicle_gps_position_s::vel_n_m_s, estimator::gps_message::vel_ned, estimator::gps_message::vel_ned_valid, vehicle_gps_position_s::vel_ned_valid, estimator::gps_message::yaw, and estimator::gps_message::yaw_offset.
Referenced by Run().
|
private |
Definition at line 2329 of file ekf2_main.cpp.
References _gps_alttitude_ellipsoid, _gps_alttitude_ellipsoid_previous_timestamp, _gps_state, _wgs84_hgt_offset, math::constrain(), dt, f(), and estimator::gps_message::time_usec.
Referenced by Run().
|
private |
Definition at line 1758 of file ekf2_main.cpp.
References EstimatorInterface::get_quaternion(), EstimatorInterface::get_velocity(), Ekf::get_wind_velocity(), matrix::Quaternion< Type >::inversed(), and velocity.
Referenced by Run().
|
private |
get subscription index of first downward-facing range sensor
Definition at line 1687 of file ekf2_main.cpp.
References _range_finder_subs, and ORB_MULTI_MAX_INSTANCES.
Referenced by Run().
bool Ekf2::init | ( | ) |
Definition at line 647 of file ekf2_main.cpp.
References _sensors_sub, and uORB::SubscriptionCallback::registerCallback().
Referenced by task_spawn().
|
override |
Definition at line 657 of file ekf2_main.cpp.
References Ekf::global_position_is_valid(), EstimatorInterface::local_position_is_valid(), and perf_print_counter().
|
static |
Definition at line 2386 of file ekf2_main.cpp.
Referenced by custom_command().
|
private |
Definition at line 1704 of file ekf2_main.cpp.
References _att_pub, EstimatorInterface::attitude_valid(), Ekf::calculate_quaternion(), vehicle_attitude_s::delta_q_reset, Ekf::get_quat_reset(), uORB::Publication< T >::publish(), vehicle_attitude_s::q, vehicle_attitude_s::quat_reset_counter, and vehicle_attitude_s::timestamp.
Referenced by Run().
|
private |
Definition at line 1730 of file ekf2_main.cpp.
References _wind_pub, Ekf::get_airspeed_innov(), Ekf::get_airspeed_innov_var(), Ekf::get_beta_innov(), Ekf::get_beta_innov_var(), EstimatorInterface::get_wind_status(), Ekf::get_wind_velocity(), Ekf::get_wind_velocity_var(), and uORB::PublicationMulti< T >::publish().
Referenced by Run().
|
private |
Definition at line 1682 of file ekf2_main.cpp.
References PreFlightChecker::reset().
Referenced by Run().
|
override |
Definition at line 703 of file ekf2_main.cpp.
References _airdata_sub, _airspeed_sub, _balt_sample_count, _blended_gps_pub, _ekf2_timestamps_pub, _ekf_gps_drift_pub, _estimator_innovations_pub, _estimator_status_pub, _ev_odom, _ev_odom_sub, _gps_alttitude_ellipsoid, _gps_output, _gps_select_index, _gps_state, _gps_subs, _imu_bias_reset_request, _landing_target_pose_sub, _last_magcal_us, _mag_sample_count, _magnetometer_sub, _optical_flow_sub, _parameter_update_sub, _range_finder_subs, _sensor_bias_pub, _sensor_selection, _sensor_selection_sub, _sensors_sub, _start_time_us, _status_sub, _vehicle_global_position_pub, _vehicle_land_detected, _vehicle_land_detected_sub, _vehicle_local_position_pub, _vehicle_odometry_pub, _vehicle_status, _vehicle_visual_odometry_aligned_pub, sensor_selection_s::accel_device_id, sensor_combined_s::accelerometer_integral_dt, sensor_combined_s::accelerometer_m_s2, ekf2_innovations_s::airspeed_innov, ekf2_innovations_s::airspeed_innov_var, vehicle_global_position_s::alt, ekf_gps_position_s::alt, estimator::gps_message::alt, vehicle_global_position_s::alt_ellipsoid, vehicle_gps_position_s::alt_ellipsoid, estimator::ext_vision_message::angErr, apply_gps_offsets(), vehicle_status_s::arming_state, ekf2_innovations_s::aux_vel_innov, vehicle_local_position_s::ax, vehicle_local_position_s::ay, vehicle_local_position_s::az, vehicle_air_data_s::baro_alt_meter, ekf2_innovations_s::beta_innov, ekf2_innovations_s::beta_innov_var, estimator_status_s::beta_test_ratio, blend_gps_data(), ekf_gps_drift_s::blocked, calc_gps_blend_output(), CONSTANTS_ONE_G, estimator_status_s::control_mode_flags, uORB::Subscription::copy(), matrix::Matrix< Type, M, N >::copyTo(), landing_target_pose_s::cov_vx_rel, landing_target_pose_s::cov_vy_rel, estimator_status_s::covariances, Ekf::covariances_diagonal(), distance_sensor_s::current_distance, vehicle_global_position_s::dead_reckoning, vehicle_global_position_s::delta_alt, estimator::imuSample::delta_ang, estimator::imuSample::delta_ang_dt, estimator::imuSample::delta_vel, estimator::imuSample::delta_vel_dt, vehicle_local_position_s::delta_vxy, vehicle_local_position_s::delta_vz, vehicle_local_position_s::delta_xy, vehicle_local_position_s::delta_z, vehicle_local_position_s::dist_bottom, vehicle_local_position_s::dist_bottom_rate, vehicle_local_position_s::dist_bottom_valid, ekf2_innovations_s::drag_innov, ekf2_innovations_s::drag_innov_var, estimator::flow_message::dt, ekf_gps_position_s::eph, vehicle_global_position_s::eph, estimator::gps_message::eph, vehicle_local_position_s::eph, ekf_gps_position_s::epv, vehicle_global_position_s::epv, estimator::gps_message::epv, vehicle_local_position_s::epv, vehicle_local_position_s::evh, vehicle_local_position_s::evv, f(), fillGpsMsgWithVehicleGpsPosData(), filter_altitude_ellipsoid(), estimator_status_s::filter_fault_flags, estimator::gps_message::fix_type, ekf_gps_position_s::fix_type, estimator::filter_control_status_u::flags, ekf2_innovations_s::flow_innov, ekf2_innovations_s::flow_innov_var, estimator::flow_message::flowdata, FLT_EPSILON, uORB::PublicationData< T >::get(), Ekf::get_accel_bias(), Ekf::get_airspeed_innov(), Ekf::get_airspeed_innov_var(), Ekf::get_aux_vel_innov(), Ekf::get_beta_innov(), Ekf::get_beta_innov_var(), EstimatorInterface::get_control_mode(), Ekf::get_drag_innov(), Ekf::get_drag_innov_var(), Ekf::get_ekf_ctrl_limits(), Ekf::get_ekf_gpos_accuracy(), Ekf::get_ekf_lpos_accuracy(), Ekf::get_ekf_origin(), Ekf::get_ekf_soln_status(), Ekf::get_ekf_vel_accuracy(), Ekf::get_ev2ekf_quaternion(), EstimatorInterface::get_filter_fault_status(), Ekf::get_flow_innov(), Ekf::get_flow_innov_var(), Ekf::get_gps_check_status(), Ekf::get_gps_drift_metrics(), Ekf::get_gyro_bias(), EstimatorInterface::get_hagl_innov(), EstimatorInterface::get_hagl_innov_var(), Ekf::get_heading_innov(), Ekf::get_heading_innov_var(), Ekf::get_imu_vibe_metrics(), Ekf::get_innovation_test_status(), Ekf::get_mag_innov(), Ekf::get_mag_innov_var(), Ekf::get_output_tracking_error(), EstimatorInterface::get_pos_d_deriv(), Ekf::get_posD_reset(), EstimatorInterface::get_position(), Ekf::get_posNE_reset(), EstimatorInterface::get_quaternion(), Ekf::get_state_delayed(), EstimatorInterface::get_terrain_valid(), EstimatorInterface::get_terrain_vert_pos(), get_vel_body_wind(), EstimatorInterface::get_vel_deriv_ned(), Ekf::get_vel_pos_innov(), Ekf::get_vel_pos_innov_var(), Ekf::get_velD_reset(), Ekf::get_velNE_reset(), EstimatorInterface::get_velocity(), getRangeSubIndex(), Ekf::global_position_is_valid(), gps, estimator_status_s::gps_check_fail_flags, estimator::parameters::gps_check_mask, sensor_selection_s::gyro_device_id, sensor_combined_s::gyro_integral_dt, sensor_combined_s::gyro_rad, optical_flow_s::gyro_x_rate_integral, optical_flow_s::gyro_y_rate_integral, optical_flow_s::gyro_z_rate_integral, estimator::flow_message::gyrodata, ekf2_innovations_s::hagl_innov, ekf2_innovations_s::hagl_innov_var, vehicle_local_position_s::hagl_max, vehicle_local_position_s::hagl_min, estimator_status_s::hagl_test_ratio, PreFlightChecker::hasFailed(), PreFlightChecker::hasHeadingFailed(), PreFlightChecker::hasHeightFailed(), PreFlightChecker::hasHorizFailed(), PreFlightChecker::hasHorizVelFailed(), PreFlightChecker::hasVertFailed(), PreFlightChecker::hasVertVelFailed(), ekf_gps_position_s::heading, ekf2_innovations_s::heading_innov, ekf2_innovations_s::heading_innov_var, ekf_gps_position_s::heading_offset, estimator_status_s::health_flags, estimator_status_s::hgt_test_ratio, estimator::ext_vision_message::hgtErr, ekf_gps_drift_s::hpos_drift_rate, hrt_abstime, ekf_gps_drift_s::hspd, vehicle_land_detected_s::in_ground_effect, airspeed_s::indicated_airspeed_m_s, EstimatorInterface::inertial_dead_reckoning(), estimator_status_s::innovation_check_flags, optical_flow_s::integration_timespan, is_fixed_wing(), landing_target_pose_s::is_static, vehicle_land_detected_s::landed, ekf_gps_position_s::lat, vehicle_global_position_s::lat, estimator::gps_message::lat, vehicle_global_position_s::lat_lon_reset_counter, map_projection_reference_s::lat_rad, EstimatorInterface::local_position_is_valid(), ekf_gps_position_s::lon, vehicle_global_position_s::lon, estimator::gps_message::lon, map_projection_reference_s::lon_rad, M_PI, estimator::filter_control_status_u::mag_3D, sensor_selection_s::mag_device_id, estimator::filter_control_status_u::mag_field_disturbed, ekf2_innovations_s::mag_innov, ekf2_innovations_s::mag_innov_var, estimator_status_s::mag_test_ratio, vehicle_magnetometer_s::magnetometer_ga, map_projection_reproject(), distance_sensor_s::max_distance, optical_flow_s::max_flow_rate, optical_flow_s::max_ground_distance, distance_sensor_s::min_distance, optical_flow_s::min_ground_distance, estimator_status_s::n_states, estimator::gps_message::nsats, ekf2_innovations_s::output_tracking_error, perf_begin(), perf_end(), optical_flow_s::pixel_flow_x_integral, optical_flow_s::pixel_flow_y_integral, estimator::ext_vision_message::pos, estimator_status_s::pos_horiz_accuracy, estimator_status_s::pos_test_ratio, estimator_status_s::pos_vert_accuracy, vehicle_odometry_s::pose_covariance, estimator::ext_vision_message::posErr, position, estimator_status_s::pre_flt_fail_innov_heading, estimator_status_s::pre_flt_fail_innov_height, estimator_status_s::pre_flt_fail_innov_vel_horiz, estimator_status_s::pre_flt_fail_innov_vel_vert, estimator_status_s::pre_flt_fail_mag_field_disturbed, uORB::Publication< T >::publish(), publish_attitude(), publish_wind_estimate(), vehicle_odometry_s::q, vehicle_odometry_s::q_offset, optical_flow_s::quality, estimator::flow_message::quality, estimator::ext_vision_message::quat, vehicle_local_position_s::ref_alt, vehicle_local_position_s::ref_lat, vehicle_local_position_s::ref_lon, vehicle_local_position_s::ref_timestamp, landing_target_pose_s::rel_vel_valid, Ekf::reset_imu_bias(), resetPreFlightChecks(), vehicle_air_data_s::rho, runPreFlightChecks(), ekf_gps_position_s::s_variance_m_s, estimator::gps_message::sacc, ekf_gps_position_s::satellites_used, ekf_gps_position_s::selected, estimator::parameters::sensor_interval_min_ms, EstimatorInterface::set_air_density(), EstimatorInterface::set_fuse_beta_flag(), EstimatorInterface::set_gnd_effect_flag(), EstimatorInterface::set_in_air_status(), EstimatorInterface::set_is_fixed_wing(), EstimatorInterface::set_optical_flow_limits(), EstimatorInterface::set_rangefinder_limits(), EstimatorInterface::setAirspeedData(), EstimatorInterface::setAuxVelData(), EstimatorInterface::setBaroData(), EstimatorInterface::setExtVisionData(), EstimatorInterface::setGpsData(), EstimatorInterface::setIMUData(), EstimatorInterface::setMagData(), EstimatorInterface::setOpticalFlowData(), EstimatorInterface::setRangeData(), distance_sensor_s::signal_quality, estimator_status_s::solution_status_flags, estimator_status_s::states, status, estimator_status_s::tas_test_ratio, vehicle_global_position_s::terrain_alt, vehicle_global_position_s::terrain_alt_valid, estimator::filter_control_status_u::tilt_align, estimator_status_s::time_slip, estimator::imuSample::time_us, estimator::gps_message::time_usec, estimator_status_s::timeout_flags, vehicle_air_data_s::timestamp, ekf_gps_position_s::timestamp, vehicle_gps_position_s::timestamp, ekf_gps_drift_s::timestamp, landing_target_pose_s::timestamp, optical_flow_s::timestamp, vehicle_magnetometer_s::timestamp, sensor_bias_s::timestamp, vehicle_local_position_s::timestamp, airspeed_s::timestamp, ekf2_innovations_s::timestamp, sensor_selection_s::timestamp, vehicle_global_position_s::timestamp, sensor_combined_s::timestamp, ekf2_timestamps_s::timestamp, vehicle_odometry_s::timestamp, distance_sensor_s::timestamp, estimator_status_s::timestamp, airspeed_s::true_airspeed_m_s, uORB::SubscriptionCallback::unregisterCallback(), Ekf::update(), uORB::SubscriptionInterval::update(), uORB::Subscription::update(), uORB::PublicationData< T >::update(), update_mag_bias(), update_mag_decl(), uORB::Subscription::updated(), vehicle_local_position_s::v_xy_valid, vehicle_local_position_s::v_z_valid, estimator::filter_control_status_u::value, vehicle_status_s::vehicle_type, VEHICLE_TYPE_FIXED_WING, estimator::ext_vision_message::vel, vehicle_global_position_s::vel_d, ekf_gps_position_s::vel_d_m_s, vehicle_global_position_s::vel_e, ekf_gps_position_s::vel_e_m_s, ekf_gps_position_s::vel_m_s, estimator::gps_message::vel_m_s, vehicle_global_position_s::vel_n, ekf_gps_position_s::vel_n_m_s, estimator::gps_message::vel_ned, ekf_gps_position_s::vel_ned_valid, estimator::gps_message::vel_ned_valid, ekf2_innovations_s::vel_pos_innov, ekf2_innovations_s::vel_pos_innov_var, estimator_status_s::vel_test_ratio, estimator::ext_vision_message::velErr, velocity, estimator_status_s::vibe, ekf_gps_drift_s::vpos_drift_rate, vehicle_local_position_s::vx, vehicle_odometry_s::vx, landing_target_pose_s::vx_rel, vehicle_local_position_s::vxy_max, vehicle_local_position_s::vxy_reset_counter, vehicle_local_position_s::vy, vehicle_odometry_s::vy, landing_target_pose_s::vy_rel, vehicle_local_position_s::vz, vehicle_odometry_s::vz, vehicle_local_position_s::vz_max, vehicle_local_position_s::vz_reset_counter, vehicle_local_position_s::x, vehicle_odometry_s::x, vehicle_local_position_s::xy_global, vehicle_local_position_s::xy_reset_counter, vehicle_local_position_s::xy_valid, vehicle_local_position_s::y, vehicle_odometry_s::y, vehicle_global_position_s::yaw, estimator::gps_message::yaw, vehicle_local_position_s::yaw, estimator::gps_message::yaw_offset, vehicle_local_position_s::z, vehicle_odometry_s::z, vehicle_local_position_s::z_deriv, vehicle_local_position_s::z_global, vehicle_local_position_s::z_reset_counter, and vehicle_local_position_s::z_valid.
|
private |
Definition at line 1667 of file ekf2_main.cpp.
References estimator::filter_control_status_u::ev_pos, estimator::filter_control_status_u::flags, estimator::filter_control_status_u::gps, estimator::filter_control_status_u::opt_flow, PreFlightChecker::setUsingEvPosAiding(), PreFlightChecker::setUsingFlowAiding(), PreFlightChecker::setUsingGpsAiding(), PreFlightChecker::setVehicleCanObserveHeadingInFlight(), PreFlightChecker::update(), and vehicle_status_s::vehicle_type.
Referenced by Run().
|
static |
Definition at line 2356 of file ekf2_main.cpp.
References Ekf2(), init(), and ll40ls::instance.
|
private |
Definition at line 2017 of file ekf2_main.cpp.
References _gps_blended_state, _gps_state, add_vector_to_global_position(), estimator::gps_message::alt, estimator::gps_message::eph, estimator::gps_message::epv, f(), estimator::gps_message::fix_type, get_vector_to_next_waypoint(), GPS_MAX_RECEIVERS, estimator::gps_message::lat, estimator::gps_message::lon, estimator::gps_message::nsats, estimator::gps_message::pdop, estimator::gps_message::sacc, estimator::gps_message::time_usec, estimator::gps_message::vel_m_s, estimator::gps_message::vel_ned, estimator::gps_message::vel_ned_valid, estimator::gps_message::yaw, and estimator::gps_message::yaw_offset.
Referenced by blend_gps_data().
|
private |
Definition at line 2168 of file ekf2_main.cpp.
References _gps_blended_state, _gps_state, estimator::gps_message::alt, math::constrain(), f(), get_vector_to_next_waypoint(), GPS_MAX_RECEIVERS, estimator::gps_message::lat, estimator::gps_message::lon, and estimator::gps_message::time_usec.
Referenced by blend_gps_data().
|
private |
Definition at line 670 of file ekf2_main.cpp.
References math::constrain(), and f().
Referenced by Run().
|
private |
Definition at line 689 of file ekf2_main.cpp.
References EstimatorInterface::get_mag_decl_deg().
Referenced by Run().
|
private |
Definition at line 246 of file ekf2_main.cpp.
Referenced by Run().
|
private |
Definition at line 247 of file ekf2_main.cpp.
Referenced by Run().
|
private |
Definition at line 276 of file ekf2_main.cpp.
Referenced by publish_attitude().
|
private |
summed pressure altitude readings (m)
Definition at line 193 of file ekf2_main.cpp.
|
private |
number of barometric altitude measurements summed
Definition at line 195 of file ekf2_main.cpp.
Referenced by Run().
|
private |
time stamp of the last averaged barometric altitude measurement sent to the EKF (mSec)
Definition at line 196 of file ekf2_main.cpp.
|
private |
summed pressure altitude time stamps (mSec)
Definition at line 194 of file ekf2_main.cpp.
|
private |
blend weight for each GPS. The blend weights must sum to 1.0 across all instances.
Definition at line 223 of file ekf2_main.cpp.
|
private |
blended antenna offset
Definition at line 222 of file ekf2_main.cpp.
|
private |
Definition at line 273 of file ekf2_main.cpp.
Referenced by Run().
|
private |
Definition at line 283 of file ekf2_main.cpp.
|
private |
Definition at line 271 of file ekf2_main.cpp.
Referenced by Run().
|
private |
Definition at line 272 of file ekf2_main.cpp.
Referenced by Run().
|
private |
Definition at line 181 of file ekf2_main.cpp.
|
private |
Definition at line 270 of file ekf2_main.cpp.
Referenced by Run().
|
private |
Definition at line 274 of file ekf2_main.cpp.
Referenced by Run().
|
private |
Definition at line 244 of file ekf2_main.cpp.
Referenced by Run().
|
private |
Definition at line 248 of file ekf2_main.cpp.
Referenced by Run().
|
private |
altitude in 1E-3 meters (millimeters) above ellipsoid
Definition at line 236 of file ekf2_main.cpp.
Referenced by filter_altitude_ellipsoid(), and Run().
|
private |
storage for previous timestamp to compute dt
Definition at line 237 of file ekf2_main.cpp.
Referenced by filter_altitude_ellipsoid().
|
private |
index of the physical receiver with the lowest reported error
Definition at line 225 of file ekf2_main.cpp.
|
private |
internal state data for the blended GPS
Definition at line 218 of file ekf2_main.cpp.
Referenced by blend_gps_data(), calc_gps_blend_output(), update_gps_blend_states(), and update_gps_offsets().
|
private |
average time step in seconds.
Definition at line 232 of file ekf2_main.cpp.
|
private |
true if there is new output data for the EKF
Definition at line 233 of file ekf2_main.cpp.
|
private |
index of the physical receiver with the newest data
Definition at line 230 of file ekf2_main.cpp.
Referenced by blend_gps_data().
|
private |
index of the physical receiver with the oldest data
Definition at line 229 of file ekf2_main.cpp.
|
private |
output state data for the physical and blended GPS
Definition at line 219 of file ekf2_main.cpp.
Referenced by apply_gps_offsets(), calc_gps_blend_output(), and Run().
|
private |
|
private |
index of the physical receiver with the slowest update rate
Definition at line 231 of file ekf2_main.cpp.
Referenced by blend_gps_data().
|
private |
internal state data for the physical GPS
Definition at line 217 of file ekf2_main.cpp.
Referenced by apply_gps_offsets(), blend_gps_data(), filter_altitude_ellipsoid(), Run(), update_gps_blend_states(), and update_gps_offsets().
|
private |
Definition at line 264 of file ekf2_main.cpp.
Referenced by Run().
|
private |
index of the receiver that is used as the timing reference for the blending update
Definition at line 227 of file ekf2_main.cpp.
Referenced by blend_gps_data().
|
private |
true if at any time there was a valid terrain estimate
Definition at line 234 of file ekf2_main.cpp.
|
private |
Filtered height offset from GPS instance relative to blended solution in _output_state.location (mm)
Definition at line 221 of file ekf2_main.cpp.
|
private |
Definition at line 240 of file ekf2_main.cpp.
Referenced by Run().
|
private |
integral of gyro delta time from start (uSec)
Definition at line 177 of file ekf2_main.cpp.
|
private |
number of times an invalid magnetomer device ID has been detected
Definition at line 184 of file ekf2_main.cpp.
|
private |
Definition at line 249 of file ekf2_main.cpp.
Referenced by Run().
|
private |
last time the EKF was operating a mode that estimates magnetomer biases (uSec)
Definition at line 200 of file ekf2_main.cpp.
Referenced by Run().
|
private |
Last time slip (uSec)
Definition at line 179 of file ekf2_main.cpp.
|
private |
last valid XYZ magnetometer bias estimates (mGauss)
Definition at line 203 of file ekf2_main.cpp.
|
private |
variances for the last valid magnetometer XYZ bias estimates (mGauss**2)
Definition at line 205 of file ekf2_main.cpp.
|
private |
summed magnetometer readings (Gauss)
Definition at line 187 of file ekf2_main.cpp.
|
private |
true when the magnetic declination has been saved
Definition at line 208 of file ekf2_main.cpp.
|
private |
number of magnetometer measurements summed during downsampling
Definition at line 189 of file ekf2_main.cpp.
Referenced by Run().
|
private |
time stamp of the last averaged magnetometer measurement sent to the EKF (mSec)
Definition at line 190 of file ekf2_main.cpp.
|
private |
summed magnetoemter time stamps (mSec)
Definition at line 188 of file ekf2_main.cpp.
|
private |
Definition at line 250 of file ekf2_main.cpp.
Referenced by Run().
|
private |
Filtered North,East position offset from GPS instance to blended solution in _output_state.location (m)
Definition at line 220 of file ekf2_main.cpp.
|
private |
Definition at line 251 of file ekf2_main.cpp.
Referenced by Run().
|
private |
Definition at line 252 of file ekf2_main.cpp.
Referenced by Run().
|
private |
pointer to ekf parameter struct (located in _ekf class instance)
Definition at line 285 of file ekf2_main.cpp.
|
private |
Definition at line 120 of file ekf2_main.cpp.
|
private |
Definition at line 261 of file ekf2_main.cpp.
|
private |
Definition at line 260 of file ekf2_main.cpp.
Referenced by getRangeSubIndex(), and Run().
|
private |
true when we use replay data from a log
Definition at line 174 of file ekf2_main.cpp.
|
private |
Definition at line 275 of file ekf2_main.cpp.
Referenced by Run().
|
private |
Definition at line 266 of file ekf2_main.cpp.
Referenced by Run().
|
private |
Definition at line 253 of file ekf2_main.cpp.
Referenced by Run().
|
private |
Definition at line 257 of file ekf2_main.cpp.
|
private |
|
private |
Definition at line 254 of file ekf2_main.cpp.
Referenced by Run().
|
private |
the previous value of time_us for that GPS instance - used to detect new data.
Definition at line 224 of file ekf2_main.cpp.
|
private |
accumulated calibration time since the last save
Definition at line 201 of file ekf2_main.cpp.
|
private |
true when an unsaved valid calibration for the XYZ magnetometer bias is available
Definition at line 204 of file ekf2_main.cpp.
|
private |
Definition at line 279 of file ekf2_main.cpp.
Referenced by Run().
|
private |
Definition at line 267 of file ekf2_main.cpp.
Referenced by Run().
|
private |
Definition at line 255 of file ekf2_main.cpp.
Referenced by Run().
|
private |
Definition at line 280 of file ekf2_main.cpp.
Referenced by Run().
|
private |
Definition at line 277 of file ekf2_main.cpp.
Referenced by Run().
|
private |
Definition at line 268 of file ekf2_main.cpp.
Referenced by Run().
|
private |
Definition at line 281 of file ekf2_main.cpp.
Referenced by Run().
|
private |
height offset between AMSL and WGS84
Definition at line 238 of file ekf2_main.cpp.
Referenced by filter_altitude_ellipsoid().
|
private |
Definition at line 278 of file ekf2_main.cpp.
Referenced by publish_wind_estimate().
|
staticprivate |
Maximum permissible standard deviation for estimated orientation.
Definition at line 213 of file ekf2_main.cpp.
|
staticprivate |
Maximum permissible standard deviation for estimated position.
Definition at line 212 of file ekf2_main.cpp.
|
private |
Definition at line 243 of file ekf2_main.cpp.