滑动滤波算法

思路

数据由队列管理，先减去头部的数，再加上新的数据到尾部，除以队列有效数据长度

代码实现

static int PowerMag_CaptureBatteryVoltage(RobotBatteryPowerInfo_typedef* BatteryInfor)
{
    #define BATTERY_VOLTAGE_LENGTH (uint8_t)20
    #define BATTERY_VOLTAGE_TOP_ERR (uint32_t)40000   //mV
    static float s_fReadBatteryVoltage = 0.0;
    static float s_fReadBatteryVoltageArry[BATTERY_VOLTAGE_LENGTH] = {0.0};
    static float s_fReadBatteryVoltageTotoal = 0.0;
    static uint8_t  s_ReadBatteryVoltageIndex = 0;
    static uint8_t  s_TempCnt = 0;

    s_fReadBatteryVoltage = ad74xx.readBatteryVoltage(BATTERY_VOL_SENSOR_1ST);

    /* Judge the voltage is valid  */
    if(s_fReadBatteryVoltage == 0)  
        return (-1);
    if(s_fReadBatteryVoltage > BATTERY_VOLTAGE_TOP_ERR) 
        return (-2);

    s_fReadBatteryVoltageTotoal -= s_fReadBatteryVoltageArry[s_ReadBatteryVoltageIndex];
    s_fReadBatteryVoltageArry[s_ReadBatteryVoltageIndex] = s_fReadBatteryVoltage;
    s_fReadBatteryVoltageTotoal += s_fReadBatteryVoltageArry[s_ReadBatteryVoltageIndex];

    if(s_ReadBatteryVoltageIndex < (BATTERY_VOLTAGE_LENGTH-1)){
        s_ReadBatteryVoltageIndex++;
        s_TempCnt ++;
    }else{
        s_ReadBatteryVoltageIndex = 0;
    }
#if 1  /* This way is always to be avange */
    if(s_TempCnt >= 1){
        if(s_TempCnt >=  BATTERY_VOLTAGE_LENGTH){
            s_TempCnt = BATTERY_VOLTAGE_LENGTH;
        }
        BatteryInfor->RobotVoltage = s_fReadBatteryVoltageTotoal/s_TempCnt;
    }
#else  /* This way RobotVoltage would be appeared after start 2s */
    if(s_TempCnt >= BATTERY_VOLTAGE_LENGTH){
        BatteryInfor->RobotVoltage = s_fReadBatteryVoltageTotoal / BATTERY_VOLTAGE_LENGTH;
        s_TempCnt  = BATTERY_VOLTAGE_LENGTH; 
    }
#endif  
    return 0;
}