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

VTOL mc_pos reset internal state during FW mode

Browse Source
This commit is contained in:
Daniel Agar
2017-02-25 17:57:05 +01:00
committed by Lorenz Meier
parent 1b2973f602
commit fe4ea6dd63
1 changed files with 8 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+8 -8
View File
@@ -735,7 +735,6 @@ MulticopterPositionControl::poll_subscriptions()
_att_sp.yaw_body += delta_euler(2); _att_sp.yaw_body += delta_euler(2);
} }
} }
} }
orb_check(_att_sp_sub, &updated); orb_check(_att_sp_sub, &updated);
@@ -1102,22 +1101,19 @@ MulticopterPositionControl::control_non_manual(float dt)
} }
/* weather-vane mode for vtol: disable yaw control */ /* weather-vane mode for vtol: disable yaw control */
if (_pos_sp_triplet.current.disable_mc_yaw_control == true) { if (_vehicle_status.is_vtol) {
_att_sp.disable_mc_yaw_control = true; _att_sp.disable_mc_yaw_control = _pos_sp_triplet.current.disable_mc_yaw_control;
} else { } else {
/* reset in case of setpoint updates */
_att_sp.disable_mc_yaw_control = false; _att_sp.disable_mc_yaw_control = false;
} }
// guard against any bad velocity values // guard against any bad velocity values
bool velocity_valid = PX4_ISFINITE(_pos_sp_triplet.current.vx) && bool velocity_valid = PX4_ISFINITE(_pos_sp_triplet.current.vx) &&
PX4_ISFINITE(_pos_sp_triplet.current.vy) && PX4_ISFINITE(_pos_sp_triplet.current.vy) &&
_pos_sp_triplet.current.velocity_valid; _pos_sp_triplet.current.velocity_valid;
// do not go slower than the follow target velocity when position tracking is active (set to valid) // do not go slower than the follow target velocity when position tracking is active (set to valid)
if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET && if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET &&
velocity_valid && velocity_valid &&
_pos_sp_triplet.current.position_valid) { _pos_sp_triplet.current.position_valid) {
@@ -2295,10 +2291,14 @@ MulticopterPositionControl::task_main()
_yaw_takeoff = _yaw; _yaw_takeoff = _yaw;
} }
/* reset yaw and altitude setpoint for VTOL which are in fw mode */ /* reset setpoints and integrators VTOL in FW mode */
if (_vehicle_status.is_vtol && !_vehicle_status.is_rotary_wing) { if (_vehicle_status.is_vtol && !_vehicle_status.is_rotary_wing) {
_reset_yaw_sp = true;
_reset_alt_sp = true; _reset_alt_sp = true;
_reset_int_xy = true;
_reset_int_z = true;
_reset_pos_sp = true;
_reset_yaw_sp = true;
_vel_sp_prev = _vel;
} }
//Update previous arming state //Update previous arming state