hotfixes 2

ino/ControllerMduino57Plus/ControllerMduino57Plus.ino

@@ -248,7 +248,7 @@ void read_int(byte **buf, int *x)
 void write_bools(byte **buf, bool b1, bool b2, bool b3, bool b4, bool b5, bool b6, bool b7, bool b8)
 {
   **buf = (b1 << 7) | (b2 << 6) | (b3 << 5) | (b4 << 4) | (b5 << 3) | (b6 << 2) | (b7 << 1) | (b8 << 0);
-  (*buf)++;  
+  (*buf)++;
 }
 
 void read_bools(byte **buf, bool *b1, bool *b2, bool *b3, bool *b4, bool *b5, bool *b6, bool *b7, bool *b8)
@@ -289,7 +289,10 @@ void measure_turbopumps()
 
 void handle_message(client_t &client)
 {
-    int msg_type = client.data[0];
+    byte msg_type;
+    byte *local = client.data;
+    byte **p = &local;
+    read_byte(p, &msg_type);
 
 
     if( msg_type == 0 ) // ping
@@ -305,27 +308,16 @@ void handle_message(client_t &client)
     else if( msg_type == 1 ) // new desired state
     {
         running_state_t  req_state;
-        
-        req_state.sysOverride      = client.data[ 2 ] & (1 << 7);
-        req_state.linked        = client.data[ 2 ] & (1 << 6);
-
-        req_state.pGratActive   = client.data[ 2 ] & (1 << 5);
-        req_state.pMcpActive    = client.data[ 2 ] & (1 << 4);
-        req_state.pVvActive     = client.data[ 2 ] & (1 << 3);
-
-        req_state.tDetActive    = client.data[ 2 ] & (1 << 2);
-        req_state.tDpsActive    = client.data[ 2 ] & (1 << 1);
-
 
-        req_state.vTdetOpen     = client.data[ 3 ] & (1 << 7);
-        req_state.vDetOpen      = client.data[ 3 ] & (1 << 6);
-        req_state.vFoilOpen     = client.data[ 3 ] & (1 << 5);
-        req_state.vDpsOpen      = client.data[ 3 ] & (1 << 4);
-
-        req_state.hvmcpHigh     = client.data[ 3 ] & (1 << 3);
-        req_state.rbeamlineHigh = client.data[ 3 ] & (1 << 2);
-
-        req_state.pumpEnabled   = client.data[ 3 ] & (1 << 1);
+        read_bools(p, &req_state.sysOverride, &req_state.linked, 
+                      &req_state.pGratActive, &req_state.pMcpActive, &req_state.pVvActive,
+                      &req_state.tDetActive, &req_state.tDpsActive, 
+                      NULL);
+        
+        read_bools(p, &req_state.vTdetOpen, &req_state.vDetOpen, &req_state.vFoilOpen, &req_state.vDpsOpen,
+                      &req_state.hvmcpHigh, &req_state.rbeamlineHigh,
+                      &req_state.pumpEnabled,
+                      NULL);
 
         running_state_t new_state = current_state;
 
@@ -657,6 +649,22 @@ bool update_state()
             if(current_state.pumpEnabled == true) updated = true;
             current_state.pumpEnabled = false;
         }
+
+        if((current_state.tDetActive && tp_states[TP_DET] < t_det_cutoff) 
+            || (current_state.pGratActive && gauge_pressures[GRAT] > p_grat_cutoff)
+            || (current_state.pMcpActive && gauge_pressures[MCP] > p_mcp_cutoff))
+        {
+            if(current_state.hvmcpHigh == false) {
+              updated = true;
+            }
+            current_state.hvmcpHigh= true;
+        }
+        else
+        {
+            if(current_state.hvmcpHigh == true) updated = false;
+            current_state.hvmcpHigh = false;
+        }
+        
     }
     return updated;
 }