diff --git a/arduino_code/Test/README.md b/arduino_code/Test/README.md index e16c65c..513c652 100644 --- a/arduino_code/Test/README.md +++ b/arduino_code/Test/README.md @@ -1,129 +1,138 @@ -Ниже представлена структурированная документация по MQTT-топикам, составленная на основе предоставленного вами кода. Документация разделена на логические блоки для удобства использования (управление и телеметрия). +Вот обновленная и расширенная документация по MQTT-интерфейсу, включающая новую функциональность работы с датчиками расстояния **VL53L0X**, а также оптимизации, появившиеся в коде. --- -# 📡 Документация по MQTT интерфейсу (ESP32 + TMC2209 + Servo) +# 📡 Документация по MQTT интерфейсу (ESP32 + TMC2209 + Servo + VL53L0X) ## 📌 Общая информация - **Архитектура**: ESP32 (FreeRTOS задача `mqttTask`). - **Период опроса телеметрии**: 500 мс. -- **Оптимизация трафика**: Публикация данных происходит **только при изменении значения** (реализован кэш последних отправленных значений). +- **Оптимизация трафика**: + 1. Публикация данных происходит **только при изменении значения** (строгое кэширование). + 2. Используется статический буфер (`intToString`/`uintToString`) вместо динамического класса `String` для экономии памяти и предотвращения фрагментации кучи. - **Префиксы**: - - `.../control/...` — топики для **отправки команд** устройству (подписка устройства). - - `.../feedback/...` — топики для **получения статуса/телеметрии** от устройства (публикация устройства). + - `.../control/...` — топики для **отправки команд** устройству (подписка). + - `.../feedback/...` — топики для **получения статуса/телеметрии** от устройства (публикация). --- ## ⚙️ 1. Управление шаговым двигателем (Motor Control) -Устройство подписывается на эти топики для получения команд. - -| Топик | Тип данных | Описание | Пример полезной нагрузки (Payload) | +| Топик | Тип данных | Описание | Пример Payload | | :--- | :---: | :--- | :--- | -| `motor/control/rpm` | Integer | Целевая скорость в об/мин (RPM). **Отрицательные значения** включают реверс. | `-150`, `0`, `300` | -| `motor/control/driver` | String | Аппаратное включение/выключение драйвера (пин `EN_PIN`). `on` = LOW (включен), любое другое = HIGH (выключен). | `on`, `off` | -| `motor/control/totalsteps/reset` | Any | Сброс счетчика шагов в ноль. После сброса устройство само опубликует `0` в топик обратной связи. | `1`, `reset`, `""` | +| `motor/control/rpm` | Integer | Целевая скорость в об/мин (RPM). Отрицательные значения включают реверс. | `-150`, `0`, `300` | +| `motor/control/driver` | String | Аппаратное вкл/выкл драйвера (пин `EN_PIN`). `on` = LOW (вкл), иначе = HIGH (выкл). | `on`, `off` | +| `motor/control/totalsteps/reset`| Any | Сброс счетчика шагов в ноль. Устройство сразу опубликует `"0"` в feedback. | `1`, `reset` | + +### Телеметрия двигателя (Motor Feedback) +*(Публикуется только при изменении)* +- `motor/feedback/rpm` (Integer): Текущая скорость. +- `motor/feedback/totalsteps` (Integer): Общее количество шагов. +- `motor/feedback/is_run` (String: `true`/`false`): Двигатель движется. +- `motor/feedback/driver/status` (String: `on`/`off`): Общий статус драйвера. +- `motor/feedback/tmc/status` (String: `on`/`off`): Статус программного включения TMC. + +#### Детальная телеметрия TMC2209 (`motor/feedback/tmc/...`) +- `current_percent` (Integer): Текущий % рабочего тока. +- `microsteps` (Integer): Текущий режим микрошага. +- `sg_result` (Integer): Текущее значение StallGuard (нагрузка). +- `interstep_duration` (Integer): Длительность между шагами. +- `status/over_temp` (String: `true`/`false`): Перегрев. +- `status/short_to_ground` (String: `true`/`false`): КЗ на землю. +- `status/open_load` (String: `true`/`false`): Обрыв нагрузки. +- `status/stealth_chop_active` (String: `true`/`false`): Активен ли StealthChop. +- `status/standstill` (String: `true`/`false`): Двигатель в покое. +- `status/current_scaling` (Integer): Внутренний масштабный коэффициент тока. --- -## 🛠️ 2. Управление драйвером TMC2209 (TMC Control) +## 🦾 2. Управление сервоприводами (Servo Control) -Специфические команды для настройки микросхемы TMC2209. - -| Топик | Тип данных | Описание | Пример полезной нагрузки | -| :--- | :---: | :--- | :--- | -| `motor/control/tmc/current_percent` | Integer (0-100) | Установка рабочего тока в процентах. Ток удержания (Hold) автоматически устанавливается как 50% от рабочего. | `75` | -| `motor/control/tmc/microsteps` | Integer | Установка режима микрошага. | `16`, `32`, `256` | -| `motor/control/tmc/stallguard` | Integer | Установка порога чувствительности StallGuard (защита от заклинивания/определение нагрузки). | `10`, `50` | -| `motor/control/tmc/enable` | String | Программное включение/выключение чипа TMC. | `on`, `off` | -| `motor/control/tmc/stealthchop` | String | Включение/выключение режима StealthChop (бесшумный режим). | `on`, `off` | -| `motor/control/tmc/coolstep` | String | Включение/выключение режима CoolStep (динамическое управление током). | `on`, `off` | - ---- - -## 📊 3. Телеметрия шагового двигателя (Motor Feedback) - -Устройство публикует эти данные **только при изменении значения**, проверка происходит каждые 500 мс. - -| Топик | Тип данных | Описание | -| :--- | :---: | :--- | -| `motor/feedback/rpm` | Integer | Текущая фактическая скорость вращения (RPM). | -| `motor/feedback/totalsteps` | Integer (String) | Общее количество сделанных шагов (накапливаемое). | -| `motor/feedback/is_run` | String (`true`/`false`) | Флаг: двигатель в движении (`true`) или остановлен (`false`). | -| `motor/feedback/driver/status` | String (`on`/`off`) | Общий статус драйвера (результат `checkDriverStatus()`). | -| `motor/feedback/tmc/status` | String (`on`/`off`) | Статус программного включения TMC (результат `checkTmcSoftwareEnable()`). | - -### Детальная телеметрия TMC2209 -*(Публикуется только если `tmcIsInitialized() == true`)* - -| Топик | Тип данных | Описание | -| :--- | :---: | :--- | -| `motor/feedback/tmc/current_percent` | Integer | Текущий установленный процент рабочего тока. | -| `motor/feedback/tmc/microsteps` | Integer | Текущее значение микрошага. | -| `motor/feedback/tmc/sg_result` | Integer | Текущее значение StallGuard (нагрузка на вал). | -| `motor/feedback/tmc/interstep_duration`| Integer | Длительность между шагами (мкс/такты). | -| `motor/feedback/tmc/status/over_temp` | String (`true`/`false`) | Предупреждение или отключение по перегреву. | -| `motor/feedback/tmc/status/short_to_ground`| String (`true`/`false`) | Короткое замыкание на землю (фаза A или B). | -| `motor/feedback/tmc/status/open_load` | String (`true`/`false`) | Обрыв нагрузки (отключен двигатель) на фазе A или B. | -| `motor/feedback/tmc/status/stealth_chop_active`| String (`true`/`false`) | Фактически активный режим StealthChop. | -| `motor/feedback/tmc/status/standstill` | String (`true`/`false`) | Двигатель находится в состоянии покоя. | -| `motor/feedback/tmc/status/current_scaling`| Integer | Внутренний масштабный коэффициент тока драйвера. | - ---- - -## 🦾 4. Управление сервоприводами (Servo Control) - -Поддержка нескольких каналов (сервоприводов). Топики используют динамический параметр `{channel}` (номер канала, от `0` до `MAX_SERVOS - 1`). - -### Команды (Подписка устройства) -Базовый паттерн: `servo/control/{channel}/{command}` +Поддержка нескольких каналов (`{channel}` от `0` до `MAX_SERVOS - 1`). +### Команды | Топик (пример для канала 0) | Тип данных | Описание | Пример Payload | | :--- | :---: | :--- | :--- | -| `servo/control/0/angle` | Integer (0-180) | Установить угол поворота сервопривода. | `90`, `180`, `0` | -| `servo/control/0/enable` | String | Включить (`on`, `1`, `true`) или выключить сервопривод. | `on`, `off`, `true` | +| `servo/control/0/angle` | Integer (0-180) | Установить угол поворота. | `90` | +| `servo/control/0/enable` | String | Включить (`on`, `1`, `true`) или выключить. | `on` | -*Примечание: Код использует `sscanf(topic, "servo/control/%d/%s", &channel, command)`, поэтому команда может быть любой строкой, но обрабатываются только `angle` и `enable`.* +### Обратная связь +- `servo/0/feedback/status` (String: `on`/`off`): Статус питания сервопривода. +- `servo/0/feedback/angle` (Integer): Текущий установленный угол. -### Обратная связь (Публикация устройства) -Устройство публикует состояние конкретного канала только при его изменении. +--- + +## 📏 3. Управление датчиками VL53L0X (Sensor Control) **(НОВОЕ)** + +Поддержка до 8 каналов (`VL53L0X_MAX_CHANNELS = 8`). Топики используют параметр `{channel}` (0–7). + +| Топик | Тип данных | Описание | Пример Payload | +| :--- | :---: | :--- | :--- | +| `sensor/control/mode` | Integer | Установка глобального режима измерения (0 до `MODE_COUNT - 1`). | `0`, `1` | +| `sensor/control/mode_name` | String | *Заглушка/Логирование.* Принимает имя режима для отладки. | `LongRange` | +| `sensor/control/calibrate/start/{ch}`| Integer | Начало калибровки: указать близкое расстояние в мм. | `50` | +| `sensor/control/calibrate/finish/{ch}`| Integer | Завершение калибровки: указать дальнее расстояние в мм. | `500` | +| `sensor/control/clear_cal/{ch}` | Any | Сбросить калибровку для указанного канала. | `1` | +| `sensor/control/enable/{ch}` | String | Включить (`on`, `1`, `true`) или выключить конкретный канал. | `on` | +| `sensor/control/publish_all` | Any | **Принудительный сброс кэша.** Заставляет устройство немедленно опубликовать текущие значения всех датчиков, даже если они не изменились. | `1` | + +--- + +## 📊 4. Телеметрия датчиков VL53L0X (Sensor Feedback) **(НОВОЕ)** + +### Глобальная информация о режиме +Публикуется только при смене режима измерения: +- `sensor/feedback/mode` (String): Человекочитаемое имя текущего режима (например, "Default", "LongRange"). +- `sensor/feedback/mode_id` (Integer): Числовой ID текущего режима. +- `sensor/feedback/max_range` (Integer): Максимальная дальность для текущего режима (в мм). + +### Постатусная информация по каналам (`{channel}` = 0..7) +*(Публикуется только при изменении состояния или значения)* | Топик (пример для канала 0) | Тип данных | Описание | | :--- | :---: | :--- | -| `servo/0/feedback/status` | String (`on`/`off`) | Текущий статус включения сервопривода на канале 0. | -| `servo/0/feedback/angle` | Integer | Текущий установленный угол (в градусах) сервопривода на канале 0. | +| `sensor/feedback/0/status` | String (`on`/`off`) | Включен ли логически данный канал. | +| `sensor/feedback/0/calibrated` | String (`true`/`false`)| Была ли проведена калибровка для этого канала. | +| `sensor/feedback/0/distance` | Integer или String | **Калиброванное** расстояние в мм. Если значение `65535` (ошибка/вне диапазона), публикуется строка `"out_of_range"`. | +| `sensor/feedback/0/raw` | Integer | **Сырое** (некалиброванное) значение расстояния в мм. | -*(Замените `0` на актуальный номер канала при использовании)* +*Примечание: Топики `distance` и `raw` публикуются только если канал активен (`vl53l0xIsChannelActive`).* --- -## 💡 Важные особенности реализации +## 💡 Важные особенности реализации (Обновлено) -1. **Анти-спам (Кэширование)**: В коде реализована строгая проверка `if (current_value != last_pub_value)`. Это означает, что если двигатель стоит, а его параметры не меняются, топик `motor/feedback/rpm` **не будет** засорять сеть сообщениями каждые 500 мс. Сообщение придет только при изменении. -2. **Сброс шагов**: При получении команды на `motor/control/totalsteps/reset`, устройство не только сбрасывает внутренний счетчик, но и принудительно публикует `"0"` в `motor/feedback/totalsteps`, чтобы синхронизировать состояние с клиентом. -3. **Логика пина Enable**: Для `motor/control/driver` значение `"on"` подает `LOW` на `EN_PIN` (что стандартно для TMC2209 означает **включение** драйвера), а любое другое значение подает `HIGH` (выключение). -4. **Безопасность серво**: При отправке команды `angle`, код проверяет диапазон `0 <= angle <= 180`. Значения вне этого диапазона будут проигнорированы. +1. **Безопасная работа со строками**: В новом коде добавлены функции `intToString` и `uintToString`, использующие статические буферы. Это полностью устраняет риск фрагментации памяти (heap fragmentation) при частой публикации телеметрии, который был присущ использованию класса `String`. +2. **Принудительная публикация**: Топик `sensor/control/publish_all` сбрасывает кэш значений `distance` и `raw` на `65535`. При следующем цикле (через 500 мс) система "увидит" изменение и гарантированно отправит актуальные данные. Это полезно при подключении нового клиента, которому нужно получить текущее состояние без перезагрузки устройства. +3. **Обработка ошибок дальности**: Если датчик возвращает `65535` (стандартный код ошибки "вне диапазона" или сбоя измерения для VL53L0X), в топик `distance` публикуется понятная строка `"out_of_range"`, а не число, что упрощает обработку на стороне клиента. +4. **Изоляция каналов**: Цикл телеметрии VL53L0X предварительно проверяет `vl53l0xIsChannelPresent(ch)`, поэтому несуществующие или отключенные на аппаратном уровне каналы не создают лишнего трафика. --- -### Пример сценария использования (CLI / mosquitto) +### 🛠️ Примеры использования (CLI / mosquitto) ```bash -# Включить драйвер -mosquitto_pub -t "motor/control/driver" -m "on" - -# Запустить двигатель на 200 об/мин вперед -mosquitto_pub -t "motor/control/rpm" -m "200" - -# Включить бесшумный режим +# --- Двигатель --- +mosquitto_pub -t "motor/control/rpm" -m "100" mosquitto_pub -t "motor/control/tmc/stealthchop" -m "on" -# Повернуть сервопривод на канале 1 на 90 градусов и включить его -mosquitto_pub -t "servo/control/1/enable" -m "on" -mosquitto_pub -t "servo/control/1/angle" -m "90" +# --- Сервопривод (канал 0) --- +mosquitto_pub -t "servo/control/0/enable" -m "on" +mosquitto_pub -t "servo/control/0/angle" -m "90" -# Подписаться на всю телеметрию двигателя для отладки -mosquitto_sub -t "motor/feedback/#" -v +# --- Датчики VL53L0X --- +# Включить канал 1 +mosquitto_pub -t "sensor/control/enable/1" -m "on" + +# Начать калибровку канала 1 (близкая точка 50 мм) +mosquitto_pub -t "sensor/control/calibrate/start/1" -m "50" +# ... передвинуть объект ... +# Завершить калибровку канала 1 (дальняя точка 400 мм) +mosquitto_pub -t "sensor/control/calibrate/finish/1" -m "400" + +# Принудительно запросить публикацию всех текущих показаний датчиков +mosquitto_pub -t "sensor/control/publish_all" -m "1" + +# Подписаться на все события датчиков для отладки +mosquitto_sub -t "sensor/feedback/#" -v ``` - -Если вам нужно добавить эту документацию в ваш репозиторий, я могу оформить её в виде готового файла `README.md` или `MQTT_API.md` с дополнительными разделами (например, схемой подключения или настройкой `config.h`). \ No newline at end of file diff --git a/arduino_code/Test/platformio.ini b/arduino_code/Test/platformio.ini index 1464849..d23aaa0 100644 --- a/arduino_code/Test/platformio.ini +++ b/arduino_code/Test/platformio.ini @@ -20,3 +20,4 @@ lib_deps = janelia-arduino/TMC2209@^9.4.0 madhephaestus/ESP32Servo@^3.2.1 adafruit/Adafruit PWM Servo Driver Library@^3.0.3 + pololu/VL53L0X@^1.3.1 diff --git a/arduino_code/Test/src/config.cpp b/arduino_code/Test/src/config.cpp index 23f78b8..877eb3f 100644 --- a/arduino_code/Test/src/config.cpp +++ b/arduino_code/Test/src/config.cpp @@ -29,7 +29,7 @@ const uint16_t TMC_MICROSTEPS = 1; // I2C Настройка const int16_t I2C_SDA_PIN = 21; -const int16_t I2C_SLC_PIN = 22; +const int16_t I2C_SCL_PIN = 22; // ========== SERVO CONFIGURATION ========== diff --git a/arduino_code/Test/src/config.h b/arduino_code/Test/src/config.h index 5046fe2..c4e03a0 100644 --- a/arduino_code/Test/src/config.h +++ b/arduino_code/Test/src/config.h @@ -33,6 +33,6 @@ extern const uint16_t TMC_MICROSTEPS; // I2C Настройка extern const int16_t I2C_SDA_PIN; -extern const int16_t I2C_SLC_PIN; +extern const int16_t I2C_SCL_PIN; #endif \ No newline at end of file diff --git a/arduino_code/Test/src/main.cpp b/arduino_code/Test/src/main.cpp index 5212cd6..5206e9f 100644 --- a/arduino_code/Test/src/main.cpp +++ b/arduino_code/Test/src/main.cpp @@ -2,6 +2,7 @@ #include #include "config.h" #include "motor.h" +#include "vl53l0x_sensor.h" #include "mqtt_handler.h" void setup() { @@ -10,6 +11,16 @@ void setup() { // Инициализация мотора (Core 1 context initially) motorInit(); + xTaskCreatePinnedToCore( + sensorTask, + "SensorTask", + 4096, + NULL, + 1, + NULL, + 0 + ); + // Запуск задачи MQTT на Core 0 xTaskCreatePinnedToCore( mqttTask, diff --git a/arduino_code/Test/src/mqtt_handler.cpp b/arduino_code/Test/src/mqtt_handler.cpp index f77a4d2..d4862e9 100644 --- a/arduino_code/Test/src/mqtt_handler.cpp +++ b/arduino_code/Test/src/mqtt_handler.cpp @@ -2,8 +2,7 @@ #include "config.h" #include "motor.h" #include "servo_control.h" -#include -#include +#include "vl53l0x_sensor.h" static WiFiClient espClient; static PubSubClient client(espClient); @@ -30,47 +29,117 @@ static int last_pub_driver_status = -1; static int last_pub_tmc_software_enable = -1; static uint8_t last_pub_current_scaling = 255; -// Сервопривод -static int last_pub_angle = -1; -static int last_pub_enabled = -1; -static int last_pub_attached = -1; +// ============================================ +// КЭШИРОВАНИЕ ДЛЯ VL53L0X +// ============================================ + +#define VL53L0X_MAX_CHANNELS 8 +static uint16_t last_pub_vl53_distance[VL53L0X_MAX_CHANNELS] = {65535}; +static uint16_t last_pub_vl53_raw[VL53L0X_MAX_CHANNELS] = {65535}; +static int last_pub_vl53_status[VL53L0X_MAX_CHANNELS] = {-1}; +static int last_pub_vl53_calibrated[VL53L0X_MAX_CHANNELS] = {-1}; +static MeasurementMode last_pub_vl53_mode = MODE_COUNT; + +// ============================================ +// БУФЕРЫ ДЛЯ ПРЕОБРАЗОВАНИЯ +// ============================================ + +static char int_buffer[16]; +static char uint_buffer[16]; + +static const char* intToString(int value) { + snprintf(int_buffer, sizeof(int_buffer), "%d", value); + return int_buffer; +} + +static const char* uintToString(unsigned long value) { + snprintf(uint_buffer, sizeof(uint_buffer), "%lu", value); + return uint_buffer; +} + +// ============================================ +// WIFI И MQTT ПОДКЛЮЧЕНИЕ +// ============================================ 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 resetAllCaches() { + // Motor + last_pub_rpm = -1; + last_pub_steps = (unsigned long)-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; + last_pub_driver_status = -1; + last_pub_tmc_software_enable = -1; + + // VL53L0X + for (int i = 0; i < VL53L0X_MAX_CHANNELS; i++) { + last_pub_vl53_distance[i] = 65535; + last_pub_vl53_raw[i] = 65535; + last_pub_vl53_status[i] = -1; + last_pub_vl53_calibrated[i] = -1; + } + last_pub_vl53_mode = MODE_COUNT; +} + +static void subscribeToAllTopics() { + // Motor + 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"); + + // Servo + client.subscribe("servo/control/+/#"); + + // VL53L0X + client.subscribe("sensor/control/mode"); + client.subscribe("sensor/control/mode_name"); + client.subscribe("sensor/control/calibrate/start/+"); + client.subscribe("sensor/control/calibrate/finish/+"); + client.subscribe("sensor/control/clear_cal/+"); + client.subscribe("sensor/control/enable/+"); + client.subscribe("sensor/control/publish_all"); +} + static void reconnect() { while (!client.connected()) { if (client.connect(MQTT_CLIENT_ID, MQTT_USER, MQTT_PASS)) { - // Сброс кэша - 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/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"); - - client.subscribe("servo/control/+/#"); + resetAllCaches(); + subscribeToAllTopics(); + Serial.println("MQTT Connected and subscribed"); } else { + Serial.printf("MQTT connection failed, rc=%d, retrying...\n", client.state()); vTaskDelay(pdMS_TO_TICKS(5000)); } } } +// ============================================ +// ОБРАБОТКА ВХОДЯЩИХ MQTT КОМАНД +// ============================================ static void callback(char* topic, byte* payload, unsigned int length) { char msg[length + 1]; memcpy(msg, payload, length); @@ -109,111 +178,224 @@ static void callback(char* topic, byte* payload, unsigned int length) { } else if (strcmp(topic, "motor/control/tmc/coolstep") == 0) { tmcSetCoolStep(strcmp(msg, "on") == 0); - } if (strncmp(topic, "servo/control", 12) == 0) { + } + else if (strncmp(topic, "servo/control", 12) == 0) { handleServoMQTTCommand(topic, msg); } + // === VL53L0X === + else if (strcmp(topic, "sensor/control/mode") == 0) { + int mode = atoi(msg); + if (mode >= 0 && mode < MODE_COUNT) { + vl53l0xSetMode((MeasurementMode)mode); + } + } + else if (strcmp(topic, "sensor/control/mode_name") == 0) { + // Маппинг имени режима на ID (если нужно) + // Пока просто логируем + Serial.printf("Mode name request: %s\n", msg); + } + else if (strncmp(topic, "sensor/control/calibrate/start/", 31) == 0) { + int channel = atoi(topic + 31); + int near_mm = atoi(msg); + if (channel >= 0 && channel < VL53L0X_MAX_CHANNELS && near_mm > 0) { + vl53l0xStartCalibration(channel, near_mm); + } + } + else if (strncmp(topic, "sensor/control/calibrate/finish/", 32) == 0) { + int channel = atoi(topic + 32); + int far_mm = atoi(msg); + if (channel >= 0 && channel < VL53L0X_MAX_CHANNELS && far_mm > 0) { + vl53l0xFinishCalibration(channel, far_mm); + } + } + else if (strncmp(topic, "sensor/control/clear_cal/", 25) == 0) { + int channel = atoi(topic + 25); + if (channel >= 0 && channel < VL53L0X_MAX_CHANNELS) { + vl53l0xClearCalibration(channel); + } + } + else if (strncmp(topic, "sensor/control/enable/", 22) == 0) { + int channel = atoi(topic + 22); + bool enable = (strcmp(msg, "on") == 0 || strcmp(msg, "1") == 0 || strcmp(msg, "true") == 0); + if (channel >= 0 && channel < VL53L0X_MAX_CHANNELS) { + vl53l0xEnableChannel(channel, enable); + } + } + else if (strcmp(topic, "sensor/control/publish_all") == 0) { + // Сбрасываем кэш для принудительной публикации + for (int i = 0; i < VL53L0X_MAX_CHANNELS; i++) { + last_pub_vl53_distance[i] = 65535; + last_pub_vl53_raw[i] = 65535; + } + } } -static void publishTelemetry() { - unsigned long now = millis(); - if (now - last_feedback_time >= 500) { // Читаем статусы раз в 500мс +// ============================================ +// ПУБЛИКАЦИЯ MOTOR TELEMETRY +// ============================================ + +static void publishMotorTelemetry() { + // RPM + int rpm = getCurrentRPM(); + if (rpm != last_pub_rpm) { + client.publish("motor/feedback/rpm", intToString(rpm)); + last_pub_rpm = rpm; + } + + // Steps + unsigned long steps = getMotorSteps(); + if (steps != last_pub_steps) { + client.publish("motor/feedback/totalsteps", uintToString(steps)); + last_pub_steps = steps; + } + + // Is running + 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; + } + + // TMC + if (tmcIsInitialized()) { + uint8_t pct = tmcGetRunCurrentPercent(); + if (pct != last_pub_current_pct) { + client.publish("motor/feedback/tmc/current_percent", intToString(pct)); + last_pub_current_pct = pct; + } + + uint16_t ms = tmcGetMicrostepsSetting(); + if (ms != last_pub_microsteps) { + client.publish("motor/feedback/tmc/microsteps", intToString(ms)); + last_pub_microsteps = ms; + } + + uint16_t sg = tmcGetStallGuardResult(); + if (sg != last_pub_sg) { + client.publish("motor/feedback/tmc/sg_result", intToString(sg)); + last_pub_sg = sg; + } + + uint32_t interstep = tmcGetInterstepDuration(); + if (interstep != last_pub_interstep) { + client.publish("motor/feedback/tmc/interstep_duration", uintToString(interstep)); + last_pub_interstep = interstep; + } + + TMC2209::Status status = tmcGetStatus(); - // Базовая телеметрия - int rpm = getCurrentRPM(); - if (rpm != last_pub_rpm) { - client.publish("motor/feedback/rpm", String(rpm).c_str()); - last_pub_rpm = rpm; + 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; } - unsigned long steps = getMotorSteps(); - if (steps != last_pub_steps) { - client.publish("motor/feedback/totalsteps", String(steps).c_str()); - last_pub_steps = steps; + 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 run = isMotorRunning() ? 1 : 0; - if (run != last_pub_is_run) { - client.publish("motor/feedback/is_run", run ? "true" : "false"); - last_pub_is_run = run; + 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; } - // 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; - } + 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; + } - uint16_t ms = tmcGetMicrostepsSetting(); - if (ms != last_pub_microsteps) { - client.publish("motor/feedback/tmc/microsteps", String(ms).c_str()); - last_pub_microsteps = ms; - } + 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; + } - uint16_t sg = tmcGetStallGuardResult(); - if (sg != last_pub_sg) { - client.publish("motor/feedback/tmc/sg_result", String(sg).c_str()); - last_pub_sg = sg; - } + if (status.current_scaling != last_pub_current_scaling) { + client.publish("motor/feedback/tmc/status/current_scaling", intToString(status.current_scaling)); + last_pub_current_scaling = status.current_scaling; + } - uint32_t interstep = tmcGetInterstepDuration(); - if (interstep != last_pub_interstep) { - client.publish("motor/feedback/tmc/interstep_duration", String(interstep).c_str()); - last_pub_interstep = interstep; - } + int cds = checkDriverStatus() ? 1 : 0; + if (cds != last_pub_driver_status) { + client.publish("motor/feedback/driver/status", cds ? "on" : "off"); + last_pub_driver_status = cds; + } + + int tse = checkTmcSoftwareEnable() ? 1 : 0; + if (tse != last_pub_tmc_software_enable) { + client.publish("motor/feedback/tmc/status", tse ? "on" : "off"); + last_pub_tmc_software_enable = tse; + } + } +} - // Чтение полного статуса (требует чтения регистра DRV_STATUS) - TMC2209::Status status = tmcGetStatus(); +// ============================================ +// ПУБЛИКАЦИЯ VL53L0X TELEMETRY +// ============================================ + +static void publishVL53L0XTelemetry() { + // Публикация режима + MeasurementMode current_mode = vl53l0xGetMode(); + if (current_mode != last_pub_vl53_mode) { + const ModeProfile* profile = vl53l0xGetModeProfile(); + + client.publish("sensor/feedback/mode", profile->name); + client.publish("sensor/feedback/mode_id", intToString(current_mode)); + client.publish("sensor/feedback/max_range", intToString(profile->max_range_mm)); + + last_pub_vl53_mode = current_mode; + } + + // Публикация данных с каждого канала + for (uint8_t ch = 0; ch < VL53L0X_MAX_CHANNELS; ch++) { + if (!vl53l0xIsChannelPresent(ch)) continue; + + char topic[64]; + + // Статус канала + int status = vl53l0xIsChannelEnabled(ch) ? 1 : 0; + if (status != last_pub_vl53_status[ch]) { + snprintf(topic, sizeof(topic), "sensor/feedback/%d/status", ch); + client.publish(topic, status ? "on" : "off"); + last_pub_vl53_status[ch] = status; + } + + // Статус калибровки + int calibrated = vl53l0xIsCalibrated(ch) ? 1 : 0; + if (calibrated != last_pub_vl53_calibrated[ch]) { + snprintf(topic, sizeof(topic), "sensor/feedback/%d/calibrated", ch); + client.publish(topic, calibrated ? "true" : "false"); + last_pub_vl53_calibrated[ch] = calibrated; + } + + // Только если канал активен, публикуем расстояния + if (vl53l0xIsChannelActive(ch)) { + // Калиброванное расстояние + uint16_t distance = vl53l0xReadDistance(ch); + if (distance != last_pub_vl53_distance[ch]) { + snprintf(topic, sizeof(topic), "sensor/feedback/%d/distance", ch); + if (distance != 65535) { + client.publish(topic, intToString(distance)); + } else { + client.publish(topic, "out_of_range"); + } + last_pub_vl53_distance[ch] = distance; + } - 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; - } - - int cds = checkDriverStatus() ? 1 : 0; - if (cds != last_pub_driver_status) { - client.publish("motor/feedback/driver/status", cds ? "on" : "off"); - last_pub_driver_status = cds; - } - int tse = checkTmcSoftwareEnable() ? 1 : 0; - if (tse != last_pub_tmc_software_enable) { - client.publish("motor/feedback/tmc/status", tse ? "on" : "off"); - last_pub_tmc_software_enable = tse; + // Сырое значение + uint16_t raw = vl53l0xReadRawDistance(ch); + if (raw != last_pub_vl53_raw[ch]) { + snprintf(topic, sizeof(topic), "sensor/feedback/%d/raw", ch); + if (raw != 65535) { + client.publish(topic, intToString(raw)); + } + last_pub_vl53_raw[ch] = raw; } } - - last_feedback_time = now; } } @@ -292,8 +474,22 @@ void handleServoMQTTCommand(const char* topic, const char* payload) { ///// +// ============================================ +// ГЛАВНЫЙ ЦИКЛ ПУБЛИКАЦИИ +// ============================================ + +static void publishTelemetry() { + unsigned long now = millis(); + if (now - last_feedback_time >= 500) { + publishMotorTelemetry(); + publishVL53L0XTelemetry(); + last_feedback_time = now; + } +} + void mqttTask(void *parameter) { static unsigned long last_stack_check = 0; + setup_wifi(); client.setServer(MQTT_SERVER, MQTT_PORT); client.setCallback(callback); @@ -305,12 +501,6 @@ void mqttTask(void *parameter) { client.loop(); publishTelemetry(); - // Вывод свободного стека каждые 10 секунд - if (millis() - last_stack_check > 10000) { - Serial.printf("[STACK] Free: %u bytes\n", uxTaskGetStackHighWaterMark(NULL)); - last_stack_check = millis(); - } - vTaskDelay(pdMS_TO_TICKS(10)); } } \ No newline at end of file diff --git a/arduino_code/Test/src/mqtt_handler.h b/arduino_code/Test/src/mqtt_handler.h index 0ea14d9..9e2da27 100644 --- a/arduino_code/Test/src/mqtt_handler.h +++ b/arduino_code/Test/src/mqtt_handler.h @@ -2,6 +2,8 @@ #define MQTT_HANDLER_H #include +#include +#include void mqttTask(void *parameter); void handleServoMQTTCommand(const char* topic, const char* payload); diff --git a/arduino_code/Test/src/servo_control.cpp b/arduino_code/Test/src/servo_control.cpp index f14f3da..8e26f1c 100644 --- a/arduino_code/Test/src/servo_control.cpp +++ b/arduino_code/Test/src/servo_control.cpp @@ -11,7 +11,7 @@ void servoInit() { Serial.println("Initializing PCA9685 servo driver..."); // Инициализация I2C на пинах 21 (SDA) и 22 (SCL) - Wire.begin(I2C_SDA_PIN, I2C_SLC_PIN); + Wire.begin(I2C_SDA_PIN, I2C_SCL_PIN); pwm.begin(); pwm.setOscillatorFrequency(27000000); diff --git a/arduino_code/Test/src/vl53l0x_sensor.cpp b/arduino_code/Test/src/vl53l0x_sensor.cpp new file mode 100644 index 0000000..79b3f92 --- /dev/null +++ b/arduino_code/Test/src/vl53l0x_sensor.cpp @@ -0,0 +1,477 @@ +#include "vl53l0x_sensor.h" +#include "config.h" + +// ============================================ +// КОНСТАНТЫ +// ============================================ + +#define TCA9548A_ADDRESS 0x70 +#define TCA9548A_CHANNELS 8 + +#define MIN_DISTANCE_MM 30 +#define OUT_OF_RANGE_VALUE 65535 + +#define FILTER_SIZE 5 + +// ============================================ +// ПРОФИЛИ РЕЖИМОВ +// ============================================ + +static const ModeProfile MODES[MODE_COUNT] = { + { + "HIGH_ACCURACY", + 200000, 14, 10, 0.5f, 500, 2 + }, + { + "PRECISION", + 66000, 14, 10, 0.3f, 1000, 5 + }, + { + "DEFAULT", + 33000, 14, 10, 0.25f, 1200, 15 + }, + { + "LONG_RANGE", + 33000, 18, 14, 0.1f, 2000, 40 + }, + { + "ULTRA_LONG", + 100000, 18, 14, 0.05f, 2500, 80 + } +}; + +// ============================================ +// ГЛОБАЛЬНЫЕ ПЕРЕМЕННЫЕ +// ============================================ + +static VL53L0X sensor; +static Preferences preferences; + +static MeasurementMode current_mode = MODE_DEFAULT; +static bool sensor_present[TCA9548A_CHANNELS] = {false}; +static bool channel_enabled[TCA9548A_CHANNELS] = {false}; +static CalibrationData calibration[TCA9548A_CHANNELS]; + +static uint16_t filter_buffer[TCA9548A_CHANNELS][FILTER_SIZE]; +static uint8_t filter_index[TCA9548A_CHANNELS] = {0}; + +static bool calibration_in_progress = false; +static uint8_t calibration_channel = 0; +static uint16_t calibration_near_raw = 0; +static uint16_t calibration_near_known = 0; + +// ============================================ +// TCA9548A МУЛЬТИПЛЕКСОР +// ============================================ + +static void TCA9548A_Select(uint8_t channel) { + Wire.beginTransmission(TCA9548A_ADDRESS); + Wire.write((channel < TCA9548A_CHANNELS) ? (1 << channel) : 0x00); + Wire.endTransmission(); + delay(3); +} + +static void TCA9548A_DisableAll() { + Wire.beginTransmission(TCA9548A_ADDRESS); + Wire.write(0x00); + Wire.endTransmission(); +} + +static bool checkTCA9548A() { + Wire.beginTransmission(TCA9548A_ADDRESS); + return (Wire.endTransmission() == 0); +} + +// ============================================ +// NVS (СОХРАНЕНИЕ В FLASH) +// ============================================ + +static void loadCalibration() { + preferences.begin("vl53_cal", true); + + for (uint8_t ch = 0; ch < TCA9548A_CHANNELS; ch++) { + char key[16]; + snprintf(key, sizeof(key), "ch%d", ch); + + size_t len = preferences.getBytesLength(key); + if (len == sizeof(CalibrationData)) { + preferences.getBytes(key, &calibration[ch], sizeof(CalibrationData)); + } else { + calibration[ch].valid = false; + calibration[ch].scale = 1.0f; + calibration[ch].offset = 0.0f; + } + } + + int saved_mode = preferences.getInt("mode", MODE_DEFAULT); + if (saved_mode >= 0 && saved_mode < MODE_COUNT) { + current_mode = (MeasurementMode)saved_mode; + } + + preferences.end(); +} + +static void saveCalibration(uint8_t channel) { + preferences.begin("vl53_cal", false); + char key[16]; + snprintf(key, sizeof(key), "ch%d", channel); + preferences.putBytes(key, &calibration[channel], sizeof(CalibrationData)); + preferences.end(); +} + +static void saveMode() { + preferences.begin("vl53_cal", false); + preferences.putInt("mode", (int)current_mode); + preferences.end(); +} + +// ============================================ +// ПРИМЕНЕНИЕ РЕЖИМА +// ============================================ + +static bool applyModeProfile() { + const ModeProfile& profile = MODES[current_mode]; + + sensor.setTimeout(500); + + if (!sensor.init()) { + return false; + } + + sensor.setAddress(0x29); + sensor.setSignalRateLimit(profile.signal_rate_limit); + + sensor.setVcselPulsePeriod(VL53L0X::VcselPeriodPreRange, profile.vcsel_prerange); + sensor.setVcselPulsePeriod(VL53L0X::VcselPeriodFinalRange, profile.vcsel_final); + sensor.setMeasurementTimingBudget(profile.timing_budget_us); + + return true; +} + +// ============================================ +// КАЛИБРОВКА И ФИЛЬТРАЦИЯ +// ============================================ + +static uint16_t applyCalibration(uint8_t channel, uint16_t raw_mm) { + if (!calibration[channel].valid || raw_mm == OUT_OF_RANGE_VALUE) { + return raw_mm; + } + + float calibrated = (float)raw_mm * calibration[channel].scale + calibration[channel].offset; + + const ModeProfile& profile = MODES[current_mode]; + if (calibrated < MIN_DISTANCE_MM) calibrated = MIN_DISTANCE_MM; + if (calibrated > profile.max_range_mm) return OUT_OF_RANGE_VALUE; + + return (uint16_t)calibrated; +} + +static uint16_t applyFilter(uint8_t channel, uint16_t new_value) { + if (new_value == OUT_OF_RANGE_VALUE) return OUT_OF_RANGE_VALUE; + + filter_buffer[channel][filter_index[channel]] = new_value; + filter_index[channel] = (filter_index[channel] + 1) % FILTER_SIZE; + + uint32_t sum = 0; + uint8_t count = 0; + for (uint8_t i = 0; i < FILTER_SIZE; i++) { + if (filter_buffer[channel][i] != 0) { + sum += filter_buffer[channel][i]; + count++; + } + } + + return (count > 0) ? (uint16_t)(sum / count) : new_value; +} + +// ============================================ +// ПУБЛИЧНЫЕ ФУНКЦИИ +// ============================================ + +void vl53l0xInit() { + Serial.println("Initializing VL53L0X sensors..."); + + Wire.begin(I2C_SDA_PIN, I2C_SCL_PIN); + Wire.setClock(400000); + delay(100); + + if (!checkTCA9548A()) { + Serial.println("ERROR: TCA9548A not found!"); + return; + } + Serial.println("✓ TCA9548A found"); + TCA9548A_DisableAll(); + + loadCalibration(); + Serial.printf("✓ Loaded mode: %s\n", MODES[current_mode].name); + + Serial.println("\nScanning VL53L0X channels:"); + for (uint8_t channel = 0; channel < TCA9548A_CHANNELS; channel++) { + TCA9548A_Select(channel); + delay(20); + + if (applyModeProfile()) { + sensor_present[channel] = true; + channel_enabled[channel] = true; + Serial.printf(" ✓ Channel %d: VL53L0X found", channel); + if (calibration[channel].valid) Serial.print(" [CALIBRATED]"); + Serial.println(); + + memset(filter_buffer[channel], 0, sizeof(filter_buffer[channel])); + filter_index[channel] = 0; + } else { + sensor_present[channel] = false; + channel_enabled[channel] = false; + Serial.printf(" ✗ Channel %d: No device\n", channel); + } + + TCA9548A_DisableAll(); + delay(5); + } + + Serial.printf("\n✓ VL53L0X initialized: %d sensors found\n\n", + vl53l0xGetChannelCount()); +} + +void vl53l0xLoop() { + // Внутренняя логика датчиков (если нужна) + // Сейчас вся публикация в mqtt_handle.cpp +} + +bool vl53l0xSetMode(MeasurementMode mode) { + if (mode < 0 || mode >= MODE_COUNT) { + Serial.printf("ERROR: Invalid mode %d\n", mode); + return false; + } + + current_mode = mode; + saveMode(); + + const ModeProfile& profile = MODES[current_mode]; + Serial.printf("✓ Mode changed to: %s (max %d mm)\n", + profile.name, profile.max_range_mm); + + return true; +} + +MeasurementMode vl53l0xGetMode() { + return current_mode; +} + +const ModeProfile* vl53l0xGetModeProfile() { + return &MODES[current_mode]; +} + +uint16_t vl53l0xReadDistance(uint8_t channel) { + if (channel >= TCA9548A_CHANNELS || !sensor_present[channel] || !channel_enabled[channel]) { + return OUT_OF_RANGE_VALUE; + } + + TCA9548A_Select(channel); + + if (!applyModeProfile()) { + TCA9548A_DisableAll(); + return OUT_OF_RANGE_VALUE; + } + + uint16_t distance = sensor.readRangeSingleMillimeters(); + bool timeout = sensor.timeoutOccurred(); + + TCA9548A_DisableAll(); + + if (distance == 65535 || timeout) return OUT_OF_RANGE_VALUE; + + const ModeProfile& profile = MODES[current_mode]; + if (distance < MIN_DISTANCE_MM || distance > profile.max_range_mm) { + return OUT_OF_RANGE_VALUE; + } + + uint16_t filtered = applyFilter(channel, distance); + uint16_t calibrated = applyCalibration(channel, filtered); + + return calibrated; +} + +uint16_t vl53l0xReadRawDistance(uint8_t channel) { + if (channel >= TCA9548A_CHANNELS || !sensor_present[channel] || !channel_enabled[channel]) { + return OUT_OF_RANGE_VALUE; + } + + TCA9548A_Select(channel); + + if (!applyModeProfile()) { + TCA9548A_DisableAll(); + return OUT_OF_RANGE_VALUE; + } + + uint16_t distance = sensor.readRangeSingleMillimeters(); + bool timeout = sensor.timeoutOccurred(); + + TCA9548A_DisableAll(); + + if (distance == 65535 || timeout) return OUT_OF_RANGE_VALUE; + + return distance; +} + +bool vl53l0xIsChannelActive(uint8_t channel) { + return (channel < TCA9548A_CHANNELS && sensor_present[channel] && channel_enabled[channel]); +} + +int vl53l0xGetChannelCount() { + int count = 0; + for (uint8_t i = 0; i < TCA9548A_CHANNELS; i++) { + if (sensor_present[i]) count++; + } + return count; +} + +bool vl53l0xStartCalibration(uint8_t channel, uint16_t near_known_mm) { + if (channel >= TCA9548A_CHANNELS || !sensor_present[channel]) { + Serial.printf("ERROR: Channel %d not available\n", channel); + return false; + } + + Serial.printf("Starting calibration for channel %d (near point: %d mm)\n", + channel, near_known_mm); + + uint32_t sum = 0; + uint8_t valid = 0; + + for (uint8_t i = 0; i < 20; i++) { + uint16_t d = vl53l0xReadRawDistance(channel); + if (d != OUT_OF_RANGE_VALUE) { + sum += d; + valid++; + } + delay(50); + } + + if (valid == 0) { + Serial.println("ERROR: Failed to read near point"); + return false; + } + + calibration_near_raw = (uint16_t)(sum / valid); + calibration_near_known = near_known_mm; + calibration_channel = channel; + calibration_in_progress = true; + + Serial.printf("✓ Near point captured: raw=%d mm, actual=%d mm\n", + calibration_near_raw, calibration_near_known); + Serial.println("Now place object at FAR point and call vl53l0xFinishCalibration()"); + + return true; +} + +bool vl53l0xFinishCalibration(uint8_t channel, uint16_t far_known_mm) { + if (!calibration_in_progress || channel != calibration_channel) { + Serial.println("ERROR: Calibration not in progress or wrong channel"); + return false; + } + + Serial.printf("Finishing calibration for channel %d (far point: %d mm)\n", + channel, far_known_mm); + + uint32_t sum = 0; + uint8_t valid = 0; + + for (uint8_t i = 0; i < 20; i++) { + uint16_t d = vl53l0xReadRawDistance(channel); + if (d != OUT_OF_RANGE_VALUE) { + sum += d; + valid++; + } + delay(50); + } + + if (valid == 0) { + Serial.println("ERROR: Failed to read far point"); + calibration_in_progress = false; + return false; + } + + uint16_t far_raw = (uint16_t)(sum / valid); + + Serial.printf("✓ Far point captured: raw=%d mm, actual=%d mm\n", + far_raw, far_known_mm); + + if (far_raw == calibration_near_raw) { + Serial.println("ERROR: Raw values are identical"); + calibration_in_progress = false; + return false; + } + + float scale = (float)(far_known_mm - calibration_near_known) / + (float)(far_raw - calibration_near_raw); + float offset = (float)calibration_near_known - + (float)calibration_near_raw * scale; + + calibration[channel].valid = true; + calibration[channel].near_raw = calibration_near_raw; + calibration[channel].near_known = calibration_near_known; + calibration[channel].far_raw = far_raw; + calibration[channel].far_known = far_known_mm; + calibration[channel].scale = scale; + calibration[channel].offset = offset; + + saveCalibration(channel); + + Serial.printf("✓ Calibration complete: scale=%.5f, offset=%.2f\n", scale, offset); + + calibration_in_progress = false; + + return true; +} + +void vl53l0xClearCalibration(uint8_t channel) { + if (channel >= TCA9548A_CHANNELS) return; + + calibration[channel].valid = false; + calibration[channel].scale = 1.0f; + calibration[channel].offset = 0.0f; + saveCalibration(channel); + + Serial.printf("✓ Channel %d calibration cleared\n", channel); +} + +bool vl53l0xIsCalibrated(uint8_t channel) { + return (channel < TCA9548A_CHANNELS && calibration[channel].valid); +} + +CalibrationData vl53l0xGetCalibration(uint8_t channel) { + if (channel >= TCA9548A_CHANNELS) { + CalibrationData empty = {false, 0, 0, 0, 0, 1.0f, 0.0f}; + return empty; + } + return calibration[channel]; +} + +void vl53l0xEnableChannel(uint8_t channel, bool enable) { + if (channel >= TCA9548A_CHANNELS) return; + + if (!sensor_present[channel]) { + Serial.printf("ERROR: Channel %d not present\n", channel); + return; + } + + channel_enabled[channel] = enable; + Serial.printf("✓ Channel %d %s\n", channel, enable ? "enabled" : "disabled"); +} + +bool vl53l0xIsChannelEnabled(uint8_t channel) { + return (channel < TCA9548A_CHANNELS && channel_enabled[channel]); +} + +bool vl53l0xIsChannelPresent(uint8_t channel) { + return (channel < TCA9548A_CHANNELS && sensor_present[channel]); +} + +void sensorTask(void *parameter) { + vl53l0xInit(); + + for (;;) { + vl53l0xLoop(); + vTaskDelay(pdMS_TO_TICKS(50)); + } +} \ No newline at end of file diff --git a/arduino_code/Test/src/vl53l0x_sensor.h b/arduino_code/Test/src/vl53l0x_sensor.h new file mode 100644 index 0000000..c3a7e91 --- /dev/null +++ b/arduino_code/Test/src/vl53l0x_sensor.h @@ -0,0 +1,68 @@ +#ifndef VL53L0X_SENSOR_H +#define VL53L0X_SENSOR_H + +#include +#include +#include +#include + +// Режимы измерения +enum MeasurementMode { + MODE_HIGH_ACCURACY = 0, + MODE_PRECISION = 1, + MODE_DEFAULT = 2, + MODE_LONG_RANGE = 3, + MODE_ULTRA_LONG = 4, + MODE_COUNT = 5 +}; + +struct ModeProfile { + const char* name; + uint32_t timing_budget_us; + uint8_t vcsel_prerange; + uint8_t vcsel_final; + float signal_rate_limit; + uint16_t max_range_mm; + uint8_t accuracy_mm; +}; + +struct CalibrationData { + bool valid; + uint16_t near_raw; + uint16_t near_known; + uint16_t far_raw; + uint16_t far_known; + float scale; + float offset; +}; + +// Инициализация и цикл +void vl53l0xInit(); +void vl53l0xLoop(); + +// Управление режимами +bool vl53l0xSetMode(MeasurementMode mode); +MeasurementMode vl53l0xGetMode(); +const ModeProfile* vl53l0xGetModeProfile(); + +// Чтение данных +uint16_t vl53l0xReadDistance(uint8_t channel); +uint16_t vl53l0xReadRawDistance(uint8_t channel); +bool vl53l0xIsChannelActive(uint8_t channel); +int vl53l0xGetChannelCount(); + +// Калибровка +bool vl53l0xStartCalibration(uint8_t channel, uint16_t near_known_mm); +bool vl53l0xFinishCalibration(uint8_t channel, uint16_t far_known_mm); +void vl53l0xClearCalibration(uint8_t channel); +bool vl53l0xIsCalibrated(uint8_t channel); +CalibrationData vl53l0xGetCalibration(uint8_t channel); + +// Управление каналами +void vl53l0xEnableChannel(uint8_t channel, bool enable); +bool vl53l0xIsChannelEnabled(uint8_t channel); +bool vl53l0xIsChannelPresent(uint8_t channel); + +void sensorTask(void *parameter); + +#endif // VL53L0X_SENSOR_H \ No newline at end of file