diff --git a/src/drivers/gimbal/gimbal.cpp b/src/drivers/gimbal/gimbal.cpp index 96eaa1becf..bb76d1229b 100644 --- a/src/drivers/gimbal/gimbal.cpp +++ b/src/drivers/gimbal/gimbal.cpp @@ -148,10 +148,12 @@ private: struct { int aux_mnt_chn; + int use_mnt; } _parameters; struct { param_t aux_mnt_chn; + param_t use_mnt; } _params_handles; /** @@ -214,6 +216,7 @@ Gimbal::Gimbal() : _debug_enabled = false; _params_handles.aux_mnt_chn = param_find("GMB_AUX_MNT_CHN"); + _params_handles.use_mnt = param_find("GMB_USE_MNT"); update_params(); // work_cancel in the dtor will explode if we don't do this... @@ -299,6 +302,7 @@ void Gimbal::update_params() { param_get(_params_handles.aux_mnt_chn, &_parameters.aux_mnt_chn); + param_get(_params_handles.use_mnt, &_parameters.use_mnt); } void @@ -488,23 +492,26 @@ Gimbal::cycle() } - switch (_mount_mode) { - case vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT: - out_mount_mode = -1.0f; - roll = 0.0f; - pitch = 0.0f; - yaw = 0.0f; - break; + /* consider mount mode if parameter is set */ + if (_parameters.use_mnt > 0) { + switch (_mount_mode) { + case vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT: + out_mount_mode = -1.0f; + roll = 0.0f; + pitch = 0.0f; + yaw = 0.0f; + break; - case vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL: - case vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING: - case vehicle_command_s::VEHICLE_MOUNT_MODE_RC_TARGETING: - case vehicle_command_s::VEHICLE_MOUNT_MODE_GPS_POINT: - out_mount_mode = 1.0f; - break; + case vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL: + case vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING: + case vehicle_command_s::VEHICLE_MOUNT_MODE_RC_TARGETING: + case vehicle_command_s::VEHICLE_MOUNT_MODE_GPS_POINT: + out_mount_mode = 1.0f; + break; - default: - out_mount_mode = -1.0f; + default: + out_mount_mode = -1.0f; + } } if (updated) { diff --git a/src/drivers/gimbal/gimbal_params.c b/src/drivers/gimbal/gimbal_params.c index 6fcd31cb82..955bfa8b23 100644 --- a/src/drivers/gimbal/gimbal_params.c +++ b/src/drivers/gimbal/gimbal_params.c @@ -43,14 +43,25 @@ #include +/** + * Consider mount operation mode. + * + * If set to 1, mount mode will be enforced. + * + * @min 0 + * @max 1 + * @group Gimbal + */ +PARAM_DEFINE_INT32(GMB_USE_MNT, 0); /** - * Auxilary switch to use for mount control. + * Auxilary switch to set mount operation mode. * - * Set to 0 to disable manual mount control. + * Set to 0 to disable manual mode control. * - * Mount control off means the gimbal is put into safe/locked position. - * Mount control on means the gimbal can move freely, and landing gear + * If set to an auxilary switch: + * Switch off means the gimbal is put into safe/locked position. + * Switch on means the gimbal can move freely, and landing gear * will be retracted if applicable. * * @min 0