diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
index 6e1f87b653..3e37fec74b 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
@@ -284,7 +284,9 @@ AP_InertialSensor::AP_InertialSensor() :
     _primary_accel(0),
     _hil_mode(false),
     _have_3D_calibration(false),
-    _calibrating(false)
+    _calibrating(false),
+    _log_raw_data(false),
+    _dataflash(NULL)
 {
     AP_Param::setup_object_defaults(this, var_info);        
     for (uint8_t i=0; i<INS_MAX_BACKENDS; i++) {
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h
index e5e17985f9..63b99eb20a 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.h
@@ -27,6 +27,12 @@
 
 class AP_InertialSensor_Backend;
 
+/*
+  forward declare DataFlash class. We can't include DataFlash.h
+  because of mutual dependencies
+ */
+class DataFlash_Class;
+
 /* AP_InertialSensor is an abstraction for gyro and accel measurements
  * which are correctly aligned to the body axes and scaled to SI units.
  *
@@ -208,6 +214,12 @@ public:
     // get the accel filter rate in Hz
     uint8_t get_accel_filter_hz(void) const { return _accel_filter_cutoff; }
 
+    // pass in a pointer to DataFlash for raw data logging
+    void set_dataflash(DataFlash_Class *dataflash) { _dataflash = dataflash; }
+
+    // enable/disable raw gyro/accel logging
+    void set_raw_logging(bool enable) { _log_raw_data = enable; }
+
 private:
 
     // load backend drivers
@@ -299,6 +311,9 @@ private:
     // are gyros or accels currently being calibrated
     bool _calibrating:1;
 
+    // should we log raw accel/gyro data?
+    bool _log_raw_data:1;
+
     // the delta time in seconds for the last sample
     float _delta_time;
 
@@ -317,6 +332,8 @@ private:
 
     uint32_t _accel_error_count[INS_MAX_INSTANCES];
     uint32_t _gyro_error_count[INS_MAX_INSTANCES];
+
+    DataFlash_Class *_dataflash;
 };
 
 #include "AP_InertialSensor_Backend.h"
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
index 60aa4d17ae..4a22654009 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
@@ -94,6 +94,11 @@ protected:
     // return the requested sample rate in Hz
     uint16_t get_sample_rate_hz(void) const;
 
+    // access to frontend dataflash
+    DataFlash_Class *get_dataflash(void) const { 
+        return _imu._log_raw_data? _imu._dataflash : NULL; 
+    }
+
     // note that each backend is also expected to have a static detect()
     // function which instantiates an instance of the backend sensor
     // driver if the sensor is available
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
index df0ca1fdfa..efeb80bed8 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
@@ -7,6 +7,8 @@
 
 const extern AP_HAL::HAL& hal;
 
+#include <DataFlash.h>
+
 #include <sys/types.h>
 #include <sys/stat.h>
 #include <fcntl.h>
@@ -389,6 +391,18 @@ bool AP_InertialSensor_PX4::_get_accel_sample(uint8_t i, struct accel_report &ac
         _accel_fd[i] != -1 && 
         ::read(_accel_fd[i], &accel_report, sizeof(accel_report)) == sizeof(accel_report) && 
         accel_report.timestamp != _last_accel_timestamp[i]) {
+        DataFlash_Class *dataflash = get_dataflash();
+        if (dataflash != NULL) {
+            struct log_ACCEL pkt = {
+                LOG_PACKET_HEADER_INIT((uint8_t)(LOG_ACC1_MSG+i)),
+                timestamp    : (uint32_t)(accel_report.timestamp/1000),
+                timestamp_us : (uint32_t)accel_report.timestamp,
+                AccX      : accel_report.x,
+                AccY      : accel_report.y,
+                AccZ      : accel_report.z
+            };
+            dataflash->WriteBlock(&pkt, sizeof(pkt));
+        }
         return true;
     }
     return false;
@@ -400,6 +414,18 @@ bool AP_InertialSensor_PX4::_get_gyro_sample(uint8_t i, struct gyro_report &gyro
         _gyro_fd[i] != -1 && 
         ::read(_gyro_fd[i], &gyro_report, sizeof(gyro_report)) == sizeof(gyro_report) && 
         gyro_report.timestamp != _last_gyro_timestamp[i]) {
+        DataFlash_Class *dataflash = get_dataflash();
+        if (dataflash != NULL) {
+            struct log_GYRO pkt = {
+                LOG_PACKET_HEADER_INIT((uint8_t)(LOG_GYR1_MSG+i)),
+                timestamp    : (uint32_t)(gyro_report.timestamp/1000),
+                timestamp_us : (uint32_t)gyro_report.timestamp,
+                GyrX      : gyro_report.x,
+                GyrY      : gyro_report.y,
+                GyrZ      : gyro_report.z
+            };
+            dataflash->WriteBlock(&pkt, sizeof(pkt));
+        }
         return true;
     }
     return false;