safety - improve handling, disable safety bypass unless intended, and with log. fix alarm handling and machine status, fix current alarms

1 files changed, 11 insertions, 2 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/General/MachineStatus.c b/Software/Embedded_SW/Embedded/Modules/General/MachineStatus.c
index d43a640a2..ff4909438 100644
--- a/Software/Embedded_SW/Embedded/Modules/General/MachineStatus.c
+++ b/Software/Embedded_SW/Embedded/Modules/General/MachineStatus.c
@@ -20,6 +20,7 @@
 #include "PMR/MachineStatus/IDSPackLevel.pb-c.h"
 
 #include "drivers/I2C_Communication/ADC_MUX/ADC_MUX.h"
+#include "drivers/Motors/Motor.h"
 
 #include "Modules/General/MachineStatus.h"
 
@@ -33,9 +34,12 @@ void SetMachineStatus (MachineState State)
 }
 char MachineUpdateToken[36+1] = {0};
 IDSPackLevel IDS_Level[MAX_SYSTEM_DISPENSERS];
+TimerMotors_t msDispenserIdToMotorId[MAX_SYSTEM_DISPENSERS] = {HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1,HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_2,HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_3,HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_4,HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_5,HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_6,HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_7,HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_8};
+
 int MachineUpdateResponseFunc(void)
 {
     int i;
+
     MessageContainer responseContainer;
     if (MachineUpdateToken[0] == 0)
         return OK;
@@ -47,12 +51,16 @@ int MachineUpdateResponseFunc(void)
     machine_status__init(&MachineStatus);
     MachineStatus.has_state = true;
     MachineStatus.state = StoredMachineStatus;
-    MachineStatus.n_idspackslevels = MAX_SYSTEM_DISPENSERS;
+    MachineStatus.n_idspackslevels = 0;
     MachineStatus.idspackslevels = (IDSPackLevel**)my_malloc(sizeof(IDSPackLevel *)*8);
     if (MachineStatus.idspackslevels)
     {
         for (i = 0; i<MAX_SYSTEM_DISPENSERS;i++)
         {
+            if (MotorsCfg[msDispenserIdToMotorId[i]].hardwaremotortype
+                        != msDispenserIdToMotorId[i]) // dispenser not configured
+                    continue;
+
             idspack_level__init(&IDS_Level[i]);
             IDS_Level[i].has_index = true;
             IDS_Level[i].index = i;
@@ -61,7 +69,8 @@ int MachineUpdateResponseFunc(void)
             IDS_Level[i].dispenserlevel = (int32_t)dispcapacity;
             IDS_Level[i].has_midtanklevel = true;
             IDS_Level[i].midtanklevel = Get_MidTank_Int100_Sensor(i);
-            MachineStatus.idspackslevels[i] = &IDS_Level[i];
+            MachineStatus.idspackslevels[MachineStatus.n_idspackslevels] = &IDS_Level[i];
+            MachineStatus.n_idspackslevels++;
         }
     }
     else