diff --git a/msg/estimator_status_flags.msg b/msg/estimator_status_flags.msg index 8ddaee34a7..2e9c737119 100644 --- a/msg/estimator_status_flags.msg +++ b/msg/estimator_status_flags.msg @@ -3,60 +3,60 @@ uint64 timestamp_sample # the timestamp of the raw data (micro # filter control status -uint32 control_status_changes # number of filter control status changes -bool control_status_tilt_align # 0 - true if the filter tilt alignment is complete -bool control_status_yaw_align # 1 - true if the filter yaw alignment is complete -bool control_status_gps # 2 - true if GPS measurement fusion is intended -bool control_status_opt_flow # 3 - true if optical flow measurements fusion is intended -bool control_status_mag_hdg # 4 - true if a simple magnetic yaw heading fusion is intended -bool control_status_mag_3d # 5 - true if 3-axis magnetometer measurement fusion is inteded -bool control_status_mag_dec # 6 - true if synthetic magnetic declination measurements fusion is intended -bool control_status_in_air # 7 - true when the vehicle is airborne -bool control_status_wind # 8 - true when wind velocity is being estimated -bool control_status_baro_hgt # 9 - true when baro height is being fused as a primary height reference -bool control_status_rng_hgt # 10 - true when range finder height is being fused as a primary height reference -bool control_status_gps_hgt # 11 - true when GPS height is being fused as a primary height reference -bool control_status_ev_pos # 12 - true when local position data fusion from external vision is intended -bool control_status_ev_yaw # 13 - true when yaw data from external vision measurements fusion is intended -bool control_status_ev_hgt # 14 - true when height data from external vision measurements is being fused -bool control_status_fuse_beta # 15 - true when synthetic sideslip measurements are being fused -bool control_status_mag_field_disturbed # 16 - true when the mag field does not match the expected strength -bool control_status_fixed_wing # 17 - true when the vehicle is operating as a fixed wing vehicle -bool control_status_mag_fault # 18 - true when the magnetometer has been declared faulty and is no longer being used -bool control_status_fuse_aspd # 19 - true when airspeed measurements are being fused -bool control_status_gnd_effect # 20 - true when protection from ground effect induced static pressure rise is active -bool control_status_rng_stuck # 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough -bool control_status_gps_yaw # 22 - true when yaw (not ground course) data fusion from a GPS receiver is intended -bool control_status_mag_aligned_in_flight # 23 - true when the in-flight mag field alignment has been completed -bool control_status_ev_vel # 24 - true when local frame velocity data fusion from external vision measurements is intended -bool control_status_synthetic_mag_z # 25 - true when we are using a synthesized measurement for the magnetometer Z component -bool control_status_vehicle_at_rest # 26 - true when the vehicle is at rest +uint32 control_status_changes # number of filter control status (cs) changes +bool cs_tilt_align # 0 - true if the filter tilt alignment is complete +bool cs_yaw_align # 1 - true if the filter yaw alignment is complete +bool cs_gps # 2 - true if GPS measurement fusion is intended +bool cs_opt_flow # 3 - true if optical flow measurements fusion is intended +bool cs_mag_hdg # 4 - true if a simple magnetic yaw heading fusion is intended +bool cs_mag_3d # 5 - true if 3-axis magnetometer measurement fusion is inteded +bool cs_mag_dec # 6 - true if synthetic magnetic declination measurements fusion is intended +bool cs_in_air # 7 - true when the vehicle is airborne +bool cs_wind # 8 - true when wind velocity is being estimated +bool cs_baro_hgt # 9 - true when baro height is being fused as a primary height reference +bool cs_rng_hgt # 10 - true when range finder height is being fused as a primary height reference +bool cs_gps_hgt # 11 - true when GPS height is being fused as a primary height reference +bool cs_ev_pos # 12 - true when local position data fusion from external vision is intended +bool cs_ev_yaw # 13 - true when yaw data from external vision measurements fusion is intended +bool cs_ev_hgt # 14 - true when height data from external vision measurements is being fused +bool cs_fuse_beta # 15 - true when synthetic sideslip measurements are being fused +bool cs_mag_field_disturbed # 16 - true when the mag field does not match the expected strength +bool cs_fixed_wing # 17 - true when the vehicle is operating as a fixed wing vehicle +bool cs_mag_fault # 18 - true when the magnetometer has been declared faulty and is no longer being used +bool cs_fuse_aspd # 19 - true when airspeed measurements are being fused +bool cs_gnd_effect # 20 - true when protection from ground effect induced static pressure rise is active +bool cs_rng_stuck # 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough +bool cs_gps_yaw # 22 - true when yaw (not ground course) data fusion from a GPS receiver is intended +bool cs_mag_aligned_in_flight # 23 - true when the in-flight mag field alignment has been completed +bool cs_ev_vel # 24 - true when local frame velocity data fusion from external vision measurements is intended +bool cs_synthetic_mag_z # 25 - true when we are using a synthesized measurement for the magnetometer Z component +bool cs_vehicle_at_rest # 26 - true when the vehicle is at rest # fault status -uint32 fault_status_changes # number of filter fault status changes -bool fault_status_bad_mag_x # 0 - true if the fusion of the magnetometer X-axis has encountered a numerical error -bool fault_status_bad_mag_y # 1 - true if the fusion of the magnetometer Y-axis has encountered a numerical error -bool fault_status_bad_mag_z # 2 - true if the fusion of the magnetometer Z-axis has encountered a numerical error -bool fault_status_bad_hdg # 3 - true if the fusion of the heading angle has encountered a numerical error -bool fault_status_bad_mag_decl # 4 - true if the fusion of the magnetic declination has encountered a numerical error -bool fault_status_bad_airspeed # 5 - true if fusion of the airspeed has encountered a numerical error -bool fault_status_bad_sideslip # 6 - true if fusion of the synthetic sideslip constraint has encountered a numerical error -bool fault_status_bad_optflow_x # 7 - true if fusion of the optical flow X axis has encountered a numerical error -bool fault_status_bad_optflow_y # 8 - true if fusion of the optical flow Y axis has encountered a numerical error -bool fault_status_bad_vel_n # 9 - true if fusion of the North velocity has encountered a numerical error -bool fault_status_bad_vel_e # 10 - true if fusion of the East velocity has encountered a numerical error -bool fault_status_bad_vel_d # 11 - true if fusion of the Down velocity has encountered a numerical error -bool fault_status_bad_pos_n # 12 - true if fusion of the North position has encountered a numerical error -bool fault_status_bad_pos_e # 13 - true if fusion of the East position has encountered a numerical error -bool fault_status_bad_pos_d # 14 - true if fusion of the Down position has encountered a numerical error -bool fault_status_bad_acc_bias # 15 - true if bad delta velocity bias estimates have been detected -bool fault_status_bad_acc_vertical # 16 - true if bad vertical accelerometer data has been detected -bool fault_status_bad_acc_clipping # 17 - true if delta velocity data contains clipping (asymmetric railing) +uint32 fault_status_changes # number of filter fault status (fs) changes +bool fs_bad_mag_x # 0 - true if the fusion of the magnetometer X-axis has encountered a numerical error +bool fs_bad_mag_y # 1 - true if the fusion of the magnetometer Y-axis has encountered a numerical error +bool fs_bad_mag_z # 2 - true if the fusion of the magnetometer Z-axis has encountered a numerical error +bool fs_bad_hdg # 3 - true if the fusion of the heading angle has encountered a numerical error +bool fs_bad_mag_decl # 4 - true if the fusion of the magnetic declination has encountered a numerical error +bool fs_bad_airspeed # 5 - true if fusion of the airspeed has encountered a numerical error +bool fs_bad_sideslip # 6 - true if fusion of the synthetic sideslip constraint has encountered a numerical error +bool fs_bad_optflow_x # 7 - true if fusion of the optical flow X axis has encountered a numerical error +bool fs_bad_optflow_y # 8 - true if fusion of the optical flow Y axis has encountered a numerical error +bool fs_bad_vel_n # 9 - true if fusion of the North velocity has encountered a numerical error +bool fs_bad_vel_e # 10 - true if fusion of the East velocity has encountered a numerical error +bool fs_bad_vel_d # 11 - true if fusion of the Down velocity has encountered a numerical error +bool fs_bad_pos_n # 12 - true if fusion of the North position has encountered a numerical error +bool fs_bad_pos_e # 13 - true if fusion of the East position has encountered a numerical error +bool fs_bad_pos_d # 14 - true if fusion of the Down position has encountered a numerical error +bool fs_bad_acc_bias # 15 - true if bad delta velocity bias estimates have been detected +bool fs_bad_acc_vertical # 16 - true if bad vertical accelerometer data has been detected +bool fs_bad_acc_clipping # 17 - true if delta velocity data contains clipping (asymmetric railing) # innovation test failures -uint32 innovation_fault_status_changes # number of filter fault status changes +uint32 innovation_fault_status_changes # number of innovation fault status (reject) changes bool reject_hor_vel # 0 - true if horizontal velocity observations have been rejected bool reject_ver_vel # 1 - true if vertical velocity observations have been rejected bool reject_hor_pos # 2 - true if horizontal position observations have been rejected diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 7d422e019b..98d9bbb44d 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -996,69 +996,69 @@ void EKF2::PublishStatusFlags(const hrt_abstime ×tamp) estimator_status_flags_s status_flags{}; status_flags.timestamp_sample = timestamp; - status_flags.control_status_changes = _filter_control_status_changes; - status_flags.control_status_tilt_align = _ekf.control_status_flags().tilt_align; - status_flags.control_status_yaw_align = _ekf.control_status_flags().yaw_align; - status_flags.control_status_gps = _ekf.control_status_flags().gps; - status_flags.control_status_opt_flow = _ekf.control_status_flags().opt_flow; - status_flags.control_status_mag_hdg = _ekf.control_status_flags().mag_hdg; - status_flags.control_status_mag_3d = _ekf.control_status_flags().mag_3D; - status_flags.control_status_mag_dec = _ekf.control_status_flags().mag_dec; - status_flags.control_status_in_air = _ekf.control_status_flags().in_air; - status_flags.control_status_wind = _ekf.control_status_flags().wind; - status_flags.control_status_baro_hgt = _ekf.control_status_flags().baro_hgt; - status_flags.control_status_rng_hgt = _ekf.control_status_flags().rng_hgt; - status_flags.control_status_gps_hgt = _ekf.control_status_flags().gps_hgt; - status_flags.control_status_ev_pos = _ekf.control_status_flags().ev_pos; - status_flags.control_status_ev_yaw = _ekf.control_status_flags().ev_yaw; - status_flags.control_status_ev_hgt = _ekf.control_status_flags().ev_hgt; - status_flags.control_status_fuse_beta = _ekf.control_status_flags().fuse_beta; - status_flags.control_status_mag_field_disturbed = _ekf.control_status_flags().mag_field_disturbed; - status_flags.control_status_fixed_wing = _ekf.control_status_flags().fixed_wing; - status_flags.control_status_mag_fault = _ekf.control_status_flags().mag_fault; - status_flags.control_status_fuse_aspd = _ekf.control_status_flags().fuse_aspd; - status_flags.control_status_gnd_effect = _ekf.control_status_flags().gnd_effect; - status_flags.control_status_rng_stuck = _ekf.control_status_flags().rng_stuck; - status_flags.control_status_gps_yaw = _ekf.control_status_flags().gps_yaw; - status_flags.control_status_mag_aligned_in_flight = _ekf.control_status_flags().mag_aligned_in_flight; - status_flags.control_status_ev_vel = _ekf.control_status_flags().ev_vel; - status_flags.control_status_synthetic_mag_z = _ekf.control_status_flags().synthetic_mag_z; - status_flags.control_status_vehicle_at_rest = _ekf.control_status_flags().vehicle_at_rest; + status_flags.control_status_changes = _filter_control_status_changes; + status_flags.cs_tilt_align = _ekf.control_status_flags().tilt_align; + status_flags.cs_yaw_align = _ekf.control_status_flags().yaw_align; + status_flags.cs_gps = _ekf.control_status_flags().gps; + status_flags.cs_opt_flow = _ekf.control_status_flags().opt_flow; + status_flags.cs_mag_hdg = _ekf.control_status_flags().mag_hdg; + status_flags.cs_mag_3d = _ekf.control_status_flags().mag_3D; + status_flags.cs_mag_dec = _ekf.control_status_flags().mag_dec; + status_flags.cs_in_air = _ekf.control_status_flags().in_air; + status_flags.cs_wind = _ekf.control_status_flags().wind; + status_flags.cs_baro_hgt = _ekf.control_status_flags().baro_hgt; + status_flags.cs_rng_hgt = _ekf.control_status_flags().rng_hgt; + status_flags.cs_gps_hgt = _ekf.control_status_flags().gps_hgt; + status_flags.cs_ev_pos = _ekf.control_status_flags().ev_pos; + status_flags.cs_ev_yaw = _ekf.control_status_flags().ev_yaw; + status_flags.cs_ev_hgt = _ekf.control_status_flags().ev_hgt; + status_flags.cs_fuse_beta = _ekf.control_status_flags().fuse_beta; + status_flags.cs_mag_field_disturbed = _ekf.control_status_flags().mag_field_disturbed; + status_flags.cs_fixed_wing = _ekf.control_status_flags().fixed_wing; + status_flags.cs_mag_fault = _ekf.control_status_flags().mag_fault; + status_flags.cs_fuse_aspd = _ekf.control_status_flags().fuse_aspd; + status_flags.cs_gnd_effect = _ekf.control_status_flags().gnd_effect; + status_flags.cs_rng_stuck = _ekf.control_status_flags().rng_stuck; + status_flags.cs_gps_yaw = _ekf.control_status_flags().gps_yaw; + status_flags.cs_mag_aligned_in_flight = _ekf.control_status_flags().mag_aligned_in_flight; + status_flags.cs_ev_vel = _ekf.control_status_flags().ev_vel; + status_flags.cs_synthetic_mag_z = _ekf.control_status_flags().synthetic_mag_z; + status_flags.cs_vehicle_at_rest = _ekf.control_status_flags().vehicle_at_rest; - status_flags.fault_status_changes = _filter_fault_status_changes; - status_flags.fault_status_bad_mag_x = _ekf.fault_status_flags().bad_mag_x; - status_flags.fault_status_bad_mag_y = _ekf.fault_status_flags().bad_mag_y; - status_flags.fault_status_bad_mag_z = _ekf.fault_status_flags().bad_mag_z; - status_flags.fault_status_bad_hdg = _ekf.fault_status_flags().bad_hdg; - status_flags.fault_status_bad_mag_decl = _ekf.fault_status_flags().bad_mag_decl; - status_flags.fault_status_bad_airspeed = _ekf.fault_status_flags().bad_airspeed; - status_flags.fault_status_bad_sideslip = _ekf.fault_status_flags().bad_sideslip; - status_flags.fault_status_bad_optflow_x = _ekf.fault_status_flags().bad_optflow_X; - status_flags.fault_status_bad_optflow_y = _ekf.fault_status_flags().bad_optflow_Y; - status_flags.fault_status_bad_vel_n = _ekf.fault_status_flags().bad_vel_N; - status_flags.fault_status_bad_vel_e = _ekf.fault_status_flags().bad_vel_E; - status_flags.fault_status_bad_vel_d = _ekf.fault_status_flags().bad_vel_D; - status_flags.fault_status_bad_pos_n = _ekf.fault_status_flags().bad_pos_N; - status_flags.fault_status_bad_pos_e = _ekf.fault_status_flags().bad_pos_E; - status_flags.fault_status_bad_pos_d = _ekf.fault_status_flags().bad_pos_D; - status_flags.fault_status_bad_acc_bias = _ekf.fault_status_flags().bad_acc_bias; - status_flags.fault_status_bad_acc_vertical = _ekf.fault_status_flags().bad_acc_vertical; - status_flags.fault_status_bad_acc_clipping = _ekf.fault_status_flags().bad_acc_clipping; + status_flags.fault_status_changes = _filter_fault_status_changes; + status_flags.fs_bad_mag_x = _ekf.fault_status_flags().bad_mag_x; + status_flags.fs_bad_mag_y = _ekf.fault_status_flags().bad_mag_y; + status_flags.fs_bad_mag_z = _ekf.fault_status_flags().bad_mag_z; + status_flags.fs_bad_hdg = _ekf.fault_status_flags().bad_hdg; + status_flags.fs_bad_mag_decl = _ekf.fault_status_flags().bad_mag_decl; + status_flags.fs_bad_airspeed = _ekf.fault_status_flags().bad_airspeed; + status_flags.fs_bad_sideslip = _ekf.fault_status_flags().bad_sideslip; + status_flags.fs_bad_optflow_x = _ekf.fault_status_flags().bad_optflow_X; + status_flags.fs_bad_optflow_y = _ekf.fault_status_flags().bad_optflow_Y; + status_flags.fs_bad_vel_n = _ekf.fault_status_flags().bad_vel_N; + status_flags.fs_bad_vel_e = _ekf.fault_status_flags().bad_vel_E; + status_flags.fs_bad_vel_d = _ekf.fault_status_flags().bad_vel_D; + status_flags.fs_bad_pos_n = _ekf.fault_status_flags().bad_pos_N; + status_flags.fs_bad_pos_e = _ekf.fault_status_flags().bad_pos_E; + status_flags.fs_bad_pos_d = _ekf.fault_status_flags().bad_pos_D; + status_flags.fs_bad_acc_bias = _ekf.fault_status_flags().bad_acc_bias; + status_flags.fs_bad_acc_vertical = _ekf.fault_status_flags().bad_acc_vertical; + status_flags.fs_bad_acc_clipping = _ekf.fault_status_flags().bad_acc_clipping; - status_flags.innovation_fault_status_changes = _innov_check_fail_status_changes; - status_flags.reject_hor_vel = _ekf.innov_check_fail_status_flags().reject_hor_vel; - status_flags.reject_ver_vel = _ekf.innov_check_fail_status_flags().reject_ver_vel; - status_flags.reject_hor_pos = _ekf.innov_check_fail_status_flags().reject_hor_pos; - status_flags.reject_ver_pos = _ekf.innov_check_fail_status_flags().reject_ver_pos; - status_flags.reject_mag_x = _ekf.innov_check_fail_status_flags().reject_mag_x; - status_flags.reject_mag_y = _ekf.innov_check_fail_status_flags().reject_mag_y; - status_flags.reject_mag_z = _ekf.innov_check_fail_status_flags().reject_mag_z; - status_flags.reject_yaw = _ekf.innov_check_fail_status_flags().reject_yaw; - status_flags.reject_airspeed = _ekf.innov_check_fail_status_flags().reject_airspeed; - status_flags.reject_sideslip = _ekf.innov_check_fail_status_flags().reject_sideslip; - status_flags.reject_hagl = _ekf.innov_check_fail_status_flags().reject_hagl; - status_flags.reject_optflow_x = _ekf.innov_check_fail_status_flags().reject_optflow_X; - status_flags.reject_optflow_y = _ekf.innov_check_fail_status_flags().reject_optflow_Y; + status_flags.innovation_fault_status_changes = _innov_check_fail_status_changes; + status_flags.reject_hor_vel = _ekf.innov_check_fail_status_flags().reject_hor_vel; + status_flags.reject_ver_vel = _ekf.innov_check_fail_status_flags().reject_ver_vel; + status_flags.reject_hor_pos = _ekf.innov_check_fail_status_flags().reject_hor_pos; + status_flags.reject_ver_pos = _ekf.innov_check_fail_status_flags().reject_ver_pos; + status_flags.reject_mag_x = _ekf.innov_check_fail_status_flags().reject_mag_x; + status_flags.reject_mag_y = _ekf.innov_check_fail_status_flags().reject_mag_y; + status_flags.reject_mag_z = _ekf.innov_check_fail_status_flags().reject_mag_z; + status_flags.reject_yaw = _ekf.innov_check_fail_status_flags().reject_yaw; + status_flags.reject_airspeed = _ekf.innov_check_fail_status_flags().reject_airspeed; + status_flags.reject_sideslip = _ekf.innov_check_fail_status_flags().reject_sideslip; + status_flags.reject_hagl = _ekf.innov_check_fail_status_flags().reject_hagl; + status_flags.reject_optflow_x = _ekf.innov_check_fail_status_flags().reject_optflow_X; + status_flags.reject_optflow_y = _ekf.innov_check_fail_status_flags().reject_optflow_Y; status_flags.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); _estimator_status_flags_pub.publish(status_flags);