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

Compare commits

base: pengsx:fdba688de41c74192079fc78f7de2cbf8de055e1
Branches Tags
pengsx:master
pengsx:servomotorV0.4.1
pengsx:servoMotorV0.3.4
pengsx:servoMotorCommuV0.1
pengsx:servoMotorV0.0
pengsx:修改adc、i2c等故障监测
...
compare: pengsx:79b49fcc0bd7e8caa606a3893a0ff3920d59d5f3
Branches Tags
pengsx:servomotorV0.4.1
pengsx:master
pengsx:servoMotorV0.3.4
pengsx:servoMotorCommuV0.1
pengsx:servoMotorV0.0
pengsx:修改adc、i2c等故障监测

7 Commits
fdba688de4 ... 79b49fcc0b

Author SHA1 Message Date
起床就犯困 79b49fcc0b 修复电机通信bug 2025-10-18 13:35:20 +08:00
REASEARCHER\18383 e082c9f795 水平、垂直电机发送，接收，解析测试通过，准备写位置 2025-10-17 18:20:03 +08:00
REASEARCHER\18383 164ba15a43 修复bug，现在水平垂直发送数据，解析数据都没有问题 2025-10-17 16:38:15 +08:00
REASEARCHER\18383 165565934a 修复错误，现在水平接收电机返回的数据完全没有问题 2025-10-16 16:35:35 +08:00
REASEARCHER\18383 c30d8e8981 第一次合并后，水平电机收发测试通过（垂直没有测试） 2025-10-16 11:44:18 +08:00
REASEARCHER\18383 d5ee07278f Merge branch 'main' into servoMotorV0.3 
# Conflicts:
#	APP/Device/Device_speed/speed_to_servoMotor.c
#	BSP/Driver/servoMotor/motorCommu.c
第一次合并
 2025-10-16 10:52:20 +08:00
REASEARCHER\18383 a2bbae0271 发送接收修改完毕，等待测试 2025-10-16 10:30:36 +08:00
23 changed files with 1302 additions and 760 deletions
Show Stats Expand all files Collapse all files

6
APP/Appcfg/app.c
View File

@ -287,10 +287,10 @@ static void task_print()
static void task_print_task()
{
while(1)
{
while(1)
{
task_print();
}
}
}
/*!

206
APP/Appcfg/app_cfg.h
View File

@ -55,88 +55,208 @@
/// CE
#define TASK_TESTQUEUE_PRIO 55u
///*****************************************************************/
//
//#define TASK_GET_ANGLE_PRIO 21u
//#define TASK_GET_ANGLE_STK_SIZE 120u
//
//#define POWER_OFF_PRIO 17u//原来为7，后导致ping包有延时，造成网络阻塞，UCOSII系统卡死，故优先级低于TCPIP
//#define POWER_OFF_STK_SIZE 200u
//
//#define TASK_PS_PRIO 22u
//#define TASK_PS_STK_SIZE 80u
//
//#define TASK_HORI_ROATE_MONITOR_PRIO 23u
//#define TASK_HORI_ROATE_MONITOR_STK_SIZE 120u
//
//#define TASK_VERT_ROATE_MONITOR_PRIO 24u
//#define TASK_VERT_ROATE_MONITOR_STK_SIZE 120u
//
//#define TASK_ELECTRIC_STABLE_PRIO 25u
//#define TASK_ELECTRIC_STABLE_STK_SIZE 150u
//
//#define TASK_VERT_ELECTRIC_STABLE_PWM_PRIO 26u
//#define TASK_VERT_ELECTRIC_STABLE_PWM_SIZE 150u
//
//#define TASK_PTZ_UPDATE_RECE_DATA_PRIO 27u//接收升级数据
//#define TASK_PTZ_UPDATE_RECE_DATA_STK_SIZE 170u
//
//#define TASK_VERT_SELF_CHECK_PRIO 28u
//#define TASK_VERT_SELF_CHECK_STK_SIZE 180u
//
//#define TASK_HORI_SELF_CHECK_PRIO 29u
//#define TASK_HORI_SELF_CHECK_STK_SIZE 180u
//
//#define TASK_HORI_PID_PRIO 30u
//#define TASK_HORI_PID_STK_SIZE 150u
//
//#define TASK_VERT_DIRECTOR_SPEED_PWM_PRIO 31u
//#define TASK_VERT_DIRECTOR_SPEED_PWM_STK_SIZE 150u
//
//#define TASK_VERT_PID_PRIO 32u
//#define TASK_VERT_PID_STK_SIZE 150u
//
//#define TASK_FAULT_DETECT_PRIO 34u
//#define TASK_FAULT_DETECT_STK_SIZE 180u
///**/
//#define TASK_AREA_SCAN_PRIO 35u
//#define TASK_AREA_SCAN_STK_SIZE 120u
//
//#define TASK_PRESET_BIT_SCAN_PRIO 36u
//#define TASK_PRESET_BIT_SCAN_STK_SIZE 100u
//
//#define TASK_LISTEN_PTZ_SERVER_PRIO 37u//云台接收指令分析
//#define TASK_LISTEN_PTZ_SERVER_STK_SIZE 600
//
//#define TASK_PTZ_UPDATE_DATA_PROCESS_PRIO 39u//处理升级数据
//#define TASK_PTZ_UPDATE_DATA_PROCESS_STK_SIZE 200u
//
//#define TASK_PTZ_UART_485_LASER_PROCESS_PRIO 40u//处理串口数据
//#define TASK_PTZ_UART_485_LASER_PROCESS_STK_SIZE 150u
//
//#define TASK_PTZ_UART_485_DATA_PROCESS_PRIO 41u//处理串口数据
//#define TASK_PTZ_UART_485_DATA_PROCESS_STK_SIZE 300u
//
//#define TASK_PTZ_UART_422_DATA_PROCESS_PRIO 42u//处理串口数据
//#define TASK_PTZ_UART_422_DATA_PROCESS_STK_SIZE 300u
//
//#define TASK_PTZ_AUTO_RETURN_PRIO 43u//角度回传任务
//#define TASK_PTZ_AUTO_RETURN_STK_SIZE 180u
//
//#define TASK_PTZ_SPEED_RETURN_PRIO 44u//角度回传任务
//#define TASK_PTZ_SPEED_RETURN_STK_SIZE 180u
//
//#define TASK_PTZ_DATA_COLLECT_PRIO 45u
//#define TASK_PTZ_DATA_COLLECT_STK_SIZE 200u
//
//#define TASK_PTZ_HEAT_RESISTOR_PRIO 46u
//#define TASK_PTZ_HEAT_RESISTOR_STK_SIZE 60u
//
//#define TASK_PTZ_RESTORE_PRIO 47u
//#define TASK_PTZ_RESTORE_STK_SIZE 60u
//
//
//#define TASK_PTZ_ERROR_COUNT_PRIO 48u
//#define TASK_PTZ_ERROR_COUNT_STK_SIZE 200u
//
//
////打印任务堆栈等信息
//#define TASK_PTZ_TASK_PRINTF_PRIO 55u
//#define TASK_PTZ_TASK_PRINTF_STK_SIZE 200u
//
//
////解析水平电机返回的数据
//#define TASK_RECV_HORI_SERVO_PRIO 56u
//#define TASK_RECV_HORI_SERVO_STK_SIZE 200u
//
////解析俯仰电机返回的数据
//#define TASK_RECV_VERT_SERVO_PRIO 57u
//#define TASK_RECV_VERT_SERVO_STK_SIZE 200u
//
//
//
////#define ????????????????????_PRIO 55u
////#define ????????????????????_STK_SIZE 150u
///*******************************************************************************/
/*****************************************************************/
#define TASK_GET_ANGLE_PRIO 21u
#define TASK_GET_ANGLE_PRIO 22u
#define TASK_GET_ANGLE_STK_SIZE 120u
#define POWER_OFF_PRIO 17u//原来为7，后导致ping包有延时，造成网络阻塞，UCOSII系统卡死，故优先级低于TCPIP
#define POWER_OFF_STK_SIZE 200u
#define TASK_PS_PRIO 22u
#define TASK_PS_STK_SIZE 80u
#define TASK_HORI_ROATE_MONITOR_PRIO 23u
#define TASK_HORI_ROATE_MONITOR_STK_SIZE 120u
#define TASK_VERT_ROATE_MONITOR_PRIO 24u
#define TASK_VERT_ROATE_MONITOR_STK_SIZE 120u
/*********电机通讯协议任务优先级在此**********/
//解析水平电机返回的数据
#define TASK_RECV_HORI_SERVO_PRIO 18u
#define TASK_RECV_HORI_SERVO_STK_SIZE 200u
#define TASK_ELECTRIC_STABLE_PRIO 25u
#define TASK_ELECTRIC_STABLE_STK_SIZE 150u
//解析俯仰电机返回的数据
#define TASK_RECV_VERT_SERVO_PRIO 19u
#define TASK_RECV_VERT_SERVO_STK_SIZE 200u
#define TASK_VERT_ELECTRIC_STABLE_PWM_PRIO 26u
#define TASK_VERT_ELECTRIC_STABLE_PWM_SIZE 150u
#define TASK_PTZ_UPDATE_RECE_DATA_PRIO 27u//接收升级数据
#define TASK_PTZ_UPDATE_RECE_DATA_STK_SIZE 170u
#define TASK_VERT_SELF_CHECK_PRIO 28u
#define TASK_VERT_SELF_CHECK_STK_SIZE 180u
#define TASK_HORI_SELF_CHECK_PRIO 29u
#define TASK_HORI_SELF_CHECK_STK_SIZE 180u
#define TASK_HORI_PID_PRIO 30u
#define TASK_HORI_PID_PRIO 20u
#define TASK_HORI_PID_STK_SIZE 150u
#define TASK_VERT_DIRECTOR_SPEED_PWM_PRIO 31u
#define TASK_VERT_DIRECTOR_SPEED_PWM_STK_SIZE 150u
#define TASK_VERT_PID_PRIO 32u
#define TASK_VERT_PID_PRIO 21u
#define TASK_VERT_PID_STK_SIZE 150u
#define TASK_FAULT_DETECT_PRIO 34u
#define TASK_VERT_DIRECTOR_SPEED_PWM_PRIO 21u
#define TASK_VERT_DIRECTOR_SPEED_PWM_STK_SIZE 150u
/***************/
#define TASK_PS_PRIO 23u
#define TASK_PS_STK_SIZE 80u
#define TASK_HORI_ROATE_MONITOR_PRIO 24u
#define TASK_HORI_ROATE_MONITOR_STK_SIZE 120u
#define TASK_VERT_ROATE_MONITOR_PRIO 25u
#define TASK_VERT_ROATE_MONITOR_STK_SIZE 120u
#define TASK_ELECTRIC_STABLE_PRIO 26u
#define TASK_ELECTRIC_STABLE_STK_SIZE 150u
#define TASK_VERT_ELECTRIC_STABLE_PWM_PRIO 27u
#define TASK_VERT_ELECTRIC_STABLE_PWM_SIZE 150u
#define TASK_PTZ_UPDATE_RECE_DATA_PRIO 28u//接收升级数据
#define TASK_PTZ_UPDATE_RECE_DATA_STK_SIZE 170u
#define TASK_VERT_SELF_CHECK_PRIO 29u
#define TASK_VERT_SELF_CHECK_STK_SIZE 180u
#define TASK_HORI_SELF_CHECK_PRIO 32u
#define TASK_HORI_SELF_CHECK_STK_SIZE 180u
// #define TASK_VERT_PID_PRIO 34u
// #define TASK_VERT_PID_STK_SIZE 150u
#define TASK_FAULT_DETECT_PRIO 35u
#define TASK_FAULT_DETECT_STK_SIZE 180u
/**/
#define TASK_AREA_SCAN_PRIO 35u
#define TASK_AREA_SCAN_PRIO 36u
#define TASK_AREA_SCAN_STK_SIZE 120u
#define TASK_PRESET_BIT_SCAN_PRIO 36u
#define TASK_PRESET_BIT_SCAN_PRIO 37u
#define TASK_PRESET_BIT_SCAN_STK_SIZE 100u
#define TASK_LISTEN_PTZ_SERVER_PRIO 37u//云台接收指令分析
#define TASK_LISTEN_PTZ_SERVER_PRIO 39u//云台接收指令分析
#define TASK_LISTEN_PTZ_SERVER_STK_SIZE 600
#define TASK_PTZ_UPDATE_DATA_PROCESS_PRIO 39u//处理升级数据
#define TASK_PTZ_UPDATE_DATA_PROCESS_PRIO 40u//处理升级数据
#define TASK_PTZ_UPDATE_DATA_PROCESS_STK_SIZE 200u
#define TASK_PTZ_UART_485_LASER_PROCESS_PRIO 40u//处理串口数据
#define TASK_PTZ_UART_485_LASER_PROCESS_PRIO 41u//处理串口数据
#define TASK_PTZ_UART_485_LASER_PROCESS_STK_SIZE 150u
#define TASK_PTZ_UART_485_DATA_PROCESS_PRIO 41u//处理串口数据
#define TASK_PTZ_UART_485_DATA_PROCESS_PRIO 42u//处理串口数据
#define TASK_PTZ_UART_485_DATA_PROCESS_STK_SIZE 300u
#define TASK_PTZ_UART_422_DATA_PROCESS_PRIO 42u//处理串口数据
#define TASK_PTZ_UART_422_DATA_PROCESS_PRIO 43u//处理串口数据
#define TASK_PTZ_UART_422_DATA_PROCESS_STK_SIZE 300u
#define TASK_PTZ_AUTO_RETURN_PRIO 43u//角度回传任务
#define TASK_PTZ_AUTO_RETURN_PRIO 44u//角度回传任务
#define TASK_PTZ_AUTO_RETURN_STK_SIZE 180u
#define TASK_PTZ_SPEED_RETURN_PRIO 44u//角度回传任务
#define TASK_PTZ_SPEED_RETURN_PRIO 45u//角度回传任务
#define TASK_PTZ_SPEED_RETURN_STK_SIZE 180u
#define TASK_PTZ_DATA_COLLECT_PRIO 45u
#define TASK_PTZ_DATA_COLLECT_PRIO 46u
#define TASK_PTZ_DATA_COLLECT_STK_SIZE 200u
#define TASK_PTZ_HEAT_RESISTOR_PRIO 46u
#define TASK_PTZ_HEAT_RESISTOR_PRIO 47u
#define TASK_PTZ_HEAT_RESISTOR_STK_SIZE 60u
#define TASK_PTZ_RESTORE_PRIO 47u
#define TASK_PTZ_RESTORE_PRIO 48u
#define TASK_PTZ_RESTORE_STK_SIZE 60u
#define TASK_PTZ_ERROR_COUNT_PRIO 48u
#define TASK_PTZ_ERROR_COUNT_PRIO 49u
#define TASK_PTZ_ERROR_COUNT_STK_SIZE 200u
@ -145,13 +265,7 @@
#define TASK_PTZ_TASK_PRINTF_STK_SIZE 200u
//解析水平电机返回的数据
#define TASK_RECV_HORI_SERVO_PRIO 56u
#define TASK_RECV_HORI_SERVO_STK_SIZE 200u
//解析俯仰电机返回的数据
#define TASK_RECV_VERT_SERVO_PRIO 57u
#define TASK_RECV_VERT_SERVO_STK_SIZE 200u

2
APP/Appcfg/os_cfg.h
View File

@ -135,6 +135,6 @@
#define OS_TMR_CFG_MAX 12u /* Maximum number of timers */
#define OS_TMR_CFG_NAME_EN 1u /* Determine timer names */
#define OS_TMR_CFG_WHEEL_SIZE 8u /* Size of timer wheel (#Spokes) */
#define OS_TMR_CFG_TICKS_PER_SEC 1u /* 1s Rate at which timer management task runs (Hz) */
#define OS_TMR_CFG_TICKS_PER_SEC 5u /* 1s Rate at which timer management task runs (Hz) */
#endif

2
APP/Common/ptz_default_value.h
View File

@ -537,7 +537,7 @@
#define PTZ_HORI_MOTOR_DIRECTION 1
///水平电机加速度时间常数 单位ms (0-->>1000rpm)
#define PTZ_HORI_MOTOR_AccelerationTimeConstant 3000
///水平电机加速度时间常数 单位ms (1000-->>0rpm)
///水平电机减速度时间常数 单位ms (1000-->>0rpm)
#define PTZ_HORI_MOTOR_DecelerationTimeConstant 2000

170
APP/Device/Device_rotate/rotate_servo.c
View File

@ -1,12 +1,14 @@
#include "rotate_servo.h"
#include "speed_to_servoMotor.h"
#ifdef PTZ_MEDIUM_WORM_SERVO_MOTOR_24V
static BSP_OS_SEM ptz_hori_stop_mutex;//共享资源锁
static BSP_OS_SEM ptz_vert_stop_mutex;
static char ptz_hori_stop_count;//水平停止计数，防止多次停止多次延时
static char ptz_vert_stop_count;//垂直停止计数，防止多次停止多次延时
float ptz_vert_break_angle()
{
@ -49,10 +51,83 @@ void ptz_sem_post_stop_mutex()
BSP_OS_SemPost(&ptz_vert_stop_mutex);
}
void ptz_hori_start(char direction, float speed)
/*!
\brief 设置伺服电机速度
\param[in] data：储存发送数据的链表
\param[out] none
\retval none
*/
void set_speed_to_servoMotor(uint8_t motorType, float speed)
{
//转换为电机端的r/min
speed = speed * PTZ_HORI_RATIO + 0.5;
if (motorType == horiMotorType) {
if (speed > PTZ_HORI_MOTOR_MAX_SPEED) {
speed = PTZ_HORI_MOTOR_MAX_SPEED;
}
else if (speed < PTZ_HORI_MIN_SPEED) {
speed = PTZ_HORI_MOTOR_MIN_SPEED;
}
//设置速度，电机的r/min范围-6000~6000
servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, (uint16_t)speed)
, WRITE_ONE_REG_FRAME_NUM, lowPriority);
}
else if (motorType == vertMotorType) {
if (speed > PTZ_VERT_MOTOR_MAX_SPEED) {
speed = PTZ_VERT_MOTOR_MAX_SPEED;
}
else if (speed < PTZ_VERT_MOTOR_MIN_SPEED) {
speed = PTZ_VERT_MOTOR_MIN_SPEED;
}
//设置速度，电机的r/min范围-6000~6000
servoSendData(vertMotorType, WriteMotorOneReg(V_MOTOR, H06_SPEED_COMMU_SET_VALUE, (uint16_t)speed)
, WRITE_ONE_REG_FRAME_NUM, lowPriority);
}
}
/*
///云台水平右转
#define PTZ_HORI_DIR_RIGHT 1
///云台水平左转
#define PTZ_HORI_DIR_LEFT 3//0
///云台处于停止状态
#define PTZ_HORI_DIR_STOP 2
*/
void ptz_hori_start(char direction, float speed)//输入参数的speed是云台末端的r/min
{
BSP_OS_SemWait(&ptz_hori_stop_mutex, 0u);
switch ( direction )
{
case PTZ_HORI_DIR_RIGHT://伺服电机默认速度为正使云台右转
case PTZ_HORI_DIR_STOP:
break;
case PTZ_HORI_DIR_LEFT:
speed = -speed;
default:
break;
}
/*
-------------------------------------add speed change to here--------------------------------------
*/
//设定转动速度
// servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, speed)//设置速度，电机的r/min范围-6000~6000
// , WRITE_ONE_REG_FRAME_NUM, lowPriority);
set_speed_to_servoMotor(horiMotorType, speed);
servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 1)
, WRITE_ONE_REG_FRAME_NUM, lowPriority); //使能电机
g_ptz.hori_speed_set = speed;
//设定转动方向
g_ptz.hori_direction_last = g_ptz.hori_direction_set;
g_ptz.hori_direction_set = direction;
g_ptz.hori_direction_actual = g_ptz.hori_direction_set;
//启动电机
g_ptz.hori_start_stop_set = PTZ_HORI_START;
g_ptz.hori_start_stop_actual = g_ptz.hori_start_stop_set;
ptz_hori_stop_count = 0;
BSP_OS_SemPost(&ptz_hori_stop_mutex);
}
@ -60,13 +135,68 @@ void ptz_hori_stop(unsigned short int time)
{
BSP_OS_SemWait(&ptz_hori_stop_mutex, 0u);
//停止电机
g_ptz.hori_start_stop_set = PTZ_HORI_STOP;
g_ptz.hori_start_stop_actual = g_ptz.hori_start_stop_set;
g_ptz.hori_direction_actual = PTZ_HORI_DIR_STOP;
//设定转动速度
// servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, 0)//设置速度
// , WRITE_ONE_REG_FRAME_NUM, lowPriority);
set_speed_to_servoMotor(horiMotorType, 0);
g_ptz.hori_speed_set = 0;
g_ptz.hori_speed_actual = 0;
if(ptz_hori_stop_count <= 0)
{
OSTimeDlyHMSM(0u, 0u, 0u, time);
ptz_hori_stop_count = 0;
}
ptz_hori_stop_count ++;
servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 0)
, WRITE_ONE_REG_FRAME_NUM, highPriority); //失能电机
//电子稳定
#ifdef PTZ_ELECTRIC_STABLE_L6235D
ptz_hori_electric_stable_init();
#endif
BSP_OS_SemPost(&ptz_hori_stop_mutex);
}
void ptz_vert_start(char direction, float speed)
void ptz_vert_start(char direction, float speed)//输入参数的speed是云台末端的r/min
{
BSP_OS_SemWait(&ptz_vert_stop_mutex, 0u);
switch ( direction )
{
case PTZ_VERT_DIR_UP://伺服电机默认速度为正使云台向上
case PTZ_VERT_DIR_DOWN:
break;
case PTZ_VERT_DIR_STOP:
speed = -speed;
default:
break;
}
/*
-------------------------------------add speed change to here--------------------------------------
*/
//设定转动速度
set_speed_to_servoMotor(vertMotorType, speed);
servoSendData(vertMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 1)
, WRITE_ONE_REG_FRAME_NUM, lowPriority); //使能电机
g_ptz.hori_speed_set = speed;
//设定转动方向
g_ptz.hori_direction_last = g_ptz.hori_direction_set;
g_ptz.hori_direction_set = direction;
g_ptz.hori_direction_actual = g_ptz.hori_direction_set;
//启动电机
g_ptz.hori_start_stop_set = PTZ_HORI_START;
g_ptz.hori_start_stop_actual = g_ptz.hori_start_stop_set;
ptz_vert_stop_count = 0;
BSP_OS_SemPost(&ptz_vert_stop_mutex);
}
@ -74,6 +204,32 @@ void ptz_vert_stop(unsigned short int time)
{
BSP_OS_SemWait(&ptz_vert_stop_mutex, 0u);
//停止电机
g_ptz.vert_start_stop_set = PTZ_VERT_STOP;
g_ptz.vert_start_stop_actual = g_ptz.vert_start_stop_set;
g_ptz.vert_direction_actual = PTZ_VERT_DIR_STOP;
//设定转动速度
// servoSendData(vertMotorType, WriteMotorOneReg(V_MOTOR, H06_SPEED_COMMU_SET_VALUE, 0)//设置速度
// , WRITE_ONE_REG_FRAME_NUM, lowPriority);
set_speed_to_servoMotor(vertMotorType, 0);
g_ptz.vert_speed_set = 0;
g_ptz.vert_speed_actual = 0;
if(ptz_vert_stop_count <= 0)
{
OSTimeDlyHMSM(0u, 0u, 0u, time);
ptz_vert_stop_count = 0;
}
servoSendData(vertMotorType, WriteMotorOneReg(V_MOTOR, H03_DI1_LOGICAL_SELEC, 0)
, WRITE_ONE_REG_FRAME_NUM, highPriority); //失能电机
ptz_vert_stop_count ++;
//电子稳定
#ifdef PTZ_ELECTRIC_STABLE_L6235D
ptz_hori_electric_stable_init();
#endif
BSP_OS_SemPost(&ptz_vert_stop_mutex);
}
@ -93,7 +249,7 @@ static void ptz_hori_rotate_task()
while(1)
{
ptz_hori_rotate_monitor_task();
OSTimeDlyHMSM(0u, 0u, 0u, 1u);
OSTimeDlyHMSM(0u, 0u, 0u, 10u);
}
}
@ -103,7 +259,7 @@ static void ptz_vert_rotate_task()
while(1)
{
ptz_vert_rotate_monitor_task();
OSTimeDlyHMSM(0u, 0u, 0u, 1u);
OSTimeDlyHMSM(0u, 0u, 0u, 10u);
}
}
@ -161,6 +317,8 @@ static void creat_task_vert_rotate(void)
}
}
void init_rotate_monitor_module(void)
{
BSP_OS_SemCreate(&ptz_hori_stop_mutex,1u,"ptz_hori_stop_mutex");

2
APP/Device/Device_rotate/rotate_servo.h
View File

@ -48,6 +48,8 @@ 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 set_speed_to_servoMotor(uint8_t motorType, float speed);
void init_rotate_monitor_module(void);
#endif

261
APP/Device/Device_speed/servoMotor_recv.c
View File

@ -9,6 +9,237 @@
BSP_OS_SEM g_horiMotorMutex;
BSP_OS_SEM g_vertMotorMutex;
/**
* @brief 写单个寄存器成功后，电机会返回一帧与写格式完全一致的数据
* @param motorNo：写垂直电机还是水平电机
* @return false:错误，读出的内容与写入的内容不一致
*/
static bool MotorReplyForWrite(uint8_t motorNo)
{
static uint8_t motorReplybuff[WRITE_ONE_REG_FRAME_NUM];
if (motorNo == horiMotorType )
{
if ( highPriority == g_servoMotorLinkList.horiMotor.linkListPriority )
{
if ( g_servoMotorLinkList.horiMotor.LinkListHead_H->data[1] == WRITE_ONE_REG )//为写单个寄存器
{
/*提取数据失败*/
if ( CommuRsvData(motorNo, motorReplybuff, WRITE_ONE_REG_FRAME_NUM) == false )
{
return false;
}
if ( 0 != memcmp( motorReplybuff, g_servoMotorLinkList.horiMotor.LinkListHead_H->data, WRITE_ONE_REG_FRAME_NUM ) )
{
return false;
}
servoLinkListMemPut(horiMotorType);
}
else
{
return false;
}
}
else if ( lowPriority == g_servoMotorLinkList.horiMotor.linkListPriority )
{
if ( g_servoMotorLinkList.horiMotor.LinkListHead_L->data[1] == WRITE_ONE_REG )//为写单个寄存器
{
/*提取数据失败*/
if ( CommuRsvData(motorNo, motorReplybuff, WRITE_ONE_REG_FRAME_NUM) == false )
{
return false;
}
if ( 0 != memcmp( motorReplybuff, g_servoMotorLinkList.horiMotor.LinkListHead_L->data, WRITE_ONE_REG_FRAME_NUM ) )
{
return false;
}
servoLinkListMemPut(horiMotorType);
}
else
{
return false;
}
}
// servoLinkListMemPut(horiMotorType);
}
else if ( motorNo == vertMotorType )
{
if ( highPriority == g_servoMotorLinkList.vertMotor.linkListPriority )
{
if ( g_servoMotorLinkList.vertMotor.LinkListHead_H->data[1] == WRITE_ONE_REG )//为写单个寄存器
{
/*提取数据失败*/
if ( CommuRsvData(motorNo, motorReplybuff, WRITE_ONE_REG_FRAME_NUM) == false )
{
return false;
}
if ( 0 != memcmp( motorReplybuff, g_servoMotorLinkList.vertMotor.LinkListHead_H->data, WRITE_ONE_REG_FRAME_NUM ) )
{
return false;
}
servoLinkListMemPut(vertMotorType);
}
else
{
return false;
}
}
else if ( lowPriority == g_servoMotorLinkList.vertMotor.linkListPriority )
{
if ( g_servoMotorLinkList.vertMotor.LinkListHead_L->data[1] == WRITE_ONE_REG )//为写单个寄存器
{
/*提取数据失败*/
if ( CommuRsvData(motorNo, motorReplybuff, WRITE_ONE_REG_FRAME_NUM) == false )
{
return false;
}
if ( 0 != memcmp( motorReplybuff, g_servoMotorLinkList.vertMotor.LinkListHead_L->data, WRITE_ONE_REG_FRAME_NUM ) )
{
return false;
}
servoLinkListMemPut(vertMotorType);
}
else
{
return false;
}
}
// servoLinkListMemPut(vertMotorType);
}
return true;
}
/**
* @brief 读单个寄存器成功后，电机会返回一帧7字节数据
* @param motorNo：写垂直电机还是水平电机
* @param speed：获取读取命令后，电机返回的数据内容
* @return false:读取返回错误
*/
static bool MotorReplyForRead(uint8_t motorNo)
{
int16_t speed = 0;
static uint8_t motorReplybuff[READ_ONE_REG_FRAME_NUM];
if (motorNo == horiMotorType )
{
if ( highPriority == g_servoMotorLinkList.horiMotor.linkListPriority )
{
if ( g_servoMotorLinkList.horiMotor.LinkListHead_H->data[1] == READ_ONE_REG )//为读单个寄存器
{
/*提取数据失败*/
if ( CommuRsvData(motorNo, motorReplybuff, READ_ONE_REG_FRAME_NUM-1) == false )
{
return false;
}
if ( motorReplybuff[0] != H_MOTOR_ADDR )return false;//从机地址不对
if ( motorReplybuff[1] != READ_ONE_REG )return false;//功能码不对
if ( motorReplybuff[2] != 0x02 )return false;//返回数据的长度不对
speed = ((motorReplybuff[3]) << 8 ) | motorReplybuff[4];
if (speed < 0)
{
speed = -speed;
}
g_ptz.hori_speed_actual = (float)speed / PTZ_HORI_RATIO;
servoLinkListMemPut(horiMotorType);
}
else
{
return false;
}
}
else if ( lowPriority == g_servoMotorLinkList.horiMotor.linkListPriority )
{
if ( g_servoMotorLinkList.horiMotor.LinkListHead_L->data[1] == READ_ONE_REG )//为读单个寄存器
{
/*提取数据失败*/
if ( CommuRsvData(motorNo, motorReplybuff, READ_ONE_REG_FRAME_NUM-1) == false )
{
return false;
}
if ( motorReplybuff[0] != H_MOTOR_ADDR )return false;//从机地址不对
if ( motorReplybuff[1] != READ_ONE_REG )return false;//功能码不对
if ( motorReplybuff[2] != 0x02 )return false;//返回数据的长度不对
speed = ((motorReplybuff[3]) << 8 ) | motorReplybuff[4];
if (speed < 0)
{
speed = -speed;
}
g_ptz.hori_speed_actual = (float)speed / PTZ_HORI_RATIO;
servoLinkListMemPut(horiMotorType);
}
else
{
return false;
}
}
// servoLinkListMemPut(horiMotorType);
}
else if ( motorNo == vertMotorType )
{
if ( highPriority == g_servoMotorLinkList.vertMotor.linkListPriority )
{
if ( g_servoMotorLinkList.vertMotor.LinkListHead_H->data[1] == READ_ONE_REG )//为读单个寄存器
{
/*提取数据失败*/
if ( CommuRsvData(motorNo, motorReplybuff, READ_ONE_REG_FRAME_NUM-1) == false )
{
return false;
}
if ( motorReplybuff[0] != V_MOTOR_ADDR )return false;//从机地址不对
if ( motorReplybuff[1] != READ_ONE_REG )return false;//功能码不对
if ( motorReplybuff[2] != 0x02 )return false;//返回数据的长度不对
speed = ((motorReplybuff[3]) << 8 ) | motorReplybuff[4];
if (speed < 0)
{
speed = -speed;
}
g_ptz.vert_speed_actual = (float)speed / PTZ_VERT_RATIO;
servoLinkListMemPut(vertMotorType);
}
else
{
return false;
}
}
else if ( lowPriority == g_servoMotorLinkList.vertMotor.linkListPriority )
{
if ( g_servoMotorLinkList.vertMotor.LinkListHead_L->data[1] == READ_ONE_REG )//为读单个寄存器
{
/*提取数据失败*/
if ( CommuRsvData(motorNo, motorReplybuff, READ_ONE_REG_FRAME_NUM-1) == false )
{
return false;
}
if ( motorReplybuff[0] != V_MOTOR_ADDR )return false;//从机地址不对
if ( motorReplybuff[1] != READ_ONE_REG )return false;//功能码不对
if ( motorReplybuff[2] != 0x02 )return false;//返回数据的长度不对
speed = ((motorReplybuff[3]) << 8 ) | motorReplybuff[4];
if (speed < 0)
{
speed = -speed;
}
g_ptz.vert_speed_actual = (float)speed / PTZ_VERT_RATIO;
servoLinkListMemPut(vertMotorType);
}
else
{
return false;
}
}
// servoLinkListMemPut(vertMotorType);
}
return true;
}
/*!
\brief 水平电机接收任务
\param[in] none
@ -18,13 +249,17 @@ BSP_OS_SEM g_vertMotorMutex;
static void ptz_recv_hori_servo_task()
{
CPU_INT08U err;
while(1) {
OSSemPend(g_horiMotorMutex, 0, &err);
stopTimeOut(H_MOTOR);
if ( ( MotorReplyForWrite(H_MOTOR) | MotorReplyForRead(H_MOTOR) ) == false)
{
// H_MOTOR_STOP;
continue;
}
// OSTimeDlyHMSM(0u, 0u, 0u, 5u);
//释放信号量，通知能发送一次
OSSemPost(g_horiSpeedMutex);
}
}
@ -37,12 +272,17 @@ static void ptz_recv_hori_servo_task()
static void ptz_recv_vert_servo_task()
{
CPU_INT08U err;
while(1) {
OSSemPend(g_vertMotorMutex, 0, &err);
stopTimeOut(V_MOTOR);
if ( ( MotorReplyForWrite(V_MOTOR) | MotorReplyForRead(V_MOTOR) ) == false)
{
// V_MOTOR_STOP;
continue;
}
//释放信号量，通知能发送一次
OSSemPost(g_vertSpeedMutex);
}
}
@ -95,8 +335,11 @@ static void creat_task_servo_recv_task(void)
void Init_ServoMotorRecv(void)
{
CPU_INT08U err;
g_horiMotorMutex = OSSemCreate(1);
g_vertMotorMutex = OSSemCreate(1);
OSSemPend(g_horiMotorMutex, 1, &err);
OSSemPend(g_vertMotorMutex, 1, &err);
creat_task_servo_recv_task();
}

144
APP/Device/Device_speed/speed_to_servoMotor.c
View File

@ -59,12 +59,12 @@ void ptz_send_speed(char dev, char speed)
\param[out] none
\retval none
*/
void servoHoriLinkListMemPut(uint8_t motorType)
void servoLinkListMemPut(uint8_t motorType)
{
//水平电机
if (motorType == horiMotorType) {
//当前发送完成的数据为高优先级链表中的数据
if (g_servoMotorLinkList.horiMotor.linkListNum == highPriority) {
if (g_servoMotorLinkList.horiMotor.linkListPriority == highPriority) {
if (g_servoMotorLinkList.horiMotor.LinkListHead_H != NULL) {
linkList *ptr;
ptr = g_servoMotorLinkList.horiMotor.LinkListHead_H;
@ -74,12 +74,13 @@ void servoHoriLinkListMemPut(uint8_t motorType)
else {
g_servoMotorLinkList.horiMotor.LinkListHead_H = NULL;
g_servoMotorLinkList.horiMotor.LinkListTail_H = NULL;
}
}
g_servoMotorLinkList.horiMotor.linkListNum--;
OSMemPut(g_memPtr, (uint8_t *)ptr);
}
}
//当前发送完成的数据为低优先级链表中的数据
else if (g_servoMotorLinkList.horiMotor.linkListNum == lowPriority) {
else if (g_servoMotorLinkList.horiMotor.linkListPriority == lowPriority) {
if (g_servoMotorLinkList.horiMotor.LinkListHead_L != NULL) {
linkList *ptr;
ptr = g_servoMotorLinkList.horiMotor.LinkListHead_L;
@ -89,13 +90,15 @@ void servoHoriLinkListMemPut(uint8_t motorType)
else {
g_servoMotorLinkList.horiMotor.LinkListHead_L = NULL;
g_servoMotorLinkList.horiMotor.LinkListTail_L = NULL;
}
}
g_servoMotorLinkList.horiMotor.linkListNum--;
OSMemPut(g_memPtr, (uint8_t *)ptr);
}
}
// g_servoMotorLinkList.horiMotor.linkListNum--;
}
else if (motorType == vertMotorType) {
if (g_servoMotorLinkList.vertMotor.linkListNum == highPriority) {
if (g_servoMotorLinkList.vertMotor.linkListPriority == highPriority) {
if (g_servoMotorLinkList.vertMotor.LinkListHead_H != NULL) {
linkList *ptr;
ptr = g_servoMotorLinkList.vertMotor.LinkListHead_H;
@ -106,10 +109,12 @@ void servoHoriLinkListMemPut(uint8_t motorType)
g_servoMotorLinkList.vertMotor.LinkListHead_H = NULL;
g_servoMotorLinkList.vertMotor.LinkListTail_H = NULL;
}
g_servoMotorLinkList.vertMotor.linkListNum--;
OSMemPut(g_memPtr, (uint8_t *)ptr);
}
}
else if (g_servoMotorLinkList.vertMotor.linkListNum == lowPriority) {
else if (g_servoMotorLinkList.vertMotor.linkListPriority == lowPriority) {
if (g_servoMotorLinkList.vertMotor.LinkListHead_L != NULL) {
linkList *ptr;
ptr = g_servoMotorLinkList.vertMotor.LinkListHead_L;
@ -120,10 +125,15 @@ void servoHoriLinkListMemPut(uint8_t motorType)
g_servoMotorLinkList.vertMotor.LinkListHead_L = NULL;
g_servoMotorLinkList.vertMotor.LinkListTail_L = NULL;
}
g_servoMotorLinkList.vertMotor.linkListNum--;
OSMemPut(g_memPtr, (uint8_t *)ptr);
}
}
// g_servoMotorLinkList.vertMotor.linkListNum--;
}
}
/*!
@ -137,19 +147,19 @@ void servoHoriLinkListMemPut(uint8_t motorType)
\param[out] none
\retval none
*/
void servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priority)
bool servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priority)
{
if ((motor != horiMotorType) && (motor!= vertMotorType)) {
return;
return false;
}
if (data == NULL) {
return;
return false;
}
if (dataLen > sendDataBufLen) {
return;
return false;
}
if ((priority != highPriority) && (priority != lowPriority)) {
return;
return false;
}
linkList *ptr = NULL;
@ -158,17 +168,25 @@ void servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priorit
//保存数据到链表节点
ptr = OSMemGet(g_memPtr, &err);
if (ptr == NULL) {
return;
return false;
}
ptr->length = dataLen;
memcpy(ptr->data, data, dataLen);
//将节点添加进入链表中
if (motor == horiMotorType) {
if (g_servoMotorLinkList.horiMotor.linkListNum > sendDataBufNumber) {
OSMemPut(g_memPtr, ptr);
return;
if (priority == highPriority) {
if (g_servoMotorLinkList.horiMotor.linkListNum >= sendDataBufNumber) {
OSMemPut(g_memPtr, ptr);
return false;
}
}
else {
if (g_servoMotorLinkList.horiMotor.linkListNum >= sendDataBufNumber - 1) {
OSMemPut(g_memPtr, ptr);
return false;
}
}
if (priority == highPriority) {
if (g_servoMotorLinkList.horiMotor.LinkListTail_H == NULL) {
@ -183,6 +201,7 @@ void servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priorit
g_servoMotorLinkList.horiMotor.LinkListHead_H
= g_servoMotorLinkList.horiMotor.LinkListTail_H;
}
g_servoMotorLinkList.horiMotor.linkListNum++;
}
else {
if (g_servoMotorLinkList.horiMotor.LinkListTail_L == NULL) {
@ -197,15 +216,24 @@ void servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priorit
g_servoMotorLinkList.horiMotor.LinkListHead_L
= g_servoMotorLinkList.horiMotor.LinkListTail_L;
}
g_servoMotorLinkList.horiMotor.linkListNum++;
}
//释放信号量，通知能发送一次
OSSemPost(g_horiSpeedSem);
}
else {
if (g_servoMotorLinkList.vertMotor.linkListNum > sendDataBufNumber) {
OSMemPut(g_memPtr, ptr);
return;
if (priority == highPriority) {
if (g_servoMotorLinkList.vertMotor.linkListNum >= sendDataBufNumber) {
OSMemPut(g_memPtr, ptr);
return false;
}
}
else {
if (g_servoMotorLinkList.vertMotor.linkListNum >= sendDataBufNumber - 1) {
OSMemPut(g_memPtr, ptr);
return false;
}
}
if (priority == highPriority) {
@ -221,6 +249,7 @@ void servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priorit
g_servoMotorLinkList.vertMotor.LinkListHead_H
= g_servoMotorLinkList.vertMotor.LinkListTail_H;
}
g_servoMotorLinkList.vertMotor.linkListNum++;
}
else {
if (g_servoMotorLinkList.vertMotor.LinkListTail_L == NULL) {
@ -234,12 +263,14 @@ void servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priorit
if (g_servoMotorLinkList.vertMotor.LinkListHead_L == NULL) {
g_servoMotorLinkList.vertMotor.LinkListHead_L
= g_servoMotorLinkList.vertMotor.LinkListTail_L;
}
}
g_servoMotorLinkList.vertMotor.linkListNum++;
}
//释放信号量，通知能发送一次
OSSemPost(g_vertSpeedSem);
}
return true;
}
static void ptz_hori_servo_task()
@ -262,7 +293,8 @@ static void ptz_hori_servo_task()
CommuTransData(H_MOTOR, g_servoMotorLinkList.horiMotor.LinkListHead_H->data
, g_servoMotorLinkList.horiMotor.LinkListHead_H->length);
g_servoMotorLinkList.horiMotor.linkListNum = highPriority;
g_servoMotorLinkList.horiMotor.linkListPriority = highPriority;
startTimeOut(horiMotorType);
continue;
}
@ -272,7 +304,8 @@ static void ptz_hori_servo_task()
CommuTransData(H_MOTOR, g_servoMotorLinkList.horiMotor.LinkListHead_L->data
, g_servoMotorLinkList.horiMotor.LinkListHead_L->length);
g_servoMotorLinkList.horiMotor.linkListNum = lowPriority;
g_servoMotorLinkList.horiMotor.linkListPriority = lowPriority;
startTimeOut(horiMotorType);
}
else {
pdebug(DEBUG_LEVEL_FATAL,"ptz_hori_servo_task no data...\n\r");
@ -296,23 +329,24 @@ static void ptz_vert_servo_task()
// 高优先级链表中数据先发送
if (g_servoMotorLinkList.vertMotor.LinkListHead_H != NULL) {
g_servoMotorLinkList.vertMotor.linkListNum = highPriority;
CommuTransData(V_MOTOR, g_servoMotorLinkList.vertMotor.LinkListHead_H->data
g_servoMotorLinkList.vertMotor.linkListPriority = highPriority;
CommuTransData(V_MOTOR, g_servoMotorLinkList.vertMotor.LinkListHead_H->data
, g_servoMotorLinkList.vertMotor.LinkListHead_H->length);
startTimeOut(V_MOTOR);
continue;
}
// 高优先级链表中无数据时，发送低优先级中的数据
if (g_servoMotorLinkList.vertMotor.LinkListHead_L != NULL) {
g_servoMotorLinkList.vertMotor.linkListNum = highPriority;
g_servoMotorLinkList.vertMotor.linkListPriority = lowPriority;
CommuTransData(V_MOTOR, g_servoMotorLinkList.vertMotor.LinkListHead_L->data
, g_servoMotorLinkList.vertMotor.LinkListHead_L->length);
startTimeOut(V_MOTOR);
}
else {
pdebug(DEBUG_LEVEL_FATAL,"ptz_vert_servo_task no data...\n\r");
}
}
}
@ -371,11 +405,13 @@ static void creat_task_vert_servo_task(void)
void horiServoTimeOut()
{
servoLinkListMemPut(horiMotorType);
OSSemPost(g_horiSpeedMutex);
}
void vertServoTimeOut()
{
servoLinkListMemPut(vertMotorType);
OSSemPost(g_vertSpeedMutex);
}
@ -384,9 +420,9 @@ BSP_OS_TMR vertServoSoftWareTim;
void servoCommSoftWareTimInit()
{
CPU_INT08U ServoSoftWareTimErr;
horiServoSoftWareTim = OSTmrCreate(10
, 10
, OS_TMR_OPT_PERIODIC
horiServoSoftWareTim = OSTmrCreate(5
, 100//1*200ms
, OS_TMR_OPT_ONE_SHOT
, (OS_TMR_CALLBACK)horiServoTimeOut
, (void *)0
, "tmr1"
@ -398,9 +434,9 @@ void servoCommSoftWareTimInit()
pdebug(DEBUG_LEVEL_FATAL,"create horiServoSoftWareTim failed...\n\r");
}
vertServoSoftWareTim = OSTmrCreate(10
, 10
, OS_TMR_OPT_PERIODIC
vertServoSoftWareTim = OSTmrCreate(5
, 100
, OS_TMR_OPT_ONE_SHOT
, (OS_TMR_CALLBACK)vertServoTimeOut
, (void *)0
, "tmr2"
@ -480,7 +516,7 @@ void init_speed_module(void)
if (err != OS_ERR_NONE) {
pdebug(DEBUG_LEVEL_FATAL, "Failed to create the motor memory pool\n\r");
}
//初始化链表头尾
g_servoMotorLinkList.horiMotor.LinkListHead_H = NULL;
g_servoMotorLinkList.horiMotor.LinkListHead_L = NULL;
@ -506,10 +542,42 @@ void init_speed_module(void)
OSTimeDlyHMSM(0u, 0u, 0u, 500u);//等待硬件初始化成功
uint8_t buffer[20] = {0};
uint8_t buffer_len = 0;
buffer_len = WriteMotorOneReg_buffer(H_MOTOR, H02_CONTR_MODE_SELEC, 0, buffer);
servoSendData(horiMotorType, buffer, buffer_len, lowPriority);
servoSendData(horiMotorType, WriteMotorOneReg(horiMotorType, H02_CONTR_MODE_SELEC, 0),
WRITE_ONE_REG_FRAME_NUM, lowPriority);//切换为速度控制模式
// OSTimeDlyHMSM(0u, 0u, 0u, 5u);
servoSendData(vertMotorType, WriteMotorOneReg(vertMotorType, H02_CONTR_MODE_SELEC, 0),
WRITE_ONE_REG_FRAME_NUM, lowPriority);//切换为速度控制模式
OSTimeDlyHMSM(0u, 0u, 0u, 5u);
// servoSendData(horiMotorType, WriteMotorOneReg(horiMotorType, H02_MOTOR_DIR_SELEC, PTZ_HORI_MOTOR_DIRECTION)
// , WRITE_ONE_REG_FRAME_NUM, lowPriority);//设置水平电机方向
// servoSendData(vertMotorType, WriteMotorOneReg(vertMotorType, H02_MOTOR_DIR_SELEC, PTZ_VERT_MOTOR_DIRECTION)
// , WRITE_ONE_REG_FRAME_NUM, lowPriority);//设置垂直电机方向
// OSTimeDlyHMSM(0u, 0u, 0u, 5u);
// servoSendData(horiMotorType, WriteMotorOneReg(horiMotorType, H06_SPEED_UP_SLOPE_VALUE, PTZ_HORI_MOTOR_AccelerationTimeConstant)
// , WRITE_ONE_REG_FRAME_NUM, lowPriority);//设置加速时间常数
// servoSendData(vertMotorType, WriteMotorOneReg(vertMotorType, H06_SPEED_UP_SLOPE_VALUE, PTZ_VERT_MOTOR_AccelerationTimeConstant)
// , WRITE_ONE_REG_FRAME_NUM, lowPriority);//设置加速时间常数
// OSTimeDlyHMSM(0u, 0u, 0u, 5u);
// servoSendData(horiMotorType, WriteMotorOneReg(horiMotorType, H06_SPEED_DOWN_SLOPE_VALUE, PTZ_HORI_MOTOR_DecelerationTimeConstant)
// , WRITE_ONE_REG_FRAME_NUM, lowPriority);//设置减速时间常数
// servoSendData(vertMotorType, WriteMotorOneReg(vertMotorType, H06_SPEED_DOWN_SLOPE_VALUE, PTZ_VERT_MOTOR_DecelerationTimeConstant)
// , WRITE_ONE_REG_FRAME_NUM, lowPriority);//设置减速时间常数
// OSTimeDlyHMSM(0u, 0u, 0u, 5u);
// servoSendData(i, WriteMotorOneReg(i, H06_SPEED_COMMU_SET_VALUE, 60)
// , WRITE_ONE_REG_FRAME_NUM, lowPriority);//设置电机转速为60r/min
// OSTimeDlyHMSM(0u, 0u, 0u, 5u);
servoSendData(horiMotorType, WriteMotorOneReg(horiMotorType, H03_DI1_LOGICAL_SELEC, 1)
, WRITE_ONE_REG_FRAME_NUM, lowPriority);//电机运行使能
servoSendData(vertMotorType, WriteMotorOneReg(vertMotorType, H03_DI1_LOGICAL_SELEC, 1)
, WRITE_ONE_REG_FRAME_NUM, lowPriority);//电机运行使能
OSTimeDlyHMSM(0u, 0u, 0u, 5u);
}

6
APP/Device/Device_speed/speed_to_servoMotor.h
View File

@ -33,6 +33,8 @@ typedef struct _linkList {
uint8_t data[sendDataBufLen];
//发送的数据长度
uint16_t length;
// uint8_t funcCode;
// uint16_t regAddr;
//下一个节点
struct _linkList *next;
} linkList;
@ -67,8 +69,8 @@ extern ptzServoLinkList g_servoMotorLinkList;
void ptz_send_speed(char dev, char speed);
void init_speed_module(void);
void servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priority);
void servoHoriLinkListMemPut(uint8_t motorType);
bool servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priority);
void servoLinkListMemPut(uint8_t motorType);
void stopTimeOut(uint8_t motorType);
void startTimeOut(uint8_t motorType);

31
APP/Device/device_Other/device_adc_collect.c
View File

@ -1,5 +1,7 @@
#include "device_adc_collect.h"
#include "rotate_servo.h"
#include "speed_to_servoMotor.h"
// ADC_Phase_current H_ADC_Collect;
// ADC_Phase_current V_ADC_Collect;
@ -487,21 +489,26 @@ static char ptz_data_collect_task()
{
int i=0,j=0;
while(1)
{
if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
{//电机处于启动状态
// H_ADC2_Phase_current();
}
if(g_ptz.vert_start_stop_set == PTZ_VERT_START)
{//电机处于启动状态
// V_ADC0_Phase_current();
}
{
// if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
// {//电机处于启动状态
// // H_ADC2_Phase_current();
// }
// if(g_ptz.vert_start_stop_set == PTZ_VERT_START)
// {//电机处于启动状态
// // V_ADC0_Phase_current();
// }
if(j >= 100)
{
// //读取水平电机实时速度
// servoSendData(horiMotorType, ReadMotorOneReg(horiMotorType, READ_MOTOR_SPEED_NOW)
// , READ_ONE_REG_FRAME_NUM, lowPriority);
// //读取俯仰电机实时速度
// servoSendData(vertMotorType, ReadMotorOneReg(vertMotorType, READ_MOTOR_SPEED_NOW)
// , READ_ONE_REG_FRAME_NUM, lowPriority);
j=0;
//云台不自检关闭，打开采集任务
#ifndef PTZ_NO_SELF_CHECK

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

@ -17,101 +17,7 @@
/// @note 修改日志
/// LH于2022-05-25
void EXTI_IRQ_init()
{
#ifdef L6235D
/*HALL中断，水平——PD13-H1,PD14-H2,PD15-H3，，垂直PC6-H1,PC7-H2,PC8-H3*/
nvic_irq_enable(EXTI10_15_IRQn, 3U, 3U);//水平，一个中断函数，中断线10-15
nvic_irq_enable(EXTI5_9_IRQn, 3U, 2U);//垂直，一个中断函数，中断线5-9
//对应引脚配置外部中断线
syscfg_exti_line_config(EXTI_SOURCE_GPIOD, EXTI_SOURCE_PIN13);
syscfg_exti_line_config(EXTI_SOURCE_GPIOD, EXTI_SOURCE_PIN14);
syscfg_exti_line_config(EXTI_SOURCE_GPIOD, EXTI_SOURCE_PIN15);
syscfg_exti_line_config(EXTI_SOURCE_GPIOC, EXTI_SOURCE_PIN6);
syscfg_exti_line_config(EXTI_SOURCE_GPIOC, EXTI_SOURCE_PIN7);
syscfg_exti_line_config(EXTI_SOURCE_GPIOC, EXTI_SOURCE_PIN8);
//外部中断线初始化
exti_init(EXTI_6, EXTI_INTERRUPT, EXTI_TRIG_RISING);//上升沿中断
exti_init(EXTI_7, EXTI_INTERRUPT, EXTI_TRIG_RISING);
exti_init(EXTI_8, EXTI_INTERRUPT, EXTI_TRIG_RISING);
exti_init(EXTI_13, EXTI_INTERRUPT, EXTI_TRIG_RISING);
exti_init(EXTI_14, EXTI_INTERRUPT, EXTI_TRIG_RISING);
exti_init(EXTI_15, EXTI_INTERRUPT, EXTI_TRIG_RISING);
//清除标志位
exti_interrupt_flag_clear(EXTI_6);
exti_interrupt_flag_clear(EXTI_7);
exti_interrupt_flag_clear(EXTI_8);
exti_interrupt_flag_clear(EXTI_13);
exti_interrupt_flag_clear(EXTI_14);
exti_interrupt_flag_clear(EXTI_15);
BSP_IntVectSet(39,EXTI5_9_IRQHandler);
BSP_IntEn(39);
BSP_IntVectSet(56,EXTI10_15_IRQHandler);
BSP_IntEn(56);
#endif
#ifdef Full_bridge //MOS+栅极驱动
rcu_periph_clock_enable(RCU_SYSCFG);
//配置引脚时钟
rcu_periph_clock_enable(RCU_GPIOE);
//水平——PE10-H1,PE11-H2,PE12-H3，，垂直PE13-H1,PE14-H2,PE15-H3
//设置引脚为输入模式
gpio_mode_set(GPIOE, GPIO_MODE_INPUT, GPIO_PUPD_NONE, GPIO_PIN_10);
gpio_mode_set(GPIOE, GPIO_MODE_INPUT, GPIO_PUPD_NONE, GPIO_PIN_11);
gpio_mode_set(GPIOE, GPIO_MODE_INPUT, GPIO_PUPD_NONE, GPIO_PIN_12);
gpio_mode_set(GPIOE, GPIO_MODE_INPUT, GPIO_PUPD_NONE, GPIO_PIN_13);
gpio_mode_set(GPIOE, GPIO_MODE_INPUT, GPIO_PUPD_NONE, GPIO_PIN_14);
gpio_mode_set(GPIOE, GPIO_MODE_INPUT, GPIO_PUPD_NONE, GPIO_PIN_15);
/*HALL中断，水平——PE10-H1,PE11-H2,PE12-H3，，垂直PE13-H1,PE14-H2,PE15-H3*/
//对应引脚配置外部中断线
syscfg_exti_line_config(EXTI_SOURCE_GPIOE, EXTI_SOURCE_PIN10);
syscfg_exti_line_config(EXTI_SOURCE_GPIOE, EXTI_SOURCE_PIN11);
syscfg_exti_line_config(EXTI_SOURCE_GPIOE, EXTI_SOURCE_PIN12);
syscfg_exti_line_config(EXTI_SOURCE_GPIOE, EXTI_SOURCE_PIN13);
syscfg_exti_line_config(EXTI_SOURCE_GPIOE, EXTI_SOURCE_PIN14);
syscfg_exti_line_config(EXTI_SOURCE_GPIOE, EXTI_SOURCE_PIN15);
//外部中断线初始化
exti_init(EXTI_10, EXTI_INTERRUPT, EXTI_TRIG_RISING);//上升沿中断
exti_init(EXTI_11, EXTI_INTERRUPT, EXTI_TRIG_RISING);
exti_init(EXTI_12, EXTI_INTERRUPT, EXTI_TRIG_RISING);
exti_init(EXTI_13, EXTI_INTERRUPT, EXTI_TRIG_RISING);//上升沿中断
exti_init(EXTI_14, EXTI_INTERRUPT, EXTI_TRIG_RISING);
exti_init(EXTI_15, EXTI_INTERRUPT, EXTI_TRIG_RISING);
exti_interrupt_enable(EXTI_10);
exti_interrupt_enable(EXTI_11);
exti_interrupt_enable(EXTI_12);
exti_interrupt_enable(EXTI_13);
exti_interrupt_enable(EXTI_14);
exti_interrupt_enable(EXTI_15);
//清除标志位
exti_interrupt_flag_clear(EXTI_10);
exti_interrupt_flag_clear(EXTI_11);
exti_interrupt_flag_clear(EXTI_12);
exti_interrupt_flag_clear(EXTI_13);
exti_interrupt_flag_clear(EXTI_14);
exti_interrupt_flag_clear(EXTI_15);
nvic_irq_enable(EXTI10_15_IRQn, 2U, 2U);//，一个中断函数，中断线10-15
BSP_IntVectSet(56,EXTI10_15_IRQHandler);
BSP_IntEn(56);
{
/*光电开关中断PE7-SW1，PE8-SW2，PE9-SW3*/
syscfg_exti_line_config(EXTI_SOURCE_GPIOE, EXTI_SOURCE_PIN7);
syscfg_exti_line_config(EXTI_SOURCE_GPIOE, EXTI_SOURCE_PIN8);
@ -142,47 +48,6 @@ void EXTI_IRQ_init()
BSP_IntVectSet(39,EXTI5_9_IRQHandler);
BSP_IntEn(39);
#else
/*光电开关中断PB0-SW1，PB1-SW2，PB2-SW3*/
nvic_irq_enable(EXTI0_IRQn, 2U, 0U);//垂直光电开关SW1
nvic_irq_enable(EXTI1_IRQn, 2U, 1U);//垂直光电开关SW2
nvic_irq_enable(EXTI2_IRQn, 2U, 2U);//水平光电开关SW3
syscfg_exti_line_config(EXTI_SOURCE_GPIOB, EXTI_SOURCE_PIN0);
syscfg_exti_line_config(EXTI_SOURCE_GPIOB, EXTI_SOURCE_PIN1);
syscfg_exti_line_config(EXTI_SOURCE_GPIOB, EXTI_SOURCE_PIN2);
//垂直光电开关SW1
#ifdef PTZ_SW1_DOWN_FALL_UPDATE
exti_init(EXTI_0, EXTI_INTERRUPT, EXTI_TRIG_FALLING);//下降沿中断
#endif
#ifdef PTZ_SW1_UP_RISE_UPDATE
exti_init(EXTI_0, EXTI_INTERRUPT, EXTI_TRIG_BOTH);//边沿中断
#endif
//垂直光电开关SW2
exti_init(EXTI_1, EXTI_INTERRUPT, EXTI_TRIG_FALLING);//下降沿中断
//水平光电开关SW3
#ifdef PTZ_SW3_LEFT_RISE_UPDATE
exti_init(EXTI_2, EXTI_INTERRUPT, EXTI_TRIG_BOTH);//边沿中断
#else
exti_init(EXTI_2, EXTI_INTERRUPT, EXTI_TRIG_FALLING);//下降沿中断
#endif
//清除标志位
exti_interrupt_flag_clear(EXTI_0);
exti_interrupt_flag_clear(EXTI_1);
exti_interrupt_flag_clear(EXTI_2);
BSP_IntVectSet(22,EXTI0_IRQHandler);
BSP_IntEn(22);
BSP_IntVectSet(23,EXTI1_IRQHandler);
BSP_IntEn(23);
BSP_IntVectSet(24,EXTI2_IRQHandler);
BSP_IntEn(24);
#endif
}
@ -594,6 +459,194 @@ void ptz_SW_IRQHandler(exti_line_enum sw_linex)
#endif
#endif
#ifdef PTZ_SERVO_MOTOR
#ifdef PTZ_NO_SELF_CHECK
if(g_ptz.Voltage > SWITCH_IRQ_V)
{
switch(sw_linex)
{
case EXTI_7://垂直光电开关SW1
#ifdef PTZ_PHOTOELECTRIC_SWITCH
asm("nop");
asm("nop");
asm("nop");
asm("nop");
asm("nop");
if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_UP)
{
if(PS_VERT_SW1_READ == PS_HIGH)
{
g_ptz.vert_ps_sw1_up_rise ++;
}
}
if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_DOWN)
{
if(PS_VERT_SW1_READ == PS_LOW)
{
g_ptz.vert_ps_sw1_down_fall ++;
}
}
#endif
break;
case EXTI_8://垂直光电开关SW2
#ifdef PTZ_PHOTOELECTRIC_SWITCH
asm("nop");
asm("nop");
asm("nop");
asm("nop");
asm("nop");
if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_UP)
{
if(PS_VERT_SW2_READ == PS_LOW)
{
g_ptz.vert_ps_sw2_up_fall ++;
}
}
if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_DOWN)
{
if(PS_VERT_SW2_READ == PS_HIGH)
{
g_ptz.vert_ps_sw2_down_rise ++;
}
}
#endif
break;
case EXTI_9://水平光电开关SW3
#ifdef PTZ_PHOTOELECTRIC_SWITCH
asm("nop");
asm("nop");
asm("nop");
asm("nop");
asm("nop");
if(PS_HORI_SW3_READ == PS_LOW)
{
if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_RIGHT)
{
g_ptz.hori_ps_sw3_right_fall ++;
}
if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_LEFT)
{
g_ptz.hori_ps_sw3_left_fall ++;
}
}
if(PS_HORI_SW3_READ == PS_HIGH)//PS_HIGH
{
if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_RIGHT)
{
g_ptz.hori_ps_sw3_right_rise ++;
}
if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_LEFT)
{
g_ptz.hori_ps_sw3_left_rise ++;
// term_printf(" hori_ps_sw3_left_rise \r\n");//中断不能使用串口打印，会占用较长时间
}
}
#endif
break;
default:break;
}
}
#else
switch(sw_linex)
{
case EXTI_7://垂直光电开关SW1
#ifdef PTZ_PHOTOELECTRIC_SWITCH
asm("nop");
asm("nop");
asm("nop");
asm("nop");
asm("nop");
if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_UP)
{
if(PS_VERT_SW1_READ == PS_HIGH)
{
g_ptz.vert_ps_sw1_up_rise ++;
}
}
if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_DOWN)
{
if(PS_VERT_SW1_READ == PS_LOW)
{
g_ptz.vert_ps_sw1_down_fall ++;
}
}
#endif
break;
case EXTI_8://垂直光电开关SW2
#ifdef PTZ_PHOTOELECTRIC_SWITCH
asm("nop");
asm("nop");
asm("nop");
asm("nop");
asm("nop");
if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_UP)
{
if(PS_VERT_SW2_READ == PS_LOW)
{
g_ptz.vert_ps_sw2_up_fall ++;
}
}
if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_DOWN)
{
if(PS_VERT_SW2_READ == PS_HIGH)
{
g_ptz.vert_ps_sw2_down_rise ++;
}
}
#endif
break;
case EXTI_9://水平光电开关SW3
#ifdef PTZ_PHOTOELECTRIC_SWITCH
asm("nop");
asm("nop");
asm("nop");
asm("nop");
asm("nop");
if(PS_HORI_SW3_READ == PS_LOW)
{
if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_RIGHT)
{
g_ptz.hori_ps_sw3_right_fall ++;
}
if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_LEFT)
{
g_ptz.hori_ps_sw3_left_fall ++;
}
}
if(PS_HORI_SW3_READ == PS_HIGH)
{
if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_RIGHT)
{
g_ptz.hori_ps_sw3_right_rise ++;
}
if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_LEFT)
{
g_ptz.hori_ps_sw3_left_rise ++;
// term_printf(" hori_ps_sw3_left_rise \r\n");//中断不能使用串口打印，会占用较长时间
}
}
#endif
break;
default:break;
}
#endif
#endif
}
/// @brief 水平HALL中断处理函数

5
APP/Device/device_Other/device_interrupt.h
View File

@ -51,6 +51,11 @@
#endif
//
#ifdef PTZ_MEDIUM_WORM_SERVO_MOTOR_24V
#define SWITCH_IRQ_V 20
#endif
void ptz_SW_IRQHandler(exti_line_enum sw_linex);
void ptz_H_HALL_IRQHandler(exti_line_enum hall_linex);
void ptz_V_HALL_IRQHandler(exti_line_enum hall_linex);

4
APP/Device/device_Other/device_wdog.c
View File

@ -16,6 +16,7 @@
#include "gd32f4xx_gpio.h"
#include <includes.h>
#include "device_wdog.h"
/// 引脚初始化
///
/// @param none
@ -46,8 +47,7 @@ static void task_feeddog ()
{
OSTimeDlyHMSM(0u,0u,0u,20u);
//翻转电平喂狗
gpio_bit_toggle(GPIOE, GPIO_PIN_5);
gpio_bit_toggle(GPIOE, GPIO_PIN_5);
}
}

3
APP/Service/service_error_count.c
View File

@ -17,6 +17,7 @@
#include "service_error_count.h"
#include "rotate_servo.h"
#include "speed_to_servoMotor.h"
char error_conut_state;
#define COUNT_STATE 1
@ -55,8 +56,10 @@ static void ptz_hori_error_count_task()
case COUNT_STATE:
g_ptz.hori_as5047d.as5047d_state = 0;//数据不可正常使用
OSTimeDlyHMSM(0u, 0u, 0u, 10u);
// set_speed_to_servoMotor(horiMotorType, ERROR_COUNT_SPEED);
// data = ptz_hori_choice_microstep(ERROR_COUNT_SPEED);
// g_ptz.hori_tmc2160 = data;
if(g_ptz.hori_ps_sw3_state == PS_COVER)//如果水平光电开关被挡住
{
ptz_hori_start(PTZ_HORI_DIR_LEFT, ERROR_COUNT_SPEED);

7
APP/Service/service_selfcheck.c
View File

@ -21,6 +21,7 @@
#include "service_error_count.h"
#include "rotate_servo.h"
#include "speed_to_servoMotor.h"
#ifdef PTZ_BLDC_MOTOR
/// 云台全范围自检
@ -809,6 +810,7 @@ static unsigned char ptz_hori_complete_self_check_task()
{
//首先让云台水平向右转动
case PTZ_HORI_SELF_CHECK_COMPLETE_STEP:
set_speed_to_servoMotor(horiMotorType, PTZ_HORI_SELF_CHECK_SPEED);
if(g_ptz.hori_ps_sw3_state == PS_COVER)//如果水平光电开关被挡住
{
ptz_hori_start(PTZ_HORI_DIR_LEFT, PTZ_HORI_SELF_CHECK_SPEED);
@ -885,7 +887,8 @@ 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)//如果水平光电开关被挡住
set_speed_to_servoMotor(horiMotorType, PTZ_HORI_SELF_CHECK_SPEED);
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;
@ -953,6 +956,7 @@ static unsigned char ptz_vert_complete_self_check_task()
{
///首先读取光电开关的状态，初步判断垂直状态
case PTZ_VERT_SELF_CHECK_COMPLETE_STEP:
set_speed_to_servoMotor(vertMotorType, PTZ_VERT_SELF_CHECK_SPEED);
g_ptz.vert_ps_sw1_down_fall = 0;
g_ptz.vert_ps_sw1_up_rise = 0;
g_ptz.vert_ps_sw2_up_fall = 0;
@ -1075,6 +1079,7 @@ static unsigned char ptz_vert_simplify_self_check_task()
switch(g_ptz.vert_self_check)
{
case PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP:
set_speed_to_servoMotor(vertMotorType, PTZ_VERT_SELF_CHECK_SPEED);
g_ptz.vert_ps_sw1_down_fall = 0;
g_ptz.vert_ps_sw1_up_rise = 0;
g_ptz.vert_ps_sw2_up_fall = 0;

4
BSP/Driver/drv_adc.c
View File

@ -233,7 +233,7 @@ float ptz_Voltage_collect_adc1_task()
#endif
/* 间接测量，11倍分压/放大 */
adc1_v[adc1_v_num] = (float)value_V / 4096.0 * 3.381 * 12.55;
adc1_v[adc1_v_num] = (float)value_V / 4096.0 * 3.3 * 11;
adc1_v_num++;
if(adc1_v_num >= LB_V_TIMES)
@ -283,7 +283,7 @@ float ptz_Current_collect_adc1_task()
#endif
/* 间接测量 */
adc1_i[adc1_i_num] = ((float)value_I / 4096.0 * 3.381) * 10;
adc1_i[adc1_i_num] = ((float)value_I / 4096.0 * 3.3) * 10;
adc1_i_num++;
if(adc1_i_num >= LB_I_TIMES)

122
BSP/Driver/servoMotor/modbus_crc.c
View File

@ -1,66 +1,70 @@
#include "stdint.h"
#include "ptz_type_select.h"
static const uint8_t aucCRCHi[] = {
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40
};
#ifdef PTZ_MEDIUM_WORM_SERVO_MOTOR_24V
static const uint8_t aucCRCLo[] = {
0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 0x07, 0xC7,
0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD, 0x0F, 0xCF, 0xCE, 0x0E,
0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09, 0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9,
0x1B, 0xDB, 0xDA, 0x1A, 0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC,
0x14, 0xD4, 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3,
0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 0xF2, 0x32,
0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4, 0x3C, 0xFC, 0xFD, 0x3D,
0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A, 0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38,
0x28, 0xE8, 0xE9, 0x29, 0xEB, 0x2B, 0x2A, 0xEA, 0xEE, 0x2E, 0x2F, 0xEF,
0x2D, 0xED, 0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26,
0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60, 0x61, 0xA1,
0x63, 0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67, 0xA5, 0x65, 0x64, 0xA4,
0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F, 0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB,
0x69, 0xA9, 0xA8, 0x68, 0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA,
0xBE, 0x7E, 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5,
0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, 0x70, 0xB0,
0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92, 0x96, 0x56, 0x57, 0x97,
0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C, 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E,
0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59, 0x58, 0x98, 0x88, 0x48, 0x49, 0x89,
0x4B, 0x8B, 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C,
0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42, 0x43, 0x83,
0x41, 0x81, 0x80, 0x40
};
static const uint8_t aucCRCHi[] = {
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40
};
uint16_t ModbusCRC16(uint8_t *pucFrame, uint16_t usLen)
{
uint8_t ucCRCHi = 0xFF;
uint8_t ucCRCLo = 0xFF;
int iIndex;
static const uint8_t aucCRCLo[] = {
0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 0x07, 0xC7,
0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD, 0x0F, 0xCF, 0xCE, 0x0E,
0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09, 0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9,
0x1B, 0xDB, 0xDA, 0x1A, 0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC,
0x14, 0xD4, 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3,
0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 0xF2, 0x32,
0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4, 0x3C, 0xFC, 0xFD, 0x3D,
0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A, 0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38,
0x28, 0xE8, 0xE9, 0x29, 0xEB, 0x2B, 0x2A, 0xEA, 0xEE, 0x2E, 0x2F, 0xEF,
0x2D, 0xED, 0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26,
0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60, 0x61, 0xA1,
0x63, 0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67, 0xA5, 0x65, 0x64, 0xA4,
0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F, 0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB,
0x69, 0xA9, 0xA8, 0x68, 0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA,
0xBE, 0x7E, 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5,
0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, 0x70, 0xB0,
0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92, 0x96, 0x56, 0x57, 0x97,
0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C, 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E,
0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59, 0x58, 0x98, 0x88, 0x48, 0x49, 0x89,
0x4B, 0x8B, 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C,
0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42, 0x43, 0x83,
0x41, 0x81, 0x80, 0x40
};
while(usLen--)
uint16_t ModbusCRC16(uint8_t *pucFrame, uint16_t usLen)
{
iIndex = ucCRCLo ^ *(pucFrame++);
ucCRCLo = (uint8_t)(ucCRCHi ^ aucCRCHi[iIndex]);
ucCRCHi = aucCRCLo[iIndex];
uint8_t ucCRCHi = 0xFF;
uint8_t ucCRCLo = 0xFF;
int iIndex;
while(usLen--)
{
iIndex = ucCRCLo ^ *(pucFrame++);
ucCRCLo = (uint8_t)(ucCRCHi ^ aucCRCHi[iIndex]);
ucCRCHi = aucCRCLo[iIndex];
}
return (uint16_t)(ucCRCHi << 8 | ucCRCLo);
}
return (uint16_t)(ucCRCHi << 8 | ucCRCLo);
}
#endif

7
BSP/Driver/servoMotor/modbus_crc.h
View File

@ -1,7 +1,12 @@
#ifndef _MODBUS_CRC_H_
#define _MODBUS_CRC_H_
#include "ptz_type_select.h"
uint16_t ModbusCRC16(uint8_t *pucFrame, uint16_t usLen);
#ifdef PTZ_MEDIUM_WORM_SERVO_MOTOR_24V
uint16_t ModbusCRC16(uint8_t *pucFrame, uint16_t usLen);
#endif
#endif

269
BSP/Driver/servoMotor/motorCommu.c
View File

@ -1,6 +1,8 @@
#include "motorCommu.h"
#include "servoMotor_recv.h"
#include "speed_to_servoMotor.h"
#ifdef PTZ_MEDIUM_WORM_SERVO_MOTOR_24V
/*
********************************************************************************************************
* 电机串口通讯驱动初始化相关结构体
@ -99,11 +101,9 @@ static MotorCommuDmaHwInfo_t g_MotorDmaBuff[] =
********************************************************************************************************
*/
/* dma接收缓冲区 */
static uint8_t g_horiDmaRxBuff1[DMA_BUFF_SIZE] = {0};//水平电机DMA接受缓存区1
static uint8_t g_horiDmaRxBuff2[DMA_BUFF_SIZE] = {0};//水平电机DMA接受缓存区2
static uint8_t g_horiDmaRxBuff[DMA_BUFF_SIZE] = {0};//水平电机DMA接受缓存区
static uint8_t g_vertDmaRxBuff1[DMA_BUFF_SIZE] = {0};//垂直电机DMA接受缓存区1
static uint8_t g_vertDmaRxBuff2[DMA_BUFF_SIZE] = {0};//垂直电机DMA接受缓存区2
static uint8_t g_vertDmaRxBuff[DMA_BUFF_SIZE] = {0};//垂直电机DMA接受缓存区
/* 处理串口通讯与数据缓冲的数据结构 */
static CommuInfo_t g_horiCommuDeal; //水平电机
static CommuInfo_t g_vertCommuDeal; //垂直电机
@ -111,22 +111,19 @@ static CommuInfo_t g_vertCommuDeal; //
typedef struct
{
CommuInfo_t* pCommuInfo; //串口通讯与数据缓冲相关的数据结构
uint8_t* dmaRxBuff1; //dma接受缓存区1指针
uint8_t* dmaRxBuff2; //dma接受缓存区2指针
uint8_t* dmaRxBuff; //dma接受缓存区指针
}CommuHwInfo_t;//方便缓冲区初始化的结构体
static CommuHwInfo_t g_commuInfoBuff[] =
{
//水平电机
{
.pCommuInfo = &g_horiCommuDeal,
.dmaRxBuff1 = g_horiDmaRxBuff1,
.dmaRxBuff2 = g_horiDmaRxBuff2,
.dmaRxBuff = g_horiDmaRxBuff,
},
//垂直电机串口5
{
.pCommuInfo = &g_vertCommuDeal,
.dmaRxBuff1 = g_vertDmaRxBuff1,
.dmaRxBuff2 = g_vertDmaRxBuff2,
.dmaRxBuff = g_vertDmaRxBuff,
},
};
@ -211,7 +208,14 @@ static void DmaCofig(void)
dma_circulation_disable(g_MotorDmaBuff[i].dmaNo, g_MotorDmaBuff[i].dmaTxch);
dma_channel_subperipheral_select(g_MotorDmaBuff[i].dmaNo, g_MotorDmaBuff[i].dmaTxch, g_MotorDmaBuff[i].dmaPeriph);
//dma中断配置
nvic_irq_enable(g_MotorDmaBuff[i].dmaTxIrq, 4, 3);
if( i == H_MOTOR )
{
nvic_irq_enable(g_MotorDmaBuff[H_MOTOR].dmaTxIrq, 1, 2);
}
else
{
nvic_irq_enable(g_MotorDmaBuff[V_MOTOR].dmaTxIrq, 1, 1);
}
dma_interrupt_enable(g_MotorDmaBuff[i].dmaNo, g_MotorDmaBuff[i].dmaTxch, DMA_CHXCTL_FTFIE);//dma发送完成中断
@ -219,7 +223,7 @@ static void DmaCofig(void)
//dma配置
dma_deinit(g_MotorDmaBuff[i].dmaNo, g_MotorDmaBuff[i].dmaRxch);
dmaStruct.direction = DMA_PERIPH_TO_MEMORY;
dmaStruct.memory0_addr = (uint32_t)(g_commuInfoBuff[i].pCommuInfo->pDmaRsvBuff1);
dmaStruct.memory0_addr = (uint32_t)(g_commuInfoBuff[i].pCommuInfo->pDmaRsvBuff);
dmaStruct.memory_inc = DMA_MEMORY_INCREASE_ENABLE;
dmaStruct.number = DMA_BUFF_SIZE;
dmaStruct.periph_addr = g_MotorDmaBuff[i].periphAddr;
@ -229,10 +233,17 @@ static void DmaCofig(void)
dma_memory_width_config(g_MotorDmaBuff[i].dmaNo, g_MotorDmaBuff[i].dmaRxch, DMA_MEMORY_WIDTH_8BIT);
dma_periph_width_config(g_MotorDmaBuff[i].dmaNo, g_MotorDmaBuff[i].dmaRxch, DMA_PERIPH_WIDTH_8BIT);
dma_single_data_mode_init(g_MotorDmaBuff[i].dmaNo, g_MotorDmaBuff[i].dmaRxch, &dmaStruct);
dma_circulation_disable(g_MotorDmaBuff[i].dmaNo, g_MotorDmaBuff[i].dmaRxch);//非循环模式
dma_circulation_enable(g_MotorDmaBuff[i].dmaNo, g_MotorDmaBuff[i].dmaRxch);//循环模式
dma_channel_subperipheral_select(g_MotorDmaBuff[i].dmaNo, g_MotorDmaBuff[i].dmaRxch, g_MotorDmaBuff[i].dmaPeriph);
//中断配置
nvic_irq_enable(g_MotorDmaBuff[i].dmaRxIrq, 4, 2);
if ( i == H_MOTOR )
{
nvic_irq_enable(g_MotorDmaBuff[H_MOTOR].dmaRxIrq, 0, 2);
}
else
{
nvic_irq_enable(g_MotorDmaBuff[V_MOTOR].dmaRxIrq, 0, 1);
}
dma_interrupt_enable(g_MotorDmaBuff[i].dmaNo, g_MotorDmaBuff[i].dmaRxch, DMA_CHXCTL_FTFIE);
dma_channel_enable(g_MotorDmaBuff[i].dmaNo, g_MotorDmaBuff[i].dmaRxch);
}
@ -257,9 +268,7 @@ static void CommuStructInit()
CommuInfo_t *pCommuDeal = g_commuInfoBuff[i].pCommuInfo;
pCommuDeal->dmaTranFlag = DMA_TRANS_IDLE;
pCommuDeal->dmaSize = DMA_BUFF_SIZE;
pCommuDeal->pDmaRsvBuff1 = g_commuInfoBuff[i].dmaRxBuff1;
pCommuDeal->pDmaRsvBuff2 = g_commuInfoBuff[i].dmaRxBuff2;
pCommuDeal->pDmaRsvBuffSelect = DMA_RSVBUFF_SELECT1;//默认使用缓冲区1
pCommuDeal->pDmaRsvBuff = g_commuInfoBuff[i].dmaRxBuff;
}
}
@ -300,7 +309,6 @@ static void CommuDmaTra(uint8_t devNo, uint8_t *buffer,uint16_t len)
void DMA0_Channel3_IRQHandler(void)
{
/*
* 配合串口的发送。功能是继续发送缓冲区未发送的数据。
* 发送完成配置的CNT次数后，会进入此中断函数
*/
if(dma_interrupt_flag_get(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaTxch, DMA_INT_FLAG_FTF))
@ -321,130 +329,37 @@ void DMA0_Channel3_IRQHandler(void)
* @return
***********************************************************
*/
static uint16_t g_horiLastPos;
static uint16_t g_horiNowPos;
void USART2_IRQHandler(void)
{
/* 串口的接收空闲中断方式进行了数据缓存。*/
dma_single_data_parameter_struct dmaStruct;
if(RESET != usart_interrupt_flag_get(g_motorCommuBuff[H_MOTOR].uartNo, USART_INT_FLAG_IDLE))
{
/* clear IDLE flag */
usart_interrupt_flag_clear(g_motorCommuBuff[H_MOTOR].uartNo, USART_INT_FLAG_IDLE); //第一步，读取stat0寄存器，清除IDLE标志位
usart_data_receive(g_motorCommuBuff[H_MOTOR].uartNo); //第二步，读取数据寄存器，清除IDLE标志位
//释放信号量，通知接收到一包数据，任务可以处理了
usart_data_receive(g_motorCommuBuff[H_MOTOR].uartNo); //第二步，读取数据寄存器，清除IDLE标志位
g_horiLastPos = g_horiNowPos;
g_horiNowPos = DMA_BUFF_SIZE - dma_transfer_number_get(g_MotorDmaBuff[H_MOTOR].dmaNo,
g_MotorDmaBuff[H_MOTOR].dmaRxch);//函数获取的是还有多少个没传输，而不是已经传输了多少
//释放信号量，通知能处理数据
OSSemPost(g_horiMotorMutex);
CommuInfo_t *pCommuDeal = g_commuInfoBuff[H_MOTOR].pCommuInfo;
dma_interrupt_disable(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch, DMA_CHXCTL_FTFIE);
/* 切换使用接收缓冲区，这样上层的解析永远从第一个字节起*/
if ( pCommuDeal->pDmaRsvBuffSelect == DMA_RSVBUFF_SELECT1 )
{
pCommuDeal->pDmaRsvBuffSelect = DMA_RSVBUFF_SELECT2;
dma_channel_disable(g_MotorDmaBuff[H_MOTOR].dmaNo
, g_MotorDmaBuff[H_MOTOR].dmaRxch);//切换缓冲区，配置缓冲区长度需要先禁用DMA
// dmaStruct.memory0_addr = (uint32_t)(pCommuDeal->pDmaRsvBuff2);
// dma_single_data_mode_init(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch, &dmaStruct);
dma_memory_address_config(g_MotorDmaBuff[H_MOTOR].dmaNo
, g_MotorDmaBuff[H_MOTOR].dmaRxch
, DMA_MEMORY_1
, (uint32_t)(pCommuDeal->pDmaRsvBuff2));
dma_transfer_number_config(g_MotorDmaBuff[H_MOTOR].dmaNo
, g_MotorDmaBuff[H_MOTOR].dmaRxch
, DMA_BUFF_SIZE);
dma_channel_enable(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch);
}
else
{
// pCommuDeal->pDmaRsvBuffSelect = DMA_RSVBUFF_SELECT1;
// dma_channel_disable(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch);//切换缓冲区，配置缓冲区长度需要先禁用DMA
// dmaStruct.memory0_addr = (uint32_t)(pCommuDeal->pDmaRsvBuff1);
// dma_single_data_mode_init(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch, &dmaStruct);
// dma_channel_enable(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch);
// dmaStruct.memory0_addr = (uint32_t)(pCommuDeal->pDmaRsvBuff2);
// dma_single_data_mode_init(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch, &dmaStruct);
pCommuDeal->pDmaRsvBuffSelect = DMA_RSVBUFF_SELECT1;
dma_channel_disable(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch);//切换缓冲区，配置缓冲区长度需要先禁用DMA
dma_memory_address_config(g_MotorDmaBuff[H_MOTOR].dmaNo
, g_MotorDmaBuff[H_MOTOR].dmaRxch
, DMA_MEMORY_0
, (uint32_t)(pCommuDeal->pDmaRsvBuff1));
dma_transfer_number_config(g_MotorDmaBuff[H_MOTOR].dmaNo
, g_MotorDmaBuff[H_MOTOR].dmaRxch
, DMA_BUFF_SIZE);
dma_channel_enable(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch);
}
dma_interrupt_flag_clear(g_MotorDmaBuff[H_MOTOR].dmaNo
, g_MotorDmaBuff[H_MOTOR].dmaRxch, DMA_INT_FLAG_FTF);
dma_interrupt_enable(g_MotorDmaBuff[H_MOTOR].dmaNo
, g_MotorDmaBuff[H_MOTOR].dmaRxch, DMA_CHXCTL_FTFIE);
/*计算在DMA缓冲区需要获取的数据长度*/
// DmaIdleNum = dma_transfer_number_get(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch);//获取的是还有多少个没传输，而不是已经传输了多少
}
}
void DMA0_Channel1_IRQHandler(void)
{
dma_single_data_parameter_struct dmaStruct;
/*
* 配合串口1的接收空闲中断。功能是复位DMA的偏移量
* 1、为了串口1的空闲中断在处理数据时防止越界，将 pUartAttr->DamOffset置为0;
* 2、DMA为循环方式进行数据搬运的，当搬运完配置的Cnt后，会进入此中断处理函数，
* 3、当有数据过来时，数据将会拷贝到缓冲区的起始位置;
* 4、注意：如果DMA为正常模式，那么当完成一次拷贝后,DMA会自动disable掉。
* 注意：DMA为正常模式，那么当完成一次拷贝后,DMA会自动disable掉。
*/
if(dma_interrupt_flag_get(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch, DMA_INT_FLAG_FTF))
{
dma_interrupt_flag_clear(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch, DMA_INT_FLAG_FTF);
CommuInfo_t *pCommuDeal = g_commuInfoBuff[H_MOTOR].pCommuInfo;
//释放信号量，通知接收到一包数据，任务可以处理了
OSSemPost(g_horiMotorMutex);
/* 切换使用接收缓冲区，这样上层的解析永远从第一个字节起*/
if ( pCommuDeal->pDmaRsvBuffSelect == DMA_RSVBUFF_SELECT1 )
{
// pCommuDeal->pDmaRsvBuffSelect = DMA_RSVBUFF_SELECT2;
// dma_channel_disable(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch);//切换缓冲区，配置缓冲区长度需要先禁用DMA
// dmaStruct.memory0_addr = (uint32_t)(pCommuDeal->pDmaRsvBuff2);
// dmaStruct.number = DMA_BUFF_SIZE;
// dma_single_data_mode_init(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch, &dmaStruct);
// dma_channel_enable(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch);
// dmaStruct.memory0_addr = (uint32_t)(pCommuDeal->pDmaRsvBuff2);
// dma_single_data_mode_init(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch, &dmaStruct);
pCommuDeal->pDmaRsvBuffSelect = DMA_RSVBUFF_SELECT2;
dma_channel_disable(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch);//切换缓冲区，配置缓冲区长度需要先禁用DMA
dma_memory_address_config(g_MotorDmaBuff[H_MOTOR].dmaNo
, g_MotorDmaBuff[H_MOTOR].dmaRxch
, DMA_MEMORY_1
, (uint32_t)(pCommuDeal->pDmaRsvBuff2));
dma_transfer_number_config(g_MotorDmaBuff[H_MOTOR].dmaNo
, g_MotorDmaBuff[H_MOTOR].dmaRxch
, DMA_BUFF_SIZE);
dma_channel_enable(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch);
}
else
{
// pCommuDeal->pDmaRsvBuffSelect = DMA_RSVBUFF_SELECT1;
// dma_channel_disable(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch);//切换缓冲区，配置缓冲区长度需要先禁用DMA
// dmaStruct.memory0_addr = (uint32_t)(pCommuDeal->pDmaRsvBuff1);
// dmaStruct.number = DMA_BUFF_SIZE;
// dma_single_data_mode_init(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch, &dmaStruct);
// dma_channel_enable(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch);
pCommuDeal->pDmaRsvBuffSelect = DMA_RSVBUFF_SELECT1;
dma_channel_disable(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch);//切换缓冲区，配置缓冲区长度需要先禁用DMA
dma_memory_address_config(g_MotorDmaBuff[H_MOTOR].dmaNo
, g_MotorDmaBuff[H_MOTOR].dmaRxch
, DMA_MEMORY_0
, (uint32_t)(pCommuDeal->pDmaRsvBuff1));
dma_transfer_number_config(g_MotorDmaBuff[H_MOTOR].dmaNo
, g_MotorDmaBuff[H_MOTOR].dmaRxch
, DMA_BUFF_SIZE);
dma_channel_enable(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch);
}
}
}
@ -474,7 +389,6 @@ void DMA1_Channel7_IRQHandler(void)
while(usart_flag_get(g_motorCommuBuff[V_MOTOR].uartNo, USART_FLAG_TC) == RESET); // 等待串口发送完成
V_COMMU_RS485_RX; //485切换为接收
pCommuDeal->dmaTranFlag = DMA_TRANS_IDLE;
}
}
@ -485,9 +399,10 @@ void DMA1_Channel7_IRQHandler(void)
* @return
***********************************************************
*/
static uint16_t g_vertLastPos;
static uint16_t g_vertNowPos;
void USART5_IRQHandler(void)
{
dma_single_data_parameter_struct dmaStruct;
/* 串口的接收空闲中断方式进行了数据缓存。*/
if(RESET != usart_interrupt_flag_get(g_motorCommuBuff[V_MOTOR].uartNo, USART_INT_FLAG_IDLE))
{
@ -495,69 +410,22 @@ void USART5_IRQHandler(void)
usart_interrupt_flag_clear(g_motorCommuBuff[V_MOTOR].uartNo, USART_INT_FLAG_IDLE); //第一步，读取stat0寄存器，清除IDLE标志位
usart_data_receive(g_motorCommuBuff[V_MOTOR].uartNo); //第二步，读取数据寄存器，清除IDLE标志位
//释放信号量，通知接收到一包数据，任务可以处理了
OSSemPost(g_horiMotorMutex);
CommuInfo_t *pCommuDeal = g_commuInfoBuff[V_MOTOR].pCommuInfo;
/* 切换使用接收缓冲区，这样上层的解析永远从第一个字节起*/
if ( pCommuDeal->pDmaRsvBuffSelect == DMA_RSVBUFF_SELECT1 )
{
pCommuDeal->pDmaRsvBuffSelect = DMA_RSVBUFF_SELECT2;
dma_channel_disable(g_MotorDmaBuff[V_MOTOR].dmaNo, g_MotorDmaBuff[V_MOTOR].dmaRxch);//切换缓冲区，配置缓冲区长度需要先禁用DMA
dmaStruct.memory0_addr = (uint32_t)(pCommuDeal->pDmaRsvBuff2);
dma_single_data_mode_init(g_MotorDmaBuff[V_MOTOR].dmaNo, g_MotorDmaBuff[V_MOTOR].dmaRxch, &dmaStruct);
dma_channel_enable(g_MotorDmaBuff[V_MOTOR].dmaNo, g_MotorDmaBuff[V_MOTOR].dmaRxch);
}
else
{
pCommuDeal->pDmaRsvBuffSelect = DMA_RSVBUFF_SELECT1;
dma_channel_disable(g_MotorDmaBuff[V_MOTOR].dmaNo, g_MotorDmaBuff[V_MOTOR].dmaRxch);//切换缓冲区，配置缓冲区长度需要先禁用DMA
dmaStruct.memory0_addr = (uint32_t)(pCommuDeal->pDmaRsvBuff1);
dma_single_data_mode_init(g_MotorDmaBuff[V_MOTOR].dmaNo, g_MotorDmaBuff[V_MOTOR].dmaRxch, &dmaStruct);
dma_channel_enable(g_MotorDmaBuff[V_MOTOR].dmaNo, g_MotorDmaBuff[V_MOTOR].dmaRxch);
}
g_vertLastPos = g_vertNowPos;
g_vertNowPos = DMA_BUFF_SIZE - dma_transfer_number_get(g_MotorDmaBuff[V_MOTOR].dmaNo,
g_MotorDmaBuff[V_MOTOR].dmaRxch);
//释放信号量，通知接收任务处理
OSSemPost(g_vertMotorMutex);
}
}
// uint8_t rx_OK = 0;
void DMA1_Channel1_IRQHandler(void)
{
dma_single_data_parameter_struct dmaStruct;
/*
* 配合串口1的接收空闲中断。功能是复位DMA的偏移量
* 1、为了串口1的空闲中断在处理数据时防止越界，将 pUartAttr->DamOffset置为0;
* 2、DMA为循环方式进行数据搬运的，当搬运完配置的Cnt后，会进入此中断处理函数，
* 3、当有数据过来时，数据将会拷贝到缓冲区的起始位置;
* 4、注意：如果DMA为正常模式，那么当完成一次拷贝后,DMA会自动disable掉。
* 注意：如果DMA为正常模式，那么当完成一次拷贝后,DMA会自动disable掉。
*/
if(dma_interrupt_flag_get(g_MotorDmaBuff[V_MOTOR].dmaNo, g_MotorDmaBuff[V_MOTOR].dmaRxch, DMA_INT_FLAG_FTF))
{
dma_interrupt_flag_clear(g_MotorDmaBuff[V_MOTOR].dmaNo, g_MotorDmaBuff[V_MOTOR].dmaRxch, DMA_INT_FLAG_FTF);
CommuInfo_t *pCommuDeal = g_commuInfoBuff[V_MOTOR].pCommuInfo;
//释放信号量，通知接收到一包数据，任务可以处理了
OSSemPost(g_horiMotorMutex);
/* 切换使用接收缓冲区，这样上层的解析永远从第一个字节起*/
if ( pCommuDeal->pDmaRsvBuffSelect == DMA_RSVBUFF_SELECT1 )
{
pCommuDeal->pDmaRsvBuffSelect = DMA_RSVBUFF_SELECT2;
dma_channel_disable(g_MotorDmaBuff[V_MOTOR].dmaNo, g_MotorDmaBuff[V_MOTOR].dmaRxch);//切换缓冲区，配置缓冲区长度需要先禁用DMA
dmaStruct.memory0_addr = (uint32_t)(pCommuDeal->pDmaRsvBuff2);
dmaStruct.number = DMA_BUFF_SIZE;
dma_single_data_mode_init(g_MotorDmaBuff[V_MOTOR].dmaNo, g_MotorDmaBuff[V_MOTOR].dmaRxch, &dmaStruct);
dma_channel_enable(g_MotorDmaBuff[V_MOTOR].dmaNo, g_MotorDmaBuff[V_MOTOR].dmaRxch);
}
else
{
pCommuDeal->pDmaRsvBuffSelect = DMA_RSVBUFF_SELECT1;
dma_channel_disable(g_MotorDmaBuff[V_MOTOR].dmaNo, g_MotorDmaBuff[V_MOTOR].dmaRxch);//切换缓冲区，配置缓冲区长度需要先禁用DMA
dmaStruct.memory0_addr = (uint32_t)(pCommuDeal->pDmaRsvBuff1);
dmaStruct.number = DMA_BUFF_SIZE;
dma_single_data_mode_init(g_MotorDmaBuff[V_MOTOR].dmaNo, g_MotorDmaBuff[V_MOTOR].dmaRxch, &dmaStruct);
dma_channel_enable(g_MotorDmaBuff[V_MOTOR].dmaNo, g_MotorDmaBuff[V_MOTOR].dmaRxch);
}
}
}
@ -588,6 +456,13 @@ void CommuDrvInit(void)
*/
bool CommuTransData(uint8_t motorNo, uint8_t* buffer, int32_t len)
{
/*简易判断*/
if( buffer == NULL || len == 0 )
{
return false;
}
/*485切换为发送*/
if( motorNo == H_MOTOR )
{
H_COMMU_RS485_TX;
@ -612,20 +487,46 @@ bool CommuTransData(uint8_t motorNo, uint8_t* buffer, int32_t len)
/**
* @brief 从接收循环缓冲区中读取指定长度的数据到用户数组
* @param motorNo：电机号，H_MOTOR：水平电机，V_MOTOR：垂直电机
* @param userBuff：从接收循环缓冲区中接收数据的数组
* @param userBuff：从接收缓冲区中接收数据的数组
* @param len：接收数据的长度
* @return none
*/
void CommuRsvData(uint8_t motorNo, uint8_t* userBuff, uint32_t len)
bool CommuRsvData(uint8_t motorNo, uint8_t* userBuff, uint32_t len)
{
CommuInfo_t *pCommuDeal = g_commuInfoBuff[motorNo].pCommuInfo;
if ( pCommuDeal->pDmaRsvBuffSelect == DMA_RSVBUFF_SELECT1 )
/*简易判断*/
if( userBuff == NULL || len == 0 )
{
memcpy(userBuff, pCommuDeal->pDmaRsvBuff2, len);
return false;
}
uint16_t horiPosTemp = g_horiLastPos;
uint16_t vertPosTemp = g_vertLastPos;
/*接收DMA缓冲区数据*/
if ( motorNo == H_MOTOR )
{
if ( ( (horiPosTemp + len) % DMA_BUFF_SIZE ) != g_horiNowPos )//假如是写寄存器，电机返回消息正常是8字节，但如果是写错误，则返回非8字节错误码
{
return false;
}
for(uint8_t i = 0; i < len; i++)
{
*(userBuff+i) = pCommuDeal->pDmaRsvBuff[horiPosTemp];
horiPosTemp = (horiPosTemp + 1) % DMA_BUFF_SIZE;
}
}
else
{
memcpy(userBuff, pCommuDeal->pDmaRsvBuff1, len);
}
if ( ( (vertPosTemp + len) % DMA_BUFF_SIZE ) != g_vertNowPos )//假如是写寄存器，电机返回消息正常是8字节，但如果是写错误，则返回非8字节错误码
{
return false;
}
for(uint8_t i = 0; i < len; i++)
{
*(userBuff+i) = pCommuDeal->pDmaRsvBuff[vertPosTemp];
vertPosTemp = (vertPosTemp + 1) % DMA_BUFF_SIZE;
}
}
return true;
}
#endif

112
BSP/Driver/servoMotor/motorCommu.h
View File

@ -4,68 +4,70 @@
#include "gd32f4xx.h"
#include "stdbool.h"
#include "string.h"
/*
********************************************************************************************************
* dma缓冲区相关
********************************************************************************************************
*/
typedef struct
{
int16_t dmaTranFlag; /*dma发送是否在工作的标志位*/
int32_t dmaSize; /*DMA接收缓冲区的大小*/
uint8_t *pDmaRsvBuff1; /*指向接收DMA缓冲区1的首地址*/
uint8_t *pDmaRsvBuff2; /*指向接收DMA缓冲区2的首地址*/
uint8_t pDmaRsvBuffSelect; /*表示当前正在使用哪个接收缓冲区*/
}CommuInfo_t;
#include "ptz_type_select.h"
#define DMA_TRANS_IDLE 0//dma当前未在发送数据
#define DMA_TRANS_BUSY 1//dma当前正在发送数据
#define DMA_BUFF_SIZE 64//dma缓冲区大小
#define DMA_RSVBUFF_SELECT1 (uint8_t)(0)//当前使用dma接收缓冲区1
#define DMA_RSVBUFF_SELECT2 (uint8_t)(1)//当前使用dma接收缓冲区2
#ifdef PTZ_MEDIUM_WORM_SERVO_MOTOR_24V
//extern CommuInfo_t g_commuDeal;//来自motorCommu.c
/*
********************************************************************************************************
* dma缓冲区相关
********************************************************************************************************
*/
typedef struct
{
int16_t dmaTranFlag; /*dma发送是否在工作的标志位*/
int32_t dmaSize; /*DMA接收缓冲区的大小*/
uint8_t *pDmaRsvBuff; /*指向接收DMA缓冲区的首地址*/
}CommuInfo_t;
/*
********************************************************************************************************
* 串口设备驱动初始化相关
********************************************************************************************************
*/
#define H_MOTOR 0//数组g_motorCommuInitBuff[MOTOR_NUM]位号
#define V_MOTOR 1
#define DMA_TRANS_IDLE 0//dma当前未在发送数据
#define DMA_TRANS_BUSY 1//dma当前正在发送数据
#define DMA_BUFF_SIZE 64//dma缓冲区大小
/*-------------485接收发送宏开关----------------------*/
#define H_COMMU_RS485_TX gpio_bit_set(GPIOD, GPIO_PIN_10)//水平电机485发送
#define H_COMMU_RS485_RX gpio_bit_reset(GPIOD, GPIO_PIN_10)//水平电机485接收
#define V_COMMU_RS485_TX gpio_bit_set(GPIOC, GPIO_PIN_8)//垂直电机485发送
#define V_COMMU_RS485_RX gpio_bit_reset(GPIOC, GPIO_PIN_8)//垂直电机485接收
//extern CommuInfo_t g_commuDeal;//来自motorCommu.c
/**
* @brief 串口设备通讯驱动初始化
* @param
* @return
*/
void CommuDrvInit(void);
/*
********************************************************************************************************
* 串口设备驱动初始化相关
********************************************************************************************************
*/
#define H_MOTOR 0//数组g_motorCommuInitBuff[MOTOR_NUM]位号
#define V_MOTOR 1
/**
* @brief 数据的发送
* @param motorNo：电机号，H_MOTOR：水平电机，V_MOTOR：垂直电机
* @param buffer：需要发送的数据
* @param len：发送数据的长度
* @return ture:DMA空闲当前数据可以发送；false：DMA正在发送数据，当前数据不能发送
*/
bool CommuTransData(uint8_t motorNo, uint8_t* buffer, int32_t len);
/*-------------485接收发送宏开关----------------------*/
#define H_COMMU_RS485_TX gpio_bit_set(GPIOD, GPIO_PIN_10)//水平电机485发送
#define H_COMMU_RS485_RX gpio_bit_reset(GPIOD, GPIO_PIN_10)//水平电机485接收
#define V_COMMU_RS485_TX gpio_bit_set(GPIOC, GPIO_PIN_8)//垂直电机485发送
#define V_COMMU_RS485_RX gpio_bit_reset(GPIOC, GPIO_PIN_8)//垂直电机485接收
/**
* @brief 从接收循环缓冲区中读取指定长度的数据到用户数组
* @param motorNo：电机号，H_MOTOR：水平电机，V_MOTOR：垂直电机
* @param userBuff：从接收循环缓冲区中接收数据的数组
* @param len：接收数据的长度
* @return none
*/
void CommuRsvData(uint8_t motorNo, uint8_t* userBuff, uint32_t len);
/**
* @brief 串口设备通讯驱动初始化
* @param
* @return
*/
void CommuDrvInit(void);
/**
* @brief 数据的发送
* @param motorNo：电机号，H_MOTOR：水平电机，V_MOTOR：垂直电机
* @param buffer：需要发送的数据
* @param len：发送数据的长度
* @return ture:DMA空闲当前数据可以发送；false：DMA正在发送数据，当前数据不能发送
*/
bool CommuTransData(uint8_t motorNo, uint8_t* buffer, int32_t len);
/**
* @brief 从接收循环缓冲区中读取指定长度的数据到用户数组
* @param motorNo：电机号，H_MOTOR：水平电机，V_MOTOR：垂直电机
* @param userBuff：从接收循环缓冲区中接收数据的数组
* @param len：接收数据的长度
* @return none
*/
bool CommuRsvData(uint8_t motorNo, uint8_t* userBuff, uint32_t len);
///*用于结构体数组赋值，方便外部使用此结构体数组*/
//CommuHwInfo_t GetMotorCommuBuffStr(uint8_t motorNo);
///*用于结构体数组赋值，方便外部使用此结构体数组*/
//CommuHwInfo_t GetMotorCommuBuffStr(uint8_t motorNo);
#endif
#endif

236
BSP/Driver/servoMotor/servoMotor.c
View File

@ -1,149 +1,111 @@
#include "servoMotor.h"
#include <ucos_ii.h>
/*
水平垂直电机使能开关引脚配置
*/
static void MotorSwitchGpioCofig(void)
{
/*GPIO时钟初始化*/
rcu_periph_clock_enable(RCU_GPIOE);
/*水平电机打开引脚*/
gpio_mode_set(GPIOE, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO_PIN_0);
gpio_output_options_set(GPIOE, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_0);
/*垂直电机打开引脚*/
gpio_mode_set(GPIOE, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO_PIN_1);
gpio_output_options_set(GPIOE, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_1);
}
static uint8_t g_writeOneRegBuff[WRITE_ONE_REG_BUFFNUM];//由于写寄存器，电机返回的数据和写入的数据完全一致，故设此数组
/*
从机地址 功能码 寄存器地址高 寄存器地址低 数据高位 数据低位 crc校验高位 crc校验低位
01H 06H 02H 00H 00H 01H 49H B2H
*/
/**
* @brief 伺服电机速度模式写单个寄存器
* @param motorNo：写垂直电机还是水平电机
* @param regAddr：要写的寄存器
* @param data：要向寄存器写入的数据
* @return false:写失败，当前DMA正在发送数据
*/
bool WriteMotorOneReg(uint8_t motorNo, uint16_t regAddr, uint16_t data)
{
uint8_t frameBuff[WRITE_ONE_REG_BUFFNUM] = {0};
uint16_t crc;
if ( motorNo == H_MOTOR )
{
frameBuff[0] = H_MOTOR_ADDR;//由于采用一主一从模式，所以水平电机垂直电机从机地址都是0x01，云台后期也不会扩展
}
else
{
frameBuff[0] = V_MOTOR_ADDR;
}
frameBuff[1] = WRITE_ONE_REG;
frameBuff[2] = regAddr >> WRITE_ONE_REG_BUFFNUM;
frameBuff[3] = regAddr & 0xff;
frameBuff[4] = data >> WRITE_ONE_REG_BUFFNUM;
frameBuff[5] = data & 0xff;
crc = ModbusCRC16(frameBuff, WRITE_ONE_REG_BUFFNUM - 2);
frameBuff[6] = crc & 0xff;
frameBuff[7] = crc >> WRITE_ONE_REG_BUFFNUM;
if ( CommuTransData(motorNo, frameBuff, WRITE_ONE_REG_BUFFNUM) == false)
{
return false;
}
memcpy(g_writeOneRegBuff, frameBuff, WRITE_ONE_REG_BUFFNUM);
return true;
}
/**
* @brief 写单个寄存器成功后，电机会返回一帧与写格式完全一致的数据
* @param motorNo：写垂直电机还是水平电机
* @param userBuff：用来接收返回的数据的数组
* @param len：接收的数据长度，对于此函数必须为WRITE_ONE_REG_BUFFNUM
* @return false:错误，读出的内容与写入的内容不一致
*/
bool MotorReplyForWrite(uint8_t motorNo, uint8_t* userBuff, uint8_t len)
{
CommuRsvData(motorNo, userBuff, len);
for( uint8_t i = 0; i < len; i++ )
#ifdef PTZ_MEDIUM_WORM_SERVO_MOTOR_24V
/*
水平垂直电机使能开关引脚配置
*/
static void MotorSwitchGpioCofig(void)
{
if ( userBuff[i] != g_writeOneRegBuff[i] )
{
memset(g_writeOneRegBuff, 0x00, len);
return false;
}
/*GPIO时钟初始化*/
rcu_periph_clock_enable(RCU_GPIOE);
/*水平电机打开引脚*/
gpio_mode_set(GPIOE, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO_PIN_0);
gpio_output_options_set(GPIOE, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_0);
/*垂直电机打开引脚*/
gpio_mode_set(GPIOE, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO_PIN_1);
gpio_output_options_set(GPIOE, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_1);
}
memset(g_writeOneRegBuff, 0x00, len);
return true;
}
/*
从机地址 功能码 寄存器地址高 寄存器地址低 数据高位 数据低位 crc校验高位 crc校验低位
01H 06H 02H 00H 00H 01H 49H B2H
*/
/**
* @brief 伺服电机速度模式写单个寄存器
* @param motorNo：写垂直电机还是水平电机
* @param regAddr：要写的寄存器
* @param data：要向寄存器写入的数据
* @param frameBuff: 用来发送的数组
* @return 写入frameBuff的长度
*/
uint8_t WriteMotorOneReg_buffer(uint8_t motorNo, uint16_t regAddr, uint16_t data, uint8_t *frameBuff)
{
uint16_t crc;
if ( motorNo == H_MOTOR )
/*
从机地址 功能码 寄存器地址高 寄存器地址低 数据高位 数据低位 crc校验高位 crc校验低位
01H 06H 02H 00H 00H 01H 49H B2H
*/
/**
* @brief 伺服电机速度模式写单个寄存器
* @param motorNo：写垂直电机还是水平电机
* @param regAddr：要写的寄存器
* @param data：要向寄存器写入的数据
* @return 返回数组地址
*/
static uint8_t g_HwriteOneRegBuff[WRITE_ONE_REG_FRAME_NUM];
static uint8_t g_VwriteOneRegBuff[WRITE_ONE_REG_FRAME_NUM];
uint8_t* WriteMotorOneReg(uint8_t motorNo, uint16_t regAddr, int16_t data)
{
frameBuff[0] = H_MOTOR_ADDR;//由于采用一主一从模式，所以水平电机垂直电机从机地址都是0x01，云台后期也不会扩展
static uint8_t g_writeOneRegBuff[WRITE_ONE_REG_FRAME_NUM];
uint16_t crc;
g_writeOneRegBuff[0] = 0x01;
g_writeOneRegBuff[1] = WRITE_ONE_REG;
g_writeOneRegBuff[2] = regAddr >> 8;
g_writeOneRegBuff[3] = (uint8_t)(regAddr & 0xff);
g_writeOneRegBuff[4] = data >> 8;
g_writeOneRegBuff[5] = data & 0xff;
crc = ModbusCRC16(g_writeOneRegBuff, WRITE_ONE_REG_FRAME_NUM - 2);
g_writeOneRegBuff[6] = (uint8_t)(crc & 0xff);
g_writeOneRegBuff[7] = crc >> 8;
if ( motorNo == H_MOTOR )
{
memcpy(g_HwriteOneRegBuff, g_writeOneRegBuff, WRITE_ONE_REG_FRAME_NUM);
return g_HwriteOneRegBuff;
}
memcpy(g_VwriteOneRegBuff, g_writeOneRegBuff, WRITE_ONE_REG_FRAME_NUM);
return g_VwriteOneRegBuff;
}
else
/*
从机地址 功能码 寄存器地址高 寄存器地址低 reg数量高位 数量低位 crc校验高位 crc校验低位
01H 03H 0BH 00H 00H 01H 86H 2EH
*/
/**
* @brief 伺服电机速度模式读单个寄存器
* @param motorNo：读垂直电机还是水平电机
* @param regAddr：要读的寄存器
* @return 返回数组地址
*/
static uint8_t g_HreadOneRegBuff[READ_ONE_REG_FRAME_NUM];
static uint8_t g_VreadOneRegBuff[READ_ONE_REG_FRAME_NUM];
uint8_t* ReadMotorOneReg(uint8_t motorNo, uint16_t regAddr)
{
frameBuff[0] = V_MOTOR_ADDR;
static uint8_t g_readOneRegBuff[READ_ONE_REG_FRAME_NUM];
uint16_t crc;
g_readOneRegBuff[0] = 0x01;
g_readOneRegBuff[1] = READ_ONE_REG;
g_readOneRegBuff[2] = regAddr >> 8;
g_readOneRegBuff[3] = regAddr & 0xff;
g_readOneRegBuff[4] = 0x00;
g_readOneRegBuff[5] = 0x01;
crc = ModbusCRC16(g_readOneRegBuff, READ_ONE_REG_FRAME_NUM - 2);
g_readOneRegBuff[6] = crc & 0xff;
g_readOneRegBuff[7] = crc >> 8;
if ( motorNo == H_MOTOR )
{
memcpy(g_HreadOneRegBuff, g_readOneRegBuff, WRITE_ONE_REG_FRAME_NUM);
return g_HreadOneRegBuff;
}
memcpy(g_VreadOneRegBuff, g_readOneRegBuff, WRITE_ONE_REG_FRAME_NUM);
return g_VreadOneRegBuff;
}
frameBuff[1] = WRITE_ONE_REG;
frameBuff[2] = regAddr >> WRITE_ONE_REG_BUFFNUM;
frameBuff[3] = regAddr & 0xff;
frameBuff[4] = data >> WRITE_ONE_REG_BUFFNUM;
frameBuff[5] = data & 0xff;
crc = ModbusCRC16(frameBuff, WRITE_ONE_REG_BUFFNUM - 2);
frameBuff[6] = crc & 0xff;
frameBuff[7] = crc >> WRITE_ONE_REG_BUFFNUM;
return 8;
}
/**
* @brief 伺服电机驱动初始化
* @param
* @return
*/
void servoMotorInit(void)
{
MotorSwitchGpioCofig();//两个电机电源的开关PE0,1引脚，如果其它地方实现了，可以不需要
H_MOTOR_OPEN;
OSTimeDlyHMSM(0u, 0u, 0u, 1000u);
V_MOTOR_OPEN;
CommuDrvInit();//伺服电机RS485通讯初始化
// OSTimeDlyHMSM(0u, 0u, 0u, 500u);//等待硬件初始化成功
// WriteMotorOneReg(H_MOTOR, H02_CONTR_MODE_SELEC, 0);//H0200,选择速度模式
// OSTimeDlyHMSM(0u, 0u, 0u, 5u);
// WriteMotorOneReg(H_MOTOR, H04_DO1_FUNC_SELEC, 19);
// OSTimeDlyHMSM(0u, 0u, 0u, 5u);
// WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, 100);//速度设置为100rpm
// OSTimeDlyHMSM(0u, 0u, 0u, 5u);
// WriteMotorOneReg(H_MOTOR, H06_SPEED_UP_SLOPE_VALUE, 3000);//加速度3000
// OSTimeDlyHMSM(0u, 0u, 0u, 5u);
// WriteMotorOneReg(H_MOTOR, H06_SPEED_DOWN_SLOPE_VALUE, 2000);//减速度2000
// OSTimeDlyHMSM(0u, 0u, 0u, 5u);
// WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 1);//只启动水平电机
// OSTimeDlyHMSM(0u, 0u, 0u, 5u);
}
/**
* @brief 伺服电机驱动初始化
* @param
* @return
*/
void servoMotorInit(void)
{
MotorSwitchGpioCofig();//两个电机电源的开关PE0,1引脚，如果其它地方实现了，可以不需要
H_MOTOR_OPEN;
OSTimeDlyHMSM(0u, 0u, 0u, 1000u);
V_MOTOR_OPEN;
CommuDrvInit();//伺服电机RS485通讯初始化
}
#endif

136
BSP/Driver/servoMotor/servoMotor.h
View File

@ -4,80 +4,88 @@
#include "motorCommu.h"
#include "modbus_crc.h"
#include "stdbool.h"
/*
********************************************************************************************************
* 电机硬件相关
********************************************************************************************************
*/
#define H_MOTOR_OPEN gpio_bit_set(GPIOE,GPIO_PIN_0) //水平电机电源打开
#define H_MOTOR_STOP gpio_bit_reset(GPIOE,GPIO_PIN_0) //水平电机电源关闭
#define V_MOTOR_OPEN gpio_bit_set(GPIOE,GPIO_PIN_1) //垂直电机电源打开
#define V_MOTOR_STOP gpio_bit_reset(GPIOE,GPIO_PIN_1) //垂直电机电源关闭
#include "ptz_type_select.h"
#define H_MOTOR_ADDR 0x01//水平电机地址
#define V_MOTOR_ADDR 0x01//垂直电机地址
/*
********************************************************************************************************
* 功能码命令
********************************************************************************************************
*/
#define READ_ONE_REG 0X03//读单个寄存器
#define READ_MULT_CONSE_REG 0X03//读多个连续的寄存器Read multiple consecutive registers
#define WRITE_ONE_REG 0X06//写单个寄存器
#define WRITE_MULT_CONSE_REG 0x10//写多个连续的寄存器
#ifdef PTZ_MEDIUM_WORM_SERVO_MOTOR_24V
/*
********************************************************************************************************
* 电机硬件相关
********************************************************************************************************
*/
#define H_MOTOR_OPEN gpio_bit_set(GPIOE,GPIO_PIN_0) //水平电机电源打开
#define H_MOTOR_STOP gpio_bit_reset(GPIOE,GPIO_PIN_0) //水平电机电源关闭
#define V_MOTOR_OPEN gpio_bit_set(GPIOE,GPIO_PIN_1) //垂直电机电源打开
#define V_MOTOR_STOP gpio_bit_reset(GPIOE,GPIO_PIN_1) //垂直电机电源关闭
#define WRITE_ONE_REG_BUFFNUM 8//写单个寄存器，数据帧的字节个数
/*
********************************************************************************************************
* 寄存器参数
********************************************************************************************************
*/
/*基本控制参数H02*/
#define H02_CONTR_MODE_SELEC 0X0200//（Control mode selection）控制模式选择0：速度模式，1：位置模式，2：转矩模式
#define H_MOTOR_ADDR 0x01//水平电机地址
#define V_MOTOR_ADDR 0x01//垂直电机地址
/*
********************************************************************************************************
* 功能码命令
********************************************************************************************************
*/
#define READ_ONE_REG 0X03//读单个寄存器
#define READ_MULT_CONSE_REG 0X03//读多个连续的寄存器Read multiple consecutive registers
#define WRITE_ONE_REG 0X06//写单个寄存器
#define WRITE_MULT_CONSE_REG 0x10//写多个连续的寄存器
/*DI/DO参数H03~H04*/
#define H03_DI1_FUNC_SELEC 0X0302//DI1端子功能选择,一个 DI 功能选项只能关联一个 DI 端子，不可重复分配
#define H03_DI1_LOGICAL_SELEC 0X0303//DI1端子逻辑选择
#define H04_DO1_FUNC_SELEC 0X0400//DO1端子功能选择
#define H04_DO1_LOGICAL_SELEC 0X0401//DO1端子逻辑选择
#define WRITE_ONE_REG_FRAME_NUM 8//写单个寄存器，数据帧的字节个数
#define READ_ONE_REG_FRAME_NUM 8//读单个寄存器，数据帧的字节个数
/*
********************************************************************************************************
* 寄存器参数
********************************************************************************************************
*/
/*基本控制参数H02*/
#define H02_CONTR_MODE_SELEC 0X0200//（Control mode selection）控制模式选择0：速度模式，1：位置模式，2：转矩模式
#define H02_MOTOR_DIR_SELEC 0X0202//电机旋转方向选择
/*DI/DO参数H03~H04*/
#define H03_DI1_FUNC_SELEC 0X0302//DI1端子功能选择,一个 DI 功能选项只能关联一个 DI 端子，不可重复分配
#define H03_DI1_LOGICAL_SELEC 0X0303//DI1端子逻辑选择
#define H04_DO1_FUNC_SELEC 0X0400//DO1端子功能选择
#define H04_DO1_LOGICAL_SELEC 0X0401//DO1端子逻辑选择
/*速度控制参数H06*/
#define H06_SPEED_COMMAND_SELEC 0X0602//速度指令选择
#define H06_SPEED_COMMU_SET_VALUE 0X0603//速度指令通讯设置值,当 H06_02=0 时，通过此参数设定电机运行转速
#define H06_SPEED_UP_SLOPE_VALUE 0X0605//速度指令加速斜坡时间常数
#define H06_SPEED_DOWN_SLOPE_VALUE 0X0606//速度指令减速斜坡时间常数
#define H06_SPEED_REACH_MAX 0X0618//速度到达信号阈值
/*速度控制参数H06*/
#define H06_SPEED_COMMAND_SELEC 0X0602//速度指令选择
#define H06_SPEED_COMMU_SET_VALUE 0X0603//速度指令通讯设置值,当 H06_02=0 时，通过此参数设定电机运行转速
#define H06_SPEED_UP_SLOPE_VALUE 0X0605//速度指令加速斜坡时间常数
#define H06_SPEED_DOWN_SLOPE_VALUE 0X0606//速度指令减速斜坡时间常数
#define H06_SPEED_REACH_MAX 0X0618//速度到达信号阈值
/*RS485通讯与功能参数H0C*/
#define H0C_COMMU_PARAM_EEPR_UPDATE 0X0C13//MODBUS通讯写入是否更新到 EEPROM，设置1为写入
/*RS485通讯与功能参数H0C*/
#define H0C_COMMU_PARAM_EEPR_UPDATE 0X0C13//MODBUS通讯写入是否更新到 EEPROM，设置1为写入
/*监视只读参数*/
#define READ_MOTOR_SPEED_NOW 0X0B00//读取电机滤波后的实时转速
/**
* @brief 伺服电机速度模式写单个寄存器
* @param motorNo：写垂直电机还是水平电机
* @param regAddr：要写的寄存器
* @param data：要向寄存器写入的数据
* @return false:写失败，当前DMA正在发送数据
*/
bool WriteMotorOneReg(uint8_t motorNo, uint16_t regAddr, uint16_t data);
/**
* @brief 伺服电机速度模式写单个寄存器
* @param motorNo：写垂直电机还是水平电机
* @param regAddr：要写的寄存器
* @param data：要向寄存器写入的数据
* @return false:写失败，当前DMA正在发送数据
*/
uint8_t* WriteMotorOneReg(uint8_t motorNo, uint16_t regAddr, int16_t data);
/**
* @brief 写单个寄存器成功后，电机会返回一帧与写格式完全一致的数据
* @param motorNo：写垂直电机还是水平电机
* @param userBuff：用来接收返回的数据的数组
* @param len：接收的数据长度，对于此函数必须为WRITE_ONE_REG_BUFFNUM
* @return false:错误，读出的内容与写入的内容不一致
*/
bool MotorReplyForWrite(uint8_t motorNo, uint8_t* userBuff, uint8_t len);
/**
* @brief 伺服电机驱动初始化
* @param
* @return
*/
void servoMotorInit(void);
uint8_t WriteMotorOneReg_buffer(uint8_t motorNo, uint16_t regAddr, uint16_t data, uint8_t *frameBuff);
/**
* @brief 伺服电机速度模式读单个寄存器
* @param motorNo：读垂直电机还是水平电机
* @param regAddr：要读的寄存器
* @return 返回数组地址
*/
uint8_t* ReadMotorOneReg(uint8_t motorNo, uint16_t regAddr);
/**
* @brief 伺服电机驱动初始化
* @param
* @return
*/
void servoMotorInit(void);
#endif
#endif

2
PROJECT/OS2.ewp
View File

@ -741,7 +741,7 @@
<data>
<extensions></extensions>
<cmdline></cmdline>
<hasPrio>56</hasPrio>
<hasPrio>1</hasPrio>
<buildSequence>inputOutputBased</buildSequence>
</data>
</settings>