Skip to content

Commit

Permalink
integration_tests: add attitude rate flips
Browse files Browse the repository at this point in the history 
Instead of implementing attitude control using attitude rate using
offboard mode it probably makes more sense to implement flips.

For attitude control it's generally easier to actually use the attitude
control which already exists in PX4.
  • Loading branch information
@julianoes
julianoes committed May 27, 2019
1 parent a74bb92 commit efcd5c2
Showing 1 changed file with 139 additions and 142 deletions.
281 changes: 139 additions & 142 deletions integration_tests/offboard_attitude.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,14 +6,15 @@
#include "plugins/telemetry/telemetry.h"
#include "plugins/offboard/offboard.h"

struct Rates {
float roll;
float pitch;
float yaw;
float thrust;
};
using namespace dronecode_sdk;
Rates close_loop(float, float, float, std::shared_ptr<Telemetry> telemetry);

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(std::shared_ptr<Offboard> offboard);
static void stop_offboard(std::shared_ptr<Offboard> offboard);
static void flip_roll(std::shared_ptr<Offboard> offboard, std::shared_ptr<Telemetry> telemetry);
static void flip_pitch(std::shared_ptr<Offboard> offboard, std::shared_ptr<Telemetry> telemetry);
static void turn_yaw(std::shared_ptr<Offboard> offboard);

TEST_F(SitlTest, OffboardAttitudeRate)
{
Expand All @@ -37,162 +38,158 @@ TEST_F(SitlTest, OffboardAttitudeRate)
std::this_thread::sleep_for(std::chrono::seconds(1));
}

Action::Result action_ret = action->arm();
ASSERT_EQ(Action::Result::SUCCESS, action_ret);
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);

ASSERT_EQ(action->takeoff(), Action::Result::SUCCESS);

while (telemetry->position().relative_altitude_m < 4.0f) {
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
}

action_ret = action->takeoff();
ASSERT_EQ(Action::Result::SUCCESS, action_ret);
void disarm_and_land(std::shared_ptr<Action> action, std::shared_ptr<Telemetry> telemetry)
{
action->land();

// Taking off and reaching an altitude of 2.5mts
std::this_thread::sleep_for(std::chrono::seconds(3));
while (telemetry->armed()) {
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
}

void start_offboard(std::shared_ptr<Offboard> offboard)
{
// Send it once before starting offboard, otherwise it will be rejected.
offboard->set_attitude_rate({0.0f, 0.0f, 0.0f, 0.0f});
// Printing Pitch, roll and yaw euler angles.
std::cout << "Roll: " << telemetry->attitude_euler_angle().roll_deg
<< " Pitch: " << telemetry->attitude_euler_angle().pitch_deg
<< " Yaw: " << telemetry->attitude_euler_angle().yaw_deg << std::endl;

// Starting Offboard Mode
Offboard::Result offboard_result = offboard->start();
EXPECT_EQ(offboard_result, Offboard::Result::SUCCESS);

// Gaining altitude
offboard->set_attitude_rate({0.0f, 0.0f, 0.0f, 0.5f});
std::this_thread::sleep_for(std::chrono::seconds(2));
offboard->set_attitude_rate({0.0f, 0.0f, 0.0f, 1.0f});
EXPECT_EQ(offboard->start(), Offboard::Result::SUCCESS);
}

// Performing Acutal tests
// Testing Roll
std::cout << "Rolling at 45 degrees/seconds" << std::endl;
offboard->set_attitude_rate({45.0f, 0.0f, 0.0f, 0.5f});
for (int i = 0; i < 1000; i++) {
if (telemetry->attitude_euler_angle().roll_deg <= 45.0f) {
auto roll_deg = close_loop(45.0f, 0.0f, 0.0f, telemetry);
offboard->set_attitude_rate(
{roll_deg.roll, roll_deg.pitch, roll_deg.yaw, roll_deg.thrust});
std::this_thread::sleep_for(std::chrono::milliseconds(10));
} else {
std::cout << "Roll Angle Reached" << std::endl;
std::cout << "Roll: " << telemetry->attitude_euler_angle().roll_deg
<< " Pitch: " << telemetry->attitude_euler_angle().pitch_deg
<< " Yaw: " << telemetry->attitude_euler_angle().yaw_deg << std::endl;
break;
}
void stop_offboard(std::shared_ptr<Offboard> offboard)
{
EXPECT_EQ(offboard->stop(), Offboard::Result::SUCCESS);
}

void flip_roll(std::shared_ptr<Offboard> offboard, std::shared_ptr<Telemetry> telemetry)
{
while (telemetry->position().relative_altitude_m < 6.0f) {
// Full speed up to avoid loosing too much altitude during the flip.
offboard->set_attitude_rate({0.0f, 0.0f, 0.0f, 1.0f});
}

// Balancing
for (int i = 0; i < 1000; i++) {
if (telemetry->attitude_euler_angle().roll_deg >= 0.2f) {
auto roll_bal = close_loop(0.0f, 0.0f, 0.0f, telemetry);
offboard->set_attitude_rate(
{roll_bal.roll, roll_bal.pitch, roll_bal.yaw, roll_bal.thrust});
std::this_thread::sleep_for(std::chrono::milliseconds(10));
} else {
std::cout << "Balancing Roll" << std::endl;
std::cout << "Roll: " << telemetry->attitude_euler_angle().roll_deg
<< " Pitch: " << telemetry->attitude_euler_angle().pitch_deg
<< " Yaw: " << telemetry->attitude_euler_angle().yaw_deg << std::endl;
break;
enum class TurningState { Init, Turned45, Turned315 } turning_state{TurningState::Init};

while (turning_state != TurningState::Turned315) {
offboard->set_attitude_rate({360.0f, 0.0f, 0.0f, 0.25f});
std::this_thread::sleep_for(std::chrono::milliseconds(10));

// We can't check for a negative angle from the beginning because we might
// have a slightly negative angle right in the beginning. Therefore, we make
// sure that we have started turning.
switch (turning_state) {
case TurningState::Init:
if (telemetry->attitude_euler_angle().roll_deg > 45.0f) {
turning_state = TurningState::Turned45;
}
break;
case TurningState::Turned45:
if (telemetry->attitude_euler_angle().roll_deg > -45.0f &&
telemetry->attitude_euler_angle().roll_deg < 0.0f) {
turning_state = TurningState::Turned315;
}
break;
case TurningState::Turned315:
break;
}
}

// Testing Pitch
std::cout << "Pitching at 45 degrees/second" << std::endl;
offboard->set_attitude_rate({0.0f, 45.0f, 0.0f, 0.5f});
for (int i = 0; i < 1000; i++) {
if (telemetry->attitude_euler_angle().pitch_deg <= 45.0f) {
auto pitch_deg = close_loop(0.0f, 45.0f, 0.0f, telemetry);
offboard->set_attitude_rate(
{pitch_deg.roll, pitch_deg.pitch, pitch_deg.yaw, pitch_deg.thrust});
std::this_thread::sleep_for(std::chrono::milliseconds(3));
} else {
std::cout << "Pitch angle achieved" << std::endl;
std::cout << "Roll: " << telemetry->attitude_euler_angle().roll_deg
<< " Pitch: " << telemetry->attitude_euler_angle().pitch_deg
<< " Yaw: " << telemetry->attitude_euler_angle().yaw_deg << std::endl;
break;
}
while (std::abs(telemetry->attitude_euler_angle().roll_deg) > 3.0f) {
offboard->set_attitude({0.0f, 0.0f, 0.0f, 0.6f});
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
}

// Balancing
for (int i = 0; i < 1000; i++) {
if (telemetry->attitude_euler_angle().pitch_deg > 0.2f) {
auto pitch_bal = close_loop(0.0f, 0.0f, 0.0f, telemetry);
offboard->set_attitude_rate(
{pitch_bal.roll, pitch_bal.pitch, pitch_bal.yaw, pitch_bal.thrust});
std::this_thread::sleep_for(std::chrono::milliseconds(10));
} else {
std::cout << "Pitch Balanced" << std::endl;
std::cout << "Roll: " << telemetry->attitude_euler_angle().roll_deg
<< " Pitch: " << telemetry->attitude_euler_angle().pitch_deg
<< " Yaw: " << telemetry->attitude_euler_angle().yaw_deg << std::endl;
break;
}
void flip_pitch(std::shared_ptr<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->set_attitude({0.0f, 0.0f, 0.0f, 1.0f});
}

// Testing Yaw
std::cout << "Yaw at -60 degrees/second" << std::endl;
offboard->set_attitude_rate({0.0f, 0.0f, -60.0f, 0.5f});
for (int i = 0; i < 1000; i++) {
if (telemetry->attitude_euler_angle().yaw_deg >=
-60.0f) // Flipping the comparison operator while turning anti-clock wise.
{
auto yaw_deg = close_loop(0.0f, 0.0f, -60.0f, telemetry);
offboard->set_attitude_rate({yaw_deg.roll, yaw_deg.pitch, yaw_deg.yaw, yaw_deg.thrust});
std::this_thread::sleep_for(std::chrono::milliseconds(5));
} else {
std::cout << "Yaw angle acheived" << std::endl;
std::cout << "Roll: " << telemetry->attitude_euler_angle().roll_deg
<< " Pitch: " << telemetry->attitude_euler_angle().pitch_deg
<< " Yaw: " << telemetry->attitude_euler_angle().yaw_deg << std::endl;
break;
enum class TurningState {
Init,
Turned45,
Turned240,
Turned315
} turning_state{TurningState::Init};

while (turning_state != TurningState::Turned315) {
offboard->set_attitude_rate({0.0f, 360.0f, 0.0f, 0.25f});
std::this_thread::sleep_for(std::chrono::milliseconds(10));

// We can't check for a negative angle from the beginning because we might
// have a slightly negative angle right in the beginning. Therefore, we make
// sure that we have started turning.
// Euler angles are impractical for this because pitch only increases to 90
// degrees before it goes down to 0 and eventually -90.
switch (turning_state) {
case TurningState::Init:
if (telemetry->attitude_euler_angle().pitch_deg > 45.0f) {
turning_state = TurningState::Turned45;
}
break;
case TurningState::Turned45:
if (telemetry->attitude_euler_angle().pitch_deg < -60.0f) {
turning_state = TurningState::Turned240;
}
break;
case TurningState::Turned240:
if (telemetry->attitude_euler_angle().pitch_deg < 0.0f &&
telemetry->attitude_euler_angle().pitch_deg > -45.0f) {
turning_state = TurningState::Turned315;
}
break;
case TurningState::Turned315:
break;
}
}
std::cout << "Roll: " << telemetry->attitude_euler_angle().roll_deg
<< " Pitch: " << telemetry->attitude_euler_angle().pitch_deg
<< " Yaw: " << telemetry->attitude_euler_angle().yaw_deg << std::endl;

// Balancing
for (int i = 0; i < 1000; i++) {
if (telemetry->attitude_euler_angle().yaw_deg <= 0.2f) {
auto yaw_bal = close_loop(0.0f, 0.0f, 0.0f, telemetry);
offboard->set_attitude_rate({yaw_bal.roll, yaw_bal.pitch, yaw_bal.yaw, yaw_bal.thrust});
std::this_thread::sleep_for(std::chrono::milliseconds(10));
} else {
std::cout << "Balanced Yaw" << std::endl;
std::cout << "Roll: " << telemetry->attitude_euler_angle().roll_deg
<< " Pitch: " << telemetry->attitude_euler_angle().pitch_deg
<< " Yaw: " << telemetry->attitude_euler_angle().yaw_deg << std::endl;

while (true) {
offboard->set_attitude({0.0f, 0.0f, 0.0f, 0.6f});
std::this_thread::sleep_for(std::chrono::milliseconds(10));

if (std::abs(telemetry->attitude_euler_angle().pitch_deg < 3.0f)) {
break;
}
}

// Stopping offboard mode and Landing.
offboard_result = offboard->stop();
EXPECT_EQ(offboard_result, Offboard::Result::SUCCESS);
action_ret = action->land();
std::cout << "Landed! " << std::endl;

std::this_thread::sleep_for(std::chrono::seconds(3));
std::cout << "Landed" << std::endl;
std::cout << "Roll: " << telemetry->attitude_euler_angle().roll_deg
<< " Pitch: " << telemetry->attitude_euler_angle().pitch_deg
<< " Yaw: " << telemetry->attitude_euler_angle().yaw_deg << std::endl;
EXPECT_EQ(action_ret, Action::Result::SUCCESS);
}

// Creating a closing function:
Rates close_loop(float roll_rate,
float pitch_rate,
float yaw_rate,
std::shared_ptr<Telemetry> telemetry)
void turn_yaw(std::shared_ptr<Offboard> offboard)
{
double roll_control = 6.0 * (double)(roll_rate - telemetry->attitude_euler_angle().roll_deg);
double pitch_control = 7.2 * (double)(pitch_rate - telemetry->attitude_euler_angle().pitch_deg);
double yaw_control = 3.80 * (double)(yaw_rate - telemetry->attitude_euler_angle().yaw_deg);
double thrust_control = 0.1 * (double)(10.5f - telemetry->position().relative_altitude_m);
if (thrust_control < 0.1) {
thrust_control = 0.10;
for (int i = 0; i < 100; ++i) {
offboard->set_attitude_rate({0.0f, 0.0f, 360.0f, 0.5f});
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}

for (int i = 0; i < 100; ++i) {
offboard->set_attitude({0.0f, 0.0f, 0.0f, 0.5f});
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
return {(float)roll_control, (float)pitch_control, (float)yaw_control, (float)(thrust_control)};
}

0 comments on commit efcd5c2

Please sign in to comment.