Power UAV-CAN (#3361)

* Power UAV-CAN

* pprzlink

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>
 

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