From decb1f3b177767eccc447573d7c5ebc0eb88acc6 Mon Sep 17 00:00:00 2001
From: Jonas Arnold <jonas.arnold@live.de>
Date: Fri, 16 Dec 2022 16:22:06 +0100
Subject: [PATCH] fixed battery voltage display, ignoring firstline when
 parsing response message

---
 .../RobotClientWpf/Views/MainView.xaml        |  2 +-
 ADIS_Csharp/RobotLib/Battery/DevBattery.cs    | 12 ++++++++----
 ADIS_ESP32_Eclipse/main/robo_wrapper.c        | 19 ++++++++++---------
 3 files changed, 19 insertions(+), 14 deletions(-)

diff --git a/ADIS_Csharp/RobotClientWpf/Views/MainView.xaml b/ADIS_Csharp/RobotClientWpf/Views/MainView.xaml
index c0eddde..405d037 100644
--- a/ADIS_Csharp/RobotClientWpf/Views/MainView.xaml
+++ b/ADIS_Csharp/RobotClientWpf/Views/MainView.xaml
@@ -23,7 +23,7 @@
             </GroupBox>
             <GroupBox Header="Robot battery" Margin="10" HorizontalAlignment="Right" Width="180">
                 <StackPanel Orientation="Horizontal">
-                    <TextBox x:Name="tbRoboVoltage" Height="25" Width="150" FontSize="30" IsReadOnly="True" TextWrapping="NoWrap" HorizontalAlignment="Left"/>
+                    <TextBox x:Name="tbRoboVoltage" Height="25" Width="100" FontSize="15" IsReadOnly="True" TextWrapping="NoWrap" HorizontalAlignment="Left">NaN</TextBox>
                     <Label Content="V" Margin="10 0"/>
                 </StackPanel>
             </GroupBox>
diff --git a/ADIS_Csharp/RobotLib/Battery/DevBattery.cs b/ADIS_Csharp/RobotLib/Battery/DevBattery.cs
index 06caaa4..80aa885 100644
--- a/ADIS_Csharp/RobotLib/Battery/DevBattery.cs
+++ b/ADIS_Csharp/RobotLib/Battery/DevBattery.cs
@@ -45,14 +45,18 @@ namespace RobotLib.Battery
         {
             if (fromTopic == TOPIC_ROBO_RESP_BATTERY)
             {
-                var parsedVoltageString = GetValueFromMesage<string>("voltage", message);
-                if (parsedVoltageString == null) parsedVoltageString = "?";
+                var parsedString = GetValueFromMesage<string>("voltage", message);
+                if (parsedString == null) parsedString = "?";
 
                 // example message = "Battery: 1.25 V"
-                var valueUnit = message.Trim().Split(' ');
+                var valueUnit = parsedString.Trim().Split(' ');
                 string voltage = valueUnit[0].Trim();
+                float fVoltage = float.NaN;
 
-                this.Voltage = float.Parse(voltage);
+                if(float.TryParse(voltage, out fVoltage))
+                {
+                    this.Voltage = fVoltage;
+                }
             }
         }
 
diff --git a/ADIS_ESP32_Eclipse/main/robo_wrapper.c b/ADIS_ESP32_Eclipse/main/robo_wrapper.c
index 2062442..2a78d8e 100644
--- a/ADIS_ESP32_Eclipse/main/robo_wrapper.c
+++ b/ADIS_ESP32_Eclipse/main/robo_wrapper.c
@@ -114,16 +114,8 @@ bool Robo_Wrapper_GetBatteryVoltage(unsigned char *voltage){
 }
 
 static bool getValueOfStatusResponse(unsigned char* response, const unsigned char* key, unsigned char* value, size_t valueStringLen){
-	int16_t pos = McuUtility_strFind(response, (unsigned char*)key);
-	unsigned char extractedString[50] = "";
-
-	if(pos == -1){   // error string not found = -1
-		ESP_LOGE(TAG, "Could not find key %s in response.", key);
-		return false;
-	}
-
 	unsigned char *p;
-	p = (unsigned char*)response + pos;
+	p = (unsigned char*)response;
 
 	// skip first line (if the keyword would also appear in the headline, this is an issue)
 	while(*p!='\n'){
@@ -131,6 +123,14 @@ static bool getValueOfStatusResponse(unsigned char* response, const unsigned cha
 	}
 	p+=1; // skip newline
 
+	// find key
+	int16_t pos = McuUtility_strFind(p, (unsigned char*)key);
+	if(pos == -1){   // error string not found = -1
+		ESP_LOGE(TAG, "Could not find key %s in response.", key);
+		return false;
+	}
+	p+=pos;  // move pointer to found key
+
 	// skip until colon
 	while(true){
 		if(*p==':'){
@@ -147,6 +147,7 @@ static bool getValueOfStatusResponse(unsigned char* response, const unsigned cha
 	}
 
 	// extract value
+	unsigned char extractedString[50] = "";
 	uint8_t i = 0;
 	while(true){
 		if(*p == '\n'){