#include "servoMotor.h"
#include <ucos_ii.h>

#ifdef PTZ_MEDIUM_WORM_SERVO_MOTOR_24V
    /*
    水平垂直电机使能开关引脚配置
    */
    static void MotorSwitchGpioCofig(void)
    {
        /*GPIO时钟初始化*/
        rcu_periph_clock_enable(RCU_GPIOE);
        /*水平电机打开引脚*/
        gpio_mode_set(GPIOE, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE,  GPIO_PIN_0);
        gpio_output_options_set(GPIOE, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ,  GPIO_PIN_0);
        /*垂直电机打开引脚*/
        gpio_mode_set(GPIOE, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE,  GPIO_PIN_1);
        gpio_output_options_set(GPIOE, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ,  GPIO_PIN_1);
        
    }


    /*
    从机地址    功能码    寄存器地址高   寄存器地址低  数据高位    数据低位    crc校验高位     crc校验低位
        01H     06H         02H             00H         00H         01H         49H             B2H
    */
    /**
    * @brief    伺服电机速度模式写单个寄存器
    * @param    motorNo：写垂直电机还是水平电机
    * @param    regAddr：要写的寄存器
    * @param    data：要向寄存器写入的数据
    * @return   返回数组地址
    */
    uint8_t g_HwriteOneRegBuff[WRITE_ONE_REG_FRAME_NUM];
    uint8_t g_VwriteOneRegBuff[WRITE_ONE_REG_FRAME_NUM];
    uint8_t* WriteMotorOneReg(uint8_t motorNo, uint16_t regAddr, int16_t data)
    {
        static uint8_t g_writeOneRegBuff[WRITE_ONE_REG_FRAME_NUM];
        uint16_t crc;
        g_writeOneRegBuff[0] = 0x01;
        g_writeOneRegBuff[1] = WRITE_ONE_REG;
        g_writeOneRegBuff[2] = regAddr >> 8;
        g_writeOneRegBuff[3] = (uint8_t)(regAddr & 0xff);
        g_writeOneRegBuff[4] = data >> 8;
        g_writeOneRegBuff[5] = data & 0xff;
        crc = ModbusCRC16(g_writeOneRegBuff, WRITE_ONE_REG_FRAME_NUM - 2);
        g_writeOneRegBuff[6] = (uint8_t)(crc & 0xff);
        g_writeOneRegBuff[7] = crc >> 8;
        if ( motorNo == H_MOTOR )
        {
            memcpy(g_HwriteOneRegBuff, g_writeOneRegBuff, WRITE_ONE_REG_FRAME_NUM);
            return g_HwriteOneRegBuff;
        }

        memcpy(g_VwriteOneRegBuff, g_writeOneRegBuff, WRITE_ONE_REG_FRAME_NUM);
        return g_VwriteOneRegBuff;
    }



    /*
    从机地址    功能码    寄存器地址高   寄存器地址低  reg数量高位    数量低位    crc校验高位     crc校验低位
        01H     03H         0BH             00H         00H         01H         86H             2EH
    */
    /**
    * @brief    伺服电机速度模式读单个寄存器
    * @param    motorNo：读垂直电机还是水平电机
    * @param    regAddr：要读的寄存器
    * @return   返回数组地址
    */
    static uint8_t g_HreadOneRegBuff[READ_ONE_REG_FRAME_NUM];
    static uint8_t g_VreadOneRegBuff[READ_ONE_REG_FRAME_NUM];
    uint8_t* ReadMotorOneReg(uint8_t motorNo, uint16_t regAddr)
    {
          static uint8_t g_readOneRegBuff[READ_ONE_REG_FRAME_NUM];
          uint16_t crc;
          g_readOneRegBuff[0] = 0x01;
          g_readOneRegBuff[1] = READ_ONE_REG;
          g_readOneRegBuff[2] = regAddr >> 8;
          g_readOneRegBuff[3] = regAddr & 0xff;
          g_readOneRegBuff[4] = 0x00;
          g_readOneRegBuff[5] = 0x01;
          crc = ModbusCRC16(g_readOneRegBuff, READ_ONE_REG_FRAME_NUM - 2);
          g_readOneRegBuff[6] = crc & 0xff;
          g_readOneRegBuff[7] = crc >> 8;
        if ( motorNo == H_MOTOR )
            {
                memcpy(g_HreadOneRegBuff, g_readOneRegBuff, WRITE_ONE_REG_FRAME_NUM);
                return g_HreadOneRegBuff;
            }

        memcpy(g_VreadOneRegBuff, g_readOneRegBuff, WRITE_ONE_REG_FRAME_NUM);
        return g_VreadOneRegBuff; 
    }



    /**
    * @brief 伺服电机驱动初始化
    * @param
    * @return 
    */
    void servoMotorInit(void)
    {
        MotorSwitchGpioCofig();//两个电机电源的开关PE0,1引脚，如果其它地方实现了，可以不需要
        H_MOTOR_OPEN;
        OSTimeDlyHMSM(0u, 0u, 0u, 1000u);
        V_MOTOR_OPEN;
        
        CommuDrvInit();//伺服电机RS485通讯初始化
    }
#endif