diff --git a/src/lib/FlightTasks/FlightTasks.hpp b/src/lib/FlightTasks/FlightTasks.hpp index 01262a3dfe..11776402d6 100644 --- a/src/lib/FlightTasks/FlightTasks.hpp +++ b/src/lib/FlightTasks/FlightTasks.hpp @@ -68,8 +68,8 @@ public: /** * Call this function initially to point all tasks to the general input data */ - void set_input_pointers(vehicle_local_position_s *vehicle_local_position, - manual_control_setpoint_s *manual_control_setpoint) + 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); @@ -77,6 +77,16 @@ public: } }; + /** + * Call this function initially to point all tasks to the general output data + */ + void set_general_output_pointers(vehicle_local_position_setpoint_s *vehicle_local_position_setpoint) + { + for (int i = 0; i < _task_count; i++) { + _tasks[i]->set_vehicle_local_position_setpoint_pointer(vehicle_local_position_setpoint); + } + }; + /** * Switch to the next task in the available list (for testing) */ @@ -111,17 +121,6 @@ public: */ int get_active_task() const { return _current_task; }; - /** - * Get result of the task execution - * @return pointer to the setpoint for the position controller - */ - const vehicle_local_position_setpoint_s *get_position_setpoint() const - { - //if (is_any_task_active()) { - return _tasks[_current_task]->get_position_setpoint(); - //} - }; - /** * Check if any task is active * @return true if a task is active, flase if not diff --git a/src/lib/FlightTasks/tasks/FlightTask.hpp b/src/lib/FlightTasks/tasks/FlightTask.hpp index b4b57a1afc..81e579ea77 100644 --- a/src/lib/FlightTasks/tasks/FlightTask.hpp +++ b/src/lib/FlightTasks/tasks/FlightTask.hpp @@ -46,15 +46,15 @@ class FlightTask public: FlightTask() { - _vehicle_local_position = NULL; - _manual_control_setpoint = NULL; + _vehicle_position = nullptr; + _manual_control_setpoint = nullptr; _reset_time(); }; virtual ~FlightTask() {}; /** * Call once on the event where you switch to the task - * Note: Set the necessary input pointers first! + * Note: Set the necessary input and output pointers first! * @return 0 on success, >0 on error */ virtual int activate() @@ -85,22 +85,22 @@ public: }; /** - * Set vehicle local position data pointer + * 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_local_position = 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 data pointer if it's needed for the task + * 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; }; /** - * Get the resulting setpoints of the task execution via pointer - * @return pointer to setpoint struct + * Set local position setpoint data pointer if it's needed for the task + * @param pointer to manual control setpoint */ - const vehicle_local_position_setpoint_s *get_position_setpoint() const { return &_vehicle_position_setpoint; }; + void set_vehicle_local_position_setpoint_pointer(vehicle_local_position_setpoint_s *vehicle_position_setpoint) { _vehicle_position_setpoint = vehicle_position_setpoint; }; protected: @@ -113,25 +113,45 @@ protected: matrix::Vector3f _position; matrix::Vector3f _velocity; + /** + * Put the position vector produced by the task into the setpoint message + */ void _set_position_setpoint(const matrix::Vector3f position_setpoint) { - _vehicle_position_setpoint.x = position_setpoint(0); - _vehicle_position_setpoint.y = position_setpoint(1); - _vehicle_position_setpoint.z = position_setpoint(2); + if (_vehicle_position_setpoint != nullptr) { + _vehicle_position_setpoint->x = position_setpoint(0); + _vehicle_position_setpoint->y = position_setpoint(1); + _vehicle_position_setpoint->z = position_setpoint(2); + } }; + /** + * Put the velocity vector produced by the task into the setpoint message + */ void _set_velocity_setpoint(const matrix::Vector3f velocity_setpoint) { - _vehicle_position_setpoint.vx = velocity_setpoint(0); - _vehicle_position_setpoint.vy = velocity_setpoint(1); - _vehicle_position_setpoint.vz = velocity_setpoint(2); + if (_vehicle_position_setpoint != nullptr) { + _vehicle_position_setpoint->vx = velocity_setpoint(0); + _vehicle_position_setpoint->vy = velocity_setpoint(1); + _vehicle_position_setpoint->vz = velocity_setpoint(2); + } }; - void _set_acceleration_setpoint(const matrix::Vector3f acceleration_setpoint) + /** + * Put the acceleration vector produced by the task into the setpoint message + * @return 0 on success, >0 on error + */ + int _set_acceleration_setpoint(const matrix::Vector3f acceleration_setpoint) { - _vehicle_position_setpoint.acc_x = acceleration_setpoint(0); - _vehicle_position_setpoint.acc_y = acceleration_setpoint(1); - _vehicle_position_setpoint.acc_z = acceleration_setpoint(2); + if (_vehicle_position_setpoint != nullptr) { + _vehicle_position_setpoint->acc_x = acceleration_setpoint(0); + _vehicle_position_setpoint->acc_y = acceleration_setpoint(1); + _vehicle_position_setpoint->acc_z = acceleration_setpoint(2); + return 0; + + } else { + return 1; + } }; private: @@ -141,23 +161,23 @@ private: hrt_abstime _last_time_stamp; /*< time stamp when task was last updated */ /* General input that every task has */ - const vehicle_local_position_s *_vehicle_local_position; + 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; + vehicle_local_position_setpoint_s *_vehicle_position_setpoint; void _evaluate_position() { - if (_vehicle_local_position != NULL && hrt_elapsed_time(&_vehicle_local_position->timestamp) < _timeout) { - _position = matrix::Vector3f(&_vehicle_local_position->x); + if (_vehicle_position != nullptr && hrt_elapsed_time(&_vehicle_position->timestamp) < _timeout) { + _position = matrix::Vector3f(&_vehicle_position->x); } } void _evaluate_velocity() { - if (_vehicle_local_position != NULL && hrt_elapsed_time(&_vehicle_local_position->timestamp) < _timeout) { - _velocity = matrix::Vector3f(&_vehicle_local_position->vx); + if (_vehicle_position != nullptr && hrt_elapsed_time(&_vehicle_position->timestamp) < _timeout) { + _velocity = matrix::Vector3f(&_vehicle_position->vx); } else { _velocity = matrix::Vector3f(); /* default is all zero */ @@ -166,7 +186,7 @@ private: void _evaluate_sticks() { - if (_manual_control_setpoint != NULL && hrt_elapsed_time(&_manual_control_setpoint->timestamp) < _timeout) { + 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] */