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

mc_pos_control: enable yawspeed execution for tasks

Browse Source
This commit is contained in:
Matthias Grob
2017-12-16 07:59:52 +01:00
committed by Beat Küng
parent a896332746
commit 975ac11635
2 changed files with 8 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/FlightTaskManual.cpp
+6 -2
View File
@@ -55,7 +55,8 @@ FlightTaskManual::FlightTaskManual(control::SuperBlock *parent, const char *name
_z_vel_max_up(parent, "MPC_Z_VEL_MAX_UP", false),
_z_vel_max_down(parent, "MPC_Z_VEL_MAX_DN", false),
_hold_max_xy(parent, "MPC_HOLD_MAX_XY", false),
_hold_max_z(parent, "MPC_HOLD_MAX_Z", false)
_hold_max_z(parent, "MPC_HOLD_MAX_Z", false),
_man_yaw_max(parent, "MPC_MAN_Y_MAX", false)
{ }
bool FlightTaskManual::initializeSubscriptions(SubscriptionArray &subscription_array)
@@ -146,7 +147,10 @@ bool FlightTaskManual::update()
void FlightTaskManual::_updateYaw()
{
_hold_yaw += _sticks(3) * _deltatime;
const float yaw_speed = _sticks(3) * math::radians(_man_yaw_max.get());
_setYawspeedSetpoint(yaw_speed);
_hold_yaw += yaw_speed * _deltatime;
_setYawSetpoint(_hold_yaw);
}
src/lib/FlightTasks/tasks/FlightTaskManual.hpp
+2 -1
View File
@@ -81,9 +81,10 @@ private:
control::BlockParamFloat _z_vel_max_down; /**< maximal vertical velocity when flying downwards with the stick */
control::BlockParamFloat _hold_max_xy; /**< velocity threshold to switch into horizontal position hold */
control::BlockParamFloat _hold_max_z; /**< velocity threshold to switch into vertical position hold */
control::BlockParamFloat _man_yaw_max; /**< maximal rotation speed around yaw axis with full stick input */
matrix::Vector3f _hold_position; /**< position at which the vehicle stays while the input is zero velocity */
float _hold_yaw; /**< absolute yaw which gets updated by the yawspeed input */
float _hold_yaw = 0.f; /**< absolute yaw which gets updated by the yawspeed input */
void _updateYaw();