AP_HAL_LINUX:toggle gpio port

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: */

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;
+
 };
 
 }