Feature/publish encoder raw values (#520)

Co-authored-by: Valentina Gaggero <[email protected]>
robotology · Oct 29, 2024 · 3cfdf30 · 3cfdf30
1 parent f82227e
commit 3cfdf30
Show file tree

Hide file tree

Showing 16 changed files with 226 additions and 136 deletions.
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, 9},
-            {2024, Month::Oct, Day::twentyone, 14, 00}
+            {2, 10},
+            {2024, Month::Oct, Day::twentyeight, 16, 00}
         },
         .OStick = 1000*embot::core::time1microsec,
         .OSstacksizeinit = 10*1024,

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,21 +81,21 @@ extern "C" {
 #define EOMTHEEMSAPPLCFG_VERSION_MAJOR          (VERSION_MAJOR_OFFSET+3)
 //  <o> minor           <0-255> 
 //  <o> minor           <0-255>
-#define EOMTHEEMSAPPLCFG_VERSION_MINOR          94
+#define EOMTHEEMSAPPLCFG_VERSION_MINOR          95
 
 //  </h>version
 
 //  <h> build date
 //  <o> year            <2010-2030>
 #define EOMTHEEMSAPPLCFG_BUILDDATE_YEAR         2024
 //  <o> month           <1-12>
-#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH        7
+#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH        10
 //  <o> day             <1-31>
-#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY          4
+#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY          28
 //  <o> hour            <0-23>
-#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR         13
+#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR         16
 //  <o> minute          <0-59>
-#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN          39
+#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN          00
 //  </h>build date
 // </h>Info 
 

diff --git a/emBODY/eBcode/arch-arm/board/ems004/appl/v2/src/eoappservices/EOtheEncoderReader.c b/emBODY/eBcode/arch-arm/board/ems004/appl/v2/src/eoappservices/EOtheEncoderReader.c
@@ -137,6 +137,11 @@
     {
         return eores_NOK_generic;
     }
+
+    extern eOresult_t eo_encoderreader_GetRaw(EOtheEncoderReader *p, uint8_t jomo, eOencoderreader_RawValuesOfJomo_t *rawValuesArray)
+    {
+        return eores_NOK_generic;
+    }
 
 
 
@@ -571,6 +576,28 @@ extern eOresult_t eo_encoderreader_Scale(EOtheEncoderReader* p, uint8_t jomo, eO
     return r;    
 }
 
+extern eOresult_t eo_encoderreader_GetRaw(EOtheEncoderReader *p, uint8_t jomo, eOencoderreader_RawValuesOfJomo_t *rawValuesArray)
+{
+    if(NULL == p)
+    {
+        return(eores_NOK_nullpointer);
+    }
+
+    if(eobool_false == s_eo_theencoderreader.service.active)
+    {   // nothing to do because we dont have it
+        return(eores_OK);
+    }   
+
+    if(jomo >= eOappEncReader_jomos_maxnumberof)
+    {
+        return(eores_NOK_generic);
+    }
+
+    eOresult_t res = eo_appEncReader_GetRawValue(s_eo_theencoderreader.reader, jomo, rawValuesArray); 
+
+    return(res);
+}
+
 // --------------------------------------------------------------------------------------------------------------------
 // - definition of extern hidden functions 
 // --------------------------------------------------------------------------------------------------------------------

diff --git a/emBODY/eBcode/arch-arm/board/ems004/appl/v2/src/eoappservices/EOtheEncoderReader.h b/emBODY/eBcode/arch-arm/board/ems004/appl/v2/src/eoappservices/EOtheEncoderReader.h
@@ -106,6 +106,27 @@ typedef enum
     eo_encreader_pos_any = 14,
     eo_encreader_pos_none = 15   
 } eOencoderreader_Position_t;    
+
+
+// The following enum and structs are the correspondent to the the ones in 
+// embot::app::eth::encoder::experimental in embot_app_eth_Encoder.h
+
+// enum for the maximum number of jomo components
+enum { eOencoderreader_maxnumberofencoderperjomo = 3 };
+
+// raw data for single component, raw value and diagnostic
+typedef struct
+{
+    int32_t val;
+    int32_t diagnInfo;
+} eOencoderreader_RawValueEncoder_t; 
+
+// struct for raw data and validity flag for entire jomo
+typedef struct
+{
+    eOencoderreader_RawValueEncoder_t rawvalues[eOencoderreader_maxnumberofencoderperjomo];
+    bool areValidReadings[eOencoderreader_maxnumberofencoderperjomo];
+} eOencoderreader_RawValuesOfJomo_t;
 
 // - declaration of extern public variables, ...deprecated: better using use _get/_set instead ------------------------
 // empty-section
@@ -143,6 +164,8 @@ extern eOmc_encoder_t eo_encoderreader_GetType(EOtheEncoderReader* p, uint8_t jo
 
 extern eOresult_t eo_encoderreader_Scale(EOtheEncoderReader* p, uint8_t jomo, eOencoderreader_Position_t position, eOencoderreader_Scaler *scaler);
 
+// Correlated to IFreader
+extern eOresult_t eo_encoderreader_GetRaw(EOtheEncoderReader *p, uint8_t jomo, eOencoderreader_RawValuesOfJomo_t *rawValuesArray);
 
 /** @}            
     end of group eo_EOtheEncoderReader

diff --git a/emBODY/eBcode/arch-arm/board/ems004/appl/v2/src/eoappservices/EOtheMotionController.c b/emBODY/eBcode/arch-arm/board/ems004/appl/v2/src/eoappservices/EOtheMotionController.c
@@ -71,7 +71,6 @@
 
 #include "EOtheMotionController_hid.h"
 
-
 #if defined(EOTHESERVICES_disable_theMotionController)
 
     // provide empty implementation, so that we dont need to change the caller of the API
@@ -1620,9 +1619,10 @@ extern void eoprot_fun_INIT_mc_motor_status(const EOnv* nv)
  */
 static eOresult_t s_eo_motioncontrol_updatedPositionsFromEncoders(EOtheMotionController *p)
 {
-
     eOresult_t res = eores_OK;
 
+    eOmc_joint_status_t *jstatus = NULL;
+
     // wait for the encoders for some time
     for (uint8_t i=0; i<30; ++i)
     {
@@ -1662,6 +1662,17 @@ static eOresult_t s_eo_motioncontrol_updatedPositionsFromEncoders(EOtheMotionCon
             res = eores_NOK_generic;
         }
 
+        embot::app::eth::encoder::experimental::RawValuesOfJomo rawValsArray = {};
+
+        embot::app::eth::theEncoderReader::getInstance().GetRaw(i, rawValsArray);
+
+        if(NULL != (jstatus = eo_entities_GetJointStatus(eo_entities_GetHandle(), i)))
+        {
+            jstatus->addinfo.multienc[0] = rawValsArray.rawvalues[0].val;
+            //jstatus->addinfo.multienc[1] = rawValsArray.rawvalues[1].val; for now do not update the raw value for the motor encoder since we are adding to it the raw planned target --> see lines 292..294 in Joint.c
+            jstatus->addinfo.multienc[2] = rawValsArray.rawvalues[0].diagnInfo;
+
+        }
     } 
 
     embot::app::eth::theEncoderReader::getInstance().Diagnostics_Tick();

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,19 +75,19 @@ extern "C" {
 #define EOMTHEEMSAPPLCFG_VERSION_MAJOR          3
 //  <o> minor           <0-255> 
 //  <o> minor           <0-255>
-#define EOMTHEEMSAPPLCFG_VERSION_MINOR          74
+#define EOMTHEEMSAPPLCFG_VERSION_MINOR          75
 
 //  </h>version
 
 //  <h> build date
 //  <o> year            <2010-2030>
 #define EOMTHEEMSAPPLCFG_BUILDDATE_YEAR         2024
 //  <o> month           <1-12>
-#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH        7
+#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH        10
 //  <o> day             <1-31>
-#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY          20
+#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY          28
 //  <o> hour            <0-23>
-#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR         14
+#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR         16
 //  <o> minute          <0-59>
 #define EOMTHEEMSAPPLCFG_BUILDDATE_MIN          0
 //  </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,19 +84,19 @@ extern "C" {
 #define EOMTHEEMSAPPLCFG_VERSION_MAJOR          (VERSION_MAJOR_OFFSET+3)
 //  <o> minor           <0-255> 
 
-#define EOMTHEEMSAPPLCFG_VERSION_MINOR          95
+#define EOMTHEEMSAPPLCFG_VERSION_MINOR          96
 
 //  </h>version
 
 //  <h> build date
 //  <o> year            <2010-2030>
 #define EOMTHEEMSAPPLCFG_BUILDDATE_YEAR         2024
 //  <o> month           <1-12>
-#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH        7
+#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH        10
 //  <o> day             <1-31>
-#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY          20
+#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY          28
 //  <o> hour            <0-23>
-#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR         14
+#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR         16
 //  <o> minute          <0-59>
 #define EOMTHEEMSAPPLCFG_BUILDDATE_MIN          0