diff --git a/src/modules/mc_pos_control/CMakeLists.txt b/src/modules/mc_pos_control/CMakeLists.txt index f649cecbef69..08c93243c292 100644 --- a/src/modules/mc_pos_control/CMakeLists.txt +++ b/src/modules/mc_pos_control/CMakeLists.txt @@ -30,6 +30,9 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ + +add_subdirectory(Takeoff) + px4_add_module( MODULE modules__mc_pos_control MAIN mc_pos_control @@ -46,4 +49,5 @@ px4_add_module( ecl_geo WeatherVane CollisionPrevention + Takeoff ) diff --git a/src/modules/mc_pos_control/Takeoff/CMakeLists.txt b/src/modules/mc_pos_control/Takeoff/CMakeLists.txt new file mode 100644 index 000000000000..164b175ed979 --- /dev/null +++ b/src/modules/mc_pos_control/Takeoff/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# 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(Takeoff + Takeoff.cpp +) +target_include_directories(Takeoff + PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR} +) + +px4_add_gtest(SRC TakeoffTest.cpp LINKLIBS Takeoff) diff --git a/src/modules/mc_pos_control/Takeoff/Takeoff.cpp b/src/modules/mc_pos_control/Takeoff/Takeoff.cpp new file mode 100644 index 000000000000..98c8747a3c51 --- /dev/null +++ b/src/modules/mc_pos_control/Takeoff/Takeoff.cpp @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * 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 Takeoff.cpp + */ + +#include "Takeoff.hpp" +#include + +void Takeoff::updateTakeoffState(const bool armed, const bool landed, const bool want_takeoff, const float takeoff_desired_thrust, const bool skip_takeoff) { + _spoolup_time_hysteresis.set_state_and_update(armed); + + switch(_takeoff_state) { + case TakeoffState::disarmed: + if (armed) { + _takeoff_state = TakeoffState::spoolup; + } else { + break; + } + case TakeoffState::spoolup: + if (_spoolup_time_hysteresis.get_state()) { + _takeoff_state = TakeoffState::ready_for_takeoff; + } else { + break; + } + case TakeoffState::ready_for_takeoff: + if (want_takeoff) { + _takeoff_state = TakeoffState::rampup; + _takeoff_ramp_thrust = 0.0f; + } else { + break; + } + case TakeoffState::rampup: + if (_takeoff_ramp_thrust <= takeoff_desired_thrust) { + _takeoff_state = TakeoffState::flight; + } else { + break; + } + case TakeoffState::flight: + if (landed) { + _takeoff_state = TakeoffState::ready_for_takeoff; + } + break; + default: + break; + } + + if (armed && skip_takeoff) { + _takeoff_state = TakeoffState::flight; + } + + // TODO: need to consider free fall here + if (!armed) { + _takeoff_state = TakeoffState::disarmed; + } +} + +float Takeoff::updateThrustRamp(const float takeoff_desired_thrust, const float dt) { + if (_takeoff_state < TakeoffState::rampup) { + return 0.f; + } + + if (_takeoff_state == TakeoffState::rampup) { + if (_takeoff_ramp_time > FLT_EPSILON) { + _takeoff_ramp_thrust += takeoff_desired_thrust * dt / _takeoff_ramp_time; + } else { + _takeoff_ramp_thrust = takeoff_desired_thrust; + } + + if (_takeoff_ramp_thrust > takeoff_desired_thrust) { + return _takeoff_ramp_thrust; + } + } + + return takeoff_desired_thrust; +} diff --git a/src/modules/mc_pos_control/Takeoff/Takeoff.hpp b/src/modules/mc_pos_control/Takeoff/Takeoff.hpp new file mode 100644 index 000000000000..fdea84cdf771 --- /dev/null +++ b/src/modules/mc_pos_control/Takeoff/Takeoff.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 Takeoff.hpp + * + * A class handling all takeoff states and a smooth ramp up of the motors. + */ + +#pragma once + +#include +#include + +using namespace time_literals; + +enum class TakeoffState { + disarmed = 0, + spoolup, + ready_for_takeoff, + rampup, + flight +}; + +class Takeoff +{ +public: + + Takeoff() = default; + ~Takeoff() = default; + + /** + * Update the state for the takeoff. + * @param setpoint a vehicle_local_position_setpoint_s structure + * @return true if setpoint has updated correctly + */ + void updateTakeoffState(const bool armed, const bool landed, const bool want_takeoff, const float takeoff_desired_thrust, const bool skip_takeoff); + float updateThrustRamp(const float dt, const float takeoff_desired_thrust); + + void setTakeoffRampTime(const float seconds) { _takeoff_ramp_time = seconds; } + void setSpoolupTime(const float seconds) { _spoolup_time_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(seconds * (float)1_s)); } + TakeoffState getTakeoffState() { return _takeoff_state; } + + // TODO: make this private as soon as tasks also run while disarmed and updateTakeoffState gets called all the time + systemlib::Hysteresis _spoolup_time_hysteresis{false}; /**< becomes true MPC_SPOOLUP_TIME seconds after the vehicle was armed */ + +private: + TakeoffState _takeoff_state = TakeoffState::disarmed; + + float _takeoff_ramp_time = 0.f; + float _takeoff_ramp_thrust = 0.f; +}; diff --git a/src/modules/mc_pos_control/Takeoff/TakeoffTest.cpp b/src/modules/mc_pos_control/Takeoff/TakeoffTest.cpp new file mode 100644 index 000000000000..9d8b77699d71 --- /dev/null +++ b/src/modules/mc_pos_control/Takeoff/TakeoffTest.cpp @@ -0,0 +1,49 @@ +/**************************************************************************** + * + * 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 +#include + +TEST(TakeoffTest, Initialization) +{ + Takeoff takeoff; + EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::disarmed); +} + +// TEST(TakeoffTest, Ramp) +// { +// Takeoff takeoff; +// takeoff.updateTakeoffState(true, false, true, 1.f, false); +// takeoff.updateThrustRamp(1.f, 0.1f); +// EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::disarmed); +// } 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 62002c6ca2d7..6ab4909cc2dd 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -67,6 +67,7 @@ #include #include "PositionControl.hpp" #include "Utility/ControlMath.hpp" +#include "Takeoff.hpp" using namespace time_literals; @@ -102,10 +103,7 @@ class MulticopterPositionControl : public ModuleBase int print_status() override; private: - // smooth takeoff vertical velocity ramp state - bool _in_takeoff_ramp = false; /**< true while takeoff ramp is applied */ - float _takeoff_ramp_thrust = 0.f; /**< current value of the smooth takeoff ramp */ - float _takeoff_reference_z = 0.f; /**< z-position when takeoff ramp was initiated */ + Takeoff _takeoff; /**< state machine and ramp to bring the vehicle off the ground without jumps */ orb_advert_t _att_sp_pub{nullptr}; /**< attitude setpoint publication */ orb_advert_t _traj_sp_pub{nullptr}; /**< trajectory setpoints publication */ @@ -177,7 +175,6 @@ class MulticopterPositionControl : public ModuleBase /** During smooth-takeoff, below ALTITUDE_THRESHOLD the yaw-control is turned off ant tilt is limited */ static constexpr float ALTITUDE_THRESHOLD = 0.3f; - systemlib::Hysteresis _spoolup_time_hysteresis{false}; /**< becomes true MPC_SPOOLUP_TIME seconds after the vehicle was armed */ systemlib::Hysteresis _failsafe_land_hysteresis{false}; /**< becomes true if task did not update correctly for LOITER_TIME_BEFORE_DESCEND */ WeatherVane *_wv_controller{nullptr}; @@ -229,14 +226,6 @@ class MulticopterPositionControl : public ModuleBase */ void publish_trajectory_sp(const vehicle_local_position_setpoint_s &traj); - /** - * Update smooth takeoff vertical velocity ramp to bring the vehicle off the ground without step - * @param vz_sp velocity setpoint in the z-Direction - * @param z_sp position setpoint in the z-Direction - * @param vz_constraint velocity to ramp to when there's only a position setpoint - */ - void update_takeoff_ramp(const bool want_takeoff, const float thr_sp); - /** * Adjust the setpoint during landing. * Thrust is adjusted to support the land-detector during detection. @@ -367,7 +356,8 @@ MulticopterPositionControl::parameters_update(bool force) _param_mpc_land_speed.set(math::min(_param_mpc_land_speed.get(), _param_mpc_z_vel_max_dn.get())); // set trigger time for takeoff delay - _spoolup_time_hysteresis.set_hysteresis_time_from(false, (int)(_param_mpc_spoolup_time.get() * (float)1_s)); + _takeoff.setSpoolupTime(_param_mpc_spoolup_time.get()); + _takeoff.setTakeoffRampTime(_param_mpc_tko_ramp_t.get()); if (_wv_controller != nullptr) { _wv_controller->update_parameters(); @@ -592,9 +582,9 @@ MulticopterPositionControl::run() } // takeoff delay for motors to reach idle speed - _spoolup_time_hysteresis.set_state_and_update(_control_mode.flag_armed); + _takeoff._spoolup_time_hysteresis.set_state_and_update(_control_mode.flag_armed); - if (_spoolup_time_hysteresis.get_state()) { + if (_takeoff._spoolup_time_hysteresis.get_state()) { // when vehicle is ready switch to the required flighttask start_flight_task(); @@ -681,39 +671,22 @@ MulticopterPositionControl::run() local_pos_sp.vz = PX4_ISFINITE(_control.getVelSp()(2)) ? _control.getVelSp()(2) : setpoint.vz; _control.getThrustSetpoint().copyTo(local_pos_sp.thrust); - // do smooth takeoff after delay if there's a valid vertical velocity or position - if (_spoolup_time_hysteresis.get_state()) { - update_takeoff_ramp(constraints.want_takeoff, local_pos_sp.thrust[2]); - } - - // disable horizontal / yaw control during smooth takeoff and limit maximum speed upwards - if (_in_takeoff_ramp) { - // altitude above reference takeoff - const float alt_above_tko = -(_states.position(2) - _takeoff_reference_z); - - // disable yaw control when close to ground - if (alt_above_tko <= ALTITUDE_THRESHOLD) { + // handle smooth takeoff + _takeoff.updateTakeoffState(_control_mode.flag_armed, _vehicle_land_detected.landed, constraints.want_takeoff, local_pos_sp.thrust[2], !_control_mode.flag_control_climb_rate_enabled); + local_pos_sp.thrust[2] = _takeoff.updateThrustRamp(local_pos_sp.thrust[2], _dt); - setpoint.yawspeed = NAN; - - // if there is a valid yaw estimate, just set setpoint to yaw - if (PX4_ISFINITE(_states.yaw)) { - setpoint.yaw = _states.yaw; - } - - // limit tilt during smooth takeoff when still close to ground - constraints.tilt = math::radians(_param_mpc_tiltmax_lnd.get()); - } - } - - if (_in_takeoff_ramp) { - local_pos_sp.thrust[2] = math::max(local_pos_sp.thrust[2], _takeoff_ramp_thrust); - //local_pos_sp.thrust[0] = local_pos_sp.thrust[1] = 0.f; + if (_takeoff.getTakeoffState() < TakeoffState::flight) { + // we set thrust to zero, this will help to decide if we are actually landed or not + setpoint.thrust[0] = setpoint.thrust[1] = 0.0f; + // set yaw-sp to current yaw to avoid any corrections + setpoint.yaw = _states.yaw; + setpoint.yawspeed = 0.f; + // prevent any integrator windup _control.resetIntegralXY(); _control.resetIntegralZ(); } - if (_vehicle_land_detected.landed && !_in_takeoff_ramp && !PX4_ISFINITE(setpoint.thrust[2])) { + if (_takeoff.getTakeoffState() < TakeoffState::rampup && !PX4_ISFINITE(setpoint.thrust[2])) { // Keep throttle low when landed and NOT in smooth takeoff setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = 0.0f; setpoint.yaw = _states.yaw; @@ -733,7 +706,7 @@ MulticopterPositionControl::run() // Part of landing logic: if ground-contact/maybe landed was detected, turn off // controller. This message does not have to be logged as part of the vehicle_local_position_setpoint topic. // Note: only adust thrust output if there was not thrust-setpoint demand in D-direction. - if (!_in_takeoff_ramp && !PX4_ISFINITE(setpoint.thrust[2])) { + if (_takeoff.getTakeoffState() > TakeoffState::rampup && !PX4_ISFINITE(setpoint.thrust[2])) { limit_thrust_during_landing(local_pos_sp); } @@ -978,33 +951,6 @@ MulticopterPositionControl::start_flight_task() } } -void -MulticopterPositionControl::update_takeoff_ramp(const bool want_takeoff, const float thr_sp) -{ - // check if takeoff is triggered - if (_vehicle_land_detected.landed && !_in_takeoff_ramp && want_takeoff) { - // takeoff was triggered, start velocity ramp - _takeoff_ramp_thrust = 0.0f; - _in_takeoff_ramp = true; - _takeoff_reference_z = _states.position(2); - } - - // if in smooth takeoff limit upwards thrust setpoint to a smooth ramp - if (_in_takeoff_ramp) { - float takeoff_desired_thrust = thr_sp; - - if (_param_mpc_tko_ramp_t.get() > _dt) { - _takeoff_ramp_thrust += takeoff_desired_thrust * _dt / _param_mpc_tko_ramp_t.get(); - } else { - // no ramp time, directly jump to desired velocity - _takeoff_ramp_thrust = takeoff_desired_thrust; - } - - // smooth takeoff is finished once desired thrust setpoint is reached - _in_takeoff_ramp = (_takeoff_ramp_thrust > takeoff_desired_thrust); - } -} - void MulticopterPositionControl::limit_thrust_during_landing(vehicle_local_position_setpoint_s &setpoint) { @@ -1014,6 +960,7 @@ MulticopterPositionControl::limit_thrust_during_landing(vehicle_local_position_s setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = 0.0f; // set yaw-sp to current yaw to avoid any corrections setpoint.yaw = _states.yaw; + setpoint.yawspeed = 0.f; // prevent any integrator windup _control.resetIntegralXY(); _control.resetIntegralZ();