修改磁编为TMR3109

APP/Common/ptz_default_value.h

@@ -73,11 +73,11 @@
 #ifdef PTZ_MEDIUM_WORM_L6235D_AS5047D
 ///*******************************标准云台******************************/
        ///水平电机减速比
-      #define PTZ_HORI_MOTOR_RATIO    10.0 // 25.0  //(25.0 * 1.5)//20
+      #define PTZ_HORI_MOTOR_RATIO    25.0 // 25.0  //(25.0 * 1.5)//20
       ///水平大齿轮减速比
       #define PTZ_HORI_BIG_GEAR_RATIO 80.0//62.0
       ///水平总减速比
-      #define PTZ_HORI_RATIO          800.0//2325//1240.0
+      #define PTZ_HORI_RATIO          2000.0//2325//1240.0
       ///水平电机调速模拟电压最大值
       #define PTZ_HORI_VR_MAX         1999
       ///水平电机调速模拟电压最小值
@@ -89,20 +89,20 @@
       ///水平电机最小转速
       #define PTZ_HORI_MOTOR_MIN_SPEED   400.0//600.0
       ///水平云台最大转速
-      #define PTZ_HORI_MAX_SPEED         3.75       //1.5//1.29//2.4
+      #define PTZ_HORI_MAX_SPEED         1.5       //1.5//1.29//2.4
       ///水平云台最小转速
-      #define PTZ_HORI_MIN_SPEED         0.5//0.3//0.26//0.4
+      #define PTZ_HORI_MIN_SPEED         0.2//0.3//0.26//0.4
       ///水平云台默认最佳速度
-      #define PTZ_HORI_BEST_SPEED        2.2//2.0
+      #define PTZ_HORI_BEST_SPEED        1.2//2.0
 
       /**********************************************************/
 
       ///磁编码器和电机之间的减速比
-      #define PTZ_VERT_MOTOR_RATIO    37.5      //75.0//(50.0 * (52.0 / 35.0))//20.0
+      #define PTZ_VERT_MOTOR_RATIO    75.0      //75.0//(50.0 * (52.0 / 35.0))//20.0
       ///磁编码器与云台轴之间的减速比
       #define PTZ_VERT_BIG_GEAR_RATIO 80.0//54.00
       ///垂直总减速比 
-      #define PTZ_VERT_RATIO          3000.0    //6000.0//4011.429//1080.0
+      #define PTZ_VERT_RATIO          6000.0    //6000.0//4011.429//1080.0
       ///垂直电机调速模拟电压最大值
       #define PTZ_VERT_VR_MAX         1999
       ///垂直电机调速模拟电压最小值
@@ -114,11 +114,11 @@
       ///垂直电机最小转速
       #define PTZ_VERT_MOTOR_MIN_SPEED   400.0//600.0
       ///垂直云台最大转速
-      #define PTZ_VERT_MAX_SPEED         1.0    //0.5//0.74//2.7
+      #define PTZ_VERT_MAX_SPEED         0.5    //0.5//0.74//2.7
       ///垂直云台最小转速
-      #define PTZ_VERT_MIN_SPEED         0.14   //0.07//0.1//0.15//0.5
+      #define PTZ_VERT_MIN_SPEED         0.07   //0.07//0.1//0.15//0.5
       ///垂直云台默认最佳速度
-      #define PTZ_VERT_BEST_SPEED        0.7    //0.4//0.7//2.0 
+      #define PTZ_VERT_BEST_SPEED        0.4    //0.4//0.7//2.0 
 
 
       //电机磁极对数

BSP/Driver/as5047d/as5047d.c

@@ -4,6 +4,7 @@
 static BSP_OS_SEM ptz_hori_get_angle_mutex;//共享资源锁
 static BSP_OS_SEM ptz_vert_get_angle_mutex;
 
+#ifdef AS5047
 
 static void as5047d_delay_nop(int as5047d_delay_time);
 static unsigned int as5047d_Parity_bit_even(unsigned int package);
@@ -931,8 +932,6 @@ As5047DJudgeReceivePackage as5047d_vert_judge_receive_package_a(unsigned int pac
   return temp1;
 }
 
-
-
 //新的读取角度方式
 float as5047d_vert_get_angle_a()
 {
@@ -962,4 +961,298 @@ float as5047d_vert_get_angle_a()
 /********************************************************************************/
 /********************************************************************************/
 /********************************************************************************/
+#endif
+
+#ifdef TMR3109
+
+/**
+  * @brief  tmr3109初始化
+  * @param  
+  * @retval
+  *
+  */
+static void tmr3109_init(void)
+{
+    //总线时钟使能
+    rcu_periph_clock_enable(RCU_GPIOA);
+    rcu_periph_clock_enable(RCU_GPIOC);
+    rcu_periph_clock_enable(RCU_GPIOD);
+    //水平PC10--CLK,PC12--MOSI
+    gpio_mode_set(GPIOC, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO_PIN_10| GPIO_PIN_12);
+    gpio_output_options_set(GPIOC, GPIO_OTYPE_PP, GPIO_OSPEED_25MHZ, GPIO_PIN_10| GPIO_PIN_12);
+    //PA15--CS
+    gpio_mode_set(GPIOA, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO_PIN_15);
+    gpio_output_options_set(GPIOA, GPIO_OTYPE_PP, GPIO_OSPEED_25MHZ, GPIO_PIN_15);
+    //PC11--MISO
+    gpio_mode_set(GPIOC, GPIO_MODE_INPUT, GPIO_PUPD_NONE, GPIO_PIN_11);
+    
+    TMR3109_HORI_CSN_DISABLE;
+    TMR3109_HORI_CLK_LOW;
+    TMR3109_HORI_MOSI_LOW;
+    
+    
+    
+    //垂直PD0--CS,PD1--CLK,PD3--MOSI
+    gpio_mode_set(GPIOD, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO_PIN_0| GPIO_PIN_1| GPIO_PIN_3);
+    gpio_output_options_set(GPIOD, GPIO_OTYPE_PP, GPIO_OSPEED_25MHZ, GPIO_PIN_0| GPIO_PIN_1| GPIO_PIN_3);
+    //PD2--MISO
+    gpio_mode_set(GPIOD, GPIO_MODE_INPUT, GPIO_PUPD_NONE, GPIO_PIN_2);
+    
+    TMR3109_VERT_CSN_DISABLE;
+    TMR3109_VERT_CLK_LOW;
+    TMR3109_VERT_MOSI_LOW;
+}
+
+/**
+  * @brief  tmr3109短延时函数,用于tmr3109的SPI通信短延时
+  * @param  delay_time 短延时时间长短
+  * @retval
+  *
+  */
+static void tmr3109_delay_nop(int delay_time)
+{
+    for(int i = 0; i < delay_time; i ++)
+    {
+      asm("nop");
+      asm("nop");
+      asm("nop");
+      asm("nop");
+    }  
+}
+
+/**
+  * @brief  将数据进行打包发送
+  * @param  Op_code 功能码
+  *         address 地址码
+  *         idleBit 空闲码
+  *         dataBit 数据位
+  * @retval 打包的数据
+  *
+  */
+unsigned int tmr3109_command_type(unsigned char Op_code, unsigned char address, unsigned char idleBit, unsigned short dataBit)
+{
+    return (unsigned int)((Op_code << 29) | (address << 21) | (idleBit << 16) | dataBit);
+}
+
+/**
+  * @brief  校验tmr3109角度数据
+  * @param  data 读取到的角度信息
+  * @retval crc 校验后的值
+  *
+  */
+// 多项式掩码 (x^4 + x^3 + x^2 + 1) -> 0b1101
+#define CRC_POLY 0x0D
+static unsigned char check_tmr3109_angle(unsigned int data_24bit)
+{
+    unsigned char crc = 0x03; // 初始值 0011
+
+    uint8_t feedback;
+    
+    // 处理 24 位数据 (MSB first)
+    for (int i = 23; i >= 0; i--) {
+        // 获取当前位 (从最高位开始)
+        uint8_t bit = (data_24bit >> i) & 0x01;
+        
+        // 计算反馈值 = CRC最高位 XOR 输入位
+        feedback = (crc >> 3) ^ bit;
+        
+        // 左移寄存器 (丢弃最高位)
+        crc = (crc << 1) & 0x0F;
+        
+        // 当反馈为1时，异或多项式
+        if (feedback) {
+            crc ^= CRC_POLY; // 异或多项式掩码 1101
+        }
+    }
+
+    return crc;
+}
+
+/**
+  * @brief  将数据发送发送到水平磁编
+  * @param  package 要发送的数据
+  * @retval 读取到的数据
+  *
+  */
+unsigned int tmr3109_hori_send_package(unsigned int package)
+{
+    unsigned int temp = 0;
+    unsigned int temp1 = 0;
+    unsigned int temp2 = 0;
+
+    TMR3109_HORI_CLK_LOW;
+    TMR3109_HORI_MOSI_LOW;
+
+    tmr3109_delay_nop(2);
+    for(int i = 31; i >= 0; i--)
+    {
+        TMR3109_HORI_CLK_LOW;
+        tmr3109_delay_nop(2);
+        temp = package & (1 << i);
+        if(temp != 0) {
+            TMR3109_HORI_MOSI_HIGH;
+        }
+        else {
+            TMR3109_HORI_MOSI_LOW;
+        }
+        tmr3109_delay_nop(2);
+        
+        TMR3109_HORI_CLK_HIGH;
+        tmr3109_delay_nop(2);
+        
+        temp1 = TMR3109_HORI_MISO_READ;
+        temp2 = temp2 | (temp1 << i);
+        
+        temp1 = 0;
+        temp  = 0;
+    }
+
+    TMR3109_HORI_CLK_LOW;
+    TMR3109_HORI_MOSI_LOW;
+
+    return temp2;
+}
+
+static volatile float vert_tmr3109_angle_actual;
+static volatile float hori_tmr3109_angle_actual;
+/**
+  * @brief  读取水平转子的位置
+  * @param  
+  * @retval 位置
+  *
+  */
+float read_horiTmr3109Angle(void)
+{
+    static float proportion = 360.0f / 8388608.0f;
+    static unsigned int temp;
+    static float angle;
+    // static unsigned int temp1;
+
+
+    // BSP_OS_SemWait(&ptz_hori_get_angle_mutex, 0u);
+    // TMR3109_HORI_CSN_DISABLE;
+    // tmr3109_delay_nop(4);
+    TMR3109_HORI_CSN_ENABLE;
+    tmr3109_delay_nop(20);
+
+    temp = tmr3109_hori_send_package(tmr3109_command_type(0b011, 0, 0, 0));
+    
+    TMR3109_HORI_CSN_DISABLE;
+
+    if (check_tmr3109_angle((temp >> 5) & 0b11111111111111111111111) == ((temp >> 1) & 0x0F)) {
+        //crc校验成功        
+        // pdebug(DEBUG_LEVEL_DEBUG, "crc check success\n");
+        angle = (float)((temp >> 5) & 0b11111111111111111111111) * proportion;
+        hori_tmr3109_angle_actual = 360.0 - angle;
+        // BSP_OS_SemPost(&ptz_hori_get_angle_mutex);
+        return hori_tmr3109_angle_actual;
+    }
+
+    // BSP_OS_SemPost(&ptz_hori_get_angle_mutex);
+    return -1;
+}
+
+/**
+  * @brief  将数据发送发送到俯仰磁编
+  * @param  package 要发送的数据
+  * @retval 读取到的数据
+  *
+  */
+unsigned int tmr3109_vert_send_package(unsigned int package)
+{
+    unsigned int temp = 0;
+    unsigned int temp1 = 0;
+    unsigned int temp2 = 0;
+
+    TMR3109_VERT_CLK_LOW;
+    TMR3109_VERT_MOSI_LOW;
+
+    tmr3109_delay_nop(2);
+    for(int i = 31; i >= 0; i--)
+    {
+        TMR3109_VERT_CLK_LOW;
+        tmr3109_delay_nop(2);
+        temp = package & (1 << i);
+        if(temp != 0) {
+            TMR3109_VERT_MOSI_HIGH;
+        }
+        else {
+            TMR3109_VERT_MOSI_LOW;
+        }
+        tmr3109_delay_nop(2);
+        
+        TMR3109_VERT_CLK_HIGH;
+        tmr3109_delay_nop(2);
+        
+        temp1 = TMR3109_VERT_MISO_READ;
+        temp2 = temp2 | (temp1 << i);
+        
+        temp1 = 0;
+        temp  = 0;
+    }
+
+    TMR3109_VERT_CLK_LOW;
+    TMR3109_VERT_MOSI_LOW;
+
+    return temp2;
+}
+
+/**
+  * @brief  读取俯仰转子的位置
+  * @param  
+  * @retval 位置
+  *
+  */
+float read_vertTmr3109Angle(void)
+{
+    static float proportion = 360.0f / 8388608.0f;
+    static unsigned int temp;
+    static float angle;
+    // static unsigned int temp1;
+
+    // BSP_OS_SemWait(&ptz_vert_get_angle_mutex, 0u);
+    TMR3109_VERT_CSN_ENABLE;
+    tmr3109_delay_nop(20);
+
+    temp = tmr3109_vert_send_package(tmr3109_command_type(0b011, 0, 0, 0));
+    
+    TMR3109_VERT_CSN_DISABLE;
+
+    if (check_tmr3109_angle((temp >> 5) & 0b11111111111111111111111) == ((temp >> 1) & 0x0F)) {
+        //crc校验成功
+        // pdebug(DEBUG_LEVEL_DEBUG, "crc check success\n");
+        angle = (float)((temp >> 5) & 0b11111111111111111111111) * proportion;
+        vert_tmr3109_angle_actual = (360.0 - angle);
+
+        // BSP_OS_SemPost(&ptz_vert_get_angle_mutex);
+        return vert_tmr3109_angle_actual;
+    }
+
+    // BSP_OS_SemPost(&ptz_vert_get_angle_mutex);
+    return -1;
+}
+
+void as5047d_init()
+{
+    tmr3109_init();
+}
+
+float as5047d_hori_get_angle_a()
+{
+    return read_horiTmr3109Angle();
+}
+
+float as5047d_vert_get_angle_a()
+{
+    return read_vertTmr3109Angle();
+}
+
+
+
+
+
+
+
+#endif
+
 

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_HORI的CSn引脚置为高电平
@@ -167,3 +174,106 @@ 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 //磁编码器反向转动
+
+///用于保存对接收数据的判断结果
+///
+///用于保存对接收数据的判断结果，并且详细保存出错的具体原因
+///_master表示as5047d对接收到的指令判断的结果，_slave表示K60对as5047d返回的数据包判断结果
+typedef struct _As5047DJudgeReceivePackage_
+{
+  ///接收到的数据包
+  unsigned int package;
+  
+  ///k60对AS5047D回复的数据进行判断
+  ///判断接收的数据包效验码是否正确,1错误，0正常
+  unsigned char parity_error_package;
+
+ 
+  
+  ///AS5047D对接收到的K60指令进行判断,指令有错时的回复结果
+  ///数据Error flag（EF）位，
+  unsigned char error_flag_command;
+  ///校验错误，1错误，0正常
+  unsigned char parity_error_command;
+  ///命令无效，1错误，0正常
+  unsigned char command_invalid_command;
+  ///帧错误，1错误，0正常
+  unsigned char framing_error_command;
+  
+  
+  ///数据操作是否成功，0不成功，1成功.即判断写入或者读取数据是否成功
+  unsigned char Operation_Result;
+
+}As5047DJudgeReceivePackage;
+
+///水平TMR3109端口
+///CSn
+///将TMR3109_HORI的CSn引脚置为高电平
+#define TMR3109_HORI_CSN_DISABLE (gpio_bit_set(GPIOA,GPIO_PIN_15)) 
+///将TMR3109_HORI的CSn引脚置为低电平
+#define TMR3109_HORI_CSN_ENABLE  (gpio_bit_reset(GPIOA,GPIO_PIN_15))
+
+///CLK
+///将TMR3109_HORI的CLK引脚置为高电平
+#define TMR3109_HORI_CLK_HIGH    (gpio_bit_set(GPIOC,GPIO_PIN_10))
+///将TMR3109_HORI的CLK引脚置为低电平
+#define TMR3109_HORI_CLK_LOW     (gpio_bit_reset(GPIOC,GPIO_PIN_10))
+
+///MISO
+///TMR3109_HORI的MISO引脚相连的引脚的电平值
+#define TMR3109_HORI_MISO_READ   (gpio_input_bit_get(GPIOC,GPIO_PIN_11))
+
+///MOSI
+///将TMR3109_HORI的MOSI引脚置为高电平
+#define TMR3109_HORI_MOSI_HIGH   (gpio_bit_set(GPIOC,GPIO_PIN_12))
+///将TMR3109_HORI的MOSI引脚置为低电平
+#define TMR3109_HORI_MOSI_LOW    (gpio_bit_reset(GPIOC,GPIO_PIN_12))
+
+///垂直TMR3109端口
+///垂直TMR3109_VERT芯片引脚控制
+///CSn
+///将TMR3109_VERT的CSn引脚置为高电平
+#define TMR3109_VERT_CSN_DISABLE (gpio_bit_set(GPIOD,GPIO_PIN_0))
+///将TMR3109_VERT的CSn引脚置为低电平
+#define TMR3109_VERT_CSN_ENABLE  (gpio_bit_reset(GPIOD,GPIO_PIN_0))
+
+///CLK
+///将TMR3109_VERT的CLK引脚置为高电平
+#define TMR3109_VERT_CLK_HIGH    (gpio_bit_set(GPIOD,GPIO_PIN_1))
+///将TMR3109_VERT的CLK引脚置为低电平
+#define TMR3109_VERT_CLK_LOW     (gpio_bit_reset(GPIOD,GPIO_PIN_1))
+
+///MISO
+///读取与TMR3109_VERT的MISO引脚相连的引脚的电平值
+#define TMR3109_VERT_MISO_READ   (gpio_input_bit_get(GPIOD,GPIO_PIN_2))
+
+///MOSI
+///将TMR3109_VERT的MOSI引脚置为高电平
+#define TMR3109_VERT_MOSI_HIGH   (gpio_bit_set(GPIOD,GPIO_PIN_3))
+///将TMR3109_VERT的MOSI引脚置为低电平
+#define TMR3109_VERT_MOSI_LOW    (gpio_bit_reset(GPIOD,GPIO_PIN_3))
+
+/* Op_code对应的功能码 */
+//写EEPROM操作
+#define Write_ee 0b001
+//写寄存器操作
+#define Write_register 0b101
+//读寄存器操作
+#define Read_register 0b110
+//模式切换操作
+#define Change_mode 0b111
+//读角度值操作
+#define Read_angle 0b011
+
+void as5047d_init();
+float as5047d_hori_get_angle_a();
+float as5047d_vert_get_angle_a();
+
+#endif
+
+#endif

BSP/Driver/full_bridge/full_bridge.c

@@ -360,21 +360,21 @@ void h_bldc_six_step()
     H_Hall_state.Hall_value = h_hall_senser_value_get();
     if((H_Hall_state.Hall_value <= 6)&&(H_Hall_state.Hall_value >= 1))
     {
-     if(g_ptz.hori_direction_set == PTZ_HORI_DIR_RIGHT)
-     {//电机正转- 1
-       h_six_step_commu[(7-H_Hall_state.Hall_value) -1]();
-     }else{//电机反转
-       h_six_step_commu[H_Hall_state.Hall_value -1]();
-     }
-    //   //直齿云台转向
 //      if(g_ptz.hori_direction_set == PTZ_HORI_DIR_RIGHT)
 //      {//电机正转- 1
-        
-    //     h_six_step_commu[H_Hall_state.Hall_value -1]();
-    //   }else{//电机反转
 //        h_six_step_commu[(7-H_Hall_state.Hall_value) -1]();
-        
+//      }else{//电机反转
+//        h_six_step_commu[H_Hall_state.Hall_value -1]();
 //      }
+      //直齿云台转向
+      if(g_ptz.hori_direction_set == PTZ_HORI_DIR_RIGHT)
+      {//电机正转- 1
+        
+        h_six_step_commu[H_Hall_state.Hall_value -1]();
+      }else{//电机反转
+        h_six_step_commu[(7-H_Hall_state.Hall_value) -1]();
+        
+      }
       
     }
   }
@@ -420,7 +420,6 @@ void v_bldc_six_step()
 
 
 
-
 /*!
     \brief      none 垂直方向调速，自锁
     \param[in]  none

PROJECT/OS2.ewp

@@ -825,7 +825,7 @@
                 </option>
                 <option>
                     <name>IlinkIcfFile</name>
-                    <state>D:\psx\Pan-Tilt\1.software\HY\6.0 MW22-01A\BSP\IAR\GD32F450xE_APP.icf</state>
+                    <state>D:\psx\Pan-Tilt\1.software\HY\6.0 MW22-01A\BSP\IAR\GD32F450xE.icf</state>
                 </option>
                 <option>
                     <name>IlinkIcfFileSlave</name>