240 lines
8.2 KiB
C
240 lines
8.2 KiB
C
/* =============================================================================
|
|
* 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));
|
|
}
|
|
}
|