redundancy

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<bool> 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<HILData, Eigen::aligned_allocator<HILData>> hil_data_;
     std::atomic<bool> gotSigInt_ {false};
 
-    bool received_heartbeats_ {false};
+    bool received_heartbeats_[2] {false, false};
 
     std::mutex receiver_buff_mtx_;
     std::queue<std::shared_ptr<mavlink_message_t>> receiver_buffer_;

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<std::string>("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<int>("mavlink_udp_remote_port");
     mavlink_interface_->SetMavlinkUdpRemotePort(mavlink_udp_remote_port);

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<mavlink_message_t> 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<std::mutex> 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;