From cfca629b20d5bfa936f212f0959f08b7a6a3cf3e Mon Sep 17 00:00:00 2001 From: Skeitt Date: Fri, 29 May 2026 22:57:27 +0200 Subject: [PATCH 01/19] feat: update float ESP-NOW runtime flow - add runtime profile, PID, balance and motor settings with persisted ESPA config and ESPB parser support - log and stream normalized syringe position alongside pressure and float depth - require acknowledged command handling, centralize LED states and clean obsolete espA_pool/build artifacts - update firmware docs and protocol/parser/storage tests for the current command contract --- .github/workflows/ci.yml | 2 +- .github/workflows/release.yml | 2 +- README.md | 96 +++--- include/config.h | 48 ++- include/float_common.h | 149 +++++++-- lib/comms/include/comms.h | 2 +- lib/comms/src/comms.cpp | 2 +- .../include/espb_bridge_core.h | 2 +- lib/espb_bridge_core/src/espb_bridge_core.cpp | 189 +++++++---- lib/flash_storage/include/flash_storage.h | 3 +- lib/flash_storage/src/flash_storage.cpp | 14 +- lib/led/include/led.h | 19 +- lib/motion_control/include/motion_control.h | 12 +- lib/motion_control/src/motion_control.cpp | 68 ++-- lib/pid/include/pid.h | 6 +- lib/pid/src/pid.cpp | 4 +- lib/profile/include/profile.h | 24 ++ lib/profile/src/profile.cpp | 156 ++++++++- lib/runtime_config/include/runtime_config.h | 66 ++++ lib/runtime_config/src/runtime_config.cpp | 302 ++++++++++++++++++ platformio.ini | 23 -- src/espA/main.cpp | 269 +++++++++++----- src/espB/main.cpp | 26 +- .../test_espnow_bridge/test_espnow_bridge.cpp | 4 +- .../espb_bridge/test_parser/test_parser.cpp | 72 ++++- .../test_protocol_contract.cpp | 36 ++- .../flash_storage/test_api/test_api.cpp | 14 +- 27 files changed, 1216 insertions(+), 394 deletions(-) create mode 100644 lib/runtime_config/include/runtime_config.h create mode 100644 lib/runtime_config/src/runtime_config.cpp 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/README.md b/README.md index edbcd5e..49f3c7c 100644 --- a/README.md +++ b/README.md @@ -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 @@ -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,8 +673,8 @@ 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) | @@ -766,11 +772,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 +824,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; the old `espA_pool` build and `POOL_TEST_PROFILE` flag have been removed. **Team Contact:** PoliTOcean @ Politecnico di Torino **Maintainers:** Colabella Davide, Benevenga Filippo diff --git a/include/config.h b/include/config.h index 150e27f..f694a94 100644 --- a/include/config.h +++ b/include/config.h @@ -50,16 +50,23 @@ constexpr uint32_t MOTOR_MAX_ACCELERATION = 1800; // Normal acceleration/deceler constexpr uint32_t MOTOR_HOMING_SPEED = 1800; // 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) @@ -90,22 +97,16 @@ 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) @@ -130,16 +131,6 @@ constexpr float SENSOR_TO_TOP_M = FLOAT_TOP_TO_SENSOR_M; constexpr float SURFACE_TARGET_OFFSET_M = 0.10f; 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 +138,6 @@ 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 constexpr float TARGET_SHALLOW_BOTTOM_DEPTH = TARGET_SHALLOW_TOP_DEPTH + SENSOR_TO_BOTTOM_M + SENSOR_TO_TOP_M; diff --git a/include/float_common.h b/include/float_common.h index c929087..69f08bb 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,111 @@ 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; +}; + +struct ProfileSetPayload { + uint8_t profileCount; + float deepTargetM; + float shallowTopTargetM; + float depthToleranceM; + float holdTimeS; + float pidTimeoutS; + float ascentTimeoutS; + float surfaceOffsetM; +}; + +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/comms/include/comms.h b/lib/comms/include/comms.h index cdde787..99709a5 100644 --- a/lib/comms/include/comms.h +++ b/lib/comms/include/comms.h @@ -29,7 +29,7 @@ class CommsManager { // 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; } + void clearCommand() { _received = makeOutputMessage(CMD_IDLE); } // Outgoing packet — callers fill status_to_send.charge before calling sendMessage() input_message status_to_send; diff --git a/lib/comms/src/comms.cpp b/lib/comms/src/comms.cpp index fe9980f..fd0bbe4 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); } // --------------------------------------------------------------------------- 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..e9cb418 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.deepTargetM = values[0]; + parsed.message.payload.profileSet.shallowTopTargetM = values[1]; + parsed.message.payload.profileSet.depthToleranceM = values[2]; + parsed.message.payload.profileSet.holdTimeS = values[3]; + parsed.message.payload.profileSet.pidTimeoutS = values[4]; + parsed.message.payload.profileSet.ascentTimeoutS = values[5]; + parsed.message.payload.profileSet.surfaceOffsetM = 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..b5965c1 100644 --- a/lib/motion_control/include/motion_control.h +++ b/lib/motion_control/include/motion_control.h @@ -21,7 +21,7 @@ class MotionController { 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; } @@ -39,8 +39,14 @@ class MotionController { 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); + 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..47b469b 100644 --- a/lib/motion_control/src/motion_control.cpp +++ b/lib/motion_control/src/motion_control.cpp @@ -4,6 +4,7 @@ #include "sensors.h" #include "comms.h" #include "DebugSerial.h" +#include "runtime_config.h" /* ******************************************************************************* @@ -128,7 +129,9 @@ bool MotionController::tofMaxExtensionStopReached(unsigned long nowMs, return false; } -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 +140,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 +160,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 +172,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; @@ -187,8 +194,10 @@ bool MotionController::homeWithTof(float stopPressureKpa, bool* pressureStop, ui _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; @@ -268,9 +277,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; } @@ -312,15 +321,14 @@ bool MotionController::homeWithTof(float stopPressureKpa, bool* pressureStop, ui 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"); return false; } _motor.setCurrentPosition(0); - _motor.setMaxSpeed(MOTOR_MAX_SPEED); - _motor.setAcceleration(MOTOR_MAX_ACCELERATION); + runtimeConfig.applyMotorConfig(); _motor.disableOutputs(); clearEmergencyStop(); @@ -377,6 +385,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 +393,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; } @@ -442,14 +451,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()); @@ -478,7 +486,9 @@ bool MotionController::_balanceStrokeTo(long targetPos, remoteStopHit = true; return false; } - if (pressureStopReached(stopPressureKpa, pressureStopSamples)) { + if (pressureStopReached(stopPressureKpa, + runtimeConfig.balance().stopPressureSamples, + pressureStopSamples)) { pressureStopHit = true; return false; } @@ -497,7 +507,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,14 +520,16 @@ 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. @@ -526,7 +538,7 @@ bool MotionController::balance(uint32_t holdMs) { 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 +548,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 +563,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/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..73fca8c 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,30 @@ ******************************************************************************* */ +struct RuntimeProfileConfig { + uint8_t profileCount = PROFILE_MAX_COUNT; + float deepTargetM = TARGET_DEPTH; + float shallowTopTargetM = TARGET_SHALLOW_TOP_DEPTH; + float depthToleranceM = DEPTH_MAX_ERROR; + float holdTimeS = STAT_TIME; + float pidTimeoutS = TIMEOUT_PID_TIME; + float ascentTimeoutS = TIMEOUT_ASCENT; + float surfaceOffsetM = SURFACE_TARGET_OFFSET_M; +}; + 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 shallowBottomTargetM() 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 +66,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..d51bebc 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,11 +26,143 @@ ProfileManager::ProfileManager() {} namespace { +constexpr uint32_t PROFILE_CONFIG_MAGIC = 0x50464C54UL; // "PFLT" +constexpr uint16_t PROFILE_CONFIG_VERSION = 1; +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::shallowBottomTargetM() const { + return _config.shallowTopTargetM + 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 shallowBottomM = + config.shallowTopTargetM + SENSOR_TO_BOTTOM_M + SENSOR_TO_TOP_M; + + return config.profileCount >= 1 && config.profileCount <= 10 && + isfinite(config.deepTargetM) && + isfinite(config.shallowTopTargetM) && + isfinite(config.depthToleranceM) && + isfinite(config.holdTimeS) && + isfinite(config.pidTimeoutS) && + isfinite(config.ascentTimeoutS) && + isfinite(config.surfaceOffsetM) && + config.deepTargetM >= 0.0f && config.deepTargetM <= 5.0f && + config.shallowTopTargetM >= 0.0f && config.shallowTopTargetM <= 5.0f && + config.depthToleranceM >= 0.005f && config.depthToleranceM <= 1.0f && + config.holdTimeS >= 1.0f && config.holdTimeS <= 600.0f && + config.pidTimeoutS >= 5.0f && config.pidTimeoutS <= 900.0f && + config.ascentTimeoutS >= 5.0f && config.ascentTimeoutS <= 900.0f && + config.surfaceOffsetM >= 0.0f && config.surfaceOffsetM <= 5.0f && + shallowBottomM < config.deepTargetM; +} + +// --------------------------------------------------------------------------- +void ProfileManager::formatConfigJson(char* buffer, size_t bufferSize) const { + if (buffer == nullptr || bufferSize == 0) return; + + snprintf(buffer, bufferSize, + "{\"profile_count\":%u," + "\"deep_target_m\":%.3f," + "\"shallow_top_m\":%.3f," + "\"shallow_bottom_m\":%.3f," + "\"depth_tolerance_m\":%.3f," + "\"hold_s\":%.1f," + "\"pid_timeout_s\":%.1f," + "\"ascent_timeout_s\":%.1f," + "\"surface_offset_m\":%.3f}", + static_cast(_config.profileCount), + _config.deepTargetM, + _config.shallowTopTargetM, + shallowBottomTargetM(), + _config.depthToleranceM, + _config.holdTimeS, + _config.pidTimeoutS, + _config.ascentTimeoutS, + _config.surfaceOffsetM); +} + +// --------------------------------------------------------------------------- +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.surfaceOffsetM); +} + // --------------------------------------------------------------------------- void ProfileManager::resetEEPROM() { _writePtr = 0; @@ -59,14 +192,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 +227,8 @@ void ProfileManager::_logProfileReading(const char* phase) { sensors.pressure() / 1000.0f, sensors.referenceDepthForPhase(phase), phase, - sensors.sensorDepth()); + sensors.sensorDepth(), + motorPosToU(motor.position())); } // --------------------------------------------------------------------------- @@ -109,8 +245,8 @@ void ProfileManager::measure(float targetDepth, float holdTimeSec, float timeout 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.deepTargetM) < 0.001f; + const bool isShallowTarget = fabsf(targetDepth - shallowBottomTargetM()) < 0.001f; // --- LED and initial motor positioning --- if (isPIDPhase) { @@ -157,7 +293,7 @@ void ProfileManager::measure(float targetDepth, float holdTimeSec, float timeout // --- 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; @@ -169,11 +305,11 @@ void ProfileManager::measure(float targetDepth, float holdTimeSec, float timeout if (isPIDPhase) { if (isShallowTarget) { - phase = (fabsf(currentDepth - targetDepth) < DEPTH_MAX_ERROR) + phase = (fabsf(currentDepth - targetDepth) < _config.depthToleranceM) ? "hold_40cm" : "ascending"; } else { - phase = (fabsf(currentDepth - targetDepth) < DEPTH_MAX_ERROR) + phase = (fabsf(currentDepth - targetDepth) < _config.depthToleranceM) ? "hold_2_5m" : "descending"; } @@ -247,7 +383,7 @@ void ProfileManager::measure(float targetDepth, float holdTimeSec, float timeout (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,7 +397,7 @@ 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 = 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/platformio.ini b/platformio.ini index 5a763e3..58b91e6 100644 --- a/platformio.ini +++ b/platformio.ini @@ -57,29 +57,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..d63dfd6 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; @@ -98,6 +98,12 @@ void setup() { }); Debug.println("DebugSerial ready"); + // --- Runtime PID / balance / motor settings --- + runtimeConfig.begin(); + + // --- Runtime mission profile --- + profileManager.beginConfig(); + // --- Internal flash mission log --- if (flashStorage.begin()) { if (!flashStorage.clearLog()) { @@ -127,6 +133,7 @@ void setup() { // --- Motor + homing --- motor.begin(); + runtimeConfig.applyMotorConfig(); Debug.println("Initializing TOF sensor..."); if (!tofSensor.begin()) { @@ -202,7 +209,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); @@ -218,38 +225,44 @@ 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(); - } + const RuntimeProfileConfig& profileConfig = profileManager.config(); + Debug.println("Mission: starting vertical profiles"); - 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); + uint8_t completedProfiles = 0; + 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.deepTargetM); + profileManager.measure(profileConfig.deepTargetM, + profileConfig.holdTimeS, + profileConfig.pidTimeoutS); if (!motionController.motionAllowed()) { break; } delay(500); - 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); + Debug.printf("Profile %d: PID ascent to %.2f m top reference\n", + completedProfiles + 1, profileConfig.shallowTopTargetM); + profileManager.measure(profileManager.shallowBottomTargetM(), + profileConfig.holdTimeS, + profileConfig.ascentTimeoutS); if (!motionController.motionAllowed()) { break; } motor.disableOutputs(); - g_profileCount++; - Debug.printf("Profile %d complete\n", g_profileCount); + completedProfiles++; + Debug.printf("Profile %d complete\n", completedProfiles); delay(500); } + + if (g_autoCommitted && completedProfiles >= profileConfig.profileCount) { + g_autoMissionDone = true; + } } g_status = CMD_IDLE; @@ -269,7 +282,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 +293,7 @@ void loop() { { if (comms.sendMessage(CMD4_ACK, 1000)) { profileManager.clearEEPROM(); + g_autoMissionDone = false; } g_status = CMD_IDLE; break; @@ -290,6 +304,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 +326,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 +354,31 @@ void loop() { } // ----------------------------------------------------------------------- - case CMD_UPDATE_PID: // Update PID gains at runtime + case CMD_PID_CONFIG_SET: { - 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 - { - 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 PidConfigPayload& payload = comms.lastCommand().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 +387,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 +436,9 @@ 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 SyringeSetPayload& payload = comms.lastCommand().payload.syringeSet; + const float u = payload.uNorm; + const float dur = payload.durationS; runSyringeSet(u, dur); } g_status = CMD_IDLE; @@ -440,8 +449,9 @@ 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 PidHoldPayload& payload = comms.lastCommand().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,94 @@ 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 ProfileSetPayload& payload = comms.lastCommand().payload.profileSet; + RuntimeProfileConfig nextConfig; + nextConfig.profileCount = payload.profileCount; + nextConfig.deepTargetM = payload.deepTargetM; + nextConfig.shallowTopTargetM = payload.shallowTopTargetM; + nextConfig.depthToleranceM = payload.depthToleranceM; + nextConfig.holdTimeS = payload.holdTimeS; + nextConfig.pidTimeoutS = payload.pidTimeoutS; + nextConfig.ascentTimeoutS = payload.ascentTimeoutS; + nextConfig.surfaceOffsetM = payload.surfaceOffsetM; + + 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 BalanceConfigPayload& payload = comms.lastCommand().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 MotorConfigPayload& payload = comms.lastCommand().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 +575,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 @@ -509,30 +603,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, " "); @@ -557,7 +656,7 @@ static void servicePidTuningSerial() { } return; } - if (g_serialLineBuf.length() < 95) g_serialLineBuf += c; + if (g_serialLineBuf.length() < 191) g_serialLineBuf += c; } } @@ -619,7 +718,7 @@ static void runPidHold(float depthTarget, float durationS) { 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(); @@ -672,7 +771,7 @@ static void runPidStep(float depthTarget) { 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(); 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..1f3a778 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.deepTargetM); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.4f, profile.message.payload.profileSet.shallowTopTargetM); + 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.pidTimeoutS); + 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.surfaceOffsetM); +} + +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"); From c081f838e74a2d6c12bf038addae32d027bb4512 Mon Sep 17 00:00:00 2001 From: Skeitt Date: Mon, 1 Jun 2026 15:33:56 +0200 Subject: [PATCH 02/19] docs: removed espA_pool dependencies --- CONTRIBUTING.md | 10 ++++------ README.md | 2 +- 2 files changed, 5 insertions(+), 7 deletions(-) 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 49f3c7c..9eb7c7e 100644 --- a/README.md +++ b/README.md @@ -827,6 +827,6 @@ Recent changes: - 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). -- Runtime profile, PID, balance, and motor settings are configurable from NEXUS/GUI and persisted on ESPA; the old `espA_pool` build and `POOL_TEST_PROFILE` flag have been removed. +- 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 From c4160e8a8fb532eef725d393141d9642617fddf2 Mon Sep 17 00:00:00 2001 From: Davide-Colabella Date: Fri, 29 May 2026 17:46:22 +0200 Subject: [PATCH 03/19] fix(motion): correct motor geometry convention to match homing MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit home (motor_pos=0) pushes water out → floats; negative positions move toward the TOF and take on water → sinks. Simplify uToMotorPos to map u in [0,1] onto negative travel, dropping the unused MOTOR_INVERT_LOGICAL flag, and update balance() comments accordingly. --- lib/motion_control/src/motion_control.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/lib/motion_control/src/motion_control.cpp b/lib/motion_control/src/motion_control.cpp index 47b469b..1680eb0 100644 --- a/lib/motion_control/src/motion_control.cpp +++ b/lib/motion_control/src/motion_control.cpp @@ -531,8 +531,8 @@ bool MotionController::balance() { stopPressureKpa, 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); From 0a9ecd0cddcd89528751d55df5b5681998a866dc Mon Sep 17 00:00:00 2001 From: Davide-Colabella Date: Tue, 2 Jun 2026 13:17:12 +0200 Subject: [PATCH 04/19] feat(diagnostics): persistent flash log + emergency-stop forensics MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Mission log survives a power-cycle (cleared only at the start of a new mission, no longer at boot), so a failed pool test can be read afterwards over USB. Add a DUMP_LOG serial command to re-read it on demand without clearing — solves the case where the monitor connects after boot. emergencyStop() records reason + TOF distance to the flash log the instant it fires (idempotent single write) instead of a posteriori in loop(), where it was missed when measure() exited on phase timeout. Expose lastStopReason()/lastStopTofMm(). TOF safety now debounces with TOF_SAFETY_STOP_SAMPLES consecutive out-of-range reads to ignore single glitches (bubbles, reflections) in water, and clamps the PID output to [PID_U_MIN, PID_U_MAX] so the syringe never reaches the TOF safety limits. Enable monitor echo + LF EOL so serial commands are visible and terminated as the parser expects. --- include/config.h | 16 ++++++ lib/motion_control/include/motion_control.h | 12 +++++ lib/motion_control/src/motion_control.cpp | 60 ++++++++++++++++++--- lib/profile/src/profile.cpp | 11 +++- platformio.ini | 2 + src/espA/main.cpp | 58 ++++++++++++++++++-- 6 files changed, 145 insertions(+), 14 deletions(-) diff --git a/include/config.h b/include/config.h index f694a94..9b41641 100644 --- a/include/config.h +++ b/include/config.h @@ -83,6 +83,22 @@ constexpr float TOF_HOMING_THRESHOLD = 75.0f; // Homing stop distance: st 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. +// 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 (40/85 mm), lasciando margine contro passi persi +// e rumore. La corsa motore (~35 mm) entra nella finestra TOF (~45 mm) ma con +// poco margine agli estremi: questo clamp lo recupera. +// 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 diff --git a/lib/motion_control/include/motion_control.h b/lib/motion_control/include/motion_control.h index b5965c1..9fec51c 100644 --- a/lib/motion_control/include/motion_control.h +++ b/lib/motion_control/include/motion_control.h @@ -30,10 +30,22 @@ 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, diff --git a/lib/motion_control/src/motion_control.cpp b/lib/motion_control/src/motion_control.cpp index 1680eb0..d717695 100644 --- a/lib/motion_control/src/motion_control.cpp +++ b/lib/motion_control/src/motion_control.cpp @@ -5,6 +5,7 @@ #include "comms.h" #include "DebugSerial.h" #include "runtime_config.h" +#include "flash_storage.h" /* ******************************************************************************* @@ -25,11 +26,37 @@ 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::serviceEmergencyStop() { @@ -112,21 +139,37 @@ bool MotionController::tofMaxExtensionStopReached(unsigned long nowMs, return false; } - if (distanceMm < TOF_SAFE_RANGE_MIN_MM) { + 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 false; + } + + // Lettura fuori range: conferma prima di fermare, 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 false; + } + + _tofOutOfRangeCount = 0; + _lastStopTofMm = distanceMm; + if (tooClose) { 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; - } - - if (distanceMm > TOF_SAFE_RANGE_MAX_MM) { + } else { 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; } - - return false; + return true; } bool MotionController::pressureStopReached(float stopPressureKpa, @@ -430,6 +473,7 @@ bool MotionController::moveToMax(uint32_t timeoutMs, 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); + _lastStopTofMm = distanceMm; emergencyStop("TOF above safe range"); return false; } diff --git a/lib/profile/src/profile.cpp b/lib/profile/src/profile.cpp index d51bebc..dcd8ff4 100644 --- a/lib/profile/src/profile.cpp +++ b/lib/profile/src/profile.cpp @@ -167,6 +167,10 @@ void ProfileManager::_applyConfigToSubsystems() { 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(); } // --------------------------------------------------------------------------- @@ -378,7 +382,12 @@ 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); diff --git a/platformio.ini b/platformio.ini index 58b91e6..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 = diff --git a/src/espA/main.cpp b/src/espA/main.cpp index d63dfd6..f22a82b 100644 --- a/src/espA/main.cpp +++ b/src/espA/main.cpp @@ -105,12 +105,21 @@ void setup() { 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"); } @@ -224,11 +233,21 @@ void loop() { { bool ack = g_autoCommitted ? true : comms.sendMessage(CMD1_ACK, 1000); + // 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(); + + // 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; + if (ack && motionController.motionAllowed()) { const RuntimeProfileConfig& profileConfig = profileManager.config(); Debug.println("Mission: starting vertical profiles"); - uint8_t completedProfiles = 0; profileManager.resetEEPROM(); profileManager.logDeploymentPacket(); @@ -240,6 +259,7 @@ void loop() { profileConfig.holdTimeS, profileConfig.pidTimeoutS); if (!motionController.motionAllowed()) { + aborted = true; break; } @@ -251,6 +271,7 @@ void loop() { profileConfig.holdTimeS, profileConfig.ascentTimeoutS); if (!motionController.motionAllowed()) { + aborted = true; break; } @@ -265,6 +286,24 @@ void loop() { } } + // Auto-recovery: senza telemetria, un emergency stop lascerebbe il + // float bloccato sul fondo col LED rosso e ogni comando successivo + // rifiutato da motionAllowed(). Logghiamo l'evento per il post-mortem + // e tentiamo un homing, che cancella l'emergency stop e riporta la + // siringa in posizione nota (galleggiamento), così il float risale e + // resta pronto per un nuovo GO. + if (aborted) { + Debug.println("Profile aborted by emergency stop — attempting auto-recovery"); + // Il record emergency_stop (reason + tof) è già stato scritto sul + // flash da emergencyStop() nell'istante dello stop: qui non serve + // ri-loggare. Tentiamo solo l'auto-recovery. + if (motionController.homeWithTof()) { + Debug.println("Auto-recovery homing complete — float ready"); + } else { + Debug.println("Auto-recovery homing FAILED — float remains in error"); + } + } + g_status = CMD_IDLE; break; } @@ -651,6 +690,15 @@ 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, "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); } From e670094d95c91adb66f2a8a07c1280b3f2f9d5dd Mon Sep 17 00:00:00 2001 From: Davide-Colabella Date: Tue, 2 Jun 2026 13:17:47 +0200 Subject: [PATCH 05/19] tune(pid): pool-test gains + softer descent kick-start MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Default PID gains iterated against pool tests: Kp 0.17 was too weak (the float barely moved), Kp 2.0 / Kd 0.13 oscillated (±15cm pumping). Settle on Kp 1.0 / Ki 0 / Kd 0.5 as a damped starting point; fine-tune further at runtime via PID_CONFIG_SET. Reduce the descent kick-start from u=0.979 (near-full syringe, which drove the float straight to the bottom before the PID could brake, ~26cm overshoot) to PID_DESCENT_KICK_U=0.30, so the descent starts gently and the PID takes over before overshooting the target. --- include/config.h | 16 ++++++++++++++-- lib/profile/src/profile.cpp | 13 ++++++------- 2 files changed, 20 insertions(+), 9 deletions(-) diff --git a/include/config.h b/include/config.h index 9b41641..97b8a34 100644 --- a/include/config.h +++ b/include/config.h @@ -124,13 +124,25 @@ constexpr uint16_t DATA_PACKET_PERIOD_MS = 5000; // Packet cadence shown to judg // 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 +// 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 (attuale): dimezza l'aggressività e aumenta lo smorzamento +// per fermare l'oscillazione. Ki=0 finché P/D non sono stabili (l'integrale +// peggiora il pompaggio). Affinare ancora a runtime con PID_CONFIG_SET. +constexpr float PID_KP_DEFAULT = 1.0f; // 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) +constexpr float PID_KD_DEFAULT = 0.5f; // 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 per avviare la discesa senza superare il target. +constexpr float PID_DESCENT_KICK_U = 0.30f; // --------------------------------------------------------------------------- // FLOAT PHYSICAL / MISSION CONSTANTS diff --git a/lib/profile/src/profile.cpp b/lib/profile/src/profile.cpp index dcd8ff4..a441746 100644 --- a/lib/profile/src/profile.cpp +++ b/lib/profile/src/profile.cpp @@ -258,10 +258,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); @@ -275,9 +274,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) { From c0ea7b5ad007ec284ddb26aeaeccc79da8de88f0 Mon Sep 17 00:00:00 2001 From: Davide-Colabella Date: Tue, 2 Jun 2026 13:22:03 +0200 Subject: [PATCH 06/19] tune(pid): set default gains to Kp=1.7 Ki=0.1 Kd=0.3 MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Validated in the pool: the float converges on the target depth with ~±1cm final oscillation. Ki vinces the buoyancy offset (recovery), Kd damps the overshoot. Replaces the previous 1.0/0/0.5 starting point. --- include/config.h | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/include/config.h b/include/config.h index 97b8a34..163ff19 100644 --- a/include/config.h +++ b/include/config.h @@ -128,12 +128,13 @@ constexpr uint16_t PID_PERIOD_DEFAULT_MS = 50; // Default tick PID (ms) // - 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 (attuale): dimezza l'aggressività e aumenta lo smorzamento -// per fermare l'oscillazione. Ki=0 finché P/D non sono stabili (l'integrale -// peggiora il pompaggio). Affinare ancora a runtime con PID_CONFIG_SET. -constexpr float PID_KP_DEFAULT = 1.0f; // frazione_corsa / m -constexpr float PID_KI_DEFAULT = 0.0f; // frazione_corsa / (m·s) -constexpr float PID_KD_DEFAULT = 0.5f; // frazione_corsa / (m/s) +// - Kp=1.0 Kd=0.5: smorzato ma si "sedeva" in superficie (ripresa debole). +// - Kp=1.7 Ki=0.1 Kd=0.3 (attuale): converge sul target con oscillazione +// finale ±1cm. Il Ki vince l'offset di galleggiamento (ripresa), Kd smorza. +// Affinare ancora a runtime con PID_CONFIG_SET se serve. +constexpr float PID_KP_DEFAULT = 1.7f; // frazione_corsa / m +constexpr float PID_KI_DEFAULT = 0.1f; // frazione_corsa / (m·s) +constexpr float PID_KD_DEFAULT = 0.3f; // 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) From 8bcb5dc150f6cae0a047163f53d50125c6f121da Mon Sep 17 00:00:00 2001 From: Davide-Colabella Date: Sat, 6 Jun 2026 17:18:21 +0200 Subject: [PATCH 07/19] feat(manual-keyboard): jog relativo non clampato per recupero pistone MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Il manual keyboard serve a riallineare un pistone disallineato, quindi deve poter uscire dal range nominale (±MAX-margin) in entrambi i versi. - motor: nuovo startJogStepsUnclamped() che bypassa volutamente il clamp software dei fine corsa (startMoveTo/startMoveSteps restano clampati per il firmware di missione). - manual keyboard: startHoldMove() ora fa un jog relativo di ~2 mm (MANUAL_JOG_STEPS) rinnovato mentre il tasto è tenuto premuto, con stop manuale (space/x). - rimosso l'auto-stop su distanza TOF: usava una costante rimossa e la vecchia convenzione geometrica (estensione=positivo), incoerente con l'homing attuale. --- lib/motor/include/motor.h | 5 ++++ lib/motor/src/motor.cpp | 7 +++++ src/espA_manual_keyboard/main.cpp | 44 +++++++++++++++---------------- 3 files changed, 34 insertions(+), 22 deletions(-) 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/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) { From 34422bd3dc6fc3e330dc017592e1a2699b2c0522 Mon Sep 17 00:00:00 2001 From: Davide-Colabella Date: Sat, 6 Jun 2026 17:18:51 +0200 Subject: [PATCH 08/19] tune(tof): ricalibra soglie di sicurezza sulla finestra misurata in piscina Soglie tarate sulla distanza TOF reale (corretta dell'offset 6 mm): pistone esteso ~29 mm, retratto ~79 mm, pendenza ~1.1 mm TOF/mm motore. - TOF_SAFE_RANGE_MIN 40->32 mm: ~3 mm sopra il fondo corsa esteso (29); oltre si apre il tappo ed entra acqua. - TOF_SAFE_RANGE_MAX 85->82 mm: ~3 mm sopra il retratto (79); oltre = anomalia (passi persi / verso sbagliato). - TOF_HOMING_THRESHOLD 75->70 mm: lo stop reale cade qualche mm sopra per polling + conferma + risoluzione grezza, restando sotto MAX. - nuovo TOF_HOMING_CONFIRM_SAMPLES=2: conferma il trigger di homing per ignorare campioni rumorosi singoli (stesso pattern di SAFETY_STOP_SAMPLES). --- include/config.h | 28 ++++++++++++++++++++++------ 1 file changed, 22 insertions(+), 6 deletions(-) diff --git a/include/config.h b/include/config.h index 163ff19..f845c19 100644 --- a/include/config.h +++ b/include/config.h @@ -79,10 +79,26 @@ 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 @@ -90,9 +106,9 @@ constexpr float TOF_SAFE_RANGE_MAX_MM = 85.0f; // Safety range upper bound 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 (40/85 mm), lasciando margine contro passi persi -// e rumore. La corsa motore (~35 mm) entra nella finestra TOF (~45 mm) ma con -// poco margine agli estremi: questo clamp lo recupera. +// 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 From 3f0f21e6ae03124a0dd92fa1aa348e010056ec60 Mon Sep 17 00:00:00 2001 From: Davide-Colabella Date: Sat, 6 Jun 2026 17:19:38 +0200 Subject: [PATCH 09/19] refactor(motion): guardia TOF asimmetrica + flash log dell'homing MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Sostituisce tofMaxExtensionStopReached() con tofGuard() che ritorna un enum TofGuard {Ok, ExtendLimit, Emergency}, perché i due estremi fisici richiedono azioni diverse: - limite inferiore (siringa estesa, vicina al TOF): oltre si apre il tappo -> ExtendLimit, il chiamante fa uno STOP PULITO senza abortire. - limite superiore (retratta, lontana): oltre = anomalia -> Emergency. waitForMotor/moveToMax/_balanceStrokeTo passano tutti dalla guardia unificata (moveToMax non duplica più la logica TOF inline). Aggiunge logHomingEvent(): scrive su flash (sopravvive al power-cycle, in piscina la USB e scollegata) ogni tappa dell'homing - start, timeout, no-approach/no-detect, settle, backoff, complete - per ricostruire a posteriori dove l'homing ha sbagliato. Le fasi di approach e home richiedono ora TOF_HOMING_CONFIRM_SAMPLES letture consecutive sopra soglia prima di accettare il trigger. --- lib/motion_control/include/motion_control.h | 23 ++- lib/motion_control/src/motion_control.cpp | 195 +++++++++++++++----- 2 files changed, 167 insertions(+), 51 deletions(-) diff --git a/lib/motion_control/include/motion_control.h b/lib/motion_control/include/motion_control.h index 9fec51c..e730201 100644 --- a/lib/motion_control/include/motion_control.h +++ b/lib/motion_control/include/motion_control.h @@ -12,10 +12,25 @@ ******************************************************************************* */ +// 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); @@ -48,9 +63,11 @@ class MotionController { uint8_t _tofOutOfRangeCount = 0; float readPressureKpa(); - bool tofMaxExtensionStopReached(unsigned long nowMs, - unsigned long& lastTofSampleMs, - const char* context); + + // 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); diff --git a/lib/motion_control/src/motion_control.cpp b/lib/motion_control/src/motion_control.cpp index d717695..06b9380 100644 --- a/lib/motion_control/src/motion_control.cpp +++ b/lib/motion_control/src/motion_control.cpp @@ -59,6 +59,21 @@ void MotionController::emergencyStop(const char* reason) { 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() { if (!_emergencyStop) { return; @@ -106,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(); @@ -122,21 +145,21 @@ 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; } const bool tooClose = distanceMm < TOF_SAFE_RANGE_MIN_MM; @@ -146,30 +169,33 @@ bool MotionController::tofMaxExtensionStopReached(unsigned long nowMs, // Lettura valida: azzera la conferma in corso (un glitch isolato non // deve accumularsi nel tempo). _tofOutOfRangeCount = 0; - return false; + return TofGuard::Ok; } - // Lettura fuori range: conferma prima di fermare, per ignorare glitch - // singoli (bolle, riflessi, torbidità) tipici del TOF in acqua. + // 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 false; + return TofGuard::Ok; } _tofOutOfRangeCount = 0; _lastStopTofMm = distanceMm; if (tooClose) { - Debug.printf("%s: TOF safety stop, too close (%.1f < %.1f mm)\n", + // 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); - emergencyStop("TOF below safe range"); - } else { - 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::ExtendLimit; } - return true; + + // 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, @@ -235,6 +261,17 @@ 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(); const RuntimeMotorConfig& motorConfig = runtimeConfig.motor(); @@ -253,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; @@ -261,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; } @@ -273,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; } } } @@ -287,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; } @@ -299,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; } @@ -311,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()) { @@ -332,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; } @@ -344,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; } } } @@ -358,6 +438,16 @@ 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; } @@ -367,6 +457,10 @@ bool MotionController::homeWithTof(float stopPressureKpa, bool* pressureStop, ui _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; } @@ -376,6 +470,7 @@ bool MotionController::homeWithTof(float stopPressureKpa, bool* pressureStop, ui clearEmergencyStop(); Debug.println("Motor homing: complete, position set to 0"); + logHomingEvent("complete pos=0"); return true; } @@ -453,30 +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); - _lastStopTofMm = distanceMm; - emergencyStop("TOF above safe range"); - return false; - } + case TofGuard::Ok: + break; } ledController.update(); @@ -525,6 +609,9 @@ 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; @@ -543,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(); } From a700d67b8f91babf21aff15183b392812e760f81 Mon Sep 17 00:00:00 2001 From: Davide-Colabella Date: Sat, 6 Jun 2026 17:19:46 +0200 Subject: [PATCH 10/19] feat(espA): supervisione TOF nelle routine syringe/PID Le routine runSyringeSet/runPidHold/runPidStep ora chiamano tofGuard() a ogni movimento: - Emergency -> abort della routine. - ExtendLimit (saturazione a piena estensione): ferma il pistone e, nelle routine PID, inibisce ulteriori comandi di estensione (riaprirebbero il tappo) finche il PID non chiede di RISALIRE verso home. --- src/espA/main.cpp | 56 +++++++++++++++++++++++++++++++++++++++++++++-- 1 file changed, 54 insertions(+), 2 deletions(-) diff --git a/src/espA/main.cpp b/src/espA/main.cpp index f22a82b..c682676 100644 --- a/src/espA/main.cpp +++ b/src/espA/main.cpp @@ -727,8 +727,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(); @@ -772,19 +783,42 @@ static void runPidHold(float depthTarget, float durationS) { const unsigned long t0 = millis(); unsigned long lastTick = 0; unsigned long lastLog = 0; + unsigned long lastTofSampleMs = 0; + bool atExtensionLimit = false; // pistone fermo al fondo corsa (tappo): non ricomandare verso l'estensione while (millis() - t0 < (unsigned long)(durationS * 1000.0f)) { 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) { lastLog = millis(); @@ -825,19 +859,37 @@ static void runPidStep(float depthTarget) { const unsigned long t0 = millis(); unsigned long lastTick = 0; unsigned long lastLog = 0; + unsigned long lastTofSampleMs = 0; + bool atExtensionLimit = false; // pistone fermo al fondo corsa (tappo): non ricomandare verso l'estensione while (millis() - t0 < 60000UL) { if (Serial.available()) { Debug.println("# aborted"); break; } if (motionController.remoteStopRequested()) { Debug.println("# remote stop"); break; } + // Supervisione TOF su ogni movimento (vedi runPidHold per la semantica). + 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) { + const bool retreating = posTarget > motor.position(); + if ((!atExtensionLimit || retreating) && + labs(posTarget - lastCommandedTarget) >= deadbandSteps) { motor.startMoveTo(posTarget); lastCommandedTarget = posTarget; + atExtensionLimit = false; } if (millis() - lastLog >= 100) { lastLog = millis(); From db036f271ce86c70335fde54c18caa3a2ed26f6c Mon Sep 17 00:00:00 2001 From: Davide-Colabella Date: Sat, 6 Jun 2026 18:18:14 +0200 Subject: [PATCH 11/19] refactor(profile): rinomina parametri profilo con schema per fase (descent/ascent) I nomi precedenti (deepTargetM, shallowTopTargetM, pidTimeoutS, surfaceOffsetM) erano poco chiari e confondevano durante i test. Rinominati in inglese con schema orientato alla fase di missione: deepTargetM -> descentTargetM (target discesa, rif. fondo) shallowTopTargetM -> ascentTargetM (target risalita, rif. top) pidTimeoutS -> descentTimeoutS surfaceOffsetM -> surfaceRestOffsetM shallowBottomTargetM()-> ascentTargetBottomM() depthToleranceM, holdTimeS, ascentTimeoutS, profileCount invariati. Chiavi JSON di formatConfigJson e comando USB PROFILE_SET allineati. Refactor di soli nomi: ordine/tipi dei campi di ProfileSetPayload e RuntimeProfileConfig invariati, quindi layout binario e config NVS restano compatibili (PROFILE_CONFIG_VERSION non incrementato). Nessuna modifica a logica di controllo, range di validazione o protocollo. Aggiunti commenti che marcano i 4 parametri essenziali vs i 4 avanzati/safety. --- include/float_common.h | 20 ++++--- lib/espb_bridge_core/src/espb_bridge_core.cpp | 10 ++-- lib/profile/include/profile.h | 12 ++--- lib/profile/src/profile.cpp | 54 ++++++++++--------- src/espA/main.cpp | 18 +++---- 5 files changed, 60 insertions(+), 54 deletions(-) diff --git a/include/float_common.h b/include/float_common.h index 69f08bb..2b71500 100644 --- a/include/float_common.h +++ b/include/float_common.h @@ -137,15 +137,19 @@ 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; - float deepTargetM; - float shallowTopTargetM; - float depthToleranceM; - float holdTimeS; - float pidTimeoutS; - float ascentTimeoutS; - float surfaceOffsetM; + 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 { diff --git a/lib/espb_bridge_core/src/espb_bridge_core.cpp b/lib/espb_bridge_core/src/espb_bridge_core.cpp index e9cb418..0309a7f 100644 --- a/lib/espb_bridge_core/src/espb_bridge_core.cpp +++ b/lib/espb_bridge_core/src/espb_bridge_core.cpp @@ -183,7 +183,7 @@ EspbParsedCommand espbParseSerialCommand(const char* line) { } if (strcmp(token, "PROFILE_SET") == 0) { - // PROFILE_SET + // PROFILE_SET long count = 0; float values[7] = {}; if (!parseLongToken(strtok(nullptr, " "), count) || @@ -203,13 +203,13 @@ EspbParsedCommand espbParseSerialCommand(const char* line) { parsed = makeForwardCommand(CMD_PROFILE_SET); parsed.message.payload.profileSet.profileCount = static_cast(count); - parsed.message.payload.profileSet.deepTargetM = values[0]; - parsed.message.payload.profileSet.shallowTopTargetM = values[1]; + 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.pidTimeoutS = values[4]; + parsed.message.payload.profileSet.descentTimeoutS = values[4]; parsed.message.payload.profileSet.ascentTimeoutS = values[5]; - parsed.message.payload.profileSet.surfaceOffsetM = values[6]; + parsed.message.payload.profileSet.surfaceRestOffsetM = values[6]; return parsed; } diff --git a/lib/profile/include/profile.h b/lib/profile/include/profile.h index 73fca8c..c22c8bd 100644 --- a/lib/profile/include/profile.h +++ b/lib/profile/include/profile.h @@ -14,13 +14,13 @@ struct RuntimeProfileConfig { uint8_t profileCount = PROFILE_MAX_COUNT; - float deepTargetM = TARGET_DEPTH; - float shallowTopTargetM = TARGET_SHALLOW_TOP_DEPTH; + 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 pidTimeoutS = TIMEOUT_PID_TIME; - float ascentTimeoutS = TIMEOUT_ASCENT; - float surfaceOffsetM = SURFACE_TARGET_OFFSET_M; + 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 { @@ -31,7 +31,7 @@ class ProfileManager { void beginConfig(); const RuntimeProfileConfig& config() const { return _config; } - float shallowBottomTargetM() const; + float ascentTargetBottomM() const; bool setConfig(const RuntimeProfileConfig& config); bool validateConfig(const RuntimeProfileConfig& config) const; void formatConfigJson(char* buffer, size_t bufferSize) const; diff --git a/lib/profile/src/profile.cpp b/lib/profile/src/profile.cpp index a441746..173a4bb 100644 --- a/lib/profile/src/profile.cpp +++ b/lib/profile/src/profile.cpp @@ -74,8 +74,10 @@ void ProfileManager::beginConfig() { } // --------------------------------------------------------------------------- -float ProfileManager::shallowBottomTargetM() const { - return _config.shallowTopTargetM + SENSOR_TO_BOTTOM_M + SENSOR_TO_TOP_M; +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; } // --------------------------------------------------------------------------- @@ -94,25 +96,25 @@ bool ProfileManager::setConfig(const RuntimeProfileConfig& config) { // --------------------------------------------------------------------------- bool ProfileManager::validateConfig(const RuntimeProfileConfig& config) const { - const float shallowBottomM = - config.shallowTopTargetM + SENSOR_TO_BOTTOM_M + SENSOR_TO_TOP_M; + const float ascentBottomM = + config.ascentTargetM + SENSOR_TO_BOTTOM_M + SENSOR_TO_TOP_M; return config.profileCount >= 1 && config.profileCount <= 10 && - isfinite(config.deepTargetM) && - isfinite(config.shallowTopTargetM) && + isfinite(config.descentTargetM) && + isfinite(config.ascentTargetM) && isfinite(config.depthToleranceM) && isfinite(config.holdTimeS) && - isfinite(config.pidTimeoutS) && + isfinite(config.descentTimeoutS) && isfinite(config.ascentTimeoutS) && - isfinite(config.surfaceOffsetM) && - config.deepTargetM >= 0.0f && config.deepTargetM <= 5.0f && - config.shallowTopTargetM >= 0.0f && config.shallowTopTargetM <= 5.0f && + 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.pidTimeoutS >= 5.0f && config.pidTimeoutS <= 900.0f && + config.descentTimeoutS >= 5.0f && config.descentTimeoutS <= 900.0f && config.ascentTimeoutS >= 5.0f && config.ascentTimeoutS <= 900.0f && - config.surfaceOffsetM >= 0.0f && config.surfaceOffsetM <= 5.0f && - shallowBottomM < config.deepTargetM; + config.surfaceRestOffsetM >= 0.0f && config.surfaceRestOffsetM <= 5.0f && + ascentBottomM < config.descentTargetM; } // --------------------------------------------------------------------------- @@ -121,23 +123,23 @@ void ProfileManager::formatConfigJson(char* buffer, size_t bufferSize) const { snprintf(buffer, bufferSize, "{\"profile_count\":%u," - "\"deep_target_m\":%.3f," - "\"shallow_top_m\":%.3f," - "\"shallow_bottom_m\":%.3f," + "\"descent_target_m\":%.3f," + "\"ascent_target_m\":%.3f," + "\"ascent_target_bottom_m\":%.3f," "\"depth_tolerance_m\":%.3f," "\"hold_s\":%.1f," - "\"pid_timeout_s\":%.1f," + "\"descent_timeout_s\":%.1f," "\"ascent_timeout_s\":%.1f," - "\"surface_offset_m\":%.3f}", + "\"surface_rest_offset_m\":%.3f}", static_cast(_config.profileCount), - _config.deepTargetM, - _config.shallowTopTargetM, - shallowBottomTargetM(), + _config.descentTargetM, + _config.ascentTargetM, + ascentTargetBottomM(), _config.depthToleranceM, _config.holdTimeS, - _config.pidTimeoutS, + _config.descentTimeoutS, _config.ascentTimeoutS, - _config.surfaceOffsetM); + _config.surfaceRestOffsetM); } // --------------------------------------------------------------------------- @@ -160,7 +162,7 @@ void ProfileManager::_saveConfig() { // --------------------------------------------------------------------------- void ProfileManager::_applyConfigToSubsystems() { - sensors.setSurfaceTargetOffset(_config.surfaceOffsetM); + sensors.setSurfaceTargetOffset(_config.surfaceRestOffsetM); } // --------------------------------------------------------------------------- @@ -249,8 +251,8 @@ void ProfileManager::measure(float targetDepth, float holdTimeSec, float timeout const bool isSurfaceTarget = (targetDepth == TARGET_SURFACE); const bool isBottomTarget = (targetDepth == TARGET_BOTTOM); const bool isPIDPhase = !isSurfaceTarget && !isBottomTarget; - const bool isDeepTarget = fabsf(targetDepth - _config.deepTargetM) < 0.001f; - const bool isShallowTarget = fabsf(targetDepth - shallowBottomTargetM()) < 0.001f; + const bool isDeepTarget = fabsf(targetDepth - _config.descentTargetM) < 0.001f; + const bool isShallowTarget = fabsf(targetDepth - ascentTargetBottomM()) < 0.001f; // --- LED and initial motor positioning --- if (isPIDPhase) { diff --git a/src/espA/main.cpp b/src/espA/main.cpp index c682676..58cdd2a 100644 --- a/src/espA/main.cpp +++ b/src/espA/main.cpp @@ -254,10 +254,10 @@ void loop() { 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.deepTargetM); - profileManager.measure(profileConfig.deepTargetM, + completedProfiles + 1, profileConfig.descentTargetM); + profileManager.measure(profileConfig.descentTargetM, profileConfig.holdTimeS, - profileConfig.pidTimeoutS); + profileConfig.descentTimeoutS); if (!motionController.motionAllowed()) { aborted = true; break; @@ -266,8 +266,8 @@ void loop() { delay(500); Debug.printf("Profile %d: PID ascent to %.2f m top reference\n", - completedProfiles + 1, profileConfig.shallowTopTargetM); - profileManager.measure(profileManager.shallowBottomTargetM(), + completedProfiles + 1, profileConfig.ascentTargetM); + profileManager.measure(profileManager.ascentTargetBottomM(), profileConfig.holdTimeS, profileConfig.ascentTimeoutS); if (!motionController.motionAllowed()) { @@ -524,13 +524,13 @@ void loop() { const ProfileSetPayload& payload = comms.lastCommand().payload.profileSet; RuntimeProfileConfig nextConfig; nextConfig.profileCount = payload.profileCount; - nextConfig.deepTargetM = payload.deepTargetM; - nextConfig.shallowTopTargetM = payload.shallowTopTargetM; + nextConfig.descentTargetM = payload.descentTargetM; + nextConfig.ascentTargetM = payload.ascentTargetM; nextConfig.depthToleranceM = payload.depthToleranceM; nextConfig.holdTimeS = payload.holdTimeS; - nextConfig.pidTimeoutS = payload.pidTimeoutS; + nextConfig.descentTimeoutS = payload.descentTimeoutS; nextConfig.ascentTimeoutS = payload.ascentTimeoutS; - nextConfig.surfaceOffsetM = payload.surfaceOffsetM; + nextConfig.surfaceRestOffsetM = payload.surfaceRestOffsetM; const bool updated = profileManager.setConfig(nextConfig); comms.sendMessage(updated ? CMD19_ACK : CMD19_ERR, 1000); From cf0a7994b4558004eb04f5edc5075019e8573a16 Mon Sep 17 00:00:00 2001 From: Davide-Colabella Date: Mon, 8 Jun 2026 09:04:45 +0200 Subject: [PATCH 12/19] docs: aggiorna riferimenti repo a PoliTOcean/Float Rinominata la repo da Float_2025 a Float: badge CI e comando cd nel README, URL repository in lib/DebugSerial/library.json. --- README.md | 10 +++++----- lib/DebugSerial/library.json | 2 +- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/README.md b/README.md index 9eb7c7e..f08c089 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) @@ -648,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: 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"], From 315a45d34608081852ae48a610b27b8bf0e1de02 Mon Sep 17 00:00:00 2001 From: Davide-Colabella Date: Mon, 8 Jun 2026 09:04:58 +0200 Subject: [PATCH 13/19] fix(comms): proteggi buffer ESPNOW ricevuto da race tra task La callback di ricezione ESPNOW gira nel task WiFi mentre lastCommand() viene letto dal loop principale: l'accesso non sincronizzato a _received poteva restituire una struct lacerata. - _received ora protetto da portMUX: scrittura in callback e lettura/clear in sezione critica - lastCommand() ritorna uno snapshot per valore; aggiornati i call site in espA che vi legavano un riferimento per evitare dangling sul temporaneo - sendMessage(): null-termination esplicita dopo strncpy - estratto runPidLoop() condiviso da runPidHold/runPidStep (logica TOF guard, deadband e saturazione fondo corsa ora in un solo punto) --- lib/comms/include/comms.h | 12 +++- lib/comms/src/comms.cpp | 19 ++++++ src/espA/main.cpp | 124 +++++++++++++------------------------- 3 files changed, 70 insertions(+), 85 deletions(-) diff --git a/lib/comms/include/comms.h b/lib/comms/include/comms.h index 99709a5..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 = makeOutputMessage(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 fd0bbe4..c19182a 100644 --- a/lib/comms/src/comms.cpp +++ b/lib/comms/src/comms.cpp @@ -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/src/espA/main.cpp b/src/espA/main.cpp index 58cdd2a..6223363 100644 --- a/src/espA/main.cpp +++ b/src/espA/main.cpp @@ -395,7 +395,8 @@ void loop() { // ----------------------------------------------------------------------- case CMD_PID_CONFIG_SET: { - const PidConfigPayload& payload = comms.lastCommand().payload.pidConfig; + const output_message cmd = comms.lastCommand(); + const PidConfigPayload& payload = cmd.payload.pidConfig; RuntimePidConfig nextConfig; nextConfig.kp = payload.kp; nextConfig.ki = payload.ki; @@ -475,7 +476,8 @@ void loop() { case CMD_SYRINGE_SET: // Test: posiziona siringa a u in [0,1] per N secondi { if (comms.sendMessage(CMD15_ACK, 1000)) { - const SyringeSetPayload& payload = comms.lastCommand().payload.syringeSet; + 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); @@ -488,7 +490,8 @@ void loop() { case CMD_PID_HOLD: // Test: PID a quota fissa per N secondi { if (comms.sendMessage(CMD16_ACK, 1000)) { - const PidHoldPayload& payload = comms.lastCommand().payload.pidHold; + 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); @@ -521,7 +524,8 @@ void loop() { // ----------------------------------------------------------------------- case CMD_PROFILE_SET: { - const ProfileSetPayload& payload = comms.lastCommand().payload.profileSet; + const output_message cmd = comms.lastCommand(); + const ProfileSetPayload& payload = cmd.payload.profileSet; RuntimeProfileConfig nextConfig; nextConfig.profileCount = payload.profileCount; nextConfig.descentTargetM = payload.descentTargetM; @@ -551,7 +555,8 @@ void loop() { // ----------------------------------------------------------------------- case CMD_BALANCE_CONFIG_SET: { - const BalanceConfigPayload& payload = comms.lastCommand().payload.balanceConfig; + const output_message cmd = comms.lastCommand(); + const BalanceConfigPayload& payload = cmd.payload.balanceConfig; RuntimeBalanceConfig nextConfig; nextConfig.holdMs = payload.holdMs; nextConfig.stopPressureDeltaKpa = payload.stopPressureDeltaKpa; @@ -577,7 +582,8 @@ void loop() { // ----------------------------------------------------------------------- case CMD_MOTOR_CONFIG_SET: { - const MotorConfigPayload& payload = comms.lastCommand().payload.motorConfig; + const output_message cmd = comms.lastCommand(); + const MotorConfigPayload& payload = cmd.payload.motorConfig; RuntimeMotorConfig nextConfig; nextConfig.maxSpeed = payload.maxSpeed; nextConfig.maxAcceleration = payload.maxAcceleration; @@ -754,25 +760,11 @@ 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(); @@ -785,7 +777,7 @@ static void runPidHold(float depthTarget, float durationS) { unsigned long lastLog = 0; unsigned long lastTofSampleMs = 0; bool atExtensionLimit = false; // pistone fermo al fondo corsa (tappo): non ricomandare verso l'estensione - while (millis() - t0 < (unsigned long)(durationS * 1000.0f)) { + while (millis() - t0 < durationMs) { if (Serial.available()) { Debug.println("# aborted"); break; } if (motionController.remoteStopRequested()) { Debug.println("# remote stop"); break; } @@ -820,7 +812,7 @@ static void runPidHold(float depthTarget, float durationS) { 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, @@ -835,6 +827,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) { @@ -849,59 +863,5 @@ 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)(pidController.minRetargetFrac * (float)usable); - long lastCommandedTarget = motor.position(); - - const unsigned long t0 = millis(); - unsigned long lastTick = 0; - unsigned long lastLog = 0; - unsigned long lastTofSampleMs = 0; - bool atExtensionLimit = false; // pistone fermo al fondo corsa (tappo): non ricomandare verso l'estensione - while (millis() - t0 < 60000UL) { - if (Serial.available()) { Debug.println("# aborted"); break; } - if (motionController.remoteStopRequested()) { Debug.println("# remote stop"); break; } - - // Supervisione TOF su ogni movimento (vedi runPidHold per la semantica). - 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); - const bool retreating = posTarget > motor.position(); - if ((!atExtensionLimit || retreating) && - labs(posTarget - lastCommandedTarget) >= deadbandSteps) { - motor.startMoveTo(posTarget); - lastCommandedTarget = posTarget; - atExtensionLimit = false; - } - 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(); - } - motor.stop(); - motor.disableOutputs(); - Debug.println("# done"); + runPidLoop(depthTarget, 60000UL, 100); } From d1b819b585db8e72c32977d952788f6e9e07720d Mon Sep 17 00:00:00 2001 From: Davide-Colabella Date: Tue, 9 Jun 2026 23:55:57 +0200 Subject: [PATCH 14/19] feat: added pid tuner pynb --- tools/pid_tuning/README.md | 135 +++++++ tools/pid_tuning/esempio_log.csv | 352 ++++++++++++++++++ tools/pid_tuning/pid_tuning.ipynb | 578 ++++++++++++++++++++++++++++++ tools/pid_tuning/requirements.txt | 7 + 4 files changed, 1072 insertions(+) create mode 100644 tools/pid_tuning/README.md create mode 100644 tools/pid_tuning/esempio_log.csv create mode 100644 tools/pid_tuning/pid_tuning.ipynb create mode 100644 tools/pid_tuning/requirements.txt diff --git a/tools/pid_tuning/README.md b/tools/pid_tuning/README.md new file mode 100644 index 0000000..9371462 --- /dev/null +++ b/tools/pid_tuning/README.md @@ -0,0 +1,135 @@ +# 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. + +> 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: + +``` +7289830 0.51 101 → timestamp(ms) depth(m) pressure(kPa) [syringe(u)] +``` + +Il notebook è robusto: capisce **2, 3 o 4 colonne**, con o senza unità (`m`, `kPa`), timestamp in **millisecondi**, separatori spazi/virgole/tab, e `u` anche in percentuale (`30` → `0.30`). Accetta pure il `raw` in formato **JSON**. + +> ⚠️ **La tabella "Raw chart" attuale mostra solo `Timestamp, Depth, Pressure` (niente siringa).** Con sole queste 3 colonne ottieni l'analisi della profondità e i consigli su `kp/ki/kd`, **ma non** `u_neutral` né la saturazione. Per quelli serve la colonna `u`: usa il **log flash** (opzione B) **oppure** aggiungi `syringe` alla tabella della GUI (la `u` è già nei dati `raw`, basta mostrarla). + +### 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 `;`. + +> Per i consigli su `kp/ki/kd` basta la tabella della GUI (modo A). Per **`u_neutral`** e il controllo della **saturazione** serve la colonna `u`: usa il **log flash** (modo B) o aggiungi `syringe` alla tabella "Raw chart". + +--- + +## 🔒 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_log.csv b/tools/pid_tuning/esempio_log.csv new file mode 100644 index 0000000..5285f2c --- /dev/null +++ b/tools/pid_tuning/esempio_log.csv @@ -0,0 +1,352 @@ +timestamp,depth_m,pressure_kpa,u +0.0,0.507,101.29,0.3031 +0.2,0.54,101.59,0.2903 +0.4,0.603,102.26,0.2934 +0.6,0.649,102.67,0.2872 +0.8,0.693,103.11,0.2832 +1.0,0.794,103.99,0.2603 +1.2,0.862,104.76,0.2594 +1.4,0.964,105.71,0.2566 +1.6,1.068,106.72,0.2465 +1.8,1.19,108.01,0.2377 +2.0,1.273,108.74,0.2198 +2.2,1.394,109.96,0.2176 +2.4,1.509,111.04,0.1975 +2.6,1.651,112.47,0.1877 +2.8,1.765,113.58,0.1728 +3.0,1.901,114.88,0.1589 +3.2,2.009,115.99,0.1555 +3.4,2.135,117.23,0.1411 +3.6,2.267,118.55,0.1453 +3.8,2.385,119.57,0.1304 +4.0,2.507,120.81,0.1163 +4.2,2.6,121.71,0.1049 +4.4,2.743,123.06,0.0897 +4.6,2.837,124.08,0.1021 +4.8,2.914,124.82,0.0705 +5.0,3.024,125.93,0.0715 +5.2,3.136,126.99,0.0724 +5.4,3.212,127.75,0.0748 +5.6,3.29,128.41,0.0629 +5.8,3.369,129.28,0.0603 +6.0,3.393,129.53,0.0463 +6.2,3.45,130.1,0.0453 +6.4,3.504,130.6,0.0529 +6.6,3.557,131.13,0.0426 +6.8,3.592,131.4,0.0456 +7.0,3.61,131.61,0.0437 +7.2,3.621,131.79,0.0424 +7.4,3.635,131.85,0.0282 +7.6,3.64,131.98,0.0351 +7.8,3.624,131.68,0.0453 +8.0,3.614,131.71,0.0429 +8.2,3.615,131.66,0.043 +8.4,3.581,131.31,0.0467 +8.6,3.55,131.03,0.0494 +8.8,3.519,130.82,0.0524 +9.0,3.471,130.23,0.0498 +9.2,3.419,129.72,0.0616 +9.4,3.372,129.15,0.0711 +9.6,3.297,128.57,0.0657 +9.8,3.254,128.16,0.066 +10.0,3.192,127.65,0.07 +10.2,3.128,126.89,0.0745 +10.4,3.054,126.03,0.0822 +10.6,2.982,125.41,0.0935 +10.8,2.917,124.88,0.098 +11.0,2.865,124.31,0.0869 +11.2,2.772,123.47,0.1056 +11.4,2.674,122.38,0.1131 +11.6,2.644,122.17,0.1022 +11.8,2.582,121.56,0.1148 +12.0,2.51,120.85,0.1208 +12.2,2.453,120.28,0.1305 +12.4,2.405,119.87,0.1213 +12.6,2.308,118.91,0.1327 +12.8,2.256,118.3,0.1393 +13.0,2.181,117.59,0.1425 +13.2,2.136,117.26,0.133 +13.4,2.11,116.89,0.1535 +13.6,2.057,116.47,0.1403 +13.8,2.037,116.31,0.1441 +14.0,1.994,115.71,0.1503 +14.2,1.968,115.53,0.1525 +14.4,1.929,115.25,0.157 +14.6,1.89,114.86,0.1625 +14.8,1.901,114.87,0.1554 +15.0,1.882,114.73,0.1578 +15.2,1.878,114.56,0.1559 +15.4,1.851,114.45,0.1464 +15.6,1.859,114.49,0.1537 +15.8,1.868,114.65,0.1574 +16.0,1.865,114.62,0.1626 +16.2,1.896,114.89,0.1515 +16.4,1.868,114.48,0.148 +16.6,1.922,115.1,0.1459 +16.8,1.928,115.13,0.1517 +17.0,1.957,115.45,0.1611 +17.2,1.987,115.73,0.1547 +17.4,1.995,115.88,0.1436 +17.6,2.022,116.14,0.1415 +17.8,2.085,116.74,0.1431 +18.0,2.113,116.89,0.134 +18.2,2.14,117.21,0.1445 +18.4,2.175,117.5,0.1322 +18.6,2.223,118.07,0.1276 +18.8,2.236,118.14,0.1344 +19.0,2.281,118.6,0.1347 +19.2,2.317,118.98,0.1229 +19.4,2.378,119.6,0.1307 +19.6,2.431,120.15,0.1259 +19.8,2.47,120.36,0.1247 +20.0,2.511,120.85,0.1279 +20.2,2.531,120.97,0.1298 +20.4,2.578,121.47,0.1309 +20.6,2.615,121.87,0.126 +20.8,2.646,122.14,0.1185 +21.0,2.669,122.45,0.1133 +21.2,2.698,122.64,0.109 +21.4,2.721,122.92,0.1143 +21.6,2.74,123.23,0.1027 +21.8,2.786,123.42,0.1106 +22.0,2.8,123.77,0.1088 +22.2,2.815,123.86,0.1047 +22.4,2.802,123.72,0.1107 +22.6,2.829,124.07,0.1119 +22.8,2.831,124.01,0.0996 +23.0,2.858,124.21,0.1009 +23.2,2.886,124.47,0.1094 +23.4,2.848,124.2,0.1134 +23.6,2.886,124.49,0.1081 +23.8,2.866,124.29,0.0904 +24.0,2.858,124.22,0.1068 +24.2,2.851,124.2,0.1068 +24.4,2.852,124.18,0.1058 +24.6,2.843,124.07,0.1053 +24.8,2.814,123.83,0.1057 +25.0,2.81,123.8,0.1063 +25.2,2.791,123.63,0.0995 +25.4,2.789,123.57,0.1105 +25.6,2.764,123.24,0.1029 +25.8,2.74,123.14,0.104 +26.0,2.707,122.73,0.0948 +26.2,2.718,122.82,0.1092 +26.4,2.668,122.43,0.1156 +26.6,2.658,122.33,0.1223 +26.8,2.633,122.14,0.118 +27.0,2.622,121.9,0.1215 +27.2,2.586,121.59,0.1207 +27.4,2.578,121.57,0.1209 +27.6,2.541,121.22,0.1335 +27.8,2.519,121.07,0.1197 +28.0,2.496,120.77,0.1252 +28.2,2.479,120.56,0.1138 +28.4,2.463,120.44,0.1284 +28.6,2.44,120.2,0.1275 +28.8,2.424,120.0,0.1235 +29.0,2.412,119.87,0.1175 +29.2,2.388,119.64,0.1156 +29.4,2.349,119.3,0.1209 +29.6,2.366,119.43,0.1252 +29.8,2.329,119.11,0.1369 +30.0,2.348,119.26,0.121 +30.2,2.303,118.88,0.1314 +30.4,2.294,118.77,0.1267 +30.6,2.288,118.63,0.1162 +30.8,2.296,118.76,0.119 +31.0,2.302,118.86,0.1313 +31.2,2.314,118.87,0.1345 +31.4,2.289,118.64,0.1212 +31.6,2.293,118.76,0.1276 +31.8,2.276,118.57,0.12 +32.0,2.295,118.75,0.1255 +32.2,2.292,118.74,0.1314 +32.4,2.305,118.84,0.1229 +32.6,2.279,118.6,0.1208 +32.8,2.3,118.81,0.1276 +33.0,2.309,118.88,0.1246 +33.2,2.34,119.19,0.1295 +33.4,2.334,119.13,0.1246 +33.6,2.363,119.38,0.1268 +33.8,2.348,119.24,0.1224 +34.0,2.363,119.39,0.1236 +34.2,2.389,119.65,0.127 +34.4,2.428,120.1,0.1215 +34.6,2.414,119.79,0.1297 +34.8,2.416,119.96,0.124 +35.0,2.466,120.48,0.124 +35.2,2.459,120.39,0.1273 +35.4,2.461,120.32,0.1243 +35.6,2.49,120.67,0.1147 +35.8,2.513,120.89,0.1191 +36.0,2.514,120.85,0.1202 +36.2,2.515,120.94,0.1231 +36.4,2.514,120.98,0.1298 +36.6,2.534,121.07,0.1205 +36.8,2.561,121.39,0.1144 +37.0,2.548,121.27,0.1141 +37.2,2.579,121.5,0.118 +37.4,2.582,121.57,0.1175 +37.6,2.598,121.69,0.1243 +37.8,2.614,121.91,0.1174 +38.0,2.585,121.5,0.1169 +38.2,2.62,121.87,0.1252 +38.4,2.586,121.65,0.1071 +38.6,2.602,121.74,0.1164 +38.8,2.609,121.83,0.1102 +39.0,2.596,121.71,0.1162 +39.2,2.621,121.89,0.1152 +39.4,2.618,121.99,0.1137 +39.6,2.625,121.96,0.1159 +39.8,2.607,121.79,0.111 +40.0,2.618,121.94,0.1198 +40.2,2.638,122.1,0.1125 +40.4,2.643,122.13,0.1057 +40.6,2.609,121.84,0.1179 +40.8,2.6,121.74,0.1193 +41.0,2.608,121.76,0.1059 +41.2,2.593,121.62,0.1112 +41.4,2.596,121.72,0.1137 +41.6,2.591,121.68,0.1196 +41.8,2.575,121.49,0.1095 +42.0,2.576,121.49,0.1149 +42.2,2.572,121.5,0.113 +42.4,2.579,121.54,0.1151 +42.6,2.548,121.24,0.1279 +42.8,2.553,121.28,0.1147 +43.0,2.535,121.17,0.1084 +43.2,2.539,121.17,0.1088 +43.4,2.519,120.96,0.1221 +43.6,2.496,120.79,0.1184 +43.8,2.5,120.69,0.1137 +44.0,2.485,120.7,0.122 +44.2,2.499,120.85,0.1216 +44.4,2.481,120.59,0.1163 +44.6,2.487,120.57,0.1144 +44.8,2.478,120.48,0.1221 +45.0,2.467,120.46,0.1175 +45.2,2.463,120.38,0.1204 +45.4,2.472,120.46,0.1293 +45.6,2.465,120.42,0.1166 +45.8,2.46,120.35,0.1303 +46.0,2.446,120.16,0.1225 +46.2,2.444,120.23,0.1173 +46.4,2.428,120.05,0.1095 +46.6,2.442,120.24,0.1182 +46.8,2.434,120.14,0.1179 +47.0,2.417,119.95,0.1174 +47.2,2.445,120.24,0.1205 +47.4,2.427,120.12,0.1233 +47.6,2.426,120.0,0.1357 +47.8,2.435,120.17,0.1225 +48.0,2.421,120.01,0.1089 +48.2,2.446,120.36,0.1252 +48.4,2.441,120.22,0.1229 +48.6,2.444,120.15,0.1313 +48.8,2.438,120.19,0.1006 +49.0,2.44,120.28,0.1268 +49.2,2.447,120.21,0.1196 +49.4,2.44,120.2,0.1173 +49.6,2.454,120.3,0.1214 +49.8,2.468,120.43,0.1239 +50.0,2.468,120.39,0.1199 +50.2,2.482,120.53,0.1236 +50.4,2.481,120.49,0.1228 +50.6,2.491,120.72,0.1226 +50.8,2.478,120.47,0.1196 +51.0,2.492,120.66,0.1206 +51.2,2.488,120.68,0.1208 +51.4,2.484,120.49,0.12 +51.6,2.487,120.7,0.1242 +51.8,2.492,120.76,0.1194 +52.0,2.496,120.8,0.1244 +52.2,2.504,120.76,0.1273 +52.4,2.51,120.86,0.1194 +52.6,2.524,120.96,0.1341 +52.8,2.507,120.77,0.1227 +53.0,2.523,120.97,0.1231 +53.2,2.527,121.05,0.1103 +53.4,2.504,120.77,0.1154 +53.6,2.521,120.96,0.1247 +53.8,2.523,121.06,0.1227 +54.0,2.53,121.11,0.1216 +54.2,2.535,121.22,0.1117 +54.4,2.56,121.34,0.1075 +54.6,2.539,121.17,0.1251 +54.8,2.532,121.08,0.113 +55.0,2.549,121.18,0.1128 +55.2,2.536,121.1,0.1077 +55.4,2.532,121.03,0.122 +55.6,2.526,121.01,0.117 +55.8,2.529,121.08,0.1194 +56.0,2.551,121.21,0.1296 +56.2,2.531,121.15,0.1045 +56.4,2.526,121.04,0.1192 +56.6,2.518,120.93,0.1222 +56.8,2.511,120.92,0.1212 +57.0,2.509,120.85,0.1243 +57.2,2.536,121.17,0.1221 +57.4,2.525,120.98,0.1247 +57.6,2.535,121.09,0.1147 +57.8,2.545,121.19,0.1223 +58.0,2.509,120.85,0.1149 +58.2,2.532,121.09,0.1222 +58.4,2.518,120.91,0.1278 +58.6,2.509,120.85,0.1251 +58.8,2.51,120.84,0.1163 +59.0,2.519,120.88,0.1219 +59.2,2.514,120.84,0.1209 +59.4,2.516,120.9,0.1182 +59.6,2.514,120.86,0.1279 +59.8,2.507,120.95,0.1147 +60.0,2.494,120.67,0.1272 +60.2,2.508,120.7,0.1333 +60.4,2.491,120.66,0.1231 +60.6,2.486,120.62,0.133 +60.8,2.472,120.4,0.1253 +61.0,2.504,120.8,0.1167 +61.2,2.504,120.72,0.1209 +61.4,2.467,120.47,0.1273 +61.6,2.476,120.55,0.1254 +61.8,2.492,120.66,0.1067 +62.0,2.494,120.74,0.1247 +62.2,2.453,120.32,0.1213 +62.4,2.512,120.86,0.1146 +62.6,2.481,120.55,0.1256 +62.8,2.494,120.71,0.1156 +63.0,2.473,120.46,0.1213 +63.2,2.46,120.38,0.1269 +63.4,2.472,120.54,0.1215 +63.6,2.467,120.46,0.1196 +63.8,2.485,120.51,0.1183 +64.0,2.494,120.7,0.1223 +64.2,2.476,120.5,0.1219 +64.4,2.468,120.41,0.1158 +64.6,2.473,120.53,0.1133 +64.8,2.466,120.37,0.1242 +65.0,2.487,120.64,0.1285 +65.2,2.474,120.51,0.1205 +65.4,2.463,120.41,0.1166 +65.6,2.48,120.59,0.1207 +65.8,2.495,120.74,0.1256 +66.0,2.484,120.59,0.1201 +66.2,2.485,120.52,0.1191 +66.4,2.486,120.57,0.12 +66.6,2.491,120.66,0.1232 +66.8,2.517,120.92,0.1045 +67.0,2.472,120.61,0.126 +67.2,2.465,120.44,0.1208 +67.4,2.493,120.57,0.1234 +67.6,2.508,120.83,0.1223 +67.8,2.492,120.65,0.1238 +68.0,2.503,120.67,0.1169 +68.2,2.501,120.8,0.1212 +68.4,2.492,120.71,0.1198 +68.6,2.505,120.91,0.1274 +68.8,2.494,120.74,0.1084 +69.0,2.524,121.03,0.1255 +69.2,2.499,120.79,0.1156 +69.4,2.496,120.67,0.109 +69.6,2.538,121.09,0.1314 +69.8,2.5,120.72,0.1213 +70.0,2.525,120.95,0.1194 diff --git a/tools/pid_tuning/pid_tuning.ipynb b/tools/pid_tuning/pid_tuning.ipynb new file mode 100644 index 0000000..9ec3e1f --- /dev/null +++ b/tools/pid_tuning/pid_tuning.ipynb @@ -0,0 +1,578 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# 🛟 Tuning PID del Float — tool plug-and-play\n", + "\n", + "Questo notebook ti aiuta a **tarare il controllo di profondità** del Float anche se non sei esperto di controlli automatici.\n", + "\n", + "**Cosa fa:**\n", + "1. Ti dà i **valori da inserire nei campi della GUI NEXUS** (`kp, ki, kd, ...`) e una stima di **`u_neutral`**.\n", + "2. **Analizza i dati** di un tuo test e ti dice **cosa correggere**. Puoi:\n", + " - **incollare** la tabella copiata dalla vista \"Raw chart\" della GUI (o un JSON), oppure\n", + " - **caricare un CSV** (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`). 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: righe tipo `timestamp depth pressure [syringe]`. 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" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# @title 1) Setup — esegui questa cella per prima\n", + "import sys, subprocess, io, os, json\n", + "\n", + "def _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", + "\n", + "import numpy as np\n", + "import pandas as pd\n", + "import matplotlib.pyplot as plt\n", + "\n", + "try:\n", + " import google.colab # noqa: F401\n", + " IN_COLAB = True\n", + "except 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. ----\n", + "FW = {\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", + " \"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", + "}\n", + "RAW_EXAMPLE_URL = (\"https://raw.githubusercontent.com/PoliTOcean/Float/\"\n", + " \"master/tools/pid_tuning/esempio_log.csv\")\n", + "\n", + "print(\"Ambiente:\", \"Google Colab\" if IN_COLAB else \"locale\", \"| setup OK\")\n" + ] + }, + { + "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, analisi, stima u_neutral)\n", + "\n", + "def 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", + "\n", + "def _leggi_csv(src):\n", + " try:\n", + " if isinstance(src, str):\n", + " with open(src, 'r', encoding='utf-8') as f:\n", + " content = f.read()\n", + " else:\n", + " content = src.read().decode('utf-8')\n", + " \n", + " import io\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", + " return df\n", + " except Exception:\n", + " # Fallback ultra-robusto (es. tabelle HTML salvate come file con spazi)\n", + " return _da_testo(content)\n", + "\n", + "def _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 df\n", + "\n", + "def _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", + "\n", + "def _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", + "\n", + "def _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), header, 'N/A',\n", + " timestamp in ms e 2-4 colonne. Se c'e' una riga d'intestazione, l'ordine delle\n", + " colonne viene dedotto dai nomi (Timestamp/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", + " 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", + "\n", + "def 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", + "\n", + "def 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_log.csv\", \"tools/pid_tuning/esempio_log.csv\", 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", + "\n", + "def _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", + "\n", + "def _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", + "def _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", + "\n", + "def _grafici(df, target, tol):\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", + " ax[0].axhline(target, color=\"green\", ls=\"--\", label=\"target\")\n", + " ax[0].axhspan(target - tol, target + tol, color=\"green\", alpha=0.12)\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", + "\n", + "def analizza(df, target, tolleranza_m=0.10, attuali=None):\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", + " m = _metriche(df, target, tolleranza_m)\n", + " _grafici(df, target, tolleranza_m)\n", + "\n", + " print(\"\\n=== METRICHE ===\")\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", + "\n", + " print(\"\\n=== DIAGNOSI E CONSIGLI ===\")\n", + " cons, note = dict(base), []\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", + " if m[\"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", + " 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 * tolleranza_m) 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(tolleranza_m, 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\"] < tolleranza_m and abs(m[\"e_ss\"]) < tolleranza_m\n", + " and m[\"overshoot_pct\"] < 20):\n", + " note.append(\"OK: risposta gia buona. Tieni questi valori o ritocca di poco.\")\n", + " for nn in note:\n", + " print(\" - \" + nn)\n", + "\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(metriche=m, consigliati=cons)\n", + "\n", + "def stima_u_neutral(df, target, tolleranza_m=0.10):\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", + " t = df[\"t\"].values; d = df[\"depth\"].values; u = df[\"u\"].values\n", + " vel = np.gradient(d, t)\n", + " mask = (np.abs(d - target) < max(tolleranza_m, 0.05)) & (np.abs(vel) < 0.02)\n", + " if mask.sum() < 5:\n", + " k = int(len(d) * 0.8)\n", + " mask = np.zeros(len(d), bool); mask[k:] = True\n", + " print(\"Pochi punti stabili: stima dall'ultimo tratto del log.\")\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", + "\n", + "print(\"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", + "Imposta il target e i guadagni, poi esegui per ottenere metriche e valori PID consigliati.\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "f80d902d", + "metadata": {}, + "outputs": [], + "source": [ + "# @title 3) 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.\n", + "DATI = r\"\"\"\n", + "\n", + "\"\"\"\n", + "\n", + "target_m = 2.5 # @param {type:\"number\"}\n", + "tolleranza_m = 0.10 # @param {type:\"number\"}\n", + "# (facoltativo) i parametri usati in QUESTO test, per consigli relativi:\n", + "kp_attuale = 1.7 # @param {type:\"number\"}\n", + "ki_attuale = 0.1 # @param {type:\"number\"}\n", + "kd_attuale = 0.3 # @param {type:\"number\"}\n", + "u_neutral_attuale = 0.011 # @param {type:\"number\"}\n", + "\n", + "df = carica(DATI)\n", + "# Calcola il nuovo u_neutral dai dati se disponibile\n", + "u_neutral_stimato = stima_u_neutral(df, target_m, tolleranza_m)\n", + "if u_neutral_stimato is not None:\n", + " u_neutral_attuale = u_neutral_stimato\n", + "\n", + "risultato = analizza(df, target_m, tolleranza_m,\n", + " attuali=dict(kp=kp_attuale, ki=ki_attuale, kd=kd_attuale, u_neutral=u_neutral_attuale))\n" + ] + }, + { + "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 +} 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 From acd52ae18caaeb36ce3780df4ba762511bd04ff3 Mon Sep 17 00:00:00 2001 From: Davide-Colabella Date: Wed, 10 Jun 2026 01:34:05 +0200 Subject: [PATCH 15/19] fix(tools): parsing robusto notebook PID (URL Colab, virgola decimale) e README tabella GUI a 4 colonne --- tools/pid_tuning/README.md | 9 +- tools/pid_tuning/pid_tuning.ipynb | 354 +----------------------------- 2 files changed, 7 insertions(+), 356 deletions(-) diff --git a/tools/pid_tuning/README.md b/tools/pid_tuning/README.md index 9371462..a6b8a94 100644 --- a/tools/pid_tuning/README.md +++ b/tools/pid_tuning/README.md @@ -99,12 +99,13 @@ Hai due sorgenti possibili. 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: ``` -7289830 0.51 101 → timestamp(ms) depth(m) pressure(kPa) [syringe(u)] +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 **millisecondi**, separatori spazi/virgole/tab, e `u` anche in percentuale (`30` → `0.30`). Accetta pure il `raw` in formato **JSON**. +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" attuale mostra solo `Timestamp, Depth, Pressure` (niente siringa).** Con sole queste 3 colonne ottieni l'analisi della profondità e i consigli su `kp/ki/kd`, **ma non** `u_neutral` né la saturazione. Per quelli serve la colonna `u`: usa il **log flash** (opzione B) **oppure** aggiungi `syringe` alla tabella della GUI (la `u` è già nei dati `raw`, basta mostrarla). +> 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/`): @@ -120,7 +121,7 @@ company_number, profile_id, time_s, pressure_kpa, depth_m, phase, sensor_depth_m 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 `;`. -> Per i consigli su `kp/ki/kd` basta la tabella della GUI (modo A). Per **`u_neutral`** e il controllo della **saturazione** serve la colonna `u`: usa il **log flash** (modo B) o aggiungi `syringe` alla tabella "Raw chart". +> 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`. --- diff --git a/tools/pid_tuning/pid_tuning.ipynb b/tools/pid_tuning/pid_tuning.ipynb index 9ec3e1f..0bb9dd9 100644 --- a/tools/pid_tuning/pid_tuning.ipynb +++ b/tools/pid_tuning/pid_tuning.ipynb @@ -85,357 +85,7 @@ "execution_count": null, "metadata": {}, "outputs": [], - "source": [ - "# @title 2) Funzioni (caricamento dati, analisi, stima u_neutral)\n", - "\n", - "def 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", - "\n", - "def _leggi_csv(src):\n", - " try:\n", - " if isinstance(src, str):\n", - " with open(src, 'r', encoding='utf-8') as f:\n", - " content = f.read()\n", - " else:\n", - " content = src.read().decode('utf-8')\n", - " \n", - " import io\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", - " return df\n", - " except Exception:\n", - " # Fallback ultra-robusto (es. tabelle HTML salvate come file con spazi)\n", - " return _da_testo(content)\n", - "\n", - "def _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 df\n", - "\n", - "def _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", - "\n", - "def _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", - "\n", - "def _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), header, 'N/A',\n", - " timestamp in ms e 2-4 colonne. Se c'e' una riga d'intestazione, l'ordine delle\n", - " colonne viene dedotto dai nomi (Timestamp/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", - " 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", - "\n", - "def 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", - "\n", - "def 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_log.csv\", \"tools/pid_tuning/esempio_log.csv\", 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", - "\n", - "def _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", - "\n", - "def _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", - "def _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", - "\n", - "def _grafici(df, target, tol):\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", - " ax[0].axhline(target, color=\"green\", ls=\"--\", label=\"target\")\n", - " ax[0].axhspan(target - tol, target + tol, color=\"green\", alpha=0.12)\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", - "\n", - "def analizza(df, target, tolleranza_m=0.10, attuali=None):\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", - " m = _metriche(df, target, tolleranza_m)\n", - " _grafici(df, target, tolleranza_m)\n", - "\n", - " print(\"\\n=== METRICHE ===\")\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", - "\n", - " print(\"\\n=== DIAGNOSI E CONSIGLI ===\")\n", - " cons, note = dict(base), []\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", - " if m[\"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", - " 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 * tolleranza_m) 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(tolleranza_m, 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\"] < tolleranza_m and abs(m[\"e_ss\"]) < tolleranza_m\n", - " and m[\"overshoot_pct\"] < 20):\n", - " note.append(\"OK: risposta gia buona. Tieni questi valori o ritocca di poco.\")\n", - " for nn in note:\n", - " print(\" - \" + nn)\n", - "\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(metriche=m, consigliati=cons)\n", - "\n", - "def stima_u_neutral(df, target, tolleranza_m=0.10):\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", - " t = df[\"t\"].values; d = df[\"depth\"].values; u = df[\"u\"].values\n", - " vel = np.gradient(d, t)\n", - " mask = (np.abs(d - target) < max(tolleranza_m, 0.05)) & (np.abs(vel) < 0.02)\n", - " if mask.sum() < 5:\n", - " k = int(len(d) * 0.8)\n", - " mask = np.zeros(len(d), bool); mask[k:] = True\n", - " print(\"Pochi punti stabili: stima dall'ultimo tratto del log.\")\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", - "\n", - "print(\"Funzioni pronte.\")" - ] + "source": "# @title 2) Funzioni (caricamento dati, 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 return df\n except Exception:\n # Fallback ultra-robusto (es. tabelle HTML salvate come file con spazi)\n return _da_testo(content)\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 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), header, 'N/A',\n timestamp in ms, decimali con la virgola (righe tab-separated) e 2-4 colonne.\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_log.csv\", \"tools/pid_tuning/esempio_log.csv\", 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\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, target, tol):\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 ax[0].axhline(target, color=\"green\", ls=\"--\", label=\"target\")\n ax[0].axhspan(target - tol, target + tol, color=\"green\", alpha=0.12)\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 analizza(df, target, tolleranza_m=0.10, attuali=None):\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 m = _metriche(df, target, tolleranza_m)\n _grafici(df, target, tolleranza_m)\n\n print(\"\\n=== METRICHE ===\")\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\n print(\"\\n=== DIAGNOSI E CONSIGLI ===\")\n cons, note = dict(base), []\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 if m[\"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 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 * tolleranza_m) 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(tolleranza_m, 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\"] < tolleranza_m and abs(m[\"e_ss\"]) < tolleranza_m\n and m[\"overshoot_pct\"] < 20):\n note.append(\"OK: risposta gia buona. Tieni questi valori o ritocca di poco.\")\n for nn in note:\n print(\" - \" + nn)\n\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(metriche=m, consigliati=cons)\n\ndef stima_u_neutral(df, target, tolleranza_m=0.10):\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 t = df[\"t\"].values; d = df[\"depth\"].values; u = df[\"u\"].values\n vel = np.gradient(d, t)\n mask = (np.abs(d - target) < max(tolleranza_m, 0.05)) & (np.abs(vel) < 0.02)\n if mask.sum() < 5:\n k = int(len(d) * 0.8)\n mask = np.zeros(len(d), bool); mask[k:] = True\n print(\"Pochi punti stabili: stima dall'ultimo tratto del log.\")\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", @@ -575,4 +225,4 @@ }, "nbformat": 4, "nbformat_minor": 5 -} +} \ No newline at end of file From 64cee193cba859f62cd135f7825cee18217c872a Mon Sep 17 00:00:00 2001 From: Davide-Colabella Date: Wed, 10 Jun 2026 18:00:07 +0200 Subject: [PATCH 16/19] test(espb): allinea test_parser ai nomi parametri profilo per fase --- test/unit_hw/espb_bridge/test_parser/test_parser.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) 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 1f3a778..984939f 100644 --- a/test/unit_hw/espb_bridge/test_parser/test_parser.cpp +++ b/test/unit_hw/espb_bridge/test_parser/test_parser.cpp @@ -90,13 +90,13 @@ void test_parameterized_commands_fill_payload() { 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.deepTargetM); - TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.4f, profile.message.payload.profileSet.shallowTopTargetM); + 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.pidTimeoutS); + 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.surfaceOffsetM); + TEST_ASSERT_FLOAT_WITHIN(0.001f, 0.10f, profile.message.payload.profileSet.surfaceRestOffsetM); } void test_output_message_protocol_shape() { From ca3cff4aafbc43dee28bde94d3ebfd9d914fd112 Mon Sep 17 00:00:00 2001 From: Davide-Colabella Date: Wed, 10 Jun 2026 18:02:31 +0200 Subject: [PATCH 17/19] feat(profile): valida timeout vs hold e logga inizio/uscita fase su flash Dal test in piscina del 10/06: un descent timeout di 10 s memorizzato in NVS troncava ogni fase qualunque fosse l'hold richiesto, senza alcuna traccia diagnostica (i Debug.println si perdono sott'acqua). - validateConfig richiede timeout >= hold + 30 s (discesa e risalita): una config che non lascia spazio all'hold viene rifiutata; al boot una NVS invalida torna automaticamente ai default di config.h. - measure() scrive su flash un record phase_start con hold/timeout effettivi e un record exit_remote_stop/exit_timeout/exit_hold_ok al break che chiude la fase: il dump post-missione dice quale config ha girato e perche'. --- lib/profile/src/profile.cpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/lib/profile/src/profile.cpp b/lib/profile/src/profile.cpp index 173a4bb..0795ab3 100644 --- a/lib/profile/src/profile.cpp +++ b/lib/profile/src/profile.cpp @@ -113,6 +113,11 @@ bool ProfileManager::validateConfig(const RuntimeProfileConfig& config) const { 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; } @@ -248,6 +253,14 @@ 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; @@ -287,12 +300,14 @@ 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; } @@ -414,6 +429,7 @@ void ProfileManager::measure(float targetDepth, float holdTimeSec, float timeout static_cast(holdTimeSec * 1000.0f / PERIOD_EEPROM_WRITE) + 1; if (stableCount >= requiredTicks) { Debug.println("Profile: PID hold complete — target depth sustained"); + _logProfileReading("exit_hold_ok"); break; } } else { From 2979fb561a54e2e422dd91a9889a3f73244b14dd Mon Sep 17 00:00:00 2001 From: Davide-Colabella Date: Wed, 10 Jun 2026 18:04:05 +0200 Subject: [PATCH 18/19] fix(tools): notebook PID legge il dump GUI e ricostruisce la profondita dalla pressione - _leggi_csv: i .txt salvati dalla GUI (celle con unita' tipo "0.53 m", nessun header) mandavano pandas in errore; ora ricade sul parser testo che gia' gestisce tab, unita' e virgole decimali. - la colonna depth del log di missione cambia riferimento fondo/top al cambio fase (salto fittizio ~0,5 m): se c'e' la pressione la profondita' viene ricostruita dal dato grezzo su riferimento FONDO (originale in depth_log). - stima_u_neutral esclude i campioni a galla: prima il fallback prendeva il float in superficie e stimava ~0,74 invece del neutro reale ~0,60. - esempio_log.csv sostituito da esempio_gui_dump.txt (dump GUI reale del test in piscina); fallback e URL d'esempio aggiornati. --- tools/pid_tuning/esempio_gui_dump.txt | 18 ++ tools/pid_tuning/esempio_log.csv | 352 -------------------------- tools/pid_tuning/pid_tuning.ipynb | 63 +---- 3 files changed, 21 insertions(+), 412 deletions(-) create mode 100644 tools/pid_tuning/esempio_gui_dump.txt delete mode 100644 tools/pid_tuning/esempio_log.csv 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/esempio_log.csv b/tools/pid_tuning/esempio_log.csv deleted file mode 100644 index 5285f2c..0000000 --- a/tools/pid_tuning/esempio_log.csv +++ /dev/null @@ -1,352 +0,0 @@ -timestamp,depth_m,pressure_kpa,u -0.0,0.507,101.29,0.3031 -0.2,0.54,101.59,0.2903 -0.4,0.603,102.26,0.2934 -0.6,0.649,102.67,0.2872 -0.8,0.693,103.11,0.2832 -1.0,0.794,103.99,0.2603 -1.2,0.862,104.76,0.2594 -1.4,0.964,105.71,0.2566 -1.6,1.068,106.72,0.2465 -1.8,1.19,108.01,0.2377 -2.0,1.273,108.74,0.2198 -2.2,1.394,109.96,0.2176 -2.4,1.509,111.04,0.1975 -2.6,1.651,112.47,0.1877 -2.8,1.765,113.58,0.1728 -3.0,1.901,114.88,0.1589 -3.2,2.009,115.99,0.1555 -3.4,2.135,117.23,0.1411 -3.6,2.267,118.55,0.1453 -3.8,2.385,119.57,0.1304 -4.0,2.507,120.81,0.1163 -4.2,2.6,121.71,0.1049 -4.4,2.743,123.06,0.0897 -4.6,2.837,124.08,0.1021 -4.8,2.914,124.82,0.0705 -5.0,3.024,125.93,0.0715 -5.2,3.136,126.99,0.0724 -5.4,3.212,127.75,0.0748 -5.6,3.29,128.41,0.0629 -5.8,3.369,129.28,0.0603 -6.0,3.393,129.53,0.0463 -6.2,3.45,130.1,0.0453 -6.4,3.504,130.6,0.0529 -6.6,3.557,131.13,0.0426 -6.8,3.592,131.4,0.0456 -7.0,3.61,131.61,0.0437 -7.2,3.621,131.79,0.0424 -7.4,3.635,131.85,0.0282 -7.6,3.64,131.98,0.0351 -7.8,3.624,131.68,0.0453 -8.0,3.614,131.71,0.0429 -8.2,3.615,131.66,0.043 -8.4,3.581,131.31,0.0467 -8.6,3.55,131.03,0.0494 -8.8,3.519,130.82,0.0524 -9.0,3.471,130.23,0.0498 -9.2,3.419,129.72,0.0616 -9.4,3.372,129.15,0.0711 -9.6,3.297,128.57,0.0657 -9.8,3.254,128.16,0.066 -10.0,3.192,127.65,0.07 -10.2,3.128,126.89,0.0745 -10.4,3.054,126.03,0.0822 -10.6,2.982,125.41,0.0935 -10.8,2.917,124.88,0.098 -11.0,2.865,124.31,0.0869 -11.2,2.772,123.47,0.1056 -11.4,2.674,122.38,0.1131 -11.6,2.644,122.17,0.1022 -11.8,2.582,121.56,0.1148 -12.0,2.51,120.85,0.1208 -12.2,2.453,120.28,0.1305 -12.4,2.405,119.87,0.1213 -12.6,2.308,118.91,0.1327 -12.8,2.256,118.3,0.1393 -13.0,2.181,117.59,0.1425 -13.2,2.136,117.26,0.133 -13.4,2.11,116.89,0.1535 -13.6,2.057,116.47,0.1403 -13.8,2.037,116.31,0.1441 -14.0,1.994,115.71,0.1503 -14.2,1.968,115.53,0.1525 -14.4,1.929,115.25,0.157 -14.6,1.89,114.86,0.1625 -14.8,1.901,114.87,0.1554 -15.0,1.882,114.73,0.1578 -15.2,1.878,114.56,0.1559 -15.4,1.851,114.45,0.1464 -15.6,1.859,114.49,0.1537 -15.8,1.868,114.65,0.1574 -16.0,1.865,114.62,0.1626 -16.2,1.896,114.89,0.1515 -16.4,1.868,114.48,0.148 -16.6,1.922,115.1,0.1459 -16.8,1.928,115.13,0.1517 -17.0,1.957,115.45,0.1611 -17.2,1.987,115.73,0.1547 -17.4,1.995,115.88,0.1436 -17.6,2.022,116.14,0.1415 -17.8,2.085,116.74,0.1431 -18.0,2.113,116.89,0.134 -18.2,2.14,117.21,0.1445 -18.4,2.175,117.5,0.1322 -18.6,2.223,118.07,0.1276 -18.8,2.236,118.14,0.1344 -19.0,2.281,118.6,0.1347 -19.2,2.317,118.98,0.1229 -19.4,2.378,119.6,0.1307 -19.6,2.431,120.15,0.1259 -19.8,2.47,120.36,0.1247 -20.0,2.511,120.85,0.1279 -20.2,2.531,120.97,0.1298 -20.4,2.578,121.47,0.1309 -20.6,2.615,121.87,0.126 -20.8,2.646,122.14,0.1185 -21.0,2.669,122.45,0.1133 -21.2,2.698,122.64,0.109 -21.4,2.721,122.92,0.1143 -21.6,2.74,123.23,0.1027 -21.8,2.786,123.42,0.1106 -22.0,2.8,123.77,0.1088 -22.2,2.815,123.86,0.1047 -22.4,2.802,123.72,0.1107 -22.6,2.829,124.07,0.1119 -22.8,2.831,124.01,0.0996 -23.0,2.858,124.21,0.1009 -23.2,2.886,124.47,0.1094 -23.4,2.848,124.2,0.1134 -23.6,2.886,124.49,0.1081 -23.8,2.866,124.29,0.0904 -24.0,2.858,124.22,0.1068 -24.2,2.851,124.2,0.1068 -24.4,2.852,124.18,0.1058 -24.6,2.843,124.07,0.1053 -24.8,2.814,123.83,0.1057 -25.0,2.81,123.8,0.1063 -25.2,2.791,123.63,0.0995 -25.4,2.789,123.57,0.1105 -25.6,2.764,123.24,0.1029 -25.8,2.74,123.14,0.104 -26.0,2.707,122.73,0.0948 -26.2,2.718,122.82,0.1092 -26.4,2.668,122.43,0.1156 -26.6,2.658,122.33,0.1223 -26.8,2.633,122.14,0.118 -27.0,2.622,121.9,0.1215 -27.2,2.586,121.59,0.1207 -27.4,2.578,121.57,0.1209 -27.6,2.541,121.22,0.1335 -27.8,2.519,121.07,0.1197 -28.0,2.496,120.77,0.1252 -28.2,2.479,120.56,0.1138 -28.4,2.463,120.44,0.1284 -28.6,2.44,120.2,0.1275 -28.8,2.424,120.0,0.1235 -29.0,2.412,119.87,0.1175 -29.2,2.388,119.64,0.1156 -29.4,2.349,119.3,0.1209 -29.6,2.366,119.43,0.1252 -29.8,2.329,119.11,0.1369 -30.0,2.348,119.26,0.121 -30.2,2.303,118.88,0.1314 -30.4,2.294,118.77,0.1267 -30.6,2.288,118.63,0.1162 -30.8,2.296,118.76,0.119 -31.0,2.302,118.86,0.1313 -31.2,2.314,118.87,0.1345 -31.4,2.289,118.64,0.1212 -31.6,2.293,118.76,0.1276 -31.8,2.276,118.57,0.12 -32.0,2.295,118.75,0.1255 -32.2,2.292,118.74,0.1314 -32.4,2.305,118.84,0.1229 -32.6,2.279,118.6,0.1208 -32.8,2.3,118.81,0.1276 -33.0,2.309,118.88,0.1246 -33.2,2.34,119.19,0.1295 -33.4,2.334,119.13,0.1246 -33.6,2.363,119.38,0.1268 -33.8,2.348,119.24,0.1224 -34.0,2.363,119.39,0.1236 -34.2,2.389,119.65,0.127 -34.4,2.428,120.1,0.1215 -34.6,2.414,119.79,0.1297 -34.8,2.416,119.96,0.124 -35.0,2.466,120.48,0.124 -35.2,2.459,120.39,0.1273 -35.4,2.461,120.32,0.1243 -35.6,2.49,120.67,0.1147 -35.8,2.513,120.89,0.1191 -36.0,2.514,120.85,0.1202 -36.2,2.515,120.94,0.1231 -36.4,2.514,120.98,0.1298 -36.6,2.534,121.07,0.1205 -36.8,2.561,121.39,0.1144 -37.0,2.548,121.27,0.1141 -37.2,2.579,121.5,0.118 -37.4,2.582,121.57,0.1175 -37.6,2.598,121.69,0.1243 -37.8,2.614,121.91,0.1174 -38.0,2.585,121.5,0.1169 -38.2,2.62,121.87,0.1252 -38.4,2.586,121.65,0.1071 -38.6,2.602,121.74,0.1164 -38.8,2.609,121.83,0.1102 -39.0,2.596,121.71,0.1162 -39.2,2.621,121.89,0.1152 -39.4,2.618,121.99,0.1137 -39.6,2.625,121.96,0.1159 -39.8,2.607,121.79,0.111 -40.0,2.618,121.94,0.1198 -40.2,2.638,122.1,0.1125 -40.4,2.643,122.13,0.1057 -40.6,2.609,121.84,0.1179 -40.8,2.6,121.74,0.1193 -41.0,2.608,121.76,0.1059 -41.2,2.593,121.62,0.1112 -41.4,2.596,121.72,0.1137 -41.6,2.591,121.68,0.1196 -41.8,2.575,121.49,0.1095 -42.0,2.576,121.49,0.1149 -42.2,2.572,121.5,0.113 -42.4,2.579,121.54,0.1151 -42.6,2.548,121.24,0.1279 -42.8,2.553,121.28,0.1147 -43.0,2.535,121.17,0.1084 -43.2,2.539,121.17,0.1088 -43.4,2.519,120.96,0.1221 -43.6,2.496,120.79,0.1184 -43.8,2.5,120.69,0.1137 -44.0,2.485,120.7,0.122 -44.2,2.499,120.85,0.1216 -44.4,2.481,120.59,0.1163 -44.6,2.487,120.57,0.1144 -44.8,2.478,120.48,0.1221 -45.0,2.467,120.46,0.1175 -45.2,2.463,120.38,0.1204 -45.4,2.472,120.46,0.1293 -45.6,2.465,120.42,0.1166 -45.8,2.46,120.35,0.1303 -46.0,2.446,120.16,0.1225 -46.2,2.444,120.23,0.1173 -46.4,2.428,120.05,0.1095 -46.6,2.442,120.24,0.1182 -46.8,2.434,120.14,0.1179 -47.0,2.417,119.95,0.1174 -47.2,2.445,120.24,0.1205 -47.4,2.427,120.12,0.1233 -47.6,2.426,120.0,0.1357 -47.8,2.435,120.17,0.1225 -48.0,2.421,120.01,0.1089 -48.2,2.446,120.36,0.1252 -48.4,2.441,120.22,0.1229 -48.6,2.444,120.15,0.1313 -48.8,2.438,120.19,0.1006 -49.0,2.44,120.28,0.1268 -49.2,2.447,120.21,0.1196 -49.4,2.44,120.2,0.1173 -49.6,2.454,120.3,0.1214 -49.8,2.468,120.43,0.1239 -50.0,2.468,120.39,0.1199 -50.2,2.482,120.53,0.1236 -50.4,2.481,120.49,0.1228 -50.6,2.491,120.72,0.1226 -50.8,2.478,120.47,0.1196 -51.0,2.492,120.66,0.1206 -51.2,2.488,120.68,0.1208 -51.4,2.484,120.49,0.12 -51.6,2.487,120.7,0.1242 -51.8,2.492,120.76,0.1194 -52.0,2.496,120.8,0.1244 -52.2,2.504,120.76,0.1273 -52.4,2.51,120.86,0.1194 -52.6,2.524,120.96,0.1341 -52.8,2.507,120.77,0.1227 -53.0,2.523,120.97,0.1231 -53.2,2.527,121.05,0.1103 -53.4,2.504,120.77,0.1154 -53.6,2.521,120.96,0.1247 -53.8,2.523,121.06,0.1227 -54.0,2.53,121.11,0.1216 -54.2,2.535,121.22,0.1117 -54.4,2.56,121.34,0.1075 -54.6,2.539,121.17,0.1251 -54.8,2.532,121.08,0.113 -55.0,2.549,121.18,0.1128 -55.2,2.536,121.1,0.1077 -55.4,2.532,121.03,0.122 -55.6,2.526,121.01,0.117 -55.8,2.529,121.08,0.1194 -56.0,2.551,121.21,0.1296 -56.2,2.531,121.15,0.1045 -56.4,2.526,121.04,0.1192 -56.6,2.518,120.93,0.1222 -56.8,2.511,120.92,0.1212 -57.0,2.509,120.85,0.1243 -57.2,2.536,121.17,0.1221 -57.4,2.525,120.98,0.1247 -57.6,2.535,121.09,0.1147 -57.8,2.545,121.19,0.1223 -58.0,2.509,120.85,0.1149 -58.2,2.532,121.09,0.1222 -58.4,2.518,120.91,0.1278 -58.6,2.509,120.85,0.1251 -58.8,2.51,120.84,0.1163 -59.0,2.519,120.88,0.1219 -59.2,2.514,120.84,0.1209 -59.4,2.516,120.9,0.1182 -59.6,2.514,120.86,0.1279 -59.8,2.507,120.95,0.1147 -60.0,2.494,120.67,0.1272 -60.2,2.508,120.7,0.1333 -60.4,2.491,120.66,0.1231 -60.6,2.486,120.62,0.133 -60.8,2.472,120.4,0.1253 -61.0,2.504,120.8,0.1167 -61.2,2.504,120.72,0.1209 -61.4,2.467,120.47,0.1273 -61.6,2.476,120.55,0.1254 -61.8,2.492,120.66,0.1067 -62.0,2.494,120.74,0.1247 -62.2,2.453,120.32,0.1213 -62.4,2.512,120.86,0.1146 -62.6,2.481,120.55,0.1256 -62.8,2.494,120.71,0.1156 -63.0,2.473,120.46,0.1213 -63.2,2.46,120.38,0.1269 -63.4,2.472,120.54,0.1215 -63.6,2.467,120.46,0.1196 -63.8,2.485,120.51,0.1183 -64.0,2.494,120.7,0.1223 -64.2,2.476,120.5,0.1219 -64.4,2.468,120.41,0.1158 -64.6,2.473,120.53,0.1133 -64.8,2.466,120.37,0.1242 -65.0,2.487,120.64,0.1285 -65.2,2.474,120.51,0.1205 -65.4,2.463,120.41,0.1166 -65.6,2.48,120.59,0.1207 -65.8,2.495,120.74,0.1256 -66.0,2.484,120.59,0.1201 -66.2,2.485,120.52,0.1191 -66.4,2.486,120.57,0.12 -66.6,2.491,120.66,0.1232 -66.8,2.517,120.92,0.1045 -67.0,2.472,120.61,0.126 -67.2,2.465,120.44,0.1208 -67.4,2.493,120.57,0.1234 -67.6,2.508,120.83,0.1223 -67.8,2.492,120.65,0.1238 -68.0,2.503,120.67,0.1169 -68.2,2.501,120.8,0.1212 -68.4,2.492,120.71,0.1198 -68.6,2.505,120.91,0.1274 -68.8,2.494,120.74,0.1084 -69.0,2.524,121.03,0.1255 -69.2,2.499,120.79,0.1156 -69.4,2.496,120.67,0.109 -69.6,2.538,121.09,0.1314 -69.8,2.5,120.72,0.1213 -70.0,2.525,120.95,0.1194 diff --git a/tools/pid_tuning/pid_tuning.ipynb b/tools/pid_tuning/pid_tuning.ipynb index 0bb9dd9..623169f 100644 --- a/tools/pid_tuning/pid_tuning.ipynb +++ b/tools/pid_tuning/pid_tuning.ipynb @@ -3,71 +3,14 @@ { "cell_type": "markdown", "metadata": {}, - "source": [ - "# 🛟 Tuning PID del Float — tool plug-and-play\n", - "\n", - "Questo notebook ti aiuta a **tarare il controllo di profondità** del Float anche se non sei esperto di controlli automatici.\n", - "\n", - "**Cosa fa:**\n", - "1. Ti dà i **valori da inserire nei campi della GUI NEXUS** (`kp, ki, kd, ...`) e una stima di **`u_neutral`**.\n", - "2. **Analizza i dati** di un tuo test e ti dice **cosa correggere**. Puoi:\n", - " - **incollare** la tabella copiata dalla vista \"Raw chart\" della GUI (o un JSON), oppure\n", - " - **caricare un CSV** (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`). 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: righe tipo `timestamp depth pressure [syringe]`. 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" - ] + "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**. 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`). 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" }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], - "source": [ - "# @title 1) Setup — esegui questa cella per prima\n", - "import sys, subprocess, io, os, json\n", - "\n", - "def _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", - "\n", - "import numpy as np\n", - "import pandas as pd\n", - "import matplotlib.pyplot as plt\n", - "\n", - "try:\n", - " import google.colab # noqa: F401\n", - " IN_COLAB = True\n", - "except 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. ----\n", - "FW = {\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", - " \"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", - "}\n", - "RAW_EXAMPLE_URL = (\"https://raw.githubusercontent.com/PoliTOcean/Float/\"\n", - " \"master/tools/pid_tuning/esempio_log.csv\")\n", - "\n", - "print(\"Ambiente:\", \"Google Colab\" if IN_COLAB else \"locale\", \"| setup OK\")\n" - ] + "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 \"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", @@ -85,7 +28,7 @@ "execution_count": null, "metadata": {}, "outputs": [], - "source": "# @title 2) Funzioni (caricamento dati, 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 return df\n except Exception:\n # Fallback ultra-robusto (es. tabelle HTML salvate come file con spazi)\n return _da_testo(content)\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 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), header, 'N/A',\n timestamp in ms, decimali con la virgola (righe tab-separated) e 2-4 colonne.\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_log.csv\", \"tools/pid_tuning/esempio_log.csv\", 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\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, target, tol):\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 ax[0].axhline(target, color=\"green\", ls=\"--\", label=\"target\")\n ax[0].axhspan(target - tol, target + tol, color=\"green\", alpha=0.12)\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 analizza(df, target, tolleranza_m=0.10, attuali=None):\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 m = _metriche(df, target, tolleranza_m)\n _grafici(df, target, tolleranza_m)\n\n print(\"\\n=== METRICHE ===\")\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\n print(\"\\n=== DIAGNOSI E CONSIGLI ===\")\n cons, note = dict(base), []\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 if m[\"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 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 * tolleranza_m) 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(tolleranza_m, 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\"] < tolleranza_m and abs(m[\"e_ss\"]) < tolleranza_m\n and m[\"overshoot_pct\"] < 20):\n note.append(\"OK: risposta gia buona. Tieni questi valori o ritocca di poco.\")\n for nn in note:\n print(\" - \" + nn)\n\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(metriche=m, consigliati=cons)\n\ndef stima_u_neutral(df, target, tolleranza_m=0.10):\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 t = df[\"t\"].values; d = df[\"depth\"].values; u = df[\"u\"].values\n vel = np.gradient(d, t)\n mask = (np.abs(d - target) < max(tolleranza_m, 0.05)) & (np.abs(vel) < 0.02)\n if mask.sum() < 5:\n k = int(len(d) * 0.8)\n mask = np.zeros(len(d), bool); mask[k:] = True\n print(\"Pochi punti stabili: stima dall'ultimo tratto del log.\")\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.\")" + "source": "# @title 2) Funzioni (caricamento dati, 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\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, target, tol):\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 ax[0].axhline(target, color=\"green\", ls=\"--\", label=\"target\")\n ax[0].axhspan(target - tol, target + tol, color=\"green\", alpha=0.12)\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 analizza(df, target, tolleranza_m=0.10, attuali=None):\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 m = _metriche(df, target, tolleranza_m)\n _grafici(df, target, tolleranza_m)\n\n print(\"\\n=== METRICHE ===\")\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\n print(\"\\n=== DIAGNOSI E CONSIGLI ===\")\n cons, note = dict(base), []\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 if m[\"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 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 * tolleranza_m) 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(tolleranza_m, 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\"] < tolleranza_m and abs(m[\"e_ss\"]) < tolleranza_m\n and m[\"overshoot_pct\"] < 20):\n note.append(\"OK: risposta gia buona. Tieni questi valori o ritocca di poco.\")\n for nn in note:\n print(\" - \" + nn)\n\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(metriche=m, consigliati=cons)\n\ndef stima_u_neutral(df, target, tolleranza_m=0.10):\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 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 mask = sommerso & fermo & (np.abs(d - target) < max(tolleranza_m, 0.05))\n if mask.sum() < 5 and (sommerso & fermo).sum() >= 2:\n mask = sommerso & fermo\n print(\"Pochi punti stabili al 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", From 711a521a7bec31648a8123a755a441f24a9680f3 Mon Sep 17 00:00:00 2001 From: Davide-Colabella Date: Thu, 11 Jun 2026 03:02:29 +0200 Subject: [PATCH 19/19] feat(tools): notebook PID analizza discesa e salita con riconoscimento automatico delle fasi Il notebook segmenta il log da solo (colonna phase del firmware o forma della traiettoria): solo discesa, solo salita o profilo completo, con metriche e diagnosi per fase e fase forzabile da parametro. Due target come nella GUI (discesa rif. FONDO, salita rif. TOP convertito +0,51 m come ascentTargetBottomM), u_neutral stimato su entrambe le quote, riemersione finale esclusa dalla salita e consigli PID combinati in un unico set (kp prudente, kd/ki incisivi) come richiede il firmware. --- tools/pid_tuning/README.md | 1 + tools/pid_tuning/pid_tuning.ipynb | 42 ++++--------------------------- 2 files changed, 6 insertions(+), 37 deletions(-) diff --git a/tools/pid_tuning/README.md b/tools/pid_tuning/README.md index a6b8a94..9865861 100644 --- a/tools/pid_tuning/README.md +++ b/tools/pid_tuning/README.md @@ -18,6 +18,7 @@ Due cose: - **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`. diff --git a/tools/pid_tuning/pid_tuning.ipynb b/tools/pid_tuning/pid_tuning.ipynb index 623169f..b5c4ddc 100644 --- a/tools/pid_tuning/pid_tuning.ipynb +++ b/tools/pid_tuning/pid_tuning.ipynb @@ -3,14 +3,14 @@ { "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**. 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`). 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" + "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 \"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\")" + "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", @@ -28,7 +28,7 @@ "execution_count": null, "metadata": {}, "outputs": [], - "source": "# @title 2) Funzioni (caricamento dati, 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\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, target, tol):\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 ax[0].axhline(target, color=\"green\", ls=\"--\", label=\"target\")\n ax[0].axhspan(target - tol, target + tol, color=\"green\", alpha=0.12)\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 analizza(df, target, tolleranza_m=0.10, attuali=None):\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 m = _metriche(df, target, tolleranza_m)\n _grafici(df, target, tolleranza_m)\n\n print(\"\\n=== METRICHE ===\")\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\n print(\"\\n=== DIAGNOSI E CONSIGLI ===\")\n cons, note = dict(base), []\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 if m[\"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 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 * tolleranza_m) 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(tolleranza_m, 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\"] < tolleranza_m and abs(m[\"e_ss\"]) < tolleranza_m\n and m[\"overshoot_pct\"] < 20):\n note.append(\"OK: risposta gia buona. Tieni questi valori o ritocca di poco.\")\n for nn in note:\n print(\" - \" + nn)\n\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(metriche=m, consigliati=cons)\n\ndef stima_u_neutral(df, target, tolleranza_m=0.10):\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 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 mask = sommerso & fermo & (np.abs(d - target) < max(tolleranza_m, 0.05))\n if mask.sum() < 5 and (sommerso & fermo).sum() >= 2:\n mask = sommerso & fermo\n print(\"Pochi punti stabili al 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.\")" + "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", @@ -54,15 +54,7 @@ "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", - "Imposta il target e i guadagni, poi esegui per ottenere metriche e valori PID consigliati.\n" - ] + "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", @@ -70,31 +62,7 @@ "id": "f80d902d", "metadata": {}, "outputs": [], - "source": [ - "# @title 3) 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.\n", - "DATI = r\"\"\"\n", - "\n", - "\"\"\"\n", - "\n", - "target_m = 2.5 # @param {type:\"number\"}\n", - "tolleranza_m = 0.10 # @param {type:\"number\"}\n", - "# (facoltativo) i parametri usati in QUESTO test, per consigli relativi:\n", - "kp_attuale = 1.7 # @param {type:\"number\"}\n", - "ki_attuale = 0.1 # @param {type:\"number\"}\n", - "kd_attuale = 0.3 # @param {type:\"number\"}\n", - "u_neutral_attuale = 0.011 # @param {type:\"number\"}\n", - "\n", - "df = carica(DATI)\n", - "# Calcola il nuovo u_neutral dai dati se disponibile\n", - "u_neutral_stimato = stima_u_neutral(df, target_m, tolleranza_m)\n", - "if u_neutral_stimato is not None:\n", - " u_neutral_attuale = u_neutral_stimato\n", - "\n", - "risultato = analizza(df, target_m, tolleranza_m,\n", - " attuali=dict(kp=kp_attuale, ki=ki_attuale, kd=kd_attuale, u_neutral=u_neutral_attuale))\n" - ] + "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",