fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 08:38:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

gimbal: account for non zero symmetrical angular ranges

Browse Source
This commit is contained in:
Pernilla
2026-01-16 11:33:45 +01:00
committed by Silvan Fuhrer
parent aed175451a
commit 0fa5a83409
7 changed files with 93 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/parameters/param_translation.cpp
+14
View File
@@ -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<float>(node->d) * 0.5f;
float mnt_min_pitch = static_cast<float>(-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;
}
src/modules/gimbal/gimbal.cpp
+6 -3
View File
@@ -545,7 +545,8 @@ void update_params(ParameterHandles &param_handles, Parameters &params)
param_get(param_handles.mnt_man_roll, &params.mnt_man_roll);
param_get(param_handles.mnt_man_yaw, &params.mnt_man_yaw);
param_get(param_handles.mnt_do_stab, &params.mnt_do_stab);
param_get(param_handles.mnt_range_pitch, &params.mnt_range_pitch);
param_get(param_handles.mnt_max_pitch, &params.mnt_max_pitch);
param_get(param_handles.mnt_min_pitch, &params.mnt_min_pitch);
param_get(param_handles.mnt_range_roll, &params.mnt_range_roll);
param_get(param_handles.mnt_range_yaw, &params.mnt_range_yaw);
param_get(param_handles.mnt_off_pitch, &params.mnt_off_pitch);
@@ -570,7 +571,8 @@ bool initialize_params(ParameterHandles &param_handles, Parameters &params)
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 &param_handles, Parameters &params)
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 ||
src/modules/gimbal/gimbal_params.c
+12 -6
View File
@@ -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).
src/modules/gimbal/gimbal_params.h
+4 -2
View File
@@ -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;
src/modules/gimbal/output.cpp
+6 -2
View File
@@ -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));
src/modules/gimbal/output_rc.cpp
+50 -12
View File
@@ -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");
src/modules/gimbal/output_rc.h
+1
View File
@@ -55,6 +55,7 @@ public:
private:
void _stream_device_attitude_status();
float anglesMappedToOutput(const uint8_t index);
uORB::Publication <gimbal_controls_s> _gimbal_controls_pub{ORB_ID(gimbal_controls)};
uORB::Publication <gimbal_device_attitude_status_s> _attitude_status_pub{ORB_ID(gimbal_device_attitude_status)};