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 <[email protected]>
illini-robomaster · Dec 30, 2023 · fc15c38 · fc15c38
1 parent b015d60
commit fc15c38
Show file tree

Hide file tree

Showing 13 changed files with 911 additions and 350 deletions.
diff --git 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
@@ -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);
   }
-}
+}
diff --git 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
@@ -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 <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);
+}
+
+// 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<CustomUART>(&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
@@ -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); 
+  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
@@ -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);
+  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);
+  }
+}