diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index d93a7a417698..d3258023ff1b 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1075,8 +1075,18 @@ Mavlink::send_autopilot_capabilities() msg.capabilities |= MAV_PROTOCOL_CAPABILITY_MISSION_FENCE; msg.capabilities |= MAV_PROTOCOL_CAPABILITY_MISSION_RALLY; - if (_param_mnt_mode_in.get() == 4) { - msg.capabilities |= MAV_PROTOCOL_CAPABILITY_COMPONENT_IMPLEMENTS_GIMBAL_MANAGER; + + { + param_t param_handle = param_find_no_notification("MNT_MODE_IN"); + int32_t mnt_mode_in = 0; + + if (mnt_mode_in != PARAM_INVALID) { + param_get(param_handle, &mnt_mode_in); + + if (mnt_mode_in == 4) { + msg.capabilities |= MAV_PROTOCOL_CAPABILITY_COMPONENT_IMPLEMENTS_GIMBAL_MANAGER; + } + } } msg.flight_sw_version = px4_firmware_version(); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 0560f922bee7..d7a7307b3ebf 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -664,8 +664,7 @@ class Mavlink final : public ModuleParams (ParamBool) _param_mav_hb_forw_en, (ParamInt) _param_mav_radio_timeout, (ParamInt) _param_sys_hitl, - (ParamBool) _param_sys_failure_injection_enabled, - (ParamInt) _param_mnt_mode_in + (ParamBool) _param_sys_failure_injection_enabled ) perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": tx run elapsed")}; /**< loop performance counter */