#include "ptz_header_file.h" #include "Timer.h" #include "tmc2160.h" #include "speed_to_step.h" #include "rotate_step.h" #ifdef PTZ_STEP_MOTOR static BSP_OS_SEM ptz_ftm_mutex;//共享资源锁 //float step_speed_for_hori; //float step_speed_for_vert; //float step_speed_for_hori_last; //float step_speed_for_vert_last; //static float h_angle,h_angle_last; //static float v_angle,v_angle_last; //发送云台实际转速 void ptz_send_speed(char dev, char speed) { unsigned char Speed1[7] = {0xff,0x00,0xd0,0x00,0x00,0x00,0x00}; unsigned char Speed2[7] = {0xff,0x00,0xd0,0x00,0x00,0x00,0x00}; unsigned short int HSpeed = 0; unsigned short int VSpeed = 0; Speed1[1] = g_ptz.address; Speed2[1] = g_ptz.address; HSpeed = (unsigned short int)(g_ptz.hori_speed_actual * 100 + 0.5); Speed1[3] = 0X03;//水平电机 Speed1[4] = (unsigned char)(HSpeed >> 8); Speed1[5] = (unsigned char)(HSpeed & 0x00ff); Speed1[6] = MotorCalPelcoDSUM(Speed1,sizeof(Speed1)); VSpeed = (unsigned short int)(g_ptz.vert_speed_actual * 100 + 0.5); Speed2[3] = 0X04;//垂直电机 Speed2[4] = (unsigned char)(VSpeed >> 8); Speed2[5] = (unsigned char)(VSpeed & 0x00ff); Speed2[6] = MotorCalPelcoDSUM(Speed2,sizeof(Speed2)); switch(speed) { case 1://只查询水平转速 ptz_send_data(dev,Speed1,sizeof(Speed1)); break; case 2://只查询垂直转速 ptz_send_data(dev,Speed2,sizeof(Speed2)); break; default://水平转速垂直转速都查询 ptz_send_data(dev,Speed1,sizeof(Speed1)); ptz_send_data(dev,Speed2,sizeof(Speed2)); break; } } #ifdef TMC2160 extern TMC2160Parameter tmc2160_parameter; static void ptz_hori_step_speed_task() { while(1) { if(g_ptz.hori_start_stop_set == PTZ_HORI_START) { g_ptz.hori_tmc2160.set_f = ptz_hori_step_v_to_f((g_ptz.hori_speed_set * PTZ_HORI_RATIO), g_ptz.hori_tmc2160.microstep_vlue);//g_ptz.hori_speed_set //加速 if(g_ptz.hori_tmc2160.f < g_ptz.hori_tmc2160.set_f) { g_ptz.hori_tmc2160.f = g_ptz.hori_tmc2160.f + ptz_hori_step_v_to_f((PTZ_HORI_MAX_SPEED * PTZ_HORI_RATIO * PTZ_HORI_ACC_DEC_INC_T / (float)tmc2160_parameter.hori_acc_time_max), g_ptz.hori_tmc2160.microstep_vlue); if(g_ptz.hori_tmc2160.f > g_ptz.hori_tmc2160.set_f) { g_ptz.hori_tmc2160.f = g_ptz.hori_tmc2160.set_f; } ptz_hori_timer_start((unsigned int)(g_ptz.hori_tmc2160.f + 0.5)); } //减速 if(g_ptz.hori_tmc2160.f > g_ptz.hori_tmc2160.set_f) { if(g_ptz.hori_dec_mode == PTZ_DEC_DEF) { g_ptz.hori_tmc2160.f = g_ptz.hori_tmc2160.f - ptz_hori_step_v_to_f((PTZ_HORI_MAX_SPEED * PTZ_HORI_RATIO * PTZ_HORI_ACC_DEC_INC_T / (float)PTZ_HORI_DEC_TIME_DEF), g_ptz.hori_tmc2160.microstep_vlue); } else { g_ptz.hori_tmc2160.f = g_ptz.hori_tmc2160.f - ptz_hori_step_v_to_f((PTZ_HORI_MAX_SPEED * PTZ_HORI_RATIO * PTZ_HORI_ACC_DEC_INC_T / (float)tmc2160_parameter.hori_dec_time_max), g_ptz.hori_tmc2160.microstep_vlue); } if(g_ptz.hori_tmc2160.f < g_ptz.hori_tmc2160.set_f) { g_ptz.hori_tmc2160.f = g_ptz.hori_tmc2160.set_f; } ptz_hori_timer_start((unsigned int)(g_ptz.hori_tmc2160.f + 0.5)); } //计算转速 g_ptz.hori_motor_speed_actual = ptz_hori_step_f_to_v(g_ptz.hori_tmc2160.f, g_ptz.hori_tmc2160.microstep_vlue); g_ptz.hori_speed_actual = g_ptz.hori_motor_speed_actual / PTZ_HORI_RATIO; // if(step_speed_for_hori > step_speed_for_hori_last) // { // h_angle = step_speed_for_hori - step_speed_for_hori_last; // }else{ // h_angle = step_speed_for_hori_last - step_speed_for_hori; // } // if(h_angle > (g_ptz.hori_as5047d.as5047d_ptz_angle_max/2.0)) // { // h_angle = h_angle_last; // } // // g_ptz.hori_speed_actual = (((h_angle / PTZ_HORI_ACC_DEC_INC_T) / 360.0) * 60 * 1000)/50.0; // // step_speed_for_hori_last = step_speed_for_hori; // h_angle_last = h_angle; } if(g_ptz.hori_start_stop_set == PTZ_HORI_STOP) { g_ptz.hori_motor_speed_actual = 0; g_ptz.hori_speed_actual = 0; g_ptz.hori_tmc2160.f = 0; } OSTimeDlyHMSM(0u, 0u, 0u, PTZ_HORI_ACC_DEC_INC_T); } } static void ptz_vert_step_speed_task() { while(1) { if(g_ptz.vert_start_stop_set == PTZ_VERT_START) { g_ptz.vert_tmc2160.set_f = ptz_vert_step_v_to_f((g_ptz.vert_speed_set * PTZ_VERT_RATIO), g_ptz.vert_tmc2160.microstep_vlue); //加速 if(g_ptz.vert_tmc2160.f < g_ptz.vert_tmc2160.set_f) { g_ptz.vert_tmc2160.f = g_ptz.vert_tmc2160.f + ptz_vert_step_v_to_f((PTZ_VERT_MAX_SPEED * PTZ_VERT_RATIO * PTZ_VERT_ACC_DEC_INC_T / (float)tmc2160_parameter.vert_acc_time_max), g_ptz.vert_tmc2160.microstep_vlue); if(g_ptz.vert_tmc2160.f > g_ptz.vert_tmc2160.set_f) { g_ptz.vert_tmc2160.f = g_ptz.vert_tmc2160.set_f; } ptz_vert_timer_start((unsigned int)(g_ptz.vert_tmc2160.f + 0.5)); } //减速 if(g_ptz.vert_tmc2160.f > g_ptz.vert_tmc2160.set_f) { if(g_ptz.vert_dec_mode == PTZ_DEC_DEF) { g_ptz.vert_tmc2160.f = g_ptz.vert_tmc2160.f - ptz_vert_step_v_to_f((PTZ_VERT_MAX_SPEED * PTZ_VERT_RATIO * PTZ_VERT_ACC_DEC_INC_T / (float)PTZ_VERT_DEC_TIME_DEF), g_ptz.vert_tmc2160.microstep_vlue); } else { g_ptz.vert_tmc2160.f = g_ptz.vert_tmc2160.f - ptz_vert_step_v_to_f((PTZ_VERT_MAX_SPEED * PTZ_VERT_RATIO * PTZ_VERT_ACC_DEC_INC_T / (float)tmc2160_parameter.vert_dec_time_max), g_ptz.vert_tmc2160.microstep_vlue); } if(g_ptz.vert_tmc2160.f < g_ptz.vert_tmc2160.set_f) { g_ptz.vert_tmc2160.f = g_ptz.vert_tmc2160.set_f; } ptz_vert_timer_start((unsigned int)(g_ptz.vert_tmc2160.f + 0.5)); } //计算转速 g_ptz.vert_motor_speed_actual = ptz_vert_step_f_to_v(g_ptz.vert_tmc2160.f, g_ptz.vert_tmc2160.microstep_vlue); g_ptz.vert_speed_actual = g_ptz.vert_motor_speed_actual / PTZ_VERT_RATIO; // if(step_speed_for_vert > step_speed_for_vert_last) // { // v_angle = step_speed_for_vert - step_speed_for_vert_last; // }else{ // v_angle = step_speed_for_vert_last - step_speed_for_vert; // } // if(v_angle > (g_ptz.vert_as5047d.as5047d_ptz_angle_max/2.0)) // { // v_angle = v_angle_last; // } // // g_ptz.vert_speed_actual = (((v_angle / 40.0) / 360.0) * 60 * 1000)/50.0; // // step_speed_for_vert_last = step_speed_for_vert; // v_angle_last = v_angle; } if(g_ptz.vert_start_stop_set == PTZ_VERT_STOP) { g_ptz.vert_motor_speed_actual = 0; g_ptz.vert_speed_actual = 0; g_ptz.vert_tmc2160.f = 0; } OSTimeDlyHMSM(0u, 0u, 0u, PTZ_VERT_ACC_DEC_INC_T); } } #endif static OS_STK task_hori_step_speed_stk[TASK_HORI_PID_STK_SIZE]; static void creat_task_hori_step_speed_task(void) { CPU_INT08U task_err; CPU_INT08U name_err; task_err = OSTaskCreateExt((void (*)(void *)) ptz_hori_step_speed_task, (void *) 0, (OS_STK *)&task_hori_step_speed_stk[TASK_HORI_PID_STK_SIZE - 1], (INT8U ) TASK_HORI_PID_PRIO, (INT16U ) TASK_HORI_PID_PRIO, (OS_STK *)&task_hori_step_speed_stk[0], (INT32U ) TASK_HORI_PID_STK_SIZE, (void *) 0, (INT16U )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR)); #if (OS_TASK_NAME_EN > 0) OSTaskNameSet(TASK_HORI_PID_PRIO, "ptz_hori_step_speed_task", &name_err); #endif if ((task_err == OS_ERR_NONE) && (name_err == OS_ERR_NONE)) { pdebug(DEBUG_LEVEL_INFO,"create ptz_hori_step_speed_task success...\n\r"); } else { pdebug(DEBUG_LEVEL_FATAL,"create ptz_hori_step_speed_task failed...\n\r"); } } static OS_STK task_vert_step_speed_stk[TASK_VERT_PID_STK_SIZE]; static void creat_task_vert_step_speed_task(void) { CPU_INT08U task_err; CPU_INT08U name_err; task_err = OSTaskCreateExt((void (*)(void *)) ptz_vert_step_speed_task, (void *) 0, (OS_STK *)&task_vert_step_speed_stk[TASK_VERT_PID_STK_SIZE - 1], (INT8U ) TASK_VERT_PID_PRIO, (INT16U ) TASK_VERT_PID_PRIO, (OS_STK *)&task_vert_step_speed_stk[0], (INT32U ) TASK_VERT_PID_STK_SIZE, (void *) 0, (INT16U )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR)); #if (OS_TASK_NAME_EN > 0) OSTaskNameSet(TASK_VERT_PID_PRIO, "ptz_vert_step_speed_task", &name_err); #endif if ((task_err == OS_ERR_NONE) && (name_err == OS_ERR_NONE)) { pdebug(DEBUG_LEVEL_INFO,"create ptz_vert_step_speed_task success...\n\r"); } else { pdebug(DEBUG_LEVEL_FATAL,"create ptz_vert_step_speed_task failed...\n\r"); } } void init_speed_module(void) { BSP_OS_SemCreate(&ptz_ftm_mutex,1u,"ptz_ftm_mutex"); step_time_init(); creat_task_hori_step_speed_task(); creat_task_vert_step_speed_task(); } #endif