diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..669c54d --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,6 @@ +{ + "files.associations": { + "service_error_count.h": "c", + "ptz_header_file.h": "c" + } +} \ No newline at end of file diff --git a/APP/Common/comm_cfginfo.h b/APP/Common/comm_cfginfo.h index 99289ba..a88b3c1 100644 --- a/APP/Common/comm_cfginfo.h +++ b/APP/Common/comm_cfginfo.h @@ -156,9 +156,10 @@ extern "C" { #define HYT_ADDRESS "hyt_address=" #define HYT_ADDRESS_DEFAULT "1" #define ERROR_compensation "compensation_switch=" -#ifdef PTZ_BLDC_MOTOR - #define ERROR_compensation_switch "on"//误差补偿开关,直流无刷电机原则上不使用误差补偿，而使用重复定位 -#endif +// #ifdef PTZ_BLDC_MOTOR +// #define ERROR_compensation_switch "on"//误差补偿开关,直流无刷电机原则上不使用误差补偿，而使用重复定位 +// #endif +#define ERROR_compensation_switch "on"//误差补偿开关,直流无刷电机原则上不使用误差补偿，而使用重复定位 /**/ #define END "[end]" diff --git a/APP/Common/ptz_default_value.h b/APP/Common/ptz_default_value.h index 0b4b671..3d36bde 100644 --- a/APP/Common/ptz_default_value.h +++ b/APP/Common/ptz_default_value.h @@ -508,7 +508,70 @@ #endif +//ŷ +#ifdef PTZ_MEDIUM_WORM_SERVO_MOTOR_24V + ///ˮƽٱ + #define PTZ_HORI_MOTOR_RATIO 1.0 + ///ˮƽּٱ + #define PTZ_HORI_BIG_GEAR_RATIO 60.0 + ///ˮƽܼٱ + #define PTZ_HORI_RATIO 60.0 + + ///תٵλ ת/ÿ + ///ˮƽת + #define PTZ_HORI_MOTOR_MAX_SPEED 600.0 + ///ˮƽСת + #define PTZ_HORI_MOTOR_MIN_SPEED 1.0 + ///ˮƽ̨ת + #define PTZ_HORI_MAX_SPEED 10.0 + ///ˮƽ̨Сת + #define PTZ_HORI_MIN_SPEED 0.02 + ///ˮƽ̨Ĭٶ + #define PTZ_HORI_BEST_SPEED 2.0 //ĬٶȲܹӦΪӦŤֵ + ///Ҫָλʱĵɲת(λr/min) + #define PTZ_HORI_BREAK_MOTOR_SPEED 15.0 + ///Ҫָλʱɲת(λ/s) + #define PTZ_HORI_BREAK_SPEED (PTZ_HORI_BREAK_MOTOR_SPEED / PTZ_HORI_RATIO * 6.0) + + ///ˮƽ 1- 0- + #define PTZ_HORI_MOTOR_DIRECTION 1 + ///ˮƽٶʱ䳣 λms (0-->>1000rpm) + #define PTZ_HORI_MOTOR_AccelerationTimeConstant 3000 + ///ˮƽٶʱ䳣 λms (1000-->>0rpm) + #define PTZ_HORI_MOTOR_DecelerationTimeConstant 2000 + /**********************************************************/ + ///ű͵֮ļٱ + #define PTZ_VERT_MOTOR_RATIO 1.0 + ///ų֮̋ļٱ + #define PTZ_VERT_BIG_GEAR_RATIO 60.0 + ///ֱܼٱ + #define PTZ_VERT_RATIO 60.0 + + ///תٵλ ת/ÿ + ///ֱת + #define PTZ_VERT_MOTOR_MAX_SPEED 600.0 + ///ֱСת + #define PTZ_VERT_MOTOR_MIN_SPEED 1.0 + ///ֱ̨ת + #define PTZ_VERT_MAX_SPEED 10.0 + ///ֱ̨Сת + #define PTZ_VERT_MIN_SPEED 0.02 + ///ֱ̨Ĭٶ + #define PTZ_VERT_BEST_SPEED 2.0//6.0 + ///Ҫָλʱĵɲת + #define PTZ_VERT_BREAK_MOTOR_SPEED 15.0 + ///Ҫָλʱɲת + #define PTZ_VERT_BREAK_SPEED (PTZ_VERT_BREAK_MOTOR_SPEED / PTZ_VERT_RATIO) + + ///ֱ 1- 0- + #define PTZ_VERT_MOTOR_DIRECTION 1 + ///ֱٶʱ䳣 λms (0-->>1000rpm) + #define PTZ_VERT_MOTOR_AccelerationTimeConstant 3000 + ///ֱٶʱ䳣 λms (1000-->>0rpm) + #define PTZ_VERT_MOTOR_DecelerationTimeConstant 2000 + +#endif #endif \ No newline at end of file diff --git a/APP/Common/ptz_global_variable.c b/APP/Common/ptz_global_variable.c index f8f25c5..4191c89 100644 --- a/APP/Common/ptz_global_variable.c +++ b/APP/Common/ptz_global_variable.c @@ -114,4 +114,11 @@ void ptz_return_clear() #endif +//====ŷ-24V +#ifdef PTZ_MEDIUM_WORM_SERVO_MOTOR_24V + + unsigned char g_program_version_number[50] = "SL_YT"; + unsigned char g_ptz_type[60] = "SL_YT"; + +#endif diff --git a/APP/Common/ptz_header_file.h b/APP/Common/ptz_header_file.h index 1b6fb36..9caae20 100644 --- a/APP/Common/ptz_header_file.h +++ b/APP/Common/ptz_header_file.h @@ -27,7 +27,7 @@ #include "service_statusmonitor.h" #include "as5047d.h" #include "rotate_step.h" - +#include "rotate_servo.h" #endif \ No newline at end of file diff --git a/APP/Common/ptz_type_select.h b/APP/Common/ptz_type_select.h index e93dbec..f640fdc 100644 --- a/APP/Common/ptz_type_select.h +++ b/APP/Common/ptz_type_select.h @@ -4,12 +4,12 @@ /**********ѡ**********/ //ʹˢֱ -#define PTZ_BLDC_MOTOR 1 +// #define PTZ_BLDC_MOTOR 1 //====ֱˢѡ==== //#define L6235D 3 //ȫ -#define Full_bridge 3 +// #define Full_bridge 3 //ʹò @@ -18,6 +18,8 @@ //====ѡ==== //#define TMC2160 3 +//ʹŷ +#define PTZ_SERVO_MOTOR 3 /**********̨ͺѡ**********/ @@ -25,7 +27,7 @@ //#define PTZ_HEAVY_WORM_L6235D_AS5047D 7 //====ֱˢ-L6235D-ϸ-̨-24V -#define PTZ_MEDIUM_WORM_L6235D_AS5047D 8 +// #define PTZ_MEDIUM_WORM_L6235D_AS5047D 8 //====ֱˢ-L6235D-ֱ-̨-24V //#define PTZ_LIGHT_GEAR_L6235D_AS5047D 9 @@ -44,6 +46,8 @@ //====-TMC2160-ϸ-̨-24V //#define PTZ_MEDIUM_WORM_STEP_TMC2160_AS5047D_24V 105 +//====ŷ-ϸ-̨-24V +#define PTZ_MEDIUM_WORM_SERVO_MOTOR_24V 107 /************ѡ***********/ diff --git a/APP/Device/Device_angle/angle_poweroffsave.h b/APP/Device/Device_angle/angle_poweroffsave.h index 43709f7..f47b6ea 100644 --- a/APP/Device/Device_angle/angle_poweroffsave.h +++ b/APP/Device/Device_angle/angle_poweroffsave.h @@ -43,6 +43,9 @@ #define PTZ_POWER_DOWN_INC 0.2 //ѹ͵ #endif +#ifdef PTZ_MEDIUM_WORM_SERVO_MOTOR_24V + #define PTZ_POWER_DOWN_INC 0.2 //ѹ͵ +#endif #define PTZ_MB85RC64_ADD_A 0x0000//洢ַA #define PTZ_MB85RC64_ADD_B 0x0400//洢ַB diff --git a/APP/Device/Device_angle/get_angle.c b/APP/Device/Device_angle/get_angle.c index fa1bc50..fc4acb5 100644 --- a/APP/Device/Device_angle/get_angle.c +++ b/APP/Device/Device_angle/get_angle.c @@ -10,6 +10,8 @@ #include "ptz_type_select.h" #include "ptz_global_variable.h" +#include "rotate_servo.h" + ///̨ǶȻظ void ptz_send_angle(char dev,char angle_choice) @@ -173,6 +175,58 @@ static void ptz_vert_ps_delay_ms() } #endif + + +#ifdef PTZ_SERVO_MOTOR + +static void ptz_hori_ps_delay_ms() +{ +// unsigned int hori_ps_time = 0; +//#ifdef PTZ_PHOTOELECTRIC_SWITCH +// if(g_ptz.hori_speed_actual > 0) +// { +// hori_ps_time = (unsigned int)((PTZ_HORI_MAX_SPEED / g_ptz.hori_speed_actual) * PTZ_HORI_PS_TIEM + 0.5); +// } +// else +// { +// hori_ps_time = (unsigned int)((PTZ_HORI_MAX_SPEED / PTZ_HORI_MIN_SPEED) * PTZ_HORI_PS_TIEM + 0.5); +// } +// +// if(hori_ps_time < PTZ_HORI_PS_TIEM) +// { +// hori_ps_time = PTZ_HORI_PS_TIEM; +// } +// +// OSTimeDlyHMSM(0u, 0u, 0u, hori_ps_time); +//#endif +// +} + +static void ptz_vert_ps_delay_ms() +{ +// unsigned int vert_ps_time = 0; +// +//#ifdef PTZ_PHOTOELECTRIC_SWITCH +// if(g_ptz.vert_speed_actual > 0) +// { +// vert_ps_time = (unsigned int)((PTZ_VERT_MAX_SPEED / g_ptz.vert_speed_actual) * PTZ_VERT_PS_TIEM + 0.5); +// } +// else +// { +// vert_ps_time = (unsigned int)((PTZ_VERT_MAX_SPEED / PTZ_VERT_MIN_SPEED) * PTZ_VERT_PS_TIEM + 0.5); +// } +// +// if(vert_ps_time < PTZ_VERT_PS_TIEM) +// { +// vert_ps_time = PTZ_VERT_PS_TIEM; +// } +// +// OSTimeDlyHMSM(0u, 0u, 0u, vert_ps_time); +//#endif +} + +#endif + /// ̨ˮƽǶ #ifdef PTZ_HORI_ANGLE_AS5047D_ANGLE_ASY static char ptz_hori_get_angle() diff --git a/APP/Device/Device_angle/get_angle.h b/APP/Device/Device_angle/get_angle.h index 62a0c79..fc49aea 100644 --- a/APP/Device/Device_angle/get_angle.h +++ b/APP/Device/Device_angle/get_angle.h @@ -515,7 +515,47 @@ #define HORI_MS_SW3_ERROR 25//ôſأʵʱſطΧʱõֵ #endif +//ŷ +#ifdef PTZ_MEDIUM_WORM_SERVO_MOTOR_24V + //űǶ̨ǶȼСűǶȼС̨Ƕ + #define PTZ_HORI_ANGLE_AS5047D_ANGLE_SYN 1//Ƕȱ仯ͬ + #define PTZ_VERT_ANGLE_AS5047D_ANGLE_SYN 1//Ƕȱ仯ͬ + //#define PTZ_HORI_GET_ANGLE_MANY 1//λȡǶƽֵ + //#define PTZ_VERT_GET_ANGLE_MANY 1//λȡǶƽֵ + #define PTZ_HORI_GET_ANGLE_SINGLE 1//λȡǶ + #define PTZ_VERT_GET_ANGLE_SINGLE 1//λȡǶ + + #define PTZ_SW3_LEFT_RISE_UPDATE 1 //ˮƽƬתظˮƽǶ + //#define PTZ_SW1_DOWN_FALL_UPDATE 1 //ֱSW1¸½ظ´ֱǶ + #define PTZ_SW1_UP_RISE_UPDATE 1 //ֱSW1ظ´ֱǶ + + + #define PTZ_RIGHT_CYCLE_C (g_ptz.hori_speed_actual * PTZ_HORI_BIG_GEAR_RATIO * 6.0 / 1000.0 * PTZ_HORI_ANGLE_TIME + 5.0)//36.0 //תһȦǷ񵽴ͬһľֵ + + #define PTZ_LEFT_CYCLE_C (g_ptz.hori_speed_actual * PTZ_HORI_BIG_GEAR_RATIO * 6.0 / 1000.0 * PTZ_HORI_ANGLE_TIME + 5.0)//36.0 //תһȦǷ񵽴ͬһľֵ + + #define PTZ_UP_CYCLE_C (g_ptz.vert_speed_actual * PTZ_VERT_BIG_GEAR_RATIO * 6.0 / 1000.0 * PTZ_VERT_ANGLE_TIME + 5.0)//36.0 //תһȦǷ񵽴ͬһľֵ + + #define PTZ_DOWN_CYCLE_C (g_ptz.vert_speed_actual * PTZ_VERT_BIG_GEAR_RATIO * 6.0 / 1000.0 * PTZ_VERT_ANGLE_TIME + 5.0)//36.0 //תһȦǷ񵽴ͬһľֵ + + #define PTZ_HORI_ANGLE_READ_NUM 4 //Ƕȡ + #define PTZ_VERT_ANGLE_READ_NUM 4 //Ƕȡ + + #define PTZ_VERT_AS5047D_ANGLE_WF_A (g_ptz.vert_speed_actual * PTZ_VERT_BIG_GEAR_RATIO * 6.0 / 1000.0 * PTZ_VERT_ANGLE_TIME + 5.0)//36.0//ֱ̨űܽǶ˲ + + #define PTZ_HORI_AS5047D_ANGLE_WF_A (g_ptz.hori_speed_actual * PTZ_HORI_BIG_GEAR_RATIO * 6.0 / 1000.0 * PTZ_HORI_ANGLE_TIME + 5.0)//36.0//̨ˮƽűܽǶ˲ + #define PTZ_HORI_AS5047D_ANGLE_WF_B (g_ptz.hori_as5047d.as5047d_ptz_angle_max - (g_ptz.hori_speed_actual * PTZ_HORI_BIG_GEAR_RATIO * 6.0 / 1000.0 * PTZ_HORI_ANGLE_TIME + 5.0))//̨ˮƽűܽǶ˲ + + + #define PTZ_HORI_ANGLE_DIFF_A (g_ptz.hori_speed_actual * PTZ_HORI_BIG_GEAR_RATIO * 6.0 / 1000.0 * PTZ_HORI_ANGLE_TIME + 5.0) //ϴβɼǶȺʹ˴βɼǶ֮IJֵ + #define PTZ_HORI_ANGLE_DIFF_B (360.0 - PTZ_HORI_ANGLE_DIFF_A) //ϴβɼǶȺʹ˴βɼǶ֮IJֵ + + #define PTZ_VERT_ANGLE_DIFF_A (g_ptz.vert_speed_actual * PTZ_VERT_BIG_GEAR_RATIO * 6.0 / 1000.0 * PTZ_VERT_ANGLE_TIME + 5.0) //ϴβɼǶȺʹ˴βɼǶ֮IJֵ + #define PTZ_VERT_ANGLE_DIFF_B (360.0 - PTZ_VERT_ANGLE_DIFF_A) //ϴβɼǶȺʹ˴βɼǶ֮IJֵ + + #define HORI_MS_SW3_ERROR 25//ôſأʵʱſطΧʱõֵ +#endif void ptz_send_sw_angle_state(char dev); void init_angle_module(); diff --git a/APP/Device/Device_rotate/rotate_servo.c b/APP/Device/Device_rotate/rotate_servo.c new file mode 100644 index 0000000..eb4b9e5 --- /dev/null +++ b/APP/Device/Device_rotate/rotate_servo.c @@ -0,0 +1,174 @@ + +#include "rotate_servo.h" + + +#ifdef PTZ_MEDIUM_WORM_SERVO_MOTOR_24V + +static BSP_OS_SEM ptz_hori_stop_mutex;//共享资源锁 +static BSP_OS_SEM ptz_vert_stop_mutex; + + +float ptz_vert_break_angle() +{ + //云台水平轴加速度(°/s²) + const float a = 1000.0f * 6.0f + / ((float)(PTZ_VERT_MOTOR_DecelerationTimeConstant / 1000.0f)) + / PTZ_VERT_RATIO; + + const float tmp = a / 2.0f; + + //平稳运行角度，以最低转速运行1秒 + const float s1 = PTZ_VERT_BREAK_SPEED * 1; + + //当前云台水平轴转速(°/s) + float Vnow = g_ptz.hori_speed_actual * 6.0f; + + return (Vnow - PTZ_VERT_BREAK_SPEED) * (Vnow + PTZ_VERT_BREAK_SPEED) / tmp + s1; +} +float ptz_hori_break_angle() +{ + //云台水平轴加速度(°/s²) + const float a = 1000.0f * 6.0f + / ((float)(PTZ_HORI_MOTOR_DecelerationTimeConstant / 1000.0f)) + / PTZ_HORI_RATIO; + + const float tmp = a / 2.0f; + + //平稳运行角度，以最低转速运行1秒 + const float s1 = PTZ_HORI_BREAK_SPEED * 1; + + //当前云台水平轴转速(°/s) + float Vnow = g_ptz.hori_speed_actual * 6.0f; + + return (Vnow - PTZ_HORI_BREAK_SPEED) * (Vnow + PTZ_HORI_BREAK_SPEED) / tmp + s1; +} + +void ptz_sem_post_stop_mutex() +{ + BSP_OS_SemPost(&ptz_hori_stop_mutex); + BSP_OS_SemPost(&ptz_vert_stop_mutex); +} + +void ptz_hori_start(char direction, float speed) +{ + BSP_OS_SemWait(&ptz_hori_stop_mutex, 0u); + + BSP_OS_SemPost(&ptz_hori_stop_mutex); +} + +void ptz_hori_stop(unsigned short int time) +{ + BSP_OS_SemWait(&ptz_hori_stop_mutex, 0u); + + BSP_OS_SemPost(&ptz_hori_stop_mutex); +} + +void ptz_vert_start(char direction, float speed) +{ + BSP_OS_SemWait(&ptz_vert_stop_mutex, 0u); + + BSP_OS_SemPost(&ptz_vert_stop_mutex); +} + +void ptz_vert_stop(unsigned short int time) +{ + BSP_OS_SemWait(&ptz_vert_stop_mutex, 0u); + + BSP_OS_SemPost(&ptz_vert_stop_mutex); +} + +static void ptz_hori_rotate_monitor_task() +{ + +} + +static void ptz_vert_rotate_monitor_task() +{ + +} + + +static void ptz_hori_rotate_task() +{ + while(1) + { + ptz_hori_rotate_monitor_task(); + OSTimeDlyHMSM(0u, 0u, 0u, 1u); + } +} + + +static void ptz_vert_rotate_task() +{ + while(1) + { + ptz_vert_rotate_monitor_task(); + OSTimeDlyHMSM(0u, 0u, 0u, 1u); + } +} + +static OS_STK task_hori_rotate_stk[TASK_HORI_ROATE_MONITOR_STK_SIZE]; +static void creat_task_hori_rotate(void) +{ + CPU_INT08U task_err; + CPU_INT08U name_err; + + task_err = OSTaskCreateExt((void (*)(void *)) ptz_hori_rotate_task, + (void *) 0, + (OS_STK *)&task_hori_rotate_stk[TASK_HORI_ROATE_MONITOR_STK_SIZE - 1], + (INT8U ) TASK_HORI_ROATE_MONITOR_PRIO, + (INT16U ) TASK_HORI_ROATE_MONITOR_PRIO, + (OS_STK *)&task_hori_rotate_stk[0], + (INT32U ) TASK_HORI_ROATE_MONITOR_STK_SIZE, + (void *) 0, + (INT16U )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR)); + #if (OS_TASK_NAME_EN > 0) + OSTaskNameSet(TASK_HORI_ROATE_MONITOR_PRIO, "ptz_hori_rotate_task", &name_err); + #endif + + if ((task_err == OS_ERR_NONE) && (name_err == OS_ERR_NONE)) { + pdebug(DEBUG_LEVEL_INFO,"create ptz_hori_rotate_task success...\n\r"); + } else { + pdebug(DEBUG_LEVEL_FATAL,"create ptz_hori_rotate_task failed...\n\r"); + } +} + + + +static OS_STK task_vert_rotate_stk[TASK_VERT_ROATE_MONITOR_STK_SIZE]; +static void creat_task_vert_rotate(void) +{ + CPU_INT08U task_err; + CPU_INT08U name_err; + + task_err = OSTaskCreateExt((void (*)(void *)) ptz_vert_rotate_task, + (void *) 0, + (OS_STK *)&task_vert_rotate_stk[TASK_VERT_ROATE_MONITOR_STK_SIZE - 1], + (INT8U ) TASK_VERT_ROATE_MONITOR_PRIO, + (INT16U ) TASK_VERT_ROATE_MONITOR_PRIO, + (OS_STK *)&task_vert_rotate_stk[0], + (INT32U ) TASK_VERT_ROATE_MONITOR_STK_SIZE, + (void *) 0, + (INT16U )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR)); + #if (OS_TASK_NAME_EN > 0) + OSTaskNameSet(TASK_VERT_ROATE_MONITOR_PRIO, "ptz_vert_rotate_task", &name_err); + #endif + + if ((task_err == OS_ERR_NONE) && (name_err == OS_ERR_NONE)) { + pdebug(DEBUG_LEVEL_INFO,"create ptz_vert_rotate_task success...\n\r"); + } else { + pdebug(DEBUG_LEVEL_FATAL,"create ptz_vert_rotate_task failed...\n\r"); + } +} + +void init_rotate_monitor_module(void) +{ + BSP_OS_SemCreate(&ptz_hori_stop_mutex,1u,"ptz_hori_stop_mutex"); + BSP_OS_SemCreate(&ptz_vert_stop_mutex,1u,"ptz_vert_stop_mutex"); + + creat_task_hori_rotate(); + creat_task_vert_rotate(); +} + + +#endif \ No newline at end of file diff --git a/APP/Device/Device_rotate/rotate_servo.h b/APP/Device/Device_rotate/rotate_servo.h new file mode 100644 index 0000000..b2c3a2f --- /dev/null +++ b/APP/Device/Device_rotate/rotate_servo.h @@ -0,0 +1,55 @@ +#ifndef __PTZ_ROATE_SERVO_H_ +#define __PTZ_ROATE_SERVO_H_ + +#include "ptz_header_file.h" +#include "ptz_type_select.h" + +#ifdef PTZ_MEDIUM_WORM_SERVO_MOTOR_24V + + ///云台水平右转 + #define PTZ_HORI_DIR_RIGHT 3//0 + ///云台水平左转 + #define PTZ_HORI_DIR_LEFT 1 + ///云台处于停止状态 + #define PTZ_HORI_DIR_STOP 2 + + ///云台垂直向上 + #define PTZ_VERT_DIR_UP 3//0 + ///云台垂直向下 + #define PTZ_VERT_DIR_DOWN 1 + ///云台处于停止状态 + #define PTZ_VERT_DIR_STOP 2 + + ///距离最近刹车点以最小转速运行的距离 + #define PTZ_HORI_BREAK_SPEED_ANGLE ptz_hori_break_angle() + #define PTZ_VERT_BREAK_SPEED_ANGLE ptz_vert_break_angle() + + ///刹车最近距离 + #define PTZ_HORI_STOP_NEAR_DISTANCE 0.01 + ///刹车最远距离 + #define PTZ_HORI_STOP_FAR_DISTANCE 4.0 + + ///刹车最近距离 + #define PTZ_VERT_STOP_NEAR_DISTANCE 0.01 + ///刹车最远距离 + #define PTZ_VERT_STOP_FAR_DISTANCE 4.0 + + ///云台定位精度，也可用于判断是不是同一个位置 + #define PTZ_HORI_ANGLE_ACCURACY 0.05 + ///云台定位精度，也可用于判断是不是同一个位置 + #define PTZ_VERT_ANGLE_ACCURACY 0.05 + +float ptz_vert_break_angle(); +float ptz_hori_break_angle(); + +void ptz_sem_post_stop_mutex(); +void ptz_hori_start(char direction, float speed); +void ptz_hori_stop(unsigned short int time); +void ptz_vert_start(char direction, float speed); +void ptz_vert_stop(unsigned short int time); + +void init_rotate_monitor_module(void); + +#endif + +#endif \ No newline at end of file diff --git a/APP/Device/Device_speed/speed_to_servoMotor.c b/APP/Device/Device_speed/speed_to_servoMotor.c new file mode 100644 index 0000000..b1d3b6e --- /dev/null +++ b/APP/Device/Device_speed/speed_to_servoMotor.c @@ -0,0 +1,149 @@ +#include "speed_to_servoMotor.h" +#include "ptz_header_file.h" +#include "servoMotor.h" + +#ifdef PTZ_SERVO_MOTOR + + +//发送云台实际转速 +void ptz_send_speed(char dev, char speed) +{ + unsigned char Speed1[7] = {0xff,0x00,0xd0,0x00,0x00,0x00,0x00}; + unsigned char Speed2[7] = {0xff,0x00,0xd0,0x00,0x00,0x00,0x00}; + unsigned short int HSpeed = 0; + unsigned short int VSpeed = 0; + Speed1[1] = g_ptz.address; + Speed2[1] = g_ptz.address; + + HSpeed = (unsigned short int)(g_ptz.hori_speed_actual * 100 + 0.5); + Speed1[3] = 0X03;//水平电机 + Speed1[4] = (unsigned char)(HSpeed >> 8); + Speed1[5] = (unsigned char)(HSpeed & 0x00ff); + Speed1[6] = MotorCalPelcoDSUM(Speed1,sizeof(Speed1)); + + VSpeed = (unsigned short int)(g_ptz.vert_speed_actual * 100 + 0.5); + Speed2[3] = 0X04;//垂直电机 + Speed2[4] = (unsigned char)(VSpeed >> 8); + Speed2[5] = (unsigned char)(VSpeed & 0x00ff); + Speed2[6] = MotorCalPelcoDSUM(Speed2,sizeof(Speed2)); + + switch(speed) + { + case 1://只查询水平转速 + ptz_send_data(dev,Speed1,sizeof(Speed1)); + break; + + case 2://只查询垂直转速 + ptz_send_data(dev,Speed2,sizeof(Speed2)); + break; + + default://水平转速垂直转速都查询 + ptz_send_data(dev,Speed1,sizeof(Speed1)); + ptz_send_data(dev,Speed2,sizeof(Speed2)); + break; + } +} + + +static void ptz_hori_step_speed_task() +{ + while(1) + { + if(g_ptz.hori_start_stop_set == PTZ_HORI_START) + { + } + + if(g_ptz.hori_start_stop_set == PTZ_HORI_STOP) + { + } + OSTimeDlyHMSM(0u, 0u, 0u, 10); + } +} + + +static void ptz_vert_step_speed_task() +{ + while(1) + { + if(g_ptz.vert_start_stop_set == PTZ_VERT_START) + { + } + if(g_ptz.vert_start_stop_set == PTZ_VERT_STOP) + { + } + OSTimeDlyHMSM(0u, 0u, 0u, 10); + } +} + + + +static OS_STK task_hori_step_speed_stk[TASK_HORI_PID_STK_SIZE]; +static void creat_task_hori_step_speed_task(void) +{ + CPU_INT08U task_err; + CPU_INT08U name_err; + + task_err = OSTaskCreateExt((void (*)(void *)) ptz_hori_step_speed_task, + (void *) 0, + (OS_STK *)&task_hori_step_speed_stk[TASK_HORI_PID_STK_SIZE - 1], + (INT8U ) TASK_HORI_PID_PRIO, + (INT16U ) TASK_HORI_PID_PRIO, + (OS_STK *)&task_hori_step_speed_stk[0], + (INT32U ) TASK_HORI_PID_STK_SIZE, + (void *) 0, + (INT16U )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR)); + #if (OS_TASK_NAME_EN > 0) + OSTaskNameSet(TASK_HORI_PID_PRIO, "ptz_hori_step_speed_task", &name_err); + #endif + + if ((task_err == OS_ERR_NONE) && (name_err == OS_ERR_NONE)) { + pdebug(DEBUG_LEVEL_INFO,"create ptz_hori_step_speed_task success...\n\r"); + } else { + pdebug(DEBUG_LEVEL_FATAL,"create ptz_hori_step_speed_task failed...\n\r"); + } +} + + + +static OS_STK task_vert_step_speed_stk[TASK_VERT_PID_STK_SIZE]; +static void creat_task_vert_step_speed_task(void) +{ + CPU_INT08U task_err; + CPU_INT08U name_err; + + task_err = OSTaskCreateExt((void (*)(void *)) ptz_vert_step_speed_task, + (void *) 0, + (OS_STK *)&task_vert_step_speed_stk[TASK_VERT_PID_STK_SIZE - 1], + (INT8U ) TASK_VERT_PID_PRIO, + (INT16U ) TASK_VERT_PID_PRIO, + (OS_STK *)&task_vert_step_speed_stk[0], + (INT32U ) TASK_VERT_PID_STK_SIZE, + (void *) 0, + (INT16U )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR)); + #if (OS_TASK_NAME_EN > 0) + OSTaskNameSet(TASK_VERT_PID_PRIO, "ptz_vert_step_speed_task", &name_err); + #endif + + if ((task_err == OS_ERR_NONE) && (name_err == OS_ERR_NONE)) { + pdebug(DEBUG_LEVEL_INFO,"create ptz_vert_step_speed_task success...\n\r"); + } else { + pdebug(DEBUG_LEVEL_FATAL,"create ptz_vert_step_speed_task failed...\n\r"); + } +} + + + + + +void init_speed_module(void) +{ + servoMotorInit(); + creat_task_hori_step_speed_task(); + creat_task_vert_step_speed_task(); +} + + + +#endif + + diff --git a/APP/Device/Device_speed/speed_to_servoMotor.h b/APP/Device/Device_speed/speed_to_servoMotor.h new file mode 100644 index 0000000..c873a23 --- /dev/null +++ b/APP/Device/Device_speed/speed_to_servoMotor.h @@ -0,0 +1,13 @@ +#ifndef __DEVICE_SPEED_SERVOMOTOR_H_ +#define __DEVICE_SPEED_SERVOMOTOR_H_ +#include "ptz_type_select.h" + + +#ifdef PTZ_SERVO_MOTOR + +void ptz_send_speed(char dev, char speed); +void init_speed_module(void); +void init_speed_module(void); +#endif + +#endif \ No newline at end of file diff --git a/APP/Device/device_Other/device_interrupt.c b/APP/Device/device_Other/device_interrupt.c index ba88d88..82cfbfd 100644 --- a/APP/Device/device_Other/device_interrupt.c +++ b/APP/Device/device_Other/device_interrupt.c @@ -8,6 +8,9 @@ #include "gd32f4xx_it.h" #include "ptz_type_select.h" #include "get_angle.h" + +#include "rotate_servo.h" + /// @brief ⲿжϳʼ /// @param[in] usart_periph:EXTI_IRQ_init /// @return none diff --git a/APP/Service/service_areascan.c b/APP/Service/service_areascan.c index 4dc1bce..46ad7dd 100644 --- a/APP/Service/service_areascan.c +++ b/APP/Service/service_areascan.c @@ -3,6 +3,8 @@ #include "enet_to_udp.h" #include "service_areascan.h" +#include "rotate_servo.h" + static BSP_OS_SEM ptz_area_crc_mutex; PtzArea g_area[AREA_AMOUNT]; diff --git a/APP/Service/service_error_count.c b/APP/Service/service_error_count.c index c00cda3..50b09f6 100644 --- a/APP/Service/service_error_count.c +++ b/APP/Service/service_error_count.c @@ -16,6 +16,8 @@ #include "tmc2160.h" #include "service_error_count.h" +#include "rotate_servo.h" + char error_conut_state; #define COUNT_STATE 1 diff --git a/APP/Service/service_selfcheck.c b/APP/Service/service_selfcheck.c index 4914833..913887d 100644 --- a/APP/Service/service_selfcheck.c +++ b/APP/Service/service_selfcheck.c @@ -20,6 +20,8 @@ #include "tmc2160.h" #include "service_error_count.h" +#include "rotate_servo.h" + #ifdef PTZ_BLDC_MOTOR /// ̨ȫΧԼ static unsigned char ptz_hori_complete_self_check_task() @@ -798,6 +800,387 @@ static unsigned char ptz_vert_simplify_self_check_task() #endif +#ifdef PTZ_SERVO_MOTOR + +/// ̨ȫΧԼ +static unsigned char ptz_hori_complete_self_check_task() +{ + switch(g_ptz.hori_self_check) + { + //̨ˮƽת + case PTZ_HORI_SELF_CHECK_COMPLETE_STEP: + if(g_ptz.hori_ps_sw3_state == PS_COVER)//ˮƽ翪رס + { + ptz_hori_start(PTZ_HORI_DIR_LEFT, PTZ_HORI_SELF_CHECK_SPEED); + g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 1; + } + else//ûбס + { + g_ptz.hori_ps_sw3_right_fall = 0; + g_ptz.hori_ps_sw3_left_rise = 0; + g_ptz.hori_ps_sw3_right_rise = 0; + g_ptz.hori_ps_sw3_left_fall = 0; + ptz_hori_start(PTZ_HORI_DIR_RIGHT, PTZ_HORI_SELF_CHECK_SPEED); + g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 2; + } + break; + + case PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 1: + if(g_ptz.hori_ps_sw3_state == PS_NO_COVER) + { + OSTimeDlyHMSM(0u, 0u, 3u, 0u);//ʱת + ptz_hori_stop(PTZ_HORI_STOP_TIME); + g_ptz.hori_ps_sw3_right_fall = 0; + g_ptz.hori_ps_sw3_left_rise = 0; + g_ptz.hori_ps_sw3_right_rise = 0; + g_ptz.hori_ps_sw3_left_fall = 0; + ptz_hori_start(PTZ_HORI_DIR_RIGHT, PTZ_HORI_SELF_CHECK_SPEED); + g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 2; + } + break; + + //ȴһˮƽ翪½ж + case (PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 2): + if(g_ptz.hori_ps_sw3_right_fall >= PS_FALL_ON) + { + g_ptz.hori_as5047d.as5047d_data_reset = 1; + g_ptz.hori_ps_sw3_right_fall = 0; + g_ptz.hori_self_check++; + + OSTimeDlyHMSM(0u, 0u, 5u, 0u); + g_ptz.hori_ps_sw3_right_fall = 0; + } + break; + + //ȴڶˮƽ翪½ж + case (PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 3): + if(g_ptz.hori_ps_sw3_right_fall >= PS_FALL_ON) + { + g_ptz.hori_as5047d.as5047d_cycle_num = g_ptz.hori_as5047d.as5047d_cycle_count; + g_ptz.hori_as5047d.as5047d_ptz_angle_max = g_ptz.hori_as5047d.as5047d_ptz_angle_actual; + g_ptz.hori_as5047d.as5047d_ptz_angle_K = 360.0 / g_ptz.hori_as5047d.as5047d_ptz_angle_max; + + g_ptz.hori_as5047d.as5047d_remain_angle = g_ptz.hori_as5047d.as5047d_ptz_angle_max - + g_ptz.hori_as5047d.as5047d_cycle_num * 360.0; + g_ptz.hori_angleP.angle_allow_max = 360.0; + g_ptz.hori_angleP.angle_allow_min = 0.0; + g_ptz.hori_as5047d.as5047d_state = 1; + + g_ptz.hori_ps_sw3_right_fall = 0; + g_ptz.hori_ps_sw3_left_rise = 0; + g_ptz.hori_ps_sw3_right_rise = 0; + g_ptz.hori_ps_sw3_left_fall = 0; + ptz_hori_stop(PTZ_HORI_STOP_TIME); + g_ptz.hori_angle_state = 1; + g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_DATA_SAVE; + } + break; + } + return g_ptz.hori_self_check; +} + +/// ̨ˮƽԼ +static unsigned char ptz_hori_simplify_self_check_task() +{ + switch(g_ptz.hori_self_check) + { + case PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP: + if(g_ptz.hori_ps_sw3_state == PS_COVER)//ˮƽ翪رס + { + ptz_hori_start(PTZ_HORI_DIR_LEFT, PTZ_HORI_SELF_CHECK_SPEED); + g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 1; + } + else//ûеס + { + g_ptz.hori_ps_sw3_right_fall = 0; + g_ptz.hori_ps_sw3_left_rise = 0; + g_ptz.hori_ps_sw3_right_rise = 0; + g_ptz.hori_ps_sw3_left_fall = 0; + ptz_hori_start(PTZ_HORI_DIR_RIGHT, PTZ_HORI_SELF_CHECK_SPEED); + g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 2; + } + break; + + case PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 1://תֱ翪뵲Ƭڵ + if(g_ptz.hori_ps_sw3_state == PS_NO_COVER) + { + OSTimeDlyHMSM(0u, 0u, 3u, 0u);//ʱת + ptz_hori_stop(PTZ_HORI_STOP_TIME); + g_ptz.hori_ps_sw3_right_fall = 0; + g_ptz.hori_ps_sw3_left_rise = 0; + g_ptz.hori_ps_sw3_right_rise = 0; + g_ptz.hori_ps_sw3_left_fall = 0; + ptz_hori_start(PTZ_HORI_DIR_RIGHT, PTZ_HORI_SELF_CHECK_SPEED); + g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 2; + } + break; + + case PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 2: + //ȴˮƽ翪ת½ + if(g_ptz.hori_ps_sw3_right_fall >= PS_FALL_ON) + { + g_ptz.hori_as5047d.as5047d_cycle_count = 0; + g_ptz.hori_as5047d.as5047d_ptz_angle_actual = 0; + g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last = 0; + g_ptz.hori_as5047d.as5047d_ptz_init_angle = g_ptz.hori_as5047d.as5047d_angle_actual; + g_ptz.hori_as5047d.as5047d_state = 1; + g_ptz.hori_ps_sw3_right_fall = 0; + g_ptz.hori_ps_sw3_left_rise = 0; + g_ptz.hori_ps_sw3_right_rise = 0; + g_ptz.hori_ps_sw3_left_fall = 0; + g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 3; + } + break; + + case PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 3://ת0λ + OSTimeDlyHMSM(0u, 0u, 1u, 0u);//ʱת + g_ptz.hori_angle_control = 0.0; + g_ptz.hori_speed_control = PTZ_HORI_BEST_SPEED; + ptz_hori_rotate_plan(PTZ_HORI_MIN_DISTANCE); + g_ptz.hori_rotate_monitor_switch = PTZ_HORI_GO_TO_ANGLE_A; + g_ptz.hori_angle_state = 1; + g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_END; + break; + } + return g_ptz.hori_self_check; +} + + +/// ֱ̨ȫΧԼ +static unsigned char ptz_vert_complete_self_check_task() +{ + switch(g_ptz.vert_self_check) + { + ///ȶȡ翪ص״̬жϴֱ״̬ + case PTZ_VERT_SELF_CHECK_COMPLETE_STEP: + g_ptz.vert_ps_sw1_down_fall = 0; + g_ptz.vert_ps_sw1_up_rise = 0; + g_ptz.vert_ps_sw2_up_fall = 0; + g_ptz.vert_ps_sw2_down_rise = 0; + if(g_ptz.vert_ps_sw1_state == PS_COVER)//̨¸λ + { + ptz_vert_start(PTZ_VERT_DIR_UP, PTZ_VERT_SELF_CHECK_SPEED); + g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 1; + } + if(g_ptz.vert_ps_sw2_state == PS_COVER)//̨λ + { + g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 2; + } + if(g_ptz.vert_ps_sw1_state == PS_NO_COVER && g_ptz.vert_ps_sw2_state == PS_NO_COVER) + //̨мλ + { + g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 2; + } + break; + + case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 1): + if(g_ptz.vert_ps_sw1_state == PS_NO_COVER) + { + OSTimeDlyHMSM(0u, 0u, 3u, 0u);//ʱת + ptz_vert_stop(PTZ_VERT_STOP_TIME); + g_ptz.vert_ps_sw1_down_fall = 0; + g_ptz.vert_ps_sw1_up_rise = 0; + g_ptz.vert_ps_sw2_up_fall = 0; + g_ptz.vert_ps_sw2_down_rise = 0; + g_ptz.vert_self_check ++; + } + break; + //ת¸λ + case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 2): + ptz_vert_start(PTZ_VERT_DIR_DOWN, PTZ_VERT_SELF_CHECK_SPEED); + g_ptz.vert_self_check ++; + break; + +#ifdef PTZ_SW1_DOWN_FALL_UPDATE + //жǷ񵽴¸λ,Ȼ̨ʼת + case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 3): + if(g_ptz.vert_ps_sw1_down_fall >= 1 || + g_ptz.vert_ps_sw1_state == PS_COVER) + { + g_ptz.vert_ps_sw1_down_fall = 0; + g_ptz.vert_as5047d.as5047d_data_reset = 1; + ptz_vert_stop(PTZ_VERT_STOP_TIME); + ptz_vert_start(PTZ_VERT_DIR_UP, PTZ_VERT_SELF_CHECK_SPEED); + g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 10; + } + break; +#endif + +#ifdef PTZ_SW1_UP_RISE_UPDATE + //жǷ񵽴¸λ,Ȼ̨ʼת + case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 3): + if(g_ptz.vert_ps_sw1_down_fall >= 1 || + g_ptz.vert_ps_sw1_state == PS_COVER) + { + g_ptz.vert_ps_sw1_down_fall = 0; + g_ptz.vert_ps_sw1_up_rise = 0; + OSTimeDlyHMSM(0u, 0u, 0u, 300u); + ptz_vert_stop(PTZ_VERT_STOP_TIME); + ptz_vert_start(PTZ_VERT_DIR_UP, PTZ_VERT_SELF_CHECK_SPEED); + g_ptz.vert_self_check++; + } + break; + //ȴ + case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 4): + if(g_ptz.vert_ps_sw1_up_rise >= 1 || + g_ptz.vert_ps_sw1_state == PS_NO_COVER) + { + g_ptz.vert_as5047d.as5047d_data_reset = 1; + g_ptz.vert_ps_sw1_down_fall = 0; + g_ptz.vert_ps_sw1_up_rise = 0; + g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 10; + } + break; +#endif + + //ȴSW2½,㴹ֱתΧ + case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 10): + if(g_ptz.vert_ps_sw2_up_fall >= 1 || + g_ptz.vert_ps_sw2_state == PS_COVER) + { + g_ptz.vert_as5047d.as5047d_cycle_num = g_ptz.vert_as5047d.as5047d_cycle_count; + g_ptz.vert_as5047d.as5047d_ptz_angle_max = g_ptz.vert_as5047d.as5047d_ptz_angle_actual; + + g_ptz.vert_angleP.angle_range = + g_ptz.vert_as5047d.as5047d_ptz_angle_max / PTZ_VERT_BIG_GEAR_RATIO; + + g_ptz.vert_angleP.angle_allow_min = g_ptz.vert_angleP.angle_range / 2.0 * (-1); + g_ptz.vert_angleP.angle_allow_max = g_ptz.vert_angleP.angle_range / 2.0; + + g_ptz.vert_as5047d.as5047d_ptz_angle_K = 1 / PTZ_VERT_BIG_GEAR_RATIO; + + + g_ptz.vert_as5047d.as5047d_remain_angle = g_ptz.vert_as5047d.as5047d_ptz_angle_max - + g_ptz.vert_as5047d.as5047d_cycle_num * 360.0; + + g_ptz.vert_as5047d.as5047d_state = 1; + + g_ptz.vert_ps_sw1_down_fall = 0; + g_ptz.vert_ps_sw1_up_rise = 0; + g_ptz.vert_ps_sw2_up_fall = 0; + g_ptz.vert_ps_sw2_down_rise = 0; + + ptz_vert_stop(PTZ_VERT_STOP_TIME); + g_ptz.vert_angle_state = 1; + g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_DATA_SAVE; + } + break; + } + return g_ptz.vert_self_check; +} + +/// ֱ̨Լ +static unsigned char ptz_vert_simplify_self_check_task() +{ + switch(g_ptz.vert_self_check) + { + case PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP: + g_ptz.vert_ps_sw1_down_fall = 0; + g_ptz.vert_ps_sw1_up_rise = 0; + g_ptz.vert_ps_sw2_up_fall = 0; + g_ptz.vert_ps_sw2_down_rise = 0; + if(g_ptz.vert_ps_sw1_state == PS_COVER)//̨¸λ + { + ptz_vert_start(PTZ_VERT_DIR_UP, PTZ_VERT_SELF_CHECK_SPEED); + g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 1; + } + if(g_ptz.vert_ps_sw2_state == PS_COVER)//̨λ + { + ptz_vert_start(PTZ_VERT_DIR_DOWN, PTZ_VERT_SELF_CHECK_SPEED); + g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 2; + } + if(g_ptz.vert_ps_sw1_state == PS_NO_COVER && g_ptz.vert_ps_sw2_state == PS_NO_COVER) + //̨мλ + { + ptz_vert_start(PTZ_VERT_DIR_DOWN, PTZ_VERT_SELF_CHECK_SPEED); + g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 2; + } + break; + + case PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 1: + if(g_ptz.vert_ps_sw1_state == PS_NO_COVER) + { + OSTimeDlyHMSM(0u, 0u, 3u, 0u);//ʱת + ptz_vert_stop(PTZ_VERT_STOP_TIME); + g_ptz.vert_ps_sw1_down_fall = 0; + g_ptz.vert_ps_sw1_up_rise = 0; + g_ptz.vert_ps_sw2_up_fall = 0; + g_ptz.vert_ps_sw2_down_rise = 0; + ptz_vert_start(PTZ_VERT_DIR_DOWN, PTZ_VERT_SELF_CHECK_SPEED); + g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 2; + } + break; + +#ifdef PTZ_SW1_DOWN_FALL_UPDATE + case PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 2: + if(g_ptz.vert_ps_sw1_down_fall >= 1 || + g_ptz.vert_ps_sw1_state == PS_COVER) + {//ȴSW1翪رڵͬʱµǰλ + g_ptz.vert_as5047d.as5047d_cycle_count = 0; + g_ptz.vert_as5047d.as5047d_ptz_angle_actual = 0; + g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = 0; + g_ptz.vert_as5047d.as5047d_ptz_init_angle = g_ptz.vert_as5047d.as5047d_angle_actual; + g_ptz.vert_as5047d.as5047d_state = 1; + g_ptz.vert_ps_sw1_down_fall = 0; + g_ptz.vert_ps_sw1_up_rise = 0; + g_ptz.vert_ps_sw2_up_fall = 0; + g_ptz.vert_ps_sw2_down_rise = 0; + ptz_vert_stop(PTZ_VERT_STOP_TIME); + g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 10; + } + break; +#endif + +#ifdef PTZ_SW1_UP_RISE_UPDATE + //жǷ񵽴¸λ,Ȼ̨ʼת + case (PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 2): + if(g_ptz.vert_ps_sw1_down_fall >= 1 || + g_ptz.vert_ps_sw1_state == PS_COVER) + { + g_ptz.vert_ps_sw1_down_fall = 0; + g_ptz.vert_ps_sw1_up_rise = 0; + g_ptz.vert_ps_sw2_up_fall = 0; + g_ptz.vert_ps_sw2_down_rise = 0; + OSTimeDlyHMSM(0u, 0u, 0u, 300u); + ptz_vert_stop(PTZ_VERT_STOP_TIME); + ptz_vert_start(PTZ_VERT_DIR_UP, PTZ_VERT_SELF_CHECK_SPEED); + g_ptz.vert_self_check++; + } + break; + + //ȴ + case (PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 3): + if(g_ptz.vert_ps_sw1_up_rise >= 1 || + g_ptz.vert_ps_sw1_state == PS_NO_COVER) + { + //ȴSW1翪رڵͬʱµǰλ + g_ptz.vert_as5047d.as5047d_cycle_count = 0; + g_ptz.vert_as5047d.as5047d_ptz_angle_actual = 0; + g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = 0; + g_ptz.vert_as5047d.as5047d_ptz_init_angle = g_ptz.vert_as5047d.as5047d_angle_actual; + g_ptz.vert_as5047d.as5047d_state = 1; + g_ptz.vert_ps_sw1_down_fall = 0; + g_ptz.vert_ps_sw1_up_rise = 0; + g_ptz.vert_ps_sw2_up_fall = 0; + g_ptz.vert_ps_sw2_down_rise = 0; + g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 10; + } + break; +#endif + + case PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 10: + OSTimeDlyHMSM(0u, 0u, 1u, 0u); + g_ptz.vert_angle_control = 0.0; + g_ptz.vert_speed_control = PTZ_VERT_BEST_SPEED; + ptz_vert_rotate_plan(PTZ_VERT_ANGLE); + g_ptz.vert_rotate_monitor_switch = PTZ_VERT_GO_TO_ANGLE_A; + g_ptz.vert_angle_state = 1; + g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_END; + break; + } + return g_ptz.vert_self_check; +} +#endif static void ptz_hori_self_check_task() { diff --git a/APP/Service/service_selfcheck.h b/APP/Service/service_selfcheck.h index 4fa3f98..204a96d 100644 --- a/APP/Service/service_selfcheck.h +++ b/APP/Service/service_selfcheck.h @@ -94,6 +94,14 @@ #define PTZ_VERT_SELF_CHECK_SPEED 0.8 #endif +//ŷ +#ifdef PTZ_MEDIUM_WORM_SERVO_MOTOR_24V + ///ˮƽԼת + #define PTZ_HORI_SELF_CHECK_SPEED 1.2 + ///ֱԼת + #define PTZ_VERT_SELF_CHECK_SPEED 0.8 +#endif + ///̨ԼҪͶȡIJ typedef struct _PtzSelfCheckData_ { diff --git a/APP/Service/service_statusmonitor.c b/APP/Service/service_statusmonitor.c index c727ad5..414736c 100644 --- a/APP/Service/service_statusmonitor.c +++ b/APP/Service/service_statusmonitor.c @@ -4,6 +4,8 @@ #include "service_selfcheck.h" #include "device_adc_collect.h" +#include "rotate_servo.h" + extern void ptz_uart_dev_send(device_handle device, void *data, int len); static char max_temperature_fault; @@ -11,6 +13,8 @@ static char max_temperature_fault; //¶ȡѹϼ static char ptz_temp_volt_current_fault_detect_task() {//ֻϣӦ + +#ifdef PTZ_BLDC_MOTOR static unsigned short int time_ms; time_ms ++; @@ -37,7 +41,7 @@ static char ptz_temp_volt_current_fault_detect_task() { time_ms = 0; } - +#endif #ifdef Full_bridge //NTC¶ȼ⣬ݶ¶ֵ100϶ȣ޸ if(g_ptz.H_boad_temp >= 100.0) @@ -742,6 +746,251 @@ static char ptz_motor_rotate_fault_detect_task() #endif +#ifdef PTZ_SERVO_MOTOR +//תϼ******Ҫڼ· +static char ptz_motor_rotate_fault_detect_task() +{ + static char hori_rotate_fault_num; + static char vert_rotate_fault_num; + + static char hori_rotate_fault_step; + static float hori_angle_a; + static float hori_angle_b; + + static char vert_rotate_fault_step; + static float vert_angle_a; + static float vert_angle_b; + static unsigned short int time_ms; + + time_ms ++; + //ֱ + if(g_ptz.vert_start_stop_set == PTZ_VERT_STOP) + { + vert_rotate_fault_step = 0;//رж + vert_rotate_fault_num = 0; + } + //ˮƽ + if(g_ptz.hori_start_stop_set == PTZ_HORI_STOP) + { + hori_rotate_fault_step = 0;//رж + hori_rotate_fault_num = 0; + } + + if(time_ms < 50) + { + return 1; + } + else + { + time_ms = 0; + } + + + + //ֱ + switch(vert_rotate_fault_step) + { + case 0: + if(g_ptz.vert_start_stop_set == PTZ_VERT_START) + { + vert_rotate_fault_num = 0; + vert_angle_a = g_ptz.vert_as5047d.as5047d_ptz_angle_actual; + vert_rotate_fault_step = vert_rotate_fault_step + 1; //ж + } + break; + + case 1://ʼж̨תǷ쳣 + vert_angle_b = g_ptz.vert_as5047d.as5047d_ptz_angle_actual; + if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_UP) + { + //ת + if(vert_angle_b > vert_angle_a && + vert_angle_b - vert_angle_a >= PTZ_VERT_MIN_SPEED * PTZ_VERT_BIG_GEAR_RATIO * 2) + {//ֻһת˵ת + vert_rotate_fault_num = 0; + //תûй + g_ptz.fault_detect.vert_rotate_fault = NO_FAULT; + } + else//ת쳣 + { + if(g_ptz.vert_pid.PidUT_float >= PTZ_VERT_VOLTAGE_LEVEL) + { + vert_rotate_fault_num ++; + } + } + } + else//g_ptz.vert_direction_actual == PTZ_VERT_DIR_DOWN + { + //ת + if(vert_angle_a > vert_angle_b && + vert_angle_a - vert_angle_b >= PTZ_VERT_MIN_SPEED * PTZ_VERT_BIG_GEAR_RATIO * 2) + {//ֻһת˵ת + vert_rotate_fault_num = 0; + //תûй + g_ptz.fault_detect.vert_rotate_fault = NO_FAULT; + } + else//ת쳣 + { + if(g_ptz.vert_pid.PidUT_float >= PTZ_VERT_VOLTAGE_LEVEL) + { + vert_rotate_fault_num ++; + } + } + } + + if(vert_rotate_fault_num >= ROTATE_FAULT_MAX_NUM)//ת쳣ת쳣 + { + vert_rotate_fault_num = 0; + //ر̨Զɨģʽ + g_preset_bit_scan.state = PRESET_BIT_SCAN_CLOSE_A; + g_area_scan.state = AREA_SCAN_CLOSE_A; + //ֱ̨ɲPTZ_HORI_STOP_TIME +// ptz_hori_stop(0); +// ptz_vert_stop(0); + + ptz_hori_stop(PTZ_HORI_STOP_TIME); + ptz_vert_stop(PTZ_VERT_STOP_TIME); + + //ת + g_ptz.fault_detect.vert_rotate_fault = FAULT; + } + + vert_angle_a = vert_angle_b; + break; + } + + /*************************************************************/ + + switch(hori_rotate_fault_step) + { + case 0: + if(g_ptz.hori_start_stop_set == PTZ_HORI_START) + { + hori_rotate_fault_num = 0; + hori_angle_a = g_ptz.hori_as5047d.as5047d_ptz_angle_actual; + hori_rotate_fault_step = hori_rotate_fault_step + 1; //ж + } + break; + + case 1://ʼж̨תǷ쳣 + hori_angle_b = g_ptz.hori_as5047d.as5047d_ptz_angle_actual; + + if(g_ptz.hori_self_check == PTZ_HORI_SELF_CHECK_END) + {//ˮƽԼ˵ + if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_RIGHT) + { + //ת + if((hori_angle_b > hori_angle_a && //תҪСٶת0.5,ҪСתת2 + hori_angle_b - hori_angle_a > PTZ_HORI_MIN_SPEED * PTZ_HORI_BIG_GEAR_RATIO * 2 && + hori_angle_b - hori_angle_a < PTZ_HORI_MAX_SPEED * PTZ_HORI_BIG_GEAR_RATIO * 12) + || + (hori_angle_a > hori_angle_b && + hori_angle_b + g_ptz.hori_as5047d.as5047d_ptz_angle_max - hori_angle_a > PTZ_HORI_MIN_SPEED * PTZ_HORI_BIG_GEAR_RATIO * 2 && + hori_angle_b + g_ptz.hori_as5047d.as5047d_ptz_angle_max - hori_angle_a < PTZ_HORI_MAX_SPEED * PTZ_HORI_BIG_GEAR_RATIO * 12) ) + {//ֻһת˵ת + hori_rotate_fault_num = 0; + //תûй + g_ptz.fault_detect.hori_rotate_fault = NO_FAULT; + } + else//ת쳣 + { + if(g_ptz.hori_pid.PidUT_float >= PTZ_HORI_VOLTAGE_LEVEL) + { + hori_rotate_fault_num ++; + } + } + } + else//g_ptz.hori_direction_actual == PTZ_HORI_DIR_LEFT + { + //ת + if((hori_angle_b < hori_angle_a && //תҪСٶת0.5,ҪСתת2 + hori_angle_a - hori_angle_b > PTZ_HORI_MIN_SPEED * PTZ_HORI_BIG_GEAR_RATIO * 2 && + hori_angle_a - hori_angle_b < PTZ_HORI_MAX_SPEED * PTZ_HORI_BIG_GEAR_RATIO * 12) + || + (hori_angle_a < hori_angle_b && + hori_angle_a + g_ptz.hori_as5047d.as5047d_ptz_angle_max - hori_angle_b > PTZ_HORI_MIN_SPEED * PTZ_HORI_BIG_GEAR_RATIO * 2 && + hori_angle_a + g_ptz.hori_as5047d.as5047d_ptz_angle_max - hori_angle_b < PTZ_HORI_MAX_SPEED * PTZ_HORI_BIG_GEAR_RATIO * 12) ) + {//ֻһת˵ת + hori_rotate_fault_num = 0; + //תûй + g_ptz.fault_detect.hori_rotate_fault = NO_FAULT; + } + else//ת쳣 + { + if(g_ptz.hori_pid.PidUT_float >= PTZ_HORI_VOLTAGE_LEVEL) + { + hori_rotate_fault_num ++; + } + } + + } + + } + else + {//ˮƽԼû + if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_RIGHT) + { + //ת + if(hori_angle_b > hori_angle_a && + hori_angle_b - hori_angle_a >= PTZ_HORI_MIN_SPEED * PTZ_HORI_BIG_GEAR_RATIO * 2) + {//ֻһת˵ת + hori_rotate_fault_num = 0; + //תûй + g_ptz.fault_detect.hori_rotate_fault = NO_FAULT; + } + else//ת쳣 + { + if(g_ptz.hori_pid.PidUT_float >= PTZ_HORI_VOLTAGE_LEVEL) + { + hori_rotate_fault_num ++; + } + } + } + else//g_ptz.hori_direction_actual == PTZ_HORI_DIR_LEFT + { + //ת + if(hori_angle_a > hori_angle_b && + hori_angle_a - hori_angle_b >= PTZ_HORI_MIN_SPEED * PTZ_HORI_BIG_GEAR_RATIO * 2) + {//ֻһת˵ת + hori_rotate_fault_num = 0; + //תûй + g_ptz.fault_detect.hori_rotate_fault = NO_FAULT; + } + else//ת쳣 + { + if(g_ptz.hori_pid.PidUT_float >= PTZ_HORI_VOLTAGE_LEVEL) + { + hori_rotate_fault_num ++; + } + } + } + } + + if(hori_rotate_fault_num >= ROTATE_FAULT_MAX_NUM)//ת쳣ת쳣 + { + hori_rotate_fault_num = 0; + //ر̨Զɨģʽ + g_preset_bit_scan.state = PRESET_BIT_SCAN_CLOSE_A; + g_area_scan.state = AREA_SCAN_CLOSE_A; + //ֱ̨ɲ +// ptz_hori_stop(0); +// ptz_vert_stop(0); + + ptz_hori_stop(PTZ_HORI_STOP_TIME); + ptz_vert_stop(PTZ_VERT_STOP_TIME); + + //ת + g_ptz.fault_detect.hori_rotate_fault = FAULT; + } + + hori_angle_a = hori_angle_b; + break; + } + + return 1; +} + +#endif //ϴԶش static char ptz_fault_return_task() diff --git a/APP/Service/service_statusmonitor.h b/APP/Service/service_statusmonitor.h index dea5c08..ca764c1 100644 --- a/APP/Service/service_statusmonitor.h +++ b/APP/Service/service_statusmonitor.h @@ -233,6 +233,20 @@ #define PTZ_CURRENT_DETECT 6.0//5.0 #endif +//ŷ +#ifdef PTZ_MEDIUM_WORM_SERVO_MOTOR_24V + //¶ֵ + #define PTZ_TEMP_DETECT_MAX 65.0 //70.0 + #define PTZ_TEMP_DETECT_MIN -25.0//70.0 + + //ѹֵ + #define PTZ_VOLT_DETECT_MAX 30.0//30.0 + #define PTZ_VOLT_DETECT_MIN 9.0//30.0 + + //ֵ + #define PTZ_CURRENT_DETECT 10.0//5.0 +#endif + ///Ҫжִܷеָ diff --git a/BSP/Driver/as5047d/as5047d.c b/BSP/Driver/as5047d/as5047d.c index 7e1a3ff..9692b60 100644 --- a/BSP/Driver/as5047d/as5047d.c +++ b/BSP/Driver/as5047d/as5047d.c @@ -1,10 +1,14 @@ #include "as5047d.h" #include +#include +#include +#include + +#ifdef AS5047 static BSP_OS_SEM ptz_hori_get_angle_mutex;//Դ static BSP_OS_SEM ptz_vert_get_angle_mutex; - static void as5047d_delay_nop(int as5047d_delay_time); static unsigned int as5047d_Parity_bit_even(unsigned int package); /// AS5047Dʱ @@ -959,7 +963,326 @@ float as5047d_vert_get_angle_a() return -1; } +#endif /********************************************************************************/ /********************************************************************************/ /********************************************************************************/ +#ifdef TMR3109 + +//TMR3109 붨 +#define TMR3109_OPCODE_WRITE_EEPROM 0x01 //001 дEEPROM +#define TMR3109_OPCODE_WRITE_REGISTER 0x05 //101 дĴ +#define TMR3109_OPCODE_READ_REGISTER 0x06 //110 Ĵ +#define TMR3109_OPCODE_CHANGE_MODE 0x07 //111 ģʽл +#define TMR3109_OPCODE_READ_ANGLE 0x03 //011 Ƕ + +//TMR3109豸ṹ +typedef struct{ + uint8_t device_id; //豸ַ + uint32_t spi_periph; //SPIַ + uint32_t cs_pin; //Ƭѡ + uint32_t cs_gpio_periph; //Ƭѡַ +}TMR3109_Device; + +//Ƕݽṹ +typedef struct{ + uint32_t angle_raw; //23λԭʼǶֵ + float angle_degree; //Ƕֵλ + uint8_t crc; //CRCУ + uint8_t error; // + bool is_valid; //ǷЧ +}TMR3109_AngleData; + +//TMR3109豸 +TMR3109_Device tmr3109_dev1 = { + .device_id = 1, + .spi_periph = SPI2, + .cs_pin = GPIO_PIN_15, + .cs_gpio_periph = GPIOA, +}; + +TMR3109_Device tmr3109_dev2 = { + .device_id = 2, + .spi_periph = SPI4, + .cs_pin = GPIO_PIN_11, + .cs_gpio_periph = GPIOE, +}; + +TMR3109_AngleData hori_angle_data = {0}; +TMR3109_AngleData vert_angle_data = {0}; + +//ݶ΢뼶ӳٺ(SysTick)(ɾӳٺΪͳһĺ) +void delay_us(uint32_t us) +{ + uint32_t start, now, delta, reload, us_tick; + start = SysTick->VAL; + reload = SysTick->LOAD; + us_tick = SystemCoreClock / 1000000UL; + + do { + now = SysTick->VAL; + delta = start > now? start - now : reload + start - now; + } while (delta < us_tick * us); +} + +//TMR3109ʼ +void TMR3109_Init(void) +{ + //ʼSPI2U33 + rcu_periph_clock_enable(RCU_GPIOA); + rcu_periph_clock_enable(RCU_GPIOC); + rcu_periph_clock_enable(RCU_SPI2); + + //SPI2 + gpio_af_set(GPIOC, GPIO_AF_6, GPIO_PIN_10 | GPIO_PIN_11 | GPIO_PIN_12); //PC10,11,12ΪSPI2 + gpio_mode_set(GPIOC, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO_PIN_10 | GPIO_PIN_11 | GPIO_PIN_12); + gpio_output_options_set(GPIOC, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_10 | /*GPIO_PIN_11 | */GPIO_PIN_12); //עһMISOģʽܻ + + //CS + gpio_mode_set(GPIOA, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO_PIN_15); + gpio_output_options_set(GPIOA, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_15); + gpio_bit_set(GPIOA, GPIO_PIN_15); //ĬCSŸߵƽ + + //SPI28MHzݶ5MHzң + spi_parameter_struct spi_init_struct; + spi_struct_para_init(&spi_init_struct); + spi_init_struct.trans_mode = SPI_TRANSMODE_FULLDUPLEX; + spi_init_struct.device_mode = SPI_MASTER; + spi_init_struct.frame_size = SPI_FRAMESIZE_16BIT; //16λ֡ + spi_init_struct.clock_polarity_phase = SPI_CK_PL_LOW_PH_2EDGE; //ʱӼΪͣλΪڶԵ(½) + spi_init_struct.nss = SPI_NSS_SOFT; // + spi_init_struct.prescale = SPI_PSC_16; //ʱԤƵϵΪ16(5MHz) + spi_init_struct.endian = SPI_ENDIAN_MSB; //λȳ + spi_init(SPI2, &spi_init_struct); + spi_enable(SPI2); + + //ʼSPI4U32 + rcu_periph_clock_enable(RCU_GPIOE); + rcu_periph_clock_enable(RCU_SPI4); + + //SPI4 + gpio_af_set(GPIOE, GPIO_AF_6, GPIO_PIN_12 | GPIO_PIN_13 | GPIO_PIN_14); //PE12,13,14ΪSPI4 + gpio_mode_set(GPIOE, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO_PIN_12 | GPIO_PIN_13 | GPIO_PIN_14); + gpio_output_options_set(GPIOE, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_12 | GPIO_PIN_13 | GPIO_PIN_14); //עһMISOģʽܻ + + //CS + gpio_mode_set(GPIOE, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO_PIN_11); + gpio_output_options_set(GPIOE, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_11); + gpio_bit_set(GPIOE, GPIO_PIN_11); //ĬCSŸߵƽ + + //SPI48MHzݶ5MHzң + spi_parameter_struct spi_init_struct2; + spi_struct_para_init(&spi_init_struct2); + spi_init_struct2.trans_mode = SPI_TRANSMODE_FULLDUPLEX; + spi_init_struct2.device_mode = SPI_MASTER; + spi_init_struct2.frame_size = SPI_FRAMESIZE_16BIT; //16λ֡ + spi_init_struct2.clock_polarity_phase = SPI_CK_PL_LOW_PH_2EDGE; //ʱӼΪͣλΪڶԵ(½) + spi_init_struct2.nss = SPI_NSS_SOFT; // + spi_init_struct2.prescale = SPI_PSC_16; //ʱԤƵϵΪ16(5MHz) + spi_init_struct2.endian = SPI_ENDIAN_MSB; //λȳ + spi_init(SPI4, &spi_init_struct2); + spi_enable(SPI4); + + //ʼӳ +} + +void as5047d_init() +{ + TMR3109_Init(); +} + + +// SPIͽ32λ(GD32SPIֻ֧8λ16λ䣬Ҫη) +static uint32_t spi_transfer_32bit(uint32_t spi_periph, uint32_t data) +{ + uint16_t temp_high, temp_low; + uint32_t received_data = 0; + uint32_t start_tick; + const uint32_t timeout_ticks = 10; // 10msʱ + + // ȡҪ͵32λݵĸ16λ͵16λ + temp_high = (uint16_t)(data >> 16); // 16λ + temp_low = (uint16_t)(data & 0xFFFF); // 16λ + + // 1. ȷ͸16λȡ16λӦ + start_tick = OSTimeGet(); + while(RESET == spi_i2s_flag_get(spi_periph, SPI_FLAG_TBE)); + { + //ʱж + if (OSTimeGet() - start_tick > timeout_ticks) { + // SPIжʹĸƬѡ + if (spi_periph == SPI2) { + gpio_bit_set(GPIOA, GPIO_PIN_15); // tmr3109_dev1 CS + } else if (spi_periph == SPI4) { + gpio_bit_set(GPIOE, GPIO_PIN_11); // tmr3109_dev2 CS + } + return 0xFFFFFFFF; + } + } + spi_i2s_data_transmit(spi_periph, temp_high); + + start_tick = OSTimeGet(); + while(RESET == spi_i2s_flag_get(spi_periph, SPI_FLAG_RBNE)); + { + if (OSTimeGet() - start_tick > timeout_ticks) { + if (spi_periph == SPI2) { + gpio_bit_set(GPIOA, GPIO_PIN_15); + } else if (spi_periph == SPI4) { + gpio_bit_set(GPIOE, GPIO_PIN_11); + } + return 0xFFFFFFFF; + } + } + received_data = (uint32_t)spi_i2s_data_receive(spi_periph) << 16; // 洢16λ + + // 2. ŷ͵16λȡ16λӦ + start_tick = OSTimeGet(); + while(RESET == spi_i2s_flag_get(spi_periph, SPI_FLAG_TBE)); + { + if (OSTimeGet() - start_tick > timeout_ticks) { + if (spi_periph == SPI2) { + gpio_bit_set(GPIOA, GPIO_PIN_15); + } else if (spi_periph == SPI4) { + gpio_bit_set(GPIOE, GPIO_PIN_11); + } + return 0xFFFFFFFF; + } + } + spi_i2s_data_transmit(spi_periph, temp_low); + + start_tick = OSTimeGet(); + while(RESET == spi_i2s_flag_get(spi_periph, SPI_FLAG_RBNE)); + { + if (OSTimeGet() - start_tick > timeout_ticks) { + if (spi_periph == SPI2) { + gpio_bit_set(GPIOA, GPIO_PIN_15); + } else if (spi_periph == SPI4) { + gpio_bit_set(GPIOE, GPIO_PIN_11); + } + return 0xFFFFFFFF; + } + } + received_data |= spi_i2s_data_receive(spi_periph); // 洢16λ + + return received_data; +} + +//CS +static void set_cs(TMR3109_Device* dev, bool state) +{ + if(state) + { + gpio_bit_set(dev->cs_gpio_periph, dev->cs_pin); + } + else + { + gpio_bit_reset(dev->cs_gpio_periph, dev->cs_pin); + } +} + +/** + * @brief TMR3109ǶݵCRC4У + * @param angle_data 23λǶ + * @return 4λCRCУ + */ +uint8_t TMR3109_CalculateCRC(uint32_t angle_data_23bit) +{ + // ʽ: x^4 + x^3 + x^2 + 1 = 0b1101 = 0x0D + const uint8_t polynomial = 0x0D; + + // ʼֵ: 0b0011 = 0x3 + uint8_t crc = 0x3; + + // 24λ: 1λ0 + 23λǶֵ + uint32_t data = angle_data_23bit; // 23λǽǶֵ + // λѾ0Ϊangle_data_23bitֻ23λ + + // 24λݣλʼ + for (int i = 23; i >= 0; i--) { + // CRCλ + uint8_t crc_msb = (crc >> 3) & 0x1; + + // ȡǰλ + uint8_t data_bit = (data >> i) & 0x1; + + // CRC1λλ0 + crc = (crc << 1) & 0x0F; + + // CRCλλΪ1ʽ + if (crc_msb ^ data_bit) { + crc ^= polynomial; + } + } + + return crc & 0x0F; // ȷ4λ +} + +/** + * @brief ֤TMR3109ǶݵCRC + * @param angle_data_23bit 23λǶ + * @param received_crc յ4λCRC + * @return trueʾCRCУͨ + */ +bool TMR3109_CheckCRC(uint32_t angle_data_23bit, uint8_t received_crc) +{ + uint8_t calculated_crc = TMR3109_CalculateCRC(angle_data_23bit); + return (calculated_crc == received_crc); +} + +//Ƕ +bool TMR3109_ReadAngle(TMR3109_Device* dev, TMR3109_AngleData* angle_data) +{ + if(dev == NULL || angle_data == NULL) return false; + + //Ƕ 루3bit+ַ8bit+У5bit+ݣ16bit + uint32_t tx_data = (TMR3109_OPCODE_READ_ANGLE << 29)| (0X00 << 21) | (0X00 << 16) | (0X00 << 0) ; + + set_cs(dev, false); //ʹCS + delay_us(20); //SPIʱ Ż + + //32λݲӦ + uint32_t rx_data = spi_transfer_32bit(dev->spi_periph, tx_data); + + set_cs(dev, true); //Ƭѡ + + //Ӧ + //d27~d5: 23λǶݣd4~d1: CRCУ룬d0: errorλ + angle_data->angle_raw = (rx_data >> 5) & 0X7FFFFF; //ȡ23λǶ + angle_data->crc = (rx_data >> 1) & 0X0F; //ȡ4λCRCУ + angle_data->error = rx_data & 0X01; //ȡ1λerrorλ + + //Ƕֵ((_{i=0}^{23} Angle * 2^i) / 8388608 * 360) + angle_data->angle_degree = (angle_data->angle_raw * 360.0f) / 8388608.0f; + + //ЧУ(CRCУ) + bool crc_valid = TMR3109_CheckCRC(angle_data->angle_raw, angle_data->crc); + // angle_data->is_valid = 0/*(angle_data->error == 0)*/; //עerrorλ⣬ֲûҵerrorλľ嶨 + angle_data->is_valid = crc_valid; + + //CRC¼߼ + // if(!crc_valid) { + // //CRCУʧܣ¼Ϣ + // //... + // } + + return angle_data->is_valid; +} + +float as5047d_hori_get_angle_a() +{ + if (TMR3109_ReadAngle(&tmr3109_dev1, &hori_angle_data)) { + return hori_angle_data.angle_degree; + } + return 0.0f; +} + +float as5047d_vert_get_angle_a() +{ + if (TMR3109_ReadAngle(&tmr3109_dev2, &vert_angle_data)) { + return vert_angle_data.angle_degree; + } + return 0.0f; +} + +#endif diff --git a/BSP/Driver/as5047d/as5047d.h b/BSP/Driver/as5047d/as5047d.h index 67b5f40..f76216b 100644 --- a/BSP/Driver/as5047d/as5047d.h +++ b/BSP/Driver/as5047d/as5047d.h @@ -2,6 +2,13 @@ #define __AS5047D_H_ #include "gd32f4xx.h" +#define TMR3109 1 + +#if !(AS5047) && !(TMR3109) + #define AS5047 +#endif + +#ifdef AS5047 ///ˮƽAS5047D˿ ///CSn ///AS5047D_HORICSnΪߵƽ @@ -163,7 +170,51 @@ float as5047d_vert_get_angle(); unsigned int as5047d_vert_cef_command(); float as5047d_hori_get_angle_a(); float as5047d_vert_get_angle_a(); -unsigned int as5047d_vert_read_data_a(); -unsigned int as5047d_hori_read_data_a(); + +#endif + +#ifdef TMR3109 +// #define AS5047D_DIR_FWD 1 //űת +// #define AS5047D_DIR_REV 0 //űת +#define AS5047D_DIR_FWD 0 //űת +#define AS5047D_DIR_REV 1 //űת + +///ڱԽݵжϽ +/// +///ڱԽݵжϽϸľԭ +///_masterʾas5047dԽյָжϵĽ_slaveʾK60as5047dصݰжϽ +typedef struct _As5047DJudgeReceivePackage_ +{ + ///յݰ + unsigned int package; + + ///k60AS5047Dظݽж + ///жϽյݰЧǷȷ,10 + unsigned char parity_error_package; + + + + ///AS5047DԽյK60ָж,ָдʱĻظ + ///Error flagEFλ + unsigned char error_flag_command; + ///У10 + unsigned char parity_error_command; + ///Ч10 + unsigned char command_invalid_command; + ///֡10 + unsigned char framing_error_command; + + + ///ݲǷɹ0ɹ1ɹ.жд߶ȡǷɹ + unsigned char Operation_Result; + +}As5047DJudgeReceivePackage; + + +void as5047d_init(); +float as5047d_hori_get_angle_a(); +float as5047d_vert_get_angle_a(); + +#endif #endif \ No newline at end of file diff --git a/BSP/Driver/servoMotor/servoMotor.c b/BSP/Driver/servoMotor/servoMotor.c new file mode 100644 index 0000000..739d291 --- /dev/null +++ b/BSP/Driver/servoMotor/servoMotor.c @@ -0,0 +1,8 @@ +#include "servoMotor.h" + + + +void servoMotorInit(void) +{ + +} \ No newline at end of file diff --git a/BSP/Driver/servoMotor/servoMotor.h b/BSP/Driver/servoMotor/servoMotor.h new file mode 100644 index 0000000..4c9dec1 --- /dev/null +++ b/BSP/Driver/servoMotor/servoMotor.h @@ -0,0 +1,8 @@ +#ifndef _DRIVER_SERVO_MOTOR_H_ +#define _DRIVER_SERVO_MOTOR_H_ + +#include "gd32f4xx.h" + +void servoMotorInit(void); + +#endif \ No newline at end of file diff --git a/PROJECT/OS2.ewp b/PROJECT/OS2.ewp index d473523..c2b1886 100644 --- a/PROJECT/OS2.ewp +++ b/PROJECT/OS2.ewp @@ -409,6 +409,7 @@ $PROJ_DIR$\..\BSP\Driver\timer $PROJ_DIR$\..\BSP\Driver\full_bridge $PROJ_DIR$\..\APP\Service + $PROJ_DIR$\..\BSP\Driver\servoMotor