| 1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021102210231024102510261027102810291030103110321033103410351036103710381039104010411042104310441045104610471048104910501051105210531054105510561057105810591060106110621063106410651066106710681069107010711072107310741075107610771078107910801081108210831084108510861087108810891090109110921093109410951096109710981099110011011102110311041105110611071108110911101111111211131114111511161117111811191120112111221123112411251126112711281129113011311132113311341135113611371138113911401141114211431144114511461147114811491150115111521153115411551156115711581159116011611162116311641165116611671168116911701171117211731174117511761177117811791180118111821183118411851186118711881189119011911192119311941195119611971198119912001201120212031204120512061207120812091210121112121213121412151216121712181219122012211222122312241225122612271228122912301231123212331234123512361237123812391240124112421243124412451246124712481249125012511252125312541255125612571258125912601261126212631264126512661267126812691270127112721273127412751276127712781279128012811282128312841285128612871288128912901291129212931294129512961297129812991300130113021303130413051306130713081309131013111312131313141315131613171318131913201321132213231324132513261327132813291330133113321333133413351336133713381339134013411342134313441345134613471348134913501351135213531354135513561357135813591360136113621363136413651366136713681369137013711372137313741375137613771378137913801381138213831384138513861387138813891390139113921393139413951396139713981399140014011402140314041405140614071408140914101411141214131414141514161417141814191420142114221423142414251426142714281429143014311432143314341435143614371438143914401441144214431444144514461447144814491450145114521453145414551456145714581459146014611462146314641465146614671468146914701471147214731474147514761477147814791480148114821483148414851486148714881489149014911492149314941495149614971498149915001501150215031504150515061507150815091510151115121513151415151516151715181519152015211522152315241525152615271528152915301531153215331534153515361537153815391540154115421543154415451546154715481549155015511552155315541555155615571558155915601561156215631564156515661567156815691570157115721573157415751576157715781579158015811582158315841585158615871588158915901591159215931594159515961597159815991600160116021603160416051606160716081609161016111612161316141615161616171618161916201621162216231624162516261627162816291630163116321633163416351636163716381639164016411642164316441645164616471648164916501651165216531654165516561657165816591660166116621663166416651666166716681669167016711672167316741675167616771678167916801681168216831684168516861687168816891690169116921693169416951696169716981699170017011702170317041705170617071708170917101711171217131714171517161717171817191720172117221723172417251726172717281729173017311732173317341735173617371738173917401741174217431744174517461747174817491750175117521753175417551756175717581759176017611762176317641765176617671768176917701771177217731774177517761777177817791780178117821783178417851786178717881789179017911792179317941795179617971798179918001801180218031804180518061807180818091810181118121813181418151816181718181819182018211822182318241825182618271828182918301831183218331834183518361837183818391840184118421843184418451846184718481849185018511852185318541855185618571858185918601861186218631864186518661867186818691870187118721873187418751876187718781879188018811882188318841885188618871888188918901891189218931894189518961897189818991900190119021903190419051906190719081909191019111912191319141915191619171918191919201921192219231924192519261927192819291930193119321933193419351936193719381939194019411942194319441945194619471948194919501951195219531954195519561957195819591960196119621963196419651966196719681969197019711972197319741975197619771978197919801981198219831984198519861987198819891990199119921993199419951996199719981999200020012002200320042005200620072008200920102011 |
- #include <control.h>
- /*
- * Copyright (c) 2006-2020, RT-Thread Development Team
- *
- * SPDX-License-Identifier: Apache-2.0
- *
- * Change Logs:
- * Date Author Notes
- * 2020-05-30 Administrator the first version
- */
- #include "beep.h"
- #include "hmi.h"
- #include "stdio.h"
- #include "sound.h"
- #include "dtu_uart2.h"
- #include "sht30_collect.h"
- #include "app_flashdb.h"
- #define DBG_ENABLE
- #define DBG_COLOR
- #define DBG_TAG "control"
- #define DBG_LEVEL DBG_LOG
- #include <rtdbg.h> //must after of DBG_LVL, DBG_TAG or other options
- /* ---- Y轴移动后等待机械振铃衰减的时间(ms) ----
- * 原实现固定等待 1000ms。但振铃量级与本次行程正相关: 对焦细扫只有 10~30 个脉冲的
- * 小步, 机械扰动远小于数百脉冲的粗扫大步, 等满 1s 属于无谓等待。
- * 32点位全程按 865 次采样计, 这一项单独就占 865s(约占总耗时 70%), 改为按行程
- * 自适应后仿真可省约 36% 的墙钟时间, 且不影响精度(精度由细扫插值+消隙回位决定)。
- *
- * 系数需按实测标定: 回位后以 100ms 间隔连采清晰度, 看多久稳定, 取该值*1.5。
- * 若不便标定, 把 AF_SETTLE_MIN_MS 设成 1000 即等价于原行为(安全回退)。 */
- #define AF_SETTLE_MIN_MS 200 /* 小步最短等待(ms), 建议 >= 实测振铃衰减时间 */
- #define AF_SETTLE_MAX_MS 1000 /* 大行程上限(ms), 与原来一致 */
- #define AF_SETTLE_MS_PER_PULSE 30 /* 每脉冲对应的等待时间(ms), 30脉冲->900ms */
- #define AF_SETTLE_MS(dis) \
- ((rt_uint32_t)(dis) * AF_SETTLE_MS_PER_PULSE < AF_SETTLE_MIN_MS \
- ? (rt_uint32_t)AF_SETTLE_MIN_MS \
- : ((rt_uint32_t)(dis) * AF_SETTLE_MS_PER_PULSE > AF_SETTLE_MAX_MS \
- ? (rt_uint32_t)AF_SETTLE_MAX_MS \
- : (rt_uint32_t)(dis) * AF_SETTLE_MS_PER_PULSE))
- //工作检测
- #define WORK_CHECK_THREAD_PRIORITY 23 //线程优先级
- #define WORK_CHECK_THREAD_STACK_SIZE 2024 //栈大小
- #define WORK_CHECK_THREAD_TIMESLICE 5 //时间片
- //工作流程
- #define WORK_LOOP_THREAD_PRIORITY 12 //线程优先级
- #define WORK_LOOP_THREAD_STACK_SIZE 2048 //栈大小
- #define WORK_LOOP_THREAD_TIMESLICE 5 //时间片
- #define PWM_EN 1 //0关闭,1-打开
- //设备状态表结构体
- Device_Status_typedef device_status;
- Device_slide_typedef slide_para;
- Flashdb_typedef flashdb_value;
- static rt_thread_t tid_workloop = RT_NULL;
- static rt_thread_t tid_wkstc = RT_NULL;
- static rt_thread_t rt_slide = RT_NULL;
- static rt_thread_t tid_motor_task = RT_NULL; /* 卷带电机任务线程: 异步执行turnlimit, 避免阻塞串口解析线程 */
- static rt_sem_t motor_task_sem = RT_NULL; /* 卷带请求信号量 */
- static struct rt_mutex turnlimit_mutex; /* turnlimit执行互斥锁(多线程串行化) */
- /***************************************************************************/
- int thread_delete_workloop(void)
- {
- /* 2026-09-28 (P2): ① 删除后必须把句柄置 NULL —— 原实现保留着已释放线程的句柄,
- * 而本函数在升级 / 自清洁等路径会被多次调用, 第二次会删除同一个已释放线程(use-after-delete);
- * ② 检查返回值, 删除失败时保留句柄以便下次重试; ③ 原 result 赋值后从未使用(编译警告)。 */
- LOG_W("thread_delete_workloop.");
- if(tid_workloop!=RT_NULL)
- {
- if (rt_thread_delete(tid_workloop) == RT_EOK) tid_workloop = RT_NULL;
- }
- if(tid_wkstc!=RT_NULL)
- {
- if (rt_thread_delete(tid_wkstc) == RT_EOK) tid_wkstc = RT_NULL;
- }
- if(rt_slide!=RT_NULL)
- {
- if (rt_thread_delete(rt_slide) == RT_EOK) rt_slide = RT_NULL;
- }
- if(tid_motor_task!=RT_NULL)
- {
- if (rt_thread_delete(tid_motor_task) == RT_EOK) tid_motor_task = RT_NULL;
- }
- return 0;
- }
- int thread_restart_workloop(void)
- {
- rt_err_t result;
- result = Work_Loop_Task();
- return result;
- }
- void slidelock();
- //滑台垂直方向开关
- uint8_t ctr_swz(uint8_t status_swz)
- {
- rt_pin_mode(SHAKE_SW, PIN_MODE_OUTPUT);
- if(status_swz == ON){
- rt_pin_write(SHAKE_SW,ON);
- }
- else if(status_swz == OFF){
- rt_pin_write(SHAKE_SW,OFF);
- }
- return status_swz;
- }
- //限位开关判断函数
- void turnlimit(void)
- {
- if(device_status.temp_status==1) return;
- rt_mutex_take(&turnlimit_mutex, RT_WAITING_FOREVER); /* 多线程(电机任务/工作循环/一键自检)互斥执行 */
- //Motor_shake(ON,0);
- Motor_Qingsao_R_PWM(ON);
- static uint16_t turn_timeout = 0;
- rt_pin_mode(LIMIT_DOWN,PIN_MODE_INPUT);
- while(rt_pin_read(LIMIT_DOWN)==0)
- {
- rt_thread_delay(20);
- while(rt_pin_read(LIMIT_DOWN)==0)
- {
- //ctr_cangmen_down(CLOSE);
- rt_thread_delay(50);
- turn_timeout++;
- /* 2026-09-28: 原用 == 比较静态累加计数, 一旦计数被跳过(多路径递增/被改写)
- * 条件永不成立 -> 在持有 turnlimit_mutex(RT_WAITING_FOREVER) 的情况下长阻塞,
- * 后续所有卷带请求永久排队(卷带功能整体失效)。改为 >= 保证必然触顶退出。 */
- if(turn_timeout>=200){
- char *translate = makeJson_Warn("translate","limit timeout");
- DTU_Write("%s",translate);
- rt_free(translate);
- break;
- }
- }
- break;
- }
- while(rt_pin_read(LIMIT_DOWN)==1)
- {
- rt_thread_delay(10);
- while(rt_pin_read(LIMIT_DOWN)==1)
- {
- turn_timeout++;
- rt_thread_delay(5);
- if(turn_timeout>=2000){ /* 2026-09-28: 同 turn_timeout>=200, 用 >= 避免永不成立 */
- char *translate = makeJson_Warn("translate","limit timeout");
- DTU_Write("%s",translate);
- rt_free(translate);
- break;
- }
- }
- break;
- }
- while(rt_pin_read(LIMIT_DOWN)==0)
- {
- rt_thread_delay(20);
- if(rt_pin_read(LIMIT_DOWN)==0)
- {
- //Motor_shake(OFF,0);
- Motor_Qingsao_R_PWM(OFF);
- break;
- }
- else {
- break;
- }
- }
- flashdb_value.tape_used+=60;
- app_flashdb_save("tape_used", &flashdb_value.tape_used, sizeof(flashdb_value.tape_used));
- //Motor_shake(OFF,0);
- Motor_Qingsao_R_PWM(OFF);
- turn_timeout = 0;
- char *translate = makeJson_Warn("translate","done");
- DTU_Write("%s",translate);
- rt_free(translate);
- rt_mutex_release(&turnlimit_mutex);
- // rt_thread_delay(1000);
- }
- uint8_t Replace_Filter(void)
- {
- //释放推杆电机
- ctr_cangmen_up(OPEN);
- //持续5S
- for(uint32_t i=0;i<10;i++){
- rt_thread_delay(1000);
- }
- //滤网传送
- ctr_cangmen_down(OPEN);
- //读取光控ADC值超时3S
- for(uint32_t i=0;i<50;i++)
- {
- rt_thread_delay(50);
- #if 0
- int value = Read_Light_Voltage();
- if(value>=3000) break;
- #endif
- /* 2026-09-28 (P1-10): 原判据写成 i==99, 而循环上限是 49, 永远不成立
- * -> 超时分支是死代码, 滤网更换卡住时平台侧收不到任何提示。改为末次迭代触发。 */
- if(i==49)
- {
- char *translate = makeJson_Warn("Replace_Filter","filter timeout");
- if (translate != RT_NULL) { DTU_Write("%s", translate); rt_free(translate); }
- LOG_W("滤网更换: 等待超时(2.5s), 未检测到预期状态");
- break;
- }
- }
- ctr_cangmen_down(OFF);
- //关闭推杆电机
- ctr_cangmen_up(CLOSE);
- //持续10S
- for(uint32_t i=0;i<10;i++){
- rt_thread_delay(1000);
- }
- ctr_cangmen_up(OFF);
- flashdb_value.filter_used+=30;
- //保存到flashdb
- app_flashdb_save("filter_used", &flashdb_value.filter_used, sizeof(flashdb_value.filter_used));
- /* 2026-09-28: 函数声明为 uint8_t, 原来没有任何 return —— 调用方若使用返回值就是未定义行为 */
- return 0;
- }
- uint32_t mapSpeed(int a1) {
- const int a1Min = 0;
- const int a1Max = 100;
- const int a2Min = 50000; //20khz
- const int a2Max = 200000; //5khz
- // 确保 a1 在有效范围内
- if (a1 < a1Min) {
- a1 = a1Min;
- } else if (a1 > a1Max) {
- a1 = a1Max;
- }
- // 执行线性映射
- uint32_t a2 = a2Max - ((a2Max - a2Min) * a1) / (a1Max - a1Min);
- return a2;
- }
- void stop_move(void)
- {
- slide_para.stop_flag = 1;
- slidelock();
- rt_pin_mode(HK_IO, PIN_MODE_OUTPUT); //PB14 EN
- pwm_stop();
- rt_pin_write(HK_IO, ON);
- }
- //DIR_ 轴选择(0-X,1-Y,2-Z) dir 轴方向
- uint32_t ctr_move(uint8_t DIR_,uint8_t dir_,uint32_t dis_,uint32_t pulse)
- {
- uint32_t j;
- switch(DIR_){
- case X_:
- ctr_lamp(ON,1); //X轴开关:关
- ctr_swz(OFF); //Z轴开关:关
- ctr_Collect_Fan(OFF); //Y轴开关:关
- break;
- case Y_:
- ctr_swz(OFF); //Z轴开关:关
- ctr_lamp(OFF,1); //X轴开关:关
- ctr_Collect_Fan(ON); //Y轴开关:开
- break;
- case Z_:
- ctr_swz(ON); //Y轴开关:关
- ctr_lamp(OFF,1); //X轴开关:关
- ctr_Collect_Fan(OFF); //Y轴开关:关
- break;
- }
- //限位检测初始化
- rt_pin_mode(Limit_Motor_X, PIN_MODE_INPUT);
- rt_pin_mode(Limit_Motor_Y, PIN_MODE_INPUT);
- rt_pin_mode(Limit_Motor_Z, PIN_MODE_INPUT);
- //驱动器初始化
- rt_pin_mode(HK_IO, PIN_MODE_OUTPUT); //PB14 EN
- rt_pin_mode(CTR_RO_LIMIT, PIN_MODE_OUTPUT); //PB15 DIR
- //rt_pin_mode(CTR_RC_LIMIT, PIN_MODE_OUTPUT); //PC6 PWM
- // if(dis_ >=1000){
- // beep_set(500000, 250000); //PWM使能
- // }else{
- // beep_set(2000000, 1000000); //PWM使能
- // }
- //驱动使能
- rt_pin_write(HK_IO, ON);
- //方向选择
- if(dir_ == 1){
- rt_pin_write(CTR_RO_LIMIT, ON);
- }else{
- rt_pin_write(CTR_RO_LIMIT, OFF);
- }
- switch(DIR_){
- case X_:
- ctr_lamp(1, ON); //X轴电源开关:开
- rt_thread_delay(1000);
- //判断限位
- if(dir_== 0 && rt_pin_read(Limit_Motor_X)==1){
- for(int z =0;z < 3; z++){
- Beep_Tip(200);
- LOG_W("X:已在初始位置!");
- }
- return 0;
- }
- rt_pin_write(HK_IO, ON);
- rt_thread_delay(100);
- beep_set(pulse, pulse/3); //PWM使能
- for(j=0;j<dis_;j++){
- if(slide_para.stop_flag == 0){
- rt_thread_delay(10); //10ms
- if(dir_==0&&rt_pin_read(Limit_Motor_X)==1){
- rt_thread_delay(10); //10ms
- if(rt_pin_read(Limit_Motor_X)==1){
- j = dis_;
- }
- }
- }else{ //急停
- for(int z =0;z < 3; z++){
- Beep_Tip(200);
- }
- break;
- }
- }
- break;
- case Y_:
- ctr_Collect_Fan(ON); //Y轴电源开关:开
- //rt_thread_delay(100);
- //判断限位
- if(dir_== 0 && rt_pin_read(Limit_Motor_Y)==1){
- for(int z =0;z < 3; z++){
- Beep_Tip(200);
- LOG_W("Y:已在初始位置!");
- }
- return 0;
- }
- rt_pin_write(HK_IO, ON);
- beep_set(pulse, pulse/2); //PWM使能
- for(j=0;j<dis_;j++){
- if(slide_para.stop_flag == 0){
- if(dis_>=8000){rt_thread_delay(10);}else{rt_thread_delay(1);}
- if(dir_==0&&rt_pin_read(Limit_Motor_Y)==1){
- if(dis_>=8000){rt_thread_delay(10);}else{rt_thread_delay(1);}
- if(rt_pin_read(Limit_Motor_Y)==1){
- j = dis_;
- }
- }
- }else{ //急停
- for(int z =0;z < 3; z++){
- Beep_Tip(200);
- }
- break;
- }
- }
- break;
- case Z_:
- ctr_swz(ON); //Z轴电源开关:开
- rt_thread_delay(1000); //
- //判断限位
- if(dir_== 1 && rt_pin_read(Limit_Motor_Z)==1){
- for(int z =0;z < 3; z++){
- Beep_Tip(200);
- LOG_W("Z:已在初始位置!");
- }
- return 0;
- }
- //驱动使能
- rt_pin_write(HK_IO, ON);
- beep_set(pulse, pulse/3); //PWM使能
- for(j=0;j<dis_;j++){
- if(slide_para.stop_flag == 0){
- rt_thread_delay(10);
- if(dir_==1&&rt_pin_read(Limit_Motor_Z)==1){
- rt_thread_delay(10);
- if(rt_pin_read(Limit_Motor_Z)==1){
- j = dis_;
- }
- }
- }else{ //急停
- for(int z =0;z < 3; z++){
- Beep_Tip(200);
- }
- break;
- }
- }
- break;
- }
- pwm_stop();
- rt_pin_write(HK_IO, ON);
- return dis_;
- }
- void slidelock()
- {
- ctr_lamp(OFF,1); //X轴开关:关
- ctr_swz(OFF); //Z轴开关:关
- ctr_Collect_Fan(OFF); //Y轴开关:关
- }
- /**********************************************************************/
- /*光控*/
- uint8_t In_Light_status(void)
- {
- rt_pin_mode(LIGHT_PIN,PIN_MODE_INPUT);
- return rt_pin_read(LIGHT_PIN);
- }
- /*雨控*/
- uint8_t In_Rain_status(void)
- {
- rt_pin_mode(RAIN_PIN,PIN_MODE_INPUT);
- return rt_pin_read(RAIN_PIN);
- }
- /*加热*/
- uint8_t ctr_Heat(uint8_t status_heat)
- {
- device_status.heat_status = status_heat;
- rt_pin_mode(Heat_Pin, PIN_MODE_OUTPUT);
- rt_pin_write(Heat_Pin,status_heat);
- return status_heat;
- }
- /*制冷*/
- uint8_t ctr_Cold(uint8_t status_cold)
- {
- device_status.cold_status = status_cold;
- rt_pin_mode(Cold_Pin, PIN_MODE_OUTPUT);
- rt_pin_write(Cold_Pin,status_cold);
- return status_cold;
- }
- /*相机供电*/
- void ctr_photo(uint8_t status_photo)
- {
- rt_pin_mode(PHOTO,PIN_MODE_OUTPUT);
- rt_pin_write(PHOTO,status_photo);
- }
- /*************************************************
- Function: ctr_Cool_Fan
- Description: 散热风机的开关
- Calls:
- Table Accessed: // 被访问的表(此项仅对于牵扯到数据库操作的程序)
- Table Updated: // 被修改的表(此项仅对于牵扯到数据库操作的程序)
- Input: ON-开,OFF-关
- Output: NULL
- Return: ON-开,OFF-关
- Others: NULL
- *************************************************/
- uint8_t ctr_Cool_Fan(uint8_t status_cool)
- {
- device_status.cool_fan_status = status_cool;
- rt_pin_mode(Cool_Fan_L, PIN_MODE_OUTPUT);
- rt_pin_mode(Cool_Fan_R, PIN_MODE_OUTPUT);
- if(status_cool==OFF)
- {
- rt_pin_write(Cool_Fan_L,OFF);
- rt_pin_write(Cool_Fan_R,OFF);
- }
- if(status_cool==ON)
- {
- rt_pin_write(Cool_Fan_L,ON);
- rt_pin_write(Cool_Fan_R,OFF);
- }
- return device_status.cool_fan_status;
- }
- /*************************************************
- Function: ctr_Collect_Fan
- Description: 采集风机的开关
- Calls:
- Table Accessed: // 被访问的表(此项仅对于牵扯到数据库操作的程序)
- Table Updated: // 被修改的表(此项仅对于牵扯到数据库操作的程序)
- Input: ON-开,OFF-关
- Output: NULL
- Return: ON-开,OFF-关
- Others: NULL
- *************************************************/
- uint8_t ctr_Collect_Fan(uint8_t status_collect)
- {
- device_status.collect_fan_status = status_collect;
- rt_pin_mode(Collect_Fan, PIN_MODE_OUTPUT);
- if(status_collect==OFF)
- {
- rt_pin_write(Collect_Fan,OFF);
- }
- if(status_collect==ON)
- {
- rt_pin_write(Collect_Fan,ON);
- }
- return device_status.collect_fan_status;
- }
- /*上仓门*/
- uint8_t ctr_cangmen_up(uint8_t status_up)
- {
- rt_pin_mode(Collect_fan_R, PIN_MODE_OUTPUT);
- rt_pin_mode(Collect_fan_L, PIN_MODE_OUTPUT);
- if(status_up==OFF)
- {
- rt_pin_write(Collect_fan_R,ON);
- rt_pin_write(Collect_fan_L,ON);
- device_status.ctr_up_motor = 0;
- rt_thread_delay(1000);
- }
- if(status_up==OPEN)
- {
- device_status.up_status = 1;
- rt_pin_write(Collect_fan_R,OFF);
- rt_pin_write(Collect_fan_L,OFF);
- rt_pin_write(Collect_fan_R,ON);
- rt_pin_write(Collect_fan_L,OFF);
- }
- if(status_up==CLOSE)
- {
- device_status.up_status = 0;
- rt_pin_write(Collect_fan_R,OFF);
- rt_pin_write(Collect_fan_L,OFF);
- rt_pin_write(Collect_fan_R,OFF);
- rt_pin_write(Collect_fan_L,ON);
- }
- return device_status.up_status;
- }
- //下仓门
- uint8_t ctr_cangmen_down(uint8_t status_down)
- {
- rt_pin_mode(C_DOWN_R, PIN_MODE_OUTPUT);
- rt_pin_mode(C_DOWN_L, PIN_MODE_OUTPUT);
- if(status_down==OFF)
- {
- rt_pin_write(C_DOWN_R,ON);
- rt_pin_write(C_DOWN_L,ON);
- device_status.ctr_down_motor = 0;
- rt_thread_delay(1000);
- }
- if(status_down==OPEN)
- {
- device_status.down_status = 1;
- rt_pin_write(C_DOWN_R,OFF);
- rt_pin_write(C_DOWN_L,OFF);
- rt_pin_write(C_DOWN_R,ON);
- rt_pin_write(C_DOWN_L,OFF);
- }
- if(status_down==CLOSE)
- {
- device_status.down_status = 0;
- rt_pin_write(C_DOWN_R,OFF);
- rt_pin_write(C_DOWN_L,OFF);
- rt_pin_write(C_DOWN_R,OFF);
- rt_pin_write(C_DOWN_L,ON);
- }
- return device_status.down_status;
- }
- //镇流器
- uint8_t ctr_lamp(uint8_t status_lamp,uint8_t lamp_num)
- {
- rt_pin_mode(LAMP_SW, PIN_MODE_OUTPUT);
- rt_pin_mode(LAMP_1_SW, PIN_MODE_OUTPUT);
- rt_pin_mode(LAMP_2_SW, PIN_MODE_OUTPUT);
- if(status_lamp==ON)
- {
- switch(lamp_num){
- case 1:
- rt_pin_write(LAMP_SW,ON);
- device_status.lamp_status = status_lamp;
- break;
- case 2:
- rt_pin_write(LAMP_1_SW,ON);
- device_status.lamp_status = status_lamp;
- break;
- case 3:
- rt_pin_write(LAMP_2_SW,ON);
- device_status.lamp_status = status_lamp;
- break;
- default:
- break;
- }
- }
- else {
- switch(lamp_num){
- case 1:
- rt_pin_write(LAMP_SW,OFF);
- device_status.lamp_status = status_lamp;
- break;
- case 2:
- rt_pin_write(LAMP_1_SW,OFF);
- device_status.lamp1_status = status_lamp;
- break;
- case 3:
- rt_pin_write(LAMP_2_SW,OFF);
- device_status.lamp2_status = status_lamp;
- break;
- default:
- break;
- }
- }
- return device_status.lamp_status;
- }
- //补光
- uint8_t ctr_led(uint8_t status_led)
- {
- rt_pin_mode(LED_SW, PIN_MODE_OUTPUT);
- if(status_led==ON)
- {
- rt_pin_write(LED_SW,ON);
- }
- else if(status_led==OFF){
- rt_pin_write(LED_SW,OFF);
- }
- return status_led;
- }
- /*************************************************
- Function: ctr_Petroleum_jelly
- Description: 涂凡士林
- Calls:
- Table Accessed: // 被访问的表(此项仅对于牵扯到数据库操作的程序)
- Table Updated: // 被修改的表(此项仅对于牵扯到数据库操作的程序)
- Input: NULL
- Output: NULL
- Return: NULL
- Others: NULL
- *************************************************/
- void ctr_Petroleum_jelly(rt_uint32_t delay)
- {
- beep_init();
- pwm_on(2,50000);
- rt_thread_delay(delay*1000);
- pwm_off(2,0);
- rt_thread_delay(1000);
- pwm_on(3, 50000);
- rt_thread_delay(delay*1000);
- pwm_off(3,0);
- device_status.fsl_sw = 0;
- }
- //1.0转仓
- void turnzp(void)
- {
- static uint16_t turn_timeout = 0;
- // rt_pin_mode(CLEAR_R, PIN_MODE_OUTPUT);
- // rt_pin_mode(CLEAR_L, PIN_MODE_OUTPUT);
- rt_pin_mode(CLEAR_R_LIMIT,PIN_MODE_INPUT);
- // rt_pin_write(CLEAR_R,OFF);
- // rt_pin_write(CLEAR_L,OFF);
- while(rt_pin_read(CLEAR_R_LIMIT)==0)
- {
- rt_thread_delay(20);
- while(rt_pin_read(CLEAR_R_LIMIT)==0)
- {
- // rt_pin_write(CLEAR_R,ON);
- // rt_pin_write(CLEAR_L,OFF);
- Motor_Juandai(ON);
- rt_thread_delay(50);
- turn_timeout++;
- /* 2026-09-28: 与 turnlimit() 同一类问题, 用 == 比较累加计数;
- * 两个循环共用同一个 turn_timeout, 第一个循环一旦累加到 600,
- * 第二个循环会从 601 起步 —— ==600 永不成立, 该分支永远等不到超时停止。改为 >= */
- if(turn_timeout>=600)
- {
- LOG_E("检测不到限位,电机强制停止");
- char *ctr_zp_limit_warn =makeJson_Warn("电流不足/限位超时","转盘电机");
- if (ctr_zp_limit_warn != RT_NULL) { DTU_Write("%s", ctr_zp_limit_warn); rt_free(ctr_zp_limit_warn); }
- else { LOG_E("makeJson_Warn failed(转盘电机限位超时)"); }
- break;
- }
- }
- break;
- }
- while(rt_pin_read(CLEAR_R_LIMIT)==1)
- {
- rt_thread_delay(20);
- while(rt_pin_read(CLEAR_R_LIMIT)==1)
- {
- // rt_pin_write(CLEAR_R,ON);
- // rt_pin_write(CLEAR_L,OFF);
- Motor_Juandai(ON);
- rt_thread_delay(50);
- turn_timeout++;
- /* 2026-09-28: 与 turnlimit() 同一类问题, 用 == 比较累加计数;
- * 两个循环共用同一个 turn_timeout, 第一个循环一旦累加到 600,
- * 第二个循环会从 601 起步 —— ==600 永不成立, 该分支永远等不到超时停止。改为 >= */
- if(turn_timeout>=600)
- {
- LOG_E("检测不到离开限位,电机或者限位开关异常,电机强制停止");
- char *ctr_zp_limit_warn =makeJson_Warn("检测不到离开限位,电机未启动或者限位开关故障","转盘电机");
- if (ctr_zp_limit_warn != RT_NULL) { DTU_Write("%s", ctr_zp_limit_warn); rt_free(ctr_zp_limit_warn); }
- else { LOG_E("makeJson_Warn failed(转盘电机限位异常)"); }
- break;
- }
- }
- break;
- }
- while(rt_pin_read(CLEAR_R_LIMIT)==0)
- {
- rt_thread_delay(20);
- if(rt_pin_read(CLEAR_R_LIMIT)==0)
- {
- // rt_pin_write(CLEAR_R,OFF);
- // rt_pin_write(CLEAR_L,OFF);
- Motor_Juandai(OFF);
- break;
- }
- else {
- break;
- }
- }
- Motor_Juandai(OFF);
- // rt_pin_write(CLEAR_R,OFF);
- // rt_pin_write(CLEAR_L,OFF);
- turn_timeout = 0;
- rt_thread_delay(1000);
- }
- //路由器供电
- void ctr_router(uint8_t status_router)
- {
- rt_pin_mode(ROUTER, PIN_MODE_OUTPUT);
- //rt_pin_mode(POWER_CLEAR, PIN_MODE_OUTPUT);
- if(status_router==ON)
- {
- rt_pin_write(ROUTER,ON);
- }
- else {
- rt_pin_write(ROUTER,OFF);
- }
- }
- //清扫-推杆电机
- void ctr_TG_Clear(uint16_t clear_timeout)
- {
- #if 0
- int current_clear[200];
- int16_t Electric_current = atoi(device_status.power); //获取电机运动前的电流
- LOG_I("清扫电机待机:%d",Electric_current);
- #endif
- static uint16_t clear_count = 0;
- clear_count = clear_timeout;
- #if PWM_EN == 0
- rt_pin_mode(CLEAR_R, PIN_MODE_OUTPUT);
- rt_pin_mode(CLEAR_L, PIN_MODE_OUTPUT);
- #endif
- while(clear_count!=0)
- {
- #if PWM_EN == 0
- rt_pin_write(CLEAR_R,ON);
- rt_pin_write(CLEAR_L,OFF);
- #else
- Motor_Qingsao_R_PWM(ON);
- Motor_Qingsao_L_PWM(OFF);
- #endif
- clear_count--;
- rt_thread_delay(100);
- }
- #if PWM_EN == 0
- rt_pin_write(CLEAR_R,ON);
- rt_pin_write(CLEAR_L,ON);
- #else
- Motor_Qingsao_R_PWM(OFF);
- Motor_Qingsao_L_PWM(OFF);
- #endif
- rt_thread_delay(3000);
- clear_count = 0;
- while(clear_timeout!=clear_count)
- {
- #if PWM_EN == 0
- rt_pin_write(CLEAR_R,OFF);
- rt_pin_write(CLEAR_L,ON);
- #else
- Motor_Qingsao_R_PWM(OFF);
- Motor_Qingsao_L_PWM(ON);
- #endif
- // if(clear_count<200)
- // current_clear[clear_count] = atoi(device_status.power);
- clear_count++;
- rt_thread_delay(100);
- }
- #if PWM_EN == 0
- rt_pin_write(CLEAR_R,ON);
- rt_pin_write(CLEAR_L,ON);
- #else
- Motor_Qingsao_R_PWM(OFF);
- Motor_Qingsao_L_PWM(OFF);
- #endif
- #if 0
- bubblesort(current_clear,200);
- if(current_clear[199]-Electric_current <100&&device_status.work_status==1&&Electric_current!=0&&Electric_current<5000)
- {
- LOG_E("清扫电机电流不足:%d!",current_clear[199]);
- char *ctr_clear_warn = makeJson_Warn("电流不足","清扫电机");
- DTU_Write("%s",ctr_clear_warn);
- rt_free(ctr_clear_warn);
- }
- #endif
- rt_thread_delay(2000);
- device_status.clear_status = 0;
- }
- //DTU,RTU供电
- void ctr_RTU_DTU(uint8_t status_rtu)
- {
- rt_pin_mode(RTU_DTU, PIN_MODE_OUTPUT);
- if(status_rtu==ON)
- {
- rt_pin_write(RTU_DTU,ON);
- }
- else
- {
- LOG_W("RTU电源复位!!!");
- rt_pin_write(RTU_DTU,OFF);
- }
- }
- //海康拍照
- void Hk_Take_io(void)
- {
- rt_pin_mode(HK_TAKE, PIN_MODE_OUTPUT);
- rt_pin_mode(HK_IO, PIN_MODE_OUTPUT);
- rt_pin_write(HK_TAKE, 1);
- rt_pin_write(HK_IO, 0);
- if(device_status.dtype==3)
- Beep_Tip(1000);
- if(device_status.dtype==7)
- rt_thread_mdelay(1000);
- rt_pin_write(HK_TAKE, 0);
- rt_pin_write(HK_IO, 1);
- }
- //拍照
- void HK_TakePhoto(void)
- {
- //打开补光灯
- DTU_Write("takephoto.mp3");
- rt_thread_delay(10000);
- char *takephotojson = makeJson_takephoto();
- DTU_Write("%s",takephotojson);
- rt_free(takephotojson);
- //吸虫塔
- if(device_status.version_type!=6)
- {
- ctr_led(ON);
- if(device_status.version_type==48)
- {
- rt_thread_delay(1000*60);
- }
- else {
- rt_thread_delay(1000*15);
- }
- }
- //触发海康相机拍照
- Hk_Take_io();
- if(device_status.version_type!=6)
- {
- rt_thread_delay(1000*45);
- }
- else {
- ctr_led(ON);
- rt_thread_delay(1000*19);
- }
- ctr_led(OFF);
- }
- /*上电初始化*/
- void ResetInit(void)
- {
- //打开相机供电
- ctr_photo(ON);
- //开启 工控机供电
- #if BZYV13_EN == 0
- ctr_router(ON);
- #endif
- //关闭风机-2.0/2.1
- #if BZYV13_EN == 0
- ctr_Heat(OFF);
- #else
- ctr_Heat(ON); //工控机-1.3
- #endif
- //关闭制冷
- ctr_Cold(OFF);
- //关闭Y轴驱动电源
- ctr_Collect_Fan(OFF);
- //自清洁推杆打开
- ctr_cangmen_up(CLOSE);
- }
- //工作状态检测
- /*孢子仪多段定时功能函数
- * 循环判断定时的触发
- *
- */
- u_int8_t alarmCheck(u_int8_t starthour,u_int8_t endhour,u_int8_t startmin,u_int8_t endmin)
- {
- u_int8_t workflag = 0;
- if(starthour == endhour) return workflag;
- time_t now;
- time(&now);
- struct tm now_tm = *localtime(&now);
- rt_uint32_t curr_sec = now_tm.tm_hour*3600 + now_tm.tm_min*60 + now_tm.tm_sec;
- rt_uint32_t start_sec = starthour * 3600 + startmin * 60;
- rt_uint32_t end_sec = endhour * 3600 + endmin * 60;
- // 处理开始和结束时间相同的情况
- if (start_sec == end_sec) {
- workflag = 0;
- }
- // 修复跨天判断逻辑
- if (start_sec < end_sec)
- {
- // 不跨天模式
- workflag = (curr_sec >= start_sec && curr_sec < end_sec) ? 1 : 0;
- }
- else
- {
- // 跨天模式
- workflag = (curr_sec >= start_sec || curr_sec < end_sec) ? 1 : 0;
- }
- /* 2026-09-28: 检测周期由 31s 缩短到约 1s 后, 本函数每轮会被调用 3 次,
- * 继续用 LOG_I 会每秒刷 3 条日志(抢占串口控制台), 降为 LOG_D */
- LOG_D("时控模式:状态[%s] 当前时间:%02d:%02d:%02d 设置:%02d:%02d-%02d:%02d",
- workflag ? "工作" : "待机",
- now_tm.tm_hour, now_tm.tm_min, now_tm.tm_sec,
- starthour,startmin, endhour,endmin);
- /* 2026-09-28 (P1-8): 原实现在这里 rt_thread_delay(10000) —— 一个"查询函数"里藏了 10 秒阻塞。
- * 调用方每轮要调 3 次, 导致工作/待机状态判定周期长达 31 秒: 到点不进工作、到点不停
- * 都与它有关。改为纯查询, 周期由调用方的 rt_thread_delay 决定(现约 1 秒)。 */
- return workflag;
- }
- /* 采集段结束处理:卷带电机转一圈 + 发送自动对焦指令
- * 进入时先清零自锁,避免执行期间(turnlimit可能阻塞较久)新的采集段结束事件被覆盖丢失
- * work_done 由工作流程线程独占清零,消除与状态检测线程的双写竞态
- */
- static void exec_work_done(void)
- {
- device_status.work_done = 0;
- LOG_I("采集段结束, 执行转带+自动对焦");
- #if BZYV13_EN == 1
- turnlimit();
- rt_thread_delay(1000);
- turnlimit();
- rt_thread_delay(1000);
- turnlimit();
- //延迟30S等胶带稳定
- for(int i=0;i<30;i++){
- rt_thread_delay(1000);
- LOG_I("等待胶带稳定:%d",i);
- }
- #else
- turnlimit();
- rt_thread_delay(2000);
- #endif
- uint8_t retry_send = 0;
- while(1){
- char *take_start = makeJson_Warn("start", "autofocus");
- if(take_start != RT_NULL) {
- DTU_Write("%s",take_start);
- rt_free(take_start);
- } else {
- LOG_E("makeJson_Warn failed! retry:%d", retry_send);
- }
- rt_thread_delay(5000);
- retry_send++;
- if(retry_send==30||device_status.autotakepic>0){
- LOG_I("send start msg done, retry=%d, autotakepic=%d", retry_send, device_status.autotakepic);
- break;
- }
- }
- }
- void WorkTask_BZYV2(void)
- {
- if(device_status.ds==1&&device_status.temp_status==0){
- /* 等待设备进入工作窗口 */
- while(device_status.work_status ==0){
- /* 待机期间若有未处理的采集段结束动作,立即补做 */
- if(device_status.work_done==1){
- exec_work_done();
- }
- rt_thread_delay(1000);
- }
- /* 处于工作窗口:按对焦状态控制路由器供电 */
- while(device_status.work_status ==1){
- if(device_status.autotakepic==0){
- #if BZYV13_EN == 1
- ctr_router(ON);
- #else
- ctr_Heat(ON);
- #endif
- }else{
- #if BZYV13_EN == 1
- ctr_router(OFF);
- #else
- ctr_Heat(OFF);
- #endif
- }
- if(device_status.temp_status==1){
- device_status.work_status = 0;
- #if BZYV13_EN == 1
- ctr_router(OFF);
- #else
- ctr_Heat(OFF);
- #endif
- break;
- }
- /* 任一采集段窗口结束(下降沿置位 work_done)立即执行转带+对焦 */
- if(device_status.work_done==1){
- exec_work_done();
- }
- rt_thread_delay(1000);
- }
- #if BZYV13_EN == 1
- ctr_router(OFF);
- #else
- ctr_Heat(OFF);
- #endif
- /* 退出工作循环后若有未处理的结束动作(如温控异常跳出), 补做 */
- if(device_status.work_done==1){
- exec_work_done();
- }
- }else{
- LOG_W("==================!!!设备已待机!!!===================");
- }
- }
- /* 入口函数-工作状态检测 */
- static void Work_status_thread_entry(void *parameter)
- {
- /* 记录上一轮各采集段的窗口状态,用于检测窗口结束(ON->OFF下降沿) */
- u_int8_t last_seg_on[3] = {OFF, OFF, OFF};
- static uint32_t takephoto_cnt = 0;
- static uint8_t work_day_checked = 0; // 23点当天只检查一次,非23点复位
- while(1)
- {
- if(device_status.ds==0 || device_status.temp_status==1)
- {
- device_status.work_status = 0;
- /* 设备待机时复位上一轮窗口状态,避免恢复工作后误判下降沿 */
- last_seg_on[0] = OFF;
- last_seg_on[1] = OFF;
- last_seg_on[2] = OFF;
- rt_thread_delay(5000);
- continue;
- }
- /* 依次判断3段采集窗口,任何一段处于窗口内即为工作状态 */
- u_int8_t seg_on[3];
- seg_on[0] = alarmCheck(device_status.collecting_sh1, device_status.collecting_eh1,
- device_status.collt_min_sh1, device_status.collt_min_eh1);
- seg_on[1] = alarmCheck(device_status.collecting_sh2, device_status.collecting_eh2,
- device_status.collt_min_sh2, device_status.collt_min_eh2);
- seg_on[2] = alarmCheck(device_status.collecting_sh3, device_status.collecting_eh3,
- device_status.collt_min_sh3, device_status.collt_min_eh3);
- device_status.work_status = (seg_on[0] || seg_on[1] || seg_on[2]) ? 1 : 0;
- /* 每段窗口结束(ON->OFF)置位一次 work_done,保证每个采集段结束后只触发一次转带+对焦 */
- u_int8_t i;
- for(i = 0; i < 3; i++)
- {
- if(last_seg_on[i] == ON && seg_on[i] == OFF)
- {
- device_status.work_done = 1;
- LOG_I("alarm%d 采集段结束, 触发转带+对焦", i+1);
- }
- last_seg_on[i] = seg_on[i];
- }
- /* 每天23点检查设定的采集时间段是否有效,且总采集时长超过1小时,满足条件则递增 worked_day */
- time_t now;
- time(&now);
- struct tm now_tm = *localtime(&now);
- if (now_tm.tm_hour == 23)
- {
- /* 整个23点整点区间只执行一次,避免每秒循环重复判断 */
- if (!work_day_checked)
- {
- work_day_checked = 1;
- /* 计算3段采集时段的累计有效时长(秒)。
- * 有效性口径与 alarmCheck 保持一致:开始小时==结束小时视为无效时段 */
- uint32_t total_sec = 0;
- u_int8_t seg;
- for (seg = 0; seg < 3; seg++)
- {
- u_int8_t sh, eh, sm, em;
- if (seg == 0)
- {
- sh = device_status.collecting_sh1; eh = device_status.collecting_eh1;
- sm = device_status.collt_min_sh1; em = device_status.collt_min_eh1;
- }
- else if (seg == 1)
- {
- sh = device_status.collecting_sh2; eh = device_status.collecting_eh2;
- sm = device_status.collt_min_sh2; em = device_status.collt_min_eh2;
- }
- else
- {
- sh = device_status.collecting_sh3; eh = device_status.collecting_eh3;
- sm = device_status.collt_min_sh3; em = device_status.collt_min_eh3;
- }
- if (sh != eh) /* 无效时段(开始结束小时相同)不参与累计 */
- {
- uint32_t start_sec = sh * 3600 + sm * 60;
- uint32_t end_sec = eh * 3600 + em * 60;
- if (start_sec < end_sec)
- total_sec += end_sec - start_sec; /* 当天时段 */
- else
- total_sec += 24 * 3600 - start_sec + end_sec; /* 跨天时段 */
- }
- }
- /* 总采集时长超过1小时则递增 worked_day */
- if (total_sec > 3600)
- {
- flashdb_value.worked_day++;
- /* worked_day 是 uint8_t, 必须显式传长度, 否则按指针宽度4字节读写会踩坏 tape_max */
- app_flashdb_save("worked_day", &flashdb_value.worked_day, sizeof(flashdb_value.worked_day));
- LOG_I("采集时段有效且总时长>1h, worked_day=%d", flashdb_value.worked_day);
- if(flashdb_value.worked_day==flashdb_value.freq_filter){
- //清零
- flashdb_value.worked_day = 0;
- app_flashdb_save("worked_day", &flashdb_value.worked_day, sizeof(flashdb_value.worked_day));
- //执行更换
- Replace_Filter();
- //上报预警消息
- }
- }
- }
- }
- else
- {
- work_day_checked = 0; /* 离开23点后复位,第二天可再次检查 */
- }
- rt_thread_delay(1000);
- }
- }
- /*入口函数-工作流程*/
- static void Work_Loop_thread_entry(void *parameter)
- {
- rt_thread_delay(1000);
- ResetInit();
- while(1)
- {
- WorkTask_BZYV2();
- rt_thread_delay(1000);
- }
- }
- /* ==================== 预对焦(Pre-Focus) ====================
- * 背景 —— 为什么要改:
- * 老策略: 复位后盲走到固定坐标 y_init_dis -> 发 takephoto_done -> 工控机开始录制
- * -> MCU 让 Y 轴下行固定行程 -> 发 record_stop -> 由工控机在视频流里挑最清晰帧。
- * Y 轴在整轮 32 个点位之间从不回限位, 每次录制都是"下行 + 回程"的相对往返:
- * 下行由重力/弹簧助力, 回程要反向克服阻力, 开环步进的丢步逐级累加,
- * 于是"走着走着就拍不到清晰位置了"(画面一路偏往 dir=1 一侧)。
- * 为了让固定行程一定罩得住这段漂移, 老策略只能把行程留得很宽(7s 量级)。
- *
- * 新策略(PF_EN=1) —— 录制前先找"清晰度开始上升"的位置:
- * 从当前位置起以 PF_STEP 为步长向下取样清晰度。一旦出现连续上升、且抬升明显超过
- * 本轮底噪, 就认定踩到了焦区的上升沿; 随即往回退 PF_HEADROOM 作为录制起点。
- * 这样随后的下行必然完整经过"上升 -> 峰值 -> 下降", 3 秒行程就够覆盖整个焦区,
- * 而且每个点位都重新锚定一次焦面, 漂移不再跨点位累积。
- * 注意这里只找"上升沿", 不找峰值本身 —— 采样量比完整自动对焦少一个数量级。
- *
- * 失败处理(阶段2, 双向搜索):
- * 阶段1(向下搜索 PF_SEARCH_LIMIT)没找到, 有两种原因 ——
- * a) 下行一开始"从未上升且连续下降": 方向反了, 焦区其实在起始位置上方;
- * b) 走完整段行程才停: 回程丢步把整条轨迹往下带, 起点已越过峰值。
- * 两种都先回退到最开始的位置(进入预对焦时的高度), 再反向(向上)搜索峰值:
- * 上行时清晰度先升后降, 越过峰值后连续回落即确认, 随后上移 PF_HEADROOM 作为录制起点
- * (录制方向是下行, 起点必须在焦区上方)。
- * 反向也失败则再退回到最初位置, 按 slide_set_record_travel(0) 的加长兜底行程录制。
- *
- * 单位口径(改动前必读, 与 ctr_move 的实现强耦合):
- * ctr_move(Y_,dir,dis_,period) 是"定时开PWM": for(j=0;j<dis_;j++){ delay },
- * dis_>=8000 时每格延迟 10ms, dis_<8000 时每格延迟 1ms —— 同一个数值在门槛两侧
- * 行程相差 10 倍。
- * · 预对焦小步: dis_<8000 且 period=PF_PWM_PERIOD_NS(1kHz)
- * => 1 格 = 1ms = 1 个脉冲, 故本段所有 PF_* 行程/步长的单位就是"脉冲"。
- * · 绝对接近 y_init_dis: dis_>=8000 且 period=mapSpeed(50)=125us(8kHz)
- * => 1 格 = 10ms = 80 个脉冲。PF_APPROACH_BACKOFF 用的是这个口径(见 control.h),
- * 且扣减后仍必须 >=8000, 否则掉到 1ms/格档, 同样的数字行程只剩 1/10。
- * ============================================================ */
- #define PF_PWM_PERIOD_NS 1000000UL /* 预对焦PWM周期(ns): 1kHz -> 1 格 = 1 脉冲 */
- #define PF_STEP 60 /* 扫描步长(脉冲): 必须 <= 焦区上升沿宽度的 1/2, 否则整步跨过上升沿漏检 */
- #define PF_SEARCH_LIMIT 600 /* 阶段1: 向下搜索行程(脉冲) */
- #define PF_PHASE2_UP 600 /* 阶段2 反向搜索的基准行程(脉冲), 用于换算 PF_UP_SEARCH_LIMIT */
- #define PF_HEADROOM 60 /* 录制起点相对上升沿再上移的量(脉冲): 保证从上升沿之前起录, 同时吃掉反向间隙 */
- #define PF_RISE_STEP_RATIO 1.05f /* 单步涨幅超过该比例才计一次上升(本机清晰度噪声实测 <1%) */
- #define PF_RISE_CONFIRM 2 /* 连续上升该次数 -> 确认处于上升沿 */
- #define PF_RISE_RATIO 1.30f /* 且当前清晰度须高于本轮底噪该倍数(排除噪声抖动; 用比值故与清晰度绝对量纲无关) */
- #define PF_FALL_GIVEUP_CNT 3 /* 阶段1: 从未上升过且连续下降该次数 -> 判为已身处焦后下降段, 提前换向省时间 */
- #define PF_UP_SEARCH_LIMIT (PF_SEARCH_LIMIT + PF_PHASE2_UP) /* 阶段2 反向(向上)搜索行程(脉冲) */
- #define PF_PEAK_FALL_CNT 2 /* 上行搜索: 越过峰值后连续下降该次数 -> 认为已离开峰值区 */
- #define PF_PEAK_DROP_RATIO 0.90f /* 且当前值较记录到的峰值回落该比例以下才确认(抗噪声) */
- #define PF_FOCUS_TIMEOUT_MS 5000 /* 单次等待 focus 回传的超时(ms) */
- #define PF_FOCUS_RETRY_MAX 3 /* 超时后补发 movy 的次数上限 */
- #define PF_TOTAL_TIMEOUT_MS 120000 /* 预对焦总时间预算(ms): 超时整体判失败走兜底, 防止通信异常时长时间卡死 */
- #define PF_RECORD_MS_OK 5000 /* 预对焦成功: 录制下行行程(ms)。起点已在上升沿前, 无需长行程 */
- #define PF_RECORD_MS_FAIL 7500 /* 预对焦失败兜底: 加大覆盖范围的录制下行行程(ms) */
- #define PF_PREROLL_MS_OK 500 /* 预对焦成功时的预启动行程(ms) */
- #define PF_PREROLL_MS_FAIL 1500 /* 预对焦失败时的预启动行程(ms)(保持旧值) */
- #define PF_RETURN_BIAS_MS 0 /* 回程额外补偿行程(ms): >0 表示回程再往限位方向多走一点。
- * 用于吸收实测的固定单向偏差(如下比上丢步多); 未标定前保持 0 */
- #define PF_MOVE_TIMEOUT_MS 60000 /* 单次 Y 移动派发的等待上限(ms): 兜底防死锁, 超时强制继续 */
- /* 滑台线程句柄: 预对焦需要判断自己是不是在滑台线程里跑(见 pf_move 的注释) */
- static rt_thread_t g_slide_thread = RT_NULL;
- /* Y 轴单次移动(供 case 6 与预对焦共用)。
- * 注意: 预对焦(pf_move)在滑台线程内执行时必须直接调本函数, 不能走"派发 en_slide=6 再等
- * 它清零"的路子 —— 执行 case 6 的就是滑台线程自己, 派发后本线程卡在等待里, 没人去执行,
- * 形成自死锁(现象: 收到 focus 之后整机再无任何输出)。 */
- static uint32_t slide_y_step(uint8_t dir, uint32_t dis, uint32_t pulse)
- {
- uint32_t ret;
- slide_para.ydir = dir;
- slide_para.ydis = dis;
- slide_para.pulse_period = pulse;
- ret = ctr_move(Y_, dir, dis, pulse);
- rt_kprintf("The slide's y movement was done ok\n");
- /* 按本次行程自适应等待振铃衰减(原固定 1000ms)。行程很小(对焦细扫)时无需等满 1s */
- {
- rt_uint32_t settle = AF_SETTLE_MS(dis);
- LOG_D("Y移动后等待 %d ms (行程 %d 脉冲)", settle, dis);
- rt_thread_delay(rt_tick_from_millisecond(settle));
- }
- slide_para.ydis = 0;
- return ret;
- }
- /* 本次录制的行程参数: 由 slide_set_record_travel() 依据预对焦结果设定, case 8 消费 */
- static uint32_t g_record_dis_1 = PF_RECORD_MS_FAIL;
- static uint32_t g_record_dis_0 = PF_RECORD_MS_FAIL;
- static uint32_t g_record_preroll = PF_PREROLL_MS_FAIL;
- /* 预对焦进行中标志: hmi.c 用它把这段时间回传的 focus 帧挡在旧的爬山/校准状态机之外
- * (dtu_uart2.c 每收到一个 focus 就会把 autotakepic 置 2 并给控制线程发信号) */
- volatile uint8_t g_pf_busy = 0;
- /* 请求一次清晰度采样: 发 movy 并等待 focus 回传。
- * 返回 1 = 收到有效清晰度(值在 device_status.focus_value); 0 = 连续超时, 判定通信异常 */
- static uint8_t pf_request_focus(void)
- {
- int retry = 0;
- char utos[8];
- rt_sprintf(utos, "%d", (int)slide_para.ystep);
- do {
- char *photo = makeJson_Warn("movy", utos);
- if (photo != RT_NULL) {
- LOG_D("%s", photo);
- focus_valid_flag = 0; /* 清零后再发送, 避免误用上次残留的 focus */
- DTU_Write("%s", photo);
- rt_free(photo);
- }
- rt_tick_t deadline = rt_tick_get() + rt_tick_from_millisecond(PF_FOCUS_TIMEOUT_MS);
- while ((rt_int32_t)(rt_tick_get() - deadline) < 0) {
- if (focus_valid_flag) {
- focus_valid_flag = 0;
- return 1;
- }
- rt_thread_delay(RT_TICK_PER_SECOND / 10); /* 100ms 轮询一次 */
- }
- retry++;
- LOG_W("[预对焦] movy 未收到 focus 回传, 重传第 %d 次", retry);
- } while (retry < PF_FOCUS_RETRY_MAX);
- return 0;
- }
- /* 相对移动 pulse 个脉冲(dir=1下行 / 0上行)。
- * ctr_move 内部会先切 Y 轴电源、结束后按行程自适应等待振铃衰减。 */
- static void pf_move(uint8_t dir, uint32_t pulse)
- {
- slide_para.stop_flag = 0;
- slide_para.pulse_period = PF_PWM_PERIOD_NS;
- slide_para.ystep = pulse;
- slide_para.ydir = dir;
- slide_para.ydis = pulse;
- /* 关键: 判断当前是不是在滑台线程里。
- * 是(点位切换 case 1/case 2 内调用) -> 只能直接调底层, 派发 en_slide=6 再死等会自死锁;
- * 否(hmi.c 的 Ctr_Mortor 线程在初始化时调用) -> 仍走派发, 保证所有滑台动作串行执行。 */
- if (g_slide_thread != RT_NULL && rt_thread_self() == g_slide_thread) {
- slide_y_step(dir, pulse, PF_PWM_PERIOD_NS);
- return;
- }
- /* 若滑台线程正忙(例如上一次录制还没走完), 先等它空闲 —— en_slide 是单槽的,
- * 直接写会覆盖掉正在执行的动作。 */
- {
- rt_tick_t busy_deadline = rt_tick_get() + rt_tick_from_millisecond(PF_MOVE_TIMEOUT_MS);
- while (slide_para.en_slide != 0) {
- if ((rt_int32_t)(rt_tick_get() - busy_deadline) > 0) {
- LOG_E("[预对焦] 等待滑台空闲超时%dms, 强制改派", PF_MOVE_TIMEOUT_MS);
- break;
- }
- rt_thread_delay(10);
- }
- }
- slide_para.en_slide = 6;
- {
- rt_tick_t deadline = rt_tick_get() + rt_tick_from_millisecond(PF_MOVE_TIMEOUT_MS);
- while (slide_para.en_slide != 0) {
- if ((rt_int32_t)(rt_tick_get() - deadline) > 0) {
- LOG_E("[预对焦] Y移动派发等待超时%dms, 强制继续(防整机卡死)", PF_MOVE_TIMEOUT_MS);
- slide_para.en_slide = OFF;
- break;
- }
- rt_thread_delay(10);
- }
- }
- }
- /* 移动一步并采样清晰度。返回 1 = 取到有效清晰度; 0 = focus 连续超时(通信异常) */
- static uint8_t pf_move_sample(uint8_t dir, uint32_t pulse, float *out)
- {
- pf_move(dir, pulse);
- if (!pf_request_focus()) {
- return 0;
- }
- *out = (float)device_status.focus_value;
- return 1;
- }
- /* 向下搜索"清晰度上升沿"。
- * limit_pulse: 本趟最大搜索行程(脉冲); deadline: 总时间预算;
- * traveled: 出参, 本趟实际下行的脉冲数;
- * pre_rise: 出参, 上升沿之前那一点相对本趟起点的位移(脉冲);
- * comm_ok: 出参, 0 表示 focus 通信链路断了(调用方不应再重试)。
- * 返回 1 = 找到上升沿(Y 停在发现点); 0 = 未找到(Y 停在起点+traveled)。 */
- static uint8_t pf_scan_down(uint32_t limit_pulse, rt_tick_t deadline,
- int *traveled, int *pre_rise, uint8_t *comm_ok,
- uint8_t *fall_giveup)
- {
- float prev = 0.0f, cur = 0.0f, base = 0.0f;
- int pos = 0, rise_cnt = 0, fall_cnt = 0;
- int ever_risen = 0, rise_from = 0;
- *fall_giveup = 0;
- /* 起点基准值: 未移动直接请求一次 */
- if (!pf_request_focus()) {
- *comm_ok = 0;
- *traveled = 0;
- return 0;
- }
- prev = base = (float)device_status.focus_value;
- while ((pos + (int)PF_STEP) <= (int)limit_pulse) {
- if (slide_para.stop_flag != 0) {
- LOG_W("[预对焦] 收到急停, 中断搜索(已下行%d脉冲)", pos);
- break;
- }
- if ((rt_int32_t)(rt_tick_get() - deadline) > 0) {
- LOG_W("[预对焦] 超出总时间预算%dms, 中断搜索(已下行%d脉冲)",
- PF_TOTAL_TIMEOUT_MS, pos);
- break;
- }
- if (!pf_move_sample(1, PF_STEP, &cur)) {
- *comm_ok = 0;
- break;
- }
- pos += (int)PF_STEP;
- if (cur < base) base = cur;
- if (cur > prev * PF_RISE_STEP_RATIO) {
- if (rise_cnt == 0) rise_from = pos - (int)PF_STEP; /* 开始上升之前的那个采样点 */
- rise_cnt++;
- ever_risen = 1;
- fall_cnt = 0;
- if (rise_cnt >= PF_RISE_CONFIRM && base > 0.0f &&
- cur > base * PF_RISE_RATIO) {
- *pre_rise = rise_from;
- *traveled = pos;
- LOG_I("[预对焦] 发现上升沿: 上升前位置=+%d脉冲 清晰度 %.2f->%.2f (底噪 %.2f)",
- rise_from, prev, cur, base);
- return 1;
- }
- } else {
- rise_cnt = 0;
- if (cur < prev) {
- fall_cnt++;
- if (!ever_risen && fall_cnt >= PF_FALL_GIVEUP_CNT) {
- LOG_W("[预对焦] 起点已在焦后下降段(从未上升且连续下滑%d次), 提前结束本趟下行",
- fall_cnt);
- *fall_giveup = 1; /* 告诉调用方: 是"走错方向", 需要反向上搜 */
- break;
- }
- } else {
- fall_cnt = 0;
- }
- }
- prev = cur;
- }
- *traveled = pos;
- return 0;
- }
- /* 阶段2 反向(向上)搜索峰值: 判定"方向反了"时使用 —— 焦区在起始位置上方。
- * 上行过程中清晰度先升后降, 越过峰值后连续回落 PF_PEAK_FALL_CNT 次且跌破峰值的
- * PF_PEAK_DROP_RATIO, 即认为峰值已过; 此时 Y 在峰值上方一点点, 再由调用方上移
- * PF_HEADROOM 作为录制起点(录制方向是下行, 起点必须在焦区上方)。
- * 返回 1 = 找到峰值; 0 = 未找到(通信中断/急停/超时/一路升到行程上限仍不见回落)。
- * *climb: 出参, 本趟实际上行的脉冲数(供失败时回原位);
- * *peak_pos: 出参, 峰值位置(相对本趟起点的上行脉冲)。 */
- static uint8_t pf_scan_up(uint32_t limit_pulse, rt_tick_t deadline,
- int *climb, int *peak_pos, uint8_t *comm_ok)
- {
- float prev = 0.0f, cur = 0.0f, peak = 0.0f;
- int pos = 0, peak_at = 0, fall_cnt = 0;
- if (!pf_request_focus()) {
- *comm_ok = 0;
- *climb = 0;
- *peak_pos = 0;
- return 0;
- }
- peak = prev = (float)device_status.focus_value;
- while ((pos + (int)PF_STEP) <= (int)limit_pulse) {
- if (slide_para.stop_flag != 0) {
- LOG_W("[预对焦] 上行搜索收到急停, 中断(已上行%d脉冲)", pos);
- break;
- }
- if ((rt_int32_t)(rt_tick_get() - deadline) > 0) {
- LOG_W("[预对焦] 上行搜索超出时间预算, 中断(已上行%d脉冲)", pos);
- break;
- }
- if (!pf_move_sample(0, PF_STEP, &cur)) { /* dir=0 = 上行 */
- *comm_ok = 0;
- break;
- }
- pos += (int)PF_STEP;
- if (cur > peak) {
- peak = cur;
- peak_at = pos;
- fall_cnt = 0;
- } else if (cur < prev && peak > 0.0f && cur < peak * PF_PEAK_DROP_RATIO) {
- if (++fall_cnt >= PF_PEAK_FALL_CNT) {
- *climb = pos;
- *peak_pos = peak_at;
- LOG_I("[预对焦] 上行搜索发现峰值: 峰值位置=+%d脉冲(清晰度%.2f), 当前已上行%d脉冲",
- peak_at, peak, pos);
- return 1;
- }
- } else {
- fall_cnt = 0;
- }
- prev = cur;
- }
- *climb = pos;
- *peak_pos = peak_at;
- return 0;
- }
- /* 预对焦主流程: 找到"清晰度上升前"的位置, 并把 Y 轴回退到该位置上方 PF_HEADROOM 处待录。
- * 返回 1 = 成功(Y 已在录制起点); 0 = 失败, 调用方应按 slide_set_record_travel(0) 的兜底行程录制 */
- int slide_pre_focus(void)
- {
- rt_tick_t deadline;
- int traveled = 0, pre_rise = 0;
- int net = 0; /* 相对进入时位置的净下行量(脉冲) */
- uint8_t comm_ok = 1, found = 0;
- uint8_t fall_giveup = 0; /* 阶段1: 是否因"连续下降=方向反了"提前结束 */
- uint32_t saved_period = slide_para.pulse_period; /* 预对焦用小步PWM, 退出时恢复,
- * 否则会污染后续 X/Z 复位的速度档 */
- if (!PF_EN) {
- return 0;
- }
- g_pf_busy = 1;
- deadline = rt_tick_get() + rt_tick_from_millisecond(PF_TOTAL_TIMEOUT_MS);
- LOG_I("[预对焦] 开始: 步长=%d脉冲 阶段1搜索%d脉冲", PF_STEP, PF_SEARCH_LIMIT);
- found = pf_scan_down(PF_SEARCH_LIMIT, deadline, &traveled, &pre_rise, &comm_ok,
- &fall_giveup);
- net += traveled;
- /* 阶段2: 向下没找到 —— 回到最开始的位置, 再反向(向上)搜一次。
- * 触发依据: ① 下行一开始就"从未上升且连续下降"(fall_giveup), 说明方向反了、焦区在起始位置上方;
- * ② 走完整段行程才停(回程丢步把轨迹整体往下带), 也按同样方式回到原点重搜,
- * 避免继续在已经跑偏的方向上白走。 */
- if (!found && comm_ok && slide_para.stop_flag == 0 &&
- (rt_int32_t)(rt_tick_get() - deadline) <= 0) {
- int up_climb = 0, peak_pos = 0;
- /* ① 回到最开始的位置(进入预对焦时的高度) */
- if (net > 0) {
- pf_move(0, (uint32_t)net);
- LOG_W("[预对焦] 阶段1未找到上升沿(%s, 已下行%d脉冲), 回退到起始位置后反向搜索",
- fall_giveup ? "方向反了" : "行程内未见上升", net);
- net = 0;
- }
- /* ② 反向(向上)搜索峰值 */
- LOG_I("[预对焦] 阶段2: 自起始位置向上搜索%d脉冲", PF_UP_SEARCH_LIMIT);
- found = pf_scan_up(PF_UP_SEARCH_LIMIT, deadline, &up_climb, &peak_pos, &comm_ok);
- net -= up_climb; /* 上行 = 净下行量减少 */
- if (found) {
- /* 录制方向是下行, 起点必须再往焦区上方让出 PF_HEADROOM;
- 但不要超出本次上行搜索的行程上限, 免得顶到机械限位 */
- if (PF_HEADROOM > 0 && slide_para.stop_flag == 0 &&
- (up_climb + (int)PF_HEADROOM) <= (int)PF_UP_SEARCH_LIMIT) {
- pf_move(0, PF_HEADROOM);
- net -= (int)PF_HEADROOM;
- }
- LOG_I("[预对焦] 反向搜索成功: 峰值+%d脉冲, 停在峰值上方%d脉冲处等待录制",
- peak_pos, -net);
- slide_para.pulse_period = saved_period;
- g_pf_busy = 0;
- return 1;
- }
- }
- if (!found) {
- LOG_W("[预对焦] 失败(净下行%d脉冲, %s), 回原位后按加长行程兜底录制",
- net, comm_ok ? "未搜到焦区(已双向搜索)" : "focus回传中断");
- /* 关键: 必须退回进入预对焦时的高度再开始录。兜底行程是照着"到位就录"的老行为
- * 标定的, 若不退回, 净下行(反向搜索后也可能是净上行)的部分会叠加在兜底行程上,
- * 有可能超出机械下限。 */
- if (comm_ok) {
- if (net > 0) {
- pf_move(0, (uint32_t)net); /* 曾在起始位置下方: 上行回原位 */
- } else if (net < 0) {
- pf_move(1, (uint32_t)(-net)); /* 曾在起始位置上方: 下行回原位 */
- }
- }
- slide_para.pulse_period = saved_period;
- g_pf_busy = 0;
- return 0;
- }
- /* 回退到"上升沿之前 0"作为录制起点(dir=0 为上行/限位方向) */
- {
- int back = (traveled - pre_rise);
- if (back > 0) {
- pf_move(0, (uint32_t)back);
- LOG_I("[预对焦] 成功: 上退%d脉冲, 停在上升沿前脉冲处等待录制", back);
- }
- }
- slide_para.pulse_period = saved_period;
- g_pf_busy = 0;
- return 1;
- }
- /* 依据预对焦结果设定本次录制的行程 */
- void slide_set_record_travel(uint8_t pf_ok)
- {
- if (pf_ok) {
- g_record_dis_1 = PF_RECORD_MS_OK;
- g_record_dis_0 = PF_RECORD_MS_OK;
- g_record_preroll = PF_PREROLL_MS_OK;
- } else {
- g_record_dis_1 = PF_RECORD_MS_FAIL;
- g_record_dis_0 = PF_RECORD_MS_FAIL;
- g_record_preroll = PF_PREROLL_MS_FAIL;
- }
- LOG_I("[录制] 行程: 预对焦%s -> 预启动%ums 下行%ums 回程%ums",
- pf_ok ? "成功" : "失败", g_record_preroll, g_record_dis_1, g_record_dis_0);
- }
- void slide_thread_entry(void *parameter)
- {
- g_slide_thread = rt_thread_self(); /* 供 pf_move 判断是否处于滑台线程内(避免自死锁) */
- slide_para.pulse_period = 1; //默认滑台的脉宽为1ms
- /* 向下丢步优化(3.7.26): 步率200Hz->133Hz, 物理行程≈1000脉冲不变 */
- const uint32_t record_pulse = 7500000UL; /* 缓慢:低速PWM(周期ns) */
- while(1)
- {
- rt_thread_delay(10);
- switch(slide_para.en_slide){
- case 1:{
- uint8_t en_before = slide_para.en_slide; /* 供预对焦后判断是否被外部改派 */
- ctr_move(X_,slide_para.xdir,slide_para.xdis,slide_para.pulse_period);
- LOG_I("The slide's x movement was done ok");
- #if FOCUS_STEP_EN == 1
- if(slide_para.xdis==55||slide_para.xdis==80)
- {
- DTU_Write("{\"cmd\": \"takephoto\" }");
- device_status.autotakepic = 4;
- }else {
- char *photo = makeJson_Warn("movx","done");
- LOG_I("%s",photo);
- DTU_Write("%s",photo);
- rt_free(photo);
- }
- #else
- if(slide_para.xdis==55||slide_para.xdis==80)
- {
- /* 点位切换(X轴走到新点位, 焦面会随之起伏): 通知工控机录制之前先做一次
- * 轻量预对焦, 重新锚定"清晰度上升沿之前"的位置 —— 既让本次录制只需约
- * 3 秒下行就罩住整个焦区, 也顺带把点位之间"下行+回程"累积的相对误差清零。 */
- uint8_t pf_ok = 0;
- ctr_swz(OFF); //Z轴开关:关
- ctr_lamp(OFF,1); //X轴开关:关
- ctr_Collect_Fan(ON); //Y轴电源开关:开
- rt_thread_delay(200);
- #if PF_EN
- slide_para.stop_flag = 0;
- pf_ok = slide_pre_focus();
- #endif
- slide_set_record_travel(pf_ok);
- char *photo = makeJson_Warn("mov_init_y","takephoto_done");
- DTU_Write("%s",photo);
- rt_free(photo);
- }
- #endif
- slide_para.stop_flag = 0;
- if (slide_para.en_slide == en_before) {
- slide_para.en_slide = OFF;
- } else {
- LOG_W("预对焦期间收到新的滑台派发(en_slide=%d), 保留不清除", slide_para.en_slide);
- }
- break;
- }
- case 2:{
- uint8_t en_before = slide_para.en_slide; /* 供预对焦后判断是否被外部改派 */
- ctr_move(Z_,slide_para.zdir,slide_para.zdis,slide_para.pulse_period);
- LOG_I("The slide's z movement was done ok\n");
- #if FOCUS_STEP_EN == 1
- if(slide_para.zdis==55||slide_para.zdis==80)
- {
- DTU_Write("{\"cmd\": \"takephoto\" }");
- device_status.autotakepic = 4;
- }else {
- char *photo = makeJson_Warn("movz","done");
- LOG_I("%s",photo);
- DTU_Write("%s",photo);
- rt_free(photo);
- }
- #else
- if(slide_para.zdis==55||slide_para.zdis==80)
- {
- /* 同 case 1: 点位切换(Z轴走到新点位)后录制前先预对焦一次 */
- uint8_t pf_ok = 0;
- ctr_swz(OFF); //Z轴开关:关
- ctr_lamp(OFF,1); //X轴开关:关
- ctr_Collect_Fan(ON); //Y轴电源开关:开
- rt_thread_delay(200);
- #if PF_EN
- slide_para.stop_flag = 0;
- pf_ok = slide_pre_focus();
- #endif
- slide_set_record_travel(pf_ok);
- char *photo = makeJson_Warn("mov_init_y","takephoto_done");
- DTU_Write("%s",photo);
- rt_free(photo);
- }
- #endif
- slide_para.zdir = 1;
- slide_para.zdis = 0;
- slide_para.stop_flag = 0;
- if (slide_para.en_slide == en_before) {
- slide_para.en_slide = OFF;
- } else {
- LOG_W("预对焦期间收到新的滑台派发(en_slide=%d), 保留不清除", slide_para.en_slide);
- }
- break;
- }
- case 3:{ //复位X
- LOG_W("X轴复位!");
- slide_para.xdir = 0;
- slide_para.xdis = 8000;
- ctr_move(X_,slide_para.xdir,slide_para.xdis,slide_para.pulse_period);
- LOG_I("The slide's X movement was done ok");
- slide_para.xdir = 0;
- slide_para.xdis = 0;
- slide_para.stop_flag = 0;
- slide_para.en_slide = OFF;
- break;
- }
- case 4:{
- //复位Z
- LOG_W("Z轴复位!");
- slide_para.zdir = 1; //250717
- slide_para.zdis = 5000;
- ctr_move(Z_,slide_para.zdir,slide_para.zdis,slide_para.pulse_period);
- LOG_I("The slide's Z movement was done ok");
- slide_para.zdis = 0;
- slide_para.stop_flag = 0;
- slide_para.en_slide = OFF;
- break;
- }
- case 6:{ //Y轴
- slide_y_step(slide_para.ydir, slide_para.ydis, slide_para.pulse_period);
- #if FOCUS_STEP_EN == 1
- if(1){
- /* 发送movy后等待相机回传focus, 5S超时未收到则重发, 最多重传3次, 避免对焦卡死 */
- int focus_ok = 0;
- int retry = 0;
- do {
- char utos[5];
- rt_sprintf(utos,"%d",slide_para.ystep);
- char *photo = makeJson_Warn("movy",utos);
- LOG_I("%s",photo);
- focus_valid_flag = 0; /* 清零后再发送, 避免误用上次残留的focus */
- DTU_Write("%s",photo);
- rt_free(photo);
- /* 等待相机回传有效focus, 超时5S */
- rt_uint32_t deadline = rt_tick_get() + 5 * RT_TICK_PER_SECOND;
- while ((rt_int32_t)(rt_tick_get() - deadline) < 0) {
- if (focus_valid_flag) {
- focus_valid_flag = 0;
- focus_ok = 1;
- break;
- }
- rt_thread_delay(RT_TICK_PER_SECOND / 10); /* 100ms轮询一次 */
- }
- if (focus_ok) {
- break;
- }
- retry++;
- LOG_W("movy 未收到focus回传, 重传第%d次", retry);
- } while (retry < 3);
- if (!focus_ok) {
- LOG_W("movy 连续4次未收到focus回传, 放弃重传");
- }
- }
- #endif
- slide_para.ydis = 0;
- slide_para.stop_flag = 0;
- slide_para.en_slide = OFF;
- break;
- }
- case 7:{ //Y轴复位
- LOG_W("Y轴复位!");
- slide_para.ydir = 0;
- slide_para.ydis = 8000;
- ctr_move(Y_,slide_para.ydir,slide_para.ydis,slide_para.pulse_period);
- LOG_I("The slide's Y movement was done ok");
- /* Y轴复位后录制行程复位为兜底值; 随后 slide_set_record_travel()
- * 会依据该点位的预对焦结果改写成短行程(成功)或保留加长行程(失败) */
- g_record_dis_1 = PF_RECORD_MS_FAIL;
- g_record_dis_0 = PF_RECORD_MS_FAIL;
- g_record_preroll = PF_PREROLL_MS_FAIL;
- slide_para.stop_flag = 0;
- slide_para.en_slide = OFF;
- break;
- }
- case 8:{ //录制:Y轴缓慢向下移动(行程由预对焦结果决定),完成后发送record_stop,再返回出发位置
- /* 说明: 录制起点已由 slide_pre_focus() 锚定在"清晰度上升沿之前",
- * 因此预对焦成功时只需约 3 秒下行即可完整经过 上升->峰值->下降;
- * 预对焦失败时才退回加长行程盲录 —— 与工控机的录制开始/停止握手保持不变
- * (takephoto_done -> 工控机开录 -> 下发 record_start -> 本分支 -> record_stop)。 */
- // LOG_W("录制:Y轴缓慢向下移动%ums(预启动%ums)!",g_record_dis_1,g_record_preroll);
- // ctr_move(Y_, 1, g_record_preroll, record_pulse); /* 预启动 */
- rt_tick_t tick_down_start = rt_tick_get();
- ctr_move(Y_, 1, g_record_dis_1, record_pulse); /* 缓慢向下移动 */
- LOG_I("向下移动耗时:%u ms",
- (unsigned int)((rt_tick_get() - tick_down_start) * 1000UL / RT_TICK_PER_SECOND));
- /* 执行完成, 通知远端停止录制 */
- char *c0 = makeJsonRpc2_0("record_stop", NULL, 0);
- if(c0 != NULL)
- {
- DTU_Write("%s", c0);
- rt_free(c0);
- }
- rt_thread_delay(500);
- /* Y轴返回出发位置(方向0为初始/限位方向)
- * 【重要修改】原实现这里硬编码 800 作为回程预启动, 而下行预启动是 1500 ——
- * 两个方向不对称, 名义上每个点位净往下走 700 个单位(≈93个脉冲), 32 个点位
- * 累计就是约 3000 个脉冲的单向爬行, 是"走着走着就拍不到清晰位置"的主要来源。
- * 现改为回程与下行共用同一套行程参数, 名义净位移为 0(实际残留差靠预对焦吸收);
- * 若实测仍有固定单向偏差, 用 PF_RETURN_BIAS_MS 做一次性补偿。 */
- //ctr_move(Y_, 0, g_record_preroll, record_pulse);
- if (PF_RETURN_BIAS_MS != 0) {
- ctr_move(Y_, 0, (uint32_t)PF_RETURN_BIAS_MS, record_pulse);
- }
- rt_tick_t tick_up_start = rt_tick_get();
- ctr_move(Y_, 0, g_record_dis_0, record_pulse);
- LOG_I("向上移动耗时:%u ms",
- (unsigned int)((rt_tick_get() - tick_up_start) * 1000UL / RT_TICK_PER_SECOND));
- slide_para.stop_flag = 0;
- slide_para.en_slide = OFF;
- break;
- }
- case 9:{
- //执行32次Y轴缓慢向下移动3S,完成后返回record_stop,再返回出发位置
- int i = 0;
- for(i = 0; i < 32; i++)
- {
- LOG_W("第%d次Y轴缓慢向下移动3S!",i+1);
- /* 向下丢步优化(3.7.26): 步率200Hz->133Hz, 行程保持≈1000脉冲 */
- uint32_t record_pulse = 7500000UL; /* 缓慢:低速PWM(周期ns) */
- uint32_t record_dis = 7500; /* 行程约7.5秒 */
- rt_tick_t tick_down_start = rt_tick_get();
- ctr_move(Y_, 1, record_dis, record_pulse); /* 缓慢向下移动 */
- LOG_I("第%d次 向下移动耗时:%u ms", i+1,
- (unsigned int)((rt_tick_get() - tick_down_start) * 1000UL / RT_TICK_PER_SECOND));
- rt_thread_delay(10000);
- /* Y轴返回出发位置(方向0为初始/限位方向) */
- rt_tick_t tick_up_start = rt_tick_get();
- ctr_move(Y_, 0, record_dis, record_pulse);
- LOG_I("第%d次 向上移动耗时:%u ms", i+1,
- (unsigned int)((rt_tick_get() - tick_up_start) * 1000UL / RT_TICK_PER_SECOND));
- }
- slide_para.stop_flag = 0;
- slide_para.en_slide = OFF;
- break;
- }
- default:
- {
- slide_para.en_slide = OFF;
- slide_para.stop_flag = 0;
- break;
- }
- }
- }
- }
- /* 创建线程 -工作状态检测*/
- int Workstatus_check(void)
- {
- /* 创建线程*/
- tid_wkstc = rt_thread_create("w_s_c_t",
- Work_status_thread_entry, RT_NULL,
- WORK_CHECK_THREAD_STACK_SIZE, //1024
- WORK_CHECK_THREAD_PRIORITY, //23
- WORK_CHECK_THREAD_TIMESLICE); //5
- if(tid_wkstc!=RT_NULL)
- {
- rt_thread_startup(tid_wkstc);
- }
- return 0;
- }
- /*工作流程线程*/
- int Work_Loop_Task(void)
- {
- rt_err_t result_loop;
- /* 创建线程*/
- tid_workloop = rt_thread_create("work_loop_thread",
- Work_Loop_thread_entry, RT_NULL,
- WORK_LOOP_THREAD_STACK_SIZE, //2048
- WORK_LOOP_THREAD_PRIORITY, //12
- WORK_LOOP_THREAD_TIMESLICE); //5
- #if Color_EN == 0
- if(tid_workloop!=RT_NULL)
- {
- result_loop = rt_thread_startup(tid_workloop);
- }
- #endif
- return result_loop;
- }
- int slide_Fresh(void)
- {
- rt_err_t ret = RT_EOK;
- rt_slide = rt_thread_create("rt_slide",
- slide_thread_entry,
- RT_NULL,
- 1024,
- 5,
- 20);
- if (rt_slide != RT_NULL)
- {
- rt_thread_startup(rt_slide);
- rt_kprintf("the thread of rt_slide start ok\n");
- }
- else
- {
- ret = RT_ERROR;
- }
- return ret;
- }
- /* 卷带电机任务线程: turnlimit最长会阻塞约10s(限位轮询),
- * 将其从串口解析线程(t_uart2)中剥离, 由本线程异步执行, 保证串口JSON解析不被电机动作卡死 */
- static void Motor_task_thread_entry(void *parameter)
- {
- while (1)
- {
- rt_sem_take(motor_task_sem, RT_WAITING_FOREVER);
- #if BZYV13_EN == 1
- turnlimit();
- rt_thread_delay(1000);
- turnlimit();
- rt_thread_delay(1000);
- turnlimit();
- rt_thread_delay(1000);
- #else
- turnlimit();
- rt_thread_delay(1000);
- #endif
- }
- }
- /* 非阻塞请求: 卷带电机旋转一次(供串口解析线程等短线程调用, 立即返回) */
- void motor_turnlimit_req(void)
- {
- if(motor_task_sem != RT_NULL)
- {
- rt_sem_release(motor_task_sem);
- }
- }
- /* 创建卷带电机任务线程(优先级10, 低于串口解析线程6, 保证串口优先响应) */
- int Motor_Task_Init(void)
- {
- motor_task_sem = rt_sem_create("motor_sem", 0, RT_IPC_FLAG_FIFO);
- if(motor_task_sem == RT_NULL)
- {
- return -1;
- }
- rt_mutex_init(&turnlimit_mutex, "turn_mutex", RT_IPC_FLAG_PRIO);
- tid_motor_task = rt_thread_create("motor_task",
- Motor_task_thread_entry, RT_NULL,
- 2048, 10, 10);
- if(tid_motor_task != RT_NULL)
- {
- rt_thread_startup(tid_motor_task);
- return 0;
- }
- return -1;
- }
- void reboot(void)
- {
- LOG_W("手动触发重启!");
- rt_hw_cpu_reset();
- }
- // ------------------------------
- // RT-Thread 控制台命令实现
- // ------------------------------
- void fan_cmd(int argc, char *argv[])
- {
- if (argc == 1)
- {
- // 只输入 fan → 打印当前状态
- rt_kprintf("Collect Fan status: %s\n",
- device_status.collect_fan_status ? "ON" : "OFF");
- return;
- }
- if (argc == 2)
- {
- if (!rt_strcmp(argv[1], "on"))
- {
- ctr_Collect_Fan(ON);
- rt_kprintf("[FAN] Turn ON\n");
- }
- else if (!rt_strcmp(argv[1], "off"))
- {
- ctr_Collect_Fan(OFF);
- rt_kprintf("[FAN] Turn OFF\n");
- }
- else
- {
- rt_kprintf("Usage: fan [on|off]\n");
- }
- }
- }
- // 导出到控制台命令
- MSH_CMD_EXPORT(fan_cmd, fan control: fan on | fan off | fan);
- MSH_CMD_EXPORT(ctr_lamp,ctr_lamp<ON|OFF>);
- MSH_CMD_EXPORT(ctr_led,sample:ctr_led <ON|OFF>);
- MSH_CMD_EXPORT(reboot,reboot);
- //INIT_APP_EXPORT(Control_heat);
|