From 99fb88ce839879e35f86c199f7dc50fa361f85e4 Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 29 Mar 2019 14:32:22 +0100 Subject: [PATCH] AltitudeSmoothVel - Create new altitude flight task that uses the velocity smoothing library to create a jerk-limited trajectory along the Z axis. This FlightTask is used when MPC_POS_MODE = 3. --- src/lib/FlightTasks/CMakeLists.txt | 1 + .../ManualAltitudeSmoothVel/CMakeLists.txt | 39 ++++ .../FlightTaskManualAltitudeSmoothVel.cpp | 167 ++++++++++++++++++ .../FlightTaskManualAltitudeSmoothVel.hpp | 82 +++++++++ .../mc_pos_control/mc_pos_control_main.cpp | 4 + 5 files changed, 293 insertions(+) create mode 100644 src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/CMakeLists.txt create mode 100644 src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp create mode 100644 src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp diff --git a/src/lib/FlightTasks/CMakeLists.txt b/src/lib/FlightTasks/CMakeLists.txt index 059e938da6..499be46a47 100644 --- a/src/lib/FlightTasks/CMakeLists.txt +++ b/src/lib/FlightTasks/CMakeLists.txt @@ -53,6 +53,7 @@ endif() list(APPEND flight_tasks_all ManualAltitude ManualAltitudeSmooth + ManualAltitudeSmoothVel ManualPosition ManualPositionSmooth ManualPositionSmoothVel diff --git a/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/CMakeLists.txt b/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/CMakeLists.txt new file mode 100644 index 0000000000..30d40387c5 --- /dev/null +++ b/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2019 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(FlightTaskManualAltitudeSmoothVel + FlightTaskManualAltitudeSmoothVel.cpp +) + +target_link_libraries(FlightTaskManualAltitudeSmoothVel PUBLIC FlightTaskManualAltitude FlightTaskUtility) +target_include_directories(FlightTaskManualAltitudeSmoothVel PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp b/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp new file mode 100644 index 0000000000..17a9126813 --- /dev/null +++ b/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp @@ -0,0 +1,167 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file FlightManualAltitude.cpp + */ + +#include "FlightTaskManualAltitudeSmoothVel.hpp" + +#include + +using namespace matrix; + +bool FlightTaskManualAltitudeSmoothVel::activate() +{ + bool ret = FlightTaskManualAltitude::activate(); + + _reset(); + + return ret; +} + +void FlightTaskManualAltitudeSmoothVel::reActivate() +{ + // The task is reacivated while the vehicle is on the ground. To detect takeoff in mc_pos_control_main properly + // using the generated jerk, reset the z derivatives to zero + _reset(true); +} + +void FlightTaskManualAltitudeSmoothVel::_reset(bool force_vz_zero) +{ + // Set the z derivatives to zero + if (force_vz_zero) { + _smoothing.reset(0.f, 0.f, _position(2)); + + } else { + // TODO: get current accel + _smoothing.reset(0.f, _velocity(2), _position(2)); + } + + // Always start unlocked + _position_lock_z_active = false; + _position_setpoint_z_locked = NAN; +} + +void FlightTaskManualAltitudeSmoothVel::_checkEkfResetCounters() +{ + if (_sub_vehicle_local_position->get().z_reset_counter != _reset_counters.z) { + _smoothing.setCurrentPosition(_position(2)); + _reset_counters.z = _sub_vehicle_local_position->get().z_reset_counter; + } + + if (_sub_vehicle_local_position->get().vz_reset_counter != _reset_counters.vz) { + _smoothing.setCurrentVelocity(_velocity(2)); + _reset_counters.vz = _sub_vehicle_local_position->get().vz_reset_counter; + } +} + +void FlightTaskManualAltitudeSmoothVel::_updateSetpoints() +{ + /* Get yaw setpont, un-smoothed position setpoints.*/ + FlightTaskManualAltitude::_updateSetpoints(); + + /* Update constraints */ + if (_velocity_setpoint(2) < 0.f) { // up + _smoothing.setMaxAccel(_param_mpc_acc_up_max.get()); + _smoothing.setMaxVel(_constraints.speed_up); + + } else { // down + _smoothing.setMaxAccel(_param_mpc_acc_down_max.get()); + _smoothing.setMaxVel(_constraints.speed_down); + } + + float jerk = _param_mpc_jerk_max.get(); + + _checkEkfResetCounters(); + + /* Check for position unlock + * 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(_sticks_expo(2)) > FLT_EPSILON) { + if (_position_lock_z_active) { + _smoothing.setCurrentVelocity(_velocity_setpoint_feedback( + 2)); // Start the trajectory at the current velocity setpoint + _position_setpoint_z_locked = NAN; + } + + _position_lock_z_active = false; + } + + // During position lock, lower jerk to help the optimizer + // to converge to 0 acceleration and velocity + jerk = _position_lock_z_active ? 1.f : _param_mpc_jerk_max.get(); + + _smoothing.setMaxJerk(jerk); + _smoothing.updateDurations(_deltatime, _velocity_setpoint(2)); + + if (!_position_lock_z_active) { + _smoothing.setCurrentPosition(_position(2)); + } + + float pos_sp_smooth; + + _smoothing.integrate(_acceleration_setpoint(2), _vel_sp_smooth, pos_sp_smooth); + _velocity_setpoint(2) = _vel_sp_smooth; // Feedforward + _jerk_setpoint(2) = _smoothing.getCurrentJerk(); + + if (fabsf(_vel_sp_smooth) < 0.1f && + fabsf(_acceleration_setpoint(2)) < .2f && + fabsf(_sticks_expo(2)) <= FLT_EPSILON) { + _position_lock_z_active = true; + } + + // Set valid position setpoint while in position lock. + // When the position lock condition above is false, it does not + // mean that the unlock condition is true. This is why + // we are checking the lock flag here. + if (_position_lock_z_active) { + _position_setpoint_z_locked = pos_sp_smooth; + + // If the velocity setpoint is smaller than 1mm/s and that the acceleration is 0, force the setpoints + // to zero. This is required because the generated velocity is never exactly zero and if the drone hovers + // for a long period of time, thr drift of the position setpoint will be noticeable. + if (fabsf(_velocity_setpoint(2)) < 1e-3f && fabsf(_acceleration_setpoint(2)) < FLT_EPSILON) { + _velocity_setpoint(2) = 0.f; + _acceleration_setpoint(2) = 0.f; + _smoothing.setCurrentVelocity(0.f); + _smoothing.setCurrentAcceleration(0.f); + } + } + + _position_setpoint(2) = _position_setpoint_z_locked; +} diff --git a/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp b/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp new file mode 100644 index 0000000000..9e8980b630 --- /dev/null +++ b/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp @@ -0,0 +1,82 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file FlightManualAltitudeSmoothVel.hpp + * + * Flight task for manual controlled altitude using the velocity smoothing library + */ + +#pragma once + +#include "FlightTaskManualAltitude.hpp" +#include "VelocitySmoothing.hpp" + +class FlightTaskManualAltitudeSmoothVel : public FlightTaskManualAltitude +{ +public: + FlightTaskManualAltitudeSmoothVel() = default; + virtual ~FlightTaskManualAltitudeSmoothVel() = default; + + bool activate() override; + void reActivate() override; + +protected: + + virtual void _updateSetpoints() override; + + DEFINE_PARAMETERS( + (ParamFloat) _param_mpc_jerk_max, + (ParamFloat) _param_mpc_acc_up_max, + (ParamFloat) _param_mpc_acc_down_max + ) + +private: + + /** + * Reset the required axes. when force_z_zero is set to true, the z derivatives are set to sero and not to the estimated states + */ + void _reset(bool force_vz_zero = false); + void _checkEkfResetCounters(); /**< Reset the trajectories when the ekf resets velocity or position */ + + VelocitySmoothing _smoothing; ///< Smoothing in z direction + float _vel_sp_smooth; + bool _position_lock_z_active{false}; + float _position_setpoint_z_locked{NAN}; + + /* counters for estimator local position resets */ + struct { + uint8_t z; + uint8_t vz; + } _reset_counters{0, 0}; +}; diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 79b7852a66..b78321d34f 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -990,6 +990,10 @@ MulticopterPositionControl::start_flight_task() error = _flight_tasks.switchTask(FlightTaskIndex::ManualAltitudeSmooth); break; + case 3: + error = _flight_tasks.switchTask(FlightTaskIndex::ManualAltitudeSmoothVel); + break; + default: error = _flight_tasks.switchTask(FlightTaskIndex::ManualAltitude); break;