From 38c668a2ee5800784a9406f7e4a26c03735467ff Mon Sep 17 00:00:00 2001 From: Joe Spall Date: Mon, 5 May 2025 20:37:21 -0700 Subject: [PATCH 01/44] WIP Comments and new fields --- .../include/control_debug_telemetry.h | 6 +++--- ateam-common-packets/include/radio.h | 14 +++++++++----- ateam-common-packets/include/stspin.h | 9 +++++---- ateam-common-packets/rust-lib/src/radio.rs | 1 + 4 files changed, 18 insertions(+), 12 deletions(-) diff --git a/ateam-common-packets/include/control_debug_telemetry.h b/ateam-common-packets/include/control_debug_telemetry.h index b754662..82e574c 100644 --- a/ateam-common-packets/include/control_debug_telemetry.h +++ b/ateam-common-packets/include/control_debug_telemetry.h @@ -3,7 +3,7 @@ * @author Will Stuckey * @brief definition for controls debug telemetry data type * @version 0.1 - * + * * @copyright Copyright (c) 2023 * */ @@ -19,7 +19,7 @@ typedef struct ControlDebugTelemetry { MotorResponse_Motion_Packet motor_bl; MotorResponse_Motion_Packet motor_br; MotorResponse_Motion_Packet motor_fr; - /// 48 bytes + /// 52*4 = 208 bytes float imu_gyro[3]; // rad/s float imu_accel[3]; // m/s^2 @@ -35,4 +35,4 @@ typedef struct ControlDebugTelemetry { float wheel_velocity_clamped_u[4]; // wheel velocities after control policy clamped for local acceleration limits /// 32 bytes } ControlDebugTelemetry; -assert_size(ControlDebugTelemetry, 192 + 24 + 48 + 32); +assert_size(ControlDebugTelemetry, (52*4) + 24 + 48 + 32); diff --git a/ateam-common-packets/include/radio.h b/ateam-common-packets/include/radio.h index ec36dbf..84e5239 100644 --- a/ateam-common-packets/include/radio.h +++ b/ateam-common-packets/include/radio.h @@ -3,7 +3,7 @@ * @author Austin Jones * @brief communication packet definitions for the radio firmware * @version 0.1 - * + * * @copyright Copyright (c) 2022 * */ @@ -21,14 +21,17 @@ const uint16_t kProtocolVersionMajor = 0; const uint16_t kProtocolVersionMinor = 1; typedef enum CommandCode : uint8_t { + // Bidirectional commands CC_ACK = 1, CC_NACK = 2, CC_GOODBYE = 3, CC_KEEPALIVE = 4, + // From robot to coach CC_HELLO_REQ = 101, CC_TELEMETRY = 102, CC_CONTROL_DEBUG_TELEMETRY = 103, CC_ROBOT_PARAMETER_COMMAND = 104, + // From coach to robot CC_CONTROL = 201, CC_HELLO_RESP = 202, } CommandCode; @@ -40,7 +43,8 @@ typedef struct RadioHeader { uint16_t minor_version; CommandCode command_code; uint16_t data_length; -} RadioHeader; + uint8_t reserved; +} __attribute__((packed)) RadioHeader; assert_size(RadioHeader, 12); typedef union RadioData { @@ -51,8 +55,9 @@ typedef union RadioData { ControlDebugTelemetry control_debug_telemetry; ParameterCommand robot_parameter_command; } RadioData; -assert_size(RadioData, 296); +assert_size(RadioData, 312); +// Same RadioPacket typedef struct RadioPacket { uint32_t crc32; uint16_t major_version; @@ -74,5 +79,4 @@ typedef struct RadioPacket { // I think this should be a valid swap when we clean packet definitions in the future // RadioData data __attribute__((aligned (4))); } RadioPacket; -assert_size(RadioPacket, 308); -static_assert(sizeof(RadioPacket) <= 320); // 256 is the current size limit of an entry in the packet buffer +assert_size(RadioPacket, 324); \ No newline at end of file diff --git a/ateam-common-packets/include/stspin.h b/ateam-common-packets/include/stspin.h index 8d614ab..f74c34c 100644 --- a/ateam-common-packets/include/stspin.h +++ b/ateam-common-packets/include/stspin.h @@ -113,9 +113,9 @@ typedef struct MotorResponse_Params_Packet { uint16_t cur_clamp; uint16_t git_dirty: 1; uint16_t reserved: 15; - uint32_t reserved2; + uint32_t reserved2[2]; } __attribute__((packed)) MotorResponse_Params_Packet; -assert_size(MotorResponse_Params_Packet, 48); // Note: Same length as MotorResponse_Params_Packet +assert_size(MotorResponse_Params_Packet, 52); // Note: Same length as MotorResponse_Params_Packet typedef struct MotorResponse_Motion_Packet { uint32_t master_error : 1; @@ -150,8 +150,9 @@ typedef struct MotorResponse_Motion_Packet { float torque_estimate; float torque_computed_error; float torque_computed_setpoint; + float vbus_voltage; } __attribute__((packed)) MotorResponse_Motion_Packet; -assert_size(MotorResponse_Motion_Packet, 48); +assert_size(MotorResponse_Motion_Packet, 52); typedef struct MotorResponsePacket { MotorResponsePacketType type; @@ -162,4 +163,4 @@ typedef struct MotorResponsePacket { MotorResponse_Motion_Packet motion; } data; } __attribute__((packed)) MotorResponsePacket; -assert_size(MotorResponsePacket, 56); +assert_size(MotorResponsePacket, 60); diff --git a/ateam-common-packets/rust-lib/src/radio.rs b/ateam-common-packets/rust-lib/src/radio.rs index c0925a1..3e1bcc6 100644 --- a/ateam-common-packets/rust-lib/src/radio.rs +++ b/ateam-common-packets/rust-lib/src/radio.rs @@ -28,5 +28,6 @@ pub fn get_motor_response_motion_packet() -> MotorResponse_Motion_Packet { torque_estimate: 0.0, torque_computed_error: 0.0, torque_computed_setpoint: 0.0, + vbus_voltage: 0.0, } } \ No newline at end of file From f420a7d6f523762dbe393bd0b2fc3f8ad799c3e6 Mon Sep 17 00:00:00 2001 From: Joe Spall Date: Fri, 30 May 2025 09:08:32 -0700 Subject: [PATCH 02/44] Updated for current control --- .../include/control_debug_telemetry.h | 4 ++-- ateam-common-packets/include/radio.h | 4 ++-- ateam-common-packets/include/stspin.h | 15 ++++++++------- ateam-common-packets/rust-lib/src/radio.rs | 7 ++++--- 4 files changed, 16 insertions(+), 14 deletions(-) diff --git a/ateam-common-packets/include/control_debug_telemetry.h b/ateam-common-packets/include/control_debug_telemetry.h index 82e574c..cc55da9 100644 --- a/ateam-common-packets/include/control_debug_telemetry.h +++ b/ateam-common-packets/include/control_debug_telemetry.h @@ -19,7 +19,7 @@ typedef struct ControlDebugTelemetry { MotorResponse_Motion_Packet motor_bl; MotorResponse_Motion_Packet motor_br; MotorResponse_Motion_Packet motor_fr; - /// 52*4 = 208 bytes + /// 56*4 = 224 bytes float imu_gyro[3]; // rad/s float imu_accel[3]; // m/s^2 @@ -35,4 +35,4 @@ typedef struct ControlDebugTelemetry { float wheel_velocity_clamped_u[4]; // wheel velocities after control policy clamped for local acceleration limits /// 32 bytes } ControlDebugTelemetry; -assert_size(ControlDebugTelemetry, (52*4) + 24 + 48 + 32); +assert_size(ControlDebugTelemetry, (56*4) + 24 + 48 + 32); diff --git a/ateam-common-packets/include/radio.h b/ateam-common-packets/include/radio.h index 84e5239..e964a12 100644 --- a/ateam-common-packets/include/radio.h +++ b/ateam-common-packets/include/radio.h @@ -55,7 +55,7 @@ typedef union RadioData { ControlDebugTelemetry control_debug_telemetry; ParameterCommand robot_parameter_command; } RadioData; -assert_size(RadioData, 312); +assert_size(RadioData, 328); // Same RadioPacket typedef struct RadioPacket { @@ -79,4 +79,4 @@ typedef struct RadioPacket { // I think this should be a valid swap when we clean packet definitions in the future // RadioData data __attribute__((aligned (4))); } RadioPacket; -assert_size(RadioPacket, 324); \ No newline at end of file +assert_size(RadioPacket, 340); \ No newline at end of file diff --git a/ateam-common-packets/include/stspin.h b/ateam-common-packets/include/stspin.h index f74c34c..1f7819e 100644 --- a/ateam-common-packets/include/stspin.h +++ b/ateam-common-packets/include/stspin.h @@ -113,9 +113,9 @@ typedef struct MotorResponse_Params_Packet { uint16_t cur_clamp; uint16_t git_dirty: 1; uint16_t reserved: 15; - uint32_t reserved2[2]; + uint32_t reserved2[3]; } __attribute__((packed)) MotorResponse_Params_Packet; -assert_size(MotorResponse_Params_Packet, 52); // Note: Same length as MotorResponse_Params_Packet +assert_size(MotorResponse_Params_Packet, 56); // Note: Same length as MotorResponse_Params_Packet typedef struct MotorResponse_Motion_Packet { uint32_t master_error : 1; @@ -139,20 +139,21 @@ typedef struct MotorResponse_Motion_Packet { uint32_t reserved : 14; float vel_setpoint; - float vel_setpoint_clamped; + float vel_computed_rads; int32_t encoder_delta; float vel_enc_estimate; float vel_computed_error; - float vel_computed_setpoint; + float vel_computed_duty; float torque_setpoint; float current_estimate; float torque_estimate; float torque_computed_error; - float torque_computed_setpoint; + float torque_computed_nm; + float torque_computed_duty; float vbus_voltage; } __attribute__((packed)) MotorResponse_Motion_Packet; -assert_size(MotorResponse_Motion_Packet, 52); +assert_size(MotorResponse_Motion_Packet, 56); typedef struct MotorResponsePacket { MotorResponsePacketType type; @@ -163,4 +164,4 @@ typedef struct MotorResponsePacket { MotorResponse_Motion_Packet motion; } data; } __attribute__((packed)) MotorResponsePacket; -assert_size(MotorResponsePacket, 60); +assert_size(MotorResponsePacket, 64); diff --git a/ateam-common-packets/rust-lib/src/radio.rs b/ateam-common-packets/rust-lib/src/radio.rs index 3e1bcc6..ea7adee 100644 --- a/ateam-common-packets/rust-lib/src/radio.rs +++ b/ateam-common-packets/rust-lib/src/radio.rs @@ -18,16 +18,17 @@ pub fn get_motor_response_motion_packet() -> MotorResponse_Motion_Packet { _bitfield_align_1: [], _bitfield_1: MotorResponse_Motion_Packet::new_bitfield_1(1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 0, 0, 0, 0, 0, 0), vel_setpoint: 0.0, - vel_setpoint_clamped: 0.0, + vel_computed_rads: 0.0, encoder_delta: 0, vel_enc_estimate: 0.0, vel_computed_error: 0.0, - vel_computed_setpoint: 0.0, + vel_computed_duty: 0.0, torque_setpoint: 0.0, current_estimate: 0.0, torque_estimate: 0.0, torque_computed_error: 0.0, - torque_computed_setpoint: 0.0, + torque_computed_nm: 0.0, + torque_computed_duty: 0.0, vbus_voltage: 0.0, } } \ No newline at end of file From b96ba164cac4470f0847fd02c6990fb432801bb4 Mon Sep 17 00:00:00 2001 From: Joe Spall Date: Tue, 3 Jun 2025 22:26:21 -0700 Subject: [PATCH 03/44] Rename both torque --- ateam-common-packets/include/stspin.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ateam-common-packets/include/stspin.h b/ateam-common-packets/include/stspin.h index 1f7819e..8119604 100644 --- a/ateam-common-packets/include/stspin.h +++ b/ateam-common-packets/include/stspin.h @@ -57,7 +57,7 @@ typedef enum MotorCommand_MotionType { OPEN_LOOP = 0, VELOCITY = 1, TORQUE = 2, - BOTH = 3 + VELOCITY_W_TORQUE = 3 } __attribute__((packed)) MotorCommand_MotionType; assert_size(MotorCommandPacketType, 1); From 3e6e6e59ae8485448e2c44c3ef500fdf75a4c7d8 Mon Sep 17 00:00:00 2001 From: Joe Spall Date: Wed, 18 Jun 2025 18:43:42 -0700 Subject: [PATCH 04/44] Update timestamp, add enable motion --- ateam-common-packets/include/stspin.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ateam-common-packets/include/stspin.h b/ateam-common-packets/include/stspin.h index 8119604..4d03897 100644 --- a/ateam-common-packets/include/stspin.h +++ b/ateam-common-packets/include/stspin.h @@ -64,7 +64,8 @@ assert_size(MotorCommandPacketType, 1); typedef struct MotorCommand_Motion_Packet { uint32_t reset : 1; uint32_t enable_telemetry: 1; - uint32_t reserved : 30; + uint32_t enable_motion: 1; + uint32_t reserved : 29; MotorCommand_MotionType motion_control_type; uint8_t reserved2[3]; @@ -99,7 +100,6 @@ typedef struct MotorResponse_Params_Packet { uint8_t version_major; uint8_t version_minor; uint16_t version_patch; - uint32_t timestamp; float vel_p; float vel_i; @@ -113,7 +113,7 @@ typedef struct MotorResponse_Params_Packet { uint16_t cur_clamp; uint16_t git_dirty: 1; uint16_t reserved: 15; - uint32_t reserved2[3]; + uint32_t reserved2[4]; } __attribute__((packed)) MotorResponse_Params_Packet; assert_size(MotorResponse_Params_Packet, 56); // Note: Same length as MotorResponse_Params_Packet @@ -158,7 +158,7 @@ assert_size(MotorResponse_Motion_Packet, 56); typedef struct MotorResponsePacket { MotorResponsePacketType type; uint8_t reserved[3]; - uint32_t crc; + uint32_t timestamp; union ResponseData { MotorResponse_Params_Packet params; MotorResponse_Motion_Packet motion; From a0d6ed2e73da3d641fa9cd84aaef8b31c05de0bc Mon Sep 17 00:00:00 2001 From: Nicholas Witten Date: Sat, 5 Jul 2025 10:54:12 -0700 Subject: [PATCH 05/44] add image hash fields --- ateam-common-packets/include/extended_telemetry.h | 4 ++-- ateam-common-packets/include/kicker.h | 4 +++- ateam-common-packets/include/radio.h | 4 ++-- ateam-common-packets/include/stspin.h | 2 +- 4 files changed, 8 insertions(+), 6 deletions(-) diff --git a/ateam-common-packets/include/extended_telemetry.h b/ateam-common-packets/include/extended_telemetry.h index effb9ac..32d5c6a 100644 --- a/ateam-common-packets/include/extended_telemetry.h +++ b/ateam-common-packets/include/extended_telemetry.h @@ -27,7 +27,7 @@ typedef struct ExtendedTelemetry { // 48 bytes each KickerTelemetry kicker_status; - // 60 bytes + // 64 bytes float imu_gyro[3]; // rad/s float imu_accel[3]; // m/s^2 @@ -43,4 +43,4 @@ typedef struct ExtendedTelemetry { float wheel_velocity_clamped_u[4]; // wheel velocities after control policy clamped for local acceleration limits /// 32 bytes } ExtendedTelemetry; -assert_size(ExtendedTelemetry, 28 + 192 + 60 + 24 + 48 + 32); +assert_size(ExtendedTelemetry, 28 + 192 + 64 + 24 + 48 + 32); diff --git a/ateam-common-packets/include/kicker.h b/ateam-common-packets/include/kicker.h index 0e58bdb..85acbb7 100644 --- a/ateam-common-packets/include/kicker.h +++ b/ateam-common-packets/include/kicker.h @@ -39,6 +39,8 @@ typedef struct KickerTelemetry { float rail_voltage; float battery_voltage; + unsigned char kicker_image_hash[4]; + MotorTelemetry dribbler_motor; } KickerTelemetry; -assert_size(KickerTelemetry, 60); \ No newline at end of file +assert_size(KickerTelemetry, 64); \ No newline at end of file diff --git a/ateam-common-packets/include/radio.h b/ateam-common-packets/include/radio.h index b5e7e48..f03b802 100644 --- a/ateam-common-packets/include/radio.h +++ b/ateam-common-packets/include/radio.h @@ -52,7 +52,7 @@ typedef union RadioData { ExtendedTelemetry extended_telemetry; ParameterCommand robot_parameter_command; } RadioData; -assert_size(RadioData, 384); +assert_size(RadioData, 388); typedef struct RadioPacket { uint32_t crc32; @@ -75,5 +75,5 @@ typedef struct RadioPacket { // I think this should be a valid swap when we clean packet definitions in the future // RadioData data __attribute__((aligned (4))); } RadioPacket; -assert_size(RadioPacket, 396); +assert_size(RadioPacket, 400); static_assert(sizeof(RadioPacket) <= 448); // 512 is the current size limit of an entry in the packet buffer diff --git a/ateam-common-packets/include/stspin.h b/ateam-common-packets/include/stspin.h index 4d00026..9a79e87 100644 --- a/ateam-common-packets/include/stspin.h +++ b/ateam-common-packets/include/stspin.h @@ -113,7 +113,7 @@ typedef struct ParameterMotorResponse { uint16_t cur_clamp; uint16_t _reserved; - unsigned char wheel_img_hash[4]; + unsigned char firmware_img_hash[4]; } __attribute__((packed)) ParameterMotorResponse; assert_size(ParameterMotorResponse, 48); // Note: Same length as MotorResponse_Params_Packet From 2ff637e5218c5e783d2a62302f1f614e13c19647 Mon Sep 17 00:00:00 2001 From: Joe Spall Date: Sat, 5 Jul 2025 19:13:10 -0700 Subject: [PATCH 06/44] Add calibrate current command --- ateam-common-packets/include/stspin.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ateam-common-packets/include/stspin.h b/ateam-common-packets/include/stspin.h index d4ec733..31b1ab9 100644 --- a/ateam-common-packets/include/stspin.h +++ b/ateam-common-packets/include/stspin.h @@ -65,7 +65,8 @@ typedef struct MotionMotorCommand { uint32_t reset : 1; uint32_t enable_telemetry: 1; uint32_t enable_motion: 1; - uint32_t _reserved : 29; + uint32_t calibrate_current: 1; + uint32_t _reserved : 28; MotionCommandType motion_control_type; uint8_t _reserved_2[3]; From 1432a64cf3cfa7e12266f780c1742d0000843f7c Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Wed, 9 Jul 2025 14:49:40 -0400 Subject: [PATCH 07/44] splits sequence into control and radio seq --- ateam-common-packets/include/basic_telemetry.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ateam-common-packets/include/basic_telemetry.h b/ateam-common-packets/include/basic_telemetry.h index 68a3b10..92e7d50 100644 --- a/ateam-common-packets/include/basic_telemetry.h +++ b/ateam-common-packets/include/basic_telemetry.h @@ -13,7 +13,8 @@ #include "common.h" typedef struct BasicTelemetry { - uint16_t sequence_number; + uint8_t transmission_sequence_number; + uint8_t control_data_sequence_number; uint8_t robot_revision_major; uint8_t robot_revision_minor; From c24fd92f12cb9570acfcca8c2c4cd78bc0e6afb4 Mon Sep 17 00:00:00 2001 From: Nicholas Witten Date: Wed, 16 Jul 2025 13:24:06 -0300 Subject: [PATCH 08/44] add ErrorTelemetry packet --- .../include/error_telemetry.h | 20 +++++++++++++++++++ ateam-common-packets/include/radio.h | 3 +++ ateam-common-packets/rust-lib/src/radio.rs | 9 +++++++++ 3 files changed, 32 insertions(+) create mode 100644 ateam-common-packets/include/error_telemetry.h diff --git a/ateam-common-packets/include/error_telemetry.h b/ateam-common-packets/include/error_telemetry.h new file mode 100644 index 0000000..38591e0 --- /dev/null +++ b/ateam-common-packets/include/error_telemetry.h @@ -0,0 +1,20 @@ +/** + * @file error_telemetry.h + * @author Nicholas Witten / Joseph Spall + * @brief Simple error messages over radio + * @version 0.1 + * + * @copyright Copyright (c) 2022 + * + */ + +#pragma once + +#include "common.h" + +typedef struct ErrorTelemetry { + uint32_t timestamp; + unsigned char error_message[60]; +} ErrorTelemetry; +assert_size(ErrorTelemetry, 64); + diff --git a/ateam-common-packets/include/radio.h b/ateam-common-packets/include/radio.h index f03b802..aac1791 100644 --- a/ateam-common-packets/include/radio.h +++ b/ateam-common-packets/include/radio.h @@ -14,6 +14,7 @@ #include "hello_data.h" #include "basic_control.h" #include "basic_telemetry.h" +#include "error_telemetry.h" #include "extended_telemetry.h" #include "power.h" #include "robot_parameters.h" @@ -30,6 +31,7 @@ typedef enum CommandCode : uint8_t { CC_TELEMETRY = 102, CC_CONTROL_DEBUG_TELEMETRY = 103, CC_ROBOT_PARAMETER_COMMAND = 104, + CC_ERROR_TELEMETRY = 105, CC_CONTROL = 201, CC_HELLO_RESP = 202, } CommandCode; @@ -69,6 +71,7 @@ typedef struct RadioPacket { BasicControl control; BasicTelemetry telemetry; ExtendedTelemetry control_debug_telemetry; + ErrorTelemetry error_telemetry; ParameterCommand robot_parameter_command; } data __attribute__((aligned (4))); diff --git a/ateam-common-packets/rust-lib/src/radio.rs b/ateam-common-packets/rust-lib/src/radio.rs index 97a7738..cfa80c0 100644 --- a/ateam-common-packets/rust-lib/src/radio.rs +++ b/ateam-common-packets/rust-lib/src/radio.rs @@ -11,4 +11,13 @@ pub enum TelemetryPacket { Basic(crate::bindings::BasicTelemetry), Extended(crate::bindings::ExtendedTelemetry), ParameterCommandResponse(crate::bindings::ParameterCommand), + ErrorTelemetry(crate::bindings::ErrorTelemetry), +} + +#[derive(Copy, Clone)] +pub enum ParameterType { + Basic(crate::bindings::BasicTelemetry), + Extended(crate::bindings::ExtendedTelemetry), + ParameterCommandResponse(crate::bindings::ParameterCommand), + ErrorTelemetry(crate::bindings::ErrorTelemetry), } \ No newline at end of file From 0fc77eee7b20b4113e452816be8dd7bc42609f21 Mon Sep 17 00:00:00 2001 From: Nicholas Witten Date: Wed, 16 Jul 2025 21:36:08 -0300 Subject: [PATCH 09/44] add dribbler_multiplier --- ateam-common-packets/include/basic_control.h | 3 ++- ateam-common-packets/include/kicker.h | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/ateam-common-packets/include/basic_control.h b/ateam-common-packets/include/basic_control.h index df7626f..f51bd26 100644 --- a/ateam-common-packets/include/basic_control.h +++ b/ateam-common-packets/include/basic_control.h @@ -21,7 +21,8 @@ typedef struct BasicControl { uint32_t body_vel_controls_enabled : 1; uint32_t wheel_vel_control_enabled : 1; uint32_t wheel_torque_control_enabled : 1; - uint32_t _reserved : 17; + uint32_t dribbler_multiplier : 8; + uint32_t _reserved : 9; uint32_t play_song : 8; float vel_x_linear; // m/s diff --git a/ateam-common-packets/include/kicker.h b/ateam-common-packets/include/kicker.h index 85acbb7..9583b04 100644 --- a/ateam-common-packets/include/kicker.h +++ b/ateam-common-packets/include/kicker.h @@ -18,7 +18,8 @@ typedef struct KickerControl { uint32_t telemetry_enabled: 1; uint32_t enable_charging: 1; uint32_t request_power_down: 1; - // 29 bits reserved + uint32_t dribbler_mult: 8; + // 21 bits reserved KickRequest kick_request; float kick_speed; From 89128bb0b3e57a73bd6cf14399b505fccca88e35 Mon Sep 17 00:00:00 2001 From: Joe Spall Date: Fri, 18 Jul 2025 13:56:11 -0400 Subject: [PATCH 10/44] Dev/nick/dribbler mult control (#27) * add image hash fields * add ErrorTelemetry packet * add dribbler_multiplier --------- Co-authored-by: Nicholas Witten --- ateam-common-packets/include/basic_control.h | 3 ++- .../include/error_telemetry.h | 20 +++++++++++++++++++ ateam-common-packets/include/kicker.h | 3 ++- ateam-common-packets/include/radio.h | 3 +++ ateam-common-packets/rust-lib/src/radio.rs | 9 +++++++++ 5 files changed, 36 insertions(+), 2 deletions(-) create mode 100644 ateam-common-packets/include/error_telemetry.h diff --git a/ateam-common-packets/include/basic_control.h b/ateam-common-packets/include/basic_control.h index df7626f..f51bd26 100644 --- a/ateam-common-packets/include/basic_control.h +++ b/ateam-common-packets/include/basic_control.h @@ -21,7 +21,8 @@ typedef struct BasicControl { uint32_t body_vel_controls_enabled : 1; uint32_t wheel_vel_control_enabled : 1; uint32_t wheel_torque_control_enabled : 1; - uint32_t _reserved : 17; + uint32_t dribbler_multiplier : 8; + uint32_t _reserved : 9; uint32_t play_song : 8; float vel_x_linear; // m/s diff --git a/ateam-common-packets/include/error_telemetry.h b/ateam-common-packets/include/error_telemetry.h new file mode 100644 index 0000000..38591e0 --- /dev/null +++ b/ateam-common-packets/include/error_telemetry.h @@ -0,0 +1,20 @@ +/** + * @file error_telemetry.h + * @author Nicholas Witten / Joseph Spall + * @brief Simple error messages over radio + * @version 0.1 + * + * @copyright Copyright (c) 2022 + * + */ + +#pragma once + +#include "common.h" + +typedef struct ErrorTelemetry { + uint32_t timestamp; + unsigned char error_message[60]; +} ErrorTelemetry; +assert_size(ErrorTelemetry, 64); + diff --git a/ateam-common-packets/include/kicker.h b/ateam-common-packets/include/kicker.h index 85acbb7..9583b04 100644 --- a/ateam-common-packets/include/kicker.h +++ b/ateam-common-packets/include/kicker.h @@ -18,7 +18,8 @@ typedef struct KickerControl { uint32_t telemetry_enabled: 1; uint32_t enable_charging: 1; uint32_t request_power_down: 1; - // 29 bits reserved + uint32_t dribbler_mult: 8; + // 21 bits reserved KickRequest kick_request; float kick_speed; diff --git a/ateam-common-packets/include/radio.h b/ateam-common-packets/include/radio.h index f03b802..aac1791 100644 --- a/ateam-common-packets/include/radio.h +++ b/ateam-common-packets/include/radio.h @@ -14,6 +14,7 @@ #include "hello_data.h" #include "basic_control.h" #include "basic_telemetry.h" +#include "error_telemetry.h" #include "extended_telemetry.h" #include "power.h" #include "robot_parameters.h" @@ -30,6 +31,7 @@ typedef enum CommandCode : uint8_t { CC_TELEMETRY = 102, CC_CONTROL_DEBUG_TELEMETRY = 103, CC_ROBOT_PARAMETER_COMMAND = 104, + CC_ERROR_TELEMETRY = 105, CC_CONTROL = 201, CC_HELLO_RESP = 202, } CommandCode; @@ -69,6 +71,7 @@ typedef struct RadioPacket { BasicControl control; BasicTelemetry telemetry; ExtendedTelemetry control_debug_telemetry; + ErrorTelemetry error_telemetry; ParameterCommand robot_parameter_command; } data __attribute__((aligned (4))); diff --git a/ateam-common-packets/rust-lib/src/radio.rs b/ateam-common-packets/rust-lib/src/radio.rs index 97a7738..cfa80c0 100644 --- a/ateam-common-packets/rust-lib/src/radio.rs +++ b/ateam-common-packets/rust-lib/src/radio.rs @@ -11,4 +11,13 @@ pub enum TelemetryPacket { Basic(crate::bindings::BasicTelemetry), Extended(crate::bindings::ExtendedTelemetry), ParameterCommandResponse(crate::bindings::ParameterCommand), + ErrorTelemetry(crate::bindings::ErrorTelemetry), +} + +#[derive(Copy, Clone)] +pub enum ParameterType { + Basic(crate::bindings::BasicTelemetry), + Extended(crate::bindings::ExtendedTelemetry), + ParameterCommandResponse(crate::bindings::ParameterCommand), + ErrorTelemetry(crate::bindings::ErrorTelemetry), } \ No newline at end of file From b5bb105ac5b87e6f8611c64c70a05fe1a1d69183 Mon Sep 17 00:00:00 2001 From: Joe Spall Date: Fri, 18 Jul 2025 14:12:22 -0400 Subject: [PATCH 11/44] Fix merge result --- ateam-common-packets/include/extended_telemetry.h | 5 ++--- ateam-common-packets/include/kicker.h | 2 +- ateam-common-packets/include/radio.h | 4 ++-- ateam-common-packets/include/stspin.h | 1 + 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/ateam-common-packets/include/extended_telemetry.h b/ateam-common-packets/include/extended_telemetry.h index bb3d0ea..3948661 100644 --- a/ateam-common-packets/include/extended_telemetry.h +++ b/ateam-common-packets/include/extended_telemetry.h @@ -15,7 +15,6 @@ #include "power.h" #include "stspin.h" - typedef struct ExtendedTelemetry { PowerTelemetry power_status; // 28 bytes @@ -27,7 +26,7 @@ typedef struct ExtendedTelemetry { // 56 bytes each KickerTelemetry kicker_status; - // 64 bytes + // 72 bytes float imu_gyro[3]; // rad/s float imu_accel[3]; // m/s^2 @@ -43,4 +42,4 @@ typedef struct ExtendedTelemetry { float wheel_velocity_clamped_u[4]; // wheel velocities after control policy clamped for local acceleration limits /// 32 bytes } ExtendedTelemetry; -assert_size(ExtendedTelemetry, 28 + 192 + 64 + 24 + 48 + 32); +assert_size(ExtendedTelemetry, 28 + (56*4) + 72 + 24 + 48 + 32); diff --git a/ateam-common-packets/include/kicker.h b/ateam-common-packets/include/kicker.h index 0faefe2..49a9319 100644 --- a/ateam-common-packets/include/kicker.h +++ b/ateam-common-packets/include/kicker.h @@ -44,4 +44,4 @@ typedef struct KickerTelemetry { MotorTelemetry dribbler_motor; } KickerTelemetry; -assert_size(KickerTelemetry, 64); +assert_size(KickerTelemetry, 72); diff --git a/ateam-common-packets/include/radio.h b/ateam-common-packets/include/radio.h index cdc5494..8e6437b 100644 --- a/ateam-common-packets/include/radio.h +++ b/ateam-common-packets/include/radio.h @@ -58,7 +58,7 @@ typedef union RadioData { ExtendedTelemetry extended_telemetry; ParameterCommand robot_parameter_command; } RadioData; -assert_size(RadioData, 388); +assert_size(RadioData, 428); // Same RadioPacket typedef struct RadioPacket { @@ -83,5 +83,5 @@ typedef struct RadioPacket { // I think this should be a valid swap when we clean packet definitions in the future // RadioData data __attribute__((aligned (4))); } RadioPacket; -assert_size(RadioPacket, 400); +assert_size(RadioPacket, 440); static_assert(sizeof(RadioPacket) <= 448); // 512 is the current size limit of an entry in the packet buffer diff --git a/ateam-common-packets/include/stspin.h b/ateam-common-packets/include/stspin.h index 9c173ec..b3808b1 100644 --- a/ateam-common-packets/include/stspin.h +++ b/ateam-common-packets/include/stspin.h @@ -115,6 +115,7 @@ typedef struct ParameterMotorResponse { uint16_t _reserved; unsigned char firmware_img_hash[4]; + uint32_t _reserved_2[3]; } __attribute__((packed)) ParameterMotorResponse; assert_size(ParameterMotorResponse, 56); // Note: Same length as MotorResponse_Params_Packet From 2c8ddaae90c537f99490d519e97ec158700e25ff Mon Sep 17 00:00:00 2001 From: Joe Spall Date: Fri, 18 Jul 2025 14:18:56 -0400 Subject: [PATCH 12/44] Style --- ateam-common-packets/include/radio.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ateam-common-packets/include/radio.h b/ateam-common-packets/include/radio.h index 8e6437b..ba24d68 100644 --- a/ateam-common-packets/include/radio.h +++ b/ateam-common-packets/include/radio.h @@ -46,7 +46,7 @@ typedef struct RadioHeader { uint16_t minor_version; CommandCode command_code; uint16_t data_length; - uint8_t reserved; + uint8_t _reserved; } __attribute__((packed)) RadioHeader; assert_size(RadioHeader, 12); From 61fe120ffb49c09c3e9d348b48995ed26d77bfb6 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sun, 14 Dec 2025 18:46:27 -0500 Subject: [PATCH 13/44] packet updates for current control --- ateam-common-packets/include/stspin.h | 27 +++++++++++++++++---------- 1 file changed, 17 insertions(+), 10 deletions(-) diff --git a/ateam-common-packets/include/stspin.h b/ateam-common-packets/include/stspin.h index b3808b1..e73c4c1 100644 --- a/ateam-common-packets/include/stspin.h +++ b/ateam-common-packets/include/stspin.h @@ -53,13 +53,15 @@ typedef struct ParameterMotorCommand { } ParameterMotorCommand; assert_size(ParameterMotorCommand, 44); -typedef enum MotionCommandType { - OPEN_LOOP = 0, - VELOCITY = 1, - TORQUE = 2, - VELOCITY_W_TORQUE = 3 -} __attribute__((packed)) MotionCommandType; -assert_size(MotionCommandType, 1); +typedef enum MotionControlType { + MOTOR_OFF = 0, + DUTY_OPENLOOP = 1, + VOLTAGE_OPENLOOP = 2, + CURRENT = 3, + VELOCITY = 4, + VELOCITY_CURRENT = 5 +} __attribute__((packed)) MotionControlType; +assert_size(MotionControlType, 1); typedef struct MotionMotorCommand { uint32_t reset : 1; @@ -68,10 +70,15 @@ typedef struct MotionMotorCommand { uint32_t calibrate_current: 1; uint32_t _reserved : 28; - MotionCommandType motion_control_type; - uint8_t _reserved_2[3]; + MotionControlType motion_control_type; + uint8_t _reserved_2[1]; + uint16_t current_setpoint_ma; - float setpoint; + union { + float duty_setpoint_f; + float voltage_setpoint_mv; + float velocity_setpoint_rads; + }; uint32_t _reserved_3[8]; } __attribute__((__packed__)) MotionMotorCommand; assert_size(MotionMotorCommand, 44); // Note: Same length as MotorCommand_Params_Packet From bb524453777840e365e7b1b39dbf8cfb936acf70 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Wed, 17 Dec 2025 23:36:15 -0500 Subject: [PATCH 14/44] packet updates --- ateam-common-packets/include/stspin.h | 53 +++--- ateam-common-packets/include/stspin_current.h | 163 ++++++++++++++++++ 2 files changed, 184 insertions(+), 32 deletions(-) create mode 100644 ateam-common-packets/include/stspin_current.h diff --git a/ateam-common-packets/include/stspin.h b/ateam-common-packets/include/stspin.h index e73c4c1..9a79e87 100644 --- a/ateam-common-packets/include/stspin.h +++ b/ateam-common-packets/include/stspin.h @@ -53,32 +53,23 @@ typedef struct ParameterMotorCommand { } ParameterMotorCommand; assert_size(ParameterMotorCommand, 44); -typedef enum MotionControlType { - MOTOR_OFF = 0, - DUTY_OPENLOOP = 1, - VOLTAGE_OPENLOOP = 2, - CURRENT = 3, - VELOCITY = 4, - VELOCITY_CURRENT = 5 -} __attribute__((packed)) MotionControlType; -assert_size(MotionControlType, 1); +typedef enum MotionCommandType { + OPEN_LOOP = 0, + VELOCITY = 1, + TORQUE = 2, + BOTH = 3 +} __attribute__((packed)) MotionCommandType; +assert_size(MotionCommandType, 1); typedef struct MotionMotorCommand { uint32_t reset : 1; uint32_t enable_telemetry: 1; - uint32_t enable_motion: 1; - uint32_t calibrate_current: 1; - uint32_t _reserved : 28; - - MotionControlType motion_control_type; - uint8_t _reserved_2[1]; - uint16_t current_setpoint_ma; - - union { - float duty_setpoint_f; - float voltage_setpoint_mv; - float velocity_setpoint_rads; - }; + uint32_t _reserved : 30; + + MotionCommandType motion_control_type; + uint8_t _reserved_2[3]; + + float setpoint; uint32_t _reserved_3[8]; } __attribute__((__packed__)) MotionMotorCommand; assert_size(MotionMotorCommand, 44); // Note: Same length as MotorCommand_Params_Packet @@ -108,6 +99,7 @@ typedef struct ParameterMotorResponse { uint8_t version_major; uint8_t version_minor; uint16_t version_patch; + uint32_t timestamp; float vel_p; float vel_i; @@ -122,9 +114,8 @@ typedef struct ParameterMotorResponse { uint16_t _reserved; unsigned char firmware_img_hash[4]; - uint32_t _reserved_2[3]; } __attribute__((packed)) ParameterMotorResponse; -assert_size(ParameterMotorResponse, 56); // Note: Same length as MotorResponse_Params_Packet +assert_size(ParameterMotorResponse, 48); // Note: Same length as MotorResponse_Params_Packet typedef struct MotorTelemetry { uint32_t master_error : 1; @@ -149,29 +140,27 @@ typedef struct MotorTelemetry { uint32_t gain_stage_index : 8; float vel_setpoint; - float vel_computed_rads; + float vel_setpoint_clamped; int32_t encoder_delta; float vel_enc_estimate; float vel_computed_error; - float vel_computed_duty; + float vel_computed_setpoint; float torque_setpoint; float current_estimate; float torque_estimate; float torque_computed_error; - float torque_computed_nm; - float torque_computed_duty; - float vbus_voltage; + float torque_computed_setpoint; } __attribute__((packed)) MotorTelemetry; -assert_size(MotorTelemetry, 56); +assert_size(MotorTelemetry, 48); typedef struct MotorResponse { MotorResponseType type; uint8_t _reserved[3]; - uint32_t timestamp; + uint32_t crc; union ResponseData { ParameterMotorResponse params; MotorTelemetry motion; } data; } __attribute__((packed)) MotorResponse; -assert_size(MotorResponse, 64); +assert_size(MotorResponse, 56); diff --git a/ateam-common-packets/include/stspin_current.h b/ateam-common-packets/include/stspin_current.h new file mode 100644 index 0000000..ae985c8 --- /dev/null +++ b/ateam-common-packets/include/stspin_current.h @@ -0,0 +1,163 @@ +/** + * @file stspin.h + * @author Will Stuckey + * @brief communication packet definitions for the stspin firmware + * @version 0.1 + * + * @copyright Copyright (c) 2022 + * + * Struct sizes should pass static checks on the following 3 platforms: + * - Embedded -> C11, 4 byte ptr + * - Bindgen -> C11, 8 byte ptr + * - ROS -> C++, 8 byte ptr + * + */ + +#pragma once + +#include "common.h" + +typedef enum _CurrentControlledMotor_CommandType { + MCP_PARAMS = 0x20, + MCP_MOTION = 0x21 +} __attribute__((packed)) CurrentControlledMotor_CommandType; +assert_size(CurrentControlledMotor_CommandType, 1); + +typedef enum _CurrentControlledMotor_Parameter { + CURRENT_PI_CONTROLLER_GAIN_P = 0, + CURRENT_PI_CONTROLLER_GAIN_I = 1, + CURRENT_PI_CONTROLLER_ERR_ILIM = 2, + CURRENT_PI_CONTROLLER_ANTIJITTER = 3, +} __attribute__((packed)) CurrentControlledMotor_Parameter; +assert_size(CurrentControlledMotor_Parameter, 1); + +typedef enum _CurrentControlledMotor_ParameterOperation { + READ = 0, + WRITE = 1, +} __attribute__((packed)) CurrentControlledMotor_ParameterOperation; +assert_size(CurrentControlledMotor_ParameterOperation, 1); + +typedef enum _CurrentControlledMotor_ParameterDirection { + COMMAND = 0, + REPLY = 1, +} __attribute__((packed)) CurrentControlledMotor_ParameterDirection; +assert_size(CurrentControlledMotor_ParameterDirection, 1); + +typedef union _CurrentControlledMotor_ParameterValue { + uint32_t val_u32; + int32_t val_i32; + float val_f32; +} CurrentControlledMotor_ParameterValue; + +typedef struct _CurrentControlledMotor_ParameterPacket { + CurrentControlledMotor_Parameter parameter; + CurrentControlledMotor_ParameterOperation parameter_operation; + CurrentControlledMotor_ParameterDirection parameter_direction; + uint8_t _reserved[1]; + CurrentControlledMotor_ParameterValue value; +} __attribute__((packed)) CurrentControlledMotor_ParameterPacket; +assert_size(CurrentControlledMotor_ParameterPacket, 8); + +typedef enum _CurrentControlledMotor_MotionControlType { + MOTOR_OFF = 0, + DUTY_OPENLOOP = 1, + VOLTAGE_OPENLOOP = 2, + CURRENT = 3, + VELOCITY = 4, + VELOCITY_CURRENT = 5 +} __attribute__((packed)) CurrentControlledMotor_MotionControlType; +assert_size(CurrentControlledMotor_MotionControlType, 1); + +typedef struct _CurrentControlledMotor_MotionCommand { + uint32_t reset : 1; + uint32_t enable_telemetry: 1; + uint32_t enable_motion: 1; + uint32_t calibrate_current: 1; + uint32_t _reserved : 28; + + CurrentControlledMotor_MotionControlType motion_control_type; + uint8_t _reserved_2[1]; + int16_t current_setpoint_ma; + + union { + float duty_setpoint_f; + float voltage_setpoint_mv; + float velocity_setpoint_rads; + }; +} __attribute__((__packed__)) CurrentControlledMotor_MotionCommand; +assert_size(CurrentControlledMotor_MotionCommand, 12); // Note: Same length as MotorCommand_Params_Packet + +typedef struct _CurrentControlledMotor_Command { + CurrentControlledMotor_CommandType type; + uint8_t _reserved[3]; + uint32_t crc32; + union CommandData { + CurrentControlledMotor_ParameterPacket param; + CurrentControlledMotor_MotionCommand motion; + } data; +} CurrentControlledMotor_Command; +assert_size(CurrentControlledMotor_Command, 20); + +///////////////// +// responses // +///////////////// + +typedef enum _CurrentControlledMotor_ResponseType { + MRP_PARAMS = 0x80, + MRP_MOTION = 0x81, +} __attribute__((packed)) CurrentControlledMotor_ResponseType; +assert_size(CurrentControlledMotor_ResponseType, 1); + +typedef struct _CurrentControlledMotor_CurrentTelemetry { + uint16_t bus_voltage_mv; + uint16_t motor_voltage_cmd_mv; + + uint16_t current_setpoint_ma; + uint8_t reserved[2]; + uint16_t current_samples_ma[20]; +} __attribute__((packed)) CurrentControlledMotor_CurrentTelemetry; +assert_size(CurrentControlledMotor_CurrentTelemetry, 48); + +typedef struct _CurrentControlledMotor_VelocityTelemetry { + uint16_t vel_setpoint_mrads; + uint16_t wheel_vel_mrads; +} __attribute__((packed)) CurrentControlledMotor_VelocityTelemetry; +assert_size(CurrentControlledMotor_VelocityTelemetry, 4); + +typedef struct _CurrentControlledMotor_Telemetry { + uint16_t master_error : 1; + uint16_t hall_power_error : 1; + uint16_t hall_disconnected_error : 1; + uint16_t bldc_transition_error : 1; + uint16_t bldc_commutation_watchdog_error : 1; + uint16_t enc_disconnected_error: 1; + uint16_t overcurrent_error : 1; + uint16_t undervoltage_error : 1; + uint16_t overvoltage_error : 1; + uint16_t torque_limited : 1; + uint16_t control_loop_time_error: 1; + uint16_t reset_watchdog_independent: 1; + uint16_t reset_watchdog_window: 1; + uint16_t reset_low_power: 1; + uint16_t reset_software: 1; + uint16_t reset_pin: 1; + + CurrentControlledMotor_MotionControlType motion_control_type; + uint8_t gain_stage_index : 8; + + CurrentControlledMotor_CurrentTelemetry current_telemetry; + CurrentControlledMotor_VelocityTelemetry velocity_telemetry; + +} __attribute__((packed)) CurrentControlledMotor_Telemetry; +assert_size(CurrentControlledMotor_Telemetry, 56); + +typedef struct _CurrentControlledMotor_Response { + CurrentControlledMotor_ResponseType type; + uint8_t _reserved[3]; + uint32_t timestamp; + union ResponseData { + CurrentControlledMotor_ParameterPacket params; + CurrentControlledMotor_Telemetry motion; + } data; +} __attribute__((packed)) CurrentControlledMotor_Response; +assert_size(CurrentControlledMotor_Response, 64); \ No newline at end of file From 54b01cf6e2413f11586f988409b882f9af5de430 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sun, 21 Dec 2025 20:39:16 -0500 Subject: [PATCH 15/44] updates for torque packets --- ateam-common-packets/include/stspin_current.h | 36 ++++++++++--------- 1 file changed, 19 insertions(+), 17 deletions(-) diff --git a/ateam-common-packets/include/stspin_current.h b/ateam-common-packets/include/stspin_current.h index ae985c8..b279482 100644 --- a/ateam-common-packets/include/stspin_current.h +++ b/ateam-common-packets/include/stspin_current.h @@ -18,28 +18,29 @@ #include "common.h" typedef enum _CurrentControlledMotor_CommandType { - MCP_PARAMS = 0x20, - MCP_MOTION = 0x21 + CCM_CMD_PARAMS = 0x20, + CCM_CMD_MOTION = 0x21 } __attribute__((packed)) CurrentControlledMotor_CommandType; assert_size(CurrentControlledMotor_CommandType, 1); typedef enum _CurrentControlledMotor_Parameter { - CURRENT_PI_CONTROLLER_GAIN_P = 0, - CURRENT_PI_CONTROLLER_GAIN_I = 1, - CURRENT_PI_CONTROLLER_ERR_ILIM = 2, - CURRENT_PI_CONTROLLER_ANTIJITTER = 3, + CCM_PARAM_CURRENT_PI_CONTROLLER_GAIN_P = 0, + CCM_PARAM_CURRENT_PI_CONTROLLER_GAIN_I = 1, + CCM_PARAM_CURRENT_PI_CONTROLLER_ERR_ILIM = 2, + CCM_PARAM_CURRENT_PI_CONTROLLER_ANTIJITTER = 3, + CCM_PARAM_FIRMWARE_IMAGE_HASH = 100, } __attribute__((packed)) CurrentControlledMotor_Parameter; assert_size(CurrentControlledMotor_Parameter, 1); typedef enum _CurrentControlledMotor_ParameterOperation { - READ = 0, - WRITE = 1, + CCM_PARAMOP_READ = 0, + CCM_PARAMOP_WRITE = 1, } __attribute__((packed)) CurrentControlledMotor_ParameterOperation; assert_size(CurrentControlledMotor_ParameterOperation, 1); typedef enum _CurrentControlledMotor_ParameterDirection { - COMMAND = 0, - REPLY = 1, + CCM_PARAMDIR_COMMAND = 0, + CCM_PARAMDIR_REPLY = 1, } __attribute__((packed)) CurrentControlledMotor_ParameterDirection; assert_size(CurrentControlledMotor_ParameterDirection, 1); @@ -47,6 +48,7 @@ typedef union _CurrentControlledMotor_ParameterValue { uint32_t val_u32; int32_t val_i32; float val_f32; + uint8_t val_u8x4[4]; } CurrentControlledMotor_ParameterValue; typedef struct _CurrentControlledMotor_ParameterPacket { @@ -103,8 +105,8 @@ assert_size(CurrentControlledMotor_Command, 20); ///////////////// typedef enum _CurrentControlledMotor_ResponseType { - MRP_PARAMS = 0x80, - MRP_MOTION = 0x81, + CCM_RESP_PARAMS = 0x80, + CCM_RESP_TELEM = 0x81, } __attribute__((packed)) CurrentControlledMotor_ResponseType; assert_size(CurrentControlledMotor_ResponseType, 1); @@ -119,10 +121,10 @@ typedef struct _CurrentControlledMotor_CurrentTelemetry { assert_size(CurrentControlledMotor_CurrentTelemetry, 48); typedef struct _CurrentControlledMotor_VelocityTelemetry { - uint16_t vel_setpoint_mrads; - uint16_t wheel_vel_mrads; + float vel_setpoint_rads; + float wheel_vel_rads; } __attribute__((packed)) CurrentControlledMotor_VelocityTelemetry; -assert_size(CurrentControlledMotor_VelocityTelemetry, 4); +assert_size(CurrentControlledMotor_VelocityTelemetry, 8); typedef struct _CurrentControlledMotor_Telemetry { uint16_t master_error : 1; @@ -149,7 +151,7 @@ typedef struct _CurrentControlledMotor_Telemetry { CurrentControlledMotor_VelocityTelemetry velocity_telemetry; } __attribute__((packed)) CurrentControlledMotor_Telemetry; -assert_size(CurrentControlledMotor_Telemetry, 56); +assert_size(CurrentControlledMotor_Telemetry, 60); typedef struct _CurrentControlledMotor_Response { CurrentControlledMotor_ResponseType type; @@ -160,4 +162,4 @@ typedef struct _CurrentControlledMotor_Response { CurrentControlledMotor_Telemetry motion; } data; } __attribute__((packed)) CurrentControlledMotor_Response; -assert_size(CurrentControlledMotor_Response, 64); \ No newline at end of file +assert_size(CurrentControlledMotor_Response, 68); \ No newline at end of file From 3727a242b605cdc60bba81105a0802b16241acc6 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Tue, 6 Jan 2026 22:18:23 -0500 Subject: [PATCH 16/44] fix packet lens for build --- .../include/extended_telemetry.h | 2 +- ateam-common-packets/include/kicker.h | 2 +- ateam-common-packets/include/radio.h | 5 +- ateam-common-packets/include/stspin_current.h | 50 +++++++++---------- ateam-common-packets/rust-lib/build.rs | 1 + 5 files changed, 29 insertions(+), 31 deletions(-) diff --git a/ateam-common-packets/include/extended_telemetry.h b/ateam-common-packets/include/extended_telemetry.h index 3948661..816811f 100644 --- a/ateam-common-packets/include/extended_telemetry.h +++ b/ateam-common-packets/include/extended_telemetry.h @@ -42,4 +42,4 @@ typedef struct ExtendedTelemetry { float wheel_velocity_clamped_u[4]; // wheel velocities after control policy clamped for local acceleration limits /// 32 bytes } ExtendedTelemetry; -assert_size(ExtendedTelemetry, 28 + (56*4) + 72 + 24 + 48 + 32); +assert_size(ExtendedTelemetry, 28 + (48*4) + 64 + 24 + 48 + 32); diff --git a/ateam-common-packets/include/kicker.h b/ateam-common-packets/include/kicker.h index 49a9319..0faefe2 100644 --- a/ateam-common-packets/include/kicker.h +++ b/ateam-common-packets/include/kicker.h @@ -44,4 +44,4 @@ typedef struct KickerTelemetry { MotorTelemetry dribbler_motor; } KickerTelemetry; -assert_size(KickerTelemetry, 72); +assert_size(KickerTelemetry, 64); diff --git a/ateam-common-packets/include/radio.h b/ateam-common-packets/include/radio.h index ba24d68..90f7a39 100644 --- a/ateam-common-packets/include/radio.h +++ b/ateam-common-packets/include/radio.h @@ -18,6 +18,7 @@ #include "extended_telemetry.h" #include "power.h" #include "robot_parameters.h" +#include "stspin_current.h" const uint16_t kProtocolVersionMajor = 0; const uint16_t kProtocolVersionMinor = 1; @@ -58,7 +59,7 @@ typedef union RadioData { ExtendedTelemetry extended_telemetry; ParameterCommand robot_parameter_command; } RadioData; -assert_size(RadioData, 428); +assert_size(RadioData, 388); // Same RadioPacket typedef struct RadioPacket { @@ -83,5 +84,5 @@ typedef struct RadioPacket { // I think this should be a valid swap when we clean packet definitions in the future // RadioData data __attribute__((aligned (4))); } RadioPacket; -assert_size(RadioPacket, 440); +assert_size(RadioPacket, 400); static_assert(sizeof(RadioPacket) <= 448); // 512 is the current size limit of an entry in the packet buffer diff --git a/ateam-common-packets/include/stspin_current.h b/ateam-common-packets/include/stspin_current.h index b279482..d58e9c5 100644 --- a/ateam-common-packets/include/stspin_current.h +++ b/ateam-common-packets/include/stspin_current.h @@ -17,13 +17,13 @@ #include "common.h" -typedef enum _CurrentControlledMotor_CommandType { +typedef enum CurrentControlledMotor_CommandType { CCM_CMD_PARAMS = 0x20, CCM_CMD_MOTION = 0x21 } __attribute__((packed)) CurrentControlledMotor_CommandType; assert_size(CurrentControlledMotor_CommandType, 1); -typedef enum _CurrentControlledMotor_Parameter { +typedef enum CurrentControlledMotor_Parameter { CCM_PARAM_CURRENT_PI_CONTROLLER_GAIN_P = 0, CCM_PARAM_CURRENT_PI_CONTROLLER_GAIN_I = 1, CCM_PARAM_CURRENT_PI_CONTROLLER_ERR_ILIM = 2, @@ -32,26 +32,26 @@ typedef enum _CurrentControlledMotor_Parameter { } __attribute__((packed)) CurrentControlledMotor_Parameter; assert_size(CurrentControlledMotor_Parameter, 1); -typedef enum _CurrentControlledMotor_ParameterOperation { +typedef enum CurrentControlledMotor_ParameterOperation { CCM_PARAMOP_READ = 0, CCM_PARAMOP_WRITE = 1, } __attribute__((packed)) CurrentControlledMotor_ParameterOperation; assert_size(CurrentControlledMotor_ParameterOperation, 1); -typedef enum _CurrentControlledMotor_ParameterDirection { +typedef enum CurrentControlledMotor_ParameterDirection { CCM_PARAMDIR_COMMAND = 0, CCM_PARAMDIR_REPLY = 1, } __attribute__((packed)) CurrentControlledMotor_ParameterDirection; assert_size(CurrentControlledMotor_ParameterDirection, 1); -typedef union _CurrentControlledMotor_ParameterValue { +typedef union CurrentControlledMotor_ParameterValue { uint32_t val_u32; int32_t val_i32; float val_f32; uint8_t val_u8x4[4]; } CurrentControlledMotor_ParameterValue; -typedef struct _CurrentControlledMotor_ParameterPacket { +typedef struct CurrentControlledMotor_ParameterPacket { CurrentControlledMotor_Parameter parameter; CurrentControlledMotor_ParameterOperation parameter_operation; CurrentControlledMotor_ParameterDirection parameter_direction; @@ -60,17 +60,17 @@ typedef struct _CurrentControlledMotor_ParameterPacket { } __attribute__((packed)) CurrentControlledMotor_ParameterPacket; assert_size(CurrentControlledMotor_ParameterPacket, 8); -typedef enum _CurrentControlledMotor_MotionControlType { - MOTOR_OFF = 0, - DUTY_OPENLOOP = 1, - VOLTAGE_OPENLOOP = 2, - CURRENT = 3, - VELOCITY = 4, - VELOCITY_CURRENT = 5 +typedef enum CurrentControlledMotor_MotionControlType { + CCM_MCT_MOTOR_OFF = 0, + CCM_MCT_DUTY_OPENLOOP = 1, + CCM_MCT_VOLTAGE_OPENLOOP = 2, + CCM_MCT_CURRENT = 3, + CCM_MCT_VELOCITY = 4, + CCM_MCT_VELOCITY_CURRENT = 5 } __attribute__((packed)) CurrentControlledMotor_MotionControlType; assert_size(CurrentControlledMotor_MotionControlType, 1); -typedef struct _CurrentControlledMotor_MotionCommand { +typedef struct CurrentControlledMotor_MotionCommand { uint32_t reset : 1; uint32_t enable_telemetry: 1; uint32_t enable_motion: 1; @@ -81,19 +81,15 @@ typedef struct _CurrentControlledMotor_MotionCommand { uint8_t _reserved_2[1]; int16_t current_setpoint_ma; - union { - float duty_setpoint_f; - float voltage_setpoint_mv; - float velocity_setpoint_rads; - }; + float setpoint; } __attribute__((__packed__)) CurrentControlledMotor_MotionCommand; assert_size(CurrentControlledMotor_MotionCommand, 12); // Note: Same length as MotorCommand_Params_Packet -typedef struct _CurrentControlledMotor_Command { +typedef struct CurrentControlledMotor_Command { CurrentControlledMotor_CommandType type; uint8_t _reserved[3]; uint32_t crc32; - union CommandData { + union CurrentControlledMotor_CommandData { CurrentControlledMotor_ParameterPacket param; CurrentControlledMotor_MotionCommand motion; } data; @@ -104,13 +100,13 @@ assert_size(CurrentControlledMotor_Command, 20); // responses // ///////////////// -typedef enum _CurrentControlledMotor_ResponseType { +typedef enum CurrentControlledMotor_ResponseType { CCM_RESP_PARAMS = 0x80, CCM_RESP_TELEM = 0x81, } __attribute__((packed)) CurrentControlledMotor_ResponseType; assert_size(CurrentControlledMotor_ResponseType, 1); -typedef struct _CurrentControlledMotor_CurrentTelemetry { +typedef struct CurrentControlledMotor_CurrentTelemetry { uint16_t bus_voltage_mv; uint16_t motor_voltage_cmd_mv; @@ -120,13 +116,13 @@ typedef struct _CurrentControlledMotor_CurrentTelemetry { } __attribute__((packed)) CurrentControlledMotor_CurrentTelemetry; assert_size(CurrentControlledMotor_CurrentTelemetry, 48); -typedef struct _CurrentControlledMotor_VelocityTelemetry { +typedef struct CurrentControlledMotor_VelocityTelemetry { float vel_setpoint_rads; float wheel_vel_rads; } __attribute__((packed)) CurrentControlledMotor_VelocityTelemetry; assert_size(CurrentControlledMotor_VelocityTelemetry, 8); -typedef struct _CurrentControlledMotor_Telemetry { +typedef struct CurrentControlledMotor_Telemetry { uint16_t master_error : 1; uint16_t hall_power_error : 1; uint16_t hall_disconnected_error : 1; @@ -153,11 +149,11 @@ typedef struct _CurrentControlledMotor_Telemetry { } __attribute__((packed)) CurrentControlledMotor_Telemetry; assert_size(CurrentControlledMotor_Telemetry, 60); -typedef struct _CurrentControlledMotor_Response { +typedef struct CurrentControlledMotor_Response { CurrentControlledMotor_ResponseType type; uint8_t _reserved[3]; uint32_t timestamp; - union ResponseData { + union CurrentControlledMotor_ResponseData { CurrentControlledMotor_ParameterPacket params; CurrentControlledMotor_Telemetry motion; } data; diff --git a/ateam-common-packets/rust-lib/build.rs b/ateam-common-packets/rust-lib/build.rs index db467bf..ff5f2bc 100644 --- a/ateam-common-packets/rust-lib/build.rs +++ b/ateam-common-packets/rust-lib/build.rs @@ -100,6 +100,7 @@ fn main() { // bindings for. .header("../include/radio.h") .allowlist_file(".*/stspin.h") + .allowlist_file(".*/stspin_current.h") .allowlist_file(".*/kicker.h") .allowlist_file(".*/basic_control.h") .allowlist_file(".*/basic_telemetry.h") From 1e9992f9e0963ee34142ab4b2b20f897cd67c783 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sat, 10 Jan 2026 17:56:25 -0500 Subject: [PATCH 17/44] change packing for build --- ateam-common-packets/include/stspin_current.h | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/ateam-common-packets/include/stspin_current.h b/ateam-common-packets/include/stspin_current.h index d58e9c5..b862f0c 100644 --- a/ateam-common-packets/include/stspin_current.h +++ b/ateam-common-packets/include/stspin_current.h @@ -113,13 +113,13 @@ typedef struct CurrentControlledMotor_CurrentTelemetry { uint16_t current_setpoint_ma; uint8_t reserved[2]; uint16_t current_samples_ma[20]; -} __attribute__((packed)) CurrentControlledMotor_CurrentTelemetry; +} CurrentControlledMotor_CurrentTelemetry; assert_size(CurrentControlledMotor_CurrentTelemetry, 48); typedef struct CurrentControlledMotor_VelocityTelemetry { float vel_setpoint_rads; float wheel_vel_rads; -} __attribute__((packed)) CurrentControlledMotor_VelocityTelemetry; +} CurrentControlledMotor_VelocityTelemetry; assert_size(CurrentControlledMotor_VelocityTelemetry, 8); typedef struct CurrentControlledMotor_Telemetry { @@ -146,16 +146,18 @@ typedef struct CurrentControlledMotor_Telemetry { CurrentControlledMotor_CurrentTelemetry current_telemetry; CurrentControlledMotor_VelocityTelemetry velocity_telemetry; -} __attribute__((packed)) CurrentControlledMotor_Telemetry; +} CurrentControlledMotor_Telemetry; assert_size(CurrentControlledMotor_Telemetry, 60); typedef struct CurrentControlledMotor_Response { CurrentControlledMotor_ResponseType type; - uint8_t _reserved[3]; + uint8_t seq_num; + uint8_t _reserved[2]; + uint32_t timestamp; union CurrentControlledMotor_ResponseData { CurrentControlledMotor_ParameterPacket params; CurrentControlledMotor_Telemetry motion; } data; -} __attribute__((packed)) CurrentControlledMotor_Response; +} CurrentControlledMotor_Response; assert_size(CurrentControlledMotor_Response, 68); \ No newline at end of file From c570bd32cc3a50534d13259c39367f5673b59fdd Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Tue, 13 Jan 2026 22:41:02 -0500 Subject: [PATCH 18/44] change packing --- ateam-common-packets/include/stspin_current.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ateam-common-packets/include/stspin_current.h b/ateam-common-packets/include/stspin_current.h index b862f0c..e66053f 100644 --- a/ateam-common-packets/include/stspin_current.h +++ b/ateam-common-packets/include/stspin_current.h @@ -93,7 +93,7 @@ typedef struct CurrentControlledMotor_Command { CurrentControlledMotor_ParameterPacket param; CurrentControlledMotor_MotionCommand motion; } data; -} CurrentControlledMotor_Command; +} __attribute__((__packed__)) CurrentControlledMotor_Command; assert_size(CurrentControlledMotor_Command, 20); ///////////////// From 4f334ae142060bf618448b0e635cd86c5dea2bfd Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sat, 31 Jan 2026 15:24:34 -0500 Subject: [PATCH 19/44] remove underscore from names for conversion --- ateam-common-packets/include/stspin_current.h | 112 +++++++++--------- 1 file changed, 56 insertions(+), 56 deletions(-) diff --git a/ateam-common-packets/include/stspin_current.h b/ateam-common-packets/include/stspin_current.h index e66053f..8ed53f7 100644 --- a/ateam-common-packets/include/stspin_current.h +++ b/ateam-common-packets/include/stspin_current.h @@ -17,112 +17,112 @@ #include "common.h" -typedef enum CurrentControlledMotor_CommandType { +typedef enum CcmCommandType { CCM_CMD_PARAMS = 0x20, CCM_CMD_MOTION = 0x21 -} __attribute__((packed)) CurrentControlledMotor_CommandType; -assert_size(CurrentControlledMotor_CommandType, 1); +} __attribute__((packed)) CcmCommandType; +assert_size(CcmCommandType, 1); -typedef enum CurrentControlledMotor_Parameter { +typedef enum CcmParameter { CCM_PARAM_CURRENT_PI_CONTROLLER_GAIN_P = 0, CCM_PARAM_CURRENT_PI_CONTROLLER_GAIN_I = 1, CCM_PARAM_CURRENT_PI_CONTROLLER_ERR_ILIM = 2, CCM_PARAM_CURRENT_PI_CONTROLLER_ANTIJITTER = 3, CCM_PARAM_FIRMWARE_IMAGE_HASH = 100, -} __attribute__((packed)) CurrentControlledMotor_Parameter; -assert_size(CurrentControlledMotor_Parameter, 1); +} __attribute__((packed)) CcmParameter; +assert_size(CcmParameter, 1); -typedef enum CurrentControlledMotor_ParameterOperation { +typedef enum CcmParameterOperation { CCM_PARAMOP_READ = 0, CCM_PARAMOP_WRITE = 1, -} __attribute__((packed)) CurrentControlledMotor_ParameterOperation; -assert_size(CurrentControlledMotor_ParameterOperation, 1); +} __attribute__((packed)) CcmParameterOperation; +assert_size(CcmParameterOperation, 1); -typedef enum CurrentControlledMotor_ParameterDirection { +typedef enum CcmParameterDirection { CCM_PARAMDIR_COMMAND = 0, CCM_PARAMDIR_REPLY = 1, -} __attribute__((packed)) CurrentControlledMotor_ParameterDirection; -assert_size(CurrentControlledMotor_ParameterDirection, 1); +} __attribute__((packed)) CcmParameterDirection; +assert_size(CcmParameterDirection, 1); -typedef union CurrentControlledMotor_ParameterValue { +typedef union CcmParameterValue { uint32_t val_u32; int32_t val_i32; float val_f32; uint8_t val_u8x4[4]; -} CurrentControlledMotor_ParameterValue; +} CcmParameterValue; typedef struct CurrentControlledMotor_ParameterPacket { - CurrentControlledMotor_Parameter parameter; - CurrentControlledMotor_ParameterOperation parameter_operation; - CurrentControlledMotor_ParameterDirection parameter_direction; + CcmParameter parameter; + CcmParameterOperation parameter_operation; + CcmParameterDirection parameter_direction; uint8_t _reserved[1]; - CurrentControlledMotor_ParameterValue value; -} __attribute__((packed)) CurrentControlledMotor_ParameterPacket; -assert_size(CurrentControlledMotor_ParameterPacket, 8); + CcmParameterValue value; +} __attribute__((packed)) CcmParameterPacket; +assert_size(CcmParameterPacket, 8); -typedef enum CurrentControlledMotor_MotionControlType { +typedef enum CcmMotionControlType { CCM_MCT_MOTOR_OFF = 0, CCM_MCT_DUTY_OPENLOOP = 1, CCM_MCT_VOLTAGE_OPENLOOP = 2, CCM_MCT_CURRENT = 3, CCM_MCT_VELOCITY = 4, CCM_MCT_VELOCITY_CURRENT = 5 -} __attribute__((packed)) CurrentControlledMotor_MotionControlType; -assert_size(CurrentControlledMotor_MotionControlType, 1); +} __attribute__((packed)) CcmMotionControlType; +assert_size(CcmMotionControlType, 1); -typedef struct CurrentControlledMotor_MotionCommand { +typedef struct CcmMotionCommand { uint32_t reset : 1; uint32_t enable_telemetry: 1; uint32_t enable_motion: 1; uint32_t calibrate_current: 1; uint32_t _reserved : 28; - CurrentControlledMotor_MotionControlType motion_control_type; + CcmMotionControlType motion_control_type; uint8_t _reserved_2[1]; int16_t current_setpoint_ma; float setpoint; -} __attribute__((__packed__)) CurrentControlledMotor_MotionCommand; -assert_size(CurrentControlledMotor_MotionCommand, 12); // Note: Same length as MotorCommand_Params_Packet +} __attribute__((__packed__)) CcmMotionCommand; +assert_size(CcmMotionCommand, 12); // Note: Same length as MotorCommand_Params_Packet -typedef struct CurrentControlledMotor_Command { - CurrentControlledMotor_CommandType type; +typedef struct CcmCommand { + CcmCommandType type; uint8_t _reserved[3]; uint32_t crc32; - union CurrentControlledMotor_CommandData { - CurrentControlledMotor_ParameterPacket param; - CurrentControlledMotor_MotionCommand motion; + union CcmCommandData { + CcmParameterPacket param; + CcmMotionCommand motion; } data; -} __attribute__((__packed__)) CurrentControlledMotor_Command; -assert_size(CurrentControlledMotor_Command, 20); +} __attribute__((__packed__)) CcmCommand; +assert_size(CcmCommand, 20); ///////////////// // responses // ///////////////// -typedef enum CurrentControlledMotor_ResponseType { +typedef enum CcmResponseType { CCM_RESP_PARAMS = 0x80, CCM_RESP_TELEM = 0x81, -} __attribute__((packed)) CurrentControlledMotor_ResponseType; -assert_size(CurrentControlledMotor_ResponseType, 1); +} __attribute__((packed)) CcmResponseType; +assert_size(CcmResponseType, 1); -typedef struct CurrentControlledMotor_CurrentTelemetry { +typedef struct CcmCurrentTelemetry { uint16_t bus_voltage_mv; uint16_t motor_voltage_cmd_mv; uint16_t current_setpoint_ma; uint8_t reserved[2]; uint16_t current_samples_ma[20]; -} CurrentControlledMotor_CurrentTelemetry; -assert_size(CurrentControlledMotor_CurrentTelemetry, 48); +} CcmCurrentTelemetry; +assert_size(CcmCurrentTelemetry, 48); -typedef struct CurrentControlledMotor_VelocityTelemetry { +typedef struct CcmVelocityTelemetry { float vel_setpoint_rads; float wheel_vel_rads; -} CurrentControlledMotor_VelocityTelemetry; -assert_size(CurrentControlledMotor_VelocityTelemetry, 8); +} CcmVelocityTelemetry; +assert_size(CcmVelocityTelemetry, 8); -typedef struct CurrentControlledMotor_Telemetry { +typedef struct CcmTelemetry { uint16_t master_error : 1; uint16_t hall_power_error : 1; uint16_t hall_disconnected_error : 1; @@ -140,24 +140,24 @@ typedef struct CurrentControlledMotor_Telemetry { uint16_t reset_software: 1; uint16_t reset_pin: 1; - CurrentControlledMotor_MotionControlType motion_control_type; + CcmMotionControlType motion_control_type; uint8_t gain_stage_index : 8; - CurrentControlledMotor_CurrentTelemetry current_telemetry; - CurrentControlledMotor_VelocityTelemetry velocity_telemetry; + CcmCurrentTelemetry current_telemetry; + CcmVelocityTelemetry velocity_telemetry; -} CurrentControlledMotor_Telemetry; -assert_size(CurrentControlledMotor_Telemetry, 60); +} CcmTelemetry; +assert_size(CcmTelemetry, 60); -typedef struct CurrentControlledMotor_Response { - CurrentControlledMotor_ResponseType type; +typedef struct CcmResponse { + CcmResponseType type; uint8_t seq_num; uint8_t _reserved[2]; uint32_t timestamp; - union CurrentControlledMotor_ResponseData { - CurrentControlledMotor_ParameterPacket params; - CurrentControlledMotor_Telemetry motion; + union CcmResponseData { + CcmParameterPacket params; + CcmTelemetry motion; } data; -} CurrentControlledMotor_Response; -assert_size(CurrentControlledMotor_Response, 68); \ No newline at end of file +} CcmResponse; +assert_size(CcmResponse, 68); \ No newline at end of file From 53524078a5b68ce20a4c6af44bd7da7db74f8323 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Tue, 3 Feb 2026 21:51:07 -0500 Subject: [PATCH 20/44] fix torque sign --- ateam-common-packets/include/stspin_current.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ateam-common-packets/include/stspin_current.h b/ateam-common-packets/include/stspin_current.h index 8ed53f7..710f6cd 100644 --- a/ateam-common-packets/include/stspin_current.h +++ b/ateam-common-packets/include/stspin_current.h @@ -110,7 +110,7 @@ typedef struct CcmCurrentTelemetry { uint16_t bus_voltage_mv; uint16_t motor_voltage_cmd_mv; - uint16_t current_setpoint_ma; + int16_t current_setpoint_ma; uint8_t reserved[2]; uint16_t current_samples_ma[20]; } CcmCurrentTelemetry; From 079162a460eaf32c17b4fb7b64e47009117860fb Mon Sep 17 00:00:00 2001 From: Nicholas Witten Date: Wed, 4 Mar 2026 08:34:38 -0800 Subject: [PATCH 21/44] updates for body pose/twist/accel control --- ateam-common-packets/include/basic_control.h | 19 ++++--- .../include/basic_telemetry.h | 6 ++- .../include/extended_telemetry.h | 52 +++++++++++------- ateam-common-packets/include/radio.h | 8 +-- .../include/robot_parameters.h | 53 +++++++++++++++---- 5 files changed, 97 insertions(+), 41 deletions(-) diff --git a/ateam-common-packets/include/basic_control.h b/ateam-common-packets/include/basic_control.h index f51bd26..04aa301 100644 --- a/ateam-common-packets/include/basic_control.h +++ b/ateam-common-packets/include/basic_control.h @@ -18,18 +18,25 @@ typedef struct BasicControl { uint32_t reboot_robot : 1; uint32_t game_state_in_stop : 1; uint32_t emergency_stop : 1; - uint32_t body_vel_controls_enabled : 1; + uint32_t body_pose_control_enabled : 1; + uint32_t body_twist_control_enabled : 1; + uint32_t body_accel_control_enabled : 1; uint32_t wheel_vel_control_enabled : 1; uint32_t wheel_torque_control_enabled : 1; + uint32_t vision_update: 1; uint32_t dribbler_multiplier : 8; - uint32_t _reserved : 9; + uint32_t _reserved : 6; uint32_t play_song : 8; - float vel_x_linear; // m/s - float vel_y_linear; // m/s - float vel_z_angular; // m/s + float pose_x_linear_vision; // m + float pose_y_linear_vision; // m + float pose_z_angular_vision; // rad + + float x_linear_cmd; // m, m/s, m/s^2 (depending on control mode) + float y_linear_cmd; // m, m/s, m/s^2 (depending on control mode) + float z_angular_cmd; // rad, rad/s, rad/s^2 (depending on control mode) float kick_vel; // m/s (also applies to chips) float dribbler_speed; // rpm KickRequest kick_request; } BasicControl; -assert_size(BasicControl, 28); +assert_size(BasicControl, 40); diff --git a/ateam-common-packets/include/basic_telemetry.h b/ateam-common-packets/include/basic_telemetry.h index 92e7d50..2ed2022 100644 --- a/ateam-common-packets/include/basic_telemetry.h +++ b/ateam-common-packets/include/basic_telemetry.h @@ -44,10 +44,12 @@ typedef struct BasicTelemetry { uint32_t kicker_board_error : 1; uint32_t chipper_available : 1; uint32_t kicker_available : 1; - uint32_t body_vel_control_enabled : 1; + uint32_t body_pose_control_enabled : 1; + uint32_t body_twist_control_enabled : 1; + uint32_t body_accel_control_enabled : 1; uint32_t wheel_vel_control_enabled : 1; uint32_t wheel_torque_control_enabled : 1; - uint32_t _reserved : 3; + uint32_t _reserved : 1; uint16_t battery_percent; uint16_t kicker_charge_percent; diff --git a/ateam-common-packets/include/extended_telemetry.h b/ateam-common-packets/include/extended_telemetry.h index 816811f..0c55feb 100644 --- a/ateam-common-packets/include/extended_telemetry.h +++ b/ateam-common-packets/include/extended_telemetry.h @@ -13,33 +13,47 @@ #include "common.h" #include "kicker.h" #include "power.h" -#include "stspin.h" +#include "stspin_current.h" typedef struct ExtendedTelemetry { - PowerTelemetry power_status; + uint32_t timestamp_us_lo; + uint32_t timestamp_us_hi; + // 8 bytes + + uint32_t body_pose_control_enabled : 1; + uint32_t body_twist_control_enabled : 1; + uint32_t body_accel_control_enabled : 1; + uint32_t vision_update : 1; + uint32_t reserved : 28; + // 4 bytes + + // PowerTelemetry power_status; // 28 bytes - MotorTelemetry front_left_motor; - MotorTelemetry back_left_motor; - MotorTelemetry back_right_motor; - MotorTelemetry front_right_motor; - // 56 bytes each + CcmTelemetry front_left_motor; + CcmTelemetry back_left_motor; + CcmTelemetry back_right_motor; + CcmTelemetry front_right_motor; + // 60 bytes each - KickerTelemetry kicker_status; - // 72 bytes + // KickerTelemetry kicker_status; + // 64 bytes float imu_gyro[3]; // rad/s float imu_accel[3]; // m/s^2 // 24 bytes - float commanded_body_velocity[3]; // commanded body velocity from the AI - float clamped_commanded_body_velocity[3]; // commanded body velocity from the AI after the local (firmware) velocity and acceleration limits are imposed - float cgkf_body_velocity_state_estimate[3]; // CG Kalman Filter estiamted body velocity - float body_velocity_u[3]; // body velocity PID output "u" - /// 48 bytes - - float wheel_velocity_u[4]; // wheel velocities calculated from transform of body velocities after control policy - float wheel_velocity_clamped_u[4]; // wheel velocities after control policy clamped for local acceleration limits - /// 32 bytes + float vision_pose[3]; // vision system pose measurement + // 12 bytes + + float body_cmd[3]; // commanded body pose, twist, or accel + float kf_body_pose_prediction[3]; // Kalman Filter body pose estimate + float kf_body_twist_prediction[3]; // Kalman Filter body twist estimate + float kf_body_pose_estimate[3]; // Kalman Filter body pose estimate + float kf_body_twist_estimate[3]; // Kalman Filter body twist estimate + float body_twist_u[3]; // body twist control outputs after control policy + float body_accel_u[3]; // body accel control outputs after control policy + /// 84 bytes } ExtendedTelemetry; -assert_size(ExtendedTelemetry, 28 + (48*4) + 64 + 24 + 48 + 32); +// assert_size(ExtendedTelemetry, 8 + 4 + 28 + 4*60 + 64 + 24 + 12 + 84); +assert_size(ExtendedTelemetry, 8 + 4 + 4*60 + 24 + 12 + 84); diff --git a/ateam-common-packets/include/radio.h b/ateam-common-packets/include/radio.h index 90f7a39..90f14f1 100644 --- a/ateam-common-packets/include/radio.h +++ b/ateam-common-packets/include/radio.h @@ -59,7 +59,8 @@ typedef union RadioData { ExtendedTelemetry extended_telemetry; ParameterCommand robot_parameter_command; } RadioData; -assert_size(RadioData, 388); +// assert_size(RadioData, 464); +assert_size(RadioData, 372); // Same RadioPacket typedef struct RadioPacket { @@ -84,5 +85,6 @@ typedef struct RadioPacket { // I think this should be a valid swap when we clean packet definitions in the future // RadioData data __attribute__((aligned (4))); } RadioPacket; -assert_size(RadioPacket, 400); -static_assert(sizeof(RadioPacket) <= 448); // 512 is the current size limit of an entry in the packet buffer +// assert_size(RadioPacket, 476); +assert_size(RadioPacket, 384); +static_assert(sizeof(RadioPacket) <= 512); // 512 is the current size limit of an entry in the packet buffer \ No newline at end of file diff --git a/ateam-common-packets/include/robot_parameters.h b/ateam-common-packets/include/robot_parameters.h index 17c63e6..c804eaf 100644 --- a/ateam-common-packets/include/robot_parameters.h +++ b/ateam-common-packets/include/robot_parameters.h @@ -32,17 +32,48 @@ typedef enum ParameterDataFormat : uint8_t { assert_size(ParameterDataFormat, 1); typedef enum ParameterName : uint8_t { - VEL_PID_X = 0, - VEL_PID_Y = 1, - ANGULAR_VEL_PID_Z = 2, - VEL_CGKF_ENCODER_NOISE = 3, - VEL_CGKF_GYRO_NOISE = 4, - VEL_CGKF_PROCESS_NOISE = 5, - VEL_CGFK_INITIAL_COVARIANCE = 6, - VEL_CGKF_K_MATRIX = 7, - RC_BODY_VEL_LIMIT = 8, - RC_BODY_ACC_LIMIT = 9, - RC_WHEEL_ACC_LIMIT = 10, + KF_PROCESS_STD_POS_LINEAR = 0, + KF_PROCESS_STD_POS_ANGULAR = 1, + KF_PROCESS_STD_VEL_LINEAR = 2, + KF_PROCESS_STD_VEL_ANGULAR = 3, + KF_VISION_STD_LINEAR = 4, + KF_VISION_STD_ANGULAR = 5, + KF_ENCODER_STD_ANGULAR = 6, + KF_GYRO_STD_ANGULAR = 7, + KF_MAX_POS_LINEAR = 8, + KF_MAX_POS_ANGULAR = 9, + KF_MAX_VEL_LINEAR = 10, + KF_MAX_VEL_ANGULAR = 11, + PHYS_WHEEL_ANGLE_ALPHA = 12, + PHYS_WHEEL_ANGLE_BETA = 13, + PHYS_WHEEL_DISTANCE = 14, + PHYS_WHEEL_RADIUS = 15, + PHYS_BODY_MASS = 16, + PHYS_BODY_MOMENT_Z = 17, + PHYS_MOTOR_TORQUE_CONSTANT = 18, + PHYS_MOTOR_EFFICIENCY_FACTOR = 19, + PIDII_X = 20, + PIDII_Y = 21, + PIDII_THETA = 22, + PIDII_XD = 23, + PIDII_YD = 24, + PIDII_THETAD = 25, + TRAJ_ALLOWABLE_ERROR_POS_LINEAR = 26, + TRAJ_ALLOWABLE_ERROR_POS_ANGULAR = 27, + TRAJ_ALLOWABLE_ERROR_VEL_LINEAR = 28, + TRAJ_ALLOWABLE_ERROR_VEL_ANGULAR = 29, + TRAJ_MAX_VEL_LINEAR = 30, + TRAJ_MAX_VEL_ANGULAR = 31, + TRAJ_MAX_ACCEL_LINEAR = 32, + TRAJ_MAX_ACCEL_ANGULAR = 33, + HYST_PID_ENTER_ERROR_POS_LINEAR = 34, + HYST_PID_ENTER_ERROR_POS_ANGULAR = 35, + HYST_PID_ENTER_ERROR_VEL_LINEAR = 36, + HYST_PID_ENTER_ERROR_VEL_ANGULAR = 37, + HYST_PID_EXIT_ERROR_POS_LINEAR = 38, + HYST_PID_EXIT_ERROR_POS_ANGULAR = 39, + HYST_PID_EXIT_ERROR_VEL_LINEAR = 40, + HYST_PID_EXIT_ERROR_VEL_ANGULAR = 41, } ParameterName; assert_size(ParameterName, 1); From 85378877a24b41b6324882af39f341377ed7515c Mon Sep 17 00:00:00 2001 From: Nicholas Witten Date: Sun, 8 Mar 2026 21:12:35 -0700 Subject: [PATCH 22/44] add traj recompute params --- ateam-common-packets/include/robot_parameters.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/ateam-common-packets/include/robot_parameters.h b/ateam-common-packets/include/robot_parameters.h index c804eaf..426e8c0 100644 --- a/ateam-common-packets/include/robot_parameters.h +++ b/ateam-common-packets/include/robot_parameters.h @@ -74,6 +74,10 @@ typedef enum ParameterName : uint8_t { HYST_PID_EXIT_ERROR_POS_ANGULAR = 39, HYST_PID_EXIT_ERROR_VEL_LINEAR = 40, HYST_PID_EXIT_ERROR_VEL_ANGULAR = 41, + TRAJ_RECOMPUTE_ERROR_POS_LINEAR = 42, + TRAJ_RECOMPUTE_ERROR_VEL_LINEAR = 43, + TRAJ_RECOMPUTE_ERROR_POS_ANGULAR = 44, + TRAJ_RECOMPUTE_ERROR_VEL_ANGULAR = 45, } ParameterName; assert_size(ParameterName, 1); From bdedeceb3c4fd3b939ea040c4e87786344172c35 Mon Sep 17 00:00:00 2001 From: Nicholas Witten Date: Tue, 10 Mar 2026 07:14:54 -0700 Subject: [PATCH 23/44] trajectory state in extended telem --- ateam-common-packets/include/extended_telemetry.h | 10 ++++++---- ateam-common-packets/include/radio.h | 4 ++-- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/ateam-common-packets/include/extended_telemetry.h b/ateam-common-packets/include/extended_telemetry.h index 0c55feb..6649642 100644 --- a/ateam-common-packets/include/extended_telemetry.h +++ b/ateam-common-packets/include/extended_telemetry.h @@ -47,13 +47,15 @@ typedef struct ExtendedTelemetry { // 12 bytes float body_cmd[3]; // commanded body pose, twist, or accel - float kf_body_pose_prediction[3]; // Kalman Filter body pose estimate - float kf_body_twist_prediction[3]; // Kalman Filter body twist estimate + float body_traj_pose[3]; // computed trajectory pose setpoint + float body_traj_twist[3]; // computed trajectory twist setpoint + float kf_body_pose_prediction[3]; // Kalman Filter body pose prediction + float kf_body_twist_prediction[3]; // Kalman Filter body twist prediction float kf_body_pose_estimate[3]; // Kalman Filter body pose estimate float kf_body_twist_estimate[3]; // Kalman Filter body twist estimate float body_twist_u[3]; // body twist control outputs after control policy float body_accel_u[3]; // body accel control outputs after control policy - /// 84 bytes + /// 108 bytes } ExtendedTelemetry; // assert_size(ExtendedTelemetry, 8 + 4 + 28 + 4*60 + 64 + 24 + 12 + 84); -assert_size(ExtendedTelemetry, 8 + 4 + 4*60 + 24 + 12 + 84); +assert_size(ExtendedTelemetry, 8 + 4 + 4*60 + 24 + 12 + 108); diff --git a/ateam-common-packets/include/radio.h b/ateam-common-packets/include/radio.h index 90f14f1..3ba6b46 100644 --- a/ateam-common-packets/include/radio.h +++ b/ateam-common-packets/include/radio.h @@ -60,7 +60,7 @@ typedef union RadioData { ParameterCommand robot_parameter_command; } RadioData; // assert_size(RadioData, 464); -assert_size(RadioData, 372); +assert_size(RadioData, 396); // Same RadioPacket typedef struct RadioPacket { @@ -86,5 +86,5 @@ typedef struct RadioPacket { // RadioData data __attribute__((aligned (4))); } RadioPacket; // assert_size(RadioPacket, 476); -assert_size(RadioPacket, 384); +assert_size(RadioPacket, 408); static_assert(sizeof(RadioPacket) <= 512); // 512 is the current size limit of an entry in the packet buffer \ No newline at end of file From 1888bf2cc6489eff0719574ad53f44bcc862745d Mon Sep 17 00:00:00 2001 From: Nicholas Witten Date: Sat, 14 Mar 2026 12:58:14 -0700 Subject: [PATCH 24/44] add friction coefficients to parameters --- .../include/robot_parameters.h | 56 ++++++++++--------- 1 file changed, 30 insertions(+), 26 deletions(-) diff --git a/ateam-common-packets/include/robot_parameters.h b/ateam-common-packets/include/robot_parameters.h index 426e8c0..f5c8bb5 100644 --- a/ateam-common-packets/include/robot_parameters.h +++ b/ateam-common-packets/include/robot_parameters.h @@ -52,32 +52,36 @@ typedef enum ParameterName : uint8_t { PHYS_BODY_MOMENT_Z = 17, PHYS_MOTOR_TORQUE_CONSTANT = 18, PHYS_MOTOR_EFFICIENCY_FACTOR = 19, - PIDII_X = 20, - PIDII_Y = 21, - PIDII_THETA = 22, - PIDII_XD = 23, - PIDII_YD = 24, - PIDII_THETAD = 25, - TRAJ_ALLOWABLE_ERROR_POS_LINEAR = 26, - TRAJ_ALLOWABLE_ERROR_POS_ANGULAR = 27, - TRAJ_ALLOWABLE_ERROR_VEL_LINEAR = 28, - TRAJ_ALLOWABLE_ERROR_VEL_ANGULAR = 29, - TRAJ_MAX_VEL_LINEAR = 30, - TRAJ_MAX_VEL_ANGULAR = 31, - TRAJ_MAX_ACCEL_LINEAR = 32, - TRAJ_MAX_ACCEL_ANGULAR = 33, - HYST_PID_ENTER_ERROR_POS_LINEAR = 34, - HYST_PID_ENTER_ERROR_POS_ANGULAR = 35, - HYST_PID_ENTER_ERROR_VEL_LINEAR = 36, - HYST_PID_ENTER_ERROR_VEL_ANGULAR = 37, - HYST_PID_EXIT_ERROR_POS_LINEAR = 38, - HYST_PID_EXIT_ERROR_POS_ANGULAR = 39, - HYST_PID_EXIT_ERROR_VEL_LINEAR = 40, - HYST_PID_EXIT_ERROR_VEL_ANGULAR = 41, - TRAJ_RECOMPUTE_ERROR_POS_LINEAR = 42, - TRAJ_RECOMPUTE_ERROR_VEL_LINEAR = 43, - TRAJ_RECOMPUTE_ERROR_POS_ANGULAR = 44, - TRAJ_RECOMPUTE_ERROR_VEL_ANGULAR = 45, + PHYS_COULOMB_FRICTION_COEFFICIENT_LINEAR = 20, + PHYS_COULOMB_FRICTION_COEFFICIENT_ANGULAR = 21, + PHYS_VISCOUS_FRICTION_COEFFICIENT_LINEAR = 22, + PHYS_VISCOUS_FRICTION_COEFFICIENT_ANGULAR = 23, + PIDII_X = 24, + PIDII_Y = 25, + PIDII_THETA = 26, + PIDII_XD = 27, + PIDII_YD = 28, + PIDII_THETAD = 29, + TRAJ_ALLOWABLE_ERROR_POS_LINEAR = 30, + TRAJ_ALLOWABLE_ERROR_POS_ANGULAR = 31, + TRAJ_ALLOWABLE_ERROR_VEL_LINEAR = 32, + TRAJ_ALLOWABLE_ERROR_VEL_ANGULAR = 33, + TRAJ_MAX_VEL_LINEAR = 34, + TRAJ_MAX_VEL_ANGULAR = 35, + TRAJ_MAX_ACCEL_LINEAR = 36, + TRAJ_MAX_ACCEL_ANGULAR = 37, + HYST_PID_ENTER_ERROR_POS_LINEAR = 38, + HYST_PID_ENTER_ERROR_POS_ANGULAR = 39, + HYST_PID_ENTER_ERROR_VEL_LINEAR = 40, + HYST_PID_ENTER_ERROR_VEL_ANGULAR = 41, + HYST_PID_EXIT_ERROR_POS_LINEAR = 42, + HYST_PID_EXIT_ERROR_POS_ANGULAR = 43, + HYST_PID_EXIT_ERROR_VEL_LINEAR = 44, + HYST_PID_EXIT_ERROR_VEL_ANGULAR = 45, + TRAJ_RECOMPUTE_ERROR_POS_LINEAR = 46, + TRAJ_RECOMPUTE_ERROR_VEL_LINEAR = 47, + TRAJ_RECOMPUTE_ERROR_POS_ANGULAR = 48, + TRAJ_RECOMPUTE_ERROR_VEL_ANGULAR = 49, } ParameterName; assert_size(ParameterName, 1); From 4dd653ba0134a95bc10080d7d6a873c2fd353505 Mon Sep 17 00:00:00 2001 From: Nicholas Witten Date: Tue, 17 Mar 2026 22:08:01 -0700 Subject: [PATCH 25/44] add ff and fb gain to parameters --- ateam-common-packets/include/robot_parameters.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ateam-common-packets/include/robot_parameters.h b/ateam-common-packets/include/robot_parameters.h index f5c8bb5..159747b 100644 --- a/ateam-common-packets/include/robot_parameters.h +++ b/ateam-common-packets/include/robot_parameters.h @@ -82,6 +82,8 @@ typedef enum ParameterName : uint8_t { TRAJ_RECOMPUTE_ERROR_VEL_LINEAR = 47, TRAJ_RECOMPUTE_ERROR_POS_ANGULAR = 48, TRAJ_RECOMPUTE_ERROR_VEL_ANGULAR = 49, + FEEDFORWARD_GAIN = 50, + FEEDBACK_GAIN = 51, } ParameterName; assert_size(ParameterName, 1); From ec25daee489f483d5cb6bfec36181a28ff105427 Mon Sep 17 00:00:00 2001 From: Nicholas Witten Date: Sat, 11 Apr 2026 17:38:22 -0700 Subject: [PATCH 26/44] add body accel friction compensation cmd to extended telem --- ateam-common-packets/include/extended_telemetry.h | 3 ++- ateam-common-packets/include/radio.h | 4 ++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/ateam-common-packets/include/extended_telemetry.h b/ateam-common-packets/include/extended_telemetry.h index 6649642..5442f0c 100644 --- a/ateam-common-packets/include/extended_telemetry.h +++ b/ateam-common-packets/include/extended_telemetry.h @@ -55,7 +55,8 @@ typedef struct ExtendedTelemetry { float kf_body_twist_estimate[3]; // Kalman Filter body twist estimate float body_twist_u[3]; // body twist control outputs after control policy float body_accel_u[3]; // body accel control outputs after control policy + float body_accel_u_fric_comp[3]; // body accel control outputs after control policy with friction compensation added in /// 108 bytes } ExtendedTelemetry; // assert_size(ExtendedTelemetry, 8 + 4 + 28 + 4*60 + 64 + 24 + 12 + 84); -assert_size(ExtendedTelemetry, 8 + 4 + 4*60 + 24 + 12 + 108); +assert_size(ExtendedTelemetry, 8 + 4 + 4*60 + 24 + 12 + 120); diff --git a/ateam-common-packets/include/radio.h b/ateam-common-packets/include/radio.h index 3ba6b46..0e6fe66 100644 --- a/ateam-common-packets/include/radio.h +++ b/ateam-common-packets/include/radio.h @@ -60,7 +60,7 @@ typedef union RadioData { ParameterCommand robot_parameter_command; } RadioData; // assert_size(RadioData, 464); -assert_size(RadioData, 396); +assert_size(RadioData, 408); // Same RadioPacket typedef struct RadioPacket { @@ -86,5 +86,5 @@ typedef struct RadioPacket { // RadioData data __attribute__((aligned (4))); } RadioPacket; // assert_size(RadioPacket, 476); -assert_size(RadioPacket, 408); +assert_size(RadioPacket, 420); static_assert(sizeof(RadioPacket) <= 512); // 512 is the current size limit of an entry in the packet buffer \ No newline at end of file From 03466a16fbd46d14d7200d686b9c24c8df16ea74 Mon Sep 17 00:00:00 2001 From: Nicholas Witten Date: Sun, 12 Apr 2026 13:24:22 -0700 Subject: [PATCH 27/44] move body controller telem to a struct, concise parameter interface --- ateam-common-packets/include/basic_control.h | 20 ++-- .../include/basic_telemetry.h | 7 +- ateam-common-packets/include/body_control.h | 46 +++++++++ .../include/extended_telemetry.h | 31 +----- .../include/robot_parameters.h | 94 +++++++------------ 5 files changed, 97 insertions(+), 101 deletions(-) create mode 100644 ateam-common-packets/include/body_control.h diff --git a/ateam-common-packets/include/basic_control.h b/ateam-common-packets/include/basic_control.h index 04aa301..cab8942 100644 --- a/ateam-common-packets/include/basic_control.h +++ b/ateam-common-packets/include/basic_control.h @@ -12,31 +12,37 @@ #include "common.h" #include "kicker.h" +#include "body_control.h" typedef struct BasicControl { + // Bit field flags uint32_t request_shutdown : 1; uint32_t reboot_robot : 1; uint32_t game_state_in_stop : 1; uint32_t emergency_stop : 1; - uint32_t body_pose_control_enabled : 1; - uint32_t body_twist_control_enabled : 1; - uint32_t body_accel_control_enabled : 1; uint32_t wheel_vel_control_enabled : 1; uint32_t wheel_torque_control_enabled : 1; uint32_t vision_update: 1; - uint32_t dribbler_multiplier : 8; - uint32_t _reserved : 6; - uint32_t play_song : 8; + uint32_t reserved1: 25; + BodyControlMode body_control_mode; + uint8_t dribbler_multiplier; // 0 - 100, percentage of full speed to spin dribbler when the ball is not detected, + uint8_t play_song; + uint8_t reserved2; + + // Vision update float pose_x_linear_vision; // m float pose_y_linear_vision; // m float pose_z_angular_vision; // rad + // Body control command float x_linear_cmd; // m, m/s, m/s^2 (depending on control mode) float y_linear_cmd; // m, m/s, m/s^2 (depending on control mode) float z_angular_cmd; // rad, rad/s, rad/s^2 (depending on control mode) + + // Dribbler and kicker commands float kick_vel; // m/s (also applies to chips) float dribbler_speed; // rpm KickRequest kick_request; } BasicControl; -assert_size(BasicControl, 40); +assert_size(BasicControl, 44); diff --git a/ateam-common-packets/include/basic_telemetry.h b/ateam-common-packets/include/basic_telemetry.h index 2ed2022..6e24366 100644 --- a/ateam-common-packets/include/basic_telemetry.h +++ b/ateam-common-packets/include/basic_telemetry.h @@ -44,12 +44,7 @@ typedef struct BasicTelemetry { uint32_t kicker_board_error : 1; uint32_t chipper_available : 1; uint32_t kicker_available : 1; - uint32_t body_pose_control_enabled : 1; - uint32_t body_twist_control_enabled : 1; - uint32_t body_accel_control_enabled : 1; - uint32_t wheel_vel_control_enabled : 1; - uint32_t wheel_torque_control_enabled : 1; - uint32_t _reserved : 1; + uint32_t _reserved : 6; uint16_t battery_percent; uint16_t kicker_charge_percent; diff --git a/ateam-common-packets/include/body_control.h b/ateam-common-packets/include/body_control.h new file mode 100644 index 0000000..2c9dfbc --- /dev/null +++ b/ateam-common-packets/include/body_control.h @@ -0,0 +1,46 @@ +/** + * @file body_control.h + * @author Nicholas Witten + * @brief definition for body control data types + * @version 0.1 + * + * @copyright Copyright (c) 2022 + * + */ +#pragma once + +#include "common.h" + + +typedef enum BodyControlMode : uint8_t { + BCM_OFF = 0, + BCM_GLOBAL_POSE = 1, + BCM_GLOBAL_TWIST = 2, + BCM_LOCAL_TWIST = 3, + BCM_GLOBAL_ACCEL = 4, + BCM_LOCAL_ACCEL = 5 +} BodyControlMode; +assert_size(BodyControlMode, 1); + +typedef struct BodyControlTelemetry { + BodyControlMode body_control_mode; + uint8_t vision_update: 1; + uint8_t _reserved1: 7; + uint8_t _reserved2[2]; + // 4 bytes + + float imu_gyro[3]; // rad/s + float imu_accel[3]; // m/s^2 + float vision_pose[3]; // vision system pose measurement + float body_cmd[3]; // commanded body pose, twist, or accel + float body_traj_pose[3]; // computed trajectory pose setpoint + float body_traj_twist[3]; // computed trajectory twist setpoint + float kf_body_pose_prediction[3]; // Kalman Filter body pose prediction + float kf_body_twist_prediction[3]; // Kalman Filter body twist prediction + float kf_body_pose_estimate[3]; // Kalman Filter body pose estimate + float kf_body_twist_estimate[3]; // Kalman Filter body twist estimate + float body_twist_u[3]; // body twist control outputs after control policy + float body_accel_u[3]; // body accel control outputs after control policy + float body_accel_u_fric_comp[3]; // body accel control outputs after control policy with friction compensation added in +} BodyControlTelemetry; +assert_size(BodyControlTelemetry, 4 + 13*3*4); \ No newline at end of file diff --git a/ateam-common-packets/include/extended_telemetry.h b/ateam-common-packets/include/extended_telemetry.h index 5442f0c..7949bb1 100644 --- a/ateam-common-packets/include/extended_telemetry.h +++ b/ateam-common-packets/include/extended_telemetry.h @@ -11,6 +11,7 @@ #pragma once #include "common.h" +#include "body_control.h" #include "kicker.h" #include "power.h" #include "stspin_current.h" @@ -20,13 +21,6 @@ typedef struct ExtendedTelemetry { uint32_t timestamp_us_hi; // 8 bytes - uint32_t body_pose_control_enabled : 1; - uint32_t body_twist_control_enabled : 1; - uint32_t body_accel_control_enabled : 1; - uint32_t vision_update : 1; - uint32_t reserved : 28; - // 4 bytes - // PowerTelemetry power_status; // 28 bytes @@ -39,24 +33,7 @@ typedef struct ExtendedTelemetry { // KickerTelemetry kicker_status; // 64 bytes - float imu_gyro[3]; // rad/s - float imu_accel[3]; // m/s^2 - // 24 bytes - - float vision_pose[3]; // vision system pose measurement - // 12 bytes - - float body_cmd[3]; // commanded body pose, twist, or accel - float body_traj_pose[3]; // computed trajectory pose setpoint - float body_traj_twist[3]; // computed trajectory twist setpoint - float kf_body_pose_prediction[3]; // Kalman Filter body pose prediction - float kf_body_twist_prediction[3]; // Kalman Filter body twist prediction - float kf_body_pose_estimate[3]; // Kalman Filter body pose estimate - float kf_body_twist_estimate[3]; // Kalman Filter body twist estimate - float body_twist_u[3]; // body twist control outputs after control policy - float body_accel_u[3]; // body accel control outputs after control policy - float body_accel_u_fric_comp[3]; // body accel control outputs after control policy with friction compensation added in - /// 108 bytes + BodyControlTelemetry body_control_telemetry; + // 160 bytes } ExtendedTelemetry; -// assert_size(ExtendedTelemetry, 8 + 4 + 28 + 4*60 + 64 + 24 + 12 + 84); -assert_size(ExtendedTelemetry, 8 + 4 + 4*60 + 24 + 12 + 120); +assert_size(ExtendedTelemetry, 8 + 4*60 + 160); diff --git a/ateam-common-packets/include/robot_parameters.h b/ateam-common-packets/include/robot_parameters.h index 159747b..4a931d9 100644 --- a/ateam-common-packets/include/robot_parameters.h +++ b/ateam-common-packets/include/robot_parameters.h @@ -23,67 +23,40 @@ assert_size(ParameterCommandCode, 1); typedef enum ParameterDataFormat : uint8_t { F32 = 0, - VEC3_F32 = 1, - VEC4_F32 = 2, - PID_F32 = 3, - PID_LIMITED_INTEGRAL_F32 = 4, - MATRIX_F32 = 5 + VEC2_F32 = 1, + VEC3_F32 = 2, + VEC4_F32 = 3, + VEC5_F32 = 4, } ParameterDataFormat; assert_size(ParameterDataFormat, 1); typedef enum ParameterName : uint8_t { - KF_PROCESS_STD_POS_LINEAR = 0, - KF_PROCESS_STD_POS_ANGULAR = 1, - KF_PROCESS_STD_VEL_LINEAR = 2, - KF_PROCESS_STD_VEL_ANGULAR = 3, - KF_VISION_STD_LINEAR = 4, - KF_VISION_STD_ANGULAR = 5, - KF_ENCODER_STD_ANGULAR = 6, - KF_GYRO_STD_ANGULAR = 7, - KF_MAX_POS_LINEAR = 8, - KF_MAX_POS_ANGULAR = 9, - KF_MAX_VEL_LINEAR = 10, - KF_MAX_VEL_ANGULAR = 11, - PHYS_WHEEL_ANGLE_ALPHA = 12, - PHYS_WHEEL_ANGLE_BETA = 13, - PHYS_WHEEL_DISTANCE = 14, - PHYS_WHEEL_RADIUS = 15, - PHYS_BODY_MASS = 16, - PHYS_BODY_MOMENT_Z = 17, - PHYS_MOTOR_TORQUE_CONSTANT = 18, - PHYS_MOTOR_EFFICIENCY_FACTOR = 19, - PHYS_COULOMB_FRICTION_COEFFICIENT_LINEAR = 20, - PHYS_COULOMB_FRICTION_COEFFICIENT_ANGULAR = 21, - PHYS_VISCOUS_FRICTION_COEFFICIENT_LINEAR = 22, - PHYS_VISCOUS_FRICTION_COEFFICIENT_ANGULAR = 23, - PIDII_X = 24, - PIDII_Y = 25, - PIDII_THETA = 26, - PIDII_XD = 27, - PIDII_YD = 28, - PIDII_THETAD = 29, - TRAJ_ALLOWABLE_ERROR_POS_LINEAR = 30, - TRAJ_ALLOWABLE_ERROR_POS_ANGULAR = 31, - TRAJ_ALLOWABLE_ERROR_VEL_LINEAR = 32, - TRAJ_ALLOWABLE_ERROR_VEL_ANGULAR = 33, - TRAJ_MAX_VEL_LINEAR = 34, - TRAJ_MAX_VEL_ANGULAR = 35, - TRAJ_MAX_ACCEL_LINEAR = 36, - TRAJ_MAX_ACCEL_ANGULAR = 37, - HYST_PID_ENTER_ERROR_POS_LINEAR = 38, - HYST_PID_ENTER_ERROR_POS_ANGULAR = 39, - HYST_PID_ENTER_ERROR_VEL_LINEAR = 40, - HYST_PID_ENTER_ERROR_VEL_ANGULAR = 41, - HYST_PID_EXIT_ERROR_POS_LINEAR = 42, - HYST_PID_EXIT_ERROR_POS_ANGULAR = 43, - HYST_PID_EXIT_ERROR_VEL_LINEAR = 44, - HYST_PID_EXIT_ERROR_VEL_ANGULAR = 45, - TRAJ_RECOMPUTE_ERROR_POS_LINEAR = 46, - TRAJ_RECOMPUTE_ERROR_VEL_LINEAR = 47, - TRAJ_RECOMPUTE_ERROR_POS_ANGULAR = 48, - TRAJ_RECOMPUTE_ERROR_VEL_ANGULAR = 49, - FEEDFORWARD_GAIN = 50, - FEEDBACK_GAIN = 51, + /// [POS_LINEAR, POS_ANGULAR, VEL_LINEAR, VEL_ANGULAR] + KF_PROCESS_STD = 0, + /// [VISION_STD_LINEAR, VISION_STD_ANGULAR, ENCODER_STD_ANGULAR, GYRO_STD_ANGULAR] + KF_MEASUREMENT_STD = 1, + /// [KF_MAX_POS_LINEAR, KF_MAX_POS_ANGULAR, KF_MAX_VEL_LINEAR, KF_MAX_VEL_ANGULAR] + KF_MAX_STATE = 2, + /// [WHEEL_ANGLE_ALPHA, WHEEL_ANGLE_BETA, WHEEL_DISTANCE, WHEEL_RADIUS] + PHYS_WHEEL = 3, + /// [BODY_MASS, BODY_MOMENT_Z] + PHYS_INERTIA = 4, + /// [MOTOR_TORQUE_CONSTANT, MOTOR_EFFICIENCY_FACTOR] + PHYS_MOTOR_MODEL = 5, + /// [COULOMB_FRICTION_COEFFICIENT_LINEAR, COULOMB_FRICTION_COEFFICIENT_ANGULAR, VISCOUS_FRICTION_COEFFICIENT_LINEAR, VISCOUS_FRICTION_COEFFICIENT_ANGULAR] + PHYS_FRICTION_MODEL = 6, + /// [FEEDFORWARD_GAIN, FEEDBACK_GAIN] + POSE_CONTROL_GAIN = 7, + /// [ERROR_POS_LINEAR, ERROR_POS_ANGULAR, ERROR_VEL_LINEAR, ERROR_VEL_ANGULAR] thresholds for when to recompute the trajectory + TRAJ_RECOMPUTE_ERROR = 8, + /// [MAX_VEL_LINEAR, MAX_VEL_ANGULAR, MAX_ACCEL_LINEAR, MAX_ACCEL_ANGULAR] + TRAJ_MAX = 9, + /// [P, I, D, I_MIN, I_MAX] + POSE_FB_PIDII_X = 10, + /// [P, I, D, I_MIN, I_MAX] + POSE_FB_PIDII_Y = 11, + /// [P, I, D, I_MIN, I_MAX] + POSE_FB_PIDII_THETA = 12, } ParameterName; assert_size(ParameterName, 1); @@ -93,11 +66,10 @@ typedef struct ParameterCommand { ParameterName parameter_name; union ParameterData { float f32; + float vec2_f32[2]; float vec3_f32[3]; float vec4_f32[4]; - float pid_f32[3]; - float pidii_f32[5]; - float matrix_f32[25]; + float vec5_f32[5]; } data __attribute__((aligned (4))); } ParameterCommand; -assert_size(ParameterCommand, 104); \ No newline at end of file +assert_size(ParameterCommand, 24); \ No newline at end of file From 37a2c443bd47ffd3797567f5635bdcc15ee24d0c Mon Sep 17 00:00:00 2001 From: Nicholas Witten Date: Sun, 12 Apr 2026 14:51:24 -0700 Subject: [PATCH 28/44] add body_control.h to rust-lib bindings --- ateam-common-packets/rust-lib/build.rs | 1 + 1 file changed, 1 insertion(+) diff --git a/ateam-common-packets/rust-lib/build.rs b/ateam-common-packets/rust-lib/build.rs index ff5f2bc..49af732 100644 --- a/ateam-common-packets/rust-lib/build.rs +++ b/ateam-common-packets/rust-lib/build.rs @@ -106,6 +106,7 @@ fn main() { .allowlist_file(".*/basic_telemetry.h") .allowlist_file(".*/extended_telemetry.h") .allowlist_file(".*/hello_data.h") + .allowlist_file(".*/body_control.h") .allowlist_file(".*/power.h") .allowlist_file(".*/robot_parameters.h") .allowlist_file(".*/radio.h") From e1559eb64abcdb95f2c2168ad4e95d3dfa591e15 Mon Sep 17 00:00:00 2001 From: Nicholas Witten Date: Sun, 12 Apr 2026 21:31:35 -0700 Subject: [PATCH 29/44] add twist pid params --- ateam-common-packets/include/robot_parameters.h | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/ateam-common-packets/include/robot_parameters.h b/ateam-common-packets/include/robot_parameters.h index 4a931d9..1a92e14 100644 --- a/ateam-common-packets/include/robot_parameters.h +++ b/ateam-common-packets/include/robot_parameters.h @@ -57,6 +57,12 @@ typedef enum ParameterName : uint8_t { POSE_FB_PIDII_Y = 11, /// [P, I, D, I_MIN, I_MAX] POSE_FB_PIDII_THETA = 12, + /// [P, I, D, I_MIN, I_MAX] + TWIST_FB_PIDII_X = 13, + /// [P, I, D, I_MIN, I_MAX] + TWIST_FB_PIDII_Y = 14, + /// [P, I, D, I_MIN, I_MAX] + TWIST_FB_PIDII_THETA = 15, } ParameterName; assert_size(ParameterName, 1); From 122fc02345add6926366159d7761e18dec5983bf Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Tue, 21 Apr 2026 21:47:42 -0400 Subject: [PATCH 30/44] initial pass of packet rework --- ateam-common-packets/include/basic_control.h | 63 +++++--- .../include/basic_telemetry.h | 33 ++-- ateam-common-packets/include/body_control.h | 45 ++---- .../include/{hello_data.h => discovery.h} | 22 +-- .../include/error_telemetry.h | 10 -- .../include/extended_telemetry.h | 22 +-- ateam-common-packets/include/kicker.h | 14 +- ateam-common-packets/include/radio.h | 75 +++------ .../include/robot_parameters.h | 10 -- .../robot_skills/global_acceleration.h | 15 ++ .../include/robot_skills/global_position.h | 19 +++ .../include/robot_skills/global_velocity.h | 17 ++ .../include/robot_skills/local_acceleration.h | 15 ++ .../include/robot_skills/local_velocity.h | 17 ++ ateam-common-packets/include/stspin.h | 15 -- ateam-common-packets/include/stspin_current.h | 15 -- ateam-common-packets/rust-lib/build.rs | 22 +-- radio-protocol/README | 77 --------- radio-protocol/ai.py | 153 ------------------ radio-protocol/robot.py | 101 ------------ 20 files changed, 212 insertions(+), 548 deletions(-) rename ateam-common-packets/include/{hello_data.h => discovery.h} (55%) create mode 100644 ateam-common-packets/include/robot_skills/global_acceleration.h create mode 100644 ateam-common-packets/include/robot_skills/global_position.h create mode 100644 ateam-common-packets/include/robot_skills/global_velocity.h create mode 100644 ateam-common-packets/include/robot_skills/local_acceleration.h create mode 100644 ateam-common-packets/include/robot_skills/local_velocity.h delete mode 100644 radio-protocol/README delete mode 100644 radio-protocol/ai.py delete mode 100644 radio-protocol/robot.py diff --git a/ateam-common-packets/include/basic_control.h b/ateam-common-packets/include/basic_control.h index cab8942..fea1102 100644 --- a/ateam-common-packets/include/basic_control.h +++ b/ateam-common-packets/include/basic_control.h @@ -1,18 +1,24 @@ -/** - * @file basic_control.h - * @author Matthew Barulic - * @brief definition for Basic Control data type - * @version 0.1 - * - * @copyright Copyright (c) 2022 - * - */ - #pragma once #include "common.h" #include "kicker.h" -#include "body_control.h" + +#include "robot_skills/global_position.h" +#include "robot_skills/global_velocity.h" +#include "robot_skills/local_velocity.h" +#include "robot_skills/global_acceleration.h" +#include "robot_skills/local_acceleration.h" + +typedef enum BodyControlMode : uint8_t { + BCM_OFF = 0, + BCM_GLOBAL_POSITION = 1, + BCM_GLOBAL_VELOCITY = 2, + BCM_LOCAL_VELOCITY = 3, + BCM_GLOBAL_ACCEL = 4, + BCM_LOCAL_ACCEL = 5 + // add additional skills and modes here +} BodyControlMode; +assert_size(BodyControlMode, 1); typedef struct BasicControl { // Bit field flags @@ -24,25 +30,32 @@ typedef struct BasicControl { uint32_t wheel_torque_control_enabled : 1; uint32_t vision_update: 1; uint32_t reserved1: 25; - - BodyControlMode body_control_mode; - uint8_t dribbler_multiplier; // 0 - 100, percentage of full speed to spin dribbler when the ball is not detected, - uint8_t play_song; - uint8_t reserved2; + // 4 bytes // Vision update - float pose_x_linear_vision; // m - float pose_y_linear_vision; // m - float pose_z_angular_vision; // rad + float vision_position_update[3]; + // 12 bytes - // Body control command - float x_linear_cmd; // m, m/s, m/s^2 (depending on control mode) - float y_linear_cmd; // m, m/s, m/s^2 (depending on control mode) - float z_angular_cmd; // rad, rad/s, rad/s^2 (depending on control mode) + // Control mode and ancillary data + BodyControlMode body_control_mode; + KickRequest kick_request; + uint8_t play_song; + uint8_t reserved2[1]; + // 4 bytes // Dribbler and kicker commands float kick_vel; // m/s (also applies to chips) float dribbler_speed; // rpm - KickRequest kick_request; + // 8 bytes + + // Body control command + union BodyControlCommand { + GlobalPositionCommand global_pos; + GlobalVelocityCommand global_vel; + LocalVelocityCommand local_vel; + GlobalAccelerationCommand global_acc; + LocalAccelerationCommand local_acc; + } cmd __attribute__((aligned (4))); + // 28 bytes } BasicControl; -assert_size(BasicControl, 44); +assert_size(BasicControl, 4 + 4 + 12 + 28 + 8); diff --git a/ateam-common-packets/include/basic_telemetry.h b/ateam-common-packets/include/basic_telemetry.h index 6e24366..c477ca1 100644 --- a/ateam-common-packets/include/basic_telemetry.h +++ b/ateam-common-packets/include/basic_telemetry.h @@ -1,22 +1,19 @@ -/** - * @file basic_telemetry.h - * @author Matthew Barulic - * @brief definition for Basic Telemetry data type - * @version 0.1 - * - * @copyright Copyright (c) 2022 - * - */ - #pragma once #include "common.h" +#include "body_control.h" + +#include "robot_skills/global_position.h" +#include "robot_skills/global_velocity.h" +#include "robot_skills/local_velocity.h" +#include "robot_skills/global_acceleration.h" +#include "robot_skills/local_acceleration.h" typedef struct BasicTelemetry { uint8_t transmission_sequence_number; uint8_t control_data_sequence_number; - uint8_t robot_revision_major; - uint8_t robot_revision_minor; + BodyControlMode body_control_mode; + uint8_t reserved1; uint32_t power_error : 1; uint32_t power_board_error : 1; @@ -48,5 +45,15 @@ typedef struct BasicTelemetry { uint16_t battery_percent; uint16_t kicker_charge_percent; + + // Body control command + union BodyControlTelemetry { + GlobalPositionTelemetry global_pos; + GlobalVelocityTelemetry global_vel; + LocalVelocityTelemetry local_vel; + GlobalAccelerationTelemetry global_acc; + LocalAccelerationTelemetry local_acc; + } cmd __attribute__((aligned (4))); + // 4 bytes } BasicTelemetry; -assert_size(BasicTelemetry, 12); +assert_size(BasicTelemetry, 16); diff --git a/ateam-common-packets/include/body_control.h b/ateam-common-packets/include/body_control.h index 2c9dfbc..f15d9b4 100644 --- a/ateam-common-packets/include/body_control.h +++ b/ateam-common-packets/include/body_control.h @@ -1,46 +1,29 @@ -/** - * @file body_control.h - * @author Nicholas Witten - * @brief definition for body control data types - * @version 0.1 - * - * @copyright Copyright (c) 2022 - * - */ #pragma once #include "common.h" +#include "basic_control.h" - -typedef enum BodyControlMode : uint8_t { - BCM_OFF = 0, - BCM_GLOBAL_POSE = 1, - BCM_GLOBAL_TWIST = 2, - BCM_LOCAL_TWIST = 3, - BCM_GLOBAL_ACCEL = 4, - BCM_LOCAL_ACCEL = 5 -} BodyControlMode; -assert_size(BodyControlMode, 1); - -typedef struct BodyControlTelemetry { +typedef struct BodyControlExtendedTelemetry { BodyControlMode body_control_mode; uint8_t vision_update: 1; uint8_t _reserved1: 7; uint8_t _reserved2[2]; // 4 bytes + // TODO union thing + float body_cmd[3]; // commanded body pose, twist, or accel + float imu_gyro[3]; // rad/s float imu_accel[3]; // m/s^2 float vision_pose[3]; // vision system pose measurement - float body_cmd[3]; // commanded body pose, twist, or accel - float body_traj_pose[3]; // computed trajectory pose setpoint - float body_traj_twist[3]; // computed trajectory twist setpoint - float kf_body_pose_prediction[3]; // Kalman Filter body pose prediction - float kf_body_twist_prediction[3]; // Kalman Filter body twist prediction - float kf_body_pose_estimate[3]; // Kalman Filter body pose estimate - float kf_body_twist_estimate[3]; // Kalman Filter body twist estimate - float body_twist_u[3]; // body twist control outputs after control policy + float body_traj_pos[3]; // computed trajectory position setpoint + float body_traj_vel[3]; // computed trajectory velocity setpoint + float kf_body_pos_prediction[3]; // Kalman Filter body position prediction + float kf_body_vel_prediction[3]; // Kalman Filter body velocity prediction + float kf_body_pos_estimate[3]; // Kalman Filter body position estimate + float kf_body_vel_estimate[3]; // Kalman Filter body velocity estimate + float body_vel_u[3]; // body velocity control outputs after control policy float body_accel_u[3]; // body accel control outputs after control policy float body_accel_u_fric_comp[3]; // body accel control outputs after control policy with friction compensation added in -} BodyControlTelemetry; -assert_size(BodyControlTelemetry, 4 + 13*3*4); \ No newline at end of file +} BodyControlExtendedTelemetry; +assert_size(BodyControlExtendedTelemetry, 4 + 13*3*4); \ No newline at end of file diff --git a/ateam-common-packets/include/hello_data.h b/ateam-common-packets/include/discovery.h similarity index 55% rename from ateam-common-packets/include/hello_data.h rename to ateam-common-packets/include/discovery.h index d6acc93..c435b87 100644 --- a/ateam-common-packets/include/hello_data.h +++ b/ateam-common-packets/include/discovery.h @@ -1,13 +1,3 @@ -/** - * @file hello_data.h - * @author Matthew Barulic - * @brief definition for Hello Data data type - * @version 0.1 - * - * @copyright Copyright (c) 2022 - * - */ - #pragma once #include "common.h" @@ -21,8 +11,18 @@ assert_size(TeamColor, 1); typedef struct HelloRequest { uint8_t robot_id; TeamColor color; + + uint8_t coms_repo_dirty : 1; + uint8_t controls_repo_dirty : 1; + uint8_t firmware_repo_dirty : 1; + uint8_t bitfield_reserved1 : 5; + uint8_t reserved[1]; + + uint8_t coms_hash[4]; + uint8_t controls_hash[4]; + uint8_t firmware_hash[4]; } HelloRequest; -assert_size(HelloRequest, 2); +assert_size(HelloRequest, 16); typedef struct HelloResponse { uint8_t ipv4[4]; diff --git a/ateam-common-packets/include/error_telemetry.h b/ateam-common-packets/include/error_telemetry.h index 38591e0..aa0c60f 100644 --- a/ateam-common-packets/include/error_telemetry.h +++ b/ateam-common-packets/include/error_telemetry.h @@ -1,13 +1,3 @@ -/** - * @file error_telemetry.h - * @author Nicholas Witten / Joseph Spall - * @brief Simple error messages over radio - * @version 0.1 - * - * @copyright Copyright (c) 2022 - * - */ - #pragma once #include "common.h" diff --git a/ateam-common-packets/include/extended_telemetry.h b/ateam-common-packets/include/extended_telemetry.h index 7949bb1..452124e 100644 --- a/ateam-common-packets/include/extended_telemetry.h +++ b/ateam-common-packets/include/extended_telemetry.h @@ -1,13 +1,3 @@ -/** - * @file extended_telemetry.h - * @author Will Stuckey - * @brief definition for controls debug telemetry data type - * @version 0.1 - * - * @copyright Copyright (c) 2023 - * - */ - #pragma once #include "common.h" @@ -21,7 +11,7 @@ typedef struct ExtendedTelemetry { uint32_t timestamp_us_hi; // 8 bytes - // PowerTelemetry power_status; + PowerTelemetry power_status; // 28 bytes CcmTelemetry front_left_motor; @@ -30,10 +20,10 @@ typedef struct ExtendedTelemetry { CcmTelemetry front_right_motor; // 60 bytes each - // KickerTelemetry kicker_status; - // 64 bytes - - BodyControlTelemetry body_control_telemetry; + BodyControlExtendedTelemetry body_control_telemetry; // 160 bytes + + KickerTelemetry kicker_status; + // 64 bytes } ExtendedTelemetry; -assert_size(ExtendedTelemetry, 8 + 4*60 + 160); +assert_size(ExtendedTelemetry, 8 + 28 + 4*60 + 160 + 64); diff --git a/ateam-common-packets/include/kicker.h b/ateam-common-packets/include/kicker.h index 0faefe2..dd08163 100644 --- a/ateam-common-packets/include/kicker.h +++ b/ateam-common-packets/include/kicker.h @@ -3,7 +3,7 @@ #include "common.h" #include "stspin.h" -typedef enum KickRequest { +typedef enum KickRequest : uint8_t { KR_ARM, KR_DISABLE, KR_KICK_NOW, @@ -15,17 +15,17 @@ typedef enum KickRequest { } KickRequest; typedef struct KickerControl { - uint32_t telemetry_enabled: 1; - uint32_t enable_charging: 1; - uint32_t request_power_down: 1; - uint32_t dribbler_mult: 8; - // 21 bits reserved + uint16_t telemetry_enabled: 1; + uint16_t enable_charging: 1; + uint16_t request_power_down: 1; + uint16_t bitfield_reserved1: 13; + uint8_t reserved1; KickRequest kick_request; float kick_speed; float drib_speed; } KickerControl; -assert_size(KickerControl, 16); +assert_size(KickerControl, 12); typedef struct KickerTelemetry { uint16_t error_detected : 1; diff --git a/ateam-common-packets/include/radio.h b/ateam-common-packets/include/radio.h index 0e6fe66..281e501 100644 --- a/ateam-common-packets/include/radio.h +++ b/ateam-common-packets/include/radio.h @@ -1,17 +1,7 @@ -/** - * @file radio.h - * @author Austin Jones - * @brief communication packet definitions for the radio firmware - * @version 0.1 - * - * @copyright Copyright (c) 2022 - * - */ - #pragma once #include "common.h" -#include "hello_data.h" +#include "discovery.h" #include "basic_control.h" #include "basic_telemetry.h" #include "error_telemetry.h" @@ -20,36 +10,35 @@ #include "robot_parameters.h" #include "stspin_current.h" -const uint16_t kProtocolVersionMajor = 0; -const uint16_t kProtocolVersionMinor = 1; - typedef enum CommandCode : uint8_t { - // Bidirectional commands + // bidirectional commands and status CC_ACK = 1, CC_NACK = 2, CC_GOODBYE = 3, CC_KEEPALIVE = 4, - // From robot to coach - CC_HELLO_REQ = 101, - CC_TELEMETRY = 102, - CC_CONTROL_DEBUG_TELEMETRY = 103, - CC_ROBOT_PARAMETER_COMMAND = 104, - CC_ERROR_TELEMETRY = 105, - // From coach to robot - CC_CONTROL = 201, - CC_HELLO_RESP = 202, + + // discovery + CC_HELLO_REQ = 21, // robot to multicast + CC_HELLO_RESP = 22, // software to robot direct + + // from robot to coach + CC_TELEMETRY = 41, + CC_CONTROL_DEBUG_TELEMETRY = 42, + CC_ROBOT_PARAMETER_COMMAND = 43, + CC_ERROR_TELEMETRY = 44, + + // from coach to robot + CC_CONTROL = 61, } CommandCode; assert_size(CommandCode, 1); typedef struct RadioHeader { uint32_t crc32; - uint16_t major_version; - uint16_t minor_version; CommandCode command_code; - uint16_t data_length; uint8_t _reserved; + uint16_t data_length; } __attribute__((packed)) RadioHeader; -assert_size(RadioHeader, 12); +assert_size(RadioHeader, 8); typedef union RadioData { HelloRequest hello_request; @@ -59,32 +48,12 @@ typedef union RadioData { ExtendedTelemetry extended_telemetry; ParameterCommand robot_parameter_command; } RadioData; -// assert_size(RadioData, 464); -assert_size(RadioData, 408); +assert_size(RadioData, 500); // Same RadioPacket typedef struct RadioPacket { - uint32_t crc32; - uint16_t major_version; - uint16_t minor_version; - CommandCode command_code; - uint16_t data_length; - - // 12 bytes - - union Data { - HelloRequest hello_request; - HelloResponse hello_response; - BasicControl control; - BasicTelemetry telemetry; - ExtendedTelemetry control_debug_telemetry; - ErrorTelemetry error_telemetry; - ParameterCommand robot_parameter_command; - } data __attribute__((aligned (4))); - - // I think this should be a valid swap when we clean packet definitions in the future - // RadioData data __attribute__((aligned (4))); -} RadioPacket; -// assert_size(RadioPacket, 476); -assert_size(RadioPacket, 420); + RadioHeader header; + RadioData data ; +} RadioPacket __attribute__((aligned (4))); +assert_size(RadioPacket, 508); static_assert(sizeof(RadioPacket) <= 512); // 512 is the current size limit of an entry in the packet buffer \ No newline at end of file diff --git a/ateam-common-packets/include/robot_parameters.h b/ateam-common-packets/include/robot_parameters.h index 1a92e14..637efd8 100644 --- a/ateam-common-packets/include/robot_parameters.h +++ b/ateam-common-packets/include/robot_parameters.h @@ -1,13 +1,3 @@ -/** - * @file robot_parameters.h - * @author Will Stuckey - * @brief packet definitions for parameter reads and write - * @version 0.1 - * - * @copyright Copyright (c) 2023 - * - */ - #pragma once #include "common.h" diff --git a/ateam-common-packets/include/robot_skills/global_acceleration.h b/ateam-common-packets/include/robot_skills/global_acceleration.h new file mode 100644 index 0000000..451b466 --- /dev/null +++ b/ateam-common-packets/include/robot_skills/global_acceleration.h @@ -0,0 +1,15 @@ +#pragma once + +#include "../common.h" + +typedef struct GlobalAccelerationCommand { + float local_xdd; + float local_ydd; + float local_alpha; +} GlobalAccelerationCommand; +assert_size(GlobalAccelerationCommand, 12); + +typedef struct GlobalAccelerationTelemetry { + uint32_t reserved; +} GlobalAccelerationTelemetry; +assert_size(GlobalAccelerationTelemetry, 4); \ No newline at end of file diff --git a/ateam-common-packets/include/robot_skills/global_position.h b/ateam-common-packets/include/robot_skills/global_position.h new file mode 100644 index 0000000..ce48b78 --- /dev/null +++ b/ateam-common-packets/include/robot_skills/global_position.h @@ -0,0 +1,19 @@ +#pragma once + +#include "../common.h" + +typedef struct GlobalPositionCommand { + float global_x; + float global_y; + float global_theta; + float max_linear_vel; + float max_angular_vel; + float max_linear_acc; + float max_angular_acc; +} GlobalPositionCommand; +assert_size(GlobalPositionCommand, 28); + +typedef struct GlobalPositionTelemetry { + uint32_t reserved; +} GlobalPositionTelemetry; +assert_size(GlobalPositionTelemetry, 4); \ No newline at end of file diff --git a/ateam-common-packets/include/robot_skills/global_velocity.h b/ateam-common-packets/include/robot_skills/global_velocity.h new file mode 100644 index 0000000..5095c0b --- /dev/null +++ b/ateam-common-packets/include/robot_skills/global_velocity.h @@ -0,0 +1,17 @@ +#pragma once + +#include "../common.h" + +typedef struct GlobalVelocityCommand { + float global_xd; + float global_yd; + float global_omega; + float max_linear_acc; + float max_angular_acc; +} GlobalVelocityCommand; +assert_size(GlobalVelocityCommand, 20); + +typedef struct GlobalVelocityTelemetry { + uint32_t reserved; +} GlobalVelocityTelemetry; +assert_size(GlobalVelocityTelemetry, 4); \ No newline at end of file diff --git a/ateam-common-packets/include/robot_skills/local_acceleration.h b/ateam-common-packets/include/robot_skills/local_acceleration.h new file mode 100644 index 0000000..ad44a2f --- /dev/null +++ b/ateam-common-packets/include/robot_skills/local_acceleration.h @@ -0,0 +1,15 @@ +#pragma once + +#include "../common.h" + +typedef struct LocalAccelerationCommand { + float local_xdd; + float local_ydd; + float local_alpha; +} LocalAccelerationCommand; +assert_size(LocalAccelerationCommand, 12); + +typedef struct LocalAccelerationTelemetry { + uint32_t reserved; +} LocalAccelerationTelemetry; +assert_size(LocalAccelerationTelemetry, 4); \ No newline at end of file diff --git a/ateam-common-packets/include/robot_skills/local_velocity.h b/ateam-common-packets/include/robot_skills/local_velocity.h new file mode 100644 index 0000000..af8e418 --- /dev/null +++ b/ateam-common-packets/include/robot_skills/local_velocity.h @@ -0,0 +1,17 @@ +#pragma once + +#include "../common.h" + +typedef struct LocalVelocityCommand { + float local_xd; + float local_yd; + float local_omega; + float max_linear_acc; + float max_angular_acc; +} LocalVelocityCommand; +assert_size(LocalVelocityCommand, 20); + +typedef struct LocalVelocityTelemetry { + uint32_t reserved; +} LocalVelocityTelemetry; +assert_size(LocalVelocityTelemetry, 4); \ No newline at end of file diff --git a/ateam-common-packets/include/stspin.h b/ateam-common-packets/include/stspin.h index 9a79e87..cb73ef0 100644 --- a/ateam-common-packets/include/stspin.h +++ b/ateam-common-packets/include/stspin.h @@ -1,18 +1,3 @@ -/** - * @file stspin.h - * @author Will Stuckey - * @brief communication packet definitions for the stspin firmware - * @version 0.1 - * - * @copyright Copyright (c) 2022 - * - * Struct sizes should pass static checks on the following 3 platforms: - * - Embedded -> C11, 4 byte ptr - * - Bindgen -> C11, 8 byte ptr - * - ROS -> C++, 8 byte ptr - * - */ - #pragma once #include "common.h" diff --git a/ateam-common-packets/include/stspin_current.h b/ateam-common-packets/include/stspin_current.h index 710f6cd..5ce4b9d 100644 --- a/ateam-common-packets/include/stspin_current.h +++ b/ateam-common-packets/include/stspin_current.h @@ -1,18 +1,3 @@ -/** - * @file stspin.h - * @author Will Stuckey - * @brief communication packet definitions for the stspin firmware - * @version 0.1 - * - * @copyright Copyright (c) 2022 - * - * Struct sizes should pass static checks on the following 3 platforms: - * - Embedded -> C11, 4 byte ptr - * - Bindgen -> C11, 8 byte ptr - * - ROS -> C++, 8 byte ptr - * - */ - #pragma once #include "common.h" diff --git a/ateam-common-packets/rust-lib/build.rs b/ateam-common-packets/rust-lib/build.rs index 49af732..8d9d6c4 100644 --- a/ateam-common-packets/rust-lib/build.rs +++ b/ateam-common-packets/rust-lib/build.rs @@ -99,17 +99,17 @@ fn main() { // The input header we would like to generate // bindings for. .header("../include/radio.h") - .allowlist_file(".*/stspin.h") - .allowlist_file(".*/stspin_current.h") - .allowlist_file(".*/kicker.h") - .allowlist_file(".*/basic_control.h") - .allowlist_file(".*/basic_telemetry.h") - .allowlist_file(".*/extended_telemetry.h") - .allowlist_file(".*/hello_data.h") - .allowlist_file(".*/body_control.h") - .allowlist_file(".*/power.h") - .allowlist_file(".*/robot_parameters.h") - .allowlist_file(".*/radio.h") + // .allowlist_file(".*/stspin.h") + // .allowlist_file(".*/stspin_current.h") + // .allowlist_file(".*/kicker.h") + // .allowlist_file(".*/basic_control.h") + // .allowlist_file(".*/basic_telemetry.h") + // .allowlist_file(".*/extended_telemetry.h") + // .allowlist_file(".*/hello_data.h") + // .allowlist_file(".*/body_control.h") + // .allowlist_file(".*/power.h") + // .allowlist_file(".*/robot_parameters.h") + // .allowlist_file(".*/radio.h") .derive_default(true) .generate() // Unwrap the Result and panic on failure. diff --git a/radio-protocol/README b/radio-protocol/README deleted file mode 100644 index e4d7cf4..0000000 --- a/radio-protocol/README +++ /dev/null @@ -1,77 +0,0 @@ -# Robot Networking Protocol Specification - -**NOTE:** This specification is being collaboratively updated [here](https://docs.google.com/document/d/1O09lvcoexoYhVQzl3QtoxvIDNEamm63JrjVRY-vIe3Q). The content below may not reflect latest spec or implementation. - -This is a specification of a basic protocol for communication between RoboCup robots and a control server. -Robot-robot communication is not supported. Multiple control servers are not supported. This specification assumes only friendly devices will be present on the network. - -1. Glossary -Bot Firmware - The code that controls the Bot and manages the Client. -Client - Network software that runs on and takes commands from the Bot Firmware; exists solely to communicate with the Control Server. -Control Software - Centralized software that controls multiple Bots. -Server - Network software that runs on and takes commands from the Control Software; exists solely to communicate with Bots. -Jersey Number - Pre-assigned number between 1 and 16 that identifies Bots. -Command Data - Arbitrary data used by the Control Software to control Bots. - -2. Basic Structure - - +-----------+ RNP Commands/Data +----------+ - | Client(s) | <--------------------> | Server | - +-----------+ +----------+ -+-----------------+ +--------------------+ -| Bot(s) Firmware | | Control Software | -+-----------------+ +--------------------+ - -When a Client connects to the network it sends a broadcast 'hello' message to notify the Server that it is alive, and waits for a reply from the Server. -When the Server connects to the network, it sends a UDP broadcast message notifying any clients on the network that it is alive, and waits for 'hello's from Clients. -The Server has various commands it can receive from Clients, and various response/command codes it can send to Clients. The Server can both respond to requests from Clients, and send unprompted messages to Clients based on input from the Control Software. Control Software functionality and how the Bot Firmware controls the Client are outside the scope of this document. -Client messages follow a command/data structure; Clients will send a command, wait for an acknowledgement from the server, then send data associated with that command. Client commands will be 1-byte numbers, and data will be TBD. Server messages will follow a status code structure; the Server will send a status code, and wait for an acknowledgement from the Client if required, and then send data if required. Server messages will be 1-byte command codes, followed by data when required. - -2.1 Packet Structure -+-----------++--------------++--------------------------------------------------------------------------+ -| Major Version || Minor Version || Patch || Command Code || Data Type || Data Length || Data | -+-----------++--------------++--------------------------------------------------------------------------+ -|------- 1B ------||----- 1B ------||-- 2B -||----- 1B -----||----1B-----||----- 2B ----||---- 500B ----| - -2.2 Handshake - -2.2.1 Server sends Server Available (220) command to broadcast address after joining the network. -2.2.2 Robots already on the network send Hello (219) to Server upon receiving 220 and store the Server address in its internal database. -2.2.3 Robot sends Hello (219) command to broadcast address after joining the network. -2.2.4 Upon receiving a 219, Server sends Jersey Request (222) to the new Robot and stores the Robots address in its internal database. -2.2.5 Robot sends Jersey Send (219) with its jersey number in the data segment to the Server upon receiving the 222, and stores the Server address in its internal database. -2.2.6 Server stores the jersey number in its internal database, and sends an OK (250). -2.2.6 Robot waits to receive commands from the Server, and Server waits for keepalive pings and new Robots. - - -3. Robot Command Codes -3.1 219; Hello -Code 219 is used when a Robot first connects to the network, or when a Robot receives a 220 (Server Start) from the Server. The Client WILL send a 219 on startup after it connects to the network, or any time it receives a 220 (Server Start). The Client will then wait for a 250 (Server Acknowledge), then send a 2-bit (padded to 1B) message containing its Jersey Number, and wait for a 250 (Server Acknowledge). If the Client receives a 255 (Server Order Stop), the Client will stop all operations. The Robot will store the IP address of the Server for future communication. If a Server IP is already stored, it will replace the stored IP with the new IP. -3.2 221; Jersey Send -Code 221 is used to send the Robot's jersey number to the Server. Upon receiving a 222 from the Server, the Robot will reply with a 221, with the jersey number in the Data segment. -3.3 250; OK -Code 250 command is used to acknowledge a command or data from the Server. Upon receiving a command code from the Server or data from the Server (as a 125 (Server Send)), Client will send a 250 indicating it understood and complied with the command, or received the data. -3.4 251; Shutting Down -Code 251 command is used when a Robot is shutting down, based on input from the Bot Firmware. When ordered to shut down by the Bot Firmware, the Robot will send a 251 to the Server and shut down. Upon receiving a 251 from a Client, the Server will remove the Client from its internal database. -3.5 100; Keepalive -Code 100 command is used to send keepalive pings to the Server. See Section 5 for more information. -3.6 126; Robot Send -Code 126 is used to notify the Server that the Robot is sending it data. Upon receipt, the Server will send a 250. - - -4. Server Command Codes -4.1. 125; Server Send -Code 125 is used to notify a Robot that the Server is sending it data. When ordered by the Control Software to send a Robot data, the Server will send a 125 to the Robot with up to 506 bytes in the data segment. Upon receipt, the Robot will send a 250. -4.2 220; Server Available -Code 220 is used to notify Robots that the Server is available. The Server will send a 220 to the broadcast address on startup, and wait for Robots to send 220s. When the Server receives a 220 from a Client, it will send a 250 to the Client, then wait for the Client to send its desired Jersey Number. The Server will check its internal database to ensure the Jersey Number is not taken, and send a 250. If the Jersey Number is taken, it will send a 502 to the requesting Client, and to the Client to which the Jersey Number is assigned. -4.3 222; Jersey Request -Code 222 is used to request a Robot's jersey number. The Server will send a 222 after receiving a 219 from a Robot, and populate its internal database with the jersey number and associate it with the sending Robot. If there is a conflict, the Server will send a 255 to the conflicting Robots. -4.4 250; OK -Code 250 is used to acknowledge a message from a Robot. When receiving a message from a Robot that warrants a response as otherwise described in this document, it will send a 250 to the Client. -4.5 254; Invalid Command -Code 254 is used to notify a Robot it sent an invalid command. When receiving an invalid command from a Robot, the Server will send a 501 to the Robot. -4.6 255; Error Stop -Code 255 is used to order a Robot to cease operations based on an error. When the Server detects an error (such as a Jersey Number conflict) it will send a 255 to the Robot, which will cease operating. Resolving errors is outside the scope of this document. - -5. Keepalive -A keepalive system is used to ensure Robots are active, and to prune inactive Robots from the Server's internal database. Robots will send a 100 to the Server based on a pre-determined timeout agreed upon by the Bot and the Control Software, which is outside the scope of this document. The Server will update the Robot's keepalive every time it receives a message from the Robot, and will remove the Robot from its database if the keepalive expires past the timeout without receiving a message from the Robot. If the Robot's keepalive expires, it will need to send a new 219 if it reconnects to the network. \ No newline at end of file diff --git a/radio-protocol/ai.py b/radio-protocol/ai.py deleted file mode 100644 index 3a2cf44..0000000 --- a/radio-protocol/ai.py +++ /dev/null @@ -1,153 +0,0 @@ -import socket, time, threading, random, logging - - -class Ai(): - - def __init__(self): - self._host = '172.25.100.221' - self._srvport = 8081 - self._cliport = 8080 - self._timeout = 30 - self._robots = [] - self._lock = threading.Lock() - self._data_size = 508 - self._socket = None - - with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as s: - self._socket = s - self._socket.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1) - self._socket.bind(("", self._srvport)) - data = bytearray(self._data_size) - data[0] = 220 - order_thread = threading.Thread(target=self.giveOrders) - order_thread.start() - keepalive_thread = threading.Thread(target=self.checkKeepalives) - keepalive_thread.start() - with self._lock: - self._socket.sendto(data, ('255.255.255.255', self._cliport)) - logging.debug(f"Sent hello to broadcast") - while True: - data, addr = s.recvfrom(self._data_size+8) - handle_thread = threading.Thread(target=self.handleRobot, args=(data, addr)) - handle_thread.start() - - def setKeepalive(self, addr): - for r in self._robots: - if r['addr'] == (addr[0], self._cliport): - with self._lock: - r['keepalive'] = time.time() - logging.debug(f"Set keepalive for {(addr[0], self._cliport)}") - - def handleRobot(self, d, addr): - data = bytearray(d) - cmd = data[0] - ret = bytearray(self._data_size) - addr = (addr[0], self._cliport) - logging.debug(f"Got traffic from {addr} {data[0]} {data[1]}") - - # Handle client Hello - if cmd == 219: - r = {'addr': addr, 'jersey': 0, 'keepalive': time.time()} - if r not in self._robots: - with self._lock: - self._robots.append(r) - logging.debug(f"Added {r} to robot list") - else: - logging.debug(f"Got hello for {r} I already knew about") - ret[0] = 222 - - # Handle client jersey number - elif cmd == 221: - isIn = False - for r in self._robots: - if r['addr'] == addr: - with self._lock: - r['jersey'] = data[1] - logging.debug(f"Set jersey to {data[1]} for {r['addr']}") - isIn = True - if not isIn: - r = {'addr': addr, 'jersey': data[1], 'keepalive': time.time()} - with self._lock: - self._robots.append(r) - logging.debug(f"Added new robot {r}") - fail = False - for r in self._robots: - if r['jersey'] == data[1] and r['addr'] != addr: - fail = True - ret[0] = 255 - e = bytearray(256) - e[0] = 255 - with self._lock: - self._socket.sendto(e, r['addr']) # Notify conflicting bot - logging.debug(f"Found conflicting jersey number {data[1]} between {r} and {addr}") - if not fail: - ret[0] = 250 - - # Handle client ACK - elif cmd == 250: - isIn = False - for r in self._robots: - if r['addr'] == addr: - isIn = True - self.setKeepalive(addr) - if not isIn: - with self._lock: - self._robots.append({"addr": addr, 'journal': 0, 'timeout': time.time()}) - ret[0] = 222 - else: - logging.debug(f"Got ack from {addr}") - return # Do nothing and don't reply - - # Handle client shutdown - elif cmd == 251: - for r in self._robots: - if r['addr'] == addr and r['jersey'] == data[1]: - with self._lock: - self._robots.remove(r) - ret[0] = 250 - self.setKeepalive(addr) - logging.debug(f"{addr} is shutting down") - - # Handle client keepalive - elif cmd == 100: - ret[0] = 250 - self.setKeepalive(addr) - logging.debug(f"Got keepalive from {addr}") - - # Handle unsupported command - else: - ret[0] = 254 - self.setKeepalive(addr) - logging.debug(f"Got unsupported command {cmd} from {addr}") - - with self._lock: - self._socket.sendto(ret, addr) - logging.debug(f"Sent {ret[0]} to {addr}") - - def giveOrders(self): - # Wait for init - while self._robots == []: - logging.debug("Waiting for robots....") - time.sleep(1) - - # Send random data - for j in range(10): - ret = bytearray(self._data_size) - ret[0] = 125 - for i in range(1,self._data_size-1): - ret[i] = random.getrandbits(8) - with self._lock: - self._socket.sendto(ret, self._robots[0]['addr']) - - def checkKeepalives(self): - while True: - for r in self._robots: - if time.time() - r['keepalive'] > 30: - with self._lock: - self._robots.remove(r) - logging.debug(f"Pruned expired bot {r}") - time.sleep(self._timeout / 6) - -logging.basicConfig(format='%(asctime)s %(message)s', level=logging.DEBUG) - -a = Ai() \ No newline at end of file diff --git a/radio-protocol/robot.py b/radio-protocol/robot.py deleted file mode 100644 index 4f4b6c0..0000000 --- a/radio-protocol/robot.py +++ /dev/null @@ -1,101 +0,0 @@ -import socket, time, threading, logging - -class Robot(): - - def __init__(self): - self._host = '172.25.100.221' - self._cliport = 8080 - self._srvport = 8081 - self._timeout = 30 - self._jersey = 1 - self._keepalive = 0 - self._server = () - self._lock = threading.Lock() - self._data_size = 508 - self._socket = None - - with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as s: - s.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1) - s.bind(("", self._cliport)) - self._socket = s - data = bytearray(self._data_size) - data[0] = 219 - with self._lock: - self._socket.sendto(data, ('255.255.255.255', self._srvport)) - logging.debug("Sent hello to broadcast") - keepalive_thread = threading.Thread(target=self.keepalive) - keepalive_thread.start() - while True: - data, addr = s.recvfrom(self._data_size + 8) - handle_thread = threading.Thread(target=self.handle, args=(data, addr)) - handle_thread.start() - - def handle(self, d, addr): - s = self._socket - if not self._server: - self._server = ('0.0.0.0', self._srvport) - data = bytearray(d) - cmd = data[0] - logging.debug(f"Received {cmd} from {addr}") - ret = bytearray(self._data_size) - - # Handle server ACK - if cmd == 250: - logging.debug(f"Received server ack") - - # Handle server hello - elif cmd == 220: - ret[0] = 250 - with self._lock: - self._server = (addr[0], self._srvport) - logging.debug(f"Received hello from {self._server}") - self._keepalive = time.time() - s.sendto(ret, self._server) - - elif cmd == 222: - ret[0] = 221 - ret[1] = self._jersey - with self._lock: - self._server = (addr[0], self._srvport) - print(self._server) - logging.debug(f"Provided jersey number {self._jersey} to {self._server}") - self._keepalive = time.time() - s.sendto(ret, self._server) - - elif cmd == 125: - ret[0] = 250 - data.pop(0) - raw = bytes(data) - logging.debug(f"Received {raw}") - with self._lock: - self._keepalive = time.time() - s.sendto(ret, self._server) - - elif cmd == 255: - ret[0] == 250 - logging.debug(f"Received server stop") - with self._lock: - self._keepalive = time.time() - s.sendto(ret, self._server) - - else: - logging.debug(f"Ignoring invalid traffic {cmd} from {addr}") - - def keepalive(self): - s = self._socket - while True: - if self._server == (): - time.sleep(self._timeout / 6) - continue - with self._lock: - if time.time() - self._keepalive > 30: - data = bytearray(self._data_size) - data[0] = 100 - s.sendto(data, self._server) - logging.debug(f"Sent keepalive") - time.sleep(self._timeout / 6) - -logging.basicConfig(format='%(asctime)s %(message)s', level=logging.DEBUG) - -r = Robot() - \ No newline at end of file From 3d175f928a5d96c50f9f0d1bc5245ee44ec316ec Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Tue, 21 Apr 2026 21:53:46 -0400 Subject: [PATCH 31/44] cleanup build.rs --- ateam-common-packets/rust-lib/build.rs | 11 ----------- 1 file changed, 11 deletions(-) diff --git a/ateam-common-packets/rust-lib/build.rs b/ateam-common-packets/rust-lib/build.rs index 8d9d6c4..935b3e2 100644 --- a/ateam-common-packets/rust-lib/build.rs +++ b/ateam-common-packets/rust-lib/build.rs @@ -99,17 +99,6 @@ fn main() { // The input header we would like to generate // bindings for. .header("../include/radio.h") - // .allowlist_file(".*/stspin.h") - // .allowlist_file(".*/stspin_current.h") - // .allowlist_file(".*/kicker.h") - // .allowlist_file(".*/basic_control.h") - // .allowlist_file(".*/basic_telemetry.h") - // .allowlist_file(".*/extended_telemetry.h") - // .allowlist_file(".*/hello_data.h") - // .allowlist_file(".*/body_control.h") - // .allowlist_file(".*/power.h") - // .allowlist_file(".*/robot_parameters.h") - // .allowlist_file(".*/radio.h") .derive_default(true) .generate() // Unwrap the Result and panic on failure. From 2f858a8029d7c19f4871d98ae1070769c6849bc1 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Tue, 21 Apr 2026 22:32:15 -0400 Subject: [PATCH 32/44] add packet safety, minor fixes --- .../include/basic_telemetry.h | 2 +- ateam-common-packets/rust-lib/src/lib.rs | 63 +++++++++++++++++++ 2 files changed, 64 insertions(+), 1 deletion(-) diff --git a/ateam-common-packets/include/basic_telemetry.h b/ateam-common-packets/include/basic_telemetry.h index c477ca1..d7a0681 100644 --- a/ateam-common-packets/include/basic_telemetry.h +++ b/ateam-common-packets/include/basic_telemetry.h @@ -53,7 +53,7 @@ typedef struct BasicTelemetry { LocalVelocityTelemetry local_vel; GlobalAccelerationTelemetry global_acc; LocalAccelerationTelemetry local_acc; - } cmd __attribute__((aligned (4))); + } control_telem __attribute__((aligned (4))); // 4 bytes } BasicTelemetry; assert_size(BasicTelemetry, 16); diff --git a/ateam-common-packets/rust-lib/src/lib.rs b/ateam-common-packets/rust-lib/src/lib.rs index 0acfdd3..9d5c13e 100644 --- a/ateam-common-packets/rust-lib/src/lib.rs +++ b/ateam-common-packets/rust-lib/src/lib.rs @@ -2,6 +2,69 @@ #![allow(non_upper_case_globals)] #![allow(non_camel_case_types)] #![allow(non_snake_case)] +#![feature(const_cmp)] + +use crate::bindings::{BasicControl, BodyControlMode, ExtendedTelemetry, GlobalAccelerationCommand, GlobalPositionCommand, GlobalVelocityCommand, LocalAccelerationCommand, LocalVelocityCommand, RadioHeader}; pub mod bindings; pub mod radio; + +// TODO these are assuming the biggest packet doesn't change... const core::cmp::max is still unstable. We +// can fix this on the next major rust version update. +pub const MAX_ROBOT_RX_PACKET_SIZE: usize = core::mem::size_of::() + core::mem::size_of::(); +pub const MAX_ROBOT_TX_PACKET_SIZE: usize = core::mem::size_of::() + core::mem::size_of::(); + +pub fn is_bcm_global_position_safe(gpc: &GlobalPositionCommand) -> bool { + gpc.global_x.is_finite() + && gpc.global_y.is_finite() + && gpc.global_theta.is_finite() + && gpc.max_linear_vel.is_finite() + && gpc.max_angular_vel.is_finite() + && gpc.max_linear_acc.is_finite() + && gpc.max_angular_acc.is_finite() +} + +pub fn is_bcm_global_vel_safe(gpc: &GlobalVelocityCommand) -> bool { + gpc.global_xd.is_finite() + && gpc.global_yd.is_finite() + && gpc.global_omega.is_finite() + && gpc.max_linear_acc.is_finite() + && gpc.max_angular_acc.is_finite() +} + +pub fn is_bcm_local_vel_safe(gpc: &LocalVelocityCommand) -> bool { + gpc.local_xd.is_finite() + && gpc.local_yd.is_finite() + && gpc.local_omega.is_finite() + && gpc.max_linear_acc.is_finite() + && gpc.max_angular_acc.is_finite() +} + +pub fn is_bcm_global_accel_safe(gpc: &GlobalAccelerationCommand) -> bool { + gpc.local_xdd.is_finite() + && gpc.local_ydd.is_finite() + && gpc.local_alpha.is_finite() +} + +pub fn is_bcm_local_accel_safe(gpc: &LocalAccelerationCommand) -> bool { + gpc.local_xdd.is_finite() + && gpc.local_ydd.is_finite() + && gpc.local_alpha.is_finite() +} + +pub fn is_basic_control_packet_safe(basic_control: &BasicControl) -> bool { + let vision_update_safe = basic_control.vision_position_update.iter().all(|vis| vis.is_finite()); + let kicker_command_safe = basic_control.kick_vel.is_finite() && basic_control.dribbler_speed.is_finite(); + + let body_control_command_safe = match basic_control.body_control_mode { + BodyControlMode::BCM_OFF => true, + BodyControlMode::BCM_GLOBAL_POSITION => is_bcm_global_position_safe(unsafe { &basic_control.cmd.global_pos }), + BodyControlMode::BCM_GLOBAL_VELOCITY => is_bcm_global_vel_safe(unsafe { &basic_control.cmd.global_vel }), + BodyControlMode::BCM_LOCAL_VELOCITY => is_bcm_local_vel_safe(unsafe { &basic_control.cmd.local_vel }), + BodyControlMode::BCM_GLOBAL_ACCEL => is_bcm_global_accel_safe(unsafe { &basic_control.cmd.global_acc }), + BodyControlMode::BCM_LOCAL_ACCEL => is_bcm_local_accel_safe(unsafe { &basic_control.cmd.local_acc }), + _ => false + }; + + return vision_update_safe && kicker_command_safe && body_control_command_safe; +} \ No newline at end of file From 1dee4e02c63bcfbf016e84e5cb6ab54cd7b8cc7f Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Tue, 21 Apr 2026 22:51:00 -0400 Subject: [PATCH 33/44] extract skills unions to defined types --- ateam-common-packets/include/basic_control.h | 17 ++++++++++------- .../include/basic_telemetry.h | 19 +++++++++++-------- 2 files changed, 21 insertions(+), 15 deletions(-) diff --git a/ateam-common-packets/include/basic_control.h b/ateam-common-packets/include/basic_control.h index fea1102..9d79e84 100644 --- a/ateam-common-packets/include/basic_control.h +++ b/ateam-common-packets/include/basic_control.h @@ -20,6 +20,15 @@ typedef enum BodyControlMode : uint8_t { } BodyControlMode; assert_size(BodyControlMode, 1); +typedef union BodyControlCommand { + GlobalPositionCommand global_pos; + GlobalVelocityCommand global_vel; + LocalVelocityCommand local_vel; + GlobalAccelerationCommand global_acc; + LocalAccelerationCommand local_acc; +} BodyControlCommand __attribute__((aligned (4))); +assert_size(BodyControlCommand, 28); + typedef struct BasicControl { // Bit field flags uint32_t request_shutdown : 1; @@ -49,13 +58,7 @@ typedef struct BasicControl { // 8 bytes // Body control command - union BodyControlCommand { - GlobalPositionCommand global_pos; - GlobalVelocityCommand global_vel; - LocalVelocityCommand local_vel; - GlobalAccelerationCommand global_acc; - LocalAccelerationCommand local_acc; - } cmd __attribute__((aligned (4))); + BodyControlCommand cmd; // 28 bytes } BasicControl; assert_size(BasicControl, 4 + 4 + 12 + 28 + 8); diff --git a/ateam-common-packets/include/basic_telemetry.h b/ateam-common-packets/include/basic_telemetry.h index d7a0681..fee03da 100644 --- a/ateam-common-packets/include/basic_telemetry.h +++ b/ateam-common-packets/include/basic_telemetry.h @@ -9,6 +9,15 @@ #include "robot_skills/global_acceleration.h" #include "robot_skills/local_acceleration.h" +typedef union BodyControlTelemetry { + GlobalPositionTelemetry global_pos; + GlobalVelocityTelemetry global_vel; + LocalVelocityTelemetry local_vel; + GlobalAccelerationTelemetry global_acc; + LocalAccelerationTelemetry local_acc; +} BodyControlTelemetry __attribute__((aligned (4))); +assert_size(BodyControlTelemetry, 4); + typedef struct BasicTelemetry { uint8_t transmission_sequence_number; uint8_t control_data_sequence_number; @@ -46,14 +55,8 @@ typedef struct BasicTelemetry { uint16_t battery_percent; uint16_t kicker_charge_percent; - // Body control command - union BodyControlTelemetry { - GlobalPositionTelemetry global_pos; - GlobalVelocityTelemetry global_vel; - LocalVelocityTelemetry local_vel; - GlobalAccelerationTelemetry global_acc; - LocalAccelerationTelemetry local_acc; - } control_telem __attribute__((aligned (4))); + // Body control command + BodyControlTelemetry control_telem; // 4 bytes } BasicTelemetry; assert_size(BasicTelemetry, 16); From 23e1aa3b8fb587c5191c429b1683bce1b853e95c Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Tue, 21 Apr 2026 23:47:30 -0400 Subject: [PATCH 34/44] fix global accel names --- .../include/robot_skills/global_acceleration.h | 6 +++--- ateam-common-packets/rust-lib/src/lib.rs | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/ateam-common-packets/include/robot_skills/global_acceleration.h b/ateam-common-packets/include/robot_skills/global_acceleration.h index 451b466..250faff 100644 --- a/ateam-common-packets/include/robot_skills/global_acceleration.h +++ b/ateam-common-packets/include/robot_skills/global_acceleration.h @@ -3,9 +3,9 @@ #include "../common.h" typedef struct GlobalAccelerationCommand { - float local_xdd; - float local_ydd; - float local_alpha; + float global_xdd; + float global_ydd; + float global_alpha; } GlobalAccelerationCommand; assert_size(GlobalAccelerationCommand, 12); diff --git a/ateam-common-packets/rust-lib/src/lib.rs b/ateam-common-packets/rust-lib/src/lib.rs index 9d5c13e..d4853d2 100644 --- a/ateam-common-packets/rust-lib/src/lib.rs +++ b/ateam-common-packets/rust-lib/src/lib.rs @@ -41,9 +41,9 @@ pub fn is_bcm_local_vel_safe(gpc: &LocalVelocityCommand) -> bool { } pub fn is_bcm_global_accel_safe(gpc: &GlobalAccelerationCommand) -> bool { - gpc.local_xdd.is_finite() - && gpc.local_ydd.is_finite() - && gpc.local_alpha.is_finite() + gpc.global_xdd.is_finite() + && gpc.global_ydd.is_finite() + && gpc.global_alpha.is_finite() } pub fn is_bcm_local_accel_safe(gpc: &LocalAccelerationCommand) -> bool { From ea1eaefb9a0a734b2f170cc0d368fc5448dc757e Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Wed, 22 Apr 2026 00:03:53 -0400 Subject: [PATCH 35/44] add extended skill telem --- ateam-common-packets/include/body_control.h | 13 ++++++++++--- ateam-common-packets/include/extended_telemetry.h | 2 +- ateam-common-packets/include/radio.h | 6 +++--- .../include/robot_skills/global_acceleration.h | 7 ++++++- .../include/robot_skills/global_position.h | 7 ++++++- .../include/robot_skills/global_velocity.h | 7 ++++++- .../include/robot_skills/local_acceleration.h | 7 ++++++- .../include/robot_skills/local_velocity.h | 7 ++++++- 8 files changed, 44 insertions(+), 12 deletions(-) diff --git a/ateam-common-packets/include/body_control.h b/ateam-common-packets/include/body_control.h index f15d9b4..1073261 100644 --- a/ateam-common-packets/include/body_control.h +++ b/ateam-common-packets/include/body_control.h @@ -3,6 +3,14 @@ #include "common.h" #include "basic_control.h" +typedef union BodyControlSkillExtendedTelemetry { + ExtendedGlobalPositionTelemetry global_pos; + ExtendedGlobalVelocityTelemetry global_vel; + ExtendedLocalVelocityTelemetry local_vel; + ExtendedGlobalAccelerationTelemetry global_acc; + ExtendedLocalAccelerationTelemetry local_acc; +} BodyControlSkillExtendedTelemetry __attribute__((aligned (4))); + typedef struct BodyControlExtendedTelemetry { BodyControlMode body_control_mode; uint8_t vision_update: 1; @@ -10,8 +18,7 @@ typedef struct BodyControlExtendedTelemetry { uint8_t _reserved2[2]; // 4 bytes - // TODO union thing - float body_cmd[3]; // commanded body pose, twist, or accel + BodyControlSkillExtendedTelemetry skill; float imu_gyro[3]; // rad/s float imu_accel[3]; // m/s^2 @@ -26,4 +33,4 @@ typedef struct BodyControlExtendedTelemetry { float body_accel_u[3]; // body accel control outputs after control policy float body_accel_u_fric_comp[3]; // body accel control outputs after control policy with friction compensation added in } BodyControlExtendedTelemetry; -assert_size(BodyControlExtendedTelemetry, 4 + 13*3*4); \ No newline at end of file +assert_size(BodyControlExtendedTelemetry, 4 + 28 + 12*3*4); \ No newline at end of file diff --git a/ateam-common-packets/include/extended_telemetry.h b/ateam-common-packets/include/extended_telemetry.h index 452124e..d187727 100644 --- a/ateam-common-packets/include/extended_telemetry.h +++ b/ateam-common-packets/include/extended_telemetry.h @@ -26,4 +26,4 @@ typedef struct ExtendedTelemetry { KickerTelemetry kicker_status; // 64 bytes } ExtendedTelemetry; -assert_size(ExtendedTelemetry, 8 + 28 + 4*60 + 160 + 64); +assert_size(ExtendedTelemetry, 8 + 28 + 4*60 + 176 + 64); diff --git a/ateam-common-packets/include/radio.h b/ateam-common-packets/include/radio.h index 281e501..e153cfa 100644 --- a/ateam-common-packets/include/radio.h +++ b/ateam-common-packets/include/radio.h @@ -48,12 +48,12 @@ typedef union RadioData { ExtendedTelemetry extended_telemetry; ParameterCommand robot_parameter_command; } RadioData; -assert_size(RadioData, 500); +assert_size(RadioData, 516); // Same RadioPacket typedef struct RadioPacket { RadioHeader header; RadioData data ; } RadioPacket __attribute__((aligned (4))); -assert_size(RadioPacket, 508); -static_assert(sizeof(RadioPacket) <= 512); // 512 is the current size limit of an entry in the packet buffer \ No newline at end of file +assert_size(RadioPacket, 524); +static_assert(sizeof(RadioPacket) <= 576); // 576 is the current size limit of an entry in the packet buffer \ No newline at end of file diff --git a/ateam-common-packets/include/robot_skills/global_acceleration.h b/ateam-common-packets/include/robot_skills/global_acceleration.h index 250faff..53c9527 100644 --- a/ateam-common-packets/include/robot_skills/global_acceleration.h +++ b/ateam-common-packets/include/robot_skills/global_acceleration.h @@ -12,4 +12,9 @@ assert_size(GlobalAccelerationCommand, 12); typedef struct GlobalAccelerationTelemetry { uint32_t reserved; } GlobalAccelerationTelemetry; -assert_size(GlobalAccelerationTelemetry, 4); \ No newline at end of file +assert_size(GlobalAccelerationTelemetry, 4); + +typedef struct ExtendedGlobalAccelerationTelemetry { + GlobalAccelerationCommand cmd_echo; +} ExtendedGlobalAccelerationTelemetry; +assert_size(ExtendedGlobalAccelerationTelemetry, 12); \ No newline at end of file diff --git a/ateam-common-packets/include/robot_skills/global_position.h b/ateam-common-packets/include/robot_skills/global_position.h index ce48b78..eac6c58 100644 --- a/ateam-common-packets/include/robot_skills/global_position.h +++ b/ateam-common-packets/include/robot_skills/global_position.h @@ -16,4 +16,9 @@ assert_size(GlobalPositionCommand, 28); typedef struct GlobalPositionTelemetry { uint32_t reserved; } GlobalPositionTelemetry; -assert_size(GlobalPositionTelemetry, 4); \ No newline at end of file +assert_size(GlobalPositionTelemetry, 4); + +typedef struct ExtendedGlobalPositionTelemetry { + GlobalPositionCommand cmd_echo; +} ExtendedGlobalPositionTelemetry; +assert_size(ExtendedGlobalPositionTelemetry, 28); \ No newline at end of file diff --git a/ateam-common-packets/include/robot_skills/global_velocity.h b/ateam-common-packets/include/robot_skills/global_velocity.h index 5095c0b..98a3589 100644 --- a/ateam-common-packets/include/robot_skills/global_velocity.h +++ b/ateam-common-packets/include/robot_skills/global_velocity.h @@ -14,4 +14,9 @@ assert_size(GlobalVelocityCommand, 20); typedef struct GlobalVelocityTelemetry { uint32_t reserved; } GlobalVelocityTelemetry; -assert_size(GlobalVelocityTelemetry, 4); \ No newline at end of file +assert_size(GlobalVelocityTelemetry, 4); + +typedef struct ExtendedGlobalVelocityTelemetry { + GlobalVelocityCommand cmd_echo; +} ExtendedGlobalVelocityTelemetry; +assert_size(ExtendedGlobalVelocityTelemetry, 20); \ No newline at end of file diff --git a/ateam-common-packets/include/robot_skills/local_acceleration.h b/ateam-common-packets/include/robot_skills/local_acceleration.h index ad44a2f..2132d7f 100644 --- a/ateam-common-packets/include/robot_skills/local_acceleration.h +++ b/ateam-common-packets/include/robot_skills/local_acceleration.h @@ -12,4 +12,9 @@ assert_size(LocalAccelerationCommand, 12); typedef struct LocalAccelerationTelemetry { uint32_t reserved; } LocalAccelerationTelemetry; -assert_size(LocalAccelerationTelemetry, 4); \ No newline at end of file +assert_size(LocalAccelerationTelemetry, 4); + +typedef struct ExtendedLocalAccelerationTelemetry { + LocalAccelerationCommand cmd_echo; +} ExtendedLocalAccelerationTelemetry; +assert_size(ExtendedLocalAccelerationTelemetry, 12); \ No newline at end of file diff --git a/ateam-common-packets/include/robot_skills/local_velocity.h b/ateam-common-packets/include/robot_skills/local_velocity.h index af8e418..12a3745 100644 --- a/ateam-common-packets/include/robot_skills/local_velocity.h +++ b/ateam-common-packets/include/robot_skills/local_velocity.h @@ -14,4 +14,9 @@ assert_size(LocalVelocityCommand, 20); typedef struct LocalVelocityTelemetry { uint32_t reserved; } LocalVelocityTelemetry; -assert_size(LocalVelocityTelemetry, 4); \ No newline at end of file +assert_size(LocalVelocityTelemetry, 4); + +typedef struct ExtendedLocalVelocityTelemetry { + LocalVelocityCommand cmd_echo; +} ExtendedLocalVelocityTelemetry; +assert_size(ExtendedLocalVelocityTelemetry, 20); \ No newline at end of file From fac979c4b5ee5768e54a9d02d6be9b94075b002d Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sun, 26 Apr 2026 21:58:38 -0400 Subject: [PATCH 36/44] add error telemetry support, add is_safe helper for data packet --- ateam-common-packets/include/radio.h | 1 + ateam-common-packets/rust-lib/src/radio.rs | 9 ++++++++- 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/ateam-common-packets/include/radio.h b/ateam-common-packets/include/radio.h index e153cfa..150acb5 100644 --- a/ateam-common-packets/include/radio.h +++ b/ateam-common-packets/include/radio.h @@ -47,6 +47,7 @@ typedef union RadioData { BasicTelemetry telemetry; ExtendedTelemetry extended_telemetry; ParameterCommand robot_parameter_command; + ErrorTelemetry error_telemetry; } RadioData; assert_size(RadioData, 516); diff --git a/ateam-common-packets/rust-lib/src/radio.rs b/ateam-common-packets/rust-lib/src/radio.rs index cfa80c0..6795bf9 100644 --- a/ateam-common-packets/rust-lib/src/radio.rs +++ b/ateam-common-packets/rust-lib/src/radio.rs @@ -1,4 +1,4 @@ -use crate::bindings::{BasicControl, ParameterCommand}; +use crate::{bindings::{BasicControl, ParameterCommand}, is_basic_control_packet_safe}; #[derive(Copy, Clone)] pub enum DataPacket { @@ -20,4 +20,11 @@ pub enum ParameterType { Extended(crate::bindings::ExtendedTelemetry), ParameterCommandResponse(crate::bindings::ParameterCommand), ErrorTelemetry(crate::bindings::ErrorTelemetry), +} + +pub fn is_data_packet_safe(data_packet: &DataPacket) -> bool { + match data_packet { + DataPacket::BasicControl(basic_control) => is_basic_control_packet_safe(basic_control), + DataPacket::ParameterCommand(_parameter_command) => true, + } } \ No newline at end of file From b969e32db321430eb058c72444e161ec3ae7e084 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Tue, 28 Apr 2026 23:25:17 -0400 Subject: [PATCH 37/44] add union helper enums --- ateam-common-packets/rust-lib/Cargo.lock | 571 ++++++++++++++++++--- ateam-common-packets/rust-lib/Cargo.toml | 4 + ateam-common-packets/rust-lib/src/lib.rs | 2 +- ateam-common-packets/rust-lib/src/radio.rs | 173 ++++++- 4 files changed, 681 insertions(+), 69 deletions(-) diff --git a/ateam-common-packets/rust-lib/Cargo.lock b/ateam-common-packets/rust-lib/Cargo.lock index a866fb8..f1fe255 100644 --- a/ateam-common-packets/rust-lib/Cargo.lock +++ b/ateam-common-packets/rust-lib/Cargo.lock @@ -1,21 +1,31 @@ # This file is automatically @generated by Cargo. # It is not intended for manual editing. -version = 3 +version = 4 [[package]] name = "aho-corasick" -version = "0.7.18" +version = "1.1.4" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1e37cfd5e7657ada45f742d6e99ca5788580b5c529dc78faf11ece6dc702656f" +checksum = "ddd31a130427c27518df266943a5308ed92d4b226cc639f5a8f1002816174301" dependencies = [ "memchr", ] +[[package]] +name = "approx" +version = "0.5.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cab112f0a86d568ea0e627cc1d6be74a1e9cd55214684db5561995f6dad897c6" +dependencies = [ + "num-traits", +] + [[package]] name = "ateam-common-packets" version = "1.0.0" dependencies = [ "bindgen", + "nalgebra", "which", ] @@ -32,9 +42,9 @@ dependencies = [ [[package]] name = "autocfg" -version = "1.1.0" +version = "1.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d468802bab17cbc0cc575e9b053f41e72aa36bfa6b7f55e3529ffa43161b97fa" +checksum = "c08606f8c3cbf4ce6ec8e28fb0014a2c086708fe954eaa885384a6165172e7e8" [[package]] name = "bindgen" @@ -42,7 +52,7 @@ version = "0.60.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "062dddbc1ba4aca46de6338e2bf87771414c335f7b2f2036e8f3e9befebf88e6" dependencies = [ - "bitflags", + "bitflags 1.3.2", "cexpr", "clang-sys", "clap", @@ -65,6 +75,12 @@ version = "1.3.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "bef38d45163c2f1dde094a7dfd33ccf595c92905c8f8f4fdc18d06fb1037718a" +[[package]] +name = "bitflags" +version = "2.11.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c4512299f36f043ab09a583e57bceb5a5aab7a73db1805848e8fef3c9e8c78b3" + [[package]] name = "cexpr" version = "0.6.0" @@ -76,15 +92,15 @@ dependencies = [ [[package]] name = "cfg-if" -version = "1.0.0" +version = "1.0.4" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd" +checksum = "9330f8b2ff13f34540b44e946ef35111825727b38d33286ef986142615121801" [[package]] name = "clang-sys" -version = "1.3.3" +version = "1.8.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5a050e2153c5be08febd6734e29298e844fdb0fa21aeddd63b4eb7baa106c69b" +checksum = "0b023947811758c97c59bf9d1c188fd619ad4718dcaa767947df1cadb14f39f4" dependencies = [ "glob", "libc", @@ -93,12 +109,12 @@ dependencies = [ [[package]] name = "clap" -version = "3.2.12" +version = "3.2.25" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ab8b79fe3946ceb4a0b1c080b4018992b8d27e9ff363644c1c9b6387c854614d" +checksum = "4ea181bf566f71cb9a5d17a59e1871af638180a18fb0035c92ae62b705207123" dependencies = [ "atty", - "bitflags", + "bitflags 1.3.2", "clap_lex", "indexmap", "strsim", @@ -117,15 +133,15 @@ dependencies = [ [[package]] name = "either" -version = "1.7.0" +version = "1.15.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3f107b87b6afc2a64fd13cac55fe06d6c8859f12d4b14cbcdd2c67d0976781be" +checksum = "48c757948c5ede0e46177b7add2e67155f70e33c07fea8284df6576da70b3719" [[package]] name = "env_logger" -version = "0.9.0" +version = "0.9.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0b2cf0344971ee6c64c31be0d530793fba457d322dfec2810c453d0ef228f9c3" +checksum = "a12e6657c4c97ebab115a42dcee77225f7f482cdd841cf7088c657a42e9e00e7" dependencies = [ "atty", "humantime", @@ -134,17 +150,189 @@ dependencies = [ "termcolor", ] +[[package]] +name = "errno" +version = "0.3.14" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "39cab71617ae0d63f51a36d69f866391735b51691dbda63cf6f96d042b63efeb" +dependencies = [ + "libc", + "windows-sys 0.61.2", +] + +[[package]] +name = "glam" +version = "0.14.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "333928d5eb103c5d4050533cec0384302db6be8ef7d3cebd30ec6a35350353da" +dependencies = [ + "num-traits", +] + +[[package]] +name = "glam" +version = "0.15.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3abb554f8ee44336b72d522e0a7fe86a29e09f839a36022fa869a7dfe941a54b" +dependencies = [ + "num-traits", +] + +[[package]] +name = "glam" +version = "0.16.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4126c0479ccf7e8664c36a2d719f5f2c140fbb4f9090008098d2c291fa5b3f16" +dependencies = [ + "num-traits", +] + +[[package]] +name = "glam" +version = "0.17.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e01732b97afd8508eee3333a541b9f7610f454bb818669e66e90f5f57c93a776" +dependencies = [ + "num-traits", +] + +[[package]] +name = "glam" +version = "0.18.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "525a3e490ba77b8e326fb67d4b44b4bd2f920f44d4cc73ccec50adc68e3bee34" +dependencies = [ + "num-traits", +] + +[[package]] +name = "glam" +version = "0.19.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2b8509e6791516e81c1a630d0bd7fbac36d2fa8712a9da8662e716b52d5051ca" +dependencies = [ + "num-traits", +] + +[[package]] +name = "glam" +version = "0.20.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f43e957e744be03f5801a55472f593d43fabdebf25a4585db250f04d86b1675f" +dependencies = [ + "num-traits", +] + +[[package]] +name = "glam" +version = "0.21.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "518faa5064866338b013ff9b2350dc318e14cc4fcd6cb8206d7e7c9886c98815" +dependencies = [ + "num-traits", +] + +[[package]] +name = "glam" +version = "0.22.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "12f597d56c1bd55a811a1be189459e8fad2bbc272616375602443bdfb37fa774" +dependencies = [ + "num-traits", +] + +[[package]] +name = "glam" +version = "0.23.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8e4afd9ad95555081e109fe1d21f2a30c691b5f0919c67dfa690a2e1eb6bd51c" +dependencies = [ + "num-traits", +] + +[[package]] +name = "glam" +version = "0.24.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b5418c17512bdf42730f9032c74e1ae39afc408745ebb2acf72fbc4691c17945" +dependencies = [ + "libm", +] + +[[package]] +name = "glam" +version = "0.25.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "151665d9be52f9bb40fc7966565d39666f2d1e69233571b71b87791c7e0528b3" +dependencies = [ + "libm", +] + +[[package]] +name = "glam" +version = "0.27.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9e05e7e6723e3455f4818c7b26e855439f7546cf617ef669d1adedb8669e5cb9" +dependencies = [ + "libm", +] + +[[package]] +name = "glam" +version = "0.28.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "779ae4bf7e8421cf91c0b3b64e7e8b40b862fba4d393f59150042de7c4965a94" +dependencies = [ + "libm", +] + +[[package]] +name = "glam" +version = "0.29.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8babf46d4c1c9d92deac9f7be466f76dfc4482b6452fc5024b5e8daf6ffeb3ee" +dependencies = [ + "libm", +] + +[[package]] +name = "glam" +version = "0.30.10" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "19fc433e8437a212d1b6f1e68c7824af3aed907da60afa994e7f542d18d12aa9" +dependencies = [ + "libm", +] + +[[package]] +name = "glam" +version = "0.31.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "556f6b2ea90b8d15a74e0e7bb41671c9bdf38cd9f78c284d750b9ce58a2b5be7" +dependencies = [ + "libm", +] + +[[package]] +name = "glam" +version = "0.32.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f70749695b063ecbf6b62949ccccde2e733ec3ecbbd71d467dca4e5c6c97cca0" +dependencies = [ + "libm", +] + [[package]] name = "glob" -version = "0.3.0" +version = "0.3.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9b919933a397b79c37e33b77bb2aa3dc8eb6e165ad809e58ff75bc7db2e34574" +checksum = "0cc23270f6e1808e30a928bdc84dea0b9b4136a8bc82338574f23baf47bbd280" [[package]] name = "hashbrown" -version = "0.12.2" +version = "0.12.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "607c8a29735385251a339424dd462993c0fed8fa09d378f259377df08c126022" +checksum = "8a9ee70c43aaf417c914396645a0fa852624801b24ebb7ae78fe8272889ac888" [[package]] name = "hermit-abi" @@ -155,17 +343,26 @@ dependencies = [ "libc", ] +[[package]] +name = "home" +version = "0.5.12" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cc627f471c528ff0c4a49e1d5e60450c8f6461dd6d10ba9dcd3a61d3dff7728d" +dependencies = [ + "windows-sys 0.61.2", +] + [[package]] name = "humantime" -version = "2.1.0" +version = "2.3.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9a3a5bfb195931eeb336b2a7b4d761daec841b97f947d34394601737a7bba5e4" +checksum = "135b12329e5e3ce057a9f972339ea52bc954fe1e9358ef27f95e89716fbc5424" [[package]] name = "indexmap" -version = "1.9.1" +version = "1.9.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "10a35a97730320ffe8e2d410b5d3b69279b98d2c14bdb8b70ea89ecf7888d41e" +checksum = "bd070e393353796e801d209ad339e89596eb4c8d430d18ede6a1cced8fafbd99" dependencies = [ "autocfg", "hashbrown", @@ -173,9 +370,9 @@ dependencies = [ [[package]] name = "lazy_static" -version = "1.4.0" +version = "1.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e2abad23fbc42b3700f2f279844dc832adb2b2eb069b2df918f455c4e18cc646" +checksum = "bbd2bcb4c963f2ddae06a2efc7e9f3591312473c50c6685e1f298068316e66fe" [[package]] name = "lazycell" @@ -185,34 +382,43 @@ checksum = "830d08ce1d1d941e6b30645f1a0eb5643013d835ce3779a5fc208261dbe10f55" [[package]] name = "libc" -version = "0.2.126" +version = "0.2.186" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "349d5a591cd28b49e1d1037471617a32ddcda5731b99419008085f72d5a53836" +checksum = "68ab91017fe16c622486840e4c83c9a37afeff978bd239b5293d61ece587de66" [[package]] name = "libloading" -version = "0.7.3" +version = "0.8.9" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "efbc0f03f9a775e9f6aed295c6a1ba2253c5757a9e03d55c6caa46a681abcddd" +checksum = "d7c4b02199fee7c5d21a5ae7d8cfa79a6ef5bb2fc834d6e9058e89c825efdc55" dependencies = [ "cfg-if", - "winapi", + "windows-link", ] +[[package]] +name = "libm" +version = "0.2.16" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b6d2cec3eae94f9f509c767b45932f1ada8350c4bdb85af2fcab4a3c14807981" + +[[package]] +name = "linux-raw-sys" +version = "0.4.15" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d26c52dbd32dccf2d10cac7725f8eae5296885fb5703b261f7d0a0739ec807ab" + [[package]] name = "log" -version = "0.4.17" +version = "0.4.29" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "abb12e687cfb44aa40f41fc3978ef76448f9b6038cad6aef4259d3c095a2382e" -dependencies = [ - "cfg-if", -] +checksum = "5e5032e24019045c762d3c0f28f5b6b8bbf38563a65908389bf7978758920897" [[package]] name = "memchr" -version = "2.5.0" +version = "2.8.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2dffe52ecf27772e601905b7522cb4ef790d2cc203488bbd0e2fe85fcb74566d" +checksum = "f8ca58f447f06ed17d5fc4043ce1b10dd205e060fb3ce5b979b8ed8e59ff3f79" [[package]] name = "minimal-lexical" @@ -220,27 +426,115 @@ version = "0.2.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "68354c5c6bd36d73ff3feceb05efa59b6acb7626617f4962be322a825e61f79a" +[[package]] +name = "nalgebra" +version = "0.34.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "df76ea0ff5c7e6b88689085804d6132ded0ddb9de5ca5b8aeb9eeadc0508a70a" +dependencies = [ + "approx", + "glam 0.14.0", + "glam 0.15.2", + "glam 0.16.0", + "glam 0.17.3", + "glam 0.18.0", + "glam 0.19.0", + "glam 0.20.5", + "glam 0.21.3", + "glam 0.22.0", + "glam 0.23.0", + "glam 0.24.2", + "glam 0.25.0", + "glam 0.27.0", + "glam 0.28.0", + "glam 0.29.3", + "glam 0.30.10", + "glam 0.31.1", + "glam 0.32.1", + "nalgebra-macros", + "num-complex", + "num-rational", + "num-traits", + "simba", + "typenum", +] + +[[package]] +name = "nalgebra-macros" +version = "0.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "973e7178a678cfd059ccec50887658d482ce16b0aa9da3888ddeab5cd5eb4889" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + [[package]] name = "nom" -version = "7.1.1" +version = "7.1.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a8903e5a29a317527874d0402f867152a3d21c908bb0b933e416c65e301d4c36" +checksum = "d273983c5a657a70a3e8f2a01329822f3b8c8172b73826411a55751e404a0a4a" dependencies = [ "memchr", "minimal-lexical", ] +[[package]] +name = "num-complex" +version = "0.4.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "73f88a1307638156682bada9d7604135552957b7818057dcef22705b4d509495" +dependencies = [ + "num-traits", +] + +[[package]] +name = "num-integer" +version = "0.1.46" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7969661fd2958a5cb096e56c8e1ad0444ac2bbcd0061bd28660485a44879858f" +dependencies = [ + "num-traits", +] + +[[package]] +name = "num-rational" +version = "0.4.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f83d14da390562dca69fc84082e73e548e1ad308d24accdedd2720017cb37824" +dependencies = [ + "num-integer", + "num-traits", +] + +[[package]] +name = "num-traits" +version = "0.2.19" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "071dfc062690e90b734c0b2273ce72ad0ffa95f0c74596bc250dcfd960262841" +dependencies = [ + "autocfg", + "libm", +] + [[package]] name = "once_cell" -version = "1.18.0" +version = "1.21.4" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "dd8b5dd2ae5ed71462c540258bedcb51965123ad7e7ccf4b9a8cafaa4a63576d" +checksum = "9f7c3e4beb33f85d45ae3e3a1792185706c8e16d043238c593331cc7cd313b50" [[package]] name = "os_str_bytes" -version = "6.1.0" +version = "6.6.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "21326818e99cfe6ce1e524c2a805c189a99b5ae555a35d19f9a284b427d86afa" +checksum = "e2355d85b9a3786f481747ced0e0ff2ba35213a1f9bd406ed906554d7af805a1" + +[[package]] +name = "paste" +version = "1.0.15" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "57c0d7b74b563b49d38dae00a0c37d4d6de9b432382b2892f0574ddcae73fd0a" [[package]] name = "peeking_take_while" @@ -250,27 +544,39 @@ checksum = "19b17cddbe7ec3f8bc800887bab5e717348c95ea2ca0b1bf0837fb964dc67099" [[package]] name = "proc-macro2" -version = "1.0.40" +version = "1.0.106" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "dd96a1e8ed2596c337f8eae5f24924ec83f5ad5ab21ea8e455d3566c69fbcaf7" +checksum = "8fd00f0bb2e90d81d1044c2b32617f68fcb9fa3bb7640c23e9c748e53fb30934" dependencies = [ "unicode-ident", ] [[package]] name = "quote" -version = "1.0.20" +version = "1.0.45" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3bcdf212e9776fbcb2d23ab029360416bb1706b1aea2d1a5ba002727cbcab804" +checksum = "41f2619966050689382d2b44f664f4bc593e129785a36d6ee376ddf37259b924" dependencies = [ "proc-macro2", ] [[package]] name = "regex" -version = "1.6.0" +version = "1.12.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4c4eb3267174b8c6c2f654116623910a0fef09c4753f8dd83db29c48a0df988b" +checksum = "e10754a14b9137dd7b1e3e5b0493cc9171fdd105e0ab477f51b72e7f3ac0e276" +dependencies = [ + "aho-corasick", + "memchr", + "regex-automata", + "regex-syntax", +] + +[[package]] +name = "regex-automata" +version = "0.4.14" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6e1dd4122fc1595e8162618945476892eefca7b88c52820e74af6262213cae8f" dependencies = [ "aho-corasick", "memchr", @@ -279,9 +585,9 @@ dependencies = [ [[package]] name = "regex-syntax" -version = "0.6.27" +version = "0.8.10" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a3f87b73ce11b1619a3c6332f45341e0047173771e8b8b73f87bfeefb7b56244" +checksum = "dc897dd8d9e8bd1ed8cdad82b5966c3e0ecae09fb1907d58efaa013543185d0a" [[package]] name = "rustc-hash" @@ -289,11 +595,36 @@ version = "1.1.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "08d43f7aa6b08d49f382cde6a7982047c3426db949b1424bc4b7ec9ae12c6ce2" +[[package]] +name = "rustix" +version = "0.38.44" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "fdb5bc1ae2baa591800df16c9ca78619bf65c0488b41b96ccec5d11220d8c154" +dependencies = [ + "bitflags 2.11.1", + "errno", + "libc", + "linux-raw-sys", + "windows-sys 0.59.0", +] + [[package]] name = "shlex" -version = "1.1.0" +version = "1.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0fda2ff0d084019ba4d7c6f371c95d8fd75ce3524c3cb8fb653a3023f6323e64" + +[[package]] +name = "simba" +version = "0.9.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "43b2853a4d09f215c24cc5489c992ce46052d359b5109343cbafbf26bc62f8a3" +checksum = "c99284beb21666094ba2b75bbceda012e610f5479dfcc2d6e2426f53197ffd95" +dependencies = [ + "approx", + "num-complex", + "num-traits", + "paste", +] [[package]] name = "strsim" @@ -301,36 +632,54 @@ version = "0.10.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "73473c0e59e6d5812c5dfe2a064a6444949f089e20eec9a2e5506596494e4623" +[[package]] +name = "syn" +version = "2.0.117" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e665b8803e7b1d2a727f4023456bbbbe74da67099c585258af0ad9c5013b9b99" +dependencies = [ + "proc-macro2", + "quote", + "unicode-ident", +] + [[package]] name = "termcolor" -version = "1.1.3" +version = "1.4.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "bab24d30b911b2376f3a13cc2cd443142f0c81dda04c118693e35b3835757755" +checksum = "06794f8f6c5c898b3275aebefa6b8a1cb24cd2c6c79397ab15774837a0bc5755" dependencies = [ "winapi-util", ] [[package]] name = "textwrap" -version = "0.15.0" +version = "0.16.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b1141d4d61095b28419e22cb0bbf02755f5e54e0526f97f1e3d1d160e60885fb" +checksum = "c13547615a44dc9c452a8a534638acdf07120d4b6847c8178705da06306a3057" + +[[package]] +name = "typenum" +version = "1.20.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "40ce102ab67701b8526c123c1bab5cbe42d7040ccfd0f64af1a385808d2f43de" [[package]] name = "unicode-ident" -version = "1.0.1" +version = "1.0.24" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5bd2fe26506023ed7b5e1e315add59d6f584c621d037f9368fea9cfb988f368c" +checksum = "e6e4313cd5fcd3dad5cafa179702e2b244f760991f45397d14d4ebf38247da75" [[package]] name = "which" -version = "4.4.0" +version = "4.4.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2441c784c52b289a054b7201fc93253e288f094e2f4be9058343127c4226a269" +checksum = "87ba24419a2078cd2b0f2ede2691b6c66d8e47836da3b6db8265ebad47afbfc7" dependencies = [ "either", - "libc", + "home", "once_cell", + "rustix", ] [[package]] @@ -351,11 +700,11 @@ checksum = "ac3b87c63620426dd9b991e5ce0329eff545bccbbb34f3be09ff6fb6ab51b7b6" [[package]] name = "winapi-util" -version = "0.1.5" +version = "0.1.11" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "70ec6ce85bb158151cae5e5c87f95a8e97d2c0c4b001223f33a334e3ce5de178" +checksum = "c2a7b1c03c876122aa43f3020e6c3c3ee5c05081c9a00739faf7503aeba10d22" dependencies = [ - "winapi", + "windows-sys 0.61.2", ] [[package]] @@ -363,3 +712,91 @@ name = "winapi-x86_64-pc-windows-gnu" version = "0.4.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "712e227841d057c1ee1cd2fb22fa7e5a5461ae8e48fa2ca79ec42cfc1931183f" + +[[package]] +name = "windows-link" +version = "0.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f0805222e57f7521d6a62e36fa9163bc891acd422f971defe97d64e70d0a4fe5" + +[[package]] +name = "windows-sys" +version = "0.59.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1e38bc4d79ed67fd075bcc251a1c39b32a1776bbe92e5bef1f0bf1f8c531853b" +dependencies = [ + "windows-targets", +] + +[[package]] +name = "windows-sys" +version = "0.61.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ae137229bcbd6cdf0f7b80a31df61766145077ddf49416a728b02cb3921ff3fc" +dependencies = [ + "windows-link", +] + +[[package]] +name = "windows-targets" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9b724f72796e036ab90c1021d4780d4d3d648aca59e491e6b98e725b84e99973" +dependencies = [ + "windows_aarch64_gnullvm", + "windows_aarch64_msvc", + "windows_i686_gnu", + "windows_i686_gnullvm", + "windows_i686_msvc", + "windows_x86_64_gnu", + "windows_x86_64_gnullvm", + "windows_x86_64_msvc", +] + +[[package]] +name = "windows_aarch64_gnullvm" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "32a4622180e7a0ec044bb555404c800bc9fd9ec262ec147edd5989ccd0c02cd3" + +[[package]] +name = "windows_aarch64_msvc" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "09ec2a7bb152e2252b53fa7803150007879548bc709c039df7627cabbd05d469" + +[[package]] +name = "windows_i686_gnu" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8e9b5ad5ab802e97eb8e295ac6720e509ee4c243f69d781394014ebfe8bbfa0b" + +[[package]] +name = "windows_i686_gnullvm" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0eee52d38c090b3caa76c563b86c3a4bd71ef1a819287c19d586d7334ae8ed66" + +[[package]] +name = "windows_i686_msvc" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "240948bc05c5e7c6dabba28bf89d89ffce3e303022809e73deaefe4f6ec56c66" + +[[package]] +name = "windows_x86_64_gnu" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "147a5c80aabfbf0c7d901cb5895d1de30ef2907eb21fbbab29ca94c5b08b1a78" + +[[package]] +name = "windows_x86_64_gnullvm" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "24d5b23dc417412679681396f2b49f3de8c1473deb516bd34410872eff51ed0d" + +[[package]] +name = "windows_x86_64_msvc" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "589f6da84c646204747d1270a2a5661ea66ed1cced2631d546fdfb155959f9ec" diff --git a/ateam-common-packets/rust-lib/Cargo.toml b/ateam-common-packets/rust-lib/Cargo.toml index db1604a..cc38f08 100644 --- a/ateam-common-packets/rust-lib/Cargo.toml +++ b/ateam-common-packets/rust-lib/Cargo.toml @@ -1,6 +1,7 @@ [package] name = "ateam-common-packets" version = "1.0.0" +edition = "2021" description = "Containes the radio packet definitions used by multiple targets" repository = "https://github.com/SSL-A-Team/common" @@ -11,3 +12,6 @@ path = "src/lib.rs" [build-dependencies] bindgen = "0.60.1" which = "4.4.0" + +[dependencies] +nalgebra = { version = "0.34.0", default-features = false, features = ["libm", "macros"] } diff --git a/ateam-common-packets/rust-lib/src/lib.rs b/ateam-common-packets/rust-lib/src/lib.rs index d4853d2..4fd8453 100644 --- a/ateam-common-packets/rust-lib/src/lib.rs +++ b/ateam-common-packets/rust-lib/src/lib.rs @@ -4,7 +4,7 @@ #![allow(non_snake_case)] #![feature(const_cmp)] -use crate::bindings::{BasicControl, BodyControlMode, ExtendedTelemetry, GlobalAccelerationCommand, GlobalPositionCommand, GlobalVelocityCommand, LocalAccelerationCommand, LocalVelocityCommand, RadioHeader}; +use crate::{bindings::{BasicControl, BodyControlMode, ExtendedTelemetry, GlobalAccelerationCommand, GlobalPositionCommand, GlobalVelocityCommand, LocalAccelerationCommand, LocalVelocityCommand, RadioHeader}, radio::SkillCommand}; pub mod bindings; pub mod radio; diff --git a/ateam-common-packets/rust-lib/src/radio.rs b/ateam-common-packets/rust-lib/src/radio.rs index 6795bf9..8735d87 100644 --- a/ateam-common-packets/rust-lib/src/radio.rs +++ b/ateam-common-packets/rust-lib/src/radio.rs @@ -1,4 +1,5 @@ -use crate::{bindings::{BasicControl, ParameterCommand}, is_basic_control_packet_safe}; +use nalgebra::Vector3; +use crate::{bindings::{BasicControl, BasicTelemetry, BodyControlExtendedTelemetry, BodyControlMode, BodyControlSkillExtendedTelemetry, BodyControlTelemetry, ExtendedGlobalAccelerationTelemetry, ExtendedGlobalPositionTelemetry, ExtendedGlobalVelocityTelemetry, ExtendedLocalAccelerationTelemetry, ExtendedLocalVelocityTelemetry, GlobalAccelerationCommand, GlobalAccelerationTelemetry, GlobalPositionCommand, GlobalPositionTelemetry, GlobalVelocityCommand, GlobalVelocityTelemetry, LocalAccelerationCommand, LocalAccelerationTelemetry, LocalVelocityCommand, LocalVelocityTelemetry, ParameterCommand}, is_basic_control_packet_safe}; #[derive(Copy, Clone)] pub enum DataPacket { @@ -6,6 +7,176 @@ pub enum DataPacket { ParameterCommand(ParameterCommand), } +pub enum SkillCommand { + Off, + GlobalPosition(GlobalPositionCommand), + GlobalVelocity(GlobalVelocityCommand), + LocalVelocity(LocalVelocityCommand), + GlobalAcceleration(GlobalAccelerationCommand), + LocalAcceleration(LocalAccelerationCommand), +} + +#[derive(Copy, Clone)] +pub enum SkillTelemetry { + Off, + GlobalPosition(GlobalPositionTelemetry), + GlobalVelocity(GlobalVelocityTelemetry), + LocalVelocity(LocalVelocityTelemetry), + GlobalAcceleration(GlobalAccelerationTelemetry), + LocalAcceleration(LocalAccelerationTelemetry), +} + +#[derive(Copy, Clone)] +pub enum SkillExtendedTelemetry { + Off, + GlobalPosition(ExtendedGlobalPositionTelemetry), + GlobalVelocity(ExtendedGlobalVelocityTelemetry), + LocalVelocity(ExtendedLocalVelocityTelemetry), + GlobalAcceleration(ExtendedGlobalAccelerationTelemetry), + LocalAcceleration(ExtendedLocalAccelerationTelemetry), +} + +impl GlobalPositionCommand { + pub fn as_vec3f(&self) -> Vector3 { + Vector3::new(self.global_x, self.global_y, self.global_theta) + } + + pub fn from_vec3f(&mut self, v: Vector3) { + self.global_x = v.x; + self.global_y = v.y; + self.global_theta = v.z; + } +} + +impl GlobalVelocityCommand { + pub fn as_vec3f(&self) -> Vector3 { + Vector3::new(self.global_xd, self.global_yd, self.global_omega) + } + + pub fn from_vec3f(&mut self, v: Vector3) { + self.global_xd = v.x; + self.global_yd = v.y; + self.global_omega = v.z; + } +} + +impl LocalVelocityCommand { + pub fn as_vec3f(&self) -> Vector3 { + Vector3::new(self.local_xd, self.local_yd, self.local_omega) + } + + pub fn from_vec3f(&mut self, v: Vector3) { + self.local_xd = v.x; + self.local_yd = v.y; + self.local_omega = v.z; + } +} + +impl GlobalAccelerationCommand { + pub fn as_vec3f(&self) -> Vector3 { + Vector3::new(self.global_xdd, self.global_ydd, self.global_alpha) + } + + pub fn from_vec3f(&mut self, v: Vector3) { + self.global_xdd = v.x; + self.global_ydd = v.y; + self.global_alpha = v.z; + } +} + +impl LocalAccelerationCommand { + pub fn as_vec3f(&self) -> Vector3 { + Vector3::new(self.local_xdd, self.local_ydd, self.local_alpha) + } + + pub fn from_vec3f(&mut self, v: Vector3) { + self.local_xdd = v.x; + self.local_ydd = v.y; + self.local_alpha = v.z; + } +} + +impl BasicControl { + pub fn get_skill_command(&self) -> SkillCommand { + // union extraction is unsafe + unsafe { + match self.body_control_mode { + BodyControlMode::BCM_OFF => SkillCommand::Off, + BodyControlMode::BCM_GLOBAL_POSITION => SkillCommand::GlobalPosition(self.cmd.global_pos), + BodyControlMode::BCM_GLOBAL_VELOCITY => SkillCommand::GlobalVelocity(self.cmd.global_vel), + BodyControlMode::BCM_LOCAL_VELOCITY => SkillCommand::LocalVelocity(self.cmd.local_vel), + BodyControlMode::BCM_GLOBAL_ACCEL => SkillCommand::GlobalAcceleration(self.cmd.global_acc), + BodyControlMode::BCM_LOCAL_ACCEL => SkillCommand::LocalAcceleration(self.cmd.local_acc), + _ => SkillCommand::Off, + } + } + } +} + +impl BasicTelemetry { + pub fn set_skill_telemetry(&mut self, telem: SkillTelemetry) { + unsafe { + match telem { + SkillTelemetry::Off => { + self.body_control_mode = BodyControlMode::BCM_OFF; + } + SkillTelemetry::GlobalPosition(t) => { + self.body_control_mode = BodyControlMode::BCM_GLOBAL_POSITION; + self.control_telem = BodyControlTelemetry { global_pos: t }; + } + SkillTelemetry::GlobalVelocity(t) => { + self.body_control_mode = BodyControlMode::BCM_GLOBAL_VELOCITY; + self.control_telem = BodyControlTelemetry { global_vel: t }; + } + SkillTelemetry::LocalVelocity(t) => { + self.body_control_mode = BodyControlMode::BCM_LOCAL_VELOCITY; + self.control_telem = BodyControlTelemetry { local_vel: t }; + } + SkillTelemetry::GlobalAcceleration(t) => { + self.body_control_mode = BodyControlMode::BCM_GLOBAL_ACCEL; + self.control_telem = BodyControlTelemetry { global_acc: t }; + } + SkillTelemetry::LocalAcceleration(t) => { + self.body_control_mode = BodyControlMode::BCM_LOCAL_ACCEL; + self.control_telem = BodyControlTelemetry { local_acc: t }; + } + } + } + } +} + +impl BodyControlExtendedTelemetry { + pub fn set_skill_telemetry(&mut self, telem: SkillExtendedTelemetry) { + unsafe { + match telem { + SkillExtendedTelemetry::Off => { + self.body_control_mode = BodyControlMode::BCM_OFF; + } + SkillExtendedTelemetry::GlobalPosition(t) => { + self.body_control_mode = BodyControlMode::BCM_GLOBAL_POSITION; + self.skill = BodyControlSkillExtendedTelemetry { global_pos: t }; + } + SkillExtendedTelemetry::GlobalVelocity(t) => { + self.body_control_mode = BodyControlMode::BCM_GLOBAL_VELOCITY; + self.skill = BodyControlSkillExtendedTelemetry { global_vel: t }; + } + SkillExtendedTelemetry::LocalVelocity(t) => { + self.body_control_mode = BodyControlMode::BCM_LOCAL_VELOCITY; + self.skill = BodyControlSkillExtendedTelemetry { local_vel: t }; + } + SkillExtendedTelemetry::GlobalAcceleration(t) => { + self.body_control_mode = BodyControlMode::BCM_GLOBAL_ACCEL; + self.skill = BodyControlSkillExtendedTelemetry { global_acc: t }; + } + SkillExtendedTelemetry::LocalAcceleration(t) => { + self.body_control_mode = BodyControlMode::BCM_LOCAL_ACCEL; + self.skill = BodyControlSkillExtendedTelemetry { local_acc: t }; + } + } + } + } +} + #[derive(Copy, Clone)] pub enum TelemetryPacket { Basic(crate::bindings::BasicTelemetry), From 238515854d466d0a02430a5cb21d9291fb6f2716 Mon Sep 17 00:00:00 2001 From: Nicholas Witten Date: Thu, 14 May 2026 22:38:34 -0700 Subject: [PATCH 38/44] remove unused imports and unnecessary unsafe --- ateam-common-packets/rust-lib/src/lib.rs | 2 +- ateam-common-packets/rust-lib/src/radio.rs | 96 +++++++++++----------- 2 files changed, 47 insertions(+), 51 deletions(-) diff --git a/ateam-common-packets/rust-lib/src/lib.rs b/ateam-common-packets/rust-lib/src/lib.rs index 4fd8453..70717bd 100644 --- a/ateam-common-packets/rust-lib/src/lib.rs +++ b/ateam-common-packets/rust-lib/src/lib.rs @@ -4,7 +4,7 @@ #![allow(non_snake_case)] #![feature(const_cmp)] -use crate::{bindings::{BasicControl, BodyControlMode, ExtendedTelemetry, GlobalAccelerationCommand, GlobalPositionCommand, GlobalVelocityCommand, LocalAccelerationCommand, LocalVelocityCommand, RadioHeader}, radio::SkillCommand}; +use crate::{bindings::{BasicControl, BodyControlMode, ExtendedTelemetry, GlobalAccelerationCommand, GlobalPositionCommand, GlobalVelocityCommand, LocalAccelerationCommand, LocalVelocityCommand, RadioHeader}}; pub mod bindings; pub mod radio; diff --git a/ateam-common-packets/rust-lib/src/radio.rs b/ateam-common-packets/rust-lib/src/radio.rs index 8735d87..8559a28 100644 --- a/ateam-common-packets/rust-lib/src/radio.rs +++ b/ateam-common-packets/rust-lib/src/radio.rs @@ -115,31 +115,29 @@ impl BasicControl { impl BasicTelemetry { pub fn set_skill_telemetry(&mut self, telem: SkillTelemetry) { - unsafe { - match telem { - SkillTelemetry::Off => { - self.body_control_mode = BodyControlMode::BCM_OFF; - } - SkillTelemetry::GlobalPosition(t) => { - self.body_control_mode = BodyControlMode::BCM_GLOBAL_POSITION; - self.control_telem = BodyControlTelemetry { global_pos: t }; - } - SkillTelemetry::GlobalVelocity(t) => { - self.body_control_mode = BodyControlMode::BCM_GLOBAL_VELOCITY; - self.control_telem = BodyControlTelemetry { global_vel: t }; - } - SkillTelemetry::LocalVelocity(t) => { - self.body_control_mode = BodyControlMode::BCM_LOCAL_VELOCITY; - self.control_telem = BodyControlTelemetry { local_vel: t }; - } - SkillTelemetry::GlobalAcceleration(t) => { - self.body_control_mode = BodyControlMode::BCM_GLOBAL_ACCEL; - self.control_telem = BodyControlTelemetry { global_acc: t }; - } - SkillTelemetry::LocalAcceleration(t) => { - self.body_control_mode = BodyControlMode::BCM_LOCAL_ACCEL; - self.control_telem = BodyControlTelemetry { local_acc: t }; - } + match telem { + SkillTelemetry::Off => { + self.body_control_mode = BodyControlMode::BCM_OFF; + } + SkillTelemetry::GlobalPosition(t) => { + self.body_control_mode = BodyControlMode::BCM_GLOBAL_POSITION; + self.control_telem = BodyControlTelemetry { global_pos: t }; + } + SkillTelemetry::GlobalVelocity(t) => { + self.body_control_mode = BodyControlMode::BCM_GLOBAL_VELOCITY; + self.control_telem = BodyControlTelemetry { global_vel: t }; + } + SkillTelemetry::LocalVelocity(t) => { + self.body_control_mode = BodyControlMode::BCM_LOCAL_VELOCITY; + self.control_telem = BodyControlTelemetry { local_vel: t }; + } + SkillTelemetry::GlobalAcceleration(t) => { + self.body_control_mode = BodyControlMode::BCM_GLOBAL_ACCEL; + self.control_telem = BodyControlTelemetry { global_acc: t }; + } + SkillTelemetry::LocalAcceleration(t) => { + self.body_control_mode = BodyControlMode::BCM_LOCAL_ACCEL; + self.control_telem = BodyControlTelemetry { local_acc: t }; } } } @@ -147,31 +145,29 @@ impl BasicTelemetry { impl BodyControlExtendedTelemetry { pub fn set_skill_telemetry(&mut self, telem: SkillExtendedTelemetry) { - unsafe { - match telem { - SkillExtendedTelemetry::Off => { - self.body_control_mode = BodyControlMode::BCM_OFF; - } - SkillExtendedTelemetry::GlobalPosition(t) => { - self.body_control_mode = BodyControlMode::BCM_GLOBAL_POSITION; - self.skill = BodyControlSkillExtendedTelemetry { global_pos: t }; - } - SkillExtendedTelemetry::GlobalVelocity(t) => { - self.body_control_mode = BodyControlMode::BCM_GLOBAL_VELOCITY; - self.skill = BodyControlSkillExtendedTelemetry { global_vel: t }; - } - SkillExtendedTelemetry::LocalVelocity(t) => { - self.body_control_mode = BodyControlMode::BCM_LOCAL_VELOCITY; - self.skill = BodyControlSkillExtendedTelemetry { local_vel: t }; - } - SkillExtendedTelemetry::GlobalAcceleration(t) => { - self.body_control_mode = BodyControlMode::BCM_GLOBAL_ACCEL; - self.skill = BodyControlSkillExtendedTelemetry { global_acc: t }; - } - SkillExtendedTelemetry::LocalAcceleration(t) => { - self.body_control_mode = BodyControlMode::BCM_LOCAL_ACCEL; - self.skill = BodyControlSkillExtendedTelemetry { local_acc: t }; - } + match telem { + SkillExtendedTelemetry::Off => { + self.body_control_mode = BodyControlMode::BCM_OFF; + } + SkillExtendedTelemetry::GlobalPosition(t) => { + self.body_control_mode = BodyControlMode::BCM_GLOBAL_POSITION; + self.skill = BodyControlSkillExtendedTelemetry { global_pos: t }; + } + SkillExtendedTelemetry::GlobalVelocity(t) => { + self.body_control_mode = BodyControlMode::BCM_GLOBAL_VELOCITY; + self.skill = BodyControlSkillExtendedTelemetry { global_vel: t }; + } + SkillExtendedTelemetry::LocalVelocity(t) => { + self.body_control_mode = BodyControlMode::BCM_LOCAL_VELOCITY; + self.skill = BodyControlSkillExtendedTelemetry { local_vel: t }; + } + SkillExtendedTelemetry::GlobalAcceleration(t) => { + self.body_control_mode = BodyControlMode::BCM_GLOBAL_ACCEL; + self.skill = BodyControlSkillExtendedTelemetry { global_acc: t }; + } + SkillExtendedTelemetry::LocalAcceleration(t) => { + self.body_control_mode = BodyControlMode::BCM_LOCAL_ACCEL; + self.skill = BodyControlSkillExtendedTelemetry { local_acc: t }; } } } From 5abca632a65004c1e6efec5679c13b43031385d0 Mon Sep 17 00:00:00 2001 From: Nicholas Witten Date: Thu, 14 May 2026 23:47:33 -0700 Subject: [PATCH 39/44] update available robot params --- ateam-common-packets/include/robot_parameters.h | 14 ++++---------- 1 file changed, 4 insertions(+), 10 deletions(-) diff --git a/ateam-common-packets/include/robot_parameters.h b/ateam-common-packets/include/robot_parameters.h index 637efd8..83d77b5 100644 --- a/ateam-common-packets/include/robot_parameters.h +++ b/ateam-common-packets/include/robot_parameters.h @@ -39,20 +39,14 @@ typedef enum ParameterName : uint8_t { POSE_CONTROL_GAIN = 7, /// [ERROR_POS_LINEAR, ERROR_POS_ANGULAR, ERROR_VEL_LINEAR, ERROR_VEL_ANGULAR] thresholds for when to recompute the trajectory TRAJ_RECOMPUTE_ERROR = 8, - /// [MAX_VEL_LINEAR, MAX_VEL_ANGULAR, MAX_ACCEL_LINEAR, MAX_ACCEL_ANGULAR] - TRAJ_MAX = 9, /// [P, I, D, I_MIN, I_MAX] - POSE_FB_PIDII_X = 10, + POSE_FB_PIDII_LINEAR = 9, /// [P, I, D, I_MIN, I_MAX] - POSE_FB_PIDII_Y = 11, + POSE_FB_PIDII_ANGULAR = 10, /// [P, I, D, I_MIN, I_MAX] - POSE_FB_PIDII_THETA = 12, + TWIST_FB_PIDII_LINEAR = 11, /// [P, I, D, I_MIN, I_MAX] - TWIST_FB_PIDII_X = 13, - /// [P, I, D, I_MIN, I_MAX] - TWIST_FB_PIDII_Y = 14, - /// [P, I, D, I_MIN, I_MAX] - TWIST_FB_PIDII_THETA = 15, + TWIST_FB_PIDII_ANGULAR = 12, } ParameterName; assert_size(ParameterName, 1); From c5676357809484c2a0d90612b1626fb5f781898f Mon Sep 17 00:00:00 2001 From: Nicholas Witten Date: Fri, 15 May 2026 17:45:20 -0700 Subject: [PATCH 40/44] add controller reset --- ateam-common-packets/include/basic_control.h | 3 ++- ateam-common-packets/include/basic_telemetry.h | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/ateam-common-packets/include/basic_control.h b/ateam-common-packets/include/basic_control.h index 9d79e84..a89d6eb 100644 --- a/ateam-common-packets/include/basic_control.h +++ b/ateam-common-packets/include/basic_control.h @@ -38,7 +38,8 @@ typedef struct BasicControl { uint32_t wheel_vel_control_enabled : 1; uint32_t wheel_torque_control_enabled : 1; uint32_t vision_update: 1; - uint32_t reserved1: 25; + uint32_t reset_controller : 1; + uint32_t reserved1: 24; // 4 bytes // Vision update diff --git a/ateam-common-packets/include/basic_telemetry.h b/ateam-common-packets/include/basic_telemetry.h index fee03da..c226796 100644 --- a/ateam-common-packets/include/basic_telemetry.h +++ b/ateam-common-packets/include/basic_telemetry.h @@ -50,7 +50,8 @@ typedef struct BasicTelemetry { uint32_t kicker_board_error : 1; uint32_t chipper_available : 1; uint32_t kicker_available : 1; - uint32_t _reserved : 6; + uint32_t controller_reset : 1; + uint32_t _reserved : 5; uint16_t battery_percent; uint16_t kicker_charge_percent; From c1535c6d91301deddc48a5f58370d1c1649220c7 Mon Sep 17 00:00:00 2001 From: Nicholas Witten Date: Wed, 20 May 2026 18:38:34 -0700 Subject: [PATCH 41/44] add coulomb friction compensation deadzone --- ateam-common-packets/include/robot_parameters.h | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/ateam-common-packets/include/robot_parameters.h b/ateam-common-packets/include/robot_parameters.h index 83d77b5..b27ae56 100644 --- a/ateam-common-packets/include/robot_parameters.h +++ b/ateam-common-packets/include/robot_parameters.h @@ -35,18 +35,20 @@ typedef enum ParameterName : uint8_t { PHYS_MOTOR_MODEL = 5, /// [COULOMB_FRICTION_COEFFICIENT_LINEAR, COULOMB_FRICTION_COEFFICIENT_ANGULAR, VISCOUS_FRICTION_COEFFICIENT_LINEAR, VISCOUS_FRICTION_COEFFICIENT_ANGULAR] PHYS_FRICTION_MODEL = 6, + /// Accel magnitude threshold below which coulomb friction comp uses estimated (deadzoned) twist instead of target twist + COULOMB_COMP_ACCEL_DEADZONE = 7, /// [FEEDFORWARD_GAIN, FEEDBACK_GAIN] - POSE_CONTROL_GAIN = 7, + POSE_CONTROL_GAIN = 8, /// [ERROR_POS_LINEAR, ERROR_POS_ANGULAR, ERROR_VEL_LINEAR, ERROR_VEL_ANGULAR] thresholds for when to recompute the trajectory - TRAJ_RECOMPUTE_ERROR = 8, + TRAJ_RECOMPUTE_ERROR = 9, /// [P, I, D, I_MIN, I_MAX] - POSE_FB_PIDII_LINEAR = 9, + POSE_FB_PIDII_LINEAR = 10, /// [P, I, D, I_MIN, I_MAX] - POSE_FB_PIDII_ANGULAR = 10, + POSE_FB_PIDII_ANGULAR = 11, /// [P, I, D, I_MIN, I_MAX] - TWIST_FB_PIDII_LINEAR = 11, + TWIST_FB_PIDII_LINEAR = 12, /// [P, I, D, I_MIN, I_MAX] - TWIST_FB_PIDII_ANGULAR = 12, + TWIST_FB_PIDII_ANGULAR = 13, } ParameterName; assert_size(ParameterName, 1); From fd0ad41eb9177df9b920f153912778c1e4efb77a Mon Sep 17 00:00:00 2001 From: Nicholas Witten Date: Mon, 25 May 2026 18:45:52 -0700 Subject: [PATCH 42/44] nix flake update --- flake.lock | 58 +++++++++++------------------------------------------- 1 file changed, 12 insertions(+), 46 deletions(-) diff --git a/flake.lock b/flake.lock index 60c4144..5f067fa 100644 --- a/flake.lock +++ b/flake.lock @@ -5,29 +5,11 @@ "systems": "systems" }, "locked": { - "lastModified": 1689068808, - "narHash": "sha256-6ixXo3wt24N/melDWjq70UuHQLxGV8jZvooRanIHXw0=", + "lastModified": 1731533236, + "narHash": "sha256-l0KFg5HjrsfsO/JpG+r7fRrqm12kzFHyUHqHCVpMMbI=", "owner": "numtide", "repo": "flake-utils", - "rev": "919d646de7be200f3bf08cb76ae1f09402b6f9b4", - "type": "github" - }, - "original": { - "owner": "numtide", - "repo": "flake-utils", - "type": "github" - } - }, - "flake-utils_2": { - "inputs": { - "systems": "systems_2" - }, - "locked": { - "lastModified": 1681202837, - "narHash": "sha256-H+Rh19JDwRtpVPAWp64F+rlEtxUWBAQW28eAi3SRSzg=", - "owner": "numtide", - "repo": "flake-utils", - "rev": "cfacdce06f30d2b68473a46042957675eebb3401", + "rev": "11707dc2f618dd54ca8739b309ec4fc024de578b", "type": "github" }, "original": { @@ -38,11 +20,11 @@ }, "nixpkgs": { "locked": { - "lastModified": 1689726558, - "narHash": "sha256-QPVY1u0n+ErmldYcn5DaIYMqn1L3dGETdm94KwKJU9s=", + "lastModified": 1779758509, + "narHash": "sha256-tWpW0CS26Fh7/iqXOLXCItpTWIq/hCOpicFEDQyBhfA=", "owner": "nixos", "repo": "nixpkgs", - "rev": "43e13111c6803551751d76a8adb9472f6f14f9c8", + "rev": "0a6619d2fd1aa2e3da9ccebc4bc99deca272c0ef", "type": "github" }, "original": { @@ -53,11 +35,11 @@ }, "nixpkgs_2": { "locked": { - "lastModified": 1681358109, - "narHash": "sha256-eKyxW4OohHQx9Urxi7TQlFBTDWII+F+x2hklDOQPB50=", + "lastModified": 1744536153, + "narHash": "sha256-awS2zRgF4uTwrOKwwiJcByDzDOdo3Q1rPZbiHQg/N38=", "owner": "NixOS", "repo": "nixpkgs", - "rev": "96ba1c52e54e74c3197f4d43026b3f3d92e83ff9", + "rev": "18dd725c29603f582cf1900e0d25f9f1063dbf11", "type": "github" }, "original": { @@ -76,15 +58,14 @@ }, "rust-overlay": { "inputs": { - "flake-utils": "flake-utils_2", "nixpkgs": "nixpkgs_2" }, "locked": { - "lastModified": 1689647697, - "narHash": "sha256-8ZX/DVpKLmr85FRKILb+2p+JuxfLQ49LjXG/gmwsoIU=", + "lastModified": 1779679233, + "narHash": "sha256-qSIAAfK66X6waos6alIYxVze1ZU3C/WPp7NlN4ooP54=", "owner": "oxalica", "repo": "rust-overlay", - "rev": "ed2774a9131ddad12e552ba04bd92f87df04a28b", + "rev": "47ab6c7b3c6a68beac60067490240efa32ae344c", "type": "github" }, "original": { @@ -107,21 +88,6 @@ "repo": "default", "type": "github" } - }, - "systems_2": { - "locked": { - "lastModified": 1681028828, - "narHash": "sha256-Vy1rq5AaRuLzOxct8nz4T6wlgyUR7zLU309k9mBC768=", - "owner": "nix-systems", - "repo": "default", - "rev": "da67096a3b9bf56a91d16901293e51ba5b49a27e", - "type": "github" - }, - "original": { - "owner": "nix-systems", - "repo": "default", - "type": "github" - } } }, "root": "root", From fd183d75ef91fcb716c1428313e41171a198a9e5 Mon Sep 17 00:00:00 2001 From: Nicholas Witten Date: Mon, 25 May 2026 18:49:15 -0700 Subject: [PATCH 43/44] update GCC ARM Embedded 12 -> 14 --- ateam-common-packets/rust-lib/Cargo.lock | 12 ++++++------ flake.nix | 4 ++-- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/ateam-common-packets/rust-lib/Cargo.lock b/ateam-common-packets/rust-lib/Cargo.lock index f1fe255..55078b2 100644 --- a/ateam-common-packets/rust-lib/Cargo.lock +++ b/ateam-common-packets/rust-lib/Cargo.lock @@ -42,9 +42,9 @@ dependencies = [ [[package]] name = "autocfg" -version = "1.5.0" +version = "1.5.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c08606f8c3cbf4ce6ec8e28fb0014a2c086708fe954eaa885384a6165172e7e8" +checksum = "f2032f911046de80f0a198e0901378627c33f59ea0ac00e363d481118bd70a53" [[package]] name = "bindgen" @@ -133,9 +133,9 @@ dependencies = [ [[package]] name = "either" -version = "1.15.0" +version = "1.16.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "48c757948c5ede0e46177b7add2e67155f70e33c07fea8284df6576da70b3719" +checksum = "91622ff5e7162018101f2fea40d6ebf4a78bbe5a49736a2020649edf9693679e" [[package]] name = "env_logger" @@ -410,9 +410,9 @@ checksum = "d26c52dbd32dccf2d10cac7725f8eae5296885fb5703b261f7d0a0739ec807ab" [[package]] name = "log" -version = "0.4.29" +version = "0.4.30" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5e5032e24019045c762d3c0f28f5b6b8bbf38563a65908389bf7978758920897" +checksum = "616ec5685824bcc94416c6d4a7a446eea774a31efd7062c8480ba6fd06d7a6e5" [[package]] name = "memchr" diff --git a/flake.nix b/flake.nix index a3168e7..8f92d64 100644 --- a/flake.nix +++ b/flake.nix @@ -33,9 +33,9 @@ buildInputs = with pkgs; [ gnumake - # GCC ARM Embedded 12 provides the sysroot/ABI defining types and type sizes + # GCC ARM Embedded 14 provides the sysroot/ABI defining types and type sizes # for bindgen - gcc-arm-embedded-12 + gcc-arm-embedded-14 # needed by bindgen clang From 55d79713c1d01dc9a54b307bad589ea1dc43c423 Mon Sep 17 00:00:00 2001 From: Nicholas Witten Date: Mon, 25 May 2026 18:50:48 -0700 Subject: [PATCH 44/44] CI steps version updates --- .github/workflows/CI.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/CI.yml b/.github/workflows/CI.yml index 93832fa..597c450 100644 --- a/.github/workflows/CI.yml +++ b/.github/workflows/CI.yml @@ -7,8 +7,8 @@ jobs: test-firmware-build: runs-on: ubuntu-24.04 steps: - - uses: actions/checkout@v2.4.0 - - uses: cachix/install-nix-action@v16 + - uses: actions/checkout@v5 + - uses: cachix/install-nix-action@v31 with: extra_nix_config: access-tokens = github.com=${{ secrets.GITHUB_TOKEN }}