From b5a5b7151260b5b1ae7b18fd32559185e83318c1 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 17 Dec 2014 10:30:09 +1100
Subject: [PATCH] GCS_MAVLink: reverse sense of check_and_forward()

also increase number of routes on larger systems and improved route
learning logic
---
 libraries/GCS_MAVLink/GCS_Common.cpp      |  2 +-
 libraries/GCS_MAVLink/MAVLink_routing.cpp | 20 +++++++++++---------
 libraries/GCS_MAVLink/MAVLink_routing.h   | 11 +++++++----
 3 files changed, 19 insertions(+), 14 deletions(-)

diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp
index caf9b1872e..c7c5fdb3fb 100644
--- a/libraries/GCS_MAVLink/GCS_Common.cpp
+++ b/libraries/GCS_MAVLink/GCS_Common.cpp
@@ -859,7 +859,7 @@ GCS_MAVLINK::update(void (*run_cli)(AP_HAL::UARTDriver *))
             if (msg_snoop != NULL) {
                 msg_snoop(&msg);
             }
-            if (!routing.check_and_forward(chan, &msg)) {
+            if (routing.check_and_forward(chan, &msg)) {
                 handleMessage(&msg);
             }
         }
diff --git a/libraries/GCS_MAVLink/MAVLink_routing.cpp b/libraries/GCS_MAVLink/MAVLink_routing.cpp
index 150cf22e7c..5b2eaad929 100644
--- a/libraries/GCS_MAVLink/MAVLink_routing.cpp
+++ b/libraries/GCS_MAVLink/MAVLink_routing.cpp
@@ -36,10 +36,9 @@ MAVLink_routing::MAVLink_routing(void) : num_routes(0) {}
   automatically learns the route for the sender if it is not
   already known.
   
-  This returns true if the message matched a route and was
-  forwarded. 
+  This returns true if the message should be processed locally
 
-  Theory of MAVLink routing   
+  Theory of MAVLink routing:
 
   When a flight controller receives a message it should process it
   locally if any of these conditions hold:
@@ -99,7 +98,7 @@ bool MAVLink_routing::check_and_forward(mavlink_channel_t in_channel, const mavl
     if (msg->msgid == MAVLINK_MSG_ID_HEARTBEAT) {
         // heartbeat needs special handling
         handle_heartbeat(in_channel, msg);
-        return false;
+        return true;
     }
 
     // extract the targets for this packet
@@ -116,7 +115,7 @@ bool MAVLink_routing::check_and_forward(mavlink_channel_t in_channel, const mavl
 
     if (process_locally && !broadcast_system && !broadcast_component) {
         // nothing more to do - it can only be for us
-        return false;
+        return true;
     }
 
     // forward on any channels matching the targets
@@ -146,7 +145,7 @@ bool MAVLink_routing::check_and_forward(mavlink_channel_t in_channel, const mavl
         process_locally = true;
     }
 
-    return !process_locally;
+    return process_locally;
 }
 
 /*
@@ -155,6 +154,11 @@ bool MAVLink_routing::check_and_forward(mavlink_channel_t in_channel, const mavl
 void MAVLink_routing::learn_route(mavlink_channel_t in_channel, const mavlink_message_t* msg)
 {
     uint8_t i;
+    if (msg->sysid == 0 || 
+        (msg->sysid == mavlink_system.sysid && 
+         msg->compid == mavlink_system.compid)) {
+        return;
+    }
     for (i=0; i<num_routes; i++) {
         if (routes[i].sysid == msg->sysid && 
             routes[i].compid == msg->compid &&
@@ -162,9 +166,7 @@ void MAVLink_routing::learn_route(mavlink_channel_t in_channel, const mavlink_me
             break;
         }
     }
-    if (i == num_routes && i<MAVLINK_MAX_ROUTES &&
-        (routes[i].sysid != mavlink_system.sysid ||
-         routes[i].compid != mavlink_system.compid)) {
+    if (i == num_routes && i<MAVLINK_MAX_ROUTES) {
         routes[i].sysid = msg->sysid;
         routes[i].compid = msg->compid;
         routes[i].channel = in_channel;
diff --git a/libraries/GCS_MAVLink/MAVLink_routing.h b/libraries/GCS_MAVLink/MAVLink_routing.h
index d5f960dc23..1160f1d98b 100644
--- a/libraries/GCS_MAVLink/MAVLink_routing.h
+++ b/libraries/GCS_MAVLink/MAVLink_routing.h
@@ -10,9 +10,13 @@
 #include <AP_Common.h>
 #include <GCS_MAVLink.h>
 
-// 5 routes should be enough for now. This will need to increase as we
-// make more extensive use of MAVLink forwarding
+// 20 routes should be enough for now. This may need to increase as
+// we make more extensive use of MAVLink forwarding
+#if HAL_CPU_CLASS > HAL_CPU_CLASS_16
+#define MAVLINK_MAX_ROUTES 20
+#else
 #define MAVLINK_MAX_ROUTES 5
+#endif
 
 /*
   object to handle MAVLink packet routing
@@ -27,8 +31,7 @@ public:
       automatically learns the route for the sender if it is not
       already known.
 
-      This returns true if the message matched a route and was
-      forwarded. 
+      This returns true if the message should be processed locally
     */
     bool check_and_forward(mavlink_channel_t in_channel, const mavlink_message_t* msg);