diff --git a/conf/airframes/ENAC/fixed-wing/funjet2.xml b/conf/airframes/ENAC/fixed-wing/funjet2.xml index 29741b2e4f..d5f3c9b5ac 100644 --- a/conf/airframes/ENAC/fixed-wing/funjet2.xml +++ b/conf/airframes/ENAC/fixed-wing/funjet2.xml @@ -10,6 +10,10 @@ + + + + diff --git a/conf/airframes/flixr/conf.xml b/conf/airframes/flixr/conf.xml index 07e0ee6266..c3054eef5d 100644 --- a/conf/airframes/flixr/conf.xml +++ b/conf/airframes/flixr/conf.xml @@ -18,19 +18,19 @@ telemetry="telemetry/default_rotorcraft.xml" flight_plan="flight_plans/rotorcraft_basic.xml" settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/estimation/ahrs_float_mlkf.xml settings/control/stabilization_att_int_quat.xml" - settings_modules="modules/geo_mag.xml modules/air_data.xml modules/video_thread.xml modules/video_rtp_stream.xml" gui_color="blue" + settings_modules="modules/geo_mag.xml modules/air_data.xml modules/video_thread.xml modules/video_rtp_stream.xml" /> + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + +
+ + + + + + + + + + + +
+ + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + +
+ + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + +
+ +
+ + + + + + +
+ +
+ + + +
+ +
+ + + + + +
+ + diff --git a/conf/airframes/untested/quad_cc3d.xml b/conf/airframes/untested/quad_cc3d.xml new file mode 100644 index 0000000000..9594b00f43 --- /dev/null +++ b/conf/airframes/untested/quad_cc3d.xml @@ -0,0 +1,216 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + +
+ + + + + + + + + + + + + + + + +
+ + + + + + + + + + +
+ +
+ + + +
+ +
+
+ +
+ + + + + + + + + + + + + + + + +
+ + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + +
+ +
+ + + + + + + +
+ +
+ + + + + +
+ +
+ + + +
+ +
+ + + + +
+ + diff --git a/conf/airframes/untested/quad_cjmcu.xml b/conf/airframes/untested/quad_cjmcu.xml new file mode 100644 index 0000000000..bef2b047fa --- /dev/null +++ b/conf/airframes/untested/quad_cjmcu.xml @@ -0,0 +1,233 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + +
+ +
+ + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + +
+ +
+ + + + + +
+ +
+ + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+
+ +
+ + + + +
+ +
+ + + +
+ +
+ + + +
+ +
+ + + +
+ + diff --git a/conf/airframes/untested/quad_flip32.xml b/conf/airframes/untested/quad_flip32.xml new file mode 100644 index 0000000000..95da23bfcb --- /dev/null +++ b/conf/airframes/untested/quad_flip32.xml @@ -0,0 +1,221 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + +
+ + + + + + + + + + + + + + + + +
+ + + + + + + + + + +
+ +
+ + + +
+ +
+
+ +
+ + + + + + + + + + + + + + + + +
+ + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + +
+ +
+ + + + + + + +
+ +
+ + + + + +
+ +
+ + + +
+ +
+ + + + +
+ + diff --git a/conf/boards/cjmcu.makefile b/conf/boards/cjmcu.makefile new file mode 100644 index 0000000000..e53b9414d1 --- /dev/null +++ b/conf/boards/cjmcu.makefile @@ -0,0 +1,69 @@ +# Hey Emacs, this is a -*- makefile -*- +# +# cjmcu.makefile +# +# https://github.com/cleanflight/cleanflight/issues/22 +# http://blog.oscarliang.net/build-fpv-micro-quadcopter-smallest-quad +# hw rev2 ? +# + +BOARD=cjmcu +BOARD_CFG=\"boards/$(BOARD).h\" + +ARCH=stm32 +$(TARGET).ARCHDIR = $(ARCH) +# not needed? +$(TARGET).OOCD_INTERFACE=flossjtag +$(TARGET).LDSCRIPT=$(SRC_ARCH)/cjmcu.ld + +# ----------------------------------------------------------------------- + +# default flash mode is via SWD +# other possibilities: DFU-UTIL, JTAG, SWD, STLINK, SERIAL +FLASH_MODE ?= SWD + +HAS_LUFTBOOT ?= 0 +ifeq (,$(findstring $(HAS_LUFTBOOT),0 FALSE)) +$(TARGET).CFLAGS+=-DLUFTBOOT +$(TARGET).LDFLAGS+=-Wl,-Ttext=0x8002000 +endif + +# +# +# some default values shared between different firmwares +# +# + + +# +# default LED configuration +# +RADIO_CONTROL_LED ?= none +BARO_LED ?= none +AHRS_ALIGNER_LED ?= 1 +GPS_LED ?= 2 +SYS_TIME_LED ?= 3 + + +# +# default uart configuration +# +RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT ?= UART3 +RADIO_CONTROL_SPEKTRUM_SECONDARY_PORT ?= UART1 + +MODEM_PORT ?= UART1 +MODEM_BAUD ?= B57600 + +GPS_PORT ?= UART3 +GPS_BAUD ?= B38400 + + +# +# default actuator configuration +# +# you can use different actuators by adding a configure option to your firmware section +# e.g. +# +ACTUATORS ?= actuators_pwm diff --git a/conf/boards/naze32_rev4.makefile b/conf/boards/naze32_rev4.makefile new file mode 100644 index 0000000000..370bcd71c9 --- /dev/null +++ b/conf/boards/naze32_rev4.makefile @@ -0,0 +1,60 @@ +# Hey Emacs, this is a -*- makefile -*- +# +# naze32.makefile +# +# https://code.google.com/p/afrodevices/wiki/AfroFlight32 +# hw rev4 +# + +BOARD=naze32 +BOARD_VERSION=rev4 +BOARD_CFG=\"boards/$(BOARD)_$(BOARD_VERSION).h\" + +ARCH=stm32 +$(TARGET).ARCHDIR = $(ARCH) +$(TARGET).LDSCRIPT=$(SRC_ARCH)/naze32.ld + +# ----------------------------------------------------------------------- + +# default flash mode is via SWD +# other possibilities: DFU-UTIL, JTAG, SWD, STLINK, SERIAL +FLASH_MODE ?= SWD + +# +# +# some default values shared between different firmwares +# +# + + +# +# default LED configuration +# +RADIO_CONTROL_LED ?= none +BARO_LED ?= none +AHRS_ALIGNER_LED ?= 2 +GPS_LED ?= none +SYS_TIME_LED ?= 1 + + +# +# default uart configuration +# +RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT ?= UART2 + +MODEM_PORT ?= UART1 +MODEM_BAUD ?= B57600 + +GPS_PORT ?= UART2 +GPS_BAUD ?= B38400 + + +# +# default actuator configuration +# +# you can use different actuators by adding a configure option to your firmware section +# e.g. +# +ACTUATORS ?= actuators_pwm diff --git a/conf/boards/naze32_rev5.makefile b/conf/boards/naze32_rev5.makefile new file mode 100644 index 0000000000..1a1d8fda2f --- /dev/null +++ b/conf/boards/naze32_rev5.makefile @@ -0,0 +1,60 @@ +# Hey Emacs, this is a -*- makefile -*- +# +# naze32.makefile +# +# https://code.google.com/p/afrodevices/wiki/AfroFlight32 +# hw rev5 +# + +BOARD=naze32 +BOARD_VERSION=rev5 +BOARD_CFG=\"boards/$(BOARD)_$(BOARD_VERSION).h\" + +ARCH=stm32 +$(TARGET).ARCHDIR = $(ARCH) +$(TARGET).LDSCRIPT=$(SRC_ARCH)/naze32.ld + +# ----------------------------------------------------------------------- + +# default flash mode is via SWD +# other possibilities: DFU-UTIL, JTAG, SWD, STLINK, SERIAL +FLASH_MODE ?= SWD + +# +# +# some default values shared between different firmwares +# +# + + +# +# default LED configuration +# +RADIO_CONTROL_LED ?= none +BARO_LED ?= none +AHRS_ALIGNER_LED ?= 2 +GPS_LED ?= none +SYS_TIME_LED ?= 1 + + +# +# default uart configuration +# +RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT ?= UART2 + +MODEM_PORT ?= UART1 +MODEM_BAUD ?= B57600 + +GPS_PORT ?= UART2 +GPS_BAUD ?= B38400 + + +# +# default actuator configuration +# +# you can use different actuators by adding a configure option to your firmware section +# e.g. +# +ACTUATORS ?= actuators_pwm diff --git a/conf/firmwares/subsystems/shared/baro_board.makefile b/conf/firmwares/subsystems/shared/baro_board.makefile index d9ed0a2f76..a6f9839cbd 100644 --- a/conf/firmwares/subsystems/shared/baro_board.makefile +++ b/conf/firmwares/subsystems/shared/baro_board.makefile @@ -189,6 +189,15 @@ else ifeq ($(BOARD), umarim) BARO_BOARD_SRCS += boards/umarim/baro_board.c endif +# Naze32 +else ifeq ($(BOARD), naze32) + BARO_BOARD_CFLAGS += -DBARO_BOARD=BARO_MS5611_I2C + BARO_BOARD_CFLAGS += -DUSE_I2C2 + BARO_BOARD_CFLAGS += -DBB_MS5611_I2C_DEV=i2c2 + BARO_BOARD_SRCS += peripherals/ms5611.c + BARO_BOARD_SRCS += peripherals/ms5611_i2c.c + BARO_BOARD_SRCS += boards/baro_board_ms5611_i2c.c + endif # check board ifneq ($(BARO_LED),none) diff --git a/conf/firmwares/subsystems/shared/imu_mpu60x0_i2c.makefile b/conf/firmwares/subsystems/shared/imu_mpu60x0_i2c.makefile index 8fc59d3662..5e6308072d 100644 --- a/conf/firmwares/subsystems/shared/imu_mpu60x0_i2c.makefile +++ b/conf/firmwares/subsystems/shared/imu_mpu60x0_i2c.makefile @@ -15,19 +15,6 @@ IMU_MPU60X0_SRCS += peripherals/mpu60x0.c IMU_MPU60X0_SRCS += peripherals/mpu60x0_i2c.c -# set default i2c bus -ifeq ($(ARCH), lpc21) -IMU_MPU60X0_I2C_DEV ?= i2c0 -else ifeq ($(ARCH), stm32) -IMU_MPU60X0_I2C_DEV ?= i2c2 -endif - -ifeq ($(TARGET), ap) -ifndef MPU60X0_I2C_DEV -$(error Error: MPU60X0_I2C_DEV not configured!) -endif -endif - # convert i2cx to upper/lower case IMU_MPU60X0_I2C_DEV_UPPER=$(shell echo $(IMU_MPU60X0_I2C_DEV) | tr a-z A-Z) IMU_MPU60X0_I2C_DEV_LOWER=$(shell echo $(IMU_MPU60X0_I2C_DEV) | tr A-Z a-z) @@ -38,6 +25,11 @@ IMU_MPU60X0_CFLAGS += -DUSE_$(IMU_MPU60X0_I2C_DEV_UPPER) # add it for all targets except sim, fbw and nps ifeq (,$(findstring $(TARGET),sim fbw nps)) + +ifndef IMU_MPU60X0_I2C_DEV +$(error Error: IMU_MPU60X0_I2C_DEV not configured!) +endif + $(TARGET).CFLAGS += $(IMU_MPU60X0_CFLAGS) $(TARGET).srcs += $(IMU_MPU60X0_SRCS) endif diff --git a/conf/firmwares/test_progs.makefile b/conf/firmwares/test_progs.makefile index b4e44bad19..c2409f2852 100644 --- a/conf/firmwares/test_progs.makefile +++ b/conf/firmwares/test_progs.makefile @@ -118,6 +118,9 @@ endif ifeq ($(BOARD), cc3d) LED_DEFINES = -DLED_BLUE=1 endif +ifeq ($(BOARD), naze32) +LED_DEFINES = -DLED_RED=1 -DLED_GREEN=2 +endif LED_DEFINES ?= -DLED_RED=2 -DLED_GREEN=3 test_sys_time_timer.ARCHDIR = $(ARCH) @@ -225,6 +228,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/conf/flash_modes.xml b/conf/flash_modes.xml index 1fcfbc607f..5ebf80e763 100644 --- a/conf/flash_modes.xml +++ b/conf/flash_modes.xml @@ -68,6 +68,7 @@ + @@ -80,6 +81,8 @@ + + diff --git a/sw/airborne/arch/stm32/cjmcu.ld b/sw/airborne/arch/stm32/cjmcu.ld new file mode 100644 index 0000000000..17adb0aaa7 --- /dev/null +++ b/sw/airborne/arch/stm32/cjmcu.ld @@ -0,0 +1,35 @@ +/* + * Hey Emacs, this is a -*- makefile -*- + * + * 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. + */ + +/* Linker script for CJMCU Quad (STM32F103CBT6, 128K flash, 20K RAM). */ + +/* Define memory regions. */ +MEMORY +{ + ram (rwx) : ORIGIN = 0x20000000, LENGTH = 20K + /* Use entire memory, no flash for stored settings */ + rom (rx) : ORIGIN = 0x08000000, LENGTH = 128K +} + +/* Include the common ld script. */ +INCLUDE libopencm3_stm32f1.ld diff --git a/sw/airborne/arch/stm32/cjmcu.makefile b/sw/airborne/arch/stm32/cjmcu.makefile new file mode 100644 index 0000000000..e53b9414d1 --- /dev/null +++ b/sw/airborne/arch/stm32/cjmcu.makefile @@ -0,0 +1,69 @@ +# Hey Emacs, this is a -*- makefile -*- +# +# cjmcu.makefile +# +# https://github.com/cleanflight/cleanflight/issues/22 +# http://blog.oscarliang.net/build-fpv-micro-quadcopter-smallest-quad +# hw rev2 ? +# + +BOARD=cjmcu +BOARD_CFG=\"boards/$(BOARD).h\" + +ARCH=stm32 +$(TARGET).ARCHDIR = $(ARCH) +# not needed? +$(TARGET).OOCD_INTERFACE=flossjtag +$(TARGET).LDSCRIPT=$(SRC_ARCH)/cjmcu.ld + +# ----------------------------------------------------------------------- + +# default flash mode is via SWD +# other possibilities: DFU-UTIL, JTAG, SWD, STLINK, SERIAL +FLASH_MODE ?= SWD + +HAS_LUFTBOOT ?= 0 +ifeq (,$(findstring $(HAS_LUFTBOOT),0 FALSE)) +$(TARGET).CFLAGS+=-DLUFTBOOT +$(TARGET).LDFLAGS+=-Wl,-Ttext=0x8002000 +endif + +# +# +# some default values shared between different firmwares +# +# + + +# +# default LED configuration +# +RADIO_CONTROL_LED ?= none +BARO_LED ?= none +AHRS_ALIGNER_LED ?= 1 +GPS_LED ?= 2 +SYS_TIME_LED ?= 3 + + +# +# default uart configuration +# +RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT ?= UART3 +RADIO_CONTROL_SPEKTRUM_SECONDARY_PORT ?= UART1 + +MODEM_PORT ?= UART1 +MODEM_BAUD ?= B57600 + +GPS_PORT ?= UART3 +GPS_BAUD ?= B38400 + + +# +# default actuator configuration +# +# you can use different actuators by adding a configure option to your firmware section +# e.g. +# +ACTUATORS ?= actuators_pwm diff --git a/sw/airborne/arch/stm32/naze32.ld b/sw/airborne/arch/stm32/naze32.ld new file mode 100644 index 0000000000..da613a8714 --- /dev/null +++ b/sw/airborne/arch/stm32/naze32.ld @@ -0,0 +1,35 @@ +/* + * Hey Emacs, this is a -*- makefile -*- + * + * Copyright (C) 2015 Felix Ruess + * + * 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. + */ + +/* Linker script for Abusemark Naze32 (STM32F103CBT6, 128K flash, 20K RAM). */ + +/* Define memory regions. */ +MEMORY +{ + ram (rwx) : ORIGIN = 0x20000000, LENGTH = 20K + /* Use entire memory, no flash for stored settings */ + rom (rx) : ORIGIN = 0x08000000, LENGTH = 128K +} + +/* Include the common ld script. */ +INCLUDE libopencm3_stm32f1.ld diff --git a/sw/airborne/boards/cjmcu.h b/sw/airborne/boards/cjmcu.h new file mode 100644 index 0000000000..277ac40f76 --- /dev/null +++ b/sw/airborne/boards/cjmcu.h @@ -0,0 +1,270 @@ +#ifndef CONFIG_CJMCU_H +#define CONFIG_CJMCU_H + +#define BOARD_CJMCU + +/* CJMCU has a 8MHz external clock and 72MHz internal. */ +#define EXT_CLK 8000000 +#define AHB_CLK 72000000 + +/* + * Onboard LEDs + */ + +/* green */ +#ifndef USE_LED_1 +#define USE_LED_1 1 +#endif +#define LED_1_GPIO GPIOC +#define LED_1_GPIO_PIN GPIO13 +#define LED_1_GPIO_ON gpio_clear +#define LED_1_GPIO_OFF gpio_set +#define LED_1_AFIO_REMAP ((void)0) + +/* red */ +#ifndef USE_LED_2 +#define USE_LED_2 1 +#endif +#define LED_2_GPIO GPIOC +#define LED_2_GPIO_PIN GPIO14 +#define LED_2_GPIO_ON gpio_clear +#define LED_2_GPIO_OFF gpio_set +#define LED_2_AFIO_REMAP ((void)0) + +/* blue */ +#ifndef USE_LED_3 +#define USE_LED_3 1 +#endif +#define LED_3_GPIO GPIOC +#define LED_3_GPIO_PIN GPIO15 +#define LED_3_GPIO_ON gpio_clear +#define LED_3_GPIO_OFF gpio_set +#define LED_3_AFIO_REMAP ((void)0) + + + +/* Default actuators driver */ +#define DEFAULT_ACTUATORS "subsystems/actuators/actuators_pwm.h" +#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y) +#define ActuatorsDefaultInit() ActuatorsPwmInit() +#define ActuatorsDefaultCommit() ActuatorsPwmCommit() + + +/* + * ADC + */ + +// Internal ADC for battery on PA2 enabled by default +#ifndef USE_ADC_1 +#define USE_ADC_1 1 +#endif +#if USE_ADC_1 +#define AD1_1_CHANNEL 2 +#define ADC_1 AD1_1 +#define ADC_1_GPIO_PORT GPIOA +#define ADC_1_GPIO_PIN GPIO2 +#endif + +/* allow to define ADC_CHANNEL_VSUPPLY in the airframe file*/ +#ifndef ADC_CHANNEL_VSUPPLY +#define ADC_CHANNEL_VSUPPLY ADC_1 +#endif + +/* 10k/2k resistor divider, 6 * 3.3V / 4096 */ +#define DefaultVoltageOfAdc(adc) (0.0049*adc) +//#define DefaultVoltageOfAdc(adc) (4) + +/* + * UART pin configuration + * + * sets on which pins the UARTs are connected + */ +/* DIAG port */ +#define UART1_GPIO_AF 0 +#define UART1_GPIO_PORT_RX GPIO_BANK_USART1_RX +#define UART1_GPIO_RX GPIO_USART1_RX +#define UART1_GPIO_PORT_TX GPIO_BANK_USART1_TX +#define UART1_GPIO_TX GPIO_USART1_TX + +/* GPS */ +#define UART3_GPIO_AF 0 +#define UART3_GPIO_PORT_RX GPIO_BANK_USART3_RX +#define UART3_GPIO_RX GPIO_USART3_RX +#define UART3_GPIO_PORT_TX GPIO_BANK_USART3_TX +#define UART3_GPIO_TX GPIO_USART3_TX + +/* LED1 & LED2 */ +/* +#define UART3_GPIO_AF AFIO_MAPR_USART3_REMAP_PARTIAL_REMAP +#define UART3_GPIO_PORT_RX GPIO_BANK_USART3_PR_RX +#define UART3_GPIO_RX GPIO_USART3_PR_RX +#define UART3_GPIO_PORT_TX GPIO_BANK_USART3_PR_TX +#define UART3_GPIO_TX GPIO_USART3_PR_TX +*/ + +/* + * Spektrum (none) + */ + +/* PPM + */ + +/* + * On Lisa/S there is no really dedicated port available. But we could use a + * bunch of different pins to do PPM Input. + */ + +/* + * Default is PPM config 1, input on PA0 + */ + +#ifndef PPM_CONFIG +#define PPM_CONFIG 1 +#endif + + +#if PPM_CONFIG == 1 +/* input on PA0 */ +#define USE_PPM_TIM2 1 +#define PPM_CHANNEL TIM_IC1 +#define PPM_TIMER_INPUT TIM_IC_IN_TI1 +#define PPM_IRQ NVIC_TIM2_IRQ +// Capture/Compare InteruptEnable and InterruptFlag +#define PPM_CC_IE TIM_DIER_CC1IE +#define PPM_CC_IF TIM_SR_CC1IF +#define PPM_GPIO_PORT GPIOA +#define PPM_GPIO_PIN GPIO0 +#define PPM_GPIO_AF 0 + +// Move default ADC timer +#if USE_AD_TIM2 +#undef USE_AD_TIM2 +#endif +#define USE_AD_TIM1 1 + +#elif PPM_CONFIG == 2 +/* input on PA1 */ +#define USE_PPM_TIM2 1 +#define PPM_CHANNEL TIM_IC2 +#define PPM_TIMER_INPUT TIM_IC_IN_TI2 +#define PPM_IRQ NVIC_TIM2_IRQ +// Capture/Compare InteruptEnable and InterruptFlag +#define PPM_CC_IE TIM_DIER_CC2IE +#define PPM_CC_IF TIM_SR_CC2IF +#define PPM_GPIO_PORT GPIOA +#define PPM_GPIO_PIN GPIO1 +#define PPM_GPIO_AF 0 + +// Move default ADC timer +#if USE_AD_TIM2 +#undef USE_AD_TIM2 +#endif +#define USE_AD_TIM1 1 + +#elif PPM_CONFIG == 3 +/* input on PA3 */ +#define USE_PPM_TIM2 1 +#define PPM_CHANNEL TIM_IC4 +#define PPM_TIMER_INPUT TIM_IC_IN_TI2 +#define PPM_IRQ NVIC_TIM2_IRQ +// Capture/Compare InteruptEnable and InterruptFlag +#define PPM_CC_IE TIM_DIER_CC4IE +#define PPM_CC_IF TIM_SR_CC4IF +#define PPM_GPIO_PORT GPIOA +#define PPM_GPIO_PIN GPIO3 +#define PPM_GPIO_AF 0 + +// Move default ADC timer +#if USE_AD_TIM2 +#undef USE_AD_TIM2 +#endif +#define USE_AD_TIM1 1 + +#else +#error "Unknown PPM config" + +#endif // PPM_CONFIG + +/* + * I2C + * + */ +#define I2C1_GPIO_PORT GPIOB +#define I2C1_GPIO_SCL GPIO6 +#define I2C1_GPIO_SDA GPIO7 + +/* GPS TX & RX */ +#define I2C2_GPIO_PORT GPIOB +#define I2C2_GPIO_SCL GPIO10 +#define I2C2_GPIO_SDA GPIO11 + +/* + * PWM + * + */ +#define PWM_USE_TIM4 1 +#define PWM_USE_TIM3 1 + +#define USE_PWM1 1 +#define USE_PWM2 1 +#define USE_PWM3 1 +#define USE_PWM4 1 + +/* PWM_SERVO_x is the index of the servo in the actuators_pwm_values array */ +#if USE_PWM1 +#define PWM_SERVO_1 0 +#define PWM_SERVO_1_TIMER TIM4 +#define PWM_SERVO_1_GPIO GPIOB +#define PWM_SERVO_1_PIN GPIO8 +#define PWM_SERVO_1_AF 0 +#define PWM_SERVO_1_OC TIM_OC3 +#define PWM_SERVO_1_OC_BIT (1<<2) +#else +#define PWM_SERVO_1_OC_BIT 0 +#endif + +#if USE_PWM2 +#define PWM_SERVO_2 1 +#define PWM_SERVO_2_TIMER TIM4 +#define PWM_SERVO_2_GPIO GPIOB +#define PWM_SERVO_2_PIN GPIO9 +#define PWM_SERVO_2_AF 0 +#define PWM_SERVO_2_OC TIM_OC4 +#define PWM_SERVO_2_OC_BIT (1<<3) +#else +#define PWM_SERVO_2_OC_BIT 0 +#endif + +#if USE_PWM3 +#define PWM_SERVO_3 2 +#define PWM_SERVO_3_TIMER TIM3 +#define PWM_SERVO_3_GPIO GPIOB +#define PWM_SERVO_3_PIN GPIO0 +#define PWM_SERVO_3_AF 0 +#define PWM_SERVO_3_OC TIM_OC3 +#define PWM_SERVO_3_OC_BIT (1<<2) +#else +#define PWM_SERVO_3_OC_BIT 0 +#endif + +#if USE_PWM4 +#define PWM_SERVO_4 3 +#define PWM_SERVO_4_TIMER TIM3 +#define PWM_SERVO_4_GPIO GPIOB +#define PWM_SERVO_4_PIN GPIO1 +#define PWM_SERVO_4_AF 0 +#define PWM_SERVO_4_OC TIM_OC4 +#define PWM_SERVO_4_OC_BIT (1<<3) +#else +#define PWM_SERVO_4_OC_BIT 0 +#endif + + + +/* servos 1-2 on TIM4 */ +#define PWM_TIM4_CHAN_MASK (PWM_SERVO_1_OC_BIT|PWM_SERVO_2_OC_BIT) +/* servos 3-4 on TIM3 */ +#define PWM_TIM3_CHAN_MASK (PWM_SERVO_3_OC_BIT|PWM_SERVO_4_OC_BIT) + + +#endif /* CONFIG_CJMCU_H */ diff --git a/sw/airborne/boards/naze32/baro_board.h b/sw/airborne/boards/naze32/baro_board.h new file mode 100644 index 0000000000..4a3bb4037b --- /dev/null +++ b/sw/airborne/boards/naze32/baro_board.h @@ -0,0 +1,19 @@ + +/* + * board specific functions for the Naze32 board + * + */ + +#ifndef BOARDS_NAZE32_BARO_H +#define BOARDS_NAZE32_BARO_H + +// only for printing the baro type during compilation +#ifndef BARO_BOARD +#define BARO_BOARD BARO_MS5611_I2C +#endif + +extern void baro_event(void); + +#define BaroEvent baro_event + +#endif /* BOARDS_NAZE32_BARO_H */ diff --git a/sw/airborne/boards/naze32_common.h b/sw/airborne/boards/naze32_common.h new file mode 100644 index 0000000000..fafb85572e --- /dev/null +++ b/sw/airborne/boards/naze32_common.h @@ -0,0 +1,360 @@ +#ifndef CONFIG_NAZE32_COMMON_H +#define CONFIG_NAZE32_COMMON_H + +#define BOARD_NAZE32 + +/* + * Onboard LEDs + */ + + +/* L0 red status led, on PB4 */ +#ifndef USE_LED_1 +#define USE_LED_1 1 +#endif +#define LED_1_GPIO GPIOB +#define LED_1_GPIO_PIN GPIO4 +#define LED_1_GPIO_ON gpio_clear +#define LED_1_GPIO_OFF gpio_set +#define LED_1_AFIO_REMAP { \ + rcc_periph_clock_enable(RCC_AFIO); \ + AFIO_MAPR |= AFIO_MAPR_SWJ_CFG_JTAG_OFF_SW_ON; \ + } + +/* L1 green status led, on PB3 */ +#ifndef USE_LED_2 +#define USE_LED_2 1 +#endif +#define LED_2_GPIO GPIOB +#define LED_2_GPIO_PIN GPIO3 +#define LED_2_GPIO_ON gpio_clear +#define LED_2_GPIO_OFF gpio_set +#define LED_2_AFIO_REMAP ((void)0) + +/* + * UART pin configuration + * + * sets on which pins the UARTs are connected + */ +/* UART1 on main port */ +#define UART1_GPIO_AF 0 +#define UART1_GPIO_PORT_RX GPIO_BANK_USART1_RX +#define UART1_GPIO_RX GPIO_USART1_RX +#define UART1_GPIO_PORT_TX GPIO_BANK_USART1_TX +#define UART1_GPIO_TX GPIO_USART1_TX + +/* UART2 on RC I/O, tx pin 5/id "3", rx pin 6/id "4") */ +#define UART2_GPIO_AF 0 +#define UART2_GPIO_PORT_RX GPIO_BANK_USART2_RX +#define UART2_GPIO_RX GPIO_USART2_RX +#define UART2_GPIO_PORT_TX GPIO_BANK_USART2_TX +#define UART2_GPIO_TX GPIO_USART2_TX + +/* + * Spektrum + */ +/* The line that is pulled low at power up to initiate the bind process */ +/* set to ppm/servo6 input pin on RC connector for now */ +#define SPEKTRUM_BIND_PIN GPIO1 +#define SPEKTRUM_BIND_PIN_PORT GPIOA + +#define SPEKTRUM_UART1_RCC RCC_USART1 +#define SPEKTRUM_UART1_BANK GPIO_BANK_USART1_RX +#define SPEKTRUM_UART1_PIN GPIO_USART1_RX +#define SPEKTRUM_UART1_AF 0 +#define SPEKTRUM_UART1_IRQ NVIC_USART1_IRQ +#define SPEKTRUM_UART1_ISR usart1_isr +#define SPEKTRUM_UART1_DEV USART1 + +#define SPEKTRUM_UART2_RCC RCC_USART2 +#define SPEKTRUM_UART2_BANK GPIO_BANK_USART2_RX +#define SPEKTRUM_UART2_PIN GPIO_USART2_RX +#define SPEKTRUM_UART2_AF 0 +#define SPEKTRUM_UART2_IRQ NVIC_USART2_IRQ +#define SPEKTRUM_UART2_ISR usart2_isr +#define SPEKTRUM_UART2_DEV USART2 + + + +/* PPM + * + * Default: PPM config 1, input on PA0: RC I/O rx_ppm pin 3/id "1" + * 6 servos: PPM config 2, input on PA7: RC I/O unused pin 8/id "6" + */ + +#ifndef PPM_CONFIG +#define PPM_CONFIG 2 +#else +#define PPM_CONFIG 1 +#endif + +#if PPM_CONFIG == 1 +/* input on PA0 (ppm_in) */ +#define USE_PPM_TIM2 1 +#define PPM_CHANNEL TIM_IC1 +#define PPM_TIMER_INPUT TIM_IC_IN_TI1 +#define PPM_IRQ NVIC_TIM2_IRQ +// Capture/Compare InteruptEnable and InterruptFlag +#define PPM_CC_IE TIM_DIER_CC1IE +#define PPM_CC_IF TIM_SR_CC1IF +#define PPM_GPIO_PORT GPIOA +#define PPM_GPIO_PIN GPIO0 +#define PPM_GPIO_AF 0 + +// Move default ADC timer +#if USE_AD_TIM2 +#undef USE_AD_TIM2 +#endif +#define USE_AD_TIM1 1 + +#if USE_SERVOS_5AND6 +#error PA0 does not work with 6 servos +#endif + +#elif PPM_CONFIG == 2 +/* input on PA7 (unused) */ +#define USE_PPM_TIM3 1 +#define PPM_CHANNEL TIM_IC2 +#define PPM_TIMER_INPUT TIM_IC_IN_TI2 +#define PPM_IRQ NVIC_TIM3_IRQ +// Capture/Compare InteruptEnable and InterruptFlag +#define PPM_CC_IE TIM_DIER_CC2IE +#define PPM_CC_IF TIM_SR_CC2IF +#define PPM_GPIO_PORT GPIOA +#define PPM_GPIO_PIN GPIO7 +#define PPM_GPIO_AF 0 + +// Move default ADC timer +#if USE_AD_TIM1 +#undef USE_AD_TIM1 +#endif +#define USE_AD_TIM2 1 + +#else +#error "Unknown PPM config" + +#endif // PPM_CONFIG + + +/* + * ADC + */ + +// Internal ADC for battery enabled by default +#ifndef USE_ADC_4 +#define USE_ADC_4 1 +#endif +#if USE_ADC_4 +#define AD1_4_CHANNEL 4 +#define ADC_4 AD1_4 +#define ADC_4_GPIO_PORT GPIOA +#define ADC_4_GPIO_PIN GPIO4 +#endif + +/* allow to define ADC_CHANNEL_VSUPPLY in the airframe file*/ +#ifndef ADC_CHANNEL_VSUPPLY +#define ADC_CHANNEL_VSUPPLY ADC_4 +#endif + +/* 10k/1k resistor divider, 11 * 3.3V / 4096 */ +#define DefaultVoltageOfAdc(adc) (0.008862*adc) + + +/* by default activate onboard baro */ +#ifndef USE_BARO_BOARD +#define USE_BARO_BOARD 1 +#endif + +/* + * I2C + */ +/* RC port */ +#define I2C2_GPIO_PORT GPIOB +#define I2C2_GPIO_SCL GPIO10 +#define I2C2_GPIO_SDA GPIO11 + + +/* + * PWM + * + * Servo output numbering on silkscreen starts with '1' + * + * silk standard USE_SERVOS_5AND6 + * + * '1' PA8 servo1 + * '2' PA11 servo2 + * '3' PB6 servo1 servo3 + * '4' PB7 servo2 servo4 + * '5' PB8 servo3 servo5 + * '6' PB9 servo4 servo6 + * + * PPM_in: PA0 PA7 + * + */ + +#if USE_SERVOS_5AND6 + +#define ACTUATORS_PWM_NB 6 + +/* servos 1-2 */ +#define PWM_USE_TIM1 1 +#define USE_PWM1 1 +#define USE_PWM2 1 + +/* servos 3-6 */ +#define PWM_USE_TIM4 1 +#define USE_PWM3 1 +#define USE_PWM4 1 +#define USE_PWM5 1 +#define USE_PWM6 1 + +#if USE_PWM1 +#define PWM_SERVO_1 0 +#define PWM_SERVO_1_TIMER TIM1 +#define PWM_SERVO_1_GPIO GPIOA +#define PWM_SERVO_1_PIN GPIO8 +#define PWM_SERVO_1_AF 0 +#define PWM_SERVO_1_OC TIM_OC1 +#define PWM_SERVO_1_OC_BIT (1<<0) +#else +#define PWM_SERVO_1_OC_BIT 0 +#endif + +#if USE_PWM2 +#define PWM_SERVO_2 1 +#define PWM_SERVO_2_TIMER TIM1 +#define PWM_SERVO_2_GPIO GPIOA +#define PWM_SERVO_2_PIN GPIO11 +#define PWM_SERVO_2_AF 0 +#define PWM_SERVO_2_OC TIM_OC4 +#define PWM_SERVO_2_OC_BIT (1<<3) +#else +#define PWM_SERVO_2_OC_BIT 0 +#endif + +#if USE_PWM3 +#define PWM_SERVO_3 2 +#define PWM_SERVO_3_TIMER TIM4 +#define PWM_SERVO_3_GPIO GPIOB +#define PWM_SERVO_3_PIN GPIO6 +#define PWM_SERVO_3_AF 0 +#define PWM_SERVO_3_OC TIM_OC1 +#define PWM_SERVO_3_OC_BIT (1<<0) +#else +#define PWM_SERVO_3_OC_BIT 0 +#endif + +#if USE_PWM4 +#define PWM_SERVO_4 3 +#define PWM_SERVO_4_TIMER TIM4 +#define PWM_SERVO_4_GPIO GPIOB +#define PWM_SERVO_4_PIN GPIO7 +#define PWM_SERVO_4_AF 0 +#define PWM_SERVO_4_OC TIM_OC2 +#define PWM_SERVO_4_OC_BIT (1<<1) +#else +#define PWM_SERVO_4_OC_BIT 0 +#endif + +#if USE_PWM5 +#define PWM_SERVO_5 4 +#define PWM_SERVO_5_TIMER TIM4 +#define PWM_SERVO_5_GPIO GPIOB +#define PWM_SERVO_5_PIN GPIO8 +#define PWM_SERVO_5_AF 0 +#define PWM_SERVO_5_OC TIM_OC3 +#define PWM_SERVO_5_OC_BIT (1<<2) +#else +#define PWM_SERVO_5_OC_BIT 0 +#endif + +#if USE_PWM6 +#define PWM_SERVO_6 5 +#define PWM_SERVO_6_TIMER TIM4 +#define PWM_SERVO_6_GPIO GPIOB +#define PWM_SERVO_6_PIN GPIO9 +#define PWM_SERVO_6_AF 0 +#define PWM_SERVO_6_OC TIM_OC4 +#define PWM_SERVO_6_OC_BIT (1<<3) +#else +#define PWM_SERVO_6_OC_BIT 0 +#endif + +/* servos 1-2 on TIM1 */ +#define PWM_TIM1_CHAN_MASK (PWM_SERVO_1_OC_BIT|PWM_SERVO_2_OC_BIT) +/* servos 3-6 on TIM4 */ +#define PWM_TIM4_CHAN_MASK (PWM_SERVO_3_OC_BIT|PWM_SERVO_4_OC_BIT|PWM_SERVO_5_OC_BIT|PWM_SERVO_6_OC_BIT) + +#else //USE_SERVOS_5AND6 + +#define ACTUATORS_PWM_NB 4 + +/* servos 1-4 */ +#define PWM_USE_TIM4 1 +#define USE_PWM1 1 +#define USE_PWM2 1 +#define USE_PWM3 1 +#define USE_PWM4 1 + +#if USE_PWM1 +#define PWM_SERVO_1 0 +#define PWM_SERVO_1_TIMER TIM4 +#define PWM_SERVO_1_GPIO GPIOB +#define PWM_SERVO_1_PIN GPIO6 +#define PWM_SERVO_1_AF 0 +#define PWM_SERVO_1_OC TIM_OC1 +#define PWM_SERVO_1_OC_BIT (1<<0) +#else +#define PWM_SERVO_1_OC_BIT 0 +#endif + +#if USE_PWM2 +#define PWM_SERVO_2 1 +#define PWM_SERVO_2_TIMER TIM4 +#define PWM_SERVO_2_GPIO GPIOB +#define PWM_SERVO_2_PIN GPIO7 +#define PWM_SERVO_2_AF 0 +#define PWM_SERVO_2_OC TIM_OC2 +#define PWM_SERVO_2_OC_BIT (1<<1) +#else +#define PWM_SERVO_2_OC_BIT 0 +#endif + +#if USE_PWM3 +#define PWM_SERVO_3 2 +#define PWM_SERVO_3_TIMER TIM4 +#define PWM_SERVO_3_GPIO GPIOB +#define PWM_SERVO_3_PIN GPIO8 +#define PWM_SERVO_3_AF 0 +#define PWM_SERVO_3_OC TIM_OC3 +#define PWM_SERVO_3_OC_BIT (1<<2) +#else +#define PWM_SERVO_3_OC_BIT 0 +#endif + +#if USE_PWM4 +#define PWM_SERVO_4 3 +#define PWM_SERVO_4_TIMER TIM4 +#define PWM_SERVO_4_GPIO GPIOB +#define PWM_SERVO_4_PIN GPIO9 +#define PWM_SERVO_4_AF 0 +#define PWM_SERVO_4_OC TIM_OC4 +#define PWM_SERVO_4_OC_BIT (1<<3) +#else +#define PWM_SERVO_4_OC_BIT 0 +#endif + +/* servos 1-4 on TIM4 */ +#define PWM_TIM4_CHAN_MASK (PWM_SERVO_1_OC_BIT|PWM_SERVO_2_OC_BIT|PWM_SERVO_3_OC_BIT|PWM_SERVO_4_OC_BIT) + +#endif //USE_SERVOS_5AND6 + +/* Default actuators driver */ +#define DEFAULT_ACTUATORS "subsystems/actuators/actuators_pwm.h" +#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y) +#define ActuatorsDefaultInit() ActuatorsPwmInit() +#define ActuatorsDefaultCommit() ActuatorsPwmCommit() + + +#endif /* CONFIG_NAZE32_COMMON_H */ + diff --git a/sw/airborne/boards/naze32_rev4.h b/sw/airborne/boards/naze32_rev4.h new file mode 100644 index 0000000000..9ce31156fc --- /dev/null +++ b/sw/airborne/boards/naze32_rev4.h @@ -0,0 +1,11 @@ +#ifndef CONFIG_NAZE32_REV4_H +#define CONFIG_NAZE32_REV4_H + +#include "boards/naze32_common.h" + +/* Naze32 rev4 has a 8MHz external clock and 72MHz internal */ +#define EXT_CLK 8000000 +#define AHB_CLK 72000000 + + +#endif /* CONFIG_NAZE32_REV4_H */ diff --git a/sw/airborne/boards/naze32_rev5.h b/sw/airborne/boards/naze32_rev5.h new file mode 100644 index 0000000000..40cec167ce --- /dev/null +++ b/sw/airborne/boards/naze32_rev5.h @@ -0,0 +1,14 @@ +#ifndef CONFIG_NAZE32_REV5_H +#define CONFIG_NAZE32_REV5_H + +#include "boards/naze32_common.h" + +/* Naze32 rev5 has a 12MHz external clock and 72MHz internal */ +#define EXT_CLK 12000000 +#define AHB_CLK 72000000 + +/* 16Mbit flash on spi2 (Naze32 full) */ +#define SPI_SELECT_SLAVE1_PORT GPIOB +#define SPI_SELECT_SLAVE1_PIN GPIO12 + +#endif /* CONFIG_NAZE32_REV5_H */ diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c index 894ea88f59..ded4e50f93 100644 --- a/sw/airborne/firmwares/fixedwing/main_ap.c +++ b/sw/airborne/firmwares/fixedwing/main_ap.c @@ -178,6 +178,10 @@ void init_ap(void) mcu_init(); #endif /* SINGLE_MCU */ +#if defined(PPRZ_TRIG_INT_COMPR_FLASH) + pprz_trig_int_init(); +#endif + /****** initialize and reset state interface ********/ stateInit(); 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..b538f33179 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,428 @@ 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 + /* USE_REAL_TABLE_12_USE uses the original table instead of unrolled "if". + * Unrolling makes it faster but uses slightly more code. + * For the higher compressions it is not faster/creates more code. + * The option is for debugging/understanding only. + */ + /* 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; + } +#else + // suppress unused-parameter warning + (void)val_prev; + (void)tab; +#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 +865,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..941082679f 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/subsystems/ahrs/ahrs_aligner.c b/sw/airborne/subsystems/ahrs/ahrs_aligner.c index ffd1b556b0..de8dfcc221 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_aligner.c +++ b/sw/airborne/subsystems/ahrs/ahrs_aligner.c @@ -36,7 +36,7 @@ struct AhrsAligner ahrs_aligner; -#define SAMPLES_NB PERIODIC_FREQUENCY +#define SAMPLES_NB 100 static struct Int32Rates gyro_sum; static struct Int32Vect3 accel_sum; @@ -98,8 +98,9 @@ void ahrs_aligner_init(void) #ifndef LOW_NOISE_THRESHOLD #define LOW_NOISE_THRESHOLD 90000 #endif +/** Number of cycles (100 samples each) with low noise */ #ifndef LOW_NOISE_TIME -#define LOW_NOISE_TIME 1 +#define LOW_NOISE_TIME 5 #endif void ahrs_aligner_run(void) 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(); +}