01.使用定点数查表法计算三角函数

丁毅桂大约 1 分钟

01.使用定点数查表法计算三角函数

返回值
- 类型: int16_t
- 映射关系：[-32768,32767] 表示 [-1,1]
- 精度：1/32768 = 0.0000305
角度
- 类型: uint16_t
- 映射关系：[0,65535] 表示 [0,2PI]
- 精度：360°/65535 = 0.00549°

具体实现

foc_utils.c

#include "foc_utils.h"
/**
 * 使用定点数计算sin
 * @param a uint16_t [0,65535] 表示 [0,2PI] 精度：360°/65535 = 0.00549°
 * @return int16_t [-32768,32767] 表示 [-1,1] 精度：1/32768 = 0.0000305
 */
__attribute__((weak)) int16_t _sin(uint16_t a)
{
    // sine_array [0,1] => [0,32767] 这里要用32767表示1，因为int16_t可以表示-32768，但是无法表示+32768
    static uint16_t sine_array[65] = {0, 804, 1608, 2410, 3212, 4011, 4808, 5602, 6393, 7179, 7962, 8739, 9512, 10278, 11039, 11793, 12539, 13279, 14010, 14732, 15446, 16151, 16846, 17530, 18204, 18868, 19519, 20159, 20787, 21403, 22005, 22594, 23170, 23731, 24279, 24811, 25329, 25832, 26319, 26790, 27245, 27683, 28105, 28510, 28898, 29268, 29621, 29956, 30273, 30571, 30852, 31113, 31356, 31580, 31785, 31971, 32137, 32285, 32412, 32521, 32609, 32678, 32728, 32757, 32767};
    int32_t t1, t2;
    uint16_t i = (uint32_t)a * 64 * 4 * 256 / _2PI_;
    uint16_t frac = i & 0xff;
    i = (i >> 8) & 0xff;
    if (i < 64)
    {
        t1 = sine_array[i];
        t2 = sine_array[i + 1];
    }
    else if (i < 128)
    {
        t1 = sine_array[128 - i];
        t2 = sine_array[128 - (i + 1)];
    }
    else if (i < 192)
    {
        // 因为-((uint16_t)0)会计算错误 所以要写成-((int32_t)(uint16_t)0)
        t1 = -(int32_t)sine_array[-128 + i];
        t2 = -(int32_t)sine_array[-128 + (i + 1)];
    }
    else
    {
        t1 = -(int32_t)sine_array[256 - i];
        t2 = -(int32_t)sine_array[256 - (i + 1)];
    }
    return (t1 + (((t2 - t1) * frac / 256)));
}

__attribute__((weak)) int16_t _cos(uint16_t a)
{
    // _cos和sin相位差90°，所以加上PI/2
    // 防止溢出所以使用uint32_t
    uint32_t a_sin = a + _PI_2_;
    a_sin = a_sin > _2PI_ ? a_sin - _2PI_ : a_sin;
    return _sin(a_sin);
}

__attribute__((weak)) void _sincos(uint16_t a, int16_t *s, int16_t *c)
{
    *s = _sin(a);
    *c = _cos(a);
}

foc_utils.h

#ifndef __FOC_UTILS_H__
#define __FOC_UTILS_H__
#include <stdint.h>
#include "typedef.h"

#ifdef __cplusplus
extern "C"
{
#endif
    /**
     * 定点数计算sin
     * @param a uint16_t [0,65535] 表示 [0,2PI] 精度：360°/65535 = 0.00549°
     * @return int16_t [-32768,32767] 表示 [-1,1] 精度：1/32768 = 0.0000305
     */
    int16_t _sin(uint16_t a);
    int16_t _cos(uint16_t a);
    void _sincos(uint16_t a, int16_t *s, int16_t *c);
#ifdef __cplusplus
}
#endif

#endif

typedef.h

// 单位1 32767
#define _INT16_ONE_ 32767
// 单位PI 180°
#define _PI_ 32767
// 360°
#define _2PI_ 65535
// 90°
#define _PI_2_ 16383
// 180°
#define _PI_4_ 16383
// -90°=270°
#define _3_PI_2_ 16383

实现效果

01.sin_and_cos.ino

#include <Arduino.h>
#include "foc_utils.h"
#include "typedef.h"

void setup() {
  Serial.begin(9600);
}

uint16_t a = 0;

void loop() {
  a += 1500;
  if (a > _2PI_) a -= _2PI_;
  Serial.print(_sin(a));
  Serial.print(',');
  Serial.print(_cos(a));
  Serial.print('\n');
}

浮点数运算与定点数运算的耗时对比

#include <Arduino.h>
#include <stdint.h>

void setup() {
  Serial.begin(9600);
}

#define Times 10000

void testUint32() {
  uint32_t t1 = micros(), t2;
  uint32_t temp = 1;
  for (uint32_t i = 1; i <= Times; i++) {
    temp += 1234567; // 防止编译器优化
    temp *= 1234567; // 防止编译器优化
  }
  t2 = micros();
  Serial.print("uint32_t:");
  Serial.print(temp);// 防止编译器优化
  Serial.print(',');
  Serial.print((float)(t2 - t1) / Times);
  Serial.print("us");
  Serial.println();
}

void testFloat32() {
  uint32_t t1 = micros(), t2;
  float temp = 1;
  for (uint32_t i = 1; i <= Times; i++) {
    temp += 0.001234f; // 防止编译器优化
    temp *= 1.001234f; // 防止编译器优化
  }
  t2 = micros();
  Serial.print("float:");
  Serial.print(temp); // 防止编译器优化
  Serial.print(',');
  Serial.print((float)(t2 - t1) / Times);
  Serial.print("us");
  Serial.println();
}

void loop() {
  testUint32();
  testFloat32();
}

输出结果

11:18:20.188 -> uint32_t:2763457473,5.80us
11:18:20.397 -> float:454365.65,18.69us
11:18:20.434 -> uint32_t:2763457473,5.80us
11:18:20.630 -> float:454365.65,18.69us
11:18:20.708 -> uint32_t:2763457473,5.80us
11:18:20.877 -> float:454365.65,18.69us
11:18:20.950 -> uint32_t:2763457473,5.80us
11:18:21.119 -> float:454365.65,18.69us
11:18:21.196 -> uint32_t:2763457473,5.80us
11:18:21.372 -> float:454365.65,18.69us
11:18:21.449 -> uint32_t:2763457473,5.80us
11:18:21.614 -> float:454365.65,18.69us
11:18:21.691 -> uint32_t:2763457473,5.80us
11:18:21.864 -> float:454365.65,18.69us
11:18:21.937 -> uint32_t:2763457473,5.80us
11:18:22.126 -> float:454365.65,18.69us
11:18:22.162 -> uint32_t:2763457473,5.80us
11:18:22.375 -> float:454365.65,18.69us

结论

在8位、16Mhz主频、无FPU浮点数计算单元的单片机上
- Arduino UNO R3 （ATmega328P）
浮点数运算耗时大约是定点数耗时的3倍。