diff --git a/src/modules/navigator/CMakeLists.txt b/src/modules/navigator/CMakeLists.txt index c55cae3f07..ea6543678f 100644 --- a/src/modules/navigator/CMakeLists.txt +++ b/src/modules/navigator/CMakeLists.txt @@ -70,4 +70,5 @@ px4_add_module( motion_planning mission_feasibility_checker rtl_time_estimator + gimbal_control ) diff --git a/src/modules/navigator/land.cpp b/src/modules/navigator/land.cpp index 2c351ffa6e..08c802943e 100644 --- a/src/modules/navigator/land.cpp +++ b/src/modules/navigator/land.cpp @@ -73,9 +73,9 @@ Land::on_activation() _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->_gimbal_control.acquireGimbalControlIfNeeded(); + _navigator->_gimbal_control.setNeutral(); + _navigator->_gimbal_control.releaseGimbalControlIfNeeded(); } diff --git a/src/modules/navigator/mission_base.cpp b/src/modules/navigator/mission_base.cpp index 0d653a1882..fb814e5baf 100644 --- a/src/modules/navigator/mission_base.cpp +++ b/src/modules/navigator/mission_base.cpp @@ -180,8 +180,8 @@ MissionBase::on_inactivation() _navigator->disable_camera_trigger(); _navigator->stop_capturing_images(); - _navigator->set_gimbal_neutral(); // point forward - _navigator->release_gimbal_control(); + _navigator->_gimbal_control.setNeutral(); // point forward + _navigator->_gimbal_control.releaseGimbalControlIfNeeded(); if (_navigator->get_precland()->is_activated()) { _navigator->get_precland()->on_inactivation(); @@ -683,9 +683,9 @@ void MissionBase::handleLanding(WorkItemType &new_work_item_type, mission_item_s _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->_gimbal_control.acquireGimbalControlIfNeeded(); + _navigator->_gimbal_control.setNeutral(); + _navigator->_gimbal_control.releaseGimbalControlIfNeeded(); } else { diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 1b05628a6c..dcc8e0e39d 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -524,7 +524,7 @@ MissionBlock::issue_command(const mission_item_s &item) // This is to support legacy DO_MOUNT_CONTROL as part of a mission. if (item.nav_cmd == NAV_CMD_DO_MOUNT_CONTROL) { - _navigator->acquire_gimbal_control(); + _navigator->_gimbal_control.acquireGimbalControlIfNeeded(); } // Mission item's NAV_CMD enums directly map to the according vehicle command diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 5e30d22ccc..b0dd92c911 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 @@ -277,9 +278,7 @@ public: bool force_vtol(); - void acquire_gimbal_control(); - void release_gimbal_control(); - void set_gimbal_neutral(); + Gimbal _gimbal_control{this}; void preproject_stop_point(double &lat, double &lon); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 201eeb6123..674aa0fabc 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1543,29 +1543,6 @@ void Navigator::publish_vehicle_command_ack(const vehicle_command_s &cmd, uint8_ _vehicle_cmd_ack_pub.publish(command_ack); } -void Navigator::acquire_gimbal_control() -{ - vehicle_command_s vcmd = {}; - vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE; - vcmd.param1 = _vstatus.system_id; - vcmd.param2 = _vstatus.component_id; - vcmd.param3 = -1.0f; // Leave unchanged. - vcmd.param4 = -1.0f; // Leave unchanged. - publish_vehicle_cmd(&vcmd); -} - -void Navigator::release_gimbal_control() -{ - vehicle_command_s vcmd = {}; - vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE; - vcmd.param1 = -3.0f; // Remove control if it had it. - vcmd.param2 = -3.0f; // Remove control if it had it. - vcmd.param3 = -1.0f; // Leave unchanged. - vcmd.param4 = -1.0f; // Leave unchanged. - publish_vehicle_cmd(&vcmd); -} - - void Navigator::stop_capturing_images() { @@ -1629,18 +1606,6 @@ void Navigator::disable_camera_trigger() publish_vehicle_cmd(&cmd); } -void Navigator::set_gimbal_neutral() -{ - vehicle_command_s vcmd = {}; - vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW; - vcmd.param1 = NAN; - vcmd.param2 = NAN; - vcmd.param3 = NAN; - vcmd.param4 = NAN; - vcmd.param5 = gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_NEUTRAL; - publish_vehicle_cmd(&vcmd); -} - 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 f27c112aac..e702a7ad81 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -238,9 +238,9 @@ void RTL::on_activation() } // 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->_gimbal_control.acquireGimbalControlIfNeeded(); + _navigator->_gimbal_control.setNeutral(); + _navigator->_gimbal_control.releaseGimbalControlIfNeeded(); } void RTL::on_active()