diff --git a/libraries/AP_HAL_Linux/GPIO_RPI.cpp b/libraries/AP_HAL_Linux/GPIO_RPI.cpp
index bdd0b9c1c5..a5d3872b7f 100644
--- a/libraries/AP_HAL_Linux/GPIO_RPI.cpp
+++ b/libraries/AP_HAL_Linux/GPIO_RPI.cpp
@@ -228,7 +228,12 @@ void GPIO_RPI::write(uint8_t pin, uint8_t value)
 
 void GPIO_RPI::toggle(uint8_t pin)
 {
-    write(pin, !read(pin));
+    if (pin >= GPIO_RPI_MAX_NUMBER_PINS) {
+        return ;
+    }
+    uint32_t flag = (1 << pin);
+    _gpio_output_port_status ^= flag;
+    write(pin, (_gpio_output_port_status & flag) >> pin);
 }
 
 /* Alternative interface: */
diff --git a/libraries/AP_HAL_Linux/GPIO_RPI.h b/libraries/AP_HAL_Linux/GPIO_RPI.h
index 768dd6eed1..f694dce679 100644
--- a/libraries/AP_HAL_Linux/GPIO_RPI.h
+++ b/libraries/AP_HAL_Linux/GPIO_RPI.h
@@ -192,6 +192,9 @@ private:
     // File descriptor for the memory device file
     // If it's negative, then there was an error opening the file.
     int _system_memory_device;
+    // store GPIO output status.
+    uint32_t _gpio_output_port_status = 0x00;
+
 };
 
 }