/**
Check if result is plausible. If it is, an ok is send and the command is stored in queue.
If not, a resend and ok is send.
*/
void GCode::checkAndPushCommand()
{
if(hasM())
{
if(M==110) // Reset line number
{
lastLineNumber = actLineNumber;
Com::printFLN(Com::tOk);
waitingForResend = -1;
return;
}
if(M==112) // Emergency kill - freeze printer
{
Commands::emergencyStop();
}
#ifdef DEBUG_COM_ERRORS
if(M==666)
{
lastLineNumber++;
return;
}
#endif // DEBUG_COM_ERRORS
}
if(hasN())
{
if((((lastLineNumber+1) & 0xffff)!=(actLineNumber&0xffff)))
{
if(waitingForResend<0) // after a resend, we have to skip the garbage in buffers, no message for this
{
if(Printer::debugErrors())
{
Com::printF(Com::tExpectedLine,lastLineNumber+1);
Com::printFLN(Com::tGot,actLineNumber);
}
requestResend(); // Line missing, force resend
}
else
{
--waitingForResend;
commandsReceivingWritePosition = 0;
Com::printFLN(Com::tSkip,actLineNumber);
Com::printFLN(Com::tOk);
}
return;
}
lastLineNumber = actLineNumber;
}
pushCommand();
#ifdef ACK_WITH_LINENUMBER
Com::printFLN(Com::tOkSpace,actLineNumber);
#else
Com::printFLN(Com::tOk);
#endif
wasLastCommandReceivedAsBinary = sendAsBinary;
waitingForResend = -1; // everything is ok.
}
/** \brief Print command on serial console */
void GCode::printCommand()
{
if(hasM())
{
Com::print('M');
Com::print((int)M);
Com::print(' ');
}
if(hasG())
{
Com::print('G');
Com::print((int)G);
Com::print(' ');
}
if(hasT())
{
Com::print('T');
Com::print((int)T);
Com::print(' ');
}
if(hasX())
{
Com::printF(Com::tX,X);
}
if(hasY())
{
Com::printF(Com::tY,Y);
}
if(hasZ())
{
Com::printF(Com::tZ,Z);
}
if(hasE())
{
Com::printF(Com::tE,E,4);
}
if(hasF())
{
Com::printF(Com::tF,F);
}
if(hasS())
{
Com::printF(Com::tS,S);
}
if(hasP())
{
Com::printF(Com::tP,P);
}
if(hasI())
{
Com::printF(Com::tI,I);
}
if(hasJ())
{
Com::printF(Com::tJ,J);
}
if(hasR())
{
Com::printF(Com::tR,R);
}
if(hasString())
{
Com::print(text);
}
Com::println();
}
/**
Converts a ascii GCode line into a GCode structure.
*/
bool GCode::parseAscii(char *line,bool fromSerial)
{
bool has_checksum = false;
char *pos;
params = 0;
params2 = 0;
if((pos = strchr(line,'N'))!=0) // Line number detected
{
actLineNumber = parseLongValue(++pos);
params |=1;
N = actLineNumber & 0xffff;
}
if((pos = strchr(line,'M'))!=0) // M command
{
M = parseLongValue(++pos) & 0xffff;
params |= 2;
if(M>255) params |= 4096;
}
if(hasM() && (M == 23 || M == 28 || M == 29 || M == 30 || M == 32 || M == 117))
{
// after M command we got a filename for sd card management
char *sp = line;
while(*sp!='M') sp++; // Search M command
while(*sp!=' ') sp++; // search next whitespace
while(*sp==' ') sp++; // skip leading whitespaces
text = sp;
while(*sp)
{
if((M != 117 && *sp==' ') || *sp=='*') break; // end of filename reached
sp++;
}
*sp = 0; // Removes checksum, but we don't care. Could also be part of the string.
waitUntilAllCommandsAreParsed = true; // don't risk string be deleted
params |= 32768;
}
else
{
if((pos = strchr(line,'G'))!=0) // G command
{
G = parseLongValue(++pos) & 0xffff;
params |= 4;
if(G>255) params |= 4096;
}
if((pos = strchr(line,'X'))!=0)
{
X = parseFloatValue(++pos);
params |= 8;
}
if((pos = strchr(line,'Y'))!=0)
{
Y = parseFloatValue(++pos);
params |= 16;
}
if((pos = strchr(line,'Z'))!=0)
{
Z = parseFloatValue(++pos);
params |= 32;
}
if((pos = strchr(line,'E'))!=0)
{
E = parseFloatValue(++pos);
params |= 64;
}
if((pos = strchr(line,'F'))!=0)
{
F = parseFloatValue(++pos);
params |= 256;
}
if((pos = strchr(line,'T'))!=0) // M command
{
T = parseLongValue(++pos) & 0xff;
params |= 512;
}
if((pos = strchr(line,'S'))!=0) // M command
{
S = parseLongValue(++pos);
params |= 1024;
}
if((pos = strchr(line,'P'))!=0) // M command
{
P = parseLongValue(++pos);
params |= 2048;
}
if((pos = strchr(line,'I'))!=0)
{
I = parseFloatValue(++pos);
params2 |= 1;
params |= 4096; // Needs V2 for saving
}
if((pos = strchr(line,'J'))!=0)
{
J = parseFloatValue(++pos);
params2 |= 2;
params |= 4096; // Needs V2 for saving
}
if((pos = strchr(line,'R'))!=0)
{
R = parseFloatValue(++pos);
params2 |= 4;
params |= 4096; // Needs V2 for saving
}
}
if((pos = strchr(line,'*'))!=0) // checksum
{
uint8_t checksum_given = parseLongValue(pos+1);
uint8_t checksum = 0;
while(line!=pos) checksum ^= *line++;
#if FEATURE_CHECKSUM_FORCED
Printer::flag0 |= PRINTER_FLAG0_FORCE_CHECKSUM;
#endif
if(checksum!=checksum_given)
{
if(Printer::debugErrors())
{
Com::printErrorFLN(Com::tWrongChecksum);
}
return false; // mismatch
}
}
#if FEATURE_CHECKSUM_FORCED
else
{
if(!fromSerial) return true;
if(hasM() && (M == 110 || hasString())) return true;
if(Printer::debugErrors())
{
Com::printErrorFLN(Com::tMissingChecksum);
}
return false;
}
#endif
if(hasFormatError() || (params & 518)==0) // Must contain G, M or T command and parameter need to have variables!
{
formatErrors++;
if(Printer::debugErrors())
Com::printErrorFLN(Com::tFormatError);
if(formatErrors<3) return false;
}
else formatErrors = 0;
return true;
}
/**
Converts a binary uint8_tfield containing one GCode line into a GCode structure.
Returns true if checksum was correct.
*/
bool GCode::parseBinary(uint8_t *buffer,bool fromSerial)
{
internalCommand = !fromSerial;
unsigned int sum1 = 0, sum2 = 0; // for fletcher-16 checksum
// first do fletcher-16 checksum tests see
// http://en.wikipedia.org/wiki/Fletcher's_checksum
uint8_t *p = buffer;
uint8_t len = binaryCommandSize - 2;
while (len)
{
uint8_t tlen = len > 21 ? 21 : len;
len -= tlen;
do
{
sum1 += *p++;
if(sum1 >= 255) sum1 -= 255;
sum2 += sum1;
if(sum2 >= 255) sum2 -= 255;
}
while (--tlen);
}
sum1 -= *p++;
sum2 -= *p;
if(sum1 | sum2)
{
if(Printer::debugErrors())
{
Com::printErrorFLN(Com::tWrongChecksum);
}
return false;
}
p = buffer;
params = *(uint16_t *)p;
p += 2;
uint8_t textlen = 16;
if(isV2())
{
params2 = *(uint16_t *)p;
p += 2;
if(hasString())
textlen = *p++;
}
else params2 = 0;
if(params & 1)
{
actLineNumber = N = *(uint16_t *)p;
p += 2;
}
if(isV2()) // Read G,M as 16 bit value
{
if(hasM())
{
M = *(uint16_t *)p;
p += 2;
}
if(hasG())
{
G = *(uint16_t *)p;
p += 2;
}
}
else
{
if(hasM())
{
M = *p++;
}
if(hasG())
{
G = *p++;
}
}
//if(code->params & 8) {memcpy(&code->X,p,4);p+=4;}
if(hasX())
{
X = *(float *)p;
p += 4;
}
if(hasY())
{
Y = *(float *)p;
p += 4;
}
if(hasZ())
{
Z = *(float *)p;
p += 4;
}
if(hasE())
{
E = *(float *)p;
p += 4;
}
if(hasF())
{
F = *(float *)p;
p += 4;
}
if(hasT())
{
T = *p++;
}
if(hasS())
{
S = *(int32_t*)p;
p += 4;
}
if(hasP())
{
P = *(int32_t*)p;
p += 4;
}
if(hasI())
{
I = *(float *)p;
p += 4;
}
if(hasJ())
{
J = *(float *)p;
p += 4;
}
if(hasR())
{
R = *(float *)p;
p += 4;
}
if(hasD())
{
D = *(float *)p;
p += 4;
}
if(hasC())
{
C = *(float *)p;
p += 4;
}
if(hasH())
{
H = *(float *)p;
p += 4;
}
if(hasA())
{
A = *(float *)p;
p += 4;
}
if(hasB())
{
B = *(float *)p;
p += 4;
}
if(hasK())
{
K = *(float *)p;
p += 4;
}
if(hasL())
{
L = *(float *)p;
p += 4;
}
if(hasO())
{
O = *(float *)p;
p += 4;
}
if(hasString()) // set text pointer to string
{
text = (char*)p;
text[textlen] = 0; // Terminate string overwriting checksum
waitUntilAllCommandsAreParsed = true; // Don't destroy string until executed
}
formatErrors = 0;
return true;
}
/**
Check if result is plausible. If it is, an ok is send and the command is stored in queue.
If not, a resend and ok is send.
*/
void GCode::checkAndPushCommand()
{
if(hasM())
{
if(M == 110) // Reset line number
{
lastLineNumber = actLineNumber;
Com::printFLN(Com::tOk);
waitingForResend = -1;
return;
}
if(M == 112) // Emergency kill - freeze printer
{
Commands::emergencyStop();
}
#ifdef DEBUG_COM_ERRORS
if(M == 666) // force an communication error
{
lastLineNumber++;
return;
} else if(M == 668) {
lastLineNumber = 0; // simulate a reset so lines are out of resend buffer
}
#endif // DEBUG_COM_ERRORS
}
if(hasN())
{
if((((lastLineNumber + 1) & 0xffff) != (actLineNumber & 0xffff)))
{
if(static_cast<uint16_t>(lastLineNumber - actLineNumber) < 40)
{
// we have seen that line already. So we assume it is a repeated resend and we ignore it
commandsReceivingWritePosition = 0;
Com::printFLN(Com::tSkip,actLineNumber);
Com::printFLN(Com::tOk);
}
else if(waitingForResend < 0) // after a resend, we have to skip the garbage in buffers, no message for this
{
if(Printer::debugErrors())
{
Com::printF(Com::tExpectedLine, lastLineNumber + 1);
Com::printFLN(Com::tGot, actLineNumber);
}
requestResend(); // Line missing, force resend
}
else
{
--waitingForResend;
commandsReceivingWritePosition = 0;
Com::printFLN(Com::tSkip, actLineNumber);
Com::printFLN(Com::tOk);
}
return;
}
lastLineNumber = actLineNumber;
} /*
This test is not compatible with all hosts. Replaced by forbidding backward switch of protocols.
else if(lastLineNumber && !(hasM() && M == 117)) { // once line number always line number!
if(Printer::debugErrors())
{
Com::printErrorFLN(PSTR("Missing linenumber"));
}
requestResend();
return;
}*/
if(GCode::hasFatalError() && !(hasM() && M==999)) {
GCode::reportFatalError();
} else {
pushCommand();
}
#ifdef DEBUG_COM_ERRORS
if(hasM() && M == 667)
return; // omit ok
#endif
#if ACK_WITH_LINENUMBER
Com::printFLN(Com::tOkSpace, actLineNumber);
#else
Com::printFLN(Com::tOk);
#endif
wasLastCommandReceivedAsBinary = sendAsBinary;
keepAlive(NotBusy);
waitingForResend = -1; // everything is ok.
}