diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index f23ec7dc02..6c28e05854 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -37,6 +37,7 @@ #include "FlightTaskAuto.hpp" #include #include +#include using namespace matrix; @@ -235,7 +236,12 @@ void FlightTaskAuto::_prepareLandSetpoints() bool range_dist_available = PX4_ISFINITE(_dist_to_bottom); if (range_dist_available && _dist_to_bottom <= _param_mpc_land_alt3.get()) { + commandGimbalNeutral(); + _was_gimbal_commanded_neutral = true; vertical_speed = _param_mpc_land_crwl.get(); + + } else { + _was_gimbal_commanded_neutral = false; } if (_type_previous != WaypointType::land) { @@ -850,3 +856,44 @@ void FlightTaskAuto::updateParams() // make sure that alt1 is above alt2 _param_mpc_land_alt1.set(math::max(_param_mpc_land_alt1.get(), _param_mpc_land_alt2.get())); } + +void FlightTaskAuto::commandGimbalNeutral() +{ + vehicle_command_s vehicle_command{}; + vehicle_command.source_system = _sub_vehicle_status.get().system_id; + vehicle_command.source_component = _sub_vehicle_status.get().component_id; + vehicle_command.target_system = _sub_vehicle_status.get().system_id; + vehicle_command.target_component = 0; + vehicle_command.confirmation = false; + vehicle_command.from_external = false; + + // Take control + vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE; + vehicle_command.param1 = _sub_vehicle_status.get().system_id; // Take primary control + vehicle_command.param2 = _sub_vehicle_status.get().component_id; + vehicle_command.param3 = -1.f; // Leave secondary control unchanged + vehicle_command.param4 = -1.f; + vehicle_command.timestamp = hrt_absolute_time(); + _vehicle_command_pub.publish(vehicle_command); + + // Command neutral position + vehicle_command = {}; + vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW; + vehicle_command.param1 = NAN; // Don't set any angles + vehicle_command.param2 = NAN; + vehicle_command.param3 = NAN; // Don't set any angular velocities + vehicle_command.param4 = NAN; + vehicle_command.param5 = gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_NEUTRAL; + vehicle_command.timestamp = hrt_absolute_time(); + _vehicle_command_pub.publish(vehicle_command); + + // Release control + vehicle_command = {}; + vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE; + vehicle_command.param1 = -3.f; // Remove primary control if it was taken + vehicle_command.param2 = -3.f; + vehicle_command.param3 = -1.f; // Leave secondary control unchanged + vehicle_command.param4 = -1.f; + vehicle_command.timestamp = hrt_absolute_time(); + _vehicle_command_pub.publish(vehicle_command); +} diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp index c177a74d5f..9346354fdb 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp @@ -132,6 +132,7 @@ protected: uORB::SubscriptionData _sub_home_position{ORB_ID(home_position)}; uORB::SubscriptionData _sub_vehicle_status{ORB_ID(vehicle_status)}; + uORB::Publication _vehicle_command_pub{ORB_ID(vehicle_command)}; State _current_state{State::none}; float _target_acceptance_radius{0.0f}; /**< Acceptances radius of the target */ @@ -207,4 +208,7 @@ private: bool _evaluateGlobalReference(); /**< Check is global reference is available. */ State _getCurrentState(); /**< Computes the current vehicle state based on the vehicle position and navigator triplets. */ void _set_heading_from_mode(); /**< @see MPC_YAW_MODE */ + + void commandGimbalNeutral(); + bool _was_gimbal_commanded_neutral{false}; };