fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-23 13:27:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

FlightTaskManualAccelerationSlow: Acquire gimbal control until expected IDs are reported (#25565)

Browse Source 
* Acquire control until expected id s are reported

* require an updated message and allow release in case early exit of FlightTask

* FlightTask Gimbal: acquiring logic simplification suggestion

* formatting

---------

Co-authored-by: Matthias Grob <maetugr@gmail.com>
This commit is contained in:
Pernilla
2025-09-18 11:09:09 +02:00
committed by GitHub
parent e8ccb23dc8
commit 2cb97e79b9
2 changed files with 26 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/flight_mode_manager/tasks/Utility/Gimbal.cpp
+23 -17
View File
@@ -63,29 +63,35 @@ bool Gimbal::checkForTelemetry(const hrt_abstime now)
void Gimbal::acquireGimbalControlIfNeeded()
{
if (!_have_gimbal_control) {
_have_gimbal_control = true;
gimbal_manager_status_s gimbal_manager_status;
vehicle_command_s vehicle_command{};
vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE;
vehicle_command.param1 = _param_mav_sys_id.get();
vehicle_command.param2 = _param_mav_comp_id.get();
vehicle_command.param3 = -1.0f; // Leave unchanged.
vehicle_command.param4 = -1.0f; // Leave unchanged.
vehicle_command.timestamp = hrt_absolute_time();
vehicle_command.source_system = _param_mav_sys_id.get();
vehicle_command.source_component = _param_mav_comp_id.get();
vehicle_command.target_system = _param_mav_sys_id.get();
vehicle_command.target_component = _param_mav_sys_id.get();
vehicle_command.from_external = false;
_vehicle_command_pub.publish(vehicle_command);
if (_gimbal_manager_status_sub.updated()) {
_gimbal_manager_status_sub.copy(&gimbal_manager_status);
if (gimbal_manager_status.primary_control_compid != _param_mav_comp_id.get() ||
gimbal_manager_status.primary_control_sysid != _param_mav_sys_id.get()) {
_tried_to_have_gimbal_control = true;
vehicle_command_s vehicle_command{};
vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE;
vehicle_command.param1 = _param_mav_sys_id.get();
vehicle_command.param2 = _param_mav_comp_id.get();
vehicle_command.param3 = -1.0f; // Leave unchanged.
vehicle_command.param4 = -1.0f; // Leave unchanged.
vehicle_command.timestamp = hrt_absolute_time();
vehicle_command.source_system = _param_mav_sys_id.get();
vehicle_command.source_component = _param_mav_comp_id.get();
vehicle_command.target_system = _param_mav_sys_id.get();
vehicle_command.target_component = _param_mav_sys_id.get();
vehicle_command.from_external = false;
_vehicle_command_pub.publish(vehicle_command);
}
}
}
void Gimbal::releaseGimbalControlIfNeeded()
{
if (_have_gimbal_control) {
_have_gimbal_control = false;
if (_tried_to_have_gimbal_control) {
_tried_to_have_gimbal_control = false;
// Restore default flags, setting rate setpoints to NAN lead to unexpected behavior
publishGimbalManagerSetAttitude(FLAGS_ROLL_PITCH_LOCKED,
src/modules/flight_mode_manager/tasks/Utility/Gimbal.hpp
+3 -2
View File
@@ -45,6 +45,7 @@
#include <uORB/Subscription.hpp>
#include <uORB/topics/gimbal_manager_set_attitude.h>
#include <uORB/topics/gimbal_device_attitude_status.h>
#include <uORB/topics/gimbal_manager_status.h>
#include <uORB/topics/vehicle_command.h>
#include "Sticks.hpp"
@@ -73,9 +74,9 @@ public:
| gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_PITCH_LOCK;
private:
bool _have_gimbal_control{false};
bool _tried_to_have_gimbal_control{false};
uORB::Subscription _gimbal_device_attitude_status_sub{ORB_ID(gimbal_device_attitude_status)};
uORB::Subscription _gimbal_manager_status_sub{ORB_ID(gimbal_manager_status)};
hrt_abstime _telemtry_timestamp{};
uint16_t _telemetry_flags{};
float _telemetry_yaw{};