diff --git a/libraries/AP_HAL_PX4/Storage.cpp b/libraries/AP_HAL_PX4/Storage.cpp
index 325c2326f2..c131542201 100644
--- a/libraries/AP_HAL_PX4/Storage.cpp
+++ b/libraries/AP_HAL_PX4/Storage.cpp
@@ -11,6 +11,8 @@
 #include <ctype.h>
 #include <nuttx/progmem.h>
 
+#include <AP_BoardConfig/AP_BoardConfig.h>
+
 #include "Storage.h"
 using namespace PX4;
 
@@ -27,6 +29,8 @@ using namespace PX4;
 
 extern const AP_HAL::HAL& hal;
 
+extern "C" int mtd_main(int, char **);
+
 PX4Storage::PX4Storage(void) :
     _perf_storage(perf_alloc(PC_ELAPSED, "APM_storage")),
     _perf_errors(perf_alloc(PC_COUNT, "APM_storage_errors"))
@@ -193,6 +197,17 @@ void PX4Storage::_mtd_write(uint16_t line)
  */
 void PX4Storage::_mtd_load(void)
 {
+    if (AP_BoardConfig::px4_start_driver(mtd_main, "mtd", "start " MTD_PARAMS_FILE)) {
+        printf("mtd: started OK\n");
+        if (AP_BoardConfig::px4_start_driver(mtd_main, "mtd", "readtest " MTD_PARAMS_FILE)) {
+            printf("mtd: readtest OK\n");
+        } else {
+            AP_BoardConfig::px4_sensor_error("mtd: failed readtest");
+        }
+    } else {
+        AP_BoardConfig::px4_sensor_error("mtd: failed start");
+    }
+
     int fd = open(MTD_PARAMS_FILE, O_RDONLY);
     if (fd == -1) {
         AP_HAL::panic("Failed to open " MTD_PARAMS_FILE);
diff --git a/mk/PX4/ROMFS/init.d/rc.APM b/mk/PX4/ROMFS/init.d/rc.APM
index 86537f830f..75e28e601d 100755
--- a/mk/PX4/ROMFS/init.d/rc.APM
+++ b/mk/PX4/ROMFS/init.d/rc.APM
@@ -82,24 +82,6 @@ then
     mkblctrl -mkmode x -d /dev/pwm_output
 fi
 
-if mtd start /fs/mtd
-then
-    echo "started mtd driver OK"
-else
-    echo "failed to start mtd driver"
-    echo "failed to start mtd driver" >> $logfile
-    sh /etc/init.d/rc.error           
-fi
-
-if mtd readtest /fs/mtd
-then
-    echo "mtd readtest OK"
-else
-    echo "failed to read mtd"
-    echo "failed to read mtd" >> $logfile
-    sh /etc/init.d/rc.error           
-fi
-
 echo Starting ArduPilot
 if ArduPilot start
 then