[vms_ecu_demo] Added debugging message

conf/modules/vms_ecu_demo.xml

@@ -31,6 +31,7 @@
   </header>
   <init fun="vms_ecu_demo_init()"/>
   <periodic fun="vms_ecu_demo_periodic()" freq="10."/>
+  <periodic fun="vms_ecu_demo_downlink()" freq="4." autorun="TRUE"/>
   <makefile>
     <file name="vms_ecu_demo.c"/>
     <define name="USE_DAC1" value="1" />

sw/airborne/modules/fsae_electric/vms_ecu_demo.c

@@ -27,6 +27,11 @@
  * for more details
  */
 #include "modules/fsae_electric/vms_ecu_demo.h"
+
+// Messages
+#include "pprzlink/messages.h"
+#include "subsystems/datalink/downlink.h"
+
 #include "mcu_periph/gpio.h"
 #include "ch.h" // for DAC
 #include "hal.h" // for DAC
@@ -72,13 +77,13 @@ uint16_t dac_1;
 uint16_t dac_2;
 
 static const DACConfig dac1cfg1 = {
-  .init         = 2047U,
-  .datamode     = DAC_DHRM_12BIT_RIGHT
+    .init         = 2047U,
+    .datamode     = DAC_DHRM_12BIT_RIGHT
 };
 
 static const DACConfig dac1cfg2 = {
-  .init         = 0U,
-  .datamode     = DAC_DHRM_12BIT_RIGHT
+    .init         = 0U,
+    .datamode     = DAC_DHRM_12BIT_RIGHT
 };
 
 
@@ -117,18 +122,18 @@ static const struct can_instance can2 = {&CAND2, 12};
  * See section 22.7.7 on the STM32 reference manual.
  */
 static const CANConfig cancfg_lb = {
-  CAN_MCR_ABOM | CAN_MCR_AWUM | CAN_MCR_TXFP,
-  CAN_BTR_LBKM | CAN_BTR_SJW(0) | CAN_BTR_TS2(1) |
-  CAN_BTR_TS1(8) | CAN_BTR_BRP(6)
+    CAN_MCR_ABOM | CAN_MCR_AWUM | CAN_MCR_TXFP,
+    CAN_BTR_LBKM | CAN_BTR_SJW(0) | CAN_BTR_TS2(1) |
+    CAN_BTR_TS1(8) | CAN_BTR_BRP(6)
 };
 
 /*
  * Normal mode, see if we can ping each other
  */
 static const CANConfig cancfg = {
-  CAN_MCR_ABOM | CAN_MCR_AWUM | CAN_MCR_TXFP,
-  CAN_BTR_SJW(0) | CAN_BTR_TS2(1) |
-  CAN_BTR_TS1(8) | CAN_BTR_BRP(6)
+    CAN_MCR_ABOM | CAN_MCR_AWUM | CAN_MCR_TXFP,
+    CAN_BTR_SJW(0) | CAN_BTR_TS2(1) |
+    CAN_BTR_TS1(8) | CAN_BTR_BRP(6)
 };
 
 /*
@@ -148,7 +153,7 @@ static THD_FUNCTION(can_rx, p) {
     if (chEvtWaitAnyTimeout(ALL_EVENTS, MS2ST(100)) == 0)
       continue;
     while (canReceive(cip->canp, CAN_ANY_MAILBOX,
-                      &rxmsg, TIME_IMMEDIATE) == MSG_OK) {
+        &rxmsg, TIME_IMMEDIATE) == MSG_OK) {
       // Process message.
       palTogglePad(GPIOD, cip->led);
     }
@@ -182,20 +187,6 @@ static THD_FUNCTION(can_tx, p) {
 }
 
 
-#if PERIODIC_TELEMETRY
-#include "subsystems/datalink/telemetry.h"
-static void send_ecu(struct transport_tx *trans, struct link_device *dev)
-{
-  pprz_msg_send_ECU(trans, dev, AC_ID,
-        &stg_in,
-        &stb_in,
-        &ain_1,
-        &ain_2,
-        &ain_3,
-        &ain_4);
-}
-#endif
-
 void vms_ecu_demo_init(void)
 {
   // Digital
@@ -232,21 +223,17 @@ void vms_ecu_demo_init(void)
    * Starting the transmitter and receiver threads.
    */
   chThdCreateStatic(can_rx1_wa, sizeof(can_rx1_wa), NORMALPRIO + 7,
-                    can_rx, (void *)&can1);
+      can_rx, (void *)&can1);
   chThdCreateStatic(can_rx2_wa, sizeof(can_rx2_wa), NORMALPRIO + 7,
-                    can_rx, (void *)&can2);
+      can_rx, (void *)&can2);
   chThdCreateStatic(can_tx_wa, sizeof(can_tx_wa), NORMALPRIO + 7,
-                    can_tx, NULL);
-
-#if PERIODIC_TELEMETRY
-  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ECU, send_ecu);
-#endif
+      can_tx, NULL);
 
   //DAC
   /*
-     * Starting DAC1 driver, setting up the output pin as analog as suggested
-     * by the Reference Manual.
-     */
+   * Starting DAC1 driver, setting up the output pin as analog as suggested
+   * by the Reference Manual.
+   */
   palSetPadMode(GPIOA, 4, PAL_MODE_INPUT_ANALOG);
   palSetPadMode(GPIOA, 5, PAL_MODE_INPUT_ANALOG);
   dacStart(&DACD1, &dac1cfg1);
@@ -311,6 +298,17 @@ void vms_ecu_demo_periodic(void)
 }
 
 
+void vms_ecu_demo_downlink(void) {
+  DOWNLINK_SEND_ECU(DefaultChannel, DefaultDevice,
+      &stg_in,
+      &stb_in,
+      &ain_1,
+      &ain_2,
+      &ain_3,
+      &ain_4);
+}
+
+
 void vms_ecu_demo_UpdateDac1(uint16_t val) {
   dac_1 = val;
   dac_ref1 = dac_1;

sw/airborne/modules/fsae_electric/vms_ecu_demo.h

@@ -45,6 +45,7 @@ extern bool rtds;
 
 void vms_ecu_demo_init(void);
 void vms_ecu_demo_periodic(void);
+void vms_ecu_demo_downlink(void);
 /** Reset sweep number */
 extern void vms_ecu_demo_UpdateDac1(uint16_t val);
 extern void vms_ecu_demo_UpdateDac2(uint16_t val);