Skip to content

Commit

Permalink
connection mode auto
Browse files Browse the repository at this point in the history
  • Loading branch information
@Consti10
Consti10 committed Oct 17, 2023
1 parent 4d069c2 commit eb72ab7
Show file tree
Hide file tree
Showing 19 changed files with 730 additions and 394 deletions.
111 changes: 72 additions & 39 deletions app/telemetry/MavlinkTelemetry.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,22 @@ MavlinkTelemetry::MavlinkTelemetry(QObject *parent):QObject(parent)
{
}

void MavlinkTelemetry::start()
{
auto cb_udp=[this](mavlink_message_t msg){
process_mavlink_message(msg);
};
const auto udp_ip="0.0.0.0"; //"127.0.0.1"
m_udp_connection=std::make_unique<UDPConnection>(udp_ip,QOPENHD_GROUND_CLIENT_UDP_PORT_IN,cb_udp);
m_udp_connection->start();
auto cb_tcp=[this](mavlink_message_t msg){
process_mavlink_message(msg);
};
m_tcp_connection=std::make_unique<TCPConnection>(cb_tcp);
m_heartbeat_thread_run=true;
m_heartbeat_thread=std::make_unique<std::thread>(&MavlinkTelemetry::send_heartbeat_loop,this);
}

MavlinkTelemetry::~MavlinkTelemetry()
{
if(m_heartbeat_thread){
Expand All @@ -30,15 +46,6 @@ MavlinkTelemetry &MavlinkTelemetry::instance()
return instance;
}

void MavlinkTelemetry::start()
{
QSettings settings;
// By default, we always use UDP / localhost mode.
enable_udp();
m_heartbeat_thread_run=true;
m_heartbeat_thread=std::make_unique<std::thread>(&MavlinkTelemetry::send_heartbeat_loop,this);
}

bool MavlinkTelemetry::sendMessage(mavlink_message_t msg){
const auto sys_id=QOpenHDMavlinkHelper::get_own_sys_id();
const auto comp_id=QOpenHDMavlinkHelper::get_own_comp_id();
Expand Down Expand Up @@ -229,35 +236,6 @@ void MavlinkTelemetry::process_message_timesync(const mavlink_message_t &msg)
}


void MavlinkTelemetry::add_tcp_connection_handler(QString ip)
{
qDebug()<<"MavlinkTelemetry::add_tcp_connection_handler"<<ip;
QSettings settings;
settings.setValue("dev_mavlink_tcp_ip",ip);
// Stop udp (if currently running)
m_udp_connection=nullptr;
auto cb_tcp=[this](mavlink_message_t msg){
process_mavlink_message(msg);
};
m_tcp_connection=std::make_unique<TCPConnection>(ip.toStdString(),QOPENHD_OPENHD_GROUND_TCP_SERVER_PORT,cb_tcp);
m_tcp_connection->start();
set_udp_localhost_mode_enabled(false);
}

void MavlinkTelemetry::enable_udp()
{
m_tcp_connection=nullptr;
m_udp_connection=nullptr;
// default, udp, passive (like QGC)
auto cb_udp=[this](mavlink_message_t msg){
process_mavlink_message(msg);
};
set_udp_localhost_mode_enabled(true);
const auto ip="0.0.0.0"; //"127.0.0.1"
m_udp_connection=std::make_unique<UDPConnection>(ip,QOPENHD_GROUND_CLIENT_UDP_PORT_IN,cb_udp);
m_udp_connection->start();
}

void MavlinkTelemetry::ping_all_systems()
{
mavlink_message_t msg;
Expand Down Expand Up @@ -285,13 +263,68 @@ void MavlinkTelemetry::re_apply_rates()

void MavlinkTelemetry::send_heartbeat_loop()
{
mavlink_message_t msg;
while(true){
qDebug()<<"send_heartbeat_loop";
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
perform_connection_management();
}
/*mavlink_message_t msg;
mavlink_heartbeat_t heartbeat{};
heartbeat.type=MAV_TYPE_GCS;
heartbeat.autopilot=MAV_AUTOPILOT_INVALID;
while(m_heartbeat_thread_run){
std::this_thread::sleep_for(std::chrono::milliseconds(500));
mavlink_msg_heartbeat_encode(QOpenHDMavlinkHelper::get_own_sys_id(),QOpenHDMavlinkHelper::get_own_comp_id(),&msg,&heartbeat);
sendMessage(msg);
}*/
}

void MavlinkTelemetry::perform_connection_management()
{
QSettings settings;
int mavlink_connection_mode=settings.value("mavlink_connection_mode",0).toInt();
if(mavlink_connection_mode<0 || mavlink_connection_mode>2)mavlink_connection_mode=0;
mavlink_connection_mode=0;
if(mavlink_connection_mode==0){
// AUTO
if(m_udp_connection->threadsafe_is_alive()){
// Stop TCP if it is running, we don't need it
m_tcp_connection->stop_receiving();
set_telemetry_connection_status("AUTO-CONNECTED(UDP,LOCALHOST)");
}else{
// UPP is not working, try TCP
const std::string IP_CONSTI_TEST="192.168.178.36";
//const std::string IP_OPENHD_WIFI_HOTSPOT="192.168.3.1";
const std::string IP_OPENHD_WIFI_HOTSPOT=IP_CONSTI_TEST;
const std::string IP_OPENHD_ETHERNET_HOTSPOT="192.168.2.1";
if(m_tcp_connection->try_connect_and_receive(IP_OPENHD_WIFI_HOTSPOT,QOPENHD_OPENHD_GROUND_TCP_SERVER_PORT)){
set_telemetry_connection_status("AUTO-CONNECTED (WIFI,TCP)");
}else if(m_tcp_connection->try_connect_and_receive(IP_OPENHD_ETHERNET_HOTSPOT,QOPENHD_OPENHD_GROUND_TCP_SERVER_PORT)){
set_telemetry_connection_status("AUTO-CONNECTED (ETH,TCP)");
}else{
set_telemetry_connection_status("AUTO-NOT CONNECTED");
}
}
}else if(mavlink_connection_mode==1){
// Explicit UDP
m_tcp_connection->stop_receiving();
std::stringstream ss;
ss<<"MANUAL UDP-"<<(m_udp_connection->threadsafe_is_alive() ? "ALIVE" : "NO DATA");
set_telemetry_connection_status(ss.str().c_str());
}else if(mavlink_connection_mode==2){
// Explicit TCP
const std::string user_ip="192.168.178.36";
const int user_port=5760;
if(m_tcp_connection->m_remote_ip==user_ip && m_tcp_connection->m_remote_port==user_port){
if(!m_tcp_connection->threadsafe_is_alive()){
m_tcp_connection->stop_receiving();
m_tcp_connection->try_connect_and_receive(user_ip,user_port);
}
}
std::stringstream ss;
ss<<"MANUAL TCP -"<<(m_udp_connection->threadsafe_is_alive() ? "CONNECTED" : "WRONG IP?");
set_telemetry_connection_status(ss.str().c_str());
}else{

}
}
6 changes: 2 additions & 4 deletions app/telemetry/MavlinkTelemetry.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -55,16 +55,13 @@ class MavlinkTelemetry : public QObject
Q_INVOKABLE void ping_all_systems();
// re-apply all FC telemetry rate(s)
Q_INVOKABLE void re_apply_rates();
// Switch from UDP to TCP
Q_INVOKABLE void add_tcp_connection_handler(QString ip);
// Back to udp
Q_INVOKABLE void enable_udp();
public:
// A couple of stats exposed as QT properties
L_RO_PROP(int,telemetry_pps_in,set_telemetry_pps_in,-1)
L_RO_PROP(int,telemetry_bps_in,set_telemetry_bps_in,-1)
L_RO_PROP(bool,udp_localhost_mode_enabled,set_udp_localhost_mode_enabled,true)
//
L_RO_PROP(QString,telemetry_connection_status,set_telemetry_connection_status,"N/A");
private:
// We follow the same practice as QGrouncontroll: Listen for incoming data on a specific UDP port,
// -> as soon as we got the first packet, we know the address to send data to for bidirectional communication
Expand Down Expand Up @@ -92,6 +89,7 @@ class MavlinkTelemetry : public QObject
std::unique_ptr<std::thread> m_heartbeat_thread;
std::atomic_bool m_heartbeat_thread_run;
void send_heartbeat_loop();
void perform_connection_management();
};

#endif // OHDMAVLINKCONNECTION_H
Loading

0 comments on commit eb72ab7

Please sign in to comment.