From 9a99d9bff54ead8bc9fb4e1b6298bba9ad835644 Mon Sep 17 00:00:00 2001
From: Tatsuya Yamaguchi <susumu-336@hotmail.co.jp>
Date: Wed, 11 Sep 2019 10:26:15 +0900
Subject: [PATCH] GCS_MAVLink: add set_message_interval functions

---
 libraries/GCS_MAVLink/GCS.h          |  5 +++++
 libraries/GCS_MAVLink/GCS_Common.cpp | 32 ++++++++++++++++++++++++++--
 2 files changed, 35 insertions(+), 2 deletions(-)

diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h
index 47e0885951..5b3fe81cb3 100644
--- a/libraries/GCS_MAVLink/GCS.h
+++ b/libraries/GCS_MAVLink/GCS.h
@@ -273,6 +273,8 @@ public:
     virtual uint64_t capabilities() const;
     uint8_t get_stream_slowdown_ms() const { return stream_slowdown_ms; }
 
+    MAV_RESULT set_message_interval(uint32_t msg_id, int32_t interval_us);
+
 protected:
 
     virtual bool in_hil_mode() const { return false; }
@@ -784,6 +786,9 @@ public:
     virtual bool simple_input_active() const { return false; }
     virtual bool supersimple_input_active() const { return false; }
 
+    MAV_RESULT set_message_interval(uint8_t port_num, uint32_t msg_id, int32_t interval_us);
+    uint8_t get_channel_from_port_number(uint8_t port_num);
+
 protected:
 
     virtual GCS_MAVLINK *new_gcs_mavlink_backend(GCS_MAVLINK_Parameters &params,
diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp
index 6ab4647b92..af7282bd3f 100644
--- a/libraries/GCS_MAVLink/GCS_Common.cpp
+++ b/libraries/GCS_MAVLink/GCS_Common.cpp
@@ -2229,9 +2229,11 @@ void GCS_MAVLINK::send_heartbeat() const
 
 MAV_RESULT GCS_MAVLINK::handle_command_set_message_interval(const mavlink_command_long_t &packet)
 {
-    const uint32_t msg_id = (uint32_t)packet.param1;
-    const int32_t interval_us = (int32_t)packet.param2;
+    return set_message_interval((uint32_t)packet.param1, (int32_t)packet.param2);
+}
 
+MAV_RESULT GCS_MAVLINK::set_message_interval(uint32_t msg_id, int32_t interval_us)
+{
     uint16_t interval_ms;
     if (interval_us == 0) {
         // zero is "reset to default rate"
@@ -2256,6 +2258,32 @@ MAV_RESULT GCS_MAVLINK::handle_command_set_message_interval(const mavlink_comman
     return MAV_RESULT_FAILED;
 }
 
+/*
+  this function is reserved for use by scripting
+ */
+MAV_RESULT GCS::set_message_interval(uint8_t port_num, uint32_t msg_id, int32_t interval_us)
+{
+    uint8_t channel = get_channel_from_port_number(port_num);
+
+    if (channel < MAVLINK_COMM_NUM_BUFFERS && chan(channel) != nullptr)
+    {
+        chan(channel)->set_message_interval(msg_id, interval_us);
+    }
+
+    return MAV_RESULT_FAILED;
+}
+
+uint8_t GCS::get_channel_from_port_number(uint8_t port_num)
+{
+    for (uint8_t i=0; i<num_gcs(); i++) {
+        if (chan(i)->get_uart() == AP::serialmanager().get_serial_by_id(port_num)) {
+            return i;
+        }
+    }
+
+    return UINT8_MAX;
+}
+
 MAV_RESULT GCS_MAVLINK::handle_command_request_message(const mavlink_command_long_t &packet)
 {
     const uint32_t mavlink_id = (uint32_t)packet.param1;