yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

Takeoff: add proper unit test

Browse Source
This commit is contained in:
Matthias Grob
2019-12-07 14:15:32 +01:00
parent 42dbddb156
commit ab4473319a
3 changed files with 41 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
src/modules/mc_pos_control/Takeoff/CMakeLists.txt
View File

@@ -34,9 +34,7 @@
px4_add_library(Takeoff
Takeoff.cpp
)
target_include_directories(Takeoff
PUBLIC
${CMAKE_CURRENT_SOURCE_DIR}
)
target_include_directories(Takeoff PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
target_link_libraries(Takeoff PUBLIC hysteresis)
px4_add_unit_gtest(SRC TakeoffTest.cpp LINKLIBS Takeoff)

6
src/modules/mc_pos_control/Takeoff/Takeoff.hpp
View File

@@ -59,8 +59,8 @@ public:
~Takeoff() = default;
// initialize parameters
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)); }
void setTakeoffRampTime(const float seconds) { _takeoff_ramp_time = seconds; }
/**
* Calculate a vertical velocity to initialize the takeoff ramp
@@ -93,9 +93,9 @@ public:
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;
systemlib::Hysteresis _spoolup_time_hysteresis{false}; /**< becomes true MPC_SPOOLUP_TIME seconds after the vehicle was armed */
};

43
src/modules/mc_pos_control/Takeoff/TakeoffTest.cpp
View File

@@ -33,6 +33,7 @@
#include <gtest/gtest.h>
#include <Takeoff.hpp>
#include <drivers/drv_hrt.h>
TEST(TakeoffTest, Initialization)
{
@@ -40,10 +41,38 @@ TEST(TakeoffTest, Initialization)
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);
// }
TEST(TakeoffTest, RegularTakeoffRamp)
{
Takeoff takeoff;
takeoff.setSpoolupTime(1.f);
takeoff.setTakeoffRampTime(2.0);
takeoff.generateInitialRampValue(.5f, 1.f);
// 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_EQ(takeoff.updateRamp(.5f, 1.5f), 0.f);
EXPECT_EQ(takeoff.updateRamp(.5f, 1.5f), .5f);
EXPECT_EQ(takeoff.updateRamp(.5f, 1.5f), 1.f);
EXPECT_EQ(takeoff.updateRamp(.5f, 1.5f), 1.5f);
EXPECT_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);
}