diff --git a/cmake/compiler_flags.cmake b/cmake/compiler_flags.cmake index 769f88c031..c0dd0f3d6e 100644 --- a/cmake/compiler_flags.cmake +++ b/cmake/compiler_flags.cmake @@ -38,6 +38,10 @@ if(APPLE) add_definitions("-DAPPLE") endif() +if(UNIX AND NOT APPLE) + add_definitions("-DLINUX") +endif() + # Add DEBUG define for Debug target because that is not done automatically. set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -DDEBUG") diff --git a/core/dronecore.h b/core/dronecore.h index 9eb796373c..03468eea82 100644 --- a/core/dronecore.h +++ b/core/dronecore.h @@ -54,6 +54,8 @@ class DroneCore * Default URL : udp://:14540. * - Default Bind host IP is localhost(127.0.0.1) * + * Warning: serial connections are not supported on Windows (they are supported on Linux and macOS). + * * @param connection_url connection URL string. * @return The result of adding the connection. */ @@ -80,6 +82,9 @@ class DroneCore /** * @brief Adds a serial connection with a specific port (COM or UART dev node) and baudrate as specified. * + * + * Warning: this method is not supported on Windows (it is supported on Linux and macOS). + * * @param dev_path COM or UART dev node name/path (defaults to "/dev/ttyS0"). * @param baudrate Baudrate of the serial port (defaults to 57600). * @return The result of adding the connection. diff --git a/core/dronecore_impl.cpp b/core/dronecore_impl.cpp index 36aa6623eb..6c236c1a60 100644 --- a/core/dronecore_impl.cpp +++ b/core/dronecore_impl.cpp @@ -193,7 +193,7 @@ ConnectionResult DroneCoreImpl::add_tcp_connection(const std::string &remote_ip, ConnectionResult DroneCoreImpl::add_serial_connection(const std::string &dev_path, int baudrate) { -#if !defined(WINDOWS) && !defined(APPLE) +#if !defined(WINDOWS) auto new_conn = std::make_shared(*this, dev_path, baudrate); ConnectionResult ret = new_conn->start(); diff --git a/core/serial_connection.cpp b/core/serial_connection.cpp index ede818ffa2..8255ed0b52 100644 --- a/core/serial_connection.cpp +++ b/core/serial_connection.cpp @@ -1,17 +1,21 @@ -#if !defined(WINDOWS) && !defined(APPLE) +#if !defined(WINDOWS) #include "serial_connection.h" #include "global_include.h" #include "log.h" +#include +#include +#include -#ifndef WINDOWS +#ifdef LINUX #include -#include -#include #include #endif -#include +#ifdef APPLE +#include +#endif +// FIXME: this macro is not needed unless we actually support Windows. #ifndef WINDOWS #define GET_ERROR(_x) strerror(_x) #else @@ -63,30 +67,60 @@ ConnectionResult SerialConnection::start() ConnectionResult SerialConnection::setup_port() { - struct termios2 tc; - - bzero(&tc, sizeof(tc)); - +#if defined(LINUX) _fd = open(_serial_node.c_str(), O_RDWR | O_NOCTTY); if (_fd == -1) { + LogErr() << "open failed: " << GET_ERROR(errno); + return ConnectionResult::CONNECTION_ERROR; + } +#elif defined(APPLE) + // open() hangs on macOS unless you give it O_NONBLOCK + _fd = open(_serial_node.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK); + if (_fd == -1) { + LogErr() << "open failed: " << GET_ERROR(errno); return ConnectionResult::CONNECTION_ERROR; } + // We need to clear the O_NONBLOCK again because we can block while reading + // as we do it in a separate thread. + if (fcntl(_fd, F_SETFL, 0) == -1) { + LogErr() << "fcntl failed: " << GET_ERROR(errno); + return ConnectionResult::CONNECTION_ERROR; + } +#endif + +#if defined(LINUX) + struct termios2 tc; + bzero(&tc, sizeof(tc)); + if (ioctl(_fd, TCGETS2, &tc) == -1) { LogErr() << "Could not get termios2 " << GET_ERROR(errno); close(_fd); return ConnectionResult::CONNECTION_ERROR; } +#elif defined(APPLE) + struct termios tc; + bzero(&tc, sizeof(tc)); + + if (tcgetattr(_fd, &tc) != 0) { + LogErr() << "tcgetattr failed: " << GET_ERROR(errno); + close(_fd); + return ConnectionResult::CONNECTION_ERROR; + } +#endif tc.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON); tc.c_oflag &= ~(OCRNL | ONLCR | ONLRET | ONOCR | OFILL | OPOST); tc.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG | TOSTOP); - tc.c_cflag &= ~(CSIZE | PARENB | CBAUD | CRTSCTS); - tc.c_cflag |= CS8 | BOTHER; + tc.c_cflag &= ~(CSIZE | PARENB | CRTSCTS); + tc.c_cflag |= CS8; tc.c_cc[VMIN] = 1; // We want at least 1 byte to be available. tc.c_cc[VTIME] = 0; // We don't timeout but wait indefinitely. - tc.c_ispeed = _baudrate; - tc.c_ospeed = _baudrate; + +#if defined(LINUX) + // CBAUD and BOTHER don't seem to be available for macOS with termios. + tc.c_cflag &= ~(CBAUD); + tc.c_cflag |= BOTHER; if (ioctl(_fd, TCSETS2, &tc) == -1) { LogErr() << "Could not set terminal attributes " << GET_ERROR(errno); @@ -94,11 +128,25 @@ ConnectionResult SerialConnection::setup_port() return ConnectionResult::CONNECTION_ERROR; } + if (ioctl(_fd, TCFLSH, TCIOFLUSH) == -1) { LogErr() << "Could not flush terminal " << GET_ERROR(errno); close(_fd); return ConnectionResult::CONNECTION_ERROR; } +#elif defined(APPLE) + tc.c_cflag |= CLOCAL; // Without this a write() blocks indefinitely. + + cfsetispeed(&tc, _baudrate); + cfsetospeed(&tc, _baudrate); + + if (tcsetattr(_fd, TCSANOW, &tc) != 0) { + LogErr() << "tcsetattr failed: " << GET_ERROR(errno); + close(_fd); + return ConnectionResult::CONNECTION_ERROR; + } +#endif + return ConnectionResult::SUCCESS; } diff --git a/core/serial_connection.h b/core/serial_connection.h index 57f3907daf..0fd9e0cc45 100644 --- a/core/serial_connection.h +++ b/core/serial_connection.h @@ -1,5 +1,5 @@ #pragma once -#if !defined(WINDOWS) && !defined(APPLE) +#if !defined(WINDOWS) #include #include diff --git a/example/fly_mission/CMakeLists.txt b/example/fly_mission/CMakeLists.txt index 37ed35c674..93162b968a 100644 --- a/example/fly_mission/CMakeLists.txt +++ b/example/fly_mission/CMakeLists.txt @@ -19,4 +19,5 @@ target_link_libraries(fly_mission dronecore_action dronecore_mission dronecore_telemetry + json11 ) diff --git a/example/fly_mission/fly_mission.cpp b/example/fly_mission/fly_mission.cpp index c37ffd305d..8c64e159c1 100644 --- a/example/fly_mission/fly_mission.cpp +++ b/example/fly_mission/fly_mission.cpp @@ -48,7 +48,9 @@ static std::shared_ptr make_mission_item(double latitude_deg, float gimbal_yaw_deg, MissionItem::CameraAction camera_action); -int main(int /*argc*/, char ** /*argv*/) +void usage(std::string arg); + +int main(int argc, char **argv) { DroneCore dc; @@ -62,8 +64,23 @@ int main(int /*argc*/, char ** /*argv*/) prom->set_value(); }); - ConnectionResult connection_result = dc.add_udp_connection(); - handle_connection_err_exit(connection_result, "Connection failed: "); + std::string connection_url; + ConnectionResult connection_result; + + if (argc == 1) { + usage(argv[0]); + connection_result = dc.add_any_connection(); + } else { + connection_url = argv[1]; + connection_result = dc.add_any_connection(connection_url); + } + + if (connection_result != ConnectionResult::SUCCESS) { + std::cout << ERROR_CONSOLE_TEXT << "Connection failed: " + << connection_result_str(connection_result) + << NORMAL_CONSOLE_TEXT << std::endl; + return 1; + } future_result.get(); } @@ -301,3 +318,13 @@ inline void handle_connection_err_exit(ConnectionResult result, exit(EXIT_FAILURE); } } + +void usage(std::string arg) +{ + std::cout << NORMAL_CONSOLE_TEXT << "Usage : " << arg << " [connection_url]" << std::endl + << "Connection URL format should be :" << std::endl + << " For TCP : tcp://[server_host][:server_port]" << std::endl + << " For UDP : udp://[bind_host][:bind_port]" << std::endl + << " For Serial : serial:///path/to/serial/dev[:baudrate]" << std::endl; + std::cout << "Default connection URL is udp://:14540" << std::endl; +}