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

@@ -24,7 +24,15 @@ 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)
 

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 <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 "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<CustomUART>(&huart8);  // see cmake for which uart
+  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);
+  }
+}
+

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

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