Diff: outDiagnostics/outDiagnostics.cpp

Revision:

34:18bcf276d3bf

Parent:

33:6bc82b6b62e5

Child:

36:0afc0fc8f86b

--- a/outDiagnostics/outDiagnostics.cpp	Tue Jan 06 20:45:26 2015 +0000
+++ b/outDiagnostics/outDiagnostics.cpp	Wed Jan 07 03:25:50 2015 +0000
@@ -1,42 +1,117 @@
 #include "outDiagnostics.h"
 
-// Macros for working with the string
-#define ADD_LINE                  len+=sprintf(buff+len,"%s\r\n",line);                                                    // Add newlines, add it to the working buffer 
-#define ADD_SPRINTF_LINE          padCenter(line,max_charsPerLine-2,temp,' '); len+=sprintf(buff+len,"%s\r\n",line);       // Add newlines, add it to the working buffer 
-#define BOOL(VAR)                 (VAR)?"ERR":"OK"
+osThreadId Serialthread_id = 0;                         // RTOS thread ID of thread_serialOut
+const int max_charsPerLine = 80;                        // Max chars per line of printed information
+const int max_lines = TX_SIZE / (max_charsPerLine+2);   // Lines per printing chunk (write chunk to MODSERIAL Buffer every x lines)
+
+// Interrupt function called when the MODSERIAL TX buffer is empty
+// The functions signals RTOS to unblock to serial thread and allow it
+// to produce the chunk of serial output data
+void empty(MODSERIAL_IRQ_INFO *q)
+{
+    osSignalSet(Serialthread_id, 1);                    // Set signal 1 for serial thread
+}
 
-// Print to a string buffer, pad to maxLen chars and center it with char pad, str must be null terminated!
-void padCenter(char *buff, int LineLen, char *str, char pad) {
-    int len = strlen(str);
-    int padL = (LineLen-len)/2;                              // -1 to save room for the null terminator
-    for (int i=0; i<padL; i++) buff[i] = pad;                // Fill in the left padding chars
-    strcpy(buff+padL, str);
-    for (int i = padL+len; i<LineLen; i++) buff[i] = pad;    // Fill remaining with padding chars
-    buff[LineLen-1] = '\0';                                  // Add null terminator
+// Add a line of text to the printing queue.
+// Actually gets added to another local buffer, which gets copied into the MODSERIAL TX buffer when it
+// empties.  Provides double-buffered output stream so inflow and outflow rates can be different.
+// Char *str NEEDS to be NULL terminated string! bool newline determines whether newline characters \r\n are appended to the line
+void AddToChunk (char *str, bool newline=true)
+{
+    static char chunk[(max_charsPerLine+5)*max_lines];          // The chunk buffer, holds one chunk= max_charsPerLine*max_lines characters
+    static int pos = 0;                                         // The position to load next character
+    static int lines = 0;                                       // Number of lines currently stored in the chunk
+    static int wait = pc.txBufferGetSize(0) * (CHAR_TIME*1000); // Max wait time to empty the tx buffer = max time to send out all chars
+
+    pos += sprintf(chunk+pos, "%s%s", str, newline?"\r\n":"");  // Append to working array
+    lines++;                                                    // New line added to array
+
+    if (lines >= max_lines) {                                   // Time to print the block
+        // Is buffer free enough to accept the block?
+        if (pc.txBufferGetCount() - pc.txBufferGetSize(0) > pos+5) {
+            for (int i = 0; i < pos; i++) pc.putc(chunk[i]);    // Add the string to the buffer
+        } else {
+            // No, it is not free enough to hold the string
+            osSignalClear(Serialthread_id, 1);                  // Clear signal if it was set
+            Thread::signal_wait(1, wait);                       // RTOS wait for empty signal, max wait if it never occurs
+            for (int i = 0; i < pos; i++) pc.putc(chunk[i]);    // Add into the output buffer
+        }
+        lines = 0;                                              // Chunk added, reset line counter
+        pos = 0;                                                // Chunk added, reset position
+    }
 }
 
-// Generates the serial dashboard, uses MODDMA MODSERIAL to speed up printing
-void outDiagnostics::thread_serialOut(void const *args) {
-    const int max_charsPerLine = 81;                 // Max chars per line
-    const int max_lines = 35;                        // Max lines that the layout prints out
+// Print to internal string buffer, pad to maxLen chars and center it with char pad, str must be null terminated!
+void padCenter(int LineLen, char *str, char pad)
+{
+    static char line[max_charsPerLine+5];                    // String buffer to work with one line at a time
+
+    int len = strlen(str);                                   // Length of input string
+    int padL = (LineLen-len)/2;                              // How many pad chars needed on left side?
+    for (int i=0; i<padL; i++) line[i] = pad;                // Fill in the left padding chars
+    strcpy(line+padL, str);                                  // Copy to line string
+    for (int i = padL+len; i<LineLen; i++) line[i] = pad;    // Fill remaining with padding chars
+    line[LineLen-1] = '\0';                                  // Add null terminator
+
+    AddToChunk(line);
+}
+
+
+// Macros for working with the strings
+#define ADD_SPRINTF_LINE    padCenter(max_charsPerLine, temp, ' ');   // Cetner the string, then add newlines, and add to chunk
+#define DIVIDER_LINE        padCenter(max_charsPerLine, "", 196);     // Generate a line full of divider char 196, add to chunk
+#define TITLE(string)       padCenter(max_charsPerLine, string, 196); // Generate a title line (centered, surrounded by char 196), add to chunk
+#define BLANK_LINE          padCenter(max_charsPerLine, "", ' ');     // Generate a line full of spaces (blank), add to chunk
+#define BOOL(VAR)           (VAR)?"ERR":"OK"
+
+// Generates the serial dashboard, uses MODSERIAL, self-paced (thread yields when buffer is full, resumes when empty)
+void outDiagnostics::thread_serialOut(void const *args)
+{
+    Serialthread_id = Thread::gettid();              // Record thread ID so empty() can signal this thread
+    char temp[max_charsPerLine+5];                   // String buffer to sprintf into, max 1 line
+
+    pc.attach(&empty, MODSERIAL::TxEmpty);           // Attach the tx empty interrupt which paces this thread
     pc.printf("\033[2J");                            // Clear the screen to get rid of reset message
 
-    char buff[max_charsPerLine*max_lines];           // Giant string to store the printout
-    char line[max_charsPerLine];                     // String buffer to work with one line at a time
-    char temp[max_charsPerLine];                     // String buffer to sprintf into
+    tempData.parseGoodChar = ' ';
+    tempData.inputStr[0] = 0;
+    
+    Timer serialLoop;                                // Timer to track the serial loop time, since this thread paces itself
+    int serialTime_ms = 0;
+    serialLoop.reset();
+    serialLoop.start();                              // Start the counter for tracking serial loop time
+
+    //const char barSpace[4] = { ' ', 179, ' ', 0 };   // Commonly used string with char 179
+
     while(1) {
 
-        int len = 0;
-        len += sprintf(buff+len, "\033[0;0H");                      // Home the cursor
-        padCenter(line, max_charsPerLine-2, "-", '-'); ADD_LINE     // Generate a line full of -'s
-        padCenter(line, max_charsPerLine-2, " Penn Electric Racing - REV0 System Management Controller Serial Dashboard ", '-'); ADD_LINE
-        padCenter(line, max_charsPerLine-2, "-", '-'); ADD_LINE     // Generate a line full of -'s
+        serialTime_ms = serialLoop.read_ms();        // Update loop timer, reset for next loop
+        serialLoop.reset();
 
-        padCenter(line, max_charsPerLine-2, " ", ' '); ADD_LINE     // Generate blank line 
-        padCenter(line, max_charsPerLine-2, " GLV Battery ", '*'); ADD_LINE
-        sprintf(temp, "Current: %4.3fA Capacity: %4.3fAh", data.glvCurrent, data.glvCapacity); ADD_SPRINTF_LINE
-        sprintf(temp, "Amphours: %4.3fAh SOC: %5.1f%% Overcurrent: %s", data.glvAmphours, data.glvSOC*100.0, BOOL(data.glvOverCurrent)); ADD_SPRINTF_LINE
+        sprintf(temp, "\033[0;0H\033[0;0H");
+        AddToChunk(temp, false);                     // Move to 0,0, do not append newline
+        
+        DIVIDER_LINE
+        TITLE(" Penn Electric Racing - REV0 System Management Controller Dashboard ")
+        DIVIDER_LINE
         
+        int tempLength=0;
+        tempLength += sprintf(temp, "Command Input:%c %s%c", tempData.parseGoodChar, tempData.inputStr, 176);  // Command input: print header, reply, input string, and cursor marker
+        for (int i = 0; i < max_charsPerLine - strlen(temp)- 1; i++) {      // Fill in the rest of the line with blanks
+            tempLength += sprintf(temp+tempLength, " ");                    // Append spaces
+        }
+        AddToChunk(temp);        // Add this to the chunk
+        
+        TITLE(" GLV Battery ")
+        BLANK_LINE
+        sprintf(temp, "Current: %4.3fA          Capacity: %4.3fAh", data.glvCurrent, data.glvCapacity);
+        ADD_SPRINTF_LINE
+        sprintf(temp, "Amphours: %4.3fAh    SOC: %5.3f    Overcurrent: %s", data.glvAmphours, data.glvSOC, BOOL(data.glvOverCurrent));
+        ADD_SPRINTF_LINE
+
+        BLANK_LINE
+        TITLE(" DC-DC Converter ")
+        BLANK_LINE
         char DCDC = data.dcdcStatus;
         char dcdcModesStr[5][12] = {"INVALID","POWER-UP","POWER-DOWN","ON","OFF"};
         int dcdcMode = 0;
@@ -44,133 +119,120 @@
         else if (DCDC & PowerDown) dcdcMode = 2;
         else if (DCDC & SetOn)     dcdcMode = 3;
         else if (!(DCDC & SetOn))  dcdcMode = 4;
-        padCenter(line, max_charsPerLine-2, " ", ' '); ADD_LINE     // Generate blank line 
-        padCenter(line, max_charsPerLine-2, " DC-DC Converter ", '*'); ADD_LINE
-        sprintf(temp, "Current: %5.3fA Overcurrent: %s SensorFault: %s", data.dcdcCurrent, BOOL(DCDC & OverCurrent), BOOL(DCDC & SensorFault)); ADD_SPRINTF_LINE
-        sprintf(temp, "Active: %s Mode: %s AIRS: %s StatusByte: 0x%x", (DCDC & ConvOn)?"YES":"NO", dcdcModesStr[dcdcMode], CANdata.airsClosed?"CLOSED":"OPEN", DCDC); ADD_SPRINTF_LINE
-        sprintf(temp, "StartFault: %s StopFault: %s CritErrors: %s", BOOL(DCDC & FailStart), BOOL(DCDC & FailStop), BOOL(data.dcdcError)); ADD_SPRINTF_LINE
+        sprintf(temp, "Active: %3s     Mode: %10s     AIRS: %6s     StatusByte: 0x%02x", (DCDC & ConvOn)?"YES":"NO", dcdcModesStr[dcdcMode], CANdata.airsClosed?"CLOSED":"OPEN", DCDC);
+        ADD_SPRINTF_LINE
+        sprintf(temp, "Current: %5.3fA     Overcurrent: %3s     SensorFault: %3s", data.dcdcCurrent, BOOL(DCDC & OverCurrent), BOOL(DCDC & SensorFault));
+        ADD_SPRINTF_LINE
+        sprintf(temp, "StartFault: %3s     StopFault:   %3s     CritErrors:  %3s", BOOL(DCDC & FailStart), BOOL(DCDC & FailStop), BOOL(data.dcdcError));
+        ADD_SPRINTF_LINE
 
-        padCenter(line, max_charsPerLine-2, " ", ' '); ADD_LINE     // Generate blank line 
-        padCenter(line, max_charsPerLine-2, " PWM Channels ", '*'); ADD_LINE
-        sprintf(temp, "Actual:   FAN1: %5.1f%% FAN2: %5.1f%% PUMP1: %5.1f%% PUMP2: %5.1f%%", data.dcdcFan1Duty*100.0, data.dcdcFan2Duty*100.0, data.dcdcPump1Duty*100.0, data.dcdcPump2Duty*100.0); ADD_SPRINTF_LINE
-        sprintf(temp, "Requestd: FAN1: %5.1f%% FAN2: %5.1f%% PUMP1: %5.1f%% PUMP2: %5.1f%%", CANdata.dcdcFan1Duty*100.0, CANdata.dcdcFan2Duty*100.0, CANdata.dcdcPump1Duty*100.0, CANdata.dcdcPump2Duty*100.0); ADD_SPRINTF_LINE
+        BLANK_LINE
+        TITLE(" PWM Channels ")
+        BLANK_LINE
+        sprintf(temp, "Actual:     FAN1: %5.3f   FAN2: %5.3f   PUMP1: %5.3f   PUMP2: %5.3f", data.dcdcFan1Duty, data.dcdcFan2Duty, data.dcdcPump1Duty, data.dcdcPump2Duty);
+        ADD_SPRINTF_LINE
+        sprintf(temp, "Requestd:   FAN1: %5.3f   FAN2: %5.3f   PUMP1: %5.3f   PUMP2: %5.3f", CANdata.dcdcFan1Duty, CANdata.dcdcFan2Duty, CANdata.dcdcPump1Duty, CANdata.dcdcPump2Duty);
+        ADD_SPRINTF_LINE
 
+        BLANK_LINE
+        TITLE(" IMD ")
+        BLANK_LINE
         const char IMDstr[7][12] = {"OFF","NORMAL","UNDERVOLT","SPEEDSTART","ERROR","GROUNDFLT","INVALID"};
-        padCenter(line, max_charsPerLine-2, " ", ' '); ADD_LINE     // Generate blank line 
-        padCenter(line, max_charsPerLine-2, " IMD ", '*'); ADD_LINE
-        sprintf(temp, "Status: %s Resistance: %7.0fkohm CritError: %s",IMDstr[data.imdStatus], data.imdResistance/1e3, BOOL(data.imdError)); ADD_SPRINTF_LINE
-        
+        sprintf(temp, "Status: %10s Resistance: %7.0fkohm CritError: %3s",IMDstr[data.imdStatus], data.imdResistance/1e3, BOOL(data.imdError));
+        ADD_SPRINTF_LINE
+
+        BLANK_LINE
+        TITLE(" Latch Circuit Monitors ")
+        BLANK_LINE
         char AMSerr = data.AMSlatchError;
         char IMDerr = data.IMDlatchError;
-        padCenter(line, max_charsPerLine-2, " ", ' '); ADD_LINE     // Generate blank line 
-        padCenter(line, max_charsPerLine-2, " Latch Circuits ", '*'); ADD_LINE
-        sprintf(temp, "AMS: OK: %s Latch: %s SoftFault: %s HardFault: %s", (AMSerr & 1)?"LOW":"HIGH", (AMSerr & 2)?"OPEN":"OK", BOOL(AMSerr & 4), BOOL(AMSerr & 8)); ADD_SPRINTF_LINE
-        sprintf(temp, "IMD: OK: %s Latch: %s SoftFault: %s HardFault: %s", (IMDerr & 1)?"LOW":"HIGH", (IMDerr & 2)?"OPEN":"OK", BOOL(IMDerr & 4), BOOL(IMDerr & 8)); ADD_SPRINTF_LINE
+        sprintf(temp, "AMS: OK: %4s Latch: %3s SoftFault: %3s HardFault: %3s", (AMSerr & 1)?"LOW":"HIGH", BOOL(AMSerr & 2), BOOL(AMSerr & 4), BOOL(AMSerr & 8));
+        ADD_SPRINTF_LINE
+        sprintf(temp, "IMD: OK: %4s Latch: %3s SoftFault: %3s HardFault: %3s", (IMDerr & 1)?"LOW":"HIGH", BOOL(IMDerr & 2), BOOL(IMDerr & 4), BOOL(IMDerr & 8));
+        ADD_SPRINTF_LINE
 
+        BLANK_LINE
+        TITLE(" Shutdown Switches ")
+        BLANK_LINE
         char switches = data.switchState;
         const char switchNames[12][26] = {"FUSE","AMS LATCH","IMD LATCH","PCM RELAY","BRAKE PLAUSIBILITY RELAY","LEFT E-STOP","INERTIA SWITCH","BRAKE OVER-TRAVEL SWITCH","COCKPIT E-STOP","RIGHT E-STOP","HVD","TSMS"};
-        padCenter(line, max_charsPerLine-2, " ", ' '); ADD_LINE     // Generate blank line
-        padCenter(line, max_charsPerLine-2, " Shutdown Switches ", '*'); ADD_LINE
         if (switches == 0) sprintf(temp, "All switches are CLOSED.");
         else sprintf(temp, "%s is OPEN.", switchNames[switches-1]);
         ADD_SPRINTF_LINE
-        
-        padCenter(line, max_charsPerLine-2, " ", ' '); ADD_LINE   // Generate blank line
-        padCenter(line, max_charsPerLine-2, " Telemetry ", '*'); ADD_LINE
-        sprintf(temp, "Channel 1: MessagesIn: %d MessagesOut: %d", xbeeRelay.counterX1in, xbeeRelay.counterX1out); ADD_SPRINTF_LINE
-        sprintf(temp, "Channel 2: MessagesIn: %d MessagesOut: %d", xbeeRelay.counterX2in, xbeeRelay.counterX2out); ADD_SPRINTF_LINE
 
-        padCenter(line, max_charsPerLine-2, " ", ' '); ADD_LINE     // Generate blank line 
-        padCenter(line, max_charsPerLine-2, " Miscellaneous ", '*'); ADD_LINE
-        sprintf(temp, "Temp: %5.1fC OverTemp: %s canFault: %s WatchdogReset: %s ErrorFrame: 0x%x", data.internalTemp, BOOL(data.internalOverTemp), BOOL(data.canFault), BOOL(data.watchdogReset), data.errorFrame); ADD_SPRINTF_LINE
+        BLANK_LINE
+        TITLE(" Telemetry ")
+        BLANK_LINE
+        sprintf(temp, "Channel 1: MessagesIn: %5d MessagesOut: %5d", xbeeRelay.counterX1in, xbeeRelay.counterX1out);
+        ADD_SPRINTF_LINE
+        sprintf(temp, "Channel 2: MessagesIn: %5d MessagesOut: %5d", xbeeRelay.counterX2in, xbeeRelay.counterX2out);
+        ADD_SPRINTF_LINE
 
-        // Write it all at once to output tx buffer
-        for (int i = 0; i < strlen(buff); i++) {
-            pc.putc(buff[i]);   
-        }
+        BLANK_LINE
+        TITLE(" Miscellaneous ")
+        BLANK_LINE
+        sprintf(temp, "Temp: %5.1fC %3s canFault: %3s Watchdog: %3s ErrorCode: 0x%02x SerailTime: %3dms", data.internalTemp, BOOL(data.internalOverTemp), BOOL(data.canFault), BOOL(data.watchdogReset), data.errorFrame, serialTime_ms);
+        ADD_SPRINTF_LINE
+
         // Erase screen every 5sec to remove glitches
         static int count=0;
         if (count%50==0) {
             pc.printf("\033[2J");        // Clear the screen
         }
         count++;
-        Thread::wait(100);
     }
 }
 
-template <typename T>
-void sendCAN(int ID, T dat) {
+// Macros to streamline can bus message formatting and sending
+#define SEND_CAN(LEN, ID)               msg.len=LEN; msg.id=ID; msg.type = CANData; msg.format = CANStandard; if (!can.txWrite(msg)) data.canFault = true; xbeeRelay.send(msg);
+#define FLOAT(DATA, ID)                 *((float*)((void*)&msg.data[0]))    = DATA;    SEND_CAN(sizeof(float), ID)
+#define UINT(DATA, ID)                  *((uint32_t*)((void*)&msg.data[0])) = DATA;    SEND_CAN(sizeof(uint32_t), ID)
+#define FLOAT_PAIR(DATA1, DATA2, ID)    *((float*)((void*)&msg.data[0]))    = DATA1;   *((float*)((void*)&msg.data[4]))    = DATA2;   SEND_CAN(2*sizeof(float), ID)
+#define UINT_PAIR(DATA1, DATA2, ID)     *((uint32_t*)((void*)&msg.data[0])) = DATA1;   *((uint32_t*)((void*)&msg.data[4])) = DATA2;   SEND_CAN(2*sizeof(uint32_t), ID)
+#define CHAR(DATA, ID)                  msg.data[0]                         = DATA;    SEND_CAN(sizeof(char), ID)
+
+void outDiagnostics::thread_canOut(void const *args)
+{
     CANMessage msg;
-    msg.id = ID;
-    msg.len = sizeof(T);
-    memcpy(&msg.data[0], (void*)&dat, sizeof(T));
-    if (!can.txWrite(msg)) data.canFault = true;
-    xbeeRelay.send(msg);
-}
-
-void outDiagnostics::thread_canOut(void const *args) {
-
     while(1) {
-        CANMessage msg;
 
         // Sys Mgmt Error Frame
-        sendCAN(SYS_ERROR_ID, data.errorFrame);
-        
+        CHAR(data.errorFrame, SYS_ERROR_ID)
+
         // Xbee1 Counter
-        msg.id = SYS_XBEE1_ID;
-        msg.len = 2*sizeof(int);
-        memcpy(&msg.data[0], (void*)&xbeeRelay.counterX1in, sizeof(int));
-        memcpy(&msg.data[4], (void*)&xbeeRelay.counterX1out, sizeof(int));
-        if (!can.txWrite(msg)) data.canFault = true;
-        xbeeRelay.send(msg);
+        UINT_PAIR(xbeeRelay.counterX1in, xbeeRelay.counterX1out, SYS_XBEE1_ID)
 
         // Xbee2 Counter
-        msg.id = SYS_XBEE2_ID;
-        msg.len = 2*sizeof(int);
-        memcpy(&msg.data[0], (void*)&xbeeRelay.counterX2in, sizeof(int));
-        memcpy(&msg.data[4], (void*)&xbeeRelay.counterX2out, sizeof(int));
-        if (!can.txWrite(msg)) data.canFault = true;
-        xbeeRelay.send(msg);
-        
+        UINT_PAIR(xbeeRelay.counterX2in, xbeeRelay.counterX2out, SYS_XBEE2_ID)
+
         // Internal temperature
-        sendCAN(SYS_TEMP_ID, data.internalTemp);
+        FLOAT(data.internalTemp, SYS_TEMP_ID)
 
         // GLV Battery
-        sendCAN(SYS_GLV_CURRENT_ID, data.glvCurrent);
-        sendCAN(SYS_GLV_CAPACITY_ID, data.glvCapacity);
-        sendCAN(SYS_GLV_AH_ID, data.glvAmphours);
-        sendCAN(SYS_GLV_SOC_ID, data.glvSOC);
-
+        FLOAT(data.glvCurrent, SYS_GLV_CURRENT_ID)
+        FLOAT(data.glvCapacity, SYS_GLV_CAPACITY_ID)
+        FLOAT(data.glvAmphours, SYS_GLV_AH_ID)
+        FLOAT(data.glvSOC, SYS_GLV_SOC_ID)
+        
         // DC-DC Converter
-        sendCAN(SYS_DCDC_CURRENT_ID, data.dcdcCurrent);
-        sendCAN(SYS_DCDC_STATUS_ID, data.dcdcStatus);
+        FLOAT(data.dcdcCurrent, SYS_DCDC_CURRENT_ID)
+        CHAR(data.dcdcStatus, SYS_DCDC_STATUS_ID)
 
-        // PWM Channels        
-        msg.id = SYS_PWM_FAN_ID;
-        msg.len = 2*sizeof(float);
-        memcpy(&msg.data[0], (void*)&data.dcdcFan1Duty, sizeof(float));
-        memcpy(&msg.data[4], (void*)&data.dcdcFan2Duty, sizeof(float));
-        if (!can.txWrite(msg)) data.canFault = true;
-        xbeeRelay.send(msg);
+        // PWM Channels
+        FLOAT_PAIR(data.dcdcFan1Duty, data.dcdcFan2Duty, SYS_PWM_FAN_ID)
+        FLOAT_PAIR(data.dcdcPump1Duty, data.dcdcPump2Duty, SYS_PWM_PUMP_ID)
 
-        msg.id = SYS_PWM_PUMP_ID;
-        msg.len = 2*sizeof(float);
-        memcpy(&msg.data[0], (void*)&data.dcdcPump1Duty, sizeof(float));
-        memcpy(&msg.data[4], (void*)&data.dcdcPump2Duty, sizeof(float));
-        if (!can.txWrite(msg)) data.canFault = true;
-        xbeeRelay.send(msg);
-        
         // IMD
-        sendCAN(SYS_IMD_STATUS_ID, data.imdStatus);
-        sendCAN(SYS_IMD_RESIST_ID, data.imdResistance);
+        FLOAT(data.imdStatus, SYS_IMD_STATUS_ID)
+        CHAR(data.imdResistance, SYS_IMD_RESIST_ID)
 
         // Latches
-        sendCAN(SYS_IMD_LATCH_ID, data.IMDlatchError);
-        sendCAN(SYS_AMS_LATCH_ID, data.AMSlatchError);
+        CHAR(data.IMDlatchError, SYS_IMD_LATCH_ID)
+        CHAR(data.AMSlatchError, SYS_AMS_LATCH_ID)
 
         // Shutdown Switches
-        sendCAN(SYS_SWITCHES_ID, data.switchState);
+        CHAR(data.switchState, SYS_SWITCHES_ID)
 
         Thread::wait(CAN_LOOP*1000);
-    }   
+    }
 }
\ No newline at end of file