Skip to content

Commit

Permalink
add referee flags
Browse files Browse the repository at this point in the history
  • Loading branch information
@irvingywang
irvingywang committed Apr 21, 2024
1 parent 9a370b1 commit 1d711e5
Show file tree
Hide file tree
Showing 3 changed files with 42 additions and 35 deletions.
3 changes: 2 additions & 1 deletion src/app/src/robot.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -200,7 +200,8 @@ void Robot_Cmd_Loop()
__MAX_LIMIT(g_robot_state.chassis_y_speed, -MAX_SPEED, MAX_SPEED);

/* power buffer*/
float power_buffer = Referee_System.Power_n_Heat.Chassis_Power_Buffer / 60.0f;
//float power_buffer = Referee_System.Power_n_Heat.Chassis_Power_Buffer / 60.0f;
float power_buffer = 1; //Temporary fix for power buffer
if (power_buffer < 0.8f)
{
g_robot_state.chassis_x_speed *= pow(power_buffer,1);
Expand Down
4 changes: 3 additions & 1 deletion src/devices/inc/jetson_orin.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@
#include "bsp_uart.h"

#define ORIN_DATA_RX_BUFER_SIZE (20)
#define ORIN_DATA_TX_BUFER_SIZE (33)
#define ORIN_DATA_TX_BUFER_SIZE (34)

#define ORIN_TIMEOUT_MS (3000)
#define JETSON_ORIN_PERIOD (100)
Expand All @@ -26,6 +26,8 @@ typedef struct
float orientation; // rad
float velocity_x; // m/s
float velocity_y; // m/s
uint8_t game_start_flag;
uint8_t enemy_color_flag;

union
{
Expand Down
70 changes: 37 additions & 33 deletions src/devices/src/jetson_orin.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@

#include "imu_task.h"
#include "bsp_daemon.h"
#include "referee_system.h"

Jetson_Orin_Data_t g_orin_data;

Expand All @@ -14,50 +15,50 @@ void Jetson_Orin_Rx_Callback(UART_Instance_t *uart_instance)
{
UNUSED(uart_instance);
// TODO: update buffer reference from uart_instance
if(g_orin_data.rx_buffer[0] == 0xAA)
if (g_orin_data.rx_buffer[0] == 0xAA)
{
// If matching frame header, reload Daemon
Daemon_Reload(g_orin_daemon_instance_ptr);

// Decode data
g_orin_data.receiving.frame_id = g_orin_data.rx_buffer[0];
g_orin_data.receiving.frame_type = g_orin_data.rx_buffer[1];
switch(g_orin_data.receiving.frame_type)
switch (g_orin_data.receiving.frame_type)
{
case 0:
memcpy(&g_orin_data.receiving.float_byte.data[0],&g_orin_data.rx_buffer[4],8*sizeof(uint8_t));
g_orin_data.receiving.auto_aiming.yaw = g_orin_data.receiving.float_byte.data[0];
g_orin_data.receiving.auto_aiming.pitch = g_orin_data.receiving.float_byte.data[1];
break;
case 1:
memcpy(&g_orin_data.receiving.float_byte.data[0],&g_orin_data.rx_buffer[4],12*sizeof(uint8_t));
g_orin_data.receiving.navigation.x_vel = g_orin_data.receiving.float_byte.data[0];
g_orin_data.receiving.navigation.y_vel = g_orin_data.receiving.float_byte.data[1];
g_orin_data.receiving.navigation.yaw_angular_rate = g_orin_data.receiving.float_byte.data[2];
g_orin_data.receiving.navigation.state = g_orin_data.rx_buffer[16];
break;
case 2:
g_orin_data.receiving.heart_beat.a = g_orin_data.rx_buffer[2];
g_orin_data.receiving.heart_beat.b = g_orin_data.rx_buffer[3];
g_orin_data.receiving.heart_beat.c = g_orin_data.rx_buffer[4];
g_orin_data.receiving.heart_beat.d = g_orin_data.rx_buffer[5];
break;
default:
break;
case 0:
memcpy(&g_orin_data.receiving.float_byte.data[0], &g_orin_data.rx_buffer[4], 8 * sizeof(uint8_t));
g_orin_data.receiving.auto_aiming.yaw = g_orin_data.receiving.float_byte.data[0];
g_orin_data.receiving.auto_aiming.pitch = g_orin_data.receiving.float_byte.data[1];
break;

case 1:
memcpy(&g_orin_data.receiving.float_byte.data[0], &g_orin_data.rx_buffer[4], 12 * sizeof(uint8_t));
g_orin_data.receiving.navigation.x_vel = g_orin_data.receiving.float_byte.data[0];
g_orin_data.receiving.navigation.y_vel = g_orin_data.receiving.float_byte.data[1];
g_orin_data.receiving.navigation.yaw_angular_rate = g_orin_data.receiving.float_byte.data[2];
g_orin_data.receiving.navigation.state = g_orin_data.rx_buffer[16];
break;

case 2:
g_orin_data.receiving.heart_beat.a = g_orin_data.rx_buffer[2];
g_orin_data.receiving.heart_beat.b = g_orin_data.rx_buffer[3];
g_orin_data.receiving.heart_beat.c = g_orin_data.rx_buffer[4];
g_orin_data.receiving.heart_beat.d = g_orin_data.rx_buffer[5];
break;

default:
break;
}
}
}

/**
* @brief Callback function for timeout of Jetson Orin used by Daemon
* @details When not receiving data from Orin for a certain time, will try
* @details When not receiving data from Orin for a certain time, will try
* to reinitialize the UART service. Daemon is reloaded to prevent
* continuous reinitialization at high frequency.
* continuous reinitialization at high frequency.
* @param void
*/
*/
void Jetson_Orin_Timeout_Callback()
{
// Attemp to reinitialize UART service
Expand All @@ -68,7 +69,7 @@ void Jetson_Orin_Init(UART_HandleTypeDef *huartx)
{
// register UART instance
g_orin_uart_instance_ptr = UART_Register(huartx, g_orin_data.rx_buffer, ORIN_DATA_RX_BUFER_SIZE, Jetson_Orin_Rx_Callback);

// register Daemon instance
// timeout is defined in the header file
uint16_t reload_value = ORIN_TIMEOUT_MS / DAEMON_PERIOD;
Expand All @@ -87,7 +88,9 @@ void Jetson_Orin_Send_Data(void)
g_orin_data.sending.orientation = g_imu.rad.yaw;
g_orin_data.sending.velocity_x = 0;
g_orin_data.sending.velocity_y = 0;

g_orin_data.sending.game_start_flag = (Referee_System.Game_Status.Progress == 4) ? 1 : 0;
g_orin_data.sending.enemy_color_flag = (Referee_System.Robot_State.ID > 11) ? 1 : 0;

// float to byte conversion
g_orin_data.sending.float_byte.data[0] = g_orin_data.sending.pitch_angle;
g_orin_data.sending.float_byte.data[1] = g_orin_data.sending.pitch_angular_rate;
Expand All @@ -97,9 +100,10 @@ void Jetson_Orin_Send_Data(void)
g_orin_data.sending.float_byte.data[5] = g_orin_data.sending.orientation;
g_orin_data.sending.float_byte.data[6] = g_orin_data.sending.velocity_x;
g_orin_data.sending.float_byte.data[7] = g_orin_data.sending.velocity_y;

g_orin_data.tx_buffer[0] = 0xAA;
memcpy(&g_orin_data.tx_buffer[1],&g_orin_data.sending.float_byte.data_bytes[0], 32*sizeof(uint8_t));

g_orin_data.tx_buffer[1] = g_orin_data.sending.enemy_color_flag << 1 | g_orin_data.sending.game_start_flag;
memcpy(&g_orin_data.tx_buffer[2], &g_orin_data.sending.float_byte.data_bytes[0], 32 * sizeof(uint8_t));

//UART_Transmit(g_orin_uart_instance_ptr, g_orin_data.tx_buffer, sizeof(g_orin_data.tx_buffer), UART_DMA);
}

0 comments on commit 1d711e5

Please sign in to comment.