drholy/ozone-tech_owl_prime
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

New: adding vl53l0x sensor

Browse Source
This commit is contained in:
drho1y
2026-07-16 03:18:20 +03:00
parent 2831c396b9
commit 3b9fc4c874
10 changed files with 970 additions and 212 deletions
Download Patch File Download Diff File Expand all files Collapse all files

187
arduino_code/Test/README.md
View File

@@ -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`).

1
arduino_code/Test/platformio.ini
View File

@@ -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

2
arduino_code/Test/src/config.cpp
View File

@@ -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 ==========

2
arduino_code/Test/src/config.h
View File

@@ -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

11
arduino_code/Test/src/main.cpp
View File

@@ -2,6 +2,7 @@
#include <TMC2209.h>
#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,

430
arduino_code/Test/src/mqtt_handler.cpp
View File

@@ -2,8 +2,7 @@
#include "config.h"
#include "motor.h"
#include "servo_control.h"
#include <WiFi.h>
#include <PubSubClient.h>
#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));
}
}

2
arduino_code/Test/src/mqtt_handler.h
View File

@@ -2,6 +2,8 @@
#define MQTT_HANDLER_H
#include <Arduino.h>
#include <WiFi.h>
#include <PubSubClient.h>
void mqttTask(void *parameter);
void handleServoMQTTCommand(const char* topic, const char* payload);

2
arduino_code/Test/src/servo_control.cpp
View File

@@ -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);

477
arduino_code/Test/src/vl53l0x_sensor.cpp Normal file
View File

@@ -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));
}
}

68
arduino_code/Test/src/vl53l0x_sensor.h Normal file
View File

@@ -0,0 +1,68 @@
#ifndef VL53L0X_SENSOR_H
#define VL53L0X_SENSOR_H
#include <Arduino.h>
#include <VL53L0X.h>
#include <Wire.h>
#include <Preferences.h>
// Режимы измерения
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