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

FlightTaskManualAltitude: initialze yaw setpoint with NAN

Browse Source 
Also update some comments.

In case we activate the task and don't have a locked yaw, we should
initialize the yaw setpoint with NAN to avoid any abrupt changes.
This commit is contained in:
Beat Küng
2018-11-13 09:30:18 +01:00
parent 2b7d3bd088
commit 330941c1fc
2 changed files with 3 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
+2 -4
View File
@@ -51,7 +51,7 @@ bool FlightTaskManualAltitude::updateInitialize()
bool FlightTaskManualAltitude::activate()
{
bool ret = FlightTaskManual::activate();
_yaw_setpoint = _yaw;
_yaw_setpoint = NAN;
_yawspeed_setpoint = 0.0f;
_thrust_setpoint = matrix::Vector3f(0.0f, 0.0f, NAN); // altitude is controlled from position/velocity
_position_setpoint(2) = _position(2);
@@ -82,11 +82,9 @@ bool FlightTaskManualAltitude::activate()
void FlightTaskManualAltitude::_scaleSticks()
{
/* Scale sticks to yaw and thrust using
* linear scale for yaw and piecewise linear map for thrust. */
// Use sticks input with deadzone and exponential curve for vertical velocity and yawspeed
_yawspeed_setpoint = _sticks_expo(3) * math::radians(MPC_MAN_Y_MAX.get());
// scale horizontal velocity with expo curve stick input
const float vel_max_z = (_sticks(2) > 0.0f) ? _constraints.speed_down : _constraints.speed_up;
_velocity_setpoint(2) = vel_max_z * _sticks_expo(2);
}
src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp
+1 -1
View File
@@ -71,7 +71,7 @@ protected:
(ParamFloat<px4::params::MPC_HOLD_MAX_Z>) MPC_HOLD_MAX_Z,
(ParamInt<px4::params::MPC_ALT_MODE>) MPC_ALT_MODE,
(ParamFloat<px4::params::MPC_HOLD_MAX_XY>) MPC_HOLD_MAX_XY,
(ParamFloat<px4::params::MPC_Z_P>) MPC_Z_P,
(ParamFloat<px4::params::MPC_Z_P>) MPC_Z_P, /**< position controller altitude propotional gain */
(ParamFloat<px4::params::MPC_MAN_Y_MAX>) MPC_MAN_Y_MAX, /**< scaling factor from stick to yaw rate */
(ParamFloat<px4::params::MPC_MAN_TILT_MAX>) MPC_MAN_TILT_MAX /**< maximum tilt allowed for manual flight */
)