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,167 @@
|
||||
#include "adc_battery_monitor.h"
|
||||
#include <esp_log.h>
|
||||
|
||||
#define TAG "AdcBatteryMonitor" ///< 日志标签,用于ESP-IDF日志系统
|
||||
|
||||
AdcBatteryMonitor::AdcBatteryMonitor(BoardSweekar& board, adc_unit_t adc_unit, adc_channel_t adc_channel, float upper_resistor, float lower_resistor)
|
||||
: board_(board) {
|
||||
|
||||
// Initialize ADC battery estimation
|
||||
adc_battery_estimation_t adc_cfg = {
|
||||
.internal = {
|
||||
.adc_unit = adc_unit,
|
||||
.adc_bitwidth = ADC_BITWIDTH_DEFAULT,
|
||||
.adc_atten = ADC_ATTEN_DB_12,
|
||||
},
|
||||
.adc_channel = adc_channel,
|
||||
.upper_resistor = upper_resistor,
|
||||
.lower_resistor = lower_resistor
|
||||
};
|
||||
adc_cfg.charging_detect_cb = [](void *user_data) -> bool {
|
||||
AdcBatteryMonitor *self = (AdcBatteryMonitor *)user_data;
|
||||
return self->IsCharging();
|
||||
};
|
||||
adc_cfg.charging_detect_user_data = this;
|
||||
adc_battery_estimation_handle_ = adc_battery_estimation_create(&adc_cfg);
|
||||
|
||||
// Initialize timer
|
||||
esp_timer_create_args_t timer_cfg = {
|
||||
.callback = [](void *arg) {
|
||||
AdcBatteryMonitor *self = (AdcBatteryMonitor *)arg;
|
||||
self->CheckBatteryStatus();
|
||||
},
|
||||
.arg = this,
|
||||
.name = "adc_battery_monitor",
|
||||
.skip_unhandled_events = false,
|
||||
};
|
||||
ESP_ERROR_CHECK(esp_timer_create(&timer_cfg, &timer_handle_));
|
||||
ESP_ERROR_CHECK(esp_timer_start_periodic(timer_handle_, 1000000));
|
||||
}
|
||||
|
||||
// 注册ADC电池回调函数
|
||||
void AdcBatteryMonitor::AdcBatteryMonitor_Init(void) {
|
||||
ESP_LOGI(TAG, "注册ADC电池回调函数");
|
||||
// 设置默认的电池状态回调函数
|
||||
OnNotCharging([]() {
|
||||
ESP_LOGI(TAG, "电池未充电状态回调 - 默认处理");
|
||||
});
|
||||
|
||||
OnFull([]() {
|
||||
ESP_LOGI(TAG, "电池充满电状态回调 - 默认处理");
|
||||
});
|
||||
|
||||
OnCharging([]() {
|
||||
ESP_LOGI(TAG, "电池充电中状态回调 - 默认处理");
|
||||
});
|
||||
}
|
||||
|
||||
AdcBatteryMonitor::~AdcBatteryMonitor() {
|
||||
if (adc_battery_estimation_handle_) {
|
||||
ESP_ERROR_CHECK(adc_battery_estimation_destroy(adc_battery_estimation_handle_));
|
||||
}
|
||||
|
||||
if (timer_handle_) {
|
||||
esp_timer_stop(timer_handle_);
|
||||
esp_timer_delete(timer_handle_);
|
||||
}
|
||||
}
|
||||
|
||||
bool AdcBatteryMonitor::IsCharging() {
|
||||
// 通过IO扩展板读取充电检测引脚状态
|
||||
// BATTERY_CHRG_PIN连接到P0_5，对应INPUT_PORT0
|
||||
uint8_t port0_data = 0;
|
||||
esp_err_t err = board_.io_expansion_.ReadInputPort0(&port0_data);
|
||||
|
||||
// 如果读取失败，返回false
|
||||
if (err != ESP_OK) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// 检查BATTERY_CHRG_PIN位状态
|
||||
// 当引脚为低电平时表示正在充电
|
||||
return (port0_data & (1 << BATTERY_CHRG_PIN)) == 0;
|
||||
}
|
||||
|
||||
bool AdcBatteryMonitor::IsDischarging() {
|
||||
return !IsCharging();
|
||||
}
|
||||
|
||||
uint8_t AdcBatteryMonitor::GetBatteryLevel() {
|
||||
// 如果句柄无效，返回默认值
|
||||
if (adc_battery_estimation_handle_ == nullptr) {
|
||||
printf("adc_battery_estimation_handle_ 为空！\n");
|
||||
return 0;
|
||||
}
|
||||
|
||||
float capacity = 0;
|
||||
esp_err_t err = adc_battery_estimation_get_capacity(adc_battery_estimation_handle_, &capacity);
|
||||
if (err != ESP_OK) {
|
||||
printf("返回值出错！\n");
|
||||
return 0; // 出错时返回默认值
|
||||
}
|
||||
return (uint8_t)capacity;
|
||||
}
|
||||
|
||||
void AdcBatteryMonitor::OnChargingStatusChanged(std::function<void(bool)> callback) {
|
||||
on_charging_status_changed_ = callback;
|
||||
}
|
||||
|
||||
void AdcBatteryMonitor::CheckBatteryStatus() {
|
||||
bool new_charging_status = IsCharging();
|
||||
if (new_charging_status != is_charging_) {
|
||||
is_charging_ = new_charging_status;
|
||||
if (on_charging_status_changed_) {
|
||||
on_charging_status_changed_(is_charging_);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 三个电池状态回调函数的实现
|
||||
void AdcBatteryMonitor::TriggerNotChrgCallback() {
|
||||
if (on_not_charging_callback_) {
|
||||
on_not_charging_callback_();
|
||||
}
|
||||
}
|
||||
|
||||
void AdcBatteryMonitor::TriggerFullCallback() {
|
||||
if (on_full_callback_) {
|
||||
on_full_callback_();
|
||||
}
|
||||
}
|
||||
|
||||
void AdcBatteryMonitor::TriggerChrgCallback() {
|
||||
if (on_charging_callback_) {
|
||||
on_charging_callback_();
|
||||
}
|
||||
}
|
||||
|
||||
// 注册电池状态回调函数的实现
|
||||
void AdcBatteryMonitor::OnNotCharging(std::function<void()> callback) {
|
||||
on_not_charging_callback_ = callback;
|
||||
}
|
||||
|
||||
void AdcBatteryMonitor::OnFull(std::function<void()> callback) {
|
||||
on_full_callback_ = callback;
|
||||
}
|
||||
|
||||
void AdcBatteryMonitor::OnCharging(std::function<void()> callback) {
|
||||
on_charging_callback_ = callback;
|
||||
}
|
||||
|
||||
//CHRG引脚充电前为高电平 充电中为低电平 充满电为高电平
|
||||
//FULL未充满为高电平 充满电为低电平
|
||||
bool AdcBatteryMonitor::IsFull() {
|
||||
// 通过IO扩展板读取电池充满检测引脚状态
|
||||
// BATTERY_FULL_PIN连接到P1_3，对应INPUT_PORT1
|
||||
uint8_t port1_data = 0;
|
||||
esp_err_t err = board_.io_expansion_.ReadInputPort1(&port1_data);
|
||||
|
||||
// 如果读取失败，返回false
|
||||
if (err != ESP_OK) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// 检查BATTERY_FULL_PIN位状态
|
||||
// 当引脚为低电平时表示电池已充满
|
||||
return (port1_data & (1 << BATTERY_FULL_PIN)) == 0;
|
||||
}
|
||||
@@ -0,0 +1,47 @@
|
||||
#ifndef ADC_BATTERY_MONITOR_H
|
||||
#define ADC_BATTERY_MONITOR_H
|
||||
|
||||
#include <functional>
|
||||
#include <driver/gpio.h>
|
||||
#include <adc_battery_estimation.h>
|
||||
#include <esp_timer.h>
|
||||
#include "02_physical/boards/board_sweekar.h"
|
||||
|
||||
class AdcBatteryMonitor {
|
||||
public:
|
||||
AdcBatteryMonitor(BoardSweekar& board, adc_unit_t adc_unit, adc_channel_t adc_channel, float upper_resistor, float lower_resistor);
|
||||
~AdcBatteryMonitor();
|
||||
|
||||
bool IsCharging();
|
||||
bool IsDischarging();
|
||||
bool IsFull(); // 电池充满检测函数
|
||||
uint8_t GetBatteryLevel();
|
||||
|
||||
// 注册电池状态回调函数
|
||||
void AdcBatteryMonitor_Init(void);
|
||||
// 三个电池状态回调函数
|
||||
void TriggerNotChrgCallback(); // 触发未充电回调
|
||||
void TriggerFullCallback(); // 触发充满电回调
|
||||
void TriggerChrgCallback(); // 触发充电中回调
|
||||
|
||||
// 注册电池状态回调函数
|
||||
void OnNotCharging(std::function<void()> callback); // 注册未充电回调
|
||||
void OnFull(std::function<void()> callback); // 注册充满电回调
|
||||
void OnCharging(std::function<void()> callback); // 注册充电中回调
|
||||
|
||||
void OnChargingStatusChanged(std::function<void(bool)> callback);
|
||||
|
||||
private:
|
||||
BoardSweekar& board_; // 添加对BoardSweekar的引用
|
||||
adc_battery_estimation_handle_t adc_battery_estimation_handle_ = nullptr;
|
||||
esp_timer_handle_t timer_handle_ = nullptr;
|
||||
bool is_charging_ = false;
|
||||
std::function<void(bool)> on_charging_status_changed_;
|
||||
std::function<void()> on_not_charging_callback_; // 未充电回调函数
|
||||
std::function<void()> on_full_callback_; // 充满电回调函数
|
||||
std::function<void()> on_charging_callback_; // 充电中回调函数
|
||||
|
||||
void CheckBatteryStatus();
|
||||
};
|
||||
|
||||
#endif // ADC_BATTERY_MONITOR_H
|
||||
@@ -0,0 +1,16 @@
|
||||
# main/05_executors/adc_battery_monitor/module.cmake
|
||||
|
||||
|
||||
# 判断 menuconfig 中的开关
|
||||
if(NOT CONFIG_ENABLE_EXEC_ADC_BATTERY_MONITOR AND NOT EARLY_EXPANSION)
|
||||
message(STATUS " [Skip] adc_battery_monitor module disabled by menuconfig.")
|
||||
return()
|
||||
endif()
|
||||
|
||||
list(APPEND MODULE_SRCS
|
||||
"05_executors/adc_battery_monitor/adc_battery_monitor.cc"
|
||||
)
|
||||
|
||||
list(APPEND MODULE_INCLUDE_DIRS
|
||||
"05_executors/adc_battery_monitor"
|
||||
)
|
||||
@@ -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"
|
||||
)
|
||||
@@ -0,0 +1,625 @@
|
||||
#include "gyroscope.h"
|
||||
#include "freertos/idf_additions.h"
|
||||
|
||||
Gyroscope::Gyroscope()
|
||||
{
|
||||
bus_handle_ = nullptr;
|
||||
on_shake_callback_ = nullptr; // 初始化回调函数
|
||||
on_pickup_callback_ = nullptr; // 初始化拿起回调函数
|
||||
on_putdown_callback_ = nullptr; // 初始化放置回调函数
|
||||
on_tilt_callback_ = nullptr; // 初始化倾斜回调函数
|
||||
timer_handle_ = nullptr; // 初始化定时器句柄
|
||||
pGyroscopeTaskStackBuffer_ = nullptr;
|
||||
pxGyroscopeTaskBuffer_ = nullptr;
|
||||
|
||||
// 初始化滤波相关变量
|
||||
last_filtered_x_ = 0;
|
||||
last_filtered_y_ = 0;
|
||||
last_filtered_z_ = 0;
|
||||
is_first_sample_ = true;
|
||||
}
|
||||
|
||||
Gyroscope::~Gyroscope()
|
||||
{
|
||||
// 删除定时器
|
||||
DeleteTimer();
|
||||
|
||||
if (pGyroscopeTaskStackBuffer_) {
|
||||
free(pGyroscopeTaskStackBuffer_);
|
||||
pGyroscopeTaskStackBuffer_ = nullptr;
|
||||
}
|
||||
if (pxGyroscopeTaskBuffer_) {
|
||||
vPortFree(pxGyroscopeTaskBuffer_);
|
||||
pxGyroscopeTaskBuffer_ = nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 创建500ms周期定时器
|
||||
* @return esp_err_t 创建结果
|
||||
*/
|
||||
esp_err_t Gyroscope::CreateTimer()
|
||||
{
|
||||
if (timer_handle_ != nullptr) {
|
||||
ESP_LOGW(TAG_GYROSCOPE, "定时器已存在");
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
// 配置定时器参数
|
||||
esp_timer_create_args_t timer_args = {};
|
||||
timer_args.callback = &TimerCallback; // 静态回调函数
|
||||
timer_args.arg = this; // 传入实例指针
|
||||
timer_args.name = "gyro_timer"; // 定时器名称
|
||||
timer_args.dispatch_method = ESP_TIMER_TASK; // 使用任务调度
|
||||
timer_args.skip_unhandled_events = false; // 不跳过未处理事件
|
||||
|
||||
// 创建定时器
|
||||
esp_err_t err = esp_timer_create(&timer_args, &timer_handle_);
|
||||
if (err != ESP_OK) {
|
||||
ESP_LOGE(TAG_GYROSCOPE, "创建定时器失败: 0x%x", err);
|
||||
return err;
|
||||
}
|
||||
|
||||
// 启动定时器，500ms周期重复
|
||||
err = esp_timer_start_periodic(timer_handle_, 50000); // 50000微秒 = 50ms 20hz
|
||||
if (err != ESP_OK) {
|
||||
ESP_LOGE(TAG_GYROSCOPE, "启动定时器失败: 0x%x", err);
|
||||
esp_timer_delete(timer_handle_);
|
||||
timer_handle_ = nullptr;
|
||||
return err;
|
||||
}
|
||||
|
||||
ESP_LOGI(TAG_GYROSCOPE, "50ms周期定时器创建并启动成功");
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 停止并删除定时器
|
||||
*/
|
||||
void Gyroscope::DeleteTimer()
|
||||
{
|
||||
if (timer_handle_ == nullptr) {
|
||||
return;
|
||||
}
|
||||
|
||||
// 停止定时器
|
||||
esp_err_t err = esp_timer_stop(timer_handle_);
|
||||
if (err != ESP_OK && err != ESP_ERR_INVALID_STATE) {
|
||||
ESP_LOGE(TAG_GYROSCOPE, "停止定时器失败: 0x%x", err);
|
||||
}
|
||||
|
||||
// 删除定时器
|
||||
err = esp_timer_delete(timer_handle_);
|
||||
if (err != ESP_OK) {
|
||||
ESP_LOGE(TAG_GYROSCOPE, "删除定时器失败: 0x%x", err);
|
||||
}
|
||||
|
||||
timer_handle_ = nullptr;
|
||||
ESP_LOGI(TAG_GYROSCOPE, "定时器已删除");
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 静态定时器回调函数
|
||||
* @param arg 传入的Gyroscope实例指针
|
||||
*/
|
||||
void Gyroscope::TimerCallback(void* arg)
|
||||
{
|
||||
Gyroscope* gyroscope = static_cast<Gyroscope*>(arg);
|
||||
if (gyroscope) {
|
||||
gyroscope->TimerInterruptHandler();
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 定时器中断处理函数
|
||||
* 每50ms调用一次，可以在这里执行周期性任务
|
||||
*/
|
||||
void Gyroscope::TimerInterruptHandler()
|
||||
{
|
||||
// 在这里添加定时器中断处理逻辑
|
||||
// 例如：读取传感器数据、检测动作、发送状态等
|
||||
AccelerationData accel_data_ = {0};
|
||||
|
||||
esp_err_t err = GetAcceleration(&accel_data_);
|
||||
if (err == ESP_OK) {
|
||||
// 根据实际安装方式重新定义姿态判断
|
||||
// 平放状态：Y轴接近±1g（因为Y轴垂直于桌面），X和Z轴接近0
|
||||
bool is_flat = (fabs(accel_data_.y) > 0.85 && fabs(accel_data_.y) < 1.15) &&
|
||||
(fabs(accel_data_.x) < 0.15 && fabs(accel_data_.z) < 0.15);
|
||||
|
||||
// 手持状态：Y轴接近0，X和Z轴中至少有一个有明显值
|
||||
bool is_held = (fabs(accel_data_.y) < 0.7) &&
|
||||
(fabs(accel_data_.x) > 0.15 || fabs(accel_data_.z) > 0.15);
|
||||
|
||||
// 计算合成加速度（总加速度大小）
|
||||
float total_accel = sqrt(accel_data_.x * accel_data_.x +
|
||||
accel_data_.y * accel_data_.y +
|
||||
accel_data_.z * accel_data_.z);
|
||||
|
||||
// 计算与上次合成加速度的差值（加速度变化量）
|
||||
float total_accel_diff = fabs(total_accel - total_accel_last_);
|
||||
|
||||
// 计算Y轴加速度变化
|
||||
float y_diff = accel_data_.y - accel_data_y_last_;
|
||||
|
||||
// 检测是否处于平放稳定状态
|
||||
if(is_flat) {
|
||||
stable_count_++;
|
||||
if(stable_count_ >= 8) { // 连续8次检测到平放状态（400ms），减少误判
|
||||
stable_count_ = 8; // 防止溢出
|
||||
if(!is_stable_) {
|
||||
ESP_LOGI(TAG_GYROSCOPE, "定时器: 检测到稳定平放状态 (x:%.2f, y:%.2f, z:%.2f, total:%.2f)",
|
||||
accel_data_.x, accel_data_.y, accel_data_.z, total_accel);
|
||||
is_stable_ = true;
|
||||
is_lifted_ = false; // 重置拿起状态
|
||||
putdown_count_ = 0; // 重置放置计数器
|
||||
lift_count_ = 0; // 重置拿起计数器
|
||||
|
||||
// 调用放置回调
|
||||
if (on_putdown_callback_) {
|
||||
on_putdown_callback_();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else {
|
||||
stable_count_ = 0; // 不在范围内则重置稳定计数器
|
||||
|
||||
// 如果之前是稳定状态，检测是否有拿起动作
|
||||
if(is_stable_) {
|
||||
// 检测拿起动作的多种特征：
|
||||
// 1. Y轴从接近1g变为接近0g（位置变化）
|
||||
// 2. 合成加速度有显著变化（表示有加速度）
|
||||
bool y_change_indicates_lift = fabs(y_diff) > 0.25;
|
||||
bool total_accel_change_indicates_lift = total_accel_diff > 0.3; // 检测加速度突变
|
||||
|
||||
if(y_change_indicates_lift || total_accel_change_indicates_lift) {
|
||||
lift_count_++;
|
||||
if(lift_count_ >= 1) { // 降低拿起检测阈值，提高灵敏度
|
||||
ESP_LOGI(TAG_GYROSCOPE, "定时器: 检测到拿起动作 (x:%.2f, y:%.2f, z:%.2f, total:%.2f, y_diff:%.2f, accel_diff:%.2f)",
|
||||
accel_data_.x, accel_data_.y, accel_data_.z, total_accel, y_diff, total_accel_diff);
|
||||
is_stable_ = false; // 重置状态
|
||||
is_lifted_ = true; // 设置拿起状态
|
||||
lift_count_ = 0;
|
||||
stable_count_ = 0;
|
||||
|
||||
// 调用拿起回调
|
||||
if (on_pickup_callback_) {
|
||||
on_pickup_callback_();
|
||||
}
|
||||
}
|
||||
} else {
|
||||
lift_count_ = 0; // 变化不符合预期，重置拿起计数器
|
||||
}
|
||||
}
|
||||
// 如果之前是被拿起状态，检测是否有放置动作
|
||||
else if(is_lifted_) {
|
||||
// 检测从手持到平放的变化 - Y轴从接近0g变为接近1g
|
||||
bool y_change_indicates_putdown = (y_diff > 0.25 && y_diff < 0.80);
|
||||
|
||||
if(y_change_indicates_putdown) {
|
||||
putdown_count_++;
|
||||
if(putdown_count_ >= 3) { // 连续3次检测到缓慢变化
|
||||
ESP_LOGI(TAG_GYROSCOPE, "定时器: 检测到缓慢放置动作 (x:%.2f, y:%.2f, z:%.2f, total:%.2f, y_diff:%.2f)",
|
||||
accel_data_.x, accel_data_.y, accel_data_.z, total_accel, y_diff);
|
||||
is_lifted_ = false; // 重置拿起状态
|
||||
putdown_count_ = 0;
|
||||
|
||||
// 调用放置回调
|
||||
if (on_putdown_callback_) {
|
||||
on_putdown_callback_();
|
||||
}
|
||||
}
|
||||
} else {
|
||||
putdown_count_ = 0; // 变化不符合预期，重置放置计数器
|
||||
}
|
||||
}
|
||||
|
||||
// 检测倾斜动作 - 仅当手持时才触发倾斜
|
||||
if (is_held && (fabs(accel_data_.x) > 0.3f || fabs(accel_data_.z) > 0.3f)) {
|
||||
// 检测到倾斜，调用倾斜回调
|
||||
if (on_tilt_callback_) {
|
||||
on_tilt_callback_();
|
||||
}
|
||||
}
|
||||
}
|
||||
// 保存当前值作为下一次的上一次值
|
||||
accel_data_y_last_ = accel_data_.y;
|
||||
total_accel_last_ = total_accel;
|
||||
}
|
||||
}
|
||||
/**
|
||||
* @brief 注册摇晃检测回调函数
|
||||
*
|
||||
* @param callback 当检测到摇晃时调用的回调函数
|
||||
*/
|
||||
void Gyroscope::OnShake(std::function<void()> callback)
|
||||
{
|
||||
on_shake_callback_ = callback;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 注册拿起检测回调函数
|
||||
*
|
||||
* @param callback 当检测到设备被拿起时调用的回调函数
|
||||
*/
|
||||
void Gyroscope::OnPickUp(std::function<void()> callback)
|
||||
{
|
||||
on_pickup_callback_ = callback;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 注册放置检测回调函数
|
||||
*
|
||||
* @param callback 当检测到设备被放置时调用的回调函数
|
||||
*/
|
||||
void Gyroscope::OnPutDown(std::function<void()> callback)
|
||||
{
|
||||
on_putdown_callback_ = callback;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 注册倾斜检测回调函数
|
||||
*
|
||||
* @param callback 当检测到设备倾斜时调用的回调函数
|
||||
*/
|
||||
void Gyroscope::OnTilt(std::function<void()> callback)
|
||||
{
|
||||
on_tilt_callback_ = callback;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 静态中断处理函数
|
||||
*
|
||||
* @param arg 传入的Gyroscope实例指针
|
||||
*/
|
||||
void IRAM_ATTR Gyroscope::GpioIsrHandler(void* arg)
|
||||
{
|
||||
Gyroscope* gyroscope = static_cast<Gyroscope*>(arg);
|
||||
if (gyroscope) {
|
||||
gyroscope->interrupt_flag_ = true;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 向陀螺仪寄存器写入数据
|
||||
*
|
||||
* @param reg 寄存器地址
|
||||
* @param data 要写入的数据
|
||||
* @return esp_err_t ESP_OK表示成功
|
||||
*/
|
||||
esp_err_t Gyroscope::WriteRegister(uint8_t reg, uint8_t data)
|
||||
{
|
||||
if (bus_handle_ == nullptr) {
|
||||
ESP_LOGE(TAG_GYROSCOPE, "I2C总线未初始化");
|
||||
return ESP_FAIL;
|
||||
}
|
||||
|
||||
return i2c_bus_write_bytes(bus_handle_, GYROSCOPE_I2C_SLAVE_ADDR << 1, ®, 1, &data, 1);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 从陀螺仪寄存器读取数据
|
||||
*
|
||||
* @param reg 寄存器地址
|
||||
* @param data 读取到的数据指针
|
||||
* @return esp_err_t ESP_OK表示成功
|
||||
*/
|
||||
esp_err_t Gyroscope::ReadRegister(uint8_t reg, uint8_t* data)
|
||||
{
|
||||
if (bus_handle_ == nullptr || data == nullptr) {
|
||||
ESP_LOGE(TAG_GYROSCOPE, "参数错误或I2C总线未初始化");
|
||||
return ESP_FAIL;
|
||||
}
|
||||
|
||||
return i2c_bus_read_bytes(bus_handle_, GYROSCOPE_I2C_SLAVE_ADDR << 1, ®, 1, data, 1);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 批量读取陀螺仪寄存器数据
|
||||
*
|
||||
* @param reg 起始寄存器地址
|
||||
* @param data 读取到的数据指针
|
||||
* @param length 要读取的字节数
|
||||
* @return esp_err_t ESP_OK表示成功
|
||||
*/
|
||||
esp_err_t Gyroscope::ReadRegisters(uint8_t reg, uint8_t* data, size_t length)
|
||||
{
|
||||
if (bus_handle_ == nullptr || data == nullptr) {
|
||||
ESP_LOGE(TAG_GYROSCOPE, "参数错误或I2C总线未初始化");
|
||||
return ESP_FAIL;
|
||||
}
|
||||
|
||||
return i2c_bus_read_bytes(bus_handle_, GYROSCOPE_I2C_SLAVE_ADDR << 1, ®, 1, data, length);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 初始化陀螺仪中断GPIO
|
||||
*
|
||||
* @param gpio_num 要初始化的GPIO引脚号
|
||||
* @return esp_err_t ESP_OK表示成功
|
||||
*/
|
||||
esp_err_t Gyroscope::InterruptGpioInit(gpio_num_t gpio_num)
|
||||
{
|
||||
gpio_config_t io_conf = {};
|
||||
io_conf.intr_type = GPIO_INTR_POSEDGE; // 上升沿触发
|
||||
io_conf.pin_bit_mask = (1ULL<<gpio_num);
|
||||
io_conf.mode = GPIO_MODE_INPUT;
|
||||
io_conf.pull_up_en = GPIO_PULLUP_DISABLE; // 禁用上拉,因为INT1已经是推挽输出
|
||||
io_conf.pull_down_en = GPIO_PULLDOWN_ENABLE; // 启用下拉
|
||||
|
||||
esp_err_t err = gpio_config(&io_conf);
|
||||
if (err != ESP_OK) {
|
||||
ESP_LOGE(TAG_GYROSCOPE, "GPIO配置失败,错误码: 0x%x", err);
|
||||
return err;
|
||||
}
|
||||
|
||||
// 配置中断服务
|
||||
err = gpio_install_isr_service(ESP_INTR_FLAG_LEVEL3 | ESP_INTR_FLAG_IRAM);
|
||||
if (err != ESP_OK && err != ESP_ERR_INVALID_STATE) {
|
||||
ESP_LOGE(TAG_GYROSCOPE, "GPIO中断服务安装失败,错误码: 0x%x", err);
|
||||
return err;
|
||||
}
|
||||
|
||||
// 注册中断处理函数
|
||||
err = gpio_isr_handler_add(gpio_num, GpioIsrHandler, this);
|
||||
if (err != ESP_OK) {
|
||||
ESP_LOGE(TAG_GYROSCOPE, "GPIO中断处理函数添加失败,错误码: 0x%x", err);
|
||||
return err;
|
||||
}
|
||||
|
||||
// 清除任何待处理的中断
|
||||
gpio_intr_disable(gpio_num);
|
||||
gpio_intr_enable(gpio_num);
|
||||
|
||||
ESP_LOGI(TAG_GYROSCOPE, "GPIO%d中断初始化完成", gpio_num);
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 清除中断标志
|
||||
*
|
||||
* 该函数会读取唤醒源寄存器和功能源寄存器来清除中断标志
|
||||
*
|
||||
* @return esp_err_t ESP_OK表示成功
|
||||
*/
|
||||
esp_err_t Gyroscope::ClearInterruptFlags()
|
||||
{
|
||||
esp_err_t err;
|
||||
uint8_t wake_up_src = 0;
|
||||
uint8_t func_src1 = 0;
|
||||
// 读取唤醒源寄存器和功能源寄存器来清除中断
|
||||
err = ReadRegister(WAKE_UP_SRC, &wake_up_src);
|
||||
if (err != ESP_OK) return err;
|
||||
err = ReadRegister(FUNC_SRC1, &func_src1);
|
||||
if (err != ESP_OK) return err;
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 初始化陀螺仪
|
||||
*
|
||||
* @param bus_handle I2C总线句柄
|
||||
* @return esp_err_t ESP_OK表示成功
|
||||
*/
|
||||
esp_err_t Gyroscope::Init(i2c_bus_handle_t bus_handle)
|
||||
{
|
||||
bus_handle_ = bus_handle;
|
||||
if (bus_handle_ == nullptr) {
|
||||
ESP_LOGE(TAG_GYROSCOPE, "I2C总线句柄为空");
|
||||
return ESP_FAIL;
|
||||
}
|
||||
|
||||
ESP_LOGI(TAG_GYROSCOPE, "开始初始化陀螺仪...");
|
||||
|
||||
// 1. 验证设备连接和通信
|
||||
uint8_t who_am_i = 0;
|
||||
esp_err_t err = ReadRegister(0x0F, &who_am_i); // WHO_AM_I寄存器
|
||||
if (err != ESP_OK) {
|
||||
ESP_LOGE(TAG_GYROSCOPE, "无法读取陀螺仪ID: 0x%x", err);
|
||||
return err;
|
||||
}
|
||||
|
||||
if (who_am_i != 0x6A) { // LSM6DS3TR-C的WHO_AM_I值
|
||||
ESP_LOGE(TAG_GYROSCOPE, "设备ID不匹配,期望0x6A,实际0x%02X", who_am_i);
|
||||
return ESP_FAIL;
|
||||
}
|
||||
ESP_LOGI(TAG_GYROSCOPE, "陀螺仪设备ID验证成功: 0x%02X", who_am_i);
|
||||
|
||||
// 2. 初始化中断引脚
|
||||
err = InterruptGpioInit(GYROSCOPE_INPUT_PIN_SEL);
|
||||
if (err != ESP_OK) {
|
||||
ESP_LOGE(TAG_GYROSCOPE, "中断引脚初始化失败: 0x%x", err);
|
||||
return err;
|
||||
}
|
||||
ESP_LOGI(TAG_GYROSCOPE, "开始配置唤醒中断...");
|
||||
|
||||
// 1. 软件复位陀螺仪
|
||||
err = WriteRegister(CTRL3_C, 0x01); // SW_RESET
|
||||
if (err != ESP_OK) return err;
|
||||
|
||||
vTaskDelay(pdMS_TO_TICKS(100)); // 等待复位完成
|
||||
|
||||
// 3. 配置加速度计: 104Hz ODR, ±2g满量程
|
||||
err = WriteRegister(CTRL1_XL, 0x50); // 0b01010000 - 104Hz ODR, ±2g
|
||||
if (err != ESP_OK) return err;
|
||||
|
||||
// 4. 配置陀螺仪: 104Hz ODR, ±125dps满量程
|
||||
err = WriteRegister(CTRL2_G, 0x50); // 0b01010000
|
||||
if (err != ESP_OK) return err;
|
||||
|
||||
// 5. 配置控制寄存器3: 高电平有效,推挽输出
|
||||
err = WriteRegister(CTRL3_C, 0x44); // 0b01000100
|
||||
if (err != ESP_OK) return err;
|
||||
|
||||
// 6. 禁用陀螺仪低功耗模式
|
||||
err = WriteRegister(CTRL7_G, 0x00); // 禁用低功耗模式
|
||||
if (err != ESP_OK) return err;
|
||||
|
||||
// 7. 配置唤醒功能: 启用X轴的唤醒检测
|
||||
err = WriteRegister(TAP_CFG, 0x81); // 0b10000000 - 仅启用高通滤波器
|
||||
if (err != ESP_OK) return err;
|
||||
|
||||
// 8. 配置唤醒阈值: 非常灵敏的设置
|
||||
err = WriteRegister(WAKE_UP_THS, 0x3F); // 31.25mg - 最灵敏的阈值
|
||||
if (err != ESP_OK) return err;
|
||||
|
||||
// 9. 配置唤醒持续时间: 6个ODR周期
|
||||
err = WriteRegister(WAKE_UP_DUR, 0x06);
|
||||
if (err != ESP_OK) return err;
|
||||
|
||||
// 10. 配置中断1路由: 将唤醒中断路由到INT1
|
||||
err = WriteRegister(MD1_CFG, 0x20); // 0b00100000 - 仅唤醒中断到INT1
|
||||
if (err != ESP_OK) return err;
|
||||
|
||||
// 11. 配置中断2路由: 禁用所有中断
|
||||
err = WriteRegister(MD2_CFG, 0x00);
|
||||
if (err != ESP_OK) return err;
|
||||
|
||||
// 12. 配置INT1_CTRL: 启用唤醒中断到INT1
|
||||
err = WriteRegister(INT1_CTRL, 0x20); // 0b00100000
|
||||
if (err != ESP_OK) return err;
|
||||
|
||||
// 13. 启用嵌入式功能
|
||||
err = WriteRegister(CTRL10_C, 0x3C); // 0b00111000
|
||||
if (err != ESP_OK) return err;
|
||||
|
||||
// 14. 等待配置生效
|
||||
vTaskDelay(pdMS_TO_TICKS(100));
|
||||
|
||||
CreateTimer();
|
||||
|
||||
ESP_LOGI(TAG_GYROSCOPE, "陀螺仪唤醒中断配置完成");
|
||||
|
||||
ESP_LOGI(TAG_GYROSCOPE, "陀螺仪初始化完成");
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 创建陀螺仪读取任务
|
||||
*
|
||||
* 该任务会周期性读取陀螺仪数据并处理中断
|
||||
*/
|
||||
void Gyroscope::CreateGyroReadTask()
|
||||
{
|
||||
if (bus_handle_ == nullptr) {
|
||||
ESP_LOGE(TAG_GYROSCOPE, "I2C总线未初始化,无法启动陀螺仪任务");
|
||||
return;
|
||||
}
|
||||
|
||||
xTaskCreateWithCaps(
|
||||
GyroReadTask,
|
||||
"gyro_read_task",
|
||||
4096,
|
||||
this,
|
||||
5, // Reduced priority to allow other tasks to run
|
||||
&xGyroscopeTaskHandle_,
|
||||
MALLOC_CAP_SPIRAM | MALLOC_CAP_8BIT
|
||||
);
|
||||
|
||||
if (xGyroscopeTaskHandle_ == NULL) {
|
||||
ESP_LOGE(TAG_GYROSCOPE, "Failed to create static gyro read task");
|
||||
} else {
|
||||
ESP_LOGI(TAG_GYROSCOPE, "Static gyro read task created successfully");
|
||||
}
|
||||
}
|
||||
|
||||
esp_err_t Gyroscope::ReadRawAcceleration(int16_t* x, int16_t* y, int16_t* z) {
|
||||
// 1. 参数合法性检查
|
||||
if (x == nullptr || y == nullptr || z == nullptr) {
|
||||
ESP_LOGE(TAG_GYROSCOPE, "参数为空指针");
|
||||
return ESP_ERR_INVALID_ARG;
|
||||
}
|
||||
|
||||
// 2. 读取原始加速度数据（6字节：X_L/X_H/Y_L/Y_H/Z_L/Z_H）
|
||||
uint8_t accel_data_[6] = {0};
|
||||
esp_err_t err = ReadRegisters(ACCEL_X_L, accel_data_, 6);
|
||||
if (err != ESP_OK) {
|
||||
ESP_LOGE(TAG_GYROSCOPE, "读取加速度寄存器失败: 0x%x", err);
|
||||
return err;
|
||||
}
|
||||
|
||||
// 3. 组合高低字节为int16_t（二进制补码，适配传感器输出）
|
||||
int16_t current_raw_x = (int16_t)((accel_data_[1] << 8) | accel_data_[0]);
|
||||
int16_t current_raw_y = (int16_t)((accel_data_[3] << 8) | accel_data_[2]);
|
||||
int16_t current_raw_z = (int16_t)((accel_data_[5] << 8) | accel_data_[4]);
|
||||
|
||||
// 4. 一阶α滤波(整数版，无浮点运算，避免精度丢失)
|
||||
int16_t filtered_x, filtered_y, filtered_z;
|
||||
if (is_first_sample_) {
|
||||
// 首次采样：直接初始化滤波值为原始值
|
||||
filtered_x = current_raw_x;
|
||||
filtered_y = current_raw_y;
|
||||
filtered_z = current_raw_z;
|
||||
is_first_sample_ = false; // 标记首次采样完成
|
||||
} else {
|
||||
// 核心滤波公式（整数版）：α*原始值 + (1-α)*上一次滤波值
|
||||
// WEIGHTFACTOR=0.9; (1-WEIGHTFACTOR)=0.1
|
||||
const int32_t weight_scaled = (int32_t)(WEIGHTFACTOR * 10); // 9
|
||||
const int32_t inv_weight_scaled = 10 - weight_scaled; // 1
|
||||
|
||||
filtered_x = (weight_scaled * (int32_t)current_raw_x +
|
||||
inv_weight_scaled * last_filtered_x_) / 10;
|
||||
filtered_y = (weight_scaled * (int32_t)current_raw_y +
|
||||
inv_weight_scaled * last_filtered_y_) / 10;
|
||||
filtered_z = (weight_scaled * (int32_t)current_raw_z +
|
||||
inv_weight_scaled * last_filtered_z_) / 10;
|
||||
}
|
||||
|
||||
// 5. 转换为int16_t输出（filtered_x/y/z是int32_t，范围仍在int16_t内）
|
||||
*x = (int16_t)filtered_x;
|
||||
*y = (int16_t)filtered_y;
|
||||
*z = (int16_t)filtered_z;
|
||||
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
esp_err_t Gyroscope::GetAcceleration(AccelerationData* data) {
|
||||
if (data == nullptr) {
|
||||
ESP_LOGE(TAG_GYROSCOPE, "数据指针为空");
|
||||
return ESP_ERR_INVALID_ARG;
|
||||
}
|
||||
|
||||
int16_t raw_x, raw_y, raw_z;
|
||||
esp_err_t err = ReadRawAcceleration(&raw_x, &raw_y, &raw_z);
|
||||
if (err != ESP_OK) {
|
||||
return err;
|
||||
}
|
||||
|
||||
// 转换公式：加速度(g) = 原始值 × 灵敏度(mg/LSB) × 10^-3
|
||||
data->x = raw_x * ACCEL_SENSITIVITY_2G / 1000.0f;
|
||||
data->y = raw_y * ACCEL_SENSITIVITY_2G / 1000.0f;
|
||||
data->z = raw_z * ACCEL_SENSITIVITY_2G / 1000.0f;
|
||||
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 陀螺仪读取任务
|
||||
* Y轴立起 Z轴躺下
|
||||
* 该任务会周期性读取陀螺仪数据并处理中断
|
||||
*/
|
||||
void IRAM_ATTR Gyroscope::GyroReadTask(void* pvParameters)
|
||||
{
|
||||
Gyroscope* gyroscope = static_cast<Gyroscope*>(pvParameters);
|
||||
uint32_t interrupt_count = 0;
|
||||
while (1) {
|
||||
if (gyroscope->interrupt_flag_) {
|
||||
interrupt_count++;
|
||||
gyroscope->ClearInterruptFlags();
|
||||
gyroscope->interrupt_flag_ = false;
|
||||
|
||||
// 调用用户注册的回调函数
|
||||
if (gyroscope->on_shake_callback_) {
|
||||
gyroscope->on_shake_callback_();
|
||||
}
|
||||
|
||||
ESP_LOGI(TAG_GYROSCOPE, "累计中断次数: %lu", interrupt_count);
|
||||
if(interrupt_count >= 2){
|
||||
interrupt_count = 0;
|
||||
ESP_LOGI(TAG_GYROSCOPE, "检测到摇晃>>>>>>>>>>");
|
||||
}
|
||||
}
|
||||
vTaskDelay(pdMS_TO_TICKS(50)); // 50ms检查一次,更快的响应
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,214 @@
|
||||
#ifndef GYROSCOPE_H
|
||||
#define GYROSCOPE_H
|
||||
|
||||
#include <esp_log.h>
|
||||
#include <esp_check.h>
|
||||
#include <esp_timer.h>
|
||||
#include <driver/gpio.h>
|
||||
#include "i2c_bus.h"
|
||||
#include "freertos/FreeRTOS.h"
|
||||
#include "freertos/event_groups.h"
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include <freertos/FreeRTOS.h>
|
||||
#include <freertos/task.h>
|
||||
#include <freertos/queue.h>
|
||||
#include <freertos/semphr.h>
|
||||
#include "esp_err.h"
|
||||
#include "config.h"
|
||||
#include "io_expansion_board.h"
|
||||
#include <functional>
|
||||
#include "esp_timer.h"
|
||||
|
||||
// 加速度数据结构体
|
||||
typedef struct {
|
||||
float x; // X轴加速度 (g)
|
||||
float y; // Y轴加速度 (g)
|
||||
float z; // Z轴加速度 (g)
|
||||
} AccelerationData;
|
||||
|
||||
// 一阶滤波系数
|
||||
#define WEIGHTFACTOR 0.93f
|
||||
|
||||
/* 陀螺仪寄存器定义 */
|
||||
#define CTRL1_XL 0x10 // 加速度计控制寄存器1
|
||||
#define CTRL2_G 0x11 // 陀螺仪控制寄存器2
|
||||
#define CTRL3_C 0x12 // 控制寄存器3
|
||||
#define CTRL4_C 0x13 // 控制寄存器4
|
||||
#define CTRL5_C 0x14 // 控制寄存器5
|
||||
#define CTRL6_C 0x15 // 控制寄存器6
|
||||
#define CTRL7_G 0x16 // 陀螺仪控制寄存器7
|
||||
#define CTRL8_XL 0x17 // 加速度计控制寄存器8
|
||||
#define CTRL9_XL 0x18 // 加速度计控制寄存器9
|
||||
#define CTRL10_C 0x19 // 控制寄存器10
|
||||
#define INT1_CTRL 0x0D // 中断1控制寄存器
|
||||
#define WAKE_UP_DUR 0x5B // 唤醒持续时间寄存器
|
||||
#define WAKE_UP_THS 0x5C // 唤醒阈值寄存器
|
||||
#define TAP_CFG 0x58 // 敲击配置寄存器
|
||||
#define TAP_THS_6D 0x59 // 6D方向阈值寄存器
|
||||
#define INT_DUR2 0x5A // 中断持续时间寄存器
|
||||
#define MD1_CFG 0x5E // 中断1配置寄存器
|
||||
#define MD2_CFG 0x5F // 中断2配置寄存器
|
||||
#define FUNC_SRC1 0x53 // 功能源寄存器1
|
||||
#define WAKE_UP_SRC 0x1B // 唤醒源寄存器
|
||||
|
||||
#define ACCEL_X_L 0x28 // OUTX_L_XL
|
||||
#define ACCEL_X_H 0x29 // OUTX_H_XL
|
||||
#define ACCEL_Y_L 0x2A // OUTY_L_XL
|
||||
#define ACCEL_Y_H 0x2B // OUTY_H_XL
|
||||
#define ACCEL_Z_L 0x2C // OUTZ_L_XL
|
||||
#define ACCEL_Z_H 0x2D // OUTZ_H_XL
|
||||
|
||||
#define ACCEL_SENSITIVITY_2G 0.061f // 2g 灵敏度 (g/LSB)
|
||||
|
||||
#define I2C_MASTER_TIMEOUT_MS 1000 // I2C主设备超时时间(毫秒)
|
||||
|
||||
/**
|
||||
* @class Gyroscope
|
||||
* @brief 陀螺仪驱动类,封装芯片操作
|
||||
*/
|
||||
#define TAG_GYROSCOPE "Gyroscope"
|
||||
class Gyroscope
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief 构造函数
|
||||
*/
|
||||
Gyroscope();
|
||||
|
||||
/**
|
||||
* @brief 析构函数
|
||||
*/
|
||||
~Gyroscope();
|
||||
|
||||
/**
|
||||
* @brief 初始化中断引脚
|
||||
* @param gpio_num 中断引脚编号
|
||||
* @return esp_err_t 初始化结果
|
||||
*/
|
||||
esp_err_t InterruptGpioInit(gpio_num_t gpio_num);
|
||||
|
||||
/**
|
||||
* @brief 创建陀螺仪读取任务
|
||||
*/
|
||||
void CreateGyroReadTask();
|
||||
|
||||
/**
|
||||
* @brief 初始化陀螺仪
|
||||
* @return esp_err_t 初始化结果
|
||||
*/
|
||||
esp_err_t Init(i2c_bus_handle_t bus_handle);
|
||||
|
||||
/**
|
||||
* @brief 清除中断标志
|
||||
* @return esp_err_t 清除结果
|
||||
*/
|
||||
esp_err_t ClearInterruptFlags();
|
||||
|
||||
|
||||
/**
|
||||
* @brief 获取当前加速度数据
|
||||
* @param data 指向AccelerationData结构体的指针,用于存储读取到的加速度数据
|
||||
* @return esp_err_t 获取结果
|
||||
*/
|
||||
esp_err_t GetAcceleration(AccelerationData* data);
|
||||
|
||||
/**
|
||||
* @brief 创建500ms周期定时器
|
||||
* @return esp_err_t 创建结果
|
||||
*/
|
||||
esp_err_t CreateTimer();
|
||||
|
||||
/**
|
||||
* @brief 停止并删除定时器
|
||||
*/
|
||||
void DeleteTimer();
|
||||
|
||||
/**
|
||||
* @brief 注册摇晃检测回调函数
|
||||
* @param callback 当检测到摇晃时调用的回调函数
|
||||
*/
|
||||
void OnShake(std::function<void()> callback);
|
||||
|
||||
/**
|
||||
* @brief 注册拿起检测回调函数
|
||||
* @param callback 当检测到设备被拿起时调用的回调函数
|
||||
*/
|
||||
void OnPickUp(std::function<void()> callback);
|
||||
|
||||
/**
|
||||
* @brief 注册放置检测回调函数
|
||||
* @param callback 当检测到设备被放置时调用的回调函数
|
||||
*/
|
||||
void OnPutDown(std::function<void()> callback);
|
||||
|
||||
/**
|
||||
* @brief 注册倾斜检测回调函数
|
||||
* @param callback 当检测到设备倾斜时调用的回调函数
|
||||
*/
|
||||
void OnTilt(std::function<void()> callback);
|
||||
|
||||
i2c_bus_handle_t bus_handle_ = nullptr; // I2C总线句柄
|
||||
volatile uint8_t interrupt_count_ = 0; // 中断计数
|
||||
volatile bool interrupt_flag_ = false; // 中断标志位)(判断是否需要处理)
|
||||
|
||||
private:
|
||||
static void GpioIsrHandler(void* arg); // 中断处理函数
|
||||
static void GyroReadTask(void* pvParameters);
|
||||
static void TimerCallback(void* arg); // 定时器回调函数
|
||||
|
||||
void TimerInterruptHandler(); // 定时器中断处理函数
|
||||
|
||||
esp_err_t WriteRegister(uint8_t reg, uint8_t data);
|
||||
esp_err_t ReadRegister(uint8_t reg, uint8_t* data);
|
||||
esp_err_t ReadRegisters(uint8_t reg, uint8_t* data, size_t length);
|
||||
|
||||
/**
|
||||
* @brief 读取原始加速度数据
|
||||
* @param x 指向X轴加速度存储的指针
|
||||
* @param y 指向Y轴加速度存储的指针
|
||||
* @param z 指向Z轴加速度存储的指针
|
||||
* @return esp_err_t 读取结果
|
||||
*/
|
||||
esp_err_t ReadRawAcceleration(int16_t* x, int16_t* y, int16_t* z);
|
||||
|
||||
std::function<void()> on_shake_callback_; // 摇晃回调函数
|
||||
std::function<void()> on_pickup_callback_; // 拿起回调函数
|
||||
std::function<void()> on_putdown_callback_; // 放置回调函数
|
||||
std::function<void()> on_tilt_callback_; // 倾斜回调函数
|
||||
|
||||
esp_timer_handle_t timer_handle_ = nullptr; // 定时器句柄
|
||||
|
||||
/**
|
||||
* @brief 分配静态任务所需的内存
|
||||
* @return bool 分配是否成功
|
||||
*/
|
||||
bool AllocateStaticTaskMemory();
|
||||
|
||||
/**
|
||||
* @brief 释放静态任务分配的内存
|
||||
*/
|
||||
void FreeStaticTaskMemory();
|
||||
|
||||
TaskHandle_t xGyroscopeTaskHandle_ = nullptr;
|
||||
StackType_t* pGyroscopeTaskStackBuffer_ = nullptr; // 任务栈指针（PSRAM）
|
||||
StaticTask_t* pxGyroscopeTaskBuffer_ = nullptr; // 任务控制块指针（内部RAM）
|
||||
|
||||
// 滤波相关成员变量
|
||||
int16_t last_filtered_x_ = 0; // 上一次滤波后的X轴值
|
||||
int16_t last_filtered_y_ = 0; // 上一次滤波后的Y轴值
|
||||
int16_t last_filtered_z_ = 0; // 上一次滤波后的Z轴值
|
||||
bool is_first_sample_ = true; // 是否为第一次采样标志
|
||||
|
||||
// 状态检测相关成员变量
|
||||
float accel_data_y_last_ = 0.0f; // 保存上一次Y轴数据
|
||||
float total_accel_last_ = 1.0f; // 保存上一次合成加速度值，初始值为1g（平放时的典型值）
|
||||
uint8_t stable_count_ = 0; // 稳定状态计数器
|
||||
uint8_t lift_count_ = 0; // 拿起动作计数器
|
||||
uint8_t putdown_count_ = 0; // 放置动作计数器
|
||||
bool is_stable_ = false; // 是否处于稳定平放状态
|
||||
bool is_lifted_ = false; // 是否处于被拿起状态
|
||||
};
|
||||
|
||||
#endif // GYROSCOPE_H
|
||||
@@ -0,0 +1,16 @@
|
||||
# main/05_executors/gyroscope/module.cmake
|
||||
|
||||
|
||||
# 判断 menuconfig 中的开关
|
||||
if(NOT CONFIG_ENABLE_EXEC_GYROSCOPE AND NOT EARLY_EXPANSION)
|
||||
message(STATUS " [Skip] gyroscope module disabled by menuconfig.")
|
||||
return()
|
||||
endif()
|
||||
|
||||
list(APPEND MODULE_SRCS
|
||||
"05_executors/gyroscope/gyroscope.cc"
|
||||
)
|
||||
|
||||
list(APPEND MODULE_INCLUDE_DIRS
|
||||
"05_executors/gyroscope"
|
||||
)
|
||||
@@ -0,0 +1,202 @@
|
||||
#include "millimeter_wave_radar.h"
|
||||
#include <cstring>
|
||||
#include <freertos/FreeRTOS.h>
|
||||
#include <freertos/task.h>
|
||||
#include <esp_log.h>
|
||||
|
||||
static const char *TAG = "MillimeterWaveRadar";
|
||||
|
||||
bool MillimeterWaveRadar::Init() {
|
||||
uart_config_t uart_config = {
|
||||
.baud_rate = MillimeterWaveRadar::BAUD_RATE,
|
||||
.data_bits = UART_DATA_8_BITS,
|
||||
.parity = UART_PARITY_DISABLE,
|
||||
.stop_bits = UART_STOP_BITS_1,
|
||||
.flow_ctrl = UART_HW_FLOWCTRL_DISABLE,
|
||||
.source_clk = UART_SCLK_DEFAULT,
|
||||
};
|
||||
|
||||
ESP_ERROR_CHECK(uart_param_config(UART_NUM, &uart_config));
|
||||
ESP_ERROR_CHECK(uart_set_pin(UART_NUM, TX_PIN, RX_PIN, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE));
|
||||
ESP_ERROR_CHECK(uart_driver_install(UART_NUM, 256, 0, 0, NULL, 0));
|
||||
|
||||
// 清空缓冲区
|
||||
uart_flush(UART_NUM);
|
||||
return true;
|
||||
}
|
||||
|
||||
uint8_t MillimeterWaveRadar::CalculateChecksum(uint8_t d1, uint8_t d2, uint8_t d3, uint8_t d4) {
|
||||
return d1 ^ d2 ^ d3 ^ d4;
|
||||
}
|
||||
|
||||
// 通过查看最高位是0还是1来判断是否是写命令。但是查询感应模式时最高位不是1（5A 11 00 00 00 11 FE），输入的是0x11，所以仅对此条不适用。
|
||||
bool MillimeterWaveRadar::IsWriteCommand(uint8_t cmd) {
|
||||
if (cmd == 0x11) {
|
||||
return false;
|
||||
}
|
||||
return (cmd & 0x80) == 0x00; // 写命令高位为0
|
||||
}
|
||||
|
||||
bool MillimeterWaveRadar::SendFrame(const uint8_t *frame, size_t len) {
|
||||
int written = uart_write_bytes(UART_NUM, frame, len);
|
||||
uart_wait_tx_done(UART_NUM, pdMS_TO_TICKS(10));
|
||||
return written == (int)len;
|
||||
}
|
||||
|
||||
MillimeterWaveRadar::Result MillimeterWaveRadar::ReceiveFrame(uint8_t *buffer, size_t maxSize, size_t &receivedLen) {
|
||||
TickType_t start = xTaskGetTickCount();
|
||||
size_t total = 0;
|
||||
while ((xTaskGetTickCount() - start) < pdMS_TO_TICKS(TIMEOUT_MS)) {
|
||||
size_t available;
|
||||
uart_get_buffered_data_len(UART_NUM, &available);
|
||||
if (available >= 7) {
|
||||
int read = uart_read_bytes(UART_NUM, buffer, 7, pdMS_TO_TICKS(10));
|
||||
if (read == 7) {
|
||||
receivedLen = 7;
|
||||
// 校验帧头尾
|
||||
if (buffer[0] != 0x5A || buffer[6] != 0xFE) {
|
||||
ESP_LOGW(TAG, "Invalid frame header/tail");
|
||||
return Result::CHECKSUM_ERROR;
|
||||
}
|
||||
// 校验 checksum
|
||||
uint8_t expected = CalculateChecksum(buffer[1], buffer[2], buffer[3], buffer[4]);
|
||||
if (expected != buffer[5]) {
|
||||
ESP_LOGW(TAG, "Checksum mismatch");
|
||||
return Result::CHECKSUM_ERROR;
|
||||
}
|
||||
return Result::OK;
|
||||
}
|
||||
}
|
||||
vTaskDelay(pdMS_TO_TICKS(1));
|
||||
}
|
||||
return Result::TIMEOUT;
|
||||
}
|
||||
|
||||
MillimeterWaveRadar::Result MillimeterWaveRadar::SetParameter(uint8_t paramId, uint32_t value) {
|
||||
if (!IsWriteCommand(paramId)) {
|
||||
return Result::INVALID_PARAM;
|
||||
}
|
||||
|
||||
uint8_t frame[7];
|
||||
frame[0] = 0x5A;
|
||||
frame[1] = paramId;
|
||||
|
||||
switch (paramId) {
|
||||
case SET_RADAR_DISTANCE_THRESHOLD:
|
||||
if (value < 100 || value > 65000) return Result::INVALID_PARAM;
|
||||
frame[2] = 0x00;
|
||||
frame[3] = (value >> 8) & 0xFF;
|
||||
frame[4] = value & 0xFF;
|
||||
ESP_LOGI(TAG, "Setting distance threshold to %d", static_cast<int>(value));
|
||||
break;
|
||||
case SET_RADAR_DELAY_TIME:
|
||||
if (value > 0xFFFFFF) return Result::INVALID_PARAM;
|
||||
frame[2] = (value >> 16) & 0xFF;
|
||||
frame[3] = (value >> 8) & 0xFF;
|
||||
frame[4] = value & 0xFF;
|
||||
ESP_LOGI(TAG, "Setting delay time to %d", static_cast<int>(value));
|
||||
break;
|
||||
case SET_RADAR_LOCKOUT_TIME:
|
||||
if (value > 0xFFFFFF) return Result::INVALID_PARAM;
|
||||
frame[2] = (value >> 16) & 0xFF;
|
||||
frame[3] = (value >> 8) & 0xFF;
|
||||
frame[4] = value & 0xFF;
|
||||
ESP_LOGI(TAG, "Setting lockout time to %d", static_cast<int>(value));
|
||||
break;
|
||||
case SET_RADAR_LIGHT_SENSOR_ENABLE:
|
||||
case SET_RADAR_OUTPUT_LEVEL:
|
||||
case SET_RADAR_POWER_MODE:
|
||||
case SET_RADAR_TRIGGER_MODE:
|
||||
case SET_RADAR_TX_POWER_LEVEL:
|
||||
case SET_RADAR_PWM_ENABLE:
|
||||
case SET_RADAR_PULSE_WIDTH:
|
||||
case SET_RADAR_SENSING_MODE:
|
||||
case SET_RADAR_SENSING_RESULT:
|
||||
if (value > 0xFF) return Result::INVALID_PARAM;
|
||||
frame[2] = 0x00;
|
||||
frame[3] = 0x00;
|
||||
frame[4] = value & 0xFF;
|
||||
break;
|
||||
case SET_RADAR_LIGHT_THRESHOLD:
|
||||
if (value > 0xFF) return Result::INVALID_PARAM;
|
||||
frame[2] = 0x00;
|
||||
frame[3] = 0x00;
|
||||
frame[4] = value & 0xFF;
|
||||
break;
|
||||
case SET_RADAR_PWM_DUTY:
|
||||
if (value > 3500) return Result::INVALID_PARAM;
|
||||
frame[2] = 0x00;
|
||||
frame[3] = (value >> 8) & 0xFF;
|
||||
frame[4] = value & 0xFF;
|
||||
break;
|
||||
default:
|
||||
return Result::INVALID_PARAM;
|
||||
}
|
||||
|
||||
frame[5] = CalculateChecksum(frame[1], frame[2], frame[3], frame[4]);
|
||||
frame[6] = 0xFE;
|
||||
|
||||
if (!SendFrame(frame, 7)) {
|
||||
return Result::UART_ERROR;
|
||||
}
|
||||
|
||||
uint8_t reply[7];
|
||||
size_t len;
|
||||
Result res = ReceiveFrame(reply, sizeof(reply), len);
|
||||
ESP_LOGW(TAG, "Received raw reply frame: %02X %02X %02X %02X %02X %02X %02X",
|
||||
reply[0], reply[1], reply[2], reply[3], reply[4], reply[5], reply[6]);
|
||||
if (res != Result::OK) return res;
|
||||
|
||||
// 发送保存指令
|
||||
uint8_t saveFrame[7] = {0x5A, 0x20, 0x00, 0x00, 0x01, 0x21, 0xFE};
|
||||
SendFrame(saveFrame, 7);
|
||||
// 等待保存回复（协议注释1：约100ms）
|
||||
vTaskDelay(pdMS_TO_TICKS(110)); // 稍微多等一点
|
||||
uart_flush(UART_NUM); // 清除可能的残留
|
||||
|
||||
return Result::OK;
|
||||
}
|
||||
|
||||
void MillimeterWaveRadar::OnRadarDetect(RadarDetectCallback callback) {
|
||||
ESP_LOGI(TAG, "Registering radar detection callback");
|
||||
radar_detect_callback_ = callback;
|
||||
}
|
||||
|
||||
void MillimeterWaveRadar::TriggerRadarDetectCallback() {
|
||||
ESP_LOGI(TAG, "Triggering radar detection callback");
|
||||
if (radar_detect_callback_) {
|
||||
radar_detect_callback_();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
MillimeterWaveRadar::Result MillimeterWaveRadar::QueryParameter(uint8_t paramId, uint32_t &outputValue) {
|
||||
if (IsWriteCommand(paramId)) {
|
||||
ESP_LOGE(TAG, "Attempted to query with a write command ID: 0x%02X", paramId);
|
||||
return Result::INVALID_PARAM;
|
||||
}
|
||||
|
||||
uint8_t frame[7] = {0x5A, paramId, 0x00, 0x00, 0x00, paramId, 0xFE};
|
||||
ESP_LOGI(TAG, "Sending query frame for param 0x%02X: %02X %02X %02X %02X %02X %02X %02X",
|
||||
paramId, frame[0], frame[1], frame[2], frame[3], frame[4], frame[5], frame[6]);
|
||||
|
||||
if (!SendFrame(frame, 7)) {
|
||||
ESP_LOGE(TAG, "UART send failed");
|
||||
return Result::UART_ERROR;
|
||||
}
|
||||
|
||||
uint8_t reply[7];
|
||||
size_t len;
|
||||
Result res = ReceiveFrame(reply, sizeof(reply), len);
|
||||
if (res != Result::OK) {
|
||||
ESP_LOGW(TAG, "Failed to receive reply for param 0x%02X, error code: %d", paramId, static_cast<int>(res));
|
||||
return res;
|
||||
}
|
||||
|
||||
// 打印原始回复帧（新增功能）
|
||||
ESP_LOGW(TAG, "Received raw reply frame: %02X %02X %02X %02X %02X %02X %02X",
|
||||
reply[0], reply[1], reply[2], reply[3], reply[4], reply[5], reply[6]);
|
||||
|
||||
return Result::OK;
|
||||
}
|
||||
@@ -0,0 +1,82 @@
|
||||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
#include <driver/uart.h>
|
||||
#include <functional>
|
||||
|
||||
// 参数名称宏定义（对应协议中的命令）
|
||||
#define SET_RADAR_DISTANCE_THRESHOLD 0x01 // 设置感应距离门限值。Data3、Data4 为感应距离门限值，取值范围为 100 ~ 65000【值越大，感应距离越近】
|
||||
#define SET_RADAR_DELAY_TIME 0x02 // 设置输出延时时间。取值范围为 0X00~ 0XFFFFFF
|
||||
#define SET_RADAR_LIGHT_SENSOR_ENABLE 0x03 // 开关光感。Data4 为 0x01 时开启光感，为 0x00 时关闭光感。
|
||||
#define SET_RADAR_LOCKOUT_TIME 0x04 // 设置封锁时间。取值范围为 0X00~ 0XFFFFFF
|
||||
#define SET_RADAR_OUTPUT_LEVEL 0x05 // 设置有效电平。Data4 为 0x01 时高电平有效，为 0x00 时低电平有效
|
||||
#define SET_RADAR_POWER_MODE 0x06 // 设置功耗模式。Data4 为 0x00 时超低功耗模式（50/60uA），为 0x01 时常规功耗模式（13mA 档位）
|
||||
#define SET_RADAR_SENSING_RESULT 0x07 // 设置触发状态。Data4 为 0x00 时触发状态为未触发感应，为 0x01 时触发状态为已触发感应。
|
||||
#define SET_RADAR_TRIGGER_MODE 0x0A // 设置触发模式。Data4 为 0x00 时触发模式为连续触发，为 0x01 时触发模式为单次触发。
|
||||
#define SET_RADAR_TX_POWER_LEVEL 0x0B // 设置发射功率档位。Data4 取值范围为 0~ 7，总共 8 档发射功率可调，值越大，功率越小，=7 时最小，相当于关闭发射功率。发射功率档位值越小，实际发射功率越大，距离越远，抗干扰性越好。
|
||||
#define SET_RADAR_LIGHT_THRESHOLD 0x0C // 设置光感门限值。取值范围为 0X00~ 0XFFFFFF。
|
||||
#define SET_RADAR_PWM_ENABLE 0x0D // 开关 PWM 功能
|
||||
#define SET_RADAR_PWM_DUTY 0x0E // 设置 PWM 占空比
|
||||
#define SET_RADAR_PULSE_WIDTH 0x0F // 设置供电脉冲宽度
|
||||
#define SET_RADAR_SENSING_MODE 0x10 // 选择感应模式。Data4 为 0x00 时为移动感应模式，为 0x01 时为手扫感应模式。
|
||||
|
||||
// 参数名称宏定义（对应协议中的命令）
|
||||
#define QUERY_RADAR_DISTANCE_THRESHOLD 0x81 // 查询感应距离门限值
|
||||
#define QUERY_RADAR_DELAY_TIME 0x82 // 查询输出延时时间
|
||||
#define QUERY_RADAR_LIGHT_SENSOR_ENABLE 0x83 // 查询开关光感
|
||||
#define QUERY_RADAR_LOCKOUT_TIME 0x84 // 查询封锁时间
|
||||
#define QUERY_RADAR_OUTPUT_LEVEL 0x85 // 查询有效电平
|
||||
#define QUERY_RADAR_POWER_MODE 0x86 // 查询功耗模式
|
||||
#define QUERY_RADAR_SENSING_RESULT 0x87 // 查询触发状态
|
||||
#define QUERY_RADAR_TRIGGER_MODE 0x8A // 查询触发模式
|
||||
#define QUERY_RADAR_TX_POWER_LEVEL 0x8B // 查询发射功率档位
|
||||
#define QUERY_RADAR_LIGHT_THRESHOLD 0x8C // 查询光感门限值
|
||||
#define QUERY_RADAR_PWM_ENABLE 0x8D // 查询开关 PWM 功能
|
||||
#define QUERY_RADAR_PWM_DUTY 0x8E // 查询 PWM 占空比
|
||||
#define QUERY_RADAR_PULSE_WIDTH 0x8F // 查询供电脉冲宽度
|
||||
#define QUERY_RADAR_SENSING_MODE 0x11 // 查询感应模式
|
||||
|
||||
class MillimeterWaveRadar {
|
||||
public:
|
||||
// 雷达检测回调函数类型定义
|
||||
typedef std::function<void(void)> RadarDetectCallback;
|
||||
enum class Result {
|
||||
OK,
|
||||
TIMEOUT,
|
||||
CHECKSUM_ERROR,
|
||||
INVALID_PARAM,
|
||||
UART_ERROR
|
||||
};
|
||||
|
||||
// 初始化 UART（使用 UART1, TX=GPIO39, RX=GPIO40）
|
||||
bool Init();
|
||||
|
||||
// 设置参数（自动保存）
|
||||
Result SetParameter(uint8_t paramId, uint32_t value);
|
||||
|
||||
// 查询参数（返回值通过 outputValue 返回）
|
||||
Result QueryParameter(uint8_t paramId, uint32_t &outputValue);
|
||||
|
||||
Result QueryRawResponse(uint8_t paramId);
|
||||
|
||||
// 注册雷达检测回调函数
|
||||
void OnRadarDetect(RadarDetectCallback callback);
|
||||
|
||||
// 触发雷达检测回调函数（供IO扩展板调用）
|
||||
void TriggerRadarDetectCallback();
|
||||
|
||||
private:
|
||||
static constexpr uart_port_t UART_NUM = UART_NUM_1;
|
||||
static constexpr int TX_PIN = 39;
|
||||
static constexpr int RX_PIN = 40;
|
||||
static constexpr uint32_t BAUD_RATE = 9600;
|
||||
static constexpr uint32_t TIMEOUT_MS = 100;
|
||||
|
||||
// 雷达检测回调函数
|
||||
RadarDetectCallback radar_detect_callback_ = nullptr;
|
||||
|
||||
bool SendFrame(const uint8_t *frame, size_t len);
|
||||
Result ReceiveFrame(uint8_t *buffer, size_t maxSize, size_t &receivedLen);
|
||||
uint8_t CalculateChecksum(uint8_t d1, uint8_t d2, uint8_t d3, uint8_t d4);
|
||||
bool IsWriteCommand(uint8_t cmd);
|
||||
};
|
||||
@@ -0,0 +1,16 @@
|
||||
# main/05_executors/millimeter_wave_radar/module.cmake
|
||||
|
||||
|
||||
# 判断 menuconfig 中的开关
|
||||
if(NOT CONFIG_ENABLE_EXEC_MILLIMETER_WAVE_RADAR AND NOT EARLY_EXPANSION)
|
||||
message(STATUS " [Skip] millimeter_wave_radar module disabled by menuconfig.")
|
||||
return()
|
||||
endif()
|
||||
|
||||
list(APPEND MODULE_SRCS
|
||||
"05_executors/millimeter_wave_radar/millimeter_wave_radar.cc"
|
||||
)
|
||||
|
||||
list(APPEND MODULE_INCLUDE_DIRS
|
||||
"05_executors/millimeter_wave_radar"
|
||||
)
|
||||
@@ -0,0 +1,16 @@
|
||||
# main/05_executors/photo_interrupter/module.cmake
|
||||
|
||||
|
||||
# 判断 menuconfig 中的开关
|
||||
if(NOT CONFIG_ENABLE_EXEC_PHOTO_INTERRUPTER AND NOT EARLY_EXPANSION)
|
||||
message(STATUS " [Skip] photo_interrupter module disabled by menuconfig.")
|
||||
return()
|
||||
endif()
|
||||
|
||||
list(APPEND MODULE_SRCS
|
||||
"05_executors/photo_interrupter/photo_interrupter.cc"
|
||||
)
|
||||
|
||||
list(APPEND MODULE_INCLUDE_DIRS
|
||||
"05_executors/photo_interrupter"
|
||||
)
|
||||
@@ -0,0 +1,129 @@
|
||||
/**
|
||||
* @file photo_interrupter.cc
|
||||
* @brief 光电限位开关类实现文件
|
||||
*
|
||||
* 该类实现了光电限位开关的完整功能,包括初始化、中断处理、
|
||||
* 回调函数管理等。支持起始位置和结束位置两个限位开关的检测。
|
||||
*/
|
||||
|
||||
#include "photo_interrupter.h"
|
||||
#include <esp_log.h>
|
||||
|
||||
#define TAG "PhotoInterrupter" ///< 日志标签,用于ESP-IDF日志系统
|
||||
|
||||
/**
|
||||
* @brief 构造函数
|
||||
*
|
||||
* 初始化PhotoInterrupter对象,设置默认状态
|
||||
* 目前构造函数为空,所有初始化操作在Init()方法中完成
|
||||
*/
|
||||
PhotoInterrupter::PhotoInterrupter()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 析构函数
|
||||
*
|
||||
* 清理资源,释放分配的内存
|
||||
* 目前析构函数为空,没有需要特殊清理的资源
|
||||
*/
|
||||
PhotoInterrupter::~PhotoInterrupter()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 初始化限位开关
|
||||
*
|
||||
* 执行限位开关的初始化操作,包括：
|
||||
* 1. 输出初始化日志
|
||||
* 2. 设置默认的起始限位开关回调函数
|
||||
* 3. 设置默认的结束限位开关回调函数
|
||||
*
|
||||
* 默认回调函数仅输出触发日志,用户可以通过OnStartTrigger()和OnEndTrigger()
|
||||
* 方法覆盖这些默认回调函数
|
||||
*/
|
||||
void PhotoInterrupter::Init()
|
||||
{
|
||||
ESP_LOGI(TAG, "限位开关初始化");
|
||||
|
||||
// 设置默认回调函数 - 起始限位开关触发时输出日志
|
||||
OnStartTrigger([]() {
|
||||
ESP_LOGI(TAG, "起始限位开关触发 - 默认回调");
|
||||
});
|
||||
|
||||
// 设置默认回调函数 - 结束限位开关触发时输出日志
|
||||
OnEndTrigger([]() {
|
||||
ESP_LOGI(TAG, "结束限位开关触发 - 默认回调");
|
||||
});
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 处理限位开关中断
|
||||
*
|
||||
* 当限位开关被触发时调用的中断处理函数
|
||||
* 目前仅输出中断触发日志,可以根据需要扩展更多处理逻辑
|
||||
*/
|
||||
void PhotoInterrupter::Interrupt()
|
||||
{
|
||||
ESP_LOGI(TAG, "限位开关中断触发");
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 设置起始限位开关触发回调函数
|
||||
* @param callback 回调函数对象,当起始限位开关被触发时执行
|
||||
*
|
||||
* 允许用户自定义起始限位开关被触发时的处理逻辑。
|
||||
* 该函数会保存传入的回调函数,并输出注册日志。
|
||||
*/
|
||||
void PhotoInterrupter::OnStartTrigger(std::function<void()> callback)
|
||||
{
|
||||
on_start_trigger_ = callback;
|
||||
ESP_LOGI(TAG, "注册起始限位开关回调函数");
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 设置结束限位开关触发回调函数
|
||||
* @param callback 回调函数对象,当结束限位开关被触发时执行
|
||||
*
|
||||
* 允许用户自定义结束限位开关被触发时的处理逻辑。
|
||||
* 该函数会保存传入的回调函数,并输出注册日志。
|
||||
*/
|
||||
void PhotoInterrupter::OnEndTrigger(std::function<void()> callback)
|
||||
{
|
||||
on_end_trigger_ = callback;
|
||||
ESP_LOGI(TAG, "注册结束限位开关回调函数");
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 触发起始限位开关回调
|
||||
*
|
||||
* 当检测到起始限位开关被触发时,执行注册的回调函数。
|
||||
* 如果回调函数已设置(不为nullptr),则执行回调并输出触发日志。
|
||||
*
|
||||
* 该函数通常由IO扩展板在检测到起始限位开关中断时调用。
|
||||
*/
|
||||
void PhotoInterrupter::TriggerStartCallback()
|
||||
{
|
||||
if (on_start_trigger_) {
|
||||
ESP_LOGI(TAG, "触发起始限位开关回调");
|
||||
on_start_trigger_();
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 触发结束限位开关回调
|
||||
*
|
||||
* 当检测到结束限位开关被触发时,执行注册的回调函数。
|
||||
* 如果回调函数已设置(不为nullptr),则执行回调并输出触发日志。
|
||||
*
|
||||
* 该函数通常由IO扩展板在检测到结束限位开关中断时调用。
|
||||
*/
|
||||
void PhotoInterrupter::TriggerEndCallback()
|
||||
{
|
||||
if (on_end_trigger_) {
|
||||
ESP_LOGI(TAG, "触发结束限位开关回调");
|
||||
on_end_trigger_();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,87 @@
|
||||
/**
|
||||
* @file photo_interrupter.h
|
||||
* @brief 光电限位开关类头文件
|
||||
*
|
||||
* 该类用于处理光电限位开关的触发事件,支持起始位置和结束位置的检测。
|
||||
* 通过回调函数机制,当限位开关被触发时执行相应的处理逻辑。
|
||||
*/
|
||||
|
||||
#ifndef PHOTO_INTERRUPTER_H
|
||||
#define PHOTO_INTERRUPTER_H
|
||||
|
||||
#include "io_expansion_board.h"
|
||||
#include <functional>
|
||||
|
||||
/**
|
||||
* @class PhotoInterrupter
|
||||
* @brief 光电限位开关类
|
||||
*
|
||||
* 提供限位开关的初始化、中断处理和回调函数管理功能。
|
||||
* 支持起始位置和结束位置两个限位开关的检测。
|
||||
*/
|
||||
class PhotoInterrupter {
|
||||
public:
|
||||
/**
|
||||
* @brief 构造函数
|
||||
*
|
||||
* 初始化PhotoInterrupter对象,设置默认状态
|
||||
*/
|
||||
PhotoInterrupter();
|
||||
|
||||
/**
|
||||
* @brief 析构函数
|
||||
*
|
||||
* 清理资源,释放分配的内存
|
||||
*/
|
||||
~PhotoInterrupter();
|
||||
|
||||
/**
|
||||
* @brief 初始化限位开关
|
||||
*
|
||||
* 执行限位开关的初始化操作,包括设置默认回调函数
|
||||
*/
|
||||
void Init();
|
||||
|
||||
/**
|
||||
* @brief 处理限位开关中断
|
||||
*
|
||||
* 当限位开关被触发时调用的中断处理函数
|
||||
*/
|
||||
void Interrupt();
|
||||
|
||||
/**
|
||||
* @brief 设置起始限位开关触发回调函数
|
||||
* @param callback 回调函数对象,当起始限位开关被触发时执行
|
||||
*
|
||||
* 允许用户自定义起始限位开关被触发时的处理逻辑
|
||||
*/
|
||||
void OnStartTrigger(std::function<void()> callback);
|
||||
|
||||
/**
|
||||
* @brief 设置结束限位开关触发回调函数
|
||||
* @param callback 回调函数对象,当结束限位开关被触发时执行
|
||||
*
|
||||
* 允许用户自定义结束限位开关被触发时的处理逻辑
|
||||
*/
|
||||
void OnEndTrigger(std::function<void()> callback);
|
||||
|
||||
/**
|
||||
* @brief 触发起始限位开关回调
|
||||
*
|
||||
* 当检测到起始限位开关被触发时,执行注册的回调函数
|
||||
*/
|
||||
void TriggerStartCallback();
|
||||
|
||||
/**
|
||||
* @brief 触发结束限位开关回调
|
||||
*
|
||||
* 当检测到结束限位开关被触发时,执行注册的回调函数
|
||||
*/
|
||||
void TriggerEndCallback();
|
||||
|
||||
private:
|
||||
std::function<void()> on_start_trigger_; ///< 起始限位开关回调函数
|
||||
std::function<void()> on_end_trigger_; ///< 结束限位开关回调函数
|
||||
};
|
||||
|
||||
#endif // PHOTO_INTERRUPTER_H
|
||||
Reference in new issue
Block a user