diff --git a/conf/modules/vms_ecu_demo.xml b/conf/modules/vms_ecu_demo.xml index 1941cc773f..c97b62912e 100644 --- a/conf/modules/vms_ecu_demo.xml +++ b/conf/modules/vms_ecu_demo.xml @@ -31,6 +31,7 @@ + diff --git a/sw/airborne/modules/fsae_electric/vms_ecu_demo.c b/sw/airborne/modules/fsae_electric/vms_ecu_demo.c index 73bb4e3c16..eab586281a 100644 --- a/sw/airborne/modules/fsae_electric/vms_ecu_demo.c +++ b/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; diff --git a/sw/airborne/modules/fsae_electric/vms_ecu_demo.h b/sw/airborne/modules/fsae_electric/vms_ecu_demo.h index 8e5e008390..f0f9010fcc 100644 --- a/sw/airborne/modules/fsae_electric/vms_ecu_demo.h +++ b/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);