Skip to content

Commit

Permalink
gimbal: correctly set gimbal_device_id
Browse files Browse the repository at this point in the history
When we use a gimbal connected via "RC", so PWM via the Aux channels, we
need to set the gimbal_device_id to 1 as per the protocol.

This was missing for GIMBAL_DEVICE_ATTITUDE_STATUS, so I added that, and
fixed the name of that variable while at it.
  • Loading branch information
julianoes committed Dec 18, 2024
1 parent fd87cd6 commit b6b7e2c
Showing 7 changed files with 19 additions and 7 deletions.
3 changes: 3 additions & 0 deletions msg/GimbalDeviceAttitudeStatus.msg
Original file line number Diff line number Diff line change
@@ -16,5 +16,8 @@ float32 angular_velocity_y
float32 angular_velocity_z

uint32 failure_flags
float32 delta_yaw
float32 delta_yaw_velocity
uint8 gimbal_device_id

bool received_from_mavlink
2 changes: 1 addition & 1 deletion msg/GimbalDeviceInformation.msg
Original file line number Diff line number Diff line change
@@ -33,4 +33,4 @@ float32 pitch_max # [rad]
float32 yaw_min # [rad]
float32 yaw_max # [rad]

uint8 gimbal_device_compid
uint8 gimbal_device_id
8 changes: 4 additions & 4 deletions src/modules/gimbal/output_mavlink.cpp
Original file line number Diff line number Diff line change
@@ -162,7 +162,7 @@ void OutputMavlinkV2::update(const ControlData &control_data, bool new_setpoints
_last_update = now;
}

gimbal_device_id = _gimbal_device_found ? _gimbal_device_compid : 0;
gimbal_device_id = _gimbal_device_found ? _gimbal_device_id : 0;

_publish_gimbal_device_set_attitude();
}
@@ -191,7 +191,7 @@ void OutputMavlinkV2::_check_for_gimbal_device_information()

if (_gimbal_device_information_sub.update(&gimbal_device_information)) {
_gimbal_device_found = true;
_gimbal_device_compid = gimbal_device_information.gimbal_device_compid;
_gimbal_device_id = gimbal_device_information.gimbal_device_id;
}
}

@@ -210,7 +210,7 @@ void OutputMavlinkV2::print_status() const
(double)_angle_velocity[2]);

if (_gimbal_device_found) {
PX4_INFO_RAW(" gimbal device compid found: %d\n", _gimbal_device_compid);
PX4_INFO_RAW(" gimbal device compid found: %d\n", _gimbal_device_id);

} else {
PX4_INFO_RAW(" gimbal device compid not found\n");
@@ -222,7 +222,7 @@ void OutputMavlinkV2::_publish_gimbal_device_set_attitude()
gimbal_device_set_attitude_s set_attitude{};
set_attitude.timestamp = hrt_absolute_time();
set_attitude.target_system = (uint8_t)_parameters.mav_sysid;
set_attitude.target_component = _gimbal_device_compid;
set_attitude.target_component = _gimbal_device_id;

set_attitude.angular_velocity_x = _angle_velocity[0];
set_attitude.angular_velocity_y = _angle_velocity[1];
2 changes: 1 addition & 1 deletion src/modules/gimbal/output_mavlink.h
Original file line number Diff line number Diff line change
@@ -81,7 +81,7 @@ class OutputMavlinkV2 : public OutputBase
uORB::Publication<gimbal_device_set_attitude_s> _gimbal_device_set_attitude_pub{ORB_ID(gimbal_device_set_attitude)};
uORB::Subscription _gimbal_device_information_sub{ORB_ID(gimbal_device_information)};

uint8_t _gimbal_device_compid{0};
uint8_t _gimbal_device_id{0};
hrt_abstime _last_gimbal_device_checked{0};
bool _gimbal_device_found {false};
};
4 changes: 4 additions & 0 deletions src/modules/gimbal/output_rc.cpp
Original file line number Diff line number Diff line change
@@ -115,6 +115,10 @@ void OutputRC::_stream_device_attitude_status()
q.copyTo(attitude_status.q);

attitude_status.failure_flags = 0;

// If the output is RC, then we signal this by referring to compid 1.
attitude_status.gimbal_device_id = 1;

_attitude_status_pub.publish(attitude_status);
}

2 changes: 1 addition & 1 deletion src/modules/mavlink/mavlink_receiver.cpp
Original file line number Diff line number Diff line change
@@ -3063,7 +3063,7 @@ MavlinkReceiver::handle_message_gimbal_device_information(mavlink_message_t *msg
gimbal_information.yaw_max = gimbal_device_info_msg.yaw_max;
gimbal_information.yaw_min = gimbal_device_info_msg.yaw_min;

gimbal_information.gimbal_device_compid = msg->compid;
gimbal_information.gimbal_device_id = msg->compid;

_gimbal_device_information_pub.publish(gimbal_information);
}
5 changes: 5 additions & 0 deletions src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp
Original file line number Diff line number Diff line change
@@ -94,6 +94,11 @@ class MavlinkStreamGimbalDeviceAttitudeStatus : public MavlinkStream

msg.failure_flags = gimbal_device_attitude_status.failure_flags;

msg.delta_yaw = gimbal_device_attitude_status.delta_yaw;
msg.delta_yaw_velocity = gimbal_device_attitude_status.delta_yaw_velocity;

msg.gimbal_device_id = gimbal_device_attitude_status.gimbal_device_id;

mavlink_msg_gimbal_device_attitude_status_send_struct(_mavlink->get_channel(), &msg);

return true;

0 comments on commit b6b7e2c

Please sign in to comment.