diff --git a/libraries/AP_HAL/GPIO.h b/libraries/AP_HAL/GPIO.h
index 02c0c09861..9be80c9958 100644
--- a/libraries/AP_HAL/GPIO.h
+++ b/libraries/AP_HAL/GPIO.h
@@ -37,6 +37,9 @@ public:
     /* Interrupt interface: */
     virtual bool    attach_interrupt(uint8_t interrupt_num, AP_HAL::Proc p,
             uint8_t mode) = 0;
+
+    /* return true if USB cable is connected */
+    virtual bool    usb_connected(void) = 0;
 };
 
 #endif // __AP_HAL_GPIO_H__
diff --git a/libraries/AP_HAL_AVR/GPIO.cpp b/libraries/AP_HAL_AVR/GPIO.cpp
index a7189e41ef..687d34a3af 100644
--- a/libraries/AP_HAL_AVR/GPIO.cpp
+++ b/libraries/AP_HAL_AVR/GPIO.cpp
@@ -202,5 +202,18 @@ void AVRDigitalSource::toggle() {
     SREG = oldSREG;
 }
 
+/*
+  return true when USB is connected
+ */
+bool AVRGPIO::usb_connected(void)
+{
+#if HAL_GPIO_USB_MUX_PIN != -1
+    pinMode(HAL_GPIO_USB_MUX_PIN, GPIO_INPUT);
+    return !read(HAL_GPIO_USB_MUX_PIN);
+#else
+    return false;
+#endif
+}
+
 
 #endif
diff --git a/libraries/AP_HAL_AVR/GPIO.h b/libraries/AP_HAL_AVR/GPIO.h
index ffe89f91c3..9e8a5628b3 100644
--- a/libraries/AP_HAL_AVR/GPIO.h
+++ b/libraries/AP_HAL_AVR/GPIO.h
@@ -11,12 +11,14 @@
  # define HAL_GPIO_C_LED_PIN        35
  # define HAL_GPIO_LED_ON           HIGH
  # define HAL_GPIO_LED_OFF          LOW
+ # define HAL_GPIO_USB_MUX_PIN	    -1
 #elif CONFIG_HAL_BOARD == HAL_BOARD_APM2
  # define HAL_GPIO_A_LED_PIN        27
  # define HAL_GPIO_B_LED_PIN        26
  # define HAL_GPIO_C_LED_PIN        25
  # define HAL_GPIO_LED_ON           LOW
  # define HAL_GPIO_LED_OFF          HIGH
+ # define HAL_GPIO_USB_MUX_PIN	    23
 #endif
 
 class AP_HAL_AVR::AVRDigitalSource : public AP_HAL::DigitalSource {
@@ -44,6 +46,10 @@ public:
     AP_HAL::DigitalSource* channel(uint16_t);
     bool    attach_interrupt(uint8_t interrupt_num, AP_HAL::Proc proc,
             uint8_t mode);
+
+    /* return true if USB cable is connected */
+    bool    usb_connected(void);
+
 /* private-ish: only to be used from the appropriate interrupt */
     static AP_HAL::Proc _interrupt_6;
 };
diff --git a/libraries/AP_HAL_Empty/GPIO.cpp b/libraries/AP_HAL_Empty/GPIO.cpp
index 820c5c5701..747aacf82d 100644
--- a/libraries/AP_HAL_Empty/GPIO.cpp
+++ b/libraries/AP_HAL_Empty/GPIO.cpp
@@ -39,6 +39,10 @@ bool EmptyGPIO::attach_interrupt(uint8_t interrupt_num, AP_HAL::Proc p,
     return true;
 }
 
+bool EmptyGPIO::usb_connected(void)
+{
+    return false;
+}
 
 EmptyDigitalSource::EmptyDigitalSource(uint8_t v) :
     _v(v)
diff --git a/libraries/AP_HAL_Empty/GPIO.h b/libraries/AP_HAL_Empty/GPIO.h
index 1cfa82fd8f..0b3875a65d 100644
--- a/libraries/AP_HAL_Empty/GPIO.h
+++ b/libraries/AP_HAL_Empty/GPIO.h
@@ -21,6 +21,8 @@ public:
     bool    attach_interrupt(uint8_t interrupt_num, AP_HAL::Proc p,
             uint8_t mode);
 
+    /* return true if USB cable is connected */
+    bool    usb_connected(void);
 };
 
 class Empty::EmptyDigitalSource : public AP_HAL::DigitalSource {
diff --git a/libraries/AP_HAL_PX4/GPIO.cpp b/libraries/AP_HAL_PX4/GPIO.cpp
index 6d7f7df1ab..b44d03f45c 100644
--- a/libraries/AP_HAL_PX4/GPIO.cpp
+++ b/libraries/AP_HAL_PX4/GPIO.cpp
@@ -16,6 +16,8 @@
 #include <drivers/drv_tone_alarm.h>
 #include <drivers/drv_gpio.h>
 #include <modules/px4iofirmware/protocol.h>
+#include <arch/board/board.h>
+#include <board_config.h>
 
 #define LOW     0
 #define HIGH    1
@@ -203,6 +205,14 @@ bool PX4GPIO::attach_interrupt(uint8_t interrupt_num, AP_HAL::Proc p, uint8_t mo
     return true;
 }
 
+/*
+  return true when USB connected
+ */
+bool PX4GPIO::usb_connected(void)
+{
+    return stm32_gpioread(GPIO_OTGFS_VBUS);
+}
+
 
 PX4DigitalSource::PX4DigitalSource(uint8_t v) :
     _v(v)
diff --git a/libraries/AP_HAL_PX4/GPIO.h b/libraries/AP_HAL_PX4/GPIO.h
index a11282b03f..6f4bde18c9 100644
--- a/libraries/AP_HAL_PX4/GPIO.h
+++ b/libraries/AP_HAL_PX4/GPIO.h
@@ -37,6 +37,9 @@ public:
     /* Interrupt interface: */
     bool attach_interrupt(uint8_t interrupt_num, AP_HAL::Proc p, uint8_t mode);
 
+    /* return true if USB cable is connected */
+    bool usb_connected(void);
+
 private:
     int _led_fd;
     int _tone_alarm_fd;
diff --git a/libraries/AP_HAL_SMACCM/GPIO.cpp b/libraries/AP_HAL_SMACCM/GPIO.cpp
index ba3283aac7..f8bd05cec7 100644
--- a/libraries/AP_HAL_SMACCM/GPIO.cpp
+++ b/libraries/AP_HAL_SMACCM/GPIO.cpp
@@ -33,6 +33,11 @@ void SMACCMGPIO::toggle(uint8_t pin)
 {
 }
 
+bool SMACCMGPIO::usb_connected(void)
+{
+    return false;
+}
+
 /* Alternative interface: */
 AP_HAL::DigitalSource* SMACCMGPIO::channel(uint16_t n)
 {
@@ -68,4 +73,4 @@ void SMACCMDigitalSource::write(uint8_t value)
 void SMACCMDigitalSource::toggle()
 {
   _v = !_v;
-}
\ No newline at end of file
+}
diff --git a/libraries/AP_HAL_SMACCM/GPIO.h b/libraries/AP_HAL_SMACCM/GPIO.h
index a4163717d9..3033d06ad0 100644
--- a/libraries/AP_HAL_SMACCM/GPIO.h
+++ b/libraries/AP_HAL_SMACCM/GPIO.h
@@ -30,6 +30,8 @@ public:
     bool    attach_interrupt(uint8_t interrupt_num, AP_HAL::Proc p,
             uint8_t mode);
 
+    /* return true if USB cable is connected */
+    bool    usb_connected(void);
 };
 
 class SMACCM::SMACCMDigitalSource : public AP_HAL::DigitalSource {