diff --git a/.gitignore b/.gitignore index e9c0f5dc8c..e760bc6580 100644 --- a/.gitignore +++ b/.gitignore @@ -101,8 +101,6 @@ msg/tmp/ msg/topics_sources/ platforms/posix/apps.cpp platforms/posix/apps.h -src/lib/flight_tasks/FlightTasks_generated.cpp -src/lib/flight_tasks/FlightTasks_generated.hpp src/lib/parameters/px4_parameters.c src/lib/parameters/px4_parameters.h src/lib/parameters/px4_parameters_public.h diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_apps b/ROMFS/px4fmu_common/init.d/rc.vtol_apps index b776dfc2cb..c6c1cacfbf 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_apps +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_apps @@ -21,7 +21,7 @@ vtol_att_control start mc_rate_control start vtol mc_att_control start vtol -flight_mode_manager start vtol +flight_mode_manager start mc_pos_control start vtol mc_hover_thrust_estimator start diff --git a/posix-configs/SITL/init/test/test_shutdown b/posix-configs/SITL/init/test/test_shutdown index 14c7efb344..8fe5fe1f7d 100644 --- a/posix-configs/SITL/init/test/test_shutdown +++ b/posix-configs/SITL/init/test/test_shutdown @@ -28,7 +28,7 @@ navigator start ekf2 start vtol_att_control start mc_hover_thrust_estimator start -flight_mode_manager start vtol +flight_mode_manager start mc_pos_control start vtol mc_att_control start vtol mc_rate_control start vtol diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index f273242105..7702ec2491 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -46,7 +46,6 @@ add_subdirectory(controllib) add_subdirectory(conversion) add_subdirectory(drivers) add_subdirectory(ecl) -add_subdirectory(flight_tasks) add_subdirectory(hysteresis) add_subdirectory(l1) add_subdirectory(landing_slope) diff --git a/src/lib/flight_tasks/CMakeLists.txt b/src/lib/flight_tasks/CMakeLists.txt deleted file mode 100644 index d7372362ce..0000000000 --- a/src/lib/flight_tasks/CMakeLists.txt +++ /dev/null @@ -1,125 +0,0 @@ -############################################################################ -# -# Copyright (c) 2017 - 2020 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. -# -############################################################################ - -########################################### -# Prepare flight tasks -########################################### - -# add upstream flight tasks (they are being handled differently from the core inside the python script) -list(APPEND flight_tasks_to_add - Orbit -) -# remove possible duplicates -list(REMOVE_DUPLICATES flight_tasks_to_add) - -# remove flight tasks depending on target -if(flight_tasks_to_remove) - list(REMOVE_ITEM flight_tasks_to_add - ${flight_tasks_to_remove} - ) -endif() - -# add core flight tasks to list -list(APPEND flight_tasks_all - ManualAltitude - ManualAltitudeSmoothVel - ManualPosition - ManualPositionSmoothVel - AutoLineSmoothVel - AutoFollowMe - Offboard - Failsafe - Descend - Transition - ManualAcceleration - ${flight_tasks_to_add} -) - -# set the files to be generated -set(files_to_generate - FlightTasks_generated.hpp - FlightTasks_generated.cpp -) - -# generate files needed for Flight Tasks -set(python_args - -t ${flight_tasks_all} - -i ${CMAKE_CURRENT_SOURCE_DIR}/Templates - -o ${CMAKE_CURRENT_BINARY_DIR} - -f ${files_to_generate} -) - -# add the additional tasks for the python script (if there are any) -if(flight_tasks_to_add) - list(APPEND python_args - -s ${flight_tasks_to_add} - ) -endif() - -# generate the files using the python script and template -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 ${python_args} - COMMENT "Generating Flight Tasks" - DEPENDS - Templates/FlightTasks_generated.cpp.em - Templates/FlightTasks_generated.hpp.em - WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} - VERBATIM -) - -########################################### -# Create Flight Tasks Library -########################################### - -add_compile_options( - -Wno-cast-align - ) # TODO: fix and enable - -px4_add_library(FlightTasks - FlightTasks.cpp - FlightTasks_generated.cpp -) - -# add directories to target -target_include_directories(FlightTasks PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_BINARY_DIR}) - -# 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/flight_tasks/FlightTasks.cpp b/src/lib/flight_tasks/FlightTasks.cpp deleted file mode 100644 index ae57fe7fc6..0000000000 --- a/src/lib/flight_tasks/FlightTasks.cpp +++ /dev/null @@ -1,191 +0,0 @@ -#include "FlightTasks.hpp" - -#include -#include - -FlightTasks::FlightTasks() -{ - // initialize all flight-tasks - // currently this is required to get all parameters read - for (int i = 0; i < static_cast(FlightTaskIndex::Count); i++) { - _initTask(static_cast(i)); - } - - // disable all tasks - _initTask(FlightTaskIndex::None); -} - -bool FlightTasks::update() -{ - _updateCommand(); - - if (isAnyTaskActive()) { - return _current_task.task->updateInitialize() && _current_task.task->update() && _current_task.task->updateFinalize(); - } - - return false; -} - -const vehicle_local_position_setpoint_s FlightTasks::getPositionSetpoint() -{ - if (isAnyTaskActive()) { - return _current_task.task->getPositionSetpoint(); - - } else { - return FlightTask::empty_setpoint; - } -} - -const ekf_reset_counters_s FlightTasks::getResetCounters() -{ - if (isAnyTaskActive()) { - return _current_task.task->getResetCounters(); - - } else { - return FlightTask::zero_reset_counters; - } -} - -const vehicle_constraints_s FlightTasks::getConstraints() -{ - if (isAnyTaskActive()) { - return _current_task.task->getConstraints(); - - } else { - return FlightTask::empty_constraints; - } -} - -const landing_gear_s FlightTasks::getGear() -{ - if (isAnyTaskActive()) { - return _current_task.task->getGear(); - - } else { - return FlightTask::empty_landing_gear_default_keep; - } -} - -FlightTaskError FlightTasks::switchTask(FlightTaskIndex new_task_index) -{ - // switch to the running task, nothing to do - if (new_task_index == _current_task.index) { - return FlightTaskError::NoError; - } - - // Save current setpoints for the next FlightTask - const vehicle_local_position_setpoint_s last_setpoint = getPositionSetpoint(); - const ekf_reset_counters_s last_reset_counters = getResetCounters(); - - if (_initTask(new_task_index)) { - // invalid task - return FlightTaskError::InvalidTask; - } - - if (!isAnyTaskActive()) { - // no task running - return FlightTaskError::NoError; - } - - // activation failed - if (!_current_task.task->updateInitialize() || !_current_task.task->activate(last_setpoint)) { - _current_task.task->~FlightTask(); - _current_task.task = nullptr; - _current_task.index = FlightTaskIndex::None; - return FlightTaskError::ActivationFailed; - } - - _current_task.task->setResetCounters(last_reset_counters); - - return FlightTaskError::NoError; -} - -FlightTaskError FlightTasks::switchTask(int new_task_index) -{ - // make sure we are in range of the enumeration before casting - if (static_cast(FlightTaskIndex::None) <= new_task_index && - static_cast(FlightTaskIndex::Count) > new_task_index) { - return switchTask(FlightTaskIndex(new_task_index)); - } - - switchTask(FlightTaskIndex::None); - return FlightTaskError::InvalidTask; -} - -void FlightTasks::handleParameterUpdate() -{ - if (isAnyTaskActive()) { - _current_task.task->handleParameterUpdate(); - } -} - -const char *FlightTasks::errorToString(const FlightTaskError error) -{ - for (auto e : _taskError) { - if (static_cast(e.error) == error) { - return e.msg; - } - } - - return "This error is not mapped to a string or is unknown."; -} - -void FlightTasks::reActivate() -{ - if (isAnyTaskActive()) { - _current_task.task->reActivate(); - } -} - -void FlightTasks::_updateCommand() -{ - // check if there's any new command - bool updated = _sub_vehicle_command.updated(); - - if (!updated) { - return; - } - - // get command - vehicle_command_s command{}; - _sub_vehicle_command.copy(&command); - - // check what command it is - FlightTaskIndex desired_task = switchVehicleCommand(command.command); - - if (desired_task == FlightTaskIndex::None) { - // ignore all unkown commands - return; - } - - // switch to the commanded task - FlightTaskError switch_result = switchTask(desired_task); - uint8_t cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED; - - // if we are in/switched to the desired task - if (switch_result >= FlightTaskError::NoError) { - 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.task->applyCommandParameters(command)) { - cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED; - - // if we just switched and parameters are not accepted, go to failsafe - if (switch_result >= FlightTaskError::NoError) { - switchTask(FlightTaskIndex::ManualPosition); - cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED; - } - } - } - - // send back acknowledgment - vehicle_command_ack_s command_ack{}; - command_ack.timestamp = hrt_absolute_time(); - command_ack.command = command.command; - command_ack.result = cmd_result; - command_ack.result_param1 = static_cast(switch_result); - command_ack.target_system = command.source_system; - command_ack.target_component = command.source_component; - - _pub_vehicle_command_ack.publish(command_ack); -} diff --git a/src/lib/flight_tasks/FlightTasks.hpp b/src/lib/flight_tasks/FlightTasks.hpp deleted file mode 100644 index 490c7b13fb..0000000000 --- a/src/lib/flight_tasks/FlightTasks.hpp +++ /dev/null @@ -1,202 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2017 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 flight_taks.h - * - * Library class to hold and manage all implemented flight task instances - * - * @author Matthias Grob - */ - -#pragma once - -#include "FlightTask.hpp" -#include "FlightTasks_generated.hpp" -#include -#include -#include -#include -#include - -#include - -enum class FlightTaskError : int { - NoError = 0, - InvalidTask = -1, - ActivationFailed = -2 -}; - -class FlightTasks -{ -public: - FlightTasks(); - - ~FlightTasks() - { - if (_current_task.task) { - _current_task.task->~FlightTask(); - } - } - - /** - * Call regularly in the control loop cycle to execute the task - * @return true on success, false on error - */ - bool update(); - - /** - * Get the output data from the current task - * @return output setpoint, to be executed by position control - */ - const vehicle_local_position_setpoint_s getPositionSetpoint(); - - /** - * Get the local frame and attitude reset counters of the estimator from the current task - * @return the reset counters - */ - const ekf_reset_counters_s getResetCounters(); - - /** - * Get task dependent constraints - * @return setpoint constraints that has to be respected by the position controller - */ - const vehicle_constraints_s getConstraints(); - - /** - * Get landing gear position. - * @return landing gear - */ - const landing_gear_s getGear(); - - /** - * Get task avoidance desired waypoints - * @return auto triplets in the mc_pos_control - */ - const vehicle_trajectory_waypoint_s getAvoidanceWaypoint(); - - /** - * Get empty avoidance desired waypoints - * @return empty triplets in the mc_pos_control - */ - const vehicle_trajectory_waypoint_s &getEmptyAvoidanceWaypoint(); - - /** - * Switch to the next task in the available list (for testing) - * @return 0 on success, <0 on error - */ - FlightTaskError switchTask() { return switchTask(static_cast(_current_task.index) + 1); } - - /** - * Switch to a specific task (for normal usage) - * @param task index to switch to - * @return 0 on success, <0 on error - */ - FlightTaskError switchTask(FlightTaskIndex new_task_index); - FlightTaskError switchTask(int new_task_index); - - /** - * Get the number of the active task - * @return number of active task, -1 if there is none - */ - int getActiveTask() const { return static_cast(_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.task; } - - /** - * Call this whenever a parameter update notification is received (parameter_update uORB message) - */ - void handleParameterUpdate(); - - /** - * Call this method to get the description of a task error. - */ - const char *errorToString(const FlightTaskError error); - - /** - * Sets an external yaw handler. The active flight task can use the yaw handler to implement a different yaw control strategy. - */ - void setYawHandler(WeatherVane *ext_yaw_handler) {_current_task.task->setYawHandler(ext_yaw_handler);} - - /** - * This method will re-activate current task. - */ - void reActivate(); - - void updateVelocityControllerIO(const matrix::Vector3f &vel_sp, const matrix::Vector3f &thrust_sp) {_current_task.task->updateVelocityControllerIO(vel_sp, thrust_sp); } - -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. - */ - TaskUnion _task_union; /**< storage for the currently active task */ - - struct flight_task_t { - FlightTask *task; - FlightTaskIndex index; - }; - flight_task_t _current_task = {nullptr, FlightTaskIndex::None}; - - struct task_error_t { - int error; - const char *msg; - }; - - static constexpr int _numError = 3; - /** - * Map from Error int to user friendly string. - */ - task_error_t _taskError[_numError] = { - {static_cast(FlightTaskError::NoError), "No Error"}, - {static_cast(FlightTaskError::InvalidTask), "Invalid Task "}, - {static_cast(FlightTaskError::ActivationFailed), "Activation Failed"} - }; - - /** - * Check for vehicle commands (received via MAVLink), evaluate and acknowledge them - */ - void _updateCommand(); - FlightTaskIndex switchVehicleCommand(const int command); - - uORB::Subscription _sub_vehicle_command{ORB_ID(vehicle_command)}; /**< topic handle on which commands are received */ - - uORB::Publication _pub_vehicle_command_ack{ORB_ID(vehicle_command_ack)}; - - int _initTask(FlightTaskIndex task_index); -}; diff --git a/src/modules/flight_mode_manager/CMakeLists.txt b/src/modules/flight_mode_manager/CMakeLists.txt index c4c071f428..991f564be1 100644 --- a/src/modules/flight_mode_manager/CMakeLists.txt +++ b/src/modules/flight_mode_manager/CMakeLists.txt @@ -31,17 +31,105 @@ # ############################################################################ + +########################################### +# Prepare flight tasks +########################################### + +# add upstream flight tasks (they are being handled differently from the core inside the python script) +list(APPEND flight_tasks_to_add + Orbit +) +# remove possible duplicates +list(REMOVE_DUPLICATES flight_tasks_to_add) + +# remove flight tasks depending on target +if(flight_tasks_to_remove) + list(REMOVE_ITEM flight_tasks_to_add + ${flight_tasks_to_remove} + ) +endif() + +# add core flight tasks to list +list(APPEND flight_tasks_all + ManualAltitude + ManualAltitudeSmoothVel + ManualPosition + ManualPositionSmoothVel + AutoLineSmoothVel + AutoFollowMe + Offboard + Failsafe + Descend + Transition + ManualAcceleration + ${flight_tasks_to_add} +) + +# set the files to be generated +set(files_to_generate + FlightTasks_generated.hpp + FlightTasks_generated.cpp +) + +# generate files needed for Flight Tasks +set(python_args + -t ${flight_tasks_all} + -i ${CMAKE_CURRENT_SOURCE_DIR}/Templates + -o ${CMAKE_CURRENT_BINARY_DIR} + -f ${files_to_generate} +) + +# add the additional tasks for the python script (if there are any) +if(flight_tasks_to_add) + list(APPEND python_args + -s ${flight_tasks_to_add} + ) +endif() + +# generate the files using the python script and template +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 ${python_args} + COMMENT "Generating Flight Tasks" + DEPENDS + Templates/FlightTasks_generated.cpp.em + Templates/FlightTasks_generated.hpp.em + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + VERBATIM +) + +add_compile_options( + -Wno-cast-align +) # TODO: fix and enable + +# add subdirectory containing all tasks +add_subdirectory(tasks) + add_subdirectory(Takeoff) px4_add_module( MODULE modules__flight_mode_manager MAIN flight_mode_manager COMPILE_FLAGS + INCLUDES + ${CMAKE_CURRENT_SOURCE_DIR} + ${CMAKE_CURRENT_BINARY_DIR} SRCS FlightModeManager.cpp FlightModeManager.hpp + + ${CMAKE_CURRENT_BINARY_DIR}/FlightTasks_generated.hpp + ${CMAKE_CURRENT_BINARY_DIR}/FlightTasks_generated.cpp DEPENDS - FlightTasks Takeoff px4_work_queue - ) + WeatherVane +) + +# add all flight task dependencies +foreach(task ${flight_tasks_all}) + target_link_libraries(modules__flight_mode_manager PUBLIC FlightTask${task}) +endforeach() diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp index 43380f350f..510a1877b5 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.cpp +++ b/src/modules/flight_mode_manager/FlightModeManager.cpp @@ -38,21 +38,28 @@ using namespace time_literals; -FlightModeManager::FlightModeManager(bool vtol) : +FlightModeManager::FlightModeManager() : ModuleParams(nullptr), - WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), - _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) + WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers) { - if (vtol) { - // if vehicle is a VTOL we want to enable weathervane capabilities - _wv_controller = new WeatherVane(); + updateParams(); + + // initialize all flight-tasks + // currently this is required to get all parameters read + for (int i = 0; i < static_cast(FlightTaskIndex::Count); i++) { + _initTask(static_cast(i)); } - updateParams(); + // disable all tasks + _initTask(FlightTaskIndex::None); } FlightModeManager::~FlightModeManager() { + if (_current_task.task) { + _current_task.task->~FlightTask(); + } + delete _wv_controller; perf_free(_loop_perf); } @@ -100,8 +107,13 @@ void FlightModeManager::Run() _home_position_sub.update(); _vehicle_control_mode_sub.update(); _vehicle_land_detected_sub.update(); - _vehicle_local_position_setpoint_sub.update(); - _vehicle_status_sub.update(); + + if (_vehicle_status_sub.update()) { + if (_vehicle_status_sub.get().is_vtol && (_wv_controller == nullptr)) { + // if vehicle is a VTOL we want to enable weathervane capabilities + _wv_controller = new WeatherVane(); + } + } // an update is necessary here because otherwise the takeoff state doesn't get skiped with non-altitude-controlled modes _takeoff.updateTakeoffState(_vehicle_control_mode_sub.get().flag_armed, _vehicle_land_detected_sub.get().landed, false, @@ -129,7 +141,7 @@ void FlightModeManager::Run() start_flight_task(); - if (_flight_tasks.isAnyTaskActive()) { + if (isAnyTaskActive()) { generateTrajectorySetpoint(dt, vehicle_local_position); } @@ -141,7 +153,10 @@ void FlightModeManager::Run() void FlightModeManager::updateParams() { ModuleParams::updateParams(); - _flight_tasks.handleParameterUpdate(); + + if (isAnyTaskActive()) { + _current_task.task->handleParameterUpdate(); + } _takeoff.setSpoolupTime(_param_mpc_spoolup_time.get()); _takeoff.setTakeoffRampTime(_param_mpc_tko_ramp_t.get()); @@ -160,7 +175,7 @@ void FlightModeManager::start_flight_task() // Do not run any flight task for VTOLs in fixed-wing mode if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { - _flight_tasks.switchTask(FlightTaskIndex::None); + switchTask(FlightTaskIndex::None); return; } @@ -168,18 +183,18 @@ void FlightModeManager::start_flight_task() // exclude Orbit mode since the task is initiated in FlightTasks through the vehicle_command and we should not switch out if (_last_vehicle_nav_state != _vehicle_status_sub.get().nav_state && _vehicle_status_sub.get().nav_state != vehicle_status_s::NAVIGATION_STATE_ORBIT) { - _flight_tasks.switchTask(FlightTaskIndex::None); + switchTask(FlightTaskIndex::None); } // Only run transition flight task if altitude control is enabled (e.g. in Altitdue, Position, Auto flight mode) if (_vehicle_status_sub.get().in_transition_mode && _vehicle_control_mode_sub.get().flag_control_altitude_enabled) { should_disable_task = false; - FlightTaskError error = _flight_tasks.switchTask(FlightTaskIndex::Transition); + FlightTaskError error = switchTask(FlightTaskIndex::Transition); if (error != FlightTaskError::NoError) { if (prev_failure_count == 0) { - PX4_WARN("Transition activation failed with error: %s", _flight_tasks.errorToString(error)); + PX4_WARN("Transition activation failed with error: %s", errorToString(error)); } task_failure = true; @@ -202,11 +217,11 @@ void FlightModeManager::start_flight_task() _vehicle_control_mode_sub.get().flag_control_acceleration_enabled)) { should_disable_task = false; - FlightTaskError error = _flight_tasks.switchTask(FlightTaskIndex::Offboard); + FlightTaskError error = switchTask(FlightTaskIndex::Offboard); if (error != FlightTaskError::NoError) { if (prev_failure_count == 0) { - PX4_WARN("Offboard activation failed with error: %s", _flight_tasks.errorToString(error)); + PX4_WARN("Offboard activation failed with error: %s", errorToString(error)); } task_failure = true; @@ -221,11 +236,11 @@ void FlightModeManager::start_flight_task() // Auto-follow me if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET) { should_disable_task = false; - FlightTaskError error = _flight_tasks.switchTask(FlightTaskIndex::AutoFollowMe); + FlightTaskError error = switchTask(FlightTaskIndex::AutoFollowMe); if (error != FlightTaskError::NoError) { if (prev_failure_count == 0) { - PX4_WARN("Follow-Me activation failed with error: %s", _flight_tasks.errorToString(error)); + PX4_WARN("Follow-Me activation failed with error: %s", errorToString(error)); } task_failure = true; @@ -241,11 +256,11 @@ void FlightModeManager::start_flight_task() should_disable_task = false; FlightTaskError error = FlightTaskError::NoError; - error = _flight_tasks.switchTask(FlightTaskIndex::AutoLineSmoothVel); + error = switchTask(FlightTaskIndex::AutoLineSmoothVel); if (error != FlightTaskError::NoError) { if (prev_failure_count == 0) { - PX4_WARN("Auto activation failed with error: %s", _flight_tasks.errorToString(error)); + PX4_WARN("Auto activation failed with error: %s", errorToString(error)); } task_failure = true; @@ -262,11 +277,11 @@ void FlightModeManager::start_flight_task() should_disable_task = false; FlightTaskError error = FlightTaskError::NoError; - error = _flight_tasks.switchTask(FlightTaskIndex::Descend); + error = switchTask(FlightTaskIndex::Descend); if (error != FlightTaskError::NoError) { if (prev_failure_count == 0) { - PX4_WARN("Descend activation failed with error: %s", _flight_tasks.errorToString(error)); + PX4_WARN("Descend activation failed with error: %s", errorToString(error)); } task_failure = true; @@ -286,22 +301,22 @@ void FlightModeManager::start_flight_task() switch (_param_mpc_pos_mode.get()) { case 0: - error = _flight_tasks.switchTask(FlightTaskIndex::ManualPosition); + error = switchTask(FlightTaskIndex::ManualPosition); break; case 3: - error = _flight_tasks.switchTask(FlightTaskIndex::ManualPositionSmoothVel); + error = switchTask(FlightTaskIndex::ManualPositionSmoothVel); break; case 4: default: - error = _flight_tasks.switchTask(FlightTaskIndex::ManualAcceleration); + error = switchTask(FlightTaskIndex::ManualAcceleration); break; } if (error != FlightTaskError::NoError) { if (prev_failure_count == 0) { - PX4_WARN("Position-Ctrl activation failed with error: %s", _flight_tasks.errorToString(error)); + PX4_WARN("Position-Ctrl activation failed with error: %s", errorToString(error)); } task_failure = true; @@ -320,18 +335,18 @@ void FlightModeManager::start_flight_task() switch (_param_mpc_pos_mode.get()) { case 0: - error = _flight_tasks.switchTask(FlightTaskIndex::ManualAltitude); + error = switchTask(FlightTaskIndex::ManualAltitude); break; case 3: default: - error = _flight_tasks.switchTask(FlightTaskIndex::ManualAltitudeSmoothVel); + error = switchTask(FlightTaskIndex::ManualAltitudeSmoothVel); break; } if (error != FlightTaskError::NoError) { if (prev_failure_count == 0) { - PX4_WARN("Altitude-Ctrl activation failed with error: %s", _flight_tasks.errorToString(error)); + PX4_WARN("Altitude-Ctrl activation failed with error: %s", errorToString(error)); } task_failure = true; @@ -352,15 +367,15 @@ void FlightModeManager::start_flight_task() // for some reason no flighttask was able to start. // go into failsafe flighttask - FlightTaskError error = _flight_tasks.switchTask(FlightTaskIndex::Failsafe); + FlightTaskError error = switchTask(FlightTaskIndex::Failsafe); if (error != FlightTaskError::NoError) { // No task was activated. - _flight_tasks.switchTask(FlightTaskIndex::None); + switchTask(FlightTaskIndex::None); } } else if (should_disable_task) { - _flight_tasks.switchTask(FlightTaskIndex::None); + switchTask(FlightTaskIndex::None); } _last_vehicle_nav_state = _vehicle_status_sub.get().nav_state; @@ -383,7 +398,6 @@ void FlightModeManager::check_failure(bool task_failure, uint8_t nav_state) void FlightModeManager::send_vehicle_cmd_do(uint8_t nav_state) { vehicle_command_s command{}; - command.timestamp = hrt_absolute_time(); command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE; command.param1 = (float)1; // base mode command.param3 = (float)0; // sub mode @@ -415,29 +429,78 @@ void FlightModeManager::send_vehicle_cmd_do(uint8_t nav_state) } // publish the vehicle command + command.timestamp = hrt_absolute_time(); _vehicle_command_pub.publish(command); } void FlightModeManager::generateTrajectorySetpoint(const float dt, const vehicle_local_position_s &vehicle_local_position) { + // In case flight task was not able to update correctly we send the empty setpoint which makes the position controller failsafe. + if (_vehicle_command_sub.updated()) { + // get command + vehicle_command_s command{}; + _vehicle_command_sub.copy(&command); + + // check what command it is + FlightTaskIndex desired_task = switchVehicleCommand(command.command); + + // ignore all unkown commands + if (desired_task != FlightTaskIndex::None) { + // switch to the commanded task + FlightTaskError switch_result = switchTask(desired_task); + uint8_t cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED; + + // if we are in/switched to the desired task + if (switch_result >= FlightTaskError::NoError) { + 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.task->applyCommandParameters(command)) { + cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED; + + // if we just switched and parameters are not accepted, go to failsafe + if (switch_result >= FlightTaskError::NoError) { + switchTask(FlightTaskIndex::ManualPosition); + cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED; + } + } + } + + // send back acknowledgment + vehicle_command_ack_s command_ack{}; + command_ack.command = command.command; + command_ack.result = cmd_result; + command_ack.result_param1 = static_cast(switch_result); + command_ack.target_system = command.source_system; + command_ack.target_component = command.source_component; + command_ack.timestamp = hrt_absolute_time(); + _vehicle_command_ack_pub.publish(command_ack); + } + } + + + _current_task.task->setYawHandler(_wv_controller); + // Inform FlightTask about the input and output of the velocity controller // This is used to properly initialize the velocity setpoint when onpening the position loop (position unlock) - _flight_tasks.updateVelocityControllerIO(Vector3f(_vehicle_local_position_setpoint_sub.get().vx, - _vehicle_local_position_setpoint_sub.get().vy, _vehicle_local_position_setpoint_sub.get().vz), - Vector3f(_vehicle_local_position_setpoint_sub.get().acceleration)); + if (_vehicle_local_position_setpoint_sub.updated()) { + vehicle_local_position_setpoint_s vehicle_local_position_setpoint; + + if (_vehicle_local_position_setpoint_sub.copy(&vehicle_local_position_setpoint)) { + const Vector3f vel_sp{vehicle_local_position_setpoint.vx, vehicle_local_position_setpoint.vy, vehicle_local_position_setpoint.vz}; + const Vector3f thrust_sp{vehicle_local_position_setpoint.acceleration}; + _current_task.task->updateVelocityControllerIO(vel_sp, thrust_sp); + } + } + + if (_current_task.task->updateInitialize() && _current_task.task->update() && _current_task.task->updateFinalize()) { + // updated + } // setpoints and constraints for the position controller from flighttask or failsafe - vehicle_local_position_setpoint_s setpoint = FlightTask::empty_setpoint; - vehicle_constraints_s constraints = FlightTask::empty_constraints; - - _flight_tasks.setYawHandler(_wv_controller); - - // In case flight task was not able to update correctly we send the empty setpoint which makes the position controller failsafe. - if (_flight_tasks.update()) { - setpoint = _flight_tasks.getPositionSetpoint(); - constraints = _flight_tasks.getConstraints(); - } + vehicle_local_position_setpoint_s setpoint = _current_task.task->getPositionSetpoint(); + vehicle_constraints_s constraints = _current_task.task->getConstraints(); // limit altitude according to land detector limitAltitude(setpoint, vehicle_local_position); @@ -456,8 +519,16 @@ void FlightModeManager::generateTrajectorySetpoint(const float dt, constraints.reset_integral = true; } + if (not_taken_off) { + // reactivate the task which will reset the setpoint to current state + _current_task.task->reActivate(); + } + + + setpoint.timestamp = hrt_absolute_time(); _trajectory_setpoint_pub.publish(setpoint); + // Allow ramping from zero thrust on takeoff if (flying) { constraints.minimum_thrust = _param_mpc_thr_min.get(); @@ -484,21 +555,18 @@ void FlightModeManager::generateTrajectorySetpoint(const float dt, _time_stamp_last_loop); constraints.speed_up = _takeoff.updateRamp(dt, constraints.speed_up); - constraints.flight_task = _flight_tasks.getActiveTask(); + constraints.flight_task = static_cast(_current_task.index); + constraints.timestamp = hrt_absolute_time(); _vehicle_constraints_pub.publish(constraints); - if (not_taken_off) { - // reactivate the task which will reset the setpoint to current state - _flight_tasks.reActivate(); - } // if there's any change in landing gear setpoint publish it - landing_gear_s landing_gear = _flight_tasks.getGear(); + landing_gear_s landing_gear = _current_task.task->getGear(); if (landing_gear.landing_gear != _old_landing_gear_position && landing_gear.landing_gear != landing_gear_s::GEAR_KEEP) { - landing_gear.timestamp = _time_stamp_last_loop; + landing_gear.timestamp = hrt_absolute_time(); _landing_gear_pub.publish(landing_gear); } @@ -533,17 +601,73 @@ void FlightModeManager::reset_setpoint_to_nan(vehicle_local_position_setpoint_s setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = NAN; } -int FlightModeManager::task_spawn(int argc, char *argv[]) +FlightTaskError FlightModeManager::switchTask(FlightTaskIndex new_task_index) { - bool vtol = false; - - if (argc > 1) { - if (strcmp(argv[1], "vtol") == 0) { - vtol = true; - } + // switch to the running task, nothing to do + if (new_task_index == _current_task.index) { + return FlightTaskError::NoError; } - FlightModeManager *instance = new FlightModeManager(vtol); + // Save current setpoints for the next FlightTask + vehicle_local_position_setpoint_s last_setpoint{}; + ekf_reset_counters_s last_reset_counters = FlightTask::zero_reset_counters; + + if (isAnyTaskActive()) { + last_setpoint = _current_task.task->getPositionSetpoint(); + last_reset_counters = _current_task.task->getResetCounters(); + } + + if (_initTask(new_task_index)) { + // invalid task + return FlightTaskError::InvalidTask; + } + + if (!isAnyTaskActive()) { + // no task running + return FlightTaskError::NoError; + } + + // activation failed + if (!_current_task.task->updateInitialize() || !_current_task.task->activate(last_setpoint)) { + _current_task.task->~FlightTask(); + _current_task.task = nullptr; + _current_task.index = FlightTaskIndex::None; + return FlightTaskError::ActivationFailed; + } + + _current_task.task->setResetCounters(last_reset_counters); + + return FlightTaskError::NoError; +} + +FlightTaskError FlightModeManager::switchTask(int new_task_index) +{ + // make sure we are in range of the enumeration before casting + if (static_cast(FlightTaskIndex::None) <= new_task_index && + static_cast(FlightTaskIndex::Count) > new_task_index) { + return switchTask(FlightTaskIndex(new_task_index)); + } + + switchTask(FlightTaskIndex::None); + return FlightTaskError::InvalidTask; +} + +const char *FlightModeManager::errorToString(const FlightTaskError error) +{ + switch (error) { + case FlightTaskError::NoError: return "No Error"; + + case FlightTaskError::InvalidTask: return "Invalid Task "; + + case FlightTaskError::ActivationFailed: return "Activation Failed"; + } + + return "This error is not mapped to a string or is unknown."; +} + +int FlightModeManager::task_spawn(int argc, char *argv[]) +{ + FlightModeManager *instance = new FlightModeManager(); if (instance) { _object.store(instance); @@ -571,8 +695,8 @@ int FlightModeManager::custom_command(int argc, char *argv[]) int FlightModeManager::print_status() { - if (_flight_tasks.isAnyTaskActive()) { - PX4_INFO("Running, active flight task: %i", _flight_tasks.getActiveTask()); + if (isAnyTaskActive()) { + PX4_INFO("Running, active flight task: %i", static_cast(_current_task.index)); } else { PX4_INFO("Running, no flight task active"); @@ -598,7 +722,6 @@ and outputs setpoints for controllers. PRINT_MODULE_USAGE_NAME("flight_mode_manager", "controller"); PRINT_MODULE_USAGE_COMMAND("start"); - PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; diff --git a/src/modules/flight_mode_manager/FlightModeManager.hpp b/src/modules/flight_mode_manager/FlightModeManager.hpp index 2af397363d..17c640756f 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.hpp +++ b/src/modules/flight_mode_manager/FlightModeManager.hpp @@ -33,17 +33,21 @@ #pragma once +#include "FlightTask.hpp" +#include "FlightTasks_generated.hpp" + #include #include #include #include -#include #include #include #include #include #include +#include +#include #include #include #include @@ -52,10 +56,18 @@ #include "Takeoff/Takeoff.hpp" +#include + +enum class FlightTaskError : int { + NoError = 0, + InvalidTask = -1, + ActivationFailed = -2 +}; + class FlightModeManager : public ModuleBase, public ModuleParams, public px4::WorkItem { public: - FlightModeManager(bool vtol = false); + FlightModeManager(); ~FlightModeManager() override; /** @see ModuleBase */ @@ -82,31 +94,67 @@ private: void limitAltitude(vehicle_local_position_setpoint_s &setpoint, const vehicle_local_position_s &vehicle_local_position); void reset_setpoint_to_nan(vehicle_local_position_setpoint_s &setpoint); + /** + * Switch to a specific task (for normal usage) + * @param task index to switch to + * @return 0 on success, <0 on error + */ + FlightTaskError switchTask(FlightTaskIndex new_task_index); + FlightTaskError switchTask(int new_task_index); + + /** + * Call this method to get the description of a task error. + */ + const char *errorToString(const FlightTaskError error); + + /** + * Check if any task is active + * @return true if a task is active, false if not + */ + bool isAnyTaskActive() const { return _current_task.task; } + + // generated + int _initTask(FlightTaskIndex task_index); + FlightTaskIndex switchVehicleCommand(const int command); + static constexpr int NUM_FAILURE_TRIES = 10; ///< number of tries before switching to a failsafe flight task - FlightTasks _flight_tasks; ///< class generating position control setpoints depending on vehicle task + /** + * 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. + */ + TaskUnion _task_union; /**< storage for the currently active task */ + + struct flight_task_t { + FlightTask *task{nullptr}; + FlightTaskIndex index{FlightTaskIndex::None}; + } _current_task{}; + Takeoff _takeoff; ///< state machine and ramp to bring the vehicle off the ground without a jump WeatherVane *_wv_controller{nullptr}; int8_t _old_landing_gear_position{landing_gear_s::GEAR_KEEP}; int _task_failure_count{0}; uint8_t _last_vehicle_nav_state{0}; - perf_counter_t _loop_perf; ///< loop duration performance counter + perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; ///< loop duration performance counter hrt_abstime _time_stamp_last_loop{0}; ///< time stamp of last loop iteration uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - uORB::SubscriptionData _home_position_sub{ORB_ID(home_position)}; uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)}; + uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; + uORB::Subscription _vehicle_local_position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint)}; + uORB::SubscriptionData _home_position_sub{ORB_ID(home_position)}; uORB::SubscriptionData _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; uORB::SubscriptionData _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; uORB::SubscriptionCallbackWorkItem _vehicle_local_position_sub{this, ORB_ID(vehicle_local_position)}; - uORB::SubscriptionData _vehicle_local_position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint)}; + uORB::SubscriptionData _vehicle_status_sub{ORB_ID(vehicle_status)}; uORB::Publication _landing_gear_pub{ORB_ID(landing_gear)}; uORB::Publication _trajectory_setpoint_pub{ORB_ID(trajectory_setpoint)}; uORB::Publication _vehicle_command_pub{ORB_ID(vehicle_command)}; + uORB::Publication _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)}; uORB::Publication _vehicle_constraints_pub{ORB_ID(vehicle_constraints)}; DEFINE_PARAMETERS( diff --git a/src/lib/flight_tasks/Templates/FlightTasks_generated.cpp.em b/src/modules/flight_mode_manager/Templates/FlightTasks_generated.cpp.em similarity index 94% rename from src/lib/flight_tasks/Templates/FlightTasks_generated.cpp.em rename to src/modules/flight_mode_manager/Templates/FlightTasks_generated.cpp.em index 99917f41da..8ae9ed4bff 100644 --- a/src/lib/flight_tasks/Templates/FlightTasks_generated.cpp.em +++ b/src/modules/flight_mode_manager/Templates/FlightTasks_generated.cpp.em @@ -39,10 +39,10 @@ * @@author Christoph Tobler */ -#include "FlightTasks.hpp" +#include "FlightModeManager.hpp" #include "FlightTasks_generated.hpp" -int FlightTasks::_initTask(FlightTaskIndex task_index) +int FlightModeManager::_initTask(FlightTaskIndex task_index) { // disable the old task if there is any @@ -79,7 +79,7 @@ firstLowercase = lambda s: s[:1].lower() + s[1:] if s else '' return 0; } -FlightTaskIndex FlightTasks::switchVehicleCommand(const int command) +FlightTaskIndex FlightModeManager::switchVehicleCommand(const int command) { switch (command) { @# loop through all additional tasks @@ -97,4 +97,4 @@ upperCase = lambda s: s[:].upper() if s else '' // ignore all unkown commands default : return FlightTaskIndex::None; } -} \ No newline at end of file +} diff --git a/src/lib/flight_tasks/Templates/FlightTasks_generated.hpp.em b/src/modules/flight_mode_manager/Templates/FlightTasks_generated.hpp.em similarity index 98% rename from src/lib/flight_tasks/Templates/FlightTasks_generated.hpp.em rename to src/modules/flight_mode_manager/Templates/FlightTasks_generated.hpp.em index 724f2a762c..6471d1e397 100644 --- a/src/lib/flight_tasks/Templates/FlightTasks_generated.hpp.em +++ b/src/modules/flight_mode_manager/Templates/FlightTasks_generated.hpp.em @@ -42,7 +42,7 @@ #pragma once // include all required headers -#include "FlightTasks.hpp" +#include "FlightModeManager.hpp" @# loop through all requested tasks @[if tasks]@ @[for task in tasks]@ diff --git a/src/lib/flight_tasks/generate_flight_tasks.py b/src/modules/flight_mode_manager/generate_flight_tasks.py similarity index 100% rename from src/lib/flight_tasks/generate_flight_tasks.py rename to src/modules/flight_mode_manager/generate_flight_tasks.py diff --git a/src/lib/flight_tasks/tasks/Auto/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/Auto/CMakeLists.txt similarity index 100% rename from src/lib/flight_tasks/tasks/Auto/CMakeLists.txt rename to src/modules/flight_mode_manager/tasks/Auto/CMakeLists.txt diff --git a/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp similarity index 100% rename from src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp rename to src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp diff --git a/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.hpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp similarity index 100% rename from src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.hpp rename to src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp diff --git a/src/lib/flight_tasks/tasks/AutoFollowMe/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/AutoFollowMe/CMakeLists.txt similarity index 100% rename from src/lib/flight_tasks/tasks/AutoFollowMe/CMakeLists.txt rename to src/modules/flight_mode_manager/tasks/AutoFollowMe/CMakeLists.txt diff --git a/src/lib/flight_tasks/tasks/AutoFollowMe/FlightTaskAutoFollowMe.cpp b/src/modules/flight_mode_manager/tasks/AutoFollowMe/FlightTaskAutoFollowMe.cpp similarity index 100% rename from src/lib/flight_tasks/tasks/AutoFollowMe/FlightTaskAutoFollowMe.cpp rename to src/modules/flight_mode_manager/tasks/AutoFollowMe/FlightTaskAutoFollowMe.cpp diff --git a/src/lib/flight_tasks/tasks/AutoFollowMe/FlightTaskAutoFollowMe.hpp b/src/modules/flight_mode_manager/tasks/AutoFollowMe/FlightTaskAutoFollowMe.hpp similarity index 100% rename from src/lib/flight_tasks/tasks/AutoFollowMe/FlightTaskAutoFollowMe.hpp rename to src/modules/flight_mode_manager/tasks/AutoFollowMe/FlightTaskAutoFollowMe.hpp diff --git a/src/lib/flight_tasks/tasks/AutoLineSmoothVel/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/CMakeLists.txt similarity index 100% rename from src/lib/flight_tasks/tasks/AutoLineSmoothVel/CMakeLists.txt rename to src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/CMakeLists.txt diff --git a/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp b/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp similarity index 100% rename from src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp rename to src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp diff --git a/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp b/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp similarity index 100% rename from src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp rename to src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp diff --git a/src/lib/flight_tasks/tasks/AutoLineSmoothVel/TrajectoryConstraints.hpp b/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/TrajectoryConstraints.hpp similarity index 100% rename from src/lib/flight_tasks/tasks/AutoLineSmoothVel/TrajectoryConstraints.hpp rename to src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/TrajectoryConstraints.hpp diff --git a/src/lib/flight_tasks/tasks/AutoLineSmoothVel/TrajectoryConstraintsTest.cpp b/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/TrajectoryConstraintsTest.cpp similarity index 99% rename from src/lib/flight_tasks/tasks/AutoLineSmoothVel/TrajectoryConstraintsTest.cpp rename to src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/TrajectoryConstraintsTest.cpp index ad0e7c0969..f59302ddf2 100644 --- a/src/lib/flight_tasks/tasks/AutoLineSmoothVel/TrajectoryConstraintsTest.cpp +++ b/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/TrajectoryConstraintsTest.cpp @@ -1,5 +1,6 @@ #include -#include + +#include "TrajectoryConstraints.hpp" using namespace matrix; using namespace math::trajectory; diff --git a/src/lib/flight_tasks/tasks/AutoMapper/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/AutoMapper/CMakeLists.txt similarity index 100% rename from src/lib/flight_tasks/tasks/AutoMapper/CMakeLists.txt rename to src/modules/flight_mode_manager/tasks/AutoMapper/CMakeLists.txt diff --git a/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp b/src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.cpp similarity index 100% rename from src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp rename to src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.cpp diff --git a/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.hpp b/src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.hpp similarity index 100% rename from src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.hpp rename to src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.hpp diff --git a/src/lib/flight_tasks/tasks/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/CMakeLists.txt similarity index 100% rename from src/lib/flight_tasks/tasks/CMakeLists.txt rename to src/modules/flight_mode_manager/tasks/CMakeLists.txt diff --git a/src/lib/flight_tasks/tasks/Descend/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/Descend/CMakeLists.txt similarity index 100% rename from src/lib/flight_tasks/tasks/Descend/CMakeLists.txt rename to src/modules/flight_mode_manager/tasks/Descend/CMakeLists.txt diff --git a/src/lib/flight_tasks/tasks/Descend/FlightTaskDescend.cpp b/src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.cpp similarity index 100% rename from src/lib/flight_tasks/tasks/Descend/FlightTaskDescend.cpp rename to src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.cpp diff --git a/src/lib/flight_tasks/tasks/Descend/FlightTaskDescend.hpp b/src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.hpp similarity index 100% rename from src/lib/flight_tasks/tasks/Descend/FlightTaskDescend.hpp rename to src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.hpp diff --git a/src/lib/flight_tasks/tasks/Failsafe/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/Failsafe/CMakeLists.txt similarity index 100% rename from src/lib/flight_tasks/tasks/Failsafe/CMakeLists.txt rename to src/modules/flight_mode_manager/tasks/Failsafe/CMakeLists.txt diff --git a/src/lib/flight_tasks/tasks/Failsafe/FlightTaskFailsafe.cpp b/src/modules/flight_mode_manager/tasks/Failsafe/FlightTaskFailsafe.cpp similarity index 100% rename from src/lib/flight_tasks/tasks/Failsafe/FlightTaskFailsafe.cpp rename to src/modules/flight_mode_manager/tasks/Failsafe/FlightTaskFailsafe.cpp diff --git a/src/lib/flight_tasks/tasks/Failsafe/FlightTaskFailsafe.hpp b/src/modules/flight_mode_manager/tasks/Failsafe/FlightTaskFailsafe.hpp similarity index 100% rename from src/lib/flight_tasks/tasks/Failsafe/FlightTaskFailsafe.hpp rename to src/modules/flight_mode_manager/tasks/Failsafe/FlightTaskFailsafe.hpp diff --git a/src/lib/flight_tasks/tasks/FlightTask/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/FlightTask/CMakeLists.txt similarity index 100% rename from src/lib/flight_tasks/tasks/FlightTask/CMakeLists.txt rename to src/modules/flight_mode_manager/tasks/FlightTask/CMakeLists.txt diff --git a/src/lib/flight_tasks/tasks/FlightTask/FlightTask.cpp b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp similarity index 100% rename from src/lib/flight_tasks/tasks/FlightTask/FlightTask.cpp rename to src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp diff --git a/src/lib/flight_tasks/tasks/FlightTask/FlightTask.hpp b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp similarity index 100% rename from src/lib/flight_tasks/tasks/FlightTask/FlightTask.hpp rename to src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp diff --git a/src/lib/flight_tasks/tasks/ManualAcceleration/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/ManualAcceleration/CMakeLists.txt similarity index 100% rename from src/lib/flight_tasks/tasks/ManualAcceleration/CMakeLists.txt rename to src/modules/flight_mode_manager/tasks/ManualAcceleration/CMakeLists.txt diff --git a/src/lib/flight_tasks/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp similarity index 100% rename from src/lib/flight_tasks/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp rename to src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp diff --git a/src/lib/flight_tasks/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp similarity index 100% rename from src/lib/flight_tasks/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp rename to src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp diff --git a/src/lib/flight_tasks/tasks/ManualAltitude/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/ManualAltitude/CMakeLists.txt similarity index 100% rename from src/lib/flight_tasks/tasks/ManualAltitude/CMakeLists.txt rename to src/modules/flight_mode_manager/tasks/ManualAltitude/CMakeLists.txt diff --git a/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp similarity index 100% rename from src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp rename to src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp diff --git a/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp similarity index 100% rename from src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp rename to src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp diff --git a/src/lib/flight_tasks/tasks/ManualAltitudeSmoothVel/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/CMakeLists.txt similarity index 100% rename from src/lib/flight_tasks/tasks/ManualAltitudeSmoothVel/CMakeLists.txt rename to src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/CMakeLists.txt diff --git a/src/lib/flight_tasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp b/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp similarity index 100% rename from src/lib/flight_tasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp rename to src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp diff --git a/src/lib/flight_tasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp b/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp similarity index 100% rename from src/lib/flight_tasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp rename to src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp diff --git a/src/lib/flight_tasks/tasks/ManualPosition/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/ManualPosition/CMakeLists.txt similarity index 92% rename from src/lib/flight_tasks/tasks/ManualPosition/CMakeLists.txt rename to src/modules/flight_mode_manager/tasks/ManualPosition/CMakeLists.txt index d45486eabd..6125d5462b 100644 --- a/src/lib/flight_tasks/tasks/ManualPosition/CMakeLists.txt +++ b/src/modules/flight_mode_manager/tasks/ManualPosition/CMakeLists.txt @@ -33,7 +33,14 @@ px4_add_library(FlightTaskManualPosition FlightTaskManualPosition.cpp + FlightTaskManualPosition.hpp +) + +target_link_libraries(FlightTaskManualPosition + PRIVATE + CollisionPrevention + PUBLIC + FlightTaskManualAltitude ) -target_link_libraries(FlightTaskManualPosition PUBLIC FlightTaskManualAltitude) target_include_directories(FlightTaskManualPosition PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.cpp b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp similarity index 100% rename from src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.cpp rename to src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp diff --git a/src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.hpp b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp similarity index 100% rename from src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.hpp rename to src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp diff --git a/src/lib/flight_tasks/tasks/ManualPositionSmoothVel/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/CMakeLists.txt similarity index 100% rename from src/lib/flight_tasks/tasks/ManualPositionSmoothVel/CMakeLists.txt rename to src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/CMakeLists.txt diff --git a/src/lib/flight_tasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp b/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp similarity index 100% rename from src/lib/flight_tasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp rename to src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp diff --git a/src/lib/flight_tasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp b/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp similarity index 100% rename from src/lib/flight_tasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp rename to src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp diff --git a/src/lib/flight_tasks/tasks/Offboard/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/Offboard/CMakeLists.txt similarity index 100% rename from src/lib/flight_tasks/tasks/Offboard/CMakeLists.txt rename to src/modules/flight_mode_manager/tasks/Offboard/CMakeLists.txt diff --git a/src/lib/flight_tasks/tasks/Offboard/FlightTaskOffboard.cpp b/src/modules/flight_mode_manager/tasks/Offboard/FlightTaskOffboard.cpp similarity index 100% rename from src/lib/flight_tasks/tasks/Offboard/FlightTaskOffboard.cpp rename to src/modules/flight_mode_manager/tasks/Offboard/FlightTaskOffboard.cpp diff --git a/src/lib/flight_tasks/tasks/Offboard/FlightTaskOffboard.hpp b/src/modules/flight_mode_manager/tasks/Offboard/FlightTaskOffboard.hpp similarity index 100% rename from src/lib/flight_tasks/tasks/Offboard/FlightTaskOffboard.hpp rename to src/modules/flight_mode_manager/tasks/Offboard/FlightTaskOffboard.hpp diff --git a/src/lib/flight_tasks/tasks/Orbit/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/Orbit/CMakeLists.txt similarity index 100% rename from src/lib/flight_tasks/tasks/Orbit/CMakeLists.txt rename to src/modules/flight_mode_manager/tasks/Orbit/CMakeLists.txt diff --git a/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp similarity index 100% rename from src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp rename to src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp index 8de9c54022..004704d2c4 100644 --- a/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp +++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp @@ -105,7 +105,6 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command) bool FlightTaskOrbit::sendTelemetry() { orbit_status_s orbit_status{}; - orbit_status.timestamp = hrt_absolute_time(); orbit_status.radius = math::signNoZero(_v) * _r; orbit_status.frame = 0; // MAV_FRAME::MAV_FRAME_GLOBAL orbit_status.yaw_behaviour = _yaw_behaviour; @@ -115,6 +114,7 @@ bool FlightTaskOrbit::sendTelemetry() return false; // don't send the message if the transformation failed } + orbit_status.timestamp = hrt_absolute_time(); _orbit_status_pub.publish(orbit_status); return true; diff --git a/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.hpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp similarity index 100% rename from src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.hpp rename to src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp diff --git a/src/lib/flight_tasks/tasks/Transition/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/Transition/CMakeLists.txt similarity index 100% rename from src/lib/flight_tasks/tasks/Transition/CMakeLists.txt rename to src/modules/flight_mode_manager/tasks/Transition/CMakeLists.txt diff --git a/src/lib/flight_tasks/tasks/Transition/FlightTaskTransition.cpp b/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp similarity index 100% rename from src/lib/flight_tasks/tasks/Transition/FlightTaskTransition.cpp rename to src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp diff --git a/src/lib/flight_tasks/tasks/Transition/FlightTaskTransition.hpp b/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.hpp similarity index 100% rename from src/lib/flight_tasks/tasks/Transition/FlightTaskTransition.hpp rename to src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.hpp diff --git a/src/lib/flight_tasks/tasks/Utility/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt similarity index 100% rename from src/lib/flight_tasks/tasks/Utility/CMakeLists.txt rename to src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt diff --git a/src/lib/flight_tasks/tasks/Utility/Makefile b/src/modules/flight_mode_manager/tasks/Utility/Makefile similarity index 100% rename from src/lib/flight_tasks/tasks/Utility/Makefile rename to src/modules/flight_mode_manager/tasks/Utility/Makefile diff --git a/src/lib/flight_tasks/tasks/Utility/ManualVelocitySmoothingXY.cpp b/src/modules/flight_mode_manager/tasks/Utility/ManualVelocitySmoothingXY.cpp similarity index 100% rename from src/lib/flight_tasks/tasks/Utility/ManualVelocitySmoothingXY.cpp rename to src/modules/flight_mode_manager/tasks/Utility/ManualVelocitySmoothingXY.cpp diff --git a/src/lib/flight_tasks/tasks/Utility/ManualVelocitySmoothingXY.hpp b/src/modules/flight_mode_manager/tasks/Utility/ManualVelocitySmoothingXY.hpp similarity index 100% rename from src/lib/flight_tasks/tasks/Utility/ManualVelocitySmoothingXY.hpp rename to src/modules/flight_mode_manager/tasks/Utility/ManualVelocitySmoothingXY.hpp diff --git a/src/lib/flight_tasks/tasks/Utility/ManualVelocitySmoothingXYTest.cpp b/src/modules/flight_mode_manager/tasks/Utility/ManualVelocitySmoothingXYTest.cpp similarity index 100% rename from src/lib/flight_tasks/tasks/Utility/ManualVelocitySmoothingXYTest.cpp rename to src/modules/flight_mode_manager/tasks/Utility/ManualVelocitySmoothingXYTest.cpp diff --git a/src/lib/flight_tasks/tasks/Utility/ManualVelocitySmoothingZ.cpp b/src/modules/flight_mode_manager/tasks/Utility/ManualVelocitySmoothingZ.cpp similarity index 100% rename from src/lib/flight_tasks/tasks/Utility/ManualVelocitySmoothingZ.cpp rename to src/modules/flight_mode_manager/tasks/Utility/ManualVelocitySmoothingZ.cpp diff --git a/src/lib/flight_tasks/tasks/Utility/ManualVelocitySmoothingZ.hpp b/src/modules/flight_mode_manager/tasks/Utility/ManualVelocitySmoothingZ.hpp similarity index 100% rename from src/lib/flight_tasks/tasks/Utility/ManualVelocitySmoothingZ.hpp rename to src/modules/flight_mode_manager/tasks/Utility/ManualVelocitySmoothingZ.hpp diff --git a/src/lib/flight_tasks/tasks/Utility/StickAccelerationXY.cpp b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp similarity index 100% rename from src/lib/flight_tasks/tasks/Utility/StickAccelerationXY.cpp rename to src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp diff --git a/src/lib/flight_tasks/tasks/Utility/StickAccelerationXY.hpp b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp similarity index 100% rename from src/lib/flight_tasks/tasks/Utility/StickAccelerationXY.hpp rename to src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp diff --git a/src/lib/flight_tasks/tasks/Utility/StickYaw.cpp b/src/modules/flight_mode_manager/tasks/Utility/StickYaw.cpp similarity index 100% rename from src/lib/flight_tasks/tasks/Utility/StickYaw.cpp rename to src/modules/flight_mode_manager/tasks/Utility/StickYaw.cpp diff --git a/src/lib/flight_tasks/tasks/Utility/StickYaw.hpp b/src/modules/flight_mode_manager/tasks/Utility/StickYaw.hpp similarity index 100% rename from src/lib/flight_tasks/tasks/Utility/StickYaw.hpp rename to src/modules/flight_mode_manager/tasks/Utility/StickYaw.hpp diff --git a/src/lib/flight_tasks/tasks/Utility/Sticks.cpp b/src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp similarity index 100% rename from src/lib/flight_tasks/tasks/Utility/Sticks.cpp rename to src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp diff --git a/src/lib/flight_tasks/tasks/Utility/Sticks.hpp b/src/modules/flight_mode_manager/tasks/Utility/Sticks.hpp similarity index 100% rename from src/lib/flight_tasks/tasks/Utility/Sticks.hpp rename to src/modules/flight_mode_manager/tasks/Utility/Sticks.hpp diff --git a/src/lib/flight_tasks/tasks/Utility/StraightLine.cpp b/src/modules/flight_mode_manager/tasks/Utility/StraightLine.cpp similarity index 100% rename from src/lib/flight_tasks/tasks/Utility/StraightLine.cpp rename to src/modules/flight_mode_manager/tasks/Utility/StraightLine.cpp diff --git a/src/lib/flight_tasks/tasks/Utility/StraightLine.hpp b/src/modules/flight_mode_manager/tasks/Utility/StraightLine.hpp similarity index 100% rename from src/lib/flight_tasks/tasks/Utility/StraightLine.hpp rename to src/modules/flight_mode_manager/tasks/Utility/StraightLine.hpp diff --git a/src/modules/mc_pos_control/CMakeLists.txt b/src/modules/mc_pos_control/CMakeLists.txt index b18b44b9d6..fb321a26c4 100644 --- a/src/modules/mc_pos_control/CMakeLists.txt +++ b/src/modules/mc_pos_control/CMakeLists.txt @@ -37,16 +37,12 @@ px4_add_module( MODULE modules__mc_pos_control MAIN mc_pos_control COMPILE_FLAGS - -Wno-implicit-fallthrough # TODO: fix and remove SRCS MulticopterPositionControl.cpp MulticopterPositionControl.hpp DEPENDS PositionControl controllib - FlightTasks git_ecl ecl_geo - WeatherVane - CollisionPrevention ) diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index cec1b2a647..88eb66e501 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -37,7 +37,6 @@ #include #include -using namespace time_literals; using namespace matrix; MulticopterPositionControl::MulticopterPositionControl(bool vtol) : @@ -271,7 +270,7 @@ void MulticopterPositionControl::Run() failsafe(time_stamp_now, setpoint, _states, !was_in_failsafe); _control.setInputSetpoint(setpoint); - constraints = FlightTask::empty_constraints; + constraints = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, false, {}}; _control.update(dt); } diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.hpp b/src/modules/mc_pos_control/MulticopterPositionControl.hpp index 7ef50bdfbe..5ca98f0efe 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.hpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.hpp @@ -40,7 +40,6 @@ #include #include #include -#include #include #include #include @@ -64,6 +63,8 @@ #include "PositionControl/PositionControl.hpp" +using namespace time_literals; + class MulticopterPositionControl : public ModuleBase, public control::SuperBlock, public ModuleParams, public px4::WorkItem {