diff --git a/src/modules/flight_mode_manager/FlightModeManager.hpp b/src/modules/flight_mode_manager/FlightModeManager.hpp index 7810d695951..335d88e93db 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.hpp +++ b/src/modules/flight_mode_manager/FlightModeManager.hpp @@ -38,9 +38,10 @@ #include #include #include -#include +#include #include +#include #include #include #include @@ -49,6 +50,8 @@ #include #include +#include "Takeoff/Takeoff.hpp" + class FlightModeManager : public ModuleBase, public ModuleParams, public px4::WorkItem { public: diff --git a/src/modules/mc_pos_control/CMakeLists.txt b/src/modules/mc_pos_control/CMakeLists.txt index 832178432f1..b18b44b9d60 100644 --- a/src/modules/mc_pos_control/CMakeLists.txt +++ b/src/modules/mc_pos_control/CMakeLists.txt @@ -31,7 +31,6 @@ # ############################################################################ -add_subdirectory(Takeoff) add_subdirectory(PositionControl) px4_add_module( @@ -50,5 +49,4 @@ px4_add_module( ecl_geo WeatherVane CollisionPrevention - Takeoff2 ) diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index ab627dcd655..5865d119a9f 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -33,6 +33,10 @@ #include "MulticopterPositionControl.hpp" +#include +#include +#include + using namespace time_literals; using namespace matrix; diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.hpp b/src/modules/mc_pos_control/MulticopterPositionControl.hpp index 99b05b7cad7..8b9fc8d606f 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.hpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.hpp @@ -42,11 +42,8 @@ #include #include #include -#include -#include #include #include -#include #include #include #include @@ -54,25 +51,18 @@ #include #include #include -#include + #include #include -#include -#include +#include +#include #include #include #include -#include #include #include -#include -#include -#include #include "PositionControl/PositionControl.hpp" -#include "Takeoff/Takeoff.hpp" - -#include class MulticopterPositionControl : public ModuleBase, public control::SuperBlock, public ModuleParams, public px4::WorkItem @@ -95,8 +85,6 @@ public: private: void Run() override; - Takeoff _takeoff; /**< state machine and ramp to bring the vehicle off the ground without jumps */ - uORB::Publication _vehicle_attitude_setpoint_pub; orb_advert_t _mavlink_log_pub{nullptr}; @@ -114,21 +102,8 @@ private: int _task_failure_count{0}; /**< counter for task failures */ - /**< vehicle-land-detection: initialze to landed */ - vehicle_land_detected_s _vehicle_land_detected = { - .timestamp = 0, - .alt_max = -1.0f, - .freefall = false, - .ground_contact = true, - .maybe_landed = true, - .landed = true, - }; - vehicle_control_mode_s _control_mode{}; /**< vehicle control mode */ vehicle_local_position_s _local_pos{}; /**< vehicle local position */ - home_position_s _home_pos{}; /**< home position */ - - int8_t _old_landing_gear_position{landing_gear_s::GEAR_KEEP}; DEFINE_PARAMETERS( // Position Control diff --git a/src/modules/mc_pos_control/Takeoff/CMakeLists.txt b/src/modules/mc_pos_control/Takeoff/CMakeLists.txt deleted file mode 100644 index b641ed936be..00000000000 --- a/src/modules/mc_pos_control/Takeoff/CMakeLists.txt +++ /dev/null @@ -1,40 +0,0 @@ -############################################################################ -# -# Copyright (c) 2019 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -px4_add_library(Takeoff2 - Takeoff.cpp -) -target_include_directories(Takeoff2 PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -target_link_libraries(Takeoff2 PUBLIC hysteresis) - -px4_add_unit_gtest(SRC TakeoffTest.cpp LINKLIBS Takeoff2) diff --git a/src/modules/mc_pos_control/Takeoff/Takeoff.cpp b/src/modules/mc_pos_control/Takeoff/Takeoff.cpp deleted file mode 100644 index 4fb98948e89..00000000000 --- a/src/modules/mc_pos_control/Takeoff/Takeoff.cpp +++ /dev/null @@ -1,132 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Takeoff.cpp - */ - -#include "Takeoff.hpp" -#include -#include - -void Takeoff::generateInitialRampValue(float velocity_p_gain) -{ - velocity_p_gain = math::max(velocity_p_gain, 0.01f); - _takeoff_ramp_vz_init = -CONSTANTS_ONE_G / velocity_p_gain; -} - -void Takeoff::updateTakeoffState(const bool armed, const bool landed, const bool want_takeoff, - const float takeoff_desired_vz, const bool skip_takeoff, const hrt_abstime &now_us) -{ - _spoolup_time_hysteresis.set_state_and_update(armed, now_us); - - switch (_takeoff_state) { - case TakeoffState::disarmed: - if (armed) { - _takeoff_state = TakeoffState::spoolup; - - } else { - break; - } - - // FALLTHROUGH - case TakeoffState::spoolup: - if (_spoolup_time_hysteresis.get_state()) { - _takeoff_state = TakeoffState::ready_for_takeoff; - - } else { - break; - } - - // FALLTHROUGH - case TakeoffState::ready_for_takeoff: - if (want_takeoff) { - _takeoff_state = TakeoffState::rampup; - _takeoff_ramp_vz = _takeoff_ramp_vz_init; - - } else { - break; - } - - // FALLTHROUGH - case TakeoffState::rampup: - if (_takeoff_ramp_vz >= takeoff_desired_vz) { - _takeoff_state = TakeoffState::flight; - - } else { - break; - } - - // FALLTHROUGH - 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::updateRamp(const float dt, const float takeoff_desired_vz) -{ - if (_takeoff_state < TakeoffState::rampup) { - return _takeoff_ramp_vz_init; - } - - if (_takeoff_state == TakeoffState::rampup) { - if (_takeoff_ramp_time > dt) { - _takeoff_ramp_vz += (takeoff_desired_vz - _takeoff_ramp_vz_init) * dt / _takeoff_ramp_time; - - } else { - _takeoff_ramp_vz = takeoff_desired_vz; - } - - if (_takeoff_ramp_vz < takeoff_desired_vz) { - return _takeoff_ramp_vz; - } - } - - return takeoff_desired_vz; -} diff --git a/src/modules/mc_pos_control/Takeoff/Takeoff.hpp b/src/modules/mc_pos_control/Takeoff/Takeoff.hpp deleted file mode 100644 index 5253164c88c..00000000000 --- a/src/modules/mc_pos_control/Takeoff/Takeoff.hpp +++ /dev/null @@ -1,101 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file 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; - - // initialize parameters - void setSpoolupTime(const float seconds) { _spoolup_time_hysteresis.set_hysteresis_time_from(false, seconds * 1_s); } - void setTakeoffRampTime(const float seconds) { _takeoff_ramp_time = seconds; } - - /** - * Calculate a vertical velocity to initialize the takeoff ramp - * that when passed to the velocity controller results in a zero throttle setpoint. - * @param hover_thrust normalized thrsut value with which the vehicle hovers - * @param velocity_p_gain proportional gain of the velocity controller to calculate the thrust - */ - void generateInitialRampValue(const float velocity_p_gain); - - /** - * 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_vz, const bool skip_takeoff, const hrt_abstime &now_us); - - /** - * Update and return the velocity constraint ramp value during takeoff. - * By ramping up _takeoff_ramp_vz during the takeoff and using it to constain the maximum climb rate a smooth takeoff behavior is achieved. - * Returns zero on the ground and takeoff_desired_vz in flight. - * @param dt time in seconds since the last call/loop iteration - * @param takeoff_desired_vz end value for the velocity ramp - * @return true if setpoint has updated correctly - */ - float updateRamp(const float dt, const float takeoff_desired_vz); - - TakeoffState getTakeoffState() { return _takeoff_state; } - -private: - TakeoffState _takeoff_state = TakeoffState::disarmed; - - systemlib::Hysteresis _spoolup_time_hysteresis{false}; /**< becomes true MPC_SPOOLUP_TIME seconds after the vehicle was armed */ - - float _takeoff_ramp_time = 0.f; - float _takeoff_ramp_vz_init = 0.f; - float _takeoff_ramp_vz = 0.f; -}; diff --git a/src/modules/mc_pos_control/Takeoff/TakeoffTest.cpp b/src/modules/mc_pos_control/Takeoff/TakeoffTest.cpp deleted file mode 100644 index c3262338acc..00000000000 --- a/src/modules/mc_pos_control/Takeoff/TakeoffTest.cpp +++ /dev/null @@ -1,79 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2019 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include -#include -#include -#include - -TEST(TakeoffTest, Initialization) -{ - Takeoff takeoff; - EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::disarmed); -} - -TEST(TakeoffTest, RegularTakeoffRamp) -{ - Takeoff takeoff; - takeoff.setSpoolupTime(1.f); - takeoff.setTakeoffRampTime(2.0); - takeoff.generateInitialRampValue(CONSTANTS_ONE_G / 0.5f); - - // disarmed, landed, don't want takeoff - takeoff.updateTakeoffState(false, true, false, 1.f, false, 0); - EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::disarmed); - - // armed, not landed anymore, don't want takeoff - takeoff.updateTakeoffState(true, false, false, 1.f, false, 500_ms); - EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::spoolup); - - // armed, not landed, don't want takeoff yet, spoolup time passed - takeoff.updateTakeoffState(true, false, false, 1.f, false, 2_s); - EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::ready_for_takeoff); - - // armed, not landed, want takeoff - takeoff.updateTakeoffState(true, false, true, 1.f, false, 3_s); - EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::rampup); - - // armed, not landed, want takeoff, ramping up - takeoff.updateTakeoffState(true, false, true, 1.f, false, 4_s); - EXPECT_FLOAT_EQ(takeoff.updateRamp(.5f, 1.5f), 0.f); - EXPECT_FLOAT_EQ(takeoff.updateRamp(.5f, 1.5f), .5f); - EXPECT_FLOAT_EQ(takeoff.updateRamp(.5f, 1.5f), 1.f); - EXPECT_FLOAT_EQ(takeoff.updateRamp(.5f, 1.5f), 1.5f); - EXPECT_FLOAT_EQ(takeoff.updateRamp(.5f, 1.5f), 1.5f); - - // armed, not landed, want takeoff, rampup time passed - takeoff.updateTakeoffState(true, false, true, 1.f, false, 6500_ms); - EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::flight); -}