From 53f0df252b7357c451c962a02bdbbd2fd03841f0 Mon Sep 17 00:00:00 2001 From: Jari Nippula Date: Tue, 6 Aug 2024 15:24:28 +0300 Subject: [PATCH] redundancy --- include/mavlink_interface.h | 9 ++++-- src/gazebo_mavlink_interface.cpp | 7 +++++ src/mavlink_interface.cpp | 54 ++++++++++++++++++++++++++++++-- 3 files changed, 65 insertions(+), 5 deletions(-) diff --git a/include/mavlink_interface.h b/include/mavlink_interface.h index 5eb90a9..6637cdc 100644 --- a/include/mavlink_interface.h +++ b/include/mavlink_interface.h @@ -159,12 +159,13 @@ class MavlinkInterface { void SetDevice(std::string device) {device_ = device;} void SetEnableLockstep(bool enable_lockstep) {enable_lockstep_ = enable_lockstep;} void SetMavlinkAddr(std::string mavlink_addr) {mavlink_addr_str_ = mavlink_addr;} + void SetSecondaryMavlinkAddr(std::string mavlink_addr) {secondary_mavlink_addr_str_ = mavlink_addr;} void SetMavlinkTcpPort(int mavlink_tcp_port) {mavlink_tcp_port_ = mavlink_tcp_port;} void SetMavlinkUdpRemotePort(int mavlink_udp_port) {mavlink_udp_remote_port_ = mavlink_udp_port;} void SetMavlinkUdpLocalPort(int mavlink_udp_port) {mavlink_udp_local_port_ = mavlink_udp_port;} bool IsRecvBuffEmpty() {return receiver_buffer_.empty();} - bool ReceivedHeartbeats() const { return received_heartbeats_; } + bool ReceivedHeartbeats() const { return received_heartbeats_[0] || received_heartbeats_[1]; } private: bool received_actuator_{false}; @@ -192,6 +193,8 @@ class MavlinkInterface { socklen_t local_simulator_addr_len_; struct sockaddr_in remote_simulator_addr_; socklen_t remote_simulator_addr_len_; + struct sockaddr_in secondary_remote_simulator_addr_; + socklen_t secondary_remote_simulator_addr_len_; unsigned char buf_[65535]; enum FD_TYPES { @@ -205,7 +208,9 @@ class MavlinkInterface { std::atomic close_conn_{false}; in_addr_t mavlink_addr_; + in_addr_t secondary_mavlink_addr_; std::string mavlink_addr_str_{"INADDR_ANY"}; + std::string secondary_mavlink_addr_str_{"INADDR_ANY"}; int mavlink_udp_remote_port_{kDefaultMavlinkUdpRemotePort}; // MAVLink refers to the PX4 simulator interface here int mavlink_udp_local_port_{kDefaultMavlinkUdpLocalPort}; // MAVLink refers to the PX4 simulator interface here int mavlink_tcp_port_{kDefaultMavlinkTcpPort}; // MAVLink refers to the PX4 simulator interface here @@ -246,7 +251,7 @@ class MavlinkInterface { //std::vector> hil_data_; std::atomic gotSigInt_ {false}; - bool received_heartbeats_ {false}; + bool received_heartbeats_[2] {false, false}; std::mutex receiver_buff_mtx_; std::queue> receiver_buffer_; diff --git a/src/gazebo_mavlink_interface.cpp b/src/gazebo_mavlink_interface.cpp index a3163e2..b2ce68d 100644 --- a/src/gazebo_mavlink_interface.cpp +++ b/src/gazebo_mavlink_interface.cpp @@ -177,6 +177,13 @@ void GazeboMavlinkInterface::Configure(const gz::sim::Entity &_entity, } } + if (_sdf->HasElement("secondary_mavlink_addr")) { + std::string mavlink_addr_str = _sdf->Get("secondary_mavlink_addr"); + if (mavlink_addr_str != "INADDR_ANY") { + mavlink_interface_->SetSecondaryMavlinkAddr(mavlink_addr_str); + } + } + if (_sdf->HasElement("mavlink_udp_remote_port")) { int mavlink_udp_remote_port = _sdf->Get("mavlink_udp_remote_port"); mavlink_interface_->SetMavlinkUdpRemotePort(mavlink_udp_remote_port); diff --git a/src/mavlink_interface.cpp b/src/mavlink_interface.cpp index f87bca3..771aa6b 100644 --- a/src/mavlink_interface.cpp +++ b/src/mavlink_interface.cpp @@ -1,4 +1,7 @@ #include "mavlink_interface.h" + +#define MAX_CONSECUTIVE_SPARE_MSG 10 + MavlinkInterface::MavlinkInterface() { } @@ -24,6 +27,10 @@ void MavlinkInterface::Load() remote_simulator_addr_.sin_family = AF_INET; remote_simulator_addr_len_ = sizeof(remote_simulator_addr_); + memset((char *)&secondary_remote_simulator_addr_, 0, sizeof(secondary_remote_simulator_addr_)); + secondary_remote_simulator_addr_.sin_family = AF_INET; + secondary_remote_simulator_addr_len_ = sizeof(secondary_remote_simulator_addr_); + memset((char *)&local_simulator_addr_, 0, sizeof(local_simulator_addr_)); local_simulator_addr_.sin_family = AF_INET; local_simulator_addr_len_ = sizeof(local_simulator_addr_); @@ -100,6 +107,8 @@ void MavlinkInterface::Load() // When connecting to SITL, we specify the port where the mavlink traffic originates from. remote_simulator_addr_.sin_addr.s_addr = mavlink_addr_; remote_simulator_addr_.sin_port = htons(mavlink_udp_remote_port_); + secondary_remote_simulator_addr_.sin_addr.s_addr = secondary_mavlink_addr_; + secondary_remote_simulator_addr_.sin_port = htons(mavlink_udp_remote_port_); local_simulator_addr_.sin_addr.s_addr = htonl(INADDR_ANY); local_simulator_addr_.sin_port = htons(mavlink_udp_local_port_); @@ -147,6 +156,9 @@ std::shared_ptr MavlinkInterface::PopRecvMessage() { } void MavlinkInterface::ReceiveWorker() { + struct sockaddr_in remote_addr; + socklen_t remote_addr_len = sizeof(remote_addr); + char thrd_name[64] = {0}; sprintf(thrd_name, "MAV_Recver_%d", gettid()); pthread_setname_np(pthread_self(), thrd_name); @@ -167,7 +179,7 @@ void MavlinkInterface::ReceiveWorker() { std::cout << "[" << thrd_name << "] Start receiving..." << std::endl; while(!close_conn_ && !gotSigInt_) { - int ret = recvfrom(fds_[CONNECTION_FD].fd, buf_, sizeof(buf_), 0, (struct sockaddr *)&remote_simulator_addr_, &remote_simulator_addr_len_); + int ret = recvfrom(fds_[CONNECTION_FD].fd, buf_, sizeof(buf_), 0, (struct sockaddr *)&remote_addr, &remote_addr_len); if (ret < 0) { std::cerr << "[" << thrd_name << "] recvfrom error: " << strerror(errno) << std::endl; if (errno == ECONNRESET) { @@ -462,15 +474,41 @@ void MavlinkInterface::handle_message(mavlink_message_t *msg) } } -void MavlinkInterface::handle_heartbeat(mavlink_message_t *) +void MavlinkInterface::handle_heartbeat(mavlink_message_t *msg) { - received_heartbeats_ = true; + if (msg->compid == 1) { + received_heartbeats_[0] = true; + } else if (msg->compid == 2) { + received_heartbeats_[1] = true; + } } void MavlinkInterface::handle_actuator_controls(mavlink_message_t *msg) { + static int consecutive_spare_msg = 0; const std::lock_guard lock(actuator_mutex_); + if (msg->compid == 2) { + if (consecutive_spare_msg < MAX_CONSECUTIVE_SPARE_MSG) { + if (consecutive_spare_msg > 4) { + std::cout << "spare " << consecutive_spare_msg << std::endl; + } + consecutive_spare_msg++; + if (consecutive_spare_msg == MAX_CONSECUTIVE_SPARE_MSG) { + std::cout << "Primary controller not updating actuators => switch to backup controller" << std::endl; + } else { + return; + } + } + } else if (msg->compid == 1) { + if (consecutive_spare_msg == MAX_CONSECUTIVE_SPARE_MSG) { + // Already switched to backup controller, ignore primary controller messages + return; + } else { + consecutive_spare_msg = 0; + } + } + mavlink_hil_actuator_controls_t controls; mavlink_msg_hil_actuator_controls_decode(msg, &controls); @@ -505,8 +543,18 @@ void MavlinkInterface::send_mavlink_message(const mavlink_message_t *message) if (use_tcp_) { len = send(fds_[CONNECTION_FD].fd, buffer, packetlen, 0); } else { + ssize_t len2; len = sendto(fds_[CONNECTION_FD].fd, buffer, packetlen, 0, (struct sockaddr *)&remote_simulator_addr_, remote_simulator_addr_len_); + len2 = sendto(fds_[CONNECTION_FD].fd, buffer, packetlen, 0, (struct sockaddr *)&secondary_remote_simulator_addr_, secondary_remote_simulator_addr_len_); + if (len < 0 && len2 < 0) { + // neither one worked => error + len = -1; + } else { + // at least one worked => success + len = 0; + } } + if (len < 0) { if (received_first_actuator_) { std::cerr << "Failed sending mavlink message: " << strerror(errno) << std::endl;