340 lines
11 KiB
C
340 lines
11 KiB
C
#include "ptz_header_file.h"
|
||
#include "Timer.h"
|
||
#include "tmc2160.h"
|
||
#include "speed_to_step.h"
|
||
#include "rotate_step.h"
|
||
#include "device_interrupt.h"
|
||
|
||
#ifdef PTZ_STEP_MOTOR
|
||
#define DCSTEP_SPEED_LIMIT_HORI 3.3//19.8°以下不使用DCSTEP
|
||
#define DCSTEP_SPEED_LIMIT_VERT 3.3//俯仰
|
||
#define DCSTEP_HORI_EN 1
|
||
#define DCSTEP_VERT_EN 1
|
||
|
||
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 ( GetHoriDcoStatus() )//DC0输出高电平
|
||
{
|
||
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 DCSTEP_HORI_EN
|
||
if ( g_ptz.hori_speed_actual <= DCSTEP_SPEED_LIMIT_HORI )
|
||
{
|
||
gpio_bit_reset(GPIOE, GPIO_PIN_7);//失能水平驱动器DCSTEP
|
||
}
|
||
else
|
||
{
|
||
gpio_bit_set(GPIOE, GPIO_PIN_7);//使能水平驱动器DCSTEP
|
||
}
|
||
#endif
|
||
// 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;
|
||
}
|
||
}
|
||
// else
|
||
// {
|
||
// if(GetDcstepPulseFlag())
|
||
// {
|
||
// ptz_hori_timer_start(4000);//水平DC0长时间未输出有效高电平，使STEP引脚输出一段125us的脉冲
|
||
// SetDcstepPulseFlag();
|
||
// }
|
||
// else
|
||
// {
|
||
// ptz_hori_timer_stop();
|
||
// }
|
||
//
|
||
// }
|
||
OSTimeDlyHMSM(0u, 0u, 0u, PTZ_HORI_ACC_DEC_INC_T);
|
||
|
||
}
|
||
}
|
||
|
||
|
||
static void ptz_vert_step_speed_task()
|
||
{
|
||
|
||
while(1)
|
||
{
|
||
if ( GetVertDcoStatus() )
|
||
{
|
||
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 DCSTEP_VERT_EN
|
||
if ( g_ptz.vert_speed_actual <= DCSTEP_SPEED_LIMIT_VERT )
|
||
{
|
||
gpio_bit_reset(GPIOE, GPIO_PIN_10);//失能垂直驱动器DCSTEP
|
||
}
|
||
else
|
||
{
|
||
gpio_bit_set(GPIOE, GPIO_PIN_10);//使能垂直驱动器DCSTEP
|
||
}
|
||
#endif
|
||
|
||
// 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
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|