From c8c6203eb5e3a555f608b8fcd3b60deac0ae829b Mon Sep 17 00:00:00 2001 From: Marco Hauswirth Date: Tue, 10 Sep 2024 18:19:05 +0200 Subject: [PATCH] fix max-hagl restriction to position/altitude control --- msg/VehicleLocalPosition.msg | 3 +- src/drivers/ins/vectornav/VectorNav.cpp | 2 +- src/modules/ekf2/EKF/ekf.h | 5 ++- src/modules/ekf2/EKF/ekf_helper.cpp | 25 ++++++++--- src/modules/ekf2/EKF2.cpp | 10 +++-- .../FlightTaskManualAcceleration.cpp | 1 + .../FlightTaskManualAltitude.cpp | 41 ++++++------------- .../FlightTaskManualAltitude.hpp | 1 + .../BlockLocalPositionEstimator.cpp | 3 +- src/modules/mavlink/mavlink_receiver.cpp | 3 +- src/modules/navigator/mission_block.cpp | 2 +- .../simulator_mavlink/SimulatorMavlink.cpp | 3 +- 12 files changed, 55 insertions(+), 44 deletions(-) diff --git a/msg/VehicleLocalPosition.msg b/msg/VehicleLocalPosition.msg index 0e74ac0f4bfc..5e9bbc415087 100644 --- a/msg/VehicleLocalPosition.msg +++ b/msg/VehicleLocalPosition.msg @@ -78,7 +78,8 @@ bool dead_reckoning # True if this position is estimated thr float32 vxy_max # maximum horizontal speed - set to 0 when limiting not required (meters/sec) float32 vz_max # maximum vertical speed - set to 0 when limiting not required (meters/sec) float32 hagl_min # minimum height above ground level - set to 0 when limiting not required (meters) -float32 hagl_max # maximum height above ground level - set to 0 when limiting not required (meters) +float32 hagl_max_z # maximum height above ground level for z-control - set to 0 when limiting not required (meters) +float32 hagl_max_xy # maximum height above ground level for xy-control - set to 0 when limiting not required (meters) # TOPICS vehicle_local_position vehicle_local_position_groundtruth external_ins_local_position # TOPICS estimator_local_position diff --git a/src/drivers/ins/vectornav/VectorNav.cpp b/src/drivers/ins/vectornav/VectorNav.cpp index 85dd4c1846af..b6bc06f1cefd 100644 --- a/src/drivers/ins/vectornav/VectorNav.cpp +++ b/src/drivers/ins/vectornav/VectorNav.cpp @@ -328,7 +328,7 @@ void VectorNav::sensorCallback(VnUartPacket *packet) local_position.vxy_max = INFINITY; local_position.vz_max = INFINITY; local_position.hagl_min = INFINITY; - local_position.hagl_max = INFINITY; + local_position.hagl_max_z = INFINITY; local_position.unaided_heading = NAN; local_position.timestamp = hrt_absolute_time(); diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index a7d59b47ef5d..be9980395c20 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -280,8 +280,9 @@ class Ekf final : public EstimatorInterface // vxy_max : Maximum ground relative horizontal speed (meters/sec). NaN when limiting is not needed. // vz_max : Maximum ground relative vertical speed (meters/sec). NaN when limiting is not needed. // hagl_min : Minimum height above ground (meters). NaN when limiting is not needed. - // hagl_max : Maximum height above ground (meters). NaN when limiting is not needed. - void get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) const; + // hagl_max_z : Maximum height above ground for vertical position control (meters). NaN when limiting is not needed. + // hagl_max_xy : Maximum height above ground for horizontal position control (meters). NaN when limiting is not needed. + void get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max_z, float *hagl_max_xy) const; void resetGyroBias(); void resetGyroBiasCov(); diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index f33cbcd5bc43..c7f9e5fc7f9f 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -318,13 +318,15 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const *ekf_evv = sqrtf(P(State::vel.idx + 2, State::vel.idx + 2)); } -void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) const +void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max_z, + float *hagl_max_xy) const { // Do not require limiting by default *vxy_max = NAN; *vz_max = NAN; *hagl_min = NAN; - *hagl_max = NAN; + *hagl_max_z = NAN; + *hagl_max_xy = NAN; #if defined(CONFIG_EKF2_RANGE_FINDER) // Calculate range finder limits @@ -339,7 +341,7 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl // Keep within range sensor limit when using rangefinder as primary height source if (relying_on_rangefinder) { *hagl_min = rangefinder_hagl_min; - *hagl_max = rangefinder_hagl_max; + *hagl_max_z = rangefinder_hagl_max; } # if defined(CONFIG_EKF2_OPTICAL_FLOW) @@ -362,11 +364,24 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl const float flow_constrained_height = math::constrain(getHagl(), flow_hagl_min, flow_hagl_max); // Allow ground relative velocity to use 50% of available flow sensor range to allow for angular motion - const float flow_vxy_max = 0.5f * _flow_max_rate * flow_constrained_height; + float flow_vxy_max = 0.5f * _flow_max_rate * flow_constrained_height; + flow_hagl_max = math::max(flow_hagl_max * 0.9f, flow_hagl_max - 1.0f); + + if (!control_status_flags().fixed_wing) { + float max_hagl_factor = (_state.terrain - _state.pos(2)) / flow_hagl_max; + + if (max_hagl_factor > 0.9f) { + // interpolate velocity limit between 90% and 100% of maximum height above ground + // to avoid sudden loss of valid position estimates due to terrain height changes + max_hagl_factor = math::min(max_hagl_factor, 1.f); + flow_vxy_max = flow_vxy_max + (max_hagl_factor - 0.9f) * (1.f - flow_vxy_max) * 10.f; + + } + } *vxy_max = flow_vxy_max; *hagl_min = flow_hagl_min; - *hagl_max = flow_hagl_max; + *hagl_max_xy = flow_hagl_max; } # endif // CONFIG_EKF2_OPTICAL_FLOW diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 38e5660e4ff4..daf5ad1f4f79 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1627,7 +1627,7 @@ void EKF2::PublishLocalPosition(const hrt_abstime ×tamp) || _ekf.control_status_flags().wind_dead_reckoning; // get control limit information - _ekf.get_ekf_ctrl_limits(&lpos.vxy_max, &lpos.vz_max, &lpos.hagl_min, &lpos.hagl_max); + _ekf.get_ekf_ctrl_limits(&lpos.vxy_max, &lpos.vz_max, &lpos.hagl_min, &lpos.hagl_max_z, &lpos.hagl_max_xy); // convert NaN to INFINITY if (!PX4_ISFINITE(lpos.vxy_max)) { @@ -1642,8 +1642,12 @@ void EKF2::PublishLocalPosition(const hrt_abstime ×tamp) lpos.hagl_min = INFINITY; } - if (!PX4_ISFINITE(lpos.hagl_max)) { - lpos.hagl_max = INFINITY; + if (!PX4_ISFINITE(lpos.hagl_max_z)) { + lpos.hagl_max_z = INFINITY; + } + + if (!PX4_ISFINITE(lpos.hagl_max_xy)) { + lpos.hagl_max_xy = INFINITY; } // publish vehicle local position data diff --git a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp index a1ea2b6b8050..07b9d7f1d1d1 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp @@ -59,6 +59,7 @@ bool FlightTaskManualAcceleration::activate(const trajectory_setpoint_s &last_se bool FlightTaskManualAcceleration::update() { + setMaxDistanceToGround(_sub_vehicle_local_position.get().hagl_max_xy); bool ret = FlightTaskManualAltitudeSmoothVel::update(); _stick_acceleration_xy.generateSetpoints(_sticks.getPitchRollExpo(), _yaw, _yaw_setpoint, _position, diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index 22eb458c81e2..2ef66cf22cb4 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -81,11 +81,8 @@ void FlightTaskManualAltitude::_updateConstraintsFromEstimator() _min_distance_to_ground = -INFINITY; } - if (PX4_ISFINITE(_sub_vehicle_local_position.get().hagl_max)) { - _max_distance_to_ground = _sub_vehicle_local_position.get().hagl_max; - - } else { - _max_distance_to_ground = INFINITY; + if (!PX4_ISFINITE(_max_distance_to_ground) && PX4_ISFINITE(_sub_vehicle_local_position.get().hagl_max_z)) { + _max_distance_to_ground = _sub_vehicle_local_position.get().hagl_max_z; } } @@ -155,7 +152,6 @@ void FlightTaskManualAltitude::_updateAltitudeLock() // terrain following _terrainFollowing(apply_brake, stopped); // respect maximum altitude - _respectMaxAltitude(); } else { // normal mode where height is dependent on local frame @@ -185,9 +181,11 @@ void FlightTaskManualAltitude::_updateAltitudeLock() // user demands velocity change _position_setpoint(2) = NAN; // ensure that maximum altitude is respected - _respectMaxAltitude(); } + } + + _respectMaxAltitude(); } void FlightTaskManualAltitude::_respectMinAltitude() @@ -229,28 +227,14 @@ void FlightTaskManualAltitude::_respectMaxAltitude() { if (PX4_ISFINITE(_dist_to_bottom)) { - // if there is a valid maximum distance to ground, linearly increase speed limit with distance - // below the maximum, preserving control loop vertical position error gain. - // TODO: manipulate the velocity setpoint instead of tweaking the saturation of the controller - if (PX4_ISFINITE(_max_distance_to_ground)) { - _constraints.speed_up = math::constrain(_param_mpc_z_p.get() * (_max_distance_to_ground - _dist_to_bottom), - -_param_mpc_z_vel_max_dn.get(), _param_mpc_z_vel_max_up.get()); + if ((-_position_setpoint(2) + _position(2) > _max_distance_to_ground - _dist_to_bottom + || -_velocity_setpoint(2) > _max_distance_to_ground - _dist_to_bottom + || _dist_to_bottom > _max_distance_to_ground) + && _velocity_setpoint(2) <= 0.f) { - } else { - _constraints.speed_up = _param_mpc_z_vel_max_up.get(); - } - - // if distance to bottom exceeded maximum distance, slowly approach maximum distance - if (_dist_to_bottom > _max_distance_to_ground) { - // difference between current distance to ground and maximum distance to ground - const float delta_distance_to_max = _dist_to_bottom - _max_distance_to_ground; - // set position setpoint to maximum distance to ground - _position_setpoint(2) = _position(2) + delta_distance_to_max; - // limit speed downwards to 0.7m/s - _constraints.speed_down = math::min(_param_mpc_z_vel_max_dn.get(), 0.7f); - - } else { - _constraints.speed_down = _param_mpc_z_vel_max_dn.get(); + _position_setpoint(2) = _position(2) - _max_distance_to_ground + _dist_to_bottom; + _velocity_setpoint(2) = - math::constrain(_param_mpc_z_p.get() * (_max_distance_to_ground - _dist_to_bottom), + -_param_mpc_z_vel_max_dn.get(), _param_mpc_z_vel_max_up.get()); } } } @@ -301,6 +285,7 @@ bool FlightTaskManualAltitude::update() _scaleSticks(); _updateSetpoints(); _constraints.want_takeoff = _checkTakeoff(); + _max_distance_to_ground = INFINITY; return ret; } diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp index 527e5ad6f753..d83ada55aec9 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp @@ -53,6 +53,7 @@ class FlightTaskManualAltitude : public FlightTask bool activate(const trajectory_setpoint_s &last_setpoint) override; bool updateInitialize() override; bool update() override; + void setMaxDistanceToGround(float max_distance) { _max_distance_to_ground = max_distance; } protected: void _ekfResetHandlerHeading(float delta_psi) override; /**< adjust heading setpoint in case of EKF reset event */ diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index 8517ff03b0eb..f28bb78c0848 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -643,7 +643,8 @@ void BlockLocalPositionEstimator::publishLocalPos() _pub_lpos.get().vxy_max = INFINITY; _pub_lpos.get().vz_max = INFINITY; _pub_lpos.get().hagl_min = INFINITY; - _pub_lpos.get().hagl_max = INFINITY; + _pub_lpos.get().hagl_max_z = INFINITY; + _pub_lpos.get().hagl_max_xy = INFINITY; _pub_lpos.get().timestamp = hrt_absolute_time();; _pub_lpos.update(); } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 9c53b16f3693..7709e4f9bf00 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2729,7 +2729,8 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) hil_local_pos.vxy_max = INFINITY; hil_local_pos.vz_max = INFINITY; hil_local_pos.hagl_min = INFINITY; - hil_local_pos.hagl_max = INFINITY; + hil_local_pos.hagl_max_z = INFINITY; + hil_local_pos.hagl_max_xy = INFINITY; hil_local_pos.timestamp = hrt_absolute_time(); _local_pos_pub.publish(hil_local_pos); } diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 84d95ae5a310..42609dff309a 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -1057,7 +1057,7 @@ void MissionBlock::updateMaxHaglFailsafe() const float target_alt = _navigator->get_position_setpoint_triplet()->current.alt; if (_navigator->get_global_position()->terrain_alt_valid - && ((target_alt - _navigator->get_global_position()->terrain_alt) > _navigator->get_local_position()->hagl_max)) { + && ((target_alt - _navigator->get_global_position()->terrain_alt) > _navigator->get_local_position()->hagl_max_xy)) { // Handle case where the altitude setpoint is above the maximum HAGL (height above ground level) mavlink_log_info(_navigator->get_mavlink_log_pub(), "Target altitude higher than max HAGL\t"); events::send(events::ID("navigator_fail_max_hagl"), events::Log::Error, "Target altitude higher than max HAGL"); diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp index 20eed499b5f0..a056307cbdd3 100644 --- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp +++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp @@ -609,7 +609,8 @@ void SimulatorMavlink::handle_message_hil_state_quaternion(const mavlink_message hil_lpos.vxy_max = std::numeric_limits::infinity(); hil_lpos.vz_max = std::numeric_limits::infinity(); hil_lpos.hagl_min = std::numeric_limits::infinity(); - hil_lpos.hagl_max = std::numeric_limits::infinity(); + hil_lpos.hagl_max_z = std::numeric_limits::infinity(); + hil_lpos.hagl_max_xy = std::numeric_limits::infinity(); // always publish ground truth attitude message _lpos_ground_truth_pub.publish(hil_lpos);