Merge branch 'master' of https://twinetfs.visualstudio.com/Tango/_git/Tango

2 files changed, 65 insertions, 20 deletions

diff --git a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/Head_Card/ADC/Head_ADC.c b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/Head_Card/ADC/Head_ADC.c
index c2e817656..29fa32ea4 100644
--- a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/Head_Card/ADC/Head_ADC.c
+++ b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/Head_Card/ADC/Head_ADC.c
@@ -68,6 +68,67 @@ uint32_t I2C_Head_ADC_Config(uint32_t I2C_Slave_Add,uint32_t channel )
     return Status;
 }
 
+double Calc_Head_Current(uint32_t VsampleInBits, HEAD_ADC_t Head_ADC_ID )
+{
+    double temp, VADC = 0.0, Measurment_mV_mA = 0.0, Vref,  Max_Value;
+    uint32_t MaxBits;
+
+
+    if(Head_ADC_ID <= HEAD_CURRENT_ZONE_12)
+    {
+        Vref = 2.5;
+        MaxBits = 4096;
+        Max_Value = 1.3;
+    }
+    else
+    if(Head_ADC_ID <= HEAD_CURRENT_HTOT_4)
+    {
+        Vref = 5;
+        MaxBits = 4096;
+        Max_Value = 2.1;
+    }
+    else
+    if(Head_ADC_ID == HEAD_CURRENT_MIXER)
+    {
+        Vref = 2.5;
+        MaxBits = 4096;
+        Max_Value = 1.7;
+    }
+    else
+    if(Head_ADC_ID == HEAD_CURRENT_MIXER)
+    {
+        //???????????????????
+    }
+    else
+    if(Head_ADC_ID == HEAD_CURRENT_MIXER)
+    {
+        Vref = 2.5;
+        MaxBits = 4096;
+        Max_Value = 0.5;
+    }
+    else
+    if(Head_ADC_ID == HEAD_CURRENT_MIXER)
+    {
+        Vref = 27.5; //???????????????????
+        MaxBits = 4096;
+        Max_Value = 2.3;
+    }
+
+    //---- VBits -> VADC ----
+
+    //ADC 12 bit -> 4096 -> 2.5V
+
+    temp = VsampleInBits * Vref;
+    VADC =  temp / MaxBits;
+
+    if(VADC > Max_Value)
+        return VADC;
+
+    Measurment_mV_mA = VADC;// Scale 1V/1A
+
+    return Measurment_mV_mA;
+}
+
 double Read_Head_ADC(HEAD_ADC_t Head_ADC_ID)
 {
     uint32_t Status = OK;
@@ -193,25 +254,9 @@ double Read_Head_ADC(HEAD_ADC_t Head_ADC_ID)
         LOG_ERROR(Status, "Head ADC I2C actions failed");
 
     Head_ADC_Bits[Head_ADC_ID] = VsampleInBits;//just for debug
-/*
-    if(Head_ADC_ID <= HEAD_CURRENT_ZONE_12)
-        Head_ADC_Measuring[Head_ADC_ID] = Calc_Head_Current(VsampleInBits, Head_ADC_ID, 1.3, 2500, 4096 );
-    else
-    if(Head_ADC_ID <= HEAD_CURRENT_HTOT_4)
-        Head_ADC_Measuring[Head_ADC_ID] = Calc_Head_Current(VsampleInBits, Head_ADC_ID, 2.1, 5000, 4096 );
-    else
-    if(Head_ADC_ID == HEAD_CURRENT_MIXER)
-        Head_ADC_Measuring[Head_ADC_ID] = Calc_Head_Current(VsampleInBits, Head_ADC_ID, 1.7, 2500, 4096 );
-    else
-    if(Head_ADC_ID == HEAD_CURRENT_MIXER)
-        Head_ADC_Measuring[Head_ADC_ID] = Calc_Head_Current(VsampleInBits, Head_ADC_ID, 2.35, 0, 4.9 ); ???
-    else
-    if(Head_ADC_ID == HEAD_CURRENT_MIXER)
-        Head_ADC_Measuring[Head_ADC_ID] = Calc_Head_Current(VsampleInBits, Head_ADC_ID, 0.5, 2500, 4096 );
-    else
-    if(Head_ADC_ID == HEAD_CURRENT_MIXER)
-        Head_ADC_Measuring[Head_ADC_ID] = Calc_Head_Current(VsampleInBits, Head_ADC_ID, 2.05, 27500, 4096 ); ???
-*/
+
+    Head_ADC_Measuring[Head_ADC_ID] = Calc_Head_Current(VsampleInBits, Head_ADC_ID);
+
    return Head_ADC_Measuring[Head_ADC_ID];
 }
 
diff --git a/Software/Embedded_SW/Embedded/Drivers/Motors/MotorActions.c b/Software/Embedded_SW/Embedded/Drivers/Motors/MotorActions.c
index 0fa76fe8f..d37fd9907 100644
--- a/Software/Embedded_SW/Embedded/Drivers/Motors/MotorActions.c
+++ b/Software/Embedded_SW/Embedded/Drivers/Motors/MotorActions.c
@@ -99,7 +99,7 @@ FPGA_GPI_ENUM     Motor_Id_to_LS_IdUp[NUM_OF_MOTORS] = { // limit switch for nor
       MAX_GPI, //MOTO_LDRIVING = 18,
       GPI_LS_LLOADMOTOR_UP, //MOTO_LLOADING = 19,
       GPI_LS_LPIVOT_UP, //MOTO_LPIVOT1 = 20,
-      GPI_LS_RDANCER_LONG, //MOTO_RDANCER = 21,
+      GPI_LS_RDANCER_UP, //MOTO_RDANCER = 21,
       MAX_GPI, //MOTO_RDRIVING = 22,
       GPI_LS_RLOADRAM_UP, //MOTO_RLOADARM = 23,
       GPI_LS_RLOADMOTOR_UP, //MOTO_RLOADING = 24,