add GRBL ^X and ? to serial uart handler

diff --git a/src/libs/Kernel.cpp b/src/libs/Kernel.cpp
index d3e2070..036740f 100644 (file)
--- a/src/libs/Kernel.cpp
+++ b/src/libs/Kernel.cpp
@@ -24,9 +24,11 @@
 #include "modules/robot/Robot.h"
 #include "modules/robot/Stepper.h"
 #include "modules/robot/Conveyor.h"
+#include "StepperMotor.h"
 
 #include <malloc.h>
 #include <array>
+#include <string>
 
 #define baud_rate_setting_checksum CHECKSUM("baud_rate")
 #define uart0_checksum             CHECKSUM("uart0")
@@ -145,6 +147,31 @@ Kernel::Kernel(){
 
 }
 
+// return a GRBL-like query string for serial ?
+std::string Kernel::get_query_string()
+{
+    std::string str;
+    str.append("<");
+    if(halted) {
+        str.append("Alarm,");
+    }else if(this->conveyor->is_queue_empty()) {
+        str.append("Idle,");
+    }else{
+        str.append("Run,");
+    }
+
+    char buf[64];
+    size_t n= snprintf(buf, sizeof(buf), "%f,%f,%f,", this->robot->actuators[X_AXIS]->get_current_position(), this->robot->actuators[Y_AXIS]->get_current_position(), this->robot->actuators[Z_AXIS]->get_current_position());
+    str.append("MPos:").append(buf, n);
+
+    float pos[3];
+    this->robot->get_axis_position(pos);
+    n= snprintf(buf, sizeof(buf), "%f,%f,%f", pos[0], pos[1], pos[2]);
+    str.append("WPos:").append(buf, n);
+    str.append(">\r\n");
+    return str;
+}
+
 // Add a module to Kernel. We don't actually hold a list of modules we just call its on_module_loaded
 void Kernel::add_module(Module* module){
     module->on_module_loaded();

diff --git a/src/libs/Kernel.h b/src/libs/Kernel.h
index a0dccaa..30599cf 100644 (file)
--- a/src/libs/Kernel.h
+++ b/src/libs/Kernel.h
@@ -45,6 +45,7 @@ class Kernel {
 
         bool is_using_leds() const { return use_leds; }
         bool is_halted() const { return halted; }
+        std::string get_query_string();
 
         // These modules are available to all other modules
         SerialConsole*    serial;

diff --git a/src/modules/communication/SerialConsole.cpp b/src/modules/communication/SerialConsole.cpp
index 301937d..983df55 100644 (file)
--- a/src/modules/communication/SerialConsole.cpp
+++ b/src/modules/communication/SerialConsole.cpp
@@ -29,9 +29,11 @@ SerialConsole::SerialConsole( PinName rx_pin, PinName tx_pin, int baud_rate ){
 void SerialConsole::on_module_loaded() {
     // We want to be called every time a new char is received
     this->serial->attach(this, &SerialConsole::on_serial_char_received, mbed::Serial::RxIrq);
+    query_flag= false;
 
     // We only call the command dispatcher in the main loop, nowhere else
     this->register_for_event(ON_MAIN_LOOP);
+    this->register_for_event(ON_IDLE);
 
     // Add to the pack of streams kernel can call to, for example for broadcasting
     THEKERNEL->streams->append_stream(this);
@@ -41,12 +43,32 @@ void SerialConsole::on_module_loaded() {
 void SerialConsole::on_serial_char_received(){
     while(this->serial->readable()){
         char received = this->serial->getc();
+        if(received == '?') {
+            query_flag= true;
+            continue;
+        }
+        if(received == 26) { // ^X
+            halt_flag= true;
+            continue;
+        }
         // convert CR to NL (for host OSs that don't send NL)
         if( received == '\r' ){ received = '\n'; }
         this->buffer.push_back(received);
     }
 }
 
+void SerialConsole::on_idle(void * argument)
+{
+    if(query_flag) {
+        query_flag= false;
+        puts(THEKERNEL->get_query_string().c_str());
+    }
+    if(halt_flag) {
+        halt_flag= false;
+        THEKERNEL->call_event(ON_HALT, nullptr);
+    }
+}
+
 // Actual event calling must happen in the main loop because if it happens in the interrupt we will loose data
 void SerialConsole::on_main_loop(void * argument){
     if( this->has_char('\n') ){

diff --git a/src/modules/communication/SerialConsole.h b/src/modules/communication/SerialConsole.h
index 6dfbcb4..49489db 100644 (file)
--- a/src/modules/communication/SerialConsole.h
+++ b/src/modules/communication/SerialConsole.h
@@ -27,6 +27,7 @@ class SerialConsole : public Module, public StreamOutput {
         void on_module_loaded();
         void on_serial_char_received();
         void on_main_loop(void * argument);
+        void on_idle(void * argument);
         bool has_char(char letter);
 
         int _putc(int c);
@@ -37,6 +38,10 @@ class SerialConsole : public Module, public StreamOutput {
         //vector<std::string> received_lines;    // Received lines are stored here until they are requested
         RingBuffer<char,256> buffer;             // Receive buffer
         mbed::Serial* serial;
+        struct {
+          bool query_flag:1;
+          bool halt_flag:1;
+        };
 };
 
 #endif