add client thread. Fix delay_ms bug ...

2022-01-19 18:05:16 +01:00
parent 450f553699
commit 3f9eb4ea85
7 changed files with 4944 additions and 5081 deletions
--- a/ecmc_plugin_grbl/ecmcPluginGrbl.c
+++ b/ecmc_plugin_grbl/ecmcPluginGrbl.c
@@ -41,6 +41,17 @@ static int    alreadyLoaded   = 0;
 int initDone = 0;
 pthread_t tid;

+
+void *ecmc_grbl_client_simulation_thread(void *ptr) {
+  printf("%s:%s:%d\n",__FILE__,__FUNCTION__,__LINE__);
+  for(;;) {
+    while(serial_get_tx_buffer_count()==0) {
+      delay_ms(1);      
+    }
+    printf("%c",ecmc_get_char_from_grbl_tx_buffer());
+  }
+}
+
 // copied for grbl main.c
 void *ecmc_grbl_main_thread(void *ptr) {
  printf("%s:%s:%d\n",__FILE__,__FUNCTION__,__LINE__);
@@ -127,17 +138,31 @@ int grblConstruct(char *configStr)
  // create SocketCAN object and register data callback
  lastConfStr = strdup(configStr);

+  // start grbl simulated client thread!
+  int err;
+
+  err = pthread_create(&(tid), NULL, *ecmc_grbl_client_simulation_thread, NULL);
+  if (err != 0) {
+    printf("\n Can't create thread :[%s]", strerror(err));
+    return 1;
+  }
+  else {
+    printf("\n grbl simulated client thread created successfully\n");
+  }
+  
  // start grbl main thread and wait for init done!
  printf("%s:%s:%d\n",__FILE__,__FUNCTION__,__LINE__);
-  int err;
+  
  err = pthread_create(&(tid), NULL, *ecmc_grbl_main_thread, NULL);
  if (err != 0) {
-      printf("\n Can't create thread :[%s]", strerror(err));
-      return 1;
+    printf("\n Can't create thread :[%s]", strerror(err));
+    return 1;
+  }
+  else {
+    printf("\n grbl main thread created successfully\n");
  }
-  else
-      printf("\n grbl main thread created successfully\n");
  
+
  // whait for initDone!
  printf("Waiting for grbl init..");
  while(!initDone) {      
@@ -150,38 +175,38 @@ int grblConstruct(char *configStr)
  sleep(1);

  // test some commands
-  printf("Test command:G0X11\n");
+  printf("Test command:G0X10Y100\n");
  ecmc_write_command_serial("G0X10Y100\n");
-  printf("Test command:$J=X10.0Y-1.5\n");
-  ecmc_write_command_serial("$J=X10.0Y-1.5\n");
-  printf("Test command:#\n");
-  ecmc_write_command_serial("#\n");
-  printf("Test command:?\n");
-  ecmc_write_command_serial("?\n");
-  printf("Test command:G1X200Y100\n");
-  ecmc_write_command_serial("G1X200Y100\n");
+  //printf("Test command:$J=X10.0Y-1.5\n");
+  //ecmc_write_command_serial("$J=X10.0Y-1.5\0");
+  //printf("Test command:#\n");
+  //ecmc_write_command_serial("#\n");
+  //printf("Test command:?\n");
+  //ecmc_write_command_serial("?\0");
+  //printf("Test command:G1X200Y100\n");
+  //ecmc_write_command_serial("G1X200Y100\0");
  return 0;

-  printf("system_execute_line(G0 X11), %d \n ",system_execute_line("G0X11\0"));
-  printf("end\n");
-  printf("system_execute_line(G1X200Y100), %d \n",system_execute_line("G1X200Y100\0"));
-  printf("end\n");
-  printf("system_execute_line($$), %d \n",system_execute_line("$$\0"));
-  printf("end\n");
-  printf("system_execute_line($), %d \n",system_execute_line("$\0"));
-  printf("end\n");
-  printf("system_execute_line(#), %d \n",system_execute_line("#\0"));
-  printf("end\n");
-  printf("system_execute_line(?), %d \n",system_execute_line("?\0"));
-  printf("end\n");
-  printf("system_execute_line($J=X10.0Y-1.5), %d \n",system_execute_line("$J=X10.0Y-1.5\0"));
-  printf("end\n");
-  printf("gc_execute_line(G0 X100.25), %d \n",gc_execute_line("G0 X100.25\0"));
-  printf("end\n");
-  printf("gc_execute_line(G1X200Y100), %d \n",gc_execute_line("G1X200Y100\0"));
-  printf("end\n");
-  
-  return 0; //createSocketCAN(configStr,getEcmcSampleTimeMS());
+//  printf("system_execute_line(G0 X11), %d \n ",system_execute_line("G0X11\0"));
+//  printf("end\n");
+//  printf("system_execute_line(G1X200Y100), %d \n",system_execute_line("G1X200Y100\0"));
+//  printf("end\n");
+//  printf("system_execute_line($$), %d \n",system_execute_line("$$\0"));
+//  printf("end\n");
+//  printf("system_execute_line($), %d \n",system_execute_line("$\0"));
+//  printf("end\n");
+//  printf("system_execute_line(#), %d \n",system_execute_line("#\0"));
+//  printf("end\n");
+//  printf("system_execute_line(?), %d \n",system_execute_line("?\0"));
+//  printf("end\n");
+//  printf("system_execute_line($J=X10.0Y-1.5), %d \n",system_execute_line("$J=X10.0Y-1.5\0"));
+//  printf("end\n");
+//  printf("gc_execute_line(G0 X100.25), %d \n",gc_execute_line("G0 X100.25\0"));
+//  printf("end\n");
+//  printf("gc_execute_line(G1X200Y100), %d \n",gc_execute_line("G1X200Y100\0"));
+//  printf("end\n");
+//  
+//  return 0; //createSocketCAN(configStr,getEcmcSampleTimeMS());
 }

 /** Optional function.
--- a/grbl/nuts_bolts.c
+++ b/grbl/nuts_bolts.c
@@ -143,7 +143,7 @@ void delay_ms(uint16_t ms)
  clock_gettime(CLOCK_MONOTONIC,&deadline);
  deadline.tv_nsec+=1E6*ms;
  if(deadline.tv_nsec>=1E9) {
-    deadline.tv_nsec-=1E6;
+    deadline.tv_nsec-=1E9;
    deadline.tv_sec++;
  }
  clock_nanosleep(CLOCK_MONOTONIC,TIMER_ABSTIME, &deadline,NULL);
@@ -161,7 +161,7 @@ void delay_us(uint32_t us)
  clock_gettime(CLOCK_MONOTONIC,&deadline);
  deadline.tv_nsec+=1E3*us;
  if(deadline.tv_nsec>=1E9) {
-    deadline.tv_nsec-=1E6;
+    deadline.tv_nsec-=1E9;
    deadline.tv_sec++;
  }
  clock_nanosleep(CLOCK_MONOTONIC,TIMER_ABSTIME, &deadline,NULL);
--- a/grbl/print.c
+++ b/grbl/print.c
@@ -24,71 +24,70 @@

 void printString(const char *s)
 {
-  printf("%s:%s:%d:%s\n",__FILE__,__FUNCTION__,__LINE__,s);
+  //printf("%s:%s:%d:%s\n",__FILE__,__FUNCTION__,__LINE__,s);
   
-  //while (*s)
-  //  serial_write(*s++);
+  while (*s)
+    serial_write(*s++);
 }


 // Print a string stored in PGM-memory
 void printPgmString(const char *s)
 {
-  printf("%s:%s:%d: %s\n",__FILE__,__FUNCTION__,__LINE__,s);
-//  char c;
-//  while ((c = pgm_read_byte_near(s++)))
-//    serial_write(c);
+  //printf("%s:%s:%d: %s\n",__FILE__,__FUNCTION__,__LINE__,s);
+  char c;
+  while ((*s)) {
+    c = s++[0];
+    serial_write(c);
+  }
 }


-// void printIntegerInBase(unsigned long n, unsigned long base)
-// {
-// 	unsigned char buf[8 * sizeof(long)]; // Assumes 8-bit chars.
-// 	unsigned long i = 0;
-//
-// 	if (n == 0) {
-// 		serial_write('0');
-// 		return;
-// 	}
-//
-// 	while (n > 0) {
-// 		buf[i++] = n % base;
-// 		n /= base;
-// 	}
-//
-// 	for (; i > 0; i--)
-// 		serial_write(buf[i - 1] < 10 ?
-// 			'0' + buf[i - 1] :
-// 			'A' + buf[i - 1] - 10);
-// }
+void printIntegerInBase(unsigned long n, unsigned long base)
+ {
+ 	unsigned char buf[8 * sizeof(long)]; // Assumes 8-bit chars.
+ 	unsigned long i = 0;
+
+if (n == 0) {
+ 		serial_write('0');
+ 		return;
+ 	}
+
+ 	while (n > 0) {
+ 		buf[i++] = n % base;
+ 		n /= base;
+ 	}
+
+ 	for (; i > 0; i--)
+ 		serial_write(buf[i - 1] < 10 ?
+ 			'0' + buf[i - 1] :
+ 			'A' + buf[i - 1] - 10);
+ }


 // Prints an uint8 variable in base 10.
 void print_uint8_base10(uint8_t n)
 {
-  printf("%s:%s:%d:%d\n",__FILE__,__FUNCTION__,__LINE__,n);
-  
-  //uint8_t digit_a = 0;
-  //uint8_t digit_b = 0;
-  //if (n >= 100) { // 100-255
-  //  digit_a = '0' + n % 10;
-  //  n /= 10;
-  //}
-  //if (n >= 10) { // 10-99
-  //  digit_b = '0' + n % 10;
-  //  n /= 10;
-  //}
-  //serial_write('0' + n);
-  //if (digit_b) { serial_write(digit_b); }
-  //if (digit_a) { serial_write(digit_a); }
+  //printf("%s:%s:%d:%d\n",__FILE__,__FUNCTION__,__LINE__,n);
+  uint8_t digit_a = 0;
+  uint8_t digit_b = 0;
+  if (n >= 100) { // 100-255
+    digit_a = '0' + n % 10;
+    n /= 10;
+  }
+  if (n >= 10) { // 10-99
+    digit_b = '0' + n % 10;
+    n /= 10;
+  }
+  serial_write('0' + n);
+  if (digit_b) { serial_write(digit_b); }
+  if (digit_a) { serial_write(digit_a); }
 }


 // Prints an uint8 variable in base 2 with desired number of desired digits.
 void print_uint8_base2_ndigit(uint8_t n, uint8_t digits) {
-
-
-  printf("%s:%s:%d:%d\n",__FILE__,__FUNCTION__,__LINE__,n);
+  //printf("%s:%s:%d:%d\n",__FILE__,__FUNCTION__,__LINE__,n);

  unsigned char buf[digits];
  uint8_t i = 0;
@@ -99,45 +98,45 @@ void print_uint8_base2_ndigit(uint8_t n, uint8_t digits) {
  }

  for (; i > 0; i--) {
-      //serial_write('0' + buf[i - 1]);
-      printf("%c", '0' + buf[i - 1]);
+      serial_write('0' + buf[i - 1]);
+      //printf("%c", '0' + buf[i - 1]);
  }
-  printf("\n");
+  //printf("\n");
 }


 void print_uint32_base10(uint32_t n)
 {
-  printf("%s:%s:%d:%d\n",__FILE__,__FUNCTION__,__LINE__,n);
+  //printf("%s:%s:%d:%d\n",__FILE__,__FUNCTION__,__LINE__,n);

-//  if (n == 0) {
-//    serial_write('0');
-//    return;
-//  }
-//
-//  unsigned char buf[10];
-//  uint8_t i = 0;
-//
-//  while (n > 0) {
-//    buf[i++] = n % 10;
-//    n /= 10;
-//  }
-//
-//  for (; i > 0; i--)
-//    serial_write('0' + buf[i-1]);
+  if (n == 0) {
+    serial_write('0');
+    return;
+  }
+
+  unsigned char buf[10];
+  uint8_t i = 0;
+
+  while (n > 0) {
+    buf[i++] = n % 10;
+    n /= 10;
+  }
+
+  for (; i > 0; i--)
+    serial_write('0' + buf[i-1]);
 }


 void printInteger(long n)
 {
-  printf("%s:%s:%d:%ld\n",__FILE__,__FUNCTION__,__LINE__,n);
+//  printf("%s:%s:%d:%ld\n",__FILE__,__FUNCTION__,__LINE__,n);

-  //if (n < 0) {
-  //  serial_write('-');
-  //  print_uint32_base10(-n);
-  //} else {
-  //  print_uint32_base10(n);
-  //}
+  if (n < 0) {
+    serial_write('-');
+    print_uint32_base10(-n);
+  } else {
+    print_uint32_base10(n);
+  }
 }


@@ -148,41 +147,41 @@ void printInteger(long n)
 // techniques are actually just slightly slower. Found this out the hard way.
 void printFloat(float n, uint8_t decimal_places)
 {
-  printf("%s:%s:%d:%f\n",__FILE__,__FUNCTION__,__LINE__,n);
+  //printf("%s:%s:%d:%f\n",__FILE__,__FUNCTION__,__LINE__,n);

-//  if (n < 0) {
-//    serial_write('-');
-//    n = -n;
-//  }
-//
-//  uint8_t decimals = decimal_places;
-//  while (decimals >= 2) { // Quickly convert values expected to be E0 to E-4.
-//    n *= 100;
-//    decimals -= 2;
-//  }
-//  if (decimals) { n *= 10; }
-//  n += 0.5; // Add rounding factor. Ensures carryover through entire value.
-//
-//  // Generate digits backwards and store in string.
-//  unsigned char buf[13];
-//  uint8_t i = 0;
-//  uint32_t a = (long)n;
-//  while(a > 0) {
-//    buf[i++] = (a % 10) + '0'; // Get digit
-//    a /= 10;
-//  }
-//  while (i < decimal_places) {
-//     buf[i++] = '0'; // Fill in zeros to decimal point for (n < 1)
-//  }
-//  if (i == decimal_places) { // Fill in leading zero, if needed.
-//    buf[i++] = '0';
-//  }
-//
-//  // Print the generated string.
-//  for (; i > 0; i--) {
-//    if (i == decimal_places) { serial_write('.'); } // Insert decimal point in right place.
-//    //serial_write(buf[i-1]);
-//  }
+  if (n < 0) {
+    serial_write('-');
+    n = -n;
+  }
+
+  uint8_t decimals = decimal_places;
+  while (decimals >= 2) { // Quickly convert values expected to be E0 to E-4.
+    n *= 100;
+    decimals -= 2;
+  }
+  if (decimals) { n *= 10; }
+  n += 0.5; // Add rounding factor. Ensures carryover through entire value.
+
+  // Generate digits backwards and store in string.
+  unsigned char buf[13];
+  uint8_t i = 0;
+  uint32_t a = (long)n;
+  while(a > 0) {
+    buf[i++] = (a % 10) + '0'; // Get digit
+    a /= 10;
+  }
+  while (i < decimal_places) {
+     buf[i++] = '0'; // Fill in zeros to decimal point for (n < 1)
+  }
+  if (i == decimal_places) { // Fill in leading zero, if needed.
+    buf[i++] = '0';
+  }
+
+  // Print the generated string.
+  for (; i > 0; i--) {
+    if (i == decimal_places) { serial_write('.'); } // Insert decimal point in right place.
+    serial_write(buf[i-1]);
+  }
 }


--- a/grbl/serial.c
+++ b/grbl/serial.c
@@ -36,7 +36,7 @@ volatile uint8_t serial_tx_buffer_tail = 0;
 // Returns the number of bytes available in the RX serial buffer.
 uint8_t serial_get_rx_buffer_available()
 {
-  printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__);  
+  //printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__);  
  uint8_t rtail = serial_rx_buffer_tail; // Copy to limit multiple calls to volatile
  if (serial_rx_buffer_head >= rtail) { return(RX_BUFFER_SIZE - (serial_rx_buffer_head-rtail)); }
  return((rtail-serial_rx_buffer_head-1));
@@ -47,7 +47,7 @@ uint8_t serial_get_rx_buffer_available()
 // NOTE: Deprecated. Not used unless classic status reports are enabled in config.h.
 uint8_t serial_get_rx_buffer_count()
 {
-  printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__);  
+  //printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__);  
  uint8_t rtail = serial_rx_buffer_tail; // Copy to limit multiple calls to volatile
  if (serial_rx_buffer_head >= rtail) { return(serial_rx_buffer_head-rtail); }
  return (RX_BUFFER_SIZE - (rtail-serial_rx_buffer_head));
@@ -58,7 +58,7 @@ uint8_t serial_get_rx_buffer_count()
 // NOTE: Not used except for debugging and ensuring no TX bottlenecks.
 uint8_t serial_get_tx_buffer_count()
 {
-  printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__);  
+  //printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__);  
  uint8_t ttail = serial_tx_buffer_tail; // Copy to limit multiple calls to volatile
  if (serial_tx_buffer_head >= ttail) { return(serial_tx_buffer_head-ttail); }
  return (TX_RING_BUFFER - (ttail-serial_tx_buffer_head));
@@ -89,7 +89,7 @@ void serial_init()

 // Writes one byte to the TX serial buffer. Called by main program.
 void serial_write(uint8_t data) {
-  printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__);  
+  //printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__);  

  // Calculate next head
  uint8_t next_head = serial_tx_buffer_head + 1;
@@ -111,22 +111,26 @@ void serial_write(uint8_t data) {


 // Data Register Empty Interrupt handler
+char ecmc_get_char_from_grbl_tx_buffer()
 //ISR(SERIAL_UDRE)
-//{
-//  uint8_t tail = serial_tx_buffer_tail; // Temporary serial_tx_buffer_tail (to optimize for volatile)
-//
-//  // Send a byte from the buffer
-//  UDR0 = serial_tx_buffer[tail];
-//
-//  // Update tail position
-//  tail++;
-//  if (tail == TX_RING_BUFFER) { tail = 0; }
-//
-//  serial_tx_buffer_tail = tail;
-//
-//  // Turn off Data Register Empty Interrupt to stop tx-streaming if this concludes the transfer
-//  if (tail == serial_tx_buffer_head) { UCSR0B &= ~(1 << UDRIE0); }
-//}
+{
+  printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__);
+  uint8_t tail = serial_tx_buffer_tail; // Temporary serial_tx_buffer_tail (to optimize for volatile)
+  char tempChar=0;
+  // Send a byte from the buffer
+  
+  //UDR0 = serial_tx_buffer[tail];
+  tempChar =serial_tx_buffer[tail];
+  // Update tail position
+  tail++;
+  if (tail == TX_RING_BUFFER) { tail = 0; }
+
+  serial_tx_buffer_tail = tail;
+
+  // Turn off Data Register Empty Interrupt to stop tx-streaming if this concludes the transfer
+  //if (tail == serial_tx_buffer_head) { UCSR0B &= ~(1 << UDRIE0); }
+  return tempChar;
+}

 // Fetches the first byte in the serial read buffer. Called by main program.
 uint8_t serial_read()
@@ -213,7 +217,6 @@ void ecmc_write_command_serial(char* line) {
  }
 }

-
 void serial_reset_read_buffer()
 {
  serial_rx_buffer_tail = serial_rx_buffer_head;
--- a/grbl/serial.h
+++ b/grbl/serial.h
@@ -23,21 +23,26 @@
 #define serial_h


-#ifndef RX_BUFFER_SIZE
-  #define RX_BUFFER_SIZE 128
-#endif
-#ifndef TX_BUFFER_SIZE
-  #ifdef USE_LINE_NUMBERS
-    #define TX_BUFFER_SIZE 112
-  #else
-    #define TX_BUFFER_SIZE 104
-  #endif
-#endif
+//#ifndef RX_BUFFER_SIZE
+//  #define RX_BUFFER_SIZE 128
+//#endif
+//#ifndef TX_BUFFER_SIZE
+//  #ifdef USE_LINE_NUMBERS
+//    #define TX_BUFFER_SIZE 112
+//  #else
+//    #define TX_BUFFER_SIZE 104
+//  #endif
+//#endif
+
+
+#define RX_BUFFER_SIZE 1024
+#define TX_BUFFER_SIZE 1024

 #define SERIAL_NO_DATA 0xff


 void ecmc_write_command_serial(char* line);
+char ecmc_get_char_from_grbl_tx_buffer();

 void serial_init();

--- a/grbl/stepper.c
+++ b/grbl/stepper.c
@@ -226,13 +226,15 @@ pthread_t tid;

 void *ecmc_dummy_thread(void *ptr) {
  printf("%s:%s:%d\n",__FILE__,__FUNCTION__,__LINE__);
-  while (stepperInterruptEnable) {
+  
+  while (stepperInterruptEnable) {    
    for(int i=0; i < 30; i++) {
      ecmc_grbl_main_rt_thread();
    }
-    printf("%s:%s:%d Positions(x,y,x)=%d,%d,%d..\n",__FILE__,__FUNCTION__,__LINE__,sys_position[X_AXIS], sys_position[Y_AXIS],sys_position[Z_AXIS] );
-    sleep(0.001);
+    printf("%s:%s:%d Positions(x,y,z)=%d,%d,%d..\n",__FILE__,__FUNCTION__,__LINE__,sys_position[X_AXIS], sys_position[Y_AXIS],sys_position[Z_AXIS] );
+    delay_ms(1);
  }
+  fflush(stdout);
 }

 void ecmc_start_dummy_thread()
--- a/iocsh/log.log
+++ b/iocsh/log.log