mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-19 19:23:33 +08:00
MulticoperPositionControl: 3rd pass get rid of zombie members
This commit is contained in:
Matthias Grob
committed by
Daniel Agar
Daniel Agar
parent
62ada2e2dc
commit
7de288877a
@@ -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)
|
||||
@@ -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 <mathlib/mathlib.h>
|
||||
#include <lib/ecl/geo/geo.h>
|
||||
|
||||
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;
|
||||
}
|
||||
@@ -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 <lib/hysteresis/hysteresis.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
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;
|
||||
};
|
||||
@@ -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 <gtest/gtest.h>
|
||||
#include <Takeoff.hpp>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <lib/ecl/geo/geo.h>
|
||||
|
||||
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);
|
||||
}
|
||||
Reference in New Issue
Block a user