reorganized and fixed several issues with checksum validation

[clinton/Smoothieware.git] / src / modules / communication / GcodeDispatch.cpp

diff --git a/src/modules/communication/GcodeDispatch.cpp b/src/modules/communication/GcodeDispatch.cpp
index 0422a88..4975237 100644 (file)
--- a/src/modules/communication/GcodeDispatch.cpp
+++ b/src/modules/communication/GcodeDispatch.cpp
@@ -7,19 +7,21 @@
 
 #include <string>
 using std::string;
-#include "mbed.h"
 #include "libs/Module.h"
 #include "libs/Kernel.h"
 #include "utils/Gcode.h"
 #include "libs/nuts_bolts.h"
 #include "GcodeDispatch.h"
 #include "modules/robot/Player.h" 
+#include "libs/SerialMessage.h"
+#include "libs/StreamOutput.h"
 
 GcodeDispatch::GcodeDispatch(){}
 
 // Called when the module has just been loaded
 void GcodeDispatch::on_module_loaded() {
     this->register_for_event(ON_CONSOLE_LINE_RECEIVED);
+    currentline = -1;
 }
 
 // When a command is received, if it is a Gcode, dispatch it as an object via an event
@@ -28,21 +30,70 @@ void GcodeDispatch::on_console_line_received(void * line){
     string possible_command = new_message.message;    
 
     char first_char = possible_command[0];
-    if( first_char == 'G' || first_char == 'M' || first_char == 'T' || first_char == 'S' ){ 
-   
+    int ln = 0;
+    int cs = 0;
+    if( first_char == 'G' || first_char == 'M' || first_char == 'T' || first_char == 'S' || first_char == 'N' ){ 
+
+        //Get linenumber
+        if( first_char == 'N' ){ 
+            Gcode full_line = Gcode();
+            full_line.command = possible_command;
+            full_line.stream = new_message.stream;
+            ln = (int) full_line.get_value('N');
+            int chksum = (int) full_line.get_value('*');
+
+            //Strip checksum value from possible_command
+            size_t chkpos = possible_command.find_first_of("*");
+            possible_command = possible_command.substr(0, chkpos); 
+            //Calculate checksum
+            if( chkpos != string::npos ){ 
+                for(int i = 0; possible_command[i] != '*' && possible_command[i] != NULL; i++)
+                    cs = cs ^ possible_command[i];
+                cs &= 0xff;  // Defensive programming...
+                cs -= chksum;
+            }
+            //Strip line number value from possible_command
+            size_t lnsize = possible_command.find_first_of(" ") + 1;
+            possible_command = possible_command.substr(lnsize); 
+        }else{
+            //Assume checks succeeded
+            cs = 0x00;
+            ln = currentline + 1;
+        }
+
         //Remove comments
         size_t comment = possible_command.find_first_of(";");
         if( comment != string::npos ){ possible_command = possible_command.substr(0, comment); }
 
-        // Dispatch
+        //Prepare gcode for dispatch
         Gcode gcode = Gcode();
         gcode.command = possible_command;
-        this->kernel->call_event(ON_GCODE_RECEIVED, &gcode ); 
-        
-        new_message.stream->printf("ok\r\n");
+        gcode.stream = new_message.stream; 
+
+        //Catch message if it is M110: Set Current Line Number
+        if( gcode.has_letter('M') ){
+            if( ((int) gcode.get_value('M')) == 110 ){
+                currentline = ln;
+                new_message.stream->printf("ok\r\n");
+                return;
+            }
+        }
+
+        //If checksum passes then process message, else request resend
+        int nextline = currentline + 1;
+        if( cs == 0x00 && ln == nextline ){
+            if( first_char == 'N' )
+                currentline = nextline;
+            //Dispatch message!
+            this->kernel->call_event(ON_GCODE_RECEIVED, &gcode ); 
+            new_message.stream->printf("ok\r\n");
+       }else{
+            //Request resend
+            new_message.stream->printf("rs N%d\r\n", nextline, ((int) gcode.get_value('M')), possible_command.c_str());
+        }
 
-    // Ignore comments 
-    }else if( first_char == ';' || first_char == '(' ){
+    // Ignore comments and blank lines
+    }else if( first_char == ';' || first_char == '(' || first_char == ' ' || first_char == '\n' || first_char == '\r' ){
         new_message.stream->printf("ok\r\n");
     }
 }