4G/code/app/lib/AppFuncLib.c

/*
 * @Author: chenjie
 * @Date: 2022-06-06
 * @LastEditTime: 2022-11-10
 * @LastEditors: chenjie
 * @Description:
 * @FilePath: \S32K146_4G\code\app\lib\AppFuncLib.c
 * Copyright (c) 2022 by chenjie, All Rights Reserved.
 */
#include "AppFuncLib.h"
#include "AppGlobalVar.h"
#include "Hal_Fls.h"
#include "AppTaskUart1.h"
/**
 * @brief : 获取故障码函数，从故障数组中获取故障码，并将之前的故障码向前移动
 * @param {UINT16} *ErrorArray
 * @param {UINT8} Errorlen
 * @return {*}
 */
static uint8 bcc_chk_fota(uint8 *data, uint8 length);
static uint8 Fota_crc_chk(uint8 *data, uint8 length);
uint16 GetErrorNum(uint16 *ErrorArray, uint8 Errorlen)
{
    uint16 OutNum;
    OutNum = *(ErrorArray);
    for (uint8 i = 0; i < Errorlen - 1; i++)
    {
        *(ErrorArray + i) = *(ErrorArray + i + 1);
        if (*(ErrorArray + i + 1) == 0)
            break;
    }
    return OutNum;
}
uint8 PutErrorNum(uint16 *ErrorArray, uint8 Errorlen, uint16 ErrorNum)
{
    for (uint8 i = 0; i < Errorlen; i++)
    {
        if (*(ErrorArray + i) == 0)
        {
            *(ErrorArray + i) = ErrorNum;
            return 0;
        }
        else
        {
            if (*(ErrorArray + i) == ErrorNum)
            {
                return 1;
            }
            else
            {
                continue;
            }
        }
    }
    return 2;
}
uint16 ATstrdel(char *str)
{
    char *p = str;
    bool flag = false;
    while (*str)
    {
        if (*str > 0x20)
        {
            *(p) = *str;
            p = p + 1;
            flag = false;
        }
        else
        {
            if (!flag)
            {
                *(p) = ',';
                p = p + 1;
                flag = true;
            }
        }
        str++;
    }
    *p = '\0';
    return 0;
}
uint16 mstrlen(const char *s)
{
    uint16 out = 0;
    const char *ss = s;
    while (*ss)
        ss++;
    out = (ss - s);
    return out;
}
int mstrncmp(const char *s1, const char *s2, int n)
{
    const unsigned char *c1 = (const unsigned char *)s1;
    const unsigned char *c2 = (const unsigned char *)s2;
    unsigned char ch;
    int d = 0;
    while (n--)
    {
        d = (int)(ch = *c1++) - (int)*c2++;
        if (d || !ch)
            break;
    }
    return d;
}
unsigned char HexToChar(unsigned char bHex)
{
    if ((bHex >= 0) && (bHex <= 9))
        bHex += 0x30;
    else if ((bHex >= 10) && (bHex <= 15)) // 大写字母
        bHex += 0x37;
    else
        bHex = 0xff;
    return bHex;
}
unsigned char CharToHex(unsigned char bChar)
{
    if ((bChar >= 0x30) && (bChar <= 0x39))
        bChar -= 0x30;
    else if ((bChar >= 0x41) && (bChar <= 0x46)) // 大写字母
        bChar -= 0x37;
    else if ((bChar >= 0x61) && (bChar <= 0x66)) // 小写字母
        bChar -= 0x57;
    else
        bChar = 0xff;
    return bChar;
}
uint8 AtStrCompare(const char *a, const char *b)
{
    uint8 out = 1;
    while (1)
    {
        if (*a == '\0' || *b == '\0') // 判断其中是否有字符串结束
        {
            if (strlen(a) == strlen(b))
            {
                out = 1;
                break;
            }
            else
            {
                out = 0;
                break;
            }
        }
        else
        {
            if (*a != *b)
            {
                out = 0;
                break;
            }
            else if (*a == '=' && *b == '=')
            {
                out = 1;
                break;
            }
        }
        a++;
        b++;
    }
    return out;
}
unsigned short CRC16_Modbus(unsigned char *pdata, int len)
{
    unsigned short crc = 0xFFFF;
    int i, j;
    for (j = 0; j < len; j++)
    {
        crc = crc ^ pdata[j];
        for (i = 0; i < 8; i++)
        {
            if ((crc & 0x0001) > 0)
            {
                crc = crc >> 1;
                crc = crc ^ 0xa001;
            }
            else
                crc = crc >> 1;
        }
    }
    return crc;
}
char *Myitoa(int value, char *result, int base)
{
    // check that the base if valid
    if (base < 2 || base > 36)
    {
        *result = '\0';
        return result;
    }

    char *ptr = result, *ptr1 = result, tmp_char;
    int tmp_value;

    do
    {
        tmp_value = value;
        value /= base;
        *ptr++ = "zyxwvutsrqponmlkjihgfedcba9876543210123456789abcdefghijklmnopqrstuvwxyz"[35 + (tmp_value - value * base)];
    } while (value);

    // Apply negative sign
    if (tmp_value < 0)
        *ptr++ = '-';
    *ptr-- = '\0';
    while (ptr1 < ptr)
    {
        tmp_char = *ptr;
        *ptr-- = *ptr1;
        *ptr1++ = tmp_char;
    }
    return result;
}

/************************************************************************
 * @brief 整数转字符串
 * @param[in] num 整数
 * @param[out] buf 字符串
 * @return 返回字符串长度
 ************************************************************************/
inline int _itoa(int num, char buf[32])
{
    return _i2a(num, buf, 10);
}

/************************************************************************
 * @brief 整数转字符串
 * @param[in] num 整数
 * @param[out] buf 字符串
 * @param[in] radix 进位制整数
 * @return 返回字符串长度
 ************************************************************************/
int _i2a(int num, char buf[32], int radix)
{
    static const char s[] = "0123456789abcdef";
    int n = num, R = radix;
    char *dst = buf;
    if (n < 0)
    {
        *dst++ = '-';
        n = -n;
    }
    if (n < 10)
    {
        *dst++ = s[n];
        *dst = 0;
    }
    else
    {
        char tmp[32], *p = tmp;
        while (n)
        {
            *p++ = s[n % R];
            n /= R;
        }
        while (--p != tmp)
            *dst++ = *p;
        *dst++ = *tmp;
        *dst = 0;
    }
    return dst - buf;
}

/************************************************************************
 * @brief 浮点数转字符串
 * @param[in] val 浮点数
 * @param[out] buf 字符串
 * @param[in] eps 精度(小数位)
 * @return 返回字符串长度
 ************************************************************************/
int _ftoa(double val, char buf[32], int eps)
{
    double f = val;
    char *p = buf;
    if (val < 0)
    {
        *p++ = '-';
        f = -f;
    }
    int n = f;
    int len = _itoa(n, p);
    return len + __ftoa(f - n, p + len, eps);
}

/************************************************************************
 * @brief 浮点数转字符串：范围(-1, 1)
 * @param[in] val 浮点数
 * @param[out] buf 字符串
 * @param[in] eps 精度(小数位)
 * @return 返回字符串长度
 ************************************************************************/
int __ftoa(double val, char buf[32], int eps)
{
    double f = val;
    char *p = buf;
    static const char s[] = "0123456789";
    if (f < 0)
    {
        *p++ = '-';
        f = -f;
    }
    *p++ = '.';
    for (int i = eps + 1, n; --i; ++p, f -= n)
        *p = s[n = f *= 10.0];
    *p = 0;
    return p - buf;
}

/************************************************************************
 * @brief 替换sprintf
 * @ref 可变长参数列表误区与陷阱——va_arg不可接受的类型
 * http://www.cppblog.com/ownwaterloo/archive/2009/04/21/80655.aspx
 ************************************************************************/
int _sprintf(char *dst, const char *format, ...)
{
    char *s = dst;
    const char *f = format;
    va_list ap, another;
    va_start(ap, format);
    va_copy(another, ap);
    while (*f)
    {
        int n = 1;
        if ('%' != *f)
        {
            *s = *f;
        }
        else
        {
            ++f;
            switch (*f)
            {
            case 's': // 字符串
            {
                const char *p = va_arg(ap, char *);
                n = strlen(p);
                memcpy(s, p, n);
            }
            break;

            case 'd':
            case 'u': // 整数
            {
                char buf[32];
                n = _itoa(va_arg(ap, int), buf);
                memcpy(s, buf, n);
            }
            break;

            case 'f': // 浮点数
            {
                char buf[32];
                n = _ftoa(va_arg(ap, double), buf, 6);
                memcpy(s, buf, n);
            }
            break;

            case 'x': // 16进制数
            {
                char buf[32];
                n = _i2a(va_arg(ap, int), buf, 16);
                memcpy(s, buf, n);
            }
            break;

            case 'c': // 字符
            {
                *s = va_arg(ap, int);
            }
            break;

            case '%': // 百分号
            {
                *s = '%';
            }
            break;

            default:
            {
                va_end(ap);
                int x = vsprintf(dst, format, another);
                va_end(another);
                return x;
            }
            break;
            }
        }
        ++f;
        s += n;
    }
    *s = 0;
    va_end(ap);
    return s - dst;
}
uint8 bcc_chk(uint8 *data, uint16 length)
{
    uint8 bcc_chk_return = 0x00;
    uint16 count = 0;
    while (count < length)
    {
        bcc_chk_return ^= data[count];
        count++;
    }
    return bcc_chk_return;
}
uint16 crc_chk(uint8 *data, uint8 length)
{
    uint8 j;
    uint16 reg_crc = 0xFFFF;
    while (length--)
    {
        reg_crc ^= *data++;
        for (j = 0; j < 8; j++)
        {
            if (reg_crc & 0x01)
            {
                reg_crc = (reg_crc >> 1) ^ 0xA001;
            }
            else
            {
                reg_crc = reg_crc >> 1;
            }
        }
    }
    return reg_crc;
}
/***************************************************************************************************/

#define PI 3.14159265358979

//-------------------uint16数组取最大-------------------------------------
uint16_T ArrMax(uint16_T *Data, uint16_T n)
{
    uint16_T i;
    uint16_T DataMax;
    DataMax = Data[0];
    for (i = 0; i < n; i++)
    {
        if (DataMax < Data[i])
        {
            DataMax = Data[i];
        }
    }
    return DataMax;
}

//-----------------uint16数组取最小-------------------------------------
uint16_T ArrMin(uint16_T *Data, uint16_T n)
{
    uint16_T i;
    uint16_T DataMin;
    DataMin = Data[0];
    for (i = 0; i < n; i++)
    {
        if (DataMin > Data[i])
        {
            DataMin = Data[i];
        }
    }
    return DataMin;
}
//

/////////////////////////// int16数组求均值////////////////////////////////////
int16_T ArrMean(int16_T *Data, uint16_T n)
{
    uint16_T i;
    int32_T Sum = 0;
    int16_T DataMean;

    for (i = 0; i < n; i++)
    {
        Sum = Sum + Data[i];
    }
    DataMean = (int16_T)Sum / n;
    return DataMean;
}

//-----------------real_T 限幅函数
real_T Saturation_r(real_T in, real_T LowLim, real_T UpLim)
{
    real_T out;

    out = in > LowLim ? in : LowLim;
    out = out > UpLim ? UpLim : out;
    return out;
}

//-----------------uint16 限幅函数

uint16_T Saturation_u(uint16_T in, uint16_T LowLim, uint16_T UpLim)
{
    uint16_T out;

    out = in > LowLim ? in : LowLim;
    out = out > UpLim ? UpLim : out;
    return out;
}

// ---------------------滤波控制变动速率--------------------------------------
uint16_T DataFilt(uint16_T in, uint16_T *out, uint16_T Lim)
{
    int16_T delt;
    delt = (int16_T)(in - *out);
    if (delt > Lim)
    {
        *out = *out + (delt > Lim ? Lim : delt);
    }
    if (delt < -Lim)
    {
        *out = *out + (delt < -Lim ? -Lim : delt);
    }
    if (delt <= Lim && delt >= -Lim)
    {
        *out = in;
    }
    return *out;
}

// ---------------------滤波控制变动速率--------------------------------------
uint8_T DataFilt8(uint8_T in, uint8_T *out, uint8_T Lim)
{
    int8_T delt;
    delt = (int8_T)(in - *out);
    if (delt > Lim)
    {
        *out = *out + (delt > Lim ? Lim : delt);
    }
    if (delt < -Lim)
    {
        *out = *out + (delt < -Lim ? -Lim : delt);
    }
    if (delt <= Lim && delt >= -Lim)
    {
        *out = in;
    }
    return *out;
}

//-------------------uint16 to uint16的一维查表--------------------------------------
uint16_T look1_u16tu16(uint16_T u0, const uint16_T *bp0, const uint16_T *table, uint16_T MaxLen)
{

    uint32_T bpIdx = 0;
    uint32_T iLeft = 0;
    uint32_T iRght = 0;
    uint16_T y = 0;
    uint32_T yL_0d0 = 0;
    uint32_T yR_0d0 = 0;
    uint32_T maxIndex = MaxLen - 1;
    if (u0 <= bp0[0U])
    {
        iLeft = 0U;
        iRght = 0U;
    }
    else if (u0 < bp0[maxIndex])
    {
        //????????u0??λ??
        bpIdx = maxIndex >> 1U;
        iLeft = 0U;
        iRght = maxIndex;
        while ((iRght - iLeft) > 1)
        {
            if (u0 < bp0[bpIdx])
            {
                iRght = bpIdx;
            }
            else
            {
                iLeft = bpIdx;
            }
            bpIdx = (iRght + iLeft) >> 1U;
        }
    }
    else
    {
        iLeft = maxIndex;
        iRght = maxIndex;
    }
    //???λ??????????
    if (iLeft != iRght)
    {
        //??????
        yR_0d0 = table[iLeft + 1U];
        yL_0d0 = table[iLeft];
        if (yR_0d0 >= yL_0d0)
        {
            y = (uint16_T)(((uint32_T)(u0 - bp0[iLeft]) * (yR_0d0 - yL_0d0)) / (bp0[iLeft + 1] - bp0[iLeft]) + yL_0d0);
        }
        else
        {
            y = (uint16_T)((yL_0d0 - ((uint32_T)(u0 - bp0[iLeft]) * (yL_0d0 - yR_0d0)) / (bp0[iLeft + 1] - bp0[iLeft])));
        }
    }
    else
    {
        y = (uint16_T)table[iLeft];
    }
    return y;
}

//-------------------int16 to uint16的一维查表--------------------------------------
uint16_T look1_i16tu16(int16_T u0, const int16_T *bp0, const uint16_T *table, uint16_T MaxLen)
{
    uint32_T bpIdx = 0;
    uint32_T iLeft = 0;
    uint32_T iRght = 0;
    uint16_T y = 0;
    uint32_T yL_0d0 = 0;
    uint32_T yR_0d0 = 0;
    uint32_T maxIndex = MaxLen - 1;
    if (u0 <= bp0[0U])
    {
        iLeft = 0U;
        iRght = 0U;
    }
    else if (u0 < bp0[maxIndex])
    {
        //????????u0??λ??
        bpIdx = maxIndex >> 1U;
        iLeft = 0U;
        iRght = maxIndex;
        while ((iRght - iLeft) > 1)
        {
            if (u0 < bp0[bpIdx])
            {
                iRght = bpIdx;
            }
            else
            {
                iLeft = bpIdx;
            }
            bpIdx = (iRght + iLeft) >> 1U;
        }
    }
    else
    {
        iLeft = maxIndex;
        iRght = maxIndex;
    }
    //???λ??????????
    if (iLeft != iRght)
    {
        //??????
        yR_0d0 = table[iLeft + 1U];
        yL_0d0 = table[iLeft];
        if (yR_0d0 >= yL_0d0)
        {
            y = (uint16_T)(((uint32_T)(u0 - bp0[iLeft]) * (yR_0d0 - yL_0d0)) / (bp0[iLeft + 1] - bp0[iLeft]) + yL_0d0);
        }
        else
        {
            y = (uint16_T)((yL_0d0 - ((uint32_T)(u0 - bp0[iLeft]) * (yL_0d0 - yR_0d0)) / (bp0[iLeft + 1] - bp0[iLeft])));
        }
    }
    else
    {
        y = (uint16_T)table[iLeft];
    }
    return y;
}

//  uint16 二维查表
uint16_T look2_u16u16tu16(uint16_T x, uint16_T y, const uint16_T xTable[], const uint16_T yTable[], const uint16_T zTable[], uint16_T xLen, uint16_T yLen)
{
    uint16_T xMaxIndex;
    uint16_T yMaxIndex;
    uint16_T xIndexLeft;
    uint16_T xIndexRight;
    uint16_T yIndexLeft;
    uint16_T yIndexRight;
    uint16_T xValueLeft;
    uint16_T xValueRight;
    uint16_T yValueLeft;
    uint16_T yValueRight;
    uint16_T bpIdx;
    uint32_T zIndex1;
    uint32_T zIndex2;
    uint32_T zValueMid_xLeft;
    uint32_T zValueMid_xRight;
    uint16_T z;
    xMaxIndex = xLen - 1;
    yMaxIndex = yLen - 1;

    if (x <= xTable[0U])
    {
        xIndexLeft = 0U;
        xIndexRight = 0U;
    }
    else if (x >= xTable[xMaxIndex])
    {
        xIndexLeft = xMaxIndex;
        xIndexRight = xMaxIndex;
    }
    else
    {
        bpIdx = xMaxIndex >> 1U;
        xIndexLeft = 0;
        xIndexRight = xMaxIndex;
        while (xIndexRight - xIndexLeft > 1U)
        {
            if (x < xTable[bpIdx])
            {
                xIndexRight = bpIdx;
            }
            else
            {
                xIndexLeft = bpIdx;
            }

            bpIdx = (xIndexRight + xIndexLeft) >> 1U;
        }
    }
    xValueLeft = xTable[xIndexLeft];
    xValueRight = xTable[xIndexRight];

    if (y <= yTable[0U])
    {
        yIndexLeft = 0U;
        yIndexRight = 0U;
    }
    else if (y >= yTable[yMaxIndex])
    {
        yIndexLeft = yMaxIndex;
        yIndexRight = yMaxIndex;
    }
    else
    {
        bpIdx = yMaxIndex >> 1U;
        yIndexLeft = 0;
        yIndexRight = yMaxIndex;
        while (yIndexRight - yIndexLeft > 1U)
        {
            if (y < yTable[bpIdx])
            {
                yIndexRight = bpIdx;
            }
            else
            {
                yIndexLeft = bpIdx;
            }

            bpIdx = (yIndexRight + yIndexLeft) >> 1U;
        }
    }
    yValueLeft = yTable[yIndexLeft];
    yValueRight = yTable[yIndexRight];

    zIndex1 = yIndexLeft * xLen + xIndexLeft;
    zIndex2 = yIndexRight * xLen + xIndexLeft;
    if (yIndexLeft != yIndexRight)
    {
        if (zTable[zIndex2] > zTable[zIndex1])
        {
            zValueMid_xLeft = (uint16_T)(((uint32_T)(y - yValueLeft) * (zTable[zIndex2] - zTable[zIndex1])) / (yValueRight - yValueLeft) + zTable[zIndex1]);
        }
        else
        {
            zValueMid_xLeft = (uint16_T)(zTable[zIndex1] - ((uint32_T)(y - yValueLeft) * (zTable[zIndex1] - zTable[zIndex2])) / (yValueRight - yValueLeft));
        }

        if (zTable[zIndex2 + 1] > zTable[zIndex1 + 1])
        {
            zValueMid_xRight = (uint16_T)(((uint32_T)(y - yValueLeft) * (zTable[zIndex2 + 1] - zTable[zIndex1 + 1])) / (yValueRight - yValueLeft) + zTable[zIndex1 + 1]);
        }
        else
        {
            zValueMid_xRight = (uint16_T)(zTable[zIndex1 + 1] - ((uint32_T)(y - yValueLeft) * (zTable[zIndex1 + 1] - zTable[zIndex2 + 1])) / (yValueRight - yValueLeft));
        }
    }
    else
    {
        zValueMid_xLeft = (uint16_T)zTable[zIndex1];
        zValueMid_xRight = (uint16_T)zTable[zIndex1 + 1];
    }
    if (xIndexLeft != xIndexRight)
    {
        if (zValueMid_xLeft < zValueMid_xRight)
        {
            z = (uint16_T)(((uint32_T)(x - xValueLeft) * (zValueMid_xRight - zValueMid_xLeft)) / ((uint16_T)(xValueRight - xValueLeft)) + zValueMid_xLeft);
        }
        else
        {
            z = (uint16_T)(zValueMid_xLeft - ((uint32_T)(x - xValueLeft) * (zValueMid_xLeft - zValueMid_xRight)) / ((uint16_T)(xValueRight - xValueLeft)));
        }
    }
    else
    {
        z = (uint16_T)(zValueMid_xLeft);
    }
    return z;
}

//-----------------------------------------------诊断大于阈值
boolean_T DiagThrSystem1(boolean_T Enable, boolean_T precondition, uint16_T Input, uint16_T fltThr, uint16_T recThr, uint8_T fltNumThr, uint8_T recNumThr, uint8_T *fltNum, uint8_T *recNum, boolean_T *fitFlg)
{

    if (Enable && precondition && Input > fltThr)
    {
        *fltNum = (*fltNum + 1) > 200 ? 200 : (*fltNum + 1);
    }
    else
    {
        *fltNum = 0;
    }

    if (Enable && precondition && Input < recThr)
    {
        *recNum = (*recNum + 1) > 200 ? 200 : (*recNum + 1);
    }
    else
    {
        *recNum = 0;
    }

    if ((*fltNum > fltNumThr || (*fitFlg && *recNum < recNumThr)) && precondition)
    {
        *fitFlg = true;
    }
    else
    {
        *fitFlg = false;
    }
    return *fitFlg;
}

//================================诊断小于阈值===================================
boolean_T DiagThrSystem2(boolean_T Enable, boolean_T precondition, uint16_T Input, uint16_T fltThr, uint16_T recThr, uint8_T fltNumThr, uint8_T recNumThr, uint8_T *fltNum, uint8_T *recNum, boolean_T *fitFlg)
{
    if (Enable && precondition && Input < fltThr)
    {
        *fltNum = (*fltNum + 1) > 200 ? 200 : (*fltNum + 1);
    }
    else
    {
        *fltNum = 0;
    }

    if (Enable && precondition && Input > recThr)
    {
        *recNum = (*recNum + 1) > 200 ? 200 : (*recNum + 1);
    }
    else
    {
        *recNum = 0;
    }

    if ((*fltNum > fltNumThr || (*fitFlg && *recNum < recNumThr)) && precondition)
    {
        *fitFlg = true;
    }
    else
    {
        *fitFlg = false;
    }

    return *fitFlg;
}

//---------------------------判断条件成立次数
boolean_T JudgeTimeSystem(boolean_T Enable, boolean_T Input, uint16_T *N, uint16_T Thr)
{
    boolean_T Flg = false;
    if (Input && Enable)
    {
        *N = (*N + 1) > 20000 ? 20000 : (*N + 1);
    }
    else
    {
        *N = 0;
    }
    if (*N > Thr && Enable)
    {
        Flg = true;
    }
    else
    {
        Flg = false;
    }
    return Flg;
}
void fft(real_T *S, uint16_T N, real_T freq, real_T *returnFreq, real_T *returnP) // N为偶数
{

    creal_T X[SIZE_FFT];
    creal_T W[SIZE_FFT];
    real_T Y1[SIZE_FFT];
    real_T Y2[SIZE_FFT / 2 + 1];
    real_T F[SIZE_FFT / 2 + 1];

    uint16_T i = 0;
    uint16_T j = 0;
    uint16_T k = 0;
    uint16_T l = 0;
    creal_T up;
    creal_T down;
    creal_T product;

    // 变换
    for (uint16_t i = 0; i < N; i++)
    {
        X[i].re = S[i];
        X[i].im = 0;
    }
    RaderReverse(X, N);

    for (i = 0; i < N; i++)
    {
        W[i].re = cos(2 * PI / N * i);
        W[i].im = -sin(2 * PI / N * i);
    }
    for (i = 0; i < log(N) / log(2); i++)
    {
        l = 1 << i;
        for (j = 0; j < N; j += 2 * l)
        {
            for (k = 0; k < l; k++)
            {
                cmul(X[j + k + l], W[N * k / 2 / l], &product);
                cadd(X[j + k], product, &up);
                csub(X[j + k], product, &down);
                X[j + k] = up;
                X[j + k + l] = down;
            }
        }
    }

    // 计算幅值
    for (i = 0; i < N; i++)
    {
        Y1[i] = sqrt((X[i].im) * (X[i].im) + (X[i].re) * (X[i].re)) / N;
    }

    Y2[0] = Y1[0];
    Y2[N / 2] = Y1[N / 2];
    for (i = 1; i < N / 2; i++)
    {
        Y2[i] = 2 * Y1[i];
    }
    // 计算频率
    for (i = 0; i < N / 2 + 1; i++)
    {
        F[i] = freq * i / N;
    }

    // 从大到小 排序
    real_T temp;
    uint16_T temp_idx;
    uint16_T idx[5];
    for (i = 0; i < N / 2 + 1; i++)
    {
        idx[i] = i;
    }

    for (j = 0; j < N / 2 + 1; j++) // 比较n-1轮
    {
        for (k = 0; k < N / 2 - j; k++) // 每轮比较n-1-i次,
        {

            if (Y2[k] < Y2[k + 1]) // 从大到小
            {
                temp = Y2[k];
                Y2[k] = Y2[k + 1];
                Y2[k + 1] = temp;

                temp_idx = idx[k];
                idx[k] = idx[k + 1];
                idx[k + 1] = temp_idx;
            }
        }
    }
    // 输出前5个
    for (i = 0; i < 5; i++)
    {
        returnFreq[i] = F[idx[i]];
        returnP[i] = Y2[i];
    }
}

//
void RaderReverse(creal_T *X, uint16_T N)
{
    uint16_T i;
    uint16_T j;
    uint16_T k;
    creal_T temp;
    j = N / 2;
    for (i = 1; i < N - 1; i++)
    {
        if (i < j)
        {
            temp = X[j];
            X[j] = X[i];
            X[i] = temp;
        }
        k = N / 2;
        while (k <= j)
        {
            j = j - k;
            k = k / 2;
        }
        j = j + k;
    }
}

void cmul(creal_T a, creal_T b, creal_T *c)
{
    c->re = a.re * b.re - a.im * b.im;
    c->im = a.re * b.im + a.im * b.re;
}
void cadd(creal_T a, creal_T b, creal_T *c)
{
    c->re = a.re + b.re;
    c->im = a.im + b.im;
}
void csub(creal_T a, creal_T b, creal_T *c)
{
    c->re = a.re - b.re;
    c->im = a.im - b.im;
}