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:
Daniel Agar 2021-07-30 11:26:13 -04:00
parent d997a8d308
commit 93aa6e3f78
9 changed files with 37 additions and 37 deletions

View File

@ -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"'

View File

@ -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

View File

@ -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

View File

@ -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:

View File

@ -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;

View File

@ -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 &timestamp)
}
}
void EKF2::PublishBaroBiasEstimate(const hrt_abstime &timestamp)
void EKF2::PublishBaroBias(const hrt_abstime &timestamp)
{
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 &timestamp)

View File

@ -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 &timestamp);
void PublishBaroBiasEstimate(const hrt_abstime &timestamp);
void PublishBaroBias(const hrt_abstime &timestamp);
void PublishEkfDriftMetrics(const hrt_abstime &timestamp);
void PublishEventFlags(const hrt_abstime &timestamp);
void PublishGlobalPosition(const hrt_abstime &timestamp);
@ -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)};

View File

@ -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);