From 5aceb95bcfae1c409e8b9c8387bcefd2526a6b6c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E8=B5=B7=E5=BA=8A=E5=B0=B1=E7=8A=AF=E5=9B=B0?= <11730503+psx123456@user.noreply.gitee.com> Date: Mon, 28 Jul 2025 14:01:57 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E7=A3=81=E7=BC=96=E4=B8=BATM?= =?UTF-8?q?R3109?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- APP/Common/ptz_default_value.h | 20 +- BSP/Driver/as5047d/as5047d.c | 297 ++++++++++++++++++++++++++- BSP/Driver/as5047d/as5047d.h | 110 ++++++++++ BSP/Driver/full_bridge/full_bridge.c | 27 ++- PROJECT/OS2.ewp | 2 +- 5 files changed, 429 insertions(+), 27 deletions(-) diff --git a/APP/Common/ptz_default_value.h b/APP/Common/ptz_default_value.h index a883f98..0a13ff6 100644 --- a/APP/Common/ptz_default_value.h +++ b/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 //电机磁极对数 diff --git a/BSP/Driver/as5047d/as5047d.c b/BSP/Driver/as5047d/as5047d.c index 7e1a3ff..05595ad 100644 --- a/BSP/Driver/as5047d/as5047d.c +++ b/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 + diff --git a/BSP/Driver/as5047d/as5047d.h b/BSP/Driver/as5047d/as5047d.h index 67b5f40..5c4f24a 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_HORI的CSn引脚置为高电平 @@ -166,4 +173,107 @@ 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 //磁编码器反向转动 + +///用于保存对接收数据的判断结果 +/// +///用于保存对接收数据的判断结果，并且详细保存出错的具体原因 +///_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 \ No newline at end of file diff --git a/BSP/Driver/full_bridge/full_bridge.c b/BSP/Driver/full_bridge/full_bridge.c index 560e33d..842787f 100644 --- a/BSP/Driver/full_bridge/full_bridge.c +++ b/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 +// 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](); + 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 diff --git a/PROJECT/OS2.ewp b/PROJECT/OS2.ewp index 1498948..7b060ff 100644 --- a/PROJECT/OS2.ewp +++ b/PROJECT/OS2.ewp @@ -825,7 +825,7 @@