From cb05c9dda6bb8da6dbf6bbbbc64e79b069d8b2a0 Mon Sep 17 00:00:00 2001 From: Michael Moon Date: Sat, 5 Feb 2011 18:39:35 +1100 Subject: big gcode_parse update- wait for whole line to ease processing, use floats to receive numerical data, plugs into existing gcode_process.c. TODO: revamp gcode_process to use new gcode_parse data structures --- func.sh | 32 ++++ gcode_parse.c | 563 +++++++++++++++++++++++----------------------------------- gcode_parse.h | 4 +- 3 files changed, 256 insertions(+), 343 deletions(-) diff --git a/func.sh b/func.sh index e59b7a5..18827e0 100755 --- a/func.sh +++ b/func.sh @@ -322,6 +322,38 @@ ENDPERL } # Read status of PID routines. +mendel_readsym_nexttarget() { + local val=$(mendel_readsym next_target) + perl - <<'ENDPERL' -- $val + $i = -1; + @a = qw/flags 2 G 1 M 1 X 4 Y 4 Z 4 E 4 F 4 S 2 P 2 T 1 N 4 eN 4 cr 1 cc 1/; + $c = 1234567; + while (length $ARGV[1]) { + if ($c > ($#a / 2)) { + $i++; + $c = 0; + } + if ($a[$c * 2 + 1] & 8) { + printf "\n"; + } + if (($a[$c * 2 + 1] & 7) == 4) { + $ARGV[1] =~ s#^(..)(..)(..)(..)##; + printf "%s: %d\t", $a[$c * 2], eval "0x$4$3$2$1"; + } + if (($a[$c * 2 + 1] & 7) == 2) { + $ARGV[1] =~ s#^(..)(..)##; + printf "%s: %d\t", $a[$c * 2], eval "0x$2$1"; + } + elsif (($a[$c * 2 + 1] & 7) == 1) { + $ARGV[1] =~ s#^(..)##; + printf "%s: %d\t", $a[$c * 2], eval "0x$1"; + } + $c++; + } + printf "\n"; +ENDPERL +} + mendel_heater_pid() { local P=$(mendel_readsym_int16 heater_p) local I=$(mendel_readsym_int16 heater_i) diff --git a/gcode_parse.c b/gcode_parse.c index 1eb4eb0..626b694 100644 --- a/gcode_parse.c +++ b/gcode_parse.c @@ -4,15 +4,12 @@ \brief Parse received G-Codes */ -#include +#include +#include "config.h" +#include "sersendf.h" #include "serial.h" #include "sermsg.h" -#include "dda_queue.h" -#include "debug.h" -#include "heater.h" -#include "sersendf.h" - #include "gcode_process.h" /// current or previous gcode word @@ -22,64 +19,232 @@ uint8_t last_field = 0; /// crude crc macro #define crc(a, b) (a ^ b) -/// crude floating point data storage -decfloat read_digit __attribute__ ((__section__ (".bss"))); - -/// this is where we store all the data for the current command before we work out what to do with it -GCODE_COMMAND next_target __attribute__ ((__section__ (".bss"))); - -/* - decfloat_to_int() is the weakest subject to variable overflow. For evaluation, we assume a build room of +-1000 mm and STEPS_PER_MM_x between 1.000 and 4096. Accordingly for metric units: - - df->mantissa: +-0..1048075 (20 bit - 500 for rounding) - df->exponent: 0, 2, 3, 4 or 5 (10 bit) - multiplicand: 1000 (10 bit) - - imperial units: - - df->mantissa: +-0..32267 (15 bit - 500 for rounding) - df->exponent: 0, 2, 3, 4 or 5 (10 bit) - multiplicand: 25400 (15 bit) -*/ -// decfloat_to_int() can handle a bit more: -#define DECFLOAT_EXP_MAX 3 // more is pointless, as 1 um is our presision -// (2^^32 - 1) / multiplicand - powers[DECFLOAT_EXP_MAX] / 2 = -// 4294967295 / 1000 - 5000 = -#define DECFLOAT_MANT_MM_MAX 4289967 // = 4290 mm -// 4294967295 / 25400 - 5000 = -#define DECFLOAT_MANT_IN_MAX 164093 // = 164 inches = 4160 mm +#define GCODE_LINE_BUFFER_LEN 64 + +#define iA 0 +#define iB 1 +#define iC 2 +#define iD 3 +#define iE 4 +#define iF 5 +#define iG 6 +#define iH 7 +#define iI 8 +#define iJ 9 +#define iK 10 +#define iL 11 +#define iM 12 +#define iN 13 +#define iO 14 +#define iP 15 +#define iQ 16 +#define iR 17 +#define iS 18 +#define iT 19 +#define iU 20 +#define iV 21 +#define iW 22 +#define iX 23 +#define iY 24 +#define iZ 25 +#define iAsterisk 26 + +GCODE_COMMAND next_target; + +uint8_t gcode_line[GCODE_LINE_BUFFER_LEN]; +uint8_t gcode_line_pointer = 0; + +float words[32]; +uint32_t seen_mask = 0; + +#define SEEN(c) (seen_mask & (1L << (c))) + +uint32_t line_number = 0; + +const uint8_t char2index(uint8_t c) __attribute__ ((pure)); +const uint8_t char2index(uint8_t c) { + if (c >= 'a' && c <= 'z') + return c - 'a'; + if (c >= 'A' && c <= 'Z') + return c - 'A'; + if (c == '*') + return 26; + return 255; +} -/* - utility functions -*/ -extern const uint32_t powers[]; // defined in sermsg.c +void gcode_parse_char(uint8_t c) { + if (gcode_line_pointer < (GCODE_LINE_BUFFER_LEN - 1)) + gcode_line[gcode_line_pointer++] = c; + if ((c == 13) || (c == 10)) { + uint8_t i; + for (i = gcode_line_pointer; i < GCODE_LINE_BUFFER_LEN; i++) + gcode_line[i] = 0; + if (gcode_line_pointer > 2) + gcode_parse_line(gcode_line); + gcode_line_pointer = 0; + } +} -/// convert a floating point input value into an integer with appropriate scaling. -/// \param *df pointer to floating point structure that holds fp value to convert -/// \param multiplicand multiply by this amount during conversion to integer -/// -/// Tested for up to 42'000 mm (accurate), 420'000 mm (precision 10 um) and -/// 4'200'000 mm (precision 100 um). -static int32_t decfloat_to_int(decfloat *df, uint16_t multiplicand) { - uint32_t r = df->mantissa; - uint8_t e = df->exponent; +void gcode_parse_line(uint8_t *c) { + enum { + STATE_FIND_WORD, + STATE_FIND_VALUE, + STATE_SEMICOLON_COMMENT, + STATE_BRACKET_COMMENT, + } state = STATE_FIND_WORD; + + uint8_t i; // string index + uint8_t w = 0; // current gcode word + uint8_t checksum = 0; + + seen_mask = 0; + + // calculate checksum + for(i = 0; c[i] != '*' && c[i] != 0; i++) + checksum = checksum ^ c[i]; + + // extract gcode words from line + for (i = 0; c[i] != 0 && c[i] != 13 && c[i] != 10; i++) { + switch (state) { + case STATE_FIND_WORD: + // start of word + if (char2index(c[i]) < 255) { + w = char2index(c[i]); + state = STATE_FIND_VALUE; + } + // comment until end of line + if (c[i] == ';') + state = STATE_SEMICOLON_COMMENT; + // comment until close bracket + if (c[i] == '(') + state = STATE_BRACKET_COMMENT; + break; + case STATE_FIND_VALUE: + if ((c[i] >= '0' && c[i] <= '9') || c[i] == '-') { + uint8_t *ep; + float v = strtod((const char *) &c[i], (char **) &ep); + state = STATE_FIND_WORD; + if (ep > &c[i]) { +// sersendf_P(PSTR("[seen %c: %lx->"), w + 'A', seen_mask); + seen_mask |= (1L << w); +// sersendf_P(PSTR("%lx]"), seen_mask); + words[w] = v; + i = ep - c - 1; + } + } + break; + case STATE_BRACKET_COMMENT: + if (c[i] == ')') + state = STATE_FIND_WORD; + break; + case STATE_SEMICOLON_COMMENT: + // dummy entry to suppress compiler warning + break; + } // switch (state) + } // for i=0 .. newline + + // TODO: process line just read + + if (SEEN(iAsterisk)) { + if (checksum != words[iAsterisk]) { + if (seen_mask & iAsterisk) + sersendf_P(PSTR("rs %d "), ((uint8_t) words[iAsterisk])); + sersendf_P(PSTR("Bad checksum, received %d, expected %d\n"), ((uint8_t) words[iAsterisk]), checksum); + seen_mask = 0; + return; + } + } + + if (SEEN(iN)) { + if (((uint32_t) words[iN]) != line_number) { + sersendf_P(PSTR("Bad line number, received %ld, expected %ld\n"), ((uint32_t) words[iN]), line_number); + seen_mask = 0; + return; + } + line_number++; + } + + serial_writestr_P(PSTR("ok ")); - // e=1 means we've seen a decimal point but no digits after it, and e=2 means we've seen a decimal point with one digit so it's too high by one if not zero - if (e) - e--; + // patch words into next_target struct + // TODO: eliminate next_target, update gcode_process to use words[] directly - // This raises range for mm by factor 1000 and for inches by factor 100. - // It's a bit expensive, but we should have the time while parsing. - while (e && multiplicand % 10 == 0) { - multiplicand /= 10; - e--; + next_target.flags = 0; + if (SEEN(iG)) { + next_target.seen_G = 1; + next_target.G = words[iG]; +// sersendf_P(PSTR("G:%d/"), next_target.G); + } + if (SEEN(iM)) { + next_target.seen_M = 1; + next_target.M = words[iM]; +// sersendf_P(PSTR("M:%d/"), next_target.M); + } + if (SEEN(iX)) { + next_target.seen_X = 1; + next_target.target.X = words[iX] * STEPS_PER_MM_X; +// sersendf_P(PSTR("X:%ld/"), next_target.target.X); + } + if (SEEN(iY)) { + next_target.seen_Y = 1; + next_target.target.Y = words[iY] * STEPS_PER_MM_Y; +// sersendf_P(PSTR("Y:%ld/"), next_target.target.Y); + } + if (SEEN(iZ)) { + next_target.seen_Z = 1; + next_target.target.Z = words[iZ] * STEPS_PER_MM_Z; +// sersendf_P(PSTR("Z:%ld/"), next_target.target.Z); + } + if (SEEN(iE)) { + next_target.seen_E = 1; + next_target.target.E = words[iE] * STEPS_PER_MM_E; +// sersendf_P(PSTR("E:%ld/"), next_target.target.E); + } + if (SEEN(iF)) { + next_target.seen_F = 1; + next_target.target.F = words[iF]; +// sersendf_P(PSTR("F:%ld/"), next_target.target.F); + } + if (SEEN(iS)) { + next_target.seen_S = 1; + // if this is temperature, multiply by 4 to convert to quarter-degree units + // cosmetically this should be done in the temperature section, + // but it takes less code, less memory and loses no precision if we do it here instead + if ((next_target.M == 104) || (next_target.M == 109)) + next_target.S = words[iS] * 4.0; + // if this is heater PID stuff, multiply by PID_SCALE because we divide by PID_SCALE later on + else if ((next_target.M >= 130) && (next_target.M <= 132)) + next_target.S = words[iS] * PID_SCALE; + else + next_target.S = words[iS]; +// sersendf_P(PSTR("S:%d/"), next_target.S); + } + if (SEEN(iP)) { + next_target.seen_P = 1; + next_target.P = words[iP]; +// sersendf_P(PSTR("P:%u/"), next_target.P); + } + if (SEEN(iT)) { + next_target.seen_T = 1; + next_target.T = words[iT]; +// sersendf_P(PSTR("T:%d/"), next_target.T); + } + if (SEEN(iN)) { + next_target.seen_N = 1; + next_target.N = words[iN]; +// sersendf_P(PSTR("N:%lu/"), next_target.N); } + next_target.N_expected = line_number; + if (SEEN(iAsterisk)) { + next_target.seen_checksum = 1; + next_target.checksum_read = words[iAsterisk]; + } + next_target.checksum_calculated = checksum; - r *= multiplicand; - if (e) - r = (r + powers[e] / 2) / powers[e]; + process_gcode_command(); + serial_writechar('\n'); - return df->sign ? -(int32_t)r : (int32_t)r; + seen_mask = 0; } void gcode_init(void) { @@ -94,290 +259,6 @@ void gcode_init(void) { #endif } -/// Character Received - add it to our command -/// \param c the next character to process -void gcode_parse_char(uint8_t c) { - uint8_t checksum_char = c; - - // uppercase - if (c >= 'a' && c <= 'z') - c &= ~32; - - // process previous field - if (last_field) { - // check if we're seeing a new field or end of line - // any character will start a new field, even invalid/unknown ones - if ((c >= 'A' && c <= 'Z') || c == '*' || (c == 10) || (c == 13)) { - switch (last_field) { - case 'G': - next_target.G = read_digit.mantissa; - if (DEBUG_ECHO && (debug_flags & DEBUG_ECHO)) - serwrite_uint8(next_target.G); - break; - case 'M': - next_target.M = read_digit.mantissa; - if (DEBUG_ECHO && (debug_flags & DEBUG_ECHO)) - serwrite_uint8(next_target.M); - break; - case 'X': - if (next_target.option_inches) - next_target.target.X = decfloat_to_int(&read_digit, 25400); - else - next_target.target.X = decfloat_to_int(&read_digit, 1000); - if (DEBUG_ECHO && (debug_flags & DEBUG_ECHO)) - serwrite_int32(next_target.target.X); - break; - case 'Y': - if (next_target.option_inches) - next_target.target.Y = decfloat_to_int(&read_digit, 25400); - else - next_target.target.Y = decfloat_to_int(&read_digit, 1000); - if (DEBUG_ECHO && (debug_flags & DEBUG_ECHO)) - serwrite_int32(next_target.target.Y); - break; - case 'Z': - if (next_target.option_inches) - next_target.target.Z = decfloat_to_int(&read_digit, 25400); - else - next_target.target.Z = decfloat_to_int(&read_digit, 1000); - if (DEBUG_ECHO && (debug_flags & DEBUG_ECHO)) - serwrite_int32(next_target.target.Z); - break; - case 'E': - if (next_target.option_inches) - next_target.target.E = decfloat_to_int(&read_digit, 25400); - else - next_target.target.E = decfloat_to_int(&read_digit, 1000); - if (DEBUG_ECHO && (debug_flags & DEBUG_ECHO)) - serwrite_uint32(next_target.target.E); - break; - case 'F': - // just use raw integer, we need move distance and n_steps to convert it to a useful value, so wait until we have those to convert it - if (next_target.option_inches) - next_target.target.F = decfloat_to_int(&read_digit, 25400); - else - next_target.target.F = decfloat_to_int(&read_digit, 1); - if (DEBUG_ECHO && (debug_flags & DEBUG_ECHO)) - serwrite_uint32(next_target.target.F); - break; - case 'S': - // if this is temperature, multiply by 4 to convert to quarter-degree units - // cosmetically this should be done in the temperature section, - // but it takes less code, less memory and loses no precision if we do it here instead - if ((next_target.M == 104) || (next_target.M == 109) || (next_target.M == 140)) - next_target.S = decfloat_to_int(&read_digit, 4); - // if this is heater PID stuff, multiply by PID_SCALE because we divide by PID_SCALE later on - else if ((next_target.M >= 130) && (next_target.M <= 132)) - next_target.S = decfloat_to_int(&read_digit, PID_SCALE); - else - next_target.S = decfloat_to_int(&read_digit, 1); - if (DEBUG_ECHO && (debug_flags & DEBUG_ECHO)) - serwrite_uint16(next_target.S); - break; - case 'P': - next_target.P = decfloat_to_int(&read_digit, 1); - if (DEBUG_ECHO && (debug_flags & DEBUG_ECHO)) - serwrite_uint16(next_target.P); - break; - case 'T': - next_target.T = read_digit.mantissa; - if (DEBUG_ECHO && (debug_flags & DEBUG_ECHO)) - serwrite_uint8(next_target.T); - break; - case 'N': - next_target.N = decfloat_to_int(&read_digit, 1); - if (DEBUG_ECHO && (debug_flags & DEBUG_ECHO)) - serwrite_uint32(next_target.N); - break; - case '*': - next_target.checksum_read = decfloat_to_int(&read_digit, 1); - if (DEBUG_ECHO && (debug_flags & DEBUG_ECHO)) - serwrite_uint8(next_target.checksum_read); - break; - } - // reset for next field - last_field = 0; - read_digit.sign = read_digit.mantissa = read_digit.exponent = 0; - } - } - - // skip comments - if (next_target.seen_semi_comment == 0 && next_target.seen_parens_comment == 0) { - // new field? - if ((c >= 'A' && c <= 'Z') || c == '*') { - last_field = c; - if (DEBUG_ECHO && (debug_flags & DEBUG_ECHO)) - serial_writechar(c); - } - - // process character - // Can't do ranges in switch..case, so process actual digits here. - // Do it early, as there are many more digits than characters expected. - if (c >= '0' && c <= '9') { - if (read_digit.exponent < DECFLOAT_EXP_MAX + 1 && - ((next_target.option_inches == 0 && - read_digit.mantissa < DECFLOAT_MANT_MM_MAX) || - (next_target.option_inches && - read_digit.mantissa < DECFLOAT_MANT_IN_MAX))) { - // this is simply mantissa = (mantissa * 10) + atoi(c) in different clothes - read_digit.mantissa = (read_digit.mantissa << 3) + - (read_digit.mantissa << 1) + (c - '0'); - if (read_digit.exponent) - read_digit.exponent++; - } - } - else { - switch (c) { - // Each currently known command is either G or M, so preserve - // previous G/M unless a new one has appeared. - // FIXME: same for T command - case 'G': - next_target.seen_G = 1; - next_target.seen_M = 0; - next_target.M = 0; - break; - case 'M': - next_target.seen_M = 1; - next_target.seen_G = 0; - next_target.G = 0; - break; - case 'X': - next_target.seen_X = 1; - break; - case 'Y': - next_target.seen_Y = 1; - break; - case 'Z': - next_target.seen_Z = 1; - break; - case 'E': - next_target.seen_E = 1; - break; - case 'F': - next_target.seen_F = 1; - break; - case 'S': - next_target.seen_S = 1; - break; - case 'P': - next_target.seen_P = 1; - break; - case 'T': - next_target.seen_T = 1; - break; - case 'N': - next_target.seen_N = 1; - break; - case '*': - next_target.seen_checksum = 1; - break; - - // comments - case ';': - next_target.seen_semi_comment = 1; - break; - case '(': - next_target.seen_parens_comment = 1; - break; - - // now for some numeracy - case '-': - read_digit.sign = 1; - // force sign to be at start of number, so 1-2 = -2 instead of -12 - read_digit.exponent = 0; - read_digit.mantissa = 0; - break; - case '.': - if (read_digit.exponent == 0) - read_digit.exponent = 1; - break; - #ifdef DEBUG - case ' ': - case '\t': - case 10: - case 13: - // ignore - break; - #endif - - default: - #ifdef DEBUG - // invalid - serial_writechar('?'); - serial_writechar(c); - serial_writechar('?'); - #endif - break; - } - } - } else if ( next_target.seen_parens_comment == 1 && c == ')') - next_target.seen_parens_comment = 0; // recognize stuff after a (comment) - - if (next_target.seen_checksum == 0) - next_target.checksum_calculated = - crc(next_target.checksum_calculated, checksum_char); - - // end of line - if ((c == 10) || (c == 13)) { - if (DEBUG_ECHO && (debug_flags & DEBUG_ECHO)) - serial_writechar(c); - - if ( - #ifdef REQUIRE_LINENUMBER - ((next_target.N >= next_target.N_expected) && (next_target.seen_N == 1)) || - (next_target.seen_M && (next_target.M == 110)) - #else - 1 - #endif - ) { - if ( - #ifdef REQUIRE_CHECKSUM - ((next_target.checksum_calculated == next_target.checksum_read) && (next_target.seen_checksum == 1)) - #else - ((next_target.checksum_calculated == next_target.checksum_read) || (next_target.seen_checksum == 0)) - #endif - ) { - // process - serial_writestr_P(PSTR("ok ")); - process_gcode_command(); - serial_writechar('\n'); - - // expect next line number - if (next_target.seen_N == 1) - next_target.N_expected = next_target.N + 1; - } - else { - sersendf_P(PSTR("rs N%ld Expected checksum %d\n"), next_target.N_expected, next_target.checksum_calculated); -// request_resend(); - } - } - else { - sersendf_P(PSTR("rs N%ld Expected line number %ld\n"), next_target.N_expected, next_target.N_expected); -// request_resend(); - } - - // reset variables - next_target.seen_X = next_target.seen_Y = next_target.seen_Z = \ - next_target.seen_E = next_target.seen_F = next_target.seen_S = \ - next_target.seen_P = next_target.seen_T = next_target.seen_N = \ - next_target.seen_M = next_target.seen_checksum = next_target.seen_semi_comment = \ - next_target.seen_parens_comment = next_target.checksum_read = \ - next_target.checksum_calculated = 0; - // last_field and read_digit are reset above already - - // assume a G1 by default - next_target.seen_G = 1; - next_target.G = 1; - - if (next_target.option_all_relative) { - next_target.target.X = next_target.target.Y = next_target.target.Z = 0; - } - if (next_target.option_all_relative || next_target.option_e_relative) { - next_target.target.E = 0; - } - } -} - /***************************************************************************\ * * * Request a resend of the current line - used from various places. * diff --git a/gcode_parse.h b/gcode_parse.h index 825bbbc..73e44c1 100644 --- a/gcode_parse.h +++ b/gcode_parse.h @@ -64,8 +64,8 @@ extern GCODE_COMMAND next_target; void gcode_init(void); -/// accept the next character and process it -void gcode_parse_char(uint8_t c); +// once we have a whole line, process it +void gcode_parse_line(uint8_t *c); // uses the global variable next_target.N void request_resend(void); -- 1.8.1.2