From 43f3f7efad1b6a7ad13f308dcc5f960d72ca96e1 Mon Sep 17 00:00:00 2001 From: Austin Yang <145237111+ywh114@users.noreply.github.com> Date: Sat, 2 Mar 2024 16:57:48 -0600 Subject: [PATCH] Add minipccontroller (#70) Co-authored-by: Austin Yang --- examples/minipc/CMakeLists.txt | 4 + examples/minipc/LatencyTest.cc | 14 +-- examples/minipc/MiniPCController.cc | 143 ++++++++++++++++++++++++++++ shared/bsp/bsp_can_bridge.cc | 3 + shared/bsp/bsp_can_bridge.h | 3 +- shared/libraries/minipc_protocol.cc | 35 +++++++ shared/libraries/minipc_protocol.h | 29 +++++- 7 files changed, 219 insertions(+), 12 deletions(-) create mode 100644 examples/minipc/MiniPCController.cc 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;