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

FlightTaskAuto: override default constraints

Browse Source
This commit is contained in:
Dennis Mannhart
2018-04-26 11:24:20 +02:00
committed by Lorenz Meier
parent 962c2eff61
commit 4d6539b076
2 changed files with 12 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/FlightTaskAuto.cpp
+10 -8
View File
@@ -59,9 +59,12 @@ bool FlightTaskAuto::initializeSubscriptions(SubscriptionArray &subscription_arr
bool FlightTaskAuto::activate()
{
bool ret = FlightTask::activate();
_prev_prev_wp = _prev_wp = _target = _next_wp = _position;
_yaw_wp = _yaw;
return FlightTask::activate();
_setDefaultConstraints();
return ret;
}
bool FlightTaskAuto::updateInitialize()
@@ -98,9 +101,9 @@ bool FlightTaskAuto::_evaluateTriplets()
// always update cruise speed since that can change without waypoint changes
_mc_cruise_speed = _sub_triplet_setpoint->get().current.cruising_speed;
if (!PX4_ISFINITE(_mc_cruise_speed) || (_mc_cruise_speed < 0.0f) || (_mc_cruise_speed > _limits.speed_NE_max)) {
if (!PX4_ISFINITE(_mc_cruise_speed) || (_mc_cruise_speed < 0.0f) || (_mc_cruise_speed > _constraints.speed_xy)) {
// use default limit
_mc_cruise_speed = _limits.speed_NE_max;
_mc_cruise_speed = _constraints.speed_xy;
}
// get target waypoint.
@@ -186,13 +189,12 @@ void FlightTaskAuto::_evaluateVehicleGlobalPosition()
}
}
void FlightTaskAuto::_updateSetpointLimits()
void FlightTaskAuto::_setDefaultConstraints()
{
FlightTask::_updateSetpointLimits();
FlightTask::_setDefaultConstraints();
// only adjust limits if the new limit is lower
if (_limits.speed_NE_max >= MPC_XY_CRUISE.get()) {
_limits.speed_NE_max = MPC_XY_CRUISE.get();
if (_constraints.speed_xy >= MPC_XY_CRUISE.get()) {
_constraints.speed_xy = MPC_XY_CRUISE.get();
}
}