dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Add a Takeoff class to handle multicopter takeoff 

In a deterministic way with clear states to go through.
This commit is contained in:
Matthias Grob 2019-05-15 18:53:15 +02:00 committed by Lorenz Meier
parent da533a7b1d
commit ad6eb19f09
6 changed files with 318 additions and 72 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/modules/mc_pos_control/CMakeLists.txt
View File

@ -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
)

42
src/modules/mc_pos_control/Takeoff/CMakeLists.txt Normal file
View File

@ -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)

121
src/modules/mc_pos_control/Takeoff/Takeoff.cpp Normal file
View File

@ -0,0 +1,121 @@
/****************************************************************************
*
* 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 <float.h>
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;
}

83
src/modules/mc_pos_control/Takeoff/Takeoff.hpp Normal file
View File

@ -0,0 +1,83 @@
/****************************************************************************
*
* 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 <systemlib/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;
/**
* 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;
};

49
src/modules/mc_pos_control/Takeoff/TakeoffTest.cpp Normal file
View File

@ -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 <gtest/gtest.h>
#include <Takeoff.hpp>
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);
// }

91
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -67,6 +67,7 @@
#include <lib/WeatherVane/WeatherVane.hpp>
#include "PositionControl.hpp"
#include "Utility/ControlMath.hpp"
#include "Takeoff.hpp"
using namespace time_literals;
@ -102,10 +103,7 @@ public:
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 @@ private:
/** 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 @@ private:
*/
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]);
}
// 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);
// 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) {
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();