diff --git a/src/lib/parameters/param_translation.cpp b/src/lib/parameters/param_translation.cpp index 3a8d63bbf6..837a2d68ba 100644 --- a/src/lib/parameters/param_translation.cpp +++ b/src/lib/parameters/param_translation.cpp @@ -163,5 +163,19 @@ param_modify_on_import_ret param_modify_on_import(bson_node_t node) } } + // 2025-11-17: translate MNT_RANGE_PITCH to MNT_MAX_PITCH, MNT_MIN_PITCH + { + if (strcmp("MNT_RANGE_PITCH", node->name) == 0) { + if (node->d > DBL_EPSILON) { + float mnt_max_pitch = static_cast(node->d) * 0.5f; + float mnt_min_pitch = static_cast(-node->d) * 0.5f; + param_set(param_find("MNT_MAX_PITCH"), &mnt_max_pitch); + param_set(param_find("MNT_MIN_PITCH"), &mnt_min_pitch); + PX4_INFO("migrating %s -> %s, %s", "MNT_RANGE_PITCH", "MNT_MAX_PITCH", "MNT_MIN_PITCH"); + } + + } + } + return param_modify_on_import_ret::PARAM_NOT_MODIFIED; } diff --git a/src/modules/gimbal/gimbal.cpp b/src/modules/gimbal/gimbal.cpp index 2aced08800..c32e885736 100644 --- a/src/modules/gimbal/gimbal.cpp +++ b/src/modules/gimbal/gimbal.cpp @@ -545,7 +545,8 @@ void update_params(ParameterHandles ¶m_handles, Parameters ¶ms) param_get(param_handles.mnt_man_roll, ¶ms.mnt_man_roll); param_get(param_handles.mnt_man_yaw, ¶ms.mnt_man_yaw); param_get(param_handles.mnt_do_stab, ¶ms.mnt_do_stab); - param_get(param_handles.mnt_range_pitch, ¶ms.mnt_range_pitch); + param_get(param_handles.mnt_max_pitch, ¶ms.mnt_max_pitch); + param_get(param_handles.mnt_min_pitch, ¶ms.mnt_min_pitch); param_get(param_handles.mnt_range_roll, ¶ms.mnt_range_roll); param_get(param_handles.mnt_range_yaw, ¶ms.mnt_range_yaw); param_get(param_handles.mnt_off_pitch, ¶ms.mnt_off_pitch); @@ -570,7 +571,8 @@ bool initialize_params(ParameterHandles ¶m_handles, Parameters ¶ms) param_handles.mnt_man_roll = param_find("MNT_MAN_ROLL"); param_handles.mnt_man_yaw = param_find("MNT_MAN_YAW"); param_handles.mnt_do_stab = param_find("MNT_DO_STAB"); - param_handles.mnt_range_pitch = param_find("MNT_RANGE_PITCH"); + param_handles.mnt_max_pitch = param_find("MNT_MAX_PITCH"); + param_handles.mnt_min_pitch = param_find("MNT_MIN_PITCH"); param_handles.mnt_range_roll = param_find("MNT_RANGE_ROLL"); param_handles.mnt_range_yaw = param_find("MNT_RANGE_YAW"); param_handles.mnt_off_pitch = param_find("MNT_OFF_PITCH"); @@ -592,7 +594,8 @@ bool initialize_params(ParameterHandles ¶m_handles, Parameters ¶ms) param_handles.mnt_man_roll == PARAM_INVALID || param_handles.mnt_man_yaw == PARAM_INVALID || param_handles.mnt_do_stab == PARAM_INVALID || - param_handles.mnt_range_pitch == PARAM_INVALID || + param_handles.mnt_max_pitch == PARAM_INVALID || + param_handles.mnt_min_pitch == PARAM_INVALID || param_handles.mnt_range_roll == PARAM_INVALID || param_handles.mnt_range_yaw == PARAM_INVALID || param_handles.mnt_off_pitch == PARAM_INVALID || diff --git a/src/modules/gimbal/gimbal_params.c b/src/modules/gimbal/gimbal_params.c index a6b1889e2f..3f889c4273 100644 --- a/src/modules/gimbal/gimbal_params.c +++ b/src/modules/gimbal/gimbal_params.c @@ -146,22 +146,28 @@ PARAM_DEFINE_INT32(MNT_MAN_YAW, 0); * @value 0 Disable * @value 1 Stabilize all axis * @value 2 Stabilize yaw for absolute/lock mode. -* @min 0 -* @max 2 +* @value 3 Stabilize pitch for absolute/lock mode. * @group Mount */ PARAM_DEFINE_INT32(MNT_DO_STAB, 0); /** -* Range of pitch channel output in degrees (only in AUX output mode). +* Max angle of pitch channel output in degrees (only in AUX output mode). * -* @min 1.0 -* @max 720.0 * @unit deg * @decimal 1 * @group Mount */ -PARAM_DEFINE_FLOAT(MNT_RANGE_PITCH, 90.0f); +PARAM_DEFINE_FLOAT(MNT_MAX_PITCH, 45.0f); + +/** +* Min angle of pitch channel output in degrees (only in AUX output mode). +* +* @unit deg +* @decimal 1 +* @group Mount +*/ +PARAM_DEFINE_FLOAT(MNT_MIN_PITCH, -45.0f); /** * Range of roll channel output in degrees (only in AUX output mode). diff --git a/src/modules/gimbal/gimbal_params.h b/src/modules/gimbal/gimbal_params.h index 5a265d0237..bfe7c7f557 100644 --- a/src/modules/gimbal/gimbal_params.h +++ b/src/modules/gimbal/gimbal_params.h @@ -64,7 +64,8 @@ struct Parameters { int32_t mnt_man_roll; int32_t mnt_man_yaw; int32_t mnt_do_stab; - float mnt_range_pitch; + float mnt_max_pitch; + float mnt_min_pitch; float mnt_range_roll; float mnt_range_yaw; float mnt_off_pitch; @@ -88,7 +89,8 @@ struct ParameterHandles { param_t mnt_man_roll; param_t mnt_man_yaw; param_t mnt_do_stab; - param_t mnt_range_pitch; + param_t mnt_max_pitch; + param_t mnt_min_pitch; param_t mnt_range_roll; param_t mnt_range_yaw; param_t mnt_off_pitch; diff --git a/src/modules/gimbal/output.cpp b/src/modules/gimbal/output.cpp index ea474bf33e..88c15dc147 100644 --- a/src/modules/gimbal/output.cpp +++ b/src/modules/gimbal/output.cpp @@ -267,8 +267,12 @@ void OutputBase::_calculate_angle_output(const hrt_abstime &t) // constrain angle outputs to [-range/2, range/2] _angle_outputs[0] = math::constrain(_angle_outputs[0], math::radians(-_parameters.mnt_range_roll / 2), math::radians(_parameters.mnt_range_roll / 2)); - _angle_outputs[1] = math::constrain(_angle_outputs[1], math::radians(-_parameters.mnt_range_pitch / 2), - math::radians(_parameters.mnt_range_pitch / 2)); + + // constrain angle outputs to [min, max] to allow for asymmetrical angular ranges + _angle_outputs[1] = math::constrain(_angle_outputs[1], + math::radians(_parameters.mnt_min_pitch), + math::radians(_parameters.mnt_max_pitch)); + // constrain angle outputs to [-range/2, range/2] _angle_outputs[2] = math::constrain(_angle_outputs[2], math::radians(-_parameters.mnt_range_yaw / 2), math::radians(_parameters.mnt_range_yaw / 2)); diff --git a/src/modules/gimbal/output_rc.cpp b/src/modules/gimbal/output_rc.cpp index dba7b02b1a..e89d8f1741 100644 --- a/src/modules/gimbal/output_rc.cpp +++ b/src/modules/gimbal/output_rc.cpp @@ -67,24 +67,62 @@ void OutputRC::update(const ControlData &control_data, bool new_setpoints, uint8 // _angle_outputs are in radians, gimbal_controls are in [-1, 1] gimbal_controls_s gimbal_controls{}; - gimbal_controls.control[gimbal_controls_s::INDEX_ROLL] = constrain( - (_angle_outputs[0] + math::radians(_parameters.mnt_off_roll)) * - (1.0f / (math::radians(_parameters.mnt_range_roll / 2.0f))), - -1.f, 1.f); - gimbal_controls.control[gimbal_controls_s::INDEX_PITCH] = constrain( - (_angle_outputs[1] + math::radians(_parameters.mnt_off_pitch)) * - (1.0f / (math::radians(_parameters.mnt_range_pitch / 2.0f))), - -1.f, 1.f); - gimbal_controls.control[gimbal_controls_s::INDEX_YAW] = constrain( - (_angle_outputs[2] + math::radians(_parameters.mnt_off_yaw)) * - (1.0f / (math::radians(_parameters.mnt_range_yaw / 2.0f))), - -1.f, 1.f); + gimbal_controls.control[gimbal_controls_s::INDEX_ROLL] = anglesMappedToOutput(gimbal_controls_s::INDEX_ROLL); + gimbal_controls.control[gimbal_controls_s::INDEX_PITCH] = anglesMappedToOutput(gimbal_controls_s::INDEX_PITCH); + gimbal_controls.control[gimbal_controls_s::INDEX_YAW] = anglesMappedToOutput(gimbal_controls_s::INDEX_YAW); gimbal_controls.timestamp = hrt_absolute_time(); _gimbal_controls_pub.publish(gimbal_controls); _last_update = now; } +float OutputRC::anglesMappedToOutput(const uint8_t index) +{ + + float value = 0.f; + float min_value = 0.f; + float max_value = 0.f; + float offset = 0.f; + + switch (index) { + case gimbal_controls_s::INDEX_ROLL: { + offset = math::radians(_parameters.mnt_off_roll); + value = _angle_outputs[0]; + max_value = math::radians(_parameters.mnt_range_roll) * 0.5f; + min_value = -math::radians(_parameters.mnt_range_roll) * 0.5f; + break; + } + + case gimbal_controls_s::INDEX_PITCH: { + value = _angle_outputs[1]; + offset = math::radians(_parameters.mnt_off_pitch); + max_value = math::radians(_parameters.mnt_max_pitch); + min_value = math::radians(_parameters.mnt_min_pitch); + break; + } + + case gimbal_controls_s::INDEX_YAW: { + value = _angle_outputs[2]; + offset = math::radians(_parameters.mnt_off_yaw); + max_value = math::radians(_parameters.mnt_range_yaw) * 0.5f; + min_value = -math::radians(_parameters.mnt_range_yaw) * 0.5f; + break; + } + + default: { + PX4_WARN("INDEX does not exist"); + break; + } + } + + if (value + offset >= FLT_EPSILON) { + return math::interpolate(value + offset, 0.f, max_value + offset, 0.f, 1.f); + + } else { + return math::interpolate(value + offset, min_value + offset, 0.f, -1.f, 0.f); + } +} + void OutputRC::print_status() const { PX4_INFO("Output: AUX"); diff --git a/src/modules/gimbal/output_rc.h b/src/modules/gimbal/output_rc.h index de497c194f..616bfe86f6 100644 --- a/src/modules/gimbal/output_rc.h +++ b/src/modules/gimbal/output_rc.h @@ -55,6 +55,7 @@ public: private: void _stream_device_attitude_status(); + float anglesMappedToOutput(const uint8_t index); uORB::Publication _gimbal_controls_pub{ORB_ID(gimbal_controls)}; uORB::Publication _attitude_status_pub{ORB_ID(gimbal_device_attitude_status)};