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

Fix up formatting in mc pos control

Browse Source
This commit is contained in:
Lorenz Meier
2016-03-27 12:10:57 +02:00
parent f2777cb6ee
commit 737ad045b2
1 changed files with 34 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+34 -35
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 - 2015 PX4 Development Team. All rights reserved.
* Copyright (c) 2013 - 2016 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -1377,53 +1377,52 @@ MulticopterPositionControl::task_main()
// 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 &&
_pos_sp_triplet.current.velocity_valid &&
_pos_sp_triplet.current.position_valid) {
if(_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET &&
_pos_sp_triplet.current.velocity_valid &&
_pos_sp_triplet.current.position_valid) {
math::Vector<3> ft_vel(_pos_sp_triplet.current.vx, _pos_sp_triplet.current.vy, 0);
math::Vector<3> ft_vel(_pos_sp_triplet.current.vx, _pos_sp_triplet.current.vy, 0);
float cos_ratio = (ft_vel*_vel_sp)/(ft_vel.length()*_vel_sp.length());
float cos_ratio = (ft_vel*_vel_sp)/(ft_vel.length()*_vel_sp.length());
// only override velocity set points when uav is traveling in same direction as target and vector component
// is greater than calculated position set point velocity component
// only override velocity set points when uav is traveling in same direction as target and vector component
// is greater than calculated position set point velocity component
if(cos_ratio > 0) {
ft_vel *= (cos_ratio);
// min speed a little faster than target vel
ft_vel += ft_vel.normalized()*1.5f;
} else {
ft_vel.zero();
}
if(cos_ratio > 0) {
ft_vel *= (cos_ratio);
// min speed a little faster than target vel
ft_vel += ft_vel.normalized()*1.5f;
} else {
ft_vel.zero();
}
_vel_sp(0) = fabs(ft_vel(0)) > fabs(_vel_sp(0)) ? ft_vel(0) : _vel_sp(0);
_vel_sp(1) = fabs(ft_vel(1)) > fabs(_vel_sp(1)) ? ft_vel(1) : _vel_sp(1);
_vel_sp(0) = fabs(ft_vel(0)) > fabs(_vel_sp(0)) ? ft_vel(0) : _vel_sp(0);
_vel_sp(1) = fabs(ft_vel(1)) > fabs(_vel_sp(1)) ? ft_vel(1) : _vel_sp(1);
// track target using velocity only
// track target using velocity only
} else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET &&
_pos_sp_triplet.current.velocity_valid) {
} else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET &&
_pos_sp_triplet.current.velocity_valid) {
_vel_sp(0) = _pos_sp_triplet.current.vx;
_vel_sp(1) = _pos_sp_triplet.current.vy;
}
_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);
}
if (_run_alt_control) {
_vel_sp(2) = (_pos_sp(2) - _pos(2)) * _params.pos_p(2);
}
/* make sure velocity setpoint is saturated in xy*/
/* 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 vel_norm_xy = sqrtf(_vel_sp(0) * _vel_sp(0) + _vel_sp(1) * _vel_sp(1));
if (vel_norm_xy > _params.vel_max(0)) {
/* note assumes vel_max(0) == vel_max(1) */
_vel_sp(0) = _vel_sp(0) * _params.vel_max(0) / vel_norm_xy;
_vel_sp(1) = _vel_sp(1) * _params.vel_max(1) / vel_norm_xy;
}
if (vel_norm_xy > _params.vel_max(0)) {
/* note assumes vel_max(0) == vel_max(1) */
_vel_sp(0) = _vel_sp(0) * _params.vel_max(0) / vel_norm_xy;
_vel_sp(1) = _vel_sp(1) * _params.vel_max(1) / vel_norm_xy;
}
/* make sure velocity setpoint is saturated in z*/
float vel_norm_z = sqrtf(_vel_sp(2) * _vel_sp(2));
if (vel_norm_z > _params.vel_max(2)) {