Add log functionality for CTO's firmware

src/_Log.zig

@@ -0,0 +1,152 @@
+//! This module represents driver's logging configuration. The driver runs the
+//! command while recording the configured items of the selected axes at 15000
+//! samples per second.
+const std = @import("std");
+const _Config = @import("_Config.zig");
+
+/// Number of cycles per 1 second in the firmware.
+pub const FREQUENCY: u32 = 15000;
+
+/// Logging duration (>0 s).
+duration: f32,
+/// Items to be logged on every selected axis.
+config: Config,
+/// Axes to be logged.
+axes: [3]bool,
+/// Command run by the driver while logging.
+command: Command,
+
+pub const Command = union(Kind) {
+    // TODO: Support the remaining commands SHDrv provide:
+    // - Offline commands with applied voltage (V) and applied angle (turn):
+    //   warm_up = 1, move_home = 2, move_forward = 3,
+    //   move_backward = 4, restart = 5, move_synchronous = 6, move_dq = 7
+    // - current_step = 10 with Q current step (A)
+    // - speed_step = 11 with speed step (m/s)
+    // - position_step = 12 with position step
+    /// Offline command that logs the driver as it stands.
+    current_state: struct {},
+    position_profile: Profile,
+    speed_profile: Profile,
+
+    pub const Kind = enum(u8) {
+        current_state = 0,
+        position_profile = 13,
+        speed_profile = 14,
+    };
+
+    pub const Profile = struct {
+        /// ID of the slider targeted by the command (>0).
+        slider_id: u8,
+        // Unit is unconfirmed: SHDrv labels it m but limits it as rad.
+        /// Travel distance. Negative distance travels backward.
+        distance: f32,
+        /// Percentage of the maximum speed (%).
+        speed: f32,
+        /// Percentage of the maximum acceleration (%).
+        acceleration: f32,
+    };
+};
+
+/// Field order is the item index. The response tags the item with its index
+/// plus one.
+pub const Config = packed struct(u32) {
+    /// Output position of the profile.
+    profile_position: bool,
+    /// Applied speed of the profile.
+    profile_speed: bool,
+    /// Output of the position controller.
+    position_output: bool,
+    /// Reference of the position controller (rad).
+    position_reference: bool,
+    /// Feedback of the position controller (rad).
+    position_feedback: bool,
+    /// Reference of the speed controller (rad/s).
+    speed_reference: bool,
+    /// Feedback of the speed controller (rad/s).
+    speed_feedback: bool,
+    current_q_reference: bool,
+    current_q_feedback: bool,
+    current_d_feedback: bool,
+    voltage_q_reference: bool,
+    voltage_d_reference: bool,
+    voltage_q_feedforward: bool,
+    voltage_d_feedforward: bool,
+    /// Calculation time of the control loop.
+    calculation_time: bool,
+    theta_section: bool,
+    electrical_angular_speed: bool,
+    in_position: bool,
+    current_a: bool,
+    current_b: bool,
+    current_c: bool,
+    voltage_alpha: bool,
+    voltage_beta: bool,
+    /// State of the slider's FSM.
+    slider_state: bool,
+    applied_angle: bool,
+    encoder_angle: bool,
+    voltage_a: bool,
+    voltage_b: bool,
+    voltage_c: bool,
+    vector_sector: bool,
+    hall_alarm_left: bool,
+    hall_alarm_right: bool,
+};
+
+/// Logged item, valued by its index in `Config`.
+pub const Item = Item: {
+    const fields = @typeInfo(Config).@"struct".fields;
+    var names: [fields.len][]const u8 = undefined;
+    var values: [fields.len]u5 = undefined;
+    for (fields, &names, &values, 0..) |field, *name, *value, index| {
+        name.* = field.name;
+        value.* = index;
+    }
+    break :Item @Enum(u5, .exhaustive, &names, &values);
+};
+
+/// Value type of a logged item.
+pub fn ConfigType(comptime item: Item) type {
+    return switch (item) {
+        .profile_position,
+        .profile_speed,
+        .position_output,
+        .position_reference,
+        .position_feedback,
+        .speed_reference,
+        .speed_feedback,
+        .current_q_reference,
+        .current_q_feedback,
+        .current_d_feedback,
+        .voltage_q_reference,
+        .voltage_d_reference,
+        .voltage_q_feedforward,
+        .voltage_d_feedforward,
+        .calculation_time,
+        .electrical_angular_speed,
+        .current_a,
+        .current_b,
+        .current_c,
+        .voltage_alpha,
+        .voltage_beta,
+        .applied_angle,
+        .encoder_angle,
+        .voltage_a,
+        .voltage_b,
+        .voltage_c,
+        => f32,
+        .theta_section,
+        .vector_sector,
+        => i32,
+        .slider_state => _Config.Fsm,
+        .in_position,
+        .hall_alarm_left,
+        .hall_alarm_right,
+        => bool,
+    };
+}
+
+test {
+    std.testing.refAllDecls(@This());
+}

src/_Message.zig

@@ -3,9 +3,12 @@
 //! message from the firmware. Caller must call `deinit()` to free allocated
 //! payload memory.
 
+// TODO: Validate values processed in `fromConfig` and `fromLogConfig`
+
 const std = @import("std");
 const builtin = @import("builtin");
 const Config = @import("_Config.zig");
+const Log = @import("_Log.zig");
 const Message = @This();
 
 const STX = 0x02;
@@ -191,9 +194,11 @@ pub const Request = union(Kind) {
     set_gain_current: CurrentGain,
     set_gain_speed: SpeedGain,
     set_gain_position: PositionGain,
+    start_log: StartLog,
     set_angle_offset: AngleOffset,
     set_section_count: SectionCount,
     get_section_count: [0]u8,
+    ack_log_fragment: bool,
 
     pub const SystemConfig = extern struct {
         pub fn fromConfig(config: Config) SystemConfig {
@@ -430,6 +435,112 @@ pub const Request = union(Kind) {
         _const_deno: u16 align(1) = 100,
     };
 
+    pub const StartLog = extern struct {
+        pub fn fromLogConfig(log: Log) StartLog {
+            // Header, then per logged item of each axis a 4-byte tag and its
+            // f32 samples, then ETX and BCC. The sample count is truncated
+            // like SHDrv's `(int)(m_Sec * 15000)`.
+            const config_count = @popCount(@as(u32, @bitCast(log.config)));
+            const axis_count: u32 =
+                @intCast(std.mem.countScalar(bool, &log.axes, true));
+            const sample_count: u32 =
+                @trunc(@as(f32, @floatFromInt(Log.FREQUENCY)) * log.duration);
+            const response_size: u32 = @sizeOf(Header) +
+                @sizeOf(Response.StartLog.Fragment) +
+                (@sizeOf(Response.StartLog.Block) +
+                    @sizeOf(f32) * sample_count) * config_count * axis_count +
+                @sizeOf(@FieldType(Message, "etx")) +
+                @sizeOf(@FieldType(Message, "bcc"));
+            var result: StartLog = .{
+                .config = undefined,
+                .command = log.command,
+                .response_size = response_size,
+                .duration = log.duration,
+            };
+            switch (log.command) {
+                .current_state => {},
+                .position_profile, .speed_profile => |profile| {
+                    result.slider_id = profile.slider_id;
+                    result.position = profile.distance;
+                    result.profile_speed = profile.speed;
+                    result.profile_acceleration = profile.acceleration;
+                },
+            }
+            for (&result.config, log.axes) |*axis_config, is_logged| {
+                inline for (@typeInfo(Log.Config).@"struct".fields) |field| {
+                    @field(axis_config.*, field.name) = AxisConfig.fromBool(
+                        is_logged and @field(log.config, field.name),
+                    );
+                }
+            }
+            return result;
+        }
+
+        /// The firmware accepts ASCII of `1` and `0` as true or false.
+        pub const AxisConfig = extern struct {
+            pub fn fromBool(value: bool) u8 {
+                return if (value) '1' else '0';
+            }
+
+            profile_position: u8,
+            profile_speed: u8,
+            position_output: u8,
+            position_reference: u8,
+            position_feedback: u8,
+            speed_reference: u8,
+            speed_feedback: u8,
+            current_q_reference: u8,
+            current_q_feedback: u8,
+            current_d_feedback: u8,
+            voltage_q_reference: u8,
+            voltage_d_reference: u8,
+            voltage_q_feedforward: u8,
+            voltage_d_feedforward: u8,
+            calculation_time: u8,
+            theta_section: u8,
+            electrical_angular_speed: u8,
+            in_position: u8,
+            current_a: u8,
+            current_b: u8,
+            current_c: u8,
+            voltage_alpha: u8,
+            voltage_beta: u8,
+            slider_state: u8,
+            applied_angle: u8,
+            encoder_angle: u8,
+            voltage_a: u8,
+            voltage_b: u8,
+            voltage_c: u8,
+            vector_sector: u8,
+            hall_alarm_left: u8,
+            hall_alarm_right: u8,
+        };
+
+        /// Logged items of each axis.
+        config: [3]AxisConfig,
+        command: Log.Command.Kind,
+        _reserved: u16 align(1) = 0,
+        /// Applied voltage (V).
+        voltage: f32 align(1) = 0,
+        /// ID of the slider targeted by the command.
+        slider_id: u8 = 0,
+        /// TODO: Used by `current_step`.
+        current: f32 align(1) = 0,
+        /// TODO: Used by `speed_step`.
+        speed: f32 align(1) = 0,
+        /// Travel distance of the profile commands, or applied angle of the
+        /// offline commands (turn).
+        position: f32 align(1) = 0,
+        /// Percentage of the maximum speed of the profile commands.
+        profile_speed: f32 align(1) = 0,
+        /// Percentage of the maximum acceleration of the profile commands.
+        profile_acceleration: f32 align(1) = 0,
+        /// Size of the whole response in bytes.
+        response_size: u32 align(1),
+        /// Logging duration (s).
+        duration: f32 align(1),
+    };
+
     pub const AngleOffset = extern struct {
         pub fn fromConfig(config: Config) AngleOffset {
             var result: AngleOffset = undefined;
@@ -568,9 +679,12 @@ pub const Response = union(Kind) {
     set_gain_current: Ack,
     set_gain_speed: Ack,
     set_gain_position: Ack,
+    start_log: StartLog,
     set_angle_offset: Ack,
     set_section_count: Ack,
     get_section_count: SectionCount,
+    /// The driver replies with the next log fragment, a `start_log` response.
+    ack_log_fragment: [0]u8,
 
     pub const SystemConfig = extern struct {
         pub fn setConfig(self: SystemConfig, config: *Config) void {
@@ -1106,6 +1220,32 @@ pub const Response = union(Kind) {
         },
     };
 
+    /// Leading byte of every log reply frame. The rest of the frame payload
+    /// is a part of the log data. Joining the parts of every frame in order
+    /// gives the whole log data: one `Block` per logged item of each logged
+    /// axis.
+    pub const StartLog = extern struct {
+        fragment: Fragment,
+
+        pub const Fragment = enum(u8) {
+            /// The frame carries the whole log data.
+            whole = 0x00,
+            first = 0x10,
+            middle = 0x20,
+            last = 0x30,
+            _,
+        };
+
+        /// Start of one logged item of one axis, followed by
+        /// `Log.FREQUENCY` * `Log.duration` f32 samples.
+        pub const Block = extern struct {
+            /// ASCII of the axis number, '1' to '3'.
+            axis: u16 align(1),
+            /// Index of the item in `Log.Config` plus one.
+            item: u16 align(1),
+        };
+    };
+
     pub const SectionCount = extern struct {
         pub fn setConfig(self: SectionCount, config: *Config) void {
             const max = self.max_section_cnt;
@@ -1243,9 +1383,11 @@ pub const Kind = enum(u8) {
     set_gain_current,
     set_gain_speed,
     set_gain_position,
+    start_log,
     set_angle_offset = 0x11,
     set_section_count = 0x15,
     get_section_count,
+    ack_log_fragment = 0x1C,
     _,
 };
 

src/drivercom.zig

@@ -7,6 +7,7 @@ pub const version =
 pub const Config = @import("Config.zig");
 pub const _Config = @import("_Config.zig");
 pub const _Message = @import("_Message.zig");
+pub const _Log = @import("_Log.zig");
 pub const OldConfig = @import("OldConfig.zig");
 pub const Log = @import("Log.zig");
 pub const Message = @import("message.zig").Message;