写入了发弹检测模块，并为电流环控制
This commit is contained in:
@@ -0,0 +1,239 @@
|
||||
/* =============================================================================
|
||||
* shoot_detect_task.c - 摩擦轮电流/电压波动发射检测实现
|
||||
* 前提条件: 摩擦轮开启且加速稳定后(STATE_ACTIVE)才进行发射检测判定
|
||||
* ========================================================================== */
|
||||
#include "shoot_detect_task.h"
|
||||
#include "gimbal.h"
|
||||
#include "shoot.h"
|
||||
#include "cmsis_os.h"
|
||||
#include "FreeRTOS.h"
|
||||
#include "task.h"
|
||||
#include <math.h>
|
||||
|
||||
static shoot_detect_t g_shoot_detect;
|
||||
|
||||
void shoot_detect_init(void)
|
||||
{
|
||||
g_shoot_detect.state = SHOOT_DETECT_STATE_STOPPED;
|
||||
g_shoot_detect.is_ready = 0;
|
||||
g_shoot_detect.spinup_start_tick = 0;
|
||||
|
||||
g_shoot_detect.shoot_count = 0;
|
||||
g_shoot_detect.fired_pulse = 0;
|
||||
g_shoot_detect.shoot_freq_hz = 0.0f;
|
||||
g_shoot_detect.last_shoot_tick = 0;
|
||||
|
||||
g_shoot_detect.current_l = 0.0f;
|
||||
g_shoot_detect.current_r = 0.0f;
|
||||
g_shoot_detect.baseline_l = 0.0f;
|
||||
g_shoot_detect.baseline_r = 0.0f;
|
||||
g_shoot_detect.delta_l = 0.0f;
|
||||
g_shoot_detect.delta_r = 0.0f;
|
||||
g_shoot_detect.delta_max = 0.0f;
|
||||
|
||||
g_shoot_detect.threshold_a = SHOOT_DETECT_CURRENT_THRESHOLD_A;
|
||||
g_shoot_detect.cooldown_ms = SHOOT_DETECT_COOLDOWN_MS;
|
||||
g_shoot_detect.speed_min_rpm = SHOOT_DETECT_SPEED_MIN_RPM;
|
||||
g_shoot_detect.spinup_timeout_ms = SHOOT_DETECT_SPINUP_TIMEOUT_MS;
|
||||
}
|
||||
|
||||
void shoot_detect_update(void)
|
||||
{
|
||||
uint32_t now = HAL_GetTick();
|
||||
|
||||
/* 检查摩擦轮电机在线状态 */
|
||||
if (motor_friction_l.fdb.last_online_tick == 0 ||
|
||||
motor_friction_r.fdb.last_online_tick == 0 ||
|
||||
(now - motor_friction_l.fdb.last_online_tick > 100) ||
|
||||
(now - motor_friction_r.fdb.last_online_tick > 100))
|
||||
{
|
||||
g_shoot_detect.state = SHOOT_DETECT_STATE_STOPPED;
|
||||
g_shoot_detect.is_ready = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
/* 核心前提: 摩擦轮必须处于启动开启状态 (shoot_friction_flag == 1) */
|
||||
if (!shoot_friction_flag)
|
||||
{
|
||||
/* 摩擦轮未启动: 强制处于 STOPPED 状态, 复位所有检测信号 */
|
||||
g_shoot_detect.state = SHOOT_DETECT_STATE_STOPPED;
|
||||
g_shoot_detect.is_ready = 0;
|
||||
g_shoot_detect.current_l = 0.0f;
|
||||
g_shoot_detect.current_r = 0.0f;
|
||||
g_shoot_detect.delta_l = 0.0f;
|
||||
g_shoot_detect.delta_r = 0.0f;
|
||||
g_shoot_detect.delta_max = 0.0f;
|
||||
g_shoot_detect.baseline_l = 0.0f;
|
||||
g_shoot_detect.baseline_r = 0.0f;
|
||||
return;
|
||||
}
|
||||
|
||||
/* 读取当前两轮绝对电流(A)与实际转速(RPM) */
|
||||
float cur_l = fabsf(motor_friction_l.fdb.torque_current);
|
||||
float cur_r = fabsf(motor_friction_r.fdb.torque_current);
|
||||
float spd_l = fabsf((float)motor_friction_l.fdb.rotor_speed);
|
||||
float spd_r = fabsf((float)motor_friction_r.fdb.rotor_speed);
|
||||
|
||||
g_shoot_detect.current_l = cur_l;
|
||||
g_shoot_detect.current_r = cur_r;
|
||||
|
||||
/* ---------------- 状态机流转 ---------------- */
|
||||
switch (g_shoot_detect.state)
|
||||
{
|
||||
case SHOOT_DETECT_STATE_STOPPED:
|
||||
/* 摩擦轮刚刚开启, 记录起转时刻并进入起转加速过渡期 */
|
||||
g_shoot_detect.spinup_start_tick = now;
|
||||
g_shoot_detect.state = SHOOT_DETECT_STATE_SPINUP;
|
||||
g_shoot_detect.is_ready = 0;
|
||||
break;
|
||||
|
||||
case SHOOT_DETECT_STATE_SPINUP:
|
||||
/* 加速过渡期: 屏蔽电机刚启动加速时的满幅大电流,
|
||||
* 等待起转时间满足(默认 500ms) 且 双轮转速均达到额定工作点(默认 4500 RPM) */
|
||||
if ((now - g_shoot_detect.spinup_start_tick >= g_shoot_detect.spinup_timeout_ms) &&
|
||||
(spd_l >= g_shoot_detect.speed_min_rpm) &&
|
||||
(spd_r >= g_shoot_detect.speed_min_rpm))
|
||||
{
|
||||
/* 捕获稳态旋转初始基线, 正式就绪激活检测 */
|
||||
g_shoot_detect.baseline_l = cur_l;
|
||||
g_shoot_detect.baseline_r = cur_r;
|
||||
g_shoot_detect.state = SHOOT_DETECT_STATE_ACTIVE;
|
||||
g_shoot_detect.is_ready = 1;
|
||||
}
|
||||
break;
|
||||
|
||||
case SHOOT_DETECT_STATE_ACTIVE:
|
||||
/* 正常工作检测期: 检查转速是否异常跌落过低(如严重堵转或掉电) */
|
||||
if (spd_l < (g_shoot_detect.speed_min_rpm - 1000.0f) ||
|
||||
spd_r < (g_shoot_detect.speed_min_rpm - 1000.0f))
|
||||
{
|
||||
/* 转速异常跌落, 退回起转过渡等待重新稳定 */
|
||||
g_shoot_detect.spinup_start_tick = now;
|
||||
g_shoot_detect.state = SHOOT_DETECT_STATE_SPINUP;
|
||||
g_shoot_detect.is_ready = 0;
|
||||
break;
|
||||
}
|
||||
|
||||
/* 计算两轮当前电流相对于空载基线的突变量 ΔI */
|
||||
g_shoot_detect.delta_l = cur_l - g_shoot_detect.baseline_l;
|
||||
g_shoot_detect.delta_r = cur_r - g_shoot_detect.baseline_r;
|
||||
|
||||
float delta_l_valid = (g_shoot_detect.delta_l > 0.0f) ? g_shoot_detect.delta_l : 0.0f;
|
||||
float delta_r_valid = (g_shoot_detect.delta_r > 0.0f) ? g_shoot_detect.delta_r : 0.0f;
|
||||
g_shoot_detect.delta_max = (delta_l_valid > delta_r_valid) ? delta_l_valid : delta_r_valid;
|
||||
|
||||
/* 自适应基线平滑更新:
|
||||
* 仅在电流未出现大波动(处于空载稳态区间)时平滑更新基线,
|
||||
* 避免弹丸过轮脉冲把基线强行拉高 */
|
||||
if (g_shoot_detect.delta_max < g_shoot_detect.threshold_a * 0.6f)
|
||||
{
|
||||
g_shoot_detect.baseline_l += SHOOT_DETECT_BASELINE_ALPHA * (cur_l - g_shoot_detect.baseline_l);
|
||||
g_shoot_detect.baseline_r += SHOOT_DETECT_BASELINE_ALPHA * (cur_r - g_shoot_detect.baseline_r);
|
||||
}
|
||||
|
||||
/* 发射事件波峰判定 */
|
||||
if (g_shoot_detect.delta_max >= g_shoot_detect.threshold_a)
|
||||
{
|
||||
/* 防抖不应期(Cooldown): 单发过轮持续 5~15ms,
|
||||
* 冷却时间内不重复计数, 确保单发只计一次 */
|
||||
if (now - g_shoot_detect.last_shoot_tick >= g_shoot_detect.cooldown_ms)
|
||||
{
|
||||
uint32_t dt = now - g_shoot_detect.last_shoot_tick;
|
||||
if (dt > 0 && dt <= 1000)
|
||||
{
|
||||
g_shoot_detect.shoot_freq_hz = 1000.0f / (float)dt;
|
||||
}
|
||||
else if (dt > 1000)
|
||||
{
|
||||
g_shoot_detect.shoot_freq_hz = 1.0f;
|
||||
}
|
||||
|
||||
g_shoot_detect.last_shoot_tick = now;
|
||||
g_shoot_detect.shoot_count++;
|
||||
g_shoot_detect.fired_pulse = 1;
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t shoot_detect_is_ready(void)
|
||||
{
|
||||
return g_shoot_detect.is_ready;
|
||||
}
|
||||
|
||||
shoot_detect_state_e shoot_detect_get_state(void)
|
||||
{
|
||||
return g_shoot_detect.state;
|
||||
}
|
||||
|
||||
uint8_t shoot_detect_has_fired(void)
|
||||
{
|
||||
if (g_shoot_detect.fired_pulse)
|
||||
{
|
||||
g_shoot_detect.fired_pulse = 0;
|
||||
return 1;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
uint32_t shoot_detect_get_count(void)
|
||||
{
|
||||
return g_shoot_detect.shoot_count;
|
||||
}
|
||||
|
||||
void shoot_detect_clear_count(void)
|
||||
{
|
||||
g_shoot_detect.shoot_count = 0;
|
||||
}
|
||||
|
||||
float shoot_detect_get_freq(void)
|
||||
{
|
||||
uint32_t now = HAL_GetTick();
|
||||
/* 若超过 1s 无新发射, 瞬时射频归零 */
|
||||
if (now - g_shoot_detect.last_shoot_tick > 1000)
|
||||
{
|
||||
g_shoot_detect.shoot_freq_hz = 0.0f;
|
||||
}
|
||||
return g_shoot_detect.shoot_freq_hz;
|
||||
}
|
||||
|
||||
const shoot_detect_t * shoot_detect_get_data(void)
|
||||
{
|
||||
return &g_shoot_detect;
|
||||
}
|
||||
|
||||
void shoot_detect_set_threshold(float threshold_a)
|
||||
{
|
||||
if (threshold_a > 0.1f)
|
||||
{
|
||||
g_shoot_detect.threshold_a = threshold_a;
|
||||
}
|
||||
}
|
||||
|
||||
void shoot_detect_task(void const * argument)
|
||||
{
|
||||
(void)argument;
|
||||
shoot_detect_init();
|
||||
|
||||
extern volatile uint8_t app_ready;
|
||||
while (!app_ready)
|
||||
osDelay(2);
|
||||
|
||||
TickType_t last_wake = xTaskGetTickCount();
|
||||
for (;;)
|
||||
{
|
||||
/* 前提: 若摩擦轮未开启, 低频 10ms 休眠等待, 不占用密集 CPU 计算 */
|
||||
if (!shoot_friction_flag)
|
||||
{
|
||||
shoot_detect_update();
|
||||
osDelay(10);
|
||||
last_wake = xTaskGetTickCount();
|
||||
continue;
|
||||
}
|
||||
|
||||
/* 摩擦轮开启中, 以 2ms 高精度周期进行波动检测 */
|
||||
shoot_detect_update();
|
||||
vTaskDelayUntil(&last_wake, pdMS_TO_TICKS(2));
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user