diff --git a/conf/firmwares/test_progs.makefile b/conf/firmwares/test_progs.makefile index b4e44bad19..6a346a0b23 100644 --- a/conf/firmwares/test_progs.makefile +++ b/conf/firmwares/test_progs.makefile @@ -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 diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c index 02c7a9b1d4..9fad32cb3a 100644 --- a/sw/airborne/firmwares/rotorcraft/main.c +++ b/sw/airborne/firmwares/rotorcraft/main.c @@ -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(); diff --git a/sw/airborne/math/pprz_trig_int.c b/sw/airborne/math/pprz_trig_int.c index fcb683714c..94683db577 100644 --- a/sw/airborne/math/pprz_trig_int.c +++ b/sw/airborne/math/pprz_trig_int.c @@ -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 } } diff --git a/sw/airborne/math/pprz_trig_int.h b/sw/airborne/math/pprz_trig_int.h index 6c5064e585..435c1a4781 100644 --- a/sw/airborne/math/pprz_trig_int.h +++ b/sw/airborne/math/pprz_trig_int.h @@ -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); } diff --git a/sw/airborne/test/test_math_trig_compressed.c b/sw/airborne/test/test_math_trig_compressed.c new file mode 100644 index 0000000000..485440dbdf --- /dev/null +++ b/sw/airborne/test/test_math_trig_compressed.c @@ -0,0 +1,294 @@ +/* + * Copyright (C) 2015 Martin Mueller + * + * 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 +#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(); +}