diff --git a/src/lib/FlightTasks/FlightTasks.cpp b/src/lib/FlightTasks/FlightTasks.cpp index 3fd9f61023..75f42c14a6 100644 --- a/src/lib/FlightTasks/FlightTasks.cpp +++ b/src/lib/FlightTasks/FlightTasks.cpp @@ -27,13 +27,19 @@ bool FlightTasks::_updateCommand() } /* evaluate command */ - //printf("YAY %d\n", int(command.param1)); - uint8_t switch_result = switchTask(int(command.param1)); uint8_t cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED; + /* switch succeeded */ if (!switch_result) { cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED; + + /* if the correct task is running apply parameters to it and see if it rejects */ + if (isAnyTaskActive()) { + if (!_current_task->applyCommandParameters(command)) { + cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED; + } + } } /* send back acknowledgment */ diff --git a/src/lib/FlightTasks/FlightTasks.hpp b/src/lib/FlightTasks/FlightTasks.hpp index f856975155..a8e5d33dd3 100644 --- a/src/lib/FlightTasks/FlightTasks.hpp +++ b/src/lib/FlightTasks/FlightTasks.hpp @@ -109,7 +109,7 @@ public: { /* switch to the running task, nothing to do */ if (task_number == _current_task_index) { - return true; + return 0; } /* disable the old task if there is any */ @@ -128,6 +128,10 @@ public: _current_task = new (&_task_union.orbit) FlightTaskOrbit(this, "ORB"); break; + case -1: + /* disable tasks is a success */ + return 0; + default: /* invalid task */ return -1; diff --git a/src/lib/FlightTasks/tasks/FlightTask.hpp b/src/lib/FlightTasks/tasks/FlightTask.hpp index 39f8063d1f..0d7803161d 100644 --- a/src/lib/FlightTasks/tasks/FlightTask.hpp +++ b/src/lib/FlightTasks/tasks/FlightTask.hpp @@ -46,6 +46,7 @@ #include #include #include +#include #include "../SubscriptionArray.hpp" @@ -71,6 +72,12 @@ public: */ virtual bool activate(); + /** + * To be called to adopt parameters from an arrived vehicle command + * @return true if accepted, false if declined + */ + virtual bool applyCommandParameters(vehicle_command_s command) = 0; + /** * Call before activate() or update() * to initialize time and input data diff --git a/src/lib/FlightTasks/tasks/FlightTaskManual.hpp b/src/lib/FlightTasks/tasks/FlightTaskManual.hpp index f06f5bc431..733a256fca 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManual.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManual.hpp @@ -56,6 +56,8 @@ public: bool initializeSubscriptions(SubscriptionArray &subscription_array) override; + bool applyCommandParameters(vehicle_command_s command) override { return false; }; + bool updateInitialize() override; bool update() override; diff --git a/src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp b/src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp index b2bfcee1c2..3869863218 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp @@ -51,6 +51,21 @@ FlightTaskOrbit::FlightTaskOrbit(control::SuperBlock *parent, const char *name) _sticks_data_required = false; } +bool FlightTaskOrbit::applyCommandParameters(vehicle_command_s command) +{ + float &r = command.param3; + float &v = command.param4; + + if (math::isInRange(r, 5.f, 50.f) && + fabs(v) < 10.f) { + _r = r; + _v = v; + return true; + } + + return false; +} + bool FlightTaskOrbit::activate() { bool ret = FlightTaskManual::activate(); diff --git a/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp b/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp index f45536632c..142987a2d6 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp @@ -50,6 +50,8 @@ public: virtual ~FlightTaskOrbit() = default; + bool applyCommandParameters(vehicle_command_s command) override; + bool activate() override; bool update() override; diff --git a/src/lib/mathlib/math/Limits.hpp b/src/lib/mathlib/math/Limits.hpp index 5777ed89ac..cf3e00f3f0 100644 --- a/src/lib/mathlib/math/Limits.hpp +++ b/src/lib/mathlib/math/Limits.hpp @@ -72,6 +72,12 @@ constexpr const _Tp &constrain(const _Tp &val, const _Tp &min_val, const _Tp &ma return (val < min_val) ? min_val : ((val > max_val) ? max_val : val); } +template +inline constexpr bool isInRange(const _Tp &val, const _Tp &min_val, const _Tp &max_val) +{ + return (min_val <= val) && (val <= max_val); +} + template constexpr T radians(const T degrees) {