Block MANUAL_CONTROL messages in SatCom queue

udp2mqtt.py

@@ -11,9 +11,14 @@
 import tornado.httpclient
 import tornado.httputil
 
+from pymavlink import mavlink
+
+
 LOG_FORMAT = '%(levelname) -10s %(asctime)s %(name) -30s %(funcName) -35s %(lineno) -5d: %(message)s'
 LOGGER = logging.getLogger(__name__)
 
+MAV = mavlink.MAVLink(0)
+
 
 class UdpInterface():
     def __init__(self, rx_port, tx_port, type):
@@ -56,6 +61,7 @@ def __init__(self, ip, port, user, pwd):
         self.__client_connected_flag = False
         self.__client_bad_connection_flag = False
         self.__publish_counter = 1
+        self.__rejection_counter = 0
         self.lte_on_message_callback = None
         self.satcom_on_message_callback = None
 
@@ -122,6 +128,25 @@ def publish_lte_message(self, data):
         self.__publish_message('telem/LTE_to_plane', data)
 
     def publish_satcom_message(self, data):
+        # check if the message is a valid one
+        m = None
+        try:
+            m = MAV.parse_buffer(data)
+        except:
+            pass
+        try:
+            if m is not None:
+                for msg in m:
+                    if (msg.get_msgId() == 69):
+                        self.__rejection_counter += 1
+
+                        if self.__rejection_counter == 100:
+                            self.__rejection_counter = 0
+                            LOGGER.warn('Satcom: Blocking MANUAL_CONTROL message')
+                        return
+        except:
+            pass
+
         LOGGER.warn('Send SatCom message to plane')
         self.__publish_message('telem/SatCom_to_plane', data)