pengsx
/
servoMotor
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

修改宏定义

This commit is contained in:
起床就犯困 2025-10-11 11:40:39 +08:00
parent f03154b2f7
commit bcaf00ed4f
26 changed files with 1662 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
.vscode/settings.json vendored Normal file
View File

@ -0,0 +1,6 @@
{
"files.associations": {
"service_error_count.h": "c",
"ptz_header_file.h": "c"
}
}

7
APP/Common/comm_cfginfo.h
View File

@ -156,9 +156,10 @@ extern "C" {
#define HYT_ADDRESS "hyt_address="
#define HYT_ADDRESS_DEFAULT "1"
#define ERROR_compensation "compensation_switch="
#ifdef PTZ_BLDC_MOTOR
#define ERROR_compensation_switch "on"//误差补偿开关,直流无刷电机原则上不使用误差补偿，而使用重复定位
#endif
// #ifdef PTZ_BLDC_MOTOR
// #define ERROR_compensation_switch "on"//误差补偿开关,直流无刷电机原则上不使用误差补偿，而使用重复定位
// #endif
#define ERROR_compensation_switch "on"//误差补偿开关,直流无刷电机原则上不使用误差补偿，而使用重复定位
/**/
#define END "[end]"

63
APP/Common/ptz_default_value.h
View File

@ -508,7 +508,70 @@
#endif
//伺服电机
#ifdef PTZ_MEDIUM_WORM_SERVO_MOTOR_24V
///水平电机减速比
#define PTZ_HORI_MOTOR_RATIO 1.0
///水平大齿轮减速比
#define PTZ_HORI_BIG_GEAR_RATIO 60.0
///水平总减速比
#define PTZ_HORI_RATIO 60.0
///转速单位 转/每分
///水平电机最大转速
#define PTZ_HORI_MOTOR_MAX_SPEED 600.0
///水平电机最小转速
#define PTZ_HORI_MOTOR_MIN_SPEED 1.0
///水平云台最大转速
#define PTZ_HORI_MAX_SPEED 10.0
///水平云台最小转速
#define PTZ_HORI_MIN_SPEED 0.02
///水平云台默认最佳速度
#define PTZ_HORI_BEST_SPEED 2.0 //步进电机默认速度不能过大，应为对应扭矩最大值
///快要到达指定位置时的电机刹车转速(单位r/min)
#define PTZ_HORI_BREAK_MOTOR_SPEED 15.0
///快要到达指定位置时的刹车转速(单位°/s)
#define PTZ_HORI_BREAK_SPEED (PTZ_HORI_BREAK_MOTOR_SPEED / PTZ_HORI_RATIO * 6.0)
///水平电机方向 1-正向 0-反向
#define PTZ_HORI_MOTOR_DIRECTION 1
///水平电机加速度时间常数 单位ms (0-->>1000rpm)
#define PTZ_HORI_MOTOR_AccelerationTimeConstant 3000
///水平电机加速度时间常数 单位ms (1000-->>0rpm)
#define PTZ_HORI_MOTOR_DecelerationTimeConstant 2000
/**********************************************************/
///磁编码器和电机之间的减速比
#define PTZ_VERT_MOTOR_RATIO 1.0
///磁编码器与云台轴之间的减速比
#define PTZ_VERT_BIG_GEAR_RATIO 60.0
///垂直总减速比
#define PTZ_VERT_RATIO 60.0
///转速单位 转/每分
///垂直电机最大转速
#define PTZ_VERT_MOTOR_MAX_SPEED 600.0
///垂直电机最小转速
#define PTZ_VERT_MOTOR_MIN_SPEED 1.0
///垂直云台最大转速
#define PTZ_VERT_MAX_SPEED 10.0
///垂直云台最小转速
#define PTZ_VERT_MIN_SPEED 0.02
///垂直云台默认最佳速度
#define PTZ_VERT_BEST_SPEED 2.0//6.0
///快要到达指定位置时的电机刹车转速
#define PTZ_VERT_BREAK_MOTOR_SPEED 15.0
///快要到达指定位置时的刹车转速
#define PTZ_VERT_BREAK_SPEED (PTZ_VERT_BREAK_MOTOR_SPEED / PTZ_VERT_RATIO)
///垂直电机方向 1-正向 0-反向
#define PTZ_VERT_MOTOR_DIRECTION 1
///垂直电机加速度时间常数 单位ms (0-->>1000rpm)
#define PTZ_VERT_MOTOR_AccelerationTimeConstant 3000
///垂直电机加速度时间常数 单位ms (1000-->>0rpm)
#define PTZ_VERT_MOTOR_DecelerationTimeConstant 2000
#endif
#endif

7
APP/Common/ptz_global_variable.c
View File

@ -114,4 +114,11 @@ void ptz_return_clear()
#endif
//====伺服电机-24V
#ifdef PTZ_MEDIUM_WORM_SERVO_MOTOR_24V
unsigned char g_program_version_number[50] = "SL_YT";
unsigned char g_ptz_type[60] = "SL_YT";
#endif

2
APP/Common/ptz_header_file.h
View File

@ -27,7 +27,7 @@
#include "service_statusmonitor.h"
#include "as5047d.h"
#include "rotate_step.h"
#include "rotate_servo.h"
#endif

10
APP/Common/ptz_type_select.h
View File

@ -4,12 +4,12 @@
/**********电机类型选择**********/
//使用无刷直流电机
#define PTZ_BLDC_MOTOR 1
// #define PTZ_BLDC_MOTOR 1
//====直流无刷电机驱动器选择====
//#define L6235D 3
//全桥驱动
#define Full_bridge 3
// #define Full_bridge 3
//使用步进电机
@ -18,6 +18,8 @@
//====步进电机驱动器选择====
//#define TMC2160 3
//使用伺服电机
#define PTZ_SERVO_MOTOR 3
/**********云台型号选择**********/
@ -25,7 +27,7 @@
//#define PTZ_HEAVY_WORM_L6235D_AS5047D 7
//====直流无刷-L6235D-蜗轮蜗杆-中型云台-24V
#define PTZ_MEDIUM_WORM_L6235D_AS5047D 8
// #define PTZ_MEDIUM_WORM_L6235D_AS5047D 8
//====直流无刷-L6235D-直齿-轻型云台-24V
//#define PTZ_LIGHT_GEAR_L6235D_AS5047D 9
@ -44,6 +46,8 @@
//====步进电机-TMC2160-涡轮蜗杆-中型云台-24V
//#define PTZ_MEDIUM_WORM_STEP_TMC2160_AS5047D_24V 105
//====伺服电机-涡轮蜗杆-轻型云台-24V
#define PTZ_MEDIUM_WORM_SERVO_MOTOR_24V 107
/************其他功能选择***********/

3
APP/Device/Device_angle/angle_poweroffsave.h
View File

@ -43,6 +43,9 @@
#define PTZ_POWER_DOWN_INC 0.2 //萇揤蔥腴腔講ㄗ睦ㄘ
#endif
#ifdef PTZ_MEDIUM_WORM_SERVO_MOTOR_24V
#define PTZ_POWER_DOWN_INC 0.2 //电压降低的量（伏）
#endif
#define PTZ_MB85RC64_ADD_A 0x0000//裁萇湔揣華硊A
#define PTZ_MB85RC64_ADD_B 0x0400//裁萇湔揣華硊B

54
APP/Device/Device_angle/get_angle.c
View File

@ -10,6 +10,8 @@
#include "ptz_type_select.h"
#include "ptz_global_variable.h"
#include "rotate_servo.h"
///云台角度回复
void ptz_send_angle(char dev,char angle_choice)
@ -173,6 +175,58 @@ static void ptz_vert_ps_delay_ms()
}
#endif
#ifdef PTZ_SERVO_MOTOR
static void ptz_hori_ps_delay_ms()
{
// unsigned int hori_ps_time = 0;
//#ifdef PTZ_PHOTOELECTRIC_SWITCH
// if(g_ptz.hori_speed_actual > 0)
// {
// hori_ps_time = (unsigned int)((PTZ_HORI_MAX_SPEED / g_ptz.hori_speed_actual) * PTZ_HORI_PS_TIEM + 0.5);
// }
// else
// {
// hori_ps_time = (unsigned int)((PTZ_HORI_MAX_SPEED / PTZ_HORI_MIN_SPEED) * PTZ_HORI_PS_TIEM + 0.5);
// }
//
// if(hori_ps_time < PTZ_HORI_PS_TIEM)
// {
// hori_ps_time = PTZ_HORI_PS_TIEM;
// }
//
// OSTimeDlyHMSM(0u, 0u, 0u, hori_ps_time);
//#endif
//
}
static void ptz_vert_ps_delay_ms()
{
// unsigned int vert_ps_time = 0;
//
//#ifdef PTZ_PHOTOELECTRIC_SWITCH
// if(g_ptz.vert_speed_actual > 0)
// {
// vert_ps_time = (unsigned int)((PTZ_VERT_MAX_SPEED / g_ptz.vert_speed_actual) * PTZ_VERT_PS_TIEM + 0.5);
// }
// else
// {
// vert_ps_time = (unsigned int)((PTZ_VERT_MAX_SPEED / PTZ_VERT_MIN_SPEED) * PTZ_VERT_PS_TIEM + 0.5);
// }
//
// if(vert_ps_time < PTZ_VERT_PS_TIEM)
// {
// vert_ps_time = PTZ_VERT_PS_TIEM;
// }
//
// OSTimeDlyHMSM(0u, 0u, 0u, vert_ps_time);
//#endif
}
#endif
/// 计算云台水平角度
#ifdef PTZ_HORI_ANGLE_AS5047D_ANGLE_ASY
static char ptz_hori_get_angle()

40
APP/Device/Device_angle/get_angle.h
View File

@ -515,7 +515,47 @@
#define HORI_MS_SW3_ERROR 25//采用磁开关，实时计算磁开关范围时用的阈值
#endif
//伺服电机
#ifdef PTZ_MEDIUM_WORM_SERVO_MOTOR_24V
//磁编码器角度增大，云台角度减小；磁编码器角度减小，云台角度增大
#define PTZ_HORI_ANGLE_AS5047D_ANGLE_SYN 1//角度变化同步
#define PTZ_VERT_ANGLE_AS5047D_ANGLE_SYN 1//角度变化同步
//#define PTZ_HORI_GET_ANGLE_MANY 1//多次获取角度算平均值
//#define PTZ_VERT_GET_ANGLE_MANY 1//多次获取角度算平均值
#define PTZ_HORI_GET_ANGLE_SINGLE 1//单次获取角度
#define PTZ_VERT_GET_ANGLE_SINGLE 1//单次获取角度
#define PTZ_SW3_LEFT_RISE_UPDATE 1 //打开水平挡片左转上升沿更新水平角度数据
//#define PTZ_SW1_DOWN_FALL_UPDATE 1 //垂直以SW1下俯下降沿更新垂直角度数据
#define PTZ_SW1_UP_RISE_UPDATE 1 //垂直以SW1上仰上升沿更新垂直角度数据
#define PTZ_RIGHT_CYCLE_C (g_ptz.hori_speed_actual * PTZ_HORI_BIG_GEAR_RATIO * 6.0 / 1000.0 * PTZ_HORI_ANGLE_TIME + 5.0)//36.0 //检测转动完一圈是否到达同一个点的距离阈值
#define PTZ_LEFT_CYCLE_C (g_ptz.hori_speed_actual * PTZ_HORI_BIG_GEAR_RATIO * 6.0 / 1000.0 * PTZ_HORI_ANGLE_TIME + 5.0)//36.0 //检测转动完一圈是否到达同一个点的距离阈值
#define PTZ_UP_CYCLE_C (g_ptz.vert_speed_actual * PTZ_VERT_BIG_GEAR_RATIO * 6.0 / 1000.0 * PTZ_VERT_ANGLE_TIME + 5.0)//36.0 //检测转动完一圈是否到达同一个点的距离阈值
#define PTZ_DOWN_CYCLE_C (g_ptz.vert_speed_actual * PTZ_VERT_BIG_GEAR_RATIO * 6.0 / 1000.0 * PTZ_VERT_ANGLE_TIME + 5.0)//36.0 //检测转动完一圈是否到达同一个点的距离阈值
#define PTZ_HORI_ANGLE_READ_NUM 4 //角度连续读取次数
#define PTZ_VERT_ANGLE_READ_NUM 4 //角度连续读取次数
#define PTZ_VERT_AS5047D_ANGLE_WF_A (g_ptz.vert_speed_actual * PTZ_VERT_BIG_GEAR_RATIO * 6.0 / 1000.0 * PTZ_VERT_ANGLE_TIME + 5.0)//36.0//云台垂直磁编码总角度滤波
#define PTZ_HORI_AS5047D_ANGLE_WF_A (g_ptz.hori_speed_actual * PTZ_HORI_BIG_GEAR_RATIO * 6.0 / 1000.0 * PTZ_HORI_ANGLE_TIME + 5.0)//36.0//云台水平磁编码总角度滤波
#define PTZ_HORI_AS5047D_ANGLE_WF_B (g_ptz.hori_as5047d.as5047d_ptz_angle_max - (g_ptz.hori_speed_actual * PTZ_HORI_BIG_GEAR_RATIO * 6.0 / 1000.0 * PTZ_HORI_ANGLE_TIME + 5.0))//云台水平磁编码总角度滤波
#define PTZ_HORI_ANGLE_DIFF_A (g_ptz.hori_speed_actual * PTZ_HORI_BIG_GEAR_RATIO * 6.0 / 1000.0 * PTZ_HORI_ANGLE_TIME + 5.0) //上次采集角度和此次采集角度之间允许的差值
#define PTZ_HORI_ANGLE_DIFF_B (360.0 - PTZ_HORI_ANGLE_DIFF_A) //上次采集角度和此次采集角度之间允许的差值
#define PTZ_VERT_ANGLE_DIFF_A (g_ptz.vert_speed_actual * PTZ_VERT_BIG_GEAR_RATIO * 6.0 / 1000.0 * PTZ_VERT_ANGLE_TIME + 5.0) //上次采集角度和此次采集角度之间允许的差值
#define PTZ_VERT_ANGLE_DIFF_B (360.0 - PTZ_VERT_ANGLE_DIFF_A) //上次采集角度和此次采集角度之间允许的差值
#define HORI_MS_SW3_ERROR 25//采用磁开关，实时计算磁开关范围时用的阈值
#endif
void ptz_send_sw_angle_state(char dev);
void init_angle_module();

174
APP/Device/Device_rotate/rotate_servo.c Normal file
View File

@ -0,0 +1,174 @@
#include "rotate_servo.h"
#ifdef PTZ_MEDIUM_WORM_SERVO_MOTOR_24V
static BSP_OS_SEM ptz_hori_stop_mutex;//共享资源锁
static BSP_OS_SEM ptz_vert_stop_mutex;
float ptz_vert_break_angle()
{
//云台水平轴加速度(°/s²)
const float a = 1000.0f * 6.0f
/ ((float)(PTZ_VERT_MOTOR_DecelerationTimeConstant / 1000.0f))
/ PTZ_VERT_RATIO;
const float tmp = a / 2.0f;
//平稳运行角度，以最低转速运行1秒
const float s1 = PTZ_VERT_BREAK_SPEED * 1;
//当前云台水平轴转速(°/s)
float Vnow = g_ptz.hori_speed_actual * 6.0f;
return (Vnow - PTZ_VERT_BREAK_SPEED) * (Vnow + PTZ_VERT_BREAK_SPEED) / tmp + s1;
}
float ptz_hori_break_angle()
{
//云台水平轴加速度(°/s²)
const float a = 1000.0f * 6.0f
/ ((float)(PTZ_HORI_MOTOR_DecelerationTimeConstant / 1000.0f))
/ PTZ_HORI_RATIO;
const float tmp = a / 2.0f;
//平稳运行角度，以最低转速运行1秒
const float s1 = PTZ_HORI_BREAK_SPEED * 1;
//当前云台水平轴转速(°/s)
float Vnow = g_ptz.hori_speed_actual * 6.0f;
return (Vnow - PTZ_HORI_BREAK_SPEED) * (Vnow + PTZ_HORI_BREAK_SPEED) / tmp + s1;
}
void ptz_sem_post_stop_mutex()
{
BSP_OS_SemPost(&ptz_hori_stop_mutex);
BSP_OS_SemPost(&ptz_vert_stop_mutex);
}
void ptz_hori_start(char direction, float speed)
{
BSP_OS_SemWait(&ptz_hori_stop_mutex, 0u);
BSP_OS_SemPost(&ptz_hori_stop_mutex);
}
void ptz_hori_stop(unsigned short int time)
{
BSP_OS_SemWait(&ptz_hori_stop_mutex, 0u);
BSP_OS_SemPost(&ptz_hori_stop_mutex);
}
void ptz_vert_start(char direction, float speed)
{
BSP_OS_SemWait(&ptz_vert_stop_mutex, 0u);
BSP_OS_SemPost(&ptz_vert_stop_mutex);
}
void ptz_vert_stop(unsigned short int time)
{
BSP_OS_SemWait(&ptz_vert_stop_mutex, 0u);
BSP_OS_SemPost(&ptz_vert_stop_mutex);
}
static void ptz_hori_rotate_monitor_task()
{
}
static void ptz_vert_rotate_monitor_task()
{
}
static void ptz_hori_rotate_task()
{
while(1)
{
ptz_hori_rotate_monitor_task();
OSTimeDlyHMSM(0u, 0u, 0u, 1u);
}
}
static void ptz_vert_rotate_task()
{
while(1)
{
ptz_vert_rotate_monitor_task();
OSTimeDlyHMSM(0u, 0u, 0u, 1u);
}
}
static OS_STK task_hori_rotate_stk[TASK_HORI_ROATE_MONITOR_STK_SIZE];
static void creat_task_hori_rotate(void)
{
CPU_INT08U task_err;
CPU_INT08U name_err;
task_err = OSTaskCreateExt((void (*)(void *)) ptz_hori_rotate_task,
(void *) 0,
(OS_STK *)&task_hori_rotate_stk[TASK_HORI_ROATE_MONITOR_STK_SIZE - 1],
(INT8U ) TASK_HORI_ROATE_MONITOR_PRIO,
(INT16U ) TASK_HORI_ROATE_MONITOR_PRIO,
(OS_STK *)&task_hori_rotate_stk[0],
(INT32U ) TASK_HORI_ROATE_MONITOR_STK_SIZE,
(void *) 0,
(INT16U )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR));
#if (OS_TASK_NAME_EN > 0)
OSTaskNameSet(TASK_HORI_ROATE_MONITOR_PRIO, "ptz_hori_rotate_task", &name_err);
#endif
if ((task_err == OS_ERR_NONE) && (name_err == OS_ERR_NONE)) {
pdebug(DEBUG_LEVEL_INFO,"create ptz_hori_rotate_task success...\n\r");
} else {
pdebug(DEBUG_LEVEL_FATAL,"create ptz_hori_rotate_task failed...\n\r");
}
}
static OS_STK task_vert_rotate_stk[TASK_VERT_ROATE_MONITOR_STK_SIZE];
static void creat_task_vert_rotate(void)
{
CPU_INT08U task_err;
CPU_INT08U name_err;
task_err = OSTaskCreateExt((void (*)(void *)) ptz_vert_rotate_task,
(void *) 0,
(OS_STK *)&task_vert_rotate_stk[TASK_VERT_ROATE_MONITOR_STK_SIZE - 1],
(INT8U ) TASK_VERT_ROATE_MONITOR_PRIO,
(INT16U ) TASK_VERT_ROATE_MONITOR_PRIO,
(OS_STK *)&task_vert_rotate_stk[0],
(INT32U ) TASK_VERT_ROATE_MONITOR_STK_SIZE,
(void *) 0,
(INT16U )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR));
#if (OS_TASK_NAME_EN > 0)
OSTaskNameSet(TASK_VERT_ROATE_MONITOR_PRIO, "ptz_vert_rotate_task", &name_err);
#endif
if ((task_err == OS_ERR_NONE) && (name_err == OS_ERR_NONE)) {
pdebug(DEBUG_LEVEL_INFO,"create ptz_vert_rotate_task success...\n\r");
} else {
pdebug(DEBUG_LEVEL_FATAL,"create ptz_vert_rotate_task failed...\n\r");
}
}
void init_rotate_monitor_module(void)
{
BSP_OS_SemCreate(&ptz_hori_stop_mutex,1u,"ptz_hori_stop_mutex");
BSP_OS_SemCreate(&ptz_vert_stop_mutex,1u,"ptz_vert_stop_mutex");
creat_task_hori_rotate();
creat_task_vert_rotate();
}
#endif

55
APP/Device/Device_rotate/rotate_servo.h Normal file
View File

@ -0,0 +1,55 @@
#ifndef __PTZ_ROATE_SERVO_H_
#define __PTZ_ROATE_SERVO_H_
#include "ptz_header_file.h"
#include "ptz_type_select.h"
#ifdef PTZ_MEDIUM_WORM_SERVO_MOTOR_24V
///云台水平右转
#define PTZ_HORI_DIR_RIGHT 3//0
///云台水平左转
#define PTZ_HORI_DIR_LEFT 1
///云台处于停止状态
#define PTZ_HORI_DIR_STOP 2
///云台垂直向上
#define PTZ_VERT_DIR_UP 3//0
///云台垂直向下
#define PTZ_VERT_DIR_DOWN 1
///云台处于停止状态
#define PTZ_VERT_DIR_STOP 2
///距离最近刹车点以最小转速运行的距离
#define PTZ_HORI_BREAK_SPEED_ANGLE ptz_hori_break_angle()
#define PTZ_VERT_BREAK_SPEED_ANGLE ptz_vert_break_angle()
///刹车最近距离
#define PTZ_HORI_STOP_NEAR_DISTANCE 0.01
///刹车最远距离
#define PTZ_HORI_STOP_FAR_DISTANCE 4.0
///刹车最近距离
#define PTZ_VERT_STOP_NEAR_DISTANCE 0.01
///刹车最远距离
#define PTZ_VERT_STOP_FAR_DISTANCE 4.0
///云台定位精度，也可用于判断是不是同一个位置
#define PTZ_HORI_ANGLE_ACCURACY 0.05
///云台定位精度，也可用于判断是不是同一个位置
#define PTZ_VERT_ANGLE_ACCURACY 0.05
float ptz_vert_break_angle();
float ptz_hori_break_angle();
void ptz_sem_post_stop_mutex();
void ptz_hori_start(char direction, float speed);
void ptz_hori_stop(unsigned short int time);
void ptz_vert_start(char direction, float speed);
void ptz_vert_stop(unsigned short int time);
void init_rotate_monitor_module(void);
#endif
#endif

149
APP/Device/Device_speed/speed_to_servoMotor.c Normal file
View File

@ -0,0 +1,149 @@
#include "speed_to_servoMotor.h"
#include "ptz_header_file.h"
#include "servoMotor.h"
#ifdef PTZ_SERVO_MOTOR
//发送云台实际转速
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;
}
}
static void ptz_hori_step_speed_task()
{
while(1)
{
if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
{
}
if(g_ptz.hori_start_stop_set == PTZ_HORI_STOP)
{
}
OSTimeDlyHMSM(0u, 0u, 0u, 10);
}
}
static void ptz_vert_step_speed_task()
{
while(1)
{
if(g_ptz.vert_start_stop_set == PTZ_VERT_START)
{
}
if(g_ptz.vert_start_stop_set == PTZ_VERT_STOP)
{
}
OSTimeDlyHMSM(0u, 0u, 0u, 10);
}
}
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)
{
servoMotorInit();
creat_task_hori_step_speed_task();
creat_task_vert_step_speed_task();
}
#endif

13
APP/Device/Device_speed/speed_to_servoMotor.h Normal file
View File

@ -0,0 +1,13 @@
#ifndef __DEVICE_SPEED_SERVOMOTOR_H_
#define __DEVICE_SPEED_SERVOMOTOR_H_
#include "ptz_type_select.h"
#ifdef PTZ_SERVO_MOTOR
void ptz_send_speed(char dev, char speed);
void init_speed_module(void);
void init_speed_module(void);
#endif
#endif

3
APP/Device/device_Other/device_interrupt.c
View File

@ -8,6 +8,9 @@
#include "gd32f4xx_it.h"
#include "ptz_type_select.h"
#include "get_angle.h"
#include "rotate_servo.h"
/// @brief 外部中断初始化
/// @param[in] usart_periph:EXTI_IRQ_init
/// @return none

2
APP/Service/service_areascan.c
View File

@ -3,6 +3,8 @@
#include "enet_to_udp.h"
#include "service_areascan.h"
#include "rotate_servo.h"
static BSP_OS_SEM ptz_area_crc_mutex;
PtzArea g_area[AREA_AMOUNT];

2
APP/Service/service_error_count.c
View File

@ -16,6 +16,8 @@
#include "tmc2160.h"
#include "service_error_count.h"
#include "rotate_servo.h"
char error_conut_state;
#define COUNT_STATE 1

383
APP/Service/service_selfcheck.c
View File

@ -20,6 +20,8 @@
#include "tmc2160.h"
#include "service_error_count.h"
#include "rotate_servo.h"
#ifdef PTZ_BLDC_MOTOR
/// 云台全范围自检
static unsigned char ptz_hori_complete_self_check_task()
@ -798,6 +800,387 @@ static unsigned char ptz_vert_simplify_self_check_task()
#endif
#ifdef PTZ_SERVO_MOTOR
/// 云台全范围自检
static unsigned char ptz_hori_complete_self_check_task()
{
switch(g_ptz.hori_self_check)
{
//首先让云台水平向右转动
case PTZ_HORI_SELF_CHECK_COMPLETE_STEP:
if(g_ptz.hori_ps_sw3_state == PS_COVER)//如果水平光电开关被挡住
{
ptz_hori_start(PTZ_HORI_DIR_LEFT, PTZ_HORI_SELF_CHECK_SPEED);
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 1;
}
else//没有被挡住
{
g_ptz.hori_ps_sw3_right_fall = 0;
g_ptz.hori_ps_sw3_left_rise = 0;
g_ptz.hori_ps_sw3_right_rise = 0;
g_ptz.hori_ps_sw3_left_fall = 0;
ptz_hori_start(PTZ_HORI_DIR_RIGHT, PTZ_HORI_SELF_CHECK_SPEED);
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 2;
}
break;
case PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 1:
if(g_ptz.hori_ps_sw3_state == PS_NO_COVER)
{
OSTimeDlyHMSM(0u, 0u, 3u, 0u);//延时转动
ptz_hori_stop(PTZ_HORI_STOP_TIME);
g_ptz.hori_ps_sw3_right_fall = 0;
g_ptz.hori_ps_sw3_left_rise = 0;
g_ptz.hori_ps_sw3_right_rise = 0;
g_ptz.hori_ps_sw3_left_fall = 0;
ptz_hori_start(PTZ_HORI_DIR_RIGHT, PTZ_HORI_SELF_CHECK_SPEED);
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 2;
}
break;
//等待第一次水平光电开关下降沿中断
case (PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 2):
if(g_ptz.hori_ps_sw3_right_fall >= PS_FALL_ON)
{
g_ptz.hori_as5047d.as5047d_data_reset = 1;
g_ptz.hori_ps_sw3_right_fall = 0;
g_ptz.hori_self_check++;
OSTimeDlyHMSM(0u, 0u, 5u, 0u);
g_ptz.hori_ps_sw3_right_fall = 0;
}
break;
//等待第二次水平光电开关下降沿中断
case (PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 3):
if(g_ptz.hori_ps_sw3_right_fall >= PS_FALL_ON)
{
g_ptz.hori_as5047d.as5047d_cycle_num = g_ptz.hori_as5047d.as5047d_cycle_count;
g_ptz.hori_as5047d.as5047d_ptz_angle_max = g_ptz.hori_as5047d.as5047d_ptz_angle_actual;
g_ptz.hori_as5047d.as5047d_ptz_angle_K = 360.0 / g_ptz.hori_as5047d.as5047d_ptz_angle_max;
g_ptz.hori_as5047d.as5047d_remain_angle = g_ptz.hori_as5047d.as5047d_ptz_angle_max -
g_ptz.hori_as5047d.as5047d_cycle_num * 360.0;
g_ptz.hori_angleP.angle_allow_max = 360.0;
g_ptz.hori_angleP.angle_allow_min = 0.0;
g_ptz.hori_as5047d.as5047d_state = 1;
g_ptz.hori_ps_sw3_right_fall = 0;
g_ptz.hori_ps_sw3_left_rise = 0;
g_ptz.hori_ps_sw3_right_rise = 0;
g_ptz.hori_ps_sw3_left_fall = 0;
ptz_hori_stop(PTZ_HORI_STOP_TIME);
g_ptz.hori_angle_state = 1;
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_DATA_SAVE;
}
break;
}
return g_ptz.hori_self_check;
}
/// 云台水平简化自检
static unsigned char ptz_hori_simplify_self_check_task()
{
switch(g_ptz.hori_self_check)
{
case PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP:
if(g_ptz.hori_ps_sw3_state == PS_COVER)//如果水平光电开关被挡住
{
ptz_hori_start(PTZ_HORI_DIR_LEFT, PTZ_HORI_SELF_CHECK_SPEED);
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 1;
}
else//没有挡住
{
g_ptz.hori_ps_sw3_right_fall = 0;
g_ptz.hori_ps_sw3_left_rise = 0;
g_ptz.hori_ps_sw3_right_rise = 0;
g_ptz.hori_ps_sw3_left_fall = 0;
ptz_hori_start(PTZ_HORI_DIR_RIGHT, PTZ_HORI_SELF_CHECK_SPEED);
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 2;
}
break;
case PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 1://向左转动，直到光电开关脱离挡片遮挡
if(g_ptz.hori_ps_sw3_state == PS_NO_COVER)
{
OSTimeDlyHMSM(0u, 0u, 3u, 0u);//延时转动
ptz_hori_stop(PTZ_HORI_STOP_TIME);
g_ptz.hori_ps_sw3_right_fall = 0;
g_ptz.hori_ps_sw3_left_rise = 0;
g_ptz.hori_ps_sw3_right_rise = 0;
g_ptz.hori_ps_sw3_left_fall = 0;
ptz_hori_start(PTZ_HORI_DIR_RIGHT, PTZ_HORI_SELF_CHECK_SPEED);
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 2;
}
break;
case PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 2:
//等待水平光电开关右转下降沿
if(g_ptz.hori_ps_sw3_right_fall >= PS_FALL_ON)
{
g_ptz.hori_as5047d.as5047d_cycle_count = 0;
g_ptz.hori_as5047d.as5047d_ptz_angle_actual = 0;
g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last = 0;
g_ptz.hori_as5047d.as5047d_ptz_init_angle = g_ptz.hori_as5047d.as5047d_angle_actual;
g_ptz.hori_as5047d.as5047d_state = 1;
g_ptz.hori_ps_sw3_right_fall = 0;
g_ptz.hori_ps_sw3_left_rise = 0;
g_ptz.hori_ps_sw3_right_rise = 0;
g_ptz.hori_ps_sw3_left_fall = 0;
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 3;
}
break;
case PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 3://转动到0位
OSTimeDlyHMSM(0u, 0u, 1u, 0u);//延时转动
g_ptz.hori_angle_control = 0.0;
g_ptz.hori_speed_control = PTZ_HORI_BEST_SPEED;
ptz_hori_rotate_plan(PTZ_HORI_MIN_DISTANCE);
g_ptz.hori_rotate_monitor_switch = PTZ_HORI_GO_TO_ANGLE_A;
g_ptz.hori_angle_state = 1;
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_END;
break;
}
return g_ptz.hori_self_check;
}
/// 云台垂直全范围自检
static unsigned char ptz_vert_complete_self_check_task()
{
switch(g_ptz.vert_self_check)
{
///首先读取光电开关的状态，初步判断垂直状态
case PTZ_VERT_SELF_CHECK_COMPLETE_STEP:
g_ptz.vert_ps_sw1_down_fall = 0;
g_ptz.vert_ps_sw1_up_rise = 0;
g_ptz.vert_ps_sw2_up_fall = 0;
g_ptz.vert_ps_sw2_down_rise = 0;
if(g_ptz.vert_ps_sw1_state == PS_COVER)//云台处于下俯极限位置
{
ptz_vert_start(PTZ_VERT_DIR_UP, PTZ_VERT_SELF_CHECK_SPEED);
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 1;
}
if(g_ptz.vert_ps_sw2_state == PS_COVER)//云台处于上仰极限位置
{
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 2;
}
if(g_ptz.vert_ps_sw1_state == PS_NO_COVER && g_ptz.vert_ps_sw2_state == PS_NO_COVER)
//云台处于中间位置
{
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 2;
}
break;
case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 1):
if(g_ptz.vert_ps_sw1_state == PS_NO_COVER)
{
OSTimeDlyHMSM(0u, 0u, 3u, 0u);//延时转动
ptz_vert_stop(PTZ_VERT_STOP_TIME);
g_ptz.vert_ps_sw1_down_fall = 0;
g_ptz.vert_ps_sw1_up_rise = 0;
g_ptz.vert_ps_sw2_up_fall = 0;
g_ptz.vert_ps_sw2_down_rise = 0;
g_ptz.vert_self_check ++;
}
break;
//开启向下转动，查找下俯极限位置
case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 2):
ptz_vert_start(PTZ_VERT_DIR_DOWN, PTZ_VERT_SELF_CHECK_SPEED);
g_ptz.vert_self_check ++;
break;
#ifdef PTZ_SW1_DOWN_FALL_UPDATE
//判断是否到达下俯极限位置,然后云台开始向上转动
case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 3):
if(g_ptz.vert_ps_sw1_down_fall >= 1 ||
g_ptz.vert_ps_sw1_state == PS_COVER)
{
g_ptz.vert_ps_sw1_down_fall = 0;
g_ptz.vert_as5047d.as5047d_data_reset = 1;
ptz_vert_stop(PTZ_VERT_STOP_TIME);
ptz_vert_start(PTZ_VERT_DIR_UP, PTZ_VERT_SELF_CHECK_SPEED);
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 10;
}
break;
#endif
#ifdef PTZ_SW1_UP_RISE_UPDATE
//判断是否到达下俯极限位置,然后云台开始向上转动
case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 3):
if(g_ptz.vert_ps_sw1_down_fall >= 1 ||
g_ptz.vert_ps_sw1_state == PS_COVER)
{
g_ptz.vert_ps_sw1_down_fall = 0;
g_ptz.vert_ps_sw1_up_rise = 0;
OSTimeDlyHMSM(0u, 0u, 0u, 300u);
ptz_vert_stop(PTZ_VERT_STOP_TIME);
ptz_vert_start(PTZ_VERT_DIR_UP, PTZ_VERT_SELF_CHECK_SPEED);
g_ptz.vert_self_check++;
}
break;
//等待上仰上升沿
case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 4):
if(g_ptz.vert_ps_sw1_up_rise >= 1 ||
g_ptz.vert_ps_sw1_state == PS_NO_COVER)
{
g_ptz.vert_as5047d.as5047d_data_reset = 1;
g_ptz.vert_ps_sw1_down_fall = 0;
g_ptz.vert_ps_sw1_up_rise = 0;
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 10;
}
break;
#endif
//等待SW2下降沿,计算垂直转动范围
case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 10):
if(g_ptz.vert_ps_sw2_up_fall >= 1 ||
g_ptz.vert_ps_sw2_state == PS_COVER)
{
g_ptz.vert_as5047d.as5047d_cycle_num = g_ptz.vert_as5047d.as5047d_cycle_count;
g_ptz.vert_as5047d.as5047d_ptz_angle_max = g_ptz.vert_as5047d.as5047d_ptz_angle_actual;
g_ptz.vert_angleP.angle_range =
g_ptz.vert_as5047d.as5047d_ptz_angle_max / PTZ_VERT_BIG_GEAR_RATIO;
g_ptz.vert_angleP.angle_allow_min = g_ptz.vert_angleP.angle_range / 2.0 * (-1);
g_ptz.vert_angleP.angle_allow_max = g_ptz.vert_angleP.angle_range / 2.0;
g_ptz.vert_as5047d.as5047d_ptz_angle_K = 1 / PTZ_VERT_BIG_GEAR_RATIO;
g_ptz.vert_as5047d.as5047d_remain_angle = g_ptz.vert_as5047d.as5047d_ptz_angle_max -
g_ptz.vert_as5047d.as5047d_cycle_num * 360.0;
g_ptz.vert_as5047d.as5047d_state = 1;
g_ptz.vert_ps_sw1_down_fall = 0;
g_ptz.vert_ps_sw1_up_rise = 0;
g_ptz.vert_ps_sw2_up_fall = 0;
g_ptz.vert_ps_sw2_down_rise = 0;
ptz_vert_stop(PTZ_VERT_STOP_TIME);
g_ptz.vert_angle_state = 1;
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_DATA_SAVE;
}
break;
}
return g_ptz.vert_self_check;
}
/// 云台垂直简化自检
static unsigned char ptz_vert_simplify_self_check_task()
{
switch(g_ptz.vert_self_check)
{
case PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP:
g_ptz.vert_ps_sw1_down_fall = 0;
g_ptz.vert_ps_sw1_up_rise = 0;
g_ptz.vert_ps_sw2_up_fall = 0;
g_ptz.vert_ps_sw2_down_rise = 0;
if(g_ptz.vert_ps_sw1_state == PS_COVER)//云台处于下俯极限位置
{
ptz_vert_start(PTZ_VERT_DIR_UP, PTZ_VERT_SELF_CHECK_SPEED);
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 1;
}
if(g_ptz.vert_ps_sw2_state == PS_COVER)//云台处于上仰极限位置
{
ptz_vert_start(PTZ_VERT_DIR_DOWN, PTZ_VERT_SELF_CHECK_SPEED);
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 2;
}
if(g_ptz.vert_ps_sw1_state == PS_NO_COVER && g_ptz.vert_ps_sw2_state == PS_NO_COVER)
//云台处于中间位置
{
ptz_vert_start(PTZ_VERT_DIR_DOWN, PTZ_VERT_SELF_CHECK_SPEED);
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 2;
}
break;
case PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 1:
if(g_ptz.vert_ps_sw1_state == PS_NO_COVER)
{
OSTimeDlyHMSM(0u, 0u, 3u, 0u);//延时转动
ptz_vert_stop(PTZ_VERT_STOP_TIME);
g_ptz.vert_ps_sw1_down_fall = 0;
g_ptz.vert_ps_sw1_up_rise = 0;
g_ptz.vert_ps_sw2_up_fall = 0;
g_ptz.vert_ps_sw2_down_rise = 0;
ptz_vert_start(PTZ_VERT_DIR_DOWN, PTZ_VERT_SELF_CHECK_SPEED);
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 2;
}
break;
#ifdef PTZ_SW1_DOWN_FALL_UPDATE
case PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 2:
if(g_ptz.vert_ps_sw1_down_fall >= 1 ||
g_ptz.vert_ps_sw1_state == PS_COVER)
{//等待SW1光电开关被遮挡，同时更新当前位置
g_ptz.vert_as5047d.as5047d_cycle_count = 0;
g_ptz.vert_as5047d.as5047d_ptz_angle_actual = 0;
g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = 0;
g_ptz.vert_as5047d.as5047d_ptz_init_angle = g_ptz.vert_as5047d.as5047d_angle_actual;
g_ptz.vert_as5047d.as5047d_state = 1;
g_ptz.vert_ps_sw1_down_fall = 0;
g_ptz.vert_ps_sw1_up_rise = 0;
g_ptz.vert_ps_sw2_up_fall = 0;
g_ptz.vert_ps_sw2_down_rise = 0;
ptz_vert_stop(PTZ_VERT_STOP_TIME);
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 10;
}
break;
#endif
#ifdef PTZ_SW1_UP_RISE_UPDATE
//判断是否到达下俯极限位置,然后云台开始向上转动
case (PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 2):
if(g_ptz.vert_ps_sw1_down_fall >= 1 ||
g_ptz.vert_ps_sw1_state == PS_COVER)
{
g_ptz.vert_ps_sw1_down_fall = 0;
g_ptz.vert_ps_sw1_up_rise = 0;
g_ptz.vert_ps_sw2_up_fall = 0;
g_ptz.vert_ps_sw2_down_rise = 0;
OSTimeDlyHMSM(0u, 0u, 0u, 300u);
ptz_vert_stop(PTZ_VERT_STOP_TIME);
ptz_vert_start(PTZ_VERT_DIR_UP, PTZ_VERT_SELF_CHECK_SPEED);
g_ptz.vert_self_check++;
}
break;
//等待上仰上升沿
case (PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 3):
if(g_ptz.vert_ps_sw1_up_rise >= 1 ||
g_ptz.vert_ps_sw1_state == PS_NO_COVER)
{
//等待SW1光电开关被遮挡，同时更新当前位置
g_ptz.vert_as5047d.as5047d_cycle_count = 0;
g_ptz.vert_as5047d.as5047d_ptz_angle_actual = 0;
g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = 0;
g_ptz.vert_as5047d.as5047d_ptz_init_angle = g_ptz.vert_as5047d.as5047d_angle_actual;
g_ptz.vert_as5047d.as5047d_state = 1;
g_ptz.vert_ps_sw1_down_fall = 0;
g_ptz.vert_ps_sw1_up_rise = 0;
g_ptz.vert_ps_sw2_up_fall = 0;
g_ptz.vert_ps_sw2_down_rise = 0;
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 10;
}
break;
#endif
case PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 10:
OSTimeDlyHMSM(0u, 0u, 1u, 0u);
g_ptz.vert_angle_control = 0.0;
g_ptz.vert_speed_control = PTZ_VERT_BEST_SPEED;
ptz_vert_rotate_plan(PTZ_VERT_ANGLE);
g_ptz.vert_rotate_monitor_switch = PTZ_VERT_GO_TO_ANGLE_A;
g_ptz.vert_angle_state = 1;
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_END;
break;
}
return g_ptz.vert_self_check;
}
#endif
static void ptz_hori_self_check_task()
{

8
APP/Service/service_selfcheck.h
View File

@ -94,6 +94,14 @@
#define PTZ_VERT_SELF_CHECK_SPEED 0.8
#endif
//伺服电机
#ifdef PTZ_MEDIUM_WORM_SERVO_MOTOR_24V
///水平自检最大转速
#define PTZ_HORI_SELF_CHECK_SPEED 1.2
///垂直自检最大转速
#define PTZ_VERT_SELF_CHECK_SPEED 0.8
#endif
///云台自检参数需要保存和读取的部分
typedef struct _PtzSelfCheckData_
{

251
APP/Service/service_statusmonitor.c
View File

@ -4,6 +4,8 @@
#include "service_selfcheck.h"
#include "device_adc_collect.h"
#include "rotate_servo.h"
extern void ptz_uart_dev_send(device_handle device, void *data, int len);
static char max_temperature_fault;
@ -11,6 +13,8 @@ static char max_temperature_fault;
//温度、电压、电流故障检测
static char ptz_temp_volt_current_fault_detect_task()
{//只报故障，不做响应
#ifdef PTZ_BLDC_MOTOR
static unsigned short int time_ms;
time_ms ++;
@ -37,7 +41,7 @@ static char ptz_temp_volt_current_fault_detect_task()
{
time_ms = 0;
}
#endif
#ifdef Full_bridge //NTC温度监测，暂定温度上限值100摄氏度，后续测试修改
if(g_ptz.H_boad_temp >= 100.0)
@ -742,6 +746,251 @@ static char ptz_motor_rotate_fault_detect_task()
#endif
#ifdef PTZ_SERVO_MOTOR
//电机转动卡死故障检测******主要用于检测电机自身故障与驱动电路器件故障
static char ptz_motor_rotate_fault_detect_task()
{
static char hori_rotate_fault_num;
static char vert_rotate_fault_num;
static char hori_rotate_fault_step;
static float hori_angle_a;
static float hori_angle_b;
static char vert_rotate_fault_step;
static float vert_angle_a;
static float vert_angle_b;
static unsigned short int time_ms;
time_ms ++;
//垂直
if(g_ptz.vert_start_stop_set == PTZ_VERT_STOP)
{
vert_rotate_fault_step = 0;//关闭判断
vert_rotate_fault_num = 0;
}
//水平
if(g_ptz.hori_start_stop_set == PTZ_HORI_STOP)
{
hori_rotate_fault_step = 0;//关闭判断
hori_rotate_fault_num = 0;
}
if(time_ms < 50)
{
return 1;
}
else
{
time_ms = 0;
}
//垂直
switch(vert_rotate_fault_step)
{
case 0:
if(g_ptz.vert_start_stop_set == PTZ_VERT_START)
{
vert_rotate_fault_num = 0;
vert_angle_a = g_ptz.vert_as5047d.as5047d_ptz_angle_actual;
vert_rotate_fault_step = vert_rotate_fault_step + 1; //启动判断
}
break;
case 1://开始判断云台转动是否异常
vert_angle_b = g_ptz.vert_as5047d.as5047d_ptz_angle_actual;
if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_UP)
{
//转动正常情况
if(vert_angle_b > vert_angle_a &&
vert_angle_b - vert_angle_a >= PTZ_VERT_MIN_SPEED * PTZ_VERT_BIG_GEAR_RATIO * 2)
{//只有有一次转动正常，说明转动就是正常的
vert_rotate_fault_num = 0;
//电机转动没有故障
g_ptz.fault_detect.vert_rotate_fault = NO_FAULT;
}
else//转动异常情况
{
if(g_ptz.vert_pid.PidUT_float >= PTZ_VERT_VOLTAGE_LEVEL)
{
vert_rotate_fault_num ++;
}
}
}
else//g_ptz.vert_direction_actual == PTZ_VERT_DIR_DOWN
{
//转动正常情况
if(vert_angle_a > vert_angle_b &&
vert_angle_a - vert_angle_b >= PTZ_VERT_MIN_SPEED * PTZ_VERT_BIG_GEAR_RATIO * 2)
{//只有有一次转动正常，说明转动就是正常的
vert_rotate_fault_num = 0;
//电机转动没有故障
g_ptz.fault_detect.vert_rotate_fault = NO_FAULT;
}
else//转动异常情况
{
if(g_ptz.vert_pid.PidUT_float >= PTZ_VERT_VOLTAGE_LEVEL)
{
vert_rotate_fault_num ++;
}
}
}
if(vert_rotate_fault_num >= ROTATE_FAULT_MAX_NUM)//连续转动异常，则转动异常
{
vert_rotate_fault_num = 0;
//关闭所有云台自动扫描模式
g_preset_bit_scan.state = PRESET_BIT_SCAN_CLOSE_A;
g_area_scan.state = AREA_SCAN_CLOSE_A;
//云台直接刹车PTZ_HORI_STOP_TIME
// ptz_hori_stop(0);
// ptz_vert_stop(0);
ptz_hori_stop(PTZ_HORI_STOP_TIME);
ptz_vert_stop(PTZ_VERT_STOP_TIME);
//电机转动故障
g_ptz.fault_detect.vert_rotate_fault = FAULT;
}
vert_angle_a = vert_angle_b;
break;
}
/*************************************************************/
switch(hori_rotate_fault_step)
{
case 0:
if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
{
hori_rotate_fault_num = 0;
hori_angle_a = g_ptz.hori_as5047d.as5047d_ptz_angle_actual;
hori_rotate_fault_step = hori_rotate_fault_step + 1; //启动判断
}
break;
case 1://开始判断云台转动是否异常
hori_angle_b = g_ptz.hori_as5047d.as5047d_ptz_angle_actual;
if(g_ptz.hori_self_check == PTZ_HORI_SELF_CHECK_END)
{//如果水平自检是完成了的
if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_RIGHT)
{
//转动正常情况
if((hori_angle_b > hori_angle_a && //转动距离要大于最小速度转动距离0.5倍,要小于最大转速转动距离的2倍
hori_angle_b - hori_angle_a > PTZ_HORI_MIN_SPEED * PTZ_HORI_BIG_GEAR_RATIO * 2 &&
hori_angle_b - hori_angle_a < PTZ_HORI_MAX_SPEED * PTZ_HORI_BIG_GEAR_RATIO * 12)
||
(hori_angle_a > hori_angle_b &&
hori_angle_b + g_ptz.hori_as5047d.as5047d_ptz_angle_max - hori_angle_a > PTZ_HORI_MIN_SPEED * PTZ_HORI_BIG_GEAR_RATIO * 2 &&
hori_angle_b + g_ptz.hori_as5047d.as5047d_ptz_angle_max - hori_angle_a < PTZ_HORI_MAX_SPEED * PTZ_HORI_BIG_GEAR_RATIO * 12) )
{//只有有一次转动正常，说明转动就是正常的
hori_rotate_fault_num = 0;
//电机转动没有故障
g_ptz.fault_detect.hori_rotate_fault = NO_FAULT;
}
else//转动异常情况
{
if(g_ptz.hori_pid.PidUT_float >= PTZ_HORI_VOLTAGE_LEVEL)
{
hori_rotate_fault_num ++;
}
}
}
else//g_ptz.hori_direction_actual == PTZ_HORI_DIR_LEFT
{
//转动正常情况
if((hori_angle_b < hori_angle_a && //转动距离要大于最小速度转动距离0.5倍,要小于最大转速转动距离的2倍
hori_angle_a - hori_angle_b > PTZ_HORI_MIN_SPEED * PTZ_HORI_BIG_GEAR_RATIO * 2 &&
hori_angle_a - hori_angle_b < PTZ_HORI_MAX_SPEED * PTZ_HORI_BIG_GEAR_RATIO * 12)
||
(hori_angle_a < hori_angle_b &&
hori_angle_a + g_ptz.hori_as5047d.as5047d_ptz_angle_max - hori_angle_b > PTZ_HORI_MIN_SPEED * PTZ_HORI_BIG_GEAR_RATIO * 2 &&
hori_angle_a + g_ptz.hori_as5047d.as5047d_ptz_angle_max - hori_angle_b < PTZ_HORI_MAX_SPEED * PTZ_HORI_BIG_GEAR_RATIO * 12) )
{//只有有一次转动正常，说明转动就是正常的
hori_rotate_fault_num = 0;
//电机转动没有故障
g_ptz.fault_detect.hori_rotate_fault = NO_FAULT;
}
else//转动异常情况
{
if(g_ptz.hori_pid.PidUT_float >= PTZ_HORI_VOLTAGE_LEVEL)
{
hori_rotate_fault_num ++;
}
}
}
}
else
{//如果水平自检没完成
if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_RIGHT)
{
//转动正常情况
if(hori_angle_b > hori_angle_a &&
hori_angle_b - hori_angle_a >= PTZ_HORI_MIN_SPEED * PTZ_HORI_BIG_GEAR_RATIO * 2)
{//只有有一次转动正常，说明转动就是正常的
hori_rotate_fault_num = 0;
//电机转动没有故障
g_ptz.fault_detect.hori_rotate_fault = NO_FAULT;
}
else//转动异常情况
{
if(g_ptz.hori_pid.PidUT_float >= PTZ_HORI_VOLTAGE_LEVEL)
{
hori_rotate_fault_num ++;
}
}
}
else//g_ptz.hori_direction_actual == PTZ_HORI_DIR_LEFT
{
//转动正常情况
if(hori_angle_a > hori_angle_b &&
hori_angle_a - hori_angle_b >= PTZ_HORI_MIN_SPEED * PTZ_HORI_BIG_GEAR_RATIO * 2)
{//只有有一次转动正常，说明转动就是正常的
hori_rotate_fault_num = 0;
//电机转动没有故障
g_ptz.fault_detect.hori_rotate_fault = NO_FAULT;
}
else//转动异常情况
{
if(g_ptz.hori_pid.PidUT_float >= PTZ_HORI_VOLTAGE_LEVEL)
{
hori_rotate_fault_num ++;
}
}
}
}
if(hori_rotate_fault_num >= ROTATE_FAULT_MAX_NUM)//连续转动异常，则转动异常
{
hori_rotate_fault_num = 0;
//关闭所有云台自动扫描模式
g_preset_bit_scan.state = PRESET_BIT_SCAN_CLOSE_A;
g_area_scan.state = AREA_SCAN_CLOSE_A;
//云台直接刹车
// ptz_hori_stop(0);
// ptz_vert_stop(0);
ptz_hori_stop(PTZ_HORI_STOP_TIME);
ptz_vert_stop(PTZ_VERT_STOP_TIME);
//电机转动故障
g_ptz.fault_detect.hori_rotate_fault = FAULT;
}
hori_angle_a = hori_angle_b;
break;
}
return 1;
}
#endif
//故障代码自动回传
static char ptz_fault_return_task()

14
APP/Service/service_statusmonitor.h
View File

@ -233,6 +233,20 @@
#define PTZ_CURRENT_DETECT 6.0//5.0
#endif
//伺服电机
#ifdef PTZ_MEDIUM_WORM_SERVO_MOTOR_24V
//工作温度阈值
#define PTZ_TEMP_DETECT_MAX 65.0 //70.0
#define PTZ_TEMP_DETECT_MIN -25.0//70.0
//工作电压阈值
#define PTZ_VOLT_DETECT_MAX 30.0//30.0
#define PTZ_VOLT_DETECT_MIN 9.0//30.0
//工作电流阈值
#define PTZ_CURRENT_DETECT 10.0//5.0
#endif
///需要判断能否执行的指令

325
BSP/Driver/as5047d/as5047d.c
View File

@ -1,10 +1,14 @@
#include "as5047d.h"
#include <includes.h>
#include <stdint.h>
#include <stdio.h>
#include <stdbool.h>
#ifdef AS5047
static BSP_OS_SEM ptz_hori_get_angle_mutex;//¹²Ïí×ÊÔ´Ëø
static BSP_OS_SEM ptz_vert_get_angle_mutex;
static void as5047d_delay_nop(int as5047d_delay_time);
static unsigned int as5047d_Parity_bit_even(unsigned int package);
/// AS5047D¶ÌÑÓʱº¯Êý
@ -959,7 +963,326 @@ float as5047d_vert_get_angle_a()
return -1;
}
#endif
/********************************************************************************/
/********************************************************************************/
/********************************************************************************/
#ifdef TMR3109
//TMR3109 操作码定义
#define TMR3109_OPCODE_WRITE_EEPROM 0x01 //001 写EEPROM
#define TMR3109_OPCODE_WRITE_REGISTER 0x05 //101 写寄存器
#define TMR3109_OPCODE_READ_REGISTER 0x06 //110 读寄存器
#define TMR3109_OPCODE_CHANGE_MODE 0x07 //111 模式切换
#define TMR3109_OPCODE_READ_ANGLE 0x03 //011 读角度
//TMR3109设备结构体
typedef struct{
uint8_t device_id; //设备地址
uint32_t spi_periph; //SPI外设地址
uint32_t cs_pin; //片选引脚
uint32_t cs_gpio_periph; //片选引脚外设地址
}TMR3109_Device;
//角度数据结构体
typedef struct{
uint32_t angle_raw; //23位原始角度值
float angle_degree; //角度值，单位：度
uint8_t crc; //CRC校验码
uint8_t error; //错误码
bool is_valid; //是否有效
}TMR3109_AngleData;
//TMR3109设备定义
TMR3109_Device tmr3109_dev1 = {
.device_id = 1,
.spi_periph = SPI2,
.cs_pin = GPIO_PIN_15,
.cs_gpio_periph = GPIOA,
};
TMR3109_Device tmr3109_dev2 = {
.device_id = 2,
.spi_periph = SPI4,
.cs_pin = GPIO_PIN_11,
.cs_gpio_periph = GPIOE,
};
TMR3109_AngleData hori_angle_data = {0};
TMR3109_AngleData vert_angle_data = {0};
//暂定微秒级延迟函数(基于SysTick)(后期删除，所有延迟函数更换为统一的函数)
void delay_us(uint32_t us)
{
uint32_t start, now, delta, reload, us_tick;
start = SysTick->VAL;
reload = SysTick->LOAD;
us_tick = SystemCoreClock / 1000000UL;
do {
now = SysTick->VAL;
delta = start > now? start - now : reload + start - now;
} while (delta < us_tick * us);
}
//TMR3109初始化
void TMR3109_Init(void)
{
//初始化SPI2（U33编码器）
rcu_periph_clock_enable(RCU_GPIOA);
rcu_periph_clock_enable(RCU_GPIOC);
rcu_periph_clock_enable(RCU_SPI2);
//配置SPI2引脚
gpio_af_set(GPIOC, GPIO_AF_6, GPIO_PIN_10 | GPIO_PIN_11 | GPIO_PIN_12); //PC10,11,12复用为SPI2
gpio_mode_set(GPIOC, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO_PIN_10 | GPIO_PIN_11 | GPIO_PIN_12);
gpio_output_options_set(GPIOC, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_10 | /*GPIO_PIN_11 | */GPIO_PIN_12); //注意一下MISO是输入模式，这里可能会有问题
//配置CS引脚
gpio_mode_set(GPIOA, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO_PIN_15);
gpio_output_options_set(GPIOA, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_15);
gpio_bit_set(GPIOA, GPIO_PIN_15); //默认CS引脚高电平
//配置SPI2参数（不超过8MHz，暂定5MHz左右）
spi_parameter_struct spi_init_struct;
spi_struct_para_init(&spi_init_struct);
spi_init_struct.trans_mode = SPI_TRANSMODE_FULLDUPLEX;
spi_init_struct.device_mode = SPI_MASTER;
spi_init_struct.frame_size = SPI_FRAMESIZE_16BIT; //16位帧长
spi_init_struct.clock_polarity_phase = SPI_CK_PL_LOW_PH_2EDGE; //时钟极性为低，相位为第二边缘(下降沿)
spi_init_struct.nss = SPI_NSS_SOFT; //软件控制
spi_init_struct.prescale = SPI_PSC_16; //时钟预分频系数为16(5MHz左右)
spi_init_struct.endian = SPI_ENDIAN_MSB; //高位先出
spi_init(SPI2, &spi_init_struct);
spi_enable(SPI2);
//初始化SPI4（U32编码器）
rcu_periph_clock_enable(RCU_GPIOE);
rcu_periph_clock_enable(RCU_SPI4);
//配置SPI4引脚
gpio_af_set(GPIOE, GPIO_AF_6, GPIO_PIN_12 | GPIO_PIN_13 | GPIO_PIN_14); //PE12,13,14复用为SPI4
gpio_mode_set(GPIOE, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO_PIN_12 | GPIO_PIN_13 | GPIO_PIN_14);
gpio_output_options_set(GPIOE, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_12 | GPIO_PIN_13 | GPIO_PIN_14); //注意一下MISO是输入模式，这里可能会有问题
//配置CS引脚
gpio_mode_set(GPIOE, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO_PIN_11);
gpio_output_options_set(GPIOE, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_11);
gpio_bit_set(GPIOE, GPIO_PIN_11); //默认CS引脚高电平
//配置SPI4参数（不超过8MHz，暂定5MHz左右）
spi_parameter_struct spi_init_struct2;
spi_struct_para_init(&spi_init_struct2);
spi_init_struct2.trans_mode = SPI_TRANSMODE_FULLDUPLEX;
spi_init_struct2.device_mode = SPI_MASTER;
spi_init_struct2.frame_size = SPI_FRAMESIZE_16BIT; //16位帧长
spi_init_struct2.clock_polarity_phase = SPI_CK_PL_LOW_PH_2EDGE; //时钟极性为低，相位为第二边缘(下降沿)
spi_init_struct2.nss = SPI_NSS_SOFT; //软件控制
spi_init_struct2.prescale = SPI_PSC_16; //时钟预分频系数为16(5MHz左右)
spi_init_struct2.endian = SPI_ENDIAN_MSB; //高位先出
spi_init(SPI4, &spi_init_struct2);
spi_enable(SPI4);
//初始化后延迟
}
void as5047d_init()
{
TMR3109_Init();
}
// SPI发送接收32位数据(GD32的SPI只支持8位或16位传输，所以需要分两次发送)
static uint32_t spi_transfer_32bit(uint32_t spi_periph, uint32_t data)
{
uint16_t temp_high, temp_low;
uint32_t received_data = 0;
uint32_t start_tick;
const uint32_t timeout_ticks = 10; // 10ms超时
// 提取要发送的32位数据的高16位和低16位
temp_high = (uint16_t)(data >> 16); // 高16位
temp_low = (uint16_t)(data & 0xFFFF); // 低16位
// 1. 先发送高16位，并读取高16位响应
start_tick = OSTimeGet();
while(RESET == spi_i2s_flag_get(spi_periph, SPI_FLAG_TBE));
{
//超时判断
if (OSTimeGet() - start_tick > timeout_ticks) {
// 根据SPI外设判断使用哪个片选引脚
if (spi_periph == SPI2) {
gpio_bit_set(GPIOA, GPIO_PIN_15); // tmr3109_dev1 CS拉高
} else if (spi_periph == SPI4) {
gpio_bit_set(GPIOE, GPIO_PIN_11); // tmr3109_dev2 CS拉高
}
return 0xFFFFFFFF;
}
}
spi_i2s_data_transmit(spi_periph, temp_high);
start_tick = OSTimeGet();
while(RESET == spi_i2s_flag_get(spi_periph, SPI_FLAG_RBNE));
{
if (OSTimeGet() - start_tick > timeout_ticks) {
if (spi_periph == SPI2) {
gpio_bit_set(GPIOA, GPIO_PIN_15);
} else if (spi_periph == SPI4) {
gpio_bit_set(GPIOE, GPIO_PIN_11);
}
return 0xFFFFFFFF;
}
}
received_data = (uint32_t)spi_i2s_data_receive(spi_periph) << 16; // 存储到高16位
// 2. 紧接着发送低16位，并读取低16位响应
start_tick = OSTimeGet();
while(RESET == spi_i2s_flag_get(spi_periph, SPI_FLAG_TBE));
{
if (OSTimeGet() - start_tick > timeout_ticks) {
if (spi_periph == SPI2) {
gpio_bit_set(GPIOA, GPIO_PIN_15);
} else if (spi_periph == SPI4) {
gpio_bit_set(GPIOE, GPIO_PIN_11);
}
return 0xFFFFFFFF;
}
}
spi_i2s_data_transmit(spi_periph, temp_low);
start_tick = OSTimeGet();
while(RESET == spi_i2s_flag_get(spi_periph, SPI_FLAG_RBNE));
{
if (OSTimeGet() - start_tick > timeout_ticks) {
if (spi_periph == SPI2) {
gpio_bit_set(GPIOA, GPIO_PIN_15);
} else if (spi_periph == SPI4) {
gpio_bit_set(GPIOE, GPIO_PIN_11);
}
return 0xFFFFFFFF;
}
}
received_data |= spi_i2s_data_receive(spi_periph); // 存储到低16位
return received_data;
}
//设置CS引脚
static void set_cs(TMR3109_Device* dev, bool state)
{
if(state)
{
gpio_bit_set(dev->cs_gpio_periph, dev->cs_pin);
}
else
{
gpio_bit_reset(dev->cs_gpio_periph, dev->cs_pin);
}
}
/**
* @brief 计算TMR3109角度数据的CRC4校验码
* @param angle_data 23位角度数据
* @return 4位CRC校验码
*/
uint8_t TMR3109_CalculateCRC(uint32_t angle_data_23bit)
{
// 多项式: x^4 + x^3 + x^2 + 1 = 0b1101 = 0x0D
const uint8_t polynomial = 0x0D;
// 初始值: 0b0011 = 0x3
uint8_t crc = 0x3;
// 构建24位数据: 1位0 + 23位角度值
uint32_t data = angle_data_23bit; // 低23位是角度值
// 最高位已经是0（因为angle_data_23bit只有23位）
// 处理24位数据（从最高位开始）
for (int i = 23; i >= 0; i--) {
// 检查CRC最高位
uint8_t crc_msb = (crc >> 3) & 0x1;
// 获取当前数据位
uint8_t data_bit = (data >> i) & 0x1;
// CRC左移1位，最低位补0
crc = (crc << 1) & 0x0F;
// 如果CRC最高位与数据位异或结果为1，则与多项式异或
if (crc_msb ^ data_bit) {
crc ^= polynomial;
}
}
return crc & 0x0F; // 确保返回4位
}
/**
* @brief 验证TMR3109角度数据的CRC
* @param angle_data_23bit 23位角度数据
* @param received_crc 接收到的4位CRC
* @return true表示CRC校验通过
*/
bool TMR3109_CheckCRC(uint32_t angle_data_23bit, uint8_t received_crc)
{
uint8_t calculated_crc = TMR3109_CalculateCRC(angle_data_23bit);
return (calculated_crc == received_crc);
}
//读角度
bool TMR3109_ReadAngle(TMR3109_Device* dev, TMR3109_AngleData* angle_data)
{
if(dev == NULL || angle_data == NULL) return false;
//读角度命令： 操作码（3bit）+地址（8bit）+空闲（5bit）+数据（16bit）
uint32_t tx_data = (TMR3109_OPCODE_READ_ANGLE << 29)| (0X00 << 21) | (0X00 << 16) | (0X00 << 0) ;
set_cs(dev, false); //使能CS
delay_us(20); //SPI启动时间 优化点
//发送32位数据并接收响应
uint32_t rx_data = spi_transfer_32bit(dev->spi_periph, tx_data);
set_cs(dev, true); //拉高片选
//解析响应数据
//d27~d5: 23位角度数据，d4~d1: CRC校验码，d0: error位
angle_data->angle_raw = (rx_data >> 5) & 0X7FFFFF; //取23位角度数据
angle_data->crc = (rx_data >> 1) & 0X0F; //取4位CRC校验码
angle_data->error = rx_data & 0X01; //取1位error位
//计算角度值((∑_{i=0}^{23} Angle<i> * 2^i) / 8388608 * 360)
angle_data->angle_degree = (angle_data->angle_raw * 360.0f) / 8388608.0f;
//数据有效性校验(加入CRC校验)
bool crc_valid = TMR3109_CheckCRC(angle_data->angle_raw, angle_data->crc);
// angle_data->is_valid = 0/*(angle_data->error == 0)*/; //注意这里的error位有问题，手册没找到error位的具体定义
angle_data->is_valid = crc_valid;
//CRC错误记录逻辑，后期添加
// if(!crc_valid) {
// //CRC校验失败，记录错误信息
// //...
// }
return angle_data->is_valid;
}
float as5047d_hori_get_angle_a()
{
if (TMR3109_ReadAngle(&tmr3109_dev1, &hori_angle_data)) {
return hori_angle_data.angle_degree;
}
return 0.0f;
}
float as5047d_vert_get_angle_a()
{
if (TMR3109_ReadAngle(&tmr3109_dev2, &vert_angle_data)) {
return vert_angle_data.angle_degree;
}
return 0.0f;
}
#endif

55
BSP/Driver/as5047d/as5047d.h
View File

@ -2,6 +2,13 @@
#define __AS5047D_H_
#include "gd32f4xx.h"
#define TMR3109 1
#if !(AS5047) && !(TMR3109)
#define AS5047
#endif
#ifdef AS5047
///水平AS5047D端口
///CSn
///将AS5047D_HORI的CSn引脚置为高电平
@ -163,7 +170,51 @@ float as5047d_vert_get_angle();
unsigned int as5047d_vert_cef_command();
float as5047d_hori_get_angle_a();
float as5047d_vert_get_angle_a();
unsigned int as5047d_vert_read_data_a();
unsigned int as5047d_hori_read_data_a();
#endif
#ifdef TMR3109
// #define AS5047D_DIR_FWD 1 //磁编码器正向转动
// #define AS5047D_DIR_REV 0 //磁编码器反向转动
#define AS5047D_DIR_FWD 0 //磁编码器正向转动
#define AS5047D_DIR_REV 1 //磁编码器反向转动
///用于保存对接收数据的判断结果
///
///用于保存对接收数据的判断结果，并且详细保存出错的具体原因
///_master表示as5047d对接收到的指令判断的结果，_slave表示K60对as5047d返回的数据包判断结果
typedef struct _As5047DJudgeReceivePackage_
{
///接收到的数据包
unsigned int package;
///k60对AS5047D回复的数据进行判断
///判断接收的数据包效验码是否正确,1错误，0正常
unsigned char parity_error_package;
///AS5047D对接收到的K60指令进行判断,指令有错时的回复结果
///数据Error flag（EF）位，
unsigned char error_flag_command;
///校验错误，1错误，0正常
unsigned char parity_error_command;
///命令无效，1错误，0正常
unsigned char command_invalid_command;
///帧错误，1错误，0正常
unsigned char framing_error_command;
///数据操作是否成功，0不成功，1成功.即判断写入或者读取数据是否成功
unsigned char Operation_Result;
}As5047DJudgeReceivePackage;
void as5047d_init();
float as5047d_hori_get_angle_a();
float as5047d_vert_get_angle_a();
#endif
#endif

8
BSP/Driver/servoMotor/servoMotor.c Normal file
View File

@ -0,0 +1,8 @@
#include "servoMotor.h"
void servoMotorInit(void)
{
}

8
BSP/Driver/servoMotor/servoMotor.h Normal file
View File

@ -0,0 +1,8 @@
#ifndef _DRIVER_SERVO_MOTOR_H_
#define _DRIVER_SERVO_MOTOR_H_
#include "gd32f4xx.h"
void servoMotorInit(void);
#endif

16
PROJECT/OS2.ewp
View File

@ -409,6 +409,7 @@
<state>$PROJ_DIR$\..\BSP\Driver\timer</state>
<state>$PROJ_DIR$\..\BSP\Driver\full_bridge</state>
<state>$PROJ_DIR$\..\APP\Service</state>
<state>$PROJ_DIR$\..\BSP\Driver\servoMotor</state>
</option>
<option>
<name>CCStdIncCheck</name>
@ -2422,6 +2423,12 @@
<file>
<name>$PROJ_DIR$\..\APP\Device\Device_rotate\rotate_plan.h</name>
</file>
<file>
<name>$PROJ_DIR$\..\APP\Device\Device_rotate\rotate_servo.c</name>
</file>
<file>
<name>$PROJ_DIR$\..\APP\Device\Device_rotate\rotate_servo.h</name>
</file>
<file>
<name>$PROJ_DIR$\..\APP\Device\Device_rotate\rotate_step.c</name>
</file>
@ -2744,6 +2751,9 @@
<file>
<name>$PROJ_DIR$\..\APP\Device\Device_speed\speed_to_bldc.h</name>
</file>
<file>
<name>$PROJ_DIR$\..\APP\Device\Device_speed\speed_to_servoMotor.c</name>
</file>
<file>
<name>$PROJ_DIR$\..\APP\Device\Device_speed\speed_to_step.c</name>
</file>
@ -2912,6 +2922,12 @@
<name>$PROJ_DIR$\..\BSP\Driver\ringqueue\ring_queue.h</name>
</file>
</group>
<group>
<name>servoMotor</name>
<file>
<name>$PROJ_DIR$\..\BSP\Driver\servoMotor\servoMotor.c</name>
</file>
</group>
<group>
<name>timer</name>
<file>

15
PROJECT/OS2.ewt
View File

@ -2999,6 +2999,12 @@
<file>
<name>$PROJ_DIR$\..\APP\Device\Device_rotate\rotate_plan.h</name>
</file>
<file>
<name>$PROJ_DIR$\..\APP\Device\Device_rotate\rotate_servo.c</name>
</file>
<file>
<name>$PROJ_DIR$\..\APP\Device\Device_rotate\rotate_servo.h</name>
</file>
<file>
<name>$PROJ_DIR$\..\APP\Device\Device_rotate\rotate_step.c</name>
</file>
@ -3014,6 +3020,9 @@
<file>
<name>$PROJ_DIR$\..\APP\Device\Device_speed\speed_to_bldc.h</name>
</file>
<file>
<name>$PROJ_DIR$\..\APP\Device\Device_speed\speed_to_servoMotor.c</name>
</file>
<file>
<name>$PROJ_DIR$\..\APP\Device\Device_speed\speed_to_step.c</name>
</file>
@ -3182,6 +3191,12 @@
<name>$PROJ_DIR$\..\BSP\Driver\ringqueue\ring_queue.h</name>
</file>
</group>
<group>
<name>servoMotor</name>
<file>
<name>$PROJ_DIR$\..\BSP\Driver\servoMotor\servoMotor.c</name>
</file>
</group>
<group>
<name>timer</name>
<file>