Skip to content

Commit

Permalink
Merge pull request #71 from xrobot-org/dev
Browse files Browse the repository at this point in the history 
修改自定义控制器&提交飞镖/无人机相关代码
  • Loading branch information
@Jiu-xiao
Jiu-xiao authored May 19, 2024
2 parents 95a7a9f + e4e33c4 commit 1886926
Show file tree
Hide file tree
Showing 12 changed files with 347 additions and 319 deletions.
45 changes: 33 additions & 12 deletions src/device/custom_controller/dev_custom_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,21 +8,20 @@ using namespace Device;

static std::array<uint8_t, REF_LEN_RX_BUFF> rxbuf;

CustomController::CustomController()
: event_(Message::Event::FindEvent("cmd_event")) {
CustomController::CustomController(Param &param)
: param_(param), event_(Message::Event::FindEvent("cmd_event")) {
auto rx_cplt_callback = [](void *arg) {
CustomController *cust_ctrl = static_cast<CustomController *>(arg);
cust_ctrl->packet_recv_.Post();
};

bsp_uart_register_callback(BSP_UART_EXT, BSP_UART_RX_CPLT_CB,
rx_cplt_callback, this);
Component::CMD::RegisterController(this->controller_angel_);
auto controller_recv_thread = [](CustomController *cust_ctrl) {
while (1) {
if (cust_ctrl->packet_recv_.Wait(20)) {
bsp_uart_receive(BSP_UART_EXT, &rxbuf[0], sizeof(rxbuf), false);
if (cust_ctrl->packet_recv_.Wait(500)) {
cust_ctrl->Prase();
cust_ctrl->controller_angel_.Publish(cust_ctrl->controller_data_);
} else {
cust_ctrl->Offline();
}
Expand All @@ -34,15 +33,37 @@ CustomController::CustomController()
}

bool CustomController::StartRecv() {
return bsp_uart_receive(BSP_UART_EXT, rxbuf, REF_LEN_RX_BUFF, false) ==
return bsp_uart_receive(BSP_UART_EXT, &rxbuf[0], REF_LEN_RX_BUFF, false) ==
BSP_OK;
}

void CustomController::Prase() {
this->controller_angel_.Publish(this->controller_data_);
auto len = bsp_uart_get_count(BSP_UART_EXT);
uint8_t *index = &rxbuf[0];

for (uint32_t i = 0; i < len; i++) {
if (index[i] == 0xa5 && index[i + 5] == 0x02 && index[i + 6] == 0x03 &&
len - i > 37) {
online_ = true;

memcpy(&data_, index + i, sizeof(data_));
for (int i = 0; i < 6; i++) {
if (data_.angle[0] != 0) {
data_.angle[i] += param_.offset[i];
} else {
online_ = false;
}
}
i += 37;
last_online_time_ = bsp_time_get_ms();
} else {
continue;
}
}

if (bsp_time_get_ms() - last_online_time_ > 500) {
online_ = false;
}
}
void CustomController::Offline() {
this->controller_data_.online = false;
memset(&(this->controller_data_), 0, sizeof(this->controller_data_));
this->controller_angel_.Publish(this->controller_data_);
};

void CustomController::Offline() { online_ = false; }
34 changes: 28 additions & 6 deletions src/device/custom_controller/dev_custom_controller.hpp
View file
Original file line number Diff line number Diff line change
@@ -1,28 +1,50 @@
#include "comp_cmd.hpp"
#include "comp_ui.hpp"
#include "device.hpp"

namespace Device {
class CustomController {
public:
CustomController();
typedef struct {
float offset[6];
} Param;

CustomController(Param& param);

typedef struct __attribute__((packed)) {
struct __attribute__((packed)) {
uint8_t sof; // 起始字节，固定值为0xA5
uint16_t data_length; // 数据帧中data的长度
uint8_t seq; // 包序号
uint8_t crc8; // 帧头CRC8校验
} frame_header; // 帧头
uint16_t cmd_id; // 命令码
union __attribute__((packed)) {
float angle[6]; // 自定义控制器的数据帧
uint8_t raw_data[30];
};
} Data; // 自定义控制器数据包

typedef enum { NUM = 144 } ControllerEvent;

Param param_;

bool online_ = false;

uint32_t last_online_time_ = 0;

bool StartRecv();

void Prase();

void Offline();

private:
System::Semaphore packet_recv_ = System::Semaphore(false);
Data data_;

Message::Topic<Component::CMD::Data> controller_angel_ =
Message::Topic<Component::CMD::Data>("collectangle");
System::Semaphore packet_recv_ = System::Semaphore(false);

Message::Event event_;
System::Thread recv_thread_;
System::Thread trans_thread_;
Component::CMD::Data controller_data_{};
};
} // namespace Device
22 changes: 22 additions & 0 deletions src/module/custom_controller/mod_custom_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,3 +3,25 @@
#include "bsp_uart.h"

using namespace Module;

Controller txdata;
void CustomController::DataConcatenation(uint8_t *data, uint16_t data_lenth) {
static uint8_t seq = 0;
/// 帧头数据
txdata.frame_header.sof = 0xA5; // 数据帧起始字节，固定值为 0xA5
txdata.frame_header.data_length = 30; // 数据帧中数据段的长度
txdata.frame_header.seq = seq; // 包序号
txdata.frame_header.crc8 = Component::CRC8::Calculate(
reinterpret_cast<uint8_t *>(&txdata.frame_header),
FRAME_HEADER_LENGTH - sizeof(uint8_t),
CRC8_INIT); // 添加帧头 CRC8 校验位
/// 命令码ID
txdata.cmd_id = 0x0302;
/// 数据段
memcpy(txdata.data, data, data_lenth);
/// 帧尾CRC16，整包校验
txdata.frame_tail = Component::CRC16::Calculate(
reinterpret_cast<uint8_t *>(&txdata),
DATA_FRAME_LENGTH - sizeof(uint16_t), CRC16_INIT);
}
using namespace Module;
51 changes: 41 additions & 10 deletions src/module/custom_controller/mod_custom_controller.hpp
View file
Original file line number Diff line number Diff line change
@@ -1,15 +1,39 @@
#include "bsp_uart.h"
#include "comp_crc16.hpp"
#include "comp_crc8.hpp"
#include "dev_mt6701.hpp"
#include "module.hpp"

static char uart_tx_buff[1024] = {0};
#define FRAME_HEADER_LENGTH 5 // 帧头数据长度
#define CMD_ID_LENGTH 2 // 命令码ID数据长度
#define DATA_LENGTH 30 // 数据段长度
#define FRAME_TAIL_LENGTH 2 // 帧尾数据长度

#define DATA_FRAME_LENGTH \
(FRAME_HEADER_LENGTH + CMD_ID_LENGTH + DATA_LENGTH + \
FRAME_TAIL_LENGTH) // 整个数据帧的长度

#define CONTROLLER_CMD_ID 0x0302 // 自定义控制器命令码

typedef struct __attribute__((packed)) {
struct __attribute__((packed)) {
uint8_t sof; // 起始字节，固定值为0xA5
uint16_t data_length; // 数据帧中data的长度
uint8_t seq; // 包序号
uint8_t crc8; // 帧头CRC8校验
} frame_header; // 帧头
__packed uint16_t cmd_id; // 命令码
__packed uint8_t data[30]; // 自定义控制器的数据帧
__packed uint16_t frame_tail; // 帧尾CRC16校验
} Controller; // 自定义控制器数据包

namespace Module {
class CustomController {
public:
typedef struct {
Device::MT6701::Param mt6701[6];
} Param;

CustomController(Param& param)
: mt6701_1(param.mt6701[0]),
mt6701_2(param.mt6701[1]),
Expand All @@ -18,24 +42,31 @@ class CustomController {
mt6701_5(param.mt6701[4]),
mt6701_6(param.mt6701[5]) {
auto task_fun = [](CustomController* ctrl) {
(void)snprintf(
uart_tx_buff, sizeof(uart_tx_buff), "%f,%f,%f,%f,%f,%f\n",
ctrl->mt6701_1.angle_.Value(), ctrl->mt6701_2.angle_.Value(),
ctrl->mt6701_3.angle_.Value(), ctrl->mt6701_4.angle_.Value(),
ctrl->mt6701_5.angle_.Value(), ctrl->mt6701_6.angle_.Value());

bsp_uart_transmit(BSP_UART_MCU, reinterpret_cast<uint8_t*>(uart_tx_buff),
strlen(uart_tx_buff), false);
ctrl->txbuff[0] = ctrl->mt6701_1.angle_.Value();
ctrl->txbuff[1] = ctrl->mt6701_2.angle_.Value();
ctrl->txbuff[2] = ctrl->mt6701_3.angle_.Value();
ctrl->txbuff[3] = ctrl->mt6701_4.angle_.Value();
ctrl->txbuff[4] = ctrl->mt6701_5.angle_.Value();
ctrl->txbuff[5] = ctrl->mt6701_6.angle_.Value();
ctrl->DataConcatenation(reinterpret_cast<uint8_t*>(ctrl->txbuff),
sizeof(txbuff));
bsp_uart_transmit(BSP_UART_MCU, reinterpret_cast<uint8_t*>(&ctrl->txdata),
DATA_FRAME_LENGTH, false);
};

System::Timer::Create(task_fun, this, 10);
System::Timer::Create(task_fun, this, 40);
}

void DataConcatenation(uint8_t* data, uint16_t data_lenth);

Device::MT6701 mt6701_1;
Device::MT6701 mt6701_2;
Device::MT6701 mt6701_3;
Device::MT6701 mt6701_4;
Device::MT6701 mt6701_5;
Device::MT6701 mt6701_6;

float txbuff[6] = {0};
Controller txdata;
};
} // namespace Module
Loading

0 comments on commit 1886926

Please sign in to comment.