Skip to content

Commit

Permalink
debug don'T merge
Browse files Browse the repository at this point in the history
  • Loading branch information
@raphaelscholle
raphaelscholle committed Nov 26, 2024
1 parent 038ed81 commit f775d31
Show file tree
Hide file tree
Showing 2 changed files with 198 additions and 335 deletions.
273 changes: 94 additions & 179 deletions app/telemetry/connection/tcp_connection.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
#include "tutil/qopenhdmavlinkhelper.hpp"

#ifdef __windows__
#define _WIN32_WINNT 0x0600 //TODO dirty
#define _WIN32_WINNT 0x0600 // TODO dirty
#include <winsock2.h>
#include <Ws2tcpip.h> // For InetPton
#include <Windows.h>
Expand All @@ -13,263 +13,178 @@
#endif
#include <unistd.h>
#include <fcntl.h>

#include <qdebug.h>

#include "mavlinkchannel.h"

static int tcp_socket_try_connect(const std::string remote_ip, const int remote_port,const int timeout_seconds){
//qDebug()<<"tcp_socket_try_connect:"<<remote_ip.c_str()<<":"<<remote_port<<" timeout:"<<timeout_seconds<<"s";
int sockfd=socket(AF_INET, SOCK_STREAM, 0);
if(sockfd<0){
qDebug()<<"Cannot create socket"<<strerror(errno);
static int tcp_socket_try_connect(const std::string remote_ip, const int remote_port, const int timeout_seconds) {
qDebug() << "Attempting to connect to" << remote_ip.c_str() << ":" << remote_port
<< "with timeout" << timeout_seconds << "seconds";

int sockfd = socket(AF_INET, SOCK_STREAM, 0);
if (sockfd < 0) {
qDebug() << "Error: Cannot create socket -" << strerror(errno);
return -1;
}
#ifdef __linux__
if(fcntl(sockfd, F_SETFL, O_NONBLOCK)<0){
qDebug()<<"Cannot set non-blocking";
qDebug() << "Socket created successfully. FD:" << sockfd;

#ifdef __linux__
if (fcntl(sockfd, F_SETFL, O_NONBLOCK) < 0) {
qDebug() << "Error: Cannot set socket to non-blocking mode -" << strerror(errno);
close(sockfd);
return -1;
}
#endif
struct sockaddr_in remote_addr {};
qDebug() << "Socket set to non-blocking mode.";
#endif

struct sockaddr_in remote_addr{};
remote_addr.sin_family = AF_INET;
remote_addr.sin_port = htons(remote_port);
remote_addr.sin_addr.s_addr = inet_addr(remote_ip.c_str());
const int connect_result=connect(sockfd, reinterpret_cast<sockaddr*>(&remote_addr), sizeof(struct sockaddr_in));
if(connect_result==0){
qDebug()<<"Got connection immeiately";
if (inet_pton(AF_INET, remote_ip.c_str(), &remote_addr.sin_addr) <= 0) {
qDebug() << "Error: Invalid IP address format -" << remote_ip.c_str();
close(sockfd);
return -1;
}

int connect_result = connect(sockfd, reinterpret_cast<sockaddr*>(&remote_addr), sizeof(struct sockaddr_in));
if (connect_result == 0) {
qDebug() << "Connection established immediately.";
return sockfd;
}
if ((connect_result== -1) && (errno != EINPROGRESS)) {
qDebug()<<"Didnt get EINPROGRESS"<<(int)connect_result;

if (connect_result == -1 && errno != EINPROGRESS) {
qDebug() << "Error: Connect failed and not EINPROGRESS -" << strerror(errno);
close(sockfd);
return -1;
}
// Wait for up to X seconds to connect

qDebug() << "Connection in progress, waiting for completion.";

fd_set fdset;
struct timeval tv;
FD_ZERO(&fdset);
FD_SET(sockfd, &fdset);
tv.tv_sec = timeout_seconds;
tv.tv_usec = 0;
const int rc= select(sockfd + 1, NULL, &fdset, NULL, &tv);
if(rc!=1){
qDebug()<<"Timed out or other crap";

int rc = select(sockfd + 1, NULL, &fdset, NULL, &tv);
if (rc != 1) {
qDebug() << "Error: Timeout or other issue in select() -" << strerror(errno);
close(sockfd);
return -1;
}
// Check if there are any errors

int so_error;
// data to read
socklen_t len = sizeof(so_error);
#ifdef __linux__
getsockopt(sockfd, SOL_SOCKET, SO_ERROR, &so_error, &len);
if (so_error != 0) {
qDebug()<<"Any socket error:"<<strerror(errno);
qDebug() << "Error: Socket error after select -" << strerror(so_error);
close(sockfd);
return -1;
}
#endif
qDebug()<<"Success";

qDebug() << "Socket connected successfully. FD:" << sockfd;
return sockfd;
}

static bool linux_send_message(int sockfd,const std::string& dest_ip,const int dest_port,const uint8_t* data,int data_len){
struct sockaddr_in dest_addr {};
static bool linux_send_message(int sockfd, const std::string& dest_ip, const int dest_port, const uint8_t* data, int data_len) {
qDebug() << "Sending message to" << dest_ip.c_str() << ":" << dest_port;

struct sockaddr_in dest_addr{};
dest_addr.sin_family = AF_INET;
inet_pton(AF_INET, dest_ip.c_str(), &dest_addr.sin_addr.s_addr);
if (inet_pton(AF_INET, dest_ip.c_str(), &dest_addr.sin_addr) <= 0) {
qDebug() << "Error: Invalid destination IP format -" << dest_ip.c_str();
return false;
}
dest_addr.sin_port = htons(dest_port);

#ifdef MSG_NOSIGNAL
auto flags = MSG_NOSIGNAL;
#else
auto flags = 0; // No MSG_NOSIGNAL available, handle it accordingly
auto flags = 0; // No MSG_NOSIGNAL available
#endif
const auto send_len = sendto(

int send_len = sendto(
sockfd,
reinterpret_cast<const char*>(data),
data_len,
flags,
reinterpret_cast<const sockaddr*>(&dest_addr),
sizeof(dest_addr));
sizeof(dest_addr)
);

if (send_len != data_len) {
qDebug() << "Error: Failed to send message. Sent bytes:" << send_len
<< "Expected:" << data_len << "-" << strerror(errno);
return false;
}
return true;
}

TCPConnection::TCPConnection(MAV_MSG_CB cb, std::string remote_ip, int remote_port, int mavlink_channel):
m_cb(cb),
m_remote_ip(remote_ip),
m_remote_port(remote_port),
m_mav_channel(mavlink_channel)
{
}

TCPConnection::~TCPConnection()
{
if(is_looping()){
stop_looping();
}
}

void TCPConnection::set_remote(std::string remote_ip, int remote_port)
{
assert(m_receive_thread==nullptr);
m_remote_ip=remote_ip;
m_remote_port=remote_port;
}

bool TCPConnection::is_looping()
{
return m_receive_thread!=nullptr;
}


void TCPConnection::start_looping()
{
assert(m_receive_thread==nullptr);
m_keep_receiving=true;
m_receive_thread=std::make_unique<std::thread>(&TCPConnection::loop_connect_receive,this);
qDebug() << "Message sent successfully. Bytes:" << send_len;
return true;
}

void TCPConnection::stop_looping_if()
{
if(is_looping())stop_looping();
}
void TCPConnection::stop_looping() {
assert(m_receive_thread != nullptr);
qDebug() << "Stopping TCPConnection loop.";

void TCPConnection::stop_looping()
{
assert(m_receive_thread!=nullptr);
qDebug()<<"TCPConnection2::stop_receiving begin";
m_keep_receiving=false;
m_keep_receiving = false;
#ifdef __windows__
shutdown(m_socket_fd, SD_BOTH);

closesocket(m_socket_fd);

WSACleanup();
#else \
// This should interrupt a recv/recvfrom call.
shutdown(m_socket_fd, SHUT_RDWR);
// But on Mac, closing is also needed to stop blocking recv/recvfrom.
close(m_socket_fd);
#endif
m_receive_thread->join();
m_receive_thread=nullptr;
qDebug()<<"TCPConnection2::stop_receiving end";
}





void TCPConnection::send_message(const mavlink_message_t &msg)
{
uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
const int buffer_len = mavlink_msg_to_send_buffer(buffer, &msg);
if(!m_keep_receiving){
return; // Otherwise sendto blocks
if (shutdown(m_socket_fd, SD_BOTH) == SOCKET_ERROR) {
qDebug() << "Error: Failed to shutdown socket -" << WSAGetLastError();
}
if(m_socket_fd<0){
//qDebug()<<"Cannot send message";
return;
if (closesocket(m_socket_fd) == SOCKET_ERROR) {
qDebug() << "Error: Failed to close socket -" << WSAGetLastError();
}
if(!linux_send_message(m_socket_fd,m_remote_ip,m_remote_port,buffer,buffer_len)){
//qDebug()<<"Cannot send message";
WSACleanup();
#else
if (shutdown(m_socket_fd, SHUT_RDWR) < 0) {
qDebug() << "Error: Failed to shutdown socket -" << strerror(errno);
}
}

bool TCPConnection::threadsafe_is_alive()
{
const int32_t now_ms=QOpenHDMavlinkHelper::getTimeMilliseconds();;
const auto elapsed=now_ms-m_last_data_ms;
return elapsed <= 3*1000;
}
if (close(m_socket_fd) < 0) {
qDebug() << "Error: Failed to close socket -" << strerror(errno);
}
#endif
m_socket_fd = -1;

void TCPConnection::process_data(const uint8_t *data, int data_len)
{
m_last_data_ms=QOpenHDMavlinkHelper::getTimeMilliseconds();
mavlink_message_t msg;
for (int i = 0; i < data_len; i++) {
uint8_t res = mavlink_parse_char(m_mav_channel,data[i], &msg, &m_recv_status);
if (res) {
process_mavlink_message(msg);
}
if (m_receive_thread->joinable()) {
m_receive_thread->join();
qDebug() << "Receive thread stopped.";
}
}

void TCPConnection::process_mavlink_message(mavlink_message_t message)
{
m_cb(message);
m_receive_thread = nullptr;
qDebug() << "TCPConnection loop stopped.";
}

void TCPConnection::receive_until_stopped() {
qDebug() << "Receiving data until stopped.";
auto buffer = std::make_unique<std::vector<uint8_t>>(1500); // Enough for MTU 1500 bytes

void TCPConnection::receive_until_stopped()
{
// Enough for MTU 1500 bytes.
auto buffer=std::make_unique<std::vector<uint8_t>>();
buffer->resize(1500);
//struct timeval tv;
//tv.tv_sec = 3;
//tv.tv_usec = 0;
//setsockopt(m_socket_fd, SOL_SOCKET, SO_RCVTIMEO, (const char*)&tv, sizeof tv);
while (m_keep_receiving) {
const auto elapsed_last_message=QOpenHDMavlinkHelper::getTimeMilliseconds()-m_last_data_ms;
if(elapsed_last_message>3*1000){
qDebug()<<"No message for more than 3 seconds, disconnecting";
const auto elapsed = QOpenHDMavlinkHelper::getTimeMilliseconds() - m_last_data_ms;
if (elapsed > 3000) {
qDebug() << "No message received for 3 seconds. Disconnecting.";
return;
}

const auto recv_len = recvfrom(
m_socket_fd,
(char*)buffer->data(),
reinterpret_cast<char*>(buffer->data()),
buffer->size(),
0,
nullptr,
nullptr);
nullptr
);

if (recv_len == 0) {
// This can happen when shutdown is called on the socket,
// therefore we check _should_exit again.
//qDebug()<<"Got recv_len==0";
//return;
qDebug() << "Received length 0. Possibly socket closed.";
continue;
}

if (recv_len < 0) {
// This happens on desctruction when close(_socket_fd) is called,
// therefore be quiet.
// LogErr() << "recvfrom error: " << GET_ERROR(errno);
// Something went wrong, we should try to re-connect in next iteration.
//qDebug()<<"Got recv_len<0 :"<<recv_len<<" : "<< strerror(errno);
//return;
qDebug() << "Error receiving data -" << strerror(errno);
continue;
}
process_data(buffer->data(),recv_len);
}
}

void TCPConnection::loop_connect_receive()
{
#ifdef __linux__
qDebug()<<"TCPConnection2::loop_connect_receive begin";
while(m_keep_receiving){
m_socket_fd=tcp_socket_try_connect(m_remote_ip,m_remote_port,3);
if(m_socket_fd>0){
m_last_data_ms=QOpenHDMavlinkHelper::getTimeMilliseconds();
receive_until_stopped();
qDebug()<<"Broke out of receive loop";
close(m_socket_fd);
m_socket_fd=-1;
}else{
int count=0;
while(m_keep_receiving && count<3){
std::this_thread::sleep_for(std::chrono::seconds(1));
count++;
}
}
qDebug() << "Received data. Bytes:" << recv_len;
process_data(buffer->data(), recv_len);
}
qDebug()<<"TCPConnection2::loop_connect_receive end";
#endif
}
Loading

0 comments on commit f775d31

Please sign in to comment.