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

Manual Smoth Vel - Override reActivate to not reset Z axis (prevented takeoff)

Browse Source
This commit is contained in:
bresch
2018-10-30 10:04:50 +01:00
committed by Roman Bapst
parent c3a4fff0cd
commit 90cee2d5ea
2 changed files with 32 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
+29 -3
View File
@@ -42,16 +42,42 @@ bool FlightTaskManualPositionSmoothVel::activate()
{
bool ret = FlightTaskManualPosition::activate();
reset(Axes::XYZ);
return ret;
}
void FlightTaskManualPositionSmoothVel::reActivate()
{
reset(Axes::XY);
}
void FlightTaskManualPositionSmoothVel::reset(Axes axes)
{
int count;
switch (axes) {
case Axes::XY:
count = 2;
break;
case Axes::XYZ:
count = 3;
break;
default:
count = 0;
break;
}
// TODO: get current accel
for (int i = 0; i < 3; ++i) {
for (int i = 0; i < count; ++i) {
_smoothing[i].reset(0.f, _velocity(i), _position(i));
}
_position_lock_xy_active = false;
_position_setpoint_xy_locked(0) = NAN;
_position_setpoint_xy_locked(1) = NAN;
return ret;
}
void FlightTaskManualPositionSmoothVel::_updateSetpoints()