修复bug，现在水平垂直发送数据，解析数据都没有问题
This commit is contained in:
parent
165565934a
commit
164ba15a43
|
|
@ -55,88 +55,205 @@
|
||||||
|
|
||||||
/// CE
|
/// CE
|
||||||
#define TASK_TESTQUEUE_PRIO 55u
|
#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 TASK_GET_ANGLE_STK_SIZE 120u
|
||||||
|
|
||||||
#define POWER_OFF_PRIO 17u//原来为7，后导致ping包有延时，造成网络阻塞，UCOSII系统卡死，故优先级低于TCPIP
|
#define POWER_OFF_PRIO 17u//原来为7，后导致ping包有延时，造成网络阻塞，UCOSII系统卡死，故优先级低于TCPIP
|
||||||
#define POWER_OFF_STK_SIZE 200u
|
#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_HORI_PID_PRIO 20u
|
||||||
#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_HORI_PID_STK_SIZE 150u
|
||||||
|
|
||||||
#define TASK_VERT_DIRECTOR_SPEED_PWM_PRIO 31u
|
#define TASK_VERT_DIRECTOR_SPEED_PWM_PRIO 21u
|
||||||
#define TASK_VERT_DIRECTOR_SPEED_PWM_STK_SIZE 150u
|
#define TASK_VERT_DIRECTOR_SPEED_PWM_STK_SIZE 150u
|
||||||
|
/***************/
|
||||||
|
|
||||||
#define TASK_VERT_PID_PRIO 32u
|
|
||||||
|
#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_VERT_PID_STK_SIZE 150u
|
||||||
|
|
||||||
#define TASK_FAULT_DETECT_PRIO 34u
|
#define TASK_FAULT_DETECT_PRIO 35u
|
||||||
#define TASK_FAULT_DETECT_STK_SIZE 180u
|
#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_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_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_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_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_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_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_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_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_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_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_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_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
|
#define TASK_PTZ_ERROR_COUNT_STK_SIZE 200u
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -145,13 +262,7 @@
|
||||||
#define TASK_PTZ_TASK_PRINTF_STK_SIZE 200u
|
#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
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -7,6 +7,8 @@
|
||||||
static BSP_OS_SEM ptz_hori_stop_mutex;//共享资源锁
|
static BSP_OS_SEM ptz_hori_stop_mutex;//共享资源锁
|
||||||
static BSP_OS_SEM ptz_vert_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()
|
float ptz_vert_break_angle()
|
||||||
{
|
{
|
||||||
|
|
@ -49,10 +51,42 @@ void ptz_sem_post_stop_mutex()
|
||||||
BSP_OS_SemPost(&ptz_vert_stop_mutex);
|
BSP_OS_SemPost(&ptz_vert_stop_mutex);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
///云台水平右转
|
||||||
|
#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)
|
void ptz_hori_start(char direction, float speed)
|
||||||
{
|
{
|
||||||
BSP_OS_SemWait(&ptz_hori_stop_mutex, 0u);
|
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*=-1;
|
||||||
|
// default:
|
||||||
|
// break;
|
||||||
|
// }
|
||||||
|
// //设定转动速度
|
||||||
|
// servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, speed)//设置速度
|
||||||
|
// , WRITE_ONE_REG_FRAME_NUM, lowPriority);
|
||||||
|
// 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_vert_stop_count = 0;
|
||||||
BSP_OS_SemPost(&ptz_hori_stop_mutex);
|
BSP_OS_SemPost(&ptz_hori_stop_mutex);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -60,13 +94,73 @@ void ptz_hori_stop(unsigned short int time)
|
||||||
{
|
{
|
||||||
BSP_OS_SemWait(&ptz_hori_stop_mutex, 0u);
|
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);
|
||||||
|
// servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 0)
|
||||||
|
// , WRITE_ONE_REG_FRAME_NUM, lowPriority); //失能电机
|
||||||
|
// 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 ++;
|
||||||
|
// //电子稳定
|
||||||
|
//#ifdef PTZ_ELECTRIC_STABLE_L6235D
|
||||||
|
// ptz_hori_electric_stable_init();
|
||||||
|
//#endif
|
||||||
|
|
||||||
BSP_OS_SemPost(&ptz_hori_stop_mutex);
|
BSP_OS_SemPost(&ptz_hori_stop_mutex);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
///云台垂直向上
|
||||||
|
#define PTZ_VERT_DIR_UP 3//0
|
||||||
|
///云台垂直向下
|
||||||
|
#define PTZ_VERT_DIR_DOWN 1
|
||||||
|
///云台处于停止状态
|
||||||
|
#define PTZ_VERT_DIR_STOP 2
|
||||||
|
*/
|
||||||
void ptz_vert_start(char direction, float speed)
|
void ptz_vert_start(char direction, float speed)
|
||||||
{
|
{
|
||||||
BSP_OS_SemWait(&ptz_vert_stop_mutex, 0u);
|
BSP_OS_SemWait(&ptz_vert_stop_mutex, 0u);
|
||||||
|
|
||||||
|
// switch ( direction )
|
||||||
|
// {
|
||||||
|
// case PTZ_HORI_DIR_RIGHT://伺服电机默认速度为正使云台右转
|
||||||
|
// case PTZ_HORI_DIR_STOP:
|
||||||
|
// break;
|
||||||
|
// case PTZ_HORI_DIR_LEFT:
|
||||||
|
// speed*=-1;
|
||||||
|
// default:
|
||||||
|
// break;
|
||||||
|
// }
|
||||||
|
// //设定转动速度
|
||||||
|
// servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, speed)//设置速度
|
||||||
|
// , WRITE_ONE_REG_FRAME_NUM, lowPriority);
|
||||||
|
// 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_vert_stop_count = 0;
|
||||||
|
|
||||||
BSP_OS_SemPost(&ptz_vert_stop_mutex);
|
BSP_OS_SemPost(&ptz_vert_stop_mutex);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -80,7 +174,7 @@ void ptz_vert_stop(unsigned short int time)
|
||||||
static void ptz_hori_rotate_monitor_task()
|
static void ptz_hori_rotate_monitor_task()
|
||||||
{
|
{
|
||||||
static uint8_t status;
|
static uint8_t status;
|
||||||
|
static int16_t speed = 20;
|
||||||
switch( status )
|
switch( status )
|
||||||
{
|
{
|
||||||
case 0://切换为速度控制模式
|
case 0://切换为速度控制模式
|
||||||
|
|
@ -103,17 +197,25 @@ static void ptz_hori_rotate_monitor_task()
|
||||||
status = 3;
|
status = 3;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case 3://设置运行速度
|
case 3://保存设定的参数并运行
|
||||||
if ( servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, 20)
|
if ( servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 1)
|
||||||
, WRITE_ONE_REG_FRAME_NUM, lowPriority) )
|
, WRITE_ONE_REG_FRAME_NUM, lowPriority) )
|
||||||
status = 4;
|
status = 4;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case 4://保存设定的参数并运行
|
case 4://读取速度
|
||||||
if ( servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 1)
|
if ( servoSendData(horiMotorType, ReadMotorOneReg(H_MOTOR, READ_MOTOR_SPEED_NOW)
|
||||||
, WRITE_ONE_REG_FRAME_NUM, lowPriority) )
|
, READ_ONE_REG_FRAME_NUM, lowPriority) )
|
||||||
status = 3;
|
status = 5;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
case 5://设置运行速度
|
||||||
|
if ( servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, speed)
|
||||||
|
, WRITE_ONE_REG_FRAME_NUM, lowPriority) )
|
||||||
|
speed+=10, speed%=100,
|
||||||
|
status = 4;
|
||||||
|
break;
|
||||||
|
|
||||||
default:
|
default:
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
@ -121,7 +223,52 @@ static void ptz_hori_rotate_monitor_task()
|
||||||
|
|
||||||
static void ptz_vert_rotate_monitor_task()
|
static void ptz_vert_rotate_monitor_task()
|
||||||
{
|
{
|
||||||
|
static uint8_t status;
|
||||||
|
static int16_t speed = 20;
|
||||||
|
switch( status )
|
||||||
|
{
|
||||||
|
case 0://切换为速度控制模式
|
||||||
|
|
||||||
|
if (servoSendData(vertMotorType, WriteMotorOneReg(V_MOTOR, H02_CONTR_MODE_SELEC, 0)
|
||||||
|
, WRITE_ONE_REG_FRAME_NUM, lowPriority) )
|
||||||
|
status = 1;
|
||||||
|
|
||||||
|
break;
|
||||||
|
|
||||||
|
case 1://设置加速度时间常数
|
||||||
|
if ( servoSendData(vertMotorType, WriteMotorOneReg(V_MOTOR, H06_SPEED_UP_SLOPE_VALUE, 3000)
|
||||||
|
, WRITE_ONE_REG_FRAME_NUM, lowPriority) )
|
||||||
|
status = 2;
|
||||||
|
break;
|
||||||
|
|
||||||
|
case 2://设置减速度时间常数
|
||||||
|
if ( servoSendData(vertMotorType, WriteMotorOneReg(V_MOTOR, H06_SPEED_DOWN_SLOPE_VALUE, 2000)
|
||||||
|
, WRITE_ONE_REG_FRAME_NUM, lowPriority) )
|
||||||
|
status = 3;
|
||||||
|
break;
|
||||||
|
|
||||||
|
case 3://保存设定的参数并运行
|
||||||
|
if ( servoSendData(vertMotorType, WriteMotorOneReg(V_MOTOR, H03_DI1_LOGICAL_SELEC, 1)
|
||||||
|
, WRITE_ONE_REG_FRAME_NUM, lowPriority) )
|
||||||
|
status = 4;
|
||||||
|
break;
|
||||||
|
|
||||||
|
case 4://读速度
|
||||||
|
if ( servoSendData(vertMotorType, ReadMotorOneReg(V_MOTOR, READ_MOTOR_SPEED_NOW)
|
||||||
|
, READ_ONE_REG_FRAME_NUM, lowPriority) )
|
||||||
|
status = 5;
|
||||||
|
break;
|
||||||
|
|
||||||
|
case 5://设置运行速度
|
||||||
|
if ( servoSendData(vertMotorType, WriteMotorOneReg(V_MOTOR, H06_SPEED_COMMU_SET_VALUE, speed)
|
||||||
|
, WRITE_ONE_REG_FRAME_NUM, lowPriority) )
|
||||||
|
speed+=10, speed%=100,
|
||||||
|
status = 4;
|
||||||
|
break;
|
||||||
|
|
||||||
|
default:
|
||||||
|
break;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -130,7 +277,7 @@ static void ptz_hori_rotate_task()
|
||||||
while(1)
|
while(1)
|
||||||
{
|
{
|
||||||
ptz_hori_rotate_monitor_task();
|
ptz_hori_rotate_monitor_task();
|
||||||
OSTimeDlyHMSM(0u, 0u, 0u, 3000u);
|
OSTimeDlyHMSM(0u, 0u, 0u, 10u);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -140,7 +287,7 @@ static void ptz_vert_rotate_task()
|
||||||
while(1)
|
while(1)
|
||||||
{
|
{
|
||||||
ptz_vert_rotate_monitor_task();
|
ptz_vert_rotate_monitor_task();
|
||||||
OSTimeDlyHMSM(0u, 0u, 0u, 1u);
|
OSTimeDlyHMSM(0u, 0u, 0u, 10u);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -9,6 +9,220 @@
|
||||||
BSP_OS_SEM g_horiMotorMutex;
|
BSP_OS_SEM g_horiMotorMutex;
|
||||||
BSP_OS_SEM g_vertMotorMutex;
|
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)
|
||||||
|
{
|
||||||
|
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];
|
||||||
|
*speed = ( (*speed) << 8 ) | motorReplybuff[4];
|
||||||
|
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];
|
||||||
|
*speed = ( (*speed) << 8 ) | motorReplybuff[4];
|
||||||
|
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];
|
||||||
|
*speed = ( (*speed) << 8 ) | motorReplybuff[4];
|
||||||
|
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];
|
||||||
|
*speed = ( (*speed) << 8 ) | motorReplybuff[4];
|
||||||
|
servoLinkListMemPut(vertMotorType);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
// servoLinkListMemPut(vertMotorType);
|
||||||
|
}
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
/*!
|
/*!
|
||||||
\brief 水平电机接收任务
|
\brief 水平电机接收任务
|
||||||
\param[in] none
|
\param[in] none
|
||||||
|
|
@ -18,19 +232,17 @@ BSP_OS_SEM g_vertMotorMutex;
|
||||||
static void ptz_recv_hori_servo_task()
|
static void ptz_recv_hori_servo_task()
|
||||||
{
|
{
|
||||||
CPU_INT08U err;
|
CPU_INT08U err;
|
||||||
|
static int16_t s_horiMotorSpeed;
|
||||||
while(1) {
|
while(1) {
|
||||||
OSSemPend(g_horiMotorMutex, 0, &err);
|
OSSemPend(g_horiMotorMutex, 0, &err);
|
||||||
if ( MotorReplyForWrite(H_MOTOR) == true )
|
stopTimeOut(H_MOTOR);
|
||||||
{
|
if ( ( MotorReplyForWrite(H_MOTOR) | MotorReplyForRead(H_MOTOR, &s_horiMotorSpeed) ) == false)
|
||||||
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
{
|
||||||
|
beep_enable();
|
||||||
H_MOTOR_STOP;
|
H_MOTOR_STOP;
|
||||||
}
|
}
|
||||||
|
//释放信号量，通知能发送一次
|
||||||
|
OSSemPost(g_horiSpeedMutex);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -43,12 +255,17 @@ static void ptz_recv_hori_servo_task()
|
||||||
static void ptz_recv_vert_servo_task()
|
static void ptz_recv_vert_servo_task()
|
||||||
{
|
{
|
||||||
CPU_INT08U err;
|
CPU_INT08U err;
|
||||||
|
static int16_t s_vertMotorSpeed;
|
||||||
while(1) {
|
while(1) {
|
||||||
OSSemPend(g_vertMotorMutex, 0, &err);
|
OSSemPend(g_vertMotorMutex, 0, &err);
|
||||||
stopTimeOut(vertMotorType);
|
stopTimeOut(V_MOTOR);
|
||||||
|
if ( ( MotorReplyForWrite(V_MOTOR) | MotorReplyForRead(V_MOTOR, &s_vertMotorSpeed) ) == false)
|
||||||
|
{
|
||||||
|
beep_enable();
|
||||||
|
V_MOTOR_STOP;
|
||||||
|
}
|
||||||
|
//释放信号量，通知能发送一次
|
||||||
|
OSSemPost(g_vertSpeedMutex);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -64,7 +64,7 @@ void servoLinkListMemPut(uint8_t motorType)
|
||||||
//水平电机
|
//水平电机
|
||||||
if (motorType == horiMotorType) {
|
if (motorType == horiMotorType) {
|
||||||
//当前发送完成的数据为高优先级链表中的数据
|
//当前发送完成的数据为高优先级链表中的数据
|
||||||
if (g_servoMotorLinkList.horiMotor.linkListNum == highPriority) {
|
if (g_servoMotorLinkList.horiMotor.linkListPriority == highPriority) {
|
||||||
if (g_servoMotorLinkList.horiMotor.LinkListHead_H != NULL) {
|
if (g_servoMotorLinkList.horiMotor.LinkListHead_H != NULL) {
|
||||||
linkList *ptr;
|
linkList *ptr;
|
||||||
ptr = g_servoMotorLinkList.horiMotor.LinkListHead_H;
|
ptr = g_servoMotorLinkList.horiMotor.LinkListHead_H;
|
||||||
|
|
@ -75,11 +75,12 @@ void servoLinkListMemPut(uint8_t motorType)
|
||||||
g_servoMotorLinkList.horiMotor.LinkListHead_H = NULL;
|
g_servoMotorLinkList.horiMotor.LinkListHead_H = NULL;
|
||||||
g_servoMotorLinkList.horiMotor.LinkListTail_H = NULL;
|
g_servoMotorLinkList.horiMotor.LinkListTail_H = NULL;
|
||||||
}
|
}
|
||||||
|
g_servoMotorLinkList.horiMotor.linkListNum--;
|
||||||
OSMemPut(g_memPtr, (uint8_t *)ptr);
|
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) {
|
if (g_servoMotorLinkList.horiMotor.LinkListHead_L != NULL) {
|
||||||
linkList *ptr;
|
linkList *ptr;
|
||||||
ptr = g_servoMotorLinkList.horiMotor.LinkListHead_L;
|
ptr = g_servoMotorLinkList.horiMotor.LinkListHead_L;
|
||||||
|
|
@ -90,14 +91,14 @@ void servoLinkListMemPut(uint8_t motorType)
|
||||||
g_servoMotorLinkList.horiMotor.LinkListHead_L = NULL;
|
g_servoMotorLinkList.horiMotor.LinkListHead_L = NULL;
|
||||||
g_servoMotorLinkList.horiMotor.LinkListTail_L = NULL;
|
g_servoMotorLinkList.horiMotor.LinkListTail_L = NULL;
|
||||||
}
|
}
|
||||||
|
g_servoMotorLinkList.horiMotor.linkListNum--;
|
||||||
OSMemPut(g_memPtr, (uint8_t *)ptr);
|
OSMemPut(g_memPtr, (uint8_t *)ptr);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
//释放信号量，通知能发送一次
|
// g_servoMotorLinkList.horiMotor.linkListNum--;
|
||||||
// OSSemPost(g_horiSpeedMutex);
|
|
||||||
}
|
}
|
||||||
else if (motorType == vertMotorType) {
|
else if (motorType == vertMotorType) {
|
||||||
if (g_servoMotorLinkList.vertMotor.linkListNum == highPriority) {
|
if (g_servoMotorLinkList.vertMotor.linkListPriority == highPriority) {
|
||||||
if (g_servoMotorLinkList.vertMotor.LinkListHead_H != NULL) {
|
if (g_servoMotorLinkList.vertMotor.LinkListHead_H != NULL) {
|
||||||
linkList *ptr;
|
linkList *ptr;
|
||||||
ptr = g_servoMotorLinkList.vertMotor.LinkListHead_H;
|
ptr = g_servoMotorLinkList.vertMotor.LinkListHead_H;
|
||||||
|
|
@ -108,10 +109,12 @@ void servoLinkListMemPut(uint8_t motorType)
|
||||||
g_servoMotorLinkList.vertMotor.LinkListHead_H = NULL;
|
g_servoMotorLinkList.vertMotor.LinkListHead_H = NULL;
|
||||||
g_servoMotorLinkList.vertMotor.LinkListTail_H = NULL;
|
g_servoMotorLinkList.vertMotor.LinkListTail_H = NULL;
|
||||||
}
|
}
|
||||||
|
g_servoMotorLinkList.vertMotor.linkListNum--;
|
||||||
OSMemPut(g_memPtr, (uint8_t *)ptr);
|
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) {
|
if (g_servoMotorLinkList.vertMotor.LinkListHead_L != NULL) {
|
||||||
linkList *ptr;
|
linkList *ptr;
|
||||||
ptr = g_servoMotorLinkList.vertMotor.LinkListHead_L;
|
ptr = g_servoMotorLinkList.vertMotor.LinkListHead_L;
|
||||||
|
|
@ -122,11 +125,12 @@ void servoLinkListMemPut(uint8_t motorType)
|
||||||
g_servoMotorLinkList.vertMotor.LinkListHead_L = NULL;
|
g_servoMotorLinkList.vertMotor.LinkListHead_L = NULL;
|
||||||
g_servoMotorLinkList.vertMotor.LinkListTail_L = NULL;
|
g_servoMotorLinkList.vertMotor.LinkListTail_L = NULL;
|
||||||
}
|
}
|
||||||
|
g_servoMotorLinkList.vertMotor.linkListNum--;
|
||||||
OSMemPut(g_memPtr, (uint8_t *)ptr);
|
OSMemPut(g_memPtr, (uint8_t *)ptr);
|
||||||
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
//释放信号量，通知能发送一次
|
// g_servoMotorLinkList.vertMotor.linkListNum--;
|
||||||
// OSSemPost(g_vertSpeedMutex);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -189,6 +193,7 @@ bool servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priorit
|
||||||
g_servoMotorLinkList.horiMotor.LinkListHead_H
|
g_servoMotorLinkList.horiMotor.LinkListHead_H
|
||||||
= g_servoMotorLinkList.horiMotor.LinkListTail_H;
|
= g_servoMotorLinkList.horiMotor.LinkListTail_H;
|
||||||
}
|
}
|
||||||
|
g_servoMotorLinkList.horiMotor.linkListNum++;
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
if (g_servoMotorLinkList.horiMotor.LinkListTail_L == NULL) {
|
if (g_servoMotorLinkList.horiMotor.LinkListTail_L == NULL) {
|
||||||
|
|
@ -203,6 +208,7 @@ bool servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priorit
|
||||||
g_servoMotorLinkList.horiMotor.LinkListHead_L
|
g_servoMotorLinkList.horiMotor.LinkListHead_L
|
||||||
= g_servoMotorLinkList.horiMotor.LinkListTail_L;
|
= g_servoMotorLinkList.horiMotor.LinkListTail_L;
|
||||||
}
|
}
|
||||||
|
g_servoMotorLinkList.horiMotor.linkListNum++;
|
||||||
}
|
}
|
||||||
|
|
||||||
//释放信号量，通知能发送一次
|
//释放信号量，通知能发送一次
|
||||||
|
|
@ -227,6 +233,7 @@ bool servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priorit
|
||||||
g_servoMotorLinkList.vertMotor.LinkListHead_H
|
g_servoMotorLinkList.vertMotor.LinkListHead_H
|
||||||
= g_servoMotorLinkList.vertMotor.LinkListTail_H;
|
= g_servoMotorLinkList.vertMotor.LinkListTail_H;
|
||||||
}
|
}
|
||||||
|
g_servoMotorLinkList.vertMotor.linkListNum++;
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
if (g_servoMotorLinkList.vertMotor.LinkListTail_L == NULL) {
|
if (g_servoMotorLinkList.vertMotor.LinkListTail_L == NULL) {
|
||||||
|
|
@ -241,6 +248,7 @@ bool servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priorit
|
||||||
g_servoMotorLinkList.vertMotor.LinkListHead_L
|
g_servoMotorLinkList.vertMotor.LinkListHead_L
|
||||||
= g_servoMotorLinkList.vertMotor.LinkListTail_L;
|
= g_servoMotorLinkList.vertMotor.LinkListTail_L;
|
||||||
}
|
}
|
||||||
|
g_servoMotorLinkList.vertMotor.linkListNum++;
|
||||||
}
|
}
|
||||||
|
|
||||||
//释放信号量，通知能发送一次
|
//释放信号量，通知能发送一次
|
||||||
|
|
@ -269,7 +277,7 @@ static void ptz_hori_servo_task()
|
||||||
CommuTransData(H_MOTOR, g_servoMotorLinkList.horiMotor.LinkListHead_H->data
|
CommuTransData(H_MOTOR, g_servoMotorLinkList.horiMotor.LinkListHead_H->data
|
||||||
, g_servoMotorLinkList.horiMotor.LinkListHead_H->length);
|
, g_servoMotorLinkList.horiMotor.LinkListHead_H->length);
|
||||||
|
|
||||||
g_servoMotorLinkList.horiMotor.linkListNum = highPriority;
|
g_servoMotorLinkList.horiMotor.linkListPriority = highPriority;
|
||||||
startTimeOut(horiMotorType);
|
startTimeOut(horiMotorType);
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
@ -280,7 +288,7 @@ static void ptz_hori_servo_task()
|
||||||
CommuTransData(H_MOTOR, g_servoMotorLinkList.horiMotor.LinkListHead_L->data
|
CommuTransData(H_MOTOR, g_servoMotorLinkList.horiMotor.LinkListHead_L->data
|
||||||
, g_servoMotorLinkList.horiMotor.LinkListHead_L->length);
|
, g_servoMotorLinkList.horiMotor.LinkListHead_L->length);
|
||||||
|
|
||||||
g_servoMotorLinkList.horiMotor.linkListNum = lowPriority;
|
g_servoMotorLinkList.horiMotor.linkListPriority = lowPriority;
|
||||||
startTimeOut(horiMotorType);
|
startTimeOut(horiMotorType);
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
|
|
@ -305,7 +313,7 @@ static void ptz_vert_servo_task()
|
||||||
|
|
||||||
// 高优先级链表中数据先发送
|
// 高优先级链表中数据先发送
|
||||||
if (g_servoMotorLinkList.vertMotor.LinkListHead_H != NULL) {
|
if (g_servoMotorLinkList.vertMotor.LinkListHead_H != NULL) {
|
||||||
g_servoMotorLinkList.vertMotor.linkListNum = highPriority;
|
g_servoMotorLinkList.vertMotor.linkListPriority = highPriority;
|
||||||
CommuTransData(V_MOTOR, g_servoMotorLinkList.vertMotor.LinkListHead_H->data
|
CommuTransData(V_MOTOR, g_servoMotorLinkList.vertMotor.LinkListHead_H->data
|
||||||
, g_servoMotorLinkList.vertMotor.LinkListHead_H->length);
|
, g_servoMotorLinkList.vertMotor.LinkListHead_H->length);
|
||||||
startTimeOut(V_MOTOR);
|
startTimeOut(V_MOTOR);
|
||||||
|
|
@ -314,7 +322,7 @@ static void ptz_vert_servo_task()
|
||||||
|
|
||||||
// 高优先级链表中无数据时，发送低优先级中的数据
|
// 高优先级链表中无数据时，发送低优先级中的数据
|
||||||
if (g_servoMotorLinkList.vertMotor.LinkListHead_L != NULL) {
|
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
|
CommuTransData(V_MOTOR, g_servoMotorLinkList.vertMotor.LinkListHead_L->data
|
||||||
, g_servoMotorLinkList.vertMotor.LinkListHead_L->length);
|
, g_servoMotorLinkList.vertMotor.LinkListHead_L->length);
|
||||||
startTimeOut(V_MOTOR);
|
startTimeOut(V_MOTOR);
|
||||||
|
|
@ -381,11 +389,13 @@ static void creat_task_vert_servo_task(void)
|
||||||
|
|
||||||
void horiServoTimeOut()
|
void horiServoTimeOut()
|
||||||
{
|
{
|
||||||
|
servoLinkListMemPut(horiMotorType);
|
||||||
OSSemPost(g_horiSpeedMutex);
|
OSSemPost(g_horiSpeedMutex);
|
||||||
}
|
}
|
||||||
|
|
||||||
void vertServoTimeOut()
|
void vertServoTimeOut()
|
||||||
{
|
{
|
||||||
|
servoLinkListMemPut(vertMotorType);
|
||||||
OSSemPost(g_vertSpeedMutex);
|
OSSemPost(g_vertSpeedMutex);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -394,7 +404,7 @@ BSP_OS_TMR vertServoSoftWareTim;
|
||||||
void servoCommSoftWareTimInit()
|
void servoCommSoftWareTimInit()
|
||||||
{
|
{
|
||||||
CPU_INT08U ServoSoftWareTimErr;
|
CPU_INT08U ServoSoftWareTimErr;
|
||||||
horiServoSoftWareTim = OSTmrCreate(1
|
horiServoSoftWareTim = OSTmrCreate(100
|
||||||
, 100//1*200ms
|
, 100//1*200ms
|
||||||
, OS_TMR_OPT_ONE_SHOT
|
, OS_TMR_OPT_ONE_SHOT
|
||||||
, (OS_TMR_CALLBACK)horiServoTimeOut
|
, (OS_TMR_CALLBACK)horiServoTimeOut
|
||||||
|
|
@ -408,8 +418,8 @@ void servoCommSoftWareTimInit()
|
||||||
pdebug(DEBUG_LEVEL_FATAL,"create horiServoSoftWareTim failed...\n\r");
|
pdebug(DEBUG_LEVEL_FATAL,"create horiServoSoftWareTim failed...\n\r");
|
||||||
}
|
}
|
||||||
|
|
||||||
vertServoSoftWareTim = OSTmrCreate(1
|
vertServoSoftWareTim = OSTmrCreate(100
|
||||||
, 1
|
, 100
|
||||||
, OS_TMR_OPT_ONE_SHOT
|
, OS_TMR_OPT_ONE_SHOT
|
||||||
, (OS_TMR_CALLBACK)vertServoTimeOut
|
, (OS_TMR_CALLBACK)vertServoTimeOut
|
||||||
, (void *)0
|
, (void *)0
|
||||||
|
|
|
||||||
|
|
@ -33,6 +33,8 @@ typedef struct _linkList {
|
||||||
uint8_t data[sendDataBufLen];
|
uint8_t data[sendDataBufLen];
|
||||||
//发送的数据长度
|
//发送的数据长度
|
||||||
uint16_t length;
|
uint16_t length;
|
||||||
|
// uint8_t funcCode;
|
||||||
|
// uint16_t regAddr;
|
||||||
//下一个节点
|
//下一个节点
|
||||||
struct _linkList *next;
|
struct _linkList *next;
|
||||||
} linkList;
|
} linkList;
|
||||||
|
|
|
||||||
|
|
@ -303,7 +303,6 @@ void DMA0_Channel3_IRQHandler(void)
|
||||||
while(usart_flag_get(g_motorCommuBuff[H_MOTOR].uartNo, USART_FLAG_TC) == RESET); // 等待串口发送完成
|
while(usart_flag_get(g_motorCommuBuff[H_MOTOR].uartNo, USART_FLAG_TC) == RESET); // 等待串口发送完成
|
||||||
H_COMMU_RS485_RX; //485切换为接收
|
H_COMMU_RS485_RX; //485切换为接收
|
||||||
pCommuDeal->dmaTranFlag = DMA_TRANS_IDLE;
|
pCommuDeal->dmaTranFlag = DMA_TRANS_IDLE;
|
||||||
servoLinkListMemPut(horiMotorType);
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -330,8 +329,6 @@ void USART2_IRQHandler(void)
|
||||||
g_MotorDmaBuff[H_MOTOR].dmaRxch);//函数获取的是还有多少个没传输，而不是已经传输了多少
|
g_MotorDmaBuff[H_MOTOR].dmaRxch);//函数获取的是还有多少个没传输，而不是已经传输了多少
|
||||||
//释放信号量，通知能处理数据
|
//释放信号量，通知能处理数据
|
||||||
OSSemPost(g_horiMotorMutex);
|
OSSemPost(g_horiMotorMutex);
|
||||||
//释放信号量，通知能发送一次
|
|
||||||
OSSemPost(g_horiSpeedMutex);
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -374,7 +371,6 @@ void DMA1_Channel7_IRQHandler(void)
|
||||||
CommuInfo_t *pCommuDeal = g_commuInfoBuff[V_MOTOR].pCommuInfo;
|
CommuInfo_t *pCommuDeal = g_commuInfoBuff[V_MOTOR].pCommuInfo;
|
||||||
|
|
||||||
while(usart_flag_get(g_motorCommuBuff[V_MOTOR].uartNo, USART_FLAG_TC) == RESET); // 等待串口发送完成
|
while(usart_flag_get(g_motorCommuBuff[V_MOTOR].uartNo, USART_FLAG_TC) == RESET); // 等待串口发送完成
|
||||||
servoLinkListMemPut(vertMotorType);
|
|
||||||
V_COMMU_RS485_RX; //485切换为接收
|
V_COMMU_RS485_RX; //485切换为接收
|
||||||
pCommuDeal->dmaTranFlag = DMA_TRANS_IDLE;
|
pCommuDeal->dmaTranFlag = DMA_TRANS_IDLE;
|
||||||
}
|
}
|
||||||
|
|
@ -399,12 +395,10 @@ void USART5_IRQHandler(void)
|
||||||
usart_data_receive(g_motorCommuBuff[V_MOTOR].uartNo); //第二步，读取数据寄存器，清除IDLE标志位
|
usart_data_receive(g_motorCommuBuff[V_MOTOR].uartNo); //第二步，读取数据寄存器，清除IDLE标志位
|
||||||
|
|
||||||
g_vertLastPos = g_vertNowPos;
|
g_vertLastPos = g_vertNowPos;
|
||||||
g_vertNowPos = DMA_BUFF_SIZE - dma_transfer_number_get(g_MotorDmaBuff[H_MOTOR].dmaNo,
|
g_vertNowPos = DMA_BUFF_SIZE - dma_transfer_number_get(g_MotorDmaBuff[V_MOTOR].dmaNo,
|
||||||
g_MotorDmaBuff[H_MOTOR].dmaRxch);
|
g_MotorDmaBuff[V_MOTOR].dmaRxch);
|
||||||
//释放信号量，通知接收任务处理
|
//释放信号量，通知接收任务处理
|
||||||
OSSemPost(g_vertMotorMutex);
|
OSSemPost(g_vertMotorMutex);
|
||||||
//释放信号量，通知能发送一次
|
|
||||||
OSSemPost(g_vertSpeedMutex);
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -490,29 +484,31 @@ bool CommuRsvData(uint8_t motorNo, uint8_t* userBuff, uint32_t len)
|
||||||
{
|
{
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
uint16_t horiPosTemp = g_horiLastPos;
|
||||||
|
uint16_t vertPosTemp = g_vertLastPos;
|
||||||
/*接收DMA缓冲区数据*/
|
/*接收DMA缓冲区数据*/
|
||||||
if ( motorNo == H_MOTOR )
|
if ( motorNo == H_MOTOR )
|
||||||
{
|
{
|
||||||
if ( ( (g_horiLastPos + len) % DMA_BUFF_SIZE ) != g_horiNowPos )//假如是写寄存器，电机返回消息正常是8字节，但如果是写错误，则返回非8字节错误码
|
if ( ( (horiPosTemp + len) % DMA_BUFF_SIZE ) != g_horiNowPos )//假如是写寄存器，电机返回消息正常是8字节，但如果是写错误，则返回非8字节错误码
|
||||||
{
|
{
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
for(uint8_t i = 0; i < len; i++)
|
for(uint8_t i = 0; i < len; i++)
|
||||||
{
|
{
|
||||||
*(userBuff+i) = pCommuDeal->pDmaRsvBuff[g_horiLastPos];
|
*(userBuff+i) = pCommuDeal->pDmaRsvBuff[horiPosTemp];
|
||||||
g_horiLastPos = (g_horiLastPos + 1) % DMA_BUFF_SIZE;
|
horiPosTemp = (horiPosTemp + 1) % DMA_BUFF_SIZE;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
if ( ( (g_vertLastPos + len) % DMA_BUFF_SIZE ) != g_vertNowPos )//假如是写寄存器，电机返回消息正常是8字节，但如果是写错误，则返回非8字节错误码
|
if ( ( (vertPosTemp + len) % DMA_BUFF_SIZE ) != g_vertNowPos )//假如是写寄存器，电机返回消息正常是8字节，但如果是写错误，则返回非8字节错误码
|
||||||
{
|
{
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
for(uint8_t i = 0; i < len; i++)
|
for(uint8_t i = 0; i < len; i++)
|
||||||
{
|
{
|
||||||
*(userBuff+i) = pCommuDeal->pDmaRsvBuff[g_vertLastPos];
|
*(userBuff+i) = pCommuDeal->pDmaRsvBuff[vertPosTemp];
|
||||||
g_horiLastPos = (g_vertLastPos + 1) % DMA_BUFF_SIZE;
|
vertPosTemp = (vertPosTemp + 1) % DMA_BUFF_SIZE;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
return true;
|
return true;
|
||||||
|
|
|
||||||
|
|
@ -28,18 +28,13 @@ static void MotorSwitchGpioCofig(void)
|
||||||
* @param data：要向寄存器写入的数据
|
* @param data：要向寄存器写入的数据
|
||||||
* @return 返回数组地址
|
* @return 返回数组地址
|
||||||
*/
|
*/
|
||||||
static uint8_t g_writeOneRegBuff[WRITE_ONE_REG_FRAME_NUM];
|
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, uint16_t data)
|
uint8_t* WriteMotorOneReg(uint8_t motorNo, uint16_t regAddr, uint16_t data)
|
||||||
{
|
{
|
||||||
|
static uint8_t g_writeOneRegBuff[WRITE_ONE_REG_FRAME_NUM];
|
||||||
uint16_t crc;
|
uint16_t crc;
|
||||||
if ( motorNo == H_MOTOR )
|
g_writeOneRegBuff[0] = 0x01;
|
||||||
{
|
|
||||||
g_writeOneRegBuff[0] = H_MOTOR_ADDR;//由于采用一主一从模式，所以水平电机垂直电机从机地址都是0x01，云台后期也不会扩展
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
g_writeOneRegBuff[0] = V_MOTOR_ADDR;
|
|
||||||
}
|
|
||||||
g_writeOneRegBuff[1] = WRITE_ONE_REG;
|
g_writeOneRegBuff[1] = WRITE_ONE_REG;
|
||||||
g_writeOneRegBuff[2] = regAddr >> 8;
|
g_writeOneRegBuff[2] = regAddr >> 8;
|
||||||
g_writeOneRegBuff[3] = (uint8_t)(regAddr & 0xff);
|
g_writeOneRegBuff[3] = (uint8_t)(regAddr & 0xff);
|
||||||
|
|
@ -48,36 +43,17 @@ uint8_t* WriteMotorOneReg(uint8_t motorNo, uint16_t regAddr, uint16_t data)
|
||||||
crc = ModbusCRC16(g_writeOneRegBuff, WRITE_ONE_REG_FRAME_NUM - 2);
|
crc = ModbusCRC16(g_writeOneRegBuff, WRITE_ONE_REG_FRAME_NUM - 2);
|
||||||
g_writeOneRegBuff[6] = (uint8_t)(crc & 0xff);
|
g_writeOneRegBuff[6] = (uint8_t)(crc & 0xff);
|
||||||
g_writeOneRegBuff[7] = crc >> 8;
|
g_writeOneRegBuff[7] = crc >> 8;
|
||||||
return g_writeOneRegBuff;
|
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;
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief 写单个寄存器成功后，电机会返回一帧与写格式完全一致的数据
|
|
||||||
* @param motorNo：写垂直电机还是水平电机
|
|
||||||
* @return false:错误，读出的内容与写入的内容不一致
|
|
||||||
*/
|
|
||||||
static uint8_t motorReplybuff[WRITE_ONE_REG_FRAME_NUM];
|
|
||||||
bool MotorReplyForWrite(uint8_t motorNo)
|
|
||||||
{
|
|
||||||
|
|
||||||
/*提取数据失败*/
|
|
||||||
if ( CommuRsvData(motorNo, motorReplybuff, WRITE_ONE_REG_FRAME_NUM) == false )
|
|
||||||
{
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
for( uint8_t i = 0; i < WRITE_ONE_REG_FRAME_NUM; i++ )
|
|
||||||
{
|
|
||||||
/*提取的数据不对*/
|
|
||||||
if ( motorReplybuff[i] != g_writeOneRegBuff[i] )
|
|
||||||
{
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
/*
|
/*
|
||||||
从机地址 功能码 寄存器地址高 寄存器地址低 reg数量高位 数量低位 crc校验高位 crc校验低位
|
从机地址 功能码 寄存器地址高 寄存器地址低 reg数量高位 数量低位 crc校验高位 crc校验低位
|
||||||
|
|
@ -89,18 +65,13 @@ bool MotorReplyForWrite(uint8_t motorNo)
|
||||||
* @param regAddr：要读的寄存器
|
* @param regAddr：要读的寄存器
|
||||||
* @return 返回数组地址
|
* @return 返回数组地址
|
||||||
*/
|
*/
|
||||||
static uint8_t g_readOneRegBuff[READ_ONE_REG_FRAME_NUM];
|
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)
|
uint8_t* ReadMotorOneReg(uint8_t motorNo, uint16_t regAddr)
|
||||||
{
|
{
|
||||||
|
static uint8_t g_readOneRegBuff[READ_ONE_REG_FRAME_NUM];
|
||||||
uint16_t crc;
|
uint16_t crc;
|
||||||
if ( motorNo == H_MOTOR )
|
g_readOneRegBuff[0] = 0x01;
|
||||||
{
|
|
||||||
g_readOneRegBuff[0] = H_MOTOR_ADDR;//由于采用一主一从模式，所以水平电机垂直电机从机地址都是0x01，云台后期也不会扩展
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
g_readOneRegBuff[0] = V_MOTOR_ADDR;
|
|
||||||
}
|
|
||||||
g_readOneRegBuff[1] = READ_ONE_REG;
|
g_readOneRegBuff[1] = READ_ONE_REG;
|
||||||
g_readOneRegBuff[2] = regAddr >> 8;
|
g_readOneRegBuff[2] = regAddr >> 8;
|
||||||
g_readOneRegBuff[3] = regAddr & 0xff;
|
g_readOneRegBuff[3] = regAddr & 0xff;
|
||||||
|
|
@ -109,40 +80,18 @@ uint8_t* ReadMotorOneReg(uint8_t motorNo, uint16_t regAddr)
|
||||||
crc = ModbusCRC16(g_readOneRegBuff, READ_ONE_REG_FRAME_NUM - 2);
|
crc = ModbusCRC16(g_readOneRegBuff, READ_ONE_REG_FRAME_NUM - 2);
|
||||||
g_readOneRegBuff[6] = crc & 0xff;
|
g_readOneRegBuff[6] = crc & 0xff;
|
||||||
g_readOneRegBuff[7] = crc >> 8;
|
g_readOneRegBuff[7] = crc >> 8;
|
||||||
return g_readOneRegBuff;
|
if ( motorNo == H_MOTOR )
|
||||||
}
|
|
||||||
/*
|
|
||||||
从机地址 功能码 返回数据字节数 返回数据高位 返回数据低位 crc校验高位 crc校验低位
|
|
||||||
01H 03H 02H 00H 00H B8H 44H
|
|
||||||
*/
|
|
||||||
/**
|
|
||||||
* @brief 读单个寄存器命令发送成功后，电机会返回一帧7字节的数据
|
|
||||||
* @param motorNo：写垂直电机还是水平电机
|
|
||||||
* @param data：写垂直电机还是水平电机
|
|
||||||
* @return false:错误，读出的内容与写入的内容不一致
|
|
||||||
*/
|
|
||||||
bool MotorReplyForRead(uint8_t motorNo, uint16_t* data)
|
|
||||||
{
|
{
|
||||||
/*简易判断*/
|
memcpy(g_HreadOneRegBuff, g_readOneRegBuff, WRITE_ONE_REG_FRAME_NUM);
|
||||||
if ( data == NULL )
|
return g_HreadOneRegBuff;
|
||||||
{
|
|
||||||
return false;
|
|
||||||
}
|
}
|
||||||
static uint8_t motorReplybuff[WRITE_ONE_REG_FRAME_NUM - 1];
|
|
||||||
/*提取数据失败*/
|
memcpy(g_VreadOneRegBuff, g_readOneRegBuff, WRITE_ONE_REG_FRAME_NUM);
|
||||||
if ( CommuRsvData(motorNo, motorReplybuff, WRITE_ONE_REG_FRAME_NUM - 1) == false )
|
return g_VreadOneRegBuff;
|
||||||
{
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
*data = motorReplybuff[3];
|
|
||||||
*data = (*data << 8) | motorReplybuff[4];
|
|
||||||
}
|
|
||||||
return true;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief 伺服电机驱动初始化
|
* @brief 伺服电机驱动初始化
|
||||||
* @param
|
* @param
|
||||||
|
|
|
||||||
|
|
@ -52,6 +52,9 @@
|
||||||
/*RS485通讯与功能参数H0C*/
|
/*RS485通讯与功能参数H0C*/
|
||||||
#define H0C_COMMU_PARAM_EEPR_UPDATE 0X0C13//MODBUS通讯写入是否更新到 EEPROM，设置1为写入
|
#define H0C_COMMU_PARAM_EEPR_UPDATE 0X0C13//MODBUS通讯写入是否更新到 EEPROM，设置1为写入
|
||||||
|
|
||||||
|
/*监视只读参数*/
|
||||||
|
#define READ_MOTOR_SPEED_NOW 0X0B00//读取电机滤波后的实时转速
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -64,12 +67,6 @@
|
||||||
*/
|
*/
|
||||||
uint8_t* WriteMotorOneReg(uint8_t motorNo, uint16_t regAddr, uint16_t data);
|
uint8_t* WriteMotorOneReg(uint8_t motorNo, uint16_t regAddr, uint16_t data);
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief 写单个寄存器成功后，电机会返回一帧与写格式完全一致的数据
|
|
||||||
* @param motorNo：写垂直电机还是水平电机
|
|
||||||
* @return false:错误，读出的内容与写入的内容不一致
|
|
||||||
*/
|
|
||||||
bool MotorReplyForWrite(uint8_t motorNo);
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief 伺服电机速度模式读单个寄存器
|
* @brief 伺服电机速度模式读单个寄存器
|
||||||
|
|
@ -79,13 +76,8 @@ bool MotorReplyForWrite(uint8_t motorNo);
|
||||||
*/
|
*/
|
||||||
uint8_t* ReadMotorOneReg(uint8_t motorNo, uint16_t regAddr);
|
uint8_t* ReadMotorOneReg(uint8_t motorNo, uint16_t regAddr);
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief 读单个寄存器命令发送成功后，电机会返回一帧7字节的数据
|
|
||||||
* @param motorNo：写垂直电机还是水平电机
|
|
||||||
* @param data：写垂直电机还是水平电机
|
|
||||||
* @return false:错误，读出的内容与写入的内容不一致
|
|
||||||
*/
|
|
||||||
bool MotorReplyForRead(uint8_t motorNo, uint16_t* data);
|
|
||||||
/**
|
/**
|
||||||
* @brief 伺服电机驱动初始化
|
* @brief 伺服电机驱动初始化
|
||||||
* @param
|
* @param
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue