diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp
index 7d2b9ddb39..9df9e0eb0b 100644
--- a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp
+++ b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp
@@ -315,7 +315,9 @@ void UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
             }
             sercfg.irq_cb = rx_irq_cb;
 #endif // HAL_UART_NODMA
-            sercfg.cr2 |= USART_CR2_STOP1_BITS;
+            if (!(sercfg.cr2 & USART_CR2_STOP2_BITS)) {
+                sercfg.cr2 |= USART_CR2_STOP1_BITS;
+            }
             sercfg.ctx = (void*)this;
 
             sdStart((SerialDriver*)sdef.serial, &sercfg);
@@ -1303,12 +1305,15 @@ void UARTDriver::set_stop_bits(int n)
 
     switch (n) {
     case 1:
-        sercfg.cr2 = _cr2_options | USART_CR2_STOP1_BITS;
+        _cr2_options &= ~USART_CR2_STOP2_BITS;
+        _cr2_options |= USART_CR2_STOP1_BITS;
         break;
     case 2:
-        sercfg.cr2 = _cr2_options | USART_CR2_STOP2_BITS;
+        _cr2_options &= ~USART_CR2_STOP1_BITS;
+        _cr2_options |= USART_CR2_STOP2_BITS;
         break;
     }
+    sercfg.cr2 = _cr2_options;
 
     sdStart((SerialDriver*)sdef.serial, &sercfg);
 #ifndef HAL_UART_NODMA