From fc15c38446bb08efd8e61d19a717520d5a3cb69b Mon Sep 17 00:00:00 2001 From: AzulRadio <50132891+AzulRadio@users.noreply.github.com> Date: Fri, 29 Dec 2023 18:59:26 -0600 Subject: [PATCH] Add minipc protocol 2.0 (#66) * Break Send() into Pack() for each message type * add function headers and helpers for Pack() * lossless float transmission * data parse rewrite * rename autoaim_protocol as minipc_protocol; add more comments. * add examples --------- Co-authored-by: Richard Xu --- examples/autoaim/CMakeLists.txt | 6 +- examples/autoaim/main.cc | 4 +- examples/minipc/CMakeLists.txt | 16 +- examples/minipc/LatencyTest.cc | 96 +++++++ examples/minipc/MotorTest.cc | 147 ++++++++++ examples/minipc/PingpongTest.cc | 95 +++++++ .../minipc/{typeC.cc => StressTestTypeC.cc} | 32 ++- examples/minipc/typeA.cc | 67 +++-- shared/libraries/autoaim_protocol.cc | 175 ------------ shared/libraries/autoaim_protocol.h | 76 ----- shared/libraries/minipc_protocol.cc | 266 ++++++++++++++++++ shared/libraries/minipc_protocol.h | 183 ++++++++++++ vehicles/Steering/gimbal.cc | 98 +++---- 13 files changed, 911 insertions(+), 350 deletions(-) create mode 100644 examples/minipc/LatencyTest.cc create mode 100644 examples/minipc/MotorTest.cc create mode 100644 examples/minipc/PingpongTest.cc rename examples/minipc/{typeC.cc => StressTestTypeC.cc} (79%) delete mode 100644 shared/libraries/autoaim_protocol.cc delete mode 100644 shared/libraries/autoaim_protocol.h create mode 100644 shared/libraries/minipc_protocol.cc create mode 100644 shared/libraries/minipc_protocol.h 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); +// } } //==================================================================================================