diff --git a/examples/minipc/CMakeLists.txt b/examples/minipc/CMakeLists.txt
index 004a9a19..8b0512d7 100644
--- a/examples/minipc/CMakeLists.txt
+++ b/examples/minipc/CMakeLists.txt
@@ -40,3 +40,7 @@ irm_add_arm_executable(${PROJECT_NAME}_latency
TARGET DJI_Board_TypeC
SOURCES LatencyTest.cc)
+irm_add_arm_executable(${PROJECT_NAME}_minipccontroller
+ TARGET DJI_Board_TypeC
+ SOURCES MiniPCController.cc)
+
diff --git a/examples/minipc/LatencyTest.cc b/examples/minipc/LatencyTest.cc
index 86e85e5c..a890a276 100644
--- a/examples/minipc/LatencyTest.cc
+++ b/examples/minipc/LatencyTest.cc
@@ -61,14 +61,13 @@ void RM_RTOS_Default_Task(const void* argument) {
auto minipc_session = communication::MinipcPort();
- communication::chassis_data_t chassis_data; // this has to be the data type that has the maximum size
+ communication::selfcheck_data_t selfcheck_data;
//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;
+ selfcheck_data.mode = 0;
+ selfcheck_data.debug_int = 0;
uint8_t packet_to_send[minipc_session.MAX_PACKET_LENGTH];
uint8_t *data;
@@ -86,9 +85,10 @@ void RM_RTOS_Default_Task(const void* argument) {
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));
+ selfcheck_data.mode = status_data->mode;
+ selfcheck_data.debug_int = status_data->debug_int;
+ minipc_session.Pack(packet_to_send, (void*)&selfcheck_data, communication::SELFCHECK_CMD_ID);
+ uart->Write(packet_to_send, minipc_session.GetPacketLen(communication::SELFCHECK_CMD_ID));
}
osDelay(10);
diff --git a/examples/minipc/MiniPCController.cc b/examples/minipc/MiniPCController.cc
new file mode 100644
index 00000000..ff50ebc1
--- /dev/null
+++ b/examples/minipc/MiniPCController.cc
@@ -0,0 +1,143 @@
+/****************************************************************************
+ * *
+ * 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::gimbal_data_t gimbal_data;
+ //communication::color_data_t color_data;
+ //communication::chassis_data_t chassis_data;
+ //communication::selfcheck_data_t selfcheck_data;
+ //communication::arm_data_t arm_data;
+ communication::selfcheck_data_t selfcheck_data;
+ communication::arm_data_t arm_data;
+
+ const communication::status_data_t* status_data;
+
+ uint8_t packet_to_send[minipc_session.MAX_PACKET_LENGTH];
+ uint8_t *data;
+ uint8_t recv_cmd_id;
+ 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);
+
+ recv_cmd_id = minipc_session.GetCmdId();
+ status_data = minipc_session.GetStatus();
+
+ switch (recv_cmd_id) {
+ case communication::GIMBAL_CMD_ID:
+ // Forward gimbal data
+ break;
+ case communication::COLOR_CMD_ID:
+ // Forward color data
+ break;
+ case communication::CHASSIS_CMD_ID:
+ // Forward gimbal data
+ break;
+ case communication::SELFCHECK_CMD_ID:
+ if (status_data->mode == 1){
+ selfcheck_data.mode = 1;
+ selfcheck_data.debug_int = status_data->debug_int;
+
+ minipc_session.Pack(packet_to_send, (void*)&selfcheck_data, communication::SELFCHECK_CMD_ID);
+ uart->Write(packet_to_send, minipc_session.GetPacketLen(communication::SELFCHECK_CMD_ID));
+ } else if (status_data->mode == 2){
+ // Respond with ID:
+ // BRD: 129 (129 - 127 = 2)
+ // See repo irm_tele_arm `config.py` for details
+ selfcheck_data.mode = 2;
+ selfcheck_data.debug_int = 129;
+
+ minipc_session.Pack(packet_to_send, (void*)&selfcheck_data, communication::SELFCHECK_CMD_ID);
+ uart->Write(packet_to_send, minipc_session.GetPacketLen(communication::SELFCHECK_CMD_ID));
+ }
+ break;
+ case communication::ARM_CMD_ID:
+ arm_data.floats[0] = status_data->floats[0];
+ arm_data.floats[1] = status_data->floats[1];
+ arm_data.floats[2] = status_data->floats[2];
+ arm_data.floats[3] = status_data->floats[3];
+ arm_data.floats[4] = status_data->floats[4];
+ arm_data.floats[5] = status_data->floats[5];
+
+ minipc_session.Pack(packet_to_send, (void*)&arm_data, communication::ARM_CMD_ID);
+ uart->Write(packet_to_send, minipc_session.GetPacketLen(communication::ARM_CMD_ID));
+ break;
+ default:
+ selfcheck_data.mode = status_data->mode;
+ selfcheck_data.debug_int = 129;
+
+ minipc_session.Pack(packet_to_send, (void*)&selfcheck_data, communication::SELFCHECK_CMD_ID);
+ uart->Write(packet_to_send, minipc_session.GetPacketLen(communication::SELFCHECK_CMD_ID));
+ break;
+ }
+
+ }
+ osDelay(10);
+ }
+}
diff --git a/shared/bsp/bsp_can_bridge.cc b/shared/bsp/bsp_can_bridge.cc
index df2b9648..7f9e4fd1 100644
--- a/shared/bsp/bsp_can_bridge.cc
+++ b/shared/bsp/bsp_can_bridge.cc
@@ -90,6 +90,9 @@ void CanBridge::UpdateData(const uint8_t* data) {
case IS_MY_COLOR_BLUE:
is_my_color_blue = cmd.data_bool;
break;
+ case SELF_CHECK_FLAG:
+ self_check_flag = cmd.data_bool;
+ break;
default:;
}
}
diff --git a/shared/bsp/bsp_can_bridge.h b/shared/bsp/bsp_can_bridge.h
index ba578fcb..7b07ba27 100644
--- a/shared/bsp/bsp_can_bridge.h
+++ b/shared/bsp/bsp_can_bridge.h
@@ -42,6 +42,7 @@ typedef enum {
GIMBAL_POWER,
RECALIBRATE,
IS_MY_COLOR_BLUE,
+ SELF_CHECK_FLAG
} can_bridge_cmd;
typedef struct {
@@ -75,10 +76,10 @@ class CanBridge {
float speed_limit1 = 0;
float speed_limit2 = 0;
unsigned int chassis_flag = 0;
- bool self_check_flag = false;
unsigned int gimbal_power = 0;
bool recalibrate = false;
bool is_my_color_blue = false;
+ bool self_check_flag = false;
// each bit represents a flag correspond to specific motor e.g.(at index 0, it represents the motor 1's connection flag)
private:
bsp::CAN* can_;
diff --git a/shared/libraries/minipc_protocol.cc b/shared/libraries/minipc_protocol.cc
index e6345d0e..79b88551 100644
--- a/shared/libraries/minipc_protocol.cc
+++ b/shared/libraries/minipc_protocol.cc
@@ -49,6 +49,11 @@ void MinipcPort::Pack(uint8_t* packet, void* data, uint8_t cmd_id) {
case CHASSIS_CMD_ID:
PackChassisData(packet, static_cast(data));
break;
+ case SELFCHECK_CMD_ID:
+ PackSelfcheckData(packet, static_cast(data));
+ break;
+ case ARM_CMD_ID:
+ PackArmData(packet, static_cast(data));
}
}
@@ -75,6 +80,24 @@ void MinipcPort::PackChassisData(uint8_t* packet, chassis_data_t* data) {
AddCRC8(packet, CHASSIS_CMD_ID);
}
+void MinipcPort::PackSelfcheckData(uint8_t* packet, selfcheck_data_t* data) {
+ AddHeaderTail(packet, SELFCHECK_CMD_ID);
+ packet[0 + DATA_OFFSET] = data->mode;
+ packet[1 + DATA_OFFSET] = data->debug_int;
+ AddCRC8(packet, SELFCHECK_CMD_ID);
+}
+
+void MinipcPort::PackArmData(uint8_t* packet, arm_data_t* data) {
+ AddHeaderTail(packet, ARM_CMD_ID);
+ memcpy(&packet[0 + DATA_OFFSET], &data->floats[0], sizeof(float));
+ memcpy(&packet[4 + DATA_OFFSET], &data->floats[1], sizeof(float));
+ memcpy(&packet[8 + DATA_OFFSET], &data->floats[2], sizeof(float));
+ memcpy(&packet[12 + DATA_OFFSET], &data->floats[3], sizeof(float));
+ memcpy(&packet[16 + DATA_OFFSET], &data->floats[4], sizeof(float));
+ memcpy(&packet[20 + DATA_OFFSET], &data->floats[5], sizeof(float));
+ AddCRC8(packet, ARM_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;
@@ -258,6 +281,18 @@ void MinipcPort::ParseData(uint8_t cmd_id) {
memcpy(&(status_.vy), &possible_packet[4 + DATA_OFFSET], sizeof(float));
memcpy(&(status_.vw), &possible_packet[8 + DATA_OFFSET], sizeof(float));
break;
+ case SELFCHECK_CMD_ID:
+ status_.mode = possible_packet[0 + DATA_OFFSET];
+ status_.debug_int = possible_packet[1 + DATA_OFFSET];
+ break;
+ case ARM_CMD_ID:
+ memcpy(&(status_.floats[0]), &possible_packet[0 + DATA_OFFSET], sizeof(float));
+ memcpy(&(status_.floats[1]), &possible_packet[4 + DATA_OFFSET], sizeof(float));
+ memcpy(&(status_.floats[2]), &possible_packet[8 + DATA_OFFSET], sizeof(float));
+ memcpy(&(status_.floats[3]), &possible_packet[12 + DATA_OFFSET], sizeof(float));
+ memcpy(&(status_.floats[4]), &possible_packet[16 + DATA_OFFSET], sizeof(float));
+ memcpy(&(status_.floats[5]), &possible_packet[20 + DATA_OFFSET], sizeof(float));
+ break;
}
}
diff --git a/shared/libraries/minipc_protocol.h b/shared/libraries/minipc_protocol.h
index 2ae105ed..5908c529 100644
--- a/shared/libraries/minipc_protocol.h
+++ b/shared/libraries/minipc_protocol.h
@@ -59,6 +59,16 @@ typedef struct {
float vw;
} __packed chassis_data_t;
+typedef struct {
+ // FLUSH is 0. ECHO is 1. ID is 2.
+ uint8_t mode;
+ uint8_t debug_int;
+} __packed selfcheck_data_t;
+
+typedef struct {
+ float floats[6];
+} __packed arm_data_t;
+
// summary of all information transmitted between minipc and stm32
typedef struct {
// RED is 0; BLUE is one
@@ -71,15 +81,20 @@ typedef struct {
float vx;
float vy;
float vw;
+ float floats[6];
} __packed status_data_t;
// GIMBAL_CMD_ID : 0x00 Autoaim gimbal RelYaw RelPitch
// COLOR_CMD_ID : 0x01
// CHASSIS_CMD_ID : 0x02
+// SELFCHECK_CMD_ID :0x03
+// ARM_CMD-ID : 0x04
// TOTAL_NUM_OF_ID: length of the enum
enum CMD_ID {GIMBAL_CMD_ID,
COLOR_CMD_ID,
CHASSIS_CMD_ID,
+ SELFCHECK_CMD_ID,
+ ARM_CMD_ID,
TOTAL_NUM_OF_ID};
// WARNING: THIS CLASS IS NOT THREAD SAFE!!!
@@ -96,6 +111,8 @@ class MinipcPort {
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);
+ void PackSelfcheckData(uint8_t* packet, selfcheck_data_t* data);
+ void PackArmData(uint8_t* packet, arm_data_t* data);
/**
* @brief Total length of packet in bytes
@@ -130,16 +147,20 @@ class MinipcPort {
/**
* 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
+ * 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
+ * Selfcheck CMD: id = 0x03, total packet length = 11 - 9 = 2
+ * Arm CMD: id = 0x04, total packet length = 33 - 9 = 24
*/
static constexpr uint8_t CMD_TO_LEN[TOTAL_NUM_OF_ID] = {
sizeof(gimbal_data_t),
sizeof(color_data_t),
sizeof(chassis_data_t),
+ sizeof(selfcheck_data_t),
+ sizeof(arm_data_t),
};
- static constexpr uint8_t MAX_PACKET_LENGTH = 21;
+ static constexpr uint8_t MAX_PACKET_LENGTH = 33;
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;