/
offboard_attitude.cpp
166 lines (130 loc) · 5.54 KB
/
offboard_attitude.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
#include <iostream>
#include <cmath>
#include "integration_test_helper.h"
#include "mavsdk.h"
#include "plugins/action/action.h"
#include "plugins/telemetry/telemetry.h"
#include "plugins/offboard/offboard.h"
using namespace mavsdk;
static void arm_and_takeoff(std::shared_ptr<Action> action, std::shared_ptr<Telemetry> telemetry);
static void disarm_and_land(std::shared_ptr<Action> action, std::shared_ptr<Telemetry> telemetry);
static void start_offboard(const Offboard& offboard);
static void stop_offboard(const Offboard& offboard);
static void flip_roll(const Offboard& offboard, std::shared_ptr<Telemetry> telemetry);
static void flip_pitch(const Offboard& offboard, std::shared_ptr<Telemetry> telemetry);
static void turn_yaw(const Offboard& offboard);
TEST(SitlTestDisabled, OffboardAttitudeRate)
{
Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}};
ConnectionResult ret = mavsdk.add_udp_connection();
ASSERT_EQ(ConnectionResult::Success, ret);
// Wait for system to connect via heartbeat.
std::this_thread::sleep_for(std::chrono::seconds(2));
ASSERT_TRUE(mavsdk.systems().at(0)->has_autopilot());
auto system = mavsdk.systems().at(0);
auto telemetry = std::make_shared<Telemetry>(system);
auto action = std::make_shared<Action>(system);
// FIXME: trying new plugin instantiation.
auto offboard = Offboard{system};
LogInfo() << "Waiting for system to be ready";
ASSERT_TRUE(poll_condition_with_timeout(
[telemetry]() { return telemetry->health_all_ok(); }, std::chrono::seconds(10)));
arm_and_takeoff(action, telemetry);
start_offboard(offboard);
flip_roll(offboard, telemetry);
flip_pitch(offboard, telemetry);
turn_yaw(offboard);
stop_offboard(offboard);
disarm_and_land(action, telemetry);
}
void arm_and_takeoff(std::shared_ptr<Action> action, std::shared_ptr<Telemetry> telemetry)
{
ASSERT_EQ(action->arm(), Action::Result::Success);
ASSERT_EQ(action->set_takeoff_altitude(5.0f), Action::Result::Success);
// FIXME: workaround to prevent a race between set_takeoff_altitude and takeoff.
std::this_thread::sleep_for(std::chrono::seconds(1));
ASSERT_EQ(action->takeoff(), Action::Result::Success);
while (telemetry->position().relative_altitude_m < 4.0f) {
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
}
void disarm_and_land(std::shared_ptr<Action> action, std::shared_ptr<Telemetry> telemetry)
{
action->land();
while (telemetry->armed()) {
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
}
void start_offboard(const Offboard& offboard)
{
// Send it once before starting offboard, otherwise it will be rejected.
// Also, turn yaw towards North.
Offboard::Attitude full_up{};
full_up.thrust_value = 1.0f;
offboard.set_attitude(full_up);
EXPECT_EQ(offboard.start(), Offboard::Result::Success);
std::this_thread::sleep_for(std::chrono::seconds(1));
}
void stop_offboard(const Offboard& offboard)
{
EXPECT_EQ(offboard.stop(), Offboard::Result::Success);
}
void flip_roll(const Offboard& offboard, std::shared_ptr<Telemetry> telemetry)
{
while (telemetry->position().relative_altitude_m < 10.0f) {
// Full speed up to avoid loosing too much altitude during the flip.
Offboard::AttitudeRate full_up{};
full_up.thrust_value = 1.0f;
offboard.set_attitude_rate(full_up);
}
Offboard::AttitudeRate roll{};
roll.roll_deg_s = 360.0f;
roll.thrust_value = 0.25f;
offboard.set_attitude_rate(roll);
// FIXME: This only properly works at 1x speed right now.
// For lockstep setups running faster, we would need to use the
// speed factor into account which will be available in info soon.
std::this_thread::sleep_for(std::chrono::milliseconds(750));
Offboard::Attitude some_up{};
some_up.thrust_value = 0.8f;
offboard.set_attitude(some_up);
std::this_thread::sleep_for(std::chrono::seconds(2));
}
void flip_pitch(const Offboard& offboard, std::shared_ptr<Telemetry> telemetry)
{
while (telemetry->position().relative_altitude_m < 10.0f) {
// Full speed up to avoid loosing too much altitude during the flip.
Offboard::AttitudeRate full_up{};
full_up.thrust_value = 1.0f;
offboard.set_attitude_rate(full_up);
}
Offboard::AttitudeRate pitch{};
pitch.roll_deg_s = 0.0f;
pitch.pitch_deg_s = 360.0f;
pitch.yaw_deg_s = 0.0f;
pitch.thrust_value = 0.25f;
offboard.set_attitude_rate(pitch);
// FIXME: This only properly works at 1x speed right now.
// For lockstep setups running faster, we would need to use the
// speed factor into account which will be available in info soon.
std::this_thread::sleep_for(std::chrono::milliseconds(750));
Offboard::Attitude some_up{};
some_up.thrust_value = 0.8f;
offboard.set_attitude(some_up);
std::this_thread::sleep_for(std::chrono::seconds(2));
}
void turn_yaw(const Offboard& offboard)
{
Offboard::AttitudeRate yaw{};
yaw.yaw_deg_s = 360.0f;
yaw.thrust_value = 0.5;
offboard.set_attitude_rate(yaw);
// FIXME: This only properly works at 1x speed right now.
// For lockstep setups running faster, we would need to use the
// speed factor into account which will be available in info soon.
std::this_thread::sleep_for(std::chrono::milliseconds(750));
Offboard::Attitude some_up{};
some_up.thrust_value = 0.8f;
offboard.set_attitude(some_up);
std::this_thread::sleep_for(std::chrono::seconds(2));
}