autocore_std/fb/beckhoff/

el3356.rs

//! Function block for the Beckhoff EL3356 strain-gauge EtherCAT terminal.
//!
//! # Responsibilities
//!
//! - **Peak tracking**: maintains `peak_load` as the largest-magnitude `load`
//!   value seen since construction or the last [`reset_peak`](El3356::reset_peak) /
//!   [`tare`](El3356::tare) call.
//! - **Tare pulse**: [`tare`](El3356::tare) sets the device's tare bit and
//!   automatically clears it after 100 ms. Also resets the peak.
//! - **Load cell configuration via SDO**: [`configure`](El3356::configure)
//!   sequences three SDO writes to `0x8000`:
//!     - sub `0x23` — sensitivity (mV/V)
//!     - sub `0x24` — full-scale load
//!     - sub `0x27` — scale factor
//!
//! # Wiring in project.json
//!
//! The FB is device-agnostic at construction; the EtherCAT device name is
//! passed to [`new`](El3356::new) and used for SDO topic routing. The five
//! PDO-linked GM variables (`{prefix}_load`, `_load_steady`, `_load_error`,
//! `_load_overrange`, `_tare`) must exist and be linked to the terminal's
//! corresponding FQDNs in `project.json`.
//!
//! # Example
//!
//! ```ignore
//! use autocore_std::fb::beckhoff::El3356;
//! use autocore_std::el3356_view;
//!
//! pub struct MyProgram {
//!     load_cell: El3356,
//!     manual_tare_edge: autocore_std::fb::RTrig,
//! }
//!
//! impl MyProgram {
//!     pub fn new() -> Self {
//!         Self {
//!             load_cell: El3356::new("EL3356_0"),
//!             manual_tare_edge: Default::default(),
//!         }
//!     }
//! }
//!
//! impl ControlProgram for MyProgram {
//!     type Memory = GlobalMemory;
//!
//!     fn process_tick(&mut self, ctx: &mut TickContext<Self::Memory>) {
//!         // Edge-detect a manual tare button from an HMI
//!         if self.manual_tare_edge.call(ctx.gm.manual_tare) {
//!             self.load_cell.tare();
//!         }
//!
//!         let mut view = el3356_view!(ctx.gm, impact);
//!         self.load_cell.tick(&mut view, ctx.client);
//!
//!         // Peak load is exposed for display / logging
//!         ctx.gm.impact_peak_load = self.load_cell.peak_load;
//!     }
//! }
//! ```

use crate::ethercat::{SdoClient, SdoResult};
use crate::CommandClient;
use serde_json::json;
use std::time::{Duration, Instant};

use super::El3356View;

/// Duration of the tare pulse. Matches the `T#100MS` preset in the original
/// TwinCAT implementation.
const TARE_PULSE: Duration = Duration::from_millis(100);

/// Per-SDO timeout. SDO transfers against a functioning EL3356 complete in
/// tens of milliseconds; a multi-second ceiling is generous.
const SDO_TIMEOUT: Duration = Duration::from_secs(3);

// SDO sub-indices on object 0x8000.
const SDO_IDX: u16 = 0x8000;
const SUB_MV_V: u8 = 0x23;
const SUB_FULL_SCALE: u8 = 0x24;
const SUB_SCALE_FACTOR: u8 = 0x27;

#[derive(Debug, Clone, Copy, PartialEq, Eq)]
enum State {
    Idle,
    WritingMvV,
    WritingFullScale,
    WritingScaleFactor,
}

/// Function block for the Beckhoff EL3356 strain-gauge terminal.
pub struct El3356 {
    // Configuration
    sdo: SdoClient,

    // Outputs — read by the control program each tick
    /// Largest absolute load seen since construction, last reset, or last tare.
    pub peak_load: f32,
    /// True while a configuration sequence is in progress.
    pub busy: bool,
    /// True after an SDO operation failed. Clear with [`clear_error`](El3356::clear_error).
    pub error: bool,
    /// Last error message, if any.
    pub error_message: String,
    /// Last successfully written sensitivity (mV/V). `None` until a
    /// configure() has completed.
    pub configured_mv_v: Option<f32>,
    /// Last successfully written full-scale load. `None` until a configure()
    /// has completed.
    pub configured_full_scale_load: Option<f32>,
    /// Last successfully written scale factor. `None` until a configure()
    /// has completed.
    pub configured_scale_factor: Option<f32>,

    // Internal state
    state: State,
    pending_tid: Option<u32>,
    /// Values buffered for the current configure() sequence.
    pending_full_scale: f32,
    pending_mv_v: f32,
    pending_scale_factor: f32,
    /// When set, `tick()` will hold `view.tare = true` until this moment is
    /// reached, at which point it clears the tare bit.
    tare_release_at: Option<Instant>,
}

impl El3356 {
    /// Create a new EL3356 function block for the given EtherCAT device name.
    ///
    /// `device` must match the name used in `project.json`'s ethercat device
    /// list (it's the `device` field sent with every SDO request).
    pub fn new(device: &str) -> Self {
        Self {
            sdo: SdoClient::new(device),
            peak_load: 0.0,
            busy: false,
            error: false,
            error_message: String::new(),
            configured_mv_v: None,
            configured_full_scale_load: None,
            configured_scale_factor: None,
            state: State::Idle,
            pending_tid: None,
            pending_full_scale: 0.0,
            pending_mv_v: 0.0,
            pending_scale_factor: 0.0,
            tare_release_at: None,
        }
    }

    /// Call every control cycle.
    ///
    /// Performs three things, in order:
    /// 1. Updates `peak_load` from `*view.load`.
    /// 2. Releases the tare pulse after 100 ms.
    /// 3. Progresses any in-flight SDO operation from [`configure`](Self::configure).
    pub fn tick(&mut self, view: &mut El3356View, client: &mut CommandClient) {
        // 1. Peak tracking
        let abs_load = view.load.abs();
        if abs_load > self.peak_load.abs() {
            self.peak_load = *view.load;
        }

        // 2. Tare pulse release
        if let Some(release_at) = self.tare_release_at {
            if Instant::now() >= release_at {
                *view.tare = false;
                self.tare_release_at = None;
            } else {
                *view.tare = true;
            }
        }

        // 3. SDO sequence
        self.progress_sdo(client);
    }

    /// Begin an SDO configuration sequence: sensitivity (mV/V), full-scale
    /// load, and scale factor written to object `0x8000` subs `0x23`, `0x24`,
    /// and `0x27` respectively. Non-blocking — sets `busy = true` and returns
    /// immediately. Poll `busy` / `error` on subsequent ticks.
    ///
    /// No-op (logs a warning) if the FB is already `busy`. Any existing error
    /// flag is cleared at the start of a new sequence.
    pub fn configure(
        &mut self,
        client: &mut CommandClient,
        full_scale_load: f32,
        sensitivity_mv_v: f32,
        scale_factor: f32,
    ) {
        if self.busy {
            log::warn!("El3356::configure called while busy; request ignored");
            return;
        }
        self.error = false;
        self.error_message.clear();
        self.pending_full_scale = full_scale_load;
        self.pending_mv_v = sensitivity_mv_v;
        self.pending_scale_factor = scale_factor;

        // Start with mV/V (sub 0x23)
        let tid = self.sdo.write(client, SDO_IDX, SUB_MV_V, json!(sensitivity_mv_v));
        self.pending_tid = Some(tid);
        self.state = State::WritingMvV;
        self.busy = true;
    }

    /// Reset `peak_load` to `0.0`. Immediate; no IPC.
    pub fn reset_peak(&mut self) {
        self.peak_load = 0.0;
    }

    /// Pulse the tare bit high for 100 ms, and reset the peak.
    ///
    /// The device-side bit (`view.tare`) is actually written by [`tick`](Self::tick),
    /// so `tick` must be called every cycle. If `tare` is called while a
    /// previous pulse is still in progress, the 100 ms window restarts.
    pub fn tare(&mut self) {
        self.peak_load = 0.0;
        self.tare_release_at = Some(Instant::now() + TARE_PULSE);
    }

    /// Clear the error flag and message.
    pub fn clear_error(&mut self) {
        self.error = false;
        self.error_message.clear();
    }

    /// Low-level SDO write pass-through. Does **not** interact with the FB's
    /// `busy`/`state` fields — use for advanced operations (e.g. changing the
    /// filter mode at runtime) that don't fit the [`configure`](Self::configure)
    /// pattern.
    pub fn sdo_write(
        &mut self,
        client: &mut CommandClient,
        index: u16,
        sub_index: u8,
        value: serde_json::Value,
    ) -> u32 {
        self.sdo.write(client, index, sub_index, value)
    }

    // ──────────────────────────────────────────────────────────────
    // Internal helpers
    // ──────────────────────────────────────────────────────────────

    fn progress_sdo(&mut self, client: &mut CommandClient) {
        let tid = match self.pending_tid {
            Some(t) => t,
            None => return,
        };

        let result = self.sdo.result(client, tid, SDO_TIMEOUT);
        match result {
            SdoResult::Pending => {} // keep waiting
            SdoResult::Ok(_) => match self.state {
                State::WritingMvV => {
                    self.configured_mv_v = Some(self.pending_mv_v);
                    let next_tid = self.sdo.write(
                        client, SDO_IDX, SUB_FULL_SCALE, json!(self.pending_full_scale),
                    );
                    self.pending_tid = Some(next_tid);
                    self.state = State::WritingFullScale;
                }
                State::WritingFullScale => {
                    self.configured_full_scale_load = Some(self.pending_full_scale);
                    let next_tid = self.sdo.write(
                        client, SDO_IDX, SUB_SCALE_FACTOR, json!(self.pending_scale_factor),
                    );
                    self.pending_tid = Some(next_tid);
                    self.state = State::WritingScaleFactor;
                }
                State::WritingScaleFactor => {
                    self.configured_scale_factor = Some(self.pending_scale_factor);
                    self.pending_tid = None;
                    self.state = State::Idle;
                    self.busy = false;
                }
                State::Idle => {
                    // Response arrived but we're not in a sequence; discard.
                    self.pending_tid = None;
                }
            },
            SdoResult::Err(e) => {
                self.set_error(&format!("SDO {:?} failed: {}", self.state, e));
            }
            SdoResult::Timeout => {
                self.set_error(&format!("SDO {:?} timed out after {:?}", self.state, SDO_TIMEOUT));
            }
        }
    }

    fn set_error(&mut self, message: &str) {
        log::error!("El3356: {}", message);
        self.error = true;
        self.error_message = message.to_string();
        self.pending_tid = None;
        self.state = State::Idle;
        self.busy = false;
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    use mechutil::ipc::CommandMessage;
    use tokio::sync::mpsc;

    /// Local storage for PDO fields used in tests. Avoids depending on
    /// auto-generated GlobalMemory field names.
    #[derive(Default)]
    struct TestPdo {
        tare: bool,
        load: f32,
        load_steady: bool,
        load_error: bool,
        load_overrange: bool,
    }

    impl TestPdo {
        fn view(&mut self) -> El3356View<'_> {
            El3356View {
                tare:           &mut self.tare,
                load:           &self.load,
                load_steady:    &self.load_steady,
                load_error:     &self.load_error,
                load_overrange: &self.load_overrange,
            }
        }
    }

    fn test_client() -> (
        CommandClient,
        mpsc::UnboundedSender<CommandMessage>,
        mpsc::UnboundedReceiver<String>,
    ) {
        let (write_tx, write_rx) = mpsc::unbounded_channel();
        let (response_tx, response_rx) = mpsc::unbounded_channel();
        let client = CommandClient::new(write_tx, response_rx);
        (client, response_tx, write_rx)
    }

    /// Read the transaction_id from the most recently sent IPC message.
    fn last_sent_tid(rx: &mut mpsc::UnboundedReceiver<String>) -> u32 {
        let msg_json = rx.try_recv().expect("expected a message on the wire");
        let msg: CommandMessage = serde_json::from_str(&msg_json).unwrap();
        msg.transaction_id
    }

    fn assert_last_sent(
        rx: &mut mpsc::UnboundedReceiver<String>,
        expected_topic: &str,
        expected_sub: u8,
    ) -> u32 {
        let msg_json = rx.try_recv().expect("expected a message on the wire");
        let msg: CommandMessage = serde_json::from_str(&msg_json).unwrap();
        assert_eq!(msg.topic, expected_topic);
        assert_eq!(msg.data["index"], format!("0x{:04X}", SDO_IDX));
        assert_eq!(msg.data["sub"], expected_sub);
        msg.transaction_id
    }

    // ── peak tracking ──

    #[test]
    fn peak_follows_largest_magnitude() {
        let (mut client, _resp_tx, _write_rx) = test_client();
        let mut fb = El3356::new("EL3356_0");
        let mut pdo = TestPdo::default();

        // Positive then larger negative: peak should track absolute magnitude,
        // but store the signed value at the peak (matches TwinCAT code exactly).
        pdo.load = 10.0;
        fb.tick(&mut pdo.view(), &mut client);
        assert_eq!(fb.peak_load, 10.0);

        pdo.load = -25.0;
        fb.tick(&mut pdo.view(), &mut client);
        assert_eq!(fb.peak_load, -25.0);

        pdo.load = 20.0; // smaller than |-25|, shouldn't overwrite
        fb.tick(&mut pdo.view(), &mut client);
        assert_eq!(fb.peak_load, -25.0);
    }

    #[test]
    fn reset_peak_zeroes_it() {
        let (mut client, _resp_tx, _write_rx) = test_client();
        let mut fb = El3356::new("EL3356_0");
        let mut pdo = TestPdo { load: 42.0, ..Default::default() };
        fb.tick(&mut pdo.view(), &mut client);
        assert_eq!(fb.peak_load, 42.0);
        fb.reset_peak();
        assert_eq!(fb.peak_load, 0.0);
    }

    // ── tare pulse ──

    #[test]
    fn tare_resets_peak_and_pulses_bit() {
        let (mut client, _resp_tx, _write_rx) = test_client();
        let mut fb = El3356::new("EL3356_0");
        let mut pdo = TestPdo { load: 50.0, ..Default::default() };

        fb.tick(&mut pdo.view(), &mut client);
        assert_eq!(fb.peak_load, 50.0);

        fb.tare();
        // Tare itself sets peak=0 immediately
        assert_eq!(fb.peak_load, 0.0);

        // tick() writes the tare bit high while within the pulse window
        fb.tick(&mut pdo.view(), &mut client);
        assert!(pdo.tare, "tare bit should be high within pulse window");

        // Wait past the 100 ms window (small margin)
        std::thread::sleep(TARE_PULSE + Duration::from_millis(20));
        fb.tick(&mut pdo.view(), &mut client);
        assert!(!pdo.tare, "tare bit should be cleared after pulse window");
    }

    // ── configure state machine ──

    #[test]
    fn configure_sequences_three_sdo_writes() {
        let (mut client, resp_tx, mut write_rx) = test_client();
        let mut fb = El3356::new("EL3356_0");
        let mut pdo = TestPdo::default();

        fb.configure(&mut client, 1000.0, 2.0, 100000.0);
        assert!(fb.busy);
        assert_eq!(fb.state, State::WritingMvV);

        // 1st write: mV/V
        let tid1 = assert_last_sent(&mut write_rx, "ethercat.write_sdo", SUB_MV_V);
        resp_tx.send(CommandMessage::response(tid1, json!(null))).unwrap();
        client.poll();
        fb.tick(&mut pdo.view(), &mut client);
        assert_eq!(fb.configured_mv_v, Some(2.0));
        assert_eq!(fb.state, State::WritingFullScale);
        assert!(fb.busy);

        // 2nd write: full-scale
        let tid2 = assert_last_sent(&mut write_rx, "ethercat.write_sdo", SUB_FULL_SCALE);
        resp_tx.send(CommandMessage::response(tid2, json!(null))).unwrap();
        client.poll();
        fb.tick(&mut pdo.view(), &mut client);
        assert_eq!(fb.configured_full_scale_load, Some(1000.0));
        assert_eq!(fb.state, State::WritingScaleFactor);
        assert!(fb.busy);

        // 3rd write: scale factor
        let tid3 = assert_last_sent(&mut write_rx, "ethercat.write_sdo", SUB_SCALE_FACTOR);
        resp_tx.send(CommandMessage::response(tid3, json!(null))).unwrap();
        client.poll();
        fb.tick(&mut pdo.view(), &mut client);
        assert_eq!(fb.configured_scale_factor, Some(100000.0));
        assert_eq!(fb.state, State::Idle);
        assert!(!fb.busy);
        assert!(!fb.error);
    }

    #[test]
    fn configure_while_busy_is_noop() {
        let (mut client, _resp_tx, mut write_rx) = test_client();
        let mut fb = El3356::new("EL3356_0");

        fb.configure(&mut client, 1000.0, 2.0, 100000.0);
        let _tid1 = last_sent_tid(&mut write_rx);
        assert!(fb.busy);

        // Second call while busy: nothing on the wire, state unchanged.
        fb.configure(&mut client, 9999.0, 9.0, 99.0);
        assert!(write_rx.try_recv().is_err(), "no new message should have been sent");
        assert_eq!(fb.pending_mv_v, 2.0);
    }

    #[test]
    fn sdo_error_sets_error_and_clears_busy() {
        let (mut client, resp_tx, mut write_rx) = test_client();
        let mut fb = El3356::new("EL3356_0");
        let mut pdo = TestPdo::default();

        fb.configure(&mut client, 1000.0, 2.0, 100000.0);
        let tid1 = last_sent_tid(&mut write_rx);

        // Simulate error response
        let mut err_msg = CommandMessage::response(tid1, json!(null));
        err_msg.success = false;
        err_msg.error_message = "device offline".to_string();
        resp_tx.send(err_msg).unwrap();
        client.poll();

        fb.tick(&mut pdo.view(), &mut client);
        assert!(fb.error);
        assert!(fb.error_message.contains("device offline"));
        assert!(!fb.busy);
        assert_eq!(fb.state, State::Idle);
    }

    #[test]
    fn clear_error_resets_flag() {
        let mut fb = El3356::new("EL3356_0");
        fb.error = true;
        fb.error_message = "boom".to_string();
        fb.clear_error();
        assert!(!fb.error);
        assert!(fb.error_message.is_empty());
    }
}