From 03b600d4c8d1c851a6488a745d017e53d8fff2ba Mon Sep 17 00:00:00 2001
From: Fisch <interfisch@gmx.net>
Date: Wed, 5 Jun 2019 14:50:42 +0200
Subject: [PATCH] add voltage and current measurement

---
 controller.ino | 26 ++++++++++++++++++++++++++
 1 file changed, 26 insertions(+)

diff --git a/controller.ino b/controller.ino
index 7690efa..9d71992 100644
--- a/controller.ino
+++ b/controller.ino
@@ -14,6 +14,17 @@ uint8_t imu_no_change_counter = 0;
 
 #define PIN_LED PC13
 
+#define PIN_VBAT PA0 //battery voltage after voltage divider
+#define VBAT_DIV_FACTOR 0.010700 //how much voltage (V) equals one adc unit.  measured at 40V and averaged
+#define PIN_CURRENT PA1 //output of hall sensor for current measurement
+#define CURRENT_OFFSET 2034 //adc reading at 0A, with CJMCU-758 typically at Vcc/2
+#define CURRENT_FACTOR 0.4320376 //how much current (A) equals one adc unit
+double vbat=0; //battery voltage
+double ibat=0; //battery current
+long last_uiupdated=0;
+#define UI_UPDATEPERIOD 10 //in ms
+
+
 #define SENDPERIOD 20 //ms
 
 #define CONTROLUPDATEPERIOD 10
@@ -103,6 +114,10 @@ void setup() {
   pinMode(PIN_LED, OUTPUT);
   digitalWrite(PIN_LED, HIGH);
 
+  
+  pinMode(PIN_VBAT,INPUT_ANALOG);
+  pinMode(PIN_CURRENT,INPUT_ANALOG);
+
 
   Serial.println("Initializing nrf24");
 
@@ -134,6 +149,17 @@ void loop() {
     updateIMU();
     last_imuupdated = millis();
   }
+ 
+  if (millis() - last_uiupdated > UI_UPDATEPERIOD) { //update current and voltage
+    vbat=analogRead(PIN_VBAT)*VBAT_DIV_FACTOR;
+    ibat=(analogRead(PIN_CURRENT)-CURRENT_OFFSET)*CURRENT_FACTOR;
+    last_uiupdated = millis();
+    /*
+    Serial.print("vbat=");
+    Serial.print(vbat);
+    Serial.print(", ibat=");
+    Serial.println(ibat);*/
+  }
 
   //NRF24
   nrf_delay = millis() - last_nrfreceive; //update nrf delay