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

FlightTasks: switched output position setpoint to be pointer based

Browse Source 
changed "NULL"-pointers to "nullptr" for better compliance
This commit is contained in:
Matthias Grob
2018-04-05 07:30:12 +02:00
committed by Beat Küng
parent 28f4d18062
commit a8a2b4b6f3
2 changed files with 58 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/FlightTasks.hpp
+12 -13
View File
@@ -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
src/lib/FlightTasks/tasks/FlightTask.hpp
+46 -26
View File
@@ -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] */