diff --git a/msg/vehicle_odometry.msg b/msg/vehicle_odometry.msg index 4e2a0d13f9..5f597f4d96 100644 --- a/msg/vehicle_odometry.msg +++ b/msg/vehicle_odometry.msg @@ -62,5 +62,7 @@ float32 yawspeed # Angular velocity about Z body axis # If angular velocity covariance invalid/unknown, 16th cell is NaN float32[21] velocity_covariance +uint8 reset_counter + # TOPICS vehicle_odometry vehicle_mocap_odometry vehicle_visual_odometry # TOPICS estimator_odometry estimator_visual_odometry_aligned diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 931e3c899f..639eb1977a 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -152,6 +152,7 @@ struct extVisionSample { Matrix3f velCov; ///< XYZ velocity covariances ((m/sec)**2) float angVar; ///< angular heading variance (rad**2) velocity_frame_t vel_frame = velocity_frame_t::BODY_FRAME_FRD; + uint8_t reset_counter{0}; }; struct dragSample { diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 37360967a1..dcb227349e 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -212,6 +212,12 @@ void Ekf::controlExternalVisionFusion() // Check for new external vision data if (_ev_data_ready) { + bool reset = false; + + if (_ev_sample_delayed.reset_counter != _ev_sample_delayed_prev.reset_counter) { + reset = true; + } + if (_inhibit_ev_yaw_use) { stopEvYawFusion(); } @@ -257,6 +263,9 @@ void Ekf::controlExternalVisionFusion() // determine if we should use the horizontal position observations if (_control_status.flags.ev_pos) { + if (reset && _control_status_prev.flags.ev_pos) { + resetHorizontalPosition(); + } Vector3f ev_pos_obs_var; Vector2f ev_pos_innov_gates; @@ -281,10 +290,8 @@ void Ekf::controlExternalVisionFusion() } else { // calculate the change in position since the last measurement - Vector3f ev_delta_pos = _ev_sample_delayed.pos - _pos_meas_prev; - // rotate measurement into body frame is required when fusing with GPS - ev_delta_pos = _R_ev_to_ekf * ev_delta_pos; + Vector3f ev_delta_pos = _R_ev_to_ekf * Vector3f(_ev_sample_delayed.pos - _ev_sample_delayed_prev.pos); // use the change in position since the last measurement _ev_pos_innov(0) = _state.pos(0) - _hpos_pred_prev(0) - ev_delta_pos(0); @@ -296,11 +303,6 @@ void Ekf::controlExternalVisionFusion() ev_pos_obs_var(0) = fmaxf(ev_pos_var(0, 0), sq(0.5f)); ev_pos_obs_var(1) = fmaxf(ev_pos_var(1, 1), sq(0.5f)); } - - // record observation and estimate for use next time - _pos_meas_prev = _ev_sample_delayed.pos; - _hpos_pred_prev = _state.pos.xy(); - } else { // use the absolute position Vector3f ev_pos_meas = _ev_sample_delayed.pos; @@ -337,6 +339,9 @@ void Ekf::controlExternalVisionFusion() // determine if we should use the velocity observations if (_control_status.flags.ev_vel) { + if (reset && _control_status_prev.flags.ev_vel) { + resetVelocity(); + } Vector2f ev_vel_innov_gates; @@ -362,9 +367,17 @@ void Ekf::controlExternalVisionFusion() // determine if we should use the yaw observation if (_control_status.flags.ev_yaw) { + if (reset && _control_status_prev.flags.ev_yaw) { + resetYawToEv(); + } + fuseHeading(); } + // record observation and estimate for use next time + _ev_sample_delayed_prev = _ev_sample_delayed; + _hpos_pred_prev = _state.pos.xy(); + } else if ((_control_status.flags.ev_pos || _control_status.flags.ev_vel || _control_status.flags.ev_yaw) && isTimedOut(_time_last_ext_vision, (uint64_t)_params.reset_timeout_max)) { diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index cfcfc4c792..c9a009eadb 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -252,6 +252,7 @@ public: const auto &state_reset_status() const { return _state_reset_status; } // return the amount the local vertical position changed in the last reset and the number of reset events + uint8_t get_posD_reset_count() const { return _state_reset_status.posD_counter; } void get_posD_reset(float *delta, uint8_t *counter) const { *delta = _state_reset_status.posD_change; @@ -259,6 +260,7 @@ public: } // return the amount the local vertical velocity changed in the last reset and the number of reset events + uint8_t get_velD_reset_count() const { return _state_reset_status.velD_counter; } void get_velD_reset(float *delta, uint8_t *counter) const { *delta = _state_reset_status.velD_change; @@ -266,6 +268,7 @@ public: } // return the amount the local horizontal position changed in the last reset and the number of reset events + uint8_t get_posNE_reset_count() const { return _state_reset_status.posNE_counter; } void get_posNE_reset(float delta[2], uint8_t *counter) const { _state_reset_status.posNE_change.copyTo(delta); @@ -273,6 +276,7 @@ public: } // return the amount the local horizontal velocity changed in the last reset and the number of reset events + uint8_t get_velNE_reset_count() const { return _state_reset_status.velNE_counter; } void get_velNE_reset(float delta[2], uint8_t *counter) const { _state_reset_status.velNE_change.copyTo(delta); @@ -280,6 +284,7 @@ public: } // return the amount the quaternion has changed in the last reset and the number of reset events + uint8_t get_quat_reset_count() const { return _state_reset_status.quat_counter; } void get_quat_reset(float delta_quat[4], uint8_t *counter) const { _state_reset_status.quat_change.copyTo(delta_quat); @@ -352,7 +357,6 @@ private: // variables used when position data is being fused using a relative position odometry model bool _fuse_hpos_as_odom{false}; ///< true when the NE position data is being fused using an odometry assumption - Vector3f _pos_meas_prev{}; ///< previous value of NED position measurement fused using odometry assumption (m) Vector2f _hpos_pred_prev{}; ///< previous value of NE position state used by odometry fusion (m) bool _hpos_prev_available{false}; ///< true when previous values of the estimate and measurement are available for use Dcmf _R_ev_to_ekf; ///< transformation matrix that rotates observations from the EV to the EKF navigation frame, initialized with Identity diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index d740a7f2f6..3f99570217 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -1566,7 +1566,7 @@ void Ekf::stopGpsFusion() // We do not need to know the true North anymore // EV yaw can start again - _inhibit_ev_yaw_use = false;; + _inhibit_ev_yaw_use = false; } void Ekf::stopGpsPosFusion() diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 764c1600dc..41b6379675 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -306,6 +306,7 @@ protected: airspeedSample _airspeed_sample_delayed{}; flowSample _flow_sample_delayed{}; extVisionSample _ev_sample_delayed{}; + extVisionSample _ev_sample_delayed_prev{}; dragSample _drag_sample_delayed{}; dragSample _drag_down_sampled{}; // down sampled drag specific force data (filter prediction rate -> observation rate) auxVelSample _auxvel_sample_delayed{}; diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 98f8115d4d..4bd85f9a8b 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1048,6 +1048,10 @@ void EKF2::PublishOdometry(const hrt_abstime ×tamp, const imuSample &imu) odom.velocity_covariance[odom.COVARIANCE_MATRIX_VY_VARIANCE] = covariances[5]; odom.velocity_covariance[odom.COVARIANCE_MATRIX_VZ_VARIANCE] = covariances[6]; + odom.reset_counter = _ekf.get_quat_reset_count() + + _ekf.get_velNE_reset_count() + _ekf.get_velD_reset_count() + + _ekf.get_posNE_reset_count() + _ekf.get_posD_reset_count(); + // publish vehicle odometry data odom.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); _odometry_pub.publish(odom); diff --git a/src/modules/ekf2/EKF2Selector.cpp b/src/modules/ekf2/EKF2Selector.cpp index 7ea1f71407..4150519c94 100644 --- a/src/modules/ekf2/EKF2Selector.cpp +++ b/src/modules/ekf2/EKF2Selector.cpp @@ -531,6 +531,24 @@ void EKF2Selector::PublishVehicleOdometry() vehicle_odometry_s odometry; if (_instance[_selected_instance].estimator_odometry_sub.update(&odometry)) { + + bool instance_change = false; + + if (_instance[_selected_instance].estimator_odometry_sub.get_instance() != _odometry_instance_prev) { + _odometry_instance_prev = _instance[_selected_instance].estimator_odometry_sub.get_instance(); + instance_change = true; + } + + if (_odometry_last.timestamp != 0) { + // reset + if (instance_change || (odometry.reset_counter != _odometry_last.reset_counter)) { + ++_odometry_reset_counter; + } + + } else { + _odometry_reset_counter = odometry.reset_counter; + } + bool publish = true; // ensure monotonically increasing timestamp_sample through reset, don't publish @@ -541,10 +559,13 @@ void EKF2Selector::PublishVehicleOdometry() publish = false; } - // save last primary estimator_odometry + // save last primary estimator_odometry as published with original resets _odometry_last = odometry; if (publish) { + // republish with total reset count and current timestamp + odometry.reset_counter = _odometry_reset_counter; + odometry.timestamp = hrt_absolute_time(); _vehicle_odometry_pub.publish(odometry); } diff --git a/src/modules/ekf2/EKF2Selector.hpp b/src/modules/ekf2/EKF2Selector.hpp index 95dc71a2e8..a45925561a 100644 --- a/src/modules/ekf2/EKF2Selector.hpp +++ b/src/modules/ekf2/EKF2Selector.hpp @@ -211,6 +211,7 @@ private: // vehicle_odometry vehicle_odometry_s _odometry_last{}; + uint8_t _odometry_reset_counter{0}; // vehicle_global_position: reset counters vehicle_global_position_s _global_position_last{}; @@ -226,6 +227,7 @@ private: uint8_t _attitude_instance_prev{INVALID_INSTANCE}; uint8_t _local_position_instance_prev{INVALID_INSTANCE}; uint8_t _global_position_instance_prev{INVALID_INSTANCE}; + uint8_t _odometry_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)}; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 24351ea38e..5a05c1b3f7 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1377,6 +1377,8 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg) visual_odom.yawspeed = NAN; visual_odom.velocity_covariance[0] = NAN; + visual_odom.reset_counter = ev.reset_counter; + _visual_odometry_pub.publish(visual_odom); } @@ -1444,6 +1446,8 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg) PX4_ERR("Body frame %" PRIu8 " not supported. Unable to publish velocity", odom.child_frame_id); } + odometry.reset_counter = odom.reset_counter; + /** * Supported local frame of reference is MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_FRD * The supported sources of the data/tesimator type are MAV_ESTIMATOR_TYPE_VISION, diff --git a/src/modules/mavlink/streams/ODOMETRY.hpp b/src/modules/mavlink/streams/ODOMETRY.hpp index c905dab0c3..2a0e148541 100644 --- a/src/modules/mavlink/streams/ODOMETRY.hpp +++ b/src/modules/mavlink/streams/ODOMETRY.hpp @@ -159,6 +159,8 @@ private: msg.velocity_covariance[i] = odom.velocity_covariance[i]; } + msg.reset_counter = odom.reset_counter; + mavlink_msg_odometry_send_struct(_mavlink->get_channel(), &msg); return true; diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 4663f4d6f1..8f24197f99 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -1358,6 +1358,8 @@ int Simulator::publish_odometry_topic(const mavlink_message_t *odom_mavlink) odom.velocity_covariance[i] = odom_msg.velocity_covariance[i]; } + odom.reset_counter = odom_msg.reset_counter; + /* Publish the odometry based on the source */ if (odom_msg.estimator_type == MAV_ESTIMATOR_TYPE_VISION || odom_msg.estimator_type == MAV_ESTIMATOR_TYPE_VIO) { _visual_odometry_pub.publish(odom); @@ -1403,6 +1405,8 @@ int Simulator::publish_odometry_topic(const mavlink_message_t *odom_mavlink) /* The velocity covariance URT - unknown */ odom.velocity_covariance[0] = NAN; + odom.reset_counter = ev.reset_counter; + /* Publish the odometry */ _visual_odometry_pub.publish(odom); }