diff --git a/libraries/AP_HAL_ESP32/GPIO.cpp b/libraries/AP_HAL_ESP32/GPIO.cpp
new file mode 100644
index 0000000000..66cbff5b31
--- /dev/null
+++ b/libraries/AP_HAL_ESP32/GPIO.cpp
@@ -0,0 +1,141 @@
+/*
+ * This file is free software: you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ * Code by Bayu Laksono
+ */
+
+#include "AP_HAL_ESP32.h"
+#include "GPIO.h"
+
+#include "hal/gpio_types.h"
+#include "driver/gpio.h"
+
+using namespace ESP32;
+
+static gpio_num_t gpio_by_pin_num(uint8_t pin)
+{
+    if (pin < GPIO_NUM_MAX) {
+        return (gpio_num_t)pin;
+    }
+    return GPIO_NUM_NC;
+}
+
+GPIO::GPIO()
+{}
+
+void GPIO::init()
+{}
+
+void GPIO::pinMode(uint8_t pin, uint8_t output)
+{
+    gpio_num_t g = gpio_by_pin_num(pin);
+    if (g != GPIO_NUM_NC) {
+        gpio_config_t io_conf = {};
+        io_conf.intr_type = GPIO_INTR_DISABLE;
+        io_conf.mode = output ? GPIO_MODE_OUTPUT : GPIO_MODE_INPUT;
+        io_conf.pin_bit_mask = 1ULL<<g;
+        io_conf.pull_down_en = output ? GPIO_PULLDOWN_DISABLE : GPIO_PULLDOWN_ENABLE;
+        io_conf.pull_up_en = GPIO_PULLUP_DISABLE;
+        gpio_config(&io_conf);
+    }
+}
+
+uint8_t GPIO::read(uint8_t pin)
+{
+    gpio_num_t g = gpio_by_pin_num(pin);
+    if (g != GPIO_NUM_NC) {
+        return gpio_get_level(g);
+    }
+    return 0;
+}
+
+void GPIO::write(uint8_t pin, uint8_t value)
+{
+    gpio_num_t g = gpio_by_pin_num(pin);
+    if (g != GPIO_NUM_NC) {
+        gpio_set_level(g, value);
+    }
+}
+
+void GPIO::toggle(uint8_t pin)
+{
+    gpio_num_t g = gpio_by_pin_num(pin);
+    if (g != GPIO_NUM_NC) {
+        if (gpio_get_level(g)) {
+            gpio_set_level(g, 0);
+        }
+        else {
+            gpio_set_level(g, 1);
+        }
+    }
+}
+
+/* Alternative interface: */
+AP_HAL::DigitalSource* GPIO::channel(uint16_t pin) {
+    gpio_num_t g = gpio_by_pin_num(pin);
+    if (g != GPIO_NUM_NC) {
+        return NEW_NOTHROW DigitalSource(pin);
+    }
+    return nullptr;
+}
+
+bool GPIO::usb_connected(void)
+{
+    return false;
+}
+
+DigitalSource::DigitalSource(uint8_t pin) :
+    _pin(pin)
+{}
+
+void DigitalSource::mode(uint8_t output)
+{
+    gpio_num_t g = gpio_by_pin_num(_pin);
+    if (g != GPIO_NUM_NC) {
+        gpio_config_t io_conf = {};
+        io_conf.intr_type = GPIO_INTR_DISABLE;
+        io_conf.mode = output ? GPIO_MODE_OUTPUT : GPIO_MODE_INPUT;
+        io_conf.pin_bit_mask = 1ULL<<g;
+        io_conf.pull_down_en = output ? GPIO_PULLDOWN_DISABLE : GPIO_PULLDOWN_ENABLE;
+        io_conf.pull_up_en = GPIO_PULLUP_DISABLE;
+        gpio_config(&io_conf);
+    }
+}
+
+uint8_t DigitalSource::read() {
+    gpio_num_t g = gpio_by_pin_num(_pin);
+    if (g != GPIO_NUM_NC) {
+        return gpio_get_level(g);
+    }
+    return 0;
+}
+
+void DigitalSource::write(uint8_t value) {
+    gpio_num_t g = gpio_by_pin_num(_pin);
+    if (g != GPIO_NUM_NC) {
+        gpio_set_level(g, value);
+    }
+}
+
+void DigitalSource::toggle() {
+    gpio_num_t g = gpio_by_pin_num(_pin);
+    if (g != GPIO_NUM_NC) {
+        if (gpio_get_level(g)) {
+            gpio_set_level(g, 0);
+        }
+        else {
+            gpio_set_level(g, 1);
+        }
+    }
+}
diff --git a/libraries/AP_HAL_ESP32/GPIO.h b/libraries/AP_HAL_ESP32/GPIO.h
new file mode 100644
index 0000000000..277044e315
--- /dev/null
+++ b/libraries/AP_HAL_ESP32/GPIO.h
@@ -0,0 +1,45 @@
+/*
+ * This file is free software: you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "AP_HAL_ESP32.h"
+
+class ESP32::GPIO : public AP_HAL::GPIO {
+public:
+    GPIO();
+    void    init() override;
+    void    pinMode(uint8_t pin, uint8_t output) override;
+    uint8_t read(uint8_t pin) override;
+    void    write(uint8_t pin, uint8_t value) override;
+    void    toggle(uint8_t pin) override;
+
+    /* Alternative interface: */
+    AP_HAL::DigitalSource* channel(uint16_t n) override;
+
+    /* return true if USB cable is connected */
+    bool    usb_connected(void) override;
+};
+
+class ESP32::DigitalSource : public AP_HAL::DigitalSource {
+public:
+    DigitalSource(uint8_t pin);
+    void    mode(uint8_t output) override;
+    uint8_t read() override;
+    void    write(uint8_t value) override;
+    void    toggle() override;
+private:
+    uint8_t _pin;
+};
diff --git a/libraries/AP_HAL_ESP32/HAL_ESP32_Class.cpp b/libraries/AP_HAL_ESP32/HAL_ESP32_Class.cpp
index fb37729dec..c0b039e386 100644
--- a/libraries/AP_HAL_ESP32/HAL_ESP32_Class.cpp
+++ b/libraries/AP_HAL_ESP32/HAL_ESP32_Class.cpp
@@ -25,6 +25,7 @@
 #include "WiFiUdpDriver.h"
 #include "RCInput.h"
 #include "RCOutput.h"
+#include "GPIO.h"
 #include "Storage.h"
 #include "AnalogIn.h"
 #include "Util.h"
@@ -69,7 +70,7 @@ static Empty::Storage storageDriver;
 #else
 static ESP32::Storage storageDriver;
 #endif
-static Empty::GPIO gpioDriver;
+static ESP32::GPIO gpioDriver;
 #if AP_SIM_ENABLED
 static Empty::RCOutput rcoutDriver;
 #else