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

@@ -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)

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;
-
-	switch (command.command) {
-	case vehicle_command_s::VEHICLE_CMD_DO_ORBIT :
-		desired_task = FlightTaskIndex::Orbit;
-		break;
+	FlightTaskIndex desired_task = switchVehicleCommand(command.command);
 
-	// 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;
 			}
 		}

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 <new>
 
-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 */
 

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

+/****************************************************************************
+ *
+ *   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;
+	}
+}
\ No newline at end of file

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

+/****************************************************************************
+ *
+ *   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]@
+};

src/lib/FlightTasks/generate_flight_tasks.py

+#!/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()

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));