diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 89c138a..a1d5d57 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -18,7 +18,7 @@ jobs: strategy: fail-fast: false matrix: - environment: [espA, espB, espA_pool] + environment: [espA, espB] steps: - name: Checkout diff --git a/.github/workflows/release.yml b/.github/workflows/release.yml index 05992dd..1d2be5d 100644 --- a/.github/workflows/release.yml +++ b/.github/workflows/release.yml @@ -15,7 +15,7 @@ jobs: strategy: fail-fast: true matrix: - environment: [espA, espB, espA_pool] + environment: [espA, espB] steps: - name: Checkout diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index 90516f2..a1e0355 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -57,17 +57,16 @@ Subject in imperative ("add X", not "added X" / "adds X"), <72 chars. Body expla ### Pull request expectations - The branch must be **rebased on `master`** before merging (or at least up-to-date) — the PR UI will warn if not. -- All [CI checks](.github/workflows/ci.yml) must be green: `build (espA)`, `build (espB)`, `build (espA_pool)`. +- All [CI checks](.github/workflows/ci.yml) must be green: `build (espA)`, `build (espB)`. - At least **one approving review** from another maintainer. - Description should state *what* changes and *why*, plus a manual test plan if the change touches motion/PID/comms (CI does not run hardware tests). ## CI -`.github/workflows/ci.yml` runs on every push to any branch and on every pull request to `master`. It compiles all three PlatformIO environments in parallel: +`.github/workflows/ci.yml` runs on every push to any branch and on every pull request to `master`. It compiles both PlatformIO environments in parallel: - `espA` — float controller firmware - `espB` — communication bridge firmware -- `espA_pool` — float controller with shallow-pool profile Caching of PlatformIO core and build artifacts keeps a typical run under 2 minutes after the first warm-up. @@ -75,7 +74,7 @@ Caching of PlatformIO core and build artifacts keeps a typical run under 2 minut ### Releases -Push a tag like `v11.3.0` and the [release workflow](.github/workflows/release.yml) builds all three environments and publishes a GitHub Release with the `firmware.bin` and `firmware.elf` for each one attached. +Push a tag like `v11.3.0` and the [release workflow](.github/workflows/release.yml) builds both environments and publishes a GitHub Release with the `firmware.bin` and `firmware.elf` for each one attached. ```bash # After the change is merged to master: @@ -99,7 +98,7 @@ Enable: - Dismiss stale pull request approvals when new commits are pushed - Require status checks to pass before merging - Require branches to be up to date before merging - - Status checks: `Build espA`, `Build espB`, `Build espA_pool` + - Status checks: `Build espA`, `Build espB` - Do not allow bypassing the above settings (recommended) - Restrict who can push to matching branches (admin only — for emergency hotfixes) @@ -110,7 +109,6 @@ gh api -X PUT repos/:owner/:repo/branches/master/protection \ -F required_status_checks.strict=true \ -F 'required_status_checks.contexts[]=Build espA' \ -F 'required_status_checks.contexts[]=Build espB' \ - -F 'required_status_checks.contexts[]=Build espA_pool' \ -F enforce_admins=false \ -F required_pull_request_reviews.required_approving_review_count=1 \ -F required_pull_request_reviews.dismiss_stale_reviews=true \ diff --git a/README.md b/README.md index edbcd5e..1cc4bc3 100644 --- a/README.md +++ b/README.md @@ -1,17 +1,17 @@ -# PoliTOcean Float 2025 - Technical Documentation +# PoliTOcean Float - Technical Documentation -[![CI](https://github.com/PoliTOcean/Float_2025/actions/workflows/ci.yml/badge.svg)](https://github.com/PoliTOcean/Float_2025/actions/workflows/ci.yml) +[![CI](https://github.com/PoliTOcean/Float/actions/workflows/ci.yml/badge.svg)](https://github.com/PoliTOcean/Float/actions/workflows/ci.yml) **Version:** 11.2.0 **Team:** PoliTOcean @ Politecnico di Torino **Maintainers:** Colabella Davide, Benevenga Filippo -**Competition:** MATE ROV 2025/26 +**Competition:** MATE ROV 2026 -------------------------------------------------------------------------- ## TABLE OF CONTENTS -- [PoliTOcean Float 2025 - Technical Documentation](#politocean-float-2025---technical-documentation) +- [PoliTOcean Float - Technical Documentation](#politocean-float---technical-documentation) - [TABLE OF CONTENTS](#table-of-contents) - [PROJECT OVERVIEW](#project-overview) - [Introduction and Requirements](#introduction-and-requirements) @@ -71,22 +71,22 @@ The FLOAT must complete **two vertical profiles** using a buoyancy engine (fluid **Data Collection & Transmission:** -- Collect depth/pressure measurements during both profiles and transmit judge packets every **5 seconds** (minimum 20 data packets) -- Store data in ESP32 internal flash as a LittleFS CSV containing: company number, profile id, time, pressure, judge/reference depth, phase, and raw sensor depth -- After recovery, transmit all collected data wirelessly to the Mission Station +- Collect depth/pressure measurements during both profiles and keep them locally while the antenna is underwater. +- Store data in ESP32 internal flash as a LittleFS CSV containing: company number, profile id, time, pressure, judge/reference depth, phase, raw sensor depth, and normalized syringe position `syringe_u`. +- After surfacing/recovery, replay selected data packets to the Mission Station at the configured packet cadence (`DATA_PACKET_PERIOD_MS`, default 5 s). - Data packets must show **7 sequential measurements** (spanning 30 seconds at 5-second intervals: 0, 5, 10, 15, 20, 25, 30) confirming proper depth maintenance at both 2.5m and 0.4m **Post-Mission Requirements:** -- Upon surface recovery, autonomously transmit all profile data to the CS -- CS GUI plots depth over time using received data (minimum 20 data packets required) -- Graph must display time (X-axis) vs depth (Y-axis) for both completed profiles +- Upon surface recovery, make buffered flash-backed profile data available to the CS. +- CS GUI fetches the stored profile after the `LISTENING` command. +- The active GUI plots depth, pressure, and normalized syringe position over time; the judging requirement still only mandates depth over time. **Current firmware storage note:** the active implementation uses the internal flash CSV log (`FLASH_LOG_PATH`) as the primary mission data source. EEPROM compact records remain only as an internal legacy buffer. The legacy serial command name is still `CLEAR_SD`, but it now resets the flash CSV log and the legacy EEPROM buffer. **Auto Mode (AM):** -An autonomous operating mode that triggers profile execution in case of connection loss with the CS, ensuring mission completion if communication is temporarily unavailable. AM will autonomously commit up to two profiles when connection is lost, preventing incomplete missions due to transient WiFi failures. +An autonomous operating mode that triggers profile execution in case of connection loss with the CS, ensuring mission completion if communication is temporarily unavailable. AM will autonomously commit the configured runtime profile count when connection is lost, preventing incomplete missions due to transient WiFi failures. **Penalties:** - Breaking surface or contacting ice sheet during profile: **-5 points per profile** @@ -102,9 +102,9 @@ The FLOAT has two main logical states: the command execution one, and the idle o The FLOAT changes its buoyancy by pulling and pushing water through a pair of syringes driven by a stepper motor through a lead screw. The mechanical convention is: -- **Home (`motor_pos = 0`)**: piston fully inserted, **syringes empty of water** → the FLOAT floats. At this position the TOF reads ≈ `TOF_HOMING_THRESHOLD` (75 mm) because the piston is far from the sensor. -- **Full extension (`motor_pos = uToMotorPos(1.0f)`)**: piston extracted, **syringes full of water** → the FLOAT sinks. TOF reads ≈ `TOF_SAFE_RANGE_MIN_MM` (40 mm). -- **PID logical convention**: `u ∈ [0, 1]` with `u = 0` → float (empty) and `u = 1` → sink (full). The helper `uToMotorPos(u)` in `include/config.h` maps `u` to the actual motor target while respecting `MOTOR_INVERT_LOGICAL`, so callers never hard-code signs. +- **Home (`motor_pos = 0`)**: plate/piston far from the TOF, water pushed out, **syringes empty** → the FLOAT floats. At this position the TOF reads ≈ `TOF_HOMING_THRESHOLD` (75 mm). +- **Sink direction (`motor_pos < 0`)**: plate moves toward the TOF, the syringes take in water, and the FLOAT sinks. Full logical extension is `uToMotorPos(1.0f) = -(MOTOR_MAX_STEPS - 2*MOTOR_ENDSTOP_MARGIN)`. +- **PID/profile logical convention**: `u ∈ [0, 1]` with `u = 0` → float (empty) and `u = 1` → sink (full). `uToMotorPos(u)` maps logical `u` to motor steps, and `motorPosToU(position)` converts the measured motor position back to the logged `syringe_u`. TOF safety limits used during motion: @@ -122,7 +122,7 @@ When the FLOAT is "floating", we usually want its top a few centimetres below th Two ways to change it: - **At compile time**: edit `SURFACE_TARGET_OFFSET_M` in `include/config.h`. -- **At runtime**: send command `SURFACE_OFFSET ` via the CS, or `SURFACE_OFFSET ` over USB serial on ESPA. The change persists until the next reboot. +- **At runtime**: set the profile surface offset from NEXUS/GUI to persist it on ESPA, or send `SURFACE_OFFSET ` via CS/USB for a temporary tuning change until reboot. The offset is geometry-agnostic: `FLOAT_TOP_TO_SENSOR_M` (geometric distance between the top of the float and the barometer) and `SURFACE_TARGET_OFFSET_M` (operational target) are kept as separate constants in `include/config.h`. @@ -263,6 +263,7 @@ The project follows a modular architecture with separate compilation units: - **Communication** (`lib/comms`) - ESP-NOW wireless protocol and ElegantOTA session management - **Sensors** (`lib/sensors`) - Bar02 pressure/depth and INA219 battery monitoring - **PID Controller** (`lib/pid`) - depth control algorithm with runtime gain updates +- **Runtime Config** (`lib/runtime_config`) - persisted PID, balance, and motor settings - **Profile Manager** (`lib/profile`) - mission profile execution and flash-backed mission logging - **Flash Storage** (`lib/flash_storage`) - LittleFS CSV mission log and replay helpers - **LED Controller** (`lib/led`) - RGB status indication system @@ -288,9 +289,9 @@ stateDiagram-v2 EXECUTING --> BALANCE: BALANCE Command EXECUTING --> SEND_DATA: LISTENING Command EXECUTING --> CLEAR_DATA: CLEAR_SD Command - EXECUTING --> UPDATE_PID: PARAMS Command - EXECUTING --> UPDATE_PID_EXT: PARAMS_EXT Command - EXECUTING --> TEST_SPEED: TEST_FREQ Command + EXECUTING --> UPDATE_PID: PID_CONFIG_SET Command + EXECUTING --> READ_CONFIG: *_CONFIG_GET Command + EXECUTING --> UPDATE_CONFIG: PROFILE/BALANCE/MOTOR_CONFIG_SET Command EXECUTING --> TEST_STEPS: TEST_STEPS Command EXECUTING --> DEBUG_MODE: DEBUG Command EXECUTING --> HOMING: HOME_MOTOR Command @@ -308,9 +309,9 @@ stateDiagram-v2 BALANCE --> IDLE: Balance Complete SEND_DATA --> IDLE: Data Sent CLEAR_DATA --> IDLE: Flash Log Cleared - UPDATE_PID --> IDLE: Gains Updated - UPDATE_PID_EXT --> IDLE: Period/alpha Updated - TEST_SPEED --> IDLE: Speed Stored + UPDATE_PID --> IDLE: PID Config Stored + READ_CONFIG --> IDLE: JSON Sent + UPDATE_CONFIG --> IDLE: Runtime Config Stored TEST_STEPS --> IDLE: Test Move Complete DEBUG_MODE --> IDLE: Debug Toggle Complete OTA --> IDLE: Upload Complete @@ -443,24 +444,30 @@ Table of FLOAT commands with relative effects and acknowledgements: | Cmd string | Cmd ESPA number | Cmd effects | ESPA ack string | ESPA ack effects on ESPB state | | :--------------: | :-------------: | --------------------------------------------------------------------------------------------------------------------------------------------------------------- | :-------------------------: | :---------------------------------------: | -| GO | 1 | Performs the two MATE vertical profiles, sends the pre-descent data packet before the first descent, and logs pressure/depth records to flash CSV | GO_RECVD | status to 2 (command execution)
| -| LISTENING | 2 | Streams flash CSV records as JSON data packets at 5-second cadence, followed by `STOP_DATA` | Ack is data itself | status to 2 after first package arrival | +| GO | 1 | Runs the configured runtime profile sequence, sends the pre-descent data packet before the first descent, and logs pressure/depth/syringe records to flash CSV | GO_RECVD | status to 2 (command execution)
| +| LISTENING | 2 | Replays stored flash CSV records as JSON data packets after recovery/fetch, filtered by `DATA_PACKET_PERIOD_MS`, followed by `STOP_DATA` | Ack is data itself | status to 2 after first package arrival | | BALANCE | 3 | Cycles full extension and retraction with `holdMs` holds until Bar02 pressure rises above the startup baseline by `BALANCE_STOP_PRESSURE_DELTA_KPA`. Requires the motor to be homed first — otherwise the command fails with `Balance: homing required` | CMD3_RECVD | status to 2 | | CLEAR_SD | 4 | Clears and recreates the flash CSV log, and clears the legacy EEPROM buffer. The command string is kept as `CLEAR_SD` for compatibility | CMD4_RECVD | status to 2 | | SWITCH_AUTO_MODE | 5 | Toggles FLOAT Auto Mode | SWITCH_AM_RECVD | status to 2, AM activation state toggled | -| SEND_PACKAGE | 6 | Sends a single live JSON snapshot containing company number, time, pressure, judge/reference depth, phase, and raw sensor depth | Ack is the package itself | status to 2 | +| SEND_PACKAGE | 6 | Sends a single live JSON snapshot containing company number, time, pressure, judge/reference depth, phase, raw sensor depth, and `syringe_u` | Ack is the package itself | status to 2 | | TRY_UPLOAD | 7 | Starts the ElegantOTA access point on ESPA for a 5-minute upload window, then restores ESP-NOW | TRY_UPLOAD_RECVD | status to 2 | -| `PARAMS kp ki kd` | 8 | Updates PID gains at runtime | CHNG_PARMS_RECVD | status to 2 | -| `TEST_FREQ freq` | 9 | Sets manual test movement speed, clamped to 10-1200 steps/s | TEST_FREQ_RECVD | status to 2 | -| `TEST_STEPS n` | 10 | Moves the motor by `n` relative steps at the current test speed | TEST_STEPS_RECVD | status to 2 | +| `PID_CONFIG_SET kp ki kd period_ms alpha_d integral_limit min_retarget_frac u_neutral` | 8 | Updates and persists PID runtime configuration | PID_CONFIG_SET_RECVD | status to 2 | +| reserved | 9 | Reserved for backwards-compatible command numbering | - | - | +| `TEST_STEPS n` | 10 | Moves the motor by `n` relative steps at the configured motor test speed | TEST_STEPS_RECVD | status to 2 | | DEBUG | 11 | Toggles remote debug forwarding through `DebugSerial` | DEBUG_MODE_RECVD | status to 2 | | HOME_MOTOR | 12 | Runs TOF-based homing remotely | HOME_RECVD | status to 2 | | STOP | 13 | Triggers a remote emergency stop, stops the motor, disables outputs, and returns to idle | STOP_RECVD | status to 2 | -| `PARAMS_EXT period alpha` | 14 | Updates PID tick period (ms) and derivative LPF coefficient `alphaD` at runtime | CHNG_PID_EXT_RECVD | status to 2 | +| `PID_CONFIG_GET` | 14 | Returns current PID runtime configuration as JSON | JSON packet | status to 2 | | `SYRINGE_SET u dur_s` | 15 | Bench test: drives the syringe to normalized position `u ∈ [0,1]` for `dur_s` seconds, logging depth — bypasses the PID (DC gain / time-constant characterization) | SYRINGE_SET_RECVD | status to 2 | | `PID_HOLD depth dur_s` | 16 | Bench test: holds depth at `depth_m` for `dur_s` seconds with the PID active, logging at 5 Hz | PID_HOLD_RECVD | status to 2 | | `PID_STEP depth` | 17 | Bench test: step response — drives the PID to `depth_m` for up to 60 s, logging at 10 Hz | PID_STEP_RECVD | status to 2 | | `SURFACE_OFFSET m` | 18 | Sets the surface target offset (`SURFACE_TARGET_OFFSET_M`) at runtime: the FLOAT will hold its top `m` metres below the waterline when "floating" (default `0.10`) | SURFACE_OFF_RECVD | status to 2 | +| `PROFILE_SET count deep shallow_top tol hold pid_timeout ascent_timeout surface_offset` | 19 | Updates and persists mission profile configuration used by `GO` | PROFILE_SET_RECVD / PROFILE_SET_ERR | status to 2 | +| `PROFILE_GET` | 20 | Returns current mission profile configuration as JSON | JSON packet | status to 2 | +| `BALANCE_CONFIG_SET hold_ms stop_delta_kpa stop_samples sample_period_ms` | 21 | Updates and persists balance routine configuration | BALANCE_CONFIG_SET_RECVD / BALANCE_CONFIG_SET_ERR | status to 2 | +| `BALANCE_CONFIG_GET` | 22 | Returns current balance configuration as JSON | JSON packet | status to 2 | +| `MOTOR_CONFIG_SET max_speed max_accel homing_speed test_speed` | 23 | Updates and persists motor speed/acceleration configuration | MOTOR_CONFIG_SET_RECVD / MOTOR_CONFIG_SET_ERR | status to 2 | +| `MOTOR_CONFIG_GET` | 24 | Returns current motor configuration as JSON | JSON packet | status to 2 | | STATUS | - | Requests stale ESPB status plus AM state, WiFi connection state, battery millivolts, and last RSSI | - | - | Once a command is completed, ESPA acknowledgement can be: @@ -557,18 +564,23 @@ The GUI sends command strings to ESPB over USB serial. ESPB parses the string, s | `SWITCH_AUTO_MODE` | 5 | `SWITCH_AM_RECVD` | | `SEND_PACKAGE` | 6 | Live JSON packet | | `TRY_UPLOAD` | 7 | `TRY_UPLOAD_RECVD` | -| `PARAMS kp ki kd` | 8 | `CHNG_PARMS_RECVD` | -| `TEST_FREQ freq` | 9 | `TEST_FREQ_RECVD` | +| `PID_CONFIG_SET kp ki kd period_ms alpha_d integral_limit min_retarget_frac u_neutral` | 8 | `PID_CONFIG_SET_RECVD` / `PID_CONFIG_SET_ERR` | | `TEST_STEPS n` | 10 | `TEST_STEPS_RECVD` | | `DEBUG` | 11 | `DEBUG_MODE_RECVD` | | `HOME_MOTOR` | 12 | `HOME_RECVD` | | `STOP` | 13 | `STOP_RECVD` | -| `PARAMS_EXT period_ms alpha_d` | 14 | `CHNG_PID_EXT_RECVD` | +| `PID_CONFIG_GET` | 14 | PID config JSON | | `SYRINGE_SET u dur_s` | 15 | `SYRINGE_SET_RECVD` | | `PID_HOLD depth_m dur_s` | 16 | `PID_HOLD_RECVD` | | `PID_STEP depth_m` | 17 | `PID_STEP_RECVD` | | `SURFACE_OFFSET m` | 18 | `SURFACE_OFF_RECVD` | -| `STATUS` | - | ESPB local status line with five ` | `-separated fields | +| `PROFILE_SET count deep shallow_top tol hold pid_timeout ascent_timeout surface_offset` | 19 | `PROFILE_SET_RECVD` / `PROFILE_SET_ERR` | +| `PROFILE_GET` | 20 | Profile JSON | +| `BALANCE_CONFIG_SET hold_ms stop_delta_kpa stop_samples sample_period_ms` | 21 | `BALANCE_CONFIG_SET_RECVD` / `BALANCE_CONFIG_SET_ERR` | +| `BALANCE_CONFIG_GET` | 22 | Balance config JSON | +| `MOTOR_CONFIG_SET max_speed max_accel homing_speed test_speed` | 23 | `MOTOR_CONFIG_SET_RECVD` / `MOTOR_CONFIG_SET_ERR` | +| `MOTOR_CONFIG_GET` | 24 | Motor config JSON | +| `STATUS` | - | ESPB local status line with five pipe-separated fields | The peer MAC addresses are configured centrally in `include/config.h`: `MAC_ESPA` is used by ESPB, and `MAC_ESPB` is used by ESPA. @@ -598,15 +610,15 @@ Driven by `LEDState` (scoped enum in [`lib/led/include/led.h`](lib/led/include/l | **Orange Blink** | `LEDState::OTA_MODE` | OTA update mode active | | **Off** | `LEDState::OFF` | System off or disabled | -> ESPB uses a separate `FloatLEDState` enum (`LED_*` prefix) defined in [`include/float_common.h`](include/float_common.h); the two enums are deliberately independent because the two boards have different LED states to signal. +> ESPA and ESPB share the logical `LEDState` enum from [`include/float_common.h`](include/float_common.h). ESPA maps every state to the external RGB LED; ESPB maps only the built-in LED states it can represent. ### ESPB (Communication Bridge) LED States: | LED Pattern | State | Description | |:----------------:|:-----:|:------------| -| **Solid On** | `LED_IDLE` | Connected and ready | -| **Very Fast Blink** | `LED_ERROR` | Communication error | -| **Off** | `LED_OFF` | System off or disabled | +| **Solid On** | `LEDState::IDLE` | Connected and ready | +| **Very Fast Blink** | `LEDState::ERROR` | Communication error | +| **Off** | `LEDState::OFF` | System off or disabled | > **Note**: ESPB uses the built-in LED (pin 2) with different blink patterns to indicate status, as it does not have external RGB connections. @@ -619,7 +631,6 @@ Driven by `LEDState` (scoped enum in [`lib/led/include/led.h`](lib/led/include/l | Environment | Purpose | Main Source | |:------------|:--------|:------------| | `espA` | Float controller firmware with sensors, TOF homing, motion control, PID, ESP-NOW, and OTA | `src/espA/main.cpp` | -| `espA_pool` | ESPA firmware compiled with conservative 70 cm pool-test targets (`POOL_TEST_PROFILE`) | `src/espA/main.cpp` | | `espB` | USB-to-ESP-NOW bridge for the Control Station | `src/espB/main.cpp` | | `espA_manual_keyboard` | Bench firmware for serial keyboard continuous motor movement without homing | `src/espA_manual_keyboard/main.cpp` | @@ -627,7 +638,6 @@ Common commands: ```bash pio run -e espA -pio run -e espA_pool pio run -e espB pio run -e espA_manual_keyboard pio test -e espA @@ -638,7 +648,7 @@ pio test -e espA Run all commands from the project root: ```bash -cd Float_2025 +cd Float ``` To build and upload the main firmware targets: @@ -648,11 +658,7 @@ pio run -e espA -t upload pio run -e espB -t upload ``` -For a conservative shallow-pool test at about 70 cm, upload ESPA with: - -```bash -pio run -e espA_pool -t upload -``` +Shallow-pool profile values are configured at runtime from the Control Station GUI. To open the serial monitor at 115200 baud: @@ -667,12 +673,45 @@ All commands in the FLOAT Commands table can be sent over the ESPB USB serial br | Command | Effect | |:--------|:-------| -| `PARAMS ` | Update PID gains at runtime (same effect as command 8) | -| `PARAMS_EXT ` | Update PID tick period and derivative LPF coefficient (command 14) | +| `PID_CONFIG_SET ` | Update and persist PID runtime configuration (command 8) | +| `PID_CONFIG_GET` | Print current PID configuration JSON (command 14) | | `SYRINGE_SET ` | Drive the syringe to position `u ∈ [0,1]` for `dur_s` seconds and log depth — bypasses the PID, useful for DC-gain and time-constant estimation (command 15) | | `PID_HOLD ` | Hold PID at `depth_m` for `dur_s` seconds, log at 5 Hz (command 16) | | `PID_STEP ` | Step response: PID at `depth_m` for up to 60 s, log at 10 Hz (command 17) | | `SURFACE_OFFSET ` | Set the surface target offset (`SURFACE_TARGET_OFFSET_M`) at runtime (command 18) | +| `SIM_ON` / `SIM_OFF` | Enable/disable the **bench barometer simulator** (the motor still moves for real) | +| `SIM_GET` | Print simulator state and physics parameters | +| `SIM_CONFIG ` | Retune the simulator physics at runtime | +| `GO` | Run the full mission (all profiles: descent+hold+ascent+hold, then surface rest) straight from serial, no GUI/ESPB — with `SIM_ON` this is a dry end-to-end bench test (blocking; reset ESPA to abort) | + +#### Bench PID Tuning with the Simulator (dry, no water) + +`SIM_ON` replaces the Bar02 depth reading with an on-board second-order physics +model (buoyancy + quadratic drag) driven by the **real syringe position**, so the +motor moves exactly as in the water but depth is synthetic. Because `sensors.depth()` +is overridden transparently, `PID_STEP`, `PID_HOLD`, and a full `GO` mission all run +against the simulated water column with no other changes — let you tune the PID at a +desk. + +Model: `a = accelGain·(u − uNeutral) − dragQuad·v·|v|`, where `u = motorPosToU(motor.position())` +(`u > uNeutral` ⇒ sinks). Defaults (`config.h` `SIM_*`) reproduce the real float's +slow actuator (~2 mm/s) and downward overshoot; **set `uNeutral` to your float's +real neutral** (the steady-state `u` you observe in a `PID_HOLD` run) for faithful tuning. + +Typical loop (over ESPA USB serial, 115200 baud): + +```text +SIM_ON # enable simulator (motor still moves) +SIM_CONFIG 0.35 0.05 1.5 3.0 # optional: match your float's physics +PID_CONFIG_SET 0.5 0.1 3.0 50 0.25 5 0.001 0.011 +PID_STEP 2.5 # watch the CSV: t_ms,depth,target,error,u,motor_pos +PID_CONFIG_SET 0.5 0.1 6.0 50 0.25 5 0.001 0.011 # e.g. raise Kd to cut overshoot +PID_STEP 2.5 # compare +SIM_OFF # back to the real barometer +``` + +The same serial CSV feeds `tools/pid_tuning/pid_tuning.ipynb` for overshoot/settling +metrics and gain suggestions. ### CLI Tests @@ -766,11 +805,11 @@ Hardware-oriented tests are stored under `test/`: ### Continuous Integration -GitHub Actions builds all three PlatformIO environments (`espA`, `espB`, `espA_pool`) on every push to any branch and on every pull request to `master`. Workflow file: [.github/workflows/ci.yml](.github/workflows/ci.yml). +GitHub Actions builds the production PlatformIO environments (`espA`, `espB`) on every push to any branch and on every pull request to `master`. Workflow file: [.github/workflows/ci.yml](.github/workflows/ci.yml). CI does **not** run the `unit_hw/` or `integration/` PlatformIO tests because they need a real ESP32 with the float wired up. Run those locally on the bench. -Pushing a `v*` tag triggers [.github/workflows/release.yml](.github/workflows/release.yml), which builds all three environments and attaches the resulting `firmware.bin` / `firmware.elf` to a GitHub Release auto-named after the tag. +Pushing a `v*` tag triggers [.github/workflows/release.yml](.github/workflows/release.yml), which builds the production environments and attaches the resulting `firmware.bin` / `firmware.elf` to a GitHub Release auto-named after the tag. See [CONTRIBUTING.md](CONTRIBUTING.md) for the full git workflow (trunk-based with PR review on `master`), commit conventions, and one-time branch protection setup. @@ -818,9 +857,9 @@ See [CONTRIBUTING.md](CONTRIBUTING.md) for the full git workflow (trunk-based wi Recent changes: - PID output normalized to `u ∈ [0, 1]` (fraction of syringe travel). Default gains `Kp = 0.17`, `Kd = 0.13`, expressed per metre of depth error so they stay valid if `MOTOR_MAX_STEPS` changes. -- Motor geometry: home = piston fully inserted (empty syringes, floats); full extension = piston extracted (full syringes, sinks). The mapping `uToMotorPos()` in `include/config.h` encapsulates `MOTOR_INVERT_LOGICAL` so motion code never hard-codes signs. +- Motor geometry: home = `motor_pos = 0`, empty syringes, floats; increasing `u` maps to negative motor positions, fills the syringes, and sinks. `motorPosToU()` is logged as `syringe_u`. - TOF safety range widened to `[40, 85] mm` to give 10 mm of margin above the homing threshold without risking the mechanical end stop. - `balance` now refuses to start without a prior homing (was forcing `pos = 0` as a fallback, mechanically risky). -- New `SURFACE_TARGET_OFFSET_M` constant and `SURFACE_OFFSET ` command (number 18) for tuning the surface idle position at runtime. +- Runtime profile, PID, balance, and motor settings are configurable from NEXUS/GUI and persisted on ESPA. **Team Contact:** PoliTOcean @ Politecnico di Torino **Maintainers:** Colabella Davide, Benevenga Filippo diff --git a/include/config.h b/include/config.h index 150e27f..846e496 100644 --- a/include/config.h +++ b/include/config.h @@ -42,24 +42,31 @@ constexpr float MOTOR_REVS_PER_MM = constexpr float MOTOR_STEPS_PER_MM = MOTOR_STEPS_PER_REV * MOTOR_MICROSTEP * MOTOR_REVS_PER_MM; -constexpr float MOTOR_TRAVEL_MM = 35.0f; // Normal commanded syringe travel (mm) +constexpr float MOTOR_TRAVEL_MM = 45.0f; // Normal commanded syringe travel (mm) constexpr uint32_t MOTOR_MAX_STEPS = static_cast(MOTOR_TRAVEL_MM * MOTOR_STEPS_PER_MM + 0.5f); -constexpr uint32_t MOTOR_MAX_SPEED = 1800; // Normal operating speed (steps/s) -constexpr uint32_t MOTOR_MAX_ACCELERATION = 1800; // Normal acceleration/deceleration (steps/s^2) -constexpr uint32_t MOTOR_HOMING_SPEED = 1800; // Homing speed (steps/s) +constexpr uint32_t MOTOR_MAX_SPEED = 1400; // Normal operating speed (steps/s) +constexpr uint32_t MOTOR_MAX_ACCELERATION = 1400; // Normal acceleration/deceleration (steps/s^2) +constexpr uint32_t MOTOR_HOMING_SPEED = 1400; // Homing speed (steps/s) constexpr uint16_t MOTOR_ENDSTOP_MARGIN = 10; // Safety margin from endstops (steps) -// Geometria reale: home (motor_pos=0) = siringa retratta, vuota → float galleggia. -// MOTOR_MAX_STEPS = siringa estesa, piena d'acqua → float affonda. -// Convenzione "logica" del PID/profile: u=0 → galleggia (siringa vuota), -// u=1 → affonda (siringa piena). -// La geometria coincide con la convenzione logica: nessuna inversione necessaria. -constexpr bool MOTOR_INVERT_LOGICAL = false; +// Geometria reale (verificata col balance, coerente con l'homing): +// home (motor_pos=0) = piastra lontana dal TOF, acqua spinta fuori → galleggia. +// motor_pos negativo = piastra verso il TOF, prende acqua → affonda. +// La direzione "verso il TOF / prende acqua" è NEGATIVA (vedi homeWithTof fase 1). +// Convenzione "logica" del PID/profile: u=0 → galleggia, u=1 → affonda. +// Quindi u cresce muovendosi in direzione negativa: +// uToMotorPos(0.0f) = 0 (home), uToMotorPos(1.0f) = -(MAX - 2*margin). inline long uToMotorPos(float u) { - const long usable = (long)MOTOR_MAX_STEPS - 2L * (long)MOTOR_ENDSTOP_MARGIN; - const float uPhys = MOTOR_INVERT_LOGICAL ? (1.0f - u) : u; - return (long)MOTOR_ENDSTOP_MARGIN + (long)(uPhys * (float)usable); + const long travel = (long)MOTOR_MAX_STEPS - 2L * (long)MOTOR_ENDSTOP_MARGIN; + return -(long)(u * (float)travel); +} +inline float motorPosToU(long position) { + const long travel = (long)MOTOR_MAX_STEPS - 2L * (long)MOTOR_ENDSTOP_MARGIN; + if (travel <= 0) return 0.0f; + + const float u = -(float)position / (float)travel; + return constrain(u, 0.0f, 1.0f); } constexpr uint32_t MOTOR_HOMING_TIMEOUT = 30000; // Homing timeout (ms) constexpr uint16_t MOTOR_HOMING_TOF_PERIOD_MS = 50; // TOF polling period during homing (ms) @@ -72,10 +79,42 @@ constexpr uint8_t TOF_GPIO1_PIN = 15; // Optional INT pin, unused in constexpr uint8_t TOF_MATRIX_ZONE_COUNT = 16; constexpr uint16_t TOF_ZONE_ENABLE_MASK = 0x0660; // Central zones: 5, 6, 9, 10 constexpr float TOF_DISTANCE_RAW_OFFSET_MM = 6.0f; // Raw distance is this much higher than real distance -constexpr float TOF_HOMING_THRESHOLD = 75.0f; // Homing stop distance: stop when TOF reads ABOVE this (siringa retratta = lontana dal TOF) (mm) +constexpr float TOF_HOMING_THRESHOLD = 70.0f; // Homing stop distance: stop when TOF reads ABOVE this (siringa retratta = lontana dal TOF) (mm). Lo stop reale cade qualche mm sopra (polling 50ms + conferma + risoluzione TOF grezza): a 70 lo stop effettivo ~73-76 mm, con margine sotto TOF_SAFE_RANGE_MAX_MM=82. constexpr float TOF_HOMING_APPROACH_MM = 50.0f; // Approach phase: move toward TOF until reading BELOW this, then start homing (mm) -constexpr float TOF_SAFE_RANGE_MIN_MM = 40.0f; // Safety range lower bound: siringa estesa, troppo vicina al TOF (mm) -constexpr float TOF_SAFE_RANGE_MAX_MM = 85.0f; // Safety range upper bound: siringa retratta, troppo lontana dal TOF (mm). 10 mm sopra TOF_HOMING_THRESHOLD per coprire il rumore TOF post-homing senza spingere il pistone a sbattere meccanicamente. +// Letture TOF consecutive oltre soglia richieste prima di accettare il trigger +// di homing in ciascuna fase. Un singolo campione (frame recuperato, zona valida +// ma rumorosa, riflesso) non deve fermare la fase: serve conferma. Stesso pattern +// di TOF_SAFETY_STOP_SAMPLES. +constexpr uint8_t TOF_HOMING_CONFIRM_SAMPLES = 2; +// Soglie tarate sulla finestra TOF reale misurata in piscina (distanza CORRETTA, +// cioè raw - TOF_DISTANCE_RAW_OFFSET_MM): pistone esteso ≈ 29 mm, retratto ≈ 79 mm +// (raw 85). Pendenza ≈ 1.1 mm TOF per mm motore. +// MIN: sotto il fondo corsa esteso si APRE IL TAPPO ed entra acqua. 32 mm lascia +// ~3 mm di margine sopra il 29 fisico: al raggiungimento si fa uno STOP PULITO +// (clamp, niente emergency) per fermare il pistone PRIMA del tappo senza abortire +// la missione (vedi MotionController::tofGuard / TofGuard::ExtendLimit). +constexpr float TOF_SAFE_RANGE_MIN_MM = 32.0f; // Safety range lower bound: siringa estesa, vicina al TOF — oltre = tappo aperto (mm) +// MAX: massimo gestibile raw 85 → 79 mm corretta (offset 6 mm). 82 mm = ~3 mm +// sopra il 79 fisico, copre il rumore post-homing. Verso il retratto il motore +// può sforare ancora parecchio: oltre 82 è un'ANOMALIA (passi persi, verso +// sbagliato) → emergency stop (TofGuard::Emergency). +constexpr float TOF_SAFE_RANGE_MAX_MM = 82.0f; // Safety range upper bound: siringa retratta, lontana dal TOF — oltre = anomalia (mm) +// Soglia (numero di letture TOF consecutive fuori range) prima di scatenare un +// emergency stop durante un movimento. Un singolo campione fuori soglia in +// acqua (bolle, riflessi, torbidità) non deve fermare la missione: serve una +// conferma. Stesso pattern del pressure-stop del balance. +constexpr uint8_t TOF_SAFETY_STOP_SAMPLES = 3; +// Range PID utile: limitiamo l'output u del PID a [MIN, MAX] (anziché [0,1]) +// così la siringa non raggiunge mai gli estremi meccanici che coincidono con +// le soglie TOF di sicurezza (32/82 mm), lasciando margine contro passi persi +// e rumore. Con lo zero a TOF≈75 mm e pendenza ~1.1, a u=0.92 il TOF ≈ 40 mm, +// sopra MIN=32 con margine; a u=1.0 ≈ 34 mm (ecco perché MAX resta < 1.0). +// PID_U_MIN=0 così il PID può svuotare completamente la siringa per risalire +// (un MIN>0 lasciava il float troppo galleggiante e nascondeva la dinamica +// reale agli u bassi). Il MAX resta sotto 1.0 per margine verso la soglia TOF +// in piena estensione. +constexpr float PID_U_MIN = 0.0f; +constexpr float PID_U_MAX = 0.92f; // --------------------------------------------------------------------------- // BALANCE / PURGE CONTROL @@ -90,56 +129,65 @@ constexpr uint16_t BALANCE_PRESSURE_SAMPLE_PERIOD_MS = 50; // Bar02 polling peri constexpr uint16_t PERIOD_MEASUREMENT = 100; // Between depth readings constexpr uint16_t PERIOD_CONN_CHECK = 500; // Between idle acknowledgements -#ifdef POOL_TEST_PROFILE -constexpr uint16_t PERIOD_EEPROM_WRITE = 2000; // Faster hold checks for shallow pool tests -constexpr uint16_t PROFILE_LOG_PERIOD_MS = 500; // Denser flash log for short pool runs -constexpr uint16_t DATA_PACKET_PERIOD_MS = 2000; // Denser replay packets for short pool runs -#else constexpr uint16_t PERIOD_EEPROM_WRITE = 5000; // Between EEPROM writes / hold checks constexpr uint16_t PROFILE_LOG_PERIOD_MS = 1000; // Between flash profile writes constexpr uint16_t DATA_PACKET_PERIOD_MS = 5000; // Packet cadence shown to judges -#endif // --------------------------------------------------------------------------- // PID TUNING (output normalizzato in [0,1] = frazione di corsa siringa) // --------------------------------------------------------------------------- -// Kp/Ki/Kd mutabili a runtime via CMD_UPDATE_PID (8); periodMs e alphaD via -// CMD_UPDATE_PID_EXT (14). Espressi in "frazione di corsa per metro di errore", -// portabili tra siringhe — se cambia MOTOR_MAX_STEPS, i guadagni restano validi. +// Defaults runtime per PID_CONFIG_SET/GET. Espressi in "frazione di corsa per +// metro di errore", portabili tra siringhe — se cambia MOTOR_MAX_STEPS, i +// guadagni restano validi. constexpr uint16_t PID_PERIOD_DEFAULT_MS = 50; // Default tick PID (ms) -constexpr float PID_KP_DEFAULT = 0.17f; // frazione_corsa / m -constexpr float PID_KI_DEFAULT = 0.0f; // frazione_corsa / (m·s) -constexpr float PID_KD_DEFAULT = 0.13f; // frazione_corsa / (m/s) +// Tuning iterato sui test in piscina: +// - Kp=0.17 (originale): troppo debole, il float non si muoveva (u≈0.03). +// - Kp=2.0 Kd=0.13: il float si muoveva ma OSCILLAVA (±15cm, pompaggio) — +// Kp troppo alto e Kd insufficiente per un sistema lento come il float. +// - Kp=1.0 Kd=0.5: smorzato ma si "sedeva" in superficie (ripresa debole). +// - Kp=1.7 Ki=0.1 Kd=0.3: converge sul target con oscillazione finale ±1cm. +// - Kp=1.0 Kd=2.0 (dump 2026-06-19): in discesa il float sfonda il target di +// ~0.7 m (picco 2.22 m con target 1.5). Dal log ad alta risoluzione il +// problema NON è u saturo a fondo corsa (u sale gradualmente, max ~0.93, poi +// scende da solo): è INERZIA idrodinamica — u è già a 0 ma il float continua +// ad affondare per il momento accumulato. Serve frenare PRIMA, quindi più Kd. +// - Kp=0.5 Kd=3.0 (attuale): meno spinta proporzionale in discesa + freno +// derivativo più deciso, per anticipare l'arresto e contenere l'overshoot. +// Affinare ancora a runtime con PID_CONFIG_SET se serve. +constexpr float PID_KP_DEFAULT = 0.5f; // frazione_corsa / m +constexpr float PID_KI_DEFAULT = 0.1f; // frazione_corsa / (m·s) +constexpr float PID_KD_DEFAULT = 3.0f; // frazione_corsa / (m/s) constexpr float PID_INTEGRAL_LIMIT = 5.0f; // m·s (bound conservativo) constexpr float PID_ALPHA_D_DEFAULT = 0.25f; // LPF IIR coeff per derivata constexpr float PID_U_NEUTRAL = 0.011f;// kick-start offset (~500/47100) constexpr float PID_MIN_RETARGET_FRAC = 0.001f;// dead-band ri-comando (frazione corsa) +// Pre-posizionamento siringa all'inizio della discesa PID (kick-start): u alto +// per avviare l'affondamento. Era 0.979 (siringa quasi piena) ma faceva tirare +// il float dritto fino al fondo prima che il PID frenasse (overshoot ~26cm in +// vasca). Ridotto a 0.30, poi a 0.15: con 0.30 + spinta del PID (Kp*error) la +// discesa partiva ancora troppo veloce e si sfondava il target di oltre 1 m. +constexpr float PID_DESCENT_KICK_U = 0.15f; // --------------------------------------------------------------------------- // FLOAT PHYSICAL / MISSION CONSTANTS // --------------------------------------------------------------------------- -constexpr float FLOAT_LENGTH = 0.51f; // Bottom-to-sensor height (m) +constexpr float FLOAT_LENGTH = 0.49f; // Bottom-to-sensor height (m): corpo 48 cm + barometro 1 cm sopra il tappo constexpr float SENSOR_TO_BOTTOM_M = FLOAT_LENGTH; // Pressure sensor to bottom reference -// Geometric offset between physical top of the float and the barometer. -// The Bar02 sits at the top, so this is ~0 m; calibrate on hardware if needed. -constexpr float FLOAT_TOP_TO_SENSOR_M = 0.0f; +// Vertical offset of the physical top of the float relative to the barometer, +// signed so that topDepth = sensorDepth - SENSOR_TO_TOP_M (see sensors.cpp). +// Positive = top is BELOW the sensor; negative = top is ABOVE the sensor. +// Misurato in hardware: il Bar02 sporge 1 cm SOPRA la cima del float, quindi la +// cima è 1 cm più in profondità del sensore → offset NEGATIVO (-0.01 m). +constexpr float FLOAT_TOP_TO_SENSOR_M = -0.01f; constexpr float SENSOR_TO_TOP_M = FLOAT_TOP_TO_SENSOR_M; // Operational target: how deep the *top* of the float should sit below the -// water surface when the float is "floating". Runtime-tunable via +// water surface when the float is "floating"/resting. Runtime-tunable via // CMD_SET_SURFACE_OFFSET / USB SURFACE_OFFSET command — this is the default. -constexpr float SURFACE_TARGET_OFFSET_M = 0.10f; +// 0.15 m tiene l'antenna (~12 cm sopra il tappo) sommersa di qualche cm a riposo, +// così il float non rompe la superficie (penalità -5 punti) in attesa del recupero. +constexpr float SURFACE_TARGET_OFFSET_M = 0.15f; constexpr float DEPTH_EPSILON = 0.01f; // "Stationary" tolerance (m) -#ifdef POOL_TEST_PROFILE -constexpr float POOL_TEST_WATER_DEPTH = 0.70f; // Reference only: assumed test pool depth (m) -constexpr uint8_t PROFILE_MAX_COUNT = 1; // One cycle keeps shallow-pool tests shorter and safer -constexpr float DEPTH_MAX_ERROR = 0.025f; // Narrow tolerance because pool targets are close together -constexpr float TARGET_DEPTH = 0.63f; // Deep hold: bottom reference (m), ~7 cm above a 70 cm floor -constexpr float TARGET_SHALLOW_TOP_DEPTH = 0.06f; // Shallow hold: top reference (m) -constexpr float STAT_TIME = 8.0f; // Short pool hold; actual check cadence is PERIOD_EEPROM_WRITE -constexpr float TIMEOUT_PID_TIME = 45.0f; // Max PID phase time (s) -constexpr float TIMEOUT_ASCENT = 45.0f; // Max ascent + shallow hold time (s) -#else constexpr uint8_t PROFILE_MAX_COUNT = 2; // Profiles before auto-stop constexpr float DEPTH_MAX_ERROR = 0.33f; // MATE depth tolerance (m) constexpr float TARGET_DEPTH = 2.50f; // Deep hold: bottom reference (m) @@ -147,7 +195,9 @@ constexpr float TARGET_SHALLOW_TOP_DEPTH = 0.40f; // Shallow hold: top refere constexpr float STAT_TIME = 30.0f; // MATE hold time at target (s) constexpr float TIMEOUT_PID_TIME = 180.0f; // Max PID phase time (s) constexpr float TIMEOUT_ASCENT = 120.0f; // Max ascent + shallow hold time (s) -#endif +// Sosta finale sotto il pelo (top a SURFACE_TARGET_OFFSET_M) dopo i due profili: +// tiene attivo il PID finché il float non viene recuperato (o scade la finestra). +constexpr float REST_WINDOW_S = 120.0f; // Active surface-rest hold window (s) constexpr float TARGET_SHALLOW_BOTTOM_DEPTH = TARGET_SHALLOW_TOP_DEPTH + SENSOR_TO_BOTTOM_M + SENSOR_TO_TOP_M; @@ -162,6 +212,27 @@ constexpr int8_t TARGET_BOTTOM = -1; // Descend to pool floor constexpr float WATER_DENSITY_FRESH = 997.0f; // kg/m³ constexpr float GRAVITY = 9.80665f; +// --------------------------------------------------------------------------- +// FLOAT SIMULATOR (HIL da banco: motore reale, barometro SIMULATO) +// --------------------------------------------------------------------------- +// Con SIM attivo le letture Bar02 sono sostituite da un modello fisico 2° ordine: +// la siringa (u = motorPosToU(motor.position())) genera spinta netta, la quota +// del SENSORE viene integrata con inerzia idrodinamica e drag quadratico: +// a = SIM_ACCEL_GAIN*(u - SIM_U_NEUTRAL) - SIM_DRAG_QUAD*v*|v| +// v += a*dt ; z += v*dt +// Convenzione coerente col resto del firmware: u > neutral => affonda (z cresce). +// Tutti i parametri sono ritarabili a runtime con SIM_CONFIG senza riflashare. +// Default tarati con la simulazione offline del loop chiuso (PID + motore lento +// ~2 mm/s + modello): a uNeutral=0.35 la discesa converge ~2.5 m con overshoot +// ~0.28 m (realistico, sink-biased come il float vero) e l'effetto di Kd è ben +// visibile. RITARALI sul TUO float con SIM_CONFIG: uNeutral = u a cui la siringa +// regge la quota senza muoversi (lo vedi col valore di u a regime in PID_HOLD). +constexpr float SIM_U_NEUTRAL = 0.35f; // u di galleggiamento neutro (net buoyancy = 0) +constexpr float SIM_ACCEL_GAIN = 0.05f; // accelerazione [m/s^2] per unità di (u - neutral) +constexpr float SIM_DRAG_QUAD = 1.50f; // coeff. drag quadratico [1/m] +constexpr float SIM_POOL_DEPTH = 3.00f; // profondità vasca simulata [m] (vasca NRC ~3 m) +constexpr float SIM_MAX_DT_S = 0.20f; // clamp dt integrazione per stabilità [s] + // --------------------------------------------------------------------------- // NETWORK / OTA // --------------------------------------------------------------------------- @@ -170,13 +241,13 @@ constexpr char WIFI_PASSWORD[] = "politocean"; // constexpr uint8_t MAC_ESPB[6] = {0xEC, 0xE3, 0x34, 0xCE, 0x59, 0x1C}; constexpr uint8_t MAC_ESPB[6] = {0x88, 0x57, 0x21, 0x84, 0x8C, 0xE8}; -constexpr uint8_t MAC_ESPA[6] = {0x88, 0x57, 0x21, 0x84, 0x83, 0x8C}; +constexpr uint8_t MAC_ESPA[6] = {0x88, 0x57, 0x21, 0x84, 0x7E, 0xCC}; constexpr uint8_t ESPNOW_CHANNEL = 1; // --------------------------------------------------------------------------- // EEPROM / DATA // --------------------------------------------------------------------------- -constexpr char COMPANY_NUMBER[] = "EX10"; +constexpr char COMPANY_NUMBER[] = "EX12"; constexpr char FLASH_LOG_PATH[] = "/mission/current_profile.csv"; // EEPROM_SIZE and sensor_data struct come from float_common.h diff --git a/include/float_common.h b/include/float_common.h index c929087..2b71500 100644 --- a/include/float_common.h +++ b/include/float_common.h @@ -2,6 +2,7 @@ #define FLOAT_COMMON_H #include +#include /* ******************************************************************************* @@ -28,17 +29,23 @@ enum FloatCommand : uint8_t { CMD_AUTO_MODE = 5, CMD_SEND_PACKAGE = 6, CMD_OTA = 7, - CMD_UPDATE_PID = 8, - CMD_SET_SPEED = 9, + CMD_PID_CONFIG_SET = 8, + CMD_RESERVED_9 = 9, CMD_TEST_STEPS = 10, CMD_DEBUG_MODE = 11, CMD_HOME = 12, CMD_STOP = 13, - CMD_UPDATE_PID_EXT = 14, + CMD_PID_CONFIG_GET = 14, CMD_SYRINGE_SET = 15, CMD_PID_HOLD = 16, CMD_PID_STEP = 17, CMD_SET_SURFACE_OFFSET = 18, + CMD_PROFILE_SET = 19, + CMD_PROFILE_GET = 20, + CMD_BALANCE_CONFIG_SET = 21, + CMD_BALANCE_CONFIG_GET = 22, + CMD_MOTOR_CONFIG_SET = 23, + CMD_MOTOR_CONFIG_GET = 24, }; // List of messages for the ESPA acknowledgements: CS has to be aware of these @@ -49,17 +56,22 @@ enum FloatCommand : uint8_t { #define CMD4_ACK "CMD4_RECVD" #define CMD5_ACK "SWITCH_AM_RECVD" #define CMD7_ACK "TRY_UPLOAD_RECVD" -#define CMD8_ACK "CHNG_PARMS_RECVD" -#define CMD9_ACK "TEST_FREQ_RECVD" +#define CMD8_ACK "PID_CONFIG_SET_RECVD" +#define CMD8_ERR "PID_CONFIG_SET_ERR" #define CMD10_ACK "TEST_STEPS_RECVD" #define CMD11_ACK "DEBUG_MODE_RECVD" #define CMD12_ACK "HOME_RECVD" #define CMD13_ACK "STOP_RECVD" -#define CMD14_ACK "CHNG_PID_EXT_RECVD" #define CMD15_ACK "SYRINGE_SET_RECVD" #define CMD16_ACK "PID_HOLD_RECVD" #define CMD17_ACK "PID_STEP_RECVD" #define CMD18_ACK "SURFACE_OFF_RECVD" +#define CMD19_ACK "PROFILE_SET_RECVD" +#define CMD19_ERR "PROFILE_SET_ERR" +#define CMD21_ACK "BALANCE_CONFIG_SET_RECVD" +#define CMD21_ERR "BALANCE_CONFIG_SET_ERR" +#define CMD23_ACK "MOTOR_CONFIG_SET_RECVD" +#define CMD23_ERR "MOTOR_CONFIG_SET_ERR" // Sensor data structure typedef struct sensor_data { @@ -74,32 +86,115 @@ typedef struct input_message { char message[OUTPUT_LEN]; } input_message; -typedef struct output_message { - float params[3]; +struct EmptyPayload { + uint8_t reserved; +}; + +struct PidConfigPayload { + float kp; + float ki; + float kd; + float periodMs; + float alphaD; + float integralLimit; + float minRetargetFrac; + float uNeutral; +}; + +struct BalanceConfigPayload { + uint32_t holdMs; + float stopPressureDeltaKpa; + uint8_t stopPressureSamples; + uint16_t samplePeriodMs; +}; + +struct MotorConfigPayload { + uint32_t maxSpeed; + uint32_t maxAcceleration; + uint32_t homingSpeed; + uint32_t testSpeed; +}; + +struct TestStepsPayload { int32_t steps; - uint16_t freq; - uint8_t command = CMD_IDLE; +}; + +struct SyringeSetPayload { + float uNorm; + float durationS; +}; + +struct PidHoldPayload { + float depthM; + float durationS; +}; + +struct PidStepPayload { + float depthM; +}; + +struct SurfaceOffsetPayload { + float meters; +}; + +// Ordine dei campi invariato rispetto alla versione precedente (solo rinominati): +// il layout binario e il contratto di serializzazione con la GUI restano compatibili. +// [essenziale] = si imposta per ogni prova (vedi regolamento MATE Task 4.1). +// [avanzato] = safety/calibrazione, cambia di rado (nella GUI sta in "Avanzate"). +struct ProfileSetPayload { + uint8_t profileCount; // [avanzato] n. immersioni complete + float descentTargetM; // [essenziale] target discesa, riferito al FONDO del float + float ascentTargetM; // [essenziale] target risalita, riferito al TOP del float + float depthToleranceM; // [essenziale] banda +/- attorno al target + float holdTimeS; // [essenziale] tempo di mantenimento al target + float descentTimeoutS; // [avanzato] timeout assoluto fase discesa (hold incluso) + float ascentTimeoutS; // [avanzato] timeout assoluto fase risalita (hold incluso) + float surfaceRestOffsetM; // [avanzato] quanto il top del float resta sotto pelo a riposo +}; + +union FloatCommandPayload { + EmptyPayload empty; + PidConfigPayload pidConfig; + BalanceConfigPayload balanceConfig; + MotorConfigPayload motorConfig; + TestStepsPayload testSteps; + SyringeSetPayload syringeSet; + PidHoldPayload pidHold; + PidStepPayload pidStep; + SurfaceOffsetPayload surfaceOffset; + ProfileSetPayload profileSet; +}; + +typedef struct output_message { + FloatCommand command = CMD_IDLE; + FloatCommandPayload payload; } output_message; -// MAC addresses - UPDATE THESE TO YOUR ACTUAL MAC ADDRESSES -extern uint8_t espA_mac[6]; -extern uint8_t espB_mac[6]; - -// LED States for better status indication -enum FloatLEDState { - LED_OFF, - LED_INIT, // Green solid - Initializing - LED_IDLE, // Green solid - Ready/Idle - LED_IDLE_DATA, // Green fast blink - Idle with data - LED_LOW_BATTERY, // Red solid - Low battery - LED_ERROR, // Red fast blink - Error state - LED_PROFILE, // Blue blink - Running profile - LED_AUTO_MODE, // Yellow blink - Auto mode active - LED_HOMING, // Purple blink - Motor homing - LED_MOTOR_MOVING, // Purple solid - Motor moving - LED_PID_CONTROL, // Cyan blink - PID active - LED_COMMUNICATION, // White blink - Communicating - LED_OTA_MODE // Orange blink - OTA update mode +inline output_message makeOutputMessage(FloatCommand command) { + output_message message; + memset(&message, 0, sizeof(message)); + message.command = command; + return message; +} + +static_assert(sizeof(output_message) <= 250, "output_message must fit in one ESP-NOW packet"); + +// Shared logical LED states. ESPA maps them to the RGB LED; ESPB maps the +// subset it can represent on the built-in single-colour LED. +enum class LEDState : uint8_t { + OFF, // Off / disabled + INIT, // Green solid or boot blinks - Initializing + IDLE, // Green solid / ESPB solid on - Ready/idle + IDLE_WITH_DATA, // Green fast blink - Idle with stored profile data + LOW_BATTERY, // Red solid - Battery voltage below threshold + ERROR, // Red fast blink / ESPB fast blink - Error state + PROFILE, // Blue solid - Running non-PID profile phase + AUTO_MODE, // Yellow blink - Auto mode active + HOMING, // Purple blink - Motor homing + MOTOR_MOVING, // Purple solid - Motor moving + PID_CONTROL, // Cyan blink - PID depth control active + COMMUNICATION, // White solid - Communicating + OTA_MODE, // Orange blink - OTA update mode }; #endif diff --git a/lib/DebugSerial/library.json b/lib/DebugSerial/library.json index dcb42e9..c8e397e 100644 --- a/lib/DebugSerial/library.json +++ b/lib/DebugSerial/library.json @@ -11,7 +11,7 @@ ], "repository": { "type": "git", - "url": "https://github.com/politocean/Float_2025.git" + "url": "https://github.com/politocean/Float.git" }, "frameworks": ["arduino"], "platforms": ["espressif32"], diff --git a/lib/comms/include/comms.h b/lib/comms/include/comms.h index cdde787..9dae72f 100644 --- a/lib/comms/include/comms.h +++ b/lib/comms/include/comms.h @@ -27,9 +27,11 @@ class CommsManager { // Returns true if the peer acknowledged delivery. bool sendMessage(const char* message, uint32_t timeoutMs = 1000); - // Access the last received command (set by the ESP-NOW receive callback) - const output_message& lastCommand() const { return _received; } - void clearCommand() { _received.command = CMD_IDLE; } + // Access the last received command (set by the ESP-NOW receive callback). + // Returns a snapshot by value: the receive callback runs in the WiFi task, + // so the read/write must be guarded to avoid a torn struct. + output_message lastCommand() const; + void clearCommand(); // Outgoing packet — callers fill status_to_send.charge before calling sendMessage() input_message status_to_send; @@ -43,6 +45,10 @@ class CommsManager { output_message _received; volatile int8_t _sendResult = -1; // -1=pending, 0=fail, 1=success + // Guards _received against concurrent access between the WiFi-task receive + // callback (writer) and the main loop (reader). + mutable portMUX_TYPE _recvMux = portMUX_INITIALIZER_UNLOCKED; + void _initEspNow(); void _deInitEspNow(); void _reInitEspNow(); diff --git a/lib/comms/src/comms.cpp b/lib/comms/src/comms.cpp index fe9980f..c19182a 100644 --- a/lib/comms/src/comms.cpp +++ b/lib/comms/src/comms.cpp @@ -31,7 +31,7 @@ void setEspNowChannel() { CommsManager::CommsManager() { _instance = this; memset(&status_to_send, 0, sizeof(status_to_send)); - memset(&_received, 0, sizeof(_received)); + _received = makeOutputMessage(CMD_IDLE); } // --------------------------------------------------------------------------- @@ -90,9 +90,26 @@ void CommsManager::_reInitEspNow() { Serial.println("ESP-NOW re-initialised"); } +// --------------------------------------------------------------------------- +output_message CommsManager::lastCommand() const { + portENTER_CRITICAL(&_recvMux); + const output_message snapshot = _received; + portEXIT_CRITICAL(&_recvMux); + return snapshot; +} + +// --------------------------------------------------------------------------- +void CommsManager::clearCommand() { + const output_message idle = makeOutputMessage(CMD_IDLE); + portENTER_CRITICAL(&_recvMux); + _received = idle; + portEXIT_CRITICAL(&_recvMux); +} + // --------------------------------------------------------------------------- bool CommsManager::sendMessage(const char* message, uint32_t timeoutMs) { strncpy(status_to_send.message, message, sizeof(status_to_send.message) - 1); + status_to_send.message[sizeof(status_to_send.message) - 1] = '\0'; _sendResult = -1; esp_err_t err = esp_now_send( @@ -175,6 +192,8 @@ void CommsManager::_onDataSent(const uint8_t* /*mac*/, esp_now_send_status_t sta void CommsManager::_onDataRecv(const uint8_t* /*mac*/, const uint8_t* data, int len) { if (_instance && len == sizeof(output_message)) { + portENTER_CRITICAL(&_instance->_recvMux); memcpy(&_instance->_received, data, sizeof(output_message)); + portEXIT_CRITICAL(&_instance->_recvMux); } } diff --git a/lib/espb_bridge_core/include/espb_bridge_core.h b/lib/espb_bridge_core/include/espb_bridge_core.h index 6b4216a..3027f93 100644 --- a/lib/espb_bridge_core/include/espb_bridge_core.h +++ b/lib/espb_bridge_core/include/espb_bridge_core.h @@ -37,7 +37,7 @@ struct EspbBridgeState { struct EspbProtocolCommand { const char* commandText; - uint8_t commandCode; + FloatCommand commandCode; const char* expectedAck; }; diff --git a/lib/espb_bridge_core/src/espb_bridge_core.cpp b/lib/espb_bridge_core/src/espb_bridge_core.cpp index 1f41a94..0309a7f 100644 --- a/lib/espb_bridge_core/src/espb_bridge_core.cpp +++ b/lib/espb_bridge_core/src/espb_bridge_core.cpp @@ -16,7 +16,7 @@ */ namespace { -constexpr size_t COMMAND_BUFFER_SIZE = 96; +constexpr size_t COMMAND_BUFFER_SIZE = 192; constexpr EspbProtocolCommand PROTOCOL_COMMANDS[] = { {"GO", CMD_GO, CMD1_ACK}, @@ -26,23 +26,24 @@ constexpr EspbProtocolCommand PROTOCOL_COMMANDS[] = { {"SWITCH_AUTO_MODE", CMD_AUTO_MODE, CMD5_ACK}, {"SEND_PACKAGE", CMD_SEND_PACKAGE, "JSON_LIVE_PACKET"}, {"TRY_UPLOAD", CMD_OTA, CMD7_ACK}, - {"PARAMS", CMD_UPDATE_PID, CMD8_ACK}, - {"TEST_FREQ", CMD_SET_SPEED, CMD9_ACK}, + {"PID_CONFIG_SET", CMD_PID_CONFIG_SET, CMD8_ACK}, {"TEST_STEPS", CMD_TEST_STEPS, CMD10_ACK}, {"DEBUG", CMD_DEBUG_MODE, CMD11_ACK}, {"HOME_MOTOR", CMD_HOME, CMD12_ACK}, {"STOP", CMD_STOP, CMD13_ACK}, - {"PARAMS_EXT", CMD_UPDATE_PID_EXT, CMD14_ACK}, + {"PID_CONFIG_GET", CMD_PID_CONFIG_GET, "PID_CONFIG_JSON"}, {"SYRINGE_SET", CMD_SYRINGE_SET, CMD15_ACK}, {"PID_HOLD", CMD_PID_HOLD, CMD16_ACK}, {"PID_STEP", CMD_PID_STEP, CMD17_ACK}, {"SURFACE_OFFSET", CMD_SET_SURFACE_OFFSET, CMD18_ACK}, + {"PROFILE_SET", CMD_PROFILE_SET, CMD19_ACK}, + {"PROFILE_GET", CMD_PROFILE_GET, "PROFILE_JSON"}, + {"BALANCE_CONFIG_SET", CMD_BALANCE_CONFIG_SET, CMD21_ACK}, + {"BALANCE_CONFIG_GET", CMD_BALANCE_CONFIG_GET, "BALANCE_CONFIG_JSON"}, + {"MOTOR_CONFIG_SET", CMD_MOTOR_CONFIG_SET, CMD23_ACK}, + {"MOTOR_CONFIG_GET", CMD_MOTOR_CONFIG_GET, "MOTOR_CONFIG_JSON"}, }; -void zeroMessage(output_message& message) { - memset(&message, 0, sizeof(message)); -} - void copyTrimmedCommand(const char* input, char* output, size_t outputSize) { if (outputSize == 0) { return; @@ -92,18 +93,17 @@ bool hasNoExtraToken() { return strtok(nullptr, " ") == nullptr; } -EspbParsedCommand makeForwardCommand(uint8_t commandCode) { +EspbParsedCommand makeForwardCommand(FloatCommand commandCode) { EspbParsedCommand parsed; parsed.type = EspbParsedCommandType::ForwardToEspA; - zeroMessage(parsed.message); - parsed.message.command = commandCode; + parsed.message = makeOutputMessage(commandCode); return parsed; } } EspbParsedCommand espbParseSerialCommand(const char* line) { EspbParsedCommand parsed; - zeroMessage(parsed.message); + parsed.message = makeOutputMessage(CMD_IDLE); if (line == nullptr) { return parsed; @@ -128,41 +128,6 @@ EspbParsedCommand espbParseSerialCommand(const char* line) { return parsed; } - if (strcmp(token, "PARAMS") == 0) { - float kp = 0.0f; - float ki = 0.0f; - float kd = 0.0f; - if (!parseFloatToken(strtok(nullptr, " "), kp) || - !parseFloatToken(strtok(nullptr, " "), ki) || - !parseFloatToken(strtok(nullptr, " "), kd) || - !hasNoExtraToken()) { - return parsed; - } - - parsed = makeForwardCommand(CMD_UPDATE_PID); - parsed.message.params[0] = kp; - parsed.message.params[1] = ki; - parsed.message.params[2] = kd; - return parsed; - } - - if (strcmp(token, "PARAMS_EXT") == 0) { - // PARAMS_EXT period_ms alpha_d (third param reserved, always 0) - float periodMs = 0.0f; - float alphaD = 0.0f; - if (!parseFloatToken(strtok(nullptr, " "), periodMs) || - !parseFloatToken(strtok(nullptr, " "), alphaD) || - !hasNoExtraToken()) { - return parsed; - } - - parsed = makeForwardCommand(CMD_UPDATE_PID_EXT); - parsed.message.params[0] = periodMs; - parsed.message.params[1] = alphaD; - parsed.message.params[2] = 0.0f; - return parsed; - } - if (strcmp(token, "SYRINGE_SET") == 0) { // SYRINGE_SET float u = 0.0f; @@ -173,9 +138,8 @@ EspbParsedCommand espbParseSerialCommand(const char* line) { return parsed; } parsed = makeForwardCommand(CMD_SYRINGE_SET); - parsed.message.params[0] = u; - parsed.message.params[1] = dur; - parsed.message.params[2] = 0.0f; + parsed.message.payload.syringeSet.uNorm = u; + parsed.message.payload.syringeSet.durationS = dur; return parsed; } @@ -189,9 +153,8 @@ EspbParsedCommand espbParseSerialCommand(const char* line) { return parsed; } parsed = makeForwardCommand(CMD_PID_HOLD); - parsed.message.params[0] = depth; - parsed.message.params[1] = dur; - parsed.message.params[2] = 0.0f; + parsed.message.payload.pidHold.depthM = depth; + parsed.message.payload.pidHold.durationS = dur; return parsed; } @@ -203,9 +166,7 @@ EspbParsedCommand espbParseSerialCommand(const char* line) { return parsed; } parsed = makeForwardCommand(CMD_PID_STEP); - parsed.message.params[0] = depth; - parsed.message.params[1] = 0.0f; - parsed.message.params[2] = 0.0f; + parsed.message.payload.pidStep.depthM = depth; return parsed; } @@ -217,22 +178,111 @@ EspbParsedCommand espbParseSerialCommand(const char* line) { return parsed; } parsed = makeForwardCommand(CMD_SET_SURFACE_OFFSET); - parsed.message.params[0] = offset; - parsed.message.params[1] = 0.0f; - parsed.message.params[2] = 0.0f; + parsed.message.payload.surfaceOffset.meters = offset; + return parsed; + } + + if (strcmp(token, "PROFILE_SET") == 0) { + // PROFILE_SET + long count = 0; + float values[7] = {}; + if (!parseLongToken(strtok(nullptr, " "), count) || + count < 1 || count > 10) { + return parsed; + } + + for (float& value : values) { + if (!parseFloatToken(strtok(nullptr, " "), value)) { + return parsed; + } + } + + if (!hasNoExtraToken()) { + return parsed; + } + + parsed = makeForwardCommand(CMD_PROFILE_SET); + parsed.message.payload.profileSet.profileCount = static_cast(count); + parsed.message.payload.profileSet.descentTargetM = values[0]; + parsed.message.payload.profileSet.ascentTargetM = values[1]; + parsed.message.payload.profileSet.depthToleranceM = values[2]; + parsed.message.payload.profileSet.holdTimeS = values[3]; + parsed.message.payload.profileSet.descentTimeoutS = values[4]; + parsed.message.payload.profileSet.ascentTimeoutS = values[5]; + parsed.message.payload.profileSet.surfaceRestOffsetM = values[6]; + return parsed; + } + + if (strcmp(token, "PID_CONFIG_SET") == 0) { + float values[8] = {}; + for (float& value : values) { + if (!parseFloatToken(strtok(nullptr, " "), value)) { + return parsed; + } + } + + if (!hasNoExtraToken()) { + return parsed; + } + + parsed = makeForwardCommand(CMD_PID_CONFIG_SET); + parsed.message.payload.pidConfig.kp = values[0]; + parsed.message.payload.pidConfig.ki = values[1]; + parsed.message.payload.pidConfig.kd = values[2]; + parsed.message.payload.pidConfig.periodMs = values[3]; + parsed.message.payload.pidConfig.alphaD = values[4]; + parsed.message.payload.pidConfig.integralLimit = values[5]; + parsed.message.payload.pidConfig.minRetargetFrac = values[6]; + parsed.message.payload.pidConfig.uNeutral = values[7]; + return parsed; + } + + if (strcmp(token, "BALANCE_CONFIG_SET") == 0) { + long holdMs = 0; + float stopDeltaKpa = 0.0f; + long stopSamples = 0; + long samplePeriodMs = 0; + if (!parseLongToken(strtok(nullptr, " "), holdMs) || + !parseFloatToken(strtok(nullptr, " "), stopDeltaKpa) || + !parseLongToken(strtok(nullptr, " "), stopSamples) || + !parseLongToken(strtok(nullptr, " "), samplePeriodMs) || + holdMs < 0 || + stopSamples < 0 || stopSamples > UINT8_MAX || + samplePeriodMs < 0 || samplePeriodMs > UINT16_MAX || + !hasNoExtraToken()) { + return parsed; + } + + parsed = makeForwardCommand(CMD_BALANCE_CONFIG_SET); + parsed.message.payload.balanceConfig.holdMs = static_cast(holdMs); + parsed.message.payload.balanceConfig.stopPressureDeltaKpa = stopDeltaKpa; + parsed.message.payload.balanceConfig.stopPressureSamples = static_cast(stopSamples); + parsed.message.payload.balanceConfig.samplePeriodMs = static_cast(samplePeriodMs); return parsed; } - if (strcmp(token, "TEST_FREQ") == 0) { - long freq = 0; - if (!parseLongToken(strtok(nullptr, " "), freq) || - freq < 0 || freq > UINT16_MAX || + if (strcmp(token, "MOTOR_CONFIG_SET") == 0) { + long maxSpeed = 0; + long maxAcceleration = 0; + long homingSpeed = 0; + long testSpeed = 0; + if (!parseLongToken(strtok(nullptr, " "), maxSpeed) || + !parseLongToken(strtok(nullptr, " "), maxAcceleration) || + !parseLongToken(strtok(nullptr, " "), homingSpeed) || + !parseLongToken(strtok(nullptr, " "), testSpeed) || + maxSpeed < 0 || + maxAcceleration < 0 || + homingSpeed < 0 || + testSpeed < 0 || !hasNoExtraToken()) { return parsed; } - parsed = makeForwardCommand(CMD_SET_SPEED); - parsed.message.freq = static_cast(freq); + parsed = makeForwardCommand(CMD_MOTOR_CONFIG_SET); + parsed.message.payload.motorConfig.maxSpeed = static_cast(maxSpeed); + parsed.message.payload.motorConfig.maxAcceleration = static_cast(maxAcceleration); + parsed.message.payload.motorConfig.homingSpeed = static_cast(homingSpeed); + parsed.message.payload.motorConfig.testSpeed = static_cast(testSpeed); return parsed; } @@ -245,20 +295,21 @@ EspbParsedCommand espbParseSerialCommand(const char* line) { } parsed = makeForwardCommand(CMD_TEST_STEPS); - parsed.message.steps = static_cast(steps); + parsed.message.payload.testSteps.steps = static_cast(steps); return parsed; } for (const EspbProtocolCommand& command : PROTOCOL_COMMANDS) { if (strcmp(token, command.commandText) == 0) { - if (command.commandCode == CMD_UPDATE_PID || - command.commandCode == CMD_UPDATE_PID_EXT || - command.commandCode == CMD_SET_SPEED || + if (command.commandCode == CMD_PID_CONFIG_SET || + command.commandCode == CMD_BALANCE_CONFIG_SET || + command.commandCode == CMD_MOTOR_CONFIG_SET || command.commandCode == CMD_TEST_STEPS || command.commandCode == CMD_SYRINGE_SET || command.commandCode == CMD_PID_HOLD || command.commandCode == CMD_PID_STEP || command.commandCode == CMD_SET_SURFACE_OFFSET || + command.commandCode == CMD_PROFILE_SET || !hasNoExtraToken()) { return parsed; } diff --git a/lib/flash_storage/include/flash_storage.h b/lib/flash_storage/include/flash_storage.h index ac59cda..8785916 100644 --- a/lib/flash_storage/include/flash_storage.h +++ b/lib/flash_storage/include/flash_storage.h @@ -33,7 +33,8 @@ class FlashStorageManager { float pressureKpa, float depthM, const char* phase, - float sensorDepthM); + float sensorDepthM, + float syringeU); // Transmit CSV records whose time matches DATA_PACKET_PERIOD_MS. bool transmitDataPackets(PacketSender sender, uint32_t timeoutMs); diff --git a/lib/flash_storage/src/flash_storage.cpp b/lib/flash_storage/src/flash_storage.cpp index fb6ac0d..0624432 100644 --- a/lib/flash_storage/src/flash_storage.cpp +++ b/lib/flash_storage/src/flash_storage.cpp @@ -21,7 +21,7 @@ namespace { constexpr char CSV_HEADER[] = - "company_number,profile_id,time_s,pressure_kpa,depth_m,phase,sensor_depth_m"; + "company_number,profile_id,time_s,pressure_kpa,depth_m,phase,sensor_depth_m,syringe_u"; constexpr char LITTLEFS_BASE_PATH[] = "/littlefs"; @@ -70,7 +70,8 @@ bool FlashStorageManager::appendRecord(const char* companyNumber, float pressureKpa, float depthM, const char* phase, - float sensorDepthM) { + float sensorDepthM, + float syringeU) { if (!ensureLogFile()) return false; File file = LittleFS.open(FLASH_LOG_PATH, FILE_APPEND); @@ -92,6 +93,8 @@ bool FlashStorageManager::appendRecord(const char* companyNumber, _writeCsvField(file, phase); file.print(','); file.print(sensorDepthM, 2); + file.print(','); + file.print(syringeU, 4); file.println(); const bool ok = file.getWriteError() == 0; @@ -150,6 +153,7 @@ bool FlashStorageManager::transmitDataPackets(PacketSender sender, uint32_t time char* depthM = strtok_r(nullptr, ",", &save); char* phase = strtok_r(nullptr, ",", &save); char* sensorDepthM = strtok_r(nullptr, ",", &save); + char* syringeU = strtok_r(nullptr, ",", &save); if (companyNumber == nullptr || profileId == nullptr || timeS == nullptr || pressureKpa == nullptr || depthM == nullptr || phase == nullptr) { @@ -172,14 +176,16 @@ bool FlashStorageManager::transmitDataPackets(PacketSender sender, uint32_t time "\"pressure_kpa\":%.2f," "\"depth_m\":%.2f," "\"phase\":\"%s\"," - "\"sensor_depth_m\":%.2f}", + "\"sensor_depth_m\":%.2f," + "\"syringe_u\":%.4f}", companyNumber, static_cast(atoi(profileId)), atof(timeS), atof(pressureKpa), atof(depthM), phase, - sensorDepthM == nullptr ? 0.0 : atof(sensorDepthM)); + sensorDepthM == nullptr ? 0.0 : atof(sensorDepthM), + syringeU == nullptr ? 0.0 : atof(syringeU)); sender(packet, timeoutMs); packetCount++; diff --git a/lib/led/include/led.h b/lib/led/include/led.h index 7e3d555..54355d2 100644 --- a/lib/led/include/led.h +++ b/lib/led/include/led.h @@ -2,6 +2,7 @@ #include #include +#include "float_common.h" /* ******************************************************************************* @@ -12,22 +13,6 @@ ******************************************************************************* */ -enum class LEDState : uint8_t { - OFF, - INIT, - IDLE, - IDLE_WITH_DATA, - LOW_BATTERY, - ERROR, - PROFILE, - AUTO_MODE, - HOMING, - MOTOR_MOVING, - PID_CONTROL, - COMMUNICATION, - OTA_MODE, -}; - class LEDController { public: LEDController(uint8_t rPin, uint8_t gPin, uint8_t bPin); @@ -46,4 +31,4 @@ class LEDController { }; // Singleton — defined in led.cpp, used throughout the project -extern LEDController ledController; \ No newline at end of file +extern LEDController ledController; diff --git a/lib/motion_control/include/motion_control.h b/lib/motion_control/include/motion_control.h index 47df707..e730201 100644 --- a/lib/motion_control/include/motion_control.h +++ b/lib/motion_control/include/motion_control.h @@ -12,16 +12,31 @@ ******************************************************************************* */ +// Esito della supervisione TOF durante un movimento. L'azione è ASIMMETRICA +// perché i due estremi fisici sono diversi: +// - limite inferiore (siringa estesa, vicina al TOF): oltre si apre il tappo +// ed entra acqua → ExtendLimit, il chiamante fa uno STOP PULITO (ferma il +// pistone, NON abortisce la missione). +// - limite superiore (siringa retratta, lontana): il motore può sforare ancora +// senza danno immediato, ma oltre soglia è un'anomalia → Emergency, lo stop +// d'emergenza è già scattato dentro tofGuard(). +enum class TofGuard { Ok, ExtendLimit, Emergency }; + class MotionController { public: MotionController(MotorController& motor, TofSensor& tof); + // Supervisione TOF da chiamare nel loop di OGNI movimento (campiona al più + // ogni MOTOR_HOMING_TOF_PERIOD_MS tramite la guardia su lastTofSampleMs). + // context è solo per i log. Vedi enum TofGuard per la semantica dell'esito. + TofGuard tofGuard(unsigned long nowMs, unsigned long& lastTofSampleMs, const char* context); + bool homeWithTof(float stopPressureKpa = 0.0f, bool* pressureStop = nullptr, uint8_t* pressureStopSamples = nullptr); bool waitForMotor(uint32_t timeoutMs); bool moveToMax(uint32_t timeoutMs = 0, float stopPressureKpa = 0.0f, bool* pressureStop = nullptr, uint8_t* pressureStopSamples = nullptr); bool moveToWithTimeout(long targetPosition, uint32_t timeoutMs, bool keepOutputsEnabled = false); bool manualStepTest(long steps, uint32_t speed); - bool balance(uint32_t holdMs); + bool balance(); bool motionAllowed(); bool emergencyStopActive() const { return _emergencyStop; } @@ -30,17 +45,37 @@ class MotionController { void serviceEmergencyStop(); bool remoteStopRequested(); + // Diagnostica dell'ultimo emergency stop, per loggarla nel flash CSV: il + // reason è stampato solo su Serial in tempo reale, ma in piscina la USB è + // scollegata, quindi va salvato. _lastStopTofMm vale -1 se lo stop non è + // stato causato dal TOF (timeout, remote stop, ...). + const char* lastStopReason() const { return _lastStopReason; } + float lastStopTofMm() const { return _lastStopTofMm; } + private: MotorController& _motor; TofSensor& _tof; bool _emergencyStop = false; + const char* _lastStopReason = ""; + float _lastStopTofMm = -1.0f; + // Letture TOF consecutive fuori range: un emergency stop scatta solo dopo + // TOF_SAFETY_STOP_SAMPLES conferme, per ignorare glitch singoli in acqua. + uint8_t _tofOutOfRangeCount = 0; float readPressureKpa(); - bool tofMaxExtensionStopReached(unsigned long nowMs, - unsigned long& lastTofSampleMs, - const char* context); - bool pressureStopReached(float stopPressureKpa, uint8_t* pressureStopSamples = nullptr); - bool waitWithPressureStop(uint32_t waitMs, float stopPressureKpa, uint8_t* pressureStopSamples = nullptr); + + // Registra un evento di homing sul flash log (sopravvive al power-cycle, in + // piscina la USB è scollegata). detail è una stringa libera con i numeri + // utili a capire dove si è fermato l'homing. + void logHomingEvent(const char* detail); + bool pressureStopReached(float stopPressureKpa, + uint8_t requiredSamples, + uint8_t* pressureStopSamples = nullptr); + bool waitWithPressureStop(uint32_t waitMs, + float stopPressureKpa, + uint8_t requiredSamples, + uint16_t samplePeriodMs, + uint8_t* pressureStopSamples = nullptr); // Esegue un singolo stroke del balance (extend o retract) come move assoluto // verso targetPos. Ritorna true se va a buon fine, false in caso di diff --git a/lib/motion_control/src/motion_control.cpp b/lib/motion_control/src/motion_control.cpp index ce2ae77..06b9380 100644 --- a/lib/motion_control/src/motion_control.cpp +++ b/lib/motion_control/src/motion_control.cpp @@ -4,6 +4,8 @@ #include "sensors.h" #include "comms.h" #include "DebugSerial.h" +#include "runtime_config.h" +#include "flash_storage.h" /* ******************************************************************************* @@ -24,11 +26,52 @@ void MotionController::clearEmergencyStop() { } void MotionController::emergencyStop(const char* reason) { + // Guard di idempotenza: emergencyStop() può essere richiamato durante un + // emergency stop già attivo. Logghiamo (e fermiamo) una sola volta. + const bool firstTrigger = !_emergencyStop; + _emergencyStop = true; + _lastStopReason = reason; _motor.stop(); _motor.disableOutputs(); Debug.printf("Motor emergency stop: %s\n", reason); ledController.setState(LEDState::ERROR); + + if (!firstTrigger) { + return; + } + + // Registra l'evento sul flash NELL'ISTANTE in cui scatta, non a posteriori: + // il vecchio approccio (blocco "aborted" in loop()) mancava lo stop se + // measure() usciva per timeout di fase invece che per emergency stop, o se + // l'auto-recovery azzerava lo stop prima del check. Qui è impossibile + // mancarlo. Scrittura singola (firstTrigger) → nessun rischio di append + // ripetuti. reason e tof esistono solo qui: in piscina la USB è scollegata. + sensors.read(); + char phase[64]; + snprintf(phase, sizeof(phase), "emergency_stop:%s tof=%.1fmm", + reason, _lastStopTofMm); + flashStorage.appendRecord(COMPANY_NUMBER, 0, + static_cast(millis()) / 1000.0f, + sensors.pressure() / 1000.0f, + sensors.depth(), phase, + sensors.sensorDepth(), + motorPosToU(_motor.position())); +} + +void MotionController::logHomingEvent(const char* detail) { + // Stesso meccanismo di emergencyStop(): scriviamo subito sul flash perché + // in piscina la USB è scollegata e vogliamo poter ricostruire a posteriori + // dove l'homing ha sbagliato (errore vs sforamento distanza massima). + sensors.read(); + char phase[96]; + snprintf(phase, sizeof(phase), "homing:%s", detail); + flashStorage.appendRecord(COMPANY_NUMBER, 0, + static_cast(millis()) / 1000.0f, + sensors.pressure() / 1000.0f, + sensors.depth(), phase, + sensors.sensorDepth(), + motorPosToU(_motor.position())); } void MotionController::serviceEmergencyStop() { @@ -78,8 +121,16 @@ bool MotionController::waitForMotor(uint32_t timeoutMs) { } _motor.run(); - if (tofMaxExtensionStopReached(nowMs, lastTofSampleMs, "movement")) { - return false; + switch (tofGuard(nowMs, lastTofSampleMs, "movement")) { + case TofGuard::Emergency: + return false; + case TofGuard::ExtendLimit: + // Fondo corsa esteso: stop pulito, il movimento è "completato" + // al limite fisico senza emergency (protezione tappo). + _motor.stop(); + return true; + case TofGuard::Ok: + break; } ledController.update(); @@ -94,41 +145,62 @@ float MotionController::readPressureKpa() { return sensors.pressure() / 1000.0f; } -bool MotionController::tofMaxExtensionStopReached(unsigned long nowMs, - unsigned long& lastTofSampleMs, - const char* context) { +TofGuard MotionController::tofGuard(unsigned long nowMs, + unsigned long& lastTofSampleMs, + const char* context) { if (!_tof.isInitialized()) { - return false; + return TofGuard::Ok; } if (nowMs - lastTofSampleMs < MOTOR_HOMING_TOF_PERIOD_MS) { - return false; + return TofGuard::Ok; } lastTofSampleMs = nowMs; float distanceMm = 0.0f; if (!_tof.readDistanceMm(distanceMm)) { - return false; + return TofGuard::Ok; } - if (distanceMm < TOF_SAFE_RANGE_MIN_MM) { - Debug.printf("%s: TOF safety stop, too close (%.1f < %.1f mm)\n", - context, distanceMm, TOF_SAFE_RANGE_MIN_MM); - emergencyStop("TOF below safe range"); - return true; + const bool tooClose = distanceMm < TOF_SAFE_RANGE_MIN_MM; + const bool tooFar = distanceMm > TOF_SAFE_RANGE_MAX_MM; + + if (!tooClose && !tooFar) { + // Lettura valida: azzera la conferma in corso (un glitch isolato non + // deve accumularsi nel tempo). + _tofOutOfRangeCount = 0; + return TofGuard::Ok; } - if (distanceMm > TOF_SAFE_RANGE_MAX_MM) { - Debug.printf("%s: TOF safety stop, too far (%.1f > %.1f mm)\n", - context, distanceMm, TOF_SAFE_RANGE_MAX_MM); - emergencyStop("TOF above safe range"); - return true; + // Lettura fuori range: conferma prima di agire, per ignorare glitch singoli + // (bolle, riflessi, torbidità) tipici del TOF in acqua. + if (++_tofOutOfRangeCount < TOF_SAFETY_STOP_SAMPLES) { + Debug.printf("%s: TOF out of range (%.1f mm), sample %u/%u\n", + context, distanceMm, + _tofOutOfRangeCount, TOF_SAFETY_STOP_SAMPLES); + return TofGuard::Ok; } - return false; + _tofOutOfRangeCount = 0; + _lastStopTofMm = distanceMm; + if (tooClose) { + // Limite inferiore = siringa a fondo estensione, prima del tappo. STOP + // PULITO: il chiamante ferma il pistone senza abortire la missione. + Debug.printf("%s: TOF extension limit, too close (%.1f < %.1f mm)\n", + context, distanceMm, TOF_SAFE_RANGE_MIN_MM); + return TofGuard::ExtendLimit; + } + + // Limite superiore = anomalia (passi persi, verso sbagliato): emergency stop. + Debug.printf("%s: TOF safety stop, too far (%.1f > %.1f mm)\n", + context, distanceMm, TOF_SAFE_RANGE_MAX_MM); + emergencyStop("TOF above safe range"); + return TofGuard::Emergency; } -bool MotionController::pressureStopReached(float stopPressureKpa, uint8_t* pressureStopSamples) { +bool MotionController::pressureStopReached(float stopPressureKpa, + uint8_t requiredSamples, + uint8_t* pressureStopSamples) { if (stopPressureKpa <= 0.0f) { return false; } @@ -137,7 +209,7 @@ bool MotionController::pressureStopReached(float stopPressureKpa, uint8_t* press if (pressureKpa > stopPressureKpa) { if (pressureStopSamples != nullptr) { (*pressureStopSamples)++; - if (*pressureStopSamples < BALANCE_STOP_PRESSURE_SAMPLES) { + if (*pressureStopSamples < requiredSamples) { return false; } } @@ -157,7 +229,11 @@ bool MotionController::pressureStopReached(float stopPressureKpa, uint8_t* press return false; } -bool MotionController::waitWithPressureStop(uint32_t waitMs, float stopPressureKpa, uint8_t* pressureStopSamples) { +bool MotionController::waitWithPressureStop(uint32_t waitMs, + float stopPressureKpa, + uint8_t requiredSamples, + uint16_t samplePeriodMs, + uint8_t* pressureStopSamples) { const unsigned long startMs = millis(); while (millis() - startMs < waitMs) { @@ -165,12 +241,12 @@ bool MotionController::waitWithPressureStop(uint32_t waitMs, float stopPressureK return true; } - if (pressureStopReached(stopPressureKpa, pressureStopSamples)) { + if (pressureStopReached(stopPressureKpa, requiredSamples, pressureStopSamples)) { return true; } ledController.update(); - delay(BALANCE_PRESSURE_SAMPLE_PERIOD_MS); + delay(samplePeriodMs); } return false; @@ -185,10 +261,23 @@ bool MotionController::homeWithTof(float stopPressureKpa, bool* pressureStop, ui clearEmergencyStop(); ledController.setState(LEDState::HOMING); + { + float startTof = -1.0f; + _tof.readDistanceMm(startTof); + char detail[80]; + snprintf(detail, sizeof(detail), + "start startTof=%.1fmm approachThr=%.1f homeThr=%.1f maxSteps=%ld", + startTof, (float)TOF_HOMING_APPROACH_MM, (float)TOF_HOMING_THRESHOLD, + (long)MOTOR_MAX_STEPS); + logHomingEvent(detail); + } + _motor.clearPosition(); _motor.enableOutputs(); - _motor.setMaxSpeed(MOTOR_HOMING_SPEED); - _motor.setAcceleration(MOTOR_HOMING_SPEED); + const RuntimeMotorConfig& motorConfig = runtimeConfig.motor(); + const RuntimeBalanceConfig& balanceConfig = runtimeConfig.balance(); + _motor.setMaxSpeed(motorConfig.homingSpeed); + _motor.setAcceleration(motorConfig.homingSpeed); const unsigned long startMs = millis(); unsigned long lastTofSampleMs = 0; @@ -201,6 +290,7 @@ bool MotionController::homeWithTof(float stopPressureKpa, bool* pressureStop, ui _motor.startMoveSteps(-static_cast(MOTOR_MAX_STEPS) * 2); bool approachDone = false; + uint8_t approachSamples = 0; while (_motor.distanceToGo() != 0 && !approachDone) { if (remoteStopRequested()) { return false; @@ -209,6 +299,12 @@ bool MotionController::homeWithTof(float stopPressureKpa, bool* pressureStop, ui const unsigned long nowMs = millis(); if (millis() - startMs > MOTOR_HOMING_TIMEOUT) { Debug.println("Motor homing: timed out during approach"); + char detail[80]; + snprintf(detail, sizeof(detail), + "phase1_timeout pos=%ld toGo=%ld elapsed=%lums", + (long)_motor.position(), (long)_motor.distanceToGo(), + millis() - startMs); + logHomingEvent(detail); emergencyStop("homing timeout"); return false; } @@ -221,10 +317,14 @@ bool MotionController::homeWithTof(float stopPressureKpa, bool* pressureStop, ui float distanceMm = 0.0f; if (_tof.readDistanceMm(distanceMm)) { if (distanceMm < TOF_HOMING_APPROACH_MM) { - Debug.printf("Motor homing: approach reached (%.1f < %.1f mm)\n", - distanceMm, TOF_HOMING_APPROACH_MM); - approachDone = true; - _motor.stop(); + if (++approachSamples >= TOF_HOMING_CONFIRM_SAMPLES) { + Debug.printf("Motor homing: approach reached (%.1f < %.1f mm)\n", + distanceMm, TOF_HOMING_APPROACH_MM); + approachDone = true; + _motor.stop(); + } + } else { + approachSamples = 0; } } } @@ -235,6 +335,16 @@ bool MotionController::homeWithTof(float stopPressureKpa, bool* pressureStop, ui if (!approachDone) { Debug.println("Motor homing: approach phase failed"); + // distanceToGo()==0 senza approachDone significa che il motore ha + // esaurito la corsa massima (2*MOTOR_MAX_STEPS) senza che il TOF + // scendesse sotto la soglia di approccio → "sfora la distanza massima". + float distanceMm = -1.0f; + _tof.readDistanceMm(distanceMm); + char detail[80]; + snprintf(detail, sizeof(detail), + "phase1_no_approach pos=%ld lastTof=%.1fmm thr=%.1f", + (long)_motor.position(), distanceMm, (float)TOF_HOMING_APPROACH_MM); + logHomingEvent(detail); emergencyStop("homing approach failed"); return false; } @@ -247,6 +357,11 @@ bool MotionController::homeWithTof(float stopPressureKpa, bool* pressureStop, ui } if (millis() - settleStart > 2000) { Debug.println("Motor homing: timeout settling approach"); + char detail[64]; + snprintf(detail, sizeof(detail), + "phase1_settle_timeout pos=%ld toGo=%ld", + (long)_motor.position(), (long)_motor.distanceToGo()); + logHomingEvent(detail); emergencyStop("homing approach settle timeout"); return false; } @@ -259,8 +374,15 @@ bool MotionController::homeWithTof(float stopPressureKpa, bool* pressureStop, ui // Phase 2: homing — invert direction (positive, siringa che si retrae) finché TOF legge // sopra TOF_HOMING_THRESHOLD. Debug.println("Motor homing: phase 2 (retract away from TOF)"); + { + char detail[48]; + snprintf(detail, sizeof(detail), "phase2_start pos=%ld", + (long)_motor.position()); + logHomingEvent(detail); + } _motor.startMoveSteps(static_cast(MOTOR_MAX_STEPS) * 2); bool homeDetected = false; + uint8_t homeSamples = 0; while (_motor.distanceToGo() != 0 && !homeDetected) { if (remoteStopRequested()) { @@ -268,9 +390,9 @@ bool MotionController::homeWithTof(float stopPressureKpa, bool* pressureStop, ui } const unsigned long nowMs = millis(); - if (nowMs - lastPressureSampleMs >= BALANCE_PRESSURE_SAMPLE_PERIOD_MS) { + if (nowMs - lastPressureSampleMs >= balanceConfig.samplePeriodMs) { lastPressureSampleMs = nowMs; - if (pressureStopReached(stopPressureKpa, pressureStopSamples)) { + if (pressureStopReached(stopPressureKpa, balanceConfig.stopPressureSamples, pressureStopSamples)) { if (pressureStop != nullptr) { *pressureStop = true; } @@ -280,6 +402,12 @@ bool MotionController::homeWithTof(float stopPressureKpa, bool* pressureStop, ui if (millis() - startMs > MOTOR_HOMING_TIMEOUT) { Debug.println("Motor homing: timed out"); + char detail[80]; + snprintf(detail, sizeof(detail), + "phase2_timeout pos=%ld toGo=%ld elapsed=%lums", + (long)_motor.position(), (long)_motor.distanceToGo(), + millis() - startMs); + logHomingEvent(detail); emergencyStop("homing timeout"); return false; } @@ -292,10 +420,14 @@ bool MotionController::homeWithTof(float stopPressureKpa, bool* pressureStop, ui float distanceMm = 0.0f; if (_tof.readDistanceMm(distanceMm)) { if (distanceMm > TOF_HOMING_THRESHOLD) { - Debug.printf("Motor homing: threshold reached (%.1f > %.1f mm)\n", - distanceMm, TOF_HOMING_THRESHOLD); - homeDetected = true; - _motor.stop(); + if (++homeSamples >= TOF_HOMING_CONFIRM_SAMPLES) { + Debug.printf("Motor homing: threshold reached (%.1f > %.1f mm)\n", + distanceMm, TOF_HOMING_THRESHOLD); + homeDetected = true; + _motor.stop(); + } + } else { + homeSamples = 0; } } } @@ -306,25 +438,39 @@ bool MotionController::homeWithTof(float stopPressureKpa, bool* pressureStop, ui if (!homeDetected) { Debug.println("Motor homing: no TOF detection"); + // distanceToGo()==0 senza homeDetected: il motore ha consumato tutta la + // corsa (2*MOTOR_MAX_STEPS) in retrazione senza che il TOF superasse la + // soglia di home → è il caso "sfora la distanza massima". + float distanceMm = -1.0f; + _tof.readDistanceMm(distanceMm); + char detail[80]; + snprintf(detail, sizeof(detail), + "phase2_no_detect pos=%ld lastTof=%.1fmm thr=%.1f", + (long)_motor.position(), distanceMm, (float)TOF_HOMING_THRESHOLD); + logHomingEvent(detail); emergencyStop("homing finished without TOF detection"); return false; } delay(100); - _motor.startMoveSteps(-static_cast(MOTOR_ENDSTOP_MARGIN)); + _motor.startMoveSteps(static_cast(MOTOR_ENDSTOP_MARGIN)); if (!waitForMotor(2000)) { Debug.println("Motor homing: timeout during backoff"); + char detail[48]; + snprintf(detail, sizeof(detail), "backoff_timeout pos=%ld", + (long)_motor.position()); + logHomingEvent(detail); return false; } _motor.setCurrentPosition(0); - _motor.setMaxSpeed(MOTOR_MAX_SPEED); - _motor.setAcceleration(MOTOR_MAX_ACCELERATION); + runtimeConfig.applyMotorConfig(); _motor.disableOutputs(); clearEmergencyStop(); Debug.println("Motor homing: complete, position set to 0"); + logHomingEvent("complete pos=0"); return true; } @@ -377,6 +523,7 @@ bool MotionController::moveToMax(uint32_t timeoutMs, const unsigned long startMs = millis(); unsigned long lastTofSampleMs = 0; unsigned long lastPressureSampleMs = 0; + const RuntimeBalanceConfig& balanceConfig = runtimeConfig.balance(); while (_motor.distanceToGo() != 0) { if (remoteStopRequested()) { @@ -384,9 +531,9 @@ bool MotionController::moveToMax(uint32_t timeoutMs, } const unsigned long nowMs = millis(); - if (nowMs - lastPressureSampleMs >= BALANCE_PRESSURE_SAMPLE_PERIOD_MS) { + if (nowMs - lastPressureSampleMs >= balanceConfig.samplePeriodMs) { lastPressureSampleMs = nowMs; - if (pressureStopReached(stopPressureKpa, pressureStopSamples)) { + if (pressureStopReached(stopPressureKpa, balanceConfig.stopPressureSamples, pressureStopSamples)) { if (pressureStop != nullptr) { *pressureStop = true; } @@ -401,29 +548,19 @@ bool MotionController::moveToMax(uint32_t timeoutMs, _motor.run(); - // Limite TOF inferiore = siringa completamente estesa: stop pulito, - // non emergency stop. Permette al chiamante (es. balance) di fare - // l'hold a fine corsa invece di considerarlo un errore. - if (_tof.isInitialized() && nowMs - lastTofSampleMs >= MOTOR_HOMING_TOF_PERIOD_MS) { - lastTofSampleMs = nowMs; - float distanceMm = 0.0f; - if (_tof.readDistanceMm(distanceMm) && distanceMm <= TOF_SAFE_RANGE_MIN_MM) { - Debug.printf("moveToMax: TOF reached extension limit (%.1f <= %.1f mm)\n", - distanceMm, TOF_SAFE_RANGE_MIN_MM); + // Limite TOF inferiore = siringa completamente estesa: stop PULITO (non + // emergency). Permette al chiamante (es. balance) di fare l'hold a fine + // corsa invece di considerarlo un errore. Limite superiore = anomalia → + // emergency stop (già scattato dentro tofGuard). + switch (tofGuard(nowMs, lastTofSampleMs, "moveToMax")) { + case TofGuard::Emergency: + return false; + case TofGuard::ExtendLimit: _motor.stop(); - while (_motor.distanceToGo() != 0) { - _motor.run(); - yield(); - } _motor.disableOutputs(); return true; - } - if (distanceMm > TOF_SAFE_RANGE_MAX_MM) { - Debug.printf("moveToMax: TOF safety stop, too far (%.1f > %.1f mm)\n", - distanceMm, TOF_SAFE_RANGE_MAX_MM); - emergencyStop("TOF above safe range"); - return false; - } + case TofGuard::Ok: + break; } ledController.update(); @@ -442,14 +579,13 @@ bool MotionController::manualStepTest(long steps, uint32_t speed) { Debug.printf("Test: moving %ld steps at %u steps/s\n", steps, speed); _motor.setMaxSpeed(speed); - _motor.setAcceleration(MOTOR_MAX_ACCELERATION); + _motor.setAcceleration(runtimeConfig.motor().maxAcceleration); _motor.enableOutputs(); _motor.startMoveSteps(steps); const bool success = waitForMotor(0); - _motor.setMaxSpeed(MOTOR_MAX_SPEED); - _motor.setAcceleration(MOTOR_MAX_ACCELERATION); + runtimeConfig.applyMotorConfig(); if (success) { _motor.disableOutputs(); Debug.printf("Test complete — pos %ld\n", _motor.position()); @@ -473,12 +609,17 @@ bool MotionController::_balanceStrokeTo(long targetPos, _motor.startMoveTo(targetPos); const unsigned long moveStart = millis(); + unsigned long lastTofSampleMs = 0; + char tofCtx[32]; + snprintf(tofCtx, sizeof(tofCtx), "balance %s", label); while (_motor.distanceToGo() != 0) { if (remoteStopRequested()) { remoteStopHit = true; return false; } - if (pressureStopReached(stopPressureKpa, pressureStopSamples)) { + if (pressureStopReached(stopPressureKpa, + runtimeConfig.balance().stopPressureSamples, + pressureStopSamples)) { pressureStopHit = true; return false; } @@ -489,6 +630,18 @@ bool MotionController::_balanceStrokeTo(long targetPos, return false; } _motor.run(); + // Supervisione TOF: fondo corsa esteso (tappo) = stop pulito a fine + // stroke; oltre il limite superiore = emergency (già scattato). + switch (tofGuard(millis(), lastTofSampleMs, tofCtx)) { + case TofGuard::Emergency: + return false; + case TofGuard::ExtendLimit: + _motor.stop(); + _motor.disableOutputs(); + return true; + case TofGuard::Ok: + break; + } ledController.update(); yield(); } @@ -497,7 +650,7 @@ bool MotionController::_balanceStrokeTo(long targetPos, return true; } -bool MotionController::balance(uint32_t holdMs) { +bool MotionController::balance() { // Reset di sicurezza: la balance è una routine di spurgo manuale, parte // sempre pulita anche se un emergency stop precedente non è stato cancellato. clearEmergencyStop(); @@ -510,23 +663,25 @@ bool MotionController::balance(uint32_t holdMs) { return false; } + const RuntimeBalanceConfig& balanceConfig = runtimeConfig.balance(); + const uint32_t holdMs = balanceConfig.holdMs; const float baselinePressureKpa = readPressureKpa(); - const float stopPressureKpa = baselinePressureKpa + BALANCE_STOP_PRESSURE_DELTA_KPA; + const float stopPressureKpa = baselinePressureKpa + balanceConfig.stopPressureDeltaKpa; uint8_t pressureStopSamples = 0; Debug.printf("Balance: baseline=%.2f kPa stop=%.2f kPa delta=%.2f kPa\n", baselinePressureKpa, stopPressureKpa, - BALANCE_STOP_PRESSURE_DELTA_KPA); + balanceConfig.stopPressureDeltaKpa); - // u=1 → siringa piena (extend), u=0 → siringa vuota (retract a home). - // uToMotorPos() rispetta MOTOR_INVERT_LOGICAL: nessuna ipotesi sul segno qui. + // u=1 → prende acqua (verso il TOF, direzione negativa) = extend. + // u=0 → spinge acqua fuori (home, pos=0) = retract. const long extendedPos = uToMotorPos(1.0f); const long retractedPos = uToMotorPos(0.0f); while (motionAllowed()) { if (remoteStopRequested()) return false; - if (pressureStopReached(stopPressureKpa, &pressureStopSamples)) return true; + if (pressureStopReached(stopPressureKpa, balanceConfig.stopPressureSamples, &pressureStopSamples)) return true; bool pressureHit = false, remoteHit = false; if (!_balanceStrokeTo(extendedPos, "extend", stopPressureKpa, @@ -536,7 +691,11 @@ bool MotionController::balance(uint32_t holdMs) { } Debug.printf("Balance: hold extended (%lu ms)\n", (unsigned long)holdMs); - if (waitWithPressureStop(holdMs, stopPressureKpa, &pressureStopSamples)) { + if (waitWithPressureStop(holdMs, + stopPressureKpa, + balanceConfig.stopPressureSamples, + balanceConfig.samplePeriodMs, + &pressureStopSamples)) { return true; } @@ -547,7 +706,11 @@ bool MotionController::balance(uint32_t holdMs) { } Debug.printf("Balance: hold retracted (%lu ms)\n", (unsigned long)holdMs); - if (waitWithPressureStop(holdMs, stopPressureKpa, &pressureStopSamples)) { + if (waitWithPressureStop(holdMs, + stopPressureKpa, + balanceConfig.stopPressureSamples, + balanceConfig.samplePeriodMs, + &pressureStopSamples)) { return true; } } diff --git a/lib/motor/include/motor.h b/lib/motor/include/motor.h index e04c8ed..d361717 100644 --- a/lib/motor/include/motor.h +++ b/lib/motor/include/motor.h @@ -40,6 +40,11 @@ class MotorController { // Non-blocking movement primitives for firmware-level procedures. void startMoveTo(long targetPosition); void startMoveSteps(long steps); + // Jog relativo SENZA clamp ai fine corsa software. Da usare SOLO per il + // rientro meccanico manuale (manual keyboard) quando il pistone è + // disallineato e serve uscire dal range nominale. Il firmware di missione + // non deve usarlo: vedi startMoveSteps/startMoveTo (clampati). + void startJogStepsUnclamped(long steps); void run(); void stop(); long distanceToGo(); diff --git a/lib/motor/src/motor.cpp b/lib/motor/src/motor.cpp index a68248c..b06fbc4 100644 --- a/lib/motor/src/motor.cpp +++ b/lib/motor/src/motor.cpp @@ -88,6 +88,13 @@ void MotorController::startMoveTo(long targetPosition) { _startRawMoveTo(_clampTarget(targetPosition)); } +// --------------------------------------------------------------------------- +void MotorController::startJogStepsUnclamped(long steps) { + // Bypassa volutamente _clampTarget: serve a recuperare un pistone + // disallineato spingendolo oltre il range nominale. Vedi motor.h. + _startRawMoveTo(position() + steps); +} + // --------------------------------------------------------------------------- void MotorController::startMoveSteps(long steps) { _startRawMoveTo(position() + steps); diff --git a/lib/pid/include/pid.h b/lib/pid/include/pid.h index 260add3..d70bce6 100644 --- a/lib/pid/include/pid.h +++ b/lib/pid/include/pid.h @@ -15,14 +15,16 @@ class PIDController { public: - // Mutabili a runtime via CMD_UPDATE_PID (8) + // Mutabili a runtime via PID_CONFIG_SET (8) float Kp; float Ki; float Kd; - // Mutabili a runtime via CMD_UPDATE_PID_EXT (14) + // Mutabili a runtime via PID_CONFIG_SET (8) float alphaD; // LPF coefficient sulla derivata, in (0, 1] uint16_t periodMs; // Periodo del tick PID nel loop (ms) + float integralLimit; + float minRetargetFrac; // Offset costante di kick-start; somma direttamente all'output normalizzato float uNeutral; diff --git a/lib/pid/src/pid.cpp b/lib/pid/src/pid.cpp index 74fdf08..f7ab648 100644 --- a/lib/pid/src/pid.cpp +++ b/lib/pid/src/pid.cpp @@ -18,6 +18,8 @@ PIDController::PIDController(float kp, float ki, float kd) : Kp(kp), Ki(ki), Kd(kd), alphaD(PID_ALPHA_D_DEFAULT), periodMs(PID_PERIOD_DEFAULT_MS), + integralLimit(PID_INTEGRAL_LIMIT), + minRetargetFrac(PID_MIN_RETARGET_FRAC), uNeutral(PID_U_NEUTRAL) {} void PIDController::reset() { @@ -56,7 +58,7 @@ float PIDController::computeNormalized(float targetDepth, float currentDepth) { const bool satLow = (uRaw < 0.0f); if (!((satHigh && error > 0.0f) || (satLow && error < 0.0f))) { _integral += error * dt; - _integral = constrain(_integral, -PID_INTEGRAL_LIMIT, PID_INTEGRAL_LIMIT); + _integral = constrain(_integral, -integralLimit, integralLimit); } _lastDepth = currentDepth; diff --git a/lib/profile/include/profile.h b/lib/profile/include/profile.h index 7269cb4..dbe4d3a 100644 --- a/lib/profile/include/profile.h +++ b/lib/profile/include/profile.h @@ -1,6 +1,7 @@ #pragma once #include +#include "config.h" /* ******************************************************************************* @@ -11,10 +12,33 @@ ******************************************************************************* */ +struct RuntimeProfileConfig { + uint8_t profileCount = PROFILE_MAX_COUNT; + float descentTargetM = TARGET_DEPTH; // target discesa, riferito al FONDO del float + float ascentTargetM = TARGET_SHALLOW_TOP_DEPTH; // target risalita, riferito al TOP del float + float depthToleranceM = DEPTH_MAX_ERROR; + float holdTimeS = STAT_TIME; + float descentTimeoutS = TIMEOUT_PID_TIME; // timeout assoluto fase discesa (hold incluso) + float ascentTimeoutS = TIMEOUT_ASCENT; // timeout assoluto fase risalita (hold incluso) + float surfaceRestOffsetM = SURFACE_TARGET_OFFSET_M; // top del float sotto pelo a riposo +}; + class ProfileManager { public: ProfileManager(); + // Load runtime profile settings from NVS, falling back to config.h defaults. + void beginConfig(); + + const RuntimeProfileConfig& config() const { return _config; } + float ascentTargetBottomM() const; + // Target della sosta finale (top del float a surfaceRestOffsetM sotto il pelo), + // convertito in riferimento FONDO come il PID. Vedi ascentTargetBottomM(). + float restTargetBottomM() const; + bool setConfig(const RuntimeProfileConfig& config); + bool validateConfig(const RuntimeProfileConfig& config) const; + void formatConfigJson(char* buffer, size_t bufferSize) const; + // Reset EEPROM read/write pointers (call before starting a new profile) void resetEEPROM(); @@ -45,10 +69,13 @@ class ProfileManager { uint8_t _activeProfileId = 0; unsigned long _missionStartMs = 0; bool _missionClockRunning = false; + RuntimeProfileConfig _config; void _logReading(float pressure, float temperature); void _logProfileReading(const char* phase); float _missionTimeS() const; + void _saveConfig(); + void _applyConfigToSubsystems(); }; // Singleton diff --git a/lib/profile/src/profile.cpp b/lib/profile/src/profile.cpp index 4e96517..d25d80c 100644 --- a/lib/profile/src/profile.cpp +++ b/lib/profile/src/profile.cpp @@ -8,6 +8,7 @@ #include "comms.h" #include "flash_storage.h" #include +#include #include "float_common.h" #include "DebugSerial.h" @@ -25,15 +26,167 @@ ProfileManager::ProfileManager() {} namespace { +constexpr uint32_t PROFILE_CONFIG_MAGIC = 0x50464C54UL; // "PFLT" +// v2: surfaceRestOffsetM default 0.10 -> 0.15 (antenna sommersa). Il bump +// invalida la config NVS stantia così il nuovo default da config.h viene ricaricato. +constexpr uint16_t PROFILE_CONFIG_VERSION = 2; +constexpr char PROFILE_CONFIG_NAMESPACE[] = "float_profile"; +constexpr char PROFILE_CONFIG_KEY[] = "cfg"; + +struct StoredProfileConfig { + uint32_t magic; + uint16_t version; + RuntimeProfileConfig config; +}; + bool sendPacketFromStorage(const char* message, uint32_t timeoutMs) { return comms.sendMessage(message, timeoutMs); } } +// --------------------------------------------------------------------------- +void ProfileManager::beginConfig() { + RuntimeProfileConfig loaded; + bool hasValidStoredConfig = false; + + Preferences preferences; + if (preferences.begin(PROFILE_CONFIG_NAMESPACE, true)) { + if (preferences.getBytesLength(PROFILE_CONFIG_KEY) == sizeof(StoredProfileConfig)) { + StoredProfileConfig stored; + preferences.getBytes(PROFILE_CONFIG_KEY, &stored, sizeof(stored)); + if (stored.magic == PROFILE_CONFIG_MAGIC && + stored.version == PROFILE_CONFIG_VERSION && + validateConfig(stored.config)) { + loaded = stored.config; + hasValidStoredConfig = true; + } + } + preferences.end(); + } + + _config = loaded; + _applyConfigToSubsystems(); + + if (!hasValidStoredConfig) { + _saveConfig(); + Debug.println("Profile config: using config.h defaults"); + } else { + Debug.println("Profile config: loaded from NVS"); + } +} + +// --------------------------------------------------------------------------- +float ProfileManager::ascentTargetBottomM() const { + // ascentTargetM e' riferito al TOP del float; il PID lavora in riferimento + // FONDO (come il sensore), quindi convertiamo aggiungendo la geometria. + return _config.ascentTargetM + SENSOR_TO_BOTTOM_M + SENSOR_TO_TOP_M; +} + +// --------------------------------------------------------------------------- +float ProfileManager::restTargetBottomM() const { + // surfaceRestOffsetM e' la quota del TOP del float sotto il pelo a riposo; + // convertita in riferimento FONDO come ascentTargetBottomM(). + return _config.surfaceRestOffsetM + SENSOR_TO_BOTTOM_M + SENSOR_TO_TOP_M; +} + +// --------------------------------------------------------------------------- +bool ProfileManager::setConfig(const RuntimeProfileConfig& config) { + if (!validateConfig(config)) { + Debug.println("Profile config rejected: invalid values"); + return false; + } + + _config = config; + _applyConfigToSubsystems(); + _saveConfig(); + Debug.println("Profile config updated"); + return true; +} + +// --------------------------------------------------------------------------- +bool ProfileManager::validateConfig(const RuntimeProfileConfig& config) const { + const float ascentBottomM = + config.ascentTargetM + SENSOR_TO_BOTTOM_M + SENSOR_TO_TOP_M; + + return config.profileCount >= 1 && config.profileCount <= 10 && + isfinite(config.descentTargetM) && + isfinite(config.ascentTargetM) && + isfinite(config.depthToleranceM) && + isfinite(config.holdTimeS) && + isfinite(config.descentTimeoutS) && + isfinite(config.ascentTimeoutS) && + isfinite(config.surfaceRestOffsetM) && + config.descentTargetM >= 0.0f && config.descentTargetM <= 5.0f && + config.ascentTargetM >= 0.0f && config.ascentTargetM <= 5.0f && + config.depthToleranceM >= 0.005f && config.depthToleranceM <= 1.0f && + config.holdTimeS >= 1.0f && config.holdTimeS <= 600.0f && + config.descentTimeoutS >= 5.0f && config.descentTimeoutS <= 900.0f && + config.ascentTimeoutS >= 5.0f && config.ascentTimeoutS <= 900.0f && + // Il timeout di fase include anche l'hold: se non lascia spazio + // all'hold piu' un margine di discesa/risalita, tronca la fase + // in silenzio (successo in piscina: timeout 10 s con hold 60 s). + config.descentTimeoutS >= config.holdTimeS + 30.0f && + config.ascentTimeoutS >= config.holdTimeS + 30.0f && + config.surfaceRestOffsetM >= 0.0f && config.surfaceRestOffsetM <= 5.0f && + ascentBottomM < config.descentTargetM; +} + +// --------------------------------------------------------------------------- +void ProfileManager::formatConfigJson(char* buffer, size_t bufferSize) const { + if (buffer == nullptr || bufferSize == 0) return; + + snprintf(buffer, bufferSize, + "{\"profile_count\":%u," + "\"descent_target_m\":%.3f," + "\"ascent_target_m\":%.3f," + "\"ascent_target_bottom_m\":%.3f," + "\"depth_tolerance_m\":%.3f," + "\"hold_s\":%.1f," + "\"descent_timeout_s\":%.1f," + "\"ascent_timeout_s\":%.1f," + "\"surface_rest_offset_m\":%.3f}", + static_cast(_config.profileCount), + _config.descentTargetM, + _config.ascentTargetM, + ascentTargetBottomM(), + _config.depthToleranceM, + _config.holdTimeS, + _config.descentTimeoutS, + _config.ascentTimeoutS, + _config.surfaceRestOffsetM); +} + +// --------------------------------------------------------------------------- +void ProfileManager::_saveConfig() { + StoredProfileConfig stored = { + PROFILE_CONFIG_MAGIC, + PROFILE_CONFIG_VERSION, + _config, + }; + + Preferences preferences; + if (!preferences.begin(PROFILE_CONFIG_NAMESPACE, false)) { + Debug.println("Profile config: NVS open failed"); + return; + } + + preferences.putBytes(PROFILE_CONFIG_KEY, &stored, sizeof(stored)); + preferences.end(); +} + +// --------------------------------------------------------------------------- +void ProfileManager::_applyConfigToSubsystems() { + sensors.setSurfaceTargetOffset(_config.surfaceRestOffsetM); +} + // --------------------------------------------------------------------------- void ProfileManager::resetEEPROM() { _writePtr = 0; _readPtr = 0; + // Azzera il flash log all'inizio di una nuova missione: non più al boot, + // così il log di un test fallito sopravvive al power-cycle ed è leggibile + // con DUMP_LOG finché non si avvia un nuovo profilo. + flashStorage.clearLog(); } // --------------------------------------------------------------------------- @@ -59,14 +212,16 @@ void ProfileManager::logDeploymentPacket() { "\"pressure_kpa\":%.2f," "\"depth_m\":%.2f," "\"phase\":\"%s\"," - "\"sensor_depth_m\":%.2f}", + "\"sensor_depth_m\":%.2f," + "\"syringe_u\":%.4f}", COMPANY_NUMBER, static_cast(_activeProfileId), _missionTimeS(), sensors.pressure() / 1000.0f, sensors.referenceDepthForPhase("deployed"), "deployed", - sensors.sensorDepth()); + sensors.sensorDepth(), + motorPosToU(motor.position())); comms.sendMessage(packet, 1000); } @@ -92,7 +247,8 @@ void ProfileManager::_logProfileReading(const char* phase) { sensors.pressure() / 1000.0f, sensors.referenceDepthForPhase(phase), phase, - sensors.sensorDepth()); + sensors.sensorDepth(), + motorPosToU(motor.position())); } // --------------------------------------------------------------------------- @@ -106,11 +262,23 @@ void ProfileManager::measure(float targetDepth, float holdTimeSec, float timeout Debug.printf("Profile phase: target=%.2f hold=%.0fs timeout=%.0fs\n", targetDepth, holdTimeSec, timeoutSec); + // Record di inizio fase con i parametri EFFETTIVI: dal dump flash si vede + // quale hold/timeout ha girato davvero, non solo quello atteso dalla GUI. + sensors.read(); + char phaseTag[48]; + snprintf(phaseTag, sizeof(phaseTag), "phase_start hold=%.0f timeout=%.0f", + holdTimeSec, timeoutSec); + _logProfileReading(phaseTag); + const bool isSurfaceTarget = (targetDepth == TARGET_SURFACE); const bool isBottomTarget = (targetDepth == TARGET_BOTTOM); const bool isPIDPhase = !isSurfaceTarget && !isBottomTarget; - const bool isDeepTarget = fabsf(targetDepth - TARGET_DEPTH) < 0.001f; - const bool isShallowTarget = fabsf(targetDepth - TARGET_SHALLOW_BOTTOM_DEPTH) < 0.001f; + const bool isDeepTarget = fabsf(targetDepth - _config.descentTargetM) < 0.001f; + const bool isShallowTarget = fabsf(targetDepth - ascentTargetBottomM()) < 0.001f; + // Sosta finale: hold PID al target di riposo che NON termina al raggiungimento + // dell'hold ma resta attivo finché non arriva il recupero (remote stop) o + // scade timeoutSec — così il float non risale e non rompe la superficie. + const bool isRestTarget = fabsf(targetDepth - restTargetBottomM()) < 0.001f; // --- LED and initial motor positioning --- if (isPIDPhase) { @@ -118,10 +286,9 @@ void ProfileManager::measure(float targetDepth, float holdTimeSec, float timeout pidController.reset(); if (isDeepTarget) { // Pre-position syringe to kick-start the deep descent only. - // u=0.979 → siringa quasi piena → spinta iniziale per "affondare". - // MOTOR_INVERT_LOGICAL=false: uToMotorPos mappa direttamente la - // convenzione logica sulla geometria nativa. - motionController.moveToWithTimeout(uToMotorPos(0.979f), 0); + // PID_DESCENT_KICK_U → spinta iniziale per "affondare", contenuta + // per non far superare il target prima che il PID prenda il controllo. + motionController.moveToWithTimeout(uToMotorPos(PID_DESCENT_KICK_U), 0); } } else { ledController.setState(LEDState::PROFILE); @@ -135,9 +302,9 @@ void ProfileManager::measure(float targetDepth, float holdTimeSec, float timeout float lastDepth = 0.0f; int stableCount = 0; // Per la fase PID: ultimo target assoluto comandato al motore (in step). - // Inizializzato al pre-position (u=0.979) per isDeepTarget, altrimenti alla - // posizione corrente — letta dopo il primo sensors.read() qui sotto. - long lastCommandedTarget = isDeepTarget ? uToMotorPos(0.979f) : motor.position(); + // Inizializzato al pre-position (PID_DESCENT_KICK_U) per isDeepTarget, + // altrimenti alla posizione corrente — letta dopo il primo sensors.read(). + long lastCommandedTarget = isDeepTarget ? uToMotorPos(PID_DESCENT_KICK_U) : motor.position(); // ----------------------------------------------------------------------- while (true) { @@ -146,18 +313,20 @@ void ProfileManager::measure(float targetDepth, float holdTimeSec, float timeout if (motionController.remoteStopRequested()) { Debug.println("Profile phase: remote stop"); + _logProfileReading("exit_remote_stop"); break; } // Abort if phase timeout exceeded if (millis() - phaseStart > static_cast(timeoutSec * 1000UL)) { Debug.println("Profile phase: timeout"); + _logProfileReading("exit_timeout"); break; } // --- Measurement tick --- // Fase PID gira al ritmo configurabile pidController.periodMs (default 50 ms, - // modificabile via CMD_UPDATE_PID_EXT). Fasi simple restano a PERIOD_MEASUREMENT. + // modificabile via PID_CONFIG_SET). Fasi simple restano a PERIOD_MEASUREMENT. const uint16_t measPeriodMs = isPIDPhase ? pidController.periodMs : PERIOD_MEASUREMENT; if (millis() - lastMeasMs < measPeriodMs) continue; @@ -168,14 +337,14 @@ void ProfileManager::measure(float targetDepth, float holdTimeSec, float timeout const char* phase = "descending"; if (isPIDPhase) { - if (isShallowTarget) { - phase = (fabsf(currentDepth - targetDepth) < DEPTH_MAX_ERROR) - ? "hold_40cm" - : "ascending"; + const bool atTarget = + fabsf(currentDepth - targetDepth) < _config.depthToleranceM; + if (isRestTarget) { + phase = atTarget ? "rest_surface" : "resting"; + } else if (isShallowTarget) { + phase = atTarget ? "hold_40cm" : "ascending"; } else { - phase = (fabsf(currentDepth - targetDepth) < DEPTH_MAX_ERROR) - ? "hold_2_5m" - : "descending"; + phase = atTarget ? "hold_2_5m" : "descending"; } } else if (isSurfaceTarget) { // Riferimento: top del float a `surfaceTargetOffset` sotto il pelo. @@ -242,12 +411,17 @@ void ProfileManager::measure(float targetDepth, float holdTimeSec, float timeout // Output PID = posizione assoluta della siringa, frazione di corsa in [0, 1]. // Comando motore NON bloccante: startMoveTo aggiorna il target di FastAccelStepper // al volo, anche se il motore sta ancora viaggiando dal tick precedente. - const float u = pidController.computeNormalized(targetDepth, currentDepth); + // Clamp dell'output PID a [PID_U_MIN, PID_U_MAX]: tiene la siringa + // lontana dagli estremi meccanici che coincidono con le soglie TOF, + // così il controllo non si auto-ferma in emergency stop al limite. + const float u = constrain( + pidController.computeNormalized(targetDepth, currentDepth), + PID_U_MIN, PID_U_MAX); const long usableSteps = (long)MOTOR_MAX_STEPS - 2L * (long)MOTOR_ENDSTOP_MARGIN; const long posTarget = uToMotorPos(u); const long deadbandSteps = - (long)(PID_MIN_RETARGET_FRAC * (float)usableSteps); + (long)(pidController.minRetargetFrac * (float)usableSteps); if (labs(posTarget - lastCommandedTarget) >= deadbandSteps) { motor.enableOutputs(); motor.startMoveTo(posTarget); @@ -261,13 +435,17 @@ void ProfileManager::measure(float targetDepth, float holdTimeSec, float timeout // Mark PID-phase records with temperature sentinel _logReading(sensors.pressure(), 100.0f); - if (fabsf(currentDepth - targetDepth) < DEPTH_MAX_ERROR) { + if (fabsf(currentDepth - targetDepth) < _config.depthToleranceM) { stableCount++; // Seven 5-second packets span the required 30-second hold. const int requiredTicks = static_cast(holdTimeSec * 1000.0f / PERIOD_EEPROM_WRITE) + 1; - if (stableCount >= requiredTicks) { + // La sosta finale (isRestTarget) NON esce all'hold completo: resta + // attiva finché non arriva il recupero (remote stop) o scade il + // timeout, così la cima resta sotto il pelo in attesa dell'ROV. + if (!isRestTarget && stableCount >= requiredTicks) { Debug.println("Profile: PID hold complete — target depth sustained"); + _logProfileReading("exit_hold_ok"); break; } } else { diff --git a/lib/runtime_config/include/runtime_config.h b/lib/runtime_config/include/runtime_config.h new file mode 100644 index 0000000..c40dd63 --- /dev/null +++ b/lib/runtime_config/include/runtime_config.h @@ -0,0 +1,66 @@ +#pragma once + +#include + +struct RuntimePidConfig { + float kp; + float ki; + float kd; + uint16_t periodMs; + float alphaD; + float integralLimit; + float minRetargetFrac; + float uNeutral; +}; + +struct RuntimeBalanceConfig { + uint32_t holdMs; + float stopPressureDeltaKpa; + uint8_t stopPressureSamples; + uint16_t samplePeriodMs; +}; + +struct RuntimeMotorConfig { + uint32_t maxSpeed; + uint32_t maxAcceleration; + uint32_t homingSpeed; + uint32_t testSpeed; +}; + +class RuntimeConfigManager { +public: + void begin(); + + const RuntimePidConfig& pid() const { return _pid; } + const RuntimeBalanceConfig& balance() const { return _balance; } + const RuntimeMotorConfig& motor() const { return _motor; } + + bool setPidConfig(const RuntimePidConfig& config); + bool setBalanceConfig(const RuntimeBalanceConfig& config); + bool setMotorConfig(const RuntimeMotorConfig& config); + + bool validatePidConfig(const RuntimePidConfig& config) const; + bool validateBalanceConfig(const RuntimeBalanceConfig& config) const; + bool validateMotorConfig(const RuntimeMotorConfig& config) const; + + void applyPidConfig() const; + void applyMotorConfig() const; + + void formatPidConfigJson(char* buffer, size_t size) const; + void formatBalanceConfigJson(char* buffer, size_t size) const; + void formatMotorConfigJson(char* buffer, size_t size) const; + +private: + RuntimePidConfig _pid{}; + RuntimeBalanceConfig _balance{}; + RuntimeMotorConfig _motor{}; + + void loadPidConfig(); + void loadBalanceConfig(); + void loadMotorConfig(); + bool savePidConfig() const; + bool saveBalanceConfig() const; + bool saveMotorConfig() const; +}; + +extern RuntimeConfigManager runtimeConfig; diff --git a/lib/runtime_config/src/runtime_config.cpp b/lib/runtime_config/src/runtime_config.cpp new file mode 100644 index 0000000..f08aa2d --- /dev/null +++ b/lib/runtime_config/src/runtime_config.cpp @@ -0,0 +1,302 @@ +#include "runtime_config.h" + +#include +#include + +#include "DebugSerial.h" +#include "config.h" +#include "motor.h" +#include "pid.h" + +namespace { +constexpr char NVS_NAMESPACE[] = "float_runtime"; +constexpr uint32_t PID_MAGIC = 0x50494432; // PID2 +constexpr uint32_t BALANCE_MAGIC = 0x42414C31; // BAL1 +constexpr uint32_t MOTOR_MAGIC = 0x4D4F5431; // MOT1 + +constexpr uint32_t MOTOR_SPEED_MIN = 10; +constexpr uint32_t MOTOR_SPEED_MAX = 5000; +constexpr uint32_t MOTOR_ACCEL_MIN = 10; +constexpr uint32_t MOTOR_ACCEL_MAX = 10000; + +template +struct StoredConfig { + uint32_t magic; + T config; +}; + +RuntimePidConfig defaultPidConfig() { + return { + PID_KP_DEFAULT, + PID_KI_DEFAULT, + PID_KD_DEFAULT, + PID_PERIOD_DEFAULT_MS, + PID_ALPHA_D_DEFAULT, + PID_INTEGRAL_LIMIT, + PID_MIN_RETARGET_FRAC, + PID_U_NEUTRAL, + }; +} + +RuntimeBalanceConfig defaultBalanceConfig() { + return { + 5000, + BALANCE_STOP_PRESSURE_DELTA_KPA, + BALANCE_STOP_PRESSURE_SAMPLES, + BALANCE_PRESSURE_SAMPLE_PERIOD_MS, + }; +} + +RuntimeMotorConfig defaultMotorConfig() { + return { + MOTOR_MAX_SPEED, + MOTOR_MAX_ACCELERATION, + MOTOR_HOMING_SPEED, + MOTOR_MAX_SPEED, + }; +} + +bool finiteNonNegative(float value) { + return isfinite(value) && value >= 0.0f; +} +} + +RuntimeConfigManager runtimeConfig; + +void RuntimeConfigManager::begin() { + _pid = defaultPidConfig(); + _balance = defaultBalanceConfig(); + _motor = defaultMotorConfig(); + + loadPidConfig(); + loadBalanceConfig(); + loadMotorConfig(); + applyPidConfig(); +} + +bool RuntimeConfigManager::validatePidConfig(const RuntimePidConfig& config) const { + return isfinite(config.kp) && + isfinite(config.ki) && + isfinite(config.kd) && + config.periodMs >= 20 && + config.periodMs <= 500 && + config.alphaD >= 0.05f && + config.alphaD <= 1.0f && + isfinite(config.integralLimit) && + config.integralLimit > 0.0f && + finiteNonNegative(config.minRetargetFrac) && + finiteNonNegative(config.uNeutral); +} + +bool RuntimeConfigManager::validateBalanceConfig(const RuntimeBalanceConfig& config) const { + return config.holdMs <= 60000 && + isfinite(config.stopPressureDeltaKpa) && + config.stopPressureDeltaKpa >= 0.1f && + config.stopPressureDeltaKpa <= 50.0f && + config.stopPressureSamples >= 1 && + config.stopPressureSamples <= 20 && + config.samplePeriodMs >= 20 && + config.samplePeriodMs <= 1000; +} + +bool RuntimeConfigManager::validateMotorConfig(const RuntimeMotorConfig& config) const { + return config.maxSpeed >= MOTOR_SPEED_MIN && + config.maxSpeed <= MOTOR_SPEED_MAX && + config.maxAcceleration >= MOTOR_ACCEL_MIN && + config.maxAcceleration <= MOTOR_ACCEL_MAX && + config.homingSpeed >= MOTOR_SPEED_MIN && + config.homingSpeed <= MOTOR_SPEED_MAX && + config.testSpeed >= MOTOR_SPEED_MIN && + config.testSpeed <= MOTOR_SPEED_MAX; +} + +bool RuntimeConfigManager::setPidConfig(const RuntimePidConfig& config) { + if (!validatePidConfig(config)) { + Debug.println("PID config rejected"); + return false; + } + const RuntimePidConfig previous = _pid; + _pid = config; + const bool saved = savePidConfig(); + if (!saved) { + _pid = previous; + applyPidConfig(); + Debug.println("PID config save failed"); + return false; + } + applyPidConfig(); + Debug.printf("PID config: Kp=%.4f Ki=%.4f Kd=%.4f period=%u alpha=%.3f " + "integral=%.3f retarget=%.5f neutral=%.4f\n", + _pid.kp, _pid.ki, _pid.kd, _pid.periodMs, _pid.alphaD, + _pid.integralLimit, _pid.minRetargetFrac, _pid.uNeutral); + return saved; +} + +bool RuntimeConfigManager::setBalanceConfig(const RuntimeBalanceConfig& config) { + if (!validateBalanceConfig(config)) { + Debug.println("Balance config rejected"); + return false; + } + const RuntimeBalanceConfig previous = _balance; + _balance = config; + const bool saved = saveBalanceConfig(); + if (!saved) { + _balance = previous; + Debug.println("Balance config save failed"); + return false; + } + Debug.printf("Balance config: hold=%lu delta=%.2f samples=%u period=%u\n", + static_cast(_balance.holdMs), + _balance.stopPressureDeltaKpa, + _balance.stopPressureSamples, + _balance.samplePeriodMs); + return saved; +} + +bool RuntimeConfigManager::setMotorConfig(const RuntimeMotorConfig& config) { + if (!validateMotorConfig(config)) { + Debug.println("Motor config rejected"); + return false; + } + const RuntimeMotorConfig previous = _motor; + _motor = config; + const bool saved = saveMotorConfig(); + if (!saved) { + _motor = previous; + applyMotorConfig(); + Debug.println("Motor config save failed"); + return false; + } + applyMotorConfig(); + Debug.printf("Motor config: max=%lu accel=%lu homing=%lu test=%lu\n", + static_cast(_motor.maxSpeed), + static_cast(_motor.maxAcceleration), + static_cast(_motor.homingSpeed), + static_cast(_motor.testSpeed)); + return saved; +} + +void RuntimeConfigManager::applyPidConfig() const { + pidController.Kp = _pid.kp; + pidController.Ki = _pid.ki; + pidController.Kd = _pid.kd; + pidController.periodMs = _pid.periodMs; + pidController.alphaD = _pid.alphaD; + pidController.integralLimit = _pid.integralLimit; + pidController.minRetargetFrac = _pid.minRetargetFrac; + pidController.uNeutral = _pid.uNeutral; +} + +void RuntimeConfigManager::applyMotorConfig() const { + ::motor.setMaxSpeed(_motor.maxSpeed); + ::motor.setAcceleration(_motor.maxAcceleration); +} + +void RuntimeConfigManager::formatPidConfigJson(char* buffer, size_t size) const { + snprintf(buffer, size, + "{\"kp\":%.6f,\"ki\":%.6f,\"kd\":%.6f," + "\"period_ms\":%u,\"alpha_d\":%.6f," + "\"integral_limit\":%.6f,\"min_retarget_frac\":%.6f," + "\"u_neutral\":%.6f}", + _pid.kp, _pid.ki, _pid.kd, + _pid.periodMs, _pid.alphaD, + _pid.integralLimit, _pid.minRetargetFrac, _pid.uNeutral); +} + +void RuntimeConfigManager::formatBalanceConfigJson(char* buffer, size_t size) const { + snprintf(buffer, size, + "{\"hold_ms\":%lu,\"stop_delta_kpa\":%.6f," + "\"stop_samples\":%u,\"sample_period_ms\":%u}", + static_cast(_balance.holdMs), + _balance.stopPressureDeltaKpa, + _balance.stopPressureSamples, + _balance.samplePeriodMs); +} + +void RuntimeConfigManager::formatMotorConfigJson(char* buffer, size_t size) const { + snprintf(buffer, size, + "{\"max_speed\":%lu,\"max_accel\":%lu," + "\"homing_speed\":%lu,\"test_speed\":%lu}", + static_cast(_motor.maxSpeed), + static_cast(_motor.maxAcceleration), + static_cast(_motor.homingSpeed), + static_cast(_motor.testSpeed)); +} + +void RuntimeConfigManager::loadPidConfig() { + Preferences prefs; + if (!prefs.begin(NVS_NAMESPACE, true)) { + return; + } + StoredConfig stored{}; + const size_t read = prefs.getBytes("pid", &stored, sizeof(stored)); + prefs.end(); + if (read == sizeof(stored) && + stored.magic == PID_MAGIC && + validatePidConfig(stored.config)) { + _pid = stored.config; + } +} + +void RuntimeConfigManager::loadBalanceConfig() { + Preferences prefs; + if (!prefs.begin(NVS_NAMESPACE, true)) { + return; + } + StoredConfig stored{}; + const size_t read = prefs.getBytes("balance", &stored, sizeof(stored)); + prefs.end(); + if (read == sizeof(stored) && + stored.magic == BALANCE_MAGIC && + validateBalanceConfig(stored.config)) { + _balance = stored.config; + } +} + +void RuntimeConfigManager::loadMotorConfig() { + Preferences prefs; + if (!prefs.begin(NVS_NAMESPACE, true)) { + return; + } + StoredConfig stored{}; + const size_t read = prefs.getBytes("motor", &stored, sizeof(stored)); + prefs.end(); + if (read == sizeof(stored) && + stored.magic == MOTOR_MAGIC && + validateMotorConfig(stored.config)) { + _motor = stored.config; + } +} + +bool RuntimeConfigManager::savePidConfig() const { + Preferences prefs; + if (!prefs.begin(NVS_NAMESPACE, false)) { + return false; + } + StoredConfig stored{PID_MAGIC, _pid}; + const size_t written = prefs.putBytes("pid", &stored, sizeof(stored)); + prefs.end(); + return written == sizeof(stored); +} + +bool RuntimeConfigManager::saveBalanceConfig() const { + Preferences prefs; + if (!prefs.begin(NVS_NAMESPACE, false)) { + return false; + } + StoredConfig stored{BALANCE_MAGIC, _balance}; + const size_t written = prefs.putBytes("balance", &stored, sizeof(stored)); + prefs.end(); + return written == sizeof(stored); +} + +bool RuntimeConfigManager::saveMotorConfig() const { + Preferences prefs; + if (!prefs.begin(NVS_NAMESPACE, false)) { + return false; + } + StoredConfig stored{MOTOR_MAGIC, _motor}; + const size_t written = prefs.putBytes("motor", &stored, sizeof(stored)); + prefs.end(); + return written == sizeof(stored); +} diff --git a/lib/sensors/include/sensors.h b/lib/sensors/include/sensors.h index edd34b2..ea2da49 100644 --- a/lib/sensors/include/sensors.h +++ b/lib/sensors/include/sensors.h @@ -55,6 +55,19 @@ class SensorManager { // Battery bus voltage (mV) — reads from INA219 on demand uint32_t batteryMilliVolts(); + // ----------------------------------------------------------------------- + // SIMULATORE (HIL da banco): quando attivo, sensorDepth()/pressure() — e + // quindi depth()/bottomDepth()/topDepth() che li usano — restituiscono una + // quota SIMULATA da un modello fisico mosso dalla posizione reale del motore. + // Il motore si muove davvero: PID_HOLD/PID_STEP/measure()/GO girano invariati. + // ----------------------------------------------------------------------- + void simEnable(bool on); + bool simEnabled() const { return _simEnabled; } + void simConfigure(float uNeutral, float accelGain, float dragQuad, float poolDepth); + void simReset(float sensorDepthM = 0.0f); + float simSensorDepth() const { return _simZ; } + void simFormatStatus(char* buffer, size_t bufferSize) const; + private: MS5837 _bar02; INA_Class _ina; @@ -63,6 +76,17 @@ class SensorManager { float _atmPressurePa = 0.0f; // Reference pressure set at startup float _surfaceTargetOffsetM = SURFACE_TARGET_OFFSET_M; + // --- Stato simulatore --- + bool _simEnabled = false; + float _simZ = 0.0f; // quota sensore simulata [m] + float _simV = 0.0f; // velocità verticale [m/s] (+ = giù/affonda) + unsigned long _simLastMs = 0; // 0 = primo step (inizializza dt) + float _simUNeutral = SIM_U_NEUTRAL; + float _simAccelGain = SIM_ACCEL_GAIN; + float _simDragQuad = SIM_DRAG_QUAD; + float _simPoolDepth = SIM_POOL_DEPTH; + void _simStep(); + void _initPressureSensor(); void _initPowerMonitor(); }; diff --git a/lib/sensors/src/sensors.cpp b/lib/sensors/src/sensors.cpp index 16bab94..74a5a4b 100644 --- a/lib/sensors/src/sensors.cpp +++ b/lib/sensors/src/sensors.cpp @@ -1,6 +1,7 @@ #include "sensors.h" #include "config.h" #include "led.h" +#include "motor.h" #include #include #include @@ -74,6 +75,10 @@ void SensorManager::_initPressureSensor() { // --------------------------------------------------------------------------- void SensorManager::read() { + if (_simEnabled) { + _simStep(); + return; + } _bar02.read(); } @@ -82,6 +87,7 @@ float SensorManager::depth() { } float SensorManager::sensorDepth() { + if (_simEnabled) return _simZ; return depthFromPressure(_bar02.pressure(MS5837::Pa)); } @@ -104,16 +110,83 @@ void SensorManager::setSurfaceTargetOffset(float meters) { } float SensorManager::referenceDepthForPhase(const char* phase) { - if (phase != nullptr && strcmp(phase, "hold_40cm") == 0) { - return topDepth(); - } - return bottomDepth(); + // Profondità RIPORTATA (pacchetti/grafico): sempre riferita alla CIMA del + // float, così parte da ~0 in superficie ed è un riferimento unico e continuo. + // Il CONTROLLO resta riferito al FONDO (depth() = bottomDepth()): il PID porta + // comunque il fondo a 2.5 m e la cima a 40 cm. L'offset cima→fondo va + // comunicato al giudice per l'hold profondo (regolamento Task 4). + (void)phase; + return topDepth(); } float SensorManager::pressure() { + if (_simEnabled) { + // Pressione coerente con la quota simulata del sensore (Stevino), così i + // log/pacchetti mostrano un kPa plausibile e depthFromPressure() tornerebbe _simZ. + return _atmPressurePa + WATER_DENSITY_FRESH * GRAVITY * _simZ; + } return _bar02.pressure(MS5837::Pa); } +// --------------------------------------------------------------------------- +// SIMULATORE +// --------------------------------------------------------------------------- +void SensorManager::simEnable(bool on) { + _simEnabled = on; + if (on) { + _simZ = 0.0f; // parte in superficie (sensore a quota 0) + _simV = 0.0f; + _simLastMs = 0; // forza l'inizializzazione del dt al primo step + } + Debug.printf("Float SIM %s\n", on ? "ON (barometro simulato, motore reale)" : "OFF"); +} + +void SensorManager::simConfigure(float uNeutral, float accelGain, float dragQuad, float poolDepth) { + _simUNeutral = constrain(uNeutral, 0.0f, 1.0f); + _simAccelGain = (accelGain > 0.0f) ? accelGain : _simAccelGain; + _simDragQuad = (dragQuad >= 0.0f) ? dragQuad : _simDragQuad; + _simPoolDepth = (poolDepth > 0.0f) ? poolDepth : _simPoolDepth; + Debug.printf("SIM cfg: uNeutral=%.3f accelGain=%.4f dragQuad=%.3f pool=%.2f m\n", + _simUNeutral, _simAccelGain, _simDragQuad, _simPoolDepth); +} + +void SensorManager::simReset(float sensorDepthM) { + _simZ = (sensorDepthM < 0.0f) ? 0.0f : sensorDepthM; + _simV = 0.0f; + _simLastMs = 0; +} + +void SensorManager::simFormatStatus(char* buffer, size_t bufferSize) const { + if (buffer == nullptr || bufferSize == 0) return; + snprintf(buffer, bufferSize, + "SIM %s | z=%.3f m v=%.3f m/s | uNeutral=%.3f accelGain=%.4f dragQuad=%.3f pool=%.2f m", + _simEnabled ? "ON" : "OFF", + _simZ, _simV, _simUNeutral, _simAccelGain, _simDragQuad, _simPoolDepth); +} + +void SensorManager::_simStep() { + const unsigned long now = millis(); + if (_simLastMs == 0) { _simLastMs = now; return; } // primo campione: solo inizializza + float dt = (now - _simLastMs) / 1000.0f; + _simLastMs = now; + if (dt <= 0.0f) return; + if (dt > SIM_MAX_DT_S) dt = SIM_MAX_DT_S; // evita salti d'integrazione se il loop si ferma + + // u reale dalla posizione del motore: include il ritardo di corsa del motore, + // così la taratura PID vede la stessa lentezza meccanica del float vero. + const float u = motorPosToU(motor.position()); + const float aBuoy = _simAccelGain * (u - _simUNeutral); // u>neutral => +a => affonda + const float aDrag = -_simDragQuad * _simV * fabsf(_simV); // drag quadratico, frena + _simV += (aBuoy + aDrag) * dt; + _simZ += _simV * dt; + + // Vincolo superficie: il sensore non emerge sopra il pelo (z>=0). + if (_simZ < 0.0f) { _simZ = 0.0f; if (_simV < 0.0f) _simV = 0.0f; } + // Vincolo fondo vasca: il FONDO del float tocca il fondo (z = pool - lunghezza). + const float zMax = _simPoolDepth - SENSOR_TO_BOTTOM_M; + if (zMax > 0.0f && _simZ > zMax) { _simZ = zMax; if (_simV > 0.0f) _simV = 0.0f; } +} + float SensorManager::temperature() { return _bar02.temperature(); } diff --git a/platformio.ini b/platformio.ini index 5a763e3..10a4d92 100644 --- a/platformio.ini +++ b/platformio.ini @@ -9,6 +9,8 @@ framework = arduino upload_speed = 921600 ; 460800 is also common, but 921600 is faster monitor_speed = 115200 monitor_filters = esp32_exception_decoder +monitor_echo = yes ; mostra a schermo ciò che digiti nel monitor +monitor_eol = LF ; Invio manda \n, atteso dal parser comandi seriale ; Common libraries for ESP32 environments lib_deps = @@ -57,29 +59,6 @@ lib_deps = ESPAsyncWebServer AsyncTCP -; ESPA pool test profile - conservative 70 cm water-depth targets -[env:espA_pool] -platform = ${esp32_common.platform} -board = ${esp32_common.board} -framework = ${esp32_common.framework} -upload_speed = ${esp32_common.upload_speed} -monitor_speed = ${esp32_common.monitor_speed} -monitor_filters = ${esp32_common.monitor_filters} -build_src_filter = + - -test_framework = unity -test_build_src = false - -build_flags = - ${esp32_common.build_flags} - -DESPA_BUILD - -DPOOL_TEST_PROFILE - -lib_deps = - ${esp32_common.lib_deps} - ESPAsyncWebServer - AsyncTCP - ; ESPA manual motor keyboard control [env:espA_manual_keyboard] platform = ${esp32_common.platform} diff --git a/src/espA/main.cpp b/src/espA/main.cpp index 2ddcc4c..8af60d9 100644 --- a/src/espA/main.cpp +++ b/src/espA/main.cpp @@ -39,17 +39,17 @@ #include "comms.h" #include "profile.h" #include "flash_storage.h" +#include "runtime_config.h" // --------------------------------------------------------------------------- // Global state // --------------------------------------------------------------------------- static uint8_t g_status = CMD_IDLE; -static uint8_t g_profileCount = 0; static bool g_autoModeActive = false; static bool g_autoCommitted = false; +static bool g_autoMissionDone = false; static bool g_idle = false; static bool g_debugModeActive = false; -static uint32_t g_testSpeed = MOTOR_MAX_SPEED; // Make debug_mode_active reachable by DebugSerial / comms (extern linkage) bool debug_mode_active = false; @@ -69,6 +69,8 @@ static void servicePidTuningSerial(); static void runSyringeSet(float uNorm, float durationS); static void runPidHold(float depthTarget, float durationS); static void runPidStep(float depthTarget); +static bool runVerticalProfiles(uint8_t& completedProfiles); +static void attemptAutoRecovery(); // --------------------------------------------------------------------------- // SETUP // --------------------------------------------------------------------------- @@ -98,13 +100,28 @@ void setup() { }); Debug.println("DebugSerial ready"); + // --- Runtime PID / balance / motor settings --- + runtimeConfig.begin(); + + // --- Runtime mission profile --- + profileManager.beginConfig(); + // --- Internal flash mission log --- + // LittleFS è persistente al ciclo di alimentazione: al boot il log della + // sessione precedente è ancora presente. Lo dumpiamo qui su Serial come + // comodità (se il monitor è già connesso), ma NON lo azzeriamo: dopo un + // test fallito spesso il monitor si collega in ritardo, quindi il log deve + // sopravvivere al power-cycle e restare leggibile con il comando DUMP_LOG. + // L'azzeramento avviene solo all'inizio di una nuova missione (resetEEPROM) + // o su comando esplicito (CMD_CLEAR_EEPROM). Stampa diretta su Serial (non + // Debug) per un CSV pulito, indipendente da debug_mode_active. if (flashStorage.begin()) { - if (!flashStorage.clearLog()) { - Debug.println("WARNING: flash log reset failed"); - } else { - Debug.println("Flash log ready"); + Serial.println("===== FLASH LOG DUMP (previous session) BEGIN ====="); + if (!flashStorage.printLogTo(Serial)) { + Serial.println("(no previous log or flash unavailable)"); } + Serial.println("===== FLASH LOG DUMP END ====="); + Debug.println("Flash log ready (use DUMP_LOG to re-read)"); } else { Debug.println("WARNING: flash log unavailable; stored data disabled"); } @@ -127,6 +144,7 @@ void setup() { // --- Motor + homing --- motor.begin(); + runtimeConfig.applyMotorConfig(); Debug.println("Initializing TOF sensor..."); if (!tofSensor.begin()) { @@ -202,7 +220,7 @@ void loop() { g_idle = false; ledController.setState(LEDState::COMMUNICATION); } - } else if (g_profileCount < PROFILE_MAX_COUNT && g_autoModeActive) { + } else if (!g_autoMissionDone && g_autoModeActive) { // No comms — activate autonomous mode Debug.println("No comms — entering auto mode"); ledController.setState(LEDState::AUTO_MODE); @@ -217,41 +235,33 @@ void loop() { { bool ack = g_autoCommitted ? true : comms.sendMessage(CMD1_ACK, 1000); - if (ack && motionController.motionAllowed()) { - Debug.println("MATE mission: starting vertical profiles"); - if (!g_autoCommitted) { - g_profileCount = 0; - } - profileManager.resetEEPROM(); - if (g_profileCount == 0) { - profileManager.logDeploymentPacket(); - } - - while (g_profileCount < PROFILE_MAX_COUNT && motionController.motionAllowed()) { - profileManager.beginProfile(g_profileCount + 1); - Debug.printf("Profile %d: PID descent to 2.5 m bottom reference\n", - g_profileCount + 1); - profileManager.measure(TARGET_DEPTH, STAT_TIME, TIMEOUT_PID_TIME); - if (!motionController.motionAllowed()) { - break; - } - - delay(500); + // Il comando GO resta inchiodato in _received perché sott'acqua non + // arrivano nuovi pacchetti: lo consumiamo subito così non riparte da + // solo al ritorno in IDLE e non dipendiamo da g_idle per il clear. + comms.clearCommand(); - Debug.printf("Profile %d: PID ascent to 40 cm top reference\n", - g_profileCount + 1); - profileManager.measure(TARGET_SHALLOW_BOTTOM_DEPTH, STAT_TIME, TIMEOUT_ASCENT); - if (!motionController.motionAllowed()) { - break; - } + // true se un profilo è stato interrotto da emergency stop (es. safety + // TOF): serve a decidere se tentare l'auto-recovery a fine missione. + bool aborted = false; + // Dichiarato qui (scope esterno) perché serve anche al blocco aborted. + uint8_t completedProfiles = 0; - motor.disableOutputs(); - g_profileCount++; - Debug.printf("Profile %d complete\n", g_profileCount); - delay(500); + if (ack && motionController.motionAllowed()) { + aborted = runVerticalProfiles(completedProfiles); + if (g_autoCommitted && + completedProfiles >= profileManager.config().profileCount) { + g_autoMissionDone = true; } } + // Auto-recovery: senza telemetria, un emergency stop lascerebbe il float + // bloccato sul fondo col LED rosso e ogni comando successivo rifiutato da + // motionAllowed(). Il record emergency_stop è già su flash; qui un homing + // cancella lo stop e riporta la siringa a galleggiamento, pronto per un GO. + if (aborted) { + attemptAutoRecovery(); + } + g_status = CMD_IDLE; break; } @@ -269,7 +279,7 @@ void loop() { case CMD_BALANCE: // Drive syringe to full extension then retraction { if (comms.sendMessage(CMD3_ACK, 1000)) { - motionController.balance(5000); + motionController.balance(); } g_status = CMD_IDLE; break; @@ -280,6 +290,7 @@ void loop() { { if (comms.sendMessage(CMD4_ACK, 1000)) { profileManager.clearEEPROM(); + g_autoMissionDone = false; } g_status = CMD_IDLE; break; @@ -290,6 +301,9 @@ void loop() { { if (comms.sendMessage(CMD5_ACK, 1000)) { g_autoModeActive = !g_autoModeActive; + if (g_autoModeActive) { + g_autoMissionDone = false; + } Debug.printf("Auto mode: %s\n", g_autoModeActive ? "ON" : "OFF"); ledController.setState(g_autoModeActive ? LEDState::AUTO_MODE : LEDState::IDLE); } @@ -309,13 +323,15 @@ void loop() { "\"pressure_kpa\":%.2f," "\"depth_m\":%.2f," "\"phase\":\"%s\"," - "\"sensor_depth_m\":%.2f}", + "\"sensor_depth_m\":%.2f," + "\"syringe_u\":%.4f}", COMPANY_NUMBER, static_cast(millis()) / 1000.0f, sensors.pressure() / 1000.0f, sensors.referenceDepthForPhase("live"), "live", - sensors.sensorDepth()); + sensors.sensorDepth(), + motorPosToU(motor.position())); comms.sendMessage(packet, 1000); Debug.println("Live snapshot sent"); @@ -335,42 +351,32 @@ void loop() { } // ----------------------------------------------------------------------- - case CMD_UPDATE_PID: // Update PID gains at runtime - { - if (comms.sendMessage(CMD8_ACK, 1000)) { - pidController.Kp = comms.lastCommand().params[0]; - pidController.Ki = comms.lastCommand().params[1]; - pidController.Kd = comms.lastCommand().params[2]; - Debug.printf("PID updated: Kp=%.3f Ki=%.3f Kd=%.3f\n", - pidController.Kp, pidController.Ki, pidController.Kd); - } - g_status = CMD_IDLE; - break; - } - - // ----------------------------------------------------------------------- - case CMD_UPDATE_PID_EXT: // Update PID period and derivative LPF coefficient + case CMD_PID_CONFIG_SET: { - if (comms.sendMessage(CMD14_ACK, 1000)) { - const float periodMs = comms.lastCommand().params[0]; - const float alphaD = comms.lastCommand().params[1]; - pidController.periodMs = (uint16_t)constrain(periodMs, 20.0f, 500.0f); - pidController.alphaD = constrain(alphaD, 0.05f, 1.0f); - Debug.printf("PID ext updated: periodMs=%u alphaD=%.3f\n", - pidController.periodMs, pidController.alphaD); - } + const output_message cmd = comms.lastCommand(); + const PidConfigPayload& payload = cmd.payload.pidConfig; + RuntimePidConfig nextConfig; + nextConfig.kp = payload.kp; + nextConfig.ki = payload.ki; + nextConfig.kd = payload.kd; + nextConfig.periodMs = static_cast(payload.periodMs); + nextConfig.alphaD = payload.alphaD; + nextConfig.integralLimit = payload.integralLimit; + nextConfig.minRetargetFrac = payload.minRetargetFrac; + nextConfig.uNeutral = payload.uNeutral; + + const bool updated = runtimeConfig.setPidConfig(nextConfig); + comms.sendMessage(updated ? CMD8_ACK : CMD8_ERR, 1000); g_status = CMD_IDLE; break; } // ----------------------------------------------------------------------- - case CMD_SET_SPEED: // Set test movement speed + case CMD_PID_CONFIG_GET: { - if (comms.sendMessage(CMD9_ACK, 1000)) { - uint32_t freq = comms.lastCommand().freq; - g_testSpeed = constrain(freq, 10u, 1200u); - Debug.printf("Test speed set to %u steps/s\n", g_testSpeed); - } + char packet[OUTPUT_LEN]; + runtimeConfig.formatPidConfigJson(packet, sizeof(packet)); + comms.sendMessage(packet, 1000); g_status = CMD_IDLE; break; } @@ -379,8 +385,8 @@ void loop() { case CMD_TEST_STEPS: // Manual stepper test { if (comms.sendMessage(CMD10_ACK, 1000)) { - long steps = comms.lastCommand().steps; - motionController.manualStepTest(steps, g_testSpeed); + long steps = comms.lastCommand().payload.testSteps.steps; + motionController.manualStepTest(steps, runtimeConfig.motor().testSpeed); } g_status = CMD_IDLE; break; @@ -428,8 +434,10 @@ void loop() { case CMD_SYRINGE_SET: // Test: posiziona siringa a u in [0,1] per N secondi { if (comms.sendMessage(CMD15_ACK, 1000)) { - const float u = comms.lastCommand().params[0]; - const float dur = comms.lastCommand().params[1]; + const output_message cmd = comms.lastCommand(); + const SyringeSetPayload& payload = cmd.payload.syringeSet; + const float u = payload.uNorm; + const float dur = payload.durationS; runSyringeSet(u, dur); } g_status = CMD_IDLE; @@ -440,8 +448,10 @@ void loop() { case CMD_PID_HOLD: // Test: PID a quota fissa per N secondi { if (comms.sendMessage(CMD16_ACK, 1000)) { - const float depth = comms.lastCommand().params[0]; - const float dur = comms.lastCommand().params[1]; + const output_message cmd = comms.lastCommand(); + const PidHoldPayload& payload = cmd.payload.pidHold; + const float depth = payload.depthM; + const float dur = payload.durationS; runPidHold(depth, dur); } g_status = CMD_IDLE; @@ -452,7 +462,7 @@ void loop() { case CMD_PID_STEP: // Test: step response PID a quota X per 60 s { if (comms.sendMessage(CMD17_ACK, 1000)) { - const float depth = comms.lastCommand().params[0]; + const float depth = comms.lastCommand().payload.pidStep.depthM; runPidStep(depth); } g_status = CMD_IDLE; @@ -463,12 +473,97 @@ void loop() { case CMD_SET_SURFACE_OFFSET: // Imposta target di galleggiamento (m sotto pelo) { if (comms.sendMessage(CMD18_ACK, 1000)) { - sensors.setSurfaceTargetOffset(comms.lastCommand().params[0]); + sensors.setSurfaceTargetOffset(comms.lastCommand().payload.surfaceOffset.meters); } g_status = CMD_IDLE; break; } + // ----------------------------------------------------------------------- + case CMD_PROFILE_SET: + { + const output_message cmd = comms.lastCommand(); + const ProfileSetPayload& payload = cmd.payload.profileSet; + RuntimeProfileConfig nextConfig; + nextConfig.profileCount = payload.profileCount; + nextConfig.descentTargetM = payload.descentTargetM; + nextConfig.ascentTargetM = payload.ascentTargetM; + nextConfig.depthToleranceM = payload.depthToleranceM; + nextConfig.holdTimeS = payload.holdTimeS; + nextConfig.descentTimeoutS = payload.descentTimeoutS; + nextConfig.ascentTimeoutS = payload.ascentTimeoutS; + nextConfig.surfaceRestOffsetM = payload.surfaceRestOffsetM; + + const bool updated = profileManager.setConfig(nextConfig); + comms.sendMessage(updated ? CMD19_ACK : CMD19_ERR, 1000); + g_status = CMD_IDLE; + break; + } + + // ----------------------------------------------------------------------- + case CMD_PROFILE_GET: + { + char packet[OUTPUT_LEN]; + profileManager.formatConfigJson(packet, sizeof(packet)); + comms.sendMessage(packet, 1000); + g_status = CMD_IDLE; + break; + } + + // ----------------------------------------------------------------------- + case CMD_BALANCE_CONFIG_SET: + { + const output_message cmd = comms.lastCommand(); + const BalanceConfigPayload& payload = cmd.payload.balanceConfig; + RuntimeBalanceConfig nextConfig; + nextConfig.holdMs = payload.holdMs; + nextConfig.stopPressureDeltaKpa = payload.stopPressureDeltaKpa; + nextConfig.stopPressureSamples = payload.stopPressureSamples; + nextConfig.samplePeriodMs = payload.samplePeriodMs; + + const bool updated = runtimeConfig.setBalanceConfig(nextConfig); + comms.sendMessage(updated ? CMD21_ACK : CMD21_ERR, 1000); + g_status = CMD_IDLE; + break; + } + + // ----------------------------------------------------------------------- + case CMD_BALANCE_CONFIG_GET: + { + char packet[OUTPUT_LEN]; + runtimeConfig.formatBalanceConfigJson(packet, sizeof(packet)); + comms.sendMessage(packet, 1000); + g_status = CMD_IDLE; + break; + } + + // ----------------------------------------------------------------------- + case CMD_MOTOR_CONFIG_SET: + { + const output_message cmd = comms.lastCommand(); + const MotorConfigPayload& payload = cmd.payload.motorConfig; + RuntimeMotorConfig nextConfig; + nextConfig.maxSpeed = payload.maxSpeed; + nextConfig.maxAcceleration = payload.maxAcceleration; + nextConfig.homingSpeed = payload.homingSpeed; + nextConfig.testSpeed = payload.testSpeed; + + const bool updated = runtimeConfig.setMotorConfig(nextConfig); + comms.sendMessage(updated ? CMD23_ACK : CMD23_ERR, 1000); + g_status = CMD_IDLE; + break; + } + + // ----------------------------------------------------------------------- + case CMD_MOTOR_CONFIG_GET: + { + char packet[OUTPUT_LEN]; + runtimeConfig.formatMotorConfigJson(packet, sizeof(packet)); + comms.sendMessage(packet, 1000); + g_status = CMD_IDLE; + break; + } + // ----------------------------------------------------------------------- default: Debug.printf("Unknown command: %d\n", g_status); @@ -483,8 +578,10 @@ void loop() { // PID TUNING — comandi via seriale USB diretta // // Comandi accettati (uno per riga, terminato da \n): -// PARAMS — aggiorna guadagni PID -// PARAMS_EXT — aggiorna periodo tick e LPF coeff +// PID_CONFIG_SET +// +// — aggiorna e salva configurazione PID +// PID_CONFIG_GET — stampa configurazione PID corrente // SYRINGE_SET — siringa a posizione normalizzata [0,1] // per N secondi, log depth ogni 100 ms // PID_HOLD — PID a quota X per N secondi, log a 5 Hz @@ -492,6 +589,16 @@ void loop() { // max 60 s (esci a regime), log a 10 Hz // SURFACE_OFFSET — target di galleggiamento: il top del // float sta a sotto il pelo (default 0.10) +// SIM_ON / SIM_OFF — simulatore barometro on/off. Il motore si +// muove DAVVERO; la quota è simulata da un +// modello fisico mosso dalla siringa. Poi usa +// PID_STEP/PID_HOLD/GO per tarare il PID a secco. +// SIM_GET — stato e parametri del simulatore +// SIM_CONFIG +// — ritara la fisica del simulatore a runtime +// GO — lancia la missione completa (profili + +// sosta) da seriale, senza GUI/ESPB. Con SIM +// attivo = test end-to-end al banco a secco. // // Tutto il logging finisce su Serial (USB), formato CSV per facile import. // --------------------------------------------------------------------------- @@ -509,30 +616,35 @@ static void servicePidTuningSerial() { // Tokenize semplice (solo separatore spazio) const char* cstr = line.c_str(); - char buf[96]; + char buf[192]; strncpy(buf, cstr, sizeof(buf) - 1); buf[sizeof(buf) - 1] = '\0'; char* tok = strtok(buf, " "); if (!tok) return; - if (strcmp(tok, "PARAMS") == 0) { - char* a = strtok(nullptr, " "); - char* b = strtok(nullptr, " "); - char* d = strtok(nullptr, " "); - if (!a || !b || !d) { Debug.println("ERR: PARAMS "); return; } - pidController.Kp = atof(a); - pidController.Ki = atof(b); - pidController.Kd = atof(d); - Debug.printf("OK PARAMS Kp=%.4f Ki=%.4f Kd=%.4f\n", - pidController.Kp, pidController.Ki, pidController.Kd); - } else if (strcmp(tok, "PARAMS_EXT") == 0) { - char* a = strtok(nullptr, " "); - char* b = strtok(nullptr, " "); - if (!a || !b) { Debug.println("ERR: PARAMS_EXT "); return; } - pidController.periodMs = (uint16_t)constrain(atof(a), 20.0f, 500.0f); - pidController.alphaD = constrain((float)atof(b), 0.05f, 1.0f); - Debug.printf("OK PARAMS_EXT period=%u alpha=%.3f\n", - pidController.periodMs, pidController.alphaD); + if (strcmp(tok, "PID_CONFIG_SET") == 0) { + char* values[8] = {}; + for (char*& value : values) { + value = strtok(nullptr, " "); + if (!value) { + Debug.println("ERR: PID_CONFIG_SET "); + return; + } + } + RuntimePidConfig config; + config.kp = atof(values[0]); + config.ki = atof(values[1]); + config.kd = atof(values[2]); + config.periodMs = static_cast(atof(values[3])); + config.alphaD = atof(values[4]); + config.integralLimit = atof(values[5]); + config.minRetargetFrac = atof(values[6]); + config.uNeutral = atof(values[7]); + Debug.println(runtimeConfig.setPidConfig(config) ? "OK PID_CONFIG_SET" : "ERR PID_CONFIG_SET invalid"); + } else if (strcmp(tok, "PID_CONFIG_GET") == 0) { + char packet[OUTPUT_LEN]; + runtimeConfig.formatPidConfigJson(packet, sizeof(packet)); + Debug.println(packet); } else if (strcmp(tok, "SYRINGE_SET") == 0) { char* a = strtok(nullptr, " "); char* b = strtok(nullptr, " "); @@ -552,12 +664,59 @@ static void servicePidTuningSerial() { if (!a) { Debug.println("ERR: SURFACE_OFFSET "); return; } sensors.setSurfaceTargetOffset(atof(a)); Debug.printf("OK SURFACE_OFFSET %.3f m\n", sensors.surfaceTargetOffset()); + } else if (strcmp(tok, "SIM_ON") == 0) { + sensors.simEnable(true); + Debug.println("OK SIM_ON (barometro simulato, motore reale)"); + } else if (strcmp(tok, "SIM_OFF") == 0) { + sensors.simEnable(false); + Debug.println("OK SIM_OFF"); + } else if (strcmp(tok, "SIM_GET") == 0) { + char buf[176]; + sensors.simFormatStatus(buf, sizeof(buf)); + Debug.println(buf); + } else if (strcmp(tok, "SIM_CONFIG") == 0) { + char* values[4] = {}; + for (char*& value : values) { + value = strtok(nullptr, " "); + if (!value) { + Debug.println("ERR: SIM_CONFIG "); + return; + } + } + sensors.simConfigure(atof(values[0]), atof(values[1]), + atof(values[2]), atof(values[3])); + Debug.println("OK SIM_CONFIG"); + } else if (strcmp(tok, "GO") == 0) { + // Lancia la missione completa (profileCount profili + sosta) + // direttamente da seriale, senza GUI/ESPB: utile col SIM per il + // test end-to-end al banco. Bloccante fino a fine missione; + // per fermarla prima resetta ESPA. + if (!motionController.motionAllowed()) { + Debug.println("ERR: GO — motion not allowed (serve homing ok)"); + return; + } + Debug.println("# GO: missione completa (profili + sosta). Reset ESPA per abortire."); + uint8_t completed = 0; + const bool aborted = runVerticalProfiles(completed); + if (aborted) attemptAutoRecovery(); + Debug.printf("# GO done: %u/%u profili%s\n", + completed, profileManager.config().profileCount, + aborted ? " (ABORT)" : ""); + } else if (strcmp(tok, "DUMP_LOG") == 0) { + // Dump del flash log su richiesta: risolve il caso in cui il + // serial monitor si collega dopo il dump automatico nel setup(). + // NON azzera il log, così può essere riletto più volte. + Serial.println("===== FLASH LOG DUMP (on demand) BEGIN ====="); + if (!flashStorage.printLogTo(Serial)) { + Serial.println("(no log or flash unavailable)"); + } + Serial.println("===== FLASH LOG DUMP END ====="); } else { Debug.printf("ERR: unknown cmd '%s'\n", tok); } return; } - if (g_serialLineBuf.length() < 95) g_serialLineBuf += c; + if (g_serialLineBuf.length() < 191) g_serialLineBuf += c; } } @@ -580,8 +739,19 @@ static void runSyringeSet(float uNorm, float durationS) { const unsigned long t0 = millis(); unsigned long lastLog = 0; + unsigned long lastTofSampleMs = 0; while (millis() - t0 < (unsigned long)(durationS * 1000.0f)) { if (Serial.available()) { Debug.println("# aborted"); break; } + // Supervisione TOF su ogni movimento: fondo corsa esteso (tappo) = stop + // pulito; oltre il limite superiore = emergency (già scattato dentro). + const TofGuard guard = motionController.tofGuard(millis(), lastTofSampleMs, "syringe"); + if (guard == TofGuard::ExtendLimit) { + motor.stop(); + Debug.println("# extension limit (TOF)"); + } else if (guard == TofGuard::Emergency) { + Debug.println("# aborted (TOF)"); + break; + } if (millis() - lastLog >= 100) { lastLog = millis(); sensors.read(); @@ -596,50 +766,59 @@ static void runSyringeSet(float uNorm, float durationS) { Debug.println("# done"); } -// Hold PID a quota fissa per N secondi, log a 5 Hz con CSV completo. -// Versione "tarable" di un profile PID phase, senza pre-position né hold check. -static void runPidHold(float depthTarget, float durationS) { - if (depthTarget < 0.1f || depthTarget > 5.0f) { - Debug.println("ERR: depthTarget in [0.1, 5.0] m"); - return; - } - if (durationS < 1.0f || durationS > 600.0f) { - Debug.println("ERR: durationS in [1, 600]"); - return; - } - - Debug.printf("# PID_HOLD target=%.3fm dur=%.1fs Kp=%.4f Ki=%.4f Kd=%.4f " - "period=%u alpha=%.3f\n", - depthTarget, durationS, - pidController.Kp, pidController.Ki, pidController.Kd, - pidController.periodMs, pidController.alphaD); - Debug.println("# t_ms,depth_m,target_m,error_m,u_norm,motor_pos"); - +// Loop PID condiviso da PID_HOLD e PID_STEP: tiene la quota target per +// durationMs, ricalcolando il setpoint a pidController.periodMs e loggando il +// CSV ogni logPeriodMs. La supervisione TOF, il deadband e la saturazione al +// fondo corsa sono identici fra i due comandi: vivono qui per non divergere. +static void runPidLoop(float depthTarget, unsigned long durationMs, unsigned long logPeriodMs) { pidController.reset(); motor.enableOutputs(); const long usable = (long)MOTOR_MAX_STEPS - 2L * (long)MOTOR_ENDSTOP_MARGIN; - const long deadbandSteps = (long)(PID_MIN_RETARGET_FRAC * (float)usable); + const long deadbandSteps = (long)(pidController.minRetargetFrac * (float)usable); long lastCommandedTarget = motor.position(); const unsigned long t0 = millis(); unsigned long lastTick = 0; unsigned long lastLog = 0; - while (millis() - t0 < (unsigned long)(durationS * 1000.0f)) { + unsigned long lastTofSampleMs = 0; + bool atExtensionLimit = false; // pistone fermo al fondo corsa (tappo): non ricomandare verso l'estensione + while (millis() - t0 < durationMs) { if (Serial.available()) { Debug.println("# aborted"); break; } if (motionController.remoteStopRequested()) { Debug.println("# remote stop"); break; } + // Supervisione TOF su ogni movimento. ExtendLimit = saturazione normale a + // piena estensione: NON aborte, ferma e inibisce ulteriore estensione + // finché il PID non chiede di risalire. Emergency = anomalia → abort. + const TofGuard guard = motionController.tofGuard(millis(), lastTofSampleMs, "pid"); + if (guard == TofGuard::ExtendLimit) { + if (!atExtensionLimit) { + motor.stop(); + lastCommandedTarget = motor.position(); + atExtensionLimit = true; + } + } else if (guard == TofGuard::Emergency) { + Debug.println("# aborted (TOF)"); + break; + } + if (millis() - lastTick >= pidController.periodMs) { lastTick = millis(); sensors.read(); const float depth = sensors.depth(); const float u = pidController.computeNormalized(depthTarget, depth); const long posTarget = uToMotorPos(u); - if (labs(posTarget - lastCommandedTarget) >= deadbandSteps) { + // In saturazione al fondo corsa accetta solo target che fanno + // RISALIRE (verso home = pos più alta); ignora richieste di ulteriore + // estensione, che riaprirebbero il tappo. + const bool retreating = posTarget > motor.position(); + if ((!atExtensionLimit || retreating) && + labs(posTarget - lastCommandedTarget) >= deadbandSteps) { motor.startMoveTo(posTarget); lastCommandedTarget = posTarget; + atExtensionLimit = false; } - if (millis() - lastLog >= 200) { + if (millis() - lastLog >= logPeriodMs) { lastLog = millis(); Debug.printf("%lu,%.3f,%.3f,%.3f,%.3f,%ld\n", millis() - t0, depth, depthTarget, @@ -654,6 +833,28 @@ static void runPidHold(float depthTarget, float durationS) { Debug.println("# done"); } +// Hold PID a quota fissa per N secondi, log a 5 Hz con CSV completo. +// Versione "tarable" di un profile PID phase, senza pre-position né hold check. +static void runPidHold(float depthTarget, float durationS) { + if (depthTarget < 0.1f || depthTarget > 5.0f) { + Debug.println("ERR: depthTarget in [0.1, 5.0] m"); + return; + } + if (durationS < 1.0f || durationS > 600.0f) { + Debug.println("ERR: durationS in [1, 600]"); + return; + } + + Debug.printf("# PID_HOLD target=%.3fm dur=%.1fs Kp=%.4f Ki=%.4f Kd=%.4f " + "period=%u alpha=%.3f\n", + depthTarget, durationS, + pidController.Kp, pidController.Ki, pidController.Kd, + pidController.periodMs, pidController.alphaD); + Debug.println("# t_ms,depth_m,target_m,error_m,u_norm,motor_pos"); + + runPidLoop(depthTarget, (unsigned long)(durationS * 1000.0f), 200); +} + // Step response: cambio istantaneo del setpoint, esci a 60 s. // Identico a PID_HOLD ma con durata fissa e log a 10 Hz per catturare la rampa. static void runPidStep(float depthTarget) { @@ -668,41 +869,69 @@ static void runPidStep(float depthTarget) { pidController.periodMs, pidController.alphaD); Debug.println("# t_ms,depth_m,target_m,error_m,u_norm,motor_pos"); - pidController.reset(); - motor.enableOutputs(); - - const long usable = (long)MOTOR_MAX_STEPS - 2L * (long)MOTOR_ENDSTOP_MARGIN; - const long deadbandSteps = (long)(PID_MIN_RETARGET_FRAC * (float)usable); - long lastCommandedTarget = motor.position(); + runPidLoop(depthTarget, 60000UL, 100); +} - const unsigned long t0 = millis(); - unsigned long lastTick = 0; - unsigned long lastLog = 0; - while (millis() - t0 < 60000UL) { - if (Serial.available()) { Debug.println("# aborted"); break; } - if (motionController.remoteStopRequested()) { Debug.println("# remote stop"); break; } +// --------------------------------------------------------------------------- +// Missione completa: profileCount profili (discesa→hold→risalita→hold) + sosta +// finale. Riusata dal case CMD_GO (GUI/ESP-NOW/auto) e dal comando seriale +// diretto "GO" (test al banco / simulatore). Ritorna true se abortita da +// emergency stop; scrive in completedProfiles il numero di profili conclusi. +static bool runVerticalProfiles(uint8_t& completedProfiles) { + completedProfiles = 0; + bool aborted = false; + const RuntimeProfileConfig& profileConfig = profileManager.config(); + Debug.println("Mission: starting vertical profiles"); + + profileManager.resetEEPROM(); + profileManager.logDeploymentPacket(); + + while (completedProfiles < profileConfig.profileCount && motionController.motionAllowed()) { + profileManager.beginProfile(completedProfiles + 1); + Debug.printf("Profile %d: PID descent to %.2f m bottom reference\n", + completedProfiles + 1, profileConfig.descentTargetM); + profileManager.measure(profileConfig.descentTargetM, + profileConfig.holdTimeS, + profileConfig.descentTimeoutS); + if (!motionController.motionAllowed()) { aborted = true; break; } + + delay(500); + + Debug.printf("Profile %d: PID ascent to %.2f m top reference\n", + completedProfiles + 1, profileConfig.ascentTargetM); + profileManager.measure(profileManager.ascentTargetBottomM(), + profileConfig.holdTimeS, + profileConfig.ascentTimeoutS); + if (!motionController.motionAllowed()) { aborted = true; break; } + + motor.disableOutputs(); + completedProfiles++; + Debug.printf("Profile %d complete\n", completedProfiles); + delay(500); + } + + // Sosta finale: tieni la CIMA del float a surfaceRestOffsetM sotto il pelo + // (antenna sommersa) finché non arriva il recupero o scade la finestra — + // evita di rompere la superficie (penalità) in attesa dell'ROV. + if (!aborted && motionController.motionAllowed()) { + Debug.printf("Mission: surface rest, top at %.2f m below surface\n", + profileConfig.surfaceRestOffsetM); + profileManager.measure(profileManager.restTargetBottomM(), + profileConfig.holdTimeS, + REST_WINDOW_S); + if (!motionController.motionAllowed()) aborted = true; + } + + return aborted; +} - if (millis() - lastTick >= pidController.periodMs) { - lastTick = millis(); - sensors.read(); - const float depth = sensors.depth(); - const float u = pidController.computeNormalized(depthTarget, depth); - const long posTarget = uToMotorPos(u); - if (labs(posTarget - lastCommandedTarget) >= deadbandSteps) { - motor.startMoveTo(posTarget); - lastCommandedTarget = posTarget; - } - if (millis() - lastLog >= 100) { - lastLog = millis(); - Debug.printf("%lu,%.3f,%.3f,%.3f,%.3f,%ld\n", - millis() - t0, depth, depthTarget, - depthTarget - depth, u, motor.position()); - } - } - ledController.update(); - yield(); +// Auto-recovery su abort: l'homing cancella l'emergency stop e riporta la siringa +// in posizione nota (galleggiamento), così il float risale e resta pronto. +static void attemptAutoRecovery() { + Debug.println("Profile aborted by emergency stop — attempting auto-recovery"); + if (motionController.homeWithTof()) { + Debug.println("Auto-recovery homing complete — float ready"); + } else { + Debug.println("Auto-recovery homing FAILED — float remains in error"); } - motor.stop(); - motor.disableOutputs(); - Debug.println("# done"); } diff --git a/src/espA_manual_keyboard/main.cpp b/src/espA_manual_keyboard/main.cpp index 4a916ba..c2c9fa1 100644 --- a/src/espA_manual_keyboard/main.cpp +++ b/src/espA_manual_keyboard/main.cpp @@ -12,6 +12,11 @@ constexpr float MANUAL_MOTOR_SPEED = MOTOR_MAX_SPEED; constexpr float MANUAL_MOTOR_ACCELERATION = MOTOR_MAX_ACCELERATION; constexpr uint16_t MANUAL_HOLD_TIMEOUT_MS = 250; constexpr uint16_t TOF_PRINT_PERIOD_MS = 100; +// Passo del jog manuale (~2 mm). Abbastanza lungo da non esaurirsi tra due +// ripetizioni del tasto tenuto premuto, abbastanza corto da fermarsi presto al +// rilascio. Bypassa il clamp software (vedi startJogStepsUnclamped). +constexpr long MANUAL_JOG_STEPS = + static_cast(2.0f * MOTOR_STEPS_PER_MM + 0.5f); uint8_t escapeState = 0; bool outputsEnabledForMove = false; @@ -42,12 +47,12 @@ void printHelp() { } void startHoldMove(int8_t direction) { - const long target = (direction < 0) - ? static_cast(MOTOR_ENDSTOP_MARGIN) - : static_cast(MOTOR_MAX_STEPS - MOTOR_ENDSTOP_MARGIN); - const long deltaSteps = target - motor.position(); - const float estimatedSeconds = - fabs(static_cast(deltaSteps)) / MANUAL_MOTOR_SPEED; + // Jog relativo NON clampato: il manual keyboard serve a recuperare un + // pistone disallineato, quindi deve poter uscire dal range nominale + // (±MAX-margin) in entrambi i versi. Ogni comando spinge di MANUAL_JOG_STEPS + // nel verso scelto; tenendo premuto il tasto il movimento si rinnova prima + // di esaurirsi (vedi loop()). Lo stop è manuale (space/x). + const long jog = static_cast(direction) * MANUAL_JOG_STEPS; lastMoveCommandMs = millis(); @@ -60,15 +65,14 @@ void startHoldMove(int8_t direction) { } motor.enableOutputs(); - motor.startMoveTo(target); + motor.startJogStepsUnclamped(jog); outputsEnabledForMove = true; activeDirection = direction; - Serial.printf("hold target=%ld step, delta=%ld step (%.1f mm), estimated-to-limit=%.1f s\n", - target, - deltaSteps, - static_cast(deltaSteps) / MOTOR_STEPS_PER_MM, - estimatedSeconds); + Serial.printf("jog %+ld step (%.1f mm) from pos=%ld step\n", + jog, + static_cast(jog) / MOTOR_STEPS_PER_MM, + motor.position()); } void moveTowardHome() { @@ -112,16 +116,12 @@ void printTofReading(const char* prefix) { static_cast(motor.position()) / MOTOR_STEPS_PER_MM ); - if (outputsEnabledForMove && - activeDirection > 0 && - TOF_MAX_STOP_DISTANCE_MM > 0.0f && - measurement.valid && - measurement.distanceMm >= TOF_MAX_STOP_DISTANCE_MM) { - Serial.printf("TOF max extension stop reached: %.1f >= %.1f mm\n", - measurement.distanceMm, - TOF_MAX_STOP_DISTANCE_MM); - stopMotorNow(); - } + // Nessun auto-stop su distanza TOF: il manual keyboard è puramente manuale + // (vedi printHelp: "no homing, start away from endstops"). La protezione + // contro i fine corsa è il clamp software di startMoveTo (±MAX-margin); lo + // stop d'emergenza resta su space/x. La vecchia guardia usava una costante + // rimossa e la convenzione geometrica precedente (estensione=positivo), + // incoerente con l'homing attuale. } void handleCommand(char command) { diff --git a/src/espB/main.cpp b/src/espB/main.cpp index c5a5ea6..668e5f6 100644 --- a/src/espB/main.cpp +++ b/src/espB/main.cpp @@ -36,7 +36,7 @@ const uint8_t BUILTIN_LED_PIN = 2; // Built-in LED pin on ESP32 /** PROGRAM GLOBAL CONSTANTS **/ -#define BUFFER_SIZE 64 // Serial command buffer size +#define BUFFER_SIZE 192 // Serial command buffer size const uint16_t MAX_CONN_TIME = 100; // Time in ms that has to elapse before send_message function stops to try a sending /** GLOBAL OBJECTS **/ @@ -50,7 +50,7 @@ char serialInput[BUFFER_SIZE]; // Serial software buffer used to empty the ha EspbBridgeState bridgeState; // Cached FLOAT status exposed to the GUI /** LED STATE MANAGEMENT **/ -FloatLEDState current_led_state = LED_INIT; +LEDState current_led_state = LEDState::OFF; unsigned long led_last_update = 0; @@ -77,14 +77,14 @@ input_message input; // Message received from espA /** FUNCTION DECLARATIONS **/ void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status); void OnDataRecv(const uint8_t * mac, const uint8_t *incomingData, int len); -void setLEDState(FloatLEDState state); +void setLEDState(LEDState state); void updateLED(); uint8_t send_command(const output_message& message, uint16_t max_conn_time); void serial_handler(); void setEspNowChannel(); /** LED CONTROL FUNCTIONS **/ -void setLEDState(FloatLEDState state) { +void setLEDState(LEDState state) { if (current_led_state != state) { current_led_state = state; led_last_update = millis(); @@ -95,16 +95,16 @@ void updateLED() { unsigned long currentTime = millis(); switch (current_led_state) { - case LED_OFF: + case LEDState::OFF: digitalWrite(BUILTIN_LED_PIN, LOW); break; - case LED_IDLE: + case LEDState::IDLE: // Solid on when idle and connected digitalWrite(BUILTIN_LED_PIN, HIGH); break; - case LED_ERROR: + case LEDState::ERROR: // Very fast blink for errors (150ms on/off) if ((currentTime - led_last_update) % 300 < 150) { digitalWrite(BUILTIN_LED_PIN, HIGH); @@ -177,7 +177,7 @@ void OnDataSent(const uint8_t * mac, esp_now_send_status_t status) { send_result = 1; // If sending succeeds sets the flag to 1 } else { send_result = 0; // Otherwise sets it to 0 - setLEDState(LED_ERROR); // Indicate communication error + setLEDState(LEDState::ERROR); // Indicate communication error } } @@ -213,10 +213,10 @@ uint8_t send_command(const output_message& message, uint16_t max_conn_time) { } if (send_result) { - setLEDState(LED_IDLE); // Communication successful + setLEDState(LEDState::IDLE); // Communication successful return 1; // If sending succeeds, function returns 1 } else if (millis() - prec_time > max_conn_time) { - setLEDState(LED_ERROR); // Communication failed + setLEDState(LEDState::ERROR); // Communication failed return 0; // If sending failed and max_conn_time milliseconds elapsed } } @@ -293,7 +293,7 @@ void setup() { } // Initialization complete - setLEDState(LED_IDLE); + setLEDState(LEDState::IDLE); Serial.println("ESPB initialized successfully"); } @@ -316,9 +316,7 @@ void loop() { if (parsed.type == EspbParsedCommandType::ForwardToEspA) { send_command(parsed.message, MAX_CONN_TIME); } else if (parsed.type == EspbParsedCommandType::Status) { - output_message dummy; - memset(&dummy, 0, sizeof(dummy)); - dummy.command = CMD_IDLE; + output_message dummy = makeOutputMessage(CMD_IDLE); const bool connectionOk = send_command(dummy, MAX_CONN_TIME); char statusLine[128]; diff --git a/test/integration/test_espnow_bridge/test_espnow_bridge.cpp b/test/integration/test_espnow_bridge/test_espnow_bridge.cpp index 7d4cc0f..b4b9f8d 100644 --- a/test/integration/test_espnow_bridge/test_espnow_bridge.cpp +++ b/test/integration/test_espnow_bridge/test_espnow_bridge.cpp @@ -56,9 +56,7 @@ bool sendBroadcastCommand(uint8_t command, uint32_t timeoutMs) { } bool sendCommandTo(const uint8_t* peerMac, uint8_t command, uint32_t timeoutMs) { - output_message message; - memset(&message, 0, sizeof(message)); - message.command = command; + output_message message = makeOutputMessage(static_cast(command)); sendResult = -1; const esp_err_t err = esp_now_send(peerMac, reinterpret_cast(&message), sizeof(message)); diff --git a/test/unit_hw/espb_bridge/test_parser/test_parser.cpp b/test/unit_hw/espb_bridge/test_parser/test_parser.cpp index fcf6965..984939f 100644 --- a/test/unit_hw/espb_bridge/test_parser/test_parser.cpp +++ b/test/unit_hw/espb_bridge/test_parser/test_parser.cpp @@ -39,6 +39,10 @@ void test_simple_commands_map_to_espa_codes() { {"DEBUG", CMD_DEBUG_MODE}, {"HOME_MOTOR", CMD_HOME}, {"STOP", CMD_STOP}, + {"PID_CONFIG_GET", CMD_PID_CONFIG_GET}, + {"PROFILE_GET", CMD_PROFILE_GET}, + {"BALANCE_CONFIG_GET", CMD_BALANCE_CONFIG_GET}, + {"MOTOR_CONFIG_GET", CMD_MOTOR_CONFIG_GET}, }; for (const Case& testCase : cases) { @@ -49,22 +53,58 @@ void test_simple_commands_map_to_espa_codes() { } void test_parameterized_commands_fill_payload() { - EspbParsedCommand params = espbParseSerialCommand("PARAMS 1.2 0.3 0.01"); + EspbParsedCommand params = espbParseSerialCommand("PID_CONFIG_SET 1.2 0.3 0.01 50 0.25 5 0.001 0.011"); TEST_ASSERT_EQUAL_UINT8(commandType(EspbParsedCommandType::ForwardToEspA), commandType(params.type)); - TEST_ASSERT_EQUAL_UINT8(CMD_UPDATE_PID, params.message.command); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.2f, params.message.params[0]); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.3f, params.message.params[1]); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.01f, params.message.params[2]); + TEST_ASSERT_EQUAL_UINT8(CMD_PID_CONFIG_SET, params.message.command); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 1.2f, params.message.payload.pidConfig.kp); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.3f, params.message.payload.pidConfig.ki); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.01f, params.message.payload.pidConfig.kd); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 50.0f, params.message.payload.pidConfig.periodMs); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.25f, params.message.payload.pidConfig.alphaD); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 5.0f, params.message.payload.pidConfig.integralLimit); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.001f, params.message.payload.pidConfig.minRetargetFrac); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.011f, params.message.payload.pidConfig.uNeutral); - EspbParsedCommand freq = espbParseSerialCommand("TEST_FREQ 300"); - TEST_ASSERT_EQUAL_UINT8(commandType(EspbParsedCommandType::ForwardToEspA), commandType(freq.type)); - TEST_ASSERT_EQUAL_UINT8(CMD_SET_SPEED, freq.message.command); - TEST_ASSERT_EQUAL_UINT16(300, freq.message.freq); + EspbParsedCommand balance = espbParseSerialCommand("BALANCE_CONFIG_SET 5000 5.0 3 50"); + TEST_ASSERT_EQUAL_UINT8(commandType(EspbParsedCommandType::ForwardToEspA), commandType(balance.type)); + TEST_ASSERT_EQUAL_UINT8(CMD_BALANCE_CONFIG_SET, balance.message.command); + TEST_ASSERT_EQUAL_UINT32(5000, balance.message.payload.balanceConfig.holdMs); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 5.0f, balance.message.payload.balanceConfig.stopPressureDeltaKpa); + TEST_ASSERT_EQUAL_UINT8(3, balance.message.payload.balanceConfig.stopPressureSamples); + TEST_ASSERT_EQUAL_UINT16(50, balance.message.payload.balanceConfig.samplePeriodMs); + + EspbParsedCommand motorConfig = espbParseSerialCommand("MOTOR_CONFIG_SET 1800 1800 1200 300"); + TEST_ASSERT_EQUAL_UINT8(commandType(EspbParsedCommandType::ForwardToEspA), commandType(motorConfig.type)); + TEST_ASSERT_EQUAL_UINT8(CMD_MOTOR_CONFIG_SET, motorConfig.message.command); + TEST_ASSERT_EQUAL_UINT32(1800, motorConfig.message.payload.motorConfig.maxSpeed); + TEST_ASSERT_EQUAL_UINT32(1800, motorConfig.message.payload.motorConfig.maxAcceleration); + TEST_ASSERT_EQUAL_UINT32(1200, motorConfig.message.payload.motorConfig.homingSpeed); + TEST_ASSERT_EQUAL_UINT32(300, motorConfig.message.payload.motorConfig.testSpeed); EspbParsedCommand steps = espbParseSerialCommand("TEST_STEPS -100"); TEST_ASSERT_EQUAL_UINT8(commandType(EspbParsedCommandType::ForwardToEspA), commandType(steps.type)); TEST_ASSERT_EQUAL_UINT8(CMD_TEST_STEPS, steps.message.command); - TEST_ASSERT_EQUAL_INT32(-100, steps.message.steps); + TEST_ASSERT_EQUAL_INT32(-100, steps.message.payload.testSteps.steps); + + EspbParsedCommand profile = espbParseSerialCommand("PROFILE_SET 2 2.5 0.4 0.33 30 180 120 0.10"); + TEST_ASSERT_EQUAL_UINT8(commandType(EspbParsedCommandType::ForwardToEspA), commandType(profile.type)); + TEST_ASSERT_EQUAL_UINT8(CMD_PROFILE_SET, profile.message.command); + TEST_ASSERT_EQUAL_UINT8(2, profile.message.payload.profileSet.profileCount); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 2.5f, profile.message.payload.profileSet.descentTargetM); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.4f, profile.message.payload.profileSet.ascentTargetM); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.33f, profile.message.payload.profileSet.depthToleranceM); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 30.0f, profile.message.payload.profileSet.holdTimeS); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 180.0f, profile.message.payload.profileSet.descentTimeoutS); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 120.0f, profile.message.payload.profileSet.ascentTimeoutS); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.10f, profile.message.payload.profileSet.surfaceRestOffsetM); +} + +void test_output_message_protocol_shape() { + TEST_ASSERT_LESS_OR_EQUAL_UINT16(250, sizeof(output_message)); + + output_message message = makeOutputMessage(CMD_GO); + TEST_ASSERT_EQUAL_UINT8(CMD_GO, message.command); + TEST_ASSERT_EQUAL_UINT8(0, message.payload.empty.reserved); } void test_status_is_local_command() { @@ -77,9 +117,16 @@ void test_invalid_commands_are_rejected() { TEST_ASSERT_EQUAL_UINT8(commandType(EspbParsedCommandType::Invalid), commandType(espbParseSerialCommand("").type)); TEST_ASSERT_EQUAL_UINT8(commandType(EspbParsedCommandType::Invalid), commandType(espbParseSerialCommand("UNKNOWN").type)); TEST_ASSERT_EQUAL_UINT8(commandType(EspbParsedCommandType::Invalid), commandType(espbParseSerialCommand("GO extra").type)); - TEST_ASSERT_EQUAL_UINT8(commandType(EspbParsedCommandType::Invalid), commandType(espbParseSerialCommand("PARAMS 1 2").type)); - TEST_ASSERT_EQUAL_UINT8(commandType(EspbParsedCommandType::Invalid), commandType(espbParseSerialCommand("TEST_FREQ -1").type)); + TEST_ASSERT_EQUAL_UINT8(commandType(EspbParsedCommandType::Invalid), commandType(espbParseSerialCommand("PARAMS 1 2 3").type)); + TEST_ASSERT_EQUAL_UINT8(commandType(EspbParsedCommandType::Invalid), commandType(espbParseSerialCommand("PARAMS_EXT 50 0.25").type)); + TEST_ASSERT_EQUAL_UINT8(commandType(EspbParsedCommandType::Invalid), commandType(espbParseSerialCommand("TEST_FREQ 300").type)); + TEST_ASSERT_EQUAL_UINT8(commandType(EspbParsedCommandType::Invalid), commandType(espbParseSerialCommand("PID_CONFIG_SET 1 2 3").type)); + TEST_ASSERT_EQUAL_UINT8(commandType(EspbParsedCommandType::Invalid), commandType(espbParseSerialCommand("BALANCE_CONFIG_SET 5000 5 3").type)); + TEST_ASSERT_EQUAL_UINT8(commandType(EspbParsedCommandType::Invalid), commandType(espbParseSerialCommand("MOTOR_CONFIG_SET 1800 1800 1200").type)); TEST_ASSERT_EQUAL_UINT8(commandType(EspbParsedCommandType::Invalid), commandType(espbParseSerialCommand("TEST_STEPS nope").type)); + TEST_ASSERT_EQUAL_UINT8(commandType(EspbParsedCommandType::Invalid), commandType(espbParseSerialCommand("PROFILE_GET extra").type)); + TEST_ASSERT_EQUAL_UINT8(commandType(EspbParsedCommandType::Invalid), commandType(espbParseSerialCommand("PROFILE_SET 0 2.5 0.4 0.33 30 180 120 0.10").type)); + TEST_ASSERT_EQUAL_UINT8(commandType(EspbParsedCommandType::Invalid), commandType(espbParseSerialCommand("PROFILE_SET 2 2.5 0.4 0.33 30 180 120").type)); } void setup() { @@ -87,6 +134,7 @@ void setup() { UNITY_BEGIN(); RUN_TEST(test_simple_commands_map_to_espa_codes); RUN_TEST(test_parameterized_commands_fill_payload); + RUN_TEST(test_output_message_protocol_shape); RUN_TEST(test_status_is_local_command); RUN_TEST(test_invalid_commands_are_rejected); UNITY_END(); diff --git a/test/unit_hw/espb_bridge/test_protocol_contract/test_protocol_contract.cpp b/test/unit_hw/espb_bridge/test_protocol_contract/test_protocol_contract.cpp index 4f13737..faa7554 100644 --- a/test/unit_hw/espb_bridge/test_protocol_contract/test_protocol_contract.cpp +++ b/test/unit_hw/espb_bridge/test_protocol_contract/test_protocol_contract.cpp @@ -22,19 +22,19 @@ void tearDown() {} void test_protocol_contract_table_matches_parser() { size_t count = 0; const EspbProtocolCommand* commands = espbProtocolCommands(count); - TEST_ASSERT_EQUAL_UINT8(18, count); + TEST_ASSERT_EQUAL_UINT8(23, count); for (size_t i = 0; i < count; ++i) { char commandLine[64]; switch (commands[i].commandCode) { - case CMD_UPDATE_PID: - snprintf(commandLine, sizeof(commandLine), "%s 1 2 3", commands[i].commandText); + case CMD_PID_CONFIG_SET: + snprintf(commandLine, sizeof(commandLine), "%s 1 2 3 50 0.25 5 0.001 0.011", commands[i].commandText); break; - case CMD_UPDATE_PID_EXT: - snprintf(commandLine, sizeof(commandLine), "%s 50 0.25", commands[i].commandText); + case CMD_BALANCE_CONFIG_SET: + snprintf(commandLine, sizeof(commandLine), "%s 5000 5 3 50", commands[i].commandText); break; - case CMD_SET_SPEED: - snprintf(commandLine, sizeof(commandLine), "%s 300", commands[i].commandText); + case CMD_MOTOR_CONFIG_SET: + snprintf(commandLine, sizeof(commandLine), "%s 1800 1800 1200 300", commands[i].commandText); break; case CMD_TEST_STEPS: snprintf(commandLine, sizeof(commandLine), "%s -100", commands[i].commandText); @@ -51,6 +51,9 @@ void test_protocol_contract_table_matches_parser() { case CMD_SET_SURFACE_OFFSET: snprintf(commandLine, sizeof(commandLine), "%s 0.10", commands[i].commandText); break; + case CMD_PROFILE_SET: + snprintf(commandLine, sizeof(commandLine), "%s 2 2.5 0.4 0.33 30 180 120 0.10", commands[i].commandText); + break; default: snprintf(commandLine, sizeof(commandLine), "%s", commands[i].commandText); break; @@ -60,7 +63,7 @@ void test_protocol_contract_table_matches_parser() { TEST_ASSERT_EQUAL_UINT8_MESSAGE(commandType(EspbParsedCommandType::ForwardToEspA), commandType(parsed.type), commands[i].commandText); - TEST_ASSERT_EQUAL_UINT8_MESSAGE(commands[i].commandCode, + TEST_ASSERT_EQUAL_UINT8_MESSAGE(static_cast(commands[i].commandCode), parsed.message.command, commands[i].commandText); TEST_ASSERT_NOT_NULL_MESSAGE(commands[i].expectedAck, commands[i].commandText); @@ -76,17 +79,27 @@ void test_ack_constants_match_gui_contract() { TEST_ASSERT_EQUAL_STRING("CMD4_RECVD", CMD4_ACK); TEST_ASSERT_EQUAL_STRING("SWITCH_AM_RECVD", CMD5_ACK); TEST_ASSERT_EQUAL_STRING("TRY_UPLOAD_RECVD", CMD7_ACK); - TEST_ASSERT_EQUAL_STRING("CHNG_PARMS_RECVD", CMD8_ACK); - TEST_ASSERT_EQUAL_STRING("TEST_FREQ_RECVD", CMD9_ACK); + TEST_ASSERT_EQUAL_STRING("PID_CONFIG_SET_RECVD", CMD8_ACK); + TEST_ASSERT_EQUAL_STRING("PID_CONFIG_SET_ERR", CMD8_ERR); TEST_ASSERT_EQUAL_STRING("TEST_STEPS_RECVD", CMD10_ACK); TEST_ASSERT_EQUAL_STRING("DEBUG_MODE_RECVD", CMD11_ACK); TEST_ASSERT_EQUAL_STRING("HOME_RECVD", CMD12_ACK); TEST_ASSERT_EQUAL_STRING("STOP_RECVD", CMD13_ACK); - TEST_ASSERT_EQUAL_STRING("CHNG_PID_EXT_RECVD", CMD14_ACK); TEST_ASSERT_EQUAL_STRING("SYRINGE_SET_RECVD", CMD15_ACK); TEST_ASSERT_EQUAL_STRING("PID_HOLD_RECVD", CMD16_ACK); TEST_ASSERT_EQUAL_STRING("PID_STEP_RECVD", CMD17_ACK); TEST_ASSERT_EQUAL_STRING("SURFACE_OFF_RECVD", CMD18_ACK); + TEST_ASSERT_EQUAL_STRING("PROFILE_SET_RECVD", CMD19_ACK); + TEST_ASSERT_EQUAL_STRING("PROFILE_SET_ERR", CMD19_ERR); + TEST_ASSERT_EQUAL_STRING("BALANCE_CONFIG_SET_RECVD", CMD21_ACK); + TEST_ASSERT_EQUAL_STRING("BALANCE_CONFIG_SET_ERR", CMD21_ERR); + TEST_ASSERT_EQUAL_STRING("MOTOR_CONFIG_SET_RECVD", CMD23_ACK); + TEST_ASSERT_EQUAL_STRING("MOTOR_CONFIG_SET_ERR", CMD23_ERR); +} + +void test_reserved_command_is_not_parseable() { + EspbParsedCommand parsed = espbParseSerialCommand("CMD_RESERVED_9"); + TEST_ASSERT_EQUAL_UINT8(commandType(EspbParsedCommandType::Invalid), commandType(parsed.type)); } void test_status_tokens_are_gui_parseable() { @@ -102,6 +115,7 @@ void setup() { UNITY_BEGIN(); RUN_TEST(test_protocol_contract_table_matches_parser); RUN_TEST(test_ack_constants_match_gui_contract); + RUN_TEST(test_reserved_command_is_not_parseable); RUN_TEST(test_status_tokens_are_gui_parseable); UNITY_END(); } diff --git a/test/unit_hw/flash_storage/test_api/test_api.cpp b/test/unit_hw/flash_storage/test_api/test_api.cpp index 82a1025..f72c1b9 100644 --- a/test/unit_hw/flash_storage/test_api/test_api.cpp +++ b/test/unit_hw/flash_storage/test_api/test_api.cpp @@ -20,10 +20,10 @@ namespace { constexpr char EXPECTED_CSV[] = - "company_number,profile_id,time_s,pressure_kpa,depth_m,phase,sensor_depth_m\n" - "EX10,1,0.00,101.30,0.00,start,-0.51\n" - "EX10,1,5.00,126.00,2.50,hold_2_5m,1.99\n" - "EX10,1,10.00,126.20,2.52,hold_2_5m,2.01\n"; + "company_number,profile_id,time_s,pressure_kpa,depth_m,phase,sensor_depth_m,syringe_u\n" + "EX10,1,0.00,101.30,0.00,start,-0.51,0.0000\n" + "EX10,1,5.00,126.00,2.50,hold_2_5m,1.99,0.5000\n" + "EX10,1,10.00,126.20,2.52,hold_2_5m,2.01,1.0000\n"; } void setUp() {} @@ -45,9 +45,9 @@ void test_flash_csv_logging() { const size_t headerSize = flashStorage.logSize(); TEST_ASSERT_GREATER_THAN_UINT32_MESSAGE(0, headerSize, "CSV header was not written"); - TEST_ASSERT_TRUE(flashStorage.appendRecord(COMPANY_NUMBER, 1, 0.0f, 101.3f, 0.00f, "start", -0.51f)); - TEST_ASSERT_TRUE(flashStorage.appendRecord(COMPANY_NUMBER, 1, 5.0f, 126.0f, 2.50f, "hold_2_5m", 1.99f)); - TEST_ASSERT_TRUE(flashStorage.appendRecord(COMPANY_NUMBER, 1, 10.0f, 126.2f, 2.52f, "hold_2_5m", 2.01f)); + TEST_ASSERT_TRUE(flashStorage.appendRecord(COMPANY_NUMBER, 1, 0.0f, 101.3f, 0.00f, "start", -0.51f, 0.0f)); + TEST_ASSERT_TRUE(flashStorage.appendRecord(COMPANY_NUMBER, 1, 5.0f, 126.0f, 2.50f, "hold_2_5m", 1.99f, 0.5f)); + TEST_ASSERT_TRUE(flashStorage.appendRecord(COMPANY_NUMBER, 1, 10.0f, 126.2f, 2.52f, "hold_2_5m", 2.01f, 1.0f)); TEST_ASSERT_GREATER_THAN_UINT32_MESSAGE(headerSize, flashStorage.logSize(), "CSV log did not grow"); TEST_ASSERT_EQUAL_UINT32_MESSAGE(strlen(EXPECTED_CSV), flashStorage.logSize(), "CSV log size mismatch"); diff --git a/tools/pid_tuning/README.md b/tools/pid_tuning/README.md new file mode 100644 index 0000000..9865861 --- /dev/null +++ b/tools/pid_tuning/README.md @@ -0,0 +1,137 @@ +# Tuning PID del Float — guida + tool + +Strumento **plug-and-play** per chi deve tarare il controllo di profondità del Float **senza essere esperto di controlli automatici**. + +Due cose: +1. Un **notebook** (gira su Google Colab, niente da installare) che ti **dà i valori da mettere nella GUI** e **analizza i log** dei tuoi test dicendoti cosa correggere. +2. Questa guida, che spiega **ogni parametro** e **cosa succede se lo cambi**. + +--- + +## 🚀 Apri il tool su Google Colab + +[![Open In Colab](https://colab.research.google.com/assets/colab-badge.svg)](https://colab.research.google.com/github/PoliTOcean/Float/blob/master/tools/pid_tuning/pid_tuning.ipynb) + +1. Clicca il badge qui sopra (o apri il link). +2. In alto: **Runtime → Esegui tutto** (`Runtime → Run all`). +3. Nella sezione 4 dai i tuoi dati in **uno** di questi modi: + - **Incolla** nella variabile `DATI` la **tabella** copiata dalla vista *"Raw chart"* della GUI (o un JSON), **oppure** + - lascia `DATI` vuoto e **carica un CSV** (export GUI o log flash `DUMP_LOG`). + - Se non metti nulla, usa un log di esempio così vedi subito come funziona. +4. Il notebook **riconosce da solo le fasi** del log: solo discesa, solo salita o profilo completo (discesa → hold → salita), con metriche e diagnosi **per fase**. Imposti due target come nella GUI (`target_discesa_m` riferito al FONDO, `target_salita_m` riferito al TOP, convertito da solo); con `fase = "discesa"` o `"salita"` puoi forzare l'analisi su una sola fase. L'eventuale riemersione finale in superficie viene esclusa dall'analisi della salita. + +> Uso locale (senza Colab): `pip install -r requirements.txt` e poi `jupyter notebook pid_tuning.ipynb`. + +--- + +## ⚠️ Premessa importante: dove sta davvero il float + +Il **barometro è in cima al float** (lungo ~0,51 m). Il controllo ragiona sul **fondo del float**: + +- **Target di DISCESA** = profondità che deve raggiungere il **FONDO** del float. Quindi quando sei "a target", il barometro in cima legge **target − 0,51 m**. +- **Target di RISALITA** = riferito al **TOP** del float (barometro). + +**Conseguenza pratica:** in una vasca bassa (< ~1 m) il barometro resta a pochi centimetri dal pelo dell'acqua → letture rumorose, il sensore può uscire dall'acqua → **il float oscilla e questo NON è colpa del PID**. Per tarare sul serio servono **almeno ~1,5–2 m** di acqua. (Es.: target discesa 0,55 in vasca da 0,8 m mette il sensore a soli ~4 cm dal pelo: condizione impossibile da stabilizzare.) + +--- + +## 📖 I tre concetti minimi + +- **`u` (apertura siringa)** = quanto la siringa è "piena", da **0** a **1**. `u = 0` → siringa vuota → **galleggia**; `u = 1` → siringa piena → **affonda**. È l'**uscita** che il controllo calcola da solo; nei log è una colonna. **Non si imposta a mano.** +- **PID** = il "cervello" che decide `u` in base all'errore di profondità (quanto sei lontano dal target). Ha tre manopole: **P** (reazione all'errore ora), **I** (recupero dell'errore che persiste), **D** (freno/anticipo sulle variazioni). +- **`u_neutral`** = l'apertura siringa di **assetto neutro** (quella a cui il float né sale né scende). È un **parametro** da impostare; il notebook lo **stima dai dati**. Diverso da `u`! + +--- + +## 🎛️ Parametri PID (campo per campo) + +Valori di **default** e **range validi** presi dal firmware (`include/config.h`, `lib/runtime_config/src/runtime_config.cpp`). + +| Parametro | Cosa fa | Default | Range valido | Se lo **aumenti** | Se lo **diminuisci** | +|---|---|---|---|---|---| +| **kp** | Forza della reazione all'errore di profondità | `1.7` | > 0 | Reagisce più pronto, ma **oscilla / overshoot** se troppo | Più lento e dolce, ma può restare un **offset** | +| **ki** | Recupera l'errore che **persiste** nel tempo (offset) | `0.1` | ≥ 0 | Elimina l'offset; troppo → **oscillazione lenta / overshoot** | Resta un errore stazionario (float un po' troppo alto/basso) | +| **kd** | **Smorza**: frena in base alla velocità di avvicinamento | `0.3` | ≥ 0 | Riduce **oscillazioni e overshoot**; troppo → amplifica il **rumore** | Più overshoot e oscillazione | +| **period_ms** | Ogni quanto ricalcola il comando | `50` | `[20, 500]` | Reagisce **meno spesso** (più lento) | Più reattivo, ma più rumore/carico | +| **alpha_d** | Filtro sulla derivata (0 = liscio, 1 = grezzo) | `0.25` | `[0.05, 1.0]` | Derivata più "viva" ma più **rumorosa** | Derivata più liscia ma più in **ritardo** | +| **integral_limit** | Tetto anti-windup dell'integrale | `5.0` | > 0 | L'integrale può accumulare di più (recupero forte, rischio overshoot) | Limita il recupero dell'offset | +| **min_retarget_frac** | Zona morta: di quanto deve cambiare il comando prima di muovere il motore | `0.001` | ≥ 0 | Meno micro-movimenti (motore più fermo), meno preciso | Insegue ogni minima variazione: più preciso, più usura | +| **u_neutral** | Apertura siringa di **base** (assetto neutro / spinta iniziale) | `0.011` | ≥ 0 (utile ≤ 0.92) | Parte più "**affondante**" | Parte più "**galleggiante**" | + +> Nota: l'uscita `u` è sempre limitata dal firmware a **[0 ; 0,92]** (margine di sicurezza dai finecorsa/TOF). Se nei log vedi `u` incollata a 0 o 0,92, il problema è di **assetto/zavorra**, non dei guadagni. + +--- + +## 🌊 Parametri del profilo (PROFILE_SET) + +| Parametro | Cosa fa | Default | Range valido | Note | +|---|---|---|---|---| +| **descent_target** | Profondità del **FONDO** del float in discesa | `2.5` m | `[0, 5]` | Vedi premessa geometria (+0,51 m) | +| **ascent_target** | Profondità del **TOP** del float in risalita | `0.40` m | `[0, 5]` | Riferito al barometro | +| **depth_tolerance** | Semi-banda ± attorno al target per dirsi "a target" | `0.33` m | `[0.005, 1.0]` | In vasca **abbassala** (es. `0.05`) o "a target" non significa niente | +| **hold_time** | Quanto resta fermo al target | `30` s | `[1, 600]` | Il check scatta ogni 5 s | +| **descent_timeout** | Tempo massimo della fase di discesa | `180` s | `[5, 900]` | Hold incluso | +| **ascent_timeout** | Tempo massimo della fase di risalita | `120` s | `[5, 900]` | Hold incluso | +| **surface_offset** | Quanto il top del float resta sotto il pelo a riposo | `0.10` m | `[0, 5]` | — | + +> **Vincolo del firmware:** `ascent_target + 0,51 < descent_target` (la risalita deve restare più in alto della discesa), altrimenti la GUI rifiuta la configurazione. + +--- + +## 🛠️ Ricetta di tuning (passo per passo) + +1. **Prima l'assetto (`u_neutral`).** Trova l'apertura siringa a cui il float **né sale né scende** alla quota di prova (osserva dove la colonna `u` si stabilizza in un mantenimento, oppure fai uno sweep manuale). Imposta `u_neutral` ≈ quel valore. **Senza un buon assetto nessun guadagno funziona.** +2. **Parti dai default** della tabella PID. +3. **Regola una manopola alla volta**, guardando il comportamento: + - **Oscilla** con ampiezza che non si spegne → **riduci kp** (×0,7) oppure **aumenta kd** (×1,5). + - **Lento** e resta un **offset** (galleggia troppo alto/basso) → **aumenta ki** (×1,5–2). + - **Overshoot** grande e poi si assesta → **aumenta kd**, eventualmente riduci un po' kp. + - `u` sempre a **0 o 0,92** → **non è il PID**: è l'assetto/zavorra o `u_neutral`. +4. **Fai il test**, poi **esporta il log** dalla GUI. +5. **Carica il log nel notebook**: leggi la diagnosi, applica i **valori suggeriti**, e ripeti dal punto 3. + +--- + +## 📤 Quali dati dare al notebook + +Hai due sorgenti possibili. + +### A) Incollare la tabella dalla GUI — modo più semplice +Nel pannello *Profile Data Log* attiva lo switch **"Raw chart"**: compare una **tabella**. Selezionala, copiala e incollala nel notebook (variabile `DATI`). Le righe sono tipo: + +``` +Time (s) Depth Pressure Syringe +12.40 0.51 m 101.30 kPa 0.30 u +``` + +Il notebook è robusto: capisce **2, 3 o 4 colonne**, con o senza unità (`m`, `kPa`), timestamp in secondi o **millisecondi**, separatori spazi/virgole/tab, decimali con la virgola, righe `N/A`, e `u` anche in percentuale (`30` → `0.30`). Accetta pure il `raw` in formato **JSON**. + +> La tabella "Raw chart" include la colonna **Syringe (`u`)**: incollandola ottieni il tuning completo (`kp/ki/kd`, `u_neutral` e controllo saturazione). Se per qualche motivo la colonna `u` manca, il notebook analizza comunque la profondità e ti avvisa di usare il log flash per il resto. + +### B) Caricare il log flash del Float (CSV, consigliato per il tuning completo) +Il Float salva su memoria flash, **dopo ogni profilo**, un CSV a **8 colonne** (`lib/flash_storage/`): + +``` +company_number, profile_id, time_s, pressure_kpa, depth_m, phase, sensor_depth_m, syringe_u +``` + +- **`depth_m`** = profondità del **FONDO** del float — quella su cui ragiona il PID, confrontata col target. +- **`sensor_depth_m`** = profondità del **barometro** (cima del float). +- **`phase`** = stato (`descending`, `hold_2_5m`, `ascending`, `emergency_stop:...`): se compare un emergency stop, il notebook **te lo segnala**. +- **`syringe_u`** = apertura siringa `u` → **serve per `u_neutral` e per la saturazione**. + +Il notebook riconosce **per nome di colonna** sia il JSON `raw` della GUI, sia il CSV a 8 colonne, sia un export ridotto (`timestamp, profondità, pressione, syringe/u`), con separatore `,` o `;`. + +> La tabella della GUI (modo A) basta per il tuning completo, colonna `Syringe` inclusa. Il log flash (modo B) resta utile come sorgente alternativa: ha anche `phase` (segnala gli emergency stop) e `sensor_depth_m`. + +--- + +## 🔒 Sicurezza e limiti + +- L'uscita `u` è limitata a **0,92** e c'è una **guardia TOF** che ferma il pistone ai finecorsa: non forzare oltre. +- **Vasca troppo bassa = test inaffidabile** (vedi premessa geometria). +- I valori che il tool propone restano **dentro i range accettati dal firmware**; se inserisci a mano valori fuori range, la GUI/firmware li rifiuta. + +--- + +*Manutentori: Team PoliTOcean. I default e i limiti sono allineati al firmware in `include/config.h`, `lib/runtime_config/`, `lib/profile/`.* diff --git a/tools/pid_tuning/esempio_gui_dump.txt b/tools/pid_tuning/esempio_gui_dump.txt new file mode 100644 index 0000000..45c5289 --- /dev/null +++ b/tools/pid_tuning/esempio_gui_dump.txt @@ -0,0 +1,18 @@ +0 0.53 m 98.47 kPa 0.42 u +1 0.54 m 98.61 kPa 0.81 u +2 0.62 m 99.34 kPa 0.92 u +3 0.78 m 100.97 kPa 0.92 u +4 1.03 m 103.39 kPa 0.89 u +5 1.33 m 106.32 kPa 0.50 u +6 1.43 m 107.26 kPa 0.26 u +7 1.29 m 105.97 kPa 0.45 u +8 1.20 m 105.03 kPa 0.60 u +9 1.20 m 105.04 kPa 0.59 u +10 1.21 m 105.14 kPa 0.19 u +11 0.36 m 101.84 kPa 0.13 u +12 0.55 m 98.66 kPa 0.53 u +13 0.52 m 98.40 kPa 0.85 u +14 0.52 m 98.40 kPa 0.89 u +15 0.52 m 98.40 kPa 0.88 u +16 0.52 m 98.40 kPa 0.89 u +17 0.52 m 98.40 kPa 0.89 u diff --git a/tools/pid_tuning/pid_tuning.ipynb b/tools/pid_tuning/pid_tuning.ipynb new file mode 100644 index 0000000..b5c4ddc --- /dev/null +++ b/tools/pid_tuning/pid_tuning.ipynb @@ -0,0 +1,139 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": "# 🛟 Tuning PID del Float — tool plug-and-play\n\nQuesto notebook ti aiuta a **tarare il controllo di profondità** del Float anche se non sei esperto di controlli automatici.\n\n**Cosa fa:**\n1. Ti dà i **valori da inserire nei campi della GUI NEXUS** (`kp, ki, kd, ...`) e una stima di **`u_neutral`**.\n2. **Analizza i dati** di un tuo test e ti dice **cosa correggere**, **riconoscendo da solo le fasi** del profilo: solo discesa, solo salita o profilo completo (discesa → hold → salita), con metriche e diagnosi **per fase**. Puoi:\n - **incollare** la tabella copiata dalla vista \"Raw chart\" della GUI (o un JSON), oppure\n - **caricare un CSV/TXT** (export GUI o log flash `DUMP_LOG` del Float).\n\n**Come si usa:** in alto **Runtime → Esegui tutto**. Poi nella sezione 4 incolli i dati (o carichi il CSV); se non metti nulla, viene usato un log di esempio.\n\n> ⚠️ Il **target di discesa è riferito al FONDO del float** (il barometro in cima legge `target − 0,51 m`); il **target di salita è riferito al TOP** (come il campo `ascent_target` della GUI) e il tool lo converte da solo (+0,51 m), esattamente come fa il firmware. In vasca bassa (< ~1 m) il sensore è a pochi cm dal pelo → oscillazioni **non** colpa del PID.\n>\n> ℹ️ Incolla i dati dalla vista **\"Raw chart\"** della GUI **così come sono**: righe tipo `0 → 0.53 m → 98.47 kPa → 0.42 u` (tab, unità e virgole decimali sono gestiti, con o senza intestazione; vedi `esempio_gui_dump.txt`). Il tool capisce **2, 3 o 4 colonne**. Se c'è anche la **siringa (`u`)** fa il tuning completo (`u_neutral`, saturazione); se la tabella ha solo profondità/pressione, analizza la profondità e per `u` usa il **log flash** `DUMP_LOG`.\n>\n> ℹ️ Se c'è la colonna **pressione**, la profondità viene **ricostruita dal dato grezzo** (riferimento FONDO per tutto il log): il log di missione del firmware cambia riferimento fondo/top al cambio fase e introduce salti fittizi di ~0,5 m che falserebbero metriche e stima di `u_neutral`. La colonna originale resta in `depth_log`.\n>\n> ℹ️ Le **fasi** vengono riconosciute dalla colonna `phase` del log flash (`descending`/`hold_2_5m` → discesa, `ascending`/`hold_40cm` → salita) oppure, se manca (dump GUI), dalla **forma della traiettoria** di profondità. Se vuoi forzare l'analisi su una sola fase, imposta `fase = \"discesa\"` o `\"salita\"` nella sezione 4." + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": "# @title 1) Setup — esegui questa cella per prima\nimport sys, subprocess, io, os, json\n\ndef _ensure(pkgs):\n for p in pkgs:\n try:\n __import__(p)\n except ImportError:\n subprocess.run([sys.executable, \"-m\", \"pip\", \"install\", \"-q\", p], check=False)\n\n_ensure([\"numpy\", \"pandas\", \"matplotlib\"])\n\nimport numpy as np\nimport pandas as pd\nimport matplotlib.pyplot as plt\n\ntry:\n import google.colab # noqa: F401\n IN_COLAB = True\nexcept ImportError:\n IN_COLAB = False\n\n# ---- Default e limiti presi DAL FIRMWARE (fonte di verità: include/config.h,\n# lib/runtime_config/, lib/profile/). Non inventati. ----\nFW = {\n \"pid_default\": dict(kp=1.7, ki=0.1, kd=0.3, period_ms=50,\n alpha_d=0.25, integral_limit=5.0,\n min_retarget_frac=0.001, u_neutral=0.011),\n \"pid_range\": dict(period_ms=(20, 500), alpha_d=(0.05, 1.0)),\n \"u_min\": 0.0, \"u_max\": 0.92,\n \"float_length_m\": 0.51,\n \"sensor_to_top_m\": 0.0, # FLOAT_TOP_TO_SENSOR_M: sensore in cima al float\n \"profile_default\": dict(descent_target=2.5, ascent_target=0.40,\n depth_tolerance=0.33, hold_time=30,\n descent_timeout=180, ascent_timeout=120,\n surface_offset=0.10),\n}\nRAW_EXAMPLE_URL = (\"https://raw.githubusercontent.com/PoliTOcean/Float/\"\n \"master/tools/pid_tuning/esempio_gui_dump.txt\")\n\nprint(\"Ambiente:\", \"Google Colab\" if IN_COLAB else \"locale\", \"| setup OK\")" + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## 2) I tre concetti minimi\n", + "\n", + "- **`u` (apertura siringa)**: da **0** (vuota → *galleggia*) a **1** (piena → *affonda*). È l'**uscita** che il controllo calcola da solo; nei log è una colonna. **Non si imposta a mano.**\n", + "- **PID**: decide `u` dall'errore di profondità. Tre manopole — **P** (reazione ora), **I** (recupero dell'errore che resta), **D** (freno/anticipo).\n", + "- **`u_neutral`**: apertura siringa di **assetto neutro** (il float né sale né scende). È un **parametro** da impostare; qui lo **stimiamo dai dati** (se è presente `u`). Diverso da `u`!\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": "# @title 2) Funzioni (caricamento dati, segmentazione fasi, analisi, stima u_neutral)\n\ndef mostra_valori(d, titolo=\"\"):\n if titolo:\n print(titolo)\n print(\"-\" * len(titolo))\n for k, v in d.items():\n print(f\" {k:18s} = {v}\")\n\ndef _leggi_csv(src):\n if isinstance(src, str):\n if src.startswith((\"http://\", \"https://\")):\n from urllib.request import urlopen\n content = urlopen(src, timeout=15).read().decode(\"utf-8\")\n else:\n with open(src, 'r', encoding='utf-8') as f:\n content = f.read()\n else:\n content = src.read().decode('utf-8')\n try:\n # Cerchiamo di leggere con pandas sniffer\n df = pd.read_csv(io.StringIO(content), sep=None, engine=\"python\")\n if df.shape[1] < 2:\n raise ValueError(\"Troppe poche colonne, probabile separatore non standard\")\n # Dump GUI salvato su file: celle con unita' (\"0.53 m\", \"98.47 kPa\") e\n # prima riga dati scambiata per header -> servono almeno 2 colonne\n # numeriche, altrimenti si riparsa il testo grezzo riga per riga.\n numeriche = sum(pd.to_numeric(df[c], errors=\"coerce\").notna().mean() > 0.5\n for c in df.columns)\n if numeriche < 2:\n raise ValueError(\"Celle non numeriche (unita' m/kPa nel testo?)\")\n return df\n except Exception:\n # Fallback ultra-robusto (unita' nelle celle, tabelle salvate con spazi)\n return _da_testo(content)\n\ndef _ricostruisci_depth(df):\n \"\"\"Il log di missione del firmware cambia riferimento di profondita' al\n cambio fase (FONDO in discesa/risalita, TOP durante hold_40cm): la colonna\n depth puo' avere salti fittizi di ~0,5 m che falsano metriche e stime.\n Se c'e' la pressione (kPa), ricostruiamo la profondita' dal dato grezzo,\n riferita al FONDO per tutto il log. L'originale resta in 'depth_log'.\"\"\"\n if \"pressure\" not in df.columns or \"depth\" not in df.columns:\n return df\n p = pd.to_numeric(df[\"pressure\"], errors=\"coerce\")\n if p.isna().all():\n return df\n n_atm = max(3, len(p) // 10)\n p_atm = p.nsmallest(n_atm).median()\n depth_p = (p - p_atm) / 9.79 + FW[\"float_length_m\"] # kPa -> m, rif. FONDO\n scarto = (df[\"depth\"] - depth_p).abs()\n # Sostituiamo solo con la firma del cambio riferimento: log per lo piu'\n # coerente con la pressione (mediana piccola) ma con salti localizzati.\n if scarto.median() < 0.15 and scarto.max() > 0.3:\n df[\"depth_log\"] = df[\"depth\"]\n df[\"depth\"] = depth_p\n print(\"Nota: la profondita' del log cambiava riferimento (fondo/top) al \"\n \"cambio fase -> ricostruita dalla pressione, riferimento FONDO \"\n f\"(p_atm stimata {p_atm:.2f} kPa). Originale nella colonna 'depth_log'.\")\n return df\n\ndef _normalizza(df):\n cols = list(df.columns)\n numeric_header = all(str(c).replace(\".\", \"\", 1).replace(\"-\", \"\", 1).isdigit()\n for c in cols)\n if numeric_header:\n df = df.copy()\n df.columns = list(range(df.shape[1]))\n cols = list(df.columns)\n\n ren, used = {}, set()\n for c in cols:\n cl = str(c).strip().lower()\n target = None\n if cl in (\"t\", \"time\", \"timestamp\", \"times\", \"time_s\", \"tempo\", \"t_ms\", \"millis\", \"ms\") or \"time (s)\" in cl:\n target = \"t\"\n elif \"sensor\" in cl and \"depth\" in cl:\n target = \"sensor_depth\"\n elif (\"depth\" in cl or cl.startswith(\"profond\")) and \"sensor\" not in cl:\n target = \"depth\" # nota: NON usare \"prof\" generico, matcha \"profile_id\"\n elif cl in (\"u\", \"syringe_u\", \"apertura\", \"apertura_siringa_u\", \"u_norm\", \"syringe\"):\n target = \"u\"\n elif \"press\" in cl:\n target = \"pressure\"\n elif \"phase\" in cl or cl == \"fase\":\n target = \"phase\"\n if target and target not in used:\n ren[c] = target\n used.add(target)\n df = df.rename(columns=ren)\n\n # ripiego posizionale solo se non ho riconosciuto le colonne per nome\n if \"depth\" not in df.columns and (\"u\" not in df.columns and \"pressure\" not in df.columns):\n n = df.shape[1]\n if n == 4: # GUI: timestamp, depth, pressure, u\n df.columns = [\"t\", \"depth\", \"pressure\", \"u\"]\n elif n >= 8: # DUMP_LOG firmware (8 colonne)\n df = df.rename(columns={df.columns[2]: \"t\", df.columns[4]: \"depth\",\n df.columns[7]: \"u\"})\n\n if \"depth\" not in df.columns:\n raise ValueError(\"Non trovo la colonna profondita. Servono almeno tempo e profondita (depth_m).\")\n\n for c in (\"t\", \"depth\", \"u\", \"pressure\", \"sensor_depth\"):\n if c in df.columns:\n df[c] = pd.to_numeric(df[c], errors=\"coerce\")\n df = df.dropna(subset=[\"depth\"]).reset_index(drop=True)\n if \"u\" in df.columns and df[\"u\"].abs().median() > 1.5:\n print(\"Nota: 'u' sembra in percentuale (0..100) -> converto in 0..1.\")\n df[\"u\"] = df[\"u\"] / 100.0\n\n if \"t\" not in df.columns or df[\"t\"].isna().all():\n df[\"t\"] = np.arange(len(df)) * 0.2\n df[\"t\"] = df[\"t\"] - df[\"t\"].iloc[0]\n if df[\"t\"].max() > 3600: # quasi certamente in millisecondi\n df[\"t\"] = df[\"t\"] / 1000.0\n return _ricostruisci_depth(df)\n\ndef _da_json(testo):\n \"\"\"Costruisce il DataFrame dal JSON 'raw' della GUI (array paralleli) o da una\n lista di punti [{timestamp, depth, pressure}, ...].\"\"\"\n obj = json.loads(testo)\n raw = obj.get(\"raw\", obj) if isinstance(obj, dict) else obj\n if isinstance(raw, list):\n df = pd.DataFrame(raw)\n else:\n def pick(*names):\n for n in names:\n v = raw.get(n)\n if isinstance(v, list) and len(v) > 0:\n return v\n return None\n campi = {\n \"t\": pick(\"times\", \"time_s\", \"timestamp\", \"t\"),\n \"depth\": pick(\"depth_m\", \"depth\", \"profondita_m\", \"profondita\"),\n \"pressure\": pick(\"pressure_kpa\", \"pressure\", \"pressione_kpa\", \"pressione\"),\n \"sensor_depth\": pick(\"sensor_depth_m\"),\n \"phase\": pick(\"phase\"),\n \"u\": pick(\"syringe_u\", \"syringe\", \"u\", \"u_norm\", \"apertura\"),\n }\n campi = {k: v for k, v in campi.items() if v is not None}\n if \"depth\" not in campi:\n raise ValueError(\"JSON senza profondita (depth_m/depth).\")\n n = max(len(v) for v in campi.values())\n campi = {k: v for k, v in campi.items() if len(v) == n}\n df = pd.DataFrame(campi)\n return _normalizza(df)\n\ndef _ordine_header(line):\n \"\"\"Deduce l'ordine delle colonne dai nomi nell'intestazione della tabella.\"\"\"\n hl = line.lower()\n coppie = [(\"timestamp\", \"t\"), (\"time\", \"t\"), (\"tempo\", \"t\"),\n (\"depth\", \"depth\"), (\"profond\", \"depth\"),\n (\"pressure\", \"pressure\"), (\"pressione\", \"pressure\"), (\"press\", \"pressure\"),\n (\"syringe\", \"u\"), (\"siringa\", \"u\")]\n trovati = []\n for kw, canon in coppie:\n idx = hl.find(kw)\n if idx >= 0:\n trovati.append((idx, canon))\n trovati.sort()\n ordine, visti = [], set()\n for _, canon in trovati:\n if canon not in visti:\n visti.add(canon)\n ordine.append(canon)\n return ordine\n\ndef _da_testo(testo):\n \"\"\"Parsa il TESTO della tabella copiato dalla GUI (vista 'Raw chart'): righe di\n numeri separati da spazi/virgole/tab. Robusto a unita' (m, kPa, u), header,\n 'N/A', timestamp in ms, decimali con la virgola (righe tab-separated) e 2-4\n colonne. Accetta il dump GUI cosi' com'e': `0\\t0.53 m\\t98.47 kPa\\t0.42 u`.\n Se c'e' una riga d'intestazione, l'ordine delle colonne viene dedotto dai nomi\n (Timestamp/Time (s)/Depth/Pressure/Syringe).\"\"\"\n import re\n from collections import Counter\n num_re = re.compile(r\"[-+]?\\d*\\.?\\d+(?:[eE][-+]?\\d+)?\")\n righe = [r for r in testo.strip().splitlines() if r.strip()]\n if not righe:\n raise ValueError(\"Niente da leggere: incolla la tabella copiata dalla GUI.\")\n ordine_header = None\n kw = (\"time\", \"depth\", \"prof\", \"press\", \"syringe\", \"siringa\")\n if any(k in righe[0].lower() for k in kw) and len(num_re.findall(righe[0])) < 2:\n ordine_header = _ordine_header(righe[0])\n righe = righe[1:]\n rows = []\n for r in righe:\n if \"\\t\" in r:\n # riga tab-separated (copia da tabella): una virgola fra cifre\n # e' un decimale (locale it), non un separatore di colonna\n r = re.sub(r\"(?<=\\d),(?=\\d)\", \".\", r)\n nums = num_re.findall(r)\n if nums:\n rows.append([float(x) for x in nums])\n if not rows:\n raise ValueError(\"Nessun numero trovato: incolla la tabella (una riga per campione).\")\n ncol = Counter(len(r) for r in rows).most_common(1)[0][0]\n if ncol < 2:\n raise ValueError(\"Servono almeno 2 colonne: tempo e profondita.\")\n arr = np.array([r for r in rows if len(r) == ncol], dtype=float)\n if ordine_header and len(ordine_header) == ncol:\n nomi = ordine_header\n else:\n nomi = [\"t\", \"depth\", \"pressure\", \"u\"][:min(ncol, 4)]\n if ncol > 4:\n arr = arr[:, :4]\n nomi = [\"t\", \"depth\", \"pressure\", \"u\"]\n df = pd.DataFrame(arr[:, :len(nomi)], columns=nomi)\n return _normalizza(df)\n\ndef carica(dati=\"\"):\n \"\"\"Sorgente dati: testo incollato (tabella o JSON) > upload CSV (Colab) > esempio.\"\"\"\n if dati and dati.strip():\n s = dati.strip()\n try:\n df = _da_json(s) if s[0] in \"[{\" else _da_testo(s)\n cols = [c for c in (\"t\", \"depth\", \"u\", \"pressure\", \"phase\") if c in df.columns]\n print(f\"Dati incollati: {len(df)} campioni. Colonne: {cols}\")\n if \"u\" not in df.columns:\n print(\"Nota: niente 'u' (apertura siringa) -> analisi solo di profondita. \"\n \"Per u_neutral/saturazione usa il log flash DUMP_LOG.\")\n return df\n except Exception as e:\n print(\"Non riesco a leggere i dati incollati:\", e)\n print(\"Uso un CSV/esempio come ripiego.\")\n return carica_log()\n\ndef carica_log():\n if IN_COLAB:\n from google.colab import files\n up = files.upload()\n if up:\n nome = list(up.keys())[0]\n print(\"Caricato:\", nome)\n return _normalizza(_leggi_csv(io.BytesIO(up[nome])))\n print(\"Nessun file caricato -> uso l'esempio.\")\n for src in (\"esempio_gui_dump.txt\", \"tools/pid_tuning/esempio_gui_dump.txt\", RAW_EXAMPLE_URL):\n try:\n df = _normalizza(_leggi_csv(src))\n print(\"Uso log di esempio:\", src)\n return df\n except Exception:\n continue\n raise RuntimeError(\"Nessun dato disponibile (incolla i dati o carica un CSV).\")\n\ndef _valida_pid(d):\n d = dict(d)\n lo, hi = FW[\"pid_range\"][\"period_ms\"]\n d[\"period_ms\"] = int(min(max(d[\"period_ms\"], lo), hi))\n lo, hi = FW[\"pid_range\"][\"alpha_d\"]\n d[\"alpha_d\"] = round(min(max(d[\"alpha_d\"], lo), hi), 3)\n d[\"u_neutral\"] = max(0.0, d[\"u_neutral\"])\n for k in (\"kp\", \"ki\", \"kd\"):\n d[k] = max(0.0, d[k])\n return d\n\ndef _stringa_pid(d):\n return (\"PID_CONFIG_SET {kp} {ki} {kd} {period_ms} {alpha_d} \"\n \"{integral_limit} {min_retarget_frac} {u_neutral}\").format(**d)\n\n# ---------- Segmentazione fasi (discesa / salita) ----------\n\ndef _target_salita_fondo(target_salita_top):\n \"\"\"Il target di salita della GUI (ascent_target) e' riferito al TOP del float;\n il PID e il log lavorano in riferimento FONDO. Stessa conversione del\n firmware (ProfileManager::ascentTargetBottomM).\"\"\"\n return target_salita_top + FW[\"float_length_m\"] + FW[\"sensor_to_top_m\"]\n\ndef _segmenta_da_phase(df):\n \"\"\"Segmenta usando i nomi di fase del firmware: descending/hold_2_5m ->\n discesa, ascending/hold_40cm -> salita. I tag di servizio (phase_start,\n exit_*, deployed) non appartengono a nessuna delle due.\"\"\"\n ph = df[\"phase\"].astype(str).str.lower()\n is_dis = (ph.str.contains(\"descend\") | ph.str.contains(\"hold_2\")).values\n is_sal = (ph.str.contains(\"ascend\") | ph.str.contains(\"hold_40\")).values\n idx_dis, idx_sal = np.flatnonzero(is_dis), np.flatnonzero(is_sal)\n segs = {}\n if len(idx_dis) >= 5:\n segs[\"discesa\"] = (int(idx_dis[0]), int(idx_dis[-1]))\n if len(idx_sal) >= 5:\n i0 = int(idx_sal[0])\n if \"discesa\" in segs:\n i0 = max(i0, segs[\"discesa\"][1])\n if i0 < int(idx_sal[-1]):\n segs[\"salita\"] = (i0, int(idx_sal[-1]))\n return segs\n\ndef _segmenta_da_profondita(df, tgt_dis, tgt_sal_b, tol):\n \"\"\"Euristica senza colonna 'phase': individua il plateau piu' profondo; cio'\n che lo precede e' la discesa, se dopo si risale in modo netto e' la salita.\"\"\"\n d = df[\"depth\"].rolling(5, center=True, min_periods=1).median().values\n n = len(d)\n dmax = float(np.percentile(d, 98))\n escursione = dmax - float(np.percentile(d, 2))\n if escursione < 0.3:\n # log ~piatto: una sola quota tenuta -> e' la fase col target piu' vicino\n med = float(np.median(d))\n quale = \"discesa\" if abs(med - tgt_dis) <= abs(med - tgt_sal_b) else \"salita\"\n print(f\"Log senza transizioni evidenti (quota ~{med:.2f} m) -> \"\n f\"lo analizzo come sola {quale}.\")\n return {quale: (0, n - 1)}\n # Fine del plateau profondo: ultimo campione entro ~2*tolleranza dal massimo.\n # Banda stretta apposta: se entrasse la rampa di risalita, gonfierebbe\n # l'oscillazione misurata sulla coda della discesa (falsa diagnosi).\n soglia_plateau = dmax - max(0.15, 2 * tol)\n deep = np.flatnonzero(d >= soglia_plateau)\n i_fine_plateau = int(deep[-1])\n segs = {}\n if (dmax - d[0]) > 0.3 * escursione:\n segs[\"discesa\"] = (0, i_fine_plateau)\n if i_fine_plateau < n - 5 and \\\n (dmax - float(np.min(d[i_fine_plateau:]))) > 0.3 * escursione:\n segs[\"salita\"] = (i_fine_plateau, n - 1)\n return segs\n\ndef segmenta_fasi(df, tgt_dis, tgt_sal_b, tol, fase=\"auto\"):\n \"\"\"Capisce da solo quali fasi contiene il log: solo discesa, solo salita o\n profilo completo. Usa la colonna 'phase' del firmware se presente,\n altrimenti la forma della traiettoria. Con fase=\"discesa\"/\"salita\" forza\n TUTTO il log come quella sola fase.\"\"\"\n if fase in (\"discesa\", \"salita\"):\n return {fase: (0, len(df) - 1)}\n segs = {}\n if \"phase\" in df.columns:\n segs = _segmenta_da_phase(df)\n if not segs:\n segs = _segmenta_da_profondita(df, tgt_dis, tgt_sal_b, tol)\n if not segs:\n print(\"Nota: non riconosco fasi distinte -> analizzo tutto il log come discesa.\")\n segs = {\"discesa\": (0, len(df) - 1)}\n return segs\n\ndef _taglia_riemersione(d, i0, i1, tgt_sal_b, tol):\n \"\"\"Se dopo l'hold al target di salita il log prosegue con la riemersione in\n superficie, escludiamo quel tratto finale: falserebbe errore a regime e\n oscillazione della fase di salita (non e' piu' il PID a inseguire il target).\"\"\"\n soglia = tgt_sal_b - max(2 * tol, 0.25)\n seg = d[i0:i1 + 1]\n sopra = seg < soglia # depth minore = piu' in alto del target\n idx_dentro = np.flatnonzero(~sopra)\n if len(idx_dentro) == 0:\n return i1 # mai vicino al target: non taglio nulla\n j = int(idx_dentro[-1])\n if (len(seg) - 1 - j) > max(5, 0.1 * len(seg)):\n return i0 + j\n return i1\n\n# ---------- Metriche, grafici, diagnosi ----------\n\ndef _metriche(df, target, tol):\n t = df[\"t\"].values; d = df[\"depth\"].values\n n = len(d); start = float(d[0])\n e_ss = float(np.mean(d[int(n * 0.8):]) - target)\n overshoot = (np.max(d) - target) if target >= start else (target - np.min(d))\n span = abs(target - start) if abs(target - start) > 1e-6 else 1.0\n overshoot_pct = 100.0 * max(0.0, float(overshoot)) / span\n fuori = np.abs(d - target) > tol\n settling = float(t[np.where(fuori)[0][-1]]) if fuori.any() else 0.0\n half = d[int(n * 0.5):]\n osc_pp = float(np.max(half) - np.min(half))\n err = half - np.mean(half)\n zc = np.where(np.diff(np.sign(err)) != 0)[0]\n thalf = t[int(n * 0.5):]\n periodo = float(2 * np.mean(np.diff(thalf[zc]))) if len(zc) >= 2 else float(\"nan\")\n if \"u\" in df.columns:\n u = df[\"u\"].values\n sat = float(np.mean((u <= FW[\"u_min\"] + 1e-3) | (u >= FW[\"u_max\"] - 1e-3)) * 100.0)\n else:\n sat = None\n sensor_depth = float(np.median(d) - FW[\"float_length_m\"])\n return dict(start=start, e_ss=e_ss, overshoot=float(overshoot),\n overshoot_pct=overshoot_pct, settling=settling, osc_pp=osc_pp,\n periodo=periodo, sat=sat, sensor_depth=sensor_depth)\n\ndef _grafici(df, segs, targets, tol):\n colori = {\"discesa\": \"tab:green\", \"salita\": \"tab:blue\"}\n ha_u = \"u\" in df.columns\n fig, ax = plt.subplots(2, 1, figsize=(9, 6), sharex=True)\n ax[0].plot(df[\"t\"], df[\"depth\"], label=\"profondita (fondo float)\")\n for nome, (i0, i1) in segs.items():\n t0, t1 = float(df[\"t\"].iloc[i0]), float(df[\"t\"].iloc[i1])\n tgt, col = targets[nome], colori.get(nome, \"green\")\n ax[0].hlines(tgt, t0, t1, color=col, ls=\"--\",\n label=f\"target {nome} ({tgt:.2f} m)\")\n ax[0].fill_between([t0, t1], tgt - tol, tgt + tol, color=col, alpha=0.12)\n if len(segs) > 1 and i0 > 0:\n ax[0].axvline(t0, color=\"gray\", ls=\":\", alpha=0.6)\n ax[0].set_ylabel(\"profondita [m]\"); ax[0].invert_yaxis()\n ax[0].legend(loc=\"best\"); ax[0].grid(alpha=0.3)\n if ha_u:\n ax[1].plot(df[\"t\"], df[\"u\"], color=\"orange\", label=\"u (apertura siringa)\")\n ax[1].axhline(FW[\"u_max\"], color=\"red\", ls=\":\", label=\"limite 0,92\")\n ax[1].axhline(FW[\"u_min\"], color=\"red\", ls=\":\")\n ax[1].set_ylim(-0.05, 1.0); ax[1].set_ylabel(\"u [0..1]\")\n elif \"pressure\" in df.columns:\n ax[1].plot(df[\"t\"], df[\"pressure\"], color=\"purple\", label=\"pressione [kPa]\")\n ax[1].set_ylabel(\"pressione [kPa]\")\n else:\n ax[1].text(0.5, 0.5, \"(nessun dato u / pressione)\", ha=\"center\", va=\"center\")\n ax[1].set_xlabel(\"tempo [s]\"); ax[1].legend(loc=\"best\"); ax[1].grid(alpha=0.3)\n plt.tight_layout(); plt.show()\n\ndef _stampa_metriche(m, nome):\n print(f\"\\n=== METRICHE - fase {nome.upper()} ===\")\n print(f\" errore a regime ......... {m['e_ss']:+.3f} m\")\n print(f\" overshoot ............... {m['overshoot']:.3f} m ({m['overshoot_pct']:.0f}%)\")\n print(f\" tempo di assestamento ... {m['settling']:.1f} s\")\n osc = f\" oscillazione residua .... {m['osc_pp']:.3f} m picco-picco\"\n if m[\"periodo\"] == m[\"periodo\"]:\n osc += f\", periodo ~{m['periodo']:.1f} s\"\n print(osc)\n if m[\"sat\"] is None:\n print(\" u saturata .............. n/d (manca la colonna u)\")\n else:\n print(f\" u saturata (0 o 0,92) ... {m['sat']:.0f}% del tempo\")\n print(f\" prof. sensore (mediana) . {m['sensor_depth']:.2f} m\")\n\ndef _consigli_fase(m, tol, base):\n \"\"\"Applica le regole di correzione PID alle metriche di UNA fase.\"\"\"\n cons, note = dict(base), []\n if m[\"sat\"] is not None and m[\"sat\"] > 30:\n note.append(\"ATTENZIONE: la siringa resta spesso a fondo corsa (u a 0 o 0,92): e un problema di \"\n \"ASSETTO/ZAVORRA o di u_neutral, non dei guadagni. Sistema prima quello.\")\n grossa_osc = (m[\"osc_pp\"] > 2 * tol) and (m[\"periodo\"] == m[\"periodo\"])\n if grossa_osc:\n cons[\"kp\"] = round(cons[\"kp\"] * 0.7, 3)\n cons[\"kd\"] = round(cons[\"kd\"] * 1.5, 3)\n note.append(\"Oscillazione ampia e regolare -> riduci kp e aumenta kd.\")\n if m[\"overshoot_pct\"] > 20 and not grossa_osc:\n cons[\"kd\"] = round(cons[\"kd\"] * 1.5, 3)\n if m[\"overshoot_pct\"] > 50:\n cons[\"kp\"] = round(cons[\"kp\"] * 0.8, 3)\n note.append(\"Overshoot marcato -> aumenta kd (ed eventualmente abbassa kp).\")\n sat_ok = (m[\"sat\"] is None) or (m[\"sat\"] < 30)\n if abs(m[\"e_ss\"]) > max(tol, 0.03) and sat_ok:\n cons[\"ki\"] = round(cons[\"ki\"] * 1.8, 3)\n note.append(f\"Errore a regime {m['e_ss']:+.2f} m -> aumenta ki per recuperarlo.\")\n if (m[\"osc_pp\"] < tol and abs(m[\"e_ss\"]) < tol and m[\"overshoot_pct\"] < 20):\n note.append(\"OK: risposta gia buona in questa fase.\")\n return cons, note\n\ndef _unisci_consigli(consigli_fase, base):\n \"\"\"Il firmware usa UN solo set PID per discesa e salita: fra i consigli delle\n due fasi prendiamo il kp piu' prudente (min) e kd/ki piu' incisivi (max).\"\"\"\n cons = dict(base)\n cons[\"kp\"] = min(c[\"kp\"] for c in consigli_fase.values())\n cons[\"kd\"] = max(c[\"kd\"] for c in consigli_fase.values())\n cons[\"ki\"] = max(c[\"ki\"] for c in consigli_fase.values())\n return cons\n\ndef analizza(df, target_discesa_m=None, target_salita_m=None,\n tolleranza_m=0.10, attuali=None, fase=\"auto\"):\n \"\"\"Analizza il log riconoscendo da solo le fasi presenti (discesa, salita o\n entrambe). target_discesa_m e' riferito al FONDO (campo GUI descent_target),\n target_salita_m al TOP (campo GUI ascent_target, convertito internamente).\n fase: \"auto\" | \"discesa\" | \"salita\" (forza tutto il log come quella fase).\"\"\"\n base = dict(FW[\"pid_default\"])\n if attuali:\n base.update({k: v for k, v in attuali.items() if v is not None})\n if target_discesa_m is None:\n target_discesa_m = FW[\"profile_default\"][\"descent_target\"]\n if target_salita_m is None:\n target_salita_m = FW[\"profile_default\"][\"ascent_target\"]\n tgt_sal_b = _target_salita_fondo(target_salita_m)\n targets = {\"discesa\": float(target_discesa_m), \"salita\": float(tgt_sal_b)}\n\n segs = segmenta_fasi(df, targets[\"discesa\"], targets[\"salita\"],\n tolleranza_m, fase)\n if \"salita\" in segs:\n i0, i1 = segs[\"salita\"]\n i1n = _taglia_riemersione(df[\"depth\"].values, i0, i1,\n targets[\"salita\"], tolleranza_m)\n if i1n < i1:\n print(\"Nota: escludo dall'analisi della salita il tratto finale di \"\n \"riemersione in superficie (non e' piu' il PID a inseguire il target).\")\n segs[\"salita\"] = (i0, i1n)\n ordine = sorted(segs, key=lambda k: segs[k][0])\n print(\"Fasi riconosciute: \" + \"; \".join(\n f\"{nome} t={df['t'].iloc[segs[nome][0]]:.0f}-{df['t'].iloc[segs[nome][1]]:.0f} s \"\n f\"(target {targets[nome]:.2f} m rif. FONDO)\" for nome in ordine))\n if \"salita\" in segs:\n print(f\" (target salita GUI {target_salita_m:.2f} m rif. TOP -> \"\n f\"{targets['salita']:.2f} m rif. FONDO, come nel firmware)\")\n\n _grafici(df, segs, targets, tolleranza_m)\n\n metriche, consigli, note_fasi = {}, {}, {}\n for nome in ordine:\n i0, i1 = segs[nome]\n seg = df.iloc[i0:i1 + 1].copy()\n seg[\"t\"] = seg[\"t\"] - seg[\"t\"].iloc[0]\n if len(seg) < 10:\n print(f\"\\nFase {nome}: troppi pochi campioni ({len(seg)}), la salto.\")\n continue\n m = _metriche(seg, targets[nome], tolleranza_m)\n metriche[nome] = m\n _stampa_metriche(m, nome)\n consigli[nome], note_fasi[nome] = _consigli_fase(m, tolleranza_m, base)\n\n print(\"\\n=== DIAGNOSI E CONSIGLI ===\")\n note = []\n if \"u\" not in df.columns:\n note.append(\"MANCA la colonna 'u' (apertura siringa): NON posso stimare u_neutral ne la \"\n \"saturazione. Per il tuning completo usa il log flash (DUMP_LOG, che include \"\n \"'syringe_u') oppure aggiungi syringe_u all'export della GUI.\")\n if \"phase\" in df.columns and df[\"phase\"].astype(str).str.contains(\"emergency\", case=False, regex=False).any():\n note.append(\"ATTENZIONE: nel log compare un EMERGENCY STOP (sicurezza TOF): il profilo si e \"\n \"interrotto per sicurezza, non e un problema di tuning. Controlla hardware/assetto.\")\n m_fondale = metriche.get(\"discesa\") or next(iter(metriche.values()), None)\n if m_fondale and m_fondale[\"sensor_depth\"] < 0.15:\n note.append(\"ATTENZIONE: barometro a < 15 cm dal pelo -> test poco affidabile, serve una vasca \"\n \"piu profonda. L'oscillazione qui NON e colpa del PID.\")\n for nn in note:\n print(\" - \" + nn)\n for nome in ordine:\n for nn in note_fasi.get(nome, []):\n print(f\" - [{nome}] \" + nn)\n\n if not consigli:\n print(\"Nessuna fase analizzabile: controlla i dati.\")\n return dict(fasi=segs, metriche=metriche, consigliati=None)\n if len(consigli) > 1:\n cons = _unisci_consigli(consigli, base)\n print(\"\\nNota: il firmware usa UN solo set PID per entrambe le fasi -> \"\n \"combino i consigli (kp piu' prudente, kd/ki piu' incisivi).\")\n else:\n cons = next(iter(consigli.values()))\n cons = _valida_pid(cons)\n print(\"\\n=== VALORI PID CONSIGLIATI (mettili nella GUI) ===\")\n mostra_valori(cons)\n print(\"\\nStringa firmware equivalente:\")\n print(\" \" + _stringa_pid(cons))\n return dict(fasi=segs, metriche=metriche, consigliati=cons)\n\ndef stima_u_neutral(df, targets, tolleranza_m=0.10):\n \"\"\"Stima u_neutral dai campioni stabili vicino a una delle quote in `targets`\n (lista di profondita' rif. FONDO, es. [target discesa, target salita+0,51]).\"\"\"\n if \"u\" not in df.columns:\n print(\"Impossibile stimare u_neutral: manca la colonna 'u' (apertura siringa).\")\n print(\"-> Usa il log flash (DUMP_LOG) che include 'syringe_u', \"\n \"oppure aggiungi syringe_u all'export della GUI.\")\n return None\n targets = np.atleast_1d(np.asarray(targets, dtype=float))\n t = df[\"t\"].values; d = df[\"depth\"].values; u = df[\"u\"].values\n vel = np.gradient(d, t)\n # Solo campioni SOMMERSI e ~fermi: a galla la spinta extra del volume\n # emerso rende u scorrelata dall'assetto neutro in quota (il float puo'\n # restare in superficie anche con la siringa quasi piena).\n sommerso = (d - FW[\"float_length_m\"]) > 0.15\n fermo = np.abs(vel) < 0.05\n vicino = np.zeros(len(d), bool)\n for tgt in targets:\n vicino |= np.abs(d - tgt) < max(tolleranza_m, 0.05)\n mask = sommerso & fermo & vicino\n if mask.sum() < 5 and (sommerso & fermo).sum() >= 2:\n mask = sommerso & fermo\n print(\"Pochi punti stabili ai target: stimo dai tratti fermi in quota.\")\n if mask.sum() < 2:\n k = int(len(d) * 0.8)\n mask = np.zeros(len(d), bool); mask[k:] = True\n print(\"Pochi punti stabili sommersi: stima dall'ultimo tratto del log \"\n \"(ATTENZIONE: se li' il float era a galla la stima NON vale).\")\n u_neu = float(np.clip(np.median(u[mask]), FW[\"u_min\"], FW[\"u_max\"]))\n print(f\"u_neutral stimato ~ {u_neu:.3f} (su {int(mask.sum())} campioni stabili)\")\n print(\"-> Mettilo come 'u_neutral' nella GUI.\")\n return u_neu\n\nprint(\"Funzioni pronte.\")" + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## 3) Valori di partenza consigliati\n", + "\n", + "Se non hai ancora dati, **parti da questi** (sono i valori già tarati nel firmware). Inseriscili nei campi della GUI e fai un primo test.\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "mostra_valori(FW[\"pid_default\"], \"PID - valori di partenza (mettili nella GUI)\")\n", + "print()\n", + "mostra_valori(FW[\"profile_default\"], \"Profilo - valori di partenza\")\n" + ] + }, + { + "cell_type": "markdown", + "id": "44262073", + "metadata": {}, + "source": "## 4) Analizza il tuo test\n\n**Due modi per dare i dati:**\n- Incolla nella variabile DATI qui sotto la tabella copiata dalla vista Raw chart della GUI.\n- Lascia DATI vuoto: su Colab appare il bottone per caricare un CSV, senza nulla usa l esempio.\n\n**Fasi del profilo:** con `fase = \"auto\"` il tool capisce **da solo** se il log contiene solo la discesa, solo la salita o il profilo completo (discesa → hold → salita), usando la colonna `phase` se c'è o la forma della traiettoria. Se vuoi forzare l'analisi su una sola fase scegli `discesa` o `salita`. L'eventuale riemersione finale in superficie viene esclusa automaticamente dall'analisi della salita.\n\n**Target:** `target_discesa_m` è riferito al **FONDO** del float (campo GUI `descent_target`); `target_salita_m` è riferito al **TOP** (campo GUI `ascent_target`) e viene convertito da solo (+0,51 m). Se il log copre entrambe le fasi ottieni metriche e diagnosi **per fase** e un **unico set PID consigliato** (il firmware usa gli stessi guadagni in discesa e salita)." + }, + { + "cell_type": "code", + "execution_count": null, + "id": "f80d902d", + "metadata": {}, + "outputs": [], + "source": "# @title 4) Carica i dati e analizza\n# >>> Per usare i TUOI dati: incolla qui la TABELLA copiata dalla vista \"Raw chart\"\n# (INTESTAZIONE COMPRESA) oppure un JSON, fra le triple virgolette. Vuoto = CSV/esempio.\nDATI = r\"\"\"\n\n\"\"\"\n\n# Quali fasi analizzare: \"auto\" riconosce da solo se il log contiene la sola\n# discesa, la sola salita o il profilo completo; \"discesa\"/\"salita\" forzano\n# TUTTO il log come quella sola fase.\nfase = \"auto\" # @param [\"auto\", \"discesa\", \"salita\"]\n# target di discesa: riferito al FONDO del float (campo GUI descent_target)\ntarget_discesa_m = 2.5 # @param {type:\"number\"}\n# target di salita: riferito al TOP del float (campo GUI ascent_target);\n# il tool lo converte da solo in riferimento FONDO (+0,51 m)\ntarget_salita_m = 0.40 # @param {type:\"number\"}\ntolleranza_m = 0.10 # @param {type:\"number\"}\n# (facoltativo) i parametri usati in QUESTO test, per consigli relativi:\nkp_attuale = 1.7 # @param {type:\"number\"}\nki_attuale = 0.1 # @param {type:\"number\"}\nkd_attuale = 0.3 # @param {type:\"number\"}\nu_neutral_attuale = 0.011 # @param {type:\"number\"}\n\ndf = carica(DATI)\n# Calcola il nuovo u_neutral dai dati se disponibile (campioni stabili\n# vicino a uno dei due target, in riferimento FONDO)\nu_neutral_stimato = stima_u_neutral(\n df, [target_discesa_m, _target_salita_fondo(target_salita_m)], tolleranza_m)\nif u_neutral_stimato is not None:\n u_neutral_attuale = u_neutral_stimato\n\nrisultato = analizza(df, target_discesa_m, target_salita_m, tolleranza_m,\n attuali=dict(kp=kp_attuale, ki=ki_attuale, kd=kd_attuale,\n u_neutral=u_neutral_attuale),\n fase=fase)" + }, + { + "cell_type": "markdown", + "id": "ad544561", + "metadata": {}, + "source": [ + "## 5) Simulatore didattico (facoltativo)\n", + "\n", + "Per **capire** l'effetto di `kp/ki/kd` senza il Float in acqua. Cambia i valori e riesegui la cella.\n", + "\n", + "> ⚠️ **Modello APPROSSIMATO**: serve solo a farsi un'idea. I valori finali vanno **sempre** validati sui dati reali." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "5b383dac", + "metadata": {}, + "outputs": [], + "source": [ + "# @title Simulatore APPROSSIMATO - opzionale\n", + "sim_kp = 1.7 # @param {type:\"number\"}\n", + "sim_ki = 0.1 # @param {type:\"number\"}\n", + "sim_kd = 0.3 # @param {type:\"number\"}\n", + "sim_target = 2.5 # @param {type:\"number\"}\n", + "\n", + "def simula(kp, ki, kd, target, u_neutral=0.12, T=80.0, dt=0.05):\n", + " # modello 1-DOF APPROSSIMATO: m*z'' = k_b*(u - u_neutral) - c*z'\n", + " m, k_b, c = 6.0, 8.0, 9.0\n", + " z, v = 0.51, 0.0\n", + " integ, dfilt, last_e = 0.0, 0.0, None\n", + " ts, zs, us = [], [], []\n", + " for i in range(int(T / dt)):\n", + " e = target - z\n", + " integ = float(np.clip(integ + e * dt, -5.0, 5.0))\n", + " deriv = 0.0 if last_e is None else (e - last_e) / dt\n", + " dfilt = 0.25 * deriv + 0.75 * dfilt\n", + " last_e = e\n", + " u = float(np.clip(u_neutral + kp * e + ki * integ + kd * dfilt, 0.0, 0.92))\n", + " a = (k_b * (u - u_neutral) - c * v) / m\n", + " v += a * dt; z += v * dt\n", + " ts.append(i * dt); zs.append(z); us.append(u)\n", + " return np.array(ts), np.array(zs), np.array(us)\n", + "\n", + "ts, zs, us = simula(sim_kp, sim_ki, sim_kd, sim_target)\n", + "fig, ax = plt.subplots(2, 1, figsize=(9, 5), sharex=True)\n", + "ax[0].plot(ts, zs); ax[0].axhline(sim_target, color=\"green\", ls=\"--\", label=\"target\")\n", + "ax[0].set_ylabel(\"profondita [m]\"); ax[0].invert_yaxis()\n", + "ax[0].set_title(\"Simulatore APPROSSIMATO - solo per capire l'effetto dei guadagni\")\n", + "ax[0].legend(loc=\"best\"); ax[0].grid(alpha=0.3)\n", + "ax[1].plot(ts, us, color=\"orange\"); ax[1].set_ylim(-0.05, 1.0)\n", + "ax[1].set_ylabel(\"u\"); ax[1].set_xlabel(\"tempo [s]\"); ax[1].grid(alpha=0.3)\n", + "plt.tight_layout(); plt.show()\n", + "print(\"Modello semplificato: per le decisioni finali usa i dati reali.\")\n" + ] + } + ], + "metadata": { + "colab": { + "provenance": [], + "toc_visible": true + }, + "kernelspec": { + "display_name": "Python 3", + "language": "python", + "name": "python3" + }, + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} \ No newline at end of file diff --git a/tools/pid_tuning/requirements.txt b/tools/pid_tuning/requirements.txt new file mode 100644 index 0000000..97ae105 --- /dev/null +++ b/tools/pid_tuning/requirements.txt @@ -0,0 +1,7 @@ +# Dipendenze per l'uso LOCALE del notebook (su Google Colab sono già presenti). +# Installa con: pip install -r requirements.txt +pandas>=1.5 +numpy>=1.23 +matplotlib>=3.6 +jupyter>=1.0 +ipywidgets>=8.0