robotology · marcoaccame · Mar 26, 2024 · Feb 7, 2024
diff --git a/emBODY/eBcode/arch-arm/board/amc/application/v2/cfg/theApplication_config.h b/emBODY/eBcode/arch-arm/board/amc/application/v2/cfg/theApplication_config.h
@@ -38,8 +38,8 @@ namespace embot { namespace app { namespace eth {
         .property =
         {
             Process::eApplication,
-            {2, 1},
-            {2024, Month::Mar, Day::fifteen, 12, 00}
+            {2, 2},
+            {2024, Month::Mar, Day::twentyone, 15, 00}
         },
         .OStick = 1000*embot::core::time1microsec,
         .OSstacksizeinit = 10*1024,

diff --git a/emBODY/eBcode/arch-arm/board/bat/BAT_Rev_B/Inc/BAT_B.h b/emBODY/eBcode/arch-arm/board/bat/BAT_Rev_B/Inc/BAT_B.h
@@ -140,5 +140,6 @@ extern adc_measure_t mean;
 extern uint32_t adc_values[9];
 extern fifo adc_samples;
 extern uint32_t vBatterydV;
+extern uint32_t iBatterydA;
 
 #endif
diff --git a/emBODY/eBcode/arch-arm/board/bat/BAT_Rev_B/Src/BAT_B.c b/emBODY/eBcode/arch-arm/board/bat/BAT_Rev_B/Src/BAT_B.c
@@ -11,7 +11,7 @@
 
 char Firmware_vers = 1;
 char Revision_vers = 3;
-char Build_number  = 3;
+char Build_number  = 4;
 
 uint32_t vtol=100;  // voltage tolerance for hysteresis
 uint32_t vhyst=0;    // voltage hysteresis
@@ -108,13 +108,14 @@ uint16_t Battery_low=3300*7;    // 7s5p battery
 #ifdef BAT_B_Generic
 uint32_t VTH[7]={32000, 34000, 36000, 38000, 40000, 42000, 44000};   // threshold in mV iCub 2.5 Battery
 uint16_t Battery_high=4200*10;   // 10s3p battery
-uint16_t Battery_low=3300*10;    // 10s3p battery
+uint16_t Battery_low=3000*10;    // 10s3p battery
 #endif
 
 adc_measure_t adc_measure = {0};  // initialize all adc values to 0
 adc_measure_t mean = {0};         // initialize all average values to 0
 uint32_t adc_values[9];           // contains all ADC channels conversion
-uint32_t vBatterydV = 0;          // varibale used for sending mean.V_BATTERY to EMS in deciVolt
+uint32_t vBatterydV = 0;          // variable used for sending mean.V_BATTERY to EMS in deciVolt
+uint32_t iBatterydA = 0;          // variable used for sending the mean.I_BATTERY_M_B in deciAmpere
 
 uint16_t adc_sample = 0;
 

diff --git a/emBODY/eBcode/arch-arm/board/bat/BAT_Rev_B/Src/Display_uOLED.c b/emBODY/eBcode/arch-arm/board/bat/BAT_Rev_B/Src/Display_uOLED.c
@@ -24,9 +24,6 @@ void Display_uOLED_init(){
 
 
 void Display_uOLED_128_G2(){
-  if(mean.V_BATTERY > Battery_high)       {Battery_charge=100;}
-  else if(mean.V_BATTERY < Battery_low)   {Battery_charge=0;}
-  else                                    {Battery_charge = 100*(mean.V_BATTERY-Battery_low)/(Battery_high-Battery_low);}
 
   //uOLED_Parameters[0] 
   //uOLED_Parameters[1] 

diff --git a/emBODY/eBcode/arch-arm/board/bat/BAT_Rev_B/Src/main.c b/emBODY/eBcode/arch-arm/board/bat/BAT_Rev_B/Src/main.c
@@ -97,6 +97,7 @@ void checkHSM(void);
 void dcdc_management(void);
 void FAULT_CHECK(void);
 void dcdcStatusUpdate(void);
+void evaluateSoCPercentage(void);
 
 /* USER CODE END PFP */
 
@@ -300,9 +301,11 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim){
     #ifdef BAT_B_Generic
       FAULT_CHECK();
     #endif
+    evaluateSoCPercentage();
   }
   if(htim->Instance==TIM16){        // timer 100ms
     toggle_100ms = toggle_100ms^1;
+
     if(TX_ENABLED == 0)
         return;        
     CANBUS();
@@ -606,14 +609,16 @@ void CANBUS(void){
 #endif
     {
         // Battery Pack Info message
-        vBatterydV = 0.01 * (mean.V_BATTERY);
-        TxData_620[0] = vBatterydV & 0xFF;              // b7-b6 Battery pack voltage            
+        iBatterydA = 0.01 * (mean.I_HSM + ((mean.V_V12board * mean.I_V12board) / mean.V_VINPUT) + ((mean.V_V12motor * mean.I_V12motor) / mean.V_VINPUT));
+        vBatterydV = 0.01 * (mean.V_VINPUT);
+
+        TxData_620[0] = vBatterydV & 0xFF;              // b7-b6 Input voltage (either battery pack voltage or power supply voltage - highest between the 2)            
         TxData_620[1] = (vBatterydV >> 8) & 0xFF;
-        TxData_620[2] = 0x00;                           // b5-b4 Instant current
-        TxData_620[3] = 0x00;
+        TxData_620[2] = iBatterydA & 0xFF;              // b5-b4 Total current absorbed by the system
+        TxData_620[3] = (iBatterydA >> 8) & 0xFF;
         TxData_620[4] = Battery_charge & 0xFF;          // b3-b2 State of charge of battery
         TxData_620[5] = 0x00;
-        TxData_620[6] = 0x00;                           // b1-b0 Average Temperature of the batteries
+        TxData_620[6] = 0x00;                           // b1-b0 Ready for Average Temperature of the batteries (Not available for now)
         TxData_620[7] = 0x00;
         if (HAL_CAN_AddTxMessage(&hcan1, &TxHeader_620, TxData_620, &TxMailbox_620) != HAL_OK){
             /* Transmission request Error */
@@ -1182,6 +1187,18 @@ void dcdcStatusUpdate(void)
     }
 }
 
+// -----------------------------------------------------------------------------------------------------------------------------
+// Evaluation of most possible accurate battery charge percentage without overdesigning (going to use parallel 1deg polynomials, i.e. line)
+// -----------------------------------------------------------------------------------------------------------------------------
+void evaluateSoCPercentage()
+{
+  if(mean.V_BATTERY > Battery_high)       {Battery_charge=100;}
+  else if(mean.V_BATTERY < Battery_low)   {Battery_charge=0;}
+  else                                    
+  {
+    Battery_charge = 100*(mean.V_BATTERY-Battery_low)/(Battery_high-Battery_low);
+  }
+}
 
 void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef* hcan1)
 {

diff --git a/emBODY/eBcode/arch-arm/board/ems004/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h b/emBODY/eBcode/arch-arm/board/ems004/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h
@@ -81,7 +81,7 @@ extern "C" {
 #define EOMTHEEMSAPPLCFG_VERSION_MAJOR          (VERSION_MAJOR_OFFSET+3)
 //  <o> minor           <0-255> 
 //  <o> minor           <0-255>
-#define EOMTHEEMSAPPLCFG_VERSION_MINOR          86
+#define EOMTHEEMSAPPLCFG_VERSION_MINOR          87
 
 //  </h>version
 
@@ -91,9 +91,9 @@ extern "C" {
 //  <o> month           <1-12>
 #define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH        3
 //  <o> day             <1-31>
-#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY          15
+#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY          21
 //  <o> hour            <0-23>
-#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR         12
+#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR         15
 //  <o> minute          <0-59>
 #define EOMTHEEMSAPPLCFG_BUILDDATE_MIN          00
 //  </h>build date

diff --git a/...DY/eBcode/arch-arm/board/ems004/appl/v2/src/eoappservices/embot_app_eth_theBATservice.cpp b/...DY/eBcode/arch-arm/board/ems004/appl/v2/src/eoappservices/embot_app_eth_theBATservice.cpp
@@ -90,7 +90,7 @@ struct embot::app::eth::theBATservice::Impl {
   CANmonitor canmonitor{};
   static constexpr CANmonitor::Config defaultcanmonitorconfig{
       {}, // the map is left empty
-      250 * embot::core::time1millisec,
+      1000 * embot::core::time1millisec, // this is the time interval used by the ems to touch the CAN boards that initialize this service. We MUST use 1000 ms since we are using the servicefor both BAT (that streams data at 100ms) and BMS (which instead streams at 1s)
       CANmonitor::Report::ALL,
       10 * embot::core::time1second,
       s_eobj_ownname,
@@ -556,28 +556,30 @@ eOresult_t embot::app::eth::theBATservice::Impl::AcceptCANframe(const canFrameDe
 
     case canFrameDescriptor::Type::info:
     {
-        bat->status.timedvalue.temperature =
-            10 * ((static_cast<uint16_t>(cfd.frame->data[7]) << 8) +
-            static_cast<uint16_t>(cfd.frame->data[6]));
 
+    bat->status.timedvalue.temperature =
+            10 * static_cast<int16_t>((static_cast<uint16_t>(cfd.frame->data[7]) << 8) +
+            static_cast<uint16_t>(cfd.frame->data[6]));
+
     bat->status.timedvalue.charge = 
         static_cast<float32_t>(
             (static_cast<uint16_t>(cfd.frame->data[5]) << 8) +
             static_cast<uint16_t>(cfd.frame->data[4])
         );
-        
+
     int16_t curr = 
         (static_cast<int16_t>(cfd.frame->data[3]) << 8) +
         static_cast<int16_t>(cfd.frame->data[2]);
-
+    
     bat->status.timedvalue.current =
         0.1 * static_cast<float32_t>(curr);
-
+        
     bat->status.timedvalue.voltage =
         0.1 * static_cast<float32_t>(
                   (static_cast<uint16_t>(cfd.frame->data[1]) << 8) +
                   static_cast<uint16_t>(cfd.frame->data[0])
         );
+
     } break;
 
     case canFrameDescriptor::Type::status_bms: 
@@ -597,10 +599,8 @@ eOresult_t embot::app::eth::theBATservice::Impl::AcceptCANframe(const canFrameDe
         If we have an alram, this will persist in the minute after the alarm generation
         even if the alarm re-enter
         **/
-        // using embot methods copy the 8bits of bms_status into the two least significant bytes of the general timedvalue.status
-        embot::core::binary::mask::clear(bat->status.timedvalue.status, static_cast<uint32_t>(0x0000ffff), 0);
-        embot::core::binary::bit::set(bat->status.timedvalue.status, 0);
-        embot::core::binary::mask::set(bat->status.timedvalue.status, static_cast<uint32_t>(cfd.frame->data[0]), 1);
+
+        bat->status.timedvalue.status = static_cast<uint16_t>(cfd.frame->data[0]);
         bat->status.timedvalue.age = embot::core::now();
 
     } break;
@@ -624,12 +624,8 @@ eOresult_t embot::app::eth::theBATservice::Impl::AcceptCANframe(const canFrameDe
         If we have an alram, this will persist in the minute after the alarm generation
         even if the alarm re-enter
         **/
-        // using embot methods copy the 16bits of bms_status into the two most significant bytes of the general timedvalue.status
-        uint16_t bat_status_temp16 = {0x0120};
-        embot::core::binary::mask::clear(bat->status.timedvalue.status, static_cast<uint32_t>(0xffff0000), 0);
 
-        bat_status_temp16 = static_cast<uint16_t>((cfd.frame->data[1] << 8 ) | cfd.frame->data[0]);
-        embot::core::binary::mask::set(bat->status.timedvalue.status, static_cast<uint32_t>(bat_status_temp16), 16);
+        bat->status.timedvalue.status = static_cast<uint16_t>((cfd.frame->data[1] << 8 ) | cfd.frame->data[0]);
         bat->status.timedvalue.age = embot::core::now();
     } break;
 

diff --git a/emBODY/eBcode/arch-arm/board/ems004/appl/v2/src/eoappservices/embot_app_eth_theBATservice.h b/emBODY/eBcode/arch-arm/board/ems004/appl/v2/src/eoappservices/embot_app_eth_theBATservice.h
@@ -95,7 +95,7 @@ class theBATservice {
   static constexpr eOas_battery_status_t defaultBATstatus{
       .timedvalue = {.age = 0,
                      .temperature = 0,
-                     //.status = 0,
+                     .status = 0,
                      .voltage = 0,
                      .current = 0,
                      .charge = 0}};

diff --git a/emBODY/eBcode/arch-arm/board/mc2plus/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h b/emBODY/eBcode/arch-arm/board/mc2plus/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h
@@ -75,7 +75,7 @@ extern "C" {
 #define EOMTHEEMSAPPLCFG_VERSION_MAJOR          3
 //  <o> minor           <0-255> 
 //  <o> minor           <0-255>
-#define EOMTHEEMSAPPLCFG_VERSION_MINOR          68
+#define EOMTHEEMSAPPLCFG_VERSION_MINOR          69
 
 //  </h>version
 
@@ -85,9 +85,9 @@ extern "C" {
 //  <o> month           <1-12>
 #define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH        3
 //  <o> day             <1-31>
-#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY          15
+#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY          21
 //  <o> hour            <0-23>
-#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR         12
+#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR         15
 //  <o> minute          <0-59>
 #define EOMTHEEMSAPPLCFG_BUILDDATE_MIN          00
 //  </h>build date

diff --git a/emBODY/eBcode/arch-arm/board/mc4plus/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h b/emBODY/eBcode/arch-arm/board/mc4plus/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h
@@ -84,7 +84,7 @@ extern "C" {
 #define EOMTHEEMSAPPLCFG_VERSION_MAJOR          (VERSION_MAJOR_OFFSET+3)
 //  <o> minor           <0-255> 
 
-#define EOMTHEEMSAPPLCFG_VERSION_MINOR          89
+#define EOMTHEEMSAPPLCFG_VERSION_MINOR          90
 
 //  </h>version
 
@@ -94,9 +94,9 @@ extern "C" {
 //  <o> month           <1-12>
 #define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH        3
 //  <o> day             <1-31>
-#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY          15
+#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY          21
 //  <o> hour            <0-23>
-#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR         12
+#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR         15
 //  <o> minute          <0-59>
 #define EOMTHEEMSAPPLCFG_BUILDDATE_MIN          00