forked from Archive/PX4-Autopilot
ekf2: publish GNSS, rng and EV height biases
This commit is contained in:
parent
6833c7e311
commit
aba2eac0df
|
@ -70,7 +70,7 @@ set(msg_files
|
|||
estimator_aid_source_1d.msg
|
||||
estimator_aid_source_2d.msg
|
||||
estimator_aid_source_3d.msg
|
||||
estimator_baro_bias.msg
|
||||
estimator_bias.msg
|
||||
estimator_event_flags.msg
|
||||
estimator_gps_status.msg
|
||||
estimator_innovations.msg
|
||||
|
|
|
@ -8,3 +8,6 @@ float32 bias_var # estimated barometric altitude bias variance (m^2)
|
|||
float32 innov # innovation of the last measurement fusion (m)
|
||||
float32 innov_var # innovation variance of the last measurement fusion (m^2)
|
||||
float32 innov_test_ratio # normalized innovation squared test ratio
|
||||
|
||||
# TOPICS estimator_bias
|
||||
# TOPICS estimator_baro_bias estimator_gnss_hgt_bias estimator_rng_hgt_bias estimator_ev_hgt_bias
|
|
@ -392,6 +392,9 @@ public:
|
|||
bool isYawEmergencyEstimateAvailable() const;
|
||||
|
||||
const BiasEstimator::status &getBaroBiasEstimatorStatus() const { return _baro_b_est.getStatus(); }
|
||||
const BiasEstimator::status &getGpsHgtBiasEstimatorStatus() const { return _gps_hgt_b_est.getStatus(); }
|
||||
const BiasEstimator::status &getRngHgtBiasEstimatorStatus() const { return _rng_hgt_b_est.getStatus(); }
|
||||
const BiasEstimator::status &getEvHgtBiasEstimatorStatus() const { return _ev_hgt_b_est.getStatus(); }
|
||||
|
||||
const auto &aid_src_airspeed() const { return _aid_src_airspeed; }
|
||||
|
||||
|
|
|
@ -257,6 +257,8 @@ public:
|
|||
|
||||
const baroSample &get_baro_sample_delayed() const { return _baro_sample_delayed; }
|
||||
const gpsSample &get_gps_sample_delayed() const { return _gps_sample_delayed; }
|
||||
const rangeSample &get_rng_sample_delayed() { return *(_range_sensor.getSampleAddress()); }
|
||||
const extVisionSample &get_ev_sample_delayed() const { return _ev_sample_delayed; }
|
||||
|
||||
const bool &global_origin_valid() const { return _NED_origin_initialised; }
|
||||
const MapProjection &global_origin() const { return _pos_ref; }
|
||||
|
|
|
@ -604,6 +604,9 @@ void EKF2::Run()
|
|||
|
||||
// publish status/logging messages
|
||||
PublishBaroBias(now);
|
||||
PublishGnssHgtBias(now);
|
||||
PublishRngHgtBias(now);
|
||||
PublishEvHgtBias(now);
|
||||
PublishEventFlags(now);
|
||||
PublishGpsStatus(now);
|
||||
PublishInnovations(now);
|
||||
|
@ -699,22 +702,63 @@ void EKF2::PublishBaroBias(const hrt_abstime ×tamp)
|
|||
const BiasEstimator::status &status = _ekf.getBaroBiasEstimatorStatus();
|
||||
|
||||
if (fabsf(status.bias - _last_baro_bias_published) > 0.001f) {
|
||||
estimator_baro_bias_s baro_bias{};
|
||||
baro_bias.timestamp_sample = _ekf.get_baro_sample_delayed().time_us;
|
||||
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);
|
||||
_estimator_baro_bias_pub.publish(fillEstimatorBiasMsg(status, _ekf.get_baro_sample_delayed().time_us, timestamp,
|
||||
_device_id_baro));
|
||||
|
||||
_last_baro_bias_published = status.bias;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void EKF2::PublishGnssHgtBias(const hrt_abstime ×tamp)
|
||||
{
|
||||
const BiasEstimator::status &status = _ekf.getGpsHgtBiasEstimatorStatus();
|
||||
|
||||
if (fabsf(status.bias - _last_gnss_hgt_bias_published) > 0.001f) {
|
||||
_estimator_gnss_hgt_bias_pub.publish(fillEstimatorBiasMsg(status, _ekf.get_gps_sample_delayed().time_us, timestamp));
|
||||
|
||||
_last_gnss_hgt_bias_published = status.bias;
|
||||
}
|
||||
}
|
||||
|
||||
void EKF2::PublishRngHgtBias(const hrt_abstime ×tamp)
|
||||
{
|
||||
const BiasEstimator::status &status = _ekf.getRngHgtBiasEstimatorStatus();
|
||||
|
||||
if (fabsf(status.bias - _last_rng_hgt_bias_published) > 0.001f) {
|
||||
_estimator_rng_hgt_bias_pub.publish(fillEstimatorBiasMsg(status, _ekf.get_rng_sample_delayed().time_us, timestamp));
|
||||
|
||||
_last_rng_hgt_bias_published = status.bias;
|
||||
}
|
||||
}
|
||||
|
||||
void EKF2::PublishEvHgtBias(const hrt_abstime ×tamp)
|
||||
{
|
||||
const BiasEstimator::status &status = _ekf.getEvHgtBiasEstimatorStatus();
|
||||
|
||||
if (fabsf(status.bias - _last_ev_hgt_bias_published) > 0.001f) {
|
||||
_estimator_ev_hgt_bias_pub.publish(fillEstimatorBiasMsg(status, _ekf.get_ev_sample_delayed().time_us, timestamp));
|
||||
|
||||
_last_ev_hgt_bias_published = status.bias;
|
||||
}
|
||||
}
|
||||
|
||||
estimator_bias_s EKF2::fillEstimatorBiasMsg(const BiasEstimator::status &status, uint64_t timestamp_sample_us,
|
||||
uint64_t timestamp, uint32_t device_id)
|
||||
{
|
||||
estimator_bias_s bias{};
|
||||
bias.timestamp_sample = timestamp_sample_us;
|
||||
bias.baro_device_id = device_id;
|
||||
bias.bias = status.bias;
|
||||
bias.bias_var = status.bias_var;
|
||||
bias.innov = status.innov;
|
||||
bias.innov_var = status.innov_var;
|
||||
bias.innov_test_ratio = status.innov_test_ratio;
|
||||
bias.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
|
||||
|
||||
return bias;
|
||||
}
|
||||
|
||||
void EKF2::PublishEventFlags(const hrt_abstime ×tamp)
|
||||
{
|
||||
// information events
|
||||
|
|
|
@ -67,7 +67,7 @@
|
|||
#include <uORB/topics/airspeed_validated.h>
|
||||
#include <uORB/topics/distance_sensor.h>
|
||||
#include <uORB/topics/ekf2_timestamps.h>
|
||||
#include <uORB/topics/estimator_baro_bias.h>
|
||||
#include <uORB/topics/estimator_bias.h>
|
||||
#include <uORB/topics/estimator_event_flags.h>
|
||||
#include <uORB/topics/estimator_gps_status.h>
|
||||
#include <uORB/topics/estimator_innovations.h>
|
||||
|
@ -137,6 +137,11 @@ private:
|
|||
void PublishAidSourceStatus(const hrt_abstime ×tamp);
|
||||
void PublishAttitude(const hrt_abstime ×tamp);
|
||||
void PublishBaroBias(const hrt_abstime ×tamp);
|
||||
void PublishGnssHgtBias(const hrt_abstime ×tamp);
|
||||
void PublishRngHgtBias(const hrt_abstime ×tamp);
|
||||
void PublishEvHgtBias(const hrt_abstime ×tamp);
|
||||
estimator_bias_s fillEstimatorBiasMsg(const BiasEstimator::status &status, uint64_t timestamp_sample_us,
|
||||
uint64_t timestamp, uint32_t device_id = 0);
|
||||
void PublishEventFlags(const hrt_abstime ×tamp);
|
||||
void PublishGlobalPosition(const hrt_abstime ×tamp);
|
||||
void PublishGpsStatus(const hrt_abstime ×tamp);
|
||||
|
@ -269,6 +274,9 @@ private:
|
|||
hrt_abstime _status_aux_vel_pub_last{0};
|
||||
|
||||
float _last_baro_bias_published{};
|
||||
float _last_gnss_hgt_bias_published{};
|
||||
float _last_rng_hgt_bias_published{};
|
||||
float _last_ev_hgt_bias_published{};
|
||||
|
||||
float _airspeed_scale_factor{1.0f}; ///< scale factor correction applied to airspeed measurements
|
||||
hrt_abstime _airspeed_validated_timestamp_last{0};
|
||||
|
@ -312,7 +320,10 @@ private:
|
|||
uint32_t _filter_information_event_changes{0};
|
||||
|
||||
uORB::PublicationMulti<ekf2_timestamps_s> _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)};
|
||||
uORB::PublicationMulti<estimator_baro_bias_s> _estimator_baro_bias_pub{ORB_ID(estimator_baro_bias)};
|
||||
uORB::PublicationMulti<estimator_bias_s> _estimator_baro_bias_pub{ORB_ID(estimator_baro_bias)};
|
||||
uORB::PublicationMulti<estimator_bias_s> _estimator_gnss_hgt_bias_pub{ORB_ID(estimator_gnss_hgt_bias)};
|
||||
uORB::PublicationMulti<estimator_bias_s> _estimator_rng_hgt_bias_pub{ORB_ID(estimator_rng_hgt_bias)};
|
||||
uORB::PublicationMulti<estimator_bias_s> _estimator_ev_hgt_bias_pub{ORB_ID(estimator_ev_hgt_bias)};
|
||||
uORB::PublicationMultiData<estimator_event_flags_s> _estimator_event_flags_pub{ORB_ID(estimator_event_flags)};
|
||||
uORB::PublicationMulti<estimator_gps_status_s> _estimator_gps_status_pub{ORB_ID(estimator_gps_status)};
|
||||
uORB::PublicationMulti<estimator_innovations_s> _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)};
|
||||
|
|
|
@ -144,6 +144,9 @@ void LoggedTopics::add_default_topics()
|
|||
|
||||
// always add the first instance
|
||||
add_topic("estimator_baro_bias", 500);
|
||||
add_topic("estimator_gnss_hgt_bias", 500);
|
||||
add_topic("estimator_rng_hgt_bias", 500);
|
||||
add_topic("estimator_ev_hgt_bias", 500);
|
||||
add_topic("estimator_event_flags", 0);
|
||||
add_topic("estimator_gps_status", 1000);
|
||||
add_topic("estimator_innovation_test_ratios", 500);
|
||||
|
@ -158,6 +161,9 @@ void LoggedTopics::add_default_topics()
|
|||
add_topic("yaw_estimator_status", 1000);
|
||||
|
||||
add_optional_topic_multi("estimator_baro_bias", 500, MAX_ESTIMATOR_INSTANCES);
|
||||
add_optional_topic_multi("estimator_gnss_hgt_bias", 500, MAX_ESTIMATOR_INSTANCES);
|
||||
add_optional_topic_multi("estimator_rng_hgt_bias", 500, MAX_ESTIMATOR_INSTANCES);
|
||||
add_optional_topic_multi("estimator_ev_hgt_bias", 500, MAX_ESTIMATOR_INSTANCES);
|
||||
add_optional_topic_multi("estimator_event_flags", 0, MAX_ESTIMATOR_INSTANCES);
|
||||
add_optional_topic_multi("estimator_gps_status", 1000, MAX_ESTIMATOR_INSTANCES);
|
||||
add_optional_topic_multi("estimator_innovation_test_ratios", 500, MAX_ESTIMATOR_INSTANCES);
|
||||
|
@ -241,6 +247,9 @@ void LoggedTopics::add_default_topics()
|
|||
|
||||
// EKF replay
|
||||
add_topic("estimator_baro_bias");
|
||||
add_topic("estimator_gnss_hgt_bias");
|
||||
add_topic("estimator_rng_hgt_bias");
|
||||
add_topic("estimator_ev_hgt_bias");
|
||||
add_topic("estimator_event_flags");
|
||||
add_topic("estimator_gps_status");
|
||||
add_topic("estimator_innovation_test_ratios");
|
||||
|
|
Loading…
Reference in New Issue