diff --git a/msg/VehicleCommand.msg b/msg/VehicleCommand.msg index e5a9c0fdc2..801e5b279f 100644 --- a/msg/VehicleCommand.msg +++ b/msg/VehicleCommand.msg @@ -101,6 +101,8 @@ uint16 VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002 # Control a pre-programmed pay uint16 VEHICLE_CMD_FIXED_MAG_CAL_YAW = 42006 # Magnetometer calibration based on provided known yaw. This allows for fast calibration using WMM field tables in the vehicle, given only the known yaw of the vehicle. If Latitude and longitude are both zero then use the current vehicle location. uint16 VEHICLE_CMD_DO_WINCH = 42600 # Command to operate winch. +uint16 VEHICLE_CMD_EXTERNAL_POSITION_ESTIMATE = 43003 # external reset of estimator global position when deadreckoning + # PX4 vehicle commands (beyond 16 bit mavlink commands) uint32 VEHICLE_CMD_PX4_INTERNAL_START = 65537 # start of PX4 internal only vehicle commands (> UINT16_MAX) uint32 VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN = 100000 # Sets the GPS coordinates of the vehicle local origin (0,0,0) position. |Empty|Empty|Empty|Empty|Latitude|Longitude|Altitude| diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 1b9a741f36..683f24814a 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -655,6 +655,7 @@ union information_event_status_u { bool reset_hgt_to_gps : 1; ///< 14 - true when the vertical position state is reset to the gps measurement bool reset_hgt_to_rng : 1; ///< 15 - true when the vertical position state is reset to the rng measurement bool reset_hgt_to_ev : 1; ///< 16 - true when the vertical position state is reset to the ev measurement + bool reset_pos_to_ext_obs : 1; ///< 17 - true when horizontal position was reset to an external observation while deadreckoning } flags; uint32_t value; }; diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index 58a09f4cad..c539c0655a 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -341,6 +341,22 @@ void Ekf::predictState(const imuSample &imu_delayed) _height_rate_lpf = _height_rate_lpf * (1.0f - alpha_height_rate_lpf) + _state.vel(2) * alpha_height_rate_lpf; } +void +Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy, uint64_t timestamp_observation) { + + if (!_pos_ref.isInitialized()) { + return; + } + + // apply a first order correction using velocity at the delated time horizon and the delta time + timestamp_observation = math::min(_time_latest_us, timestamp_observation); + const float dt = _time_delayed_us > timestamp_observation ? static_cast(_time_delayed_us - timestamp_observation) * 1e-6f : -static_cast(timestamp_observation - _time_delayed_us) * 1e-6f; + + Vector2f pos_corrected = _pos_ref.project(lat_deg, lon_deg) + _state.vel.xy() * dt; + + resetHorizontalPositionToExternal(pos_corrected, math::max(accuracy, FLT_EPSILON)); +} + void Ekf::updateParameters() { #if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME) diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index e816c50b17..15feb6a46a 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -502,6 +502,8 @@ public: return is_healthy; } + void resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy, uint64_t timestamp_observation); + void updateParameters(); friend class AuxGlobalPosition; @@ -812,6 +814,7 @@ private: void resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var); void resetHorizontalPositionToLastKnown(); + void resetHorizontalPositionToExternal(const Vector2f &new_horiz_pos, float horiz_accuracy); void resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f &new_horz_pos_var); void resetHorizontalPositionTo(const Vector2f &new_horz_pos, const float pos_var = NAN) { resetHorizontalPositionTo(new_horz_pos, Vector2f(pos_var, pos_var)); } diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 75544f4e3f..a4ca66b010 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -171,6 +171,12 @@ bool Ekf::isHeightResetRequired() const return (continuous_bad_accel_hgt || hgt_fusion_timeout); } +void Ekf::resetHorizontalPositionToExternal(const Vector2f &new_horiz_pos, float horiz_accuracy) { + _information_events.flags.reset_pos_to_ext_obs = true; + ECL_INFO("reset position to external observation"); + resetHorizontalPositionTo(new_horiz_pos, sq(horiz_accuracy)); +} + void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_var) { const float old_vert_pos = _state.pos(2); diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index e72bf3ce71..76abef5df6 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -40,6 +40,9 @@ using matrix::Eulerf; using matrix::Quatf; using matrix::Vector3f; +static constexpr float kDefaultExternalPosAccuracy = 50.0f; // [m] +static constexpr float kMaxDelaySecondsExternalPosMeasurement = 15.0f; // [s] + pthread_mutex_t ekf2_module_mutex = PTHREAD_MUTEX_INITIALIZER; static px4::atomic _objects[EKF2_MAX_INSTANCES] {}; #if defined(CONFIG_EKF2_MULTI_INSTANCE) @@ -554,6 +557,26 @@ void EKF2::Run() _instance, latitude, longitude, static_cast(altitude)); } } + + if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_EXTERNAL_POSITION_ESTIMATE) { + if ((_ekf.control_status_flags().wind_dead_reckoning || _ekf.control_status_flags().inertial_dead_reckoning) && + PX4_ISFINITE(vehicle_command.param2) && PX4_ISFINITE(vehicle_command.param5) && PX4_ISFINITE(vehicle_command.param6)) { + + const float measurement_delay_seconds = math::constrain(vehicle_command.param2, 0.0f, + kMaxDelaySecondsExternalPosMeasurement); + const uint64_t timestamp_observation = vehicle_command.timestamp - measurement_delay_seconds * 1_s; + + float accuracy = kDefaultExternalPosAccuracy; + + if (PX4_ISFINITE(vehicle_command.param3) && vehicle_command.param3 > FLT_EPSILON) { + accuracy = vehicle_command.param3; + } + + // TODO add check for lat and long validity + _ekf.resetGlobalPosToExternalObservation(vehicle_command.param5, vehicle_command.param6, + accuracy, timestamp_observation); + } + } } }