fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

FlightTasks: remove FlightTaskManualPositionSmooth

Browse Source 
alias first smooth position control version from 2017.

RIP it served well.
This commit is contained in:
Matthias Grob 2020-11-11 20:00:52 +01:00
parent e6338d8a2f
commit ff801fbc08
20 changed files with 5 additions and 1309 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/lib/flight_tasks/CMakeLists.txt
View File

@ -52,10 +52,8 @@ endif()
# add core flight tasks to list
list(APPEND flight_tasks_all
ManualAltitude
ManualAltitudeSmooth
ManualAltitudeSmoothVel
ManualPosition
ManualPositionSmooth
ManualPositionSmoothVel
AutoLineSmoothVel
AutoFollowMe

39
src/lib/flight_tasks/tasks/ManualAltitudeSmooth/CMakeLists.txt
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(FlightTaskManualAltitudeSmooth
FlightTaskManualAltitudeSmooth.cpp
)
target_link_libraries(FlightTaskManualAltitudeSmooth PUBLIC FlightTaskManualAltitude FlightTaskUtility)
target_include_directories(FlightTaskManualAltitudeSmooth PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

56
src/lib/flight_tasks/tasks/ManualAltitudeSmooth/FlightTaskManualAltitudeSmooth.cpp
View File

@ -1,56 +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 FlightManualAltitude.cpp
*/
#include "FlightTaskManualAltitudeSmooth.hpp"
using namespace matrix;
FlightTaskManualAltitudeSmooth::FlightTaskManualAltitudeSmooth() :
_smoothing(this, _velocity(2), _sticks.getPosition()(2))
{}
void FlightTaskManualAltitudeSmooth::_updateSetpoints()
{
/* Get yaw, thrust */
FlightTaskManualAltitude::_updateSetpoints();
/* Smooth velocity in z*/
_smoothing.smoothVelFromSticks(_velocity_setpoint(2), _deltatime);
/* Check for altitude lock*/
_updateAltitudeLock();
}

56
src/lib/flight_tasks/tasks/ManualAltitudeSmooth/FlightTaskManualAltitudeSmooth.hpp
View File

@ -1,56 +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 FlightManualAltitude.hpp
*
* Flight task for manual controlled altitude.
*/
#pragma once
#include "FlightTaskManualAltitude.hpp"
#include "ManualSmoothingZ.hpp"
class FlightTaskManualAltitudeSmooth : public FlightTaskManualAltitude
{
public:
FlightTaskManualAltitudeSmooth();
virtual ~FlightTaskManualAltitudeSmooth() = default;
protected:
virtual void _updateSetpoints() override;
private:
ManualSmoothingZ _smoothing; /**< smoothing for velocity setpoints */
};

39
src/lib/flight_tasks/tasks/ManualPositionSmooth/CMakeLists.txt
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(FlightTaskManualPositionSmooth
FlightTaskManualPositionSmooth.cpp
)
target_link_libraries(FlightTaskManualPositionSmooth PUBLIC FlightTaskManualPosition FlightTaskUtility)
target_include_directories(FlightTaskManualPositionSmooth PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

68
src/lib/flight_tasks/tasks/ManualPositionSmooth/FlightTaskManualPositionSmooth.cpp
View File

@ -1,68 +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 FlightManualPositionSmooth.cpp
*/
#include "FlightTaskManualPositionSmooth.hpp"
using namespace matrix;
FlightTaskManualPositionSmooth::FlightTaskManualPositionSmooth() :
_smoothingXY(this, Vector2f(_velocity)),
_smoothingZ(this, _velocity(2), _sticks.getPosition()(2))
{}
void FlightTaskManualPositionSmooth::_updateSetpoints()
{
/* Get yaw setpont, un-smoothed position setpoints.*/
FlightTaskManualPosition::_updateSetpoints();
/* Smooth velocity setpoint in xy.*/
Vector2f vel(_velocity);
Vector2f vel_sp_xy(_velocity_setpoint);
_smoothingXY.updateMaxVelocity(_constraints.speed_xy);
_smoothingXY.smoothVelocity(vel_sp_xy, vel, _yaw, _yawspeed_setpoint, _deltatime);
_velocity_setpoint(0) = vel_sp_xy(0);
_velocity_setpoint(1) = vel_sp_xy(1);
/* Check for xy position lock.*/
_updateXYlock();
/* Smooth velocity in z.*/
_smoothingZ.smoothVelFromSticks(_velocity_setpoint(2), _deltatime);
/* Check for altitude lock*/
_updateAltitudeLock();
}

60
src/lib/flight_tasks/tasks/ManualPositionSmooth/FlightTaskManualPositionSmooth.hpp
View File

@ -1,60 +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 FlightManualPositionSmooth.hpp
*
* Flight task for smooth manual controlled position.
*/
#pragma once
#include "FlightTaskManualPosition.hpp"
#include "ManualSmoothingXY.hpp"
#include "ManualSmoothingZ.hpp"
class FlightTaskManualPositionSmooth : public FlightTaskManualPosition
{
public:
FlightTaskManualPositionSmooth();
virtual ~FlightTaskManualPositionSmooth() = default;
protected:
virtual void _updateSetpoints() override;
private:
ManualSmoothingXY _smoothingXY; /**< smoothing for velocity setpoints in xy */
ManualSmoothingZ _smoothingZ; /**< smoothing for velocity in z */
};

2
src/lib/flight_tasks/tasks/Orbit/CMakeLists.txt
View File

@ -35,5 +35,5 @@ px4_add_library(FlightTaskOrbit
FlightTaskOrbit.cpp
)
target_link_libraries(FlightTaskOrbit PUBLIC FlightTaskManualAltitudeSmooth)
target_link_libraries(FlightTaskOrbit PUBLIC FlightTaskManualAltitudeSmoothVel)
target_include_directories(FlightTaskOrbit PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

4
src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp
View File

@ -156,7 +156,7 @@ bool FlightTaskOrbit::checkAcceleration(float r, float v, float a)
bool FlightTaskOrbit::activate(const vehicle_local_position_setpoint_s &last_setpoint)
{
bool ret = FlightTaskManualAltitudeSmooth::activate(last_setpoint);
bool ret = FlightTaskManualAltitudeSmoothVel::activate(last_setpoint);
_r = _radius_min;
_v = 1.f;
_center = Vector2f(_position);
@ -177,7 +177,7 @@ bool FlightTaskOrbit::activate(const vehicle_local_position_setpoint_s &last_set
bool FlightTaskOrbit::update()
{
// update altitude
bool ret = FlightTaskManualAltitudeSmooth::update();
bool ret = FlightTaskManualAltitudeSmoothVel::update();
// stick input adjusts parameters within a fixed time frame
const float r = _r - _sticks.getPositionExpo()(0) * _deltatime * (_radius_max / 8.f);

4
src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.hpp
View File

@ -41,12 +41,12 @@
#pragma once
#include "FlightTaskManualAltitudeSmooth.hpp"
#include "FlightTaskManualAltitudeSmoothVel.hpp"
#include <uORB/Publication.hpp>
#include <uORB/topics/orbit_status.h>
#include <StraightLine.hpp>
class FlightTaskOrbit : public FlightTaskManualAltitudeSmooth
class FlightTaskOrbit : public FlightTaskManualAltitudeSmoothVel
{
public:
FlightTaskOrbit();

2
src/lib/flight_tasks/tasks/Utility/CMakeLists.txt
View File

@ -32,8 +32,6 @@
############################################################################
px4_add_library(FlightTaskUtility
ManualSmoothingZ.cpp
ManualSmoothingXY.cpp
StraightLine.cpp
VelocitySmoothing.cpp
ManualVelocitySmoothingXY.cpp

227
src/lib/flight_tasks/tasks/Utility/ManualSmoothingXY.cpp
View File

@ -1,227 +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 ManualSmoothingXY.cpp
*/
#include "ManualSmoothingXY.hpp"
#include <mathlib/mathlib.h>
#include <float.h>
#include <px4_platform_common/defines.h>
using namespace matrix;
ManualSmoothingXY::ManualSmoothingXY(ModuleParams *parent, const Vector2f &vel) :
ModuleParams(parent), _vel_sp_prev(vel)
{
_acc_state_dependent = _param_mpc_acc_hor.get();
_jerk_state_dependent = _param_mpc_jerk_max.get();
}
void
ManualSmoothingXY::smoothVelocity(Vector2f &vel_sp, const Vector2f &vel, const float &yaw,
const float &yawrate_sp, const float dt)
{
_updateAcceleration(vel_sp, vel, yaw, yawrate_sp, dt);
_velocitySlewRate(vel_sp, dt);
_vel_sp_prev = vel_sp;
}
void
ManualSmoothingXY::_updateAcceleration(Vector2f &vel_sp, const Vector2f &vel, const float &yaw,
const float &yawrate_sp, const float dt)
{
Intention intention = _getIntention(vel_sp, vel, yaw, yawrate_sp);
// Adapt acceleration and jerk based on current state and
// intention. Jerk is only used for braking.
_setStateAcceleration(vel_sp, vel, intention, dt);
}
ManualSmoothingXY::Intention
ManualSmoothingXY::_getIntention(const Vector2f &vel_sp, const Vector2f &vel, const float &yaw,
const float &yawrate_sp)
{
if (vel_sp.length() > FLT_EPSILON) {
// Distinguish between acceleration, deceleration and direction change
// Check if stick direction and current velocity are within 135.
// If current setpoint and velocity are more than 135 apart, we assume
// that the user demanded a direction change.
// The detection is done in body frame.
// Rotate velocity setpoint into body frame
Vector2f vel_sp_heading = _getWorldToHeadingFrame(vel_sp, yaw);
Vector2f vel_heading = _getWorldToHeadingFrame(vel, yaw);
if (vel_sp_heading.length() > FLT_EPSILON) {
vel_sp_heading.normalize();
}
if (vel_heading.length() > FLT_EPSILON) {
vel_heading.normalize();
}
const bool is_aligned = (vel_sp_heading * vel_heading) > -0.707f;
// In almost all cases we want to use acceleration.
// Only use direction change if not aligned, no yawspeed demand, demand larger than 0.7 of max speed and velocity larger than 2m/s.
// Only use deceleration if stick input is lower than previous setpoint, aligned and no yawspeed demand.
bool yawspeed_demand = fabsf(yawrate_sp) > 0.05f && PX4_ISFINITE(yawrate_sp);
bool direction_change = !is_aligned && (vel_sp.length() > 0.7f * _vel_max) && !yawspeed_demand
&& (vel.length() > 2.0f);
bool deceleration = is_aligned && (vel_sp.length() < _vel_sp_prev.length()) && !yawspeed_demand;
if (direction_change) {
return Intention::direction_change;
} else if (deceleration) {
return Intention::deceleration;
} else {
return Intention::acceleration;
}
}
return Intention::brake; //default is brake
}
void
ManualSmoothingXY::_setStateAcceleration(const Vector2f &vel_sp, const Vector2f &vel,
const Intention &intention, const float dt)
{
switch (intention) {
case Intention::brake: {
if (_intention == Intention::direction_change) {
// Vehicle switched from direction change to brake.
// Don't do any slwerate and just stop.
_jerk_state_dependent = 1e6f;
_vel_sp_prev = vel;
} else if (intention != _intention) {
// start the brake with lowest acceleration which
// makes stopping smoother
_acc_state_dependent = _param_mpc_dec_hor_slow.get();
// Adjust jerk based on current velocity. This ensures
// that the vehicle will stop much quicker at large speed but
// very slow at low speed.
_jerk_state_dependent = 1e6f; // default
if (_param_mpc_jerk_max.get() > _param_mpc_jerk_min.get() && _param_mpc_jerk_min.get() > FLT_EPSILON) {
_jerk_state_dependent = math::min((_param_mpc_jerk_max.get() - _param_mpc_jerk_min.get())
/ _vel_max * vel.length() + _param_mpc_jerk_min.get(), _param_mpc_jerk_max.get());
}
// User wants to brake smoothly but does NOT want the vehicle to
// continue to fly in the opposite direction. slewrate has to be reset
// by setting previous velocity setpoint to current velocity. */
_vel_sp_prev = vel;
}
/* limit jerk when braking to zero */
float jerk = (_param_mpc_acc_hor_max.get() - _acc_state_dependent) / dt;
if (jerk > _jerk_state_dependent) {
_acc_state_dependent = _jerk_state_dependent * dt
+ _acc_state_dependent;
} else {
_acc_state_dependent = _param_mpc_acc_hor_max.get();
}
break;
}
case Intention::direction_change: {
// We allow for fast change by setting previous setpoint to current setpoint.
// Because previous setpoint is equal to current setpoint,
// slewrate will have no effect. Nonetheless, just set
// acceleration to maximum.
_acc_state_dependent = _param_mpc_acc_hor_max.get();
break;
}
case Intention::acceleration: {
// Limit acceleration linearly based on velocity setpoint.
_acc_state_dependent = (_param_mpc_acc_hor.get() - _param_mpc_dec_hor_slow.get())
/ _vel_max * vel_sp.length() + _param_mpc_dec_hor_slow.get();
break;
}
case Intention::deceleration: {
_acc_state_dependent = _param_mpc_dec_hor_slow.get();
break;
}
}
// Update intention for next iteration.
_intention = intention;
}
void
ManualSmoothingXY::_velocitySlewRate(Vector2f &vel_sp, const float dt)
{
// Adjust velocity setpoint if demand exceeds acceleration. /
Vector2f acc{};
if (dt > FLT_EPSILON) {
acc = (vel_sp - _vel_sp_prev) / dt;
}
if (acc.length() > _acc_state_dependent) {
vel_sp = acc.normalized() * _acc_state_dependent * dt + _vel_sp_prev;
}
}
Vector2f
ManualSmoothingXY::_getWorldToHeadingFrame(const Vector2f &vec, const float &yaw)
{
Quatf q_yaw = AxisAnglef(Vector3f(0.0f, 0.0f, 1.0f), yaw);
Vector3f vec_heading = q_yaw.conjugate_inversed(Vector3f(vec(0), vec(1), 0.0f));
return Vector2f(vec_heading);
}
Vector2f
ManualSmoothingXY::_getHeadingToWorldFrame(const Vector2f &vec, const float &yaw)
{
Quatf q_yaw = AxisAnglef(Vector3f(0.0f, 0.0f, 1.0f), yaw);
Vector3f vec_world = q_yaw.conjugate(Vector3f(vec(0), vec(1), 0.0f));
return Vector2f(vec_world);
}

175
src/lib/flight_tasks/tasks/Utility/ManualSmoothingXY.hpp
View File

@ -1,175 +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 SmoothingXY.hpp
*
* This Class is used for smoothing the velocity setpoint in XY-direction.
* The velocity setpoint is smoothed by applying a velocity change limit, which
* we call acceleration. Depending on the user intention, the acceleration limit
* will differ.
* In manual mode we consider four states with different acceleration handling:
* 1. user wants to stop
* 2. user wants to quickly change direction
* 3. user wants to accelerate
* 4. user wants to decelerate
*/
#pragma once
#include <px4_platform_common/module_params.h>
#include <matrix/matrix/math.hpp>
class ManualSmoothingXY : public ModuleParams
{
public:
ManualSmoothingXY(ModuleParams *parent, const matrix::Vector2f &vel);
~ManualSmoothingXY() = default;
/**
* Maximum velocity is required to detect user intention.
* Maximum velocity depends on flight-task.
* In order to deduce user intention from velocity, the maximum
* allowed velocity has to be updated.
* @param vel_max corresponds to vehicle constraint
*/
void updateMaxVelocity(const float &vel_max) {_vel_max = vel_max;};
/**
* Smoothing of velocity setpoint horizontally based
* on flight direction.
* @param vel_sp: velocity setpoint in xy
* @param dt: time delta in seconds
*/
void smoothVelocity(matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float &yaw,
const float &yawrate_sp, const float dt);
/*
* User intention.
* - brake when user demands a brake
* - acceleration when vehicle keeps speed or accelerates in the same direction
* - deceleration when vehicle slows down in the same direction
* - direction_change whne vehcile demands an abrupt direction change
*/
enum class Intention {
brake,
acceleration,
deceleration,
direction_change
};
/**
* Get user intention.
* @see Intention
*/
Intention getIntention() { return _intention; }
/**
* Overwrite methods:
* Needed if different parameter values than default required.
*/
void overwriteHoverAcceleration(float acc) { _param_mpc_acc_hor_max.set(acc); }
void overwriteMaxAcceleration(float acc) { _param_mpc_acc_hor.set(acc); }
void overwriteDecelerationMin(float dec) { _param_mpc_dec_hor_slow.set(dec); }
void overwriteJerkMax(float jerk) { _param_mpc_jerk_max.set(jerk); }
void overwriteJerkMin(float jerk) { _param_mpc_jerk_min.set(jerk); }
private:
/**
* Sets velocity change limits (=acceleration).
* Depending on the user intention, the acceleration differs.
* @param vel_sp is desired velocity setpoint before slewrate.
* @param vel is current velocity in horizontal direction
* @param yaw is vehicle yaw
* @param yawrate_sp is desired yawspeed
* @param dt is delta-time
*/
void _updateAcceleration(matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float &yaw,
const float &yawrate_sp, const float dt);
/**
* Gets user intention.
* The intention is deduced from desired velocity setpoint.
* @param vel_sp is desired velocity setpoint before slewrate.
* @param vel is vehicle velocity in xy-direction.
* @param yaw is vehicle yaw
* @param yawrate_sp is the desired yaw-speed
*/
Intention _getIntention(const matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float &yaw,
const float &yawrate_sp);
/**
* Set acceleration depending on Intention.
* @param vel_sp is desired velocity septoint in xy-direction
* @param vel is vehicle velociy in xy-direction
* @param intention is the user intention during flight
* @param dt is delta-time
*/
void _setStateAcceleration(const matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const Intention &intention,
const float dt);
/**
* Limits the velocity setpoint change.
* @param vel_sp that gets limited based on acceleration and previous velocity setpoint
* @param dt is delta-time
*/
void _velocitySlewRate(matrix::Vector2f &vel_sp, const float dt);
/**
* Rotate vector from local frame into heading frame.
* @param vec is an arbitrary vector in local frame
* @param yaw is the vehicle heading
*/
matrix::Vector2f _getWorldToHeadingFrame(const matrix::Vector2f &vec, const float &yaw);
/**
* Rotate vector from heading frame to local frame.
* @param vec is an arbitrary vector in heading frame
* @param yaw is the vehicle yaw
*/
matrix::Vector2f _getHeadingToWorldFrame(const matrix::Vector2f &vec, const float &yaw);
Intention _intention{Intention::acceleration}; /**< user intention */
float _acc_state_dependent; /**< velocity change limit that depends on Intention */
float _jerk_state_dependent; /**< acceleration change limit during brake */
float _vel_max{10.0f}; /**< maximum horizontal speed allowed */
matrix::Vector2f _vel_sp_prev; /**< previous velocity setpoint */
DEFINE_PARAMETERS(
(ParamFloat<px4::params::MPC_ACC_HOR_MAX>) _param_mpc_acc_hor_max, /**< acceleration in hover */
(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor, /**< acceleration in flight */
(ParamFloat<px4::params::MPC_DEC_HOR_SLOW>) _param_mpc_dec_hor_slow, /**< deceleration in flight */
(ParamFloat<px4::params::MPC_JERK_MIN>) _param_mpc_jerk_min, /**< jerk min during brake */
(ParamFloat<px4::params::MPC_JERK_MAX>) _param_mpc_jerk_max, /**< jerk max during brake */
(ParamFloat<px4::params::MPC_VEL_MANUAL>) _param_mpc_vel_manual /**< maximum velocity in manual controlled mode */
)
};

159
src/lib/flight_tasks/tasks/Utility/ManualSmoothingZ.cpp
View File

@ -1,159 +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 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 ManualSmoothingZ.cpp
*/
#include "ManualSmoothingZ.hpp"
#include <mathlib/mathlib.h>
#include <float.h>
ManualSmoothingZ::ManualSmoothingZ(ModuleParams *parent, const float &vel, const float &stick) :
ModuleParams(parent),
_vel(vel), _stick(stick), _vel_sp_prev(vel)
{
_acc_state_dependent = _param_mpc_acc_up_max.get();
_max_acceleration = _param_mpc_acc_up_max.get();
}
void
ManualSmoothingZ::smoothVelFromSticks(float &vel_sp, const float dt)
{
updateAcceleration(vel_sp, dt);
velocitySlewRate(vel_sp, dt);
_vel_sp_prev = vel_sp;
}
void
ManualSmoothingZ::updateAcceleration(float &vel_sp, const float dt)
{
// check for max acceleration
setMaxAcceleration();
// check if zero input stick
const bool is_current_zero = (fabsf(_stick) <= FLT_EPSILON);
// default is acceleration
ManualIntentionZ intention = ManualIntentionZ::acceleration;
// check zero input stick
if (is_current_zero) {
intention = ManualIntentionZ::brake;
}
// update intention
if ((_intention != ManualIntentionZ::brake) && (intention == ManualIntentionZ::brake)) {
// we start with lowest acceleration
_acc_state_dependent = _param_mpc_acc_down_max.get();
// reset slew-rate: this ensures that there
// is no delay present when user demands to brake
_vel_sp_prev = _vel;
}
switch (intention) {
case ManualIntentionZ::brake: {
// limit jerk when braking to zero
float jerk = (_param_mpc_acc_up_max.get() - _acc_state_dependent) / dt;
if (jerk > _param_mpc_jerk_max.get()) {
_acc_state_dependent = _param_mpc_jerk_max.get() * dt + _acc_state_dependent;
} else {
_acc_state_dependent = _param_mpc_acc_up_max.get();
}
break;
}
case ManualIntentionZ::acceleration: {
_acc_state_dependent = (getMaxAcceleration() - _param_mpc_acc_down_max.get())
* fabsf(_stick) + _param_mpc_acc_down_max.get();
break;
}
}
_intention = intention;
}
void
ManualSmoothingZ::setMaxAcceleration()
{
if (_stick < -FLT_EPSILON) {
// accelerating upward
_max_acceleration = _param_mpc_acc_up_max.get();
} else if (_stick > FLT_EPSILON) {
// accelerating downward
_max_acceleration = _param_mpc_acc_down_max.get();
} else {
// want to brake
if (fabsf(_vel_sp_prev) < FLT_EPSILON) {
// at rest
_max_acceleration = _param_mpc_acc_up_max.get();
} else if (_vel_sp_prev < 0.0f) {
// braking downward
_max_acceleration = _param_mpc_acc_down_max.get();
} else {
// braking upward
_max_acceleration = _param_mpc_acc_up_max.get();
}
}
}
void
ManualSmoothingZ::velocitySlewRate(float &vel_sp, const float dt)
{
// limit vertical acceleration
float acc = 0.f;
if (dt > FLT_EPSILON) {
acc = (vel_sp - _vel_sp_prev) / dt;
}
float max_acc = (acc < 0.0f) ? -_acc_state_dependent : _acc_state_dependent;
if (fabsf(acc) > fabsf(max_acc)) {
vel_sp = max_acc * dt + _vel_sp_prev;
}
}

133
src/lib/flight_tasks/tasks/Utility/ManualSmoothingZ.hpp
View File

@ -1,133 +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 SmoothingZ.hpp
*
* This Class is used for smoothing the velocity setpoints in Z-direction.
* In manual altitude control apply acceleration limit based on stick input.
*/
#pragma once
#include <px4_platform_common/module_params.h>
/**
* User-intention.
* - brake: vehicle should stop
* - acceleration: vehicle should move up or down
*/
enum class ManualIntentionZ {
brake,
acceleration,
};
class ManualSmoothingZ : public ModuleParams
{
public:
ManualSmoothingZ(ModuleParams *parent, const float &vel, const float &stick);
~ManualSmoothingZ() = default;
/**
* Smooth velocity setpoint based on flight direction.
* @param vel_sp[2] array: vel_sp[0] = current velocity set-point;
* vel_sp[1] = previous velocity set-point
* vel_sp will contain smoothed current / previous set-point.
* @param dt: time delta in seconds
*/
void smoothVelFromSticks(float &vel_sp, const float dt);
/**
* Get max accleration.
*/
float getMaxAcceleration() { return _max_acceleration; }
/**
* Get user intention.
* @see ManualIntentionZ
*/
ManualIntentionZ getIntention() { return _intention; }
/*
* Overwrite methods:
* Needed if different parameter values than default required.
*/
void overwriteAccelerationUp(float acc_max_up) { _param_mpc_acc_up_max.set(acc_max_up); }
void overwriteAccelerationDown(float acc_max_down) {_param_mpc_acc_down_max.set(acc_max_down); }
void overwriteJerkMax(float jerk_max) {_param_mpc_jerk_max.set(jerk_max); }
private:
/**
* Add delay to velocity setpoint change.
* This method is used to smooth the velocity setpoint change.
* @param vel_sp current velocity setpoint
* @param dt delta-time
*/
void velocitySlewRate(float &vel_sp, const float dt);
/**
* Computes the velocity setpoint change limit.
* This method computes the limit with which the velocity setpoint change
* is limited.
* @see velocitySlewRate
* @param vel_sp current velocity setpoint
* @param dt delta-time
*/
void updateAcceleration(float &vel_sp, const float dt);
/**
* Set maximum acceleration.
* The maximum acceleration depends on the desired direction (up vs down).
* @see _max_acceleration
*/
void setMaxAcceleration();
/**
* User intention
* @see ManualIntentionZ
*/
ManualIntentionZ _intention{ManualIntentionZ::acceleration};
const float &_vel; /**< vehicle velocity (dependency injection) */
const float &_stick; /**< stick input (dependency injection) */
float _acc_state_dependent; /**< acceleration that depends on _intention */
float _max_acceleration; /**< can be up or down maximum acceleration */
float _vel_sp_prev; /**< previous velocity setpoint */
DEFINE_PARAMETERS(
(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_MAX>) _param_mpc_jerk_max
)
};

8
src/modules/mc_pos_control/MulticopterPositionControl.cpp
View File

@ -593,10 +593,6 @@ void MulticopterPositionControl::start_flight_task()
error = _flight_tasks.switchTask(FlightTaskIndex::ManualPosition);
break;
case 1:
error = _flight_tasks.switchTask(FlightTaskIndex::ManualPositionSmooth);
break;
case 3:
error = _flight_tasks.switchTask(FlightTaskIndex::ManualPositionSmoothVel);
break;
@ -631,10 +627,6 @@ void MulticopterPositionControl::start_flight_task()
error = _flight_tasks.switchTask(FlightTaskIndex::ManualAltitude);
break;
case 1:
error = _flight_tasks.switchTask(FlightTaskIndex::ManualAltitudeSmooth);
break;
case 3:
default:
error = _flight_tasks.switchTask(FlightTaskIndex::ManualAltitudeSmoothVel);

1
src/systemcmds/tests/CMakeLists.txt
View File

@ -68,7 +68,6 @@ set(srcs
test_search_min.cpp
test_servo.c
test_sleep.c
test_smooth_z.cpp
test_uart_baudchange.c
test_uart_console.c
test_uart_loopback.c

277
src/systemcmds/tests/test_smooth_z.cpp
View File

@ -1,277 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012-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 test_smooth_z.cpp
* Tests flight path smoothing algorithms.
*/
#include <unit_test.h>
#include <lib/flight_tasks/tasks/Utility/ManualSmoothingZ.hpp>
#include <float.h>
#include <math.h>
class SmoothZTest : public UnitTest
{
public:
virtual bool run_tests();
bool brakeUpward();
bool brakeDownward();
bool accelerateUpwardFromBrake();
bool accelerateDownwardFromBrake();
};
bool SmoothZTest::run_tests()
{
ut_run_test(brakeUpward);
ut_run_test(brakeDownward);
ut_run_test(accelerateUpwardFromBrake);
ut_run_test(accelerateDownwardFromBrake);
return (_tests_failed == 0);
}
bool SmoothZTest::brakeUpward()
{
/* Downward flight and want to stop */
float stick_current = 0.0f; // sticks are at zero position
float vel_sp_current = 0.0f; // desired velocity is at 0m/s
float vel_sp_previous = 5.0f; // the demanded previous setpoint was 5m/s downwards
float vel = vel_sp_previous; // assume that current velocity is equal to previous vel setpoint
float acc_max_up = 5.0f;
float acc_max_down = 2.0f;
ManualSmoothingZ smooth(nullptr, vel, stick_current);
/* overwrite parameters since they might change depending on configuration */
smooth.overwriteAccelerationDown(acc_max_down); // downward max acceleration of 2m/ss
smooth.overwriteAccelerationUp(acc_max_up); // upward max acceleration of 5m/ss
smooth.overwriteJerkMax(0.1f); // maximum jerk of 0.1
float dt = 0.1f; // dt is set to 0.1s
/* It should start with acceleration */
ut_assert_true(smooth.getIntention() == ManualIntentionZ::acceleration);
for (int i = 0; i < 100; i++) {
smooth.smoothVelFromSticks(vel_sp_current, dt);
/* Test if intention is brake */
ut_assert_true(smooth.getIntention() == ManualIntentionZ::brake);
/* we should always use upward acceleration */
ut_assert_true((smooth.getMaxAcceleration() - acc_max_up < FLT_EPSILON));
/* New setpoint has to be lower than previous setpoint (NED frame) or equal 0. 0 velocity
* occurs once the vehicle is at perfect rest. */
ut_assert_true((vel_sp_current < vel_sp_previous) || (fabsf(vel_sp_current) < FLT_EPSILON));
/* We reset the previou setpoint to newest setpoint
* and set the current setpoint to 0 because we still want to brake.
* We also set vel to previous setpoint where we make the assumption that
* the vehicle can perfectly track the setpoints.
*/
vel_sp_previous = vel_sp_current;
vel_sp_current = 0.0f;
vel = vel_sp_previous;
}
return true;
}
bool SmoothZTest::brakeDownward()
{
/* Downward flight and want to stop */
float stick_current = 0.0f; // sticks are at zero position
float vel_sp_current = 0.0f; // desired velocity is 0m/s
float vel_sp_previous = -5.0f; // the demanded previous setpoint was -5m/s downwards
float vel = vel_sp_previous; // assume that current velocity is equal to previous vel setpoint
float acc_max_up = 5.0f;
float acc_max_down = 2.0f;
ManualSmoothingZ smooth(nullptr, vel, stick_current);
/* overwrite parameters since they might change depending on configuration */
smooth.overwriteAccelerationDown(acc_max_down); // downward max acceleration of 2m/ss
smooth.overwriteAccelerationUp(acc_max_up); // upward max acceleration of 5m/ss
smooth.overwriteJerkMax(0.1f); // maximum jerk of 0.1
float dt = 0.1f; // dt is set to 0.1s
/* It should start with acceleration */
ut_assert_true(smooth.getIntention() == ManualIntentionZ::acceleration);
for (int i = 0; i < 100; i++) {
smooth.smoothVelFromSticks(vel_sp_current, dt);
/* Test if intention is brake */
ut_assert_true(smooth.getIntention() == ManualIntentionZ::brake);
/* New setpoint has to be larger than previous setpoint (NED frame) or equal 0. 0 velocity
* occurs once the vehicle is at perfect rest. */
ut_assert_true((vel_sp_current > vel_sp_previous) || (fabsf(vel_sp_current) < FLT_EPSILON));
/* we should always use downward acceleration except when vehicle is at rest*/
if (fabsf(vel_sp_previous) < FLT_EPSILON) {
ut_assert_true(fabsf(smooth.getMaxAcceleration() - acc_max_up) < FLT_EPSILON);
} else {
ut_assert_true(fabsf(smooth.getMaxAcceleration() - acc_max_down) < FLT_EPSILON);
}
/* We reset the previou setpoint to newest setpoint
* and set the current setpoint to 0 because we still want to brake.
* We also set vel to previous setpoint where we make the assumption that
* the vehicle can perfectly track the setpoints.
*/
vel_sp_previous = vel_sp_current;
vel_sp_current = 0.0f;
vel = vel_sp_previous;
}
return true;
}
bool SmoothZTest::accelerateUpwardFromBrake()
{
/* Downward flight and want to stop */
float stick_current = -1.0f; // sticks are at full upward position
float vel_sp_target = -5.0f; // desired velocity is at -5m/s
float vel_sp_current = vel_sp_target;
float vel_sp_previous = 0.0f; // the demanded previous setpoint was 0m/s downwards
float vel = vel_sp_previous; // assume that current velocity is equal to previous vel setpoint
float acc_max_up = 5.0f;
float acc_max_down = 2.0f;
ManualSmoothingZ smooth(nullptr, vel, stick_current);
/* overwrite parameters since they might change depending on configuration */
smooth.overwriteAccelerationDown(acc_max_down); // downward max acceleration of 2m/ss
smooth.overwriteAccelerationUp(acc_max_up); // upward max acceleration of 5m/ss
smooth.overwriteJerkMax(0.1f); // maximum jerk of 0.1
float dt = 0.1f; // dt is set to 0.1s
for (int i = 0; i < 100; i++) {
smooth.smoothVelFromSticks(vel_sp_current, dt);
/* Test if intention is acceleration */
ut_assert_true(smooth.getIntention() == ManualIntentionZ::acceleration);
/* we should always use upward acceleration */
ut_assert_true(fabsf(smooth.getMaxAcceleration() - acc_max_up) < FLT_EPSILON);
/* New setpoint has to be larger than previous setpoint or equal to target velocity
* vel_sp_current. The negative sign is because of NED frame.
*/
ut_assert_true((-vel_sp_current > -vel_sp_previous) || (fabsf(vel_sp_current - vel_sp_previous) < FLT_EPSILON));
/* We reset the previous setpoint to newest setpoint and reset the current setpoint.
* We also set the current velocity to the previous setpoint with the assumption that
* the vehicle does perfect tracking.
*/
vel_sp_previous = vel_sp_current;
vel_sp_current = vel_sp_target;
vel = vel_sp_previous;
}
return true;
}
bool SmoothZTest::accelerateDownwardFromBrake()
{
/* Downward flight and want to stop */
float stick_current = 1.0f; // sticks are at full downward position
float vel_sp_target = 5.0f; // desired velocity is at 5m/s
float vel_sp_current = vel_sp_target;
float vel_sp_previous = 0.0f; // the demanded previous setpoint was 0m/s downwards
float vel = vel_sp_previous; // assume that current velocity is equal to previous vel setpoint
float acc_max_up = 5.0f;
float acc_max_down = 2.0f;
ManualSmoothingZ smooth(nullptr, vel, stick_current);
/* overwrite parameters since they might change depending on configuration */
smooth.overwriteAccelerationDown(acc_max_down); // downward max acceleration of 2m/ss
smooth.overwriteAccelerationUp(acc_max_up); // upward max acceleration of 5m/ss
smooth.overwriteJerkMax(0.1f); // maximum jerk of 0.1
float dt = 0.1f; // dt is set to 0.1s
for (int i = 0; i < 100; i++) {
smooth.smoothVelFromSticks(vel_sp_current, dt);
/* Test if intention is acceleration */
ut_assert_true(smooth.getIntention() == ManualIntentionZ::acceleration);
/* we should always use downward acceleration except when target velocity is reached */
if (fabsf(vel_sp_current - vel_sp_target) < FLT_EPSILON) {
ut_assert_true(smooth.getMaxAcceleration() - acc_max_up < FLT_EPSILON);
} else {
ut_assert_true(fabsf(smooth.getMaxAcceleration() - acc_max_down) < FLT_EPSILON);
}
/* New setpoint has to be larger than previous setpoint or equal to target velocity
* vel_sp_current (NED frame).
*/
ut_assert_true((vel_sp_current > vel_sp_previous) || (fabsf(vel_sp_current - vel_sp_target) < FLT_EPSILON));
/* We reset the previous setpoint to newest setpoint and reset the current setpoint.
* We also set the current velocity to the previous setpoint with the assumption that
* the vehicle does perfect tracking.
*/
vel_sp_previous = vel_sp_current;
vel_sp_current = vel_sp_target;
vel = vel_sp_previous;
}
return true;
}
ut_declare_test_c(test_smooth_z, SmoothZTest)

1
src/systemcmds/tests/tests_main.c
View File

@ -114,7 +114,6 @@ const struct {
{"search_min", test_search_min, 0},
{"servo", test_servo, OPT_NOJIGTEST | OPT_NOALLTEST},
{"sleep", test_sleep, OPT_NOJIGTEST},
{"smoothz", test_smooth_z, 0},
{"uart_loopback", test_uart_loopback, OPT_NOJIGTEST | OPT_NOALLTEST},
{"uart_send", test_uart_send, OPT_NOJIGTEST | OPT_NOALLTEST},
{"versioning", test_versioning, 0},

1
src/systemcmds/tests/tests_main.h
View File

@ -80,7 +80,6 @@ extern int test_rc(int argc, char *argv[]);
extern int test_search_min(int argc, char *argv[]);
extern int test_servo(int argc, char *argv[]);
extern int test_sleep(int argc, char *argv[]);
extern int test_smooth_z(int argc, char *argv[]);
extern int test_time(int argc, char *argv[]);
extern int test_uart_baudchange(int argc, char *argv[]);
extern int test_uart_break(int argc, char *argv[]);