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

FlightTaskOffboard: valid position and velocity is required

Browse Source
This commit is contained in:
Dennis Mannhart
2018-05-24 08:53:18 +02:00
committed by Lorenz Meier
parent bd2de0e585
commit be6f3a5b48
2 changed files with 11 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/FlightTaskOffboard.cpp
+10 -9
View File
@@ -52,21 +52,22 @@ bool FlightTaskOffboard::initializeSubscriptions(SubscriptionArray &subscription
return true;
}
bool FlightTaskOffboard::updateInitialize()
{
bool ret = FlightTask::updateInitialize();
// require valid position / velocity in xy
return ret && PX4_ISFINITE(_position(0))
&& PX4_ISFINITE(_position(1))
&& PX4_ISFINITE(_velocity(0))
&& PX4_ISFINITE(_velocity(1));
}
bool FlightTaskOffboard::activate()
{
bool ret = FlightTask::activate();
_position_setpoint = _position;
_velocity_setpoint *= 0.0f;
_position_lock *= NAN;
// need a valid position and velocity
ret = ret && PX4_ISFINITE(_position(0))
&& PX4_ISFINITE(_position(1))
&& PX4_ISFINITE(_position(2))
&& PX4_ISFINITE(_velocity(0))
&& PX4_ISFINITE(_velocity(1))
&& PX4_ISFINITE(_velocity(2));
return ret;
}
src/lib/FlightTasks/tasks/FlightTaskOffboard.hpp
+1
View File
@@ -50,6 +50,7 @@ public:
bool initializeSubscriptions(SubscriptionArray &subscription_array) override;
bool update() override;
bool activate() override;
bool updateInitialize() override;
protected:
uORB::Subscription<position_setpoint_triplet_s> *_sub_triplet_setpoint{nullptr};