diff --git a/TODO b/TODO deleted file mode 100644 index 0012cf0db3..0000000000 --- a/TODO +++ /dev/null @@ -1,140 +0,0 @@ -This is so totally out of date.... - - -.... it seems. - - - - -add date/time of generation in generated code - -############################################################## - -add an irc client to morphix - preconfigured for paparazzi channel - - - -make clean doesn't delete boa config - -make a "single node" mode where boa isn't needed and serve file:// URL -force ivybus to 127 in single node -default to that mode - - - - - - -######################################## obsolete ? ############## - -dans flybywire -chop servo ne depends pas du servo - - -pour les missions -possibilité de "transformer" (rotation Z, translation XYZ) une mission - -possibilitite de faire la meme chose pour une partie des waypoints (on en a une partie pour les evolutions et une partie pour le circuit d'atterissage. On deplace ceux du circuit d'atterissage pour qu'il colle a la piste. et on deplace ceux des evolution pour etre en face du jury :) - - -On stocke la/les transformations et on peut avoir des missions communes muret/ricou - -des declarations de points locales aux blocs et des transformations par blocs - - -faire medit avec visu3d - -proposer d'ajouter un waypoint en relatif par rapport a un autre - et en coordonnees polaires (dist, QDM) - - - - - -l'interface du captureur de video - c'est aussi visu3d . Il a une liste de textures (photos) et on peut les transformer. La description est sauvées dans un fichier xml et peut etre rechargée. les photos vont dans var/photos - - -dans visu3d, il faudrait pouvoir pivoter en Z sur la position courante (en tenant compte du zoom ) - - -###################### - -logger les simus comme les vols - y penser en refaisant receive - code commun - -Pour les simus a plusieurs avions, il doit y avoir partage d'un certain nombre d'informations entre les differentes instances des simu (par exemple le vent) - -Dans un circle, il faut afficher le QDR - calcul au sol?? - -Pour les missions, il faudrait pouvoir dire . faire un cercle pendant 180° ou faire un cercle pendant n secondes. Il faudrait donc disposer du temps depuis le block et du de l'angle parcouru depuis le debut du cercle. -C'est pour faire un palier en haut de la monté. Pour laisser le terme accumulateur se recaler avant la descente. - -########################################### - -Sujet : procedure automatique d'interuption de vol pour microdrone - --identifier des scenarios: - -cause de l'interuption : autonomie, meteo, defaillance systeme - --modeliser la zone d'evolution et les autres contraintres (systemes defaillants, meteo) - -(dans un meeting, on veut a tout prix eviter le public, les routes etc...) - -- initialisation -- iteratif ? - -####################################### - -integrer les gazs pour estimer l'autonomie restante - - -########################################### - - -Sujet Drone Thales - -Les eleves (1A ou 2A) construisent un avion et apprennent a le faire voler d'ici juillet. - -commande PCB -commande composants radiospare/melexys/coronis/ublox -labo pour assembler (labo micro onde ?) - -commande garat (avion, moteur, servos, batteries, radiocommande etc....) -achat de petit outillage (dremel, fer a souder etc...) -assemblage au labo drone - -cours de pilotages sur le twinstar - -On leur donne les petits projets pendant l'année sur Paparazzi. - - - -############################################ - -mettre les projets enac sur la page web enac - -############################################ - - -Pourquoi on ne laisse pas message.xml modifiable, pour les taux de telemesure par exemple. - -on split les Makefiles - - -########### - -manque gerbmerge dans les dependance??? ha non ca n'existe pas... a packager ! - - -> -> -> - Pitch should more be in degree than in radian in flight plan files. ->It could be in the future version of Paparazzi. -> -> -> - It's strange MIN_HEIGHT_CARROT and MAX_HEIGHT_CARROT are in the ->code. Shouldn't it be in a conf file so as to change it ? -> -- In the same way, MIN_SPEED_FOR_TAKEOFF should be in the airframe -conf file ? - - diff --git a/conf/Makefile.stm32 b/conf/Makefile.stm32 index fb7fe7ea20..be9a360e33 100644 --- a/conf/Makefile.stm32 +++ b/conf/Makefile.stm32 @@ -138,7 +138,7 @@ else LDFLAGS = -D__thumb2__ -T$(LDSCRIPT) -nostartfiles -L$(GCC_LIB_DIR) -O$(OPT) --gc-sections endif LDFLAGS += -Wl,-Map=$(OBJDIR)/$(TARGET).map,--cref,--gc-sections -LDLIBS += -lc -lm -lgcc -lcmsis -lstm32 +LDLIBS += -lc -lm -lgcc -lcmsis -lstm32 -lopencm3_stm32 CPFLAGS = -j .isr_vector -j .text -j .data CPFLAGS_BIN = -Obinary diff --git a/conf/airframes/ENAC/quadrotor/blender.xml b/conf/airframes/ENAC/quadrotor/blender.xml index 72313de308..ee48120890 100644 --- a/conf/airframes/ENAC/quadrotor/blender.xml +++ b/conf/airframes/ENAC/quadrotor/blender.xml @@ -18,9 +18,6 @@ - - - diff --git a/conf/airframes/ENAC/quadrotor/booz2_g1.xml b/conf/airframes/ENAC/quadrotor/booz2_g1.xml index b1405b3931..b8acf7af5e 100644 --- a/conf/airframes/ENAC/quadrotor/booz2_g1.xml +++ b/conf/airframes/ENAC/quadrotor/booz2_g1.xml @@ -4,7 +4,9 @@ - + diff --git a/conf/airframes/Poine/booz2_a7.xml b/conf/airframes/Poine/booz2_a7.xml index 091401ebed..61428ed284 100644 --- a/conf/airframes/Poine/booz2_a7.xml +++ b/conf/airframes/Poine/booz2_a7.xml @@ -61,14 +61,6 @@ - @@ -77,7 +69,7 @@
- +
@@ -124,7 +116,7 @@ - + + --> + + + + + + + + + + + + + + + + + + +
+ + + + +
+ +
- +
@@ -155,21 +173,29 @@ + + + + + + - +
- + - + +
@@ -182,15 +208,19 @@
- + +--> - - + + + + + + @@ -201,29 +231,35 @@ - + + + + + - - - - - - - - - - - - - + + + + + + + + + + + + + + + diff --git a/conf/airframes/Poine/test_settings.xml b/conf/airframes/Poine/test_settings.xml new file mode 100644 index 0000000000..72186562de --- /dev/null +++ b/conf/airframes/Poine/test_settings.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + diff --git a/conf/airframes/booz2_flixr.xml b/conf/airframes/booz2_flixr.xml index 38255e1429..323dc620f4 100644 --- a/conf/airframes/booz2_flixr.xml +++ b/conf/airframes/booz2_flixr.xml @@ -4,28 +4,21 @@ - - + - - + + - - - - - + + - - - @@ -33,6 +26,8 @@ + + @@ -213,8 +208,7 @@ - - +
diff --git a/conf/airframes/esden/lisa_m_pwm.xml b/conf/airframes/esden/lisa_m_pwm.xml index 0545cff7a6..2d93394455 100644 --- a/conf/airframes/esden/lisa_m_pwm.xml +++ b/conf/airframes/esden/lisa_m_pwm.xml @@ -226,11 +226,12 @@ - - + @@ -90,7 +90,7 @@
- +
@@ -138,15 +138,15 @@ - - - + + + - - - + + + - + @@ -167,14 +167,15 @@ - - - + + + + - +
@@ -220,7 +221,9 @@ - + + + diff --git a/conf/airframes/twinjet_example.xml b/conf/airframes/twinjet_example.xml index 70fd68b0b7..b5974575d7 100644 --- a/conf/airframes/twinjet_example.xml +++ b/conf/airframes/twinjet_example.xml @@ -36,7 +36,7 @@ - + diff --git a/conf/autopilot/booz2_test_progs.makefile b/conf/autopilot/booz2_test_progs.makefile index ec7cc603ac..ff3604c885 100644 --- a/conf/autopilot/booz2_test_progs.makefile +++ b/conf/autopilot/booz2_test_progs.makefile @@ -131,6 +131,29 @@ tunnel_bb.CFLAGS += -DUSE_LED tunnel_bb.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c +# +# usb tunnels +# +usb_tunnel_0.ARCHDIR = $(ARCH) +usb_tunnel_0.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) +usb_tunnel_0.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B38400 -DPERIPHERALS_AUTO_INIT +usb_tunnel_0.CFLAGS += -DUSE_USB_LINE_CODING -DUSE_USB_SERIAL -DUSE_LED +usb_tunnel_0.srcs += $(SRC_ARCH)/usb_tunnel.c $(SRC_ARCH)/usb_ser_hw.c $(SRC_ARCH)/mcu_periph/uart_arch.c +usb_tunnel_0.srcs += $(SRC_ARCH)/lpcusb/usbhw_lpc.c $(SRC_ARCH)/lpcusb/usbinit.c +usb_tunnel_0.srcs += $(SRC_ARCH)/lpcusb/usbcontrol.c $(SRC_ARCH)/lpcusb/usbstdreq.c +usb_tunnel_0.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c +usb_tunnel_0.srcs += mcu.c $(SRC_ARCH)/mcu_arch.c + +usb_tunnel_1.ARCHDIR = $(ARCH) +usb_tunnel_1.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) +usb_tunnel_1.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B38400 -DPERIPHERALS_AUTO_INIT +usb_tunnel_1.CFLAGS += -DUSE_USB_LINE_CODING -DUSE_USB_SERIAL -DUSE_LED +usb_tunnel_1.srcs += $(SRC_ARCH)/usb_tunnel.c $(SRC_ARCH)/usb_ser_hw.c $(SRC_ARCH)/mcu_periph/uart_arch.c +usb_tunnel_1.srcs += $(SRC_ARCH)/lpcusb/usbhw_lpc.c $(SRC_ARCH)/lpcusb/usbinit.c +usb_tunnel_1.srcs += $(SRC_ARCH)/lpcusb/usbcontrol.c $(SRC_ARCH)/lpcusb/usbstdreq.c +usb_tunnel_1.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c +usb_tunnel_1.srcs += mcu.c $(SRC_ARCH)/mcu_arch.c + # # test GPS @@ -189,6 +212,7 @@ test_usb.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))' # -DTIME_LED=1 test_usb.CFLAGS += -DUSE_LED test_usb.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c +test_usb.srcs += mcu.c $(SRC_ARCH)/mcu_arch.c #test_usb.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600 #test_usb.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c diff --git a/conf/autopilot/fixedwing.makefile b/conf/autopilot/fixedwing.makefile index d4e98b98c4..139a7ca919 100644 --- a/conf/autopilot/fixedwing.makefile +++ b/conf/autopilot/fixedwing.makefile @@ -37,6 +37,9 @@ ifeq ($(TARGET),$(ACTUATOR_TARGET)) ifeq ($(BOARD),lisa_l) include $(CFG_SHARED)/actuators_direct.makefile endif + ifeq ($(BOARD),lisa_m) + include $(CFG_SHARED)/actuators_direct.makefile + endif else include $(CFG_SHARED)/$(ACTUATORS).makefile diff --git a/conf/autopilot/lisa_l_test_progs.makefile b/conf/autopilot/lisa_l_test_progs.makefile index e6cf380777..95aef22c04 100644 --- a/conf/autopilot/lisa_l_test_progs.makefile +++ b/conf/autopilot/lisa_l_test_progs.makefile @@ -276,11 +276,11 @@ test_adc.ARCHDIR = $(ARCH) test_adc.CFLAGS = -I$(SRC_LISA) -I$(ARCH) -DPERIPHERALS_AUTO_INIT test_adc.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) -test_adc.srcs = $(SRC_AIRBORNE)/mcu.c \ +test_adc.srcs = $(SRC_LISA)/test_adc.c \ + $(SRC_AIRBORNE)/mcu.c \ $(SRC_ARCH)/mcu_arch.c \ - $(SRC_LISA)/test_adc.c \ - $(SRC_ARCH)/stm32_exceptions.c \ - $(SRC_ARCH)/stm32_vector_table.c + $(SRC_ARCH)/stm32_exceptions.c \ + $(SRC_ARCH)/stm32_vector_table.c test_adc.CFLAGS += -DUSE_LED test_adc.srcs += $(SRC_ARCH)/led_hw.c @@ -299,138 +299,175 @@ test_adc.srcs += downlink.c pprz_transport.c test_adc.srcs += $(SRC_ARCH)/mcu_periph/adc_arch.c test_adc.CFLAGS += -DUSE_AD1 -DUSE_AD1_1 -DUSE_AD1_2 -DUSE_AD1_3 -DUSE_AD1_4 +#test_adc.CFLAGS += -DUSE_AD1 -DUSE_AD1_3 test_adc.CFLAGS += -DUSE_ADC1_2_IRQ_HANDLER + + # -# test IMU b2 +# common test # # configuration # SYS_TIME_LED # MODEM_PORT # MODEM_BAUD # +PERIODIC_FREQUENCY = 512 + +COMMON_TEST_CFLAGS = -I$(SRC_FIRMWARE) -I$(ARCH) -DPERIPHERALS_AUTO_INIT +COMMON_TEST_CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) +COMMON_TEST_SRCS = $(SRC_AIRBORNE)/mcu.c \ + $(SRC_ARCH)/mcu_arch.c \ + $(SRC_ARCH)/stm32_exceptions.c \ + $(SRC_ARCH)/stm32_vector_table.c +COMMON_TEST_CFLAGS += -DUSE_LED +COMMON_TEST_SRCS += $(SRC_ARCH)/led_hw.c +COMMON_TEST_CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=$(SYS_TIME_LED) +COMMON_TEST_CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./$(PERIODIC_FREQUENCY).))' +COMMON_TEST_CFLAGS += -DPERIODIC_FREQUENCY=$(PERIODIC_FREQUENCY) +COMMON_TEST_SRCS += sys_time.c $(SRC_ARCH)/sys_time_hw.c +COMMON_TEST_CFLAGS += -DUSE_$(MODEM_PORT) -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD) +COMMON_TEST_SRCS += $(SRC_ARCH)/mcu_periph/uart_arch.c +COMMON_TEST_CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT) +COMMON_TEST_SRCS += downlink.c pprz_transport.c +COMMON_TEST_SRCS += math/pprz_trig_int.c + + +# +# test IMU b2 v1.1 +# +IMU_B2_CFLAGS = -DIMU_TYPE_H=\"subsystems/imu/imu_b2.h\" +IMU_B2_CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_MS2100 -DIMU_B2_VERSION_1_1 +IMU_B2_SRCS = $(SRC_SUBSYSTEMS)/imu.c +IMU_B2_CFLAGS += -DMAX_1168_DRDY_PORT=$(MAX_1168_DRDY_PORT) +IMU_B2_CFLAGS += -DMAX_1168_DRDY_PORT_SOURCE=$(MAX_1168_DRDY_PORT_SOURCE) +IMU_B2_CFLAGS += -DUSE_SPI2 -DUSE_DMA1_C4_IRQ -DUSE_EXTI2_IRQ -DUSE_SPI2_IRQ +IMU_B2_SRCS += $(SRC_SUBSYSTEMS)/imu/imu_b2.c $(SRC_ARCH)/subsystems/imu/imu_b2_arch.c +IMU_B2_SRCS += peripherals/max1168.c $(SRC_ARCH)/peripherals/max1168_arch.c +IMU_B2_SRCS += peripherals/ms2100.c $(SRC_ARCH)/peripherals/ms2100_arch.c + test_imu_b2.ARCHDIR = $(ARCH) -test_imu_b2.CFLAGS = -I$(SRC_LISA) -I$(ARCH) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) -DPERIPHERALS_AUTO_INIT -test_imu_b2.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) -test_imu_b2.srcs += $(SRC_AIRBORNE)/mcu.c \ - $(SRC_ARCH)/mcu_arch.c \ - $(SRC_BOOZ_TEST)/booz_test_imu.c \ - $(SRC_ARCH)/stm32_exceptions.c \ - $(SRC_ARCH)/stm32_vector_table.c +test_imu_b2.srcs = test/subsystems/test_imu.c +test_imu_b2.CFLAGS = $(COMMON_TEST_CFLAGS) +test_imu_b2.srcs += $(COMMON_TEST_SRCS) +test_imu_b2.CFLAGS += $(IMU_B2_CFLAGS) +test_imu_b2.srcs += $(IMU_B2_SRCS) -test_imu_b2.CFLAGS += -DUSE_LED -test_imu_b2.srcs += $(SRC_ARCH)/led_hw.c -test_imu_b2.CFLAGS += -DUSE_SYS_TIME -test_imu_b2.CFLAGS += -DSYS_TIME_LED=$(SYS_TIME_LED) -test_imu_b2.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))' -test_imu_b2.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c -test_imu_b2.CFLAGS += -DUSE_$(MODEM_PORT) -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD) -test_imu_b2.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c - -test_imu_b2.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2 -test_imu_b2.srcs += downlink.c pprz_transport.c - -test_imu_b2.srcs += math/pprz_trig_int.c - -test_imu_b2.CFLAGS += -DIMU_TYPE_H=\"subsystems/imu/imu_b2.h\" -test_imu_b2.CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_MS2100 -DIMU_B2_VERSION_1_1 -test_imu_b2.srcs += $(SRC_SUBSYSTEMS)/imu.c -test_imu_b2.CFLAGS += -DMAX_1168_DRDY_PORT=$(MAX_1168_DRDY_PORT) -test_imu_b2.CFLAGS += -DMAX_1168_DRDY_PORT_SOURCE=$(MAX_1168_DRDY_PORT_SOURCE) -test_imu_b2.CFLAGS += -DUSE_SPI2 -DUSE_DMA1_C4_IRQ -DUSE_EXTI2_IRQ -DUSE_SPI2_IRQ -test_imu_b2.srcs += $(SRC_SUBSYSTEMS)/imu/imu_b2.c $(SRC_ARCH)/subsystems/imu/imu_b2_arch.c -test_imu_b2.srcs += peripherals/max1168.c $(SRC_ARCH)/peripherals/max1168_arch.c -test_imu_b2.srcs += peripherals/ms2100.c $(SRC_ARCH)/peripherals/ms2100_arch.c # -# test IMU b2 1.2 -# -# configuration -# SYS_TIME_LED -# MODEM_PORT -# MODEM_BAUD +# test IMU b2 v1.2 # +IMU_B2_2_CFLAGS = -DIMU_TYPE_H=\"subsystems/imu/imu_b2.h\" +IMU_B2_2_CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_HMC5843 -DIMU_B2_VERSION_1_2 +IMU_B2_2_SRCS = $(SRC_SUBSYSTEMS)/imu.c +IMU_B2_2_CFLAGS += -DMAX_1168_DRDY_PORT=$(MAX_1168_DRDY_PORT) +IMU_B2_2_CFLAGS += -DMAX_1168_DRDY_PORT_SOURCE=$(MAX_1168_DRDY_PORT_SOURCE) +IMU_B2_2_CFLAGS += -DUSE_SPI2 -DUSE_DMA1_C4_IRQ -DUSE_EXTI2_IRQ -DUSE_SPI2_IRQ +IMU_B2_2_SRCS += $(SRC_SUBSYSTEMS)/imu/imu_b2.c $(SRC_ARCH)/subsystems/imu/imu_b2_arch.c +IMU_B2_2_SRCS += peripherals/max1168.c $(SRC_ARCH)/peripherals/max1168_arch.c +IMU_B2_2_CFLAGS += -DUSE_I2C2 +IMU_B2_2_SRCS += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c +IMU_B2_2_SRCS += peripherals/hmc5843.c $(SRC_ARCH)/peripherals/hmc5843_arch.c +IMU_B2_2_CFLAGS += -DUSE_EXTI9_5_IRQ # Mag Int on PB5 + test_imu_b2_2.ARCHDIR = $(ARCH) -test_imu_b2_2.CFLAGS = -I$(SRC_LISA) -I$(ARCH) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) -DPERIPHERALS_AUTO_INIT -test_imu_b2_2.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) -test_imu_b2_2.srcs += $(SRC_AIRBORNE)/mcu.c \ - $(SRC_ARCH)/mcu_arch.c \ - $(SRC_BOOZ_TEST)/booz_test_imu.c \ - $(SRC_ARCH)/stm32_exceptions.c \ - $(SRC_ARCH)/stm32_vector_table.c +test_imu_b2_2.srcs = test/subsystems/test_imu.c +test_imu_b2_2.CFLAGS = $(COMMON_TEST_CFLAGS) +test_imu_b2_2.srcs += $(COMMON_TEST_SRCS) +test_imu_b2_2.CFLAGS += $(IMU_B2_2_CFLAGS) +test_imu_b2_2.srcs += $(IMU_B2_2_SRCS) -test_imu_b2_2.CFLAGS += -DUSE_LED -test_imu_b2_2.srcs += $(SRC_ARCH)/led_hw.c -test_imu_b2_2.CFLAGS += -DUSE_SYS_TIME -test_imu_b2_2.CFLAGS += -DSYS_TIME_LED=$(SYS_TIME_LED) -test_imu_b2_2.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))' -test_imu_b2_2.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c -test_imu_b2_2.CFLAGS += -DUSE_$(MODEM_PORT) -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD) -test_imu_b2_2.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c - -test_imu_b2_2.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2 -test_imu_b2_2.srcs += downlink.c pprz_transport.c - -test_imu_b2_2.srcs += math/pprz_trig_int.c - -test_imu_b2_2.CFLAGS += -DIMU_TYPE_H=\"subsystems/imu/imu_b2.h\" -test_imu_b2_2.CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_HMC5843 -DIMU_B2_VERSION_1_2 -test_imu_b2_2.srcs += $(SRC_SUBSYSTEMS)/imu.c -test_imu_b2_2.CFLAGS += -DMAX_1168_DRDY_PORT=$(MAX_1168_DRDY_PORT) -test_imu_b2_2.CFLAGS += -DMAX_1168_DRDY_PORT_SOURCE=$(MAX_1168_DRDY_PORT_SOURCE) -test_imu_b2_2.CFLAGS += -DUSE_SPI2 -DUSE_DMA1_C4_IRQ -DUSE_EXTI2_IRQ -DUSE_SPI2_IRQ -test_imu_b2_2.srcs += $(SRC_SUBSYSTEMS)/imu/imu_b2.c $(SRC_ARCH)/subsystems/imu/imu_b2_arch.c -test_imu_b2_2.srcs += peripherals/max1168.c $(SRC_ARCH)/peripherals/max1168_arch.c -test_imu_b2_2.CFLAGS += -DUSE_I2C2 -test_imu_b2_2.srcs += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c -test_imu_b2_2.srcs += peripherals/hmc5843.c $(SRC_ARCH)/peripherals/hmc5843_arch.c -test_imu_b2_2.CFLAGS += -DUSE_EXTI9_5_IRQ # Mag Int on PB5 # # test IMU aspirin # +IMU_ASPIRIN_CFLAGS = -DIMU_TYPE_H=\"imu/imu_aspirin.h\" -DIMU_OVERRIDE_CHANNELS +IMU_ASPIRIN_SRCS = $(SRC_SUBSYSTEMS)/imu.c \ + $(SRC_SUBSYSTEMS)/imu/imu_aspirin.c \ + $(SRC_ARCH)/subsystems/imu/imu_aspirin_arch.c +IMU_ASPIRIN_SRCS += peripherals/hmc5843.c $(SRC_ARCH)/peripherals/hmc5843_arch.c +IMU_ASPIRIN_CFLAGS += -DUSE_I2C2 +IMU_ASPIRIN_SRCS += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c +IMU_ASPIRIN_CFLAGS += -DUSE_EXTI15_10_IRQ # Gyro Int on PC14 +IMU_ASPIRIN_CFLAGS += -DUSE_EXTI9_5_IRQ # Mag Int on PB5 +IMU_ASPIRIN_CFLAGS += -DUSE_EXTI2_IRQ # Accel Int on PD2 +IMU_ASPIRIN_CFLAGS += -DUSE_DMA1_C4_IRQ # SPI2 Rx DMA + test_imu_aspirin.ARCHDIR = $(ARCH) -test_imu_aspirin.CFLAGS = -I$(SRC_FIRMWARE) -I$(SRC_LISA) -I$(ARCH) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) -DPERIPHERALS_AUTO_INIT -test_imu_aspirin.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) -test_imu_aspirin.srcs += $(SRC_AIRBORNE)/mcu.c \ - $(SRC_ARCH)/mcu_arch.c \ - $(SRC_BOOZ_TEST)/booz_test_imu.c \ - $(SRC_ARCH)/stm32_exceptions.c \ - $(SRC_ARCH)/stm32_vector_table.c +test_imu_aspirin.srcs = test/subsystems/test_imu.c +test_imu_aspirin.CFLAGS = $(COMMON_TEST_CFLAGS) +test_imu_aspirin.srcs += $(COMMON_TEST_SRCS) +test_imu_aspirin.CFLAGS += $(IMU_ASPIRIN_CFLAGS) +test_imu_aspirin.srcs += $(IMU_ASPIRIN_SRCS) -test_imu_aspirin.CFLAGS += -DUSE_LED -test_imu_aspirin.srcs += $(SRC_ARCH)/led_hw.c -test_imu_aspirin.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=$(SYS_TIME_LED) -test_imu_aspirin.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))' -test_imu_aspirin.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c +# +# test AHRS +# +test_ahrs.ARCHDIR = $(ARCH) +test_ahrs.srcs = test/subsystems/test_ahrs.c +test_ahrs.CFLAGS = $(COMMON_TEST_CFLAGS) +test_ahrs.srcs += $(COMMON_TEST_SRCS) +test_ahrs.CFLAGS += $(IMU_ASPIRIN_CFLAGS) +test_ahrs.srcs += $(IMU_ASPIRIN_SRCS) -test_imu_aspirin.CFLAGS += -DUSE_$(MODEM_PORT) -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD) -test_imu_aspirin.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c +#AHRS = ice +AHRS = icq +#AHRS = flq +#AHRS = fcr +#AHRS = fcr2 +#AHRS = fcq -test_imu_aspirin.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT) -test_imu_aspirin.srcs += downlink.c pprz_transport.c +test_ahrs.srcs += subsystems/ahrs.c \ + subsystems/ahrs/ahrs_aligner.c -test_imu_aspirin.srcs += math/pprz_trig_int.c +ifeq ($(AHRS), ice) +test_ahrs.CFLAGS += -DFACE_REINJ_1=1024 +test_ahrs.CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_euler.h\" +test_ahrs.srcs += subsystems/ahrs/ahrs_int_cmpl_euler.c \ + lisa/plug_sys.c +endif -test_imu_aspirin.CFLAGS += -DIMU_TYPE_H=\"imu/imu_aspirin.h\" -DIMU_OVERRIDE_CHANNELS -test_imu_aspirin.srcs += $(SRC_SUBSYSTEMS)/imu.c \ - $(SRC_SUBSYSTEMS)/imu/imu_aspirin.c \ - $(SRC_ARCH)/subsystems/imu/imu_aspirin_arch.c -test_imu_aspirin.srcs += peripherals/hmc5843.c $(SRC_ARCH)/peripherals/hmc5843_arch.c +ifeq ($(AHRS), icq) +#test_ahrs.CFLAGS += -DAHRS_TYPE=\"ICQ\" +test_ahrs.CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=512 +test_ahrs.CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl.h\" +test_ahrs.srcs +=subsystems/ahrs/ahrs_int_cmpl.c +endif -test_imu_aspirin.CFLAGS += -DUSE_I2C2 -test_imu_aspirin.srcs += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c -test_imu_aspirin.CFLAGS += -DUSE_EXTI15_10_IRQ # Gyro Int on PC14 -test_imu_aspirin.CFLAGS += -DUSE_EXTI9_5_IRQ # Mag Int on PB5 -test_imu_aspirin.CFLAGS += -DUSE_EXTI2_IRQ # Accel Int on PD2 -test_imu_aspirin.CFLAGS += -DUSE_DMA1_C4_IRQ # SPI2 Rx DMA +ifeq ($(AHRS), flq) +test_ahrs.CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_lkf_quat.h\" +test_ahrs.CFLAGS += -DAHRS_H_X=0.51562740288882 -DAHRS_H_Y=-0.05707735220832 -DAHRS_H_Z=0.85490967783446 +test_ahrs.srcs += subsystems/ahrs/ahrs_float_lkf_quat.c +endif +ifeq ($(AHRS), fcr) +test_ahrs.CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_dcm.h\" +test_ahrs.CFLAGS += -DINS_ROLL_NEUTRAL_DEFAULT=0 +test_ahrs.CFLAGS += -DINS_PITCH_NEUTRAL_DEFAULT=0 +test_ahrs.CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=512 +test_ahrs.CFLAGS += -DDCM_UPDATE_AFTER_PROPAGATE +test_ahrs.srcs += subsystems/ahrs/ahrs_float_dcm.c +endif + +ifeq ($(AHRS), fcr2) +test_ahrs.CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_rmat.h\" +test_ahrs.CFLAGS += -DAHRS_H_X=0.51562740288882 -DAHRS_H_Y=-0.05707735220832 -DAHRS_H_Z=0.85490967783446 +test_ahrs.CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=512 +test_ahrs.srcs += subsystems/ahrs/ahrs_float_cmpl_rmat.c +endif + +ifeq ($(AHRS), fcq) +test_ahrs.CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_rmat.h\" +test_ahrs.CFLAGS += -DAHRS_H_X=0.51562740288882 -DAHRS_H_Y=-0.05707735220832 -DAHRS_H_Z=0.85490967783446 +test_ahrs.CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=512 +test_ahrs.srcs += subsystems/ahrs/ahrs_float_cmpl_quat.c +endif @@ -663,65 +700,74 @@ test_bmp085.srcs += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c + # -# Test manual : a simple test with rc and servos - I want to fly lisa/M +# tunnel sw # -test_manual.ARCHDIR = $(ARCH) -test_manual.CFLAGS = -I$(SRC_FIRMWARE) -I$(ARCH) -DPERIPHERALS_AUTO_INIT -test_manual.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) -test_manual.srcs = $(SRC_AIRBORNE)/mcu.c \ - $(SRC_ARCH)/mcu_arch.c \ - test/test_manual.c \ - $(SRC_ARCH)/stm32_exceptions.c \ - $(SRC_ARCH)/stm32_vector_table.c -test_manual.CFLAGS += -DUSE_LED -test_manual.srcs += $(SRC_ARCH)/led_hw.c -test_manual.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=$(SYS_TIME_LED) -test_manual.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)' -test_manual.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c - -test_manual.CFLAGS += -DUSE_$(MODEM_PORT) -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD) -test_manual.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c - -test_manual.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT) -test_manual.srcs += downlink.c pprz_transport.c - -test_manual.srcs += $(SRC_BOOZ)/booz2_commands.c - -test_manual.CFLAGS += -I$(SRC_FIRMWARE)/actuators/arch/$(ARCH) -#test_manual.srcs += $(SRC_FIRMWARE)/actuators/actuators_pwm.c -test_manual.srcs += $(SRC_FIRMWARE)/actuators/arch/$(ARCH)/actuators_pwm_arch.c -test_manual.srcs += $(SRC_FIRMWARE)/actuators/actuators_heli.c +tunnel_sw.ARCHDIR = $(ARCH) +tunnel_sw.CFLAGS += -I$(SRC_LISA) -I$(ARCH) -DPERIPHERALS_AUTO_INIT +tunnel_sw.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) +tunnel_sw.srcs += $(SRC_AIRBORNE)/mcu.c \ + $(SRC_ARCH)/mcu_arch.c \ + $(SRC_LISA)/tunnel_hw.c \ + $(SRC_ARCH)/stm32_exceptions.c \ + $(SRC_ARCH)/stm32_vector_table.c +tunnel_sw.CFLAGS += -DUSE_LED +tunnel_sw.srcs += $(SRC_ARCH)/led_hw.c +tunnel_sw.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=$(SYS_TIME_LED) +tunnel_sw.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)' +tunnel_sw.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c -test_manual.CFLAGS += -I$(SRC_BOOZ) -I$(SRC_BOOZ)/arch/$(ARCH) -test_manual.CFLAGS += -DRADIO_CONTROL -ifdef RADIO_CONTROL_LED -test_manual.CFLAGS += -DRADIO_CONTROL_LED=$(RADIO_CONTROL_LED) -endif -test_manual.CFLAGS += -DRADIO_CONTROL_BIND_IMPL_FUNC=radio_control_spektrum_try_bind -test_manual.CFLAGS += -DRADIO_CONTROL_TYPE_H=\"subsystems/radio_control/spektrum.h\" -test_manual.CFLAGS += -DRADIO_CONTROL_SPEKTRUM_PRIMARY_PORT=$(RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT) -test_manual.CFLAGS += -DOVERRIDE_$(RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT)_IRQ_HANDLER -DUSE_TIM6_IRQ -test_manual.srcs += $(SRC_SUBSYSTEMS)/radio_control.c \ - subsystems/radio_control/spektrum.c \ - $(SRC_ARCH)/subsystems/radio_control/spektrum_arch.c +# +# tunnel hw +# +tunnel_hw.ARCHDIR = $(ARCH) +tunnel_hw.CFLAGS += -I$(SRC_LISA) -I$(ARCH) -DPERIPHERALS_AUTO_INIT +tunnel_hw.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) +tunnel_hw.srcs += lisa/test/lisa_tunnel.c \ + $(SRC_AIRBORNE)/mcu.c \ + $(SRC_ARCH)/mcu_arch.c \ + $(SRC_ARCH)/stm32_exceptions.c \ + $(SRC_ARCH)/stm32_vector_table.c +tunnel_hw.CFLAGS += -DUSE_LED +tunnel_hw.srcs += $(SRC_ARCH)/led_hw.c +tunnel_hw.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=$(SYS_TIME_LED) +tunnel_hw.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)' +tunnel_hw.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c +tunnel_hw.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600 +tunnel_hw.CFLAGS += -DUSE_UART2 -DUART2_BAUD=B57600 +tunnel_hw.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c # -# tunnel +# test_settings : # -tunnel.ARCHDIR = $(ARCH) -tunnel.CFLAGS += -I$(SRC_LISA) -I$(ARCH) -DPERIPHERALS_AUTO_INIT -tunnel.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) -tunnel.srcs += $(SRC_AIRBORNE)/mcu.c \ - $(SRC_ARCH)/mcu_arch.c \ - $(SRC_LISA)/tunnel_hw.c \ - $(SRC_ARCH)/stm32_exceptions.c \ - $(SRC_ARCH)/stm32_vector_table.c -tunnel.CFLAGS += -DUSE_LED -tunnel.srcs += $(SRC_ARCH)/led_hw.c -tunnel.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=$(SYS_TIME_LED) -tunnel.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)' -tunnel.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c +# configuration +# MODEM_PORT : +# MODEM_BAUD : +# +test_settings.ARCHDIR = $(ARCH) +test_settings.CFLAGS += -I$(SRC_LISA) -I$(SRC_ARCH) -DPERIPHERALS_AUTO_INIT +test_settings.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) +test_settings.srcs = test/subsystems/test_settings.c \ + $(SRC_AIRBORNE)/mcu.c \ + $(SRC_ARCH)/mcu_arch.c \ + $(SRC_ARCH)/stm32_exceptions.c \ + $(SRC_ARCH)/stm32_vector_table.c +test_settings.CFLAGS += -DUSE_LED +test_settings.srcs += $(SRC_ARCH)/led_hw.c +test_settings.CFLAGS += -DUSE_SYS_TIME +test_settings.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)' +test_settings.CFLAGS += -DSYS_TIME_LED=$(SYS_TIME_LED) +test_settings.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c +test_settings.CFLAGS += -DUSE_$(MODEM_PORT) +test_settings.CFLAGS += -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD) +test_settings.srcs += downlink.c pprz_transport.c +test_settings.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT) +test_settings.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c +test_settings.CFLAGS += -DDATALINK=PPRZ -DPPRZ_UART=$(MODEM_PORT) +test_settings.srcs += subsystems/settings.c +test_settings.srcs += $(SRC_ARCH)/subsystems/settings_arch.c +test_settings.CFLAGS += -DUSE_PERSISTENT_SETTINGS diff --git a/conf/autopilot/lisa_m_test_progs.makefile b/conf/autopilot/lisa_m_test_progs.makefile index bc4448dfaa..9fa3bd820f 100644 --- a/conf/autopilot/lisa_m_test_progs.makefile +++ b/conf/autopilot/lisa_m_test_progs.makefile @@ -92,6 +92,7 @@ test_uart_lisam.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c test_uart_lisam.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600 test_uart_lisam.CFLAGS += -DUSE_UART2 -DUART2_BAUD=B57600 test_uart_lisam.CFLAGS += -DUSE_UART3 -DUART3_BAUD=B57600 +test_uart_lisam.CFLAGS += -DUSE_UART5 -DUART5_BAUD=B57600 test_uart_lisam.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c @@ -101,22 +102,22 @@ test_uart_lisam.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c # #SRC_BOOZ_ARCH=$(SRC_BOOZ)/arch/$(ARCH) # -#test_servos.ARCHDIR = $(ARCH) -#test_servos.CFLAGS = -I$(SRC_FIRMWARE)/actuators/arch/$(ARCH) -I$(SRC_LISA) -I$(ARCH) -DPERIPHERALS_AUTO_INIT -#test_servos.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) -#test_servos.LDFLAGS += -lm -#test_servos.srcs += $(SRC_AIRBORNE)/mcu.c \ -# $(SRC_ARCH)/mcu_arch.c \ -# $(SRC_LISA)/test_servos.c \ -# $(SRC_ARCH)/stm32_exceptions.c \ -# $(SRC_ARCH)/stm32_vector_table.c -#test_servos.CFLAGS += -DUSE_LED -#test_servos.srcs += $(SRC_ARCH)/led_hw.c -#test_servos.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=$(SYS_TIME_LED) -#test_servos.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))' -#test_servos.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c -# -#test_servos.srcs += $(SRC_FIRMWARE)/actuators/actuators_pwm.c $(SRC_FIRMWARE)/actuators/arch/$(ARCH)/actuators_pwm_arch.c +test_servos.ARCHDIR = $(ARCH) +test_servos.CFLAGS = -I$(SRC_FIRMWARE)/actuators/arch/$(ARCH) -I$(SRC_LISA) -I$(ARCH) -DPERIPHERALS_AUTO_INIT +test_servos.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) +test_servos.LDFLAGS += -lm +test_servos.srcs += $(SRC_AIRBORNE)/mcu.c \ + $(SRC_ARCH)/mcu_arch.c \ + $(SRC_LISA)/test_servos.c \ + $(SRC_ARCH)/stm32_exceptions.c \ + $(SRC_ARCH)/stm32_vector_table.c +test_servos.CFLAGS += -DUSE_LED +test_servos.srcs += $(SRC_ARCH)/led_hw.c +test_servos.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=$(SYS_TIME_LED) -DUSE_SERVOS_7AND8 +test_servos.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))' +test_servos.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c + +test_servos.srcs += $(SRC_FIRMWARE)/actuators/actuators_pwm.c $(SRC_FIRMWARE)/actuators/arch/$(ARCH)/actuators_pwm_arch.c # # ## @@ -126,25 +127,25 @@ test_uart_lisam.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c ## MODEM_PORT : ## MODEM_BAUD : ## -#test_telemetry.ARCHDIR = $(ARCH) -#test_telemetry.CFLAGS += -I$(SRC_LISA) -I$(SRC_ARCH) -DPERIPHERALS_AUTO_INIT -#test_telemetry.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) -#test_telemetry.srcs = $(SRC_AIRBORNE)/mcu.c \ -# $(SRC_ARCH)/mcu_arch.c \ -# test/test_telemetry.c \ -# $(SRC_ARCH)/stm32_exceptions.c \ -# $(SRC_ARCH)/stm32_vector_table.c -#test_telemetry.CFLAGS += -DUSE_LED -#test_telemetry.srcs += $(SRC_ARCH)/led_hw.c -#test_telemetry.CFLAGS += -DUSE_SYS_TIME -#test_telemetry.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)' -#test_telemetry.CFLAGS += -DSYS_TIME_LED=$(SYS_TIME_LED) -#test_telemetry.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c -#test_telemetry.CFLAGS += -DUSE_$(MODEM_PORT) -#test_telemetry.CFLAGS += -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD) -#test_telemetry.srcs += downlink.c pprz_transport.c -#test_telemetry.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT) -#test_telemetry.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c +test_telemetry.ARCHDIR = $(ARCH) +test_telemetry.CFLAGS += -I$(SRC_LISA) -I$(SRC_ARCH) -DPERIPHERALS_AUTO_INIT +test_telemetry.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) +test_telemetry.srcs = $(SRC_AIRBORNE)/mcu.c \ + $(SRC_ARCH)/mcu_arch.c \ + test/test_telemetry.c \ + $(SRC_ARCH)/stm32_exceptions.c \ + $(SRC_ARCH)/stm32_vector_table.c +test_telemetry.CFLAGS += -DUSE_LED +test_telemetry.srcs += $(SRC_ARCH)/led_hw.c +test_telemetry.CFLAGS += -DUSE_SYS_TIME +test_telemetry.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)' +test_telemetry.CFLAGS += -DSYS_TIME_LED=$(SYS_TIME_LED) +test_telemetry.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c +test_telemetry.CFLAGS += -DUSE_$(MODEM_PORT) +test_telemetry.CFLAGS += -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD) +test_telemetry.srcs += downlink.c pprz_transport.c +test_telemetry.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT) +test_telemetry.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c # # ## @@ -155,27 +156,27 @@ test_uart_lisam.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c ## MODEM_PORT ## MODEM_BAUD ## -#test_baro.ARCHDIR = $(ARCH) -#test_baro.CFLAGS = -I$(SRC_LISA) -I$(SRC_ARCH) -I$(SRC_BOARD) -DPERIPHERALS_AUTO_INIT -#test_baro.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) -#test_baro.srcs = $(SRC_AIRBORNE)/mcu.c \ -# $(SRC_ARCH)/mcu_arch.c \ -# $(SRC_BOARD)/test_baro.c \ -# $(SRC_ARCH)/stm32_exceptions.c \ -# $(SRC_ARCH)/stm32_vector_table.c -#test_baro.CFLAGS += -DUSE_LED -#test_baro.srcs += $(SRC_ARCH)/led_hw.c -#test_baro.CFLAGS += -DUSE_SYS_TIME -#test_baro.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)' -#test_baro.CFLAGS += -DSYS_TIME_LED=$(SYS_TIME_LED) -#test_baro.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c -#test_baro.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT) -#test_baro.srcs += downlink.c pprz_transport.c -#test_baro.CFLAGS += -DUSE_$(MODEM_PORT) -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD) -#test_baro.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c -#test_baro.srcs += $(SRC_BOARD)/baro_board.c -#test_baro.CFLAGS += -DUSE_I2C2 -#test_baro.srcs += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c +test_baro.ARCHDIR = $(ARCH) +test_baro.CFLAGS = -I$(SRC_LISA) -I$(SRC_ARCH) -I$(SRC_BOARD) -DPERIPHERALS_AUTO_INIT +test_baro.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) +test_baro.srcs = $(SRC_AIRBORNE)/mcu.c \ + $(SRC_ARCH)/mcu_arch.c \ + $(SRC_BOARD)/test_baro.c \ + $(SRC_ARCH)/stm32_exceptions.c \ + $(SRC_ARCH)/stm32_vector_table.c +test_baro.CFLAGS += -DUSE_LED +test_baro.srcs += $(SRC_ARCH)/led_hw.c +test_baro.CFLAGS += -DUSE_SYS_TIME +test_baro.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)' +test_baro.CFLAGS += -DSYS_TIME_LED=$(SYS_TIME_LED) +test_baro.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c +test_baro.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT) +test_baro.srcs += downlink.c pprz_transport.c +test_baro.CFLAGS += -DUSE_$(MODEM_PORT) -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD) +test_baro.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c +test_baro.srcs += $(SRC_BOARD)/baro_board.c +test_baro.CFLAGS += -DUSE_I2C2 +test_baro.srcs += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c # # ## @@ -188,39 +189,41 @@ test_uart_lisam.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c ## RADIO_CONTROL_LED ## RADIO_CONROL_SPEKTRUM_PRIMARY_PORT ## -#test_rc_spektrum.ARCHDIR = $(ARCH) -# -#test_rc_spektrum.CFLAGS += -I$(SRC_ARCH) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) -DPERIPHERALS_AUTO_INIT -#test_rc_spektrum.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) -#test_rc_spektrum.srcs += $(SRC_AIRBORNE)/mcu.c \ -# $(SRC_ARCH)/mcu_arch.c \ -# $(SRC_BOOZ_TEST)/booz2_test_radio_control.c \ -# $(SRC_ARCH)/stm32_exceptions.c \ -# $(SRC_ARCH)/stm32_vector_table.c -# -#test_rc_spektrum.CFLAGS += -DUSE_LED -#test_rc_spektrum.srcs += $(SRC_ARCH)/led_hw.c -#test_rc_spektrum.CFLAGS += -DUSE_SYS_TIME -#test_rc_spektrum.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))' -#test_rc_spektrum.CFLAGS += -DSYS_TIME_LED=$(SYS_TIME_LED) -#test_rc_spektrum.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c -#test_rc_spektrum.CFLAGS += -DUSE_$(MODEM_PORT) -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD) -#test_rc_spektrum.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c -#test_rc_spektrum.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT) -#test_rc_spektrum.srcs += downlink.c pprz_transport.c -#test_rc_spektrum.CFLAGS += -DRADIO_CONTROL -#ifdef RADIO_CONTROL_LED -#test_rc_spektrum.CFLAGS += -DRADIO_CONTROL_LED=$(RADIO_CONTROL_LED) -#endif -#test_rc_spektrum.CFLAGS += -DRADIO_CONTROL_BIND_IMPL_FUNC=radio_control_spektrum_try_bind -#test_rc_spektrum.CFLAGS += -DRADIO_CONTROL_TYPE_H=\"subsystems/radio_control/spektrum.h\" -#test_rc_spektrum.CFLAGS += -DRADIO_CONTROL_SPEKTRUM_PRIMARY_PORT=$(RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT) -#test_rc_spektrum.CFLAGS += -DOVERRIDE_$(RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT)_IRQ_HANDLER -DUSE_TIM6_IRQ -#test_rc_spektrum.srcs += $(SRC_SUBSYSTEMS)/radio_control.c \ -# subsystems/radio_control/spektrum.c \ -# $(SRC_ARCH)/subsystems/radio_control/spektrum_arch.c -# -# +test_rc_spektrum.ARCHDIR = $(ARCH) + +test_rc_spektrum.CFLAGS += -I$(SRC_ARCH) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) -DPERIPHERALS_AUTO_INIT +test_rc_spektrum.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) +test_rc_spektrum.srcs += $(SRC_AIRBORNE)/mcu.c \ + $(SRC_ARCH)/mcu_arch.c \ + booz/test/booz2_test_radio_control.c \ + $(SRC_ARCH)/stm32_exceptions.c \ + $(SRC_ARCH)/stm32_vector_table.c + +test_rc_spektrum.CFLAGS += -DUSE_LED +test_rc_spektrum.srcs += $(SRC_ARCH)/led_hw.c +test_rc_spektrum.CFLAGS += -DUSE_SYS_TIME +test_rc_spektrum.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))' +test_rc_spektrum.CFLAGS += -DSYS_TIME_LED=$(SYS_TIME_LED) +test_rc_spektrum.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c +test_rc_spektrum.CFLAGS += -DUSE_$(MODEM_PORT) -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD) +test_rc_spektrum.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c +test_rc_spektrum.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT) +test_rc_spektrum.srcs += downlink.c pprz_transport.c +test_rc_spektrum.CFLAGS += -DRADIO_CONTROL +ifdef RADIO_CONTROL_LED +test_rc_spektrum.CFLAGS += -DRADIO_CONTROL_LED=$(RADIO_CONTROL_LED) +endif +test_rc_spektrum.CFLAGS += -DRADIO_CONTROL_BIND_IMPL_FUNC=radio_control_spektrum_try_bind +test_rc_spektrum.CFLAGS += -DRADIO_CONTROL_TYPE_H=\"subsystems/radio_control/spektrum.h\" +test_rc_spektrum.CFLAGS += -DRADIO_CONTROL_SPEKTRUM_PRIMARY_PORT=$(RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT) +test_rc_spektrum.CFLAGS += -DRADIO_CONTROL_SPEKTRUM_SECONDARY_PORT=$(RADIO_CONTROL_SPEKTRUM_SECONDARY_PORT) +test_rc_spektrum.CFLAGS += -DOVERRIDE_$(RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT)_IRQ_HANDLER -DUSE_TIM6_IRQ +test_rc_spektrum.CFLAGS += -DOVERRIDE_$(RADIO_CONTROL_SPEKTRUM_SECONDARY_PORT)_IRQ_HANDLER +test_rc_spektrum.srcs += $(SRC_SUBSYSTEMS)/radio_control.c \ + subsystems/radio_control/spektrum.c \ + $(SRC_ARCH)/subsystems/radio_control/spektrum_arch.c + + ## ## test_rc_ppm ## @@ -388,52 +391,45 @@ test_uart_lisam.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c # # ## -## test IMU aspirin +## test_imu_aspirin : test aspirin imu ## -#test_imu_aspirin.ARCHDIR = $(ARCH) -#test_imu_aspirin.CFLAGS = -I$(SRC_FIRMWARE) -I$(SRC_LISA) -I$(ARCH) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) -DPERIPHERALS_AUTO_INIT -#test_imu_aspirin.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) -#test_imu_aspirin.srcs += $(SRC_AIRBORNE)/mcu.c \ -# $(SRC_ARCH)/mcu_arch.c \ -# $(SRC_BOOZ_TEST)/booz_test_imu.c \ -# $(SRC_ARCH)/stm32_exceptions.c \ -# $(SRC_ARCH)/stm32_vector_table.c -# -#test_imu_aspirin.CFLAGS += -DUSE_LED -#test_imu_aspirin.srcs += $(SRC_ARCH)/led_hw.c -# -#test_imu_aspirin.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1 -#test_imu_aspirin.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))' -#test_imu_aspirin.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c -# -#test_imu_aspirin.CFLAGS += -DUSE_UART2 -DUART2_BAUD=B57600 -#test_imu_aspirin.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c -# -#test_imu_aspirin.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2 -#test_imu_aspirin.srcs += downlink.c pprz_transport.c -# -#test_imu_aspirin.srcs += math/pprz_trig_int.c -# -#test_imu_aspirin.CFLAGS += -DIMU_TYPE_H=\"imu/imu_aspirin.h\" -DIMU_OVERRIDE_CHANNELS -#test_imu_aspirin.srcs += $(SRC_SUBSYSTEMS)/imu.c \ -# $(SRC_SUBSYSTEMS)/imu/imu_aspirin.c \ -# $(SRC_ARCH)/subsystems/imu/imu_aspirin_arch.c -#test_imu_aspirin.srcs += peripherals/hmc5843.c $(SRC_ARCH)/peripherals/hmc5843_arch.c -# -#test_imu_aspirin.CFLAGS += -DUSE_I2C2 -#test_imu_aspirin.srcs += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c -#test_imu_aspirin.CFLAGS += -DUSE_EXTI15_10_IRQ # Gyro Int on PC14 -#test_imu_aspirin.CFLAGS += -DUSE_EXTI9_5_IRQ # Mag Int on PB5 -#test_imu_aspirin.CFLAGS += -DUSE_EXTI2_IRQ # Accel Int on PD2 -#test_imu_aspirin.CFLAGS += -DUSE_DMA1_C4_IRQ # SPI2 Rx DMA -# -# -# -# -# -# -# -# +## configuration +## MODEM_PORT : +## MODEM_BAUD : +## +test_imu_aspirin.ARCHDIR = $(ARCH) +test_imu_aspirin.CFLAGS += -I$(SRC_LISA) -I$(SRC_ARCH) -DPERIPHERALS_AUTO_INIT +test_imu_aspirin.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) +test_imu_aspirin.srcs = $(SRC_AIRBORNE)/mcu.c \ + $(SRC_ARCH)/mcu_arch.c \ + $(SRC_ARCH)/stm32_exceptions.c \ + $(SRC_ARCH)/stm32_vector_table.c\ + booz/test/booz_test_imu.c + +test_imu_aspirin.CFLAGS += -DUSE_LED +test_imu_aspirin.srcs += $(SRC_ARCH)/led_hw.c +test_imu_aspirin.CFLAGS += -DUSE_SYS_TIME +test_imu_aspirin.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)' +test_imu_aspirin.CFLAGS += -DSYS_TIME_LED=$(SYS_TIME_LED) +test_imu_aspirin.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c +test_imu_aspirin.CFLAGS += -DUSE_$(MODEM_PORT) +test_imu_aspirin.CFLAGS += -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD) +test_imu_aspirin.srcs += downlink.c pprz_transport.c +test_imu_aspirin.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT) +test_imu_aspirin.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c +test_imu_aspirin.srcs += math/pprz_trig_int.c +test_imu_aspirin.CFLAGS += -DIMU_TYPE_H=\"imu/imu_aspirin.h\" -DIMU_OVERRIDE_CHANNELS +test_imu_aspirin.srcs += $(SRC_SUBSYSTEMS)/imu.c \ + $(SRC_SUBSYSTEMS)/imu/imu_aspirin.c \ + $(SRC_ARCH)/subsystems/imu/imu_aspirin_arch.c +test_imu_aspirin.srcs += peripherals/hmc5843.c $(SRC_ARCH)/peripherals/hmc5843_arch.c + +test_imu_aspirin.CFLAGS += -DUSE_I2C2 +test_imu_aspirin.srcs += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c +test_imu_aspirin.CFLAGS += -DUSE_EXTI15_10_IRQ # Gyro Int on PC14 +test_imu_aspirin.CFLAGS += -DUSE_EXTI9_5_IRQ # Mag Int on PB5 +test_imu_aspirin.CFLAGS += -DUSE_EXTI2_IRQ # Accel Int on PD2 +test_imu_aspirin.CFLAGS += -DUSE_DMA1_C4_IRQ # SPI2 Rx DMA # ## ## test hmc5843 diff --git a/conf/autopilot/rotorcraft.makefile b/conf/autopilot/rotorcraft.makefile index 8966f4bed8..c3506ccafa 100644 --- a/conf/autopilot/rotorcraft.makefile +++ b/conf/autopilot/rotorcraft.makefile @@ -93,6 +93,8 @@ endif # or # include subsystems/rotorcraft/telemetry_xbee_api.makefile # +ap.srcs += subsystems/settings.c +ap.srcs += $(SRC_ARCH)/subsystems/settings_arch.c ap.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c # I2C is needed for speed controllers and barometers on lisa @@ -134,7 +136,7 @@ ap.srcs += $(SRC_BOOZ)/booz2_commands.c # ap.srcs += $(SRC_BOARD)/baro_board.c ifeq ($(BOARD), booz) -ap.CFLAGS += -DROTORCRAFT_BARO_LED=$(BARO_LED) -DBOOZ2_ANALOG_BARO_PERIOD='SYS_TICS_OF_SEC((1./100.))' +ap.CFLAGS += -DROTORCRAFT_BARO_LED=$(BARO_LED) else ifeq ($(BOARD), lisa_l) ap.CFLAGS += -DUSE_I2C2 endif @@ -142,18 +144,26 @@ endif # # Analog Backend # + ifeq ($(ARCH), lpc21) -ap.CFLAGS += -DBOOZ2_ANALOG_BATTERY_PERIOD='SYS_TICS_OF_SEC((1./10.))' -ap.srcs += $(SRC_FIRMWARE)/battery.c ap.CFLAGS += -DADC0_VIC_SLOT=2 ap.CFLAGS += -DADC1_VIC_SLOT=3 -ap.srcs += $(SRC_BOOZ)/booz2_analog.c \ - $(SRC_BOOZ_ARCH)/booz2_analog_hw.c +ap.CFLAGS += -DUSE_ADC +ap.srcs += $(SRC_ARCH)/mcu_periph/adc_arch.c +ap.srcs += subsystems/electrical.c +# baro has variable offset amplifier on booz board +ap.CFLAGS += -DUSE_DAC +ap.srcs += $(SRC_ARCH)/mcu_periph/dac_arch.c else ifeq ($(ARCH), stm32) -ap.srcs += lisa/lisa_analog_plug.c +ap.CFLAGS += -DUSE_ADC +ap.CFLAGS += -DUSE_AD1 -DUSE_AD1_1 -DUSE_AD1_2 -DUSE_AD1_3 -DUSE_AD1_4 +ap.CFLAGS += -DUSE_ADC1_2_IRQ_HANDLER +ap.srcs += $(SRC_ARCH)/mcu_periph/adc_arch.c +ap.srcs += subsystems/electrical.c endif + # # GPS choice # diff --git a/conf/autopilot/setup.makefile b/conf/autopilot/setup.makefile index b71b547167..bd1757dbb9 100644 --- a/conf/autopilot/setup.makefile +++ b/conf/autopilot/setup.makefile @@ -23,6 +23,7 @@ tunnel.srcs += mcu.c $(SRC_ARCH)/mcu_arch.c # for the usb_tunnel we need to set PCLK higher with the flag USE_USB_HIGH_PCLK # a configuration program to access both uart through usb +ifeq ($(ARCH), lpc21) usb_tunnel_0.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B115200 -DPERIPHERALS_AUTO_INIT usb_tunnel_0.CFLAGS += -DUSE_USB_LINE_CODING -DUSE_USB_SERIAL -DUSE_LED -DUSE_USB_HIGH_PCLK usb_tunnel_0.srcs += $(SRC_ARCH)/usb_tunnel.c $(SRC_ARCH)/usb_ser_hw.c $(SRC_ARCH)/mcu_periph/uart_arch.c @@ -38,6 +39,9 @@ usb_tunnel_1.srcs += $(SRC_ARCH)/lpcusb/usbhw_lpc.c $(SRC_ARCH)/lpcusb/usbinit.c usb_tunnel_1.srcs += $(SRC_ARCH)/lpcusb/usbcontrol.c $(SRC_ARCH)/lpcusb/usbstdreq.c usb_tunnel_1.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c usb_tunnel_1.srcs += mcu.c $(SRC_ARCH)/mcu_arch.c +else +$(error usb_tunnel currently only implemented for the lpc21) +endif diff --git a/conf/autopilot/subsystems/fixedwing/autopilot.makefile b/conf/autopilot/subsystems/fixedwing/autopilot.makefile index fe9ba0a275..4881ed55f9 100644 --- a/conf/autopilot/subsystems/fixedwing/autopilot.makefile +++ b/conf/autopilot/subsystems/fixedwing/autopilot.makefile @@ -142,6 +142,7 @@ ns_srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c #ifeq ($(ARCH), lpc21) ns_srcs += $(SRC_ARCH)/mcu_periph/adc_arch.c ifeq ($(ARCH), stm32) + ns_CFLAGS += -DUSE_AD1 -DUSE_AD1_1 -DUSE_AD1_2 -DUSE_AD1_3 -DUSE_AD1_4 ns_CFLAGS += -DUSE_ADC1_2_IRQ_HANDLER endif diff --git a/conf/autopilot/subsystems/fixedwing/control_new.makefile b/conf/autopilot/subsystems/fixedwing/control_new.makefile index 267b9eff8f..db21505cbc 100644 --- a/conf/autopilot/subsystems/fixedwing/control_new.makefile +++ b/conf/autopilot/subsystems/fixedwing/control_new.makefile @@ -1,4 +1,4 @@ # new fixed wing control loops with merged auto pitch and auto throttle, adaptive horizontal control +$(TARGET).srcs += $(SRC_FIRMWARE)/stabilization/stabilization_adaptive.c $(SRC_FIRMWARE)/guidance/guidance_v_n.c -$(TARGET).srcs += $(SRC_FIXEDWING)/fw_h_ctl_a.c $(SRC_FIXEDWING)/fw_v_ctl_n.c diff --git a/conf/autopilot/subsystems/fixedwing/telemetry_transparent_usb.makefile b/conf/autopilot/subsystems/fixedwing/telemetry_transparent_usb.makefile index fdc2d31baf..4221fce81d 100644 --- a/conf/autopilot/subsystems/fixedwing/telemetry_transparent_usb.makefile +++ b/conf/autopilot/subsystems/fixedwing/telemetry_transparent_usb.makefile @@ -1,9 +1,15 @@ #serial USB (e.g. /dev/ttyACM0) +ifeq ($(ARCH), lpc21) ap.CFLAGS += -DDOWNLINK -DDOWNLINK_FBW_DEVICE=UsbS -DDOWNLINK_AP_DEVICE=UsbS -DPPRZ_UART=UsbS ap.CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=PPRZ -DUSE_USB_SERIAL -DUSE_USB_HIGH_PCLK ap.srcs += $(SRC_FIXEDWING)/downlink.c $(SRC_FIRMWARE)/datalink.c $(SRC_FIXEDWING)/pprz_transport.c ap.srcs += $(SRC_ARCH)/usb_ser_hw.c $(SRC_ARCH)/lpcusb/usbhw_lpc.c $(SRC_ARCH)/lpcusb/usbcontrol.c ap.srcs += $(SRC_ARCH)/lpcusb/usbstdreq.c $(SRC_ARCH)/lpcusb/usbinit.c +else +ifneq ($(ARCH), sim) +$(error telemetry_transparent_usb currently only implemented for the lpc21) +endif +endif diff --git a/conf/autopilot/subsystems/lisa_passthrough/ahrs_cmpl.makefile b/conf/autopilot/subsystems/lisa_passthrough/ahrs_cmpl.makefile index f13e52aeba..423f48203e 100644 --- a/conf/autopilot/subsystems/lisa_passthrough/ahrs_cmpl.makefile +++ b/conf/autopilot/subsystems/lisa_passthrough/ahrs_cmpl.makefile @@ -2,7 +2,7 @@ # Complementary filter for attitude estimation # -ap.CFLAGS += -DUSE_AHRS_CMPL -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) -DAHRS_FIXED_POINT +ap.CFLAGS += -DUSE_AHRS_CMPL -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) stm_passthrough.srcs += $(SRC_SUBSYSTEMS)/ahrs.c stm_passthrough.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c stm_passthrough.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_int_cmpl_euler.c diff --git a/conf/autopilot/subsystems/rotorcraft/actuators_asctec.makefile b/conf/autopilot/subsystems/rotorcraft/actuators_asctec.makefile index be29427092..e31987e894 100644 --- a/conf/autopilot/subsystems/rotorcraft/actuators_asctec.makefile +++ b/conf/autopilot/subsystems/rotorcraft/actuators_asctec.makefile @@ -14,7 +14,7 @@ endif # Simulator sim.srcs += $(SRC_FIRMWARE)/actuators/actuators_asctec.c -sim.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=150 -DI2C0_SCLH=150 -DI2C0_VIC_SLOT=10 +sim.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=150 -DI2C0_SCLH=150 -DI2C0_VIC_SLOT=11 sim.srcs += mcu_periph/i2c.c sim.srcs += $(SRC_ARCH)/mcu_periph/i2c_arch.c diff --git a/conf/autopilot/subsystems/rotorcraft/actuators_asctec_v2.makefile b/conf/autopilot/subsystems/rotorcraft/actuators_asctec_v2.makefile index 6b9d300c2e..30451deebb 100644 --- a/conf/autopilot/subsystems/rotorcraft/actuators_asctec_v2.makefile +++ b/conf/autopilot/subsystems/rotorcraft/actuators_asctec_v2.makefile @@ -22,7 +22,7 @@ ap.srcs += $(SRC_FIRMWARE)/actuators/actuators_asctec.c ifeq ($(ARCH), lpc21) ap.CFLAGS += -DACTUATORS_ASCTEC_DEVICE=i2c0 -ap.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=150 -DI2C0_SCLH=150 -DI2C0_VIC_SLOT=10 +ap.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=150 -DI2C0_SCLH=150 -DI2C0_VIC_SLOT=11 endif ifeq ($(ARCH), stm32) diff --git a/conf/autopilot/subsystems/rotorcraft/actuators_dummy.makefile b/conf/autopilot/subsystems/rotorcraft/actuators_dummy.makefile new file mode 100644 index 0000000000..5c6b93839a --- /dev/null +++ b/conf/autopilot/subsystems/rotorcraft/actuators_dummy.makefile @@ -0,0 +1,5 @@ +# +# empty dummy actuators for testing +# + +ap.srcs += $(SRC_FIRMWARE)/actuators/actuators_dummy.c diff --git a/conf/autopilot/subsystems/rotorcraft/actuators_mkk.makefile b/conf/autopilot/subsystems/rotorcraft/actuators_mkk.makefile index 3529c03923..a55d0e4313 100644 --- a/conf/autopilot/subsystems/rotorcraft/actuators_mkk.makefile +++ b/conf/autopilot/subsystems/rotorcraft/actuators_mkk.makefile @@ -41,7 +41,7 @@ ap.srcs += $(SRC_FIRMWARE)/actuators/actuators_mkk.c ifeq ($(ARCH), lpc21) ap.CFLAGS += -DACTUATORS_MKK_DEVICE=i2c0 -ap.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=$(MKK_I2C_SCL_TIME) -DI2C0_SCLH=$(MKK_I2C_SCL_TIME) -DI2C0_VIC_SLOT=10 +ap.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=$(MKK_I2C_SCL_TIME) -DI2C0_SCLH=$(MKK_I2C_SCL_TIME) -DI2C0_VIC_SLOT=11 else ifeq ($(ARCH), stm32) ap.CFLAGS += -DACTUATORS_MKK_DEVICE=i2c1 ap.CFLAGS += -DUSE_I2C1 diff --git a/conf/autopilot/subsystems/rotorcraft/ahrs_cmpl.makefile b/conf/autopilot/subsystems/rotorcraft/ahrs_cmpl.makefile index 9482514737..457f08f575 100644 --- a/conf/autopilot/subsystems/rotorcraft/ahrs_cmpl.makefile +++ b/conf/autopilot/subsystems/rotorcraft/ahrs_cmpl.makefile @@ -3,15 +3,14 @@ # ifdef AHRS_ALIGNER_LED -ap.CFLAGS += -DUSE_AHRS_CMPL -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) -DAHRS_FIXED_POINT -else -ap.CFLAGS += -DUSE_AHRS_CMPL -DAHRS_FIXED_POINT +ap.CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) endif +ap.CFLAGS += -DUSE_AHRS_CMPL ap.srcs += $(SRC_SUBSYSTEMS)/ahrs.c ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_int_cmpl_euler.c -sim.CFLAGS += -DUSE_AHRS_CMPL -DAHRS_ALIGNER_LED=3 -DAHRS_FIXED_POINT +sim.CFLAGS += -DUSE_AHRS_CMPL -DAHRS_ALIGNER_LED=3 sim.srcs += $(SRC_SUBSYSTEMS)/ahrs.c sim.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c sim.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_int_cmpl_euler.c diff --git a/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile b/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile index 1cd8ef6246..592c196ea3 100644 --- a/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile +++ b/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile @@ -22,7 +22,7 @@ sim.ARCHDIR = $(ARCH) sim.CFLAGS += -DSITL -DNPS sim.CFLAGS += `pkg-config glib-2.0 --cflags` -I /usr/include/meschach -sim.LDFLAGS += `pkg-config glib-2.0 --libs` -lm -lmeschach -lpcre -lglibivy +sim.LDFLAGS += `pkg-config glib-2.0 --libs` -lm -lpcre -lglibivy -lgsl -lgslcblas sim.CFLAGS += -I$(NPSDIR) -I$(SRC_FIRMWARE) -I$(SRC_BOOZ) -I$(SRC_BOOZ_SIM) -I$(SRC_BOARD) -I../simulator -I$(PAPARAZZI_HOME)/conf/simulator/nps # use the paparazzi-jsbsim package if it is installed, otherwise look for JSBsim under /opt/jsbsim @@ -65,7 +65,6 @@ sim.srcs += math/pprz_trig_int.c \ sim.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) -sim.srcs += $(SRC_BOOZ_SIM)/booz2_unsimulated_peripherals.c sim.srcs += firmwares/rotorcraft/main.c sim.srcs += mcu.c sim.srcs += $(SRC_ARCH)/mcu_arch.c @@ -75,6 +74,8 @@ sim.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))' #sim.CFLAGS += -DUSE_LED sim.srcs += sys_time.c +sim.srcs += subsystems/settings.c +sim.srcs += $(SRC_ARCH)/subsystems/settings_arch.c sim.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport sim.srcs += $(SRC_FIRMWARE)/telemetry.c \ @@ -90,13 +91,16 @@ sim.srcs += $(SRC_FIRMWARE)/datalink.c # -sim.CFLAGS += -DBOOZ2_ANALOG_BARO_LED=2 -DBOOZ2_ANALOG_BARO_PERIOD='SYS_TICS_OF_SEC((1./100.))' +sim.CFLAGS += -DROTORCRAFT_BARO_LED=2 sim.srcs += $(SRC_BOARD)/baro_board.c -sim.CFLAGS += -DBOOZ2_ANALOG_BATTERY_PERIOD='SYS_TICS_OF_SEC((1./10.))' -sim.srcs += $(SRC_FIRMWARE)/battery.c +sim.CFLAGS += -DUSE_ADC +sim.srcs += $(SRC_ARCH)/mcu_periph/adc_arch.c +sim.srcs += subsystems/electrical.c +# baro has variable offset amplifier on booz board +#sim.CFLAGS += -DUSE_DAC +#sim.srcs += $(SRC_ARCH)/mcu_periph/dac_arch.c -sim.srcs += $(SRC_BOOZ)/booz2_analog.c $(SRC_BOOZ_SIM)/booz2_analog_hw.c #sim.CFLAGS += -DIMU_TYPE_H=\"imu/imu_b2.h\" #sim.CFLAGS += -DIMU_B2_VERSION_1_1 diff --git a/conf/autopilot/subsystems/rotorcraft/gps_skytraq.makefile b/conf/autopilot/subsystems/rotorcraft/gps_skytraq.makefile index e3f8faf488..9a06840e50 100644 --- a/conf/autopilot/subsystems/rotorcraft/gps_skytraq.makefile +++ b/conf/autopilot/subsystems/rotorcraft/gps_skytraq.makefile @@ -10,10 +10,5 @@ ifneq ($(GPS_LED),none) ap.CFLAGS += -DGPS_LED=$(GPS_LED) endif -ifeq ($(ARCH), lpc21) -ap.CFLAGS += -D$(GPS_PORT)_VIC_SLOT=5 -endif - - sim.CFLAGS += -DUSE_GPS sim.srcs += $(SRC_BOOZ)/booz_gps.c diff --git a/conf/autopilot/subsystems/rotorcraft/gps_ublox.makefile b/conf/autopilot/subsystems/rotorcraft/gps_ublox.makefile index 1e476c2c29..784099f03a 100644 --- a/conf/autopilot/subsystems/rotorcraft/gps_ublox.makefile +++ b/conf/autopilot/subsystems/rotorcraft/gps_ublox.makefile @@ -10,9 +10,5 @@ ifneq ($(GPS_LED),none) ap.CFLAGS += -DGPS_LED=$(GPS_LED) endif -ifeq ($(ARCH), lpc21) -ap.CFLAGS += -D$(GPS_PORT)_VIC_SLOT=5 -endif - sim.CFLAGS += -DUSE_GPS sim.srcs += $(SRC_BOOZ)/booz_gps.c diff --git a/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.0.makefile b/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.0.makefile index f3074ca2bb..0e435f803d 100644 --- a/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.0.makefile +++ b/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.0.makefile @@ -50,7 +50,7 @@ imu_CFLAGS += -DMAX1168_EOC_VIC_SLOT=8 imu_CFLAGS += -DUSE_AMI601 imu_srcs += peripherals/ami601.c -imu_CFLAGS += -DUSE_I2C1 -DI2C1_SCLL=150 -DI2C1_SCLH=150 -DI2C1_VIC_SLOT=11 +imu_CFLAGS += -DUSE_I2C1 -DI2C1_SCLL=150 -DI2C1_SCLH=150 -DI2C1_VIC_SLOT=12 # Keep CFLAGS/Srcs for imu in separate expression so we can assign it to other targets diff --git a/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.1.makefile b/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.1.makefile index 18db231194..afea1237dd 100644 --- a/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.1.makefile +++ b/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.1.makefile @@ -57,7 +57,7 @@ imu_srcs += $(SRC_ARCH)/peripherals/ms2100_arch.c ifeq ($(ARCH), lpc21) imu_CFLAGS += -DSSP_VIC_SLOT=9 imu_CFLAGS += -DMAX1168_EOC_VIC_SLOT=8 -imu_CFLAGS += -DMS2100_DRDY_VIC_SLOT=11 +imu_CFLAGS += -DMS2100_DRDY_VIC_SLOT=12 else ifeq ($(ARCH), stm32) imu_CFLAGS += -DUSE_SPI2 -DUSE_DMA1_C4_IRQ -DUSE_EXTI2_IRQ -DUSE_SPI2_IRQ imu_CFLAGS += -DMAX_1168_DRDY_PORT=$(MAX_1168_DRDY_PORT) diff --git a/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.2.makefile b/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.2.makefile index e208c41664..91ba3d16d4 100644 --- a/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.2.makefile +++ b/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.2.makefile @@ -58,7 +58,7 @@ ifeq ($(ARCH), lpc21) imu_CFLAGS += -DSSP_VIC_SLOT=9 imu_CFLAGS += -DMAX1168_EOC_VIC_SLOT=8 #FIXME ms2100 not used on this imu -imu_CFLAGS += -DMS2100_DRDY_VIC_SLOT=11 +imu_CFLAGS += -DMS2100_DRDY_VIC_SLOT=12 else ifeq ($(ARCH), stm32) imu_CFLAGS += -DUSE_SPI2 -DUSE_DMA1_C4_IRQ -DUSE_EXTI2_IRQ -DUSE_SPI2_IRQ imu_CFLAGS += -DMAX_1168_DRDY_PORT=$(MAX_1168_DRDY_PORT) diff --git a/conf/autopilot/subsystems/rotorcraft/imu_crista.makefile b/conf/autopilot/subsystems/rotorcraft/imu_crista.makefile index 449da2f909..aa1dfb9cc9 100644 --- a/conf/autopilot/subsystems/rotorcraft/imu_crista.makefile +++ b/conf/autopilot/subsystems/rotorcraft/imu_crista.makefile @@ -70,7 +70,7 @@ imu_srcs += peripherals/ami601.c imu_CFLAGS += -DUSE_I2C1 ifeq ($(ARCH), lpc21) -imu_CFLAGS += -DI2C1_SCLL=150 -DI2C1_SCLH=150 -DI2C1_VIC_SLOT=11 -DI2C1_BUF_LEN=16 +imu_CFLAGS += -DI2C1_SCLL=150 -DI2C1_SCLH=150 -DI2C1_VIC_SLOT=12 -DI2C1_BUF_LEN=16 else ifeq ($(ARCH), stm32) #FIXME endif diff --git a/conf/autopilot/subsystems/rotorcraft/telemetry_transparent.makefile b/conf/autopilot/subsystems/rotorcraft/telemetry_transparent.makefile index fbc1b89c2e..920b031d66 100644 --- a/conf/autopilot/subsystems/rotorcraft/telemetry_transparent.makefile +++ b/conf/autopilot/subsystems/rotorcraft/telemetry_transparent.makefile @@ -12,7 +12,3 @@ ap.CFLAGS += -DDOWNLINK -DDOWNLINK_DEVICE=$(MODEM_PORT) -DPPRZ_UART=$(MODEM_PORT ap.CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=PPRZ ap.srcs += downlink.c pprz_transport.c ap.srcs += $(SRC_FIRMWARE)/datalink.c $(SRC_FIRMWARE)/telemetry.c - -ifeq ($(ARCH), lpc21) -ap.CFLAGS += -D$(MODEM_PORT)_VIC_SLOT=6 -endif diff --git a/conf/autopilot/subsystems/rotorcraft/telemetry_transparent_usb.makefile b/conf/autopilot/subsystems/rotorcraft/telemetry_transparent_usb.makefile new file mode 100644 index 0000000000..a15920babb --- /dev/null +++ b/conf/autopilot/subsystems/rotorcraft/telemetry_transparent_usb.makefile @@ -0,0 +1,16 @@ + +#serial USB (e.g. /dev/ttyACM0) + +ifeq ($(ARCH), lpc21) +ap.CFLAGS += -DDOWNLINK -DDOWNLINK_DEVICE=UsbS -DPPRZ_UART=UsbS +ap.CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=PPRZ -DUSE_USB_SERIAL +ap.srcs += downlink.c pprz_transport.c +ap.srcs += $(SRC_FIRMWARE)/datalink.c $(SRC_FIRMWARE)/telemetry.c +ap.srcs += $(SRC_ARCH)/usb_ser_hw.c $(SRC_ARCH)/lpcusb/usbhw_lpc.c $(SRC_ARCH)/lpcusb/usbcontrol.c +ap.srcs += $(SRC_ARCH)/lpcusb/usbstdreq.c $(SRC_ARCH)/lpcusb/usbinit.c +else +ifneq ($(ARCH), sim) +$(error telemetry_transparent_usb currently only implemented for the lpc21) +endif +endif + diff --git a/conf/autopilot/subsystems/rotorcraft/telemetry_xbee_api.makefile b/conf/autopilot/subsystems/rotorcraft/telemetry_xbee_api.makefile index a10f3fa95e..d083461928 100644 --- a/conf/autopilot/subsystems/rotorcraft/telemetry_xbee_api.makefile +++ b/conf/autopilot/subsystems/rotorcraft/telemetry_xbee_api.makefile @@ -13,7 +13,3 @@ ap.CFLAGS += -DDOWNLINK -DDOWNLINK_DEVICE=$(MODEM_PORT) -DXBEE_UART=$(MODEM_PORT ap.CFLAGS += -DDOWNLINK_TRANSPORT=XBeeTransport -DDATALINK=XBEE ap.srcs += downlink.c xbee.c ap.srcs += $(SRC_FIRMWARE)/datalink.c $(SRC_FIRMWARE)/telemetry.c - -ifeq ($(ARCH), lpc21) -ap.CFLAGS += -D$(MODEM_PORT)_VIC_SLOT=6 -endif diff --git a/conf/autopilot/subsystems/shared/ahrs_ic.makefile b/conf/autopilot/subsystems/shared/ahrs_ic.makefile new file mode 100644 index 0000000000..bda8b08365 --- /dev/null +++ b/conf/autopilot/subsystems/shared/ahrs_ic.makefile @@ -0,0 +1,22 @@ +# +# AHRS_PROPAGATE_FREQUENCY +# AHRS_H_X +# AHRS_H_Y +# AHRS_H_Z +# + +AHRS_CFLAGS = -DUSE_AHRS +ifdef AHRS_ALIGNER_LED +AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) +endif +AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl.h\" +AHRS_SRCS += subsystems/ahrs.c +AHRS_SRCS += subsystems/ahrs/ahrs_int_cmpl.c +AHRS_SRCS += subsystems/ahrs/ahrs_aligner.c + +ap.CFLAGS += $(AHRS_CFLAGS) +ap.srcs += $(AHRS_SRCS) + +sim.CFLAGS += $(AHRS_CFLAGS) +sim.srcs += $(AHRS_SRCS) + diff --git a/conf/autopilot/subsystems/shared/imu_aspirin.makefile b/conf/autopilot/subsystems/shared/imu_aspirin.makefile new file mode 100644 index 0000000000..29d685d4c9 --- /dev/null +++ b/conf/autopilot/subsystems/shared/imu_aspirin.makefile @@ -0,0 +1,17 @@ + +IMU_ASPIRIN_CFLAGS = -DUSE_IMU +IMU_ASPIRIN_CFLAGS += -DIMU_TYPE_H=\"imu/imu_aspirin.h\" -DIMU_OVERRIDE_CHANNELS +IMU_ASPIRIN_SRCS = $(SRC_SUBSYSTEMS)/imu.c \ + $(SRC_SUBSYSTEMS)/imu/imu_aspirin.c \ + $(SRC_ARCH)/subsystems/imu/imu_aspirin_arch.c \ + math/pprz_trig_int.c +IMU_ASPIRIN_SRCS += peripherals/hmc5843.c $(SRC_ARCH)/peripherals/hmc5843_arch.c +IMU_ASPIRIN_CFLAGS += -DUSE_I2C2 +IMU_ASPIRIN_SRCS += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c +IMU_ASPIRIN_CFLAGS += -DUSE_EXTI15_10_IRQ # Gyro Int on PC14 +IMU_ASPIRIN_CFLAGS += -DUSE_EXTI9_5_IRQ # Mag Int on PB5 +IMU_ASPIRIN_CFLAGS += -DUSE_EXTI2_IRQ # Accel Int on PD2 +IMU_ASPIRIN_CFLAGS += -DUSE_DMA1_C4_IRQ # SPI2 Rx DMA + +ap.CFLAGS += $(IMU_ASPIRIN_CFLAGS) +ap.srcs += $(IMU_ASPIRIN_SRCS) diff --git a/conf/flight_plans/EMAV2008.xml b/conf/flight_plans/EMAV2008.xml index 61457ec38c..2cadcfc0a2 100644 --- a/conf/flight_plans/EMAV2008.xml +++ b/conf/flight_plans/EMAV2008.xml @@ -84,7 +84,7 @@ - + @@ -130,7 +130,7 @@ - + @@ -149,7 +149,7 @@ - + diff --git a/conf/flight_plans/IS.xml b/conf/flight_plans/IS.xml index 9c264b7f37..dddd2b9cb5 100644 --- a/conf/flight_plans/IS.xml +++ b/conf/flight_plans/IS.xml @@ -148,7 +148,7 @@ diff --git a/conf/flight_plans/basic.xml b/conf/flight_plans/basic.xml index 57f3531d50..f9ead2281c 100644 --- a/conf/flight_plans/basic.xml +++ b/conf/flight_plans/basic.xml @@ -78,7 +78,7 @@ - + diff --git a/conf/flight_plans/ccc_gl.xml b/conf/flight_plans/ccc_gl.xml index 47a16f5a85..1a7656c6f6 100644 --- a/conf/flight_plans/ccc_gl.xml +++ b/conf/flight_plans/ccc_gl.xml @@ -112,7 +112,7 @@ - + diff --git a/conf/flight_plans/fp_tp_auto.xml b/conf/flight_plans/fp_tp_auto.xml index d9f0b4f940..3f61b05c1b 100644 --- a/conf/flight_plans/fp_tp_auto.xml +++ b/conf/flight_plans/fp_tp_auto.xml @@ -111,7 +111,7 @@ - + diff --git a/conf/flight_plans/grosslobke_demo.xml b/conf/flight_plans/grosslobke_demo.xml index 03e7dee1a1..92cecfe5f7 100755 --- a/conf/flight_plans/grosslobke_demo.xml +++ b/conf/flight_plans/grosslobke_demo.xml @@ -77,7 +77,7 @@ - + diff --git a/conf/flight_plans/joystick.xml b/conf/flight_plans/joystick.xml index 55b6b6e0cd..585a5542e4 100644 --- a/conf/flight_plans/joystick.xml +++ b/conf/flight_plans/joystick.xml @@ -80,7 +80,7 @@ - + diff --git a/conf/flight_plans/landing.xml b/conf/flight_plans/landing.xml index bcbc1cf8b0..953b26464b 100644 --- a/conf/flight_plans/landing.xml +++ b/conf/flight_plans/landing.xml @@ -18,7 +18,7 @@ - + diff --git a/conf/flight_plans/mav07.xml b/conf/flight_plans/mav07.xml index fb2a87092a..d4e3429ae0 100644 --- a/conf/flight_plans/mav07.xml +++ b/conf/flight_plans/mav07.xml @@ -130,7 +130,7 @@ - + diff --git a/conf/flight_plans/mav08.xml b/conf/flight_plans/mav08.xml index d418ee1cf7..084723612c 100644 --- a/conf/flight_plans/mav08.xml +++ b/conf/flight_plans/mav08.xml @@ -98,7 +98,7 @@ - + diff --git a/conf/flight_plans/nordlys.xml b/conf/flight_plans/nordlys.xml index 61fa5a0fa6..9e279fe850 100644 --- a/conf/flight_plans/nordlys.xml +++ b/conf/flight_plans/nordlys.xml @@ -85,7 +85,7 @@ - + diff --git a/conf/flight_plans/poles.xml b/conf/flight_plans/poles.xml index fad085ec5e..e1476ff1d8 100644 --- a/conf/flight_plans/poles.xml +++ b/conf/flight_plans/poles.xml @@ -70,7 +70,7 @@ - + diff --git a/conf/flight_plans/slayer_training.xml b/conf/flight_plans/slayer_training.xml index 16773c0183..92d84fe58c 100644 --- a/conf/flight_plans/slayer_training.xml +++ b/conf/flight_plans/slayer_training.xml @@ -75,7 +75,7 @@ - + diff --git a/conf/flight_plans/snav.xml b/conf/flight_plans/snav.xml index 866073c27e..d22bd1c962 100644 --- a/conf/flight_plans/snav.xml +++ b/conf/flight_plans/snav.xml @@ -55,7 +55,7 @@ - + diff --git a/conf/flight_plans/standard.xml b/conf/flight_plans/standard.xml index 906defbb70..ffa5e8dbda 100644 --- a/conf/flight_plans/standard.xml +++ b/conf/flight_plans/standard.xml @@ -68,7 +68,7 @@ - + diff --git a/conf/flight_plans/tcas.xml b/conf/flight_plans/tcas.xml index c0b327457c..9a3c2ce1ba 100644 --- a/conf/flight_plans/tcas.xml +++ b/conf/flight_plans/tcas.xml @@ -72,7 +72,7 @@ - + diff --git a/conf/flight_plans/versatile.xml b/conf/flight_plans/versatile.xml index c5d5e9becc..7544e5dda3 100644 --- a/conf/flight_plans/versatile.xml +++ b/conf/flight_plans/versatile.xml @@ -119,7 +119,7 @@ - + diff --git a/conf/flight_plans/xsens_cachejunction.xml b/conf/flight_plans/xsens_cachejunction.xml index 84aa6cf7bf..2179cf4094 100644 --- a/conf/flight_plans/xsens_cachejunction.xml +++ b/conf/flight_plans/xsens_cachejunction.xml @@ -89,7 +89,7 @@ - + diff --git a/conf/gps/rotorcraft_ublox6.txt b/conf/gps/rotorcraft_ublox6.txt new file mode 100644 index 0000000000..ed3c8389d5 --- /dev/null +++ b/conf/gps/rotorcraft_ublox6.txt @@ -0,0 +1,65 @@ +MON-VER - 0A 04 46 00 37 2E 30 31 20 28 34 34 31 37 39 29 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 30 30 30 34 30 30 30 37 00 00 36 2E 30 32 20 28 33 36 30 32 33 29 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 +CFG-ANT - 06 13 04 00 1B 00 8B A9 +CFG-DAT - 06 06 02 00 00 00 +CFG-FXN - 06 0E 24 00 0C 00 00 00 00 00 00 00 00 00 00 00 10 27 00 00 10 27 00 00 D0 07 00 00 18 FC FF FF 00 00 00 00 00 00 00 00 +CFG-INF - 06 02 0A 00 00 00 00 00 00 00 00 00 00 00 +CFG-INF - 06 02 0A 00 01 00 00 00 87 87 87 87 87 87 +CFG-INF - 06 02 0A 00 03 00 00 00 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 01 01 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 01 02 00 05 00 00 00 00 +CFG-MSG - 06 01 08 00 01 03 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 01 04 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 01 06 00 01 00 00 00 00 +CFG-MSG - 06 01 08 00 01 11 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 01 12 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 01 20 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 01 21 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 01 22 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 01 30 00 04 00 00 00 00 +CFG-MSG - 06 01 08 00 01 31 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 01 32 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 02 20 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 02 23 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0A 02 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0A 05 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0A 06 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0A 07 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0A 08 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0A 09 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0A 0A 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0A 20 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0A 21 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0B 00 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0B 30 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0B 31 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0B 32 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0D 01 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0D 03 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 F0 00 01 01 01 01 01 01 +CFG-MSG - 06 01 08 00 F0 01 01 01 01 01 01 01 +CFG-MSG - 06 01 08 00 F0 02 01 01 01 01 01 01 +CFG-MSG - 06 01 08 00 F0 03 01 01 01 01 01 01 +CFG-MSG - 06 01 08 00 F0 04 01 01 01 01 01 01 +CFG-MSG - 06 01 08 00 F0 05 01 01 01 01 01 01 +CFG-MSG - 06 01 08 00 F0 06 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 F0 07 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 F0 08 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 F0 09 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 F0 0A 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 F1 00 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 F1 03 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 F1 04 00 00 00 00 00 00 +CFG-NAV5 - 06 24 24 00 FF FF 08 02 00 00 00 00 10 27 00 00 05 00 FA 00 FA 00 64 00 2C 01 00 3C 00 00 00 00 00 00 00 00 00 00 00 00 +CFG-NAVX5 - 06 23 28 00 00 00 FF FF 03 00 00 00 03 02 03 10 07 00 00 01 00 00 43 06 00 00 00 00 01 01 00 00 00 64 78 00 00 00 00 00 00 00 00 00 +CFG-NMEA - 06 17 04 00 00 23 00 02 +CFG-PM - 06 32 18 00 00 06 00 00 04 90 00 00 E8 03 00 00 10 27 00 00 00 00 00 00 02 00 00 00 +CFG-PRT - 06 00 14 00 00 00 00 00 84 00 00 00 00 00 00 00 07 00 07 00 00 00 00 00 +CFG-PRT - 06 00 14 00 01 00 00 00 C0 08 00 00 00 E1 00 00 07 00 01 00 00 00 00 00 +CFG-PRT - 06 00 14 00 02 00 00 00 C0 08 00 00 80 25 00 00 00 00 00 00 00 00 00 00 +CFG-PRT - 06 00 14 00 03 00 00 00 00 00 00 00 00 00 00 00 07 00 07 00 00 00 00 00 +CFG-RATE - 06 08 06 00 7D 00 01 00 01 00 +CFG-RXM - 06 11 02 00 08 00 +CFG-SBAS - 06 16 08 00 03 03 03 00 51 08 00 00 +CFG-TP - 06 07 14 00 40 42 0F 00 A0 86 01 00 01 01 00 00 32 00 00 00 00 00 00 00 +CFG-TP5 - 06 31 20 00 00 E9 03 00 32 00 00 00 40 42 0F 00 40 42 0F 00 00 00 00 00 A0 86 01 00 00 00 00 00 F7 00 00 00 +CFG-USB - 06 1B 6C 00 46 15 A6 01 00 00 00 00 64 00 00 01 75 2D 62 6C 6F 78 20 41 47 20 2D 20 77 77 77 2E 75 2D 62 6C 6F 78 2E 63 6F 6D 00 00 00 00 00 00 75 2D 62 6C 6F 78 20 36 20 20 2D 20 20 47 50 53 20 52 65 63 65 69 76 65 72 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 diff --git a/conf/messages.xml b/conf/messages.xml index 2029c96743..27a0c43441 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -439,8 +439,19 @@ - - + + + + + + + + + + + + + @@ -823,19 +834,19 @@ - + - + - + @@ -1316,7 +1327,13 @@ - + + + + + + + diff --git a/conf/modules/ins_arduimu_basic.xml b/conf/modules/ins_arduimu_basic.xml new file mode 100644 index 0000000000..b1d4493404 --- /dev/null +++ b/conf/modules/ins_arduimu_basic.xml @@ -0,0 +1,18 @@ + + + +
+ +
+ + + + + + + + + + + + diff --git a/conf/modules/servo_switch.xml b/conf/modules/servo_switch.xml index f6d93e2a34..9255184d55 100644 --- a/conf/modules/servo_switch.xml +++ b/conf/modules/servo_switch.xml @@ -6,7 +6,7 @@ - + +
- + +
- - - - - + + + + + + + + diff --git a/conf/settings/flight_params.xml b/conf/settings/flight_params.xml new file mode 100644 index 0000000000..1fcf980283 --- /dev/null +++ b/conf/settings/flight_params.xml @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/settings/fw_ctl_n.xml b/conf/settings/fw_ctl_n.xml index eb3a8ac9e7..df393b0711 100644 --- a/conf/settings/fw_ctl_n.xml +++ b/conf/settings/fw_ctl_n.xml @@ -4,13 +4,19 @@ - + + + + + + + @@ -54,8 +60,6 @@ - - diff --git a/conf/settings/infrared.xml b/conf/settings/infrared.xml new file mode 100644 index 0000000000..ee2d434796 --- /dev/null +++ b/conf/settings/infrared.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/conf/settings/ins_basic.xml b/conf/settings/ins_basic.xml new file mode 100644 index 0000000000..386a15c7db --- /dev/null +++ b/conf/settings/ins_basic.xml @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/conf/settings/ir_i2c.xml b/conf/settings/ir_i2c.xml index 40304d0c2a..b386b4c6d9 100644 --- a/conf/settings/ir_i2c.xml +++ b/conf/settings/ir_i2c.xml @@ -3,6 +3,17 @@ + + + + + + + + + + + diff --git a/conf/settings/pbn.xml b/conf/settings/pbn.xml new file mode 100644 index 0000000000..97f53f5f32 --- /dev/null +++ b/conf/settings/pbn.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/conf/settings/settings.dtd b/conf/settings/settings.dtd index 25cda6c3be..bde71f467e 100644 --- a/conf/settings/settings.dtd +++ b/conf/settings/settings.dtd @@ -36,6 +36,7 @@ unit CDATA #IMPLIED alt_unit CDATA #IMPLIED alt_unit_coef CDATA #IMPLIED values CDATA #IMPLIED +persistent CDATA #IMPLIED > + + + + + + + + + + + + + + diff --git a/conf/telemetry/fw_h_ctl_a.xml b/conf/telemetry/fw_h_ctl_a.xml index f8b3dc8d19..7ec85992a5 100644 --- a/conf/telemetry/fw_h_ctl_a.xml +++ b/conf/telemetry/fw_h_ctl_a.xml @@ -38,7 +38,7 @@ - + diff --git a/conf/telemetry/telemetry_booz2.xml b/conf/telemetry/telemetry_booz2.xml index ee9f58aac8..4852249e86 100644 --- a/conf/telemetry/telemetry_booz2.xml +++ b/conf/telemetry/telemetry_booz2.xml @@ -40,9 +40,9 @@ - - - + + + diff --git a/conf/telemetry/telemetry_booz2_flixr.xml b/conf/telemetry/telemetry_booz2_flixr.xml deleted file mode 100644 index 441e1d228f..0000000000 --- a/conf/telemetry/telemetry_booz2_flixr.xml +++ /dev/null @@ -1,124 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/doc/pprz_algebra/quaternion.tex b/doc/pprz_algebra/quaternion.tex index 7977222283..3cb937575f 100644 --- a/doc/pprz_algebra/quaternion.tex +++ b/doc/pprz_algebra/quaternion.tex @@ -162,8 +162,8 @@ It is also possible to directly normalise the quaternion \begin{equation} \quat{} := \frac{\quat{}}{\norm{\quat{}}} \end{equation} -\inHfile{INT32\_QUAT\_NORMALISE(q)}{pprz\_algebra\_int} -\inHfile{FLOAT\_QUAT\_NORMALISE(q)}{pprz\_algebra\_float} +\inHfile{INT32\_QUAT\_NORMALIZE(q)}{pprz\_algebra\_int} +\inHfile{FLOAT\_QUAT\_NORMALIZE(q)}{pprz\_algebra\_float} \subsection*{Making the real value positive} It is possible to invert the quaternion if its real value is negative diff --git a/sw/airborne/arch/lpc21/mcu_periph/adc_arch.h b/sw/airborne/arch/lpc21/mcu_periph/adc_arch.h index 947003f64b..6e5fe748c9 100644 --- a/sw/airborne/arch/lpc21/mcu_periph/adc_arch.h +++ b/sw/airborne/arch/lpc21/mcu_periph/adc_arch.h @@ -25,6 +25,8 @@ #ifndef ADC_HW_H #define ADC_HW_H +#include BOARD_CONFIG + #define AdcBank0(x) (x) #define AdcBank1(x) (x+NB_ADC) diff --git a/sw/airborne/arch/lpc21/mcu_periph/dac_arch.c b/sw/airborne/arch/lpc21/mcu_periph/dac_arch.c new file mode 100644 index 0000000000..adda46e12b --- /dev/null +++ b/sw/airborne/arch/lpc21/mcu_periph/dac_arch.c @@ -0,0 +1,6 @@ +#include "mcu_periph/dac.h" + +/* turn on DAC pins */ +void dac_init(void) { + PINSEL1 |= 2 << 18; +} diff --git a/sw/airborne/arch/lpc21/mcu_periph/dac_arch.h b/sw/airborne/arch/lpc21/mcu_periph/dac_arch.h new file mode 100644 index 0000000000..e27b929e34 --- /dev/null +++ b/sw/airborne/arch/lpc21/mcu_periph/dac_arch.h @@ -0,0 +1,12 @@ +#ifndef LPC21_MCU_PERIPH_DAC_ARCH_H +#define LPC21_MCU_PERIPH_DAC_ARCH_H + +#include "std.h" +#include "LPC21xx.h" + +static inline void DACSet(uint16_t x) { + DACR = x << 6; +} + + +#endif /* LPC21_MCU_PERIPH_DAC_ARCH_H */ diff --git a/sw/airborne/arch/lpc21/mcu_periph/i2c_arch.c b/sw/airborne/arch/lpc21/mcu_periph/i2c_arch.c index 378337c50b..f8f02fc83e 100644 --- a/sw/airborne/arch/lpc21/mcu_periph/i2c_arch.c +++ b/sw/airborne/arch/lpc21/mcu_periph/i2c_arch.c @@ -28,7 +28,6 @@ #include "interrupt_hw.h" #include BOARD_CONFIG - /////////////////// // I2C Automaton // /////////////////// @@ -56,22 +55,18 @@ __attribute__ ((always_inline)) static inline void I2cEndOfTransaction(struct i2 } } -__attribute__ ((always_inline)) static inline void I2cFinished(struct i2c_periph* p, struct i2c_transaction* t) { +__attribute__ ((always_inline)) static inline void I2cSendStop(struct i2c_periph* p, struct i2c_transaction* t) { + ((i2cRegs_t *)(p->reg_addr))->conset = _BV(STO); // transaction finished with success t->status = I2CTransSuccess; I2cEndOfTransaction(p); } -__attribute__ ((always_inline)) static inline void I2cSendStop(struct i2c_periph* p, struct i2c_transaction* t) { - ((i2cRegs_t *)(p->reg_addr))->conset = _BV(STO); - I2cFinished(p,t); -} - __attribute__ ((always_inline)) static inline void I2cFail(struct i2c_periph* p, struct i2c_transaction* t) { ((i2cRegs_t *)(p->reg_addr))->conset = _BV(STO); + // transaction failed t->status = I2CTransFailed; - p->status = I2CFailed; - // FIXME I2C should be reseted here + // FIXME I2C should be reseted here ? I2cEndOfTransaction(p); } @@ -119,35 +114,7 @@ __attribute__ ((always_inline)) static inline void I2cAutomaton(int32_t state, s } else { /* error , we should have got NACK */ - I2cSendStop(p,trans); - } - break; - case I2C_MR_SLA_ACK: /* At least one char */ - /* Wait and reply with ACK or NACK */ - I2cReceive(p->reg_addr,p->idx_buf < trans->len_r - 1); - break; - case I2C_MR_SLA_NACK: - case I2C_MT_SLA_NACK: - I2cSendStart(p); - break; - case I2C_MT_SLA_ACK: - case I2C_MT_DATA_ACK: - if (p->idx_buf < trans->len_w) { - I2cSendByte(p->reg_addr,trans->buf[p->idx_buf]); - p->idx_buf++; - } else { - if (trans->type == I2CTransTxRx) { - trans->type = I2CTransRx; /* FIXME should not change type */ - p->idx_buf = 0; - trans->slave_addr |= 1; - I2cSendStart(p); - } else { - if (trans->stop_after_transmit) { - I2cSendStop(p,trans); - } else { - I2cFinished(p,trans); - } - } + I2cFail(p,trans); } break; case I2C_MR_DATA_NACK: @@ -156,9 +123,33 @@ __attribute__ ((always_inline)) static inline void I2cAutomaton(int32_t state, s } I2cSendStop(p,trans); break; + case I2C_MR_SLA_ACK: /* At least one char */ + /* Wait and reply with ACK or NACK */ + I2cReceive(p->reg_addr,p->idx_buf < trans->len_r - 1); + break; + case I2C_MR_SLA_NACK: + case I2C_MT_SLA_NACK: + /* Slave is not responding, transaction is failed */ + I2cFail(p,trans); + break; + case I2C_MT_SLA_ACK: + case I2C_MT_DATA_ACK: + if (p->idx_buf < trans->len_w) { + I2cSendByte(p->reg_addr,trans->buf[p->idx_buf]); + p->idx_buf++; + } else { + if (trans->type == I2CTransTxRx) { + //trans->type = I2CTransRx; /* FIXME should not change type */ + p->idx_buf = 0; + trans->slave_addr |= 1; + I2cSendStart(p); + } else { + I2cSendStop(p,trans); + } + } + break; default: - I2cSendStop(p,trans); - //I2cFail(p,trans); + I2cFail(p,trans); /* FIXME log error */ break; } diff --git a/sw/airborne/arch/lpc21/subsystems/settings_arch.c b/sw/airborne/arch/lpc21/subsystems/settings_arch.c new file mode 100644 index 0000000000..517749b900 --- /dev/null +++ b/sw/airborne/arch/lpc21/subsystems/settings_arch.c @@ -0,0 +1,10 @@ +#include "subsystems/settings.h" + + +int32_t persistent_write(uint32_t ptr, uint32_t size) { + return 0; +} + +int32_t persistent_read(uint32_t ptr, uint32_t size) { + return 0; +} diff --git a/sw/airborne/arch/lpc21/vic_slots.txt b/sw/airborne/arch/lpc21/vic_slots.txt deleted file mode 100644 index c75e66ef1e..0000000000 --- a/sw/airborne/arch/lpc21/vic_slots.txt +++ /dev/null @@ -1,15 +0,0 @@ - 1 TIMER0_VIC_SLOT - 2 AD0_VIC_SLOT - 4 AD1_VIC_SLOT - 5 UART0_VIC_SLOT - 6 UART1_VIC_SLOT - 9 I2C0_VIC_SLOT - I2C1_VIC_SLOT -11 SSP_VIC_SLOT -11 MS2100_DRDY_VIC_SLOT - -12 MICROMAG_DRDY_VIC_SLOT -12 MAX11040_DRDY_VIC_SLOT - -?? MAX1167_EOC_VIC_SLOT - diff --git a/sw/airborne/arch/lpc21/vic_slots_fw.txt b/sw/airborne/arch/lpc21/vic_slots_fw.txt new file mode 100644 index 0000000000..6c94aa7bfe --- /dev/null +++ b/sw/airborne/arch/lpc21/vic_slots_fw.txt @@ -0,0 +1,22 @@ +VIC slots used for fixedwings with the LPC2148 + + +define name slot (default) used for +------------------------------------------------------------------ +TIMER0_VIC_SLOT 1 (1) system timer +AD0_VIC_SLOT 2 (2) was for adc battery (not needed anymore?) +hardcoded, no define 3 PWM_ISR in servos_4015 +AD1_VIC_SLOT 4 (4) was for adc baro (not needed anymore?) +UART0_VIC_SLOT 5 (5) uart_arch, e.g. gps +UART1_VIC_SLOT 6 (6) uart_arch, e.g. modem +hardcoded, no define 7 SPI1 in mcu_periph/spi_arch.c, imu_crista_arch, max3100 module, baro_scp module, lcd_dogm module +MAX1168_EOC_VIC_SLOT 8 imu_b2_v1.0, imu_b2_v1.1, imu_b2_v1.2 +hardcoded, no define 8 EXTINT in max3100 module +I2C0_VIC_SLOT ? (9) (9 is default in mcu_periph/i2c_arch.c) +SSP_VIC_SLOT 9 imu_b2_v1.0, imu_b2_v1.1, imu_b2_v1.2 +MICROMAG_DRDY_VIC_SLOT 9 micromag +hardcoded, no define 10 usb, e.g. telemetry_transparent_usb +hardcoded, no define 11 EXTINT in baro_scp module +I2C1_VIC_SLOT 12 ami601 in imu_b2_v1.0, imu_crista +MS2100_DRDY_VIC_SLOT 12 ms2100 mag in imu_b2_v1.1 +MAX11040_DRDY_VIC_SLOT ? max11040 adc module diff --git a/sw/airborne/arch/lpc21/vic_slots_rotorcrafts.txt b/sw/airborne/arch/lpc21/vic_slots_rotorcrafts.txt new file mode 100644 index 0000000000..0acf1429f4 --- /dev/null +++ b/sw/airborne/arch/lpc21/vic_slots_rotorcrafts.txt @@ -0,0 +1,18 @@ +VIC slots used for rotorcrafts with the LPC2148 + +define name slot (default) used for +------------------------------------------------------------------ +TIMER0_VIC_SLOT 1 (1) system timer +AD0_VIC_SLOT 2 (2) was for adc battery (not needed anymore?) + +AD1_VIC_SLOT 4 (4) was for adc baro (not needed anymore?) +UART0_VIC_SLOT 5 (5) uart_arch, e.g. gps +UART1_VIC_SLOT 6 (6) uart_arch, e.g. modem +hardcoded, no define 7 SPI1 in mcu_periph/spi_arch.c, imu_crista_arch +MAX1168_EOC_VIC_SLOT 8 imu_b2_v1.0, imu_b2_v1.1, imu_b2_v1.2 +SSP_VIC_SLOT 9 imu_b2_v1.0, imu_b2_v1.1, imu_b2_v1.2 +hardcoded, no define 10 usb, e.g. telemetry_transparent_usb +I2C0_VIC_SLOT 11 (9) actuators_acstec, actuators_acstec_v2, actuators_mkk, (9 is default in mcu_periph/i2c_arch.c) +I2C1_VIC_SLOT 12 ami601 in imu_b2_v1.0, imu_crista +MS2100_DRDY_VIC_SLOT 12 ms2100 mag in imu_b2_v1.1 + diff --git a/sw/airborne/arch/sim/subsystems/settings_arch.c b/sw/airborne/arch/sim/subsystems/settings_arch.c new file mode 100644 index 0000000000..517749b900 --- /dev/null +++ b/sw/airborne/arch/sim/subsystems/settings_arch.c @@ -0,0 +1,10 @@ +#include "subsystems/settings.h" + + +int32_t persistent_write(uint32_t ptr, uint32_t size) { + return 0; +} + +int32_t persistent_read(uint32_t ptr, uint32_t size) { + return 0; +} diff --git a/sw/airborne/arch/stm32/mcu_arch.c b/sw/airborne/arch/stm32/mcu_arch.c index 3c8a01fd10..9a6fb255ec 100644 --- a/sw/airborne/arch/stm32/mcu_arch.c +++ b/sw/airborne/arch/stm32/mcu_arch.c @@ -24,17 +24,24 @@ #include "mcu.h" +#include BOARD_CONFIG #include #include #include #include #include +#ifdef USE_OPENCM3 +#include +#endif -#include BOARD_CONFIG void mcu_arch_init(void) { - +#ifdef USE_OPENCM3 + rcc_clock_setup_in_hse_12mhz_out_72mhz(); + NVIC_SetVectorTable(NVIC_VectTab_FLASH, 0x0); + return; +#endif #ifdef HSE_TYPE_EXT_CLK #warning Using external clock /* Setup the microcontroller system. diff --git a/sw/airborne/arch/stm32/mcu_periph/adc_arch.c b/sw/airborne/arch/stm32/mcu_periph/adc_arch.c index 8ff6734e31..114ede55b7 100644 --- a/sw/airborne/arch/stm32/mcu_periph/adc_arch.c +++ b/sw/airborne/arch/stm32/mcu_periph/adc_arch.c @@ -129,12 +129,12 @@ static inline void adc_init_irq( void ); */ #ifdef USE_AD1 #ifndef ADC1_GPIO_INIT -#define ADC1_GPIO_INIT(gpio) { \ +#define ADC1_GPIO_INIT(gpio) { \ (gpio).GPIO_Pin = GPIO_Pin_1 | GPIO_Pin_0; \ - (gpio).GPIO_Mode = GPIO_Mode_AIN; \ - GPIO_Init(GPIOB, (&gpio)); \ + (gpio).GPIO_Mode = GPIO_Mode_AIN; \ + GPIO_Init(GPIOB, (&gpio)); \ (gpio).GPIO_Pin = GPIO_Pin_5 | GPIO_Pin_3; \ - GPIO_Init(GPIOC, (&gpio)); \ + GPIO_Init(GPIOC, (&gpio)); \ } #endif // ADC1_GPIO_INIT #endif // USE_AD1 @@ -145,13 +145,13 @@ static inline void adc_init_irq( void ); Uses the same GPIOs as ADC1 (lisa specific). */ #ifdef USE_AD2 -#define ADC2_GPIO_INIT(gpio) { \ +#define ADC2_GPIO_INIT(gpio) { \ (gpio).GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1; \ - (gpio).GPIO_Mode = GPIO_Mode_AIN; \ - GPIO_Init(GPIOB, (&gpio)); \ + (gpio).GPIO_Mode = GPIO_Mode_AIN; \ + GPIO_Init(GPIOB, (&gpio)); \ (gpio).GPIO_Pin = GPIO_Pin_3 | GPIO_Pin_5; \ - GPIO_Init(GPIOC, (&gpio)); \ -} + GPIO_Init(GPIOC, (&gpio)); \ + } #ifndef ADC2_GPIO_INIT #define ADC2_GPIO_INIT(gpio) { } #endif // ADC2_GPIO_INIT @@ -362,10 +362,10 @@ static inline void adc_init_single(ADC_TypeDef * adc_t, void adc_init( void ) { /* initialize buffer pointers with 0 (not set). - buffer null pointers will be ignored in interrupt - handler, which is important as there are no - buffers registered at the time the ADC trigger - interrupt is enabled. + buffer null pointers will be ignored in interrupt + handler, which is important as there are no + buffers registered at the time the ADC trigger + interrupt is enabled. */ uint8_t channel; #ifdef USE_AD1 @@ -382,14 +382,12 @@ void adc_init( void ) { adc_injected_channels[1] = ADC_InjectedChannel_2; adc_injected_channels[2] = ADC_InjectedChannel_3; adc_injected_channels[3] = ADC_InjectedChannel_4; - // TODO: Channel selection could be configured - // using defines. - adc_channel_map[0] = ADC_Channel_8; - adc_channel_map[1] = ADC_Channel_9; - // adc_channel_map[2] = ADC_Channel_13; + adc_channel_map[0] = BOARD_ADC_CHANNEL_1; + adc_channel_map[1] = BOARD_ADC_CHANNEL_2; // FIXME for now we get battery voltage this way - adc_channel_map[2] = ADC_Channel_0; - adc_channel_map[3] = ADC_Channel_15; + // adc_channel_map[2] = BOARD_ADC_CHANNEL_3; + adc_channel_map[2] = BOARD_ADC_CHANNEL_3; + adc_channel_map[3] = BOARD_ADC_CHANNEL_4; adc_init_rcc(); adc_init_irq(); diff --git a/sw/airborne/arch/stm32/mcu_periph/uart_arch.c b/sw/airborne/arch/stm32/mcu_periph/uart_arch.c index 3edce0c1d1..e0406d9c85 100644 --- a/sw/airborne/arch/stm32/mcu_periph/uart_arch.c +++ b/sw/airborne/arch/stm32/mcu_periph/uart_arch.c @@ -28,6 +28,7 @@ #include #include #include "std.h" +#include "pprz_baudrate.h" #ifdef USE_UART1 @@ -42,6 +43,7 @@ uint8_t uart1_tx_buffer[UART1_TX_BUFFER_SIZE]; void uart1_init( void ) { /* init RCC */ RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE); + RCC_APB2PeriphClockCmd(UART1_Periph, ENABLE); /* Enable USART1 interrupts */ NVIC_InitTypeDef nvic; @@ -74,6 +76,9 @@ void uart1_init( void ) { USART_Init(USART1, &usart); /* Enable USART1 Receive interrupts */ USART_ITConfig(USART1, USART_IT_RXNE, ENABLE); + + pprz_usart_set_baudrate(USART1, UART1_BAUD); + /* Enable the USART1 */ USART_Cmd(USART1, ENABLE); @@ -165,6 +170,7 @@ uint8_t uart2_tx_buffer[UART2_TX_BUFFER_SIZE]; void uart2_init( void ) { /* init RCC */ RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE); + RCC_APB2PeriphClockCmd(UART2_Periph, ENABLE); /* Enable USART2 interrupts */ NVIC_InitTypeDef nvic; @@ -197,6 +203,9 @@ void uart2_init( void ) { USART_Init(USART2, &usart); /* Enable USART2 Receive interrupts */ USART_ITConfig(USART2, USART_IT_RXNE, ENABLE); + + pprz_usart_set_baudrate(USART2, UART2_BAUD); + /* Enable the USART2 */ USART_Cmd(USART2, ENABLE); @@ -287,6 +296,7 @@ void uart3_init( void ) { /* init RCC */ RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE); RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE); + RCC_APB2PeriphClockCmd(UART3_Periph, ENABLE); /* Enable USART3 interrupts */ NVIC_InitTypeDef nvic; @@ -320,6 +330,9 @@ void uart3_init( void ) { USART_Init(USART3, &usart); /* Enable USART3 Receive interrupts */ USART_ITConfig(USART3, USART_IT_RXNE, ENABLE); + + pprz_usart_set_baudrate(USART3, UART3_BAUD); + /* Enable the USART3 */ USART_Cmd(USART3, ENABLE); @@ -392,6 +405,128 @@ void usart3_irq_handler(void) { #endif /* USE_UART3 */ +#ifdef USE_UART5 + +volatile uint16_t uart5_rx_insert_idx, uart5_rx_extract_idx; +uint8_t uart5_rx_buffer[UART5_RX_BUFFER_SIZE]; + +volatile uint16_t uart5_tx_insert_idx, uart5_tx_extract_idx; +volatile bool_t uart5_tx_running; +uint8_t uart5_tx_buffer[UART5_TX_BUFFER_SIZE]; + +void uart5_init( void ) { + + /* init RCC */ + RCC_APB1PeriphClockCmd(RCC_APB1Periph_UART5, ENABLE); + RCC_APB2PeriphClockCmd(UART5_PeriphTx, ENABLE); + RCC_APB2PeriphClockCmd(UART5_PeriphRx, ENABLE); + + /* Enable UART5 interrupts */ + NVIC_InitTypeDef nvic; + nvic.NVIC_IRQChannel = UART5_IRQn; + nvic.NVIC_IRQChannelPreemptionPriority = 2; + nvic.NVIC_IRQChannelSubPriority = 1; + nvic.NVIC_IRQChannelCmd = ENABLE; + NVIC_Init(&nvic); + + /* Init GPIOS */ + GPIO_InitTypeDef gpio; + /* GPIOC: GPIO_Pin_10 UART5 Tx push-pull */ + gpio.GPIO_Pin = UART5_TxPin; + gpio.GPIO_Mode = GPIO_Mode_AF_PP; + gpio.GPIO_Speed = GPIO_Speed_50MHz; + GPIO_Init(UART5_TxPort, &gpio); + /* GPIOC: GPIO_Pin_11 UART5 Rx pin as floating input */ + gpio.GPIO_Pin = UART5_RxPin; + gpio.GPIO_Mode = GPIO_Mode_IN_FLOATING; + GPIO_Init(UART5_RxPort, &gpio); + + /* Configure UART5 */ + USART_InitTypeDef usart; + usart.USART_BaudRate = UART5_BAUD; + usart.USART_WordLength = USART_WordLength_8b; + usart.USART_StopBits = USART_StopBits_1; + usart.USART_Parity = USART_Parity_No; + usart.USART_HardwareFlowControl = USART_HardwareFlowControl_None; + usart.USART_Mode = USART_Mode_Rx | USART_Mode_Tx; + USART_Init(USART5, &usart); + /* Enable UART5 Receive interrupts */ + USART_ITConfig(UART5, USART_IT_RXNE, ENABLE); + + pprz_usart_set_baudrate(UART5, UART5_BAUD); + + /* Enable the UART5 */ + USART_Cmd(UART5, ENABLE); + + // initialize the transmit data queue + uart5_tx_extract_idx = 0; + uart5_tx_insert_idx = 0; + uart5_tx_running = FALSE; + + // initialize the receive data queuenn + uart5_rx_extract_idx = 0; + uart5_rx_insert_idx = 0; + +} + +void uart5_transmit( uint8_t data ) { + + uint16_t temp = (uart5_tx_insert_idx + 1) % UART5_TX_BUFFER_SIZE; + + if (temp == uart5_tx_extract_idx) + return; // no room + + USART_ITConfig(USART5, USART_IT_TXE, DISABLE); + + // check if in process of sending data + if (uart5_tx_running) { // yes, add to queue + uart5_tx_buffer[uart5_tx_insert_idx] = data; + uart5_tx_insert_idx = temp; + } + else { // no, set running flag and write to output register + uart5_tx_running = TRUE; + USART_SendData(USART5, data); + } + USART_ITConfig(USART5, USART_IT_TXE, ENABLE); + +} + +bool_t uart5_check_free_space( uint8_t len) { + int16_t space = uart5_tx_extract_idx - uart5_tx_insert_idx; + if (space <= 0) + space += UART5_TX_BUFFER_SIZE; + return (uint16_t)(space - 1) >= len; +} + + +void usart5_irq_handler(void) { + + if(USART_GetITStatus(USART5, USART_IT_TXE) != RESET){ + // check if more data to send + if (uart5_tx_insert_idx != uart5_tx_extract_idx) { + USART_SendData(USART5,uart5_tx_buffer[uart5_tx_extract_idx]); + uart5_tx_extract_idx++; + uart5_tx_extract_idx %= UART5_TX_BUFFER_SIZE; + } + else { + uart5_tx_running = FALSE; // clear running flag + USART_ITConfig(USART5, USART_IT_TXE, DISABLE); + } + } + + if(USART_GetITStatus(USART5, USART_IT_RXNE) != RESET){ + uint16_t temp = (uart5_rx_insert_idx + 1) % UART5_RX_BUFFER_SIZE;; + uart5_rx_buffer[uart5_rx_insert_idx] = USART_ReceiveData(USART5); + // check for more room in queue + if (temp != uart5_rx_extract_idx) + uart5_rx_insert_idx = temp; // update insert index + } + +} + + +#endif /* USE_UART5 */ + void uart_init( void ) { #ifdef USE_UART1 @@ -403,6 +538,9 @@ void uart_init( void ) #ifdef USE_UART3 uart3_init(); #endif +#ifdef USE_UART5 + uart5_init(); +#endif } diff --git a/sw/airborne/arch/stm32/mcu_periph/uart_arch.h b/sw/airborne/arch/stm32/mcu_periph/uart_arch.h index e281700dba..3d7113ef42 100644 --- a/sw/airborne/arch/stm32/mcu_periph/uart_arch.h +++ b/sw/airborne/arch/stm32/mcu_periph/uart_arch.h @@ -44,6 +44,7 @@ #define UART1_TxPin GPIO_Pin_9 #define UART2_TxPin GPIO_Pin_2 #define UART3_TxPin GPIO_Pin_10 +#define UART5_TxPin GPIO_Pin_12 #define UART1_RxPin GPIO_Pin_10 #define UART2_RxPin GPIO_Pin_3 @@ -53,6 +54,7 @@ #define UART1_TxPort GPIOA #define UART2_TxPort GPIOA #define UART3_TxPort GPIOC +#define UART5_TxPort GPIOC #define UART1_RxPort GPIOA #define UART2_RxPort GPIOA @@ -62,6 +64,13 @@ #define UART1_Periph RCC_APB2Periph_GPIOA #define UART2_Periph RCC_APB2Periph_GPIOA #define UART3_Periph RCC_APB2Periph_GPIOC +#define UART5_PeriphTx RCC_APB2Periph_GPIOC +#define UART5_PeriphRx RCC_APB2Periph_GPIOD + +/* this is unexpected the macros in spektrum_arch.c + didn't expect that rx and tx would be spilt over + two ports. As the spektrum code is only interested + in the rx pin we define this to be the Peripheral */ #define UART5_Periph RCC_APB2Periph_GPIOD #define UART1_UartPeriph RCC_APB2Periph_USART1 @@ -180,6 +189,27 @@ extern uint8_t uart3_tx_buffer[UART3_TX_BUFFER_SIZE]; #endif /* USE_UART3 */ +#ifdef USE_UART5 + +#define UART5_RX_BUFFER_SIZE 128 +#define UART5_TX_BUFFER_SIZE 128 + +extern volatile uint16_t uart5_rx_insert_idx, uart5_rx_extract_idx; +extern uint8_t uart5_rx_buffer[UART5_RX_BUFFER_SIZE]; + +extern volatile uint16_t uart5_tx_insert_idx, uart5_tx_extract_idx; +extern volatile bool_t uart5_tx_running; +extern uint8_t uart5_tx_buffer[UART5_TX_BUFFER_SIZE]; + +#define Uart5ChAvailable() (uart5_rx_insert_idx != uart5_rx_extract_idx) +#define Uart5Getch() ({ \ + uint8_t ret = uart5_rx_buffer[uart5_rx_extract_idx]; \ + uart5_rx_extract_idx = (uart5_rx_extract_idx + 1)%UART5_RX_BUFFER_SIZE; \ + ret; \ + }) + +#endif /* USE_UART5 */ + void uart_init( void ); diff --git a/sw/airborne/arch/stm32/pprz_baudrate.h b/sw/airborne/arch/stm32/pprz_baudrate.h new file mode 100644 index 0000000000..bb4a7369a2 --- /dev/null +++ b/sw/airborne/arch/stm32/pprz_baudrate.h @@ -0,0 +1,13 @@ +#ifndef __PPRZ_BAUDRATE_H +#define __PPRZ_BAUDRATE_H + +#include BOARD_CONFIG + +#ifdef USE_OPENCM3 +void usart_set_baudrate(void *usart, uint32_t baud); +#define pprz_usart_set_baudrate(x, y) usart_set_baudrate(x, y) +#else +#define pprz_usart_set_baudrate(x, y) do { } while(0); +#endif + +#endif diff --git a/sw/airborne/arch/stm32/subsystems/radio_control/spektrum_arch.c b/sw/airborne/arch/stm32/subsystems/radio_control/spektrum_arch.c index 583b90c089..ae8927d88f 100644 --- a/sw/airborne/arch/stm32/subsystems/radio_control/spektrum_arch.c +++ b/sw/airborne/arch/stm32/subsystems/radio_control/spektrum_arch.c @@ -30,6 +30,8 @@ #include "subsystems/radio_control.h" #include "subsystems/radio_control/spektrum_arch.h" #include "mcu_periph/uart.h" +#include "pprz_baudrate.h" + #define SPEKTRUM_CHANNELS_PER_FRAME 7 @@ -549,6 +551,9 @@ void SpektrumUartInit(void) { USART_Init(PrimaryUart(_reg), &usart); /* Enable Primary UART Receive interrupts */ USART_ITConfig(PrimaryUart(_reg), USART_IT_RXNE, ENABLE); + + /* required to get the correct baudrate on lisa m */ + pprz_usart_set_baudrate(PrimaryUart(_reg), B115200); /* Enable the Primary UART */ USART_Cmd(PrimaryUart(_reg), ENABLE); @@ -580,6 +585,9 @@ void SpektrumUartInit(void) { USART_Init(SecondaryUart(_reg), &usart); /* Enable Secondary UART Receive interrupts */ USART_ITConfig(SecondaryUart(_reg), USART_IT_RXNE, ENABLE); + + /* required to get the correct baudrate on lisa m */ + pprz_usart_set_baudrate(SecondaryUart(_reg), B115200); /* Enable the Primary UART */ USART_Cmd(SecondaryUart(_reg), ENABLE); #endif @@ -695,7 +703,7 @@ void radio_control_spektrum_try_bind(void) { #endif /* We have no idea how long the window for allowing binding after - power up is .This works for the moment but will need revisiting */ + power up is. This works for the moment but will need revisiting */ DelayMs(61); for (int i = 0; i < MASTER_RECEIVER_PULSES ; i++) diff --git a/sw/airborne/arch/stm32/subsystems/settings_arch.c b/sw/airborne/arch/stm32/subsystems/settings_arch.c new file mode 100644 index 0000000000..d25d80c1f6 --- /dev/null +++ b/sw/airborne/arch/stm32/subsystems/settings_arch.c @@ -0,0 +1,189 @@ +#include "subsystems/settings.h" + +#include +#include + +struct FlashInfo { + uint32_t addr; + uint32_t total_size; + uint32_t page_nr; + uint32_t page_size; +}; + + +static uint32_t pflash_checksum(uint32_t ptr, uint32_t size); +static int32_t flash_detect(struct FlashInfo* flash); +static int32_t pflash_program_bytes(struct FlashInfo* flash, + uint32_t src, + uint32_t size, + uint32_t chksum); + +#define FLASH_BEGIN 0x08000000 +#define FLASH_SIZE 0x1FFFF7E0 +#define FSIZ 8 +#define FCHK 4 + + +static uint32_t pflash_checksum(uint32_t ptr, uint32_t size) { + uint32_t i; + + /* reset crc */ + CRC_CR = CRC_CR_RESET; + + if (ptr % 4) { + /* calc in 8bit chunks */ + for (i=0; i<(size & ~3); i+=4) { + CRC_DR = (*(uint8_t*) (ptr+i)) | + (*(uint8_t*) (ptr+i+1)) << 8 | + (*(uint8_t*) (ptr+i+2)) << 16 | + (*(uint8_t*) (ptr+i+3)) << 24; + } + } else { + /* calc in 32bit */ + for (i=0; i<(size & ~3); i+=4) { + CRC_DR = *(uint32_t*) (ptr+i); + } + } + + /* remaining bytes */ + switch (size % 4) { + case 1: + CRC_DR = *(uint8_t*) (ptr+i); + break; + case 2: + CRC_DR = (*(uint8_t*) (ptr+i)) | + (*(uint8_t*) (ptr+i+1)) << 8; + break; + case 3: + CRC_DR = (*(uint8_t*) (ptr+i)) | + (*(uint8_t*) (ptr+i+1)) << 8 | + (*(uint8_t*) (ptr+i+2)) << 16; + break; + default: + break; + } + + return CRC_DR; +} + + + +static int32_t flash_detect(struct FlashInfo* flash) { + + flash->total_size = (MMIO32(FLASH_SIZE) * 0x400)&0x00FFFFFF; + + switch (flash->total_size) { + /* low density */ + case 0x00004000: /* 16 kBytes */ + case 0x00008000: /* 32 kBytes */ + /* medium density, e.g. STM32F103RBT6 (Olimex STM32-H103) */ + case 0x00010000: /* 64 kBytes */ + case 0x00200000: /* 128 kBytes */ + { + flash->page_size = 0x400; + break; + } + /* high density, e.g. STM32F103RE (Joby Lisa/M, Lisa/L) */ + case 0x00040000: /* 256 kBytes */ + case 0x00080000: /* 512 kBytes */ + /* XL density */ + case 0x000C0000: /* 768 kBytes */ + case 0x00100000: /* 1 MByte */ + { + flash->page_size = 0x800; + break; + } + default: {return -1;} + } + + flash->page_nr = (flash->total_size / flash->page_size) - 1; + flash->addr = FLASH_BEGIN + flash->page_nr * flash->page_size; + + return 0; +} + +// (gdb) p *flash +// $1 = {addr = 134739968, total_size = 524288, page_nr = 255, page_size = 2048} +// 0x807F800 0x80000 +static int32_t pflash_program_bytes(struct FlashInfo* flash, + uint32_t src, + uint32_t size, + uint32_t chksum) { + uint32_t i; + + /* erase */ + flash_unlock(); + flash_erase_page(flash->addr); + flash_lock(); + + /* verify erase */ + for (i=0; ipage_size; i+=4) { + if ((*(uint32_t*) (flash->addr + i)) != 0xFFFFFFFF) return -1; + } + + + flash_unlock(); + /* write full 16 bit words */ + for (i=0; i<(size & ~1); i+=2) { + flash_program_half_word(flash->addr+i, + (uint16_t)(*(uint8_t*)(src+i) | (*(uint8_t*)(src+i+1)) << 8)); + } + /* fill bytes with a zero */ + if (size & 1) { + flash_program_half_word(flash->addr+i, (uint16_t)(*(uint8_t*)(src+i))); + } + /* write size */ + flash_program_half_word(flash->addr+flash->page_size-FSIZ, + (uint16_t)(size & 0xFFFF)); + flash_program_half_word(flash->addr+flash->page_size-FSIZ+2, + (uint16_t)((size >> 16) & 0xFFFF)); + /* write checksum */ + flash_program_half_word(flash->addr+flash->page_size-FCHK, + (uint16_t)(chksum & 0xFFFF)); + flash_program_half_word(flash->addr+flash->page_size-FCHK+2, + (uint16_t)((chksum >> 16) & 0xFFFF)); + flash_lock(); + + /* verify data */ + for (i=0; iaddr+i)) != (*(uint8_t*) (src+i))) return -2; + } + if (*(uint32_t*) (flash->addr+flash->page_size-FSIZ) != size) return -3; + if (*(uint32_t*) (flash->addr+flash->page_size-FCHK) != chksum) return -4; + + return 0; +} + + +int32_t persistent_write(uint32_t ptr, uint32_t size) { + struct FlashInfo flash_info; + if (flash_detect(&flash_info)) return -1; + if ((size > flash_info.page_size-FSIZ) || (size == 0)) return -2; + + return pflash_program_bytes(&flash_info, + ptr, + size, + pflash_checksum(ptr, size)); +} + +int32_t persistent_read(uint32_t ptr, uint32_t size) { + struct FlashInfo flash; + uint32_t i; + + /* check parameters */ + if (flash_detect(&flash)) return -1; + if ((size > flash.page_size-FSIZ) || (size == 0)) return -2; + + /* check consistency */ + if (size != *(uint32_t*)(flash.addr+flash.page_size-FSIZ)) return -3; + if (pflash_checksum(flash.addr, size) != + *(uint32_t*)(flash.addr+flash.page_size-FCHK)) + return -4; + + /* copy data */ + for (i=0; i + * + * 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. + * + */ + + +/* + + TODO: + - remove last sector from usable flash memory in STM32 linker script + + flash data is located in the last page/sector of flash + + data_begin flash_info.addr + data_size flash_info.addr + flash_info.size - 8 + checksum flash_info.addr + flash_info.size - 4 +*/ + +#ifndef STM32_SUBSYSTEMS_SETTINGS_H +#define STM32_SUBSYSTEMS_SETTINGS_H + + + +#endif /* STM32_SUBSYSTEMS_SETTINGS_H */ diff --git a/sw/airborne/boards/booz/baro_board.c b/sw/airborne/boards/booz/baro_board.c index aa43c684a5..482aa7944b 100644 --- a/sw/airborne/boards/booz/baro_board.c +++ b/sw/airborne/boards/booz/baro_board.c @@ -31,20 +31,20 @@ #define BOOZ_ANALOG_BARO_THRESHOLD 850 #endif -// pressure on AD0.1 on P0.28 -// offset on DAC on P0.25 - struct Baro baro; struct BaroBoard baro_board; + void baro_init( void ) { + + adc_buf_channel(ADC_CHANNEL_BARO, &baro_board.buf, DEFAULT_AV_NB_SAMPLE); baro.status = BS_UNINITIALIZED; baro.absolute = 0; baro.differential = 0; /* not handled on this board */ baro_board.offset = 1023; - Booz2AnalogSetDAC(baro_board.offset); + DACSet(baro_board.offset); baro_board.value_filtered = 0; baro_board.data_available = FALSE; @@ -53,7 +53,16 @@ void baro_init( void ) { #endif } -void baro_periodic(void) {} +void baro_periodic(void) { + + baro.absolute = baro_board.buf.sum/baro_board.buf.av_nb_sample; + baro_board.value_filtered = (3*baro_board.value_filtered + baro.absolute)/4; + if (baro.status == BS_UNINITIALIZED) { + RunOnceEvery(10, { baro_board_calibrate();}); + } + /* else */ + baro_board.data_available = TRUE; +} /* decrement offset until adc reading is over a threshold */ void baro_board_calibrate(void) { @@ -62,7 +71,7 @@ void baro_board_calibrate(void) { baro_board.offset -= 15; else baro_board.offset--; - Booz2AnalogSetDAC(baro_board.offset); + DACSet(baro_board.offset); #ifdef ROTORCRAFT_BARO_LED LED_TOGGLE(ROTORCRAFT_BARO_LED); #endif diff --git a/sw/airborne/boards/booz/baro_board.h b/sw/airborne/boards/booz/baro_board.h index f1fae16a48..902aef2db1 100644 --- a/sw/airborne/boards/booz/baro_board.h +++ b/sw/airborne/boards/booz/baro_board.h @@ -4,14 +4,15 @@ #include "std.h" #include "subsystems/sensors/baro.h" -#include "booz/booz2_analog.h" +#include "mcu_periph/adc.h" +#include "mcu_periph/dac.h" -/* we don't need that on this board */ struct BaroBoard { uint16_t offset; uint16_t value_filtered; bool_t data_available; + struct adc_buf buf; }; extern struct BaroBoard baro_board; @@ -27,20 +28,8 @@ extern void baro_board_calibrate(void); static inline void baro_board_SetOffset(uint16_t _o) { baro_board.offset = _o; - Booz2AnalogSetDAC(_o); + DACSet(_o); } -static inline void BoozBaroISRHandler(uint16_t _val) { - baro.absolute = _val; - baro_board.value_filtered = (3*baro_board.value_filtered + baro.absolute)/4; - if (baro.status == BS_UNINITIALIZED) { - RunOnceEvery(10, { baro_board_calibrate();}); - } - /* else */ - baro_board.data_available = TRUE; -} - - - #endif /* BOARDS_BOOZ_BARO_H */ diff --git a/sw/airborne/boards/booz_1.0.h b/sw/airborne/boards/booz_1.0.h index 9f44e89514..db7e8069b6 100644 --- a/sw/airborne/boards/booz_1.0.h +++ b/sw/airborne/boards/booz_1.0.h @@ -54,11 +54,57 @@ /* ADC */ -/* pressure : P0.10 AD1.2 */ -#define ANALOG_BARO_PINSEL PINSEL0 -#define ANALOG_BARO_PINSEL_VAL 0x03 -#define ANALOG_BARO_PINSEL_BIT 20 -#define ANALOG_BARO_ADC 1 +/* select P0.13 (ADC_SPARE) as AD1.4 for ADC_0 */ +#define ADC_0 AdcBank1(4) +#ifdef USE_ADC_0 +#ifndef USE_AD1 +#define USE_AD1 +#endif +#define USE_AD1_4 +#endif + +/* select P0.4 (SCK_0) as AD0.6 for ADC_1 */ +#define ADC_1 AdcBank0(6) +#ifdef USE_ADC_1 +#ifndef USE_AD0 +#define USE_AD0 +#endif +#define USE_AD0_6 +#endif + +/* select P0.5 (MISO_0) as AD0.7 for ADC_2 */ +#define ADC_2 AdcBank0(7) +#ifdef USE_ADC_2 +#ifndef USE_AD0 +#define USE_AD0 +#endif +#define USE_AD0_7 +#endif + +/* select P0.6 (MOSI_0) as AD1.0 for ADC_3 */ +#define ADC_3 AdcBank1(0) +#ifdef USE_ADC_3 +#ifndef USE_AD1 +#define USE_AD1 +#endif +#define USE_AD1_0 +#endif + +/* battery */ +#define ADC_CHANNEL_VSUPPLY AdcBank0(2) +#ifndef USE_AD0 +#define USE_AD0 +#endif +#define USE_AD0_2 + +#define DefaultVoltageOfAdc(adc) (0.0183*adc) + +/* baro */ +#define ADC_CHANNEL_BARO AdcBank1(2) +#ifndef USE_AD1 +#define USE_AD1 +#endif +#define USE_AD1_2 diff --git a/sw/airborne/boards/lisa_l_1.0.h b/sw/airborne/boards/lisa_l_1.0.h index 0a157060a0..0923a56a40 100644 --- a/sw/airborne/boards/lisa_l_1.0.h +++ b/sw/airborne/boards/lisa_l_1.0.h @@ -1,5 +1,5 @@ -#ifndef CONFIG_LISA_V1_0_H -#define CONFIG_LISA_V1_0_H +#ifndef CONFIG_LISA_L_1_0_H +#define CONFIG_LISA_L_1_0_H #define BOARD_LISA_L @@ -23,9 +23,17 @@ #define IMU_ACC_DRDY_GPIO_PORTSOURCE GPIO_PortSourceGPIOD +/* PA0 - ADC0 */ #define ADC_CHANNEL_VSUPPLY 2 #define DefaultVoltageOfAdc(adc) (0.0059*adc) +/* Onboard ADCs */ +#define BOARD_ADC_CHANNEL_1 ADC_Channel_8 +#define BOARD_ADC_CHANNEL_2 ADC_Channel_9 +// FIXME - removed for now and used for battery monitoring +//#define BOARD_ADC_CHANNEL_3 ADC_Channel_13 +#define BOARD_ADC_CHANNEL_3 ADC_Channel_0 +#define BOARD_ADC_CHANNEL_4 ADC_Channel_15 #define BOARD_HAS_BARO -#endif /* CONFIG_LISA_V1_0_H */ +#endif /* CONFIG_LISA_L_1_0_H */ diff --git a/sw/airborne/boards/lisa_m/baro_board.c b/sw/airborne/boards/lisa_m/baro_board.c index 11829b5acf..4d87ea5003 100644 --- a/sw/airborne/boards/lisa_m/baro_board.c +++ b/sw/airborne/boards/lisa_m/baro_board.c @@ -9,12 +9,11 @@ struct i2c_transaction baro_trans; static inline void baro_board_write_to_register(uint8_t baro_addr, uint8_t reg_addr, uint8_t val_msb, uint8_t val_lsb); static inline void baro_board_read_from_register(uint8_t baro_addr, uint8_t reg_addr); static inline void baro_board_set_current_register(uint8_t baro_addr, uint8_t reg_addr); -static inline void baro_board_read_from_current_register(uint8_t baro_addr); +static inline void baro_board_read(void); -// absolute -#define BARO_ABS_ADDR 0x90 -// differential -#define BARO_DIFF_ADDR 0x92 + +#define BMP085_SAMPLE_PERIOD_MS (3 + (2 << BMP085_OSS) * 3) +#define BMP085_SAMPLE_PERIOD (BMP075_SAMPLE_PERIOD_MS >> 1) void baro_init(void) { baro.status = BS_UNINITIALIZED; @@ -23,6 +22,22 @@ void baro_init(void) { baro_board.status = LBS_UNINITIALIZED; } +static inline void bmp085_write_reg(uint8_t addr, uint8_t value) +{ + baro_trans.type = I2CTransTx; + baro_trans.slave_addr = BMP085_ADDR; + baro_trans.len_w = 2; + baro_trans.buf[0] = addr; + baro_trans.buf[1] = value; + i2c_submit(&i2c2, &baro_trans); + while (baro_trans.status == I2CTransPending || baro_trans.status == I2CTransRunning); +} + +static inline void bmp085_request_pressure(void) +{ + bmp085_write_reg(0xF4, 0x34 + (BMP085_OSS << 6)); +} + void baro_periodic(void) { // check i2c_done @@ -33,31 +48,22 @@ void baro_periodic(void) { baro_board.status = LBS_RESETED; break; case LBS_RESETED: - baro_board_send_config_abs(); - baro_board.status = LBS_INITIALIZING_ABS; + baro_board_send_config(); + baro_board.status = LBS_INITIALIZING; break; - case LBS_INITIALIZING_ABS: - baro_board_set_current_register(BARO_ABS_ADDR, 0x00); - baro_board.status = LBS_INITIALIZING_ABS_1; + case LBS_INITIALIZING: + baro_board_set_current_register(BMP085_ADDR, 0x00); + baro_board.status = LBS_INITIALIZING_1; break; - case LBS_INITIALIZING_ABS_1: - baro_board_send_config_diff(); - baro_board.status = LBS_INITIALIZING_DIFF; - break; - case LBS_INITIALIZING_DIFF: - baro_board_set_current_register(BARO_DIFF_ADDR, 0x00); - baro_board.status = LBS_INITIALIZING_DIFF_1; - // baro_board.status = LBS_UNINITIALIZED; - break; - case LBS_INITIALIZING_DIFF_1: + case LBS_INITIALIZING_1: baro.status = BS_RUNNING; - case LBS_READ_DIFF: - baro_board_read_from_current_register(BARO_ABS_ADDR); - baro_board.status = LBS_READING_ABS; + case LBS_REQUEST: + bmp085_request_pressure(); + baro_board.status = LBS_READ; break; - case LBS_READ_ABS: - baro_board_read_from_current_register(BARO_DIFF_ADDR); - baro_board.status = LBS_READING_DIFF; + case LBS_READ: + baro_board_read(); + baro_board.status = LBS_READING; break; default: break; @@ -66,12 +72,9 @@ void baro_periodic(void) { } -void baro_board_send_config_abs(void) { - baro_board_write_to_register(BARO_ABS_ADDR, 0x01, 0x86, 0x83); -} - -void baro_board_send_config_diff(void) { - baro_board_write_to_register(BARO_DIFF_ADDR, 0x01, 0x84, 0x83); +void baro_board_send_config(void) { + /* maybe we should read factory calibration here */ + //baro_board_write_to_register(BMP085_ADDR, 0x01, 0x86, 0x83); } void baro_board_send_reset(void) { @@ -82,6 +85,19 @@ void baro_board_send_reset(void) { i2c_submit(&i2c2,&baro_trans); } +static inline void bmp085_read_reg24(uint8_t addr) +{ + baro_trans.type = I2CTransTxRx; + baro_trans.slave_addr = BMP085_ADDR; + baro_trans.len_w = 1; + baro_trans.len_r = 3; + baro_trans.buf[0] = addr; + i2c_submit(&i2c2, &baro_trans); + //while (baro_trans.status == I2CTransPending || baro_trans.status == I2CTransRunning); + + //return (baro_trans.buf[0] << 16) | (baro_trans.buf[1] >> 8) | (baro_trans.buf[2]); +} + static inline void baro_board_write_to_register(uint8_t baro_addr, uint8_t reg_addr, uint8_t val_msb, uint8_t val_lsb) { baro_trans.type = I2CTransTx; baro_trans.slave_addr = baro_addr; @@ -99,8 +115,6 @@ static inline void baro_board_read_from_register(uint8_t baro_addr, uint8_t reg_ baro_trans.len_r = 2; baro_trans.buf[0] = reg_addr; i2c_submit(&i2c2,&baro_trans); - // i2c2.buf[0] = reg_addr; - // i2c2_transceive(baro_addr, 1, 2, &baro_board.i2c_done); } static inline void baro_board_set_current_register(uint8_t baro_addr, uint8_t reg_addr) { @@ -109,14 +123,21 @@ static inline void baro_board_set_current_register(uint8_t baro_addr, uint8_t re baro_trans.len_w = 1; baro_trans.buf[0] = reg_addr; i2c_submit(&i2c2,&baro_trans); - // i2c2.buf[0] = reg_addr; - // i2c2_transmit(baro_addr, 1, &baro_board.i2c_done); } -static inline void baro_board_read_from_current_register(uint8_t baro_addr) { - baro_trans.type = I2CTransRx; - baro_trans.slave_addr = baro_addr; - baro_trans.len_r = 2; - i2c_submit(&i2c2,&baro_trans); - // i2c2_receive(baro_addr, 2, &baro_board.i2c_done); + +static inline void bmp085_read_pressure(void) +{ + bmp085_read_reg24(0xF6); +} + +static inline void baro_board_read() +{ + //int32_t x; + //bmp085_request_pressure(); + bmp085_read_pressure(); + //baro_trans.type = I2CTransRx; + //baro_trans.slave_addr = BMP085_ADDR; + //baro_trans.len_r = 2; + //i2c_submit(&i2c2,&baro_trans); } diff --git a/sw/airborne/boards/lisa_m/baro_board.h b/sw/airborne/boards/lisa_m/baro_board.h index e2d61a79db..374f9d30ae 100644 --- a/sw/airborne/boards/lisa_m/baro_board.h +++ b/sw/airborne/boards/lisa_m/baro_board.h @@ -4,24 +4,25 @@ * */ -#ifndef BOARDS_LISA_L_BARO_H -#define BOARDS_LISA_L_BARO_H +#ifndef BOARDS_LISA_M_BARO_H +#define BOARDS_LISA_M_BARO_H #include "std.h" #include "mcu_periph/i2c.h" +// absolute +#define BMP085_ADDR 0xEE +#define BMP085_OSS 3 + enum LisaBaroStatus { LBS_UNINITIALIZED, LBS_RESETED, - LBS_INITIALIZING_ABS, - LBS_INITIALIZING_ABS_1, - LBS_INITIALIZING_DIFF, - LBS_INITIALIZING_DIFF_1, + LBS_INITIALIZING, + LBS_INITIALIZING_1, LBS_IDLE, - LBS_READING_ABS, - LBS_READ_ABS, - LBS_READING_DIFF, - LBS_READ_DIFF + LBS_REQUEST, + LBS_READING, + LBS_READ, }; struct BaroBoard { @@ -32,31 +33,21 @@ extern struct BaroBoard baro_board; extern struct i2c_transaction baro_trans; extern void baro_board_send_reset(void); -extern void baro_board_send_config_abs(void); -extern void baro_board_send_config_diff(void); +extern void baro_board_send_config(void); #define BaroEvent(_b_abs_handler, _b_diff_handler) { \ - if (baro_board.status == LBS_READING_ABS && \ + if (baro_board.status == LBS_READING && \ baro_trans.status != I2CTransPending) { \ - baro_board.status = LBS_READ_ABS; \ + baro_board.status = LBS_REQUEST; \ if (baro_trans.status == I2CTransSuccess) { \ - int16_t tmp = baro_trans.buf[0]<<8 | baro_trans.buf[1]; \ - baro.absolute = tmp; \ + int32_t tmp = (baro_trans.buf[0]<<16) | (baro_trans.buf[1] << 8) | baro_trans.buf[0]; \ + baro.absolute = tmp >> ( 8 - BMP085_OSS); \ _b_abs_handler(); \ } \ } \ - else if (baro_board.status == LBS_READING_DIFF && \ - baro_trans.status != I2CTransPending) { \ - baro_board.status = LBS_READ_DIFF; \ - if (baro_trans.status == I2CTransSuccess) { \ - int16_t tmp = baro_trans.buf[0]<<8 | baro_trans.buf[1]; \ - baro.differential = tmp; \ - _b_diff_handler(); \ - } \ - } \ } -#endif /* BOARDS_LISA_L_BARO_H */ +#endif /* BOARDS_LISA_M_BARO_H */ diff --git a/sw/airborne/boards/lisa_m/test_baro.c b/sw/airborne/boards/lisa_m/test_baro.c new file mode 100644 index 0000000000..8a24041f53 --- /dev/null +++ b/sw/airborne/boards/lisa_m/test_baro.c @@ -0,0 +1,108 @@ +/* + * $Id$ + * + * Copyright (C) 2009 Antoine Drouin + * + * 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. + */ + +/* + * + * test baro using interrupts + * + */ + +#include BOARD_CONFIG + +#include "mcu.h" +#include "sys_time.h" +#include "mcu_periph/uart.h" + +#include "downlink.h" + +#include "subsystems/sensors/baro.h" +//#include "my_debug_servo.h" + +static inline void main_init( void ); +static inline void main_periodic_task( void ); +static inline void main_event_task( void ); + +static inline void main_on_baro_diff(void); +static inline void main_on_baro_abs(void); + + +int main(void) { + main_init(); + + while(1) { + if (sys_time_periodic()) + main_periodic_task(); + main_event_task(); + } + + return 0; +} + +static inline void main_init( void ) { + mcu_init(); + sys_time_init(); + baro_init(); + + // DEBUG_SERVO1_INIT(); + // DEBUG_SERVO2_INIT(); + + +} + + + +static inline void main_periodic_task( void ) { + + RunOnceEvery(2, {baro_periodic();}); + LED_PERIODIC(); + RunOnceEvery(256, {DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);}); + RunOnceEvery(256, + { + DOWNLINK_SEND_I2C_ERRORS(DefaultChannel, + &i2c2_errors.ack_fail_cnt, + &i2c2_errors.miss_start_stop_cnt, + &i2c2_errors.arb_lost_cnt, + &i2c2_errors.over_under_cnt, + &i2c2_errors.pec_recep_cnt, + &i2c2_errors.timeout_tlow_cnt, + &i2c2_errors.smbus_alert_cnt, + &i2c2_errors.unexpected_event_cnt, + &i2c2_errors.last_unexpected_event); + }); +} + + + +static inline void main_event_task( void ) { + BaroEvent(main_on_baro_abs, main_on_baro_diff); +} + + + +static inline void main_on_baro_diff(void) { + +} + +static inline void main_on_baro_abs(void) { + RunOnceEvery(5,{DOWNLINK_SEND_BARO_RAW(DefaultChannel, &baro.absolute, &baro.differential);}); +} diff --git a/sw/airborne/boards/lisa_m_1.0.h b/sw/airborne/boards/lisa_m_1.0.h index a0941c8737..fd3ddee6bb 100644 --- a/sw/airborne/boards/lisa_m_1.0.h +++ b/sw/airborne/boards/lisa_m_1.0.h @@ -18,18 +18,48 @@ #define LED_2_GPIO_PIN GPIO_Pin_13 #define LED_2_AFIO_REMAP ((void)0) + /* configuration for aspirin - and more generaly IMUs */ #define IMU_ACC_DRDY_RCC_GPIO RCC_APB2Periph_GPIOB #define IMU_ACC_DRDY_GPIO GPIOB #define IMU_ACC_DRDY_GPIO_PORTSOURCE GPIO_PortSourceGPIOB -#define ADC_CHANNEL_VSUPPLY 4 -#define DefaultVoltageOfAdc(adc) (0.01787109375*adc) + +#define ADC_CHANNEL_VSUPPLY 2 +#define DefaultVoltageOfAdc(adc) (0.00485*adc) + +/* Onboard ADCs */ +/* + ADC1 PC3/ADC13 + ADC2 PA0/ADC0 + ADC3 PC0/ADC10 + ADC4 PC1/ADC11 + ADC5 PC5/ADC15 + ADC6 PA1/ADC1 + ADC7 PC2/ADC12 + BATT PC4/ADC14 +*/ +#define BOARD_ADC_CHANNEL_1 ADC_Channel_13 +#define BOARD_ADC_CHANNEL_2 ADC_Channel_0 +// FIXME - removed for now and used for battery monitoring +//#define BOARD_ADC_CHANNEL_3 ADC_Channel_10 +#define BOARD_ADC_CHANNEL_3 ADC_Channel_14 +#define BOARD_ADC_CHANNEL_4 ADC_Channel_11 #define BOARD_HAS_BARO +#define USE_OPENCM3 + #define HSE_TYPE_EXT_CLK #define STM32_RCC_MODE RCC_HSE_ON #define STM32_PLL_MULT RCC_PLLMul_6 +#define PWM_5AND6_TIMER TIM5 +#define PWM_5AND6_RCC RCC_APB1Periph_TIM5 +#define PWM5_OC 1 +#define PWM6_OC 2 +#define PWM_5AND6_GPIO GPIOA +#define PWM5_Pin GPIO_Pin_0 +#define PWM6_Pin GPIO_Pin_1 + #endif /* CONFIG_LISA_M_1_0_H */ diff --git a/sw/airborne/booz/arch/lpc21/booz2_analog_hw.c b/sw/airborne/booz/arch/lpc21/booz2_analog_hw.c deleted file mode 100644 index a991d3e3ed..0000000000 --- a/sw/airborne/booz/arch/lpc21/booz2_analog_hw.c +++ /dev/null @@ -1,229 +0,0 @@ -/* - * $Id$ - * - * Copyright (C) 2008-2009 Antoine Drouin - * - * 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. - */ - -#include "booz2_analog.h" - -/* analog_arch includes baro ??? naaaa we don't want double references */ -#include "subsystems/sensors/baro.h" -#include "firmwares/rotorcraft/battery.h" - -#ifndef USE_EXTRA_ADC -/* Default mode - * Bust OFF - * Only one ADC can be read on each bank - * Baro and Bat are read on interrupt - */ - -#include "armVIC.h" -#include "sys_time.h" - -void ADC0_ISR ( void ) __attribute__((naked)); -void ADC1_ISR ( void ) __attribute__((naked)); - -void booz2_analog_init_hw( void ) { - - /* start ADC0 */ - /* select P0.29 as AD0.2 for bat meas*/ - PINSEL1 |= 0x01 << 26; - /* sample AD0.2 - PCLK/4 ( 3.75MHz) - ON */ - AD0CR = 1 << 2 | 0x03 << 8 | 1 << 21; - /* AD0 selected as IRQ */ - VICIntSelect &= ~VIC_BIT(VIC_AD0); - /* AD0 interrupt enabled */ - VICIntEnable = VIC_BIT(VIC_AD0); - /* AD0 interrupt as VIC2 */ - _VIC_CNTL(ADC0_VIC_SLOT) = VIC_ENABLE | VIC_AD0; - _VIC_ADDR(ADC0_VIC_SLOT) = (uint32_t)ADC0_ISR; - /* start convertion on T0M1 match */ - AD0CR |= 4 << 24; - - - /* clear match 1 */ - T0EMR &= ~TEMR_EM1; - /* set high on match 1 */ - T0EMR |= TEMR_EMC1_2; - /* first match in a while */ - T0MR1 = 1024; - - - /* start ADC1 */ - /* select P0.10 as AD1.2 for baro*/ - ANALOG_BARO_PINSEL |= ANALOG_BARO_PINSEL_VAL << ANALOG_BARO_PINSEL_BIT; - /* sample AD1.2 - PCLK/4 ( 3.75MHz) - ON */ - AD1CR = 1 << 2 | 0x03 << 8 | 1 << 21; - /* AD0 selected as IRQ */ - VICIntSelect &= ~VIC_BIT(VIC_AD1); - /* AD0 interrupt enabled */ - VICIntEnable = VIC_BIT(VIC_AD1); - /* AD0 interrupt as VIC2 */ - _VIC_CNTL(ADC1_VIC_SLOT) = VIC_ENABLE | VIC_AD1; - _VIC_ADDR(ADC1_VIC_SLOT) = (uint32_t)ADC1_ISR; - /* start convertion on T0M3 match */ - AD1CR |= 5 << 24; - - - /* clear match 2 */ - T0EMR &= ~TEMR_EM3; - /* set high on match 2 */ - T0EMR |= TEMR_EMC3_2; - /* first match in a while */ - T0MR3 = 512; - - /* turn on DAC pins */ - PINSEL1 |= 2 << 18; -} - - -void ADC0_ISR ( void ) { - ISR_ENTRY(); - uint32_t tmp = AD0GDR; - uint16_t tmp2 = (uint16_t)(tmp >> 6) & 0x03FF; - BatteryISRHandler(tmp2); - /* trigger next convertion */ - T0MR1 += BOOZ2_ANALOG_BATTERY_PERIOD; - /* lower clock */ - T0EMR &= ~TEMR_EM1; - VICVectAddr = 0x00000000; // clear this interrupt from the VIC - ISR_EXIT(); // recover registers and return -} - -void ADC1_ISR ( void ) { - ISR_ENTRY(); - uint32_t tmp = AD1GDR; - uint16_t tmp2 = (uint16_t)(tmp >> 6) & 0x03FF; - BoozBaroISRHandler(tmp2); - /* trigger next convertion */ - T0MR3 += BOOZ2_ANALOG_BARO_PERIOD; - /* lower clock */ - T0EMR &= ~TEMR_EM3; - VICVectAddr = 0x00000000; // clear this interrupt from the VIC - ISR_EXIT(); // recover registers and return -} - -#else // USE_EXTRA_ADC -/* Extra ADCs are read - * Bust ON - * Baro and Bat values are updated by hand - * Four ADCs can be configured - * ADC_1 is available on the cam connector - */ - -#include "LPC21xx.h" -#include "sys_time.h" - -uint16_t booz2_adc_1; -uint16_t booz2_adc_2; -uint16_t booz2_adc_3; -uint16_t booz2_adc_4; - -void booz2_analog_init_hw( void ) { - - /* AD0 */ - /* PCLK/4 ( 3.75MHz) - BURST - ON */ - AD0CR = 0x03 << 8 | 1 << 16 | 1 << 21; - /* disable global interrupt */ - ClearBit(AD0INTEN,8); - - /* AD1 */ - /* PCLK/4 ( 3.75MHz) - BURST - ON */ - AD1CR = 0x03 << 8 | 1 << 16 | 1 << 21; - /* disable global interrupt */ - ClearBit(AD1INTEN,8); - - /* select P0.29 as AD0.2 for bat meas*/ - PINSEL1 |= 0x01 << 26; - /* sample AD0.2 */ - AD0CR |= 1 << 2; - - - /* select P0.10 as AD1.2 for baro*/ - ANALOG_BARO_PINSEL |= ANALOG_BARO_PINSEL_VAL << ANALOG_BARO_PINSEL_BIT; - /* sample AD1.2 */ - AD1CR |= 1 << 2; - /* turn on DAC pins */ - PINSEL1 |= 2 << 18; - -#ifdef USE_ADC_1 - /* select P0.13 (ADC_SPARE) as AD1.4 adc 1 */ - PINSEL0 |= 0x03 << 26; - AD1CR |= 1 << 4; -#endif -#ifdef USE_ADC_2 - /* select P0.4 (SCK_0) as AD0.6 adc 2 */ - PINSEL0 |= 0x03 << 8; - AD0CR |= 1 << 6; -#endif -#ifdef USE_ADC_3 - /* select P0.5 (MISO_0) as AD0.7 adc 3 */ - PINSEL0 |= 0x03 << 10; - AD0CR |= 1 << 7; -#endif -#ifdef USE_ADC_4 - /* select P0.6 (MOSI_0) as AD1.0 adc 4 */ - PINSEL0 |= 0x03 << 12; - AD1CR |= 1 << 0; -#endif - - booz2_adc_1 = 0; - booz2_adc_2 = 0; - booz2_adc_3 = 0; - booz2_adc_4 = 0; -} - -void booz2_analog_baro_read(void) { - uint32_t tmp = AD1DR2; - uint16_t tmp2 = (uint16_t)(tmp >> 6) & 0x03FF; - BoozBaroISRHandler(tmp2); -} - -void booz2_analog_bat_read(void) { - uint32_t tmp = AD0DR2; - uint16_t tmp2 = (uint16_t)(tmp >> 6) & 0x03FF; - BatteryISRHandler(tmp2); -} - -void booz2_analog_extra_adc_read(void) { - uint32_t tmp,tmp2; -#ifdef USE_ADC_1 - tmp = AD1DR4; - tmp2 = (uint16_t)(tmp >> 6) & 0x03FF; - booz2_adc_1 = tmp2; -#endif -#ifdef USE_ADC_2 - tmp = AD0DR6; - tmp2 = (uint16_t)(tmp >> 6) & 0x03FF; - booz2_adc_2 = tmp2; -#endif -#ifdef USE_ADC_3 - tmp = AD0DR7; - tmp2 = (uint16_t)(tmp >> 6) & 0x03FF; - booz2_adc_3 = tmp2; -#endif -#ifdef USE_ADC_4 - tmp = AD1DR0; - tmp2 = (uint16_t)(tmp >> 6) & 0x03FF; - booz2_adc_4 = tmp2; -#endif -} - -#endif // USE_EXTRA_ADC diff --git a/sw/airborne/booz/arch/lpc21/booz2_analog_hw.h b/sw/airborne/booz/arch/lpc21/booz2_analog_hw.h deleted file mode 100644 index ae87ad0c84..0000000000 --- a/sw/airborne/booz/arch/lpc21/booz2_analog_hw.h +++ /dev/null @@ -1,36 +0,0 @@ -/* - * $Id$ - * - * Copyright (C) 2008-2009 Antoine Drouin - * - * 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. - */ - -#ifndef BOOZ2_ANALOG_HW_H -#define BOOZ2_ANALOG_HW_H - -#include "LPC21xx.h" -#include "std.h" - -static inline void Booz2AnalogSetDAC(uint16_t x) { - DACR = x << 6; -} - -extern void booz2_analog_init_hw(void); - -#endif /* BOOZ2_ANALOG_HW_H */ diff --git a/sw/airborne/booz/arch/sim/booz2_analog_hw.h b/sw/airborne/booz/arch/sim/booz2_analog_hw.h deleted file mode 100644 index 7e2c3231d9..0000000000 --- a/sw/airborne/booz/arch/sim/booz2_analog_hw.h +++ /dev/null @@ -1,31 +0,0 @@ -/* - * $Id$ - * - * Copyright (C) 2008-2009 Antoine Drouin - * - * 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. - */ - -#ifndef BOOZ2_ANALOG_HW_H -#define BOOZ2_ANALOG_HW_H - -#define Booz2AnalogSetDAC(x) { } - -extern void booz2_analog_init_hw(void); - -#endif /* BOOZ2_ANALOG_HW_H */ diff --git a/sw/airborne/booz/arch/sim/booz2_unsimulated_peripherals.c b/sw/airborne/booz/arch/sim/booz2_unsimulated_peripherals.c deleted file mode 100644 index 5a86889e63..0000000000 --- a/sw/airborne/booz/arch/sim/booz2_unsimulated_peripherals.c +++ /dev/null @@ -1,31 +0,0 @@ -/* - * $Id$ - * - * Copyright (C) 2008-2009 Antoine Drouin - * - * 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. - */ - -#include "mcu_periph/uart.h" -//#include "mcu_periph/i2c.h" - -void uart0_init( void ) {} -void uart1_init( void ) {} - -//void i2c0_hw_init( void ) {} -//void i2c1_hw_init( void ) {} diff --git a/sw/airborne/booz/arch/stm32/booz2_analog_hw.h b/sw/airborne/booz/arch/stm32/booz2_analog_hw.h deleted file mode 100644 index 9c074731f2..0000000000 --- a/sw/airborne/booz/arch/stm32/booz2_analog_hw.h +++ /dev/null @@ -1,33 +0,0 @@ -/* - * $Id: booz2_analog_hw.h 4172 2009-09-18 11:57:13Z flixr $ - * - * Copyright (C) 2008-2009 Antoine Drouin - * - * 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. - */ - -#ifndef BOOZ2_ANALOG_HW_H -#define BOOZ2_ANALOG_HW_H - -#include "std.h" - -static inline void Booz2AnalogSetDAC(uint16_t x) {} - -extern void booz2_analog_init_hw(void); - -#endif /* BOOZ2_ANALOG_HW_H */ diff --git a/sw/airborne/booz/booz2_analog.h b/sw/airborne/booz/booz2_analog.h deleted file mode 100644 index 1c4aa63481..0000000000 --- a/sw/airborne/booz/booz2_analog.h +++ /dev/null @@ -1,47 +0,0 @@ -/* - * $Id$ - * - * Copyright (C) 2008-2009 Antoine Drouin - * - * 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. - */ - -#ifndef BOOZ2_ANALOG_H -#define BOOZ2_ANALOG_H - -extern void booz2_analog_init( void ); - -#ifdef USE_EXTRA_ADC -#include "std.h" - -extern uint16_t booz2_adc_1; -extern uint16_t booz2_adc_2; -extern uint16_t booz2_adc_3; -extern uint16_t booz2_adc_4; - -extern void booz2_analog_periodic( void ); - -extern void booz2_analog_baro_read(void); -extern void booz2_analog_bat_read(void); -extern void booz2_analog_extra_adc_read(void); -#endif - - -#include "booz2_analog_hw.h" - -#endif /* BOOZ2_ANALOG_H */ diff --git a/sw/airborne/booz/test/booz2_test_crista.c b/sw/airborne/booz/test/booz2_test_crista.c index 82f990afe4..1231b7e6ab 100644 --- a/sw/airborne/booz/test/booz2_test_crista.c +++ b/sw/airborne/booz/test/booz2_test_crista.c @@ -108,11 +108,11 @@ static inline void on_imu_event(void) { &imu_accel_unscaled.z); } else if (cnt == 7) { - DOWNLINK_SEND_BOOZ2_GYRO(&imu_gyro.x, + DOWNLINK_SEND_IMU_GYRO_SCALED(&imu_gyro.x, &imu_gyro.y, &imu_gyro.z); - DOWNLINK_SEND_BOOZ2_ACCEL(&imu_accel.x, + DOWNLINK_SEND_IMU_ACCEL_SCALED(&imu_accel.x, &imu_accel.y, &imu_accel.z); } diff --git a/sw/airborne/booz/test/booz_test_imu.c b/sw/airborne/booz/test/booz_test_imu.c index fa4bd2591d..19146b63cb 100644 --- a/sw/airborne/booz/test/booz_test_imu.c +++ b/sw/airborne/booz/test/booz_test_imu.c @@ -23,6 +23,9 @@ #include +#ifdef BOARD_CONFIG +#include BOARD_CONFIG +#endif #include "std.h" #include "mcu.h" #include "sys_time.h" @@ -61,16 +64,20 @@ static inline void main_init( void ) { sys_time_init(); imu_init(); - DEBUG_SERVO1_INIT(); - DEBUG_SERVO2_INIT(); - mcu_int_enable(); } +static inline void led_toggle ( void ) { + +#ifdef BOARD_LISA_L + LED_TOGGLE(3); +#endif +} + static inline void main_periodic_task( void ) { RunOnceEvery(100, { - LED_TOGGLE(3); + led_toggle(); DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM); }); #ifdef USE_I2C2 @@ -95,6 +102,7 @@ static inline void main_event_task( void ) { ImuEvent(on_gyro_accel_event, on_accel_event, on_mag_event); + } static inline void on_accel_event(void) { @@ -110,7 +118,7 @@ static inline void on_accel_event(void) { &imu.accel_unscaled.z); } else if (cnt == 7) { - DOWNLINK_SEND_BOOZ2_ACCEL(DefaultChannel, + DOWNLINK_SEND_IMU_ACCEL_SCALED(DefaultChannel, &imu.accel.x, &imu.accel.y, &imu.accel.z); @@ -132,7 +140,7 @@ static inline void on_gyro_accel_event(void) { &imu.gyro_unscaled.r); } else if (cnt == 7) { - DOWNLINK_SEND_BOOZ2_GYRO(DefaultChannel, + DOWNLINK_SEND_IMU_GYRO_SCALED(DefaultChannel, &imu.gyro.p, &imu.gyro.q, &imu.gyro.r); @@ -147,7 +155,7 @@ static inline void on_mag_event(void) { if (cnt > 10) cnt = 0; if (cnt == 0) { - DOWNLINK_SEND_BOOZ2_MAG(DefaultChannel, + DOWNLINK_SEND_IMU_MAG_SCALED(DefaultChannel, &imu.mag.x, &imu.mag.y, &imu.mag.z); diff --git a/sw/airborne/csc/mercury_xsens.h b/sw/airborne/csc/mercury_xsens.h index 9b04d79737..a2f1857a97 100644 --- a/sw/airborne/csc/mercury_xsens.h +++ b/sw/airborne/csc/mercury_xsens.h @@ -64,20 +64,20 @@ extern int xsens_setzero; #include "subsystems/ahrs.h" -#define PERIODIC_SEND_BOOZ2_GYRO() { \ - DOWNLINK_SEND_BOOZ2_GYRO(&imu.gyro.p, \ +#define PERIODIC_SEND_IMU_GYRO_SCALED() { \ + DOWNLINK_SEND_IMU_GYRO_SCALED(&imu.gyro.p, \ &imu.gyro.q, \ &imu.gyro.r); \ } -#define PERIODIC_SEND_BOOZ2_ACCEL() { \ - DOWNLINK_SEND_BOOZ2_ACCEL(&imu.accel.x, \ +#define PERIODIC_SEND_IMU_ACCEL_SCALED() { \ + DOWNLINK_SEND_IMU_ACCEL_SCALED(&imu.accel.x, \ &imu.accel.y, \ &imu.accel.z); \ } -#define PERIODIC_SEND_BOOZ2_MAG() { \ - DOWNLINK_SEND_BOOZ2_MAG(&imu.mag.x, \ +#define PERIODIC_SEND_IMU_MAG_SCALED() { \ + DOWNLINK_SEND_IMU_MAG_SCALED(&imu.mag.x, \ &imu.mag.y, \ &imu.mag.z); \ } diff --git a/sw/airborne/downlink.c b/sw/airborne/downlink.c index dbd07749aa..003b816beb 100644 --- a/sw/airborne/downlink.c +++ b/sw/airborne/downlink.c @@ -29,6 +29,7 @@ #include "std.h" +#include "generated/airframe.h" #ifdef FBW #ifndef TELEMETRY_MODE_FBW diff --git a/sw/airborne/downlink.h b/sw/airborne/downlink.h index 17052669ed..26dcab2988 100644 --- a/sw/airborne/downlink.h +++ b/sw/airborne/downlink.h @@ -51,6 +51,9 @@ #include "pprz_transport.h" #include "modem.h" #include "xbee.h" +#ifdef USE_USB_SERIAL +#include "mcu_periph/usb_serial.h" +#endif #endif /** !SITL */ #ifndef DefaultChannel diff --git a/sw/airborne/firmwares/beth/main_overo.c b/sw/airborne/firmwares/beth/main_overo.c index 6a52bc090a..b24958321f 100644 --- a/sw/airborne/firmwares/beth/main_overo.c +++ b/sw/airborne/firmwares/beth/main_overo.c @@ -159,13 +159,13 @@ static void main_periodic(int my_sig_num) { RunOnceEvery(10, {DOWNLINK_SEND_IMU_ACCEL_RAW(gcs_com.udp_transport, //&msg_in.payload.msg_up.accel.x,&msg_in.payload.msg_up.accel.y,&msg_in.payload.msg_up.accel.z &imu.accel_unscaled.x,&imu.accel_unscaled.y,&imu.accel_unscaled.z);}) - RunOnceEvery(50, {DOWNLINK_SEND_BOOZ2_GYRO(gcs_com.udp_transport, + RunOnceEvery(50, {DOWNLINK_SEND_IMU_GYRO_SCALED(gcs_com.udp_transport, //&msg_in.payload.msg_up.gyro.p,&msg_in.payload.msg_up.gyro.q,&msg_in.payload.msg_up.gyro.r) &imu.gyro.p,&imu.gyro.q,&imu.gyro.r);}); RunOnceEvery(50, {DOWNLINK_SEND_AHRS_EULER(gcs_com.udp_transport, &estimator.tilt, &estimator.elevation, &estimator.azimuth );}); - RunOnceEvery(50, {DOWNLINK_SEND_BOOZ2_ACCEL(DefaultChannel, + RunOnceEvery(50, {DOWNLINK_SEND_IMU_ACCEL_SCALED(DefaultChannel, //&msg_in.payload.msg_up.accel.x,&msg_in.payload.msg_up.accel.y,&msg_in.payload.msg_up.accel.z &imu.accel.x,&imu.accel.y,&imu.accel.z);});*/ diff --git a/sw/airborne/firmwares/beth/main_stm32.c b/sw/airborne/firmwares/beth/main_stm32.c index ee5aca10d2..af09ab6a0a 100644 --- a/sw/airborne/firmwares/beth/main_stm32.c +++ b/sw/airborne/firmwares/beth/main_stm32.c @@ -176,12 +176,12 @@ static inline void on_gyro_accel_event(void) { &imu.accel_unscaled.z); } else if (cnt == 7) { - DOWNLINK_SEND_BOOZ2_GYRO(DefaultChannel, + DOWNLINK_SEND_IMU_GYRO_SCALED(DefaultChannel, &imu.gyro.p, &imu.gyro.q, &imu.gyro.r); - DOWNLINK_SEND_BOOZ2_ACCEL(DefaultChannel, + DOWNLINK_SEND_IMU_ACCEL_SCALED(DefaultChannel, &imu.accel.x, &imu.accel.y, &imu.accel.z); @@ -196,7 +196,7 @@ static inline void on_mag_event(void) { if (cnt > 1) cnt = 0; if (cnt%2) { - DOWNLINK_SEND_BOOZ2_MAG(DefaultChannel, + DOWNLINK_SEND_IMU_MAG_SCALED(DefaultChannel, &imu.mag.x, &imu.mag.y, &imu.mag.z); diff --git a/sw/airborne/firmwares/fixedwing/datalink.c b/sw/airborne/firmwares/fixedwing/datalink.c index 89e9c735b7..8900d5447c 100644 --- a/sw/airborne/firmwares/fixedwing/datalink.c +++ b/sw/airborne/firmwares/fixedwing/datalink.c @@ -47,10 +47,6 @@ #include "joystick.h" #endif -#ifdef USE_USB_SERIAL -#include "mcu_periph/usb_serial.h" -#endif - #ifdef HITL #include "gps.h" #endif diff --git a/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c b/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c index 839a0da20b..02ecaaf826 100644 --- a/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c +++ b/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c @@ -57,7 +57,7 @@ float v_ctl_auto_throttle_pgain; float v_ctl_auto_throttle_igain; float v_ctl_auto_throttle_dgain; float v_ctl_auto_throttle_sum_err; -#define V_CTL_AUTO_THROTTLE_MAX_SUM_ERR 150 +#define V_CTL_AUTO_THROTTLE_MAX_SUM_ERR 0.3 float v_ctl_auto_throttle_pitch_of_vz_pgain; float v_ctl_auto_throttle_pitch_of_vz_dgain; @@ -70,14 +70,21 @@ float v_ctl_auto_pitch_pgain; float v_ctl_auto_pitch_dgain; float v_ctl_auto_pitch_igain; float v_ctl_auto_pitch_sum_err; -#define V_CTL_AUTO_PITCH_MAX_SUM_ERR 100 +#define V_CTL_AUTO_PITCH_MAX_SUM_ERR (RadOfDeg(10.)) + +#ifndef V_CTL_AUTO_PITCH_DGAIN +#define V_CTL_AUTO_PITCH_DGAIN 0. +#endif +#ifndef V_CTL_AUTO_PITCH_IGAIN +#define V_CTL_AUTO_PITCH_IGAIN 0. +#endif float controlled_throttle; pprz_t v_ctl_throttle_setpoint; pprz_t v_ctl_throttle_slewed; // Set higher than 2*V_CTL_ALTITUDE_MAX_CLIMB to disable -#define V_CTL_AUTO_CLIMB_LIMIT 0.5/4.0 // m/s/s +#define V_CTL_AUTO_CLIMB_LIMIT (0.5/4.0) // m/s/s uint8_t v_ctl_speed_mode; @@ -181,16 +188,21 @@ void v_ctl_altitude_loop( void ) { static inline void v_ctl_set_pitch ( void ) { static float last_err = 0.; + if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) + v_ctl_auto_pitch_sum_err = 0; + // Compute errors float err = estimator_z_dot - v_ctl_climb_setpoint; float d_err = err - last_err; last_err = err; - v_ctl_auto_pitch_sum_err += err*(1./60.); - BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR); + if (v_ctl_auto_pitch_igain < 0.) { + v_ctl_auto_pitch_sum_err += err*(1./60.); + BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR / v_ctl_auto_pitch_igain); + } // PI loop + feedforward ctl - nav_pitch = nav_pitch + nav_pitch = 0. //nav_pitch FIXME it really sucks ! + v_ctl_auto_throttle_pitch_of_vz_pgain * v_ctl_climb_setpoint + v_ctl_auto_pitch_pgain * err + v_ctl_auto_pitch_dgain * d_err @@ -201,13 +213,18 @@ static inline void v_ctl_set_pitch ( void ) { static inline void v_ctl_set_throttle( void ) { static float last_err = 0.; + if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) + v_ctl_auto_throttle_sum_err = 0; + // Compute errors float err = estimator_z_dot - v_ctl_climb_setpoint; float d_err = err - last_err; last_err = err; - v_ctl_auto_throttle_sum_err += err*(1./60.); - BoundAbs(v_ctl_auto_throttle_sum_err, V_CTL_AUTO_THROTTLE_MAX_SUM_ERR); + if (v_ctl_auto_throttle_igain < 0.) { + v_ctl_auto_throttle_sum_err += err*(1./60.); + BoundAbs(v_ctl_auto_throttle_sum_err, V_CTL_AUTO_THROTTLE_MAX_SUM_ERR / v_ctl_auto_throttle_igain); + } // PID loop + feedforward ctl controlled_throttle = v_ctl_auto_throttle_cruise_throttle @@ -269,8 +286,7 @@ void v_ctl_climb_loop ( void ) { } // Set Throttle output - float f_throttle = controlled_throttle; - v_ctl_throttle_setpoint = TRIM_UPPRZ(f_throttle * MAX_PPRZ); + v_ctl_throttle_setpoint = TRIM_UPPRZ(controlled_throttle * MAX_PPRZ); } @@ -279,7 +295,7 @@ void v_ctl_climb_loop ( void ) { #endif #ifndef V_CTL_THROTTLE_SLEW -#define V_CTL_THROTTLE_SLEW 1. +#define V_CTL_THROTTLE_SLEW (1.) #endif /** \brief Computes slewed throttle from throttle setpoint called at 20Hz diff --git a/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.h b/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.h index 4a6261d35b..d020a8cbb2 100644 --- a/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.h +++ b/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.h @@ -39,9 +39,7 @@ extern float v_ctl_auto_pitch_dgain; extern uint8_t v_ctl_speed_mode; -#ifdef PITCH_TRIM extern float v_ctl_pitch_loiter_trim; extern float v_ctl_pitch_dash_trim; -#endif #endif /* FW_V_CTL_N_H */ diff --git a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c index 4b138cd8b0..29b3489cc5 100644 --- a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c +++ b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c @@ -60,11 +60,11 @@ float h_ctl_ref_roll_accel; float h_ctl_ref_pitch_angle; float h_ctl_ref_pitch_rate; float h_ctl_ref_pitch_accel; -#define H_CTL_REF_W 2.5 +#define H_CTL_REF_W 5. #define H_CTL_REF_XI 0.85 -#define H_CTL_REF_MAX_P RadOfDeg(100.) +#define H_CTL_REF_MAX_P RadOfDeg(150.) #define H_CTL_REF_MAX_P_DOT RadOfDeg(500.) -#define H_CTL_REF_MAX_Q RadOfDeg(100.) +#define H_CTL_REF_MAX_Q RadOfDeg(150.) #define H_CTL_REF_MAX_Q_DOT RadOfDeg(500.) /* inner roll loop parameters */ @@ -96,21 +96,55 @@ float h_ctl_aileron_of_throttle; float h_ctl_elevator_of_roll; float h_ctl_pitch_of_roll; // Should be used instead of elevator_of_roll +bool_t use_airspeed_ratio; +float airspeed_ratio2; +#define AIRSPEED_RATIO_MIN 0.5 +#define AIRSPEED_RATIO_MAX 2. + +float v_ctl_pitch_loiter_trim; +float v_ctl_pitch_dash_trim; inline static void h_ctl_roll_loop( void ); inline static void h_ctl_pitch_loop( void ); +// Some default course gains +// H_CTL_COURSE_PGAIN needs to be define in airframe #ifndef H_CTL_COURSE_PRE_BANK_CORRECTION #define H_CTL_COURSE_PRE_BANK_CORRECTION 1. #endif - #ifndef H_CTL_COURSE_DGAIN #define H_CTL_COURSE_DGAIN 0. #endif +// Some default roll gains +// H_CTL_ROLL_ATTITUDE_GAIN needs to be define in airframe #ifndef H_CTL_ROLL_RATE_GAIN #define H_CTL_ROLL_RATE_GAIN 0. #endif +#ifndef H_CTL_ROLL_IGAIN +#define H_CTL_ROLL_IGAIN 0. +#endif +#ifndef H_CTL_ROLL_KFFA +#define H_CTL_ROLL_KFFA 0. +#endif +#ifndef H_CTL_ROLL_KFFD +#define H_CTL_ROLL_KFFD 0. +#endif + +// Some default pitch gains +// H_CTL_PITCH_PGAIN needs to be define in airframe +#ifndef H_CTL_PITCH_DGAIN +#define H_CTL_PITCH_DGAIN 0. +#endif +#ifndef H_CTL_PITCH_IGAIN +#define H_CTL_PITCH_IGAIN 0. +#endif +#ifndef H_CTL_PITCH_KFFA +#define H_CTL_PITCH_KFFA 0. +#endif +#ifndef H_CTL_PITCH_KFFD +#define H_CTL_PITCH_KFFD 0. +#endif void h_ctl_init( void ) { h_ctl_course_setpoint = 0.; @@ -148,6 +182,17 @@ void h_ctl_init( void ) { h_ctl_pitch_of_roll = H_CTL_PITCH_OF_ROLL; #endif + use_airspeed_ratio = FALSE; + airspeed_ratio2 = 1.; + +#ifdef USE_PITCH_TRIM + v_ctl_pitch_loiter_trim = V_CTL_PITCH_LOITER_TRIM; + v_ctl_pitch_dash_trim = V_CTL_PITCH_DASH_TRIM; +#else + v_ctl_pitch_loiter_trim = 0.; + v_ctl_pitch_dash_trim = 0.; +#endif + } /** @@ -175,24 +220,27 @@ void h_ctl_course_loop ( void ) { BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint); } -static float airspeed_ratio2; - +#ifdef USE_AIRSPEED static inline void compute_airspeed_ratio( void ) { - float throttle_diff = v_ctl_throttle_setpoint / (float)MAX_PPRZ - v_ctl_auto_throttle_nominal_cruise_throttle; - float airspeed = NOMINAL_AIRSPEED; /* Estimated from the throttle */ - if (throttle_diff > 0) - airspeed += throttle_diff / (V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE - v_ctl_auto_throttle_nominal_cruise_throttle) * (MAXIMUM_AIRSPEED - NOMINAL_AIRSPEED); - else - airspeed += throttle_diff / (v_ctl_auto_throttle_nominal_cruise_throttle - V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE) * (NOMINAL_AIRSPEED - MINIMUM_AIRSPEED); - + if (use_airspeed_ratio) { + // low pass airspeed + static float airspeed = 0.; + airspeed = ( 15*airspeed + estimator_airspeed ) / 16; + // compute ratio float airspeed_ratio = airspeed / NOMINAL_AIRSPEED; - Bound(airspeed_ratio, 0.5, 2.); + Bound(airspeed_ratio, AIRSPEED_RATIO_MIN, AIRSPEED_RATIO_MAX); airspeed_ratio2 = airspeed_ratio*airspeed_ratio; + } else { + airspeed_ratio2 = 1.; + } } +#endif void h_ctl_attitude_loop ( void ) { if (!h_ctl_disabled) { - // compute_airspeed_ratio(); +#ifdef USE_AIRSPEED + compute_airspeed_ratio(); +#endif h_ctl_roll_loop(); h_ctl_pitch_loop(); } @@ -206,9 +254,7 @@ inline static void h_ctl_roll_loop( void ) { static float cmd_fb = 0.; - if (pprz_mode == PPRZ_MODE_MANUAL) - h_ctl_roll_sum_err = 0; - +#ifdef USE_ANGLE_REF // Update reference setpoints for roll h_ctl_ref_roll_angle += h_ctl_ref_roll_rate * H_CTL_REF_DT; h_ctl_ref_roll_rate += h_ctl_ref_roll_accel * H_CTL_REF_DT; @@ -223,6 +269,11 @@ inline static void h_ctl_roll_loop( void ) { h_ctl_ref_roll_rate = - H_CTL_REF_MAX_P; if (h_ctl_ref_roll_accel < 0.) h_ctl_ref_roll_accel = 0.; } +#else + h_ctl_ref_roll_angle = h_ctl_roll_setpoint; + h_ctl_ref_roll_rate = 0.; + h_ctl_ref_roll_accel = 0.; +#endif #ifdef USE_KFF_UPDATE // update Kff gains @@ -242,26 +293,35 @@ inline static void h_ctl_roll_loop( void ) { estimator_p = (err - last_err)/(1/60.); last_err = err; #endif - float d_err = (estimator_p - h_ctl_ref_roll_rate) / H_CTL_REF_DT; + float d_err = estimator_p - h_ctl_ref_roll_rate; - h_ctl_roll_sum_err += err * H_CTL_REF_DT; - BoundAbs(h_ctl_roll_sum_err, H_CTL_ROLL_SUM_ERR_MAX); + if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) { + h_ctl_roll_sum_err = 0.; + } + else { + if (h_ctl_roll_igain < 0.) { + h_ctl_roll_sum_err += err * H_CTL_REF_DT; + BoundAbs(h_ctl_roll_sum_err, (- H_CTL_ROLL_SUM_ERR_MAX / h_ctl_roll_igain)); + } else { + h_ctl_roll_sum_err = 0.; + } + } - cmd_fb = h_ctl_roll_attitude_gain * err;// + h_ctl_roll_rate_gain * derr; + cmd_fb = h_ctl_roll_attitude_gain * err;// + h_ctl_roll_rate_gain * d_err; float cmd = h_ctl_roll_Kffa * h_ctl_ref_roll_accel + h_ctl_roll_Kffd * h_ctl_ref_roll_rate - cmd_fb - h_ctl_roll_rate_gain * d_err - h_ctl_roll_igain * h_ctl_roll_sum_err + v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle; - //float cmd = h_ctl_roll_Kffp * h_ctl_ref_roll_accel - // + h_ctl_roll_Kffd * h_ctl_ref_roll_rate - // - h_ctl_roll_attitude_gain * err - // - h_ctl_roll_rate_gain * derr - // - h_ctl_roll_igain * h_ctl_roll_sum_err - // + v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle; - - //x cmd /= airspeed_ratio2; +// float cmd = h_ctl_roll_Kffa * h_ctl_ref_roll_accel +// + h_ctl_roll_Kffd * h_ctl_ref_roll_rate +// - h_ctl_roll_attitude_gain * err +// - h_ctl_roll_rate_gain * d_err +// - h_ctl_roll_igain * h_ctl_roll_sum_err +// + v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle; +// + cmd /= airspeed_ratio2; // Set aileron commands h_ctl_aileron_setpoint = TRIM_PPRZ(cmd); @@ -274,13 +334,17 @@ float v_ctl_auto_throttle_loiter_trim = V_CTL_AUTO_THROTTLE_LOITER_TRIM; float v_ctl_auto_throttle_dash_trim = V_CTL_AUTO_THROTTLE_DASH_TRIM; #endif -#ifdef PITCH_TRIM -float v_ctl_pitch_loiter_trim = V_CTL_PITCH_LOITER_TRIM; -float v_ctl_pitch_dash_trim = V_CTL_PITCH_DASH_TRIM; - +#ifdef USE_PITCH_TRIM inline static void loiter(void) { float pitch_trim; +#ifdef USE_AIRSPEED + if (estimator_airspeed > NOMINAL_AIRSPEED) { + pitch_trim = v_ctl_pitch_dash_trim * (airspeed_ratio2-1) / ((AIRSPEED_RATIO_MAX * AIRSPEED_RATIO_MAX) - 1); + } else { + pitch_trim = v_ctl_pitch_loiter_trim * (airspeed_ratio2-1) / ((AIRSPEED_RATIO_MIN * AIRSPEED_RATIO_MIN) - 1); + } +#else float throttle_diff = v_ctl_throttle_setpoint / (float)MAX_PPRZ - v_ctl_auto_throttle_nominal_cruise_throttle; if (throttle_diff > 0) { float max_diff = Max(V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE - v_ctl_auto_throttle_nominal_cruise_throttle, 0.1); @@ -289,6 +353,7 @@ inline static void loiter(void) { float max_diff = Max(v_ctl_auto_throttle_nominal_cruise_throttle - V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE, 0.1); pitch_trim = -throttle_diff / max_diff * v_ctl_pitch_loiter_trim; } +#endif h_ctl_pitch_loop_setpoint += pitch_trim; } @@ -301,18 +366,16 @@ inline static void h_ctl_pitch_loop( void ) { if (h_ctl_pitch_of_roll <0.) h_ctl_pitch_of_roll = 0.; - if (pprz_mode == PPRZ_MODE_MANUAL) - h_ctl_pitch_sum_err = 0; - h_ctl_pitch_loop_setpoint = h_ctl_pitch_setpoint + h_ctl_pitch_of_roll * fabs(estimator_phi); -#ifdef PITCH_TRIM +#ifdef USE_PITCH_TRIM loiter(); #endif +#ifdef USE_ANGLE_REF // Update reference setpoints for pitch - h_ctl_ref_pitch_accel = H_CTL_REF_W*H_CTL_REF_W * (h_ctl_pitch_loop_setpoint - h_ctl_ref_pitch_angle) - 2*H_CTL_REF_XI*H_CTL_REF_W * h_ctl_ref_pitch_rate; - h_ctl_ref_pitch_rate += h_ctl_ref_pitch_accel * H_CTL_REF_DT; h_ctl_ref_pitch_angle += h_ctl_ref_pitch_rate * H_CTL_REF_DT; + h_ctl_ref_pitch_rate += h_ctl_ref_pitch_accel * H_CTL_REF_DT; + h_ctl_ref_pitch_accel = H_CTL_REF_W*H_CTL_REF_W * (h_ctl_pitch_loop_setpoint - h_ctl_ref_pitch_angle) - 2*H_CTL_REF_XI*H_CTL_REF_W * h_ctl_ref_pitch_rate; // Saturation on references BoundAbs(h_ctl_ref_pitch_accel, H_CTL_REF_MAX_Q_DOT); if (h_ctl_ref_pitch_rate > H_CTL_REF_MAX_Q) { @@ -323,14 +386,28 @@ inline static void h_ctl_pitch_loop( void ) { h_ctl_ref_pitch_rate = - H_CTL_REF_MAX_Q; if (h_ctl_ref_pitch_accel < 0.) h_ctl_ref_pitch_accel = 0.; } +#else + h_ctl_ref_pitch_angle = h_ctl_pitch_loop_setpoint; + h_ctl_ref_pitch_rate = 0.; + h_ctl_ref_pitch_accel = 0.; +#endif // Compute errors float err = estimator_theta - h_ctl_ref_pitch_angle; float d_err = (err - last_err)/H_CTL_REF_DT - h_ctl_ref_pitch_rate; last_err = err; - h_ctl_pitch_sum_err += err * H_CTL_REF_DT; - BoundAbs(h_ctl_pitch_sum_err, H_CTL_ROLL_SUM_ERR_MAX); + if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) { + h_ctl_pitch_sum_err = 0.; + } + else { + if (h_ctl_pitch_igain < 0.) { + h_ctl_pitch_sum_err += err * H_CTL_REF_DT; + BoundAbs(h_ctl_pitch_sum_err, (- H_CTL_PITCH_SUM_ERR_MAX / h_ctl_pitch_igain)); + } else { + h_ctl_pitch_sum_err = 0.; + } + } float cmd = h_ctl_pitch_Kffa * h_ctl_ref_pitch_accel + h_ctl_pitch_Kffd * h_ctl_ref_pitch_rate @@ -338,7 +415,8 @@ inline static void h_ctl_pitch_loop( void ) { + h_ctl_pitch_dgain * d_err + h_ctl_pitch_igain * h_ctl_pitch_sum_err; - // cmd /= airspeed_ratio2; + cmd /= airspeed_ratio2; h_ctl_elevator_setpoint = TRIM_PPRZ(cmd); } + diff --git a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.h b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.h index eb084c9811..4a7c082f22 100644 --- a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.h +++ b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.h @@ -46,16 +46,16 @@ extern float h_ctl_pitch_Kffa; extern float h_ctl_pitch_Kffd; extern float h_ctl_pitch_of_roll; -#define H_CTL_ROLL_SUM_ERR_MAX 100. -#define H_CTL_PITCH_SUM_ERR_MAX 100. +#define H_CTL_ROLL_SUM_ERR_MAX (MAX_PPRZ/2.) +#define H_CTL_PITCH_SUM_ERR_MAX (MAX_PPRZ/2.) #define fw_h_ctl_a_SetRollIGain(_gain) { \ - h_ctl_roll_sum_err = 0; \ + h_ctl_roll_sum_err = 0.; \ h_ctl_roll_igain = _gain; \ } #define fw_h_ctl_a_SetPitchIGain(_gain) { \ - h_ctl_pitch_sum_err = 0; \ + h_ctl_pitch_sum_err = 0.; \ h_ctl_pitch_igain = _gain; \ } @@ -69,4 +69,6 @@ extern float h_ctl_ref_pitch_angle; extern float h_ctl_ref_pitch_rate; extern float h_ctl_ref_pitch_accel; +extern bool_t use_airspeed_ratio; + #endif /* FW_H_CTL_A_H */ diff --git a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/ADC.pde b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/ADC.pde new file mode 100644 index 0000000000..3d4728d168 --- /dev/null +++ b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/ADC.pde @@ -0,0 +1,91 @@ +// We are using an oversampling and averaging method to increase the ADC resolution +// The theorical ADC resolution is now 11.7 bits. Now we store the ADC readings in float format +void Read_adc_raw(void) +{ + int i; + uint16_t temp1; + uint8_t temp2; + + // ADC readings... + for (i=0;i<6;i++) + { + do{ + temp1= analog_buffer[sensors[i]]; // sensors[] maps sensors to correct order + temp2= analog_count[sensors[i]]; + } while(temp1 != analog_buffer[sensors[i]]); // Check if there was an ADC interrupt during readings... + + if (temp2>0) AN[i] = float(temp1)/float(temp2); // Check for divide by zero + + } + // Initialization for the next readings... + for (int i=0;i<8;i++){ + do{ + analog_buffer[i]=0; + analog_count[i]=0; + } while(analog_buffer[i]!=0); // Check if there was an ADC interrupt during initialization... + } +} + +float read_adc(int select) +{ + float temp; + if (SENSOR_SIGN[select]<0){ + temp = (AN_OFFSET[select]-AN[select]); + if (abs(temp)>900) { +#if PRINT_DEBUG != 0 + Serial.print("!!!ADC:1,VAL:"); + Serial.print (temp); + Serial.println("***"); +#endif +#if PERFORMANCE_REPORTING == 1 + adc_constraints++; +#endif + } + return constrain(temp,-900,900); //Throw out nonsensical values + } else { + temp = (AN[select]-AN_OFFSET[select]); + if (abs(temp)>900) { +#if PRINT_DEBUG != 0 + Serial.print("!!!ADC:2,VAL:"); + Serial.print (temp); + Serial.println("***"); +#endif +#if PERFORMANCE_REPORTING == 1 + adc_constraints++; +#endif + } + return constrain(temp,-900,900); + } +} + +//Activating the ADC interrupts. +void Analog_Init(void) +{ + ADCSRA|=(1<=8) MuxSel=0; + ADMUX = (analog_reference << 6) | MuxSel; + // start the conversion + ADCSRA|= (1< 0.64f) { + renorm= .5 * (3-renorm); //eq.21 + } else if (renorm < 100.0f && renorm > 0.01f) { + renorm= 1. / sqrt(renorm); +#if PERFORMANCE_REPORTING == 1 + renorm_sqrt_count++; +#endif +#if PRINT_DEBUG != 0 + Serial.print("???SQT:1,RNM:"); + Serial.print (renorm); + Serial.print (",ERR:"); + Serial.print (error); + Serial.println("***"); +#endif + } else { + problem = TRUE; +#if PERFORMANCE_REPORTING == 1 + renorm_blowup_count++; +#endif +#if PRINT_DEBUG != 0 + Serial.print("???PRB:1,RNM:"); + Serial.print (renorm); + Serial.print (",ERR:"); + Serial.print (error); + Serial.println("***"); +#endif + } + Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm); + + renorm= Vector_Dot_Product(&temporary[1][0],&temporary[1][0]); + if (renorm < 1.5625f && renorm > 0.64f) { + renorm= .5 * (3-renorm); //eq.21 + } else if (renorm < 100.0f && renorm > 0.01f) { + renorm= 1. / sqrt(renorm); +#if PERFORMANCE_REPORTING == 1 + renorm_sqrt_count++; +#endif +#if PRINT_DEBUG != 0 + Serial.print("???SQT:2,RNM:"); + Serial.print (renorm); + Serial.print (",ERR:"); + Serial.print (error); + Serial.println("***"); +#endif + } else { + problem = TRUE; +#if PERFORMANCE_REPORTING == 1 + renorm_blowup_count++; +#endif +#if PRINT_DEBUG != 0 + Serial.print("???PRB:2,RNM:"); + Serial.print (renorm); + Serial.print (",ERR:"); + Serial.print (error); + Serial.println("***"); +#endif + } + Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm); + + renorm= Vector_Dot_Product(&temporary[2][0],&temporary[2][0]); + if (renorm < 1.5625f && renorm > 0.64f) { + renorm= .5 * (3-renorm); //eq.21 + } else if (renorm < 100.0f && renorm > 0.01f) { + renorm= 1. / sqrt(renorm); +#if PERFORMANCE_REPORTING == 1 + renorm_sqrt_count++; +#endif +#if PRINT_DEBUG != 0 + Serial.print("???SQT:3,RNM:"); + Serial.print (renorm); + Serial.print (",ERR:"); + Serial.print (error); + Serial.println("***"); +#endif + } else { + problem = TRUE; +#if PERFORMANCE_REPORTING == 1 + renorm_blowup_count++; +#endif +#if PRINT_DEBUG != 0 + Serial.print("???PRB:3,RNM:"); + Serial.print (renorm); + Serial.println("***"); +#endif + } + Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm); + + if (problem) { // Our solution is blowing up and we will force back to initial condition. Hope we are not upside down! + DCM_Matrix[0][0]= 1.0f; + DCM_Matrix[0][1]= 0.0f; + DCM_Matrix[0][2]= 0.0f; + DCM_Matrix[1][0]= 0.0f; + DCM_Matrix[1][1]= 1.0f; + DCM_Matrix[1][2]= 0.0f; + DCM_Matrix[2][0]= 0.0f; + DCM_Matrix[2][1]= 0.0f; + DCM_Matrix[2][2]= 1.0f; + problem = FALSE; + } +} + +/**************************************************/ +void Drift_correction(void) +{ + //Compensation the Roll, Pitch and Yaw drift. + static float Scaled_Omega_P[3]; + static float Scaled_Omega_I[3]; + float Accel_magnitude; + float Accel_weight; + float Integrator_magnitude; + float tempfloat; + + //*****Roll and Pitch*************** + + // Calculate the magnitude of the accelerometer vector + Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]); + Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity. + // Dynamic weighting of accelerometer info (reliability filter) + // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0) + Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude),0,1); // + +#if PERFORMANCE_REPORTING == 1 + tempfloat = ((Accel_weight - 0.5) * 256.0f); //amount added was determined to give imu_health a time constant about twice the time constant of the roll/pitch drift correction + imu_health += tempfloat; + imu_health = constrain(imu_health,129,65405); +#endif + + Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of reference + Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight); + + Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight); + Vector_Add(Omega_I,Omega_I,Scaled_Omega_I); + + //*****YAW*************** + + // Use GPS Ground course to correct yaw gyro drift + if(ground_speed>=SPEEDFILT) + { + COGX = cos(ToRad(ground_course)); + COGY = sin(ToRad(ground_course)); + errorCourse=(DCM_Matrix[0][0]*COGY) - (DCM_Matrix[1][0]*COGX); //Calculating YAW error + Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position. + + Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW); + Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional. + + Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW); + Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I + } + // Here we will place a limit on the integrator so that the integrator cannot ever exceed half the saturation limit of the gyros + Integrator_magnitude = sqrt(Vector_Dot_Product(Omega_I,Omega_I)); + if (Integrator_magnitude > ToRad(300)) { + Vector_Scale(Omega_I,Omega_I,0.5f*ToRad(300)/Integrator_magnitude); +#if PRINT_DEBUG != 0 + Serial.print("!!!INT:1,MAG:"); + Serial.print (ToDeg(Integrator_magnitude)); + + Serial.println("***"); +#endif + } + +} +/**************************************************/ +void Accel_adjust(void) +{ + Accel_Vector[1] += Accel_Scale(speed_3d*Omega[2]); // Centrifugal force on Acc_y = GPS_speed*GyroZ + Accel_Vector[2] -= Accel_Scale(speed_3d*Omega[1]); // Centrifugal force on Acc_z = GPS_speed*GyroY +} +/**************************************************/ + +void Matrix_update(void) +{ + Gyro_Vector[0]=Gyro_Scaled_X(read_adc(0)); //gyro x roll + Gyro_Vector[1]=Gyro_Scaled_Y(read_adc(1)); //gyro y pitch + Gyro_Vector[2]=Gyro_Scaled_Z(read_adc(2)); //gyro Z yaw + + Accel_Vector[0]=read_adc(3); // acc x + Accel_Vector[1]=read_adc(4); // acc y + Accel_Vector[2]=read_adc(5); // acc z + + Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]); //adding proportional term + Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term + + Accel_adjust(); //Remove centrifugal acceleration. + +#if OUTPUTMODE==1 + Update_Matrix[0][0]=0; + Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z + Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y + Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z + Update_Matrix[1][1]=0; + Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x + Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y + Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x + Update_Matrix[2][2]=0; +#else // Uncorrected data (no drift correction) + Update_Matrix[0][0]=0; + Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z + Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y + Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z + Update_Matrix[1][1]=0; + Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0]; + Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1]; + Update_Matrix[2][1]=G_Dt*Gyro_Vector[0]; + Update_Matrix[2][2]=0; +#endif + + Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c + + for(int x=0; x<3; x++) //Matrix Addition (update) + { + for(int y=0; y<3; y++) + { + DCM_Matrix[x][y]+=Temporary_Matrix[x][y]; + } + } +} + +void Euler_angles(void) +{ +#if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes) + roll = atan2(Accel_Vector[1],Accel_Vector[2]); // atan2(acc_y,acc_z) + pitch = -asin((Accel_Vector[0])/(double)GRAVITY); // asin(acc_x) + yaw = 0; +#else + pitch = -asin(DCM_Matrix[2][0]); + roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]); + yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]); +#endif +} + diff --git a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/GPS_pprz.pde b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/GPS_pprz.pde new file mode 100644 index 0000000000..6a2ddf704d --- /dev/null +++ b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/GPS_pprz.pde @@ -0,0 +1,64 @@ +/**************************************************************** + * Parse GPS data from a Paparazzi autopilot + ****************************************************************/ + // Code from Jordi, modified by Jose, modified by Gautier + + +void init_gps(void) +{ + //Serial.begin(38400); + pinMode(2,OUTPUT); //Serial Mux + digitalWrite(2,HIGH); //Serial Mux +} + +/**************************************************************** + * + ****************************************************************/ +void parse_pprz_gps() { + +#if PERFORMANCE_REPORTING == 1 + gps_pos_fix_count++; +#endif + + speed_3d = (float)join_4_bytes(&Paparazzi_GPS_buffer[0])/100.0; // m/s 0,1,2,3 + ground_speed = (float)join_4_bytes(&Paparazzi_GPS_buffer[4])/100.0; // Ground speed 2D 4,5,6,7 + ground_course = (float)join_4_bytes(&Paparazzi_GPS_buffer[8])/100000.0; // Heading 2D 8,9,10,11 + stGpsFix = Paparazzi_GPS_buffer[12]; + stFlags = Paparazzi_GPS_buffer[13]; + + if((stGpsFix >= 0x03) && (stFlags&0x01)) { + gpsFix = 0; //valid position + digitalWrite(6,HIGH); //Turn LED when gps is fixed. + GPS_timer = DIYmillis(); //Restarting timer... + } + else { + gpsFix = 1; //invalid position + digitalWrite(6,LOW); + } + + if (ground_speed > SPEEDFILT && gpsFix==0) gc_offset = ground_course - ToDeg(yaw); + +} + + +/**************************************************************** + * Join 4 bytes into a long + ****************************************************************/ +int32_t join_4_bytes(byte Buffer[]) +{ + longUnion.byte[0] = *Buffer; + longUnion.byte[1] = *(Buffer+1); + longUnion.byte[2] = *(Buffer+2); + longUnion.byte[3] = *(Buffer+3); + return(longUnion.dword); +} + +/**************************************************************** + * +void checksum(byte ubx_data) +{ + ck_a+=ubx_data; + ck_b+=ck_a; +} + + ****************************************************************/ diff --git a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/Output.pde b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/Output.pde new file mode 100644 index 0000000000..49170c774c --- /dev/null +++ b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/Output.pde @@ -0,0 +1,200 @@ + +//*****I2C Output************************************************************ + +void requestEvent(){ +#if PRINT_DEBUG != 0 + Serial.println("Sending IMU Data"); +#endif + + // Message Array : Roll; Pitch; YaW; GyroX; GyroY; GyroZ; ACCX; ACCY; ACCZ + // Float Number is multipited with 10000 and converted to an Integer, for sending via I2C. + // Resolution for angles: 12, rates: 12, accel:10 (from pprza BFP math lib) + I2C_Message_ar[0] = int(roll*(1<<12)); + I2C_Message_ar[1] = int(pitch*(1<<12)); + I2C_Message_ar[2] = int(yaw*(1<<12)); + I2C_Message_ar[3] = int(Gyro_Vector[0]*(1<<12)); + I2C_Message_ar[4] = int(Gyro_Vector[1]*(1<<12)); + I2C_Message_ar[5] = int(Gyro_Vector[2]*(1<<12)); + I2C_Message_ar[6] = int((9.81*Accel_Vector[0]/GRAVITY)*(1<<10)); + I2C_Message_ar[7] = int((9.81*Accel_Vector[1]/GRAVITY)*(1<<10)); + I2C_Message_ar[8] = int((9.81*Accel_Vector[2]/GRAVITY)*(1<<10)); + + byte* pointer; + pointer = (byte*) &I2C_Message_ar; + Wire.send(pointer, 18); + +} + +//******** GPS Data from Paparazzi AP ****************************************** + +void receiveEvent(int howMany){ +#if PRINT_DEBUG != 0 + Serial.print("Receiving GPS Bytes : "); + Serial.println(howMany); +#endif + + for(int i=0; i < howMany; i++){ + Paparazzi_GPS_buffer[i]=Wire.receive(); + } + + parse_pprz_gps(); // Parse new GPS packet... + GPS_timer=DIYmillis(); //Restarting timer... + + gpsDataReady=1; + +} + + +//************************************************************************************************************* + + +void printdata(void){ + +#if PRINT_I2C_MSG == 1 + Serial.print("Time "); + Serial.print(millis()); + Serial.print("; Roll "); + Serial.print(I2C_Message_ar[0]); + Serial.print("; Pitch "); + Serial.print(I2C_Message_ar[1]); + Serial.print("; YaW "); + Serial.print(I2C_Message_ar[2]); + Serial.print("; GyroX "); + Serial.print(I2C_Message_ar[3]); + Serial.print("; GyroY "); + Serial.print(I2C_Message_ar[4]); + Serial.print("; GyroZ "); + Serial.print(I2C_Message_ar[5]); + Serial.print("; ACCX "); + Serial.print(I2C_Message_ar[6]); + Serial.print("; ACCY "); + Serial.print(I2C_Message_ar[7]); + Serial.print("; ACCZ "); + Serial.println(I2C_Message_ar[8]); +#endif + + //Serial.print("!!!VER:"); + //Serial.print(SOFTWARE_VER); //output the software version + //Serial.print(","); + +#if PRINT_ANALOGS == 1 + Serial.print("AN0:"); + Serial.print(read_adc(0)); //Reversing the sign. + Serial.print(",AN1:"); + Serial.print(read_adc(1)); + Serial.print(",AN2:"); + Serial.print(read_adc(2)); + Serial.print(",AN3:"); + Serial.print(read_adc(3)); + Serial.print (",AN4:"); + Serial.print(read_adc(4)); + Serial.print (",AN5:"); + Serial.print(read_adc(5)); + Serial.print (","); +#endif + +#if PRINT_DCM == 1 + Serial.print ("EX0:"); + Serial.print(convert_to_dec(DCM_Matrix[0][0])); + Serial.print (",EX1:"); + Serial.print(convert_to_dec(DCM_Matrix[0][1])); + Serial.print (",EX2:"); + Serial.print(convert_to_dec(DCM_Matrix[0][2])); + Serial.print (",EX3:"); + Serial.print(convert_to_dec(DCM_Matrix[1][0])); + Serial.print (",EX4:"); + Serial.print(convert_to_dec(DCM_Matrix[1][1])); + Serial.print (",EX5:"); + Serial.print(convert_to_dec(DCM_Matrix[1][2])); + Serial.print (",EX6:"); + Serial.print(convert_to_dec(DCM_Matrix[2][0])); + Serial.print (",EX7:"); + Serial.print(convert_to_dec(DCM_Matrix[2][1])); + Serial.print (",EX8:"); + Serial.print(convert_to_dec(DCM_Matrix[2][2])); + Serial.print (","); +#endif + +#if PRINT_EULER == 1 + Serial.print("RLL:"); + Serial.print(ToDeg(roll)); + Serial.print(",PCH:"); + Serial.print(ToDeg(pitch)); + Serial.print(",YAW:"); + Serial.print(ToDeg(yaw)); + Serial.print(",IMUH:"); + Serial.print((imu_health>>8)&0xff); + Serial.print (","); +#endif + +#if PRINT_GPS == 1 + if(gpsFixnew==1) { + gpsFixnew=0; + Serial.print("COG:"); + Serial.print((ground_course)); + Serial.print(",SOG:"); + Serial.print(ground_speed); + Serial.print(",FIX:"); + Serial.print((int)gpsFix); + Serial.print (","); +#if PERFORMANCE_REPORTING == 1 + gps_messages_sent++; +#endif + } +#endif + +} + +void printPerfData(long time) +{ + Serial.print("PPP"); + Serial.print("pTm:"); + Serial.print(time,DEC); + Serial.print(",mLc:"); + Serial.print(mainLoop_count,DEC); + Serial.print(",DtM:"); + Serial.print(G_Dt_max,DEC); + Serial.print(",gsc:"); + Serial.print(gyro_sat_count,DEC); + Serial.print(",adc:"); + Serial.print(adc_constraints,DEC); + Serial.print(",rsc:"); + Serial.print(renorm_sqrt_count,DEC); + Serial.print(",rbc:"); + Serial.print(renorm_blowup_count,DEC); + Serial.print(",gpe:"); + Serial.print(gps_payload_error_count,DEC); + Serial.print(",gce:"); + Serial.print(gps_checksum_error_count,DEC); + Serial.print(",gpf:"); + Serial.print(gps_pos_fix_count,DEC); + Serial.print(",gnf:"); + Serial.print(gps_nav_fix_count,DEC); + Serial.print(",gms:"); + Serial.print(gps_messages_sent,DEC); + Serial.print(",imu:"); + Serial.print((imu_health>>8),DEC); + Serial.print(",***"); + + // Reset counters + mainLoop_count = 0; + G_Dt_max = 0; + gyro_sat_count = 0; + adc_constraints = 0; + renorm_sqrt_count = 0; + renorm_blowup_count = 0; + gps_payload_error_count = 0; + gps_checksum_error_count = 0; + gps_pos_fix_count = 0; + gps_nav_fix_count = 0; + gps_messages_sent = 0; + + +} + + +long convert_to_dec(float x) +{ + return x*10000000; +} + diff --git a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/Vector.pde b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/Vector.pde new file mode 100644 index 0000000000..3c78b874db --- /dev/null +++ b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/Vector.pde @@ -0,0 +1,40 @@ +//Computes the dot product of two vectors +float Vector_Dot_Product(float vector1[3],float vector2[3]) +{ + float op=0; + + for(int c=0; c<3; c++) + { + op+=vector1[c]*vector2[c]; + } + + return op; +} + +//Computes the cross product of two vectors +void Vector_Cross_Product(float vectorOut[3], float v1[3],float v2[3]) +{ + vectorOut[0]= (v1[1]*v2[2]) - (v1[2]*v2[1]); + vectorOut[1]= (v1[2]*v2[0]) - (v1[0]*v2[2]); + vectorOut[2]= (v1[0]*v2[1]) - (v1[1]*v2[0]); +} + +//Multiply the vector by a scalar. +void Vector_Scale(float vectorOut[3],float vectorIn[3], float scale2) +{ + for(int c=0; c<3; c++) + { + vectorOut[c]=vectorIn[c]*scale2; + } +} + +void Vector_Add(float vectorOut[3],float vectorIn1[3], float vectorIn2[3]) +{ + for(int c=0; c<3; c++) + { + vectorOut[c]=vectorIn1[c]+vectorIn2[c]; + } +} + + + diff --git a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/arduimu_basic.pde b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/arduimu_basic.pde new file mode 100644 index 0000000000..df41fd5778 --- /dev/null +++ b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/arduimu_basic.pde @@ -0,0 +1,548 @@ +// Released under Creative Commons License +// Code by Jordi Munoz and William Premerlani, Supported by Chris Anderson (Wired) and Nathan Sindle (SparkFun). +// Version 1.0 for flat board updated by Doug Weibel and Jose Julio +// Version 1.7 includes support for SCP1000 absolute pressure sensor +// Version modified by Gautier Hattenberger for use with Paparazzi autopilot only (no barometer, no compass) + +// Axis definition: X axis pointing forward, Y axis pointing to the right and Z axis pointing down. +// Positive pitch : nose up +// Positive roll : right wing down +// Positive yaw : clockwise + +#include +#include + +//********************************************************************** +// This section contains USER PARAMETERS !!! +// +//********************************************************************** + +// *** NOTE! Hardware version - Can be used for v1 (daughterboards) or v2 (flat) +#define BOARD_VERSION 2 // 1 For V1 and 2 for V2 + +// Enable Air Start uses Remove Before Fly flag - connection to pin 6 on ArduPilot +#define ENABLE_AIR_START 1 // 1 if using airstart/groundstart signaling, 0 if not +#define GROUNDSTART_PIN 8 // Pin number used for ground start signal (recommend 10 on v1 and 8 on v2 hardware) + +/*Min Speed Filter for Yaw drift Correction*/ +#define SPEEDFILT 2 // >1 use min speed filter for yaw drift cancellation, 0=do not use speed filter + +/*For debugging propurses*/ +#define PRINT_DEBUG 0 //Will print Debug messages + +//OUTPUTMODE=1 will print the corrected data, 0 will print uncorrected data of the gyros (with drift), 2 will print accelerometer only data +#define OUTPUTMODE 1 + +#define PRINT_I2C_MSG 0 //Will print the I2C output buffer +#define PRINT_DCM 0 //Will print the whole direction cosine matrix +#define PRINT_ANALOGS 0 //Will print the analog raw data +#define PRINT_EULER 0 //Will print the Euler angles Roll, Pitch and Yaw +#define PRINT_GPS 0 //Will print GPS data + + + + +//**********I2C Parameter ******************************************** +int I2C_Message_ar[9]; + +//******************************************************************** + + +// *** NOTE! To use ArduIMU with ArduPilot you must select binary output messages (change to 1 here) +#define PRINT_BINARY 0 //Will print binary message and suppress ASCII messages (above) + +// *** NOTE! Performance reporting is only supported for Ublox. Set to 0 for others +#define PERFORMANCE_REPORTING 1 //Will include performance reports in the binary output ~ 1/2 min + +//********************************************************************** +// End of user parameters +//********************************************************************** + +#define SOFTWARE_VER "1.7" + +// ADC : Voltage reference 3.3v / 10bits(1024 steps) => 3.22mV/ADC step +// ADXL335 Sensitivity(from datasheet) => 330mV/g, 3.22mV/ADC step => 330/3.22 = 102.48 +// Tested value : 101 +#define GRAVITY 101 //this equivalent to 1G in the raw data coming from the accelerometer +#define Accel_Scale(x) x*(GRAVITY/9.81)//Scaling the raw data of the accel to actual acceleration in meters for seconds square + +#define ToRad(x) (x*0.01745329252) // *pi/180 +#define ToDeg(x) (x*57.2957795131) // *180/pi + +// LPR530 & LY530 Sensitivity (from datasheet) => 3.33mV/º/s, 3.22mV/ADC step => 1.03 +// Tested values : 0.96,0.96,0.94 +#define Gyro_Gain_X 0.92 //X axis Gyro gain +#define Gyro_Gain_Y 0.92 //Y axis Gyro gain +#define Gyro_Gain_Z 0.94 //Z axis Gyro gain +#define Gyro_Scaled_X(x) x*ToRad(Gyro_Gain_X) //Return the scaled ADC raw data of the gyro in radians for second +#define Gyro_Scaled_Y(x) x*ToRad(Gyro_Gain_Y) //Return the scaled ADC raw data of the gyro in radians for second +#define Gyro_Scaled_Z(x) x*ToRad(Gyro_Gain_Z) //Return the scaled ADC raw data of the gyro in radians for second + +#define Kp_ROLLPITCH 0.015 +#define Ki_ROLLPITCH 0.000010 +#define Kp_YAW 1.2 +//#define Kp_YAW 2.5 //High yaw drift correction gain - use with caution! +#define Ki_YAW 0.00005 + +#define ADC_WARM_CYCLES 75 + +#define FALSE 0 +#define TRUE 1 + + +float G_Dt=0.02; // Integration time (DCM algorithm) + +long timeNow=0; // Hold the milliseond value for now +long timer=0; //general purpuse timer +long timer_old; +long timer24=0; //Second timer used to print values +boolean groundstartDone = false; // Used to not repeat ground start + +float AN[8]; //array that store the 6 ADC filtered data +float AN_OFFSET[8]; //Array that stores the Offset of the gyros + +float Accel_Vector[3]= {0,0,0}; //Store the acceleration in a vector +float Gyro_Vector[3]= {0,0,0};//Store the gyros rutn rate in a vector +float Omega_Vector[3]= {0,0,0}; //Corrected Gyro_Vector data +float Omega_P[3]= {0,0,0};//Omega Proportional correction +float Omega_I[3]= {0,0,0};//Omega Integrator +float Omega[3]= {0,0,0}; + +//Magnetometer variables +//int magnetom_x; +//int magnetom_y; +//int magnetom_z; +//float MAG_Heading; + +// Euler angles +float roll; +float pitch; +float yaw; + +float errorRollPitch[3]= {0,0,0}; +float errorYaw[3]= {0,0,0}; +float errorCourse=180; +float COGX=0; //Course overground X axis +float COGY=1; //Course overground Y axis + +unsigned int cycleCount=0; +byte gyro_sat=0; + +float DCM_Matrix[3][3] = { + { 1,0,0 }, + { 0,1,0 }, + { 0,0,1 } +}; + +float Update_Matrix[3][3] = { + { 0,1,2 }, + { 3,4,5 }, + { 6,7,8 } +}; //Gyros here + +float Temporary_Matrix[3][3]={ + { 0,0,0 }, + { 0,0,0 }, + { 0,0,0 } +}; + +//GPS + +//GPS stuff +union long_union { + int32_t dword; + uint8_t byte[4]; +} longUnion; + +union int_union { + int16_t word; + uint8_t byte[2]; +} intUnion; + +/*Flight GPS variables*/ +int gpsFix=1; //This variable store the status of the GPS +int gpsFixnew=0; //used to flag when new gps data received - used for binary output message flags +int gps_fix_count = 5; //used to count 5 good fixes at ground startup +float speed_3d=0; //Speed (3-D) +float ground_speed=0;// This is the velocity your "plane" is traveling in meters for second, 1Meters/Second= 3.6Km/H = 1.944 knots +float ground_course=90;//This is the runaway direction of you "plane" in degrees +float gc_offset = 0; // Force yaw output to ground course when fresh data available (only implemented for ublox&binary message) +unsigned long GPS_timer=0; + +//***********************GPS PAPARAZZI************************************************************************ +#define PPRZ_MAXPAYLOAD 32 +byte Paparazzi_GPS_buffer[PPRZ_MAXPAYLOAD]; +int gpsDataReady = 0; // sind neuen GPS daten vorhanden ?? +byte stGpsFix; +byte stFlags; +byte messageNr; +//************************************************************************************************************ + +//ADC variables +volatile uint8_t MuxSel=0; +volatile uint8_t analog_reference = DEFAULT; +volatile uint16_t analog_buffer[8]; +volatile uint8_t analog_count[8]; + + +#if BOARD_VERSION == 1 +uint8_t sensors[6] = {0,2,1,3,5,4}; // Use these two lines for Hardware v1 (w/ daughterboards) +int SENSOR_SIGN[]= {1,-1,1,-1,1,-1,-1,-1,-1}; //Sensor: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ +#endif + +#if BOARD_VERSION == 2 +uint8_t sensors[6] = {6,7,3,0,1,2}; // For Hardware v2 flat +int SENSOR_SIGN[] = {1,-1,-1,1,-1,1,-1,-1,-1}; +#endif + +// Performance Monitoring variables +// Data collected and reported for ~1/2 minute intervals +#if PERFORMANCE_REPORTING == 1 +int mainLoop_count = 0; //Main loop cycles since last report +int G_Dt_max = 0.0; //Max main loop cycle time in milliseconds +byte gyro_sat_count = 0; +byte adc_constraints = 0; +byte renorm_sqrt_count = 0; +byte renorm_blowup_count = 0; +byte gps_payload_error_count = 0; +byte gps_checksum_error_count = 0; +byte gps_pos_fix_count = 0; +byte gps_nav_fix_count = 0; +byte gps_messages_sent = 0; +long perf_mon_timer = 0; +#endif +unsigned int imu_health = 65012; + +//****************************************************************** +void setup() +{ + Serial.begin(38400); + pinMode(2,OUTPUT); //Serial Mux + digitalWrite(2,HIGH); //Serial Mux + pinMode(5,OUTPUT); //Red LED + pinMode(6,OUTPUT); // Blue LED + pinMode(7,OUTPUT); // Yellow LED + pinMode(GROUNDSTART_PIN,INPUT); // Remove Before Fly flag (pin 6 on ArduPilot) + digitalWrite(GROUNDSTART_PIN,HIGH); + + //************Define I2C Output Handler************************ + Wire.begin(17); // join i2c bus with address #2 + Wire.onRequest(requestEvent); + Wire.onReceive(receiveEvent); // register event --> Output + //************************************************************* + + Analog_Reference(EXTERNAL); //Using external analog reference + Analog_Init(); + + debug_print("Welcome..."); + +#if BOARD_VERSION == 1 + debug_print("You are using Hardware Version 1..."); +#endif + +#if BOARD_VERSION == 2 + debug_print("You are using Hardware Version 2..."); +#endif + + //Serial.print("ArduIMU: v"); + //Serial.println(SOFTWARE_VER); + debug_handler(0);//Printing version + + if(ENABLE_AIR_START) { + debug_handler(1); + startup_air(); + } else { + debug_handler(2); + startup_ground(); + } + + delay(250); + Read_adc_raw(); // ADC initialization + timer=DIYmillis(); + delay(20); + +} + +//*************************************************************************************** +void loop() //Main Loop +{ + timeNow = millis(); + + if((timeNow-timer)>=20) // Main loop runs at 50Hz + { + timer_old = timer; + timer = timeNow; + +#if PERFORMANCE_REPORTING == 1 + mainLoop_count++; + if (timer-timer_old > G_Dt_max) G_Dt_max = timer-timer_old; +#endif + + G_Dt = (timer-timer_old)/1000.0; // Real time of loop run. We use this on the DCM algorithm (gyro integration time) + if(G_Dt > 1) G_Dt = 0; //Something is wrong - keeps dt from blowing up, goes to zero to keep gyros from departing + + // *** DCM algorithm + + Read_adc_raw(); + + Matrix_update(); + + Normalize(); + + Drift_correction(); + + Euler_angles(); + +#if PRINT_BINARY == 1 + printdata(); //Send info via serial +#endif + + //Turn on the LED when you saturate any of the gyros. + if((abs(Gyro_Vector[0])>=ToRad(300))||(abs(Gyro_Vector[1])>=ToRad(300))||(abs(Gyro_Vector[2])>=ToRad(300))) + { + gyro_sat=1; +#if PERFORMANCE_REPORTING == 1 + gyro_sat_count++; +#endif + digitalWrite(5,HIGH); + } + + cycleCount++; + + // Do these things every 6th time through the main cycle + // This section gets called every 1000/(20*6) = 8 1/3 Hz + // doing it this way removes the need for another 'millis()' call + // and balances the processing load across main loop cycles. + switch (cycleCount) { + case(0): + if(DIYmillis() - GPS_timer > 2000) { + digitalWrite(6, LOW); //If we don't receive any byte in two seconds turn off gps fix LED... + debug_print("No GPS data"); + gpsFix = 1; + } + break; + + case(1): + //Here we will check if we are getting a signal to ground start + if (digitalRead(GROUNDSTART_PIN) == LOW && groundstartDone == false) startup_ground(); + break; + + case(2): + // GYRO Saturation indication + if(gyro_sat>=1) { + digitalWrite(5,HIGH); //Turn Red LED when gyro is saturated. + if(gyro_sat>=8) // keep the LED on for 8/10ths of a second + gyro_sat=0; + else + gyro_sat++; + } else { + digitalWrite(5,LOW); + } + break; + + case(3): + // YAW drift correction indication + if(ground_speed 20000) { + printPerfData(timeNow-perf_mon_timer); + perf_mon_timer=timeNow; + } +#endif + + } + +} + +//******************************************************************************** +void startup_ground(void) +{ + uint16_t store=0; + int flashcount = 0; + + debug_handler(2); + for(int c=0; c= 10) { + flashcount = 0; + digitalWrite(7,HIGH); + digitalWrite(6,LOW); + digitalWrite(5,HIGH); + } + flashcount++; + } + digitalWrite(5,LOW); + digitalWrite(6,LOW); + digitalWrite(7,LOW); + + AN_OFFSET[5]-=GRAVITY*SENSOR_SIGN[5]; + + // Store parameters in eeprom + for(int y=0; y<=5; y++) + { + Serial.println(AN_OFFSET[y]); + store = ((AN_OFFSET[y]-200.f)*100.0f); + eeprom_busy_wait(); + eeprom_write_word((uint16_t *) (y*2+2), store); + } + + groundstartDone = true; + debug_handler(6); +} + +//************************************************************************************ +void startup_air(void) +{ + uint16_t temp=0; + + for(int y=0; y<=5; y++) + { + eeprom_busy_wait(); + temp = eeprom_read_word((uint16_t *) (y*2+2)); + AN_OFFSET[y] = temp/100.f+200.f; + Serial.println(AN_OFFSET[y]); + } + debug_handler(5); +} + + +void debug_print(char string[]) +{ +#if PRINT_DEBUG != 0 + Serial.print("???"); + Serial.print(string); + Serial.println("***"); +#endif +} + +void debug_handler(byte message) +{ +#if PRINT_DEBUG != 0 + + static unsigned long BAD_Checksum=0; + + switch(message) + { + case 0: + Serial.print("???Software Version "); + Serial.print(SOFTWARE_VER); + Serial.println("***"); + break; + + case 1: + Serial.println("???Air Start!***"); + break; + + case 2: + Serial.println("???Ground Start!***"); + break; + + case 3: + Serial.println("???Enabling Magneto...***"); + break; + + case 4: + Serial.println("???Enabling Pressure Altitude...***"); + break; + + case 5: + Serial.println("???Air Start complete"); + break; + + case 6: + Serial.println("???Ground Start complete"); + break; + + case 10: + BAD_Checksum++; + Serial.print("???GPS Bad Checksum: "); + Serial.print(BAD_Checksum); + Serial.println("...***"); + break; + + default: + Serial.println("???Invalid debug ID...***"); + break; + + } +#endif + +} + +/* +EEPROM memory map + +0 0x00 Unused +1 0x01 .. +2 0x02 AN_OFFSET[0] +3 0x03 .. +4 0x04 AN_OFFSET[1] +5 0x05 .. +6 0x06 AN_OFFSET[2] +7 0x07 .. +8 0x08 AN_OFFSET[3] +9 0x09 .. +10 0x0A AN_OFFSET[4] +11 0x0B .. +12 0x0C AN_OFFSET[5] +13 0x0D .. +14 0x0E Unused +15 0x0F .. +16 0x10 Ground Pressure +17 0x11 .. +18 0x12 .. +19 0x13 .. +20 0x14 Ground Temp +21 0x15 .. +22 0x16 Ground Altitude +23 0x17 .. +*/ diff --git a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/matrix.pde b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/matrix.pde new file mode 100644 index 0000000000..fd825e0e30 --- /dev/null +++ b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/matrix.pde @@ -0,0 +1,22 @@ +/**************************************************/ +//Multiply two 3x3 matrixs. This function developed by Jordi can be easily adapted to multiple n*n matrix's. (Pero me da flojera!). +void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3]) +{ + float op[3]; + for(int x=0; x<3; x++) + { + for(int y=0; y<3; y++) + { + for(int w=0; w<3; w++) + { + op[w]=a[x][w]*b[w][y]; + } + mat[x][y]=0; + mat[x][y]=op[0]+op[1]+op[2]; + + float test=mat[x][y]; + } + } +} + + diff --git a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/timing.pde b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/timing.pde new file mode 100644 index 0000000000..5c736e5518 --- /dev/null +++ b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/timing.pde @@ -0,0 +1,23 @@ +extern unsigned long timer0_millis; + +// this function replaces the arduino millis() funcion +unsigned long DIYmillis() +{ + unsigned long m; + unsigned long m2; + + // timer0_millis could change inside timer0 interrupt and we don´t want to disable interrupts + // we can do two readings and compare. + m = timer0_millis; + m2 = timer0_millis; + if (m!=m2) // timer0_millis corrupted? + m = timer0_millis; // this should be fine... + return m; +} + +void DIYdelay(unsigned long ms) +{ + unsigned long start = DIYmillis(); + while (DIYmillis() - start <= ms) + ; +} diff --git a/sw/airborne/booz/arch/sim/booz2_analog_hw.c b/sw/airborne/firmwares/rotorcraft/actuators/actuators_dummy.c similarity index 87% rename from sw/airborne/booz/arch/sim/booz2_analog_hw.c rename to sw/airborne/firmwares/rotorcraft/actuators/actuators_dummy.c index 25404f6ec4..3204727e9f 100644 --- a/sw/airborne/booz/arch/sim/booz2_analog_hw.c +++ b/sw/airborne/firmwares/rotorcraft/actuators/actuators_dummy.c @@ -21,9 +21,15 @@ * Boston, MA 02111-1307, USA. */ -#include "booz2_analog.h" +#include "firmwares/rotorcraft/actuators.h" -void booz2_analog_init_hw(void) { + + + +void actuators_init(void) { } + +void actuators_set(bool_t motors_on) { +} diff --git a/sw/airborne/firmwares/rotorcraft/actuators/actuators_mkk.c b/sw/airborne/firmwares/rotorcraft/actuators/actuators_mkk.c index 90bb98f8f9..07602f1430 100644 --- a/sw/airborne/firmwares/rotorcraft/actuators/actuators_mkk.c +++ b/sw/airborne/firmwares/rotorcraft/actuators/actuators_mkk.c @@ -43,7 +43,6 @@ void actuators_init(void) { actuators_mkk.trans[i].type = I2CTransTx; actuators_mkk.trans[i].len_w = 1; actuators_mkk.trans[i].slave_addr = actuators_addr[i]; - actuators_mkk.trans[i].stop_after_transmit = TRUE; actuators_mkk.trans[i].status = I2CTransSuccess; } diff --git a/sw/airborne/firmwares/rotorcraft/actuators/actuators_pwm.h b/sw/airborne/firmwares/rotorcraft/actuators/actuators_pwm.h index a38ed0b6e2..6d17dc4561 100644 --- a/sw/airborne/firmwares/rotorcraft/actuators/actuators_pwm.h +++ b/sw/airborne/firmwares/rotorcraft/actuators/actuators_pwm.h @@ -27,12 +27,13 @@ #include "std.h" #include "firmwares/rotorcraft/actuators.h" -#define ACTUATORS_PWM_NB 6 +#include BOARD_CONFIG +#include "actuators_pwm_arch.h" + extern int32_t actuators_pwm_values[ACTUATORS_PWM_NB]; //already declared in actuators.h //extern void actuators_init(void); -#include "actuators_pwm_arch.h" #endif /* ACTUATORS_PWM_H */ diff --git a/sw/airborne/firmwares/rotorcraft/actuators/actuators_pwm_supervision.c b/sw/airborne/firmwares/rotorcraft/actuators/actuators_pwm_supervision.c index 06fb68f5ad..9debad1cfc 100644 --- a/sw/airborne/firmwares/rotorcraft/actuators/actuators_pwm_supervision.c +++ b/sw/airborne/firmwares/rotorcraft/actuators/actuators_pwm_supervision.c @@ -46,6 +46,7 @@ void actuators_set(bool_t motors_on) { booz2_commands[COMMAND_PITCH] = booz2_commands[COMMAND_PITCH] * PWM_GAIN_SCALE; booz2_commands[COMMAND_ROLL] = booz2_commands[COMMAND_ROLL] * PWM_GAIN_SCALE; booz2_commands[COMMAND_YAW] = booz2_commands[COMMAND_YAW] * PWM_GAIN_SCALE; + booz2_commands[COMMAND_THRUST] = (booz2_commands[COMMAND_THRUST] * ((SUPERVISION_MAX_MOTOR - SUPERVISION_MIN_MOTOR) / 200)) + SUPERVISION_MIN_MOTOR; supervision_run(motors_on, FALSE, booz2_commands); diff --git a/sw/airborne/firmwares/rotorcraft/actuators/arch/stm32/actuators_pwm_arch.c b/sw/airborne/firmwares/rotorcraft/actuators/arch/stm32/actuators_pwm_arch.c index f412414b9e..c31ef54232 100644 --- a/sw/airborne/firmwares/rotorcraft/actuators/arch/stm32/actuators_pwm_arch.c +++ b/sw/airborne/firmwares/rotorcraft/actuators/arch/stm32/actuators_pwm_arch.c @@ -36,13 +36,35 @@ #define SERVO_HZ 40 #endif +#ifndef PWM_5AND6_TIMER +#define PWM_5AND6_TIMER TIM4 +#define PWM_5AND6_RCC RCC_APB1Periph_TIM4 +#define PWM_5AND6_GPIO GPIOB +#define PWM5_OC 3 +#define PWM6_OC 4 +#define PWM5_Pin GPIO_Pin_8 +#define PWM6_Pin GPIO_Pin_9 +#endif + +#define _TIM_OC_INIT(n) TIM_OC##n##Init +#define TIM_OC_INIT(n) _TIM_OC_INIT(n) + +#define _TIM_OC_PRELOADCONFIG(n) TIM_OC##n##PreloadConfig +#define TIM_OC_PRELOADCONFIG(n) _TIM_OC_PRELOADCONFIG(n) + +#define _TIM_SETCOMPARE(n) TIM_SetCompare##n +#define TIM_SETCOMPARE(n) _TIM_SETCOMPARE(n) + void actuators_pwm_arch_init(void) { /* TIM3 and TIM4 clock enable */ RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE); + RCC_APB1PeriphClockCmd(PWM_5AND6_RCC, ENABLE); +#ifdef USE_SERVOS_7AND8 RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE); +#endif - /* GPIOB and GPIOC clock enable */ + /* GPIO A,B and C clock enable */ RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB | RCC_APB2Periph_GPIOC | RCC_APB2Periph_AFIO, ENABLE); /* GPIO C */ @@ -55,10 +77,15 @@ void actuators_pwm_arch_init(void) { /* need to remate alternate function, pins 37, 38, 39, 40 on LQFP64 */ GPIO_PinRemapConfig(GPIO_FullRemap_TIM3, ENABLE); - /* GPIO B */ + /* PWM 5/6 GPIO */ /* PB8=servo5 PB9=servo6 */ - GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8 | GPIO_Pin_9; + GPIO_InitStructure.GPIO_Pin = PWM5_Pin | PWM6_Pin; + GPIO_Init(PWM_5AND6_GPIO, &GPIO_InitStructure); + +#ifdef USE_SERVOS_7AND8 + GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7; GPIO_Init(GPIOB, &GPIO_InitStructure); +#endif /* Time base configuration */ TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; @@ -68,7 +95,10 @@ void actuators_pwm_arch_init(void) { TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure); + TIM_TimeBaseInit(PWM_5AND6_TIMER, &TIM_TimeBaseStructure); +#ifdef USE_SERVOS_7AND8 TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure); +#endif /* PWM1 Mode configuration: All Channels */ TIM_OCInitTypeDef TIM_OCInitStructure; @@ -81,35 +111,50 @@ void actuators_pwm_arch_init(void) { TIM_OC1Init(TIM3, &TIM_OCInitStructure); TIM_OC1PreloadConfig(TIM3, TIM_OCPreload_Enable); - /* PWM1 Mode configuration: TIM3 Channel2 */ + /* PWM2 Mode configuration: TIM3 Channel2 */ TIM_OC2Init(TIM3, &TIM_OCInitStructure); TIM_OC2PreloadConfig(TIM3, TIM_OCPreload_Enable); - /* PWM1 Mode configuration: TIM3 Channel3 */ + /* PWM3 Mode configuration: TIM3 Channel3 */ TIM_OC3Init(TIM3, &TIM_OCInitStructure); TIM_OC3PreloadConfig(TIM3, TIM_OCPreload_Enable); - /* PWM1 Mode configuration: TIM3 Channel4 */ + /* PWM4 Mode configuration: TIM3 Channel4 */ TIM_OC4Init(TIM3, &TIM_OCInitStructure); TIM_OC4PreloadConfig(TIM3, TIM_OCPreload_Enable); - /* PWM1 Mode configuration: TIM4 Channel3 */ - TIM_OC3Init(TIM4, &TIM_OCInitStructure); - TIM_OC3PreloadConfig(TIM4, TIM_OCPreload_Enable); + /* PWM5 Mode configuration: TIM4 Channel3 */ + TIM_OC_INIT(PWM5_OC) (PWM_5AND6_TIMER, &TIM_OCInitStructure); + TIM_OC_PRELOADCONFIG(PWM5_OC)(PWM_5AND6_TIMER, TIM_OCPreload_Enable); - /* PWM1 Mode configuration: TIM4 Channel4 */ - TIM_OC4Init(TIM4, &TIM_OCInitStructure); - TIM_OC4PreloadConfig(TIM4, TIM_OCPreload_Enable); + /* PWM6 Mode configuration: TIM4 Channel4 */ + TIM_OC_INIT(PWM6_OC)(PWM_5AND6_TIMER, &TIM_OCInitStructure); + TIM_OC_PRELOADCONFIG(PWM6_OC)(PWM_5AND6_TIMER, TIM_OCPreload_Enable); - /* TIM3 enable */ - TIM_ARRPreloadConfig(TIM3, ENABLE); - TIM_CtrlPWMOutputs(TIM3, ENABLE); - TIM_Cmd(TIM3, ENABLE); +#ifdef USE_SERVOS_7AND8 + /* PWM7 Mode configuration: TIM4 Channel3 */ + TIM_OC1Init(TIM4, &TIM_OCInitStructure); + TIM_OC1PreloadConfig(TIM4, TIM_OCPreload_Enable); + + /* PWM8 Mode configuration: TIM4 Channel4 */ + TIM_OC2Init(TIM4, &TIM_OCInitStructure); + TIM_OC2PreloadConfig(TIM4, TIM_OCPreload_Enable); /* TIM4 enable */ TIM_ARRPreloadConfig(TIM4, ENABLE); TIM_CtrlPWMOutputs(TIM4, ENABLE); TIM_Cmd(TIM4, ENABLE); +#endif + + /* PWM1-4 enable */ + TIM_ARRPreloadConfig(TIM3, ENABLE); + TIM_CtrlPWMOutputs(TIM3, ENABLE); + TIM_Cmd(TIM3, ENABLE); + + /* PWM5/6 enable */ + TIM_ARRPreloadConfig(PWM_5AND6_TIMER, ENABLE); + TIM_CtrlPWMOutputs(PWM_5AND6_TIMER, ENABLE); + TIM_Cmd(PWM_5AND6_TIMER, ENABLE); } @@ -119,6 +164,12 @@ void actuators_pwm_commit(void) { TIM_SetCompare2(TIM3, actuators_pwm_values[1]); TIM_SetCompare3(TIM3, actuators_pwm_values[2]); TIM_SetCompare4(TIM3, actuators_pwm_values[3]); - TIM_SetCompare3(TIM4, actuators_pwm_values[4]); - TIM_SetCompare4(TIM4, actuators_pwm_values[5]); + + TIM_SETCOMPARE(PWM5_OC)(PWM_5AND6_TIMER, actuators_pwm_values[4]); + TIM_SETCOMPARE(PWM6_OC)(PWM_5AND6_TIMER, actuators_pwm_values[5]); + +#ifdef USE_SERVOS_7AND8 + TIM_SetCompare1(TIM4, actuators_pwm_values[6]); + TIM_SetCompare2(TIM4, actuators_pwm_values[7]); +#endif } diff --git a/sw/airborne/firmwares/rotorcraft/actuators/arch/stm32/actuators_pwm_arch.h b/sw/airborne/firmwares/rotorcraft/actuators/arch/stm32/actuators_pwm_arch.h index e1482b22b1..7dbcb8f611 100644 --- a/sw/airborne/firmwares/rotorcraft/actuators/arch/stm32/actuators_pwm_arch.h +++ b/sw/airborne/firmwares/rotorcraft/actuators/arch/stm32/actuators_pwm_arch.h @@ -29,6 +29,12 @@ #ifndef ACTUATORS_PWM_ARCH_H #define ACTUATORS_PWM_ARCH_H +#ifdef USE_SERVOS_7AND8 +#define ACTUATORS_PWM_NB 8 +#else +#define ACTUATORS_PWM_NB 6 +#endif + extern void actuators_pwm_arch_init(void); extern void actuators_pwm_commit(void); diff --git a/sw/airborne/firmwares/rotorcraft/battery.c b/sw/airborne/firmwares/rotorcraft/battery.c deleted file mode 100644 index 357cc448d2..0000000000 --- a/sw/airborne/firmwares/rotorcraft/battery.c +++ /dev/null @@ -1,30 +0,0 @@ -/* - * $Id$ - * - * Copyright (C) 2008-2009 Antoine Drouin - * - * 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. - */ - -#include "firmwares/rotorcraft/battery.h" - -uint8_t battery_voltage; - -void battery_init(void) { - battery_voltage = 0; -} diff --git a/sw/airborne/firmwares/rotorcraft/battery.h b/sw/airborne/firmwares/rotorcraft/battery.h deleted file mode 100644 index 41aa171692..0000000000 --- a/sw/airborne/firmwares/rotorcraft/battery.h +++ /dev/null @@ -1,43 +0,0 @@ -/* - * $Id$ - * - * Copyright (C) 2008-2009 Antoine Drouin - * - * 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. - */ - -#ifndef BATTERY_H -#define BATTERY_H - -#include "std.h" - -#include "generated/airframe.h" - -/* decivolts */ -extern uint8_t battery_voltage; - -static inline void BatteryISRHandler(uint16_t _val) { - uint32_t cal_v = (uint32_t)(_val) * BATTERY_SENS_NUM / BATTERY_SENS_DEN; - uint32_t sum = (uint32_t)battery_voltage + cal_v; - battery_voltage = (uint8_t)(sum/2); -} - - -extern void battery_init(void); - -#endif /* BATTERY_H */ diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c index 03a4b251b2..8dbdf59828 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c @@ -111,7 +111,7 @@ void guidance_v_read_rc(void) { // used in RC_DIRECT directly and as saturation in CLIMB and HOVER #ifndef USE_HELI - guidance_v_rc_delta_t = (int32_t)radio_control.values[RADIO_THROTTLE] * (SUPERVISION_MAX_MOTOR - SUPERVISION_MIN_MOTOR) / MAX_PPRZ + SUPERVISION_MIN_MOTOR; + guidance_v_rc_delta_t = (int32_t)radio_control.values[RADIO_THROTTLE] * 200 / MAX_PPRZ; #else guidance_v_rc_delta_t = (int32_t)radio_control.values[RADIO_THROTTLE] * 4 / 5; #endif diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c index ad0bf0e3c0..1a960eda0c 100644 --- a/sw/airborne/firmwares/rotorcraft/main.c +++ b/sw/airborne/firmwares/rotorcraft/main.c @@ -32,6 +32,7 @@ #include "downlink.h" #include "firmwares/rotorcraft/telemetry.h" #include "datalink.h" +#include "subsystems/settings.h" #include "xbee.h" #include "booz2_commands.h" @@ -41,13 +42,11 @@ #include "subsystems/imu.h" #include "booz_gps.h" -#include "booz/booz2_analog.h" #include "subsystems/sensors/baro.h" #include "baro_board.h" -#include "firmwares/rotorcraft/battery.h" +#include "subsystems/electrical.h" -// #include "booz_fms.h" // FIXME #include "firmwares/rotorcraft/autopilot.h" #include "firmwares/rotorcraft/stabilization.h" @@ -98,6 +97,7 @@ STATIC_INLINE void main_init( void ) { mcu_init(); sys_time_init(); + electrical_init(); actuators_init(); radio_control_init(); @@ -106,13 +106,8 @@ STATIC_INLINE void main_init( void ) { xbee_init(); #endif - booz2_analog_init(); baro_init(); - - battery_init(); imu_init(); - - // booz_fms_init(); // FIXME autopilot_init(); nav_init(); guidance_h_init(); @@ -130,6 +125,8 @@ STATIC_INLINE void main_init( void ) { modules_init(); + settings_init(); + mcu_int_enable(); } @@ -156,7 +153,7 @@ STATIC_INLINE void main_periodic( void ) { /* booz_fms_periodic(); FIXME */ \ }, \ { \ - /*BoozControlSurfacesSetFromCommands();*/ \ + electrical_periodic(); \ }, \ { \ LED_PERIODIC(); \ @@ -172,16 +169,12 @@ STATIC_INLINE void main_periodic( void ) { } ); #ifdef USE_GPS - if (radio_control.status != RC_OK && \ + if (radio_control.status != RC_OK && \ autopilot_mode == AP_MODE_NAV && GpsIsLost()) \ autopilot_set_mode(AP_MODE_FAILSAFE); \ booz_gps_periodic(); #endif -#ifdef USE_EXTRA_ADC - booz2_analog_periodic(); -#endif - modules_periodic_task(); if (autopilot_in_flight) { diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c index 2680feef43..36f6dd9df7 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c @@ -200,7 +200,7 @@ void stabilization_attitude_run(bool_t enable_integrator) { scaled_att_err.qy = att_err.qy / IERROR_SCALE; scaled_att_err.qz = att_err.qz / IERROR_SCALE; FLOAT_QUAT_COMP_INV(new_sum_err, stabilization_att_sum_err_quat, scaled_att_err); - FLOAT_QUAT_NORMALISE(new_sum_err); + FLOAT_QUAT_NORMALIZE(new_sum_err); FLOAT_QUAT_COPY(stabilization_att_sum_err_quat, new_sum_err); FLOAT_EULERS_OF_QUAT(stabilization_att_sum_err_eulers, stabilization_att_sum_err_quat); } else { diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c index 7799d9995b..c6478d5b40 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c @@ -117,7 +117,7 @@ void stabilization_attitude_ref_update() { FLOAT_QUAT_DERIVATIVE(qdot, stab_att_ref_rate, stab_att_ref_quat); QUAT_SMUL(qdot, qdot, DT_UPDATE); QUAT_ADD(stab_att_ref_quat, qdot); - FLOAT_QUAT_NORMALISE(stab_att_ref_quat); + FLOAT_QUAT_NORMALIZE(stab_att_ref_quat); /* integrate reference rotational speeds */ struct FloatRates delta_rate; diff --git a/sw/airborne/firmwares/rotorcraft/telemetry.h b/sw/airborne/firmwares/rotorcraft/telemetry.h index 1cf5801b80..65a755cbb6 100644 --- a/sw/airborne/firmwares/rotorcraft/telemetry.h +++ b/sw/airborne/firmwares/rotorcraft/telemetry.h @@ -45,7 +45,7 @@ #define PERIODIC_SEND_ALIVE(_chan) DOWNLINK_SEND_ALIVE(_chan, 16, MD5SUM) -#include "firmwares/rotorcraft/battery.h" +#include "subsystems/electrical.h" #include "subsystems/imu.h" #include "booz_gps.h" #include "subsystems/ins.h" @@ -56,43 +56,43 @@ extern uint8_t telemetry_mode_Main_DefaultChannel; #ifdef USE_GPS -#define PERIODIC_SEND_ROTORCRAFT_STATUS(_chan) { \ - uint32_t imu_nb_err = 0; \ +#define PERIODIC_SEND_ROTORCRAFT_STATUS(_chan) { \ + uint32_t imu_nb_err = 0; \ uint8_t _twi_blmc_nb_err = 0; \ - DOWNLINK_SEND_ROTORCRAFT_STATUS(_chan, \ - &imu_nb_err, \ - &_twi_blmc_nb_err, \ - &radio_control.status, \ - &radio_control.frame_rate, \ - &booz_gps_state.fix, \ - &autopilot_mode, \ - &autopilot_in_flight, \ - &autopilot_motors_on, \ - &guidance_h_mode, \ - &guidance_v_mode, \ - &battery_voltage, \ - &cpu_time_sec \ - ); \ + DOWNLINK_SEND_ROTORCRAFT_STATUS(_chan, \ + &imu_nb_err, \ + &_twi_blmc_nb_err, \ + &radio_control.status, \ + &radio_control.frame_rate, \ + &booz_gps_state.fix, \ + &autopilot_mode, \ + &autopilot_in_flight, \ + &autopilot_motors_on, \ + &guidance_h_mode, \ + &guidance_v_mode, \ + &electrical.vsupply, \ + &cpu_time_sec \ + ); \ } #else /* !USE_GPS */ -#define PERIODIC_SEND_ROTORCRAFT_STATUS(_chan) { \ - uint32_t imu_nb_err = 0; \ +#define PERIODIC_SEND_ROTORCRAFT_STATUS(_chan) { \ + uint32_t imu_nb_err = 0; \ uint8_t twi_blmc_nb_err = 0; \ uint8_t fix = BOOZ2_GPS_FIX_NONE; \ - DOWNLINK_SEND_ROTORCRAFT_STATUS(_chan, \ - &imu_nb_err, \ - &twi_blmc_nb_err, \ - &radio_control.status, \ - &radio_control.frame_rate, \ - &fix, \ - &autopilot_mode, \ - &autopilot_in_flight, \ - &autopilot_motors_on, \ - &guidance_h_mode, \ - &guidance_v_mode, \ - &battery_voltage, \ - &cpu_time_sec \ - ); \ + DOWNLINK_SEND_ROTORCRAFT_STATUS(_chan, \ + &imu_nb_err, \ + &twi_blmc_nb_err, \ + &radio_control.status, \ + &radio_control.frame_rate, \ + &fix, \ + &autopilot_mode, \ + &autopilot_in_flight, \ + &autopilot_motors_on, \ + &guidance_h_mode, \ + &guidance_v_mode, \ + &electrical.vsupply, \ + &cpu_time_sec \ + ); \ } #endif /* USE_GPS */ @@ -130,22 +130,22 @@ extern uint8_t telemetry_mode_Main_DefaultChannel; #define PERIODIC_SEND_PPM(_chan) {} #endif -#define PERIODIC_SEND_BOOZ2_GYRO(_chan) { \ - DOWNLINK_SEND_BOOZ2_GYRO(_chan, \ +#define PERIODIC_SEND_IMU_GYRO_SCALED(_chan) { \ + DOWNLINK_SEND_IMU_GYRO_SCALED(_chan, \ &imu.gyro.p, \ &imu.gyro.q, \ &imu.gyro.r); \ } -#define PERIODIC_SEND_BOOZ2_ACCEL(_chan) { \ - DOWNLINK_SEND_BOOZ2_ACCEL(_chan, \ +#define PERIODIC_SEND_IMU_ACCEL_SCALED(_chan) { \ + DOWNLINK_SEND_IMU_ACCEL_SCALED(_chan, \ &imu.accel.x, \ &imu.accel.y, \ &imu.accel.z); \ } -#define PERIODIC_SEND_BOOZ2_MAG(_chan) { \ - DOWNLINK_SEND_BOOZ2_MAG(_chan, \ +#define PERIODIC_SEND_IMU_MAG_SCALED(_chan) { \ + DOWNLINK_SEND_IMU_MAG_SCALED(_chan, \ &imu.mag.x, \ &imu.mag.y, \ &imu.mag.z); \ diff --git a/sw/airborne/fms/fms_spi_autopilot_msg.c b/sw/airborne/fms/fms_spi_autopilot_msg.c index 33c0a53ee1..8df37b8ba3 100644 --- a/sw/airborne/fms/fms_spi_autopilot_msg.c +++ b/sw/airborne/fms/fms_spi_autopilot_msg.c @@ -106,7 +106,7 @@ int spi_ap_link_init() FLOAT_QUAT_OF_AXIS_ANGLE(imuFloat.body_to_imu_quat, x_axis, QUAT_SETPOINT_HOVER_PITCH); #endif - FLOAT_QUAT_NORMALISE(imuFloat.body_to_imu_quat); + FLOAT_QUAT_NORMALIZE(imuFloat.body_to_imu_quat); FLOAT_EULERS_OF_QUAT(imuFloat.body_to_imu_eulers, imuFloat.body_to_imu_quat); FLOAT_RMAT_OF_QUAT(imuFloat.body_to_imu_rmat, imuFloat.body_to_imu_quat); diff --git a/sw/airborne/lisa/lisa_analog_plug.c b/sw/airborne/lisa/lisa_analog_plug.c deleted file mode 100644 index 3e49d689d9..0000000000 --- a/sw/airborne/lisa/lisa_analog_plug.c +++ /dev/null @@ -1,10 +0,0 @@ -#include "std.h" - -uint8_t battery_voltage; - -void booz2_analog_init(void); -void battery_init(void); - -void booz2_analog_init(void) {} -void battery_init(void) {} - diff --git a/sw/airborne/lisa/test_servos.c b/sw/airborne/lisa/test_servos.c index 533feedc1a..98ae30c1f2 100644 --- a/sw/airborne/lisa/test_servos.c +++ b/sw/airborne/lisa/test_servos.c @@ -50,14 +50,9 @@ static inline void main_periodic( void ) { static float foo = 0.; foo += 0.0025; int32_t bar = 1500 + 500. * sin(foo); - // int32_t bar = 1450; - // int32_t bar = 1950; - actuators_pwm_values[0] = bar; - actuators_pwm_values[1] = bar; - actuators_pwm_values[2] = bar; - actuators_pwm_values[3] = bar; - actuators_pwm_values[4] = bar; - actuators_pwm_values[5] = bar; + for (int i = 0; i < ACTUATORS_PWM_NB; i++) { + actuators_pwm_values[i] = bar; + } actuators_pwm_commit(); LED_PERIODIC(); diff --git a/sw/airborne/lisa/test_uart_lisam.c b/sw/airborne/lisa/test_uart_lisam.c index 4e898b0793..368b31bed5 100644 --- a/sw/airborne/lisa/test_uart_lisam.c +++ b/sw/airborne/lisa/test_uart_lisam.c @@ -57,6 +57,7 @@ static inline void main_periodic( void ) { Uart1Transmit('a'); Uart2Transmit('b'); Uart3Transmit('c'); + Uart5Transmit('d'); LED_OFF(1); LED_OFF(2); @@ -87,4 +88,13 @@ static inline void main_periodic( void ) { LED_ON(2); } } + + if (Uart5ChAvailable()) { + ch = Uart5Getch(); + if (ch == 'd') { + LED_ON(1); + } else { + LED_ON(2); + } + } } diff --git a/sw/airborne/math/pprz_algebra.h b/sw/airborne/math/pprz_algebra.h index 6e687e8dd3..81b35c7b54 100644 --- a/sw/airborne/math/pprz_algebra.h +++ b/sw/airborne/math/pprz_algebra.h @@ -26,6 +26,7 @@ #define PPRZ_ALGEBRA_H #include /* for FLT_EPSILON */ +#include /* for memcpy */ #define SQUARE(_a) ((_a)*(_a)) @@ -128,6 +129,13 @@ (_c).z = (_a).z + (_b).z; \ } +/* a += b*s */ +#define VECT3_ADD_SCALED(_a, _b, _s) { \ + (_a).x += ((_b).x * (_s)); \ + (_a).y += ((_b).y * (_s)); \ + (_a).z += ((_b).z * (_s)); \ + } + /* c = a + _s * b */ #define VECT3_SUM_SCALED(_c, _a, _b, _s) { \ (_c).x = (_a).x + (_s)*(_b).x; \ @@ -500,6 +508,19 @@ RMAT_ELMT((_rmat), 2, 2) * (_vin).z; \ } +#define RMAT_VECT3_TRANSP_MUL(_vout, _rmat, _vin) { \ + (_vout).x = RMAT_ELMT((_rmat), 0, 0) * (_vin).x + \ + RMAT_ELMT((_rmat), 1, 0) * (_vin).y + \ + RMAT_ELMT((_rmat), 2, 0) * (_vin).z; \ + (_vout).y = RMAT_ELMT((_rmat), 0, 1) * (_vin).x + \ + RMAT_ELMT((_rmat), 1, 1) * (_vin).y + \ + RMAT_ELMT((_rmat), 2, 1) * (_vin).z; \ + (_vout).z = RMAT_ELMT((_rmat), 0, 2) * (_vin).x + \ + RMAT_ELMT((_rmat), 1, 2) * (_vin).y + \ + RMAT_ELMT((_rmat), 2, 2) * (_vin).z; \ + } + + #define RMAT_COPY(_o, _i) { memcpy(&(_o), &(_i), sizeof(_o));} diff --git a/sw/airborne/math/pprz_algebra_float.h b/sw/airborne/math/pprz_algebra_float.h index 3b2bfb9688..5a31345f6c 100644 --- a/sw/airborne/math/pprz_algebra_float.h +++ b/sw/airborne/math/pprz_algebra_float.h @@ -153,6 +153,14 @@ struct FloatRates { (_vo).z = (_v1).x*(_v2).y - (_v1).y*(_v2).x; \ } +#define FLOAT_VECT3_INTEGRATE_FI(_vo, _dv, _dt) { \ + (_vo).x += (_dv).x * (_dt); \ + (_vo).y += (_dv).y * (_dt); \ + (_vo).z += (_dv).z * (_dt); \ + } + + + #define FLOAT_VECT3_NORMALIZE(_v) { \ const float n = FLOAT_VECT3_NORM(_v); \ FLOAT_VECT3_SMUL(_v, _v, 1./n); \ @@ -170,6 +178,34 @@ struct FloatRates { _ro.r += _v.z * _s; \ } + +#define FLOAT_RATES_LIN_CMB(_ro, _r1, _s1, _r2, _s2) { \ + _ro.p = _s1 * _r1.p + _s2 * _r2.p; \ + _ro.q = _s1 * _r1.q + _s2 * _r2.q; \ + _ro.r = _s1 * _r1.r + _s2 * _r2.r; \ + } + + +#define FLOAT_RATES_SCALE(_ro,_s) { \ + _ro.p *= _s; \ + _ro.q *= _s; \ + _ro.r *= _s; \ + } + +#define FLOAT_RATES_INTEGRATE_FI(_ra, _racc, _dt) { \ + (_ra).p += (_racc).p * (_dt); \ + (_ra).q += (_racc).q * (_dt); \ + (_ra).r += (_racc).r * (_dt); \ + } + +#define FLOAT_RATES_OF_EULER_DOT(_ra, _e, _ed) { \ + _ra.p = _ed.phi - sinf(_e.theta) *_ed.psi; \ + _ra.q = cosf(_e.phi)*_ed.theta + sinf(_e.phi)*cosf(_e.theta)*_ed.psi; \ + _ra.r = -sinf(_e.phi)*_ed.theta + cosf(_e.phi)*cosf(_e.theta)*_ed.psi; \ + } + + + /* * 3x3 matrices */ @@ -232,6 +268,7 @@ struct FloatRates { /* multiply _vin by _rmat, store in _vout */ #define FLOAT_RMAT_VECT3_MUL(_vout, _rmat, _vin) RMAT_VECT3_MUL(_vout, _rmat, _vin) +#define FLOAT_RMAT_VECT3_TRANSP_MUL(_vout, _rmat, _vin) RMAT_VECT3_TRANSP_MUL(_vout, _rmat, _vin) #define FLOAT_RMAT_TRANSP_RATEMULT(_vb, _m_b2a, _va) { \ (_vb).p = ( (_m_b2a).m[0]*(_va).p + (_m_b2a).m[3]*(_va).q + (_m_b2a).m[6]*(_va).r); \ @@ -395,6 +432,18 @@ struct FloatRates { } #endif +/* in place first order integration of a rotation matrix */ +#define FLOAT_RMAT_INTEGRATE_FI(_rm, _omega, _dt ) { \ + struct FloatRMat exp_omega_dt = { \ + { 1. , dt*omega.r, -dt*omega.q, \ + -dt*omega.r, 1. , dt*omega.p, \ + dt*omega.q, -dt*omega.p, 1. }}; \ + struct FloatRMat R_tdt; \ + FLOAT_RMAT_COMP(R_tdt, _rm, exp_omega_dt); \ + memcpy(&(_rm), &R_tdt, sizeof(R_tdt)); \ + } + + static inline float renorm_factor(float n) { if (n < 1.5625f && n > 0.64f) return .5 * (3-n); @@ -445,7 +494,7 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) { #define FLOAT_QUAT_NORM(_q) (sqrtf(SQUARE(_q.qi) + SQUARE(_q.qx)+ \ SQUARE(_q.qy) + SQUARE(_q.qz))) \ -#define FLOAT_QUAT_NORMALISE(q) { \ +#define FLOAT_QUAT_NORMALIZE(q) { \ float norm = FLOAT_QUAT_NORM(q); \ if (norm > FLT_MIN) { \ q.qi = q.qi / norm; \ @@ -466,7 +515,7 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) { #define FLOAT_QUAT_COMP_NORM_SHORTEST(_a2c, _a2b, _b2c) { \ FLOAT_QUAT_COMP(_a2c, _a2b, _b2c); \ FLOAT_QUAT_WRAP_SHORTEST(_a2c); \ - FLOAT_QUAT_NORMALISE(_a2c); \ + FLOAT_QUAT_NORMALIZE(_a2c); \ } /* _a2c = _a2b comp _b2c , aka _a2c = _a2b * _b2c */ @@ -483,7 +532,7 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) { #define FLOAT_QUAT_COMP_INV_NORM_SHORTEST(_a2b, _a2c, _b2c) { \ FLOAT_QUAT_COMP_INV(_a2b, _a2c, _b2c); \ FLOAT_QUAT_WRAP_SHORTEST(_a2b); \ - FLOAT_QUAT_NORMALISE(_a2b); \ + FLOAT_QUAT_NORMALIZE(_a2b); \ } /* _a2b = _a2c comp_inv _b2c , aka _a2b = _a2c * inv(_b2c) */ @@ -498,7 +547,7 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) { #define FLOAT_QUAT_INV_COMP_NORM_SHORTEST(_b2c, _a2b, _a2c) { \ FLOAT_QUAT_INV_COMP(_b2c, _a2b, _a2c); \ FLOAT_QUAT_WRAP_SHORTEST(_b2c); \ - FLOAT_QUAT_NORMALISE(_b2c); \ + FLOAT_QUAT_NORMALIZE(_b2c); \ } /* _b2c = _a2b inv_comp _a2c , aka _b2c = inv(_a2b) * _a2c */ @@ -509,7 +558,7 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) { (_b2c).qz = (_a2b).qi*(_a2c).qz - (_a2b).qx*(_a2c).qy + (_a2b).qy*(_a2c).qx - (_a2b).qz*(_a2c).qi; \ } -#define FLOAT_QUAT_DIFFERENTIAL(q_out, w, dt) { \ +#define FLOAT_QUAT_DIFFERENTIAL(q_out, w, dt) { \ const float v_norm = sqrt((w).p*(w).p + (w).q*(w).q + (w).r*(w).r); \ const float c2 = cos(dt*v_norm/2.0); \ const float s2 = sin(dt*v_norm/2.0); \ @@ -526,6 +575,27 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) { } \ } +/* in place quaternion integration with constante rotational velocity */ +#define FLOAT_QUAT_INTEGRATE(_q, _omega, _dt) { \ + const float no = FLOAT_RATES_NORM(_omega); \ + if (no > FLT_MIN) { \ + const float a = 0.5*no*_dt; \ + const float ca = cosf(a); \ + const float sa_ov_no = sinf(a)/no; \ + const float dp = sa_ov_no*_omega.p; \ + const float dq = sa_ov_no*_omega.q; \ + const float dr = sa_ov_no*_omega.r; \ + const float qi = _q.qi; \ + const float qx = _q.qx; \ + const float qy = _q.qy; \ + const float qz = _q.qz; \ + _q.qi = ca*qi - dp*qx - dq*qy - dr*qz; \ + _q.qx = dp*qi + ca*qx + dr*qy - dq*qz; \ + _q.qy = dq*qi - dr*qx + ca*qy + dp*qz; \ + _q.qz = dr*qi + dq*qx - dp*qy + ca*qz; \ + } \ + } + #ifdef ALGEBRA_FLOAT_USE_SLOW_FUNCTIONS #define FLOAT_QUAT_VMULT(v_out, q, v_in) { \ const float qi2 = q.qi*q.qi; \ diff --git a/sw/airborne/math/pprz_algebra_int.h b/sw/airborne/math/pprz_algebra_int.h index 5570f8ee76..0a410b2e6a 100644 --- a/sw/airborne/math/pprz_algebra_int.h +++ b/sw/airborne/math/pprz_algebra_int.h @@ -75,7 +75,7 @@ struct Int32Vect3 { /* Rotation quaternions */ #define INT32_QUAT_FRAC 15 /** - * @brief Roation quaternion + * @brief Rotation quaternion * @details Units: INT32_QUAT_FRAC */ struct Int32Quat { int32_t qi; @@ -84,6 +84,15 @@ struct Int32Quat { int32_t qz; }; + +struct Int64Quat { + int32_t qi; + int32_t qx; + int32_t qy; + int32_t qz; +}; + + /* Euler angles */ #define INT32_ANGLE_FRAC 12 #define INT32_RATE_FRAC 12 @@ -154,6 +163,13 @@ struct Int32Rates { int32_t r; ///< in rad/s^2 with INT32_RATE_FRAC }; +struct Int64Rates { + int64_t p; ///< in rad/s^2 with INT32_RATE_FRAC + int64_t q; ///< in rad/s^2 with INT32_RATE_FRAC + int64_t r; ///< in rad/s^2 with INT32_RATE_FRAC +}; + + struct Int64Vect2 { int64_t x; int64_t y; @@ -253,6 +269,11 @@ struct Int64Vect3 { (_o).z = ((_i).z << (_l)); \ } +#define INT32_VECT3_CROSS_PRODUCT(_vo, _v1, _v2) { \ + (_vo).x = (_v1).y*(_v2).z - (_v1).z*(_v2).y; \ + (_vo).y = (_v1).z*(_v2).x - (_v1).x*(_v2).z; \ + (_vo).z = (_v1).x*(_v2).y - (_v1).y*(_v2).x; \ + } @@ -272,15 +293,15 @@ struct Int64Vect3 { } #define INT32_MAT33_DIAG(_m, _d00, _d11, _d22) { \ - MAT33_ELMT((_m), 0, 0) = (_d00); \ + MAT33_ELMT((_m), 0, 0) = (_d00); \ MAT33_ELMT((_m), 0, 1) = 0; \ MAT33_ELMT((_m), 0, 2) = 0; \ MAT33_ELMT((_m), 1, 0) = 0; \ - MAT33_ELMT((_m), 1, 1) = (_d11); \ + MAT33_ELMT((_m), 1, 1) = (_d11); \ MAT33_ELMT((_m), 1, 2) = 0; \ MAT33_ELMT((_m), 2, 0) = 0; \ MAT33_ELMT((_m), 2, 1) = 0; \ - MAT33_ELMT((_m), 2, 2) = (_d22); \ + MAT33_ELMT((_m), 2, 2) = (_d22); \ } @@ -507,9 +528,9 @@ struct Int64Vect3 { #define INT32_QUAT_ZERO(_q) { \ (_q).qi = QUAT1_BFP_OF_REAL(1); \ - (_q).qx = 0; \ - (_q).qy = 0; \ - (_q).qz = 0; \ + (_q).qx = 0; \ + (_q).qy = 0; \ + (_q).qz = 0; \ } #define INT32_QUAT_INVERT(_qo, _qi) QUAT_INVERT(_qo, _qi) @@ -520,11 +541,11 @@ struct Int64Vect3 { } #define INT32_QUAT_WRAP_SHORTEST(q) { \ - if ((q).qi < 0) \ + if ((q).qi < 0) \ QUAT_EXPLEMENTARY(q,q); \ } -#define INT32_QUAT_NORMALISE(q) { \ +#define INT32_QUAT_NORMALIZE(q) { \ int32_t n; \ INT32_QUAT_NORM(n, q); \ (q).qi = (q).qi * QUAT1_BFP_OF_REAL(1) / n; \ @@ -557,6 +578,8 @@ struct Int64Vect3 { (_b2c).qz = ((_a2b).qi*(_a2c).qz - (_a2b).qx*(_a2c).qy + (_a2b).qy*(_a2c).qx - (_a2b).qz*(_a2c).qi)>>INT32_QUAT_FRAC; \ } + + #ifdef ALGEBRA_INT_USE_SLOW_FUNCTIONS #define INT32_QUAT_VMULT(v_out, q, v_in) { \ const int32_t qi2 = ((q).qi*(q).qi)>>INT32_QUAT_FRAC; \ @@ -793,7 +816,33 @@ struct Int64Vect3 { #define INT_RATES_ZERO(_e) RATES_ASSIGN(_e, 0, 0, 0) -#define INT32_RATES_OF_EULERS_DOT_321(_r, _e, _ed) { \ +#define INT_RATES_ADD_SCALED_VECT(_ro, _v, _s) { \ + _ro.p += _v.x * _s; \ + _ro.q += _v.y * _s; \ + _ro.r += _v.z * _s; \ + } + +#define INT_RATES_SDIV(_ro, _s, _ri) { \ + _ro.p = _ri.p / _s; \ + _ro.q = _ri.q / _s; \ + _ro.r = _ri.r / _s; \ + } + +#define INT_RATES_RSHIFT(_o, _i, _r) { \ + (_o).p = ((_i).p >> (_r)); \ + (_o).q = ((_i).q >> (_r)); \ + (_o).r = ((_i).r >> (_r)); \ + } + +#define INT_RATES_LSHIFT(_o, _i, _r) { \ + (_o).p = ((_i).p << (_r)); \ + (_o).q = ((_i).q << (_r)); \ + (_o).r = ((_i).r << (_r)); \ + } + + + +#define INT32_RATES_OF_EULERS_DOT_321(_r, _e, _ed) { \ \ int32_t sphi; \ PPRZ_ITRIG_SIN(sphi, (_e).phi); \ diff --git a/sw/airborne/mcu.c b/sw/airborne/mcu.c index e0853e4bab..1e8fee07d7 100644 --- a/sw/airborne/mcu.c +++ b/sw/airborne/mcu.c @@ -48,6 +48,9 @@ #ifdef USE_SPI #include "mcu_periph/spi.h" #endif +#ifdef USE_DAC +#include "mcu_periph/dac.h" +#endif #endif /* PERIPHERALS_AUTO_INIT */ void mcu_init(void) { @@ -98,6 +101,9 @@ void mcu_init(void) { #ifdef USE_SPI spi_init(); #endif +#ifdef USE_DAC + dac_init(); +#endif #endif /* PERIPHERALS_AUTO_INIT */ } diff --git a/sw/airborne/mcu_periph/dac.h b/sw/airborne/mcu_periph/dac.h new file mode 100644 index 0000000000..f1a7d23b41 --- /dev/null +++ b/sw/airborne/mcu_periph/dac.h @@ -0,0 +1,9 @@ +#ifndef MCU_PERIPH_DAC_H +#define MCU_PERIPH_DAC_H + +#include "mcu_periph/dac_arch.h" + +extern void dac_init(void); + + +#endif /* MCU_PERIPH_DAC_H */ diff --git a/sw/airborne/mcu_periph/i2c.h b/sw/airborne/mcu_periph/i2c.h index 3495bbafec..93bd0f77ba 100644 --- a/sw/airborne/mcu_periph/i2c.h +++ b/sw/airborne/mcu_periph/i2c.h @@ -43,7 +43,6 @@ struct i2c_transaction { uint8_t slave_addr; uint16_t len_r; uint8_t len_w; - bool_t stop_after_transmit; volatile uint8_t buf[I2C_BUF_LEN]; volatile enum I2CTransactionStatus status; }; @@ -136,40 +135,28 @@ extern bool_t i2c_idle(struct i2c_periph* p); extern bool_t i2c_submit(struct i2c_periph* p, struct i2c_transaction* t); #define I2CReceive(_p, _t, _s_addr, _len) { \ - _t.type = I2CTransRx; \ - _t.slave_addr = _s_addr; \ - _t.len_r = _len; \ - _t.len_w = 0; \ - _t.stop_after_transmit = TRUE; \ - i2c_submit(&(_p),&(_t)); \ - } + _t.type = I2CTransRx; \ + _t.slave_addr = _s_addr; \ + _t.len_r = _len; \ + _t.len_w = 0; \ + i2c_submit(&(_p),&(_t)); \ +} #define I2CTransmit(_p, _t, _s_addr, _len) { \ - _t.type = I2CTransTx; \ - _t.slave_addr = _s_addr; \ - _t.len_r = 0; \ - _t.len_w = _len; \ - _t.stop_after_transmit = TRUE; \ - i2c_submit(&(_p),&(_t)); \ - } + _t.type = I2CTransTx; \ + _t.slave_addr = _s_addr; \ + _t.len_r = 0; \ + _t.len_w = _len; \ + i2c_submit(&(_p),&(_t)); \ +} -#define I2CTransmitNoStop(_p, _t, _s_addr, _len) { \ - _t.type = I2CTransTx; \ - _t.slave_addr = _s_addr; \ - _t.len_r = 0; \ - _t.len_w = _len; \ - _t.stop_after_transmit = FALSE; \ - i2c_submit(&(_p),&(_t)); \ - } - -#define I2CTransceive(_p, _t, _s_addr, _len_w, _len_r) { \ - _t.type = I2CTransTxRx; \ - _t.slave_addr = _s_addr; \ - _t.len_r = _len_r; \ - _t.len_w = _len_w; \ - _t.stop_after_transmit = TRUE; \ - i2c_submit(&(_p),&(_t)); \ - } +#define I2CTransceive(_p, _t, _s_addr, _len_w, _len_r) { \ + _t.type = I2CTransTxRx; \ + _t.slave_addr = _s_addr; \ + _t.len_r = _len_r; \ + _t.len_w = _len_w; \ + i2c_submit(&(_p),&(_t)); \ +} #endif /* I2C_H */ diff --git a/sw/airborne/mcu_periph/uart.h b/sw/airborne/mcu_periph/uart.h index 32bc3588d2..848fcbad74 100644 --- a/sw/airborne/mcu_periph/uart.h +++ b/sw/airborne/mcu_periph/uart.h @@ -121,4 +121,24 @@ extern bool_t uart3_check_free_space( uint8_t len); #endif /* USE_UART3 */ +#ifdef USE_UART5 + +extern void uart5_init( void ); +extern void uart5_transmit( uint8_t data ); +extern bool_t uart5_check_free_space( uint8_t len); + +#define Uart5Init uart5_init +#define Uart5CheckFreeSpace(_x) uart5_check_free_space(_x) +#define Uart5Transmit(_x) uart5_transmit(_x) +#define Uart5SendMessage() {} + +#define UART5Init Uart5Init +#define UART5CheckFreeSpace Uart5CheckFreeSpace +#define UART5Transmit Uart5Transmit +#define UART5SendMessage Uart5SendMessage +#define UART5ChAvailable Uart5ChAvailable +#define UART5Getch Uart5Getch + +#endif /* USE_UART5 */ + #endif /* MCU_PERIPH_UART_H */ diff --git a/sw/airborne/modules/ins/ins_arduimu_basic.c b/sw/airborne/modules/ins/ins_arduimu_basic.c new file mode 100644 index 0000000000..1ff1c7151a --- /dev/null +++ b/sw/airborne/modules/ins/ins_arduimu_basic.c @@ -0,0 +1,147 @@ +/* +C Datei für die Einbindung eines ArduIMU's +Autoren@ZHAW: schmiemi + chaneren +*/ + + +#include +#include "modules/ins/ins_arduimu_basic.h" +//#include "firmwares/fixedwing/main_fbw.h" +#include "mcu_periph/i2c.h" + +// test +#include "estimator.h" + +// für das Senden von GPS-Daten an den ArduIMU +#include "gps.h" + +#define NB_DATA 9 + +#ifndef ARDUIMU_I2C_DEV +#define ARDUIMU_I2C_DEV i2c0 +#endif + +// Adresse des I2C Slaves: 0001 0110 letztes Bit ist für Read/Write +// einzugebende Adresse im ArduIMU ist 0000 1011 +//da ArduIMU das Read/Write Bit selber anfügt. +#define ArduIMU_SLAVE_ADDR 0x22 + +#ifndef DOWNLINK_DEVICE +#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE +#endif + +#include "mcu_periph/uart.h" +#include "messages.h" +#include "downlink.h" + +struct i2c_transaction ardu_gps_trans; +struct i2c_transaction ardu_ins_trans; + +static int16_t recievedData[NB_DATA]; + +struct FloatEulers arduimu_eulers; +struct FloatRates arduimu_rates; +struct FloatVect3 arduimu_accel; + +float ins_roll_neutral; +float ins_pitch_neutral; + +void ArduIMU_init( void ) { + FLOAT_EULERS_ZERO(arduimu_eulers); + FLOAT_RATES_ZERO(arduimu_rates); + FLOAT_VECT3_ZERO(arduimu_accel); + + ardu_ins_trans.status = I2CTransDone; + ardu_gps_trans.status = I2CTransDone; + + ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT; + ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT; +} + +#define FillBufWith32bit(_buf, _index, _value) { \ + _buf[_index] = (uint8_t) (_value); \ + _buf[_index+1] = (uint8_t) ((_value) >> 8); \ + _buf[_index+2] = (uint8_t) ((_value) >> 16); \ + _buf[_index+3] = (uint8_t) ((_value) >> 24); \ +} + +void ArduIMU_periodicGPS( void ) { + + if (ardu_gps_trans.status != I2CTransDone) { return; } + + FillBufWith32bit(ardu_gps_trans.buf, 0, (int32_t)gps_speed_3d); // speed 3D + FillBufWith32bit(ardu_gps_trans.buf, 4, (int32_t)gps_gspeed); // ground speed + FillBufWith32bit(ardu_gps_trans.buf, 8, (int32_t)gps_course); // course + ardu_gps_trans.buf[12] = gps_mode; // status gps fix + ardu_gps_trans.buf[13] = gps_status_flags; // status flags + I2CTransmit(ARDUIMU_I2C_DEV, ardu_gps_trans, ArduIMU_SLAVE_ADDR, 14); + +} + +void ArduIMU_periodic( void ) { + //Frequence defined in conf/modules/ins_arduimu.xml + + if (ardu_ins_trans.status == I2CTransDone) { + I2CReceive(ARDUIMU_I2C_DEV, ardu_ins_trans, ArduIMU_SLAVE_ADDR, NB_DATA*2); + } + +} + +#include "math/pprz_algebra_int.h" +/* + Buffer O: Roll + Buffer 1: Pitch + Buffer 2: Yaw + Buffer 3: Gyro X + Buffer 4: Gyro Y + Buffer 5: Gyro Z + Buffer 6: Accel X + Buffer 7: Accel Y + Buffer 8: Accel Z + */ + +void ArduIMU_event( void ) { + // Handle INS I2C event + if (ardu_ins_trans.status == I2CTransSuccess) { + // received data from I2C transaction + recievedData[0] = (ardu_ins_trans.buf[1]<<8) | ardu_ins_trans.buf[0]; + recievedData[1] = (ardu_ins_trans.buf[3]<<8) | ardu_ins_trans.buf[2]; + recievedData[2] = (ardu_ins_trans.buf[5]<<8) | ardu_ins_trans.buf[4]; + recievedData[3] = (ardu_ins_trans.buf[7]<<8) | ardu_ins_trans.buf[6]; + recievedData[4] = (ardu_ins_trans.buf[9]<<8) | ardu_ins_trans.buf[8]; + recievedData[5] = (ardu_ins_trans.buf[11]<<8) | ardu_ins_trans.buf[10]; + recievedData[6] = (ardu_ins_trans.buf[13]<<8) | ardu_ins_trans.buf[12]; + recievedData[7] = (ardu_ins_trans.buf[15]<<8) | ardu_ins_trans.buf[14]; + recievedData[8] = (ardu_ins_trans.buf[17]<<8) | ardu_ins_trans.buf[16]; + + // Update ArduIMU data + arduimu_eulers.phi = ANGLE_FLOAT_OF_BFP(recievedData[0]); + arduimu_eulers.theta = ANGLE_FLOAT_OF_BFP(recievedData[1]); + arduimu_eulers.psi = ANGLE_FLOAT_OF_BFP(recievedData[2]); + arduimu_rates.p = RATE_FLOAT_OF_BFP(recievedData[3]); + arduimu_rates.q = RATE_FLOAT_OF_BFP(recievedData[4]); + arduimu_rates.r = RATE_FLOAT_OF_BFP(recievedData[5]); + arduimu_accel.x = ACCEL_FLOAT_OF_BFP(recievedData[6]); + arduimu_accel.y = ACCEL_FLOAT_OF_BFP(recievedData[7]); + arduimu_accel.z = ACCEL_FLOAT_OF_BFP(recievedData[8]); + + // Update estimator + estimator_phi = arduimu_eulers.phi - ins_roll_neutral; + estimator_theta = arduimu_eulers.theta - ins_pitch_neutral; + estimator_p = arduimu_rates.p; + ardu_ins_trans.status = I2CTransDone; + + RunOnceEvery(15, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, &arduimu_eulers.phi, &arduimu_eulers.theta, &arduimu_eulers.psi)); + RunOnceEvery(15, DOWNLINK_SEND_IMU_GYRO(DefaultChannel, &arduimu_rates.p, &arduimu_rates.q, &arduimu_rates.r)); + RunOnceEvery(15, DOWNLINK_SEND_IMU_ACCEL(DefaultChannel, &arduimu_accel.x, &arduimu_accel.y, &arduimu_accel.z)); + } + else if (ardu_ins_trans.status == I2CTransFailed) { + ardu_ins_trans.status = I2CTransDone; + } + // Handle GPS I2C event + if (ardu_gps_trans.status == I2CTransSuccess || ardu_gps_trans.status == I2CTransFailed) { + ardu_gps_trans.status = I2CTransDone; + } +} + diff --git a/sw/airborne/modules/ins/ins_arduimu_basic.h b/sw/airborne/modules/ins/ins_arduimu_basic.h new file mode 100644 index 0000000000..3e64cf3125 --- /dev/null +++ b/sw/airborne/modules/ins/ins_arduimu_basic.h @@ -0,0 +1,19 @@ +#ifndef ArduIMU_H +#define ArduIMU_H + +#include +#include "math/pprz_algebra_float.h" + +extern struct FloatEulers arduimu_eulers; +extern struct FloatRates arduimu_rates; +extern struct FloatVect3 arduimu_accel; + +extern float ins_roll_neutral; +extern float ins_pitch_neutral; + +void ArduIMU_init( void ); +void ArduIMU_periodic( void ); +void ArduIMU_periodicGPS( void ); +void ArduIMU_event( void ); + +#endif // ArduIMU_H diff --git a/sw/airborne/modules/sensors/pressure_board_navarro.c b/sw/airborne/modules/sensors/pressure_board_navarro.c index 987dc09f44..63e57f2d25 100644 --- a/sw/airborne/modules/sensors/pressure_board_navarro.c +++ b/sw/airborne/modules/sensors/pressure_board_navarro.c @@ -46,6 +46,15 @@ /* Weight for offset IIR filter */ #define PBN_OFFSET_FILTER 7 +/* Quadratic scale factor for airspeed */ +#ifndef PBN_AIRSPEED_SCALE +#define PBN_AIRSPEED_SCALE (1./0.54) +#endif + +/* Linear scale factor for altitude */ +#ifndef PBN_ALTITUDE_SCALE +#define PBN_ALTITUDE_SCALE 0.32 +#endif // Global variables uint16_t altitude_adc; @@ -61,6 +70,7 @@ uint16_t altitude_offset; uint16_t airspeed_offset; float pbn_altitude; float pbn_airspeed; +float airspeed_filter; uint16_t startup_delay; void pbn_init( void ) { @@ -73,6 +83,7 @@ void pbn_init( void ) { offset_cnt = OFFSET_NBSAMPLES_AVRG; pbn_airspeed = 0.; pbn_altitude = 0.; + airspeed_filter = PBN_OFFSET_FILTER; } @@ -106,20 +117,31 @@ void pbn_read_event( void ) { if (offset_cnt > 0) { // IIR filter to compute an initial offset +#ifndef PBN_AIRSPEED_OFFSET airspeed_offset = (PBN_OFFSET_FILTER * airspeed_offset + airspeed_adc) / (PBN_OFFSET_FILTER + 1); +#else + airspeed_offset = PBN_AIRSPEED_OFFSET; +#endif +#ifndef PBN_ALTITUDE_OFFSET altitude_offset = (PBN_OFFSET_FILTER * altitude_offset + altitude_adc) / (PBN_OFFSET_FILTER + 1); +#else + altitude_offset = PBN_ALTITUDE_OFFSET; +#endif // decrease init counter --offset_cnt; } else { // Compute airspeed and altitude - pbn_airspeed = (-4.45 + sqrtf(19.84-0.57*(float)(airspeed_offset-airspeed_adc)))/0.28; - pbn_altitude = 0.32*(float)(altitude_adc-altitude_offset); + //pbn_airspeed = (-4.45 + sqrtf(19.84-0.57*(float)(airspeed_offset-airspeed_adc)))/0.28; + uint16_t diff = Max(airspeed_adc-airspeed_offset, 0); + float tmp_airspeed = sqrtf((float)diff * PBN_AIRSPEED_SCALE); + pbn_altitude = PBN_ALTITUDE_SCALE*(float)(altitude_adc-altitude_offset); - //estimator_airspeed = (7*estimator_airspeed + pbn_airspeed ) / 8; - //estimator_airspeed = Max(estimator_airspeed, 0.); - //EstimatorSetAirspeed(pbn_airspeed); + pbn_airspeed = (airspeed_filter*pbn_airspeed + tmp_airspeed) / (airspeed_filter + 1.); +#ifdef USE_AIRSPEED + EstimatorSetAirspeed(pbn_airspeed); +#endif //alt_kalman(pbn_altitude); } diff --git a/sw/airborne/modules/sensors/pressure_board_navarro.h b/sw/airborne/modules/sensors/pressure_board_navarro.h index 2bd4edba89..5b1fe68bff 100644 --- a/sw/airborne/modules/sensors/pressure_board_navarro.h +++ b/sw/airborne/modules/sensors/pressure_board_navarro.h @@ -43,6 +43,7 @@ extern uint16_t airspeed_adc; extern uint16_t altitude_offset; extern uint16_t airspeed_offset; extern float pbn_altitude, pbn_airspeed; +extern float airspeed_filter; extern bool_t data_valid; extern struct i2c_transaction pbn_trans; diff --git a/sw/airborne/modules/sonar/sonar_maxbotix_booz.c b/sw/airborne/modules/sonar/sonar_maxbotix.c similarity index 81% rename from sw/airborne/modules/sonar/sonar_maxbotix_booz.c rename to sw/airborne/modules/sonar/sonar_maxbotix.c index fad7db7e8d..c339212cea 100644 --- a/sw/airborne/modules/sonar/sonar_maxbotix_booz.c +++ b/sw/airborne/modules/sonar/sonar_maxbotix.c @@ -1,5 +1,4 @@ /* - * $Id: demo_module.c 3079 2009-03-11 16:55:42Z gautier $ * * Copyright (C) 2010 Gautier Hattenberger * @@ -22,22 +21,25 @@ * */ -#include "sonar_maxbotix_booz.h" -#include "booz2_analog.h" +#include "modules/sonar/sonar_maxbotix.h" +#include "mcu_periph/adc.h" uint16_t sonar_meas; bool_t sonar_data_available; +static struct adc_buf sonar_adc; + void maxbotix_init(void) { sonar_meas = 0; sonar_data_available = FALSE; + + adc_buf_channel(ADC_CHANNEL_SONAR, &sonar_adc, DEFAULT_AV_NB_SAMPLE); } /** Read ADC value to update sonar measurement */ void maxbotix_read(void) { - booz2_analog_extra_adc_read(); - sonar_meas = booz2_adc_1; + sonar_meas = sonar_adc.sum / sonar_adc.av_nb_sample; sonar_data_available = TRUE; } diff --git a/sw/airborne/modules/sonar/sonar_maxbotix_booz.h b/sw/airborne/modules/sonar/sonar_maxbotix.h similarity index 84% rename from sw/airborne/modules/sonar/sonar_maxbotix_booz.h rename to sw/airborne/modules/sonar/sonar_maxbotix.h index 596a8e5a75..b692f0889a 100644 --- a/sw/airborne/modules/sonar/sonar_maxbotix_booz.h +++ b/sw/airborne/modules/sonar/sonar_maxbotix.h @@ -1,5 +1,4 @@ /* - * $Id: demo_module.h 3079 2009-03-11 16:55:42Z gautier $ * * Copyright (C) 2010 Gautier Hattenberger * @@ -22,9 +21,9 @@ * */ -/** \file sonar_maxbotix_booz.h +/** \file sonar_maxbotix.h * - * simple driver to deal with one maxbotix sensor on booz AP + * simple driver to deal with one maxbotix sensor */ #ifndef SONAR_MAXBOTIX_BOOZ_H @@ -39,7 +38,7 @@ extern bool_t sonar_data_available; extern void maxbotix_init(void); extern void maxbotix_read(void); -#include "subsystems/ins.h" // needed because ins is not a module +//#include "subsystems/ins.h" // needed because ins is not a module #define SonarEvent(_handler) { \ if (sonar_data_available) { \ diff --git a/sw/airborne/peripherals/ami601.c b/sw/airborne/peripherals/ami601.c index df98efaf31..1f7f3d4c76 100644 --- a/sw/airborne/peripherals/ami601.c +++ b/sw/airborne/peripherals/ami601.c @@ -17,7 +17,6 @@ void ami601_init( void ) { } ami601_i2c_trans.status = I2CTransSuccess; ami601_i2c_trans.slave_addr = AMI601_SLAVE_ADDR; - ami601_i2c_trans.stop_after_transmit = TRUE; ami601_nb_err = 0; ami601_status = AMI601_IDLE; diff --git a/sw/airborne/peripherals/hmc5843.c b/sw/airborne/peripherals/hmc5843.c index 71050c442c..f02c148312 100644 --- a/sw/airborne/peripherals/hmc5843.c +++ b/sw/airborne/peripherals/hmc5843.c @@ -11,7 +11,6 @@ void hmc5843_init(void) { hmc5843.i2c_trans.status = I2CTransSuccess; hmc5843.i2c_trans.slave_addr = HMC5843_ADDR; - hmc5843.i2c_trans.stop_after_transmit = TRUE; hmc5843_arch_init(); } diff --git a/sw/airborne/subsystems/ahrs/ahrs_aligner.c b/sw/airborne/subsystems/ahrs/ahrs_aligner.c index 935eb98970..2f80e57298 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_aligner.c +++ b/sw/airborne/subsystems/ahrs/ahrs_aligner.c @@ -51,7 +51,9 @@ void ahrs_aligner_init(void) { #ifndef LOW_NOISE_THRESHOLD #define LOW_NOISE_THRESHOLD 90000 #endif +#ifndef LOW_NOISE_TIME #define LOW_NOISE_TIME 5 +#endif void ahrs_aligner_run(void) { diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_rmat.c b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_rmat.c index 1b03e931f9..6eed92bc4d 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_rmat.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_rmat.c @@ -31,12 +31,15 @@ #include "math/pprz_simple_matrix.h" #include "generated/airframe.h" -#include #include "../../test/pprz_algebra_print.h" +void ahrs_update_mag_full(void); +void ahrs_update_mag_2d(void); +void ahrs_update_mag_2d_dumb(void); static inline void compute_imu_quat_and_euler_from_rmat(void); +static inline void compute_imu_rmat_and_euler_from_quat(void); static inline void compute_body_orientation_and_rates(void); @@ -45,25 +48,23 @@ struct AhrsFloatCmplRmat ahrs_impl; void ahrs_init(void) { ahrs_float.status = AHRS_UNINIT; - /* - * Initialises our IMU alignement variables - * This should probably done in the IMU code instead - */ + /* Initialises IMU alignement */ struct FloatEulers body_to_imu_euler = {IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI}; FLOAT_QUAT_OF_EULERS(ahrs_impl.body_to_imu_quat, body_to_imu_euler); FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler); - - /* set ltp_to_body to zero */ + + /* Set ltp_to_body to zero */ FLOAT_QUAT_ZERO(ahrs_float.ltp_to_body_quat); FLOAT_EULERS_ZERO(ahrs_float.ltp_to_body_euler); FLOAT_RMAT_ZERO(ahrs_float.ltp_to_body_rmat); FLOAT_RATES_ZERO(ahrs_float.body_rate); - /* set ltp_to_imu so that body is zero */ + /* Set ltp_to_imu so that body is zero */ QUAT_COPY(ahrs_float.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat); RMAT_COPY(ahrs_float.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat); EULERS_COPY(ahrs_float.ltp_to_imu_euler, body_to_imu_euler); + FLOAT_RATES_ZERO(ahrs_float.imu_rate); } @@ -95,30 +96,32 @@ void ahrs_propagate(void) { struct FloatRates gyro_float; RATES_FLOAT_OF_BFP(gyro_float, imu.gyro_prev); /* unbias measurement */ - RATES_DIFF(ahrs_float.imu_rate, gyro_float, ahrs_impl.gyro_bias); - const float dt = 1./512.; + RATES_SUB(gyro_float, ahrs_impl.gyro_bias); + +#ifdef AHRS_PROPAGATE_LOW_PASS_RATES + const float alpha = 0.1; + FLOAT_RATES_LIN_CMB(ahrs_float.imu_rate, ahrs_float.imu_rate, (1.-alpha), gyro_float, alpha); +#else + RATES_COPY(ahrs_float.imu_rate,gyro_float); +#endif + /* add correction */ struct FloatRates omega; - RATES_SUM(omega, ahrs_float.imu_rate, ahrs_impl.rate_correction); - // DISPLAY_FLOAT_RATES("omega ", omega); + RATES_SUM(omega, gyro_float, ahrs_impl.rate_correction); /* and zeros it */ FLOAT_RATES_ZERO(ahrs_impl.rate_correction); - /* first order integration of rotation matrix */ - struct FloatRMat exp_omega_dt = { - { 1. , dt*omega.r, -dt*omega.q, - -dt*omega.r, 1. , dt*omega.p, - dt*omega.q, -dt*omega.p, 1. }}; - struct FloatRMat R_tdt; - FLOAT_RMAT_COMP(R_tdt, ahrs_float.ltp_to_imu_rmat, exp_omega_dt); - memcpy(&ahrs_float.ltp_to_imu_rmat, &R_tdt, sizeof(R_tdt)); - + const float dt = 1./AHRS_PROPAGATE_FREQUENCY; +#ifdef AHRS_PROPAGATE_RMAT + FLOAT_RMAT_INTEGRATE_FI(ahrs_float.ltp_to_imu_rmat, omega, dt ); float_rmat_reorthogonalize(&ahrs_float.ltp_to_imu_rmat); - // struct FloatRMat test; - // FLOAT_RMAT_COMP_INV(test, ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_rmat); - // DISPLAY_FLOAT_RMAT("foo", test); - compute_imu_quat_and_euler_from_rmat(); +#endif +#ifdef AHRS_PROPAGATE_QUAT + FLOAT_QUAT_INTEGRATE(ahrs_float.ltp_to_imu_quat, omega, dt); + FLOAT_QUAT_NORMALIZE(ahrs_float.ltp_to_imu_quat); + compute_imu_rmat_and_euler_from_quat(); +#endif compute_body_orientation_and_rates(); } @@ -127,23 +130,102 @@ void ahrs_update_accel(void) { struct FloatVect3 accel_float; ACCELS_FLOAT_OF_BFP(accel_float, imu.accel); - struct FloatVect3* r2 = (struct FloatVect3*)(&RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 2,0)); + +#ifdef AHRS_GRAVITY_UPDATE_COORDINATED_TURN + float v = FLOAT_VECT3_NORM(ahrs_impl.est_ltp_speed); + accel_float.y -= v * ahrs_float.imu_rate.r; + accel_float.z -= -v * ahrs_float.imu_rate.q; +#endif + + struct FloatVect3 c2 = { RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 0,2), + RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 1,2), + RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 2,2)}; struct FloatVect3 residual; - FLOAT_VECT3_CROSS_PRODUCT(residual, accel_float, (*r2)); + FLOAT_VECT3_CROSS_PRODUCT(residual, accel_float, c2); +#ifdef AHRS_GRAVITY_UPDATE_NORM_HEURISTIC /* heuristic on acceleration norm */ const float acc_norm = FLOAT_VECT3_NORM(accel_float); - const float weight = Chop(1.-2*fabs(1-acc_norm/9.81), 0., 1.); - //const float weight = 1.; + const float weight = Chop(1.-6*fabs((9.81-acc_norm)/9.81), 0., 1.); +#else + const float weight = 1.; +#endif /* compute correction */ - const float gravity_rate_update_gain = 5e-2; + const float gravity_rate_update_gain = -5e-2; // -5e-2 FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual, weight*gravity_rate_update_gain); - const float gravity_bias_update_gain = -5e-6; +#if 1 + const float gravity_bias_update_gain = 1e-5; // -5e-6 FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual, weight*gravity_bias_update_gain); +#else + const float alpha = 5e-4; + FLOAT_RATES_SCALE(ahrs_impl.gyro_bias, 1.-alpha); + FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual, alpha); +#endif /* FIXME: saturate bias */ } + void ahrs_update_mag(void) { +#ifdef AHRS_MAG_UPDATE_YAW_ONLY + ahrs_update_mag_2d(); + // ahrs_update_mag_2d_dumb(); +#else + ahrs_update_mag_full(); +#endif +} + +void ahrs_update_mag_full(void) { + + const struct FloatVect3 expected_ltp = {AHRS_H_X, AHRS_H_Y, AHRS_H_Z}; + struct FloatVect3 expected_imu; + FLOAT_RMAT_VECT3_MUL(expected_imu, ahrs_float.ltp_to_imu_rmat, expected_ltp); + + struct FloatVect3 measured_imu; + MAGS_FLOAT_OF_BFP(measured_imu, imu.mag); + + struct FloatVect3 residual_imu; + FLOAT_VECT3_CROSS_PRODUCT(residual_imu, measured_imu, expected_imu); + // DISPLAY_FLOAT_VECT3("# expected", expected_imu); + // DISPLAY_FLOAT_VECT3("# measured", measured_imu); + // DISPLAY_FLOAT_VECT3("# residual", residual); + + const float mag_rate_update_gain = 2.5; + FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual_imu, mag_rate_update_gain); + + const float mag_bias_update_gain = -2.5e-3; + FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual_imu, mag_bias_update_gain); + +} + +void ahrs_update_mag_2d(void) { + + const struct FloatVect2 expected_ltp = {AHRS_H_X, AHRS_H_Y}; + + struct FloatVect3 measured_imu; + MAGS_FLOAT_OF_BFP(measured_imu, imu.mag); + struct FloatVect3 measured_ltp; + FLOAT_RMAT_VECT3_TRANSP_MUL(measured_ltp, ahrs_float.ltp_to_imu_rmat, measured_imu); + + const struct FloatVect3 residual_ltp = + { 0, + 0, + measured_ltp.x * expected_ltp.y - measured_ltp.y * expected_ltp.x }; + + // printf("res : %f\n", residual_ltp.z); + + struct FloatVect3 residual_imu; + FLOAT_RMAT_VECT3_MUL(residual_imu, ahrs_float.ltp_to_imu_rmat, residual_ltp); + + const float mag_rate_update_gain = 2.5; + FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual_imu, mag_rate_update_gain); + + const float mag_bias_update_gain = -2.5e-3; + FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual_imu, mag_bias_update_gain); + +} + + +void ahrs_update_mag_full_2d_dumb(void) { /* project mag on local tangeant plane */ struct FloatVect3 magf; @@ -156,39 +238,19 @@ void ahrs_update_mag(void) { const float me = 0. * magf.x + cphi *magf.y - sphi *magf.z; const float res_norm = -RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 0,0)*me + RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 1,0)*mn; - printf("res norm %f\n", res_norm); struct FloatVect3* r2 = (struct FloatVect3*)(&RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 2,0)); - DISPLAY_FLOAT_VECT3("r2", (*r2)); const float mag_rate_update_gain = 2.5; FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, (*r2), (mag_rate_update_gain*res_norm)); - DISPLAY_FLOAT_RATES("corr", ahrs_impl.rate_correction); const float mag_bias_update_gain = -2.5e-4; FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, (*r2), (mag_bias_update_gain*res_norm)); - /* FIXME: saturate bias */ } -void ahrs_update_mag2(void) { - const struct FloatVect3 expected_ltp = {AHRS_H_X, AHRS_H_Y, AHRS_H_Z}; - struct FloatVect3 expected_imu; - FLOAT_RMAT_VECT3_MUL(expected_imu, ahrs_float.ltp_to_imu_rmat, expected_ltp); - struct FloatVect3 measured_imu; - MAGS_FLOAT_OF_BFP(measured_imu, imu.mag); - struct FloatVect3 residual; - FLOAT_VECT3_CROSS_PRODUCT(residual, measured_imu, expected_imu); - // FLOAT_VECT3_DIFF(residual, expected_imu, measured_imu); - DISPLAY_FLOAT_VECT3(" expected", expected_imu); - DISPLAY_FLOAT_VECT3(" measured", measured_imu); - DISPLAY_FLOAT_VECT3("residual", residual); - const float mag_rate_update_gain = 2.5; - FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual, mag_rate_update_gain); - -} /* * Compute ltp to imu rotation in euler angles and quaternion representations @@ -199,6 +261,14 @@ static inline void compute_imu_quat_and_euler_from_rmat(void) { FLOAT_EULERS_OF_RMAT(ahrs_float.ltp_to_imu_euler, ahrs_float.ltp_to_imu_rmat); } + +static inline void compute_imu_rmat_and_euler_from_quat(void) { + FLOAT_RMAT_OF_QUAT(ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_quat); + FLOAT_EULERS_OF_RMAT(ahrs_float.ltp_to_imu_euler, ahrs_float.ltp_to_imu_rmat); +} + + + /* * Compute body orientation and rates from imu orientation and rates */ diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_rmat.h b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_rmat.h index ba5aaa553c..ecfa367ccf 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_rmat.h +++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_rmat.h @@ -27,6 +27,9 @@ struct AhrsFloatCmplRmat { struct FloatRates gyro_bias; struct FloatRates rate_correction; + /* for gravity correction during coordinated turns */ + struct FloatVect3 est_ltp_speed; + /* Holds float version of IMU alignement in order to be able to run against the fixed point diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_lkf.c b/sw/airborne/subsystems/ahrs/ahrs_float_lkf.c index 50e2c6f6df..f1e781a36a 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_lkf.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_lkf.c @@ -315,7 +315,7 @@ void ahrs_propagate(void) { bafl_qnorm = FLOAT_QUAT_NORM(bafl_quat); //TODO check if broot force normalization is good, use lagrange normalization? - FLOAT_QUAT_NORMALISE(bafl_quat); + FLOAT_QUAT_NORMALIZE(bafl_quat); /* diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_utils.h b/sw/airborne/subsystems/ahrs/ahrs_float_utils.h index 679f0774b5..03374c8149 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_utils.h +++ b/sw/airborne/subsystems/ahrs/ahrs_float_utils.h @@ -8,6 +8,7 @@ static inline void ahrs_float_get_euler_from_accel_mag(struct FloatEulers* e, st const float phi = atan2f(-accelf.y, -accelf.z); const float cphi = cosf(phi); const float theta = atan2f(cphi*accelf.x, -accelf.z); + /* get psi from magnetometer */ /* project mag on local tangeant plane */ struct FloatVect3 magf; @@ -17,8 +18,10 @@ static inline void ahrs_float_get_euler_from_accel_mag(struct FloatEulers* e, st const float stheta = sinf(theta); const float mn = ctheta * magf.x + sphi*stheta*magf.y + cphi*stheta*magf.z; const float me = 0. * magf.x + cphi *magf.y - sphi *magf.z; - const float psi = -atan2f(me, mn); + float psi = -atan2f(me, mn) + atan2(AHRS_H_Y, AHRS_H_X); + if (psi > M_PI) psi -= 2.*M_PI; if (psi < -M_PI) psi+= 2.*M_PI; EULERS_ASSIGN(*e, phi, theta, psi); + } #endif /* AHRS_FLOAT_UTILS_H */ diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.c new file mode 100644 index 0000000000..a74f631cb3 --- /dev/null +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.c @@ -0,0 +1,322 @@ +/* + * $Id$ + * + * Copyright (C) 2008-2011 The Paparazzi Team + * + * 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. + */ + + +// TODO +// +// gravity heuristic +// gps based gravity correction +// gps update for yaw on fixed wing ? +// + +#include "subsystems/ahrs/ahrs_int_cmpl.h" +#include "subsystems/ahrs/ahrs_aligner.h" +#include "subsystems/ahrs/ahrs_int_utils.h" + +#include "subsystems/imu.h" +#include "math/pprz_trig_int.h" +#include "math/pprz_algebra_int.h" + +#include "generated/airframe.h" + +//#include "../../test/pprz_algebra_print.h" + +static inline void ahrs_update_mag_full(void); +static inline void ahrs_update_mag_2d(void); + + +/* in place quaternion first order integration with constante rotational velocity */ +/* */ +#define INT32_QUAT_INTEGRATE_FI(_q, _hr, _omega, _f) { \ + _hr.qi += -_omega.p*_q.qx - _omega.q*_q.qy - _omega.r*_q.qz; \ + _hr.qx += _omega.p*_q.qi + _omega.r*_q.qy - _omega.q*_q.qz; \ + _hr.qy += _omega.q*_q.qi - _omega.r*_q.qx + _omega.p*_q.qz; \ + _hr.qz += _omega.r*_q.qi + _omega.q*_q.qx - _omega.p*_q.qy; \ + \ + ldiv_t _div = ldiv(_hr.qi, ((1<>INT32_ACCEL_FRAC) * ahrs.imu_rate.r) + >>(INT32_SPEED_FRAC+INT32_RATE_FRAC-INT32_ACCEL_FRAC-INT32_ACCEL_FRAC), + -((ahrs_impl.ltp_vel_norm>>INT32_ACCEL_FRAC) * ahrs.imu_rate.q) + >>(INT32_SPEED_FRAC+INT32_RATE_FRAC-INT32_ACCEL_FRAC-INT32_ACCEL_FRAC) + }; + struct Int32Vect3 corrected_gravity; + VECT3_DIFF(corrected_gravity, imu.accel, Xdd_imu); + INT32_VECT3_CROSS_PRODUCT(residual, corrected_gravity, c2); +#else + INT32_VECT3_CROSS_PRODUCT(residual, imu.accel, c2); +#endif + + // residual FRAC : ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24 + // rate_correction FRAC = RATE_FRAC = 12 + // 2^12 / 2^24 * 5e-2 = 1/81920 + ahrs_impl.rate_correction.p += -residual.x/82000; + ahrs_impl.rate_correction.q += -residual.y/82000; + ahrs_impl.rate_correction.r += -residual.z/82000; + + // residual FRAC = ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24 + // high_rez_bias = RATE_FRAC+28 = 40 + // 2^40 / 2^24 * 5e-6 = 1/3.05 + + // ahrs_impl.high_rez_bias.p += residual.x*3; + // ahrs_impl.high_rez_bias.q += residual.y*3; + // ahrs_impl.high_rez_bias.r += residual.z*3; + + ahrs_impl.high_rez_bias.p += residual.x; + ahrs_impl.high_rez_bias.q += residual.y; + ahrs_impl.high_rez_bias.r += residual.z; + + + /* */ + INT_RATES_RSHIFT(ahrs_impl.gyro_bias, ahrs_impl.high_rez_bias, 28); + + +} + +void ahrs_update_mag(void) { +#ifdef AHRS_MAG_UPDATE_YAW_ONLY + ahrs_update_mag_2d(); +#else + ahrs_update_mag_full(); +#endif +} + + +static inline void ahrs_update_mag_full(void) { + const struct Int32Vect3 expected_ltp = {MAG_BFP_OF_REAL(AHRS_H_X), + MAG_BFP_OF_REAL(AHRS_H_Y), + MAG_BFP_OF_REAL(AHRS_H_Z)}; + struct Int32Vect3 expected_imu; + INT32_RMAT_VMULT(expected_imu, ahrs.ltp_to_imu_rmat, expected_ltp); + + struct Int32Vect3 residual; + INT32_VECT3_CROSS_PRODUCT(residual, imu.mag, expected_imu); + + ahrs_impl.rate_correction.p += residual.x/32/16; + ahrs_impl.rate_correction.q += residual.y/32/16; + ahrs_impl.rate_correction.r += residual.z/32/16; + + + ahrs_impl.high_rez_bias.p += -residual.x/32*1024; + ahrs_impl.high_rez_bias.q += -residual.y/32*1024; + ahrs_impl.high_rez_bias.r += -residual.z/32*1024; + + + INT_RATES_RSHIFT(ahrs_impl.gyro_bias, ahrs_impl.high_rez_bias, 28); + +} + + +static inline void ahrs_update_mag_2d(void) { + + const struct Int32Vect2 expected_ltp = {MAG_BFP_OF_REAL(AHRS_H_X), + MAG_BFP_OF_REAL(AHRS_H_Y)}; + + struct Int32Vect3 measured_ltp; + INT32_RMAT_TRANSP_VMULT(measured_ltp, ahrs.ltp_to_imu_rmat, imu.mag); + + struct Int32Vect3 residual_ltp = + { 0, + 0, + (measured_ltp.x * expected_ltp.y - measured_ltp.y * expected_ltp.x)/(1<<5)}; + + struct Int32Vect3 residual_imu; + INT32_RMAT_VMULT(residual_imu, ahrs.ltp_to_imu_rmat, residual_ltp); + + // residual_ltp FRAC = 2 * MAG_FRAC = 22 + // rate_correction FRAC = RATE_FRAC = 12 + // 2^12 / 2^22 * 2.5 = 1/410 + + // ahrs_impl.rate_correction.p += residual_imu.x*(1<<5)/410; + // ahrs_impl.rate_correction.q += residual_imu.y*(1<<5)/410; + // ahrs_impl.rate_correction.r += residual_imu.z*(1<<5)/410; + + ahrs_impl.rate_correction.p += residual_imu.x/16; + ahrs_impl.rate_correction.q += residual_imu.y/16; + ahrs_impl.rate_correction.r += residual_imu.z/16; + + + // residual_ltp FRAC = 2 * MAG_FRAC = 22 + // high_rez_bias = RATE_FRAC+28 = 40 + // 2^40 / 2^22 * 2.5e-3 = 655 + + // ahrs_impl.high_rez_bias.p -= residual_imu.x*(1<<5)*655; + // ahrs_impl.high_rez_bias.q -= residual_imu.y*(1<<5)*655; + // ahrs_impl.high_rez_bias.r -= residual_imu.z*(1<<5)*655; + + ahrs_impl.high_rez_bias.p -= residual_imu.x*1024; + ahrs_impl.high_rez_bias.q -= residual_imu.y*1024; + ahrs_impl.high_rez_bias.r -= residual_imu.z*1024; + + + INT_RATES_RSHIFT(ahrs_impl.gyro_bias, ahrs_impl.high_rez_bias, 28); + +} + + +/* Compute ltp to imu rotation in quaternion and rotation matrice representation + from the euler angle representation */ +__attribute__ ((always_inline)) static inline void compute_imu_quat_and_rmat_from_euler(void) { + + /* Compute LTP to IMU quaternion */ + INT32_QUAT_OF_EULERS(ahrs.ltp_to_imu_quat, ahrs.ltp_to_imu_euler); + /* Compute LTP to IMU rotation matrix */ + INT32_RMAT_OF_EULERS(ahrs.ltp_to_imu_rmat, ahrs.ltp_to_imu_euler); + +} + +/* Compute ltp to imu rotation in euler angles and rotation matrice representation + from the quaternion representation */ +__attribute__ ((always_inline)) static inline void compute_imu_euler_and_rmat_from_quat(void) { + + /* Compute LTP to IMU euler */ + INT32_EULERS_OF_QUAT(ahrs.ltp_to_imu_euler, ahrs.ltp_to_imu_quat); + /* Compute LTP to IMU rotation matrix */ + INT32_RMAT_OF_QUAT(ahrs.ltp_to_imu_rmat, ahrs.ltp_to_imu_quat); + +} + +__attribute__ ((always_inline)) static inline void compute_body_orientation(void) { + + /* Compute LTP to BODY quaternion */ + INT32_QUAT_COMP_INV(ahrs.ltp_to_body_quat, ahrs.ltp_to_imu_quat, imu.body_to_imu_quat); + /* Compute LTP to BODY rotation matrix */ + INT32_RMAT_COMP_INV(ahrs.ltp_to_body_rmat, ahrs.ltp_to_imu_rmat, imu.body_to_imu_rmat); + /* compute LTP to BODY eulers */ + INT32_EULERS_OF_RMAT(ahrs.ltp_to_body_euler, ahrs.ltp_to_body_rmat); + /* compute body rates */ + INT32_RMAT_TRANSP_RATEMULT(ahrs.body_rate, imu.body_to_imu_rmat, ahrs.imu_rate); + +} diff --git a/sw/airborne/booz/booz2_analog.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.h similarity index 63% rename from sw/airborne/booz/booz2_analog.c rename to sw/airborne/subsystems/ahrs/ahrs_int_cmpl.h index 2e0f153b78..f1f833b656 100644 --- a/sw/airborne/booz/booz2_analog.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.h @@ -1,7 +1,7 @@ /* * $Id$ * - * Copyright (C) 2008-2009 Antoine Drouin + * Copyright (C) 2011 The Paparazzi Team * * This file is part of paparazzi. * @@ -21,29 +21,23 @@ * Boston, MA 02111-1307, USA. */ -#include "booz2_analog.h" +#ifndef AHRS_INT_CMPL_H +#define AHRS_INT_CMPL_H +#include "subsystems/ahrs.h" #include "std.h" +#include "math/pprz_algebra_int.h" -// battery on AD0.3 on P0.30 -// baro on AD0.1 on P0.28 - -#define CHAN_BAT 3 -#define CHAN_BARO 1 - - -void booz2_analog_init( void ) { - - booz2_analog_init_hw(); - -} - -#ifdef USE_EXTRA_ADC -// Read manually baro (100Hz) and bat (10Hz) -void booz2_analog_periodic( void ) { - // baro - RunOnceEvery(5,booz2_analog_baro_read()); - // bat - RunOnceEvery(50,booz2_analog_bat_read()); -} +struct AhrsIntCmpl { + struct Int32Rates gyro_bias; + struct Int32Rates rate_correction; + struct Int64Quat high_rez_quat; + struct Int64Rates high_rez_bias; +#ifdef AHRS_GRAVITY_UPDATE_COORDINATED_TURN + int32_t ltp_vel_norm; #endif +}; + +extern struct AhrsIntCmpl ahrs_impl; + +#endif /* AHRS_INT_CMPL_H */ diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_utils.h b/sw/airborne/subsystems/ahrs/ahrs_int_utils.h new file mode 100644 index 0000000000..84918fcc62 --- /dev/null +++ b/sw/airborne/subsystems/ahrs/ahrs_int_utils.h @@ -0,0 +1,47 @@ +#ifndef AHRS_INT_UTILS_H +#define AHRS_INT_UTILS_H + +//#include "../../test/pprz_algebra_print.h" +#include "math/pprz_algebra_int.h" + +#include "generated/airframe.h" + +static inline void ahrs_int_get_euler_from_accel_mag(struct Int32Eulers* e, struct Int32Vect3* accel, struct Int32Vect3* mag) { + // DISPLAY_INT32_VECT3("# accel", (*accel)); + const float fphi = atan2f(-accel->y, -accel->z); + // printf("# atan float %f\n", DegOfRad(fphi)); + e->phi = ANGLE_BFP_OF_REAL(fphi); + + int32_t cphi; + PPRZ_ITRIG_COS(cphi, e->phi); + int32_t cphi_ax = -INT_MULT_RSHIFT(cphi, accel->x, INT32_TRIG_FRAC); + const float ftheta = atan2f(-cphi_ax, -accel->z); + e->theta = ANGLE_BFP_OF_REAL(ftheta); + + int32_t sphi; + PPRZ_ITRIG_SIN(sphi, e->phi); + int32_t stheta; + PPRZ_ITRIG_SIN(stheta, e->theta); + int32_t ctheta; + PPRZ_ITRIG_COS(ctheta, e->theta); + + int32_t sphi_stheta = (sphi*stheta)>>INT32_TRIG_FRAC; + int32_t cphi_stheta = (cphi*stheta)>>INT32_TRIG_FRAC; + //int32_t sphi_ctheta = (sphi*ctheta)>>INT32_TRIG_FRAC; + //int32_t cphi_ctheta = (cphi*ctheta)>>INT32_TRIG_FRAC; + + const int32_t mn = ctheta * mag->x + sphi_stheta * mag->y + cphi_stheta * mag->z; + const int32_t me = 0 * mag->x + cphi * mag->y - sphi * mag->z; + //const int32_t md = + // -stheta * imu.mag.x + + // sphi_ctheta * imu.mag.y + + // cphi_ctheta * imu.mag.z; + // float m_psi = -atan2(me, mn); + const float mag_dec = atan2(-AHRS_H_Y, AHRS_H_X); + const float fpsi = atan2f(-me, mn) - mag_dec; + e->psi = ANGLE_BFP_OF_REAL(fpsi); + INT32_ANGLE_NORMALIZE(e->psi); + +} + +#endif /* AHRS_INT_UTILS_H */ diff --git a/sw/airborne/subsystems/imu.c b/sw/airborne/subsystems/imu.c index e6baede3e8..515adc6b38 100644 --- a/sw/airborne/subsystems/imu.c +++ b/sw/airborne/subsystems/imu.c @@ -43,7 +43,7 @@ void imu_init(void) { ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_THETA), ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_PSI) }; INT32_QUAT_OF_EULERS(imu.body_to_imu_quat, body_to_imu_eulers); - INT32_QUAT_NORMALISE(imu.body_to_imu_quat); + INT32_QUAT_NORMALIZE(imu.body_to_imu_quat); INT32_RMAT_OF_EULERS(imu.body_to_imu_rmat, body_to_imu_eulers); #else INT32_QUAT_ZERO(imu.body_to_imu_quat); @@ -64,7 +64,7 @@ void imu_float_init(struct ImuFloat* imuf) { EULERS_ASSIGN(imuf->body_to_imu_eulers, IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI); FLOAT_QUAT_OF_EULERS(imuf->body_to_imu_quat, imuf->body_to_imu_eulers); - FLOAT_QUAT_NORMALISE(imuf->body_to_imu_quat); + FLOAT_QUAT_NORMALIZE(imuf->body_to_imu_quat); FLOAT_RMAT_OF_EULERS(imuf->body_to_imu_rmat, imuf->body_to_imu_eulers); #else EULERS_ASSIGN(imuf->body_to_imu_eulers, 0., 0., 0.); diff --git a/sw/airborne/subsystems/imu/imu_dummy.c b/sw/airborne/subsystems/imu/imu_dummy.c new file mode 100644 index 0000000000..8ef21d644c --- /dev/null +++ b/sw/airborne/subsystems/imu/imu_dummy.c @@ -0,0 +1,7 @@ + + +void imu_impl_init(void) { + + +} + diff --git a/sw/airborne/subsystems/ins.c b/sw/airborne/subsystems/ins.c index cd098c51b5..0e8e560273 100644 --- a/sw/airborne/subsystems/ins.c +++ b/sw/airborne/subsystems/ins.c @@ -70,7 +70,7 @@ bool_t ins_hf_realign; int32_t ins_qfe; bool_t ins_baro_initialised; int32_t ins_baro_alt; -#ifdef BOOZ2_SONAR +#ifdef USE_SONAR bool_t ins_update_on_agl; int32_t ins_sonar_offset; #endif @@ -190,7 +190,7 @@ void ins_update_baro() { if (ins_vf_realign) { ins_vf_realign = FALSE; ins_qfe = baro.absolute; -#ifdef BOOZ2_SONAR +#ifdef USE_SONAR ins_sonar_offset = sonar_meas; #endif vff_realign(0.); @@ -267,13 +267,13 @@ void ins_update_gps(void) { } void ins_update_sonar() { -#if defined BOOZ2_SONAR && defined USE_VFF +#if defined USE_SONAR && defined USE_VFF static int32_t sonar_filtered = 0; sonar_filtered = (sonar_meas + 2*sonar_filtered) / 3; /* update baro_qfe assuming a flat ground */ - if (ins_update_on_agl && booz2_analog_baro_status == BOOZ2_ANALOG_BARO_RUNNING) { + if (ins_update_on_agl && baro.status == BS_RUNNING) { int32_t d_sonar = (((int32_t)sonar_filtered - ins_sonar_offset) * INS_SONAR_SENS_NUM) / INS_SONAR_SENS_DEN; - ins_qfe = (int32_t)booz2_analog_baro_value + (d_sonar * (INS_BARO_SENS_DEN))/INS_BARO_SENS_NUM; + ins_qfe = baro.absolute + (d_sonar * (INS_BARO_SENS_DEN))/INS_BARO_SENS_NUM; } #endif } diff --git a/sw/airborne/subsystems/ins.h b/sw/airborne/subsystems/ins.h index d2d374c121..6353bbb553 100644 --- a/sw/airborne/subsystems/ins.h +++ b/sw/airborne/subsystems/ins.h @@ -37,7 +37,7 @@ extern struct NedCoor_i ins_gps_speed_cm_s_ned; extern int32_t ins_baro_alt; extern int32_t ins_qfe; extern bool_t ins_baro_initialised; -#ifdef BOOZ2_SONAR +#ifdef USE_SONAR extern bool_t ins_update_on_agl; /* use sonar to update agl if available */ extern int32_t ins_sonar_offset; #endif diff --git a/sw/airborne/subsystems/navigation/spiral.c b/sw/airborne/subsystems/navigation/spiral.c index b25ef937e2..9ab41fb2b1 100644 --- a/sw/airborne/subsystems/navigation/spiral.c +++ b/sw/airborne/subsystems/navigation/spiral.c @@ -118,7 +118,7 @@ bool_t SpiralNav(void) //dc_Circle(360/Segmente); } break; - case Circle: + case Circle: { nav_circle_XY(waypoints[Center].x, waypoints[Center].y, SRad); // Trigonometrische Berechnung des bereits geflogenen Winkels alpha // equation: @@ -135,6 +135,7 @@ bool_t SpiralNav(void) CSpiralStatus = IncSpiral; } break; + } case IncSpiral: // increasing circle radius as long as it is smaller than max helix radius if(SRad + IRad < Spiralradius) diff --git a/sw/airborne/subsystems/settings.c b/sw/airborne/subsystems/settings.c new file mode 100644 index 0000000000..04754e2ead --- /dev/null +++ b/sw/airborne/subsystems/settings.c @@ -0,0 +1,21 @@ +#include "subsystems/settings.h" + + +struct PersistentSettings pers_settings; +bool_t settings_store_now; + + +void settings_init(void) { +#ifdef USE_PERSISTENT_SETTINGS + if (persistent_read((uint32_t)&pers_settings, sizeof(struct PersistentSettings))) + return; // return -1 ? + persitent_settings_load(); +#endif +} + + +void settings_store(void) { + persitent_settings_store(); + persistent_write((uint32_t)&pers_settings, sizeof(struct PersistentSettings)); +} + diff --git a/sw/airborne/subsystems/settings.h b/sw/airborne/subsystems/settings.h new file mode 100644 index 0000000000..8c39b680eb --- /dev/null +++ b/sw/airborne/subsystems/settings.h @@ -0,0 +1,20 @@ +#ifndef SUBSYSTEMS_SETTINGS_H +#define SUBSYSTEMS_SETTINGS_H + +#include "std.h" + +extern void settings_init(void); +extern void settings_store(void); + +extern bool_t settings_store_now; + +#define settings_StoreSettings(_v) { settings_store_now = _v; settings_store(); } + +#include "generated/settings.h" + +/* implemented in arch dependant code */ +int32_t persistent_write(uint32_t ptr, uint32_t size); +int32_t persistent_read(uint32_t ptr, uint32_t size); + + +#endif /* SUBSYSTEMS_SETTINGS_H */ diff --git a/sw/airborne/test/ahrs/Makefile b/sw/airborne/test/ahrs/Makefile index 196de6300d..2cde23ddab 100644 --- a/sw/airborne/test/ahrs/Makefile +++ b/sw/airborne/test/ahrs/Makefile @@ -1,21 +1,31 @@ +# Launch with "make Q=''" to get full command display +Q=@ CC = gcc CFLAGS = -std=c99 -I.. -I../.. -I../../../include -I../../booz -I../../../booz -Wall -LDFLAGS = -lm +LDFLAGS = -lm -lgsl -lgslcblas + +CFLAGS += # imu wants airframe to fetch its neutrals # ahrs wants airframe to fetch IMU_BODY_TO_IMU_ANGLES -CFLAGS += -I../../../../var/BOOZ2_A7 +#CFLAGS += -I../../../../var/BOOZ2_A7 +CFLAGS += -I../../../../var/TA #CFLAGS += -DIMU_BODY_TO_IMU_PHI=0 -DIMU_BODY_TO_IMU_THETA=0 -DIMU_BODY_TO_IMU_PSI=0 # toulouse 0.51562740288882, -0.05707735220832, 0.85490967783446 CFLAGS += -DAHRS_H_X=0.51562740288882 -DAHRS_H_Y=-0.05707735220832 -DAHRS_H_Z=0.85490967783446 -#CFLAGS += -DOUTPUT_IN_BODY_FRAME -CFLAGS += -DENABLE_MAG_UPDATE -CFLAGS += -DENABLE_ACCEL_UPDATE +#CFLAGS += -DDISABLE_ALIGNEMENT +#CFLAGS += -DDISABLE_PROPAGATE +#CFLAGS += -DDISABLE_ACCEL_UPDATE +#CFLAGS += -DDISABLE_MAG_UPDATE +#CFLAGS += -DPERFECT_SENSORS +#CFLAGS += -DLOW_NOISE_THRESHOLD='ACCEL_BFP_OF_REAL(5.5)' +CFLAGS += -DLOW_NOISE_THRESHOLD=500000 +CFLAGS += -DLOW_NOISE_TIME=2 SRCS= run_ahrs_on_flight_log.c \ ../../math/pprz_trig_int.c \ @@ -34,5 +44,102 @@ run_ahrs_fcr_on_flight_log: ../../subsystems/ahrs/ahrs_float_cmpl_rmat.c $(SRCS) run_ahrs_ice_on_flight_log: ../../subsystems/ahrs/ahrs_int_cmpl_euler.c $(SRCS) $(CC) -DAHRS_TYPE=AHRS_TYPE_ICE $(CFLAGS) -o $@ $^ $(LDFLAGS) +ifndef AHRS_TYPE +#AHRS_TYPE = AHRS_TYPE_ICE +AHRS_TYPE = AHRS_TYPE_ICQ +#AHRS_TYPE = AHRS_TYPE_FLQ +#AHRS_TYPE = AHRS_TYPE_FCR # doesn't work - inverted accel +#AHRS_TYPE = AHRS_TYPE_FCR2 +#AHRS_TYPE = AHRS_TYPE_FCQ +endif +ifndef PROPAGATE_LOW_PASS_RATES +PROPAGATE_LOW_PASS_RATES = 0 +endif +ifndef GRAVITY_UPDATE_NORM_HEURISTIC +GRAVITY_UPDATE_NORM_HEURISTIC = 0 +endif +ifndef GRAVITY_UPDATE_COORDINATED_TURN +GRAVITY_UPDATE_COORDINATED_TURN = 0 +endif +ifndef MAG_UPDATE_YAW_ONLY +MAG_UPDATE_YAW_ONLY = 0 +endif + + +AHRS_CFLAGS += -DAHRS_TYPE=$(AHRS_TYPE) +AHRS_CFLAGS += -DPERIODIC_FREQUENCY=512 +AHRS_CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=512 + +ifeq ($(PROPAGATE_LOW_PASS_RATES), 1) +AHRS_CFLAGS += -DAHRS_PROPAGATE_LOW_PASS_RATES +endif +ifeq ($(GRAVITY_UPDATE_NORM_HEURISTIC), 1) +AHRS_CFLAGS += -DAHRS_GRAVITY_UPDATE_NORM_HEURISTIC +endif +ifeq ($(GRAVITY_UPDATE_COORDINATED_TURN), 1) +AHRS_CFLAGS += -DAHRS_GRAVITY_UPDATE_COORDINATED_TURN +endif +ifeq ($(MAG_UPDATE_YAW_ONLY), 1) +AHRS_CFLAGS += -DAHRS_MAG_UPDATE_YAW_ONLY +endif + +AHRS_SRCS += ../../subsystems/ahrs.c \ + ../../subsystems/ahrs/ahrs_aligner.c \ + +ifeq ($(AHRS_TYPE), AHRS_TYPE_ICE) +AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_euler.h\" +AHRS_CFLAGS += -DFACE_REINJ_1=1024 +AHRS_SRCS += ../../subsystems/ahrs/ahrs_int_cmpl_euler.c +endif +ifeq ($(AHRS_TYPE), AHRS_TYPE_ICQ) +AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl.h\" +AHRS_SRCS += ../../subsystems/ahrs/ahrs_int_cmpl.c +endif +ifeq ($(AHRS_TYPE), AHRS_TYPE_FLQ) +AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_lkf_quat.h\" +AHRS_SRCS += ../../subsystems/ahrs/ahrs_float_lkf_quat.c +endif +ifeq ($(AHRS_TYPE), AHRS_TYPE_FCR) +AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_dcm.h\" +AHRS_CFLAGS += -DINS_ROLL_NEUTRAL_DEFAULT=0 +AHRS_CFLAGS += -DINS_PITCH_NEUTRAL_DEFAULT=0 +AHRS_CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=512 +AHRS_CFLAGS += -DDCM_UPDATE_AFTER_PROPAGATE +AHRS_SRCS += ../../subsystems/ahrs/ahrs_float_dcm.c +endif +ifeq ($(AHRS_TYPE), AHRS_TYPE_FCR2) +AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_rmat.h\" +AHRS_CFLAGS += -DAHRS_PROPAGATE_RMAT +AHRS_SRCS += ../../subsystems/ahrs/ahrs_float_cmpl_rmat.c +endif +ifeq ($(AHRS_TYPE), AHRS_TYPE_FCQ) +AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_rmat.h\" +AHRS_CFLAGS += -DAHRS_PROPAGATE_QUAT +AHRS_SRCS += ../../subsystems/ahrs/ahrs_float_cmpl_rmat.c +endif + + +RAOS_SRCS = ./ahrs_on_synth.c \ + ../../subsystems/imu.c \ + ../../subsystems/imu/imu_dummy.c \ + ../../math/pprz_trig_int.c \ + ../../../simulator/nps/nps_random.c + + +run_ahrs_on_synth: run_ahrs_on_synth.c $(RAOS_SRCS) $(AHRS_SRCS) + @echo "Building run_ahrs_on_synth for $(AHRS_TYPE)" + $(Q) $(CC) $(CFLAGS) $(AHRS_CFLAGS) -o $@ $^ $(LDFLAGS) + +IVY_CFLAGS=-g -O2 -Wall `pkg-config glib-2.0 --cflags` +IVY_LDFLAGS=`pkg-config glib-2.0 --libs` `pcre-config --libs` -lglibivy + +run_ahrs_on_synth_ivy: run_ahrs_on_synth_ivy.c $(RAOS_SRCS) $(AHRS_SRCS) + $(CC) $(CFLAGS) $(AHRS_CFLAGS) $(IVY_CFLAGS) -o $@ $^ $(LDFLAGS) $(IVY_LDFLAGS) + + + + + clean: - rm -f *~ run_ahrs_*_on_flight_log + @echo "cleaning ..." + $(Q) rm -f *~ run_ahrs_*_on_flight_log run_ahrs_on_synth_ivy run_ahrs_on_synth diff --git a/sw/airborne/test/ahrs/ahrs_on_synth.c b/sw/airborne/test/ahrs/ahrs_on_synth.c new file mode 100644 index 0000000000..21d451a307 --- /dev/null +++ b/sw/airborne/test/ahrs/ahrs_on_synth.c @@ -0,0 +1,413 @@ +#include "test/ahrs/ahrs_on_synth.h" + +#include + +#include "subsystems/imu.h" +#include "subsystems/ahrs.h" +#include "subsystems/ahrs/ahrs_aligner.h" +#include "../simulator/nps/nps_random.h" + +#include "../pprz_algebra_print.h" + + +char* ahrs_type_str[AHRS_TYPE_NB] = { + "Int Compl Euler", + "Int Compl Quat", + "Float LKF Quat", + "Float Compl Rmat", + "Float Compl Rmat 2", + "Float Compl Quat" }; + +static void traj_static_static_init(void); +static void traj_static_static_update(void); + +static void traj_step_phi_init(void); +static void traj_step_phi_update(void); + +static void traj_step_phi_2nd_order_init(void); +static void traj_step_phi_2nd_order_update(void); + +static void traj_step_biasp_init(void); +static void traj_step_biasp_update(void); + +static void traj_static_sine_init(void); +static void traj_static_sine_update(void); + +static void traj_sineX_quad_init(void); +static void traj_sineX_quad_update(void); + +static void traj_coordinated_circle_init(void); +static void traj_coordinated_circle_update(void); + +static void traj_stop_stop_x_init(void); +static void traj_stop_stop_x_update(void); + +struct traj traj[] = { + {.name="static", .desc="blaa", + .init_fun=traj_static_static_init, .update_fun=traj_static_static_update}, + {.name="sine", .desc="blaa2", + .init_fun=traj_static_sine_init, .update_fun=traj_static_sine_update}, + {.name="sineX", .desc="blaa2", + .init_fun=traj_sineX_quad_init, .update_fun=traj_sineX_quad_update}, + {.name="step_phi", .desc="blaa2", + .init_fun=traj_step_phi_init, .update_fun=traj_step_phi_update}, + {.name="step_phi2", .desc="blaa2", + .init_fun=traj_step_phi_2nd_order_init, .update_fun=traj_step_phi_2nd_order_update}, + {.name="step_bias", .desc="blaa2", + .init_fun=traj_step_biasp_init, .update_fun=traj_step_biasp_update}, + {.name="coordinated circle", .desc="blaa2", + .init_fun=traj_coordinated_circle_init, .update_fun=traj_coordinated_circle_update}, + {.name="stop stop x", .desc="blaa2", + .init_fun=traj_stop_stop_x_init, .update_fun=traj_stop_stop_x_update} +}; + + +struct AhrsOnSynth aos; + + +void aos_init(int traj_nb) { + + aos.traj = &traj[traj_nb]; + + aos.time = 0; + aos.dt = 1./AHRS_PROPAGATE_FREQUENCY; + aos.traj->ts = 0; + aos.traj->ts = 1.; // default to one second + + /* default state */ + EULERS_ASSIGN(aos.ltp_to_imu_euler, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(0.)); + FLOAT_QUAT_OF_EULERS(aos.ltp_to_imu_quat, aos.ltp_to_imu_euler); + RATES_ASSIGN(aos.imu_rates, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(0.)); + FLOAT_VECT3_ZERO(aos.ltp_pos); + FLOAT_VECT3_ZERO(aos.ltp_vel); + FLOAT_VECT3_ZERO(aos.ltp_accel); + FLOAT_VECT3_ZERO(aos.ltp_jerk); + aos.traj->init_fun(); + + imu_init(); + ahrs_init(); + ahrs_aligner_init(); + +#ifdef PERFECT_SENSORS + RATES_ASSIGN(aos.gyro_bias, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(0.)); + RATES_ASSIGN(aos.gyro_noise, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(0.)); + VECT3_ASSIGN(aos.accel_noise, 0., 0., 0.); +#else + RATES_ASSIGN(aos.gyro_bias, RadOfDeg(1.), RadOfDeg(2.), RadOfDeg(3.)); + RATES_ASSIGN(aos.gyro_noise, RadOfDeg(1.), RadOfDeg(1.), RadOfDeg(1.)); + VECT3_ASSIGN(aos.accel_noise, .5, .5, .5); +#endif + + +#ifdef FORCE_ALIGNEMENT + // DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("# oas quat", aos.ltp_to_imu_quat); + aos_compute_sensors(); + // DISPLAY_FLOAT_RATES_DEG("# oas gyro_bias", aos.gyro_bias); + // DISPLAY_FLOAT_RATES_DEG("# oas imu_rates", aos.imu_rates); + VECT3_COPY(ahrs_aligner.lp_accel, imu.accel); + VECT3_COPY(ahrs_aligner.lp_mag, imu.mag); + RATES_COPY(ahrs_aligner.lp_gyro, imu.gyro); + // DISPLAY_INT32_RATES_AS_FLOAT_DEG("# ahrs_aligner.lp_gyro", ahrs_aligner.lp_gyro); + ahrs_align(); + // DISPLAY_FLOAT_RATES_DEG("# ahrs_impl.gyro_bias", ahrs_impl.gyro_bias); + +#endif + + + + +#ifdef DISABLE_ALIGNEMENT + printf("# DISABLE_ALIGNEMENT\n"); +#endif +#ifdef DISABLE_PROPAGATE + printf("# DISABLE_PROPAGATE\n"); +#endif +#ifdef DISABLE_ACCEL_UPDATE + printf("# DISABLE_ACCEL_UPDATE\n"); +#endif +#ifdef DISABLE_MAG_UPDATE + printf("# DISABLE_MAG_UPDATE\n"); +#endif + printf("# AHRS_TYPE %s\n", ahrs_type_str[AHRS_TYPE]); + printf("# AHRS_PROPAGATE_FREQUENCY %d\n", AHRS_PROPAGATE_FREQUENCY); +#ifdef AHRS_PROPAGATE_LOW_PASS_RATES + printf("# AHRS_PROPAGATE_LOW_PASS_RATES\n"); +#endif +#ifdef AHRS_MAG_UPDATE_YAW_ONLY + printf("# AHRS_MAG_UPDATE_YAW_ONLY\n"); +#endif +#ifdef AHRS_GRAVITY_UPDATE_COORDINATED_TURN + printf("# AHRS_GRAVITY_UPDATE_COORDINATED_TURN\n"); +#endif +#ifdef AHRS_GRAVITY_UPDATE_NORM_HEURISTIC + printf("# AHRS_GRAVITY_UPDATE_NORM_HEURISTIC\n"); +#endif +#ifdef PERFECT_SENSORS + printf("# PERFECT_SENSORS\n"); +#endif + + printf("# tajectory : %s\n", aos.traj->name); + +}; + + +void aos_compute_sensors(void) { + + struct FloatRates gyro; + RATES_SUM(gyro, aos.imu_rates, aos.gyro_bias); + // printf("#aos.gyro_bias %f\n",DegOfRad( aos.gyro_bias.r)); + + float_rates_add_gaussian_noise(&gyro, &aos.gyro_noise); + + RATES_BFP_OF_REAL(imu.gyro, gyro); + RATES_BFP_OF_REAL(imu.gyro_prev, gyro); + + struct FloatVect3 g_ltp = {0., 0., 9.81}; + struct FloatVect3 accelero_ltp; + VECT3_DIFF(accelero_ltp, aos.ltp_accel, g_ltp); + struct FloatVect3 accelero_imu; + FLOAT_QUAT_VMULT(accelero_imu, aos.ltp_to_imu_quat, accelero_ltp); + + float_vect3_add_gaussian_noise(&accelero_imu, &aos.accel_noise); + ACCELS_BFP_OF_REAL(imu.accel, accelero_imu); + + + struct FloatVect3 h_earth = {AHRS_H_X, AHRS_H_Y, AHRS_H_Z}; + struct FloatVect3 h_imu; + FLOAT_QUAT_VMULT(h_imu, aos.ltp_to_imu_quat, h_earth); + MAGS_BFP_OF_REAL(imu.mag, h_imu); + +#ifdef AHRS_GRAVITY_UPDATE_COORDINATED_TURN +#if AHRS_TYPE == AHRS_TYPE_FCR2 || AHRS_TYPE == AHRS_TYPE_FCQ || AHRS_TYPE == AHRS_TYPE_FLQ + VECT3_COPY(ahrs_impl.est_ltp_speed, aos.ltp_vel); +#endif +#if AHRS_TYPE == AHRS_TYPE_ICQ + ahrs_impl.ltp_vel_norm = SPEED_BFP_OF_REAL(FLOAT_VECT3_NORM(aos.ltp_vel)); +#endif +#endif + + +} + +void aos_compute_state(void) { + + aos.time += aos.dt; + aos.traj->update_fun(); + +} + + +void aos_run(void) { + + aos_compute_state(); + aos_compute_sensors(); +#ifndef DISABLE_ALIGNEMENT + if (ahrs.status == AHRS_UNINIT) { + ahrs_aligner_run(); + if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED) + ahrs_align(); + } + else { +#endif /* DISABLE_ALIGNEMENT */ + ahrs_propagate(); + ahrs_update_accel(); + ahrs_update_mag(); +#ifndef DISABLE_ALIGNEMENT + } +#endif + +} + + + + +static void traj_static_static_init(void) { + + aos.traj->te = 120.; + +} + +static void traj_static_static_update(void) { + + // if (aos.time > 3) { + // EULERS_ASSIGN(aos.ltp_to_imu_euler, RadOfDeg(5), 0, 0); + // FLOAT_QUAT_OF_EULERS(aos.ltp_to_imu_quat, aos.ltp_to_imu_euler); + // } + // aos.imu_rates.p = 0.; + // aos.imu_rates.q = 0.; + // aos.imu_rates.r = 0.; + +} + + +// +// +// +static void traj_static_sine_init(void) { + + aos.traj->te = 10.; + +} + +static void traj_static_sine_update(void) { + + + aos.imu_rates.p = RadOfDeg(200)*cos(aos.time); + aos.imu_rates.q = RadOfDeg(200)*cos(0.7*aos.time+2); + aos.imu_rates.r = RadOfDeg(200)*cos(0.8*aos.time+1); + FLOAT_QUAT_INTEGRATE(aos.ltp_to_imu_quat, aos.imu_rates, aos.dt); + FLOAT_EULERS_OF_QUAT(aos.ltp_to_imu_euler, aos.ltp_to_imu_quat); + +} + + +// +// this is a sine trajectory along the x axis +// we pretend a dragless vectoriel trust vehicle banks +// to achieve it +// +static void traj_sineX_quad_init(void) { aos.traj->te = 60.; } +static void traj_sineX_quad_update(void) { + + const float om = RadOfDeg(10); + + if ( aos.time > (M_PI/om) ) { + const float a = 20; + + struct FloatVect3 jerk; + VECT3_ASSIGN(jerk , -a*om*om*om*cos(om*aos.time), 0, 0); + VECT3_ASSIGN(aos.ltp_accel , -a*om*om *sin(om*aos.time), 0, 0); + VECT3_ASSIGN(aos.ltp_vel , a*om *cos(om*aos.time), 0, 0); + VECT3_ASSIGN(aos.ltp_pos , a *sin(om*aos.time), 0, 0); + + // this is based on differential flatness of the quad + EULERS_ASSIGN(aos.ltp_to_imu_euler, 0., atan2(aos.ltp_accel.x, 9.81), 0.); + FLOAT_QUAT_OF_EULERS(aos.ltp_to_imu_quat, aos.ltp_to_imu_euler); + const struct FloatEulers e_dot = { + 0., + 9.81*jerk.x / ( (9.81*9.81) + (aos.ltp_accel.x*aos.ltp_accel.x)), + 0. }; + FLOAT_RATES_OF_EULER_DOT(aos.imu_rates, aos.ltp_to_imu_euler, e_dot); + } +} + + +// +// +// +static void traj_step_phi_init(void) { aos.traj->te = 40.;} +static void traj_step_phi_update(void) { + if (aos.time > 5) { + EULERS_ASSIGN(aos.ltp_to_imu_euler, RadOfDeg(5), 0, 0); + FLOAT_QUAT_OF_EULERS(aos.ltp_to_imu_quat, aos.ltp_to_imu_euler); + } +} + +// +// +// +static void traj_step_phi_2nd_order_init(void) { + aos.traj->te = 0.; + aos.traj->te = 40.; +} + +static void traj_step_phi_2nd_order_update(void) { + + if (aos.time > 15) { + const float omega = RadOfDeg(100); + const float xi = 0.9; + struct FloatRates raccel; + RATES_ASSIGN(raccel, -2.*xi*omega*aos.imu_rates.p - omega*omega*(aos.ltp_to_imu_euler.phi - RadOfDeg(5)), 0., 0.); + FLOAT_RATES_INTEGRATE_FI(aos.imu_rates, raccel, aos.dt); + FLOAT_QUAT_INTEGRATE(aos.ltp_to_imu_quat, aos.imu_rates, aos.dt); + FLOAT_EULERS_OF_QUAT(aos.ltp_to_imu_euler, aos.ltp_to_imu_quat); + } + +} + + +static void traj_step_biasp_init(void) { aos.traj->te = 120.; } +static void traj_step_biasp_update(void) { if (aos.time > 5) aos.gyro_bias.p = RadOfDeg(3);} + + +static void traj_coordinated_circle_init(void) { + + aos.traj->te = 120.; + + const float speed = 15; // m/s + const float R = 100; // radius in m + // tan phi = v^2/Rg + float phi = atan2(speed*speed, R*9.81); + EULERS_ASSIGN(aos.ltp_to_imu_euler, phi, 0, M_PI_2); + FLOAT_QUAT_OF_EULERS(aos.ltp_to_imu_quat, aos.ltp_to_imu_euler); +} + +static void traj_coordinated_circle_update(void) { + const float speed = 15; // m/s + const float R = 100; // radius in m + float omega = speed / R; + // tan phi = v^2/Rg + float phi = atan2(speed*speed, R*9.81); + if ( aos.time > 2.*M_PI/omega) { + VECT3_ASSIGN(aos.ltp_pos, R*cos(omega*aos.time), R*sin(omega*aos.time), 0.); + VECT3_ASSIGN(aos.ltp_vel, -omega*R*sin(omega*aos.time), omega*R*cos(omega*aos.time), 0.); + VECT3_ASSIGN(aos.ltp_accel, -omega*omega*R*cos(omega*aos.time), -omega*omega*R*sin(omega*aos.time), 0.); + + + // float psi = atan2(aos.ltp_pos.y, aos.ltp_pos.x); + float psi = M_PI_2 + omega*aos.time; while (psi>M_PI) psi -= 2.*M_PI; + EULERS_ASSIGN(aos.ltp_to_imu_euler, phi, 0, psi); + FLOAT_QUAT_OF_EULERS(aos.ltp_to_imu_quat, aos.ltp_to_imu_euler); + + struct FloatEulers e_dot; + EULERS_ASSIGN(e_dot, 0., 0., omega); + FLOAT_RATES_OF_EULER_DOT(aos.imu_rates, aos.ltp_to_imu_euler, e_dot); + } + +} + + + +//static char** traj_stop_stop_x_desc(void) { + // static const char** desc = + // {"stop top", NULL}; +// return desc; +//} +static void traj_stop_stop_x_init(void) { aos.traj->te = 30.;} + +static void traj_stop_stop_x_update(void){ + + const float t0 = 5.; + const float dt_jerk = 0.75; + const float dt_nojerk = 10.; + const float val_jerk = 5.; + + FLOAT_VECT3_INTEGRATE_FI(aos.ltp_pos, aos.ltp_vel, aos.dt); + FLOAT_VECT3_INTEGRATE_FI(aos.ltp_vel, aos.ltp_accel, aos.dt); + FLOAT_VECT3_INTEGRATE_FI(aos.ltp_accel, aos.ltp_jerk, aos.dt); + + if (aos.time < t0) return; + else if (aos.time < t0+dt_jerk) { + VECT3_ASSIGN(aos.ltp_jerk , val_jerk, 0., 0.); } + else if (aos.time < t0 + 2.*dt_jerk) { + VECT3_ASSIGN(aos.ltp_jerk , -val_jerk, 0., 0.); } + else if (aos.time < t0 + 2.*dt_jerk + dt_nojerk) { + VECT3_ASSIGN(aos.ltp_jerk , 0. , 0., 0.); } + else if (aos.time < t0 + 3.*dt_jerk + dt_nojerk) { + VECT3_ASSIGN(aos.ltp_jerk , -val_jerk, 0., 0.); } + else if (aos.time < t0 + 4.*dt_jerk + dt_nojerk) { + VECT3_ASSIGN(aos.ltp_jerk , val_jerk, 0., 0.); } + else { + VECT3_ASSIGN(aos.ltp_jerk , 0. , 0., 0.); } + + + // this is based on differential flatness of the quad + EULERS_ASSIGN(aos.ltp_to_imu_euler, 0., atan2(aos.ltp_accel.x, 9.81), 0.); + FLOAT_QUAT_OF_EULERS(aos.ltp_to_imu_quat, aos.ltp_to_imu_euler); + const struct FloatEulers e_dot = { + 0., + 9.81*aos.ltp_jerk.x / ( (9.81*9.81) + (aos.ltp_accel.x*aos.ltp_accel.x)), + 0. }; + FLOAT_RATES_OF_EULER_DOT(aos.imu_rates, aos.ltp_to_imu_euler, e_dot); + +} diff --git a/sw/airborne/test/ahrs/ahrs_on_synth.h b/sw/airborne/test/ahrs/ahrs_on_synth.h new file mode 100644 index 0000000000..20644a59db --- /dev/null +++ b/sw/airborne/test/ahrs/ahrs_on_synth.h @@ -0,0 +1,60 @@ +#ifndef AHRS_ON_SYNTH_H +#define AHRS_ON_SYNTH_H + +#include "math/pprz_algebra_float.h" + +struct traj { + char* name; + char* desc; + void (*init_fun)(void); + void (*update_fun)(void); + + double ts; + double te; +}; + +struct AhrsOnSynth { + + struct traj* traj; + + double time; + double dt; + + /* sensors */ + struct FloatRates gyro_bias; + struct FloatRates gyro_noise; + struct FloatVect3 accel_noise; + + /* state */ + struct FloatEulers ltp_to_imu_euler; + struct FloatQuat ltp_to_imu_quat; + struct FloatRates imu_rates; + + struct FloatVect3 ltp_jerk; + struct FloatVect3 ltp_accel; + struct FloatVect3 ltp_vel; + struct FloatVect3 ltp_pos; + + + +}; + +extern struct AhrsOnSynth aos; + +extern void aos_init(int traj_nb); +extern void aos_run(void); +extern void aos_compute_sensors(void); +extern void aos_compute_state(void); + + +#define AHRS_TYPE_ICE 0 +#define AHRS_TYPE_ICQ 1 +#define AHRS_TYPE_FLQ 2 +#define AHRS_TYPE_FCR 3 +#define AHRS_TYPE_FCR2 4 +#define AHRS_TYPE_FCQ 5 +#define AHRS_TYPE_NB 6 + +extern char* ahrs_type_str[AHRS_TYPE_NB]; + +#endif /* AHRS_ON_SYNTH_H */ diff --git a/sw/airborne/test/ahrs/ahrs_utils.py b/sw/airborne/test/ahrs/ahrs_utils.py new file mode 100644 index 0000000000..a31d502d5a --- /dev/null +++ b/sw/airborne/test/ahrs/ahrs_utils.py @@ -0,0 +1,158 @@ +#! /usr/bin/env python + +# $Id$ +# Copyright (C) 2011 Antoine Drouin +# +# 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. +# + +#import os +#from optparse import OptionParser +#import scipy +#from scipy import optimize +import shlex, subprocess +from pylab import * +from array import array +import numpy +import matplotlib.pyplot as plt + +def run_simulation(ahrs_type, build_opt, traj_nb): + print "\nBuilding ahrs" + args = ["make", "clean", "run_ahrs_on_synth", "AHRS_TYPE=AHRS_TYPE_"+ahrs_type] + build_opt +# print args + p = subprocess.Popen(args=args, stdout=subprocess.PIPE, stderr=subprocess.STDOUT, shell=False) + outputlines = p.stdout.readlines() + p.wait() + for i in outputlines: + print " # "+i, + print + + print "Running simulation" + print " using traj " + str(traj_nb) + p = subprocess.Popen(args=["./run_ahrs_on_synth",str(traj_nb)], stdout=subprocess.PIPE, stderr=subprocess.STDOUT, shell=False) + outputlines = p.stdout.readlines() + p.wait() +# for i in outputlines: +# print " "+i, +# print "\n" + + ahrs_data_type = [('time', 'float32'), + ('phi_true', 'float32'), ('theta_true', 'float32'), ('psi_true', 'float32'), + ('p_true', 'float32'), ('q_true', 'float32'), ('r_true', 'float32'), + ('bp_true', 'float32'), ('bq_true', 'float32'), ('br_true', 'float32'), + ('phi_ahrs', 'float32'), ('theta_ahrs', 'float32'), ('psi_ahrs', 'float32'), + ('p_ahrs', 'float32'), ('q_ahrs', 'float32'), ('r_ahrs', 'float32'), + ('bp_ahrs', 'float32'), ('bq_ahrs', 'float32'), ('br_ahrs', 'float32') + ] + pos_data_type = [ ('x0_true', 'float32'), ('y0_true', 'float32'), ('z0_true', 'float32'), + ('x1_true', 'float32'), ('y1_true', 'float32'), ('z1_true', 'float32'), + ('x2_true', 'float32'), ('y2_true', 'float32'), ('z2_true', 'float32'), + ('x3_true', 'float32'), ('y3_true', 'float32'), ('z3_true', 'float32'), + ] + mydescr = numpy.dtype(ahrs_data_type) + data = [[] for dummy in xrange(len(mydescr))] +# import code; code.interact(local=locals()) + for line in outputlines: + if line.startswith("#"): + print " "+line, + else: + fields = line.strip().split(' '); +# print fields + for i, number in enumerate(fields): + data[i].append(number) + + print + for i in xrange(len(mydescr)): + data[i] = cast[mydescr[i]](data[i]) + + return numpy.rec.array(data, dtype=mydescr) + + + +def plot_simulation_results(plot_true_state, lsty, sim_res): + print "Plotting Results" + + # f, (ax1, ax2, ax3) = plt.subplots(3, sharex=True, sharey=True) + + subplot(3,3,1) + plt.plot(sim_res.time, sim_res.phi_ahrs, lsty) + ylabel('degres') + title('phi') + + subplot(3,3,2) + plot(sim_res.time, sim_res.theta_ahrs, lsty) + title('theta') + + subplot(3,3,3) + plot(sim_res.time, sim_res.psi_ahrs, lsty) + title('psi') + + subplot(3,3,4) + plt.plot(sim_res.time, sim_res.p_ahrs, lsty) + ylabel('degres/s') + title('p') + + subplot(3,3,5) + plt.plot(sim_res.time, sim_res.q_ahrs, lsty) + title('q') + + subplot(3,3,6) + plt.plot(sim_res.time, sim_res.r_ahrs, lsty) + title('r') + + subplot(3,3,7) + plt.plot(sim_res.time, sim_res.bp_ahrs, lsty) + ylabel('degres/s') + xlabel('time in s') + title('bp') + + subplot(3,3,8) + plt.plot(sim_res.time, sim_res.bq_ahrs, lsty) + xlabel('time in s') + title('bq') + + subplot(3,3,9) + plt.plot(sim_res.time, sim_res.br_ahrs, lsty) + xlabel('time in s') + title('br') + + + if plot_true_state: + subplot(3,3,1) + plt.plot(sim_res.time, sim_res.phi_true, 'r--') + subplot(3,3,2) + plot(sim_res.time, sim_res.theta_true, 'r--') + subplot(3,3,3) + plot(sim_res.time, sim_res.psi_true, 'r--') + subplot(3,3,4) + plot(sim_res.time, sim_res.p_true, 'r--') + subplot(3,3,5) + plot(sim_res.time, sim_res.q_true, 'r--') + subplot(3,3,6) + plot(sim_res.time, sim_res.r_true, 'r--') + subplot(3,3,7) + plot(sim_res.time, sim_res.bp_true, 'r--') + subplot(3,3,8) + plot(sim_res.time, sim_res.bq_true, 'r--') + subplot(3,3,9) + plot(sim_res.time, sim_res.br_true, 'r--') + + + +def show_plot(): + plt.show(); diff --git a/sw/airborne/test/ahrs/compare_ahrs.py b/sw/airborne/test/ahrs/compare_ahrs.py new file mode 100755 index 0000000000..8ac53e56ec --- /dev/null +++ b/sw/airborne/test/ahrs/compare_ahrs.py @@ -0,0 +1,67 @@ +#! /usr/bin/env python + +# $Id$ +# Copyright (C) 2011 Antoine Drouin +# +# 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. +# + +import ahrs_utils + +def main(): + +# traj_nb = 0; # static +# traj_nb = 1; # sine orientation +# traj_nb = 2; # sine X quad + traj_nb = 3; # step_phi +# traj_nb = 4; # step_phi_2nd_order +# traj_nb = 5; # step_bias +# traj_nb = 6; # coordinated circle +# traj_nb = 7; # stop stop X quad + + build_opt1 = []; +# build_opt1 = ["Q="]; + build_opt1 += ["PROPAGATE_LOW_PASS_RATES=1"]; +# build_opt1 = ["GRAVITY_UPDATE_COORDINATED_TURN=1"]; + build_opt1 += ["GRAVITY_UPDATE_NORM_HEURISTIC=0"]; +# build_opt1 = ["MAG_UPDATE_YAW_ONLY=1"]; + ahrs_type1 = "FCQ"; +# ahrs_type1 = "FCR2"; +# ahrs_type1 = "FLQ"; +# ahrs_type1 = "ICQ"; + sim_res1 = ahrs_utils.run_simulation(ahrs_type1, build_opt1, traj_nb) +# import code; code.interact(local=locals()) + + build_opt2 = []; +# build_opt2 = ["GRAVITY_UPDATE_COORDINATED_TURN=1"]; + build_opt2 += ["GRAVITY_UPDATE_NORM_HEURISTIC=1"]; +# build_opt2 = ["MAG_UPDATE_YAW_ONLY=0"]; + build_opt2 += ["PROPAGATE_LOW_PASS_RATES=1"]; +# build_opt2 = build_opt1; +# ahrs_type2 = "FLQ"; + ahrs_type2 = "FCQ"; +# ahrs_type2 = "ICQ"; + ahrs_type2 = ahrs_type1; + sim_res2 = ahrs_utils.run_simulation(ahrs_type2, build_opt2, traj_nb) + + ahrs_utils.plot_simulation_results(False, 'b', sim_res1) + ahrs_utils.plot_simulation_results(True, 'g', sim_res2) + ahrs_utils.show_plot() + +if __name__ == "__main__": + main() diff --git a/sw/airborne/test/ahrs/run_ahrs_on_synth.c b/sw/airborne/test/ahrs/run_ahrs_on_synth.c new file mode 100644 index 0000000000..ec0e4dc6cb --- /dev/null +++ b/sw/airborne/test/ahrs/run_ahrs_on_synth.c @@ -0,0 +1,92 @@ +#include + +#include "test/ahrs/ahrs_on_synth.h" + +#include "subsystems/imu.h" +#include "subsystems/ahrs.h" +#include "subsystems/ahrs/ahrs_aligner.h" + +#include "math/pprz_algebra_float.h" +#include "math/pprz_algebra_int.h" +#include "test/pprz_algebra_print.h" + + +static void report(void); + +int main(int argc, char** argv) { + + int traj = 0; + if (argc > 1) + traj = atoi(argv[1]); + + aos_init(traj); + report(); + + // while (aos.time < 0.01) { + while (aos.time < aos.traj->te) { + aos_run(); + report(); + } + + return 0; +} + + +static void report(void) { + + int output_sensors = FALSE; + int output_pos = FALSE; + +#if AHRS_TYPE == AHRS_TYPE_ICE || AHRS_TYPE == AHRS_TYPE_ICQ + EULERS_FLOAT_OF_BFP(ahrs_float.ltp_to_imu_euler, ahrs.ltp_to_imu_euler); + RATES_FLOAT_OF_BFP(ahrs_float.imu_rate, ahrs.imu_rate); +#endif + printf("%f ", aos.time); + + printf("%f %f %f ", DegOfRad(aos.ltp_to_imu_euler.phi), + DegOfRad(aos.ltp_to_imu_euler.theta), + DegOfRad(aos.ltp_to_imu_euler.psi)); + + printf("%f %f %f ", DegOfRad(aos.imu_rates.p), + DegOfRad(aos.imu_rates.q), + DegOfRad(aos.imu_rates.r)); + + printf("%f %f %f ", DegOfRad(aos.gyro_bias.p), + DegOfRad(aos.gyro_bias.q), + DegOfRad(aos.gyro_bias.r)); + + + printf("%f %f %f ", DegOfRad(ahrs_float.ltp_to_imu_euler.phi), + DegOfRad(ahrs_float.ltp_to_imu_euler.theta), + DegOfRad(ahrs_float.ltp_to_imu_euler.psi)); + + printf("%f %f %f ", DegOfRad(ahrs_float.imu_rate.p), + DegOfRad(ahrs_float.imu_rate.q), + DegOfRad(ahrs_float.imu_rate.r)); + +#if AHRS_TYPE == AHRS_TYPE_ICQ + struct FloatRates bias; + RATES_FLOAT_OF_BFP(bias, ahrs_impl.gyro_bias); + printf("%f %f %f ", DegOfRad(bias.p), + DegOfRad(bias.q), + DegOfRad(bias.r)); +#endif +#if AHRS_TYPE == AHRS_TYPE_FLQ || AHRS_TYPE == AHRS_TYPE_FCR2 || AHRS_TYPE == AHRS_TYPE_FCQ + printf("%f %f %f ", DegOfRad(ahrs_impl.gyro_bias.p), + DegOfRad(ahrs_impl.gyro_bias.q), + DegOfRad(ahrs_impl.gyro_bias.r)); +#endif + + if (output_pos) { + printf("%f %f %f ", aos.ltp_pos.x, + aos.ltp_pos.y, + aos.ltp_pos.z); + } + + if (output_sensors) { + + } + + printf("\n"); + +} diff --git a/sw/airborne/test/ahrs/run_ahrs_on_synth_ivy.c b/sw/airborne/test/ahrs/run_ahrs_on_synth_ivy.c new file mode 100644 index 0000000000..f9809717f7 --- /dev/null +++ b/sw/airborne/test/ahrs/run_ahrs_on_synth_ivy.c @@ -0,0 +1,102 @@ +#include +#include +#include +#include +#include + +#include +#include + +#include "test/ahrs/ahrs_on_synth.h" + +#include "subsystems/imu.h" +#include "subsystems/ahrs.h" + +#include "math/pprz_algebra_float.h" +#include "math/pprz_algebra_int.h" +#include "test/pprz_algebra_print.h" + + + + +gboolean timeout_callback(gpointer data) { + + for (int i=0; i<20; i++) { + aos_compute_state(); + aos_compute_sensors(); +#ifndef DISABLE_PROPAGATE + ahrs_propagate(); +#endif +#ifndef DISABLE_ACCEL_UPDATE + ahrs_update_accel(); +#endif +#ifndef DISABLE_MAG_UPDATE + if (!(i%5)) ahrs_update_mag(); +#endif + } + +#if AHRS_TYPE == AHRS_TYPE_ICE || AHRS_TYPE == AHRS_TYPE_ICQ + EULERS_FLOAT_OF_BFP(ahrs_float.ltp_to_imu_euler, ahrs.ltp_to_imu_euler); +#endif + +#if AHRS_TYPE == AHRS_TYPE_ICQ + IvySendMsg("183 BOOZ_AHRS_BIAS %d %d %d", + ahrs_impl.gyro_bias.p, + ahrs_impl.gyro_bias.q, + ahrs_impl.gyro_bias.r); +#endif +#if AHRS_TYPE == AHRS_TYPE_FLQ || AHRS_TYPE == AHRS_TYPE_FCR2 + struct Int32Rates bias_i; + RATES_BFP_OF_REAL(bias_i, ahrs_impl.gyro_bias); + IvySendMsg("183 BOOZ_AHRS_BIAS %d %d %d", + bias_i.p, + bias_i.q, + bias_i.r); +#endif + + IvySendMsg("183 AHRS_EULER %f %f %f", + ahrs_float.ltp_to_imu_euler.phi, + ahrs_float.ltp_to_imu_euler.theta, + ahrs_float.ltp_to_imu_euler.psi); + + IvySendMsg("183 BOOZ_SIM_RATE_ATTITUDE %f %f %f %f %f %f", + DegOfRad(aos.imu_rates.p), + DegOfRad(aos.imu_rates.q), + DegOfRad(aos.imu_rates.r), + DegOfRad(aos.ltp_to_imu_euler.phi), + DegOfRad(aos.ltp_to_imu_euler.theta), + DegOfRad(aos.ltp_to_imu_euler.psi)); + + IvySendMsg("183 BOOZ_SIM_GYRO_BIAS %f %f %f", + DegOfRad(aos.gyro_bias.p), + DegOfRad(aos.gyro_bias.q), + DegOfRad(aos.gyro_bias.r)); + + return TRUE; +} + + + + + +int main ( int argc, char** argv) { + + printf("hello\n"); + + g_timeout_add(1000/25, timeout_callback, NULL); + + GMainLoop *ml = g_main_loop_new(NULL, FALSE); + + IvyInit ("test_ahrs", "test_ahrs READY", NULL, NULL, NULL, NULL); + IvyStart("127.255.255.255"); + + imu_init(); + ahrs_init(); + + aos_init(); + + g_main_loop_run(ml); + + return 0; +} + diff --git a/sw/airborne/test/pprz_algebra_print.h b/sw/airborne/test/pprz_algebra_print.h index adbd26ab2a..2f67df9cc5 100644 --- a/sw/airborne/test/pprz_algebra_print.h +++ b/sw/airborne/test/pprz_algebra_print.h @@ -12,6 +12,10 @@ printf("%s %f %f %f\n",text, (_v).p, (_v).q, (_v).r); \ } +#define DISPLAY_FLOAT_RATES_DEG(text, _v) { \ + printf("%s %f %f %f\n",text, DegOfRad((_v).p), DegOfRad((_v).q), DegOfRad((_v).r)); \ + } + #define DISPLAY_FLOAT_RMAT(text, mat) { \ printf("%s\n %f %f %f\n %f %f %f\n %f %f %f\n",text, \ mat.m[0], mat.m[1], mat.m[2], mat.m[3], mat.m[4], mat.m[5], \ @@ -70,6 +74,19 @@ printf("%s %d %d %d\n",text, (_v).p, (_v).q, (_v).r); \ } +#define DISPLAY_INT32_RATES_AS_FLOAT(text, _r) { \ + struct FloatRates _fr; \ + RATES_FLOAT_OF_BFP(_fr, (_r)); \ + DISPLAY_FLOAT_RATES(text, _fr); \ + } + +#define DISPLAY_INT32_RATES_AS_FLOAT_DEG(text, _r) { \ + struct FloatRates _fr; \ + RATES_FLOAT_OF_BFP(_fr, (_r)); \ + DISPLAY_FLOAT_RATES_DEG(text, _fr); \ + } + + #define DISPLAY_INT32_EULERS(text, _e) { \ printf("%s %d %d %d\n",text, (_e).phi, (_e).theta, (_e).psi); \ diff --git a/sw/airborne/test/subsystems/test_ahrs.c b/sw/airborne/test/subsystems/test_ahrs.c new file mode 100644 index 0000000000..ac42d4b344 --- /dev/null +++ b/sw/airborne/test/subsystems/test_ahrs.c @@ -0,0 +1,191 @@ +/* + * $Id$ + * + * Copyright (C) 2011 The Paparazzi Team + * + * 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. + */ + +#include + +#include "std.h" +#include "mcu.h" +#include "sys_time.h" +#include "led.h" +#include "mcu_periph/uart.h" +#include "messages.h" +#include "downlink.h" + +#include "subsystems/imu.h" +#include "subsystems/ahrs.h" +#include "subsystems/ahrs/ahrs_aligner.h" + +#include "my_debug_servo.h" + +static inline void main_init( void ); +static inline void main_periodic_task( void ); +static inline void main_event_task( void ); +static inline void main_report(void); + +static inline void on_gyro_event(void); +static inline void on_accel_event(void); +static inline void on_mag_event(void); + +int main( void ) { + main_init(); + while(1) { + if (sys_time_periodic()) + main_periodic_task(); + main_event_task(); + } + return 0; +} + +static inline void main_init( void ) { + + mcu_init(); + sys_time_init(); + imu_init(); + ahrs_aligner_init(); + ahrs_init(); + + DEBUG_SERVO1_INIT(); + // DEBUG_SERVO2_INIT(); + + mcu_int_enable(); +} + +static inline void main_periodic_task( void ) { + + if (cpu_time_sec > 1) imu_periodic(); + RunOnceEvery(10, { LED_PERIODIC();}); + main_report(); +} + +static inline void main_event_task( void ) { + + ImuEvent(on_gyro_event, on_accel_event, on_mag_event); + +} + +static inline void on_gyro_event(void) { + ImuScaleGyro(imu); + if (ahrs.status == AHRS_UNINIT) { + ahrs_aligner_run(); + if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED) + ahrs_align(); + } + else { + DEBUG_S1_ON(); + ahrs_propagate(); + DEBUG_S1_OFF(); + } +} + +static inline void on_accel_event(void) { + ImuScaleAccel(imu); + if (ahrs.status != AHRS_UNINIT) { + DEBUG_S2_ON(); + ahrs_update_accel(); + DEBUG_S2_OFF(); + } +} + +static inline void on_mag_event(void) { + ImuScaleMag(imu); + if (ahrs.status == AHRS_RUNNING) { + ahrs_update_mag(); + } +} + + +static inline void main_report(void) { + + PeriodicPrescaleBy10( + { + DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, + &imu.accel_unscaled.x, + &imu.accel_unscaled.y, + &imu.accel_unscaled.z); + }, + { + DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, + &imu.gyro_unscaled.p, + &imu.gyro_unscaled.q, + &imu.gyro_unscaled.r); + }, + { + DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, + &imu.mag_unscaled.x, + &imu.mag_unscaled.y, + &imu.mag_unscaled.z); + }, + { + DOWNLINK_SEND_IMU_ACCEL_SCALED(DefaultChannel, + &imu.accel.x, + &imu.accel.y, + &imu.accel.z); + }, + { + DOWNLINK_SEND_IMU_GYRO_SCALED(DefaultChannel, + &imu.gyro.p, + &imu.gyro.q, + &imu.gyro.r); + }, + + { + DOWNLINK_SEND_IMU_MAG_SCALED(DefaultChannel, + &imu.mag.x, + &imu.mag.y, + &imu.mag.z); + }, + + { + DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM); + }, + { +#ifdef USE_I2C2 + DOWNLINK_SEND_I2C_ERRORS(DefaultChannel, + &i2c2_errors.ack_fail_cnt, + &i2c2_errors.miss_start_stop_cnt, + &i2c2_errors.arb_lost_cnt, + &i2c2_errors.over_under_cnt, + &i2c2_errors.pec_recep_cnt, + &i2c2_errors.timeout_tlow_cnt, + &i2c2_errors.smbus_alert_cnt, + &i2c2_errors.unexpected_event_cnt, + &i2c2_errors.last_unexpected_event); +#endif + }, + { + DOWNLINK_SEND_BOOZ2_AHRS_EULER(DefaultChannel, + &ahrs.ltp_to_imu_euler.phi, + &ahrs.ltp_to_imu_euler.theta, + &ahrs.ltp_to_imu_euler.psi, + &ahrs.ltp_to_body_euler.phi, + &ahrs.ltp_to_body_euler.theta, + &ahrs.ltp_to_body_euler.psi); + }, + { + DOWNLINK_SEND_BOOZ_AHRS_BIAS(DefaultChannel, + &ahrs_impl.gyro_bias.p, + &ahrs_impl.gyro_bias.q, + &ahrs_impl.gyro_bias.r); + + }); +} diff --git a/sw/airborne/test/subsystems/test_imu.c b/sw/airborne/test/subsystems/test_imu.c new file mode 100644 index 0000000000..9591438b63 --- /dev/null +++ b/sw/airborne/test/subsystems/test_imu.c @@ -0,0 +1,159 @@ +/* + * $Id$ + * + * Copyright (C) 2008-2011 The Paparazzi Team + * + * 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. + */ + +#include + +#include "std.h" +#include "mcu.h" +#include "sys_time.h" +#include "led.h" +#include "mcu_periph/uart.h" +#include "messages.h" +#include "downlink.h" + +#include "subsystems/imu.h" + +#include "my_debug_servo.h" + +static inline void main_init( void ); +static inline void main_periodic_task( void ); +static inline void main_event_task( void ); + +static inline void on_gyro_accel_event(void); +static inline void on_accel_event(void); +static inline void on_mag_event(void); + +int main( void ) { + main_init(); + while(1) { + if (sys_time_periodic()) + main_periodic_task(); + main_event_task(); + } + return 0; +} + +static inline void main_init( void ) { + + mcu_init(); + sys_time_init(); + imu_init(); + + // DEBUG_SERVO1_INIT(); + // DEBUG_SERVO2_INIT(); + + + mcu_int_enable(); +} + +static inline void main_periodic_task( void ) { + RunOnceEvery(100, { + // LED_TOGGLE(3); + DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM); + }); +#ifdef USE_I2C2 + RunOnceEvery(111, { + DOWNLINK_SEND_I2C_ERRORS(DefaultChannel, + &i2c2_errors.ack_fail_cnt, + &i2c2_errors.miss_start_stop_cnt, + &i2c2_errors.arb_lost_cnt, + &i2c2_errors.over_under_cnt, + &i2c2_errors.pec_recep_cnt, + &i2c2_errors.timeout_tlow_cnt, + &i2c2_errors.smbus_alert_cnt, + &i2c2_errors.unexpected_event_cnt, + &i2c2_errors.last_unexpected_event); + }); +#endif + if (cpu_time_sec > 1) imu_periodic(); + RunOnceEvery(10, { LED_PERIODIC();}); +} + +static inline void main_event_task( void ) { + + ImuEvent(on_gyro_accel_event, on_accel_event, on_mag_event); + +} + +static inline void on_accel_event(void) { + ImuScaleAccel(imu); + + static uint8_t cnt; + cnt++; + if (cnt > 15) cnt = 0; + if (cnt == 0) { + DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, + &imu.accel_unscaled.x, + &imu.accel_unscaled.y, + &imu.accel_unscaled.z); + } + else if (cnt == 7) { + DOWNLINK_SEND_IMU_ACCEL_SCALED(DefaultChannel, + &imu.accel.x, + &imu.accel.y, + &imu.accel.z); + } +} + +static inline void on_gyro_accel_event(void) { + ImuScaleGyro(imu); + + static uint8_t cnt; + cnt++; + if (cnt > 15) cnt = 0; + + if (cnt == 0) { + DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, + &imu.gyro_unscaled.p, + &imu.gyro_unscaled.q, + &imu.gyro_unscaled.r); + } + else if (cnt == 7) { + DOWNLINK_SEND_IMU_GYRO_SCALED(DefaultChannel, + &imu.gyro.p, + &imu.gyro.q, + &imu.gyro.r); + } +} + + +static inline void on_mag_event(void) { + ImuScaleMag(imu); + static uint8_t cnt; + cnt++; + if (cnt > 10) cnt = 0; + + if (cnt == 0) { + DOWNLINK_SEND_IMU_MAG_SCALED(DefaultChannel, + &imu.mag.x, + &imu.mag.y, + &imu.mag.z); + } + else if (cnt == 5) { + DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, + &imu.mag_unscaled.x, + &imu.mag_unscaled.y, + &imu.mag_unscaled.z); + } +} + diff --git a/sw/airborne/test/subsystems/test_settings.c b/sw/airborne/test/subsystems/test_settings.c new file mode 100644 index 0000000000..1f33f4aad1 --- /dev/null +++ b/sw/airborne/test/subsystems/test_settings.c @@ -0,0 +1,110 @@ +/* + * $Id$ + * + * Copyright (C) 2011 The Paparazzi Team + * + * 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. + */ +#define DATALINK_C + +#include BOARD_CONFIG + +#include "std.h" +#include "mcu.h" +#include "sys_time.h" +#include "downlink.h" +#include "datalink.h" +#include "subsystems/settings.h" + +#include "mcu_periph/uart.h" +#include "messages.h" + +#include "my_debug_servo.h" + +static inline void main_init( void ); +static inline void main_periodic( void ); +static inline void main_event( void ); + + +float setting_a; +float setting_b; +float setting_c; +float setting_d; + +int main( void ) { + main_init(); + while(1) { + if (sys_time_periodic()) + main_periodic(); + main_event(); + } + return 0; +} + + +static inline void main_init( void ) { + + mcu_init(); + sys_time_init(); + settings_init(); + // DEBUG_SERVO2_INIT(); + // LED_ON(1); + // LED_ON(2); + // DEBUG_S4_ON(); + // DEBUG_S5_ON(); + // DEBUG_S6_ON(); + mcu_int_enable(); + +} + +static inline void main_periodic( void ) { + + RunOnceEvery(100, { + DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM); + PeriodicSendDlValue(DefaultChannel); + }); + +} + +static inline void main_event( void ) { + + DatalinkEvent(); + +} + +void dl_parse_msg(void) { + datalink_time = 0; + uint8_t msg_id = dl_buffer[1]; + switch (msg_id) { + + case DL_PING: { + DOWNLINK_SEND_PONG(DefaultChannel); + } + break; + case DL_SETTING: + if(DL_SETTING_ac_id(dl_buffer) == AC_ID) { + uint8_t i = DL_SETTING_index(dl_buffer); + float val = DL_SETTING_value(dl_buffer); + DlSetting(i, val); + DOWNLINK_SEND_DL_VALUE(DefaultChannel, &i, &val); + } + break; + default: + break; + } +} diff --git a/sw/airborne/test/subsystems/test_settings.h b/sw/airborne/test/subsystems/test_settings.h new file mode 100644 index 0000000000..045893f992 --- /dev/null +++ b/sw/airborne/test/subsystems/test_settings.h @@ -0,0 +1,11 @@ +#ifndef TEST_SETTINGS_H +#define TEST_SETTINGS_H + +#include "std.h" + +extern float setting_a; +extern float setting_b; +extern float setting_c; +extern float setting_d; + +#endif diff --git a/sw/airborne/test/test_algebra.c b/sw/airborne/test/test_algebra.c index b8b7ce4d8b..ee67f3dffe 100644 --- a/sw/airborne/test/test_algebra.c +++ b/sw/airborne/test/test_algebra.c @@ -108,7 +108,7 @@ static void test_2(void) { struct Int32Quat quat_i; INT32_QUAT_OF_EULERS(quat_i, euler_i); DISPLAY_INT32_QUAT("quat_i", quat_i); - INT32_QUAT_NORMALISE(quat_i); + INT32_QUAT_NORMALIZE(quat_i); DISPLAY_INT32_QUAT("quat_i_n", quat_i); struct Int32Vect3 v2; @@ -155,7 +155,7 @@ static void test_3(void) { struct Int32Quat b2i_q; INT32_QUAT_OF_EULERS(b2i_q, b2i_e); DISPLAY_INT32_QUAT_AS_EULERS_DEG("b2i_q", b2i_q); - // INT32_QUAT_NORMALISE(b2i_q); + // INT32_QUAT_NORMALIZE(b2i_q); // DISPLAY_INT32_QUAT_AS_EULERS_DEG("b2i_q_n", b2i_q); /* Compute BODY to IMU rotation matrix */ @@ -232,7 +232,7 @@ static void test_4_int(void) { struct Int32Quat _q; INT32_QUAT_OF_EULERS(_q, _e); DISPLAY_INT32_QUAT_AS_EULERS_DEG("quat1 ", _q); - // INT32_QUAT_NORMALISE(_q); + // INT32_QUAT_NORMALIZE(_q); // DISPLAY_INT32_QUAT_2("_q_n", _q); /* back to eulers */ @@ -257,7 +257,7 @@ static void test_4_float(void) { struct FloatQuat q; FLOAT_QUAT_OF_EULERS(q, e); // DISPLAY_FLOAT_QUAT("q", q); - FLOAT_QUAT_NORMALISE(q); + FLOAT_QUAT_NORMALIZE(q); DISPLAY_FLOAT_QUAT("q_n", q); DISPLAY_FLOAT_QUAT_AS_INT("q_n as int", q); /* back to eulers */ diff --git a/sw/airborne/test/test_esc_mkk_simple.c b/sw/airborne/test/test_esc_mkk_simple.c index 232890665a..983f19564b 100644 --- a/sw/airborne/test/test_esc_mkk_simple.c +++ b/sw/airborne/test/test_esc_mkk_simple.c @@ -59,7 +59,6 @@ static inline void main_periodic_task( void ) { trans.buf[0] = 0x04; trans.len_w = 1; trans.slave_addr = 0x58; - trans.stop_after_transmit = TRUE; i2c_submit(&ACTUATORS_MKK_DEV,&trans); LED_PERIODIC(); diff --git a/sw/ground_segment/joystick/input2ivy.ml b/sw/ground_segment/joystick/input2ivy.ml index df2a3b8fcb..2caae99c23 100644 --- a/sw/ground_segment/joystick/input2ivy.ml +++ b/sw/ground_segment/joystick/input2ivy.ml @@ -247,7 +247,8 @@ let eval_expr = fun buttons axis inputs expr -> | Syntax.Call (ident, exprs) | Syntax.CallOperator (ident, exprs) -> eval_call ident (List.map eval exprs) | Syntax.Index _ -> failwith "eval_expr: index" - | Syntax.Field _ -> failwith "eval_expr: Field" in + | Syntax.Field _ -> failwith "eval_expr: Field" + | Syntax.Deref _ -> failwith "eval_expr: deref" in eval expr diff --git a/sw/lib/ocaml/expr_lexer.mll b/sw/lib/ocaml/expr_lexer.mll index 695409d5a9..dbfa4fab10 100644 --- a/sw/lib/ocaml/expr_lexer.mll +++ b/sw/lib/ocaml/expr_lexer.mll @@ -42,6 +42,7 @@ rule token = parse | '}' { RC } | '[' { LB } | ']' { RB } + | "->" { DEREF } | "==" { EQ } | "&&" { AND } | "||" { OR } diff --git a/sw/lib/ocaml/expr_parser.mly b/sw/lib/ocaml/expr_parser.mly index c2f63b4a00..7aa963ce37 100644 --- a/sw/lib/ocaml/expr_parser.mly +++ b/sw/lib/ocaml/expr_parser.mly @@ -29,7 +29,7 @@ open Expr_syntax %token FLOAT %token IDENT %token EOF -%token DOT COMMA SEMICOLON LP RP LC RC LB RB AND COLON OR +%token DOT COMMA SEMICOLON LP RP LC RC LB RB DEREF AND COLON OR %token EQ GT ASSIGN GEQ NOT %token PLUS MINUS %token MULT DIV MOD @@ -39,7 +39,8 @@ open Expr_syntax %left PLUS MINUS %left MULT DIV MOD %nonassoc NOT -%nonassoc UMINUS /* highest precedence */ +%nonassoc UMINUS +%left DEREF /* highest precedence */ %start expression /* the entry point */ %type expression @@ -63,6 +64,7 @@ expression: | FLOAT { Float $1 } | IDENT { Ident $1 } | IDENT DOT IDENT { Field ($1,$3) } + | expression DEREF IDENT { Deref($1, $3) } | IDENT LP Args RP { Call ($1, $3) } | LP expression RP { $2 } | IDENT LB expression RB { Index ($1, $3) } diff --git a/sw/lib/ocaml/expr_syntax.ml b/sw/lib/ocaml/expr_syntax.ml index be15604c8d..5f8f5034b5 100644 --- a/sw/lib/ocaml/expr_syntax.ml +++ b/sw/lib/ocaml/expr_syntax.ml @@ -37,6 +37,7 @@ type expression = | CallOperator of ident * (expression list) | Index of ident * expression | Field of ident * ident + | Deref of expression * ident let c_var_of_ident = fun x -> "_var_" ^ x @@ -55,6 +56,7 @@ let rec sprint = function sprintf "%s(%s)" i (String.concat "," ses) | Index (i,e) -> sprintf "%s[%s]" i (sprint e) | Field (i,f) -> sprintf "%s.%s" i f + | Deref (e,f) -> sprintf "(%s)->%s" (sprint e) f (* Valid functions : FIXME *) let functions = [ @@ -113,3 +115,5 @@ let rec check_expression = fun e -> | Field (i, _field) -> if not (List.mem i variables) then unexpected "ident" i + | Deref (e, _field) -> + check_expression e diff --git a/sw/lib/ocaml/expr_syntax.mli b/sw/lib/ocaml/expr_syntax.mli index 0d266c2354..7cb9619094 100644 --- a/sw/lib/ocaml/expr_syntax.mli +++ b/sw/lib/ocaml/expr_syntax.mli @@ -34,6 +34,7 @@ type expression = | CallOperator of ident * expression list | Index of ident * expression | Field of ident * ident + | Deref of expression * ident val c_var_of_ident : ident -> string (** Encapsulate a user ident into a C variable *) diff --git a/sw/lib/ocaml/papget.ml b/sw/lib/ocaml/papget.ml index 23ed642836..586f428724 100644 --- a/sw/lib/ocaml/papget.ml +++ b/sw/lib/ocaml/papget.ml @@ -92,6 +92,7 @@ let hash_vars = fun expr -> | E.Int _ | E.Float _ -> () | E.Call (_id, list) | E.CallOperator (_id, list) -> List.iter loop list | E.Index (_id, e) -> loop e + | E.Deref (_e, _f) as deref -> fprintf stderr "Warning: Deref operator is not allowed in Papgets expressions (%s)" (E.sprint deref) | E.Field (i, f) -> if not (Hashtbl.mem htable (i,f)) then let msg_obj = new message_field i f in @@ -121,6 +122,7 @@ let eval_expr = fun (extra_functions:(string * (string list -> string)) list) h | E.Call (ident, _l) | E.CallOperator (ident, _l) -> failwith (sprintf "Papget.eval_expr '%s(...)'" ident) | E.Index (ident, _e) -> failwith (sprintf "Papget.eval_expr '%s[...]'" ident) + | E.Deref (_e, _f) as deref -> failwith (sprintf "Papget.eval_expr Deref operator is not allowed in Papgets expressions (%s)" (E.sprint deref)) | E.Field (i, f) -> try (Hashtbl.find h (i,f))#last_value diff --git a/sw/simulator/nps/nps_autopilot_booz.c b/sw/simulator/nps/nps_autopilot_booz.c index 80f3f0bbfd..c82a02c3bf 100644 --- a/sw/simulator/nps/nps_autopilot_booz.c +++ b/sw/simulator/nps/nps_autopilot_booz.c @@ -7,7 +7,7 @@ #include "subsystems/imu.h" #include "subsystems/sensors/baro.h" #include "baro_board.h" -#include "firmwares/rotorcraft/battery.h" +#include "subsystems/electrical.h" #include "actuators/supervision.h" @@ -25,9 +25,9 @@ void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, cha main_init(); #ifdef MAX_BAT_LEVEL - battery_voltage = MAX_BAT_LEVEL * 10; + electrical.vsupply = MAX_BAT_LEVEL * 10; #else - battery_voltage = 111; + electrical.vsupply = 111; #endif } diff --git a/sw/simulator/nps/nps_random.c b/sw/simulator/nps/nps_random.c index 05315d483e..0b7966f545 100644 --- a/sw/simulator/nps/nps_random.c +++ b/sw/simulator/nps/nps_random.c @@ -4,6 +4,8 @@ #include #include + +#if 0 /* * R250 * Kirkpatrick, S., and E. Stoll, 1981; "A Very Fast @@ -26,7 +28,7 @@ static double dr250( void ); static long set_seed(long); //static long get_seed(void); static unsigned long int randlcg(void); - +#endif @@ -37,6 +39,20 @@ void double_vect3_add_gaussian_noise(struct DoubleVect3* vect, struct DoubleVect vect->z += get_gaussian_noise() * std_dev->z; } +void float_vect3_add_gaussian_noise(struct FloatVect3* vect, struct FloatVect3* std_dev) { + vect->x += get_gaussian_noise() * std_dev->x; + vect->y += get_gaussian_noise() * std_dev->y; + vect->z += get_gaussian_noise() * std_dev->z; +} + +void float_rates_add_gaussian_noise(struct FloatRates* vect, struct FloatRates* std_dev) { + vect->p += get_gaussian_noise() * std_dev->p; + vect->q += get_gaussian_noise() * std_dev->q; + vect->r += get_gaussian_noise() * std_dev->r; +} + + + void double_vect3_get_gaussian_noise(struct DoubleVect3* vect, struct DoubleVect3* std_dev) { vect->x = get_gaussian_noise() * std_dev->x; vect->y = get_gaussian_noise() * std_dev->y; @@ -54,11 +70,14 @@ void double_vect3_update_random_walk(struct DoubleVect3* rw, struct DoubleVect3* VECT3_ADD(*rw, drw); } + + + + +#if 0 /* http://www.taygeta.com/random/gaussian.html */ - - double get_gaussian_noise(void) { double x1; @@ -79,10 +98,20 @@ double get_gaussian_noise(void) { else return x2 * w; } +#else +#include +#include +#include +double get_gaussian_noise(void) { + static gsl_rng * r = NULL; + // select random number generator + if (!r) r = gsl_rng_alloc (gsl_rng_mt19937); + return gsl_ran_gaussian(r, 1.); +} +#endif - - +#if 0 /* * R250 * Kirkpatrick, S., and E. Stoll, 1981; "A Very Fast @@ -242,3 +271,4 @@ unsigned long int randlcg() { return seed_val; } +#endif /* 0*/ diff --git a/sw/simulator/nps/nps_random.h b/sw/simulator/nps/nps_random.h index bb430dfc05..e31c1954d7 100644 --- a/sw/simulator/nps/nps_random.h +++ b/sw/simulator/nps/nps_random.h @@ -8,5 +8,10 @@ extern void double_vect3_add_gaussian_noise(struct DoubleVect3* vect, struct Dou extern void double_vect3_get_gaussian_noise(struct DoubleVect3* vect, struct DoubleVect3* std_dev); extern void double_vect3_update_random_walk(struct DoubleVect3* rw, struct DoubleVect3* std_dev, double dt, double thau); +extern void float_vect3_add_gaussian_noise(struct FloatVect3* vect, struct FloatVect3* std_dev); +extern void float_rates_add_gaussian_noise(struct FloatRates* vect, struct FloatRates* std_dev); + + + #endif /* NPS_RANDOM_H */ diff --git a/sw/tools/fp_proc.ml b/sw/tools/fp_proc.ml index 2a2e70dec4..93f37bab4c 100644 --- a/sw/tools/fp_proc.ml +++ b/sw/tools/fp_proc.ml @@ -59,7 +59,8 @@ let subst_expression = fun env e -> | Int _ | Float _ | Field _ -> e | Call (i, es) -> Call (i, List.map sub es) | CallOperator (i, es) -> CallOperator (i, List.map sub es) - | Index (i,e) -> Index (i,sub e) in + | Index (i,e) -> Index (i,sub e) + | Deref (e,f) -> Deref (sub e, f) in sub e diff --git a/sw/tools/gen_settings.ml b/sw/tools/gen_settings.ml index 7bc25f6424..a537587827 100644 --- a/sw/tools/gen_settings.ml +++ b/sw/tools/gen_settings.ml @@ -134,11 +134,67 @@ let print_dl_settings = fun settings -> lprintf "default: return 0.;\n"; lprintf "}\n"; left (); - lprintf "}\n" - - + lprintf "}\n"; + left() + +(* + Generate code for persitent settings +*) +let print_persistent_settings = fun settings -> + let settings = flatten settings [] in + let pers_settings = + List.filter (fun x -> try let _ = Xml.attrib x "persistent" in true with _ -> false) settings in + (* structure declaration *) +(* if List.length pers_settings > 0 then begin *) + lprintf "\n/* Persistent Settings */\n"; + lprintf "struct PersistentSettings {\n"; + right(); + let idx = ref 0 in + List.iter + (fun s -> + let v = ExtXml.attrib s "var" in + lprintf "float s_%d; /* %s */\n" !idx v; incr idx) + pers_settings; + left(); + lprintf "};\n\n"; + lprintf "extern struct PersistentSettings pers_settings;\n\n"; + (* Inline function to store persistent settings *) + idx := 0; + lprintf "static inline void persitent_settings_store( void ) {\n"; + right(); + List.iter + (fun s -> + let v = ExtXml.attrib s "var" in + lprintf "pers_settings.s_%d = %s;\n" !idx v; incr idx) + pers_settings; + left(); + lprintf "}\n\n"; + (* Inline function to load persistent settings *) + idx := 0; + lprintf "static inline void persitent_settings_load( void ) {\n"; + right(); + List.iter + (fun s -> + let v = ExtXml.attrib s "var" in + begin + try + let h = ExtXml.attrib s "handler" and + m = ExtXml.attrib s "module" in + lprintf "%s_%s( pers_settings.s_%d );\n" (Filename.basename m) h !idx ; +(* lprintf "%s = pers_settings.s_%d;\n" v !idx *) (* do we want to set the value too or just call the handler ? *) + with + ExtXml.Error e -> lprintf "%s = pers_settings.s_%d;\n" v !idx + end; + incr idx) + pers_settings; + left(); + lprintf "};\n" +(* end *) +(* + Blaaaaaa2 +*) let calib_mode_of_rc = function "gain_1_up" -> 1, "up" | "gain_1_down" -> 1, "down" @@ -233,6 +289,9 @@ let _ = left (); lprintf "}\n"; print_dl_settings dl_settings; + + print_persistent_settings dl_settings; + finish h_name with Xml.Error e -> prerr_endline (Xml.error e); exit 1