fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-28 18:54:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

mc_pos_control: gradual vel fix for slope

Browse Source
This commit is contained in:
Dennis Mannhart 2017-03-22 11:55:36 +01:00
parent 20a47a1a72
commit be500723cd
1 changed files with 1 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -923,7 +923,7 @@ MulticopterPositionControl::limit_vel_xy_gradually()
* with x= (curr_sp - pos) and y = _vel_sp with min limit of 0.01
*/
math::Vector<3> dist = _curr_pos_sp - _pos;
float slope = get_cruising_speed_xy() / _target_threshold_xy.get();
float slope = (get_cruising_speed_xy() - 0.01f) / _target_threshold_xy.get();
float vel_limit = slope * sqrtf(dist(0) * dist(0) + dist(1) * dist(1)) + 0.01f;
float vel_mag_xy = sqrtf(_vel_sp(0) * _vel_sp(0) + _vel_sp(1) * _vel_sp(1));