mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-22 01:53:33 +08:00
flight_mode_manager: merge with flight_tasks
This commit is contained in:
Daniel Agar
committed by
Lorenz Meier
Lorenz Meier
parent
b1e442b830
commit
4d7b875ee2
@@ -0,0 +1,46 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2018 - 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(FlightTaskUtility
|
||||
StraightLine.cpp
|
||||
ManualVelocitySmoothingXY.cpp
|
||||
ManualVelocitySmoothingZ.cpp
|
||||
Sticks.cpp
|
||||
StickAccelerationXY.cpp
|
||||
StickYaw.cpp
|
||||
)
|
||||
|
||||
target_link_libraries(FlightTaskUtility PUBLIC FlightTask hysteresis bezier SlewRate motion_planning)
|
||||
target_include_directories(FlightTaskUtility PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
|
||||
px4_add_unit_gtest(SRC ManualVelocitySmoothingXYTest.cpp LINKLIBS FlightTaskUtility)
|
||||
@@ -0,0 +1,12 @@
|
||||
|
||||
.PHONY: all tests clean
|
||||
all: test_velocity_smoothing
|
||||
|
||||
test_velocity_smoothing: test_velocity_smoothing.cpp VelocitySmoothing.cpp
|
||||
@g++ $^ -std=c++11 -I ../../../ -o $@
|
||||
|
||||
tests: test_velocity_smoothing
|
||||
@echo "Test velocity smoothing"
|
||||
|
||||
clean:
|
||||
@rm test_velocity_smoothing
|
||||
@@ -0,0 +1,121 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "ManualVelocitySmoothingXY.hpp"
|
||||
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <float.h>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
void ManualVelocitySmoothingXY::reset(const Vector2f &accel, const Vector2f &vel, const Vector2f &pos)
|
||||
{
|
||||
for (int i = 0; i < 2; i++) {
|
||||
_trajectory[i].reset(accel(i), vel(i), pos(i));
|
||||
}
|
||||
|
||||
resetPositionLock();
|
||||
}
|
||||
|
||||
void ManualVelocitySmoothingXY::resetPositionLock()
|
||||
{
|
||||
_position_lock_active = false;
|
||||
_position_setpoint_locked(0) = NAN;
|
||||
_position_setpoint_locked(1) = NAN;
|
||||
}
|
||||
|
||||
void ManualVelocitySmoothingXY::update(float dt, const Vector2f &velocity_target)
|
||||
{
|
||||
// Update state
|
||||
updateTrajectories(dt);
|
||||
|
||||
// Lock or unlock position
|
||||
// Has to be done before _updateTrajDurations()
|
||||
checkPositionLock(velocity_target);
|
||||
|
||||
// Update durations and sync XY
|
||||
updateTrajDurations(velocity_target);
|
||||
}
|
||||
|
||||
void ManualVelocitySmoothingXY::updateTrajectories(float dt)
|
||||
{
|
||||
for (int i = 0; i < 2; ++i) {
|
||||
_trajectory[i].updateTraj(dt);
|
||||
|
||||
_state.j(i) = _trajectory[i].getCurrentJerk();
|
||||
_state.a(i) = _trajectory[i].getCurrentAcceleration();
|
||||
_state.v(i) = _trajectory[i].getCurrentVelocity();
|
||||
_state.x(i) = _trajectory[i].getCurrentPosition();
|
||||
}
|
||||
}
|
||||
|
||||
void ManualVelocitySmoothingXY::updateTrajDurations(const Vector2f &velocity_target)
|
||||
{
|
||||
for (int i = 0; i < 2; ++i) {
|
||||
_trajectory[i].updateDurations(velocity_target(i));
|
||||
}
|
||||
|
||||
VelocitySmoothing::timeSynchronization(_trajectory, 2);
|
||||
}
|
||||
|
||||
void ManualVelocitySmoothingXY::checkPositionLock(const Vector2f &velocity_target)
|
||||
{
|
||||
/**
|
||||
* During a position lock -> position unlock transition, we have to make sure that the velocity setpoint
|
||||
* is continuous. We know that the output of the position loop (part of the velocity setpoint)
|
||||
* will suddenly become null
|
||||
* and only the feedforward (generated by this flight task) will remain.
|
||||
* This is why the previous input of the velocity controller
|
||||
* is used to set current velocity of the trajectory.
|
||||
*/
|
||||
if (_state.v.length() < 0.1f &&
|
||||
_state.a.length() < .2f &&
|
||||
velocity_target.length() <= FLT_EPSILON) {
|
||||
// Lock position
|
||||
_position_lock_active = true;
|
||||
_position_setpoint_locked = _state.x;
|
||||
|
||||
} else {
|
||||
// Unlock position
|
||||
if (_position_lock_active) {
|
||||
// Start the trajectory at the current velocity setpoint
|
||||
_trajectory[0].setCurrentVelocity(_velocity_setpoint_feedback(0));
|
||||
_trajectory[1].setCurrentVelocity(_velocity_setpoint_feedback(1));
|
||||
_state.v = _velocity_setpoint_feedback;
|
||||
resetPositionLock();
|
||||
}
|
||||
|
||||
_trajectory[0].setCurrentPosition(_position_estimate(0));
|
||||
_trajectory[1].setCurrentPosition(_position_estimate(1));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,126 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 ManualVelocitySmoothingXY.hpp
|
||||
*
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <motion_planning/VelocitySmoothing.hpp>
|
||||
|
||||
#include <matrix/matrix/math.hpp>
|
||||
|
||||
using matrix::Vector2f;
|
||||
|
||||
class ManualVelocitySmoothingXY final
|
||||
{
|
||||
public:
|
||||
ManualVelocitySmoothingXY() = default;
|
||||
~ManualVelocitySmoothingXY() = default;
|
||||
|
||||
void reset(const Vector2f &accel, const Vector2f &vel, const Vector2f &pos);
|
||||
void update(float dt, const Vector2f &velocity_target);
|
||||
|
||||
void setVelSpFeedback(const Vector2f &fb) { _velocity_setpoint_feedback = fb; }
|
||||
|
||||
void setMaxJerk(const float max_jerk)
|
||||
{
|
||||
_trajectory[0].setMaxJerk(max_jerk);
|
||||
_trajectory[1].setMaxJerk(max_jerk);
|
||||
}
|
||||
float getMaxJerk() const { return _trajectory[0].getMaxJerk(); }
|
||||
|
||||
void setMaxAccel(const float max_accel)
|
||||
{
|
||||
_trajectory[0].setMaxAccel(max_accel);
|
||||
_trajectory[1].setMaxAccel(max_accel);
|
||||
}
|
||||
float getMaxAccel() const { return _trajectory[0].getMaxAccel(); }
|
||||
|
||||
void setMaxVel(const float max_vel)
|
||||
{
|
||||
_trajectory[0].setMaxVel(max_vel);
|
||||
_trajectory[1].setMaxVel(max_vel);
|
||||
}
|
||||
float getMaxVel() const { return _trajectory[0].getMaxVel(); }
|
||||
|
||||
Vector2f getCurrentJerk() const { return _state.j; }
|
||||
Vector2f getCurrentAcceleration() const { return _state.a; }
|
||||
|
||||
void setCurrentVelocity(const Vector2f &vel)
|
||||
{
|
||||
_state.v = vel;
|
||||
_trajectory[0].setCurrentVelocity(vel(0));
|
||||
_trajectory[1].setCurrentVelocity(vel(1));
|
||||
}
|
||||
Vector2f getCurrentVelocity() const { return _state.v; }
|
||||
|
||||
void setCurrentPosition(const Vector2f &pos)
|
||||
{
|
||||
_state.x = pos;
|
||||
_trajectory[0].setCurrentPosition(pos(0));
|
||||
_trajectory[1].setCurrentPosition(pos(1));
|
||||
_position_estimate = pos;
|
||||
|
||||
if (_position_lock_active) {
|
||||
_position_setpoint_locked = pos;
|
||||
}
|
||||
}
|
||||
Vector2f getCurrentPosition() const { return _position_setpoint_locked; }
|
||||
|
||||
void setCurrentPositionEstimate(const Vector2f &pos) { _position_estimate = pos; }
|
||||
|
||||
private:
|
||||
void resetPositionLock();
|
||||
void updateTrajectories(float dt);
|
||||
void checkPositionLock(const Vector2f &velocity_target);
|
||||
void updateTrajDurations(const Vector2f &velocity_target);
|
||||
|
||||
VelocitySmoothing _trajectory[2]; ///< Trajectory in x and y directions
|
||||
|
||||
bool _position_lock_active{false};
|
||||
|
||||
Vector2f _position_setpoint_locked{};
|
||||
|
||||
Vector2f _velocity_setpoint_feedback{};
|
||||
Vector2f _position_estimate{};
|
||||
|
||||
struct {
|
||||
Vector2f j;
|
||||
Vector2f a;
|
||||
Vector2f v;
|
||||
Vector2f x;
|
||||
} _state{};
|
||||
};
|
||||
@@ -0,0 +1,87 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* Test code for the Manual Velocity Smoothing library
|
||||
* Run this test only using make tests TESTFILTER=ManualVelocitySmoothing
|
||||
*/
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <matrix/matrix/math.hpp>
|
||||
|
||||
#include "ManualVelocitySmoothingXY.hpp"
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
class ManualVelocitySmoothingXYTest : public ::testing::Test
|
||||
{
|
||||
public:
|
||||
ManualVelocitySmoothingXY _smoothing;
|
||||
};
|
||||
|
||||
|
||||
TEST_F(ManualVelocitySmoothingXYTest, setGet)
|
||||
{
|
||||
// GIVEN: Some max values
|
||||
_smoothing.setMaxJerk(11.f);
|
||||
_smoothing.setMaxAccel(7.f);
|
||||
_smoothing.setMaxVel(5.f);
|
||||
|
||||
// THEN: We should be able to get them back
|
||||
EXPECT_FLOAT_EQ(_smoothing.getMaxJerk(), 11.f);
|
||||
EXPECT_FLOAT_EQ(_smoothing.getMaxAccel(), 7.f);
|
||||
EXPECT_FLOAT_EQ(_smoothing.getMaxVel(), 5.f);
|
||||
}
|
||||
|
||||
TEST_F(ManualVelocitySmoothingXYTest, getCurrentState)
|
||||
{
|
||||
// GIVEN: the initial conditions
|
||||
Vector2f v0(11.f, 13.f);
|
||||
_smoothing.setCurrentVelocity(v0);
|
||||
|
||||
// WHEN: we get the current state
|
||||
Vector3f j_end;
|
||||
Vector3f a_end;
|
||||
Vector3f v_end;
|
||||
Vector3f x_end;
|
||||
j_end.xy() = _smoothing.getCurrentJerk();
|
||||
a_end.xy() = _smoothing.getCurrentAcceleration();
|
||||
v_end.xy() = _smoothing.getCurrentVelocity();
|
||||
x_end.xy() = _smoothing.getCurrentPosition();
|
||||
|
||||
// THEN: the returned values should match the input
|
||||
EXPECT_EQ(j_end, Vector3f(0.f, 0.f, 0.f));
|
||||
EXPECT_EQ(a_end, Vector3f(0.f, 0.f, 0.f));
|
||||
EXPECT_EQ(v_end, Vector3f(11.f, 13.f, 0.f));
|
||||
EXPECT_EQ(x_end, Vector3f(0.f, 0.f, 0.f));
|
||||
}
|
||||
@@ -0,0 +1,119 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "ManualVelocitySmoothingZ.hpp"
|
||||
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <float.h>
|
||||
|
||||
void ManualVelocitySmoothingZ::reset(float accel, float vel, float pos)
|
||||
{
|
||||
_trajectory.reset(accel, vel, pos);
|
||||
|
||||
resetPositionLock();
|
||||
}
|
||||
|
||||
void ManualVelocitySmoothingZ::resetPositionLock()
|
||||
{
|
||||
_position_lock_active = false;
|
||||
_position_setpoint_locked = NAN;
|
||||
}
|
||||
|
||||
void ManualVelocitySmoothingZ::update(float dt, float velocity_target)
|
||||
{
|
||||
// Update state
|
||||
updateTrajectories(dt);
|
||||
|
||||
// Set max accel/vel/jerk
|
||||
// Has to be done before _updateTrajDurations()
|
||||
updateTrajConstraints(velocity_target);
|
||||
|
||||
// Lock or unlock position
|
||||
// Has to be done before _updateTrajDurations()
|
||||
checkPositionLock(velocity_target);
|
||||
|
||||
// Update durations
|
||||
_trajectory.updateDurations(velocity_target);
|
||||
}
|
||||
|
||||
void ManualVelocitySmoothingZ::updateTrajectories(float dt)
|
||||
{
|
||||
_trajectory.updateTraj(dt);
|
||||
|
||||
_state.j = _trajectory.getCurrentJerk();
|
||||
_state.a = _trajectory.getCurrentAcceleration();
|
||||
_state.v = _trajectory.getCurrentVelocity();
|
||||
_state.x = _trajectory.getCurrentPosition();
|
||||
}
|
||||
|
||||
void ManualVelocitySmoothingZ::updateTrajConstraints(float velocity_target)
|
||||
{
|
||||
if (velocity_target < 0.f) { // up
|
||||
_trajectory.setMaxAccel(_max_accel_up);
|
||||
_trajectory.setMaxVel(_max_vel_up);
|
||||
|
||||
} else { // down
|
||||
_trajectory.setMaxAccel(_max_accel_down);
|
||||
_trajectory.setMaxVel(_max_vel_down);
|
||||
}
|
||||
}
|
||||
|
||||
void ManualVelocitySmoothingZ::checkPositionLock(float velocity_target)
|
||||
{
|
||||
/**
|
||||
* During a position lock -> position unlock transition, we have to make sure that the velocity setpoint
|
||||
* is continuous. We know that the output of the position loop (part of the velocity setpoint)
|
||||
* will suddenly become null
|
||||
* and only the feedforward (generated by this flight task) will remain.
|
||||
* This is why the previous input of the velocity controller
|
||||
* is used to set current velocity of the trajectory.
|
||||
*/
|
||||
if (fabsf(_state.v) < 0.1f &&
|
||||
fabsf(_state.a) < .2f &&
|
||||
fabsf(velocity_target) <= FLT_EPSILON) {
|
||||
// Lock position
|
||||
_position_lock_active = true;
|
||||
_position_setpoint_locked = _state.x;
|
||||
|
||||
} else {
|
||||
// Unlock position
|
||||
if (_position_lock_active) {
|
||||
// Start the trajectory at the current velocity setpoint
|
||||
_trajectory.setCurrentVelocity(_velocity_setpoint_feedback);
|
||||
_state.v = _velocity_setpoint_feedback;
|
||||
resetPositionLock();
|
||||
}
|
||||
|
||||
_trajectory.setCurrentPosition(_position_estimate);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,123 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 ManualVelocitySmoothingZ.hpp
|
||||
*
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <motion_planning/VelocitySmoothing.hpp>
|
||||
|
||||
class ManualVelocitySmoothingZ final
|
||||
{
|
||||
public:
|
||||
ManualVelocitySmoothingZ() = default;
|
||||
~ManualVelocitySmoothingZ() = default;
|
||||
|
||||
void reset(float accel, float vel, float pos);
|
||||
void update(float dt, float velocity_target);
|
||||
|
||||
void setVelSpFeedback(const float fb) { _velocity_setpoint_feedback = fb; }
|
||||
|
||||
void setMaxJerk(const float max_jerk)
|
||||
{
|
||||
_trajectory.setMaxJerk(max_jerk);
|
||||
}
|
||||
void setMaxAccelUp(const float max_accel_up)
|
||||
{
|
||||
_max_accel_up = max_accel_up;
|
||||
}
|
||||
void setMaxAccelDown(const float max_accel_down)
|
||||
{
|
||||
_max_accel_down = max_accel_down;
|
||||
}
|
||||
void setMaxVelUp(const float max_vel_up)
|
||||
{
|
||||
_max_vel_up = max_vel_up;
|
||||
}
|
||||
void setMaxVelDown(const float max_vel_down)
|
||||
{
|
||||
_max_vel_down = max_vel_down;
|
||||
}
|
||||
|
||||
float getCurrentJerk() const { return _state.j; }
|
||||
float getCurrentAcceleration() const { return _state.a; }
|
||||
void setCurrentVelocity(const float vel)
|
||||
{
|
||||
_state.v = vel;
|
||||
_trajectory.setCurrentVelocity(vel);
|
||||
}
|
||||
float getCurrentVelocity() const { return _state.v; }
|
||||
void setCurrentPosition(const float pos)
|
||||
{
|
||||
_state.x = pos;
|
||||
_trajectory.setCurrentPosition(pos);
|
||||
_position_estimate = pos;
|
||||
|
||||
if (_position_lock_active) {
|
||||
_position_setpoint_locked = pos;
|
||||
}
|
||||
}
|
||||
float getCurrentPosition() const { return _position_setpoint_locked; }
|
||||
void setCurrentPositionEstimate(float pos) { _position_estimate = pos; }
|
||||
|
||||
private:
|
||||
void resetPositionLock();
|
||||
void updateTrajectories(float dt);
|
||||
void updateTrajConstraints(float vel_target);
|
||||
void checkPositionLock(float velocity_target);
|
||||
void updateTrajDurations(float velocity_target);
|
||||
|
||||
VelocitySmoothing _trajectory; ///< Trajectory in z direction
|
||||
|
||||
bool _position_lock_active{false};
|
||||
|
||||
float _position_setpoint_locked{};
|
||||
|
||||
float _velocity_setpoint_feedback{};
|
||||
float _position_estimate{};
|
||||
|
||||
struct {
|
||||
float j;
|
||||
float a;
|
||||
float v;
|
||||
float x;
|
||||
} _state{};
|
||||
|
||||
float _max_accel_up{0.f};
|
||||
float _max_accel_down{0.f};
|
||||
float _max_vel_up{0.f};
|
||||
float _max_vel_down{0.f};
|
||||
};
|
||||
@@ -0,0 +1,151 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 spec{fic 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 StickAccelerationXY.cpp
|
||||
*/
|
||||
|
||||
#include "StickAccelerationXY.hpp"
|
||||
|
||||
#include <ecl/geo/geo.h>
|
||||
#include "Sticks.hpp"
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
StickAccelerationXY::StickAccelerationXY(ModuleParams *parent) :
|
||||
ModuleParams(parent)
|
||||
{
|
||||
_brake_boost_filter.reset(1.f);
|
||||
resetPosition();
|
||||
}
|
||||
|
||||
void StickAccelerationXY::resetPosition()
|
||||
{
|
||||
_position.setNaN();
|
||||
}
|
||||
|
||||
void StickAccelerationXY::resetVelocity(const matrix::Vector2f &velocity)
|
||||
{
|
||||
_velocity = velocity;
|
||||
}
|
||||
|
||||
void StickAccelerationXY::resetAcceleration(const matrix::Vector2f &acceleration)
|
||||
{
|
||||
_acceleration_slew_rate_x.setForcedValue(acceleration(0));
|
||||
_acceleration_slew_rate_x.setForcedValue(acceleration(1));
|
||||
}
|
||||
|
||||
void StickAccelerationXY::generateSetpoints(Vector2f stick_xy, const float yaw, const float yaw_sp, const Vector3f &pos,
|
||||
const float dt)
|
||||
{
|
||||
// maximum commanded acceleration and velocity
|
||||
Vector2f acceleration_scale(_param_mpc_acc_hor.get(), _param_mpc_acc_hor.get());
|
||||
Vector2f velocity_scale(_param_mpc_vel_manual.get(), _param_mpc_vel_manual.get());
|
||||
|
||||
acceleration_scale *= 2.f; // because of drag the average aceleration is half
|
||||
|
||||
// Map stick input to acceleration
|
||||
Sticks::limitStickUnitLengthXY(stick_xy);
|
||||
Sticks::rotateIntoHeadingFrameXY(stick_xy, yaw, yaw_sp);
|
||||
_acceleration = stick_xy.emult(acceleration_scale);
|
||||
applyFeasibilityLimit(_acceleration, dt);
|
||||
|
||||
// Add drag to limit speed and brake again
|
||||
_acceleration -= calculateDrag(acceleration_scale.edivide(velocity_scale), dt, stick_xy, _velocity);
|
||||
|
||||
applyTiltLimit(_acceleration);
|
||||
|
||||
// Generate velocity setpoint by forward integrating commanded acceleration
|
||||
_velocity += Vector2f(_acceleration) * dt;
|
||||
|
||||
lockPosition(_velocity, pos, dt, _position);
|
||||
}
|
||||
|
||||
void StickAccelerationXY::getSetpoints(Vector3f &pos_sp, Vector3f &vel_sp, Vector3f &acc_sp)
|
||||
{
|
||||
pos_sp.xy() = _position;
|
||||
vel_sp.xy() = _velocity;
|
||||
acc_sp.xy() = _acceleration;
|
||||
}
|
||||
|
||||
void StickAccelerationXY::applyFeasibilityLimit(Vector2f &acceleration, const float dt)
|
||||
{
|
||||
// Apply jerk limit - acceleration slew rate
|
||||
_acceleration_slew_rate_x.setSlewRate(_param_mpc_jerk_max.get());
|
||||
_acceleration_slew_rate_y.setSlewRate(_param_mpc_jerk_max.get());
|
||||
acceleration(0) = _acceleration_slew_rate_x.update(acceleration(0), dt);
|
||||
acceleration(1) = _acceleration_slew_rate_y.update(acceleration(1), dt);
|
||||
}
|
||||
|
||||
Vector2f StickAccelerationXY::calculateDrag(Vector2f drag_coefficient, const float dt, const Vector2f &stick_xy,
|
||||
const Vector2f &vel_sp)
|
||||
{
|
||||
_brake_boost_filter.setParameters(dt, .8f);
|
||||
|
||||
if (stick_xy.norm_squared() < FLT_EPSILON) {
|
||||
_brake_boost_filter.update(2.f);
|
||||
|
||||
} else {
|
||||
_brake_boost_filter.update(1.f);
|
||||
}
|
||||
|
||||
drag_coefficient *= _brake_boost_filter.getState();
|
||||
|
||||
// increase drag with sqareroot function when velocity is lower than 1m/s
|
||||
const Vector2f velocity_with_sqrt_boost = vel_sp.unit_or_zero() * math::sqrt_linear(vel_sp.norm());
|
||||
return drag_coefficient.emult(velocity_with_sqrt_boost);
|
||||
}
|
||||
|
||||
void StickAccelerationXY::applyTiltLimit(Vector2f &acceleration)
|
||||
{
|
||||
// Check if acceleration would exceed the tilt limit
|
||||
const float acc = acceleration.length();
|
||||
const float acc_tilt_max = tanf(M_DEG_TO_RAD_F * _param_mpc_tiltmax_air.get()) * CONSTANTS_ONE_G;
|
||||
|
||||
if (acc > acc_tilt_max) {
|
||||
acceleration *= acc_tilt_max / acc;
|
||||
}
|
||||
}
|
||||
|
||||
void StickAccelerationXY::lockPosition(const Vector2f &vel_sp, const Vector3f &pos, const float dt, Vector2f &pos_sp)
|
||||
{
|
||||
if (vel_sp.norm_squared() < FLT_EPSILON) {
|
||||
if (!PX4_ISFINITE(pos_sp(0))) {
|
||||
pos_sp = Vector2f(pos);
|
||||
}
|
||||
|
||||
} else {
|
||||
pos_sp.setNaN();
|
||||
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,84 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 StickAccelerationXY.hpp
|
||||
* @brief Generate horizontal position, velocity and acceleration from stick input
|
||||
* @author Matthias Grob <maetugr@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <float.h> // TODO add this include to AlphaFilter since it's used there
|
||||
#include <lib/ecl/AlphaFilter/AlphaFilter.hpp>
|
||||
#include <matrix/math.hpp>
|
||||
|
||||
#include "SlewRate.hpp"
|
||||
|
||||
class StickAccelerationXY : public ModuleParams
|
||||
{
|
||||
public:
|
||||
StickAccelerationXY(ModuleParams *parent);
|
||||
~StickAccelerationXY() = default;
|
||||
|
||||
void resetPosition();
|
||||
void resetVelocity(const matrix::Vector2f &velocity);
|
||||
void resetAcceleration(const matrix::Vector2f &acceleration);
|
||||
void generateSetpoints(matrix::Vector2f stick_xy, const float yaw, const float yaw_sp, const matrix::Vector3f &pos,
|
||||
const float dt);
|
||||
void getSetpoints(matrix::Vector3f &pos_sp, matrix::Vector3f &vel_sp, matrix::Vector3f &acc_sp);
|
||||
|
||||
private:
|
||||
void applyFeasibilityLimit(matrix::Vector2f &acceleration, const float dt);
|
||||
matrix::Vector2f calculateDrag(matrix::Vector2f drag_coefficient, const float dt, const matrix::Vector2f &stick_xy,
|
||||
const matrix::Vector2f &vel_sp);
|
||||
void applyTiltLimit(matrix::Vector2f &acceleration);
|
||||
void lockPosition(const matrix::Vector2f &vel_sp, const matrix::Vector3f &pos, const float dt,
|
||||
matrix::Vector2f &pos_sp);
|
||||
|
||||
SlewRate<float> _acceleration_slew_rate_x;
|
||||
SlewRate<float> _acceleration_slew_rate_y;
|
||||
AlphaFilter<float> _brake_boost_filter;
|
||||
|
||||
matrix::Vector2f _position;
|
||||
matrix::Vector2f _velocity;
|
||||
matrix::Vector2f _acceleration;
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::MPC_VEL_MANUAL>) _param_mpc_vel_manual,
|
||||
(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor,
|
||||
(ParamFloat<px4::params::MPC_JERK_MAX>) _param_mpc_jerk_max,
|
||||
(ParamFloat<px4::params::MPC_TILTMAX_AIR>) _param_mpc_tiltmax_air
|
||||
)
|
||||
};
|
||||
@@ -0,0 +1,66 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 StickYaw.cpp
|
||||
*/
|
||||
|
||||
#include "StickYaw.hpp"
|
||||
|
||||
#include <px4_platform_common/defines.h>
|
||||
|
||||
void StickYaw::generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint, const float desired_yawspeed,
|
||||
const float yaw, const float deltatime)
|
||||
{
|
||||
_yawspeed_slew_rate.setSlewRate(2.f * M_PI_F);
|
||||
yawspeed_setpoint = _yawspeed_slew_rate.update(desired_yawspeed, deltatime);
|
||||
yaw_setpoint = updateYawLock(yaw, yawspeed_setpoint, yaw_setpoint);
|
||||
}
|
||||
|
||||
float StickYaw::updateYawLock(const float yaw, const float yawspeed_setpoint, const float yaw_setpoint)
|
||||
{
|
||||
// Yaw-lock depends on desired yawspeed input. If not locked, yaw_sp is set to NAN.
|
||||
if (fabsf(yawspeed_setpoint) > FLT_EPSILON) {
|
||||
// no fixed heading when rotating around yaw by stick
|
||||
return NAN;
|
||||
|
||||
} else {
|
||||
// break down and hold the current heading when no more rotation commanded
|
||||
if (!PX4_ISFINITE(yaw_setpoint)) {
|
||||
return yaw;
|
||||
|
||||
} else {
|
||||
return yaw_setpoint;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,68 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 StickYaw.hpp
|
||||
* @brief Generate yaw and angular yawspeed setpoints from stick input
|
||||
* @author Matthias Grob <maetugr@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "SlewRate.hpp"
|
||||
|
||||
class StickYaw
|
||||
{
|
||||
public:
|
||||
StickYaw() = default;
|
||||
~StickYaw() = default;
|
||||
|
||||
void generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint, const float desired_yawspeed, const float yaw,
|
||||
const float deltatime);
|
||||
|
||||
private:
|
||||
SlewRate<float> _yawspeed_slew_rate;
|
||||
|
||||
/**
|
||||
* Lock yaw when not currently turning
|
||||
* When applying a yawspeed the vehicle is turning, when the speed is
|
||||
* set to zero the vehicle needs to slow down and then lock at the yaw
|
||||
* it stops at to not drift over time.
|
||||
* @param yawspeed current yaw rotational rate state
|
||||
* @param yaw current yaw rotational rate state
|
||||
* @param yawspeed_setpoint rotation rate at which to turn around yaw axis
|
||||
* @param yaw current yaw setpoint which then will be overwritten by the return value
|
||||
* @return yaw setpoint to execute to have a yaw lock at the correct moment in time
|
||||
*/
|
||||
static float updateYawLock(const float yaw, const float yawspeed_setpoint, const float yaw_setpoint);
|
||||
};
|
||||
@@ -0,0 +1,106 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 Sticks.cpp
|
||||
*/
|
||||
|
||||
#include "Sticks.hpp"
|
||||
|
||||
using namespace time_literals;
|
||||
using namespace matrix;
|
||||
|
||||
Sticks::Sticks(ModuleParams *parent) :
|
||||
ModuleParams(parent)
|
||||
{};
|
||||
|
||||
bool Sticks::checkAndSetStickInputs()
|
||||
{
|
||||
// Sticks are rescaled linearly and exponentially to [-1,1]
|
||||
manual_control_setpoint_s manual_control_setpoint;
|
||||
|
||||
if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
|
||||
// Linear scale
|
||||
_positions(0) = manual_control_setpoint.x; // NED x, pitch [-1,1]
|
||||
_positions(1) = manual_control_setpoint.y; // NED y, roll [-1,1]
|
||||
_positions(2) = -(manual_control_setpoint.z - 0.5f) * 2.f; // NED z, thrust resacaled from [0,1] to [-1,1]
|
||||
_positions(3) = manual_control_setpoint.r; // yaw [-1,1]
|
||||
|
||||
// Exponential scale
|
||||
_positions_expo(0) = math::expo_deadzone(_positions(0), _param_mpc_xy_man_expo.get(), _param_mpc_hold_dz.get());
|
||||
_positions_expo(1) = math::expo_deadzone(_positions(1), _param_mpc_xy_man_expo.get(), _param_mpc_hold_dz.get());
|
||||
_positions_expo(2) = math::expo_deadzone(_positions(2), _param_mpc_z_man_expo.get(), _param_mpc_hold_dz.get());
|
||||
_positions_expo(3) = math::expo_deadzone(_positions(3), _param_mpc_yaw_expo.get(), _param_mpc_hold_dz.get());
|
||||
|
||||
// valid stick inputs are required
|
||||
const bool valid_sticks = PX4_ISFINITE(_positions(0))
|
||||
&& PX4_ISFINITE(_positions(1))
|
||||
&& PX4_ISFINITE(_positions(2))
|
||||
&& PX4_ISFINITE(_positions(3));
|
||||
|
||||
_input_available = valid_sticks;
|
||||
|
||||
} else {
|
||||
vehicle_status_s vehicle_status;
|
||||
|
||||
if (_vehicle_status_sub.update(&vehicle_status)) {
|
||||
if (vehicle_status.rc_signal_lost) {
|
||||
_input_available = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (!_input_available) {
|
||||
// Timeout: set all sticks to zero
|
||||
_positions.zero();
|
||||
_positions_expo.zero();
|
||||
}
|
||||
|
||||
return _input_available;
|
||||
}
|
||||
|
||||
void Sticks::limitStickUnitLengthXY(Vector2f &v)
|
||||
{
|
||||
const float vl = v.length();
|
||||
|
||||
if (vl > 1.0f) {
|
||||
v /= vl;
|
||||
}
|
||||
}
|
||||
|
||||
void Sticks::rotateIntoHeadingFrameXY(Vector2f &v, const float yaw, const float yaw_setpoint)
|
||||
{
|
||||
Vector3f v3(v(0), v(1), 0.f);
|
||||
const float yaw_rotate = PX4_ISFINITE(yaw_setpoint) ? yaw_setpoint : yaw;
|
||||
v = Vector2f(Dcmf(Eulerf(0.0f, 0.0f, yaw_rotate)) * v3);
|
||||
}
|
||||
@@ -0,0 +1,90 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 Sticks.hpp
|
||||
*
|
||||
* Library abstracting stick input from manual_control_setpoint
|
||||
*
|
||||
* @author Matthias Grob <maetugr@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <matrix/matrix/math.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
|
||||
class Sticks : public ModuleParams
|
||||
{
|
||||
public:
|
||||
Sticks(ModuleParams *parent);
|
||||
~Sticks() = default;
|
||||
|
||||
bool checkAndSetStickInputs();
|
||||
bool isAvailable() { return _input_available; };
|
||||
const matrix::Vector<float, 4> &getPosition() { return _positions; };
|
||||
const matrix::Vector<float, 4> &getPositionExpo() { return _positions_expo; };
|
||||
|
||||
|
||||
/**
|
||||
* Limit the the horizontal input from a square shaped joystick gimbal to a unit circle
|
||||
* @param v Vector containing x, y, z axis of the joystick gimbal. x, y get adjusted
|
||||
*/
|
||||
static void limitStickUnitLengthXY(matrix::Vector2f &v);
|
||||
|
||||
/**
|
||||
* Rotate horizontal component of joystick input into the vehicle body orientation
|
||||
* @param v Vector containing x, y, z axis of the joystick input.
|
||||
* @param yaw Current vehicle yaw heading
|
||||
* @param yaw_setpoint Current yaw setpoint if it's locked else NAN
|
||||
*/
|
||||
static void rotateIntoHeadingFrameXY(matrix::Vector2f &v, const float yaw, const float yaw_setpoint);
|
||||
|
||||
private:
|
||||
bool _input_available{false};
|
||||
matrix::Vector<float, 4> _positions; ///< unmodified manual stick inputs
|
||||
matrix::Vector<float, 4> _positions_expo; ///< modified manual sticks using expo function
|
||||
|
||||
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
|
||||
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::MPC_HOLD_DZ>) _param_mpc_hold_dz,
|
||||
(ParamFloat<px4::params::MPC_XY_MAN_EXPO>) _param_mpc_xy_man_expo,
|
||||
(ParamFloat<px4::params::MPC_Z_MAN_EXPO>) _param_mpc_z_man_expo,
|
||||
(ParamFloat<px4::params::MPC_YAW_EXPO>) _param_mpc_yaw_expo
|
||||
)
|
||||
};
|
||||
@@ -0,0 +1,90 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 StraightLine.cpp
|
||||
*/
|
||||
|
||||
#include "StraightLine.hpp"
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <px4_platform_common/defines.h>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
void StraightLine::generateSetpoints(matrix::Vector3f &position_setpoint, matrix::Vector3f &velocity_setpoint)
|
||||
{
|
||||
if (isEndReached()) {
|
||||
// Vehicle has reached target, lock position
|
||||
position_setpoint = _end;
|
||||
velocity_setpoint.setNaN();
|
||||
return;
|
||||
}
|
||||
|
||||
Vector3f start_to_end = _end - _start;
|
||||
float distance_start_to_end = start_to_end.norm();
|
||||
|
||||
// capture progress as ratio between 0 and 1 of entire distance
|
||||
Vector3f vehicle_to_end = _end - _position;
|
||||
float distance_vehicle_to_end = vehicle_to_end.norm();
|
||||
float distance_from_start = Vector3f(_start - _position).norm();
|
||||
float progress = distance_from_start / (distance_from_start + distance_vehicle_to_end);
|
||||
|
||||
float distance_from_boundary = 0.f;
|
||||
|
||||
// calculate the distance to the closer boundary
|
||||
if (progress < 0.5f) {
|
||||
distance_from_boundary = (2 * progress) * distance_start_to_end;
|
||||
|
||||
} else {
|
||||
distance_from_boundary = (2 * (1 - progress)) * distance_start_to_end;
|
||||
}
|
||||
|
||||
// ramp velocity based on the distance to the boundary
|
||||
float velocity = 0.5f + (distance_from_boundary / 4.f);
|
||||
velocity = math::min(velocity, _speed);
|
||||
velocity_setpoint = vehicle_to_end.unit_or_zero() * velocity;
|
||||
|
||||
// check if we plan to go against the line direction which indicates we reached the goal
|
||||
if (start_to_end * vehicle_to_end < 0) {
|
||||
_end_reached = true;
|
||||
}
|
||||
}
|
||||
|
||||
void StraightLine::setLineFromTo(const Vector3f &start, const Vector3f &end)
|
||||
{
|
||||
if (PX4_ISFINITE(start.norm_squared()) && PX4_ISFINITE(end.norm_squared())) {
|
||||
_start = start;
|
||||
_end = end;
|
||||
_end_reached = false;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,81 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 StraightLine.hpp
|
||||
*
|
||||
* lib to return setpoints on a straight line
|
||||
*
|
||||
* @author Christoph Tobler <christoph@px4.io>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <matrix/matrix/math.hpp>
|
||||
|
||||
class StraightLine
|
||||
{
|
||||
public:
|
||||
StraightLine(const matrix::Vector3f &pos) : _position(pos) {};
|
||||
~StraightLine() = default;
|
||||
|
||||
// setter functions
|
||||
void setLineFromTo(const matrix::Vector3f &start, const matrix::Vector3f &end);
|
||||
void setSpeed(const float &speed) { _speed = speed; };
|
||||
|
||||
/**
|
||||
* Generate setpoints on a straight line according to parameters
|
||||
*
|
||||
* @param position_setpoint: reference to the 3D vector with the position setpoint to update
|
||||
* @param velocity_setpoint: reference to the 3D vector with the velocity setpoint to update
|
||||
*/
|
||||
void generateSetpoints(matrix::Vector3f &position_setpoint, matrix::Vector3f &velocity_setpoint);
|
||||
|
||||
/**
|
||||
* Check if the end was reached
|
||||
*
|
||||
* @return false when on the way from start to end, true when end was reached
|
||||
*/
|
||||
bool isEndReached() const { return _end_reached; }
|
||||
|
||||
void reset() { _end_reached = true; }
|
||||
|
||||
private:
|
||||
const matrix::Vector3f &_position; /**< vehicle position (dependency injection) */
|
||||
|
||||
matrix::Vector3f _start; /**< Start point of the straight line */
|
||||
matrix::Vector3f _end; /**< End point of the straight line */
|
||||
float _speed = 1.f; /**< desired speed between accelerating and decelerating */
|
||||
|
||||
bool _end_reached = true; /**< Flag to lock further movement when end is reached */
|
||||
};
|
||||
Reference in New Issue
Block a user