/* * @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); static int fft_cmp(const void*p1,const void*p2) { return (*(struct _fft_Freq*)p2).amp>(*(struct _fft_Freq*)p1).amp ? 1 : -1; } 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]; fft_type fft_data[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; } fft_data[0].amp = Y1[0]; fft_data[N / 2].amp = Y1[N / 2]; for (i = 1; i < N / 2; i++) { fft_data[i].amp = 2 * Y1[i]; } // 计算频率 for (i = 0; i < N / 2 + 1; i++) { fft_data[i].freq = freq * i / N; } // 从大到小 排序 // real_T temp; // uint16_T temp_idx; // uint16_T idx[N / 2 + 1]; // 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个 qsort(fft_data,sizeof(fft_data) / sizeof(fft_data[0]),sizeof(fft_data[0]),fft_cmp); for (i = 0; i < 5; i++) { returnFreq[i] = fft_data[i].freq; returnP[i] = fft_data[i].amp; } } // 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; }