fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-01 05:01:35 +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
co-authored by Matthias Grob
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() void Gimbal::acquireGimbalControlIfNeeded()
{ {
if (!_have_gimbal_control) { gimbal_manager_status_s gimbal_manager_status;
_have_gimbal_control = true;
vehicle_command_s vehicle_command{}; if (_gimbal_manager_status_sub.updated()) {
vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE; _gimbal_manager_status_sub.copy(&gimbal_manager_status);
vehicle_command.param1 = _param_mav_sys_id.get();
vehicle_command.param2 = _param_mav_comp_id.get(); if (gimbal_manager_status.primary_control_compid != _param_mav_comp_id.get() ||
vehicle_command.param3 = -1.0f; // Leave unchanged. gimbal_manager_status.primary_control_sysid != _param_mav_sys_id.get()) {
vehicle_command.param4 = -1.0f; // Leave unchanged. _tried_to_have_gimbal_control = true;
vehicle_command.timestamp = hrt_absolute_time(); vehicle_command_s vehicle_command{};
vehicle_command.source_system = _param_mav_sys_id.get(); vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE;
vehicle_command.source_component = _param_mav_comp_id.get(); vehicle_command.param1 = _param_mav_sys_id.get();
vehicle_command.target_system = _param_mav_sys_id.get(); vehicle_command.param2 = _param_mav_comp_id.get();
vehicle_command.target_component = _param_mav_sys_id.get(); vehicle_command.param3 = -1.0f; // Leave unchanged.
vehicle_command.from_external = false; vehicle_command.param4 = -1.0f; // Leave unchanged.
_vehicle_command_pub.publish(vehicle_command); 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() void Gimbal::releaseGimbalControlIfNeeded()
{ {
if (_have_gimbal_control) { if (_tried_to_have_gimbal_control) {
_have_gimbal_control = false; _tried_to_have_gimbal_control = false;
// Restore default flags, setting rate setpoints to NAN lead to unexpected behavior // Restore default flags, setting rate setpoints to NAN lead to unexpected behavior
publishGimbalManagerSetAttitude(FLAGS_ROLL_PITCH_LOCKED, 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/Subscription.hpp>
#include <uORB/topics/gimbal_manager_set_attitude.h> #include <uORB/topics/gimbal_manager_set_attitude.h>
#include <uORB/topics/gimbal_device_attitude_status.h> #include <uORB/topics/gimbal_device_attitude_status.h>
#include <uORB/topics/gimbal_manager_status.h>
#include <uORB/topics/vehicle_command.h> #include <uORB/topics/vehicle_command.h>
#include "Sticks.hpp" #include "Sticks.hpp"
@@ -73,9 +74,9 @@ public:
| gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_PITCH_LOCK; | gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_PITCH_LOCK;
private: 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_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{}; hrt_abstime _telemtry_timestamp{};
uint16_t _telemetry_flags{}; uint16_t _telemetry_flags{};
float _telemetry_yaw{}; float _telemetry_yaw{};