Packet Updates for New Control Modes

.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 }}

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);

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);

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);

ateam-common-packets/include/hello_data.h → 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];

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);
+

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);

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);
+assert_size(KickerTelemetry, 64);