fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-28 12:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

FlightTasks: switched output setpoint to reference getter, FlightTask holds it's data

Browse Source
This commit is contained in:
Matthias Grob
2018-04-05 07:30:12 +02:00
committed by Beat Küng
parent d48ba8be72
commit e49f80eaa8
2 changed files with 32 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/FlightTasks.hpp
+10 -4
View File
@@ -69,14 +69,20 @@ public:
} }
}; };
/**
* Get the output data from the current task
*/
const vehicle_local_position_setpoint_s &get_position_setpoint()
{
return _tasks[_current_task]->get_position_setpoint();
};
/** /**
* Call this function initially to point all tasks to the general output data * 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) inline const vehicle_local_position_setpoint_s &operator()()
{ {
for (int i = 0; i < _task_count; i++) { return get_position_setpoint();
_tasks[i]->set_vehicle_local_position_setpoint_pointer(vehicle_local_position_setpoint);
}
}; };
/** /**
src/lib/FlightTasks/tasks/FlightTask.hpp
+22 -26
View File
@@ -83,10 +83,12 @@ public:
}; };
/** /**
* Set local position setpoint data pointer if it's needed for the task * Get the output data
* @param pointer to manual control setpoint
*/ */
void set_vehicle_local_position_setpoint_pointer(vehicle_local_position_setpoint_s *vehicle_position_setpoint) { _vehicle_position_setpoint = vehicle_position_setpoint; }; const vehicle_local_position_setpoint_s &get_position_setpoint()
{
return _vehicle_local_position_setpoint;
};
protected: protected:
static constexpr int _timeout = 500000; /*< maximal time in us before a loop or data times out */ static constexpr int _timeout = 500000; /*< maximal time in us before a loop or data times out */
@@ -104,11 +106,9 @@ protected:
*/ */
void _set_position_setpoint(const matrix::Vector3f position_setpoint) void _set_position_setpoint(const matrix::Vector3f position_setpoint)
{ {
if (_vehicle_position_setpoint != nullptr) { _vehicle_local_position_setpoint.x = position_setpoint(0);
_vehicle_position_setpoint->x = position_setpoint(0); _vehicle_local_position_setpoint.y = position_setpoint(1);
_vehicle_position_setpoint->y = position_setpoint(1); _vehicle_local_position_setpoint.z = position_setpoint(2);
_vehicle_position_setpoint->z = position_setpoint(2);
}
}; };
/** /**
@@ -116,38 +116,34 @@ protected:
*/ */
void _set_velocity_setpoint(const matrix::Vector3f velocity_setpoint) void _set_velocity_setpoint(const matrix::Vector3f velocity_setpoint)
{ {
if (_vehicle_position_setpoint != nullptr) { _vehicle_local_position_setpoint.vx = velocity_setpoint(0);
_vehicle_position_setpoint->vx = velocity_setpoint(0); _vehicle_local_position_setpoint.vy = velocity_setpoint(1);
_vehicle_position_setpoint->vy = velocity_setpoint(1); _vehicle_local_position_setpoint.vz = velocity_setpoint(2);
_vehicle_position_setpoint->vz = velocity_setpoint(2);
}
}; };
/** /**
* Put the acceleration vector produced by the task into the setpoint message * 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) void _set_acceleration_setpoint(const matrix::Vector3f acceleration_setpoint)
{ {
if (_vehicle_position_setpoint != nullptr) { _vehicle_local_position_setpoint.acc_x = acceleration_setpoint(0);
_vehicle_position_setpoint->acc_x = acceleration_setpoint(0); _vehicle_local_position_setpoint.acc_y = acceleration_setpoint(1);
_vehicle_position_setpoint->acc_y = acceleration_setpoint(1); _vehicle_local_position_setpoint.acc_z = acceleration_setpoint(2);
_vehicle_position_setpoint->acc_z = acceleration_setpoint(2);
return 0;
} else {
return 1;
}
}; };
/**
* Put the yaw angle produced by the task into the setpoint message
*/
void _set_yaw_setpoint(float yaw) { _vehicle_local_position_setpoint.yaw = yaw; };
private: private:
uORB::Subscription<vehicle_local_position_s> _sub_vehicle_local_position; uORB::Subscription<vehicle_local_position_s> _sub_vehicle_local_position;
hrt_abstime _starting_time_stamp = 0; /*< time stamp when task was activated */ 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 */ hrt_abstime _last_time_stamp = 0; /*< time stamp when task was last updated */
/* General output that every task has */ /* Output position setpoint that every task has */
vehicle_local_position_setpoint_s *_vehicle_position_setpoint; vehicle_local_position_setpoint_s _vehicle_local_position_setpoint;
void _evaluate_vehicle_position() void _evaluate_vehicle_position()
{ {