Добавлены датчики и IO UDS

CanUds.c

@@ -776,7 +776,6 @@ static uint16_t InputOutputControlByIdentifier_2F(tCanUds *env) {
                 }
 
 
-
                 return 4;
             }
 
@@ -1804,6 +1803,7 @@ static uint16_t RequestTransferExit_37(tCanUds *env) {
 // конец --------------------------- RequestTransferExit ---------------------------------------------------------
 // конец --------------------------- RequestTransferExit ---------------------------------------------------------
 
+// Тут данные должны быть с модели &env->Status_Sensor_For_Model_input и т.д.
 void CanUdsInput_Set_Model(tCanUds *env) {
     osStatus_t status = osOK;
 
@@ -1829,7 +1829,7 @@ void CanUdsInput_Set_Model(tCanUds *env) {
     }
 
     countQueue_space = osMessageQueueGetSpace(env->queue_input_get_Status_IO);
-    if (countQueue_space) {
+    if (!countQueue_space) {
         status = osMessageQueuePut(env->queue_input_get_Status_IO, &env->Status_IO_For_Model_input, 0, 0);
     }
 
@@ -1869,23 +1869,18 @@ void CanUdsInput_Set_Model(tCanUds *env) {
 // Это на вход UDS
 static void CanUdsInput_Get_Model(tCanUds *env) {
 
-    osStatus_t status = osMessageQueueGet(env->queue_input_get_Status_Sensor, &env->Status_Sensor_For_Model_input, 0,
+    osStatus_t status = osOK;
-                                          0);
-    status = osMessageQueueGet(env->queue_input_get_Status_Actuator_DoorsActuator, &env->Status_Sensor_For_Model_input,
-                               0, 0);
-    status = osMessageQueueGet(env->queue_input_get_Status_Duct_Tgt, &env->Status_Duct_Tgt_For_Model_input, 0, 0);
-    status = osMessageQueueGet(env->queue_input_get_Status_Eva_Tgt, &env->Status_Eva_Tgt_For_Model_input, 0, 0);
-    status = osMessageQueueGet(env->queue_input_get_Status_IO, &env->Status_IO_For_Model_input, 0, 0);
-    status = osMessageQueueGet(env->queue_input_get_Status_Pwm, &env->Status_Pwm_For_Model_input, 0, 0);
-    status = osMessageQueueGet(env->queue_input_get_Status_eComp, &env->Status_eComp_For_Model_input, 0, 0);
-    status = osMessageQueueGet(env->queue_input_get_Status_External, &env->Status_External_For_Model_input, 0, 0);
-    status = osMessageQueueGet(env->queue_input_get_Status_EMS, &env->Status_EMS_For_Model_input, 0, 0);
-    status = osMessageQueueGet(env->queue_input_get_Status_Ionizer_Aroma, &env->Status_Ionizer_Aroma_For_Model_input, 0,
-                               0);
-    status = osMessageQueueGet(env->queue_input_get_Status_Duct_Select, &env->Status_Duct_Select_For_Model_input, 0, 0);
 
 //----------------------    tStatus_Sensor Status_Sensor; --------------------------------------------------------------
+    uint32_t countQueue_space = osMessageQueueGetSpace(env->queue_input_get_Status_Sensor);
 
+    if (countQueue_space == 0) {
+
+        status = osMessageQueueGet(env->queue_input_get_Status_Sensor, &env->Status_Sensor_For_Model_input,
+                                   0,
+                                   0);
+
+        if (status == osOK) {
             statusData.Status_Sensor.Ignition = env->Status_Sensor_For_Model_input.Ignition;
             statusData.Status_Sensor.Battery = env->Status_Sensor_For_Model_input.Battery;
             statusData.Status_Sensor.AMB = env->Status_Sensor_For_Model_input.AMB;
@@ -1907,9 +1902,19 @@ static void CanUdsInput_Get_Model(tCanUds *env) {
             statusData.Status_Sensor.Duct_Side_RL = env->Status_Sensor_For_Model_input.Duct_Side_RL;
             statusData.Status_Sensor.Duct_Side_RR = env->Status_Sensor_For_Model_input.Duct_Side_RR;
             statusData.Status_Sensor.AQS = env->Status_Sensor_For_Model_input.AQS;
+        }
+    }
 
 //----------------------   Status_Actuator_DoorsActuator ---------------------------------------------------------------
+    countQueue_space = osMessageQueueGetSpace(env->queue_input_get_Status_Actuator_DoorsActuator);
 
+    if (countQueue_space == 0) {
+
+        status = osMessageQueueGet(env->queue_input_get_Status_Actuator_DoorsActuator,
+                                   &env->Status_Sensor_For_Model_input,
+                                   0, 0);
+
+        if (status == osOK) {
             statusData.Status_Actuator_DoorsActuator_FB.Motor_Def = env->Status_Actuator_DoorsActuator_For_Model_input.Motor_Def;
             statusData.Status_Actuator_DoorsActuator_FB.Motor_Vent_SFL = env->Status_Actuator_DoorsActuator_For_Model_input.Motor_Vent_SFL;
             statusData.Status_Actuator_DoorsActuator_FB.Motor_Vent_CFL = env->Status_Actuator_DoorsActuator_For_Model_input.Motor_Vent_CFL;
@@ -1934,23 +1939,42 @@ static void CanUdsInput_Get_Model(tCanUds *env) {
             statusData.Status_Actuator_DoorsActuator_FB.Motor_REC = env->Status_Actuator_DoorsActuator_For_Model_input.Motor_REC;
             statusData.Status_Actuator_DoorsActuator_FB.Motor_OSA = env->Status_Actuator_DoorsActuator_For_Model_input.Motor_OSA;
             statusData.Status_Actuator_DoorsActuator_FB.Motor_SealingValve = env->Status_Actuator_DoorsActuator_For_Model_input.Motor_SealingValve;
-
+        }
+    }
 //----------------------   Status_Duct_Tgt -----------------------------------------------------------------------------
+    countQueue_space = osMessageQueueGetSpace(env->queue_input_get_Status_Duct_Tgt);
 
+    if (countQueue_space == 0) {
+        status = osMessageQueueGet(env->queue_input_get_Status_Duct_Tgt, &env->Status_Duct_Tgt_For_Model_input, 0, 0);
+
+        if (status == osOK) {
             statusData.Status_Duct_Tgt_FB.DuctTgt_Vent_FL = env->Status_Duct_Tgt_For_Model_input.DuctTgt_Vent_FL;
             statusData.Status_Duct_Tgt_FB.DuctTgt_Foot_FL = env->Status_Duct_Tgt_For_Model_input.DuctTgt_Foot_FL;
             statusData.Status_Duct_Tgt_FB.DuctTgt_Vent_FR = env->Status_Duct_Tgt_For_Model_input.DuctTgt_Vent_FR;
             statusData.Status_Duct_Tgt_FB.DuctTgt_Foot_FR = env->Status_Duct_Tgt_For_Model_input.DuctTgt_Foot_FR;
             statusData.Status_Duct_Tgt_FB.DuctTgt_RL = env->Status_Duct_Tgt_For_Model_input.DuctTgt_RL;
             statusData.Status_Duct_Tgt_FB.DuctTgt_RR = env->Status_Duct_Tgt_For_Model_input.DuctTgt_RR;
-
+        }
+    }
 //----------------------   Status_Eva_Tgt ------------------------------------------------------------------------------
+    countQueue_space = osMessageQueueGetSpace(env->queue_input_get_Status_Eva_Tgt);
 
+    if (countQueue_space == 0) {
+        status = osMessageQueueGet(env->queue_input_get_Status_Eva_Tgt, &env->Status_Eva_Tgt_For_Model_input, 0, 0);
+
+        if (status == osOK) {
             statusData.Status_Eva_Tgt_FB.EvaTgt_F = env->Status_Eva_Tgt_For_Model_input.EvaTgt_F;
             statusData.Status_Eva_Tgt_FB.EvaTgt_R = env->Status_Eva_Tgt_For_Model_input.EvaTgt_R;
-
+        }
+    }
 
 //----------------------   Status_IO -----------------------------------------------------------------------------------
+    countQueue_space = osMessageQueueGetSpace(env->queue_input_get_Status_IO);
+
+    if (countQueue_space == 0) {
+        status = osMessageQueueGet(env->queue_input_get_Status_IO, &env->Status_IO_For_Model_input, 0, 0);
+
+        if (status == osOK) {
 
             statusData.Status_IO_FB._2_Way_Valve = env->Status_IO_For_Model_input._2_Way_Valve;
             statusData.Status_IO_FB.IncarMotor_Front = env->Status_IO_For_Model_input.IncarMotor_Front;
@@ -1960,33 +1984,83 @@ static void CanUdsInput_Get_Model(tCanUds *env) {
             statusData.Status_IO_FB.ShutOff_Rear = env->Status_IO_For_Model_input.ShutOff_Rear;
             statusData.Status_IO_FB.PTC_1 = env->Status_IO_For_Model_input.PTC_1;
             statusData.Status_IO_FB.PTC_2 = env->Status_IO_For_Model_input.PTC_2;
-
+        }
+    }
 //----------------------   Status_Pwm ----------------------------------------------------------------------------------
+    countQueue_space = osMessageQueueGetSpace(env->queue_input_get_Status_Pwm);
 
+    if (countQueue_space == 0) {
+        status = osMessageQueueGet(env->queue_input_get_Status_Pwm, &env->Status_Pwm_For_Model_input, 0, 0);
+
+        if (status == osOK) {
             statusData.Status_Pwm_FB.Pwm_Front = env->Status_Pwm_For_Model_input.Pwm_Front;
             statusData.Status_Pwm_FB.Pwm_Rear = env->Status_Pwm_For_Model_input.Pwm_Rear;
-
+        }
+    }
 
 //----------------------   Status_eComp---------------------------------------------------------------------------------
+    countQueue_space = osMessageQueueGetSpace(env->queue_input_get_Status_eComp);
+
+    if (countQueue_space == 0) {
+        status = osMessageQueueGet(env->queue_input_get_Status_eComp, &env->Status_eComp_For_Model_input, 0, 0);
+
+        if (status == osOK) {
+
             statusData.Status_eComp_FB.eComp_Statue_Request = env->Status_eComp_For_Model_input.eComp_Statue_Request;
             statusData.Status_eComp_FB.Pwm_Front = env->Status_eComp_For_Model_input.Pwm_Front;
             statusData.Status_eComp_FB.LowTempValve = env->Status_eComp_For_Model_input.LowTempValve;
+        }
+    }
 
 //----------------------   Status_External------------------------------------------------------------------------------
+    countQueue_space = osMessageQueueGetSpace(env->queue_input_get_Status_External);
+
+    if (countQueue_space == 0) {
+        status = osMessageQueueGet(env->queue_input_get_Status_External, &env->Status_External_For_Model_input, 0, 0);
+
+        if (status == osOK) {
+
             statusData.Status_External_FB.CoolingFan = env->Status_External_For_Model_input.CoolingFan;
-
+        }
+    }
 //----------------------   Status_EMS-----------------------------------------------------------------------------------
-    statusData.Status_EMS_FB.EMS_Chiller_Req = env->Status_EMS_For_Model_input.EMS_Chiller_Req;
+    countQueue_space = osMessageQueueGetSpace(env->queue_input_get_Status_EMS);
 
+    if (countQueue_space == 0) {
+        status = osMessageQueueGet(env->queue_input_get_Status_EMS, &env->Status_EMS_For_Model_input, 0, 0);
+
+        if (status == osOK) {
+
+            statusData.Status_EMS_FB.EMS_Chiller_Req = env->Status_EMS_For_Model_input.EMS_Chiller_Req;
+        }
+    }
 //----------------------   Status_Ionizer_Aroma-------------------------------------------------------------------------
+    countQueue_space = osMessageQueueGetSpace(env->queue_input_get_Status_Ionizer_Aroma);
+
+    if (countQueue_space == 0) {
+        status = osMessageQueueGet(env->queue_input_get_Status_Ionizer_Aroma,
+                                   &env->Status_Ionizer_Aroma_For_Model_input, 0,
+                                   0);
+        if (status == osOK) {
             statusData.Status_Ionizer_Aroma_FB.Ionizer = env->Status_Ionizer_Aroma_For_Model_input.Ionizer;
             statusData.Status_Ionizer_Aroma_FB.AromatizationIntensivity = env->Status_Ionizer_Aroma_For_Model_input.AromatizationIntensivity;
+        }
+    }
 
 //----------------------   Status_Duct_Select-------------------------------------------------------------------------
+    countQueue_space = osMessageQueueGetSpace(env->queue_input_get_Status_Duct_Select);
+
+    if (countQueue_space == 0) {
+        status = osMessageQueueGet(env->queue_input_get_Status_Duct_Select, &env->Status_Duct_Select_For_Model_input, 0,
+                                   0);
+
+        if (status == osOK) {
             statusData.Status_Duct_Select_FB.Duct_FL = env->Status_Duct_Select_For_Model_input.Duct_FL;
             statusData.Status_Duct_Select_FB.Duct_FR = env->Status_Duct_Select_For_Model_input.Duct_FR;
             statusData.Status_Duct_Select_FB.Duct_RL = env->Status_Duct_Select_For_Model_input.Duct_RL;
             statusData.Status_Duct_Select_FB.Duct_RR = env->Status_Duct_Select_For_Model_input.Duct_RR;
+        }
+    }
 
 }
 
@@ -2106,20 +2180,69 @@ void CanUdsOutput_Set_Model(tCanUds *env) {
     env->Status_Duct_Select_For_Model_output.Duct_RR = statusData.Status_Duct_Select.Duct_RR;
 
 
-    osStatus_t status = osMessageQueuePut(env->queue_input_get_Status_Sensor, &env->Status_Set_For_Model_output, 0, 0);
+    osStatus_t status = osOK;
+
+    uint32_t countQueue_space = osMessageQueueGetSpace(env->queue_input_get_Status_Sensor);
+    if (countQueue_space) {
+         status = osMessageQueuePut(env->queue_input_get_Status_Sensor, &env->Status_Set_For_Model_output, 0,
+                                              0);
+    }
+
+    countQueue_space = osMessageQueueGetSpace(env->queue_input_get_Status_Actuator_DoorsActuator);
+    if (countQueue_space) {
         status = osMessageQueuePut(env->queue_input_get_Status_Actuator_DoorsActuator,
                                    &env->Status_Actuator_DoorsActuator_For_Model_output, 0, 0);
+    }
+
+    countQueue_space = osMessageQueueGetSpace(env->queue_input_get_Status_Duct_Tgt);
+    if (countQueue_space) {
         status = osMessageQueuePut(env->queue_input_get_Status_Duct_Tgt, &env->Status_Duct_Tgt_For_Model_output, 0, 0);
+    }
+
+    countQueue_space = osMessageQueueGetSpace(env->queue_input_get_Status_Eva_Tgt);
+    if (countQueue_space) {
         status = osMessageQueuePut(env->queue_input_get_Status_Eva_Tgt, &env->Status_Eva_Tgt_For_Model_output, 0, 0);
+    }
+
+
+    countQueue_space = osMessageQueueGetSpace(env->queue_input_get_Status_IO);
+    if (countQueue_space) {
         status = osMessageQueuePut(env->queue_input_get_Status_IO, &env->Status_IO_For_Model_output, 0, 0);
+    }
+
+    countQueue_space = osMessageQueueGetSpace(env->queue_input_get_Status_Pwm);
+    if (countQueue_space) {
         status = osMessageQueuePut(env->queue_input_get_Status_Pwm, &env->Status_Pwm_For_Model_output, 0, 0);
+    }
+
+    countQueue_space = osMessageQueueGetSpace(env->queue_input_get_Status_eComp);
+    if (countQueue_space) {
         status = osMessageQueuePut(env->queue_input_get_Status_eComp, &env->Status_eComp_For_Model_output, 0, 0);
+    }
+
+    countQueue_space = osMessageQueueGetSpace(env->queue_input_get_Status_External);
+    if (countQueue_space) {
         status = osMessageQueuePut(env->queue_input_get_Status_External, &env->Status_External_For_Model_output, 0, 0);
+    }
+
+    countQueue_space = osMessageQueueGetSpace(env->queue_input_get_Status_EMS);
+    if (countQueue_space) {
         status = osMessageQueuePut(env->queue_input_get_Status_EMS, &env->Status_EMS_For_Model_output, 0, 0);
-    status = osMessageQueuePut(env->queue_input_get_Status_Ionizer_Aroma, &env->Status_Ionizer_Aroma_For_Model_output,
+    }
+
+    countQueue_space = osMessageQueueGetSpace(env->queue_input_get_Status_Ionizer_Aroma);
+    if (countQueue_space) {
+        status = osMessageQueuePut(env->queue_input_get_Status_Ionizer_Aroma,
+                                   &env->Status_Ionizer_Aroma_For_Model_output,
                                    0, 0);
-    status = osMessageQueuePut(env->queue_input_get_Status_Duct_Select, &env->Status_Duct_Select_For_Model_output, 0,
+    }
+
+    countQueue_space = osMessageQueueGetSpace(env->queue_input_get_Status_Duct_Select);
+    if (countQueue_space) {
+        status = osMessageQueuePut(env->queue_input_get_Status_Duct_Select, &env->Status_Duct_Select_For_Model_output,
+                                   0,
                                    0);
+    }
 
 }