Test-Jig-Software/src/fieldbus.rs

use std::sync::{Arc, Mutex};

use profirust::dp;
use profirust::fdl;
use profirust::phy;

pub use dp::OperatingState;

// Bus Parameters
const MASTER_ADDRESS: u8 = 3;
const BUS_DEVICE: &'static str = "/dev/ttyAMA0";
const BAUDRATE: profirust::Baudrate = profirust::Baudrate::B500000;

// Peripheral Addresses
const RIO_ADDRESS: u8 = 42;

pub const PIQ_SIZE: usize = 256;
pub const PII_SIZE: usize = 256;

#[derive(Debug)]
pub struct Fieldbus {
    inner: Arc<Mutex<FieldbusInner>>,
}

#[derive(Debug)]
struct FieldbusInner {
    state: OperatingState,
    is_online: bool,
    pii: [u8; PII_SIZE],
    piq: [u8; PIQ_SIZE],
}

impl Default for FieldbusInner {
    fn default() -> Self {
        Self {
            state: OperatingState::Stop,
            is_online: false,
            pii: [0u8; PII_SIZE],
            piq: [0u8; PIQ_SIZE],
        }
    }
}

impl Fieldbus {
    pub fn new() -> Self {
        let inner: Arc<Mutex<FieldbusInner>> = Default::default();
        std::thread::spawn({
            let inner = inner.clone();
            move || {
                fieldbus_task(inner);
            }
        });
        Self { inner }
    }

    pub fn enter_state(&mut self, state: OperatingState) {
        self.inner.lock().unwrap().state = state;
    }

    pub fn is_online(&self) -> bool {
        self.inner.lock().unwrap().is_online
    }

    pub fn update_process_images(&mut self, pii: &mut [u8; PII_SIZE], piq: &[u8; PIQ_SIZE]) {
        let mut data = self.inner.lock().unwrap();
        pii.copy_from_slice(&data.pii);
        data.piq.copy_from_slice(piq);
    }

    pub fn with_process_images<F, R>(&mut self, f: F) -> R
    where
        F: FnOnce(&[u8; PII_SIZE], &mut [u8; PIQ_SIZE]) -> R,
    {
        let mut data = self.inner.lock().unwrap();
        let data = &mut *data;
        f(&data.pii, &mut data.piq)
    }
}

struct PeripheralInfo {
    pub handle: dp::PeripheralHandle,
    pub pii_offset: usize,
    pub piq_offset: usize,
    pub liveness_bit: (usize, u8),
}

fn fieldbus_task(fieldbus_data: Arc<Mutex<FieldbusInner>>) {
    let mut dp_master = dp::DpMaster::new(vec![]);
    let mut peripherals: Vec<PeripheralInfo> = Default::default();

    // Options generated by `gsdtool` using "B751_P39.GSD"
    let options = profirust::dp::PeripheralOptions {
        // "WAGO 750-303 (FW: 01 ... 06) PRO" by "WAGO Kontakttechnik GmbH"
        ident_number: 0xb751,

        // Global Parameters:
        //   - Register-Interface.............: is not used
        //   - RESET on terminalbus failure...: POWER ON RESET
        //   - Terminalbus diagnostics........: disabled
        //   - Evaluation of Clear_Data.......: enabled
        //   - Diagnostics of binary modules..: is not mapped into PI
        //   - Extended PI-Update.............: according to PI-Update
        //   - Evaluation of complex modules..: process data only
        //   - Data format....................: MOTOROLA
        //   - Process image update...........: free running
        //   - Response to PROFIBUS failure...: Output image is cleared
        //   - Response to terminalbus failure: PROFIBUS communication stops
        //   - Maximum length of diagnostics..: 16 Byte
        //
        // Selected Modules:
        //   [0]   8 Bit binary inputs
        //   [1]   8 Bit binary outputs
        user_parameters: Some(&[
            0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x14, 0x00, 0x6b, 0x00, 0x10, 0x00, 0x0a,
            0x00,
        ]),
        config: Some(&[0x10, 0x20]),

        // Set max_tsdr depending on baudrate and assert
        // that a supported baudrate is used.
        max_tsdr: match BAUDRATE {
            profirust::Baudrate::B9600 => 60,
            profirust::Baudrate::B19200 => 60,
            profirust::Baudrate::B93750 => 60,
            profirust::Baudrate::B187500 => 60,
            profirust::Baudrate::B500000 => 100,
            profirust::Baudrate::B1500000 => 150,
            profirust::Baudrate::B3000000 => 250,
            profirust::Baudrate::B6000000 => 350,
            profirust::Baudrate::B12000000 => 550,
            b => panic!(
                "Peripheral \"WAGO 750-303 (FW: 01 ... 06) PRO\" does not support baudrate {b:?}!"
            ),
        },

        fail_safe: false,
        ..Default::default()
    };
    let mut buffer_inputs = [0u8; 1];
    let mut buffer_outputs = [0u8; 1];
    let mut buffer_diagnostics = [0u8; 64];

    let handle = dp_master.add(
        dp::Peripheral::new(
            RIO_ADDRESS,
            options,
            &mut buffer_inputs[..],
            &mut buffer_outputs[..],
        )
        .with_diag_buffer(&mut buffer_diagnostics[..]),
    );

    peripherals.push(PeripheralInfo {
        handle,
        pii_offset: 0,
        piq_offset: 0,
        liveness_bit: (64, 0),
    });

    let mut fdl = fdl::FdlActiveStation::new(
        fdl::ParametersBuilder::new(MASTER_ADDRESS, BAUDRATE)
            // We use a rather large T_slot time because USB-RS485 converters
            // can induce large delays at times.
            .slot_bits(2500)
            .watchdog_timeout(profirust::time::Duration::from_secs(2))
            .build_verified(&dp_master),
    );

    // We must not poll() too often or to little. T_slot / 2 seems to be a good compromise.
    let sleep_time: std::time::Duration = (fdl.parameters().slot_time() / 2).into();

    let mut phy = phy::LinuxRs485Phy::new(BUS_DEVICE, fdl.parameters().baudrate);

    fdl.set_online();

    loop {
        let now = profirust::time::Instant::now();
        fdl.poll(now, &mut phy, &mut dp_master);
        let events = dp_master.take_last_events();

        {
            let mut data = fieldbus_data.lock().unwrap();

            if dp_master.operating_state() != data.state {
                dp_master.enter_state(data.state);
            }

            data.is_online = fdl.is_in_ring();

            if events.cycle_completed {
                for peripheral_info in peripherals.iter() {
                    let peripheral = dp_master.get_mut(peripheral_info.handle);

                    {
                        let mut liveness_bit = process_image::tag_mut!(
                            &mut data.pii,
                            peripheral_info.liveness_bit.0,
                            peripheral_info.liveness_bit.1
                        );
                        *liveness_bit = peripheral.is_running();
                    }

                    if peripheral.is_running() {
                        {
                            let pii = peripheral.pi_i();
                            data.pii[peripheral_info.pii_offset..][..pii.len()]
                                .copy_from_slice(pii);
                        }
                        {
                            let piq = peripheral.pi_q_mut();
                            piq.copy_from_slice(
                                &data.piq[peripheral_info.piq_offset..][..piq.len()],
                            );
                        }
                    }
                }
            }
        }

        std::thread::sleep(sleep_time);
    }
}