hpm6750/controller_yy_app/controlware/my_math.c

#include "my_math.h"
#include "hpm_math.h"
#include <float.h>

/*
关于 DSP库的使用

=================================已经测试可以使用的========================
100个float数据的运算
sin() 1839us-----sinf() 71us------arm_sin_f32 92us

100个float数据的运算
sqrt() 421us-----sqrtf() 39us------__sqrtf() 24us------arm_sin_f32  29us
*/

// @brief 把 buf 中数据按照小端形式搬运到 float 中
void buf2float(float *tfloat, unsigned char *buf)
{
    int i;
    unsigned char *p1 = (unsigned char *)tfloat;
    unsigned char *p3 = buf;
    for (i = 0; i < 4; i++)
    {
        *p1 = *p3;
        p1++;
        p3++;
    }
}

// @brief 把 buf 中数据按照小端形式搬运到 long 中
void buf2long(long *tfloat, unsigned char *buf)
{
    int i;
    unsigned char *p1 = (unsigned char *)tfloat;
    unsigned char *p3 = buf;
    for (i = 0; i < 4; i++)
    {
        *p1 = *p3;
        p1++;
        p3++;
    }
}

// @brief 把 buf 中数据按照小端形式搬运到 int 中
void buf2int(int *tint, unsigned char *buf)
{
    int i;
    unsigned char *p1 = (unsigned char *)tint;
    unsigned char *p3 = buf;
    for (i = 0; i < 4; i++)
    {
        *p1 = *p3;
        p1++;
        p3++;
    }
}

// @brief 把 buf 中数据按照小端形式搬运到 short 中
void buf2short(short *tshort, unsigned char *buf)
{
    int i;
    unsigned char *p1 = (unsigned char *)tshort;
    unsigned char *p3 = buf;
    for (i = 0; i < 2; i++)
    {
        *p1 = *p3;
        p1++;
        p3++;
    }
}

void float2buf(unsigned char *buf, float *tfloat)
{
    int i;
    unsigned char *p1 = (unsigned char *)tfloat;
    unsigned char *p3 = buf;
    for (i = 0; i < 4; i++)
    {
        *p3 = *p1;
        p1++;
        p3++;
    }
}

void int2buf(unsigned char *buf, int *tint)
{
    int i;
    unsigned char *p1 = (unsigned char *)tint;
    unsigned char *p3 = buf;
    for (i = 0; i < 4; i++)
    {
        *p3 = *p1;
        p1++;
        p3++;
    }
}

/*
STM32默认是小端模式

数据： 11 22 33 44
字节： 高→→→→→→→低

小端模式
地址：0   1   2   3
内存：44  33  22  11

大端模式
地址：0   1   2   3
内存：11  22  33  44
*/
void short2buf(unsigned char *buf, short *tshort)
{
    int i;
    unsigned char *p1 = (unsigned char *)tshort;
    unsigned char *p3 = buf;
    for (i = 0; i < 2; i++)
    {
        *p3 = *p1;
        p1++;
        p3++;
    }
}

int brinv(float a[], int n)
{
    int *is, *js, i, j, k, l, u, v;
    int temp1[5];
    int temp2[5];
    float d, p;
    is = temp1;
    js = temp2;
    for (k = 0; k <= n - 1; k++)
    {
        d = 0.0f;
        for (i = k; i <= n - 1; i++)
            for (j = k; j <= n - 1; j++)
            {
                l = i * n + j;
                p = fabsf(a[l]);
                if (p > d)
                {
                    d = p;
                    is[k] = i;
                    js[k] = j;
                }
            }

        if (d + 1.0f == 1.0f)
        {
            return (0);
        }
        if (is[k] != k)
        {
            for (j = 0; j <= n - 1; j++)
            {
                u = k * n + j;
                v = is[k] * n + j;
                p = a[u];
                a[u] = a[v];
                a[v] = p;
            }
        }
        if (js[k] != k)
            for (i = 0; i <= n - 1; i++)
            {
                u = i * n + k;
                v = i * n + js[k];
                p = a[u];
                a[u] = a[v];
                a[v] = p;
            }
        l = k * n + k;
        a[l] = 1.0f / a[l];
        for (j = 0; j <= n - 1; j++)
            if (j != k)
            {
                u = k * n + j;
                a[u] = a[u] * a[l];
            }
        for (i = 0; i <= n - 1; i++)
            if (i != k)
                for (j = 0; j <= n - 1; j++)
                    if (j != k)
                    {
                        u = i * n + j;
                        a[u] = a[u] - a[i * n + k] * a[k * n + j];
                    }
        for (i = 0; i <= n - 1; i++)
            if (i != k)
            {
                u = i * n + k;
                a[u] = -a[u] * a[l];
            }
    }
    for (k = n - 1; k >= 0; k--)
    {
        if (js[k] != k)
            for (j = 0; j <= n - 1; j++)
            {
                u = k * n + j;
                v = js[k] * n + j;
                p = a[u];
                a[u] = a[v];
                a[v] = p;
            }
        if (is[k] != k)
            for (i = 0; i <= n - 1; i++)
            {
                u = i * n + k;
                v = i * n + is[k];
                p = a[u];
                a[u] = a[v];
                a[v] = p;
            }
    }
    return (1);
}

unsigned char constrain_uint8(unsigned char value, unsigned char min,
                              unsigned char max)
{
    if (value > max)
        value = max;
    else if (value < min)
        value = min;
    return value;
}

short math_constrain(short value, short min, short max)
{
    if (value > max)
        value = max;
    else if (value < min)
        value = min;
    return value;
}

// constrain a value
// float constrain_float(float amt, float low, float high)
// {
//     // the check for NaN as a float prevents propogation of
//     // floating point errors through any function that uses
//     // constrain_float(). The normal float semantics already handle -Inf
//     // and +Inf
//     if (isnan(amt))
//     {
//         return (low + high) * 0.5f;
//     }
//     return ((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)));
// }

// constrain a int16_t value
int16_t constrain_int16(int16_t amt, int16_t low, int16_t high)
{
    return ((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)));
}

// constrain a int32_t value
int32_t constrain_int32(int32_t amt, int32_t low, int32_t high)
{
    return ((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)));
}

short abs_int16(short value)
{
    if ((value > 0) || (value == 0))
        return value;
    return -value;
}

float min_float(float value1, float value2)
{
    if (value1 < value2)
        return value1;
    return value2;
}

int min_int32(int value1, int value2)
{
    if (value1 < value2)
        return value1;
    return value2;
}

short min_int16(short value1, short value2)
{
    if (value1 < value2)
        return value1;
    return value2;
}

float max_float(float value1, float value2)
{
    if (value1 > value2)
        return value1;
    return value2;
}

int max_int32(int value1, int value2)
{
    if (value1 > value2)
        return value1;
    return value2;
}

short max_int16(short value1, short value2)
{
    if (value1 > value2)
        return value1;
    return value2;
}

// is a float is zero
bool is_zero(const float fVal1)
{
    return fabsf(fVal1) < FLT_EPSILON ? true : false;
}

float no_zero_float(float Val1)
{
    if (Val1 == 0.0f)
        return 0.1f;
    return Val1;
}

int no_zero_int32(int Val1)
{
    if (Val1 == 0)
        return 1;
    return Val1;
}

// square
float sq(float v) { return v * v; }

// @brief 获取两个数的平方和开根号
float get_norm(float a, float b)
{
    float norm;
    arm_sqrt_f32(sq(a) + sq(b), &norm);

    return norm;
}

// 计算数据的标准差
// float stdev(short num[], int n)
// {
//     int i = 0;
//     float sum = 0.0f;
//     float avg = 0.0f;
//
//     sum = 0.0f;
//     for (i = 0; i < n; i++)
//     {
//         sum += num[i];
//     }
//
//     avg = sum / n;
//
//     sum = 0.0f;
//     for (i = 0; i < n; i++)
//     {
//         sum += (num[i] - avg) * (num[i] - avg);
//     }
//
//     return (__sqrtf(sum / n));
// }

/** @brief 插入排序算法
 * @param int arr[]被排数列，int n 排序窗宽
 * @retval none
 */
void insert_sort(int arr[], int n)
{
    int temp;
    int i, j;
    for (i = 1; i < n; ++i)
    {
        temp = arr[i];
        for (j = i; j > 0 && temp < arr[j - 1]; --j)
        {
            arr[j] = arr[j - 1];
        }
        arr[j] = temp;
    }
}

// =========================