From 93bab205104c92684e8017caea9fcdb943887e08 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 29 May 2018 14:37:59 +0100 Subject: [PATCH] FlightTaskOrbit: switch to FlightTaskAltitudeSmooth Nice smooth altitude control is already provided by the existing flight task for altitude mode. We inherit from it to resuse the functionality and just all its update first not altering the setpoints for the z-axis it produced. --- src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp | 14 +++++++------- src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp | 4 ++-- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp b/src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp index 774cec7902..87cb3bf2d5 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp @@ -51,7 +51,7 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command) const float &v = command.param4; // commanded velocity if (setRadius(r) && setVelocity(v)) { - return FlightTaskManual::applyCommandParameters(command); + return FlightTaskManualAltitudeSmooth::applyCommandParameters(command); } return false; @@ -90,10 +90,9 @@ bool FlightTaskOrbit::checkAcceleration(float r, float v, float a) bool FlightTaskOrbit::activate() { - bool ret = FlightTaskManual::activate(); + bool ret = FlightTaskManualAltitudeSmooth::activate(); _r = _radius_min; _v = 0.5f; - _z = _position(2); _center = Vector2f(_position.data()); _center(0) -= _r; @@ -110,16 +109,16 @@ bool FlightTaskOrbit::activate() bool FlightTaskOrbit::update() { + // update altitude + FlightTaskManualAltitudeSmooth::update(); + // stick input adjusts parameters within a fixed time frame const float r = _r + _sticks_expo(0) * _deltatime * (_radius_max / 8.f); const float v = _v - _sticks_expo(1) * _deltatime * (_velocity_max / 4.f); - _z += _sticks_expo(2) * _deltatime; setRadius(r); setVelocity(v); - _position_setpoint = Vector3f(NAN, NAN, _z); - // xy velocity to go around in a circle Vector2f center_to_position = Vector2f(_position.data()) - _center; Vector2f velocity_xy = Vector2f(center_to_position(1), -center_to_position(0)); @@ -129,7 +128,8 @@ bool FlightTaskOrbit::update() // xy velocity adjustment to stay on the radius distance velocity_xy += (_r - center_to_position.norm()) * center_to_position.unit_or_zero(); - _velocity_setpoint = Vector3f(velocity_xy(0), velocity_xy(1), 0.f); + _velocity_setpoint(0) = velocity_xy(0); + _velocity_setpoint(1) = velocity_xy(1); // make vehicle front always point towards the center _yaw_setpoint = atan2f(center_to_position(1), center_to_position(0)) + M_PI_F; diff --git a/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp b/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp index 37fb0ca0cc..2ca1fdcb3a 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp @@ -41,9 +41,9 @@ #pragma once -#include "FlightTaskManual.hpp" +#include "FlightTaskManualAltitudeSmooth.hpp" -class FlightTaskOrbit : public FlightTaskManual +class FlightTaskOrbit : public FlightTaskManualAltitudeSmooth { public: FlightTaskOrbit();