[test_progs] fixes for cc3d

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>
 

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
 #

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)

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;
+  }
+}

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; }

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;
   }