fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-21 15:57:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

gimbal: add pitch stabilization

Browse Source
This commit is contained in:
Pernilla
2025-11-11 08:56:02 +01:00
committed by Silvan Fuhrer
parent df42ef84f1
commit aed175451a
3 changed files with 28 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/gimbal/common.h
+7
View File
@@ -86,5 +86,12 @@ struct ControlData {
uint64_t timestamp_last_update{0}; // Timestamp when there was the last setpoint set by the input used for timeout
};
// Must match paraneter MNT_DO_STAB
enum MntDoStabilize {
DISABLED = 0,
ALL_AXES, // Stabilize all axis
YAW_LOCK, // Stabilize yaw for absolute/lock mode.
PITCH_LOCK, // Stabilize pitch for absolute/lock mode.
};
} /* namespace gimbal */
src/modules/gimbal/gimbal.cpp
+18 -6
View File
@@ -254,14 +254,26 @@ static int gimbal_thread_main(int argc, char *argv[])
}
}
if (params.mnt_do_stab == 1) {
thread_data.output_obj->set_stabilize(true, true, true);
switch (params.mnt_do_stab) {
case MntDoStabilize::ALL_AXES: {
thread_data.output_obj->set_stabilize(true, true, true);
break;
}
} else if (params.mnt_do_stab == 2) {
thread_data.output_obj->set_stabilize(false, false, true);
case MntDoStabilize::YAW_LOCK: {
thread_data.output_obj->set_stabilize(false, false, true);
break;
}
} else {
thread_data.output_obj->set_stabilize(false, false, false);
case MntDoStabilize::PITCH_LOCK: {
thread_data.output_obj->set_stabilize(false, true, false);
break;
}
default: {
thread_data.output_obj->set_stabilize(false, false, false);
break;
}
}
if (thread_data.output_obj->check_and_handle_setpoint_timeout(thread_data.control_data, hrt_absolute_time())) {
src/modules/gimbal/input_rc.cpp
+3 -1
View File
@@ -141,7 +141,9 @@ InputRC::UpdateResult InputRC::_read_control_data_from_subscription(ControlData
control_data.type_data.angle.frames[0] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame;
control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame;
control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
control_data.type_data.angle.frames[2] = (_parameters.mnt_do_stab == MntDoStabilize::ALL_AXES
|| _parameters.mnt_do_stab == MntDoStabilize::YAW_LOCK) ?
ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame : ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
control_data.type_data.angle.angular_velocity[0] = NAN;
control_data.type_data.angle.angular_velocity[1] = NAN;