yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

FlightTask StraightLine: check if target and origin are the same

Browse Source
This commit is contained in:
ChristophTobler
2018-07-23 16:34:54 +02:00
committed by Lorenz Meier
parent 8998714130
commit 9e8c3ff0dc
1 changed files with 10 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
src/lib/FlightTasks/tasks/Utility/StraightLine.cpp
View File

@@ -100,6 +100,11 @@ void StraightLine::generateSetpoints(matrix::Vector3f &position_setpoint, matrix
float StraightLine::getMaxAcc()
{
// check if origin and target are different points
if ((_target - _origin).length() < FLT_EPSILON) {
return MPC_ACC_HOR_MAX.get();
}
// unit vector in the direction of the straight line
Vector3f u_orig_to_target = (_target - _origin).unit_or_zero();
@@ -130,6 +135,11 @@ float StraightLine::getMaxAcc()
float StraightLine::getMaxVel()
{
// check if origin and target are different points
if ((_target - _origin).length() < FLT_EPSILON) {
return MPC_XY_VEL_MAX.get();
}
// unit vector in the direction of the straight line
Vector3f u_orig_to_target = (_target - _origin).unit_or_zero();