From d48ba8be728207318675545316dc227e14bbf243 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Sun, 5 Nov 2017 16:35:28 +0100 Subject: [PATCH] FlightTask: move position and stick data subscription into tasks, Orbit introduce variable center position --- src/lib/FlightTasks/FlightTasks.hpp | 12 ---- src/lib/FlightTasks/tasks/FlightTask.hpp | 55 ++++--------------- .../FlightTasks/tasks/FlightTaskManual.hpp | 44 +++++++++++---- src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp | 35 ++++++------ 4 files changed, 62 insertions(+), 84 deletions(-) diff --git a/src/lib/FlightTasks/FlightTasks.hpp b/src/lib/FlightTasks/FlightTasks.hpp index a58e17615c..c99a214397 100644 --- a/src/lib/FlightTasks/FlightTasks.hpp +++ b/src/lib/FlightTasks/FlightTasks.hpp @@ -69,18 +69,6 @@ public: } }; - /** - * Call this function initially to point all tasks to the general input data - */ - void set_general_input_pointers(vehicle_local_position_s *vehicle_local_position, - manual_control_setpoint_s *manual_control_setpoint) - { - for (int i = 0; i < _task_count; i++) { - _tasks[i]->set_vehicle_local_position_pointer(vehicle_local_position); - _tasks[i]->set_manual_control_setpoint_pointer(manual_control_setpoint); - } - }; - /** * Call this function initially to point all tasks to the general output data */ diff --git a/src/lib/FlightTasks/tasks/FlightTask.hpp b/src/lib/FlightTasks/tasks/FlightTask.hpp index 5eca602818..6501fc6dd9 100644 --- a/src/lib/FlightTasks/tasks/FlightTask.hpp +++ b/src/lib/FlightTasks/tasks/FlightTask.hpp @@ -45,11 +45,9 @@ class FlightTask : public control::SuperBlock { public: FlightTask(SuperBlock *parent, const char *name) : - SuperBlock(parent, name) - { - _vehicle_position = nullptr; - _manual_control_setpoint = nullptr; - }; + SuperBlock(parent, name), + _sub_vehicle_local_position(ORB_ID(vehicle_local_position), 0, 0, &getSubscriptions()) + { }; virtual ~FlightTask() {}; /** @@ -80,23 +78,10 @@ public: _deltatime = math::min((int)hrt_elapsed_time(&_last_time_stamp), _timeout) / 1e6f; _last_time_stamp = hrt_absolute_time(); updateSubscriptions(); - _evaluate_sticks(); _evaluate_vehicle_position(); return 0; }; - /** - * Set vehicle local position input data pointer - * @param pointer to vehicle local position - */ - void set_vehicle_local_position_pointer(const vehicle_local_position_s *vehicle_local_position) { _vehicle_position = vehicle_local_position; }; - - /** - * Set manual control setpoint input data pointer if it's needed for the task - * @param pointer to manual control setpoint - */ - void set_manual_control_setpoint_pointer(const manual_control_setpoint_s *manual_control_setpoint) { _manual_control_setpoint = manual_control_setpoint; }; - /** * Set local position setpoint data pointer if it's needed for the task * @param pointer to manual control setpoint @@ -104,12 +89,12 @@ public: void set_vehicle_local_position_setpoint_pointer(vehicle_local_position_setpoint_s *vehicle_position_setpoint) { _vehicle_position_setpoint = vehicle_position_setpoint; }; protected: + static constexpr int _timeout = 500000; /*< maximal time in us before a loop or data times out */ float _time = 0; /*< passed time in seconds since the task was activated */ float _deltatime = 0; /*< passed time in seconds since the task was last updated */ - /* Prepared general inputs for every task */ - matrix::Vector _sticks; + /* Current vehicle position for every task */ matrix::Vector3f _position; /*< current vehicle position */ matrix::Vector3f _velocity; /*< current vehicle velocity */ float _yaw = 0.f; @@ -156,41 +141,23 @@ protected: }; private: - static constexpr int _timeout = 500000; /*< maximal time in us before a loop or data times out */ + uORB::Subscription _sub_vehicle_local_position; hrt_abstime _starting_time_stamp = 0; /*< time stamp when task was activated */ hrt_abstime _last_time_stamp = 0; /*< time stamp when task was last updated */ - /* General input that every task has */ - const vehicle_local_position_s *_vehicle_position; - const manual_control_setpoint_s *_manual_control_setpoint; - /* General output that every task has */ vehicle_local_position_setpoint_s *_vehicle_position_setpoint; void _evaluate_vehicle_position() { - if (_vehicle_position != nullptr && hrt_elapsed_time(&_vehicle_position->timestamp) < _timeout) { - _position = matrix::Vector3f(&_vehicle_position->x); - _velocity = matrix::Vector3f(&_vehicle_position->vx); - _yaw = _vehicle_position->yaw; + if (hrt_elapsed_time(&_sub_vehicle_local_position.get().timestamp) < _timeout) { + _position = matrix::Vector3f(&_sub_vehicle_local_position.get().x); + _velocity = matrix::Vector3f(&_sub_vehicle_local_position.get().vx); + _yaw = _sub_vehicle_local_position.get().yaw; } else { - _velocity = matrix::Vector3f(); /* default velocity is all zero */ + _velocity.zero(); /* default velocity is all zero */ } } - - void _evaluate_sticks() - { - if (_manual_control_setpoint != nullptr && hrt_elapsed_time(&_manual_control_setpoint->timestamp) < _timeout) { - _sticks(0) = _manual_control_setpoint->x; /* NED x, "pitch" [-1,1] */ - _sticks(1) = _manual_control_setpoint->y; /* NED y, "roll" [-1,1] */ - _sticks(2) = (_manual_control_setpoint->z - 0.5f) * 2.f; /* NED z, "thrust" resacaled from [0,1] to [-1,1] */ - _sticks(3) = _manual_control_setpoint->r; /* "yaw" [-1,1] */ - - } else { - _sticks = matrix::Vector(); /* default is all zero */ - } - } - }; diff --git a/src/lib/FlightTasks/tasks/FlightTaskManual.hpp b/src/lib/FlightTasks/tasks/FlightTaskManual.hpp index 78f9806682..59884ea36e 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManual.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManual.hpp @@ -49,6 +49,7 @@ class FlightTaskManual : public FlightTask public: FlightTaskManual(SuperBlock *parent, const char *name) : FlightTask(parent, name), + _sub_manual_control_setpoint(ORB_ID(manual_control_setpoint), 0, 0, &getSubscriptions()), _xy_vel_man_expo(parent, "MPC_XY_MAN_EXPO", false), _z_vel_man_expo(parent, "MPC_Z_MAN_EXPO", false), _hold_dz(parent, "MPC_HOLD_DZ", false), @@ -103,13 +104,11 @@ public: */ virtual int update() { - FlightTask::update(); + int ret = FlightTask::update(); + ret += _evaluate_sticks(); /* prepare stick input */ - matrix::Vector2f stick_xy; /**< horizontal two dimensional stick input within a unit circle */ - stick_xy(0) = math::expo_deadzone(_sticks(0), _xy_vel_man_expo.get(), _hold_dz.get()); - stick_xy(1) = math::expo_deadzone(_sticks(1), _xy_vel_man_expo.get(), _hold_dz.get()); - float stick_z = -math::expo_deadzone(_sticks(2), _z_vel_man_expo.get(), _hold_dz.get()); + matrix::Vector2f stick_xy(_sticks.data()); /**< horizontal two dimensional stick input within a unit circle */ const float stick_xy_norm = stick_xy.norm(); @@ -119,7 +118,7 @@ public: } /* rotate stick input to produce velocity setpoint in NED frame */ - matrix::Vector3f velocity_setpoint(stick_xy(0), stick_xy(1), stick_z); + matrix::Vector3f velocity_setpoint(stick_xy(0), stick_xy(1), _sticks(3)); velocity_setpoint = matrix::Dcmf(matrix::Eulerf(0.0f, 0.0f, get_input_frame_yaw())) * velocity_setpoint; /* scale [0,1] length velocity vector to maximal manual speed (in m/s) */ @@ -129,14 +128,14 @@ public: velocity_setpoint = velocity_setpoint.emult(vel_scale); /* smooth out velocity setpoint by slewrate and return it */ - vel_sp_slewrate(velocity_setpoint, stick_xy, stick_z); + vel_sp_slewrate(velocity_setpoint, stick_xy, _sticks(3)); _set_velocity_setpoint(velocity_setpoint); /* handle position and altitude hold */ const bool stick_xy_zero = stick_xy_norm <= FLT_EPSILON; - const bool stick_z_zero = fabsf(stick_z) <= FLT_EPSILON; + const bool stick_z_zero = fabsf(_sticks(3)) <= FLT_EPSILON; - float velocity_xy_norm = matrix::Vector2f(_velocity._data).norm(); + float velocity_xy_norm = matrix::Vector2f(_velocity.data()).norm(); const bool stopped_xy = (_hold_max_xy.get() < FLT_EPSILON || velocity_xy_norm < _hold_max_xy.get()); const bool stopped_z = (_hold_max_z.get() < FLT_EPSILON || fabsf(_velocity(2)) < _hold_max_z.get()); @@ -157,16 +156,20 @@ public: } _set_position_setpoint(_hold_position); - return 0; + return ret; }; protected: + matrix::Vector _sticks; + float get_input_frame_yaw() { return _yaw; }; private: + uORB::Subscription _sub_manual_control_setpoint; + control::BlockParamFloat _xy_vel_man_expo; /**< ratio of exponential curve for stick input in xy direction pos mode */ control::BlockParamFloat _z_vel_man_expo; /**< ratio of exponential curve for stick input in xy direction pos mode */ control::BlockParamFloat _hold_dz; /**< deadzone around the center for the sticks when flying in position mode */ @@ -178,6 +181,27 @@ private: matrix::Vector3f _hold_position; /**< position at which the vehicle stays while the input is zero velocity */ + int _evaluate_sticks() + { + if (hrt_elapsed_time(&_sub_manual_control_setpoint.get().timestamp) < _timeout) { + /* get data and scale correctly */ + _sticks(0) = _sub_manual_control_setpoint.get().x; /* NED x, "pitch" [-1,1] */ + _sticks(1) = _sub_manual_control_setpoint.get().y; /* NED y, "roll" [-1,1] */ + _sticks(2) = -(_sub_manual_control_setpoint.get().z - 0.5f) * 2.f; /* NED z, "thrust" resacaled from [0,1] to [-1,1] */ + _sticks(3) = _sub_manual_control_setpoint.get().r; /* "yaw" [-1,1] */ + + /* apply expo and deadzone */ + _sticks(0) = math::expo_deadzone(_sticks(0), _xy_vel_man_expo.get(), _hold_dz.get()); + _sticks(1) = math::expo_deadzone(_sticks(1), _xy_vel_man_expo.get(), _hold_dz.get()); + _sticks(2) = math::expo_deadzone(_sticks(2), _z_vel_man_expo.get(), _hold_dz.get()); + + return 0; + + } else { + _sticks.zero(); /* default is all zero */ + return 1; + } + } /* --- Acceleration Smoothing --- */ diff --git a/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp b/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp index 8599135154..0ea7232061 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp @@ -41,13 +41,13 @@ #pragma once -#include "FlightTask.hpp" +#include "FlightTaskManual.hpp" -class FlightTaskOrbit : public FlightTask +class FlightTaskOrbit : public FlightTaskManual { public: FlightTaskOrbit(SuperBlock *parent, const char *name) : - FlightTask(parent, name) + FlightTaskManual(parent, name) {}; virtual ~FlightTaskOrbit() {}; @@ -60,7 +60,9 @@ public: FlightTask::activate(); r = 1.f; v = 0.5f; - altitude = 6.f; + z = _position(2); + _center = matrix::Vector2f(_position.data()); + _center(0) -= r; return 0; }; @@ -80,37 +82,34 @@ public: */ virtual int update() { - FlightTask::update(); + int ret = FlightTaskManual::update(); r += _sticks(0) * _deltatime; r = math::constrain(r, 1.f, 20.f); v -= _sticks(1) * _deltatime; v = math::constrain(v, -7.f, 7.f); - altitude += _sticks(2) * _deltatime; - altitude = math::constrain(altitude, 2.f, 5.f); + z += _sticks(2) * _deltatime; - matrix::Vector2 target_to_position = matrix::Vector2f(_position.data()) - matrix::Vector2f(); - // TODO: add local frame target position here + matrix::Vector2 center_to_position = matrix::Vector2f(_position.data()) - _center; /* xy velocity to go around in a circle */ - matrix::Vector2 velocity_xy = matrix::Vector2f(target_to_position(1), -target_to_position(0)); + matrix::Vector2 velocity_xy = matrix::Vector2f(center_to_position(1), -center_to_position(0)); velocity_xy.normalize(); velocity_xy *= v; /* xy velocity adjustment to stay on the radius distance */ - velocity_xy += (r - target_to_position.norm()) * target_to_position.normalized(); + velocity_xy += (r - center_to_position.norm()) * center_to_position.normalized(); - //printf("%f %f %f\n", (double)altitude, (double)r, (double)v); - - _set_position_setpoint(matrix::Vector3f(NAN, NAN, -altitude)); + _set_position_setpoint(matrix::Vector3f(NAN, NAN, z)); _set_velocity_setpoint(matrix::Vector3f(velocity_xy(0), velocity_xy(1), 0.f)); - return 0; + return ret; }; private: - float r = 1.f; /* radius with which to orbit the target */ - float v = 0.1f; /* linear velocity for orbiting in m/s */ - float altitude = 2.f; /* altitude in meters */ + float r = 0.f; /* radius with which to orbit the target */ + float v = 0.f; /* linear velocity for orbiting in m/s */ + float z = 0.f; /* local z coordinate in meters */ + matrix::Vector2f _center; };