From 6d356efa221656c2a37b7e07a0054357d13f033f Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Tue, 17 Dec 2013 11:56:49 +1100
Subject: [PATCH] GCS_MAVLink: moved some more functions into GCS_Common.cpp

---
 libraries/GCS_MAVLink/GCS_Common.cpp | 114 +++++++++++++++++++++++++++
 1 file changed, 114 insertions(+)

diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp
index c359dbf4bb..e0c070166c 100644
--- a/libraries/GCS_MAVLink/GCS_Common.cpp
+++ b/libraries/GCS_MAVLink/GCS_Common.cpp
@@ -19,8 +19,122 @@
 
 #include <GCS.h>
 
+extern const AP_HAL::HAL& hal;
+
 GCS_MAVLINK::GCS_MAVLINK() :
     waypoint_receive_timeout(1000)
 {
     AP_Param::setup_object_defaults(this, var_info);
 }
+
+void
+GCS_MAVLINK::init(AP_HAL::UARTDriver *port)
+{
+    GCS_Class::init(port);
+    if (port == (AP_HAL::BetterStream*)hal.uartA) {
+        mavlink_comm_0_port = port;
+        chan = MAVLINK_COMM_0;
+        initialised = true;
+    } else if (port == (AP_HAL::BetterStream*)hal.uartC) {
+        mavlink_comm_1_port = port;
+        chan = MAVLINK_COMM_1;
+        initialised = true;
+#if MAVLINK_COMM_NUM_BUFFERS > 2
+    } else if (port == (AP_HAL::BetterStream*)hal.uartD) {
+        mavlink_comm_2_port = port;
+        chan = MAVLINK_COMM_2;
+        initialised = true;
+#endif
+    }
+    _queued_parameter = NULL;
+    reset_cli_timeout();
+}
+
+
+uint16_t
+GCS_MAVLINK::_count_parameters()
+{
+    // if we haven't cached the parameter count yet...
+    if (0 == _parameter_count) {
+        AP_Param  *vp;
+        AP_Param::ParamToken token;
+
+        vp = AP_Param::first(&token, NULL);
+        do {
+            _parameter_count++;
+        } while (NULL != (vp = AP_Param::next_scalar(&token, NULL)));
+    }
+    return _parameter_count;
+}
+
+/**
+ * @brief Send the next pending parameter, called from deferred message
+ * handling code
+ */
+void
+GCS_MAVLINK::queued_param_send()
+{
+    if (!initialised || _queued_parameter == NULL) {
+        return;
+    }
+
+    uint16_t bytes_allowed;
+    uint8_t count;
+    uint32_t tnow = hal.scheduler->millis();
+
+    // use at most 30% of bandwidth on parameters. The constant 26 is
+    // 1/(1000 * 1/8 * 0.001 * 0.3)
+    bytes_allowed = 57 * (tnow - _queued_parameter_send_time_ms) * 26;
+    if (bytes_allowed > comm_get_txspace(chan)) {
+        bytes_allowed = comm_get_txspace(chan);
+    }
+    count = bytes_allowed / (MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES);
+
+    while (_queued_parameter != NULL && count--) {
+        AP_Param      *vp;
+        float value;
+
+        // copy the current parameter and prepare to move to the next
+        vp = _queued_parameter;
+
+        // if the parameter can be cast to float, report it here and break out of the loop
+        value = vp->cast_to_float(_queued_parameter_type);
+
+        char param_name[AP_MAX_NAME_SIZE];
+        vp->copy_name_token(_queued_parameter_token, param_name, sizeof(param_name), true);
+
+        mavlink_msg_param_value_send(
+            chan,
+            param_name,
+            value,
+            mav_var_type(_queued_parameter_type),
+            _queued_parameter_count,
+            _queued_parameter_index);
+
+        _queued_parameter = AP_Param::next_scalar(&_queued_parameter_token, &_queued_parameter_type);
+        _queued_parameter_index++;
+    }
+    _queued_parameter_send_time_ms = tnow;
+}
+
+/**
+ * @brief Send the next pending waypoint, called from deferred message
+ * handling code
+ */
+void
+GCS_MAVLINK::queued_waypoint_send()
+{
+    if (initialised &&
+        waypoint_receiving &&
+        waypoint_request_i <= waypoint_request_last) {
+        mavlink_msg_mission_request_send(
+            chan,
+            waypoint_dest_sysid,
+            waypoint_dest_compid,
+            waypoint_request_i);
+    }
+}
+
+void GCS_MAVLINK::reset_cli_timeout() {
+      _cli_timeout = hal.scheduler->millis();
+}