diff --git a/conf/airframes/test_cc3d.xml b/conf/airframes/test_cc3d.xml
index e26b5f2c86..ef76893d8b 100644
--- a/conf/airframes/test_cc3d.xml
+++ b/conf/airframes/test_cc3d.xml
@@ -40,6 +40,7 @@
     <target name="test_ahrs" board="cc3d">
       <subsystem name="imu" type="mpu6000"/>
       <subsystem name="ahrs" type="int_cmpl_quat"/>
+      <configure name="USE_MAGNETOMETER" value="FALSE"/>
     </target>
   </firmware>
 
diff --git a/conf/firmwares/subsystems/shared/imu_mpu6000.makefile b/conf/firmwares/subsystems/shared/imu_mpu6000.makefile
index 264f62fc4d..6d906a4a74 100644
--- a/conf/firmwares/subsystems/shared/imu_mpu6000.makefile
+++ b/conf/firmwares/subsystems/shared/imu_mpu6000.makefile
@@ -42,6 +42,9 @@ ap.srcs   += $(IMU_SRCS)
 test_imu.CFLAGS += $(IMU_CFLAGS)
 test_imu.srcs   += $(IMU_SRCS)
 
+test_ahrs.CFLAGS += $(IMU_CFLAGS)
+test_ahrs.srcs   += $(IMU_SRCS)
+
 #
 # NPS simulator
 #
diff --git a/conf/firmwares/test_progs.makefile b/conf/firmwares/test_progs.makefile
index f76cccfc28..0a2c927772 100644
--- a/conf/firmwares/test_progs.makefile
+++ b/conf/firmwares/test_progs.makefile
@@ -115,6 +115,9 @@ endif
 ifeq ($(BOARD), navstik)
 LED_DEFINES = -DLED_RED=1 -DLED_GREEN=2
 endif
+ifeq ($(BOARD), cc3d)
+LED_DEFINES = -DLED_BLUE=1
+endif
 LED_DEFINES ?= -DLED_RED=2 -DLED_GREEN=3
 
 test_sys_time_timer.ARCHDIR = $(ARCH)
diff --git a/sw/airborne/test/subsystems/test_ahrs.c b/sw/airborne/test/subsystems/test_ahrs.c
index c1f490c932..55600e2f5f 100644
--- a/sw/airborne/test/subsystems/test_ahrs.c
+++ b/sw/airborne/test/subsystems/test_ahrs.c
@@ -21,6 +21,8 @@
 
 #include <inttypes.h>
 
+#define DATALINK_C
+
 /* PERIODIC_C_MAIN is defined before generated/periodic_telemetry.h
  * in order to implement telemetry_mode_Main_*
  */
@@ -40,6 +42,9 @@
 #include "subsystems/datalink/downlink.h"
 #include "subsystems/datalink/telemetry.h"
 
+#include "subsystems/datalink/datalink.h"
+#include "generated/settings.h"
+
 #include "subsystems/imu.h"
 #include "subsystems/ahrs.h"
 #include "subsystems/ahrs/ahrs_aligner.h"
@@ -94,6 +99,7 @@ static inline void main_periodic_task(void)
 static inline void main_event_task(void)
 {
   mcu_event();
+  DatalinkEvent();
   ImuEvent(on_gyro_event, on_accel_event, on_mag_event);
 }
 
@@ -141,3 +147,33 @@ static inline void main_report(void)
 
   periodic_telemetry_send_Main(DefaultPeriodic, &(DefaultChannel).trans_tx, &(DefaultDevice).device);
 }
+
+void dl_parse_msg(void)
+{
+  datalink_time = 0;
+  uint8_t msg_id = dl_buffer[1];
+  switch (msg_id) {
+
+    case  DL_PING: {
+      DOWNLINK_SEND_PONG(DefaultChannel, DefaultDevice);
+    }
+    break;
+    case DL_SETTING:
+      if (DL_SETTING_ac_id(dl_buffer) == AC_ID) {
+        uint8_t i = DL_SETTING_index(dl_buffer);
+        float val = DL_SETTING_value(dl_buffer);
+        DlSetting(i, val);
+        DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
+      }
+      break;
+    case DL_GET_SETTING : {
+      if (DL_GET_SETTING_ac_id(dl_buffer) != AC_ID) { break; }
+      uint8_t i = DL_GET_SETTING_index(dl_buffer);
+      float val = settings_get_value(i);
+      DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
+    }
+    break;
+    default:
+      break;
+  }
+}
diff --git a/sw/airborne/test/subsystems/test_imu.c b/sw/airborne/test/subsystems/test_imu.c
index f319e3f05c..3b4baaa692 100644
--- a/sw/airborne/test/subsystems/test_imu.c
+++ b/sw/airborne/test/subsystems/test_imu.c
@@ -124,7 +124,9 @@ static inline void on_accel_event(void)
 {
   imu_scale_accel(&imu);
 
+#if USE_LED_3
   RunOnceEvery(50, LED_TOGGLE(3));
+#endif
   static uint8_t cnt;
   cnt++;
   if (cnt > 15) { cnt = 0; }
@@ -145,7 +147,9 @@ static inline void on_gyro_event(void)
 {
   imu_scale_gyro(&imu);
 
+#if USE_LED_2
   RunOnceEvery(50, LED_TOGGLE(2));
+#endif
   static uint8_t cnt;
   cnt++;
   if (cnt > 15) { cnt = 0; }
diff --git a/sw/airborne/test/subsystems/test_settings.c b/sw/airborne/test/subsystems/test_settings.c
index 4c8062be69..d8fcbce30a 100644
--- a/sw/airborne/test/subsystems/test_settings.c
+++ b/sw/airborne/test/subsystems/test_settings.c
@@ -110,6 +110,13 @@ void dl_parse_msg(void)
         DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
       }
       break;
+    case DL_GET_SETTING : {
+      if (DL_GET_SETTING_ac_id(dl_buffer) != AC_ID) { break; }
+      uint8_t i = DL_GET_SETTING_index(dl_buffer);
+      float val = settings_get_value(i);
+      DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
+    }
+    break;
     default:
       break;
   }