GCS_MAVLink: implement UART pass-thru

libraries/GCS_MAVLink/GCS.h

@@ -754,6 +754,9 @@ public:
     // get the VFR_HUD throttle
     int16_t get_hud_throttle(void) const { return num_gcs()>0?chan(0).vfr_hud_throttle():0; }
 
+    // update uart pass-thru
+    void update_passthru();
+
 private:
 
     static GCS *_singleton;
@@ -778,6 +781,22 @@ private:
 
     // true if we are running short on time in our main loop
     bool _out_of_time;
+
+    // handle passthru between two UARTs
+    struct {
+        bool enabled;
+        bool timer_installed;
+        AP_HAL::UARTDriver *port1;
+        AP_HAL::UARTDriver *port2;
+        uint32_t start_ms;
+        uint32_t last_ms;
+        uint32_t last_port1_data_ms;
+        uint8_t timeout_s;
+        HAL_Semaphore sem;
+    } _passthru;
+
+    // timer called to implement pass-thru
+    void passthru_timer();
 };
 
 GCS &gcs();

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -1944,6 +1944,8 @@ void GCS::update_receive(void)
             chan(i).update_receive();
         }
     }
+    // also update UART pass-thru, if enabled
+    update_passthru();
 }
 
 void GCS::send_mission_item_reached_message(uint16_t mission_index)
@@ -4012,6 +4014,91 @@ bool GCS_MAVLINK::accept_packet(const mavlink_status_t &status,
     return false;
 }
 
+/*
+  update UART pass-thru, if enabled
+ */
+void GCS::update_passthru(void)
+{
+    WITH_SEMAPHORE(_passthru.sem);
+    uint32_t now = AP_HAL::millis();
+
+    bool enabled = AP::serialmanager().get_passthru(_passthru.port1, _passthru.port2, _passthru.timeout_s);
+    if (enabled && !_passthru.enabled) {
+        _passthru.start_ms = now;
+        _passthru.last_ms = 0;
+        _passthru.enabled = true;
+        _passthru.last_port1_data_ms = now;
+        gcs().send_text(MAV_SEVERITY_INFO, "Passthru enabled");
+        if (!_passthru.timer_installed) {
+            _passthru.timer_installed = true;
+            hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&GCS::passthru_timer, void));
+        }
+    } else if (!enabled && _passthru.enabled) {
+        _passthru.enabled = false;
+        _passthru.port1->lock_port(0, 0);
+        _passthru.port2->lock_port(0, 0);
+        gcs().send_text(MAV_SEVERITY_INFO, "Passthru disabled");
+    } else if (enabled &&
+               _passthru.timeout_s &&
+               now - _passthru.last_port1_data_ms > uint32_t(_passthru.timeout_s)*1000U) {
+        // timed out, disable
+        _passthru.enabled = false;
+        _passthru.port1->lock_port(0, 0);
+        _passthru.port2->lock_port(0, 0);
+        AP::serialmanager().disable_passthru();
+        gcs().send_text(MAV_SEVERITY_INFO, "Passthru timed out");
+    }
+}
+
+/*
+  called at 1kHz to handle pass-thru between SERIA0_PASSTHRU port and hal.console
+ */
+void GCS::passthru_timer(void)
+{
+    WITH_SEMAPHORE(_passthru.sem);
+
+    if (!_passthru.enabled) {
+        // it has been disabled after starting
+        return;
+    }
+    if (_passthru.start_ms != 0) {
+        uint32_t now = AP_HAL::millis();
+        if (now - _passthru.start_ms < 1000) {
+            // delay for 1s so the reply for the SERIAL0_PASSTHRU param set can be seen by GCS
+            return;
+        }
+        _passthru.start_ms = 0;
+    }
+
+    // while pass-thru is enabled lock both ports. They remain
+    // locked until disabled again, or reboot
+    const uint32_t lock_key = 0x3256AB9F;
+    _passthru.port1->lock_port(lock_key, lock_key);
+    _passthru.port2->lock_port(lock_key, lock_key);
+
+    int16_t b;
+    uint8_t buf[64];
+    uint8_t nbytes = 0;
+
+    // read from port1, and write to port2
+    while (nbytes < sizeof(buf) && (b = _passthru.port1->read_locked(lock_key)) >= 0) {
+        buf[nbytes++] = b;
+    }
+    if (nbytes > 0) {
+        _passthru.last_port1_data_ms = AP_HAL::millis();
+        _passthru.port2->write_locked(buf, nbytes, lock_key);
+    }
+
+    // read from port2, and write to port1
+    nbytes = 0;
+    while (nbytes < sizeof(buf) && (b = _passthru.port2->read_locked(lock_key)) >= 0) {
+        buf[nbytes++] = b;
+    }
+    if (nbytes > 0) {
+        _passthru.port1->write_locked(buf, nbytes, lock_key);
+    }
+}
+
 
 GCS &gcs()
 {