 }
 
 //--------------------------------------------------------------------------------
+//! \brief   Read and evaluate internal SOC - experimental
+//! \return  report success (boolean)
+//--------------------------------------------------------------------------------
+boolean ReadSOCinternal() {
+  setCAN_Filter(0x2D5);
+  CAN_Timeout.Reset();
+  
+  if(!digitalRead(2)) {    
+    do {    
+    CAN0.readMsgBuf(&rxID, &len, rxBuf); 
+      if (rxID == 0x2D5) {
+        BattDiag.realSOC =  (unsigned int) rxBuf[4] * 256 + (unsigned int) rxBuf[5];
+        return true;
+      }
+    } while (!CAN_Timeout.Expired(false));
+  }
+  return false;
+}
+
+//--------------------------------------------------------------------------------
+//! \brief   Read and evaluate internal SOC - experimental
+//! \return  report success (boolean)
+//--------------------------------------------------------------------------------
+boolean ReadPower() {
+  setCAN_Filter(0x508);
+  CAN_Timeout.Reset();
+  
+  if(!digitalRead(2)) {    
+    do {    
+    CAN0.readMsgBuf(&rxID, &len, rxBuf); 
+      if (rxID == 0x508) {
+        BattDiag.Power =  (unsigned int) (rxBuf[2] & 0x3F) * 256 + (unsigned int) rxBuf[3];
+        /*Serial.println();
+        for (int i = 0; i<8; i++) {
+          Serial.print(rxBuf[i], HEX); Serial.print(" ");
+        }
+        Serial.println(BattDiag.Power_or_Amps,HEX);*/
+        return true;
+      }
+    } while (!CAN_Timeout.Expired(false));
+  }
+  return false;
+}
+
+//--------------------------------------------------------------------------------
 //! \brief   Read and evaluate system High Voltage
 //! \return  report success (boolean)
 //--------------------------------------------------------------------------------

//! \brief   Wait for serial data to be avaiable.
//--------------------------------------------------------------------------------
void WaitforSerial() {
  Serial.println(F("Press a key to start query..."));
  while (!Serial.available()) {}                  // Wait for serial input to start
}


unsigned char len = 0;
unsigned char rxLength = 0;
unsigned char rxBuf[8];
byte rqFC_length = 8;            //!< Interval to send flow control messages (rqFC) 
unsigned char rqFlowControl[8]               = {0x30, 0x08, 0x14, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
unsigned char rqBattTemperatures[8]          = {0x03, 0x22, 0x02, 0x01, 0xFF, 0xFF, 0xFF, 0xFF}; 
unsigned char rqBattModuleTemperatures[8]    = {0x03, 0x22, 0x02, 0x02, 0xFF, 0xFF, 0xFF, 0xFF}; 
unsigned char rqBattADCref[8]                = {0x03, 0x22, 0x02, 0x07, 0xFF, 0xFF, 0xFF, 0xFF};
unsigned char rqBattVolts[8]                 = {0x03, 0x22, 0x02, 0x08, 0xFF, 0xFF, 0xFF, 0xFF};
unsigned char rqBattAmps[8]                  = {0x03, 0x22, 0x02, 0x03, 0xFF, 0xFF, 0xFF, 0xFF};
unsigned char rqBattDate[8]                  = {0x03, 0x22, 0x03, 0x04, 0xFF, 0xFF, 0xFF, 0xFF};
unsigned char rqBattCapInit[8]               = {0x03, 0x22, 0x03, 0x05, 0xFF, 0xFF, 0xFF, 0xFF};
unsigned char rqBattCapLoss[8]               = {0x03, 0x22, 0x03, 0x09, 0xFF, 0xFF, 0xFF, 0xFF};
unsigned char rqBattCapacity[8]              = {0x03, 0x22, 0x03, 0x10, 0xFF, 0xFF, 0xFF, 0xFF};
unsigned char rqBattHVContactorCyclesLeft[8] = {0x03, 0x22, 0x03, 0x0B, 0xFF, 0xFF, 0xFF, 0xFF};
unsigned char rqBattHVContactorMax[8]        = {0x03, 0x22, 0x03, 0x0C, 0xFF, 0xFF, 0xFF, 0xFF};
unsigned char rqBattHVContactorState[8]      = {0x03, 0x22, 0xD0, 0x00, 0xFF, 0xFF, 0xFF, 0xFF};

#define CS     10                //!< chip select pin of MCP2515 CAN-Controller
#define CS_SD  8                 //!< CS for SD card, if you plan to use a logger...
MCP_CAN CAN0(CS);                //!< Set CS pin

BatteryDiag_t BattDiag;
CTimeout CAN_Timeout(15000);     //!< Timeout value for CAN response in millis

//--------------------------------------------------------------------------------
//! \brief   SETUP()
