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

FlightTask StraightLine: set origin and target at the same time

Browse Source 
This avoids issues with reseting/calculating the max vel/acc
This commit is contained in:
ChristophTobler 2018-07-23 16:35:54 +02:00 committed by Lorenz Meier
parent 9e8c3ff0dc
commit ccd8a47015
2 changed files with 5 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -176,23 +176,18 @@ void StraightLine::setAllDefaults()
_desired_deceleration = DECELERATION_MAX;
}
void StraightLine::setTarget(const matrix::Vector3f &target)
void StraightLine::setLineFromTo(const matrix::Vector3f &origin, const matrix::Vector3f &target)
{
if (PX4_ISFINITE(target(0)) && PX4_ISFINITE(target(1)) && PX4_ISFINITE(target(2))) {
if (PX4_ISFINITE(target(0)) && PX4_ISFINITE(target(1)) && PX4_ISFINITE(target(2)) &&
PX4_ISFINITE(origin(0)) && PX4_ISFINITE(origin(1)) && PX4_ISFINITE(origin(2))) {
_target = target;
_origin = origin;
// set all parameters to their default value (depends on the direction)
setAllDefaults();
}
}
void StraightLine::setOrigin(const matrix::Vector3f &origin)
{
if (PX4_ISFINITE(origin(0)) && PX4_ISFINITE(origin(1)) && PX4_ISFINITE(origin(2))) {
_origin = origin;
}
}
void StraightLine::setSpeed(const float &speed)
{
float vel_max = getMaxVel();

3
src/lib/FlightTasks/tasks/Utility/StraightLine.hpp
View File

@ -51,8 +51,7 @@ public:
~StraightLine() = default;
// setter functions
void setTarget(const matrix::Vector3f &target);
void setOrigin(const matrix::Vector3f &origin);
void setLineFromTo(const matrix::Vector3f &origin, const matrix::Vector3f &target);
void setSpeed(const float &speed);
void setSpeedAtTarget(const float &speed_at_target);
void setAcceleration(const float &acc);