From cf228530f3a4c8163d26d7476b7f7dbd17992950 Mon Sep 17 00:00:00 2001 From: olliw42 Date: Sat, 25 May 2024 11:48:05 +0200 Subject: [PATCH] brad's passthrough pr 27152 --- libraries/AP_HAL/UARTDriver.h | 8 ++- libraries/AP_HAL_ChibiOS/UARTDriver.cpp | 22 ++++++++ libraries/AP_HAL_ChibiOS/UARTDriver.h | 9 +++ .../AP_HAL_ChibiOS/hwdef/common/usbcfg.c | 14 +++++ .../AP_HAL_ChibiOS/hwdef/common/usbcfg.h | 3 + .../hwdef/common/usbcfg_dualcdc.c | 15 +++++ libraries/GCS_MAVLink/GCS.h | 4 ++ libraries/GCS_MAVLink/GCS_Common.cpp | 55 +++++++++++++++++++ 8 files changed, 129 insertions(+), 1 deletion(-) diff --git a/libraries/AP_HAL/UARTDriver.h b/libraries/AP_HAL/UARTDriver.h index 829d7907b315f..f7639bce74978 100644 --- a/libraries/AP_HAL/UARTDriver.h +++ b/libraries/AP_HAL/UARTDriver.h @@ -108,6 +108,9 @@ class AP_HAL::UARTDriver : public AP_HAL::BetterStream { virtual enum flow_control get_flow_control(void) { return FLOW_CONTROL_DISABLE; } virtual void configure_parity(uint8_t v){}; +//BRAD + virtual uint8_t get_parity(void){ return 0; } +//BRADEND virtual void set_stop_bits(int n){}; /* unbuffered writes bypass the ringbuffer and go straight to the @@ -184,7 +187,10 @@ class AP_HAL::UARTDriver : public AP_HAL::BetterStream { // return true requested baud on USB port virtual uint32_t get_usb_baud(void) const { return 0; } - +//BRAD + // return requested parity on USB port + virtual uint8_t get_usb_parity(void) const { return 0; } +//BRADend // disable TX/RX pins for unusued uart virtual void disable_rxtx(void) const {} diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp index 069f64085f98d..b2060947245fd 100644 --- a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp +++ b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp @@ -672,6 +672,21 @@ uint32_t UARTDriver::get_usb_baud() const return 0; } +//BRAD +/* + get the requested usb parity. Valid if get_usb_baud() returned non-zero. +*/ +uint8_t UARTDriver::get_usb_parity() const +{ +#if HAL_USE_SERIAL_USB + if (sdef.is_usb) { + return ::get_usb_parity(sdef.endpoint_id); + } +#endif + return 0; +} +//BRADEND + uint32_t UARTDriver::_available() { if (!_rx_initialised || _uart_owner_thd != chThdGetSelfX()) { @@ -1457,6 +1472,13 @@ void UARTDriver::configure_parity(uint8_t v) #endif // HAL_USE_SERIAL } +//BRAD +uint8_t UARTDriver::get_parity(void) +{ + return UARTDriver::parity; +} +//BRADEND + /* set stop bits */ diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.h b/libraries/AP_HAL_ChibiOS/UARTDriver.h index b28091e053e90..54046ed9fb44f 100644 --- a/libraries/AP_HAL_ChibiOS/UARTDriver.h +++ b/libraries/AP_HAL_ChibiOS/UARTDriver.h @@ -38,6 +38,9 @@ class ChibiOS::UARTDriver : public AP_HAL::UARTDriver { bool is_initialized() override; bool tx_pending() override; uint32_t get_usb_baud() const override; +//BRAD + uint8_t get_usb_parity() const override; +//BRADEND // disable TX/RX pins for unusued uart void disable_rxtx(void) const override; @@ -88,6 +91,9 @@ class ChibiOS::UARTDriver : public AP_HAL::UARTDriver { bool set_unbuffered_writes(bool on) override; void configure_parity(uint8_t v) override; +//BRAD + uint8_t get_parity(void) override; +//BRADEND void set_stop_bits(int n) override; /* @@ -215,6 +221,9 @@ class ChibiOS::UARTDriver : public AP_HAL::UARTDriver { uint32_t _cr2_options; uint32_t _cr3_options; uint16_t _last_options; +//BRAD + uint8_t parity; +//BRADEND // half duplex control. After writing we throw away bytes for 4 byte widths to // prevent reading our own bytes back diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.c b/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.c index eed51ebae3ea3..c485099682e43 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.c +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.c @@ -251,6 +251,20 @@ uint32_t get_usb_baud(uint16_t endpoint_id) } return 0; } + +//BRAD +/* + get the requested usb parity. Valid if get_usb_baud() returned non-zero +*/ +uint8_t get_usb_parity(uint16_t endpoint_id) +{ + if (endpoint_id == 0) { + return linecoding.bParityType; + } + + return 0; +} +//BRADEND #endif /** * @brief IN EP1 state. diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.h b/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.h index 2add6e6c7f9bb..1cb2161c5e970 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.h @@ -46,6 +46,9 @@ extern SerialUSBDriver SDU2; extern const SerialUSBConfig serusbcfg2; #endif //HAL_HAVE_DUAL_USB_CDC uint32_t get_usb_baud(uint16_t endpoint_id); +//BRAD +uint8_t get_usb_parity(uint16_t endpoint_id); +//BRADEND #endif #define USB_DESC_MAX_STRLEN 100 void setup_usb_strings(void); diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg_dualcdc.c b/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg_dualcdc.c index cb1a127287ca1..cf93382bf57fb 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg_dualcdc.c +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg_dualcdc.c @@ -315,6 +315,21 @@ uint32_t get_usb_baud(uint16_t endpoint_id) } return 0; } + +//BRAD +/* + get the requested usb parity. Valid if get_usb_baud() returned non-zero +*/ +uint8_t get_usb_parity(uint16_t endpoint_id) +{ + for (uint8_t i = 0; i < ARRAY_SIZE(linecoding); i++) { + if (endpoint_id == ep_index[i]) { + return linecoding[i].bParityType; + } + } + return 0; +} +//BRADEND #endif /** * @brief IN EP1 state. diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 89bc6635e14f0..5f4964357d693 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -1340,6 +1340,10 @@ class GCS uint32_t last_port1_data_ms; uint32_t baud1; uint32_t baud2; +//BRAD + uint8_t parity1; + uint8_t parity2; +//BRADEND uint8_t timeout_s; HAL_Semaphore sem; } _passthru; diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 26d2388205c44..a16b45ad051ec 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -6702,6 +6702,9 @@ void GCS::update_passthru(void) WITH_SEMAPHORE(_passthru.sem); uint32_t now = AP_HAL::millis(); uint32_t baud1, baud2; +//BRAD + uint8_t parity1 = 0, parity2 = 0; +//BRADEND bool enabled = AP::serialmanager().get_passthru(_passthru.port1, _passthru.port2, _passthru.timeout_s, baud1, baud2); if (enabled && !_passthru.enabled) { @@ -6711,6 +6714,10 @@ void GCS::update_passthru(void) _passthru.last_port1_data_ms = now; _passthru.baud1 = baud1; _passthru.baud2 = baud2; +//BRAD + _passthru.parity1 = parity1 = _passthru.port1->get_parity(); + _passthru.parity2 = parity2 = _passthru.port2->get_parity(); +//BRADEND gcs().send_text(MAV_SEVERITY_INFO, "Passthru enabled"); if (!_passthru.timer_installed) { _passthru.timer_installed = true; @@ -6729,6 +6736,15 @@ void GCS::update_passthru(void) _passthru.port2->end(); _passthru.port2->begin(baud2); } +//BRAD + // Restore original parity + if (_passthru.parity1 != parity1) { + _passthru.port1->configure_parity(parity1); + } + if (_passthru.parity2 != parity2) { + _passthru.port2->configure_parity(parity2); + } +//BRADEND gcs().send_text(MAV_SEVERITY_INFO, "Passthru disabled"); } else if (enabled && _passthru.timeout_s && @@ -6747,6 +6763,15 @@ void GCS::update_passthru(void) _passthru.port2->end(); _passthru.port2->begin(baud2); } +//BRAD + // Restore original parity + if (_passthru.parity1 != parity1) { + _passthru.port1->configure_parity(parity1); + } + if (_passthru.parity2 != parity2) { + _passthru.port2->configure_parity(parity2); + } +//BRAD gcs().send_text(MAV_SEVERITY_INFO, "Passthru timed out"); } } @@ -6781,18 +6806,48 @@ void GCS::passthru_timer(void) // Check for requested Baud rates over USB uint32_t baud = _passthru.port1->get_usb_baud(); +//BRAD +/* if (_passthru.baud2 != baud && baud != 0) { _passthru.baud2 = baud; _passthru.port2->end(); _passthru.port2->begin_locked(baud, 0, 0, lock_key); + } */ + uint8_t parity = _passthru.port1->get_usb_parity(); + if (baud != 0) { // port1 is USB + if (_passthru.baud2 != baud) { + _passthru.baud2 = baud; + _passthru.port2->end(); + _passthru.port2->begin_locked(baud, 0, 0, lock_key); + } + if (_passthru.parity2 != parity) { + _passthru.parity2 = parity; + _passthru.port2->configure_parity(parity); + } } +//BRADEND baud = _passthru.port2->get_usb_baud(); +//BRAD +/* if (_passthru.baud1 != baud && baud != 0) { _passthru.baud1 = baud; _passthru.port1->end(); _passthru.port1->begin_locked(baud, 0, 0, lock_key); + } */ + parity = _passthru.port2->get_usb_parity(); + if (baud != 0) { // port2 is USB + if (_passthru.baud1 != baud) { + _passthru.baud1 = baud; + _passthru.port1->end(); + _passthru.port1->begin_locked(baud, 0, 0, lock_key); + } + if (_passthru.parity1 != parity) { + _passthru.parity1 = parity; + _passthru.port1->configure_parity(parity); + } } +//BRADEND uint8_t buf[64];