Add initial board, HAL, and executor modules
This commit is contained in:
wang committed
1 parent
3b9e129c88
commit
0b2eebe76d
82 files changed
+20611
No files matched your search
@@ -0,0 +1,257 @@
|
||||
#include "base_board_control.h"
|
||||
|
||||
#include "freertos/FreeRTOS.h"
|
||||
#include "freertos/task.h"
|
||||
#include "esp_log.h"
|
||||
|
||||
static const char *TAG = "BaseBoardCtrl";
|
||||
|
||||
esp_err_t BaseBoardControl::Init(BaseBoardHal *hal)
|
||||
{
|
||||
if (hal == nullptr)
|
||||
{
|
||||
ESP_LOGE(TAG, "Init: hal is null");
|
||||
return ESP_ERR_INVALID_ARG;
|
||||
}
|
||||
hal_ = hal;
|
||||
return hal_->Init();
|
||||
}
|
||||
|
||||
esp_err_t BaseBoardControl::GetAngle(float *angle)
|
||||
{
|
||||
if (hal_ == nullptr)
|
||||
{
|
||||
return ESP_ERR_INVALID_STATE;
|
||||
}
|
||||
return hal_->GetAngle(angle);
|
||||
}
|
||||
|
||||
esp_err_t BaseBoardControl::SendSetAngleAbs(uint16_t angle_deg,
|
||||
uint8_t speed_percent,
|
||||
uint8_t flags)
|
||||
{
|
||||
if (hal_ == nullptr)
|
||||
{
|
||||
return ESP_ERR_INVALID_STATE;
|
||||
}
|
||||
|
||||
// 角度限制在 0~359 之间
|
||||
if (angle_deg > 359)
|
||||
{
|
||||
angle_deg = static_cast<uint16_t>(angle_deg % 360);
|
||||
}
|
||||
|
||||
// 速度百分比限制在 0~100
|
||||
if (speed_percent > 100)
|
||||
{
|
||||
speed_percent = 100;
|
||||
}
|
||||
|
||||
uint8_t payload[4];
|
||||
|
||||
payload[0] = static_cast<uint8_t>(angle_deg & 0xFF); // LSB
|
||||
payload[1] = static_cast<uint8_t>((angle_deg >> 8) & 0xFF); // MSB
|
||||
payload[2] = speed_percent;
|
||||
payload[3] = flags; // 预留/扩展位
|
||||
|
||||
return SendFrame(0x01, payload, sizeof(payload));
|
||||
}
|
||||
|
||||
esp_err_t BaseBoardControl::SendSetAngleRel(int16_t delta_deg,
|
||||
uint8_t speed_percent,
|
||||
uint8_t flags)
|
||||
{
|
||||
if (hal_ == nullptr)
|
||||
{
|
||||
return ESP_ERR_INVALID_STATE;
|
||||
}
|
||||
|
||||
if (speed_percent > 100)
|
||||
{
|
||||
speed_percent = 100;
|
||||
}
|
||||
|
||||
uint8_t payload[4];
|
||||
|
||||
// int16_t 直接按补码低字节在前的顺序发
|
||||
uint16_t delta_u = static_cast<uint16_t>(delta_deg);
|
||||
|
||||
payload[0] = static_cast<uint8_t>(delta_u & 0xFF); // LSB
|
||||
payload[1] = static_cast<uint8_t>((delta_u >> 8) & 0xFF); // MSB
|
||||
payload[2] = speed_percent;
|
||||
payload[3] = flags; // 预留/扩展位
|
||||
|
||||
return SendFrame(0x02, payload, sizeof(payload));
|
||||
}
|
||||
|
||||
esp_err_t BaseBoardControl::SendSetMotorMove(MotorMode mode, uint8_t speed_percent, uint16_t duration_ms)
|
||||
{
|
||||
if (hal_ == nullptr)
|
||||
{
|
||||
return ESP_ERR_INVALID_STATE;
|
||||
}
|
||||
|
||||
if (speed_percent > 100) {
|
||||
speed_percent = 100;
|
||||
}
|
||||
|
||||
uint8_t payload[4];
|
||||
|
||||
payload[0] = mode;
|
||||
payload[1] = speed_percent;
|
||||
payload[2] = static_cast<uint8_t>(duration_ms & 0xFF);
|
||||
payload[3] = static_cast<uint8_t>((duration_ms >> 8) & 0xFF);
|
||||
|
||||
return SendFrame(0x03, payload, sizeof(payload));
|
||||
}
|
||||
|
||||
esp_err_t BaseBoardControl::SendSetLedRgb(uint8_t mode,
|
||||
uint8_t target_mask,
|
||||
uint8_t r,
|
||||
uint8_t g,
|
||||
uint8_t b,
|
||||
uint8_t speed)
|
||||
{
|
||||
if (hal_ == nullptr)
|
||||
{
|
||||
return ESP_ERR_INVALID_STATE;
|
||||
}
|
||||
|
||||
if (speed > 100)
|
||||
{
|
||||
speed = 100;
|
||||
}
|
||||
|
||||
uint8_t payload[7];
|
||||
|
||||
payload[0] = mode; // 灯效模式
|
||||
payload[1] = target_mask; // 位掩码，指示哪些 LED 生效
|
||||
payload[2] = r;
|
||||
payload[3] = g;
|
||||
payload[4] = b;
|
||||
payload[5] = speed; // 呼吸/渐变速度
|
||||
payload[6] = 0; // Reserved，写死为 0
|
||||
|
||||
return SendFrame(0x10, payload, sizeof(payload));
|
||||
}
|
||||
|
||||
esp_err_t BaseBoardControl::SendFrame(uint8_t cmd,
|
||||
const uint8_t *payload,
|
||||
uint8_t payload_len)
|
||||
{
|
||||
if (hal_ == nullptr)
|
||||
{
|
||||
return ESP_ERR_INVALID_STATE;
|
||||
}
|
||||
|
||||
// LEN = CMD(1) + payload_len
|
||||
uint8_t len = static_cast<uint8_t>(1u + payload_len);
|
||||
|
||||
// 总长度 = HEAD1 + HEAD2 + LEN + (CMD + payload_len) + CRC = 3 + len + 1
|
||||
size_t frame_len = static_cast<size_t>(3u + len + 1u);
|
||||
if (frame_len > kMaxFrameLen)
|
||||
{
|
||||
ESP_LOGE(TAG, "SendFrame: frame too long, len=%u", static_cast<unsigned>(frame_len));
|
||||
return ESP_ERR_INVALID_SIZE;
|
||||
}
|
||||
|
||||
uint8_t frame[kMaxFrameLen];
|
||||
|
||||
// 帧头
|
||||
frame[0] = 0xAA;
|
||||
frame[1] = 0x55;
|
||||
|
||||
// LEN 字段：从 CMD 开始计数
|
||||
frame[2] = len;
|
||||
|
||||
// CMD
|
||||
frame[3] = cmd;
|
||||
|
||||
// payload
|
||||
for (uint8_t i = 0; i < payload_len; ++i)
|
||||
{
|
||||
frame[4u + i] = payload[i];
|
||||
}
|
||||
|
||||
uint8_t crc = CRC8(&frame[2], len + 1); // 从 LEN 开始算起
|
||||
|
||||
// CRC 放在数据之后
|
||||
frame[3u + len] = crc;
|
||||
|
||||
const int kMaxRetries = 5;
|
||||
const int kRetryDelayMs = 50;
|
||||
|
||||
for (int i = 0; i < kMaxRetries; i++)
|
||||
{
|
||||
esp_err_t err = hal_->Send(frame, frame_len);
|
||||
if (err == ESP_OK)
|
||||
{
|
||||
// 等待从机处理并准备好 ACK
|
||||
// 增加延时以确保从机有足够时间写入 I2C 缓冲
|
||||
vTaskDelay(pdMS_TO_TICKS(20));
|
||||
|
||||
uint8_t ack_buf[kAckFrameLen] = {0};
|
||||
err = hal_->Read(ack_buf, kAckFrameLen);
|
||||
if (err == ESP_OK)
|
||||
{
|
||||
// 校验 ACK
|
||||
if (ack_buf[0] == 0xAA && ack_buf[1] == 0x55 &&
|
||||
ack_buf[2] == 0x01 && ack_buf[3] == 0xFF)
|
||||
{
|
||||
uint8_t crc = CRC8(&ack_buf[2], 2);
|
||||
if (crc == ack_buf[4])
|
||||
{
|
||||
return ESP_OK; // 握手成功
|
||||
}
|
||||
else
|
||||
{
|
||||
ESP_LOGW(TAG, "ACK CRC Fail: Recv=0x%02X Calc=0x%02X", ack_buf[4], crc);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
ESP_LOGW(TAG, "Invalid ACK: %02X %02X %02X %02X",
|
||||
ack_buf[0], ack_buf[1], ack_buf[2], ack_buf[3]);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
ESP_LOGW(TAG, "Read ACK Failed: %s", esp_err_to_name(err));
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
ESP_LOGW(TAG, "Send Failed: %s", esp_err_to_name(err));
|
||||
}
|
||||
|
||||
// 重试前等待
|
||||
vTaskDelay(pdMS_TO_TICKS(kRetryDelayMs));
|
||||
}
|
||||
|
||||
ESP_LOGE(TAG, "SendFrame Failed after %d retries", kMaxRetries);
|
||||
return ESP_FAIL;
|
||||
}
|
||||
|
||||
uint8_t BaseBoardControl::CRC8(const uint8_t *data, size_t len)
|
||||
{
|
||||
uint8_t crc = 0x00;
|
||||
|
||||
for (size_t i = 0; i < len; ++i)
|
||||
{
|
||||
crc ^= data[i];
|
||||
|
||||
for (int b = 0; b < 8; ++b)
|
||||
{
|
||||
if (crc & 0x80)
|
||||
{
|
||||
crc = static_cast<uint8_t>((crc << 1) ^ 0x07);
|
||||
}
|
||||
else
|
||||
{
|
||||
crc = static_cast<uint8_t>(crc << 1);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return crc;
|
||||
}
|
||||
@@ -0,0 +1,63 @@
|
||||
// base_board_control.h
|
||||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
#include "esp_err.h"
|
||||
#include "base_board_hal.h"
|
||||
|
||||
class BaseBoardControl
|
||||
{
|
||||
public:
|
||||
/** 帧最大长度，按你的协议冗余留一点空间 */
|
||||
static constexpr size_t kMaxFrameLen = 16;
|
||||
/** ACK 帧长度: HEAD(2) + LEN(1) + CMD(1) + CRC(1) = 5 */
|
||||
static constexpr size_t kAckFrameLen = 5;
|
||||
|
||||
BaseBoardControl() = default;
|
||||
~BaseBoardControl() = default;
|
||||
|
||||
esp_err_t Init(BaseBoardHal *hal);
|
||||
|
||||
/** 发送绝对角度命令 CMD=0x01 */
|
||||
esp_err_t SendSetAngleAbs(uint16_t angle_deg,
|
||||
uint8_t speed_percent,
|
||||
uint8_t flags = 0);
|
||||
|
||||
/** 发送相对角度命令 CMD=0x02 */
|
||||
esp_err_t SendSetAngleRel(int16_t delta_deg,
|
||||
uint8_t speed_percent,
|
||||
uint8_t flags = 0);
|
||||
|
||||
/** 电机工作模式枚举 */
|
||||
enum MotorMode
|
||||
{
|
||||
MotorOff = 0,
|
||||
MotorForward = 1,
|
||||
MotorBackward = 2
|
||||
};
|
||||
|
||||
/** 发送电机开环控制命令 CMD=0x03 */
|
||||
esp_err_t SendSetMotorMove(MotorMode mode, uint8_t speed_percent, uint16_t duration_ms);
|
||||
|
||||
/** 发送 RGB 命令 CMD=0x10 */
|
||||
esp_err_t SendSetLedRgb(uint8_t mode,
|
||||
uint8_t target_mask,
|
||||
uint8_t r,
|
||||
uint8_t g,
|
||||
uint8_t b,
|
||||
uint8_t speed);
|
||||
|
||||
/** 获取当前角度 */
|
||||
esp_err_t GetAngle(float *angle);
|
||||
|
||||
private:
|
||||
BaseBoardHal *hal_ = nullptr;
|
||||
|
||||
/** 通用打包 + 发送接口：传 CMD 和 payload，内部加头、LEN、CRC */
|
||||
esp_err_t SendFrame(uint8_t cmd,
|
||||
const uint8_t *payload,
|
||||
uint8_t payload_len);
|
||||
|
||||
/** CRC8 工具函数 */
|
||||
static uint8_t CRC8(const uint8_t *data, size_t len);
|
||||
};
|
||||
@@ -0,0 +1,16 @@
|
||||
# main/05_executors/base_board_control/module.cmake
|
||||
|
||||
|
||||
# 判断 menuconfig 中的开关
|
||||
if(NOT CONFIG_ENABLE_EXEC_BASE_BOARD_CONTROL AND NOT EARLY_EXPANSION)
|
||||
message(STATUS " [Skip] base_board_control module disabled by menuconfig.")
|
||||
return()
|
||||
endif()
|
||||
|
||||
list(APPEND MODULE_SRCS
|
||||
"05_executors/base_board_control/base_board_control.cc"
|
||||
)
|
||||
|
||||
list(APPEND MODULE_INCLUDE_DIRS
|
||||
"05_executors/base_board_control"
|
||||
)
|
||||
Reference in new issue
Block a user