Skip to content

Commit

Permalink
Add minipccontroller (#70)
Browse files Browse the repository at this point in the history 
Co-authored-by: Austin Yang <[email protected]>
  • Loading branch information
@ywh114
ywh114 and Austin Yang authored Mar 2, 2024
1 parent 1854c94 commit 43f3f7e
Show file tree
Hide file tree
Showing 7 changed files with 219 additions and 12 deletions.
4 changes: 4 additions & 0 deletions examples/minipc/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)

14 changes: 7 additions & 7 deletions examples/minipc/LatencyTest.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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);
Expand Down
143 changes: 143 additions & 0 deletions examples/minipc/MiniPCController.cc
View file
Original file line number Diff line number Diff line change
@@ -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 <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::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);
}
}
3 changes: 3 additions & 0 deletions shared/bsp/bsp_can_bridge.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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:;
}
}
Expand Down
3 changes: 2 additions & 1 deletion shared/bsp/bsp_can_bridge.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,7 @@ typedef enum {
GIMBAL_POWER,
RECALIBRATE,
IS_MY_COLOR_BLUE,
SELF_CHECK_FLAG
} can_bridge_cmd;

typedef struct {
Expand Down Expand Up @@ -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_;
Expand Down
35 changes: 35 additions & 0 deletions shared/libraries/minipc_protocol.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,11 @@ void MinipcPort::Pack(uint8_t* packet, void* data, uint8_t cmd_id) {
case CHASSIS_CMD_ID:
PackChassisData(packet, static_cast<chassis_data_t*>(data));
break;
case SELFCHECK_CMD_ID:
PackSelfcheckData(packet, static_cast<selfcheck_data_t*>(data));
break;
case ARM_CMD_ID:
PackArmData(packet, static_cast<arm_data_t*>(data));
}
}

Expand All @@ -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;
Expand Down Expand Up @@ -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;
}
}

Expand Down
29 changes: 25 additions & 4 deletions shared/libraries/minipc_protocol.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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!!!
Expand All @@ -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
Expand Down Expand Up @@ -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;
Expand Down

0 comments on commit 43f3f7e

Please sign in to comment.