void sersendf_P(PGM_P format, ...) {
va_list args;
va_start(args, format);
uint16_t i = 0;
uint8_t c = 1, j = 0;
while ((c = pgm_read_byte(&format[i++]))) {
if (j) {
switch(c) {
case 's':
j = 1;
break;
case 'l':
j = 4;
break;
case 'u':
if (j == 4)
serwrite_uint32(va_arg(args, uint32_t));
else
serwrite_uint16(va_arg(args, unsigned int));
j = 0;
break;
case 'd':
if (j == 4)
serwrite_int32(va_arg(args, int32_t));
else
serwrite_int16(va_arg(args, int));
j = 0;
break;
/* case 'x':
serial_writestr_P(str_ox);
if (j == 4)
serwrite_hex32(va_arg(args, uint32_t));
else if (j == 1)
serwrite_hex8(va_arg(args, uint16_t));
else
serwrite_hex16(va_arg(args, uint16_t));
j = 0;
break;
case 'c':
serial_writechar(va_arg(args, uint16_t));
case 'p':
serwrite_hex16(va_arg(args, uint16_t));
default:
j = 0;
break;*/
}
}
else {
if (c == '%') {
j = 2;
}
else {
serial_writechar(c);
}
}
}
va_end(args);
}
void sersendf(char *format, ...) {
va_list args;
va_start(args, format);
uint16_t i = 0;
uint8_t c, j = 0;
while ((c = format[i++])) {
if (j) {
switch(c) {
case 'l':
j = 4;
case 'u':
if (j == 4)
serwrite_uint32(va_arg(args, uint32_t));
else
serwrite_uint16(va_arg(args, unsigned int));
j = 0;
break;
case 'd':
if (j == 4)
serwrite_int32(va_arg(args, int32_t));
else
serwrite_int16(va_arg(args, int));
j = 0;
break;
case 'p':
case 'x':
serial_writestr_P(str_ox);
if (j == 4)
serwrite_hex32(va_arg(args, uint32_t));
else
serwrite_hex16(va_arg(args, unsigned int));
j = 0;
break;
case 'c':
serial_writechar(va_arg(args, unsigned int));
j = 0;
break;
case 's':
serial_writestr(va_arg(args, uint8_t *));
j = 0;
break;
default:
j = 0;
break;
}
}
else {
if (c == '%') {
j = 2;
}
else {
serial_writechar(c);
}
}
}
va_end(args);
}
void read_device_serial_number(void)
{
param_table[0] = READ_DEVICE_SERIAL;
IAP_entry(param_table,result_table);
if(result_table[0] != CMD_SUCCESS)
{
serial_writestr("Error: reading serial ");
}
else
{
serwrite_uint32(result_table[1]);
serial_writestr(" ");
serwrite_uint32(result_table[2]);
serial_writestr(" ");
serwrite_uint32(result_table[3]);
serial_writestr(" ");
serwrite_uint32(result_table[4]);
serial_writestr(" ");
}
}
eParseResult gcode_parse_line (tLineBuffer *pLine)
{
int j;
eParseResult result = PR_OK;
for (j=0; j < pLine->len; j++)
{
gcode_parse_char (pLine->data [j]);
}
// end of line
//if ((c == 10) || (c == 13))
{
if (
#ifdef REQUIRE_LINENUMBER
(next_target.N >= next_target.N_expected) && (next_target.seen_N == 1)
#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
)
{
if (sd_writing_file)
{
if (next_target.seen_M && (next_target.M >= 20) && (next_target.M <= 29))
{
if (next_target.seen_M && next_target.M == 29)
{
// M29 - stop writing
sd_writing_file = false;
sd_close (&file);
serial_writestr("Done saving file\r\n");
}
else
{
// else - do not write SD M-codes to file
serial_writestr("ok\r\n");
}
}
else
{
// lines in files must be LF terminated for sd_read_file to work
if (pLine->data [pLine->len-1] == 13)
{
pLine->data [pLine->len-1] = 10;
}
if (sd_write_to_file(pLine->data, pLine->len))
{
serial_writestr("ok\r\n");
}
else
{
serial_writestr("error writing to file\r\n");
}
}
}
else
{
// process
result = process_gcode_command();
// expect next line number
if (next_target.seen_N == 1)
{
next_target.N_expected = next_target.N + 1;
}
}
}
else
{
serial_writestr("Expected checksum ");
serwrite_uint8(next_target.checksum_calculated);
serial_writestr("\r\n");
request_resend();
}
}
else
{
serial_writestr("Expected line number ");
serwrite_uint32(next_target.N_expected);
serial_writestr("\r\n");
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_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;
next_target.chpos = 0;
last_field = 0;
read_digit.sign = read_digit.exponent = 0;
value = 0;
// dont assume a G1 by default
next_target.seen_G = 0;
next_target.G = 0;
if (next_target.option_relative)
{
next_target.target.x = next_target.target.y = next_target.target.z = 0.0;
next_target.target.e = 0.0;
}
}
return result;
}
void dda_create(DDA *dda, TARGET *target) {
uint32_t distance, c_limit, c_limit_calc;
// initialise DDA to a known state
dda->allflags = 0;
if (debug_flags & DEBUG_DDA)
serial_writestr_P(PSTR("\n{DDA_CREATE: ["));
// we end at the passed target
memcpy(&(dda->endpoint), target, sizeof(TARGET));
dda->x_delta = ABS(target->X - startpoint.X);
dda->y_delta = ABS(target->Y - startpoint.Y);
dda->z_delta = ABS(target->Z - startpoint.Z);
dda->e_delta = ABS(target->E - startpoint.E);
dda->x_direction = (target->X >= startpoint.X)?1:0;
dda->y_direction = (target->Y >= startpoint.Y)?1:0;
dda->z_direction = (target->Z >= startpoint.Z)?1:0;
dda->e_direction = (target->E >= startpoint.E)?1:0;
if (debug_flags & DEBUG_DDA) {
if (dda->x_direction == 0)
serial_writechar('-');
serwrite_uint32(dda->x_delta);
serial_writechar(',');
if (dda->y_direction == 0)
serial_writechar('-');
serwrite_uint32(dda->y_delta);
serial_writechar(',');
if (dda->z_direction == 0)
serial_writechar('-');
serwrite_uint32(dda->z_delta);
serial_writechar(',');
if (dda->e_direction == 0)
serial_writechar('-');
serwrite_uint32(dda->e_delta);
serial_writestr_P(PSTR("] ["));
}
dda->total_steps = dda->x_delta;
if (dda->y_delta > dda->total_steps)
dda->total_steps = dda->y_delta;
if (dda->z_delta > dda->total_steps)
dda->total_steps = dda->z_delta;
if (dda->e_delta > dda->total_steps)
dda->total_steps = dda->e_delta;
if (debug_flags & DEBUG_DDA) {
serial_writestr_P(PSTR("ts:"));
serwrite_uint32(dda->total_steps);
}
if (dda->total_steps == 0) {
dda->nullmove = 1;
}
else {
// get steppers ready to go
steptimeout = 0;
power_on();
dda->x_counter = dda->y_counter = dda->z_counter = dda->e_counter =
-(dda->total_steps >> 1);
// since it's unusual to combine X, Y and Z changes in a single move on reprap, check if we can use simpler approximations before trying the full 3d approximation.
if (dda->z_delta == 0)
distance = approx_distance(dda->x_delta * UM_PER_STEP_X, dda->y_delta * UM_PER_STEP_Y);
else if (dda->x_delta == 0 && dda->y_delta == 0)
distance = dda->z_delta * UM_PER_STEP_Z;
else
distance = approx_distance_3(dda->x_delta * UM_PER_STEP_X, dda->y_delta * UM_PER_STEP_Y, dda->z_delta * UM_PER_STEP_Z);
if (distance < 2)
distance = dda->e_delta * UM_PER_STEP_E;
if (debug_flags & DEBUG_DDA) {
serial_writestr_P(PSTR(",ds:"));
serwrite_uint32(distance);
}
// pre-calculate move speed in millimeter microseconds per step minute for less math in interrupt context
// mm (distance) * 60000000 us/min / step (total_steps) = mm.us per step.min
// note: um (distance) * 60000 == mm * 60000000
// so in the interrupt we must simply calculate
// mm.us per step.min / mm per min (F) = us per step
// break this calculation up a bit and lose some precision because 300,000um * 60000 is too big for a uint32
// calculate this with a uint64 if you need the precision, but it'll take longer so routines with lots of short moves may suffer
// 2^32/6000 is about 715mm which should be plenty
// changed * 10 to * (F_CPU / 100000) so we can work in cpu_ticks rather than microseconds.
// timer.c setTimer() routine altered for same reason
// changed distance * 6000 .. * F_CPU / 100000 to
// distance * 2400 .. * F_CPU / 40000 so we can move a distance of up to 1800mm without overflowing
uint32_t move_duration = ((distance * 2400) / dda->total_steps) * (F_CPU / 40000);
// similarly, find out how fast we can run our axes.
// do this for each axis individually, as the combined speed of two or more axes can be higher than the capabilities of a single one.
c_limit = 0;
c_limit_calc = ( (dda->x_delta * (UM_PER_STEP_X * 2400L)) / dda->total_steps * (F_CPU / 40000) / MAXIMUM_FEEDRATE_X) << 8;
if (c_limit_calc > c_limit)
c_limit = c_limit_calc;
c_limit_calc = ( (dda->y_delta * (UM_PER_STEP_Y * 2400L)) / dda->total_steps * (F_CPU / 40000) / MAXIMUM_FEEDRATE_Y) << 8;
if (c_limit_calc > c_limit)
c_limit = c_limit_calc;
c_limit_calc = ( (dda->z_delta * (UM_PER_STEP_Z * 2400L)) / dda->total_steps * (F_CPU / 40000) / MAXIMUM_FEEDRATE_Z) << 8;
if (c_limit_calc > c_limit)
c_limit = c_limit_calc;
c_limit_calc = ( (dda->e_delta * (UM_PER_STEP_E * 2400L)) / dda->total_steps * (F_CPU / 40000) / MAXIMUM_FEEDRATE_E) << 8;
if (c_limit_calc > c_limit)
c_limit = c_limit_calc;
#ifdef ACCELERATION_REPRAP
// c is initial step time in IOclk ticks
dda->c = (move_duration / startpoint.F) << 8;
if (dda->c < c_limit)
dda->c = c_limit;
dda->end_c = (move_duration / target->F) << 8;
if (dda->end_c < c_limit)
dda->end_c = c_limit;
if (debug_flags & DEBUG_DDA) {
serial_writestr_P(PSTR(",md:"));
serwrite_uint32(move_duration);
serial_writestr_P(PSTR(",c:"));
serwrite_uint32(dda->c >> 8);
}
if (dda->c != dda->end_c) {
uint32_t stF = startpoint.F / 4;
uint32_t enF = target->F / 4;
// now some constant acceleration stuff, courtesy of http://www.embedded.com/columns/technicalinsights/56800129?printable=true
uint32_t ssq = (stF * stF);
uint32_t esq = (enF * enF);
int32_t dsq = (int32_t) (esq - ssq) / 4;
uint8_t msb_ssq = msbloc(ssq);
uint8_t msb_tot = msbloc(dda->total_steps);
// the raw equation WILL overflow at high step rates, but 64 bit math routines take waay too much space
// at 65536 mm/min (1092mm/s), ssq/esq overflows, and dsq is also close to overflowing if esq/ssq is small
// but if ssq-esq is small, ssq/dsq is only a few bits
// we'll have to do it a few different ways depending on the msb locations of each
if ((msb_tot + msb_ssq) <= 30) {
// we have room to do all the multiplies first
if (debug_flags & DEBUG_DDA)
serial_writechar('A');
dda->n = ((int32_t) (dda->total_steps * ssq) / dsq) + 1;
}
else if (msb_tot >= msb_ssq) {
// total steps has more precision
if (debug_flags & DEBUG_DDA)
serial_writechar('B');
dda->n = (((int32_t) dda->total_steps / dsq) * (int32_t) ssq) + 1;
}
else {
// otherwise
if (debug_flags & DEBUG_DDA)
serial_writechar('C');
dda->n = (((int32_t) ssq / dsq) * (int32_t) dda->total_steps) + 1;
}
if (debug_flags & DEBUG_DDA) {
sersendf_P(PSTR("\n{DDA:CA end_c:%lu, n:%ld, md:%lu, ssq:%lu, esq:%lu, dsq:%lu, msbssq:%u, msbtot:%u}\n"),
(long unsigned int)dda->end_c >> 8,
(long int)dda->n,
(long unsigned int)move_duration,
(long unsigned int)ssq,
(long unsigned int)esq,
(long unsigned int)dsq,
msb_ssq,
msb_tot);
}
/// Character Received - add it to our command
/// \param c the next character to process
void gcode_parse_char(uint8_t 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, STEPS_PER_IN_X, 0);
else
next_target.target.X = decfloat_to_int(&read_digit, STEPS_PER_M_X, 1);
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, STEPS_PER_IN_Y, 0);
else
next_target.target.Y = decfloat_to_int(&read_digit, STEPS_PER_M_Y, 1);
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, STEPS_PER_IN_Z, 0);
else
next_target.target.Z = decfloat_to_int(&read_digit, STEPS_PER_M_Z, 1);
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, STEPS_PER_IN_E, 0);
else
next_target.target.E = decfloat_to_int(&read_digit, STEPS_PER_M_E, 1);
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, 1);
else
next_target.target.F = decfloat_to_int(&read_digit, 1, 0);
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, 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 = decfloat_to_int(&read_digit, PID_SCALE, 0);
else
next_target.S = decfloat_to_int(&read_digit, 1, 0);
if (DEBUG_ECHO && (debug_flags & DEBUG_ECHO))
serwrite_uint16(next_target.S);
break;
case 'P':
next_target.P = decfloat_to_int(&read_digit, 1, 0);
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, 0);
if (DEBUG_ECHO && (debug_flags & DEBUG_ECHO))
serwrite_uint32(next_target.N);
break;
case '*':
next_target.checksum_read = decfloat_to_int(&read_digit, 1, 0);
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
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:
// can't do ranges in switch..case, so process actual digits here.
if (c >= '0' && c <= '9') {
if (read_digit.exponent < DECFLOAT_EXP_MAX &&
((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++;
}
}
#ifdef DEBUG
else {
// invalid
serial_writechar('?');
serial_writechar(c);
serial_writechar('?');
}
#endif
}
} 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, c);
// 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
#ifdef DEBUG
sersendf_P(PSTR("received line %lu\n"),next_target.N);
#endif
serial_writestr_P(PSTR("ok "));
process_gcode_command();
serial_writechar('\n');
#ifdef DEBUG
print_queue();
serial_writechar('\n');
#endif
// 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_relative) {
next_target.target.X = next_target.target.Y = next_target.target.Z = 0;
#ifdef E_ABSOLUTE
next_target.target.E = 0;
#endif
}
#ifndef E_ABSOLUTE
// E always relative
next_target.target.E = 0;
#endif
}
}
/** Character received - add it to our command.
\param c The next character to process.
\return Whether end of line was reached.
This parser operates character by character, so there's no need for a
buffer holding the entire line of G-code.
*/
uint8_t gcode_parse_char(uint8_t c) {
uint8_t checksum_char = c;
// uppercase
if (c >= 'a' && c <= 'z')
c &= ~32;
#ifdef SIMULATOR
sim_gcode_ch(c);
#endif
// An asterisk is a quasi-EOL and always ends all fields.
if (c == '*') {
next_target.seen_semi_comment = next_target.seen_parens_comment =
next_target.read_string = 0;
}
// Skip comments and strings.
if (next_target.seen_semi_comment == 0 &&
next_target.seen_parens_comment == 0 &&
next_target.read_string == 0
) {
// Check if the field has ended. Either by a new field, space or EOL.
if (last_field && (c < '0' || c > '9') && c != '.') {
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;
#ifdef SD
if (next_target.M == 23) {
// SD card command with a filename.
next_target.read_string = 1; // Reset by string handler or EOL.
str_buf_ptr = 0;
last_field = 0;
}
#endif
if (DEBUG_ECHO && (debug_flags & DEBUG_ECHO))
serwrite_uint8(next_target.M);
break;
case 'X':
if (next_target.option_inches)
next_target.target.axis[X] = decfloat_to_int(&read_digit, 25400);
else
next_target.target.axis[X] = decfloat_to_int(&read_digit, 1000);
if (DEBUG_ECHO && (debug_flags & DEBUG_ECHO))
serwrite_int32(next_target.target.axis[X]);
break;
case 'Y':
if (next_target.option_inches)
next_target.target.axis[Y] = decfloat_to_int(&read_digit, 25400);
else
next_target.target.axis[Y] = decfloat_to_int(&read_digit, 1000);
if (DEBUG_ECHO && (debug_flags & DEBUG_ECHO))
serwrite_int32(next_target.target.axis[Y]);
break;
case 'Z':
if (next_target.option_inches)
next_target.target.axis[Z] = decfloat_to_int(&read_digit, 25400);
else
next_target.target.axis[Z] = decfloat_to_int(&read_digit, 1000);
if (DEBUG_ECHO && (debug_flags & DEBUG_ECHO))
serwrite_int32(next_target.target.axis[Z]);
break;
case 'E':
if (next_target.option_inches)
next_target.target.axis[E] = decfloat_to_int(&read_digit, 25400);
else
next_target.target.axis[E] = decfloat_to_int(&read_digit, 1000);
if (DEBUG_ECHO && (debug_flags & DEBUG_ECHO))
serwrite_int32(next_target.target.axis[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_int32(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;
}
}
// new field?
if ((c >= 'A' && c <= 'Z') || c == '*') {
last_field = c;
read_digit.sign = read_digit.mantissa = read_digit.exponent = 0;
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; // Reset by EOL.
break;
case '(':
next_target.seen_parens_comment = 1; // Reset by ')' or EOL.
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);
// Assume G1 for unspecified movements.
if ( ! next_target.seen_G && ! next_target.seen_M && ! next_target.seen_T &&
(next_target.seen_X || next_target.seen_Y || next_target.seen_Z ||
next_target.seen_E || next_target.seen_F)) {
next_target.seen_G = 1;
next_target.G = 1;
}
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
process_gcode_command();
// Acknowledgement ("ok") is sent in the main loop, in mendel.c.
// 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_G = next_target.seen_M = next_target.seen_checksum = \
next_target.seen_semi_comment = next_target.seen_parens_comment = \
next_target.read_string = next_target.checksum_read = \
next_target.checksum_calculated = 0;
last_field = 0;
read_digit.sign = read_digit.mantissa = read_digit.exponent = 0;
if (next_target.option_all_relative) {
next_target.target.axis[X] = next_target.target.axis[Y] = next_target.target.axis[Z] = 0;
}
if (next_target.option_all_relative || next_target.option_e_relative) {
next_target.target.axis[E] = 0;
}
return 1;
}
#ifdef SD
// Handle string reading. After checking for EOL.
if (next_target.read_string) {
if (c == ' ') {
if (str_buf_ptr)
next_target.read_string = 0;
}
else if (str_buf_ptr < STR_BUF_LEN) {
gcode_str_buf[str_buf_ptr] = c;
str_buf_ptr++;
gcode_str_buf[str_buf_ptr] = '\0';
}
}
#endif /* SD */
return 0;
}
