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 }} diff --git a/ateam-common-packets/include/basic_control.h b/ateam-common-packets/include/basic_control.h index df7626f..a89d6eb 100644 --- a/ateam-common-packets/include/basic_control.h +++ b/ateam-common-packets/include/basic_control.h @@ -1,34 +1,65 @@ -/** - * @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 "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 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; 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 wheel_vel_control_enabled : 1; uint32_t wheel_torque_control_enabled : 1; - uint32_t _reserved : 17; - uint32_t play_song : 8; + uint32_t vision_update: 1; + uint32_t reset_controller : 1; + uint32_t reserved1: 24; + // 4 bytes + + // Vision update + float vision_position_update[3]; + // 12 bytes - float vel_x_linear; // m/s - float vel_y_linear; // m/s - float vel_z_angular; // m/s + // 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 + BodyControlCommand cmd; + // 28 bytes } BasicControl; -assert_size(BasicControl, 28); +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 68a3b10..c226796 100644 --- a/ateam-common-packets/include/basic_telemetry.h +++ b/ateam-common-packets/include/basic_telemetry.h @@ -1,21 +1,28 @@ -/** - * @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 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 { - uint16_t sequence_number; - uint8_t robot_revision_major; - uint8_t robot_revision_minor; + uint8_t transmission_sequence_number; + uint8_t control_data_sequence_number; + BodyControlMode body_control_mode; + uint8_t reserved1; uint32_t power_error : 1; uint32_t power_board_error : 1; @@ -43,12 +50,14 @@ 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 wheel_vel_control_enabled : 1; - uint32_t wheel_torque_control_enabled : 1; - uint32_t _reserved : 3; + uint32_t controller_reset : 1; + uint32_t _reserved : 5; uint16_t battery_percent; uint16_t kicker_charge_percent; + + // Body control command + BodyControlTelemetry control_telem; + // 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 new file mode 100644 index 0000000..1073261 --- /dev/null +++ b/ateam-common-packets/include/body_control.h @@ -0,0 +1,36 @@ +#pragma once + +#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; + uint8_t _reserved1: 7; + uint8_t _reserved2[2]; + // 4 bytes + + BodyControlSkillExtendedTelemetry skill; + + float imu_gyro[3]; // rad/s + float imu_accel[3]; // m/s^2 + float vision_pose[3]; // vision system pose measurement + 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 +} BodyControlExtendedTelemetry; +assert_size(BodyControlExtendedTelemetry, 4 + 28 + 12*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 new file mode 100644 index 0000000..aa0c60f --- /dev/null +++ b/ateam-common-packets/include/error_telemetry.h @@ -0,0 +1,10 @@ +#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/extended_telemetry.h b/ateam-common-packets/include/extended_telemetry.h index 32d5c6a..d187727 100644 --- a/ateam-common-packets/include/extended_telemetry.h +++ b/ateam-common-packets/include/extended_telemetry.h @@ -1,46 +1,29 @@ -/** - * @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" +#include "body_control.h" #include "kicker.h" #include "power.h" -#include "stspin.h" - +#include "stspin_current.h" typedef struct ExtendedTelemetry { + uint32_t timestamp_us_lo; + uint32_t timestamp_us_hi; + // 8 bytes + PowerTelemetry power_status; // 28 bytes - MotorTelemetry front_left_motor; - MotorTelemetry back_left_motor; - MotorTelemetry back_right_motor; - MotorTelemetry front_right_motor; - // 48 bytes each + CcmTelemetry front_left_motor; + CcmTelemetry back_left_motor; + CcmTelemetry back_right_motor; + CcmTelemetry front_right_motor; + // 60 bytes each + + BodyControlExtendedTelemetry body_control_telemetry; + // 160 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 } ExtendedTelemetry; -assert_size(ExtendedTelemetry, 28 + 192 + 64 + 24 + 48 + 32); +assert_size(ExtendedTelemetry, 8 + 28 + 4*60 + 176 + 64); diff --git a/ateam-common-packets/include/kicker.h b/ateam-common-packets/include/kicker.h index 85acbb7..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,16 +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; - // 29 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; @@ -43,4 +44,4 @@ typedef struct KickerTelemetry { MotorTelemetry dribbler_motor; } KickerTelemetry; -assert_size(KickerTelemetry, 64); \ No newline at end of file +assert_size(KickerTelemetry, 64); diff --git a/ateam-common-packets/include/radio.h b/ateam-common-packets/include/radio.h index f03b802..150acb5 100644 --- a/ateam-common-packets/include/radio.h +++ b/ateam-common-packets/include/radio.h @@ -1,48 +1,44 @@ -/** - * @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" #include "extended_telemetry.h" #include "power.h" #include "robot_parameters.h" - -const uint16_t kProtocolVersionMajor = 0; -const uint16_t kProtocolVersionMinor = 1; +#include "stspin_current.h" typedef enum CommandCode : uint8_t { + // bidirectional commands and status CC_ACK = 1, CC_NACK = 2, CC_GOODBYE = 3, CC_KEEPALIVE = 4, - CC_HELLO_REQ = 101, - CC_TELEMETRY = 102, - CC_CONTROL_DEBUG_TELEMETRY = 103, - CC_ROBOT_PARAMETER_COMMAND = 104, - 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; + uint8_t _reserved; uint16_t data_length; -} RadioHeader; -assert_size(RadioHeader, 12); +} __attribute__((packed)) RadioHeader; +assert_size(RadioHeader, 8); typedef union RadioData { HelloRequest hello_request; @@ -51,29 +47,14 @@ typedef union RadioData { BasicTelemetry telemetry; ExtendedTelemetry extended_telemetry; ParameterCommand robot_parameter_command; + ErrorTelemetry error_telemetry; } RadioData; -assert_size(RadioData, 388); +assert_size(RadioData, 516); +// 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; - 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, 400); -static_assert(sizeof(RadioPacket) <= 448); // 512 is the current size limit of an entry in the packet buffer + RadioHeader header; + RadioData data ; +} RadioPacket __attribute__((aligned (4))); +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_parameters.h b/ateam-common-packets/include/robot_parameters.h index 17c63e6..b27ae56 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" @@ -23,26 +13,42 @@ 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 { - 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, + /// [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, + /// 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 = 8, + /// [ERROR_POS_LINEAR, ERROR_POS_ANGULAR, ERROR_VEL_LINEAR, ERROR_VEL_ANGULAR] thresholds for when to recompute the trajectory + TRAJ_RECOMPUTE_ERROR = 9, + /// [P, I, D, I_MIN, I_MAX] + POSE_FB_PIDII_LINEAR = 10, + /// [P, I, D, I_MIN, I_MAX] + POSE_FB_PIDII_ANGULAR = 11, + /// [P, I, D, I_MIN, I_MAX] + TWIST_FB_PIDII_LINEAR = 12, + /// [P, I, D, I_MIN, I_MAX] + TWIST_FB_PIDII_ANGULAR = 13, } ParameterName; assert_size(ParameterName, 1); @@ -52,11 +58,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 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..53c9527 --- /dev/null +++ b/ateam-common-packets/include/robot_skills/global_acceleration.h @@ -0,0 +1,20 @@ +#pragma once + +#include "../common.h" + +typedef struct GlobalAccelerationCommand { + float global_xdd; + float global_ydd; + float global_alpha; +} GlobalAccelerationCommand; +assert_size(GlobalAccelerationCommand, 12); + +typedef struct GlobalAccelerationTelemetry { + uint32_t reserved; +} GlobalAccelerationTelemetry; +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 new file mode 100644 index 0000000..eac6c58 --- /dev/null +++ b/ateam-common-packets/include/robot_skills/global_position.h @@ -0,0 +1,24 @@ +#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); + +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 new file mode 100644 index 0000000..98a3589 --- /dev/null +++ b/ateam-common-packets/include/robot_skills/global_velocity.h @@ -0,0 +1,22 @@ +#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); + +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 new file mode 100644 index 0000000..2132d7f --- /dev/null +++ b/ateam-common-packets/include/robot_skills/local_acceleration.h @@ -0,0 +1,20 @@ +#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); + +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 new file mode 100644 index 0000000..12a3745 --- /dev/null +++ b/ateam-common-packets/include/robot_skills/local_velocity.h @@ -0,0 +1,22 @@ +#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); + +typedef struct ExtendedLocalVelocityTelemetry { + LocalVelocityCommand cmd_echo; +} ExtendedLocalVelocityTelemetry; +assert_size(ExtendedLocalVelocityTelemetry, 20); \ 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 new file mode 100644 index 0000000..5ce4b9d --- /dev/null +++ b/ateam-common-packets/include/stspin_current.h @@ -0,0 +1,148 @@ +#pragma once + +#include "common.h" + +typedef enum CcmCommandType { + CCM_CMD_PARAMS = 0x20, + CCM_CMD_MOTION = 0x21 +} __attribute__((packed)) CcmCommandType; +assert_size(CcmCommandType, 1); + +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)) CcmParameter; +assert_size(CcmParameter, 1); + +typedef enum CcmParameterOperation { + CCM_PARAMOP_READ = 0, + CCM_PARAMOP_WRITE = 1, +} __attribute__((packed)) CcmParameterOperation; +assert_size(CcmParameterOperation, 1); + +typedef enum CcmParameterDirection { + CCM_PARAMDIR_COMMAND = 0, + CCM_PARAMDIR_REPLY = 1, +} __attribute__((packed)) CcmParameterDirection; +assert_size(CcmParameterDirection, 1); + +typedef union CcmParameterValue { + uint32_t val_u32; + int32_t val_i32; + float val_f32; + uint8_t val_u8x4[4]; +} CcmParameterValue; + +typedef struct CurrentControlledMotor_ParameterPacket { + CcmParameter parameter; + CcmParameterOperation parameter_operation; + CcmParameterDirection parameter_direction; + uint8_t _reserved[1]; + CcmParameterValue value; +} __attribute__((packed)) CcmParameterPacket; +assert_size(CcmParameterPacket, 8); + +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)) CcmMotionControlType; +assert_size(CcmMotionControlType, 1); + +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; + + CcmMotionControlType motion_control_type; + uint8_t _reserved_2[1]; + int16_t current_setpoint_ma; + + float setpoint; +} __attribute__((__packed__)) CcmMotionCommand; +assert_size(CcmMotionCommand, 12); // Note: Same length as MotorCommand_Params_Packet + +typedef struct CcmCommand { + CcmCommandType type; + uint8_t _reserved[3]; + uint32_t crc32; + union CcmCommandData { + CcmParameterPacket param; + CcmMotionCommand motion; + } data; +} __attribute__((__packed__)) CcmCommand; +assert_size(CcmCommand, 20); + +///////////////// +// responses // +///////////////// + +typedef enum CcmResponseType { + CCM_RESP_PARAMS = 0x80, + CCM_RESP_TELEM = 0x81, +} __attribute__((packed)) CcmResponseType; +assert_size(CcmResponseType, 1); + +typedef struct CcmCurrentTelemetry { + uint16_t bus_voltage_mv; + uint16_t motor_voltage_cmd_mv; + + int16_t current_setpoint_ma; + uint8_t reserved[2]; + uint16_t current_samples_ma[20]; +} CcmCurrentTelemetry; +assert_size(CcmCurrentTelemetry, 48); + +typedef struct CcmVelocityTelemetry { + float vel_setpoint_rads; + float wheel_vel_rads; +} CcmVelocityTelemetry; +assert_size(CcmVelocityTelemetry, 8); + +typedef struct CcmTelemetry { + 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; + + CcmMotionControlType motion_control_type; + uint8_t gain_stage_index : 8; + + CcmCurrentTelemetry current_telemetry; + CcmVelocityTelemetry velocity_telemetry; + +} CcmTelemetry; +assert_size(CcmTelemetry, 60); + +typedef struct CcmResponse { + CcmResponseType type; + uint8_t seq_num; + uint8_t _reserved[2]; + + uint32_t timestamp; + union CcmResponseData { + CcmParameterPacket params; + CcmTelemetry motion; + } data; +} CcmResponse; +assert_size(CcmResponse, 68); \ No newline at end of file diff --git a/ateam-common-packets/rust-lib/Cargo.lock b/ateam-common-packets/rust-lib/Cargo.lock index a866fb8..55078b2 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.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d468802bab17cbc0cc575e9b053f41e72aa36bfa6b7f55e3529ffa43161b97fa" +checksum = "f2032f911046de80f0a198e0901378627c33f59ea0ac00e363d481118bd70a53" [[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.16.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3f107b87b6afc2a64fd13cac55fe06d6c8859f12d4b14cbcdd2c67d0976781be" +checksum = "91622ff5e7162018101f2fea40d6ebf4a78bbe5a49736a2020649edf9693679e" [[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.30" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "abb12e687cfb44aa40f41fc3978ef76448f9b6038cad6aef4259d3c095a2382e" -dependencies = [ - "cfg-if", -] +checksum = "616ec5685824bcc94416c6d4a7a446eea774a31efd7062c8480ba6fd06d7a6e5" [[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/build.rs b/ateam-common-packets/rust-lib/build.rs index db467bf..935b3e2 100644 --- a/ateam-common-packets/rust-lib/build.rs +++ b/ateam-common-packets/rust-lib/build.rs @@ -99,15 +99,6 @@ fn main() { // The input header we would like to generate // bindings for. .header("../include/radio.h") - .allowlist_file(".*/stspin.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(".*/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/ateam-common-packets/rust-lib/src/lib.rs b/ateam-common-packets/rust-lib/src/lib.rs index 0acfdd3..70717bd 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.global_xdd.is_finite() + && gpc.global_ydd.is_finite() + && gpc.global_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 diff --git a/ateam-common-packets/rust-lib/src/radio.rs b/ateam-common-packets/rust-lib/src/radio.rs index 97a7738..8559a28 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}; +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,9 +7,191 @@ 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) { + 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) { + 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), 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), +} + +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 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", 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 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