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

FlightTasks: create struct that contains FlightTask and FlightTaskIndex

Browse Source
This commit is contained in:
Dennis Mannhart
2018-07-20 09:26:18 +02:00
committed by Lorenz Meier
parent 83cc2b0e72
commit b1530d1a81
2 changed files with 71 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/FlightTasks.cpp
+59 -40
View File
@@ -9,7 +9,7 @@ bool FlightTasks::update()
if (isAnyTaskActive()) {
_subscription_array.update();
return _current_task->updateInitialize() && _current_task->update();
return _current_task.task->updateInitialize() && _current_task.task->update();
}
return false;
@@ -18,7 +18,7 @@ bool FlightTasks::update()
const vehicle_local_position_setpoint_s FlightTasks::getPositionSetpoint()
{
if (isAnyTaskActive()) {
return _current_task->getPositionSetpoint();
return _current_task.task->getPositionSetpoint();
} else {
return FlightTask::empty_setpoint;
@@ -28,96 +28,115 @@ const vehicle_local_position_setpoint_s FlightTasks::getPositionSetpoint()
const vehicle_constraints_s FlightTasks::getConstraints()
{
if (isAnyTaskActive()) {
return _current_task->getConstraints();
return _current_task.task->getConstraints();
} else {
return FlightTask::empty_constraints;
}
}
int FlightTasks::switchTask(FlightTaskIndex new_task_index)
int FlightTasks::_initTask(FlightTaskIndex task_index)
{
/* switch to the running task, nothing to do */
if (new_task_index == _current_task_index) {
return 0;
// disable the old task if there is any
if (_current_task.task) {
_current_task.task->~FlightTask();
_current_task.task = nullptr;
_current_task.index = FlightTaskIndex::None;
}
/* disable the old task if there is any */
if (_current_task) {
_current_task->~FlightTask();
_current_task = nullptr;
_current_task_index = FlightTaskIndex::None;
}
switch (new_task_index) {
switch (task_index) {
case FlightTaskIndex::None:
/* disable tasks is a success */
return 0;
// already disabled task
break;
case FlightTaskIndex::Stabilized:
_current_task = new (&_task_union.stabilized) FlightTaskManualStabilized();
_current_task.task = new (&_task_union.stabilized) FlightTaskManualStabilized();
break;
case FlightTaskIndex::Altitude:
_current_task = new (&_task_union.altitude) FlightTaskManualAltitude();
_current_task.task = new (&_task_union.altitude) FlightTaskManualAltitude();
break;
case FlightTaskIndex::AltitudeSmooth:
_current_task = new (&_task_union.altitude_smooth) FlightTaskManualAltitudeSmooth();
_current_task.task = new (&_task_union.altitude_smooth) FlightTaskManualAltitudeSmooth();
break;
case FlightTaskIndex::Position:
_current_task = new (&_task_union.position) FlightTaskManualPosition();
_current_task.task = new (&_task_union.position) FlightTaskManualPosition();
break;
case FlightTaskIndex::PositionSmooth:
_current_task = new (&_task_union.position_smooth) FlightTaskManualPositionSmooth();
_current_task.task =
new (&_task_union.position_smooth) FlightTaskManualPositionSmooth();
break;
case FlightTaskIndex::Orbit:
_current_task = new (&_task_union.orbit) FlightTaskOrbit();
_current_task.task = new (&_task_union.orbit) FlightTaskOrbit();
break;
case FlightTaskIndex::Sport:
_current_task = new (&_task_union.sport) FlightTaskSport();
_current_task.task = new (&_task_union.sport) FlightTaskSport();
break;
case FlightTaskIndex::AutoLine:
_current_task = new (&_task_union.autoLine) FlightTaskAutoLine();
_current_task.task = new (&_task_union.autoLine) FlightTaskAutoLine();
break;
case FlightTaskIndex::AutoFollowMe:
_current_task = new (&_task_union.autoFollowMe) FlightTaskAutoFollowMe();
_current_task.task =
new (&_task_union.autoFollowMe) FlightTaskAutoFollowMe();
break;
case FlightTaskIndex::Offboard:
_current_task = new (&_task_union.offboard) FlightTaskOffboard();
_current_task.task = new (&_task_union.offboard) FlightTaskOffboard();
break;
default:
/* invalid task */
// invalid task
return 1;
}
// task construction succeeded
_current_task.index = task_index;
return 0;
}
int FlightTasks::switchTask(FlightTaskIndex new_task_index)
{
// switch to the running task, nothing to do
if (new_task_index == _current_task.index) {
return 0;
}
if (_initTask(new_task_index)) {
// invalid task
return -1;
}
if (!_current_task.task) {
// no task running
return 0;
}
/* subscription failed */
if (!_current_task->initializeSubscriptions(_subscription_array)) {
_current_task->~FlightTask();
_current_task = nullptr;
_current_task_index = FlightTaskIndex::None;
if (!_current_task.task->initializeSubscriptions(_subscription_array)) {
_current_task.task->~FlightTask();
_current_task.task = nullptr;
_current_task.index = FlightTaskIndex::None;
return -2;
}
_subscription_array.forcedUpdate(); // make sure data is available for all new subscriptions
/* activation failed */
if (!_current_task->updateInitialize() || !_current_task->activate()) {
_current_task->~FlightTask();
_current_task = nullptr;
_current_task_index = FlightTaskIndex::None;
if (!_current_task.task->updateInitialize() || !_current_task.task->activate()) {
_current_task.task->~FlightTask();
_current_task.task = nullptr;
_current_task.index = FlightTaskIndex::None;
return -3;
}
_current_task_index = new_task_index;
return 0;
}
@@ -135,8 +154,8 @@ int FlightTasks::switchTask(int new_task_index)
void FlightTasks::handleParameterUpdate()
{
if (_current_task) {
_current_task->handleParameterUpdate();
if (_current_task.task) {
_current_task.task->handleParameterUpdate();
}
}
@@ -185,7 +204,7 @@ void FlightTasks::_updateCommand()
// cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
//
// /* if the task is running apply parameters to it and see if it rejects */
// if (isAnyTaskActive() && !_current_task->applyCommandParameters(command)) {
// if (isAnyTaskActive() && !_current_task.task->applyCommandParameters(command)) {
// cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
//
// /* if we just switched and parameters are not accepted, go to failsafe */
src/lib/FlightTasks/FlightTasks.hpp
+12 -7
View File
@@ -80,8 +80,8 @@ public:
~FlightTasks()
{
if (_current_task) {
_current_task->~FlightTask();
if (_current_task.task) {
_current_task.task->~FlightTask();
}
}
@@ -107,7 +107,7 @@ public:
* Switch to the next task in the available list (for testing)
* @return true on success, false on error
*/
int switchTask() { return switchTask(static_cast<int>(_current_task_index) + 1); }
int switchTask() { return switchTask(static_cast<int>(_current_task.index) + 1); }
/**
* Switch to a specific task (for normal usage)
@@ -121,13 +121,13 @@ public:
* Get the number of the active task
* @return number of active task, -1 if there is none
*/
int getActiveTask() const { return static_cast<int>(_current_task_index); }
int getActiveTask() const { return static_cast<int>(_current_task.index); }
/**
* Check if any task is active
* @return true if a task is active, false if not
*/
bool isAnyTaskActive() const { return _current_task; }
bool isAnyTaskActive() const { return _current_task.task; }
/**
* Call this whenever a parameter update notification is received (parameter_update uORB message)
@@ -161,8 +161,11 @@ private:
FlightTaskOffboard offboard;
} _task_union; /**< storage for the currently active task */
FlightTask *_current_task = nullptr;
FlightTaskIndex _current_task_index = FlightTaskIndex::None;
struct flight_task_t {
FlightTask *task;
FlightTaskIndex index;
};
flight_task_t _current_task = {nullptr, FlightTaskIndex::None};
SubscriptionArray _subscription_array;
@@ -185,6 +188,8 @@ private:
* Check for vehicle commands (received via MAVLink), evaluate and acknowledge them
*/
void _updateCommand();
int _initTask(FlightTaskIndex task_index);
// int _sub_vehicle_command = -1; /**< topic handle on which commands are received */
// orb_advert_t _pub_vehicle_command_ack = nullptr; /**< topic handle to which commands get acknowledged */
};