Add minipc protocol 2.0

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)

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

examples/minipc/CMakeLists.txt

@@ -20,11 +20,15 @@
 
 project(example_minipc ASM C CXX)
 
-irm_add_arm_executable(${PROJECT_NAME}
-    TARGET DJI_Board_TypeA
-    SOURCES typeA.cc)
+#irm_add_arm_executable(${PROJECT_NAME}
+#    TARGET DJI_Board_TypeA
+#    SOURCES typeA.cc)
+#
+irm_add_arm_executable(${PROJECT_NAME}_stresstesttypec
+    TARGET DJI_Board_TypeC
+    SOURCES StressTestTypeC.cc)
 
-irm_add_arm_executable(${PROJECT_NAME}_typeC
+irm_add_arm_executable(${PROJECT_NAME}_pingpongtest
     TARGET DJI_Board_TypeC
-    SOURCES typeC.cc)
+    SOURCES PingpongTest.cc)
 

examples/minipc/PingpongTest.cc

@@ -0,0 +1,121 @@
+/****************************************************************************
+ *                                                                          *
+ *  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 <http://www.gnu.org/licenses/>.    *
+ *                                                                          *
+ ****************************************************************************/
+
+#include "main.h"
+
+#include <cstring>
+#include <memory>
+
+#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<CustomUART>(&huart1);  // 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;
+
+  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;
+
+  color_data.my_color = 1;
+
+  chassis_data.vx = 20;
+  chassis_data.vy = 30;
+  chassis_data.vw = 40;
+
+  uint8_t packet_to_send[minipc_session.MAX_PACKET_LENGTH];
+  uint8_t *data;
+  int32_t length;
+
+  UNUSED(color_data);
+  UNUSED(chassis_data);
+
+  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_TX_RX = True for this test
+    // The value for TESTING_CRC doesn't matter
+
+    // 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;
+      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);
+
+
+    // example code for prepare other packets and send
+    /*
+    uart->Write(packet_to_send, minipc_session.GetPacketLen(communication::GIMBAL_CMD_ID));
+    osDelay(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(1);
+
+    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(1);
+    */
+  }
+}

examples/minipc/typeC.cc → 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)
@@ -59,9 +59,13 @@ void RM_RTOS_Default_Task(const void* argument) {
   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_TX_RX = False and TESTING_CRC = True
+  **/
+
   while (true) {
     /* wait until rx data is available */
     led->Display(0xFF0000FF);
@@ -76,8 +80,10 @@ void RM_RTOS_Default_Task(const void* argument) {
       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
@@ -87,16 +93,17 @@ void RM_RTOS_Default_Task(const void* argument) {
         led->Display(0xFF00FF00);
         osDelay(10000);
         // 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) {
             my_color = 1; // blue
           } 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 +113,4 @@ void RM_RTOS_Default_Task(const void* argument) {
     }
     osDelay(2);
   }
-}
+}

examples/minipc/typeA.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"
 
 #define RX_SIGNAL (1 << 0)