fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-01 18:54:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

FlightTaskAuto: Draft impülementation to command gimbal to neutral position when entering crawl speed at landing

Browse Source 
Note: This would douplicate some commands issued by navigator in Land and RTL. We have to check what cases are really not covered.
This commit is contained in:
Matthias Grob 2025-04-25 11:08:34 +02:00
parent f402928888
commit ead4de9874
2 changed files with 51 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
View File

@ -37,6 +37,7 @@
#include "FlightTaskAuto.hpp"
#include <mathlib/mathlib.h>
#include <float.h>
#include <uORB/topics/gimbal_manager_set_attitude.h>
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);
}

4
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp
View File

@ -132,6 +132,7 @@ protected:
uORB::SubscriptionData<home_position_s> _sub_home_position{ORB_ID(home_position)};
uORB::SubscriptionData<vehicle_status_s> _sub_vehicle_status{ORB_ID(vehicle_status)};
uORB::Publication<vehicle_command_s> _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};
};