Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-29 03:36:07 +08:00
Code Issues Actions 10 Packages Projects Releases Wiki Activity

flight_mode_manager: merge with flight_tasks

Browse Source
This commit is contained in:
Daniel Agar
2021-01-04 10:21:47 -05:00
committed by Lorenz Meier
parent b1e442b830
commit 4d7b875ee2
79 changed files with 353 additions and 611 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/CMakeLists.txt
-1
View File
@@ -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)
src/lib/flight_tasks/CMakeLists.txt
-125
View File
@@ -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)
src/lib/flight_tasks/FlightTasks.cpp
-191
View File
@@ -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);
}
src/lib/flight_tasks/FlightTasks.hpp
-202
View File
@@ -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);
};
src/lib/flight_tasks/Templates/FlightTasks_generated.cpp.em
-100
View File
@@ -1,100 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @@file FlightTasks_generated.cpp
*
* Generated file to switch between all required flight tasks
*
* @@author Christoph Tobler <christoph@@px4.io>
*/
#include "FlightTasks.hpp"
#include "FlightTasks_generated.hpp"
int FlightTasks::_initTask(FlightTaskIndex task_index)
{
// disable the old task if there is any
if (_current_task.task) {
_current_task.task->~FlightTask();
_current_task.task = nullptr;
_current_task.index = FlightTaskIndex::None;
}
switch (task_index) {
case FlightTaskIndex::None:
// already disabled task
break;
@# loop through all requested tasks
@[if tasks]@
@[for task in tasks]@
@{
firstLowercase = lambda s: s[:1].lower() + s[1:] if s else ''
}@
case FlightTaskIndex::@(task):
_current_task.task = new (&_task_union.@(firstLowercase(task))) FlightTask@(task)();
break;
@[end for]@
@[end if]@
default:
// invalid task
return 1;
}
// task construction succeeded
_current_task.index = task_index;
return 0;
}
FlightTaskIndex FlightTasks::switchVehicleCommand(const int command)
{
switch (command) {
@# loop through all additional tasks
@[if tasks_add]@
@[for task in tasks_add]@
@{
upperCase = lambda s: s[:].upper() if s else ''
}@
case vehicle_command_s::VEHICLE_CMD_DO_@(upperCase(task)) :
return FlightTaskIndex::@(task);
break;
@[end for]@
@[end if]@
// ignore all unkown commands
default : return FlightTaskIndex::None;
}
}
src/lib/flight_tasks/Templates/FlightTasks_generated.hpp.em
-78
View File
@@ -1,78 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @@file FlightTasks_generated.hpp
*
* Generated Header to list all required flight tasks
*
* @@author Christoph Tobler <christoph@@px4.io>
*/
#pragma once
// include all required headers
#include "FlightTasks.hpp"
@# loop through all requested tasks
@[if tasks]@
@[for task in tasks]@
#include "FlightTask@(task).hpp"
@[end for]@
@[end if]@
enum class FlightTaskIndex : int {
None = -1,
@# loop through all requested tasks
@[if tasks]@
@[for task in tasks]@
@(task),
@[end for]@
@[end if]@
Count // number of tasks
};
union TaskUnion {
TaskUnion() {}
~TaskUnion() {}
@# loop through all requested tasks
@[if tasks]@
@{
firstLowercase = lambda s: s[:1].lower() + s[1:] if s else ''
}@
@[for task in tasks]@
FlightTask@(task) @(firstLowercase(task));
@[end for]@
@[end if]@
};
src/lib/flight_tasks/generate_flight_tasks.py
-27
View File
@@ -1,27 +0,0 @@
#!/usr/bin/env python
import em
import os
import argparse
parser = argparse.ArgumentParser()
parser.add_argument("-t", "--tasks", dest='tasks_all', nargs='+', required=True, help="All tasks to be generated")
parser.add_argument("-s", "--tasks_additional", dest='tasks_add', nargs='+', help="Additional tasks to be generated (on top of the core)")
parser.add_argument("-i", "--input_directory", dest='directory_in', required=True, help="Output directory")
parser.add_argument("-o", "--output_directory", dest='directory_out', required=True, help="Input directory")
parser.add_argument("-f", "--files", dest='gen_files', nargs='+', required=True, help="Files to generate")
# Parse arguments
args = parser.parse_args()
for gen_file in args.gen_files:
ofile = args.directory_out + "/" + gen_file
output_file = open(ofile, 'w')
# need to specify the em_globals inside the loop -> em.Error: interpreter globals collision
em_globals = {
"tasks": args.tasks_all,
"tasks_add": args.tasks_add,
}
interpreter = em.Interpreter(output=output_file, globals=em_globals)
interpreter.file(open(args.directory_in + "/" + gen_file + ".em"))
interpreter.shutdown()
src/lib/flight_tasks/tasks/Auto/CMakeLists.txt
-39
View File
@@ -1,39 +0,0 @@
############################################################################
#
# Copyright (c) 2018 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_library(FlightTaskAuto
FlightTaskAuto.cpp
)
target_link_libraries(FlightTaskAuto PUBLIC avoidance FlightTask FlightTaskUtility)
target_include_directories(FlightTaskAuto PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp
-519
View File
File diff suppressed because it is too large Load Diff
src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.hpp
-160
View File
@@ -1,160 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file FlightTaskAuto.hpp
*
* Map from global triplet to local quadruple.
*/
#pragma once
#include "FlightTask.hpp"
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/position_setpoint.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <lib/ecl/geo/geo.h>
// TODO: make this switchable in the board config, like a module
#if CONSTRAINED_FLASH
#include <lib/avoidance/ObstacleAvoidance_dummy.hpp>
#else
#include <lib/avoidance/ObstacleAvoidance.hpp>
#endif
/**
* This enum has to agree with position_setpoint_s type definition
* The only reason for not using the struct position_setpoint is because
* of the size
*/
enum class WaypointType : int {
position = position_setpoint_s::SETPOINT_TYPE_POSITION,
velocity = position_setpoint_s::SETPOINT_TYPE_VELOCITY,
loiter = position_setpoint_s::SETPOINT_TYPE_LOITER,
takeoff = position_setpoint_s::SETPOINT_TYPE_TAKEOFF,
land = position_setpoint_s::SETPOINT_TYPE_LAND,
idle = position_setpoint_s::SETPOINT_TYPE_IDLE,
offboard = position_setpoint_s::SETPOINT_TYPE_OFFBOARD, // only part of this structure due to legacy reason. It is not used within the Auto flighttasks
follow_target = position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET,
};
enum class State {
offtrack, /**< Vehicle is more than cruise speed away from track */
target_behind, /**< Vehicle is in front of target. */
previous_infront, /**< Vehilce is behind previous waypoint.*/
none /**< Vehicle is in normal tracking mode from triplet previous to triplet target */
};
class FlightTaskAuto : public FlightTask
{
public:
FlightTaskAuto();
virtual ~FlightTaskAuto() = default;
bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
bool updateInitialize() override;
bool updateFinalize() override;
/**
* Sets an external yaw handler which can be used to implement a different yaw control strategy.
*/
void setYawHandler(WeatherVane *ext_yaw_handler) override {_ext_yaw_handler = ext_yaw_handler;}
protected:
void _setDefaultConstraints() override;
matrix::Vector2f _getTargetVelocityXY(); /**< only used for follow-me and only here because of legacy reason.*/
void _updateInternalWaypoints(); /**< Depending on state of vehicle, the internal waypoints might differ from target (for instance if offtrack). */
bool _compute_heading_from_2D_vector(float &heading, matrix::Vector2f v); /**< Computes and sets heading a 2D vector */
matrix::Vector3f _prev_prev_wp{}; /**< Pre-previous waypoint (local frame). This will be used for smoothing trajectories -> not used yet. */
matrix::Vector3f _prev_wp{}; /**< Previous waypoint (local frame). If no previous triplet is available, the prev_wp is set to current position. */
bool _prev_was_valid{false};
matrix::Vector3f _target{}; /**< Target waypoint (local frame).*/
matrix::Vector3f _next_wp{}; /**< The next waypoint after target (local frame). If no next setpoint is available, next is set to target. */
bool _next_was_valid{false};
float _mc_cruise_speed{0.0f}; /**< Requested cruise speed. If not valid, default cruise speed is used. */
WaypointType _type{WaypointType::idle}; /**< Type of current target triplet. */
uORB::SubscriptionData<home_position_s> _sub_home_position{ORB_ID(home_position)};
uORB::SubscriptionData<vehicle_status_s> _sub_vehicle_status{ORB_ID(vehicle_status)};
State _current_state{State::none};
float _target_acceptance_radius{0.0f}; /**< Acceptances radius of the target */
int _mission_gear{landing_gear_s::GEAR_KEEP};
float _yaw_sp_prev{NAN};
bool _yaw_sp_aligned{false};
ObstacleAvoidance _obstacle_avoidance; /**< class adjusting setpoints according to external avoidance module's input */
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
(ParamFloat<px4::params::MPC_XY_CRUISE>) _param_mpc_xy_cruise,
(ParamFloat<px4::params::NAV_MC_ALT_RAD>)
_param_nav_mc_alt_rad, //vertical acceptance radius at which waypoints are updated
(ParamInt<px4::params::MPC_YAW_MODE>) _param_mpc_yaw_mode, // defines how heading is executed,
(ParamInt<px4::params::COM_OBS_AVOID>) _param_com_obs_avoid, // obstacle avoidance active
(ParamFloat<px4::params::MPC_YAWRAUTO_MAX>) _param_mpc_yawrauto_max,
(ParamFloat<px4::params::MIS_YAW_ERR>) _param_mis_yaw_err, // yaw-error threshold
(ParamBool<px4::params::WV_EN>) _param_wv_en // enable/disable weather vane (VTOL)
);
private:
matrix::Vector2f _lock_position_xy{NAN, NAN}; /**< if no valid triplet is received, lock positition to current position */
bool _yaw_lock{false}; /**< if within acceptance radius, lock yaw to current yaw */
uORB::SubscriptionData<position_setpoint_triplet_s> _sub_triplet_setpoint{ORB_ID(position_setpoint_triplet)};
matrix::Vector3f
_triplet_target; /**< current triplet from navigator which may differ from the intenal one (_target) depending on the vehicle state. */
matrix::Vector3f
_triplet_prev_wp; /**< previous triplet from navigator which may differ from the intenal one (_prev_wp) depending on the vehicle state.*/
matrix::Vector3f
_triplet_next_wp; /**< next triplet from navigator which may differ from the intenal one (_next_wp) depending on the vehicle state.*/
matrix::Vector2f _closest_pt; /**< closest point to the vehicle position on the line previous - target */
map_projection_reference_s _reference_position{}; /**< Structure used to project lat/lon setpoint into local frame. */
float _reference_altitude{NAN}; /**< Altitude relative to ground. */
hrt_abstime _time_stamp_reference{0}; /**< time stamp when last reference update occured. */
WeatherVane *_ext_yaw_handler{nullptr}; /**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
void _limitYawRate(); /**< Limits the rate of change of the yaw setpoint. */
bool _evaluateTriplets(); /**< Checks and sets triplets. */
bool _isFinite(const position_setpoint_s &sp); /**< Checks if all waypoint triplets are finite. */
bool _evaluateGlobalReference(); /**< Check is global reference is available. */
State _getCurrentState(); /**< Computes the current vehicle state based on the vehicle position and navigator triplets. */
void _set_heading_from_mode(); /**< @see MPC_YAW_MODE */
};
src/lib/flight_tasks/tasks/AutoFollowMe/CMakeLists.txt
-39
View File
@@ -1,39 +0,0 @@
############################################################################
#
# Copyright (c) 2018 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_library(FlightTaskAutoFollowMe
FlightTaskAutoFollowMe.cpp
)
target_link_libraries(FlightTaskAutoFollowMe PUBLIC FlightTaskAuto)
target_include_directories(FlightTaskAutoFollowMe PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
src/lib/flight_tasks/tasks/AutoFollowMe/FlightTaskAutoFollowMe.cpp
-50
View File
@@ -1,50 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file FlightTaskAutoFollowMe.cpp
*/
#include "FlightTaskAutoFollowMe.hpp"
#include <mathlib/mathlib.h>
using namespace matrix;
bool FlightTaskAutoFollowMe::update()
{
bool ret = FlightTaskAuto::update();
_position_setpoint = _target;
matrix::Vector2f vel_sp = _getTargetVelocityXY();
_velocity_setpoint = matrix::Vector3f(vel_sp(0), vel_sp(1), 0.0f);
return ret;
}
src/lib/flight_tasks/tasks/AutoFollowMe/FlightTaskAutoFollowMe.hpp
-50
View File
@@ -1,50 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file FlightTaskAutoFollowMe.hpp
*
* Flight task for autonomous, gps driven follow-me mode.
*/
#pragma once
#include "FlightTaskAuto.hpp"
class FlightTaskAutoFollowMe : public FlightTaskAuto
{
public:
FlightTaskAutoFollowMe() = default;
virtual ~FlightTaskAutoFollowMe() = default;
bool update() override;
};
src/lib/flight_tasks/tasks/AutoLineSmoothVel/CMakeLists.txt
-42
View File
@@ -1,42 +0,0 @@
############################################################################
#
# Copyright (c) 2018 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_library(FlightTaskAutoLineSmoothVel
FlightTaskAutoLineSmoothVel.cpp
)
target_link_libraries(FlightTaskAutoLineSmoothVel PUBLIC FlightTaskAutoMapper FlightTaskUtility)
target_include_directories(FlightTaskAutoLineSmoothVel PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
px4_add_unit_gtest(SRC TrajectoryConstraintsTest.cpp)
src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
-490
View File
@@ -1,490 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file FlightAutoLine.cpp
*/
#include "FlightTaskAutoLineSmoothVel.hpp"
#include "TrajectoryConstraints.hpp"
using namespace matrix;
bool FlightTaskAutoLineSmoothVel::activate(const vehicle_local_position_setpoint_s &last_setpoint)
{
bool ret = FlightTaskAutoMapper::activate(last_setpoint);
Vector3f vel_prev{last_setpoint.vx, last_setpoint.vy, last_setpoint.vz};
Vector3f pos_prev{last_setpoint.x, last_setpoint.y, last_setpoint.z};
Vector3f accel_prev{last_setpoint.acceleration};
for (int i = 0; i < 3; i++) {
// If the position setpoint is unknown, set to the current postion
if (!PX4_ISFINITE(pos_prev(i))) { pos_prev(i) = _position(i); }
// If the velocity setpoint is unknown, set to the current velocity
if (!PX4_ISFINITE(vel_prev(i))) { vel_prev(i) = _velocity(i); }
// No acceleration estimate available, set to zero if the setpoint is NAN
if (!PX4_ISFINITE(accel_prev(i))) { accel_prev(i) = 0.f; }
}
for (int i = 0; i < 3; ++i) {
_trajectory[i].reset(accel_prev(i), vel_prev(i), pos_prev(i));
}
_yaw_sp_prev = PX4_ISFINITE(last_setpoint.yaw) ? last_setpoint.yaw : _yaw;
_updateTrajConstraints();
return ret;
}
void FlightTaskAutoLineSmoothVel::reActivate()
{
FlightTaskAutoMapper::reActivate();
// On ground, reset acceleration and velocity to zero
for (int i = 0; i < 2; ++i) {
_trajectory[i].reset(0.f, 0.f, _position(i));
}
_trajectory[2].reset(0.f, 0.7f, _position(2));
}
/**
* EKF reset handling functions
* Those functions are called by the base FlightTask in
* case of an EKF reset event
*/
void FlightTaskAutoLineSmoothVel::_ekfResetHandlerPositionXY()
{
_trajectory[0].setCurrentPosition(_position(0));
_trajectory[1].setCurrentPosition(_position(1));
}
void FlightTaskAutoLineSmoothVel::_ekfResetHandlerVelocityXY()
{
_trajectory[0].setCurrentVelocity(_velocity(0));
_trajectory[1].setCurrentVelocity(_velocity(1));
}
void FlightTaskAutoLineSmoothVel::_ekfResetHandlerPositionZ()
{
_trajectory[2].setCurrentPosition(_position(2));
}
void FlightTaskAutoLineSmoothVel::_ekfResetHandlerVelocityZ()
{
_trajectory[2].setCurrentVelocity(_velocity(2));
}
void FlightTaskAutoLineSmoothVel::_ekfResetHandlerHeading(float delta_psi)
{
_yaw_sp_prev += delta_psi;
}
void FlightTaskAutoLineSmoothVel::_generateSetpoints()
{
_updateTurningCheck();
_prepareSetpoints();
_generateTrajectory();
if (!PX4_ISFINITE(_yaw_setpoint) && !PX4_ISFINITE(_yawspeed_setpoint)) {
// no valid heading -> generate heading in this flight task
_generateHeading();
}
}
void FlightTaskAutoLineSmoothVel::_updateTurningCheck()
{
const Vector2f vel_traj(_trajectory[0].getCurrentVelocity(),
_trajectory[1].getCurrentVelocity());
const Vector2f pos_traj(_trajectory[0].getCurrentPosition(),
_trajectory[1].getCurrentPosition());
const Vector2f target_xy(_target);
const Vector2f u_vel_traj = vel_traj.unit_or_zero();
const Vector2f pos_to_target = Vector2f(target_xy - pos_traj);
const float cos_align = u_vel_traj * pos_to_target.unit_or_zero();
// The vehicle is turning if the angle between the velocity vector
// and the direction to the target is greater than 10 degrees, the
// velocity is large enough and the drone isn't in the acceptance
// radius of the last WP.
_is_turning = vel_traj.longerThan(0.2f)
&& cos_align < 0.98f
&& pos_to_target.longerThan(_target_acceptance_radius);
}
void FlightTaskAutoLineSmoothVel::_generateHeading()
{
// Generate heading along trajectory if possible, otherwise hold the previous yaw setpoint
if (!_generateHeadingAlongTraj()) {
_yaw_setpoint = _yaw_sp_prev;
}
}
bool FlightTaskAutoLineSmoothVel::_generateHeadingAlongTraj()
{
bool res = false;
Vector2f vel_sp_xy(_velocity_setpoint);
Vector2f traj_to_target = Vector2f(_target) - Vector2f(_position);
if ((vel_sp_xy.length() > .1f) &&
(traj_to_target.length() > 2.f)) {
// Generate heading from velocity vector, only if it is long enough
// and if the drone is far enough from the target
_compute_heading_from_2D_vector(_yaw_setpoint, vel_sp_xy);
res = true;
}
return res;
}
/* Constrain some value vith a constrain depending on the sign of the constraint
* Example: - if the constrain is -5, the value will be constrained between -5 and 0
* - if the constrain is 5, the value will be constrained between 0 and 5
*/
float FlightTaskAutoLineSmoothVel::_constrainOneSide(float val, float constraint)
{
const float min = (constraint < FLT_EPSILON) ? constraint : 0.f;
const float max = (constraint > FLT_EPSILON) ? constraint : 0.f;
return math::constrain(val, min, max);
}
float FlightTaskAutoLineSmoothVel::_constrainAbs(float val, float max)
{
return sign(val) * math::min(fabsf(val), fabsf(max));
}
float FlightTaskAutoLineSmoothVel::_getMaxXYSpeed() const
{
Vector3f pos_traj(_trajectory[0].getCurrentPosition(),
_trajectory[1].getCurrentPosition(),
_trajectory[2].getCurrentPosition());
math::trajectory::VehicleDynamicLimits config;
config.z_accept_rad = _param_nav_mc_alt_rad.get();
config.xy_accept_rad = _target_acceptance_radius;
config.max_acc_xy = _trajectory[0].getMaxAccel();
config.max_jerk = _trajectory[0].getMaxJerk();
config.max_speed_xy = _mc_cruise_speed;
config.max_acc_xy_radius_scale = _param_mpc_xy_traj_p.get();
// constrain velocity to go to the position setpoint first if the position setpoint has been modified by an external source
// (eg. Obstacle Avoidance)
Vector3f waypoints[3] = {pos_traj, _target, _next_wp};
if (isTargetModified()) {
waypoints[2] = waypoints[1] = _position_setpoint;
}
float max_xy_speed = math::trajectory::computeXYSpeedFromWaypoints<3>(waypoints, config);
return max_xy_speed;
}
float FlightTaskAutoLineSmoothVel::_getMaxZSpeed() const
{
Vector3f pos_traj(_trajectory[0].getCurrentPosition(),
_trajectory[1].getCurrentPosition(),
_trajectory[2].getCurrentPosition());
float z_setpoint = _target(2);
// constrain velocity to go to the position setpoint first if the position setpoint has been modified by an external source
// (eg. Obstacle Avoidance)
bool z_valid = PX4_ISFINITE(_position_setpoint(2));
bool z_modified = z_valid && fabsf((_target - _position_setpoint)(2)) > FLT_EPSILON;
if (z_modified) {
z_setpoint = _position_setpoint(2);
}
const float distance_start_target = fabs(z_setpoint - pos_traj(2));
const float arrival_z_speed = 0.f;
float max_speed = math::min(_trajectory[2].getMaxVel(), math::trajectory::computeMaxSpeedFromDistance(
_trajectory[2].getMaxJerk(), _trajectory[2].getMaxAccel(),
distance_start_target, arrival_z_speed));
return max_speed;
}
Vector3f FlightTaskAutoLineSmoothVel::getCrossingPoint() const
{
Vector3f pos_crossing_point{};
if (isTargetModified()) {
// Strictly follow the modified setpoint
pos_crossing_point = _position_setpoint;
} else {
if (_is_turning) {
// Get the crossing point using L1-style guidance
pos_crossing_point.xy() = getL1Point();
pos_crossing_point(2) = _target(2);
} else {
pos_crossing_point = _target;
}
}
return pos_crossing_point;
}
bool FlightTaskAutoLineSmoothVel::isTargetModified() const
{
const bool xy_modified = (_target - _position_setpoint).xy().longerThan(FLT_EPSILON);
const bool z_valid = PX4_ISFINITE(_position_setpoint(2));
const bool z_modified = z_valid && fabs((_target - _position_setpoint)(2)) > FLT_EPSILON;
return xy_modified || z_modified;
}
Vector2f FlightTaskAutoLineSmoothVel::getL1Point() const
{
const Vector2f pos_traj(_trajectory[0].getCurrentPosition(),
_trajectory[1].getCurrentPosition());
const Vector2f target_xy(_target);
const Vector2f u_prev_to_target = Vector2f(target_xy - Vector2f(_prev_wp)).unit_or_zero();
const Vector2f prev_to_pos(pos_traj - Vector2f(_prev_wp));
const Vector2f prev_to_closest(u_prev_to_target * (prev_to_pos * u_prev_to_target));
const Vector2f closest_pt = Vector2f(_prev_wp) + prev_to_closest;
// Compute along-track error using L1 distance and cross-track error
const float crosstrack_error = Vector2f(closest_pt - pos_traj).length();
const float l1 = math::max(_target_acceptance_radius, 5.f);
float alongtrack_error = 0.f;
// Protect against sqrt of a negative number
if (l1 > crosstrack_error) {
alongtrack_error = sqrtf(l1 * l1 - crosstrack_error * crosstrack_error);
}
// Position of the point on the line where L1 intersect the line between the two waypoints
const Vector2f l1_point = closest_pt + alongtrack_error * u_prev_to_target;
return l1_point;
}
void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
{
// Interface: A valid position setpoint generates a velocity target using conservative motion constraints.
// If a velocity is specified, that is used as a feedforward to track the position setpoint
// (ie. it assumes the position setpoint is moving at the specified velocity)
// If the position setpoints are set to NAN, the values in the velocity setpoints are used as velocity targets: nothing to do here.
_want_takeoff = false;
if (_param_mpc_yaw_mode.get() == 4 && !_yaw_sp_aligned) {
// Wait for the yaw setpoint to be aligned
_velocity_setpoint.setAll(0.f);
return;
}
const bool xy_pos_setpoint_valid = PX4_ISFINITE(_position_setpoint(0)) && PX4_ISFINITE(_position_setpoint(1));
const bool z_pos_setpoint_valid = PX4_ISFINITE(_position_setpoint(2));
if (xy_pos_setpoint_valid && z_pos_setpoint_valid) {
// Use 3D position setpoint to generate a 3D velocity setpoint
Vector3f pos_traj(_trajectory[0].getCurrentPosition(),
_trajectory[1].getCurrentPosition(),
_trajectory[2].getCurrentPosition());
const Vector3f u_pos_traj_to_dest((getCrossingPoint() - pos_traj).unit_or_zero());
float xy_speed = _getMaxXYSpeed();
const float z_speed = _getMaxZSpeed();
if (_is_turning) {
// Lock speed during turn
xy_speed = math::min(_max_speed_prev, xy_speed);
} else {
_max_speed_prev = xy_speed;
}
Vector3f vel_sp_constrained = u_pos_traj_to_dest * sqrtf(xy_speed * xy_speed + z_speed * z_speed);
math::trajectory::clampToXYNorm(vel_sp_constrained, xy_speed, 0.5f);
math::trajectory::clampToZNorm(vel_sp_constrained, z_speed, 0.5f);
for (int i = 0; i < 3; i++) {
// If available, use the existing velocity as a feedforward, otherwise replace it
if (PX4_ISFINITE(_velocity_setpoint(i))) {
_velocity_setpoint(i) += vel_sp_constrained(i);
} else {
_velocity_setpoint(i) = vel_sp_constrained(i);
}
}
}
else if (xy_pos_setpoint_valid) {
// Use 2D position setpoint to generate a 2D velocity setpoint
// Get various path specific vectors
Vector2f pos_traj(_trajectory[0].getCurrentPosition(), _trajectory[1].getCurrentPosition());
Vector2f pos_traj_to_dest_xy = Vector2f(getCrossingPoint()) - pos_traj;
Vector2f u_pos_traj_to_dest_xy(pos_traj_to_dest_xy.unit_or_zero());
float xy_speed = _getMaxXYSpeed();
if (_is_turning) {
// Lock speed during turn
xy_speed = math::min(_max_speed_prev, xy_speed);
} else {
_max_speed_prev = xy_speed;
}
Vector2f vel_sp_constrained_xy = u_pos_traj_to_dest_xy * xy_speed;
for (int i = 0; i < 2; i++) {
// If available, use the existing velocity as a feedforward, otherwise replace it
if (PX4_ISFINITE(_velocity_setpoint(i))) {
_velocity_setpoint(i) += vel_sp_constrained_xy(i);
} else {
_velocity_setpoint(i) = vel_sp_constrained_xy(i);
}
}
}
else if (z_pos_setpoint_valid) {
// Use Z position setpoint to generate a Z velocity setpoint
const float z_dir = sign(_position_setpoint(2) - _trajectory[2].getCurrentPosition());
const float vel_sp_z = z_dir * _getMaxZSpeed();
// If available, use the existing velocity as a feedforward, otherwise replace it
if (PX4_ISFINITE(_velocity_setpoint(2))) {
_velocity_setpoint(2) += vel_sp_z;
} else {
_velocity_setpoint(2) = vel_sp_z;
}
}
_want_takeoff = _velocity_setpoint(2) < -0.3f;
}
void FlightTaskAutoLineSmoothVel::_updateTrajConstraints()
{
// Update the constraints of the trajectories
_trajectory[0].setMaxAccel(_param_mpc_acc_hor.get()); // TODO : Should be computed using heading
_trajectory[1].setMaxAccel(_param_mpc_acc_hor.get());
_trajectory[0].setMaxVel(_param_mpc_xy_vel_max.get());
_trajectory[1].setMaxVel(_param_mpc_xy_vel_max.get());
_trajectory[0].setMaxJerk(_param_mpc_jerk_auto.get()); // TODO : Should be computed using heading
_trajectory[1].setMaxJerk(_param_mpc_jerk_auto.get());
_trajectory[2].setMaxJerk(_param_mpc_jerk_auto.get());
if (_velocity_setpoint(2) < 0.f) { // up
float z_accel_constraint = _param_mpc_acc_up_max.get();
float z_vel_constraint = _param_mpc_z_vel_max_up.get();
// The constraints are broken because they are used as hard limits by the position controller, so put this here
// until the constraints don't do things like cause controller integrators to saturate. Once the controller
// doesn't use z speed constraints, this can go in AutoMapper::_prepareTakeoffSetpoints(). Accel limit is to
// emulate the motor ramp (also done in the controller) so that the controller can actually track the setpoint.
if (_type == WaypointType::takeoff && _dist_to_ground < _param_mpc_land_alt1.get()) {
z_vel_constraint = _param_mpc_tko_speed.get();
z_accel_constraint = math::min(z_accel_constraint, _param_mpc_tko_speed.get() / _param_mpc_tko_ramp_t.get());
// Keep the altitude setpoint at the current altitude
// to avoid having it going down into the ground during
// the initial ramp as the velocity does not start at 0
_trajectory[2].setCurrentPosition(_position(2));
}
_trajectory[2].setMaxVel(z_vel_constraint);
_trajectory[2].setMaxAccel(z_accel_constraint);
} else { // down
_trajectory[2].setMaxAccel(_param_mpc_acc_down_max.get());
_trajectory[2].setMaxVel(_param_mpc_z_vel_max_dn.get());
}
}
void FlightTaskAutoLineSmoothVel::_generateTrajectory()
{
if (!PX4_ISFINITE(_velocity_setpoint(0)) || !PX4_ISFINITE(_velocity_setpoint(1))
|| !PX4_ISFINITE(_velocity_setpoint(2))) {
return;
}
/* Slow down the trajectory by decreasing the integration time based on the position error.
* This is only performed when the drone is behind the trajectory
*/
Vector2f position_trajectory_xy(_trajectory[0].getCurrentPosition(), _trajectory[1].getCurrentPosition());
Vector2f position_xy(_position);
Vector2f vel_traj_xy(_trajectory[0].getCurrentVelocity(), _trajectory[1].getCurrentVelocity());
Vector2f drone_to_trajectory_xy(position_trajectory_xy - position_xy);
float position_error = drone_to_trajectory_xy.length();
float time_stretch = 1.f - math::constrain(position_error * 0.5f, 0.f, 1.f);
// Don't stretch time if the drone is ahead of the position setpoint
if (drone_to_trajectory_xy.dot(vel_traj_xy) < 0.f) {
time_stretch = 1.f;
}
Vector3f jerk_sp_smooth;
Vector3f accel_sp_smooth;
Vector3f vel_sp_smooth;
Vector3f pos_sp_smooth;
for (int i = 0; i < 3; ++i) {
_trajectory[i].updateTraj(_deltatime, time_stretch);
jerk_sp_smooth(i) = _trajectory[i].getCurrentJerk();
accel_sp_smooth(i) = _trajectory[i].getCurrentAcceleration();
vel_sp_smooth(i) = _trajectory[i].getCurrentVelocity();
pos_sp_smooth(i) = _trajectory[i].getCurrentPosition();
}
_updateTrajConstraints();
for (int i = 0; i < 3; ++i) {
_trajectory[i].updateDurations(_velocity_setpoint(i));
}
VelocitySmoothing::timeSynchronization(_trajectory, 3);
_jerk_setpoint = jerk_sp_smooth;
_acceleration_setpoint = accel_sp_smooth;
_velocity_setpoint = vel_sp_smooth;
_position_setpoint = pos_sp_smooth;
}
src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp
-101
View File
@@ -1,101 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file FlightTaskAutoLineSmoothVel.hpp
*
* Flight task for autonomous, gps driven mode. The vehicle flies
* along a straight line in between waypoints.
*/
#pragma once
#include "FlightTaskAutoMapper.hpp"
#include <motion_planning/VelocitySmoothing.hpp>
class FlightTaskAutoLineSmoothVel : public FlightTaskAutoMapper
{
public:
FlightTaskAutoLineSmoothVel() = default;
virtual ~FlightTaskAutoLineSmoothVel() = default;
bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
void reActivate() override;
protected:
/** Reset position or velocity setpoints in case of EKF reset event */
void _ekfResetHandlerPositionXY() override;
void _ekfResetHandlerVelocityXY() override;
void _ekfResetHandlerPositionZ() override;
void _ekfResetHandlerVelocityZ() override;
void _ekfResetHandlerHeading(float delta_psi) override;
void _generateSetpoints() override; /**< Generate setpoints along line. */
void _generateHeading();
void _updateTurningCheck();
bool _generateHeadingAlongTraj(); /**< Generates heading along trajectory. */
static float _constrainOneSide(float val, float constraint); /**< Constrain val between INF and constraint */
static float _constrainAbs(float val, float max); /** Constrain the value -max <= val <= max */
float _getMaxXYSpeed() const;
float _getMaxZSpeed() const;
matrix::Vector3f getCrossingPoint() const;
bool isTargetModified() const;
matrix::Vector2f getL1Point() const;
float _max_speed_prev{};
bool _is_turning{false};
void _prepareSetpoints(); /**< Generate velocity target points for the trajectory generator. */
void _updateTrajConstraints();
void _generateTrajectory();
/** determines when to trigger a takeoff (ignored in flight) */
bool _checkTakeoff() override { return _want_takeoff; };
bool _want_takeoff{false};
VelocitySmoothing _trajectory[3]; ///< Trajectories in x, y and z directions
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAutoMapper,
(ParamFloat<px4::params::MIS_YAW_ERR>) _param_mis_yaw_err, // yaw-error threshold
(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor, // acceleration in flight
(ParamFloat<px4::params::MPC_ACC_UP_MAX>) _param_mpc_acc_up_max,
(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _param_mpc_acc_down_max,
(ParamFloat<px4::params::MPC_JERK_AUTO>) _param_mpc_jerk_auto,
(ParamFloat<px4::params::MPC_XY_TRAJ_P>) _param_mpc_xy_traj_p
);
};
src/lib/flight_tasks/tasks/AutoLineSmoothVel/TrajectoryConstraints.hpp
-180
View File
@@ -1,180 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019 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.
*
****************************************************************************/
#pragma once
#include <px4_defines.h>
#include <matrix/math.hpp>
#include <mathlib/mathlib.h>
namespace math
{
namespace trajectory
{
using matrix::Vector3f;
using matrix::Vector2f;
struct VehicleDynamicLimits {
float z_accept_rad;
float xy_accept_rad;
float max_acc_xy;
float max_jerk;
float max_speed_xy;
// TODO: remove this
float max_acc_xy_radius_scale;
};
/*
* Compute the maximum allowed speed at the waypoint assuming that we want to
* connect the two lines (current-target and target-next)
* with a tangent circle with constant speed and desired centripetal acceleration: a_centripetal = speed^2 / radius
* The circle should in theory start and end at the intersection of the lines and the waypoint's acceptance radius.
* This is not exactly true in reality since Navigator switches the waypoint so we have to take in account that
* the real acceptance radius is smaller.
*
*/
inline float computeStartXYSpeedFromWaypoints(const Vector3f &start_position, const Vector3f &target,
const Vector3f &next_target, float exit_speed, const VehicleDynamicLimits &config)
{
const float distance_target_next = (target - next_target).xy().norm();
const bool target_next_different = distance_target_next > 0.001f;
const bool waypoint_overlap = distance_target_next < config.xy_accept_rad;
const bool has_reached_altitude = fabsf(target(2) - start_position(2)) < config.z_accept_rad;
const bool altitude_stays_same = fabsf(next_target(2) - target(2)) < config.z_accept_rad;
float speed_at_target = 0.0f;
if (target_next_different &&
!waypoint_overlap &&
has_reached_altitude &&
altitude_stays_same
) {
const float alpha = acosf(Vector2f((target - start_position).xy()).unit_or_zero().dot(
Vector2f((target - next_target).xy()).unit_or_zero()));
const float safe_alpha = constrain(alpha, 0.f, M_PI_F - FLT_EPSILON);
float accel_tmp = config.max_acc_xy_radius_scale * config.max_acc_xy;
float max_speed_in_turn = computeMaxSpeedInWaypoint(safe_alpha, accel_tmp, config.xy_accept_rad);
speed_at_target = min(min(max_speed_in_turn, exit_speed), config.max_speed_xy);
}
float start_to_target = (start_position - target).xy().norm();
float max_speed = computeMaxSpeedFromDistance(config.max_jerk, config.max_acc_xy, start_to_target, speed_at_target);
return min(config.max_speed_xy, max_speed);
}
/*
* This function computes the maximum speed XY that can be travelled, given a set of waypoints and vehicle dynamics
*
* The first waypoint should be the starting location, and the later waypoints the desired points to be followed.
*
* @param waypoints the list of waypoints to be followed, the first of which should be the starting location
* @param config the vehicle dynamic limits
*
* @return the maximum speed at waypoint[0] which allows it to follow the trajectory while respecting the dynamic limits
*/
template <size_t N>
float computeXYSpeedFromWaypoints(const Vector3f waypoints[N], const VehicleDynamicLimits &config)
{
static_assert(N >= 2, "Need at least 2 points to compute speed");
float max_speed = 0.f;
for (size_t j = 0; j < N - 1; j++) {
size_t i = N - 2 - j;
max_speed = computeStartXYSpeedFromWaypoints(waypoints[i],
waypoints[i + 1],
waypoints[min(i + 2, N - 1)],
max_speed, config);
}
return max_speed;
}
/*
* Constrain the 3D vector given a maximum XY norm
* If the XY norm of the 3D vector is larger than the maximum norm, the whole vector
* is scaled down to respect the constraint.
* If the maximum norm is small (defined by the "accuracy" parameter),
* only the XY components are scaled down to avoid affecting
* Z in case of numerical issues
*/
inline void clampToXYNorm(Vector3f &target, float max_xy_norm, float accuracy = FLT_EPSILON)
{
const float xynorm = target.xy().norm();
const float scale_factor = (xynorm > FLT_EPSILON)
? max_xy_norm / xynorm
: 1.f;
if (scale_factor < 1.f) {
if (max_xy_norm < accuracy && xynorm < accuracy) {
target.xy() = Vector2f(target) * scale_factor;
} else {
target *= scale_factor;
}
}
}
/*
* Constrain the 3D vector given a maximum Z norm
* If the Z component of the 3D vector is larger than the maximum norm, the whole vector
* is scaled down to respect the constraint.
* If the maximum norm is small (defined by the "accuracy parameter),
* only the Z component is scaled down to avoid affecting
* XY in case of numerical issues
*/
inline void clampToZNorm(Vector3f &target, float max_z_norm, float accuracy = FLT_EPSILON)
{
const float znorm = fabs(target(2));
const float scale_factor = (znorm > FLT_EPSILON)
? max_z_norm / znorm
: 1.f;
if (scale_factor < 1.f) {
if (max_z_norm < accuracy && znorm < accuracy) {
target(2) *= scale_factor;
} else {
target *= scale_factor;
}
}
}
}
}
src/lib/flight_tasks/tasks/AutoLineSmoothVel/TrajectoryConstraintsTest.cpp
-363
View File
@@ -1,363 +0,0 @@
#include <gtest/gtest.h>
#include <flight_tasks/tasks/AutoLineSmoothVel/TrajectoryConstraints.hpp>
using namespace matrix;
using namespace math::trajectory;
class TrajectoryConstraintsTest : public ::testing::Test
{
public:
VehicleDynamicLimits config;
Vector3f vehicle_location;
Vector3f target;
Vector3f next_target;
float final_speed = 0;
void SetUp() override
{
config.z_accept_rad = 1.f;
config.xy_accept_rad = 0.99f;
config.max_acc_xy = 3.f;
config.max_jerk = 10.f;
config.max_speed_xy = 10.f;
config.max_acc_xy_radius_scale = 0.8f;
/*
* (20,20)
* Next target
*
* ^
* |
*
* (10,10) (20,10)
* Vehicle -> Target
*
*/
vehicle_location = Vector3f(10, 10, 5);
target = Vector3f(20, 10, 5);
next_target = Vector3f(20, 20, 5);
}
};
TEST_F(TrajectoryConstraintsTest, testStraight)
{
// GIVEN: 3 waypoints in straight line
next_target = target + 2.f * (target - vehicle_location);
target = vehicle_location + 0.5f * (next_target - vehicle_location);
// WHEN: we get the speed for straight line travel
Vector3f waypoints[3] = {vehicle_location, target, next_target};
float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config);
// THEN: it should be the same as speed directly to the end point
Vector3f direct_points[2] = {vehicle_location, next_target};
float direct_speed = computeXYSpeedFromWaypoints<2>(direct_points, config);
EXPECT_FLOAT_EQ(through_speed, direct_speed);
}
TEST_F(TrajectoryConstraintsTest, testStraightNaN)
{
// GIVEN: 3 waypoints in straight line
next_target = target + 2.f * (target - vehicle_location);
target = vehicle_location + 0.5f * (next_target - vehicle_location);
next_target(0) = NAN;
next_target(1) = NAN;
// WHEN: we get the speed for points which are NaN afterwards
Vector3f waypoints[3] = {vehicle_location, target, next_target};
float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config);
// THEN: it should be the same as speed to the closer point
Vector3f direct_points[2] = {vehicle_location, target};
float direct_speed = computeXYSpeedFromWaypoints<2>(direct_points, config);
EXPECT_FLOAT_EQ(through_speed, direct_speed);
}
TEST_F(TrajectoryConstraintsTest, testStraightLowJerkClose)
{
// GIVEN: 3 waypoints in straight line
next_target = target + 2.f * (target - vehicle_location);
target = vehicle_location + 0.05f * (next_target - vehicle_location);
config.max_jerk = 8.f;
// WHEN: we get the speed for straight line travel
Vector3f waypoints[3] = {vehicle_location, target, next_target};
float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config);
// THEN: it should be the same as speed directly to the end point
Vector3f direct_points[2] = {vehicle_location, next_target};
float direct_speed = computeXYSpeedFromWaypoints<2>(direct_points, config);
EXPECT_FLOAT_EQ(through_speed, direct_speed);
}
TEST_F(TrajectoryConstraintsTest, testStraightMidClose)
{
// GIVEN: 3 waypoints in straight line
next_target = target + 2.f * (target - vehicle_location);
target = vehicle_location + 0.05f * (next_target - vehicle_location);
// WHEN: we get the speed for straight line travel
Vector3f waypoints[3] = {vehicle_location, target, next_target};
float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config);
// THEN: it should be the same as speed directly to the end point
Vector3f direct_points[2] = {vehicle_location, next_target};
float direct_speed = computeXYSpeedFromWaypoints<2>(direct_points, config);
EXPECT_FLOAT_EQ(through_speed, direct_speed);
}
TEST_F(TrajectoryConstraintsTest, testStraightMidFar)
{
// GIVEN: 3 waypoints in straight line
next_target = target + 2.f * (target - vehicle_location);
target = vehicle_location + 0.95f * (next_target - vehicle_location);
// WHEN: we get the speed for straight line travel
Vector3f waypoints[3] = {vehicle_location, target, next_target};
float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config);
// THEN: it should be the same as speed directly to the end point
Vector3f direct_points[2] = {vehicle_location, next_target};
float direct_speed = computeXYSpeedFromWaypoints<2>(direct_points, config);
EXPECT_FLOAT_EQ(through_speed, direct_speed);
}
TEST_F(TrajectoryConstraintsTest, test90Angle)
{
// GIVEN: 3 waypoints in 90 degree angle
EXPECT_FLOAT_EQ(0.f, (vehicle_location - target).dot(target - next_target));
// WHEN: we get the speed for travel around the path
Vector3f waypoints[3] = {vehicle_location, target, next_target};
float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config);
// THEN: it should be slightly faster than stopping at the intermediate point
Vector3f stop_points[2] = {vehicle_location, target};
float stop_speed = computeXYSpeedFromWaypoints<2>(stop_points, config);
EXPECT_GT(through_speed, stop_speed); //faster
EXPECT_LT(through_speed, stop_speed * 1.03f); // but less than 3% faster
}
TEST_F(TrajectoryConstraintsTest, test45Angle)
{
// GIVEN: 3 waypoints in 45 degree angle
next_target = Vector3f(25, 15, 5);
// WHEN: we get the speed for travel around the path
Vector3f waypoints[3] = {vehicle_location, target, next_target};
float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config);
// THEN: it should be slightly faster than stopping at the intermediate point
Vector3f stop_points[2] = {vehicle_location, target};
float stop_speed = computeXYSpeedFromWaypoints<2>(stop_points, config);
EXPECT_GT(through_speed, stop_speed * 1.03f); // more than 3% faster
EXPECT_LT(through_speed, stop_speed * 1.06f); // but less than 6% faster
}
TEST_F(TrajectoryConstraintsTest, test10Angle)
{
// GIVEN: 3 waypoints in 10 degree angle
next_target = Vector3f(30, 11.7, 5);
// WHEN: we get the speed for travel around the path
Vector3f waypoints[3] = {vehicle_location, target, next_target};
float through_speed = computeXYSpeedFromWaypoints<3>(waypoints, config);
// THEN: it should be slightly faster than stopping at the intermediate point
Vector3f stop_points[2] = {vehicle_location, target};
float stop_speed = computeXYSpeedFromWaypoints<2>(stop_points, config);
EXPECT_GT(through_speed, stop_speed * 1.25f); // more than 25% faster
EXPECT_LT(through_speed, stop_speed * 1.3f); // but less than 30% faster
}
TEST_F(TrajectoryConstraintsTest, test10AngleFarNext)
{
// GIVEN: 3 waypoints in 10 degree angle, but next waypoint is far
next_target = 2.f * (Vector3f(30, 11.7, 5) - target) + target;
// WHEN: we get the speed for travel around the path
Vector3f far_waypoints[3] = {vehicle_location, target, next_target};
float far_speed = computeXYSpeedFromWaypoints<3>(far_waypoints, config);
// THEN: it should be the same speed as a closer next waypoint at the same angle, since the bottleneck is the turn
next_target = Vector3f(30, 11.7, 5);
Vector3f close_waypoints[3] = {vehicle_location, target, next_target};
float close_speed = computeXYSpeedFromWaypoints<3>(close_waypoints, config);
EXPECT_FLOAT_EQ(far_speed, close_speed);
}
TEST_F(TrajectoryConstraintsTest, test10AngleCloseNext)
{
// GIVEN: 3 waypoints in right angle, but next waypoint is far
next_target = .2f * (Vector3f(30, 11.7, 5) - target) + target;
// WHEN: we get the speed for travel around the path
Vector3f close_waypoints[3] = {vehicle_location, target, next_target};
float close_speed = computeXYSpeedFromWaypoints<3>(close_waypoints, config);
// THEN: it should be slower than a further next waypoint at the same angle, since the bottleneck is the distance
next_target = Vector3f(30, 11.7, 5);
Vector3f normal_waypoints[3] = {vehicle_location, target, next_target};
float normal_speed = computeXYSpeedFromWaypoints<3>(normal_waypoints, config);
EXPECT_LT(close_speed, normal_speed);
}
TEST(TrajectoryConstraintsClamp, clampToXYNormNoEffectLarge)
{
// GIVEN: a short vector
Vector3f vec(1, 2, 3);
// WHEN: we clamp it on XY with a long cutoff
clampToXYNorm(vec, 1000.f);
// THEN: it shouldn't change
EXPECT_EQ(vec, Vector3f(1, 2, 3));
}
TEST(TrajectoryConstraintsClamp, clampToZNormNoEffect)
{
// GIVEN: a short vector
Vector3f vec(1, 2, 3);
// WHEN: we clamp it on XY with a long cutoff
clampToZNorm(vec, 1000.f);
// THEN: it shouldn't change
EXPECT_EQ(vec, Vector3f(1, 2, 3));
}
TEST(TrajectoryConstraintsClamp, clampToXYNormNoEffectExact)
{
// GIVEN: a vector
Vector3f vec(3, 4, 1);
// WHEN: we clamp it on XY with exact cutoff
clampToXYNorm(vec, 5.f);
// THEN: it shouldn't change
EXPECT_EQ(vec, Vector3f(3, 4, 1));
}
TEST(TrajectoryConstraintsClamp, clampToZNormNoEffectExact)
{
// GIVEN: a vector
Vector3f vec(3, 4, -1);
// WHEN: we clamp it on Z with exact cutoff
clampToZNorm(vec, 1.f);
// THEN: it shouldn't change
EXPECT_EQ(vec, Vector3f(3, 4, -1));
}
TEST(TrajectoryConstraintsClamp, clampToXYNormHalf)
{
// GIVEN: a vector
Vector3f vec(3, 4, 1);
// WHEN: we clamp it on XY with half hypot length
clampToXYNorm(vec, 2.5f);
// THEN: it should be half length
EXPECT_TRUE(vec == Vector3f(1.5f, 2.f, 0.5f));
}
TEST(TrajectoryConstraintsClamp, clampToZNormHalf)
{
// GIVEN: a vector
Vector3f vec(3, 4, 10);
// WHEN: we clamp it on Z with half length
clampToZNorm(vec, 5.f);
// THEN: it should be half length
EXPECT_TRUE(vec == Vector3f(1.5f, 2.f, 5.f));
}
TEST(TrajectoryConstraintsClamp, clampToXYNormZero)
{
// GIVEN: a vector
Vector3f vec(3, 4, 1);
// WHEN: we clamp it on XY with half hypot length
clampToXYNorm(vec, 0.f);
// THEN: it should be 0
EXPECT_TRUE(vec == Vector3f(0.f, 0.f, 0.f));
}
TEST(TrajectoryConstraintsClamp, clampToZNormZero)
{
// GIVEN: a vector
Vector3f vec(3, 4, 1);
// WHEN: we clamp it on Z with half hypot length
clampToZNorm(vec, 0.f);
// THEN: it should be 0
EXPECT_TRUE(vec == Vector3f(0.f, 0.f, 0.f));
}
TEST(TrajectoryConstraintsClamp, clampToXYNormVecZero)
{
// GIVEN: a vector
Vector3f vec(0, 0, 0);
// WHEN: we clamp it on XY
clampToXYNorm(vec, 1.f);
// THEN: it should be 0 still
EXPECT_TRUE(vec == Vector3f(0.f, 0.f, 0.f));
}
TEST(TrajectoryConstraintsClamp, clampToZNormVecZero)
{
// GIVEN: a vector
Vector3f vec(0, 0, 0);
// WHEN: we clamp it on Z
clampToZNorm(vec, 1.f);
// THEN: it should be 0 still
EXPECT_TRUE(vec == Vector3f(0.f, 0.f, 0.f));
}
TEST(TrajectoryConstraintsClamp, clampToXYNormVecZeroToZero)
{
// GIVEN: a vector
Vector3f vec(0, 0, 0);
// WHEN: we clamp it on XY
clampToXYNorm(vec, 0.f);
// THEN: it should be 0 still
EXPECT_TRUE(vec == Vector3f(0.f, 0.f, 0.f));
}
TEST(TrajectoryConstraintsClamp, clampToZNormVecZeroToZero)
{
// GIVEN: a vector
Vector3f vec(0, 0, 0);
// WHEN: we clamp it on XY
clampToZNorm(vec, 0.f);
// THEN: it should be 0 still
EXPECT_TRUE(vec == Vector3f(0.f, 0.f, 0.f));
}
src/lib/flight_tasks/tasks/AutoMapper/CMakeLists.txt
-39
View File
@@ -1,39 +0,0 @@
############################################################################
#
# Copyright (c) 2018 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_library(FlightTaskAutoMapper
FlightTaskAutoMapper.cpp
)
target_link_libraries(FlightTaskAutoMapper PUBLIC FlightTaskAuto)
target_include_directories(FlightTaskAutoMapper PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp
-196
View File
@@ -1,196 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file FlightAutoMapper.cpp
*/
#include "FlightTaskAutoMapper.hpp"
#include <mathlib/mathlib.h>
using namespace matrix;
FlightTaskAutoMapper::FlightTaskAutoMapper() :
_sticks(this)
{}
bool FlightTaskAutoMapper::activate(const vehicle_local_position_setpoint_s &last_setpoint)
{
bool ret = FlightTaskAuto::activate(last_setpoint);
_reset();
return ret;
}
bool FlightTaskAutoMapper::update()
{
bool ret = FlightTaskAuto::update();
// always reset constraints because they might change depending on the type
_setDefaultConstraints();
// The only time a thrust set-point is sent out is during
// idle. Hence, reset thrust set-point to NAN in case the
// vehicle exits idle.
if (_type_previous == WaypointType::idle) {
_acceleration_setpoint.setNaN();
}
// during mission and reposition, raise the landing gears but only
// if altitude is high enough
if (_highEnoughForLandingGear()) {
_gear.landing_gear = landing_gear_s::GEAR_UP;
}
switch (_type) {
case WaypointType::idle:
_prepareIdleSetpoints();
break;
case WaypointType::land:
_prepareLandSetpoints();
break;
case WaypointType::loiter:
/* fallthrought */
case WaypointType::position:
_preparePositionSetpoints();
break;
case WaypointType::takeoff:
_prepareTakeoffSetpoints();
break;
case WaypointType::velocity:
_prepareVelocitySetpoints();
break;
default:
_preparePositionSetpoints();
break;
}
if (_param_com_obs_avoid.get()) {
_obstacle_avoidance.updateAvoidanceDesiredSetpoints(_position_setpoint, _velocity_setpoint, (int)_type);
_obstacle_avoidance.injectAvoidanceSetpoints(_position_setpoint, _velocity_setpoint, _yaw_setpoint,
_yawspeed_setpoint);
}
_generateSetpoints();
// update previous type
_type_previous = _type;
return ret;
}
void FlightTaskAutoMapper::_reset()
{
// Set setpoints equal current state.
_velocity_setpoint = _velocity;
_position_setpoint = _position;
}
void FlightTaskAutoMapper::_prepareIdleSetpoints()
{
// Send zero thrust setpoint
_position_setpoint.setNaN(); // Don't require any position/velocity setpoints
_velocity_setpoint.setNaN();
_acceleration_setpoint = Vector3f(0.f, 0.f, 100.f); // High downwards acceleration to make sure there's no thrust
}
void FlightTaskAutoMapper::_prepareLandSetpoints()
{
// Keep xy-position and go down with landspeed
float land_speed = _getLandSpeed();
_position_setpoint = Vector3f(_target(0), _target(1), NAN);
_velocity_setpoint = Vector3f(Vector3f(NAN, NAN, land_speed));
_gear.landing_gear = landing_gear_s::GEAR_DOWN;
}
void FlightTaskAutoMapper::_prepareTakeoffSetpoints()
{
// Takeoff is completely defined by target position
_position_setpoint = _target;
_velocity_setpoint = Vector3f(NAN, NAN, NAN);
_gear.landing_gear = landing_gear_s::GEAR_DOWN;
}
void FlightTaskAutoMapper::_prepareVelocitySetpoints()
{
// XY Velocity waypoint
// TODO : Rewiew that. What is the expected behavior?
_position_setpoint = Vector3f(NAN, NAN, _position(2));
Vector2f vel_sp_xy = Vector2f(_velocity).unit_or_zero() * _mc_cruise_speed;
_velocity_setpoint = Vector3f(vel_sp_xy(0), vel_sp_xy(1), NAN);
}
void FlightTaskAutoMapper::_preparePositionSetpoints()
{
// Simple waypoint navigation: go to xyz target, with standard limitations
_position_setpoint = _target;
_velocity_setpoint.setNaN(); // No special velocity limitations
}
void FlightTaskAutoMapper::updateParams()
{
FlightTaskAuto::updateParams();
// make sure that alt1 is above alt2
_param_mpc_land_alt1.set(math::max(_param_mpc_land_alt1.get(), _param_mpc_land_alt2.get()));
}
bool FlightTaskAutoMapper::_highEnoughForLandingGear()
{
// return true if altitude is above two meters
return _dist_to_ground > 2.0f;
}
float FlightTaskAutoMapper::_getLandSpeed()
{
float land_speed = math::gradual(_dist_to_ground,
_param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(),
_param_mpc_land_speed.get(), _constraints.speed_down);
// user input assisted land speed
if (_param_mpc_land_rc_help.get()
&& (_dist_to_ground < _param_mpc_land_alt1.get())
&& _sticks.checkAndSetStickInputs()) {
// stick full up -1 -> stop, stick full down 1 -> double the speed
land_speed *= (1 + _sticks.getPositionExpo()(2));
}
return land_speed;
}
src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.hpp
-85
View File
@@ -1,85 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file FlightTaskAutoMapper.hpp
*
* Abstract Flight task which generates local setpoints
* based on the triplet type.
*/
#pragma once
#include "FlightTaskAuto.hpp"
#include "Sticks.hpp"
class FlightTaskAutoMapper : public FlightTaskAuto
{
public:
FlightTaskAutoMapper();
virtual ~FlightTaskAutoMapper() = default;
bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
bool update() override;
protected:
virtual void _generateSetpoints() = 0; /**< Generate velocity and position setpoint for following line. */
void _prepareIdleSetpoints();
void _prepareLandSetpoints();
void _prepareVelocitySetpoints();
void _prepareTakeoffSetpoints();
void _preparePositionSetpoints();
void updateParams() override; /**< See ModuleParam class */
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAuto,
(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
(ParamFloat<px4::params::MPC_TILTMAX_LND>) _param_mpc_tiltmax_lnd,
(ParamInt<px4::params::MPC_LAND_RC_HELP>) _param_mpc_land_rc_help,
(ParamFloat<px4::params::MPC_LAND_ALT1>)
_param_mpc_land_alt1, // altitude at which speed limit downwards reaches maximum speed
(ParamFloat<px4::params::MPC_LAND_ALT2>)
_param_mpc_land_alt2, // altitude at which speed limit downwards reached minimum speed
(ParamFloat<px4::params::MPC_TKO_SPEED>) _param_mpc_tko_speed,
(ParamFloat<px4::params::MPC_TKO_RAMP_T>)
_param_mpc_tko_ramp_t // time constant for smooth takeoff ramp
);
private:
Sticks _sticks;
void _reset(); /**< Resets member variables to current vehicle state */
WaypointType _type_previous{WaypointType::idle}; /**< Previous type of current target triplet. */
bool _highEnoughForLandingGear(); /**< Checks if gears can be lowered. */
float _getLandSpeed(); /**< Returns landing descent speed. */
};
src/lib/flight_tasks/tasks/CMakeLists.txt
-43
View File
@@ -1,43 +0,0 @@
############################################################################
#
# Copyright (c) 2018 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
# add the core Flight tasks that the additional flight tasks depend on
add_subdirectory(FlightTask)
add_subdirectory(Utility)
add_subdirectory(Auto)
add_subdirectory(AutoMapper)
# add all additional flight tasks
foreach(task ${flight_tasks_all})
add_subdirectory(${task})
endforeach()
src/lib/flight_tasks/tasks/Descend/CMakeLists.txt
-39
View File
@@ -1,39 +0,0 @@
############################################################################
#
# Copyright (c) 2019 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.
#
############################################################################
px4_add_library(FlightTaskDescend
FlightTaskDescend.cpp
)
target_link_libraries(FlightTaskDescend PUBLIC FlightTask)
target_include_directories(FlightTaskDescend PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
src/lib/flight_tasks/tasks/Descend/FlightTaskDescend.cpp
-65
View File
@@ -1,65 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019 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 FlightTaskDescend.cpp
*/
#include "FlightTaskDescend.hpp"
bool FlightTaskDescend::activate(const vehicle_local_position_setpoint_s &last_setpoint)
{
bool ret = FlightTask::activate(last_setpoint);
// stay level to minimize horizontal drift
_acceleration_setpoint = matrix::Vector3f(0.f, 0.f, NAN);
// keep heading
_yaw_setpoint = _yaw;
return ret;
}
bool FlightTaskDescend::update()
{
bool ret = FlightTask::update();
if (PX4_ISFINITE(_velocity(2))) {
// land with landspeed
_velocity_setpoint(2) = _param_mpc_land_speed.get();
_acceleration_setpoint(2) = NAN;
} else {
// descend with constant acceleration (crash landing)
_velocity_setpoint(2) = NAN;
_acceleration_setpoint(2) = .3f;
}
return ret;
}
src/lib/flight_tasks/tasks/Descend/FlightTaskDescend.hpp
-56
View File
@@ -1,56 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019 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 FlightTaskDescend.hpp
*/
#pragma once
#include "FlightTask.hpp"
class FlightTaskDescend : public FlightTask
{
public:
FlightTaskDescend() = default;
virtual ~FlightTaskDescend() = default;
bool update() override;
bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
private:
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
(ParamFloat<px4::params::MPC_THR_HOVER>) _param_mpc_thr_hover, ///< thrust at hover equilibrium
(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed ///< velocity for controlled descend
)
};
src/lib/flight_tasks/tasks/Failsafe/CMakeLists.txt
-39
View File
@@ -1,39 +0,0 @@
############################################################################
#
# Copyright (c) 2018 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_library(FlightTaskFailsafe
FlightTaskFailsafe.cpp
)
target_link_libraries(FlightTaskFailsafe PUBLIC FlightTask)
target_include_directories(FlightTaskFailsafe PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
src/lib/flight_tasks/tasks/Failsafe/FlightTaskFailsafe.cpp
-78
View File
@@ -1,78 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file FlightTaskFailsafe.cpp
*/
#include "FlightTaskFailsafe.hpp"
bool FlightTaskFailsafe::activate(const vehicle_local_position_setpoint_s &last_setpoint)
{
bool ret = FlightTask::activate(last_setpoint);
_position_setpoint = _position;
_velocity_setpoint.zero();
_acceleration_setpoint = matrix::Vector3f(0.f, 0.f, .3f);
_yaw_setpoint = _yaw;
_yawspeed_setpoint = 0.f;
return ret;
}
bool FlightTaskFailsafe::update()
{
bool ret = FlightTask::update();
if (PX4_ISFINITE(_position(0)) && PX4_ISFINITE(_position(1))) {
// stay at current position setpoint
_velocity_setpoint(0) = _velocity_setpoint(1) = 0.f;
_acceleration_setpoint(0) = _acceleration_setpoint(1) = 0.f;
} else if (PX4_ISFINITE(_velocity(0)) && PX4_ISFINITE(_velocity(1))) {
// don't move along xy
_position_setpoint(0) = _position_setpoint(1) = NAN;
_acceleration_setpoint(0) = _acceleration_setpoint(1) = NAN;
}
if (PX4_ISFINITE(_position(2))) {
// stay at current altitude setpoint
_velocity_setpoint(2) = 0.f;
_acceleration_setpoint(2) = NAN;
} else if (PX4_ISFINITE(_velocity(2))) {
// land with landspeed
_velocity_setpoint(2) = _param_mpc_land_speed.get();
_position_setpoint(2) = NAN;
_acceleration_setpoint(2) = NAN;
}
return ret;
}
src/lib/flight_tasks/tasks/Failsafe/FlightTaskFailsafe.hpp
-56
View File
@@ -1,56 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file FlightTaskFailsafe.hpp
*
*/
#pragma once
#include "FlightTask.hpp"
class FlightTaskFailsafe : public FlightTask
{
public:
FlightTaskFailsafe() = default;
virtual ~FlightTaskFailsafe() = default;
bool update() override;
bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
private:
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed
)
};
src/lib/flight_tasks/tasks/FlightTask/CMakeLists.txt
-38
View File
@@ -1,38 +0,0 @@
############################################################################
#
# Copyright (c) 2018 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_library(FlightTask
FlightTask.cpp
)
target_include_directories(FlightTask PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
src/lib/flight_tasks/tasks/FlightTask/FlightTask.cpp
-207
View File
@@ -1,207 +0,0 @@
#include "FlightTask.hpp"
#include <mathlib/mathlib.h>
#include <lib/ecl/geo/geo.h>
constexpr uint64_t FlightTask::_timeout;
// First index of empty_setpoint corresponds to time-stamp and requires a finite number.
const vehicle_local_position_setpoint_s FlightTask::empty_setpoint = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {}};
const ekf_reset_counters_s FlightTask::zero_reset_counters = {};
const vehicle_constraints_s FlightTask::empty_constraints = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, false, {}};
const landing_gear_s FlightTask::empty_landing_gear_default_keep = {0, landing_gear_s::GEAR_KEEP, {}};
bool FlightTask::activate(const vehicle_local_position_setpoint_s &last_setpoint)
{
_resetSetpoints();
_setDefaultConstraints();
_time_stamp_activate = hrt_absolute_time();
_gear = empty_landing_gear_default_keep;
return true;
}
void FlightTask::reActivate()
{
activate(getPositionSetpoint());
}
bool FlightTask::updateInitialize()
{
_time_stamp_current = hrt_absolute_time();
_time = (_time_stamp_current - _time_stamp_activate) / 1e6f;
_deltatime = math::min((_time_stamp_current - _time_stamp_last), _timeout) / 1e6f;
_time_stamp_last = _time_stamp_current;
_sub_vehicle_local_position.update();
_sub_home_position.update();
_evaluateVehicleLocalPosition();
_evaluateDistanceToGround();
return true;
}
bool FlightTask::update()
{
_checkEkfResetCounters();
return true;
}
void FlightTask::_checkEkfResetCounters()
{
// Check if a reset event has happened
if (_sub_vehicle_local_position.get().xy_reset_counter != _reset_counters.xy) {
_ekfResetHandlerPositionXY();
_reset_counters.xy = _sub_vehicle_local_position.get().xy_reset_counter;
}
if (_sub_vehicle_local_position.get().vxy_reset_counter != _reset_counters.vxy) {
_ekfResetHandlerVelocityXY();
_reset_counters.vxy = _sub_vehicle_local_position.get().vxy_reset_counter;
}
if (_sub_vehicle_local_position.get().z_reset_counter != _reset_counters.z) {
_ekfResetHandlerPositionZ();
_reset_counters.z = _sub_vehicle_local_position.get().z_reset_counter;
}
if (_sub_vehicle_local_position.get().vz_reset_counter != _reset_counters.vz) {
_ekfResetHandlerVelocityZ();
_reset_counters.vz = _sub_vehicle_local_position.get().vz_reset_counter;
}
if (_sub_vehicle_local_position.get().heading_reset_counter != _reset_counters.heading) {
_ekfResetHandlerHeading(_sub_vehicle_local_position.get().delta_heading);
_reset_counters.heading = _sub_vehicle_local_position.get().heading_reset_counter;
}
}
const vehicle_local_position_setpoint_s FlightTask::getPositionSetpoint()
{
/* fill position setpoint message */
vehicle_local_position_setpoint_s vehicle_local_position_setpoint{};
vehicle_local_position_setpoint.timestamp = hrt_absolute_time();
vehicle_local_position_setpoint.x = _position_setpoint(0);
vehicle_local_position_setpoint.y = _position_setpoint(1);
vehicle_local_position_setpoint.z = _position_setpoint(2);
vehicle_local_position_setpoint.vx = _velocity_setpoint(0);
vehicle_local_position_setpoint.vy = _velocity_setpoint(1);
vehicle_local_position_setpoint.vz = _velocity_setpoint(2);
_acceleration_setpoint.copyTo(vehicle_local_position_setpoint.acceleration);
_jerk_setpoint.copyTo(vehicle_local_position_setpoint.jerk);
vehicle_local_position_setpoint.yaw = _yaw_setpoint;
vehicle_local_position_setpoint.yawspeed = _yawspeed_setpoint;
// deprecated, only kept for output logging
matrix::Vector3f(NAN, NAN, NAN).copyTo(vehicle_local_position_setpoint.thrust);
return vehicle_local_position_setpoint;
}
void FlightTask::_resetSetpoints()
{
_position_setpoint.setNaN();
_velocity_setpoint.setNaN();
_acceleration_setpoint.setNaN();
_jerk_setpoint.setNaN();
_yaw_setpoint = _yawspeed_setpoint = NAN;
}
void FlightTask::_evaluateVehicleLocalPosition()
{
_position.setAll(NAN);
_velocity.setAll(NAN);
_yaw = NAN;
_dist_to_bottom = NAN;
// Only use vehicle-local-position topic fields if the topic is received within a certain timestamp
if ((_time_stamp_current - _sub_vehicle_local_position.get().timestamp) < _timeout) {
// yaw
_yaw = _sub_vehicle_local_position.get().heading;
// position
if (_sub_vehicle_local_position.get().xy_valid) {
_position(0) = _sub_vehicle_local_position.get().x;
_position(1) = _sub_vehicle_local_position.get().y;
}
if (_sub_vehicle_local_position.get().z_valid) {
_position(2) = _sub_vehicle_local_position.get().z;
}
// velocity
if (_sub_vehicle_local_position.get().v_xy_valid) {
_velocity(0) = _sub_vehicle_local_position.get().vx;
_velocity(1) = _sub_vehicle_local_position.get().vy;
}
if (_sub_vehicle_local_position.get().v_z_valid) {
_velocity(2) = _sub_vehicle_local_position.get().vz;
}
// distance to bottom
if (_sub_vehicle_local_position.get().dist_bottom_valid
&& PX4_ISFINITE(_sub_vehicle_local_position.get().dist_bottom)) {
_dist_to_bottom = _sub_vehicle_local_position.get().dist_bottom;
}
// global frame reference coordinates to enable conversions
if (_sub_vehicle_local_position.get().xy_global && _sub_vehicle_local_position.get().z_global) {
globallocalconverter_init(_sub_vehicle_local_position.get().ref_lat, _sub_vehicle_local_position.get().ref_lon,
_sub_vehicle_local_position.get().ref_alt, _sub_vehicle_local_position.get().ref_timestamp);
}
}
}
void FlightTask::_evaluateDistanceToGround()
{
// Altitude above ground is local z-position or altitude above home or distance sensor altitude depending on what's available
_dist_to_ground = -_position(2);
if (PX4_ISFINITE(_dist_to_bottom)) {
_dist_to_ground = _dist_to_bottom;
} else if (_sub_home_position.get().valid_alt) {
_dist_to_ground = -(_position(2) - _sub_home_position.get().z);
}
}
void FlightTask::_setDefaultConstraints()
{
_constraints.speed_xy = _param_mpc_xy_vel_max.get();
_constraints.speed_up = _param_mpc_z_vel_max_up.get();
_constraints.speed_down = _param_mpc_z_vel_max_dn.get();
_constraints.tilt = NAN;
_constraints.min_distance_to_ground = NAN;
_constraints.max_distance_to_ground = NAN;
_constraints.want_takeoff = false;
}
bool FlightTask::_checkTakeoff()
{
// position setpoint above the minimum altitude
bool position_triggered_takeoff = false;
if (PX4_ISFINITE(_position_setpoint(2))) {
// minimal altitude either 20cm or what is necessary for correct estimation e.g. optical flow
float min_altitude = 0.2f;
const float min_distance_to_ground = _sub_vehicle_local_position.get().hagl_min;
if (PX4_ISFINITE(min_distance_to_ground)) {
min_altitude = min_distance_to_ground + 0.05f;
}
position_triggered_takeoff = _position_setpoint(2) < (_position(2) - min_altitude);
}
// upwards velocity setpoint
bool velocity_triggered_takeoff = false;
if (PX4_ISFINITE(_velocity_setpoint(2))) {
velocity_triggered_takeoff = _velocity_setpoint(2) < -0.3f;
}
return position_triggered_takeoff || velocity_triggered_takeoff;
}
src/lib/flight_tasks/tasks/FlightTask/FlightTask.hpp
-273
View File
@@ -1,273 +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 FlightTask.hpp
*
* Abstract base class for different advanced flight tasks like orbit, follow me, ...
*
* @author Matthias Grob <maetugr@gmail.com>
*/
#pragma once
#include <px4_platform_common/module_params.h>
#include <drivers/drv_hrt.h>
#include <matrix/matrix/math.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/landing_gear.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_constraints.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_trajectory_waypoint.h>
#include <uORB/topics/home_position.h>
#include <lib/weather_vane/WeatherVane.hpp>
struct ekf_reset_counters_s {
uint8_t xy;
uint8_t vxy;
uint8_t z;
uint8_t vz;
uint8_t heading;
};
class FlightTask : public ModuleParams
{
public:
FlightTask() :
ModuleParams(nullptr)
{
_resetSetpoints();
_constraints = empty_constraints;
}
virtual ~FlightTask() = default;
/**
* Call once on the event where you switch to the task
* @param last_setpoint last output of the previous task
* @return true on success, false on error
*/
virtual bool activate(const vehicle_local_position_setpoint_s &last_setpoint);
/**
* Call this to reset an active Flight Task
*/
virtual void reActivate();
/**
* To be called to adopt parameters from an arrived vehicle command
* @param command received command message containing the parameters
* @return true if accepted, false if declined
*/
virtual bool applyCommandParameters(const vehicle_command_s &command) { return false; }
/**
* Call before activate() or update()
* to initialize time and input data
* @return true on success, false on error
*/
virtual bool updateInitialize();
/**
* To be called regularly in the control loop cycle to execute the task
* @return true on success, false on error
*/
virtual bool update();
/**
* Call after update()
* to constrain the generated setpoints in order to comply
* with the constraints of the current mode
* @return true on success, false on error
*/
virtual bool updateFinalize() { return true; };
/**
* Get the output data
* @return task output setpoints that get executed by the positon controller
*/
const vehicle_local_position_setpoint_s getPositionSetpoint();
const ekf_reset_counters_s getResetCounters() const { return _reset_counters; }
void setResetCounters(const ekf_reset_counters_s &counters) { _reset_counters = counters; }
/**
* Get vehicle constraints.
* The constraints can vary with task.
* @return constraints
*/
const vehicle_constraints_s &getConstraints() { return _constraints; }
/**
* Get landing gear position.
* The constraints can vary with task.
* @return landing gear
*/
const landing_gear_s &getGear() { return _gear; }
/**
* Get avoidance desired waypoint
* @return desired waypoints
*/
const vehicle_trajectory_waypoint_s &getAvoidanceWaypoint() { return _desired_waypoint; }
/**
* Empty setpoint.
* All setpoints are set to NAN.
*/
static const vehicle_local_position_setpoint_s empty_setpoint;
/**.
* All counters are set to 0.
*/
static const ekf_reset_counters_s zero_reset_counters;
/**
* Empty constraints.
* All constraints are set to NAN.
*/
static const vehicle_constraints_s empty_constraints;
/**
* default landing gear state
*/
static const landing_gear_s empty_landing_gear_default_keep;
/**
* Call this whenever a parameter update notification is received (parameter_update uORB message)
*/
void handleParameterUpdate()
{
updateParams();
}
/**
* Sets an external yaw handler which can be used by any flight task to implement a different yaw control strategy.
* This method does nothing, each flighttask which wants to use the yaw handler needs to override this method.
*/
virtual void setYawHandler(WeatherVane *ext_yaw_handler) {}
void updateVelocityControllerIO(const matrix::Vector3f &vel_sp,
const matrix::Vector3f &acc_sp)
{
_velocity_setpoint_feedback = vel_sp;
_acceleration_setpoint_feedback = acc_sp;
}
protected:
uORB::SubscriptionData<vehicle_local_position_s> _sub_vehicle_local_position{ORB_ID(vehicle_local_position)};
uORB::SubscriptionData<home_position_s> _sub_home_position{ORB_ID(home_position)};
/** Reset all setpoints to NAN */
void _resetSetpoints();
/** Check and update local position */
void _evaluateVehicleLocalPosition();
void _evaluateDistanceToGround();
/** Set constraints to default values */
virtual void _setDefaultConstraints();
/** Determine when to trigger a takeoff (ignored in flight) */
virtual bool _checkTakeoff();
/**
* Monitor the EKF reset counters and
* call the appropriate handling functions in case of a reset event
* TODO: add the delta values to all the handlers
*/
void _checkEkfResetCounters();
virtual void _ekfResetHandlerPositionXY() {};
virtual void _ekfResetHandlerVelocityXY() {};
virtual void _ekfResetHandlerPositionZ() {};
virtual void _ekfResetHandlerVelocityZ() {};
virtual void _ekfResetHandlerHeading(float delta_psi) {};
/* Time abstraction */
static constexpr uint64_t _timeout = 500000; /**< maximal time in us before a loop or data times out */
float _time{}; /**< passed time in seconds since the task was activated */
float _deltatime{}; /**< passed time in seconds since the task was last updated */
hrt_abstime _time_stamp_activate{}; /**< time stamp when task was activated */
hrt_abstime _time_stamp_current{}; /**< time stamp at the beginning of the current task update */
hrt_abstime _time_stamp_last{}; /**< time stamp when task was last updated */
/* Current vehicle state */
matrix::Vector3f _position; /**< current vehicle position */
matrix::Vector3f _velocity; /**< current vehicle velocity */
float _yaw{}; /**< current vehicle yaw heading */
float _dist_to_bottom{}; /**< current height above ground level */
float _dist_to_ground{}; /**< equals _dist_to_bottom if valid, height above home otherwise */
/**
* Setpoints which the position controller has to execute.
* Setpoints that are set to NAN are not controlled. Not all setpoints can be set at the same time.
* If more than one type of setpoint is set, then order of control is a as follow: position, velocity,
* acceleration, thrust. The exception is _position_setpoint together with _velocity_setpoint, where the
* _velocity_setpoint is used as feedforward.
* _acceleration_setpoint and _jerk_setpoint are currently not supported.
*/
matrix::Vector3f _position_setpoint;
matrix::Vector3f _velocity_setpoint;
matrix::Vector3f _acceleration_setpoint;
matrix::Vector3f _jerk_setpoint;
float _yaw_setpoint{};
float _yawspeed_setpoint{};
matrix::Vector3f _velocity_setpoint_feedback;
matrix::Vector3f _acceleration_setpoint_feedback;
ekf_reset_counters_s _reset_counters{}; ///< Counters for estimator local position resets
/**
* Vehicle constraints.
* The constraints can vary with tasks.
*/
vehicle_constraints_s _constraints{};
landing_gear_s _gear{};
/**
* Desired waypoints.
* Goals set by the FCU to be sent to the obstacle avoidance system.
*/
vehicle_trajectory_waypoint_s _desired_waypoint{};
DEFINE_PARAMETERS_CUSTOM_PARENT(ModuleParams,
(ParamFloat<px4::params::MPC_XY_VEL_MAX>) _param_mpc_xy_vel_max,
(ParamFloat<px4::params::MPC_Z_VEL_MAX_DN>) _param_mpc_z_vel_max_dn,
(ParamFloat<px4::params::MPC_Z_VEL_MAX_UP>) _param_mpc_z_vel_max_up
)
};
src/lib/flight_tasks/tasks/ManualAcceleration/CMakeLists.txt
-39
View File
@@ -1,39 +0,0 @@
############################################################################
#
# Copyright (c) 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.
#
############################################################################
px4_add_library(FlightTaskManualAcceleration
FlightTaskManualAcceleration.cpp
)
target_include_directories(FlightTaskManualAcceleration PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
target_link_libraries(FlightTaskManualAcceleration PUBLIC FlightTaskManualAltitudeSmoothVel FlightTaskUtility)
src/lib/flight_tasks/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp
-115
View File
@@ -1,115 +0,0 @@
/****************************************************************************
*
* Copyright (c) 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.
*
****************************************************************************/
/**
* @file FlightTaskManualAcceleration.cpp
*/
#include "FlightTaskManualAcceleration.hpp"
using namespace matrix;
FlightTaskManualAcceleration::FlightTaskManualAcceleration() :
_stick_acceleration_xy(this)
{};
bool FlightTaskManualAcceleration::activate(const vehicle_local_position_setpoint_s &last_setpoint)
{
bool ret = FlightTaskManualAltitudeSmoothVel::activate(last_setpoint);
if (PX4_ISFINITE(last_setpoint.vx)) {
_velocity_setpoint.xy() = Vector2f(last_setpoint.vx, last_setpoint.vy);
} else {
_velocity_setpoint.xy() = Vector2f(_velocity);
}
_stick_acceleration_xy.resetPosition();
if (PX4_ISFINITE(last_setpoint.acceleration[0])) {
_stick_acceleration_xy.resetAcceleration(Vector2f(last_setpoint.acceleration[0], last_setpoint.acceleration[1]));
}
return ret;
}
bool FlightTaskManualAcceleration::update()
{
bool ret = FlightTaskManualAltitudeSmoothVel::update();
_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint,
_sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get()), _yaw, _deltatime);
_stick_acceleration_xy.generateSetpoints(_sticks.getPositionExpo().slice<2, 1>(0, 0), _yaw, _yaw_setpoint, _position,
_deltatime);
_stick_acceleration_xy.getSetpoints(_position_setpoint, _velocity_setpoint, _acceleration_setpoint);
_constraints.want_takeoff = _checkTakeoff();
return ret;
}
void FlightTaskManualAcceleration::_ekfResetHandlerPositionXY()
{
if (PX4_ISFINITE(_position_setpoint(0))) {
_position_setpoint(0) = _position(0);
_position_setpoint(1) = _position(1);
}
}
void FlightTaskManualAcceleration::_ekfResetHandlerVelocityXY()
{
if (PX4_ISFINITE(_velocity_setpoint(0))) {
_velocity_setpoint(0) = _velocity(0);
_velocity_setpoint(1) = _velocity(1);
}
}
void FlightTaskManualAcceleration::_ekfResetHandlerPositionZ()
{
if (PX4_ISFINITE(_position_setpoint(2))) {
_position_setpoint(2) = _position(2);
}
}
void FlightTaskManualAcceleration::_ekfResetHandlerVelocityZ()
{
if (PX4_ISFINITE(_velocity_setpoint(2))) {
_velocity_setpoint(2) = _velocity(2);
}
}
void FlightTaskManualAcceleration::_ekfResetHandlerHeading(float delta_psi)
{
if (PX4_ISFINITE(_yaw_setpoint)) {
_yaw_setpoint += delta_psi;
}
}
src/lib/flight_tasks/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp
-64
View File
@@ -1,64 +0,0 @@
/****************************************************************************
*
* Copyright (c) 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.
*
****************************************************************************/
/**
* @file FlightTaskManualPosition.hpp
*
* Flight task for manual position controlled mode.
*
*/
#pragma once
#include "FlightTaskManualAltitudeSmoothVel.hpp"
#include "StickAccelerationXY.hpp"
#include "StickYaw.hpp"
class FlightTaskManualAcceleration : public FlightTaskManualAltitudeSmoothVel
{
public:
FlightTaskManualAcceleration();
virtual ~FlightTaskManualAcceleration() = default;
bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
bool update() override;
private:
StickAccelerationXY _stick_acceleration_xy;
StickYaw _stick_yaw;
void _ekfResetHandlerPositionXY() override;
void _ekfResetHandlerVelocityXY() override;
void _ekfResetHandlerPositionZ() override;
void _ekfResetHandlerVelocityZ() override;
void _ekfResetHandlerHeading(float delta_psi) override;
};
src/lib/flight_tasks/tasks/ManualAltitude/CMakeLists.txt
-39
View File
@@ -1,39 +0,0 @@
############################################################################
#
# Copyright (c) 2018 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_library(FlightTaskManualAltitude
FlightTaskManualAltitude.cpp
)
target_link_libraries(FlightTaskManualAltitude PUBLIC FlightTask FlightTaskUtility)
target_include_directories(FlightTaskManualAltitude PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
-385
View File
@@ -1,385 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file FlightManualAltitude.cpp
*/
#include "FlightTaskManualAltitude.hpp"
#include <float.h>
#include <mathlib/mathlib.h>
#include <ecl/geo/geo.h>
using namespace matrix;
FlightTaskManualAltitude::FlightTaskManualAltitude() :
_sticks(this)
{}
bool FlightTaskManualAltitude::updateInitialize()
{
bool ret = FlightTask::updateInitialize();
_sticks.checkAndSetStickInputs();
if (_sticks_data_required) {
ret = ret && _sticks.isAvailable();
}
// in addition to manual require valid position and velocity in D-direction and valid yaw
return ret && PX4_ISFINITE(_position(2)) && PX4_ISFINITE(_velocity(2)) && PX4_ISFINITE(_yaw);
}
bool FlightTaskManualAltitude::activate(const vehicle_local_position_setpoint_s &last_setpoint)
{
bool ret = FlightTask::activate(last_setpoint);
_yaw_setpoint = NAN;
_yawspeed_setpoint = 0.f;
_acceleration_setpoint = Vector3f(0.f, 0.f, NAN); // altitude is controlled from position/velocity
_position_setpoint(2) = _position(2);
_velocity_setpoint(2) = 0.f;
_setDefaultConstraints();
_updateConstraintsFromEstimator();
_max_speed_up = _constraints.speed_up;
_max_speed_down = _constraints.speed_down;
return ret;
}
void FlightTaskManualAltitude::_updateConstraintsFromEstimator()
{
if (PX4_ISFINITE(_sub_vehicle_local_position.get().hagl_min)) {
_constraints.min_distance_to_ground = _sub_vehicle_local_position.get().hagl_min;
} else {
_constraints.min_distance_to_ground = -INFINITY;
}
if (PX4_ISFINITE(_sub_vehicle_local_position.get().hagl_max)) {
_constraints.max_distance_to_ground = _sub_vehicle_local_position.get().hagl_max;
} else {
_constraints.max_distance_to_ground = INFINITY;
}
}
void FlightTaskManualAltitude::_scaleSticks()
{
// Use stick input with deadzone, exponential curve and first order lpf for yawspeed
const float yawspeed_target = _sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get());
_yawspeed_setpoint = _applyYawspeedFilter(yawspeed_target);
// Use sticks input with deadzone and exponential curve for vertical velocity
const float vel_max_z = (_sticks.getPosition()(2) > 0.0f) ? _constraints.speed_down : _constraints.speed_up;
_velocity_setpoint(2) = vel_max_z * _sticks.getPositionExpo()(2);
}
float FlightTaskManualAltitude::_applyYawspeedFilter(float yawspeed_target)
{
const float den = math::max(_param_mpc_man_y_tau.get() + _deltatime, 0.001f);
const float alpha = _deltatime / den;
_yawspeed_filter_state = (1.f - alpha) * _yawspeed_filter_state + alpha * yawspeed_target;
return _yawspeed_filter_state;
}
void FlightTaskManualAltitude::_updateAltitudeLock()
{
// Depending on stick inputs and velocity, position is locked.
// If not locked, altitude setpoint is set to NAN.
// Check if user wants to break
const bool apply_brake = fabsf(_sticks.getPositionExpo()(2)) <= FLT_EPSILON;
// Check if vehicle has stopped
const bool stopped = (_param_mpc_hold_max_z.get() < FLT_EPSILON || fabsf(_velocity(2)) < _param_mpc_hold_max_z.get());
// Manage transition between use of distance to ground and distance to local origin
// when terrain hold behaviour has been selected.
if (_param_mpc_alt_mode.get() == 2) {
// Use horizontal speed as a transition criteria
float spd_xy = Vector2f(_velocity).length();
// Use presence of horizontal stick inputs as a transition criteria
float stick_xy = Vector2f(_sticks.getPositionExpo().slice<2, 1>(0, 0)).length();
bool stick_input = stick_xy > 0.001f;
if (_terrain_hold) {
bool too_fast = spd_xy > _param_mpc_hold_max_xy.get();
if (stick_input || too_fast || !PX4_ISFINITE(_dist_to_bottom)) {
// Stop using distance to ground
_terrain_hold = false;
_terrain_follow = false;
// Adjust the setpoint to maintain the same height error to reduce control transients
if (PX4_ISFINITE(_dist_to_ground_lock) && PX4_ISFINITE(_dist_to_bottom)) {
_position_setpoint(2) = _position(2) - (_dist_to_ground_lock - _dist_to_bottom);
} else {
_position_setpoint(2) = _position(2);
}
}
} else {
bool not_moving = spd_xy < 0.5f * _param_mpc_hold_max_xy.get();
if (!stick_input && not_moving && PX4_ISFINITE(_dist_to_bottom)) {
// Start using distance to ground
_terrain_hold = true;
_terrain_follow = true;
// Adjust the setpoint to maintain the same height error to reduce control transients
if (PX4_ISFINITE(_position_setpoint(2))) {
_dist_to_ground_lock = _dist_to_bottom - (_position_setpoint(2) - _position(2));
}
}
}
}
if ((_param_mpc_alt_mode.get() == 1 || _terrain_follow) && PX4_ISFINITE(_dist_to_bottom)) {
// terrain following
_terrainFollowing(apply_brake, stopped);
// respect maximum altitude
_respectMaxAltitude();
} else {
// normal mode where height is dependent on local frame
if (apply_brake && stopped && !PX4_ISFINITE(_position_setpoint(2))) {
// lock position
_position_setpoint(2) = _position(2);
// Ensure that minimum altitude is respected if
// there is a distance sensor and distance to bottom is below minimum.
if (PX4_ISFINITE(_dist_to_bottom) && _dist_to_bottom < _constraints.min_distance_to_ground) {
_terrainFollowing(apply_brake, stopped);
} else {
_dist_to_ground_lock = NAN;
}
} else if (PX4_ISFINITE(_position_setpoint(2)) && apply_brake) {
// Position is locked but check if a reset event has happened.
// We will shift the setpoints.
if (_sub_vehicle_local_position.get().z_reset_counter != _reset_counter) {
_position_setpoint(2) = _position(2);
_reset_counter = _sub_vehicle_local_position.get().z_reset_counter;
}
} else {
// user demands velocity change
_position_setpoint(2) = NAN;
// ensure that maximum altitude is respected
_respectMaxAltitude();
}
}
}
void FlightTaskManualAltitude::_respectMinAltitude()
{
const bool respectAlt = PX4_ISFINITE(_dist_to_bottom)
&& _dist_to_bottom < _constraints.min_distance_to_ground;
// Height above ground needs to be limited (flow / range-finder)
if (respectAlt) {
// increase altitude to minimum flow distance
_position_setpoint(2) = _position(2)
- (_constraints.min_distance_to_ground - _dist_to_bottom);
}
}
void FlightTaskManualAltitude::_terrainFollowing(bool apply_brake, bool stopped)
{
if (apply_brake && stopped && !PX4_ISFINITE(_dist_to_ground_lock)) {
// User wants to break and vehicle reached zero velocity. Lock height to ground.
// lock position
_position_setpoint(2) = _position(2);
// ensure that minimum altitude is respected
_respectMinAltitude();
// lock distance to ground but adjust first for minimum altitude
_dist_to_ground_lock = _dist_to_bottom - (_position_setpoint(2) - _position(2));
} else if (apply_brake && PX4_ISFINITE(_dist_to_ground_lock)) {
// vehicle needs to follow terrain
// difference between the current distance to ground and the desired distance to ground
const float delta_distance_to_ground = _dist_to_ground_lock - _dist_to_bottom;
// adjust position setpoint for the delta (note: NED frame)
_position_setpoint(2) = _position(2) - delta_distance_to_ground;
} else {
// user demands velocity change in D-direction
_dist_to_ground_lock = _position_setpoint(2) = NAN;
}
}
void FlightTaskManualAltitude::_respectMaxAltitude()
{
if (PX4_ISFINITE(_dist_to_bottom)) {
// if there is a valid maximum distance to ground, linearly increase speed limit with distance
// below the maximum, preserving control loop vertical position error gain.
if (PX4_ISFINITE(_constraints.max_distance_to_ground)) {
_constraints.speed_up = math::constrain(_param_mpc_z_p.get() * (_constraints.max_distance_to_ground - _dist_to_bottom),
-_max_speed_down, _max_speed_up);
} else {
_constraints.speed_up = _max_speed_up;
}
// if distance to bottom exceeded maximum distance, slowly approach maximum distance
if (_dist_to_bottom > _constraints.max_distance_to_ground) {
// difference between current distance to ground and maximum distance to ground
const float delta_distance_to_max = _dist_to_bottom - _constraints.max_distance_to_ground;
// set position setpoint to maximum distance to ground
_position_setpoint(2) = _position(2) + delta_distance_to_max;
// limit speed downwards to 0.7m/s
_constraints.speed_down = math::min(_max_speed_down, 0.7f);
} else {
_constraints.speed_down = _max_speed_down;
}
}
}
void FlightTaskManualAltitude::_respectGroundSlowdown()
{
// limit speed gradually within the altitudes MPC_LAND_ALT1 and MPC_LAND_ALT2
if (PX4_ISFINITE(_dist_to_ground)) {
const float limit_down = math::gradual(_dist_to_ground,
_param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(),
_param_mpc_land_speed.get(), _constraints.speed_down);
const float limit_up = math::gradual(_dist_to_ground,
_param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(),
_param_mpc_tko_speed.get(), _constraints.speed_up);
_velocity_setpoint(2) = math::constrain(_velocity_setpoint(2), -limit_up, limit_down);
}
}
void FlightTaskManualAltitude::_rotateIntoHeadingFrame(Vector2f &v)
{
float yaw_rotate = PX4_ISFINITE(_yaw_setpoint) ? _yaw_setpoint : _yaw;
Vector3f v_r = Vector3f(Dcmf(Eulerf(0.0f, 0.0f, yaw_rotate)) * Vector3f(v(0), v(1), 0.0f));
v(0) = v_r(0);
v(1) = v_r(1);
}
void FlightTaskManualAltitude::_updateHeadingSetpoints()
{
if (_isYawInput()) {
_unlockYaw();
} else {
_lockYaw();
}
}
bool FlightTaskManualAltitude::_isYawInput()
{
/*
* A threshold larger than FLT_EPSILON is required because the
* _yawspeed_setpoint comes from an IIR filter and takes too much
* time to reach zero.
*/
return fabsf(_yawspeed_setpoint) > 0.001f;
}
void FlightTaskManualAltitude::_unlockYaw()
{
// no fixed heading when rotating around yaw by stick
_yaw_setpoint = NAN;
}
void FlightTaskManualAltitude::_lockYaw()
{
// hold the current heading when no more rotation commanded
if (!PX4_ISFINITE(_yaw_setpoint)) {
_yaw_setpoint = _yaw;
}
}
void FlightTaskManualAltitude::_ekfResetHandlerHeading(float delta_psi)
{
// Only reset the yaw setpoint when the heading is locked
if (PX4_ISFINITE(_yaw_setpoint)) {
_yaw_setpoint += delta_psi;
}
}
void FlightTaskManualAltitude::_updateSetpoints()
{
_updateHeadingSetpoints(); // get yaw setpoint
// Thrust in xy are extracted directly from stick inputs. A magnitude of
// 1 means that maximum thrust along xy is demanded. A magnitude of 0 means no
// thrust along xy is demanded. The maximum thrust along xy depends on the thrust
// setpoint along z-direction, which is computed in PositionControl.cpp.
Vector2f sp(_sticks.getPosition().slice<2, 1>(0, 0));
_man_input_filter.setParameters(_deltatime, _param_mc_man_tilt_tau.get());
_man_input_filter.update(sp);
sp = _man_input_filter.getState();
_rotateIntoHeadingFrame(sp);
if (sp.length() > 1.0f) {
sp.normalize();
}
_acceleration_setpoint.xy() = sp * tanf(math::radians(_param_mpc_man_tilt_max.get())) * CONSTANTS_ONE_G;
_updateAltitudeLock();
_respectGroundSlowdown();
}
bool FlightTaskManualAltitude::_checkTakeoff()
{
// stick is deflected above 65% throttle (throttle stick is in the range [-1,1])
return _sticks.getPosition()(2) < -0.3f;
}
bool FlightTaskManualAltitude::update()
{
bool ret = FlightTask::update();
_updateConstraintsFromEstimator();
_scaleSticks();
_updateSetpoints();
_constraints.want_takeoff = _checkTakeoff();
return ret;
}
src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp
-151
View File
@@ -1,151 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file FlightManualAltitude.hpp
*
* Flight task for manual controlled altitude.
*/
#pragma once
#include "FlightTask.hpp"
#include "Sticks.hpp"
#include <lib/ecl/AlphaFilter/AlphaFilter.hpp>
#include <uORB/Subscription.hpp>
class FlightTaskManualAltitude : public FlightTask
{
public:
FlightTaskManualAltitude();
virtual ~FlightTaskManualAltitude() = default;
bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
bool updateInitialize() override;
bool update() override;
protected:
void _updateHeadingSetpoints(); /**< sets yaw or yaw speed */
void _ekfResetHandlerHeading(float delta_psi) override; /**< adjust heading setpoint in case of EKF reset event */
virtual void _updateSetpoints(); /**< updates all setpoints */
virtual void _scaleSticks(); /**< scales sticks to velocity in z */
bool _checkTakeoff() override;
void _updateConstraintsFromEstimator();
/**
* rotates vector into local frame
*/
void _rotateIntoHeadingFrame(matrix::Vector2f &vec);
/**
* Check and sets for position lock.
* If sticks are at center position, the vehicle
* will exit velocity control and enter position control.
*/
void _updateAltitudeLock();
Sticks _sticks;
bool _sticks_data_required = true; ///< let inherited task-class define if it depends on stick data
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
(ParamFloat<px4::params::MPC_HOLD_MAX_Z>) _param_mpc_hold_max_z,
(ParamInt<px4::params::MPC_ALT_MODE>) _param_mpc_alt_mode,
(ParamFloat<px4::params::MPC_HOLD_MAX_XY>) _param_mpc_hold_max_xy,
(ParamFloat<px4::params::MPC_Z_P>) _param_mpc_z_p, /**< position controller altitude propotional gain */
(ParamFloat<px4::params::MPC_MAN_Y_MAX>) _param_mpc_man_y_max, /**< scaling factor from stick to yaw rate */
(ParamFloat<px4::params::MPC_MAN_Y_TAU>) _param_mpc_man_y_tau,
(ParamFloat<px4::params::MPC_MAN_TILT_MAX>) _param_mpc_man_tilt_max, /**< maximum tilt allowed for manual flight */
(ParamFloat<px4::params::MPC_LAND_ALT1>) _param_mpc_land_alt1, /**< altitude at which to start downwards slowdown */
(ParamFloat<px4::params::MPC_LAND_ALT2>) _param_mpc_land_alt2, /**< altitude below wich to land with land speed */
(ParamFloat<px4::params::MPC_LAND_SPEED>)
_param_mpc_land_speed, /**< desired downwards speed when approaching the ground */
(ParamFloat<px4::params::MPC_TKO_SPEED>)
_param_mpc_tko_speed, /**< desired upwards speed when still close to the ground */
(ParamFloat<px4::params::MC_MAN_TILT_TAU>) _param_mc_man_tilt_tau
)
private:
bool _isYawInput();
void _unlockYaw();
void _lockYaw();
/**
* Filter between stick input and yaw setpoint
*
* @param yawspeed_target yaw setpoint desired by the stick
* @return filtered value from independent filter state
*/
float _applyYawspeedFilter(float yawspeed_target);
/**
* Terrain following.
* During terrain following, the position setpoint is adjusted
* such that the distance to ground is kept constant.
* @param apply_brake is true if user wants to break
* @param stopped is true if vehicle has stopped moving in D-direction
*/
void _terrainFollowing(bool apply_brake, bool stopped);
/**
* Minimum Altitude during range sensor operation.
* If a range sensor is used for altitude estimates, for
* best operation a minimum altitude is required. The minimum
* altitude is only enforced during altitude lock.
*/
void _respectMinAltitude();
void _respectMaxAltitude();
/**
* Sets downwards velocity constraint based on the distance to ground.
* To ensure a slowdown to land speed before hitting the ground.
*/
void _respectGroundSlowdown();
void setGearAccordingToSwitch();
float _yawspeed_filter_state{}; /**< state of low-pass filter in rad/s */
uint8_t _reset_counter = 0; /**< counter for estimator resets in z-direction */
float _max_speed_up = 10.0f;
float _max_speed_down = 1.0f;
bool _terrain_follow{false}; /**< true when the vehicle is following the terrain height */
bool _terrain_hold{false}; /**< true when vehicle is controlling height above a static ground position */
/**
* Distance to ground during terrain following.
* If user does not demand velocity change in D-direction and the vehcile
* is in terrain-following mode, then height to ground will be locked to
* _dist_to_ground_lock.
*/
float _dist_to_ground_lock = NAN;
AlphaFilter<matrix::Vector2f> _man_input_filter;
};
src/lib/flight_tasks/tasks/ManualAltitudeSmoothVel/CMakeLists.txt
-39
View File
@@ -1,39 +0,0 @@
############################################################################
#
# Copyright (c) 2019 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.
#
############################################################################
px4_add_library(FlightTaskManualAltitudeSmoothVel
FlightTaskManualAltitudeSmoothVel.cpp
)
target_link_libraries(FlightTaskManualAltitudeSmoothVel PUBLIC FlightTaskManualAltitude FlightTaskUtility)
target_include_directories(FlightTaskManualAltitudeSmoothVel PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
src/lib/flight_tasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp
-118
View File
@@ -1,118 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019 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 FlightManualAltitude.cpp
*/
#include "FlightTaskManualAltitudeSmoothVel.hpp"
#include <float.h>
using namespace matrix;
bool FlightTaskManualAltitudeSmoothVel::activate(const vehicle_local_position_setpoint_s &last_setpoint)
{
bool ret = FlightTaskManualAltitude::activate(last_setpoint);
// Check if the previous FlightTask provided setpoints
// If the position setpoint is unknown, set to the current postion
float z_sp_last = PX4_ISFINITE(last_setpoint.z) ? last_setpoint.z : _position(2);
// If the velocity setpoint is unknown, set to the current velocity
float vz_sp_last = PX4_ISFINITE(last_setpoint.vz) ? last_setpoint.vz : _velocity(2);
// No acceleration estimate available, set to zero if the setpoint is NAN
float az_sp_last = PX4_ISFINITE(last_setpoint.acceleration[2]) ? last_setpoint.acceleration[2] : 0.f;
_smoothing.reset(az_sp_last, vz_sp_last, z_sp_last);
return ret;
}
void FlightTaskManualAltitudeSmoothVel::reActivate()
{
FlightTaskManualAltitude::reActivate();
// The task is reacivated while the vehicle is on the ground. To detect takeoff in mc_pos_control_main properly
// using the generated jerk, reset the z derivatives to zero
_smoothing.reset(0.f, 0.f, _position(2));
}
void FlightTaskManualAltitudeSmoothVel::_ekfResetHandlerPositionZ()
{
_smoothing.setCurrentPosition(_position(2));
}
void FlightTaskManualAltitudeSmoothVel::_ekfResetHandlerVelocityZ()
{
_smoothing.setCurrentVelocity(_velocity(2));
}
void FlightTaskManualAltitudeSmoothVel::_updateSetpoints()
{
// Set max accel/vel/jerk
// Has to be done before _updateTrajectories()
_updateTrajConstraints();
_smoothing.setVelSpFeedback(_velocity_setpoint_feedback(2));
_smoothing.setCurrentPositionEstimate(_position(2));
// Get yaw setpoint, un-smoothed position setpoints
FlightTaskManualAltitude::_updateSetpoints();
_smoothing.update(_deltatime, _velocity_setpoint(2));
// Fill the jerk, acceleration, velocity and position setpoint vectors
_setOutputState();
}
void FlightTaskManualAltitudeSmoothVel::_updateTrajConstraints()
{
_smoothing.setMaxJerk(_param_mpc_jerk_max.get());
_smoothing.setMaxAccelUp(_param_mpc_acc_up_max.get());
_smoothing.setMaxVelUp(_constraints.speed_up);
_smoothing.setMaxAccelDown(_param_mpc_acc_down_max.get());
_smoothing.setMaxVelDown(_constraints.speed_down);
}
void FlightTaskManualAltitudeSmoothVel::_setOutputState()
{
_jerk_setpoint(2) = _smoothing.getCurrentJerk();
_acceleration_setpoint(2) = _smoothing.getCurrentAcceleration();
_velocity_setpoint(2) = _smoothing.getCurrentVelocity();
_position_setpoint(2) = _smoothing.getCurrentPosition();
}
src/lib/flight_tasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp
-72
View File
@@ -1,72 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019 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 FlightManualAltitudeSmoothVel.hpp
*
* Flight task for manual controlled altitude using the velocity smoothing library
*/
#pragma once
#include "FlightTaskManualAltitude.hpp"
#include "ManualVelocitySmoothingZ.hpp"
class FlightTaskManualAltitudeSmoothVel : public FlightTaskManualAltitude
{
public:
FlightTaskManualAltitudeSmoothVel() = default;
virtual ~FlightTaskManualAltitudeSmoothVel() = default;
bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
void reActivate() override;
protected:
virtual void _updateSetpoints() override;
/** Reset position or velocity setpoints in case of EKF reset event */
void _ekfResetHandlerPositionZ() override;
void _ekfResetHandlerVelocityZ() override;
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualAltitude,
(ParamFloat<px4::params::MPC_JERK_MAX>) _param_mpc_jerk_max,
(ParamFloat<px4::params::MPC_ACC_UP_MAX>) _param_mpc_acc_up_max,
(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _param_mpc_acc_down_max
)
private:
void _updateTrajConstraints();
void _setOutputState();
ManualVelocitySmoothingZ _smoothing; ///< Smoothing in z direction
};
src/lib/flight_tasks/tasks/ManualPosition/CMakeLists.txt
-39
View File
@@ -1,39 +0,0 @@
############################################################################
#
# Copyright (c) 2018 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_library(FlightTaskManualPosition
FlightTaskManualPosition.cpp
)
target_link_libraries(FlightTaskManualPosition PUBLIC FlightTaskManualAltitude)
target_include_directories(FlightTaskManualPosition PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.cpp
-180
View File
@@ -1,180 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file FlightTaskManualPosition.cpp
*/
#include "FlightTaskManualPosition.hpp"
#include <mathlib/mathlib.h>
#include <float.h>
using namespace matrix;
FlightTaskManualPosition::FlightTaskManualPosition() : _collision_prevention(this)
{
}
bool FlightTaskManualPosition::updateInitialize()
{
bool ret = FlightTaskManualAltitude::updateInitialize();
// require valid position / velocity in xy
return ret && PX4_ISFINITE(_position(0))
&& PX4_ISFINITE(_position(1))
&& PX4_ISFINITE(_velocity(0))
&& PX4_ISFINITE(_velocity(1));
}
bool FlightTaskManualPosition::activate(const vehicle_local_position_setpoint_s &last_setpoint)
{
// all requirements from altitude-mode still have to hold
bool ret = FlightTaskManualAltitude::activate(last_setpoint);
// set task specific constraint
if (_constraints.speed_xy >= _param_mpc_vel_manual.get()) {
_constraints.speed_xy = _param_mpc_vel_manual.get();
}
_position_setpoint(0) = _position(0);
_position_setpoint(1) = _position(1);
_velocity_setpoint(0) = _velocity_setpoint(1) = 0.0f;
_velocity_scale = _constraints.speed_xy;
// for position-controlled mode, we need a valid position and velocity state
// in NE-direction
return ret;
}
void FlightTaskManualPosition::_scaleSticks()
{
/* Use same scaling as for FlightTaskManualAltitude */
FlightTaskManualAltitude::_scaleSticks();
/* Constrain length of stick inputs to 1 for xy*/
Vector2f stick_xy = _sticks.getPositionExpo().slice<2, 1>(0, 0);
const float mag = math::constrain(stick_xy.length(), 0.0f, 1.0f);
if (mag > FLT_EPSILON) {
stick_xy = stick_xy.normalized() * mag;
}
const float max_speed_from_estimator = _sub_vehicle_local_position.get().vxy_max;
if (PX4_ISFINITE(max_speed_from_estimator)) {
// use the minimum of the estimator and user specified limit
_velocity_scale = fminf(_constraints.speed_xy, max_speed_from_estimator);
// Allow for a minimum of 0.3 m/s for repositioning
_velocity_scale = fmaxf(_velocity_scale, 0.3f);
} else {
_velocity_scale = _constraints.speed_xy;
}
_velocity_scale = fminf(_computeVelXYGroundDist(), _velocity_scale);
// scale velocity to its maximum limits
Vector2f vel_sp_xy = stick_xy * _velocity_scale;
/* Rotate setpoint into local frame. */
_rotateIntoHeadingFrame(vel_sp_xy);
// collision prevention
if (_collision_prevention.is_active()) {
_collision_prevention.modifySetpoint(vel_sp_xy, _velocity_scale, Vector2f(_position),
Vector2f(_velocity));
}
_velocity_setpoint.xy() = vel_sp_xy;
}
float FlightTaskManualPosition::_computeVelXYGroundDist()
{
float max_vel_xy = _constraints.speed_xy;
// limit speed gradually within the altitudes MPC_LAND_ALT1 and MPC_LAND_ALT2
if (PX4_ISFINITE(_dist_to_ground)) {
max_vel_xy = math::gradual(_dist_to_ground,
_param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(),
_param_mpc_land_vel_xy.get(), _constraints.speed_xy);
}
return max_vel_xy;
}
void FlightTaskManualPosition::_updateXYlock()
{
/* If position lock is not active, position setpoint is set to NAN.*/
const float vel_xy_norm = Vector2f(_velocity).length();
const bool apply_brake = Vector2f(_velocity_setpoint).length() < FLT_EPSILON;
const bool stopped = (_param_mpc_hold_max_xy.get() < FLT_EPSILON || vel_xy_norm < _param_mpc_hold_max_xy.get());
if (apply_brake && stopped && !PX4_ISFINITE(_position_setpoint(0))) {
_position_setpoint(0) = _position(0);
_position_setpoint(1) = _position(1);
} else if (PX4_ISFINITE(_position_setpoint(0)) && apply_brake) {
// Position is locked but check if a reset event has happened.
// We will shift the setpoints.
if (_sub_vehicle_local_position.get().xy_reset_counter != _reset_counter) {
_position_setpoint(0) = _position(0);
_position_setpoint(1) = _position(1);
_reset_counter = _sub_vehicle_local_position.get().xy_reset_counter;
}
} else {
/* don't lock*/
_position_setpoint(0) = NAN;
_position_setpoint(1) = NAN;
}
}
void FlightTaskManualPosition::_updateSetpoints()
{
FlightTaskManualAltitude::_updateSetpoints(); // needed to get yaw and setpoints in z-direction
_acceleration_setpoint.setNaN(); // don't use the horizontal setpoints from FlightTaskAltitude
_updateXYlock(); // check for position lock
// check if an external yaw handler is active and if yes, let it update the yaw setpoints
if (_weathervane_yaw_handler != nullptr && _weathervane_yaw_handler->is_active()) {
_yaw_setpoint = NAN;
// only enable the weathervane to change the yawrate when position lock is active (and thus the pos. sp. are NAN)
if (PX4_ISFINITE(_position_setpoint(0)) && PX4_ISFINITE(_position_setpoint(1))) {
// vehicle is steady
_yawspeed_setpoint += _weathervane_yaw_handler->get_weathervane_yawrate();
}
}
}
src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.hpp
-81
View File
@@ -1,81 +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 FlightTaskManualPosition.hpp
*
* Flight task for manual position controlled mode.
*
*/
#pragma once
#include <lib/collision_prevention/CollisionPrevention.hpp>
#include "FlightTaskManualAltitude.hpp"
class FlightTaskManualPosition : public FlightTaskManualAltitude
{
public:
FlightTaskManualPosition();
virtual ~FlightTaskManualPosition() = default;
bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
bool updateInitialize() override;
/**
* Sets an external yaw handler which can be used to implement a different yaw control strategy.
*/
void setYawHandler(WeatherVane *yaw_handler) override { _weathervane_yaw_handler = yaw_handler; }
protected:
void _updateXYlock(); /**< applies position lock based on stick and velocity */
void _updateSetpoints() override;
void _scaleSticks() override;
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualAltitude,
(ParamFloat<px4::params::MPC_VEL_MANUAL>) _param_mpc_vel_manual,
(ParamFloat<px4::params::MPC_LAND_VEL_XY>) _param_mpc_land_vel_xy,
(ParamFloat<px4::params::MPC_ACC_HOR_MAX>) _param_mpc_acc_hor_max,
(ParamFloat<px4::params::MPC_HOLD_MAX_XY>) _param_mpc_hold_max_xy
)
private:
float _computeVelXYGroundDist();
float _velocity_scale{0.0f}; //scales the stick input to velocity
uint8_t _reset_counter{0}; /**< counter for estimator resets in xy-direction */
WeatherVane *_weathervane_yaw_handler =
nullptr; /**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
CollisionPrevention _collision_prevention; /**< collision avoidance setpoint amendment */
};
src/lib/flight_tasks/tasks/ManualPositionSmoothVel/CMakeLists.txt
-39
View File
@@ -1,39 +0,0 @@
############################################################################
#
# Copyright (c) 2018 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_library(FlightTaskManualPositionSmoothVel
FlightTaskManualPositionSmoothVel.cpp
)
target_link_libraries(FlightTaskManualPositionSmoothVel PUBLIC FlightTaskManualPosition FlightTaskUtility)
target_include_directories(FlightTaskManualPositionSmoothVel PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
src/lib/flight_tasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
-175
View File
@@ -1,175 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2018-2019 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.
*
****************************************************************************/
#include "FlightTaskManualPositionSmoothVel.hpp"
#include <mathlib/mathlib.h>
#include <float.h>
using namespace matrix;
bool FlightTaskManualPositionSmoothVel::activate(const vehicle_local_position_setpoint_s &last_setpoint)
{
bool ret = FlightTaskManualPosition::activate(last_setpoint);
// Check if the previous FlightTask provided setpoints
Vector3f accel_prev{last_setpoint.acceleration};
Vector3f vel_prev{last_setpoint.vx, last_setpoint.vy, last_setpoint.vz};
Vector3f pos_prev{last_setpoint.x, last_setpoint.y, last_setpoint.z};
for (int i = 0; i < 3; i++) {
// If the position setpoint is unknown, set to the current postion
if (!PX4_ISFINITE(pos_prev(i))) { pos_prev(i) = _position(i); }
// If the velocity setpoint is unknown, set to the current velocity
if (!PX4_ISFINITE(vel_prev(i))) { vel_prev(i) = _velocity(i); }
// No acceleration estimate available, set to zero if the setpoint is NAN
if (!PX4_ISFINITE(accel_prev(i))) { accel_prev(i) = 0.f; }
}
_smoothing_xy.reset(Vector2f{accel_prev}, Vector2f{vel_prev}, Vector2f{pos_prev});
_smoothing_z.reset(accel_prev(2), vel_prev(2), pos_prev(2));
return ret;
}
void FlightTaskManualPositionSmoothVel::reActivate()
{
FlightTaskManualPosition::reActivate();
// The task is reacivated while the vehicle is on the ground. To detect takeoff in mc_pos_control_main properly
// using the generated jerk, reset the z derivatives to zero
_smoothing_xy.reset(Vector2f(), Vector2f(_velocity), Vector2f(_position));
_smoothing_z.reset(0.f, 0.f, _position(2));
}
void FlightTaskManualPositionSmoothVel::_ekfResetHandlerPositionXY()
{
_smoothing_xy.setCurrentPosition(_position.xy());
}
void FlightTaskManualPositionSmoothVel::_ekfResetHandlerVelocityXY()
{
_smoothing_xy.setCurrentVelocity(_velocity.xy());
}
void FlightTaskManualPositionSmoothVel::_ekfResetHandlerPositionZ()
{
_smoothing_z.setCurrentPosition(_position(2));
}
void FlightTaskManualPositionSmoothVel::_ekfResetHandlerVelocityZ()
{
_smoothing_z.setCurrentVelocity(_velocity(2));
}
void FlightTaskManualPositionSmoothVel::_updateSetpoints()
{
// Set max accel/vel/jerk
// Has to be done before _updateTrajectories()
_updateTrajConstraints();
_updateTrajVelFeedback();
_updateTrajCurrentPositionEstimate();
// Get yaw setpoint, un-smoothed position setpoints
FlightTaskManualPosition::_updateSetpoints();
_updateTrajectories(_velocity_setpoint);
// Fill the jerk, acceleration, velocity and position setpoint vectors
_setOutputState();
}
void FlightTaskManualPositionSmoothVel::_updateTrajConstraints()
{
_updateTrajConstraintsXY();
_updateTrajConstraintsZ();
}
void FlightTaskManualPositionSmoothVel::_updateTrajConstraintsXY()
{
_smoothing_xy.setMaxJerk(_param_mpc_jerk_max.get());
_smoothing_xy.setMaxAccel(_param_mpc_acc_hor_max.get());
_smoothing_xy.setMaxVel(_constraints.speed_xy);
}
void FlightTaskManualPositionSmoothVel::_updateTrajConstraintsZ()
{
_smoothing_z.setMaxJerk(_param_mpc_jerk_max.get());
_smoothing_z.setMaxAccelUp(_param_mpc_acc_up_max.get());
_smoothing_z.setMaxVelUp(_constraints.speed_up);
_smoothing_z.setMaxAccelDown(_param_mpc_acc_down_max.get());
_smoothing_z.setMaxVelDown(_constraints.speed_down);
}
void FlightTaskManualPositionSmoothVel::_updateTrajVelFeedback()
{
_smoothing_xy.setVelSpFeedback(Vector2f(_velocity_setpoint_feedback));
_smoothing_z.setVelSpFeedback(_velocity_setpoint_feedback(2));
}
void FlightTaskManualPositionSmoothVel::_updateTrajCurrentPositionEstimate()
{
_smoothing_xy.setCurrentPositionEstimate(Vector2f(_position));
_smoothing_z.setCurrentPositionEstimate(_position(2));
}
void FlightTaskManualPositionSmoothVel::_updateTrajectories(Vector3f vel_target)
{
_smoothing_xy.update(_deltatime, Vector2f(vel_target));
_smoothing_z.update(_deltatime, vel_target(2));
}
void FlightTaskManualPositionSmoothVel::_setOutputState()
{
_setOutputStateXY();
_setOutputStateZ();
}
void FlightTaskManualPositionSmoothVel::_setOutputStateXY()
{
_jerk_setpoint.xy() = _smoothing_xy.getCurrentJerk();
_acceleration_setpoint.xy() = _smoothing_xy.getCurrentAcceleration();
_velocity_setpoint.xy() = _smoothing_xy.getCurrentVelocity();
_position_setpoint.xy() = _smoothing_xy.getCurrentPosition();
}
void FlightTaskManualPositionSmoothVel::_setOutputStateZ()
{
_jerk_setpoint(2) = _smoothing_z.getCurrentJerk();
_acceleration_setpoint(2) = _smoothing_z.getCurrentAcceleration();
_velocity_setpoint(2) = _smoothing_z.getCurrentVelocity();
_position_setpoint(2) = _smoothing_z.getCurrentPosition();
}
src/lib/flight_tasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp
-91
View File
@@ -1,91 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2018-2019 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 FlightTaskManualPositionSmoothVel.hpp
*
* Flight task for smooth manual controlled position.
*/
#pragma once
#include "FlightTaskManualPosition.hpp"
#include "ManualVelocitySmoothingXY.hpp"
#include "ManualVelocitySmoothingZ.hpp"
using matrix::Vector2f;
using matrix::Vector3f;
class FlightTaskManualPositionSmoothVel : public FlightTaskManualPosition
{
public:
FlightTaskManualPositionSmoothVel() = default;
virtual ~FlightTaskManualPositionSmoothVel() = default;
bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
void reActivate() override;
protected:
virtual void _updateSetpoints() override;
/** Reset position or velocity setpoints in case of EKF reset event */
void _ekfResetHandlerPositionXY() override;
void _ekfResetHandlerVelocityXY() override;
void _ekfResetHandlerPositionZ() override;
void _ekfResetHandlerVelocityZ() override;
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualPosition,
(ParamFloat<px4::params::MPC_JERK_MAX>) _param_mpc_jerk_max,
(ParamFloat<px4::params::MPC_ACC_UP_MAX>) _param_mpc_acc_up_max,
(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _param_mpc_acc_down_max
)
private:
void _updateTrajConstraints();
void _updateTrajConstraintsXY();
void _updateTrajConstraintsZ();
void _updateTrajVelFeedback();
void _updateTrajCurrentPositionEstimate();
void _updateTrajectories(Vector3f vel_target);
void _setOutputState();
void _setOutputStateXY();
void _setOutputStateZ();
ManualVelocitySmoothingXY _smoothing_xy; ///< Smoothing in x and y directions
ManualVelocitySmoothingZ _smoothing_z; ///< Smoothing in z direction
};
src/lib/flight_tasks/tasks/Offboard/CMakeLists.txt
-39
View File
@@ -1,39 +0,0 @@
############################################################################
#
# Copyright (c) 2018 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_library(FlightTaskOffboard
FlightTaskOffboard.cpp
)
target_link_libraries(FlightTaskOffboard PUBLIC FlightTask)
target_include_directories(FlightTaskOffboard PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
src/lib/flight_tasks/tasks/Offboard/FlightTaskOffboard.cpp
-243
View File
@@ -1,243 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file FlightTaskOffboard.cpp
*/
#include "FlightTaskOffboard.hpp"
#include <mathlib/mathlib.h>
#include <float.h>
using namespace matrix;
bool FlightTaskOffboard::updateInitialize()
{
bool ret = FlightTask::updateInitialize();
// require valid position / velocity in xy
return ret && PX4_ISFINITE(_position(0))
&& PX4_ISFINITE(_position(1))
&& PX4_ISFINITE(_velocity(0))
&& PX4_ISFINITE(_velocity(1));
}
bool FlightTaskOffboard::activate(const vehicle_local_position_setpoint_s &last_setpoint)
{
bool ret = FlightTask::activate(last_setpoint);
_position_setpoint = _position;
_velocity_setpoint.setZero();
_position_lock.setAll(NAN);
return ret;
}
bool FlightTaskOffboard::update()
{
bool ret = FlightTask::update();
// reset setpoint for every loop
_resetSetpoints();
_sub_triplet_setpoint.update();
if (!_sub_triplet_setpoint.get().current.valid) {
_setDefaultConstraints();
_position_setpoint = _position;
return false;
}
// Yaw / Yaw-speed
if (_sub_triplet_setpoint.get().current.yaw_valid) {
// yaw control required
_yaw_setpoint = _sub_triplet_setpoint.get().current.yaw;
if (_sub_triplet_setpoint.get().current.yawspeed_valid) {
// yawspeed is used as feedforward
_yawspeed_setpoint = _sub_triplet_setpoint.get().current.yawspeed;
}
} else if (_sub_triplet_setpoint.get().current.yawspeed_valid) {
// only yawspeed required
_yawspeed_setpoint = _sub_triplet_setpoint.get().current.yawspeed;
// set yaw setpoint to NAN since not used
_yaw_setpoint = NAN;
}
// Loiter
if (_sub_triplet_setpoint.get().current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
// loiter just means that the vehicle should keep position
if (!PX4_ISFINITE(_position_lock(0))) {
_position_setpoint = _position_lock = _position;
} else {
_position_setpoint = _position_lock;
}
// don't have to continue
return ret;
} else {
_position_lock.setAll(NAN);
}
// Takeoff
if (_sub_triplet_setpoint.get().current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
// just do takeoff to default altitude
if (!PX4_ISFINITE(_position_lock(0))) {
_position_setpoint = _position_lock = _position;
_position_setpoint(2) = _position_lock(2) = _position(2) - _param_mis_takeoff_alt.get();
} else {
_position_setpoint = _position_lock;
}
// don't have to continue
return ret;
} else {
_position_lock.setAll(NAN);
}
// Land
if (_sub_triplet_setpoint.get().current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
// land with landing speed, but keep position in xy
if (!PX4_ISFINITE(_position_lock(0))) {
_position_setpoint = _position_lock = _position;
_position_setpoint(2) = _position_lock(2) = NAN;
_velocity_setpoint(2) = _param_mpc_land_speed.get();
} else {
_position_setpoint = _position_lock;
_velocity_setpoint(2) = _param_mpc_land_speed.get();
}
// don't have to continue
return ret;
} else {
_position_lock.setAll(NAN);
}
// IDLE
if (_sub_triplet_setpoint.get().current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
_position_setpoint.setNaN(); // Don't require any position/velocity setpoints
_velocity_setpoint.setNaN();
_acceleration_setpoint = Vector3f(0.f, 0.f, 100.f); // High downwards acceleration to make sure there's no thrust
return ret;
}
// Possible inputs:
// 1. position setpoint
// 2. position setpoint + velocity setpoint (velocity used as feedforward)
// 3. velocity setpoint
// 4. acceleration setpoint -> this will be mapped to normalized thrust setpoint because acceleration is not supported
const bool position_ctrl_xy = _sub_triplet_setpoint.get().current.position_valid
&& _sub_vehicle_local_position.get().xy_valid;
const bool position_ctrl_z = _sub_triplet_setpoint.get().current.alt_valid
&& _sub_vehicle_local_position.get().z_valid;
const bool velocity_ctrl_xy = _sub_triplet_setpoint.get().current.velocity_valid
&& _sub_vehicle_local_position.get().v_xy_valid;
const bool velocity_ctrl_z = _sub_triplet_setpoint.get().current.velocity_valid
&& _sub_vehicle_local_position.get().v_z_valid;
const bool feedforward_ctrl_xy = position_ctrl_xy && velocity_ctrl_xy;
const bool feedforward_ctrl_z = position_ctrl_z && velocity_ctrl_z;
const bool acceleration_ctrl = _sub_triplet_setpoint.get().current.acceleration_valid;
// if nothing is valid in xy, then exit offboard
if (!(position_ctrl_xy || velocity_ctrl_xy || acceleration_ctrl)) {
return false;
}
// if nothing is valid in z, then exit offboard
if (!(position_ctrl_z || velocity_ctrl_z || acceleration_ctrl)) {
return false;
}
// XY-direction
if (feedforward_ctrl_xy) {
_position_setpoint(0) = _sub_triplet_setpoint.get().current.x;
_position_setpoint(1) = _sub_triplet_setpoint.get().current.y;
_velocity_setpoint(0) = _sub_triplet_setpoint.get().current.vx;
_velocity_setpoint(1) = _sub_triplet_setpoint.get().current.vy;
} else if (position_ctrl_xy) {
_position_setpoint(0) = _sub_triplet_setpoint.get().current.x;
_position_setpoint(1) = _sub_triplet_setpoint.get().current.y;
} else if (velocity_ctrl_xy) {
if (_sub_triplet_setpoint.get().current.velocity_frame == position_setpoint_s::VELOCITY_FRAME_LOCAL_NED) {
// in local frame: don't require any transformation
_velocity_setpoint(0) = _sub_triplet_setpoint.get().current.vx;
_velocity_setpoint(1) = _sub_triplet_setpoint.get().current.vy;
} else if (_sub_triplet_setpoint.get().current.velocity_frame == position_setpoint_s::VELOCITY_FRAME_BODY_NED) {
// in body frame: need to transorm first
// Note, this transformation is wrong because body-xy is not neccessarily on the same plane as locale-xy
_velocity_setpoint(0) = cosf(_yaw) * _sub_triplet_setpoint.get().current.vx - sinf(
_yaw) * _sub_triplet_setpoint.get().current.vy;
_velocity_setpoint(1) = sinf(_yaw) * _sub_triplet_setpoint.get().current.vx + cosf(
_yaw) * _sub_triplet_setpoint.get().current.vy;
} else {
// no valid frame
return false;
}
}
// Z-direction
if (feedforward_ctrl_z) {
_position_setpoint(2) = _sub_triplet_setpoint.get().current.z;
_velocity_setpoint(2) = _sub_triplet_setpoint.get().current.vz;
} else if (position_ctrl_z) {
_position_setpoint(2) = _sub_triplet_setpoint.get().current.z;
} else if (velocity_ctrl_z) {
_velocity_setpoint(2) = _sub_triplet_setpoint.get().current.vz;
}
// Acceleration
// Note: this is not supported yet and will be mapped to normalized thrust directly.
if (_sub_triplet_setpoint.get().current.acceleration_valid) {
_acceleration_setpoint(0) = _sub_triplet_setpoint.get().current.a_x;
_acceleration_setpoint(1) = _sub_triplet_setpoint.get().current.a_y;
_acceleration_setpoint(2) = _sub_triplet_setpoint.get().current.a_z;
}
// use default conditions of upwards position or velocity to take off
_constraints.want_takeoff = _checkTakeoff();
return ret;
}
src/lib/flight_tasks/tasks/Offboard/FlightTaskOffboard.hpp
-65
View File
@@ -1,65 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file FlightTaskOffboard.hpp
*/
#pragma once
#include "FlightTask.hpp"
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/position_setpoint.h>
class FlightTaskOffboard : public FlightTask
{
public:
FlightTaskOffboard() = default;
virtual ~FlightTaskOffboard() = default;
bool update() override;
bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
bool updateInitialize() override;
protected:
uORB::SubscriptionData<position_setpoint_triplet_s> _sub_triplet_setpoint{ORB_ID(position_setpoint_triplet)};
private:
matrix::Vector3f _position_lock{};
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
(ParamFloat<px4::params::MPC_TKO_SPEED>) _param_mpc_tko_speed,
(ParamFloat<px4::params::MIS_TAKEOFF_ALT>) _param_mis_takeoff_alt
)
};
src/lib/flight_tasks/tasks/Orbit/CMakeLists.txt
-39
View File
@@ -1,39 +0,0 @@
############################################################################
#
# Copyright (c) 2018 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_library(FlightTaskOrbit
FlightTaskOrbit.cpp
)
target_link_libraries(FlightTaskOrbit PUBLIC FlightTaskManualAltitudeSmoothVel SlewRate)
target_include_directories(FlightTaskOrbit PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

Some files were not shown because too many files have changed in this diff Show More