diff --git a/libraries/tests/mbed/i2c_MMA8451Q/main.cpp b/libraries/tests/mbed/i2c_MMA8451Q/main.cpp
index 476052e3dd..1ccc4dcda3 100644
--- a/libraries/tests/mbed/i2c_MMA8451Q/main.cpp
+++ b/libraries/tests/mbed/i2c_MMA8451Q/main.cpp
@@ -1,28 +1,46 @@
 #include "mbed.h"
 #include "MMA8451Q.h"
-
-#define MMA8451_I2C_ADDRESS (0x1d<<1)
+#include "test_env.h"
 
 #ifdef TARGET_KL05Z
-	#define SDA		PTB4
-	#define SCL		PTB3
+#define SDA     PTB4
+#define SCL     PTB3
 #else
-	#define SDA		PTE25
-	#define SCL		PTE24
+#define SDA     PTE25
+#define SCL     PTE24
 #endif
 
+namespace {
+    const int MMA8451_I2C_ADDRESS = 0x1D << 1;          // I2C bus address
+    const float MMA8451_DIGITAL_SENSITIVITY = 4096.0;   // Counts/g
+}
+
+float calc_3d_vector_len(float x, float y, float z) {
+    return sqrt(x*x + y*y + z*z);
+}
+
+#define TEST_ITERATIONS                 25
+#define TEST_ITERATIONS_SKIP            5
+#define MEASURE_DEVIATION_TOLERANCE     0.025   // 2.5%
+
 int main(void) {
     DigitalOut led(LED_GREEN);
     MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS);
-    printf("WHO AM I: 0x%2X\r\n", acc.getWhoAmI());
+    bool result = true;
+    printf("WHO AM I: 0x%2X\r\n\n", acc.getWhoAmI());
 
-    while (true) {
-        printf("-----------\r\n");
-        printf("acc_x: %d\r\n", acc.getAccX());
-        printf("acc_y: %d\r\n", acc.getAccY());
-        printf("acc_z: %d\r\n", acc.getAccZ());
-
-        wait(1);
+    for (int i = 0; i < TEST_ITERATIONS; i++) {
+        if (i < TEST_ITERATIONS_SKIP) {
+            // Skip first 5 measurements
+            continue;
+        }
+        const float g_vect_len = calc_3d_vector_len(acc.getAccX(), acc.getAccY(), acc.getAccZ()) / MMA8451_DIGITAL_SENSITIVITY;
+        const float deviation = fabs(g_vect_len - 1.0);
+        const char *succes_str = deviation <= MEASURE_DEVIATION_TOLERANCE ? "[OK]" : "[FAIL]";
+        result = result && (deviation <= MEASURE_DEVIATION_TOLERANCE);
+        printf("X:% 6d Y:% 6d Z:% 5d GF:%0.3fg, dev:%0.3f ... %s\r\n", acc.getAccX(), acc.getAccY(), acc.getAccZ(), g_vect_len, deviation, succes_str);
+        wait(0.5);
         led = !led;
     }
+    notify_completion(result);
 }
diff --git a/workspace_tools/tests.py b/workspace_tools/tests.py
index 3b13ea9b2e..fab2cc0530 100644
--- a/workspace_tools/tests.py
+++ b/workspace_tools/tests.py
@@ -764,7 +764,9 @@ TESTS = [
         "source_dir": join(TEST_DIR, "mbed", "i2c_MMA8451Q"),
         "dependencies": [MBED_LIBRARIES, TEST_MBED_LIB, join(PERIPHERALS, 'MMA8451Q')],
         "mcu": ["KL25Z", "KL05Z", "KL46Z"],
-    },
+        "automated": True,
+        "duration": 15,
+        },
 
     # Examples
     {