UAV-develop/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-07-24 15:27:41 +08:00
Code Issues Packages Projects Releases Wiki Activity

fix(simulation): fix Gazebo gimbal ROI yaw frame and tracking

Browse Source 
Gazebo gimbal joints consume vehicle-relative angles, but the gz_bridge
passed Earth-frame ROI attitude setpoints straight through, applying the
vehicle heading twice. Respect the yaw-frame flags and rotate Earth-frame
setpoints into the vehicle frame with the inverse vehicle attitude before
commanding the joints.

The MAVLink v2 gimbal output also only recomputed the geographic ROI
bearing when a new ROI command arrived, so the setpoint froze while the
vehicle translated. Refresh position-based setpoints every update cycle,
matching the v1 output.

Signed-off-by: jbotwina <jimbotwina16@gmail.com>
This commit is contained in:
jbotwina
2026-07-20 16:32:23 -04:00
committed by Julian Oes
parent a617a1433c
commit c0112e70eb
3 changed files with 21 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
src/modules/gimbal/output_mavlink.cpp
View File

@@ -160,11 +160,11 @@ void OutputMavlinkV2::update(const ControlData &control_data, bool new_setpoints
if (new_setpoints) {
//got new command
_set_angle_setpoints(control_data);
_handle_position_update(control_data);
_last_update = now;
}
_handle_position_update(control_data);
gimbal_device_id = _gimbal_device_found ? _gimbal_device_id : 0;
_publish_gimbal_device_set_attitude();

18
src/modules/simulation/gz_bridge/GZGimbal.cpp
View File

@@ -145,7 +145,23 @@ bool GZGimbal::pollSetpoint()
gimbal_device_set_attitude_s msg;
if (_gimbal_device_set_attitude_sub.copy(&msg)) {
const matrix::Eulerf gimbal_att_stp(matrix::Quatf(msg.q));
matrix::Quatf attitude_setpoint(msg.q);
const bool yaw_in_vehicle_frame = msg.flags
& gimbal_device_set_attitude_s::GIMBAL_DEVICE_FLAGS_YAW_IN_VEHICLE_FRAME;
const bool yaw_in_earth_frame = msg.flags
& gimbal_device_set_attitude_s::GIMBAL_DEVICE_FLAGS_YAW_IN_EARTH_FRAME;
const bool legacy_yaw_lock = !yaw_in_vehicle_frame && !yaw_in_earth_frame
&& (msg.flags & gimbal_device_set_attitude_s::GIMBAL_DEVICE_FLAGS_YAW_LOCK);
if ((yaw_in_earth_frame && !yaw_in_vehicle_frame) || legacy_yaw_lock) {
vehicle_attitude_s vehicle_attitude{};
if (_vehicle_attitude_sub.copy(&vehicle_attitude)) {
attitude_setpoint = matrix::Quatf(vehicle_attitude.q).inversed() * attitude_setpoint;
}
}
const matrix::Eulerf gimbal_att_stp(attitude_setpoint);
_roll_stp = gimbal_att_stp.phi();
_pitch_stp = gimbal_att_stp.theta();
_yaw_stp = gimbal_att_stp.psi();

2
src/modules/simulation/gz_bridge/GZGimbal.hpp
View File

@@ -10,6 +10,7 @@
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/gimbal_controls.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
@@ -41,6 +42,7 @@ private:
uORB::Subscription _gimbal_device_set_attitude_sub{ORB_ID(gimbal_device_set_attitude)};
uORB::Subscription _gimbal_controls_sub{ORB_ID(gimbal_controls)};
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::SubscriptionCallbackWorkItem _vehicle_command_sub{this, ORB_ID(vehicle_command)};
uORB::Publication<gimbal_device_attitude_status_s> _gimbal_device_attitude_status_pub{ORB_ID(gimbal_device_attitude_status)};