From cfca629b20d5bfa936f212f0959f08b7a6a3cf3e Mon Sep 17 00:00:00 2001 From: Skeitt Date: Fri, 29 May 2026 22:57:27 +0200 Subject: [PATCH 01/22] 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/22] 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/22] 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/22] 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/22] 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/22] 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/22] 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/22] 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/22] 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/22] 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/22] 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/22] 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/22] 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/22] 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/22] 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/22] 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/22] 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/22] 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/22] 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", From 98ff8d379999a3072e2ef6aacb366d661b9068e4 Mon Sep 17 00:00:00 2001 From: Davide-Colabella Date: Tue, 16 Jun 2026 23:43:29 +0200 Subject: [PATCH 20/22] =?UTF-8?q?tune(config):=20kick=20discesa=200.15,=20?= =?UTF-8?q?velocit=C3=A0=20motore=201400,=20MAC=20ESPA?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - PID_DESCENT_KICK_U 0.30 → 0.15: con 0.30 + spinta del PID (Kp*error) la discesa partiva troppo veloce e sfondava il target di oltre 1 m - velocità/accelerazione/homing motore 1800 → 1400 steps/s - aggiornato MAC_ESPA alla nuova scheda --- include/config.h | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/include/config.h b/include/config.h index f845c19..d70d246 100644 --- a/include/config.h +++ b/include/config.h @@ -45,9 +45,9 @@ constexpr float MOTOR_STEPS_PER_MM = constexpr float MOTOR_TRAVEL_MM = 35.0f; // Normal commanded syringe travel (mm) constexpr uint32_t MOTOR_MAX_STEPS = static_cast(MOTOR_TRAVEL_MM * MOTOR_STEPS_PER_MM + 0.5f); -constexpr uint32_t MOTOR_MAX_SPEED = 1800; // Normal operating speed (steps/s) -constexpr uint32_t MOTOR_MAX_ACCELERATION = 1800; // Normal acceleration/deceleration (steps/s^2) -constexpr uint32_t MOTOR_HOMING_SPEED = 1800; // Homing speed (steps/s) +constexpr uint32_t MOTOR_MAX_SPEED = 1400; // Normal operating speed (steps/s) +constexpr uint32_t MOTOR_MAX_ACCELERATION = 1400; // Normal acceleration/deceleration (steps/s^2) +constexpr uint32_t MOTOR_HOMING_SPEED = 1400; // Homing speed (steps/s) constexpr uint16_t MOTOR_ENDSTOP_MARGIN = 10; // Safety margin from endstops (steps) // Geometria reale (verificata col balance, coerente con l'homing): @@ -158,8 +158,9 @@ constexpr float PID_MIN_RETARGET_FRAC = 0.001f;// dead-band ri-comando (fraz // 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; +// vasca). Ridotto a 0.30, poi a 0.15: con 0.30 + spinta del PID (Kp*error) la +// discesa partiva ancora troppo veloce e si sfondava il target di oltre 1 m. +constexpr float PID_DESCENT_KICK_U = 0.15f; // --------------------------------------------------------------------------- // FLOAT PHYSICAL / MISSION CONSTANTS @@ -205,7 +206,7 @@ constexpr char WIFI_PASSWORD[] = "politocean"; // constexpr uint8_t MAC_ESPB[6] = {0xEC, 0xE3, 0x34, 0xCE, 0x59, 0x1C}; constexpr uint8_t MAC_ESPB[6] = {0x88, 0x57, 0x21, 0x84, 0x8C, 0xE8}; -constexpr uint8_t MAC_ESPA[6] = {0x88, 0x57, 0x21, 0x84, 0x83, 0x8C}; +constexpr uint8_t MAC_ESPA[6] = {0x88, 0x57, 0x21, 0x84, 0x7E, 0xCC}; constexpr uint8_t ESPNOW_CHANNEL = 1; // --------------------------------------------------------------------------- From 29e6b9082c9c8f6c0a7bddf9f4d801fa7220bd41 Mon Sep 17 00:00:00 2001 From: Davide-Colabella Date: Mon, 22 Jun 2026 20:55:15 +0200 Subject: [PATCH 21/22] feat(float): quota da cima, sosta finale e simulatore PID da banco MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Profondità riportata sempre riferita alla CIMA del float, così parte da ~0 in superficie; il controllo resta sul FONDO e il PID porta comunque il fondo a 2.5 m e la cima a 40 cm. Sosta finale dopo i due profili: tiene la cima a 0.15 m sotto il pelo (antenna sommersa) fino al recupero. FLOAT_LENGTH 0.49, PROFILE_CONFIG_VERSION 2 per ricaricare i default. Simulatore HIL da banco (SIM_ON): il barometro e' sostituito da un modello spinta+drag mosso dalla siringa reale, cosi' PID_STEP/PID_HOLD/GO girano a secco col motore che si muove davvero. Comandi seriali SIM_ON/SIM_OFF/SIM_GET/ SIM_CONFIG e GO (missione completa da seriale per test end-to-end al banco); missione fattorizzata in runVerticalProfiles()/attemptAutoRecovery(). --- README.md | 33 +++++++ include/config.h | 59 +++++++++--- lib/profile/include/profile.h | 3 + lib/profile/src/profile.cpp | 34 +++++-- lib/sensors/include/sensors.h | 24 +++++ lib/sensors/src/sensors.cpp | 81 +++++++++++++++- src/espA/main.cpp | 174 ++++++++++++++++++++++++---------- 7 files changed, 331 insertions(+), 77 deletions(-) diff --git a/README.md b/README.md index f08c089..1cc4bc3 100644 --- a/README.md +++ b/README.md @@ -679,6 +679,39 @@ All commands in the FLOAT Commands table can be sent over the ESPB USB serial br | `PID_HOLD ` | Hold PID at `depth_m` for `dur_s` seconds, log at 5 Hz (command 16) | | `PID_STEP ` | Step response: PID at `depth_m` for up to 60 s, log at 10 Hz (command 17) | | `SURFACE_OFFSET ` | Set the surface target offset (`SURFACE_TARGET_OFFSET_M`) at runtime (command 18) | +| `SIM_ON` / `SIM_OFF` | Enable/disable the **bench barometer simulator** (the motor still moves for real) | +| `SIM_GET` | Print simulator state and physics parameters | +| `SIM_CONFIG ` | Retune the simulator physics at runtime | +| `GO` | Run the full mission (all profiles: descent+hold+ascent+hold, then surface rest) straight from serial, no GUI/ESPB — with `SIM_ON` this is a dry end-to-end bench test (blocking; reset ESPA to abort) | + +#### Bench PID Tuning with the Simulator (dry, no water) + +`SIM_ON` replaces the Bar02 depth reading with an on-board second-order physics +model (buoyancy + quadratic drag) driven by the **real syringe position**, so the +motor moves exactly as in the water but depth is synthetic. Because `sensors.depth()` +is overridden transparently, `PID_STEP`, `PID_HOLD`, and a full `GO` mission all run +against the simulated water column with no other changes — let you tune the PID at a +desk. + +Model: `a = accelGain·(u − uNeutral) − dragQuad·v·|v|`, where `u = motorPosToU(motor.position())` +(`u > uNeutral` ⇒ sinks). Defaults (`config.h` `SIM_*`) reproduce the real float's +slow actuator (~2 mm/s) and downward overshoot; **set `uNeutral` to your float's +real neutral** (the steady-state `u` you observe in a `PID_HOLD` run) for faithful tuning. + +Typical loop (over ESPA USB serial, 115200 baud): + +```text +SIM_ON # enable simulator (motor still moves) +SIM_CONFIG 0.35 0.05 1.5 3.0 # optional: match your float's physics +PID_CONFIG_SET 0.5 0.1 3.0 50 0.25 5 0.001 0.011 +PID_STEP 2.5 # watch the CSV: t_ms,depth,target,error,u,motor_pos +PID_CONFIG_SET 0.5 0.1 6.0 50 0.25 5 0.001 0.011 # e.g. raise Kd to cut overshoot +PID_STEP 2.5 # compare +SIM_OFF # back to the real barometer +``` + +The same serial CSV feeds `tools/pid_tuning/pid_tuning.ipynb` for overshoot/settling +metrics and gain suggestions. ### CLI Tests diff --git a/include/config.h b/include/config.h index d70d246..846e496 100644 --- a/include/config.h +++ b/include/config.h @@ -42,7 +42,7 @@ constexpr float MOTOR_REVS_PER_MM = constexpr float MOTOR_STEPS_PER_MM = MOTOR_STEPS_PER_REV * MOTOR_MICROSTEP * MOTOR_REVS_PER_MM; -constexpr float MOTOR_TRAVEL_MM = 35.0f; // Normal commanded syringe travel (mm) +constexpr float MOTOR_TRAVEL_MM = 45.0f; // Normal commanded syringe travel (mm) constexpr uint32_t MOTOR_MAX_STEPS = static_cast(MOTOR_TRAVEL_MM * MOTOR_STEPS_PER_MM + 0.5f); constexpr uint32_t MOTOR_MAX_SPEED = 1400; // Normal operating speed (steps/s) @@ -145,12 +145,18 @@ constexpr uint16_t PID_PERIOD_DEFAULT_MS = 50; // Default tick PID (ms) // - Kp=2.0 Kd=0.13: il float si muoveva ma OSCILLAVA (±15cm, pompaggio) — // Kp troppo alto e Kd insufficiente per un sistema lento come il float. // - Kp=1.0 Kd=0.5: smorzato ma si "sedeva" in superficie (ripresa debole). -// - Kp=1.7 Ki=0.1 Kd=0.3 (attuale): converge sul target con oscillazione -// finale ±1cm. Il Ki vince l'offset di galleggiamento (ripresa), Kd smorza. +// - Kp=1.7 Ki=0.1 Kd=0.3: converge sul target con oscillazione finale ±1cm. +// - Kp=1.0 Kd=2.0 (dump 2026-06-19): in discesa il float sfonda il target di +// ~0.7 m (picco 2.22 m con target 1.5). Dal log ad alta risoluzione il +// problema NON è u saturo a fondo corsa (u sale gradualmente, max ~0.93, poi +// scende da solo): è INERZIA idrodinamica — u è già a 0 ma il float continua +// ad affondare per il momento accumulato. Serve frenare PRIMA, quindi più Kd. +// - Kp=0.5 Kd=3.0 (attuale): meno spinta proporzionale in discesa + freno +// derivativo più deciso, per anticipare l'arresto e contenere l'overshoot. // Affinare ancora a runtime con PID_CONFIG_SET se serve. -constexpr float PID_KP_DEFAULT = 1.7f; // frazione_corsa / m +constexpr float PID_KP_DEFAULT = 0.5f; // frazione_corsa / m constexpr float PID_KI_DEFAULT = 0.1f; // frazione_corsa / (m·s) -constexpr float PID_KD_DEFAULT = 0.3f; // frazione_corsa / (m/s) +constexpr float PID_KD_DEFAULT = 3.0f; // frazione_corsa / (m/s) constexpr float PID_INTEGRAL_LIMIT = 5.0f; // m·s (bound conservativo) constexpr float PID_ALPHA_D_DEFAULT = 0.25f; // LPF IIR coeff per derivata constexpr float PID_U_NEUTRAL = 0.011f;// kick-start offset (~500/47100) @@ -165,16 +171,21 @@ constexpr float PID_DESCENT_KICK_U = 0.15f; // --------------------------------------------------------------------------- // FLOAT PHYSICAL / MISSION CONSTANTS // --------------------------------------------------------------------------- -constexpr float FLOAT_LENGTH = 0.51f; // Bottom-to-sensor height (m) +constexpr float FLOAT_LENGTH = 0.49f; // Bottom-to-sensor height (m): corpo 48 cm + barometro 1 cm sopra il tappo constexpr float SENSOR_TO_BOTTOM_M = FLOAT_LENGTH; // Pressure sensor to bottom reference -// Geometric offset between physical top of the float and the barometer. -// The Bar02 sits at the top, so this is ~0 m; calibrate on hardware if needed. -constexpr float FLOAT_TOP_TO_SENSOR_M = 0.0f; +// Vertical offset of the physical top of the float relative to the barometer, +// signed so that topDepth = sensorDepth - SENSOR_TO_TOP_M (see sensors.cpp). +// Positive = top is BELOW the sensor; negative = top is ABOVE the sensor. +// Misurato in hardware: il Bar02 sporge 1 cm SOPRA la cima del float, quindi la +// cima è 1 cm più in profondità del sensore → offset NEGATIVO (-0.01 m). +constexpr float FLOAT_TOP_TO_SENSOR_M = -0.01f; constexpr float SENSOR_TO_TOP_M = FLOAT_TOP_TO_SENSOR_M; // Operational target: how deep the *top* of the float should sit below the -// water surface when the float is "floating". Runtime-tunable via +// water surface when the float is "floating"/resting. Runtime-tunable via // CMD_SET_SURFACE_OFFSET / USB SURFACE_OFFSET command — this is the default. -constexpr float SURFACE_TARGET_OFFSET_M = 0.10f; +// 0.15 m tiene l'antenna (~12 cm sopra il tappo) sommersa di qualche cm a riposo, +// così il float non rompe la superficie (penalità -5 punti) in attesa del recupero. +constexpr float SURFACE_TARGET_OFFSET_M = 0.15f; constexpr float DEPTH_EPSILON = 0.01f; // "Stationary" tolerance (m) constexpr uint8_t PROFILE_MAX_COUNT = 2; // Profiles before auto-stop @@ -184,6 +195,9 @@ constexpr float TARGET_SHALLOW_TOP_DEPTH = 0.40f; // Shallow hold: top refere constexpr float STAT_TIME = 30.0f; // MATE hold time at target (s) constexpr float TIMEOUT_PID_TIME = 180.0f; // Max PID phase time (s) constexpr float TIMEOUT_ASCENT = 120.0f; // Max ascent + shallow hold time (s) +// Sosta finale sotto il pelo (top a SURFACE_TARGET_OFFSET_M) dopo i due profili: +// tiene attivo il PID finché il float non viene recuperato (o scade la finestra). +constexpr float REST_WINDOW_S = 120.0f; // Active surface-rest hold window (s) constexpr float TARGET_SHALLOW_BOTTOM_DEPTH = TARGET_SHALLOW_TOP_DEPTH + SENSOR_TO_BOTTOM_M + SENSOR_TO_TOP_M; @@ -198,6 +212,27 @@ constexpr int8_t TARGET_BOTTOM = -1; // Descend to pool floor constexpr float WATER_DENSITY_FRESH = 997.0f; // kg/m³ constexpr float GRAVITY = 9.80665f; +// --------------------------------------------------------------------------- +// FLOAT SIMULATOR (HIL da banco: motore reale, barometro SIMULATO) +// --------------------------------------------------------------------------- +// Con SIM attivo le letture Bar02 sono sostituite da un modello fisico 2° ordine: +// la siringa (u = motorPosToU(motor.position())) genera spinta netta, la quota +// del SENSORE viene integrata con inerzia idrodinamica e drag quadratico: +// a = SIM_ACCEL_GAIN*(u - SIM_U_NEUTRAL) - SIM_DRAG_QUAD*v*|v| +// v += a*dt ; z += v*dt +// Convenzione coerente col resto del firmware: u > neutral => affonda (z cresce). +// Tutti i parametri sono ritarabili a runtime con SIM_CONFIG senza riflashare. +// Default tarati con la simulazione offline del loop chiuso (PID + motore lento +// ~2 mm/s + modello): a uNeutral=0.35 la discesa converge ~2.5 m con overshoot +// ~0.28 m (realistico, sink-biased come il float vero) e l'effetto di Kd è ben +// visibile. RITARALI sul TUO float con SIM_CONFIG: uNeutral = u a cui la siringa +// regge la quota senza muoversi (lo vedi col valore di u a regime in PID_HOLD). +constexpr float SIM_U_NEUTRAL = 0.35f; // u di galleggiamento neutro (net buoyancy = 0) +constexpr float SIM_ACCEL_GAIN = 0.05f; // accelerazione [m/s^2] per unità di (u - neutral) +constexpr float SIM_DRAG_QUAD = 1.50f; // coeff. drag quadratico [1/m] +constexpr float SIM_POOL_DEPTH = 3.00f; // profondità vasca simulata [m] (vasca NRC ~3 m) +constexpr float SIM_MAX_DT_S = 0.20f; // clamp dt integrazione per stabilità [s] + // --------------------------------------------------------------------------- // NETWORK / OTA // --------------------------------------------------------------------------- @@ -212,7 +247,7 @@ constexpr uint8_t ESPNOW_CHANNEL = 1; // --------------------------------------------------------------------------- // EEPROM / DATA // --------------------------------------------------------------------------- -constexpr char COMPANY_NUMBER[] = "EX10"; +constexpr char COMPANY_NUMBER[] = "EX12"; constexpr char FLASH_LOG_PATH[] = "/mission/current_profile.csv"; // EEPROM_SIZE and sensor_data struct come from float_common.h diff --git a/lib/profile/include/profile.h b/lib/profile/include/profile.h index c22c8bd..dbe4d3a 100644 --- a/lib/profile/include/profile.h +++ b/lib/profile/include/profile.h @@ -32,6 +32,9 @@ class ProfileManager { const RuntimeProfileConfig& config() const { return _config; } float ascentTargetBottomM() const; + // Target della sosta finale (top del float a surfaceRestOffsetM sotto il pelo), + // convertito in riferimento FONDO come il PID. Vedi ascentTargetBottomM(). + float restTargetBottomM() const; bool setConfig(const RuntimeProfileConfig& config); bool validateConfig(const RuntimeProfileConfig& config) const; void formatConfigJson(char* buffer, size_t bufferSize) const; diff --git a/lib/profile/src/profile.cpp b/lib/profile/src/profile.cpp index 0795ab3..d25d80c 100644 --- a/lib/profile/src/profile.cpp +++ b/lib/profile/src/profile.cpp @@ -27,7 +27,9 @@ ProfileManager::ProfileManager() {} namespace { constexpr uint32_t PROFILE_CONFIG_MAGIC = 0x50464C54UL; // "PFLT" -constexpr uint16_t PROFILE_CONFIG_VERSION = 1; +// v2: surfaceRestOffsetM default 0.10 -> 0.15 (antenna sommersa). Il bump +// invalida la config NVS stantia così il nuovo default da config.h viene ricaricato. +constexpr uint16_t PROFILE_CONFIG_VERSION = 2; constexpr char PROFILE_CONFIG_NAMESPACE[] = "float_profile"; constexpr char PROFILE_CONFIG_KEY[] = "cfg"; @@ -80,6 +82,13 @@ float ProfileManager::ascentTargetBottomM() const { return _config.ascentTargetM + SENSOR_TO_BOTTOM_M + SENSOR_TO_TOP_M; } +// --------------------------------------------------------------------------- +float ProfileManager::restTargetBottomM() const { + // surfaceRestOffsetM e' la quota del TOP del float sotto il pelo a riposo; + // convertita in riferimento FONDO come ascentTargetBottomM(). + return _config.surfaceRestOffsetM + SENSOR_TO_BOTTOM_M + SENSOR_TO_TOP_M; +} + // --------------------------------------------------------------------------- bool ProfileManager::setConfig(const RuntimeProfileConfig& config) { if (!validateConfig(config)) { @@ -266,6 +275,10 @@ void ProfileManager::measure(float targetDepth, float holdTimeSec, float timeout const bool isPIDPhase = !isSurfaceTarget && !isBottomTarget; const bool isDeepTarget = fabsf(targetDepth - _config.descentTargetM) < 0.001f; const bool isShallowTarget = fabsf(targetDepth - ascentTargetBottomM()) < 0.001f; + // Sosta finale: hold PID al target di riposo che NON termina al raggiungimento + // dell'hold ma resta attivo finché non arriva il recupero (remote stop) o + // scade timeoutSec — così il float non risale e non rompe la superficie. + const bool isRestTarget = fabsf(targetDepth - restTargetBottomM()) < 0.001f; // --- LED and initial motor positioning --- if (isPIDPhase) { @@ -324,14 +337,14 @@ void ProfileManager::measure(float targetDepth, float holdTimeSec, float timeout const char* phase = "descending"; if (isPIDPhase) { - if (isShallowTarget) { - phase = (fabsf(currentDepth - targetDepth) < _config.depthToleranceM) - ? "hold_40cm" - : "ascending"; + const bool atTarget = + fabsf(currentDepth - targetDepth) < _config.depthToleranceM; + if (isRestTarget) { + phase = atTarget ? "rest_surface" : "resting"; + } else if (isShallowTarget) { + phase = atTarget ? "hold_40cm" : "ascending"; } else { - phase = (fabsf(currentDepth - targetDepth) < _config.depthToleranceM) - ? "hold_2_5m" - : "descending"; + phase = atTarget ? "hold_2_5m" : "descending"; } } else if (isSurfaceTarget) { // Riferimento: top del float a `surfaceTargetOffset` sotto il pelo. @@ -427,7 +440,10 @@ void ProfileManager::measure(float targetDepth, float holdTimeSec, float timeout // Seven 5-second packets span the required 30-second hold. const int requiredTicks = static_cast(holdTimeSec * 1000.0f / PERIOD_EEPROM_WRITE) + 1; - if (stableCount >= requiredTicks) { + // La sosta finale (isRestTarget) NON esce all'hold completo: resta + // attiva finché non arriva il recupero (remote stop) o scade il + // timeout, così la cima resta sotto il pelo in attesa dell'ROV. + if (!isRestTarget && stableCount >= requiredTicks) { Debug.println("Profile: PID hold complete — target depth sustained"); _logProfileReading("exit_hold_ok"); break; diff --git a/lib/sensors/include/sensors.h b/lib/sensors/include/sensors.h index edd34b2..ea2da49 100644 --- a/lib/sensors/include/sensors.h +++ b/lib/sensors/include/sensors.h @@ -55,6 +55,19 @@ class SensorManager { // Battery bus voltage (mV) — reads from INA219 on demand uint32_t batteryMilliVolts(); + // ----------------------------------------------------------------------- + // SIMULATORE (HIL da banco): quando attivo, sensorDepth()/pressure() — e + // quindi depth()/bottomDepth()/topDepth() che li usano — restituiscono una + // quota SIMULATA da un modello fisico mosso dalla posizione reale del motore. + // Il motore si muove davvero: PID_HOLD/PID_STEP/measure()/GO girano invariati. + // ----------------------------------------------------------------------- + void simEnable(bool on); + bool simEnabled() const { return _simEnabled; } + void simConfigure(float uNeutral, float accelGain, float dragQuad, float poolDepth); + void simReset(float sensorDepthM = 0.0f); + float simSensorDepth() const { return _simZ; } + void simFormatStatus(char* buffer, size_t bufferSize) const; + private: MS5837 _bar02; INA_Class _ina; @@ -63,6 +76,17 @@ class SensorManager { float _atmPressurePa = 0.0f; // Reference pressure set at startup float _surfaceTargetOffsetM = SURFACE_TARGET_OFFSET_M; + // --- Stato simulatore --- + bool _simEnabled = false; + float _simZ = 0.0f; // quota sensore simulata [m] + float _simV = 0.0f; // velocità verticale [m/s] (+ = giù/affonda) + unsigned long _simLastMs = 0; // 0 = primo step (inizializza dt) + float _simUNeutral = SIM_U_NEUTRAL; + float _simAccelGain = SIM_ACCEL_GAIN; + float _simDragQuad = SIM_DRAG_QUAD; + float _simPoolDepth = SIM_POOL_DEPTH; + void _simStep(); + void _initPressureSensor(); void _initPowerMonitor(); }; diff --git a/lib/sensors/src/sensors.cpp b/lib/sensors/src/sensors.cpp index 16bab94..74a5a4b 100644 --- a/lib/sensors/src/sensors.cpp +++ b/lib/sensors/src/sensors.cpp @@ -1,6 +1,7 @@ #include "sensors.h" #include "config.h" #include "led.h" +#include "motor.h" #include #include #include @@ -74,6 +75,10 @@ void SensorManager::_initPressureSensor() { // --------------------------------------------------------------------------- void SensorManager::read() { + if (_simEnabled) { + _simStep(); + return; + } _bar02.read(); } @@ -82,6 +87,7 @@ float SensorManager::depth() { } float SensorManager::sensorDepth() { + if (_simEnabled) return _simZ; return depthFromPressure(_bar02.pressure(MS5837::Pa)); } @@ -104,16 +110,83 @@ void SensorManager::setSurfaceTargetOffset(float meters) { } float SensorManager::referenceDepthForPhase(const char* phase) { - if (phase != nullptr && strcmp(phase, "hold_40cm") == 0) { - return topDepth(); - } - return bottomDepth(); + // Profondità RIPORTATA (pacchetti/grafico): sempre riferita alla CIMA del + // float, così parte da ~0 in superficie ed è un riferimento unico e continuo. + // Il CONTROLLO resta riferito al FONDO (depth() = bottomDepth()): il PID porta + // comunque il fondo a 2.5 m e la cima a 40 cm. L'offset cima→fondo va + // comunicato al giudice per l'hold profondo (regolamento Task 4). + (void)phase; + return topDepth(); } float SensorManager::pressure() { + if (_simEnabled) { + // Pressione coerente con la quota simulata del sensore (Stevino), così i + // log/pacchetti mostrano un kPa plausibile e depthFromPressure() tornerebbe _simZ. + return _atmPressurePa + WATER_DENSITY_FRESH * GRAVITY * _simZ; + } return _bar02.pressure(MS5837::Pa); } +// --------------------------------------------------------------------------- +// SIMULATORE +// --------------------------------------------------------------------------- +void SensorManager::simEnable(bool on) { + _simEnabled = on; + if (on) { + _simZ = 0.0f; // parte in superficie (sensore a quota 0) + _simV = 0.0f; + _simLastMs = 0; // forza l'inizializzazione del dt al primo step + } + Debug.printf("Float SIM %s\n", on ? "ON (barometro simulato, motore reale)" : "OFF"); +} + +void SensorManager::simConfigure(float uNeutral, float accelGain, float dragQuad, float poolDepth) { + _simUNeutral = constrain(uNeutral, 0.0f, 1.0f); + _simAccelGain = (accelGain > 0.0f) ? accelGain : _simAccelGain; + _simDragQuad = (dragQuad >= 0.0f) ? dragQuad : _simDragQuad; + _simPoolDepth = (poolDepth > 0.0f) ? poolDepth : _simPoolDepth; + Debug.printf("SIM cfg: uNeutral=%.3f accelGain=%.4f dragQuad=%.3f pool=%.2f m\n", + _simUNeutral, _simAccelGain, _simDragQuad, _simPoolDepth); +} + +void SensorManager::simReset(float sensorDepthM) { + _simZ = (sensorDepthM < 0.0f) ? 0.0f : sensorDepthM; + _simV = 0.0f; + _simLastMs = 0; +} + +void SensorManager::simFormatStatus(char* buffer, size_t bufferSize) const { + if (buffer == nullptr || bufferSize == 0) return; + snprintf(buffer, bufferSize, + "SIM %s | z=%.3f m v=%.3f m/s | uNeutral=%.3f accelGain=%.4f dragQuad=%.3f pool=%.2f m", + _simEnabled ? "ON" : "OFF", + _simZ, _simV, _simUNeutral, _simAccelGain, _simDragQuad, _simPoolDepth); +} + +void SensorManager::_simStep() { + const unsigned long now = millis(); + if (_simLastMs == 0) { _simLastMs = now; return; } // primo campione: solo inizializza + float dt = (now - _simLastMs) / 1000.0f; + _simLastMs = now; + if (dt <= 0.0f) return; + if (dt > SIM_MAX_DT_S) dt = SIM_MAX_DT_S; // evita salti d'integrazione se il loop si ferma + + // u reale dalla posizione del motore: include il ritardo di corsa del motore, + // così la taratura PID vede la stessa lentezza meccanica del float vero. + const float u = motorPosToU(motor.position()); + const float aBuoy = _simAccelGain * (u - _simUNeutral); // u>neutral => +a => affonda + const float aDrag = -_simDragQuad * _simV * fabsf(_simV); // drag quadratico, frena + _simV += (aBuoy + aDrag) * dt; + _simZ += _simV * dt; + + // Vincolo superficie: il sensore non emerge sopra il pelo (z>=0). + if (_simZ < 0.0f) { _simZ = 0.0f; if (_simV < 0.0f) _simV = 0.0f; } + // Vincolo fondo vasca: il FONDO del float tocca il fondo (z = pool - lunghezza). + const float zMax = _simPoolDepth - SENSOR_TO_BOTTOM_M; + if (zMax > 0.0f && _simZ > zMax) { _simZ = zMax; if (_simV > 0.0f) _simV = 0.0f; } +} + float SensorManager::temperature() { return _bar02.temperature(); } diff --git a/src/espA/main.cpp b/src/espA/main.cpp index 6223363..8af60d9 100644 --- a/src/espA/main.cpp +++ b/src/espA/main.cpp @@ -69,6 +69,8 @@ static void servicePidTuningSerial(); static void runSyringeSet(float uNorm, float durationS); static void runPidHold(float depthTarget, float durationS); static void runPidStep(float depthTarget); +static bool runVerticalProfiles(uint8_t& completedProfiles); +static void attemptAutoRecovery(); // --------------------------------------------------------------------------- // SETUP // --------------------------------------------------------------------------- @@ -245,63 +247,19 @@ void loop() { uint8_t completedProfiles = 0; if (ack && motionController.motionAllowed()) { - const RuntimeProfileConfig& profileConfig = profileManager.config(); - Debug.println("Mission: starting vertical profiles"); - - profileManager.resetEEPROM(); - profileManager.logDeploymentPacket(); - - while (completedProfiles < profileConfig.profileCount && motionController.motionAllowed()) { - profileManager.beginProfile(completedProfiles + 1); - Debug.printf("Profile %d: PID descent to %.2f m bottom reference\n", - completedProfiles + 1, profileConfig.descentTargetM); - profileManager.measure(profileConfig.descentTargetM, - profileConfig.holdTimeS, - profileConfig.descentTimeoutS); - if (!motionController.motionAllowed()) { - aborted = true; - break; - } - - delay(500); - - Debug.printf("Profile %d: PID ascent to %.2f m top reference\n", - completedProfiles + 1, profileConfig.ascentTargetM); - profileManager.measure(profileManager.ascentTargetBottomM(), - profileConfig.holdTimeS, - profileConfig.ascentTimeoutS); - if (!motionController.motionAllowed()) { - aborted = true; - break; - } - - motor.disableOutputs(); - completedProfiles++; - Debug.printf("Profile %d complete\n", completedProfiles); - delay(500); - } - - if (g_autoCommitted && completedProfiles >= profileConfig.profileCount) { + aborted = runVerticalProfiles(completedProfiles); + if (g_autoCommitted && + completedProfiles >= profileManager.config().profileCount) { g_autoMissionDone = true; } } - // Auto-recovery: senza telemetria, un emergency stop lascerebbe il - // float bloccato sul fondo col LED rosso e ogni comando successivo - // rifiutato da motionAllowed(). 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. + // Auto-recovery: senza telemetria, un emergency stop lascerebbe il float + // bloccato sul fondo col LED rosso e ogni comando successivo rifiutato da + // motionAllowed(). Il record emergency_stop è già su flash; qui un homing + // cancella lo stop e riporta la siringa a galleggiamento, pronto per un GO. if (aborted) { - 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"); - } + attemptAutoRecovery(); } g_status = CMD_IDLE; @@ -631,6 +589,16 @@ void loop() { // max 60 s (esci a regime), log a 10 Hz // SURFACE_OFFSET — target di galleggiamento: il top del // float sta a sotto il pelo (default 0.10) +// SIM_ON / SIM_OFF — simulatore barometro on/off. Il motore si +// muove DAVVERO; la quota è simulata da un +// modello fisico mosso dalla siringa. Poi usa +// PID_STEP/PID_HOLD/GO per tarare il PID a secco. +// SIM_GET — stato e parametri del simulatore +// SIM_CONFIG +// — ritara la fisica del simulatore a runtime +// GO — lancia la missione completa (profili + +// sosta) da seriale, senza GUI/ESPB. Con SIM +// attivo = test end-to-end al banco a secco. // // Tutto il logging finisce su Serial (USB), formato CSV per facile import. // --------------------------------------------------------------------------- @@ -696,6 +664,44 @@ static void servicePidTuningSerial() { if (!a) { Debug.println("ERR: SURFACE_OFFSET "); return; } sensors.setSurfaceTargetOffset(atof(a)); Debug.printf("OK SURFACE_OFFSET %.3f m\n", sensors.surfaceTargetOffset()); + } else if (strcmp(tok, "SIM_ON") == 0) { + sensors.simEnable(true); + Debug.println("OK SIM_ON (barometro simulato, motore reale)"); + } else if (strcmp(tok, "SIM_OFF") == 0) { + sensors.simEnable(false); + Debug.println("OK SIM_OFF"); + } else if (strcmp(tok, "SIM_GET") == 0) { + char buf[176]; + sensors.simFormatStatus(buf, sizeof(buf)); + Debug.println(buf); + } else if (strcmp(tok, "SIM_CONFIG") == 0) { + char* values[4] = {}; + for (char*& value : values) { + value = strtok(nullptr, " "); + if (!value) { + Debug.println("ERR: SIM_CONFIG "); + return; + } + } + sensors.simConfigure(atof(values[0]), atof(values[1]), + atof(values[2]), atof(values[3])); + Debug.println("OK SIM_CONFIG"); + } else if (strcmp(tok, "GO") == 0) { + // Lancia la missione completa (profileCount profili + sosta) + // direttamente da seriale, senza GUI/ESPB: utile col SIM per il + // test end-to-end al banco. Bloccante fino a fine missione; + // per fermarla prima resetta ESPA. + if (!motionController.motionAllowed()) { + Debug.println("ERR: GO — motion not allowed (serve homing ok)"); + return; + } + Debug.println("# GO: missione completa (profili + sosta). Reset ESPA per abortire."); + uint8_t completed = 0; + const bool aborted = runVerticalProfiles(completed); + if (aborted) attemptAutoRecovery(); + Debug.printf("# GO done: %u/%u profili%s\n", + completed, profileManager.config().profileCount, + aborted ? " (ABORT)" : ""); } else if (strcmp(tok, "DUMP_LOG") == 0) { // Dump del flash log su richiesta: risolve il caso in cui il // serial monitor si collega dopo il dump automatico nel setup(). @@ -865,3 +871,67 @@ static void runPidStep(float depthTarget) { runPidLoop(depthTarget, 60000UL, 100); } + +// --------------------------------------------------------------------------- +// Missione completa: profileCount profili (discesa→hold→risalita→hold) + sosta +// finale. Riusata dal case CMD_GO (GUI/ESP-NOW/auto) e dal comando seriale +// diretto "GO" (test al banco / simulatore). Ritorna true se abortita da +// emergency stop; scrive in completedProfiles il numero di profili conclusi. +static bool runVerticalProfiles(uint8_t& completedProfiles) { + completedProfiles = 0; + bool aborted = false; + const RuntimeProfileConfig& profileConfig = profileManager.config(); + Debug.println("Mission: starting vertical profiles"); + + profileManager.resetEEPROM(); + profileManager.logDeploymentPacket(); + + while (completedProfiles < profileConfig.profileCount && motionController.motionAllowed()) { + profileManager.beginProfile(completedProfiles + 1); + Debug.printf("Profile %d: PID descent to %.2f m bottom reference\n", + completedProfiles + 1, profileConfig.descentTargetM); + profileManager.measure(profileConfig.descentTargetM, + profileConfig.holdTimeS, + profileConfig.descentTimeoutS); + if (!motionController.motionAllowed()) { aborted = true; break; } + + delay(500); + + Debug.printf("Profile %d: PID ascent to %.2f m top reference\n", + completedProfiles + 1, profileConfig.ascentTargetM); + profileManager.measure(profileManager.ascentTargetBottomM(), + profileConfig.holdTimeS, + profileConfig.ascentTimeoutS); + if (!motionController.motionAllowed()) { aborted = true; break; } + + motor.disableOutputs(); + completedProfiles++; + Debug.printf("Profile %d complete\n", completedProfiles); + delay(500); + } + + // Sosta finale: tieni la CIMA del float a surfaceRestOffsetM sotto il pelo + // (antenna sommersa) finché non arriva il recupero o scade la finestra — + // evita di rompere la superficie (penalità) in attesa dell'ROV. + if (!aborted && motionController.motionAllowed()) { + Debug.printf("Mission: surface rest, top at %.2f m below surface\n", + profileConfig.surfaceRestOffsetM); + profileManager.measure(profileManager.restTargetBottomM(), + profileConfig.holdTimeS, + REST_WINDOW_S); + if (!motionController.motionAllowed()) aborted = true; + } + + return aborted; +} + +// Auto-recovery su abort: l'homing cancella l'emergency stop e riporta la siringa +// in posizione nota (galleggiamento), così il float risale e resta pronto. +static void attemptAutoRecovery() { + Debug.println("Profile aborted by emergency stop — attempting auto-recovery"); + if (motionController.homeWithTof()) { + Debug.println("Auto-recovery homing complete — float ready"); + } else { + Debug.println("Auto-recovery homing FAILED — float remains in error"); + } +} From f6ecaec955cda571996fde98d0b8309ef890b992 Mon Sep 17 00:00:00 2001 From: Davide Colabella Date: Wed, 29 Jul 2026 00:46:10 +0200 Subject: [PATCH 22/22] feat(mbsd): control core model-based con catena MIL/SIL validata MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Rifà il control core del float in Model-Based Software Design, testabile in simulazione lungo la V (MIL -> SIL), senza toccare il firmware di gara (espA). Aggiunge: - lib/float_core: control core portabile in C puro, tick-based e senza I/O (float_core_step). Riproduce missione/PID/safety/homing come funzione pura; e' la specifica di riferimento per il modello. - tools/mil: harness MIL nativo (plant 2o ordine + main) che chiude l'anello su PC e verifica i requisiti MATE. Compila con gcc, senza MATLAB. - model/: mondo Simulink/Stateflow. chart float_core (float_core_ref.slx), plant + testbench (float_mil.slx), parametri/plant/harness (.m), spec del chart (STATEFLOW_CHART.md), diagrammi. Equivalenza MIL vs float_core.c certificata; SIL: codice generato da Embedded Coder identico al modello. - src/espA_core (+ env in platformio.ini): firmware sperimentale che fa da scheduler HAL attorno al core, parallelo a espA (non lo tocca). Tuning della risalita fatto in MIL (PASS) e congelato in config.h: PID_U_MIN 0.0 -> 0.20 e TARGET_SHALLOW_TOP_DEPTH 0.40 -> 0.65 (margine anti-rottura superficie, dentro tolleranza MATE). .gitignore: esclude gli artefatti di build Simulink/Embedded Coder (slprj/, *.slxc, *_ert_rtw/, ...); si versionano solo i sorgenti. --- .gitignore | 12 + include/config.h | 10 +- lib/float_core/include/float_core.h | 43 ++ lib/float_core/include/float_core_types.h | 235 +++++++++ lib/float_core/src/float_core.c | 494 +++++++++++++++++++ model/README.md | 63 +++ model/STATEFLOW_CHART.md | 284 +++++++++++ model/diagrams/float_core_homing.svg | 53 ++ model/diagrams/float_core_mission.svg | 58 +++ model/diagrams/float_core_overview.svg | 66 +++ model/float_core_ref.slx | Bin 0 -> 146798 bytes model/float_mil.slx | Bin 0 -> 137488 bytes model/params/core_params.m | 124 +++++ model/params/declare_chart_data.m | 62 +++ model/plant/float_plant_fcn.m | 98 ++++ model/tests/run_mil.m | 97 ++++ platformio.ini | 22 + src/espA_core/main.cpp | 571 ++++++++++++++++++++++ tools/mil/.gitignore | 1 + tools/mil/Makefile | 26 + tools/mil/README.md | 42 ++ tools/mil/mil_main.c | 213 ++++++++ tools/mil/plant.c | 118 +++++ tools/mil/plant.h | 55 +++ 24 files changed, 2745 insertions(+), 2 deletions(-) create mode 100644 lib/float_core/include/float_core.h create mode 100644 lib/float_core/include/float_core_types.h create mode 100644 lib/float_core/src/float_core.c create mode 100644 model/README.md create mode 100644 model/STATEFLOW_CHART.md create mode 100644 model/diagrams/float_core_homing.svg create mode 100644 model/diagrams/float_core_mission.svg create mode 100644 model/diagrams/float_core_overview.svg create mode 100644 model/float_core_ref.slx create mode 100644 model/float_mil.slx create mode 100644 model/params/core_params.m create mode 100644 model/params/declare_chart_data.m create mode 100644 model/plant/float_plant_fcn.m create mode 100644 model/tests/run_mil.m create mode 100644 src/espA_core/main.cpp create mode 100644 tools/mil/.gitignore create mode 100644 tools/mil/Makefile create mode 100644 tools/mil/README.md create mode 100644 tools/mil/mil_main.c create mode 100644 tools/mil/plant.c create mode 100644 tools/mil/plant.h diff --git a/.gitignore b/.gitignore index f388e1f..4d36f63 100644 --- a/.gitignore +++ b/.gitignore @@ -53,3 +53,15 @@ dkms.conf .pio .vscode **/.DS_Store + +# MATLAB / Simulink build artifacts (rigenerabili: si versionano solo i sorgenti +# .slx / .m / .md, non l'output di code-gen di Embedded Coder ne' le cache) +slprj/ +*.slxc +*.autosave +*.slx.autosave +*.asv +*.mex* +*_ert_rtw/ +*_grt_rtw/ +codegen/ diff --git a/include/config.h b/include/config.h index 846e496..94f9046 100644 --- a/include/config.h +++ b/include/config.h @@ -113,7 +113,10 @@ constexpr uint8_t TOF_SAFETY_STOP_SAMPLES = 3; // (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; +// PID_U_MIN tarato in MIL (model/) a 0.20: NON svuotare del tutto la siringa in +// risalita limita la spinta verso l'alto e riduce l'overshoot che rompeva la +// superficie. Era 0.0 (svuotamento completo → risalita troppo veloce, sfondava). +constexpr float PID_U_MIN = 0.20f; constexpr float PID_U_MAX = 0.92f; // --------------------------------------------------------------------------- @@ -191,7 +194,10 @@ constexpr float DEPTH_EPSILON = 0.01f; // "Stationary" tolerance (m) 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) -constexpr float TARGET_SHALLOW_TOP_DEPTH = 0.40f; // Shallow hold: top reference (m) +// Tarato in MIL (model/) a 0.65 m: tenere la cima piu' in basso (dentro la +// tolleranza MATE 0.40 +/- 0.33 = [0.07, 0.73]) lascia margine all'overshoot di +// risalita cosi' il float non rompe la superficie (penalita' -5). Era 0.40. +constexpr float TARGET_SHALLOW_TOP_DEPTH = 0.65f; // Shallow hold: top reference (m) 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) diff --git a/lib/float_core/include/float_core.h b/lib/float_core/include/float_core.h new file mode 100644 index 0000000..c0c5604 --- /dev/null +++ b/lib/float_core/include/float_core.h @@ -0,0 +1,43 @@ +#ifndef FLOAT_CORE_H +#define FLOAT_CORE_H + +/* + ******************************************************************************* + * float_core.h + * API del control core tick-based. Due sole funzioni pure: + * - float_core_init(): azzera lo stato del modello. + * - float_core_step(): un passo del modello. Chiamata dall'HAL a passo fisso + * (pid_period_s). NON blocca, NON fa I/O: legge CoreInputs, aggiorna + * CoreState, produce CoreOutputs. + * + * Questa e' esattamente la firma che Embedded Coder generera' dal modello + * Simulink/Stateflow: quando il modello sara' pronto, la libreria generata + * rimpiazza float_core.c senza toccare l'HAL. + * + * Maintainers: Colabella Davide, Benevenga Filippo — Team PoliTOcean + ******************************************************************************* + */ + +#include "float_core_types.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/* Inizializza lo stato del core (mode = INIT). Chiamare una volta al boot. */ +void float_core_init(CoreState* st); + +/* Un passo del modello. in->cfg deve essere valido e non-NULL. */ +void float_core_step(const CoreInputs* in, CoreState* st, CoreOutputs* out); + +/* Helper di mappatura logica <-> passi, esposti per HAL e test. + * u in [0,1]: u=0 -> home (galleggia), u=1 -> siringa piena (affonda). + * Convenzione: u cresce verso posizioni motore NEGATIVE. */ +long float_core_u_to_motor_pos(const CoreConfig* cfg, float u); +float float_core_motor_pos_to_u(const CoreConfig* cfg, long position); + +#ifdef __cplusplus +} +#endif + +#endif /* FLOAT_CORE_H */ diff --git a/lib/float_core/include/float_core_types.h b/lib/float_core/include/float_core_types.h new file mode 100644 index 0000000..052e198 --- /dev/null +++ b/lib/float_core/include/float_core_types.h @@ -0,0 +1,235 @@ +#ifndef FLOAT_CORE_TYPES_H +#define FLOAT_CORE_TYPES_H + +/* + ******************************************************************************* + * float_core_types.h + * CONTRATTO D'INTERFACCIA del control core (Model-Based Software Design). + * + * Questo header e' l'unica "single source of truth" del confine modello <-> HAL: + * - definisce i segnali di ingresso/uscita del core (CoreInputs/CoreOutputs) + * - e' C PURO, zero dipendenze da Arduino/ESP-IDF: compila identico nel + * firmware, nell'harness nativo (MIL/SIL su PC) e nel codice generato da + * Embedded Coder. + * + * Il core NON tocca mai hardware: niente I2C, ESP-NOW, LittleFS, millis(), + * delay(), Serial. Riceve i sensori gia' letti + dt, restituisce comandi ed + * EVENTI discreti (ack/log) che l'HAL traduce in azioni reali. + * + * Maintainers: Colabella Davide, Benevenga Filippo — Team PoliTOcean + ******************************************************************************* + */ + +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* ------------------------------------------------------------------------- */ +/* Eventi/comandi cui il core reagisce (sottoinsieme mission-critical). */ +/* La decodifica GUI/ESP-NOW/seriale resta nell'HAL: qui arriva solo l'evento */ +/* latched, tipicamente NONE. */ +/* ------------------------------------------------------------------------- */ +typedef enum { + CORE_EVT_NONE = 0, + CORE_EVT_GO, /* avvia la missione (profili + sosta finale) */ + CORE_EVT_HOME, /* esegue l'homing TOF */ + CORE_EVT_STOP /* emergency stop remoto */ +} CoreEvent; + +/* Stato di supervisione del core. */ +typedef enum { + CORE_MODE_INIT = 0, + CORE_MODE_HOMING, + CORE_MODE_IDLE, + CORE_MODE_MISSION, + CORE_MODE_SAFE_STOP +} CoreMode; + +/* Sotto-fase della missione (tag per telemetria/log + scelta del setpoint). */ +typedef enum { + CORE_PHASE_NONE = 0, + CORE_PHASE_PRE_DESCENT, /* pacchetto di deployment pre-discesa */ + CORE_PHASE_DESCEND_HOLD, /* PID verso il target di fondo + hold */ + CORE_PHASE_ASCEND_HOLD, /* PID verso il target di risalita + hold */ + CORE_PHASE_SURFACE_REST, /* sosta finale sotto pelo (non esce all'hold) */ + CORE_PHASE_DONE +} CorePhase; + +/* Sotto-fase dell'homing TOF (approach -> retract -> backoff). */ +typedef enum { + CORE_HOME_APPROACH = 0, + CORE_HOME_RETRACT, + CORE_HOME_BACKOFF, + CORE_HOME_DONE, + CORE_HOME_FAILED +} CoreHomePhase; + +/* Richiesta LED (mirror logico di LEDState, tenuto indipendente/portabile). */ +typedef enum { + CORE_LED_OFF = 0, + CORE_LED_INIT, + CORE_LED_IDLE, + CORE_LED_IDLE_WITH_DATA, + CORE_LED_ERROR, + CORE_LED_PROFILE, + CORE_LED_HOMING, + CORE_LED_PID_CONTROL, + CORE_LED_COMMUNICATION +} CoreLed; + +/* Modo del comando motore emesso dal core. */ +typedef enum { + CORE_MOTOR_HOLD = 0, /* mantieni posizione (l'HAL puo' disabilitare uscite) */ + CORE_MOTOR_GOTO, /* vai a motor_target (assoluto, in step) */ + CORE_MOTOR_JOG, /* homing: moto continuo nel verso di sign(motor_target) */ + CORE_MOTOR_STOP /* ferma ora + disabilita uscite */ +} CoreMotorMode; + +/* Evento discreto verso la CS (comms). L'HAL sceglie la stringa ACK reale. */ +typedef enum { + CORE_ACK_NONE = 0, + CORE_ACK_GO_RECVD, + CORE_ACK_HOME_RECVD, + CORE_ACK_STOP_RECVD, + CORE_ACK_IDLE, /* heartbeat: FLOAT_IDLE */ + CORE_ACK_IDLE_W_DATA /* heartbeat: FLOAT_IDLE_W_DATA */ +} CoreAckEvent; + +/* Tipo di record che il core chiede all'HAL di scrivere sul flash log. */ +typedef enum { + CORE_LOG_NONE = 0, + CORE_LOG_DEPLOY, /* pacchetto pre-discesa */ + CORE_LOG_PROFILE, /* campione periodico di profilo */ + CORE_LOG_PHASE_START, /* inizio fase con parametri effettivi */ + CORE_LOG_EXIT_HOLD_OK, /* hold completato con successo */ + CORE_LOG_EXIT_TIMEOUT, /* fase troncata dal timeout */ + CORE_LOG_EXIT_REMOTE, /* uscita per remote stop */ + CORE_LOG_EMERGENCY, /* emergency stop (TOF/anomalia) */ + CORE_LOG_HOMING /* evento di homing */ +} CoreLogKind; + +/* ------------------------------------------------------------------------- */ +/* Configurazione: parametri di missione, PID, geometria e safety TOF. */ +/* Popolata dall'HAL da config.h/NVS. Il core la tratta come read-only. */ +/* I target sono gia' convertiti in riferimento FONDO (come il sensore). */ +/* ------------------------------------------------------------------------- */ +typedef struct { + /* --- missione --- */ + uint8_t profile_count; + float descent_target_m; /* target discesa (rif. fondo) */ + float ascent_target_bottom_m; /* target risalita (rif. fondo, gia' convertito) */ + float rest_target_bottom_m; /* target sosta finale (rif. fondo) */ + float depth_tolerance_m; + float hold_time_s; + float descent_timeout_s; + float ascent_timeout_s; + float rest_window_s; + + /* --- PID (output normalizzato u in [0,1], guadagni per metro d'errore) --- */ + float kp, ki, kd; + float alpha_d; + float integral_limit; + float u_neutral; /* offset kick-start sommato all'output */ + float min_retarget_frac; /* dead-band ri-comando (frazione di corsa) */ + float pid_period_s; /* passo fisso del modello (tempo di campionamento) */ + float descent_kick_u; /* pre-posizionamento a inizio discesa */ + float u_min, u_max; /* clamp dell'output PID (margine dai fine corsa) */ + + /* --- geometria motore --- */ + long usable_steps; /* MOTOR_MAX_STEPS - 2*margine */ + long endstop_margin_steps; + long max_steps; + + /* --- safety / homing TOF --- */ + float tof_safe_min_mm; + float tof_safe_max_mm; + uint8_t tof_safety_stop_samples; + float tof_homing_approach_mm; + float tof_homing_threshold_mm; + uint8_t tof_homing_confirm_samples; + float homing_timeout_s; +} CoreConfig; + +/* ------------------------------------------------------------------------- */ +/* Ingressi: HAL -> core, campionati a ogni tick a passo fisso pid_period_s. */ +/* ------------------------------------------------------------------------- */ +typedef struct { + float dt_s; /* passo dall'ultimo step (== pid_period_s) */ + float depth_m; /* profondita' fusa (rif. fondo), reale o SIM */ + float pressure_kpa; + float tof_mm; + bool tof_valid; /* true se tof_mm e' un campione fresco valido */ + long motor_pos; /* posizione corrente (step) */ + bool motor_position_known; /* homing eseguito almeno una volta */ + bool motor_busy; /* distanceToGo() != 0 */ + uint16_t battery_mv; + bool has_stored_data; /* flash log ha dati non ancora inviati */ + CoreEvent event; /* comando latched (NONE nella maggioranza dei tick) */ + const CoreConfig* cfg; +} CoreInputs; + +/* ------------------------------------------------------------------------- */ +/* Uscite: core -> HAL. Comandi motore/LED + eventi discreti da attuare. */ +/* ------------------------------------------------------------------------- */ +typedef struct { + CoreMotorMode motor_mode; + long motor_target; /* GOTO: posizione assoluta; JOG: verso = segno */ + bool motor_enable; + bool motor_zero_here;/* HAL: azzera la posizione qui (fine homing) */ + + CoreLed led; + CorePhase phase; /* fase corrente (per log/telemetria) */ + + CoreAckEvent ack_event; /* !=NONE se c'e' un ack/heartbeat da inviare */ + bool log_now; /* true se l'HAL deve scrivere un record */ + CoreLogKind log_kind; + + bool mission_active; + uint8_t completed_profiles; +} CoreOutputs; + +/* ------------------------------------------------------------------------- */ +/* Stato persistente del core tra un tick e l'altro (il "memory" del modello).*/ +/* Nessun timestamp assoluto: i tempi sono accumulati con dt (portabile). */ +/* ------------------------------------------------------------------------- */ +typedef struct { + CoreMode mode; + CorePhase phase; + CoreHomePhase home_phase; + uint8_t completed_profiles; + + /* timer accumulati (s) */ + float phase_elapsed_s; + float stable_s; /* tempo continuo entro tolleranza */ + float home_elapsed_s; + float log_elapsed_s; /* cadenza scrittura record di profilo */ + + /* stato PID */ + float pid_integral; + float pid_dfilt; + float pid_last_depth; + bool pid_has_last; + + /* memoria comando motore */ + long last_commanded_target; + bool at_extension_limit; + + /* contatori di conferma TOF */ + uint8_t tof_oor_count; /* out-of-range consecutivi (safety) */ + uint8_t home_confirm_count; /* conferme soglia homing */ + + /* flag di controllo */ + bool emergency; + bool home_after_stop; /* SAFE_STOP -> auto-recovery via HOMING */ + bool kick_done; /* pre-posizionamento discesa gia' comandato */ + bool entered; /* helper: entry-action della fase corrente eseguita */ +} CoreState; + +#ifdef __cplusplus +} +#endif + +#endif /* FLOAT_CORE_TYPES_H */ diff --git a/lib/float_core/src/float_core.c b/lib/float_core/src/float_core.c new file mode 100644 index 0000000..17647c3 --- /dev/null +++ b/lib/float_core/src/float_core.c @@ -0,0 +1,494 @@ +/* + ******************************************************************************* + * float_core.c + * Implementazione di RIFERIMENTO del control core, tick-based e senza I/O. + * Riproduce fedelmente la logica del firmware attuale (runVerticalProfiles + + * ProfileManager::measure + PIDController + MotionController::tofGuard/homing) + * ma come funzione pura float_core_step(): niente millis()/delay()/Serial, + * niente while bloccanti. I tempi si accumulano con dt. + * + * Questa e' la specifica eseguibile che il modello Simulink/Stateflow dovra' + * riprodurre in MIL; in seguito il codice generato da Embedded Coder rimpiazza + * questo file mantenendo la stessa firma. + * + * Maintainers: Colabella Davide, Benevenga Filippo — Team PoliTOcean + ******************************************************************************* + */ + +#include "float_core.h" + +/* ------------------------------------------------------------------------- */ +/* Helper numerici */ +/* ------------------------------------------------------------------------- */ +static float clampf(float v, float lo, float hi) { + if (v < lo) return lo; + if (v > hi) return hi; + return v; +} + +static long labs_l(long v) { return v < 0 ? -v : v; } + +static float fabsf_l(float v) { return v < 0.0f ? -v : v; } + +long float_core_u_to_motor_pos(const CoreConfig* cfg, float u) { + /* u=0 -> home (0); u=1 -> -usable. u cresce verso posizioni negative. */ + return -(long)(u * (float)cfg->usable_steps); +} + +float float_core_motor_pos_to_u(const CoreConfig* cfg, long position) { + if (cfg->usable_steps <= 0) return 0.0f; + float u = -(float)position / (float)cfg->usable_steps; + return clampf(u, 0.0f, 1.0f); +} + +/* ------------------------------------------------------------------------- */ +/* PID di profondita' — identico a pid.cpp ma con dt esplicito. */ +/* ------------------------------------------------------------------------- */ +static void pid_reset(CoreState* st) { + st->pid_integral = 0.0f; + st->pid_dfilt = 0.0f; + st->pid_last_depth = 0.0f; + st->pid_has_last = false; +} + +static float pid_compute(CoreState* st, const CoreConfig* cfg, + float target, float depth, float dt) { + if (dt <= 0.0f || dt > 1.0f) dt = cfg->pid_period_s; + + const float error = target - depth; + const float P = cfg->kp * error; + + const float dRaw = st->pid_has_last ? (depth - st->pid_last_depth) / dt : 0.0f; + st->pid_dfilt = cfg->alpha_d * dRaw + (1.0f - cfg->alpha_d) * st->pid_dfilt; + const float D = -cfg->kd * st->pid_dfilt; + + const float I = cfg->ki * st->pid_integral; + + const float uRaw = cfg->u_neutral + P + I + D; + const float uSat = clampf(uRaw, 0.0f, 1.0f); + + const bool satHigh = (uRaw > 1.0f); + const bool satLow = (uRaw < 0.0f); + if (!((satHigh && error > 0.0f) || (satLow && error < 0.0f))) { + st->pid_integral += error * dt; + st->pid_integral = clampf(st->pid_integral, -cfg->integral_limit, cfg->integral_limit); + } + + st->pid_last_depth = depth; + st->pid_has_last = true; + return uSat; +} + +/* ------------------------------------------------------------------------- */ +/* Utility di transizione */ +/* ------------------------------------------------------------------------- */ +static void enter_phase(CoreState* st, CorePhase phase) { + st->phase = phase; + st->entered = false; +} + +static void enter_safe_stop(CoreState* st, bool auto_recover) { + st->mode = CORE_MODE_SAFE_STOP; + st->emergency = true; + st->home_after_stop = auto_recover; + st->entered = false; +} + +static void enter_homing(CoreState* st) { + st->mode = CORE_MODE_HOMING; + st->home_phase = CORE_HOME_APPROACH; + st->home_elapsed_s = 0.0f; + st->home_confirm_count = 0; + st->entered = false; +} + +static void enter_mission(CoreState* st) { + st->mode = CORE_MODE_MISSION; + st->completed_profiles = 0; + st->emergency = false; + enter_phase(st, CORE_PHASE_PRE_DESCENT); +} + +/* ------------------------------------------------------------------------- */ +/* init */ +/* ------------------------------------------------------------------------- */ +void float_core_init(CoreState* st) { + /* azzeramento completo dello stato */ + st->mode = CORE_MODE_INIT; + st->phase = CORE_PHASE_NONE; + st->home_phase = CORE_HOME_APPROACH; + st->completed_profiles = 0; + st->phase_elapsed_s = 0.0f; + st->stable_s = 0.0f; + st->home_elapsed_s = 0.0f; + st->log_elapsed_s = 0.0f; + pid_reset(st); + st->last_commanded_target = 0; + st->at_extension_limit = false; + st->tof_oor_count = 0; + st->home_confirm_count = 0; + st->emergency = false; + st->home_after_stop = false; + st->kick_done = false; + st->entered = false; +} + +/* ------------------------------------------------------------------------- */ +/* Supervisore di safety TOF (attivo solo in MISSION). Ritorna true se ha */ +/* scatenato un emergency stop (il chiamante deve uscire subito). */ +/* ------------------------------------------------------------------------- */ +static bool safety_supervisor(const CoreInputs* in, CoreState* st, CoreOutputs* out) { + const CoreConfig* cfg = in->cfg; + if (!in->tof_valid) return false; + + const bool tooClose = in->tof_mm < cfg->tof_safe_min_mm; + const bool tooFar = in->tof_mm > cfg->tof_safe_max_mm; + + if (!tooClose && !tooFar) { + st->tof_oor_count = 0; + return false; + } + + /* conferma prima di agire: ignora glitch singoli (bolle, riflessi) */ + if (++st->tof_oor_count < cfg->tof_safety_stop_samples) { + return false; + } + st->tof_oor_count = 0; + + if (tooClose) { + /* fondo corsa esteso (tappo): STOP PULITO, inibisci ulteriore + * estensione ma NON abortire la missione */ + st->at_extension_limit = true; + st->last_commanded_target = in->motor_pos; + out->motor_mode = CORE_MOTOR_STOP; + return false; + } + + /* siringa troppo lontana dal TOF = anomalia (passi persi, verso sbagliato): + * emergency stop con auto-recovery */ + enter_safe_stop(st, true); + out->motor_mode = CORE_MOTOR_STOP; + out->motor_enable = false; + out->led = CORE_LED_ERROR; + out->log_now = true; + out->log_kind = CORE_LOG_EMERGENCY; + return true; +} + +/* ------------------------------------------------------------------------- */ +/* Una fase PID di profilo: guida la siringa verso target, accumula il tempo */ +/* di stabilita' entro tolleranza e logga periodicamente. Ritorna: */ +/* 0 = in corso, 1 = hold completato, 2 = timeout. */ +/* La sosta finale usa la stessa logica ma ignora l'esito 1 (vedi chiamante). */ +/* ------------------------------------------------------------------------- */ +static int pid_phase_run(const CoreInputs* in, CoreState* st, CoreOutputs* out, + float target, float hold_s, float timeout_s, + bool exit_on_hold) { + const CoreConfig* cfg = in->cfg; + + out->led = CORE_LED_PID_CONTROL; + out->motor_enable = true; + + if (safety_supervisor(in, st, out)) return 0; /* emergency: mode cambiato */ + + const float u = clampf(pid_compute(st, cfg, target, in->depth_m, in->dt_s), + cfg->u_min, cfg->u_max); + const long posTarget = float_core_u_to_motor_pos(cfg, u); + const long deadband = (long)(cfg->min_retarget_frac * (float)cfg->usable_steps); + const bool retreating = posTarget > in->motor_pos; + + if ((!st->at_extension_limit || retreating) && + labs_l(posTarget - st->last_commanded_target) >= deadband) { + out->motor_mode = CORE_MOTOR_GOTO; + out->motor_target = posTarget; + st->last_commanded_target = posTarget; + st->at_extension_limit = false; + } + + /* accumulo tempo entro tolleranza (equivalente all'hold check del firmware, + * ma continuo invece che quantizzato a 5 s) */ + if (fabsf_l(in->depth_m - target) < cfg->depth_tolerance_m) { + st->stable_s += in->dt_s; + } else { + st->stable_s = 0.0f; + } + + /* log periodico di profilo (~1 Hz) */ + st->log_elapsed_s += in->dt_s; + if (st->log_elapsed_s >= 1.0f) { + st->log_elapsed_s = 0.0f; + out->log_now = true; + out->log_kind = CORE_LOG_PROFILE; + } + + st->phase_elapsed_s += in->dt_s; + + if (exit_on_hold && st->stable_s >= hold_s) return 1; + if (st->phase_elapsed_s >= timeout_s) return 2; + return 0; +} + +/* Entry-action comune di una fase PID: reset PID/timer, kick opzionale. */ +static void pid_phase_enter(const CoreInputs* in, CoreState* st, CoreOutputs* out, + bool with_kick) { + const CoreConfig* cfg = in->cfg; + pid_reset(st); + st->phase_elapsed_s = 0.0f; + st->stable_s = 0.0f; + st->log_elapsed_s = 0.0f; + st->at_extension_limit = false; + st->last_commanded_target = in->motor_pos; + out->led = CORE_LED_PID_CONTROL; + out->motor_enable = true; + out->log_now = true; + out->log_kind = CORE_LOG_PHASE_START; + + if (with_kick) { + const long kick = float_core_u_to_motor_pos(cfg, cfg->descent_kick_u); + out->motor_mode = CORE_MOTOR_GOTO; + out->motor_target = kick; + st->last_commanded_target = kick; + } + st->entered = true; +} + +/* ------------------------------------------------------------------------- */ +/* HOMING */ +/* ------------------------------------------------------------------------- */ +static void step_homing(const CoreInputs* in, CoreState* st, CoreOutputs* out) { + const CoreConfig* cfg = in->cfg; + out->led = CORE_LED_HOMING; + out->motor_enable = true; + out->phase = CORE_PHASE_NONE; + + st->home_elapsed_s += in->dt_s; + if (st->home_elapsed_s > cfg->homing_timeout_s && + st->home_phase != CORE_HOME_DONE) { + out->motor_mode = CORE_MOTOR_STOP; + out->log_now = true; + out->log_kind = CORE_LOG_HOMING; + enter_safe_stop(st, false); /* homing fallito = errore fatale, no auto-recovery */ + return; + } + + switch (st->home_phase) { + case CORE_HOME_APPROACH: + /* muovi VERSO il TOF (negativo) finche' tof < approach_mm */ + out->motor_mode = CORE_MOTOR_JOG; + out->motor_target = -1; /* segno = verso */ + if (in->tof_valid && in->tof_mm < cfg->tof_homing_approach_mm) { + if (++st->home_confirm_count >= cfg->tof_homing_confirm_samples) { + st->home_confirm_count = 0; + out->motor_mode = CORE_MOTOR_STOP; + st->home_phase = CORE_HOME_RETRACT; + } + } else if (in->tof_valid) { + st->home_confirm_count = 0; + } + break; + + case CORE_HOME_RETRACT: + /* inverti (positivo, retrae) finche' tof > threshold */ + out->motor_mode = CORE_MOTOR_JOG; + out->motor_target = +1; + if (in->tof_valid && in->tof_mm > cfg->tof_homing_threshold_mm) { + if (++st->home_confirm_count >= cfg->tof_homing_confirm_samples) { + st->home_confirm_count = 0; + out->motor_mode = CORE_MOTOR_STOP; + /* backoff target = posizione attuale + margine fine corsa */ + st->last_commanded_target = in->motor_pos + cfg->endstop_margin_steps; + st->home_phase = CORE_HOME_BACKOFF; + } + } else if (in->tof_valid) { + st->home_confirm_count = 0; + } + break; + + case CORE_HOME_BACKOFF: + out->motor_mode = CORE_MOTOR_GOTO; + out->motor_target = st->last_commanded_target; + if (!in->motor_busy) { + out->motor_zero_here = true; /* HAL: setCurrentPosition(0) */ + out->log_now = true; + out->log_kind = CORE_LOG_HOMING; + st->home_phase = CORE_HOME_DONE; + } + break; + + case CORE_HOME_DONE: + default: + st->emergency = false; + st->mode = CORE_MODE_IDLE; + st->phase = CORE_PHASE_NONE; + break; + } +} + +/* ------------------------------------------------------------------------- */ +/* IDLE */ +/* ------------------------------------------------------------------------- */ +static void step_idle(const CoreInputs* in, CoreState* st, CoreOutputs* out) { + st->emergency = false; + out->motor_mode = CORE_MOTOR_HOLD; + out->motor_enable = false; + out->phase = CORE_PHASE_NONE; + out->led = in->has_stored_data ? CORE_LED_IDLE_WITH_DATA : CORE_LED_IDLE; + + /* heartbeat ~2 Hz (PERIOD_CONN_CHECK = 500 ms) */ + st->phase_elapsed_s += in->dt_s; + if (st->phase_elapsed_s >= 0.5f) { + st->phase_elapsed_s = 0.0f; + out->ack_event = in->has_stored_data ? CORE_ACK_IDLE_W_DATA : CORE_ACK_IDLE; + } + + if (in->event == CORE_EVT_GO) { + out->ack_event = CORE_ACK_GO_RECVD; + out->led = CORE_LED_COMMUNICATION; + enter_mission(st); + } else if (in->event == CORE_EVT_HOME) { + out->ack_event = CORE_ACK_HOME_RECVD; + out->led = CORE_LED_COMMUNICATION; + enter_homing(st); + } +} + +/* ------------------------------------------------------------------------- */ +/* MISSION */ +/* ------------------------------------------------------------------------- */ +static void step_mission(const CoreInputs* in, CoreState* st, CoreOutputs* out) { + const CoreConfig* cfg = in->cfg; + out->mission_active = true; + out->phase = st->phase; + + switch (st->phase) { + + case CORE_PHASE_PRE_DESCENT: + /* pacchetto di deployment pre-discesa, poi discesa profilo 1 */ + out->log_now = true; + out->log_kind = CORE_LOG_DEPLOY; + out->led = CORE_LED_PID_CONTROL; + enter_phase(st, CORE_PHASE_DESCEND_HOLD); + break; + + case CORE_PHASE_DESCEND_HOLD: + if (!st->entered) { pid_phase_enter(in, st, out, /*with_kick=*/true); break; } + { + const int r = pid_phase_run(in, st, out, cfg->descent_target_m, + cfg->hold_time_s, cfg->descent_timeout_s, + /*exit_on_hold=*/true); + if (r != 0) { + out->log_now = true; + out->log_kind = (r == 1) ? CORE_LOG_EXIT_HOLD_OK : CORE_LOG_EXIT_TIMEOUT; + enter_phase(st, CORE_PHASE_ASCEND_HOLD); + } + } + break; + + case CORE_PHASE_ASCEND_HOLD: + if (!st->entered) { pid_phase_enter(in, st, out, /*with_kick=*/false); break; } + { + const int r = pid_phase_run(in, st, out, cfg->ascent_target_bottom_m, + cfg->hold_time_s, cfg->ascent_timeout_s, + /*exit_on_hold=*/true); + if (r != 0) { + out->log_now = true; + out->log_kind = (r == 1) ? CORE_LOG_EXIT_HOLD_OK : CORE_LOG_EXIT_TIMEOUT; + st->completed_profiles++; + out->completed_profiles = st->completed_profiles; + if (st->completed_profiles < cfg->profile_count) { + enter_phase(st, CORE_PHASE_DESCEND_HOLD); /* profilo successivo */ + } else { + enter_phase(st, CORE_PHASE_SURFACE_REST); + } + } + } + break; + + case CORE_PHASE_SURFACE_REST: + if (!st->entered) { pid_phase_enter(in, st, out, /*with_kick=*/false); break; } + { + /* come una fase PID, ma NON esce all'hold: resta attiva finche' non + * scade la finestra (o arriva un remote stop, gestito globalmente) */ + const int r = pid_phase_run(in, st, out, cfg->rest_target_bottom_m, + cfg->hold_time_s, cfg->rest_window_s, + /*exit_on_hold=*/false); + if (r == 2) { /* solo timeout della finestra di sosta */ + out->log_now = true; + out->log_kind = CORE_LOG_EXIT_TIMEOUT; + enter_phase(st, CORE_PHASE_DONE); + } + } + break; + + case CORE_PHASE_DONE: + default: + out->motor_mode = CORE_MOTOR_STOP; + out->motor_enable = false; + st->mode = CORE_MODE_IDLE; + st->phase = CORE_PHASE_NONE; + st->phase_elapsed_s = 0.0f; + break; + } +} + +/* ------------------------------------------------------------------------- */ +/* SAFE_STOP */ +/* ------------------------------------------------------------------------- */ +static void step_safe_stop(const CoreInputs* in, CoreState* st, CoreOutputs* out) { + out->led = CORE_LED_ERROR; + out->motor_mode = CORE_MOTOR_STOP; + out->motor_enable = false; + out->phase = CORE_PHASE_NONE; + + if (st->home_after_stop) { + /* auto-recovery: l'homing cancella l'emergency e riporta a galleggiamento */ + st->home_after_stop = false; + enter_homing(st); + } else if (in->event == CORE_EVT_HOME) { + enter_homing(st); + } + /* altrimenti resta in SAFE_STOP: ogni GO e' ignorato finche' non si fa HOME */ +} + +/* ------------------------------------------------------------------------- */ +/* STEP principale */ +/* ------------------------------------------------------------------------- */ +void float_core_step(const CoreInputs* in, CoreState* st, CoreOutputs* out) { + /* --- default delle uscite (ogni tick riparte da qui) --- */ + out->motor_mode = CORE_MOTOR_HOLD; + out->motor_target = in->motor_pos; + out->motor_enable = false; + out->motor_zero_here = false; + out->led = CORE_LED_OFF; + out->phase = st->phase; + out->ack_event = CORE_ACK_NONE; + out->log_now = false; + out->log_kind = CORE_LOG_NONE; + out->mission_active = (st->mode == CORE_MODE_MISSION); + out->completed_profiles = st->completed_profiles; + + /* --- remote STOP: priorita' massima (tranne se gia' in SAFE_STOP) --- */ + if (in->event == CORE_EVT_STOP && st->mode != CORE_MODE_SAFE_STOP) { + enter_safe_stop(st, /*auto_recover=*/false); + out->ack_event = CORE_ACK_STOP_RECVD; + out->motor_mode = CORE_MOTOR_STOP; + out->led = CORE_LED_ERROR; + out->log_now = true; + out->log_kind = CORE_LOG_EMERGENCY; + return; + } + + switch (st->mode) { + case CORE_MODE_INIT: + out->led = CORE_LED_INIT; + enter_homing(st); /* il boot esegue l'homing prima di IDLE */ + break; + case CORE_MODE_HOMING: step_homing(in, st, out); break; + case CORE_MODE_IDLE: step_idle(in, st, out); break; + case CORE_MODE_MISSION: step_mission(in, st, out); break; + case CORE_MODE_SAFE_STOP: step_safe_stop(in, st, out); break; + default: + st->mode = CORE_MODE_IDLE; + break; + } +} diff --git a/model/README.md b/model/README.md new file mode 100644 index 0000000..769df1f --- /dev/null +++ b/model/README.md @@ -0,0 +1,63 @@ +# model/ — Modello Simulink/Stateflow del control core (Fase 1: MIL) + +Questo è il mondo Model-Based del float. Riproduce in Simulink/Stateflow il +**control core** già validato in C (`lib/float_core/float_core.c`) e in +simulazione nativa (`tools/mil/`). Il core in C resta la **specifica eseguibile +di riferimento**: il modello deve dare la stessa traccia (equivalenza MIL), e in +seguito il codice generato da Embedded Coder rimpiazzerà `float_core.c`. + +## Struttura del testbench `float_mil.slx` + +``` + ┌─────────────────────────────┐ + In (bus) │ float_core (Stateflow) │ Out (bus) + ┌──────────►│ controllore, Ts = 0.05 s ├──────────┐ + │ │ INIT/HOMING/IDLE/MISSION/ │ │ + │ │ SAFE_STOP + PID │ │ + │ └─────────────────────────────┘ │ + │ ▼ + │ ┌─────────────────────────────┐ + └───────────┤ float_plant (MATLAB Fcn) │◄─────────┘ + In │ attuatore + 2° ordine + │ Out.motor_* + (sensori) │ sensori TOF/pressione │ + └─────────────────────────────┘ +``` + +- **`float_core`** — chart Stateflow. Ingresso: bus `In` (= `CoreInputs` senza + `cfg`). Uscita: bus `Out` (= `CoreOutputs`). `cfg` è un **parametro** (struct + `P`), non un segnale. Spec completa in [STATEFLOW_CHART.md](STATEFLOW_CHART.md). +- **`float_plant`** — blocco MATLAB Function. Codice in + [plant/float_plant_fcn.m](plant/float_plant_fcn.m): incolla il contenuto nel + blocco. Riceve `Out` + `P` (plant params `PL`), restituisce il nuovo `In`. +- **Trigger** — una sorgente che, dopo l'homing (quando `Out.phase`==IDLE e il + plant è pronto), emette un impulso `event = GO` una volta. Vedi run_mil.m. + +## Ordine di costruzione (in MATLAB) + +1. **Parametri e bus**: esegui [`params/core_params.m`](params/core_params.m). + Crea nel base workspace: `P` (config del core), `PL` (config del plant), e i + bus `CoreInputsBus` / `CoreOutputsBus`. +2. **Nuovo modello** `float_mil.slx`, sample time discreto fisso `P.pid_period_s`. +3. Aggiungi il **chart Stateflow** `float_core` seguendo STATEFLOW_CHART.md. + - Imposta ingresso `In` (bus `CoreInputsBus`), uscita `Out` (`CoreOutputsBus`), + parametro `P` (Data, scope Parameter). +4. Aggiungi il blocco **MATLAB Function** `float_plant`, incolla + `plant/float_plant_fcn.m`. Parametro `PL`. +5. **Chiudi l'anello**: `float_core.Out → float_plant`, `float_plant → In → + float_core`. Usa un blocco **Unit Delay** (o Memory) sul ramo di retroazione + per rompere l'algebraic loop (In dipende da Out dello stesso tick). +6. Logga almeno `In.depth_m`, `Out.phase`, `In.motor_pos`, `Out.completed_profiles` + (Signal Logging o To Workspace: vedi run_mil.m per i nomi attesi). +7. **Esegui** [`tests/run_mil.m`](tests/run_mil.m): lancia la sim e verifica i + requisiti MATE (2.5 m, 0.88 m, sosta, no rottura superficie). + +## Roadmap dopo il MIL + +- **SIL**: Embedded Coder genera C/C++ dal chart `float_core`; test back-to-back + contro il modello E contro `lib/float_core/float_core.c` (deve coincidere). +- **PIL**: stesso codice cross-compilato sull'ESP32, plant in Simulink via seriale. +- **HIL**: core generato sull'ESP32 con driver reali (l'HAL di `src/espA_core/`). + +> Nota: gli `.m` sono scritti da riferimento ma NON eseguiti in questo ambiente. +> Al primo `run` potrebbero servire micro-aggiustamenti (nomi segnali, versione +> MATLAB). La logica è quella, validata in `tools/mil`. diff --git a/model/STATEFLOW_CHART.md b/model/STATEFLOW_CHART.md new file mode 100644 index 0000000..c9f41be --- /dev/null +++ b/model/STATEFLOW_CHART.md @@ -0,0 +1,284 @@ +# Chart Stateflow `float_core` — specifica di costruzione + +Traduzione 1:1 di `lib/float_core/src/float_core.c` in Stateflow. Costruiscilo +seguendo questo documento; la logica è già validata (`tools/mil`). Linguaggio di +azione: **MATLAB** (non C). + +## 1. Proprietà del chart +- **Update method**: `Discrete`, **Sample Time** = `P.pid_period_s` (0.05 s). +- **Action Language**: MATLAB. +- **Enable C-bit operations**: off. **Execute at initialization**: on. +- Superstate radice implicita = il chart. + +## 2. Dati del chart (Model Explorer → chart) +| Nome | Scope | Tipo | Note | +|---|---|---|---| +| `In` | Input | Bus: `CoreInputsBus` | sensori + evento | +| `Out` | Output | Bus: `CoreOutputsBus` | comandi + eventi | +| `P` | Parameter | Bus/struct (da `core_params.m`) | CoreConfig | +| `st_completed` | Local | uint8 | profili completati | +| `st_phase_t`, `st_stable_t`, `st_home_t`, `st_log_t` | Local | double | timer (s) | +| `pid_integral`, `pid_dfilt`, `pid_last_depth` | Local | double | stato PID | +| `pid_has_last` | Local | boolean | | +| `last_cmd_target` | Local | double | ultimo target motore comandato | +| `at_ext_limit` | Local | boolean | inibizione estensione (safety) | +| `tof_oor`, `home_confirm` | Local | uint8 | contatori di conferma | +| `req_safe`, `home_after` | Local | boolean | richiesta SAFE_STOP / auto-recovery | +| `phase_r` | Local | double | esito fase PID (0/1/2) | + +## 3. Codici enum (== float_core_types.h) +``` +motor_mode : HOLD=0 GOTO=1 JOG=2 STOP=3 +event : NONE=0 GO=1 HOME=2 STOP=3 +led : OFF=0 INIT=1 IDLE=2 IDLE_WD=3 ERROR=4 PROFILE=5 HOMING=6 PID=7 COMM=8 +phase : NONE=0 PRE_DESCENT=1 DESCEND_HOLD=2 ASCEND_HOLD=3 SURFACE_REST=4 DONE=5 +ack : NONE=0 GO_RECVD=1 HOME_RECVD=2 STOP_RECVD=3 IDLE=4 IDLE_W_DATA=5 +log : NONE=0 DEPLOY=1 PROFILE=2 PHASE_START=3 EXIT_HOLD_OK=4 EXIT_TIMEOUT=5 EXIT_REMOTE=6 EMERGENCY=7 HOMING=8 +``` + +## 4. Funzioni grafiche (Chart → Add → Function, linguaggio MATLAB) + +```matlab +function reset_outputs() + % Default di ogni tick: chiamala per PRIMA in ogni entry: e during:. + Out.motor_mode = int32(0); % HOLD + Out.motor_target = In.motor_pos; % HOLD mantiene il target agganciato + Out.motor_enable = false; + Out.motor_zero_here = false; + Out.led = int32(0); + Out.phase = int32(0); + Out.ack_event = int32(0); + Out.log_now = false; + Out.log_kind = int32(0); + Out.mission_active = false; + Out.completed_profiles = st_completed; +end +``` +```matlab +function pos = u_to_pos(u) + pos = int32(-round(u * double(P.usable_steps))); +end +``` +```matlab +function pid_reset() + pid_integral = 0; pid_dfilt = 0; pid_last_depth = 0; pid_has_last = false; +end +``` +```matlab +function u = pid_compute(target, depth) + dt = double(In.dt_s); + if dt <= 0 || dt > 1, dt = double(P.pid_period_s); end + e = target - depth; + P_ = double(P.kp) * e; + if pid_has_last, dRaw = (depth - pid_last_depth)/dt; else, dRaw = 0; end + pid_dfilt = double(P.alpha_d)*dRaw + (1-double(P.alpha_d))*pid_dfilt; + D_ = -double(P.kd)*pid_dfilt; + I_ = double(P.ki)*pid_integral; + uRaw = double(P.u_neutral) + P_ + I_ + D_; + u = min(1, max(0, uRaw)); + satHigh = uRaw > 1; satLow = uRaw < 0; + if ~((satHigh && e > 0) || (satLow && e < 0)) + pid_integral = pid_integral + e*dt; + pid_integral = min(double(P.integral_limit), max(-double(P.integral_limit), pid_integral)); + end + pid_last_depth = depth; pid_has_last = true; +end +``` +```matlab +function em = safety() + % Supervisore TOF (attivo solo in MISSION). Ritorna true su emergency. + em = false; + if ~In.tof_valid, return; end + tooClose = In.tof_mm < P.tof_safe_min_mm; + tooFar = In.tof_mm > P.tof_safe_max_mm; + if ~tooClose && ~tooFar, tof_oor = uint8(0); return; end + tof_oor = tof_oor + uint8(1); + if tof_oor < P.tof_safety_stop_samples, return; end + tof_oor = uint8(0); + if tooClose + at_ext_limit = true; last_cmd_target = double(In.motor_pos); + Out.motor_mode = int32(3); return; % STOP pulito, inibisci estensione + end + req_safe = true; home_after = true; % anomalia -> SAFE_STOP + auto-recovery + Out.motor_mode = int32(3); Out.motor_enable = false; + Out.led = int32(4); Out.log_now = true; Out.log_kind = int32(7); + em = true; +end +``` +```matlab +function pid_phase_enter(with_kick) + pid_reset(); + phase_r = 0; % IMPORTANTE in Stateflow: azzera l'esito fase all'ingresso, + % altrimenti la transizione uscente [phase_r~=0] scatta subito + % (ereditando l'1 della fase precedente) e la during non gira. + st_phase_t = 0; st_stable_t = 0; st_log_t = 0; + at_ext_limit = false; last_cmd_target = double(In.motor_pos); + Out.led = int32(7); Out.motor_enable = true; + if with_kick + Out.motor_mode = int32(1); % GOTO + Out.motor_target = u_to_pos(double(P.descent_kick_u)); + last_cmd_target = double(Out.motor_target); + end +end +``` +```matlab +function r = pid_phase(target, hold_s, timeout_s, exit_on_hold) + r = 0; + Out.led = int32(7); Out.motor_enable = true; + if safety(), return; end % emergency: req_safe settato + u = min(double(P.u_max), max(double(P.u_min), pid_compute(target, double(In.depth_m)))); + posTarget = double(u_to_pos(u)); + deadband = double(P.min_retarget_frac) * double(P.usable_steps); + retreating = posTarget > double(In.motor_pos); + if (~at_ext_limit || retreating) && abs(posTarget - last_cmd_target) >= deadband + Out.motor_mode = int32(1); Out.motor_target = int32(posTarget); + last_cmd_target = posTarget; at_ext_limit = false; + end + if abs(double(In.depth_m) - target) < double(P.depth_tolerance_m) + st_stable_t = st_stable_t + double(In.dt_s); + else + st_stable_t = 0; + end + st_log_t = st_log_t + double(In.dt_s); + if st_log_t >= 1.0 + st_log_t = 0; Out.log_now = true; Out.log_kind = int32(2); % PROFILE + end + st_phase_t = st_phase_t + double(In.dt_s); + if exit_on_hold && st_stable_t >= hold_s, r = 1; return; end + if st_phase_t >= timeout_s, r = 2; end +end +``` + +## 5. Gerarchia degli stati + +Struttura: un superstato **RUN** contiene INIT/HOMING/IDLE/MISSION; **SAFE_STOP** +è fuori. Due transizioni dal bordo di RUN gestiscono lo stop globale. + +``` +float_core +├─ RUN +│ ├─ INIT (default di RUN) +│ ├─ HOMING ─ H_APPROACH(def) · H_RETRACT · H_BACKOFF +│ ├─ IDLE +│ └─ MISSION ─ M_PRE_DESCEND(def) · M_DESCEND · M_ASCEND · M_REST +└─ SAFE_STOP +``` + +### Transizioni globali dal bordo di RUN (massima priorità) +``` +RUN --[In.event==3]--> SAFE_STOP + { Out.ack_event=int32(3); Out.motor_mode=int32(3); Out.led=int32(4); + Out.log_now=true; Out.log_kind=int32(7); home_after=false; } +RUN --[req_safe]--> SAFE_STOP { req_safe=false; } +``` + +### INIT +``` +during: reset_outputs(); Out.led=int32(1); +INIT --> HOMING { st_home_t=0; home_confirm=uint8(0); } % transizione incondizionata +``` + +### HOMING (comune a tutte le foglie) +Metti nel **during del superstato HOMING**: +``` +during: reset_outputs(); Out.led=int32(6); Out.motor_enable=true; + st_home_t = st_home_t + double(In.dt_s); + if st_home_t > P.homing_timeout_s + req_safe=true; home_after=false; + Out.motor_mode=int32(3); Out.log_now=true; Out.log_kind=int32(8); + end +``` +Foglie: +``` +H_APPROACH during: + Out.motor_mode=int32(2); Out.motor_target=int32(-1); % JOG verso il TOF + if In.tof_valid + if In.tof_mm < P.tof_homing_approach_mm, home_confirm=home_confirm+uint8(1); + else, home_confirm=uint8(0); end + end +H_APPROACH --[home_confirm >= P.tof_homing_confirm_samples]--> H_RETRACT + { home_confirm=uint8(0); Out.motor_mode=int32(3); } + +H_RETRACT during: + Out.motor_mode=int32(2); Out.motor_target=int32(1); % JOG via dal TOF + if In.tof_valid + if In.tof_mm > P.tof_homing_threshold_mm, home_confirm=home_confirm+uint8(1); + else, home_confirm=uint8(0); end + end +H_RETRACT --[home_confirm >= P.tof_homing_confirm_samples]--> H_BACKOFF + { home_confirm=uint8(0); + last_cmd_target=double(In.motor_pos)+double(P.endstop_margin_steps); + Out.motor_mode=int32(3); } + +H_BACKOFF during: + Out.motor_mode=int32(1); Out.motor_target=int32(last_cmd_target); % GOTO backoff +H_BACKOFF --[~In.motor_busy]--> IDLE + { Out.motor_zero_here=true; Out.log_now=true; Out.log_kind=int32(8); } +``` + +### IDLE +``` +entry: req_safe=false; +during: reset_outputs(); + if In.has_stored_data, Out.led=int32(3); else, Out.led=int32(2); end + st_phase_t = st_phase_t + double(In.dt_s); + if st_phase_t >= 0.5 + st_phase_t = 0; + if In.has_stored_data, Out.ack_event=int32(5); else, Out.ack_event=int32(4); end + end +IDLE --[In.event==1]--> MISSION + { Out.ack_event=int32(1); Out.led=int32(8); st_completed=uint8(0); } +IDLE --[In.event==2]--> HOMING + { Out.ack_event=int32(2); Out.led=int32(8); st_home_t=0; home_confirm=uint8(0); } +``` + +### MISSION +``` +M_PRE_DESCEND during: + reset_outputs(); Out.mission_active=true; Out.phase=int32(1); + Out.led=int32(7); Out.log_now=true; Out.log_kind=int32(1); % DEPLOY +M_PRE_DESCEND --[after(1,tick)]--> M_DESCEND + +M_DESCEND entry: pid_phase_enter(true); +M_DESCEND during: + reset_outputs(); Out.mission_active=true; Out.phase=int32(2); + phase_r = pid_phase(double(P.descent_target_m), double(P.hold_time_s), ... + double(P.descent_timeout_s), true); +M_DESCEND --[phase_r~=0]--> M_ASCEND + +M_ASCEND entry: pid_phase_enter(false); +M_ASCEND during: + reset_outputs(); Out.mission_active=true; Out.phase=int32(3); + phase_r = pid_phase(double(P.ascent_target_bottom_m), double(P.hold_time_s), ... + double(P.ascent_timeout_s), true); +M_ASCEND --[phase_r~=0 && (st_completed+1) < P.profile_count]--> M_DESCEND + { st_completed = st_completed + uint8(1); } +M_ASCEND --[phase_r~=0]--> M_REST + { st_completed = st_completed + uint8(1); } + +M_REST entry: pid_phase_enter(false); +M_REST during: + reset_outputs(); Out.mission_active=true; Out.phase=int32(4); + phase_r = pid_phase(double(P.rest_target_bottom_m), double(P.hold_time_s), ... + double(P.rest_window_s), false); +M_REST --[phase_r==2]--> IDLE +``` + +### SAFE_STOP +``` +during: reset_outputs(); Out.led=int32(4); Out.motor_mode=int32(3); Out.motor_enable=false; +SAFE_STOP --[home_after || In.event==2]--> HOMING + { home_after=false; st_home_t=0; home_confirm=uint8(0); } +``` + +## 6. Divergenze volute dal riferimento C (accettabili in MIL) +- **Granularità del flash-log**: il modello emette solo `DEPLOY` (pre-discesa) e + `PROFILE` (~1 Hz). I record diagnostici `PHASE_START`/`EXIT_*` del C sono + omessi (non influenzano il controllo, solo il contenuto del log). Se li vuoi, + aggiungi stati-ponte da 1 tick sulle transizioni di fase. +- **`cfg`**: nel C è un puntatore dentro `CoreInputs`; qui è il parametro `P`. + In SIL l'interfaccia del codice generato avrà `P` come argomento separato: si + adatta l'harness back-to-back di conseguenza. +- **`after(1,tick)`** in `M_PRE_DESCEND`: fa dimorare la fase 1 tick per emettere + il pacchetto di deployment una sola volta (equivalente all'entry-once del C). +``` diff --git a/model/diagrams/float_core_homing.svg b/model/diagrams/float_core_homing.svg new file mode 100644 index 0000000..7328009 --- /dev/null +++ b/model/diagrams/float_core_homing.svg @@ -0,0 +1,53 @@ + + HOMING - sottostati + Sequenza APPROACH, RETRACT, BACKOFF verso IDLE, con timeout verso SAFE_STOP. + + + + + + + HOMING ▸ (motore/TOF reali) + + + + SAFE_STOP + + [st_home_t > homing_timeout] + + + + + + + + + H_APPROACH + JOG − (verso TOF) + + + H_RETRACT + JOG + (via dal TOF) + + + H_BACKOFF + GOTO pos+margine + + + IDLE + + + + + + + + + + [tof<approach ×2] + {STOP} + [tof>thresh ×2] + [~motor_busy] + {zero pos} + + diff --git a/model/diagrams/float_core_mission.svg b/model/diagrams/float_core_mission.svg new file mode 100644 index 0000000..b2bd344 --- /dev/null +++ b/model/diagrams/float_core_mission.svg @@ -0,0 +1,58 @@ + + MISSION - sottostati + Sequenza PRE_DESCEND, DESCEND, ASCEND, REST verso IDLE, con loop dei profili. + + + + + + MISSION ▸ + safety() TOF attivo in ogni fase PID → anomalia = SAFE_STOP (auto-recovery) + + + + + + + + [phase_r≠0 && completati < count] {completati++} + + + + + M_PRE_DESCEND + log deployment + + + M_DESCEND + PID→2.5 m +hold + (kick) + + + M_ASCEND + PID→0.88 m +hold + + + M_REST + PID→0.63 m + esce solo a timeout + + + IDLE + + + + + + + + + + + [after 1 tick] + [phase_r≠0] + [phase_r≠0 && + compl.==count] + [phase_r==2] + + diff --git a/model/diagrams/float_core_overview.svg b/model/diagrams/float_core_overview.svg new file mode 100644 index 0000000..e547803 --- /dev/null +++ b/model/diagrams/float_core_overview.svg @@ -0,0 +1,66 @@ + + float_core - livello superiore + Superstato RUN con INIT, HOMING, IDLE, MISSION e lo stato SAFE_STOP esterno, con le transizioni. + + + + + + + float_core — chart Stateflow (livello superiore) + Ts = 0.05 s · un tick = un passo del modello (= float_core_step) + + + + RUN + + + + + + + + + INIT + + + + HOMING ▸ + + + IDLE + + + + MISSION ▸ + + + SAFE_STOP + motore STOP · LED rosso + + + + + + + + + + + + + + + + + [homing ok] + [event==GO] + [mission done] + [event==HOME] + [event==STOP] + [req_safe] + + + [home_after || event==HOME] (auto-recovery) + + diff --git a/model/float_core_ref.slx b/model/float_core_ref.slx new file mode 100644 index 0000000000000000000000000000000000000000..0a5642e536fba1a1638f9a8cedcb2f038c54c3a2 GIT binary patch literal 146798 zcmeFacUY6#(msqEd%@lX5y1ik5=bDSD@{P@y&96xn<1eWdj|vr6lnrVlOk2RiV6Zs z7o@86E?xR>9u<%KY>(%>*Y{of{ByVnraWcVtXVVn+%s7!D=;qK#_;Xu0K=;5&WE&D zF)$zxW_Z)y+?sA>Zez)7?`%(_SlfU9*9gPoWNjrP4u4o3C{%lMTN_a}3=f)(m10A( zC7aurin7sZCMZ0cxY$||wXf%jtz~5uQD#*nT2n;XPTG=)G&qY*Ob>jYWx0v!m1OL6=LK{M}g8P%?BI(pOD=S1haA(mOv73QhPi zsPD(A6HVE~)MzkwCRVnNtpB(WEB{YI{aOQjztaE)kIWMq_gxJb0o-pifG7NPv+t5E z$&Xkho0z?ck-Z7s=C6k-6RB_jm>%d#`4=Dm5FKn8!^-yyA%18|iehJF>ugQ2p`EfN z!#!Ku(=J9W=s9CH2(3{&!&XI{Ie-cJuzxN%u@0$EsODD~3DF3w9mwe5V>_07CH7W^~?N@(x5GGLQ7l%mM z)4m;oCLA=k`Ei5@@6w$_mY)1{-M`ND4}ZNR4c`wl`0>$6qA|q^)`lL;!>^ps4{1RZ zMA~0m;)k>t+|eF;U!wQFfDzc=_yHUNf)NIf=Vle)L&6gJSHWP(Z26Wj#7_$l ze2x^6Mnqh*8V!aFK|qLA(w0gAVIWq+%U}48zeuh|H~wZxORfw8FOg$^2 z_5ELwhX4VIy8?jK1aXM#<>OzP&Ywo|W0Xq@{^_%m=EhVa)mhD!P9;$wa**j@LaeG3 z6Hzu}vb7T#G$8b=V~qX^h@`1hTPg&?pAZI*I+bW+Zv}XRhqm*~w|^1Cmty9h zVmNX-(G)a7Rx>xXAtKHiEbYg@K%RdS(BX)07WHkG?7y!3rRn%Nj{U_J;XeNqQ~}+7 z#pAyVsDk*Vg@nia`-SuiX!kD{5+cvPTuHwOiA#3;uYwHr_wnUdejb$hn;R|pxSx#V zTWI<1jeZ$6|5ZSlKTSnoMF@V_mKO3U;2pqMm}D8EeX*0pI@Iex2Ju* zA09qgy5I7zNB#MJOZP=SLhgmUR{I_q5P6aN{_cEa42ayw&n6g=+R1O@yaTV7&RhEY z=W+gg-v5{T{Ga8M0lj^P3&>axFwDA&tbPX~u7Q_Z;T^s2KE?=fMD_*FvI*KRA^2p-qp(Em7}vS{vj?PHVU`r2hGs>hq})sIcix8Bt1 z9~qQS=vd)?>s`kH=Ns+wG7ZOrIZegG_RECpY+<|e$8Fr5G~CL-X2q55-5Eu zHH&)+>{z6{_8hlh-V=ZOo#oZ^@$HVRJCqL;zD-rq*WOz7Ld8n7fqaX-{b*o{Iwrd* zPC`W;kl_4mbVX<6^eas4QNdb%CT;~+jp9j>mmEF$k*GU{iJBT*P+_#O9$34 z?NZ+mglm1HwX?4z)4eCyk>-x`hh-kRqexe}GxWDlHp`A&XPYI8P4M;>}>ZhW|7@5hRS{(FWgd!$fr_8#*$k9R-i&*I8SDqET?b}hLt^-u!KM z-)l+PHgDk#nyG84ThBm9-;NWMeaW3#%w<0ov%ydX{r`k0pJpS}h z@{Ze_V-wcb7Wq?Z%u0g|ZX7>c7I(J%Y;(<{TM5TwIc9mr!l@r-!>gRt3f`HRUyxGB z)MLJM;qwX2{`zOwm)kTirJWLZ>v==#^^h((H_7zkHO;t^)~Yw^@q5iOW;Aj>`X;rG ztXHt`eyPr0r+{u+*-*7}XN_U}M8Gh)m^nl2AeLXu48M#TYIY&{)iAYvxx@aRh4t$r z{ly*bH2iU0=xmq(lLkFo|DbRvVBmEl4;ibHB~zTLyoDvi(%CjX*Sk}+(xm&Dp>vVO z^n9v7_vVC_zV_Cgf1ajiWQGTHn5LKD%-CU?e(-JI=F1o%x5(y1Q=lM_mHT!65^I|* z+R>XPOsn|f>Ffkg6aZKICS&gr4z2ip#u7gY8w))&|n{e)%WvXAz$lUI(L z_p_>A-lktG&MKG%kcmp6h$mw)o&l=mA;L0}jS78#+c? z3;68Dq}ILr@|3(oNO6^_2;I1K%f^m>)J8%~SIjQgL`ScZF(c z#V+>3t+p^AZFk)=&Ru=g7O}6y*eWEl1ZWLq3oA!s44S7|hD$y$8P7Bve9Nh;$lOys z6Zkppfj}a!ZBotDZqfJa2di-hBJc63SY^L^xh>T?j21KRUMrM*BQ?FXlJDmH&CJ`H zBjcU=gM>!^Ehe!J3!+i2d;dIbJx|FJm3A9#oN;yC$7$6 zl9|%K3jIP%$@jvgvsv01hmI6I-l?IVnSkjO-;2JGiD!Y<#p8` z-$>PXTbRvLJsYBIJY#r}m$P*+%Q){NL&h$fV6PVOKM!6<=h~ZBFlZDE{^-wxw_jrU zc)wY%H4&h-4do{Vlvxy^H%U(|ti{agIz({XE_uH_r{qv{KVQjK1#MoeMfIG!>F%1& z=8s`TqWe5a%7XC&_I&6`#v}3q6=JBB1w8wCU6q^BZ+21H7&jU#7sm;Q96jy7Mc7Dt z4aehU^}5dW!HH2hzp3IoWM0bAzpi7752!<8J)(~L?%cvRw#dH% zb!_MrTAyjLaZ+U0DZiOvt`at!LEm*(rYWbHk#)_48#kVv))QR4ZwCJay=N}RZGdUD zs@1N6EsT5hbv7v)&_%|KZv>L5auP?6tyg>y?a0=9cv7?P4!);b*u?fmm~%9F_PzJX z*@wiNqq-!iVa?TNj?U&6KNH3z$N`ovteblcuomnPq2 z!u8I z+tkq$ZTFupm<2!Ef^(%_tJ>{ln~_iQO6RaP3y+ zjlx=+Z=#wmL<@{w9PLuvD;Fvr{D;FsWd%L{FW!gG{1 zT(U5I$$e zI?j(tv3C6Hv`}sx`?fB{E>#u;l8)SF>0U&FP`rP6!Z`f{uBFI`G=6+O42&+VYXrlzcSvVbzF@+A~SH-3k*W z#+x^9q&~tGa=mnTvAS6%wp}#fn%=$0($j~ZMD*pJN`9Az_1e^UDE`C*))afo{_~E$ z>XR)cqOlFdmY=a17{Yp8QB|ctgOL+F<@+~PxhFZ#Sf1i#mOOCg7UG%LT(-=1MSG(k>cW*tsD(okHru6zDTcIlIj5MR- zqEGyX6@MPCz1@KmMKDZ3nD`^V8?GwFibAvpX6ajmqI$-FetBZ_tBZSlDVTY5!-xMs+XUS(QyQtQxJ`0&)`P2U4W3 z6@Fy7cVKyO>xzIJft($o_Xd}{Rt3b!v~7rHXCGOiI(_T;35gOz#XjdRREMc%#?y=+ z*qKgjS~+%vDaz^f=aNv_DV;nY6Q|}-%@h9p@pNgYcRM2eC3Y`7Fbe8)JG|}Ia;uW( zLi^6B8kWx4gk@MuL|v@j&aqyplJv!BdgGEJVAPNY_0-2U`v@~IAdekCD}rW5w>GvR znp^SM*_d8aR+QbacF$TkV1wKVDOJe+#*xQrfXxhL@7k`zKWpqx=-4wb?A(GpmYIfE z!%?gZa#F|Cok9j%oSoDtpRz`5CzH}?QQT`)H%m*c%uH^!R^IV@;hnP5XqgJCV?_aW!Q+zusdn)>m2dGokV$I?G zLh?0Of_Zf_p~-$4W6Zpv zSh!$o+G-7to*CyN8I<;y%r0Ihwc)&}`>JYcIJYl$2A=a1@~2J(RCN1;yk(u&rv^T4 zlCC_5PqEf}c3OGsp_7$^S#3!Jk50}HwhGhUXav1(A9>+AX~&SDnOxU0B^n*no&V6A zS=i#$&E2JCW&UuX`hJRr$fxrWj2M%qOnC(boK*w*%^i%SmXY(_Ln6~x{rvquiCV@! z*to;<(G8wl89zz27)6cxB!hrEcaDTt9L{PwRl8XY26vz-bNlM$w9WvnKBL8%pjQip zt7v1D{m$*SFWxQY9h+Ztx}=$`6SLFEP33SX%@2NV-?}C7?cDpP(Xm(dduUI!IEEim zayu@#KAW>O?V^jay80vkMY}+ULC&hO#f2=2utuD*Tyb}*ZfFx=}wZ%nvF_uEk42))4V!#Wo}f;{++P=4;WhP$RpxocL?BA9V`{c@YF4MyI~EjvJ0Kt+?#UDF z_wVnHgl&IuLeU|UJuW3h8FUj!*-VpSKDH{7J1;(<>Nykh$8Mp0qjt6D%-G<($NaVT z%QCHX0v{+JdC||HB3pUxi10Qa@l5Vd8HbxPH@>c4W_$w#bvRPsn}*gsx{rF^Tl+(v z2ZIIbL-H-=wMRCkbc_t-+n%*<8r3`Gb+!9eO}l@S#%$Kft~*ScFRr7D9@BDe(qd^`jrUA$IlbCN`ziKe!w|Dz|<>^JMj29Cm77Z6bplbW|n03 zqoQ?DM{F)Bf9qSMlAYL?IEt@0#c`$RKu{S+ zlSAq2-5qD2z-3O>S=Z70iv6s$n3UVca{AqbCytt}*giv9Bf9N++cJK|7&RNmN3Eq6 zGp;_1V;3Yh9$U5NT|u(MuCO65&oTEbyFVC@6l_{HOtR_}a~sO=ielGS#cqg z^L#*4rKn;dHO;c1RFteU>N44SE%EvOMePs`5)kLJN;`=o&xvm`A!TLj{`KVGS#nsN7J?(;ao0T2MaSq&nbpI$`$V&7PKx zH+rs$o@c@;uURAJoWI7^z;MfvzzRy){ss=Y!kfH9gxFbdvY!I?oD~5+MyP^H9l+4b#f2Sl&lZ0fvK-|xLXcaPFBt|PY0 zK6+!bjDD+yz8&>|9nZ01YJ_>gYT1C7<4RT&i>$<~=xBAl>wjqQD2xgP-2R|)w{WKE zOo^(4OsPkK`C=+9uQ=qA)q5r%t6YjH~(YySLN%d|OGfzl+h4u&_!(h51 zc`6v2NWELUUG=F^eD(Fd3m?x6dkSZkb+&fDb;wZ3-Z1|`#>_r9bnQwmrDtnDeArBj zI7iLamsgNWqUxkXO0itgIu$I?QlX?Vs&!lb`ogVP79ZJTWvT{&k6Ubltn}7ivLb|f zQI0a3Jpmt~hBMmUKjs{*d?ui*nMIDJ-Y{q}P*_GG9|RfNv%fFX_E`kVbnfk-s=_0x zY!QSX?N9C=yjI6o^~pv}>#u)$^8Tg?#y&=c9*Oc77E)>2v2Ob^Q7is9$EZK0#S_HO zwUW8H+t&|SRz7GLbIC7#{aVGwCP`6z zo)&8VmeAMmI0_t4x!&fq2(7SGOoZp+Y}+1wlbf24v=7Mj)yDZNii}8Qm{lFu(b37k z6Qstw(!hsKPfyQ)pU2vFofMsFwzqiq=(FrV=?%KP7 zSx7zHZS9bHpfJ65pONwVhQsT#?RqS4*qo`#OC&M_|I zJ@-30$Q*|baUM8u%rIHYCQEpT7tIpGG?y}Tv ztCy#nRk5{D%TS15$R&zdTu-{sjH}4V$mE$m*4-;$RcV==|1c)pLs#+&Q(bMG+Qf$o z8~eVr`}oLlMHU<=P36v?=}YK)mv~RWra4w8&7`+Kx$vErU~9t%5|7;O-Me$#*Gk!{nHEYI*ga4hV6VsY**Yf91Aw4`!_7m$OG(~z+uDC+LryS~FsnTg2-GxGc0 zKA%Gdk*Jr&xoxeqeV_$9Kfl_WdxG_GF=6pZ2Dvg~r$XuPbY{kDl;}kp1kL8YjOZ46 zmHL%+6qvB{CbP~=QX5R^x;;6!r7ML4m*4YH5+SI$%>%sodz`D%NzG2wq@Wx zQKw=pU3=c9DRT#pe(cAhE0&cJ$ASf{gGCD0H||@#9BbW(6`kpcq6&rf)g=V!X4xc0 zc?)q4g%=bxRSH@)L;$MS*(+#MdrGh+TF*9L1MGWr+ASDEx;#2Migg(`>+I^X`268n zsA=`dt3JprIr>ar@!PyOFC{oFo9#JF?Sq9f#i` z3*g$;!|{+bAl9IpVetS!@+B*ray~y_NB{EiN`GgNk!BE& z!Ka02b<>s{y32=it7zj@vX*Z{2*}Fow8PUXM5lTH4OiYis#cprx11PwucjDdpS%V;;qhGluI4dmkYSzktYp@lQyqgc>zgRhO)a^5Or%9FI36y@2ytg z)krqXPq%u{*V)U~JiY&2Z&L1% zYFQwvE>0~bHl2;r(>;*V8>nZaa{TytLzmtMC1>Ih5SimRoCH~nnr+_5WkvkQiz}J! z$li8TTv7eeoS$u5 z)HF7ZWN{y_f;r;!Tr-?M71rv~k7{upMqIJ4dUmw1-3N&`p~6~P`e&rELz;f$FSaTj z0f!W*QD_>B*u@TB!$(cFg$6fv@&>X`yNP)*3u{7x2^tAfS67dQjHX0!;|8K9L#kYR*PH0R)f3av*6{PgN8G?fZ&0irJ#nBwC+E* ziluV1?7sWpzYP7ht-VBh%@Jf)eg~uCPW?Jadc8q>Z0yxiKXvdc2gCO`4mNVg^|j{b zg3($b25{-prN#`)Ajtm!xA)IWuJ|+zBTbA+(aJC%(>xV6nHT6jsgd7oNZPi3CFN3| zE$2~GmAH|MhG3hJZjgbyzs+uXp-KK{KY4it3lHJCxea4e^9eZ@TQaQ!f`a-V7qis7 zHw{k&kE!72Fxa?VhtC!ysqLCBm>sH_n;z3pQP~{OGz?0H%rr`YC!2lC05TZs#`LQ3 z-kPZD>ndliW_j4Rh)oiVcqJ*o}2 z=1+j9m*v^ZN4^SnHvV#_kyKbTBz~H?d#x%`7W<7Bg?dtw4W^=O_IU(#z41&7>qORzy@v3_9nxC|I!{WnpqBsP~iJAUnE!P(1Bheeh=agZ{H82%BvqkoO zF+4Q9i{{ z;M(HS)6+{cH)m!C55nBmfkO0)QtHXut@E^^iY)`EohodQUZ-;(DHK zk0HcDMVM$5b-ybh^qk>>S**iALkVSmb}Goa$+vE3;yswb47kSFkdS@!p=KSwafqju zFyDPuQL^rB0qngK^+xs84;1KQZv^{q@D?a*YHF$vnJR%DsTdlDwR$eP%LN>^pDx$NC6M*(1+_A~=Q@^t3kS zXkWXw#*ee2%I@P?v6+qwS^@r{am1{f?=E|-sPZ^*CB5%Cj1#d@h`5|MoQB`{!t8K= z$w43W{&$H{eB%Cxl{8gUe8Zf(cIS_)ZV;=iyAUSoYBAWDF*DsA4*3bhP)L^_*cMDb zY}Hd4KGv2(ya1t)S!a(CfJsH+!SoWM&u7P=)*fadbXD5@@_e$RSVUp#^w|Qv$GSTWUGjCo z${QVrlE8jdR7R47+H6SbHKOkqakl3Kx@@aI->>hvGbuYXCiKSMe*A-bmU;b9OYpRh zr(FCTYgNMVgFTDmuTah?Sle(4{)9C5S`QNL)@n8FA{ciy_p!Ao^-YY5#T zQEx+qxda6ThmzZ8!YnL|h*{hU;6p6{6Lz@l_nbRZyJLaf{LAN$Gas+)r?qIt`Q#A@F*E-%hJlIpKYIZN5IbF zgtVWPj;8Xk$LGhIk95=O?MGk781;#VyYa}Cngu`1%M4KP$$pbsH_|XU>eO5(7Cz$9 zA`o48!>&Pz(76qPpa3Qt6Lz|K7__|=>gRs&;DPkudNZI z1)iw~t*Zjx=es%vaC92vI=rO4CTOavs;cYj@0%9ND?OqaK2wS3HY%8la448=^AYcl zD;};-*}F<1{$`Q6C~sw1a^C&OPX_Xqe!5EbjVbRl4WbGY>5H>+ezgm+eLgxi!C?nZPs7jP_xS?hxoDsl?lBWLl+({6C&cao ziCK;oFGT-@koJ7B^M=C;Jp=-wG0)k)qu=CBu*P)bHoYtx4I3Mqj(&(YWxcdl^EG^G zMcP|@ImH8%Lhu+qn_~t~^m#Vyc9ZL{yi#=E#z090y{qiB8oC0SNn`y?vq4iq2 zx(5oyS(_Yc&9r;`{8Z&ssj8orCEp7UhGA8;(H?hO7T0>#%fMQHs1%;0BsML@6J(ie zXa&wuF{|ko_yO}|L#%mqbg8R?foHgD{1nG{b8|Ci!VZL61W1%rFnnm}-6NeS#ZV#X zg0~eFT7WaOk6%GRp-PmDdC9E<06W1k&nLGv5ARRRpQ2moovNw1edj~MsQt&#Jnpji zuE|JldB=V`b#JkqU9?7OuH}MY9rs~9zx1?N?Yp~ACFBofT3@mVyQemGH*L@;MzpZC zjqdr(_4tFlJdqY~{Cluwx0-FT)~;I@5E`1V#k&Cb<<%LUC=zgjdqkYRuFfa9ey^&{PdHL5w z(Pe(3ega%hKjdKE>#0SxPhvzXwmcF4$T136v4C)*@phYrR-KoZ;r{TRQ#B9lW#y_< zD+-b(T`b=%C|weEh*v8nF*)74U-@i4tj%rglsjAzl&jWkR6HSCC*^d_4S;hF827=$ z2hRwFBGIW}VWz%2-nOkk_lezol?#(rt$8|-8dd|E0e&iNJcr(Y``$f0!sPj~I=f=( za8p(?tPCX0lT!ugIWsk!3{dagl)=k4=U1$wA!xefAlFo+LB30dX0l--6SGjspabv< zKn#?%k9_W(+f*5^!6gc?dScJ0qyg(Du^lai^_bq-;OA$utv)cwRjXB$$4*X$2dhud zEAYne<2^WcSy$!p)Khg~haGiI?LxOZIy=n)lDJ+HDet7lX(byXeizK+pqxd0QYe$~ zKnf$1e9fKm5P}+nDW5!EZr$_~9)(7LM2~-s+#V2~j{unrRfik;0+6PN0)xQG{b=E{ z_bb+2>-!)Hd*XSe(H{az<+vhMLG?dIM$iokJR&*^msl4Uckoby>Y zIHoOV=L01osL_ceAA-E(N~6fE)@kJ_Z)dmgtogYR&;H({X|&O6p%XFd-UdXMyrzJc z&ncyAJ<`psf86)6(EQb%hAdG@F{jTT=EGQOfh=ye=ki?qIHfG-^7+{+z$cEoG1VqS zB9VMLBgJ{F>nMSsQ_SqBj||7s|IVFQh^yN6UFB7a3sd590pU3a;{Zq%H=}VhtHt47 zW0vjy-R}`HoK)-$333>4D?H&9F&8f`2hi*+!gAgwqV2MX$cvG48jdA(1xacx70f-8 z#u(g5(thSdL(zqZRr^CD{1t~|MwFE5^QLWk-}myyR!nR4mL51rCmZg~N4LjuKJIOs z;&z{WDVQ^ubS1q*Jly=%7;k)Ac#@{3$t&LaShwcgr?@t{28&K@_wqE!aTsWhdo@0L zEim}9Nco@x7cZ|;;o|)8M2Ejpe0n}jP(V~!j>8i&N@46y2)!OW(Pv1!OYdrCSUvH_{#wzF*bLqA&sj}WH8sD9S6rU; zTNfRDGItJDUTqq#Me=C;(l1lFEyG=Ff`YV;VEWAv+NuZ)Y&0D) zJk^TV%+75Y%Fpz7717{TIPryEJ~a37QuMq0hqsJDg5955#-lbnIY|F_X{Su$ z_6d)DIqKPIl2mHy!&CPj0;ejQ?>hC4D>*MxO_3lC>@`eTO~kPaUzw)yO3A*q!a^w0 z-VP0Ao?6|QnK;;#WeG5}uk!wdmFss!0Ie2Z`Z)u!QyMG`bGXwUAzCb?SaCORt_P5Y z_{D42u8q3nTZykaYXRT|+#AleRlh=H>?W9QEI|9djC#Ww$jlFb|3eC#Kz~Msxlih} z&}vT2KyDCl?_NTAdHECD>U3@eKuC4aS%Ns3p7ZN?DxaKbza19FlKnQ7QieVKl;J^i zv{^$65%lJ*3kDI8tG@fC1S?WNOw@jS98~m9NqjyUNrbhG+$Ri`fGWN_IyRO8K78fl zZZjaVLHO*71FPF$og27~fKe-G8PEfS9ETo3LIqVt>Bz9);LIMe=WB_=YYrdK`9B{S0%fInNdY@yYv$eOq2Ip_2~yhj_%sJAG+ebi;(adCV|9_%@w;_U2PT9)83 z*Xe{PlMaS$u`oAn0pREBk}|K60iv$7yj&f~Td1KU#Jx|kWcg#krlgRQr)3w?d=~4c zS5C!^HheBUQzUuch|xypz@_6jqtr7(%yezhyyCtwD1EbU z*6Xp$4dUlIK8-lthG+_!KXp&w6mR~xACk}Uzr{;Jv<*Q_(7r-c3l^*fB(WE+ML^Yr zo4XeL;v=N!jhJU3j{yKa_E67-X`%i{X}5iS?}O>bgA?^t(sdnrygI@Izt4LQ?{2N) z{bo*0hAJ7`fh>xR@~(!oIT@ju0m&eh2Sw2d^$okw63BufUSWQC^XAPb$u=9)xD^n} z0;r3KOY3&_Azo>;vkc0)5BigG6-8afua2`dCSEP&oDzj<4HQ?9N-rQ#V3UIRO}!E7 z30#y&>h+dh8Y0yhpB(F^#%%kTKh8;XqymAZA1dtVeNlviLpm-l&X;*FG#dnjgggQV zA#;K1;(=if*%aCz zTS(XLKj`;_Ihn$?4%pc%c7ex#BmcX*oFw%YC^9*hCo!Q&==W z&E1Dg8yXahR^I+XKPvi8N&U-C=7s1d-5;jctXZS+%3`B93zHIKWO1j+XA}IDBAxEK zc{6P@>6Vw{2Q9xGyAqCuK9RCiHYLR`ZTFRr3|&dSBu+=0XNpjUyv7wHe)R3zO6co0EHvmESH_*_+|K!1S_BX+COuM$Ksf z&iTn8Muw-hRoCZ9?Y!`nFf+75&(j0X?reS$pEUWXkW|*!b=vz}GDhaQB%^HQvRa+W z)NQGfqpOb3iqN=srC87cb0k=!84XaIRu4G}n>%SYl=3@`WL}osn(W3v-?8>mM?%sI zhc}85s{WYwTvO~x3oag>5skx)hoFjIekVOBSADFwreXhB*x_&@uOn7}VB0GA65bw4 zCB%z_=4Smw-3(}D`6FTvK$X{eeWdGC3~UmT^+&X_ArUfwDNmxxU?a|;DH@NMr^BtA zW<^;V230m>-4|wZ%0E3(l&y8W&wB`|w7xrXfhmIc*+q18c7Aq4T<*&D6ovTrM8nB* zjxMB)j{8b)dQ8-JtUt-Xr05=!(A-Wh8&1aeG_6@ax5|AlFltVV679tD1O&|WHD{+~ z+O)*`9+Zs-+l68=;xldXCp2LNzjWPkrX+2{y*{6!k5KY9xAQL^Yr?G`nVx6B5zDvC z%uMu8Ko?W2=i-7cfERcf1J%2HsFk^Vx^@_0!U6fcaury{aCI%|JWHm1w<0J(9{2#i z=-lA~z2bfLHy1{HJbPXr4o4PCNcW~I-2OY7Pr7htJQF)(_VlJsCyRjbi>oKfIpV-$ z3Qg3iA$7L7xjFpX-3$y>q~#4#AAjzlgj!cC zh_uc$4Qe`QX08>m-YzYU-DcIw8Any-f?koB8o7qebT4XvaOEBF1D#mQ9nSt&ra8s) zkg6nbqrPjd9zrevyiG zFtDo{>gfvmd1qBe#C-^A=`r=KUnEevSfAzjRPnXvy?c62%aJe+sVX%!L*+Uh+w|}B^&}2VZYA1C0l=spAS(<;jy16kUSIS3ZT0hTpjY_I>nS%a47wJ~-DUSa!zWq<=P`f7|e2-pL8!_YNnrd|0?VS!1Enqzt8K zuqRdUTTsjjQEau@Y>@Z->=ApRPylJ*nri_C2SAWq2x={lcPtVSod^$}sHm@vQQDtj z)1r%1%;n@%;PgC0-ea=0BWoEW%fEtdqznX^TP-l}0fpxGcyjtQ18xVtbe&Y*Xz1C0 zaKrwI#o7l7J16?z2PMF}<`9*rr|hiQfCX$#ZA++5p+-%Fw9o`@OkG;+^faH-fj(=wcc07r|e z$V(8h0s;dc*4EaRR1*@KHm_r9${cj6SZ;SD#aa(|V<464fV@se7Oj4^D&(|PQ*EBf z-o^XkW{2}s+~3Yz*s|4aB+Ou-IGEl)QEqQPF`k}H6*3T-9Y~{3zBlC(7S=;RS4oNT zchGhFHhXwQai7DBFE_POUlD6h4s4) zl$6W52o_Z$dUtpCN*!Q=Mga8`=XaN!<)>ztdzl+ZhFNJ2hXn#djpyO(RYs zkTuVoc?_lpMWA>{b*)=+GypiM!JAf1nnzwZOlpG#%PT2q$;ezrW*|6t&-j5w&WA_pHq^c5d(k|zy^2bO@hmr%^02& zH?F*}7Jiri;aie+kwtt~@AwqJ9w!lvx6v{sn{DBs)6bhqr$U||+vpiC-qf?MSYMUB z=Of3&XQ>xex4!nqKm9C?INb4UN6F^-^o*dpPF`?bbgyn;k;uj6g8TEbL+-2AMCP;AtV-69|(7U;} zK7`KHgCkISkF)Q7>(ZZ?#V)r~-|ifeWC3<}D-=(2nXoVb5(K8w4rg+1UvH>`h$73^W4(mrQ;3qjX21p@mye8OLwc}e_q(S9IdI$0FSVkdS1W@IWon~6`jw47v z64sd4E==S*p9jzb1?;oe(35s1`ys97lG{U|Mk6gZFil9ARSVpaq3a;7 z#ja?>%-mST8sR;>dnKayfKt%&%Qt>`y@UdHOXYYEZPY1XHjL-awj>z{H+{5g0B~** z5bg=B%~o=gP?xd+R;U3CwJeA?i{m|#!|3mr%qpCJ6G&d1NcQy7&jij|&(9t5xZ3G6 zdB6Z=vgZN^GzsUh1?}9ntpUK0128@Lmi0-x;(@#QO&a{HlItx!p+^FanS>TvEAY$E z?Nbv-cI>Nt-TIg$+u?BjS%mwu6+0XJ2Bycl>!&fxMqY3_K9xXbI2dP%B%@FuE-al| z${qXblk1^v=BOO_uNr8NQMn-~QDZru}POS|K4MoWzJwfcfzOpPTE-o$|Ie;%<)(hEtv%A9*>WkOBSewwi zjPX)d!j-A>i-|~86G*5TXm6@3^z>wQXdvc%yu6oP4$`kmkTu<5PiY{%dh}TGncjMi*_Vq_QsjhWq zR;z_hM}PsIsP@RR-VZnNi~jWKQwe3baIpaCgP8|Bc1=GJ0v^&ZqB=jkp#WyK#M`kh zu8ic%**%{HbwsE=6c@}N<>r3m;Bofy^$5n5@`{Rq>V)8?f{VNrB_+4m-65_CL30U^GQhk(7SH84Yyh17Q-xNwd4IE@`0BHr^9vN z05QBC*s@i5%n=8S_kz?=tT-`4-#%uGn5`djDX2yOK@1%VP}@;#f8|vZzA%26nO>^` zfDdW%W3OA>G6Q}E)mwj#Iteu_c3>I|Z2`0=?pL6D6S{U?I(`5_+JIv;4#@=rp(_T{ zDJ06m+KT822yQNDg$J)xR~o>T;ARVLNx`D7nc!&Xpe%W$D9u}IYY8Jk;wMZX0Lci$8q+-qKlufIbj}N3(Sb*CRf`w->uN}`1t4OXof2uX%G+MjU z6^Q@36k^dh!b$!xQxI#9piKzUS5~7WZU7`kF482polCO^u_|-(IHY7(TdOXo8ZPFJ zoNLih;)}#Rz_{t)u)X$+8$t)zZFPDRRZ-D9x|E|4A}~T^LC2U@V%NxZuB6Iz^O|w! zJ~D&4Dbl_N{Zv@zQ8H4U%7JbJq_I&hzK#_4c7tGZ1j6LZPqk^mw;7O5E{HFdRgu!c zSaUz1b?ML|^ses=!v>jtP5(E(_hNC%qETH*RhaEjxG#r6>AefHo=CtdP= z`vLFI0x6j`Ha0d0^V=X6o?QC438B~qX+qX=lb=5p^^a>q^V1;ou#`Fgnw~v%;zWw9 z*B**qphisb7BQxINNk#h-I;dtMVFES$WwiFWeZ3T!8rcx<)_4^_2l(;$}_DIXodg` zC@RsxsSi!%ApBdJi5E!b*rS@jzTWn7SXbh{YArxlpk>la#PkU#2gHc&DuyWbH z^vwrlkPbpz!2%FpxHF$#9C^Fht_ND`nV^c=1mN-Q9wTwEH2{JtAqPKHN`C!99Ag>7 zFN6O7-ux#N^go6CPa!`b@!w+%9~K$cuKvGNX{`Rup7wJmlNjzu$;hLtW{MOU2IY=Ut0oM>0Di z$&zSF(V&{!i?W%~Xm&!py!LPtYa)*|k!I#-OSQD;A=z5ev_Hz$j-5irJl1QHgF z!W#1vQCLAV35Dk~=0_Q0aRlTosF45IebCg()|hC8Y!>*d3(Nlabm`iBcw>GXmPkbL z5e4{ASezgRg(nDLQ2c^;5}AlK7UaV(?c57Hk0IMPAlq_@A$zyU!LE!y-uSwEG8}=7 zMoblUocy*;q6qKTH@@wy%=-`FzVB)Jk2n4^X>nM7GzMc#LE*>*kQPfOqm0RXd?-GC zvN0NGY=XyAmNfBKX^o7DG}x`z*~r@AZx@_`Hz5%ze8wna0+t_zCG%rYFl{C%0$KoT zjDbxPiRh&*sDB|inPNvXGqV2MrN$cz5-|c~Jc=mDCy2rl`EejMP7sAAqD=$@h$MbK zobmS^BY!0|%?P%o{F^NVk3|zm7%TyW#bdvUEPx{NVTmXcel(Uyz~M1O!aue`{AYVM zqBy{&i+{h&c&srW(FBho@DX5r!y<%dCO8xcgT_*@e1d`k{Qr6l8krI8jqG6~R0`P$ z(e>XhJ&vCrheHeUp-d<^3V09#vL<1PQbdy+93F?_!}EdQ6h17tRSE_rKt@x@IAc5-i~GmU*MBjUZ-U#|{{1E{fFYp?{AdUV zcoNt-#srIkMTtRS2t>Rf7PO8tS=vGV7pwAz++Vl1H?p*`b^QDNIEG-1HX)G(Q33)K zEaDCYzXmfD8iJ7k#+U><{{P0}?Whz;i>MSMOS`|{+=*DCi3uN(fZ`)#NGL1;1IyNg zEP&#}P;f*v8E?Wb2phF7g~J~v{ViY`*?-+<74zQ=MOXn)JedzxDZepzNPd1Y3J*~g zF&~_uF+mWGB?w0wxqeG#dMx(6l7_enhbN!`&fo=MF8ILt7?aU}D)D#-yd*wjqRGi4;M?e=rsQ)lfztV8LeiAos&S5P;|WFdJkv3J=2~8xzqa z0b_xGKN~b#6C>-tKR2Q93-IydDQGYyLBt~&<4^B>G+7WwCc}Tb^LwHHJIclp@B(~9KEMimki%lpXuv#Zq5w(& zL*b_g^5M|@CjZLu!^ZINUv~V!aZ3~OgXcgtjt4U2|8Jthglr;!CxN3!d?X+;A_|We zGy%L!!l4O73>L%x|Jr-k*hrEsJ&gOXR~o~x7m_WlyldyJojdNH=_*!cy?fZaF21_! zUcQD!_S~8AHIbE>Mb>n&inkt3a<1+buzw6&@&jx5uMNR4U_+38NbnlFupsO%){reo zg7w4F55ZU%{vpFM3_<#5Snzk^l^K~CSy|Q7-LpMpgY0@_oH%jf#EJ8Y6DEHFA$$(E z)na4z^`LG8@qPs>(e^>%st;Sd2~(>GQB#H?+h_zelT=}-!<%+J#I;KP`SoPn9}I&` z3+wfaq2~1~uu+1dS0*jffy&@|u*e&MS9i-j-^Sfq20mwF7zUXF<@F#P_+Hbm_j<*8 z&4+W$Yr>wj`^59TmhJm>yXBcI6OQ!w;{3&6^cFBLJHAtOnqINzL6}r=ZxAqtcdXW{ zReC|$59+l{ZXW`3*s*VZnZn>`RdB@`L<+3%Y8^t+b?ZIo^#N@0X0=*vnEa89E&18DDZ4 zg7FP|ydRYDMHzFEC1E)3Gk02cpf*!c2SgYd%Ft)5d;w@PNxP~eF7SQf_ ze%*B{5R@(8UUjROfj;?KdwvBTHN=|AxB*V;C`xG|1qF;Cy-%;s?bnmRcruvX?F=Tk z;s6qClkVX>ME%@gvhPm8k;x|*RvY3Ht}R>`gFBYCf=l-$K&3^0VvopxRkaDVzXr{y z+J_Ua>NZK*w)#c8S9e-Q{R;55g7>6!QARfPrtZT>`tSiE8bR2ria`qVFP9Q0=!sR;Okc z8&J9Gh-*3s;)Ohn8xv;1*B2!f7pKBG5{Rq3Hv72u3ba)3B^5^$=iK3U#%-*#ck(OA z+?%kRvBNhj^(roHQ=iJmrt=<;DW`;ZpHi?{lB6E^4LD(5eYd4ezG1mU)^ zirquBxmGUI+5ka&bx2@%Zd(lx#zFv55Jei)bVs8x?!6%4xC7K8ZzJ6^st?DL&7$of zTfla3XPGyq5A=ty-LNavrefuX@Ac;fJ|x|s57D~0J#?X5dbrSTfQy&Cox#+bz^p2l z$~Dj;@}LfZ=)vxN-NaMjo8&{?>%dFnL82Rd7o!J8itA>9$|exNT=3UASk@o4jg5Xf z8xS`1U!(y?Pu*UN4%XSikk6ojSb+Q!JTF8Py~npH*%-K#nx}EFg}bSzo&425|K&gV zjbC3|qhGqvLfy$D(@waQUpn!G%hFfLJlWwl0rCDR{{+DtQ1v~pRznm^?yO);jA)3R2Qq`dlD#sQ(*Vv9x`DFD7tp|qI-bxN zEWiA$G@5?RZuvDRQGFnY*mtqj^pT>{gBf0N;ApLe+Aum%v`VEMVX>G%+6LIGH>wUo zd)WIt=md3W2d#1+Y!SeY^IfOc!2RKB?>5tjzM%vpwt7pfix_=KCI|vasUZ1Gyk8b~ z=%HZTDm;q4D)w3E0!Wt$dPTe0Yu1CnZ}x&vAr*5gwTj3DXCMxJUEK7Qw)&Vbld?>3 zUwUqt{-xy302kA?$Pw+=$fjt)%jo(Z)KUa80)*lbu#pkf4P(BR0G-S)tMzRtMV*xa)O}|)a`W~ja=J(6yB#SIV<<;`bjQBz~!WNHIIJegTH39_Jns&2aY}7y{ z*cKjWrFOPNQplUgFf*cFhMHJyx=yj-^&tHMcrINi)lgS(Lvj-is%AYSxc1ru`bNBw zCKUL68%`+X4Po)ySod}lLfrS8^*+K<(5}rSf4!l6BhnsrlU^MFC!B2nuGX8to;g-4ZsLR*KPsp3Y=vwE~m!!>mZ8OYBgJa26w-)@VycL zN)znSpg4#PT30FF+9YQ8dOl(#0X8u&gT=Ye?DgiF8By;cd(ubjtk?|TH-jG3r+6Q9 zlX}AooQCgV9?V>>Lf&sdB*Mh4L0b!o6_@fV;hcodh8?=^_{jV7+#V8&a$x>Og7rr7 zBS6`QyA1JR$3}1j8V@)J;-&$|nd6j$Mn5CZ8Ih_)yh)b<8)B?5%Mt4R2(A^Qp57V{!d_`lY?Gb8^1$BGTjrPxPKD^xsE zul-h~Sb+-_>GSw9Pol@$+O&{tUiN)R`MQgcW;N(x(?SX+PAni@(eta7AWzcM+uyue zP7lgsfIO8RCGVmbE>hF`aQ!;I@7KUa&8WjybKZAG(szIURyQ%^2{rpZ;+;4YfFT6E zI@UAOddl@$1xg%N_u^IXHaD{-N)Utp5wEI}ivXDvh@imh4kM>*yR`}uG9xa6>`9Pj z@as;ZHm+;kgt$WW*k zeQ0;ha;1(FNUq~S+V;I5$Bex0gec=2xCq+RVCBPO(!!Fcw(7-J1~jjhE)T? z!_U`q!2AKJMl~1tu#M0spU^1YCc$B)x8M>)I#G`>@At7Cw{S)YDU0=Hz24{}1JEEv zDeEH31bD@8zmZ4kjvv552RVt995NhXLbl*w@DQu=>t3(XH~JO`d1>iJ}z8Sb%z2E)gB#s zs00XwK!x+5_cvRaJpaZr(~Ki%F|mTwh(?P}MS20k<2A_3dd(}>8hxkUuVjzl&E%#T z@$WZko(~2odJRet^lQlPK#Uis4G}YL;K(F=Cs~ZowdB{Foo2*7Kyn>W#tA*338yUO z2;z`64xadK+4bvw#mzze*Pfqd#J>gX;kH7=9%=wWS}@fqn*vUsRs{#K%gC||Lt46a zz?Lqj{+q~9GY+BRI4vgd?Yf6;tAfmf9vO{&#K?NRzKuAANx@-5c-=W_M#K?(ww($h zN)?>d1MeUQ8KDVyuPPOsaxOO-zMVmPX_vgZnnU4!76VKV(P|OMTv0p~0QN2iLw^#C z(g80W`;~Ahv??;|Hq#SUHYg)QM#N)5KT5pV!3l6BR9FTK&nEfxe!p6);IwQaxnv_u zCE-bB;d>EyW?-n_P0AhNSr;M3nZa0=&=JkhFvPZH+;%~98;^-ds2+Py4Uevn8W4>z z%iNLlDb{!=SM)j`#g-bOh-1q@ZmuYPbcnJ181%g^8Czm2lMzw^)~|wUt~3yVDo@*C z1ShP*yd5EbM66q7?(i6SEOqC%|H?0%{=v_$t3;qrq%%JfVkA0)OJ(JSn&%cX;;|QcT!~V8s3SwyR0P&*btoOgXw@%fq;(3!_&bv`$kSP$O&DaW^*zg zD*hAikFF_^ZmL7k;Gc{^l~Hh0u%00``73~Nkpqj2H}8@oa6G}mqS^k~V>x+G3bXNb z!8$?~GM*&hsPCg17OXB2=t*HX7zOSG2qOpelojeZ9=d}Odtb12Z;@(CuuCFYj6K4q zaO2qGV`rb8opp8!n+PcSNa>;6bxOVuV4FZ;KN9L}Bx0FsfI>r@>kM!Nma<(gk=rn9T;G3$;~6 z(WZ1tf9nnl5Cbl3e)ogn?Bh1p&<7W@j}NV|bXZ5dFF`>IOnS{{l5~vy@x=w`bp&xR zK`JR05__t%11>07EcKK>%-|13FG^uY)V{>nK|mp9iYP6LkXOWM=K|c5f_I6PxcPq1 zC%rC$VTAN!fe%3s8_ox#8QF^EGrd)NGJyyV2L|!eo>Ka$m~n>V6ma;;-wuYug0+KO znbWywV|N4|2=osllfgN2KB3L8JMT^JrnBJcY>MTEwdhVp;2$*~8|!JnHUPq# zPoQYec5ef3PU0rfT`GT`PTbM7A3$=4jOnB^W|*_48b#(sE^4ui?Hm~qKw19_-42+8wb=ijEli4TM zmj#v&F6fAkHO7X#TnC^!JIS%^m@v$oW7@q1DXzy{HXQ`B%ds!$Y3f^(^t6C<9$m;N z8(qeJP*M6Qw#QzdlN{muEw)yYHMFO)7m@94^F4h({ab(SFaOh@{m$AN{c4(|$MzGj zXL%V-5{P3LLM!6A4Z+JH&l-fOY9+*`5}Ay!LRr!|QtB0wyGT6iPi%6iSg^Ft6w4^s z+k7SN3@?sp4eNrgJ03#zrZi&XJDyxoGm&Dgnktdhfa>tb;(uFZn3?nj|3Os6)qtG< z=NJY|9aM1G&JZ7<*i7|@Hl11ta6_* zAgxKw^Z{2*1LkKl4}+W8c+|n3#MXEy;3dJZ4vknZHzkA7a?fj2IY@1d$Y&Zs(`Nzc zis_GYtMOnyoDFE>h87Z=09aw^E!)H4_(nBE0{Waz)#7hn0GM~7&ZMaf#(ue+ND=@Q&#@}&ab9uBe=5E4HpJK0ten@r;?>E+Z%C?Lm zX(|(`3m67*>0SrNSg*n4C2ISoVGoB2>6w_KbP%88toin2Je{UDKbr>R85m0KasiO` z@Pf`iUtYzA1T6)rH`tu0m_r(`qG8aRxC3v0Jig|069chmDoHgth4lrU8h(p;xP-a* zGBQzTN|4*vI6f5kol&=Q9P8NFlB0P7>Zi}+9y{=zHd?;nzr@t>RN_u$P~9qQmJ*ggxtlB`8l0q)ixBlV>uZ+*5}^V z1eK5ia_wYodPW^-k+P|6?70+9t%cGh!&NMYWn>C2=X~O(1+8=&Db2d52qucTYREy; zIZD?{!IWSpG7w@6XxN;gvoH@a+bv^FzL-6-<_6~98^Ep1c9E2Q@0NX=zG{^I2%!<{ z?^b-5-f(|#HJH)RkH-)eA#E3}FZb5i$gIlkk6k~DG)BQj4Ks#lY)Agkkft%hWVSkj z)WU?1V7`Ga%d`}JOeya;A&FSnNz&(_%gW;lw;)5AJ6)Fnfq|xLWuZx{Ekf#sFY9?$zn|(}ZWqWNKlvg6ndoNSK?05t*5z1maf#EZyZOCvl`( z5U{z#*PEHGSi|S;gayV@7C3fnjD9>fu?eOu#nv@Z<|IG>V1&R6$C8gRK_0`*G1ed+ zNkQ}0h#_HTXf!i6;)ofV-5Hyy87Gf?3YKtdPmPU3#!PAHPt9|{k>#$Y7*BJ(pFco^ zFc`v-2VwwBn_v1wrXX${E=Miaonh+|3-)x3s9tW?7UGT1NvZ&x5k*k_!bBF)Ep?hc zGP-#rU3`kBq^wO4#ErA~7qGSZkKh{a8#di{T-~XPc_*?yDJZ`l@$l8)E2h+>D?KdE zipcw%)w+stk4frmA;ulJBVq^^tkm}~Q!D8DC=~_u${h~xc+dmoSm0xbPo@cg2vn%Z zvn+cR!@4t~(6+(ufi=7{c!_{Lk`u<0!(i4O!ge8Tby#>>D4-Rb1XU-nCUF1WIE_lQfTOmp> z0#JOm%&VE*9+iQyVW6bTjxJ`GMhR*fY1@CvMv6%l9IFphgnTr09yvK8MPxbJ2ys1a zPqzj$WPA+$+;ou*2uBD!2pCFE(d8TEAe^73O9iyQ8BBwt3HI*+oRCNHXbGH1f#puE zD+*GYR;BQ32Up&8=wlMbt6^de*u%+4h;%~Kjp2l2)D~mB&xLDyask4{hKwp=*hGBT z78c5s&dCpK&;(PUnUS)M9-naPcj&?_X3#5~4rZDOu^q`l4MLHz%qS6_boOsfE z0UjDp?%FeSFqzHcoNJUN1cK?QP;^!VL2i(};cVI-`8&NKf|&$U&G!Pg%^|e<+v^$B zoUJ=*2Y(tRiY5J8#u4oH`zXW(re^AigTZXW5uUey)%w6iTGb&nPeb3ZaQ6p&C=zbm z2WJl9Y*VryT=?T5OQMNkSu|Kc&`&U}-it1r1FFQKo;urjSs==!9+!lar+wf%T&)2eHG9!t*aji&*ru<@9& zkw-LbBrW;@WzrVAxuI&u`R6Wwvj)#?zlYm1?J_QF8^K5EKaWYwe64u&M zZFd5m?`bd+k#tYnH#2+sh<&HiONla(D$Wc(+KXdUH!;dP?scdMGQ_+NJ3(*Ga%j_C z4kxFGclQTz`w;22W5cEl?Q@DWLumAO+n0ftlGs4U1Jb9EWkE67JYBjicXc5wedvys zZ#NDE+%+kH6Q zz9uk)*Q&^983sup@x)urc4|+kcCd%^t5<Auc2ykZuZ_I@L&IDG{=z`fD zQ}X!5#AXKXv%#s%Xfi*8+Zp5r)5VQrHEkqfB`GB_)#TpHw?1penJw(VYw#+36No6JnC zJ7_amy*ba*L8>idizMFWK?`L*@K2Fp(w%t8wwZXVlsE*Fl1;P7C6n;LY6yo19+I?I z10Yj`mTS(Pz?6%Z(cVlpmc&Prj{Gx{*^~HZ(hhnOj_ZN)bcSFhEU;|Jr_2u=gy}|0 z;=y^w#dds4RKc+!Iy{gBVy1@h7|@X*(iM|UGwFcV6$ha`I_BKH0Ibmj zV<5H0Nc<-E0YXFle$w5={Vm2!sw6n1?J`%>>fq#T?UGV#!suuez+slY6)V~^%VjVG z$EOoNWA>RF2^>3}tu*~%b2*yi)SH)I(Q%a zc#cc46wGD7z+Z<1VdC6mc78s!8|m-ko9^9JZ;VSKa8fnayUEOvemZrwm9`QLCf?j+ zIj{hziYGF45UX%TKDI-4o|qm$6cG6nOwX9kxsDoxpE1394Ts5Tb%@{$za@lad_G4xhoyvqju~es z@?Ncg7%b5lOJky(_sdUl>Nhs$=2_D7PD#6*;HC}kX9P3+X(CE@dwBa5(^*l-CPi!x9Ts3?+qy2-iH88JsBU|{o2(k*jhAzlU^ z5{yz*=UvhdSs1|(=Rzyu?=~IXi6KlZivZ(EaP1BZj#tb-0C44DdI}Fd<_=i|iK97Y zR*y(*o)tvBVmplCk~w}-T#xz@NWkbyRDbwd!0FSS4l;vfqL@i7QwJIHWNh3SP??{p zb7X9qLG!({(R>>CgbOBkm#&DM5}a7)nMCgbNN`JO*E}5u2aTBHJq1q2Ai8^$*M&>y zncBz@Lcno~ZDkw!7*++&B`l5D;h5>8GMO+@zaRx-CW^bzO74!`+2!f@8I(oD{FjA5 zN4WTnThNtAM@Omw4x0~Qg@N`d&=!&&YYk>6!ph1AN(in3$!8atViG4fUqSwZRrw(t zB;{Q|GO+Lhgd?Ab<#FTY0MtLo6tRHxKZ zATF2&s&W;MdinNh?%Isg5quD_{bfN32UM4F714gl9fT>*fls?3FBTCG!NR^w_ajgf ztX40Kz^&3X1#+?P4Lkw|juBRH91EiozunzF2}an*QesD$b9jQ5M?KgcFKI0;i8?lG zC}rcT9xPA6t8D*=BROe*m<$LSG0D9iv?pEurlt7BTo&;4L$0Jvw^{nL=&Ry9jA8HK z?D@dO0FL^?9M$pIG&;)K>>x1C7mv6h*<5cZC;B!}Y}37)OH)A2Pk3YLMyoNFu~I)Mc;@Ogaq6^z?=JWrr5 zh#Sijc;-%g%vg|~CIDXb;oijISlSQN^vGmKW2$4=4rS^hhQ4fQ*G);1VN1x{M28h) za%R|We;m#Es&;H1EABJH)o%j+oMn9W1dio;AuRO zlaeFNEq9?_;X?CiT+HqdX1Gh0e`{_{B^k98$9z-Vy&UvsC%E?>ddrOM0x@ctc4|f| zECS+X607lMrd^WSJ^OSgcscNbl+0b4vSE?R1Ku|4xVGJT@abUWk8h^+s-eQzDb4t7 z75NcuxvT_J&oT+yC&&R!1don>W7v}2Pr$v5hBd6KWK zQKBqru89Oos z&8Wwdblv?iZyIl7$gzfUynEFPP(qr;%qDyA{h{h(d>1n918B~CbjXh*PuQ}}rDnJ_ zhKeYr zoRuBw180XHa^sW^W0$<~N2qDeT%N}`B^~2f({f1%chElFZ*Ng?hhRdU(ikxF=Ae%Z z_3>oLaw;4_F-R<|nLdpW9oOy!<{j3vrbzM)3c1m!6U9!-^ynPU6i4v+S-x~|3^N>I z3$RCwaKtD^@HCf*J{WeM4l!xwPo8ewDVB&0$wX-#8bT6C2c~lcM;zb-(a@DjtrqGb6w)75aY1;#V^on(kT}L;)?)Bb0wh8 zQ4Exk84^RJLqzIw0SPa@IL!)hh*TaxhHXb0q3npXC@UQs5KCwpc|v79L92Pdc4xoA!AmfYghjLYHa@mU7W#z_FaXn6qJXq-|)T=h#e?2Xt103!L` z3ns;|hG;>%;hAy?#Kw)U*-)4%wVa*E@t0Fi1)YLz_uA!|yE^?1GY;&9NY1^RV=LMh zv&?ha^p0@pjOk!1?PB{B7)OSFG9k*eS-<02dP|Tmrlp^pernLs*>x9u0#a((A?LU> z8yeQ9=04g%h8=dkME!;|PLYCvd8cCYZI(%NX%=Tr*&Y?EW3A0=6OARlD>Ogd-=^9D zNaD~tOdB1PBC=X2cV2%uLm6^kP@cpUZVZbxxxzQ+lbPXoy-2YP4)9bTicqYvX$>4h zz#u7Fwq_1W1O*0f%@^?>UVulwX8XJtkbiY3DvKrFK5MGE~_|(QxEV zu+-92a;5bvM~O7eXf&Je%|?E~fn-+e-ibb&@5#YW%a_`X3xgsh&xXT zd6PZ^zX?q?cruQgNsq##OyOVR`Ry@sjFOr2#-X4Kenug0AFSTrV6?Xf(S$9t2cFE^ z6jWZ>;NoqucgTG%X{wp1$XhJq%n;&g2~P%ZAq;$-9%;-SMJeyoB6mFR<4`T(2FUyL z)_%L2KAmRt_uI$q?H?@L3XHRClcl^voV0mdL(N@46gtYoF-3?oWErKB3LFO|xo zx)HIyi8!pZBUBheAw%sEvXVgkG}FqgZY^sJOU;K_Wr=Ohya=9AN+$Ht=nEJ4;rS zx<=7>qa0dsHyT9=R6D-d{Q)j{gm*K2r&BG>V|2_8IJD*-eO`7{TmT(Iz$p9B`c-{* z0O8F^hmQAY$TDKjcaQd;@188v0=jel$5-$q0xjb83tJL~$#DEN+MKRTkF}jI$~N5D-w$wLmDG4#ucxkPkAX4kA}qrbrVo_E(M;?yN&Dvq zL4evG+%hNsi$Mj`fSCn^U9rmz<6#A(19?;W7cJVj3~@5U6zDh4vK)cGl4}`-krznAb@ROv(uqv_E0`7Akvm&q8e4mO=tE5pT{$@%~=!aquLSR5-a| z01aG$5W{3Ld%v^PU~s=_A{$^b>zywJH!VQX;Bh&iR1a`&- z5!sX|u_;{}$rAXy>*Mk84i`1B6Y`85bQa3qq~#XJ@HgR*F@|Wnl;y;}rh*v)w0T9b z-Mx>}VqC8@p^`-ozH!R9+VbS&CcD10mZ2r(d%{E+c92p}MXr&Gnuw*FR%WtotY>xt z5|xnj!(7?|75A;7Ixl4%r5#j-Cv~XXG#UUDmZO3&Nnc5(mIeU^E{Y~GCv>c|S&Q2x zQ2^|_yjjrN5a{9=I9+e^u$`HOy`)0`Wh*1YKdnm$%1J-% zzLu2YYg&6L9m#b-Nv+3*5FC^s8C#-@(#(V4YvZUJlP_B^b6G;#?k%f$WCoxWvE<&- zr08Xzn_*B{(9RK@BB<&ULuBFIXc~D?zYlZ9Wl{#vY&Qo*DB8yzQKv43Pv*)NEt$%f zYx|JN60%Ej4y@9Uk98STAr?`;p7?&t9nnpEA|WjXgA&m^%;!0r7A#?{XULLr+aUl}(es~A3+=>1ap8gmy4A=dOQR9s> zEH&9R_Fh;ioe>P&oxn?6;2q2L)!0FT;2P9QAn24W5$|lI3M+!WU0l+En+=l!JYziE zTB9|JGPXf~#!7G`c7U7!Sc#pbA<%g(=8^7P!J$t+?(OF&TN|I0H0wuhU)ZE!ET#n% ze((X*hmTLp>?oQhFOJhAc-jme1~-WG1Zn+=X)yN(trZ6>F-(6J@|~1r&1^-J31aee zMN&G*U67csG+HGsMU_4!V9SRpv{BMVUE@mqC86(9P6ADDzkTLkKW^pN#b5{GFuDWldU=K3tCIu zlg&Xm!aYZO;2vK)5_h|v2lr%aj{Abv68B_tfqRbjz&*BxCF;#Qm?zqD#IxH`#TLKsgcPJO1K`D;VF9bK8 z`HvsJWcPEKC6+i_?2T~lEQSsh%Q&C)hv)2=4~~7_l;93s1PXAvIM$z$eXI?J_YV`8 zLN{nj6P`RmWs=xPI%9PuVIE$>@}?7K@ku>AI<-27)>(IVA?8T*felW#+{bsW?N-Zr z(CHrO)mXKGP<$`4_oN=<+ssY{ASs2qHYB!;xs=(SA?EwOK~Y@dF078&jPm06me0-% zW`sNy7(>odvN(fAiq&g8XkcVU;of{W1>(;duBy9x%7mAq;lO820C5Q&9Vz=%Un7!2tmEbB3mhLTAMbQ-@S0RR%y zk>L~*5V-4`{Cw~g(Vc1u7-^0gsUWj`RqpAu0c|l+k(8DTa#Mt_kcGeqm&A2PnT#c< zNm@1TB0&}@bd1=#%$8AD4Pj7%M`aRZ$RMRCC@8NqO{-Uw(Ha-EA;*p)@>n1&nIwig z!b|3T0^1CYTio^*Yne$k>Eq?eG}Ql;a*L1`B9}TlPZbB5F%sHulxR?)fEWPmHUKTSz6P@B$G#@DGg$U&@;9V zxZp3*|5T6D@qCU$TvdRiE;BPCa6#!4w2#Gxy|5`?WGw%ORvN-%2RmIZbHoKjF^Do< zuhbp)XLMx;*GfxDREcJ0Y}s~8SKbq?X_XjPs;9`g)0IesSuxn$gD~^W1Sip zq?6cM@Mg2y-9`<7Y527oHAHAQ5pCtM)FJ%GILYiKDLHaj(z$e-e3o2oD z8v1y4da$2bm&fotjT!SYVHqgN9Vsp*G_2@CMV3gVq|8x2?t?SUDrhH9O5ofuyj3sb z8%IV=?8@PprOX*s4E-m(JrF(*2uOp6j}gT$;?1;Y-Zlz0+bkwWG<^2 z?t<1FRTABJdY49>+4y?tMhBkn(KN=eJ8-8ju&#}C+2u3d8KJ%?1P?R)St$c?h=%LX z#xWZ8l}%|`qv!%sERoKK7m-4FcqfwM(^1QP3MfLjyeEKEla-YYO^;a?T&TZ56J|8Q zKDhbb@eh9C7yslM|M||^`Jem!Km3b$L%)9pzf)0DV%?K1$lxvAUJW?Ni|@?iF^sa2cw!IjxAR zNFu-x((n<;;vqZjYwr6CAIgKTT=wuNYjYq!kMvXE*Z2tdC<90(DS~TqUh$rO`rc1< z-ue93UjCgH_=bLeHOf0z5PZY1()HqHadx^Vmc_wdIl6dobPqvN%AXe5Ku-!&RNX6B z@KR81=CjFQ`lN6nBdxskrC(>y+zG zli3^_A`nadK_u9+BHsmTJ|Fl`3Qf;z1{J$eEcY8uvD$K)#g^~(i{+-%ss}B**>Bmx zaM@66#Nv?DdkL&%*ZNhvSuM6|0e}H?(XG{6MY~qE>$R$llWcu{_07gQM*|{a9mLqw zN&f7k-~H>K|IXSP{c6hA=rK+5$?<-mn`|QF59e;n@7bZna7^%fbD}jha=p8&;*-#LFLvokcaIx_c~FY+Bg- zqKJ7ZwQ5bvw(I4RQ*|t-+$cG=Z8hnTpKT|ae>EOo?O~h5ZA6=PsZy!L;ZyyyUj#OGUrgK&ru*>uk0L2VL!`!Ef$ z|4Eu`@1J!~cTdi@pS4d;<9IUKGbMFS2YU;f324Zs7~k;2D%?wXI)*P0GAQt+n;SHg zwB z4zX5_r4Ky|#J9|c~cRp+pC z`kp^0!aaTud0fFvSX0vn4?lhnJ5LF9mY)nLo%ex_PakqZo53_j7t9Oe2Vsi ze&l<)#M(M8p(W<}dx zi!*Rzlu$jtx_ZPJi&;sNt0#=h#G8-k)IgI-8G=ba0dmV@E%-xaUs|LwbuD2R^QGrrXt6O%XUTT(G&A4dNU}Z3r_FSfj(w@u|Q`(ccMHOom z7gy{@w4E#@uSAgH5|@ig8VB5~l{yYi8?N*wp_vIy3+zMTA3W9-a`R=lEfNwYv zIWWVeT+_?((0|*4fGocfc9f>otV7ph9Mx);s@Pl_rTV)p0%j9R9w((+Xw!|>0=7C< zM#~p=z3RYNvT8P}yf&<|RemK>O($NcPZ5vOkCNNi4B#_El00yQ%~rOGu-DM1K%xvG z&=Nx2gvfeI4;%gz&OF{%_*eBxCPrbi5!LNeo{Hf{Xch4XEY*b8%c%qe)h=U1Fa;Tf z%GJO>rn0AGu!bOs^w_vE3GXAwp$uueUTKv$TGdh=1~^fTHa_|<6HNYH%%8Y7{EYQx z_aL-rL&{-Z$>$PeP##HT-iy1!=GM{aGuTF*?H{CV56pFw0HbIY$aLhGWg23#u^uh~ z9h;5t3%NFQ3yOfKo-_sQi>MxBk*56ws&+*32^9xQHXWg#{^%#s22VnrCfqqAQX2a8 zPt#~2w@QXebeT45v@mK`qhVvlq3u9XZnog{cs0?MvYH|US!v}sY)Qjhbet+2JTS@| zwU%SmD=-}GTFoe8R$tlDmlAS>Hm!-34XMMHcDV(D!UYA-Qmxzy_l;Lw+5lN0E5@-J zb`$Hgq4^yomWu7Pr7a0YcwzUdHYDLBdiTp)5)h1HHe97iolHsyO@|DF$O_V zE>+59h*`L}fEUcn=0XJXDgdnpMvq8?0+~5Snv@XekbGr?s{lr=)Tq|27Ch;-YWAk^ ztx^II8`b>IY-@;{(wgV5Qr2XMlGB=caL~c#cytISqH;=Wn@+^mb!^-=UUguZzg|&PEP4a+u1$d zKl&^-_1e&))9;k4rHW(2&4~b2snUcpfs~lrYBcCJR#(WhqL~Xh3tQ2mL{7^cRe2D zgUQWMwQ9+3lKof1W>j$+b*t(?#6Xt7EJUa|E={yaeWw^IuLJGi9CxuY!NU3JJvFa_ zomfC5!cPfaGRhnJA}VVrbdt$GuEsvS@oYA}%Hkr$IkMr(j8nl@(_oC#Y&0Nx>b6y{ zIm9?H&RU60=-p+TypHmN^ON1~sfEZE38B}>G}T2+1AbHzXEDo2!J}A*HRo9zOfm^} z&nt1N6=>zeIF18;X_TSMAZw;X8yTFg@3Oc_SeUjgGlFP&@E^4a$K5vA=aCiz1IMnE zYqa$n%^K!t8hL`8{Vr(CQyVsxYu z5vd48r6w6s%xPjpVRHIs^QvZJp>dr}8F<_V_hS+T?G%QYjGtd2dq-WZ@b+nj6s;9~ zr6^|%OGIlf{Oz#j~bL~B%;-ur5dtQ-hf|pS$?lz@l!;hT1JrC zsZ?sEW(5%iy9!5Nvxd)%?7sSnTrubY{{m!vh@#RnjVR__kr9}cDrTyZ9k0+=BoY8i z)e0fjEIF8RMmdTZGc`CnY<#~QQ5sQ?ju@AtP+x!cBs&2fV1GJ&;NMc}Mix3LxxKQ18|iMFQnv9BDZuq>kQ6T@?JQehm?_8am!lo#yb3kav^$2&(P9O) z*&pixW!@kGd5F4^{zvvhnWTlhtUgY|;k)^0_M`WHLivK~Ii6k^zWvBj8Cup)Q?xgI zzm5bXow`#kHLK9p8VyRjg)18Mvnz;5R?-*F*Q4O_h~&y^sZs^Q)|$w8V5@!cQhHN6 zuarklrHpLl5W~gtDDPqqVK&Q1v084i*D%Krkc1f$4(=B_Q)QRPIa%i4j21t zT)W7bY|zF>N!*O%Aj3&({syjHxiV_yGE~w!^7!k7DdObNHCkXr<2JbZ+FdSEsCMc` zvkZ?&h@$e97*Wi-cteK_O_-GJO1!hRurx${D(wmc=p70Lr)JfgY zlrpq@w9wI$?eq;(8n~UE{axWiq%eiZ%oj=cGK7(EIr9{3gt3q&fJ0e6D}`PREf&=Wu%O9G3=- zWFC4A*pT20F^xX~FDOd^Po{q>vPJ@Hc2%Z+!z)?G`2n(78;}n;sKSn>)yqyoqg8-1 z9Bt~EOx9OVWyaIgQ@LYli8xv^%cZJ2R#YExgUqhdohHyI5P73hJ87~~HO?YPLX@gj zF`u6DILfYZ8cQ4)N)dt>s-7>J^fRiCxaOx$?hzyLAN52G6(yGK2Ax|Hq#zGy7D6$J ztbj)|H$zaItO@z4T4{n9?h0fg(`?{O8oWRaK^los++I>p^0DCpFJfA_zZFQ0O>sr z{%_*=5u)^ngQa;a6pYoD$_~Dl@Oi?A4TD z^N5pdVLJ2cUnLFQj!>l0R>>DX!-+1QOxT-G?|$?X7Q1GL4!;T} zQJ?5%`|>ZTGGt||P(uz5 zL$r)T=uJ8=lFGtM?@+B3)>Mm9!X!eWHj|1_t~DjcR>x-7syeyX6k;f4n!Fl$cj$#6 z4w(u}#rl?SQhAj9FR=Q!-uomGZd9z9P}S<-LK|^VyIMyMFH*-}-!8NQha&OM`%8&U4Dufp15&?LqW3Bo(V=Z7} ziM!~i7KON=WjJLVXK@^yA7QpV;6iQ~qwlkGNkK6=OU1gT>0*vzPA1&l5 zS5QzR9G#Jg^~J!+qOf6e*o4u;AR%~LHnTzYYLMZBKjKynDjjG+ST(o00wM#4!X6WinY6^?>Rv*b`sN(sA?7@3)?uwl*QPVT{xUNa9~w&lEZ=L0xxh>pq0oHSF`xeEu6R$78(Ur zzT(LX+EI;qc)CE5Lt9CjPc@>LtEZ#>MC5yw?FNKg9!kt4I-Wg~MQqdZJ+^r+V zDj%AvbXpeSYHWI_Q$)oLk8RSkI)>k2nB4FX&)i6SVX zfqQX*JL?ASY#al~YxDqMYP}p~OeY_JZ$65<0?Pz@>Nm<4p8Bn@(Nj9YxfL?iQ(IkW z=VOwMRUW{(NE(>pnP|E|yr$2_y5b=7qHHZ%l$aFsGe4h=&++oXJbKZaV#>6Ii9Ej& zBG6=(-Y?8TVqqre%Gi#7%9l|Q*KCgb2XpJg@W=|ku}GXmeaIt5QArt2ImA}VSvpbx z#o>AsOSGtzAZnndYdKR^ynT~|*(pxXop%n8&rZ*G+NbSEkF?G=z>wEdJzZ^sUDc?6 z7L}xQi~MF$R=o?Q#m0I{!wrhs7-3zp8$uwt_kiNF-ILDg?#}t?XUEanZnA@rV<&V$ z7nKsHb~{IBTl>2U8x5vZ@-y(`;+I2Lu<4pXaX;XuU5^n9L|9J_N44$K4V(<8y_5FA zZaAVOm^m(dG7^Osi5!gJJ{-)FZP(+CH`WuAsBj{tIb~JzIvn0(Yi}~X;h zSk*+Wr;T-L8KSrmN&j%0rHVFEX0|hm&htNI{J)|dmq)Jj!gm}wp zIrzc#>@xg}@-OIfd?K_!Jv||pPE-jG4XH`hroY?X+l79AbgWLaYNkuFawnY_3pHQ8 zv`pj+QDstKzJRxyaQ`>xq;eJR)G7;`H>vClj;SNSrdQEY!&qzy<+E^JxqNDdMjA%V z6xtf~=a^h#2U3XVv)}=@3vjNNSYeTR3Zm7q9;-OdfK&Zcb{5Vv<}gPxIlYLw095~r z$@b0~klEB`tJSCFCQdKHm=ncMNhw0Y5x%5?lvuUapZ&A!9hIfCF=Z`$MgUJ(7$J3 z9&+(6DJ#c@zK9m;?MGJdA!dakner@+NKrsGI%hZT;&?s{G^BIl>u5)k5YgEVIRjWR zmf@R^odO^k_B@b|M$jM;hz1zKK5%# z_G1$EUI5K7j99xFGT zGiy4xa7865oR7Ra9;af!BT*rkC6slk>gYd0m<%_wj6%JCHXd&e$5V=Fi3jz<`ZV|F z&_hv3o2#j4u;)&otZEIy3f#Pbd#f4{4yu=ks!SgO63R@We*bt(Wv#+w=_H!+8ToYN zjHtARYnTIum(=Rz z5gvsX%SmTdEYwE~E{E-oa5vfz1=owrAX2^_m{l&W-YQOTlMt@$xnq4~pd5Z>3QR`h zj&z!aT7z3j=^HlYCnD2IXisXd2G2~Ax*=z zh99^HqI?|SqJ%O0_}GSV;Tr!8cPF8fDbM|gopGdW2qYi%P)|XG3Ge|4RQ`@372Bw| zQn1uc#UyehV+6uDvU4d}&&4tg8#}#XN`?b{814>Q>WsX>MSnziKqF}w%KatlTN1~g z&!t0)nj3fDuxk!iOc!mJtNXN&&Mi$kHQ0Qc$-Q6e-m&lsj)i6RlCYAblIPl9qSuh+ ztAnn;LYdep6mnV0^N|(Y4scK32={9Z{YCqXgL@5G3>bHumYQS%QUx@W=0H6JtqPXH z>07+VtTqEos!~%Cr&vWqbCc)+0tgW!i{z;J=ewbu`53Kup#Te4I3kmjZ%`CG)1nn~ z+$C@;f)q-I*5rs&-Gyuhr)FYhuI64#UCDg3)Tds8B8SHmlmblkJm5nF~(HYkm zNn4A&F{2li!cksAW2FXi%%un?8G#6AWQ8*n_)ul7YhU^+X-4}~QUgoSX@4ud5Qf+Y@t zsrHcrVBOBa*?#Bn2j@py-{0LnO*_%0nqB8S0;U?EKGRX}OWcTD*xW`=0%(H#^Xb-n z8i_QKMXMT^9lPq0jI(0#b7HDZ{EXP)k%quV;%t|!$6!7bwxJyeiya129TRKtgJ2l+ zW6MB<=&`S;^-)t1g3!7*GS&Xkc6&d5*f7CUa$raSm<`sH3%xP?-0nzCfKf0gqpUMB@ zn%>yi^CiUoTT(YVqi=?`@%Ds@QaX~PFfx>~t&WZSblj$s_NTii3)PKxj;FTAqyFIH z?Q5{#0G>*c!pKu9UsA?=jjZ)5TI@G_%L;C|o4iBJV6LU4WZgS&@8tA@G?gY1Nm;pPg_sWUt?-02qk9C)th(oe zZzUfnCpl$<%IX#Oyzs5$1?46uUSQR)?it}*$q33#PK>}*N4;)>(yT;C_K%(wd9O7V zHuGwYDv6W0LDnUDEt27z?Sp#f&4|1{rlv}%jGk1u=LmIL;hl-A=H>_$NE6i`UJEna zdz*N*Y2^VV<>XDI-{kZ*so9#>6gta*y5_rhQ0#Bv@osb@RHd|6?eDs!P3pHcFAd?7Hp#b$Q?4O$LQU% z3Id~7J^eH3x$sx_Xe{5aWg6Ar_mq~Evnm+3cyQ&Fr)<#4^EIR7TRa-NDyv4=%)geJoWo;R{JyX*t&r z)T~+=H{7*w=T}15 z=Uqihtl}a%DODipBg@!~z`|8z<3@5Dw-`zgsWdgMB1V7;+SW_>i|d$|T|!b?{Sn!0 zaIdF9;ZgTp%JdytWAe4!T5>Xcqf%<%8tXdld1yIRt3|gcV-I|Unpk`=qtmBP3l z_|0*9v)M!?EwyPG`_Ikn0>Uc1UUzD^oENi$>ku$IsUXZ-##`t2@T%9iauuz4v$4)D z-r0Qb_y@o6i+^&B|9ofd^y%#R-}t$;HTwNG@!@oEg>s3b7yNd<{&0M=e&yoAw^2~K zy&7(O4}UCFHKc^DCj}IGD4?{3H}+BS1NTGC`o(79dze?Fp{^{t{-{9)G@y)a|>W`P1^iRWPY~1_|ly!_tzYf{`VOxIWk%Gm)@9cmn z3f4BF6;uA0oSxFwPV{*}=mga0GZC1Glc&MtPt(v8yC_0-rfnFXxay0yd{TfKH1OP^ zevOxUe1Ghr)D03F<>TBh59_Unh4IKL;+B<1s1}HuQSpBhm#^#MJ~5I5T#h*0nt=4d zgkcyN4#K@@!$~yZRAT-mDVUxQrUTRlQfQ`*DzavF#@-8M?8~tclNC1igT8)Am?Flf z1Alg@x?s9zVY3!4WDTQd0j`sn<$Zb&xB1{IO^Jzst=|i)@HoH`uPdC(@YVAi^P})k z0j*C?dr7Q~#8m9wUJtR@CR=V|CJ<~*n~8V}2|GgAToaK4H=n8)v*j6GM2P*R;>DiU z7(RkhK9<)SoVvYUguIKm>G>eI0eRVRKskkcO5{2}9bfOmejh5yrt!-D*!8>a%b>Fp zn^6OA?cSoeFMKGnFSRJ`CM;wu#6}gXGrGq|M)&!6e6{C#AaA059d<#b9{Cr_Y7O6` zejk;+63xG;(fzYd12x?m^;V;1H)qJi*Lh4E zPB59>bw<-`kZe5B7%M{61WU=5@F*>4nrkU*=x$SCv|t+rN_();)eN$wAe0DtK7!sE zwt*cu1@k<+NBRMjF!iijmijV@fX@)~CnTOtR7;oicBqBMhLJApaoL@zgIppU1BnT7 ze3D?DhT7yc%BHZ6JHbL>J1euHk%5f!^>s1O=1)HO z@q9e{m@dh~ZPu6L8{o|Jh$;W#G5?QzKm66Blm^{-FZbg|qv74o04tJFMC*h(X!$2U zg|2!W?wS!!{d$=@p~FOadTYFWHo~T%jX%!b8|zH)N&+c`hby#9$ybY?@2_qAtl!Yz zofiFVv)A-uo8E7IbbPZ-@3xMo^xvoL-A~%Ro2@U~Pp9qYH(S^3r=PSxzu6kLpMBCk zxY?y=Zt=VI5xo*W9}�&u}ts>t*}tLEF9A`eFO&i}vU3tx@~wS=(>7%ly%ub^4L* zfK!8BpIvM{YwzB)pMSEo*WT&0JMFD<`&p;``6uWX*nM_GaLa9v!LQQibHZo;X0>*# z|6{_WiI-cocDs+)H2$rN_8##6WNX%bcGm8-Y5Y};r(JLFp3!)&sbA3NuDvrRyoc?l zpAkNHG|t`y(TzR#l*W(tpxXiA*=3*8^L$)$q7&M^pz)ntP(L^H{ws!CJEYU~4ZS8j z+gtoQ`n%I?U(o;082z5NbvhhfY<)ug0M8rh*SjEkzG!zD-42M3!21C60=nWk;FtP5 z=HdG^eow%`Je1o{n}A1sgRkg^=Kk_C_L_Z2@5Rr0oAJxmN9;j1hIU)g{oqsf8qaTi zMU4&^1GYb*#vS^@$NQ22+@TNn4}<9t{utks==Rws7$@kAc^e2ie$sxrOY?zog0C3g zG-*Dz>2uKIK+H#(uZNJ{vKRc%`zL&ME(8zW5RA+Avo_HLa|Rj{E%(5qL|fv4_SPBE zh~c)|q0jHydmX|JYvBSkKVV%b+C@6#xUlinUvYdGXOrfa%~y}nG%{bJ&xokmh%0o* z<+g+2uwX^Bw(^Izbbe?8lw1dA^FiGHs?-{Fzg%t=>-~OEtkzro zqFd{G#cHM3a{6`GbD9y?hPJgUQMa{|0@x0!w$tjjioHsuUaU5nwPLGM_KQ_N2t2Q5 zdp+9~qYXvs#(KnSKrXSL`}yK~p;PRa{=>Tue)vmkYxMhT!c`XjH&rgOJ5cFrx;|z9 zpWEatiG!N{(r^67f8t+UTcckqgn?K88GxHWO~&I{8nplK!QcD8|5JGC=(nsv`#=8W z2(+GmbxZ#J_5DF_;!f_m z<30bbzxH3x(eMv`ac%8=t>HBgQB>j(dP7(o{=mhhzgL7sD#U+uyZfhq2atXpP`|4| z{I~zL1Vnb&@}e8Syy7meRFI#2_8E0{FnY+0R6i^zqa;oXn-ql_n|t0 z%XoyGio)<21k{b#@Y-~HC#1%NJq>C547{z?SU)y-m1|M z_x?-Ngp7!_|Dp2{qtY`li&FDwKe+HC-N76k^+WXFlm5(`>*`M=^ua_=oh>d z`S~0FMFddK_k+GWAI@UjK+k_??fUor!aw*G$S(F90kO9B+kY_%B(_T9wE0_q?Jxh+ zpJ9d4ua3(f|8FB;)Y2@c4e0#O{r(@K(>3}9|3rTN2k+-XxuJlVCi`c<{F}e^&%r+Q zYek@}t^MbI1P&z5^(A?PJCv%_cfFwW<#arX5AE|`d--=-fJDFGmB`Qk(vLu+Sp0Bj z!1A3Sq|?Ut?4#fP>z{)_^lL@ntgY=_Mj(YEhctgkcm%Xk_M&*KCj|Hp3qK!HD})vi oUGXu-g9jn3zyE#&)=d}_;28Z2*cxat{L0z@JoNw6z=>=BKk94N%>V!Z literal 0 HcmV?d00001 diff --git a/model/float_mil.slx b/model/float_mil.slx new file mode 100644 index 0000000000000000000000000000000000000000..a88d76640b4b0305f36d60c35785c65cb0ce8cba GIT binary patch literal 137488 zcmeEv1zc2Hv^R>1Vh1)Mf=cI55++I`-8jMkL(JeXfV9n3R0IVKKvX(KMWl^uA|WZQ zB2p4cNq=iIGYE=Y?|tvS?}h#x80PG=W9^myway-OCHh$_=qUd-(49Ebr#mW3M~7Z3 z;X^wd8EcLuV+F`oj&=tKSiCL2J;Cyny6T>VjB6R8!9s<-yEN$NW^|+1+&S<`Zdu)F z_%+Xduf78v-JCk~nqk$s1+KBtDeT&=btt&4#`%zz_QL?m5uLN_nA_*mllz%3Yk7a;wWR85BWQ=40{Cqnrd=rSL%j&w5AI91t& z^53kS+dYS$oqdPbaI0yGZH2H}&`dfL?9kWG)@AXRqjICgW-_OLdC7wzeb}oOGy^U@ zIn5Nx|L%<7%YURIVoVRpJAHbyA*#5!Sy+8xc6N44;q_xtxpeAEbIHMesv%M?>9e#h z^ z7r!0n3AgmQCF^EMb+>19#P0T48IYah)u;Yt{Ry;`F(5PC0#Fas^r=uM%QYDreAf!gjS^C1$^FP0C5UapQ(!YIDV%DA|#( zW*&Vt&go4zd_>KkFOgioNTDcH#%%=)OJWr<$GD=R!XWcd@{XN5F$dr8;xkUys`Kaz zNlb6y-YqX*R?78Ika*TQDzp3X?SYHKjV~MQpNq5Y6*HAvtnFlBM2gw6S;>F5RP)Ke zoA>uV9_df6>}75Dt5nZf+O$wlOY5Tix%Dwej~?ajfDM^{mPN+k)TvYc9IULY`UyIH zURAbYI7`diclOcI(R;I8d)Qwc)HxTmNr^vb{gV^z~_j{d`$bg?8d@% z(b&f)@o#QnwJXIoZrpj~$dNC7Nr&#YW%bp@-MGQt-ycvtV072YFIY9oR_v+~|0?#B`t$oIw+#~79|lLP6flYZ^vO1>yGRMw ztD>q}RuZF`;x+17<6P%~b^+w}<)^EcFYCV(;}25Wuwg@#%(EhO=QN|zp=P?#oniEy z*0P=>=^!0(C6+AA%n3RP1`6D328GB?>I>IPs8`*y)0CG#d5>6^2*Y5AwMI*qF2(iA zxPQGh(&;T{_2~FKTyM=COW%_V*07tJn&NP{=EiGYiJjKzH|$_7rT6TrxI-GF_uVw_ z`kYX7Z`*EA4-eviBfj(`cov4ZwWqmuzuprN92~E3VG|c0pUS>{`)*gY!GFY^u01Uc zwb^cc?XDmE_T05BV0rTFSw>$7#wKvtvSlT}rn4Cs;%?o#^@s=W^Mq7sr|Ood6K8IB zs9rUD=!;U@o8+6-#ux4|fo#6Mxu@LfYVDbA4ExVyw5Kcim)zX5SV04ez3#THu~<#c z*uwSw#fvLAIXTn%n72B`GtQr1<2+Nf+WF(F%l_f3Ox^eO_xFG4>f*UWG&eU-vzs|{ zrgfLr*)Q*X1gzixd8)nL|BA3%udT6D1&(piqQuuuSxiS3J*OOaa3Dq(K6g4lgmRWUu?6Ma)vsHsy&e)oFLWeECw6Vx#NWR;hjP;6X)Y z<@nbIhwp3;qmN!?z46rrfB%cHDJoaRxP*lEUD&Kv=1?FL1r$QcZvezTASkFY^s{|w ztTyabd*{OW^EVc@z5%5wZG5zxz3>Pl?^pAnbK&cI$ZNg|1m$u@73#DgsL_WRSk9Z^FW~}{W}(T*dYT@1)rSUPB`w`wQGx{WLjCv%=|gW-XFdY z?9j(BpjQ9by8GF7#frN+dG&K#(>ap-gMy-hgV$s1D(~i>(|0}Lc>@eeTWwaxE0LCn zst`hYsYZffYFTZT8!pA5=K=lVXvfwv>&m+^ZhSL?awAU6U-c5HLS)Eo%K3ZRYciC- z>X30E(Cy*Fbj*^ZUEm!EtSURZ7DG}ytXXBmAEcq7v0~}ceGv~mqjTnx-_Bx=%)EXh z*qw>tL(G#0T|F(wc|`G==~1~~7(q})EM66Y=zg%5AlJGy;`_fefPG3#HnXj~tI*Y0 zpc|F*D&Wcpu_&mB=S@fUpel%5HV3|=Hme2rJZ<)2q-G9%e$UM;ASY*f$0ARowJZT+ z|8|?PW2v@vapYMJ?XTDOL$57h(FO<4oLL|<)cLr*bhai;5|qI3U|%bAS@zs-BgSK( zo7Cn$I^1VnaXVuDBJZ*!)1)KxHjO*$&oOT$*Ep6Vc@NCV$yv#76b<4WrE^YOTW;j* zO%|`AgLxrab!*Zc<9bJ`Uqr7zvS>)xTR|Z9YxSZ5iO(z*U5aIOEUOJiSro2FIz+O# zHeDwr?b*E>|Mbk7_PBzU%2eXDvUvUa5zo9Un6(o2VW4A3Pg6o)y|Zt+Qy^Qsht(ro z>{yaUwTeScN172oGFdHU30H65zD@q{d?|=>45-(3bpdW}4HD^gV%oXL9$M0mJWb5VqALxApGQiGz*m**STiddX6 zNF^lDGc1Y@3)|)^I}mN+*^fhu1Di!ZXMuf3-KRb^M{={Se>nc$ovRWA5DO{SyXL%m zLBT|-T#XdS{<;G+n`Ld2K8VXeU(RR(mF)rNY4>qWOprApvpQW7MB}{t4#%SHmu|^+*5u<+T}Kg^L0aFmbQ4c{jqy+Pewd850V{N&F+gB=GgUltUo zJ$(33QBAGfts^U{Kz2|W*gB~wd$Ve328>xuU|qR#6YI5>XVoVcuDfep%H7jem0CII zy6nigIUOB7x?66TF6Lu2NvIup#^Jp)@jZLY{k@WDF6kZB`}y7&q2oEqml_N%LvL5}QxTSt5x(;iEOyj%h|~W!>*HGBREa2{C@x)*>?R z&!RoW`Q|yxL2YTp#Ke%&jJScdK@&cD^r$4+Y|n|~$D_@jokMyGJalf`n}(_Fx62CM z+gZJal0jN^KBj=C3ThVXJkG?UvxSXq+l5UkMQ7HCh&r~EaJT6OERpl8A;{IML#tGs zBopI?#p`MhZ`*S$Xt*>-)JuJSPy2dK$qKXzE>mj0&*>5JV{ zFc$ci7rCr!GyB)1i0^+u2Rc+0l(oMW2_sFcmF~-I#$X%sLqOux~iJ~iA3Hu2hEPJ&KcI=T5qS%TrU zgH;Y6U+uH7h`b_-zu_x4qF4X)?AAB~wX!>wYQ}DDDX!LW;kQ=>!WJU;C$~*g^H?}I z_IJ_tdAxm*pU>|+RJ61!Vq*>8N*^Syj~?ylqf65@^(Y;FhaDN}$5qa?EmHJL=F}}T z&QKCMV(F<0kxWUfcHG6QSL0^T&#`JMyxvk%b;s8Xq8rU}Fji|ih{<}$?cvVHLjHo7U1gIut7mjkdO@_yg`k9@ zulZv7yeqRZ?R=whKP!q|RWVK{`{h~071tG?PCRaNrPNv*+?l)Jy>rXdf{zddJEPcY z4LI%(I5MDQCF%Dg<28 zBbVg0Ge-tmLHCA$9LyyzHVFLC2~L54u&~B4hQ(zNnVDEVi;p{{tH`P!w}|Ct?AF%u zr1I)C2elk5J&qj4z`%gwxuS4|3)03f@v~>oZtkn|5)e7Cs#CtXx!E9%gbQK0!=G6A zZg`-lHLALLpmerQ(4_$;dG4$fy#T{A+WMc~{kgQ7ssF^?MgS(7p|rDxBX6H2OnWS3g=F}ucO;1Y`TIk?!^9%R*AekHGM3K2;Q z;3rANW;!L_gk(MQWWDMFt)P)%`*j&33ZQ?tc4XFP`{-zb!=`()%_ZYxSJINT7nfTy zT?hv&WL*&Izvp#tdrcgQ6$Ed6m{E6nWx;txi>*8hyWpdj4J3R)s7$+<(yERv!z196 zIHu3M93@Jq)&t&)>8cN1pC5rGq!`rHuti+RThi2%TQ|}xo?x1j{pd1%ffleMxhqe+ z{HymYqE5s%@CJ%NC*Ob|mq$j%`1eXET!X(Jg&Jm})?N zw2me3O3;kTH~09KA+%#tNrQhct8_R$qvp1E2cL+|QwL|$9JV60w)L3hH4mKvnZDqX zZ5?C--jx-$_e4x8S8;Ny)FFqn>y?OWb41X~3$WcTkL(WY0;5!!zqD9Na65(jXQU2)Uvo3LzvxVSQE5)Zf~-?)m= z9NgwDB~cWR(IpOy38Ha-72$Jll4YoiJerKKuyD(eCF32fwM~m4n-C!Mg+*1C_1+W5aJZelk?y2+V*P6eMn4g$!(DOLwWAH&YU})0W678$YmM@2h+c58vP<=c@f>yV4^1=G2 zBpb2EZU79~fJ4Hv)e~kqGSn#7`Xprz88(b5*1RtK~Jn>R_Qh#m$WuG z;n4(G7DSJ6_TB(-!o5o1z_yozFl>fEAKc_E7)**St1dHGtH2sUIruZ-V4t+X(rND7 z=f{Bu#}pK7#M5)kZaUBY$QT~gdUG?beHgq42X4oDskHS@9;08DEzE9YeF>h_P=6QG z>ebOOUwp&MfVE<_mk{Ux`hi+;~=}A{9aUXm8DBtN_oqNL#l55KF7nuITpf>HHJ8cx_fvu zbaW)ipIsY}76wArU74~JwlESTIWXsDGwH?0V*95Xu3KoYZc>f9Q}oadT+#I5-l|+) zg>-!JriwPFtp1Nzk*6+m+u+Tq;XbDfV=gYP_ECMYyeDPu*z=EJy8$;Re=q8!8mpivt$H0PO!L~|~ zKKua)O2qT$dlfVe9!y3NSzp)d+V_9DyqeF*NdDT1bq>3BC?9l6SxaeLLw)_2+=0q~ zselkTT)R`;%-iSe2Ob6TKg2SeJ2n+&>AAY5k91udEvfi?rB7Pz+2S_8*6{rshSjwZ`rsbj;uPoC6qo!|Y8)r+^!pqW?I zx%!~@KyO>CA47j1m;eMIdJWaH0@5Vi(^{_980wZ7?(g`=^5x45?zeRZjH-9CM#lP> z240(7#qMx51ONQ}!K&X)C1rdrsUud31G2qTFI&G|@ zu6~|_AD35{>YFb6*vp=Eujdbz!36WQh2gB(TDx`~4|Q$fk_=I;uoX)o)EGcy77#9+ zbWN`wRBD!|MkrwXco?n)K*6*=RoTYn<)N$)N4u#veEnQsleBoX;I=s3BquL9NI8IS zgnvL6x;lDsi|fI_qQmumo8Fdido1z*_wkCD-IZF9>V97&$cvz`!@f+MQ{ z%2fw65nzBOaZg;2Szb6z-f792b;Z1o%+(VJ?kc1=WQ^zJtAn$S!es-Qr-!93?e6Qb z)2L#@S4$Qd+k}5SQnXvBC{Q#;Z8_J2Jp-J^FL^FrnYH)WjflKG5wa=~i=CB{uWPtJ zzSOp=XIDfB22cS2eoH{ZVhGg|$kbIPn|T)%7x#1($l26pxU=ccAjLW$)cuPBSR-x;2|ZAO=^wAyl*H?c zobdJ*za+@8HXZyrz`Lr7aV3AO++H-_FoLI2!Df-d_Uxj=ABzmPMyTwGm}eZ}w81}@ zbJ$gRfGgsIB1@-yRNM7rgM`y(&VWCEliX9P8wYp@e^Bo5C(E7s=Nm4l$-W-T8VS?+ zcJ=gNBHh}q^cI~pRV*i;&(?iW+V8)M>yc@C=GpE}zXv- zLfR*FFne>TP!iwrP0EGy1v1}yYFPvOB76^MrqO)H3TO-e0EsOIeQsnidI7RJ*GA(Z z2-gH13dskQ+YHroWu-*)9DJAHCd`GBW#*dJ!MWNo>2Vf1!I_5*-~& z1gZt#^a;l%C=v=7AWvozyIrmm{4xPWe?ofmCWRuv;W(P_b_E9221vM!0+vudJe*qR z8JAk?ssxj9u5koSI8yE0^-LQbx~PCfvZb9U^vybLIdx1mCo$B!+rtC9@?$-ce?`aO2nGDk1p>wB^J8gkEyDoW{ypuw&e$c4E*etkT|*7r=RSzdv40)*kx943<_^1;B$-~py>JrLi4NdcIoTu`zy}L0b336kOC~zx;!yt z5v%Mc0uN+y0UXo0ChGxN(J07XfxW7}#3j<#QI`bH?aj@rg-!t^%FfPi%~7~>$Bq&R z`x6Ui2o#6W4?_Us+7c605~aopF^UN|25W`Q|6n=vb_So(eRV9B(`Tb0?@CDMG0S-x zX-27*zPc>Z^LVk`80HCz9pm!FT&Azu!F8m+vBnN(Zr=2C9kB!&VIUm|glamswOw#i zq2WE-7t5j-YC*8(@9!^dJm>HQ(Wb`2RB);u9%E=#^BTAS@Eo|qqSRCm;PE74BmB!w zXMqW&J9qFaNI8AFl)tq@8bVf=N(d=0-}4;q(H-tCR%@^72B(aTuJsK>cQ?V6-nQr5 zddUW-fIJa(YtXlrH76ASghD!JjF%Ql71r0+gT>?K)4R28k@rqy*;zcg_o2?f!Wv$- z1DB*5BKp9~*(u=3xOP5dK+QoJMkyO2fE%VuB9S?OMEedVkn;FWUij&9HDGht$43aIcJ9T`S20`MH<{5ByliDA+DLO?Ig zQ>z@`K9~V$F&88bio}!~$Wq$s>TjEc>4L+z*O3H(BP1JxZ(h2zy5YkIL~AX;bccqA zQy?7$T&Ku=xqkT6=Fs!G4MitbnQR%uvQ0dCw2^LCR8f>)50%HUbB>41zO^W&V0ddc{FT zHz}+43gmVoQQAQY5^w;A-vBhjR89Kxt`l~bUN)0MNmtD2LSKg^6+Q+rhJ8gIhIs9v zn;dDIonE|prDb4n1wxBz`gf`$41)khY6EJ?;@g+8Rh{D^{!s zhZy!bK)tF_$Z638x9{k&W2Koc4tdg4uBfWTcqHI(O7Y)rf<4581WP_^P9p04#w^ zW%a_J6Xl#iO+pfx0R#c)j!JK~fn@7g(?|z?E(QddDRy+8tjB!CnwR zfLal4r%#_=Ya0d&@(!&GynHzZ0qswp2DY}0O_DMKN;#&n_Q#)I$!A~%n45L={bZpd zp%*WPM<@lARSH4Qwi^=DF~quTOXj9U-ZvGiz(a|>e_wA++HS=HuE1if?)hsful=@L zZ`-Mmx$MZIFaxMq03W`ixc9nlA7g%bxh_infhP~1IZ2Gu^{hqS6)u2T$|?&W+YMU6 zBw?{CX2ZsU#6oqhd-`Sy8cu}k{);W6?ar*R(Js-QC3V>P1w@-*us%WfzWv=1S$XN(oN{NS0ONtFRZ8iX_j3 z_8MFjvx~L2HrIqLQsnN>^7`^(Gk_kEkU_p!5owy^SqX~ZEbAb^KnZ2Nh_3VI%|obR zC?o;b61Nt9Jg)@ZSb%SGMb;x7{0eDf#gIRIeS?yck`S_3amSJevXhX$BwV(;XIt_9 zddo|8#PJpu7AZ;VV>kX;7ugiKos`D0)uX5LR?bj^K>9#STxq1rIt6fn{JnW%*Lwi@ zIROj?%kPqqLbPGK8MW^bz$IWNaVCbh_HR$A+^edZ_`?6?eMmD`W82&A?*Q64CQB$? zSmHHXucf@H86}q*gB{LoQodBUe;}wp9<~U%Xh3dbaPF=MVvoMBuV-4fF3!}me>Vb+ zAYP3FMZhg0qDd(V(1hT6&Z70vC>1+0GSUKx=k$&uK~Eed$3*}CFv1w z0Z7fXb}cug*h02C1Jp>wuY=ww_)zN`}G&Dj~a@FVknlv}#oxK+(4=Y*DZnUv|>ttRj>W z1OliE3>V^bFt7z(6ULn$=<7&=OiP(nmE$`wL9m}!R{(K=9c*4*5=!qWGpy+Vx08Fr zhV7p}f3|cCMmhfOrq`B+iRDl@p$>ius{Bw1me>d0e*4yKDER_VdvB3pGnC!6mwpAb z&E5TvWQ`#XIxQuzaG-ajjZcQr~sW3VmN5t7ybKy85{Hwdo-pfC?$GSG{uetm7EdvjaGojp*+0PvBJZ|HTC zF(zKIa-}sChOtQYzVMZTjkf`d4a{-c*H;&23@MTnsJKHeDlz~d_}$>W0#B#0o{-wt z{2;bSZvOoFj<0vGih=SxM^DcO6;ST1r35yq6p~ZEW$FXb#q^7%*}NZbxSC*o`c2WZ zODlQx!T=JfU@iiL#5qY<1}2_hl4YQw@JEpBTJ3Z#T#`vvORxFiwP0PN1LNa)Js<*v zbW(!otB6r!+_j;(T6lo(b^9ZR-!JrFUv>%VtCMi5M|xSCgLd%KPTYx3I1fNRY!#Z<{q#p;To| zwamAEwsWyeIP45W*oLEhUZYU}7^AR%t>FH{02n($DP1tBvIga(<%T+UGV6SDfho?%7@r_kW=e+)jp#vjKR)A9JfJ`+P{1@11j#`1 zLU%JcymDB@*ebI#vWXE6BYs6jUvhZ%Gc0-jL0Lu?(=KCT!MAc~mUj076 zlh$ZnDAOP}^4a%_I58CE|H8w=Nk?RHFE1KMI)UE>oE(=agS-%75fK18Oc64TSOUCw zNYZKWLDd8#R&WEZ7FDojBMQU5Mq7EOG z`)ppkcmah&d23Rf(**@m07iu|%id_x6-TKh>0DAm_WzGlr(Q$sWzzmS5{hi}(QZQ~ z76Z9mySi*+s8lUw>_$~pg=^Lu6Nyxy5bh9E+UIkNGdpZ6kR5VB5&gIWg4l;JqPb)76 zp@H(ufHbL!94{2U$5!{ognA|V!bp%?=>b%xl;X$sl$(OLoZmWg_)nKUK$5FxWP+V$ zmT}cW$q7^og`CkDGiHEd^#(Y~Iq$N_9*EX*Kv3IrMn{D0R)Q9V@}_if8oHz8h84Ct zeL4>X4d5Ur47O*efzmDNZZ7Ug@)}k`6|*^>!(=F1MF|Lhh*>&LvF;ApP&gwv1+sNP zZhT-ZuX*(92UJ->UTmSx2j&9@4wR%hG#c2*L3C{D(GviP66?0rmMtG#`ji-|0(xJg z(i2~q-b?3LWMZ!p(iLEb(g8f|8R{wkfaAs^2Pv0Y51%JDAp0W&Q97via$shj;Ho-{ zJ9sFaeOjG+$SbEZqHA%8;(}{HIT!*5WOpTntmB>ajn?VBJvWU@6wg zdnP;)G;8d+f%^@M=r9kO$!q0K5A6CO%Gl2n=NfXdtBQdxrK!x2SQ;$p0Oe0m^loh| zA8O36kes(R&P+M_=uGzj_cudby-rXITypr;E9Java;6}dBC>BD(|dd%DP)|32=yT^ zj-^1i>XN-$Q0d!)xy*-k?IqTaRT7Kh->0;yTzd(1;v50W#!{f5V0}8ENn<|Ohet$| z9=dADE?TJTx7`!m-HgZYnn`RknGXlun&dILrUzSR^j6wHA=f698v%d46h&(}BRxs& zea8Gj`4KAhNm`&`SUgI5rh-2Tg^l8r|A#<4U=quEk@ZIjTJ+IS5R}(v9*~fBvzwU2L=R zrLeDG4?)lhp5?==HZVk5Mn>@fVL*hXjXYr_Ved|?a&2dzODfEAb4GYfxrti>?@FIB z1WhBNX(z+m0H~zZHZtM`dk8g$fKKf}(*>KS4LS@^C}Z@X2-IJeW_!3nia#$)tQW*S z>C&Z3!dQLVJkSP~bn)>!_3hh_c;~OjJZT`s+`s=dzVZ;1nzTMS zEO82o!y~}Rp(Ufe1T!L#8tMcQLA8yg_lXmD;6+&PV0W|TJ4Hi~V*@CRfL+^XQAKsr z2S8tyB8=$@Mk&a|6(15x^auWf^GYM(1pHQc(3p-hwU?|>a6VZyjcE8pU@U`6dC>cm3Asq;nRsmYK zFIs?ChI~vwb($aZ^w?>nwlD6i&v|&pjHS1qT+?( z)#@L=Zg#pJ4?+g`D)*Kx=fSmytVjfSPF#RE0iwX~91NupdG`q`0hlc@j51W)3KjRs z$;seHYomY}(!7B#qkoQWdKH&+*N`2m<-n1MLl}(TMyPwsw}LDnA@j3*h(o?4LWXbO zxr4DPWE*Y}@Oljt0|>3VEwwI|8kyxwHm+}IXh2XUnB@VWF!F%$AT15&Oe;^^2IGMs zY@*lT)xkHggDp^k2k2yEb#*oAnNaf86Fg+Pr%<=r+2mS@{0vmWAsLcyfcJv@6_i)` z?bspR#rqZVdNA-^k3lEoEdnxloOh+t<@PgB{-s?snyv`eM|QYdvkk&eAbT+s`Z zAqa-g@SYZ!R7m(k2dIV&RW6}z0t-uqXQTopJ@Km)p@I`?yXHVG2cs`vfcx96pCtfJ zb9BD503PfJG9oF|48OIW2dX#afyJFM>;d{5)c<&;42*Hlr;4tNzTMgZHH4%hC%^=8 zIstd|cu_IBlyiGLRGorDkY2^pHgG;XKV+-Y*n<_wI74Uv&nY>WCIk_@o-pY?hl(J0 zGz6*_AdExVS&&eGHSfCiSSv6&U*B8w=*0Y7W;b}U3YTA8LRblpSXv)}!m9vX6D}x2 zx6y6#hkU<{c}1B`Vw${=kWf@+8vtRTvP_%9g(I;1F^{&N@<)Zxuz6`oZ+I%R?+%8O zuNHOjwC&*XNU$sjWnziCZFX%aQ~}Em5RkP3EGFe%0VUsY=E{**2a+M+379WH6vY>D zpqd@XNCyz=p=99c*>yql+EFkJLN9&OL#E~gy8>c~&c}-bAh*$0_xTQ>h;jGtOTeRM zT3=U(JZZ>VUOY282n=}lqxk|`1O(Kex;&{!7CE@$gkWG39RXd#osa#X5c2L_QQvs? zRg|Z*;E|(1H(v-#c%c3f>f-|jEtD+Xx;}r7S#{x1*gJ^XfQBsk(NAEy)+wYD@Mvnx zp+*1bX|vzYuKuRLZwmY?6nHy2Lrf^IH;r+T1w27cr?6XfSK^NSN7W}iGJh|=$JYWL zG)FHGU^)l9oue(DU?bq*;y}jPIZ%I_O7b7Fvz3;EA8bxIk^`PdkYN|%7i4F{5zL6@ zc!H%2yCd0xPl8=emQh-B>|R+$Ha2N>HdU-0PKI5XXoe-jUF@=k!oniFY$75OylfJ} zV(>#m#8_HDeNt;pgpXSY}~QA{>40SPB;SD6i@gz8DWXPnvDpMV{A4U(P?HQL?gw7nW`fS$O==Zi7vng z>Lj8)jzq@e94J(ve#}N88xavofnbh11UpHc*<@P5?O03L0&`8gB>`*ez%DzssPRFd zKLIojmBwT5CyyyT9ldtp>>Ll#wvBD-kLA)3RWL<@@_ zJKRmc9%iEsheran-LTo#we@iOu&)Gr1kqQ|CWphPbDK1>L`38%zscO zv_RkC30N|bG%gt5KAOl$;@@-91kENYhPGW4sW!v~!kFoFn~(&R#?#6w5(l6bZBhyo z)Y=a`{ryZRN_LDNr`NKWDQ2RKC*ZylbDCBb7W|PrG)ZRcvPki71RWtolU=0vr~;=> z#DgLMUPSW|+{y+&F;np5E(cJj(2p=?Bc3qckF)@7I?}W^6S^Ls*A&Cj<|Qogvx!X} zktY4qkN5-kjW2V2F0_s&%u^YA0Ed*hA)>$(YagF0W^Arg^e|3DA*5U>ROAzxj*kC? zj3PhZUSYAnB%}C0*f`N?Mx+p9imm-d3=EAHLetU$CO-vFNej@trpXT4`VB_QQW)sy z(2Hdn2Te%$|F?%mv!c>};Th5VGa=#sVh3#+hXc(;_j)}}YZnh`+ zT>m{>j`6vH4ioxc>QeltdfMMy2$}(B`}Psds~F>!|G#d<*rKT+0GiGZy!&6}TF^X^ z308B=DxyjKwQC_ZZU}^@vae&7o+6u583Gt{3g?1i>L+u>57vGPH~yE5J#{k3W%`d> z`>~Dt^@Y(q6`J*!isXM`?#HJ859~e7^PO_Y|0H`qHn+cJ@B?So+TJ#xqeCyYX$(Gz zNF@K~nS4Ho7Wl*=WW-ef?A z0`S1czX}3Th`q^{yp=l6CQI_x@ex3BCLlmaj;8KR!Qh1()i=;-e@)ZPl~fTM1w9RMM`W<)!C zTfkV%P3=iU3%o7v-`e7_k*C>%|KJ9r#r&h*Y4SD+p8Up{KhkHKQT{d`?L>Gp&2OUq zLE#!2YC5i;Cb%aEdK3}{#=xLAqEp4j<1$XOxzn;Ug2Ze!a27ZcAnrKYe8xVdaq%~N zd1@}67BK~F&dVl_CM=2AdCCZOe8y8C&|@<}d$J3UwIu?o34kV+Y-&a%;Y>+53w{UN zL*r^gdOL}PbzwuojP8(OA4d_llPnz(0Ej+zV5cf6O)?1*4D4*X@NkKmdf*lu7sB&r@LrK(tDdnGlrF=I@n4KdTscj0$e>TYp5a*;xe%BjCmHl;h&~8GsIdRqBbvQmf zQ0Hm~tie40oRB+#*T&jAK;v(_GEMK_ru&QCQ*QVN{Znp)QMLmpO>&%W8Ixe|Uz$9{ z7nptnVFJHIl5v77W65S#J6v`W?Oj0n6hQFIag>36&dC!2`(K*5HUUrGeaIeV4}KyY zc!Cv;CB>j&N1xd+df97v{XXy6tQgjpd87UMr3RR)}-2|Pc z8RQRXmWKKV#qx>%)B&C*Cx=1SWz6vv6M}cb!s9W<1f4i;Vrb6)gll_9SbHnH8P--4 zMyr|uf7h!7Mo>~hP()H#2qPvT zCX6Qf3m?(C1bg9tBRS#5I(49~-vZLOIO;m6=7~lb&`LPky$FDqNK->fBRK^;>Chjnly;gCB>r6YB0L23geh?<}d zKoxw`qtrH3Eij=8^#bLW3YOrAwWYkL_6BAMOE6a>*t$?#PFVhgZh(^7_VA?=INEZA zXwyMc!J(GQuxhfycCsd!X#Znh`r8lT9d^ODQfOcamb7(4Hj>hEyDijOj5VR&N%^%C zh(Kd!T1(jDu_5(PhmZdL7gvnjfhyaX=E==fx52RXXY&1y;2|;m5q|B+~A%WZO0P_OZ#nBw6j-vG`=&or+ zblyY4n^P}MU?y~hBI{G!riCYuTQbUJP9**m$BUvH0Dz%fp-xDDqROCLq5S-A9<{M33Z3UhYldhCl$%I1LE1Gj@-IpA@6LQO1O8v*)St2)l`J%qLt!N;A?nA7`>CAx-4%6Pu=PmWQ$H8^~q`VkF495QPii?TS-inBe)85i@s1xvzF-S%p)!%-ru6G_AV2XjU4w_ge zocVSKUEBc*mh&zDI-yT_gxW(u^?j)Mp0d~F<%hU!Bh|JFMW0qNvbIR zwbTBs>HjsaeTtn1Uuqne1=MQ-bWUTe?NEL0MB5hqvB=xe4xU_?B98fyIlj9YdkAtL zaP#+Zln5>DGi{>kN7QgU&HA$fyJ^#_;;5WPNL&(CB_P;PTxbdcjl+ghl#%>~4gYP} zkasM)ML%dBI|{sh4j@vM02NN0{MW=DCLxFbF7i@ev^GeUIdUsU5b#Tn4q|XD8fJ>i7m*F znf#dud?X<@>7TeUx_9DjaEmas6P?sdYz%In&^c=LFW{x)r3{q)rV{d7v56hE7U74> zQx_9Zn$f8LAI4loXg&Q?%=KSg-f!G?9Jif_r2YnO{~2&A!Y=}uKZ*qX==V*!`uAKt z1O`v|M;+ofKKm`5G7h}{#%CvF5+<=6CCxAa`k!?5zdu9%O&C^$DkZ;R*s;Qa-w5Gv zgzz^)_!}VotqlIRW=nnzh7}hdhhIf85;V;9e-RQp$*cMId>#oJ!23VQ3!ef`{0?uO zP(A$%c`r zH*r}wNQ6?(`R%DsC>Gs0@u-!*D(8d)ZsD<)FU+eMc5Mozm3HYb5!Q(ff5cBGUi}d}rCpsKH>I`uw@^`9|5KDUQDBWpDB`b_b545V z?LQe@9Ivzcr{Ll=llu(~jzfbJD*ApSgZ~g26cQ7XLUuqx=tYO{lA-W!otaf=w#Gq!8|R0p~v|J*6?z_oc?)bIgyZ_5VD+{F^x8?{dva zHvKnF_!}qujT8RH34h~+|F?0%@p8@or6}QduJSK9J~T&x`V7^#r(mXexaxcV>VJ?& z{?}k34UGFa-iIEG94pQor^An+-%lXIAN@!DeO7!e6L3D#0U>1#fl;cp~$y);L{7cDk>P<#n zY3Dmndt8e!I#)&%j_MLchs+3xjh~@Df$heNVy8GLR~dhRgeAFX5*$ z4bGR5VLxDQcgP%2WvQv!m`aYHyFw%7ZW4(|g5#Da9AwR}MZyvs;P@0AKbiLKM;=#4 zhaHpm;&Ct&v#|&JHE<3@TSv+P?eILVE#3@IMo;Aac&`@P|LyRQEzu5dhB$NT15q^P z$Ikge=drNMQn-bUcCz)4ol>7moA#uYf924I38$;T{{6+74bXzZT9oZ_7&~mAwlCAC z0KR8ROQI>9tz&5khul+B0ko64#!m8@lnVHTQ`d#yS!g&)Ul<+pfZkywrcjsTTscKn zWh##QC9V|u@nq89Z28mU*k}h=O*&5tJ>mP^St65Ti^r>iEd%`beZ<|PIyr4`@hi{m1E;c zz!lnfv&LGXe!q=J8y8(dW1+ukFb0}08h5(;(HhXa(Z40IB28t=IxoTBwEJk+CyqPu zzW>NLe|vuNK(|TXFE?>_g<#g>-!C`)?!vd3{NCPiwnf|j-)(ylD{r97V->cp1P=6o zKbONNhExc}6d^7S#^`+dCxL1Xm4Zv2bhtD6CN2mp-Wl1ppES2wnV+vXyfL3W$&P7w zox-1;Z5eY~9xTz=QQf>SnON76U~c|YoF~D5bN6LbpCx2(Ok)WIlO3UFEqx^Uc9mE=w@hb7LH>qBRVXHWU@l%kS`CMFV8e${nr!Q8wJRR7*_gIM%XC8Zk z^!+QMEujPM^JaSI9L4aa8Tu7C#pbOVTDHJLLr|@UG1I%po`2!jo+EvU-B+?o+TP}C zF`R81Sa^9+D*0|ze#y;Bs~vpCtNq^|-d<{7@wR59We0P`z1D`R^2q+RfL}?Lnh}$aM*6)KiIqk?^I+B?_!djx1wGH!! z!4=(eJ!>*9^d7wDfO~R)`$+AB0nv27$XhCJMk~(rf6RGo-not@=@eE_f1cOv39P^h zifKNu0-XA6>mb07enynk5RMA2VCZzacB46K#pMd!UBv2T0)f0&-41V**|yP6!7wU& z_@%7Yih1cu65U>&@9r|Ih}~wpW))w^9eIo`e3tf&Gj?k8e#O|cvl>}do+|DQJio!`QD@-g z#|xy|mumQMim(*BWc$ybeIv|#(WafNG*2y(kl5dzwDKZ%SC8GPQIT84Rxhs@`|Q}9 z8*}*8;p*b}^Km<(IS2W>u94mjUVH7LnO$aqKekKho+0yzW1V|-TFW1bK3So2;*N?~ zzL$^gi*^I^j6}=hr*vYT*S^lvid|=wGN7IQ&M&d{^8zL7b5FE5N|gkw=Dc~mYE`jG zY)?Rk`7`De*^QzivQ`o^NFi3olAd;u>SsBzHjOM;aN~lU)1@~vPfHyR6v7Mr2f{=BgLz@cFZD%vHPak3hq59(tg(Sz-%FA#)yPtd%0vk+&+^$6023nDK&RZ zX_Ka<>L<3yJ^We=U1!XW_3<3N^WN#?`o80De3+JB$nEGob>u38!R@E4$`w)PvI zY_vZj6r}bvVc{-yW}*dQ;j6;%p2V$}8Xs?}{9=E=`D3jf;iQ7I z%>z}N_JwR?$<~z~8hz`%!Y+`hc3J(oibRX%wQ66aHTf5BaWkt8t6sD2l&{ce3+u~@ zb1igC3mownt>vtKjbVoCq}p*g^!;7@?JbAdkGiB z%bVR!g%Z4-ny)N*op))=J)^4!7%MOZ>BS=(tS^-B*4HOmF>Ufx-KrROXoPf<;aqR{ z@ij47bq)9XzT3a1L%Ua}!@g~VJ>>jx|ICO43%n&9kgGtjAdYV!&NqYbSa88^XRi|v zvLZES?cTR5Unk7wg6Cm2k=y%ymhI~C%MplTlb1I++Sti*&I^x`)8e$acdL~_@K0Ga zzhftox(17RR^!iYPPN!HUw4_{v*fMrMJ-oYUaa?79dS$Vk|7WAkN1{}ssl!UI6Az* zq(z!7)C)E>VsmFXxhlSI!RoDMVtQRd`kj{rgo2}+EiHBFQwHO8Ge4H^SWME+&q?Di z8VpuHFkrG#fV;LW^+4u3x|G#~D`#uuCNr<>4%xofu;>(6JlDj1p0F28oM z;%QhIaqE&5|LIo6#Q`;>m5dyjU~3Y0Zb0 zamkwSpNG9s@nD0$q3}(S`agz7(oZQw6kdjv15fghCmc$AU`^zpKf4e_uv+ z>rVX92HPX?8!r3Y61aD(=ZLC8y_f0spiA4L&ZXDaGL)L&y?PZOC76E^}TR0 zB$eZ7tz~`WtFqE75p`>et_`)T)D4T&7PjoEde1PKM;tb>jbecgh2R9Q16shhlVVXi zO8|Wp!M9^95o?_F->3FSaK(wbekP^5R01;_em{rxA^$(O$k!X%2^8MHr)0Q&B&+7l zM=cijPm-o;1?}zb{V^|_=*kpsuG*rcvEs-_`Mp_&VNrS9q1T_gM9$_pdvdnak)5w^ zug-JXotA7*RGD3-WWR53(YY_yM|x{@SC~EP3%jH=^ZKzgjE)fNIVW$Pr*##J1Qm+8 zblzUxANo#8FmYg4$^UEbUBDzusygvvU{-cSg!%JyQ7%c(*xhti<@?c7Rgm@S%9(mi zWmY#cBNjI^Z&u}WK5Ft&kHH!J@VO6UJ`h%yMgQXd<>3m83L=j`t1g1>qM)wpj_jbH zuFJBp3xbQ_qWe1$H}1U=_r|T8NsB|z^n6uQnfIO(Cr+F=apJ^@b5?G81Zti{Nty?QEDSmkItt$`T@MHHp`4@iV*V147 zt`81=@(XYHqmSP7%rm$AUf~1(R_br15zx{9i;_@rsGe7#oZv7AMeAi!m@9n>x`+<-B%dONu`mxX4@ZOuB_0(T@ zUX~Cw}1Wryz`YG`qVeeBlE?lqt-n}q=M0* zTGP(RbdLOEV?9*57jn7$rTHKH*#H0f`q>|gz36?P*!t7i#4o+^8LPSC=07hz_{PJT-uA(b{iogh zC&pgm+`E7FS+((`nJ`-%5kp7OI_euB)2&;0VOxBnbC@Cbfx4dcY}F-kvB$qN$Km;LB%H-EqJjHy>_{_4kG zc=M}%>M8fW?)T5V=84Aqi+|?(p7@-Pe)bFh{WT9B?VWt!=bk(FZ{PdzPu={BiC=s2 zFJ(UamcPFFlf7rZrv7K||Jcv}_^W>C<`@5}S-tXvD{sg=>+OGV;pJa_@>4&s`0(qF zKlfnq&QJf@)a~!t`iC!W|Hj7jkF9)s`6J~ozT{h2^eESVQ@K^8s z(awMTR}Vew%gI>lH=a}O|HMN-`y;o#X8FF~`X{&cE`GfKmE|XH{OzmX_QyBM-5ngf zV3pKU9s_?ith?JNz=U+VV(l9J=trOW%EZ%t;D612=(j%bjC=1~yY~YpiO(-Az4~Vt zp82L{{O+4R|M|{;{m$=x|7Rae-*#L33vYk^=U(%0^;LKO^_@@qim~&^GhX-Jpa1G- z{>}0~{@|Z{#~WVoqz^xG&8 z%v+wa`LX(6?SA=N1tZXFY?t1A>67nxBPy-a&vWsOV|-PRv!O9+BUMqKEn4A~)_1Ba zcBS<}dw8%5TkT-qTb3OOw+@j0-rTcx`mMoSY=1aBxHvO2KqDPLak9z&92%_k9 zW;)Hp48zjt%xtF;sjRt+-(( znDIH!pzj-saz@@&(w3pxLItZ<7q3MC|Jc}Hl1|4JLBoEeSS>V3=Muz_vw4l zPQ%RSQmJe`-Zbl}cqUs+#S8U%CSErordGqsB{HH682Sx6dT3ocA-C^Q0wal~B zLbEf@XTilT9FX(&9kSUO_DMVvGhjZRN@g&9(zy(Lvd*{SwZW*yRYJfFy+pb~u%uwt zYKc4~Id7#PX~}FeUd)==cq&~_S!pYoY!*dLDdtQbf}MlY_?)aKz~)>wo=g#+6Ag@~ zm4bX>;$-q^dL-_f5>l|F29-}EWlNXy(e4j>=kIDf7;*i|Kxpx4IrsZc`O-9XXG+CwRS#uq5D*5Ac*7Jcp!!`TW>T9=|m%5OVNzZTb zK9P@`^?V~=EH+Fl@6Kpy7(7Q^iXQe%q-QyCo-aHEu$h4JDf6i;Vgu?_NyXcLJVyF^ z7GgE9N6-B%qe1nc?y%K_@?{OSx})A`kU+mZ4`c}Hd%4@Nj_2`~`@#QG<-n@zctp>f z+CseVAo>8g=6eka`~%FTT9SmDU4gJppz{wj(Uqu?ZbgkCN*HUJ@Q6yDg zr!lM-)A2;p$|iDIXuq61SLBgf!eV#IxM3DzF-OgtuWj^P_M=JngNFy8jJ}_EELaTY z*kb`|P8*Ba8K$rpjCIfyftB$X9UyE6XN_^zT(d7wR}Ar&%maEfs;?hYi1#z%ru)=+Qh`2r4eEamom$3%+i{c zuxhn})xh#&K@Vn{87p3_)tm8DCIfpXWg*?9z>DFAm#E!9Ma5vm8&vtvwq=u<0t}}l ztg;;0f2lf1&E)d&d_7UCr(vWO@~-&+%#Jz_*6nPvsG8mG&o<@}1#wGy3l<3}-xu;E z(E+nLnkHuL3WX}>1X~e?RC&Z+4EW#9?DpxmiK%ZkGubraqI@gKSkyCJLE0vu1EdpA zB|ogMWHil_4FWv$r^^M~Exz1lRVr+e5x#OhU8gBXr#-!lrEZ8YdAWO$B+n++Co~|eAIk9zMLm@O&uvALo-mK-VY#~vPH&V4)JcB6&u}tJ( zccqh=I=-~BBBYnEezPHchuB8%qpe*;CmY#DHfvfYc10#FW%YX8Y$g-fU?gB!n@MB} z$w-}4^f2jOF&hQXZCV{;SQ^*KNj1}H@V}Oh=W^HvWg3YDOr=#%q^EeY~m5o@c>yRp@)yNl8a@vLV0&SFlDKNOLKT6CF|zF<%w zyYK?%LpWY5z(14aP^o-Qm9&G@pStGK7!}vKYvZZTAh?o6&T{!=~+uW4j=`VY?S> zM=-mNt*!XNKq>hqs`O$bM0uR%Vkq`1t^wIu9*i>fy6ol=o_1sG{HVgym?l+H+KFk#4{4^678w-h>XNTmlxEj%gsYUp{6%nMYc>Z8VA6`b;F|7ZY!nh zz1Ocm2ED;M@1vF;3Gu_5GpudT7tv^`Aj{vR1T}gitW&u3fSY`w)XdKC!4%Z_B>*MF zaeP1`Yc+U1%Yjz{aP2in0#?_{(?O7;9k4~^0`8R#5TR>z>ku(M3jB(&G!E(W$~fS9 zD1XgG&30oeEvwzI>mM;?B@b2|8;H7PrsHOzk-_dRosSn2*oI}`*vr5L+bkrSvoj1y z2x6wTw{SA&3LuwUTw*4p0Sc?vKOGH5T1AjVr21*CS4Opfwmo*Gj}iQeHJKI6Me8;y z1O|%#^mZ|osh)QT8s?%sm3-99HCdHqmk!Q7rF8P?6oC`CTstN+mN5DG!soU>79Wsj zfrtACz~eAB3M;E-f6p2&jG9fWkNE{F$Lv5g`w>RsXqn%AUw|8=F(HaoX!vrsvQ(ZB zeyP`iRYlX+MZgAC6-#ycb3kThxmqtk0VEX#z+~J}0%|L&?)A+G7_%rTeHy(m*n-r| zY~Z)hZKo)oMyiQ3(iGf3aP`0|*=WGqnNGqHgbK-KJzc}E(}g~g@_}FN)Oi@7eQ<1a zD%ka`eLOctBk zs80?{$Tbgv-xE!J1XJ>+tk-xD%>f@AnxO{BYS$ZO0bAkR2mIVp+xObbI z4<oYWHepo17dDkh@!Sd>ghKnZ3f!WfrXAk!5u&h|!}6Dr~6P9$GjOmb(MExr~Md z4OQfw(9DycxpZo?zAb=K3SQ-^~Y6{$od?qfn_qJ#b%kob1 z$0&CW;Fg$~fhQjS55T}0*y?u(hkan@;1S#Qd-8B#AbB18T})upOds&SfQ(j4)}Y>R zvC{|#k9Vi)^w##}$}b_bZkp}MxA=tjxwFW_4WD**h?!t}vqUC5&MW5y$-hJ~-)lD z#2f`fN*Xk{!_0XVG}=70f5SQ&_PS-PPi(%s%3d&x`4NNQLG^e;(&~=)zQmRFKxp_x zf=CM<1uHIchLCtc&kB{s&n(wR?O}@+amq3&#D+@%&H#fODz)3aBL|R`i06*ks0c9b z_KFo`K4{zJY9c^+JF|-p!-8q%u(zA_Uae)?%Pz>E6JCXCz2GmIIPt5~A{iNk-Ah1z zOW2BZ1i_bk1scgGq5a}ywe*Eupy|ZEd4S7=*1+l?;?hnK^*GKan-fD6jN)#q-&^eW z27_R5RBWYl>9(*mPeD>@@6q*{{f^QlSS+~xh_&g^JrtFM4}ccZtF-ECz1{&I8wJH_ zLrG>z74@lLU*pt#*8=*;YSkk_`4m-`PnJK`8jhiMvGm+s z$0?(_S1Q-W|TNp}*9ZfS*rtGNbA z(}h!GLQmWJjj1+nK;*Sgodkm}!CZmfTv;Y=p5!+Z+Lg zbWsaO!8k5Y6~=@@ZQ_;)^hFn&H@^}Mg|vfcWJVIRu#yCi0gqF#BTg8}JmO~@6zFKM zQKnfKsTn3{k(t7dzilE_AWb7nmG$ue_=$40cg5#q((SacSRpEA=&;b$4yUXT{3I2@ z#PIoz$2oTD4I1foGZq(hzUhU70ZP?1V9S1~^F->CtJO1q4`IW|stl+=vmOVC(Y&9A>+H!p}bt)b}{nKv#}2t9d$$9bAv@ z(<;kiPcU3rAveP5h zAdd%B$5dlk;4Vn>ghK>2sT<7YF66h4czA4WSKzm;qm~JTCQD7{XH;F9-Mg$J623O5$_983+{ga zGCfVmRjl#&-~`gXnzCda4ENcF-XTeJ2}d(Jc;*0R^O$!^?T=1vZ1>u2J-yl^D7Le} z&|^33Ye_dDXw4ZDgqJpBjqz?RcSjvtF9yiz2JE0i%U}*a3nB19o=I}z3GW3&sMkL! z4beb`mzW&c+~bC=ymg8!`vY7Ao4Y{cCFP!;tx|bMD^!4 zigp(Zq#50|Spx_+UwK*9tnES?G`Hbm=?AIk_|f$jRTt@Vg_VCj?XiVaoYYwN4fx}h%B>+D`wtv9g9fK?BSwacYs zi;Q*lAs}hG(^N&fbkV@4CNzx0C6*EnCHA^>^oHF-8@`#HgNENDs+*omEq1Sg?9BTu z6!xP}W7#OM-f$aq-MqKg*BzCe#x}MeTf-5HdQRD<@7$srl1JS;zT(tL8RrEPMFC+x z1q-Yf!QyB@_3r43uYBsHVLPjKaS5sTtj1W(;wQ=oM?l@f2AvD~##t+xEGC2JGQ>`8 zEZ8NBS2WUW5OHO_feRLF%Y$NyN5SlJ6q&uBWSc!&hgFv8dhqCRIr#3i$R{m46o~HR zNnt;@)gsDrk|5%gg81#T46{m3BW5St+TI_SybzEuy1ub~_X!LV!&GAm&)HY|6!B06Hj z?2ZqNx}4p;YJb$Nb3b4(f;dg`aMHr|oSevP%yTq1LbaVMFP5A~SKQ=?4vC zmxu)g%Mq4g)p0L$G6W)2Z9H&(7(pSSA@F*(u~<^lbmS%!!tSR(ly^LOpXc=+Ohg5y zEtyUH4vZW2HYOchW=t%{$HbLU8|NLv6F(9?H^}TnIvL7(Li-j!Xeb{5olfD%myVnu z4TocbJL|+!?+B*EVCaVrkwXVsPka<;XV2zuT+cSAZRqVfJNjEgV51*HDEAnW%IJXk zVV77glWLDk_bF(#-T;PD0>=FVa%AC%ve|TYHHv=IM)dj7A$pv-JSRsdMr)ajVjV@B zT?>xeV6)fZJ=;aM+K~7On9p3Av}g<`5&#!m^v6H=dhv0grLh_Z1*6*8d5P22@i25P z9A^#SVkGb(PGN%Qu)ZIcS>~5@rho{Ra@*jx+88VMO1-;?V`jMB5RnC65U`Lrs|OZcTRg zD##;6k2Vq0D!zXqp0>`>Y9_V;($Dsj!TYd{r z8hJ&vg(~1}+-{^{qx^%`eIpe2aBz-;2Hs=D-IL_{uZcu6ym1escT_p))Ozg}%uw~7 zgl?lFoW_9Qg?MN&L1z@=wX22j7x9B_W)NZ)4$0xW!AiW-3;bM?SsH^8w^9}SA;T-T zV8?trla!Q?@0z4u=w)Q(%}Md>bgy<{aFE%&503d|E>vh_DoAqeK(h6~?u>zJ15QM2 zLhCrHqj>Ji$Z9#3QjjU*;za(d2?)bpbjSkg$d$i;1y|9PG1rfh90ASTnkZ_*-8>K= zXV83$L@1|ZtZbFQol_x|dZZz;xzX#|_jT>cPI4Q5u?WG1Wqc1eToC$A4i8=tTHNO+ zwLIC@(n$&61fRHgaDYMSQLC2k95sYYtkm^=qURDP7K^pL;1W7^=0zK0%;FO!AH&a^aVu7D!>F3}w_g=4f4yCbF-b&tm8@oCg5mP3V>(EgL@y4mz#0!1BcQCXn~!*+|Cjf@~1eh;v$EW#ectiTPKu?gGgF?&>mwfZt#1Yis$aT!+0 z38f6I_EupmBKl-p40M^0Ct8QCL^e8tUXTHyjeQt+AAs#t)?=XL?ZxQ=pa@t zN*jR`ER+ky8<*=`+zh~Y$}mu}Z@1Uu!#iVFVv>&I2RqJWTvWCTs=}m-ct7R}!i;CD zL8~D=kwdGa1_WS#8SF_Q5X{!Pdj>a2({xi9>t3r)=pZrnE?A{PYs4>B7PqY~*0F%o zqtG$j2IOW9dyhk!OJkCb#TrIguT#U$)0S1X{==2*-+$;1AE?Kq_oyO?tbc=IO%<1g z*tGZw*K2=v{_02``mnOTbGc=r1Dj2IJ1WyLuigBV*o}1DcCB-}uh)ktu*fMyr-^y5 z4r0hGRY8SKs?ekFo9)lPT&hx_SmpAf0vRd`ZtKWTv(X!p;#oMU_72#lU>r=$y~d72 z``CfB8q5*_LnhwbUI#%do{Ter5hUPtush~!jzh@VL#MXEzPAt>>|uMT1cR1A#-ru^s-f zlZ%PyUE;aNMeLHGI7*}JYH&dMohYz$tXlOE90hV~*k?}rF@e`8UUe`mQEXq`UaB1w zu;P%vo`fuTfu-S<5Xk)I;Osj+JlaI$@iqm+R@IPY3cS)oF2i_3Mc09<#MfFwR1@P* z_0UxJLJH=MHpG?vR&%(G>cFsyhHQ;dxTUiL7_Tr1NR~-yt6;h{nBZ2gELn%Gx)l(i zPD9p?QwfISLN1=m7-z4vx{cn^AeYf3)w7dzPoO`%kmG6)FDz(wdM~)HR|Q_kKt&K8 zG!R@rxx6jQl06>POXKPiZ^Jrm60O(4`Xm~xR->E-E~x|(P@F(J1AdxL;eD4-$h_wW zHnd@L9Kpo%)NX%*naUy2U#&hP3%Ln{ToZ)E5$M#~4ohhe62rv7wR-KeQG<{304F|( zwV^vTIHw+elFBG{CP5LR>YUTiV5QExZ#(}LantS+NV_iI$3ayXO<+5W!DSSNN1ZNq zBG+GmXA#%B)Ky5g4gj2a01M)m^Y;%fTGHxu2JAP&xHWOlB zgW1lnCc@{B;~*T;t~c71I=i(%C#@7}tG1z)UiNknmBB|sA-h{n6YULgC%i4AWY>Hm z(-e$jnSA}5C>+Nes$0kgph&E4x&)&`#fr7k0wpoA`s9aIkm(l(xo1cNDGo#jwwp=8 z#12h^hmmB|+{3tIZ!Q^NR)#1fdt~#=lqEy#P9eg|`=n{kG~y4SAjIMJR+;mWKHd5R4+bY$Eue zN_rS9)~~G;+El zZl5fjbWsPbjw}6ST&TAWbV3&h>kp(AMsY5iO>6zOkY0*F<;=7rSA3dc9@@W{)xtn3Eft z*B_M9BDO}S?zjNpkG9gO&FkDqFWT%=jP&j56^)6`LM8YB1f*=->kp@gZO#0Yyhlq2 z-^0S^OH>jhDMB$Y=ae$udYwdt$+>U}cqC?5p!IVTz=GZhz^iMEbh{oAB65R4y@O3e zn-)3=mR}CBh0LoIgwa7&b_Jw^C%gTjCX<96ziF-GttRXs1z0x+P!anoBCZ8A^G_m( zj@=+ex^tzEda`XiBx`cZL2RMmCB!r8XoaNh!s|_|*zT~SsU?Bj_z22iajSvs3TjvM z5;36O!)(4bYvlzK24U)yy+-bt){aM-Rb(*(Simd_YlV5uR(m~qfme#V>U0H!@!jf5oOhyF-mnYtDaagP! zVx9Bv)O1|r5aakVY;`q;BZEWO`+J!mLBj1NDeBvHV0zToxfdYn+Y4)@O7L*%$zLmN zl@{+97Yc$iF34EUAGS;RCZq&7p7IF*t@sWj$7r|GoT#^)=;hAXO<%EU zhb`-9+(v2CQLG4;v5={D9Az!|MF>3eWHKZOnnvLA*Mtf(CtMMBUbINsPCez}SBsI{ z)lLz@yrQHF=3HL?vZw`GNp#qc<9~h`g)Q>Rux{!EQQQb;*LiVKlBcrP zdx^vRN>C$rpe&Kqru`oIg4G0PL9_#i$0U}Q9GZo#5!cpD1Qd6p(IJ5Y#}~Us3zbaa z-3(so97ug1Q+fv+pt(bzr`KIKyVwCKXM%eD4Ct@bcLx;p5!uj6r*%7d_I_n^?eg;W zI1sR%qrX0ZClO?ktcN%d=d~H03D%h{As95$Cgxbg!$h=(NH(g*%vPrXT&`Tm9sUSu z)p~6w^VtpjO8CZg2H9$i35C$Nj+xiDo7|utyvW`oz z&{O5c8mkH6%tE1@(&|zwL1M9HfhvASZ8)2~aFi+ygXZ7*s9OQ4W%AiCw^1pm3!g^= zt0V4Zj)_8C%PV!eJ-%Zdi)--ojmTj^iW8Dfv)ADs*PY=VSeX!zAI~FhbKDkTbfCzN zW}HKZ**5lAXy~Y;!Jyvyl7`EcZkB-t2dH*XhUe}W=h&I;GaF~t^;1Pb6L1{`@Y`qB zmC_zzq^wYF5oO@UA_60!g0o(nS&7^a9)<@>rcgQD0wdKz~@< z9Scdda{%X*LdsYe*3G)Fe8iZtu`_I`TU(GIpYDgr#d7C3K)kHZFh&(pH z<(Cqmih_iq_^Q3DR+q;e$XwN}gpQp{I&t7@TL;EH@=)e*O>U2T%b3rYlKAHZj@%+! zttV{I0~@(L9uGWWgI?gsEy5{^C1xna$d?pXF z427@MVwGbZ(9sZL6?x$T<`a8N$vJG)`YFYh_$JbrajQ~CN*Fu%$|;W7#%Cwr!OL@N z99~lNCw46DG6L)=r8WXneMjrMsp*GRT4whlk#$MF+9l0URw2$pvFZ*5Aq8Dqg=4iX zZ*e39NMc9HXZ)^{P)i3I3{vC`X^v!Pshblo0`@FwOtjPmDz^k5OwH<(&C0TmDU9@A zQjrj7cWJtji@Y6^nt^;fTQR%bi7?5P(|ZP{GE2F~r12{z&!>t~>H&t!Uou0#Ls9b& z5Q4xA1{{UEKZy*eDv-#4isBs;&ZjgZ!&kh9ePk%fs*emMw6kX`5gFFIUejV98PK~7 zjs@shu*ZV)>%Pdeswyuspt94L6wcY`Op1VJ?xe76Ycnx$9jm%p+(t9G6=sR;%%t$J zt+17u6q;>eCI#kOmr0@7wq;Ua+OSLtYj0M*25|6vOlW0e5*gYxA|itX8p1))N*Nv| zt~1iwTV#8Dc}Su3JBT3 zt$HcwK1Z1CYd1&>@ab_U#3kpFgAM7#8nLFOz<1l*6|xJkalp}7?YH)pkll|$ULA%B z%Otz)aE-^d7FSnwo- z++_B&CzHC zq(qVW;-JxZzEg=zsPlXBi$f9_;%sVV)Nb=D%u0WhvwR7dWdlGCf8qo?FNIh|3KEmB zoK_DYL@?=^mcfCMyZpMfTmX)$-C5WyD%0) z2O?GmDD0@&@i9=Vh#6wqdJ+n7T+LG<44qKYNzmA#bm)u-LbiPF%+SL|G^(cc1Onf1 zgct-i2EHJ%H3IH!lqdJ$2(xJ}^?M_n&U2?4ZGjh-6YHrQz@L?ND_Yp@|Bf#L!A3Iz&e=wz}_j=A51njesh8tz3g)mfC@7#L~Aaa04pK;*Y{vEuZt z(SV{xj3yQqk`;aLkRQJ;{BV)bQ=#FAxVMn9sf`d;mYi%{m#RJF=nX14K;Xt;lMTxB zhNTt~4QB?c{SkiOI+U3V9yHrdnaY?tgGXE2o=vg})Lw__3Jy#@hJQbABG3G_4iH0zl^`#0|Il-mdgQNA^d*W8^ZV?`9T>QCVxQ-aM&O&Zf&t%Kt*Wb zLnM&YFse~$LBpC}UwY|8aA29El*W-)p#|A)J79GVqZJ+r=f)5PwQC6v;gx7#iSt#v z5Q2epTAwY^?rJ9X*Fi@ZV#mqPrVuK=-)oEqiDXe%M+a3>z}&&ZZHf|#SZ($}j6!_! zLh3^LLgvEs^t4`(*LJF2Tnvw>qQeU4xFqszBB_T93%DnAl?P&7VDyaH0E$k~iLHPA z8W%e|7gDlXa5@)JwXf5Scg%a0($SK^kDQOZZhER!M5RB6|xd&OHT?& z83zP*gltBE!41Eh6ayKDp%m;k@P>bo)MGV%!-*+07yL5 zRVvGfvh{o01N@mD&zwxn%6ds*Y^d34-(A3!iU#a%XlPZCL!tSgUK+tS0B1#H_Xh05 zYHxjMg~nnWVy~}5*JQLqSDuPX*`)>NZyfGLTTG3E@IFP{D&3mV(njea0=8}S-l1>Pi)>v9d;9qMJ_tqtIei*{HPmM^h!h;cUYm$Lsg_gAZf4`M*^QxY()(<4e#D!tMD5^(X-T|~>TB^@z z&<5GN$Po&RGiTVAfyCsDmUe2EcDn%+$OFDx51&iC-<#bna}+sm7{rLthXc#8^_T;A z%+f(C=l`1dLhcX>Z(m+t3vN($7JAuDzNK+t7N}wqlG9+%QuSo>7cNFzXtUy3?YIbN zlx7ZsMgWlJ-ZI!q{#9JUWqw_h6BtJvhp5{s9ncAK)rcdBJHy_=*g)&`MxBOHp>EwA zTxH9#MxQS85ifUN6|Z^ zI0)V4L@--%KRY1GZY3=4UN$-;i)37ME-Dy0j3-zJ=Wp7&pFV~O{$l(xz+<`&KNrJ1h#4s8fWzJH-dt=nY{m<*7th}` z>oi4xw1soOF|eykjA5r6fQ@#xc(}f~w7j;vu(r8)M@(w+0q4`pTcx~x9up3d4+_2tWFit%87O&-lHaKH5C90*H>V{-1in)D-O1810&4|u2F8qCG^ zhr@%5Gc$wwzSS`)c6J}W*QNlt!t4zFpRs&)hGDZmILnpY zlJi-#R!nA_nPedoFJ>)}j1I)jY_1qjW)sO=Hj~8MZ}OThXoF?Z#%etSme0!rd*Soe z^ZxWZrl#oU`QCvYz*A$9uXl!ydWX9xt2>5Wz+epGZ8FC??btJ2>~v!De6d5m!mbE@ zc3wDQl|PY7xy2Od{5_de%E+dR)5T2ENEV7lCYgSjZQnUSluV8#r60uR?@1U*WY5oM z4f-REKYXuKjxMW z)?YXKS7Y;e^_>QMEwE5U*A~}yD%Iuf-Nn_?c2#|sL@YLseNgaY#$mQ89MMJ8*u2QF z&)V4$dJ)=1JH&Pb4UbKY(u42WIdm`{q{ zOV1H@fa*mg$g%m#&VrZC%6lvuA#u?eSLp>l7Ard&i@TL-sp^e1aEQ^2t?zw@>rxA? z0Y=+zvHDCD(uFvPU61EGv^*O)Urljtmb4HpaxAs@rtR`-IjwRY~NTV4oz9aT%a zH{IfW)rjM1X`>2l+veXJ;F8g58|&q@X=A-bMedA7i>3V?%ye4q88%O+>(fAF7^UHG zM75|agMzW+JXp11oP985bUHJG3kEh5Lp&Px3?x*q@1MH>WDqP^;e?e|AXd2Eyw${L z-^OqfDXE?TKO{;z8D|FvEjWseL5qrHn8u5Y2J#sh_l->aANQ7eK*`pfcFPzbc!FrD zZmt*>!^IA)VR~607hmjDHbD`xUDc4@q?R8b-q<{pv&8kaU_RyWs| zXDZdrEr4#V8D|eDi4~G%_6QKx5^14mK>95}pl!@CwD*ewv;-MSd#2&+PpSc~O8LvF^bWMJW`iq>kjUn~J z+mqdc7VJQ)-|98ao;!CNp4|cyYk*;Hf=N9CPNb|cV6@qB$UyC~fq%D)Q92NO=$Oa! zQF;yEv3IDYU78Qz8N+#t5s$|WsE5{}c|T24S33+wP)L1f5Bkm+AkQy^6Mxx*Cx#aA z3=_r?({WmPdMhGEZezcU2Yo|NoREgVD>8Ei?Ws6J+NNmL0(1IA5oY^=)$XyTiM@o& z+W@-X?(LDUft0aAmaq4=3O%bTOX7~yJ2s8(M+y1eedr4Ih+1~VlnE67=!hXi%#|Cv zUlQSiqZU-N7*I?!xbN8ZQ?(LbV>S>Zfu-mdIPrS7^YT2A2+UTD` zkS#xMLmb>Qkz6294qmz)e31cRI*TAfCS_3lt?pU(10&leChkH(vK?NrZltIi&^=r; zu| zZAlDJMm7LX4-xA9Bj{LQBHe#Bf%Sj{9Sk?!CB-I12-T2JEh3fvIe4#~EI3M$>!2{J z#5n7iBj>zR($&-sPevpTF10fT9|@PO^a4pRq=7^?dBGTY$Hv5CSG}4@s?*-xgOBUn z`LiR~YVM=aZr2*YvTC0@XCU0zA%M8$eUFT#T{4!=oj*Kp)&^$}J(55x&mk--^L%6I zl1Mu7etdf)ZCBmO1(NjVF%RDfvXAJ z^mK_^Z@a_ZZmkD{vEw0>8YeU~ldzHVsCCdp-_cV}7X8JhBPe5U!)`(IP=XR=%J8ce zq)@y~q;aS9Qah%@UbEAYx`%Ie_hDo1b~@BQ!f^hqJ8;Tl`GJMI-T*sB5T1{B`HoIrXcETUK_$aKac4ojXaYG&j)2BkB#%H8S4aCbn$ zBmJAhCV&o*&o+j3OY|o9SxPU0ANHH~yDu_s;)>Mn)dR9}`9J4H_SAiuMhW98|8rhu zgD$_!ihz4x_b#*`)71%{m*&6-*z#v_>#*B!*(%203(wh=q$uIGH#Ixn(;bQ_o> zexEq+)L>{?{gd6!q5BG0H3AXI+}UIbK4Q01aFfDj!#<(GBH1~_oWaZ)zYEcxYc4q3 zu6UYZe~SK!y$oJwvf}JyhqTylNk%D?KK4D?qA)DnszPO z-2p>udp4-$S>dvvKbf1tc3CuVm~MM4GBJZZ9&<5F8o%>mz~XS`%Ntue)m?1vN?z>P z#vTkfM%J3y@0%y?XYbw}cR6D7ap84gTmeZ6z%1>Y0^zZR!zC6tFjjUe<@KGl^2Qyz zn+vxuFIIP}cWrrH5sVPwdO7IREm9n=G03IOorSgKPyomczobIvaNeD+r`yC!nNY|5 z!3E@)$`TW)i<|3PYs)X)EpMzYV|To~Bx6BMM4V1?UPxqf4^*oQ3!)`lgMwvVpxGH| z1ROGGbgRsBwlEr8RQBu+4YM=!9w%VdBM@035M?+AZSow{wmfX^R3DQ9;dlicF0z5U zHu4~(7E33&hao^K+okp8-HlDTHLnQ#UCWKX!#c6a39%7cPAfcr-^xVR3gwMzIu)uE zS_3*svY>us{pHbb6#H)Hw2-~0*GyUHgyZi|x3D$bkxZRfoDrVa+DZf}r zCoZp0k6dnhz`^Vxh{0?*r&9N%3e&I*jbwVdfI~DC-I>m2GDb2pUCd>~iI}8(b>}dH zeqG4Dr#Va>_<0fIfR3=!Ivl)VhnGrEXY-jnwUa~Z2_qr3HRC}x+ZcGP2WNq%W2o4C z+S5gStJGrL+iV8Cq~?9?dfIYY>a-dx$^zsIvQuDBCRdPX7K)2T+&P_0;gk_9WcW00iV~9 zxiaAPlH82=mRz4{@i4Dzw%(<~$2Aq`d&!RM-eu^sC57;255RrnrPO zU!c<${BukK#e#%|U>}iF9Bs<+O%HH(;;a>(9c}}~<0wp!ZL5)}mO`l=kAhK&J0WjX z7BS_`SB1rveJ6J7vpjwm*Hh+V?9bgI4<=@|pYI`JeC_Le-W?TLI5saBUl1sQOu%(S zj8Ir`*6xDC8T-}PJffoQdu7fu&9=zXc}7!gUdYkxjA+f@bSg7`)1FXEKXrE7PUxt& zaYSE>Kx%bs`M}wqxcAG2sVVw-j(2OxH+jfnIo@5YD=cuDfw4~W(;Q<9CsiBllxv~k zc_rpeTyNj+;xtu;SbhdY5vH(xN~I6}Wb5#Y$He8DKdK_h#VeEeCby9^8c-VuszF$z81 zNe8pY2g11|ZILdw1}$Wh3-6VuUrRltJP^JS!@fUAG8UU(vzp4Gm@s~>v>Kk<_-u<4 zo6owsMWSfcLY%srwfK~`mD}<5+hV2>Vh{1_2$SsK#8bKLJI}6aIa3D!N<{^~Tv38N zoI~~(Oy6MO+}24%>8Z1zLQFaqKHAiTG|P<^j>*|f6G_{gNyDFU@o}=_K;NC05_XC1 zav7xPd7%##eny)U(h!sf$QD@hj6mQtl0Yu=YYkQMpUDy@`h}ztEaRwW2qg?_J+ndg z{XK=$xyZ^Q=#dnb_|B87*_pj`e}!gjCX21N*AXZ+cz3VYSwXNbF3Jgt1%~8wE>pXtdTT2>SJwEI9L5SSH}tZm9YnpXX+i!DWsR@WED#-6XfJ9>I92rW8LS{OUp-P2 zB6_LmOi}tLJdZwR>qHN7H=Ro+a>Yy`ol9p@h5Q904{E4jJb!lQ$;Tt z{tHGnRh&*0n1RZE;rc-;oyWnD`&B}^l>;IaogO55d1u*|X3XEa=nY4WRE1w=6xMZOTudMe`NynM za6_Z6M^VV=>=M0JcUFD55&Bs_K{;d*v2 zPH^Jz*cF89+PV0+m0&xFB1AK!u3z6``l85i%h?fW2l<##2~wQ~utkjyr1;H6QHUZq zNKkquAXd;TR{roUCxF=&R1pIj5WaV5ZD%=kk%D!t{t$)Y2L~*-ylzM6i{2BE$Pao7 zs5Z(~>yP4P#TLzWyM_$kPLLc68}w9%@dzGjru3I^mpaP3D^yMaxucwSohV2xK_P*+ z+Y~UX<63a+t=>5w9DbWFXpJ#lP%Q#{B$}ecP2e7Z8yD;0ZGiBI*w2eRruCvzt)fAB zr&00IQ~t$+$tOfEoDmAB6(Q|@50ht_uTZtQAp{BB0%QRTzaw#pq zJzO7K^gX6T=QL-Tq1duMN9wX}Rxx_sxtN@=NFDwh@#rPXrj z?mN*gsJrWk@Fq%ihCf5!kBFYNqlw@asLeLf+|JC#<9&k6Ojwnuzp?_Hw)10H-^Cps(adwQSu zYeakk+*Rt|_8zr!M4#_3-3c-dd-%08@>ixt`M90X;FUUT+}NM%_-NjXUkj^;M{ImchxA}^AHOc8 z**lmAL?fe}@v>BEGMUd1ZPn7kWg6r4J&ZMI0gu?2f`+S1CK%6{EQ8iNiB`tjZF-Ke z7Q94zB`!~F%-F}$77?(7Zk8_f=+9%DUtU@c4~e`PHvT1No-D2GvA^@|p{E~gPMaK$ zgLm&L{ikYI4OU%E&b9@l3i_Of_?6Jymearpn8& z6LPz-6o427Z>(HCTa4G5jY2%rOf}+WrjUpy6Zu-ck*U@5Sy8LgMLRp=xlBmcC7*CZ z{3f@2-4Fh=FJ1ZD`Kc-Tc@{2A%Kwlj)1=%AGsAsUMCqC+=Wx*7Q*pfeU;pK+oBwcX zihjObLXdv3H@gojnKPIfu>bF}e=7@>_e#3L_Y}$+q-_wgRR!+95arswNQ7W ze^Tj<`gN+=+!)n|=yT_oB`H;D^f8U?IkpPcTMevLV44;0{(=m>L%86WUv`kbO><|@!7(aEq{G9g!(+h$17762@ z{gMwOa@wMjG|D!TLqLmieqsJg9|x*;0p&Rf<)6IEgA!M*?ea~WT+glcKuW9s@|()P z4s>q-+80S^zx-}5T6My&V%_>Jj|hD$(>-jFv-_ii(|% zQ5lgS@&*SlSS1GLLEc#j=a0WH1Sd+zTBx!hhTIiNLh*vBH(&bXJKhLYML*A#u>SP> zJ;PE#Ikd*8ZBgQ0nK+K-;U9ja_g;FqG$SJ z9z?ZnG{@H h?f>M#b>t>U;Ie%qRtTzR_`IoI=$JP=4MIKje*vzqE&%`l literal 0 HcmV?d00001 diff --git a/model/params/core_params.m b/model/params/core_params.m new file mode 100644 index 0000000..a150e50 --- /dev/null +++ b/model/params/core_params.m @@ -0,0 +1,124 @@ +%% core_params.m +% Single source of truth dei parametri del modello MIL, allineato a +% include/config.h e a tools/mil/mil_main.c (gia' validato). +% Crea nel base workspace: +% P - config del control core (= CoreConfig del C) +% PL - config del plant (attuatore + fisica + sensori) +% CoreInputsBus / CoreOutputsBus - bus objects per gli I/O del chart +% +% Esegui questo script PRIMA di aprire/eseguire float_mil.slx. +% Maintainers: Colabella Davide, Benevenga Filippo - Team PoliTOcean + +%% ---- Geometria motore (mirror config.h) -------------------------------- +STEPS_PER_MM = 200 * 1 * (26.85124 / 8); % 671.28 step/mm +TRAVEL_MM = 45; +MAX_STEPS = round(TRAVEL_MM * STEPS_PER_MM);% 30208 +ENDSTOP_MARG = 10; + +SENSOR_TO_BOTTOM = 0.49; +SENSOR_TO_TOP = -0.01; + +%% ---- P : CoreConfig ---------------------------------------------------- +P = struct(); +% missione +P.profile_count = uint8(2); +P.descent_target_m = single(2.50); +P.ascent_target_bottom_m = single(0.65 + SENSOR_TO_BOTTOM + SENSOR_TO_TOP); +P.rest_target_bottom_m = single(0.15 + SENSOR_TO_BOTTOM + SENSOR_TO_TOP); % 0.63 +P.depth_tolerance_m = single(0.33); +P.hold_time_s = single(30); +P.descent_timeout_s = single(180); +P.ascent_timeout_s = single(120); +P.rest_window_s = single(120); +% PID +P.kp = single(0.5); P.ki = single(0.1); P.kd = single(3.0); +P.alpha_d = single(0.25); +P.integral_limit = single(5.0); +P.u_neutral = single(0.011); +P.min_retarget_frac = single(0.001); +P.pid_period_s = single(0.05); +P.descent_kick_u = single(0.15); +P.u_min = single(0.20); P.u_max = single(0.92); +% geometria motore +P.usable_steps = int32(MAX_STEPS - 2*ENDSTOP_MARG); +P.endstop_margin_steps = int32(ENDSTOP_MARG); +P.max_steps = int32(MAX_STEPS); +% safety / homing TOF +P.tof_safe_min_mm = single(32); +P.tof_safe_max_mm = single(82); +P.tof_safety_stop_samples = uint8(3); +P.tof_homing_approach_mm = single(50); +P.tof_homing_threshold_mm = single(70); +P.tof_homing_confirm_samples = uint8(2); +P.homing_timeout_s = single(30); + +%% ---- PL : plant (mirror tools/mil/plant.c) ----------------------------- +PL = struct(); +PL.max_step_per_tick = int32(70); % MOTOR_MAX_SPEED(1400) * 0.05 +PL.u_neutral = single(0.35); % SIM_U_NEUTRAL +PL.accel_gain = single(0.05); % SIM_ACCEL_GAIN +PL.drag_quad = single(1.50); % SIM_DRAG_QUAD +PL.pool_depth = single(3.00); % SIM_POOL_DEPTH +PL.usable_steps = P.usable_steps; +PL.tof_home_mm = single(75); % TOF a pos=0 +PL.tof_ext_mm = single(34); % TOF a u=1 +PL.initial_pos = int32(3000); % non-homed: forza l'homing +% geometria per il calcolo top-depth nelle verifiche +PL.sensor_to_bottom = single(SENSOR_TO_BOTTOM); +PL.sensor_to_top = single(SENSOR_TO_TOP); + +%% ---- Bus objects per gli I/O del chart -------------------------------- +% Ordine dei campi = ordine delle struct C (per codegen coerente in SIL). +% NB: 'cfg' NON e' nel bus di ingresso: e' il parametro P. +elems = { ... + 'dt_s','single'; 'depth_m','single'; 'pressure_kpa','single'; ... + 'tof_mm','single'; 'tof_valid','boolean'; 'motor_pos','int32'; ... + 'motor_position_known','boolean'; 'motor_busy','boolean'; ... + 'battery_mv','uint16'; 'has_stored_data','boolean'; 'event','int32' }; +CoreInputsBus = local_make_bus(elems); + +elems = { ... + 'motor_mode','int32'; 'motor_target','int32'; 'motor_enable','boolean'; ... + 'motor_zero_here','boolean'; 'led','int32'; 'phase','int32'; ... + 'ack_event','int32'; 'log_now','boolean'; 'log_kind','int32'; ... + 'mission_active','boolean'; 'completed_profiles','uint8' }; +CoreOutputsBus = local_make_bus(elems); + +% Bus per il PARAMETRO P (= CoreConfig). Ordine/tipi == struct P sopra. +% Serve per dichiarare P nel chart come Parameter di tipo Bus: CoreConfigBus. +elems = { ... + 'profile_count','uint8'; 'descent_target_m','single'; ... + 'ascent_target_bottom_m','single'; 'rest_target_bottom_m','single'; ... + 'depth_tolerance_m','single'; 'hold_time_s','single'; ... + 'descent_timeout_s','single'; 'ascent_timeout_s','single'; ... + 'rest_window_s','single'; 'kp','single'; 'ki','single'; 'kd','single'; ... + 'alpha_d','single'; 'integral_limit','single'; 'u_neutral','single'; ... + 'min_retarget_frac','single'; 'pid_period_s','single'; ... + 'descent_kick_u','single'; 'u_min','single'; 'u_max','single'; ... + 'usable_steps','int32'; 'endstop_margin_steps','int32'; 'max_steps','int32'; ... + 'tof_safe_min_mm','single'; 'tof_safe_max_mm','single'; ... + 'tof_safety_stop_samples','uint8'; 'tof_homing_approach_mm','single'; ... + 'tof_homing_threshold_mm','single'; 'tof_homing_confirm_samples','uint8'; ... + 'homing_timeout_s','single' }; +CoreConfigBus = local_make_bus(elems); + +assignin('base','P',P); +assignin('base','PL',PL); +assignin('base','CoreInputsBus',CoreInputsBus); +assignin('base','CoreOutputsBus',CoreOutputsBus); +assignin('base','CoreConfigBus',CoreConfigBus); +fprintf('core_params: P, PL, CoreInputsBus/OutputsBus/ConfigBus creati.\n'); + +%% ---- helper ------------------------------------------------------------ +function bus = local_make_bus(elems) + n = size(elems,1); + be(n) = Simulink.BusElement; + for i = 1:n + be(i) = Simulink.BusElement; + be(i).Name = elems{i,1}; + be(i).DataType = elems{i,2}; + be(i).Dimensions = 1; + end + bus = Simulink.Bus; + bus.Elements = be; +end diff --git a/model/params/declare_chart_data.m b/model/params/declare_chart_data.m new file mode 100644 index 0000000..a21975a --- /dev/null +++ b/model/params/declare_chart_data.m @@ -0,0 +1,62 @@ +%% declare_chart_data.m +% Dichiara/aggiorna IN BLOCCO tutti i dati LOCALI del chart float_mil/Chart via +% API Stateflow: scope Local, tipo corretto, Size 1 e valore iniziale (evita il +% "read before write"). Idempotente: se un dato esiste gia', lo aggiorna. +% +% USO: apri float_mil.slx, poi in MATLAB esegui declare_chart_data +% (In/Out/P e le FUNZIONI restano da fare a mano: qui solo i locali.) +% +% Maintainers: Colabella Davide, Benevenga Filippo - Team PoliTOcean + +mdl = 'float_mil'; +if ~bdIsLoaded(mdl), load_system(mdl); end + +rt = sfroot; +charts = rt.find('-isa','Stateflow.Chart'); +ch = []; +for k = 1:numel(charts) + if strcmp(charts(k).Path, [mdl '/Chart']) + ch = charts(k); break; + end +end +assert(~isempty(ch), 'Chart "%s/Chart" non trovato. Apri float_mil.slx.', mdl); + +% nome ; tipo ; valore iniziale +locals = { ... + 'st_completed' , 'uint8' , '0' ; ... + 'st_phase_t' , 'double' , '0' ; ... + 'st_stable_t' , 'double' , '0' ; ... + 'st_home_t' , 'double' , '0' ; ... + 'st_log_t' , 'double' , '0' ; ... + 'pid_integral' , 'double' , '0' ; ... + 'pid_dfilt' , 'double' , '0' ; ... + 'pid_last_depth' , 'double' , '0' ; ... + 'pid_has_last' , 'boolean' , 'false' ; ... + 'last_cmd_target', 'double' , '0' ; ... + 'at_ext_limit' , 'boolean' , 'false' ; ... + 'tof_oor' , 'uint8' , '0' ; ... + 'home_confirm' , 'uint8' , '0' ; ... + 'req_safe' , 'boolean' , 'false' ; ... + 'home_after' , 'boolean' , 'false' ; ... + 'phase_r' , 'double' , '0' ; ... +}; + +existing = ch.find('-isa','Stateflow.Data'); +for i = 1:size(locals,1) + name = locals{i,1}; dtype = locals{i,2}; init = locals{i,3}; + d = []; + for j = 1:numel(existing) + if strcmp(existing(j).Name, name), d = existing(j); break; end + end + if isempty(d) + d = Stateflow.Data(ch); + d.Name = name; + end + d.Scope = 'Local'; + d.DataType = dtype; + d.Props.Array.Size = '1'; + d.Props.InitialValue = init; + fprintf(' %-16s %-8s init=%s\n', name, dtype, init); +end +fprintf('declare_chart_data: %d locali dichiarati/aggiornati su %s/Chart.\n', ... + size(locals,1), mdl); diff --git a/model/plant/float_plant_fcn.m b/model/plant/float_plant_fcn.m new file mode 100644 index 0000000..9fc8fc2 --- /dev/null +++ b/model/plant/float_plant_fcn.m @@ -0,0 +1,98 @@ +function In = float_plant(Out, event_in, PL) +%#codegen +% float_plant - PLANT del float per il MIL. Mirror di tools/mil/plant.c. +% Attuatore rate-limited (~2 mm/s, contratto latch), dinamica di profondita' +% del 2o ordine (spinta siringa + drag quadratico), modelli sensore TOF e +% pressione. +% +% USO come blocco MATLAB Function in float_mil.slx: +% Ingressi : Out (bus CoreOutputsBus, dal chart, RITARDATO di 1 tick con +% Unit Delay per rompere l'algebraic loop), event_in (int32, dal +% trigger GO/HOME/STOP), PL (parametro, da core_params.m). +% Uscita : In (bus CoreInputsBus, verso il chart). +% +% Codici enum (== float_core_types.h): motor_mode HOLD=0 GOTO=1 JOG=2 STOP=3. + + % Stato persistente del plant + persistent motor_pos motor_target z v dyn_active pos_known prev_mission + if isempty(motor_pos) + motor_pos = double(PL.initial_pos); + motor_target = double(PL.initial_pos); + z = 0; v = 0; + dyn_active = false; + pos_known = false; + prev_mission = false; + end + + dt = double(P_pid_period()); % passo fisso 0.05 s + usable = double(PL.usable_steps); + maxstep = double(PL.max_step_per_tick); + + % --- attivazione dinamica sul fronte di salita di mission_active --- + if Out.mission_active && ~prev_mission + z = 0; v = 0; dyn_active = true; % la missione parte dal pelo + end + prev_mission = logical(Out.mission_active); + + % --- attuatore (contratto latch) --- + if Out.motor_zero_here + motor_pos = 0; motor_target = 0; pos_known = true; + end + switch double(Out.motor_mode) + case 1 % GOTO: aggancia il target e inseguilo + motor_target = double(Out.motor_target); + motor_pos = motor_pos + max(-maxstep, min(maxstep, motor_target - motor_pos)); + case 2 % JOG: moto continuo nel verso del segno di motor_target + dir = -1; if double(Out.motor_target) >= 0, dir = 1; end + motor_pos = motor_pos + dir*maxstep; + motor_pos = max(-2*usable, min(usable, motor_pos)); + motor_target = motor_pos; + case 3 % STOP: ferma qui + motor_target = motor_pos; + otherwise % HOLD(0): continua verso il target agganciato + motor_pos = motor_pos + max(-maxstep, min(maxstep, motor_target - motor_pos)); + end + + % --- dinamica di profondita' (2o ordine) --- + if dyn_active + u = -motor_pos / usable; + u = max(0, min(1, u)); + a = double(PL.accel_gain)*(u - double(PL.u_neutral)) ... + - double(PL.drag_quad)*v*abs(v); + v = v + a*dt; + z = z + v*dt; + if z < 0, z = 0; if v < 0, v = 0; end, end + zmax = double(PL.pool_depth) - double(PL.sensor_to_bottom); + if zmax > 0 && z > zmax, z = zmax; if v > 0, v = 0; end, end + end + + % --- modelli sensore --- + frac = -motor_pos / usable; + tofmm = double(PL.tof_home_mm) - frac*(double(PL.tof_home_mm) - double(PL.tof_ext_mm)); + press = 997.0*9.80665*z/1000.0; + + busy = false; + m = double(Out.motor_mode); + if m == 2 + busy = true; + elseif m ~= 3 + busy = (motor_pos ~= motor_target); + end + + % --- assembla il bus di ingresso per il chart --- + In.dt_s = single(dt); + In.depth_m = single(z); + In.pressure_kpa = single(press); + In.tof_mm = single(tofmm); + In.tof_valid = true; + In.motor_pos = int32(round(motor_pos)); + In.motor_position_known = pos_known; + In.motor_busy = busy; + In.battery_mv = uint16(12500); + In.has_stored_data = (Out.completed_profiles > 0); + In.event = int32(event_in); +end + +function ts = P_pid_period() + ts = 0.05; % deve coincidere con P.pid_period_s +end diff --git a/model/tests/run_mil.m b/model/tests/run_mil.m new file mode 100644 index 0000000..14c767c --- /dev/null +++ b/model/tests/run_mil.m @@ -0,0 +1,97 @@ +%% run_mil.m +% Lancia la simulazione MIL di float_mil.slx e verifica i requisiti MATE. +% Replica la verifica di tools/mil/mil_main.c ma sul modello Simulink/Stateflow. +% +% Prerequisiti nel modello float_mil.slx: +% - Signal Logging attivo su tre segnali, rinominati esattamente cosi': +% 'depth_m' = In.depth_m +% 'phase' = Out.phase (codici CorePhase, vedi sotto) +% 'completed_profiles' = Out.completed_profiles +% - Trigger GO one-shot (vedi go_trigger sotto o un Pulse a t=30 s). +% +% Codici CorePhase: NONE=0 PRE_DESCENT=1 DESCEND_HOLD=2 ASCEND_HOLD=3 +% SURFACE_REST=4 DONE=5. + +run(fullfile(fileparts(mfilename('fullpath')),'..','params','core_params.m')); + +mdl = 'float_mil'; +if ~bdIsLoaded(mdl), load_system(mdl); end + +% StopTime 600: la missione (con l'oscillazione in risalita) dura oltre 450 s; +% 600 s cattura i 2 profili completi + la sosta finale. +simOut = sim(mdl, 'StopTime', '600', 'SignalLogging','on', ... + 'SignalLoggingName','logsout'); + +% Controllo esplicito: se i segnali non sono marcati per il logging, logsout +% non esiste. Serve "Log Selected Signals" sui fili depth_m/phase/completed_profiles. +if ~ismember('logsout', simOut.who) + error(['Nessun segnale loggato. Nel modello, click destro sui fili ' ... + '"depth_m", "phase" e "completed_profiles" -> "Log Selected Signals", ' ... + 'e imposta il Logging name uguale al nome. Poi ri-lancia run_mil.']); +end +lg = simOut.logsout; + +% I segnali da Bus Selector si loggano col nome tra parentesi (es. ''). +% Confrontiamo togliendo '< >' e spazi, cosi' funziona in entrambi i casi. +strip = @(s) erase(string(s), ["<",">"," "]); +avail = arrayfun(@(i) strip(lg{i}.Name), 1:lg.numElements); +findsig = @(want) lg{find(avail==want,1)}.Values; + +need = ["depth_m","phase","completed_profiles"]; +missing = need(~ismember(need, avail)); +if ~isempty(missing) + fprintf('Elementi loggati trovati: %s\n', strjoin(avail, ', ')); + error('Manca(no) nel log: %s.', strjoin(missing, ', ')); +end + +depth = findsig("depth_m"); +phase = findsig("phase"); +comp = findsig("completed_profiles"); + +t = depth.Time; +d = double(depth.Data); +ph = double(phase.Data); + +% Diagnostica: quali codici di fase compaiono davvero nel log. +% CorePhase: 0=NONE 1=PRE_DESCEND 2=DESCEND_HOLD 3=ASCEND_HOLD 4=SURFACE_REST 5=DONE +fprintf('Fasi (phase) presenti nel log: %s\n', mat2str(unique(ph(:))')); + +DESCEND = 2; ASCEND = 3; +tol = double(P.depth_tolerance_m); +top_off = double(PL.sensor_to_bottom) + double(PL.sensor_to_top); % 0.48 + +safemin = @(x) min([x(:); nan]); % NaN se vuoto (evita crash su fasi assenti) + +deep_closest = safemin(abs(d(ph==DESCEND) - double(P.descent_target_m))); +shallow_closest = safemin(abs(d(ph==ASCEND) - double(P.ascent_target_bottom_m))); +max_depth = max(d); +top_depth = d - top_off; +min_top_depth = safemin(top_depth(ph==ASCEND)); +completed = double(comp.Data(end)); + +fprintf('\n===== MIL SUMMARY (Simulink) =====\n'); +fprintf('profiles completed : %d / %d\n', completed, P.profile_count); +fprintf('deep closest to %.2f m : %.3f m (tol %.2f)\n', double(P.descent_target_m), deep_closest, tol); +fprintf('shallow closest to %.2f m : %.3f m (tol %.2f)\n', double(P.ascent_target_bottom_m), shallow_closest, tol); +fprintf('max depth (overshoot) : %.3f m\n', max_depth); +fprintf('min top-depth (surface) : %.3f m (>=0 = non rompe la superficie)\n', min_top_depth); + +pass = completed == double(P.profile_count) && ... + deep_closest < tol && shallow_closest < tol && min_top_depth >= 0; +if pass + fprintf('RESULT: PASS\n'); +else + fprintf('RESULT: CHECK (vedi metriche/tuning)\n'); +end + +% Confronto rapido con la traccia del MIL nativo (tools/mil/build/mission.csv), +% se disponibile: le due profondita' dovrebbero sovrapporsi (equivalenza MIL). +figure; plot(t, d, 'LineWidth',1.2); grid on; hold on; +yline(double(P.descent_target_m),'--'); yline(double(P.ascent_target_bottom_m),'--'); +xlabel('t [s]'); ylabel('depth [m]'); title('MIL Simulink - profondita di missione'); + +%% --- go_trigger: incolla in un blocco MATLAB Function alimentato da Clock --- +% function event = go_trigger(t) +% event = int32(0); +% if t >= 30 && t < 30.05, event = int32(1); end % pulse GO di 1 tick +% end diff --git a/platformio.ini b/platformio.ini index 10a4d92..98a1af7 100644 --- a/platformio.ini +++ b/platformio.ini @@ -59,6 +59,28 @@ lib_deps = ESPAsyncWebServer AsyncTCP +; ESPA - Float board driven dal control core tick-based (Model-Based). +; Firmware SPERIMENTALE parallelo a espA: stesso hardware, ma il main fa da +; scheduler HAL attorno a lib/float_core (float_core_step). NON tocca espA (gara). +[env:espA_core] +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 = + + +build_flags = + ${esp32_common.build_flags} + -I lib/float_core/include + -DESPA_BUILD + +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_core/main.cpp b/src/espA_core/main.cpp new file mode 100644 index 0000000..ade2de0 --- /dev/null +++ b/src/espA_core/main.cpp @@ -0,0 +1,571 @@ +/* + ******************************************************************************* + * FLOAT board — ESP32-A (ESPA) — CORE-DRIVEN build + * + * main.cpp (env: espA_core) + * Scheduler HAL attorno al control core Model-Based (lib/float_core). + * + * Differenza rispetto a src/espA/main.cpp (firmware di gara, INTATTO): + * - la logica di missione NON vive qui: e' in float_core_step() (tick-based, + * pura, la stessa che il modello Simulink/Stateflow riprodurra' in MIL). + * - questo file fa SOLO da HAL: legge i sensori -> costruisce CoreInputs -> + * chiama float_core_step() -> attua motore/LED/comms/flash dai CoreOutputs. + * - i comandi mission-critical (GO/HOME/STOP) diventano eventi per il core; + * i comandi di manutenzione (config get/set, dati) sono gestiti dall'HAL + * solo quando il core e' IDLE. + * + * Non ancora portati in questo build sperimentale (restano in espA): OTA, + * balance, bench PID (SYRINGE/HOLD/STEP), auto-mode, simulatore seriale. + * + * Maintainers: Colabella Davide, Benevenga Filippo — Team PoliTOcean + ******************************************************************************* + */ + +#include +#include +#include +#include +#include + +#include +#include "led.h" +#include "motor.h" +#include "tof.h" +#include "motion_control.h" +#include "pid.h" +#include "sensors.h" +#include "comms.h" +#include "profile.h" +#include "flash_storage.h" +#include "runtime_config.h" + +#include "float_core.h" + +// --------------------------------------------------------------------------- +// Singleton (definiti qui perche' src/espA/main.cpp non viene compilato in +// questo environment: stesse istanze del firmware di gara). +// --------------------------------------------------------------------------- +LEDController ledController(PIN_LED_R, PIN_LED_G, PIN_LED_B); +MotorController motor; +TofSensor tofSensor(Wire, TOF_XSHUT_PIN, TOF_GPIO1_PIN); +MotionController motionController(motor, tofSensor); +SensorManager sensors; +PIDController pidController(PID_KP_DEFAULT, PID_KI_DEFAULT, PID_KD_DEFAULT); +ProfileManager profileManager; + +bool debug_mode_active = false; + +// --------------------------------------------------------------------------- +// Stato del core + config + memoria di edge-detection per l'attuatore. +// --------------------------------------------------------------------------- +static CoreState g_core; +static CoreConfig g_cfg; + +static unsigned long g_lastTickMs = 0; +static unsigned long g_missionStart = 0; +static CoreMode g_lastCoreMode = CORE_MODE_INIT; +static CoreMotorMode g_lastMotorMode = CORE_MOTOR_HOLD; +static long g_lastIssuedTarget = LONG_MIN; +static int g_lastJogDir = 0; + +// --------------------------------------------------------------------------- +// Costruzione della CoreConfig da config.h + runtime NVS. +// Richiamata al boot e dopo ogni comando che cambia la configurazione. +// --------------------------------------------------------------------------- +static void buildCoreConfig() { + const RuntimeProfileConfig& pc = profileManager.config(); + const RuntimePidConfig& pid = runtimeConfig.pid(); + + g_cfg.profile_count = pc.profileCount; + g_cfg.descent_target_m = pc.descentTargetM; + g_cfg.ascent_target_bottom_m = profileManager.ascentTargetBottomM(); + g_cfg.rest_target_bottom_m = profileManager.restTargetBottomM(); + g_cfg.depth_tolerance_m = pc.depthToleranceM; + g_cfg.hold_time_s = pc.holdTimeS; + g_cfg.descent_timeout_s = pc.descentTimeoutS; + g_cfg.ascent_timeout_s = pc.ascentTimeoutS; + g_cfg.rest_window_s = REST_WINDOW_S; + + g_cfg.kp = pid.kp; g_cfg.ki = pid.ki; g_cfg.kd = pid.kd; + g_cfg.alpha_d = pid.alphaD; + g_cfg.integral_limit = pid.integralLimit; + g_cfg.u_neutral = pid.uNeutral; + g_cfg.min_retarget_frac = pid.minRetargetFrac; + g_cfg.pid_period_s = pid.periodMs / 1000.0f; + g_cfg.descent_kick_u = PID_DESCENT_KICK_U; + g_cfg.u_min = PID_U_MIN; g_cfg.u_max = PID_U_MAX; + + g_cfg.usable_steps = (long)MOTOR_MAX_STEPS - 2L * (long)MOTOR_ENDSTOP_MARGIN; + g_cfg.endstop_margin_steps = MOTOR_ENDSTOP_MARGIN; + g_cfg.max_steps = MOTOR_MAX_STEPS; + + g_cfg.tof_safe_min_mm = TOF_SAFE_RANGE_MIN_MM; + g_cfg.tof_safe_max_mm = TOF_SAFE_RANGE_MAX_MM; + g_cfg.tof_safety_stop_samples = TOF_SAFETY_STOP_SAMPLES; + g_cfg.tof_homing_approach_mm = TOF_HOMING_APPROACH_MM; + g_cfg.tof_homing_threshold_mm = TOF_HOMING_THRESHOLD; + g_cfg.tof_homing_confirm_samples = TOF_HOMING_CONFIRM_SAMPLES; + g_cfg.homing_timeout_s = MOTOR_HOMING_TIMEOUT / 1000.0f; +} + +// --------------------------------------------------------------------------- +// Attuazione motore (contratto latch: GOTO aggancia, HOLD insegue, STOP ferma, +// JOG moto continuo). FastAccelStepper e' timer-driven: startMoveTo/Steps sono +// fire-and-forget, quindi HOLD non richiede nessuna azione (il motore prosegue +// verso il target agganciato). Edge-detection per non ri-comandare ogni tick. +// --------------------------------------------------------------------------- +static void applyMotor(const CoreOutputs& out) { + if (out.motor_zero_here) { + motor.setCurrentPosition(0); + g_lastIssuedTarget = 0; + } + + switch (out.motor_mode) { + case CORE_MOTOR_GOTO: + if (out.motor_enable) motor.enableOutputs(); + if (out.motor_target != g_lastIssuedTarget || g_lastMotorMode != CORE_MOTOR_GOTO) { + motor.startMoveTo(out.motor_target); + g_lastIssuedTarget = out.motor_target; + } + break; + + case CORE_MOTOR_JOG: { + const int dir = (out.motor_target < 0) ? -1 : +1; + if (g_lastMotorMode != CORE_MOTOR_JOG || dir != g_lastJogDir) { + motor.enableOutputs(); + motor.startMoveSteps(dir * 2L * (long)MOTOR_MAX_STEPS); + g_lastJogDir = dir; + } + break; + } + + case CORE_MOTOR_STOP: + if (g_lastMotorMode != CORE_MOTOR_STOP) motor.stop(); + if (!out.motor_enable) motor.disableOutputs(); + break; + + case CORE_MOTOR_HOLD: + default: + // Il motore prosegue autonomamente verso il target agganciato. + if (!out.motor_enable && motor.distanceToGo() == 0) motor.disableOutputs(); + break; + } + g_lastMotorMode = out.motor_mode; +} + +// --------------------------------------------------------------------------- +static void applyLed(const CoreOutputs& out, const CoreInputs& in) { + LEDState s = LEDState::OFF; + switch (out.led) { + case CORE_LED_INIT: s = LEDState::INIT; break; + case CORE_LED_IDLE: s = LEDState::IDLE; break; + case CORE_LED_IDLE_WITH_DATA: s = LEDState::IDLE_WITH_DATA; break; + case CORE_LED_ERROR: s = LEDState::ERROR; break; + case CORE_LED_PROFILE: s = LEDState::PROFILE; break; + case CORE_LED_HOMING: s = LEDState::HOMING; break; + case CORE_LED_PID_CONTROL: s = LEDState::PID_CONTROL; break; + case CORE_LED_COMMUNICATION: s = LEDState::COMMUNICATION; break; + case CORE_LED_OFF: + default: s = LEDState::OFF; break; + } + // Low-battery override in idle (come nel firmware di gara). + if (g_core.mode == CORE_MODE_IDLE && in.battery_mv < BATT_THRESH) { + s = LEDState::LOW_BATTERY; + } + ledController.setState(s); +} + +// --------------------------------------------------------------------------- +static void applyAck(const CoreOutputs& out, const CoreInputs& in) { + comms.status_to_send.charge = in.battery_mv; + switch (out.ack_event) { + case CORE_ACK_GO_RECVD: comms.sendMessage(CMD1_ACK, 1000); break; + case CORE_ACK_HOME_RECVD: comms.sendMessage(CMD12_ACK, 1000); break; + case CORE_ACK_STOP_RECVD: comms.sendMessage(CMD13_ACK, 1000); break; + case CORE_ACK_IDLE: comms.sendMessage(IDLE_ACK, 1000); break; + case CORE_ACK_IDLE_W_DATA: comms.sendMessage(IDLE_W_DATA_ACK, 1000); break; + case CORE_ACK_NONE: + default: break; + } +} + +// --------------------------------------------------------------------------- +static const char* phaseTag(CorePhase p) { + switch (p) { + case CORE_PHASE_DESCEND_HOLD: return "descending"; + case CORE_PHASE_ASCEND_HOLD: return "ascending"; + case CORE_PHASE_SURFACE_REST: return "resting"; + default: return "profile"; + } +} + +// Traduce l'evento di log del core in un record sul flash CSV (come +// ProfileManager::_logProfileReading, ma guidato dal core). +static void applyLog(const CoreOutputs& out) { + const char* tag = "profile"; + uint8_t profileId = (uint8_t)(out.completed_profiles + 1); + + switch (out.log_kind) { + case CORE_LOG_DEPLOY: tag = "deployed"; profileId = 0; + g_missionStart = millis(); break; + case CORE_LOG_PHASE_START: tag = "phase_start"; break; + case CORE_LOG_PROFILE: tag = phaseTag(out.phase); break; + case CORE_LOG_EXIT_HOLD_OK: tag = "exit_hold_ok"; break; + case CORE_LOG_EXIT_TIMEOUT: tag = "exit_timeout"; break; + case CORE_LOG_EXIT_REMOTE: tag = "exit_remote_stop"; break; + case CORE_LOG_EMERGENCY: tag = "emergency_stop"; profileId = 0; break; + case CORE_LOG_HOMING: tag = "homing"; profileId = 0; break; + case CORE_LOG_NONE: + default: return; + } + + const float t = g_missionStart ? (millis() - g_missionStart) / 1000.0f : 0.0f; + flashStorage.appendRecord(COMPANY_NUMBER, profileId, t, + sensors.pressure() / 1000.0f, + sensors.referenceDepthForPhase(tag), tag, + sensors.sensorDepth(), + motorPosToU(motor.position())); +} + +// --------------------------------------------------------------------------- +// Comandi di manutenzione (gestiti solo quando il core e' IDLE). Riusano i +// sottosistemi esistenti. I comandi mission-critical (GO/HOME/STOP) NON passano +// di qui: sono eventi per il core. +// --------------------------------------------------------------------------- +static void dispatchMaintenance(const output_message& cmd) { + char packet[OUTPUT_LEN]; + switch (cmd.command) { + case CMD_SEND_DATA: + profileManager.sendStoredData(); + break; + + case CMD_SEND_PACKAGE: + sensors.read(); + snprintf(packet, OUTPUT_LEN, + "{\"company_number\":\"%s\",\"time_s\":%.2f,\"pressure_kpa\":%.2f," + "\"depth_m\":%.2f,\"phase\":\"live\",\"sensor_depth_m\":%.2f," + "\"syringe_u\":%.4f}", + COMPANY_NUMBER, millis() / 1000.0f, sensors.pressure() / 1000.0f, + sensors.referenceDepthForPhase("live"), sensors.sensorDepth(), + motorPosToU(motor.position())); + comms.sendMessage(packet, 1000); + break; + + case CMD_CLEAR_EEPROM: + if (comms.sendMessage(CMD4_ACK, 1000)) profileManager.clearEEPROM(); + break; + + case CMD_SET_SURFACE_OFFSET: + if (comms.sendMessage(CMD18_ACK, 1000)) + sensors.setSurfaceTargetOffset(cmd.payload.surfaceOffset.meters); + break; + + case CMD_DEBUG_MODE: + if (comms.sendMessage(CMD11_ACK, 1000)) debug_mode_active = !debug_mode_active; + break; + + case CMD_PID_CONFIG_SET: { + const PidConfigPayload& p = cmd.payload.pidConfig; + RuntimePidConfig c; + c.kp = p.kp; c.ki = p.ki; c.kd = p.kd; + c.periodMs = (uint16_t)p.periodMs; c.alphaD = p.alphaD; + c.integralLimit = p.integralLimit; c.minRetargetFrac = p.minRetargetFrac; + c.uNeutral = p.uNeutral; + const bool ok = runtimeConfig.setPidConfig(c); + buildCoreConfig(); + comms.sendMessage(ok ? CMD8_ACK : CMD8_ERR, 1000); + break; + } + case CMD_PID_CONFIG_GET: + runtimeConfig.formatPidConfigJson(packet, sizeof(packet)); + comms.sendMessage(packet, 1000); + break; + + case CMD_PROFILE_SET: { + const ProfileSetPayload& p = cmd.payload.profileSet; + RuntimeProfileConfig c; + c.profileCount = p.profileCount; c.descentTargetM = p.descentTargetM; + c.ascentTargetM = p.ascentTargetM; c.depthToleranceM = p.depthToleranceM; + c.holdTimeS = p.holdTimeS; c.descentTimeoutS = p.descentTimeoutS; + c.ascentTimeoutS = p.ascentTimeoutS; c.surfaceRestOffsetM = p.surfaceRestOffsetM; + const bool ok = profileManager.setConfig(c); + buildCoreConfig(); + comms.sendMessage(ok ? CMD19_ACK : CMD19_ERR, 1000); + break; + } + case CMD_PROFILE_GET: + profileManager.formatConfigJson(packet, sizeof(packet)); + comms.sendMessage(packet, 1000); + break; + + case CMD_BALANCE_CONFIG_SET: { + const BalanceConfigPayload& p = cmd.payload.balanceConfig; + RuntimeBalanceConfig c; + c.holdMs = p.holdMs; c.stopPressureDeltaKpa = p.stopPressureDeltaKpa; + c.stopPressureSamples = p.stopPressureSamples; c.samplePeriodMs = p.samplePeriodMs; + const bool ok = runtimeConfig.setBalanceConfig(c); + comms.sendMessage(ok ? CMD21_ACK : CMD21_ERR, 1000); + break; + } + case CMD_BALANCE_CONFIG_GET: + runtimeConfig.formatBalanceConfigJson(packet, sizeof(packet)); + comms.sendMessage(packet, 1000); + break; + + case CMD_MOTOR_CONFIG_SET: { + const MotorConfigPayload& p = cmd.payload.motorConfig; + RuntimeMotorConfig c; + c.maxSpeed = p.maxSpeed; c.maxAcceleration = p.maxAcceleration; + c.homingSpeed = p.homingSpeed; c.testSpeed = p.testSpeed; + const bool ok = runtimeConfig.setMotorConfig(c); + buildCoreConfig(); + comms.sendMessage(ok ? CMD23_ACK : CMD23_ERR, 1000); + break; + } + case CMD_MOTOR_CONFIG_GET: + runtimeConfig.formatMotorConfigJson(packet, sizeof(packet)); + comms.sendMessage(packet, 1000); + break; + + default: + Debug.printf("espA_core: cmd %d non supportato in questo build\n", cmd.command); + break; + } +} + +// --------------------------------------------------------------------------- +// TEST DA BANCO — comandi seriali USB diretti. +// Con SIM attivo: motore/TOF/homing REALI, quota SIMULATA -> missione completa +// a secco sul chip vero, traccia confrontabile con l'harness MIL nativo. +// +// SIM_ON | SIM_OFF | SIM_GET | SIM_CONFIG +// GO | HOME | STOP — inietta l'evento nel core (one-shot) +// TELEM_ON | TELEM_OFF — telemetria CSV su Serial +// DUMP_LOG — dump del flash log +// ATTENZIONE: con GO il pistone si estende DAVVERO (u fino a 0.92). Verifica che +// il meccanismo sia libero di muoversi prima di lanciare la missione al banco. +// --------------------------------------------------------------------------- +static volatile CoreEvent g_serialEvent = CORE_EVT_NONE; +static bool g_telem = false; +static unsigned long g_telemLastMs = 0; +static CoreMode g_telemLastMode = CORE_MODE_INIT; +static CorePhase g_telemLastPhase = CORE_PHASE_NONE; +static String g_serialBuf; + +static const char* coreModeName(CoreMode m) { + switch (m) { + case CORE_MODE_INIT: return "INIT"; + case CORE_MODE_HOMING: return "HOMING"; + case CORE_MODE_IDLE: return "IDLE"; + case CORE_MODE_MISSION: return "MISSION"; + case CORE_MODE_SAFE_STOP: return "SAFE_STOP"; + default: return "?"; + } +} +static const char* corePhaseName(CorePhase p) { + switch (p) { + case CORE_PHASE_PRE_DESCENT: return "pre_descent"; + case CORE_PHASE_DESCEND_HOLD: return "descend_hold"; + case CORE_PHASE_ASCEND_HOLD: return "ascend_hold"; + case CORE_PHASE_SURFACE_REST: return "surface_rest"; + case CORE_PHASE_DONE: return "done"; + default: return "-"; + } +} +static float coreTargetForPhase(CorePhase p) { + switch (p) { + case CORE_PHASE_DESCEND_HOLD: return g_cfg.descent_target_m; + case CORE_PHASE_ASCEND_HOLD: return g_cfg.ascent_target_bottom_m; + case CORE_PHASE_SURFACE_REST: return g_cfg.rest_target_bottom_m; + default: return 0.0f; + } +} + +// Telemetria: una riga ~1 Hz + a ogni cambio di modo/fase. Stesse colonne del +// MIL nativo (tools/mil) per confronto diretto on-target vs simulazione. +static void serviceTelemetry(const CoreInputs& in, const CoreOutputs& out) { + if (!g_telem) return; + const unsigned long now = millis(); + const bool changed = (g_core.mode != g_telemLastMode) || (out.phase != g_telemLastPhase); + if (!changed && (now - g_telemLastMs) < 1000) return; + g_telemLastMs = now; + g_telemLastMode = g_core.mode; + g_telemLastPhase = out.phase; + Serial.printf("%.2f,%s,%s,%.3f,%.3f,%.4f,%ld,%.1f\n", + now / 1000.0f, coreModeName(g_core.mode), corePhaseName(out.phase), + in.depth_m, coreTargetForPhase(out.phase), + motorPosToU(in.motor_pos), in.motor_pos, + in.tof_valid ? in.tof_mm : -1.0f); +} + +static void serviceSerial() { + while (Serial.available()) { + char c = Serial.read(); + if (c == '\r') continue; + if (c != '\n') { if (g_serialBuf.length() < 120) g_serialBuf += c; continue; } + + String line = g_serialBuf; g_serialBuf = ""; line.trim(); + if (line.length() == 0) return; + char buf[128]; + strncpy(buf, line.c_str(), sizeof(buf) - 1); buf[sizeof(buf) - 1] = '\0'; + char* tok = strtok(buf, " "); + if (!tok) return; + + if (!strcmp(tok, "GO")) { g_serialEvent = CORE_EVT_GO; Serial.println("# GO"); } + else if (!strcmp(tok, "HOME")) { g_serialEvent = CORE_EVT_HOME; Serial.println("# HOME"); } + else if (!strcmp(tok, "STOP")) { g_serialEvent = CORE_EVT_STOP; Serial.println("# STOP"); } + else if (!strcmp(tok, "SIM_ON")) { sensors.simEnable(true); Serial.println("# SIM ON (motore/TOF reali, quota simulata)"); } + else if (!strcmp(tok, "SIM_OFF")) { sensors.simEnable(false); Serial.println("# SIM OFF"); } + else if (!strcmp(tok, "SIM_GET")) { char s[176]; sensors.simFormatStatus(s, sizeof(s)); Serial.println(s); } + else if (!strcmp(tok, "SIM_CONFIG")) { + char* v[4] = {}; + for (auto& x : v) { x = strtok(nullptr, " "); if (!x) { Serial.println("# ERR: SIM_CONFIG "); return; } } + sensors.simConfigure(atof(v[0]), atof(v[1]), atof(v[2]), atof(v[3])); + } + else if (!strcmp(tok, "TELEM_ON")) { g_telem = true; Serial.println("t_s,mode,phase,depth_m,target_m,u,motor_pos,tof_mm"); } + else if (!strcmp(tok, "TELEM_OFF")) { g_telem = false; } + else if (!strcmp(tok, "DUMP_LOG")) { + Serial.println("===== FLASH LOG DUMP BEGIN ====="); + if (!flashStorage.printLogTo(Serial)) Serial.println("(no log)"); + Serial.println("===== FLASH LOG DUMP END ====="); + } + else { Serial.printf("# ERR: comando sconosciuto '%s'\n", tok); } + return; + } +} + +// --------------------------------------------------------------------------- +// SETUP +// --------------------------------------------------------------------------- +void setup() { + delay(100); + Serial.begin(115200); + Debug.println("=== Float ESPA (core-driven) — starting ==="); + + ledController.setState(LEDState::INIT); + + if (!EEPROM.begin(EEPROM_SIZE)) { + Debug.println("CRITICAL: EEPROM init failed"); + ledController.setState(LEDState::ERROR); + while (true) { ledController.update(); yield(); } + } + + comms.begin(); + Debug.begin(&debug_mode_active, + [](const char* msg, uint32_t timeout) -> uint8_t { + return comms.sendMessage(msg, timeout) ? 1 : 0; + }); + + runtimeConfig.begin(); + profileManager.beginConfig(); + + if (flashStorage.begin()) { + 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 ====="); + } else { + Debug.println("WARNING: flash log unavailable"); + } + + sensors.begin(); + + motor.begin(); + runtimeConfig.applyMotorConfig(); + + Debug.println("Initializing TOF sensor..."); + if (!tofSensor.begin()) { + Debug.println("CRITICAL: TOF sensor init failed"); + ledController.setState(LEDState::ERROR); + while (true) { ledController.update(); yield(); } + } + + // A differenza di espA, l'homing NON e' fatto qui: lo guida il core + // (INIT -> HOMING) al primo tick, via i CoreOutputs. + buildCoreConfig(); + float_core_init(&g_core); + comms.status_to_send.charge = BATT_THRESH + 1; + + Debug.println("=== Init complete — core scheduler running ==="); +} + +// --------------------------------------------------------------------------- +// LOOP — scheduler HAL a passo fisso (pid_period). Nessuna logica di missione: +// e' tutta in float_core_step(). +// --------------------------------------------------------------------------- +void loop() { + ledController.update(); + serviceSerial(); // reattivo ogni iterazione, indipendente dal tick del core + + const unsigned long now = millis(); + const unsigned long periodMs = runtimeConfig.pid().periodMs; + if (g_lastTickMs != 0 && (now - g_lastTickMs) < periodMs) { + delay(1); + return; + } + const float dt = (g_lastTickMs == 0) ? (periodMs / 1000.0f) + : (now - g_lastTickMs) / 1000.0f; + g_lastTickMs = now; + + // --- Ingressi: sensori + comando --- + sensors.read(); + float tofMm = 0.0f; + const bool tofValid = tofSensor.isInitialized() && tofSensor.readDistanceMm(tofMm); + + output_message cmd = comms.lastCommand(); + CoreEvent event = CORE_EVT_NONE; + if (cmd.command == CMD_GO) event = CORE_EVT_GO; + else if (cmd.command == CMD_HOME) event = CORE_EVT_HOME; + else if (cmd.command == CMD_STOP) event = CORE_EVT_STOP; + if (event != CORE_EVT_NONE) comms.clearCommand(); // consuma l'evento mission + + // Evento da seriale (test da banco): one-shot, se non c'e' gia' un comando radio. + if (event == CORE_EVT_NONE && g_serialEvent != CORE_EVT_NONE) { + event = g_serialEvent; + g_serialEvent = CORE_EVT_NONE; + } + + CoreInputs in; + in.dt_s = dt; + in.depth_m = sensors.depth(); + in.pressure_kpa = sensors.pressure() / 1000.0f; + in.tof_mm = tofValid ? tofMm : 0.0f; + in.tof_valid = tofValid; + in.motor_pos = motor.position(); + in.motor_position_known = motor.isPositionKnown(); + in.motor_busy = motor.distanceToGo() != 0; + in.battery_mv = sensors.batteryMilliVolts(); + in.has_stored_data = profileManager.writePtr() > profileManager.readPtr(); + in.event = event; + in.cfg = &g_cfg; + + // --- Un passo del core --- + CoreOutputs out; + float_core_step(&in, &g_core, &out); + + // --- Velocita' motore per modo (homing vs missione) --- + if (g_core.mode != g_lastCoreMode) { + if (g_core.mode == CORE_MODE_HOMING) { + motor.setMaxSpeed(runtimeConfig.motor().homingSpeed); + motor.setAcceleration(runtimeConfig.motor().homingSpeed); + } else { + motor.setMaxSpeed(runtimeConfig.motor().maxSpeed); + motor.setAcceleration(runtimeConfig.motor().maxAcceleration); + } + g_lastCoreMode = g_core.mode; + } + + // --- Attuazione --- + applyMotor(out); + applyLed(out, in); + applyAck(out, in); + if (out.log_now) applyLog(out); + serviceTelemetry(in, out); + + // --- Comandi di manutenzione: solo a core IDLE --- + if (g_core.mode == CORE_MODE_IDLE && + cmd.command != CMD_IDLE && event == CORE_EVT_NONE) { + dispatchMaintenance(cmd); + comms.clearCommand(); + } +} diff --git a/tools/mil/.gitignore b/tools/mil/.gitignore new file mode 100644 index 0000000..567609b --- /dev/null +++ b/tools/mil/.gitignore @@ -0,0 +1 @@ +build/ diff --git a/tools/mil/Makefile b/tools/mil/Makefile new file mode 100644 index 0000000..31db789 --- /dev/null +++ b/tools/mil/Makefile @@ -0,0 +1,26 @@ +# Harness MIL nativo (host-only). Nessun hardware, nessun MATLAB. +# make -> compila build/float_mil +# make run -> compila ed esegue, salvando il CSV in build/mission.csv +# make clean + +CC ?= cc +CFLAGS ?= -std=c11 -O2 -Wall -Wextra -Wno-unused-parameter +COREINC := ../../lib/float_core/include +CORESRC := ../../lib/float_core/src/float_core.c + +BUILD := build +BIN := $(BUILD)/float_mil +SRCS := mil_main.c plant.c $(CORESRC) + +.PHONY: all run clean +all: $(BIN) + +$(BIN): $(SRCS) plant.h $(COREINC)/float_core.h $(COREINC)/float_core_types.h + @mkdir -p $(BUILD) + $(CC) $(CFLAGS) -I$(COREINC) -I. $(SRCS) -lm -o $(BIN) + +run: $(BIN) + ./$(BIN) | tee $(BUILD)/mission.csv + +clean: + rm -rf $(BUILD) diff --git a/tools/mil/README.md b/tools/mil/README.md new file mode 100644 index 0000000..19df2bb --- /dev/null +++ b/tools/mil/README.md @@ -0,0 +1,42 @@ +# MIL nativo — anello chiuso control core + plant (host, no MATLAB) + +Harness di simulazione **host-only** che chiude l'anello tra: + +- il **control core** portabile (`lib/float_core/`, C puro, tick-based) e +- un **plant** del float (`plant.c`): attuatore motore rate-limited (~2 mm/s), + dinamica di profondità del 2° ordine, modelli sensore TOF e pressione. + +È il primo anello della catena MBSD **prima** di aprire Simulink: dà feedback +immediato sul comportamento del core ed è la "specifica eseguibile" che il +modello Stateflow/Simulink dovrà riprodurre (equivalenza MIL). + +## Build & run + +Serve un compilatore C (`gcc`/`cc`) e, opzionalmente, `make`. + +```bash +# con make +make -C tools/mil run + +# oppure a mano +cd tools/mil +gcc -std=c11 -O2 -I../../lib/float_core/include -I. \ + mil_main.c plant.c ../../lib/float_core/src/float_core.c -lm -o build/float_mil +./build/float_mil > build/mission.csv +``` + +Esegue: `boot → homing → IDLE → GO → 2 profili (discesa 2.5 m / hold / +risalita / hold) → sosta finale → IDLE`, stampa il CSV della missione e un +riepilogo con la verifica dei requisiti MATE. + +## Cosa NON è + +Non usa Embedded Coder né MATLAB: il core qui è l'implementazione di +**riferimento** scritta a mano (`float_core.c`). Quando il modello Simulink +sarà pronto, il codice generato rimpiazzerà `float_core.c` mantenendo la stessa +firma `float_core_step()`, e questo stesso harness servirà a confrontare i due +(back-to-back, verso il SIL). + +## Colonne del CSV + +`t_s, mode, phase, depth_m, target_m, u, motor_pos, tof_mm, stable_s` diff --git a/tools/mil/mil_main.c b/tools/mil/mil_main.c new file mode 100644 index 0000000..dd928fd --- /dev/null +++ b/tools/mil/mil_main.c @@ -0,0 +1,213 @@ +/* + ******************************************************************************* + * mil_main.c (host-only, MIL) + * Anello chiuso NATIVO (pre-Simulink): control core (float_core.c) + plant + * (plant.c) su PC, senza hardware ne' MATLAB. Esegue: + * boot -> homing -> IDLE -> GO -> 2 profili (discesa/hold/risalita/hold) + * -> sosta finale -> IDLE. + * Stampa il CSV della missione e verifica i requisiti MATE. + * + * Scopo: feedback immediato sul comportamento del core e "specifica eseguibile" + * che il modello Simulink/Stateflow dovra' riprodurre (equivalenza MIL). + * + * Build: make -C tools/mil (oppure vedi Makefile) + ******************************************************************************* + */ + +#include "float_core.h" +#include "plant.h" +#include +#include + +/* --- Costanti geometriche/di missione (mirror di include/config.h) --------- */ +#define STEPS_PER_MM (200.0 * 1.0 * (26.85124 / 8.0)) /* 671.28 */ +#define TRAVEL_MM 45.0 +#define ENDSTOP_MARGIN 10L + +/* Riferimenti FONDO (come li calcola ProfileManager): + * SENSOR_TO_BOTTOM_M = 0.49 ; SENSOR_TO_TOP_M = -0.01 + * ascent_bottom = ascent_top(0.40) + 0.49 + (-0.01) = 0.88 + * rest_bottom = rest_top(0.15) + 0.49 + (-0.01) = 0.63 */ +#define ASCENT_TOP 0.40 +#define REST_TOP 0.15 +#define SENSOR_TO_BOTTOM 0.49 +#define SENSOR_TO_TOP (-0.01) + +static CoreConfig make_config(void) { + CoreConfig c; + const long max_steps = (long)(TRAVEL_MM * STEPS_PER_MM + 0.5); + + c.profile_count = 2; + c.descent_target_m = 2.50; + c.ascent_target_bottom_m = ASCENT_TOP + SENSOR_TO_BOTTOM + SENSOR_TO_TOP; /* 0.88 */ + c.rest_target_bottom_m = REST_TOP + SENSOR_TO_BOTTOM + SENSOR_TO_TOP; /* 0.63 */ + c.depth_tolerance_m = 0.33; + c.hold_time_s = 30.0; + c.descent_timeout_s = 180.0; + c.ascent_timeout_s = 120.0; + c.rest_window_s = 120.0; + + c.kp = 0.5f; c.ki = 0.1f; c.kd = 3.0f; + c.alpha_d = 0.25f; + c.integral_limit = 5.0f; + c.u_neutral = 0.011f; + c.min_retarget_frac = 0.001f; + c.pid_period_s = 0.05f; + c.descent_kick_u = 0.15f; + c.u_min = 0.0f; c.u_max = 0.92f; + + c.usable_steps = max_steps - 2L * ENDSTOP_MARGIN; + c.endstop_margin_steps = ENDSTOP_MARGIN; + c.max_steps = max_steps; + + c.tof_safe_min_mm = 32.0f; + c.tof_safe_max_mm = 82.0f; + c.tof_safety_stop_samples = 3; + c.tof_homing_approach_mm = 50.0f; + c.tof_homing_threshold_mm = 70.0f; + c.tof_homing_confirm_samples = 2; + c.homing_timeout_s = 30.0f; + return c; +} + +static const char* phase_name(CorePhase p) { + switch (p) { + case CORE_PHASE_PRE_DESCENT: return "pre_descent"; + case CORE_PHASE_DESCEND_HOLD: return "descend_hold"; + case CORE_PHASE_ASCEND_HOLD: return "ascend_hold"; + case CORE_PHASE_SURFACE_REST: return "surface_rest"; + case CORE_PHASE_DONE: return "done"; + default: return "-"; + } +} + +static const char* mode_name(CoreMode m) { + switch (m) { + case CORE_MODE_INIT: return "INIT"; + case CORE_MODE_HOMING: return "HOMING"; + case CORE_MODE_IDLE: return "IDLE"; + case CORE_MODE_MISSION: return "MISSION"; + case CORE_MODE_SAFE_STOP: return "SAFE_STOP"; + default: return "?"; + } +} + +static double target_for_phase(const CoreConfig* c, CorePhase p) { + switch (p) { + case CORE_PHASE_DESCEND_HOLD: return c->descent_target_m; + case CORE_PHASE_ASCEND_HOLD: return c->ascent_target_bottom_m; + case CORE_PHASE_SURFACE_REST: return c->rest_target_bottom_m; + default: return 0.0; + } +} + +int main(void) { + const CoreConfig cfg = make_config(); + const double dt = 0.05; + + CoreState st; + CoreOutputs out; + Plant p; + + float_core_init(&st); + plant_init(&p, &cfg, /*initial_pos=*/3000); /* non-homed: forza l'homing */ + + int homed = 0, go_sent = 0, mission_ran = 0; + + /* metriche per la verifica MATE (per fase) */ + double deep_closest = 1e9, shallow_closest = 1e9; + double max_depth = 0.0, min_top_depth = 1e9; + + printf("t_s,mode,phase,depth_m,target_m,u,motor_pos,tof_mm,stable_s\n"); + + const long max_ticks = (long)(1000.0 / dt); /* limite di sicurezza: 1000 s */ + for (long k = 0; k < max_ticks; k++) { + const double t = k * dt; + + CoreInputs in; + in.dt_s = (float)dt; + in.depth_m = (float)p.z; + in.pressure_kpa = (float)plant_pressure_kpa(&p); + in.tof_mm = (float)plant_tof_mm(&p); + in.tof_valid = true; + in.motor_pos = p.motor_pos; + in.motor_position_known = p.position_known; + in.motor_busy = plant_motor_busy(&p); + in.battery_mv = 12500; + in.has_stored_data = (st.completed_profiles > 0); + in.cfg = &cfg; + + /* iniezione comando: appena si raggiunge IDLE dopo l'homing, invia GO */ + in.event = CORE_EVT_NONE; + if (st.mode == CORE_MODE_IDLE && !homed) { + homed = 1; + } + if (st.mode == CORE_MODE_IDLE && homed && !go_sent) { + in.event = CORE_EVT_GO; + go_sent = 1; + p.z = 0.0; p.v = 0.0; p.dynamics_active = true; /* missione parte dal pelo */ + } + + float_core_step(&in, &st, &out); + plant_apply_and_step(&p, &out, dt); + + /* --- raccolta metriche durante le fasi di missione --- */ + if (out.mission_active) { + mission_ran = 1; + const double tgt = target_for_phase(&cfg, out.phase); + const double top_depth = p.z - SENSOR_TO_BOTTOM - SENSOR_TO_TOP; /* z - 0.48 */ + if (p.z > max_depth) max_depth = p.z; + + if (out.phase == CORE_PHASE_DESCEND_HOLD) { + double e = fabs(p.z - tgt); + if (e < deep_closest) deep_closest = e; + } else if (out.phase == CORE_PHASE_ASCEND_HOLD) { + double e = fabs(p.z - tgt); + if (e < shallow_closest) shallow_closest = e; + if (top_depth < min_top_depth) min_top_depth = top_depth; + } + } + + /* --- CSV: una riga al secondo, piu' i cambi di modo/fase --- */ + static CoreMode last_mode = CORE_MODE_INIT; + static CorePhase last_phase = CORE_PHASE_NONE; + if ((k % 20) == 0 || st.mode != last_mode || out.phase != last_phase) { + printf("%.2f,%s,%s,%.3f,%.3f,%.4f,%ld,%.1f,%.1f\n", + t, mode_name(st.mode), phase_name(out.phase), + p.z, target_for_phase(&cfg, out.phase), + float_core_motor_pos_to_u(&cfg, p.motor_pos), + p.motor_pos, plant_tof_mm(&p), st.stable_s); + last_mode = st.mode; + last_phase = out.phase; + } + + /* fine: missione conclusa e tornati a IDLE */ + if (mission_ran && go_sent && st.mode == CORE_MODE_IDLE) { + printf("# mission complete at t=%.1fs\n", t); + break; + } + if (st.mode == CORE_MODE_SAFE_STOP) { + printf("# SAFE_STOP reached at t=%.1fs (unexpected in nominal MIL)\n", t); + } + } + + /* ------------------------------------------------------------------ */ + /* VERIFICA REQUISITI MATE */ + /* ------------------------------------------------------------------ */ + printf("\n===== MIL SUMMARY =====\n"); + printf("homing completed : %s\n", homed ? "YES" : "NO"); + printf("mission executed : %s\n", mission_ran ? "YES" : "NO"); + printf("profiles completed : %u / %u\n", st.completed_profiles, cfg.profile_count); + printf("deep closest to 2.50 m : %.3f m (tol %.2f)\n", deep_closest, cfg.depth_tolerance_m); + printf("shallow closest to 0.88 m : %.3f m (tol %.2f)\n", shallow_closest, cfg.depth_tolerance_m); + printf("max depth (overshoot) : %.3f m\n", max_depth); + printf("min top-depth (surface) : %.3f m (>=0 = non rompe la superficie)\n", min_top_depth); + + int pass = homed && mission_ran && + (st.completed_profiles == cfg.profile_count) && + (deep_closest < cfg.depth_tolerance_m) && + (shallow_closest < cfg.depth_tolerance_m) && + (min_top_depth >= 0.0); + printf("RESULT: %s\n", pass ? "PASS" : "CHECK (vedi metriche/tuning)"); + return pass ? 0 : 1; +} diff --git a/tools/mil/plant.c b/tools/mil/plant.c new file mode 100644 index 0000000..653ac41 --- /dev/null +++ b/tools/mil/plant.c @@ -0,0 +1,118 @@ +/* + ******************************************************************************* + * plant.c (host-only, MIL) — vedi plant.h + ******************************************************************************* + */ + +#include "plant.h" +#include "float_core.h" + +static long clamp_l(long v, long lo, long hi) { + if (v < lo) return lo; + if (v > hi) return hi; + return v; +} + +void plant_init(Plant* p, const CoreConfig* cfg, long initial_pos) { + p->motor_pos = initial_pos; + p->motor_target = initial_pos; + p->motor_mode = CORE_MOTOR_HOLD; + p->motor_jog_dir = 0; + p->position_known = false; + /* ~2 mm/s: MOTOR_MAX_SPEED (1400 step/s) * dt */ + p->max_step_per_tick = 70; /* 1400 * 0.05 */ + + p->z = 0.0; + p->v = 0.0; + p->dynamics_active = false; + + p->u_neutral = 0.35; /* SIM_U_NEUTRAL */ + p->accel_gain = 0.05; /* SIM_ACCEL_GAIN */ + p->drag_quad = 1.50; /* SIM_DRAG_QUAD */ + p->pool_depth = 3.00; /* SIM_POOL_DEPTH */ + + p->usable_steps = cfg->usable_steps; + p->tof_home_mm = 75.0; /* pos=0 */ + p->tof_ext_mm = 34.0; /* u=1 */ +} + +double plant_tof_mm(const Plant* p) { + /* frazione di estensione: 0 = home, 1 = piena estensione (pos = -usable) */ + const double frac = -(double)p->motor_pos / (double)p->usable_steps; + return p->tof_home_mm - frac * (p->tof_home_mm - p->tof_ext_mm); +} + +double plant_pressure_kpa(const Plant* p) { + /* pressione idrostatica di gauge: rho*g*z */ + return 997.0 * 9.80665 * (p->z) / 1000.0; +} + +bool plant_motor_busy(const Plant* p) { + if (p->motor_mode == CORE_MOTOR_JOG) return true; + /* GOTO/HOLD: "busy" finche' non ha raggiunto il target agganciato */ + if (p->motor_mode == CORE_MOTOR_STOP) return false; + return p->motor_pos != p->motor_target; +} + +/* Contratto HAL dell'attuatore (come FastAccelStepper.startMoveTo): + * GOTO -> AGGANCIA un nuovo target; l'attuatore lo insegue. + * HOLD -> nessun nuovo comando: continua a inseguire il target agganciato. + * STOP -> ferma qui (target = posizione attuale). + * JOG -> moto continuo nel verso indicato. */ +static void move_toward_target(Plant* p) { + long d = p->motor_target - p->motor_pos; + if (d > p->max_step_per_tick) d = p->max_step_per_tick; + if (d < -p->max_step_per_tick) d = -p->max_step_per_tick; + p->motor_pos += d; +} + +static void actuator_step(Plant* p, const CoreOutputs* out) { + p->motor_mode = out->motor_mode; + + if (out->motor_zero_here) { + p->motor_pos = 0; + p->motor_target = 0; + p->position_known = true; + } + + switch (out->motor_mode) { + case CORE_MOTOR_GOTO: + p->motor_target = out->motor_target; /* aggancia il nuovo target */ + move_toward_target(p); + break; + case CORE_MOTOR_HOLD: + move_toward_target(p); /* continua verso il target agganciato */ + break; + case CORE_MOTOR_JOG: + p->motor_jog_dir = (out->motor_target < 0) ? -1 : +1; + p->motor_pos += p->motor_jog_dir * p->max_step_per_tick; + p->motor_pos = clamp_l(p->motor_pos, -2 * p->usable_steps, p->usable_steps); + p->motor_target = p->motor_pos; /* fermo qui se poi arriva HOLD */ + break; + case CORE_MOTOR_STOP: + default: + p->motor_target = p->motor_pos; /* ferma qui */ + break; + } +} + +static void dynamics_step(Plant* p, double dt) { + if (!p->dynamics_active) return; + + /* u dalla posizione reale del motore (come motorPosToU nel firmware) */ + double u = -(double)p->motor_pos / (double)p->usable_steps; + if (u < 0.0) u = 0.0; + if (u > 1.0) u = 1.0; + + const double a = p->accel_gain * (u - p->u_neutral) - p->drag_quad * p->v * (p->v < 0 ? -p->v : p->v); + p->v += a * dt; + p->z += p->v * dt; + + if (p->z < 0.0) { p->z = 0.0; if (p->v < 0.0) p->v = 0.0; } + if (p->z > p->pool_depth) { p->z = p->pool_depth; if (p->v > 0.0) p->v = 0.0; } +} + +void plant_apply_and_step(Plant* p, const CoreOutputs* out, double dt) { + actuator_step(p, out); + dynamics_step(p, dt); +} diff --git a/tools/mil/plant.h b/tools/mil/plant.h new file mode 100644 index 0000000..14e7630 --- /dev/null +++ b/tools/mil/plant.h @@ -0,0 +1,55 @@ +#ifndef FLOAT_MIL_PLANT_H +#define FLOAT_MIL_PLANT_H + +/* + ******************************************************************************* + * plant.h (host-only, MIL) + * Modello del PLANT del float: attuatore motore (rate-limited ~2 mm/s), + * dinamica di profondita' del 2o ordine (spinta siringa + drag quadratico), + * e modelli sensore TOF e pressione. E' la controparte "fisica" del control + * core: in Simulink diventera' float_plant.slx. + * + * Convenzione coerente col firmware: u>uNeutral => affonda (z cresce). + ******************************************************************************* + */ + +#include "float_core_types.h" + +typedef struct { + /* --- attuatore --- */ + long motor_pos; /* step correnti */ + long motor_target; /* target del GOTO / verso del JOG */ + int motor_mode; /* CoreMotorMode dell'ultimo comando */ + int motor_jog_dir; /* -1/0/+1 durante il JOG */ + bool position_known; + long max_step_per_tick; + + /* --- dinamica di profondita' (2o ordine) --- */ + double z; /* quota (rif. fondo), m */ + double v; /* velocita' verticale, m/s (+ = affonda) */ + bool dynamics_active; /* integra solo durante la missione */ + + /* --- parametri fisici (come i SIM_* di config.h) --- */ + double u_neutral; + double accel_gain; + double drag_quad; + double pool_depth; + + /* --- geometria per i modelli sensore --- */ + long usable_steps; + double tof_home_mm; /* TOF a pos=0 (home) */ + double tof_ext_mm; /* TOF a u=1 (piena estensione) */ +} Plant; + +/* Inizializza il plant (posizione iniziale non-homed per esercitare l'homing). */ +void plant_init(Plant* p, const CoreConfig* cfg, long initial_pos); + +/* Applica un comando del core all'attuatore e avanza la dinamica di dt secondi. */ +void plant_apply_and_step(Plant* p, const CoreOutputs* out, double dt); + +/* Modelli sensore. */ +double plant_tof_mm(const Plant* p); +double plant_pressure_kpa(const Plant* p); +bool plant_motor_busy(const Plant* p); + +#endif /* FLOAT_MIL_PLANT_H */