diff --git a/msg/VehicleGlobalPosition.msg b/msg/VehicleGlobalPosition.msg index c7d9ee7812..e37155fe25 100644 --- a/msg/VehicleGlobalPosition.msg +++ b/msg/VehicleGlobalPosition.msg @@ -14,8 +14,10 @@ float32 alt # Altitude AMSL, (meters) float32 alt_ellipsoid # Altitude above ellipsoid, (meters) float32 delta_alt # Reset delta for altitude +float32 delta_terrain # Reset delta for terrain uint8 lat_lon_reset_counter # Counter for reset events on horizontal position coordinates uint8 alt_reset_counter # Counter for reset events on altitude +uint8 terrain_reset_counter # Counter for reset events on terrain float32 eph # Standard deviation of horizontal position error, (metres) float32 epv # Standard deviation of vertical position error, (metres) diff --git a/msg/VehicleLocalPosition.msg b/msg/VehicleLocalPosition.msg index 25551ab4fb..0e74ac0f4b 100644 --- a/msg/VehicleLocalPosition.msg +++ b/msg/VehicleLocalPosition.msg @@ -55,9 +55,13 @@ float64 ref_lon # Reference point longitude, (degrees) float32 ref_alt # Reference altitude AMSL, (metres) # Distance to surface +bool dist_bottom_valid # true if distance to bottom surface is valid float32 dist_bottom # Distance from from bottom surface to ground, (metres) float32 dist_bottom_var # terrain estimate variance (m^2) -bool dist_bottom_valid # true if distance to bottom surface is valid + +float32 delta_dist_bottom # Amount of vertical shift of dist bottom estimate in latest reset [m] +uint8 dist_bottom_reset_counter # Index of latest dist bottom estimate reset + uint8 dist_bottom_sensor_bitfield # bitfield indicating what type of sensor is used to estimate dist_bottom uint8 DIST_BOTTOM_SENSOR_NONE = 0 uint8 DIST_BOTTOM_SENSOR_RANGE = 1 # (1 << 0) a range sensor is used to estimate dist_bottom field diff --git a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp index 89bbb6add0..a6c0569ac7 100644 --- a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp @@ -219,15 +219,29 @@ void Ekf::resetFlowFusion() void Ekf::resetTerrainToFlow() { ECL_INFO("reset hagl to flow"); + // TODO: use the flow data - _state.terrain = fmaxf(0.0f, _state.pos(2)); + const float new_terrain = fmaxf(0.0f, _state.pos(2)); + const float delta_terrain = new_terrain - _state.terrain; + _state.terrain = new_terrain; P.uncorrelateCovarianceSetVariance(State::terrain.idx, 100.f); - _terrain_vpos_reset_counter++; resetAidSourceStatusZeroInnovation(_aid_src_optical_flow); _innov_check_fail_status.flags.reject_optflow_X = false; _innov_check_fail_status.flags.reject_optflow_Y = false; + + + // record the state change + if (_state_reset_status.reset_count.hagl == _state_reset_count_prev.hagl) { + _state_reset_status.hagl_change = delta_terrain; + + } else { + // there's already a reset this update, accumulate total delta + _state_reset_status.hagl_change += delta_terrain; + } + + _state_reset_status.reset_count.hagl++; } void Ekf::stopFlowFusion() diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp index ce8377e06b..541dede819 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp @@ -267,9 +267,23 @@ float Ekf::getRngVar() const void Ekf::resetTerrainToRng(estimator_aid_source1d_s &aid_src) { - _state.terrain = _state.pos(2) + aid_src.observation; + const float new_terrain = _state.pos(2) + aid_src.observation; + const float delta_terrain = new_terrain - _state.terrain; + + _state.terrain = new_terrain; P.uncorrelateCovarianceSetVariance(State::terrain.idx, aid_src.observation_variance); - _terrain_vpos_reset_counter++; + + // record the state change + if (_state_reset_status.reset_count.hagl == _state_reset_count_prev.hagl) { + _state_reset_status.hagl_change = delta_terrain; + + } else { + // there's already a reset this update, accumulate total delta + _state_reset_status.hagl_change += delta_terrain; + } + + _state_reset_status.reset_count.hagl++; + aid_src.time_last_fuse = _time_delayed_us; } diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 85ea4a7625..f23f514fd6 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -104,9 +104,6 @@ public: float getTerrainVertPos() const { return _state.terrain; }; float getHagl() const { return _state.terrain - _state.pos(2); } - // get the number of times the vertical terrain position has been reset - uint8_t getTerrainVertPosResetCounter() const { return _terrain_vpos_reset_counter; }; - // get the terrain variance float getTerrainVariance() const { return P(State::terrain.idx, State::terrain.idx); } @@ -358,6 +355,13 @@ public: *counter = _state_reset_status.reset_count.posD; } + uint8_t get_hagl_reset_count() const { return _state_reset_status.reset_count.hagl; } + void get_hagl_reset(float *delta, uint8_t *counter) const + { + *delta = _state_reset_status.hagl_change; + *counter = _state_reset_status.reset_count.hagl; + } + // 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.reset_count.velD; } void get_velD_reset(float *delta, uint8_t *counter) const @@ -551,6 +555,7 @@ private: uint8_t posNE{0}; ///< number of horizontal position reset events (allow to wrap if count exceeds 255) uint8_t posD{0}; ///< number of vertical position reset events (allow to wrap if count exceeds 255) uint8_t quat{0}; ///< number of quaternion reset events (allow to wrap if count exceeds 255) + uint8_t hagl{0}; ///< number of height above ground level reset events (allow to wrap if count exceeds 255) }; struct StateResets { @@ -559,6 +564,7 @@ private: Vector2f posNE_change; ///< North, East position change due to last reset (m) float posD_change; ///< Down position change due to last reset (m) Quatf quat_change; ///< quaternion delta due to last reset - multiply pre-reset quaternion by this to get post-reset quaternion + float hagl_change; ///< Height above ground level change due to last reset (m) StateResetCounts reset_count{}; }; @@ -599,8 +605,6 @@ private: #if defined(CONFIG_EKF2_TERRAIN) // Terrain height state estimation - uint8_t _terrain_vpos_reset_counter{0}; ///< number of times _terrain_vpos has been reset - float _last_on_ground_posD{0.0f}; ///< last vertical position when the in_air status was false (m) #endif // CONFIG_EKF2_TERRAIN diff --git a/src/modules/ekf2/EKF/position_fusion.cpp b/src/modules/ekf2/EKF/position_fusion.cpp index 6cc503de73..cf8e7de04b 100644 --- a/src/modules/ekf2/EKF/position_fusion.cpp +++ b/src/modules/ekf2/EKF/position_fusion.cpp @@ -167,7 +167,18 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v #if defined(CONFIG_EKF2_TERRAIN) _state.terrain += delta_z; -#endif + + // record the state change + if (_state_reset_status.reset_count.hagl == _state_reset_count_prev.hagl) { + _state_reset_status.hagl_change = delta_z; + + } else { + // there's already a reset this update, accumulate total delta + _state_reset_status.hagl_change += delta_z; + } + + _state_reset_status.reset_count.hagl++; +#endif // CONFIG_EKF2_TERRAIN // Reset the timout timer _time_last_hgt_fuse = _time_delayed_us; diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index cee124db0c..44656a2067 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1191,6 +1191,9 @@ void EKF2::PublishGlobalPosition(const hrt_abstime ×tamp) global_pos.terrain_alt_valid = true; } + float delta_hagl = 0.f; + _ekf.get_hagl_reset(&delta_hagl, &global_pos.terrain_reset_counter); + global_pos.delta_terrain = -delta_z; #endif // CONFIG_EKF2_TERRAIN global_pos.dead_reckoning = _ekf.control_status_flags().inertial_dead_reckoning @@ -1619,9 +1622,10 @@ void EKF2::PublishLocalPosition(const hrt_abstime ×tamp) #if defined(CONFIG_EKF2_TERRAIN) // Distance to bottom surface (ground) in meters, must be positive + lpos.dist_bottom_valid = _ekf.isTerrainEstimateValid(); lpos.dist_bottom = math::max(_ekf.getHagl(), 0.f); lpos.dist_bottom_var = _ekf.getTerrainVariance(); - lpos.dist_bottom_valid = _ekf.isTerrainEstimateValid(); + _ekf.get_hagl_reset(&lpos.delta_dist_bottom, &lpos.dist_bottom_reset_counter); lpos.dist_bottom_sensor_bitfield = vehicle_local_position_s::DIST_BOTTOM_SENSOR_NONE; diff --git a/src/modules/ekf2/EKF2Selector.cpp b/src/modules/ekf2/EKF2Selector.cpp index 99fb863eed..f5f67e2cde 100644 --- a/src/modules/ekf2/EKF2Selector.cpp +++ b/src/modules/ekf2/EKF2Selector.cpp @@ -479,18 +479,32 @@ void EKF2Selector::PublishVehicleLocalPosition() _delta_heading_reset = matrix::wrap_pi(local_position.heading - _local_position_last.heading); } + // HAGL (dist_bottom) reset + if (!instance_change + && (local_position.dist_bottom_reset_counter == _local_position_last.dist_bottom_reset_counter + 1)) { + ++_hagl_reset_counter; + _delta_hagl_reset = local_position.delta_dist_bottom; + + } else if (instance_change + || (local_position.dist_bottom_reset_counter != _local_position_last.dist_bottom_reset_counter)) { + ++_hagl_reset_counter; + _delta_hagl_reset = local_position.dist_bottom - _local_position_last.dist_bottom; + } + } 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; + _hagl_reset_counter = local_position.dist_bottom_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; + _delta_hagl_reset = local_position.delta_dist_bottom; } bool publish = true; @@ -513,6 +527,7 @@ void EKF2Selector::PublishVehicleLocalPosition() local_position.vxy_reset_counter = _vxy_reset_counter; local_position.vz_reset_counter = _vz_reset_counter; local_position.heading_reset_counter = _heading_reset_counter; + local_position.dist_bottom_reset_counter = _hagl_reset_counter; _delta_xy_reset.copyTo(local_position.delta_xy); local_position.delta_z = _delta_z_reset; @@ -612,6 +627,16 @@ void EKF2Selector::PublishVehicleGlobalPosition() _delta_alt_reset = global_position.delta_alt - _global_position_last.delta_alt; } + // terrain reset + if (!instance_change && (global_position.terrain_reset_counter == _global_position_last.terrain_reset_counter + 1)) { + ++_terrain_reset_counter; + _delta_terrain_reset = global_position.delta_terrain; + + } else if (instance_change || (global_position.terrain_reset_counter != _global_position_last.terrain_reset_counter)) { + ++_terrain_reset_counter; + _delta_terrain_reset = global_position.delta_terrain - _global_position_last.delta_terrain; + } + } else { _lat_lon_reset_counter = global_position.lat_lon_reset_counter; _alt_reset_counter = global_position.alt_reset_counter; diff --git a/src/modules/ekf2/EKF2Selector.hpp b/src/modules/ekf2/EKF2Selector.hpp index 6507233bd3..535af97e6d 100644 --- a/src/modules/ekf2/EKF2Selector.hpp +++ b/src/modules/ekf2/EKF2Selector.hpp @@ -202,11 +202,14 @@ private: matrix::Vector2f _delta_vxy_reset{}; float _delta_vz_reset{0.f}; float _delta_heading_reset{0}; + float _delta_hagl_reset{0.f}; + uint8_t _xy_reset_counter{0}; uint8_t _z_reset_counter{0}; uint8_t _vxy_reset_counter{0}; uint8_t _vz_reset_counter{0}; uint8_t _heading_reset_counter{0}; + uint8_t _hagl_reset_counter{0}; // vehicle_odometry vehicle_odometry_s _odometry_last{}; @@ -217,8 +220,11 @@ private: double _delta_lat_reset{0}; double _delta_lon_reset{0}; float _delta_alt_reset{0.f}; + float _delta_terrain_reset{0.f}; + uint8_t _lat_lon_reset_counter{0}; uint8_t _alt_reset_counter{0}; + uint8_t _terrain_reset_counter{0}; // wind estimate wind_s _wind_last{}; diff --git a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp index 063b78dcf6..55b587d270 100644 --- a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp +++ b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp @@ -63,6 +63,11 @@ void FlightTask::_checkEkfResetCounters() _reset_counters.z = _sub_vehicle_local_position.get().z_reset_counter; } + if (_sub_vehicle_local_position.get().dist_bottom_reset_counter != _reset_counters.hagl) { + _ekfResetHandlerHagl(_sub_vehicle_local_position.get().delta_dist_bottom); + _reset_counters.hagl = _sub_vehicle_local_position.get().dist_bottom_reset_counter; + } + if (_sub_vehicle_local_position.get().vz_reset_counter != _reset_counters.vz) { _ekfResetHandlerVelocityZ(_sub_vehicle_local_position.get().delta_vz); _reset_counters.vz = _sub_vehicle_local_position.get().vz_reset_counter; @@ -138,7 +143,7 @@ void FlightTask::_evaluateVehicleLocalPosition() // distance to bottom if (_sub_vehicle_local_position.get().dist_bottom_valid && PX4_ISFINITE(_sub_vehicle_local_position.get().dist_bottom)) { - _dist_to_bottom = _sub_vehicle_local_position.get().dist_bottom; + _dist_to_bottom = _sub_vehicle_local_position.get().dist_bottom; } // global frame reference coordinates to enable conversions diff --git a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp index 7e938bb0a9..6ee861f3cd 100644 --- a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp +++ b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp @@ -61,6 +61,7 @@ struct ekf_reset_counters_s { uint8_t z; uint8_t vz; uint8_t heading; + uint8_t hagl; }; class FlightTask : public ModuleParams @@ -191,6 +192,7 @@ protected: virtual void _ekfResetHandlerPositionXY(const matrix::Vector2f &delta_xy) {}; virtual void _ekfResetHandlerVelocityXY(const matrix::Vector2f &delta_vxy) {}; virtual void _ekfResetHandlerPositionZ(float delta_z) {}; + virtual void _ekfResetHandlerHagl(float delta_hagl) {}; virtual void _ekfResetHandlerVelocityZ(float delta_vz) {}; virtual void _ekfResetHandlerHeading(float delta_psi) {};