diff --git a/src/modules/navigator/land.cpp b/src/modules/navigator/land.cpp index b51999ac55..08834b9e17 100644 --- a/src/modules/navigator/land.cpp +++ b/src/modules/navigator/land.cpp @@ -72,7 +72,7 @@ Land::on_activation() // reset cruising speed to default _navigator->reset_cruising_speed(); - _navigator->activate_set_gimbal_neutral_timer(hrt_absolute_time()); + _navigator->gimbal_neutral_delayed(); } void diff --git a/src/modules/navigator/mission_base.cpp b/src/modules/navigator/mission_base.cpp index 79fa7aefd9..f05ae7b1d6 100644 --- a/src/modules/navigator/mission_base.cpp +++ b/src/modules/navigator/mission_base.cpp @@ -182,7 +182,7 @@ MissionBase::on_inactivation() _navigator->stop_capturing_images(); if (!_navigator->get_land_detected()->landed) { - _navigator->activate_set_gimbal_neutral_timer(hrt_absolute_time()); + _navigator->gimbal_neutral_delayed(); } if (_navigator->get_precland()->is_activated()) { @@ -702,7 +702,7 @@ void MissionBase::handleLanding(WorkItemType &new_work_item_type, mission_item_s // if the vehicle drifted off the path during back-transition it should just go straight to the landing point _navigator->reset_position_setpoint(pos_sp_triplet->previous); - _navigator->activate_set_gimbal_neutral_timer(hrt_absolute_time()); + _navigator->gimbal_neutral_delayed(); } else { diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index d70c7bd69b..1e7efdb7dd 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -60,6 +60,7 @@ #if CONFIG_NAVIGATOR_ADSB #include #endif // CONFIG_NAVIGATOR_ADSB +#include #include #include #include @@ -281,13 +282,7 @@ public: void acquire_gimbal_control(); void release_gimbal_control(); void set_gimbal_neutral(); - - /* Set gimbal to neutral position (level with horizon) to reduce risk of damage on landing. - The commands are executed after time delay. */ - void neutralize_gimbal_if_control_activated(); - /* Accepts a new timestamp only if the current timestamp is UINT64_MAX, preventing the - timer from resetting during an ongoing neutral command. */ - void activate_set_gimbal_neutral_timer(const hrt_abstime timestamp); + void gimbal_neutral_delayed() { _gimbal_neutral_hysteresis.set_state_and_update(true, hrt_absolute_time()); }; // Acquires and releases control void preproject_stop_point(double &lat, double &lon); @@ -397,9 +392,7 @@ private: bool _is_capturing_images{false}; // keep track if we need to stop capturing images - - // timer to trigger a delayed set gimbal neutral command - hrt_abstime _gimbal_neutral_activation_time{UINT64_MAX}; + systemlib::Hysteresis _gimbal_neutral_hysteresis{false}; ///< Delay to avoid race condition with previous mode still wanting gimbal control e.g. slow mode flight task // update subscriptions void params_update(); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index b07d92c176..644ae6a9af 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -114,6 +114,8 @@ Navigator::Navigator() : _distance_sensor_mode_change_request_pub.get().request_on_off = distance_sensor_mode_change_request_s::REQUEST_OFF; _distance_sensor_mode_change_request_pub.update(); + _gimbal_neutral_hysteresis.set_hysteresis_time_from(false, 250_ms); + reset_triplets(); } @@ -924,7 +926,16 @@ void Navigator::run() publish_mission_result(); } - neutralize_gimbal_if_control_activated(); + // Set gimbal neutral if requested and delay is over + const hrt_abstime now = hrt_absolute_time(); + _gimbal_neutral_hysteresis.update(now); + + if (_gimbal_neutral_hysteresis.get_state()) { + acquire_gimbal_control(); + set_gimbal_neutral(); + release_gimbal_control(); + _gimbal_neutral_hysteresis.set_state_and_update(false, now); // instantly resets from true to false + } publish_navigator_status(); @@ -1651,27 +1662,6 @@ void Navigator::set_gimbal_neutral() publish_vehicle_command(vehicle_command); } -void Navigator::activate_set_gimbal_neutral_timer(const hrt_abstime timestamp) -{ - if (_gimbal_neutral_activation_time == UINT64_MAX) { - _gimbal_neutral_activation_time = timestamp; - } -} - -void Navigator::neutralize_gimbal_if_control_activated() -{ - const hrt_abstime now{hrt_absolute_time()}; - - // The time delay must be sufficiently long to allow flight tasks to complete its - // destruction and release gimbal control before the navigator takes control of the gimbal. - if (_gimbal_neutral_activation_time != UINT64_MAX && now > _gimbal_neutral_activation_time + 250_ms) { - acquire_gimbal_control(); - set_gimbal_neutral(); - release_gimbal_control(); - _gimbal_neutral_activation_time = UINT64_MAX; - } -} - void Navigator::sendWarningDescentStoppedDueToTerrain() { mavlink_log_critical(&_mavlink_log_pub, "Terrain collision risk, descent is stopped\t"); diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 8d551e87bd..97318dde10 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -237,7 +237,7 @@ void RTL::on_activation() break; } - _navigator->activate_set_gimbal_neutral_timer(hrt_absolute_time()); + _navigator->gimbal_neutral_delayed(); } void RTL::on_active()