Update transmission of temperature CAN message on 2foc

robotology · Oct 26, 2023 · 0bc2b91 · 0bc2b91
1 parent f52463e
commit 0bc2b91
Show file tree

Hide file tree

Showing 2 changed files with 53 additions and 38 deletions.
diff --git a/emBODY/eBcode/arch-dspic/board/2foc/appl/2FOC-V3/src/can_icubProto_trasmitter.c b/emBODY/eBcode/arch-dspic/board/2foc/appl/2FOC-V3/src/can_icubProto_trasmitter.c
@@ -133,20 +133,7 @@ extern void CanIcubProtoTrasmitterSendPeriodicData(void)
 
     if (!bequiet)
     {
-        payload.b[0] = gControlMode;
-        payload.b[1] = gEncoderError.bitmask;
-
-        payload.w[1] = (VqRef>>(IKs-VOLT_REF_SHIFT));
-
-        payload.b[4] = SysError.b[0];
-        payload.b[5] = SysError.b[1];
-        payload.b[6] = SysError.b[2];
-        payload.b[7] = SysError.b[3];
-
-        msgid = CAN_ICUBPROTO_STDID_MAKE_TX(ICUBCANPROTO_CLASS_PERIODIC_MOTORCONTROL, canprototransmitter_bid, ICUBCANPROTO_PER_MC_MSG__STATUS);
-
-        ECANSend(msgid, 8, &payload);
-
+        BOOL transmit_addStatus = FALSE;
         if (MotorConfig.has_tsens && isActiveI2CTsens())
         {
             int Tsend = 100 + canprototransmitter_bid*100;
@@ -155,18 +142,37 @@ extern void CanIcubProtoTrasmitterSendPeriodicData(void)
             if (++noflood >= Tsend)
             {
                 noflood = 0;
+                transmit_addStatus = TRUE;
+            }
+        }
+        if(transmit_addStatus)
+        {
+            payload.w[1] = gTemperature;
 
-                payload.w[1] = gTemperature;
+            msgid = CAN_ICUBPROTO_STDID_MAKE_TX(ICUBCANPROTO_CLASS_PERIODIC_MOTORCONTROL, canprototransmitter_bid, ICUBCANPROTO_PER_MC_MSG__ADDITIONAL_STATUS );
+            ECANSend(msgid, 4, &payload);
+        }
+        else
+        {
+            payload.b[0] = gControlMode;
+            payload.b[1] = gEncoderError.bitmask;
 
-                msgid = CAN_ICUBPROTO_STDID_MAKE_TX(ICUBCANPROTO_CLASS_PERIODIC_MOTORCONTROL, canprototransmitter_bid, ICUBCANPROTO_PER_MC_MSG__ADDITIONAL_STATUS );
-                ECANSend(msgid, 4, &payload);
-            }
+            payload.w[1] = (VqRef>>(IKs-VOLT_REF_SHIFT));
+
+            payload.b[4] = SysError.b[0];
+            payload.b[5] = SysError.b[1];
+            payload.b[6] = SysError.b[2];
+            payload.b[7] = SysError.b[3];
+
+            msgid = CAN_ICUBPROTO_STDID_MAKE_TX(ICUBCANPROTO_CLASS_PERIODIC_MOTORCONTROL, canprototransmitter_bid, ICUBCANPROTO_PER_MC_MSG__STATUS);
+
+            ECANSend(msgid, 8, &payload);
         }
+
     }
 }
 
 extern void CanIcubProtoTransmitterUpdateBoardId(unsigned char bid)
 {
     canprototransmitter_bid = bid;
 }
-
diff --git a/emBODY/eBcode/arch-dspic/board/2foc/appl/2FOC-V3_6/src/can_icubProto_trasmitter.c b/emBODY/eBcode/arch-dspic/board/2foc/appl/2FOC-V3_6/src/can_icubProto_trasmitter.c
@@ -106,30 +106,20 @@ extern void CanIcubProtoTrasmitterSendPeriodicData(void)
         if (++noflood >= Tsend)
         {
             noflood = 0;
-            payload.w[0] = gTemperature;
-            payload.w[1] = I2Cerrcode;
-            payload.w[2] = I2Cerrors;
-            payload.w[3] = residual_enc;
+            payload.w[0] = I2Cerrcode;
+            payload.w[1] = I2Cerrors;
+            payload.w[2] = residual_enc;
 
             msgid = CAN_ICUBPROTO_STDID_MAKE_TX(ICUBCANPROTO_CLASS_PERIODIC_MOTORCONTROL, canprototransmitter_bid, ICUBCANPROTO_PER_MC_MSG__DEBUG );
 
             ECANSend(msgid, 8, &payload);
         }
     }
 
+
     if (!bequiet)
     {
-        payload.b[0] = gControlMode;
-        payload.b[1] = gEncoderError.bitmask;
-
-        payload.w[1] = VqFbk;
-
-        payload.dw[1] = SysError.L;
-
-        msgid = CAN_ICUBPROTO_STDID_MAKE_TX(ICUBCANPROTO_CLASS_PERIODIC_MOTORCONTROL, canprototransmitter_bid, ICUBCANPROTO_PER_MC_MSG__STATUS);
-
-        ECANSend(msgid, 8, &payload);
-
+        BOOL transmit_addStatus = FALSE;
         if (MotorConfig.has_tsens && isActiveI2CTsens())
         {
             int Tsend = 100 + canprototransmitter_bid*100;
@@ -138,14 +128,33 @@ extern void CanIcubProtoTrasmitterSendPeriodicData(void)
             if (++noflood >= Tsend)
             {
                 noflood = 0;
+                transmit_addStatus = TRUE;
+            }
+        }
+        if(transmit_addStatus)
+        {
+            payload.w[1] = gTemperature;
 
-                payload.w[1] = gTemperature;
+            msgid = CAN_ICUBPROTO_STDID_MAKE_TX(ICUBCANPROTO_CLASS_PERIODIC_MOTORCONTROL, canprototransmitter_bid, ICUBCANPROTO_PER_MC_MSG__ADDITIONAL_STATUS );
+            ECANSend(msgid, 4, &payload);
+        }
+        else
+        {
+            payload.b[0] = gControlMode;
+            payload.b[1] = gEncoderError.bitmask;
 
-                msgid = CAN_ICUBPROTO_STDID_MAKE_TX(ICUBCANPROTO_CLASS_PERIODIC_MOTORCONTROL, canprototransmitter_bid, ICUBCANPROTO_PER_MC_MSG__ADDITIONAL_STATUS );
-                ECANSend(msgid, 4, &payload);
-            }
+            payload.w[1] = VqFbk;
+
+            payload.dw[1] = SysError.L;
+
+            msgid = CAN_ICUBPROTO_STDID_MAKE_TX(ICUBCANPROTO_CLASS_PERIODIC_MOTORCONTROL, canprototransmitter_bid, ICUBCANPROTO_PER_MC_MSG__STATUS);
+
+            ECANSend(msgid, 8, &payload);
         }
+
     }
+
+
 }
 
 extern void CanIcubProtoTransmitterUpdateBoardId(unsigned char bid)