diff --git a/drivers/boards/bsp_drv.h b/drivers/boards/bsp_drv.h
index 2b46830..d30ad64 100644
--- a/drivers/boards/bsp_drv.h
+++ b/drivers/boards/bsp_drv.h
@@ -6,8 +6,15 @@
 #define LED2 2
 #define LED3 3
 
-#define LED_DISABLE 0
-#define LED_ENABLE 1
+#define GPIO_PIN0 0
+
+#define GPIO_RESET 0
+#define GPIO_SET 1
+#define GPIO_TOGGLE 2
+
+#define LED_DISABLE GPIO_RESET
+#define LED_ENABLE GPIO_SET
+#define LED_TOGGLE GPIO_TOGGLE
 
 void led_write(int fd, int state);
 
diff --git a/drivers/boards/dynamics_wizard.c b/drivers/boards/dynamics_wizard.c
index 3ee35d7..e674f5e 100644
--- a/drivers/boards/dynamics_wizard.c
+++ b/drivers/boards/dynamics_wizard.c
@@ -19,18 +19,38 @@ int led_open(struct inode *inode, struct file *file)
 
 int led_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
 {
-    if (arg != LED_ENABLE && arg != LED_DISABLE)
+    if (arg != LED_ENABLE && arg != LED_DISABLE && arg != LED_TOGGLE)
         return -EINVAL;
 
+    GPIO_TypeDef *gpio_group;
+    uint16_t gpio_pin;
+
+    /* Pin selection */
     switch (cmd) {
     case LED0:
-        GPIO_WriteBit(GPIOA, GPIO_Pin_2, arg);
+        gpio_group = GPIOA;
+        gpio_pin = GPIO_Pin_2;
         break;
     case LED1:
-        GPIO_WriteBit(GPIOA, GPIO_Pin_0, arg);
+        gpio_group = GPIOA;
+        gpio_pin = GPIO_Pin_0;
         break;
     case LED2:
-        GPIO_WriteBit(GPIOA, GPIO_Pin_3, arg);
+        gpio_group = GPIOA;
+        gpio_pin = GPIO_Pin_3;
+        break;
+    default:
+        return -EINVAL;
+    }
+
+    /* Write new pin state */
+    switch (arg) {
+    case LED_ENABLE:
+    case LED_DISABLE:
+        GPIO_WriteBit(gpio_group, gpio_pin, arg);
+        break;
+    case LED_TOGGLE:
+        GPIO_ToggleBits(gpio_group, gpio_pin);
         break;
     default:
         return -EINVAL;
@@ -56,7 +76,8 @@ static void led_init(void)
         .GPIO_Mode = GPIO_Mode_OUT,
         .GPIO_Speed = GPIO_Speed_50MHz,
         .GPIO_OType = GPIO_OType_PP,
-        .GPIO_PuPd = GPIO_PuPd_DOWN};
+        .GPIO_PuPd = GPIO_PuPd_DOWN,
+    };
 
     GPIO_Init(GPIOA, &GPIO_InitStruct);
 
@@ -70,6 +91,70 @@ void led_write(int fd, int state)
     ioctl(fd, LED2, state);
 }
 
+int gpio_open(struct inode *inode, struct file *file)
+{
+    return 0;
+}
+
+int gpio_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
+{
+    if (arg != GPIO_SET && arg != GPIO_RESET && arg != GPIO_TOGGLE)
+        return -EINVAL;
+
+    GPIO_TypeDef *gpio_group;
+    uint16_t gpio_pin;
+
+    /* Pin selection */
+    switch (cmd) {
+    case GPIO_PIN0:
+        gpio_group = GPIOB;
+        gpio_pin = GPIO_Pin_14;
+        break;
+    default:
+        return -EINVAL;
+    }
+
+    /* Write new pin state */
+    switch (arg) {
+    case LED_ENABLE:
+    case LED_DISABLE:
+        GPIO_WriteBit(gpio_group, gpio_pin, arg);
+        break;
+    case LED_TOGGLE:
+        GPIO_ToggleBits(gpio_group, gpio_pin);
+        break;
+    default:
+        return -EINVAL;
+    }
+
+    return 0;
+}
+
+static struct file_operations gpio_file_ops = {
+    .ioctl = gpio_ioctl,
+    .open = gpio_open,
+};
+
+static void gpio_init(void)
+{
+    /* Register GPIO to the file system */
+    register_chrdev("freq_tester", &gpio_file_ops);
+
+    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);
+
+    GPIO_InitTypeDef GPIO_InitStruct = {
+        .GPIO_Pin = GPIO_Pin_14,
+        .GPIO_Mode = GPIO_Mode_OUT,
+        .GPIO_Speed = GPIO_Speed_50MHz,
+        .GPIO_OType = GPIO_OType_PP,
+        .GPIO_PuPd = GPIO_PuPd_DOWN,
+    };
+
+    GPIO_Init(GPIOB, &GPIO_InitStruct);
+
+    printk("freq_tester: gpio");
+}
+
 void early_write(char *buf, size_t size)
 {
     uart_puts(USART1, buf, size);
@@ -77,11 +162,12 @@ void early_write(char *buf, size_t size)
 
 void __board_init(void)
 {
-    led_init();
-    pwm_init();
     serial1_init(115200, "console", "shell (alias serial0)");
     serial3_init(115200, "dbglink", "debug-link (alias serial1)");
     // uartX_init(115200, "mavlink", "mavlink (alias serialX)"); //TODO
+    led_init();
+    gpio_init();
+    pwm_init();
     sbus_init();
     mpu6500_init();
 }
diff --git a/drivers/boards/stm32f429disc.c b/drivers/boards/stm32f429disc.c
index b8346af..b078cf0 100644
--- a/drivers/boards/stm32f429disc.c
+++ b/drivers/boards/stm32f429disc.c
@@ -39,15 +39,34 @@ int led_open(struct inode *inode, struct file *file)
 
 int led_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
 {
-    if (arg != LED_ENABLE && arg != LED_DISABLE)
+    if (arg != LED_ENABLE && arg != LED_DISABLE && arg != LED_TOGGLE)
         return -EINVAL;
 
+    GPIO_TypeDef *gpio_group;
+    uint16_t gpio_pin;
+
+    /* Pin selection */
     switch (cmd) {
     case LED0:
-        GPIO_WriteBit(GPIOG, GPIO_Pin_13, arg);
+        gpio_group = GPIOG;
+        gpio_pin = GPIO_Pin_13;
         break;
     case LED1:
-        GPIO_WriteBit(GPIOG, GPIO_Pin_14, arg);
+        gpio_group = GPIOG;
+        gpio_pin = GPIO_Pin_14;
+        break;
+    default:
+        return -EINVAL;
+    }
+
+    /* Write new pin state */
+    switch (arg) {
+    case LED_ENABLE:
+    case LED_DISABLE:
+        GPIO_WriteBit(gpio_group, gpio_pin, arg);
+        break;
+    case LED_TOGGLE:
+        GPIO_ToggleBits(gpio_group, gpio_pin);
         break;
     default:
         return -EINVAL;
@@ -113,8 +132,8 @@ void __board_init(void)
 {
     SDRAM_Init();
     lcd_init();
-    led_init();
     serial1_init(115200, "console", "shell (alias: serial0)");
     serial2_init(115200, "mavlink", "mavlink (alias: serial1)");
     serial3_init(115200, "dbglink", "debug-link (alias: serial2)");
+    led_init();
 }
diff --git a/drivers/boards/stm32f4disc.c b/drivers/boards/stm32f4disc.c
index dc11e43..d472614 100644
--- a/drivers/boards/stm32f4disc.c
+++ b/drivers/boards/stm32f4disc.c
@@ -16,21 +16,42 @@ int led_open(struct inode *inode, struct file *file)
 
 int led_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
 {
-    if (arg != LED_ENABLE && arg != LED_DISABLE)
+    if (arg != LED_ENABLE && arg != LED_DISABLE && arg != LED_TOGGLE)
         return -EINVAL;
 
+    GPIO_TypeDef *gpio_group;
+    uint16_t gpio_pin;
+
+    /* Pin selection */
     switch (cmd) {
     case LED0:
-        GPIO_WriteBit(GPIOD, GPIO_Pin_12, arg);
+        gpio_group = GPIOD;
+        gpio_pin = GPIO_Pin_12;
         break;
     case LED1:
-        GPIO_WriteBit(GPIOD, GPIO_Pin_13, arg);
+        gpio_group = GPIOD;
+        gpio_pin = GPIO_Pin_13;
         break;
     case LED2:
-        GPIO_WriteBit(GPIOD, GPIO_Pin_14, arg);
+        gpio_group = GPIOD;
+        gpio_pin = GPIO_Pin_14;
         break;
     case LED3:
-        GPIO_WriteBit(GPIOD, GPIO_Pin_15, arg);
+        gpio_group = GPIOD;
+        gpio_pin = GPIO_Pin_15;
+        break;
+    default:
+        return -EINVAL;
+    }
+
+    /* Write new pin state */
+    switch (arg) {
+    case LED_ENABLE:
+    case LED_DISABLE:
+        GPIO_WriteBit(gpio_group, gpio_pin, arg);
+        break;
+    case LED_TOGGLE:
+        GPIO_ToggleBits(gpio_group, gpio_pin);
         break;
     default:
         return -EINVAL;
@@ -79,8 +100,8 @@ void early_write(char *buf, size_t size)
 
 void __board_init(void)
 {
-    led_init();
     serial1_init(115200, "console", "shell (alias: serial0)");
     serial2_init(115200, "mavlink", "mavlink (alias: serial1)");
     serial3_init(115200, "dbglink", "debug-link (alias: serial2)");
+    led_init();
 }
diff --git a/user/tasks/examples/quadrotor.c b/user/tasks/examples/quadrotor.c
index ab12fce..414ed15 100644
--- a/user/tasks/examples/quadrotor.c
+++ b/user/tasks/examples/quadrotor.c
@@ -17,6 +17,7 @@
 #define LED_R LED0
 #define LED_G LED1
 #define LED_B LED2
+#define FREQ_TESTER GPIO_PIN0
 
 #define deg_to_rad(angle) (angle * M_PI / 180.0)
 #define rad_to_deg(radian) (radian * 180.0 / M_PI)
@@ -241,6 +242,13 @@ void flight_control_task(void)
         exit(1);
     }
 
+    /* Open frequency test pin */
+    int freq_tester_fd = open("/dev/freq_tester", 0);
+    if (freq_tester_fd < 0) {
+        printf("failed to open the frequency tester.\n\r");
+        exit(1);
+    }
+
     /* Open Inertial Measurement Unit (IMU) */
     int accel_fd = open("/dev/accel0", 0);
     int gyro_fd = open("/dev/gyro0", 0);
@@ -349,6 +357,9 @@ void flight_control_task(void)
             ioctl(pwm_fd, SET_PWM_CHANNEL3, motors[2]);
             ioctl(pwm_fd, SET_PWM_CHANNEL4, motors[3]);
         }
+
+        /* Frequency testing */
+        ioctl(freq_tester_fd, FREQ_TESTER, GPIO_TOGGLE);
     }
 }