diff --git a/src/modules/navigator/land.cpp b/src/modules/navigator/land.cpp index 55116c78e3..b51999ac55 100644 --- a/src/modules/navigator/land.cpp +++ b/src/modules/navigator/land.cpp @@ -72,11 +72,7 @@ Land::on_activation() // reset cruising speed to default _navigator->reset_cruising_speed(); - // set gimbal to neutral position (level with horizon) to reduce change of damage on landing - _navigator->acquire_gimbal_control(); - _navigator->set_gimbal_neutral(); - _navigator->release_gimbal_control(); - + _navigator->activate_set_gimbal_neutral_timer(hrt_absolute_time()); } void diff --git a/src/modules/navigator/mission_base.cpp b/src/modules/navigator/mission_base.cpp index 052968ed7e..d95e0cea15 100644 --- a/src/modules/navigator/mission_base.cpp +++ b/src/modules/navigator/mission_base.cpp @@ -180,8 +180,10 @@ MissionBase::on_inactivation() _navigator->disable_camera_trigger(); _navigator->stop_capturing_images(); - _navigator->set_gimbal_neutral(); // point forward - _navigator->release_gimbal_control(); + + if (!_navigator->get_land_detected()->landed) { + _navigator->activate_set_gimbal_neutral_timer(hrt_absolute_time()); + } if (_navigator->get_precland()->is_activated()) { _navigator->get_precland()->on_inactivation(); @@ -698,10 +700,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); - // set gimbal to neutral position (level with horizon) to reduce change of damage on landing - _navigator->acquire_gimbal_control(); - _navigator->set_gimbal_neutral(); - _navigator->release_gimbal_control(); + _navigator->activate_set_gimbal_neutral_timer(hrt_absolute_time()); } else { diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index cdabb9d36e..d70c7bd69b 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -282,6 +282,13 @@ public: 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 preproject_stop_point(double &lat, double &lon); void stop_capturing_images(); @@ -391,6 +398,9 @@ 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}; + // update subscriptions void params_update(); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c3c2fb3656..7ee9bcaabe 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -904,6 +904,8 @@ void Navigator::run() publish_mission_result(); } + neutralize_gimbal_if_control_activated(); + publish_navigator_status(); publish_distance_sensor_mode_request(); @@ -1629,6 +1631,27 @@ 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 b2898be415..8d551e87bd 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -237,10 +237,7 @@ void RTL::on_activation() break; } - // set gimbal to neutral position (level with horizon) to reduce change of damage on landing - _navigator->acquire_gimbal_control(); - _navigator->set_gimbal_neutral(); - _navigator->release_gimbal_control(); + _navigator->activate_set_gimbal_neutral_timer(hrt_absolute_time()); } void RTL::on_active()