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

mc_pos_control: fix overwrite for state velocity

Browse Source
This commit is contained in:
Dennis Mannhart 2018-06-27 09:50:20 +02:00 committed by Lorenz Meier
parent f2d559a4c2
commit f11a65a74f
1 changed files with 2 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -440,6 +440,8 @@ MulticopterPositionControl::set_vehicle_states(const float &vel_sp_z)
} else if (PX4_ISFINITE(_local_pos.vz)) {
_states.velocity(2) = _local_pos.vz;
if (PX4_ISFINITE(vel_sp_z) && fabsf(vel_sp_z) > FLT_EPSILON && PX4_ISFINITE(_local_pos.z_deriv)) {
// A change in velocity is demanded. Set velocity to the derivative of position
// because it has less bias but blend it in across the landing speed range
@ -447,7 +449,6 @@ MulticopterPositionControl::set_vehicle_states(const float &vel_sp_z)
_states.velocity(2) = _local_pos.z_deriv * weighting + _local_pos.vz * (1.0f - weighting);
}
_states.velocity(2) = _local_pos.vz;
_states.acceleration(2) = _vel_z_deriv.update(-_states.velocity(2));
} else {