diff --git a/examples/autoaim/CMakeLists.txt b/examples/autoaim/CMakeLists.txt
index 43978ebd..62c3064d 100644
--- a/examples/autoaim/CMakeLists.txt
+++ b/examples/autoaim/CMakeLists.txt
@@ -20,6 +20,6 @@
project(example_autoaim ASM C CXX)
-#irm_add_arm_executable(${PROJECT_NAME}
-# TARGET DJI_Board_TypeC
-# SOURCES main.cc)
+irm_add_arm_executable(${PROJECT_NAME}
+ TARGET DJI_Board_TypeC
+ SOURCES main.cc)
diff --git a/examples/autoaim/main.cc b/examples/autoaim/main.cc
index 52971731..67a8c3e7 100644
--- a/examples/autoaim/main.cc
+++ b/examples/autoaim/main.cc
@@ -27,7 +27,7 @@
#include "gimbal.h"
#include "rgb.h"
#include "bsp_gpio.h"
-#include "minipc_protocol.h"
+#include "autoaim_protocol.h"
#include "filtering.h"
#include "i2c.h"
#include "bsp_imu.h"
@@ -305,4 +305,4 @@ void RM_RTOS_Default_Task(const void* args) {
control::MotorCANBase::TransmitOutput(motors, 2);
osDelay(10);
}
-}
+}
\ No newline at end of file
diff --git a/examples/minipc/CMakeLists.txt b/examples/minipc/CMakeLists.txt
index 004a9a19..d436ac53 100644
--- a/examples/minipc/CMakeLists.txt
+++ b/examples/minipc/CMakeLists.txt
@@ -24,19 +24,7 @@ irm_add_arm_executable(${PROJECT_NAME}
TARGET DJI_Board_TypeA
SOURCES typeA.cc)
-irm_add_arm_executable(${PROJECT_NAME}_stresstesttypec
+irm_add_arm_executable(${PROJECT_NAME}_typeC
TARGET DJI_Board_TypeC
- SOURCES StressTestTypeC.cc)
-
-irm_add_arm_executable(${PROJECT_NAME}_pingpongtest
- TARGET DJI_Board_TypeC
- SOURCES PingpongTest.cc)
-
-irm_add_arm_executable(${PROJECT_NAME}_motor
- TARGET DJI_Board_TypeA
- SOURCES MotorTest.cc)
-
-irm_add_arm_executable(${PROJECT_NAME}_latency
- TARGET DJI_Board_TypeC
- SOURCES LatencyTest.cc)
+ SOURCES typeC.cc)
diff --git a/examples/minipc/LatencyTest.cc b/examples/minipc/LatencyTest.cc
deleted file mode 100644
index 86e85e5c..00000000
--- a/examples/minipc/LatencyTest.cc
+++ /dev/null
@@ -1,96 +0,0 @@
-/****************************************************************************
- * *
- * Copyright (C) 2023 RoboMaster. *
- * Illini RoboMaster @ University of Illinois at Urbana-Champaign *
- * *
- * This program is free software: you can redistribute it and/or modify *
- * it under the terms of the GNU General Public License as published by *
- * the Free Software Foundation, either version 3 of the License, or *
- * (at your option) any later version. *
- * *
- * This program is distributed in the hope that it will be useful, *
- * but WITHOUT ANY WARRANTY; without even the implied warranty of *
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
- * GNU General Public License for more details. *
- * *
- * You should have received a copy of the GNU General Public License *
- * along with this program. If not, see . *
- * *
- ****************************************************************************/
-
-#include "main.h"
-
-#include
-#include
-
-#include "bsp_gpio.h"
-#include "bsp_print.h"
-#include "bsp_uart.h"
-#include "cmsis_os.h"
-#include "minipc_protocol.h"
-#include "rgb.h"
-
-#define RX_SIGNAL (1 << 0)
-
-extern osThreadId_t defaultTaskHandle;
-
-static display::RGB* led = nullptr;
-
-class CustomUART : public bsp::UART {
- public:
- using bsp::UART::UART;
-
- protected:
- /* notify application when rx data is pending read */
- void RxCompleteCallback() override final { osThreadFlagsSet(defaultTaskHandle, RX_SIGNAL); }
-};
-
-
-void RM_RTOS_Init(void) {
- led = new display::RGB(&htim5, 3, 2, 1, 1000000);
-}
-
-// Latency test. Use with communication/communicator.py in iRM_Vision_2023 repo
-// In the communicator.py, need to set testing = Test.LATENCY for this test
-void RM_RTOS_Default_Task(const void* argument) {
- UNUSED(argument);
-
- auto uart = std::make_unique(&huart1);
- uart->SetupRx(50);
- uart->SetupTx(50);
-
- auto minipc_session = communication::MinipcPort();
-
- communication::chassis_data_t chassis_data; // this has to be the data type that has the maximum size
- //if changed, please make sure the data type in `case Test.LATENCY` communicator.py in the vision repo is also changed
-
- const communication::status_data_t* status_data;
-
- chassis_data.vx = 0.0;
- chassis_data.vy = 0.0;
- chassis_data.vw = 0.0;
-
- uint8_t packet_to_send[minipc_session.MAX_PACKET_LENGTH];
- uint8_t *data;
- int32_t length;
-
- while (true) {
- /* wait until rx data is available */
- //led->Display(0xFF0000FF);
-
- // Wait until first packet from minipc.
- uint32_t flags = osThreadFlagsWait(RX_SIGNAL, osFlagsWaitAll, osWaitForever);
- if (flags & RX_SIGNAL) {
- length = uart->Read(&data);
- minipc_session.ParseUartBuffer(data, length);
- status_data = minipc_session.GetStatus();
-
-
- chassis_data.vx = status_data->vx;
- minipc_session.Pack(packet_to_send, (void*)&chassis_data, communication::CHASSIS_CMD_ID);
- uart->Write(packet_to_send, minipc_session.GetPacketLen(communication::CHASSIS_CMD_ID));
-
- }
- osDelay(10);
- }
-}
diff --git a/examples/minipc/MotorTest.cc b/examples/minipc/MotorTest.cc
deleted file mode 100644
index d89584e8..00000000
--- a/examples/minipc/MotorTest.cc
+++ /dev/null
@@ -1,147 +0,0 @@
-/****************************************************************************
- * *
- * Copyright (C) 2023 RoboMaster. *
- * Illini RoboMaster @ University of Illinois at Urbana-Champaign *
- * *
- * This program is free software: you can redistribute it and/or modify *
- * it under the terms of the GNU General Public License as published by *
- * the Free Software Foundation, either version 3 of the License, or *
- * (at your option) any later version. *
- * *
- * This program is distributed in the hope that it will be useful, *
- * but WITHOUT ANY WARRANTY; without even the implied warranty of *
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
- * GNU General Public License for more details. *
- * *
- * You should have received a copy of the GNU General Public License *
- * along with this program. If not, see . *
- * *
- ****************************************************************************/
-
-#include "main.h"
-
-#include
-#include
-
-#include "bsp_gpio.h"
-#include "bsp_print.h"
-#include "bsp_uart.h"
-#include "cmsis_os.h"
-#include "minipc_protocol.h"
-#include "controller.h"
-#include "motor.h"
-
-#include "bsp_gpio.h"
-
-#define RX_SIGNAL (1 << 0)
-
-// They don't need to be different. RX_SIGNAL is thread flag, VX_READY_SIGNAL is event flag
-#define VX_READY_SIGNAL (1 << 1)
-
-osEventFlagsId_t vx_flag_id;
-
-static bsp::GPIO *gpio_red;
-
-static float vx = 0;
-
-/* init new task START */
-static osThreadId_t MotorTaskHandle;
-
-const osThreadAttr_t MotorTaskAttributes = {.name = "MotorTask",
- .attr_bits = osThreadDetached,
- .cb_mem = nullptr,
- .cb_size = 0,
- .stack_mem = nullptr,
- .stack_size = 128 * 4,
- .priority = (osPriority_t)osPriorityNormal,
- .tz_module = 0,
- .reserved = 0};
-
-
-bsp::CAN* can = nullptr;
-control::MotorCANBase* motor1 = nullptr;
-
-void MotorTask(void* argument) {
- UNUSED(argument);
- uint32_t flags;
- control::PIDController pid1(20, 0, 0);
- control::MotorCANBase* motors[] = {motor1};
-
- while (true) {
- // Wait time = 50 ticks, 50ms?
- flags = 0;
- flags = osEventFlagsWait(vx_flag_id, VX_READY_SIGNAL, osFlagsWaitAny, 50);
- // When timeout it returns -2 so we need extra checks here
- if (flags != osFlagsErrorTimeout && flags & VX_READY_SIGNAL) {
- // if receives packet, drive the motor and toggle RED LED
- float diff = 0;
- int16_t out = 0;
- diff = motor1->GetOmegaDelta(vx);
- out = pid1.ComputeConstrainedOutput(diff);
- motor1->SetOutput(out);
- gpio_red->Toggle();
- } else {
- // if timeout (no packet, stop the motor)
- float diff = 0;
- int16_t out = 0;
- diff = motor1->GetOmegaDelta(0);
- out = pid1.ComputeConstrainedOutput(diff);
- motor1->SetOutput(out);
- }
- control::MotorCANBase::TransmitOutput(motors, 1);
- osDelay(10);
- }
-}
-
-extern osThreadId_t defaultTaskHandle;
-
-class CustomUART : public bsp::UART {
- public:
- using bsp::UART::UART;
-
- protected:
- /* notify application when rx data is pending read */
- void RxCompleteCallback() override final { osThreadFlagsSet(defaultTaskHandle, RX_SIGNAL); }
-};
-
-
-void RM_RTOS_Threads_Init(void) {
- MotorTaskHandle = osThreadNew(MotorTask, nullptr, &MotorTaskAttributes);
-}
-
-void RM_RTOS_Init(void) {
- can = new bsp::CAN(&hcan1, true);
- motor1 = new control::Motor3508(can, 0x201);
- gpio_red = new bsp::GPIO(LED_RED_GPIO_Port, LED_RED_Pin);
- gpio_red->High();
- vx_flag_id = osEventFlagsNew(nullptr);
-}
-
-void RM_RTOS_Default_Task(const void* argument) {
- UNUSED(argument);
-
- auto uart = std::make_unique(&huart8);
- uart->SetupRx(50);
- uart->SetupTx(50);
-
- auto minipc_session = communication::MinipcPort();
-
- const communication::status_data_t* status_data;
-
- uint8_t *data;
- int32_t length;
-
- // When packet arrives, raise a eventflag
- while (true) {
- uint32_t flags = osThreadFlagsWait(RX_SIGNAL, osFlagsWaitAll, osWaitForever);
- if (flags & RX_SIGNAL) {
- length = uart->Read(&data);
- minipc_session.ParseUartBuffer(data, length);
- status_data = minipc_session.GetStatus();
- vx = status_data->vx;
- osEventFlagsSet(vx_flag_id, VX_READY_SIGNAL);
- }
- osDelay(10);
- }
-}
-
diff --git a/examples/minipc/PingpongTest.cc b/examples/minipc/PingpongTest.cc
deleted file mode 100644
index 077475cb..00000000
--- a/examples/minipc/PingpongTest.cc
+++ /dev/null
@@ -1,95 +0,0 @@
-/****************************************************************************
- * *
- * Copyright (C) 2023 RoboMaster. *
- * Illini RoboMaster @ University of Illinois at Urbana-Champaign *
- * *
- * This program is free software: you can redistribute it and/or modify *
- * it under the terms of the GNU General Public License as published by *
- * the Free Software Foundation, either version 3 of the License, or *
- * (at your option) any later version. *
- * *
- * This program is distributed in the hope that it will be useful, *
- * but WITHOUT ANY WARRANTY; without even the implied warranty of *
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
- * GNU General Public License for more details. *
- * *
- * You should have received a copy of the GNU General Public License *
- * along with this program. If not, see . *
- * *
- ****************************************************************************/
-
-#include "main.h"
-
-#include
-#include
-
-#include "bsp_gpio.h"
-#include "bsp_print.h"
-#include "bsp_uart.h"
-#include "cmsis_os.h"
-#include "minipc_protocol.h"
-#include "rgb.h"
-
-#define RX_SIGNAL (1 << 0)
-
-extern osThreadId_t defaultTaskHandle;
-
-static display::RGB* led = nullptr;
-
-class CustomUART : public bsp::UART {
- public:
- using bsp::UART::UART;
-
- protected:
- /* notify application when rx data is pending read */
- void RxCompleteCallback() override final { osThreadFlagsSet(defaultTaskHandle, RX_SIGNAL); }
-};
-
-void RM_RTOS_Init(void) {
- led = new display::RGB(&htim5, 3, 2, 1, 1000000);
-}
-
-void RM_RTOS_Default_Task(const void* argument) {
- UNUSED(argument);
-
- auto uart = std::make_unique(&huart1);
- uart->SetupRx(50);
- uart->SetupTx(50);
-
- auto minipc_session = communication::MinipcPort();
-
- communication::gimbal_data_t gimbal_data;
-
- const communication::status_data_t* status_data;
-
- gimbal_data.rel_yaw = 100;
- gimbal_data.rel_pitch = 200;
- gimbal_data.debug_int = 50;
- gimbal_data.mode = 1;
-
- uint8_t packet_to_send[minipc_session.MAX_PACKET_LENGTH];
- uint8_t *data;
- int32_t length;
-
- while (true) {
- /* wait until rx data is available */
- //led->Display(0xFF0000FF);
-
- // TX RX test. Use with communication/communicator.py in iRM_Vision_2023 repo
- // In the communicator.py, need to set testing=Test.PINGPONG for this test
-
- // Wait until first packet from minipc.
- uint32_t flags = osThreadFlagsWait(RX_SIGNAL, osFlagsWaitAll, osWaitForever);
- if (flags & RX_SIGNAL) {
- // When packet received from miniPC, increase rel_pitch by 1 and send back
- length = uart->Read(&data);
- minipc_session.ParseUartBuffer(data, length);
- status_data = minipc_session.GetStatus();
- gimbal_data.rel_pitch = status_data->rel_pitch + 1;
- gimbal_data.rel_yaw = status_data->rel_yaw;
- minipc_session.Pack(packet_to_send, (void*)&gimbal_data, communication::GIMBAL_CMD_ID);
- uart->Write(packet_to_send, minipc_session.GetPacketLen(communication::GIMBAL_CMD_ID));
- }
- osDelay(10);
- }
-}
diff --git a/examples/minipc/typeA.cc b/examples/minipc/typeA.cc
index 9150317c..2afbe633 100644
--- a/examples/minipc/typeA.cc
+++ b/examples/minipc/typeA.cc
@@ -23,66 +23,59 @@
#include
#include
+#include "bsp_gpio.h"
#include "bsp_print.h"
#include "bsp_uart.h"
#include "cmsis_os.h"
-#include "minipc_protocol.h"
+#include "autoaim_protocol.h"
#define RX_SIGNAL (1 << 0)
extern osThreadId_t defaultTaskHandle;
+static bsp::GPIO *gpio_red, *gpio_green;
+
class CustomUART : public bsp::UART {
-public:
+ public:
using bsp::UART::UART;
-protected:
+ protected:
/* notify application when rx data is pending read */
void RxCompleteCallback() override final { osThreadFlagsSet(defaultTaskHandle, RX_SIGNAL); }
};
-void RM_RTOS_Init(void) {
-}
-
void RM_RTOS_Default_Task(const void* argument) {
UNUSED(argument);
- auto uart = std::make_unique(&huart8);
+ uint32_t length;
+ uint8_t* data;
+
+ auto uart = std::make_unique(&huart8); // see cmake for which uart
uart->SetupRx(50);
uart->SetupTx(50);
- auto minipc_session = communication::MinipcPort();
-
- communication::gimbal_data_t gimbal_data;
- communication::color_data_t color_data;
- communication::chassis_data_t chassis_data;
-
- uint8_t packet_to_send[minipc_session.MAX_PACKET_LENGTH];
+ gpio_red = new bsp::GPIO(LED_RED_GPIO_Port, LED_RED_Pin);
+ gpio_green = new bsp::GPIO(LED_GREEN_GPIO_Port, LED_GREEN_Pin);
- /* To run this test, use Communication/communicator.py in the iRM_Vision_2023 repo
- * Need to set testing=Test.TYPE_A
- **/
+ auto miniPCreceiver = communication::AutoaimProtocol();
while (true) {
- // Send packet example. Send packet at 1 Hz
- gimbal_data.rel_yaw = 100;
- gimbal_data.rel_pitch = 200;
- gimbal_data.debug_int = 50;
- gimbal_data.mode = 1;
- minipc_session.Pack(packet_to_send, (void*)&gimbal_data, communication::GIMBAL_CMD_ID);
- uart->Write(packet_to_send, minipc_session.GetPacketLen(communication::GIMBAL_CMD_ID));
- osDelay(1000);
-
- color_data.my_color = 1;
- minipc_session.Pack(packet_to_send, (void*)&color_data, communication::COLOR_CMD_ID);
- uart->Write(packet_to_send, minipc_session.GetPacketLen(communication::COLOR_CMD_ID));
- osDelay(1000);
-
- chassis_data.vx = 20;
- chassis_data.vy = 30;
- chassis_data.vw = 40;
- minipc_session.Pack(packet_to_send, (void*)&chassis_data, communication::CHASSIS_CMD_ID);
- uart->Write(packet_to_send, minipc_session.GetPacketLen(communication::CHASSIS_CMD_ID));
- osDelay(1000);
+ /* wait until rx data is available */
+ uint32_t flags = osThreadFlagsWait(RX_SIGNAL, osFlagsWaitAll, osWaitForever);
+ if (flags & RX_SIGNAL) { // unnecessary check
+ /* time the non-blocking rx / tx calls (should be <= 1 osTick) */
+ length = uart->Read(&data);
+
+ // if read anything, flash red
+ gpio_red->High();
+
+ miniPCreceiver.Receive(data, length);
+ if (miniPCreceiver.get_valid_flag() == 1) {
+ gpio_green->High();
+ }
+ osDelay(200);
+ gpio_green->Low();
+ gpio_red->Low();
+ }
}
}
diff --git a/examples/minipc/StressTestTypeC.cc b/examples/minipc/typeC.cc
similarity index 79%
rename from examples/minipc/StressTestTypeC.cc
rename to examples/minipc/typeC.cc
index eefe2973..703316a4 100644
--- a/examples/minipc/StressTestTypeC.cc
+++ b/examples/minipc/typeC.cc
@@ -27,7 +27,7 @@
#include "bsp_print.h"
#include "bsp_uart.h"
#include "cmsis_os.h"
-#include "minipc_protocol.h"
+#include "autoaim_protocol.h"
#include "rgb.h"
#define RX_SIGNAL (1 << 0)
@@ -55,31 +55,29 @@ void RM_RTOS_Default_Task(const void* argument) {
uint32_t length;
uint8_t* data;
- auto uart = std::make_unique(&huart1);
+ auto uart = std::make_unique(&huart1); // see cmake for which uart
uart->SetupRx(50);
uart->SetupTx(50);
- auto minipc_session = communication::MinipcPort();
+ auto miniPCreceiver = communication::AutoaimProtocol();
int total_processed_bytes = 0;
- /* To run this test, use Communication/communicator.py in the iRM_Vision_2023 repo
- * Need to set testing=Test.CRC
- **/
-
while (true) {
/* wait until rx data is available */
led->Display(0xFF0000FF);
- int32_t flags = osThreadFlagsWait(RX_SIGNAL, osFlagsWaitAll, osWaitForever);
+ // An alternative is to use osThreadFlagsWait.
+ // However, we want to experiment with periodic sending
+ uint32_t flags = osThreadFlagsGet();
if (flags & RX_SIGNAL) {
/* time the non-blocking rx / tx calls (should be <= 1 osTick) */
// max length of the UART buffer at 150Hz is ~50 bytes
length = uart->Read(&data);
total_processed_bytes += length;
- minipc_session.ParseUartBuffer(data, length);
- uint32_t valid_packet_cnt = minipc_session.GetValidPacketCnt();
- uint8_t packet_to_send[minipc_session.MAX_PACKET_LENGTH];
+
+ miniPCreceiver.Receive(data, length);
+ uint32_t valid_packet_cnt = miniPCreceiver.get_valid_packet_cnt();
// Jetson / PC sends 200Hz valid packets for stress testing
// For testing script, please see iRM_Vision_2023/Communication/communicator.py
@@ -87,10 +85,9 @@ void RM_RTOS_Default_Task(const void* argument) {
if (valid_packet_cnt == 1000) {
// Jetson test cases write 1000 packets. Pass
led->Display(0xFF00FF00);
- osDelay(4000);
- led->Display(0xFFFF0000);
+ osDelay(10000);
// after 10 seconds, write 1000 alternating packets to Jetson
- communication::color_data_t color_data;
+ communication::STMToJetsonData packet_to_send;
uint8_t my_color = 1; // blue
for (int i = 0; i < 1000; ++i) {
if (i % 2 == 0) {
@@ -98,9 +95,8 @@ void RM_RTOS_Default_Task(const void* argument) {
} else {
my_color = 0; // red
}
- color_data.my_color = my_color;
- minipc_session.Pack(packet_to_send, (void*)&color_data, communication::COLOR_CMD_ID);
- uart->Write(packet_to_send, minipc_session.GetPacketLen(communication::COLOR_CMD_ID));
+ miniPCreceiver.Send(&packet_to_send, my_color, 0.5, 0.42, 0);
+ uart->Write((uint8_t*)&packet_to_send, sizeof(communication::STMToJetsonData));
// NOTE: THIS BREAKS WHEN WORKING AT 1000HZ!
osDelay(2);
}
@@ -110,4 +106,4 @@ void RM_RTOS_Default_Task(const void* argument) {
}
osDelay(2);
}
-}
+}
\ No newline at end of file
diff --git a/shared/libraries/autoaim_protocol.cc b/shared/libraries/autoaim_protocol.cc
new file mode 100644
index 00000000..e355f9b9
--- /dev/null
+++ b/shared/libraries/autoaim_protocol.cc
@@ -0,0 +1,175 @@
+/****************************************************************************
+ * *
+ * Copyright (C) 2023 RoboMaster. *
+ * Illini RoboMaster @ University of Illinois at Urbana-Champaign *
+ * *
+ * This program is free software: you can redistribute it and/or modify *
+ * it under the terms of the GNU General Public License as published by *
+ * the Free Software Foundation, either version 3 of the License, or *
+ * (at your option) any later version. *
+ * *
+ * This program is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
+ * GNU General Public License for more details. *
+ * *
+ * You should have received a copy of the GNU General Public License *
+ * along with this program. If not, see . *
+ * *
+ ****************************************************************************/
+
+#include "autoaim_protocol.h"
+
+#include
+#include
+
+namespace communication {
+
+AutoaimProtocol::AutoaimProtocol() {
+ index = 0; // current pointer to write
+ flag = 0;
+}
+
+void AutoaimProtocol::Send(STMToJetsonData* packet, uint8_t color, float cur_yaw, float cur_pitch, uint32_t additional_info) {
+ packet->header[0] = 'H';
+ packet->header[1] = 'D';
+ packet->my_color = color;
+ packet->cur_yaw = (int32_t)(cur_yaw * INT_FP_SCALE);
+ packet->cur_pitch = (int32_t)(cur_pitch * INT_FP_SCALE);
+ packet->additional_info = additional_info;
+
+ const int tail_offset = 3; // size of data minus uint8_t checksum and 2 uint8_t tail
+ packet->crc8_checksum = get_crc8_check_sum((uint8_t*)packet,
+ sizeof(STMToJetsonData) - tail_offset,
+ 0);
+
+ packet->tail[0] = 'E';
+ packet->tail[1] = 'D';
+}
+
+void AutoaimProtocol::Receive(const uint8_t* data, uint8_t length) {
+ // Four cases
+ // Case 1: everything is fresh with complete package(s)
+ // Case 2: everything is fresh; package is incomplete
+ // Case 3: package contains half previous package and half new package
+ // Case 4: package contains half previous package and new package(s)
+
+ if (index > 0) {
+ // Case 3 and 4
+ // copy the remaining bytes from previous package
+ int remain = std::min((int)PKG_LEN - index, (int)length);
+ memcpy(host_command + index, data, remain);
+ index += remain;
+
+ if (index == PKG_LEN) {
+ // Case 3
+ // done package reading
+ index = 0;
+ handle();
+ } else {
+ if (remain == length) {
+ // exhausted current package already; and not reaching PKG_LEN
+ return;
+ }
+ }
+ }
+
+ int i = 0;
+
+ while (i < (int)length) {
+ if (index == 0) {
+ if (i == (int)length - 1) {
+ // A special case to handle the last byte; index must be zero
+ if (data[i] == 'S' || data[i] == 'M') {
+ host_command[index++] = data[i];
+ }
+ return;
+ }
+ if ((data[i] == 'S' && data[i + 1] == 'T') ||
+ (data[i] == 'M' && data[i + 1] == 'Y')) {
+ // Detect packet head; start copying
+ host_command[index++] = data[i++];
+ host_command[index++] = data[i++];
+ } else {
+ i++;
+ }
+ } else {
+ host_command[index++] = data[i++];
+ if (index == PKG_LEN) {
+ index = 0;
+ handle();
+ }
+ }
+ }
+}
+
+void AutoaimProtocol::handle(void) {
+ // TODO: implement thread-safe logic here (use a lock to handle changes from interrupt)
+ // here we can assume that the package is complete
+ // in the host_command buffer
+
+ // TODO: add a logic here such that when the checking fails; it moves the write pointer 'index'
+ // to the next 'S' or 'M' for more robustness.
+ // A minor issue with current implementation: imagine the following case:
+ // Two packets arrive in two UART calls.
+ // The first packet misses 1 byte, but the second one is complete.
+ // In this case, when the host_command buffer is filled
+ // (the last byte is 'S' or 'M' for the second packet), handle() will be called. The whole buffer
+ // would be tossed, resulting in two unusable packets. However, if we implement this logic, we would be
+ // able to recover the second packet.
+
+ // This is a minor issue because
+ // 1) we don't observe this even when packets are sent at 200Hz
+ // 2) probability of this happening is very low. The second packet has to be sent in two slices to
+ // trigger this issue. (first slice: S/T is sent to host_command; second slide: the rest)
+
+ // check end of packet is 'ED'
+ if (host_command[PKG_LEN - 2] != 'E' || host_command[PKG_LEN - 1] != 'D') {
+ flag = 0;
+ return;
+ }
+
+ if (verify_crc8_check_sum(host_command, PKG_LEN - 2)) {
+ process_data();
+ valid_packet_cnt++;
+ flag = 1;
+ } else {
+ flag = 0;
+ }
+}
+
+float AutoaimProtocol::get_relative_yaw(void) {
+ return relative_yaw;
+}
+
+float AutoaimProtocol::get_relative_pitch(void) {
+ return relative_pitch;
+}
+
+uint32_t AutoaimProtocol::get_seqnum(void) {
+ return seqnum;
+}
+
+uint32_t AutoaimProtocol::get_valid_packet_cnt(void) {
+ return valid_packet_cnt;
+}
+
+void AutoaimProtocol::process_data() {
+ // Assume that the host_command is a complete and verified message
+
+ // char pointer because host_command is a byte array
+ uint8_t* seq_num_start = host_command + this->SEQNUM_OFFSET;
+ uint8_t* rel_yaw_start = host_command + this->REL_YAW_OFFSET;
+ uint8_t* rel_pitch_start = host_command + this->REL_PITCH_OFFSET;
+
+ seqnum = (*(uint32_t *)seq_num_start);
+ relative_yaw = (*(int32_t *)rel_yaw_start) * 1.0f / this->INT_FP_SCALE;
+ relative_pitch = *(int32_t *)rel_pitch_start *1.0f / this->INT_FP_SCALE;
+}
+
+uint8_t AutoaimProtocol::get_valid_flag(void) {
+ uint8_t temp = flag;
+ flag = 0;
+ return temp;
+}
+} // namespace communication
\ No newline at end of file
diff --git a/shared/libraries/autoaim_protocol.h b/shared/libraries/autoaim_protocol.h
new file mode 100644
index 00000000..e7a2131d
--- /dev/null
+++ b/shared/libraries/autoaim_protocol.h
@@ -0,0 +1,76 @@
+/****************************************************************************
+ * *
+ * Copyright (C) 2023 RoboMaster. *
+ * Illini RoboMaster @ University of Illinois at Urbana-Champaign *
+ * *
+ * This program is free software: you can redistribute it and/or modify *
+ * it under the terms of the GNU General Public License as published by *
+ * the Free Software Foundation, either version 3 of the License, or *
+ * (at your option) any later version. *
+ * *
+ * This program is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
+ * GNU General Public License for more details. *
+ * *
+ * You should have received a copy of the GNU General Public License *
+ * along with this program. If not, see . *
+ * *
+ ****************************************************************************/
+
+#pragma once
+
+#include "bsp_print.h"
+#include "bsp_uart.h"
+#include "cmsis_os.h"
+#include "crc8.h"
+
+namespace communication {
+
+typedef struct {
+ char header[2];
+ uint8_t my_color; // RED is 0; BLUE is one
+ int32_t cur_yaw;
+ int32_t cur_pitch;
+ uint32_t additional_info;
+ uint8_t crc8_checksum;
+ char tail[2];
+} __packed STMToJetsonData;
+
+// WARNING: THIS CLASS IS NOT THREAD SAFE!!!
+
+class AutoaimProtocol {
+ public:
+ AutoaimProtocol();
+ void Receive(const uint8_t* data, uint8_t len);
+ // dummy send
+ void Send(STMToJetsonData* packet, uint8_t color, float cur_yaw, float cur_pitch, uint32_t additional_info);
+ uint8_t get_valid_flag(void);
+ float get_relative_yaw(void);
+ float get_relative_pitch(void);
+ uint32_t get_seqnum(void);
+ uint32_t get_valid_packet_cnt(void);
+
+ private:
+ // For definitions of constants, check out the documentation at either
+ // https://github.com/illini-robomaster/iRM_Vision_2023/blob/roger/crc_comm/docs/comm_protocol.md
+ // or https://github.com/illini-robomaster/iRM_Vision_2023/tree/docs/comm_protocol.md
+ static constexpr uint8_t PKG_LEN = 17; // jetson to STM32 length
+ static constexpr int32_t INT_FP_SCALE = 1000000;
+ static constexpr uint8_t SEQNUM_OFFSET = 2;
+ static constexpr uint8_t REL_YAW_OFFSET = SEQNUM_OFFSET + 4;
+ static constexpr uint8_t REL_PITCH_OFFSET = REL_YAW_OFFSET + 4;
+
+ int index;
+ uint8_t flag;
+ uint8_t host_command[PKG_LEN];
+ void handle();
+ void process_data();
+
+ float relative_yaw;
+ float relative_pitch;
+ uint32_t seqnum;
+ uint32_t valid_packet_cnt = 0;
+}; /* class AutoaimProtocol */
+
+} /* namespace communication */
\ No newline at end of file
diff --git a/shared/libraries/minipc_protocol.cc b/shared/libraries/minipc_protocol.cc
deleted file mode 100644
index e6345d0e..00000000
--- a/shared/libraries/minipc_protocol.cc
+++ /dev/null
@@ -1,266 +0,0 @@
-/****************************************************************************
- * *
- * Copyright (C) 2023 RoboMaster. *
- * Illini RoboMaster @ University of Illinois at Urbana-Champaign *
- * *
- * This program is free software: you can redistribute it and/or modify *
- * it under the terms of the GNU General Public License as published by *
- * the Free Software Foundation, either version 3 of the License, or *
- * (at your option) any later version. *
- * *
- * This program is distributed in the hope that it will be useful, *
- * but WITHOUT ANY WARRANTY; without even the implied warranty of *
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
- * GNU General Public License for more details. *
- * *
- * You should have received a copy of the GNU General Public License *
- * along with this program. If not, see . *
- * *
- ****************************************************************************/
-
-#include "minipc_protocol.h"
-
-#include
-#include
-
-namespace communication {
-
-// For definitions of constants, check out the documentation at
-// illini-robomaster/iRM_Vision_2023/docs/comm_protocol.md
-static constexpr uint8_t SEQNUM_OFFSET = 2;
-static constexpr uint8_t DATA_LENGTH_OFFSET = SEQNUM_OFFSET + 2;
-static constexpr uint8_t CMD_ID_OFFSET = DATA_LENGTH_OFFSET + 1;
-static constexpr uint8_t DATA_OFFSET = CMD_ID_OFFSET + 1;
-
-MinipcPort::MinipcPort() {
- buffer_index_ = 0; // current pointer to write
- valid_flag_ = 0;
- seqnum_ = 0;
-}
-
-void MinipcPort::Pack(uint8_t* packet, void* data, uint8_t cmd_id) {
- switch (cmd_id) {
- case GIMBAL_CMD_ID:
- PackGimbalData(packet, static_cast(data));
- break;
- case COLOR_CMD_ID:
- PackColorData(packet, static_cast(data));
- break;
- case CHASSIS_CMD_ID:
- PackChassisData(packet, static_cast(data));
- break;
- }
-}
-
-void MinipcPort::PackGimbalData(uint8_t* packet, gimbal_data_t* data) {
- AddHeaderTail(packet, GIMBAL_CMD_ID);
- memcpy(&packet[0 + DATA_OFFSET], &data->rel_yaw, sizeof(float));
- memcpy(&packet[4 + DATA_OFFSET], &data->rel_pitch, sizeof(float));
- packet[8 + DATA_OFFSET] = data->mode;
- packet[9 + DATA_OFFSET] = data->debug_int;
- AddCRC8(packet, GIMBAL_CMD_ID);
-}
-
-void MinipcPort::PackColorData(uint8_t* packet, color_data_t* data) {
- AddHeaderTail(packet, COLOR_CMD_ID);
- packet[DATA_OFFSET] = data->my_color;
- AddCRC8(packet, COLOR_CMD_ID);
-}
-
-void MinipcPort::PackChassisData(uint8_t* packet, chassis_data_t* data) {
- AddHeaderTail(packet, CHASSIS_CMD_ID);
- memcpy(&packet[0 + DATA_OFFSET], &data->vx, sizeof(float));
- memcpy(&packet[4 + DATA_OFFSET], &data->vy, sizeof(float));
- memcpy(&packet[8 + DATA_OFFSET], &data->vw, sizeof(float));
- AddCRC8(packet, CHASSIS_CMD_ID);
-}
-
-uint8_t MinipcPort::GetPacketLen(uint8_t cmd_id) {
- // Total length = Data length + header & tail (8 bytes) + crc checksum (1 byte)
- return CMD_TO_LEN[cmd_id] + 9;
-}
-
-void MinipcPort::ParseUartBuffer(const uint8_t* data, int32_t length) {
- // Four cases
- // Case 1: everything is fresh with complete package(s)
- // Case 2: everything is fresh; package is incomplete
- // Case 3: package contains half previous package and half new package
- // Case 4: package contains half previous package and new package(s)
- int i = 0;
- if (buffer_index_ > 0) {
- // Case 3 and 4
- // copy the remaining bytes from previous package
- while (i < (int)length) {
- possible_packet[buffer_index_] = data[i];
- // Parse possible packet if detect tail
- if (possible_packet[buffer_index_ - 1] == 'E' &&
- possible_packet[buffer_index_] == 'D') {
- buffer_index_ = 0;
- VerifyAndParseData();
- break;
- }
- buffer_index_++;
- i++;
- // drop packet if buffer overflow
- if (buffer_index_ >= MAX_PACKET_LENGTH) {
- buffer_index_ = 0;
- break;
- }
- }
- }
-
- while (i < (int)length) {
- if (buffer_index_ == 0) {
- if (i == (int)length - 1) {
- // A special case to handle the last byte; buffer_index_ must be zero
- if (data[i] == 'S') {
- possible_packet[buffer_index_++] = data[i];
- }
- return;
- }
- if (data[i] == 'S' && data[i + 1] == 'T'){
- // Detect packet head; start copying
- possible_packet[buffer_index_++] = data[i++];
- possible_packet[buffer_index_++] = data[i++];
- } else {
- i++;
- }
- } else {
- possible_packet[buffer_index_] = data[i];
- // Parse possible packet if detect tail
- if (possible_packet[buffer_index_ - 1] == 'E' &&
- possible_packet[buffer_index_] == 'D') {
- buffer_index_ = 0;
- VerifyAndParseData();
- } else {
- // If not packet end, continue
- buffer_index_++;
- i++;
- // drop packet if buffer overflow
- if (buffer_index_ >= MAX_PACKET_LENGTH) {
- buffer_index_ = 0;
- }
- }
- }
- }
-}
-
-uint8_t MinipcPort::GetCmdId(void) {
- return cmd_id_;
-}
-
-const status_data_t* MinipcPort::GetStatus(void) {
- return &status_;
-}
-
-uint8_t MinipcPort::GetValidFlag(void) {
- uint8_t temp = valid_flag_;
- valid_flag_ = 0;
- return temp;
-}
-
-uint16_t MinipcPort::GetSeqnum(void) {
- return seqnum_;
-}
-
-uint32_t MinipcPort::GetValidPacketCnt(void) {
- return valid_packet_cnt_;
-}
-
-/**
- * Every function below is a private function
- */
-
-
-// 8 bytes
-void MinipcPort::AddHeaderTail (uint8_t* packet, uint8_t cmd_id) {
- // Add header
- packet[0] = 'S';
- packet[1] = 'T';
- // dummy seq num
- packet[SEQNUM_OFFSET] = 0;
- packet[SEQNUM_OFFSET + 1] = 0 >> 8;
- packet[DATA_LENGTH_OFFSET] = CMD_TO_LEN[cmd_id];
- packet[CMD_ID_OFFSET] = cmd_id;
-
- // Add tail WITHOUT crc8
- const int EOF_OFFSET = DATA_OFFSET + CMD_TO_LEN[cmd_id] + 1;
- packet[EOF_OFFSET] = 'E';
- packet[EOF_OFFSET + 1] = 'D';
-}
-
-// 1 bytes
-void MinipcPort::AddCRC8 (uint8_t* packet, int8_t cmd_id) {
- const int CRC8_OFFSET = DATA_OFFSET + CMD_TO_LEN[cmd_id];
- packet[CRC8_OFFSET] = get_crc8_check_sum(packet, CRC8_OFFSET, 0);
-}
-
-void MinipcPort::VerifyAndParseData(void) {
- // TODO: implement thread-safe logic here (use a lock to handle changes from interrupt)
- // here we can assume that the package is complete
- // in the possible_packet buffer
-
- // A minor issue with current implementation: imagine the following case:
- // Two packets arrive in two UART calls.
- // The first packet misses 1 byte, but the second one is complete.
- // In this case, when the possible_packet buffer is filled
- // (the last bytes are from the second packet), VerifyAndParseData() will be called. The whole buffer
- // would be tossed, resulting in two unusable packets. However, if we implement this logic, we would be
- // able to recover the second packet.
-
- // This is a minor issue because
- // 1) we don't observe this even when packets are sent at 200Hz
- // 2) probability of this happening is very low. The second packet has to be sent in two slices to
- // trigger this issue. (first slice: S/T is sent to possible_packet; second slide: the rest)
- // Note: The issue discussed above might be deprecated.
-
- // if packet Head or Tail is corrupted
- uint8_t cmd_id = possible_packet[CMD_ID_OFFSET];
- if (possible_packet[0] != 'S' || possible_packet[1] != 'T' ||
- possible_packet[CMD_TO_LEN[cmd_id] + HT_LEN - 2] != 'E' ||
- possible_packet[CMD_TO_LEN[cmd_id] + HT_LEN - 1] != 'D') {
- valid_flag_ = 0;
- return;
- }
-
- // if packet crc8 checksum is valid
- if (verify_crc8_check_sum(possible_packet, CMD_TO_LEN[cmd_id] + HT_LEN - 2)) {
- ParseData(cmd_id);
- valid_packet_cnt_++;
- valid_flag_ = 1;
- } else {
- valid_flag_ = 0;
- }
-}
-
-
-
-void MinipcPort::ParseData(uint8_t cmd_id) {
- // Update member variable
- cmd_id_ = cmd_id;
-
- // Update seqnum
- memcpy(&seqnum_, &possible_packet[SEQNUM_OFFSET], sizeof(uint16_t));
-
- // Update data section
- switch (cmd_id) {
- case GIMBAL_CMD_ID:
- memcpy(&(status_.rel_yaw), &possible_packet[0 + DATA_OFFSET], sizeof(float));
- memcpy(&(status_.rel_pitch), &possible_packet[4 + DATA_OFFSET], sizeof(float));
- status_.mode = possible_packet[8 + DATA_OFFSET];
- status_.debug_int = possible_packet[9 + DATA_OFFSET];
- break;
- case COLOR_CMD_ID:
- status_.my_color = possible_packet[DATA_OFFSET];
- break;
- case CHASSIS_CMD_ID:
- memcpy(&(status_.vx), &possible_packet[0 + DATA_OFFSET], sizeof(float));
- memcpy(&(status_.vy), &possible_packet[4 + DATA_OFFSET], sizeof(float));
- memcpy(&(status_.vw), &possible_packet[8 + DATA_OFFSET], sizeof(float));
- break;
- }
-}
-
-
-} // namespace communication
-
diff --git a/shared/libraries/minipc_protocol.h b/shared/libraries/minipc_protocol.h
deleted file mode 100644
index 2ae105ed..00000000
--- a/shared/libraries/minipc_protocol.h
+++ /dev/null
@@ -1,183 +0,0 @@
-/****************************************************************************
- * *
- * Copyright (C) 2023 RoboMaster. *
- * Illini RoboMaster @ University of Illinois at Urbana-Champaign *
- * *
- * This program is free software: you can redistribute it and/or modify *
- * it under the terms of the GNU General Public License as published by *
- * the Free Software Foundation, either version 3 of the License, or *
- * (at your option) any later version. *
- * *
- * This program is distributed in the hope that it will be useful, *
- * but WITHOUT ANY WARRANTY; without even the implied warranty of *
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
- * GNU General Public License for more details. *
- * *
- * You should have received a copy of the GNU General Public License *
- * along with this program. If not, see . *
- * *
- ****************************************************************************/
-
-#pragma once
-
-#include "bsp_print.h"
-#include "bsp_uart.h"
-#include "cmsis_os.h"
-#include "crc8.h"
-
-namespace communication {
-
-// WARNING: CRC8 works only for total length < 64 bytes.
-typedef struct {
- char header[2];
- uint16_t seq_num;
- // data_length might not be necessary because all command length are fixed
- uint8_t data_length; // length of data as in bytes, data length as an arry * 4 = data_length
- uint8_t cmd_id;
- int32_t *data;
- uint8_t crc8_checksum;
- char tail[2];
-} __packed minipc_data_t;
-// This might be not necessary at all
-
-// RED is 0; BLUE is one
-typedef struct {
- uint8_t my_color;
-} __packed color_data_t;
-
-typedef struct {
- float rel_yaw;
- float rel_pitch;
- // Search Target is 0. Move Yoke is 1.
- uint8_t mode;
- uint8_t debug_int;
-} __packed gimbal_data_t;
-
-typedef struct {
- float vx;
- float vy;
- float vw;
-} __packed chassis_data_t;
-
-// summary of all information transmitted between minipc and stm32
-typedef struct {
- // RED is 0; BLUE is one
- uint8_t my_color;
- float rel_yaw;
- float rel_pitch;
- // Search Target is 0. Move Yoke is 1.
- uint8_t mode;
- uint8_t debug_int;
- float vx;
- float vy;
- float vw;
-} __packed status_data_t;
-
-// GIMBAL_CMD_ID : 0x00 Autoaim gimbal RelYaw RelPitch
-// COLOR_CMD_ID : 0x01
-// CHASSIS_CMD_ID : 0x02
-// TOTAL_NUM_OF_ID: length of the enum
-enum CMD_ID {GIMBAL_CMD_ID,
- COLOR_CMD_ID,
- CHASSIS_CMD_ID,
- TOTAL_NUM_OF_ID};
-
-// WARNING: THIS CLASS IS NOT THREAD SAFE!!!
-// See docs/comm_protocol.md in vision repo for docs
-class MinipcPort {
- public:
- MinipcPort();
-
- /**
- * @brief Pack data into a packet array
- * @note For the smallest length of the packet, see CMD_TO_LEN[] or GetPacketLength()
- */
- void Pack(uint8_t* packet, void* data, uint8_t cmd_id);
- void PackGimbalData(uint8_t* packet, gimbal_data_t* data);
- void PackColorData(uint8_t* packet, color_data_t* data);
- void PackChassisData(uint8_t* packet, chassis_data_t* data);
-
- /**
- * @brief Total length of packet in bytes
- * Header/tail/crc8 included.
- */
- uint8_t GetPacketLen(uint8_t cmd_id);
-
- /**
- * @brief parse and handle the uart buffer
- * @param data: pointer to the uart buffer
- * len: length of the data received in bytes
- */
- void ParseUartBuffer(const uint8_t* data, int32_t len);
-
- /**
- * @brief Return the cmd_id of the most recently parsed packet
- */
- uint8_t GetCmdId(void);
-
- /**
- * @brief Get command status of the robot
- */
- const status_data_t* GetStatus(void);
-
- /**
- * @brief Get the valid flag, 1 when the packet is valid, 0 otherwise
- * @note Flag can only be acquired once. Once asked, flag will be reset to 0 (invalid)
- */
- uint8_t GetValidFlag(void);
- uint16_t GetSeqnum(void);
- uint32_t GetValidPacketCnt(void);
-
- /**
- * Length of the data section ONLY in bytes. Header/tail/crc8 (total len = 9) NOT included.
- * Gimbal CMD: id = 0x00, total packet length = 19 - 9 = 10
- * Color CMD: id = 0x01, total packet length = 10 - 9 = 1
- * Chassis CMD: id = 0x02, total packet length = 21 - 9 = 12
- */
- static constexpr uint8_t CMD_TO_LEN[TOTAL_NUM_OF_ID] = {
- sizeof(gimbal_data_t),
- sizeof(color_data_t),
- sizeof(chassis_data_t),
- };
- static constexpr uint8_t MAX_PACKET_LENGTH = 21;
- static constexpr uint8_t MIN_PACKET_LENGTH = 10;
- // sum of header and tail = 9. Total packet length = data length (CMD_TO_LEN) + 9
- static constexpr uint8_t HT_LEN = 9;
-
- private:
- /**
- * @brief Add header and tail to the packet array based on cmd_id
- * @note For the smallest length of the packet, see CMD_TO_LEN[]
- * The sum of length for header and tail is 8 bytes
- */
- void AddHeaderTail (uint8_t* packet, uint8_t cmd_id);
-
- /**
- * @brief Add CRC8 checksum for the packet array based on cmd_id
- * CRC8 calulated based on the entire array except the tail ('ED')
- * @note Only call this function after packet has data and header/tails written
- * The length of CRC8 checksum is 1 byte
- */
- void AddCRC8 (uint8_t* packet, int8_t cmd_id);
-
- // Wrapper of ParseData(uint8_t), do some verification.
- void VerifyAndParseData();
-
- // Assume that the possible_packet is a complete and verified message
- void ParseData(uint8_t cmd_id);
-
- uint8_t cmd_id_;
- status_data_t status_;
- uint8_t possible_packet[MAX_PACKET_LENGTH];
- // keep track of the index of the current packet
- // in case of 1 packet being sent in multiple uart transmissions
- int buffer_index_;
-
- // Least current available sequence number
- uint16_t seqnum_;
- uint8_t valid_flag_;
- uint32_t valid_packet_cnt_ = 0;
-}; /* class MinipcPort */
-
-} /* namespace communication */
-
diff --git a/vehicles/Steering/gimbal.cc b/vehicles/Steering/gimbal.cc
index 25c7b359..5ea2f7a5 100644
--- a/vehicles/Steering/gimbal.cc
+++ b/vehicles/Steering/gimbal.cc
@@ -41,7 +41,6 @@
#include "shooter.h"
#include "stepper.h"
#include "minipc_protocol.h"
-#include "autoaim_protocol.h"
static bsp::CAN* can1 = nullptr;
@@ -377,55 +376,53 @@ class CustomUART : public bsp::UART {
void jetsonCommTask(void* arg) {
UNUSED(arg);
-// Comment because of upstream AutoaimProtocol Class rewrite in progress
-// youhy, 2023.10
-//
-// uint32_t length;
-// uint8_t* data;
-//
-// auto uart = std::make_unique(&huart1); // see cmake for which uart
-// uart->SetupRx(50);
-// uart->SetupTx(50);
-//
-// auto miniPCreceiver = communication::AutoaimProtocol();
-//
-// while (!imu->CaliDone()) {
-// osDelay(10);
-// }
-//
-// int total_receive_count = 0;
-//
-// while (true) {
-// uint32_t flags = osThreadFlagsGet();
-// if (flags & JETSON_RX_SIGNAL) {
-// /* time the non-blocking rx / tx calls (should be <= 1 osTick) */
-//
-// // max length of the UART buffer at 150Hz is ~50 bytes
-// length = uart->Read(&data);
-//
-// miniPCreceiver.Receive(data, length);
-//
-// if (miniPCreceiver.get_valid_flag()) {
-// // there is at least one unprocessed valid packet
-// abs_yaw_jetson = miniPCreceiver.get_relative_yaw();
-// abs_pitch_jetson = miniPCreceiver.get_relative_pitch();
-// total_receive_count++;
-// }
-// }
-// // send IMU data anyway
-// communication::STMToJetsonData packet_to_send;
-// uint8_t my_color; // 1 for blue; 0 for red
-// if (send->is_my_color_blue) {
-// my_color = 1;
-// } else {
-// my_color = 0;
-// }
-// const float pitch_curr = pitch_pos;
-// const float yaw_curr = imu->INS_angle[0];
-// miniPCreceiver.Send(&packet_to_send, my_color, yaw_curr, pitch_curr, 0);
-// uart->Write((uint8_t*)&packet_to_send, sizeof(communication::STMToJetsonData));
-// osDelay(2);
-// }
+
+ uint32_t length;
+ uint8_t* data;
+
+ auto uart = std::make_unique(&huart1); // see cmake for which uart
+ uart->SetupRx(50);
+ uart->SetupTx(50);
+
+ auto miniPCreceiver = communication::AutoaimProtocol();
+
+ while (!imu->CaliDone()) {
+ osDelay(10);
+ }
+
+ int total_receive_count = 0;
+
+ while (true) {
+ uint32_t flags = osThreadFlagsGet();
+ if (flags & JETSON_RX_SIGNAL) {
+ /* time the non-blocking rx / tx calls (should be <= 1 osTick) */
+
+ // max length of the UART buffer at 150Hz is ~50 bytes
+ length = uart->Read(&data);
+
+ miniPCreceiver.Receive(data, length);
+
+ if (miniPCreceiver.get_valid_flag()) {
+ // there is at least one unprocessed valid packet
+ abs_yaw_jetson = miniPCreceiver.get_relative_yaw();
+ abs_pitch_jetson = miniPCreceiver.get_relative_pitch();
+ total_receive_count++;
+ }
+ }
+ // send IMU data anyway
+ communication::STMToJetsonData packet_to_send;
+ uint8_t my_color; // 1 for blue; 0 for red
+ if (send->is_my_color_blue) {
+ my_color = 1;
+ } else {
+ my_color = 0;
+ }
+ const float pitch_curr = pitch_pos;
+ const float yaw_curr = imu->INS_angle[0];
+ miniPCreceiver.Send(&packet_to_send, my_color, yaw_curr, pitch_curr, 0);
+ uart->Write((uint8_t*)&packet_to_send, sizeof(communication::STMToJetsonData));
+ osDelay(2);
+ }
}
//==================================================================================================