fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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 2017-11-05 19:48:20 +01:00 committed by Beat Küng
parent d48ba8be72
commit e49f80eaa8
2 changed files with 32 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
src/lib/FlightTasks/FlightTasks.hpp
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
*/
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++) {
_tasks[i]->set_vehicle_local_position_setpoint_pointer(vehicle_local_position_setpoint);
}
return get_position_setpoint();
};
/**

48
src/lib/FlightTasks/tasks/FlightTask.hpp
View File

@ -83,10 +83,12 @@ public:
};
/**
* Set local position setpoint data pointer if it's needed for the task
* @param pointer to manual control setpoint
* Get the output data
*/
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:
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)
{
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);
}
_vehicle_local_position_setpoint.x = position_setpoint(0);
_vehicle_local_position_setpoint.y = position_setpoint(1);
_vehicle_local_position_setpoint.z = position_setpoint(2);
};
/**
@ -116,38 +116,34 @@ protected:
*/
void _set_velocity_setpoint(const matrix::Vector3f velocity_setpoint)
{
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);
}
_vehicle_local_position_setpoint.vx = velocity_setpoint(0);
_vehicle_local_position_setpoint.vy = velocity_setpoint(1);
_vehicle_local_position_setpoint.vz = velocity_setpoint(2);
};
/**
* 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_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;
}
_vehicle_local_position_setpoint.acc_x = acceleration_setpoint(0);
_vehicle_local_position_setpoint.acc_y = acceleration_setpoint(1);
_vehicle_local_position_setpoint.acc_z = acceleration_setpoint(2);
};
/**
* 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:
uORB::Subscription<vehicle_local_position_s> _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 output that every task has */
vehicle_local_position_setpoint_s *_vehicle_position_setpoint;
/* Output position setpoint that every task has */
vehicle_local_position_setpoint_s _vehicle_local_position_setpoint;
void _evaluate_vehicle_position()
{