Thanks to visit codestin.com
Credit goes to github.com

Skip to content

Commit ad6eb19

Browse files
MaEtUgRLorenzMeier
MaEtUgR
authored and
LorenzMeier
committed
Add a Takeoff class to handle multicopter takeoff
In a deterministic way with clear states to go through.
1 parent da533a7 commit ad6eb19

File tree

6 files changed

+318
-72
lines changed

6 files changed

+318
-72
lines changed

‎src/modules/mc_pos_control/CMakeLists.txt

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -30,6 +30,9 @@
3030
# POSSIBILITY OF SUCH DAMAGE.
3131
#
3232
############################################################################
33+
34+
add_subdirectory(Takeoff)
35+
3336
px4_add_module(
3437
MODULE modules__mc_pos_control
3538
MAIN mc_pos_control
@@ -46,4 +49,5 @@ px4_add_module(
4649
ecl_geo
4750
WeatherVane
4851
CollisionPrevention
52+
Takeoff
4953
)

‎src/modules/mc_pos_control/Takeoff/CMakeLists.txt

Lines changed: 42 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,42 @@
1+
############################################################################
2+
#
3+
# Copyright (c) 2019 PX4 Development Team. All rights reserved.
4+
#
5+
# Redistribution and use in source and binary forms, with or without
6+
# modification, are permitted provided that the following conditions
7+
# are met:
8+
#
9+
# 1. Redistributions of source code must retain the above copyright
10+
# notice, this list of conditions and the following disclaimer.
11+
# 2. Redistributions in binary form must reproduce the above copyright
12+
# notice, this list of conditions and the following disclaimer in
13+
# the documentation and/or other materials provided with the
14+
# distribution.
15+
# 3. Neither the name PX4 nor the names of its contributors may be
16+
# used to endorse or promote products derived from this software
17+
# without specific prior written permission.
18+
#
19+
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20+
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21+
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22+
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23+
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24+
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25+
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26+
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27+
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28+
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29+
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30+
# POSSIBILITY OF SUCH DAMAGE.
31+
#
32+
############################################################################
33+
34+
px4_add_library(Takeoff
35+
Takeoff.cpp
36+
)
37+
target_include_directories(Takeoff
38+
PUBLIC
39+
${CMAKE_CURRENT_SOURCE_DIR}
40+
)
41+
42+
px4_add_gtest(SRC TakeoffTest.cpp LINKLIBS Takeoff)

‎src/modules/mc_pos_control/Takeoff/Takeoff.cpp

Lines changed: 121 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,121 @@
1+
/****************************************************************************
2+
*
3+
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
4+
*
5+
* Redistribution and use in source and binary forms, with or without
6+
* modification, are permitted provided that the following conditions
7+
* are met:
8+
*
9+
* 1. Redistributions of source code must retain the above copyright
10+
* notice, this list of conditions and the following disclaimer.
11+
* 2. Redistributions in binary form must reproduce the above copyright
12+
* notice, this list of conditions and the following disclaimer in
13+
* the documentation and/or other materials provided with the
14+
* distribution.
15+
* 3. Neither the name PX4 nor the names of its contributors may be
16+
* used to endorse or promote products derived from this software
17+
* without specific prior written permission.
18+
*
19+
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20+
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21+
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22+
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23+
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24+
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25+
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26+
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27+
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28+
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29+
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30+
* POSSIBILITY OF SUCH DAMAGE.
31+
*
32+
****************************************************************************/
33+
34+
/**
35+
* @file Takeoff.cpp
36+
*/
37+
38+
#include "Takeoff.hpp"
39+
#include <float.h>
40+
41+
void Takeoff::updateTakeoffState(const bool armed, const bool landed, const bool want_takeoff,
42+
const float takeoff_desired_thrust, const bool skip_takeoff)
43+
{
44+
_spoolup_time_hysteresis.set_state_and_update(armed);
45+
46+
switch (_takeoff_state) {
47+
case TakeoffState::disarmed:
48+
if (armed) {
49+
_takeoff_state = TakeoffState::spoolup;
50+
51+
} else {
52+
break;
53+
}
54+
55+
case TakeoffState::spoolup:
56+
if (_spoolup_time_hysteresis.get_state()) {
57+
_takeoff_state = TakeoffState::ready_for_takeoff;
58+
59+
} else {
60+
break;
61+
}
62+
63+
case TakeoffState::ready_for_takeoff:
64+
if (want_takeoff) {
65+
_takeoff_state = TakeoffState::rampup;
66+
_takeoff_ramp_thrust = 0.0f;
67+
68+
} else {
69+
break;
70+
}
71+
72+
case TakeoffState::rampup:
73+
if (_takeoff_ramp_thrust <= takeoff_desired_thrust) {
74+
_takeoff_state = TakeoffState::flight;
75+
76+
} else {
77+
break;
78+
}
79+
80+
case TakeoffState::flight:
81+
if (landed) {
82+
_takeoff_state = TakeoffState::ready_for_takeoff;
83+
}
84+
85+
break;
86+
87+
default:
88+
break;
89+
}
90+
91+
if (armed && skip_takeoff) {
92+
_takeoff_state = TakeoffState::flight;
93+
}
94+
95+
// TODO: need to consider free fall here
96+
if (!armed) {
97+
_takeoff_state = TakeoffState::disarmed;
98+
}
99+
}
100+
101+
float Takeoff::updateThrustRamp(const float takeoff_desired_thrust, const float dt)
102+
{
103+
if (_takeoff_state < TakeoffState::rampup) {
104+
return 0.f;
105+
}
106+
107+
if (_takeoff_state == TakeoffState::rampup) {
108+
if (_takeoff_ramp_time > FLT_EPSILON) {
109+
_takeoff_ramp_thrust += takeoff_desired_thrust * dt / _takeoff_ramp_time;
110+
111+
} else {
112+
_takeoff_ramp_thrust = takeoff_desired_thrust;
113+
}
114+
115+
if (_takeoff_ramp_thrust > takeoff_desired_thrust) {
116+
return _takeoff_ramp_thrust;
117+
}
118+
}
119+
120+
return takeoff_desired_thrust;
121+
}

‎src/modules/mc_pos_control/Takeoff/Takeoff.hpp

Lines changed: 83 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,83 @@
1+
/****************************************************************************
2+
*
3+
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
4+
*
5+
* Redistribution and use in source and binary forms, with or without
6+
* modification, are permitted provided that the following conditions
7+
* are met:
8+
*
9+
* 1. Redistributions of source code must retain the above copyright
10+
* notice, this list of conditions and the following disclaimer.
11+
* 2. Redistributions in binary form must reproduce the above copyright
12+
* notice, this list of conditions and the following disclaimer in
13+
* the documentation and/or other materials provided with the
14+
* distribution.
15+
* 3. Neither the name PX4 nor the names of its contributors may be
16+
* used to endorse or promote products derived from this software
17+
* without specific prior written permission.
18+
*
19+
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20+
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21+
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22+
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23+
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24+
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25+
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26+
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27+
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28+
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29+
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30+
* POSSIBILITY OF SUCH DAMAGE.
31+
*
32+
****************************************************************************/
33+
34+
/**
35+
* @file Takeoff.hpp
36+
*
37+
* A class handling all takeoff states and a smooth ramp up of the motors.
38+
*/
39+
40+
#pragma once
41+
42+
#include <systemlib/hysteresis/hysteresis.h>
43+
#include <drivers/drv_hrt.h>
44+
45+
using namespace time_literals;
46+
47+
enum class TakeoffState {
48+
disarmed = 0,
49+
spoolup,
50+
ready_for_takeoff,
51+
rampup,
52+
flight
53+
};
54+
55+
class Takeoff
56+
{
57+
public:
58+
59+
Takeoff() = default;
60+
~Takeoff() = default;
61+
62+
/**
63+
* Update the state for the takeoff.
64+
* @param setpoint a vehicle_local_position_setpoint_s structure
65+
* @return true if setpoint has updated correctly
66+
*/
67+
void updateTakeoffState(const bool armed, const bool landed, const bool want_takeoff,
68+
const float takeoff_desired_thrust, const bool skip_takeoff);
69+
float updateThrustRamp(const float dt, const float takeoff_desired_thrust);
70+
71+
void setTakeoffRampTime(const float seconds) { _takeoff_ramp_time = seconds; }
72+
void setSpoolupTime(const float seconds) { _spoolup_time_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(seconds * (float)1_s)); }
73+
TakeoffState getTakeoffState() { return _takeoff_state; }
74+
75+
// TODO: make this private as soon as tasks also run while disarmed and updateTakeoffState gets called all the time
76+
systemlib::Hysteresis _spoolup_time_hysteresis{false}; /**< becomes true MPC_SPOOLUP_TIME seconds after the vehicle was armed */
77+
78+
private:
79+
TakeoffState _takeoff_state = TakeoffState::disarmed;
80+
81+
float _takeoff_ramp_time = 0.f;
82+
float _takeoff_ramp_thrust = 0.f;
83+
};

‎src/modules/mc_pos_control/Takeoff/TakeoffTest.cpp

Lines changed: 49 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,49 @@
1+
/****************************************************************************
2+
*
3+
* Copyright (C) 2019 PX4 Development Team. All rights reserved.
4+
*
5+
* Redistribution and use in source and binary forms, with or without
6+
* modification, are permitted provided that the following conditions
7+
* are met:
8+
*
9+
* 1. Redistributions of source code must retain the above copyright
10+
* notice, this list of conditions and the following disclaimer.
11+
* 2. Redistributions in binary form must reproduce the above copyright
12+
* notice, this list of conditions and the following disclaimer in
13+
* the documentation and/or other materials provided with the
14+
* distribution.
15+
* 3. Neither the name PX4 nor the names of its contributors may be
16+
* used to endorse or promote products derived from this software
17+
* without specific prior written permission.
18+
*
19+
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20+
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21+
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22+
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23+
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24+
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25+
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26+
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27+
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28+
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29+
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30+
* POSSIBILITY OF SUCH DAMAGE.
31+
*
32+
****************************************************************************/
33+
34+
#include <gtest/gtest.h>
35+
#include <Takeoff.hpp>
36+
37+
TEST(TakeoffTest, Initialization)
38+
{
39+
Takeoff takeoff;
40+
EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::disarmed);
41+
}
42+
43+
// TEST(TakeoffTest, Ramp)
44+
// {
45+
// Takeoff takeoff;
46+
// takeoff.updateTakeoffState(true, false, true, 1.f, false);
47+
// takeoff.updateThrustRamp(1.f, 0.1f);
48+
// EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::disarmed);
49+
// }

0 commit comments

Comments
 (0)