Compare commits
No commits in common. "main" and "project-refactoring-v2" have entirely different histories.
main
...
project-re
|
|
@ -1,7 +0,0 @@
|
||||||
feat(modbus): Implement persistent and improved reconfiguration for Modbus server
|
|
||||||
|
|
||||||
This commit enhances the Modbus server's configuration handling by:
|
|
||||||
|
|
||||||
- Loading saved baudrate and unit ID settings during initialization, ensuring persistence across reboots.
|
|
||||||
- Providing improved feedback during `modbus_reconfigure`, including logging for successful changes and informing the user when a device restart is required for changes to take effect.
|
|
||||||
- Saving new configuration settings even if immediate reinitialization fails, allowing them to be applied on the next boot.
|
|
||||||
|
|
@ -1,65 +1 @@
|
||||||
**/build
|
**/build
|
||||||
|
|
||||||
# Zephyr build directories
|
|
||||||
build/
|
|
||||||
build-*/
|
|
||||||
*/build/
|
|
||||||
**/build/
|
|
||||||
|
|
||||||
# Zephyr out-of-tree build directories
|
|
||||||
out-of-tree-build/
|
|
||||||
|
|
||||||
# Files generated by the build system
|
|
||||||
zephyr.elf
|
|
||||||
zephyr.bin
|
|
||||||
zephyr.hex
|
|
||||||
zephyr.map
|
|
||||||
zephyr.strip
|
|
||||||
zephyr.lst
|
|
||||||
zephyr.asm
|
|
||||||
zephyr.stat
|
|
||||||
zephyr.a
|
|
||||||
zephyr.o
|
|
||||||
*.o
|
|
||||||
*.a
|
|
||||||
*.so
|
|
||||||
*.so.*
|
|
||||||
*.dll
|
|
||||||
*.exe
|
|
||||||
|
|
||||||
# Cmake
|
|
||||||
CMakeCache.txt
|
|
||||||
CMakeFiles/
|
|
||||||
cmake_install.cmake
|
|
||||||
CTestTestfile.cmake
|
|
||||||
compile_commands.json
|
|
||||||
|
|
||||||
# Kconfig generated files
|
|
||||||
.config
|
|
||||||
.config.old
|
|
||||||
autoconf.h
|
|
||||||
|
|
||||||
# Doxygen
|
|
||||||
doxygen/
|
|
||||||
|
|
||||||
# west
|
|
||||||
.west/
|
|
||||||
west.yml.bak
|
|
||||||
|
|
||||||
# Editor-specific files
|
|
||||||
.vscode/
|
|
||||||
.idea/
|
|
||||||
*.swp
|
|
||||||
*~
|
|
||||||
*.bak
|
|
||||||
*.orig
|
|
||||||
|
|
||||||
# Python
|
|
||||||
__pycache__/
|
|
||||||
*.pyc
|
|
||||||
|
|
||||||
# Mac OS X
|
|
||||||
.DS_Store
|
|
||||||
|
|
||||||
# Windows
|
|
||||||
Thumbs.db
|
|
||||||
|
|
@ -1,3 +0,0 @@
|
||||||
[submodule "software/modules/zephyr_vnd7050aj_driver"]
|
|
||||||
path = software/modules/zephyr_vnd7050aj_driver
|
|
||||||
url = https://gitea.iten.pro/edi/zephyr_vnd7050aj_driver.git
|
|
||||||
|
|
@ -1,14 +0,0 @@
|
||||||
{
|
|
||||||
"version": "2.0.0",
|
|
||||||
"tasks": [
|
|
||||||
{
|
|
||||||
"type": "shell",
|
|
||||||
"label": "Build Zephyr app",
|
|
||||||
"command": "west build -b weact_stm32g431_core .",
|
|
||||||
"group": "build",
|
|
||||||
"problemMatcher": [
|
|
||||||
"$gcc"
|
|
||||||
]
|
|
||||||
}
|
|
||||||
]
|
|
||||||
}
|
|
||||||
|
|
@ -39,7 +39,7 @@ Die Slave-Nodes sind die Arbeitseinheiten im Feld. Um bei der Fertigung kleiner
|
||||||
|
|
||||||
* **Mikrocontroller:** Ein `STM32G431PB`. Dieser ist zwar leistungsstark, bietet aber alle nötigen Peripherien (mehrere UARTs, ADCs, CAN) und ermöglicht ein einheitliches Hardware- und Software-Design.
|
* **Mikrocontroller:** Ein `STM32G431PB`. Dieser ist zwar leistungsstark, bietet aber alle nötigen Peripherien (mehrere UARTs, ADCs, CAN) und ermöglicht ein einheitliches Hardware- und Software-Design.
|
||||||
* **Peripherie pro Node:**
|
* **Peripherie pro Node:**
|
||||||
* **Zwei High-Side Ausgänge (+12V):** Realisiert über einen `VND7050AJ`. Perfekt zur Ansteuerung der 12V-Motorventile (`Öffnen`/`Schliessen`). Die `Sense`-Leitung des Treibers wird über einen AD-Wandler ausgelesen, um durch Messung des Motorstroms eine Endlagen-Erkennung ohne physische Endschalter zu realisieren (Motorstrom im Stillstand ≈ 0). Zusätzlich können die Temperatur und die Versorgungsspannung des Treibers ausgelesen werden.
|
* **Zwei High-Side Ausgänge (+12V):** Realisiert über einen `VND7050AJ`. Perfekt zur Ansteuerung der 12V-Motorventile (`Öffnen`/`Schliessen`). Die `Sense`-Leitung des Treibers wird über einen AD-Wandler ausgelesen, um durch Messung des Motorstroms eine Endlagen-Erkennung ohne physische Endschalter zu realisieren (Motorstrom im Stillstand ≈ 0).
|
||||||
* **Zwei Low-Side Ausgänge (0V):** Über N-Kanal-MOSFETs geschaltete Ausgänge. Nutzbar zur Ansteuerung von 12V-LEDs in Tastern oder zum Schalten des Halbleiter-Relais für die Pumpe.
|
* **Zwei Low-Side Ausgänge (0V):** Über N-Kanal-MOSFETs geschaltete Ausgänge. Nutzbar zur Ansteuerung von 12V-LEDs in Tastern oder zum Schalten des Halbleiter-Relais für die Pumpe.
|
||||||
* **Zwei digitale Eingänge:** Direkte, geschützte Eingänge am Controller zum Anschluss von Tastern oder den kapazitiven NPN-Sensoren.
|
* **Zwei digitale Eingänge:** Direkte, geschützte Eingänge am Controller zum Anschluss von Tastern oder den kapazitiven NPN-Sensoren.
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -29,38 +29,32 @@ Alle Register sind in einer einzigen, durchgehenden Liste pro Register-Typ (`Inp
|
||||||
|
|
||||||
| Adresse (hex) | Name | Zugehörigkeit | Beschreibung |
|
| Adresse (hex) | Name | Zugehörigkeit | Beschreibung |
|
||||||
| :------------ | :----------------------------- | :---------------- | :---------------------------------------------------------------------------------------------------------------------------------------- |
|
| :------------ | :----------------------------- | :---------------- | :---------------------------------------------------------------------------------------------------------------------------------------- |
|
||||||
| **0x0000** | `VALVE_STATE_MOVEMENT` | Ventil | Kombiniertes Status-Register. **High-Byte**: Bewegung (`0`=Idle, `1`=Öffnet, `2`=Schliesst, `3`=Fehler). **Low-Byte**: Zustand (`0`=Geschlossen, `1`=Geöffnet). |
|
| **0x0000** | `VENTIL_ZUSTAND_BEWEGUNG` | Ventil | Kombiniertes Status-Register. **High-Byte**: Bewegung (`0`=Idle, `1`=Öffnet, `2`=Schliesst, `3`=Fehler). **Low-Byte**: Zustand (`0`=Geschlossen, `1`=Geöffnet). |
|
||||||
| **0x0001** | `REG_INPUT_MOTOR_OPEN_CURRENT_MA` | Ventil | Motorstrom beim Öffnen in Milliampere (mA). |
|
| **0x0001** | `MOTORSTROM_MA` | Ventil | Aktueller Motorstrom in Milliampere (mA). |
|
||||||
| **0x0002** | `REG_INPUT_MOTOR_CLOSE_CURRENT_MA` | Ventil | Motorstrom beim Schließen in Milliampere (mA). |
|
| **0x0020** | `DIGITAL_EINGAENGE_ZUSTAND` | Eingänge | Bitmaske der digitalen Eingänge. Bit 0: Eingang 1, Bit 1: Eingang 2. `1`=Aktiv. |
|
||||||
| **0x0020** | `REG_INPUT_DIGITAL_INPUTS_STATE` | Eingänge | Bitmaske der digitalen Eingänge. Bit 0: Eingang 1, Bit 1: Eingang 2. `1`=Aktiv. |
|
| **0x0021** | `TASTER_EVENTS` | Eingänge | Event-Flags für Taster (Clear-on-Read). Bit 0: Taster 1 gedrückt. Bit 1: Taster 2 gedrückt. |
|
||||||
| **0x0021** | `REG_INPUT_BUTTON_EVENTS` | Eingänge | Event-Flags für Taster (Clear-on-Read). Bit 0: Taster 1 gedrückt. Bit 1: Taster 2 gedrückt. |
|
| **0x00F0** | `FIRMWARE_VERSION_MAJOR_MINOR` | System | z.B. `0x0102` für v1.2. |
|
||||||
| **0x00F0** | `REG_INPUT_FIRMWARE_VERSION_MAJOR_MINOR` | System | z.B. `0x0102` für v1.2. |
|
| **0x00F1** | `FIRMWARE_VERSION_PATCH` | System | z.B. `3` für v1.2.3. |
|
||||||
| **0x00F1** | `REG_INPUT_FIRMWARE_VERSION_PATCH` | System | z.B. `3` für v1.2.3. |
|
| **0x00F2** | `DEVICE_STATUS` | System | `0`=OK, `1`=Allgemeiner Fehler. |
|
||||||
| **0x00F2** | `REG_INPUT_DEVICE_STATUS` | System | `0`=OK, `1`=Allgemeiner Fehler. |
|
| **0x00F3** | `UPTIME_SECONDS_LOW` | System | Untere 16 Bit der Uptime in Sekunden. |
|
||||||
| **0x00F3** | `REG_INPUT_UPTIME_SECONDS_LOW` | System | Untere 16 Bit der Uptime in Sekunden. |
|
| **0x00F4** | `UPTIME_SECONDS_HIGH` | System | Obere 16 Bit der Uptime. |
|
||||||
| **0x00F4** | `REG_INPUT_UPTIME_SECONDS_HIGH` | System | Obere 16 Bit der Uptime. |
|
| **0x0100** | `FWU_LAST_CHUNK_CRC` | Firmware-Update | Enthält den CRC16 des zuletzt im Puffer empfangenen Daten-Chunks. |
|
||||||
| **0x00F5** | `REG_INPUT_SUPPLY_VOLTAGE_MV` | System | Aktuelle Versorgungsspannung in Millivolt (mV). |
|
|
||||||
| **0x0100** | `REG_INPUT_FWU_LAST_CHUNK_CRC` | Firmware-Update | Enthält den CRC16 des zuletzt im Puffer empfangenen Daten-Chunks. |
|
|
||||||
|
|
||||||
## 3. Holding Registers (4xxxx, Read/Write)
|
## 3. Holding Registers (4xxxx, Read/Write)
|
||||||
|
|
||||||
| Adresse (hex) | Name | Zugehörigkeit | Beschreibung |
|
| Adresse (hex) | Name | Zugehörigkeit | Beschreibung |
|
||||||
| :------------ | :---------------------------- | :---------------- | :---------------------------------------------------------------------------------------------------------------------------------------- |
|
| :------------ | :---------------------------- | :---------------- | :---------------------------------------------------------------------------------------------------------------------------------------- |
|
||||||
| **0x0000** | `REG_HOLDING_VALVE_COMMAND` | Ventil | `1`=Öffnen, `2`=Schliessen, `0`=Bewegung stoppen. |
|
| **0x0000** | `VENTIL_BEFEHL` | Ventil | `1`=Öffnen, `2`=Schliessen, `0`=Bewegung stoppen. |
|
||||||
| **0x0001** | `REG_HOLDING_MAX_OPENING_TIME_S` | Ventil | Sicherheits-Timeout in Sekunden für den Öffnen-Vorgang. |
|
| **0x0001** | `MAX_OEFFNUNGSZEIT_S` | Ventil | Sicherheits-Timeout in Sekunden für den Öffnen-Vorgang. |
|
||||||
| **0x0002** | `REG_HOLDING_MAX_CLOSING_TIME_S` | Ventil | Sicherheits-Timeout in Sekunden für den Schliessen-Vorgang. |
|
| **0x0002** | `MAX_SCHLIESSZEIT_S` | Ventil | Sicherheits-Timeout in Sekunden für den Schliessen-Vorgang. |
|
||||||
| **0x0003** | `REG_HOLDING_END_CURRENT_THRESHOLD_OPEN_MA` | Ventil | Minimaler Stromschwellenwert in mA zur Endlagenerkennung beim Öffnen. |
|
| **0x0010** | `DIGITAL_AUSGAENGE_ZUSTAND` | Ausgänge | Bitmaske zum Lesen und Schreiben der Ausgänge. Bit 0: Ausgang 1, Bit 1: Ausgang 2. `1`=AN, `0`=AUS. |
|
||||||
| **0x0004** | `REG_HOLDING_END_CURRENT_THRESHOLD_CLOSE_MA` | Ventil | Minimaler Stromschwellenwert in mA zur Endlagenerkennung beim Schliessen. |
|
| **0x00F0** | `WATCHDOG_TIMEOUT_S` | System | Timeout des Fail-Safe-Watchdogs in Sekunden. `0`=Deaktiviert. |
|
||||||
| **0x0005** | `REG_HOLDING_OBSTACLE_THRESHOLD_OPEN_MA` | Ventil | Stromschwellenwert in mA für die Hinderniserkennung beim Öffnen. |
|
| **0x00F1** | `DEVICE_RESET` | System | Schreibt `1` um das Gerät neu zu starten. |
|
||||||
| **0x0006** | `REG_HOLDING_OBSTACLE_THRESHOLD_CLOSE_MA` | Ventil | Stromschwellenwert in mA für die Hinderniserkennung beim Schließen. |
|
| **0x0100** | `FWU_COMMAND` | Firmware-Update | `1`: **Verify Chunk**: Der zuletzt übertragene Chunk wurde vom Client als gültig befunden. Der Slave soll ihn nun ins Flash schreiben. `2`: **Finalize Update**: Alle Chunks sind übertragen. Installation abschliessen und neu starten. |
|
||||||
| **0x0010** | `REG_HOLDING_DIGITAL_OUTPUTS_STATE` | Ausgänge | Bitmaske zum Lesen und Schreiben der Ausgänge. Bit 0: Ausgang 1, Bit 1: Ausgang 2. `1`=AN, `0`=AUS. |
|
| **0x0101** | `FWU_CHUNK_OFFSET_LOW` | Firmware-Update | Untere 16 Bit des 32-Bit-Offsets, an den der nächste Chunk geschrieben werden soll. |
|
||||||
| **0x00F0** | `REG_HOLDING_WATCHDOG_TIMEOUT_S` | System | Timeout des Fail-Safe-Watchdogs in Sekunden. `0`=Deaktiviert. |
|
| **0x0102** | `FWU_CHUNK_OFFSET_HIGH` | Firmware-Update | Obere 16 Bit des 32-Bit-Offsets. |
|
||||||
| **0x00F1** | `REG_HOLDING_DEVICE_RESET` | System | Schreibt `1` um das Gerät neu zu starten. |
|
| **0x0103** | `FWU_CHUNK_SIZE` | Firmware-Update | Grösse des nächsten Chunks in Bytes (max. 256). |
|
||||||
| **0x0100** | `REG_HOLDING_FWU_COMMAND` | Firmware-Update | `1`: **Verify Chunk**: Der zuletzt übertragene Chunk wurde vom Client als gültig befunden. Der Slave soll ihn nun ins Flash schreiben. `2`: **Finalize Update**: Alle Chunks sind übertragen. Installation abschliessen und neu starten. |
|
| **0x0180** | `FWU_DATA_BUFFER` | Firmware-Update | **Startadresse** eines 128x16-bit Puffers (256 Bytes). Entspricht den Registern `40384` bis `40511`. |
|
||||||
| **0x0101** | `REG_HOLDING_FWU_CHUNK_OFFSET_LOW` | Firmware-Update | Untere 16 Bit des 32-Bit-Offsets, an den der nächste Chunk geschrieben werden soll. |
|
|
||||||
| **0x0102** | `REG_HOLDING_FWU_CHUNK_OFFSET_HIGH` | Firmware-Update | Obere 16 Bit des 32-Bit-Offsets. |
|
|
||||||
| **0x0103** | `REG_HOLDING_FWU_CHUNK_SIZE` | Firmware-Update | Grösse des nächsten Chunks in Bytes (max. 256). |
|
|
||||||
| **0x0180** | `REG_HOLDING_FWU_DATA_BUFFER` | Firmware-Update | **Startadresse** eines 128x16-bit Puffers (256 Bytes). Entspricht den Registern `40384` bis `40511`. |
|
|
||||||
|
|
||||||
## 4. Detaillierter Firmware-Update-Prozess
|
## 4. Detaillierter Firmware-Update-Prozess
|
||||||
|
|
||||||
|
|
@ -85,10 +79,10 @@ Diese Register gehören zum externen Füllstandsensor und können auf dem Bus eb
|
||||||
|
|
||||||
| Adresse (hex) | Name | R/W | Beschreibung |
|
| Adresse (hex) | Name | R/W | Beschreibung |
|
||||||
| :------------ | :------------------------- | :-- | :---------------------------------------------------------------------------------------------------------------------------------------- |
|
| :------------ | :------------------------- | :-- | :---------------------------------------------------------------------------------------------------------------------------------------- |
|
||||||
| **0x0000** | `NODE_ADDRESS` | R/W | Geräteadresse des Sensors (1-255). |
|
| **0x0000** | `NODE_ADRESSE` | R/W | Geräteadresse des Sensors (1-255). |
|
||||||
| **0x0001** | `BAUDRATE` | R/W | `0`=1200, `1`=2400, `2`=4800, `3`=9600, `4`=19200, `5`=38400, `6`=57600, `7`=115200. |
|
| **0x0001** | `BAUDRATE` | R/W | `0`=1200, `1`=2400, `2`=4800, `3`=9600, `4`=19200, `5`=38400, `6`=57600, `7`=115200. |
|
||||||
| **0x0002** | `UNIT` | R/W | `0`=Keine, `1`=cm, `2`=mm, `3`=MPa, `4`=Pa, `5`=kPa. |
|
| **0x0002** | `EINHEIT` | R/W | `0`=Keine, `1`=cm, `2`=mm, `3`=MPa, `4`=Pa, `5`=kPa. |
|
||||||
| **0x0003** | `DECIMAL_PLACES` | R/W | Anzahl der Dezimalstellen für den Messwert (0-3). |
|
| **0x0003** | `NACHKOMMASTELLEN` | R/W | Anzahl der Dezimalstellen für den Messwert (0-3). |
|
||||||
| **0x0004** | `CURRENT_MEASUREMENT` | R | Der skalierte Messwert als vorzeichenbehafteter 16-Bit-Integer. |
|
| **0x0004** | `MESSWERT_AKTUELL` | R | Der skalierte Messwert als vorzeichenbehafteter 16-Bit-Integer. |
|
||||||
| **0x0005** | `MEASUREMENT_RANGE_ZERO_POINT` | R/W | Rohwert für den Nullpunkt der Skala. |
|
| **0x0005** | `MESSBEREICH_NULLPUNKT` | R/W | Rohwert für den Nullpunkt der Skala. |
|
||||||
| **0x0006** | `MEASUREMENT_RANGE_END_POINT` | R/W | Rohwert für den Endpunkt der Skala. |
|
| **0x0006** | `MESSBEREICH_ENDPUNKT` | R/W | Rohwert für den Endpunkt der Skala. |
|
||||||
|
|
|
||||||
|
|
@ -9,13 +9,11 @@
|
||||||
| ✅ | **Phase 0: Planung & Definition** | | |
|
| ✅ | **Phase 0: Planung & Definition** | | |
|
||||||
| ✅ | Konzept erstellen und finalisieren | 30.06.2025 | Architektur, Komponenten und grundlegende Architektur sind festgelegt. |
|
| ✅ | Konzept erstellen und finalisieren | 30.06.2025 | Architektur, Komponenten und grundlegende Architektur sind festgelegt. |
|
||||||
| ✅ | MODBUS Register Map definieren | 30.06.2025 | Die "API" der Slaves ist definiert und bildet die Grundlage für die Software-Entwicklung. |
|
| ✅ | MODBUS Register Map definieren | 30.06.2025 | Die "API" der Slaves ist definiert und bildet die Grundlage für die Software-Entwicklung. |
|
||||||
| ✅ | Header- und deutsche Dokumentation aktualisiert | 10.07.2025 | Doxygen-Kommentare in Headern und deutsche .md-Dateien auf den neuesten Stand gebracht und übersetzt. |
|
|
||||||
| ☐ | **Phase 1: Slave-Node Prototyp (STM32 Eval-Board)** | | **Ziel:** Ein einzelner Slave wird auf dem Eval-Board zum Leben erweckt. |
|
| ☐ | **Phase 1: Slave-Node Prototyp (STM32 Eval-Board)** | | **Ziel:** Ein einzelner Slave wird auf dem Eval-Board zum Leben erweckt. |
|
||||||
| ✅ | 1.1 Entwicklungsumgebung für STM32/Zephyr einrichten | 30.06.2025 | Toolchain, VS Code, Zephyr-SDK, MCUBoot etc. installieren und ein "Hello World" zum Laufen bringen. |
|
| ✅ | 1.1 Entwicklungsumgebung für STM32/Zephyr einrichten | 30.06.2025 | Toolchain, VS Code, Zephyr-SDK, MCUBoot etc. installieren und ein "Hello World" zum Laufen bringen. |
|
||||||
| ✅ | 1.2 Hardware-Abstraktion (VND7050AJ, RS485) | 10.07.2025 | Implementierung der Treiber für den VND7050AJ und die RS485-Kommunikation. |
|
| ☐ | 1.2 Basis-Firmware für Slave-Node erstellen | | Hardware-Abstraktion (GPIOs, ADC, UART für RS485) implementieren. |
|
||||||
| ✅ | 1.3 Basis-Firmware für Slave-Node erstellen | 10.07.2025 | Hardware-Abstraktion (GPIOs) implementiert. |
|
| ☐ | 1.3 MODBUS-RTU Stack auf dem Slave implementieren | | Basierend auf der definierten Register-Map. Zuerst nur lesende Funktionen (Status, Version). |
|
||||||
| ✅ | 1.3 MODBUS-RTU Stack auf dem Slave implementieren | 10.07.2025 | Basierend auf der definierten Register-Map. Zuerst nur lesende Funktionen (Status, Version). |
|
| ☐ | 1.4 Kernlogik implementieren (z.B. Ventilsteuerung) | | Umsetzung der `VENTIL_ZUSTAND_BEWEGUNG` Logik, Strommessung für Endlagen etc. |
|
||||||
| ✅ | 1.4 Kernlogik implementieren (z.B. Ventilsteuerung) | 10.07.2025 | Umsetzung der `VALVE_STATE_MOVEMENT` Logik, Strommessung für Endlagen etc. |
|
|
||||||
| ☐ | **Phase 2: Verifikation der Slave-Firmware** | | **Ziel:** Nachweisen, dass der Slave sich exakt an die MODBUS-Spezifikation hält. |
|
| ☐ | **Phase 2: Verifikation der Slave-Firmware** | | **Ziel:** Nachweisen, dass der Slave sich exakt an die MODBUS-Spezifikation hält. |
|
||||||
| ☐ | 2.1 Slave-Node mit PC via USB-MODBUS-Adapter testen | | **Kritischer Meilenstein.** Mit Tools wie "QModMaster" oder einem Python-Skript die Register lesen & schreiben. Die Slave-Firmware wird so unabhängig vom Gateway validiert. |
|
| ☐ | 2.1 Slave-Node mit PC via USB-MODBUS-Adapter testen | | **Kritischer Meilenstein.** Mit Tools wie "QModMaster" oder einem Python-Skript die Register lesen & schreiben. Die Slave-Firmware wird so unabhängig vom Gateway validiert. |
|
||||||
| ☐ | 2.2 Firmware-Update Mechanismus testen | | Den kompletten Update-Prozess (Chunking, CRC-Check) mit einem Skript vom PC aus testen. Der Slave schreibt die Firmware dabei vorerst nur in einen ungenutzten RAM-Bereich. |
|
| ☐ | 2.2 Firmware-Update Mechanismus testen | | Den kompletten Update-Prozess (Chunking, CRC-Check) mit einem Skript vom PC aus testen. Der Slave schreibt die Firmware dabei vorerst nur in einen ungenutzten RAM-Bereich. |
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,6 @@
|
||||||
|
cmake_minimum_required(VERSION 3.20)
|
||||||
|
|
||||||
|
project(fwu)
|
||||||
|
|
||||||
|
target_sources(fwu PRIVATE src/fwu.c)
|
||||||
|
target_include_directories(fwu PUBLIC include)
|
||||||
|
|
@ -0,0 +1,6 @@
|
||||||
|
cmake_minimum_required(VERSION 3.20)
|
||||||
|
|
||||||
|
project(modbus_server)
|
||||||
|
|
||||||
|
target_sources(modbus_server PRIVATE src/modbus_server.c)
|
||||||
|
target_include_directories(modbus_server PUBLIC include)
|
||||||
|
|
@ -0,0 +1,6 @@
|
||||||
|
cmake_minimum_required(VERSION 3.20)
|
||||||
|
|
||||||
|
project(valve)
|
||||||
|
|
||||||
|
target_sources(valve PRIVATE src/valve.c)
|
||||||
|
target_include_directories(valve PUBLIC include)
|
||||||
|
|
@ -1,56 +0,0 @@
|
||||||
#!/bin/sh
|
|
||||||
|
|
||||||
# This script sets up a Git pre-commit hook to automatically format C/C++ files
|
|
||||||
# in the 'software/' subdirectory using clang-format.
|
|
||||||
|
|
||||||
# Define the path for the pre-commit hook
|
|
||||||
HOOK_DIR=".git/hooks"
|
|
||||||
HOOK_FILE="$HOOK_DIR/pre-commit"
|
|
||||||
|
|
||||||
# Create the hooks directory if it doesn't exist
|
|
||||||
mkdir -p "$HOOK_DIR"
|
|
||||||
|
|
||||||
# Create the pre-commit hook script using a 'here document'
|
|
||||||
cat > "$HOOK_FILE" << 'EOF'
|
|
||||||
#!/bin/sh
|
|
||||||
|
|
||||||
# --- Pre-commit hook for clang-format ---
|
|
||||||
#
|
|
||||||
# This hook formats staged C, C++, and Objective-C files in the 'software/'
|
|
||||||
# subdirectory before a commit is made.
|
|
||||||
# It automatically finds the .clang-format file in the software/ directory.
|
|
||||||
#
|
|
||||||
|
|
||||||
# Directory to be formatted
|
|
||||||
TARGET_DIR="software/"
|
|
||||||
|
|
||||||
# Use git diff to find staged files that are Added (A), Copied (C), or Modified (M).
|
|
||||||
# We filter for files only within the TARGET_DIR.
|
|
||||||
# The grep regex matches common C/C++ and Objective-C file extensions.
|
|
||||||
FILES_TO_FORMAT=$(git diff --cached --name-only --diff-filter=ACM "$TARGET_DIR" | grep -E '\.(c|h|cpp|hpp|cxx|hxx|cc|hh|m|mm)$')
|
|
||||||
|
|
||||||
if [ -z "$FILES_TO_FORMAT" ]; then
|
|
||||||
# No relevant files to format, exit successfully.
|
|
||||||
exit 0
|
|
||||||
fi
|
|
||||||
|
|
||||||
echo "› Running clang-format on staged files in '$TARGET_DIR'..."
|
|
||||||
|
|
||||||
# Run clang-format in-place on the identified files.
|
|
||||||
# clang-format will automatically find the .clang-format file in the software/ directory
|
|
||||||
# or any of its parent directories.
|
|
||||||
echo "$FILES_TO_FORMAT" | xargs clang-format -i
|
|
||||||
|
|
||||||
# Since clang-format may have changed the files, we need to re-stage them.
|
|
||||||
echo "$FILES_TO_FORMAT" | xargs git add
|
|
||||||
|
|
||||||
echo "› Formatting complete."
|
|
||||||
|
|
||||||
exit 0
|
|
||||||
EOF
|
|
||||||
|
|
||||||
# Make the hook executable
|
|
||||||
chmod +x "$HOOK_FILE"
|
|
||||||
|
|
||||||
echo "✅ Git pre-commit hook has been set up successfully."
|
|
||||||
echo " It will now automatically format files in the '$PWD/software' directory before each commit."
|
|
||||||
|
|
@ -1,142 +0,0 @@
|
||||||
# Zephyr Project .clang-format configuration
|
|
||||||
# Based on Linux kernel style with Zephyr-specific adaptations
|
|
||||||
|
|
||||||
# Use LLVM as the base style and customize from there
|
|
||||||
BasedOnStyle: LLVM
|
|
||||||
|
|
||||||
# Language settings
|
|
||||||
Language: Cpp
|
|
||||||
|
|
||||||
# Indentation settings
|
|
||||||
IndentWidth: 8
|
|
||||||
TabWidth: 8
|
|
||||||
UseTab: ForIndentation
|
|
||||||
|
|
||||||
# Line length
|
|
||||||
ColumnLimit: 100
|
|
||||||
|
|
||||||
# Brace settings
|
|
||||||
BreakBeforeBraces: Linux
|
|
||||||
BraceWrapping:
|
|
||||||
AfterClass: true
|
|
||||||
AfterControlStatement: false
|
|
||||||
AfterEnum: true
|
|
||||||
AfterFunction: true
|
|
||||||
AfterNamespace: true
|
|
||||||
AfterStruct: true
|
|
||||||
AfterUnion: true
|
|
||||||
BeforeCatch: true
|
|
||||||
BeforeElse: false
|
|
||||||
IndentBraces: false
|
|
||||||
SplitEmptyFunction: true
|
|
||||||
SplitEmptyRecord: true
|
|
||||||
SplitEmptyNamespace: true
|
|
||||||
|
|
||||||
# Always add braces for control statements (Zephyr requirement)
|
|
||||||
RemoveBracesLLVM: false
|
|
||||||
|
|
||||||
# Control statement settings
|
|
||||||
SpaceBeforeParens: ControlStatements
|
|
||||||
SpacesInParentheses: false
|
|
||||||
|
|
||||||
# Function settings
|
|
||||||
AllowShortFunctionsOnASingleLine: None
|
|
||||||
AllowShortBlocksOnASingleLine: Empty
|
|
||||||
AllowShortIfStatementsOnASingleLine: Never
|
|
||||||
AllowShortLoopsOnASingleLine: false
|
|
||||||
AllowShortCaseLabelsOnASingleLine: false
|
|
||||||
|
|
||||||
# Pointer and reference alignment
|
|
||||||
PointerAlignment: Right
|
|
||||||
ReferenceAlignment: Right
|
|
||||||
|
|
||||||
# Spacing settings
|
|
||||||
SpaceAfterCStyleCast: false
|
|
||||||
SpaceAfterLogicalNot: false
|
|
||||||
SpaceBeforeAssignmentOperators: true
|
|
||||||
SpaceBeforeCpp11BracedList: false
|
|
||||||
SpaceBeforeCtorInitializerColon: true
|
|
||||||
SpaceBeforeInheritanceColon: true
|
|
||||||
SpaceBeforeRangeBasedForLoopColon: true
|
|
||||||
SpaceInEmptyParentheses: false
|
|
||||||
SpacesBeforeTrailingComments: 1
|
|
||||||
SpacesInAngles: false
|
|
||||||
SpacesInCStyleCastParentheses: false
|
|
||||||
SpacesInContainerLiterals: false
|
|
||||||
SpacesInSquareBrackets: false
|
|
||||||
|
|
||||||
# Alignment settings
|
|
||||||
AlignAfterOpenBracket: DontAlign
|
|
||||||
AlignConsecutiveAssignments: false
|
|
||||||
AlignConsecutiveDeclarations: false
|
|
||||||
AlignEscapedNewlines: Right
|
|
||||||
AlignOperands: false
|
|
||||||
AlignTrailingComments: false
|
|
||||||
|
|
||||||
# Breaking settings
|
|
||||||
AlwaysBreakAfterDefinitionReturnType: None
|
|
||||||
AlwaysBreakAfterReturnType: None
|
|
||||||
AlwaysBreakBeforeMultilineStrings: false
|
|
||||||
AlwaysBreakTemplateDeclarations: false
|
|
||||||
BinPackArguments: false
|
|
||||||
BinPackParameters: false
|
|
||||||
BreakBeforeBinaryOperators: None
|
|
||||||
BreakBeforeTernaryOperators: true
|
|
||||||
BreakConstructorInitializersBeforeComma: false
|
|
||||||
BreakAfterJavaFieldAnnotations: false
|
|
||||||
BreakStringLiterals: true
|
|
||||||
|
|
||||||
# Penalties (used for line breaking decisions)
|
|
||||||
PenaltyBreakAssignment: 2
|
|
||||||
PenaltyBreakBeforeFirstCallParameter: 19
|
|
||||||
PenaltyBreakComment: 300
|
|
||||||
PenaltyBreakFirstLessLess: 120
|
|
||||||
PenaltyBreakString: 1000
|
|
||||||
PenaltyExcessCharacter: 1000000
|
|
||||||
PenaltyReturnTypeOnItsOwnLine: 60
|
|
||||||
|
|
||||||
# Comment settings
|
|
||||||
ReflowComments: true
|
|
||||||
CommentPragmas: '^ IWYU pragma:'
|
|
||||||
|
|
||||||
# Sorting settings
|
|
||||||
SortIncludes: true
|
|
||||||
SortUsingDeclarations: true
|
|
||||||
|
|
||||||
# Preprocessor settings
|
|
||||||
IndentPPDirectives: None
|
|
||||||
MacroBlockBegin: ''
|
|
||||||
MacroBlockEnd: ''
|
|
||||||
|
|
||||||
# Misc settings
|
|
||||||
CompactNamespaces: false
|
|
||||||
ConstructorInitializerAllOnOneLineOrOnePerLine: false
|
|
||||||
ConstructorInitializerIndentWidth: 4
|
|
||||||
ContinuationIndentWidth: 4
|
|
||||||
Cpp11BracedListStyle: true
|
|
||||||
DerivePointerAlignment: false
|
|
||||||
DisableFormat: false
|
|
||||||
ExperimentalAutoDetectBinPacking: false
|
|
||||||
FixNamespaceComments: true
|
|
||||||
ForEachMacros: ['LISTIFY', 'FOR_EACH', 'FOR_EACH_FIXED_ARG', 'FOR_EACH_IDX', 'FOR_EACH_IDX_FIXED_ARG', 'FOR_EACH_NONEMPTY_TERM', 'Z_FOR_EACH', 'Z_FOR_EACH_FIXED_ARG', 'Z_FOR_EACH_IDX', 'Z_FOR_EACH_IDX_FIXED_ARG']
|
|
||||||
IncludeBlocks: Preserve
|
|
||||||
IncludeCategories:
|
|
||||||
- Regex: '^<zephyr/.*\.h>'
|
|
||||||
Priority: 1
|
|
||||||
- Regex: '^<.*\.h>'
|
|
||||||
Priority: 2
|
|
||||||
- Regex: '^<.*'
|
|
||||||
Priority: 3
|
|
||||||
- Regex: '.*'
|
|
||||||
Priority: 4
|
|
||||||
IndentCaseLabels: false
|
|
||||||
IndentWrappedFunctionNames: false
|
|
||||||
JavaScriptQuotes: Leave
|
|
||||||
JavaScriptWrapImports: true
|
|
||||||
KeepEmptyLinesAtTheStartOfBlocks: false
|
|
||||||
MaxEmptyLinesToKeep: 1
|
|
||||||
NamespaceIndentation: None
|
|
||||||
ObjCBinPackProtocolList: Auto
|
|
||||||
ObjCBlockIndentWidth: 2
|
|
||||||
ObjCSpaceAfterProperty: false
|
|
||||||
ObjCSpaceBeforeProtocolList: true
|
|
||||||
|
|
@ -1,15 +1,12 @@
|
||||||
{
|
{
|
||||||
// Hush CMake
|
// Hush CMake
|
||||||
"cmake.configureOnOpen": false,
|
"cmake.configureOnOpen": false,
|
||||||
|
|
||||||
// IntelliSense
|
// IntelliSense
|
||||||
"C_Cpp.default.compilerPath": "${userHome}/zephyr-sdk-0.17.1/arm-zephyr-eabi/bin/arm-zephyr-eabi-gcc.exe",
|
"C_Cpp.default.compilerPath": "${userHome}/zephyr-sdk-0.17.1/arm-zephyr-eabi/bin/arm-zephyr-eabi-gcc.exe",
|
||||||
"C_Cpp.default.compileCommands": "${workspaceFolder}/build/compile_commands.json",
|
"C_Cpp.default.compileCommands": "${workspaceFolder}/build/compile_commands.json",
|
||||||
|
|
||||||
// File Associations
|
// File Associations
|
||||||
"files.associations": {
|
"files.associations": {
|
||||||
"app_version.h": "c"
|
}
|
||||||
},
|
|
||||||
"C_Cpp.clang_format_style": "file",
|
|
||||||
"nrf-connect.applications": [
|
|
||||||
"${workspaceFolder}/apps/slave_node"
|
|
||||||
],
|
|
||||||
}
|
}
|
||||||
|
|
@ -2,19 +2,31 @@
|
||||||
"version": "2.0.0",
|
"version": "2.0.0",
|
||||||
"tasks": [
|
"tasks": [
|
||||||
{
|
{
|
||||||
"label": "Format All C/C++ Files",
|
"label": "West Build",
|
||||||
"type": "shell",
|
"type": "shell",
|
||||||
"command": "find . -name \"*.c\" -o -name \"*.h\" | xargs clang-format -i",
|
|
||||||
"problemMatcher": [],
|
|
||||||
"group": {
|
"group": {
|
||||||
"kind": "build",
|
"kind": "build",
|
||||||
"isDefault": true
|
"isDefault": true
|
||||||
},
|
},
|
||||||
"presentation": {
|
"linux": {
|
||||||
"reveal": "silent",
|
"command": "${userHome}/zephyrproject/.venv/bin/west"
|
||||||
"clear": true,
|
},
|
||||||
"panel": "shared"
|
"windows": {
|
||||||
}
|
"command": "${userHome}/zephyrproject/.venv/Scripts/west.exe"
|
||||||
|
},
|
||||||
|
"osx": {
|
||||||
|
"command": "${userHome}/zephyrproject/.venv/bin/west"
|
||||||
|
},
|
||||||
|
"args": [
|
||||||
|
"build",
|
||||||
|
"-p",
|
||||||
|
"auto",
|
||||||
|
"-b",
|
||||||
|
"valve_node"
|
||||||
|
],
|
||||||
|
"problemMatcher": [
|
||||||
|
"$gcc"
|
||||||
|
]
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"label": "West Configurable Build",
|
"label": "West Configurable Build",
|
||||||
|
|
|
||||||
|
|
@ -1,7 +0,0 @@
|
||||||
cmake_minimum_required(VERSION 3.20.0)
|
|
||||||
|
|
||||||
find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE})
|
|
||||||
project(bl_test)
|
|
||||||
|
|
||||||
# Add application source files
|
|
||||||
target_sources(app PRIVATE src/main.c)
|
|
||||||
|
|
@ -1,5 +0,0 @@
|
||||||
VERSION_MAJOR = 0
|
|
||||||
VERSION_MINOR = 0
|
|
||||||
PATCHLEVEL = 1
|
|
||||||
VERSION_TWEAK = 1
|
|
||||||
EXTRAVERSION = devel
|
|
||||||
|
|
@ -1,46 +0,0 @@
|
||||||
# Enable Console and printk for logging via UART
|
|
||||||
CONFIG_CONSOLE=y
|
|
||||||
CONFIG_LOG=y
|
|
||||||
CONFIG_UART_CONSOLE=y
|
|
||||||
|
|
||||||
# Enable more detailed MCUMGR logging
|
|
||||||
CONFIG_MCUMGR_LOG_LEVEL_DBG=y
|
|
||||||
CONFIG_IMG_MANAGER_LOG_LEVEL_DBG=y
|
|
||||||
CONFIG_STREAM_FLASH_LOG_LEVEL_DBG=y
|
|
||||||
|
|
||||||
# Enable USB for MCUMGR only
|
|
||||||
CONFIG_USB_DEVICE_STACK=y
|
|
||||||
CONFIG_USB_CDC_ACM=y
|
|
||||||
CONFIG_USB_DEVICE_INITIALIZE_AT_BOOT=y
|
|
||||||
|
|
||||||
# USB CDC ACM buffer configuration for better MCUMGR performance
|
|
||||||
CONFIG_USB_CDC_ACM_RINGBUF_SIZE=1024
|
|
||||||
|
|
||||||
# Set log level to info for reasonable size
|
|
||||||
CONFIG_LOG_DEFAULT_LEVEL=3
|
|
||||||
|
|
||||||
# Enable MCUMGR info logging (not debug to save space)
|
|
||||||
CONFIG_MCUMGR_LOG_LEVEL_INF=y
|
|
||||||
|
|
||||||
# Enable USB CDC info logging
|
|
||||||
CONFIG_USB_CDC_ACM_LOG_LEVEL_INF=y
|
|
||||||
|
|
||||||
# STEP 5.2 - Enable mcumgr DFU in application
|
|
||||||
# Enable MCUMGR
|
|
||||||
CONFIG_MCUMGR=y # Enable MCUMGR management for both OS and Images
|
|
||||||
CONFIG_MCUMGR_GRP_OS=y
|
|
||||||
CONFIG_MCUMGR_GRP_IMG=y
|
|
||||||
|
|
||||||
# Configure MCUMGR transport to UART (will use USB-CDC via chosen device)
|
|
||||||
CONFIG_MCUMGR_TRANSPORT_UART=y
|
|
||||||
|
|
||||||
# Dependencies
|
|
||||||
# Configure dependencies for CONFIG_MCUMGR
|
|
||||||
CONFIG_NET_BUF=y
|
|
||||||
CONFIG_ZCBOR=y
|
|
||||||
CONFIG_CRC=y # Configure dependencies for CONFIG_MCUMGR_GRP_IMG
|
|
||||||
CONFIG_FLASH=y
|
|
||||||
CONFIG_IMG_MANAGER=y # Configure dependencies for CONFIG_IMG_MANAGER
|
|
||||||
CONFIG_STREAM_FLASH=y
|
|
||||||
CONFIG_FLASH_MAP=y # Configure dependencies for CONFIG_MCUMGR_TRANSPORT_USB_CDC
|
|
||||||
CONFIG_BASE64=y
|
|
||||||
|
|
@ -1,11 +0,0 @@
|
||||||
#include <zephyr/kernel.h>
|
|
||||||
#include <zephyr/logging/log.h>
|
|
||||||
#include <app_version.h>
|
|
||||||
|
|
||||||
LOG_MODULE_REGISTER(bl_test_app, LOG_LEVEL_INF);
|
|
||||||
|
|
||||||
int main(void)
|
|
||||||
{
|
|
||||||
LOG_INF("Hello World from bl_test! This is version %s", APP_VERSION_EXTENDED_STRING);
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
@ -1 +0,0 @@
|
||||||
SB_CONFIG_BOOTLOADER_MCUBOOT=y
|
|
||||||
|
|
@ -1,9 +0,0 @@
|
||||||
#include "common.dtsi"
|
|
||||||
|
|
||||||
/* Application Configuration - Firmware wird in slot0_partition geschrieben */
|
|
||||||
/ {
|
|
||||||
chosen {
|
|
||||||
zephyr,code-partition = &slot0_partition;
|
|
||||||
zephyr,uart-mcumgr = &cdc_acm_uart0;
|
|
||||||
};
|
|
||||||
};
|
|
||||||
|
|
@ -1,94 +0,0 @@
|
||||||
/*
|
|
||||||
* Common Devicetree Configuration für weact_stm32g431_core
|
|
||||||
* - Konfiguriert einen W25Q128 Flash-Speicher auf SPI2
|
|
||||||
* - Konfiguriert USB-CDC für MCUMGR
|
|
||||||
* - Setzt den Chip Select (CS) Pin auf PA5
|
|
||||||
* - Weist das Label "flash1" zu
|
|
||||||
*/
|
|
||||||
|
|
||||||
/* Partitions für internes Flash (STM32G431) */
|
|
||||||
&flash0 {
|
|
||||||
/delete-node/ partitions; /* Entferne die Standard-Partitionen */
|
|
||||||
partitions {
|
|
||||||
compatible = "fixed-partitions";
|
|
||||||
#address-cells = <1>;
|
|
||||||
#size-cells = <1>;
|
|
||||||
|
|
||||||
/* MCUboot bootloader - 48 KB */
|
|
||||||
boot_partition: partition@0 {
|
|
||||||
label = "mcuboot";
|
|
||||||
reg = <0x00000000 0x0000C000>;
|
|
||||||
};
|
|
||||||
|
|
||||||
/* Slot0 partition für primäres Application Image - 80 KB (20 sectors @ 4KB) */
|
|
||||||
slot0_partition: partition@C000 {
|
|
||||||
label = "image-0";
|
|
||||||
reg = <0x0000C000 0x00014000>;
|
|
||||||
};
|
|
||||||
};
|
|
||||||
};
|
|
||||||
|
|
||||||
/* USB-CDC Konfiguration für MCUMGR */
|
|
||||||
&usb {
|
|
||||||
status = "okay";
|
|
||||||
cdc_acm_uart0: cdc_acm_uart0 {
|
|
||||||
compatible = "zephyr,cdc-acm-uart";
|
|
||||||
};
|
|
||||||
};
|
|
||||||
|
|
||||||
/ {
|
|
||||||
chosen {
|
|
||||||
zephyr,uart-mcumgr = &cdc_acm_uart0;
|
|
||||||
};
|
|
||||||
};
|
|
||||||
|
|
||||||
&spi2 {
|
|
||||||
/* Definiere die Pins für SCK, MISO, MOSI auf Port B */
|
|
||||||
pinctrl-0 = <&spi2_sck_pb13 &spi2_miso_pb14 &spi2_mosi_pb15>;
|
|
||||||
pinctrl-names = "default";
|
|
||||||
status = "okay";
|
|
||||||
|
|
||||||
/* === Chip Select (CS) auf PA5 gesetzt === */
|
|
||||||
cs-gpios = <&gpioa 5 (GPIO_ACTIVE_LOW | GPIO_PULL_UP)>;
|
|
||||||
|
|
||||||
/* Definiere den Flash-Chip als SPI NOR Gerät */
|
|
||||||
flash1: flash@0 {
|
|
||||||
compatible = "jedec,spi-nor";
|
|
||||||
reg = <0>;
|
|
||||||
label = "flash1";
|
|
||||||
|
|
||||||
/* JEDEC ID für einen Winbond W25Q128 (16 MBytes) */
|
|
||||||
jedec-id = [ef 40 18];
|
|
||||||
|
|
||||||
/* Speichergröße in Bytes (16 MBytes) */
|
|
||||||
size = <DT_SIZE_M(16)>;
|
|
||||||
|
|
||||||
/* Maximale Taktfrequenz - angepasst an STM32G431 Limits */
|
|
||||||
spi-max-frequency = <1000000>;
|
|
||||||
|
|
||||||
/* Partitions für externes Flash */
|
|
||||||
partitions {
|
|
||||||
compatible = "fixed-partitions";
|
|
||||||
#address-cells = <1>;
|
|
||||||
#size-cells = <1>;
|
|
||||||
|
|
||||||
/* Slot1 partition für MCUboot (sekundäres Image) - 80 KB (20 sectors @ 4KB) */
|
|
||||||
slot1_partition: partition@0 {
|
|
||||||
label = "image-1";
|
|
||||||
reg = <0x00000000 0x00014000>;
|
|
||||||
};
|
|
||||||
|
|
||||||
/* Scratch partition für MCUboot - 80 KB (20 sectors @ 4KB) */
|
|
||||||
scratch_partition: partition@14000 {
|
|
||||||
label = "scratch";
|
|
||||||
reg = <0x00014000 0x00014000>;
|
|
||||||
};
|
|
||||||
|
|
||||||
/* Speicher partition für LittleFS - ~15.83 MB */
|
|
||||||
storage_partition: partition@28000 {
|
|
||||||
label = "storage";
|
|
||||||
reg = <0x00028000 0x00FD8000>;
|
|
||||||
};
|
|
||||||
};
|
|
||||||
};
|
|
||||||
};
|
|
||||||
|
|
@ -1,23 +0,0 @@
|
||||||
CONFIG_LOG=y
|
|
||||||
CONFIG_MCUBOOT_LOG_LEVEL_INF=y
|
|
||||||
|
|
||||||
# Enable UART console for MCUboot debug output
|
|
||||||
CONFIG_UART_CONSOLE=y
|
|
||||||
CONFIG_CONSOLE=y
|
|
||||||
CONFIG_MCUBOOT_INDICATION_LED=y
|
|
||||||
|
|
||||||
# Enable external SPI flash support
|
|
||||||
CONFIG_SPI=y
|
|
||||||
CONFIG_SPI_NOR=y
|
|
||||||
CONFIG_SPI_NOR_SFDP_DEVICETREE=n
|
|
||||||
CONFIG_FLASH=y
|
|
||||||
CONFIG_FLASH_MAP=y
|
|
||||||
CONFIG_GPIO=y
|
|
||||||
|
|
||||||
# Add SPI NOR specific configurations - use 4KB page size (required by driver)
|
|
||||||
CONFIG_SPI_NOR_FLASH_LAYOUT_PAGE_SIZE=4096
|
|
||||||
CONFIG_SPI_NOR_INIT_PRIORITY=80
|
|
||||||
|
|
||||||
# Set maximum image sectors manually since auto doesn't work with external flash
|
|
||||||
CONFIG_BOOT_MAX_IMG_SECTORS_AUTO=n
|
|
||||||
CONFIG_BOOT_MAX_IMG_SECTORS=80
|
|
||||||
|
|
@ -1,8 +0,0 @@
|
||||||
#include "common.dtsi"
|
|
||||||
|
|
||||||
/* MCUboot Configuration - Bootloader wird in boot_partition geschrieben */
|
|
||||||
/ {
|
|
||||||
chosen {
|
|
||||||
zephyr,code-partition = &boot_partition;
|
|
||||||
};
|
|
||||||
};
|
|
||||||
|
|
@ -1,10 +0,0 @@
|
||||||
cmake_minimum_required(VERSION 3.20)
|
|
||||||
|
|
||||||
find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE})
|
|
||||||
|
|
||||||
project(can_node LANGUAGES C)
|
|
||||||
|
|
||||||
zephyr_include_directories(../../include)
|
|
||||||
add_subdirectory(../../lib lib)
|
|
||||||
|
|
||||||
target_sources(app PRIVATE src/main.c)
|
|
||||||
|
|
@ -1,2 +0,0 @@
|
||||||
rsource "../../lib/Kconfig"
|
|
||||||
source "Kconfig.zephyr"
|
|
||||||
|
|
@ -1,5 +0,0 @@
|
||||||
VERSION_MAJOR = 0
|
|
||||||
VERSION_MINOR = 0
|
|
||||||
PATCHLEVEL = 1
|
|
||||||
VERSION_TWEAK = 1
|
|
||||||
EXTRAVERSION = devel
|
|
||||||
|
|
@ -1,7 +0,0 @@
|
||||||
# Disable UART console
|
|
||||||
CONFIG_UART_CONSOLE=n
|
|
||||||
|
|
||||||
# Enable RTT console
|
|
||||||
CONFIG_RTT_CONSOLE=y
|
|
||||||
CONFIG_USE_SEGGER_RTT=y
|
|
||||||
CONFIG_SHELL_BACKEND_RTT=y
|
|
||||||
|
|
@ -1,43 +0,0 @@
|
||||||
/ {
|
|
||||||
chosen {
|
|
||||||
zephyr,console = &rtt;
|
|
||||||
zephyr,shell = &rtt;
|
|
||||||
zephyr,settings-partition = &storage_partition;
|
|
||||||
};
|
|
||||||
|
|
||||||
rtt: rtt {
|
|
||||||
compatible = "segger,rtt-uart";
|
|
||||||
#address-cells = <1>;
|
|
||||||
#size-cells = <0>;
|
|
||||||
label = "RTT";
|
|
||||||
status = "okay";
|
|
||||||
};
|
|
||||||
};
|
|
||||||
|
|
||||||
&flash0 {
|
|
||||||
partitions {
|
|
||||||
compatible = "fixed-partitions";
|
|
||||||
#address-cells = <1>;
|
|
||||||
#size-cells = <1>;
|
|
||||||
|
|
||||||
/* Application partition starts at the beginning of flash */
|
|
||||||
slot0_partition: partition@0 {
|
|
||||||
label = "image-0";
|
|
||||||
reg = <0x00000000 DT_SIZE_K(120)>;
|
|
||||||
};
|
|
||||||
|
|
||||||
/* Use the last 8K for settings */
|
|
||||||
storage_partition: partition@1E000 {
|
|
||||||
label = "storage";
|
|
||||||
reg = <0x0001E000 DT_SIZE_K(8)>;
|
|
||||||
};
|
|
||||||
};
|
|
||||||
};
|
|
||||||
|
|
||||||
&usart1 {
|
|
||||||
modbus0 {
|
|
||||||
compatible = "zephyr,modbus-serial";
|
|
||||||
status = "okay";
|
|
||||||
};
|
|
||||||
status = "okay";
|
|
||||||
};
|
|
||||||
|
|
@ -1,47 +0,0 @@
|
||||||
/ {
|
|
||||||
aliases {
|
|
||||||
vnd7050aj = &vnd7050aj;
|
|
||||||
};
|
|
||||||
|
|
||||||
vnd7050aj: vnd7050aj {
|
|
||||||
compatible = "st,vnd7050aj";
|
|
||||||
status = "okay";
|
|
||||||
|
|
||||||
input0-gpios = <&gpio0 1 GPIO_ACTIVE_HIGH>;
|
|
||||||
input1-gpios = <&gpio0 2 GPIO_ACTIVE_HIGH>;
|
|
||||||
select0-gpios = <&gpio0 3 GPIO_ACTIVE_HIGH>;
|
|
||||||
select1-gpios = <&gpio0 4 GPIO_ACTIVE_HIGH>;
|
|
||||||
sense-enable-gpios = <&gpio0 5 GPIO_ACTIVE_HIGH>;
|
|
||||||
fault-reset-gpios = <&gpio0 6 GPIO_ACTIVE_LOW>;
|
|
||||||
io-channels = <&adc0 0>;
|
|
||||||
r-sense-ohms = <1500>;
|
|
||||||
k-vcc = <4000>;
|
|
||||||
};
|
|
||||||
|
|
||||||
modbus_uart: uart_2 {
|
|
||||||
compatible = "zephyr,native-pty-uart";
|
|
||||||
status = "okay";
|
|
||||||
current-speed = <19200>;
|
|
||||||
|
|
||||||
modbus0: modbus0 {
|
|
||||||
compatible = "zephyr,modbus-serial";
|
|
||||||
status = "okay";
|
|
||||||
};
|
|
||||||
};
|
|
||||||
|
|
||||||
};
|
|
||||||
|
|
||||||
&adc0 {
|
|
||||||
#address-cells = <1>;
|
|
||||||
#size-cells = <0>;
|
|
||||||
ref-internal-mv = <3300>;
|
|
||||||
ref-external1-mv = <5000>;
|
|
||||||
|
|
||||||
channel@0 {
|
|
||||||
reg = <0>;
|
|
||||||
zephyr,gain = "ADC_GAIN_1";
|
|
||||||
zephyr,reference = "ADC_REF_INTERNAL";
|
|
||||||
zephyr,acquisition-time = <ADC_ACQ_TIME_DEFAULT>;
|
|
||||||
zephyr,resolution = <12>;
|
|
||||||
};
|
|
||||||
};
|
|
||||||
|
|
@ -1,48 +0,0 @@
|
||||||
/ {
|
|
||||||
aliases {
|
|
||||||
vnd7050aj = &vnd7050aj;
|
|
||||||
};
|
|
||||||
|
|
||||||
vnd7050aj: vnd7050aj {
|
|
||||||
compatible = "st,vnd7050aj";
|
|
||||||
status = "okay";
|
|
||||||
|
|
||||||
input0-gpios = <&gpiob 3 GPIO_ACTIVE_HIGH>;
|
|
||||||
input1-gpios = <&gpiob 4 GPIO_ACTIVE_HIGH>;
|
|
||||||
select0-gpios = <&gpiob 0 GPIO_ACTIVE_HIGH>;
|
|
||||||
select1-gpios = <&gpiob 1 GPIO_ACTIVE_HIGH>;
|
|
||||||
sense-enable-gpios = <&gpiob 6 GPIO_ACTIVE_HIGH>;
|
|
||||||
fault-reset-gpios = <&gpiob 5 GPIO_ACTIVE_LOW>;
|
|
||||||
io-channels = <&adc1 1>;
|
|
||||||
r-sense-ohms = <1500>;
|
|
||||||
k-vcc = <4000>;
|
|
||||||
};
|
|
||||||
};
|
|
||||||
|
|
||||||
&adc1 {
|
|
||||||
status = "okay";
|
|
||||||
pinctrl-0 = <&adc1_in1_pa0>;
|
|
||||||
pinctrl-names = "default";
|
|
||||||
st,adc-clock-source = "SYNC";
|
|
||||||
st,adc-prescaler = <4>;
|
|
||||||
#address-cells = <1>;
|
|
||||||
#size-cells = <0>;
|
|
||||||
|
|
||||||
channel@1 {
|
|
||||||
reg = <1>;
|
|
||||||
zephyr,gain = "ADC_GAIN_1";
|
|
||||||
zephyr,reference = "ADC_REF_INTERNAL";
|
|
||||||
zephyr,acquisition-time = <ADC_ACQ_TIME_DEFAULT>;
|
|
||||||
zephyr,resolution = <12>;
|
|
||||||
};
|
|
||||||
};
|
|
||||||
|
|
||||||
&usart1 {
|
|
||||||
modbus0 {
|
|
||||||
compatible = "zephyr,modbus-serial";
|
|
||||||
status = "okay";
|
|
||||||
};
|
|
||||||
status = "okay";
|
|
||||||
pinctrl-0 = <&usart1_tx_pa9 &usart1_rx_pa10>; // PA9=TX, PA10=RX for Modbus communication
|
|
||||||
pinctrl-names = "default";
|
|
||||||
};
|
|
||||||
|
|
@ -1,16 +0,0 @@
|
||||||
#include <zephyr/dt-bindings/gpio/gpio.h>
|
|
||||||
|
|
||||||
&zephyr_udc0 {
|
|
||||||
cdc_acm_uart0: cdc_acm_uart0 {
|
|
||||||
compatible = "zephyr,cdc-acm-uart";
|
|
||||||
|
|
||||||
modbus0 {
|
|
||||||
compatible = "zephyr,modbus-serial";
|
|
||||||
status = "okay";
|
|
||||||
};
|
|
||||||
};
|
|
||||||
};
|
|
||||||
|
|
||||||
&usart1 {
|
|
||||||
/delete-node/ modbus0;
|
|
||||||
};
|
|
||||||
|
|
@ -1,88 +0,0 @@
|
||||||
# Copyright (c) 2024, Eduard Iten
|
|
||||||
# SPDX-License-Identifier: Apache-2.0
|
|
||||||
|
|
||||||
description: |
|
|
||||||
STMicroelectronics VND7050AJ dual-channel high-side driver.
|
|
||||||
This is a GPIO and ADC controlled device.
|
|
||||||
|
|
||||||
compatible: "st,vnd7050aj"
|
|
||||||
|
|
||||||
include: base.yaml
|
|
||||||
|
|
||||||
properties:
|
|
||||||
input0-gpios:
|
|
||||||
type: phandle-array
|
|
||||||
required: true
|
|
||||||
description: GPIO to control output channel 0.
|
|
||||||
|
|
||||||
input1-gpios:
|
|
||||||
type: phandle-array
|
|
||||||
required: true
|
|
||||||
description: GPIO to control output channel 1.
|
|
||||||
|
|
||||||
select0-gpios:
|
|
||||||
type: phandle-array
|
|
||||||
required: true
|
|
||||||
description: GPIO for MultiSense selection bit 0.
|
|
||||||
|
|
||||||
select1-gpios:
|
|
||||||
type: phandle-array
|
|
||||||
required: true
|
|
||||||
description: GPIO for MultiSense selection bit 1.
|
|
||||||
|
|
||||||
sense-enable-gpios:
|
|
||||||
type: phandle-array
|
|
||||||
required: true
|
|
||||||
description: GPIO to enable the MultiSense output.
|
|
||||||
|
|
||||||
fault-reset-gpios:
|
|
||||||
type: phandle-array
|
|
||||||
required: true
|
|
||||||
description: GPIO to reset a latched fault (active-low).
|
|
||||||
|
|
||||||
io-channels:
|
|
||||||
type: phandle-array
|
|
||||||
required: true
|
|
||||||
description: |
|
|
||||||
ADC channel connected to the MultiSense pin. This should be an
|
|
||||||
io-channels property pointing to the ADC controller and channel number.
|
|
||||||
|
|
||||||
r-sense-ohms:
|
|
||||||
type: int
|
|
||||||
required: true
|
|
||||||
description: |
|
|
||||||
Value of the external sense resistor connected from the MultiSense
|
|
||||||
pin to GND, specified in Ohms. This is critical for correct
|
|
||||||
conversion of the analog readings.
|
|
||||||
|
|
||||||
k-factor:
|
|
||||||
type: int
|
|
||||||
default: 1500
|
|
||||||
description: |
|
|
||||||
Factor between PowerMOS and SenseMOS.
|
|
||||||
|
|
||||||
k-vcc:
|
|
||||||
type: int
|
|
||||||
default: 8000
|
|
||||||
description: |
|
|
||||||
VCC sense ratio multiplied by 1000. Used for supply voltage calculation.
|
|
||||||
|
|
||||||
t-sense-0:
|
|
||||||
type: int
|
|
||||||
default: 25
|
|
||||||
description: |
|
|
||||||
Temperature sense reference temperature in degrees Celsius.
|
|
||||||
|
|
||||||
v-sense-0:
|
|
||||||
type: int
|
|
||||||
default: 2070
|
|
||||||
description: |
|
|
||||||
Temperature sense reference voltage in millivolts.
|
|
||||||
|
|
||||||
k-tchip:
|
|
||||||
type: int
|
|
||||||
default: -5500
|
|
||||||
description: |
|
|
||||||
Temperature sense gain coefficient multiplied by 1000.
|
|
||||||
Used for chip temperature calculation.
|
|
||||||
|
|
||||||
|
|
@ -1,4 +0,0 @@
|
||||||
CONFIG_USB_DEVICE_STACK=y
|
|
||||||
CONFIG_USB_DEVICE_PRODUCT="Modbus slave node"
|
|
||||||
CONFIG_UART_LINE_CTRL=y
|
|
||||||
CONFIG_USB_DEVICE_INITIALIZE_AT_BOOT=n
|
|
||||||
|
|
@ -1,31 +0,0 @@
|
||||||
# Enable Console and printk for logging
|
|
||||||
CONFIG_CONSOLE=y
|
|
||||||
CONFIG_LOG=y
|
|
||||||
|
|
||||||
# Enable Shell
|
|
||||||
CONFIG_SHELL=y
|
|
||||||
CONFIG_REBOOT=y
|
|
||||||
CONFIG_SHELL_MODBUS=n
|
|
||||||
CONFIG_SHELL_VALVE=y
|
|
||||||
CONFIG_SHELL_SYSTEM=y
|
|
||||||
|
|
||||||
# Enable Settings Subsystem
|
|
||||||
CONFIG_SETTINGS=y
|
|
||||||
CONFIG_SETTINGS_NVS=y
|
|
||||||
CONFIG_NVS=y
|
|
||||||
CONFIG_FLASH=y
|
|
||||||
CONFIG_FLASH_MAP=y
|
|
||||||
CONFIG_FLASH_PAGE_LAYOUT=y
|
|
||||||
|
|
||||||
# Config modbus
|
|
||||||
CONFIG_UART_INTERRUPT_DRIVEN=y
|
|
||||||
CONFIG_MODBUS=y
|
|
||||||
CONFIG_MODBUS_ROLE_SERVER=y
|
|
||||||
CONFIG_MODBUS_LOG_LEVEL_DBG=y
|
|
||||||
|
|
||||||
# enable Valve Driver
|
|
||||||
CONFIG_LIB_VALVE=y
|
|
||||||
CONFIG_LOG_VALVE_LEVEL=4
|
|
||||||
|
|
||||||
# Enable VND7050AJ
|
|
||||||
CONFIG_VND7050AJ=y
|
|
||||||
|
|
@ -1,40 +0,0 @@
|
||||||
#include <zephyr/kernel.h>
|
|
||||||
#include <zephyr/logging/log.h>
|
|
||||||
#include <zephyr/settings/settings.h>
|
|
||||||
#include <app_version.h>
|
|
||||||
#include <lib/valve.h>
|
|
||||||
#include <lib/vnd7050aj.h>
|
|
||||||
|
|
||||||
LOG_MODULE_REGISTER(main, LOG_LEVEL_INF);
|
|
||||||
|
|
||||||
int main(void)
|
|
||||||
{
|
|
||||||
int rc;
|
|
||||||
LOG_INF("Starting Irrigation System CAN Node, Version %s", APP_VERSION_EXTENDED_STRING);
|
|
||||||
|
|
||||||
/* Initialize settings subsystem */
|
|
||||||
rc = settings_subsys_init();
|
|
||||||
if (rc != 0) {
|
|
||||||
LOG_ERR("Failed to initialize settings subsystem (%d)", rc);
|
|
||||||
return rc;
|
|
||||||
}
|
|
||||||
LOG_INF("Settings subsystem initialized");
|
|
||||||
|
|
||||||
/* Load settings from storage */
|
|
||||||
rc = settings_load();
|
|
||||||
if (rc == 0) {
|
|
||||||
LOG_INF("Settings loaded successfully");
|
|
||||||
} else {
|
|
||||||
LOG_WRN("Failed to load settings (%d)", rc);
|
|
||||||
}
|
|
||||||
|
|
||||||
/* Initialize valve system */
|
|
||||||
rc = valve_init();
|
|
||||||
if (rc != 0) {
|
|
||||||
LOG_ERR("Failed to initialize valve system (%d)", rc);
|
|
||||||
return rc;
|
|
||||||
}
|
|
||||||
LOG_INF("Valve system initialized");
|
|
||||||
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
@ -1,5 +0,0 @@
|
||||||
SB_CONFIG_BOOTLOADER_MCUBOOT=y
|
|
||||||
SB_CONFIG_MCUBOOT_MODE_SINGLE_APP=y
|
|
||||||
|
|
||||||
CONFIG_LOG=y
|
|
||||||
CONFIG_MCUBOOT_LOG_LEVEL_INF=y
|
|
||||||
|
|
@ -1,8 +1,9 @@
|
||||||
cmake_minimum_required(VERSION 3.20.5)
|
cmake_minimum_required(VERSION 3.20)
|
||||||
|
|
||||||
|
# Include the main 'software' directory as a module to find boards, libs, etc.
|
||||||
|
list(APPEND ZEPHYR_EXTRA_MODULES ${CMAKE_CURRENT_SOURCE_DIR}/../..)
|
||||||
|
|
||||||
find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE})
|
find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE})
|
||||||
|
|
||||||
project(gateway)
|
project(gateway)
|
||||||
target_sources(app PRIVATE src/main.c)
|
|
||||||
|
|
||||||
target_include_directories(app PRIVATE include)
|
target_sources(app PRIVATE src/main.c)
|
||||||
|
|
|
||||||
|
|
@ -1,44 +0,0 @@
|
||||||
# README for the Hello World Zephyr Application
|
|
||||||
|
|
||||||
## Overview
|
|
||||||
|
|
||||||
This is a minimal Hello World application built using the Zephyr RTOS. The application demonstrates basic logging functionality by printing a message every 5 seconds, including the version number of the application.
|
|
||||||
|
|
||||||
## Project Structure
|
|
||||||
|
|
||||||
The project consists of the following files:
|
|
||||||
|
|
||||||
- `src/main.c`: The entry point of the application that initializes logging and sets up a timer.
|
|
||||||
- `include/app_version.h`: Header file that defines the application version.
|
|
||||||
- `VERSION`: A text file containing the version number of the application.
|
|
||||||
- `prj.conf`: Configuration file for the Zephyr project, specifying necessary options.
|
|
||||||
- `CMakeLists.txt`: Build configuration file for CMake.
|
|
||||||
- `README.md`: Documentation for the project.
|
|
||||||
|
|
||||||
## Building the Application
|
|
||||||
|
|
||||||
To build the application, follow these steps:
|
|
||||||
|
|
||||||
1. Ensure you have the Zephyr development environment set up.
|
|
||||||
2. Navigate to the `apps/gateway` directory.
|
|
||||||
3. Run the following command to build the application:
|
|
||||||
|
|
||||||
```
|
|
||||||
west build -b <your_board> .
|
|
||||||
```
|
|
||||||
|
|
||||||
Replace `<your_board>` with the appropriate board name.
|
|
||||||
|
|
||||||
## Running the Application
|
|
||||||
|
|
||||||
After building the application, you can flash it to your board using:
|
|
||||||
|
|
||||||
```
|
|
||||||
west flash
|
|
||||||
```
|
|
||||||
|
|
||||||
Once the application is running, you will see log messages printed every 5 seconds, including the version number.
|
|
||||||
|
|
||||||
## Version
|
|
||||||
|
|
||||||
The version of this application can be found in the `VERSION` file and is also included in the log messages.
|
|
||||||
|
|
@ -1,5 +0,0 @@
|
||||||
VERSION_MAJOR = 0
|
|
||||||
VERSION_MINOR = 0
|
|
||||||
PATCHLEVEL = 1
|
|
||||||
VERSION_TWEAK = 0
|
|
||||||
EXTRAVERSION = devel
|
|
||||||
|
|
@ -1,16 +0,0 @@
|
||||||
&flash0 {
|
|
||||||
reg = <0x0 0x400000>; /* 4MB flash */
|
|
||||||
};
|
|
||||||
|
|
||||||
#include "espressif/partitions_0x0_default_4M.dtsi"
|
|
||||||
|
|
||||||
/ {
|
|
||||||
chosen {
|
|
||||||
zephyr,shell-uart = &uart0;
|
|
||||||
zephyr,uart-mcumgr = &usb_serial;
|
|
||||||
};
|
|
||||||
};
|
|
||||||
|
|
||||||
&usb_serial {
|
|
||||||
status = "okay";
|
|
||||||
};
|
|
||||||
|
|
@ -1 +0,0 @@
|
||||||
#include "common_4MB.dtsi"
|
|
||||||
|
|
@ -1,47 +1,2 @@
|
||||||
# -------------------
|
# Gateway Configuration
|
||||||
# Logging and Console
|
CONFIG_NETWORKING=y
|
||||||
# -------------------
|
|
||||||
CONFIG_LOG=y
|
|
||||||
CONFIG_UART_CONSOLE=y
|
|
||||||
|
|
||||||
# -------------
|
|
||||||
# Zephyr Shell
|
|
||||||
# -------------
|
|
||||||
CONFIG_SHELL=y
|
|
||||||
CONFIG_KERNEL_SHELL=y
|
|
||||||
CONFIG_REBOOT=y
|
|
||||||
|
|
||||||
# -------------------
|
|
||||||
# MCUmgr OS Management
|
|
||||||
# -------------------
|
|
||||||
CONFIG_MCUMGR=y
|
|
||||||
CONFIG_MCUMGR_GRP_OS=y
|
|
||||||
CONFIG_MCUMGR_TRANSPORT_UART=y
|
|
||||||
|
|
||||||
# -------------------
|
|
||||||
# MCUmgr Filesystem Group
|
|
||||||
# -------------------
|
|
||||||
CONFIG_MCUMGR_GRP_FS=y
|
|
||||||
|
|
||||||
# -------------------
|
|
||||||
# LittleFS and Flash
|
|
||||||
# -------------------
|
|
||||||
CONFIG_FILE_SYSTEM=y
|
|
||||||
CONFIG_FILE_SYSTEM_LITTLEFS=y
|
|
||||||
CONFIG_FLASH=y
|
|
||||||
CONFIG_FLASH_MAP=y
|
|
||||||
|
|
||||||
# -------------------
|
|
||||||
# Settings Subsystem
|
|
||||||
# -------------------
|
|
||||||
CONFIG_SETTINGS=y
|
|
||||||
CONFIG_SETTINGS_FILE=y
|
|
||||||
CONFIG_SETTINGS_FILE_PATH="/lfs/settings.bin"
|
|
||||||
|
|
||||||
# -------------------
|
|
||||||
# Dependencies
|
|
||||||
# -------------------
|
|
||||||
CONFIG_NET_BUF=y
|
|
||||||
CONFIG_ZCBOR=y
|
|
||||||
CONFIG_CRC=y
|
|
||||||
CONFIG_BASE64=y
|
|
||||||
|
|
|
||||||
|
|
@ -1,136 +1,13 @@
|
||||||
#include <zephyr/fs/fs.h>
|
/*
|
||||||
#include <zephyr/fs/littlefs.h>
|
* Copyright (c) 2025 Eduard Iten
|
||||||
|
*
|
||||||
|
* SPDX-License-Identifier: Apache-2.0
|
||||||
|
*/
|
||||||
|
|
||||||
#include <zephyr/kernel.h>
|
#include <zephyr/kernel.h>
|
||||||
#include <zephyr/logging/log.h>
|
|
||||||
#include <zephyr/settings/settings.h>
|
|
||||||
#include <zephyr/shell/shell.h>
|
|
||||||
#include <app_version.h>
|
|
||||||
#include <string.h>
|
|
||||||
|
|
||||||
LOG_MODULE_REGISTER(hello_world);
|
|
||||||
|
|
||||||
/* LittleFS mount configuration for 'storage_partition' partition */
|
|
||||||
FS_LITTLEFS_DECLARE_DEFAULT_CONFIG(storage_partition);
|
|
||||||
static struct fs_mount_t littlefs_mnt = {
|
|
||||||
.type = FS_LITTLEFS,
|
|
||||||
.mnt_point = "/lfs",
|
|
||||||
.fs_data = &storage_partition, // default config macro
|
|
||||||
.storage_dev = (void *)FIXED_PARTITION_ID(storage_partition),
|
|
||||||
};
|
|
||||||
|
|
||||||
static char my_setting[32] = "default";
|
|
||||||
|
|
||||||
static int my_settings_set(const char *name, size_t len, settings_read_cb read_cb, void *cb_arg)
|
|
||||||
{
|
|
||||||
if (strcmp(name, "value") == 0) {
|
|
||||||
if (len > sizeof(my_setting) - 1) {
|
|
||||||
len = sizeof(my_setting) - 1;
|
|
||||||
}
|
|
||||||
if (read_cb(cb_arg, my_setting, len) == len) {
|
|
||||||
my_setting[len] = '\0';
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
return -EINVAL;
|
|
||||||
}
|
|
||||||
return -ENOENT;
|
|
||||||
}
|
|
||||||
|
|
||||||
static int my_settings_export(int (*export_func)(const char *, const void *, size_t))
|
|
||||||
{
|
|
||||||
return export_func("my/setting/value", my_setting, strlen(my_setting));
|
|
||||||
}
|
|
||||||
|
|
||||||
SETTINGS_STATIC_HANDLER_DEFINE(my, "my/setting", NULL, my_settings_set, NULL, my_settings_export);
|
|
||||||
|
|
||||||
static int cmd_my_get(const struct shell *shell, size_t argc, char **argv)
|
|
||||||
{
|
|
||||||
shell_print(shell, "my_setting = '%s'", my_setting);
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
static int cmd_my_reset(const struct shell *shell, size_t argc, char **argv)
|
|
||||||
{
|
|
||||||
strcpy(my_setting, "default");
|
|
||||||
settings_save();
|
|
||||||
shell_print(shell, "my_setting reset to default");
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Improved set command: join all arguments for whitespace support
|
|
||||||
static int cmd_my_set(const struct shell *shell, size_t argc, char **argv)
|
|
||||||
{
|
|
||||||
if (argc < 2) {
|
|
||||||
shell_error(shell, "Usage: my set <value>");
|
|
||||||
return -EINVAL;
|
|
||||||
}
|
|
||||||
// Join all argv[1..] with spaces
|
|
||||||
size_t i, pos = 0;
|
|
||||||
my_setting[0] = '\0';
|
|
||||||
for (i = 1; i < argc; ++i) {
|
|
||||||
size_t left = sizeof(my_setting) - 1 - pos;
|
|
||||||
if (left == 0)
|
|
||||||
break;
|
|
||||||
strncat(my_setting, argv[i], left);
|
|
||||||
pos = strlen(my_setting);
|
|
||||||
if (i < argc - 1 && left > 1) {
|
|
||||||
strncat(my_setting, " ", left - 1);
|
|
||||||
pos = strlen(my_setting);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
my_setting[sizeof(my_setting) - 1] = '\0';
|
|
||||||
settings_save();
|
|
||||||
shell_print(shell, "my_setting set to '%s'", my_setting);
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
SHELL_STATIC_SUBCMD_SET_CREATE(my_subcmds,
|
|
||||||
SHELL_CMD(get, NULL, "Get my_setting", cmd_my_get),
|
|
||||||
SHELL_CMD(set, NULL, "Set my_setting (supports spaces)", cmd_my_set),
|
|
||||||
SHELL_CMD(reset, NULL, "Reset my_setting to default and compact settings file", cmd_my_reset),
|
|
||||||
SHELL_SUBCMD_SET_END);
|
|
||||||
|
|
||||||
SHELL_CMD_REGISTER(my, &my_subcmds, "My settings commands", NULL);
|
|
||||||
|
|
||||||
static void compact_settings_file(void)
|
|
||||||
{
|
|
||||||
struct fs_file_t file;
|
|
||||||
fs_file_t_init(&file);
|
|
||||||
int rc = fs_open(&file, "/lfs/settings.bin", FS_O_WRITE | FS_O_CREATE | FS_O_TRUNC);
|
|
||||||
if (rc == 0) {
|
|
||||||
fs_close(&file);
|
|
||||||
LOG_INF("Settings file compacted (truncated and recreated)");
|
|
||||||
} else if (rc == -ENOENT) {
|
|
||||||
LOG_INF("Settings file did not exist, created new");
|
|
||||||
} else {
|
|
||||||
LOG_ERR("Failed to compact settings file (%d)", rc);
|
|
||||||
}
|
|
||||||
settings_save();
|
|
||||||
}
|
|
||||||
|
|
||||||
int main(void)
|
int main(void)
|
||||||
{
|
{
|
||||||
int rc = fs_mount(&littlefs_mnt);
|
printk("Hello from Gateway!\n");
|
||||||
if (rc < 0) {
|
|
||||||
LOG_ERR("Error mounting LittleFS [%d]", rc);
|
|
||||||
} else {
|
|
||||||
LOG_INF("LittleFS mounted at /lfs");
|
|
||||||
}
|
|
||||||
|
|
||||||
/* Initialize settings subsystem */
|
|
||||||
settings_subsys_init();
|
|
||||||
LOG_INF("Settings subsystem initialized");
|
|
||||||
|
|
||||||
/* Load settings from storage */
|
|
||||||
rc = settings_load();
|
|
||||||
if (rc == 0) {
|
|
||||||
LOG_INF("Settings loaded: my_setting='%s'", my_setting);
|
|
||||||
} else {
|
|
||||||
LOG_ERR("Failed to load settings (%d)", rc);
|
|
||||||
}
|
|
||||||
|
|
||||||
/* Compact settings file on each start */
|
|
||||||
compact_settings_file();
|
|
||||||
|
|
||||||
LOG_INF("Hello World! Version: %s", APP_VERSION_EXTENDED_STRING);
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
@ -1,2 +0,0 @@
|
||||||
SB_CONFIG_BOOTLOADER_MCUBOOT=y
|
|
||||||
SB_CONFIG_MCUBOOT_MODE_SINGLE_APP=y
|
|
||||||
|
|
@ -1,8 +0,0 @@
|
||||||
#include "../boards/common_4MB.dtsi"
|
|
||||||
|
|
||||||
/* Application Configuration - Firmware goes to slot0_partition (0x20000) */
|
|
||||||
/ {
|
|
||||||
chosen {
|
|
||||||
zephyr,code-partition = &slot0_partition;
|
|
||||||
};
|
|
||||||
};
|
|
||||||
|
|
@ -1,3 +0,0 @@
|
||||||
CONFIG_LOG=y
|
|
||||||
CONFIG_MCUBOOT_LOG_LEVEL_INF=y
|
|
||||||
CONFIG_UART_CONSOLE=n
|
|
||||||
|
|
@ -1,12 +0,0 @@
|
||||||
#include "../boards/common_4MB.dtsi"
|
|
||||||
|
|
||||||
/* MCUboot Configuration - Bootloader goes to boot_partition (0x0) */
|
|
||||||
/ {
|
|
||||||
chosen {
|
|
||||||
zephyr,code-partition = &boot_partition;
|
|
||||||
};
|
|
||||||
aliases {
|
|
||||||
mcuboot-button0 = &user_button1;
|
|
||||||
};
|
|
||||||
|
|
||||||
};
|
|
||||||
|
|
@ -1 +0,0 @@
|
||||||
Subproject commit 6e669cfc4e400c3ef6e55c16401788ce0d804577
|
|
||||||
|
|
@ -1,12 +0,0 @@
|
||||||
{
|
|
||||||
"configurations": [
|
|
||||||
{
|
|
||||||
"name": "Linux",
|
|
||||||
"compileCommands": "${workspaceFolder}/build/compile_commands.json",
|
|
||||||
"cStandard": "c99",
|
|
||||||
"cppStandard": "gnu++17",
|
|
||||||
"intelliSenseMode": "linux-gcc-arm"
|
|
||||||
}
|
|
||||||
],
|
|
||||||
"version": 4
|
|
||||||
}
|
|
||||||
|
|
@ -3,8 +3,6 @@ cmake_minimum_required(VERSION 3.20)
|
||||||
find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE})
|
find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE})
|
||||||
|
|
||||||
project(slave_node LANGUAGES C)
|
project(slave_node LANGUAGES C)
|
||||||
|
|
||||||
zephyr_include_directories(../../include)
|
zephyr_include_directories(../../include)
|
||||||
add_subdirectory(../../lib lib)
|
add_subdirectory(../../lib lib)
|
||||||
|
|
||||||
target_sources(app PRIVATE src/main.c)
|
target_sources(app PRIVATE src/main.c)
|
||||||
|
|
@ -1,5 +0,0 @@
|
||||||
VERSION_MAJOR = 0
|
|
||||||
VERSION_MINOR = 0
|
|
||||||
PATCHLEVEL = 1
|
|
||||||
VERSION_TWEAK = 1
|
|
||||||
EXTRAVERSION = devel
|
|
||||||
|
|
@ -1,47 +0,0 @@
|
||||||
/ {
|
|
||||||
aliases {
|
|
||||||
vnd7050aj = &vnd7050aj;
|
|
||||||
};
|
|
||||||
|
|
||||||
vnd7050aj: vnd7050aj {
|
|
||||||
compatible = "st,vnd7050aj";
|
|
||||||
status = "okay";
|
|
||||||
|
|
||||||
input0-gpios = <&gpio0 1 GPIO_ACTIVE_HIGH>;
|
|
||||||
input1-gpios = <&gpio0 2 GPIO_ACTIVE_HIGH>;
|
|
||||||
select0-gpios = <&gpio0 3 GPIO_ACTIVE_HIGH>;
|
|
||||||
select1-gpios = <&gpio0 4 GPIO_ACTIVE_HIGH>;
|
|
||||||
sense-enable-gpios = <&gpio0 5 GPIO_ACTIVE_HIGH>;
|
|
||||||
fault-reset-gpios = <&gpio0 6 GPIO_ACTIVE_LOW>;
|
|
||||||
io-channels = <&adc0 0>;
|
|
||||||
r-sense-ohms = <1500>;
|
|
||||||
k-vcc = <4000>;
|
|
||||||
};
|
|
||||||
|
|
||||||
modbus_uart: uart_2 {
|
|
||||||
compatible = "zephyr,native-pty-uart";
|
|
||||||
status = "okay";
|
|
||||||
current-speed = <19200>;
|
|
||||||
|
|
||||||
modbus0: modbus0 {
|
|
||||||
compatible = "zephyr,modbus-serial";
|
|
||||||
status = "okay";
|
|
||||||
};
|
|
||||||
};
|
|
||||||
|
|
||||||
};
|
|
||||||
|
|
||||||
&adc0 {
|
|
||||||
#address-cells = <1>;
|
|
||||||
#size-cells = <0>;
|
|
||||||
ref-internal-mv = <3300>;
|
|
||||||
ref-external1-mv = <5000>;
|
|
||||||
|
|
||||||
channel@0 {
|
|
||||||
reg = <0>;
|
|
||||||
zephyr,gain = "ADC_GAIN_1";
|
|
||||||
zephyr,reference = "ADC_REF_INTERNAL";
|
|
||||||
zephyr,acquisition-time = <ADC_ACQ_TIME_DEFAULT>;
|
|
||||||
zephyr,resolution = <12>;
|
|
||||||
};
|
|
||||||
};
|
|
||||||
|
|
@ -1,48 +1,9 @@
|
||||||
/ {
|
|
||||||
aliases {
|
|
||||||
vnd7050aj = &vnd7050aj;
|
|
||||||
};
|
|
||||||
|
|
||||||
vnd7050aj: vnd7050aj {
|
|
||||||
compatible = "st,vnd7050aj";
|
|
||||||
status = "okay";
|
|
||||||
|
|
||||||
input0-gpios = <&gpiob 3 GPIO_ACTIVE_HIGH>;
|
|
||||||
input1-gpios = <&gpiob 4 GPIO_ACTIVE_HIGH>;
|
|
||||||
select0-gpios = <&gpiob 7 GPIO_ACTIVE_HIGH>;
|
|
||||||
select1-gpios = <&gpiob 9 GPIO_ACTIVE_HIGH>;
|
|
||||||
sense-enable-gpios = <&gpiob 6 GPIO_ACTIVE_HIGH>;
|
|
||||||
fault-reset-gpios = <&gpiob 5 GPIO_ACTIVE_LOW>;
|
|
||||||
io-channels = <&adc1 1>;
|
|
||||||
r-sense-ohms = <1500>;
|
|
||||||
k-vcc = <4000>;
|
|
||||||
};
|
|
||||||
};
|
|
||||||
|
|
||||||
&adc1 {
|
|
||||||
status = "okay";
|
|
||||||
pinctrl-0 = <&adc1_in1_pa0>;
|
|
||||||
pinctrl-names = "default";
|
|
||||||
st,adc-clock-source = "SYNC";
|
|
||||||
st,adc-prescaler = <4>;
|
|
||||||
#address-cells = <1>;
|
|
||||||
#size-cells = <0>;
|
|
||||||
|
|
||||||
channel@1 {
|
|
||||||
reg = <1>;
|
|
||||||
zephyr,gain = "ADC_GAIN_1";
|
|
||||||
zephyr,reference = "ADC_REF_INTERNAL";
|
|
||||||
zephyr,acquisition-time = <ADC_ACQ_TIME_DEFAULT>;
|
|
||||||
zephyr,resolution = <12>;
|
|
||||||
};
|
|
||||||
};
|
|
||||||
|
|
||||||
&usart1 {
|
&usart1 {
|
||||||
modbus0 {
|
modbus0 {
|
||||||
compatible = "zephyr,modbus-serial";
|
compatible = "zephyr,modbus-serial";
|
||||||
status = "okay";
|
status = "okay";
|
||||||
};
|
};
|
||||||
status = "okay";
|
status = "okay";
|
||||||
pinctrl-0 = <&usart1_tx_pa9 &usart1_rx_pa10>; // PA9=TX, PA10=RX for Modbus communication
|
pinctrl-0 = <&usart1_tx_pa9 &usart1_rx_pa10>;
|
||||||
pinctrl-names = "default";
|
pinctrl-names = "default";
|
||||||
};
|
};
|
||||||
|
|
|
||||||
|
|
@ -1,5 +1,3 @@
|
||||||
#include <zephyr/dt-bindings/gpio/gpio.h>
|
|
||||||
|
|
||||||
&zephyr_udc0 {
|
&zephyr_udc0 {
|
||||||
cdc_acm_uart0: cdc_acm_uart0 {
|
cdc_acm_uart0: cdc_acm_uart0 {
|
||||||
compatible = "zephyr,cdc-acm-uart";
|
compatible = "zephyr,cdc-acm-uart";
|
||||||
|
|
@ -10,7 +8,3 @@
|
||||||
};
|
};
|
||||||
};
|
};
|
||||||
};
|
};
|
||||||
|
|
||||||
&usart1 {
|
|
||||||
/delete-node/ modbus0;
|
|
||||||
};
|
|
||||||
|
|
@ -1,88 +0,0 @@
|
||||||
# Copyright (c) 2024, Eduard Iten
|
|
||||||
# SPDX-License-Identifier: Apache-2.0
|
|
||||||
|
|
||||||
description: |
|
|
||||||
STMicroelectronics VND7050AJ dual-channel high-side driver.
|
|
||||||
This is a GPIO and ADC controlled device.
|
|
||||||
|
|
||||||
compatible: "st,vnd7050aj"
|
|
||||||
|
|
||||||
include: base.yaml
|
|
||||||
|
|
||||||
properties:
|
|
||||||
input0-gpios:
|
|
||||||
type: phandle-array
|
|
||||||
required: true
|
|
||||||
description: GPIO to control output channel 0.
|
|
||||||
|
|
||||||
input1-gpios:
|
|
||||||
type: phandle-array
|
|
||||||
required: true
|
|
||||||
description: GPIO to control output channel 1.
|
|
||||||
|
|
||||||
select0-gpios:
|
|
||||||
type: phandle-array
|
|
||||||
required: true
|
|
||||||
description: GPIO for MultiSense selection bit 0.
|
|
||||||
|
|
||||||
select1-gpios:
|
|
||||||
type: phandle-array
|
|
||||||
required: true
|
|
||||||
description: GPIO for MultiSense selection bit 1.
|
|
||||||
|
|
||||||
sense-enable-gpios:
|
|
||||||
type: phandle-array
|
|
||||||
required: true
|
|
||||||
description: GPIO to enable the MultiSense output.
|
|
||||||
|
|
||||||
fault-reset-gpios:
|
|
||||||
type: phandle-array
|
|
||||||
required: true
|
|
||||||
description: GPIO to reset a latched fault (active-low).
|
|
||||||
|
|
||||||
io-channels:
|
|
||||||
type: phandle-array
|
|
||||||
required: true
|
|
||||||
description: |
|
|
||||||
ADC channel connected to the MultiSense pin. This should be an
|
|
||||||
io-channels property pointing to the ADC controller and channel number.
|
|
||||||
|
|
||||||
r-sense-ohms:
|
|
||||||
type: int
|
|
||||||
required: true
|
|
||||||
description: |
|
|
||||||
Value of the external sense resistor connected from the MultiSense
|
|
||||||
pin to GND, specified in Ohms. This is critical for correct
|
|
||||||
conversion of the analog readings.
|
|
||||||
|
|
||||||
k-factor:
|
|
||||||
type: int
|
|
||||||
default: 1500
|
|
||||||
description: |
|
|
||||||
Factor between PowerMOS and SenseMOS.
|
|
||||||
|
|
||||||
k-vcc:
|
|
||||||
type: int
|
|
||||||
default: 8000
|
|
||||||
description: |
|
|
||||||
VCC sense ratio multiplied by 1000. Used for supply voltage calculation.
|
|
||||||
|
|
||||||
t-sense-0:
|
|
||||||
type: int
|
|
||||||
default: 25
|
|
||||||
description: |
|
|
||||||
Temperature sense reference temperature in degrees Celsius.
|
|
||||||
|
|
||||||
v-sense-0:
|
|
||||||
type: int
|
|
||||||
default: 2070
|
|
||||||
description: |
|
|
||||||
Temperature sense reference voltage in millivolts.
|
|
||||||
|
|
||||||
k-tchip:
|
|
||||||
type: int
|
|
||||||
default: -5500
|
|
||||||
description: |
|
|
||||||
Temperature sense gain coefficient multiplied by 1000.
|
|
||||||
Used for chip temperature calculation.
|
|
||||||
|
|
||||||
|
|
@ -5,9 +5,6 @@ CONFIG_LOG=y
|
||||||
# Enable Shell
|
# Enable Shell
|
||||||
CONFIG_SHELL=y
|
CONFIG_SHELL=y
|
||||||
CONFIG_REBOOT=y
|
CONFIG_REBOOT=y
|
||||||
CONFIG_SHELL_MODBUS=y
|
|
||||||
CONFIG_SHELL_VALVE=y
|
|
||||||
CONFIG_SHELL_SYSTEM=y
|
|
||||||
|
|
||||||
# Enable Settings Subsystem
|
# Enable Settings Subsystem
|
||||||
CONFIG_SETTINGS=y
|
CONFIG_SETTINGS=y
|
||||||
|
|
@ -22,7 +19,5 @@ CONFIG_SETTINGS_LOG_LEVEL_DBG=y
|
||||||
CONFIG_UART_INTERRUPT_DRIVEN=y
|
CONFIG_UART_INTERRUPT_DRIVEN=y
|
||||||
CONFIG_MODBUS=y
|
CONFIG_MODBUS=y
|
||||||
CONFIG_MODBUS_ROLE_SERVER=y
|
CONFIG_MODBUS_ROLE_SERVER=y
|
||||||
CONFIG_MODBUS_LOG_LEVEL_DBG=y
|
CONFIG_MODBUS_BUFFER_SIZE=256
|
||||||
|
|
||||||
# Enable VND7050AJ
|
|
||||||
CONFIG_VND7050AJ=y
|
|
||||||
|
|
|
||||||
|
|
@ -1,15 +1,14 @@
|
||||||
#include <zephyr/kernel.h>
|
#include <zephyr/kernel.h>
|
||||||
#include <zephyr/logging/log.h>
|
|
||||||
#include <zephyr/settings/settings.h>
|
#include <zephyr/settings/settings.h>
|
||||||
#include <lib/fwu.h>
|
#include <zephyr/logging/log.h>
|
||||||
#include <lib/modbus_server.h>
|
#include <lib/modbus_server.h>
|
||||||
#include <lib/valve.h>
|
#include <lib/valve.h>
|
||||||
|
#include <lib/fwu.h>
|
||||||
|
|
||||||
LOG_MODULE_REGISTER(main, LOG_LEVEL_INF);
|
LOG_MODULE_REGISTER(main, LOG_LEVEL_INF);
|
||||||
|
|
||||||
int main(void)
|
int main(void)
|
||||||
{
|
{
|
||||||
int rc;
|
|
||||||
LOG_INF("Starting Irrigation System Slave Node");
|
LOG_INF("Starting Irrigation System Slave Node");
|
||||||
|
|
||||||
if (settings_subsys_init() || settings_load()) {
|
if (settings_subsys_init() || settings_load()) {
|
||||||
|
|
@ -19,10 +18,9 @@ int main(void)
|
||||||
valve_init();
|
valve_init();
|
||||||
fwu_init();
|
fwu_init();
|
||||||
|
|
||||||
rc = modbus_server_init();
|
if (modbus_server_init()) {
|
||||||
if (rc) {
|
LOG_ERR("Modbus RTU server initialization failed");
|
||||||
LOG_ERR("Modbus server initialization failed: %d", rc);
|
return 0;
|
||||||
return rc;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
LOG_INF("Irrigation System Slave Node started successfully");
|
LOG_INF("Irrigation System Slave Node started successfully");
|
||||||
|
|
|
||||||
|
|
@ -1,5 +0,0 @@
|
||||||
SB_CONFIG_BOOTLOADER_MCUBOOT=y
|
|
||||||
SB_CONFIG_MCUBOOT_MODE_SINGLE_APP=y
|
|
||||||
|
|
||||||
CONFIG_LOG=y
|
|
||||||
CONFIG_MCUBOOT_LOG_LEVEL_INF=y
|
|
||||||
|
|
@ -1,5 +0,0 @@
|
||||||
# Gitignore settings for ESPHome
|
|
||||||
# This is an example and may include too much for your use-case.
|
|
||||||
# You can modify this file to suit your needs.
|
|
||||||
/.esphome/
|
|
||||||
/secrets.yaml
|
|
||||||
|
|
@ -1,106 +0,0 @@
|
||||||
# ===================================================================
|
|
||||||
# ESPHome Configuration
|
|
||||||
# CAN-Bus Master für ein Bewässerungssystem auf Basis des ESP32-C6
|
|
||||||
#
|
|
||||||
# Version 10: Finale Korrektur der Lambda-Signatur gemäß Dokumentation
|
|
||||||
# ===================================================================
|
|
||||||
|
|
||||||
esphome:
|
|
||||||
name: can-bridge
|
|
||||||
friendly_name: Irrigation can bridge
|
|
||||||
|
|
||||||
esp32:
|
|
||||||
board: esp32-c6-devkitm-1
|
|
||||||
framework:
|
|
||||||
type: esp-idf # Erforderlich für den ESP32-C6
|
|
||||||
|
|
||||||
# --- Netzwerk & Sicherheit ---
|
|
||||||
wifi:
|
|
||||||
ssid: !secret wifi_ssid
|
|
||||||
password: !secret wifi_password
|
|
||||||
fast_connect: true
|
|
||||||
|
|
||||||
api:
|
|
||||||
encryption:
|
|
||||||
key: !secret api_key
|
|
||||||
|
|
||||||
ota:
|
|
||||||
platform: esphome
|
|
||||||
password: !secret ota_password
|
|
||||||
|
|
||||||
logger:
|
|
||||||
|
|
||||||
web_server:
|
|
||||||
|
|
||||||
# --- Globale Variablen ---
|
|
||||||
globals:
|
|
||||||
- id: ventil_2_can_state
|
|
||||||
type: int
|
|
||||||
initial_value: '0' # Startet als "geschlossen"
|
|
||||||
|
|
||||||
# --- CAN-Bus Konfiguration ---
|
|
||||||
canbus:
|
|
||||||
- platform: esp32_can
|
|
||||||
id: my_can_bus
|
|
||||||
tx_pin: GPIO5
|
|
||||||
rx_pin: GPIO4
|
|
||||||
bit_rate: 125kbps
|
|
||||||
can_id: 0x000 # Erforderlich, um Parser-Fehler zu beheben.
|
|
||||||
on_frame:
|
|
||||||
# Horcht nur auf die Statusmeldung von Knoten 2 (ID 0x422)
|
|
||||||
- can_id: 0x422
|
|
||||||
then:
|
|
||||||
- lambda: |-
|
|
||||||
if (x.size() < 1) {
|
|
||||||
ESP_LOGW("on_can_frame", "Received empty Frame for ID 0x422");
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
int received_state = x[0];
|
|
||||||
id(ventil_2_can_state) = received_state;
|
|
||||||
ESP_LOGD("on_can_frame", "Received state from Valve 2: %i", received_state);
|
|
||||||
- valve.template.publish:
|
|
||||||
id: ventil_2
|
|
||||||
current_operation: !lambda |-
|
|
||||||
int state = id(ventil_2_can_state);
|
|
||||||
if (state == 2) {
|
|
||||||
return VALVE_OPERATION_OPENING;
|
|
||||||
} else if (state == 3) {
|
|
||||||
return VALVE_OPERATION_CLOSING;
|
|
||||||
} else {
|
|
||||||
return VALVE_OPERATION_IDLE;
|
|
||||||
}
|
|
||||||
|
|
||||||
# --- Home Assistant Entitäten ---
|
|
||||||
valve:
|
|
||||||
- platform: template
|
|
||||||
name: "Ventil 2"
|
|
||||||
id: ventil_2
|
|
||||||
|
|
||||||
# Diese Lambda meldet nur den binären End-Zustand (offen/geschlossen)
|
|
||||||
lambda: |-
|
|
||||||
if (id(ventil_2_can_state) == 0) {
|
|
||||||
return VALVE_CLOSED;
|
|
||||||
} else if (id(ventil_2_can_state) == 1) {
|
|
||||||
return VALVE_OPEN;
|
|
||||||
} else {
|
|
||||||
return NAN;
|
|
||||||
}
|
|
||||||
|
|
||||||
# Aktionen zum Steuern des Ventils
|
|
||||||
open_action:
|
|
||||||
- canbus.send:
|
|
||||||
canbus_id: my_can_bus
|
|
||||||
can_id: 0x210
|
|
||||||
data: [0x02, 0x01]
|
|
||||||
|
|
||||||
close_action:
|
|
||||||
- canbus.send:
|
|
||||||
canbus_id: my_can_bus
|
|
||||||
can_id: 0x210
|
|
||||||
data: [0x02, 0x00]
|
|
||||||
|
|
||||||
stop_action:
|
|
||||||
- canbus.send:
|
|
||||||
canbus_id: my_can_bus
|
|
||||||
can_id: 0x210
|
|
||||||
data: [0x02, 0x03]
|
|
||||||
|
|
@ -1,55 +0,0 @@
|
||||||
#!/usr/bin/env python3
|
|
||||||
|
|
||||||
import secrets
|
|
||||||
import string
|
|
||||||
import os
|
|
||||||
import base64
|
|
||||||
from ruamel.yaml import YAML
|
|
||||||
|
|
||||||
def generate_password(length=32):
|
|
||||||
"""Generate a random password."""
|
|
||||||
alphabet = string.ascii_letters + string.digits
|
|
||||||
return ''.join(secrets.choice(alphabet) for i in range(length))
|
|
||||||
|
|
||||||
def generate_api_key():
|
|
||||||
"""Generate a random 32-byte key and base64 encode it."""
|
|
||||||
return base64.b64encode(secrets.token_bytes(32)).decode('utf-8')
|
|
||||||
|
|
||||||
SECRETS_FILE = 'secrets.yaml'
|
|
||||||
# In a real ESPHome project, secrets are often included from a central location
|
|
||||||
# but for this script, we'll assume it's in the current directory.
|
|
||||||
# You might need to adjust this path.
|
|
||||||
secrets_path = os.path.join(os.path.dirname(os.path.abspath(__file__)), SECRETS_FILE)
|
|
||||||
|
|
||||||
yaml = YAML()
|
|
||||||
yaml.preserve_quotes = True
|
|
||||||
# To prevent line wrapping
|
|
||||||
yaml.width = 4096
|
|
||||||
|
|
||||||
try:
|
|
||||||
with open(secrets_path, 'r') as f:
|
|
||||||
secrets_data = yaml.load(f)
|
|
||||||
if secrets_data is None:
|
|
||||||
secrets_data = {}
|
|
||||||
except FileNotFoundError:
|
|
||||||
print(f"Info: '{SECRETS_FILE}' not found. A new file will be created.")
|
|
||||||
secrets_data = {}
|
|
||||||
|
|
||||||
# Generate new random passwords
|
|
||||||
new_api_key = generate_api_key()
|
|
||||||
new_ota_password = generate_password()
|
|
||||||
|
|
||||||
# Update the dictionary with the new passwords
|
|
||||||
if 'api_password' in secrets_data:
|
|
||||||
del secrets_data['api_password']
|
|
||||||
secrets_data['api_key'] = new_api_key
|
|
||||||
secrets_data['ota_password'] = new_ota_password
|
|
||||||
|
|
||||||
# Write the updated dictionary back to the YAML file
|
|
||||||
with open(secrets_path, 'w') as f:
|
|
||||||
yaml.dump(secrets_data, f)
|
|
||||||
|
|
||||||
print(f"Successfully updated '{SECRETS_FILE}'.")
|
|
||||||
print("New values:")
|
|
||||||
print(f" api_key: {new_api_key}")
|
|
||||||
print(f" ota_password: {new_ota_password}")
|
|
||||||
|
|
@ -1,166 +0,0 @@
|
||||||
# ===================================================================
|
|
||||||
# ESPHome Configuration - Final Version
|
|
||||||
#
|
|
||||||
# This version corrects the C++ function call inside the valve actions
|
|
||||||
# to use the correct `send` method from the ModbusDevice base class,
|
|
||||||
# which is compatible with the esp-idf framework.
|
|
||||||
# ===================================================================
|
|
||||||
|
|
||||||
esphome:
|
|
||||||
name: irrigation-system
|
|
||||||
friendly_name: Bewässerung
|
|
||||||
|
|
||||||
esp32:
|
|
||||||
board: esp32-c6-devkitm-1
|
|
||||||
framework:
|
|
||||||
type: esp-idf # Set to esp-idf as required by the ESP32-C6 board
|
|
||||||
|
|
||||||
wifi:
|
|
||||||
ssid: !secret wifi_ssid
|
|
||||||
password: !secret wifi_password
|
|
||||||
fast_connect: true
|
|
||||||
|
|
||||||
api:
|
|
||||||
encryption:
|
|
||||||
key: !secret api_key
|
|
||||||
|
|
||||||
ota:
|
|
||||||
platform: esphome
|
|
||||||
password: !secret ota_password
|
|
||||||
|
|
||||||
logger:
|
|
||||||
|
|
||||||
web_server:
|
|
||||||
|
|
||||||
# ===================================================================
|
|
||||||
# HARDWARE SETUP - COMPLETE
|
|
||||||
# ===================================================================
|
|
||||||
|
|
||||||
# --- UART for RS485 Communication ---
|
|
||||||
uart:
|
|
||||||
id: uart_bus
|
|
||||||
tx_pin: GPIO1
|
|
||||||
rx_pin: GPIO2
|
|
||||||
baud_rate: 9600
|
|
||||||
data_bits: 8
|
|
||||||
stop_bits: 1
|
|
||||||
parity: NONE
|
|
||||||
|
|
||||||
# --- Base Modbus component for the bus ---
|
|
||||||
modbus:
|
|
||||||
- id: modbus_hub
|
|
||||||
uart_id: uart_bus
|
|
||||||
|
|
||||||
# --- Modbus Controller for the specific valve device ---
|
|
||||||
modbus_controller:
|
|
||||||
- id: valve_controller
|
|
||||||
modbus_id: modbus_hub
|
|
||||||
address: 0 # The Modbus address of your valve. Change if not 0.
|
|
||||||
# update_interval: 1s
|
|
||||||
|
|
||||||
# ===================================================================
|
|
||||||
# SENSORS - COMPLETE
|
|
||||||
# ===================================================================
|
|
||||||
sensor:
|
|
||||||
# This sensor reads the raw 16-bit value from the valve's input register.
|
|
||||||
- platform: modbus_controller
|
|
||||||
modbus_controller_id: valve_controller
|
|
||||||
name: "Valve Raw Status"
|
|
||||||
id: valve_raw_status
|
|
||||||
internal: true # Hide from Home Assistant UI
|
|
||||||
register_type: read # 'read' is the valid type for input registers
|
|
||||||
address: 0x0000 # The address of the register to read
|
|
||||||
value_type: U_WORD # Read the full 16-bit unsigned word
|
|
||||||
- platform: modbus_controller
|
|
||||||
modbus_controller_id: valve_controller
|
|
||||||
name: "VDD"
|
|
||||||
id: valve_vdd
|
|
||||||
register_type: read # 'read' is the valid type for input registers
|
|
||||||
address: 0x00FC # The address of the register to read
|
|
||||||
value_type: U_WORD # Read the full 16-bit unsigned word
|
|
||||||
entity_category: diagnostic # Mark as diagnostic
|
|
||||||
unit_of_measurement: "V"
|
|
||||||
accuracy_decimals: 2 # Show two decimal places
|
|
||||||
# Apply filters to convert the raw value to volts and update periodically
|
|
||||||
filters:
|
|
||||||
- lambda: |-
|
|
||||||
// Convert the raw VDD value to volts
|
|
||||||
return x / 1000.0; // Assuming the value is in millivolts
|
|
||||||
- heartbeat: 60s # Update every 60 seconds
|
|
||||||
- delta: 200 # Only update if the value changes by more than 200 mV
|
|
||||||
|
|
||||||
# ===================================================================
|
|
||||||
# TEXT SENSORS FOR HUMAN-READABLE STATUS
|
|
||||||
# ===================================================================
|
|
||||||
text_sensor:
|
|
||||||
# 1. This text sensor extracts the HIGH BYTE for the operation status.
|
|
||||||
- platform: template
|
|
||||||
name: "Valve Operation"
|
|
||||||
id: valve_operation_status
|
|
||||||
icon: "mdi:state-machine"
|
|
||||||
lambda: |-
|
|
||||||
// Extract the high byte from the raw status sensor
|
|
||||||
// using a bitwise right shift.
|
|
||||||
int operation_code = (int)id(valve_raw_status).state >> 8;
|
|
||||||
switch (operation_code) {
|
|
||||||
case 0: return {"Idle"};
|
|
||||||
case 1: return {"Opening"};
|
|
||||||
case 2: return {"Closing"};
|
|
||||||
case 3: return {"Obstacle Detected"};
|
|
||||||
case 4: return {"End Position Not Reached"};
|
|
||||||
default: return {"Unknown Operation"};
|
|
||||||
}
|
|
||||||
|
|
||||||
# 2. This text sensor extracts the LOW BYTE for the current valve state.
|
|
||||||
- platform: template
|
|
||||||
name: "Valve Position"
|
|
||||||
id: valve_position_status
|
|
||||||
icon: "mdi:valve"
|
|
||||||
lambda: |-
|
|
||||||
// Extract the low byte from the raw status sensor
|
|
||||||
// using a bitwise AND mask.
|
|
||||||
int state_code = (int)id(valve_raw_status).state & 0xFF;
|
|
||||||
switch (state_code) {
|
|
||||||
case 0: return {"Closed"};
|
|
||||||
case 1: return {"Open"};
|
|
||||||
default: return {"Unknown"};
|
|
||||||
}
|
|
||||||
|
|
||||||
# ===================================================================
|
|
||||||
# THE MAIN VALVE COMPONENT
|
|
||||||
# ===================================================================
|
|
||||||
valve:
|
|
||||||
- platform: template
|
|
||||||
name: "Modbus Controlled Valve"
|
|
||||||
id: modbus_valve
|
|
||||||
optimistic: false
|
|
||||||
# The lambda determines the current state (open or closed) of the valve.
|
|
||||||
lambda: |-
|
|
||||||
int state_code = (int)id(valve_raw_status).state & 0xFF;
|
|
||||||
if (state_code == 1) {
|
|
||||||
return true; // Open
|
|
||||||
} else if (state_code == 0) {
|
|
||||||
return false; // Closed
|
|
||||||
} else {
|
|
||||||
return {}; // Unknown
|
|
||||||
}
|
|
||||||
# Action to execute when the "OPEN" button is pressed.
|
|
||||||
open_action:
|
|
||||||
- lambda: |-
|
|
||||||
// Use the send() command inherited from ModbusDevice
|
|
||||||
// Function 0x06: Write Single Register
|
|
||||||
// Payload for value 1 is {0x00, 0x01}
|
|
||||||
const uint8_t data[] = {0x00, 0x01};
|
|
||||||
id(valve_controller).send(0x06, 0x0000, 1, 2, data);
|
|
||||||
# Action to execute when the "CLOSE" button is pressed.
|
|
||||||
close_action:
|
|
||||||
- lambda: |-
|
|
||||||
// Payload for value 2 is {0x00, 0x02}
|
|
||||||
const uint8_t data[] = {0x00, 0x02};
|
|
||||||
id(valve_controller).send(0x06, 0x0000, 1, 2, data);
|
|
||||||
# Action to execute when the "STOP" button is pressed.
|
|
||||||
stop_action:
|
|
||||||
- lambda: |-
|
|
||||||
// Payload for value 3 is {0x00, 0x03}
|
|
||||||
const uint8_t data[] = {0x00, 0x03};
|
|
||||||
id(valve_controller).send(0x06, 0x0000, 1, 2, data);
|
|
||||||
|
|
@ -1,2 +0,0 @@
|
||||||
ruamel.yaml
|
|
||||||
esphome
|
|
||||||
|
|
@ -1,4 +0,0 @@
|
||||||
wifi_ssid: 'PUT YOUR WIFI SSID HERE'
|
|
||||||
wifi_password: 'PUT YOUR WIFI PASSWORD HERE'
|
|
||||||
api_key: 'PUT YOUR KEY HERE OR USE create_secrets.py'
|
|
||||||
ota_password: 'PUT YOUR KEY HERE OR USE create_secrets.py'
|
|
||||||
|
|
@ -1,41 +0,0 @@
|
||||||
#ifndef CAN_IDS_H
|
|
||||||
#define CAN_IDS_H
|
|
||||||
|
|
||||||
/*
|
|
||||||
CAN ID structure for the irrigation system.
|
|
||||||
PPP FFFF NNNN
|
|
||||||
|
|
||||||
PPP: Priority
|
|
||||||
000: Network segment
|
|
||||||
001: Critical error
|
|
||||||
010: Commands
|
|
||||||
100: Status messages
|
|
||||||
110: measurements
|
|
||||||
111: Info messages
|
|
||||||
|
|
||||||
FFFF: Function
|
|
||||||
0001: Valve Commands
|
|
||||||
0010: Valve States
|
|
||||||
0011: IO Commands
|
|
||||||
0100: IO States
|
|
||||||
0101: Measurements
|
|
||||||
0111: Sysem Functions (e.g. reset, firmware update)
|
|
||||||
|
|
||||||
NNNN: Node ID
|
|
||||||
*/
|
|
||||||
|
|
||||||
#define CAN_ID_PRIORITY_NETWORK 0x000
|
|
||||||
#define CAN_ID_PRIORITY_CRITICAL_ERROR 0x100
|
|
||||||
#define CAN_ID_PRIORITY_COMMANDS 0x200
|
|
||||||
#define CAN_ID_PRIORITY_STATUS 0x400
|
|
||||||
#define CAN_ID_PRIORITY_MEASUREMENTS 0x600
|
|
||||||
#define CAN_ID_PRIORITY_INFO 0x700
|
|
||||||
|
|
||||||
#define CAN_ID_FUNCTION_VALVE_COMMANDS 0x010
|
|
||||||
#define CAN_ID_FUNCTION_VALVE_STATES 0x020
|
|
||||||
#define CAN_ID_FUNCTION_IO_COMMANDS 0x030
|
|
||||||
#define CAN_ID_FUNCTION_IO_STATES 0x040
|
|
||||||
#define CAN_ID_FUNCTION_MEASUREMENTS 0x050
|
|
||||||
#define CAN_ID_FUNCTION_SYSTEM_FUNCTIONS 0x070
|
|
||||||
|
|
||||||
#endif // CAN_IDS_H
|
|
||||||
|
|
@ -3,45 +3,8 @@
|
||||||
|
|
||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
|
|
||||||
/**
|
|
||||||
* @file fwu.h
|
|
||||||
* @brief API for the Firmware Update (FWU) library.
|
|
||||||
*
|
|
||||||
* This library provides the core logic for handling the over-the-air firmware
|
|
||||||
* update process via Modbus. It manages the data buffer, processes commands,
|
|
||||||
* and calculates CRC checksums for data verification.
|
|
||||||
*/
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Initializes the firmware update module.
|
|
||||||
*
|
|
||||||
* This function currently does nothing but is a placeholder for future
|
|
||||||
* initialization logic.
|
|
||||||
*/
|
|
||||||
void fwu_init(void);
|
void fwu_init(void);
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Handles incoming Modbus register writes related to firmware updates.
|
|
||||||
*
|
|
||||||
* This function is the main entry point for the FWU process. It parses the
|
|
||||||
* address and value from a Modbus write operation and takes appropriate action,
|
|
||||||
* such as storing metadata (offset, size) or data chunks, and processing
|
|
||||||
* commands (verify, finalize).
|
|
||||||
*
|
|
||||||
* @param addr The Modbus register address being written to.
|
|
||||||
* @param reg The 16-bit value being written to the register.
|
|
||||||
*/
|
|
||||||
void fwu_handler(uint16_t addr, uint16_t reg);
|
void fwu_handler(uint16_t addr, uint16_t reg);
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Gets the CRC16-CCITT of the last received firmware chunk.
|
|
||||||
*
|
|
||||||
* After a data chunk is fully received into the buffer, this function can be
|
|
||||||
* called to retrieve the calculated CRC checksum. The master can then compare
|
|
||||||
* this with its own calculated CRC to verify data integrity.
|
|
||||||
*
|
|
||||||
* @return The 16-bit CRC of the last chunk.
|
|
||||||
*/
|
|
||||||
uint16_t fwu_get_last_chunk_crc(void);
|
uint16_t fwu_get_last_chunk_crc(void);
|
||||||
|
|
||||||
#endif // FWU_H
|
#endif // FWU_H
|
||||||
|
|
|
||||||
|
|
@ -4,182 +4,49 @@
|
||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @file modbus_server.h
|
* @brief Modbus Input Register Addresses.
|
||||||
* @brief API for the Modbus server implementation.
|
|
||||||
*
|
|
||||||
* This file defines the Modbus register map and provides functions to
|
|
||||||
* initialize and manage the Modbus server.
|
|
||||||
*/
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Modbus Input Register Addresses (Read-Only).
|
|
||||||
* @see docs/modbus-registers.de.md
|
|
||||||
*/
|
*/
|
||||||
enum {
|
enum {
|
||||||
/**
|
/* Valve Control & Status */
|
||||||
* @brief Combined status register for the valve.
|
|
||||||
* High-Byte: Movement (0=Idle, 1=Opening, 2=Closing, 3=Error).
|
|
||||||
* Low-Byte: State (0=Closed, 1=Open).
|
|
||||||
*/
|
|
||||||
REG_INPUT_VALVE_STATE_MOVEMENT = 0x0000,
|
REG_INPUT_VALVE_STATE_MOVEMENT = 0x0000,
|
||||||
/**
|
REG_INPUT_MOTOR_CURRENT_MA = 0x0001,
|
||||||
* @brief Motor current during opening in milliamperes (mA).
|
/* Digital Inputs */
|
||||||
*/
|
|
||||||
REG_INPUT_MOTOR_OPEN_CURRENT_MA = 0x0001,
|
|
||||||
/**
|
|
||||||
* @brief Motor current during closing in milliamperes (mA).
|
|
||||||
*/
|
|
||||||
REG_INPUT_MOTOR_CLOSE_CURRENT_MA = 0x0002,
|
|
||||||
/**
|
|
||||||
* @brief Bitmask of digital inputs. Bit 0: Input 1, Bit 1: Input 2.
|
|
||||||
* 1=Active.
|
|
||||||
*/
|
|
||||||
REG_INPUT_DIGITAL_INPUTS_STATE = 0x0020,
|
REG_INPUT_DIGITAL_INPUTS_STATE = 0x0020,
|
||||||
/**
|
|
||||||
* @brief Event flags for buttons (Clear-on-Read). Bit 0: Button 1 pressed.
|
|
||||||
* Bit 1: Button 2 pressed.
|
|
||||||
*/
|
|
||||||
REG_INPUT_BUTTON_EVENTS = 0x0021,
|
REG_INPUT_BUTTON_EVENTS = 0x0021,
|
||||||
/**
|
/* System Config & Status */
|
||||||
* @brief Firmware version, e.g., 0x0102 for v1.2.
|
|
||||||
*/
|
|
||||||
REG_INPUT_FIRMWARE_VERSION_MAJOR_MINOR = 0x00F0,
|
REG_INPUT_FIRMWARE_VERSION_MAJOR_MINOR = 0x00F0,
|
||||||
/**
|
|
||||||
* @brief Firmware version patch level, e.g., 3 for v1.2.3.
|
|
||||||
*/
|
|
||||||
REG_INPUT_FIRMWARE_VERSION_PATCH = 0x00F1,
|
REG_INPUT_FIRMWARE_VERSION_PATCH = 0x00F1,
|
||||||
/**
|
|
||||||
* @brief Device status (0=OK, 1=General Error).
|
|
||||||
*/
|
|
||||||
REG_INPUT_DEVICE_STATUS = 0x00F2,
|
REG_INPUT_DEVICE_STATUS = 0x00F2,
|
||||||
/**
|
|
||||||
* @brief Lower 16 bits of uptime in seconds.
|
|
||||||
*/
|
|
||||||
REG_INPUT_UPTIME_SECONDS_LOW = 0x00F3,
|
REG_INPUT_UPTIME_SECONDS_LOW = 0x00F3,
|
||||||
/**
|
|
||||||
* @brief Upper 16 bits of uptime in seconds.
|
|
||||||
*/
|
|
||||||
REG_INPUT_UPTIME_SECONDS_HIGH = 0x00F4,
|
REG_INPUT_UPTIME_SECONDS_HIGH = 0x00F4,
|
||||||
/**
|
/* Firmware Update */
|
||||||
* @brief Current supply voltage in millivolts (mV).
|
REG_INPUT_FWU_LAST_CHUNK_CRC = 0x0100,
|
||||||
*/
|
|
||||||
REG_INPUT_SUPPLY_VOLTAGE_MV = 0x00F5,
|
|
||||||
/**
|
|
||||||
* @brief CRC16 of the last received data chunk in the buffer for firmware
|
|
||||||
* update.
|
|
||||||
*/
|
|
||||||
REG_INPUT_FWU_LAST_CHUNK_CRC = 0x0100
|
|
||||||
};
|
};
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief Modbus Holding Register Addresses (Read/Write).
|
* @brief Modbus Holding Register Addresses.
|
||||||
* @see docs/modbus-registers.de.md
|
|
||||||
*/
|
*/
|
||||||
enum {
|
enum {
|
||||||
/**
|
/* Valve Control */
|
||||||
* @brief Valve control command (1=Open, 2=Close, 0=Stop movement).
|
|
||||||
*/
|
|
||||||
REG_HOLDING_VALVE_COMMAND = 0x0000,
|
REG_HOLDING_VALVE_COMMAND = 0x0000,
|
||||||
/**
|
|
||||||
* @brief Safety timeout in seconds for the opening process.
|
|
||||||
*/
|
|
||||||
REG_HOLDING_MAX_OPENING_TIME_S = 0x0001,
|
REG_HOLDING_MAX_OPENING_TIME_S = 0x0001,
|
||||||
/**
|
|
||||||
* @brief Safety timeout in seconds for the closing process.
|
|
||||||
*/
|
|
||||||
REG_HOLDING_MAX_CLOSING_TIME_S = 0x0002,
|
REG_HOLDING_MAX_CLOSING_TIME_S = 0x0002,
|
||||||
/**
|
/* Digital Outputs */
|
||||||
* @brief Minimum current threshold in mA for end-position detection.
|
|
||||||
*/
|
|
||||||
REG_HOLDING_END_CURRENT_THRESHOLD_OPEN_MA = 0x0003,
|
|
||||||
/**
|
|
||||||
* @brief Minimum current threshold in mA for end-position detection during
|
|
||||||
* closing.
|
|
||||||
*/
|
|
||||||
REG_HOLDING_END_CURRENT_THRESHOLD_CLOSE_MA = 0x0004,
|
|
||||||
/**
|
|
||||||
* @brief Current threshold in mA for obstacle detection during opening.
|
|
||||||
*/
|
|
||||||
REG_HOLDING_OBSTACLE_THRESHOLD_OPEN_MA = 0x0005,
|
|
||||||
/**
|
|
||||||
* @brief Current threshold in mA for obstacle detection during closing.
|
|
||||||
*/
|
|
||||||
REG_HOLDING_OBSTACLE_THRESHOLD_CLOSE_MA = 0x0006,
|
|
||||||
/**
|
|
||||||
* @brief Bitmask for reading and writing digital outputs. Bit 0: Output 1,
|
|
||||||
* Bit 1: Output 2. 1=ON, 0=OFF.
|
|
||||||
*/
|
|
||||||
REG_HOLDING_DIGITAL_OUTPUTS_STATE = 0x0010,
|
REG_HOLDING_DIGITAL_OUTPUTS_STATE = 0x0010,
|
||||||
/**
|
/* System Config */
|
||||||
* @brief Fail-safe watchdog timeout in seconds. 0=Disabled.
|
|
||||||
*/
|
|
||||||
REG_HOLDING_WATCHDOG_TIMEOUT_S = 0x00F0,
|
REG_HOLDING_WATCHDOG_TIMEOUT_S = 0x00F0,
|
||||||
/**
|
|
||||||
* @brief Writing 1 restarts the device.
|
|
||||||
*/
|
|
||||||
REG_HOLDING_DEVICE_RESET = 0x00F1,
|
REG_HOLDING_DEVICE_RESET = 0x00F1,
|
||||||
/**
|
/* Firmware Update */
|
||||||
* @brief Command for firmware update.
|
|
||||||
* 1: Verify Chunk - Slave writes the last chunk to flash.
|
|
||||||
* 2: Finalize Update - Complete installation and restart.
|
|
||||||
*/
|
|
||||||
REG_HOLDING_FWU_COMMAND = 0x0100,
|
REG_HOLDING_FWU_COMMAND = 0x0100,
|
||||||
/**
|
|
||||||
* @brief Lower 16 bits of the 32-bit offset for the next firmware update
|
|
||||||
* chunk.
|
|
||||||
*/
|
|
||||||
REG_HOLDING_FWU_CHUNK_OFFSET_LOW = 0x0101,
|
REG_HOLDING_FWU_CHUNK_OFFSET_LOW = 0x0101,
|
||||||
/**
|
|
||||||
* @brief Upper 16 bits of the 32-bit offset for the next firmware update
|
|
||||||
* chunk.
|
|
||||||
*/
|
|
||||||
REG_HOLDING_FWU_CHUNK_OFFSET_HIGH = 0x0102,
|
REG_HOLDING_FWU_CHUNK_OFFSET_HIGH = 0x0102,
|
||||||
/**
|
|
||||||
* @brief Size of the next firmware update chunk in bytes (max. 256).
|
|
||||||
*/
|
|
||||||
REG_HOLDING_FWU_CHUNK_SIZE = 0x0103,
|
REG_HOLDING_FWU_CHUNK_SIZE = 0x0103,
|
||||||
/**
|
|
||||||
* @brief Start address of the 256-byte buffer for firmware update data.
|
|
||||||
*/
|
|
||||||
REG_HOLDING_FWU_DATA_BUFFER = 0x0180,
|
REG_HOLDING_FWU_DATA_BUFFER = 0x0180,
|
||||||
};
|
};
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Initializes the Modbus server.
|
|
||||||
*
|
|
||||||
* This function sets up the Modbus RTU server interface, loads saved settings
|
|
||||||
* (baudrate, unit ID), and starts listening for requests.
|
|
||||||
*
|
|
||||||
* @return 0 on success, or a negative error code on failure.
|
|
||||||
*/
|
|
||||||
int modbus_server_init(void);
|
int modbus_server_init(void);
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Reconfigures the Modbus server at runtime.
|
|
||||||
*
|
|
||||||
* Updates the baudrate and unit ID of the server. If the reconfiguration
|
|
||||||
* fails, the settings are saved and will be applied after a device reset.
|
|
||||||
*
|
|
||||||
* @param baudrate The new baudrate to set.
|
|
||||||
* @param unit_id The new Modbus unit ID (slave address).
|
|
||||||
* @return 0 on success, or a negative error code if immediate reconfiguration
|
|
||||||
* fails. Returns 0 even on failure if settings could be saved for the next
|
|
||||||
* boot.
|
|
||||||
*/
|
|
||||||
int modbus_reconfigure(uint32_t baudrate, uint8_t unit_id);
|
int modbus_reconfigure(uint32_t baudrate, uint8_t unit_id);
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Gets the current baudrate of the Modbus server.
|
|
||||||
*
|
|
||||||
* @return The current baudrate.
|
|
||||||
*/
|
|
||||||
uint32_t modbus_get_baudrate(void);
|
uint32_t modbus_get_baudrate(void);
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Gets the current unit ID of the Modbus server.
|
|
||||||
*
|
|
||||||
* @return The current unit ID.
|
|
||||||
*/
|
|
||||||
uint8_t modbus_get_unit_id(void);
|
uint8_t modbus_get_unit_id(void);
|
||||||
|
|
||||||
#endif // MODBUS_SERVER_H
|
#endif // MODBUS_SERVER_H
|
||||||
|
|
|
||||||
|
|
@ -1,214 +1,23 @@
|
||||||
#ifndef VALVE_H
|
#ifndef VALVE_H
|
||||||
#define VALVE_H
|
#define VALVE_H
|
||||||
|
|
||||||
#include <zephyr/drivers/gpio.h>
|
|
||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
|
|
||||||
/**
|
enum valve_state { VALVE_STATE_CLOSED, VALVE_STATE_OPEN };
|
||||||
* @file valve.h
|
enum valve_movement { VALVE_MOVEMENT_IDLE, VALVE_MOVEMENT_OPENING, VALVE_MOVEMENT_CLOSING, VALVE_MOVEMENT_ERROR };
|
||||||
* @brief API for controlling the motorized valve.
|
|
||||||
*
|
|
||||||
* This library provides functions to initialize, open, close, and stop the
|
|
||||||
* valve. It also allows getting the valve's state and movement status, and
|
|
||||||
* configuring the maximum opening and closing times.
|
|
||||||
*/
|
|
||||||
|
|
||||||
#define VALVE_CHANNEL_OPEN 0
|
void valve_init(void);
|
||||||
#define VALVE_CHANNEL_CLOSE 1
|
|
||||||
#define VALVE_CURRENT_CHECK_INTERVAL K_MSEC(CONFIG_VALVE_INTERVALL_CURRENT_CHECK_MS)
|
|
||||||
#define VALVE_INITIAL_CURRENT_CHECK_INTERVAL K_MSEC(CONFIG_VALVE_INITIAL_INTERVALL_CURRENT_CHECK_MS)
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Represents the static state of the valve (open or closed).
|
|
||||||
*/
|
|
||||||
enum valve_state {
|
|
||||||
VALVE_STATE_CLOSED, /**< The valve is fully closed. */
|
|
||||||
VALVE_STATE_OPEN, /**< The valve is fully open. */
|
|
||||||
};
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Represents the dynamic movement status of the valve.
|
|
||||||
*/
|
|
||||||
enum valve_movement {
|
|
||||||
VALVE_MOVEMENT_IDLE, /**< The valve is not moving. */
|
|
||||||
VALVE_MOVEMENT_OPENING, /**< The valve is currently opening. */
|
|
||||||
VALVE_MOVEMENT_CLOSING, /**< The valve is currently closing. */
|
|
||||||
VALVE_MOVEMENT_ERROR /**< An error occurred during movement. */
|
|
||||||
};
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Initializes the valve control system.
|
|
||||||
*
|
|
||||||
* Configures the GPIOs and loads saved settings for timeouts.
|
|
||||||
* This function must be called before any other valve functions.
|
|
||||||
*
|
|
||||||
* @return 0 on success, or a negative error code on failure.
|
|
||||||
*/
|
|
||||||
int valve_init(void);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Starts opening the valve.
|
|
||||||
*
|
|
||||||
* The valve will open for the configured maximum opening time.
|
|
||||||
*/
|
|
||||||
void valve_open(void);
|
void valve_open(void);
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Starts closing the valve.
|
|
||||||
*
|
|
||||||
* The valve will close for the configured maximum closing time.
|
|
||||||
*/
|
|
||||||
void valve_close(void);
|
void valve_close(void);
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Stops any ongoing valve movement immediately.
|
|
||||||
*/
|
|
||||||
void valve_stop(void);
|
void valve_stop(void);
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Gets the current static state of the valve.
|
|
||||||
*
|
|
||||||
* @return The current valve state (VALVE_STATE_CLOSED or VALVE_STATE_OPEN).
|
|
||||||
*/
|
|
||||||
enum valve_state valve_get_state(void);
|
enum valve_state valve_get_state(void);
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Gets the current movement status of the valve.
|
|
||||||
*
|
|
||||||
* @return The current movement status.
|
|
||||||
*/
|
|
||||||
enum valve_movement valve_get_movement(void);
|
enum valve_movement valve_get_movement(void);
|
||||||
|
uint16_t valve_get_motor_current(void);
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Sets the maximum time for the valve to open.
|
|
||||||
*
|
|
||||||
* @param seconds The timeout in seconds.
|
|
||||||
*/
|
|
||||||
void valve_set_max_open_time(uint16_t seconds);
|
void valve_set_max_open_time(uint16_t seconds);
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Sets the maximum time for the valve to close.
|
|
||||||
*
|
|
||||||
* @param seconds The timeout in seconds.
|
|
||||||
*/
|
|
||||||
void valve_set_max_close_time(uint16_t seconds);
|
void valve_set_max_close_time(uint16_t seconds);
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Sets the current threshold for end-position detection during opening.
|
|
||||||
*
|
|
||||||
* @param current_ma The current threshold in milliamps.
|
|
||||||
*/
|
|
||||||
void valve_set_end_current_threshold_open(uint16_t current_ma);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Sets the current threshold for end-position detection during closing.
|
|
||||||
*
|
|
||||||
* @param current_ma The current threshold in milliamps.
|
|
||||||
*/
|
|
||||||
void valve_set_end_current_threshold_close(uint16_t current_ma);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Gets the current threshold for end-position detection during opening.
|
|
||||||
*
|
|
||||||
* @return The current threshold in milliamps.
|
|
||||||
*/
|
|
||||||
uint16_t valve_get_end_current_threshold_open(void);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Gets the current threshold for end-position detection during closing.
|
|
||||||
*
|
|
||||||
* @return The current threshold in milliamps.
|
|
||||||
*/
|
|
||||||
uint16_t valve_get_end_current_threshold_close(void);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Gets the configured maximum opening time.
|
|
||||||
*
|
|
||||||
* @return The timeout in seconds.
|
|
||||||
*/
|
|
||||||
uint16_t valve_get_max_open_time(void);
|
uint16_t valve_get_max_open_time(void);
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Gets the configured maximum closing time.
|
|
||||||
*
|
|
||||||
* @return The timeout in seconds.
|
|
||||||
*/
|
|
||||||
uint16_t valve_get_max_close_time(void);
|
uint16_t valve_get_max_close_time(void);
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Gets the current drawn by the valve motor during opening.
|
|
||||||
*
|
|
||||||
* @return The motor current in milliamps.
|
|
||||||
*/
|
|
||||||
int32_t valve_get_opening_current(void);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Gets the current drawn by the valve motor during closing.
|
|
||||||
*
|
|
||||||
* @return The motor current in milliamps.
|
|
||||||
*/
|
|
||||||
int32_t valve_get_closing_current(void);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Gets the temperature of the valve motor driver.
|
|
||||||
*
|
|
||||||
* @return The temperature in degrees Celsius.
|
|
||||||
*/
|
|
||||||
int32_t valve_get_vnd_temp(void);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Gets the voltage supplied to the valve motor driver.
|
|
||||||
*
|
|
||||||
* @return The voltage in millivolts.
|
|
||||||
*/
|
|
||||||
int32_t valve_get_vnd_voltage(void);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Sets the current threshold for obstacle detection during opening.
|
|
||||||
*
|
|
||||||
* @param current_ma The current threshold in milliamps.
|
|
||||||
*/
|
|
||||||
void valve_set_obstacle_threshold_open(uint16_t current_ma);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Sets the current threshold for obstacle detection during closing.
|
|
||||||
*
|
|
||||||
* @param current_ma The current threshold in milliamps.
|
|
||||||
*/
|
|
||||||
void valve_set_obstacle_threshold_close(uint16_t current_ma);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Gets the current threshold for obstacle detection during opening.
|
|
||||||
*
|
|
||||||
* @return The current threshold in milliamps.
|
|
||||||
*/
|
|
||||||
uint16_t valve_get_obstacle_threshold_open(void);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Gets the current threshold for obstacle detection during closing.
|
|
||||||
*
|
|
||||||
* @return The current threshold in milliamps.
|
|
||||||
*/
|
|
||||||
uint16_t valve_get_obstacle_threshold_close(void);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Callback function called during valve opening with current readings.
|
|
||||||
*
|
|
||||||
* This is a weak function that can be overridden to provide custom handling
|
|
||||||
* of current readings during valve opening operations.
|
|
||||||
*
|
|
||||||
* @param current_ma The current reading in milliamps.
|
|
||||||
*/
|
|
||||||
void valve_current_open_callback(int current_ma);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Callback function called during valve closing with current readings.
|
|
||||||
*
|
|
||||||
* This is a weak function that can be overridden to provide custom handling
|
|
||||||
* of current readings during valve closing operations.
|
|
||||||
*
|
|
||||||
* @param current_ma The current reading in milliamps.
|
|
||||||
*/
|
|
||||||
void valve_current_close_callback(int current_ma);
|
|
||||||
|
|
||||||
#endif // VALVE_H
|
#endif // VALVE_H
|
||||||
|
|
@ -1,74 +0,0 @@
|
||||||
/*
|
|
||||||
* Copyright (c) 2025, Eduard Iten
|
|
||||||
* SPDX-License-Identifier: Apache-2.0
|
|
||||||
*/
|
|
||||||
|
|
||||||
#ifndef ZEPHYR_INCLUDE_DRIVERS_MISC_VND7050AJ_H_
|
|
||||||
#define ZEPHYR_INCLUDE_DRIVERS_MISC_VND7050AJ_H_
|
|
||||||
|
|
||||||
#include <zephyr/device.h>
|
|
||||||
#include <zephyr/kernel.h>
|
|
||||||
|
|
||||||
#ifdef __cplusplus
|
|
||||||
extern "C" {
|
|
||||||
#endif
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Channel identifiers for the VND7050AJ.
|
|
||||||
*/
|
|
||||||
#define VND7050AJ_CHANNEL_0 0
|
|
||||||
#define VND7050AJ_CHANNEL_1 1
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Sets the state of a specific output channel.
|
|
||||||
*
|
|
||||||
* @param dev Pointer to the device structure for the driver instance.
|
|
||||||
* @param channel The channel to control (VND7050AJ_CHANNEL_0 or VND7050AJ_CHANNEL_1).
|
|
||||||
* @param state The desired state (true for ON, false for OFF).
|
|
||||||
* @return 0 on success, negative error code on failure.
|
|
||||||
*/
|
|
||||||
int vnd7050aj_set_output_state(const struct device *dev, uint8_t channel, bool state);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Reads the load current for a specific channel.
|
|
||||||
*
|
|
||||||
* @param dev Pointer to the device structure for the driver instance.
|
|
||||||
* @param channel The channel to measure (VND7050AJ_CHANNEL_0 or VND7050AJ_CHANNEL_1).
|
|
||||||
* @param[out] current_ma Pointer to store the measured current in milliamperes (mA).
|
|
||||||
* @return 0 on success, negative error code on failure.
|
|
||||||
*/
|
|
||||||
int vnd7050aj_read_load_current(const struct device *dev, uint8_t channel, int32_t *current_ma);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Reads the VCC supply voltage.
|
|
||||||
*
|
|
||||||
* @param dev Pointer to the device structure for the driver instance.
|
|
||||||
* @param[out] voltage_mv Pointer to store the measured voltage in millivolts (mV).
|
|
||||||
* @return 0 on success, negative error code on failure.
|
|
||||||
*/
|
|
||||||
int vnd7050aj_read_supply_voltage(const struct device *dev, int32_t *voltage_mv);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Reads the internal chip temperature.
|
|
||||||
*
|
|
||||||
* @param dev Pointer to the device structure for the driver instance.
|
|
||||||
* @param[out] temp_c Pointer to store the measured temperature in degrees Celsius (°C).
|
|
||||||
* @return 0 on success, negative error code on failure.
|
|
||||||
*/
|
|
||||||
int vnd7050aj_read_chip_temp(const struct device *dev, int32_t *temp_c);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Resets a latched fault condition.
|
|
||||||
*
|
|
||||||
* This function sends a low pulse to the FaultRST pin.
|
|
||||||
*
|
|
||||||
* @param dev Pointer to the device structure for the driver instance.
|
|
||||||
* @return 0 on success, negative error code on failure.
|
|
||||||
*/
|
|
||||||
int vnd7050aj_reset_fault(const struct device *dev);
|
|
||||||
|
|
||||||
#ifdef __cplusplus
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#endif /* ZEPHYR_INCLUDE_DRIVERS_MISC_VND7050AJ_H_ */
|
|
||||||
|
|
@ -3,5 +3,3 @@ add_subdirectory_ifdef(CONFIG_LIB_MODBUS_SERVER modbus_server)
|
||||||
add_subdirectory_ifdef(CONFIG_LIB_VALVE valve)
|
add_subdirectory_ifdef(CONFIG_LIB_VALVE valve)
|
||||||
add_subdirectory_ifdef(CONFIG_SHELL_SYSTEM shell_system)
|
add_subdirectory_ifdef(CONFIG_SHELL_SYSTEM shell_system)
|
||||||
add_subdirectory_ifdef(CONFIG_SHELL_MODBUS shell_modbus)
|
add_subdirectory_ifdef(CONFIG_SHELL_MODBUS shell_modbus)
|
||||||
add_subdirectory_ifdef(CONFIG_SHELL_VALVE shell_valve)
|
|
||||||
add_subdirectory_ifdef(CONFIG_VND7050AJ vnd7050aj)
|
|
||||||
|
|
@ -5,6 +5,4 @@ rsource "modbus_server/Kconfig"
|
||||||
rsource "valve/Kconfig"
|
rsource "valve/Kconfig"
|
||||||
rsource "shell_system/Kconfig"
|
rsource "shell_system/Kconfig"
|
||||||
rsource "shell_modbus/Kconfig"
|
rsource "shell_modbus/Kconfig"
|
||||||
rsource "shell_valve/Kconfig"
|
|
||||||
rsource "vnd7050aj/Kconfig"
|
|
||||||
endmenu
|
endmenu
|
||||||
|
|
@ -1,44 +1,26 @@
|
||||||
/**
|
|
||||||
* @file fwu.c
|
|
||||||
* @brief Implementation of the Firmware Update (FWU) library.
|
|
||||||
*
|
|
||||||
* This file implements the logic for receiving a new firmware image in chunks
|
|
||||||
* over Modbus. It maintains a buffer for the incoming data, calculates the CRC
|
|
||||||
* of the received chunk, and handles commands to verify the chunk and finalize
|
|
||||||
* the update process. The actual writing to flash is simulated.
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include <zephyr/kernel.h>
|
#include <zephyr/kernel.h>
|
||||||
#include <zephyr/logging/log.h>
|
|
||||||
#include <zephyr/sys/byteorder.h>
|
|
||||||
#include <zephyr/sys/crc.h>
|
#include <zephyr/sys/crc.h>
|
||||||
|
#include <zephyr/sys/byteorder.h>
|
||||||
|
#include <zephyr/logging/log.h>
|
||||||
#include <lib/fwu.h>
|
#include <lib/fwu.h>
|
||||||
|
|
||||||
LOG_MODULE_REGISTER(fwu, LOG_LEVEL_INF);
|
LOG_MODULE_REGISTER(fwu, LOG_LEVEL_INF);
|
||||||
|
|
||||||
#define FWU_BUFFER_SIZE 256
|
#define FWU_BUFFER_SIZE 256
|
||||||
static uint8_t fwu_buffer[FWU_BUFFER_SIZE]; // Buffer to store incoming
|
static uint8_t fwu_buffer[FWU_BUFFER_SIZE];
|
||||||
// firmware data chunks
|
static uint32_t fwu_chunk_offset = 0;
|
||||||
static uint32_t fwu_chunk_offset = 0; // Offset for the current firmware chunk in the overall image
|
static uint16_t fwu_chunk_size = 0;
|
||||||
static uint16_t fwu_chunk_size = 0; // Size of the current firmware chunk
|
static uint16_t fwu_last_chunk_crc = 0;
|
||||||
static uint16_t fwu_last_chunk_crc = 0; // CRC16 of the last received firmware chunk
|
|
||||||
|
|
||||||
void fwu_init(void)
|
void fwu_init(void) {}
|
||||||
{
|
|
||||||
}
|
|
||||||
|
|
||||||
void fwu_handler(uint16_t addr, uint16_t reg)
|
void fwu_handler(uint16_t addr, uint16_t reg)
|
||||||
{
|
{
|
||||||
// This is a simplified handler. In a real scenario, you would have a proper
|
// This is a simplified handler. In a real scenario, you would have a proper mapping
|
||||||
// mapping between register addresses and actions.
|
// between register addresses and actions.
|
||||||
if (addr == 0x0100) { // FWU_COMMAND
|
if (addr == 0x0100) { // FWU_COMMAND
|
||||||
if (reg == 1) {
|
if (reg == 1) { LOG_INF("FWU: Chunk at offset %u (size %u) verified.", fwu_chunk_offset, fwu_chunk_size); }
|
||||||
LOG_INF("FWU: Chunk at offset %u (size %u) verified.",
|
else if (reg == 2) { LOG_INF("FWU: Finalize command received. Rebooting (simulated)."); }
|
||||||
fwu_chunk_offset,
|
|
||||||
fwu_chunk_size);
|
|
||||||
} else if (reg == 2) {
|
|
||||||
LOG_INF("FWU: Finalize command received. Rebooting (simulated).");
|
|
||||||
}
|
|
||||||
} else if (addr == 0x0101) { // FWU_CHUNK_OFFSET_LOW
|
} else if (addr == 0x0101) { // FWU_CHUNK_OFFSET_LOW
|
||||||
fwu_chunk_offset = (fwu_chunk_offset & 0xFFFF0000) | reg;
|
fwu_chunk_offset = (fwu_chunk_offset & 0xFFFF0000) | reg;
|
||||||
} else if (addr == 0x0102) { // FWU_CHUNK_OFFSET_HIGH
|
} else if (addr == 0x0102) { // FWU_CHUNK_OFFSET_HIGH
|
||||||
|
|
@ -50,8 +32,7 @@ void fwu_handler(uint16_t addr, uint16_t reg)
|
||||||
if (index < sizeof(fwu_buffer)) {
|
if (index < sizeof(fwu_buffer)) {
|
||||||
sys_put_be16(reg, &fwu_buffer[index]);
|
sys_put_be16(reg, &fwu_buffer[index]);
|
||||||
if (index + 2 >= fwu_chunk_size) {
|
if (index + 2 >= fwu_chunk_size) {
|
||||||
fwu_last_chunk_crc =
|
fwu_last_chunk_crc = crc16_ccitt(0xffff, fwu_buffer, fwu_chunk_size);
|
||||||
crc16_ccitt(0xffff, fwu_buffer, fwu_chunk_size);
|
|
||||||
LOG_INF("FWU: Chunk received, CRC is 0x%04X", fwu_last_chunk_crc);
|
LOG_INF("FWU: Chunk received, CRC is 0x%04X", fwu_last_chunk_crc);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -1,5 +1,5 @@
|
||||||
config LIB_MODBUS_SERVER
|
config LIB_MODBUS_SERVER
|
||||||
bool "Enable Modbus Server Library"
|
bool "Enable Modbus Server Library"
|
||||||
default n
|
default y
|
||||||
help
|
help
|
||||||
Enable the Modbus Server Library.
|
Enable the Modbus Server Library.
|
||||||
|
|
|
||||||
|
|
@ -1,25 +1,15 @@
|
||||||
/**
|
|
||||||
* @file modbus_server.c
|
|
||||||
* @brief Modbus RTU server implementation for the irrigation system slave node.
|
|
||||||
*
|
|
||||||
* This file implements the Modbus server logic, including register callbacks,
|
|
||||||
* watchdog handling, and dynamic reconfiguration. It interfaces with other
|
|
||||||
* libraries like valve control, ADC sensors, and firmware updates.
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include <zephyr/device.h>
|
|
||||||
#include <zephyr/drivers/uart.h>
|
|
||||||
#include <zephyr/kernel.h>
|
#include <zephyr/kernel.h>
|
||||||
#include <zephyr/logging/log.h>
|
#include <zephyr/drivers/uart.h>
|
||||||
|
#include <zephyr/device.h>
|
||||||
#include <zephyr/modbus/modbus.h>
|
#include <zephyr/modbus/modbus.h>
|
||||||
|
#include <zephyr/logging/log.h>
|
||||||
#include <zephyr/settings/settings.h>
|
#include <zephyr/settings/settings.h>
|
||||||
#include <zephyr/sys/reboot.h>
|
#include <zephyr/sys/reboot.h>
|
||||||
#include <zephyr/usb/usb_device.h>
|
|
||||||
#include <app_version.h>
|
|
||||||
#include <lib/fwu.h>
|
|
||||||
#include <lib/modbus_server.h>
|
#include <lib/modbus_server.h>
|
||||||
#include <lib/valve.h>
|
#include <lib/valve.h>
|
||||||
#include <lib/vnd7050aj.h>
|
#include <lib/fwu.h>
|
||||||
|
|
||||||
|
#include <zephyr/usb/usb_device.h>
|
||||||
|
|
||||||
LOG_MODULE_REGISTER(modbus_server, LOG_LEVEL_INF);
|
LOG_MODULE_REGISTER(modbus_server, LOG_LEVEL_INF);
|
||||||
|
|
||||||
|
|
@ -33,45 +23,25 @@ static struct modbus_iface_param server_param = {
|
||||||
static uint16_t watchdog_timeout_s = 0;
|
static uint16_t watchdog_timeout_s = 0;
|
||||||
static struct k_timer watchdog_timer;
|
static struct k_timer watchdog_timer;
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Timer handler for the Modbus watchdog.
|
|
||||||
*
|
|
||||||
* This function is called when the watchdog timer expires, indicating a loss
|
|
||||||
* of communication with the Modbus master. It triggers a fail-safe action,
|
|
||||||
* which is to close the valve.
|
|
||||||
*
|
|
||||||
* @param timer_id Pointer to the timer instance.
|
|
||||||
*/
|
|
||||||
static void watchdog_timer_handler(struct k_timer *timer_id)
|
static void watchdog_timer_handler(struct k_timer *timer_id)
|
||||||
{
|
{
|
||||||
LOG_WRN("Modbus watchdog expired! Closing valve as a fail-safe.");
|
LOG_WRN("Modbus watchdog expired! Closing valve as a fail-safe.");
|
||||||
valve_close();
|
valve_close();
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Resets the Modbus watchdog timer.
|
|
||||||
*
|
|
||||||
* This function should be called upon receiving any valid Modbus request
|
|
||||||
* to prevent the watchdog from expiring.
|
|
||||||
*/
|
|
||||||
static inline void reset_watchdog(void)
|
static inline void reset_watchdog(void)
|
||||||
{
|
{
|
||||||
if (watchdog_timeout_s > 0) {
|
if (watchdog_timeout_s > 0)
|
||||||
|
{
|
||||||
k_timer_start(&watchdog_timer, K_SECONDS(watchdog_timeout_s), K_NO_WAIT);
|
k_timer_start(&watchdog_timer, K_SECONDS(watchdog_timeout_s), K_NO_WAIT);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Callback for reading Modbus holding registers.
|
|
||||||
*
|
|
||||||
* @param addr Register address.
|
|
||||||
* @param reg Pointer to store the read value.
|
|
||||||
* @return 0 on success.
|
|
||||||
*/
|
|
||||||
static int holding_reg_rd(uint16_t addr, uint16_t *reg)
|
static int holding_reg_rd(uint16_t addr, uint16_t *reg)
|
||||||
{
|
{
|
||||||
reset_watchdog();
|
reset_watchdog();
|
||||||
switch (addr) {
|
switch (addr)
|
||||||
|
{
|
||||||
case REG_HOLDING_MAX_OPENING_TIME_S:
|
case REG_HOLDING_MAX_OPENING_TIME_S:
|
||||||
*reg = valve_get_max_open_time();
|
*reg = valve_get_max_open_time();
|
||||||
break;
|
break;
|
||||||
|
|
@ -81,18 +51,6 @@ static int holding_reg_rd(uint16_t addr, uint16_t *reg)
|
||||||
case REG_HOLDING_WATCHDOG_TIMEOUT_S:
|
case REG_HOLDING_WATCHDOG_TIMEOUT_S:
|
||||||
*reg = watchdog_timeout_s;
|
*reg = watchdog_timeout_s;
|
||||||
break;
|
break;
|
||||||
case REG_HOLDING_END_CURRENT_THRESHOLD_OPEN_MA:
|
|
||||||
*reg = valve_get_end_current_threshold_open();
|
|
||||||
break;
|
|
||||||
case REG_HOLDING_END_CURRENT_THRESHOLD_CLOSE_MA:
|
|
||||||
*reg = valve_get_end_current_threshold_close();
|
|
||||||
break;
|
|
||||||
case REG_HOLDING_OBSTACLE_THRESHOLD_OPEN_MA:
|
|
||||||
*reg = valve_get_obstacle_threshold_open();
|
|
||||||
break;
|
|
||||||
case REG_HOLDING_OBSTACLE_THRESHOLD_CLOSE_MA:
|
|
||||||
*reg = valve_get_obstacle_threshold_close();
|
|
||||||
break;
|
|
||||||
default:
|
default:
|
||||||
*reg = 0;
|
*reg = 0;
|
||||||
break;
|
break;
|
||||||
|
|
@ -100,23 +58,22 @@ static int holding_reg_rd(uint16_t addr, uint16_t *reg)
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Callback for writing Modbus holding registers.
|
|
||||||
*
|
|
||||||
* @param addr Register address.
|
|
||||||
* @param reg Value to write.
|
|
||||||
* @return 0 on success.
|
|
||||||
*/
|
|
||||||
static int holding_reg_wr(uint16_t addr, uint16_t reg)
|
static int holding_reg_wr(uint16_t addr, uint16_t reg)
|
||||||
{
|
{
|
||||||
reset_watchdog();
|
reset_watchdog();
|
||||||
switch (addr) {
|
switch (addr)
|
||||||
|
{
|
||||||
case REG_HOLDING_VALVE_COMMAND:
|
case REG_HOLDING_VALVE_COMMAND:
|
||||||
if (reg == 1) {
|
if (reg == 1)
|
||||||
|
{
|
||||||
valve_open();
|
valve_open();
|
||||||
} else if (reg == 2) {
|
}
|
||||||
|
else if (reg == 2)
|
||||||
|
{
|
||||||
valve_close();
|
valve_close();
|
||||||
} else if (reg == 0) {
|
}
|
||||||
|
else if (reg == 0)
|
||||||
|
{
|
||||||
valve_stop();
|
valve_stop();
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
|
|
@ -126,30 +83,22 @@ static int holding_reg_wr(uint16_t addr, uint16_t reg)
|
||||||
case REG_HOLDING_MAX_CLOSING_TIME_S:
|
case REG_HOLDING_MAX_CLOSING_TIME_S:
|
||||||
valve_set_max_close_time(reg);
|
valve_set_max_close_time(reg);
|
||||||
break;
|
break;
|
||||||
case REG_HOLDING_END_CURRENT_THRESHOLD_OPEN_MA:
|
|
||||||
valve_set_end_current_threshold_open(reg);
|
|
||||||
break;
|
|
||||||
case REG_HOLDING_END_CURRENT_THRESHOLD_CLOSE_MA:
|
|
||||||
valve_set_end_current_threshold_close(reg);
|
|
||||||
break;
|
|
||||||
case REG_HOLDING_OBSTACLE_THRESHOLD_OPEN_MA:
|
|
||||||
valve_set_obstacle_threshold_open(reg);
|
|
||||||
break;
|
|
||||||
case REG_HOLDING_OBSTACLE_THRESHOLD_CLOSE_MA:
|
|
||||||
valve_set_obstacle_threshold_close(reg);
|
|
||||||
break;
|
|
||||||
case REG_HOLDING_WATCHDOG_TIMEOUT_S:
|
case REG_HOLDING_WATCHDOG_TIMEOUT_S:
|
||||||
watchdog_timeout_s = reg;
|
watchdog_timeout_s = reg;
|
||||||
if (watchdog_timeout_s > 0) {
|
if (watchdog_timeout_s > 0)
|
||||||
|
{
|
||||||
LOG_INF("Watchdog enabled with %u s timeout.", watchdog_timeout_s);
|
LOG_INF("Watchdog enabled with %u s timeout.", watchdog_timeout_s);
|
||||||
reset_watchdog();
|
reset_watchdog();
|
||||||
} else {
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
LOG_INF("Watchdog disabled.");
|
LOG_INF("Watchdog disabled.");
|
||||||
k_timer_stop(&watchdog_timer);
|
k_timer_stop(&watchdog_timer);
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
case REG_HOLDING_DEVICE_RESET:
|
case REG_HOLDING_DEVICE_RESET:
|
||||||
if (reg == 1) {
|
if (reg == 1)
|
||||||
|
{
|
||||||
LOG_WRN("Modbus reset command received. Rebooting...");
|
LOG_WRN("Modbus reset command received. Rebooting...");
|
||||||
sys_reboot(SYS_REBOOT_WARM);
|
sys_reboot(SYS_REBOOT_WARM);
|
||||||
}
|
}
|
||||||
|
|
@ -161,26 +110,17 @@ static int holding_reg_wr(uint16_t addr, uint16_t reg)
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Callback for reading Modbus input registers.
|
|
||||||
*
|
|
||||||
* @param addr Register address.
|
|
||||||
* @param reg Pointer to store the read value.
|
|
||||||
* @return 0 on success.
|
|
||||||
*/
|
|
||||||
static int input_reg_rd(uint16_t addr, uint16_t *reg)
|
static int input_reg_rd(uint16_t addr, uint16_t *reg)
|
||||||
{
|
{
|
||||||
reset_watchdog();
|
reset_watchdog();
|
||||||
uint32_t uptime_s = k_uptime_get_32() / 1000;
|
uint32_t uptime_s = k_uptime_get_32() / 1000;
|
||||||
switch (addr) {
|
switch (addr)
|
||||||
|
{
|
||||||
case REG_INPUT_VALVE_STATE_MOVEMENT:
|
case REG_INPUT_VALVE_STATE_MOVEMENT:
|
||||||
*reg = (valve_get_movement() << 8) | (valve_get_state() & 0xFF);
|
*reg = (valve_get_movement() << 8) | (valve_get_state() & 0xFF);
|
||||||
break;
|
break;
|
||||||
case REG_INPUT_MOTOR_OPEN_CURRENT_MA:
|
case REG_INPUT_MOTOR_CURRENT_MA:
|
||||||
*reg = (uint16_t)valve_get_opening_current();
|
*reg = valve_get_motor_current();
|
||||||
break;
|
|
||||||
case REG_INPUT_MOTOR_CLOSE_CURRENT_MA:
|
|
||||||
*reg = (uint16_t)valve_get_closing_current();
|
|
||||||
break;
|
break;
|
||||||
case REG_INPUT_UPTIME_SECONDS_LOW:
|
case REG_INPUT_UPTIME_SECONDS_LOW:
|
||||||
*reg = (uint16_t)(uptime_s & 0xFFFF);
|
*reg = (uint16_t)(uptime_s & 0xFFFF);
|
||||||
|
|
@ -188,17 +128,14 @@ static int input_reg_rd(uint16_t addr, uint16_t *reg)
|
||||||
case REG_INPUT_UPTIME_SECONDS_HIGH:
|
case REG_INPUT_UPTIME_SECONDS_HIGH:
|
||||||
*reg = (uint16_t)(uptime_s >> 16);
|
*reg = (uint16_t)(uptime_s >> 16);
|
||||||
break;
|
break;
|
||||||
case REG_INPUT_SUPPLY_VOLTAGE_MV:
|
|
||||||
*reg = (uint16_t)valve_get_vnd_voltage();
|
|
||||||
break;
|
|
||||||
case REG_INPUT_FWU_LAST_CHUNK_CRC:
|
case REG_INPUT_FWU_LAST_CHUNK_CRC:
|
||||||
*reg = fwu_get_last_chunk_crc();
|
*reg = fwu_get_last_chunk_crc();
|
||||||
break;
|
break;
|
||||||
case REG_INPUT_FIRMWARE_VERSION_MAJOR_MINOR:
|
case REG_INPUT_FIRMWARE_VERSION_MAJOR_MINOR:
|
||||||
*reg = (APP_VERSION_MAJOR << 8) | APP_VERSION_MINOR;
|
*reg = (0 << 8) | 0;
|
||||||
break;
|
break;
|
||||||
case REG_INPUT_FIRMWARE_VERSION_PATCH:
|
case REG_INPUT_FIRMWARE_VERSION_PATCH:
|
||||||
*reg = APP_PATCHLEVEL;
|
*reg = 2;
|
||||||
break;
|
break;
|
||||||
default:
|
default:
|
||||||
*reg = 0;
|
*reg = 0;
|
||||||
|
|
@ -208,7 +145,6 @@ static int input_reg_rd(uint16_t addr, uint16_t *reg)
|
||||||
}
|
}
|
||||||
|
|
||||||
static struct modbus_user_callbacks mbs_cbs = {
|
static struct modbus_user_callbacks mbs_cbs = {
|
||||||
// Modbus server callback functions
|
|
||||||
.holding_reg_rd = holding_reg_rd,
|
.holding_reg_rd = holding_reg_rd,
|
||||||
.holding_reg_wr = holding_reg_wr,
|
.holding_reg_wr = holding_reg_wr,
|
||||||
.input_reg_rd = input_reg_rd,
|
.input_reg_rd = input_reg_rd,
|
||||||
|
|
@ -219,27 +155,18 @@ static struct modbus_user_callbacks mbs_cbs = {
|
||||||
int modbus_server_init(void)
|
int modbus_server_init(void)
|
||||||
{
|
{
|
||||||
k_timer_init(&watchdog_timer, watchdog_timer_handler, NULL);
|
k_timer_init(&watchdog_timer, watchdog_timer_handler, NULL);
|
||||||
|
|
||||||
// Load saved settings
|
|
||||||
uint32_t saved_baudrate = 19200;
|
|
||||||
uint8_t saved_unit_id = 1;
|
|
||||||
settings_load_one("modbus/baudrate", &saved_baudrate, sizeof(saved_baudrate));
|
|
||||||
settings_load_one("modbus/unit_id", &saved_unit_id, sizeof(saved_unit_id));
|
|
||||||
|
|
||||||
// Apply loaded settings
|
|
||||||
server_param.serial.baud = saved_baudrate;
|
|
||||||
server_param.server.unit_id = saved_unit_id;
|
|
||||||
|
|
||||||
const char iface_name[] = {DEVICE_DT_NAME(MODBUS_NODE)};
|
const char iface_name[] = {DEVICE_DT_NAME(MODBUS_NODE)};
|
||||||
#if DT_NODE_HAS_COMPAT(DT_PARENT(MODBUS_NODE), zephyr_cdc_acm_uart)
|
#if DT_NODE_HAS_COMPAT(DT_PARENT(MODBUS_NODE), zephyr_cdc_acm_uart)
|
||||||
const struct device *const dev = DEVICE_DT_GET(DT_PARENT(MODBUS_NODE));
|
const struct device *const dev = DEVICE_DT_GET(DT_PARENT(MODBUS_NODE));
|
||||||
uint32_t dtr = 0;
|
uint32_t dtr = 0;
|
||||||
|
|
||||||
if (!device_is_ready(dev) || usb_enable(NULL)) {
|
if (!device_is_ready(dev) || usb_enable(NULL))
|
||||||
|
{
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
while (!dtr) {
|
while (!dtr)
|
||||||
|
{
|
||||||
uart_line_ctrl_get(dev, UART_LINE_CTRL_DTR, &dtr);
|
uart_line_ctrl_get(dev, UART_LINE_CTRL_DTR, &dtr);
|
||||||
k_sleep(K_MSEC(100));
|
k_sleep(K_MSEC(100));
|
||||||
}
|
}
|
||||||
|
|
@ -247,49 +174,29 @@ int modbus_server_init(void)
|
||||||
LOG_INF("Client connected to server on %s", dev->name);
|
LOG_INF("Client connected to server on %s", dev->name);
|
||||||
#endif
|
#endif
|
||||||
modbus_iface = modbus_iface_get_by_name(iface_name);
|
modbus_iface = modbus_iface_get_by_name(iface_name);
|
||||||
if (modbus_iface < 0) {
|
if (modbus_iface < 0)
|
||||||
|
{
|
||||||
return modbus_iface;
|
return modbus_iface;
|
||||||
}
|
}
|
||||||
server_param.server.user_cb = &mbs_cbs;
|
server_param.server.user_cb = &mbs_cbs;
|
||||||
|
|
||||||
LOG_INF("Starting Modbus server: baudrate=%u, unit_id=%u", saved_baudrate, saved_unit_id);
|
|
||||||
return modbus_init_server(modbus_iface, server_param);
|
return modbus_init_server(modbus_iface, server_param);
|
||||||
}
|
}
|
||||||
|
|
||||||
int modbus_reconfigure(uint32_t baudrate, uint8_t unit_id)
|
int modbus_reconfigure(uint32_t baudrate, uint8_t unit_id)
|
||||||
{
|
{
|
||||||
// Update parameters
|
|
||||||
server_param.serial.baud = baudrate;
|
server_param.serial.baud = baudrate;
|
||||||
server_param.server.unit_id = unit_id;
|
server_param.server.unit_id = unit_id;
|
||||||
|
|
||||||
// Try to reinitialize - this should work for most cases
|
|
||||||
int ret = modbus_init_server(modbus_iface, server_param);
|
int ret = modbus_init_server(modbus_iface, server_param);
|
||||||
|
|
||||||
if (ret == 0) {
|
if (ret == 0)
|
||||||
|
{
|
||||||
settings_save_one("modbus/baudrate", &baudrate, sizeof(baudrate));
|
settings_save_one("modbus/baudrate", &baudrate, sizeof(baudrate));
|
||||||
settings_save_one("modbus/unit_id", &unit_id, sizeof(unit_id));
|
settings_save_one("modbus/unit_id", &unit_id, sizeof(unit_id));
|
||||||
LOG_INF("Modbus reconfigured: baudrate=%u, unit_id=%u", baudrate, unit_id);
|
|
||||||
} else {
|
|
||||||
LOG_ERR("Failed to reconfigure Modbus: %d", ret);
|
|
||||||
LOG_INF("Modbus reconfiguration requires restart to take effect");
|
|
||||||
|
|
||||||
// Save settings for next boot
|
|
||||||
settings_save_one("modbus/baudrate", &baudrate, sizeof(baudrate));
|
|
||||||
settings_save_one("modbus/unit_id", &unit_id, sizeof(unit_id));
|
|
||||||
|
|
||||||
LOG_INF("Settings saved. Type 'reset' to restart the device and apply the "
|
|
||||||
"change.");
|
|
||||||
return 0; // Return success since settings are saved
|
|
||||||
}
|
}
|
||||||
|
|
||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
uint32_t modbus_get_baudrate(void)
|
uint32_t modbus_get_baudrate(void) { return server_param.serial.baud; }
|
||||||
{
|
uint8_t modbus_get_unit_id(void) { return server_param.server.unit_id; }
|
||||||
return server_param.serial.baud;
|
|
||||||
}
|
|
||||||
uint8_t modbus_get_unit_id(void)
|
|
||||||
{
|
|
||||||
return server_param.server.unit_id;
|
|
||||||
}
|
|
||||||
|
|
@ -1,7 +1,5 @@
|
||||||
config SHELL_MODBUS
|
config SHELL_MODBUS
|
||||||
bool "Enable Shell Modbus"
|
bool "Enable Shell Modbus"
|
||||||
default n
|
default y
|
||||||
depends on SHELL
|
|
||||||
depends on LIB_MODBUS_SERVER
|
|
||||||
help
|
help
|
||||||
Enable the modbus shell commands.
|
Enable the modnbus shell commands.
|
||||||
|
|
@ -1,30 +1,12 @@
|
||||||
/**
|
|
||||||
* @file shell_modbus.c
|
|
||||||
* @brief Provides shell commands for Modbus and valve configuration.
|
|
||||||
*
|
|
||||||
* This file implements a set of commands for the Zephyr shell to allow
|
|
||||||
* runtime configuration of the Modbus server (baudrate, slave ID) and the
|
|
||||||
* valve (max opening/closing times). The settings are persisted to non-volatile
|
|
||||||
* storage.
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include <zephyr/shell/shell.h>
|
#include <zephyr/shell/shell.h>
|
||||||
#include <lib/modbus_server.h>
|
|
||||||
#include <stdio.h>
|
|
||||||
#include <stdlib.h>
|
#include <stdlib.h>
|
||||||
|
#include <lib/modbus_server.h>
|
||||||
|
#include <lib/valve.h>
|
||||||
|
|
||||||
/**
|
static int cmd_modbus_set_baud(const struct shell *sh, size_t argc, char **argv)
|
||||||
* @brief Shell command to set the Modbus baudrate.
|
|
||||||
*
|
|
||||||
* @param sh The shell instance.
|
|
||||||
* @param argc Argument count.
|
|
||||||
* @param argv Argument values.
|
|
||||||
* @return 0 on success, -EINVAL on error.
|
|
||||||
*/
|
|
||||||
static int cmd_modbus_setb(const struct shell *sh, size_t argc, char **argv)
|
|
||||||
{
|
{
|
||||||
if (argc != 2) {
|
if (argc != 2) {
|
||||||
shell_error(sh, "Usage: setb <baudrate>");
|
shell_error(sh, "Usage: set_baud <baudrate>");
|
||||||
return -EINVAL;
|
return -EINVAL;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -41,13 +23,9 @@ static int cmd_modbus_setb(const struct shell *sh, size_t argc, char **argv)
|
||||||
|
|
||||||
if (!is_valid) {
|
if (!is_valid) {
|
||||||
char error_msg[128];
|
char error_msg[128];
|
||||||
int offset =
|
int offset = snprintf(error_msg, sizeof(error_msg), "Invalid baudrate. Valid rates are: ");
|
||||||
snprintf(error_msg, sizeof(error_msg), "Invalid baudrate. Valid rates are: ");
|
|
||||||
for (int i = 0; i < ARRAY_SIZE(valid_baud_rates); i++) {
|
for (int i = 0; i < ARRAY_SIZE(valid_baud_rates); i++) {
|
||||||
offset += snprintf(error_msg + offset,
|
offset += snprintf(error_msg + offset, sizeof(error_msg) - offset, "%u ", valid_baud_rates[i]);
|
||||||
sizeof(error_msg) - offset,
|
|
||||||
"%u ",
|
|
||||||
valid_baud_rates[i]);
|
|
||||||
}
|
}
|
||||||
shell_error(sh, "%s", error_msg);
|
shell_error(sh, "%s", error_msg);
|
||||||
return -EINVAL;
|
return -EINVAL;
|
||||||
|
|
@ -62,18 +40,10 @@ static int cmd_modbus_setb(const struct shell *sh, size_t argc, char **argv)
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
static int cmd_modbus_set_id(const struct shell *sh, size_t argc, char **argv)
|
||||||
* @brief Shell command to set the Modbus slave ID.
|
|
||||||
*
|
|
||||||
* @param sh The shell instance.
|
|
||||||
* @param argc Argument count.
|
|
||||||
* @param argv Argument values.
|
|
||||||
* @return 0 on success, -EINVAL on error.
|
|
||||||
*/
|
|
||||||
static int cmd_modbus_setid(const struct shell *sh, size_t argc, char **argv)
|
|
||||||
{
|
{
|
||||||
if (argc != 2) {
|
if (argc != 2) {
|
||||||
shell_error(sh, "Usage: setid <slave_id>");
|
shell_error(sh, "Usage: set_id <slave_id>");
|
||||||
return -EINVAL;
|
return -EINVAL;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -93,28 +63,57 @@ static int cmd_modbus_setid(const struct shell *sh, size_t argc, char **argv)
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
static int cmd_valve_set_open_time(const struct shell *sh, size_t argc, char **argv)
|
||||||
* @brief Shell command to show the current Modbus configuration.
|
|
||||||
*
|
|
||||||
* @param sh The shell instance.
|
|
||||||
* @param argc Argument count.
|
|
||||||
* @param argv Argument values.
|
|
||||||
* @return 0 on success.
|
|
||||||
*/
|
|
||||||
static int cmd_modbus_show(const struct shell *sh, size_t argc, char **argv)
|
|
||||||
{
|
{
|
||||||
const int label_width = 15;
|
if (argc != 2) {
|
||||||
|
shell_error(sh, "Usage: set_open_time <seconds>");
|
||||||
|
return -EINVAL;
|
||||||
|
}
|
||||||
|
|
||||||
shell_print(sh, "Modbus Settings:");
|
uint16_t seconds = (uint16_t)strtoul(argv[1], NULL, 10);
|
||||||
shell_print(sh, "%*s %u", label_width, "Baudrate:", modbus_get_baudrate());
|
valve_set_max_open_time(seconds);
|
||||||
shell_print(sh, "%*s %u", label_width, "Slave ID:", modbus_get_unit_id());
|
shell_print(sh, "Max opening time set to: %u seconds (and saved)", seconds);
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
static int cmd_valve_set_close_time(const struct shell *sh, size_t argc, char **argv)
|
||||||
|
{
|
||||||
|
if (argc != 2) {
|
||||||
|
shell_error(sh, "Usage: set_close_time <seconds>");
|
||||||
|
return -EINVAL;
|
||||||
|
}
|
||||||
|
|
||||||
|
uint16_t seconds = (uint16_t)strtoul(argv[1], NULL, 10);
|
||||||
|
valve_set_max_close_time(seconds);
|
||||||
|
shell_print(sh, "Max closing time set to: %u seconds (and saved)", seconds);
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
static int cmd_config_show(const struct shell *sh, size_t argc, char **argv)
|
||||||
|
{
|
||||||
|
shell_print(sh, "Current Modbus Configuration:");
|
||||||
|
shell_print(sh, " Baudrate: %u", modbus_get_baudrate());
|
||||||
|
shell_print(sh, " Slave ID: %u", modbus_get_unit_id());
|
||||||
|
shell_print(sh, "Current Valve Configuration:");
|
||||||
|
shell_print(sh, " Max Opening Time: %u s", valve_get_max_open_time());
|
||||||
|
shell_print(sh, " Max Closing Time: %u s", valve_get_max_close_time());
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
SHELL_STATIC_SUBCMD_SET_CREATE(sub_modbus_cmds,
|
SHELL_STATIC_SUBCMD_SET_CREATE(sub_modbus_cmds,
|
||||||
SHELL_CMD(setb, NULL, "Set Modbus baudrate", cmd_modbus_setb),
|
SHELL_CMD(set_baud, NULL, "Set Modbus baudrate", cmd_modbus_set_baud),
|
||||||
SHELL_CMD(setid, NULL, "Set Modbus slave ID", cmd_modbus_setid),
|
SHELL_CMD(set_id, NULL, "Set Modbus slave ID", cmd_modbus_set_id),
|
||||||
SHELL_CMD(show, NULL, "Show Modbus configuration", cmd_modbus_show),
|
SHELL_SUBCMD_SET_END
|
||||||
SHELL_SUBCMD_SET_END);
|
);
|
||||||
|
|
||||||
SHELL_CMD_REGISTER(modbus, &sub_modbus_cmds, "Modbus commands", NULL);
|
SHELL_STATIC_SUBCMD_SET_CREATE(sub_valve_cmds,
|
||||||
|
SHELL_CMD(set_open_time, NULL, "Set max valve opening time", cmd_valve_set_open_time),
|
||||||
|
SHELL_CMD(set_close_time, NULL, "Set max valve closing time", cmd_valve_set_close_time),
|
||||||
|
SHELL_SUBCMD_SET_END
|
||||||
|
);
|
||||||
|
|
||||||
|
SHELL_CMD_REGISTER(modbus, &sub_modbus_cmds, "Modbus configuration", NULL);
|
||||||
|
SHELL_CMD_REGISTER(valve, &sub_valve_cmds, "Valve configuration", NULL);
|
||||||
|
SHELL_CMD_REGISTER(show_config, NULL, "Show all configurations", cmd_config_show);
|
||||||
|
|
@ -1,5 +1,5 @@
|
||||||
config SHELL_SYSTEM
|
config SHELL_SYSTEM
|
||||||
bool "Enable Shell System"
|
bool "Enable Shell System"
|
||||||
default n
|
default y
|
||||||
help
|
help
|
||||||
Enable the system commands.
|
Enable the system commands.
|
||||||
|
|
@ -1,25 +1,6 @@
|
||||||
/**
|
|
||||||
* @file shell_system.c
|
|
||||||
* @brief Provides basic system-level shell commands.
|
|
||||||
*
|
|
||||||
* This file implements essential system commands for the Zephyr shell,
|
|
||||||
* such as rebooting the device.
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include <zephyr/shell/shell.h>
|
#include <zephyr/shell/shell.h>
|
||||||
#include <zephyr/sys/reboot.h>
|
#include <zephyr/sys/reboot.h>
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Shell command to reset the system.
|
|
||||||
*
|
|
||||||
* This command performs a warm reboot of the device after a short delay
|
|
||||||
* to ensure the shell message is printed.
|
|
||||||
*
|
|
||||||
* @param sh The shell instance.
|
|
||||||
* @param argc Argument count.
|
|
||||||
* @param argv Argument values.
|
|
||||||
* @return 0 on success.
|
|
||||||
*/
|
|
||||||
static int cmd_reset(const struct shell *sh, size_t argc, char **argv)
|
static int cmd_reset(const struct shell *sh, size_t argc, char **argv)
|
||||||
{
|
{
|
||||||
shell_print(sh, "Rebooting system...");
|
shell_print(sh, "Rebooting system...");
|
||||||
|
|
|
||||||
|
|
@ -1 +0,0 @@
|
||||||
zephyr_library_sources(shell_valve.c)
|
|
||||||
|
|
@ -1,7 +0,0 @@
|
||||||
config SHELL_VALVE
|
|
||||||
bool "Shell Valve commands"
|
|
||||||
default n
|
|
||||||
depends on SHELL
|
|
||||||
depends on LIB_VALVE
|
|
||||||
help
|
|
||||||
Enable the valve shell commands.
|
|
||||||
|
|
@ -1,185 +0,0 @@
|
||||||
#include <zephyr/kernel.h>
|
|
||||||
#include <zephyr/shell/shell.h>
|
|
||||||
#include <lib/valve.h>
|
|
||||||
#include <stdlib.h>
|
|
||||||
|
|
||||||
static int cmd_valve_set_open_t(const struct shell *sh, size_t argc, char **argv)
|
|
||||||
{
|
|
||||||
if (argc != 2) {
|
|
||||||
shell_print(sh, "Usage: valve set_open_t <seconds>");
|
|
||||||
return -EINVAL;
|
|
||||||
}
|
|
||||||
|
|
||||||
uint16_t seconds = (uint16_t)atoi(argv[1]);
|
|
||||||
valve_set_max_open_time(seconds);
|
|
||||||
shell_print(sh, "Max open time set to %u seconds.", seconds);
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
static int cmd_valve_set_close_t(const struct shell *sh, size_t argc, char **argv)
|
|
||||||
{
|
|
||||||
if (argc != 2) {
|
|
||||||
shell_print(sh, "Usage: valve set_close_t <seconds>");
|
|
||||||
return -EINVAL;
|
|
||||||
}
|
|
||||||
|
|
||||||
uint16_t seconds = (uint16_t)atoi(argv[1]);
|
|
||||||
valve_set_max_close_time(seconds);
|
|
||||||
shell_print(sh, "Max close time set to %u seconds.", seconds);
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
static int cmd_valve_set_end_curr_open(const struct shell *sh, size_t argc, char **argv)
|
|
||||||
{
|
|
||||||
if (argc != 2) {
|
|
||||||
shell_print(sh, "Usage: valve set_end_curr_open <milliamps>");
|
|
||||||
return -EINVAL;
|
|
||||||
}
|
|
||||||
|
|
||||||
uint16_t current_ma = (uint16_t)atoi(argv[1]);
|
|
||||||
valve_set_end_current_threshold_open(current_ma);
|
|
||||||
shell_print(sh, "End current threshold (open) set to %u mA.", current_ma);
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
static int cmd_valve_set_end_curr_close(const struct shell *sh, size_t argc, char **argv)
|
|
||||||
{
|
|
||||||
if (argc != 2) {
|
|
||||||
shell_print(sh, "Usage: valve set_end_curr_close <milliamps>");
|
|
||||||
return -EINVAL;
|
|
||||||
}
|
|
||||||
|
|
||||||
uint16_t current_ma = (uint16_t)atoi(argv[1]);
|
|
||||||
valve_set_end_current_threshold_close(current_ma);
|
|
||||||
shell_print(sh, "End current threshold (close) set to %u mA.", current_ma);
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
static int cmd_valve_set_obstacle_open(const struct shell *sh, size_t argc, char **argv)
|
|
||||||
{
|
|
||||||
if (argc != 2) {
|
|
||||||
shell_print(sh, "Usage: valve set_obstacle_open <milliamps>");
|
|
||||||
return -EINVAL;
|
|
||||||
}
|
|
||||||
|
|
||||||
uint16_t current_ma = (uint16_t)atoi(argv[1]);
|
|
||||||
valve_set_obstacle_threshold_open(current_ma);
|
|
||||||
shell_print(sh, "Obstacle threshold (open) set to %u mA.", current_ma);
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
static int cmd_valve_set_obstacle_close(const struct shell *sh, size_t argc, char **argv)
|
|
||||||
{
|
|
||||||
if (argc != 2) {
|
|
||||||
shell_print(sh, "Usage: valve set_obstacle_close <milliamps>");
|
|
||||||
return -EINVAL;
|
|
||||||
}
|
|
||||||
|
|
||||||
uint16_t current_ma = (uint16_t)atoi(argv[1]);
|
|
||||||
valve_set_obstacle_threshold_close(current_ma);
|
|
||||||
shell_print(sh, "Obstacle threshold (close) set to %u mA.", current_ma);
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
static int cmd_valve_show(const struct shell *sh, size_t argc, char **argv)
|
|
||||||
{
|
|
||||||
const int label_width = 30;
|
|
||||||
|
|
||||||
shell_print(sh, "Valve Settings:");
|
|
||||||
shell_print(sh, "%*s %u s", label_width, "Max Open Time:", valve_get_max_open_time());
|
|
||||||
shell_print(sh, "%*s %u s", label_width, "Max Close Time:", valve_get_max_close_time());
|
|
||||||
shell_print(sh,
|
|
||||||
"%*s %u mA",
|
|
||||||
label_width,
|
|
||||||
"End Current Threshold (Open):",
|
|
||||||
valve_get_end_current_threshold_open());
|
|
||||||
shell_print(sh,
|
|
||||||
"%*s %u mA",
|
|
||||||
label_width,
|
|
||||||
"End Current Threshold (Close):",
|
|
||||||
valve_get_end_current_threshold_close());
|
|
||||||
shell_print(sh,
|
|
||||||
"%*s %u mA",
|
|
||||||
label_width,
|
|
||||||
"Obstacle Threshold (Open):",
|
|
||||||
valve_get_obstacle_threshold_open());
|
|
||||||
shell_print(sh,
|
|
||||||
"%*s %u mA",
|
|
||||||
label_width,
|
|
||||||
"Obstacle Threshold (Close):",
|
|
||||||
valve_get_obstacle_threshold_close());
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
static int cmd_valve_open(const struct shell *sh, size_t argc, char **argv)
|
|
||||||
{
|
|
||||||
ARG_UNUSED(argc);
|
|
||||||
ARG_UNUSED(argv);
|
|
||||||
if (valve_get_movement() != VALVE_MOVEMENT_IDLE) {
|
|
||||||
shell_print(sh, "Valve is already moving.");
|
|
||||||
return -EBUSY;
|
|
||||||
}
|
|
||||||
|
|
||||||
valve_open();
|
|
||||||
shell_print(sh, "Valve is opening.");
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
static int cmd_valve_close(const struct shell *sh, size_t argc, char **argv)
|
|
||||||
{
|
|
||||||
ARG_UNUSED(argc);
|
|
||||||
ARG_UNUSED(argv);
|
|
||||||
if (valve_get_movement() != VALVE_MOVEMENT_IDLE) {
|
|
||||||
shell_print(sh, "Valve is already moving.");
|
|
||||||
return -EBUSY;
|
|
||||||
}
|
|
||||||
|
|
||||||
valve_close();
|
|
||||||
shell_print(sh, "Valve is closing.");
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
static int cmd_valve_stop(const struct shell *sh, size_t argc, char **argv)
|
|
||||||
{
|
|
||||||
ARG_UNUSED(argc);
|
|
||||||
ARG_UNUSED(argv);
|
|
||||||
if (valve_get_movement() == VALVE_MOVEMENT_IDLE) {
|
|
||||||
shell_print(sh, "Valve is already stopped.");
|
|
||||||
return -EINVAL;
|
|
||||||
}
|
|
||||||
|
|
||||||
valve_stop();
|
|
||||||
shell_print(sh, "Valve movement stopped.");
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
SHELL_STATIC_SUBCMD_SET_CREATE(sub_valve_settings,
|
|
||||||
SHELL_CMD(open_t, NULL, "Set max open time (seconds)", cmd_valve_set_open_t),
|
|
||||||
SHELL_CMD(close_t, NULL, "Set max close time (seconds)", cmd_valve_set_close_t),
|
|
||||||
SHELL_CMD(end_curr_open,
|
|
||||||
NULL,
|
|
||||||
"Set end current threshold for opening (mA)",
|
|
||||||
cmd_valve_set_end_curr_open),
|
|
||||||
SHELL_CMD(end_curr_close,
|
|
||||||
NULL,
|
|
||||||
"Set end current threshold for closing (mA)",
|
|
||||||
cmd_valve_set_end_curr_close),
|
|
||||||
SHELL_CMD(obstacle_curr_open,
|
|
||||||
NULL,
|
|
||||||
"Set obstacle threshold for opening (mA)",
|
|
||||||
cmd_valve_set_obstacle_open),
|
|
||||||
SHELL_CMD(obstacle_curr_close,
|
|
||||||
NULL,
|
|
||||||
"Set obstacle threshold for closing (mA)",
|
|
||||||
cmd_valve_set_obstacle_close),
|
|
||||||
|
|
||||||
SHELL_SUBCMD_SET_END);
|
|
||||||
|
|
||||||
SHELL_STATIC_SUBCMD_SET_CREATE(valve_cmds,
|
|
||||||
SHELL_CMD(show, NULL, "Show valve configuration", cmd_valve_show),
|
|
||||||
SHELL_CMD(set, &sub_valve_settings, "Valve settings commands", NULL),
|
|
||||||
SHELL_CMD(open, NULL, "Open the valve", cmd_valve_open),
|
|
||||||
SHELL_CMD(close, NULL, "Close the valve", cmd_valve_close),
|
|
||||||
SHELL_CMD(stop, NULL, "Stop the valve movement", cmd_valve_stop),
|
|
||||||
SHELL_SUBCMD_SET_END);
|
|
||||||
|
|
||||||
SHELL_CMD_REGISTER(valve, &valve_cmds, "Valve commands", NULL);
|
|
||||||
|
|
@ -1,45 +1,5 @@
|
||||||
config LIB_VALVE
|
config LIB_VALVE
|
||||||
bool "Enable Valve Library"
|
bool "Enable Valve Library"
|
||||||
default n
|
default y
|
||||||
help
|
help
|
||||||
Enable the Valve Library.
|
Enable the Valve Library.
|
||||||
|
|
||||||
if LIB_VALVE
|
|
||||||
config LOG_VALVE_LEVEL
|
|
||||||
int "Valve Log Level"
|
|
||||||
default 3
|
|
||||||
help
|
|
||||||
Set the log level for the Valve Library.
|
|
||||||
0 = None, 1 = Error, 2 = Warning, 3 = Info, 4 = Debug
|
|
||||||
|
|
||||||
config VALVE_INTERVALL_CURRENT_CHECK_MS
|
|
||||||
int "Interval Current Check (ms)"
|
|
||||||
default 100
|
|
||||||
help
|
|
||||||
Set the interval in milliseconds for checking the motor current
|
|
||||||
during valve operation. This is used to detect obstacles.
|
|
||||||
|
|
||||||
config VALVE_INITIAL_INTERVALL_CURRENT_CHECK_MS
|
|
||||||
int "Initial Current Check (ms)"
|
|
||||||
default 200
|
|
||||||
help
|
|
||||||
Set the initial delay in milliseconds before the first current check
|
|
||||||
after starting the valve operation. This allows the motor to stabilize.
|
|
||||||
|
|
||||||
config VALVE_OBSTACLE_THRESHOLD_OPEN_MA
|
|
||||||
int "Obstacle Threshold Open (mA)"
|
|
||||||
default 200
|
|
||||||
help
|
|
||||||
Set the current threshold in milliamps for obstacle detection
|
|
||||||
during valve opening. If the motor current exceeds this value,
|
|
||||||
an obstacle is detected and the valve stops.
|
|
||||||
|
|
||||||
config VALVE_OBSTACLE_THRESHOLD_CLOSE_MA
|
|
||||||
int "Obstacle Threshold Close (mA)"
|
|
||||||
default 200
|
|
||||||
help
|
|
||||||
Set the current threshold in milliamps for obstacle detection
|
|
||||||
during vaslve closing. If the motor current exceeds this value,
|
|
||||||
an obstacle is detected and the valve stops.
|
|
||||||
|
|
||||||
endif # LIB_VALVE
|
|
||||||
|
|
|
||||||
|
|
@ -1,304 +1,62 @@
|
||||||
/**
|
|
||||||
* @file valve.c
|
|
||||||
* @brief Implementation of the motorized valve control library.
|
|
||||||
*
|
|
||||||
* This file contains the logic for controlling a motorized valve using a
|
|
||||||
* VND7050AJ high-side driver. It uses a delayed work item to handle the
|
|
||||||
* safety timeouts for opening and closing operations.
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include <zephyr/device.h>
|
|
||||||
#include <zephyr/drivers/gpio.h>
|
|
||||||
#include <zephyr/kernel.h>
|
#include <zephyr/kernel.h>
|
||||||
#include <zephyr/logging/log.h>
|
|
||||||
#include <zephyr/settings/settings.h>
|
#include <zephyr/settings/settings.h>
|
||||||
|
#include <zephyr/logging/log.h>
|
||||||
#include <lib/valve.h>
|
#include <lib/valve.h>
|
||||||
#include <lib/vnd7050aj.h>
|
|
||||||
|
|
||||||
#define VND_NODE DT_ALIAS(vnd7050aj)
|
LOG_MODULE_REGISTER(valve, LOG_LEVEL_INF);
|
||||||
#if !DT_NODE_HAS_STATUS(VND_NODE, okay)
|
|
||||||
#error VND7050AJ node is not defined or enabled
|
|
||||||
#endif
|
|
||||||
const struct device *vnd7050aj_dev = DEVICE_DT_GET(VND_NODE);
|
|
||||||
|
|
||||||
LOG_MODULE_REGISTER(valve, CONFIG_LOG_VALVE_LEVEL);
|
static enum valve_state current_state = VALVE_STATE_CLOSED;
|
||||||
|
|
||||||
static enum valve_state current_state = VALVE_STATE_OPEN;
|
|
||||||
static enum valve_movement current_movement = VALVE_MOVEMENT_IDLE;
|
static enum valve_movement current_movement = VALVE_MOVEMENT_IDLE;
|
||||||
static uint16_t max_opening_time_s = 10;
|
static uint16_t max_opening_time_s = 60;
|
||||||
static uint16_t max_closing_time_s = 10;
|
static uint16_t max_closing_time_s = 60;
|
||||||
static uint16_t end_current_threshold_open_ma = 10;
|
|
||||||
static uint16_t end_current_threshold_close_ma = 10;
|
|
||||||
static uint16_t obstacle_threshold_open_ma = CONFIG_VALVE_OBSTACLE_THRESHOLD_OPEN_MA;
|
|
||||||
static uint16_t obstacle_threshold_close_ma = CONFIG_VALVE_OBSTACLE_THRESHOLD_CLOSE_MA;
|
|
||||||
static struct k_work_delayable valve_work;
|
static struct k_work_delayable valve_work;
|
||||||
static struct k_timer movement_timer;
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Work handler for end position checks of the valve.
|
|
||||||
*
|
|
||||||
* This function is called periodically to check if the valve has reached its
|
|
||||||
* end position. It reads the current load on the motor and determines if the
|
|
||||||
* valve has reached its target position.
|
|
||||||
*
|
|
||||||
* @param work Pointer to the k_work item.
|
|
||||||
*/
|
|
||||||
static void valve_work_handler(struct k_work *work)
|
static void valve_work_handler(struct k_work *work)
|
||||||
{
|
{
|
||||||
int current_ma = 0;
|
|
||||||
|
|
||||||
if (current_movement == VALVE_MOVEMENT_OPENING) {
|
if (current_movement == VALVE_MOVEMENT_OPENING) {
|
||||||
vnd7050aj_read_load_current(vnd7050aj_dev, VALVE_CHANNEL_OPEN, ¤t_ma);
|
LOG_INF("Virtual valve finished opening");
|
||||||
valve_current_open_callback(current_ma);
|
|
||||||
if (current_ma > obstacle_threshold_open_ma) {
|
|
||||||
LOG_ERR(
|
|
||||||
"Obstacle detected during opening (current: %d mA), stopping motor.",
|
|
||||||
current_ma);
|
|
||||||
current_movement = VALVE_MOVEMENT_ERROR;
|
|
||||||
valve_stop();
|
|
||||||
return;
|
|
||||||
} else if (current_ma > end_current_threshold_open_ma) {
|
|
||||||
k_work_schedule(&valve_work, VALVE_CURRENT_CHECK_INTERVAL);
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
LOG_DBG("Valve finished opening");
|
|
||||||
} else if (current_movement == VALVE_MOVEMENT_CLOSING) {
|
} else if (current_movement == VALVE_MOVEMENT_CLOSING) {
|
||||||
vnd7050aj_read_load_current(vnd7050aj_dev, VALVE_CHANNEL_CLOSE, ¤t_ma);
|
|
||||||
valve_current_close_callback(current_ma);
|
|
||||||
if (current_ma > obstacle_threshold_close_ma) {
|
|
||||||
LOG_ERR(
|
|
||||||
"Obstacle detected during closing (current: %d mA), stopping motor.",
|
|
||||||
current_ma);
|
|
||||||
current_movement = VALVE_MOVEMENT_ERROR;
|
|
||||||
valve_stop();
|
|
||||||
return;
|
|
||||||
} else if (current_ma > end_current_threshold_close_ma) {
|
|
||||||
k_work_schedule(&valve_work, VALVE_CURRENT_CHECK_INTERVAL);
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
current_state = VALVE_STATE_CLOSED;
|
current_state = VALVE_STATE_CLOSED;
|
||||||
LOG_DBG("Valve finished closing");
|
LOG_INF("Virtual valve finished closing");
|
||||||
}
|
}
|
||||||
current_movement = VALVE_MOVEMENT_IDLE;
|
current_movement = VALVE_MOVEMENT_IDLE;
|
||||||
|
|
||||||
valve_stop();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
void valve_init(void)
|
||||||
* @brief Timer handler for valve movement timeouts.
|
|
||||||
*
|
|
||||||
* This function is called when the maximum allowed time for valve movement
|
|
||||||
* (opening or closing) has been reached. It stops the valve motor, cancels
|
|
||||||
* any pending end-position checks, and sets the movement status to error.
|
|
||||||
*
|
|
||||||
* @param timer Pointer to the k_timer instance that expired.
|
|
||||||
*/
|
|
||||||
void movement_timeout_handler(struct k_timer *timer)
|
|
||||||
{
|
{
|
||||||
// Stop the end position check if the timer expires
|
|
||||||
k_work_cancel_delayable(&valve_work);
|
|
||||||
if (current_movement == VALVE_MOVEMENT_OPENING) {
|
|
||||||
LOG_WRN("Valve opening timeout reached, stopping motor.");
|
|
||||||
current_movement = VALVE_MOVEMENT_ERROR;
|
|
||||||
} else if (current_movement == VALVE_MOVEMENT_CLOSING) {
|
|
||||||
LOG_WRN("Valve closing timeout reached, stopping motor.");
|
|
||||||
current_movement = VALVE_MOVEMENT_ERROR;
|
|
||||||
}
|
|
||||||
vnd7050aj_set_output_state(vnd7050aj_dev, VALVE_CHANNEL_OPEN, false);
|
|
||||||
vnd7050aj_set_output_state(vnd7050aj_dev, VALVE_CHANNEL_CLOSE, false);
|
|
||||||
current_state = VALVE_STATE_OPEN;
|
|
||||||
current_movement = VALVE_MOVEMENT_IDLE;
|
|
||||||
}
|
|
||||||
|
|
||||||
int valve_init(void)
|
|
||||||
{
|
|
||||||
if (!device_is_ready(vnd7050aj_dev)) {
|
|
||||||
LOG_ERR("VND7050AJ device is not ready");
|
|
||||||
return -ENODEV;
|
|
||||||
}
|
|
||||||
|
|
||||||
k_work_init_delayable(&valve_work, valve_work_handler);
|
k_work_init_delayable(&valve_work, valve_work_handler);
|
||||||
k_timer_init(&movement_timer, movement_timeout_handler, NULL);
|
|
||||||
|
|
||||||
settings_load_one("valve/max_open_time", &max_opening_time_s, sizeof(max_opening_time_s));
|
settings_load_one("valve/max_open_time", &max_opening_time_s, sizeof(max_opening_time_s));
|
||||||
settings_load_one("valve/max_close_time", &max_closing_time_s, sizeof(max_closing_time_s));
|
settings_load_one("valve/max_close_time", &max_closing_time_s, sizeof(max_closing_time_s));
|
||||||
settings_load_one("valve/end_current_threshold_open",
|
|
||||||
&end_current_threshold_open_ma,
|
|
||||||
sizeof(end_current_threshold_open_ma));
|
|
||||||
settings_load_one("valve/end_current_threshold_close",
|
|
||||||
&end_current_threshold_close_ma,
|
|
||||||
sizeof(end_current_threshold_close_ma));
|
|
||||||
settings_load_one("valve/obstacle_threshold_open",
|
|
||||||
&obstacle_threshold_open_ma,
|
|
||||||
sizeof(obstacle_threshold_open_ma));
|
|
||||||
settings_load_one("valve/obstacle_threshold_close",
|
|
||||||
&obstacle_threshold_close_ma,
|
|
||||||
sizeof(obstacle_threshold_close_ma));
|
|
||||||
|
|
||||||
LOG_INF("Valve initialized: max_open=%us, max_close=%us, end_curr_open=%umA, "
|
|
||||||
"end_curr_close=%umA, obs_open=%umA, obs_close=%umA",
|
|
||||||
max_opening_time_s,
|
|
||||||
max_closing_time_s,
|
|
||||||
end_current_threshold_open_ma,
|
|
||||||
end_current_threshold_close_ma,
|
|
||||||
obstacle_threshold_open_ma,
|
|
||||||
obstacle_threshold_close_ma);
|
|
||||||
valve_close();
|
|
||||||
return 0;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void valve_open(void)
|
void valve_open(void)
|
||||||
{
|
{
|
||||||
LOG_DBG("Opening valve");
|
if (current_state == VALVE_STATE_CLOSED) {
|
||||||
vnd7050aj_reset_fault(vnd7050aj_dev);
|
current_state = VALVE_STATE_OPEN;
|
||||||
vnd7050aj_set_output_state(vnd7050aj_dev, VALVE_CHANNEL_CLOSE, false);
|
|
||||||
vnd7050aj_set_output_state(vnd7050aj_dev, VALVE_CHANNEL_OPEN, true);
|
|
||||||
current_state =
|
|
||||||
VALVE_STATE_OPEN; /* Security: assume valve open as soon as it starts opening */
|
|
||||||
current_movement = VALVE_MOVEMENT_OPENING;
|
current_movement = VALVE_MOVEMENT_OPENING;
|
||||||
if (max_opening_time_s > 0) {
|
k_work_schedule(&valve_work, K_SECONDS(max_opening_time_s));
|
||||||
k_timer_start(&movement_timer, K_SECONDS(max_opening_time_s), K_NO_WAIT);
|
|
||||||
}
|
}
|
||||||
k_work_schedule(&valve_work, VALVE_INITIAL_CURRENT_CHECK_INTERVAL);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void valve_close(void)
|
void valve_close(void)
|
||||||
{
|
{
|
||||||
LOG_DBG("Closing valve");
|
if (current_state == VALVE_STATE_OPEN) {
|
||||||
vnd7050aj_reset_fault(vnd7050aj_dev);
|
|
||||||
vnd7050aj_set_output_state(vnd7050aj_dev, VALVE_CHANNEL_OPEN, false);
|
|
||||||
vnd7050aj_set_output_state(vnd7050aj_dev, VALVE_CHANNEL_CLOSE, true);
|
|
||||||
if (max_closing_time_s > 0) {
|
|
||||||
k_timer_start(&movement_timer, K_SECONDS(max_closing_time_s), K_NO_WAIT);
|
|
||||||
}
|
|
||||||
current_movement = VALVE_MOVEMENT_CLOSING;
|
current_movement = VALVE_MOVEMENT_CLOSING;
|
||||||
k_work_schedule(&valve_work, VALVE_INITIAL_CURRENT_CHECK_INTERVAL);
|
k_work_schedule(&valve_work, K_SECONDS(max_closing_time_s));
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void valve_stop(void)
|
void valve_stop(void)
|
||||||
{
|
{
|
||||||
k_work_cancel_delayable(&valve_work);
|
k_work_cancel_delayable(&valve_work);
|
||||||
k_timer_stop(&movement_timer);
|
|
||||||
vnd7050aj_set_output_state(vnd7050aj_dev, VALVE_CHANNEL_OPEN, false);
|
|
||||||
vnd7050aj_set_output_state(vnd7050aj_dev, VALVE_CHANNEL_CLOSE, false);
|
|
||||||
current_movement = VALVE_MOVEMENT_IDLE;
|
current_movement = VALVE_MOVEMENT_IDLE;
|
||||||
}
|
}
|
||||||
|
|
||||||
enum valve_state valve_get_state(void)
|
enum valve_state valve_get_state(void) { return current_state; }
|
||||||
{
|
enum valve_movement valve_get_movement(void) { return current_movement; }
|
||||||
return current_state;
|
uint16_t valve_get_motor_current(void) { return (current_movement != VALVE_MOVEMENT_IDLE) ? 150 : 10; }
|
||||||
}
|
|
||||||
enum valve_movement valve_get_movement(void)
|
|
||||||
{
|
|
||||||
return current_movement;
|
|
||||||
}
|
|
||||||
|
|
||||||
void valve_set_max_open_time(uint16_t seconds)
|
void valve_set_max_open_time(uint16_t seconds) { max_opening_time_s = seconds; settings_save_one("valve/max_open_time", &max_opening_time_s, sizeof(max_opening_time_s)); }
|
||||||
{
|
void valve_set_max_close_time(uint16_t seconds) { max_closing_time_s = seconds; settings_save_one("valve/max_close_time", &max_closing_time_s, sizeof(max_closing_time_s)); }
|
||||||
max_opening_time_s = seconds;
|
uint16_t valve_get_max_open_time(void) { return max_opening_time_s; }
|
||||||
settings_save_one("valve/max_open_time", &max_opening_time_s, sizeof(max_opening_time_s));
|
uint16_t valve_get_max_close_time(void) { return max_closing_time_s; }
|
||||||
}
|
|
||||||
void valve_set_max_close_time(uint16_t seconds)
|
|
||||||
{
|
|
||||||
max_closing_time_s = seconds;
|
|
||||||
settings_save_one("valve/max_close_time", &max_closing_time_s, sizeof(max_closing_time_s));
|
|
||||||
}
|
|
||||||
|
|
||||||
void valve_set_end_current_threshold_open(uint16_t current_ma)
|
|
||||||
{
|
|
||||||
end_current_threshold_open_ma = current_ma;
|
|
||||||
settings_save_one("valve/end_current_threshold_open",
|
|
||||||
&end_current_threshold_open_ma,
|
|
||||||
sizeof(end_current_threshold_open_ma));
|
|
||||||
}
|
|
||||||
|
|
||||||
void valve_set_end_current_threshold_close(uint16_t current_ma)
|
|
||||||
{
|
|
||||||
end_current_threshold_close_ma = current_ma;
|
|
||||||
settings_save_one("valve/end_current_threshold_close",
|
|
||||||
&end_current_threshold_close_ma,
|
|
||||||
sizeof(end_current_threshold_close_ma));
|
|
||||||
}
|
|
||||||
|
|
||||||
uint16_t valve_get_max_open_time(void)
|
|
||||||
{
|
|
||||||
return max_opening_time_s;
|
|
||||||
}
|
|
||||||
uint16_t valve_get_max_close_time(void)
|
|
||||||
{
|
|
||||||
return max_closing_time_s;
|
|
||||||
}
|
|
||||||
|
|
||||||
uint16_t valve_get_end_current_threshold_open(void)
|
|
||||||
{
|
|
||||||
return end_current_threshold_open_ma;
|
|
||||||
}
|
|
||||||
|
|
||||||
uint16_t valve_get_end_current_threshold_close(void)
|
|
||||||
{
|
|
||||||
return end_current_threshold_close_ma;
|
|
||||||
}
|
|
||||||
|
|
||||||
int32_t valve_get_opening_current(void)
|
|
||||||
{
|
|
||||||
int32_t current;
|
|
||||||
vnd7050aj_read_load_current(vnd7050aj_dev, VALVE_CHANNEL_OPEN, ¤t);
|
|
||||||
return current;
|
|
||||||
}
|
|
||||||
|
|
||||||
int32_t valve_get_closing_current(void)
|
|
||||||
{
|
|
||||||
int32_t current;
|
|
||||||
vnd7050aj_read_load_current(vnd7050aj_dev, VALVE_CHANNEL_CLOSE, ¤t);
|
|
||||||
return current;
|
|
||||||
}
|
|
||||||
|
|
||||||
int32_t valve_get_vnd_temp(void)
|
|
||||||
{
|
|
||||||
int32_t temp_c;
|
|
||||||
vnd7050aj_read_chip_temp(vnd7050aj_dev, &temp_c);
|
|
||||||
return temp_c;
|
|
||||||
}
|
|
||||||
|
|
||||||
int32_t valve_get_vnd_voltage(void)
|
|
||||||
{
|
|
||||||
int32_t voltage_mv;
|
|
||||||
vnd7050aj_read_supply_voltage(vnd7050aj_dev, &voltage_mv);
|
|
||||||
return voltage_mv;
|
|
||||||
}
|
|
||||||
|
|
||||||
void valve_set_obstacle_threshold_open(uint16_t current_ma)
|
|
||||||
{
|
|
||||||
obstacle_threshold_open_ma = current_ma;
|
|
||||||
settings_save_one("valve/obstacle_threshold_open",
|
|
||||||
&obstacle_threshold_open_ma,
|
|
||||||
sizeof(obstacle_threshold_open_ma));
|
|
||||||
}
|
|
||||||
|
|
||||||
void valve_set_obstacle_threshold_close(uint16_t current_ma)
|
|
||||||
{
|
|
||||||
obstacle_threshold_close_ma = current_ma;
|
|
||||||
settings_save_one("valve/obstacle_threshold_close",
|
|
||||||
&obstacle_threshold_close_ma,
|
|
||||||
sizeof(obstacle_threshold_close_ma));
|
|
||||||
}
|
|
||||||
|
|
||||||
uint16_t valve_get_obstacle_threshold_open(void)
|
|
||||||
{
|
|
||||||
return obstacle_threshold_open_ma;
|
|
||||||
}
|
|
||||||
|
|
||||||
uint16_t valve_get_obstacle_threshold_close(void)
|
|
||||||
{
|
|
||||||
return obstacle_threshold_close_ma;
|
|
||||||
}
|
|
||||||
|
|
||||||
__weak void valve_current_open_callback(int current_ma)
|
|
||||||
{
|
|
||||||
LOG_DBG("Open current callback: %d mA", current_ma);
|
|
||||||
}
|
|
||||||
|
|
||||||
__weak void valve_current_close_callback(int current_ma)
|
|
||||||
{
|
|
||||||
LOG_DBG("Close current callback: %d mA", current_ma);
|
|
||||||
}
|
|
||||||
|
|
|
||||||
|
|
@ -1 +0,0 @@
|
||||||
zephyr_library_sources(vnd7050aj.c)
|
|
||||||
|
|
@ -1,33 +0,0 @@
|
||||||
# SPDX-License-Identifier: Apache-2.0
|
|
||||||
|
|
||||||
config VND7050AJ
|
|
||||||
bool "VND7050AJ driver"
|
|
||||||
default n
|
|
||||||
select ADC
|
|
||||||
select GPIO
|
|
||||||
help
|
|
||||||
Enable support for the VND7050AJ high-side driver.
|
|
||||||
|
|
||||||
if VND7050AJ
|
|
||||||
config VND7050AJ_INIT_PRIORITY
|
|
||||||
int "VND7050AJ initialization priority"
|
|
||||||
default 80
|
|
||||||
help
|
|
||||||
VND7050AJ driver initialization priority. This should be set to a value
|
|
||||||
that ensures the driver is initialized after the required subsystems
|
|
||||||
(like GPIO, ADC) but before application code runs.
|
|
||||||
|
|
||||||
config VND7050AJ_LOG_LEVEL
|
|
||||||
int "VND7050AJ Log level"
|
|
||||||
depends on LOG
|
|
||||||
default 3
|
|
||||||
range 0 4
|
|
||||||
help
|
|
||||||
Sets log level for VND7050AJ driver.
|
|
||||||
Levels are:
|
|
||||||
0 OFF, do not write
|
|
||||||
1 ERROR, only write LOG_ERR
|
|
||||||
2 WARNING, write LOG_WRN in addition to previous level
|
|
||||||
3 INFO, write LOG_INF in addition to previous levels
|
|
||||||
4 DEBUG, write LOG_DBG in addition to previous levels
|
|
||||||
endif # VND7050AJ
|
|
||||||
|
|
@ -1,333 +0,0 @@
|
||||||
/*
|
|
||||||
* Copyright (c) 2025, Eduard Iten
|
|
||||||
* SPDX-License-Identifier: Apache-2.0
|
|
||||||
*/
|
|
||||||
|
|
||||||
#define DT_DRV_COMPAT st_vnd7050aj
|
|
||||||
|
|
||||||
#include <zephyr/device.h>
|
|
||||||
#include <zephyr/drivers/adc.h>
|
|
||||||
#include <zephyr/drivers/gpio.h>
|
|
||||||
#include <zephyr/kernel.h>
|
|
||||||
#include <zephyr/logging/log.h>
|
|
||||||
#include <zephyr/sys/util.h>
|
|
||||||
|
|
||||||
#include <lib/vnd7050aj.h>
|
|
||||||
|
|
||||||
LOG_MODULE_REGISTER(VND7050AJ, CONFIG_VND7050AJ_LOG_LEVEL);
|
|
||||||
|
|
||||||
/* Diagnostic selection modes */
|
|
||||||
enum vnd7050aj_diag_mode {
|
|
||||||
DIAG_CURRENT_CH0,
|
|
||||||
DIAG_CURRENT_CH1,
|
|
||||||
DIAG_VCC,
|
|
||||||
DIAG_TEMP,
|
|
||||||
};
|
|
||||||
|
|
||||||
struct vnd7050aj_config {
|
|
||||||
struct gpio_dt_spec input0_gpio;
|
|
||||||
struct gpio_dt_spec input1_gpio;
|
|
||||||
struct gpio_dt_spec sel0_gpio;
|
|
||||||
struct gpio_dt_spec sel1_gpio;
|
|
||||||
struct gpio_dt_spec sen_gpio;
|
|
||||||
struct gpio_dt_spec fault_reset_gpio;
|
|
||||||
struct adc_dt_spec io_channels;
|
|
||||||
uint32_t r_sense_ohms;
|
|
||||||
uint32_t k_factor; /* Current sense ratio */
|
|
||||||
uint32_t k_vcc; /* VCC sense ratio * 1000 */
|
|
||||||
int32_t t_sense_0; /* Temp sense reference temperature in °C */
|
|
||||||
uint32_t v_sense_0; /* Temp sense reference voltage in mV */
|
|
||||||
uint32_t k_tchip; /* Temp sense gain in °C/mV * 1000 */
|
|
||||||
};
|
|
||||||
|
|
||||||
struct vnd7050aj_data {
|
|
||||||
struct k_mutex lock;
|
|
||||||
};
|
|
||||||
|
|
||||||
static int vnd7050aj_init(const struct device *dev)
|
|
||||||
{
|
|
||||||
const struct vnd7050aj_config *config = dev->config;
|
|
||||||
struct vnd7050aj_data *data = dev->data;
|
|
||||||
int err;
|
|
||||||
|
|
||||||
k_mutex_init(&data->lock);
|
|
||||||
|
|
||||||
LOG_DBG("Initializing VND7050AJ device %s", dev->name);
|
|
||||||
|
|
||||||
/* --- Check if all required devices are ready --- */
|
|
||||||
if (!gpio_is_ready_dt(&config->input0_gpio)) {
|
|
||||||
LOG_ERR("Input0 GPIO port is not ready");
|
|
||||||
return -ENODEV;
|
|
||||||
}
|
|
||||||
if (!gpio_is_ready_dt(&config->input1_gpio)) {
|
|
||||||
LOG_ERR("Input1 GPIO port is not ready");
|
|
||||||
return -ENODEV;
|
|
||||||
}
|
|
||||||
if (!gpio_is_ready_dt(&config->sel0_gpio)) {
|
|
||||||
LOG_ERR("Select0 GPIO port is not ready");
|
|
||||||
return -ENODEV;
|
|
||||||
}
|
|
||||||
if (!gpio_is_ready_dt(&config->sel1_gpio)) {
|
|
||||||
LOG_ERR("Select1 GPIO port is not ready");
|
|
||||||
return -ENODEV;
|
|
||||||
}
|
|
||||||
if (!gpio_is_ready_dt(&config->sen_gpio)) {
|
|
||||||
LOG_ERR("Sense GPIO port is not ready");
|
|
||||||
return -ENODEV;
|
|
||||||
}
|
|
||||||
if (!gpio_is_ready_dt(&config->fault_reset_gpio)) {
|
|
||||||
LOG_ERR("Fault Reset GPIO port is not ready");
|
|
||||||
return -ENODEV;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (!adc_is_ready_dt(&config->io_channels)) {
|
|
||||||
LOG_ERR("ADC controller not ready");
|
|
||||||
return -ENODEV;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* --- Configure GPIOs to their initial states --- */
|
|
||||||
err = gpio_pin_configure_dt(&config->input0_gpio, GPIO_OUTPUT_INACTIVE);
|
|
||||||
if (err) {
|
|
||||||
LOG_ERR("Failed to configure input0 GPIO: %d", err);
|
|
||||||
return err;
|
|
||||||
}
|
|
||||||
|
|
||||||
err = gpio_pin_configure_dt(&config->input1_gpio, GPIO_OUTPUT_INACTIVE);
|
|
||||||
if (err) {
|
|
||||||
LOG_ERR("Failed to configure input1 GPIO: %d", err);
|
|
||||||
return err;
|
|
||||||
}
|
|
||||||
err = gpio_pin_configure_dt(&config->sel0_gpio, GPIO_OUTPUT_INACTIVE);
|
|
||||||
if (err) {
|
|
||||||
LOG_ERR("Failed to configure select0 GPIO: %d", err);
|
|
||||||
return err;
|
|
||||||
}
|
|
||||||
err = gpio_pin_configure_dt(&config->sel1_gpio, GPIO_OUTPUT_INACTIVE);
|
|
||||||
if (err) {
|
|
||||||
LOG_ERR("Failed to configure select1 GPIO: %d", err);
|
|
||||||
return err;
|
|
||||||
}
|
|
||||||
err = gpio_pin_configure_dt(&config->sen_gpio, GPIO_OUTPUT_INACTIVE);
|
|
||||||
if (err) {
|
|
||||||
LOG_ERR("Failed to configure sense GPIO: %d", err);
|
|
||||||
return err;
|
|
||||||
}
|
|
||||||
err = gpio_pin_configure_dt(
|
|
||||||
&config->fault_reset_gpio, GPIO_OUTPUT_ACTIVE); /* Active-low, so init high */
|
|
||||||
if (err) {
|
|
||||||
LOG_ERR("Failed to configure fault reset GPIO: %d", err);
|
|
||||||
return err;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* --- Configure the ADC channel --- */
|
|
||||||
err = adc_channel_setup_dt(&config->io_channels);
|
|
||||||
if (err) {
|
|
||||||
LOG_ERR("Failed to setup ADC channel: %d", err);
|
|
||||||
return err;
|
|
||||||
}
|
|
||||||
|
|
||||||
LOG_DBG("Device %s initialized", dev->name);
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
#define VND7050AJ_DEFINE(inst) \
|
|
||||||
static struct vnd7050aj_data vnd7050aj_data_##inst; \
|
|
||||||
\
|
|
||||||
static const struct vnd7050aj_config vnd7050aj_config_##inst = { \
|
|
||||||
.input0_gpio = GPIO_DT_SPEC_INST_GET(inst, input0_gpios), \
|
|
||||||
.input1_gpio = GPIO_DT_SPEC_INST_GET(inst, input1_gpios), \
|
|
||||||
.sel0_gpio = GPIO_DT_SPEC_INST_GET(inst, select0_gpios), \
|
|
||||||
.sel1_gpio = GPIO_DT_SPEC_INST_GET(inst, select1_gpios), \
|
|
||||||
.sen_gpio = GPIO_DT_SPEC_INST_GET(inst, sense_enable_gpios), \
|
|
||||||
.fault_reset_gpio = GPIO_DT_SPEC_INST_GET(inst, fault_reset_gpios), \
|
|
||||||
.io_channels = ADC_DT_SPEC_INST_GET(inst), \
|
|
||||||
.r_sense_ohms = DT_INST_PROP(inst, r_sense_ohms), \
|
|
||||||
.k_factor = DT_INST_PROP(inst, k_factor), \
|
|
||||||
.k_vcc = DT_INST_PROP(inst, k_vcc), \
|
|
||||||
.t_sense_0 = DT_INST_PROP(inst, t_sense_0), \
|
|
||||||
.v_sense_0 = DT_INST_PROP(inst, v_sense_0), \
|
|
||||||
.k_tchip = DT_INST_PROP(inst, k_tchip), \
|
|
||||||
}; \
|
|
||||||
\
|
|
||||||
DEVICE_DT_INST_DEFINE(inst, \
|
|
||||||
vnd7050aj_init, \
|
|
||||||
NULL, /* No PM support yet */ \
|
|
||||||
&vnd7050aj_data_##inst, \
|
|
||||||
&vnd7050aj_config_##inst, \
|
|
||||||
POST_KERNEL, \
|
|
||||||
CONFIG_VND7050AJ_INIT_PRIORITY, \
|
|
||||||
NULL); /* No API struct needed for custom API */
|
|
||||||
|
|
||||||
DT_INST_FOREACH_STATUS_OKAY(VND7050AJ_DEFINE)
|
|
||||||
|
|
||||||
int vnd7050aj_set_output_state(const struct device *dev, uint8_t channel, bool state)
|
|
||||||
{
|
|
||||||
const struct vnd7050aj_config *config = dev->config;
|
|
||||||
|
|
||||||
if (channel != VND7050AJ_CHANNEL_0 && channel != VND7050AJ_CHANNEL_1) {
|
|
||||||
return -EINVAL;
|
|
||||||
}
|
|
||||||
|
|
||||||
const struct gpio_dt_spec *gpio =
|
|
||||||
(channel == VND7050AJ_CHANNEL_0) ? &config->input0_gpio : &config->input1_gpio;
|
|
||||||
|
|
||||||
return gpio_pin_set_dt(gpio, (int)state);
|
|
||||||
}
|
|
||||||
|
|
||||||
static int vnd7050aj_read_sense_voltage(
|
|
||||||
const struct device *dev, enum vnd7050aj_diag_mode mode, int32_t *voltage_mv)
|
|
||||||
{
|
|
||||||
const struct vnd7050aj_config *config = dev->config;
|
|
||||||
struct vnd7050aj_data *data = dev->data;
|
|
||||||
int err = 0;
|
|
||||||
|
|
||||||
/* Initialize the buffer to zero */
|
|
||||||
*voltage_mv = 0;
|
|
||||||
|
|
||||||
struct adc_sequence sequence = {
|
|
||||||
.buffer = voltage_mv,
|
|
||||||
.buffer_size = sizeof(*voltage_mv),
|
|
||||||
#ifdef CONFIG_ADC_CALIBRATION
|
|
||||||
.calibrate = true,
|
|
||||||
#endif
|
|
||||||
};
|
|
||||||
adc_sequence_init_dt(&config->io_channels, &sequence);
|
|
||||||
|
|
||||||
k_mutex_lock(&data->lock, K_FOREVER);
|
|
||||||
|
|
||||||
/* Step 1: Select diagnostic mode */
|
|
||||||
switch (mode) {
|
|
||||||
case DIAG_CURRENT_CH0:
|
|
||||||
gpio_pin_set_dt(&config->sel0_gpio, 0);
|
|
||||||
gpio_pin_set_dt(&config->sel1_gpio, 0);
|
|
||||||
gpio_pin_set_dt(&config->sen_gpio, 1);
|
|
||||||
break;
|
|
||||||
case DIAG_CURRENT_CH1:
|
|
||||||
gpio_pin_set_dt(&config->sel0_gpio, 0);
|
|
||||||
gpio_pin_set_dt(&config->sel1_gpio, 1);
|
|
||||||
gpio_pin_set_dt(&config->sen_gpio, 1);
|
|
||||||
break;
|
|
||||||
case DIAG_TEMP:
|
|
||||||
gpio_pin_set_dt(&config->sel0_gpio, 1);
|
|
||||||
gpio_pin_set_dt(&config->sel1_gpio, 0);
|
|
||||||
gpio_pin_set_dt(&config->sen_gpio, 1);
|
|
||||||
break;
|
|
||||||
case DIAG_VCC:
|
|
||||||
gpio_pin_set_dt(&config->sel1_gpio, 1);
|
|
||||||
gpio_pin_set_dt(&config->sel0_gpio, 1);
|
|
||||||
gpio_pin_set_dt(&config->sen_gpio, 1);
|
|
||||||
break;
|
|
||||||
default:
|
|
||||||
err = -ENOTSUP;
|
|
||||||
goto cleanup;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* Allow time for GPIO changes to settle and ADC input to stabilize */
|
|
||||||
k_msleep(1);
|
|
||||||
|
|
||||||
/* Initialize buffer before read */
|
|
||||||
*voltage_mv = 0;
|
|
||||||
err = adc_read_dt(&config->io_channels, &sequence);
|
|
||||||
if (err) {
|
|
||||||
LOG_ERR("ADC read failed: %d", err);
|
|
||||||
goto cleanup;
|
|
||||||
}
|
|
||||||
|
|
||||||
LOG_DBG("ADC read completed, raw value: %d", *voltage_mv);
|
|
||||||
err = adc_raw_to_millivolts_dt(&config->io_channels, voltage_mv);
|
|
||||||
if (err) {
|
|
||||||
LOG_ERR("ADC raw to millivolts conversion failed: %d", err);
|
|
||||||
goto cleanup;
|
|
||||||
}
|
|
||||||
LOG_DBG("ADC Reading (without processing) %dmV", *voltage_mv);
|
|
||||||
|
|
||||||
cleanup:
|
|
||||||
/* Deactivate sense enable to save power */
|
|
||||||
gpio_pin_set_dt(&config->sen_gpio, 0);
|
|
||||||
k_mutex_unlock(&data->lock);
|
|
||||||
return err;
|
|
||||||
}
|
|
||||||
|
|
||||||
int vnd7050aj_read_load_current(const struct device *dev, uint8_t channel, int32_t *current_ma)
|
|
||||||
{
|
|
||||||
const struct vnd7050aj_config *config = dev->config;
|
|
||||||
int32_t sense_mv;
|
|
||||||
int err;
|
|
||||||
|
|
||||||
if (channel != VND7050AJ_CHANNEL_0 && channel != VND7050AJ_CHANNEL_1) {
|
|
||||||
return -EINVAL;
|
|
||||||
}
|
|
||||||
|
|
||||||
enum vnd7050aj_diag_mode mode =
|
|
||||||
(channel == VND7050AJ_CHANNEL_0) ? DIAG_CURRENT_CH0 : DIAG_CURRENT_CH1;
|
|
||||||
|
|
||||||
err = vnd7050aj_read_sense_voltage(dev, mode, &sense_mv);
|
|
||||||
if (err) {
|
|
||||||
return err;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* Formula according to datasheet: I_OUT = (V_SENSE / R_SENSE) * K_IL */
|
|
||||||
/* To avoid floating point, we calculate in microamps and then convert to milliamps */
|
|
||||||
int64_t current_ua = ((int64_t)sense_mv * 1000 * config->k_factor) / config->r_sense_ohms;
|
|
||||||
*current_ma = (int32_t)(current_ua / 1000);
|
|
||||||
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
int vnd7050aj_read_chip_temp(const struct device *dev, int32_t *temp_c)
|
|
||||||
{
|
|
||||||
const struct vnd7050aj_config *config = dev->config;
|
|
||||||
int32_t sense_mv;
|
|
||||||
int err;
|
|
||||||
|
|
||||||
err = vnd7050aj_read_sense_voltage(dev, DIAG_TEMP, &sense_mv);
|
|
||||||
if (err) {
|
|
||||||
return err;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* Calculate temperature difference in kelvin first to avoid overflow */
|
|
||||||
int32_t voltage_diff = sense_mv - (int32_t)config->v_sense_0;
|
|
||||||
int32_t temp_diff_kelvin = (voltage_diff * 1000) / (int32_t)config->k_tchip;
|
|
||||||
|
|
||||||
*temp_c = config->t_sense_0 + temp_diff_kelvin;
|
|
||||||
|
|
||||||
LOG_DBG("Voltage diff: %d mV, Temp diff: %d milli°C, Final temp: %d °C",
|
|
||||||
voltage_diff,
|
|
||||||
temp_diff_kelvin,
|
|
||||||
*temp_c);
|
|
||||||
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
int vnd7050aj_read_supply_voltage(const struct device *dev, int32_t *voltage_mv)
|
|
||||||
{
|
|
||||||
const struct vnd7050aj_config *config = dev->config;
|
|
||||||
int32_t sense_mv;
|
|
||||||
int err;
|
|
||||||
|
|
||||||
err = vnd7050aj_read_sense_voltage(dev, DIAG_VCC, &sense_mv);
|
|
||||||
if (err) {
|
|
||||||
return err;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* Formula from datasheet: VCC = V_SENSE * K_VCC */
|
|
||||||
*voltage_mv = (sense_mv * config->k_vcc) / 1000;
|
|
||||||
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
int vnd7050aj_reset_fault(const struct device *dev)
|
|
||||||
{
|
|
||||||
const struct vnd7050aj_config *config = dev->config;
|
|
||||||
int err;
|
|
||||||
|
|
||||||
/* Pulse the active-low fault reset pin */
|
|
||||||
err = gpio_pin_set_dt(&config->fault_reset_gpio, 0);
|
|
||||||
if (err) {
|
|
||||||
return err;
|
|
||||||
}
|
|
||||||
k_msleep(1); /* Short pulse */
|
|
||||||
err = gpio_pin_set_dt(&config->fault_reset_gpio, 1);
|
|
||||||
|
|
||||||
return err;
|
|
||||||
}
|
|
||||||
|
|
@ -9,9 +9,9 @@ from pymodbus.client import ModbusSerialClient
|
||||||
from pymodbus.exceptions import ModbusException
|
from pymodbus.exceptions import ModbusException
|
||||||
|
|
||||||
# --- Register Definitions ---
|
# --- Register Definitions ---
|
||||||
|
# (omitted for brevity, no changes here)
|
||||||
REG_INPUT_VALVE_STATE_MOVEMENT = 0x0000
|
REG_INPUT_VALVE_STATE_MOVEMENT = 0x0000
|
||||||
REG_INPUT_MOTOR_OPEN_CURRENT_MA = 0x0001
|
REG_INPUT_MOTOR_CURRENT_MA = 0x0001
|
||||||
REG_INPUT_MOTOR_CLOSE_CURRENT_MA = 0x0002
|
|
||||||
REG_INPUT_DIGITAL_INPUTS_STATE = 0x0020
|
REG_INPUT_DIGITAL_INPUTS_STATE = 0x0020
|
||||||
REG_INPUT_BUTTON_EVENTS = 0x0021
|
REG_INPUT_BUTTON_EVENTS = 0x0021
|
||||||
REG_INPUT_FIRMWARE_VERSION_MAJOR_MINOR = 0x00F0
|
REG_INPUT_FIRMWARE_VERSION_MAJOR_MINOR = 0x00F0
|
||||||
|
|
@ -19,15 +19,10 @@ REG_INPUT_FIRMWARE_VERSION_PATCH = 0x00F1
|
||||||
REG_INPUT_DEVICE_STATUS = 0x00F2
|
REG_INPUT_DEVICE_STATUS = 0x00F2
|
||||||
REG_INPUT_UPTIME_SECONDS_LOW = 0x00F3
|
REG_INPUT_UPTIME_SECONDS_LOW = 0x00F3
|
||||||
REG_INPUT_UPTIME_SECONDS_HIGH = 0x00F4
|
REG_INPUT_UPTIME_SECONDS_HIGH = 0x00F4
|
||||||
REG_INPUT_SUPPLY_VOLTAGE_MV = 0x00F5
|
|
||||||
REG_INPUT_FWU_LAST_CHUNK_CRC = 0x0100
|
REG_INPUT_FWU_LAST_CHUNK_CRC = 0x0100
|
||||||
REG_HOLDING_VALVE_COMMAND = 0x0000
|
REG_HOLDING_VALVE_COMMAND = 0x0000
|
||||||
REG_HOLDING_MAX_OPENING_TIME_S = 0x0001
|
REG_HOLDING_MAX_OPENING_TIME_S = 0x0001
|
||||||
REG_HOLDING_MAX_CLOSING_TIME_S = 0x0002
|
REG_HOLDING_MAX_CLOSING_TIME_S = 0x0002
|
||||||
REG_HOLDING_END_CURRENT_THRESHOLD_OPEN_MA = 0x0003
|
|
||||||
REG_HOLDING_END_CURRENT_THRESHOLD_CLOSE_MA = 0x0004
|
|
||||||
REG_HOLDING_OBSTACLE_THRESHOLD_OPEN_MA = 0x0005
|
|
||||||
REG_HOLDING_OBSTACLE_THRESHOLD_CLOSE_MA = 0x0006
|
|
||||||
REG_HOLDING_DIGITAL_OUTPUTS_STATE = 0x0010
|
REG_HOLDING_DIGITAL_OUTPUTS_STATE = 0x0010
|
||||||
REG_HOLDING_WATCHDOG_TIMEOUT_S = 0x00F0
|
REG_HOLDING_WATCHDOG_TIMEOUT_S = 0x00F0
|
||||||
REG_HOLDING_DEVICE_RESET = 0x00F1
|
REG_HOLDING_DEVICE_RESET = 0x00F1
|
||||||
|
|
@ -80,24 +75,21 @@ def poll_status(slave_id, interval):
|
||||||
# Attempt to connect
|
# Attempt to connect
|
||||||
if client.connect():
|
if client.connect():
|
||||||
reconnect_attempts = 0
|
reconnect_attempts = 0
|
||||||
with status_lock:
|
new_data["error"] = None # Clear error on successful reconnect
|
||||||
status_data["error"] = None # Clear error in status_data immediately
|
|
||||||
time.sleep(0.1) # Allow UI to refresh with cleared error
|
|
||||||
else:
|
else:
|
||||||
new_data["error"] = f"Connection lost. Attempting to reconnect ({reconnect_attempts}/{max_reconnect_attempts})..."
|
new_data["error"] = f"Connection lost. Attempting to reconnect ({reconnect_attempts}/{max_reconnect_attempts})..."
|
||||||
time.sleep(reconnect_delay)
|
time.sleep(reconnect_delay)
|
||||||
continue
|
continue
|
||||||
|
|
||||||
# If connected, try to read data
|
# If connected, try to read data
|
||||||
ir_valve = client.read_input_registers(REG_INPUT_VALVE_STATE_MOVEMENT, count=1, slave=slave_id)
|
ir_valve = client.read_input_registers(REG_INPUT_VALVE_STATE_MOVEMENT, count=2, slave=slave_id)
|
||||||
ir_current = client.read_input_registers(REG_INPUT_MOTOR_OPEN_CURRENT_MA, count=2, slave=slave_id)
|
|
||||||
ir_dig = client.read_input_registers(REG_INPUT_DIGITAL_INPUTS_STATE, count=2, slave=slave_id)
|
ir_dig = client.read_input_registers(REG_INPUT_DIGITAL_INPUTS_STATE, count=2, slave=slave_id)
|
||||||
ir_sys = client.read_input_registers(REG_INPUT_FIRMWARE_VERSION_MAJOR_MINOR, count=6, slave=slave_id)
|
ir_sys = client.read_input_registers(REG_INPUT_FIRMWARE_VERSION_MAJOR_MINOR, count=5, slave=slave_id)
|
||||||
hr_valve = client.read_holding_registers(REG_HOLDING_MAX_OPENING_TIME_S, count=6, slave=slave_id)
|
hr_valve = client.read_holding_registers(REG_HOLDING_MAX_OPENING_TIME_S, count=2, slave=slave_id)
|
||||||
hr_dig = client.read_holding_registers(REG_HOLDING_DIGITAL_OUTPUTS_STATE, count=1, slave=slave_id)
|
hr_dig = client.read_holding_registers(REG_HOLDING_DIGITAL_OUTPUTS_STATE, count=1, slave=slave_id)
|
||||||
hr_sys = client.read_holding_registers(REG_HOLDING_WATCHDOG_TIMEOUT_S, count=1, slave=slave_id)
|
hr_sys = client.read_holding_registers(REG_HOLDING_WATCHDOG_TIMEOUT_S, count=1, slave=slave_id)
|
||||||
|
|
||||||
for res in [ir_valve, ir_current, ir_dig, ir_sys, hr_valve, hr_dig, hr_sys]:
|
for res in [ir_valve, ir_dig, ir_sys, hr_valve, hr_dig, hr_sys]:
|
||||||
if res.isError():
|
if res.isError():
|
||||||
raise ModbusException(str(res))
|
raise ModbusException(str(res))
|
||||||
|
|
||||||
|
|
@ -106,14 +98,9 @@ def poll_status(slave_id, interval):
|
||||||
state_map = {0: "Closed", 1: "Open"}
|
state_map = {0: "Closed", 1: "Open"}
|
||||||
new_data["movement"] = movement_map.get(valve_state_raw >> 8, 'Unknown')
|
new_data["movement"] = movement_map.get(valve_state_raw >> 8, 'Unknown')
|
||||||
new_data["state"] = state_map.get(valve_state_raw & 0xFF, 'Unknown')
|
new_data["state"] = state_map.get(valve_state_raw & 0xFF, 'Unknown')
|
||||||
new_data["motor_current_open"] = f"{ir_current.registers[0]} mA"
|
new_data["motor_current"] = f"{ir_valve.registers[1]} mA"
|
||||||
new_data["motor_current_close"] = f"{ir_current.registers[1]} mA"
|
|
||||||
new_data["open_time"] = f"{hr_valve.registers[0]}s"
|
new_data["open_time"] = f"{hr_valve.registers[0]}s"
|
||||||
new_data["close_time"] = f"{hr_valve.registers[1]}s"
|
new_data["close_time"] = f"{hr_valve.registers[1]}s"
|
||||||
new_data["end_curr_open"] = f"{hr_valve.registers[2]}mA"
|
|
||||||
new_data["end_curr_close"] = f"{hr_valve.registers[3]}mA"
|
|
||||||
new_data["obstacle_open"] = f"{hr_valve.registers[4]}mA"
|
|
||||||
new_data["obstacle_close"] = f"{hr_valve.registers[5]}mA"
|
|
||||||
new_data["digital_inputs"] = f"0x{ir_dig.registers[0]:04X}"
|
new_data["digital_inputs"] = f"0x{ir_dig.registers[0]:04X}"
|
||||||
new_data["button_events"] = f"0x{ir_dig.registers[1]:04X}"
|
new_data["button_events"] = f"0x{ir_dig.registers[1]:04X}"
|
||||||
new_data["digital_outputs"] = f"0x{hr_dig.registers[0]:04X}"
|
new_data["digital_outputs"] = f"0x{hr_dig.registers[0]:04X}"
|
||||||
|
|
@ -122,11 +109,9 @@ def poll_status(slave_id, interval):
|
||||||
fw_minor = ir_sys.registers[0] & 0xFF
|
fw_minor = ir_sys.registers[0] & 0xFF
|
||||||
fw_patch = ir_sys.registers[1]
|
fw_patch = ir_sys.registers[1]
|
||||||
uptime_seconds = (ir_sys.registers[4] << 16) | ir_sys.registers[3]
|
uptime_seconds = (ir_sys.registers[4] << 16) | ir_sys.registers[3]
|
||||||
supply_voltage_mv = ir_sys.registers[5]
|
|
||||||
new_data["firmware"] = f"v{fw_major}.{fw_minor}.{fw_patch}"
|
new_data["firmware"] = f"v{fw_major}.{fw_minor}.{fw_patch}"
|
||||||
new_data["device_status"] = "OK" if ir_sys.registers[2] == 0 else "ERROR"
|
new_data["device_status"] = "OK" if ir_sys.registers[2] == 0 else "ERROR"
|
||||||
new_data["uptime"] = format_uptime(uptime_seconds)
|
new_data["uptime"] = format_uptime(uptime_seconds)
|
||||||
new_data["supply_voltage"] = f"{supply_voltage_mv / 1000.0:.2f} V"
|
|
||||||
new_data["watchdog"] = f"{hr_sys.registers[0]}s"
|
new_data["watchdog"] = f"{hr_sys.registers[0]}s"
|
||||||
new_data["error"] = None # Clear any previous error on successful read
|
new_data["error"] = None # Clear any previous error on successful read
|
||||||
reconnect_attempts = 0 # Reset attempts on successful communication
|
reconnect_attempts = 0 # Reset attempts on successful communication
|
||||||
|
|
@ -194,7 +179,7 @@ def file_browser(stdscr):
|
||||||
selected_index = 0
|
selected_index = 0
|
||||||
|
|
||||||
while True:
|
while True:
|
||||||
stdscr.erase()
|
stdscr.clear()
|
||||||
h, w = stdscr.getmaxyx()
|
h, w = stdscr.getmaxyx()
|
||||||
stdscr.addstr(0, 0, f"Select Firmware File: {path}".ljust(w-1), curses.color_pair(2))
|
stdscr.addstr(0, 0, f"Select Firmware File: {path}".ljust(w-1), curses.color_pair(2))
|
||||||
|
|
||||||
|
|
@ -238,9 +223,7 @@ def main_menu(stdscr, slave_id):
|
||||||
curses.start_color(); curses.init_pair(1, curses.COLOR_WHITE, curses.COLOR_BLUE); curses.init_pair(2, curses.COLOR_BLUE, curses.COLOR_WHITE); curses.init_pair(3, curses.COLOR_RED, curses.COLOR_BLUE)
|
curses.start_color(); curses.init_pair(1, curses.COLOR_WHITE, curses.COLOR_BLUE); curses.init_pair(2, curses.COLOR_BLUE, curses.COLOR_WHITE); curses.init_pair(3, curses.COLOR_RED, curses.COLOR_BLUE)
|
||||||
stdscr.bkgd(' ', curses.color_pair(1))
|
stdscr.bkgd(' ', curses.color_pair(1))
|
||||||
|
|
||||||
menu = ["Open Valve", "Close Valve", "Stop Valve", "Settings", "Reset Node", "Firmware Update", "Exit"]
|
menu = ["Open Valve", "Close Valve", "Stop Valve", "Toggle Output 1", "Toggle Output 2", "Set Watchdog", "Reset Node", "Firmware Update", "Exit"]
|
||||||
settings_menu = ["Set Max Open Time", "Set Max Close Time", "Set End Current Open", "Set End Current Close", "Set Obstacle Current Open", "Set Obstacle Current Close", "Set Watchdog", "Back"]
|
|
||||||
current_menu = menu
|
|
||||||
current_row_idx = 0
|
current_row_idx = 0
|
||||||
message, message_time = "", 0
|
message, message_time = "", 0
|
||||||
input_mode, input_prompt, input_str, input_target_reg = False, "", "", 0
|
input_mode, input_prompt, input_str, input_target_reg = False, "", "", 0
|
||||||
|
|
@ -264,31 +247,24 @@ def main_menu(stdscr, slave_id):
|
||||||
elif key == curses.KEY_BACKSPACE or key == 127: input_str = input_str[:-1]
|
elif key == curses.KEY_BACKSPACE or key == 127: input_str = input_str[:-1]
|
||||||
elif key != -1 and chr(key).isprintable(): input_str += chr(key)
|
elif key != -1 and chr(key).isprintable(): input_str += chr(key)
|
||||||
else:
|
else:
|
||||||
if key == curses.KEY_UP: current_row_idx = (current_row_idx - 1) % len(current_menu)
|
if key == curses.KEY_UP: current_row_idx = (current_row_idx - 1) % len(menu)
|
||||||
elif key == curses.KEY_DOWN: current_row_idx = (current_row_idx + 1) % len(current_menu)
|
elif key == curses.KEY_DOWN: current_row_idx = (current_row_idx + 1) % len(menu)
|
||||||
elif key == curses.KEY_ENTER or key in [10, 13]:
|
elif key == curses.KEY_ENTER or key in [10, 13]:
|
||||||
selected_option = current_menu[current_row_idx]
|
selected_option = menu[current_row_idx]
|
||||||
message_time = time.time()
|
message_time = time.time()
|
||||||
if selected_option == "Exit": stop_event.set(); continue
|
if selected_option == "Exit": stop_event.set(); continue
|
||||||
elif selected_option == "Back": current_menu = menu; current_row_idx = 0; continue
|
|
||||||
elif selected_option == "Settings": current_menu = settings_menu; current_row_idx = 0; continue
|
|
||||||
elif selected_option == "Open Valve": client.write_register(REG_HOLDING_VALVE_COMMAND, 1, slave=slave_id); message = "-> Sent OPEN command"
|
elif selected_option == "Open Valve": client.write_register(REG_HOLDING_VALVE_COMMAND, 1, slave=slave_id); message = "-> Sent OPEN command"
|
||||||
elif selected_option == "Close Valve": client.write_register(REG_HOLDING_VALVE_COMMAND, 2, slave=slave_id); message = "-> Sent CLOSE command"
|
elif selected_option == "Close Valve": client.write_register(REG_HOLDING_VALVE_COMMAND, 2, slave=slave_id); message = "-> Sent CLOSE command"
|
||||||
elif selected_option == "Stop Valve": client.write_register(REG_HOLDING_VALVE_COMMAND, 0, slave=slave_id); message = "-> Sent STOP command"
|
elif selected_option == "Stop Valve": client.write_register(REG_HOLDING_VALVE_COMMAND, 0, slave=slave_id); message = "-> Sent STOP command"
|
||||||
elif selected_option == "Set Max Open Time":
|
elif "Toggle Output" in selected_option:
|
||||||
input_mode, input_prompt, input_target_reg = True, "Enter Max Open Time (s): ", REG_HOLDING_MAX_OPENING_TIME_S
|
bit = 0 if "1" in selected_option else 1
|
||||||
elif selected_option == "Set Max Close Time":
|
try:
|
||||||
input_mode, input_prompt, input_target_reg = True, "Enter Max Close Time (s): ", REG_HOLDING_MAX_CLOSING_TIME_S
|
current_val = client.read_holding_registers(REG_HOLDING_DIGITAL_OUTPUTS_STATE, count=1, slave=slave_id).registers[0]
|
||||||
elif selected_option == "Set End Current Open":
|
client.write_register(REG_HOLDING_DIGITAL_OUTPUTS_STATE, current_val ^ (1 << bit), slave=slave_id)
|
||||||
input_mode, input_prompt, input_target_reg = True, "Enter End Current Threshold Open (mA): ", REG_HOLDING_END_CURRENT_THRESHOLD_OPEN_MA
|
message = f"-> Toggled Output {bit+1}"
|
||||||
elif selected_option == "Set End Current Close":
|
except Exception as e: message = f"-> Error: {e}"
|
||||||
input_mode, input_prompt, input_target_reg = True, "Enter End Current Threshold Close (mA): ", REG_HOLDING_END_CURRENT_THRESHOLD_CLOSE_MA
|
|
||||||
elif selected_option == "Set Watchdog":
|
elif selected_option == "Set Watchdog":
|
||||||
input_mode, input_prompt, input_target_reg = True, "Enter Watchdog Timeout (s): ", REG_HOLDING_WATCHDOG_TIMEOUT_S
|
input_mode, input_prompt, input_target_reg = True, "Enter Watchdog Timeout (s): ", REG_HOLDING_WATCHDOG_TIMEOUT_S
|
||||||
elif selected_option == "Set Obstacle Current Open":
|
|
||||||
input_mode, input_prompt, input_target_reg = True, "Enter Obstacle Threshold Open (mA): ", REG_HOLDING_OBSTACLE_THRESHOLD_OPEN_MA
|
|
||||||
elif selected_option == "Set Obstacle Current Close":
|
|
||||||
input_mode, input_prompt, input_target_reg = True, "Enter Obstacle Threshold Close (mA): ", REG_HOLDING_OBSTACLE_THRESHOLD_CLOSE_MA
|
|
||||||
elif selected_option == "Reset Node":
|
elif selected_option == "Reset Node":
|
||||||
try:
|
try:
|
||||||
client.write_register(REG_HOLDING_DEVICE_RESET, 1, slave=slave_id)
|
client.write_register(REG_HOLDING_DEVICE_RESET, 1, slave=slave_id)
|
||||||
|
|
@ -302,7 +278,7 @@ def main_menu(stdscr, slave_id):
|
||||||
else:
|
else:
|
||||||
message = "-> Firmware update cancelled."
|
message = "-> Firmware update cancelled."
|
||||||
|
|
||||||
stdscr.erase()
|
stdscr.clear()
|
||||||
if is_updating:
|
if is_updating:
|
||||||
with update_lock: prog, msg = update_status["progress"], update_status["message"]
|
with update_lock: prog, msg = update_status["progress"], update_status["message"]
|
||||||
stdscr.addstr(h // 2 - 1, w // 2 - 25, "FIRMWARE UPDATE IN PROGRESS", curses.A_BOLD | curses.color_pair(2))
|
stdscr.addstr(h // 2 - 1, w // 2 - 25, "FIRMWARE UPDATE IN PROGRESS", curses.A_BOLD | curses.color_pair(2))
|
||||||
|
|
@ -316,30 +292,24 @@ def main_menu(stdscr, slave_id):
|
||||||
col1, col2, col3, col4 = 2, 30, 58, 88
|
col1, col2, col3, col4 = 2, 30, 58, 88
|
||||||
stdscr.addstr(1, col1, "State:", bold); stdscr.addstr(1, col1 + 18, str(current_data.get('state', 'N/A')), normal)
|
stdscr.addstr(1, col1, "State:", bold); stdscr.addstr(1, col1 + 18, str(current_data.get('state', 'N/A')), normal)
|
||||||
stdscr.addstr(2, col1, "Movement:", bold); stdscr.addstr(2, col1 + 18, str(current_data.get('movement', 'N/A')), normal)
|
stdscr.addstr(2, col1, "Movement:", bold); stdscr.addstr(2, col1 + 18, str(current_data.get('movement', 'N/A')), normal)
|
||||||
stdscr.addstr(3, col1, "Open Current:", bold); stdscr.addstr(3, col1 + 18, str(current_data.get('motor_current_open', 'N/A')), normal)
|
stdscr.addstr(3, col1, "Motor Current:", bold); stdscr.addstr(3, col1 + 18, str(current_data.get('motor_current', 'N/A')), normal)
|
||||||
stdscr.addstr(4, col1, "Close Current:", bold); stdscr.addstr(4, col1 + 18, str(current_data.get('motor_current_close', 'N/A')), normal)
|
|
||||||
stdscr.addstr(1, col2, "Digital Inputs:", bold); stdscr.addstr(1, col2 + 18, str(current_data.get('digital_inputs', 'N/A')), normal)
|
stdscr.addstr(1, col2, "Digital Inputs:", bold); stdscr.addstr(1, col2 + 18, str(current_data.get('digital_inputs', 'N/A')), normal)
|
||||||
stdscr.addstr(2, col2, "Digital Outputs:", bold); stdscr.addstr(2, col2 + 18, str(current_data.get('digital_outputs', 'N/A')), normal)
|
stdscr.addstr(2, col2, "Digital Outputs:", bold); stdscr.addstr(2, col2 + 18, str(current_data.get('digital_outputs', 'N/A')), normal)
|
||||||
stdscr.addstr(3, col2, "Button Events:", bold); stdscr.addstr(3, col2 + 18, str(current_data.get('button_events', 'N/A')), normal)
|
stdscr.addstr(3, col2, "Button Events:", bold); stdscr.addstr(3, col2 + 18, str(current_data.get('button_events', 'N/A')), normal)
|
||||||
stdscr.addstr(1, col3, "Max Open Time:", bold); stdscr.addstr(1, col3 + 16, str(current_data.get('open_time', 'N/A')), normal)
|
stdscr.addstr(1, col3, "Max Open Time:", bold); stdscr.addstr(1, col3 + 16, str(current_data.get('open_time', 'N/A')), normal)
|
||||||
stdscr.addstr(2, col3, "Max Close Time:", bold); stdscr.addstr(2, col3 + 16, str(current_data.get('close_time', 'N/A')), normal)
|
stdscr.addstr(2, col3, "Max Close Time:", bold); stdscr.addstr(2, col3 + 16, str(current_data.get('close_time', 'N/A')), normal)
|
||||||
stdscr.addstr(3, col3, "Watchdog:", bold); stdscr.addstr(3, col3 + 16, str(current_data.get('watchdog', 'N/A')), normal)
|
stdscr.addstr(3, col3, "Watchdog:", bold); stdscr.addstr(3, col3 + 16, str(current_data.get('watchdog', 'N/A')), normal)
|
||||||
stdscr.addstr(4, col3, "End Curr Open:", bold); stdscr.addstr(4, col3 + 16, str(current_data.get('end_curr_open', 'N/A')), normal)
|
|
||||||
stdscr.addstr(5, col3, "End Curr Close:", bold); stdscr.addstr(5, col3 + 16, str(current_data.get('end_curr_close', 'N/A')), normal)
|
|
||||||
stdscr.addstr(6, col3, "Obstacle Open:", bold); stdscr.addstr(6, col3 + 16, str(current_data.get('obstacle_open', 'N/A')), normal)
|
|
||||||
stdscr.addstr(7, col3, "Obstacle Close:", bold); stdscr.addstr(7, col3 + 16, str(current_data.get('obstacle_close', 'N/A')), normal)
|
|
||||||
stdscr.addstr(1, col4, "Firmware:", bold); stdscr.addstr(1, col4 + 14, str(current_data.get('firmware', 'N/A')), normal)
|
stdscr.addstr(1, col4, "Firmware:", bold); stdscr.addstr(1, col4 + 14, str(current_data.get('firmware', 'N/A')), normal)
|
||||||
stdscr.addstr(2, col4, "Uptime:", bold); stdscr.addstr(2, col4 + 14, str(current_data.get('uptime', 'N/A')), normal)
|
stdscr.addstr(2, col4, "Uptime:", bold); stdscr.addstr(2, col4 + 14, str(current_data.get('uptime', 'N/A')), normal)
|
||||||
stdscr.addstr(3, col4, "Dev. Status:", bold); stdscr.addstr(3, col4 + 14, str(current_data.get('device_status', 'N/A')), normal)
|
stdscr.addstr(3, col4, "Dev. Status:", bold); stdscr.addstr(3, col4 + 14, str(current_data.get('device_status', 'N/A')), normal)
|
||||||
stdscr.addstr(4, col4, "Supply V:", bold); stdscr.addstr(4, col4 + 14, str(current_data.get('supply_voltage', 'N/A')), normal)
|
stdscr.addstr(5, 0, "─" * (w - 1), normal)
|
||||||
stdscr.addstr(8, 0, "─" * (w - 1), normal)
|
for idx, row in enumerate(menu):
|
||||||
for idx, row in enumerate(current_menu):
|
draw_button(stdscr, h // 2 - len(menu) + (idx * 2), w // 2 - len(row) // 2, row, idx == current_row_idx)
|
||||||
draw_button(stdscr, 9 + (idx * 2), w // 2 - len(row) // 2, row, idx == current_row_idx)
|
|
||||||
if time.time() - message_time < 2.0: stdscr.addstr(h - 2, 0, message.ljust(w - 1), curses.color_pair(1) | curses.A_BOLD)
|
if time.time() - message_time < 2.0: stdscr.addstr(h - 2, 0, message.ljust(w - 1), curses.color_pair(1) | curses.A_BOLD)
|
||||||
if input_mode:
|
if input_mode:
|
||||||
curses.curs_set(1); stdscr.addstr(h - 2, 0, (input_prompt + input_str).ljust(w-1), curses.color_pair(2)); stdscr.move(h - 2, len(input_prompt) + len(input_str))
|
curses.curs_set(1); stdscr.addstr(h - 2, 0, (input_prompt + input_str).ljust(w-1), curses.color_pair(2)); stdscr.move(h - 2, len(input_prompt) + len(input_str))
|
||||||
else: curses.curs_set(0)
|
else: curses.curs_set(0)
|
||||||
curses.doupdate()
|
stdscr.refresh()
|
||||||
|
|
||||||
def main():
|
def main():
|
||||||
global client
|
global client
|
||||||
|
|
|
||||||
|
|
@ -1,216 +0,0 @@
|
||||||
import argparse
|
|
||||||
import time
|
|
||||||
import logging
|
|
||||||
import threading
|
|
||||||
import serial
|
|
||||||
import asyncio
|
|
||||||
from pymodbus.server import StartSerialServer
|
|
||||||
from pymodbus.datastore import ModbusSequentialDataBlock, ModbusSlaveContext, ModbusServerContext
|
|
||||||
from pymodbus.framer.rtu import FramerRTU
|
|
||||||
|
|
||||||
# --- Configuration ---
|
|
||||||
logging.basicConfig(level=logging.INFO, format='%(asctime)s - %(levelname)s - %(message)s', handlers=[logging.FileHandler('communication_debug.log', 'w')])
|
|
||||||
log = logging.getLogger()
|
|
||||||
logging.getLogger("pymodbus").setLevel(logging.DEBUG)
|
|
||||||
|
|
||||||
# --- Constants from Documentation ---
|
|
||||||
# Input Registers (3xxxx)
|
|
||||||
REG_INPUT_VALVE_STATE_MOVEMENT = 0x0000
|
|
||||||
REG_INPUT_MOTOR_OPEN_CURRENT_MA = 0x0001
|
|
||||||
REG_INPUT_MOTOR_CLOSE_CURRENT_MA = 0x0002
|
|
||||||
REG_INPUT_SUPPLY_VOLTAGE_MV = 0x00F5
|
|
||||||
|
|
||||||
# Holding Registers (4xxxx)
|
|
||||||
REG_HOLDING_VALVE_COMMAND = 0x0000
|
|
||||||
|
|
||||||
# --- Simulation Parameters ---
|
|
||||||
SUPPLY_VOLTAGE = 12345 # mV
|
|
||||||
MOTOR_CURRENT_IDLE = 0 # mA
|
|
||||||
MOTOR_CURRENT_MOVING = 80 # mA
|
|
||||||
VALVE_TRAVEL_TIME = 4.5 # seconds
|
|
||||||
|
|
||||||
# --- Valve Logic ---
|
|
||||||
class ValveController:
|
|
||||||
"""Holds the state and logic for the simulated valve based on documentation."""
|
|
||||||
def __init__(self, node_id):
|
|
||||||
self.node_id = node_id
|
|
||||||
self.lock = threading.Lock()
|
|
||||||
|
|
||||||
# Internal State
|
|
||||||
self.movement = 0 # 0=Idle, 1=Öffnet, 2=Schliesst
|
|
||||||
self.state = 0 # 0=Geschlossen, 1=Geöffnet
|
|
||||||
self.is_moving = False
|
|
||||||
self.movement_start_time = 0
|
|
||||||
self.target_state = 0 # 0 for close, 1 for open
|
|
||||||
|
|
||||||
def start_movement(self, command):
|
|
||||||
"""Initiates a valve movement based on a holding register command."""
|
|
||||||
with self.lock:
|
|
||||||
if self.is_moving:
|
|
||||||
log.warning(f"[Node {self.node_id}] Valve is already moving. Ignoring command.")
|
|
||||||
return
|
|
||||||
|
|
||||||
# Command 1: Open
|
|
||||||
if command == 1 and self.state == 0:
|
|
||||||
log.info(f"[Node {self.node_id}] Received command to OPEN valve.")
|
|
||||||
self.movement = 1 # Öffnet
|
|
||||||
self.target_state = 1 # Geöffnet
|
|
||||||
# Command 2: Close
|
|
||||||
elif command == 2 and self.state == 1:
|
|
||||||
log.info(f"[Node {self.node_id}] Received command to CLOSE valve.")
|
|
||||||
self.movement = 2 # Schliesst
|
|
||||||
self.target_state = 0 # Geschlossen
|
|
||||||
# Command 0: Stop
|
|
||||||
elif command == 0:
|
|
||||||
if self.is_moving:
|
|
||||||
log.info(f"[Node {self.node_id}] Received command to STOP valve.")
|
|
||||||
self.is_moving = False
|
|
||||||
self.movement = 0 # Idle
|
|
||||||
return
|
|
||||||
else:
|
|
||||||
log.info(f"[Node {self.node_id}] Valve is already in the requested state or command is invalid.")
|
|
||||||
return
|
|
||||||
|
|
||||||
self.is_moving = True
|
|
||||||
self.movement_start_time = time.time()
|
|
||||||
|
|
||||||
def update_state(self):
|
|
||||||
"""Updates the valve's position and state if it's moving."""
|
|
||||||
with self.lock:
|
|
||||||
if not self.is_moving:
|
|
||||||
return
|
|
||||||
|
|
||||||
elapsed_time = time.time() - self.movement_start_time
|
|
||||||
|
|
||||||
if elapsed_time >= VALVE_TRAVEL_TIME:
|
|
||||||
# Movement is complete
|
|
||||||
self.is_moving = False
|
|
||||||
self.state = self.target_state
|
|
||||||
self.movement = 0 # Idle
|
|
||||||
log.info(f"[Node {self.node_id}] Valve movement finished. State: {'Open' if self.state == 1 else 'Closed'}")
|
|
||||||
|
|
||||||
# --- Modbus Datastore Blocks ---
|
|
||||||
class CustomDataBlock(ModbusSequentialDataBlock):
|
|
||||||
def __init__(self, controller):
|
|
||||||
self.controller = controller
|
|
||||||
# Initialize registers to a safe default size, they will be dynamically updated.
|
|
||||||
super().__init__(0, [0] * 256)
|
|
||||||
|
|
||||||
def setValues(self, address, values):
|
|
||||||
# Handle writes to the VALVE_COMMAND register
|
|
||||||
if address == REG_HOLDING_VALVE_COMMAND:
|
|
||||||
if values:
|
|
||||||
self.controller.start_movement(values[0])
|
|
||||||
else:
|
|
||||||
log.info(f"[Node {self.controller.node_id}] Write to unhandled holding register 0x{address:04X} with value(s): {values}")
|
|
||||||
super().setValues(address, values)
|
|
||||||
|
|
||||||
def getValues(self, address, count=1):
|
|
||||||
self.controller.update_state() # Update valve state before returning values
|
|
||||||
log.debug(f"getValues: requested address={address}")
|
|
||||||
|
|
||||||
# Handle specific input registers
|
|
||||||
if (address - 1) == REG_INPUT_VALVE_STATE_MOVEMENT:
|
|
||||||
valve_state_movement = (self.controller.movement << 8) | self.controller.state
|
|
||||||
return [valve_state_movement]
|
|
||||||
elif (address - 1) == REG_INPUT_MOTOR_OPEN_CURRENT_MA:
|
|
||||||
motor_current = MOTOR_CURRENT_MOVING if self.controller.movement == 1 else MOTOR_CURRENT_IDLE
|
|
||||||
return [motor_current]
|
|
||||||
elif (address - 1) == REG_INPUT_MOTOR_CLOSE_CURRENT_MA:
|
|
||||||
motor_current = MOTOR_CURRENT_MOVING if self.controller.movement == 2 else MOTOR_CURRENT_IDLE
|
|
||||||
return [motor_current]
|
|
||||||
elif (address - 1) == REG_INPUT_SUPPLY_VOLTAGE_MV:
|
|
||||||
return [SUPPLY_VOLTAGE]
|
|
||||||
else:
|
|
||||||
# For any other register, return 0xbeaf
|
|
||||||
return [0xbeaf] * count
|
|
||||||
|
|
||||||
# --- Main Server ---
|
|
||||||
async def run_server(port, node_id, baudrate):
|
|
||||||
"""Sets up and runs the Modbus RTU server."""
|
|
||||||
controller = ValveController(node_id)
|
|
||||||
datablock = CustomDataBlock(controller)
|
|
||||||
|
|
||||||
store = ModbusSlaveContext(
|
|
||||||
di=datablock, # Input Registers
|
|
||||||
co=None, # Coils (not used)
|
|
||||||
hr=datablock, # Holding Registers
|
|
||||||
ir=datablock, # Re-using for simplicity, maps to the same logic
|
|
||||||
)
|
|
||||||
context = ModbusServerContext(slaves={node_id: store}, single=False)
|
|
||||||
|
|
||||||
log.info(f"Starting Modbus RTU Valve Simulator on {port}")
|
|
||||||
log.info(f"Node ID: {node_id}, Baudrate: {baudrate}")
|
|
||||||
log.info("--- Register Map ---")
|
|
||||||
log.info("Input Registers (Read-Only):")
|
|
||||||
log.info(f" 0x{REG_INPUT_VALVE_STATE_MOVEMENT:04X}: VALVE_STATE_MOVEMENT")
|
|
||||||
log.info(f" 0x{REG_INPUT_MOTOR_OPEN_CURRENT_MA:04X}: MOTOR_OPEN_CURRENT_MA")
|
|
||||||
log.info(f" 0x{REG_INPUT_MOTOR_CLOSE_CURRENT_MA:04X}: MOTOR_CLOSE_CURRENT_MA")
|
|
||||||
log.info(f" 0x{REG_INPUT_SUPPLY_VOLTAGE_MV:04X}: SUPPLY_VOLTAGE_MV")
|
|
||||||
log.info("Holding Registers (Read/Write):")
|
|
||||||
log.info(f" 0x{REG_HOLDING_VALVE_COMMAND:04X}: VALVE_COMMAND (1=Open, 2=Close, 0=Stop)")
|
|
||||||
|
|
||||||
log.info("Server listening.")
|
|
||||||
while True:
|
|
||||||
try:
|
|
||||||
data = ser.read(ser.in_waiting or 1) # Read available data or wait for 1 byte
|
|
||||||
if data:
|
|
||||||
# Process the request
|
|
||||||
request = framer.processIncomingPacket(data)
|
|
||||||
if request:
|
|
||||||
log_line = "<-- Reg: "
|
|
||||||
reg_addr_hex = f"0x{request.address:04X}" if hasattr(request, 'address') else "N/A"
|
|
||||||
rw_indicator = ""
|
|
||||||
reg_type_indicator = ""
|
|
||||||
data_info = ""
|
|
||||||
|
|
||||||
if request.function_code in [0x01, 0x02, 0x03, 0x04]: # Read operations
|
|
||||||
rw_indicator = "r"
|
|
||||||
if request.function_code == 0x03: reg_type_indicator = "Hold."
|
|
||||||
elif request.function_code == 0x04: reg_type_indicator = "Input"
|
|
||||||
log_line += f"{reg_addr_hex}{rw_indicator}"
|
|
||||||
elif request.function_code in [0x05, 0x06, 0x0F, 0x10]: # Write operations
|
|
||||||
rw_indicator = "w"
|
|
||||||
if request.function_code == 0x06 or request.function_code == 0x10: reg_type_indicator = "Hold."
|
|
||||||
elif request.function_code == 0x05 or request.function_code == 0x0F: reg_type_indicator = "Coil"
|
|
||||||
|
|
||||||
if hasattr(request, 'value') and request.value is not None: # For single write (0x05, 0x06)
|
|
||||||
data_info = f" Data: 0x{request.value:04X}"
|
|
||||||
elif hasattr(request, 'values') and request.values is not None: # For multiple write (0x0F, 0x10)
|
|
||||||
data_info = " Data: 0x" + "".join([f"{val:04X}" for val in request.values])
|
|
||||||
elif hasattr(request, 'bits') and request.bits is not None: # For multiple coil write (0x0F)
|
|
||||||
data_info = " Data: 0x" + "".join([f"{int(bit):X}" for bit in request.bits])
|
|
||||||
|
|
||||||
log_line += f"{reg_addr_hex}{rw_indicator} Type: {reg_type_indicator}{data_info}"
|
|
||||||
else:
|
|
||||||
log_line = f"<-- Func: 0x{request.function_code:02X} Raw: {data.hex()}"
|
|
||||||
|
|
||||||
print(log_line)
|
|
||||||
sys.stdout.flush()
|
|
||||||
|
|
||||||
response = request.execute(context)
|
|
||||||
if response:
|
|
||||||
pdu = framer.buildPacket(response)
|
|
||||||
ser.write(pdu)
|
|
||||||
|
|
||||||
response_reg_addr = f"0x{request.address:04X}" if hasattr(request, 'address') else "N/A"
|
|
||||||
response_data_hex = ""
|
|
||||||
response_data_dec = ""
|
|
||||||
|
|
||||||
if hasattr(response, 'registers') and response.registers is not None:
|
|
||||||
response_data_hex = "".join([f"{val:04X}" for val in response.registers])
|
|
||||||
response_data_dec = ", ".join([str(val) for val in response.registers])
|
|
||||||
elif hasattr(response, 'bits') and response.bits is not None:
|
|
||||||
response_data_hex = "".join([f"{int(bit):X}" for bit in response.bits])
|
|
||||||
response_data_dec = ", ".join([str(int(bit)) for bit in response.bits])
|
|
||||||
elif hasattr(response, 'value') and response.value is not None: # For single write response
|
|
||||||
response_data_hex = f"{response.value:04X}"
|
|
||||||
response_data_dec = str(response.value)
|
|
||||||
|
|
||||||
print(f"--> Reg: {response_reg_addr} Data: 0x{response_data_hex} (Dec: {response_data_dec})")
|
|
||||||
sys.stdout.flush()
|
|
||||||
|
|
||||||
except Exception as e:
|
|
||||||
print(f"Error during serial communication: {e}")
|
|
||||||
sys.stderr.flush()
|
|
||||||
await asyncio.sleep(0.001) # Small delay to prevent busy-waiting
|
|
||||||
|
|
@ -1,2 +0,0 @@
|
||||||
pymodbus
|
|
||||||
pyserial
|
|
||||||
|
|
@ -1,58 +0,0 @@
|
||||||
#!/bin/bash
|
|
||||||
#
|
|
||||||
# This script creates a pair of virtual serial ports (pseudo-terminals)
|
|
||||||
# that are linked together, allowing two applications to communicate
|
|
||||||
# as if they were connected by a physical null-modem cable.
|
|
||||||
#
|
|
||||||
# It uses `socat`, a powerful command-line utility for data transfer.
|
|
||||||
|
|
||||||
# --- Check for socat ---
|
|
||||||
if ! command -v socat &> /dev/null
|
|
||||||
then
|
|
||||||
echo "Error: 'socat' is not installed. It is required to create virtual serial ports."
|
|
||||||
echo "Please install it using your package manager."
|
|
||||||
echo "For Debian/Ubuntu: sudo apt-get update && sudo apt-get install socat"
|
|
||||||
echo "For Fedora: sudo dnf install socat"
|
|
||||||
exit 1
|
|
||||||
fi
|
|
||||||
|
|
||||||
# --- Configuration ---
|
|
||||||
# The script will create symlinks to the virtual ports in the current directory
|
|
||||||
# for easy access.
|
|
||||||
PORT1="./vcom_a"
|
|
||||||
PORT2="./vcom_b"
|
|
||||||
|
|
||||||
# --- Cleanup function ---
|
|
||||||
# This function will be called when the script exits to remove the symlinks.
|
|
||||||
trap 'cleanup' EXIT
|
|
||||||
|
|
||||||
cleanup() {
|
|
||||||
echo -e "\nCleaning up..."
|
|
||||||
rm -f "$PORT1" "$PORT2"
|
|
||||||
echo "Removed symlinks '$PORT1' and '$PORT2'."
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
# --- Main Execution ---
|
|
||||||
echo "=================================================="
|
|
||||||
echo " Virtual Serial Port Pair Setup"
|
|
||||||
echo "=================================================="
|
|
||||||
echo
|
|
||||||
echo "Creating a linked pair of virtual serial ports."
|
|
||||||
echo " - Port A will be available at: $PORT1"
|
|
||||||
echo " - Port B will be available at: $PORT2"
|
|
||||||
echo
|
|
||||||
echo "You can now connect the simulator to one port and your client script to the other."
|
|
||||||
echo "Example:"
|
|
||||||
echo " Terminal 1: python modbus_valve_simulator.py $PORT1"
|
|
||||||
echo " Terminal 2: python your_client_script.py $PORT2"
|
|
||||||
echo
|
|
||||||
echo "Press [Ctrl+C] to shut down the virtual ports and exit."
|
|
||||||
echo "--------------------------------------------------"
|
|
||||||
|
|
||||||
# The core command.
|
|
||||||
# -d -d: Increases verbosity to show data transfer.
|
|
||||||
# pty: Creates a pseudo-terminal (virtual port).
|
|
||||||
# raw,echo=0: Puts the terminal in raw mode, suitable for serial data.
|
|
||||||
# link=<path>: Creates a symbolic link to the PTY device for a stable name.
|
|
||||||
socat -d -d pty,raw,echo=0,link="$PORT1" pty,raw,echo=0,link="$PORT2"
|
|
||||||
|
|
@ -1 +0,0 @@
|
||||||
pyserial-asyncio>=0.6
|
|
||||||
|
|
@ -1,163 +0,0 @@
|
||||||
import asyncio
|
|
||||||
import argparse
|
|
||||||
import serial
|
|
||||||
import serial_asyncio
|
|
||||||
from datetime import datetime
|
|
||||||
|
|
||||||
# Globale Variablen für den seriellen Reader und Writer
|
|
||||||
serial_reader = None
|
|
||||||
serial_writer = None
|
|
||||||
|
|
||||||
def log_message(message: str):
|
|
||||||
"""Gibt eine formatierte Log-Nachricht mit Zeitstempel aus."""
|
|
||||||
print(f"[{datetime.now().strftime('%Y-%m-%d %H:%M:%S')}] {message}")
|
|
||||||
|
|
||||||
def log_verbose_request(req: bytes):
|
|
||||||
"""Gibt eine detaillierte, formatierte Log-Nachricht für eine Anfrage aus."""
|
|
||||||
if len(req) < 6: return
|
|
||||||
log_message(f"VERBOSE: --> REQ from HA: {req.hex(' ')}")
|
|
||||||
slave_id = req[0]
|
|
||||||
func_code = req[1]
|
|
||||||
addr = int.from_bytes(req[2:4], 'big')
|
|
||||||
|
|
||||||
# Für Lese-/Schreibbefehle
|
|
||||||
if func_code in [1, 2, 3, 4, 5, 6, 15, 16] and len(req) >= 6:
|
|
||||||
count_or_data = int.from_bytes(req[4:6], 'big')
|
|
||||||
log_message(f"VERBOSE: Parsed: slave={slave_id}, func={func_code}, addr={addr}, count/val={count_or_data}")
|
|
||||||
else:
|
|
||||||
log_message(f"VERBOSE: Parsed: slave={slave_id}, func={func_code}")
|
|
||||||
|
|
||||||
|
|
||||||
def log_verbose_response(res: bytes):
|
|
||||||
"""Gibt eine detaillierte, formatierte Log-Nachricht für eine Antwort aus."""
|
|
||||||
if len(res) < 5: return
|
|
||||||
log_message(f"VERBOSE: <-- RES from DEV: {res.hex(' ')}")
|
|
||||||
slave_id = res[0]
|
|
||||||
func_code = res[1]
|
|
||||||
|
|
||||||
if func_code < 0x80: # Keine Fehler-Antwort
|
|
||||||
byte_count = res[2]
|
|
||||||
data = res[3:-2].hex(' ')
|
|
||||||
log_message(f"VERBOSE: Parsed: slave={slave_id}, func={func_code}, bytes={byte_count}, data=[{data}]")
|
|
||||||
else: # Fehler-Antwort
|
|
||||||
error_code = res[2]
|
|
||||||
log_message(f"VERBOSE: ERROR: slave={slave_id}, func={func_code}, err_code={error_code}")
|
|
||||||
|
|
||||||
|
|
||||||
async def handle_tcp_client(reader: asyncio.StreamReader, writer: asyncio.StreamWriter, verbose: bool):
|
|
||||||
"""Bearbeitet eine einzelne TCP-Client-Verbindung."""
|
|
||||||
global serial_reader, serial_writer
|
|
||||||
peername = writer.get_extra_info('peername')
|
|
||||||
log_message(f"✅ Client verbunden: {peername}")
|
|
||||||
|
|
||||||
if not serial_writer or not serial_reader:
|
|
||||||
log_message("❌ Fehler: Serielle Verbindung ist nicht aktiv. Client wird getrennt.")
|
|
||||||
writer.close(); await writer.wait_closed()
|
|
||||||
return
|
|
||||||
|
|
||||||
try:
|
|
||||||
while True:
|
|
||||||
tcp_request = await reader.read(256)
|
|
||||||
if not tcp_request: break
|
|
||||||
|
|
||||||
if verbose: log_verbose_request(tcp_request)
|
|
||||||
|
|
||||||
# *** HINZUGEFÜGTE ÄNDERUNG: Eine kleine Pause vor dem Senden ***
|
|
||||||
# Dies gibt empfindlichen Geräten oder langsamen Bussen Zeit.
|
|
||||||
await asyncio.sleep(0.05) # 50ms Verzögerung
|
|
||||||
|
|
||||||
serial_writer.write(tcp_request)
|
|
||||||
await serial_writer.drain()
|
|
||||||
|
|
||||||
try:
|
|
||||||
serial_response = await asyncio.wait_for(serial_reader.read(256), timeout=2.0)
|
|
||||||
if verbose: log_verbose_response(serial_response)
|
|
||||||
writer.write(serial_response)
|
|
||||||
await writer.drain()
|
|
||||||
except asyncio.TimeoutError:
|
|
||||||
log_message("VERBOSE: <-- Timeout from DEV")
|
|
||||||
|
|
||||||
except asyncio.CancelledError:
|
|
||||||
log_message("TCP-Handler wurde abgebrochen.")
|
|
||||||
except Exception as e:
|
|
||||||
log_message(f"TCP-Verbindungsfehler: {e}")
|
|
||||||
finally:
|
|
||||||
log_message(f"🔌 Client getrennt: {peername}")
|
|
||||||
writer.close(); await writer.wait_closed()
|
|
||||||
|
|
||||||
|
|
||||||
async def serial_reconnector(comport, baudrate):
|
|
||||||
"""Versucht, die serielle Verbindung wiederherzustellen."""
|
|
||||||
global serial_reader, serial_writer
|
|
||||||
for attempt in range(1, 6):
|
|
||||||
log_message(f"🚨 Serielle Verbindung verloren! Versuch {attempt}/5 in 5 Sekunden...")
|
|
||||||
await asyncio.sleep(5)
|
|
||||||
try:
|
|
||||||
reader_obj, writer_obj = await serial_asyncio.open_serial_connection(
|
|
||||||
url=comport, baudrate=baudrate, rtscts=False, dsrdtr=False
|
|
||||||
)
|
|
||||||
serial_reader, serial_writer = reader_obj, writer_obj
|
|
||||||
log_message(f"✅ Serielle Verbindung zu {comport} wiederhergestellt.")
|
|
||||||
return True
|
|
||||||
except (serial.SerialException, FileNotFoundError) as e:
|
|
||||||
log_message(f"❌ Wiederverbindung fehlgeschlagen: {e}")
|
|
||||||
|
|
||||||
log_message("💥 Konnte serielle Verbindung nach 5 Versuchen nicht wiederherstellen. Programm wird beendet.")
|
|
||||||
try:
|
|
||||||
asyncio.get_running_loop().stop()
|
|
||||||
except RuntimeError: pass
|
|
||||||
return False
|
|
||||||
|
|
||||||
async def main(args):
|
|
||||||
"""Hauptfunktion zum Starten des Servers und der seriellen Verbindung."""
|
|
||||||
global serial_reader, serial_writer
|
|
||||||
log_message("--- Modbus RTU zu TCP Gateway ---")
|
|
||||||
log_message(f"Serieller Port: {args.comport}")
|
|
||||||
log_message(f"Baudrate: {args.baudrate}")
|
|
||||||
log_message(f"TCP Port: {args.tcpport}")
|
|
||||||
log_message(f"Verbose Modus: {'Aktiv' if args.verbose else 'Inaktiv'}")
|
|
||||||
log_message("---------------------------------")
|
|
||||||
|
|
||||||
try:
|
|
||||||
serial_reader, serial_writer = await serial_asyncio.open_serial_connection(
|
|
||||||
url=args.comport, baudrate=args.baudrate, rtscts=False, dsrdtr=False
|
|
||||||
)
|
|
||||||
log_message(f"✅ Serielle Verbindung zu {args.comport} erfolgreich hergestellt.")
|
|
||||||
except (serial.SerialException, FileNotFoundError) as e:
|
|
||||||
log_message(f"❌ Kritischer Fehler bei der initialen Verbindung: {e}")
|
|
||||||
if not await serial_reconnector(args.comport, args.baudrate): return
|
|
||||||
|
|
||||||
server = await asyncio.start_server(
|
|
||||||
lambda r, w: handle_tcp_client(r, w, args.verbose), '0.0.0.0', args.tcpport
|
|
||||||
)
|
|
||||||
addr = server.sockets[0].getsockname()
|
|
||||||
log_message(f"👂 Server lauscht auf {addr}")
|
|
||||||
|
|
||||||
async with server:
|
|
||||||
while True:
|
|
||||||
try:
|
|
||||||
if serial_reader and hasattr(serial_reader, '_transport') and serial_reader._transport:
|
|
||||||
_ = serial_reader._transport.serial.cts
|
|
||||||
else:
|
|
||||||
raise serial.SerialException("Transport nicht verfügbar.")
|
|
||||||
except (serial.SerialException, AttributeError, BrokenPipeError, TypeError):
|
|
||||||
log_message("Serielle Verbindung unterbrochen. Starte Wiederverbindungs-Logik...")
|
|
||||||
if not await serial_reconnector(args.comport, args.baudrate):
|
|
||||||
server.close(); break
|
|
||||||
await asyncio.sleep(2)
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
parser = argparse.ArgumentParser(description="Modbus RTU zu TCP Gateway für Home Assistant.")
|
|
||||||
parser.add_argument("comport", help="Der serielle Port (z.B. /dev/ttyUSB0 oder COM3)")
|
|
||||||
parser.add_argument("-b", "--baudrate", type=int, default=19200, help="Baudrate der seriellen Verbindung (Standard: 19200)")
|
|
||||||
parser.add_argument("-p", "--tcpport", type=int, default=502, help="TCP-Port, auf dem der Server lauscht (Standard: 502)")
|
|
||||||
parser.add_argument("-v", "--verbose", action="store_true", help="Verbose-Modus aktivieren für detaillierte Logs")
|
|
||||||
|
|
||||||
args = parser.parse_args()
|
|
||||||
|
|
||||||
try:
|
|
||||||
asyncio.run(main(args))
|
|
||||||
except KeyboardInterrupt:
|
|
||||||
log_message("\n👋 Programm wird durch Benutzer beendet.")
|
|
||||||
except Exception as e:
|
|
||||||
log_message(f"💥 Unerwarteter Fehler im Hauptprogramm: {e}")
|
|
||||||
Loading…
Reference in New Issue