forked from Archive/PX4-Autopilot
msg: new estimator_status_flags message for more accessible ekf2 status logging
- log all estimator (ekf2) flags as separate booleans in a new dedicated low rate message (only publishes at 1 Hz or immediately on any change) - this is a bit verbose, but it avoids the duplicate bit definitions we currently have across PX4 msgs, ecl analysis script, flight review, and many other custom tools and it's much easier for casual log review in FlightPlot, PlotJuggler, csv, etc - for compatibility I've left estimator_status filter_fault_flags, innovation_check_flags, and solution_status_flags in place, but they can gradually be removed as tooling is updated Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com>
This commit is contained in:
Daniel Agar
GitHub
parent
ddc1f964d2
commit
4f62355dec
|
|
@ -837,6 +837,7 @@ void statusFTDI() {
|
|||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener estimator_selector_status"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener estimator_sensor_bias"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener estimator_status"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener estimator_status_flags"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener logger_status"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener sensor_accel"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener sensor_accel_fifo"'
|
||||
|
|
@ -915,6 +916,7 @@ void statusSEGGER() {
|
|||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener estimator_selector_status"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener estimator_sensor_bias"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener estimator_status"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener estimator_status_flags"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener logger_status"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener sensor_accel"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener sensor_accel_fifo"'
|
||||
|
|
|
|||
|
|
@ -61,6 +61,7 @@ set(msg_files
|
|||
estimator_sensor_bias.msg
|
||||
estimator_states.msg
|
||||
estimator_status.msg
|
||||
estimator_status_flags.msg
|
||||
follow_target.msg
|
||||
generator_status.msg
|
||||
geofence_result.msg
|
||||
|
|
|
|||
|
|
@ -0,0 +1,72 @@
|
|||
uint64 timestamp # time since system start (microseconds)
|
||||
uint64 timestamp_sample # the timestamp of the raw data (microseconds)
|
||||
|
||||
|
||||
# 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
|
||||
|
||||
|
||||
# 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)
|
||||
|
||||
|
||||
# innovation test failures
|
||||
uint32 innovation_fault_status_changes # number of filter fault status 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
|
||||
bool reject_ver_pos # 3 - true if vertical position observations have been rejected
|
||||
bool reject_mag_x # 4 - true if the X magnetometer observation has been rejected
|
||||
bool reject_mag_y # 5 - true if the Y magnetometer observation has been rejected
|
||||
bool reject_mag_z # 6 - true if the Z magnetometer observation has been rejected
|
||||
bool reject_yaw # 7 - true if the yaw observation has been rejected
|
||||
bool reject_airspeed # 8 - true if the airspeed observation has been rejected
|
||||
bool reject_sideslip # 9 - true if the synthetic sideslip observation has been rejected
|
||||
bool reject_hagl # 10 - true if the height above ground observation has been rejected
|
||||
bool reject_optflow_x # 11 - true if the X optical flow observation has been rejected
|
||||
bool reject_optflow_y # 12 - true if the Y optical flow observation has been rejected
|
||||
|
|
@ -291,6 +291,8 @@ rtps:
|
|||
id: 139
|
||||
- msg: manual_control_switches
|
||||
id: 140
|
||||
- msg: estimator_status_flags
|
||||
id: 141
|
||||
########## multi topics: begin ##########
|
||||
- msg: actuator_controls_0
|
||||
id: 150
|
||||
|
|
|
|||
|
|
@ -181,6 +181,7 @@ EKF2::EKF2(int instance, const px4::wq_config_t &config, int imu, int mag, bool
|
|||
_estimator_sensor_bias_pub.advertise();
|
||||
_estimator_states_pub.advertise();
|
||||
_estimator_status_pub.advertise();
|
||||
_estimator_status_flags_pub.advertise();
|
||||
_estimator_visual_odometry_aligned_pub.advertised();
|
||||
_wind_pub.advertise();
|
||||
_yaw_est_pub.advertise();
|
||||
|
|
@ -443,6 +444,7 @@ void EKF2::Run()
|
|||
PublishEkfDriftMetrics(now);
|
||||
PublishStates(now);
|
||||
PublishStatus(now);
|
||||
PublishStatusFlags(now);
|
||||
PublishInnovations(now, imu_sample_new);
|
||||
PublishInnovationTestRatios(now);
|
||||
PublishInnovationVariances(now);
|
||||
|
|
@ -973,6 +975,107 @@ void EKF2::PublishStatus(const hrt_abstime ×tamp)
|
|||
_estimator_status_pub.publish(status);
|
||||
}
|
||||
|
||||
void EKF2::PublishStatusFlags(const hrt_abstime ×tamp)
|
||||
{
|
||||
// publish at ~ 1 Hz (or immediately if filter control status or fault status changes)
|
||||
bool update = (hrt_elapsed_time(&_last_status_flag_update) >= 1_s);
|
||||
|
||||
// filter control status
|
||||
if (_ekf.control_status().value != _filter_control_status) {
|
||||
update = true;
|
||||
_filter_control_status = _ekf.control_status().value;
|
||||
_filter_control_status_changes++;
|
||||
}
|
||||
|
||||
// filter fault status
|
||||
if (_ekf.fault_status().value != _filter_fault_status) {
|
||||
update = true;
|
||||
_filter_fault_status = _ekf.fault_status().value;
|
||||
_filter_fault_status_changes++;
|
||||
}
|
||||
|
||||
// innovation check fail status
|
||||
if (_ekf.innov_check_fail_status().value != _innov_check_fail_status) {
|
||||
update = true;
|
||||
_innov_check_fail_status = _ekf.innov_check_fail_status().value;
|
||||
_innov_check_fail_status_changes++;
|
||||
}
|
||||
|
||||
if (update) {
|
||||
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.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.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);
|
||||
|
||||
_last_status_flag_update = status_flags.timestamp;
|
||||
}
|
||||
}
|
||||
|
||||
void EKF2::PublishYawEstimatorStatus(const hrt_abstime ×tamp)
|
||||
{
|
||||
static_assert(sizeof(yaw_estimator_status_s::yaw) / sizeof(float) == N_MODELS_EKFGSF,
|
||||
|
|
|
|||
|
|
@ -69,6 +69,7 @@
|
|||
#include <uORB/topics/estimator_sensor_bias.h>
|
||||
#include <uORB/topics/estimator_states.h>
|
||||
#include <uORB/topics/estimator_status.h>
|
||||
#include <uORB/topics/estimator_status_flags.h>
|
||||
#include <uORB/topics/landing_target_pose.h>
|
||||
#include <uORB/topics/optical_flow.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
|
|
@ -136,6 +137,7 @@ private:
|
|||
void PublishSensorBias(const hrt_abstime ×tamp);
|
||||
void PublishStates(const hrt_abstime ×tamp);
|
||||
void PublishStatus(const hrt_abstime ×tamp);
|
||||
void PublishStatusFlags(const hrt_abstime ×tamp);
|
||||
void PublishWindEstimate(const hrt_abstime ×tamp);
|
||||
void PublishYawEstimatorStatus(const hrt_abstime ×tamp);
|
||||
|
||||
|
|
@ -228,6 +230,16 @@ private:
|
|||
bool _armed{false};
|
||||
bool _standby{false}; // standby arming state
|
||||
|
||||
hrt_abstime _last_status_flag_update{0};
|
||||
|
||||
uint32_t _filter_control_status{0};
|
||||
uint32_t _filter_fault_status{0};
|
||||
uint32_t _innov_check_fail_status{0};
|
||||
|
||||
uint32_t _filter_control_status_changes{0};
|
||||
uint32_t _filter_fault_status_changes{0};
|
||||
uint32_t _innov_check_fail_status_changes{0};
|
||||
|
||||
uORB::PublicationMulti<ekf2_timestamps_s> _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)};
|
||||
uORB::PublicationMulti<ekf_gps_drift_s> _ekf_gps_drift_pub{ORB_ID(ekf_gps_drift)};
|
||||
uORB::PublicationMulti<estimator_innovations_s> _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)};
|
||||
|
|
@ -237,6 +249,7 @@ private:
|
|||
uORB::PublicationMulti<estimator_sensor_bias_s> _estimator_sensor_bias_pub{ORB_ID(estimator_sensor_bias)};
|
||||
uORB::PublicationMulti<estimator_states_s> _estimator_states_pub{ORB_ID(estimator_states)};
|
||||
uORB::PublicationMulti<estimator_status_s> _estimator_status_pub{ORB_ID(estimator_status)};
|
||||
uORB::PublicationMulti<estimator_status_flags_s> _estimator_status_flags_pub{ORB_ID(estimator_status_flags)};
|
||||
uORB::PublicationMulti<vehicle_odometry_s> _estimator_visual_odometry_aligned_pub{ORB_ID(estimator_visual_odometry_aligned)};
|
||||
uORB::PublicationMulti<yaw_estimator_status_s> _yaw_est_pub{ORB_ID(yaw_estimator_status)};
|
||||
uORB::PublicationMulti<wind_estimate_s> _wind_pub{ORB_ID(wind_estimate)};
|
||||
|
|
|
|||
|
|
@ -126,6 +126,7 @@ void LoggedTopics::add_default_topics()
|
|||
add_topic_multi("estimator_sensor_bias", 0, MAX_ESTIMATOR_INSTANCES);
|
||||
add_topic_multi("estimator_states", 1000, MAX_ESTIMATOR_INSTANCES);
|
||||
add_topic_multi("estimator_status", 500, MAX_ESTIMATOR_INSTANCES);
|
||||
add_topic_multi("estimator_status_flags", 0, MAX_ESTIMATOR_INSTANCES);
|
||||
add_topic_multi("estimator_visual_odometry_aligned", 200, MAX_ESTIMATOR_INSTANCES);
|
||||
add_topic_multi("yaw_estimator_status", 1000, MAX_ESTIMATOR_INSTANCES);
|
||||
add_topic_multi("wind_estimate", 1000); // published by both ekf2 and airspeed_selector
|
||||
|
|
|
|||
Loading…
Reference in New Issue