forked from Archive/PX4-Autopilot
ekf2: baro bias publish minor cleanup
- naming consistency (estimator prefix as "namespace") - only publish if baro is available and bias is changing as a small logging optimization - avoid unnecessary copying (get const reference to status directly) - trivial code style fixes
This commit is contained in:
parent
d997a8d308
commit
93aa6e3f78
|
@ -902,6 +902,7 @@ void printTopics() {
|
|||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener ekf_gps_drift"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener esc_status"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_attitude"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_baro_bias"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_event_flags"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_global_position"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_innovation_test_ratios"'
|
||||
|
|
|
@ -44,7 +44,6 @@ set(msg_files
|
|||
airspeed.msg
|
||||
airspeed_validated.msg
|
||||
airspeed_wind.msg
|
||||
baro_bias_estimate.msg
|
||||
battery_status.msg
|
||||
camera_capture.msg
|
||||
camera_status.msg
|
||||
|
@ -59,9 +58,9 @@ set(msg_files
|
|||
distance_sensor.msg
|
||||
ekf2_timestamps.msg
|
||||
ekf_gps_drift.msg
|
||||
event.msg
|
||||
esc_report.msg
|
||||
esc_status.msg
|
||||
estimator_baro_bias.msg
|
||||
estimator_event_flags.msg
|
||||
estimator_innovations.msg
|
||||
estimator_optical_flow_vel.msg
|
||||
|
@ -70,6 +69,7 @@ set(msg_files
|
|||
estimator_states.msg
|
||||
estimator_status.msg
|
||||
estimator_status_flags.msg
|
||||
event.msg
|
||||
follow_target.msg
|
||||
generator_status.msg
|
||||
geofence_result.msg
|
||||
|
|
|
@ -343,7 +343,7 @@ rtps:
|
|||
id: 159
|
||||
- msg: event
|
||||
id: 160
|
||||
- msg: baro_bias_estimate
|
||||
- msg: estimator_baro_bias
|
||||
id: 161
|
||||
- msg: internal_combustion_engine_status
|
||||
id: 162
|
||||
|
|
|
@ -298,7 +298,8 @@ public:
|
|||
// returns false when data is not available
|
||||
bool getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF],
|
||||
float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]);
|
||||
void getBaroBiasEstimatorStatus(float &bias, float &bias_var, float &innov, float &innov_var, float &innov_test_ratio);
|
||||
|
||||
const BaroBiasEstimator::status &getBaroBiasEstimatorStatus() const { return _baro_b_est.getStatus(); }
|
||||
|
||||
private:
|
||||
|
||||
|
|
|
@ -1320,9 +1320,8 @@ void Ekf::updateBaroHgtOffset()
|
|||
|
||||
// apply a 10 second first order low pass filter to baro offset
|
||||
const float unbiased_baro = _baro_sample_delayed.hgt - _baro_b_est.getBias();
|
||||
;
|
||||
const float offset_rate_correction = 0.1f * (unbiased_baro + _state.pos(2) -
|
||||
_baro_hgt_offset);
|
||||
|
||||
const float offset_rate_correction = 0.1f * (unbiased_baro + _state.pos(2) - _baro_hgt_offset);
|
||||
_baro_hgt_offset += local_time_step * math::constrain(offset_rate_correction, -0.1f, 0.1f);
|
||||
}
|
||||
}
|
||||
|
@ -1348,7 +1347,7 @@ void Ekf::updateBaroHgtBias()
|
|||
}
|
||||
|
||||
if (_gps_data_ready && !_gps_hgt_intermittent
|
||||
&& _gps_checks_passed &&_NED_origin_initialised
|
||||
&& _gps_checks_passed && _NED_origin_initialised
|
||||
&& !_baro_hgt_faulty) {
|
||||
// Use GPS altitude as a reference to compute the baro bias measurement
|
||||
const float baro_bias = (_baro_sample_delayed.hgt - _baro_hgt_offset)
|
||||
|
@ -1358,16 +1357,6 @@ void Ekf::updateBaroHgtBias()
|
|||
}
|
||||
}
|
||||
|
||||
void Ekf::getBaroBiasEstimatorStatus(float &bias, float &bias_var, float &innov, float &innov_var, float &innov_test_ratio)
|
||||
{
|
||||
BaroBiasEstimator::status status = _baro_b_est.getStatus();
|
||||
bias = status.bias;
|
||||
bias_var = status.bias_var;
|
||||
innov = status.innov;
|
||||
innov_var = status.innov_var;
|
||||
innov_test_ratio = status.innov_test_ratio;
|
||||
}
|
||||
|
||||
Vector3f Ekf::getVisionVelocityInEkfFrame() const
|
||||
{
|
||||
Vector3f vel;
|
||||
|
|
|
@ -192,6 +192,7 @@ bool EKF2::multi_init(int imu, int mag)
|
|||
|
||||
_ekf2_timestamps_pub.advertise();
|
||||
_ekf_gps_drift_pub.advertise();
|
||||
_estimator_baro_bias_pub.advertise();
|
||||
_estimator_innovation_test_ratios_pub.advertise();
|
||||
_estimator_innovation_variances_pub.advertise();
|
||||
_estimator_innovations_pub.advertise();
|
||||
|
@ -202,7 +203,6 @@ bool EKF2::multi_init(int imu, int mag)
|
|||
_estimator_status_flags_pub.advertise();
|
||||
_estimator_visual_odometry_aligned_pub.advertised();
|
||||
_yaw_est_pub.advertise();
|
||||
_baro_bias_estimate_pub.advertise();
|
||||
|
||||
bool changed_instance = _vehicle_imu_sub.ChangeInstance(imu) && _magnetometer_sub.ChangeInstance(mag);
|
||||
|
||||
|
@ -497,6 +497,7 @@ void EKF2::Run()
|
|||
PublishWindEstimate(now);
|
||||
|
||||
// publish status/logging messages
|
||||
PublishBaroBias(now);
|
||||
PublishEkfDriftMetrics(now);
|
||||
PublishEventFlags(now);
|
||||
PublishStates(now);
|
||||
|
@ -506,7 +507,6 @@ void EKF2::Run()
|
|||
PublishInnovationTestRatios(now);
|
||||
PublishInnovationVariances(now);
|
||||
PublishYawEstimatorStatus(now);
|
||||
PublishBaroBiasEstimate(now);
|
||||
|
||||
UpdateMagCalibration(now);
|
||||
|
||||
|
@ -550,19 +550,26 @@ void EKF2::PublishAttitude(const hrt_abstime ×tamp)
|
|||
}
|
||||
}
|
||||
|
||||
void EKF2::PublishBaroBiasEstimate(const hrt_abstime ×tamp)
|
||||
void EKF2::PublishBaroBias(const hrt_abstime ×tamp)
|
||||
{
|
||||
if (_device_id_baro != 0) {
|
||||
const BaroBiasEstimator::status &status = _ekf.getBaroBiasEstimatorStatus();
|
||||
|
||||
baro_bias_estimate_s bbe{};
|
||||
bbe.timestamp = timestamp;
|
||||
bbe.timestamp_sample = timestamp;
|
||||
bbe.baro_device_id = _device_id_baro;
|
||||
_ekf.getBaroBiasEstimatorStatus(bbe.bias,
|
||||
bbe.bias_var,
|
||||
bbe.innov,
|
||||
bbe.innov_var,
|
||||
bbe.innov_test_ratio);
|
||||
_baro_bias_estimate_pub.publish(bbe);
|
||||
if (fabsf(status.bias - _last_baro_bias_published) > 0.001f) {
|
||||
estimator_baro_bias_s baro_bias{};
|
||||
baro_bias.timestamp_sample = timestamp;
|
||||
baro_bias.baro_device_id = _device_id_baro;
|
||||
baro_bias.bias = status.bias;
|
||||
baro_bias.bias_var = status.bias_var;
|
||||
baro_bias.innov = status.innov;
|
||||
baro_bias.innov_var = status.innov_var;
|
||||
baro_bias.innov_test_ratio = status.innov_test_ratio;
|
||||
baro_bias.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
|
||||
_estimator_baro_bias_pub.publish(baro_bias);
|
||||
|
||||
_last_baro_bias_published = status.bias;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void EKF2::PublishEkfDriftMetrics(const hrt_abstime ×tamp)
|
||||
|
|
|
@ -41,13 +41,15 @@
|
|||
#ifndef EKF2_HPP
|
||||
#define EKF2_HPP
|
||||
|
||||
#include "EKF/ekf.h"
|
||||
#include "Utility/PreFlightChecker.hpp"
|
||||
|
||||
#include "EKF2Selector.hpp"
|
||||
|
||||
#include <float.h>
|
||||
|
||||
#include <containers/LockGuard.hpp>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include "EKF/ekf.h"
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
#include <px4_platform_common/defines.h>
|
||||
|
@ -62,10 +64,10 @@
|
|||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include <uORB/SubscriptionMultiArray.hpp>
|
||||
#include <uORB/topics/airspeed.h>
|
||||
#include <uORB/topics/baro_bias_estimate.h>
|
||||
#include <uORB/topics/distance_sensor.h>
|
||||
#include <uORB/topics/ekf2_timestamps.h>
|
||||
#include <uORB/topics/ekf_gps_drift.h>
|
||||
#include <uORB/topics/estimator_baro_bias.h>
|
||||
#include <uORB/topics/estimator_event_flags.h>
|
||||
#include <uORB/topics/estimator_innovations.h>
|
||||
#include <uORB/topics/estimator_optical_flow_vel.h>
|
||||
|
@ -92,7 +94,6 @@
|
|||
#include <uORB/topics/wind.h>
|
||||
#include <uORB/topics/yaw_estimator_status.h>
|
||||
|
||||
#include "Utility/PreFlightChecker.hpp"
|
||||
|
||||
extern pthread_mutex_t ekf2_module_mutex;
|
||||
|
||||
|
@ -130,7 +131,7 @@ private:
|
|||
void Run() override;
|
||||
|
||||
void PublishAttitude(const hrt_abstime ×tamp);
|
||||
void PublishBaroBiasEstimate(const hrt_abstime ×tamp);
|
||||
void PublishBaroBias(const hrt_abstime ×tamp);
|
||||
void PublishEkfDriftMetrics(const hrt_abstime ×tamp);
|
||||
void PublishEventFlags(const hrt_abstime ×tamp);
|
||||
void PublishGlobalPosition(const hrt_abstime ×tamp);
|
||||
|
@ -218,6 +219,7 @@ private:
|
|||
Vector3f _last_accel_bias_published{};
|
||||
Vector3f _last_gyro_bias_published{};
|
||||
Vector3f _last_mag_bias_published{};
|
||||
float _last_baro_bias_published{};
|
||||
|
||||
float _airspeed_scale_factor{1.0f}; ///< scale factor correction applied to airspeed measurements
|
||||
|
||||
|
@ -258,9 +260,9 @@ private:
|
|||
uint32_t _filter_warning_event_changes{0};
|
||||
uint32_t _filter_information_event_changes{0};
|
||||
|
||||
uORB::PublicationMulti<baro_bias_estimate_s> _baro_bias_estimate_pub{ORB_ID(baro_bias_estimate)};
|
||||
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_baro_bias_s> _estimator_baro_bias_pub{ORB_ID(estimator_baro_bias)};
|
||||
uORB::PublicationMulti<estimator_innovations_s> _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)};
|
||||
uORB::PublicationMulti<estimator_innovations_s> _estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)};
|
||||
uORB::PublicationMulti<estimator_innovations_s> _estimator_innovations_pub{ORB_ID(estimator_innovations)};
|
||||
|
|
|
@ -138,7 +138,6 @@ void LoggedTopics::add_default_topics()
|
|||
#else
|
||||
static constexpr uint8_t MAX_ESTIMATOR_INSTANCES = 6; // artificially limited until PlotJuggler fixed
|
||||
add_topic("estimator_selector_status");
|
||||
add_topic_multi("baro_bias_estimate", 500, MAX_ESTIMATOR_INSTANCES);
|
||||
add_topic_multi("estimator_attitude", 500, MAX_ESTIMATOR_INSTANCES);
|
||||
add_topic_multi("estimator_global_position", 1000, MAX_ESTIMATOR_INSTANCES);
|
||||
add_topic_multi("estimator_local_position", 500, MAX_ESTIMATOR_INSTANCES);
|
||||
|
@ -146,6 +145,7 @@ void LoggedTopics::add_default_topics()
|
|||
#endif
|
||||
|
||||
add_topic_multi("ekf_gps_drift", 1000, MAX_ESTIMATOR_INSTANCES);
|
||||
add_topic_multi("estimator_baro_bias", 500, MAX_ESTIMATOR_INSTANCES);
|
||||
add_topic_multi("estimator_event_flags", 0, MAX_ESTIMATOR_INSTANCES);
|
||||
add_topic_multi("estimator_innovation_test_ratios", 500, MAX_ESTIMATOR_INSTANCES);
|
||||
add_topic_multi("estimator_innovation_variances", 500, MAX_ESTIMATOR_INSTANCES);
|
||||
|
|
Loading…
Reference in New Issue