diff --git a/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.cpp b/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.cpp index 7dd8c84986a23c8b4101fb57a65de9510b59a5e6..812fb1608d6428789394f88b56dd9c1a03a90584 100644 --- a/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.cpp +++ b/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.cpp @@ -55,6 +55,8 @@ FlightTaskOrbit::~FlightTaskOrbit() bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command) { bool ret = true; + // save previous velocity and roatation direction + float v = fabsf(_v); bool clockwise = _v > 0; // commanded radius @@ -66,10 +68,11 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command) // commanded velocity, take sign of radius as rotation direction if (PX4_ISFINITE(command.param2)) { - const float v = command.param2 * (clockwise ? 1.f : -1.f); - ret = ret && setVelocity(v); + v = command.param2; } + ret = ret && setVelocity(v * (clockwise ? 1.f : -1.f)); + // TODO: apply x,y / z independently in geo library // commanded center coordinates // if(PX4_ISFINITE(command.param5) && PX4_ISFINITE(command.param6)) {