Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add minipccontroller example #70

Merged
merged 1 commit into from
Mar 2, 2024
Merged

Add minipccontroller example #70

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 4 additions & 0 deletions examples/minipc/CMakeLists.txt
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Loading