-
-
Notifications
You must be signed in to change notification settings - Fork 503
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Offboard example #116
Offboard example #116
Changes from all commits
e7725b8
44a5c8c
f13f02c
133b936
368a625
f9632fc
6ea8c21
File filter
Filter by extension
Conversations
Jump to
Diff view
Diff view
There are no files selected for viewing
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -3,3 +3,8 @@ | |
**/install | ||
**/logs | ||
.vscode | ||
# aut-generated files (below) from Qt Creator | ||
*.config | ||
*.creator | ||
*.files | ||
*.includes |
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,39 @@ | ||
cmake_minimum_required(VERSION 2.8.12) | ||
|
||
project(offboard) | ||
|
||
if(NOT MSVC) | ||
add_definitions("-std=c++11 -Wall -Wextra -Werror") | ||
else() | ||
add_definitions("-std=c++11 -WX -W2") | ||
set(platform_libs "Ws2_32.lib") | ||
endif() | ||
|
||
# Add DEBUG define for Debug target | ||
set(CMAKE_CXX_FLAGS_DEBUG "-DDEBUG") | ||
|
||
# This finds thread libs on Linux, Mac, and Windows. | ||
find_package(Threads REQUIRED) | ||
|
||
# Not needed if DroneCore installed system-wide | ||
include_directories( | ||
${CMAKE_SOURCE_DIR}/../../install/include | ||
) | ||
|
||
add_executable(offboard | ||
offboard_velocity.cpp | ||
) | ||
|
||
# Not needed if DroneCore installed system-wide | ||
if(WINDOWS) | ||
set(dronecore_lib "${CMAKE_SOURCE_DIR}/../../install/lib/dronecore.lib") | ||
else() | ||
set(dronecore_lib "${CMAKE_SOURCE_DIR}/../../install/lib/libdronecore.a") | ||
endif() | ||
|
||
target_link_libraries(offboard | ||
${dronecore_lib} # Remove/comment out this line if DroneCore used locally | ||
# dronecore # Uncomment/add this line if DroneCore installed system-wide | ||
${CMAKE_THREAD_LIBS_INIT} | ||
${platform_libs} | ||
) |
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,266 @@ | ||
/** | ||
* @file offboard_velocity.cpp | ||
* @brief Example that demonstrates offboard velocity control in local NED and body coordinates | ||
* @author Author: Julian Oes <julian@oes.ch>, Shakthi Prashanth <shakthi.prashanth.m@intel.com> | ||
* @date 2017-10-17 | ||
*/ | ||
|
||
#include <iostream> | ||
#include <cmath> | ||
#include <thread> | ||
#include <chrono> | ||
#include <dronecore/dronecore.h> | ||
|
||
using namespace dronecore; | ||
using std::this_thread::sleep_for; | ||
using std::chrono::milliseconds; | ||
using std::chrono::seconds; | ||
|
||
#define ERROR_CONSOLE_TEXT "\033[31m" //Turn text on console red | ||
#define TELEMETRY_CONSOLE_TEXT "\033[34m" //Turn text on console blue | ||
#define NORMAL_CONSOLE_TEXT "\033[0m" //Restore normal console colour | ||
|
||
// Handles Action's result | ||
inline void action_error_exit(Action::Result result, const std::string &message) | ||
{ | ||
if (result != Action::Result::SUCCESS) { | ||
std::cerr << ERROR_CONSOLE_TEXT << message << Action::result_str( | ||
result) << NORMAL_CONSOLE_TEXT << std::endl; | ||
exit(EXIT_FAILURE); | ||
} | ||
} | ||
|
||
// Handles Offboard's result | ||
inline void offboard_error_exit(Offboard::Result result, const std::string &message) | ||
{ | ||
if (result != Offboard::Result::SUCCESS) { | ||
std::cerr << ERROR_CONSOLE_TEXT << message << Offboard::result_str( | ||
result) << NORMAL_CONSOLE_TEXT << std::endl; | ||
exit(EXIT_FAILURE); | ||
} | ||
} | ||
|
||
// Handles connection result | ||
inline void connection_error_exit(DroneCore::ConnectionResult result, const std::string &message) | ||
{ | ||
if (result != DroneCore::ConnectionResult::SUCCESS) { | ||
std::cerr << ERROR_CONSOLE_TEXT << message | ||
<< DroneCore::connection_result_str(result) | ||
<< NORMAL_CONSOLE_TEXT << std::endl; | ||
exit(EXIT_FAILURE); | ||
} | ||
} | ||
|
||
// Logs during Offboard control | ||
inline void offboard_log(const std::string &offb_mode, const std::string msg) | ||
{ | ||
std::cout << "[" << offb_mode << "]" << msg << std::endl; | ||
} | ||
|
||
/** | ||
* @name connected device | ||
* @brief Does Offboard control using NED co-ordinates | ||
* returns true if everything went well in Offboard control, exits with a log otherwise. | ||
**/ | ||
bool offb_ctrl_ned(Device &device) | ||
{ | ||
const std::string offb_mode = "NED"; | ||
// Send it once before starting offboard, otherwise it will be rejected. | ||
device.offboard().set_velocity_ned({0.0f, 0.0f, 0.0f, 0.0f}); | ||
offboard_log(offb_mode, " Sent Null velocity command once before starting OFFBOARD"); | ||
|
||
Offboard::Result offboard_result = device.offboard().start(); | ||
offboard_error_exit(offboard_result, "Offboard start failed"); | ||
offboard_log(offb_mode, " OFFBOARD started"); | ||
sleep_for(seconds(1)); | ||
|
||
// Let yaw settle. | ||
offboard_log(offb_mode, " Let yaw settle..."); | ||
for (unsigned i = 0; i < 100; ++i) { | ||
device.offboard().set_velocity_ned({0.0f, 0.0f, 0.0f, 90.0f}); | ||
sleep_for(milliseconds(10)); | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. @julianoes Are we expecting the API client to maintain the resend rate? Not saying we shouldn't, just that we should think about whether there is any way to make this easier for developers. Perhaps I am overthinking this. There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. I agree that we might want to handle that internally. |
||
} | ||
offboard_log(offb_mode, " Done..."); | ||
sleep_for(seconds(5)); | ||
|
||
{ | ||
const float step_size = 0.01f; | ||
const float one_cycle = 2.0f * (float)M_PI; | ||
const unsigned steps = (unsigned)(one_cycle / step_size); | ||
|
||
offboard_log(offb_mode, " Go right & oscillate"); | ||
for (unsigned i = 0; i < steps; ++i) { | ||
float vx = 5.0f * sinf(i * step_size); | ||
device.offboard().set_velocity_ned({vx, 0.0f, 0.0f, 90.0f}); | ||
sleep_for(milliseconds(10)); | ||
} | ||
} | ||
offboard_log(offb_mode, " Done..."); | ||
// NOTE: Use sleep_for() after each velocity-ned command to closely monitor their behaviour. | ||
|
||
offboard_log(offb_mode, " Turn clock-wise 270 deg"); | ||
for (unsigned i = 0; i < 400; ++i) { | ||
device.offboard().set_velocity_ned({0.0f, 0.0f, 0.0f, 270.0f}); | ||
sleep_for(milliseconds(10)); | ||
} | ||
offboard_log(offb_mode, " Done"); | ||
|
||
offboard_log(offb_mode, " Go UP 2 m/s, Turn clock-wise 180 deg"); | ||
for (unsigned i = 0; i < 400; ++i) { | ||
device.offboard().set_velocity_ned({0.0f, 0.0f, -2.0f, 180.0f}); | ||
sleep_for(milliseconds(10)); | ||
} | ||
offboard_log(offb_mode, " Done..."); | ||
|
||
offboard_log(offb_mode, " Turn clock-wise 90 deg"); | ||
for (unsigned i = 0; i < 400; ++i) { | ||
device.offboard().set_velocity_ned({0.0f, 0.0f, 0.0f, 90.0f}); | ||
sleep_for(milliseconds(10)); | ||
} | ||
offboard_log(offb_mode, " Done..."); | ||
|
||
offboard_log(offb_mode, " Go DOWN 1.0 m/s"); | ||
for (unsigned i = 0; i < 400; ++i) { | ||
device.offboard().set_velocity_ned({0.0f, 0.0f, 1.0f, 0.0f}); | ||
sleep_for(milliseconds(10)); | ||
} | ||
offboard_log(offb_mode, " Done..."); | ||
|
||
// Now, stop offboard mode. | ||
offboard_result = device.offboard().stop(); | ||
offboard_error_exit(offboard_result, "Offboard stop failed: "); | ||
offboard_log(offb_mode, " OFFBOARD stopped"); | ||
|
||
return true; | ||
} | ||
|
||
/** | ||
* @name connected device | ||
* @brief Does Offboard control using BODY co-ordinates | ||
* returns true if everything went well in Offboard control, exits with a log otherwise. | ||
*/ | ||
bool offb_ctrl_body(Device &device) | ||
{ | ||
const std::string offb_mode = "BODY"; | ||
|
||
device.offboard().set_velocity_body({0.0f, 0.0f, 0.0f, 0.0f}); | ||
offboard_log(offb_mode, " Sent once before starting OFFBOARD"); | ||
|
||
Offboard::Result offboard_result = device.offboard().start(); | ||
offboard_error_exit(offboard_result, "Offboard start failed: "); | ||
offboard_log(offb_mode, " OFFBOARD started"); | ||
|
||
// Turn around yaw and climb | ||
offboard_log(offb_mode, " Turn around yaw & climb"); | ||
for (unsigned i = 0; i < 200; ++i) { | ||
device.offboard().set_velocity_body({0.0f, 0.0f, -1.0f, 60.0f}); | ||
sleep_for(milliseconds(10)); | ||
} | ||
|
||
// Turn back | ||
offboard_log(offb_mode, " Turn back"); | ||
for (unsigned i = 0; i < 200; ++i) { | ||
device.offboard().set_velocity_body({0.0f, 0.0f, 0.0f, -60.0f}); | ||
sleep_for(milliseconds(10)); | ||
} | ||
|
||
// Wait for a bit | ||
offboard_log(offb_mode, " Wait for a bit"); | ||
for (unsigned i = 0; i < 200; ++i) { | ||
device.offboard().set_velocity_body({0.0f, 0.0f, 0.0f, 0.0f}); | ||
sleep_for(milliseconds(10)); | ||
} | ||
// NOTE: Use sleep_for() after each velocity-ned command to closely monitor their behaviour. | ||
|
||
// Fly a circle | ||
offboard_log(offb_mode, " Fly a circle"); | ||
for (unsigned i = 0; i < 500; ++i) { | ||
device.offboard().set_velocity_body({5.0f, 0.0f, 0.0f, 60.0f}); | ||
sleep_for(milliseconds(10)); | ||
} | ||
|
||
// Wait for a bit | ||
offboard_log(offb_mode, " Wait for a bit"); | ||
for (unsigned i = 0; i < 500; ++i) { | ||
device.offboard().set_velocity_body({0.0f, 0.0f, 0.0f, 0.0f}); | ||
sleep_for(milliseconds(10)); | ||
} | ||
|
||
// Fly a circle sideways | ||
offboard_log(offb_mode, " Fly a circle sideways..."); | ||
for (unsigned i = 0; i < 500; ++i) { | ||
device.offboard().set_velocity_body({0.0f, -5.0f, 0.0f, 60.0f}); | ||
sleep_for(milliseconds(10)); | ||
} | ||
|
||
// Wait for a bit | ||
offboard_log(offb_mode, " Wait for a bit"); | ||
for (unsigned i = 0; i < 500; ++i) { | ||
device.offboard().set_velocity_body({0.0f, 0.0f, 0.0f, 0.0f}); | ||
sleep_for(milliseconds(10)); | ||
} | ||
|
||
// Now, stop offboard mode. | ||
offboard_result = device.offboard().stop(); | ||
offboard_error_exit(offboard_result, "Offboard stop failed: "); | ||
offboard_log(offb_mode, " OFFBOARD stopped"); | ||
|
||
return true; | ||
} | ||
|
||
int main(int, char **) | ||
{ | ||
DroneCore dc; | ||
|
||
// add udp connection | ||
DroneCore::ConnectionResult conn_result = dc.add_udp_connection(); | ||
connection_error_exit(conn_result, "Connection failed"); | ||
|
||
// Wait for the device to connect via heartbeat | ||
while (!dc.is_connected()) { | ||
std::cout << "Wait for device to connect via heartbeat" << std::endl; | ||
sleep_for(seconds(1)); | ||
} | ||
|
||
// Device got connected... | ||
Device &device = dc.device(); | ||
|
||
while (!device.telemetry().health_all_ok()) { | ||
std::cout << "Waiting for device to be ready" << std::endl; | ||
sleep_for(seconds(1)); | ||
} | ||
std::cout << "Device is ready" << std::endl; | ||
|
||
// Arm | ||
Action::Result arm_result = device.action().arm(); | ||
action_error_exit(arm_result, "Arming failed"); | ||
std::cout << "Armed" << std::endl; | ||
|
||
// Takeoff | ||
Action::Result takeoff_result = device.action().takeoff(); | ||
action_error_exit(takeoff_result, "Takeoff failed"); | ||
std::cout << "In Air..." << std::endl; | ||
sleep_for(seconds(5)); | ||
|
||
// using LOCAL NED co-ordinates | ||
bool ret = offb_ctrl_ned(device); | ||
if (ret == false) { | ||
return EXIT_FAILURE; | ||
} | ||
std::cout << "---------------------------" << std::endl; | ||
|
||
// using BODY NED co-ordinates | ||
ret = offb_ctrl_body(device); | ||
if (ret == false) { | ||
return EXIT_FAILURE; | ||
} | ||
|
||
const Action::Result land_result = device.action().land(); | ||
action_error_exit(land_result, "Landing failed"); | ||
|
||
// We are relying on auto-disarming but let's keep watching the telemetry for a bit longer. | ||
sleep_for(seconds(10)); | ||
std::cout << "Landed" << std::endl; | ||
|
||
return EXIT_SUCCESS; | ||
} |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Just for me, where is EXIT_FAILURE defined?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
stdlib.h