/**
  ******************************************************************************
  * @file    uart_TORG4BBSensor.h
  * @author
  * @version V1.0.0
  * @date    05-02-2025
  * @brief   TORG4BBSensor function.
  ******************************************************************************
  *
  * COPYRIGHT(c) 2015 STMicroelectronics
  *
  * Redistribution and use in source and binary forms, with or without modification,
  * are permitted provided that the following conditions are met:
  *   1. Redistributions of source code must retain the above copyright notice,
  *      this list of conditions and the following disclaimer.
  *   2. Redistributions in binary form must reproduce the above copyright notice,
  *      this list of conditions and the following disclaimer in the documentation
  *      and/or other materials provided with the distribution.
  *   3. Neither the name of STMicroelectronics nor the names of its contributors
  *      may be used to endorse or promote products derived from this software
  *      without specific prior written permission.
  *
  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
  ******************************************************************************
  */
/* Includes ------------------------------------------------------------------*/
#include "uart_TORG4BBSensor.h"
#include "FuncLayerAPI.h"
#include "mathtool.h"
#include "syspar.h"
/******************************
 *
 *  Parameter
 *
 ******************************/
static LPF_OUT scm_stTORG4BBTorLpf;
TORG4BBInfo_Struct_t stTORG4BBInfo;
TORG4BB_OUT stTORG4BB_stTorSensorOut;
/******************************
 *
 *  Functions
 *
 ******************************/
/***************************************************************
 Function: TORG4BB_Init;
 Description:Data Init
 Call by:
 Input Variables: N/A
 Output/Return Variables: N/A
 Subroutine Call: N/A;
 Reference: N/A
****************************************************************/
void TORG4BB_Init(void)
{
#if (TORG4BBTORQU_ENABLE == 1)
    stTORG4BB_stTorSensorOut.uwTorqueRegOffset = 217;//0.7/3.3*1024
    stTORG4BB_stTorSensorOut.uwTorqueReg2Pu = (((SQWORD)3300 << 14) >> 10) / 35 * 10 / TORQUEBASE;
    stTORG4BB_stTorSensorOut.uwTorqueLPFgain = TORG4BBCADENCE_LPF_GAIN;
    stTORG4BB_stTorSensorOut.uwMaxTimeOutCnt = TORG4BBTORQUE_TIMEOUT_TIME / TORG4BBTORQUE_TIMEOUT_UNIT;

    stTORG4BB_stTorSensorOut.uwTimeOutCnt = 0;
    stTORG4BBInfo.uwTorqueReg = stTORG4BB_stTorSensorOut.uwTorqueRegOffset;

    mth_voLPFilterCoef(1000000/2, 100, &scm_stTORG4BBTorLpf.uwKx);
#endif
}
/***************************************************************
 Function: TORG4BB_USART_DataProcess;
 Description:Get TORG4BBSensor Data
 Call by:
 Input Variables: N/A
 Output/Return Variables: N/A
 Subroutine Call: N/A;
 Reference: N/A
****************************************************************/
void TORG4BB_USART_DataProcess(USART_Buf_TypeDef* ptUartTx, UBYTE* Data)
{
    stTORG4BBInfo.ucForwardCntLast = stTORG4BBInfo.ucForwardCnt;
    stTORG4BBInfo.uwTorqueRegLast = stTORG4BBInfo.uwTorqueReg;

    stTORG4BBInfo.swTemp = (SWORD)(Data[1] - 30);
    stTORG4BBInfo.uwTorqueReg = (((Data[4] & 0xC0)<<2) + Data[2]);
    stTORG4BBInfo.ucForwardCnt = Data[3] & 0x7F;
    stTORG4BBInfo.uwFlyWheelSpeedCnt = (((Data[4] & 0x1F)<<8) + Data[5]);
    stTORG4BBInfo.uwFlyWheelSpeed = 30000/(stTORG4BBInfo.uwFlyWheelSpeedCnt*TORG4BBTORQUE_NUMBERS_PULSES);//rpm 1/(Cnt*0.2*TORG4BBTORQUE_NUMBERS_PULSES/1000/60)
}
/***************************************************************
 Function: TORG4BB_USART_RxData_Process;
 Description: Get Uart Data following the protocol
 Call by:
 Input Variables: N/A
 Output/Return Variables: N/A
 Subroutine Call: N/A;
 Reference: N/A
****************************************************************/
void TORG4BB_USART_RxData_Process(USART_Buf_TypeDef* ptUartTx, UBYTE* buf, UWORD dataCount)
{
#if (TORG4BBTORQU_ENABLE == 1)
    static UBYTE Data[255];
    //UWORD Cmd, ID;
    UWORD i;
    UBYTE checksum = 0;
    
    if(dataCount == 8)
    {
        if(buf[0] == 0xFF)//起始
        {
            for(i=0; i<(dataCount-1); i++)//数据
            {
                Data[i] = buf[i];
                checksum += buf[i];
            }

            if(checksum == buf[dataCount-1])// 校验和正常
            {
                TORG4BB_USART_DataProcess(ptUartTx, Data);
            }
        }
    }
#endif
}
/***************************************************************
 Function: TORG4BB_Torque_Handler;
 Description: Get TORG4BB_Torque Data
 Call by:
 Input Variables: N/A
 Output/Return Variables: N/A
 Subroutine Call: N/A;
 Reference: N/A
****************************************************************/
void TORG4BB_Torque_Handler(void)
{
    if(stTORG4BBInfo.uwTorqueReg < stTORG4BB_stTorSensorOut.uwTorqueRegOffset)
    {
        stTORG4BB_stTorSensorOut.uwTorquePu = 0;
    }
    else
    {
        stTORG4BB_stTorSensorOut.uwTorquePu = (stTORG4BBInfo.uwTorqueReg - stTORG4BB_stTorSensorOut.uwTorqueRegOffset) * stTORG4BB_stTorSensorOut.uwTorqueReg2Pu;
    }

    //通信超时，清除力矩
    if(stTORG4BBInfo.uwTorqueRegLast == stTORG4BBInfo.uwTorqueReg)
    {
        if(stTORG4BB_stTorSensorOut.uwTimeOutCnt < stTORG4BB_stTorSensorOut.uwMaxTimeOutCnt)
        {
            stTORG4BB_stTorSensorOut.uwTimeOutCnt++;
        }
        else
        {
            stTORG4BB_stTorSensorOut.uwTorquePu = 0;
        }
    }
    else
    {
        stTORG4BB_stTorSensorOut.uwTimeOutCnt = 0;
    }

    //mth_voLPFilter(stTORG4BB_stTorSensorOut.uwTorquePu, &scm_stTORG4BBTorLpf);
    //stTORG4BB_stTorSensorOut.uwTorqueLPFPu = scm_stTORG4BBTorLpf.slY.sw.hi;
    stTORG4BB_stTorSensorOut.uwTorqueLPFPuLast = stTORG4BB_stTorSensorOut.uwTorqueLPFPu;
    stTORG4BB_stTorSensorOut.uwTorqueLPFPu = (stTORG4BB_stTorSensorOut.uwTorqueLPFPu * stTORG4BB_stTorSensorOut.uwTorqueLPFgain +
            stTORG4BB_stTorSensorOut.uwTorquePu * (100 - stTORG4BB_stTorSensorOut.uwTorqueLPFgain)) /
           100;

    torsensor_stTorSensorOut.uwTorqueReg = stTORG4BBInfo.uwTorqueReg;
    torsensor_stTorSensorOut.uwTorquePu = stTORG4BB_stTorSensorOut.uwTorquePu;
    torsensor_stTorSensorOut.uwTorqueLPFPu = stTORG4BB_stTorSensorOut.uwTorqueLPFPu;
}
/***************************************************************
 Function: TORG4BB_Get_CAD_PORT;
 Description: Get TORG4BB_Candance pulse timing
 Call by:
 Input Variables: N/A
 Output/Return Variables: N/A
 Subroutine Call: N/A;
 Reference: N/A
****************************************************************/
uint32_t TORG4BB_Get_CAD_PORT(void)
{
    if((stTORG4BBInfo.ucForwardCnt > stTORG4BBInfo.ucForwardCntLast) || ((stTORG4BBInfo.ucForwardCnt == 1) && (stTORG4BBInfo.ucForwardCntLast == 127)))
    {
        return 0x0000;
    }
    else
    {
        return 0x0200;
    }
}
/***************************************************************
 Function: TORG4BB_Get_Temp;
 Description: Get TORG4BB temp
 Call by:
 Input Variables: N/A
 Output/Return Variables: N/A
 Subroutine Call: N/A;
 Reference: N/A
****************************************************************/
SWORD TORG4BB_Get_Temp(void)
{
    return stTORG4BBInfo.swTemp;
}
/************************ (C) END OF FILE *********************************/