Switch controlling step driver to serial interface

arduino_code/Test/src/mqtt_handler.cpp

@@ -4,90 +4,70 @@
 #include <WiFi.h>
 #include <PubSubClient.h>
 
-// --- ГЛОБАЛЬНЫЕ ОБЪЕКТЫ (Static внутри cpp) ---
 static WiFiClient espClient;
 static PubSubClient client(espClient);
 
-// --- ПЕРЕМЕННЫЕ ДЛЯ ОТСЛЕЖИВАНИЯ ИЗМЕНЕНИЙ ТЕЛЕМЕТРИИ ---
+// Кэш телеметрии
 static unsigned long last_feedback_time = 0;
 static int last_pub_rpm = -1;
-static unsigned long last_pub_sps = -1;
 static unsigned long last_pub_steps = -1;
-static int last_pub_driver_state = -1; // -1: unknown, 0: off, 1: on
-static int last_pub_is_run = -1;       // -1: unknown, 0: stop, 1: run
+static int last_pub_is_run = -1;
 
-// --- ВСПОМОГАТЕЛЬНЫЕ ФУНКЦИИ ---
+// TMC Кэш
+static uint16_t last_pub_sg = 65535;
+static uint32_t last_pub_interstep = 0;
+static uint8_t last_pub_current_pct = 255;
+static uint16_t last_pub_microsteps = 0;
+
+// Статусы (битовые флаги)
+static int last_pub_over_temp = -1;
+static int last_pub_short_gnd = -1;
+static int last_pub_open_load = -1;
+static int last_pub_stealth_active = -1;
+static int last_pub_standstill = -1;
+static uint8_t last_pub_current_scaling = 255;
 
 static void setup_wifi() {
     Serial.print("Connecting to WiFi");
     WiFi.begin(WIFI_SSID, WIFI_PASS);
-    while (WiFi.status() != WL_CONNECTED) {
-        vTaskDelay(pdMS_TO_TICKS(500));
-        Serial.print(".");
-    }
+    while (WiFi.status() != WL_CONNECTED) { vTaskDelay(pdMS_TO_TICKS(500)); Serial.print("."); }
     Serial.println("\nWiFi Connected");
 }
 
 static void reconnect() {
     while (!client.connected()) {
-        Serial.print("Attempting MQTT connection...");
         if (client.connect(MQTT_CLIENT_ID, MQTT_USER, MQTT_PASS)) {
-            Serial.println("connected");
-            
-            // Сбрасываем кэш телеметрии, чтобы при переподключении 
-            // сразу отправить актуальное состояние, даже если оно не менялось
-            last_pub_rpm = -1;
-            last_pub_sps = -1;
-            last_pub_steps = -1;
-            last_pub_driver_state = -1;
-            last_pub_is_run = -1;
+            // Сброс кэша
+            last_pub_rpm = -1; last_pub_steps = -1; last_pub_is_run = -1;
+            last_pub_sg = 65535; last_pub_interstep = 0; last_pub_current_pct = 255;
+            last_pub_microsteps = 0; last_pub_over_temp = -1; last_pub_short_gnd = -1;
+            last_pub_open_load = -1; last_pub_stealth_active = -1; last_pub_standstill = -1;
+            last_pub_current_scaling = 255;
 
-            // Подписка на топики управления
-            client.subscribe("motor/control/command");
-            client.subscribe("motor/control/driver");
-            client.subscribe("motor/control/step_per_sec");
+            // Подписки
             client.subscribe("motor/control/rpm");
+            client.subscribe("motor/control/driver");
             client.subscribe("motor/control/totalsteps/reset");
+            
+            client.subscribe("motor/control/tmc/current_percent");
+            client.subscribe("motor/control/tmc/microsteps");
+            client.subscribe("motor/control/tmc/stallguard");
+            client.subscribe("motor/control/tmc/enable");
+            client.subscribe("motor/control/tmc/stealthchop");
+            client.subscribe("motor/control/tmc/coolstep");
         } else {
-            Serial.printf("failed, rc=%d, retry in 5s\n", client.state());
             vTaskDelay(pdMS_TO_TICKS(5000));
         }
     }
 }
 
 static void callback(char* topic, byte* payload, unsigned int length) {
-    // Безопасное преобразование payload в строку
     char msg[length + 1];
     memcpy(msg, payload, length);
     msg[length] = '\0';
 
-    Serial.printf("MQTT Received [%s]: %s\n", topic, msg);
-
-    if (strcmp(topic, "motor/control/command") == 0) {
-        if (strcmp(msg, "start") == 0) {
-            // Если была команда stop (RPM 0), запускаем на дефолтной скорости или последней целевой
-            // Здесь для простоты запускаем на 60 RPM, если текущая скорость близка к 0
-            if (getCurrentRPM() < 5) {
-                setTargetRPM(60);
-            } else {
-                // Если мотор уже крутится, можно просто возобновить рампу к текущей цели, 
-                // но setTargetRPM и так это делает.
-                setTargetRPM(getCurrentRPM()); 
-            }
-        } else if (strcmp(msg, "stop") == 0) {
-            setTargetRPM(0);
-        }
-    } 
-    else if (strcmp(topic, "motor/control/rpm") == 0) {
-        setTargetRPM(atoi(msg));
-    }
-    else if (strcmp(topic, "motor/control/step_per_sec") == 0) {
-        unsigned long sps = strtoul(msg, NULL, 10);
-        unsigned long total_steps_per_rev = (unsigned long)STEPS_PER_REVOLUTION * MICROSTEP_FACTOR;
-        if (sps > 0) {
-            int calc_rpm = (sps * 60) / total_steps_per_rev;
-            setTargetRPM(calc_rpm);
-        }
+    if (strcmp(topic, "motor/control/rpm") == 0) {
+        setTargetRPM(atoi(msg)); // Поддерживает отрицательные для реверса!
     }
     else if (strcmp(topic, "motor/control/driver") == 0) {
         // TMC2209: LOW = Enabled, HIGH = Disabled
@@ -96,85 +76,132 @@ static void callback(char* topic, byte* payload, unsigned int length) {
     }
     else if (strcmp(topic, "motor/control/totalsteps/reset") == 0) {
         resetSteps();
-        // Принудительно публикуем 0 сразу, не дожидаясь цикла телеметрии
-        if (client.connected()) {
-            client.publish("motor/feedback/totalsteps", "0");
-        }
+        if (client.connected()) client.publish("motor/feedback/totalsteps", "0");
         last_pub_steps = 0;
     }
+    // --- TMC Control ---
+    else if (strcmp(topic, "motor/control/tmc/current_percent") == 0) {
+        uint8_t pct = atoi(msg);
+        if (pct <= 100) tmcSetCurrentPercent(pct, pct / 2); // Hold = 50% от Run
+    }
+    else if (strcmp(topic, "motor/control/tmc/microsteps") == 0) {
+        uint16_t ms = atoi(msg);
+        tmcSetMicrosteps(ms);
+    }
+    else if (strcmp(topic, "motor/control/tmc/stallguard") == 0) {
+        tmcSetStallGuard(atoi(msg));
+    }
+    else if (strcmp(topic, "motor/control/tmc/enable") == 0) {
+        tmcSoftwareEnable(strcmp(msg, "on") == 0);
+    }
+    else if (strcmp(topic, "motor/control/tmc/stealthchop") == 0) {
+        tmcSetStealthChop(strcmp(msg, "on") == 0);
+    }
+    else if (strcmp(topic, "motor/control/tmc/coolstep") == 0) {
+        tmcSetCoolStep(strcmp(msg, "on") == 0);
+    }
 }
 
 static void publishTelemetry() {
     unsigned long now = millis();
-    
-    // Проверяем каждые 200 мс
-    if (now - last_feedback_time >= 200) {
+    if (now - last_feedback_time >= 500) { // Читаем статусы раз в 500мс
         
-        // 1. RPM
-        int current_rpm = getCurrentRPM();
-        if (current_rpm != last_pub_rpm) {
-            client.publish("motor/feedback/rpm", String(current_rpm).c_str());
-            last_pub_rpm = current_rpm;
+        // Базовая телеметрия
+        int rpm = getCurrentRPM();
+        if (rpm != last_pub_rpm) {
+            client.publish("motor/feedback/rpm", String(rpm).c_str());
+            last_pub_rpm = rpm;
         }
 
-        // 2. Steps Per Second (SPS)
-        unsigned long interval_us = getStepIntervalUs();
-        unsigned long current_sps = 0;
-        if (interval_us > 0 && interval_us < 100000) {
-            current_sps = 1000000UL / interval_us;
-        }
-        
-        if (current_sps != last_pub_sps) {
-            client.publish("motor/feedback/step_per_seconds", String(current_sps).c_str());
-            last_pub_sps = current_sps;
+        unsigned long steps = getMotorSteps();
+        if (steps != last_pub_steps) {
+            client.publish("motor/feedback/totalsteps", String(steps).c_str());
+            last_pub_steps = steps;
         }
 
-        // 3. Total Steps
-        unsigned long current_steps = getMotorSteps();
-        if (current_steps != last_pub_steps) {
-            client.publish("motor/feedback/totalsteps", String(current_steps).c_str());
-            last_pub_steps = current_steps;
+        int run = isMotorRunning() ? 1 : 0;
+        if (run != last_pub_is_run) {
+            client.publish("motor/feedback/is_run", run ? "true" : "false");
+            last_pub_is_run = run;
         }
 
-        // 4. Driver State (EN_PIN)
-        // LOW = On, HIGH = Off
-        int current_driver_state = (digitalRead(EN_PIN) == LOW) ? 1 : 0;
-        if (current_driver_state != last_pub_driver_state) {
-            client.publish("motor/feedback/driver", current_driver_state ? "on" : "off");
-            last_pub_driver_state = current_driver_state;
-        }
+        // TMC Телеметрия
+        if (tmcIsInitialized()) {
+            uint8_t pct = tmcGetRunCurrentPercent();
+            if (pct != last_pub_current_pct) {
+                client.publish("motor/feedback/tmc/current_percent", String(pct).c_str());
+                last_pub_current_pct = pct;
+            }
 
-        // 5. Is Running
-        int current_is_run = isMotorRunning() ? 1 : 0;
-        if (current_is_run != last_pub_is_run) {
-            client.publish("motor/feedback/is_run", current_is_run ? "true" : "false");
-            last_pub_is_run = current_is_run;
-        }
+            uint16_t ms = tmcGetMicrostepsSetting();
+            if (ms != last_pub_microsteps) {
+                client.publish("motor/feedback/tmc/microsteps", String(ms).c_str());
+                last_pub_microsteps = ms;
+            }
 
+            uint16_t sg = tmcGetStallGuardResult();
+            if (sg != last_pub_sg) {
+                client.publish("motor/feedback/tmc/sg_result", String(sg).c_str());
+                last_pub_sg = sg;
+            }
+
+            uint32_t interstep = tmcGetInterstepDuration();
+            if (interstep != last_pub_interstep) {
+                client.publish("motor/feedback/tmc/interstep_duration", String(interstep).c_str());
+                last_pub_interstep = interstep;
+            }
+
+            // Чтение полного статуса (требует чтения регистра DRV_STATUS)
+            TMC2209::Status status = tmcGetStatus();
+            
+            int ot = (status.over_temperature_warning || status.over_temperature_shutdown) ? 1 : 0;
+            if (ot != last_pub_over_temp) {
+                client.publish("motor/feedback/tmc/status/over_temp", ot ? "true" : "false");
+                last_pub_over_temp = ot;
+            }
+
+            int sgnd = (status.short_to_ground_a || status.short_to_ground_b) ? 1 : 0;
+            if (sgnd != last_pub_short_gnd) {
+                client.publish("motor/feedback/tmc/status/short_to_ground", sgnd ? "true" : "false");
+                last_pub_short_gnd = sgnd;
+            }
+
+            int ol = (status.open_load_a || status.open_load_b) ? 1 : 0;
+            if (ol != last_pub_open_load) {
+                client.publish("motor/feedback/tmc/status/open_load", ol ? "true" : "false");
+                last_pub_open_load = ol;
+            }
+
+            int sa = status.stealth_chop_mode ? 1 : 0;
+            if (sa != last_pub_stealth_active) {
+                client.publish("motor/feedback/tmc/status/stealth_chop_active", sa ? "true" : "false");
+                last_pub_stealth_active = sa;
+            }
+
+            int ss = status.standstill ? 1 : 0;
+            if (ss != last_pub_standstill) {
+                client.publish("motor/feedback/tmc/status/standstill", ss ? "true" : "false");
+                last_pub_standstill = ss;
+            }
+
+            if (status.current_scaling != last_pub_current_scaling) {
+                client.publish("motor/feedback/tmc/status/current_scaling", String(status.current_scaling).c_str());
+                last_pub_current_scaling = status.current_scaling;
+            }
+        }
         last_feedback_time = now;
     }
 }
 
-// --- ОСНОВНАЯ ЗАДАЧА FREE RTOS ---
 void mqttTask(void *parameter) {
-    // Инициализация WiFi и MQTT клиента
     setup_wifi();
     client.setServer(MQTT_SERVER, MQTT_PORT);
     client.setCallback(callback);
 
     for (;;) {
-        // Поддержание соединения
-        if (!client.connected()) {
-            reconnect();
-        }
-        
-        // Обработка входящих сообщений
+        if (!client.connected()) reconnect();
         client.loop();
-        
-        // Отправка телеметрии
         publishTelemetry();
-        
-        // Yield для других задач
         vTaskDelay(pdMS_TO_TICKS(10));
     }
 }