mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-05 15:30:08 +08:00
Add compressed sine table for flash and RAM
This commit is contained in:
Martin Mueller
committed by
Felix Ruess
Felix Ruess
parent
3a50277747
commit
eea7ece463
@@ -225,6 +225,21 @@ test_telemetry.CFLAGS += $(COMMON_TELEMETRY_CFLAGS)
|
||||
test_telemetry.srcs += $(COMMON_TELEMETRY_SRCS)
|
||||
test_telemetry.srcs += test/test_telemetry.c
|
||||
|
||||
#
|
||||
# test_math_trig_compressed: Test math trigonometric using compressed data
|
||||
#
|
||||
# configuration
|
||||
# MODEM_PORT :
|
||||
# MODEM_BAUD :
|
||||
#
|
||||
test_math_trig_compressed.ARCHDIR = $(ARCH)
|
||||
test_math_trig_compressed.CFLAGS += $(COMMON_TEST_CFLAGS)
|
||||
test_math_trig_compressed.srcs += $(COMMON_TEST_SRCS)
|
||||
test_math_trig_compressed.CFLAGS += $(COMMON_TELEMETRY_CFLAGS)
|
||||
test_math_trig_compressed.CFLAGS += -DPPRZ_TRIG_INT_TEST
|
||||
test_math_trig_compressed.srcs += $(COMMON_TELEMETRY_SRCS)
|
||||
test_math_trig_compressed.srcs += test/test_math_trig_compressed.c math/pprz_trig_int.c
|
||||
|
||||
|
||||
#
|
||||
# test ms2100 mag
|
||||
|
||||
@@ -160,6 +160,10 @@ STATIC_INLINE void main_init(void)
|
||||
{
|
||||
mcu_init();
|
||||
|
||||
#if defined(PPRZ_TRIG_INT_COMPR_FLASH)
|
||||
pprz_trig_int_init();
|
||||
#endif
|
||||
|
||||
electrical_init();
|
||||
|
||||
stateInit();
|
||||
|
||||
@@ -27,6 +27,7 @@
|
||||
#include "pprz_trig_int.h"
|
||||
#include "pprz_algebra_int.h"
|
||||
|
||||
#if !defined(PPRZ_TRIG_INT_COMPR_FLASH) || defined(PPRZ_TRIG_INT_TEST)
|
||||
PPRZ_TRIG_CONST int16_t pprz_trig_int[6434] = { 0,
|
||||
3, 7, 11, 15, 19, 23, 27, 31, 35, 39, 43, 47, 51, 55, 59, 63,
|
||||
67, 71, 75, 79, 83, 87, 91, 95, 99, 103, 107, 111, 115, 119, 123, 127,
|
||||
@@ -432,7 +433,391 @@ PPRZ_TRIG_CONST int16_t pprz_trig_int[6434] = { 0,
|
||||
16383, 16383, 16383, 16383, 16383, 16383, 16383, 16383, 16383, 16383, 16383, 16383, 16383, 16383, 16383, 16383,
|
||||
16383
|
||||
};
|
||||
#endif
|
||||
|
||||
#if defined(PPRZ_TRIG_INT_COMPR_FLASH)
|
||||
|
||||
const uint8_t pprz_trig_int_compr[TRIG_INT_COMPR_LEN] = {
|
||||
0xF3, 0xCF, 0xFF, 0xFC, 0xCF, 0xFF, 0xFC, 0xCF, 0xFF, 0xFC, 0xCF, 0x9E,
|
||||
0xF3, 0xCF, 0x8F, 0x3C, 0xFF, 0xFC, 0x41, 0xF3, 0xCF, 0x34, 0x9F, 0x3C,
|
||||
0x4F, 0x3C, 0x1F, 0x3C, 0xFF, 0x34, 0xCF, 0x34, 0xBF, 0x34, 0xAF, 0x34,
|
||||
0x8F, 0x34, 0x7F, 0x34, 0x6F, 0x34, 0x6F, 0x34, 0x4F, 0x34, 0x4F, 0x34,
|
||||
0x4F, 0x34, 0x3F, 0x34, 0x2F, 0x34, 0x3F, 0x34, 0x1F, 0x34, 0x1F, 0x34,
|
||||
0x1F, 0x34, 0x1F, 0x34, 0x0F, 0x34, 0x0F, 0x34, 0x0F, 0x34, 0x0F, 0x34,
|
||||
0xEE, 0x3E, 0xEE, 0x3E, 0xEE, 0x3E, 0xEE, 0x39, 0xEE, 0x3E, 0xEE, 0x39,
|
||||
0xEE, 0x39, 0xEE, 0x39, 0xEE, 0x39, 0xEE, 0x34, 0xEE, 0x39, 0xEE, 0x34,
|
||||
0xEE, 0x34, 0xEE, 0x34, 0xEE, 0x34, 0xEE, 0x34, 0xEE, 0x34, 0xEE, 0xE3,
|
||||
0x4E, 0xE3, 0x3E, 0xEE, 0xE3, 0x4E, 0xE3, 0x3E, 0xEE, 0xE3, 0x3E, 0x9E,
|
||||
0xE3, 0x3E, 0xEE, 0xE3, 0x39, 0xEE, 0xE3, 0x39, 0xEE, 0xE3, 0x39, 0x9E,
|
||||
0xE3, 0x39, 0x9E, 0xE3, 0x39, 0x9E, 0xE3, 0x39, 0x9E, 0xE3, 0x39, 0x9E,
|
||||
0xE3, 0x34, 0x9E, 0xE3, 0x39, 0x4E, 0xE3, 0x39, 0x4E, 0xE3, 0x39, 0x4E,
|
||||
0xE3, 0x34, 0x4E, 0xE3, 0x39, 0x4E, 0xE3, 0x34, 0x4E, 0xE3, 0x34, 0x4E,
|
||||
0xE3, 0x34, 0x4E, 0xE3, 0x34, 0x4E, 0xE3, 0x34, 0x4E, 0xE3, 0xE3, 0x34,
|
||||
0x4E, 0xE3, 0xE3, 0x34, 0x4E, 0xE3, 0xE3, 0x34, 0x3E, 0x4E, 0xE3, 0xE3,
|
||||
0x34, 0x3E, 0x3E, 0x4E, 0xE3, 0xE3, 0xE3, 0x34, 0x3E, 0x3E, 0x3E, 0x3E,
|
||||
0x4E, 0xE3, 0xE3, 0xE3, 0xE3, 0xE3, 0xE3, 0xE3, 0xE3, 0xE3, 0xE3, 0xE3,
|
||||
0xE3, 0x93, 0xE3, 0xE3, 0xE3, 0xE3, 0x93, 0xE3, 0xE3, 0xE3, 0x93, 0xE3,
|
||||
0xE3, 0x93, 0xE3, 0xE3, 0x93, 0xE3, 0x93, 0xE3, 0x93, 0xE3, 0x93, 0xE3,
|
||||
0x93, 0xE3, 0x93, 0xE3, 0x93, 0x93, 0xE3, 0x93, 0x93, 0xE3, 0x93, 0x93,
|
||||
0xE3, 0x93, 0x93, 0x93, 0xE3, 0x93, 0x93, 0x93, 0xE3, 0x93, 0x93, 0x93,
|
||||
0x93, 0x93, 0x93, 0xE3, 0x93, 0x93, 0x93, 0x93, 0x93, 0x93, 0x93, 0x93,
|
||||
0x93, 0x93, 0x93, 0x93, 0x93, 0x93, 0x93, 0x93, 0x93, 0x93, 0x93, 0x43,
|
||||
0x93, 0x93, 0x93, 0x93, 0x93, 0x93, 0x43, 0x93, 0x93, 0x93, 0x93, 0x43,
|
||||
0x93, 0x93, 0x93, 0x43, 0x93, 0x93, 0x93, 0x43, 0x93, 0x93, 0x93, 0x43,
|
||||
0x93, 0x93, 0x43, 0x93, 0x93, 0x43, 0x93, 0x43, 0x93, 0x93, 0x43, 0x93,
|
||||
0x43, 0x93, 0x93, 0x43, 0x93, 0x43, 0x93, 0x43, 0x93, 0x43, 0x93, 0x43,
|
||||
0x93, 0x43, 0x93, 0x43, 0x93, 0x43, 0x93, 0x43, 0x93, 0x43, 0x93, 0x43,
|
||||
0x43, 0x93, 0x43, 0x93, 0x43, 0x43, 0x93, 0x43, 0x93, 0x43, 0x43, 0x93,
|
||||
0x43, 0x43, 0x93, 0x43, 0x43, 0x93, 0x43, 0x43, 0x93, 0x43, 0x43, 0x93,
|
||||
0x43, 0x43, 0x43, 0x93, 0x43, 0x43, 0x93, 0x43, 0x43, 0x43, 0x93, 0x43,
|
||||
0x43, 0x43, 0x43, 0x93, 0x43, 0x43, 0x43, 0x43, 0x93, 0x43, 0x43, 0x43,
|
||||
0x43, 0x43, 0x93, 0x43, 0x43, 0x43, 0x43, 0x43, 0x93, 0x43, 0x43, 0x43,
|
||||
0x43, 0x43, 0x43, 0x43, 0x43, 0x93, 0x43, 0x43, 0x43, 0x43, 0x43, 0x43,
|
||||
0x43, 0x43, 0x43, 0x43, 0x43, 0x43, 0x43, 0x43, 0x43, 0x43, 0x43, 0x43,
|
||||
0x93, 0x43, 0x43, 0x43, 0x43, 0x43, 0x43, 0x43, 0x43, 0x48, 0x43, 0x43,
|
||||
0x43, 0x43, 0x43, 0x43, 0x43, 0x43, 0x43, 0x43, 0x43, 0x43, 0x43, 0x43,
|
||||
0x43, 0x43, 0x43, 0x48, 0x43, 0x43, 0x43, 0x43, 0x43, 0x43, 0x43, 0x43,
|
||||
0x48, 0x43, 0x43, 0x43, 0x43, 0x43, 0x43, 0x48, 0x43, 0x43, 0x43, 0x43,
|
||||
0x48, 0x43, 0x43, 0x43, 0x43, 0x48, 0x43, 0x43, 0x43, 0x43, 0x48, 0x43,
|
||||
0x43, 0x48, 0x43, 0x43, 0x43, 0x48, 0x43, 0x43, 0x48, 0x43, 0x43, 0x43,
|
||||
0x48, 0x43, 0x43, 0x48, 0x43, 0x48, 0x43, 0x43, 0x48, 0x43, 0x43, 0x48,
|
||||
0x43, 0x48, 0x43, 0x48, 0x43, 0x43, 0x48, 0x43, 0x48, 0x43, 0x48, 0x43,
|
||||
0x48, 0x43, 0x48, 0x43, 0x48, 0x43, 0x48, 0x48, 0x43, 0x48, 0x43, 0x48,
|
||||
0x43, 0x48, 0x48, 0x43, 0x48, 0x48, 0x43, 0x48, 0x48, 0x43, 0x48, 0x48,
|
||||
0x43, 0x48, 0x48, 0x48, 0x43, 0x48, 0x48, 0x48, 0x48, 0x43, 0x48, 0x48,
|
||||
0x48, 0x48, 0x48, 0x48, 0x48, 0x48, 0x43, 0x48, 0x48, 0x48, 0x48, 0x48,
|
||||
0x48, 0x48, 0x4D, 0x48, 0x48, 0x48, 0x48, 0x48, 0x48, 0x48, 0x4D, 0x48,
|
||||
0x48, 0x48, 0x4D, 0x48, 0x48, 0x48, 0x4D, 0x48, 0x4D, 0x48, 0x48, 0x4D,
|
||||
0x48, 0x4D, 0x48, 0x4D, 0x48, 0x4D, 0x4D, 0x48, 0x4D, 0x4D, 0x48, 0x4D,
|
||||
0x4D, 0x4D, 0x4D, 0x4D, 0x48, 0x4D, 0x4D, 0x4D, 0x3D, 0xD4, 0xD4, 0xD4,
|
||||
0xD4, 0xD4, 0x43, 0x4D, 0x4D, 0x3D, 0xD4, 0xD4, 0x43, 0x4D, 0x3D, 0xD4,
|
||||
0x43, 0x4D, 0x3D, 0xD4, 0x43, 0x3D, 0xD4, 0x43, 0x3D, 0xD4, 0x43, 0x8D,
|
||||
0xD4, 0x43, 0x3D, 0xD4, 0x48, 0x8D, 0xD4, 0x43, 0x8D, 0xD4, 0x48, 0x8D,
|
||||
0xD4, 0x4D, 0x8D, 0xD4, 0x4D, 0xDD, 0xD4, 0x4D, 0xDD, 0xD4, 0x4D, 0xDD,
|
||||
0x43, 0xDD, 0x43, 0xDD, 0x43, 0xDD, 0x48, 0xDD, 0x48, 0xDD, 0x4D, 0xDD,
|
||||
0x4D, 0x0F, 0x43, 0x1F, 0x43, 0x2F, 0x43, 0x4F, 0x43, 0x7F, 0x43, 0xCF,
|
||||
0x43, 0xFF, 0xDB, 0x43, 0xBF, 0x23, 0xFF, 0xDB, 0xF2, 0x3C, 0xF2, 0x36,
|
||||
0xF2, 0x34, 0xF2, 0x32, 0xF2, 0x31, 0xD2, 0xDD, 0xD2, 0xDD, 0xD2, 0xDD,
|
||||
0xD2, 0x8D, 0xD2, 0x3D, 0xD2, 0x3D, 0xD2, 0x3D, 0xD2, 0x2D, 0xDD, 0x23,
|
||||
0x8D, 0xD2, 0x2D, 0xDD, 0xD2, 0x28, 0x8D, 0xD2, 0x28, 0x8D, 0xD2, 0x28,
|
||||
0x3D, 0xD2, 0x28, 0x3D, 0xD2, 0x23, 0x8D, 0xD2, 0x23, 0x3D, 0xD2, 0x23,
|
||||
0x2D, 0x3D, 0xD2, 0x23, 0x2D, 0x3D, 0xD2, 0xD2, 0x23, 0x2D, 0x3D, 0xD2,
|
||||
0xD2, 0xD2, 0xD2, 0xD2, 0xD2, 0xD2, 0xD2, 0xD2, 0xD2, 0xD2, 0x82, 0xD2,
|
||||
0xD2, 0x82, 0xD2, 0x82, 0xD2, 0xD2, 0x82, 0x82, 0xD2, 0x82, 0xD2, 0x82,
|
||||
0x82, 0xD2, 0x82, 0x82, 0x82, 0x82, 0xD2, 0x82, 0x82, 0x82, 0x82, 0x82,
|
||||
0x82, 0x82, 0x82, 0x82, 0x82, 0x82, 0x82, 0x82, 0x82, 0x82, 0x32, 0x82,
|
||||
0x82, 0x82, 0x82, 0x32, 0x82, 0x82, 0x82, 0x32, 0x82, 0x82, 0x32, 0x82,
|
||||
0x82, 0x32, 0x82, 0x32, 0x82, 0x82, 0x32, 0x82, 0x32, 0x82, 0x32, 0x82,
|
||||
0x32, 0x82, 0x32, 0x82, 0x32, 0x82, 0x32, 0x32, 0x82, 0x32, 0x82, 0x32,
|
||||
0x32, 0x82, 0x32, 0x32, 0x82, 0x32, 0x32, 0x32, 0x82, 0x32, 0x32, 0x82,
|
||||
0x32, 0x32, 0x32, 0x82, 0x32, 0x32, 0x32, 0x32, 0x82, 0x32, 0x32, 0x32,
|
||||
0x32, 0x32, 0x32, 0x82, 0x32, 0x32, 0x32, 0x32, 0x32, 0x32, 0x32, 0x32,
|
||||
0x32, 0x82, 0x32, 0x32, 0x32, 0x32, 0x32, 0x32, 0x32, 0x32, 0x32, 0x32,
|
||||
0x32, 0x32, 0x32, 0x32, 0x32, 0x32, 0x32, 0x32, 0x32, 0x37, 0x32, 0x32,
|
||||
0x32, 0x32, 0x32, 0x32, 0x32, 0x32, 0x32, 0x37, 0x32, 0x32, 0x32, 0x32,
|
||||
0x32, 0x37, 0x32, 0x32, 0x32, 0x32, 0x37, 0x32, 0x32, 0x32, 0x37, 0x32,
|
||||
0x32, 0x32, 0x37, 0x32, 0x32, 0x37, 0x32, 0x32, 0x37, 0x32, 0x32, 0x37,
|
||||
0x32, 0x37, 0x32, 0x32, 0x37, 0x32, 0x37, 0x32, 0x37, 0x32, 0x37, 0x32,
|
||||
0x37, 0x32, 0x37, 0x32, 0x37, 0x37, 0x32, 0x37, 0x37, 0x32, 0x37, 0x37,
|
||||
0x32, 0x37, 0x37, 0x32, 0x37, 0x37, 0x37, 0x37, 0x32, 0x37, 0x37, 0x37,
|
||||
0x37, 0x37, 0x37, 0x37, 0x37, 0x37, 0x37, 0x37, 0x37, 0x37, 0x37, 0x37,
|
||||
0x3C, 0x37, 0x37, 0x37, 0x3C, 0x37, 0x37, 0x3C, 0x37, 0x37, 0x3C, 0x37,
|
||||
0x3C, 0x3C, 0x37, 0x3C, 0x37, 0x3C, 0x3C, 0x3C, 0x3C, 0x3C, 0x3C, 0x3C,
|
||||
0x3C, 0x3C, 0x3C, 0x3C, 0x3C, 0x2C, 0xC3, 0xC3, 0x32, 0x3C, 0x2C, 0xC3,
|
||||
0x32, 0x2C, 0xC3, 0x32, 0x2C, 0xC3, 0x32, 0x2C, 0xC3, 0x32, 0x7C, 0xC3,
|
||||
0x37, 0x2C, 0xC3, 0x37, 0xCC, 0xC3, 0x37, 0xCC, 0xC3, 0x37, 0xCC, 0x32,
|
||||
0xCC, 0xC3, 0x2C, 0xC3, 0x2C, 0xC3, 0x7C, 0xC3, 0x7C, 0xC3, 0xCC, 0xF3,
|
||||
0x21, 0xF3, 0x21, 0xF3, 0x24, 0xF3, 0x27, 0xF3, 0xA1, 0xF3, 0xAF, 0x3F,
|
||||
0x12, 0x2F, 0x1A, 0x7F, 0x12, 0x3F, 0x12, 0x2F, 0x12, 0x0F, 0x12, 0xCC,
|
||||
0x1C, 0xCC, 0x17, 0xCC, 0x17, 0xCC, 0x12, 0xCC, 0xC1, 0x2C, 0xC1, 0x1C,
|
||||
0xCC, 0xC1, 0x17, 0x7C, 0xC1, 0x17, 0x7C, 0xC1, 0x17, 0x7C, 0xC1, 0x12,
|
||||
0x2C, 0xC1, 0x12, 0x2C, 0xC1, 0x12, 0x2C, 0xC1, 0x12, 0x2C, 0xC1, 0xC1,
|
||||
0x12, 0x1C, 0x1C, 0x2C, 0xC1, 0xC1, 0xC1, 0xC1, 0xC1, 0xC1, 0xC1, 0x71,
|
||||
0xC1, 0xC1, 0xC1, 0x71, 0xC1, 0x71, 0xC1, 0x71, 0xC1, 0x71, 0x71, 0xC1,
|
||||
0x71, 0x71, 0xC1, 0x71, 0x71, 0x71, 0x71, 0x71, 0x71, 0x71, 0x71, 0x71,
|
||||
0x71, 0x71, 0x71, 0x71, 0x71, 0x71, 0x71, 0x71, 0x21, 0x71, 0x71, 0x71,
|
||||
0x21, 0x71, 0x71, 0x21, 0x71, 0x71, 0x21, 0x71, 0x71, 0x21, 0x71, 0x21,
|
||||
0x71, 0x21, 0x71, 0x21, 0x71, 0x21, 0x71, 0x21, 0x71, 0x21, 0x21, 0x71,
|
||||
0x21, 0x21, 0x71, 0x21, 0x21, 0x71, 0x21, 0x21, 0x71, 0x21, 0x21, 0x21,
|
||||
0x71, 0x21, 0x21, 0x21, 0x71, 0x21, 0x21, 0x21, 0x21, 0x21, 0x71, 0x21,
|
||||
0x21, 0x21, 0x21, 0x21, 0x21, 0x21, 0x21, 0x21, 0x71, 0x21, 0x21, 0x21,
|
||||
0x21, 0x21, 0x21, 0x21, 0x21, 0x21, 0x21, 0x21, 0x21, 0x21, 0x21, 0x21,
|
||||
0x21, 0x26, 0x21, 0x21, 0x21, 0x21, 0x21, 0x21, 0x21, 0x21, 0x26, 0x21,
|
||||
0x21, 0x21, 0x21, 0x21, 0x26, 0x21, 0x21, 0x21, 0x21, 0x26, 0x21, 0x21,
|
||||
0x21, 0x26, 0x21, 0x21, 0x26, 0x21, 0x21, 0x26, 0x21, 0x26, 0x21, 0x21,
|
||||
0x26, 0x21, 0x26, 0x21, 0x26, 0x21, 0x26, 0x21, 0x26, 0x21, 0x26, 0x21,
|
||||
0x26, 0x21, 0x26, 0x26, 0x21, 0x26, 0x26, 0x21, 0x26, 0x26, 0x26, 0x26,
|
||||
0x21, 0x26, 0x26, 0x26, 0x26, 0x26, 0x26, 0x26, 0x26, 0x26, 0x26, 0x26,
|
||||
0x26, 0x26, 0x26, 0x26, 0x26, 0x2B, 0x26, 0x26, 0x2B, 0x26, 0x26, 0x2B,
|
||||
0x26, 0x2B, 0x26, 0x2B, 0x2B, 0x26, 0x2B, 0x2B, 0x2B, 0x2B, 0x2B, 0x2B,
|
||||
0x2B, 0x2B, 0x2B, 0x2B, 0x1B, 0xB2, 0xB2, 0x21, 0x2B, 0x1B, 0xB2, 0x21,
|
||||
0x1B, 0xB2, 0x21, 0x1B, 0xB2, 0x21, 0x1B, 0xB2, 0x26, 0x6B, 0xB2, 0x26,
|
||||
0x6B, 0xB2, 0x26, 0xBB, 0xB2, 0x2B, 0xBB, 0xB2, 0x2B, 0xBB, 0x21, 0xBB,
|
||||
0x26, 0xBB, 0x26, 0xBB, 0x2B, 0x1F, 0x21, 0x1F, 0x21, 0x5F, 0x21, 0xAF,
|
||||
0x21, 0xFF, 0xF9, 0x9B, 0xF0, 0x1A, 0xF0, 0x15, 0xF0, 0x11, 0xF0, 0x10,
|
||||
0xF0, 0x10, 0xB0, 0x6B, 0xB0, 0x1B, 0xB0, 0x1B, 0xB0, 0x1B, 0xB0, 0x0B,
|
||||
0xBB, 0xB0, 0x06, 0xBB, 0xB0, 0x06, 0x6B, 0xB0, 0x01, 0x6B, 0xB0, 0x01,
|
||||
0x1B, 0xB0, 0x01, 0x1B, 0xB0, 0x01, 0x1B, 0xB0, 0x01, 0x0B, 0x1B, 0xB0,
|
||||
0xB0, 0xB0, 0x01, 0x0B, 0x0B, 0x0B, 0x0B, 0x0B, 0x0B, 0x06, 0x0B, 0x0B,
|
||||
0x0B, 0x06, 0x0B, 0x06, 0x0B, 0x06, 0x0B, 0x06, 0x06, 0x0B, 0x06, 0x06,
|
||||
0x06, 0x06, 0x0B, 0x06, 0x06, 0x06, 0x06, 0x06, 0x06, 0x06, 0x06, 0x06,
|
||||
0x01, 0x06, 0x06, 0x06, 0x06, 0x01, 0x06, 0x06, 0x06, 0x01, 0x06, 0x06,
|
||||
0x01, 0x06, 0x01, 0x06, 0x06, 0x01, 0x06, 0x01, 0x06, 0x01, 0x06, 0x01,
|
||||
0x06, 0x01, 0x01, 0x06, 0x01, 0x06, 0x01, 0x01, 0x06, 0x01, 0x01, 0x06,
|
||||
0x01, 0x01, 0x01, 0x06, 0x01, 0x01, 0x01, 0x06, 0x01, 0x01, 0x01, 0x01,
|
||||
0x01, 0x06, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x06, 0x01, 0x01,
|
||||
0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01,
|
||||
0x01, 0x01, 0x01, 0x51, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01,
|
||||
0x51, 0x01, 0x01, 0x01, 0x01, 0x51, 0x01, 0x01, 0x01, 0x01, 0x51, 0x01,
|
||||
0x01, 0x51, 0x01, 0x01, 0x51, 0x01, 0x01, 0x51, 0x01, 0x01, 0x51, 0x01,
|
||||
0x51, 0x01, 0x51, 0x01, 0x51, 0x01, 0x51, 0x01, 0x51, 0x01, 0x51, 0x01,
|
||||
0x51, 0x51, 0x01, 0x51, 0x51, 0x01, 0x51, 0x51, 0x51, 0x51, 0x01, 0x51,
|
||||
0x51, 0x51, 0x51, 0x51, 0x51, 0x51, 0x51, 0x51, 0x51, 0x51, 0x51, 0x51,
|
||||
0x51, 0xA1, 0x51, 0x51, 0x51, 0xA1, 0x51, 0xA1, 0x51, 0xA1, 0x51, 0xA1,
|
||||
0x51, 0xA1, 0xA1, 0xA1, 0xA1, 0x51, 0xA1, 0xA1, 0xA1, 0x10, 0x1A, 0x1A,
|
||||
0x1A, 0x0A, 0xA1, 0xA1, 0x10, 0x1A, 0x0A, 0xA1, 0x10, 0x0A, 0xA1, 0x10,
|
||||
0x0A, 0xA1, 0x15, 0x0A, 0xA1, 0x15, 0x5A, 0xA1, 0x15, 0xAA, 0xA1, 0x15,
|
||||
0xAA, 0xA1, 0x0A, 0xA1, 0x1A, 0xAA, 0x15, 0xAA, 0x15, 0xAA, 0x15, 0x0F,
|
||||
0x10, 0x1F, 0x10, 0x3F, 0x10, 0x8F, 0x10, 0xFF, 0xA8, 0x00 };
|
||||
#endif
|
||||
|
||||
|
||||
#if defined(PPRZ_TRIG_INT_COMPR_FLASH)
|
||||
|
||||
#if defined(PPRZ_TRIG_INT_COMPR_HIGHEST)
|
||||
uint8_t data_buf_4[TRIG_INT_SIZE/2];
|
||||
uint16_t tree_buf_4[(1 << TREE_SIZE_4)];
|
||||
|
||||
void table_encode_4(int16_t val, int16_t val_prev, int16_t cnt, int16_t* tab) {
|
||||
|
||||
/* build 4 bit table */
|
||||
if ((cnt % 2) == 0)
|
||||
data_buf_4[cnt/2] = val % 0x10;
|
||||
else
|
||||
data_buf_4[cnt/2] |= (val % 0x10) << 4;
|
||||
/* build 4 bit tree */
|
||||
if (cnt == 0)
|
||||
tree_buf_4[(*tab)++] = cnt;
|
||||
else if ((val / 0x10) != (val_prev / 0x10))
|
||||
tree_buf_4[(*tab)++] = cnt;
|
||||
}
|
||||
|
||||
int16_t pprz_trig_int_4(int16_t val) {
|
||||
int8_t j;
|
||||
int16_t k;
|
||||
|
||||
/* search tree for the 4 bit data */
|
||||
k = 1 << (TREE_SIZE_4-1);
|
||||
for (j = TREE_SIZE_4-2; j >= 0; j--) {
|
||||
if (val < tree_buf_4[k])
|
||||
k -= (1 << j);
|
||||
else
|
||||
k += (1 << j);
|
||||
}
|
||||
if (val < tree_buf_4[k])
|
||||
k -= 1;
|
||||
|
||||
return (k << 4) | ((data_buf_4[val/2] >> ((val % 2)?4:0)) & 0xF);
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined(PPRZ_TRIG_INT_COMPR_HIGH)
|
||||
uint8_t data_buf_8[TRIG_INT_SIZE];
|
||||
uint16_t tree_buf_8[(1 << TREE_SIZE_8)];
|
||||
|
||||
void table_encode_8(int16_t val, int16_t val_prev, int16_t cnt, int16_t* tab) {
|
||||
|
||||
/* build 8 bit table */
|
||||
data_buf_8[cnt] = val % 0x100;
|
||||
/* build tree for 8 bit */
|
||||
if (cnt == 0)
|
||||
tree_buf_8[(*tab)++] = cnt;
|
||||
else if ((val / 0x100) != (val_prev / 0x100))
|
||||
tree_buf_8[(*tab)++] = cnt;
|
||||
}
|
||||
|
||||
int16_t pprz_trig_int_8(int16_t val) {
|
||||
int8_t j, k;
|
||||
|
||||
/* search the tree for 8 bit data */
|
||||
k = 1 << (TREE_SIZE_8-1);
|
||||
for (j = TREE_SIZE_8-2; j >= 0; j--) {
|
||||
if (val < tree_buf_8[k])
|
||||
k -= (1 << j);
|
||||
else
|
||||
k += (1 << j);
|
||||
}
|
||||
if (val < tree_buf_8[k])
|
||||
k -= 1;
|
||||
|
||||
return (k << 8) | data_buf_8[val];
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined(PPRZ_TRIG_INT_COMPR_LOW)
|
||||
uint8_t data_buf_12[(TRIG_INT_SIZE*3)/2];
|
||||
#if USE_REAL_TABLE_12_USE
|
||||
uint16_t tree_buf_12[(1 << TREE_SIZE_12)];
|
||||
#endif
|
||||
|
||||
void table_encode_12(int16_t val, int16_t val_prev, int16_t cnt, int16_t* tab) {
|
||||
|
||||
/* build 12 bit table */
|
||||
data_buf_12[cnt] = val % 0x100;
|
||||
if ((cnt % 2) == 0)
|
||||
data_buf_12[TRIG_INT_SIZE+(cnt/2)] = (val & 0xF00) >> 8 ;
|
||||
else
|
||||
data_buf_12[TRIG_INT_SIZE+(cnt/2)] |= (val & 0xF00) >> 4;
|
||||
#if USE_REAL_TABLE_12_USE
|
||||
/* build tree for 12 bit */
|
||||
if (cnt == 0)
|
||||
tree_buf_12[(*tab)++] = cnt;
|
||||
else if ((val / 0x1000) != (val_prev / 0x1000))
|
||||
tree_buf_12[(*tab)++] = cnt;
|
||||
#endif
|
||||
}
|
||||
|
||||
int16_t pprz_trig_int_12(int16_t val) {
|
||||
#if USE_REAL_TABLE_12_USE
|
||||
int8_t k;
|
||||
|
||||
/* search tree for the 12 bit data */
|
||||
k = 1 << (TREE_SIZE_12-1);
|
||||
if (val < tree_buf_12[k])
|
||||
k -= 1;
|
||||
else
|
||||
k += 1;
|
||||
if (val < tree_buf_12[k])
|
||||
k -= 1;
|
||||
return (k << 12) | ((data_buf_12[TRIG_INT_SIZE+(val/2)] << ((val % 2)?4:8)) & 0xF00) | (data_buf_12[val]);
|
||||
#else
|
||||
int16_t k;
|
||||
|
||||
/* search tree for the 12 bit data */
|
||||
if (val < TREE_BUF_12_2) {
|
||||
if (val < TREE_BUF_12_1)
|
||||
k = (0 << 12);
|
||||
else
|
||||
k = (1 << 12);
|
||||
} else {
|
||||
if (val < TREE_BUF_12_3)
|
||||
k = (2 << 12);
|
||||
else
|
||||
k = (3 << 12);
|
||||
}
|
||||
|
||||
return k | ((data_buf_12[TRIG_INT_SIZE+(val >> 1)] << ((val % 2)?4:8)) & 0xF00) | (data_buf_12[val]);
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined(PPRZ_TRIG_INT_COMPR_NONE)
|
||||
int16_t data_buf_16[TRIG_INT_SIZE];
|
||||
|
||||
inline int16_t pprz_trig_int_16(int32_t val) {
|
||||
return data_buf_16[val];
|
||||
}
|
||||
|
||||
void table_encode_16(int16_t val, int16_t cnt) {
|
||||
data_buf_16[cnt] = val;
|
||||
}
|
||||
#endif
|
||||
|
||||
uint8_t get_nibble(uint16_t pos) {
|
||||
return (pprz_trig_int_compr[pos/2] >> (4 * (pos % 2))) & 0xF;
|
||||
}
|
||||
|
||||
int pprz_trig_int_init(void)
|
||||
{
|
||||
int16_t i, j, k, cnt, temp, val, val_prev;
|
||||
|
||||
#if defined(PPRZ_TRIG_INT_COMPR_HIGHEST)
|
||||
int16_t tab_4 = 0;
|
||||
#endif
|
||||
#if defined(PPRZ_TRIG_INT_COMPR_HIGH)
|
||||
int16_t tab_8 = 0;
|
||||
#endif
|
||||
#if defined(PPRZ_TRIG_INT_COMPR_LOW)
|
||||
int16_t tab_12 = 0;
|
||||
#endif
|
||||
|
||||
cnt = 0;
|
||||
val = 0;
|
||||
val_prev = 0;
|
||||
/* decode run length encoding */
|
||||
for (i = 0; i < TRIG_INT_RLE_LEN; ) {
|
||||
temp = get_nibble(i++);
|
||||
if (temp < 0xF) {
|
||||
/* single */
|
||||
for (j = 0; j < (temp / 5) + 1; j++) {
|
||||
#if defined(PPRZ_TRIG_INT_COMPR_HIGHEST)
|
||||
table_encode_4(val, val_prev, cnt, &tab_4);
|
||||
#endif
|
||||
#if defined(PPRZ_TRIG_INT_COMPR_HIGH)
|
||||
table_encode_8(val, val_prev, cnt, &tab_8);
|
||||
#endif
|
||||
#if defined(PPRZ_TRIG_INT_COMPR_LOW)
|
||||
table_encode_12(val, val_prev, cnt, &tab_12);
|
||||
#endif
|
||||
#if defined(PPRZ_TRIG_INT_COMPR_NONE)
|
||||
table_encode_16(val, cnt);
|
||||
#endif
|
||||
val_prev = val;
|
||||
cnt++;
|
||||
val += temp % 5;
|
||||
if (cnt > TRIG_INT_SIZE)
|
||||
return -1;
|
||||
}
|
||||
} else {
|
||||
/* multi */
|
||||
k = get_nibble(i++);
|
||||
if (i > TRIG_INT_RLE_LEN)
|
||||
return -1;
|
||||
temp = get_nibble(i++);
|
||||
if (i > TRIG_INT_RLE_LEN)
|
||||
return -1;
|
||||
k |= (temp & 0x8) << 1;
|
||||
for (j = 0; j < k + ONE_MIN; j++) {
|
||||
#if defined(PPRZ_TRIG_INT_COMPR_HIGHEST)
|
||||
table_encode_4(val, val_prev, cnt, &tab_4);
|
||||
#endif
|
||||
#if defined(PPRZ_TRIG_INT_COMPR_HIGH)
|
||||
table_encode_8(val, val_prev, cnt, &tab_8);
|
||||
#endif
|
||||
#if defined(PPRZ_TRIG_INT_COMPR_LOW)
|
||||
table_encode_12(val, val_prev, cnt, &tab_12);
|
||||
#endif
|
||||
#if defined(PPRZ_TRIG_INT_COMPR_NONE)
|
||||
table_encode_16(val, cnt);
|
||||
#endif
|
||||
val_prev = val;
|
||||
cnt++;
|
||||
val += temp & 7;
|
||||
if (cnt > TRIG_INT_SIZE)
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (cnt != TRIG_INT_SIZE)
|
||||
return -1;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
inline int16_t pprz_trig_int_f(int32_t angle)
|
||||
{
|
||||
#if defined(PPRZ_TRIG_INT_COMPR_HIGHEST)
|
||||
return pprz_trig_int_4(angle);
|
||||
#elif defined(PPRZ_TRIG_INT_COMPR_HIGH)
|
||||
return pprz_trig_int_8(angle);
|
||||
#elif defined(PPRZ_TRIG_INT_COMPR_LOW)
|
||||
return pprz_trig_int_12(angle);
|
||||
#elif defined(PPRZ_TRIG_INT_COMPR_NONE)
|
||||
return pprz_trig_int_16(angle);
|
||||
#endif
|
||||
}
|
||||
|
||||
#endif // PPRZ_TRIG_INT_COMPR_FLASH
|
||||
|
||||
int32_t pprz_itrig_sin(int32_t angle)
|
||||
{
|
||||
@@ -443,9 +828,15 @@ int32_t pprz_itrig_sin(int32_t angle)
|
||||
angle = -INT32_ANGLE_PI - angle;
|
||||
}
|
||||
if (angle >= 0) {
|
||||
#if defined(PPRZ_TRIG_INT_COMPR_FLASH)
|
||||
return pprz_trig_int_f(angle);
|
||||
} else {
|
||||
return -pprz_trig_int_f(-angle);
|
||||
#else
|
||||
return pprz_trig_int[angle];
|
||||
} else {
|
||||
return -pprz_trig_int[-angle];
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -40,13 +40,71 @@ extern "C" {
|
||||
#define PPRZ_TRIG_CONST
|
||||
#endif
|
||||
|
||||
#if defined(PPRZ_TRIG_INT_TEST)
|
||||
#define PPRZ_TRIG_INT_COMPR_FLASH
|
||||
#define PPRZ_TRIG_INT_COMPR_HIGHEST
|
||||
#define PPRZ_TRIG_INT_COMPR_HIGH
|
||||
#define PPRZ_TRIG_INT_COMPR_LOW
|
||||
#define PPRZ_TRIG_INT_COMPR_NONE
|
||||
#endif
|
||||
|
||||
#if defined(PPRZ_TRIG_INT_COMPR_FLASH) && !defined(PPRZ_TRIG_INT_COMPR_HIGHEST) && !defined(PPRZ_TRIG_INT_COMPR_HIGH) && !defined(PPRZ_TRIG_INT_COMPR_LOW)
|
||||
#define PPRZ_TRIG_INT_COMPR_NONE
|
||||
#endif
|
||||
|
||||
#define TRIG_INT_SIZE 6434
|
||||
#define TRIG_INT_VAL_MAX 14
|
||||
#define TREE_SIZE_4 (TRIG_INT_VAL_MAX - 4)
|
||||
#define TREE_SIZE_8 (TRIG_INT_VAL_MAX - 8)
|
||||
#define TREE_SIZE_12 (TRIG_INT_VAL_MAX - 12)
|
||||
|
||||
/* resulting compressed size */
|
||||
#define TRIG_INT_RLE_LEN 3379
|
||||
#define TRIG_INT_COMPR_LEN ((TRIG_INT_RLE_LEN + 1) / 2)
|
||||
|
||||
/* minimum size to use multi */
|
||||
#define ONE_MIN ((3 * 3) + 1)
|
||||
|
||||
#define TREE_BUF_12_1 1035
|
||||
#define TREE_BUF_12_2 2145
|
||||
#define TREE_BUF_12_3 3474
|
||||
|
||||
#if !defined(PPRZ_TRIG_INT_COMPR_FLASH) || defined(PPRZ_TRIG_INT_TEST)
|
||||
extern PPRZ_TRIG_CONST int16_t pprz_trig_int[];
|
||||
#endif
|
||||
|
||||
extern int32_t pprz_itrig_sin(int32_t angle);
|
||||
extern int32_t pprz_itrig_cos(int32_t angle);
|
||||
extern int32_t int32_atan2(int32_t y, int32_t x);
|
||||
extern int32_t int32_atan2_2(int32_t y, int32_t x);
|
||||
|
||||
#if defined(PPRZ_TRIG_INT_COMPR_FLASH)
|
||||
uint8_t get_nibble(uint16_t pos);
|
||||
int pprz_trig_int_init(void);
|
||||
|
||||
#if defined(PPRZ_TRIG_INT_COMPR_HIGHEST)
|
||||
void table_encode_4(int16_t val, int16_t val_prev, int16_t cnt, int16_t* tab);
|
||||
int16_t pprz_trig_int_4(int16_t val);
|
||||
#endif
|
||||
|
||||
#if defined(PPRZ_TRIG_INT_COMPR_HIGH)
|
||||
void table_encode_8(int16_t val, int16_t val_prev, int16_t cnt, int16_t* tab);
|
||||
int16_t pprz_trig_int_8(int16_t val);
|
||||
#endif
|
||||
|
||||
#if defined(PPRZ_TRIG_INT_COMPR_LOW)
|
||||
void table_encode_12(int16_t val, int16_t val_prev, int16_t cnt, int16_t* tab);
|
||||
int16_t pprz_trig_int_12(int16_t val);
|
||||
#endif
|
||||
|
||||
#if defined(PPRZ_TRIG_INT_COMPR_NONE)
|
||||
int16_t pprz_trig_int_16(int32_t val);
|
||||
void table_encode_16(int16_t val, int16_t cnt);
|
||||
#endif
|
||||
|
||||
int16_t pprz_trig_int_f(int32_t angle);
|
||||
#endif // PPRZ_TRIG_INT_COMPR_FLASH
|
||||
|
||||
/* for backwards compatibility */
|
||||
#define PPRZ_ITRIG_SIN(_s, _a) { _s = pprz_itrig_sin(_a); }
|
||||
#define PPRZ_ITRIG_COS(_c, _a) { _c = pprz_itrig_cos(_a); }
|
||||
|
||||
@@ -0,0 +1,294 @@
|
||||
/*
|
||||
* Copyright (C) 2015 Martin Mueller <martinmm@pfump.org>
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file test_math_trig_compressed.c
|
||||
*
|
||||
* Run flash / RAM compressed sine look up table
|
||||
*/
|
||||
|
||||
/*
|
||||
|
||||
simple 4-bit run length encoding for sine table storage in flash
|
||||
reduces flash usage from 12868 bytes to 1690 bytes
|
||||
|
||||
PPRZ_TRIG_INT_COMPR_FLASH
|
||||
|
||||
0 0000: 0
|
||||
1 0001: 1
|
||||
2 0010: 2
|
||||
3 0011: 3
|
||||
4 0100: 4
|
||||
5 0101: 00
|
||||
6 0110: 11
|
||||
7 0111: 22
|
||||
8 1000: 33
|
||||
9 1001: 44
|
||||
10 1010: 000
|
||||
11 1011: 111
|
||||
12 1100: 222
|
||||
13 1101: 333
|
||||
14 1110: 444
|
||||
15 1111 nnnn nyyy: ((n+10) * y), n=00000..11111, y=0000..0100
|
||||
|
||||
|
||||
binary trees for sine table storage in RAM
|
||||
reduces RAM usage from 12868 bytes to
|
||||
|
||||
PPRZ_TRIG_INT_COMPR_HIGHEST
|
||||
4 bit data + 10 bit tree 5265 bytes (6434 * 4 + 1024 * 16 bits)
|
||||
PPRZ_TRIG_INT_COMPR_HIGH
|
||||
8 bit data + 6 bit tree 6562 bytes (6434 * 8 + 64 * 16 bits)
|
||||
PPRZ_TRIG_INT_COMPR_LOW
|
||||
12 bit data 9647 bytes (6434 * 12)
|
||||
|
||||
code size Cortex-M4 thumb2
|
||||
|
||||
PPRZ_TRIG_INT_COMPR_HIGHEST 104 bytes
|
||||
PPRZ_TRIG_INT_COMPR_HIGH 76 bytes
|
||||
PPRZ_TRIG_INT_COMPR_LOW 84 bytes
|
||||
- 12 bytes
|
||||
|
||||
machine cycles STM32F4 (average over 10 calls)
|
||||
|
||||
PPRZ_TRIG_INT_COMPR_HIGHEST 197 cycles
|
||||
PPRZ_TRIG_INT_COMPR_HIGH 170 cycles
|
||||
PPRZ_TRIG_INT_COMPR_LOW 46 cycles
|
||||
- 16 cycles
|
||||
|
||||
*/
|
||||
|
||||
#define DATALINK_C
|
||||
|
||||
#include BOARD_CONFIG
|
||||
#include "mcu.h"
|
||||
#include "mcu_periph/sys_time.h"
|
||||
#include "subsystems/datalink/downlink.h"
|
||||
#include "led.h"
|
||||
#include "math/pprz_trig_int.h"
|
||||
#include "math/pprz_algebra_int.h"
|
||||
|
||||
/* cycle counter only exists for STM32 architecture */
|
||||
#if defined(STM32F1) || defined(STM32F4)
|
||||
#include <libopencm3/cm3/dwt.h>
|
||||
#else
|
||||
#define dwt_read_cycle_counter() 0
|
||||
#define dwt_enable_cycle_counter() 0
|
||||
#endif
|
||||
|
||||
static inline void main_init(void);
|
||||
static inline void main_periodic(void);
|
||||
static inline void main_event(void);
|
||||
|
||||
int test_tables(void);
|
||||
|
||||
int32_t pprz_itrig_sin_4(int32_t angle);
|
||||
int32_t pprz_itrig_sin_8(int32_t angle);
|
||||
int32_t pprz_itrig_sin_12(int32_t angle);
|
||||
int32_t pprz_itrig_sin_16(int32_t angle);
|
||||
|
||||
int test_tables(void)
|
||||
{
|
||||
int16_t i;
|
||||
|
||||
/* test all functions */
|
||||
for (i = 0; i < TRIG_INT_SIZE; i++) {
|
||||
if (pprz_trig_int[i] != pprz_trig_int_4(i)) return -1;
|
||||
if (pprz_trig_int[i] != pprz_trig_int_8(i)) return -1;
|
||||
if (pprz_trig_int[i] != pprz_trig_int_12(i)) return -1;
|
||||
if (pprz_trig_int[i] != pprz_trig_int_16(i)) return -1;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
#ifdef TRIG_INT_COMPR_4
|
||||
int32_t pprz_itrig_sin_4(int32_t angle)
|
||||
{
|
||||
INT32_ANGLE_NORMALIZE(angle);
|
||||
if (angle > INT32_ANGLE_PI_2) {
|
||||
angle = INT32_ANGLE_PI - angle;
|
||||
} else if (angle < -INT32_ANGLE_PI_2) {
|
||||
angle = -INT32_ANGLE_PI - angle;
|
||||
}
|
||||
if (angle >= 0) {
|
||||
return pprz_trig_int_4(angle);
|
||||
} else {
|
||||
return -pprz_trig_int_4(-angle);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef TRIG_INT_COMPR_8
|
||||
int32_t pprz_itrig_sin_8(int32_t angle)
|
||||
{
|
||||
INT32_ANGLE_NORMALIZE(angle);
|
||||
if (angle > INT32_ANGLE_PI_2) {
|
||||
angle = INT32_ANGLE_PI - angle;
|
||||
} else if (angle < -INT32_ANGLE_PI_2) {
|
||||
angle = -INT32_ANGLE_PI - angle;
|
||||
}
|
||||
if (angle >= 0) {
|
||||
return pprz_trig_int_8(angle);
|
||||
} else {
|
||||
return -pprz_trig_int_8(-angle);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef TRIG_INT_COMPR_12
|
||||
int32_t pprz_itrig_sin_12(int32_t angle)
|
||||
{
|
||||
INT32_ANGLE_NORMALIZE(angle);
|
||||
if (angle > INT32_ANGLE_PI_2) {
|
||||
angle = INT32_ANGLE_PI - angle;
|
||||
} else if (angle < -INT32_ANGLE_PI_2) {
|
||||
angle = -INT32_ANGLE_PI - angle;
|
||||
}
|
||||
if (angle >= 0) {
|
||||
return pprz_trig_int_12(angle);
|
||||
} else {
|
||||
return -pprz_trig_int_12(-angle);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
int32_t pprz_itrig_sin_16(int32_t angle)
|
||||
{
|
||||
INT32_ANGLE_NORMALIZE(angle);
|
||||
if (angle > INT32_ANGLE_PI_2) {
|
||||
angle = INT32_ANGLE_PI - angle;
|
||||
} else if (angle < -INT32_ANGLE_PI_2) {
|
||||
angle = -INT32_ANGLE_PI - angle;
|
||||
}
|
||||
if (angle >= 0) {
|
||||
return pprz_trig_int_16(angle);
|
||||
} else {
|
||||
return -pprz_trig_int_16(-angle);
|
||||
}
|
||||
}
|
||||
|
||||
int main(void)
|
||||
{
|
||||
main_init();
|
||||
|
||||
dwt_enable_cycle_counter();
|
||||
|
||||
while (1) {
|
||||
if (sys_time_check_and_ack_timer(0)) {
|
||||
main_periodic();
|
||||
}
|
||||
main_event();
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
static inline void main_init(void)
|
||||
{
|
||||
mcu_init();
|
||||
sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL);
|
||||
mcu_int_enable();
|
||||
|
||||
downlink_init();
|
||||
}
|
||||
|
||||
static inline void main_periodic(void)
|
||||
{
|
||||
volatile uint32_t result;
|
||||
volatile uint32_t pre_time, post_time;
|
||||
uint32_t result1=0, result2=0, result3=0, result4=0;
|
||||
static int16_t i = 2342;
|
||||
|
||||
i += TRIG_INT_SIZE / 4;
|
||||
i = i % TRIG_INT_SIZE;
|
||||
|
||||
RunOnceEvery(10, {DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);});
|
||||
LED_PERIODIC();
|
||||
|
||||
pprz_trig_int_init();
|
||||
if (test_tables() == 0) {
|
||||
|
||||
/* run 10x without a loop */
|
||||
pre_time = dwt_read_cycle_counter();
|
||||
result = pprz_trig_int_4(i);
|
||||
result = pprz_trig_int_4(i);
|
||||
result = pprz_trig_int_4(i);
|
||||
result = pprz_trig_int_4(i);
|
||||
result = pprz_trig_int_4(i);
|
||||
result = pprz_trig_int_4(i);
|
||||
result = pprz_trig_int_4(i);
|
||||
result = pprz_trig_int_4(i);
|
||||
result = pprz_trig_int_4(i);
|
||||
result = pprz_trig_int_4(i);
|
||||
post_time = dwt_read_cycle_counter();
|
||||
result1 = (post_time - pre_time);
|
||||
|
||||
pre_time = dwt_read_cycle_counter();
|
||||
result = pprz_trig_int_8(i);
|
||||
result = pprz_trig_int_8(i);
|
||||
result = pprz_trig_int_8(i);
|
||||
result = pprz_trig_int_8(i);
|
||||
result = pprz_trig_int_8(i);
|
||||
result = pprz_trig_int_8(i);
|
||||
result = pprz_trig_int_8(i);
|
||||
result = pprz_trig_int_8(i);
|
||||
result = pprz_trig_int_8(i);
|
||||
result = pprz_trig_int_8(i);
|
||||
post_time = dwt_read_cycle_counter();
|
||||
result2 = (post_time - pre_time);
|
||||
|
||||
pre_time = dwt_read_cycle_counter();
|
||||
result = pprz_trig_int_12(i);
|
||||
result = pprz_trig_int_12(i);
|
||||
result = pprz_trig_int_12(i);
|
||||
result = pprz_trig_int_12(i);
|
||||
result = pprz_trig_int_12(i);
|
||||
result = pprz_trig_int_12(i);
|
||||
result = pprz_trig_int_12(i);
|
||||
result = pprz_trig_int_12(i);
|
||||
result = pprz_trig_int_12(i);
|
||||
result = pprz_trig_int_12(i);
|
||||
post_time = dwt_read_cycle_counter();
|
||||
result3 = (post_time - pre_time);
|
||||
|
||||
pre_time = dwt_read_cycle_counter();
|
||||
result = pprz_trig_int_16(i);
|
||||
result = pprz_trig_int_16(i);
|
||||
result = pprz_trig_int_16(i);
|
||||
result = pprz_trig_int_16(i);
|
||||
result = pprz_trig_int_16(i);
|
||||
result = pprz_trig_int_16(i);
|
||||
result = pprz_trig_int_16(i);
|
||||
result = pprz_trig_int_16(i);
|
||||
result = pprz_trig_int_16(i);
|
||||
result = pprz_trig_int_16(i);
|
||||
post_time = dwt_read_cycle_counter();
|
||||
result4 = (post_time - pre_time);
|
||||
|
||||
result = result;
|
||||
|
||||
DOWNLINK_SEND_CSC_CAN_MSG(DefaultChannel, DefaultDevice, &result1, &result2, &result3, &result4);
|
||||
}
|
||||
}
|
||||
|
||||
static inline void main_event(void)
|
||||
{
|
||||
mcu_event();
|
||||
}
|
||||
Reference in New Issue
Block a user