forked from Archive/PX4-Autopilot
flight_mode_manager: merge with flight_tasks
This commit is contained in:
parent
b1e442b830
commit
4d7b875ee2
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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)
|
||||
|
|
|
@ -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)
|
|
@ -1,191 +0,0 @@
|
|||
#include "FlightTasks.hpp"
|
||||
|
||||
#include <uORB/topics/vehicle_command.h>
|
||||
#include <uORB/topics/vehicle_command_ack.h>
|
||||
|
||||
FlightTasks::FlightTasks()
|
||||
{
|
||||
// initialize all flight-tasks
|
||||
// currently this is required to get all parameters read
|
||||
for (int i = 0; i < static_cast<int>(FlightTaskIndex::Count); i++) {
|
||||
_initTask(static_cast<FlightTaskIndex>(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<int>(FlightTaskIndex::None) <= new_task_index &&
|
||||
static_cast<int>(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<FlightTaskError>(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<int>(switch_result);
|
||||
command_ack.target_system = command.source_system;
|
||||
command_ack.target_component = command.source_component;
|
||||
|
||||
_pub_vehicle_command_ack.publish(command_ack);
|
||||
}
|
|
@ -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 <maetugr@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "FlightTask.hpp"
|
||||
#include "FlightTasks_generated.hpp"
|
||||
#include <lib/weather_vane/WeatherVane.hpp>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/vehicle_command_ack.h>
|
||||
#include <uORB/topics/vehicle_command.h>
|
||||
|
||||
#include <new>
|
||||
|
||||
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<int>(_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<int>(_current_task.index); }
|
||||
|
||||
/**
|
||||
* Check if any task is active
|
||||
* @return true if a task is active, false if not
|
||||
*/
|
||||
bool isAnyTaskActive() const { return _current_task.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<int>(FlightTaskError::NoError), "No Error"},
|
||||
{static_cast<int>(FlightTaskError::InvalidTask), "Invalid Task "},
|
||||
{static_cast<int>(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<vehicle_command_ack_s> _pub_vehicle_command_ack{ORB_ID(vehicle_command_ack)};
|
||||
|
||||
int _initTask(FlightTaskIndex task_index);
|
||||
};
|
|
@ -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()
|
||||
|
|
|
@ -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<int>(FlightTaskIndex::Count); i++) {
|
||||
_initTask(static_cast<FlightTaskIndex>(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<int>(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<uint32_t>(_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<int>(FlightTaskIndex::None) <= new_task_index &&
|
||||
static_cast<int>(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<uint32_t>(_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;
|
||||
|
|
|
@ -33,17 +33,21 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include "FlightTask.hpp"
|
||||
#include "FlightTasks_generated.hpp"
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||
#include <lib/flight_tasks/FlightTasks.hpp>
|
||||
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
#include <uORB/topics/vehicle_command.h>
|
||||
#include <uORB/topics/vehicle_command_ack.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
#include <uORB/topics/vehicle_land_detected.h>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
|
@ -52,10 +56,18 @@
|
|||
|
||||
#include "Takeoff/Takeoff.hpp"
|
||||
|
||||
#include <new>
|
||||
|
||||
enum class FlightTaskError : int {
|
||||
NoError = 0,
|
||||
InvalidTask = -1,
|
||||
ActivationFailed = -2
|
||||
};
|
||||
|
||||
class FlightModeManager : public ModuleBase<FlightModeManager>, 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_s> _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_s> _home_position_sub{ORB_ID(home_position)};
|
||||
uORB::SubscriptionData<vehicle_control_mode_s> _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
|
||||
uORB::SubscriptionData<vehicle_land_detected_s> _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_s> _vehicle_local_position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint)};
|
||||
|
||||
uORB::SubscriptionData<vehicle_status_s> _vehicle_status_sub{ORB_ID(vehicle_status)};
|
||||
|
||||
uORB::Publication<landing_gear_s> _landing_gear_pub{ORB_ID(landing_gear)};
|
||||
uORB::Publication<vehicle_local_position_setpoint_s> _trajectory_setpoint_pub{ORB_ID(trajectory_setpoint)};
|
||||
uORB::Publication<vehicle_command_s> _vehicle_command_pub{ORB_ID(vehicle_command)};
|
||||
uORB::Publication<vehicle_command_ack_s> _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
|
||||
uORB::Publication<vehicle_constraints_s> _vehicle_constraints_pub{ORB_ID(vehicle_constraints)};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
|
|
|
@ -39,10 +39,10 @@
|
|||
* @@author Christoph Tobler <christoph@@px4.io>
|
||||
*/
|
||||
|
||||
#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
|
|
@ -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]@
|
|
@ -1,5 +1,6 @@
|
|||
#include <gtest/gtest.h>
|
||||
#include <flight_tasks/tasks/AutoLineSmoothVel/TrajectoryConstraints.hpp>
|
||||
|
||||
#include "TrajectoryConstraints.hpp"
|
||||
|
||||
using namespace matrix;
|
||||
using namespace math::trajectory;
|
|
@ -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})
|
|
@ -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;
|
|
@ -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
|
||||
)
|
||||
|
|
|
@ -37,7 +37,6 @@
|
|||
#include <lib/mathlib/mathlib.h>
|
||||
#include <lib/matrix/matrix/math.hpp>
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
|
|
|
@ -40,7 +40,6 @@
|
|||
#include <commander/px4_custom_mode.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <lib/controllib/blocks.hpp>
|
||||
#include <lib/flight_tasks/FlightTasks.hpp>
|
||||
#include <lib/hysteresis/hysteresis.h>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
#include <lib/systemlib/mavlink_log.h>
|
||||
|
@ -64,6 +63,8 @@
|
|||
|
||||
#include "PositionControl/PositionControl.hpp"
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
class MulticopterPositionControl : public ModuleBase<MulticopterPositionControl>, public control::SuperBlock,
|
||||
public ModuleParams, public px4::WorkItem
|
||||
{
|
||||
|
|
Loading…
Reference in New Issue