From 8090708f76906246caee481f66153995148453b8 Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Tue, 31 Jul 2018 17:21:52 +0200 Subject: [PATCH] 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 --- src/lib/FlightTasks/CMakeLists.txt | 73 ++++++++++--- src/lib/FlightTasks/FlightTasks.cpp | 81 +------------- src/lib/FlightTasks/FlightTasks.hpp | 49 +-------- .../FlightTasks_generated.cpp.template | 100 ++++++++++++++++++ .../FlightTasks_generated.hpp.template | 78 ++++++++++++++ src/lib/FlightTasks/generate_flight_tasks.py | 27 +++++ .../mc_pos_control/mc_pos_control_main.cpp | 10 +- 7 files changed, 280 insertions(+), 138 deletions(-) create mode 100644 src/lib/FlightTasks/Templates/FlightTasks_generated.cpp.template create mode 100644 src/lib/FlightTasks/Templates/FlightTasks_generated.hpp.template create mode 100644 src/lib/FlightTasks/generate_flight_tasks.py diff --git a/src/lib/FlightTasks/CMakeLists.txt b/src/lib/FlightTasks/CMakeLists.txt index 249fc51749..334aa9fe06 100644 --- a/src/lib/FlightTasks/CMakeLists.txt +++ b/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 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 - FlightTaskManual - FlightTaskManualStabilized - FlightTaskManualAltitude - FlightTaskManualAltitudeSmooth - FlightTaskManualPosition - FlightTaskManualPositionSmooth - FlightTaskSport - FlightTaskAutoLine - FlightTaskAutoFollowMe - FlightTaskOffboard -) +# add all flight task dependencies +foreach(task ${flight_tasks_all}) + target_link_libraries(FlightTasks PUBLIC FlightTask${task}) +endforeach() +# add subdirectory containing all tasks add_subdirectory(tasks) diff --git a/src/lib/FlightTasks/FlightTasks.cpp b/src/lib/FlightTasks/FlightTasks.cpp index 71c8b6f4b9..2fe0b7063a 100644 --- a/src/lib/FlightTasks/FlightTasks.cpp +++ b/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) { // 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); // 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 - default : return; + if (desired_task == FlightTaskIndex::None) { + // ignore all unkown commands + return; } // 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 (switch_result == 1) { - switchTask(FlightTaskIndex::Position); + switchTask(FlightTaskIndex::ManualPosition); cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED; } } diff --git a/src/lib/FlightTasks/FlightTasks.hpp b/src/lib/FlightTasks/FlightTasks.hpp index a59704f678..1676bd3176 100644 --- a/src/lib/FlightTasks/FlightTasks.hpp +++ b/src/lib/FlightTasks/FlightTasks.hpp @@ -41,38 +41,12 @@ #pragma once -#include "FlightTask.hpp" -#include "FlightTaskManualAltitude.hpp" -#include "FlightTaskManualAltitudeSmooth.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 "tasks/FlightTask.hpp" +#include "tasks/SubscriptionArray.hpp" +#include "FlightTasks_generated.hpp" #include -enum class FlightTaskIndex : int { - None = -1, - Stabilized, - Altitude, - AltitudeSmooth, - Position, - PositionSmooth, - Orbit, - Sport, - AutoLine, - AutoFollowMe, - Offboard, - - Count // number of tasks -}; - class FlightTasks { 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 * task is needed, and to avoid using dynamic memory allocations. */ - union TaskUnion { - 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 */ + TaskUnion _task_union; /**< storage for the currently active task */ struct flight_task_t { FlightTask *task; @@ -188,6 +148,7 @@ private: * Check for vehicle commands (received via MAVLink), evaluate and acknowledge them */ void _updateCommand(); + FlightTaskIndex switchVehicleCommand(const int command); 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 */ diff --git a/src/lib/FlightTasks/Templates/FlightTasks_generated.cpp.template b/src/lib/FlightTasks/Templates/FlightTasks_generated.cpp.template new file mode 100644 index 0000000000..99917f41da --- /dev/null +++ b/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 + */ + +#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; + } +} \ No newline at end of file diff --git a/src/lib/FlightTasks/Templates/FlightTasks_generated.hpp.template b/src/lib/FlightTasks/Templates/FlightTasks_generated.hpp.template new file mode 100644 index 0000000000..724f2a762c --- /dev/null +++ b/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 + */ + + #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]@ +}; diff --git a/src/lib/FlightTasks/generate_flight_tasks.py b/src/lib/FlightTasks/generate_flight_tasks.py new file mode 100644 index 0000000000..8f8ea7e732 --- /dev/null +++ b/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() diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 48cb8df9b1..c808fedc9b 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -727,11 +727,11 @@ MulticopterPositionControl::start_flight_task() switch (MPC_POS_MODE.get()) { case 0: - error = _flight_tasks.switchTask(FlightTaskIndex::Position); + error = _flight_tasks.switchTask(FlightTaskIndex::ManualPosition); break; case 1: - error = _flight_tasks.switchTask(FlightTaskIndex::PositionSmooth); + error = _flight_tasks.switchTask(FlightTaskIndex::ManualPositionSmooth); break; case 2: @@ -739,7 +739,7 @@ MulticopterPositionControl::start_flight_task() break; default: - error = _flight_tasks.switchTask(FlightTaskIndex::Position); + error = _flight_tasks.switchTask(FlightTaskIndex::ManualPosition); break; } @@ -754,7 +754,7 @@ MulticopterPositionControl::start_flight_task() // manual altitude control 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) { PX4_WARN("Altitude-Ctrl activation failed with error: %s", _flight_tasks.errorToString(error)); @@ -769,7 +769,7 @@ MulticopterPositionControl::start_flight_task() // manual stabilized control if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_MANUAL || _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) { PX4_WARN("Stabilized-Ctrl failed with error: %s", _flight_tasks.errorToString(error));