塔基力矩传感器

1.FrameLayer/Source/TimeTask_Event.c

@@ -28,6 +28,7 @@
 #include "MosResCalib.h"
 #include "bikespeed.h"
 #include "UserGpio_Config.h"
+#include "uart_TORG4BBSensor.h"
 //#include "asr.h"
 
 //#include "STLmain.h"
@@ -123,7 +124,7 @@ void  Event_1ms(void)
     else if(cp_stFlg.RunModelSelect == TorqAssist)
     {
         /* 力矩AD值处理 */
-        torsensor_voTorADC();
+//        torsensor_voTorADC();
         /* 力矩中轴助力输入 */
         ass_voTorqAssistInput();        
         /* 根据踏频脉冲对力矩信号进行滑动平均处理 */
@@ -239,6 +240,8 @@ void Event_10ms(void)
     cadence_stFreGetOut.uwFreqPercent = ((ULONG)cadence_stFreGetOut.uwLPFFrequencyPu << 14) / cadence_stFreGetCof.uwMaxCadenceFre; // Q14
     
 
+    TORG4BB_Torque_Handler();
+
     /* Trip cal when open */
     bikespeed_votempTripCal();
 
@@ -766,7 +769,7 @@ UWORD uwIntSource = 0;
     cadence_voCadenceCal(uwIntSource);
 
 
-    if(Get_CAD_PORT() == RESET)
+    if(TORG4BB_Get_CAD_PORT() == RESET)
       {
         if(cadence_stFreGetOut.uwTaPinPortSta!=0)
         {

1.FrameLayer/Source/main.c

@@ -230,6 +230,8 @@ int  main(void)
     /* Interrupts of peripherals enable*/
     hw_voEnInt();
     
+    TORG4BB_Init();
+
     /* self test init */
     //stl_voRunTimeChecksInit();
     /* watchdog 1s */

3.BasicFunction/Include/FuncLayerAPI.h

@@ -23,6 +23,7 @@
 #include "Temp.h"
 #include "AssistCurve.h"
 #include "bikeinformation.h"
+#include "uart_TORG4BBSensor.h"
 /****************************************
  *
  *          Definitions & Macros

3.BasicFunction/Include/uart_TORG4BBSensor.h

@@ -0,0 +1,83 @@
+/**
+  ******************************************************************************
+  * @file    uart_TORG4BBSensor.h
+  * @author
+  * @version V1.0.0
+  * @date    05-02-2025
+  * @brief   TORG4BBSensor function.
+  ******************************************************************************
+  * @attention
+  *
+  * <h2><center>&copy; COPYRIGHT(c) 2015 STMicroelectronics</center></h2>
+  *
+  * 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.
+  *
+  ******************************************************************************
+  */
+
+/* Define to prevent recursive inclusion -------------------------------------*/
+#ifndef __UART_TORG4BBSENSOR_H
+#define __UART_TORG4BBSENSOR_H
+/* Includes ------------------------------------------------------------------*/
+#include "usart.h"
+
+#define TORG4BBCADENCE_LPF_GAIN                 80
+#define TORG4BBTORQUE_TIMEOUT_UNIT               10  //ms
+#define TORG4BBTORQUE_TIMEOUT_TIME               100  //ms
+/* Define to variables declare---------------------------------------------------*/
+//先发送低位后高位
+typedef struct
+{
+    UBYTE ucTemp;//温度
+    UWORD uwTorqueReg;//力矩
+    UWORD uwTorqueRegLast;//力矩
+    UBYTE ucForwardCnt; //踏频速度
+    UBYTE ucForwardCntLast; //踏频速度
+    UWORD uwMotorSpeed;//电机转速
+} TORG4BBInfo_Struct_t;
+
+typedef struct
+{
+    UWORD uwTorqueRegOffset;//力矩零点
+    UWORD uwTorqueReg2Pu;
+    UWORD uwTorqueLPFgain;
+    UWORD uwMaxTimeOutCnt;
+
+    UWORD uwTimeOutCnt;
+    UWORD uwTorquePu;//力矩
+    UWORD uwTorqueLPFPu;
+    UWORD uwTorqueLPFPuLast;
+} TORG4BB_OUT;
+
+/* Public variables declare---------------------------------------------------*/
+
+/*Private function declare---------------------------------------------------*/
+void TORG4BB_Init(void);
+void TORG4BB_USART_DataProcess(USART_Buf_TypeDef* ptUartTx, UBYTE* Data);
+void TORG4BB_USART_RxData_Process(USART_Buf_TypeDef* ptUartTx, UBYTE* buf, UWORD dataCount);
+void TORG4BB_Torque_Handler(void);
+uint32_t TORG4BB_Get_CAD_PORT(void);
+
+#endif
+
+/*---------------------------- file end -------------------------------------*/

3.BasicFunction/Source/AssistCurve.c

@@ -405,9 +405,9 @@ void AssitCuvApplPerVolt(void)
     /* 启动标志判断 */
     if(ass_CalIn.uwtorquePer > ass_CalCoef.uwAssThreshold)
     {
-        if(CadGpioSta !=Get_CAD_PORT()   )
+        if(CadGpioSta != TORG4BB_Get_CAD_PORT()   )
         {
- 			CadGpioSta = Get_CAD_PORT();
+ 			CadGpioSta = TORG4BB_Get_CAD_PORT();
             if(ass_CalIn.uwStartRunPulse < 100)
             {
                 ass_CalIn.uwStartRunPulse ++;
@@ -461,7 +461,7 @@ void AssitCuvApplPerVolt(void)
         exit_pulse_cnt = 200;
     }
     
-    if( Get_CAD_PORT() == 0)
+    if( TORG4BB_Get_CAD_PORT() == 0)
     {
         if(ass_CalIn.uwcadLowStopCnt<=4000)
         {

3.BasicFunction/Source/uart_TORG4BBSensor.c

@@ -0,0 +1,195 @@
+/**
+  ******************************************************************************
+  * @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)
+{
+    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);
+}
+/***************************************************************
+ 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.ucTemp = ((Data[1]>>2) + 71);
+    stTORG4BBInfo.uwTorqueReg = (((Data[1] & 0x03)<<8) + Data[2]);
+    stTORG4BBInfo.ucForwardCnt = Data[3] & 0x7F;
+    stTORG4BBInfo.uwMotorSpeed = (((Data[4] & 0x7F)<<8) + Data[5]);
+}
+/***************************************************************
+ 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)
+{
+    static UBYTE Data[255];
+    //UWORD Cmd, ID;
+    UWORD i;
+    UBYTE checksum = 0;
+    
+    if(dataCount == 7)
+    {
+        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);
+            }
+        }
+    }
+}
+/***************************************************************
+ 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;
+    }
+}
+/************************ (C) END OF FILE *********************************/

3.BasicFunction/Source/usart.c

@@ -55,6 +55,7 @@
 #include "uart_J.h"
 #include "hwsetup.h"
 #include "UserGpio_Config.h"
+#include "uart_TORG4BBSensor.h"
 //#include "api.h"
 //#include "board_config.h"
 /* Private variables ---------------接收缓存区------------------------------------------*/
@@ -133,6 +134,7 @@ void UsartRx_Process(USART_Buf_TypeDef *USARTx_RxBuf_Struct, USART_Buf_TypeDef *
 #elif (UART_ID == 6)
         Lanfeng9_USART_RxData_Process(USARTx_TxBuf_Struct, data, datacount);
 #endif
+        TORG4BB_USART_RxData_Process(USARTx_TxBuf_Struct, data, datacount);
         /*clear*/
         USART_RxFrameBuf_Struct->uwDataCount[USART_RxFrameBuf_Struct->ucBufRdIndex] = 0;
         USART_RxFrameBuf_Struct->uwFramecount--;