diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index 5b65256e2d..5aa031072d 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -261,7 +261,7 @@ else mavlink start -x -u $udp_onboard_payload_port_local -r 4000 -f -m onboard -o $udp_onboard_payload_port_remote # Onboard link to gimbal - mavlink start -x -u $udp_onboard_gimbal_port_local -r 400000 -f -m gimbal + mavlink start -x -u $udp_onboard_gimbal_port_local -r 400000 -m gimbal fi # execute autostart post script if any diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 645ba92343..e05f83bfdc 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1129,6 +1129,11 @@ Mavlink::handle_message(const mavlink_message_t *msg) /* forward any messages to other mavlink instances */ Mavlink::forward_message(msg, this); } + + // Special case for gimbals that need to forward GIMBAL_DEVICE_ATTITUDE_STATUS. + else if (msg->msgid == MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS) { + Mavlink::forward_message(msg, this); + } } void