diff --git a/src/modules/ekf2/EKF2Selector.cpp b/src/modules/ekf2/EKF2Selector.cpp index 898dbc87f5..41076a4740 100644 --- a/src/modules/ekf2/EKF2Selector.cpp +++ b/src/modules/ekf2/EKF2Selector.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2021 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -37,7 +37,6 @@ using namespace time_literals; using matrix::Quatf; using matrix::Vector2f; using math::constrain; -using math::max; using math::radians; EKF2Selector::EKF2Selector() : @@ -118,12 +117,6 @@ bool EKF2Selector::SelectInstance(uint8_t ekf_instance) _instance[i].relative_test_ratio = 0; } - // publish new data immediately with resets - PublishVehicleAttitude(true); - PublishVehicleLocalPosition(true); - PublishVehicleGlobalPosition(true); - PublishWindEstimate(true); - return true; } @@ -330,104 +323,152 @@ bool EKF2Selector::UpdateErrorScores() return (primary_updated || updated); } -void EKF2Selector::PublishVehicleAttitude(bool reset) +void EKF2Selector::PublishVehicleAttitude() { + // selected estimator_attitude -> vehicle_attitude vehicle_attitude_s attitude; - if (_instance[_selected_instance].estimator_attitude_sub.copy(&attitude)) { - if (reset) { - // on reset compute deltas from last published data - ++_quat_reset_counter; + if (_instance[_selected_instance].estimator_attitude_sub.update(&attitude)) { + bool instance_change = false; - _delta_q_reset = (Quatf(attitude.q) * Quatf(_attitude_last.q).inversed()).normalized(); - - // ensure monotonically increasing timestamp_sample through reset - attitude.timestamp_sample = max(attitude.timestamp_sample, _attitude_last.timestamp_sample); - - } else { - // otherwise propogate deltas from estimator data while maintaining the overall reset counts - if (attitude.quat_reset_counter > _attitude_last.quat_reset_counter) { - ++_quat_reset_counter; - _delta_q_reset = Quatf{attitude.delta_q_reset}; - } + if (_instance[_selected_instance].estimator_attitude_sub.get_instance() != _attitude_instance_prev) { + _attitude_instance_prev = _instance[_selected_instance].estimator_attitude_sub.get_instance(); + instance_change = true; } - // save last primary estimator_attitude + if (_attitude_last.timestamp != 0) { + if (!instance_change && (attitude.quat_reset_counter == _attitude_last.quat_reset_counter + 1)) { + // propogate deltas from estimator data while maintaining the overall reset counts + ++_quat_reset_counter; + _delta_q_reset = Quatf{attitude.delta_q_reset}; + + } else if (instance_change || (attitude.quat_reset_counter != _attitude_last.quat_reset_counter)) { + // on reset compute deltas from last published data + ++_quat_reset_counter; + _delta_q_reset = (Quatf(attitude.q) * Quatf(_attitude_last.q).inversed()).normalized(); + } + + } else { + _quat_reset_counter = attitude.quat_reset_counter; + _delta_q_reset = Quatf{attitude.delta_q_reset}; + } + + bool publish = true; + + // ensure monotonically increasing timestamp_sample through reset, don't publish + // estimator's attitude for system (vehicle_attitude) if it's stale + if ((attitude.timestamp_sample <= _attitude_last.timestamp_sample) + || (attitude.timestamp_sample < _instance[_selected_instance].timestamp_sample_last)) { + + publish = false; + } + + // save last primary estimator_attitude as published with original resets _attitude_last = attitude; - // republish with total reset count and current timestamp - attitude.quat_reset_counter = _quat_reset_counter; - _delta_q_reset.copyTo(attitude.delta_q_reset); + if (publish) { + // republish with total reset count and current timestamp + attitude.quat_reset_counter = _quat_reset_counter; + _delta_q_reset.copyTo(attitude.delta_q_reset); - attitude.timestamp = hrt_absolute_time(); - _vehicle_attitude_pub.publish(attitude); - - _instance[_selected_instance].timestamp_sample_last = attitude.timestamp_sample; + attitude.timestamp = hrt_absolute_time(); + _vehicle_attitude_pub.publish(attitude); + } } } -void EKF2Selector::PublishVehicleLocalPosition(bool reset) +void EKF2Selector::PublishVehicleLocalPosition() { - // vehicle_local_position + // selected estimator_local_position -> vehicle_local_position vehicle_local_position_s local_position; - if (_instance[_selected_instance].estimator_local_position_sub.copy(&local_position)) { - if (reset) { - // on reset compute deltas from last published data - ++_xy_reset_counter; - ++_z_reset_counter; - ++_vxy_reset_counter; - ++_vz_reset_counter; - ++_heading_reset_counter; + if (_instance[_selected_instance].estimator_local_position_sub.update(&local_position)) { + bool instance_change = false; - _delta_xy_reset = Vector2f{local_position.x, local_position.y} - Vector2f{_local_position_last.x, _local_position_last.y}; - _delta_z_reset = local_position.z - _local_position_last.z; - _delta_vxy_reset = Vector2f{local_position.vx, local_position.vy} - Vector2f{_local_position_last.vx, _local_position_last.vy}; - _delta_vz_reset = local_position.vz - _local_position_last.vz; - _delta_heading_reset = matrix::wrap_2pi(local_position.heading - _local_position_last.heading); - - // ensure monotonically increasing timestamp_sample through reset - local_position.timestamp_sample = max(local_position.timestamp_sample, _local_position_last.timestamp_sample); - - } else { - // otherwise propogate deltas from estimator data while maintaining the overall reset counts + if (_instance[_selected_instance].estimator_local_position_sub.get_instance() != _local_position_instance_prev) { + _local_position_instance_prev = _instance[_selected_instance].estimator_local_position_sub.get_instance(); + instance_change = true; + } + if (_local_position_last.timestamp != 0) { // XY reset - if (local_position.xy_reset_counter > _local_position_last.xy_reset_counter) { + if (!instance_change && (local_position.xy_reset_counter == _local_position_last.xy_reset_counter + 1)) { ++_xy_reset_counter; _delta_xy_reset = Vector2f{local_position.delta_xy}; + + } else if (instance_change || (local_position.xy_reset_counter != _local_position_last.xy_reset_counter)) { + ++_xy_reset_counter; + _delta_xy_reset = Vector2f{local_position.x, local_position.y} - Vector2f{_local_position_last.x, _local_position_last.y}; } // Z reset - if (local_position.z_reset_counter > _local_position_last.z_reset_counter) { + if (!instance_change && (local_position.z_reset_counter == _local_position_last.z_reset_counter + 1)) { ++_z_reset_counter; _delta_z_reset = local_position.delta_z; + + } else if (instance_change || (local_position.z_reset_counter != _local_position_last.z_reset_counter)) { + ++_z_reset_counter; + _delta_z_reset = local_position.z - _local_position_last.z; } // VXY reset - if (local_position.vxy_reset_counter > _local_position_last.vxy_reset_counter) { + if (!instance_change && (local_position.vxy_reset_counter == _local_position_last.vxy_reset_counter + 1)) { ++_vxy_reset_counter; _delta_vxy_reset = Vector2f{local_position.delta_vxy}; + + } else if (instance_change || (local_position.vxy_reset_counter != _local_position_last.vxy_reset_counter)) { + ++_vxy_reset_counter; + _delta_vxy_reset = Vector2f{local_position.vx, local_position.vy} - Vector2f{_local_position_last.vx, _local_position_last.vy}; } // VZ reset - if (local_position.vz_reset_counter > _local_position_last.vz_reset_counter) { + if (!instance_change && (local_position.vz_reset_counter == _local_position_last.vz_reset_counter + 1)) { ++_vz_reset_counter; _delta_vz_reset = local_position.delta_vz; + + } else if (instance_change || (local_position.vz_reset_counter != _local_position_last.vz_reset_counter)) { + ++_vz_reset_counter; + _delta_vz_reset = local_position.vz - _local_position_last.vz; } // heading reset - if (local_position.heading_reset_counter > _local_position_last.heading_reset_counter) { + if (!instance_change && (local_position.heading_reset_counter == _local_position_last.heading_reset_counter + 1)) { ++_heading_reset_counter; _delta_heading_reset = local_position.delta_heading; + + } else if (instance_change || (local_position.heading_reset_counter != _local_position_last.heading_reset_counter)) { + ++_heading_reset_counter; + _delta_heading_reset = matrix::wrap_pi(local_position.heading - _local_position_last.heading); } + + } else { + _xy_reset_counter = local_position.xy_reset_counter; + _z_reset_counter = local_position.z_reset_counter; + _vxy_reset_counter = local_position.vxy_reset_counter; + _vz_reset_counter = local_position.vz_reset_counter; + _heading_reset_counter = local_position.heading_reset_counter; + + _delta_xy_reset = Vector2f{local_position.delta_xy}; + _delta_z_reset = local_position.delta_z; + _delta_vxy_reset = Vector2f{local_position.delta_vxy}; + _delta_vz_reset = local_position.delta_vz; + _delta_heading_reset = local_position.delta_heading; } - // save last primary estimator_local_position + bool publish = true; + + // ensure monotonically increasing timestamp_sample through reset, don't publish + // estimator's local position for system (vehicle_local_position) if it's stale + if ((local_position.timestamp_sample <= _local_position_last.timestamp_sample) + || (local_position.timestamp_sample < _instance[_selected_instance].timestamp_sample_last)) { + + publish = false; + } + + // save last primary estimator_local_position as published with original resets _local_position_last = local_position; - // publish estimator's local position for system (vehicle_local_position) unless it's stale - if (local_position.timestamp_sample >= _instance[_selected_instance].timestamp_sample_last) { + if (publish) { // republish with total reset count and current timestamp local_position.xy_reset_counter = _xy_reset_counter; local_position.z_reset_counter = _z_reset_counter; @@ -447,47 +488,92 @@ void EKF2Selector::PublishVehicleLocalPosition(bool reset) } } -void EKF2Selector::PublishVehicleGlobalPosition(bool reset) +void EKF2Selector::PublishVehicleOdometry() { + // selected estimator_odometry -> vehicle_odometry + vehicle_odometry_s odometry; + + if (_instance[_selected_instance].estimator_odometry_sub.update(&odometry)) { + bool publish = true; + + // ensure monotonically increasing timestamp_sample through reset, don't publish + // estimator's odometry for system (vehicle_odometry) if it's stale + if ((odometry.timestamp_sample <= _odometry_last.timestamp_sample) + || (odometry.timestamp_sample < _instance[_selected_instance].timestamp_sample_last)) { + + publish = false; + } + + // save last primary estimator_odometry + _odometry_last = odometry; + + if (publish) { + odometry.timestamp = hrt_absolute_time(); + _vehicle_odometry_pub.publish(odometry); + } + } +} + +void EKF2Selector::PublishVehicleGlobalPosition() +{ + // selected estimator_global_position -> vehicle_global_position vehicle_global_position_s global_position; - if (_instance[_selected_instance].estimator_global_position_sub.copy(&global_position)) { - if (reset) { - // on reset compute deltas from last published data - ++_lat_lon_reset_counter; - ++_alt_reset_counter; + if (_instance[_selected_instance].estimator_global_position_sub.update(&global_position)) { + bool instance_change = false; - _delta_lat_reset = global_position.lat - _global_position_last.lat; - _delta_lon_reset = global_position.lon - _global_position_last.lon; - _delta_alt_reset = global_position.delta_alt - _global_position_last.delta_alt; - - // ensure monotonically increasing timestamp_sample through reset - global_position.timestamp_sample = max(global_position.timestamp_sample, _global_position_last.timestamp_sample); - - } else { - // otherwise propogate deltas from estimator data while maintaining the overall reset counts + if (_instance[_selected_instance].estimator_global_position_sub.get_instance() != _global_position_instance_prev) { + _global_position_instance_prev = _instance[_selected_instance].estimator_global_position_sub.get_instance(); + instance_change = true; + } + if (_global_position_last.timestamp != 0) { // lat/lon reset - if (global_position.lat_lon_reset_counter > _global_position_last.lat_lon_reset_counter) { + if (!instance_change && (global_position.lat_lon_reset_counter == _global_position_last.lat_lon_reset_counter + 1)) { ++_lat_lon_reset_counter; // TODO: delta latitude/longitude - //_delta_lat_reset = global_position.delta_lat; - //_delta_lon_reset = global_position.delta_lon; + _delta_lat_reset = global_position.lat - _global_position_last.lat; + _delta_lon_reset = global_position.lon - _global_position_last.lon; + + } else if (instance_change || (global_position.lat_lon_reset_counter != _global_position_last.lat_lon_reset_counter)) { + ++_lat_lon_reset_counter; + + _delta_lat_reset = global_position.lat - _global_position_last.lat; + _delta_lon_reset = global_position.lon - _global_position_last.lon; } // alt reset - if (global_position.alt_reset_counter > _global_position_last.alt_reset_counter) { + if (!instance_change && (global_position.alt_reset_counter == _global_position_last.alt_reset_counter + 1)) { ++_alt_reset_counter; _delta_alt_reset = global_position.delta_alt; + + } else if (instance_change || (global_position.alt_reset_counter != _global_position_last.alt_reset_counter)) { + ++_alt_reset_counter; + _delta_alt_reset = global_position.delta_alt - _global_position_last.delta_alt; } + + } else { + _lat_lon_reset_counter = global_position.lat_lon_reset_counter; + _alt_reset_counter = global_position.alt_reset_counter; + + _delta_alt_reset = global_position.delta_alt; } - // save last primary estimator_global_position + bool publish = true; + + // ensure monotonically increasing timestamp_sample through reset, don't publish + // estimator's global position for system (vehicle_global_position) if it's stale + if ((global_position.timestamp_sample <= _global_position_last.timestamp_sample) + || (global_position.timestamp_sample < _instance[_selected_instance].timestamp_sample_last)) { + + publish = false; + } + + // save last primary estimator_global_position as published with original resets _global_position_last = global_position; - // publish estimator's global position for system (vehicle_global_position) unless it's stale - if (global_position.timestamp_sample >= _instance[_selected_instance].timestamp_sample_last) { + if (publish) { // republish with total reset count and current timestamp global_position.lat_lon_reset_counter = _lat_lon_reset_counter; global_position.alt_reset_counter = _alt_reset_counter; @@ -499,22 +585,31 @@ void EKF2Selector::PublishVehicleGlobalPosition(bool reset) } } -void EKF2Selector::PublishWindEstimate(bool reset) +void EKF2Selector::PublishWindEstimate() { + // selected estimator_wind -> wind wind_s wind; - if (_instance[_selected_instance].estimator_wind_sub.copy(&wind)) { - if (reset) { - // ensure monotonically increasing timestamp_sample through reset - wind.timestamp_sample = max(wind.timestamp_sample, _wind_last.timestamp_sample); + if (_instance[_selected_instance].estimator_wind_sub.update(&wind)) { + bool publish = true; + + // ensure monotonically increasing timestamp_sample through reset, don't publish + // estimator's wind for system (wind) if it's stale + if ((wind.timestamp_sample <= _wind_last.timestamp_sample) + || (wind.timestamp_sample < _instance[_selected_instance].timestamp_sample_last)) { + + publish = false; } // save last primary wind _wind_last = wind; - // republish with current timestamp - wind.timestamp = hrt_absolute_time(); - _wind_pub.publish(wind); + // publish estimator's wind for system unless it's stale + if (publish) { + // republish with current timestamp + wind.timestamp = hrt_absolute_time(); + _wind_pub.publish(wind); + } } } @@ -655,38 +750,11 @@ void EKF2Selector::Run() } // republish selected estimator data for system - - // selected estimator_attitude -> vehicle_attitude - if (_instance[_selected_instance].estimator_attitude_sub.updated()) { - PublishVehicleAttitude(); - } - - // selected estimator_local_position -> vehicle_local_position - if (_instance[_selected_instance].estimator_local_position_sub.updated()) { - PublishVehicleLocalPosition(); - } - - // selected estimator_global_position -> vehicle_global_position - if (_instance[_selected_instance].estimator_global_position_sub.updated()) { - PublishVehicleGlobalPosition(); - } - - // selected estimator_wind -> wind - if (_instance[_selected_instance].estimator_wind_sub.updated()) { - PublishWindEstimate(); - } - - // selected estimator_odometry -> vehicle_odometry - if (_instance[_selected_instance].estimator_odometry_sub.updated()) { - vehicle_odometry_s vehicle_odometry; - - if (_instance[_selected_instance].estimator_odometry_sub.update(&vehicle_odometry)) { - if (vehicle_odometry.timestamp_sample >= _instance[_selected_instance].timestamp_sample_last) { - vehicle_odometry.timestamp = hrt_absolute_time(); - _vehicle_odometry_pub.publish(vehicle_odometry); - } - } - } + PublishVehicleAttitude(); + PublishVehicleLocalPosition(); + PublishVehicleGlobalPosition(); + PublishVehicleOdometry(); + PublishWindEstimate(); } void EKF2Selector::PrintStatus() diff --git a/src/modules/ekf2/EKF2Selector.hpp b/src/modules/ekf2/EKF2Selector.hpp index fc63297f36..d24f44abfc 100644 --- a/src/modules/ekf2/EKF2Selector.hpp +++ b/src/modules/ekf2/EKF2Selector.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2021 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -78,10 +78,11 @@ private: static constexpr uint64_t FILTER_UPDATE_PERIOD{10_ms}; void Run() override; - void PublishVehicleAttitude(bool reset = false); - void PublishVehicleLocalPosition(bool reset = false); - void PublishVehicleGlobalPosition(bool reset = false); - void PublishWindEstimate(bool reset = false); + void PublishVehicleAttitude(); + void PublishVehicleLocalPosition(); + void PublishVehicleGlobalPosition(); + void PublishVehicleOdometry(); + void PublishWindEstimate(); bool SelectInstance(uint8_t instance); // Update the error scores for all available instances @@ -191,6 +192,9 @@ private: uint8_t _vz_reset_counter{0}; uint8_t _heading_reset_counter{0}; + // vehicle_odometry + vehicle_odometry_s _odometry_last{}; + // vehicle_global_position: reset counters vehicle_global_position_s _global_position_last{}; double _delta_lat_reset{0}; @@ -202,6 +206,10 @@ private: // wind estimate wind_s _wind_last{}; + uint8_t _attitude_instance_prev{INVALID_INSTANCE}; + uint8_t _local_position_instance_prev{INVALID_INSTANCE}; + uint8_t _global_position_instance_prev{INVALID_INSTANCE}; + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::Subscription _sensors_status_imu{ORB_ID(sensors_status_imu)};