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

code clean up complete

Browse Source
This commit is contained in:
Jimmy Johnson
2016-03-27 12:10:57 +02:00
committed by Lorenz Meier
parent 26cb645ee0
commit 8829db8844
7 changed files with 215 additions and 140 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+10 -7
View File
@@ -1369,20 +1369,23 @@ MulticopterPositionControl::task_main()
} else {
/* run position & altitude controllers, if enabled (otherwise use already computed velocity setpoints) */
if (_run_pos_control) {
_vel_sp(0) = (_pos_sp(0) - _pos(0)) * _params.pos_p(0) + _pos_sp_triplet.current.vx;
_vel_sp(1) = (_pos_sp(1) - _pos(1)) * _params.pos_p(1) + _pos_sp_triplet.current.vy;
_vel_sp(0) = (_pos_sp(0) - _pos(0)) * _params.pos_p(0);
_vel_sp(1) = (_pos_sp(1) - _pos(1)) * _params.pos_p(1);
}
// if there is a velocity sp available, use it
if(_pos_sp_triplet.current.velocity_valid) {
_vel_sp(0) += _pos_sp_triplet.current.vx;
_vel_sp(1) += _pos_sp_triplet.current.vy;
}
if (_run_alt_control) {
_vel_sp(2) = (_pos_sp(2) - _pos(2)) * _params.pos_p(2);
}
/* make sure velocity setpoint is saturated in xy*/
float vel_norm_xy = sqrtf(_vel_sp(0) * _vel_sp(0) +
_vel_sp(1) * _vel_sp(1));
// float sp_vel = sqrtf(_pos_sp_triplet.current.vx*_pos_sp_triplet.current.vx +
// _pos_sp_triplet.current.vy*_pos_sp_triplet.current.vy);
float vel_norm_xy = sqrtf(_vel_sp(0) * _vel_sp(0) + _vel_sp(1) * _vel_sp(1));
if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET) {
_vel_sp(0) = _pos_sp_triplet.current.vx;