From 19e9fbb14b2dc38d6e74e6775313735fe07d6b8e Mon Sep 17 00:00:00 2001 From: aaumar25 Date: Thu, 15 Oct 2026 17:39:22 +0900 Subject: [PATCH 1/7] feat: Add log functionality for CTO firmware Decided to only support two kind of command, because the rest of the command will not be useful for the user. --- src/_Log.zig | 92 +++++++++++++++++++++++++++++++++++++++++++++++ src/drivercom.zig | 1 + 2 files changed, 93 insertions(+) create mode 100644 src/_Log.zig diff --git a/src/_Log.zig b/src/_Log.zig new file mode 100644 index 0000000..65ccca8 --- /dev/null +++ b/src/_Log.zig @@ -0,0 +1,92 @@ +//! 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"); + +/// Logging duration (s). +duration: f32, +/// Items to be logged on every selected axis. +config: Config, +/// Axes to be logged. +axes: [3]bool, +/// ID of the slider targeted by the command. +slider_id: u8, +/// 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): + // current_state = 0, 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 + position_profile: Profile, + speed_profile: Profile, + + pub const Kind = enum(u8) { + position_profile = 13, + speed_profile = 14, + }; + + pub const Profile = struct { + // 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, +}; + +test { + std.testing.refAllDecls(@This()); +} diff --git a/src/drivercom.zig b/src/drivercom.zig index 9dbcf25..0c263e2 100644 --- a/src/drivercom.zig +++ b/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; From 0c07efe8c9810b0a9e76968c2d873883039ffea9 Mon Sep 17 00:00:00 2001 From: aaumar25 Date: Fri, 16 Oct 2026 09:24:26 +0900 Subject: [PATCH 2/7] refactor: Change the duration to `u32` --- src/_Log.zig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/_Log.zig b/src/_Log.zig index 65ccca8..08c32fc 100644 --- a/src/_Log.zig +++ b/src/_Log.zig @@ -4,7 +4,7 @@ const std = @import("std"); /// Logging duration (s). -duration: f32, +duration: u32, /// Items to be logged on every selected axis. config: Config, /// Axes to be logged. From c02b2cc9433a56c60d63e88058bdb62f778d8647 Mon Sep 17 00:00:00 2001 From: aaumar25 Date: Fri, 16 Oct 2026 09:24:48 +0900 Subject: [PATCH 3/7] refactor: Introduce `FREQUENCY` --- src/_Log.zig | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/_Log.zig b/src/_Log.zig index 08c32fc..da3e9b7 100644 --- a/src/_Log.zig +++ b/src/_Log.zig @@ -3,6 +3,9 @@ //! samples per second. const std = @import("std"); +/// Number of cycles per 1 second in the firmware. +pub const FREQUENCY: u32 = 15000; + /// Logging duration (s). duration: u32, /// Items to be logged on every selected axis. From 91a761808b4953eda8ebc5c68b8c1891700186f0 Mon Sep 17 00:00:00 2001 From: aaumar25 Date: Mon, 19 Oct 2026 15:41:52 +0900 Subject: [PATCH 4/7] feat: Add message definition of `start_log` --- src/_Message.zig | 136 +++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 136 insertions(+) diff --git a/src/_Message.zig b/src/_Message.zig index d70fe14..9dff007 100644 --- a/src/_Message.zig +++ b/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,106 @@ pub const Request = union(Kind) { _const_deno: u16 align(1) = 100, }; + pub const StartLog = extern struct { + pub fn fromLogConfig(log: Log) StartLog { + const profile: Log.Command.Profile = switch (log.command) { + .position_profile, .speed_profile => |value| value, + }; + // Same f32 math and truncation as SHDrv's `CalculateRspSize()`: + // header, then per logged item of each axis a 4-byte tag and + // `Log.FREQUENCY` f32 samples per second, then ETX and BCC. + const item_count = @popCount(@as(u32, @bitCast(log.config))); + const axis_count = std.mem.countScalar(bool, &log.axes, true); + const response_size: u32 = @sizeOf(Header) + + @sizeOf(Response.StartLog.Fragment) + + (@sizeOf(Response.StartLog.Block) + + @sizeOf(f32) * Log.FREQUENCY * log.duration) * + item_count * axis_count + + @sizeOf(@FieldType(Message, "etx")) + + @sizeOf(@FieldType(Message, "bcc")); + var result: StartLog = .{ + .config = undefined, + .command = log.command, + .slider_id = log.slider_id, + .position = profile.distance, + .profile_speed = profile.speed, + .profile_acceleration = profile.acceleration, + .response_size = @intFromFloat(response_size), + .duration = log.duration, + }; + 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, + /// TODO: Used by the offline commands. + voltage: f32 align(1) = 0, + slider_id: u8, + /// 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. + position: f32 align(1), + /// Percentage of the maximum speed of the profile commands. + profile_speed: f32 align(1), + /// Percentage of the maximum acceleration of the profile commands. + profile_acceleration: f32 align(1), + /// 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 +673,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 +1214,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 +1377,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, _, }; From 9f75aa1da575649cd96713e85863d15df3abb31f Mon Sep 17 00:00:00 2001 From: aaumar25 Date: Mon, 19 Oct 2026 16:15:45 +0900 Subject: [PATCH 5/7] fix: Logging duration is floating number --- src/_Log.zig | 4 ++-- src/_Message.zig | 18 ++++++++++-------- 2 files changed, 12 insertions(+), 10 deletions(-) diff --git a/src/_Log.zig b/src/_Log.zig index da3e9b7..b4a26d0 100644 --- a/src/_Log.zig +++ b/src/_Log.zig @@ -6,8 +6,8 @@ const std = @import("std"); /// Number of cycles per 1 second in the firmware. pub const FREQUENCY: u32 = 15000; -/// Logging duration (s). -duration: u32, +/// Logging duration (>0 s). +duration: f32, /// Items to be logged on every selected axis. config: Config, /// Axes to be logged. diff --git a/src/_Message.zig b/src/_Message.zig index 9dff007..91b3a6d 100644 --- a/src/_Message.zig +++ b/src/_Message.zig @@ -440,16 +440,18 @@ pub const Request = union(Kind) { const profile: Log.Command.Profile = switch (log.command) { .position_profile, .speed_profile => |value| value, }; - // Same f32 math and truncation as SHDrv's `CalculateRspSize()`: - // header, then per logged item of each axis a 4-byte tag and - // `Log.FREQUENCY` f32 samples per second, then ETX and BCC. - const item_count = @popCount(@as(u32, @bitCast(log.config))); - const axis_count = std.mem.countScalar(bool, &log.axes, true); + // 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) * Log.FREQUENCY * log.duration) * - item_count * axis_count + + @sizeOf(f32) * sample_count) * config_count * axis_count + @sizeOf(@FieldType(Message, "etx")) + @sizeOf(@FieldType(Message, "bcc")); var result: StartLog = .{ @@ -459,7 +461,7 @@ pub const Request = union(Kind) { .position = profile.distance, .profile_speed = profile.speed, .profile_acceleration = profile.acceleration, - .response_size = @intFromFloat(response_size), + .response_size = response_size, .duration = log.duration, }; for (&result.config, log.axes) |*axis_config, is_logged| { From 0e7af866caefce5d6746c69923e2aee89326baaf Mon Sep 17 00:00:00 2001 From: aaumar25 Date: Tue, 20 Oct 2026 13:21:16 +0900 Subject: [PATCH 6/7] feat: Add `current_state` command --- src/_Log.zig | 9 ++++++--- src/_Message.zig | 30 +++++++++++++++++------------- 2 files changed, 23 insertions(+), 16 deletions(-) diff --git a/src/_Log.zig b/src/_Log.zig index b4a26d0..01e24bb 100644 --- a/src/_Log.zig +++ b/src/_Log.zig @@ -12,28 +12,31 @@ duration: f32, config: Config, /// Axes to be logged. axes: [3]bool, -/// ID of the slider targeted by the command. -slider_id: u8, /// 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): - // current_state = 0, warm_up = 1, move_home = 2, move_forward = 3, + // 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, diff --git a/src/_Message.zig b/src/_Message.zig index 91b3a6d..d607ef7 100644 --- a/src/_Message.zig +++ b/src/_Message.zig @@ -437,9 +437,6 @@ pub const Request = union(Kind) { pub const StartLog = extern struct { pub fn fromLogConfig(log: Log) StartLog { - const profile: Log.Command.Profile = switch (log.command) { - .position_profile, .speed_profile => |value| value, - }; // 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)`. @@ -457,13 +454,18 @@ pub const Request = union(Kind) { var result: StartLog = .{ .config = undefined, .command = log.command, - .slider_id = log.slider_id, - .position = profile.distance, - .profile_speed = profile.speed, - .profile_acceleration = profile.acceleration, .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( @@ -518,19 +520,21 @@ pub const Request = union(Kind) { config: [3]AxisConfig, command: Log.Command.Kind, _reserved: u16 align(1) = 0, - /// TODO: Used by the offline commands. + /// Applied voltage (V). voltage: f32 align(1) = 0, - slider_id: u8, + /// 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. - position: f32 align(1), + /// 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), + profile_speed: f32 align(1) = 0, /// Percentage of the maximum acceleration of the profile commands. - profile_acceleration: f32 align(1), + profile_acceleration: f32 align(1) = 0, /// Size of the whole response in bytes. response_size: u32 align(1), /// Logging duration (s). From a8a25397af00f995ddba37c510b99b129cf4800f Mon Sep 17 00:00:00 2001 From: aaumar25 Date: Tue, 20 Oct 2026 14:45:33 +0900 Subject: [PATCH 7/7] feat: Add type for each config in log --- src/_Log.zig | 54 ++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 54 insertions(+) diff --git a/src/_Log.zig b/src/_Log.zig index 01e24bb..66465bc 100644 --- a/src/_Log.zig +++ b/src/_Log.zig @@ -2,6 +2,7 @@ //! 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; @@ -93,6 +94,59 @@ pub const Config = packed struct(u32) { 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()); }