From c5316c14fbc29fa98b887ea1fa23aa28bace7d97 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Fri, 27 Sep 2024 15:40:35 +0200
Subject: [PATCH] Power UAV-CAN (#3361)

* Power UAV-CAN

* pprzlink
---
 conf/telemetry/highspeed_rotorcraft.xml    |  1 +
 sw/airborne/modules/sensors/power_uavcan.c | 31 ++++++++++++++++++++++
 sw/ext/pprzlink                            |  2 +-
 3 files changed, 33 insertions(+), 1 deletion(-)

diff --git a/conf/telemetry/highspeed_rotorcraft.xml b/conf/telemetry/highspeed_rotorcraft.xml
index 89cab29068..c20a7f3118 100644
--- a/conf/telemetry/highspeed_rotorcraft.xml
+++ b/conf/telemetry/highspeed_rotorcraft.xml
@@ -326,6 +326,7 @@
       <message name="DEBUG"                    period="0.02"/> <!-- For the parachute module -->
       <message name="WLS_V"                    period="0.002"/>
       <message name="WLS_U"                    period="0.002"/>
+      <message name="POWER_DEVICE"             period="0.12"/>
     </mode>
   </process>
 
diff --git a/sw/airborne/modules/sensors/power_uavcan.c b/sw/airborne/modules/sensors/power_uavcan.c
index c254d956d0..fd2603c3cf 100644
--- a/sw/airborne/modules/sensors/power_uavcan.c
+++ b/sw/airborne/modules/sensors/power_uavcan.c
@@ -26,6 +26,7 @@
 #include "power_uavcan.h"
 #include "uavcan/uavcan.h"
 #include "modules/energy/electrical.h"
+#include "math/pprz_random.h"
 
 /* uavcan EQUIPMENT_ESC_STATUS message definition */
 #define UAVCAN_EQUIPMENT_POWER_BATTERYINFO_ID 1092
@@ -98,6 +99,28 @@ struct uavcan_circuit_battery_t {
 };
 static struct uavcan_circuit_battery_t battery_circuits[] = POWER_UAVCAN_BATTERY_CIRCUITS;
 
+#if PERIODIC_TELEMETRY
+#include "modules/datalink/telemetry.h"
+
+static void power_uavcan_send_power_device(struct transport_tx *trans, struct link_device *dev)
+{
+  static uint8_t idx = 0;
+  // Send the circuit status
+  if(circuits[idx].set) {
+    uint8_t cid = circuits[idx].circuit_id;
+    pprz_msg_send_POWER_DEVICE(trans, dev, AC_ID, &circuits[idx].node_id, &cid, &circuits[idx].current, &circuits[idx].voltage);
+  }
+
+  // Go to the next
+  if (rand_uniform() > 0.02) {
+    idx++;
+  }
+  if(idx >= POWER_UAVCAN_CIRCUITS_MAX || !circuits[idx].set) {
+    idx = 0;
+  }
+}
+#endif /* PERIODIC_TELEMETRY */
+
 
 static void power_uavcan_battery_cb(struct uavcan_iface_t *iface __attribute__((unused)), CanardRxTransfer *transfer)
 {
@@ -250,4 +273,12 @@ void power_uavcan_init(void)
   // Bind uavcan CIRCUIT_STATUS message from EQUIPMENT.POWER
   uavcan_bind(UAVCAN_EQUIPMENT_POWER_CIRCUITSTATUS_ID, UAVCAN_EQUIPMENT_POWER_CIRCUITSTATUS_SIGNATURE, &circuit_uavcan_ev,
               &power_uavcan_circuit_cb);
+
+  // Configure telemetry
+#if PERIODIC_TELEMETRY
+  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_POWER_DEVICE, power_uavcan_send_power_device);
+#endif
+
+  // Initialize Random (for telemetry)
+  init_random();
 }
diff --git a/sw/ext/pprzlink b/sw/ext/pprzlink
index c96e87501e..b48202f7dd 160000
--- a/sw/ext/pprzlink
+++ b/sw/ext/pprzlink
@@ -1 +1 @@
-Subproject commit c96e87501eb0ffe2be11bf32816bc29b5c0e1055
+Subproject commit b48202f7dde543b0770bbf42c1bfaaf587549145