diff --git a/examples/autoaim/CMakeLists.txt b/examples/autoaim/CMakeLists.txt
index 62c3064d..43978ebd 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 67a8c3e7..52971731 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 "autoaim_protocol.h"
+#include "minipc_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 d436ac53..004a9a19 100644
--- a/examples/minipc/CMakeLists.txt
+++ b/examples/minipc/CMakeLists.txt
@@ -24,7 +24,19 @@ irm_add_arm_executable(${PROJECT_NAME}
TARGET DJI_Board_TypeA
SOURCES typeA.cc)
-irm_add_arm_executable(${PROJECT_NAME}_typeC
+irm_add_arm_executable(${PROJECT_NAME}_stresstesttypec
TARGET DJI_Board_TypeC
- SOURCES typeC.cc)
+ 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)
diff --git a/examples/minipc/LatencyTest.cc b/examples/minipc/LatencyTest.cc
new file mode 100644
index 00000000..86e85e5c
--- /dev/null
+++ b/examples/minipc/LatencyTest.cc
@@ -0,0 +1,96 @@
+/****************************************************************************
+ * *
+ * 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
new file mode 100644
index 00000000..d89584e8
--- /dev/null
+++ b/examples/minipc/MotorTest.cc
@@ -0,0 +1,147 @@
+/****************************************************************************
+ * *
+ * 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
new file mode 100644
index 00000000..077475cb
--- /dev/null
+++ b/examples/minipc/PingpongTest.cc
@@ -0,0 +1,95 @@
+/****************************************************************************
+ * *
+ * 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/typeC.cc b/examples/minipc/StressTestTypeC.cc
similarity index 79%
rename from examples/minipc/typeC.cc
rename to examples/minipc/StressTestTypeC.cc
index 703316a4..eefe2973 100644
--- a/examples/minipc/typeC.cc
+++ b/examples/minipc/StressTestTypeC.cc
@@ -27,7 +27,7 @@
#include "bsp_print.h"
#include "bsp_uart.h"
#include "cmsis_os.h"
-#include "autoaim_protocol.h"
+#include "minipc_protocol.h"
#include "rgb.h"
#define RX_SIGNAL (1 << 0)
@@ -55,29 +55,31 @@ void RM_RTOS_Default_Task(const void* argument) {
uint32_t length;
uint8_t* data;
- auto uart = std::make_unique(&huart1); // see cmake for which uart
+ auto uart = std::make_unique(&huart1);
uart->SetupRx(50);
uart->SetupTx(50);
- auto miniPCreceiver = communication::AutoaimProtocol();
+ auto minipc_session = communication::MinipcPort();
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);
- // An alternative is to use osThreadFlagsWait.
- // However, we want to experiment with periodic sending
- uint32_t flags = osThreadFlagsGet();
+ int32_t flags = osThreadFlagsWait(RX_SIGNAL, osFlagsWaitAll, osWaitForever);
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;
-
- miniPCreceiver.Receive(data, length);
- uint32_t valid_packet_cnt = miniPCreceiver.get_valid_packet_cnt();
+ minipc_session.ParseUartBuffer(data, length);
+ uint32_t valid_packet_cnt = minipc_session.GetValidPacketCnt();
+ uint8_t packet_to_send[minipc_session.MAX_PACKET_LENGTH];
// Jetson / PC sends 200Hz valid packets for stress testing
// For testing script, please see iRM_Vision_2023/Communication/communicator.py
@@ -85,9 +87,10 @@ void RM_RTOS_Default_Task(const void* argument) {
if (valid_packet_cnt == 1000) {
// Jetson test cases write 1000 packets. Pass
led->Display(0xFF00FF00);
- osDelay(10000);
+ osDelay(4000);
+ led->Display(0xFFFF0000);
// after 10 seconds, write 1000 alternating packets to Jetson
- communication::STMToJetsonData packet_to_send;
+ communication::color_data_t color_data;
uint8_t my_color = 1; // blue
for (int i = 0; i < 1000; ++i) {
if (i % 2 == 0) {
@@ -95,8 +98,9 @@ void RM_RTOS_Default_Task(const void* argument) {
} else {
my_color = 0; // red
}
- miniPCreceiver.Send(&packet_to_send, my_color, 0.5, 0.42, 0);
- uart->Write((uint8_t*)&packet_to_send, sizeof(communication::STMToJetsonData));
+ 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));
// NOTE: THIS BREAKS WHEN WORKING AT 1000HZ!
osDelay(2);
}
@@ -106,4 +110,4 @@ void RM_RTOS_Default_Task(const void* argument) {
}
osDelay(2);
}
-}
\ No newline at end of file
+}
diff --git a/examples/minipc/typeA.cc b/examples/minipc/typeA.cc
index 2afbe633..9150317c 100644
--- a/examples/minipc/typeA.cc
+++ b/examples/minipc/typeA.cc
@@ -23,59 +23,66 @@
#include
#include
-#include "bsp_gpio.h"
#include "bsp_print.h"
#include "bsp_uart.h"
#include "cmsis_os.h"
-#include "autoaim_protocol.h"
+#include "minipc_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);
- uint32_t length;
- uint8_t* data;
-
- auto uart = std::make_unique(&huart8); // see cmake for which uart
+ auto uart = std::make_unique(&huart8);
uart->SetupRx(50);
uart->SetupTx(50);
- gpio_red = new bsp::GPIO(LED_RED_GPIO_Port, LED_RED_Pin);
- gpio_green = new bsp::GPIO(LED_GREEN_GPIO_Port, LED_GREEN_Pin);
+ 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];
- auto miniPCreceiver = communication::AutoaimProtocol();
+ /* To run this test, use Communication/communicator.py in the iRM_Vision_2023 repo
+ * Need to set testing=Test.TYPE_A
+ **/
while (true) {
- /* 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();
- }
+ // 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);
}
}
diff --git a/shared/libraries/autoaim_protocol.cc b/shared/libraries/autoaim_protocol.cc
deleted file mode 100644
index e355f9b9..00000000
--- a/shared/libraries/autoaim_protocol.cc
+++ /dev/null
@@ -1,175 +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 "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
deleted file mode 100644
index e7a2131d..00000000
--- a/shared/libraries/autoaim_protocol.h
+++ /dev/null
@@ -1,76 +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 {
-
-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
new file mode 100644
index 00000000..e6345d0e
--- /dev/null
+++ b/shared/libraries/minipc_protocol.cc
@@ -0,0 +1,266 @@
+/****************************************************************************
+ * *
+ * 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
new file mode 100644
index 00000000..2ae105ed
--- /dev/null
+++ b/shared/libraries/minipc_protocol.h
@@ -0,0 +1,183 @@
+/****************************************************************************
+ * *
+ * 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 e7b530d0..b0413a14 100644
--- a/vehicles/Steering/gimbal.cc
+++ b/vehicles/Steering/gimbal.cc
@@ -40,7 +40,7 @@
#include "rgb.h"
#include "shooter.h"
#include "stepper.h"
-#include "autoaim_protocol.h"
+#include "minipc_protocol.h"
static bsp::CAN* can1 = nullptr;
static bsp::CAN* can2 = nullptr;
@@ -375,53 +375,55 @@ class CustomUART : public bsp::UART {
void jetsonCommTask(void* arg) {
UNUSED(arg);
-
- 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);
- }
+// 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);
+// }
}
//==================================================================================================