From e746e2a12c8f5e04b5b0682b436692649340007c Mon Sep 17 00:00:00 2001
From: Ryan Summers <ryan.summers@vertigo-designs.com>
Date: Thu, 29 Apr 2021 15:55:36 +0200
Subject: [PATCH] Adding WIP telemetry

---
 src/bin/dual-iir.rs  |  7 ++++---
 src/bin/lockin.rs    |  7 ++++---
 src/net/mod.rs       |  2 +-
 src/net/telemetry.rs | 31 ++++++++++++++++++++++++++++---
 4 files changed, 37 insertions(+), 10 deletions(-)

diff --git a/src/bin/dual-iir.rs b/src/bin/dual-iir.rs
index 4ceb04c..f043a40 100644
--- a/src/bin/dual-iir.rs
+++ b/src/bin/dual-iir.rs
@@ -57,7 +57,7 @@ const APP: () = {
         adcs: (Adc0Input, Adc1Input),
         dacs: (Dac0Output, Dac1Output),
         mqtt: MqttInterface<Settings>,
-        telemetry: net::Telemetry,
+        telemetry: net::TelemetryBuffer,
         settings: Settings,
 
         #[init([[[0.; 5]; IIR_CASCADE_LENGTH]; 2])]
@@ -99,7 +99,7 @@ const APP: () = {
             dacs: stabilizer.dacs,
             mqtt,
             digital_inputs: stabilizer.digital_inputs,
-            telemetry: net::Telemetry::default(),
+            telemetry: net::TelemetryBuffer::default(),
             settings: Settings::default(),
         }
     }
@@ -197,7 +197,8 @@ const APP: () = {
         let telemetry =
             c.resources.telemetry.lock(|telemetry| telemetry.clone());
 
-        c.resources.mqtt.publish_telemetry(&telemetry);
+        let gains = c.resources.settings.lock(|settings| settings.afe.clone());
+        c.resources.mqtt.publish_telemetry(&telemetry.to_telemetry(gains[0], gains[1]));
 
         let telemetry_period = c
             .resources
diff --git a/src/bin/lockin.rs b/src/bin/lockin.rs
index 6e0b4c9..13408ff 100644
--- a/src/bin/lockin.rs
+++ b/src/bin/lockin.rs
@@ -66,7 +66,7 @@ const APP: () = {
         dacs: (Dac0Output, Dac1Output),
         mqtt: MqttInterface<Settings>,
         settings: Settings,
-        telemetry: net::Telemetry,
+        telemetry: net::TelemetryBuffer,
         digital_inputs: (DigitalInput0, DigitalInput1),
 
         timestamper: InputStamper,
@@ -123,7 +123,7 @@ const APP: () = {
             mqtt,
             digital_inputs: stabilizer.digital_inputs,
             timestamper: stabilizer.timestamper,
-            telemetry: net::Telemetry::default(),
+            telemetry: net::TelemetryBuffer::default(),
 
             settings,
 
@@ -245,7 +245,8 @@ const APP: () = {
             c.resources.digital_inputs.1.is_high().unwrap(),
         ];
 
-        c.resources.mqtt.publish_telemetry(&telemetry);
+        let gains = c.resources.settings.lock(|settings| settings.afe.clone());
+        c.resources.mqtt.publish_telemetry(&telemetry.to_telemetry(gains[0], gains[1]));
 
         let telemetry_period = c
             .resources
diff --git a/src/net/mod.rs b/src/net/mod.rs
index 6a36652..f6dbe28 100644
--- a/src/net/mod.rs
+++ b/src/net/mod.rs
@@ -15,7 +15,7 @@ use messages::{MqttMessage, SettingsResponse, SettingsResponseCode};
 pub use mqtt_interface::MqttInterface;
 
 mod telemetry;
-pub use telemetry::Telemetry;
+pub use telemetry::{Telemetry, TelemetryBuffer};
 
 /// Potential actions for firmware to take.
 pub enum Action {
diff --git a/src/net/telemetry.rs b/src/net/telemetry.rs
index 5c948d5..f5736a6 100644
--- a/src/net/telemetry.rs
+++ b/src/net/telemetry.rs
@@ -1,13 +1,22 @@
 use serde::Serialize;
 
-#[derive(Serialize, Clone)]
-pub struct Telemetry {
+use crate::hardware::AfeGain;
+
+#[derive(Copy, Clone)]
+pub struct TelemetryBuffer {
     pub latest_samples: [i16; 2],
     pub latest_outputs: [i16; 2],
     pub digital_inputs: [bool; 2],
 }
 
-impl Default for Telemetry {
+#[derive(Serialize)]
+pub struct Telemetry {
+    input_levels: [f32; 2],
+    output_levels: [f32; 2],
+    digital_inputs: [bool; 2]
+}
+
+impl Default for TelemetryBuffer {
     fn default() -> Self {
         Self {
             latest_samples: [0, 0],
@@ -16,3 +25,19 @@ impl Default for Telemetry {
         }
     }
 }
+
+impl TelemetryBuffer {
+    pub fn to_telemetry(self, afe0: AfeGain, afe1: AfeGain) -> Telemetry {
+        let in0_volts = self.latest_samples[0] as f32 / (i16::MAX as f32 * 5.0 * afe0.to_multiplier() as f32) * 4.096;
+        let in1_volts = self.latest_samples[1] as f32 / (i16::MAX as f32 * 5.0 * afe1.to_multiplier() as f32) * 4.096;
+
+        let out0_volts = self.latest_outputs[0] as f32 / (i16::MAX as f32) * 10.24;
+        let out1_volts = self.latest_outputs[1] as f32 / (i16::MAX as f32) * 10.24;
+
+        Telemetry {
+            input_levels: [in0_volts, in1_volts],
+            output_levels: [out0_volts, out1_volts],
+            digital_inputs: self.digital_inputs,
+        }
+    }
+}