Browse Source

FlightTasks: generate tasks depending on target

- rename flight tasks to use camelCase
- add core tasks to flight tasks cmake
- add additional tasks in targets (TODO)
- add templates
- generate hpp and cpp which contain all specified tasks
sbg
ChristophTobler 7 years ago committed by Dennis Mannhart
parent
commit
8090708f76
  1. 73
      src/lib/FlightTasks/CMakeLists.txt
  2. 81
      src/lib/FlightTasks/FlightTasks.cpp
  3. 49
      src/lib/FlightTasks/FlightTasks.hpp
  4. 100
      src/lib/FlightTasks/Templates/FlightTasks_generated.cpp.template
  5. 78
      src/lib/FlightTasks/Templates/FlightTasks_generated.hpp.template
  6. 27
      src/lib/FlightTasks/generate_flight_tasks.py
  7. 10
      src/modules/mc_pos_control/mc_pos_control_main.cpp

73
src/lib/FlightTasks/CMakeLists.txt

@ -31,23 +31,70 @@
# #
############################################################################ ############################################################################
# add core flight tasks to list
set(flight_tasks_all
ManualStabilized
ManualAltitude
ManualAltitudeSmooth
ManualPosition
ManualPositionSmooth
Sport
AutoLine
AutoFollowMe
Offboard
${flight_tasks_additional}
)
set(files_to_generate
FlightTasks_generated.hpp
FlightTasks_generated.cpp
)
# generate files needed for Flight Tasks
if(${flight_tasks_additional}) # TODO look for nicer solution
add_custom_command(
OUTPUT
${CMAKE_CURRENT_BINARY_DIR}/FlightTasks_generated.hpp
${CMAKE_CURRENT_BINARY_DIR}/FlightTasks_generated.cpp
COMMAND ${PYTHON_EXECUTABLE} generate_flight_tasks.py
-t ${flight_tasks_all}
-s ${flight_tasks_additional}
-i ${CMAKE_CURRENT_SOURCE_DIR}/Templates
-o ${CMAKE_CURRENT_BINARY_DIR}
-f ${files_to_generate}
COMMENT "Generating Flight Tasks"
WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
VERBATIM
)
else()
add_custom_command(
OUTPUT
${CMAKE_CURRENT_BINARY_DIR}/FlightTasks_generated.hpp
${CMAKE_CURRENT_BINARY_DIR}/FlightTasks_generated.cpp
COMMAND ${PYTHON_EXECUTABLE} generate_flight_tasks.py
-t ${flight_tasks_all}
-i ${CMAKE_CURRENT_SOURCE_DIR}/Templates
-o ${CMAKE_CURRENT_BINARY_DIR}
-f ${files_to_generate}
COMMENT "Generating Flight Tasks"
WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
VERBATIM
)
endif()
# add the Flight Tasks library
px4_add_library(FlightTasks px4_add_library(FlightTasks
FlightTasks.cpp FlightTasks.cpp
FlightTasks_generated.cpp
) )
target_include_directories(FlightTasks PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) # add directories to target
target_include_directories(FlightTasks PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_BINARY_DIR})
target_link_libraries(FlightTasks PUBLIC # add all flight task dependencies
FlightTaskManual foreach(task ${flight_tasks_all})
FlightTaskManualStabilized target_link_libraries(FlightTasks PUBLIC FlightTask${task})
FlightTaskManualAltitude endforeach()
FlightTaskManualAltitudeSmooth
FlightTaskManualPosition
FlightTaskManualPositionSmooth
FlightTaskSport
FlightTaskAutoLine
FlightTaskAutoFollowMe
FlightTaskOffboard
)
# add subdirectory containing all tasks
add_subdirectory(tasks) add_subdirectory(tasks)

81
src/lib/FlightTasks/FlightTasks.cpp

@ -47,73 +47,6 @@ const vehicle_constraints_s FlightTasks::getConstraints()
} }
} }
int FlightTasks::_initTask(FlightTaskIndex task_index)
{
// 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;
}
switch (task_index) {
case FlightTaskIndex::None:
// already disabled task
break;
case FlightTaskIndex::Stabilized:
_current_task.task = new (&_task_union.stabilized) FlightTaskManualStabilized();
break;
case FlightTaskIndex::Altitude:
_current_task.task = new (&_task_union.altitude) FlightTaskManualAltitude();
break;
case FlightTaskIndex::AltitudeSmooth:
_current_task.task = new (&_task_union.altitude_smooth) FlightTaskManualAltitudeSmooth();
break;
case FlightTaskIndex::Position:
_current_task.task = new (&_task_union.position) FlightTaskManualPosition();
break;
case FlightTaskIndex::PositionSmooth:
_current_task.task =
new (&_task_union.position_smooth) FlightTaskManualPositionSmooth();
break;
case FlightTaskIndex::Orbit:
_current_task.task = new (&_task_union.orbit) FlightTaskOrbit();
break;
case FlightTaskIndex::Sport:
_current_task.task = new (&_task_union.sport) FlightTaskSport();
break;
case FlightTaskIndex::AutoLine:
_current_task.task = new (&_task_union.autoLine) FlightTaskAutoLine();
break;
case FlightTaskIndex::AutoFollowMe:
_current_task.task =
new (&_task_union.autoFollowMe) FlightTaskAutoFollowMe();
break;
case FlightTaskIndex::Offboard:
_current_task.task = new (&_task_union.offboard) FlightTaskOffboard();
break;
default:
// invalid task
return 1;
}
// task construction succeeded
_current_task.index = task_index;
return 0;
}
int FlightTasks::switchTask(FlightTaskIndex new_task_index) int FlightTasks::switchTask(FlightTaskIndex new_task_index)
{ {
// switch to the running task, nothing to do // switch to the running task, nothing to do
@ -202,15 +135,11 @@ void FlightTasks::_updateCommand()
orb_copy(ORB_ID(vehicle_command), _sub_vehicle_command, &command); orb_copy(ORB_ID(vehicle_command), _sub_vehicle_command, &command);
// check what command it is // check what command it is
FlightTaskIndex desired_task = FlightTaskIndex::None; FlightTaskIndex desired_task = switchVehicleCommand(command.command);
switch (command.command) {
case vehicle_command_s::VEHICLE_CMD_DO_ORBIT :
desired_task = FlightTaskIndex::Orbit;
break;
// ignore all unkown commands if (desired_task == FlightTaskIndex::None) {
default : return; // ignore all unkown commands
return;
} }
// switch to the commanded task // switch to the commanded task
@ -227,7 +156,7 @@ void FlightTasks::_updateCommand()
// if we just switched and parameters are not accepted, go to failsafe // if we just switched and parameters are not accepted, go to failsafe
if (switch_result == 1) { if (switch_result == 1) {
switchTask(FlightTaskIndex::Position); switchTask(FlightTaskIndex::ManualPosition);
cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED; cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
} }
} }

49
src/lib/FlightTasks/FlightTasks.hpp

@ -41,38 +41,12 @@
#pragma once #pragma once
#include "FlightTask.hpp" #include "tasks/FlightTask.hpp"
#include "FlightTaskManualAltitude.hpp" #include "tasks/SubscriptionArray.hpp"
#include "FlightTaskManualAltitudeSmooth.hpp" #include "FlightTasks_generated.hpp"
#include "FlightTaskManualPosition.hpp"
#include "FlightTaskManualPositionSmooth.hpp"
#include "FlightTaskManualStabilized.hpp"
#include "FlightTaskAutoLine.hpp"
#include "FlightTaskAutoFollowMe.hpp"
#include "FlightTaskOrbit.hpp"
#include "tasks/Sport/FlightTaskSport.hpp"
#include "FlightTaskOffboard.hpp"
#include "SubscriptionArray.hpp"
#include <new> #include <new>
enum class FlightTaskIndex : int {
None = -1,
Stabilized,
Altitude,
AltitudeSmooth,
Position,
PositionSmooth,
Orbit,
Sport,
AutoLine,
AutoFollowMe,
Offboard,
Count // number of tasks
};
class FlightTasks class FlightTasks
{ {
public: public:
@ -145,21 +119,7 @@ private:
* Union with all existing tasks: we use it to make sure that only the memory of the largest existing * Union with all existing tasks: we use it to make sure that only the memory of the largest existing
* task is needed, and to avoid using dynamic memory allocations. * task is needed, and to avoid using dynamic memory allocations.
*/ */
union TaskUnion { TaskUnion _task_union; /**< storage for the currently active task */
TaskUnion() {}
~TaskUnion() {}
FlightTaskManualStabilized stabilized;
FlightTaskManualAltitude altitude;
FlightTaskManualAltitudeSmooth altitude_smooth;
FlightTaskManualPosition position;
FlightTaskManualPositionSmooth position_smooth;
FlightTaskOrbit orbit;
FlightTaskSport sport;
FlightTaskAutoLine autoLine;
FlightTaskAutoFollowMe autoFollowMe;
FlightTaskOffboard offboard;
} _task_union; /**< storage for the currently active task */
struct flight_task_t { struct flight_task_t {
FlightTask *task; FlightTask *task;
@ -188,6 +148,7 @@ private:
* Check for vehicle commands (received via MAVLink), evaluate and acknowledge them * Check for vehicle commands (received via MAVLink), evaluate and acknowledge them
*/ */
void _updateCommand(); void _updateCommand();
FlightTaskIndex switchVehicleCommand(const int command);
int _sub_vehicle_command = -1; /**< topic handle on which commands are received */ 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 */ orb_advert_t _pub_vehicle_command_ack = nullptr; /**< topic handle to which commands get acknowledged */

100
src/lib/FlightTasks/Templates/FlightTasks_generated.cpp.template

@ -0,0 +1,100 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @@file FlightTasks_generated.cpp
*
* Generated file to switch between all required flight tasks
*
* @@author Christoph Tobler <christoph@@px4.io>
*/
#include "FlightTasks.hpp"
#include "FlightTasks_generated.hpp"
int FlightTasks::_initTask(FlightTaskIndex task_index)
{
// 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;
}
switch (task_index) {
case FlightTaskIndex::None:
// already disabled task
break;
@# loop through all requested tasks
@[if tasks]@
@[for task in tasks]@
@{
firstLowercase = lambda s: s[:1].lower() + s[1:] if s else ''
}@
case FlightTaskIndex::@(task):
_current_task.task = new (&_task_union.@(firstLowercase(task))) FlightTask@(task)();
break;
@[end for]@
@[end if]@
default:
// invalid task
return 1;
}
// task construction succeeded
_current_task.index = task_index;
return 0;
}
FlightTaskIndex FlightTasks::switchVehicleCommand(const int command)
{
switch (command) {
@# loop through all additional tasks
@[if tasks_add]@
@[for task in tasks_add]@
@{
upperCase = lambda s: s[:].upper() if s else ''
}@
case vehicle_command_s::VEHICLE_CMD_DO_@(upperCase(task)) :
return FlightTaskIndex::@(task);
break;
@[end for]@
@[end if]@
// ignore all unkown commands
default : return FlightTaskIndex::None;
}
}

78
src/lib/FlightTasks/Templates/FlightTasks_generated.hpp.template

@ -0,0 +1,78 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @@file FlightTasks_generated.hpp
*
* Generated Header to list all required flight tasks
*
* @@author Christoph Tobler <christoph@@px4.io>
*/
#pragma once
// include all required headers
#include "FlightTasks.hpp"
@# loop through all requested tasks
@[if tasks]@
@[for task in tasks]@
#include "FlightTask@(task).hpp"
@[end for]@
@[end if]@
enum class FlightTaskIndex : int {
None = -1,
@# loop through all requested tasks
@[if tasks]@
@[for task in tasks]@
@(task),
@[end for]@
@[end if]@
Count // number of tasks
};
union TaskUnion {
TaskUnion() {}
~TaskUnion() {}
@# loop through all requested tasks
@[if tasks]@
@{
firstLowercase = lambda s: s[:1].lower() + s[1:] if s else ''
}@
@[for task in tasks]@
FlightTask@(task) @(firstLowercase(task));
@[end for]@
@[end if]@
};

27
src/lib/FlightTasks/generate_flight_tasks.py

@ -0,0 +1,27 @@
#!/usr/bin/env python
import em
import os
import argparse
parser = argparse.ArgumentParser()
parser.add_argument("-t", "--tasks", dest='tasks_all', nargs='+', required=True, help="All tasks to be generated")
parser.add_argument("-s", "--tasks_additional", dest='tasks_add', nargs='+', help="Additional tasks to be generated (on top of the core)")
parser.add_argument("-i", "--input_directory", dest='directory_in', required=True, help="Output directory")
parser.add_argument("-o", "--output_directory", dest='directory_out', required=True, help="Input directory")
parser.add_argument("-f", "--files", dest='gen_files', nargs='+', required=True, help="Files to generate")
# Parse arguments
args = parser.parse_args()
for gen_file in args.gen_files:
ofile = args.directory_out + "/" + gen_file
output_file = open(ofile, 'w')
# need to specify the em_globals inside the loop -> em.Error: interpreter globals collision
em_globals = {
"tasks": args.tasks_all,
"tasks_add": args.tasks_add,
}
interpreter = em.Interpreter(output=output_file, globals=em_globals)
interpreter.file(open(args.directory_in + "/" + gen_file + ".template"))
interpreter.shutdown()

10
src/modules/mc_pos_control/mc_pos_control_main.cpp

@ -727,11 +727,11 @@ MulticopterPositionControl::start_flight_task()
switch (MPC_POS_MODE.get()) { switch (MPC_POS_MODE.get()) {
case 0: case 0:
error = _flight_tasks.switchTask(FlightTaskIndex::Position); error = _flight_tasks.switchTask(FlightTaskIndex::ManualPosition);
break; break;
case 1: case 1:
error = _flight_tasks.switchTask(FlightTaskIndex::PositionSmooth); error = _flight_tasks.switchTask(FlightTaskIndex::ManualPositionSmooth);
break; break;
case 2: case 2:
@ -739,7 +739,7 @@ MulticopterPositionControl::start_flight_task()
break; break;
default: default:
error = _flight_tasks.switchTask(FlightTaskIndex::Position); error = _flight_tasks.switchTask(FlightTaskIndex::ManualPosition);
break; break;
} }
@ -754,7 +754,7 @@ MulticopterPositionControl::start_flight_task()
// manual altitude control // manual altitude control
if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ALTCTL || task_failure) { if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ALTCTL || task_failure) {
int error = _flight_tasks.switchTask(FlightTaskIndex::Altitude); int error = _flight_tasks.switchTask(FlightTaskIndex::ManualAltitude);
if (error != 0) { if (error != 0) {
PX4_WARN("Altitude-Ctrl activation failed with error: %s", _flight_tasks.errorToString(error)); PX4_WARN("Altitude-Ctrl activation failed with error: %s", _flight_tasks.errorToString(error));
@ -769,7 +769,7 @@ MulticopterPositionControl::start_flight_task()
// manual stabilized control // manual stabilized control
if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_MANUAL if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_MANUAL
|| _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_STAB || task_failure) { || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_STAB || task_failure) {
int error = _flight_tasks.switchTask(FlightTaskIndex::Stabilized); int error = _flight_tasks.switchTask(FlightTaskIndex::ManualStabilized);
if (error != 0) { if (error != 0) {
PX4_WARN("Stabilized-Ctrl failed with error: %s", _flight_tasks.errorToString(error)); PX4_WARN("Stabilized-Ctrl failed with error: %s", _flight_tasks.errorToString(error));

Loading…
Cancel
Save