From 0b6323d5edb27f6d0d6ec174cd99f607f402fc85 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 10 Jun 2015 15:01:12 +0900 Subject: [PATCH] Mount: default MNT_TYPE to servo gimbal if rc outputs defined --- libraries/AP_Mount/AP_Mount.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index e53b57c38c..18e7006850 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -483,6 +483,15 @@ void AP_Mount::init(const AP_SerialManager& serial_manager) return; } + // default mount to servo mount if rc output channels to control roll, tilt or pan have been defined + if (!state[0]._type.load()) { + if (RC_Channel_aux::function_assigned(RC_Channel_aux::Aux_servo_function_t::k_mount_pan) || + RC_Channel_aux::function_assigned(RC_Channel_aux::Aux_servo_function_t::k_mount_tilt) || + RC_Channel_aux::function_assigned(RC_Channel_aux::Aux_servo_function_t::k_mount_roll)) { + state[0]._type.set_and_save(Mount_Type_Servo); + } + } + // primary is reset to the first instantiated mount bool primary_set = false;