diff --git a/conf/airframes/ENAC/quadrotor/blender.xml b/conf/airframes/ENAC/quadrotor/blender.xml index ee48120890..f772e82372 100644 --- a/conf/airframes/ENAC/quadrotor/blender.xml +++ b/conf/airframes/ENAC/quadrotor/blender.xml @@ -29,6 +29,7 @@ + diff --git a/conf/airframes/ENAC/quadrotor/booz2_g1.xml b/conf/airframes/ENAC/quadrotor/booz2_g1.xml index b8acf7af5e..e8f6fbd90f 100644 --- a/conf/airframes/ENAC/quadrotor/booz2_g1.xml +++ b/conf/airframes/ENAC/quadrotor/booz2_g1.xml @@ -34,6 +34,7 @@ + diff --git a/conf/airframes/ENAC/quadrotor/mkk1.xml b/conf/airframes/ENAC/quadrotor/mkk1.xml index 37229f6b1d..19222336b0 100644 --- a/conf/airframes/ENAC/quadrotor/mkk1.xml +++ b/conf/airframes/ENAC/quadrotor/mkk1.xml @@ -16,6 +16,7 @@ + diff --git a/conf/airframes/ENAC/quadrotor/nova1.xml b/conf/airframes/ENAC/quadrotor/nova1.xml index e894ef65de..edda84a77b 100644 --- a/conf/airframes/ENAC/quadrotor/nova1.xml +++ b/conf/airframes/ENAC/quadrotor/nova1.xml @@ -15,6 +15,7 @@ + diff --git a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml index dc2fddc2bd..e2428d84fe 100644 --- a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml +++ b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml @@ -206,13 +206,12 @@ --> - + @@ -243,13 +242,13 @@ ---> - + --> - + + diff --git a/conf/airframes/Poine/beth.xml b/conf/airframes/Poine/beth.xml index aaa4ec37d5..b542d440ec 100644 --- a/conf/airframes/Poine/beth.xml +++ b/conf/airframes/Poine/beth.xml @@ -142,7 +142,7 @@ main_stm32.srcs += subsystems/imu.c \ arch/$(ARCH)/subsystems/imu/imu_crista_arch.c main_stm32.CFLAGS += -DUSE_DMA1_C4_IRQ -main_stm32.srcs += $(SRC_BOOZ)/booz2_commands.c +main_stm32.srcs += $(SRC_FIRMWARE)/commands.c main_stm32.srcs += firmwares/rotorcraft/actuators/actuators_asctec.c #\ # $(SRC_BOOZ_ARCH)/actuators/actuators_asctec_arch.c diff --git a/conf/airframes/Poine/booz2_a1.xml b/conf/airframes/Poine/booz2_a1.xml index 395583d8e9..098f386727 100644 --- a/conf/airframes/Poine/booz2_a1.xml +++ b/conf/airframes/Poine/booz2_a1.xml @@ -206,6 +206,7 @@ + diff --git a/conf/airframes/Poine/booz2_a7.xml b/conf/airframes/Poine/booz2_a7.xml index 213a7d9510..76468c990c 100644 --- a/conf/airframes/Poine/booz2_a7.xml +++ b/conf/airframes/Poine/booz2_a7.xml @@ -235,6 +235,7 @@ + diff --git a/conf/airframes/Poine/h_hex.xml b/conf/airframes/Poine/h_hex.xml index 478600ab30..e5c5226b4d 100644 --- a/conf/airframes/Poine/h_hex.xml +++ b/conf/airframes/Poine/h_hex.xml @@ -185,6 +185,7 @@ + diff --git a/conf/airframes/TU_Delft/MicrojetBR.xml b/conf/airframes/TU_Delft/MicrojetBR.xml index ee3384716b..3ef28141cd 100644 --- a/conf/airframes/TU_Delft/MicrojetBR.xml +++ b/conf/airframes/TU_Delft/MicrojetBR.xml @@ -5,9 +5,9 @@ - - - + + + @@ -253,7 +253,7 @@ - + diff --git a/conf/airframes/TU_Delft/MicrojetCDW.xml b/conf/airframes/TU_Delft/MicrojetCDW.xml index 34d651058f..5026c40a27 100644 --- a/conf/airframes/TU_Delft/MicrojetCDW.xml +++ b/conf/airframes/TU_Delft/MicrojetCDW.xml @@ -5,7 +5,7 @@ - + @@ -28,7 +28,7 @@ - + @@ -42,7 +42,7 @@
- + @@ -72,18 +72,20 @@ - - + + - - - - + + + + + + - - - + + + @@ -92,7 +94,7 @@ - + @@ -161,7 +163,7 @@ - +
@@ -190,7 +192,7 @@ - + diff --git a/conf/airframes/TU_Delft/skywalker.xml b/conf/airframes/TU_Delft/skywalker.xml new file mode 100644 index 0000000000..78943c5ba4 --- /dev/null +++ b/conf/airframes/TU_Delft/skywalker.xml @@ -0,0 +1,242 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + + + + + + + + +
+ + + +
+ +
+ + + +
+ +
+ + +
+ +
+ + + + + +
+ +
+ + +
+ +
+ + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + +
+ +
+ + + + + + + +
+ +
+ + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/airframes/TU_Delft/yapa_xsens.xml b/conf/airframes/TU_Delft/yapa_xsens.xml index d9e489ef66..f27b129aeb 100644 --- a/conf/airframes/TU_Delft/yapa_xsens.xml +++ b/conf/airframes/TU_Delft/yapa_xsens.xml @@ -1,6 +1,6 @@ - @@ -42,16 +42,21 @@ - -
+ + + - + + @@ -83,6 +88,11 @@ +
+ + +
+
@@ -177,14 +187,17 @@ + + - + + @@ -204,11 +217,11 @@ - + - + - + diff --git a/conf/airframes/UofAdelaide/A1000_BOOZ.xml b/conf/airframes/UofAdelaide/A1000_BOOZ.xml index 66b39fd1ed..d8a7ec2290 100644 --- a/conf/airframes/UofAdelaide/A1000_BOOZ.xml +++ b/conf/airframes/UofAdelaide/A1000_BOOZ.xml @@ -293,6 +293,7 @@ second attempt + diff --git a/conf/airframes/UofAdelaide/A1000_LISA.xml b/conf/airframes/UofAdelaide/A1000_LISA.xml index 36dde5dd6d..3c7e9766cd 100644 --- a/conf/airframes/UofAdelaide/A1000_LISA.xml +++ b/conf/airframes/UofAdelaide/A1000_LISA.xml @@ -202,6 +202,7 @@ + diff --git a/conf/airframes/UofAdelaide/A1000_NOVA.xml b/conf/airframes/UofAdelaide/A1000_NOVA.xml index 9847fa9a0e..a624234444 100644 --- a/conf/airframes/UofAdelaide/A1000_NOVA.xml +++ b/conf/airframes/UofAdelaide/A1000_NOVA.xml @@ -252,6 +252,7 @@ + diff --git a/conf/airframes/booz2_flixr.xml b/conf/airframes/booz2_flixr.xml index 323dc620f4..568821c2ce 100644 --- a/conf/airframes/booz2_flixr.xml +++ b/conf/airframes/booz2_flixr.xml @@ -4,7 +4,7 @@ - + @@ -18,7 +18,8 @@ - + + @@ -107,6 +108,12 @@
+
+ + + + +
diff --git a/conf/airframes/booz2_ppzuav.xml b/conf/airframes/booz2_ppzuav.xml index b0ec00deec..d24d7a156e 100644 --- a/conf/airframes/booz2_ppzuav.xml +++ b/conf/airframes/booz2_ppzuav.xml @@ -208,6 +208,7 @@ + diff --git a/conf/airframes/esden/jt_lisam.xml b/conf/airframes/esden/jt_lisam.xml index f80c519748..bf2dfe8528 100644 --- a/conf/airframes/esden/jt_lisam.xml +++ b/conf/airframes/esden/jt_lisam.xml @@ -12,15 +12,15 @@ - + - - + + @@ -30,17 +30,20 @@
+
+ +
- + - - + +
@@ -49,7 +52,7 @@
- +
@@ -58,6 +61,7 @@ +
@@ -80,41 +84,43 @@
- + + + - - - - + + + + - - - - + + + + - - - + + + - - - + + + @@ -145,6 +151,7 @@
+ @@ -192,6 +199,7 @@ + diff --git a/conf/airframes/esden/lisa_asctec.xml b/conf/airframes/esden/lisa_asctec.xml index b677e8e3cc..532218f44a 100644 --- a/conf/airframes/esden/lisa_asctec.xml +++ b/conf/airframes/esden/lisa_asctec.xml @@ -202,6 +202,7 @@ + diff --git a/conf/airframes/esden/lisa_asctec_aspirin.xml b/conf/airframes/esden/lisa_asctec_aspirin.xml index 6aa143ec58..04f00b91c6 100644 --- a/conf/airframes/esden/lisa_asctec_aspirin.xml +++ b/conf/airframes/esden/lisa_asctec_aspirin.xml @@ -204,6 +204,7 @@ + diff --git a/conf/airframes/esden/lisa_m_pwm.xml b/conf/airframes/esden/lisa_m_pwm.xml index 9d87c99f6a..1e9368344f 100644 --- a/conf/airframes/esden/lisa_m_pwm.xml +++ b/conf/airframes/esden/lisa_m_pwm.xml @@ -225,6 +225,7 @@ + diff --git a/conf/airframes/esden/lisa_pwm_aspirin.xml b/conf/airframes/esden/lisa_pwm_aspirin.xml index 34491a043a..b32a7b33c4 100644 --- a/conf/airframes/esden/lisa_pwm_aspirin.xml +++ b/conf/airframes/esden/lisa_pwm_aspirin.xml @@ -204,6 +204,7 @@ + diff --git a/conf/airframes/esden/synerani_4B.xml b/conf/airframes/esden/synerani_4B.xml index 5146d71f5e..f8ebedbca7 100644 --- a/conf/airframes/esden/synerani_4B.xml +++ b/conf/airframes/esden/synerani_4B.xml @@ -208,6 +208,7 @@ + diff --git a/conf/airframes/flixr_discovery.xml b/conf/airframes/flixr_discovery.xml new file mode 100644 index 0000000000..978c3a456d --- /dev/null +++ b/conf/airframes/flixr_discovery.xml @@ -0,0 +1,366 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + +
+ + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + +
+ + + +
+ + +
+ + +
+ + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + +
+ + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + +
+ + +
+ + +
+ + + + + + + + +
+ + +
+ + + + + +
+ + + +
+ +
+ + + diff --git a/conf/airframes/mm/rotor/qmk1.xml b/conf/airframes/mm/rotor/qmk1.xml index 409ec50983..2e27d93ad0 100644 --- a/conf/airframes/mm/rotor/qmk1.xml +++ b/conf/airframes/mm/rotor/qmk1.xml @@ -203,6 +203,7 @@ + diff --git a/conf/autopilot/fixedwing.xml b/conf/autopilot/fixedwing.xml index 0899eb9b68..f65f6282b1 100644 --- a/conf/autopilot/fixedwing.xml +++ b/conf/autopilot/fixedwing.xml @@ -34,7 +34,7 @@ - + diff --git a/conf/autopilot/lisa_l_test_progs.makefile b/conf/autopilot/lisa_l_test_progs.makefile index 95aef22c04..22f35e0081 100644 --- a/conf/autopilot/lisa_l_test_progs.makefile +++ b/conf/autopilot/lisa_l_test_progs.makefile @@ -631,7 +631,7 @@ test_actuators_mkk.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c test_actuators_mkk.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2 test_actuators_mkk.srcs += downlink.c pprz_transport.c -test_actuators_mkk.srcs += $(SRC_BOOZ)/booz2_commands.c +test_actuators_mkk.srcs += $(SRC_FIRMWARE)/commands.c test_actuators_mkk.srcs += $(SRC_FIRMWARE)/actuators/actuators_mkk.c test_actuators_mkk.CFLAGS += -DACTUATORS_MKK_DEVICE=i2c1 test_actuators_mkk.srcs += $(SRC_FIRMWARE)/actuators/supervision.c @@ -663,7 +663,7 @@ test_actuators_asctecv1.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c test_actuators_asctecv1.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2 test_actuators_asctecv1.srcs += downlink.c pprz_transport.c -test_actuators_asctecv1.srcs += $(SRC_BOOZ)/booz2_commands.c +test_actuators_asctecv1.srcs += $(SRC_FIRMWARE)/commands.c test_actuators_asctecv1.CFLAGS += -DACTUATORS_ASCTEC_DEVICE=i2c1 test_actuators_asctecv1.srcs += $(SRC_FIRMWARE)/actuators/actuators_asctec.c test_actuators_asctecv1.CFLAGS += -DUSE_I2C1 @@ -701,6 +701,52 @@ 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 +# +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_FIRMWARE)/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 + + +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 sw # diff --git a/conf/autopilot/lisa_passthrough.makefile b/conf/autopilot/lisa_passthrough.makefile index a61cf4bfe1..05526f258a 100644 --- a/conf/autopilot/lisa_passthrough.makefile +++ b/conf/autopilot/lisa_passthrough.makefile @@ -61,7 +61,7 @@ stm_passthrough.srcs += $(SRC_LISA)/lisa_overo_link.c \ stm_passthrough.srcs += math/pprz_trig_int.c stm_passthrough.srcs += lisa/plug_sys.c -stm_passthrough.srcs += $(SRC_BOOZ)/booz2_commands.c +stm_passthrough.srcs += $(SRC_FIRMWARE)/commands.c # Radio control # diff --git a/conf/autopilot/obsolete/booz_test_progs.makefile b/conf/autopilot/obsolete/booz_test_progs.makefile index 00fa88a447..0574420200 100644 --- a/conf/autopilot/obsolete/booz_test_progs.makefile +++ b/conf/autopilot/obsolete/booz_test_progs.makefile @@ -197,7 +197,7 @@ test_actuators_mkk.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c test_actuators_mkk.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT) test_actuators_mkk.srcs += downlink.c pprz_transport.c -test_actuators_mkk.srcs += $(SRC_BOOZ)/booz2_commands.c +test_actuators_mkk.srcs += $(SRC_FIRMWARE)/commands.c test_actuators_mkk.srcs += $(SRC_FIRMWARE)/actuators/actuators_mkk.c test_actuators_mkk.CFLAGS += -DACTUATORS_MKK_DEVICE=i2c0 test_actuators_mkk.srcs += $(SRC_FIRMWARE)/actuators/supervision.c diff --git a/conf/autopilot/subsystems/fixedwing/gps_ugear.makefile b/conf/autopilot/obsolete/gps_ugear.makefile similarity index 100% rename from conf/autopilot/subsystems/fixedwing/gps_ugear.makefile rename to conf/autopilot/obsolete/gps_ugear.makefile diff --git a/conf/autopilot/obsolete/lisa_test_progs.makefile b/conf/autopilot/obsolete/lisa_test_progs.makefile index 1a48c121c4..233820d1e6 100644 --- a/conf/autopilot/obsolete/lisa_test_progs.makefile +++ b/conf/autopilot/obsolete/lisa_test_progs.makefile @@ -607,7 +607,7 @@ test_actuators_mkk.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c test_actuators_mkk.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2 test_actuators_mkk.srcs += downlink.c pprz_transport.c -test_actuators_mkk.srcs += $(SRC_BOOZ)/booz2_commands.c +test_actuators_mkk.srcs += $(SRC_FIRMWARE)/commands.c test_actuators_mkk.srcs += $(SRC_FIRMWARE)/actuators/actuators_mkk.c test_actuators_mkk.CFLAGS += -DACTUATORS_MKK_DEVICE=i2c1 -DUSE_TIM2_IRQ #test_actuators_mkk.CFLAGS += -DACTUATORS_ASCTEC_V2_PROTOCOL -DACTUATORS_ASCTEC_DEVICE=i2c1 @@ -640,7 +640,7 @@ test_actuators_asctec.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c test_actuators_asctec.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2 test_actuators_asctec.srcs += downlink.c pprz_transport.c -test_actuators_asctec.srcs += $(SRC_BOOZ)/booz2_commands.c +test_actuators_asctec.srcs += $(SRC_FIRMWARE)/commands.c test_actuators_asctec.srcs += $(SRC_FIRMWARE)/actuators/actuators_asctec.c test_actuators_asctec.CFLAGS += -DACTUATORS_ASCTEC_DEVICE=i2c1 # -DBOOZ_START_DELAY=3 @@ -1100,7 +1100,7 @@ ptw.srcs += peripherals/max1168.c $(SRC_ARCH)/peripherals/max1168_arch.c ptw.srcs += peripherals/ms2100.c $(SRC_ARCH)/peripherals/ms2100_arch.c ptw.srcs += math/pprz_trig_int.c -ptw.srcs += $(SRC_BOOZ)/booz2_commands.c +ptw.srcs += $(SRC_FIRMWARE)/commands.c # Radio control ptw.CFLAGS += -DRADIO_CONTROL diff --git a/conf/autopilot/sitl.makefile b/conf/autopilot/obsolete/sitl.makefile similarity index 100% rename from conf/autopilot/sitl.makefile rename to conf/autopilot/obsolete/sitl.makefile diff --git a/conf/autopilot/sitl_jsbsim.makefile b/conf/autopilot/obsolete/sitl_jsbsim.makefile similarity index 100% rename from conf/autopilot/sitl_jsbsim.makefile rename to conf/autopilot/obsolete/sitl_jsbsim.makefile diff --git a/conf/autopilot/sitl_link_pprz.makefile b/conf/autopilot/obsolete/sitl_link_pprz.makefile similarity index 100% rename from conf/autopilot/sitl_link_pprz.makefile rename to conf/autopilot/obsolete/sitl_link_pprz.makefile diff --git a/conf/autopilot/sitl_link_xbee.makefile b/conf/autopilot/obsolete/sitl_link_xbee.makefile similarity index 100% rename from conf/autopilot/sitl_link_xbee.makefile rename to conf/autopilot/obsolete/sitl_link_xbee.makefile diff --git a/conf/autopilot/rotorcraft.makefile b/conf/autopilot/rotorcraft.makefile index c3506ccafa..c2e3c421c2 100644 --- a/conf/autopilot/rotorcraft.makefile +++ b/conf/autopilot/rotorcraft.makefile @@ -26,8 +26,6 @@ CFG_SHARED=$(PAPARAZZI_SRC)/conf/autopilot/subsystems/shared CFG_ROTORCRAFT=$(PAPARAZZI_SRC)/conf/autopilot/subsystems/rotorcraft -SRC_BOOZ=booz -SRC_BOOZ_ARCH=$(SRC_BOOZ)/arch/$(ARCH) SRC_BOOZ_TEST=$(SRC_BOOZ)/test SRC_BOOZ_PRIV=booz_priv @@ -39,7 +37,7 @@ SRC_ARCH=arch/$(ARCH) CFG_BOOZ=$(PAPARAZZI_SRC)/conf/autopilot/ -ROTORCRAFT_INC = -I$(SRC_FIRMWARE) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) -I$(SRC_BOARD) +ROTORCRAFT_INC = -I$(SRC_FIRMWARE) -I$(SRC_BOARD) ap.ARCHDIR = $(ARCH) @@ -51,6 +49,11 @@ ap.srcs = $(SRC_FIRMWARE)/main.c ap.srcs += mcu.c ap.srcs += $(SRC_ARCH)/mcu_arch.c +# +# Math functions +# +$(TARGET).srcs += math/pprz_geodetic_int.c math/pprz_geodetic_float.c math/pprz_geodetic_double.c math/pprz_trig_int.c + ifeq ($(ARCH), stm32) ap.srcs += lisa/plug_sys.c endif @@ -101,7 +104,7 @@ ap.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c ap.srcs += mcu_periph/i2c.c ap.srcs += $(SRC_ARCH)/mcu_periph/i2c_arch.c -ap.srcs += $(SRC_BOOZ)/booz2_commands.c +ap.srcs += $(SRC_FIRMWARE)/commands.c # # Radio control choice @@ -185,23 +188,14 @@ endif ap.srcs += $(SRC_FIRMWARE)/autopilot.c -ap.srcs += math/pprz_trig_int.c ap.srcs += $(SRC_FIRMWARE)/stabilization.c ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_rate.c - -ap.CFLAGS += -DSTABILISATION_ATTITUDE_TYPE_INT -ap.CFLAGS += -DSTABILISATION_ATTITUDE_H=\"stabilization/stabilization_attitude_int.h\" -ap.CFLAGS += -DSTABILISATION_ATTITUDE_REF_H=\"stabilization/stabilization_attitude_ref_euler_int.h\" -ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_euler_int.c -ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_euler_int.c - ap.CFLAGS += -DUSE_NAVIGATION ap.srcs += $(SRC_FIRMWARE)/guidance/guidance_h.c ap.srcs += $(SRC_FIRMWARE)/guidance/guidance_v.c ap.srcs += $(SRC_SUBSYSTEMS)/ins.c -ap.srcs += math/pprz_geodetic_int.c math/pprz_geodetic_float.c math/pprz_geodetic_double.c # # INS choice @@ -216,7 +210,7 @@ ap.srcs += $(SRC_SUBSYSTEMS)/ins/vf_float.c ap.CFLAGS += -DUSE_VFF -DDT_VFILTER='(1./$(PERIODIC_FREQUENCY).)' ap.srcs += $(SRC_FIRMWARE)/navigation.c - +ap.srcs += subsystems/navigation/common_flight_plan.c # # FMS choice diff --git a/conf/autopilot/rotorcraft.xml b/conf/autopilot/rotorcraft.xml index 93467a93ff..142d0057c6 100644 --- a/conf/autopilot/rotorcraft.xml +++ b/conf/autopilot/rotorcraft.xml @@ -23,5 +23,6 @@ + diff --git a/conf/autopilot/subsystems/fixedwing/ahrs_ic.makefile b/conf/autopilot/subsystems/fixedwing/ahrs_ic.makefile new file mode 100644 index 0000000000..9e945577ee --- /dev/null +++ b/conf/autopilot/subsystems/fixedwing/ahrs_ic.makefile @@ -0,0 +1,42 @@ +# +# AHRS_PROPAGATE_FREQUENCY +# AHRS_H_X +# AHRS_H_Y +# AHRS_H_Z +# + +AHRS_CFLAGS = -DUSE_AHRS -DAHRS_UPDATE_FW_ESTIMATOR +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) + + +# Extra stuff for fixedwings + +ifdef CPU_LED + ap.CFLAGS += -DAHRS_CPU_LED=$(CPU_LED) +endif + +ifdef AHRS_PROPAGATE_FREQUENCY +else + AHRS_PROPAGATE_FREQUENCY = 60 +endif + +ifdef AHRS_CORRECT_FREQUENCY +else + AHRS_CORRECT_FREQUENCY = 60 +endif + +ap.CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=$(AHRS_PROPAGATE_FREQUENCY) +ap.CFLAGS += -DAHRS_CORRECT_FREQUENCY=$(AHRS_CORRECT_FREQUENCY) + diff --git a/conf/autopilot/subsystems/fixedwing/attitude_dcm.makefile b/conf/autopilot/subsystems/fixedwing/attitude_dcm.makefile index 9aa26c0cdf..91b1eb5ecd 100644 --- a/conf/autopilot/subsystems/fixedwing/attitude_dcm.makefile +++ b/conf/autopilot/subsystems/fixedwing/attitude_dcm.makefile @@ -10,7 +10,6 @@ ap.CFLAGS += -DUSE_AHRS ap.srcs += $(SRC_SUBSYSTEMS)/ahrs.c ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_float_dcm.c -ap.srcs += math/pprz_trig_int.c ifdef AHRS_ALIGNER_LED ap.CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) diff --git a/conf/autopilot/subsystems/fixedwing/autopilot.makefile b/conf/autopilot/subsystems/fixedwing/autopilot.makefile index 44b395950e..1df9a246cc 100644 --- a/conf/autopilot/subsystems/fixedwing/autopilot.makefile +++ b/conf/autopilot/subsystems/fixedwing/autopilot.makefile @@ -81,6 +81,11 @@ $(TARGET).srcs += sys_time.c $(TARGET).CFLAGS += -DINTER_MCU $(TARGET).srcs += $(SRC_FIXEDWING)/inter_mcu.c +# +# Math functions +# +$(TARGET).srcs += math/pprz_geodetic_int.c math/pprz_geodetic_float.c math/pprz_geodetic_double.c math/pprz_trig_int.c + ###################################################################### ## ## COMMON FOR ALL NON-SIMULATION TARGETS diff --git a/conf/autopilot/subsystems/fixedwing/gps_nmea.makefile b/conf/autopilot/subsystems/fixedwing/gps_nmea.makefile new file mode 100644 index 0000000000..9479e0289d --- /dev/null +++ b/conf/autopilot/subsystems/fixedwing/gps_nmea.makefile @@ -0,0 +1,23 @@ +# NMEA GPS unit + + +ap.CFLAGS += -DUSE_GPS -DNMEA -DGPS_USE_LATLONG +ap.CFLAGS += -DGPS_LINK=$(GPS_PORT) +ap.CFLAGS += -DUSE_$(GPS_PORT) +ap.CFLAGS += -D$(GPS_PORT)_BAUD=$(GPS_BAUD) + +ifneq ($(GPS_LED),none) + ap.CFLAGS += -DGPS_LED=$(GPS_LED) +endif + +ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_nmea.h\" +ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_nmea.c + +$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c + +sim.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG +sim.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\" +sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c + +jsbsim.CFLAGS += -DUSE_GPS -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\" +jsbsim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c diff --git a/conf/autopilot/subsystems/fixedwing/gps_ublox_lea4p.makefile b/conf/autopilot/subsystems/fixedwing/gps_ublox_lea4p.makefile index 2e3249f679..368d4e76a8 100644 --- a/conf/autopilot/subsystems/fixedwing/gps_ublox_lea4p.makefile +++ b/conf/autopilot/subsystems/fixedwing/gps_ublox_lea4p.makefile @@ -10,7 +10,13 @@ ifneq ($(GPS_LED),none) ap.CFLAGS += -DGPS_LED=$(GPS_LED) endif -ap.srcs += $(SRC_FIXEDWING)/gps_ubx.c +ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_ubx.h\" +ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_ubx.c -$(TARGET).srcs += $(SRC_FIXEDWING)/gps.c $(SRC_FIXEDWING)/latlong.c +$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c +sim.CFLAGS += -DUSE_GPS -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\" +sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c + +jsbsim.CFLAGS += -DUSE_GPS -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\" +jsbsim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c diff --git a/conf/autopilot/subsystems/fixedwing/gps_ublox_lea5h.makefile b/conf/autopilot/subsystems/fixedwing/gps_ublox_lea5h.makefile index de753df714..d8d5723be0 100644 --- a/conf/autopilot/subsystems/fixedwing/gps_ublox_lea5h.makefile +++ b/conf/autopilot/subsystems/fixedwing/gps_ublox_lea5h.makefile @@ -10,6 +10,14 @@ ifneq ($(GPS_LED),none) ap.CFLAGS += -DGPS_LED=$(GPS_LED) endif -ap.srcs += $(SRC_FIXEDWING)/gps_ubx.c +ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_ubx.h\" +ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_ubx.c -$(TARGET).srcs += $(SRC_FIXEDWING)/gps.c $(SRC_FIXEDWING)/latlong.c +$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c + +sim.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG +sim.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\" +sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c + +jsbsim.CFLAGS += -DUSE_GPS -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\" +jsbsim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c diff --git a/conf/autopilot/subsystems/fixedwing/gps_ublox_lea5h_hitl.makefile b/conf/autopilot/subsystems/fixedwing/gps_ublox_lea5h_hitl.makefile index 4cde327f95..40338ab345 100644 --- a/conf/autopilot/subsystems/fixedwing/gps_ublox_lea5h_hitl.makefile +++ b/conf/autopilot/subsystems/fixedwing/gps_ublox_lea5h_hitl.makefile @@ -2,5 +2,5 @@ ap.CFLAGS += -DUSE_GPS -DUBX -DGPS_USE_LATLONG - -ap.srcs += $(SRC_FIXEDWING)/gps_ubx.c $(SRC_FIXEDWING)/gps.c $(SRC_FIXEDWING)/latlong.c +ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_ubx.h\" +ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_ubx.c $(SRC_SUBSYSTEMS)/gps.c diff --git a/conf/autopilot/subsystems/fixedwing/gps_xsens.makefile b/conf/autopilot/subsystems/fixedwing/gps_xsens.makefile index 5042fbd81e..ed0607f397 100644 --- a/conf/autopilot/subsystems/fixedwing/gps_xsens.makefile +++ b/conf/autopilot/subsystems/fixedwing/gps_xsens.makefile @@ -3,6 +3,5 @@ # ap.CFLAGS += -DGPS -ap.srcs += $(SRC_FIXEDWING)/gps_xsens.c $(SRC_FIXEDWING)/gps.c $(SRC_FIXEDWING)/latlong.c +$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c -sim.srcs += $(SRC_FIXEDWING)/gps.c $(SRC_FIXEDWING)/latlong.c diff --git a/conf/autopilot/subsystems/fixedwing/imu_analog.makefile b/conf/autopilot/subsystems/fixedwing/imu_analog.makefile index b2b3e8ca83..dbf35a8755 100644 --- a/conf/autopilot/subsystems/fixedwing/imu_analog.makefile +++ b/conf/autopilot/subsystems/fixedwing/imu_analog.makefile @@ -55,7 +55,6 @@ imu_CFLAGS += -DADC_CHANNEL_ACCEL_X=$(ACCEL_X) -DADC_CHANNEL_ACCEL_Y=$(ACCEL_Y) imu_srcs += $(SRC_SUBSYSTEMS)/imu.c imu_srcs += $(SRC_SUBSYSTEMS)/imu/imu_analog.c -imu_srcs += math/pprz_trig_int.c endif diff --git a/conf/autopilot/subsystems/fixedwing/imu_booz.makefile b/conf/autopilot/subsystems/fixedwing/imu_booz.makefile index e6ad633f17..34dcab210d 100644 --- a/conf/autopilot/subsystems/fixedwing/imu_booz.makefile +++ b/conf/autopilot/subsystems/fixedwing/imu_booz.makefile @@ -52,7 +52,6 @@ imu_srcs += $(SRC_ARCH)/peripherals/max1168_arch.c #ifeq ($(ARCH), lpc21) imu_CFLAGS += -DSSP_VIC_SLOT=9 imu_CFLAGS += -DMAX1168_EOC_VIC_SLOT=8 -#FIXME ms2100 not used on this imu #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) @@ -63,8 +62,6 @@ imu_CFLAGS += -DMAX1168_EOC_VIC_SLOT=8 ap.srcs += $(imu_srcs) ap.CFLAGS += $(imu_CFLAGS) -imu_srcs += math/pprz_trig_int.c - # # Simulator # diff --git a/conf/autopilot/subsystems/fixedwing/navigation.makefile b/conf/autopilot/subsystems/fixedwing/navigation.makefile index b1eb8b1dad..5f699bc858 100644 --- a/conf/autopilot/subsystems/fixedwing/navigation.makefile +++ b/conf/autopilot/subsystems/fixedwing/navigation.makefile @@ -5,6 +5,7 @@ $(TARGET).CFLAGS += -DNAV $(TARGET).srcs += $(SRC_SUBSYSTEMS)/nav.c +$(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/common_flight_plan.c $(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/traffic_info.c $(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/nav_survey_rectangle.c $(SRC_SUBSYSTEMS)/navigation/nav_line.c diff --git a/conf/autopilot/subsystems/fixedwing/navigation_extra.makefile b/conf/autopilot/subsystems/fixedwing/navigation_extra.makefile index 60cc01c1af..0ddcd7ca86 100644 --- a/conf/autopilot/subsystems/fixedwing/navigation_extra.makefile +++ b/conf/autopilot/subsystems/fixedwing/navigation_extra.makefile @@ -5,6 +5,7 @@ $(TARGET).CFLAGS += -DNAV $(TARGET).srcs += $(SRC_SUBSYSTEMS)/nav.c +$(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/common_flight_plan.c $(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/traffic_info.c $(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/nav_survey_rectangle.c $(SRC_SUBSYSTEMS)/navigation/nav_line.c diff --git a/conf/autopilot/subsystems/shared/ahrs_ic.makefile b/conf/autopilot/subsystems/rotorcraft/ahrs_ic.makefile similarity index 72% rename from conf/autopilot/subsystems/shared/ahrs_ic.makefile rename to conf/autopilot/subsystems/rotorcraft/ahrs_ic.makefile index bda8b08365..193d347d13 100644 --- a/conf/autopilot/subsystems/shared/ahrs_ic.makefile +++ b/conf/autopilot/subsystems/rotorcraft/ahrs_ic.makefile @@ -10,9 +10,9 @@ 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 +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) @@ -20,3 +20,4 @@ ap.srcs += $(AHRS_SRCS) sim.CFLAGS += $(AHRS_CFLAGS) sim.srcs += $(AHRS_SRCS) + diff --git a/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile b/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile index 592c196ea3..51e0e4c1ce 100644 --- a/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile +++ b/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile @@ -8,9 +8,6 @@ JSBSIM_ROOT = /opt/jsbsim JSBSIM_INC = $(JSBSIM_ROOT)/include/JSBSim JSBSIM_LIB = $(JSBSIM_ROOT)/lib -SRC_BOOZ=booz -SRC_BOOZ_SIM = $(SRC_BOOZ)/arch/sim - SRC_FIRMWARE=firmwares/rotorcraft SRC_BOARD=boards/$(BOARD) @@ -23,7 +20,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 -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 +sim.CFLAGS += -I$(NPSDIR) -I$(SRC_FIRMWARE) -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 ifndef JSBSIM_PKG @@ -39,7 +36,7 @@ else endif -sim.srcs = $(NPSDIR)/nps_main.c \ +sim.srcs += $(NPSDIR)/nps_main.c \ $(NPSDIR)/nps_fdm_jsbsim.c \ $(NPSDIR)/nps_random.c \ $(NPSDIR)/nps_sensors.c \ @@ -57,11 +54,6 @@ sim.srcs = $(NPSDIR)/nps_main.c \ $(NPSDIR)/nps_flightgear.c \ -sim.srcs += math/pprz_trig_int.c \ - math/pprz_geodetic_float.c \ - math/pprz_geodetic_double.c \ - - sim.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) @@ -82,7 +74,7 @@ sim.srcs += $(SRC_FIRMWARE)/telemetry.c \ downlink.c \ $(SRC_ARCH)/ivy_transport.c -sim.srcs += $(SRC_BOOZ)/booz2_commands.c +sim.srcs += $(SRC_FIRMWARE)/commands.c sim.srcs += $(SRC_FIRMWARE)/datalink.c @@ -152,7 +144,6 @@ endif sim.CFLAGS += -DUSE_NAVIGATION sim.srcs += $(SRC_FIRMWARE)/guidance/guidance_h.c sim.srcs += $(SRC_FIRMWARE)/guidance/guidance_v.c -sim.srcs += math/pprz_geodetic_int.c sim.srcs += $(SRC_SUBSYSTEMS)/ins.c # vertical filter float version @@ -169,3 +160,4 @@ sim.CFLAGS += -DUSE_VFF -DDT_VFILTER='(1./512.)' sim.srcs += $(SRC_FIRMWARE)/navigation.c +sim.srcs += $(SRC_SUBSYSTEMS)/navigation/common_flight_plan.c diff --git a/conf/autopilot/subsystems/rotorcraft/gps_skytraq.makefile b/conf/autopilot/subsystems/rotorcraft/gps_skytraq.makefile index 9a06840e50..3e6049af23 100644 --- a/conf/autopilot/subsystems/rotorcraft/gps_skytraq.makefile +++ b/conf/autopilot/subsystems/rotorcraft/gps_skytraq.makefile @@ -1,7 +1,7 @@ -ap.srcs += $(SRC_BOOZ)/booz_gps.c -ap.CFLAGS += -DBOOZ_GPS_TYPE_H=\"gps/booz_gps_skytraq.h\" -ap.srcs += $(SRC_BOOZ)/gps/booz_gps_skytraq.c +ap.srcs += $(SRC_SUBSYSTEMS)/gps.c +ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_skytraq.h\" +ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_skytraq.c ap.CFLAGS += -DUSE_$(GPS_PORT) -D$(GPS_PORT)_BAUD=$(GPS_BAUD) ap.CFLAGS += -DUSE_GPS -DGPS_LINK=$(GPS_PORT) @@ -11,4 +11,4 @@ ifneq ($(GPS_LED),none) endif sim.CFLAGS += -DUSE_GPS -sim.srcs += $(SRC_BOOZ)/booz_gps.c +sim.srcs += $(SRC_SUBSYSTEMS)/gps.c diff --git a/conf/autopilot/subsystems/rotorcraft/gps_ublox.makefile b/conf/autopilot/subsystems/rotorcraft/gps_ublox.makefile index 784099f03a..14073950e1 100644 --- a/conf/autopilot/subsystems/rotorcraft/gps_ublox.makefile +++ b/conf/autopilot/subsystems/rotorcraft/gps_ublox.makefile @@ -1,7 +1,7 @@ -ap.srcs += $(SRC_BOOZ)/booz_gps.c -ap.CFLAGS += -DBOOZ_GPS_TYPE_H=\"gps/booz_gps_ubx.h\" -ap.srcs += $(SRC_BOOZ)/gps/booz_gps_ubx.c +ap.srcs += $(SRC_SUBSYSTEMS)/gps.c +ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_ubx.h\" +ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_ubx.c ap.CFLAGS += -DUSE_$(GPS_PORT) -D$(GPS_PORT)_BAUD=$(GPS_BAUD) ap.CFLAGS += -DUSE_GPS -DGPS_LINK=$(GPS_PORT) @@ -11,4 +11,6 @@ ifneq ($(GPS_LED),none) endif sim.CFLAGS += -DUSE_GPS -sim.srcs += $(SRC_BOOZ)/booz_gps.c +sim.srcs += $(SRC_SUBSYSTEMS)/gps.c +sim.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\" +sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c diff --git a/conf/autopilot/subsystems/rotorcraft/imu_aspirin.makefile b/conf/autopilot/subsystems/rotorcraft/imu_aspirin.makefile index d328823c55..bcaacda6b3 100644 --- a/conf/autopilot/subsystems/rotorcraft/imu_aspirin.makefile +++ b/conf/autopilot/subsystems/rotorcraft/imu_aspirin.makefile @@ -60,3 +60,7 @@ imu_CFLAGS += -DUSE_I2C2 -DUSE_EXTI9_5_IRQ # see: conf/autopilot/subsystems/lisa_passthrough/imu_b2_v1.1.makefile for example ap.CFLAGS += $(imu_CFLAGS) ap.srcs += $(imu_srcs) + + +sim.CFLAGS += $(imu_CFLAGS) +sim.srcs += $(imu_srcs) diff --git a/conf/autopilot/subsystems/rotorcraft/imu_crista_hmc5843.makefile b/conf/autopilot/subsystems/rotorcraft/imu_crista_hmc5843.makefile new file mode 100644 index 0000000000..bbb1c2ea0a --- /dev/null +++ b/conf/autopilot/subsystems/rotorcraft/imu_crista_hmc5843.makefile @@ -0,0 +1,95 @@ +# +# Rotorcraft IMU crista +# +# +# required xml: +#
+# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +# +#
+# +# + +# imu Crista + +imu_CFLAGS += -DIMU_TYPE_H=\"subsystems/imu/imu_crista.h\" +imu_srcs += $(SRC_SUBSYSTEMS)/imu.c +imu_srcs += $(SRC_SUBSYSTEMS)/imu/imu_crista.c +imu_srcs += $(SRC_ARCH)/subsystems/imu/imu_crista_arch.c + +imu_srcs += peripherals/hmc5843.c +imu_srcs += $(SRC_ARCH)/peripherals/hmc5843_arch.c +imu_srcs += -DUSE_HMC5843 + +ifeq ($(ARCH), lpc21) +imu_CFLAGS += -DUSE_I2C1 +imu_CFLAGS += -DI2C1_SCLL=150 -DI2C1_SCLH=150 -DI2C1_VIC_SLOT=12 -DI2C1_BUF_LEN=16 +else ifeq ($(ARCH), stm32) +imu_CFLAGS += -DUSE_I2C2 -DUSE_EXTI9_5_IRQ +endif + +# Keep CFLAGS/Srcs for imu in separate expression so we can assign it to other targets +# see: conf/autopilot/subsystems/lisa_passthrough/imu_b2_v1.1.makefile for example +ap.CFLAGS += $(imu_CFLAGS) +ap.srcs += $(imu_srcs) + +# +# Simulator +# + +sim.CFLAGS += -DIMU_TYPE_H=\"subsystems/imu/imu_crista.h\" +sim.srcs += $(SRC_SUBSYSTEMS)/imu.c +sim.srcs += $(SRC_SUBSYSTEMS)/imu/imu_crista.c +sim.srcs += $(SRC_ARCH)/subsystems/imu/imu_crista_arch.c + +sim.CFLAGS += -DUSE_AMI601 +sim.srcs += peripherals/ami601.c +sim.CFLAGS += -DUSE_I2C1 diff --git a/conf/autopilot/subsystems/rotorcraft/stabilization_euler.makefile b/conf/autopilot/subsystems/rotorcraft/stabilization_euler.makefile new file mode 100644 index 0000000000..d34d793e8a --- /dev/null +++ b/conf/autopilot/subsystems/rotorcraft/stabilization_euler.makefile @@ -0,0 +1,5 @@ +ap.CFLAGS += -DSTABILISATION_ATTITUDE_TYPE_INT +ap.CFLAGS += -DSTABILISATION_ATTITUDE_H=\"stabilization/stabilization_attitude_int.h\" +ap.CFLAGS += -DSTABILISATION_ATTITUDE_REF_H=\"stabilization/stabilization_attitude_ref_euler_int.h\" +ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_euler_int.c +ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_euler_int.c diff --git a/conf/autopilot/subsystems/rotorcraft/stabilization_quaternion.makefile b/conf/autopilot/subsystems/rotorcraft/stabilization_quaternion.makefile new file mode 100644 index 0000000000..e9d4cc7edd --- /dev/null +++ b/conf/autopilot/subsystems/rotorcraft/stabilization_quaternion.makefile @@ -0,0 +1,6 @@ +ap.CFLAGS += -DSTABILISATION_ATTITUDE_TYPE_INT +ap.CFLAGS += -DSTABILISATION_ATTITUDE_H=\"stabilization/stabilization_attitude_int.h\" +ap.CFLAGS += -DSTABILISATION_ATTITUDE_REF_H=\"stabilization/stabilization_attitude_ref_quat_int.h\" +ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_quat_int.c +ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_quat_int.c +ap.srcs += $(SRC_FIRMWARE)/stabilization/quat_setpoint.c diff --git a/conf/autopilot/subsystems/shared/imu_aspirin.makefile b/conf/autopilot/subsystems/shared/imu_aspirin.makefile index 29d685d4c9..2a9b99ba94 100644 --- a/conf/autopilot/subsystems/shared/imu_aspirin.makefile +++ b/conf/autopilot/subsystems/shared/imu_aspirin.makefile @@ -3,8 +3,8 @@ 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 + $(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 diff --git a/conf/messages.xml b/conf/messages.xml index fc430e03e7..4d44358c05 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -19,15 +19,7 @@ - - - - - - - - - + @@ -48,10 +40,10 @@ - + - + @@ -442,7 +434,7 @@ - + @@ -761,17 +753,53 @@ - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - @@ -1053,7 +1081,7 @@ - + @@ -1116,25 +1144,26 @@ - - - - + + + + - - - - - - - - - - - - + + + + + + + + + + + + + @@ -1260,15 +1289,15 @@ - - - - - - - - - + + + + + + + + + @@ -1582,7 +1611,14 @@ - + + + + + + + + @@ -1870,9 +1906,9 @@ - - - + + + diff --git a/conf/modules/baro_ms5611_i2c.xml b/conf/modules/baro_ms5611_i2c.xml new file mode 100644 index 0000000000..d3bbf48f7e --- /dev/null +++ b/conf/modules/baro_ms5611_i2c.xml @@ -0,0 +1,21 @@ + + + + + +
+ +
+ + + + + + + + + + diff --git a/conf/modules/charge_sens.xml b/conf/modules/charge_sens.xml new file mode 100644 index 0000000000..561b0132db --- /dev/null +++ b/conf/modules/charge_sens.xml @@ -0,0 +1,14 @@ + + + +
+ +
+ + + + + + + + diff --git a/conf/modules/gps_ubx_uart.xml b/conf/modules/gps_ubx_uart.xml new file mode 100644 index 0000000000..55ea724d27 --- /dev/null +++ b/conf/modules/gps_ubx_uart.xml @@ -0,0 +1,22 @@ + + + +
+ +
+ + + + + + + + + + + + + + + + diff --git a/conf/modules/humid_htm_b71.xml b/conf/modules/humid_htm_b71.xml new file mode 100644 index 0000000000..a3e520d880 --- /dev/null +++ b/conf/modules/humid_htm_b71.xml @@ -0,0 +1,19 @@ + + + + + +
+ +
+ + + + + + + + diff --git a/conf/modules/ins_aspirin_via_i2c.xml b/conf/modules/ins_aspirin_via_i2c.xml new file mode 100644 index 0000000000..d5f93cd327 --- /dev/null +++ b/conf/modules/ins_aspirin_via_i2c.xml @@ -0,0 +1,25 @@ + + + + +
+ +
+ + + + + + + + + + + + + + + + + + diff --git a/conf/modules/ins_xsens_MTiG_fixedwing.xml b/conf/modules/ins_xsens_MTiG_fixedwing.xml index 27e67847cd..6ff0911806 100644 --- a/conf/modules/ins_xsens_MTiG_fixedwing.xml +++ b/conf/modules/ins_xsens_MTiG_fixedwing.xml @@ -10,12 +10,17 @@ + + - + + + + diff --git a/conf/modules/mag_hmc5843.xml b/conf/modules/mag_hmc5843.xml new file mode 100644 index 0000000000..c8439d8e3e --- /dev/null +++ b/conf/modules/mag_hmc5843.xml @@ -0,0 +1,19 @@ + + + + +
+ +
+ + + + + + + + + + + + diff --git a/conf/radios/R6107SP_7ch.xml b/conf/radios/R6107SP_7ch.xml new file mode 100644 index 0000000000..1886310cfa --- /dev/null +++ b/conf/radios/R6107SP_7ch.xml @@ -0,0 +1,54 @@ + + + + + + + + + + + + + + + + + diff --git a/conf/settings/basic.xml b/conf/settings/basic.xml index 35991b8d98..64147a71a1 100644 --- a/conf/settings/basic.xml +++ b/conf/settings/basic.xml @@ -25,7 +25,7 @@ - + diff --git a/conf/settings/lisa.xml b/conf/settings/lisa.xml index 9facc3d61b..f717d6034f 100644 --- a/conf/settings/lisa.xml +++ b/conf/settings/lisa.xml @@ -25,7 +25,7 @@ - + diff --git a/conf/settings/tuning.xml b/conf/settings/tuning.xml index 4f83d2daaa..22ac1e1d7c 100644 --- a/conf/settings/tuning.xml +++ b/conf/settings/tuning.xml @@ -19,7 +19,7 @@ - + diff --git a/conf/settings/tuningJH.xml b/conf/settings/tuningJH.xml index f7ccad596f..911ed87aa4 100644 --- a/conf/settings/tuningJH.xml +++ b/conf/settings/tuningJH.xml @@ -18,7 +18,7 @@ - + diff --git a/conf/settings/tuning_basic_ins.xml b/conf/settings/tuning_basic_ins.xml index b052c9be53..0d84278f26 100644 --- a/conf/settings/tuning_basic_ins.xml +++ b/conf/settings/tuning_basic_ins.xml @@ -19,7 +19,7 @@ - + diff --git a/conf/settings/tuning_ins.xml b/conf/settings/tuning_ins.xml index 08250f98b9..2cf8fe880b 100644 --- a/conf/settings/tuning_ins.xml +++ b/conf/settings/tuning_ins.xml @@ -19,7 +19,7 @@ - + diff --git a/conf/settings/tuning_loiter.xml b/conf/settings/tuning_loiter.xml index b2cd6984f4..eb6a88c2fe 100644 --- a/conf/settings/tuning_loiter.xml +++ b/conf/settings/tuning_loiter.xml @@ -19,7 +19,7 @@ - + diff --git a/conf/settings/tuning_tp_auto.xml b/conf/settings/tuning_tp_auto.xml index ba8e605fe3..474c6bd9d7 100644 --- a/conf/settings/tuning_tp_auto.xml +++ b/conf/settings/tuning_tp_auto.xml @@ -18,7 +18,7 @@ - + diff --git a/conf/settings/ugv.xml b/conf/settings/ugv.xml index 71eba1ade6..623d46e027 100644 --- a/conf/settings/ugv.xml +++ b/conf/settings/ugv.xml @@ -17,7 +17,7 @@ - + diff --git a/conf/telemetry/booz_minimal.xml b/conf/telemetry/booz_minimal.xml index 7e8ad038e5..b178ddbb47 100644 --- a/conf/telemetry/booz_minimal.xml +++ b/conf/telemetry/booz_minimal.xml @@ -8,7 +8,7 @@ - + @@ -18,7 +18,7 @@ - + diff --git a/conf/telemetry/telemetry_booz2.xml b/conf/telemetry/telemetry_booz2.xml index 4852249e86..36de95539b 100644 --- a/conf/telemetry/telemetry_booz2.xml +++ b/conf/telemetry/telemetry_booz2.xml @@ -8,13 +8,13 @@ - + - + @@ -88,7 +88,7 @@ - + diff --git a/conf/telemetry/telemetry_jtm.xml b/conf/telemetry/telemetry_jtm.xml new file mode 100644 index 0000000000..2798c506e8 --- /dev/null +++ b/conf/telemetry/telemetry_jtm.xml @@ -0,0 +1,126 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/xsens_MTi-G.xml b/conf/xsens_MTi-G.xml index 4db7955d4c..63bf030e13 100644 --- a/conf/xsens_MTi-G.xml +++ b/conf/xsens_MTi-G.xml @@ -40,6 +40,11 @@ + + + + + @@ -118,6 +123,18 @@ + + + + + + + + + + + + diff --git a/sw/airborne/ap_downlink.h b/sw/airborne/ap_downlink.h index 8a0c7bc43f..f0957d6cbe 100644 --- a/sw/airborne/ap_downlink.h +++ b/sw/airborne/ap_downlink.h @@ -57,18 +57,18 @@ #define PERIODIC_SEND_ALIVE(_chan) DOWNLINK_SEND_ALIVE(_chan, 16, MD5SUM); #define PERIODIC_SEND_BAT(_chan) { \ - uint16_t zero = 0; \ - Downlink({ int16_t e = energy; \ - DOWNLINK_SEND_BAT(_chan, \ - &v_ctl_throttle_slewed, \ - &vsupply, \ - &zero, \ - &estimator_flight_time, \ - &kill_throttle, \ - &block_time, \ - &stage_time, \ - &e); \ - }); \ + uint16_t amps = (int16_t) (current/1000); \ + Downlink({ int16_t e = energy; \ + DOWNLINK_SEND_BAT(_chan, \ + &v_ctl_throttle_slewed, \ + &vsupply, \ + &s, \ + &estimator_flight_time, \ + &kill_throttle, \ + &block_time, \ + &stage_time, \ + &e); \ + }); \ } #ifdef MCU_SPI_LINK @@ -139,24 +139,34 @@ #define PERIODIC_SEND_SEGMENT(_chan) if (nav_in_segment) { DOWNLINK_SEND_SEGMENT(_chan, &nav_segment_x_1, &nav_segment_y_1, &nav_segment_x_2, &nav_segment_y_2); } #ifdef IMU_TYPE_H -#include "subsystems/imu.h" -#define PERIODIC_SEND_IMU_ACCEL_RAW(_chan) { DOWNLINK_SEND_IMU_ACCEL_RAW(_chan, &imu.accel_unscaled.x, &imu.accel_unscaled.y, &imu.accel_unscaled.z)} -#define PERIODIC_SEND_IMU_GYRO_RAW(_chan) { DOWNLINK_SEND_IMU_GYRO_RAW(_chan, &imu.gyro_unscaled.p, &imu.gyro_unscaled.q, &imu.gyro_unscaled.r)} -#define PERIODIC_SEND_IMU_MAG_RAW(_chan) { DOWNLINK_SEND_IMU_MAG_RAW(_chan, &imu.mag_unscaled.x, &imu.mag_unscaled.y, &imu.mag_unscaled.z)} -#define PERIODIC_SEND_IMU_ACCEL(_chan) { struct FloatVect3 accel_float; ACCELS_FLOAT_OF_BFP(accel_float, imu.accel); DOWNLINK_SEND_IMU_ACCEL(_chan, &accel_float.x, &accel_float.y, &accel_float.z)} -#define PERIODIC_SEND_IMU_GYRO(_chan) { struct FloatRates gyro_float; RATES_FLOAT_OF_BFP(gyro_float, imu.gyro); DOWNLINK_SEND_IMU_GYRO(_chan, &gyro_float.p, &gyro_float.q, &gyro_float.r)} +# include "subsystems/imu.h" +# define PERIODIC_SEND_IMU_ACCEL_RAW(_chan) { DOWNLINK_SEND_IMU_ACCEL_RAW(_chan, &imu.accel_unscaled.x, &imu.accel_unscaled.y, &imu.accel_unscaled.z)} +# define PERIODIC_SEND_IMU_GYRO_RAW(_chan) { DOWNLINK_SEND_IMU_GYRO_RAW(_chan, &imu.gyro_unscaled.p, &imu.gyro_unscaled.q, &imu.gyro_unscaled.r)} +# define PERIODIC_SEND_IMU_MAG_RAW(_chan) { DOWNLINK_SEND_IMU_MAG_RAW(_chan, &imu.mag_unscaled.x, &imu.mag_unscaled.y, &imu.mag_unscaled.z)} +# define PERIODIC_SEND_IMU_ACCEL(_chan) { struct FloatVect3 accel_float; ACCELS_FLOAT_OF_BFP(accel_float, imu.accel); DOWNLINK_SEND_IMU_ACCEL(_chan, &accel_float.x, &accel_float.y, &accel_float.z)} +# define PERIODIC_SEND_IMU_GYRO(_chan) { struct FloatRates gyro_float; RATES_FLOAT_OF_BFP(gyro_float, imu.gyro); DOWNLINK_SEND_IMU_GYRO(_chan, &gyro_float.p, &gyro_float.q, &gyro_float.r)} # ifdef USE_MAGNETOMETER # define PERIODIC_SEND_IMU_MAG(_chan) { struct FloatVect3 mag_float; MAGS_FLOAT_OF_BFP(mag_float, imu.mag); DOWNLINK_SEND_IMU_MAG(_chan, &mag_float.x, &mag_float.y, &mag_float.z)} # else -# define PERIODIC_SEND_IMU_MAG_RAW(_chan) {} +# define PERIODIC_SEND_IMU_MAG(_chan) {} # endif #else -#define PERIODIC_SEND_IMU_ACCEL_RAW(_chan) {} -#define PERIODIC_SEND_IMU_GYRO_RAW(_chan) {} -#define PERIODIC_SEND_IMU_MAG_RAW(_chan) {} -#define PERIODIC_SEND_IMU_ACCEL(_chan) {} -#define PERIODIC_SEND_IMU_GYRO(_chan) {} -#define PERIODIC_SEND_IMU_MAG(_chan) {} +# ifdef INS_MODULE_H +# include "modules/ins/ins_module.h" +# define PERIODIC_SEND_IMU_ACCEL_RAW(_chan) {} +# define PERIODIC_SEND_IMU_GYRO_RAW(_chan) {} +# define PERIODIC_SEND_IMU_MAG_RAW(_chan) {} +# define PERIODIC_SEND_IMU_GYRO(_chan) { DOWNLINK_SEND_IMU_GYRO(_chan, &ins_p, &ins_q, &ins_r)} +# define PERIODIC_SEND_IMU_ACCEL(_chan) { DOWNLINK_SEND_IMU_ACCEL(_chan, &ins_ax, &ins_ay, &ins_az)} +# define PERIODIC_SEND_IMU_MAG(_chan) { DOWNLINK_SEND_IMU_MAG(_chan, &ins_mx, &ins_my, &ins_mz)} +# else +# define PERIODIC_SEND_IMU_ACCEL_RAW(_chan) {} +# define PERIODIC_SEND_IMU_GYRO_RAW(_chan) {} +# define PERIODIC_SEND_IMU_MAG_RAW(_chan) {} +# define PERIODIC_SEND_IMU_ACCEL(_chan) {} +# define PERIODIC_SEND_IMU_GYRO(_chan) {} +# define PERIODIC_SEND_IMU_MAG(_chan) {} +# endif #endif #ifdef IMU_ANALOG @@ -189,17 +199,21 @@ #define PERIODIC_SEND_TUNE_ROLL(_chan) DOWNLINK_SEND_TUNE_ROLL(_chan, &estimator_p,&estimator_phi, &h_ctl_roll_setpoint); #if defined USE_GPS || defined SITL || defined USE_GPS_XSENS -#define PERIODIC_SEND_GPS_SOL(_chan) DOWNLINK_SEND_GPS_SOL(_chan, &gps_Pacc, &gps_Sacc, &gps_PDOP, &gps_numSV) +#define PERIODIC_SEND_GPS_SOL(_chan) DOWNLINK_SEND_GPS_SOL(_chan, &gps.pacc, &gps.sacc, &gps.pdop, &gps.num_sv) #endif -/* add by Haiyang Chao for debugging msg used by osam_imu*/ -#if defined UGEAR -#define PERIODIC_SEND_GPS(_chan) DOWNLINK_SEND_GPS(_chan, &gps_mode, &gps_utm_east, &gps_utm_north, &gps_course, &gps_alt, &gps_gspeed,&gps_climb, &gps_week, &gps_itow, &gps_utm_zone, &gps_nb_ovrn) -#define PERIODIC_SEND_GPS_SOL(_chan) DOWNLINK_SEND_GPS_SOL(_chan, &gps_Pacc, &gps_Sacc, &gps_PDOP, &gps_numSV) -#define PERIODIC_SEND_DebugChao(_chan) DOWNLINK_SEND_DebugChao(_chan, &ugear_debug1, &ugear_debug2, &ugear_debug3, &ugear_debug4, &ugear_debug5, &ugear_debug6) -#else -#define PERIODIC_SEND_GPS(_chan) gps_send() -#endif +#define PERIODIC_SEND_GPS(_chan) { \ + static uint8_t i; \ + int16_t climb = -gps.ned_vel.z; \ + int16_t course = (DegOfRad(gps.course)/((int32_t)1e6)); \ + DOWNLINK_SEND_GPS(DefaultChannel, &gps.fix, &gps.utm_pos.east, &gps.utm_pos.north, &course, &gps.lla_pos.alt, &gps.gspeed, &climb, &gps.week, &gps.tow, &gps.utm_pos.zone, &i); \ + if ((gps.fix != GPS_FIX_3D) && (i >= gps.nb_channels)) i = 0; \ + if (i >= gps.nb_channels * 2) i = 0; \ + if (i < gps.nb_channels && gps.svinfos[i].cno > 0) { \ + DOWNLINK_SEND_SVINFO(DefaultChannel, &i, &gps.svinfos[i].svid, &gps.svinfos[i].flags, &gps.svinfos[i].qi, &gps.svinfos[i].cno, &gps.svinfos[i].elev, &gps.svinfos[i].azim); \ + } \ + i++; \ +} #ifdef USE_BARO_MS5534A //#include "baro_MS5534A.h" diff --git a/sw/airborne/arch/lpc21/gps_hw.h b/sw/airborne/arch/lpc21/gps_hw.h deleted file mode 100644 index edf4f713fc..0000000000 --- a/sw/airborne/arch/lpc21/gps_hw.h +++ /dev/null @@ -1,4 +0,0 @@ -#ifndef GPS_HW_H -#define GPS_HW_H - -#endif /* GPS_HW_H */ diff --git a/sw/airborne/arch/lpc21/subsystems/imu/imu_aspirin_arch.c b/sw/airborne/arch/lpc21/subsystems/imu/imu_aspirin_arch.c new file mode 100644 index 0000000000..4e40189133 --- /dev/null +++ b/sw/airborne/arch/lpc21/subsystems/imu/imu_aspirin_arch.c @@ -0,0 +1,78 @@ +#include "subsystems/imu.h" + + +#include "mcu_periph/i2c.h" + + +void imu_aspirin_arch_int_enable(void) +{ +} + +void imu_aspirin_arch_int_disable(void) +{ +} + +void imu_aspirin_arch_init(void) +{ +} + + +void adxl345_write_to_reg(uint8_t addr, uint8_t val) { + +// Adxl345Select(); +// SPI_I2S_SendData(SPI2, addr); +// while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_TXE) == RESET); +// SPI_I2S_SendData(SPI2, val); +// while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_TXE) == RESET); +// while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_BSY) == SET); +// Adxl345Unselect(); + +} + +void adxl345_clear_rx_buf(void) { +} + +//void adxl345_start_reading_data(void) { +//} + +/* + * + * Gyro data ready + * + */ + +void exti15_10_irq_handler(void) { + + /* clear EXTI */ +// if(EXTI_GetITStatus(EXTI_Line14) != RESET) +// EXTI_ClearITPendingBit(EXTI_Line14); + + imu_aspirin.gyro_eoc = TRUE; + imu_aspirin.status = AspirinStatusReadingGyro; + +} + +/* + * + * Accel data ready + * + */ +void exti2_irq_handler(void) { + + /* clear EXTI */ +// if(EXTI_GetITStatus(EXTI_Line2) != RESET) +// EXTI_ClearITPendingBit(EXTI_Line2); + +// adxl345_start_reading_data(); + +} + +/* + * + * Accel end of DMA transfert + * + */ +void dma1_c4_irq_handler(void) { + + imu_aspirin.accel_available = TRUE; +} diff --git a/sw/airborne/arch/lpc21/subsystems/imu/imu_aspirin_arch.h b/sw/airborne/arch/lpc21/subsystems/imu/imu_aspirin_arch.h new file mode 100644 index 0000000000..4345e8b14c --- /dev/null +++ b/sw/airborne/arch/lpc21/subsystems/imu/imu_aspirin_arch.h @@ -0,0 +1,16 @@ +#ifndef IMU_ASPIRIN_ARCH_H +#define IMU_ASPIRIN_ARCH_H + +#include "subsystems/imu.h" + +#include "led.h" + +extern void imu_aspirin_arch_init(void); +extern void imu_aspirin_arch_int_enable(void); +extern void imu_aspirin_arch_int_disable(void); +extern void adxl345_write_to_reg(uint8_t addr, uint8_t val); +extern void adxl345_clear_rx_buf(void); +extern void adxl345_start_reading_data(void); + + +#endif /* IMU_ASPIRIN_ARCH_H */ diff --git a/sw/airborne/arch/lpc21/subsystems/imu/imu_crista_arch.h b/sw/airborne/arch/lpc21/subsystems/imu/imu_crista_arch.h index b78edce613..0f104a46a8 100644 --- a/sw/airborne/arch/lpc21/subsystems/imu/imu_crista_arch.h +++ b/sw/airborne/arch/lpc21/subsystems/imu/imu_crista_arch.h @@ -21,17 +21,16 @@ * Boston, MA 02111-1307, USA. */ -#ifndef IMU_INT_HW_H -#define IMU_INT_HW_H +#ifndef IMU_CRISTA_ARCH_H +#define IMU_CRISTA_ARCH_H #include "std.h" - #define ImuCristaArchPeriodic() { \ ADS8344_start(); \ } extern void ADS8344_start( void ); -#endif /* IMU_INT_HW_H */ +#endif /* IMU_CRISTA_ARCH_H */ diff --git a/sw/airborne/arch/sim/gps_hw.c b/sw/airborne/arch/sim/gps_hw.c deleted file mode 100644 index 4511247803..0000000000 --- a/sw/airborne/arch/sim/gps_hw.c +++ /dev/null @@ -1,39 +0,0 @@ -#include "gps.h" - -/* in gps_ubx.c */ -volatile uint8_t gps_msg_received; -bool_t gps_pos_available; -uint8_t gps_nb_ovrn; - - -uint8_t gps_mode; -uint16_t gps_week; /* weeks */ -uint32_t gps_itow; /* ms */ -int32_t gps_alt; /* cm */ -uint16_t gps_gspeed; /* cm/s */ -int16_t gps_climb; /* cm/s */ -int16_t gps_course; /* decideg */ -int32_t gps_utm_east, gps_utm_north; -uint8_t gps_utm_zone; -int32_t gps_lat, gps_lon; -struct svinfo gps_svinfos[GPS_NB_CHANNELS]; -uint8_t gps_nb_channels = 0; -uint16_t gps_PDOP; -uint32_t gps_Pacc, gps_Sacc; -uint8_t gps_numSV; -uint16_t gps_reset; - - -void parse_gps_msg( void ) {} - -void gps_feed_values(double utm_north, double utm_east, double utm_alt, double gspeed, double course, double climb) { - - gps_utm_north = utm_north; - gps_utm_east = utm_east; - gps_alt = utm_alt; - gps_gspeed = gspeed; - gps_course = course; - gps_climb = climb; - - gps_pos_available = TRUE; -} diff --git a/sw/airborne/arch/sim/gps_hw.h b/sw/airborne/arch/sim/gps_hw.h deleted file mode 100644 index 61e2085b6b..0000000000 --- a/sw/airborne/arch/sim/gps_hw.h +++ /dev/null @@ -1,9 +0,0 @@ -#ifndef GPS_HW_H -#define GPS_HW_H - -#define GpsBuffer() 0 -#define ReadGpsBuffer() {} - -void gps_feed_values(double utm_north, double utm_east, double utm_alt, double gspeed, double course, double climb); - -#endif /* GPS_HW_H */ diff --git a/sw/airborne/arch/sim/jsbsim_gps.c b/sw/airborne/arch/sim/jsbsim_gps.c index 1df8a10cd9..f489ac35a4 100644 --- a/sw/airborne/arch/sim/jsbsim_gps.c +++ b/sw/airborne/arch/sim/jsbsim_gps.c @@ -8,69 +8,59 @@ #include "generated/airframe.h" #include "generated/flight_plan.h" #include "autopilot.h" -#include "gps.h" +#include "subsystems/gps.h" #include "estimator.h" -#include "latlong.h" -#include "subsystems/navigation/common_nav.h" +#include "math/pprz_geodetic_float.h" +#include "math/pprz_geodetic_int.h" -uint8_t gps_mode; -uint16_t gps_week; /* weeks */ -uint32_t gps_itow; /* ms */ -int32_t gps_alt; /* cm */ -uint16_t gps_gspeed; /* cm/s */ -int16_t gps_climb; /* cm/s */ -int16_t gps_course; /* decideg */ -int32_t gps_utm_east, gps_utm_north; -uint8_t gps_utm_zone; -int32_t gps_lat, gps_lon; -struct svinfo gps_svinfos[GPS_NB_CHANNELS]; -uint8_t gps_nb_channels = 0; -uint16_t gps_PDOP; -uint32_t gps_Pacc, gps_Sacc; -uint8_t gps_numSV; -uint16_t gps_reset; +// currently needed to get nav_utm_zone0 +#include "subsystems/navigation/common_nav.h" void sim_use_gps_pos(double lat, double lon, double alt, double course, double gspeed, double climb, double time) { - gps_mode = 3; // Mode 3D - gps_course = DegOfRad(course) * 10.; - gps_alt = alt * 100.; - gps_gspeed = gspeed * 100.; - gps_climb = climb * 100.; - gps_week = 0; // FIXME - gps_itow = time * 1000.; + gps.fix = 3; // Mode 3D + gps.course = course * 1e7; + gps.hmsl = alt * 1000.; + gps.gspeed = gspeed * 100.; + gps.ned_vel.z = -climb * 100.; + gps.week = 0; // FIXME + gps.tow = time * 1000.; - gps_lat = DegOfRad(lat)*1e7; - gps_lon = DegOfRad(lon)*1e7; - latlong_utm_of(lat, lon, nav_utm_zone0); - gps_utm_east = latlong_utm_x * 100; - gps_utm_north = latlong_utm_y * 100; - gps_utm_zone = nav_utm_zone0; + //TODO set alt above ellipsoid and hmsl + + struct LlaCoor_f lla_f; + struct UtmCoor_f utm_f; + lla_f.lat = lat; + lla_f.lon = lon; + utm_f.zone = nav_utm_zone0; + utm_of_lla_f(&utm_f, &lla_f); + LLA_BFP_OF_REAL(gps.lla_pos, lla_f); + gps.utm_pos.east = utm_f.east*100; + gps.utm_pos.north = utm_f.north*100; + gps.utm_pos.zone = nav_utm_zone0; + + gps_available = TRUE; } /** Space vehicle info simulation */ void sim_update_sv(void) { - gps_nb_channels=7; + gps.nb_channels=7; int i; static int time; time++; - for(i = 0; i < gps_nb_channels; i++) { - gps_svinfos[i].svid = 7 + i; - gps_svinfos[i].elev = (cos(((100*i)+time)/100.) + 1) * 45; - gps_svinfos[i].azim = (time/gps_nb_channels + 50 * i) % 360; - gps_svinfos[i].cno = 40 + sin((time+i*10)/100.) * 10.; - gps_svinfos[i].flags = ((time/10) % (i+1) == 0 ? 0x00 : 0x01); - gps_svinfos[i].qi = (int)((time / 1000.) + i) % 8; + for(i = 0; i < gps.nb_channels; i++) { + gps.svinfos[i].svid = 7 + i; + gps.svinfos[i].elev = (cos(((100*i)+time)/100.) + 1) * 45; + gps.svinfos[i].azim = (time/gps.nb_channels + 50 * i) % 360; + gps.svinfos[i].cno = 40 + sin((time+i*10)/100.) * 10.; + gps.svinfos[i].flags = ((time/10) % (i+1) == 0 ? 0x00 : 0x01); + gps.svinfos[i].qi = (int)((time / 1000.) + i) % 8; } - gps_PDOP = gps_Sacc = gps_Pacc = 500+200*sin(time/100.); - gps_numSV = 7; - - gps_verbose_downlink = !launch; - UseGpsPosNoSend(estimator_update_state_gps); - gps_downlink(); + gps.pdop = gps.sacc = gps.pacc = 500+200*sin(time/100.); + gps.num_sv = 7; } diff --git a/sw/airborne/arch/sim/jsbsim_hw.h b/sw/airborne/arch/sim/jsbsim_hw.h index 88ed9404a7..4963f2ffb2 100644 --- a/sw/airborne/arch/sim/jsbsim_hw.h +++ b/sw/airborne/arch/sim/jsbsim_hw.h @@ -32,7 +32,7 @@ #include "inter_mcu.h" #include "firmwares/fixedwing/autopilot.h" #include "estimator.h" -#include "gps.h" +#include "subsystems/gps.h" #include "subsystems/navigation/traffic_info.h" #include "generated/flight_plan.h" #include "generated/airframe.h" @@ -45,7 +45,6 @@ #include "firmwares/fixedwing/main_ap.h" #include "ap_downlink.h" #include "sim_uart.h" -#include "latlong.h" #include "datalink.h" diff --git a/sw/airborne/arch/sim/jsbsim_transport.c b/sw/airborne/arch/sim/jsbsim_transport.c index 1625f6000f..9e8bf97d86 100644 --- a/sw/airborne/arch/sim/jsbsim_transport.c +++ b/sw/airborne/arch/sim/jsbsim_transport.c @@ -2,6 +2,8 @@ #include "jsbsim_hw.h" +#include "math/pprz_geodetic_float.h" + #include #define MOfCm(_x) (((float)(_x))/100.) @@ -47,15 +49,18 @@ void parse_dl_move_wp(char* argv[]) { float a = MOfCm(atoi(argv[5])); /* Computes from (lat, long) in the referenced UTM zone */ - float lat = RadOfDeg((float)(atoi(argv[3]) / 1e7)); - float lon = RadOfDeg((float)(atoi(argv[4]) / 1e7)); - latlong_utm_of(lat, lon, nav_utm_zone0); - nav_move_waypoint(wp_id, latlong_utm_x, latlong_utm_y, a); + struct LlaCoor_f lla; + lla.lat = RadOfDeg((float)(DL_MOVE_WP_lat(dl_buffer) / 1e7)); + lla.lon = RadOfDeg((float)(DL_MOVE_WP_lon(dl_buffer) / 1e7)); + struct UtmCoor_f utm; + utm.zone = nav_utm_zone0; + utm_of_lla_f(&utm, &lla); + nav_move_waypoint(wp_id, utm.east, utm.north, a); /* Waypoint range is limited. Computes the UTM pos back from the relative coordinates */ - latlong_utm_x = waypoints[wp_id].x + nav_utm_east0; - latlong_utm_y = waypoints[wp_id].y + nav_utm_north0; - DOWNLINK_SEND_WP_MOVED(DefaultChannel,&wp_id, &latlong_utm_x, &latlong_utm_y, &a, &nav_utm_zone0); + utm.east = waypoints[wp_id].x + nav_utm_east0; + utm.north = waypoints[wp_id].y + nav_utm_north0; + DOWNLINK_SEND_WP_MOVED(DefaultChannel, &wp_id, &utm.east, &utm.north, &a, &nav_utm_zone0); } diff --git a/sw/airborne/arch/sim/peripherals/hmc5843_arch.c b/sw/airborne/arch/sim/peripherals/hmc5843_arch.c new file mode 100644 index 0000000000..7c3b6c5b95 --- /dev/null +++ b/sw/airborne/arch/sim/peripherals/hmc5843_arch.c @@ -0,0 +1,26 @@ +/* + * 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 "peripherals/hmc5843.h" + +void hmc5843_arch_init( void ) {} + +void hmc5843_read( void ) {} diff --git a/sw/airborne/arch/sim/peripherals/hmc5843_arch.h b/sw/airborne/arch/sim/peripherals/hmc5843_arch.h new file mode 100644 index 0000000000..5934753575 --- /dev/null +++ b/sw/airborne/arch/sim/peripherals/hmc5843_arch.h @@ -0,0 +1,32 @@ +/* + * 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. + */ + +/* + * + * simulator plug for hmc5843 + * + */ + +#ifndef HMC5843_HW_H +#define HMC5843_HW_H + + +#endif /* HMC5843_HW_H */ diff --git a/sw/airborne/arch/sim/sim_ap.c b/sw/airborne/arch/sim/sim_ap.c index f344d8f74f..a37a453baa 100644 --- a/sw/airborne/arch/sim/sim_ap.c +++ b/sw/airborne/arch/sim/sim_ap.c @@ -11,7 +11,7 @@ #include "inter_mcu.h" #include "autopilot.h" #include "estimator.h" -#include "gps.h" +#include "subsystems/gps.h" #include "subsystems/navigation/traffic_info.h" #include "generated/settings.h" #include "subsystems/nav.h" @@ -22,7 +22,6 @@ #include "firmwares/fixedwing/main_ap.h" #include "ap_downlink.h" #include "sim_uart.h" -#include "latlong.h" #include "datalink.h" #include "generated/flight_plan.h" diff --git a/sw/airborne/arch/sim/sim_baro.c b/sw/airborne/arch/sim/sim_baro.c index e117d97a18..923961727b 100644 --- a/sw/airborne/arch/sim/sim_baro.c +++ b/sw/airborne/arch/sim/sim_baro.c @@ -1,7 +1,7 @@ #include #include "estimator.h" #include "subsystems/nav.h" -#include "gps.h" +#include "subsystems/gps.h" #include "baro_MS5534A.h" bool_t alt_baro_enabled; @@ -31,7 +31,7 @@ void baro_MS5534A_send(void) { } void baro_MS5534A_event_task( void ) { - baro_MS5534A_pressure = baro_MS5534A_ground_pressure - (gps_alt/100.-ground_alt) / 0.08 + ((10.*random()) / RAND_MAX); + baro_MS5534A_pressure = baro_MS5534A_ground_pressure - (gps.hmsl/1000.-ground_alt) / 0.08 + ((10.*random()) / RAND_MAX); baro_MS5534A_temp = 10 + estimator_z; baro_MS5534A_available = TRUE; } diff --git a/sw/airborne/arch/sim/sim_gps.c b/sw/airborne/arch/sim/sim_gps.c index 6bfe4c9058..0930c39f34 100644 --- a/sw/airborne/arch/sim/sim_gps.c +++ b/sw/airborne/arch/sim/sim_gps.c @@ -8,76 +8,68 @@ #include "generated/airframe.h" #include "generated/flight_plan.h" #include "autopilot.h" -#include "gps.h" +#include "subsystems/gps.h" #include "estimator.h" -#include "latlong.h" +#include "math/pprz_geodetic_float.h" +#include "math/pprz_geodetic_int.h" + +// currently needed for nav_utm_zone0 #include "subsystems/navigation/common_nav.h" #include -uint8_t gps_mode; -uint16_t gps_week; /* weeks */ -uint32_t gps_itow; /* ms */ -int32_t gps_alt; /* cm */ -uint16_t gps_gspeed; /* cm/s */ -int16_t gps_climb; /* cm/s */ -int16_t gps_course; /* decideg */ -int32_t gps_utm_east, gps_utm_north; -uint8_t gps_utm_zone; -int32_t gps_lat, gps_lon; -struct svinfo gps_svinfos[GPS_NB_CHANNELS]; -uint8_t gps_nb_channels = 0; -uint16_t gps_PDOP; -uint32_t gps_Pacc, gps_Sacc; -uint8_t gps_numSV; -uint16_t gps_reset; - value sim_use_gps_pos(value x, value y, value z, value c, value a, value s, value cl, value t, value m, value lat, value lon) { - gps_mode = (Bool_val(m) ? 3 : 0); - gps_course = DegOfRad(Double_val(c)) * 10.; - gps_alt = Double_val(a) * 100.; - gps_gspeed = Double_val(s) * 100.; - gps_climb = Double_val(cl) * 100.; - gps_week = 0; // FIXME - gps_itow = Double_val(t) * 1000.; + gps.fix = (Bool_val(m) ? 3 : 0); + gps.course = Double_val(c) * 1e7; + gps.hmsl = Double_val(a) * 1000.; + gps.gspeed = Double_val(s) * 100.; + gps.ned_vel.z = -Double_val(cl) * 100.; + gps.week = 0; // FIXME + gps.tow = Double_val(t) * 1000.; + + //TODO set alt above ellipsoid and hmsl #ifdef GPS_USE_LATLONG - gps_lat = DegOfRad(Double_val(lat))*1e7; - gps_lon = DegOfRad(Double_val(lon))*1e7; - latlong_utm_of(RadOfDeg(gps_lat/1e7), RadOfDeg(gps_lon/1e7), nav_utm_zone0); - gps_utm_east = latlong_utm_x * 100; - gps_utm_north = latlong_utm_y * 100; - gps_utm_zone = nav_utm_zone0; + struct LlaCoor_f lla_f; + struct UtmCoor_f utm_f; + lla_f.lat = Double_val(lat); + lla_f.lon = Double_val(lon); + utm_f.zone = nav_utm_zone0; + utm_of_lla_f(&utm_f, &lla_f); + LLA_BFP_OF_REAL(gps.lla_pos, lla_f); + gps.utm_pos.east = utm_f.east*100; + gps.utm_pos.north = utm_f.north*100; + gps.utm_pos.zone = nav_utm_zone0; x = y = z; /* Just to get rid of the "unused arg" warning */ #else // GPS_USE_LATLONG - gps_utm_east = Int_val(x); - gps_utm_north = Int_val(y); - gps_utm_zone = Int_val(z); + gps.utm_pos.east = Int_val(x); + gps.utm_pos.north = Int_val(y); + gps.utm_pos.zone = Int_val(z); lat = lon; /* Just to get rid of the "unused arg" warning */ #endif // GPS_USE_LATLONG /** Space vehicle info simulation */ - gps_nb_channels=7; + gps.nb_channels=7; int i; static int time; time++; - for(i = 0; i < gps_nb_channels; i++) { - gps_svinfos[i].svid = 7 + i; - gps_svinfos[i].elev = (cos(((100*i)+time)/100.) + 1) * 45; - gps_svinfos[i].azim = (time/gps_nb_channels + 50 * i) % 360; - gps_svinfos[i].cno = 40 + sin((time+i*10)/100.) * 10.; - gps_svinfos[i].flags = ((time/10) % (i+1) == 0 ? 0x00 : 0x01); - gps_svinfos[i].qi = (int)((time / 1000.) + i) % 8; + for(i = 0; i < gps.nb_channels; i++) { + gps.svinfos[i].svid = 7 + i; + gps.svinfos[i].elev = (cos(((100*i)+time)/100.) + 1) * 45; + gps.svinfos[i].azim = (time/gps.nb_channels + 50 * i) % 360; + gps.svinfos[i].cno = 40 + sin((time+i*10)/100.) * 10.; + gps.svinfos[i].flags = ((time/10) % (i+1) == 0 ? 0x00 : 0x01); + gps.svinfos[i].qi = (int)((time / 1000.) + i) % 8; } - gps_PDOP = gps_Sacc = gps_Pacc = 500+200*sin(time/100.); - gps_numSV = 7; + gps.pdop = gps.sacc = gps.pacc = 500+200*sin(time/100.); + gps.num_sv = 7; - gps_verbose_downlink = !launch; - UseGpsPosNoSend(estimator_update_state_gps); - gps_downlink(); + //gps_verbose_downlink = !launch; + //gps_downlink(); + gps_available = TRUE; return Val_unit; } diff --git a/sw/airborne/arch/sim/sim_jsbsim.c b/sw/airborne/arch/sim/sim_jsbsim.c deleted file mode 100644 index f81a5bcb41..0000000000 --- a/sw/airborne/arch/sim/sim_jsbsim.c +++ /dev/null @@ -1,69 +0,0 @@ -/* Definitions and declarations required to compile autopilot code on a - i386 architecture */ - -#include -#include -#include -#include -#include -#include -#include "std.h" -#include "inter_mcu.h" -#include "autopilot.h" -#include "estimator.h" -#include "gps.h" -#include "subsystems/navigation/traffic_info.h" -#include "generated/flight_plan.h" -#include "generated/settings.h" -#include "subsystems/nav.h" -#include "firmwares/fixedwing/stabilization/stabilization_attitude.h" -#include "ffirmwares/fixedwing/guidance/guidance_v.h" -#include "subsystems/sensors/infrared.h" -#include "commands.h" -#include "firmwares/fixedwing/main_ap.h" -#include "ap_downlink.h" -#include "sim_uart.h" -#include "latlong.h" -#include "datalink.h" -#include "adc_generic.h" -#include "ppm.h" - - -/* Dummy definitions to replace the ones from the files not compiled in the - simulator */ -uint8_t ir_estim_mode; -uint8_t vertical_mode; -uint8_t inflight_calib_mode; -bool_t rc_event_1, rc_event_2; -uint8_t gps_mode; -uint16_t gps_week; /* weeks */ -uint32_t gps_itow; /* ms */ -int32_t gps_alt; /* cm */ -uint16_t gps_gspeed; /* cm/s */ -int16_t gps_climb; /* cm/s */ -int16_t gps_course; /* decideg */ -int32_t gps_utm_east, gps_utm_north; -uint8_t gps_utm_zone; -int32_t gps_lat, gps_lon; -struct svinfo gps_svinfos[GPS_NB_CHANNELS]; -uint8_t gps_nb_channels = 0; -uint16_t gps_PDOP; -uint32_t gps_Pacc, gps_Sacc; -uint8_t gps_numSV; -uint16_t gps_reset; -uint8_t gps_nb_ovrn, modem_nb_ovrn, link_fbw_fbw_nb_err, link_fbw_nb_err; -float alt_roll_pgain; -float roll_rate_pgain; -bool_t gpio1_status; -uint16_t adc_generic_val1; -uint16_t adc_generic_val2; -uint16_t ppm_pulses[ PPM_NB_PULSES ]; -volatile bool_t ppm_valid; - -uint8_t ac_id; - -void ir_gain_calib(void) {} -void adc_buf_channel(uint8_t adc_channel __attribute__ ((unused)), struct adc_buf* s __attribute__ ((unused)), uint8_t av_nb_sample __attribute__ ((unused))) {} -void ubxsend_cfg_rst(uint16_t bbr __attribute__ ((unused)), uint8_t reset_mode __attribute__ ((unused))) {} -void adc_generic_init( void ) {} -void adc_generic_periodic( void ) {} diff --git a/sw/airborne/arch/sim/subsystems/imu/imu_aspirin_arch.c b/sw/airborne/arch/sim/subsystems/imu/imu_aspirin_arch.c new file mode 100644 index 0000000000..383dd78880 --- /dev/null +++ b/sw/airborne/arch/sim/subsystems/imu/imu_aspirin_arch.c @@ -0,0 +1,58 @@ +/* + * $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 "subsystems/imu.h" + +#include "generated/airframe.h" + +void imu_aspirin_arch_init(void) { + +} + +void imu_periodic(void) { + +} + +#include "nps_sensors.h" + +void imu_feed_gyro_accel(void) { + + /* do something similar to this, fill imu_aspirin.i2c_trans_gyro and + * imu_aspirin.accel_rx_buf + */ + max1168_values[IMU_GYRO_P_CHAN] = sensors.gyro.value.x; + max1168_values[IMU_GYRO_Q_CHAN] = sensors.gyro.value.y; + max1168_values[IMU_GYRO_R_CHAN] = sensors.gyro.value.z; + max1168_values[IMU_ACCEL_X_CHAN] = sensors.accel.value.x; + max1168_values[IMU_ACCEL_Y_CHAN] = sensors.accel.value.y; + max1168_values[IMU_ACCEL_Z_CHAN] = sensors.accel.value.z; + max1168_status = STA_MAX1168_DATA_AVAILABLE; +} + + +void imu_feed_mag(void) { + hmc5843.data.value[IMU_MAG_X_CHAN] = sensors.mag.value.x; + hmc5843.data.value[IMU_MAG_Y_CHAN] = sensors.mag.value.y; + hmc5843.data.value[IMU_MAG_Z_CHAN] = sensors.mag.value.z; + hmc5843.data_available = TRUE; +} diff --git a/sw/airborne/gps_xsens.h b/sw/airborne/arch/sim/subsystems/imu/imu_aspirin_arch.h similarity index 65% rename from sw/airborne/gps_xsens.h rename to sw/airborne/arch/sim/subsystems/imu/imu_aspirin_arch.h index 2395793e5a..ca9896b01a 100644 --- a/sw/airborne/gps_xsens.h +++ b/sw/airborne/arch/sim/subsystems/imu/imu_aspirin_arch.h @@ -1,7 +1,7 @@ /* - * Paparazzi autopilot $Id: gps_ubx.h 4659 2010-03-10 16:55:04Z mmm $ + * $Id$ * - * Copyright (C) 2004-2006 Pascal Brisset, Antoine Drouin + * Copyright (C) 2008-2009 Antoine Drouin * * This file is part of paparazzi. * @@ -19,30 +19,19 @@ * along with paparazzi; see the file COPYING. If not, write to * the Free Software Foundation, 59 Temple Place - Suite 330, * Boston, MA 02111-1307, USA. - * */ -/** \file gps_xsens.h - * \brief XSens GPS +/* * -*/ + * simulator plug for the booz2 v1 imu arch dependant functions + * + */ +#ifndef IMU_ASPIRIN_ARCH_H +#define IMU_ASPIRIN_ARCH_H -#ifndef XSENS_GPS_H -#define XSENS_GPS_H - -extern uint16_t gps_reset; - -#define GPS_NB_CHANNELS 16 - -extern void reset_gps_watchdog(void); +extern void imu_feed_gyro_accel(void); +extern void imu_feed_mag(void); -#define GpsFixValid() (gps_mode > 0) - -#define gps_xsens_Reset(_val) { \ - gps_reset = _val; \ -} - - -#endif +#endif /* IMU_ASPIRIN_ARCH_H */ diff --git a/sw/airborne/arch/stm32/gps_hw.h b/sw/airborne/arch/stm32/gps_hw.h deleted file mode 100644 index edf4f713fc..0000000000 --- a/sw/airborne/arch/stm32/gps_hw.h +++ /dev/null @@ -1,4 +0,0 @@ -#ifndef GPS_HW_H -#define GPS_HW_H - -#endif /* GPS_HW_H */ diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.c index 195970cc20..59dfcf823a 100644 --- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.c +++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.c @@ -14,11 +14,12 @@ static inline void i2c_reset_init(struct i2c_periph *p); #define I2C_BUSY 0x20 #ifdef DEBUG_I2C -#define SPURIOUS_INTERRUPT(_status, _event) { while(1); } -#define OUT_OF_SYNC_STATE_MACHINE(_status, _event) { while(1); } +#define SPURIOUS_INTERRUPT(_periph, _status, _event) { while(1); } +#define OUT_OF_SYNC_STATE_MACHINE(_periph, _status, _event) { while(1); } #else -#define SPURIOUS_INTERRUPT(_status, _event) {} -#define OUT_OF_SYNC_STATE_MACHINE(_status, _event) {} +//#define SPURIOUS_INTERRUPT(_periph, _status, _event) { periph->errors->unexpected_event_cnt++; abort_and_reset(_periph);} +#define SPURIOUS_INTERRUPT(_periph, _status, _event) { if (_status == I2CAddrWrSent) abort_and_reset(_periph);} +#define OUT_OF_SYNC_STATE_MACHINE(_periph, _status, _event) { abort_and_reset(_periph);} #endif #ifdef USE_I2C1 @@ -100,8 +101,9 @@ static inline void on_status_start_requested(struct i2c_periph *periph, struct i periph->status = I2CAddrWrSent; } } - else - SPURIOUS_INTERRUPT(I2CStartRequested, event); + // else + // SPURIOUS_INTERRUPT(periph, I2CStartRequested, event); + // FIXME: this one seems to get called all the time with mkk controllers } /* @@ -129,8 +131,11 @@ static inline void on_status_addr_wr_sent(struct i2c_periph *periph, struct i2c_ } } } - else - SPURIOUS_INTERRUPT(I2CAddrWrSent, event); + else { + SPURIOUS_INTERRUPT(periph, I2CAddrWrSent, event); + // FIXME: this was where the code would break with mkk controllers on april 10 2011 + // now have SPURIOUS_INTERRUPT call abort_and_reset + } } /* @@ -163,7 +168,7 @@ static inline void on_status_sending_byte(struct i2c_periph *periph, struct i2c_ } } else - SPURIOUS_INTERRUPT(I2CSendingByte, event); + SPURIOUS_INTERRUPT(periph, I2CSendingByte, event); } /* @@ -210,7 +215,7 @@ static inline void on_status_addr_rd_sent(struct i2c_periph *periph, struct i2c_ } } else - SPURIOUS_INTERRUPT(I2CAddrRdSent, event); + SPURIOUS_INTERRUPT(periph, I2CAddrRdSent, event); } @@ -226,8 +231,8 @@ static inline void on_status_reading_byte(struct i2c_periph *periph, struct i2c_ trans->buf[periph->idx_buf] = read_byte; periph->idx_buf++; if (periph->idx_buf >= trans->len_r-1) { // We're reading our last byte - I2C_AcknowledgeConfig(periph->reg_addr, DISABLE); // give them a nack once it's done - I2C_GenerateSTOP(periph->reg_addr, ENABLE); // and follow with a stop + I2C_AcknowledgeConfig(periph->reg_addr, DISABLE); // give them a nack once it's done + I2C_GenerateSTOP(periph->reg_addr, ENABLE); // and follow with a stop /* Make sure that the STOP bit is cleared by Hardware */ static __IO uint8_t counter = 0; while ((regs->CR1 & 0x200) == 0x200) { @@ -239,7 +244,7 @@ static inline void on_status_reading_byte(struct i2c_periph *periph, struct i2c_ } // else { something very wrong has happened } } else - SPURIOUS_INTERRUPT(I2CReadingByte, event); + SPURIOUS_INTERRUPT(periph, I2CReadingByte, event); } /* @@ -259,7 +264,7 @@ static inline void on_status_reading_last_byte(struct i2c_periph *periph, struct periph->status = I2CStopRequested; } else - SPURIOUS_INTERRUPT(I2CReadingLastByte, event); + SPURIOUS_INTERRUPT(periph, I2CReadingLastByte, event); } /* @@ -304,7 +309,7 @@ static inline void i2c_event(struct i2c_periph *p, uint32_t event) on_status_restart_requested(p, trans, event); break; default: - OUT_OF_SYNC_STATE_MACHINE(p->status, event); + OUT_OF_SYNC_STATE_MACHINE(p, p->status, event); break; } } @@ -352,59 +357,59 @@ static inline void i2c_hard_reset(struct i2c_periph *p) { I2C_TypeDef *regs = (I2C_TypeDef *) p->reg_addr; - I2C_DeInit(p->reg_addr); + I2C_DeInit(p->reg_addr); GPIO_InitTypeDef GPIO_InitStructure; GPIO_InitStructure.GPIO_Pin = p->scl_pin | p->sda_pin; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_10MHz; - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD; - GPIO_SetBits(GPIOB, p->scl_pin | p->sda_pin); - GPIO_Init(GPIOB, &GPIO_InitStructure); + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD; + GPIO_SetBits(GPIOB, p->scl_pin | p->sda_pin); + GPIO_Init(GPIOB, &GPIO_InitStructure); - while(GPIO_ReadInputDataBit(GPIOB, p->sda_pin) == Bit_RESET) { - // Raise SCL, wait until SCL is high (in case of clock stretching) - GPIO_SetBits(GPIOB, p->scl_pin); - while (GPIO_ReadInputDataBit(GPIOB, p->scl_pin) == Bit_RESET); + while(GPIO_ReadInputDataBit(GPIOB, p->sda_pin) == Bit_RESET) { + // Raise SCL, wait until SCL is high (in case of clock stretching) + GPIO_SetBits(GPIOB, p->scl_pin); + while (GPIO_ReadInputDataBit(GPIOB, p->scl_pin) == Bit_RESET); i2c_delay(); - - // Lower SCL, wait - GPIO_ResetBits(GPIOB, p->scl_pin); + + // Lower SCL, wait + GPIO_ResetBits(GPIOB, p->scl_pin); i2c_delay(); - - // Raise SCL, wait - GPIO_SetBits(GPIOB, p->scl_pin); + + // Raise SCL, wait + GPIO_SetBits(GPIOB, p->scl_pin); i2c_delay(); - } - - // Generate a start condition followed by a stop condition - GPIO_SetBits(GPIOB, p->scl_pin); + } + + // Generate a start condition followed by a stop condition + GPIO_SetBits(GPIOB, p->scl_pin); i2c_delay(); - GPIO_ResetBits(GPIOB, p->sda_pin); + GPIO_ResetBits(GPIOB, p->sda_pin); i2c_delay(); - GPIO_ResetBits(GPIOB, p->sda_pin); + GPIO_ResetBits(GPIOB, p->sda_pin); i2c_delay(); - - // Raise both SCL and SDA and wait for SCL high (in case of clock stretching) - GPIO_SetBits(GPIOB, p->scl_pin | p->sda_pin); - while (GPIO_ReadInputDataBit(GPIOB, p->scl_pin) == Bit_RESET); - - // Wait for SDA to be high - while (GPIO_ReadInputDataBit(GPIOB, p->sda_pin) != Bit_SET); - - // SCL and SDA should be high at this point, bus should be free - // Return the GPIO pins to the alternate function - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD; - GPIO_Init(GPIOB, &GPIO_InitStructure); - - I2C_DeInit(p->reg_addr); - + + // Raise both SCL and SDA and wait for SCL high (in case of clock stretching) + GPIO_SetBits(GPIOB, p->scl_pin | p->sda_pin); + while (GPIO_ReadInputDataBit(GPIOB, p->scl_pin) == Bit_RESET); + + // Wait for SDA to be high + while (GPIO_ReadInputDataBit(GPIOB, p->sda_pin) != Bit_SET); + + // SCL and SDA should be high at this point, bus should be free + // Return the GPIO pins to the alternate function + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD; + GPIO_Init(GPIOB, &GPIO_InitStructure); + + I2C_DeInit(p->reg_addr); + i2c_apply_config(p); - - if (regs->SR2 & I2C_BUSY) { - // Reset the I2C block - I2C_SoftwareResetCmd(p->reg_addr, ENABLE); - I2C_SoftwareResetCmd(p->reg_addr, DISABLE); - } + + if (regs->SR2 & I2C_BUSY) { + // Reset the I2C block + I2C_SoftwareResetCmd(p->reg_addr, ENABLE); + I2C_SoftwareResetCmd(p->reg_addr, DISABLE); + } } static inline void i2c_reset_init(struct i2c_periph *p) @@ -418,7 +423,7 @@ static inline void i2c_reset_init(struct i2c_periph *p) // enable error interrupts I2C_ITConfig(p->reg_addr, I2C_IT_ERR, ENABLE); } -#endif +#endif /* USE_I2C2 */ #ifdef USE_I2C1 @@ -426,28 +431,6 @@ struct i2c_errors i2c1_errors; #include "my_debug_servo.h" -#define I2C1_ABORT_AND_RESET() { \ - struct i2c_transaction* trans2 = i2c1.trans[i2c1.trans_extract_idx]; \ - trans2->status = I2CTransFailed; \ - I2C_ITConfig(I2C1, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE); \ - I2C_Cmd(I2C1, DISABLE); \ - I2C_DeInit(I2C1); \ - I2C_Cmd(I2C1, ENABLE); \ - i2c_apply_config(&i2c1); \ - I2C_ITConfig(I2C1, I2C_IT_ERR, ENABLE); \ - end_of_transaction(&i2c1); \ - } - -// -// I2C1 base 0x40005400 -// -// I2C1 CR1 0x40005400 -// I2C1 CR2 0x40005404 -// -// I2C2 base 0x40005800 -// - - void i2c1_hw_init(void) { i2c1.reg_addr = I2C1; @@ -459,6 +442,7 @@ void i2c1_hw_init(void) { /* zeros error counter */ ZEROS_ERR_COUNTER(i2c1_errors); + /* reset peripheral to default state ( sometimes not achieved on reset :( ) */ I2C_DeInit(I2C1); NVIC_PriorityGroupConfig(NVIC_PriorityGroup_0); @@ -467,14 +451,14 @@ void i2c1_hw_init(void) { /* Configure and enable I2C1 event interrupt -------------------------------*/ NVIC_InitStructure.NVIC_IRQChannel = I2C1_EV_IRQn; NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; - NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; + NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); /* Configure and enable I2C1 err interrupt -------------------------------*/ NVIC_InitStructure.NVIC_IRQChannel = I2C1_ER_IRQn; NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; - NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1; + NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); @@ -494,13 +478,9 @@ void i2c1_hw_init(void) { /* I2C configuration ----------------------------------------------------------*/ - /* I2C Peripheral Enable */ - I2C_Cmd(I2C1, ENABLE); - /* Apply I2C configuration after enabling it */ - i2c_apply_config(&i2c1); - /* Enable I2C1 error interrupts */ - I2C_ITConfig(I2C1, I2C_IT_ERR, ENABLE); + /* Reset and initialize I2C HW */ + i2c_reset_init(&i2c1); } @@ -508,106 +488,13 @@ void i2c1_hw_init(void) { void i2c1_ev_irq_handler(void) { uint32_t event = I2C_GetLastEvent(I2C1); - struct i2c_transaction* trans = i2c1.trans[i2c1.trans_extract_idx]; - switch (event) { - /* EV5 */ - case I2C_EVENT_MASTER_MODE_SELECT: - if (trans->type == I2CTransTx || trans->type == I2CTransTxRx) { - /* Master Transmitter : Send slave Address for write */ - I2C_Send7bitAddress(I2C1, (trans->slave_addr&0xFE), I2C_Direction_Transmitter); - } - else { - /* Master Receiver : Send slave Address for read */ - I2C_Send7bitAddress(I2C1, (trans->slave_addr&0xFE), I2C_Direction_Receiver); - } - break; - - /* Master Transmitter --------------------------------------------------*/ - /* Test on I2C1 EV6 and first EV8 and clear them */ - case I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED: - /* enable empty dr if we have more than one byte to send */ - // if (i2c1_len_w > 1) - I2C_ITConfig(I2C1, I2C_IT_BUF, ENABLE); - /* Send the first data */ - I2C_SendData(I2C1, trans->buf[0]); - i2c1.idx_buf = 1; - break; - - /* Test on I2C1 EV8 and clear it */ - case I2C_EVENT_MASTER_BYTE_TRANSMITTING: /* Without BTF, EV8 */ - if(i2c1.idx_buf < trans->len_w) { - I2C_SendData(I2C1, trans->buf[i2c1.idx_buf]); - i2c1.idx_buf++; - } - else { - I2C_GenerateSTOP(I2C1, ENABLE); - // I2C_GenerateSTART(I2C1, ENABLE); - I2C_ITConfig(I2C1, I2C_IT_BUF, DISABLE); - } - break; - - case I2C_EVENT_MASTER_BYTE_TRANSMITTED: /* With BTF EV8-2 */ - if(i2c1.idx_buf < trans->len_w) { - I2C_SendData(I2C1, trans->buf[i2c1.idx_buf]); - i2c1.idx_buf++; - } - else { - trans->status = I2CTransSuccess; - I2C_ITConfig(I2C1, I2C_IT_EVT, DISABLE); - end_of_transaction(&i2c1); - } - // while (I2C_GetFlagStatus(I2C1, I2C_FLAG_MSL)); - break; - - default: - i2c1_errors.unexpected_event_cnt++; - i2c1_errors.last_unexpected_event = event; - // spurious Interrupt - // I have already had I2C_EVENT_SLAVE_STOP_DETECTED ( 0x10 ) - // let's clear that by restarting I2C - // if (event == I2C_EVENT_SLAVE_STOP_DETECTED) { - // ok....? let's try that - I2C1_ABORT_AND_RESET(); - break; - } + i2c_event(&i2c1, event); } void i2c1_er_irq_handler(void) { - - if (I2C_GetITStatus(I2C1, I2C_IT_AF)) { /* Acknowledge failure */ - i2c1_errors.ack_fail_cnt++; - I2C_ClearITPendingBit(I2C1, I2C_IT_AF); - I2C_GenerateSTOP(I2C1, ENABLE); - } - if (I2C_GetITStatus(I2C1, I2C_IT_BERR)) { /* Misplaced Start or Stop condition */ - i2c1_errors.miss_start_stop_cnt++; - I2C_ClearITPendingBit(I2C1, I2C_IT_BERR); - } - if (I2C_GetITStatus(I2C1, I2C_IT_ARLO)) { /* Arbitration lost */ - i2c1_errors.arb_lost_cnt++; - I2C_ClearITPendingBit(I2C1, I2C_IT_ARLO); - } - if (I2C_GetITStatus(I2C1, I2C_IT_OVR)) { /* Overrun/Underrun */ - i2c1_errors.over_under_cnt++; - I2C_ClearITPendingBit(I2C1, I2C_IT_OVR); - } - if (I2C_GetITStatus(I2C1, I2C_IT_PECERR)) { /* PEC Error in reception */ - i2c1_errors.pec_recep_cnt++; - I2C_ClearITPendingBit(I2C1, I2C_IT_PECERR); - } - if (I2C_GetITStatus(I2C1, I2C_IT_TIMEOUT)) { /* Timeout or Tlow error */ - i2c1_errors.timeout_tlow_cnt++; - I2C_ClearITPendingBit(I2C1, I2C_IT_TIMEOUT); - } - if (I2C_GetITStatus(I2C1, I2C_IT_SMBALERT)) { /* SMBus alert */ - i2c1_errors.smbus_alert_cnt++; - I2C_ClearITPendingBit(I2C1, I2C_IT_SMBALERT); - } - - I2C1_ABORT_AND_RESET(); - + i2c_error(&i2c1); } #endif /* USE_I2C1 */ @@ -642,7 +529,7 @@ void i2c2_hw_init(void) { /* zeros error counter */ ZEROS_ERR_COUNTER(i2c2_errors); - /* reset periphearl to default state ( sometimes not achieved on reset :( ) */ + /* reset peripheral to default state ( sometimes not achieved on reset :( ) */ I2C_DeInit(I2C2); NVIC_PriorityGroupConfig(NVIC_PriorityGroup_0); @@ -651,14 +538,14 @@ void i2c2_hw_init(void) { /* Configure and enable I2C2 event interrupt --------------------------------*/ NVIC_InitStructure.NVIC_IRQChannel = I2C2_EV_IRQn; NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; - NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1; + NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); /* Configure and enable I2C2 err interrupt ----------------------------------*/ NVIC_InitStructure.NVIC_IRQChannel = I2C2_ER_IRQn; NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; - NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; + NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); @@ -724,7 +611,6 @@ bool_t i2c_submit(struct i2c_periph* p, struct i2c_transaction* t) { static void start_transaction(struct i2c_periph* p) { p->idx_buf = 0; p->status = I2CStartRequested; - // I2C_ZERO_EVENTS(); I2C_ITConfig(p->reg_addr, I2C_IT_EVT, ENABLE); I2C_GenerateSTART(p->reg_addr, ENABLE); } diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.new.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.old.c similarity index 73% rename from sw/airborne/arch/stm32/mcu_periph/i2c_arch.new.c rename to sw/airborne/arch/stm32/mcu_periph/i2c_arch.old.c index 59dfcf823a..195970cc20 100644 --- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.new.c +++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.old.c @@ -14,12 +14,11 @@ static inline void i2c_reset_init(struct i2c_periph *p); #define I2C_BUSY 0x20 #ifdef DEBUG_I2C -#define SPURIOUS_INTERRUPT(_periph, _status, _event) { while(1); } -#define OUT_OF_SYNC_STATE_MACHINE(_periph, _status, _event) { while(1); } +#define SPURIOUS_INTERRUPT(_status, _event) { while(1); } +#define OUT_OF_SYNC_STATE_MACHINE(_status, _event) { while(1); } #else -//#define SPURIOUS_INTERRUPT(_periph, _status, _event) { periph->errors->unexpected_event_cnt++; abort_and_reset(_periph);} -#define SPURIOUS_INTERRUPT(_periph, _status, _event) { if (_status == I2CAddrWrSent) abort_and_reset(_periph);} -#define OUT_OF_SYNC_STATE_MACHINE(_periph, _status, _event) { abort_and_reset(_periph);} +#define SPURIOUS_INTERRUPT(_status, _event) {} +#define OUT_OF_SYNC_STATE_MACHINE(_status, _event) {} #endif #ifdef USE_I2C1 @@ -101,9 +100,8 @@ static inline void on_status_start_requested(struct i2c_periph *periph, struct i periph->status = I2CAddrWrSent; } } - // else - // SPURIOUS_INTERRUPT(periph, I2CStartRequested, event); - // FIXME: this one seems to get called all the time with mkk controllers + else + SPURIOUS_INTERRUPT(I2CStartRequested, event); } /* @@ -131,11 +129,8 @@ static inline void on_status_addr_wr_sent(struct i2c_periph *periph, struct i2c_ } } } - else { - SPURIOUS_INTERRUPT(periph, I2CAddrWrSent, event); - // FIXME: this was where the code would break with mkk controllers on april 10 2011 - // now have SPURIOUS_INTERRUPT call abort_and_reset - } + else + SPURIOUS_INTERRUPT(I2CAddrWrSent, event); } /* @@ -168,7 +163,7 @@ static inline void on_status_sending_byte(struct i2c_periph *periph, struct i2c_ } } else - SPURIOUS_INTERRUPT(periph, I2CSendingByte, event); + SPURIOUS_INTERRUPT(I2CSendingByte, event); } /* @@ -215,7 +210,7 @@ static inline void on_status_addr_rd_sent(struct i2c_periph *periph, struct i2c_ } } else - SPURIOUS_INTERRUPT(periph, I2CAddrRdSent, event); + SPURIOUS_INTERRUPT(I2CAddrRdSent, event); } @@ -231,8 +226,8 @@ static inline void on_status_reading_byte(struct i2c_periph *periph, struct i2c_ trans->buf[periph->idx_buf] = read_byte; periph->idx_buf++; if (periph->idx_buf >= trans->len_r-1) { // We're reading our last byte - I2C_AcknowledgeConfig(periph->reg_addr, DISABLE); // give them a nack once it's done - I2C_GenerateSTOP(periph->reg_addr, ENABLE); // and follow with a stop + I2C_AcknowledgeConfig(periph->reg_addr, DISABLE); // give them a nack once it's done + I2C_GenerateSTOP(periph->reg_addr, ENABLE); // and follow with a stop /* Make sure that the STOP bit is cleared by Hardware */ static __IO uint8_t counter = 0; while ((regs->CR1 & 0x200) == 0x200) { @@ -244,7 +239,7 @@ static inline void on_status_reading_byte(struct i2c_periph *periph, struct i2c_ } // else { something very wrong has happened } } else - SPURIOUS_INTERRUPT(periph, I2CReadingByte, event); + SPURIOUS_INTERRUPT(I2CReadingByte, event); } /* @@ -264,7 +259,7 @@ static inline void on_status_reading_last_byte(struct i2c_periph *periph, struct periph->status = I2CStopRequested; } else - SPURIOUS_INTERRUPT(periph, I2CReadingLastByte, event); + SPURIOUS_INTERRUPT(I2CReadingLastByte, event); } /* @@ -309,7 +304,7 @@ static inline void i2c_event(struct i2c_periph *p, uint32_t event) on_status_restart_requested(p, trans, event); break; default: - OUT_OF_SYNC_STATE_MACHINE(p, p->status, event); + OUT_OF_SYNC_STATE_MACHINE(p->status, event); break; } } @@ -357,59 +352,59 @@ static inline void i2c_hard_reset(struct i2c_periph *p) { I2C_TypeDef *regs = (I2C_TypeDef *) p->reg_addr; - I2C_DeInit(p->reg_addr); + I2C_DeInit(p->reg_addr); GPIO_InitTypeDef GPIO_InitStructure; GPIO_InitStructure.GPIO_Pin = p->scl_pin | p->sda_pin; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_10MHz; - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD; - GPIO_SetBits(GPIOB, p->scl_pin | p->sda_pin); - GPIO_Init(GPIOB, &GPIO_InitStructure); + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD; + GPIO_SetBits(GPIOB, p->scl_pin | p->sda_pin); + GPIO_Init(GPIOB, &GPIO_InitStructure); - while(GPIO_ReadInputDataBit(GPIOB, p->sda_pin) == Bit_RESET) { - // Raise SCL, wait until SCL is high (in case of clock stretching) - GPIO_SetBits(GPIOB, p->scl_pin); - while (GPIO_ReadInputDataBit(GPIOB, p->scl_pin) == Bit_RESET); + while(GPIO_ReadInputDataBit(GPIOB, p->sda_pin) == Bit_RESET) { + // Raise SCL, wait until SCL is high (in case of clock stretching) + GPIO_SetBits(GPIOB, p->scl_pin); + while (GPIO_ReadInputDataBit(GPIOB, p->scl_pin) == Bit_RESET); i2c_delay(); - - // Lower SCL, wait - GPIO_ResetBits(GPIOB, p->scl_pin); + + // Lower SCL, wait + GPIO_ResetBits(GPIOB, p->scl_pin); i2c_delay(); - - // Raise SCL, wait - GPIO_SetBits(GPIOB, p->scl_pin); + + // Raise SCL, wait + GPIO_SetBits(GPIOB, p->scl_pin); i2c_delay(); - } - - // Generate a start condition followed by a stop condition - GPIO_SetBits(GPIOB, p->scl_pin); + } + + // Generate a start condition followed by a stop condition + GPIO_SetBits(GPIOB, p->scl_pin); i2c_delay(); - GPIO_ResetBits(GPIOB, p->sda_pin); + GPIO_ResetBits(GPIOB, p->sda_pin); i2c_delay(); - GPIO_ResetBits(GPIOB, p->sda_pin); + GPIO_ResetBits(GPIOB, p->sda_pin); i2c_delay(); - - // Raise both SCL and SDA and wait for SCL high (in case of clock stretching) - GPIO_SetBits(GPIOB, p->scl_pin | p->sda_pin); - while (GPIO_ReadInputDataBit(GPIOB, p->scl_pin) == Bit_RESET); - - // Wait for SDA to be high - while (GPIO_ReadInputDataBit(GPIOB, p->sda_pin) != Bit_SET); - - // SCL and SDA should be high at this point, bus should be free - // Return the GPIO pins to the alternate function - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD; - GPIO_Init(GPIOB, &GPIO_InitStructure); - - I2C_DeInit(p->reg_addr); - + + // Raise both SCL and SDA and wait for SCL high (in case of clock stretching) + GPIO_SetBits(GPIOB, p->scl_pin | p->sda_pin); + while (GPIO_ReadInputDataBit(GPIOB, p->scl_pin) == Bit_RESET); + + // Wait for SDA to be high + while (GPIO_ReadInputDataBit(GPIOB, p->sda_pin) != Bit_SET); + + // SCL and SDA should be high at this point, bus should be free + // Return the GPIO pins to the alternate function + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD; + GPIO_Init(GPIOB, &GPIO_InitStructure); + + I2C_DeInit(p->reg_addr); + i2c_apply_config(p); - - if (regs->SR2 & I2C_BUSY) { - // Reset the I2C block - I2C_SoftwareResetCmd(p->reg_addr, ENABLE); - I2C_SoftwareResetCmd(p->reg_addr, DISABLE); - } + + if (regs->SR2 & I2C_BUSY) { + // Reset the I2C block + I2C_SoftwareResetCmd(p->reg_addr, ENABLE); + I2C_SoftwareResetCmd(p->reg_addr, DISABLE); + } } static inline void i2c_reset_init(struct i2c_periph *p) @@ -423,7 +418,7 @@ static inline void i2c_reset_init(struct i2c_periph *p) // enable error interrupts I2C_ITConfig(p->reg_addr, I2C_IT_ERR, ENABLE); } -#endif /* USE_I2C2 */ +#endif #ifdef USE_I2C1 @@ -431,6 +426,28 @@ struct i2c_errors i2c1_errors; #include "my_debug_servo.h" +#define I2C1_ABORT_AND_RESET() { \ + struct i2c_transaction* trans2 = i2c1.trans[i2c1.trans_extract_idx]; \ + trans2->status = I2CTransFailed; \ + I2C_ITConfig(I2C1, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE); \ + I2C_Cmd(I2C1, DISABLE); \ + I2C_DeInit(I2C1); \ + I2C_Cmd(I2C1, ENABLE); \ + i2c_apply_config(&i2c1); \ + I2C_ITConfig(I2C1, I2C_IT_ERR, ENABLE); \ + end_of_transaction(&i2c1); \ + } + +// +// I2C1 base 0x40005400 +// +// I2C1 CR1 0x40005400 +// I2C1 CR2 0x40005404 +// +// I2C2 base 0x40005800 +// + + void i2c1_hw_init(void) { i2c1.reg_addr = I2C1; @@ -442,7 +459,6 @@ void i2c1_hw_init(void) { /* zeros error counter */ ZEROS_ERR_COUNTER(i2c1_errors); - /* reset peripheral to default state ( sometimes not achieved on reset :( ) */ I2C_DeInit(I2C1); NVIC_PriorityGroupConfig(NVIC_PriorityGroup_0); @@ -451,14 +467,14 @@ void i2c1_hw_init(void) { /* Configure and enable I2C1 event interrupt -------------------------------*/ NVIC_InitStructure.NVIC_IRQChannel = I2C1_EV_IRQn; NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; - NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1; + NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); /* Configure and enable I2C1 err interrupt -------------------------------*/ NVIC_InitStructure.NVIC_IRQChannel = I2C1_ER_IRQn; NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; - NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; + NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); @@ -478,9 +494,13 @@ void i2c1_hw_init(void) { /* I2C configuration ----------------------------------------------------------*/ + /* I2C Peripheral Enable */ + I2C_Cmd(I2C1, ENABLE); + /* Apply I2C configuration after enabling it */ + i2c_apply_config(&i2c1); - /* Reset and initialize I2C HW */ - i2c_reset_init(&i2c1); + /* Enable I2C1 error interrupts */ + I2C_ITConfig(I2C1, I2C_IT_ERR, ENABLE); } @@ -488,13 +508,106 @@ void i2c1_hw_init(void) { void i2c1_ev_irq_handler(void) { uint32_t event = I2C_GetLastEvent(I2C1); - i2c_event(&i2c1, event); + struct i2c_transaction* trans = i2c1.trans[i2c1.trans_extract_idx]; + switch (event) { + /* EV5 */ + case I2C_EVENT_MASTER_MODE_SELECT: + if (trans->type == I2CTransTx || trans->type == I2CTransTxRx) { + /* Master Transmitter : Send slave Address for write */ + I2C_Send7bitAddress(I2C1, (trans->slave_addr&0xFE), I2C_Direction_Transmitter); + } + else { + /* Master Receiver : Send slave Address for read */ + I2C_Send7bitAddress(I2C1, (trans->slave_addr&0xFE), I2C_Direction_Receiver); + } + break; + + /* Master Transmitter --------------------------------------------------*/ + /* Test on I2C1 EV6 and first EV8 and clear them */ + case I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED: + /* enable empty dr if we have more than one byte to send */ + // if (i2c1_len_w > 1) + I2C_ITConfig(I2C1, I2C_IT_BUF, ENABLE); + /* Send the first data */ + I2C_SendData(I2C1, trans->buf[0]); + i2c1.idx_buf = 1; + break; + + /* Test on I2C1 EV8 and clear it */ + case I2C_EVENT_MASTER_BYTE_TRANSMITTING: /* Without BTF, EV8 */ + if(i2c1.idx_buf < trans->len_w) { + I2C_SendData(I2C1, trans->buf[i2c1.idx_buf]); + i2c1.idx_buf++; + } + else { + I2C_GenerateSTOP(I2C1, ENABLE); + // I2C_GenerateSTART(I2C1, ENABLE); + I2C_ITConfig(I2C1, I2C_IT_BUF, DISABLE); + } + break; + + case I2C_EVENT_MASTER_BYTE_TRANSMITTED: /* With BTF EV8-2 */ + if(i2c1.idx_buf < trans->len_w) { + I2C_SendData(I2C1, trans->buf[i2c1.idx_buf]); + i2c1.idx_buf++; + } + else { + trans->status = I2CTransSuccess; + I2C_ITConfig(I2C1, I2C_IT_EVT, DISABLE); + end_of_transaction(&i2c1); + } + // while (I2C_GetFlagStatus(I2C1, I2C_FLAG_MSL)); + break; + + default: + i2c1_errors.unexpected_event_cnt++; + i2c1_errors.last_unexpected_event = event; + // spurious Interrupt + // I have already had I2C_EVENT_SLAVE_STOP_DETECTED ( 0x10 ) + // let's clear that by restarting I2C + // if (event == I2C_EVENT_SLAVE_STOP_DETECTED) { + // ok....? let's try that + I2C1_ABORT_AND_RESET(); + break; + } } void i2c1_er_irq_handler(void) { - i2c_error(&i2c1); + + if (I2C_GetITStatus(I2C1, I2C_IT_AF)) { /* Acknowledge failure */ + i2c1_errors.ack_fail_cnt++; + I2C_ClearITPendingBit(I2C1, I2C_IT_AF); + I2C_GenerateSTOP(I2C1, ENABLE); + } + if (I2C_GetITStatus(I2C1, I2C_IT_BERR)) { /* Misplaced Start or Stop condition */ + i2c1_errors.miss_start_stop_cnt++; + I2C_ClearITPendingBit(I2C1, I2C_IT_BERR); + } + if (I2C_GetITStatus(I2C1, I2C_IT_ARLO)) { /* Arbitration lost */ + i2c1_errors.arb_lost_cnt++; + I2C_ClearITPendingBit(I2C1, I2C_IT_ARLO); + } + if (I2C_GetITStatus(I2C1, I2C_IT_OVR)) { /* Overrun/Underrun */ + i2c1_errors.over_under_cnt++; + I2C_ClearITPendingBit(I2C1, I2C_IT_OVR); + } + if (I2C_GetITStatus(I2C1, I2C_IT_PECERR)) { /* PEC Error in reception */ + i2c1_errors.pec_recep_cnt++; + I2C_ClearITPendingBit(I2C1, I2C_IT_PECERR); + } + if (I2C_GetITStatus(I2C1, I2C_IT_TIMEOUT)) { /* Timeout or Tlow error */ + i2c1_errors.timeout_tlow_cnt++; + I2C_ClearITPendingBit(I2C1, I2C_IT_TIMEOUT); + } + if (I2C_GetITStatus(I2C1, I2C_IT_SMBALERT)) { /* SMBus alert */ + i2c1_errors.smbus_alert_cnt++; + I2C_ClearITPendingBit(I2C1, I2C_IT_SMBALERT); + } + + I2C1_ABORT_AND_RESET(); + } #endif /* USE_I2C1 */ @@ -529,7 +642,7 @@ void i2c2_hw_init(void) { /* zeros error counter */ ZEROS_ERR_COUNTER(i2c2_errors); - /* reset peripheral to default state ( sometimes not achieved on reset :( ) */ + /* reset periphearl to default state ( sometimes not achieved on reset :( ) */ I2C_DeInit(I2C2); NVIC_PriorityGroupConfig(NVIC_PriorityGroup_0); @@ -538,14 +651,14 @@ void i2c2_hw_init(void) { /* Configure and enable I2C2 event interrupt --------------------------------*/ NVIC_InitStructure.NVIC_IRQChannel = I2C2_EV_IRQn; NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; - NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; + NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); /* Configure and enable I2C2 err interrupt ----------------------------------*/ NVIC_InitStructure.NVIC_IRQChannel = I2C2_ER_IRQn; NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; - NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1; + NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); @@ -611,6 +724,7 @@ bool_t i2c_submit(struct i2c_periph* p, struct i2c_transaction* t) { static void start_transaction(struct i2c_periph* p) { p->idx_buf = 0; p->status = I2CStartRequested; + // I2C_ZERO_EVENTS(); I2C_ITConfig(p->reg_addr, I2C_IT_EVT, ENABLE); I2C_GenerateSTART(p->reg_addr, ENABLE); } diff --git a/sw/airborne/arch/stm32/subsystems/imu/imu_crista_arch.h b/sw/airborne/arch/stm32/subsystems/imu/imu_crista_arch.h index f45b33a638..7cd1f5acba 100644 --- a/sw/airborne/arch/stm32/subsystems/imu/imu_crista_arch.h +++ b/sw/airborne/arch/stm32/subsystems/imu/imu_crista_arch.h @@ -20,6 +20,7 @@ * the Free Software Foundation, 59 Temple Place - Suite 330, * Boston, MA 02111-1307, USA. */ + #ifndef IMU_CRISTA_ARCH_H #define IMU_CRISTA_ARCH_H diff --git a/sw/airborne/booz/test/booz2_test_gps.c b/sw/airborne/booz/test/booz2_test_gps.c index a85f6f30a0..fa6f92a64d 100644 --- a/sw/airborne/booz/test/booz2_test_gps.c +++ b/sw/airborne/booz/test/booz2_test_gps.c @@ -27,7 +27,7 @@ #include "mcu.h" #include "sys_time.h" #include "downlink.h" -#include "booz_gps.h" +#include "subsystems/gps.h" #include "interrupt_hw.h" static inline void main_init( void ); @@ -50,7 +50,7 @@ static inline void main_init( void ) { mcu_init(); sys_time_init(); led_init(); - booz_gps_init(); + gps_init(); mcu_int_enable(); } @@ -61,27 +61,27 @@ static inline void main_periodic_task( void ) { } static inline void main_event_task( void ) { - BoozGpsEvent(on_gps_sol); + GpsEvent(on_gps_sol); } static void on_gps_sol(void) { - DOWNLINK_SEND_BOOZ2_GPS( DefaultChannel, - &booz_gps_state.ecef_pos.x, - &booz_gps_state.ecef_pos.y, - &booz_gps_state.ecef_pos.z, - &booz_gps_state.lla_pos.lat, - &booz_gps_state.lla_pos.lon, - &booz_gps_state.lla_pos.alt, - &booz_gps_state.ecef_vel.x, - &booz_gps_state.ecef_vel.y, - &booz_gps_state.ecef_vel.z, - &booz_gps_state.pacc, - &booz_gps_state.sacc, - &booz_gps_state.tow, - &booz_gps_state.pdop, - &booz_gps_state.num_sv, - &booz_gps_state.fix); + DOWNLINK_SEND_GPS_INT( DefaultChannel, + &gps.ecef_pos.x, + &gps.ecef_pos.y, + &gps.ecef_pos.z, + &gps.lla_pos.lat, + &gps.lla_pos.lon, + &gps.lla_pos.alt, + &gps.ecef_vel.x, + &gps.ecef_vel.y, + &gps.ecef_vel.z, + &gps.pacc, + &gps.sacc, + &gps.tow, + &gps.pdop, + &gps.num_sv, + &gps.fix); } diff --git a/sw/airborne/csc/mercury_ap.c b/sw/airborne/csc/mercury_ap.c index ead159baa5..2450122ea1 100644 --- a/sw/airborne/csc/mercury_ap.c +++ b/sw/airborne/csc/mercury_ap.c @@ -127,7 +127,7 @@ void csc_ap_init(void) { } -// adapted from booz2_commands.h - cb6e74ae259a2384046f431c35735dc8018c0ecd +// adapted from commands.h - cb6e74ae259a2384046f431c35735dc8018c0ecd // mmt -- 06/16/09 const pprz_t csc_ap_commands_failsafe[COMMANDS_NB] = COMMANDS_FAILSAFE; diff --git a/sw/airborne/estimator.c b/sw/airborne/estimator.c index 4cc3220602..72b1ea4c8a 100644 --- a/sw/airborne/estimator.c +++ b/sw/airborne/estimator.c @@ -32,7 +32,7 @@ #include "estimator.h" #include "mcu_periph/uart.h" #include "ap_downlink.h" -#include "gps.h" +#include "subsystems/gps.h" #include "subsystems/nav.h" #ifdef EXTRA_DOWNLINK_DEVICE #include "core/extra_pprz_dl.h" @@ -205,8 +205,8 @@ void alt_kalman(float gps_z) { #endif // ALT_KALMAN void estimator_update_state_gps( void ) { - float gps_east = gps_utm_east / 100.; - float gps_north = gps_utm_north / 100.; + float gps_east = gps.utm_pos.east / 100.; + float gps_north = gps.utm_pos.north / 100.; /* Relative position to reference */ gps_east -= nav_utm_east0; @@ -214,12 +214,12 @@ void estimator_update_state_gps( void ) { EstimatorSetPosXY(gps_east, gps_north); #ifndef USE_BARO_ETS - float falt = gps_alt / 100.; + float falt = gps.hmsl / 1000.; EstimatorSetAlt(falt); #endif - float fspeed = gps_gspeed / 100.; - float fclimb = gps_climb / 100.; - float fcourse = RadOfDeg(gps_course / 10.); + float fspeed = gps.gspeed / 100.; + float fclimb = -gps.ned_vel.z / 100.; + float fcourse = gps.course / 1e7; EstimatorSetSpeedPol(fspeed, fcourse, fclimb); // Heading estimator from wind-information, usually computed with -DWIND_INFO diff --git a/sw/airborne/firmwares/beth/main_stm32.c b/sw/airborne/firmwares/beth/main_stm32.c index af09ab6a0a..61978e26a0 100644 --- a/sw/airborne/firmwares/beth/main_stm32.c +++ b/sw/airborne/firmwares/beth/main_stm32.c @@ -28,7 +28,7 @@ #include "mcu_periph/can.h" #include "sys_time.h" #include "downlink.h" -#include "booz/booz2_commands.h" +#include "firmwares/rotorcraft/commands.h" #include "firmwares/rotorcraft/actuators.h" //#include "booz/booz_radio_control.h" #include "subsystems/imu.h" @@ -69,8 +69,8 @@ static inline void main_init( void ) { imu_init(); overo_link_init(); bench_sensors_init(); - booz2_commands[COMMAND_ROLL] = 0; - booz2_commands[COMMAND_YAW] = 0; + commands[COMMAND_ROLL] = 0; + commands[COMMAND_YAW] = 0; } #define PITCH_MAGIC_NUMBER (121) @@ -102,14 +102,14 @@ static inline void main_periodic( void ) { //stop the motors if we've lost SPI or CAN link //If SPI has had CRC error we don't stop motors if ((spi_msg_cnt == 0) || (can_err_flags != 0)) { - booz2_commands[COMMAND_PITCH] = 0; - booz2_commands[COMMAND_THRUST] = 0; + commands[COMMAND_PITCH] = 0; + commands[COMMAND_THRUST] = 0; actuators_set(FALSE); overo_link.up.msg.can_errs = can_err_flags; overo_link.up.msg.pitch_out = PITCH_MAGIC_NUMBER; } else { - booz2_commands[COMMAND_PITCH] = pitch_out; - booz2_commands[COMMAND_THRUST] = thrust_out; + commands[COMMAND_PITCH] = pitch_out; + commands[COMMAND_THRUST] = thrust_out; actuators_set(TRUE); } } diff --git a/sw/airborne/firmwares/beth/overo_test_uart.c b/sw/airborne/firmwares/beth/overo_test_uart.c index 4b9b584a2a..60d871d922 100644 --- a/sw/airborne/firmwares/beth/overo_test_uart.c +++ b/sw/airborne/firmwares/beth/overo_test_uart.c @@ -31,7 +31,7 @@ #include -#include "gps.h" +#include "subsystems/gps.h" #include "messages2.h" //#include "dl_protocol2.h" diff --git a/sw/airborne/firmwares/beth/rcv_telemetry.c b/sw/airborne/firmwares/beth/rcv_telemetry.c index 72b1d92359..7248878cba 100755 --- a/sw/airborne/firmwares/beth/rcv_telemetry.c +++ b/sw/airborne/firmwares/beth/rcv_telemetry.c @@ -38,7 +38,6 @@ #include #include "subsystems/navigation/common_nav.h" #include "generated/settings.h" -#include "latlong.h" #ifndef DOWNLINK_DEVICE diff --git a/sw/airborne/firmwares/fixedwing/datalink.c b/sw/airborne/firmwares/fixedwing/datalink.c index 00c3c879a7..106156e564 100644 --- a/sw/airborne/firmwares/fixedwing/datalink.c +++ b/sw/airborne/firmwares/fixedwing/datalink.c @@ -49,14 +49,13 @@ #endif #ifdef HITL -#include "gps.h" +#include "subsystems/gps.h" #endif #include "subsystems/navigation/common_nav.h" #include "generated/settings.h" -#include "latlong.h" - +#include "math/pprz_geodetic_float.h" #ifndef DOWNLINK_DEVICE #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE @@ -115,16 +114,19 @@ void dl_parse_msg(void) { float a = MOfCm(DL_MOVE_WP_alt(dl_buffer)); /* Computes from (lat, long) in the referenced UTM zone */ - float lat = RadOfDeg((float)(DL_MOVE_WP_lat(dl_buffer) / 1e7)); - float lon = RadOfDeg((float)(DL_MOVE_WP_lon(dl_buffer) / 1e7)); - latlong_utm_of(lat, lon, nav_utm_zone0); - nav_move_waypoint(wp_id, latlong_utm_x, latlong_utm_y, a); + struct LlaCoor_f lla; + lla.lat = RadOfDeg((float)(DL_MOVE_WP_lat(dl_buffer) / 1e7)); + lla.lon = RadOfDeg((float)(DL_MOVE_WP_lon(dl_buffer) / 1e7)); + struct UtmCoor_f utm; + utm.zone = nav_utm_zone0; + utm_of_lla_f(&utm, &lla); + nav_move_waypoint(wp_id, utm.east, utm.north, a); /* Waypoint range is limited. Computes the UTM pos back from the relative coordinates */ - latlong_utm_x = waypoints[wp_id].x + nav_utm_east0; - latlong_utm_y = waypoints[wp_id].y + nav_utm_north0; - DOWNLINK_SEND_WP_MOVED(DefaultChannel, &wp_id, &latlong_utm_x, &latlong_utm_y, &a, &nav_utm_zone0); + utm.east = waypoints[wp_id].x + nav_utm_east0; + utm.north = waypoints[wp_id].y + nav_utm_north0; + DOWNLINK_SEND_WP_MOVED(DefaultChannel, &wp_id, &utm.east, &utm.north, &a, &nav_utm_zone0); } else if (msg_id == DL_BLOCK && DL_BLOCK_ac_id(dl_buffer) == AC_ID) { nav_goto_block(DL_BLOCK_block_id(dl_buffer)); SEND_NAVIGATION(DefaultChannel); diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c index a12f4e873a..4e98435640 100644 --- a/sw/airborne/firmwares/fixedwing/main_ap.c +++ b/sw/airborne/firmwares/fixedwing/main_ap.c @@ -38,7 +38,7 @@ #include "firmwares/fixedwing/stabilization/stabilization_attitude.h" #include "firmwares/fixedwing/guidance/guidance_v.h" -#include "gps.h" +#include "subsystems/gps.h" #ifdef USE_INFRARED #include "subsystems/sensors/infrared.h" #endif @@ -75,10 +75,13 @@ #include "subsystems/ahrs.h" #include "subsystems/ahrs/ahrs_aligner.h" #include AHRS_TYPE_H -static inline void on_gyro_accel_event( void ); +static inline void on_gyro_event( void ); static inline void on_accel_event( void ); static inline void on_mag_event( void ); #endif +#ifdef USE_GPS +static inline void on_gps_solution( void ); +#endif #if ! defined CATASTROPHIC_BAT_LEVEL && defined LOW_BATTERY #warning "LOW_BATTERY deprecated. Renamed into CATASTROPHIC_BAT_LEVEL (in airframe file)" @@ -264,6 +267,11 @@ static inline void telecommand_task( void ) { #endif } + +#ifdef FAILSAFE_DELAY_WITHOUT_GPS +#define GpsTimeoutError (cpu_time_sec - gps.last_fix_time > FAILSAFE_DELAY_WITHOUT_GPS) +#endif + /** \fn void navigation_task( void ) * \brief Compute desired_course */ @@ -277,12 +285,12 @@ static void navigation_task( void ) { if (launch) { if (GpsTimeoutError) { if (pprz_mode == PPRZ_MODE_AUTO2 || pprz_mode == PPRZ_MODE_HOME) { - last_pprz_mode = pprz_mode; - pprz_mode = PPRZ_MODE_GPS_OUT_OF_ORDER; - PERIODIC_SEND_PPRZ_MODE(DefaultChannel); - gps_lost = TRUE; + last_pprz_mode = pprz_mode; + pprz_mode = PPRZ_MODE_GPS_OUT_OF_ORDER; + PERIODIC_SEND_PPRZ_MODE(DefaultChannel); + gps_lost = TRUE; } - } else /* GPS is ok */ if (gps_lost) { + } else if (gps_lost) { /* GPS is ok */ /** If aircraft was in failsafe mode, come back in previous mode */ pprz_mode = last_pprz_mode; gps_lost = FALSE; @@ -390,7 +398,7 @@ static inline void attitude_loop( void ) { v_ctl_throttle_slew(); ap_state->commands[COMMAND_THROTTLE] = v_ctl_throttle_slewed; ap_state->commands[COMMAND_ROLL] = h_ctl_aileron_setpoint; - + ap_state->commands[COMMAND_PITCH] = h_ctl_elevator_setpoint; #if defined MCU_SPI_LINK @@ -463,11 +471,6 @@ void periodic_task_ap( void ) { switch(_4Hz) { case 0: -#ifdef SITL -#ifdef GPS_TRIGGERED_FUNCTION - GPS_TRIGGERED_FUNCTION(); -#endif -#endif estimator_propagate_state(); #ifdef EXTRA_DOWNLINK_DEVICE DOWNLINK_SEND_ATTITUDE(ExtraPprzTransport,&estimator_phi,&estimator_psi,&estimator_theta); @@ -476,7 +479,7 @@ void periodic_task_ap( void ) { break; case 1: if (!estimator_flight_time && - estimator_hspeed_mod > MIN_SPEED_FOR_TAKEOFF) { + estimator_hspeed_mod > MIN_SPEED_FOR_TAKEOFF) { estimator_flight_time = 1; launch = TRUE; /* Not set in non auto launch */ DOWNLINK_SEND_TAKEOFF(DefaultChannel, &cpu_time_sec); @@ -575,9 +578,11 @@ void init_ap( void ) { /** wait 0.5s (historical :-) */ sys_time_usleep(500000); -#if defined GPS_CONFIGURE +#ifdef GPS_CONFIGURE +#ifndef SITL gps_configure_uart(); #endif +#endif #if defined DATALINK @@ -609,36 +614,11 @@ void event_task_ap( void ) { #endif #ifdef USE_AHRS - ImuEvent(on_gyro_accel_event, on_accel_event, on_mag_event); + ImuEvent(on_gyro_event, on_accel_event, on_mag_event); #endif // USE_AHRS #ifdef USE_GPS -#if !(defined HITL) && !(defined UBX_EXTERNAL) /** else comes through the datalink */ - if (GpsBuffer()) { - ReadGpsBuffer(); - } -#endif - if (gps_msg_received) { - /* parse and use GPS messages */ -#ifdef GPS_CONFIGURE - if (gps_configuring) - gps_configure(); - else -#endif - parse_gps_msg(); - gps_msg_received = FALSE; - if (gps_pos_available) { - gps_verbose_downlink = !launch; - UseGpsPosNoSend(estimator_update_state_gps); - gps_downlink(); -#ifdef GPS_TRIGGERED_FUNCTION -#ifndef SITL - GPS_TRIGGERED_FUNCTION(); -#endif -#endif - gps_pos_available = FALSE; - } - } + GpsEvent(on_gps_solution); #endif /** USE_GPS */ @@ -656,7 +636,7 @@ void event_task_ap( void ) { } modules_event_task(); - + #ifdef AHRS_TRIGGERED_ATTITUDE_LOOP if (new_ins_attitude > 0) { @@ -665,14 +645,24 @@ void event_task_ap( void ) { new_ins_attitude = 0; } #endif - + } /* event_task_ap() */ + +#ifdef USE_GPS +static inline void on_gps_solution( void ) { + estimator_update_state_gps(); +#ifdef GPS_TRIGGERED_FUNCTION + GPS_TRIGGERED_FUNCTION(); +#endif +} +#endif + #ifdef USE_AHRS static inline void on_accel_event( void ) { } -static inline void on_gyro_accel_event( void ) { +static inline void on_gyro_event( void ) { #ifdef AHRS_CPU_LED LED_ON(AHRS_CPU_LED); diff --git a/sw/airborne/firmwares/non_ap/charge_sens/charge_sens.pde b/sw/airborne/firmwares/non_ap/charge_sens/charge_sens.pde new file mode 100644 index 0000000000..ee3b26fdab --- /dev/null +++ b/sw/airborne/firmwares/non_ap/charge_sens/charge_sens.pde @@ -0,0 +1,160 @@ +/* + * $Id$ + * + * Copyright (C) 2011 Martin Mueller + * + * This file is part of Paparazzi. + * + * Paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * Paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + * + */ + +/* I2C interface for University of Reading charge sensor */ + +#define DEBUG 1 + +#include + + +/* in ms */ +#define SAMPLE_RATE 100 +/* x times samle rate */ +#define RESET_RATE 50 + +/* x times over-sampling */ +#define AVG_NB_SAMPLE 10 + +/* print every x values */ +#define LED_RATE 10 + +/* charge reset pin PB1, high active */ +#define RESET_PIN 9 +/* adc_in pin ADC0 */ +#define ADC_PIN 0 +/* green LED pin PB5 (on arduino pro mini) */ +#define LED_GR_PIN 13 + +#define CHARGE_SENS_I2C_ADDR 0x78 +#define RESET_ACTIVE 0x1000 + +struct adc_buf { + unsigned long sum; + unsigned short values[AVG_NB_SAMPLE]; + unsigned char head; +}; + +struct adc_buf buf; + +unsigned short reset_active_uart = 0; +unsigned short reset_active_i2c = 0; +unsigned long trig_next; +unsigned short adc_val; +unsigned char new_head; +int i, cnt = 0; +int i2c_cnt = 0; +int led_cnt = 0; +int reset_cnt = 0; + + +ISR(ADC_vect) { + adc_val = ADCL; + adc_val |= ADCH << 8; + new_head = buf.head + 1; + if (new_head >= AVG_NB_SAMPLE) + new_head=0; + buf.sum -= buf.values[new_head]; + buf.values[new_head] = adc_val; + buf.sum += adc_val; + buf.head = new_head; +} + +void read_i2c() { + unsigned short i2c; + i2c = (buf.sum / AVG_NB_SAMPLE) | reset_active_i2c; + reset_active_i2c = 0; + + Wire.send((uint8_t*)i2c, 2); +} + +void setup() { +#ifdef DEBUG + /* serial port */ + Serial.begin(38400); + pinMode(2,OUTPUT); + digitalWrite(2,HIGH); + Serial.println("charge sensor init"); +#endif + + /* I2C init */ + Wire.begin(CHARGE_SENS_I2C_ADDR >> 1); + Wire.onRequest(read_i2c); + + /* green LED init */ + digitalWrite(LED_GR_PIN, LOW); + pinMode(LED_GR_PIN, OUTPUT); + + /* reset pin init */ + digitalWrite(RESET_PIN, LOW); + pinMode(RESET_PIN, OUTPUT); + + /* init adc, default reference */ + ADCSRA |= (1<= trig_next) { + trig_next += SAMPLE_RATE / AVG_NB_SAMPLE; + + /* start conversion */ + ADCSRA |= 1 << ADSC; + + if (++cnt >= AVG_NB_SAMPLE) { + cnt = 0; + + if (++reset_cnt >= RESET_RATE) { + /* start reset */ + digitalWrite(RESET_PIN, HIGH); + reset_cnt = 0; + reset_active_uart = RESET_ACTIVE; + reset_active_i2c = RESET_ACTIVE; + } + else { + /* stop reset */ + digitalWrite(RESET_PIN, LOW); + } + +#ifdef DEBUG + Serial.print((buf.sum / AVG_NB_SAMPLE) | reset_active_uart); + Serial.print(" "); +#endif + if (++led_cnt >= LED_RATE) { + led_cnt = 0; + /* blink green LED */ + digitalWrite(LED_GR_PIN, HIGH); +#ifdef DEBUG + Serial.println(""); +#endif + } + else digitalWrite(LED_GR_PIN, LOW); + reset_active_uart = 0; + } + } +} + diff --git a/sw/airborne/firmwares/rotorcraft/actuators/actuators_asctec.c b/sw/airborne/firmwares/rotorcraft/actuators/actuators_asctec.c index ae69747ecc..c0b602915b 100644 --- a/sw/airborne/firmwares/rotorcraft/actuators/actuators_asctec.c +++ b/sw/airborne/firmwares/rotorcraft/actuators/actuators_asctec.c @@ -5,7 +5,7 @@ #include "firmwares/rotorcraft/actuators/supervision.h" #endif -#include "booz/booz2_commands.h" +#include "firmwares/rotorcraft/commands.h" #include "mcu_periph/i2c.h" #include "sys_time.h" @@ -61,10 +61,10 @@ void actuators_set(bool_t motors_on) { actuators_asctec.cmds[YAW] = 0; actuators_asctec.cmds[THRUST] = 0; #else /* ! KILL_MOTORS */ - actuators_asctec.cmds[PITCH] = booz2_commands[COMMAND_PITCH] + SUPERVISION_TRIM_E; - actuators_asctec.cmds[ROLL] = booz2_commands[COMMAND_ROLL] + SUPERVISION_TRIM_A; - actuators_asctec.cmds[YAW] = booz2_commands[COMMAND_YAW] + SUPERVISION_TRIM_R; - actuators_asctec.cmds[THRUST] = booz2_commands[COMMAND_THRUST]; + actuators_asctec.cmds[PITCH] = commands[COMMAND_PITCH] + SUPERVISION_TRIM_E; + actuators_asctec.cmds[ROLL] = commands[COMMAND_ROLL] + SUPERVISION_TRIM_A; + actuators_asctec.cmds[YAW] = commands[COMMAND_YAW] + SUPERVISION_TRIM_R; + actuators_asctec.cmds[THRUST] = commands[COMMAND_THRUST]; Bound(actuators_asctec.cmds[PITCH],-100, 100); Bound(actuators_asctec.cmds[ROLL], -100, 100); Bound(actuators_asctec.cmds[YAW], -100, 100); @@ -112,7 +112,7 @@ void actuators_set(bool_t motors_on) { #else /* ! ACTUATORS_ASCTEC_V2_PROTOCOL */ void actuators_set(bool_t motors_on) { if (!cpu_time_sec) return; // FIXME - supervision_run(motors_on, FALSE, booz2_commands); + supervision_run(motors_on, FALSE, commands); #ifdef KILL_MOTORS actuators_asctec.i2c_trans.buf[0] = 0; actuators_asctec.i2c_trans.buf[1] = 0; diff --git a/sw/airborne/firmwares/rotorcraft/actuators/actuators_heli.c b/sw/airborne/firmwares/rotorcraft/actuators/actuators_heli.c index 59b3f9538e..1951252929 100644 --- a/sw/airborne/firmwares/rotorcraft/actuators/actuators_heli.c +++ b/sw/airborne/firmwares/rotorcraft/actuators/actuators_heli.c @@ -23,7 +23,7 @@ #include "firmwares/rotorcraft/actuators.h" #include "actuators_heli.h" -#include "booz/booz2_commands.h" +#include "firmwares/rotorcraft/commands.h" /* let's start butchery now and use the actuators_pwm arch functions */ #include "firmwares/rotorcraft/actuators/actuators_pwm.h" @@ -53,7 +53,7 @@ void actuators_init(void) { actuators_pwm_arch_init(); } void actuators_set(bool_t motors_on) { - SetActuatorsFromCommands(booz2_commands); + SetActuatorsFromCommands(commands); } diff --git a/sw/airborne/firmwares/rotorcraft/actuators/actuators_mkk.c b/sw/airborne/firmwares/rotorcraft/actuators/actuators_mkk.c index 07602f1430..b9d7a04b98 100644 --- a/sw/airborne/firmwares/rotorcraft/actuators/actuators_mkk.c +++ b/sw/airborne/firmwares/rotorcraft/actuators/actuators_mkk.c @@ -24,7 +24,7 @@ #include "firmwares/rotorcraft/actuators.h" #include "firmwares/rotorcraft/actuators/actuators_mkk.h" -#include "booz/booz2_commands.h" +#include "firmwares/rotorcraft/commands.h" #include "mcu_periph/i2c.h" #include "sys_time.h" @@ -65,7 +65,7 @@ void actuators_set(bool_t motors_on) { } #endif - supervision_run(motors_on, FALSE, booz2_commands); + supervision_run(motors_on, FALSE, commands); for (uint8_t 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. + * + */ + +#ifndef COMMANDS_H +#define COMMANDS_H + +#include "paparazzi.h" +#include "generated/airframe.h" + +extern int32_t commands[COMMANDS_NB]; +extern const int32_t commands_failsafe[COMMANDS_NB]; + +#ifndef ROTORCRAFT_COMMANDS_YAW_ALWAYS_ENABLED +#define SetCommands(_in_cmd, _in_flight, _motors_on) { \ + commands[COMMAND_PITCH] = _in_cmd[COMMAND_PITCH]; \ + commands[COMMAND_ROLL] = _in_cmd[COMMAND_ROLL]; \ + commands[COMMAND_YAW] = (_in_flight) ? _in_cmd[COMMAND_YAW] : 0; \ + commands[COMMAND_THRUST] = (_motors_on) ? _in_cmd[COMMAND_THRUST] : 0; \ + } +#else +#define SetCommands(_in_cmd, _in_flight, _motors_on) { \ + commands[COMMAND_PITCH] = _in_cmd[COMMAND_PITCH]; \ + commands[COMMAND_ROLL] = _in_cmd[COMMAND_ROLL]; \ + commands[COMMAND_YAW] = _in_cmd[COMMAND_YAW]; \ + commands[COMMAND_THRUST] = (_motors_on) ? _in_cmd[COMMAND_THRUST] : 0; \ + } +#endif + +#endif /* COMMANDS_H */ diff --git a/sw/airborne/firmwares/rotorcraft/datalink.c b/sw/airborne/firmwares/rotorcraft/datalink.c index adb77a1825..e4f6215e41 100644 --- a/sw/airborne/firmwares/rotorcraft/datalink.c +++ b/sw/airborne/firmwares/rotorcraft/datalink.c @@ -94,7 +94,8 @@ void dl_parse_msg(void) { struct EnuCoor_i enu; lla.lat = INT32_RAD_OF_DEG(DL_MOVE_WP_lat(dl_buffer)); lla.lon = INT32_RAD_OF_DEG(DL_MOVE_WP_lon(dl_buffer)); - lla.alt = DL_MOVE_WP_alt(dl_buffer) - ins_ltp_def.hmsl + ins_ltp_def.lla.alt; + /* WP_alt is in cm, lla.alt in mm */ + lla.alt = DL_MOVE_WP_alt(dl_buffer)*10 - ins_ltp_def.hmsl + ins_ltp_def.lla.alt; enu_of_lla_point_i(&enu,&ins_ltp_def,&lla); enu.x = POS_BFP_OF_REAL(enu.x)/100; enu.y = POS_BFP_OF_REAL(enu.y)/100; diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c index 1a960eda0c..b0808d6202 100644 --- a/sw/airborne/firmwares/rotorcraft/main.c +++ b/sw/airborne/firmwares/rotorcraft/main.c @@ -35,12 +35,12 @@ #include "subsystems/settings.h" #include "xbee.h" -#include "booz2_commands.h" +#include "firmwares/rotorcraft/commands.h" #include "firmwares/rotorcraft/actuators.h" #include "subsystems/radio_control.h" #include "subsystems/imu.h" -#include "booz_gps.h" +#include "subsystems/gps.h" #include "subsystems/sensors/baro.h" #include "baro_board.h" @@ -120,7 +120,7 @@ STATIC_INLINE void main_init( void ) { ins_init(); #ifdef USE_GPS - booz_gps_init(); + gps_init(); #endif modules_init(); @@ -169,10 +169,9 @@ 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(); + autopilot_set_mode(AP_MODE_FAILSAFE); #endif modules_periodic_task(); @@ -196,7 +195,7 @@ STATIC_INLINE void main_event( void ) { BaroEvent(on_baro_abs_event, on_baro_dif_event); #ifdef USE_GPS - BoozGpsEvent(on_gps_event); + GpsEvent(on_gps_event); #endif #ifdef FAILSAFE_GROUND_DETECT @@ -250,7 +249,7 @@ static inline void on_baro_dif_event( void ) { static inline void on_gps_event(void) { ins_update_gps(); #ifdef USE_VEHICLE_INTERFACE - if (booz_gps_state.fix == BOOZ2_GPS_FIX_3D) + if (gps.fix == GPS_FIX_3D) vi_notify_gps_available(); #endif } diff --git a/sw/airborne/firmwares/rotorcraft/navigation.c b/sw/airborne/firmwares/rotorcraft/navigation.c index 463927fe1e..46b8b081f7 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.c +++ b/sw/airborne/firmwares/rotorcraft/navigation.c @@ -26,7 +26,7 @@ #include "firmwares/rotorcraft/navigation.h" #include "pprz_debug.h" -#include "booz_gps.h" +#include "subsystems/gps.h" #include "subsystems/ins.h" #include "firmwares/rotorcraft/autopilot.h" @@ -44,10 +44,6 @@ struct EnuCoor_i navigation_carrot; struct EnuCoor_i nav_last_point; -uint16_t stage_time, block_time; - -uint8_t nav_stage, nav_block; -uint8_t last_block, last_stage; uint8_t last_wp __attribute__ ((unused)); int32_t ground_alt; @@ -260,8 +256,8 @@ unit_t nav_reset_alt( void ) { ins_vf_realign = TRUE; #ifdef USE_GPS - ins_ltp_def.lla.alt = booz_gps_state.lla_pos.alt; - ins_ltp_def.hmsl = booz_gps_state.hmsl; + ins_ltp_def.lla.alt = gps.lla_pos.alt; + ins_ltp_def.hmsl = gps.hmsl; #endif return 0; @@ -274,22 +270,6 @@ void nav_init_stage( void ) { horizontal_mode = HORIZONTAL_MODE_WAYPOINT; } -void nav_init_block(void) { - if (nav_block >= NB_BLOCK) - nav_block=NB_BLOCK-1; - nav_stage = 0; - block_time = 0; - InitStage(); -} - -void nav_goto_block(uint8_t b) { - if (b != nav_block) { /* To avoid a loop in a the current block */ - last_block = nav_block; - last_stage = nav_stage; - } - GotoBlock(b); -} - #include void nav_periodic_task() { RunOnceEvery(16, { stage_time++; block_time++; }); @@ -301,7 +281,6 @@ void nav_periodic_task() { nav_run(); ground_alt = POS_BFP_OF_REAL((float)ins_ltp_def.hmsl / 100.); - } #include "downlink.h" diff --git a/sw/airborne/firmwares/rotorcraft/navigation.h b/sw/airborne/firmwares/rotorcraft/navigation.h index dff7396961..742f1b9c10 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.h +++ b/sw/airborne/firmwares/rotorcraft/navigation.h @@ -1,7 +1,5 @@ /* - * $Id$ - * - * Copyright (C) 2008-2009 Antoine Drouin + * Copyright (C) 2008-2011 The Paparazzi Team * * This file is part of paparazzi. * @@ -28,6 +26,8 @@ #include "math/pprz_geodetic_int.h" #include "math/pprz_geodetic_float.h" +#include "subsystems/navigation/common_flight_plan.h" + #define NAV_FREQ 16 // FIXME use periodic FREQ #define NAV_PRESCALER (512/NAV_FREQ) @@ -42,10 +42,6 @@ extern const uint8_t nb_waypoint; extern void nav_init(void); extern void nav_run(void); -extern uint16_t stage_time, block_time; - -extern uint8_t nav_stage, nav_block; -extern uint8_t last_block, last_stage; extern uint8_t last_wp __attribute__ ((unused)); extern int32_t ground_alt; @@ -70,9 +66,7 @@ extern float flight_altitude; #define VERTICAL_MODE_CLIMB 1 #define VERTICAL_MODE_ALT 2 -void nav_init_stage(void); -void nav_init_block(void); -void nav_goto_block(uint8_t block_id); + void compute_dist2_to_home(void); unit_t nav_reset_reference( void ) __attribute__ ((unused)); unit_t nav_reset_alt( void ) __attribute__ ((unused)); @@ -85,29 +79,6 @@ void nav_home(void); #define NavKillThrottle() ({ if (autopilot_mode == AP_MODE_NAV) { kill_throttle = 1; autopilot_motors_on = 0; } FALSE; }) #define NavResurrect() ({ if (autopilot_mode == AP_MODE_NAV) { kill_throttle = 0; autopilot_motors_on = 1; } FALSE; }) -#define InitStage() nav_init_stage(); - -#define Block(x) case x: nav_block=x; -#define NextBlock() { nav_block++; nav_init_block(); } -#define GotoBlock(b) { nav_block=b; nav_init_block(); } - -#define Stage(s) case s: nav_stage=s; -#define NextStageAndBreak() { nav_stage++; InitStage(); break; } -#define NextStageAndBreakFrom(wp) { last_wp = wp; NextStageAndBreak(); } - -#define Label(x) label_ ## x: -#define Goto(x) { goto label_ ## x; } -#define Return() ({ nav_block=last_block; nav_stage=last_stage; block_time=0; FALSE;}) - -#define And(x, y) ((x) && (y)) -#define Or(x, y) ((x) || (y)) -#define Min(x,y) (x < y ? x : y) -#define Max(x,y) (x > y ? x : y) -#define LessThan(_x, _y) ((_x) < (_y)) -#define MoreThan(_x, _y) ((_x) > (_y)) - -/** Time in s since the entrance in the current block */ -#define NavBlockTime() (block_time) #define NavSetGroundReferenceHere() ({ nav_reset_reference(); FALSE; }) #define NavSetAltitudeReferenceHere() ({ nav_reset_alt(); FALSE; }) diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint.c b/sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint.c new file mode 100644 index 0000000000..8fd5001414 --- /dev/null +++ b/sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint.c @@ -0,0 +1,159 @@ +/* + * + * Copyright (C) 2008-2011 Joby Energy Inc + * + */ + +/** \file quat_setpoint.c + * \brief Quaternion setpoint generation + * + */ + +#include "subsystems/ahrs.h" + +#include "stabilization/stabilization_attitude_ref_quat_int.h" +#include "stabilization/quat_setpoint.h" +#include "stabilization.h" + +#include "messages.h" +#include "mcu_periph/uart.h" +#include "downlink.h" + +#define QUAT_SETPOINT_HOVER_PITCH RadOfDeg(90) + +// reset to "hover" setpoint +static void reset_sp_quat(int32_t _psi, int32_t _theta, struct Int32Quat *initial) +{ + int32_t pitch_rotation_angle; + struct Int32Quat pitch_axis_quat; + + struct Int32Quat pitch_rotated_quat, pitch_rotated_quat2; + + struct Int32Vect3 y_axis = { 0, 1, 0 }; + + struct Int32Eulers rotated_eulers; + + // compose rotation about Y axis (pitch axis) from hover + pitch_rotation_angle = ANGLE_BFP_OF_REAL(-QUAT_SETPOINT_HOVER_PITCH); + INT32_QUAT_OF_AXIS_ANGLE(pitch_axis_quat, y_axis, pitch_rotation_angle); + INT32_QUAT_COMP_NORM_SHORTEST(pitch_rotated_quat, *initial, pitch_axis_quat); + + INT32_EULERS_OF_QUAT(rotated_eulers, pitch_rotated_quat); + + // reset euler angles + rotated_eulers.theta = _theta; + rotated_eulers.phi = _psi; + + INT32_QUAT_OF_EULERS(pitch_rotated_quat, rotated_eulers); + + // compose rotation about Y axis (pitch axis) to hover + pitch_rotation_angle = ANGLE_BFP_OF_REAL(QUAT_SETPOINT_HOVER_PITCH); + INT32_QUAT_OF_AXIS_ANGLE(pitch_axis_quat, y_axis, pitch_rotation_angle); + INT32_QUAT_COMP_NORM_SHORTEST(pitch_rotated_quat2, pitch_rotated_quat, pitch_axis_quat); + + // store result into setpoint + QUAT_COPY(stab_att_sp_quat, pitch_rotated_quat2); +} + +static void update_sp_quat_from_eulers(void) { + struct Int32RMat sp_rmat; + +#ifdef STICKS_RMAT312 + INT32_RMAT_OF_EULERS_312(sp_rmat, stab_att_sp_euler); +#else + INT32_RMAT_OF_EULERS_321(sp_rmat, stab_att_sp_euler); +#endif + INT32_QUAT_OF_RMAT(stab_att_sp_quat, sp_rmat); + INT32_QUAT_WRAP_SHORTEST(stab_att_sp_quat); +} + +/* +void stabilization_attitude_read_rc_incremental(bool_t enable_alpha_vane, bool_t enable_beta_vane) +{ + pprz_t roll = radio_control.values[RADIO_ROLL]; + pprz_t pitch = radio_control.values[RADIO_PITCH]; + pprz_t yaw = radio_control.values[RADIO_YAW]; + struct Int32Quat prev_sp_quat, q_e2s, temp_quat; + + struct Int32RMat R_e2s; + struct Int32Rates sticks_w_bn_e, sticks_w_bn_s; + + QUAT_COPY(prev_sp_quat, stab_att_sp_quat); + + sticks_w_bn_e.p = APPLY_DEADBAND(roll, STABILIZATION_ATTITUDE_DEADBAND_A) * ROLL_COEF_H; + sticks_w_bn_e.q = APPLY_DEADBAND(pitch, STABILIZATION_ATTITUDE_DEADBAND_E) * PITCH_COEF_RATE; + sticks_w_bn_e.r = APPLY_DEADBAND(yaw, STABILIZATION_ATTITUDE_DEADBAND_R) * YAW_COEF_RATE; + + // Don't let the sticks command a theta rotation in vane mode + if (enable_alpha_vane) { + sticks_w_bn_e.q = 0; + } + + if (enable_beta_vane) { + sticks_w_bn_e.r = 0; + } + // q_e2s = q_sp (comp) q_b^(-1) + INT_QUAT_INV_COMP_NORM_SHORTEST(q_e2s, ahrs.ltp_to_body_quat, stab_att_sp_quat); + + INT_RMAT_OF_QUAT(R_e2s, q_e2s); + + INT_RMAT_RATEMULT(sticks_w_bn_s, R_e2s, sticks_w_bn_e); + + INT_QUAT_DIFFERENTIAL(temp_quat, sticks_w_bn_s, 1/RC_UPDATE_FREQ); + + INT32_QUAT_COMP_NORM_SHORTEST(stab_att_sp_quat, prev_sp_quat, temp_quat); + + // update euler setpoints for telemetry + INT32_EULERS_OF_QUAT(stab_att_sp_euler, stab_att_sp_quat); +} +*/ + +void stabilization_attitude_read_rc_absolute(struct Int32Eulers sp, bool_t in_flight) { + +#ifdef AIRPLANE_STICKS + pprz_t roll = radio_control.values[RADIO_ROLL]; + pprz_t pitch = radio_control.values[RADIO_PITCH]; + pprz_t yaw = radio_control.values[RADIO_YAW]; +#else // QUAD STICKS + pprz_t roll = radio_control.values[RADIO_YAW]; + pprz_t pitch = radio_control.values[RADIO_PITCH]; + pprz_t yaw = -radio_control.values[RADIO_ROLL]; +#endif + struct Int32Eulers sticks_eulers; + struct Int32Quat sticks_quat, prev_sp_quat; + + // heading hold? + if (in_flight) { + // compose setpoint based on previous setpoint + pitch/roll sticks + reset_sp_quat(RATE_BFP_OF_REAL(yaw * YAW_COEF), RATE_BFP_OF_REAL(pitch * PITCH_COEF), &stab_att_sp_quat); + + // get commanded yaw rate from sticks + sticks_eulers.phi = RATE_BFP_OF_REAL(APPLY_DEADBAND(roll, STABILIZATION_ATTITUDE_DEADBAND_A) * ROLL_COEF / RC_UPDATE_FREQ); + sticks_eulers.theta = 0; + sticks_eulers.psi = 0; + + // convert yaw rate * dt into quaternion + INT32_QUAT_OF_EULERS(sticks_quat, sticks_eulers); + QUAT_COPY(prev_sp_quat, stab_att_sp_quat) + + // update setpoint by rotating by incremental yaw command + INT32_QUAT_COMP_NORM_SHORTEST(stab_att_sp_quat, prev_sp_quat, sticks_quat); + } else { /* if not flying, use current body position + pitch/yaw from sticks to compose setpoint */ + reset_sp_quat(RATE_BFP_OF_REAL(yaw * YAW_COEF), RATE_BFP_OF_REAL(pitch * PITCH_COEF), &ahrs.ltp_to_body_quat); + } + + // update euler setpoints for telemetry + INT32_EULERS_OF_QUAT(stab_att_sp_euler, stab_att_sp_quat); +} + +void stabilization_attitude_sp_enter() +{ + quat_setpoint_enter_absolute(); +} + + +void quat_setpoint_enter_absolute() +{ + // reset setpoint to "hover" + reset_sp_quat(0., 0., &ahrs.ltp_to_body_quat); +} diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint.h b/sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint.h new file mode 100644 index 0000000000..c88ee2f8cd --- /dev/null +++ b/sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint.h @@ -0,0 +1,21 @@ +/* + * Copyright (C) 2008-2011 Joby Energy Inc + */ +#ifndef QUAT_SETPOINT_H +#define QUAT_SETPOINT_H + +#include "stabilization.h" + +#include "subsystems/radio_control.h" +#include "math/pprz_algebra.h" + +#include "stabilization/stabilization_attitude_ref_int.h" + +void stabilization_attitude_sp_enter(void); +void stabilization_attitude_read_rc_incremental(bool_t enable_alpha_vane, bool_t enable_beta_vane); +void stabilization_attitude_read_rc_absolute(struct Int32Eulers sp, bool_t in_flight); +void quat_setpoint_enter_absolute(void); +void booz_stab_att_vane_on(void); +void booz_stab_att_vane_off(void); + +#endif diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c new file mode 100644 index 0000000000..a221633e5e --- /dev/null +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c @@ -0,0 +1,225 @@ +/* + * + * 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. + */ + +/** \file stabilization_attitude_quat_int.c + * \brief Booz quaternion attitude stabilization + */ + +#include "firmwares/rotorcraft/stabilization.h" +#include "firmwares/rotorcraft/stabilization/quat_setpoint.h" + +#include +#include "math/pprz_algebra_float.h" +#include "math/pprz_algebra_int.h" +#include "subsystems/ahrs.h" +#include "generated/airframe.h" + +struct Int32AttitudeGains stabilization_gains = { + {STABILIZATION_ATTITUDE_PHI_PGAIN, STABILIZATION_ATTITUDE_THETA_PGAIN, STABILIZATION_ATTITUDE_PSI_PGAIN }, + {STABILIZATION_ATTITUDE_PHI_DGAIN, STABILIZATION_ATTITUDE_THETA_DGAIN, STABILIZATION_ATTITUDE_PSI_DGAIN }, + {STABILIZATION_ATTITUDE_PHI_DDGAIN, STABILIZATION_ATTITUDE_THETA_DDGAIN, STABILIZATION_ATTITUDE_PSI_DDGAIN }, + {STABILIZATION_ATTITUDE_PHI_IGAIN, STABILIZATION_ATTITUDE_THETA_IGAIN, STABILIZATION_ATTITUDE_PSI_IGAIN } +}; + +struct Int32Quat stabilization_att_sum_err_quat; +struct Int32Eulers stabilization_att_sum_err; + +int32_t stabilization_att_fb_cmd[COMMANDS_NB]; +int32_t stabilization_att_ff_cmd[COMMANDS_NB]; + +//static int gain_idx = STABILIZATION_ATTITUDE_GAIN_IDX_DEFAULT; + +/* +static const float phi_pgain[] = STABILIZATION_ATTITUDE_PHI_PGAIN; +static const float theta_pgain[] = STABILIZATION_ATTITUDE_THETA_PGAIN; +static const float psi_pgain[] = STABILIZATION_ATTITUDE_PSI_PGAIN; + +static const float phi_dgain[] = STABILIZATION_ATTITUDE_PHI_DGAIN; +static const float theta_dgain[] = STABILIZATION_ATTITUDE_THETA_DGAIN; +static const float psi_dgain[] = STABILIZATION_ATTITUDE_PSI_DGAIN; + +static const float phi_igain[] = STABILIZATION_ATTITUDE_PHI_IGAIN; +static const float theta_igain[] = STABILIZATION_ATTITUDE_THETA_IGAIN; +static const float psi_igain[] = STABILIZATION_ATTITUDE_PSI_IGAIN; + +static const float phi_ddgain[] = STABILIZATION_ATTITUDE_PHI_DDGAIN; +static const float theta_ddgain[] = STABILIZATION_ATTITUDE_THETA_DDGAIN; +static const float psi_ddgain[] = STABILIZATION_ATTITUDE_PSI_DDGAIN; + +static const float phi_dgain_d[] = STABILIZATION_ATTITUDE_PHI_DGAIN_D; +static const float theta_dgain_d[] = STABILIZATION_ATTITUDE_THETA_DGAIN_D; +static const float psi_dgain_d[] = STABILIZATION_ATTITUDE_PSI_DGAIN_D; + +static const float phi_pgain_surface[] = STABILIZATION_ATTITUDE_PHI_PGAIN_SURFACE; +static const float theta_pgain_surface[] = STABILIZATION_ATTITUDE_THETA_PGAIN_SURFACE; +static const float psi_pgain_surface[] = STABILIZATION_ATTITUDE_PSI_PGAIN_SURFACE; + +static const float phi_dgain_surface[] = STABILIZATION_ATTITUDE_PHI_DGAIN_SURFACE; +static const float theta_dgain_surface[] = STABILIZATION_ATTITUDE_THETA_DGAIN_SURFACE; +static const float psi_dgain_surface[] = STABILIZATION_ATTITUDE_PSI_DGAIN_SURFACE; + +static const float phi_igain_surface[] = STABILIZATION_ATTITUDE_PHI_IGAIN_SURFACE; +static const float theta_igain_surface[] = STABILIZATION_ATTITUDE_THETA_IGAIN_SURFACE; +static const float psi_igain_surface[] = STABILIZATION_ATTITUDE_PSI_IGAIN_SURFACE; + +static const float phi_ddgain_surface[] = STABILIZATION_ATTITUDE_PHI_DDGAIN_SURFACE; +static const float theta_ddgain_surface[] = STABILIZATION_ATTITUDE_THETA_DDGAIN_SURFACE; +static const float psi_ddgain_surface[] = STABILIZATION_ATTITUDE_PSI_DDGAIN_SURFACE; +*/ + +#define IERROR_SCALE 1024 +#define GAIN_PRESCALER_FF 1 +#define GAIN_PRESCALER_P 1 +#define GAIN_PRESCALER_D 1 +#define GAIN_PRESCALER_I 1 + +void stabilization_attitude_init(void) { + + stabilization_attitude_ref_init(); + + /* + for (int i = 0; i < STABILIZATION_ATTITUDE_GAIN_NB; i++) { + VECT3_ASSIGN(stabilization_gains[i].p, phi_pgain[i], theta_pgain[i], psi_pgain[i]); + VECT3_ASSIGN(stabilization_gains[i].d, phi_dgain[i], theta_dgain[i], psi_dgain[i]); + VECT3_ASSIGN(stabilization_gains[i].i, phi_igain[i], theta_igain[i], psi_igain[i]); + VECT3_ASSIGN(stabilization_gains[i].dd, phi_ddgain[i], theta_ddgain[i], psi_ddgain[i]); + VECT3_ASSIGN(stabilization_gains[i].rates_d, phi_dgain_d[i], theta_dgain_d[i], psi_dgain_d[i]); + VECT3_ASSIGN(stabilization_gains[i].surface_p, phi_pgain_surface[i], theta_pgain_surface[i], psi_pgain_surface[i]); + VECT3_ASSIGN(stabilization_gains[i].surface_d, phi_dgain_surface[i], theta_dgain_surface[i], psi_dgain_surface[i]); + VECT3_ASSIGN(stabilization_gains[i].surface_i, phi_igain_surface[i], theta_igain_surface[i], psi_igain_surface[i]); + VECT3_ASSIGN(stabilization_gains[i].surface_dd, phi_ddgain_surface[i], theta_ddgain_surface[i], psi_ddgain_surface[i]); + } + */ + + INT32_QUAT_ZERO( stabilization_att_sum_err_quat ); + INT_EULERS_ZERO( stabilization_att_sum_err ); +} + + /* +void stabilization_attitude_gain_schedule(uint8_t idx) +{ + if (gain_idx >= STABILIZATION_ATTITUDE_GAIN_NB) { + // This could be bad -- Just say no. + return; + } + gain_idx = idx; + stabilization_attitude_ref_schedule(idx); +} + */ + +void stabilization_attitude_enter(void) { + + stabilization_attitude_ref_enter(); + + INT32_QUAT_ZERO( stabilization_att_sum_err_quat ); + //FLOAT_EULERS_ZERO( stabilization_att_sum_err_eulers ); + INT_EULERS_ZERO( stabilization_att_sum_err ); +} + +#define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b)) +#define OFFSET_AND_ROUND2(_a, _b) (((_a)+(1<<((_b)-1))-((_a)<0?1:0))>>(_b)) + +static void attitude_run_ff(int32_t ff_commands[], struct Int32AttitudeGains *gains, struct Int32Rates *ref_accel) +{ + /* Compute feedforward based on reference acceleration */ + + ff_commands[COMMAND_ROLL] = GAIN_PRESCALER_FF * gains->dd.x * RATE_FLOAT_OF_BFP(ref_accel->p) / (1 << 7); + ff_commands[COMMAND_PITCH] = GAIN_PRESCALER_FF * gains->dd.y * RATE_FLOAT_OF_BFP(ref_accel->q) / (1 << 7); + ff_commands[COMMAND_YAW] = GAIN_PRESCALER_FF * gains->dd.z * RATE_FLOAT_OF_BFP(ref_accel->r) / (1 << 7); +} + +static void attitude_run_fb(int32_t fb_commands[], struct Int32AttitudeGains *gains, struct Int32Quat *att_err, + struct Int32Rates *rate_err, struct Int32Quat *sum_err) +{ + /* PID feedback */ + fb_commands[COMMAND_ROLL] = + GAIN_PRESCALER_P * -gains->p.x * QUAT1_FLOAT_OF_BFP(att_err->qx) / 4 + + GAIN_PRESCALER_D * gains->d.x * RATE_FLOAT_OF_BFP(rate_err->p) / 16 + + GAIN_PRESCALER_I * gains->i.x * QUAT1_FLOAT_OF_BFP(sum_err->qx) / 2; + + fb_commands[COMMAND_PITCH] = + GAIN_PRESCALER_P * -gains->p.y * QUAT1_FLOAT_OF_BFP(att_err->qy) / 4 + + GAIN_PRESCALER_D * gains->d.y * RATE_FLOAT_OF_BFP(rate_err->q) / 16 + + GAIN_PRESCALER_I * gains->i.y * QUAT1_FLOAT_OF_BFP(sum_err->qy) / 2; + + fb_commands[COMMAND_YAW] = + GAIN_PRESCALER_P * -gains->p.z * QUAT1_FLOAT_OF_BFP(att_err->qz) / 4 + + GAIN_PRESCALER_D * gains->d.z * RATE_FLOAT_OF_BFP(rate_err->r) / 16 + + GAIN_PRESCALER_I * gains->i.z * QUAT1_FLOAT_OF_BFP(sum_err->qz) / 2; + +} + +void stabilization_attitude_run(bool_t enable_integrator) { + + /* + * Update reference + */ + stabilization_attitude_ref_update(); + + /* + * Compute errors for feedback + */ + + /* attitude error */ + struct Int32Quat att_err; + INT32_QUAT_INV_COMP(att_err, ahrs.ltp_to_body_quat, stab_att_ref_quat); + /* wrap it in the shortest direction */ + INT32_QUAT_WRAP_SHORTEST(att_err); + INT32_QUAT_NORMALIZE(att_err); + + /* rate error */ + struct Int32Rates rate_err; + RATES_DIFF(rate_err, ahrs.body_rate, stab_att_ref_rate); + + /* integrated error */ + if (enable_integrator) { + struct Int32Quat new_sum_err, scaled_att_err; + /* update accumulator */ + scaled_att_err.qi = att_err.qi; + scaled_att_err.qx = att_err.qx / IERROR_SCALE; + scaled_att_err.qy = att_err.qy / IERROR_SCALE; + scaled_att_err.qz = att_err.qz / IERROR_SCALE; + INT32_QUAT_COMP_INV(new_sum_err, stabilization_att_sum_err_quat, scaled_att_err); + INT32_QUAT_NORMALIZE(new_sum_err); + QUAT_COPY(stabilization_att_sum_err_quat, new_sum_err); + INT32_EULERS_OF_QUAT(stabilization_att_sum_err, stabilization_att_sum_err_quat); + } else { + /* reset accumulator */ + INT32_QUAT_ZERO( stabilization_att_sum_err_quat ); + INT_EULERS_ZERO( stabilization_att_sum_err ); + } + + attitude_run_ff(stabilization_att_ff_cmd, &stabilization_gains, &stab_att_ref_accel); + + attitude_run_fb(stabilization_att_fb_cmd, &stabilization_gains, &att_err, &rate_err, &stabilization_att_sum_err_quat); + + for (int i = COMMAND_ROLL; i <= COMMAND_YAW; i++) { + stabilization_cmd[i] = stabilization_att_fb_cmd[i]+stabilization_att_ff_cmd[i]; + Bound(stabilization_cmd[i], -200, 200); + } +} + +void stabilization_attitude_read_rc(bool_t in_flight) { + + STABILIZATION_ATTITUDE_READ_RC(stab_att_sp_euler, in_flight); + +} diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h index 6b445aefc6..25e38111fc 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h @@ -32,4 +32,11 @@ extern struct Int32Quat stab_att_ref_quat; extern struct Int32Rates stab_att_ref_rate; extern struct Int32Rates stab_att_ref_accel; +struct Int32RefModel { + struct Int32Rates omega; + struct Int32Rates zeta; +}; + +extern struct Int32RefModel stab_att_ref_model; + #endif /* BOOZ_STABILISATION_ATTITUDE_REF_INT_H */ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c new file mode 100644 index 0000000000..3ce53dd8a7 --- /dev/null +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c @@ -0,0 +1,202 @@ +/* + * $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. + */ + +/** \file stabilization_attitude_ref_int.c + * \brief Booz attitude reference generation (quaternion int version) + * + */ + +#include "generated/airframe.h" +#include "firmwares/rotorcraft/stabilization.h" +#include "subsystems/ahrs.h" + +#include "stabilization_attitude_ref_int.h" +//#include "quat_setpoint.h" + +#define REF_ACCEL_MAX_P BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_PDOT, REF_ACCEL_FRAC) +#define REF_ACCEL_MAX_Q BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_QDOT, REF_ACCEL_FRAC) +#define REF_ACCEL_MAX_R BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_RDOT, REF_ACCEL_FRAC) + +#define REF_RATE_MAX_P BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_P, REF_RATE_FRAC) +#define REF_RATE_MAX_Q BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_Q, REF_RATE_FRAC) +#define REF_RATE_MAX_R BFP_OF_REAL(STABILIZATION_ATTITUDE_REF_MAX_R, REF_RATE_FRAC) + +#define OMEGA_P STABILIZATION_ATTITUDE_REF_OMEGA_P +#define ZETA_P STABILIZATION_ATTITUDE_REF_ZETA_P +#define ZETA_OMEGA_P_RES 10 +#define ZETA_OMEGA_P BFP_OF_REAL((ZETA_P*OMEGA_P), ZETA_OMEGA_P_RES) +#define OMEGA_2_P_RES 7 +#define OMEGA_2_P BFP_OF_REAL((OMEGA_P*OMEGA_P), OMEGA_2_P_RES) + +#define OMEGA_Q STABILIZATION_ATTITUDE_REF_OMEGA_Q +#define ZETA_Q STABILIZATION_ATTITUDE_REF_ZETA_Q +#define ZETA_OMEGA_Q_RES 10 +#define ZETA_OMEGA_Q BFP_OF_REAL((ZETA_Q*OMEGA_Q), ZETA_OMEGA_Q_RES) +#define OMEGA_2_Q_RES 7 +#define OMEGA_2_Q BFP_OF_REAL((OMEGA_Q*OMEGA_Q), OMEGA_2_Q_RES) + +#define OMEGA_R STABILIZATION_ATTITUDE_REF_OMEGA_R +#define ZETA_R STABILIZATION_ATTITUDE_REF_ZETA_R +#define ZETA_OMEGA_R_RES 10 +#define ZETA_OMEGA_R BFP_OF_REAL((ZETA_R*OMEGA_R), ZETA_OMEGA_R_RES) +#define OMEGA_2_R_RES 7 +#define OMEGA_2_R BFP_OF_REAL((OMEGA_R*OMEGA_R), OMEGA_2_R_RES) + + +struct Int32Eulers stab_att_sp_euler; +struct Int32Quat stab_att_sp_quat; +struct Int32Eulers stab_att_ref_euler; +struct Int32Quat stab_att_ref_quat; +struct Int32Rates stab_att_ref_rate; +struct Int32Rates stab_att_ref_accel; + +struct Int32RefModel stab_att_ref_model = { + {STABILIZATION_ATTITUDE_REF_OMEGA_P, STABILIZATION_ATTITUDE_REF_OMEGA_Q, STABILIZATION_ATTITUDE_REF_OMEGA_R}, + {STABILIZATION_ATTITUDE_REF_ZETA_P, STABILIZATION_ATTITUDE_REF_ZETA_Q, STABILIZATION_ATTITUDE_REF_ZETA_R} +}; + +/* +static const float omega_p[] = STABILIZATION_ATTITUDE_REF_OMEGA_P; +static const float zeta_p[] = STABILIZATION_ATTITUDE_REF_ZETA_P; +static const float omega_q[] = STABILIZATION_ATTITUDE_REF_OMEGA_Q; +static const float zeta_q[] = STABILIZATION_ATTITUDE_REF_ZETA_Q; +static const float omega_r[] = STABILIZATION_ATTITUDE_REF_OMEGA_R; +static const float zeta_r[] = STABILIZATION_ATTITUDE_REF_ZETA_R; +*/ + +static void reset_psi_ref_from_body(void) { + stab_att_ref_euler.psi = ahrs.ltp_to_body_euler.psi; + stab_att_ref_rate.r = 0; + stab_att_ref_accel.r = 0; +} + +static void update_ref_quat_from_eulers(void) { + struct Int32RMat ref_rmat; + +#ifdef STICKS_RMAT312 + INT32_RMAT_OF_EULERS_312(ref_rmat, stab_att_ref_euler); +#else + INT32_RMAT_OF_EULERS_321(ref_rmat, stab_att_ref_euler); +#endif + INT32_QUAT_OF_RMAT(stab_att_ref_quat, ref_rmat); + INT32_QUAT_WRAP_SHORTEST(stab_att_ref_quat); +} + +void stabilization_attitude_ref_init(void) { + + INT_EULERS_ZERO(stab_att_sp_euler); + INT32_QUAT_ZERO( stab_att_sp_quat); + INT_EULERS_ZERO(stab_att_ref_euler); + INT32_QUAT_ZERO( stab_att_ref_quat); + INT_RATES_ZERO( stab_att_ref_rate); + INT_RATES_ZERO( stab_att_ref_accel); + + /* + for (int i = 0; i < STABILIZATION_ATTITUDE_GAIN_NB; i++) { + RATES_ASSIGN(stab_att_ref_model[i].omega, omega_p[i], omega_q[i], omega_r[i]); + RATES_ASSIGN(stab_att_ref_model[i].zeta, zeta_p[i], zeta_q[i], zeta_r[i]); + } + */ + +} + +void stabilization_attitude_ref_enter() +{ + reset_psi_ref_from_body(); + stabilization_attitude_sp_enter(); + memcpy(&stab_att_ref_quat, &stab_att_sp_quat, sizeof(struct Int32Quat)); + memset(&stab_att_ref_accel, 0, sizeof(struct Int32Rates)); + memset(&stab_att_ref_rate, 0, sizeof(struct Int32Rates)); + //update_ref_quat_from_eulers(); +} + +/* + * Reference + */ +#define DT_UPDATE (1./512.) +#define F_UPDATE_RES 9 + +#include "messages.h" +#include "mcu_periph/uart.h" +#include "downlink.h" + +void stabilization_attitude_ref_update() { + + /* integrate reference attitude */ + struct Int32Quat qdot; + INT32_QUAT_DERIVATIVE(qdot, stab_att_ref_rate, stab_att_ref_quat); + //QUAT_SMUL(qdot, qdot, RATE_BFP_OF_REAL(DT_UPDATE)); + QUAT_SMUL(qdot, qdot, 4); + QUAT_SMUL(qdot, qdot, DT_UPDATE); + QUAT_ADD(stab_att_ref_quat, qdot); + INT32_QUAT_NORMALIZE(stab_att_ref_quat); + + /* integrate reference rotational speeds */ + const struct Int32Rates delta_rate = { + stab_att_ref_accel.p >> ( F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC), + stab_att_ref_accel.q >> ( F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC), + stab_att_ref_accel.r >> ( F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC)}; + + //RATES_SMUL(delta_rate, stab_att_ref_accel, RATE_BFP_OF_REAL(DT_UPDATE)); + //RATES_SMUL(delta_rate, stab_att_ref_accel, DT_UPDATE); + RATES_ADD(stab_att_ref_rate, delta_rate); + + /* compute reference angular accelerations */ + struct Int32Quat err; + /* compute reference attitude error */ + INT32_QUAT_INV_COMP(err, stab_att_sp_quat, stab_att_ref_quat); + /* wrap it in the shortest direction */ + INT32_QUAT_WRAP_SHORTEST(err); + /* propagate the 2nd order linear model */ + + const struct Int32Rates accel_rate = { + ((int32_t)(-2.*ZETA_OMEGA_P) * (stab_att_ref_rate.p >> (REF_RATE_FRAC - REF_ACCEL_FRAC))) >> (ZETA_OMEGA_P_RES), + ((int32_t)(-2.*ZETA_OMEGA_Q) * (stab_att_ref_rate.q >> (REF_RATE_FRAC - REF_ACCEL_FRAC))) >> (ZETA_OMEGA_Q_RES), + ((int32_t)(-2.*ZETA_OMEGA_R) * (stab_att_ref_rate.r >> (REF_RATE_FRAC - REF_ACCEL_FRAC))) >> (ZETA_OMEGA_R_RES) }; + + const struct Int32Rates accel_angle = { + ((int32_t)(-OMEGA_2_P)* (err.qx >> (REF_ANGLE_FRAC - REF_ACCEL_FRAC))) >> (OMEGA_2_P_RES), + ((int32_t)(-OMEGA_2_Q)* (err.qy >> (REF_ANGLE_FRAC - REF_ACCEL_FRAC))) >> (OMEGA_2_Q_RES), + ((int32_t)(-OMEGA_2_R)* (err.qz >> (REF_ANGLE_FRAC - REF_ACCEL_FRAC))) >> (OMEGA_2_R_RES) }; + + RATES_SUM(stab_att_ref_accel, accel_rate, accel_angle); + + + /* + stab_att_ref_accel.p = -2.*stab_att_ref_model.zeta.p*stab_att_ref_model.omega.p*stab_att_ref_rate.p + - stab_att_ref_model.omega.p*stab_att_ref_model.omega.p*err.qx; + stab_att_ref_accel.q = -2.*stab_att_ref_model.zeta.q*stab_att_ref_model.omega.q*stab_att_ref_rate.q + - stab_att_ref_model.omega.q*stab_att_ref_model.omega.q*err.qy; + stab_att_ref_accel.r = -2.*stab_att_ref_model.zeta.r*stab_att_ref_model.omega.r*stab_att_ref_rate.r + - stab_att_ref_model.omega.r*stab_att_ref_model.omega.r*err.qz; + */ + + /* saturate acceleration */ + //const struct Int32Rates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q, -REF_ACCEL_MAX_R }; + //const struct Int32Rates MAX_ACCEL = { REF_ACCEL_MAX_P, REF_ACCEL_MAX_Q, REF_ACCEL_MAX_R }; + //RATES_BOUND_BOX(stab_att_ref_accel, MIN_ACCEL, MAX_ACCEL); + + /* compute ref_euler */ + INT32_EULERS_OF_QUAT(stab_att_ref_euler, stab_att_ref_quat); + +} diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h new file mode 100644 index 0000000000..672b6646d9 --- /dev/null +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h @@ -0,0 +1,73 @@ +/* + * 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 STABILIZATION_ATTITUDE_INT_REF_QUAT_INT_H +#define STABILIZATION_ATTITUDE_INT_REF_QUAT_INT_H + +#include "firmwares/rotorcraft/stabilization.h" +#include "firmwares/rotorcraft/stabilization/quat_setpoint.h" + +#include "subsystems/radio_control.h" +#include "math/pprz_algebra_float.h" + +#include "stabilization_attitude_ref_int.h" + +#define REF_ACCEL_FRAC 12 +#define REF_RATE_FRAC 16 +#define REF_ANGLE_FRAC 20 + +#define REF_ANGLE_PI BFP_OF_REAL(3.1415926535897932384626433832795029, REF_ANGLE_FRAC) +#define REF_ANGLE_TWO_PI BFP_OF_REAL(2.*3.1415926535897932384626433832795029, REF_ANGLE_FRAC) +#define ANGLE_REF_NORMALIZE(_a) { \ + while (_a > REF_ANGLE_PI) _a -= REF_ANGLE_TWO_PI; \ + while (_a < -REF_ANGLE_PI) _a += REF_ANGLE_TWO_PI; \ + } + + +#define RC_UPDATE_FREQ 40. +#define ROLL_COEF (STABILIZATION_ATTITUDE_SP_MAX_P / MAX_PPRZ) +#define ROLL_COEF_H (STABILIZATION_ATTITUDE_SP_MAX_P_H / MAX_PPRZ) +#define PITCH_COEF ( STABILIZATION_ATTITUDE_SP_MAX_THETA / MAX_PPRZ) +#define YAW_COEF (STABILIZATION_ATTITUDE_SP_MAX_PSI / MAX_PPRZ) + +#define ROLL_COEF_RATE (-STABILIZATION_ATTITUDE_SP_MAX_P / MAX_PPRZ) +#define PITCH_COEF_RATE ( STABILIZATION_ATTITUDE_SP_MAX_Q / MAX_PPRZ) +#define YAW_COEF_RATE ( STABILIZATION_ATTITUDE_SP_MAX_R / MAX_PPRZ) + +#define DEADBAND_EXCEEDED(VARIABLE, VALUE) ((VARIABLE > VALUE) || (VARIABLE < -VALUE)) +#define APPLY_DEADBAND(VARIABLE, VALUE) (DEADBAND_EXCEEDED(VARIABLE, VALUE) ? VARIABLE : 0.0) + +#define ROLL_DEADBAND_EXCEEDED() \ + (radio_control.values[RADIO_ROLL] > STABILIZATION_ATTITUDE_DEADBAND_A || \ + radio_control.values[RADIO_ROLL] < -STABILIZATION_ATTITUDE_DEADBAND_A) +#define PITCH_DEADBAND_EXCEEDED() \ + (radio_control.values[RADIO_PITCH] > STABILIZATION_ATTITUDE_DEADBAND_E || \ + radio_control.values[RADIO_PITCH] < -STABILIZATION_ATTITUDE_DEADBAND_E) +#define YAW_DEADBAND_EXCEEDED() \ + (radio_control.values[RADIO_YAW] > STABILIZATION_ATTITUDE_DEADBAND_R || \ + radio_control.values[RADIO_YAW] < -STABILIZATION_ATTITUDE_DEADBAND_R) + +#define STABILIZATION_ATTITUDE_READ_RC(_sp, _in_flight) do { stabilization_attitude_read_rc_absolute(_sp, _in_flight); } while(0) +#define STABILIZATION_ATTITUDE_RESET_PSI_REF(_sp) do {} while(0) + +void stabilization_attitude_ref_enter(void); +void stabilization_attitude_ref_schedule(uint8_t idx); + +#endif /* STABILIZATION_ATTITUDE_INT_REF_QUAT_INT_H */ diff --git a/sw/airborne/firmwares/rotorcraft/telemetry.h b/sw/airborne/firmwares/rotorcraft/telemetry.h index 65a755cbb6..59cc90d163 100644 --- a/sw/airborne/firmwares/rotorcraft/telemetry.h +++ b/sw/airborne/firmwares/rotorcraft/telemetry.h @@ -47,7 +47,9 @@ #include "subsystems/electrical.h" #include "subsystems/imu.h" -#include "booz_gps.h" +#ifdef USE_GPS +#include "subsystems/gps.h" +#endif #include "subsystems/ins.h" #include "subsystems/ahrs.h" //FIXME: wtf ??!! @@ -64,7 +66,7 @@ extern uint8_t telemetry_mode_Main_DefaultChannel; &_twi_blmc_nb_err, \ &radio_control.status, \ &radio_control.frame_rate, \ - &booz_gps_state.fix, \ + &gps.fix, \ &autopilot_mode, \ &autopilot_in_flight, \ &autopilot_motors_on, \ @@ -78,21 +80,21 @@ extern uint8_t telemetry_mode_Main_DefaultChannel; #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, \ + uint8_t fix = 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, \ &electrical.vsupply, \ - &cpu_time_sec \ - ); \ + &cpu_time_sec \ + ); \ } #endif /* USE_GPS */ @@ -418,6 +420,18 @@ extern uint8_t telemetry_mode_Main_DefaultChannel; #endif +#define PERIODIC_SEND_AHRS_REF_QUAT(_chan) { \ + DOWNLINK_SEND_AHRS_REF_QUAT(_chan, \ + &stab_att_ref_quat.qi, \ + &stab_att_ref_quat.qx, \ + &stab_att_ref_quat.qy, \ + &stab_att_ref_quat.qz, \ + &ahrs.ltp_to_body_quat.qi, \ + &ahrs.ltp_to_body_quat.qx, \ + &ahrs.ltp_to_body_quat.qy, \ + &ahrs.ltp_to_body_quat.qz); \ + } + #define PERIODIC_SEND_BOOZ2_AHRS_QUAT(_chan) { \ DOWNLINK_SEND_BOOZ2_AHRS_QUAT(_chan, \ &ahrs.ltp_to_imu_quat.qi, \ @@ -624,11 +638,10 @@ extern uint8_t telemetry_mode_Main_DefaultChannel; &guidance_h_accel_ref.y); \ } -#include "booz_gps.h" #include "firmwares/rotorcraft/navigation.h" -#define PERIODIC_SEND_BOOZ2_FP(_chan) { \ +#define PERIODIC_SEND_ROTORCRAFT_FP(_chan) { \ int32_t carrot_up = -guidance_v_z_sp; \ - DOWNLINK_SEND_BOOZ2_FP( _chan, \ + DOWNLINK_SEND_ROTORCRAFT_FP( _chan, \ &ins_enu_pos.x, \ &ins_enu_pos.y, \ &ins_enu_pos.z, \ @@ -647,26 +660,35 @@ extern uint8_t telemetry_mode_Main_DefaultChannel; } #ifdef USE_GPS -#define PERIODIC_SEND_BOOZ2_GPS(_chan) { \ - DOWNLINK_SEND_BOOZ2_GPS( _chan, \ - &booz_gps_state.ecef_pos.x, \ - &booz_gps_state.ecef_pos.y, \ - &booz_gps_state.ecef_pos.z, \ - &booz_gps_state.lla_pos.lat, \ - &booz_gps_state.lla_pos.lon, \ - &booz_gps_state.lla_pos.alt, \ - &booz_gps_state.ecef_vel.x, \ - &booz_gps_state.ecef_vel.y, \ - &booz_gps_state.ecef_vel.z, \ - &booz_gps_state.pacc, \ - &booz_gps_state.sacc, \ - &booz_gps_state.tow, \ - &booz_gps_state.pdop, \ - &booz_gps_state.num_sv, \ - &booz_gps_state.fix); \ +#define PERIODIC_SEND_GPS_INT(_chan) { \ + DOWNLINK_SEND_GPS_INT( _chan, \ + &gps.ecef_pos.x, \ + &gps.ecef_pos.y, \ + &gps.ecef_pos.z, \ + &gps.lla_pos.lat, \ + &gps.lla_pos.lon, \ + &gps.lla_pos.alt, \ + &gps.hmsl, \ + &gps.ecef_vel.x, \ + &gps.ecef_vel.y, \ + &gps.ecef_vel.z, \ + &gps.pacc, \ + &gps.sacc, \ + &gps.tow, \ + &gps.pdop, \ + &gps.num_sv, \ + &gps.fix); \ + static uint8_t i; \ + static uint8_t last_cnos[GPS_NB_CHANNELS]; \ + if (i == gps.nb_channels) i = 0; \ + if (i < gps.nb_channels && gps.svinfos[i].cno > 0 && gps.svinfos[i].cno != last_cnos[i]) { \ + DOWNLINK_SEND_SVINFO(DefaultChannel, &i, &gps.svinfos[i].svid, &gps.svinfos[i].flags, &gps.svinfos[i].qi, &gps.svinfos[i].cno, &gps.svinfos[i].elev, &gps.svinfos[i].azim); \ + last_cnos[i] = gps.svinfos[i].cno; \ + } \ + i++; \ } #else -#define PERIODIC_SEND_BOOZ2_GPS(_chan) {} +#define PERIODIC_SEND_GPS_INT(_chan) {} #endif #include "firmwares/rotorcraft/navigation.h" diff --git a/sw/airborne/gps_nmea.c b/sw/airborne/gps_nmea.c deleted file mode 100644 index 6d843b92b6..0000000000 --- a/sw/airborne/gps_nmea.c +++ /dev/null @@ -1,501 +0,0 @@ -/* - * - * Copyright (C) 2008 Marcus Wolschon - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ - -/** - * file gps_nmea.c - * brief Parser for the NMEA protocol - * - * This file is a drop-in replacement for gps_ubx.c - * - * TODO: THIS NMEA-PARSER IS NOT WELL TESTED AND INCOMPLETE!!! - * Status: - * Parsing GGA and RMC is complete, GSA and other records are - * incomplete. - */ - -#include -#include -#include -#ifdef LINUX -// do debug-output if run on the linux-target -#include -#endif - - -#include "generated/flight_plan.h" -#include "mcu_periph/uart.h" -#include "gps.h" -#include "subsystems/nav.h" -#include "latlong.h" - - -int32_t gps_lat; // latitude in degrees * 1e-7 -int32_t gps_lon; // longitude in degrees * 1e-7 -uint16_t gps_PDOP; //precision -bool_t gps_pos_available = FALSE; - -uint16_t gps_week; -uint32_t gps_itow; -int32_t gps_alt; -uint16_t gps_gspeed; // in cm/s -int16_t gps_climb; -int16_t gps_course; -int32_t gps_utm_east, gps_utm_north; -uint8_t gps_utm_zone; -uint8_t gps_mode; - -// true if parse_ubx() has a complete message and parse_gps_msg() shall parse it -volatile bool_t gps_msg_received = FALSE; - -uint8_t ubx_id, ubx_class; // unused -uint16_t gps_reset; // unused - -uint32_t gps_Pacc, gps_Sacc; -uint8_t gps_numSV; // number of satelites in view - -struct svinfo gps_svinfos[GPS_NB_CHANNELS]; -uint8_t gps_nb_channels; -uint8_t gps_nb_ovrn; // number if incomplete nmea-messages - - - - - -//////////////////////////////////////////////////////// -// uart-configuration - -void gps_init( void ) { -#ifdef GPS_CONFIGURE - gps_status_config = GPS_CONFIG_INIT; -#endif -} - -void ubxsend_cfg_rst(uint16_t bbr , uint8_t reset_mode) { -} - - -#ifdef GPS_CONFIGURE -/* GPS dynamic configuration */ - -void gps_configure_uart ( void ) { - //UbxSend_CFG_PRT(0x01, 0x0, 0x0, 0x000008D0, GPS_BAUD, UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0, 0x0); - //while (GpsUartRunning) ; /* FIXME */ - GpsUartInitParam( UART_BAUD(GPS_BAUD), UART_8N1, UART_FIFO_8); -} - -void gps_configure ( void ) { -} -#endif /* GPS_CONFIGURE */ - -//////////////////////////////////////////////////////// -// nmea-parser - - -/** - * The buffer, we store one nmea-line in - * for parsing. - */ -#define NMEA_MAXLEN 255 -char nmea_msg_buf[NMEA_MAXLEN]; -int nmea_msg_len = 0; - -int GpsFixValid() { - return gps_pos_available; -} - -/** - * parse GPGSA-nmea-messages stored in - * nmea_msg_buf . - */ -void parse_nmea_GPGSA() { - int i = 8; // current position in the message - char* endptr; // end of parsed substrings - - // attempt to reject empty packets right away - if(nmea_msg_buf[i]==',' && nmea_msg_buf[i+1]==',') { -#ifdef LINUX - printf("parse_nmea_GPGSA() - skipping empty message\n"); -#endif - return; - } - - // get auto2D/3D - // ignored - while(nmea_msg_buf[i++] != ',') { // next field: fix - if (i >= nmea_msg_len) { -#ifdef LINUX - printf("parse_nmea_GPGSA() - skipping incomplete message\n"); -#endif - return; - } - } - - // get 2D/3D-fix - // set gps_mode=3=3d, 2=2d, 1=no fix or 0 - gps_mode = atoi(&nmea_msg_buf[i]); - if (gps_mode == 1) - gps_mode = 0; -#ifdef LINUX - printf("parse_nmea_GPGSA() - gps_mode=%i (3=3D)\n", gps_mode); -#endif - while(nmea_msg_buf[i++] != ',') { // next field:sateline-number-0 - if (i >= nmea_msg_len) { -#ifdef LINUX - printf("parse_nmea_GPGSA() - skipping incomplete message\n"); -#endif - return; - } - } - - int satcount = 0; - - // TODO: get sateline-numbers for gps_svinfos -} - -/** - * parse GPRMC-nmea-messages stored in - * nmea_msg_buf . - */ -void parse_nmea_GPRMC() { - int i = 8; // current position in the message - char* endptr; // end of parsed substrings - - // attempt to reject empty packets right away - if(nmea_msg_buf[i]==',' && nmea_msg_buf[i+1]==',') { -#ifdef LINUX - printf("parse_nmea_GPRMC() - skipping empty message\n"); -#endif - return; - } - - // get time - // ignored - while(nmea_msg_buf[i++] != ',') { // next field: warning - if (i >= nmea_msg_len) { -#ifdef LINUX - printf("parse_nmea_GPRMC() - skipping incomplete message\n"); -#endif - return; - } - } - - // get warning - // ignored - while(nmea_msg_buf[i++] != ',') { // next field: lat - if (i >= nmea_msg_len) { -#ifdef LINUX - printf("parse_nmea_GPRMC() - skipping incomplete message\n"); -#endif - return; - } - } - // get lat - // ignored - while(nmea_msg_buf[i++] != ',') { // next field: N/S - if (i >= nmea_msg_len) { -#ifdef LINUX - printf("parse_nmea_GPRMC() - skipping incomplete message\n"); -#endif - return; - } - } - // get North/South - // ignored - while(nmea_msg_buf[i++] != ',') { // next field: lon - if (i >= nmea_msg_len) { -#ifdef LINUX - printf("parse_nmea_GPRMC() - skipping incomplete message\n"); -#endif - return; - } - } - // get lon - // ignored - while(nmea_msg_buf[i++] != ',') { // next field: E/W - if (i >= nmea_msg_len) { -#ifdef LINUX - printf("parse_nmea_GPRMC() - skipping incomplete message\n"); -#endif - return; - } - } - // get eath/west - // ignored - while(nmea_msg_buf[i++] != ',') { // next field: speed - if (i >= nmea_msg_len) { -#ifdef LINUX - printf("parse_nmea_GPRMC() - skipping incomplete message\n"); -#endif - return; - } - } - // get speed - double speed = strtod(&nmea_msg_buf[i], &endptr); - gps_gspeed = speed * 1.852 * 100 / (60*60); -#ifdef LINUX - printf("parse_nmea_GPRMC() - ground-speed=%f knot = %i cm/s\n", speed, gps_gspeed); -#endif - while(nmea_msg_buf[i++] != ',') { // next field: speed - if (i >= nmea_msg_len) { -#ifdef LINUX - printf("parse_nmea_GPRMC() - skipping incomplete message\n"); -#endif - return; - } - } - - -} - - -/** - * parse GPGGA-nmea-messages stored in - * nmea_msg_buf . - */ -void parse_nmea_GPGGA() { - int i = 8; // current position in the message - char* endptr; // end of parsed substrings - double degrees, minutesfrac; - - // attempt to reject empty packets right away - if(nmea_msg_buf[i]==',' && nmea_msg_buf[i+1]==',') { -#ifdef LINUX - printf("parse_nmea_GPGGA() - skipping empty message\n"); -#endif - return; - } - - // get UTC time [hhmmss.sss] - // ignored GpsInfo.PosLLA.TimeOfFix.f = strtod(&packet[i], &endptr); - while(nmea_msg_buf[i++] != ',') { // next field: latitude - if (i >= nmea_msg_len) { -#ifdef LINUX - printf("parse_nmea_GPGGA() - skipping incomplete message\n"); -#endif - return; - } - } - - // get latitude [ddmm.mmmmm] - double lat = strtod(&nmea_msg_buf[i], &endptr); - // convert to pure degrees [dd.dddd] format - minutesfrac = modf(lat/100, °rees); - lat = degrees + (minutesfrac*100)/60; - // convert to radians - //GpsInfo.PosLLA.lat.f *= (M_PI/180); - while(nmea_msg_buf[i++] != ',') { // next field: N/S indicator - if (i >= nmea_msg_len) { -#ifdef LINUX - printf("parse_nmea_GPGGA() - skipping incomplete message\n"); -#endif - return; - } - } - - // correct latitute for N/S - if(nmea_msg_buf[i] == 'S') - lat = -lat; - while(nmea_msg_buf[i++] != ',') { // next field: longitude - if (i >= nmea_msg_len) - return; - } - - gps_lat = lat * 1e7; // convert to fixed-point -#ifdef LINUX - printf("parse_nmea_GPGGA() - lat=%f gps_lat=%i\n", lat, gps_lat); -#endif - - // get longitude [ddmm.mmmmm] - double lon = strtod(&nmea_msg_buf[i], &endptr); - // convert to pure degrees [dd.dddd] format - minutesfrac = modf(lon/100, °rees); - lon = degrees + (minutesfrac*100)/60; - // convert to radians - //GpsInfo.PosLLA.lon.f *= (M_PI/180); - while(nmea_msg_buf[i++] != ',') { // next field: E/W indicator - if (i >= nmea_msg_len) - return; - } - - // correct latitute for E/W - if(nmea_msg_buf[i] == 'W') - lon = -lon; - while(nmea_msg_buf[i++] != ',') { // next field: position fix status - if (i >= nmea_msg_len) - return; - } - - gps_lon = lon * 1e7; // convert to fixed-point -#ifdef LINUX - printf("parse_nmea_GPGGA() - lon=%f gps_lon=%i\n", lon, gps_lon); -#endif - - latlong_utm_of(RadOfDeg(lat), RadOfDeg(lon), nav_utm_zone0); - - gps_utm_east = latlong_utm_x * 100; - gps_utm_north = latlong_utm_y * 100; - gps_utm_zone = nav_utm_zone0; - - - // position fix status - // 0 = Invalid, 1 = Valid SPS, 2 = Valid DGPS, 3 = Valid PPS - // check for good position fix - if( (nmea_msg_buf[i] != '0') && (nmea_msg_buf[i] != ',') ) { - gps_pos_available = TRUE; -#ifdef LINUX - printf("parse_nmea_GPGGA() - gps_pos_available == true\n"); -#endif - } else { - gps_pos_available = FALSE; -#ifdef LINUX - printf("parse_nmea_GPGGA() - gps_pos_available == false\n"); -#endif - } - while(nmea_msg_buf[i++] != ',') { // next field: satellites used - if (i >= nmea_msg_len) { -#ifdef LINUX - printf("parse_nmea_GPGGA() - skipping incomplete message\n"); -#endif - return; - } - } - - // get number of satellites used in GPS solution - gps_numSV = atoi(&nmea_msg_buf[i]); -#ifdef LINUX - printf("parse_nmea_GPGGA() - gps_numSatlitesUsed=%i\n", gps_numSV); -#endif - while(nmea_msg_buf[i++] != ',') { // next field: HDOP (horizontal dilution of precision) - if (i >= nmea_msg_len) { -#ifdef LINUX - printf("parse_nmea_GPGGA() - skipping incomplete message\n"); -#endif - return; - } - } - while(nmea_msg_buf[i++] != ',') { // next field: altitude - if (i >= nmea_msg_len) { -#ifdef LINUX - printf("parse_nmea_GPGGA() - skipping incomplete message\n"); -#endif - return; - } - } - - // get altitude (in meters) - double alt = strtod(&nmea_msg_buf[i], &endptr); - gps_alt = alt * 10; -#ifdef LINUX - printf("parse_nmea_GPGGA() - gps_alt=%i\n", gps_alt); -#endif - while(nmea_msg_buf[i++] != ',') { // next field: altitude units, always 'M' - if (i >= nmea_msg_len) - return; - } - while(nmea_msg_buf[i++] != ',') { // next field: geoid seperation - if (i >= nmea_msg_len) - return; - } - while(nmea_msg_buf[i++] != ',') { // next field: seperation units - if (i >= nmea_msg_len) - return; - } - while(nmea_msg_buf[i++] != ',') { // next field: DGPS age - if (i >= nmea_msg_len) - return; - } - while(nmea_msg_buf[i++] != ',') { // next field: DGPS station ID - if (i >= nmea_msg_len) - return; - } - //while(nmea_msg_buf[i++] != '*'); // next field: checksum -} - -/** - * parse_ubx() has a complete line. - * Find out what type of message it is and - * hand it to the parser for that type. - */ -void parse_gps_msg( void ) { - - if(nmea_msg_len > 7 && !strncmp(nmea_msg_buf + 1 , "$GPRMC", 6)) { - nmea_msg_buf[nmea_msg_len] = 0; -#ifdef LINUX - printf("parse_gps_msg() - parsing RMC gps-message \"%s\"\n",nmea_msg_buf); -#endif - parse_nmea_GPRMC(); - } else - if(nmea_msg_len > 7 && !strncmp(nmea_msg_buf + 1 , "$GPGGA", 6)) { - nmea_msg_buf[nmea_msg_len] = 0; -#ifdef LINUX - printf("parse_gps_msg() - parsing GGA gps-message \"%s\"\n",nmea_msg_buf); -#endif - parse_nmea_GPGGA(); - } else - if(nmea_msg_len > 7 && !strncmp(nmea_msg_buf + 1 , "$GPGSA", 6)) { - nmea_msg_buf[nmea_msg_len] = 0; -#ifdef LINUX - printf("parse_gps_msg() - parsing GSA gps-message \"%s\"\n",nmea_msg_buf); -#endif - parse_nmea_GPGSA(); - } else { - nmea_msg_buf[nmea_msg_len] = 0; -#ifdef LINUX - printf("parse_gps_msg() - ignoring unknown gps-message \"%s\" len=%i\n",nmea_msg_buf, nmea_msg_len); -#endif - } - - // reset message-buffer - nmea_msg_len = 0; -} - - -/** - * This is the actual parser. - * It reads one character at a time - * setting gps_msg_received to TRUE - * after a full line. - */ -void parse_ubx( uint8_t c ) { - - //reject empty lines - if (nmea_msg_len == 0) { - if (c == '\r' || c == '\n' || c == '$') - return; - } - - // fill the buffer, unless it's full - if (nmea_msg_len < NMEA_MAXLEN - 1) { - - // messages end with a linefeed - if (c == '\r' || c == '\n') { - gps_msg_received = TRUE; - } else { - nmea_msg_buf[nmea_msg_len] = c; - nmea_msg_len ++; - } - } - - if (nmea_msg_len >= NMEA_MAXLEN - 1) - gps_msg_received = TRUE; -} diff --git a/sw/airborne/gps_xsens.c b/sw/airborne/gps_xsens.c deleted file mode 100644 index 077d2353af..0000000000 --- a/sw/airborne/gps_xsens.c +++ /dev/null @@ -1,74 +0,0 @@ -/* - * $Id$ - * - * Copyright (C) 2005 Pascal Brisset, 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. - * - */ - -/** \file gps_xsens.c - * \brief GPS hardware for Xsens MTi-G - * - */ - -#include - -#include "gps.h" -#include "sys_time.h" -#include "generated/airframe.h" - -#include "mcu_periph/uart.h" -#include "messages.h" -#include "downlink.h" - -uint8_t gps_mode; -uint16_t gps_week; -uint32_t gps_itow; -int32_t gps_alt; -uint16_t gps_gspeed; -int16_t gps_climb; -int16_t gps_course; -int32_t gps_utm_east, gps_utm_north; -uint8_t gps_utm_zone; -int32_t gps_lat, gps_lon; -uint16_t gps_PDOP; -uint32_t gps_Pacc, gps_Sacc; -uint8_t gps_numSV; -uint8_t gps_configuring; - -uint16_t gps_reset; - -void reset_gps_watchdog(void) -{ - last_gps_msg_t = cpu_time_sec; -} - -volatile bool_t gps_msg_received; -bool_t gps_pos_available; -uint8_t gps_nb_ovrn; - -uint8_t gps_nb_channels; - -struct svinfo gps_svinfos[GPS_NB_CHANNELS]; - - -void gps_init( void ) {} -void gps_configure( void ) {} -void parse_gps_msg( void ) {} -void gps_configure_uart( void ) {} diff --git a/sw/airborne/lisa/lisa_stm_passthrough_main.c b/sw/airborne/lisa/lisa_stm_passthrough_main.c index 2a23a7c705..96a5c5e13c 100644 --- a/sw/airborne/lisa/lisa_stm_passthrough_main.c +++ b/sw/airborne/lisa/lisa_stm_passthrough_main.c @@ -26,7 +26,7 @@ #include "mcu_periph/uart.h" #include "sys_time.h" #include "downlink.h" -#include "booz/booz2_commands.h" +#include "firmwares/rotorcraft/commands.h" #include "actuators.h" #include "actuators/actuators_pwm.h" #include "subsystems/imu.h" diff --git a/sw/airborne/lisa/test/lisa_test_actuators_mkk.c b/sw/airborne/lisa/test/lisa_test_actuators_mkk.c index bc3e4c8f64..50ad516aad 100644 --- a/sw/airborne/lisa/test/lisa_test_actuators_mkk.c +++ b/sw/airborne/lisa/test/lisa_test_actuators_mkk.c @@ -24,7 +24,7 @@ #include "mcu.h" #include "sys_time.h" -#include "booz2_commands.h" +#include "firmwares/rotorcraft/commands.h" #include "actuators.h" #include "downlink.h" @@ -89,10 +89,10 @@ static inline void main_periodic_task( void ) { if (i>1000) { /* set actuators */ - booz2_commands[COMMAND_PITCH] = 0; - booz2_commands[COMMAND_ROLL] = 0; - booz2_commands[COMMAND_YAW] = 20; - booz2_commands[COMMAND_THRUST] = 0; + commands[COMMAND_PITCH] = 0; + commands[COMMAND_ROLL] = 0; + commands[COMMAND_YAW] = 20; + commands[COMMAND_THRUST] = 0; // actuators_set(TRUE); actuators_set(FALSE); } diff --git a/sw/airborne/math/pprz_algebra_float.h b/sw/airborne/math/pprz_algebra_float.h index 9bb7f5efb1..5a31345f6c 100644 --- a/sw/airborne/math/pprz_algebra_float.h +++ b/sw/airborne/math/pprz_algebra_float.h @@ -518,7 +518,7 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) { FLOAT_QUAT_NORMALIZE(_a2c); \ } -/* _a2c = _a2b comp _b2c , aka _a2c = _b2c * _a2b */ +/* _a2c = _a2b comp _b2c , aka _a2c = _a2b * _b2c */ #define FLOAT_QUAT_COMP(_a2c, _a2b, _b2c) { \ (_a2c).qi = (_a2b).qi*(_b2c).qi - (_a2b).qx*(_b2c).qx - (_a2b).qy*(_b2c).qy - (_a2b).qz*(_b2c).qz; \ (_a2c).qx = (_a2b).qi*(_b2c).qx + (_a2b).qx*(_b2c).qi + (_a2b).qy*(_b2c).qz - (_a2b).qz*(_b2c).qy; \ @@ -526,8 +526,7 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) { (_a2c).qz = (_a2b).qi*(_b2c).qz + (_a2b).qx*(_b2c).qy - (_a2b).qy*(_b2c).qx + (_a2b).qz*(_b2c).qi; \ } -/* Quaternion multiplication q_a2c = q_b2c * q_a2b */ -#define FLOAT_QUAT_MULT(_a2c, _b2c, _a2b) FLOAT_QUAT_COMP(_a2c, _a2b, _b2c) +#define FLOAT_QUAT_MULT(_a2c, _a2b, _b2c) FLOAT_QUAT_COMP(_a2c, _a2b, _b2c) /* _a2b = _a2c comp_inv _b2c , aka _a2b = _a2c * inv(_b2c) */ #define FLOAT_QUAT_COMP_INV_NORM_SHORTEST(_a2b, _a2c, _b2c) { \ diff --git a/sw/airborne/math/pprz_algebra_int.h b/sw/airborne/math/pprz_algebra_int.h index f8cc0c926a..8ae610cd3d 100644 --- a/sw/airborne/math/pprz_algebra_int.h +++ b/sw/airborne/math/pprz_algebra_int.h @@ -579,8 +579,22 @@ struct Int64Vect3 { (_b2c).qz = ((_a2b).qi*(_a2c).qz - (_a2b).qx*(_a2c).qy + (_a2b).qy*(_a2c).qx - (_a2b).qz*(_a2c).qi)>>INT32_QUAT_FRAC; \ } +/* _a2c = _a2b comp _b2c , aka _a2c = _a2b * _b2c */ +#define INT32_QUAT_COMP_NORM_SHORTEST(_a2c, _a2b, _b2c) { \ + INT32_QUAT_COMP(_a2c, _a2b, _b2c); \ + INT32_QUAT_WRAP_SHORTEST(_a2c); \ + INT32_QUAT_NORMALIZE(_a2c); \ + } +/* _qd = -0.5*omega(_r) * _q */ +#define INT32_QUAT_DERIVATIVE(_qd, _r, _q) { \ + (_qd).qi = (-1*( (_r).p*(_q).qx/2 + (_r).q*(_q).qy + (_r).r*(_q).qz))>>INT32_RATE_FRAC; \ + (_qd).qx = (-1*(-(_r).p*(_q).qi/2 - (_r).r*(_q).qy + (_r).q*(_q).qz))>>INT32_RATE_FRAC; \ + (_qd).qy = (-1*(-(_r).q*(_q).qi/2 + (_r).r*(_q).qx - (_r).p*(_q).qz))>>INT32_RATE_FRAC; \ + (_qd).qz = (-1*(-(_r).r*(_q).qi/2 - (_r).q*(_q).qx + (_r).p*(_q).qy))>>INT32_RATE_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; \ @@ -662,6 +676,18 @@ struct Int64Vect3 { INT_MULT_RSHIFT(-s_phi2, s_th_c_ps, INT32_TRIG_FRAC + INT32_TRIG_FRAC - INT32_QUAT_FRAC); \ } +#define INT32_QUAT_OF_AXIS_ANGLE(_q, _uv, _an) { \ + int32_t san2; \ + PPRZ_ITRIG_SIN(san2, (_an/2)); \ + int32_t can2; \ + PPRZ_ITRIG_COS(can2, (_an/2)); \ + _q.qi = can2; \ + _q.qx = san2 * _uv.x; \ + _q.qy = san2 * _uv.y; \ + _q.qz = san2 * _uv.z; \ + } + + #define INT32_QUAT_OF_RMAT(_q, _r) { \ const int32_t tr = RMAT_TRACE(_r); \ diff --git a/sw/airborne/math/pprz_geodetic_double.c b/sw/airborne/math/pprz_geodetic_double.c index 5a411f62ff..371eac115a 100644 --- a/sw/airborne/math/pprz_geodetic_double.c +++ b/sw/airborne/math/pprz_geodetic_double.c @@ -138,34 +138,17 @@ double gc_of_gd_lat_d(double gd_lat, double hmsl) { } +#include "math/pprz_geodetic_utm.h" -/* Computation for the WGS84 geoid only */ -#define E 0.08181919106 -#define K0 0.9996 -#define DELTA_EAST 500000. -#define DELTA_NORTH 0. -#define A 6378137.0 -#define N (K0*A) - -#define LambdaOfUtmZone(utm_zone) RadOfDeg((utm_zone-1)*6-180+3) - -static const float serie_coeff_proj_mercator[5] = { - 0.99832429842242842444, - 0.00083632803657738403, - 0.00000075957783563707, - 0.00000000119563131778, - 0.00000000000241079916 -}; - -static inline double isometric_latitude(double phi, double e) { +static inline double isometric_latitude_d(double phi, double e) { return log (tan (M_PI_4 + phi / 2.0)) - e / 2.0 * log((1.0 + e * sin(phi)) / (1.0 - e * sin(phi))); } -static inline double isometric_latitude_fast(double phi) { +static inline double isometric_latitude_fast_d(double phi) { return log (tan (M_PI_4 + phi / 2.0)); } -static inline double inverse_isometric_latitude(double lat, double e, double epsilon) { +static inline double inverse_isometric_latitude_d(double lat, double e, double epsilon) { double exp_l = exp(lat); double phi0 = 2 * atan(exp_l) - M_PI_2; double phi_; @@ -204,27 +187,26 @@ static inline double inverse_isometric_latitude(double lat, double e, double eps CI(v); \ } -void lla_of_utm(struct LlaCoor_d* out, struct UTMCoor_d* in) { +void lla_of_utm_d(struct LlaCoor_d* lla, struct UtmCoor_d* utm) { - // struct DoubleVect2 v = {in->east - YS, in->north - XS}; - struct DoubleVect2 v = {in->north - DELTA_NORTH, in->east - DELTA_EAST}; + struct DoubleVect2 v = {utm->north - DELTA_NORTH, utm->east - DELTA_EAST}; double scale = 1 / N / serie_coeff_proj_mercator[0]; VECT2_SMUL(v, v, scale); // first order taylor serie of something ? struct DoubleVect2 v1; VECT2_SMUL(v1, v, 2.); - CSin(v1) + CSin(v1); VECT2_SMUL(v1, v1, serie_coeff_proj_mercator[1]); VECT2_SUB(v, v1); - double lambda_c = LambdaOfUtmZone(in->zone); - out->lon = lambda_c + atan(sinh(v.y) / cos(v.x)); + double lambda_c = LambdaOfUtmZone(utm->zone); + lla->lon = lambda_c + atan(sinh(v.y) / cos(v.x)); double phi = asin (sin(v.x) / cosh(v.y)); - double il = isometric_latitude_fast(phi); - out->lat = inverse_isometric_latitude(il, E, 1e-8); + double il = isometric_latitude_fast_d(phi); + lla->lat = inverse_isometric_latitude_d(il, E, 1e-8); // copy alt above reference ellipsoid - out->alt = in->alt; + lla->alt = utm->alt; } diff --git a/sw/airborne/math/pprz_geodetic_double.h b/sw/airborne/math/pprz_geodetic_double.h index a0c7128fee..69df6f8e44 100644 --- a/sw/airborne/math/pprz_geodetic_double.h +++ b/sw/airborne/math/pprz_geodetic_double.h @@ -78,7 +78,7 @@ struct EnuCoor_d { /** * @brief position in UTM coordinates * Units: meters */ -struct UTMCoor_d { +struct UtmCoor_d { double north; ///< in meters double east; ///< in meters double alt; ///< in meters above WGS84 reference ellipsoid @@ -97,7 +97,7 @@ struct LtpDef_d { double hmsl; ///< height in meters above mean sea level }; -extern void lla_of_utm(struct LlaCoor_d* out, struct UTMCoor_d* in); +extern void lla_of_utm_d(struct LlaCoor_d* out, struct UtmCoor_d* in); extern void ltp_def_from_ecef_d(struct LtpDef_d* def, struct EcefCoor_d* ecef); extern void lla_of_ecef_d(struct LlaCoor_d* out, struct EcefCoor_d* in); extern void ecef_of_lla_d(struct EcefCoor_d* out, struct LlaCoor_d* in); diff --git a/sw/airborne/math/pprz_geodetic_float.c b/sw/airborne/math/pprz_geodetic_float.c index 9d5f4fdb5e..4f0ef30830 100644 --- a/sw/airborne/math/pprz_geodetic_float.c +++ b/sw/airborne/math/pprz_geodetic_float.c @@ -215,3 +215,88 @@ void ecef_of_lla_f(struct EcefCoor_f* out, struct LlaCoor_f* in) { out->z = (a_chi*(1. - e2) + in->alt) * sin_lat; } + + + +#include "math/pprz_geodetic_utm.h" + +struct complex { float re; float im; }; +#define CScal(k, z) { z.re *= k; z.im *= k; } +#define CAdd(z1, z2) { z2.re += z1.re; z2.im += z1.im; } +#define CSub(z1, z2) { z2.re -= z1.re; z2.im -= z1.im; } +#define CI(z) { float tmp = z.re; z.re = - z.im; z.im = tmp; } +#define CExp(z) { float e = exp(z.re); z.re = e*cos(z.im); z.im = e*sin(z.im); } +/* Expanded #define CSin(z) { CI(z); struct complex _z = {-z.re, -z.im}; CExp(z); CExp(_z); CSub(_z, z); CScal(-0.5, z); CI(z); } */ + +#define CSin(z) { CI(z); struct complex _z = {-z.re, -z.im}; float e = exp(z.re); float cos_z_im = cos(z.im); z.re = e*cos_z_im; float sin_z_im = sin(z.im); z.im = e*sin_z_im; _z.re = cos_z_im/e; _z.im = -sin_z_im/e; CSub(_z, z); CScal(-0.5, z); CI(z); } + + +static inline float isometric_latitude_f(float phi, float e) { + return log (tan (M_PI_4 + phi / 2.0)) - e / 2.0 * log((1.0 + e * sin(phi)) / (1.0 - e * sin(phi))); +} + +static inline float isometric_latitude_fast_f(float phi) { + return log (tan (M_PI_4 + phi / 2.0)); +} + +static inline float inverse_isometric_latitude_f(float lat, float e, float epsilon) { + float exp_l = exp(lat); + float phi0 = 2 * atan(exp_l) - M_PI_2; + float phi_; + uint8_t max_iter = 3; /* To be sure to return */ + + do { + phi_ = phi0; + float sin_phi = e * sin(phi_); + phi0 = 2 * atan (pow((1 + sin_phi) / (1. - sin_phi), e/2.) * exp_l) - M_PI_2; + max_iter--; + } while (max_iter && fabs(phi_ - phi0) > epsilon); + return phi0; +} + +void utm_of_lla_f(struct UtmCoor_f* utm, struct LlaCoor_f* lla) { + float lambda_c = LambdaOfUtmZone(utm->zone); + float ll = isometric_latitude_f(lla->lat , E); + float dl = lla->lon - lambda_c; + float phi_ = asin(sin(dl) / cosh(ll)); + float ll_ = isometric_latitude_fast_f(phi_); + float lambda_ = atan(sinh(ll) / cos(dl)); + struct complex z_ = { lambda_, ll_ }; + CScal(serie_coeff_proj_mercator[0], z_); + uint8_t k; + for(k = 1; k < 3; k++) { + struct complex z = { lambda_, ll_ }; + CScal(2*k, z); + CSin(z); + CScal(serie_coeff_proj_mercator[k], z); + CAdd(z, z_); + } + CScal(N, z_); + utm->east = DELTA_EAST + z_.im; + utm->north = DELTA_NORTH + z_.re; +} + +void lla_of_utm_f(struct LlaCoor_f* lla, struct UtmCoor_f* utm) { + float scale = 1 / N / serie_coeff_proj_mercator[0]; + float real = (utm->north - DELTA_NORTH) * scale; + float img = (utm->east - DELTA_EAST) * scale; + struct complex z = { real, img }; + + uint8_t k; + for(k = 1; k < 2; k++) { + struct complex z_ = { real, img }; + CScal(2*k, z_); + CSin(z_); + CScal(serie_coeff_proj_mercator_inverse[k], z_); + CSub(z_, z); + } + + float lambda_c = LambdaOfUtmZone(utm->zone); + lla->lon = lambda_c + atan (sinh(z.im) / cos(z.re)); + float phi_ = asin (sin(z.re) / cosh(z.im)); + float il = isometric_latitude_fast_f(phi_); + lla->lat = inverse_isometric_latitude_f(il, E, 1e-8); + + // copy alt above reference ellipsoid + lla->alt = utm->alt; +} diff --git a/sw/airborne/math/pprz_geodetic_float.h b/sw/airborne/math/pprz_geodetic_float.h index e40e502fb5..9d021880ad 100644 --- a/sw/airborne/math/pprz_geodetic_float.h +++ b/sw/airborne/math/pprz_geodetic_float.h @@ -32,6 +32,7 @@ #include "pprz_geodetic.h" #include "pprz_algebra_float.h" +#include "std.h" /** * @brief vector in EarthCenteredEarthFixed coordinates @@ -74,6 +75,16 @@ struct EnuCoor_f { float z; ///< in meters }; +/** + * @brief position in UTM coordinates + * Units: meters */ +struct UtmCoor_f { + float north; ///< in meters + float east; ///< in meters + float alt; ///< in meters above WGS84 reference ellipsoid + uint8_t zone; ///< UTM zone number +}; + /** * @brief definition of the local (flat earth) coordinate system * @details Defines the origin of the local coordinate system @@ -86,6 +97,8 @@ struct LtpDef_f { float hmsl; ///< Height above mean sea level in meters }; +extern void lla_of_utm_f(struct LlaCoor_f* lla, struct UtmCoor_f* utm); +extern void utm_of_lla_f(struct UtmCoor_f* utm, struct LlaCoor_f* lla); extern void ltp_def_from_ecef_f(struct LtpDef_f* def, struct EcefCoor_f* ecef); extern void ltp_def_from_lla_f(struct LtpDef_f* def, struct LlaCoor_f* lla); extern void lla_of_ecef_f(struct LlaCoor_f* out, struct EcefCoor_f* in); diff --git a/sw/airborne/math/pprz_geodetic_int.c b/sw/airborne/math/pprz_geodetic_int.c index 99153cc38c..4d332d5363 100644 --- a/sw/airborne/math/pprz_geodetic_int.c +++ b/sw/airborne/math/pprz_geodetic_int.c @@ -211,7 +211,7 @@ void lla_of_ecef_i(struct LlaCoor_i* out, struct EcefCoor_i* in) { /* convert the output to fixed point */ out->lon = (int32_t)rint(EM7RAD_OF_RAD(out_d.lon)); out->lat = (int32_t)rint(EM7RAD_OF_RAD(out_d.lat)); - out->alt = (int32_t)CM_OF_M(out_d.alt); + out->alt = (int32_t)MM_OF_M(out_d.alt); } @@ -221,7 +221,7 @@ void ecef_of_lla_i(struct EcefCoor_i* out, struct LlaCoor_i* in) { struct LlaCoor_d in_d; in_d.lon = RAD_OF_EM7RAD((double)in->lon); in_d.lat = RAD_OF_EM7RAD((double)in->lat); - in_d.alt = M_OF_CM((double)in->alt); + in_d.alt = M_OF_MM((double)in->alt); /* calls the floating point transformation */ struct EcefCoor_d out_d; ecef_of_lla_d(&out_d, &in_d); diff --git a/sw/airborne/math/pprz_geodetic_int.h b/sw/airborne/math/pprz_geodetic_int.h index 91a5770fd6..7813bff7a7 100644 --- a/sw/airborne/math/pprz_geodetic_int.h +++ b/sw/airborne/math/pprz_geodetic_int.h @@ -56,7 +56,7 @@ struct EcefCoor_i { struct LlaCoor_i { int32_t lon; ///< in radians*1e7 int32_t lat; ///< in radians*1e7 - int32_t alt; ///< in centimeters above WGS84 reference ellipsoid + int32_t alt; ///< in millimeters above WGS84 reference ellipsoid }; /** @@ -80,10 +80,10 @@ struct EnuCoor_i { /** * @brief position in UTM coordinates */ -struct UTMCoor_i { +struct UtmCoor_i { int32_t north; ///< in centimeters int32_t east; ///< in centimeters - int32_t alt; ///< in centimeters above WGS84 reference ellipsoid + int32_t alt; ///< in millimeters above WGS84 reference ellipsoid uint8_t zone; ///< UTM zone number }; @@ -96,7 +96,7 @@ struct LtpDef_i { struct EcefCoor_i ecef; ///< Reference point in ecef struct LlaCoor_i lla; ///< Reference point in lla struct Int32Mat33 ltp_of_ecef; ///< Rotation matrix - int32_t hmsl; ///< Height above mean sea level + int32_t hmsl; ///< Height above mean sea level in mm }; extern void ltp_def_from_ecef_i(struct LtpDef_i* def, struct EcefCoor_i* ecef); @@ -118,8 +118,10 @@ extern void ecef_of_ned_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, st #define CM_OF_M(_m) ((_m)*1e2) #define M_OF_CM(_cm) ((_cm)/1e2) -#define EM7RAD_OF_RAD(_r) (_r*1e7) -#define RAD_OF_EM7RAD(_r) (_r/1e7) +#define MM_OF_M(_m) ((_m)*1e3) +#define M_OF_MM(_mm) ((_mm)/1e3) +#define EM7RAD_OF_RAD(_r) ((_r)*1e7) +#define RAD_OF_EM7RAD(_r) ((_r)/1e7) #define INT32_VECT3_ENU_OF_NED(_o, _i) { \ (_o).x = (_i).y; \ @@ -144,13 +146,13 @@ extern void ecef_of_ned_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, st #define LLA_BFP_OF_REAL(_o, _i) { \ (_o).lat = (int32_t)EM7RAD_OF_RAD((_i).lat); \ (_o).lon = (int32_t)EM7RAD_OF_RAD((_i).lon); \ - (_o).alt = (int32_t)CM_OF_M((_i).alt); \ + (_o).alt = (int32_t)MM_OF_M((_i).alt); \ } #define LLA_FLOAT_OF_BFP(_o, _i) { \ (_o).lat = (float)RAD_OF_EM7RAD((_i).lat); \ (_o).lon = (float)RAD_OF_EM7RAD((_i).lon); \ - (_o).alt = (float)M_OF_CM((_i).alt); \ + (_o).alt = (float)M_OF_MM((_i).alt); \ } #define NED_BFP_OF_REAL(_o, _i) { \ diff --git a/sw/airborne/math/pprz_geodetic_utm.h b/sw/airborne/math/pprz_geodetic_utm.h new file mode 100644 index 0000000000..83ef1db282 --- /dev/null +++ b/sw/airborne/math/pprz_geodetic_utm.h @@ -0,0 +1,27 @@ + + +/* Computation for the WGS84 geoid only */ +#define E 0.08181919106 +#define K0 0.9996 +#define DELTA_EAST 500000. +#define DELTA_NORTH 0. +#define A 6378137.0 +#define N (K0*A) + +#define LambdaOfUtmZone(utm_zone) RadOfDeg((utm_zone-1)*6-180+3) + +static const float serie_coeff_proj_mercator[5] = { + 0.99832429842242842444, + 0.00083632803657738403, + 0.00000075957783563707, + 0.00000000119563131778, + 0.00000000000241079916 +}; + +static const float serie_coeff_proj_mercator_inverse[5] = { + 0.998324298422428424, + 0.000837732168742475825, + 5.90586914811817062e-08, + 1.6734091890305064e-10, + 2.13883575853313883e-13 +}; diff --git a/sw/airborne/math/pprz_geodetic_wgs84.h b/sw/airborne/math/pprz_geodetic_wgs84.h new file mode 100644 index 0000000000..640bd7be30 --- /dev/null +++ b/sw/airborne/math/pprz_geodetic_wgs84.h @@ -0,0 +1,71 @@ +#ifndef PPRZ_GEODETIC_WGS84 +#define PPRZ_GEODETIC_WGS84 + +/* + +Ten by Ten Degree WGS-84 Geoid Heights from -180 to +170 Degrees of Longitude + +Geoid height approximations in meters + +Source: +Defense Mapping Agency. 12 Jan 1987. GPS UE Relevant WGS-84 Data Base Package. Washington, DC: Defense Mapping Agency + +Link: +http://www.colorado.edu/geography/gcraft/notes/datum/geoid84.html + +rows are from -180 to +170 starting north +90 to south-90 + + +*/ + + +#define WGS84_H(x,y) ((float) pprz_geodetic_wgs84_int[(y)][(x)]) + +#define WGS84_ELLIPSOID_TO_GEOID(_Lat,_Lon,_diff) { \ + float x = (180.0f + DegOfRad(_Lon)) / 10.0f; \ + Bound(x,0.0f,35.99999f); \ + float y = (90.0f - DegOfRad(_Lat)) / 10.0f; \ + Bound(y,0.0f,17.99999f); \ + uint8_t ex1 = (uint8_t) x; \ + uint8_t ex2 = ex1 + 1; \ + if (ex2 >= 36) ex2 = 0; \ + uint8_t ey1 = (uint8_t) y; \ + uint8_t ey2 = ey1 + 1; \ + float lin_x = x - ((float) ex1); \ + float lin_y = y - ((float) ey1); \ + float h11 = (1.0f - lin_x) * (1.0f - lin_y) * WGS84_H(ex1,ey1); \ + float h12 = lin_x * (1.0f - lin_y) * WGS84_H(ex2,ey1); \ + float h21 = (1.0f - lin_x) * lin_y * WGS84_H(ex1,ey2); \ + float h22 = lin_x * lin_y * WGS84_H(ex2,ey2); \ + _diff = h11 + h12 + h21 + h22; \ +} + + + + +const int8_t pprz_geodetic_wgs84_int[19][36] = +{ + {13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13}, + {3,1,-2,-3,-3,-3,-1,3,1,5,9,11,19,27,31,34,33,34,33,34,28,23,17,13,9,4,4,1,-2,-2,0,2,3,2,1,1}, + {2,2,1,-1,-3,-7,-14,-24,-27,-25,-19,3,24,37,47,60,61,58,51,43,29,20,12,5,-2,-10,-14,-12,-10,-14,-12,-6,-2,3,6,4}, + {2,9,17,10,13,1,-14,-30,-39,-46,-42,-21,6,29,49,65,60,57,47,41,21,18,14,7,-3,-22,-29,-32,-32,-26,-15,-2,13,17,19,6}, + {-8,8,8,1,-11,-19,-16,-18,-22,-35,-40,-26,-12,24,45,63,62,59,47,48,42,28,12,-10,-19,-33,-43,-42,-43,-29,-2,17,23,22,6,2}, + {-12,-10,-13,-20,-31,-34,-21,-16,-26,-34,-33,-35,-26,2,33,59,52,51,52,48,35,40,33,-9,-28,-39,-48,-59,-50,-28,3,23,37,18,-1,-11}, + {-7,-5,-8,-15,-28,-40,-42,-29,-22,-26,-32,-51,-40,-17,17,31,34,44,36,28,29,17,12,-20,-15,-40,-33,-34,-34,-28,7,29,43,20,4,-6}, + {5,10,7,-7,-23,-39,-47,-34,-9,-10,-20,-45,-48,-32,-9,17,25,31,31,26,15,6,1,-29,-44,-61,-67,-59,-36,-11,21,39,49,39,22,10}, + {13,12,11,2,-11,-28,-38,-29,-10,3,1,-11,-41,-42,-16,3,17,33,22,23,2,-3,-7,-36,-59,-90,-95,-63,-24,12,53,60,58,46,36,26}, + {22,16,17,13,1,-12,-23,-20,-14,-3,14,10,-15,-27,-18,3,12,20,18,12,-13,-9,-28,-49,-62,-89,-102,-63,-9,33,58,73,74,63,50,32}, + {36,22,11,6,-1,-8,-10,-8,-11,-9,1,32,4,-18,-13,-9,4,14,12,13,-2,-14,-25,-32,-38,-60,-75,-63,-26,0,35,52,68,76,64,52}, + {51,27,10,0,-9,-11,-5,-2,-3,-1,9,35,20,-5,-6,-5,0,13,17,23,21,8,-9,-10,-11,-20,-40,-47,-45,-25,5,23,45,58,57,63}, + {46,22,5,-2,-8,-13,-10,-7,-4,1,9,32,16,4,-8,4,12,15,22,27,34,29,14,15,15,7,-9,-25,-37,-39,-23,-14,15,33,34,45}, + {21,6,1,-7,-12,-12,-12,-10,-7,-1,8,23,15,-2,-6,6,21,24,18,26,31,33,39,41,30,24,13,-2,-20,-32,-33,-27,-14,-2,5,20}, + {-15,-18,-18,-16,-17,-15,-10,-10,-8,-2,6,14,13,3,3,10,20,27,25,26,34,39,45,45,38,39,28,13,-1,-15,-22,-22,-18,-15,-14,-10}, + {-45,-43,-37,-32,-30,-26,-23,-22,-16,-10,-2,10,20,20,21,24,22,17,16,19,25,30,35,35,33,30,27,10,-2,-14,-23,-30,-33,-29,-35,-43}, + {-61,-60,-61,-55,-49,-44,-38,-31,-25,-16,-6,1,4,5,4,2,6,12,16,16,17,21,20,26,26,22,16,10,-1,-16,-29,-36,-46,-55,-54,-59}, + {-53,-54,-55,-52,-48,-42,-38,-38,-29,-26,-26,-24,-23,-21,-19,-16,-12,-8,-4,-1,1,4,4,6,5,4,2,-6,-15,-24,-33,-40,-48,-50,-53,-52}, + {-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30} +}; + + + +#endif diff --git a/sw/airborne/mcu_periph/uart.h b/sw/airborne/mcu_periph/uart.h index 848fcbad74..554fc6b9aa 100644 --- a/sw/airborne/mcu_periph/uart.h +++ b/sw/airborne/mcu_periph/uart.h @@ -47,6 +47,8 @@ extern bool_t uart0_check_free_space( uint8_t len); #define Uart0TxRunning uart0_tx_running #define Uart0InitParam uart0_init_param +#define UART0TxRunning uart0_tx_running +#define UART0InitParam uart0_init_param /* I want to trigger USE_UART and generate macros with the makefile same variable */ #define UART0Init Uart0Init @@ -71,6 +73,8 @@ extern bool_t uart1_check_free_space( uint8_t len); #define Uart1TxRunning uart1_tx_running #define Uart1InitParam uart1_init_param +#define UART1TxRunning uart1_tx_running +#define UART1InitParam uart1_init_param #define UART1Init Uart1Init #define UART1CheckFreeSpace Uart1CheckFreeSpace diff --git a/sw/airborne/modules/airborne_ant_track/airborne_ant_track.c b/sw/airborne/modules/airborne_ant_track/airborne_ant_track.c index 1e6ddb8dd0..608b9b242c 100755 --- a/sw/airborne/modules/airborne_ant_track/airborne_ant_track.c +++ b/sw/airborne/modules/airborne_ant_track/airborne_ant_track.c @@ -89,8 +89,8 @@ float airborne_ant_pan_servo = 0; svPlanePosition.fy = estimator_x; svPlanePosition.fz = estimator_z; - Home_Position.fx = waypoints[WP_HOME].y; - Home_Position.fy = waypoints[WP_HOME].x; + Home_Position.fx = WaypointY(WP_HOME); + Home_Position.fy = WaypointX(WP_HOME); Home_Position.fz = waypoints[WP_HOME].a; /* distance between plane and object */ diff --git a/sw/airborne/modules/cam_control/cam.c b/sw/airborne/modules/cam_control/cam.c index 94fd11d943..5e40d64ad6 100644 --- a/sw/airborne/modules/cam_control/cam.c +++ b/sw/airborne/modules/cam_control/cam.c @@ -28,7 +28,7 @@ #include #include "cam.h" -#include "subsystems/navigation/common_nav.h" +#include "subsystems/navigation/common_nav.h" //needed for WaypointX, WaypointY and ground_alt #include "autopilot.h" #include "generated/flight_plan.h" #include "estimator.h" @@ -190,8 +190,8 @@ void cam_nadir( void ) { void cam_waypoint_target( void ) { if (cam_target_wp < nb_waypoint) { - cam_target_x = waypoints[cam_target_wp].x; - cam_target_y = waypoints[cam_target_wp].y; + cam_target_x = WaypointX(cam_target_wp); + cam_target_y = WaypointY(cam_target_wp); } cam_target_alt = ground_alt; cam_target(); diff --git a/sw/airborne/modules/digital_cam/dc.c b/sw/airborne/modules/digital_cam/dc.c index 99543a6f56..9589bda2f5 100644 --- a/sw/airborne/modules/digital_cam/dc.c +++ b/sw/airborne/modules/digital_cam/dc.c @@ -42,13 +42,14 @@ uint16_t dc_photo_nr = 0; #include "messages.h" #include "downlink.h" #include "estimator.h" +#include "subsystems/gps.h" void dc_send_shot_position(void) { int16_t phi = DegOfRad(estimator_phi*10.0f); int16_t theta = DegOfRad(estimator_theta*10.0f); - float gps_z = ((float)gps_alt) / 100.0f; - DOWNLINK_SEND_DC_SHOT(DefaultChannel, &dc_photo_nr, &gps_utm_east, &gps_utm_north, &gps_z, &gps_utm_zone, &phi, &theta, &gps_course, &gps_gspeed, &gps_itow); + float gps_z = ((float)gps.hmsl) / 1000.0f; + DOWNLINK_SEND_DC_SHOT(DefaultChannel, &dc_photo_nr, &gps.utm_pos.east, &gps.utm_pos.north, &gps_z, &gps.utm_pos.zone, &phi, &theta, &gps.course, &gps.gspeed, &gps.tow); dc_photo_nr++; } diff --git a/sw/airborne/modules/digital_cam/dc.h b/sw/airborne/modules/digital_cam/dc.h index bed149f856..85170fc0a4 100644 --- a/sw/airborne/modules/digital_cam/dc.h +++ b/sw/airborne/modules/digital_cam/dc.h @@ -40,7 +40,7 @@ #include "std.h" #include "led.h" #include "generated/airframe.h" -#include "gps.h" +#include "subsystems/gps.h" /* Generic Set of Digital Camera Commands */ @@ -111,7 +111,7 @@ static inline void dc_init(void) /* shoot on grid */ static inline void dc_shot_on_utm_north_close_to_100m_grid( void ) { - uint32_t dist_to_100m_grid = (gps_utm_north / 100) % 100; + uint32_t dist_to_100m_grid = (gps.utm_pos.north / 100) % 100; if (dist_to_100m_grid < dc_autoshoot_meter_grid || 100 - dist_to_100m_grid < dc_autoshoot_meter_grid) { dc_send_command(DC_SHOOT); diff --git a/sw/airborne/modules/gps/gps_ubx_uart.h b/sw/airborne/modules/gps/gps_ubx_uart.h new file mode 100644 index 0000000000..3349680f44 --- /dev/null +++ b/sw/airborne/modules/gps/gps_ubx_uart.h @@ -0,0 +1,34 @@ +/* + * Copyright (C) 2011 Gautier Hattenberger + * + * 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. + * + */ + +/* + * Wrapper for UBX gps on uart + */ + +#ifndef MODULE_GPS_UBX_UART_H +#define MODULE_GPS_UBX_UART_H + +#include "subsystems/gps.h" +#include "subsystems/gps/gps_ubx.h" + +#endif // MODULE_GPS_UBX_UART_H + diff --git a/sw/airborne/modules/gps_i2c/gps_i2c.c b/sw/airborne/modules/gps_i2c/gps_i2c.c index 591b36e3c2..23a312312e 100644 --- a/sw/airborne/modules/gps_i2c/gps_i2c.c +++ b/sw/airborne/modules/gps_i2c/gps_i2c.c @@ -22,7 +22,7 @@ #include "gps_i2c.h" #include "mcu_periph/i2c.h" -#include "gps.h" +#include "subsystems/gps.h" struct i2c_transaction i2c_gps_trans; @@ -86,7 +86,7 @@ void gps_i2c_event(void) { uint8_t data_size = Min(gps_i2c_tx_insert_idx-gps_i2c_tx_extract_idx, I2C0_BUF_LEN); uint8_t i; for(i = 0; i < data_size; i++, gps_i2c_tx_extract_idx++) - i2c0_buf[i] = gps_i2c_tx_buf[gps_i2c_tx_extract_idx]; + i2c0_buf[i] = gps_i2c_tx_buf[gps_i2c_tx_extract_idx]; // Start i2c transmit i2c0_transmit(GPS_I2C_SLAVE_ADDR, data_size, &gps_i2c_done); @@ -94,8 +94,8 @@ void gps_i2c_event(void) { // Reset flag if finished if (gps_i2c_tx_extract_idx >= gps_i2c_tx_insert_idx) { - gps_i2c_data_ready_to_transmit = FALSE; - gps_i2c_tx_insert_idx = 0; + gps_i2c_data_ready_to_transmit = FALSE; + gps_i2c_tx_insert_idx = 0; } } break; diff --git a/sw/airborne/modules/gsm/gsm.c b/sw/airborne/modules/gsm/gsm.c index 7229bdc344..def37babce 100644 --- a/sw/airborne/modules/gsm/gsm.c +++ b/sw/airborne/modules/gsm/gsm.c @@ -41,7 +41,7 @@ Reporting: In: OK Out: AT+CMGS=\"GCS_NUMBER\" In: > - Out: gps_utm_east, gps_utm_north, gps_course, gps_alt, gps_gspeed, gps_climb, vsupply, estimator_flight_time, rssi CTRLZ + Out: gps.utm_pos.east, gps.utm_pos.north, gps.course, gps.hmsl, gps.gspeed, -gps.ned_vel.z, vsupply, estimator_flight_time, rssi CTRLZ Receiving: In: +CMTI: ..., @@ -69,10 +69,10 @@ Receiving: #include "mcu_periph/uart.h" #include "downlink.h" #include "ap_downlink.h" -#include "gps.h" +#include "subsystems/gps.h" #include "autopilot.h" #include "estimator.h" -#include "subsystems/navigation/common_nav.h" +//#include "subsystems/navigation/common_nav.h" //why is should this be needed? #include "generated/settings.h" #ifndef GSM_LINK @@ -408,10 +408,10 @@ void gsm_send_report_continue(void) // and we expect "OK" on the second line uint8_t rssi = atoi(gsm_buf + strlen("+CSQ: ")); - // Donnee GPS :ne sont pas envoyes gps_mode, gps_itow, gps_utm_zone, gps_nb_ovrn + // Donnee GPS :ne sont pas envoyes gps_mode, gps.tow, gps.utm_pos.zone, gps_nb_ovrn // Donnees batterie (seuls vsupply et estimator_flight_time sont envoyes) // concatenation de toutes les infos en un seul message à transmettre - sprintf(data_to_send, "%ld %ld %d %ld %d %d %d %d %d", gps_utm_east, gps_utm_north, gps_course, gps_alt, gps_gspeed, gps_climb, vsupply, estimator_flight_time, rssi); + sprintf(data_to_send, "%ld %ld %d %ld %d %d %d %d %d", gps.utm_pos.east, gps.utm_pos.north, gps_course, gps.hmsl, gps.gspeed, -gps.ned_vel.z, vsupply, estimator_flight_time, rssi); // send the number and wait for the prompt char buf[32]; diff --git a/sw/airborne/modules/ins/imu_chimu.c b/sw/airborne/modules/ins/imu_chimu.c index f102d183cd..61694472e8 100644 --- a/sw/airborne/modules/ins/imu_chimu.c +++ b/sw/airborne/modules/ins/imu_chimu.c @@ -164,7 +164,6 @@ void CHIMU_Checksum(unsigned char *data, unsigned char buflen) // Communication Definitions #define CHIMU_COM_ID_HIGH 0x1F //Must set this to the max ID expected above - /*--------------------------------------------------------------------------- Name: CHIMU_Init @@ -311,7 +310,6 @@ unsigned char CHIMU_Parse( // appropriate sentence data processor. /////////////////////////////////////////////////////////////////////////////// - static CHIMU_attitude_data GetEulersFromQuat(CHIMU_attitude_data attitude) { CHIMU_attitude_data ps; @@ -361,7 +359,6 @@ static unsigned char BitTest (unsigned char input, unsigned char n) //Test a bit in n and return TRUE or FALSE if ( input & (1 << n)) return TRUE; else return FALSE; } - unsigned char CHIMU_ProcessMessage(unsigned char *pMsgID, unsigned char *pPayloadData, CHIMU_PARSER_DATA *pstData) { //Msgs from CHIMU are off limits (i.e.any CHIMU messages sent up the uplink should go to @@ -440,7 +437,6 @@ unsigned char CHIMU_ProcessMessage(unsigned char *pMsgID, unsigned char *pPayloa pstData->m_attrates.euler.theta = pstData->m_sensor.rate[1]; pstData->m_attrates.euler.psi = pstData->m_sensor.rate[2]; - pstData->gCalStatus = pPayloadData[CHIMU_index]; CHIMU_index ++; pstData->gCHIMU_BIT = pPayloadData[CHIMU_index]; CHIMU_index ++; pstData->gConfigInfo = pPayloadData[CHIMU_index]; CHIMU_index ++; @@ -462,7 +458,6 @@ unsigned char CHIMU_ProcessMessage(unsigned char *pMsgID, unsigned char *pPayloa pstData->m_attitude = GetEulersFromQuat((pstData->m_attitude)); } - //NEW: Checks for bad attitude data (bad SPI maybe?) // Only allow globals to contain updated data if it makes sense sanity_check = (pstData->m_attitude.q.s * pstData->m_attitude.q.s); diff --git a/sw/airborne/modules/ins/imu_chimu.h b/sw/airborne/modules/ins/imu_chimu.h index 0591bc57bc..6fade8a21e 100644 --- a/sw/airborne/modules/ins/imu_chimu.h +++ b/sw/airborne/modules/ins/imu_chimu.h @@ -93,7 +93,7 @@ typedef struct { unsigned char m_MsgLen; unsigned char m_TempDeviceID; unsigned char m_DeviceID; - unsigned char m_Payload[CHIMU_RX_BUFFERSIZE]; // CHIMU data + unsigned char m_Payload[CHIMU_RX_BUFFERSIZE]; // CHIMU data unsigned char m_FullMessage[CHIMU_RX_BUFFERSIZE]; // CHIMU data CHIMU_attitude_data m_attitude; CHIMU_attitude_data m_attrates; diff --git a/sw/airborne/modules/ins/ins_arduimu.c b/sw/airborne/modules/ins/ins_arduimu.c index c4e14dd7cd..dbea8c92f2 100644 --- a/sw/airborne/modules/ins/ins_arduimu.c +++ b/sw/airborne/modules/ins/ins_arduimu.c @@ -14,7 +14,10 @@ Autoren@ZHAW: schmiemi #include "estimator.h" // für das Senden von GPS-Daten an den ArduIMU -#include "gps.h" +#ifndef UBX +#error "currently only compatible with uBlox GPS modules" +#endif +#include "subsystems/gps.h" int32_t GPS_Data[14]; #ifndef ARDUIMU_I2C_DEV @@ -77,23 +80,23 @@ void ArduIMU_periodicGPS( void ) { if ( gps_daten_abgespeichert == FALSE ) { //posllh - GPS_Data [0] = gps_itow; - GPS_Data [1] = gps_lon; - GPS_Data [2] = gps_lat; - GPS_Data [3] = gps_alt; //höhe über elipsoid - GPS_Data [4] = gps_hmsl; //höhe über sea level + GPS_Data [0] = gps.tow; + GPS_Data [1] = gps.lla_pos.lon; + GPS_Data [2] = gps.lla_pos.lat; + GPS_Data [3] = gps.lla_pos.alt; //höhe über elipsoid + GPS_Data [4] = gps.hmsl; //höhe über sea level //velend - GPS_Data [5] = gps_speed_3d; //speed 3D - GPS_Data [6] = gps_gspeed; //ground speed - GPS_Data [7] = gps_course * 100000; //Kurs + GPS_Data [5] = gps.speed_3d; //speed 3D + GPS_Data [6] = gps.gspeed; //ground speed + GPS_Data [7] = DegOfRad(gps.course / 1e6); //Kurs //status - GPS_Data [8] = gps_mode; //fix - GPS_Data [9] = gps_status_flags; //flags + GPS_Data [8] = gps.fix; //fix + GPS_Data [9] = gps_ubx.status_flags; //flags //sol - GPS_Data [10] = gps_mode; //fix - GPS_Data [11] = gps_sol_flags; //flags - GPS_Data [12] = gps_ecefVZ; //ecefVZ - GPS_Data [13] = gps_numSV; + GPS_Data [10] = gps.fix; //fix + //GPS_Data [11] = gps_ubx.sol_flags; //flags + GPS_Data [12] = -gps.ned_vel.z; + GPS_Data [13] = gps.num_sv; gps_daten_abgespeichert = TRUE; } @@ -159,7 +162,7 @@ void ArduIMU_periodicGPS( void ) { messageNr = 0; } - //DOWNLINK_SEND_DEBUG_ZHAW(DefaultChannel, &gps_mode , &gps_numSV ,&gps_alt , &gps_hmsl , &gps_itow, &gps_speed_3d); + //DOWNLINK_SEND_DEBUG_ZHAW(DefaultChannel, &gps_mode , &gps_numSV ,&gps_alt , &gps_hmsl , &gps.tow, &gps_speed_3d); } void ArduIMU_periodic( void ) { diff --git a/sw/airborne/modules/ins/ins_arduimu_basic.c b/sw/airborne/modules/ins/ins_arduimu_basic.c index e3419e1025..bafcc8be3d 100644 --- a/sw/airborne/modules/ins/ins_arduimu_basic.c +++ b/sw/airborne/modules/ins/ins_arduimu_basic.c @@ -14,7 +14,10 @@ Autoren@ZHAW: schmiemi #include "estimator.h" // GPS data for ArduIMU -#include "gps.h" +#ifndef UBX +#error "currently only compatible with uBlox GPS modules" +#endif +#include "subsystems/gps.h" // Command vector for thrust #include "generated/airframe.h" @@ -101,11 +104,11 @@ void ArduIMU_periodicGPS( void ) { } } - 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 + 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)DegOfRad(gps.course / 1e6)); // course + ardu_gps_trans.buf[12] = gps.fix; // status gps fix + ardu_gps_trans.buf[13] = gps_ubx.status_flags; // status flags ardu_gps_trans.buf[14] = (uint8_t)high_accel_flag; // high acceleration flag (disable accelerometers in the arduimu filter) I2CTransmit(ARDUIMU_I2C_DEV, ardu_gps_trans, ArduIMU_SLAVE_ADDR, 15); diff --git a/sw/airborne/modules/ins/ins_chimu_uart.c b/sw/airborne/modules/ins/ins_chimu_uart.c index 3929155ec4..7f6453795b 100644 --- a/sw/airborne/modules/ins/ins_chimu_uart.c +++ b/sw/airborne/modules/ins/ins_chimu_uart.c @@ -34,22 +34,22 @@ INS_FORMAT ins_pitch_neutral; volatile uint8_t new_ins_attitude; -void ins_init( void ) +void ins_init( void ) { uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x05, 0xff, 0x79, 0x00, 0x00, 0x01, 0x76 }; // 50Hz attitude only + SPI // uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x04, 0xff, 0x79, 0x00, 0x00, 0x01, 0xd3 }; // 25Hz attitude only + SPI // uint8_t euler[7] = {0xae, 0xae, 0x01, 0xaa, 0x09, 0x00, 0xaf }; // 25Hz attitude only + SPI uint8_t quaternions[7] = {0xae, 0xae, 0x01, 0xaa, 0x09, 0x01, 0x39 }; // 25Hz attitude only + SPI - + CHIMU_Checksum(rate,12); new_ins_attitude = 0; - + ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT; ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT; - - CHIMU_Init(&CHIMU_DATA); - + + CHIMU_Init(&CHIMU_DATA); + // Quat Filter for (int i=0;i<7;i++) { @@ -68,7 +68,7 @@ void ins_init( void ) { InsUartSend1(rate[i]); } - + } void parse_ins_msg( void ) @@ -76,7 +76,7 @@ void parse_ins_msg( void ) while (InsLink(ChAvailable())) { uint8_t ch = InsLink(Getch()); - + if (CHIMU_Parse(ch, 0, &CHIMU_DATA)) { if(CHIMU_DATA.m_MsgID==0x03) @@ -87,10 +87,10 @@ void parse_ins_msg( void ) { CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI; } - + EstimatorSetAtt(CHIMU_DATA.m_attitude.euler.phi, CHIMU_DATA.m_attitude.euler.psi, CHIMU_DATA.m_attitude.euler.theta); //EstimatorSetRate(ins_p,ins_q); - + DOWNLINK_SEND_AHRS_EULER(DefaultChannel, &CHIMU_DATA.m_attitude.euler.phi, &CHIMU_DATA.m_attitude.euler.theta, &CHIMU_DATA.m_attitude.euler.psi); } @@ -100,7 +100,7 @@ void parse_ins_msg( void ) //Frequency defined in conf *.xml -void ins_periodic_task( void ) +void ins_periodic_task( void ) { // Downlink Send } diff --git a/sw/airborne/modules/ins/ins_xsens.c b/sw/airborne/modules/ins/ins_xsens.c index 49a053b314..c8be89d68c 100644 --- a/sw/airborne/modules/ins/ins_xsens.c +++ b/sw/airborne/modules/ins/ins_xsens.c @@ -27,17 +27,21 @@ */ #include "ins_module.h" +#include "ins_xsens.h" #include #include "generated/airframe.h" +#include "sys_time.h" #include "downlink.h" #include "messages.h" #ifdef USE_GPS_XSENS -#include "gps.h" -#include "latlong.h" +#include "subsystems/gps.h" +#include "math/pprz_geodetic_wgs84.h" +#include "math/pprz_geodetic_float.h" +#include "subsystems/navigation/common_nav.h" /* needed for nav_utm_zone0 */ #endif INS_FORMAT ins_x; @@ -64,6 +68,16 @@ INS_FORMAT ins_mx; INS_FORMAT ins_my; INS_FORMAT ins_mz; +float ins_pitch_neutral; +float ins_roll_neutral; + +volatile uint8_t new_ins_attitude; + +////////////////////////////////////////////////////////////////////////////////////////// +// +// XSens Specific +// + volatile uint8_t ins_msg_received; #define XsensInitCheksum() { send_ck = 0; } @@ -152,11 +166,25 @@ uint8_t xsens_msg_buf[XSENS_MAX_PAYLOAD]; #define GOT_DATA 5 #define GOT_CHECKSUM 6 +// FIXME Debugging Only +#ifndef DOWNLINK_DEVICE +#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE +#endif +#include "mcu_periph/uart.h" +#include "messages.h" +#include "downlink.h" + + uint8_t xsens_errorcode; uint8_t xsens_msg_status; uint16_t xsens_time_stamp; uint16_t xsens_output_mode; uint32_t xsens_output_settings; +float xsens_declination = 0; +float xsens_gps_arm_x = 0; +float xsens_gps_arm_y = 0; +float xsens_gps_arm_z = 0; + int8_t xsens_hour; int8_t xsens_min; @@ -165,9 +193,6 @@ int32_t xsens_nanosec; int16_t xsens_year; int8_t xsens_month; int8_t xsens_day; -float xsens_lat; -float xsens_lon; - static uint8_t xsens_id; static uint8_t xsens_status; @@ -176,43 +201,122 @@ static uint8_t xsens_msg_idx; static uint8_t ck; uint8_t send_ck; +struct LlaCoor_f lla_f; +struct UtmCoor_f utm_f; + +volatile int xsens_configured = 0; + void ins_init( void ) { xsens_status = UNINIT; + xsens_configured = 20; + + ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT; + ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT; xsens_msg_status = 0; xsens_time_stamp = 0; xsens_output_mode = XSENS_OUTPUT_MODE; xsens_output_settings = XSENS_OUTPUT_SETTINGS; - /* send mode and settings to MT */ - XSENS_GoToConfig(); - XSENS_SetOutputMode(xsens_output_mode); - XSENS_SetOutputSettings(xsens_output_settings); - //XSENS_GoToMeasurment(); + + gps.nb_channels = 0; } void ins_periodic_task( void ) { + if (xsens_configured > 0) + { + switch (xsens_configured) + { + case 20: + /* send mode and settings to MT */ + XSENS_GoToConfig(); + XSENS_SetOutputMode(xsens_output_mode); + XSENS_SetOutputSettings(xsens_output_settings); + break; + case 18: + // Give pulses on SyncOut + XSENS_SetSyncOutSettings(0,0x0002); + break; + case 17: + // 1 pulse every 100 samples + XSENS_SetSyncOutSettings(1,100); + break; + case 2: + XSENS_ReqLeverArmGps(); + break; + + case 6: + XSENS_ReqMagneticDeclination(); + break; + + case 13: + #ifdef AHRS_H_X + #warning Sending XSens Magnetic Declination + xsens_declination = atan2(AHRS_H_Y, AHRS_H_X); + XSENS_SetMagneticDeclination(xsens_declination); + #endif + break; + case 12: + #ifdef GPS_IMU_LEVER_ARM_X + #warning Sending XSens GPS Arm + XSENS_SetLeverArmGps(GPS_IMU_LEVER_ARM_X,GPS_IMU_LEVER_ARM_Y,GPS_IMU_LEVER_ARM_Z); + #endif + break; + case 10: + { + uint8_t baud = 1; + XSENS_SetBaudrate(baud); + } + break; + + case 1: + XSENS_GoToMeasurment(); + break; + } + xsens_configured--; + return; + } + RunOnceEvery(100,XSENS_ReqGPSStatus()); } #include "estimator.h" void handle_ins_msg( void) { - EstimatorSetAtt(ins_phi, ins_psi, ins_theta); + + + // Send to Estimator (Control) + EstimatorSetAtt(ins_phi+ins_roll_neutral, ins_psi, ins_theta+ins_pitch_neutral); EstimatorSetRate(ins_p,ins_q); - if (gps_mode != 0x03) - gps_gspeed = 0; - //gps_course = ins_psi * 1800 / M_PI; - gps_course = (DegOfRad(atan2f((float)ins_vx, (float)ins_vy))*10.0f); - gps_climb = (int16_t)(-ins_vz * 100); - gps_gspeed = (uint16_t)(sqrt(ins_vx*ins_vx + ins_vy*ins_vy) * 100); + // Position + float gps_east = gps.utm_pos.east / 100.; + float gps_north = gps.utm_pos.north / 100.; + gps_east -= nav_utm_east0; + gps_north -= nav_utm_north0; + EstimatorSetPosXY(gps_east, gps_north); + + // Altitude and vertical speed + float hmsl = gps.hmsl; + hmsl /= 1000.0f; + EstimatorSetAlt(hmsl); + + // Horizontal speed + float fspeed = sqrt(ins_vx*ins_vx + ins_vy*ins_vy); + if (gps.fix != GPS_FIX_3D) + { + fspeed = 0; + } + float fclimb = -ins_vz; + float fcourse = atan2f((float)ins_vy, (float)ins_vx); + EstimatorSetSpeedPol(fspeed, fcourse, fclimb); + + + // Now also finish filling the gps struct for telemetry purposes + gps.gspeed = fspeed * 100.; + gps.speed_3d = (uint16_t)(sqrt(ins_vx*ins_vx + ins_vy*ins_vy + ins_vz*ins_vz) * 100); + gps.course = fcourse * 1e7; - EstimatorSetAtt(ins_phi, RadOfDeg(((float)gps_course) / 10.0), ins_theta); - // EstimatorSetSpeedPol(gps_gspeed, gps_course, ins_vz); - // EstimatorSetAlt(ins_z); - estimator_update_state_gps(); - reset_gps_watchdog(); } void parse_ins_msg( void ) { @@ -223,21 +327,41 @@ void parse_ins_msg( void ) { else if (xsens_id == XSENS_ReqOutputSettings_ID) { xsens_output_settings = XSENS_ReqOutputSettingsAck_settings(xsens_msg_buf); } + else if (xsens_id == XSENS_ReqMagneticDeclinationAck_ID) { + xsens_declination = DegOfRad (XSENS_ReqMagneticDeclinationAck_declination(xsens_msg_buf) ); + + DOWNLINK_SEND_IMU_MAG_SETTINGS(DefaultChannel,&xsens_declination,&xsens_declination,&xsens_gps_arm_x,&xsens_gps_arm_y,&xsens_gps_arm_z); + } + else if (xsens_id == XSENS_ReqLeverArmGpsAck_ID) { + xsens_gps_arm_x = XSENS_ReqLeverArmGpsAck_x(xsens_msg_buf); + xsens_gps_arm_y = XSENS_ReqLeverArmGpsAck_y(xsens_msg_buf); + xsens_gps_arm_z = XSENS_ReqLeverArmGpsAck_z(xsens_msg_buf); + + DOWNLINK_SEND_IMU_MAG_SETTINGS(DefaultChannel,&xsens_declination,&xsens_declination,&xsens_gps_arm_x,&xsens_gps_arm_y,&xsens_gps_arm_z); + } else if (xsens_id == XSENS_Error_ID) { xsens_errorcode = XSENS_Error_errorcode(xsens_msg_buf); } #ifdef USE_GPS_XSENS else if (xsens_id == XSENS_GPSStatus_ID) { - gps_nb_channels = XSENS_GPSStatus_nch(xsens_msg_buf); - gps_numSV = gps_nb_channels; + gps.nb_channels = XSENS_GPSStatus_nch(xsens_msg_buf); + gps.num_sv = 0; + + gps.last_fix_time = cpu_time_sec; + uint8_t i; - for(i = 0; i < Min(gps_nb_channels, GPS_NB_CHANNELS); i++) { + // Do not write outside buffer + for(i = 0; i < Min(gps.nb_channels, GPS_NB_CHANNELS); i++) { uint8_t ch = XSENS_GPSStatus_chn(xsens_msg_buf,i); - if (ch > GPS_NB_CHANNELS) continue; - gps_svinfos[ch].svid = XSENS_GPSStatus_svid(xsens_msg_buf, i); - gps_svinfos[ch].flags = XSENS_GPSStatus_bitmask(xsens_msg_buf, i); - gps_svinfos[ch].qi = XSENS_GPSStatus_qi(xsens_msg_buf, i); - gps_svinfos[ch].cno = XSENS_GPSStatus_cnr(xsens_msg_buf, i); + if (ch > gps.nb_channels) continue; + gps.svinfos[ch].svid = XSENS_GPSStatus_svid(xsens_msg_buf, i); + gps.svinfos[ch].flags = XSENS_GPSStatus_bitmask(xsens_msg_buf, i); + gps.svinfos[ch].qi = XSENS_GPSStatus_qi(xsens_msg_buf, i); + gps.svinfos[ch].cno = XSENS_GPSStatus_cnr(xsens_msg_buf, i); + if (gps.svinfos[ch].flags > 0) + { + gps.num_sv++; + } } } #endif @@ -252,27 +376,49 @@ void parse_ins_msg( void ) { } if (XSENS_MASK_RAWGPS(xsens_output_mode)) { #if defined(USE_GPS_XSENS_RAW_DATA) && defined(USE_GPS_XSENS) - gps_week = 0; // FIXME - gps_itow = XSENS_DATA_RAWGPS_itow(xsens_msg_buf,offset) * 10; - gps_lat = XSENS_DATA_RAWGPS_lat(xsens_msg_buf,offset); - gps_lon = XSENS_DATA_RAWGPS_lon(xsens_msg_buf,offset); +#ifdef GPS_LED + LED_TOGGLE(GPS_LED); +#endif + gps.last_fix_time = cpu_time_sec; + gps.week = 0; // FIXME + gps.tow = XSENS_DATA_RAWGPS_itow(xsens_msg_buf,offset) * 10; + gps.lla_pos.lat = RadOfDeg(XSENS_DATA_RAWGPS_lat(xsens_msg_buf,offset)); + gps.lla_pos.lon = RadOfDeg(XSENS_DATA_RAWGPS_lon(xsens_msg_buf,offset)); + gps.lla_pos.alt = XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset); + + /* Set the real UTM zone */ - gps_utm_zone = (gps_lon/1e7+180) / 6 + 1; - latlong_utm_of(RadOfDeg(gps_lat/1e7), RadOfDeg(gps_lon/1e7), gps_utm_zone); - /* utm */ - gps_utm_east = latlong_utm_x * 100; - gps_utm_north = latlong_utm_y * 100; - ins_x = latlong_utm_x; - ins_y = latlong_utm_y; - gps_alt = XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset) / 10; - ins_z = -(INS_FORMAT)gps_alt / 100.; - ins_vx = (INS_FORMAT)XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf,offset) / 100.; - ins_vy = (INS_FORMAT)XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf,offset) / 100.; - ins_vz = (INS_FORMAT)XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset) / 100.; - gps_climb = -XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset) / 10; - gps_Pacc = XSENS_DATA_RAWGPS_hacc(xsens_msg_buf,offset) / 100; - gps_Sacc = XSENS_DATA_RAWGPS_sacc(xsens_msg_buf,offset) / 100; - gps_PDOP = 5; + gps.utm_pos.zone = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1; + + lla_f.lat = ((float) gps.lla_pos.lat) / 1e7; + lla_f.lon = ((float) gps.lla_pos.lon) / 1e7; + utm_f.zone = nav_utm_zone0; + /* convert to utm */ + utm_of_lla_f(&utm_f, &lla_f); + /* copy results of utm conversion */ + gps.utm_pos.east = utm_f.east*100; + gps.utm_pos.north = utm_f.north*100; + gps.utm_pos.alt = gps.lla_pos.alt; + + ins_x = utm_f.east; + ins_y = utm_f.north; + // Altitude: Xsens LLH gives ellipsoid height + ins_z = -(INS_FORMAT)XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset) / 1000.; + + // Compute geoid (MSL) height + float hmsl; + WGS84_ELLIPSOID_TO_GEOID(lla_f.lat,lla_f.lon,hmsl); + gps.hmsl = XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset) - (hmsl * 1000.0f); + + ins_vx = ((INS_FORMAT)XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf,offset)) / 100.; + ins_vy = ((INS_FORMAT)XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf,offset)) / 100.; + ins_vz = ((INS_FORMAT)XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset)) / 100.; + gps.ned_vel.x = XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf,offset); + gps.ned_vel.y = XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf,offset); + gps.ned_vel.z = XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset); + gps.pacc = XSENS_DATA_RAWGPS_hacc(xsens_msg_buf,offset) / 100; + gps.sacc = XSENS_DATA_RAWGPS_sacc(xsens_msg_buf,offset) / 100; + gps.pdop = 5; // FIXME Not output by XSens #endif offset += XSENS_DATA_RAWGPS_LENGTH; } @@ -327,6 +473,7 @@ void parse_ins_msg( void ) { if (XSENS_MASK_OrientationMode(xsens_output_settings) == 0x10) { offset += XSENS_DATA_Matrix_LENGTH; } + new_ins_attitude = 1; } if (XSENS_MASK_Auxiliary(xsens_output_mode)) { uint8_t l = 0; @@ -340,18 +487,22 @@ void parse_ins_msg( void ) { } if (XSENS_MASK_Position(xsens_output_mode)) { #if (!defined(USE_GPS_XSENS_RAW_DATA)) && defined(USE_GPS_XSENS) - xsens_lat = XSENS_DATA_Position_lat(xsens_msg_buf,offset); - xsens_lon = XSENS_DATA_Position_lon(xsens_msg_buf,offset); - gps_lat = (int32_t)(xsens_lat * 1e7); - gps_lon = (int32_t)(xsens_lon * 1e7); - gps_utm_zone = (xsens_lon+180) / 6 + 1; - latlong_utm_of(RadOfDeg(xsens_lat), RadOfDeg(xsens_lon), gps_utm_zone); - ins_x = latlong_utm_x; - ins_y = latlong_utm_y; - gps_utm_east = ins_x * 100; - gps_utm_north = ins_y * 100; - ins_z = XSENS_DATA_Position_alt(xsens_msg_buf,offset); - gps_alt = ins_z * 100; + gps.last_fix_time = cpu_time_sec; + + lla_f.lat = RadOfDeg(XSENS_DATA_Position_lat(xsens_msg_buf,offset)); + lla_f.lon = RadOfDeg(XSENS_DATA_Position_lon(xsens_msg_buf,offset)); + gps.lla_pos.lat = (int32_t)(lla_f.lat * 1e7); + gps.lla_pos.lon = (int32_t)(lla_f.lon * 1e7); + gps.utm_pos.zone = (DegOfRad(lla_f.lon)+180) / 6 + 1; + /* convert to utm */ + utm_of_lla_f(&utm_f, &lla_f); + /* copy results of utm conversion */ + gps.utm_pos.east = utm_f.east*100; + gps.utm_pos.north = utm_f.north*100; + ins_x = utm_f.east; + ins_y = utm_f.north; + ins_z = XSENS_DATA_Position_alt(xsens_msg_buf,offset);//TODO is this hms or above ellipsoid? + gps.hmsl = ins_z * 1000; #endif offset += XSENS_DATA_Position_LENGTH; } @@ -366,16 +517,16 @@ void parse_ins_msg( void ) { if (XSENS_MASK_Status(xsens_output_mode)) { xsens_msg_status = XSENS_DATA_Status_status(xsens_msg_buf,offset); #ifdef USE_GPS_XSENS - if (bit_is_set(xsens_msg_status,2)) gps_mode = 0x03; // gps fix - else if (bit_is_set(xsens_msg_status,1)) gps_mode = 0x01; // efk valid - else gps_mode = 0; + if (bit_is_set(xsens_msg_status,2)) gps.fix = GPS_FIX_3D; // gps fix + else if (bit_is_set(xsens_msg_status,1)) gps.fix = 0x01; // efk valid + else gps.fix = GPS_FIX_NONE; #endif offset += XSENS_DATA_Status_LENGTH; } if (XSENS_MASK_TimeStamp(xsens_output_settings)) { xsens_time_stamp = XSENS_DATA_TimeStamp_ts(xsens_msg_buf,offset); #ifdef USE_GPS_XSENS - gps_itow = xsens_time_stamp; + gps.tow = xsens_time_stamp; #endif offset += XSENS_DATA_TimeStamp_LENGTH; } diff --git a/sw/airborne/modules/ins/ins_xsens.h b/sw/airborne/modules/ins/ins_xsens.h index 446faabad8..790e65bf5a 100644 --- a/sw/airborne/modules/ins/ins_xsens.h +++ b/sw/airborne/modules/ins/ins_xsens.h @@ -38,11 +38,8 @@ extern int32_t xsens_nanosec; extern int16_t xsens_year; extern int8_t xsens_month; extern int8_t xsens_day; -extern float xsens_lat; -extern float xsens_lon; extern uint8_t xsens_msg_status; extern uint16_t xsens_time_stamp; - #endif diff --git a/sw/airborne/modules/meteo/charge_sens.c b/sw/airborne/modules/meteo/charge_sens.c new file mode 100644 index 0000000000..91a1d2e720 --- /dev/null +++ b/sw/airborne/modules/meteo/charge_sens.c @@ -0,0 +1,73 @@ +/* + * $Id$ + * + * Copyright (C) 2011 Martin Mueller + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + * + */ + +/** \file charge_sens.c + * \brief I2C interface for University of Reading charge sensor + * + */ + +#include "modules/meteo/charge_sens.h" +#include "mcu_periph/i2c.h" +#include "mcu_periph/uart.h" +#include "messages.h" +#include "downlink.h" + +#ifndef DOWNLINK_DEVICE +#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE +#endif + +#ifndef CHARGE_SENS_DEV +#define CHARGE_SENS_DEV i2c0 +#endif + +#define CHARGE_SENS_I2C_ADDR 0x78 +#define CHARGE_NB 10 + +struct i2c_transaction charge_trans; +uint16_t charge[CHARGE_NB]; +int32_t charge_cnt; + +void charge_sens_init( void ) { + charge_cnt = 0; +} + +void charge_sens_periodic( void ) { + I2CReceive(CHARGE_SENS_DEV, charge_trans, CHARGE_SENS_I2C_ADDR, 2); +} + +void charge_sens_event( void ) { + if (charge_trans.status == I2CTransSuccess) { + /* read two byte atmosphere charge */ + charge[charge_cnt] = charge_trans.buf[1] << 8; + charge[charge_cnt] |= charge_trans.buf[0]; + charge_trans.status = I2CTransDone; + + if (++charge_cnt >= CHARGE_NB) { + DOWNLINK_SEND_ATMOSPHERE_CHARGE(DefaultChannel, + &charge[0], &charge[1], &charge[2], &charge[3], &charge[4], + &charge[5], &charge[6], &charge[7], &charge[8], &charge[9]); + charge_cnt = 0; + } + } +} diff --git a/sw/airborne/modules/meteo/charge_sens.h b/sw/airborne/modules/meteo/charge_sens.h new file mode 100644 index 0000000000..0e6ad15e74 --- /dev/null +++ b/sw/airborne/modules/meteo/charge_sens.h @@ -0,0 +1,11 @@ +#ifndef CHARGE_SENS_H +#define CHARGE_SENS_H + +#include "std.h" + +void charge_sens_init(void); +void charge_sens_periodic(void); +void charge_sens_event(void); + +#endif + diff --git a/sw/airborne/modules/meteo/humid_htm_b71.c b/sw/airborne/modules/meteo/humid_htm_b71.c new file mode 100644 index 0000000000..30af8b3dc4 --- /dev/null +++ b/sw/airborne/modules/meteo/humid_htm_b71.c @@ -0,0 +1,107 @@ +/* + * $Id$ + * + * Copyright (C) 2011 Martin Mueller + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + * + */ + +/** \file humid_htm_b71.c + * \brief TronSens HTM-B71 humidity/temperature sensor i2c interface + * + */ + +/* nice sensor but not very collaborative with others on the i2c bus */ + + +#include "modules/meteo/humid_htm_b71.h" + +#include "sys_time.h" +#include "mcu_periph/i2c.h" +#include "mcu_periph/uart.h" +#include "messages.h" +#include "downlink.h" + +#ifndef DOWNLINK_DEVICE +#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE +#endif + +#ifndef HTM_I2C_DEV +#define HTM_I2C_DEV i2c0 +#endif + +#define HTM_SLAVE_ADDR 0x28 + +struct i2c_transaction htm_trans; +uint8_t htm_status; +uint16_t humidhtm, temphtm; +float fhumidhtm, ftemphtm; + + +void humid_htm_init(void) { + htm_status = HTM_IDLE; +} + +void humid_htm_start( void ) { + if (cpu_time_sec > 1) { + /* measurement request: wake up sensor, sample temperature/humidity */ + I2CTransmit(HTM_I2C_DEV, htm_trans, HTM_SLAVE_ADDR, 0); + htm_status = HTM_MR; + } +} + +/* needs 18.5ms delay from measurement request */ +void humid_htm_read( void ) { + if (htm_status == HTM_MR_OK) { + /* read humid and temp*/ + htm_status = HTM_READ_DATA; + I2CReceive(HTM_I2C_DEV, htm_trans, HTM_SLAVE_ADDR, 4); + } +} + +void humid_htm_event( void ) { + if (htm_trans.status == I2CTransSuccess) { + switch (htm_status) { + + case HTM_MR: + htm_status = HTM_MR_OK; + htm_trans.status = I2CTransDone; + break; + + case HTM_READ_DATA: + /* check stale status */ + if (((htm_trans.buf[0] >> 6) & 0x3) == 0) { + /* humidity */ + humidhtm = ((htm_trans.buf[0] & 0x3F) << 8) | htm_trans.buf[1]; + fhumidhtm = humidhtm / 163.83; + /* temperature */ + temphtm = (htm_trans.buf[2] << 6) | (htm_trans.buf[3] >> 2); + ftemphtm = -40.00 + 0.01 * temphtm; + DOWNLINK_SEND_HTM_STATUS(DefaultChannel, &humidhtm, &temphtm, &fhumidhtm, &ftemphtm); + } + htm_trans.status = I2CTransDone; + break; + + default: + htm_trans.status = I2CTransDone; + break; + } + } +} + diff --git a/sw/airborne/modules/meteo/humid_htm_b71.h b/sw/airborne/modules/meteo/humid_htm_b71.h new file mode 100644 index 0000000000..84a03ecbbc --- /dev/null +++ b/sw/airborne/modules/meteo/humid_htm_b71.h @@ -0,0 +1,19 @@ +#ifndef HUMID_HTM_H +#define HUMID_HTM_H + +#include "std.h" + +enum htm_type { + HTM_IDLE, + HTM_MR, + HTM_MR_OK, + HTM_READ_DATA +}; + +void humid_htm_init(void); +void humid_htm_start(void); +void humid_htm_read(void); +void humid_htm_event(void); + +#endif // HUMID_HTM_H + diff --git a/sw/airborne/modules/meteo/ir_mlx.c b/sw/airborne/modules/meteo/ir_mlx.c index 8166f16192..3515b49673 100644 --- a/sw/airborne/modules/meteo/ir_mlx.c +++ b/sw/airborne/modules/meteo/ir_mlx.c @@ -23,9 +23,9 @@ */ /** \file ir_mlx.c - * \brief Melexis 90614 I2C + * \brief Melexis MLX90614 I2C * - * This reads the values for temperatures from the Melexis 90614 IR sensor through I2C. + * This reads the values for temperatures from the Melexis MLX90614 IR sensor through I2C. */ @@ -53,6 +53,8 @@ uint16_t ir_mlx_itemp_case; float ir_mlx_temp_case; uint16_t ir_mlx_itemp_obj; float ir_mlx_temp_obj; +uint32_t ir_mlx_id_01; +uint32_t ir_mlx_id_23; /* I2C address is set to 3 */ #ifndef MLX90614_ADDR @@ -68,16 +70,66 @@ void ir_mlx_init( void ) { void ir_mlx_periodic( void ) { if (cpu_time_sec > 1) { - /* start two byte case temperature */ - mlx_trans.buf[0] = MLX90614_TA; - I2CTransceive(MLX_I2C_DEV, mlx_trans, MLX90614_ADDR, 1, 2); - ir_mlx_status = IR_MLX_RD_CASE_TEMP; + if (ir_mlx_status >= IR_MLX_IDLE) { + /* start two byte case temperature */ + mlx_trans.buf[0] = MLX90614_TA; + I2CTransceive(MLX_I2C_DEV, mlx_trans, MLX90614_ADDR, 1, 2); + ir_mlx_status = IR_MLX_RD_CASE_TEMP; + /* send serial number every 30 seconds */ + RunOnceEvery((8*30), DOWNLINK_SEND_MLX_SERIAL(DefaultChannel, &ir_mlx_id_01, &ir_mlx_id_23)); + } else if (ir_mlx_status == IR_MLX_UNINIT) { + /* start two byte ID 0 */ + mlx_trans.buf[0] = MLX90614_ID_0; + I2CTransceive(MLX_I2C_DEV, mlx_trans, MLX90614_ADDR, 1, 2); + ir_mlx_status = IR_MLX_RD_ID_0; + } } } void ir_mlx_event( void ) { if ((mlx_trans.status == I2CTransSuccess)) { - if (ir_mlx_status == IR_MLX_RD_CASE_TEMP) { + switch (ir_mlx_status) { + + case IR_MLX_RD_ID_0: + /* read two byte ID 0 */ + ir_mlx_id_01 = mlx_trans.buf[0]; + ir_mlx_id_01 |= mlx_trans.buf[1] << 8; + /* start two byte ID 1 */ + mlx_trans.buf[0] = MLX90614_ID_1; + I2CTransceive(MLX_I2C_DEV, mlx_trans, MLX90614_ADDR, 1, 2); + ir_mlx_status = IR_MLX_RD_ID_1; + break; + + case IR_MLX_RD_ID_1: + /* read two byte ID 1 */ + ir_mlx_id_01 |= mlx_trans.buf[0] << 16; + ir_mlx_id_01 |= mlx_trans.buf[1] << 24; + /* start two byte ID 2 */ + mlx_trans.buf[0] = MLX90614_ID_2; + I2CTransceive(MLX_I2C_DEV, mlx_trans, MLX90614_ADDR, 1, 2); + ir_mlx_status = IR_MLX_RD_ID_2; + break; + + case IR_MLX_RD_ID_2: + /* read two byte ID 2 */ + ir_mlx_id_23 = mlx_trans.buf[0]; + ir_mlx_id_23 |= mlx_trans.buf[1] << 8; + /* start two byte ID 3 */ + mlx_trans.buf[0] = MLX90614_ID_3; + I2CTransceive(MLX_I2C_DEV, mlx_trans, MLX90614_ADDR, 1, 2); + ir_mlx_status = IR_MLX_RD_ID_3; + break; + + case IR_MLX_RD_ID_3: + /* read two byte ID 3 */ + ir_mlx_id_23 |= mlx_trans.buf[0] << 16; + ir_mlx_id_23 |= mlx_trans.buf[1] << 24; + ir_mlx_status = IR_MLX_IDLE; + mlx_trans.status = I2CTransDone; + DOWNLINK_SEND_MLX_SERIAL(DefaultChannel, &ir_mlx_id_01, &ir_mlx_id_23); + break; + + case IR_MLX_RD_CASE_TEMP: /* read two byte case temperature */ ir_mlx_itemp_case = mlx_trans.buf[1] << 8; ir_mlx_itemp_case |= mlx_trans.buf[0]; @@ -85,11 +137,11 @@ void ir_mlx_event( void ) { /* start two byte obj temperature */ mlx_trans.buf[0] = MLX90614_TOBJ; - ir_mlx_status = IR_MLX_RD_CASE_TEMP; I2CTransceive(MLX_I2C_DEV, mlx_trans, MLX90614_ADDR, 1, 2); ir_mlx_status = IR_MLX_RD_OBJ_TEMP; - } - else if (ir_mlx_status == IR_MLX_RD_OBJ_TEMP) { + break; + + case IR_MLX_RD_OBJ_TEMP: /* read two byte obj temperature */ ir_mlx_itemp_obj = mlx_trans.buf[1] << 8; ir_mlx_itemp_obj |= mlx_trans.buf[0]; @@ -101,6 +153,10 @@ void ir_mlx_event( void ) { &ir_mlx_temp_case, &ir_mlx_itemp_obj, &ir_mlx_temp_obj); + break; + default: + mlx_trans.status = I2CTransDone; + break; } } } diff --git a/sw/airborne/modules/meteo/ir_mlx.h b/sw/airborne/modules/meteo/ir_mlx.h index f9ecd9aced..cf3508ea3b 100644 --- a/sw/airborne/modules/meteo/ir_mlx.h +++ b/sw/airborne/modules/meteo/ir_mlx.h @@ -5,11 +5,21 @@ #define MLX90614_TA 0x06 #define MLX90614_TOBJ 0x07 +#define MLX90614_ID_0 0x3C +#define MLX90614_ID_1 0x3D +#define MLX90614_ID_2 0x3E +#define MLX90614_ID_3 0x3F -#define IR_MLX_UNINIT 0 -#define IR_MLX_IDLE 1 -#define IR_MLX_RD_CASE_TEMP 2 -#define IR_MLX_RD_OBJ_TEMP 3 +enum mlx_type { + IR_MLX_UNINIT, + IR_MLX_RD_ID_0, + IR_MLX_RD_ID_1, + IR_MLX_RD_ID_2, + IR_MLX_RD_ID_3, + IR_MLX_IDLE, + IR_MLX_RD_CASE_TEMP, + IR_MLX_RD_OBJ_TEMP +}; void ir_mlx_init(void); void ir_mlx_periodic(void); diff --git a/sw/airborne/modules/meteo/windturbine.c b/sw/airborne/modules/meteo/windturbine.c index 5e356d5536..24e3a2538c 100644 --- a/sw/airborne/modules/meteo/windturbine.c +++ b/sw/airborne/modules/meteo/windturbine.c @@ -31,7 +31,7 @@ #include "meteo/windturbine.h" #include "core/trigger_ext.h" -#include "gps.h" +#include "subsystems/gps.h" #include "sys_time.h" #ifndef DOWNLINK_DEVICE @@ -49,7 +49,7 @@ void windturbine_periodic( void ) { uint8_t turb_id = TURBINE_ID; uint32_t sync_itow, cycle_time; - sync_itow = itow_from_ticks(trigger_t0); + sync_itow = gps_tow_from_ticks(trigger_t0); cycle_time = MSEC_OF_SYS_TICS(trigger_delta_t0); DOWNLINK_SEND_WINDTURBINE_STATUS_(DefaultChannel, diff --git a/sw/airborne/modules/multi/formation.c b/sw/airborne/modules/multi/formation.c index 1bce893c59..48c2f53d11 100644 --- a/sw/airborne/modules/multi/formation.c +++ b/sw/airborne/modules/multi/formation.c @@ -16,7 +16,7 @@ #include "firmwares/fixedwing/stabilization/stabilization_attitude.h" #include "firmwares/fixedwing/guidance/guidance_v.h" #include "autopilot.h" -#include "gps.h" +#include "subsystems/gps.h" #include "generated/flight_plan.h" #include "generated/airframe.h" #include "dl_protocol.h" @@ -142,7 +142,7 @@ int formation_flight(void) { estimator_y += formation[the_acs_id[AC_ID]].north; } // set info for this AC - SetAcInfo(AC_ID, estimator_x, estimator_y, estimator_hspeed_dir, estimator_z, estimator_hspeed_mod, estimator_z_dot, gps_itow); + SetAcInfo(AC_ID, estimator_x, estimator_y, estimator_hspeed_dir, estimator_z, estimator_hspeed_mod, estimator_z_dot, gps.tow); // broadcast info uint8_t ac_id = AC_ID; @@ -180,7 +180,7 @@ int formation_flight(void) { for (i = 0; i < NB_ACS; ++i) { if (the_acs[i].ac_id == AC_ID) continue; struct ac_info_ * ac = get_ac_info(the_acs[i].ac_id); - float delta_t = Max((int)(gps_itow - ac->itow) / 1000., 0.); + float delta_t = Max((int)(gps.tow - ac->itow) / 1000., 0.); if (delta_t > FORM_CARROT) { // if AC not responding for too long formation[i].status = LOST; diff --git a/sw/airborne/modules/multi/potential.c b/sw/airborne/modules/multi/potential.c index cd6ae6a8b6..8572bb909e 100644 --- a/sw/airborne/modules/multi/potential.c +++ b/sw/airborne/modules/multi/potential.c @@ -16,7 +16,7 @@ #include "firmwares/fixedwing/stabilization/stabilization_attitude.h" #include "firmwares/fixedwing/guidance/guidance_v.h" #include "autopilot.h" -#include "gps.h" +#include "subsystems/gps.h" #include "generated/airframe.h" //#include @@ -70,7 +70,7 @@ int potential_task(void) { for (i = 0; i < NB_ACS; ++i) { if (the_acs[i].ac_id == AC_ID) continue; struct ac_info_ * ac = get_ac_info(the_acs[i].ac_id); - float delta_t = Max((int)(gps_itow - ac->itow) / 1000., 0.); + float delta_t = Max((int)(gps.tow - ac->itow) / 1000., 0.); // if AC not responding for too long, continue, else compute force if (delta_t > CARROT) continue; else { diff --git a/sw/airborne/modules/multi/tcas.c b/sw/airborne/modules/multi/tcas.c index 6030bb3f7d..a86db7aae7 100644 --- a/sw/airborne/modules/multi/tcas.c +++ b/sw/airborne/modules/multi/tcas.c @@ -31,7 +31,7 @@ #include "generated/airframe.h" #include "estimator.h" #include "subsystems/nav.h" -#include "gps.h" +#include "subsystems/gps.h" #include "generated/flight_plan.h" #include "messages.h" @@ -115,7 +115,7 @@ void tcas_periodic_task_1Hz( void ) { float vy = estimator_hspeed_mod * cosf(estimator_hspeed_dir); for (i = 2; i < NB_ACS; i++) { if (the_acs[i].ac_id == 0) continue; // no AC data - uint32_t dt = gps_itow - the_acs[i].itow; + uint32_t dt = gps.tow - the_acs[i].itow; if (dt > 3*TCAS_DT_MAX) { tcas_acs_status[i].status = TCAS_NO_ALARM; // timeout, reset status continue; diff --git a/sw/airborne/modules/sensors/baro_ets.c b/sw/airborne/modules/sensors/baro_ets.c index 3eaf16fd90..9ad14245ef 100644 --- a/sw/airborne/modules/sensors/baro_ets.c +++ b/sw/airborne/modules/sensors/baro_ets.c @@ -43,7 +43,7 @@ #include "subsystems/nav.h" #ifdef SITL -#include "gps.h" +#include "subsystems/gps.h" #endif #define BARO_ETS_ADDR 0xE8 @@ -98,7 +98,7 @@ void baro_ets_read_periodic( void ) { I2CReceive(BARO_ETS_I2C_DEV, baro_ets_i2c_trans, BARO_ETS_ADDR, 2); #else // SITL baro_ets_adc = 0; - baro_ets_altitude = gps_alt / 100.0; + baro_ets_altitude = gps.hmsl / 1000.0; baro_ets_valid = TRUE; EstimatorSetAlt(baro_ets_altitude); #endif diff --git a/sw/airborne/modules/sensors/baro_ms5611_i2c.c b/sw/airborne/modules/sensors/baro_ms5611_i2c.c new file mode 100644 index 0000000000..a77eeba8b2 --- /dev/null +++ b/sw/airborne/modules/sensors/baro_ms5611_i2c.c @@ -0,0 +1,235 @@ +/* + * $Id$ + * + * Copyright (C) 2011 Martin Mueller + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + * + */ + +/** \file baro_ms5611_i2c.c + * \brief Measurement Specialties (Intersema) MS5611-01BA pressure/temperature sensor interface for I2C + * + */ + + +#include "modules/sensors/baro_ms5611_i2c.h" + +#include "sys_time.h" +#include "mcu_periph/i2c.h" +#include "mcu_periph/uart.h" +#include "messages.h" +#include "downlink.h" + +#ifndef DOWNLINK_DEVICE +#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE +#endif + +#ifndef MS5611_I2C_DEV +#define MS5611_I2C_DEV i2c0 +#endif + +/* address can be 0xEC or 0xEE (CSB\ high = 0xEC) */ +#define MS5611_SLAVE_ADDR 0xEC + +#if PERIODIC_FREQUENCY > 60 +#error baro_ms5611_i2c assumes a PERIODIC_FREQUENCY of 60Hz +#endif + +struct i2c_transaction ms5611_trans; +uint8_t ms5611_status; +uint16_t ms5611_c[PROM_NB]; +uint32_t ms5611_d1, ms5611_d2; +int32_t prom_cnt; +float fbaroms, ftempms; + + +static int8_t baro_ms5611_crc(uint16_t* prom) { + int32_t i, j; + uint32_t res = 0; + uint8_t crc = prom[7] & 0xF; + prom[7] &= 0xFF00; + for (i = 0; i < 16; i++) { + if (i & 1) res ^= ((prom[i>>1]) & 0x00FF); + else res ^= (prom[i>>1]>>8); + for (j = 8; j > 0; j--) { + if (res & 0x8000) res ^= 0x1800; + res <<= 1; + } + } + prom[7] |= crc; + if (crc == ((res >> 12) & 0xF)) return 0; + else return -1; +} + +void baro_ms5611_init(void) { + ms5611_status = MS5611_UNINIT; + prom_cnt = 0; +} + +void baro_ms5611_periodic( void ) { + if (cpu_time_sec > 1) { + if (ms5611_status >= MS5611_IDLE) { + /* start D1 conversion */ + ms5611_status = MS5611_CONV_D1; + ms5611_trans.buf[0] = MS5611_START_CONV_D1; + I2CTransmit(MS5611_I2C_DEV, ms5611_trans, MS5611_SLAVE_ADDR, 1); + RunOnceEvery((4*30), DOWNLINK_SEND_MS5611_COEFF(DefaultChannel, + &ms5611_c[0], &ms5611_c[1], &ms5611_c[2], &ms5611_c[3], + &ms5611_c[4], &ms5611_c[5], &ms5611_c[6], &ms5611_c[7])); + } + else if (ms5611_status == MS5611_UNINIT) { + /* reset sensor */ + ms5611_status = MS5611_RESET; + ms5611_trans.buf[0] = MS5611_SOFT_RESET; + I2CTransmit(MS5611_I2C_DEV, ms5611_trans, MS5611_SLAVE_ADDR, 1); + } + else if (ms5611_status == MS5611_RESET_OK) { + /* start getting prom data */ + ms5611_status = MS5611_PROM; + ms5611_trans.buf[0] = MS5611_PROM_READ | (prom_cnt << 1); + I2CTransceive(MS5611_I2C_DEV, ms5611_trans, MS5611_SLAVE_ADDR, 1, 2); + } + } +} + +void baro_ms5611_d1( void ) { + if (cpu_time_sec > 1) { + if (ms5611_status == MS5611_CONV_D1_OK) { + /* read D1 adc */ + ms5611_status = MS5611_ADC_D1; + ms5611_trans.buf[0] = MS5611_ADC_READ; + I2CTransceive(MS5611_I2C_DEV, ms5611_trans, MS5611_SLAVE_ADDR, 1, 3); + } + } +} + +void baro_ms5611_d2( void ) { + if (cpu_time_sec > 1) { + if (ms5611_status == MS5611_CONV_D2_OK) { + /* read D2 adc */ + ms5611_status = MS5611_ADC_D2; + ms5611_trans.buf[0] = MS5611_ADC_READ; + I2CTransceive(MS5611_I2C_DEV, ms5611_trans, MS5611_SLAVE_ADDR, 1, 3); + } + } +} + +void baro_ms5611_event( void ) { + if (ms5611_trans.status == I2CTransSuccess) { + switch (ms5611_status) { + + case MS5611_RESET: + ms5611_status = MS5611_RESET_OK; + ms5611_trans.status = I2CTransDone; + break; + + case MS5611_PROM: + /* read prom data */ + ms5611_c[prom_cnt++] = (ms5611_trans.buf[0] << 8) | ms5611_trans.buf[1]; + if (prom_cnt < PROM_NB) { + /* get next prom data */ + ms5611_trans.buf[0] = MS5611_PROM_READ | (prom_cnt << 1); + I2CTransceive(MS5611_I2C_DEV, ms5611_trans, MS5611_SLAVE_ADDR, 1, 2); + } + else { + /* done reading prom */ + ms5611_trans.status = I2CTransDone; + /* check prom crc */ + if (baro_ms5611_crc(ms5611_c) == 0) { + DOWNLINK_SEND_MS5611_COEFF(DefaultChannel, + &ms5611_c[0], &ms5611_c[1], &ms5611_c[2], &ms5611_c[3], + &ms5611_c[4], &ms5611_c[5], &ms5611_c[6], &ms5611_c[7]); + ms5611_status = MS5611_IDLE; + } + else { + /* checksum error, try again */ + baro_ms5611_init(); + } + } + break; + + case MS5611_CONV_D1: + ms5611_status = MS5611_CONV_D1_OK; + ms5611_trans.status = I2CTransDone; + break; + + case MS5611_ADC_D1: + /* read D1 (pressure) */ + ms5611_d1 = (ms5611_trans.buf[0] << 16) | + (ms5611_trans.buf[1] << 8) | + ms5611_trans.buf[2]; + /* start D2 conversion */ + ms5611_status = MS5611_CONV_D2; + ms5611_trans.buf[0] = MS5611_START_CONV_D2; + I2CTransmit(MS5611_I2C_DEV, ms5611_trans, MS5611_SLAVE_ADDR, 1); + break; + + case MS5611_CONV_D2: + ms5611_status = MS5611_CONV_D2_OK; + ms5611_trans.status = I2CTransDone; + break; + + case MS5611_ADC_D2: { + int64_t dt, baroms, tempms, off, sens, t2, off2, sens2; + /* read D2 (temperature) */ + ms5611_d2 = (ms5611_trans.buf[0] << 16) | + (ms5611_trans.buf[1] << 8) | + ms5611_trans.buf[2]; + ms5611_status = MS5611_IDLE; + ms5611_trans.status = I2CTransDone; + + /* difference between actual and ref temperature */ + dt = ms5611_d2 - (int64_t)ms5611_c[5] * (1<<8); + /* actual temperature */ + tempms = 2000 + ((int64_t)dt * ms5611_c[6]) / (1<<23); + /* offset at actual temperature */ + off = ((int64_t)ms5611_c[2] * (1<<16)) + ((int64_t)ms5611_c[4] * dt) / (1<<7); + /* sensitivity at actual temperature */ + sens = ((int64_t)ms5611_c[1] * (1<<15)) + ((int64_t)ms5611_c[3] * dt) / (1<<8); + /* second order temperature compensation */ + if (tempms < 2000) { + t2 = (dt*dt) / (1<<31); + off2 = 5 * ((int64_t)(tempms-2000)*(tempms-2000)) / (1<<1); + sens2 = 5 * ((int64_t)(tempms-2000)*(tempms-2000)) / (1<<2); + if (tempms < -1500) { + off2 = off2 + 7 * (int64_t)(tempms+1500)*(tempms+1500); + sens2 = sens2 + 11 * ((int64_t)(tempms+1500)*(tempms+1500)) / (1<<1); + } + tempms = tempms - t2; + off = off - off2; + sens = sens - sens2; + } + /* temperature compensated pressure */ + baroms = (((int64_t)ms5611_d1 * sens) / (1<<21) - off) / (1<<15); +#ifdef SENSOR_SYNC_SEND + ftempms = tempms / 100.; + fbaroms = baroms / 100.; + DOWNLINK_SEND_BARO_MS5611(DefaultChannel, + &ms5611_d1, &ms5611_d2, &fbaroms, &ftempms); +#endif + break; + } + + default: + ms5611_trans.status = I2CTransDone; + break; + } + } +} + diff --git a/sw/airborne/modules/sensors/baro_ms5611_i2c.h b/sw/airborne/modules/sensors/baro_ms5611_i2c.h new file mode 100644 index 0000000000..6acef006a7 --- /dev/null +++ b/sw/airborne/modules/sensors/baro_ms5611_i2c.h @@ -0,0 +1,35 @@ +#ifndef BARO_MS56111_I2C_H +#define BARO_MS56111_I2C_H + +#include "std.h" + +/* we use OSR=4096 for maximum resolution */ +#define MS5611_SOFT_RESET 0x1E +#define MS5611_PROM_READ 0xA0 +#define MS5611_START_CONV_D1 0x48 +#define MS5611_START_CONV_D2 0x58 +#define MS5611_ADC_READ 0x00 + +#define PROM_NB 8 + +enum ms5611_stat{ + MS5611_UNINIT, + MS5611_RESET, + MS5611_RESET_OK, + MS5611_PROM, + MS5611_IDLE, + MS5611_CONV_D1, + MS5611_CONV_D1_OK, + MS5611_ADC_D1, + MS5611_CONV_D2, + MS5611_CONV_D2_OK, + MS5611_ADC_D2 +}; + +void baro_ms5611_init(void); +void baro_ms5611_periodic(void); +void baro_ms5611_d1(void); +void baro_ms5611_d2(void); +void baro_ms5611_event(void); + +#endif diff --git a/sw/airborne/modules/sensors/mag_hmc5843.c b/sw/airborne/modules/sensors/mag_hmc5843.c new file mode 100644 index 0000000000..cc3965c14d --- /dev/null +++ b/sw/airborne/modules/sensors/mag_hmc5843.c @@ -0,0 +1,55 @@ +/* + * 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 "estimator.h" +#include "mcu_periph/i2c.h" +#include "mcu_periph/uart.h" +#include "messages.h" +#include "downlink.h" +#include + +#include "../../peripherals/hmc5843.h" + + +int32_t mag_x, mag_y, mag_z; +bool_t mag_valid; + + +#ifndef DOWNLINK_DEVICE +#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE +#endif + + +void hmc5843_module_init( void ) { + hmc5843_init(); +} + +void hmc5843_module_periodic ( void ) +{ + hmc5843_periodic(); + mag_x = hmc5843.data.value[0]; + mag_y = hmc5843.data.value[1]; + mag_z = hmc5843.data.value[2]; + RunOnceEvery(30,DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,&mag_x,&mag_y,&mag_z)); +} + +void hmc5843_module_event( void ) +{ + hmc5843_idle_task(); +} diff --git a/sw/airborne/modules/sensors/mag_hmc5843.h b/sw/airborne/modules/sensors/mag_hmc5843.h new file mode 100644 index 0000000000..d21dbb5037 --- /dev/null +++ b/sw/airborne/modules/sensors/mag_hmc5843.h @@ -0,0 +1,33 @@ +/* + * 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 HMC5843__H +#define HMC5843__H + +#include "std.h" +#include "mcu_periph/i2c.h" + +extern int32_t mag_x, mag_y, mag_z; + +extern void hmc5843_module_init( void ); +extern void hmc5843_module_periodic( void ); +extern void hmc5843_module_event( void ); + +#endif // HMC5843__H diff --git a/sw/airborne/modules/sensors/trigger_ext.c b/sw/airborne/modules/sensors/trigger_ext.c index 635ff50df6..3f0b0643be 100644 --- a/sw/airborne/modules/sensors/trigger_ext.c +++ b/sw/airborne/modules/sensors/trigger_ext.c @@ -32,7 +32,7 @@ #include "trigger_ext.h" #include "modules/sensors/trig_ext_hw.h" -#include "gps.h" +#include "subsystems/gps.h" #include "sys_time.h" #include "mcu_periph/uart.h" #include "messages.h" @@ -49,7 +49,7 @@ void trigger_ext_periodic( void ) { uint8_t turb_id = TURBINE_ID; uint32_t sync_itow, cycle_time; - sync_itow = itow_from_ticks(trigger_t0); + sync_itow = gps_tow_from_ticks(trigger_t0); cycle_time = MSEC_OF_SYS_TICS(delta_t0); DOWNLINK_SEND_WINDTURBINE_STATUS_(DefaultChannel, diff --git a/sw/airborne/modules/vehicle_interface/vi.c b/sw/airborne/modules/vehicle_interface/vi.c index 94c53cf09a..32dae8fb97 100644 --- a/sw/airborne/modules/vehicle_interface/vi.c +++ b/sw/airborne/modules/vehicle_interface/vi.c @@ -25,7 +25,7 @@ #include "subsystems/imu.h" #include "subsystems/ahrs.h" -#include "booz/booz_gps.h" +#include "subsystems/gps.h" #include "generated/airframe.h" diff --git a/sw/airborne/modules/vehicle_interface/vi_overo_link.c b/sw/airborne/modules/vehicle_interface/vi_overo_link.c index da14c6bcdb..c5bc91e44d 100644 --- a/sw/airborne/modules/vehicle_interface/vi_overo_link.c +++ b/sw/airborne/modules/vehicle_interface/vi_overo_link.c @@ -25,7 +25,7 @@ #include "lisa/lisa_overo_link.h" #include "subsystems/imu.h" -#include +#include "subsystems/gps.h" #include "subsystems/sensors/baro.h" @@ -64,8 +64,8 @@ void vi_overo_link_on_msg_received(void) { vi.available_sensors &= ~(1< 0 && j != i && abs(cno-last_cnos[j]) >= 2) { - DOWNLINK_SEND_SVINFO(DefaultChannel, &j, &gps_svinfos[j].svid, &gps_svinfos[j].flags, &gps_svinfos[j].qi, &cno, &gps_svinfos[j].elev, &gps_svinfos[j].azim); - last_cnos[j] = gps_svinfos[j].cno; + DOWNLINK_SEND_SVINFO(DefaultChannel, &j, &gps_svinfos[j].svid, &gps_svinfos[j].flags, &gps_svinfos[j].qi, &cno, &gps_svinfos[j].elev, &gps_svinfos[j].azim); + last_cnos[j] = gps_svinfos[j].cno; } } } diff --git a/sw/airborne/gps.h b/sw/airborne/obsolete/gps.h similarity index 84% rename from sw/airborne/gps.h rename to sw/airborne/obsolete/gps.h index e201bc4980..e455e30633 100644 --- a/sw/airborne/gps.h +++ b/sw/airborne/obsolete/gps.h @@ -154,19 +154,19 @@ extern struct svinfo gps_svinfos[GPS_NB_CHANNELS]; #endif -#define GpsEventCheckAndHandle(_callback, _verbose) { \ - if (GpsBuffer()) { \ - ReadGpsBuffer(); \ - } \ - if (gps_msg_received) { \ - GpsParseOrConfigure(); \ - gps_msg_received = FALSE; \ - if (gps_pos_available) { \ - gps_verbose_downlink = _verbose; \ - UseGpsPos(_callback); \ - gps_pos_available = FALSE; \ - } \ - } \ +#define GpsEventCheckAndHandle(_callback, _verbose) { \ + if (GpsBuffer()) { \ + ReadGpsBuffer(); \ + } \ + if (gps_msg_received) { \ + GpsParseOrConfigure(); \ + gps_msg_received = FALSE; \ + if (gps_pos_available) { \ + gps_verbose_downlink = _verbose; \ + UseGpsPos(_callback); \ + gps_pos_available = FALSE; \ + } \ + } \ } diff --git a/sw/airborne/gps_ubx.c b/sw/airborne/obsolete/gps_ubx.c similarity index 100% rename from sw/airborne/gps_ubx.c rename to sw/airborne/obsolete/gps_ubx.c diff --git a/sw/airborne/gps_ubx.h b/sw/airborne/obsolete/gps_ubx.h similarity index 100% rename from sw/airborne/gps_ubx.h rename to sw/airborne/obsolete/gps_ubx.h diff --git a/sw/airborne/modules/ins/ins_osam_ugear.c b/sw/airborne/obsolete/ins_osam_ugear.c similarity index 100% rename from sw/airborne/modules/ins/ins_osam_ugear.c rename to sw/airborne/obsolete/ins_osam_ugear.c diff --git a/sw/airborne/modules/ins/ins_osam_ugear.h b/sw/airborne/obsolete/ins_osam_ugear.h similarity index 100% rename from sw/airborne/modules/ins/ins_osam_ugear.h rename to sw/airborne/obsolete/ins_osam_ugear.h diff --git a/sw/airborne/latlong.c b/sw/airborne/obsolete/latlong.c similarity index 100% rename from sw/airborne/latlong.c rename to sw/airborne/obsolete/latlong.c diff --git a/sw/airborne/latlong.h b/sw/airborne/obsolete/latlong.h similarity index 100% rename from sw/airborne/latlong.h rename to sw/airborne/obsolete/latlong.h diff --git a/sw/airborne/peripherals/hmc5843.c b/sw/airborne/peripherals/hmc5843.c index d520996831..188b3d18b0 100644 --- a/sw/airborne/peripherals/hmc5843.c +++ b/sw/airborne/peripherals/hmc5843.c @@ -1,4 +1,6 @@ #include "peripherals/hmc5843.h" +#include "mcu_periph/i2c.h" +#include "led.h" #define HMC5843_TIMEOUT 100 @@ -7,12 +9,18 @@ struct Hmc5843 hmc5843; void exti9_5_irq_handler(void); +#ifndef HMC5843_I2C_DEVICE +#define HMC5843_I2C_DEVICE i2c2 +#endif + void hmc5843_init(void) { hmc5843.i2c_trans.status = I2CTransSuccess; hmc5843.i2c_trans.slave_addr = HMC5843_ADDR; - hmc5843_arch_init(); +#ifndef HMC5843_NO_IRQ + hmc5843_arch_init(); +#endif } // blocking, only intended to be called for initialization @@ -22,25 +30,33 @@ static void send_config(void) hmc5843.i2c_trans.buf[0] = HMC5843_REG_CFGA; // set to rate to 50Hz hmc5843.i2c_trans.buf[1] = 0x00 | (0x06 << 2); hmc5843.i2c_trans.len_w = 2; - i2c_submit(&i2c2,&hmc5843.i2c_trans); - while(hmc5843.i2c_trans.status == I2CTransPending); + i2c_submit(&HMC5843_I2C_DEVICE,&hmc5843.i2c_trans); + while(hmc5843.i2c_trans.status == I2CTransPending); hmc5843.i2c_trans.type = I2CTransTx; hmc5843.i2c_trans.buf[0] = HMC5843_REG_CFGB; // set to gain to 1 Gauss hmc5843.i2c_trans.buf[1] = 0x01<<5; hmc5843.i2c_trans.len_w = 2; - i2c_submit(&i2c2,&hmc5843.i2c_trans); - while(hmc5843.i2c_trans.status == I2CTransPending); + i2c_submit(&HMC5843_I2C_DEVICE,&hmc5843.i2c_trans); + while(hmc5843.i2c_trans.status == I2CTransPending); hmc5843.i2c_trans.type = I2CTransTx; hmc5843.i2c_trans.buf[0] = HMC5843_REG_MODE; // set to continuous mode hmc5843.i2c_trans.buf[1] = 0x00; hmc5843.i2c_trans.len_w = 2; - i2c_submit(&i2c2,&hmc5843.i2c_trans); - while(hmc5843.i2c_trans.status == I2CTransPending); + i2c_submit(&HMC5843_I2C_DEVICE,&hmc5843.i2c_trans); + while(hmc5843.i2c_trans.status == I2CTransPending); } +#ifdef HMC5843_NO_IRQ +volatile uint8_t fake_mag_eoc = 0; +static uint8_t mag_eoc(void) +{ + return fake_mag_eoc; +} +#endif + void hmc5843_idle_task(void) { if (hmc5843.i2c_trans.status == I2CTransFailed) { @@ -51,11 +67,11 @@ void hmc5843_idle_task(void) if (hmc5843.i2c_trans.status == I2CTransRunning || hmc5843.i2c_trans.status == I2CTransPending) return; if (hmc5843.initialized && mag_eoc() && !hmc5843.sent_tx && !hmc5843.sent_rx) { - if (i2c2.status == I2CIdle && i2c_idle(&i2c2)) { + if (HMC5843_I2C_DEVICE.status == I2CIdle && i2c_idle(&HMC5843_I2C_DEVICE)) { hmc5843.i2c_trans.type = I2CTransTx; hmc5843.i2c_trans.len_w = 1; hmc5843.i2c_trans.buf[0] = 0x3; - i2c_submit(&i2c2, &hmc5843.i2c_trans); + i2c_submit(&HMC5843_I2C_DEVICE, &hmc5843.i2c_trans); hmc5843.sent_tx = 1; return; } @@ -66,7 +82,7 @@ void hmc5843_idle_task(void) hmc5843.i2c_trans.len_r = 6; hmc5843.i2c_trans.len_w = 1; hmc5843.i2c_trans.buf[0] = 0x3; - i2c_submit(&i2c2, &hmc5843.i2c_trans); + i2c_submit(&HMC5843_I2C_DEVICE, &hmc5843.i2c_trans); hmc5843.sent_rx = 1; hmc5843.sent_tx = 0; return; @@ -77,7 +93,7 @@ void hmc5843_idle_task(void) hmc5843.sent_tx = 0; hmc5843.timeout = 0; hmc5843.data_available = TRUE; - memcpy(hmc5843.data.buf, (const void *) hmc5843.i2c_trans.buf, 6); + memcpy(hmc5843.data.buf, (const void*) hmc5843.i2c_trans.buf, 6); for (int i = 0; i < 3; i++) { hmc5843.data.value[i] = bswap_16(hmc5843.data.value[i]); } @@ -89,20 +105,26 @@ void hmc5843_periodic(void) if (!hmc5843.initialized) { send_config(); hmc5843.initialized = TRUE; - } else if (hmc5843.timeout++ > HMC5843_TIMEOUT && i2c2.status == I2CIdle && i2c_idle(&i2c2)){ + } else if (hmc5843.timeout++ > HMC5843_TIMEOUT && HMC5843_I2C_DEVICE.status == I2CIdle && i2c_idle(&HMC5843_I2C_DEVICE)){ #ifdef USE_HMC59843_ARCH_RESET hmc5843_arch_reset(); #endif hmc5843.i2c_trans.type = I2CTransTx; hmc5843.i2c_trans.len_w = 1; hmc5843.i2c_trans.buf[0] = 0x3; - i2c_submit(&i2c2, &hmc5843.i2c_trans); + i2c_submit(&HMC5843_I2C_DEVICE, &hmc5843.i2c_trans); while(hmc5843.i2c_trans.status == I2CTransPending || hmc5843.i2c_trans.status == I2CTransRunning); hmc5843.i2c_trans.type = I2CTransRx; hmc5843.i2c_trans.len_r = 6; - i2c_submit(&i2c2, &hmc5843.i2c_trans); + i2c_submit(&HMC5843_I2C_DEVICE, &hmc5843.i2c_trans); while(hmc5843.i2c_trans.status == I2CTransPending || hmc5843.i2c_trans.status == I2CTransRunning); hmc5843.timeout = 0; } + +#ifdef HMC5843_NO_IRQ + // < 50Hz + fake_mag_eoc = 1; +#endif + } diff --git a/sw/airborne/peripherals/hmc5843.h b/sw/airborne/peripherals/hmc5843.h index 2601dae09d..a198ddab11 100644 --- a/sw/airborne/peripherals/hmc5843.h +++ b/sw/airborne/peripherals/hmc5843.h @@ -27,7 +27,6 @@ #include "std.h" #include "mcu_periph/i2c.h" - struct Hmc5843 { struct i2c_transaction i2c_trans; uint32_t timeout; @@ -45,6 +44,7 @@ extern struct Hmc5843 hmc5843; #ifndef HMC5843_NO_IRQ #include "peripherals/hmc5843_arch.h" + extern void hmc5843_arch_init( void ); extern void hmc5843_arch_reset( void ); #endif diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c index 4a3e9abce3..e1be916360 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c @@ -27,9 +27,10 @@ #include "subsystems/imu.h" #include "subsystems/ahrs/ahrs_float_dcm_algebra.h" +#include "math/pprz_algebra_float.h" #ifdef USE_GPS -#include "gps.h" +#include "subsystems/gps.h" #endif #include @@ -73,10 +74,13 @@ 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}}; #ifdef USE_MAGNETOMETER -float MAG_Heading; +float MAG_Heading_X = 1; +float MAG_Heading_Y = 0; #endif static inline void compute_body_orientation_and_rates(void); +static inline void set_dcm_matrix_from_rmat(struct FloatRMat *rmat); + void Normalize(void); void Drift_correction(void); void Matrix_update(void); @@ -88,6 +92,16 @@ float imu_health = 0.; #endif +static inline void set_dcm_matrix_from_rmat(struct FloatRMat *rmat) +{ + for (int i=0; i<3; i++) { + for (int j=0; j<3; j++) { + DCM_Matrix[i][j] = RMAT_ELMT(*rmat, j, i); + } + } +} + + /**************************************************/ void ahrs_update_fw_estimator( void ) @@ -103,18 +117,21 @@ void ahrs_update_fw_estimator( void ) ahrs_float.ltp_to_imu_euler.psi += M_PI; // Rotating the angle 180deg to fit for PPRZ #endif + /* set quaternion and rotation matrix representations as well */ + FLOAT_QUAT_OF_EULERS(ahrs_float.ltp_to_imu_quat, ahrs_float.ltp_to_imu_euler); + FLOAT_RMAT_OF_EULERS(ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_euler); - //warning, only eulers written to ahrs struct so far - //compute_body_orientation_and_rates(); + compute_body_orientation_and_rates(); // export results to estimator - estimator_phi = ahrs_float.ltp_to_imu_euler.phi - ins_roll_neutral; - estimator_theta = ahrs_float.ltp_to_imu_euler.theta - ins_pitch_neutral; - estimator_psi = ahrs_float.ltp_to_imu_euler.psi; + estimator_phi = ahrs_float.ltp_to_body_euler.phi - ins_roll_neutral; + estimator_theta = ahrs_float.ltp_to_body_euler.theta - ins_pitch_neutral; + estimator_psi = ahrs_float.ltp_to_body_euler.psi; - estimator_p = Omega_Vector[0]; - - RunOnceEvery(6,DOWNLINK_SEND_RMAT_DEBUG(DefaultChannel, + estimator_p = ahrs_float.body_rate.p; + estimator_q = ahrs_float.body_rate.q; +/* + RunOnceEvery(6,DOWNLINK_SEND_RMAT_DEBUG(DefaultChannel, &(DCM_Matrix[0][0]), &(DCM_Matrix[0][1]), &(DCM_Matrix[0][2]), @@ -128,7 +145,7 @@ void ahrs_update_fw_estimator( void ) &(DCM_Matrix[2][2]) )); - +*/ } @@ -155,6 +172,9 @@ void ahrs_init(void) { 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); + + /* set inital filter dcm */ + set_dcm_matrix_from_rmat(&ahrs_float.ltp_to_imu_rmat); } void ahrs_align(void) @@ -166,6 +186,9 @@ void ahrs_align(void) FLOAT_QUAT_OF_EULERS(ahrs_float.ltp_to_imu_quat, ahrs_float.ltp_to_imu_euler); FLOAT_RMAT_OF_QUAT(ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_quat); + /* set filter dcm */ + set_dcm_matrix_from_rmat(&ahrs_float.ltp_to_imu_rmat); + /* Compute initial body orientation */ compute_body_orientation_and_rates(); @@ -219,19 +242,69 @@ void ahrs_update_accel(void) accel_float.y = -accel_float.y; accel_float.z = -accel_float.z; + #ifdef USE_GPS - if (gps_mode==3) { //Remove centrifugal acceleration. - accel_float.y += gps_speed_3d/100. * Omega[2]; // Centrifugal force on Acc_y = GPS_speed*GyroZ - accel_float.z -= gps_speed_3d/100. * Omega[1]; // Centrifugal force on Acc_z = GPS_speed*GyroY + if (gps.fix == GPS_FIX_3D) { //Remove centrifugal acceleration. + accel_float.y += gps.speed_3d/100. * Omega[2]; // Centrifugal force on Acc_y = GPS_speed*GyroZ + accel_float.z -= gps.speed_3d/100. * Omega[1]; // Centrifugal force on Acc_z = GPS_speed*GyroY } #endif Drift_correction(); } + void ahrs_update_mag(void) { - //TODO +#ifdef USE_MAGNETOMETER +#warning MAGNETOMETER FEEDBACK NOT TESTED YET + + float cos_roll; + float sin_roll; + float cos_pitch; + float sin_pitch; + + cos_roll = cos(ahrs_float.ltp_to_imu_euler.phi); + sin_roll = sin(ahrs_float.ltp_to_imu_euler.phi); + cos_pitch = cos(ahrs_float.ltp_to_imu_euler.theta); + sin_pitch = sin(ahrs_float.ltp_to_imu_euler.theta); + + + // Pitch&Roll Compensation: + MAG_Heading_X = imu.mag.x*cos_pitch+imu.mag.y*sin_roll*sin_pitch+imu.mag.z*cos_roll*sin_pitch; + MAG_Heading_Y = imu.mag.y*cos_roll-imu.mag.z*sin_roll; + +/* + * + // Magnetic Heading + Heading = atan2(-Head_Y,Head_X); + + // Declination correction (if supplied) + if( declination != 0.0 ) + { + Heading = Heading + declination; + if (Heading > M_PI) // Angle normalization (-180 deg, 180 deg) + Heading -= (2.0 * M_PI); + else if (Heading < -M_PI) + Heading += (2.0 * M_PI); + } + + // Optimization for external DCM use. Calculate normalized components + Heading_X = cos(Heading); + Heading_Y = sin(Heading); +*/ + + struct FloatVect3 ltp_mag; + + ltp_mag.x = MAG_Heading_X; + ltp_mag.y = MAG_Heading_Y; + + // Downlink + RunOnceEvery(10,DOWNLINK_SEND_IMU_MAG(DefaultChannel, <p_mag.x, <p_mag.y, <p_mag.z)); + + // Magnetic Heading + // MAG_Heading = atan2(imu.mag.y, -imu.mag.x); +#endif } void Normalize(void) @@ -309,16 +382,8 @@ void Normalize(void) // Reset on trouble 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; + set_dcm_matrix_from_rmat(&ahrs_impl.body_to_imu_rmat); + problem = FALSE; } } @@ -365,9 +430,10 @@ void Drift_correction(void) #ifdef USE_MAGNETOMETER // We make the gyro YAW drift correction based on compass magnetic heading - float mag_heading_x = cos(MAG_Heading); - float mag_heading_y = sin(MAG_Heading); - errorCourse=(DCM_Matrix[0][0]*mag_heading_y) - (DCM_Matrix[1][0]*mag_heading_x); //Calculating YAW error +// float mag_heading_x = cos(MAG_Heading); +// float mag_heading_y = sin(MAG_Heading); + // 2D dot product + errorCourse=(DCM_Matrix[0][0]*MAG_Heading_Y) + (DCM_Matrix[1][0]*MAG_Heading_X); //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); @@ -378,10 +444,10 @@ void Drift_correction(void) #elif defined USE_GPS // Use GPS Ground course to correct yaw gyro drift - if(gps_mode==3 && gps_gspeed>= 500) { //got a 3d fix and ground speed is more than 0.5 m/s - float ground_course = gps_course/10. - 180.; //This is the runaway direction of you "plane" in degrees - float COGX = cos(RadOfDeg(ground_course)); //Course overground X axis - float COGY = sin(RadOfDeg(ground_course)); //Course overground Y axis + if(gps.fix == GPS_FIX_3D && gps.gspeed>= 500) { //got a 3d fix and ground speed is more than 0.5 m/s + float ground_course = ((float)gps.course)/1.e7 - M_PI; //This is the runaway direction of you "plane" in rad + float COGX = cosf(ground_course); //Course overground X axis + float COGY = sinf(ground_course); //Course overground Y axis 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. diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.c index a74f631cb3..bcf3c695d2 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.c @@ -100,7 +100,7 @@ void ahrs_init(void) { void ahrs_align(void) { - + /* Compute an initial orientation using euler angles */ ahrs_int_get_euler_from_accel_mag(&ahrs.ltp_to_imu_euler, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag); /* Convert initial orientation in quaternion and rotation matrice representations. */ @@ -215,12 +215,12 @@ void ahrs_update_mag(void) { 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), + 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); @@ -228,12 +228,12 @@ static inline void ahrs_update_mag_full(void) { 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); } @@ -241,17 +241,17 @@ static inline void ahrs_update_mag_full(void) { static inline void ahrs_update_mag_2d(void) { - const struct Int32Vect2 expected_ltp = {MAG_BFP_OF_REAL(AHRS_H_X), + 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 = + + 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); @@ -266,7 +266,7 @@ static inline void ahrs_update_mag_2d(void) { 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 @@ -300,12 +300,12 @@ __attribute__ ((always_inline)) static inline void compute_imu_quat_and_rmat_fro /* 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) { @@ -320,3 +320,34 @@ __attribute__ ((always_inline)) static inline void compute_body_orientation(void INT32_RMAT_TRANSP_RATEMULT(ahrs.body_rate, imu.body_to_imu_rmat, ahrs.imu_rate); } + + +#ifdef AHRS_UPDATE_FW_ESTIMATOR +// TODO use ahrs result directly +#include "estimator.h" +// remotely settable +#ifndef INS_ROLL_NEUTRAL_DEFAULT +#define INS_ROLL_NEUTRAL_DEFAULT 0 +#endif +#ifndef INS_PITCH_NEUTRAL_DEFAULT +#define INS_PITCH_NEUTRAL_DEFAULT 0 +#endif +float ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT; +float ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT; +void ahrs_update_fw_estimator(void) +{ + struct FloatEulers att; + // export results to estimator + EULERS_FLOAT_OF_BFP(att, ahrs.ltp_to_body_euler); + + estimator_phi = att.phi - ins_roll_neutral; + estimator_theta = att.theta - ins_pitch_neutral; + estimator_psi = att.psi; + + struct FloatRates rates; + RATES_FLOAT_OF_BFP(rates, ahrs_float.body_rate); + estimator_p = rates.p; + estimator_q = rates.q; + +} +#endif //AHRS_UPDATE_FW_ESTIMATOR diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.h b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.h index f1f833b656..4671621d26 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.h +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.h @@ -40,4 +40,13 @@ struct AhrsIntCmpl { extern struct AhrsIntCmpl ahrs_impl; + +#ifdef AHRS_UPDATE_FW_ESTIMATOR +// TODO copy ahrs to state instead of estimator +void ahrs_update_fw_estimator(void); +extern float ins_roll_neutral; +extern float ins_pitch_neutral; +#endif + + #endif /* AHRS_INT_CMPL_H */ diff --git a/sw/airborne/subsystems/electrical.c b/sw/airborne/subsystems/electrical.c index 125ccf5699..4694df4e89 100644 --- a/sw/airborne/subsystems/electrical.c +++ b/sw/airborne/subsystems/electrical.c @@ -50,7 +50,7 @@ void electrical_periodic(void) { #ifdef ADC_CHANNEL_CURRENT #ifndef SITL - electrical.current = MilliAmpereOfAdc((electrical_priv.current_adc_buf.sum/electrical_priv.current_adc_buf.av_nb_sample)); + electrical.current = MilliAmpereOfAdc((electrical_priv.current_adc_buf.sum/electrical_priv.current_adc_buf.av_nb_sample)); #endif #else #if defined MILLIAMP_AT_FULL_THROTTLE && defined COMMAND_THROTTLE diff --git a/sw/airborne/subsystems/electrical.h b/sw/airborne/subsystems/electrical.h index c36cea22b3..9980257d28 100644 --- a/sw/airborne/subsystems/electrical.h +++ b/sw/airborne/subsystems/electrical.h @@ -6,7 +6,7 @@ struct Electrical { uint8_t vsupply; /* supply in decivolts */ - int32_t current; /* current in miliamps */ + int32_t current; /* current in milliamps */ }; diff --git a/sw/airborne/subsystems/gps.c b/sw/airborne/subsystems/gps.c new file mode 100644 index 0000000000..4d50a634dc --- /dev/null +++ b/sw/airborne/subsystems/gps.c @@ -0,0 +1,40 @@ +/* + * 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 "subsystems/gps.h" + +#include "led.h" + +struct GpsState gps; + +#ifdef GPS_TIMESTAMP +struct GpsTimeSync gps_time; +#endif + +void gps_init(void) { + gps.fix = GPS_FIX_NONE; +#ifdef GPS_LED + LED_OFF(GPS_LED); +#endif +#ifdef GPS_TYPE_H + gps_impl_init(); +#endif +} diff --git a/sw/airborne/subsystems/gps.h b/sw/airborne/subsystems/gps.h new file mode 100644 index 0000000000..21a30c6bb1 --- /dev/null +++ b/sw/airborne/subsystems/gps.h @@ -0,0 +1,153 @@ +/* + * Copyright (C) 2003-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. + */ + +/** @file gps.h + * @brief Device independent GPS code + * + */ + +#ifndef GPS_H +#define GPS_H + + +#include "std.h" +#include "math/pprz_geodetic_int.h" + + +/* GPS model specific implementation or sim */ +#ifdef GPS_TYPE_H +#include GPS_TYPE_H +#endif + +#define GPS_FIX_NONE 0x00 +#define GPS_FIX_3D 0x03 + +#define GpsFixValid() (gps.fix == GPS_FIX_3D) + + +#ifndef GPS_NB_CHANNELS +#define GPS_NB_CHANNELS 1 +#endif + +/** Space Vehicle Information */ +struct SVinfo { + uint8_t svid; + uint8_t flags; + uint8_t qi; + uint8_t cno; + int8_t elev; ///< deg + int16_t azim; ///< deg +}; + +struct GpsState { + struct EcefCoor_i ecef_pos; ///< position in ECEF in cm + struct LlaCoor_i lla_pos; ///< position in LLA (lat,lon: rad*1e7; alt: mm over ellipsoid) + struct UtmCoor_i utm_pos; ///< position in UTM (north,east: cm; alt: mm over ellipsoid) + int32_t hmsl; ///< height above mean sea level in mm + struct EcefCoor_i ecef_vel; ///< speed ECEF in cm/s + struct NedCoor_i ned_vel; ///< speed NED in cm/s + int16_t gspeed; ///< norm of 2d ground speed in cm/s + int16_t speed_3d; ///< norm of 3d speed in cm/s + int32_t course; ///< GPS heading in rad*1e7 + uint32_t pacc; ///< position accuracy + uint32_t sacc; ///< speed accuracy + uint16_t pdop; ///< dilution of precision + uint8_t num_sv; ///< number of sat in fix + uint8_t fix; ///< status of fix + int16_t week; ///< GPS week + uint32_t tow; ///< time of week in ms + + uint8_t nb_channels; ///< Number of scanned satellites + struct SVinfo svinfos[GPS_NB_CHANNELS]; + + uint16_t last_fix_time; ///< cpu time in sec at last valid fix + uint16_t reset; ///< hotstart, warmstart, coldstart +}; + +struct GpsTimeSync { + uint32_t t0_tow; ///< for time sync: time of week in ms for time sync + int32_t t0_tow_frac; ///< for time sync: fractional ns remainder of tow [ms], range -500000 .. 500000 + uint32_t t0; ///< for time sync: hw clock ticks when GPS message is received +}; + +extern struct GpsState gps; + + + +extern void gps_init(void); + +/* GPS model specific init implementation */ +extern void gps_impl_init(void); + + +/* mark GPS as lost when no valid 3D fix was received for GPS_TIMEOUT secs */ +#ifndef GPS_TIMEOUT +#define GPS_TIMEOUT 5 +#endif +#define GpsIsLost() (cpu_time_sec - gps.last_fix_time > GPS_TIMEOUT) + + +//TODO +// this still needs to call gps specific stuff + +/* + * GPS Reset + */ + +#define CFG_RST_BBR_Hotstart 0x0000 +#define CFG_RST_BBR_Warmstart 0x0001 +#define CFG_RST_BBR_Coldstart 0xffff + +#define gps_Reset(_val) { \ +} + + + +#ifdef GPS_TIMESTAMP +#ifndef PCLK +#error unknown PCLK frequency +#endif + +extern struct GpsTimeSync gps_time; + +uint32_t gps_tow_from_ticks(uint32_t clock_ticks) +{ + uint32_t clock_delta; + uint32_t time_delta; + uint32_t itow_now; + + if (clock_ticks < gps_t0) { + clock_delta = (0xFFFFFFFF - clock_ticks) + gps_time.t0 + 1; + } else { + clock_delta = clock_ticks - gps_time.t0; + } + + time_delta = MSEC_OF_SYS_TICS(clock_delta); + + itow_now = gps_time.t0_tow + time_delta; + if (itow_now > MSEC_PER_WEEK) itow_now %= MSEC_PER_WEEK; + + return itow_now; +} +#endif + + +#endif /* GPS_H */ diff --git a/sw/airborne/subsystems/gps/gps_nmea.c b/sw/airborne/subsystems/gps/gps_nmea.c new file mode 100644 index 0000000000..252a9b11bf --- /dev/null +++ b/sw/airborne/subsystems/gps/gps_nmea.c @@ -0,0 +1,409 @@ +/* + * + * 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. + * + */ + +/** + * file gps_nmea.c + * brief Parser for the NMEA protocol + * + * This file is a drop-in replacement for gps_ubx.c + * + * TODO: THIS NMEA-PARSER IS NOT WELL TESTED AND INCOMPLETE!!! + * Status: + * Parsing GGA and RMC is complete, GSA and other records are + * incomplete. + */ + +#include "subsystems/gps.h" + +#include "led.h" + +#ifdef GPS_USE_LATLONG +#include "subsystems/nav.h" +#include "math/pprz_geodetic_float.h" +#endif + +#include +#include +#include +#include +#ifdef DEBUG_NMEA +// do debug-output if run on the DEBUG_NMEA-target + +#endif + + +struct GpsNmea gps_nmea; + +void parse_nmea_GPGSA(void); +void parse_nmea_GPRMC(void); +void parse_nmea_GPGGA(void); + + +void gps_impl_init( void ) { + gps_nmea.msg_available = FALSE; + gps_nmea.pos_available = FALSE; + gps_nmea.gps_nb_ovrn = 0; + gps_nmea.msg_len = 0; +} + + + +/** + * parse GPGSA-nmea-messages stored in + * nmea_msg_buf . + */ +void parse_nmea_GPGSA(void) { + int i = 8; // current position in the message + // char* endptr; // end of parsed substrings + + // attempt to reject empty packets right away + if(gps_nmea.msg_buf[i]==',' && gps_nmea.msg_buf[i+1]==',') { + NMEA_PRINT("p_GPGSA() - skipping empty message\n\r"); + return; + } + + // get auto2D/3D + // ignored + while(gps_nmea.msg_buf[i++] != ',') { // next field: fix + if (i >= gps_nmea.msg_len) { + NMEA_PRINT("p_GPGSA() - skipping incomplete message\n\r"); + return; + } + } + + // get 2D/3D-fix + // set gps_mode=3=3d, 2=2d, 1=no fix or 0 + gps.fix = atoi(&gps_nmea.msg_buf[i]); + if (gps.fix == 1) + gps.fix = 0; + NMEA_PRINT("p_GPGSA() - gps.fix=%i (3=3D)\n\r", gps.fix); + while(gps_nmea.msg_buf[i++] != ',') { // next field:satellite-number-0 + if (i >= gps_nmea.msg_len) { + NMEA_PRINT("p_GPGSA() - skipping incomplete message\n\r"); + return; + } + } + + //int satcount = 0; + + // TODO: get sateline-numbers for gps_svinfos +} + +/** + * parse GPRMC-nmea-messages stored in + * gps_nmea.msg_buf . + */ +void parse_nmea_GPRMC(void) { + int i = 8; // current position in the message + char* endptr; // end of parsed substrings + + // attempt to reject empty packets right away + if(gps_nmea.msg_buf[i]==',' && gps_nmea.msg_buf[i+1]==',') { + NMEA_PRINT("p_GPRMC() - skipping empty message\n\r"); + return; + } + + // get time + // ignored + while(gps_nmea.msg_buf[i++] != ',') { // next field: warning + if (i >= gps_nmea.msg_len) { + NMEA_PRINT("p_GPRMC() - skipping incomplete message\n\r"); + return; + } + } + + // get warning + // ignored + while(gps_nmea.msg_buf[i++] != ',') { // next field: lat + if (i >= gps_nmea.msg_len) { + NMEA_PRINT("p_GPRMC() - skipping incomplete message\n\r"); + return; + } + } + // get lat + // ignored + while(gps_nmea.msg_buf[i++] != ',') { // next field: N/S + if (i >= gps_nmea.msg_len) { + NMEA_PRINT("p_GPRMC() - skipping incomplete message\n\r"); + return; + } + } + // get North/South + // ignored + while(gps_nmea.msg_buf[i++] != ',') { // next field: lon + if (i >= gps_nmea.msg_len) { + NMEA_PRINT("p_GPRMC() - skipping incomplete message\n\r"); + return; + } + } + // get lon + // ignored + while(gps_nmea.msg_buf[i++] != ',') { // next field: E/W + if (i >= gps_nmea.msg_len) { + NMEA_PRINT("p_GPRMC() - skipping incomplete message\n\r"); + return; + } + } + // get eath/west + // ignored + while(gps_nmea.msg_buf[i++] != ',') { // next field: speed + if (i >= gps_nmea.msg_len) { + NMEA_PRINT("p_GPRMC() - skipping incomplete message\n\r"); + return; + } + } + // get speed + double speed = strtod(&gps_nmea.msg_buf[i], &endptr); + gps.gspeed = speed * 1.852 * 100 / (60*60); + NMEA_PRINT("p_GPRMC() - ground-speed=%d knot = %d cm/s\n\r", (speed*1000), (gps.gspeed*1000)); + while(gps_nmea.msg_buf[i++] != ',') { // next field: course + if (i >= gps_nmea.msg_len) { + NMEA_PRINT("p_GPRMC() - skipping incomplete message\n\r"); + return; + } + } + double course = strtod(&gps_nmea.msg_buf[i], &endptr); + gps.course=course*10; //FIXME should be GPS heading in rad*1e7 + NMEA_PRINT("COURSE: %d \n\r",gps_course); +} + + +/** + * parse GPGGA-nmea-messages stored in + * gps_nmea.msg_buf . + */ +void parse_nmea_GPGGA(void) { + int i = 8; // current position in the message + char* endptr; // end of parsed substrings + double degrees, minutesfrac; + struct LlaCoor_f lla_f; + + // attempt to reject empty packets right away + if(gps_nmea.msg_buf[i]==',' && gps_nmea.msg_buf[i+1]==',') { + NMEA_PRINT("p_GPGGA() - skipping empty message\n\r"); + return; + } + + // get UTC time [hhmmss.sss] + // ignored GpsInfo.PosLLA.TimeOfFix.f = strtod(&packet[i], &endptr); + double time = strtod(&gps_nmea.msg_buf[i],&endptr); + gps.tow = (uint32_t)((time+1)*1000); + + //AD TODO: strtod itow + while(gps_nmea.msg_buf[i++] != ',') { // next field: latitude + if (i >= gps_nmea.msg_len) { + NMEA_PRINT("p_GPGGA() - skipping incomplete message\n\r"); + return; + } + } + + // get latitude [ddmm.mmmmm] + double lat = strtod(&gps_nmea.msg_buf[i], &endptr); + // convert to pure degrees [dd.dddd] format + minutesfrac = modf(lat/100, °rees); + lat = degrees + (minutesfrac*100)/60; + // convert to radians + //GpsInfo.PosLLA.lat.f *= (M_PI/180); + lla_f.lat = RadOfDeg(lat); + while(gps_nmea.msg_buf[i++] != ',') { // next field: N/S indicator + if (i >= gps_nmea.msg_len) { + NMEA_PRINT("p_GPGGA() - skipping incomplete message\n\r"); + return; + } + } + + // correct latitute for N/S + if(gps_nmea.msg_buf[i] == 'S') + lat = -lat; + while(gps_nmea.msg_buf[i++] != ',') { // next field: longitude + if (i >= gps_nmea.msg_len) + return; + } + + // convert to radians + lla_f.lat = RadOfDeg(lat); + + gps.lla_pos.lat =lla_f.lat * 1e7; // convert to fixed-point + NMEA_PRINT("p_GPGGA() - lat=%d gps_lat=%i\n\r", (lat*1000), lla_f.lat); + + // get longitude [ddmm.mmmmm] + double lon = strtod(&gps_nmea.msg_buf[i], &endptr); + // convert to pure degrees [dd.dddd] format + minutesfrac = modf(lon/100, °rees); + lon = degrees + (minutesfrac*100)/60; + // convert to radians + //GpsInfo.PosLLA.lon.f *= (M_PI/180); + while(gps_nmea.msg_buf[i++] != ',') { // next field: E/W indicator + if (i >= gps_nmea.msg_len) + return; + } + + // correct latitute for E/W + if(gps_nmea.msg_buf[i] == 'W') + lon = -lon; + while(gps_nmea.msg_buf[i++] != ',') { // next field: position fix status + if (i >= gps_nmea.msg_len) + return; + } + + gps.lla_pos.lon = lla_f.lon * 1e7; // convert to fixed-point + NMEA_PRINT("p_GPGGA() - lon=%d gps_lon=%i time=%u\n\r", (lon*1000), lla_f.lon, gps.tow); + + /* convert to utm */ + struct UtmCoor_f utm_f; + utm_f.zone = nav_utm_zone0; + utm_of_lla_f(&utm_f, &lla_f); + + /* copy results of utm conversion */ + gps.utm_pos.east = utm_f.east*100; + gps.utm_pos.north = utm_f.north*100; + gps.utm_pos.alt = utm_f.alt*1000; + gps.utm_pos.zone = nav_utm_zone0; + + + + // position fix status + // 0 = Invalid, 1 = Valid SPS, 2 = Valid DGPS, 3 = Valid PPS + // check for good position fix + if( (gps_nmea.msg_buf[i] != '0') && (gps_nmea.msg_buf[i] != ',') ) { + gps_nmea.pos_available = TRUE; + NMEA_PRINT("p_GPGGA() - POS_AVAILABLE == TRUE\n\r"); + } else { + gps_nmea.pos_available = FALSE; + NMEA_PRINT("p_GPGGA() - gps_pos_available == false\n\r"); + } + while(gps_nmea.msg_buf[i++] != ',') { // next field: satellites used + if (i >= gps_nmea.msg_len) { + NMEA_PRINT("p_GPGGA() - skipping incomplete message\n\r\r"); + return; + } + } + + // get number of satellites used in GPS solution + gps.num_sv = atoi(&gps_nmea.msg_buf[i]); + NMEA_PRINT("p_GPGGA() - gps_numSatlitesUsed=%i\n\r", gps.num_sv); + while(gps_nmea.msg_buf[i++] != ',') { // next field: HDOP (horizontal dilution of precision) + if (i >= gps_nmea.msg_len) { + NMEA_PRINT("p_GPGGA() - skipping incomplete message\n\r"); + return; + } + } + while(gps_nmea.msg_buf[i++] != ',') { // next field: altitude + if (i >= gps_nmea.msg_len) { + NMEA_PRINT("p_GPGGA() - skipping incomplete message\n\r"); + return; + } + } + + // get altitude (in meters) + // FIXME alt above ellipsoid or geoid (MSL) ??? + double alt = strtod(&gps_nmea.msg_buf[i], &endptr); + gps.hmsl = alt * 100; + NMEA_PRINT("p_GPGGA() - gps_alt=%i\n\r", gps.hmsl); + + while(gps_nmea.msg_buf[i++] != ',') { // next field: altitude units, always 'M' + if (i >= gps_nmea.msg_len) + return; + } + while(gps_nmea.msg_buf[i++] != ',') { // next field: geoid seperation + if (i >= gps_nmea.msg_len) + return; + } + while(gps_nmea.msg_buf[i++] != ',') { // next field: seperation units + if (i >= gps_nmea.msg_len) + return; + } + while(gps_nmea.msg_buf[i++] != ',') { // next field: DGPS age + if (i >= gps_nmea.msg_len) + return; + } + while(gps_nmea.msg_buf[i++] != ',') { // next field: DGPS station ID + if (i >= gps_nmea.msg_len) + return; + } + //while(gps_nmea.msg_buf[i++] != '*'); // next field: checksum +} + +/** + * parse_nmea_char() has a complete line. + * Find out what type of message it is and + * hand it to the parser for that type. + */ +void nmea_parse_msg( void ) { + + if(gps_nmea.msg_len > 5 && !strncmp(gps_nmea.msg_buf , "GPRMC", 5)) { + gps_nmea.msg_buf[gps_nmea.msg_len] = 0; + NMEA_PRINT("parsing RMC: \"%s\" \n\r",gps_nmea.msg_buf); + NMEA_PRINT("RMC"); + parse_nmea_GPRMC(); + } else + if(gps_nmea.msg_len > 5 && !strncmp(gps_nmea.msg_buf , "GPGGA", 5)) { + gps_nmea.msg_buf[gps_nmea.msg_len] = 0; + NMEA_PRINT("parse_gps_msg() - parsing GGA gps-message \"%s\" \n\r",gps_nmea.msg_buf); + NMEA_PRINT("GGA"); + parse_nmea_GPGGA(); + } else + if(gps_nmea.msg_len > 5 && !strncmp(gps_nmea.msg_buf , "GPGSA", 5)) { + gps_nmea.msg_buf[gps_nmea.msg_len] = 0; + NMEA_PRINT("GSA: \"%s\" \n\r",gps_nmea.msg_buf); + NMEA_PRINT("GSA"); + parse_nmea_GPGSA(); + } else { + gps_nmea.msg_buf[gps_nmea.msg_len] = 0; + NMEA_PRINT("ignoring: len=%i \n\r \"%s\" \n\r", gps_nmea.msg_len, gps_nmea.msg_buf); + } + + // reset message-buffer + gps_nmea.msg_len = 0; +} + + +/** + * This is the actual parser. + * It reads one character at a time + * setting gps_nmea.msg_available to TRUE + * after a full line. + */ +void nmea_parse_char( uint8_t c ) { + //reject empty lines + if (gps_nmea.msg_len == 0) { + if (c == '\r' || c == '\n' || c == '$') + return; + } + + // fill the buffer, unless it's full + if (gps_nmea.msg_len < NMEA_MAXLEN - 1) { + + // messages end with a linefeed + //AD: TRUNK: if (c == '\r' || c == '\n') + if (c == '\r' || c == '\n') { + gps_nmea.msg_available = TRUE; + } else { + gps_nmea.msg_buf[gps_nmea.msg_len] = c; + gps_nmea.msg_len ++; + } + } + + if (gps_nmea.msg_len >= NMEA_MAXLEN - 1) + gps_nmea.msg_available = TRUE; +} diff --git a/sw/airborne/subsystems/gps/gps_nmea.h b/sw/airborne/subsystems/gps/gps_nmea.h new file mode 100644 index 0000000000..3e330915ff --- /dev/null +++ b/sw/airborne/subsystems/gps/gps_nmea.h @@ -0,0 +1,95 @@ +/* + * Copyright (C) 2004-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. + * + */ + +/** \file gps_nmea.h + * \brief NMEA protocol specific code + * +*/ + + +#ifndef GPS_NMEA_H +#define GPS_NMEA_H + +#include "mcu_periph/uart.h" + +#define GPS_NB_CHANNELS 16 + +#ifdef DEBUG_NMEA +#define NMEA_PRINT(...) { UsbSPrintString( __VA_ARGS__);}; +#else +#define NMEA_PRINT(...) {}; +#endif + +#define NMEA_MAXLEN 255 + +struct GpsNmea { + bool_t msg_available; + bool_t pos_available; + uint8_t gps_nb_ovrn; // number if incomplete nmea-messages + char msg_buf[NMEA_MAXLEN]; ///< buffer for storing one nmea-line + int msg_len; +}; + +extern struct GpsNmea gps_nmea; + + +/* + * This part is used by the autopilot to read data from a uart + */ +#define __GpsLink(dev, _x) dev##_x +#define _GpsLink(dev, _x) __GpsLink(dev, _x) +#define GpsLink(_x) _GpsLink(GPS_LINK, _x) + +#define GpsBuffer() GpsLink(ChAvailable()) + +#define GpsEvent(_sol_available_callback) { \ + if (GpsBuffer()) { \ + ReadGpsBuffer(); \ + } \ + if (gps_nmea.msg_available) { \ + nmea_parse_msg(); \ + if (gps_nmea.pos_available) { \ + if (gps.fix == GPS_FIX_3D) { \ + gps.last_fix_time = cpu_time_sec; \ + } \ + _sol_available_callback(); \ + } \ + gps_nmea.msg_available = FALSE; \ + } \ + } + +#define ReadGpsBuffer() { \ + while (GpsLink(ChAvailable())&&!gps_nmea.msg_available) \ + nmea_parse_char(GpsLink(Getch())); \ + } + + + +/** The function to be called when a characted friom the device is available */ +extern void nmea_parse_char(uint8_t c); + +extern void nmea_parse_msg(void); + + +#define gps_nmea_Reset(_val) { } + +#endif /* GPS_NMEA_H */ diff --git a/sw/airborne/subsystems/gps/gps_sim.c b/sw/airborne/subsystems/gps/gps_sim.c new file mode 100644 index 0000000000..6ac8669964 --- /dev/null +++ b/sw/airborne/subsystems/gps/gps_sim.c @@ -0,0 +1,44 @@ +/* + * 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 "subsystems/gps.h" + +bool_t gps_available; + + +#if 0 +void gps_feed_values(double utm_north, double utm_east, double utm_alt, double gspeed, double course, double climb) { + gps.utm_pos.north = CM_OF_M(utm_north); + gps.utm_pos.east = CM_OF_M(utm_east); + //TODO set height above ellipsoid properly + gps.hmsl = utm_alt * 1000.; + gps.gspeed = CM_OF_M(gspeed); + gps.course = EM7RAD_OF_RAD(RadOfDeg(course/10.)); + gps.ned_vel.z = -climb*100.; + gps.fix = GPS_FIX_3D; + gps_available = TRUE; +} +#endif + +void gps_impl_init(void) { + gps.fix = GPS_FIX_NONE; + gps_available = FALSE; +} diff --git a/sw/airborne/subsystems/gps/gps_sim.h b/sw/airborne/subsystems/gps/gps_sim.h new file mode 100644 index 0000000000..7794ea9c24 --- /dev/null +++ b/sw/airborne/subsystems/gps/gps_sim.h @@ -0,0 +1,24 @@ +#ifndef GPS_SIM_H +#define GPS_SIM_H + +#include "std.h" + +#define GPS_NB_CHANNELS 16 + +extern bool_t gps_available; + +//extern void gps_feed_values(double utm_north, double utm_east, double utm_alt, double gspeed, double course, double climb); + +extern void gps_impl_init(void); + +#define GpsEvent(_sol_available_callback) { \ + if (gps_available) { \ + if (gps.fix == GPS_FIX_3D) { \ + gps.last_fix_time = cpu_time_sec; \ + } \ + _sol_available_callback(); \ + gps_available = FALSE; \ + } \ + } + +#endif /* GPS_SIM_H */ diff --git a/sw/airborne/subsystems/gps/gps_sim_nps.c b/sw/airborne/subsystems/gps/gps_sim_nps.c new file mode 100644 index 0000000000..55c935af60 --- /dev/null +++ b/sw/airborne/subsystems/gps/gps_sim_nps.c @@ -0,0 +1,45 @@ +/* + * 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 "subsystems/gps/gps_sim_nps.h" +#include "subsystems/gps.h" + +bool_t gps_available; + +void gps_feed_value() { + gps.ecef_pos.x = sensors.gps.ecef_pos.x * 100.; + gps.ecef_pos.y = sensors.gps.ecef_pos.y * 100.; + gps.ecef_pos.z = sensors.gps.ecef_pos.z * 100.; + gps.ecef_vel.x = sensors.gps.ecef_vel.x * 100.; + gps.ecef_vel.y = sensors.gps.ecef_vel.y * 100.; + gps.ecef_vel.z = sensors.gps.ecef_vel.z * 100.; + //ecef pos seems to be based on geocentric model, hence we get a very high alt when converted to lla + gps.lla_pos.lat = sensors.gps.lla_pos.lat * 1e7; + gps.lla_pos.lon = sensors.gps.lla_pos.lon * 1e7; + gps.lla_pos.alt = sensors.gps.lla_pos.alt * 1000.; + gps.hmsl = sensors.gps.hmsl * 1000.; + gps.fix = GPS_FIX_3D; + gps_available = TRUE; +} + +void gps_impl_init() { + gps_available = FALSE; +} diff --git a/sw/airborne/subsystems/gps/gps_sim_nps.h b/sw/airborne/subsystems/gps/gps_sim_nps.h new file mode 100644 index 0000000000..c14e3f8f94 --- /dev/null +++ b/sw/airborne/subsystems/gps/gps_sim_nps.h @@ -0,0 +1,23 @@ +#ifndef GPS_SIM_NPS_H +#define GPS_SIM_NPS_H + +#include "nps_sensors.h" + +#define GPS_NB_CHANNELS 16 + +extern bool_t gps_available; + +extern void gps_feed_value(); + +extern void gps_impl_init(); + +#define GpsEvent(_sol_available_callback) { \ + if (gps_available) { \ + if (gps.fix == GPS_FIX_3D) \ + gps.last_fix_time = cpu_time_sec; \ + _sol_available_callback(); \ + gps_available = FALSE; \ + } \ + } + +#endif /* GPS_SIM_NPS_H */ diff --git a/sw/airborne/subsystems/gps/gps_skytraq.c b/sw/airborne/subsystems/gps/gps_skytraq.c new file mode 100644 index 0000000000..30ab9674fb --- /dev/null +++ b/sw/airborne/subsystems/gps/gps_skytraq.c @@ -0,0 +1,153 @@ +/* + * Copyright (C) 2010 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 "subsystems/gps.h" + +struct GpsSkytraq gps_skytraq; + +/* parser status */ +#define UNINIT 0 +#define GOT_SYNC1 1 +#define GOT_SYNC2 2 +#define GOT_LEN1 3 +#define GOT_LEN2 4 +#define GOT_ID 5 +#define GOT_PAYLOAD 6 +#define GOT_CHECKSUM 7 +#define GOT_SYNC3 8 + +//#include "my_debug_servo.h" +#include "led.h" + +void gps_impl_init(void) { + + gps_skytraq.status = UNINIT; + + + //DEBUG_SERVO1_INIT(); + +} + + +void gps_skytraq_read_message(void) { + + //DEBUG_S1_ON(); + + if (gps_skytraq.msg_id == SKYTRAQ_ID_NAVIGATION_DATA) { + gps.ecef_pos.x = SKYTRAQ_NAVIGATION_DATA_ECEFX(gps_skytraq.msg_buf); + gps.ecef_pos.y = SKYTRAQ_NAVIGATION_DATA_ECEFY(gps_skytraq.msg_buf); + gps.ecef_pos.z = SKYTRAQ_NAVIGATION_DATA_ECEFZ(gps_skytraq.msg_buf); + gps.ecef_vel.x = SKYTRAQ_NAVIGATION_DATA_ECEFVX(gps_skytraq.msg_buf); + gps.ecef_vel.y = SKYTRAQ_NAVIGATION_DATA_ECEFVY(gps_skytraq.msg_buf); + gps.ecef_vel.z = SKYTRAQ_NAVIGATION_DATA_ECEFVZ(gps_skytraq.msg_buf); + gps.lla_pos.lat = SKYTRAQ_NAVIGATION_DATA_LAT(gps_skytraq.msg_buf); + gps.lla_pos.lon = SKYTRAQ_NAVIGATION_DATA_LON(gps_skytraq.msg_buf); + gps.lla_pos.alt = SKYTRAQ_NAVIGATION_DATA_AEL(gps_skytraq.msg_buf); + gps.hmsl = SKYTRAQ_NAVIGATION_DATA_ASL(gps_skytraq.msg_buf); + // pacc; + // sacc; + // gps.pdop = SKYTRAQ_NAVIGATION_DATA_PDOP(gps_skytraq.msg_buf); + gps.num_sv = SKYTRAQ_NAVIGATION_DATA_NumSV(gps_skytraq.msg_buf); + gps.fix = SKYTRAQ_NAVIGATION_DATA_FixMode(gps_skytraq.msg_buf); + gps.tow = SKYTRAQ_NAVIGATION_DATA_TOW(gps_skytraq.msg_buf)/10; + //DEBUG_S2_TOGGLE(); + +#ifdef GPS_LED + if (gps.fix == GPS_FIX_3D) { + LED_ON(GPS_LED); + } + else { + LED_TOGGLE(GPS_LED); + } +#endif + } + + //DEBUG_S1_OFF(); +} + +void gps_skytraq_parse(uint8_t c) { + if (gps_skytraq.status < GOT_PAYLOAD) + gps_skytraq.checksum ^= c; + switch (gps_skytraq.status) { + case UNINIT: + if (c == SKYTRAQ_SYNC1) + gps_skytraq.status = GOT_SYNC1; + break; + case GOT_SYNC1: + if (c != SKYTRAQ_SYNC2) { + gps_skytraq.error_last = GPS_SKYTRAQ_ERR_OUT_OF_SYNC; + goto error; + } + gps_skytraq.status = GOT_SYNC2; + break; + case GOT_SYNC2: + gps_skytraq.len = c<<8; + gps_skytraq.status = GOT_LEN1; + break; + case GOT_LEN1: + gps_skytraq.len += c; + gps_skytraq.status = GOT_LEN2; + if (gps_skytraq.len > GPS_SKYTRAQ_MAX_PAYLOAD) { + gps_skytraq.error_last = GPS_SKYTRAQ_ERR_MSG_TOO_LONG; + goto error; + } + break; + case GOT_LEN2: + gps_skytraq.msg_id = c; + gps_skytraq.msg_idx = 0; + gps_skytraq.checksum = c; + gps_skytraq.status = GOT_ID; + break; + case GOT_ID: + gps_skytraq.msg_buf[gps_skytraq.msg_idx] = c; + gps_skytraq.msg_idx++; + if (gps_skytraq.msg_idx >= gps_skytraq.len-1) { + gps_skytraq.status = GOT_PAYLOAD; + } + break; + case GOT_PAYLOAD: + if (c != gps_skytraq.checksum) { + gps_skytraq.error_last = GPS_SKYTRAQ_ERR_CHECKSUM; + goto error; + } + gps_skytraq.status = GOT_CHECKSUM; + break; + case GOT_CHECKSUM: + if (c != SKYTRAQ_SYNC3) { + gps_skytraq.error_last = GPS_SKYTRAQ_ERR_OUT_OF_SYNC; + goto error; + } + gps_skytraq.status = GOT_SYNC3; + break; + case GOT_SYNC3: + gps_skytraq.msg_available = TRUE; + goto restart; + default: + gps_skytraq.error_last = GPS_SKYTRAQ_ERR_UNEXPECTED; + goto error; + } + return; + error: + gps_skytraq.error_cnt++; + restart: + gps_skytraq.status = UNINIT; + return; +} diff --git a/sw/airborne/subsystems/gps/gps_skytraq.h b/sw/airborne/subsystems/gps/gps_skytraq.h new file mode 100644 index 0000000000..fea8398af0 --- /dev/null +++ b/sw/airborne/subsystems/gps/gps_skytraq.h @@ -0,0 +1,115 @@ +/* + * Copyright (C) 2010 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 GPS_SKYTRAQ_H +#define GPS_SKYTRAQ_H + +#include "mcu_periph/uart.h" + +#define SKYTRAQ_SYNC1 0xA0 +#define SKYTRAQ_SYNC2 0xA1 + +#define SKYTRAQ_SYNC3 0x0D +#define SKYTRAQ_SYNC4 0x0A + + +#define SKYTRAQ_ID_NAVIGATION_DATA 0XA8 + +#define SKYTRAQ_NAVIGATION_DATA_FixMode(_payload) (uint8_t) (*((uint8_t*)_payload+2-2)) +#define SKYTRAQ_NAVIGATION_DATA_NumSV(_payload) (uint8_t) (*((uint8_t*)_payload+3-2)) + +//#define SKYTRAQ_NAVIGATION_DATA_TOW(_payload) (uint32_t)(_payload[7] + (((uint32_t)_payload[6])<<8) + (((uint32_t)_payload[5])<<16) + (((uint32_t)_payload[4])<<24)) +#define SKYTRAQ_NAVIGATION_DATA_TOW(_payload) __builtin_bswap32(*(uint32_t*)&_payload[ 6-2]) +#define SKYTRAQ_NAVIGATION_DATA_LAT(_payload) __builtin_bswap32(*( int32_t*)&_payload[10-2]) +#define SKYTRAQ_NAVIGATION_DATA_LON(_payload) __builtin_bswap32(*( int32_t*)&_payload[14-2]) +#define SKYTRAQ_NAVIGATION_DATA_AEL(_payload) __builtin_bswap32(*(uint32_t*)&_payload[18-2]) +#define SKYTRAQ_NAVIGATION_DATA_ASL(_payload) __builtin_bswap32(*(uint32_t*)&_payload[22-2]) +//#define SKYTRAQ_NAVIGATION_DATA_GDOP(_payload) __builtin_bswap16(*(uint16_t*)&_payload[26-2]) +//#define SKYTRAQ_NAVIGATION_DATA_PDOP(_payload) __builtin_bswap(*(uint16_t*)&_payload[28-2]) + +#define SKYTRAQ_NAVIGATION_DATA_ECEFX(_payload) __builtin_bswap32(*( int32_t*)&_payload[36-2]) +#define SKYTRAQ_NAVIGATION_DATA_ECEFY(_payload) __builtin_bswap32(*( int32_t*)&_payload[40-2]) +#define SKYTRAQ_NAVIGATION_DATA_ECEFZ(_payload) __builtin_bswap32(*( int32_t*)&_payload[44-2]) +#define SKYTRAQ_NAVIGATION_DATA_ECEFVX(_payload) __builtin_bswap32(*( int32_t*)&_payload[48-2]) +#define SKYTRAQ_NAVIGATION_DATA_ECEFVY(_payload) __builtin_bswap32(*( int32_t*)&_payload[52-2]) +#define SKYTRAQ_NAVIGATION_DATA_ECEFVZ(_payload) __builtin_bswap32(*( int32_t*)&_payload[56-2]) + + + +/* last error type */ +#define GPS_SKYTRAQ_ERR_NONE 0 +#define GPS_SKYTRAQ_ERR_OVERRUN 1 +#define GPS_SKYTRAQ_ERR_MSG_TOO_LONG 2 +#define GPS_SKYTRAQ_ERR_CHECKSUM 3 +#define GPS_SKYTRAQ_ERR_OUT_OF_SYNC 4 +#define GPS_SKYTRAQ_ERR_UNEXPECTED 5 + +#define GPS_SKYTRAQ_MAX_PAYLOAD 255 +struct GpsSkytraq { + uint8_t msg_buf[GPS_SKYTRAQ_MAX_PAYLOAD] __attribute__ ((aligned)); + bool_t msg_available; + uint8_t msg_id; + + uint8_t status; + uint16_t len; + uint8_t msg_idx; + uint8_t checksum; + uint8_t error_cnt; + uint8_t error_last; +}; + +extern struct GpsSkytraq gps_skytraq; + + +/* + * This part is used by the autopilot to read data from a uart + */ +#define __GpsLink(dev, _x) dev##_x +#define _GpsLink(dev, _x) __GpsLink(dev, _x) +#define GpsLink(_x) _GpsLink(GPS_LINK, _x) + +#define GpsBuffer() GpsLink(ChAvailable()) + +#define GpsEvent(_sol_available_callback) { \ + if (GpsBuffer()) { \ + ReadGpsBuffer(); \ + } \ + if (gps_skytraq.msg_available) { \ + gps_skytraq_read_message(); \ + if (gps_skytraq.msg_id == SKYTRAQ_ID_NAVIGATION_DATA) { \ + if (gps.fix == GPS_FIX_3D) \ + gps.last_fix_time = cpu_time_sec; \ + _sol_available_callback(); \ + } \ + gps_skytraq.msg_available = FALSE; \ + } \ + } + +#define ReadGpsBuffer() { \ + while (GpsLink(ChAvailable())&&!gps_skytraq.msg_available) \ + gps_skytraq_parse(GpsLink(Getch())); \ + } + + +extern void gps_skytraq_read_message(void); +extern void gps_skytraq_parse(uint8_t c); + +#endif /* GPS_SKYTRAQ_H */ diff --git a/sw/airborne/subsystems/gps/gps_ubx.c b/sw/airborne/subsystems/gps/gps_ubx.c new file mode 100644 index 0000000000..6a8d3fd61c --- /dev/null +++ b/sw/airborne/subsystems/gps/gps_ubx.c @@ -0,0 +1,394 @@ +/* + * Copyright (C) 2010 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 "subsystems/gps.h" + +#include "led.h" + +#ifdef GPS_USE_LATLONG +#include "subsystems/nav.h" +#include "math/pprz_geodetic_float.h" +#endif + +/* parser status */ +#define UNINIT 0 +#define GOT_SYNC1 1 +#define GOT_SYNC2 2 +#define GOT_CLASS 3 +#define GOT_ID 4 +#define GOT_LEN1 5 +#define GOT_LEN2 6 +#define GOT_PAYLOAD 7 +#define GOT_CHECKSUM1 8 + +/* last error type */ +#define GPS_UBX_ERR_NONE 0 +#define GPS_UBX_ERR_OVERRUN 1 +#define GPS_UBX_ERR_MSG_TOO_LONG 2 +#define GPS_UBX_ERR_CHECKSUM 3 +#define GPS_UBX_ERR_UNEXPECTED 4 +#define GPS_UBX_ERR_OUT_OF_SYNC 5 + +#define UTM_HEM_NORTH 0 +#define UTM_HEM_SOUTH 1 + + +#define GpsUartSend1(c) GpsLink(Transmit(c)) +#define GpsUartInitParam(_a,_b,_c) GpsLink(InitParam(_a,_b,_c)) +#define GpsUartRunning GpsLink(TxRunning) +#define GpsUartSendMessage GpsLink(SendMessage) + +#define UbxInitCheksum() { gps_ubx.send_ck_a = gps_ubx.send_ck_b = 0; } +#define UpdateChecksum(c) { gps_ubx.send_ck_a += c; gps_ubx.send_ck_b += gps_ubx.send_ck_a; } +#define UbxTrailer() { GpsUartSend1(gps_ubx.send_ck_a); GpsUartSend1(gps_ubx.send_ck_b); GpsUartSendMessage(); } + +#define UbxSend1(c) { uint8_t i8=c; GpsUartSend1(i8); UpdateChecksum(i8); } +#define UbxSend2(c) { uint16_t i16=c; UbxSend1(i16&0xff); UbxSend1(i16 >> 8); } +#define UbxSend1ByAddr(x) { UbxSend1(*x); } +#define UbxSend2ByAddr(x) { UbxSend1(*x); UbxSend1(*(x+1)); } +#define UbxSend4ByAddr(x) { UbxSend1(*x); UbxSend1(*(x+1)); UbxSend1(*(x+2)); UbxSend1(*(x+3)); } + +#define UbxHeader(nav_id, msg_id, len) { \ + GpsUartSend1(UBX_SYNC1); \ + GpsUartSend1(UBX_SYNC2); \ + UbxInitCheksum(); \ + UbxSend1(nav_id); \ + UbxSend1(msg_id); \ + UbxSend2(len); \ + } + + +struct GpsUbx gps_ubx; + +#ifdef GPS_CONFIGURE +bool_t gps_configuring; +static uint8_t gps_status_config; +#endif + +void gps_impl_init(void) { + gps_ubx.status = UNINIT; + gps_ubx.msg_available = FALSE; + gps_ubx.error_cnt = 0; + gps_ubx.error_last = GPS_UBX_ERR_NONE; +#ifdef GPS_CONFIGURE + gps_status_config = 0; + gps_configuring = TRUE; +#endif +} + + +void gps_ubx_read_message(void) { + + if (gps_ubx.msg_class == UBX_NAV_ID) { + if (gps_ubx.msg_id == UBX_NAV_SOL_ID) { +#ifdef GPS_TIMESTAMP + /* get hardware clock ticks */ + SysTimeTimerStart(gps.t0); + gps.t0_tow = UBX_NAV_SOL_ITOW(gps_ubx.msg_buf); + gps.t0_tow_frac = UBX_NAV_SOL_Frac(gps_ubx.msg_buf); +#endif + gps.tow = UBX_NAV_SOL_ITOW(gps_ubx.msg_buf); + gps.week = UBX_NAV_SOL_week(gps_ubx.msg_buf); + gps.fix = UBX_NAV_SOL_GPSfix(gps_ubx.msg_buf); + gps.ecef_pos.x = UBX_NAV_SOL_ECEF_X(gps_ubx.msg_buf); + gps.ecef_pos.y = UBX_NAV_SOL_ECEF_Y(gps_ubx.msg_buf); + gps.ecef_pos.z = UBX_NAV_SOL_ECEF_Z(gps_ubx.msg_buf); + gps.pacc = UBX_NAV_SOL_Pacc(gps_ubx.msg_buf); + gps.ecef_vel.x = UBX_NAV_SOL_ECEFVX(gps_ubx.msg_buf); + gps.ecef_vel.y = UBX_NAV_SOL_ECEFVY(gps_ubx.msg_buf); + gps.ecef_vel.z = UBX_NAV_SOL_ECEFVZ(gps_ubx.msg_buf); + gps.sacc = UBX_NAV_SOL_Sacc(gps_ubx.msg_buf); + gps.pdop = UBX_NAV_SOL_PDOP(gps_ubx.msg_buf); + gps.num_sv = UBX_NAV_SOL_numSV(gps_ubx.msg_buf); +#ifdef GPS_LED + if (gps.fix == GPS_FIX_3D) { + LED_ON(GPS_LED); + } + else { + LED_TOGGLE(GPS_LED); + } +#endif + } else if (gps_ubx.msg_id == UBX_NAV_POSLLH_ID) { + gps.lla_pos.lat = RadOfDeg(UBX_NAV_POSLLH_LAT(gps_ubx.msg_buf)); + gps.lla_pos.lon = RadOfDeg(UBX_NAV_POSLLH_LON(gps_ubx.msg_buf)); + gps.lla_pos.alt = UBX_NAV_POSLLH_HEIGHT(gps_ubx.msg_buf); + gps.hmsl = UBX_NAV_POSLLH_HMSL(gps_ubx.msg_buf); +#ifdef GPS_USE_LATLONG + /* Computes from (lat, long) in the referenced UTM zone */ + struct LlaCoor_f lla_f; + lla_f.lat = ((float) gps.lla_pos.lat) / 1e7; + lla_f.lon = ((float) gps.lla_pos.lon) / 1e7; + struct UtmCoor_f utm_f; + utm_f.zone = nav_utm_zone0; + /* convert to utm */ + utm_of_lla_f(&utm_f, &lla_f); + /* copy results of utm conversion */ + gps.utm_pos.east = utm_f.east*100; + gps.utm_pos.north = utm_f.north*100; + gps.utm_pos.alt = utm_f.alt*1000; + gps.utm_pos.zone = nav_utm_zone0; +#else + } + else if (gps_ubx.msg_id == UBX_NAV_POSUTM_ID) { + gps.utm_pos.east = UBX_NAV_POSUTM_EAST(gps_ubx.msg_buf); + gps.utm_pos.north = UBX_NAV_POSUTM_NORTH(gps_ubx.msg_buf); + uint8_t hem = UBX_NAV_POSUTM_HEM(gps_ubx.msg_buf); + if (hem == UTM_HEM_SOUTH) + gps.utm_pos.north -= 1000000000; /* Subtract false northing: -10000km */ + gps.utm_pos.alt = UBX_NAV_POSUTM_ALT(gps_ubx.msg_buf)*10; + gps.hmsl = gps.utm_pos.alt; + gps.lla_pos.alt = gps.utm_pos.alt; // FIXME: with UTM only you do not receive ellipsoid altitude + gps.utm_pos.zone = UBX_NAV_POSUTM_ZONE(gps_ubx.msg_buf); +#endif + } + else if (gps_ubx.msg_id == UBX_NAV_VELNED_ID) { + gps.speed_3d = UBX_NAV_VELNED_Speed(gps_ubx.msg_buf); + gps.gspeed = UBX_NAV_VELNED_GSpeed(gps_ubx.msg_buf); + gps.ned_vel.x = UBX_NAV_VELNED_VEL_N(gps_ubx.msg_buf); + gps.ned_vel.y = UBX_NAV_VELNED_VEL_E(gps_ubx.msg_buf); + gps.ned_vel.z = UBX_NAV_VELNED_VEL_D(gps_ubx.msg_buf); + // Ublox gives I4 heading in 1e-5 degrees, apparenty from 0 to 360 degrees (not -180 to 180) + // I4 max = 2^31 = 214 * 1e5 * 100 < 360 * 1e7: overflow on angles over 214 deg -> casted to -214 deg + // solution: First to radians, and then scale to 1e-7 radians + // First x 10 for loosing less resolution, then to radians, then multiply x 10 again + gps.course = (RadOfDeg(UBX_NAV_VELNED_Heading(gps_ubx.msg_buf)*10)) * 10; + gps.tow = UBX_NAV_VELNED_ITOW(gps_ubx.msg_buf); + } + else if (gps_ubx.msg_id == UBX_NAV_SVINFO_ID) { + gps.nb_channels = Min(UBX_NAV_SVINFO_NCH(gps_ubx.msg_buf), GPS_NB_CHANNELS); + uint8_t i; + for(i = 0; i < gps.nb_channels; i++) { + gps.svinfos[i].svid = UBX_NAV_SVINFO_SVID(gps_ubx.msg_buf, i); + gps.svinfos[i].flags = UBX_NAV_SVINFO_Flags(gps_ubx.msg_buf, i); + gps.svinfos[i].qi = UBX_NAV_SVINFO_QI(gps_ubx.msg_buf, i); + gps.svinfos[i].cno = UBX_NAV_SVINFO_CNO(gps_ubx.msg_buf, i); + gps.svinfos[i].elev = UBX_NAV_SVINFO_Elev(gps_ubx.msg_buf, i); + gps.svinfos[i].azim = UBX_NAV_SVINFO_Azim(gps_ubx.msg_buf, i); + } + } + else if (gps_ubx.msg_id == UBX_NAV_STATUS_ID) { + gps.fix = UBX_NAV_STATUS_GPSfix(gps_ubx.msg_buf); + gps_ubx.status_flags = UBX_NAV_STATUS_Flags(gps_ubx.msg_buf); + gps_ubx.sol_flags = UBX_NAV_SOL_Flags(gps_ubx.msg_buf); + } + } +} + + +/* UBX parsing */ +void gps_ubx_parse( uint8_t c ) { + if (gps_ubx.status < GOT_PAYLOAD) { + gps_ubx.ck_a += c; + gps_ubx.ck_b += gps_ubx.ck_a; + } + switch (gps_ubx.status) { + case UNINIT: + if (c == UBX_SYNC1) + gps_ubx.status++; + break; + case GOT_SYNC1: + if (c != UBX_SYNC2) { + gps_ubx.error_last = GPS_UBX_ERR_OUT_OF_SYNC; + goto error; + } + gps_ubx.ck_a = 0; + gps_ubx.ck_b = 0; + gps_ubx.status++; + break; + case GOT_SYNC2: + if (gps_ubx.msg_available) { + /* Previous message has not yet been parsed: discard this one */ + gps_ubx.error_last = GPS_UBX_ERR_OVERRUN; + goto error; + } + gps_ubx.msg_class = c; + gps_ubx.status++; + break; + case GOT_CLASS: + gps_ubx.msg_id = c; + gps_ubx.status++; + break; + case GOT_ID: + gps_ubx.len = c; + gps_ubx.status++; + break; + case GOT_LEN1: + gps_ubx.len |= (c<<8); + if (gps_ubx.len > GPS_UBX_MAX_PAYLOAD) { + gps_ubx.error_last = GPS_UBX_ERR_MSG_TOO_LONG; + goto error; + } + gps_ubx.msg_idx = 0; + gps_ubx.status++; + break; + case GOT_LEN2: + gps_ubx.msg_buf[gps_ubx.msg_idx] = c; + gps_ubx.msg_idx++; + if (gps_ubx.msg_idx >= gps_ubx.len) { + gps_ubx.status++; + } + break; + case GOT_PAYLOAD: + if (c != gps_ubx.ck_a) { + gps_ubx.error_last = GPS_UBX_ERR_CHECKSUM; + goto error; + } + gps_ubx.status++; + break; + case GOT_CHECKSUM1: + if (c != gps_ubx.ck_b) { + gps_ubx.error_last = GPS_UBX_ERR_CHECKSUM; + goto error; + } + gps_ubx.msg_available = TRUE; + goto restart; + break; + default: + gps_ubx.error_last = GPS_UBX_ERR_UNEXPECTED; + goto error; + } + return; + error: + gps_ubx.error_cnt++; + restart: + gps_ubx.status = UNINIT; + return; +} + + + +/* + * + * + * GPS dynamic configuration + * + * + */ +#ifdef GPS_CONFIGURE + +#define UBX_PROTO_MASK 0x0001 +#define NMEA_PROTO_MASK 0x0002 +#define RTCM_PROTO_MASK 0x0004 + +#define GPS_PORT_DDC 0x00 +#define GPS_PORT_UART1 0x01 +#define GPS_PORT_UART2 0x02 +#define GPS_PORT_USB 0x03 +#define GPS_PORT_SPI 0x04 + +#ifndef GPS_PORT_ID +#define GPS_PORT_ID GPS_PORT_UART1 +#endif + +#define __UBX_GPS_BAUD(_u) _u##_BAUD +#define _UBX_GPS_BAUD(_u) __UBX_GPS_BAUD(_u) +#define UBX_GPS_BAUD _UBX_GPS_BAUD(GPS_LINK) + +/* Configure the GPS baud rate using the current uart baud rate. Busy + wait for the end of the transmit. Then, BEFORE waiting for the ACK, + change the uart rate. */ +#if GPS_PORT_ID == GPS_PORT_UART1 || GPS_PORT_ID == GPS_PORT_UART2 +void gps_configure_uart(void) { +#ifdef FMS_PERIODIC_FREQ + UbxSend_CFG_PRT(GPS_PORT_ID, 0x0, 0x0, 0x000008D0, 38400, UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0, 0x0); + uint8_t loop=0; + while (GpsUartRunning) { + //doesn't work unless some printfs are used, so : + if (loop<9) { + printf("."); loop++; + } else { + printf("\b"); loop--; + } + } +#else + UbxSend_CFG_PRT(GPS_PORT_ID, 0x0, 0x0, 0x000008D0, UBX_GPS_BAUD, UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0, 0x0); + while (GpsUartRunning); /* FIXME */ +#endif + + GpsUartInitParam(UBX_GPS_BAUD, UART_8N1, UART_FIFO_8); +} +#endif + +#if GPS_PORT_ID == GPS_PORT_DDC +void gps_configure_uart(void) { + UbxSend_CFG_PRT(GPS_PORT_ID, 0x0, 0x0, GPS_I2C_SLAVE_ADDR, 0x0, UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0, 0x0); +} +#endif + +#define IGNORED 0 +#define RESERVED 0 + +#ifdef USER_GPS_CONFIGURE +#include USER_GPS_CONFIGURE +#else +static bool_t user_gps_configure(bool_t cpt) { + switch (cpt) { + case 0: + //New ublox firmware v5 or higher uses CFG_NAV5 message, CFG_NAV is no longer available + //UbxSend_CFG_NAV(NAV_DYN_AIRBORNE_2G, 3, 16, 24, 20, 5, 0, 0x3C, 0x3C, 0x14, 0x03E8 ,0x0000, 0x0, 0x17, 0x00FA, 0x00FA, 0x0064, 0x012C, 0x000F, 0x00, 0x00); + UbxSend_CFG_NAV5(0x05, NAV5_DYN_AIRBORNE_2G, NAV5_3D_ONLY, IGNORED, IGNORED, IGNORED, IGNORED, IGNORED, IGNORED, IGNORED, IGNORED, IGNORED, RESERVED, RESERVED, RESERVED, RESERVED); + break; + case 1: + UbxSend_CFG_MSG(UBX_NAV_ID, UBX_NAV_POSUTM_ID, 0, 1, 0, 0); + break; + case 2: + UbxSend_CFG_MSG(UBX_NAV_ID, UBX_NAV_VELNED_ID, 0, 1, 0, 0); + break; + case 3: + UbxSend_CFG_MSG(UBX_NAV_ID, UBX_NAV_STATUS_ID, 0, 1, 0, 0); + break; + case 4: + UbxSend_CFG_MSG(UBX_NAV_ID, UBX_NAV_SVINFO_ID, 0, 4, 0, 0); + break; + case 5: + UbxSend_CFG_MSG(UBX_NAV_ID, UBX_NAV_SOL_ID, 0, 8, 0, 0); + break; + case 6: + UbxSend_CFG_SBAS(0x00, 0x00, 0x00, 0x00, 0x00); + break; + case 7: + UbxSend_CFG_RATE(0x00FA, 0x0001, 0x0000); + return FALSE; + } + return TRUE; /* Continue, except for the last case */ +} +#endif // ! USER_GPS_CONFIGURE + +/* GPS configuration. Must be called on ack message reception while + gps_status_config < GPS_CONFIG_DONE */ +void gps_configure( void ) { + if (gps_ubx.msg_class == UBX_ACK_ID) { + if (gps_ubx.msg_id == UBX_ACK_ACK_ID) { + gps_status_config++; + } + } + gps_configuring = user_gps_configure(gps_status_config); +} +#endif /* GPS_CONFIGURE */ + +void ubxsend_cfg_rst(uint16_t bbr , uint8_t reset_mode) { +#ifdef GPS_LINK + UbxSend_CFG_RST(bbr, reset_mode, 0x00); +#endif /* else less harmful for HITL */ +} + + diff --git a/sw/airborne/subsystems/gps/gps_ubx.h b/sw/airborne/subsystems/gps/gps_ubx.h new file mode 100644 index 0000000000..e1f85d4d41 --- /dev/null +++ b/sw/airborne/subsystems/gps/gps_ubx.h @@ -0,0 +1,158 @@ +/* + * 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. + */ + +/** @file gps_ubx.h + * @brief UBX protocol specific code + * + */ + +#ifndef GPS_UBX_H +#define GPS_UBX_H + +#include "mcu_periph/uart.h" + +/** Includes macros generated from ubx.xml */ +#include "ubx_protocol.h" + +#define GPS_NB_CHANNELS 16 + +#define GPS_UBX_MAX_PAYLOAD 255 +struct GpsUbx { + bool_t msg_available; + uint8_t msg_buf[GPS_UBX_MAX_PAYLOAD] __attribute__ ((aligned)); + uint8_t msg_id; + uint8_t msg_class; + + uint8_t status; + uint16_t len; + uint8_t msg_idx; + uint8_t ck_a, ck_b; + uint8_t send_ck_a, send_ck_b; + uint8_t error_cnt; + uint8_t error_last; + + uint8_t status_flags; + uint8_t sol_flags; +}; + +extern struct GpsUbx gps_ubx; + + +/* + * This part is used by the autopilot to read data from a uart + */ +#define __GpsLink(dev, _x) dev##_x +#define _GpsLink(dev, _x) __GpsLink(dev, _x) +#define GpsLink(_x) _GpsLink(GPS_LINK, _x) + +#define GpsBuffer() GpsLink(ChAvailable()) + +#ifdef GPS_CONFIGURE +extern bool_t gps_configuring; +#define GpsParseOrConfigure() { \ + if (gps_configuring) \ + gps_configure(); \ + else \ + gps_ubx_read_message(); \ + } +#else +#define GpsParseOrConfigure() gps_ubx_read_message() +#endif + +#define GpsEvent(_sol_available_callback) { \ + if (GpsBuffer()) { \ + ReadGpsBuffer(); \ + } \ + if (gps_ubx.msg_available) { \ + GpsParseOrConfigure(); \ + if (gps_ubx.msg_class == UBX_NAV_ID && \ + gps_ubx.msg_id == UBX_NAV_VELNED_ID) { \ + if (gps.fix == GPS_FIX_3D) { \ + gps.last_fix_time = cpu_time_sec; \ + } \ + _sol_available_callback(); \ + } \ + gps_ubx.msg_available = FALSE; \ + } \ + } + +#define ReadGpsBuffer() { \ + while (GpsLink(ChAvailable())&&!gps_ubx.msg_available) \ + gps_ubx_parse(GpsLink(Getch())); \ + } + + +extern void gps_ubx_read_message(void); +extern void gps_ubx_parse(uint8_t c); + + + +/* + * GPS Reset + */ + +#define CFG_RST_Reset_Hardware 0x00 +#define CFG_RST_Reset_Controlled 0x01 +#define CFG_RST_Reset_Controlled_GPS_only 0x02 +#define CFG_RST_Reset_Controlled_GPS_stop 0x08 +#define CFG_RST_Reset_Controlled_GPS_start 0x09 + +extern void ubxsend_cfg_rst(uint16_t, uint8_t); + +#define gps_ubx_Reset(_val) { \ + gps_ubx.reset = _val; \ + if (gps_ubx.reset > CFG_RST_BBR_Warmstart) \ + gps_ubx.reset = CFG_RST_BBR_Coldstart; \ + ubxsend_cfg_rst(gps_ubx.reset, CFG_RST_Reset_Controlled); \ + } + + +/* + * dynamic GPS configuration + */ +#ifdef GPS_CONFIGURE +#define NAV_DYN_STATIONARY 1 +#define NAV_DYN_PEDESTRIAN 2 +#define NAV_DYN_AUTOMOTIVE 3 +#define NAV_DYN_SEA 4 +#define NAV_DYN_AIRBORNE_1G 5 +#define NAV_DYN_AIRBORNE_2G 6 +#define NAV_DYN_AIRBORNE_4G 7 + +#define NAV5_DYN_PORTABLE 0 +#define NAV5_DYN_FIXED 1 +#define NAV5_DYN_STATIONARY 2 +#define NAV5_DYN_PEDESTRIAN 3 +#define NAV5_DYN_AUTOMOTIVE 4 +#define NAV5_DYN_SEA 5 +#define NAV5_DYN_AIRBORNE_1G 6 +#define NAV5_DYN_AIRBORNE_2G 7 +#define NAV5_DYN_AIRBORNE_4G 8 + +#define NAV5_2D_ONLY 1 +#define NAV5_3D_ONLY 2 +#define NAV5_AUTO 3 + +extern void gps_configure(void); +extern void gps_configure_uart(void); +#endif + +#endif /* GPS_UBX_H */ diff --git a/sw/airborne/subsystems/imu/imu_aspirin.h b/sw/airborne/subsystems/imu/imu_aspirin.h index f9ac1d4f0a..60a54b49e3 100644 --- a/sw/airborne/subsystems/imu/imu_aspirin.h +++ b/sw/airborne/subsystems/imu/imu_aspirin.h @@ -77,7 +77,6 @@ extern struct ImuAspirin imu_aspirin; #define ASPIRIN_GYRO_TIMEOUT 3 #define ASPIRIN_ACCEL_TIMEOUT 3 -#include "peripherals/hmc5843.h" #define foo_handler() {} #define ImuMagEvent(_mag_handler) { \ MagEvent(foo_handler); \ diff --git a/sw/airborne/subsystems/imu/imu_crista.c b/sw/airborne/subsystems/imu/imu_crista.c index c40f60289c..a0e06875f4 100644 --- a/sw/airborne/subsystems/imu/imu_crista.c +++ b/sw/airborne/subsystems/imu/imu_crista.c @@ -35,6 +35,9 @@ void imu_impl_init(void) { #ifdef USE_AMI601 ami601_init(); #endif +#ifdef USE_HMC5843 + hmc5843_init(); +#endif } @@ -44,5 +47,7 @@ void imu_periodic(void) { #ifdef USE_AMI601 RunOnceEvery(10, { ami601_read(); }); #endif - +#ifdef USE_HMC5843 + hmc5843_periodic(); +#endif } diff --git a/sw/airborne/subsystems/imu/imu_crista.h b/sw/airborne/subsystems/imu/imu_crista.h index 6d7c1ab85a..6198079f23 100644 --- a/sw/airborne/subsystems/imu/imu_crista.h +++ b/sw/airborne/subsystems/imu/imu_crista.h @@ -60,6 +60,19 @@ extern volatile bool_t ADS8344_available; _mag_handler(); \ } \ } +#elif defined USE_HMC5843 +#include "peripherals/hmc5843.h" +#define foo_handler() {} +#define ImuMagEvent(_mag_handler) { \ + MagEvent(foo_handler); \ + if (hmc5843.data_available) { \ + imu.mag_unscaled.x = hmc5843.data.value[IMU_MAG_X_CHAN]; \ + imu.mag_unscaled.y = hmc5843.data.value[IMU_MAG_Y_CHAN]; \ + imu.mag_unscaled.z = hmc5843.data.value[IMU_MAG_Z_CHAN]; \ + _mag_handler(); \ + hmc5843.data_available = FALSE; \ + } \ + } #else #define ImuMagEvent(_mag_handler) {} #endif diff --git a/sw/airborne/subsystems/ins.c b/sw/airborne/subsystems/ins.c index 0e8e560273..d25b367a84 100644 --- a/sw/airborne/subsystems/ins.c +++ b/sw/airborne/subsystems/ins.c @@ -23,7 +23,7 @@ #include "subsystems/imu.h" #include "subsystems/sensors/baro.h" -#include "booz_gps.h" +#include "subsystems/gps.h" #include "generated/airframe.h" #include "math/pprz_algebra_int.h" @@ -94,8 +94,8 @@ void ins_init() { struct LlaCoor_i llh; /* Height above the ellipsoid */ llh.lat = INT32_RAD_OF_DEG(NAV_LAT0); llh.lon = INT32_RAD_OF_DEG(NAV_LON0); - //llh.alt = NAV_ALT0 - ins_ltp_def.hmsl + ins_ltp_def.lla.alt; - llh.alt = NAV_ALT0 + NAV_HMSL0; + /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */ + llh.alt = NAV_ALT0 + NAV_MSL0; struct EcefCoor_i nav_init; ecef_of_lla_i(&nav_init, &llh); @@ -209,15 +209,15 @@ void ins_update_baro() { void ins_update_gps(void) { #ifdef USE_GPS - if (booz_gps_state.fix == BOOZ2_GPS_FIX_3D) { + if (gps.fix == GPS_FIX_3D) { if (!ins_ltp_initialised) { - ltp_def_from_ecef_i(&ins_ltp_def, &booz_gps_state.ecef_pos); - ins_ltp_def.lla.alt = booz_gps_state.lla_pos.alt; - ins_ltp_def.hmsl = booz_gps_state.hmsl; + ltp_def_from_ecef_i(&ins_ltp_def, &gps.ecef_pos); + ins_ltp_def.lla.alt = gps.lla_pos.alt; + ins_ltp_def.hmsl = gps.hmsl; ins_ltp_initialised = TRUE; } - ned_of_ecef_point_i(&ins_gps_pos_cm_ned, &ins_ltp_def, &booz_gps_state.ecef_pos); - ned_of_ecef_vect_i(&ins_gps_speed_cm_s_ned, &ins_ltp_def, &booz_gps_state.ecef_vel); + ned_of_ecef_point_i(&ins_gps_pos_cm_ned, &ins_ltp_def, &gps.ecef_pos); + ned_of_ecef_vect_i(&ins_gps_speed_cm_s_ned, &ins_ltp_def, &gps.ecef_vel); #ifdef USE_HFF VECT2_ASSIGN(ins_gps_pos_m_ned, ins_gps_pos_cm_ned.x, ins_gps_pos_cm_ned.y); VECT2_SDIV(ins_gps_pos_m_ned, ins_gps_pos_m_ned, 100.); diff --git a/sw/airborne/subsystems/ins/hf_float.c b/sw/airborne/subsystems/ins/hf_float.c index 126a35c476..88e2c65857 100644 --- a/sw/airborne/subsystems/ins/hf_float.c +++ b/sw/airborne/subsystems/ins/hf_float.c @@ -26,7 +26,7 @@ #include "subsystems/ins.h" #include "subsystems/imu.h" #include "subsystems/ahrs.h" -#include "booz_gps.h" +#include "subsystems/gps.h" #include #include "generated/airframe.h" @@ -488,11 +488,11 @@ void b2_hff_update_gps(void) { b2_hff_lost_counter = 0; #ifdef USE_GPS_ACC4R - Rgps_pos = (float) booz_gps_state.pacc / 100.; + Rgps_pos = (float) gps.pacc / 100.; if (Rgps_pos < HFF_R_POS_MIN) Rgps_pos = HFF_R_POS_MIN; - Rgps_vel = (float) booz_gps_state.sacc / 100.; + Rgps_vel = (float) gps.sacc / 100.; if (Rgps_vel < HFF_R_SPEED_MIN) Rgps_vel = HFF_R_SPEED_MIN; #endif diff --git a/sw/airborne/subsystems/nav.c b/sw/airborne/subsystems/nav.c index 7e644c2478..12aadf70d6 100644 --- a/sw/airborne/subsystems/nav.c +++ b/sw/airborne/subsystems/nav.c @@ -31,14 +31,13 @@ #include #include "subsystems/nav.h" -#include "gps.h" +#include "subsystems/gps.h" #include "estimator.h" #include "firmwares/fixedwing/stabilization/stabilization_attitude.h" #include "firmwares/fixedwing/guidance/guidance_v.h" #include "firmwares/fixedwing/autopilot.h" #include "inter_mcu.h" #include "subsystems/navigation/traffic_info.h" -#include "latlong.h" #define RCLost() bit_is_set(fbw_state->status, RADIO_REALLY_LOST) diff --git a/sw/airborne/subsystems/nav.h b/sw/airborne/subsystems/nav.h index 1594b9f81f..9688560c8c 100644 --- a/sw/airborne/subsystems/nav.h +++ b/sw/airborne/subsystems/nav.h @@ -36,6 +36,7 @@ #include "paparazzi.h" #include "firmwares/fixedwing/guidance/guidance_v.h" #include "subsystems/navigation/nav_survey_rectangle.h" +#include "subsystems/navigation/common_flight_plan.h" #include "subsystems/navigation/common_nav.h" #define G 9.806 diff --git a/sw/airborne/subsystems/navigation/OSAMNav.c b/sw/airborne/subsystems/navigation/OSAMNav.c index efd1c72099..0fa98b71be 100644 --- a/sw/airborne/subsystems/navigation/OSAMNav.c +++ b/sw/airborne/subsystems/navigation/OSAMNav.c @@ -1,5 +1,10 @@ #include "subsystems/navigation/OSAMNav.h" +#include "subsystems/nav.h" +#include "estimator.h" +#include "autopilot.h" +#include "generated/flight_plan.h" + /************** Flower Navigation **********************************************/ /** Makes a flower pattern. @@ -37,18 +42,18 @@ bool_t InitializeFlower(uint8_t CenterWP, uint8_t EdgeWP) Center = CenterWP; Edge = EdgeWP; - EdgeCurrentX = waypoints[Edge].x - waypoints[Center].x; - EdgeCurrentY = waypoints[Edge].y - waypoints[Center].y; + EdgeCurrentX = WaypointX(Edge) - WaypointX(Center); + EdgeCurrentY = WaypointY(Edge) - WaypointY(Center); Flowerradius = sqrt(EdgeCurrentX*EdgeCurrentX+EdgeCurrentY*EdgeCurrentY); - TransCurrentX = estimator_x - waypoints[Center].x; - TransCurrentY = estimator_y - waypoints[Center].y; + TransCurrentX = estimator_x - WaypointX(Center); + TransCurrentY = estimator_y - WaypointY(Center); DistanceFromCenter = sqrt(TransCurrentX*TransCurrentX+TransCurrentY*TransCurrentY); FlowerTheta = atan2(TransCurrentY,TransCurrentX); - Fly2X = Flowerradius*cos(FlowerTheta+3.14)+waypoints[Center].x; - Fly2Y = Flowerradius*sin(FlowerTheta+3.14)+waypoints[Center].y; + Fly2X = Flowerradius*cos(FlowerTheta+3.14)+WaypointX(Center); + Fly2Y = Flowerradius*sin(FlowerTheta+3.14)+WaypointY(Center); FlyFromX = estimator_x; FlyFromY = estimator_y; @@ -64,8 +69,8 @@ bool_t InitializeFlower(uint8_t CenterWP, uint8_t EdgeWP) bool_t FlowerNav(void) { - TransCurrentX = estimator_x - waypoints[Center].x; - TransCurrentY = estimator_y - waypoints[Center].y; + TransCurrentX = estimator_x - WaypointX(Center); + TransCurrentY = estimator_y - WaypointY(Center); DistanceFromCenter = sqrt(TransCurrentX*TransCurrentX+TransCurrentY*TransCurrentY); bool_t InCircle = TRUE; @@ -85,8 +90,8 @@ bool_t FlowerNav(void) { CFlowerStatus = FlowerLine; FlowerTheta = atan2(TransCurrentY,TransCurrentX); - Fly2X = Flowerradius*cos(FlowerTheta+3.14)+waypoints[Center].x; - Fly2Y = Flowerradius*sin(FlowerTheta+3.14)+waypoints[Center].y; + Fly2X = Flowerradius*cos(FlowerTheta+3.14)+WaypointX(Center); + Fly2Y = Flowerradius*sin(FlowerTheta+3.14)+WaypointY(Center); FlyFromX = estimator_x; FlyFromY = estimator_y; nav_init_stage(); @@ -98,8 +103,8 @@ bool_t FlowerNav(void) { CFlowerStatus = Circle; CircleTheta = nav_radius/Flowerradius; - CircleX = Flowerradius*cos(FlowerTheta+3.14-CircleTheta)+waypoints[Center].x; - CircleY = Flowerradius*sin(FlowerTheta+3.14-CircleTheta)+waypoints[Center].y; + CircleX = Flowerradius*cos(FlowerTheta+3.14-CircleTheta)+WaypointX(Center); + CircleY = Flowerradius*sin(FlowerTheta+3.14-CircleTheta)+WaypointY(Center); nav_init_stage(); } break; @@ -107,16 +112,16 @@ bool_t FlowerNav(void) nav_circle_XY(CircleX, CircleY, nav_radius); if(InCircle) { - EdgeCurrentX = waypoints[Edge].x - waypoints[Center].x; - EdgeCurrentY = waypoints[Edge].y - waypoints[Center].y; + EdgeCurrentX = WaypointX(Edge) - WaypointX(Center); + EdgeCurrentY = WaypointY(Edge) - WaypointY(Center); Flowerradius = sqrt(EdgeCurrentX*EdgeCurrentX+EdgeCurrentY*EdgeCurrentY); if(DistanceFromCenter > Flowerradius) CFlowerStatus = Outside; else CFlowerStatus = FlowerLine; FlowerTheta = atan2(TransCurrentY,TransCurrentX); - Fly2X = Flowerradius*cos(FlowerTheta+3.14)+waypoints[Center].x; - Fly2Y = Flowerradius*sin(FlowerTheta+3.14)+waypoints[Center].y; + Fly2X = Flowerradius*cos(FlowerTheta+3.14)+WaypointX(Center); + Fly2Y = Flowerradius*sin(FlowerTheta+3.14)+WaypointY(Center); FlyFromX = estimator_x; FlyFromY = estimator_y; nav_init_stage(); @@ -182,8 +187,8 @@ bool_t InitializeBungeeTakeoff(uint8_t BungeeWP) TDistance = fabs(Takeoff_Distance); //Translate initial position so that the position of the bungee is (0,0) - float Currentx = initialx-(waypoints[BungeeWaypoint].x); - float Currenty = initialy-(waypoints[BungeeWaypoint].y); + float Currentx = initialx-(WaypointX(BungeeWaypoint)); + float Currenty = initialy-(WaypointY(BungeeWaypoint)); //Record bungee alt (which should be the ground alt at that point) BungeeAlt = waypoints[BungeeWaypoint].a; @@ -217,8 +222,8 @@ bool_t InitializeBungeeTakeoff(uint8_t BungeeWP) kill_throttle = 1; //Translate the throttle point back - throttlePx = throttlePx+(waypoints[BungeeWP].x); - throttlePy = throttlePy+(waypoints[BungeeWP].y); + throttlePx = throttlePx+(WaypointX(BungeeWP)); + throttlePy = throttlePy+(WaypointY(BungeeWP)); return FALSE; } @@ -248,8 +253,8 @@ bool_t BungeeTakeoff(void) initialy = estimator_y; //Translate initial position so that the position of the bungee is (0,0) - Currentx = initialx-(waypoints[BungeeWaypoint].x); - Currenty = initialy-(waypoints[BungeeWaypoint].y); + Currentx = initialx-(WaypointX(BungeeWaypoint)); + Currenty = initialy-(WaypointY(BungeeWaypoint)); //Find Launch line slope float MLaunch = Currenty/Currentx; @@ -276,8 +281,8 @@ bool_t BungeeTakeoff(void) AboveLine = FALSE; //Translate the throttle point back - throttlePx = throttlePx+(waypoints[BungeeWaypoint].x); - throttlePy = throttlePy+(waypoints[BungeeWaypoint].y); + throttlePx = throttlePx+(WaypointX(BungeeWaypoint)); + throttlePy = throttlePy+(WaypointY(BungeeWaypoint)); } //Find out if the UAV is currently above the line @@ -744,8 +749,8 @@ bool_t VerticalRaster(uint8_t l1, uint8_t l2, float radius, float AltSweep) { float alt = waypoints[l1].a; waypoints[l2].a = alt; - float l2_l1_x = waypoints[l1].x - waypoints[l2].x; - float l2_l1_y = waypoints[l1].y - waypoints[l2].y; + float l2_l1_x = WaypointX(l1) - WaypointX(l2); + float l2_l1_y = WaypointY(l1) - WaypointY(l2); float d = sqrt(l2_l1_x*l2_l1_x+l2_l1_y*l2_l1_y); /* Unit vector from l1 to l2 */ @@ -753,24 +758,24 @@ bool_t VerticalRaster(uint8_t l1, uint8_t l2, float radius, float AltSweep) { float u_y = l2_l1_y / d; /* The half circle centers and the other leg */ - struct point l2_c1 = { waypoints[l1].x + radius * u_y, - waypoints[l1].y + radius * -u_x, + struct point l2_c1 = { WaypointX(l1) + radius * u_y, + WaypointY(l1) + radius * -u_x, alt }; - struct point l2_c2 = { waypoints[l1].x + 1.732*radius * u_x, - waypoints[l1].y + 1.732*radius * u_y, + struct point l2_c2 = { WaypointX(l1) + 1.732*radius * u_x, + WaypointY(l1) + 1.732*radius * u_y, alt }; - struct point l2_c3 = { waypoints[l1].x + radius * -u_y, - waypoints[l1].y + radius * u_x, + struct point l2_c3 = { WaypointX(l1) + radius * -u_y, + WaypointY(l1) + radius * u_x, alt }; - struct point l1_c1 = { waypoints[l2].x + radius * -u_y, - waypoints[l2].y + radius * u_x, + struct point l1_c1 = { WaypointX(l2) + radius * -u_y, + WaypointY(l2) + radius * u_x, alt }; - struct point l1_c2 = { waypoints[l2].x +1.732*radius * -u_x, - waypoints[l2].y + 1.732*radius * -u_y, + struct point l1_c2 = { WaypointX(l2) +1.732*radius * -u_x, + WaypointY(l2) + 1.732*radius * -u_y, alt }; - struct point l1_c3 = { waypoints[l2].x + radius * u_y, - waypoints[l2].y + radius * -u_x, + struct point l1_c3 = { WaypointX(l2) + radius * u_y, + WaypointY(l2) + radius * -u_x, alt }; float qdr_out_2_1 = M_PI/3. - atan2(u_y, u_x); @@ -889,18 +894,18 @@ bool_t InitializeSkidLanding(uint8_t AFWP, uint8_t TDWP, float radius) FinalLandCount = 1; waypoints[AFWaypoint].a = waypoints[TDWaypoint].a + Landing_AFHeight; - float x_0 = waypoints[TDWaypoint].x - waypoints[AFWaypoint].x; - float y_0 = waypoints[TDWaypoint].y - waypoints[AFWaypoint].y; + float x_0 = WaypointX(TDWaypoint) - WaypointX(AFWaypoint); + float y_0 = WaypointY(TDWaypoint) - WaypointY(AFWaypoint); /* Unit vector from AF to TD */ float d = sqrt(x_0*x_0+y_0*y_0); float x_1 = x_0 / d; float y_1 = y_0 / d; - LandCircle.x = waypoints[AFWaypoint].x + y_1 * LandRadius; - LandCircle.y = waypoints[AFWaypoint].y - x_1 * LandRadius; + LandCircle.x = WaypointX(AFWaypoint) + y_1 * LandRadius; + LandCircle.y = WaypointY(AFWaypoint) - x_1 * LandRadius; - LandCircleQDR = atan2(waypoints[AFWaypoint].x-LandCircle.x, waypoints[AFWaypoint].y-LandCircle.y); + LandCircleQDR = atan2(WaypointX(AFWaypoint)-LandCircle.x, WaypointY(AFWaypoint)-LandCircle.y); if(LandRadius > 0) { @@ -970,7 +975,7 @@ bool_t SkidLanding(void) kill_throttle = 1; NavVerticalAutoThrottleMode(0); NavVerticalAltitudeMode(waypoints[TDWaypoint].a+FinalLandAltitude, 0); - nav_route_xy(waypoints[AFWaypoint].x,waypoints[AFWaypoint].y,waypoints[TDWaypoint].x,waypoints[TDWaypoint].y); + nav_route_xy(WaypointX(AFWaypoint),WaypointY(AFWaypoint),WaypointX(TDWaypoint),WaypointY(TDWaypoint)); if(stage_time >= Landing_FinalStageTime*FinalLandCount) { FinalLandAltitude = FinalLandAltitude/2; @@ -1004,15 +1009,15 @@ bool_t FlightLine(uint8_t From_WP, uint8_t To_WP, float radius, float Space_Befo case FLInitialize: //Translate WPs so From_WP is origin - V.x = waypoints[To_WP].x - waypoints[From_WP].x; - V.y = waypoints[To_WP].y - waypoints[From_WP].y; + V.x = WaypointX(To_WP) - WaypointX(From_WP); + V.y = WaypointY(To_WP) - WaypointY(From_WP); //Record Aircraft Position P.x = estimator_x; P.y = estimator_y; //Rotate Aircraft Position so V is aligned with x axis - TranslateAndRotateFromWorld(&P, atan2(V.y,V.x), waypoints[From_WP].x, waypoints[From_WP].y); + TranslateAndRotateFromWorld(&P, atan2(V.y,V.x), WaypointX(From_WP), WaypointY(From_WP)); //Find which side of the flight line the aircraft is on if(P.y > 0) @@ -1040,14 +1045,14 @@ bool_t FlightLine(uint8_t From_WP, uint8_t To_WP, float radius, float Space_Befo FLQDR = atan2(FLFROMWP.x-FLCircle.x, FLFROMWP.y-FLCircle.y); //Translate back - FLFROMWP.x = FLFROMWP.x + waypoints[From_WP].x; - FLFROMWP.y = FLFROMWP.y + waypoints[From_WP].y; + FLFROMWP.x = FLFROMWP.x + WaypointX(From_WP); + FLFROMWP.y = FLFROMWP.y + WaypointY(From_WP); - FLTOWP.x = FLTOWP.x + waypoints[From_WP].x; - FLTOWP.y = FLTOWP.y + waypoints[From_WP].y; + FLTOWP.x = FLTOWP.x + WaypointX(From_WP); + FLTOWP.y = FLTOWP.y + WaypointY(From_WP); - FLCircle.x = FLCircle.x + waypoints[From_WP].x; - FLCircle.y = FLCircle.y + waypoints[From_WP].y; + FLCircle.x = FLCircle.x + WaypointX(From_WP); + FLCircle.y = FLCircle.y + WaypointY(From_WP); CFLStatus = FLCircleS; nav_init_stage(); diff --git a/sw/airborne/subsystems/navigation/OSAMNav.h b/sw/airborne/subsystems/navigation/OSAMNav.h index e98813e858..4cb346a288 100644 --- a/sw/airborne/subsystems/navigation/OSAMNav.h +++ b/sw/airborne/subsystems/navigation/OSAMNav.h @@ -2,10 +2,7 @@ #define OSAMNav_H #include "std.h" -#include "subsystems/nav.h" -#include "estimator.h" -#include "autopilot.h" -#include "generated/flight_plan.h" + struct Point2D {float x; float y;}; struct Line {float m;float b;float x;}; diff --git a/sw/airborne/booz/booz2_commands.h b/sw/airborne/subsystems/navigation/common_flight_plan.c similarity index 54% rename from sw/airborne/booz/booz2_commands.h rename to sw/airborne/subsystems/navigation/common_flight_plan.c index 1681eb298b..25f33c990b 100644 --- a/sw/airborne/booz/booz2_commands.h +++ b/sw/airborne/subsystems/navigation/common_flight_plan.c @@ -1,6 +1,7 @@ -/* $Id$ +/* + * $Id$ * - * (c) 2009 Antoine Drouin + * Copyright (C) 2007-2009 ENAC, Pascal Brisset, Antoine Drouin * * This file is part of paparazzi. * @@ -21,20 +22,33 @@ * */ -#ifndef BOOZ2_COMMANDS_H -#define BOOZ2_COMMANDS_H +#include "subsystems/navigation/common_flight_plan.h" -#include "paparazzi.h" -#include "generated/airframe.h" +#include "generated/flight_plan.h" -extern int32_t booz2_commands[COMMANDS_NB]; -extern const int32_t booz2_commands_failsafe[COMMANDS_NB]; -#define SetCommands(_in_cmd, _in_flight, _motors_on) { \ - booz2_commands[COMMAND_PITCH] = _in_cmd[COMMAND_PITCH]; \ - booz2_commands[COMMAND_ROLL] = _in_cmd[COMMAND_ROLL]; \ - booz2_commands[COMMAND_YAW] = (_in_flight) ? _in_cmd[COMMAND_YAW] : 0; \ - booz2_commands[COMMAND_THRUST] = (_motors_on) ? _in_cmd[COMMAND_THRUST] : 0; \ +/** In s */ +uint16_t stage_time, block_time; + +uint8_t nav_stage, nav_block; + +/** To save the current block/stage to enable return */ +uint8_t last_block, last_stage; + + + +void nav_init_block(void) { + if (nav_block >= NB_BLOCK) + nav_block=NB_BLOCK-1; + nav_stage = 0; + block_time = 0; + InitStage(); +} + +void nav_goto_block(uint8_t b) { + if (b != nav_block) { /* To avoid a loop in a the current block */ + last_block = nav_block; + last_stage = nav_stage; } - -#endif /* BOOZ2_COMMANDS_H */ + GotoBlock(b); +} diff --git a/sw/airborne/subsystems/navigation/common_flight_plan.h b/sw/airborne/subsystems/navigation/common_flight_plan.h new file mode 100644 index 0000000000..b0394c7e45 --- /dev/null +++ b/sw/airborne/subsystems/navigation/common_flight_plan.h @@ -0,0 +1,64 @@ +/* + * Copyright (C) 2007-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. + * + */ + +#ifndef COMMON_FLIGHT_PLAN_H +#define COMMON_FLIGHT_PLAN_H + +#include "std.h" + +/** In s */ +extern uint16_t stage_time, block_time; + +extern uint8_t nav_stage, nav_block; +extern uint8_t last_block, last_stage; + + +void nav_init_stage(void); /* needs to be implemented by fixedwing and rotorcraft seperately */ + +void nav_init_block(void); +void nav_goto_block(uint8_t block_id); + +#define InitStage() nav_init_stage(); + +#define Block(x) case x: nav_block=x; +#define NextBlock() { nav_block++; nav_init_block(); } +#define GotoBlock(b) { nav_block=b; nav_init_block(); } + +#define Stage(s) case s: nav_stage=s; +#define NextStageAndBreak() { nav_stage++; InitStage(); break; } +#define NextStageAndBreakFrom(wp) { last_wp = wp; NextStageAndBreak(); } + +#define Label(x) label_ ## x: +#define Goto(x) { goto label_ ## x; } +#define Return() ({ nav_block=last_block; nav_stage=last_stage; block_time=0; FALSE;}) + +#define And(x, y) ((x) && (y)) +#define Or(x, y) ((x) || (y)) +#define Min(x,y) (x < y ? x : y) +#define Max(x,y) (x > y ? x : y) +#define LessThan(_x, _y) ((_x) < (_y)) +#define MoreThan(_x, _y) ((_x) > (_y)) + +/** Time in s since the entrance in the current block */ +#define NavBlockTime() (block_time) + +#endif /* COMMON_FLIGHT_PLAN_H */ diff --git a/sw/airborne/subsystems/navigation/common_nav.c b/sw/airborne/subsystems/navigation/common_nav.c index 8a6551fa5b..c2609a9a1c 100644 --- a/sw/airborne/subsystems/navigation/common_nav.c +++ b/sw/airborne/subsystems/navigation/common_nav.c @@ -25,7 +25,8 @@ #include "subsystems/navigation/common_nav.h" #include "estimator.h" #include "generated/flight_plan.h" -#include "gps.h" +#include "subsystems/gps.h" +#include "math/pprz_geodetic_float.h" float dist2_to_home; float dist2_to_wp; @@ -35,12 +36,6 @@ bool_t too_far_from_home; const uint8_t nb_waypoint = NB_WAYPOINT; struct point waypoints[NB_WAYPOINT] = WAYPOINTS; -uint8_t nav_stage, nav_block; -uint16_t stage_time, block_time; - -/** To save the current block/stage to enable return */ -uint8_t last_block, last_stage; - float ground_alt; int32_t nav_utm_east0 = NAV_UTM_EAST0; @@ -68,20 +63,25 @@ static float previous_ground_alt; unit_t nav_reset_reference( void ) { #ifdef GPS_USE_LATLONG /* Set the real UTM zone */ - nav_utm_zone0 = (gps_lon/10000000+180) / 6 + 1; + nav_utm_zone0 = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1; /* Recompute UTM coordinates in this zone */ - latlong_utm_of(RadOfDeg(gps_lat/1e7), RadOfDeg(gps_lon/1e7), nav_utm_zone0); - nav_utm_east0 = latlong_utm_x; - nav_utm_north0 = latlong_utm_y; + struct LlaCoor_f lla; + lla.lat = gps.lla_pos.lat/1e7; + lla.lon = gps.lla_pos.lon/1e7; + struct UtmCoor_f utm; + utm.zone = nav_utm_zone0; + utm_of_lla_f(&utm, &lla); + nav_utm_east0 = utm.east; + nav_utm_north0 = utm.north; #else - nav_utm_zone0 = gps_utm_zone; - nav_utm_east0 = gps_utm_east/100; - nav_utm_north0 = gps_utm_north/100; + nav_utm_zone0 = gps.utm_pos.zone; + nav_utm_east0 = gps.utm_pos.east/100; + nav_utm_north0 = gps.utm_pos.north/100; #endif previous_ground_alt = ground_alt; - ground_alt = gps_alt/100; + ground_alt = gps.hmsl/1000.; return 0; } @@ -94,22 +94,6 @@ unit_t nav_update_waypoints_alt( void ) { return 0; } -void nav_init_block(void) { - if (nav_block >= NB_BLOCK) - nav_block=NB_BLOCK-1; - nav_stage = 0; - block_time = 0; - InitStage(); -} - -void nav_goto_block(uint8_t b) { - if (b != nav_block) { /* To avoid a loop in a the current block */ - last_block = nav_block; - last_stage = nav_stage; - } - GotoBlock(b); -} - void common_nav_periodic_task_4Hz() { RunOnceEvery(4, { stage_time++; block_time++; }); } diff --git a/sw/airborne/subsystems/navigation/common_nav.h b/sw/airborne/subsystems/navigation/common_nav.h index 9c9dbcc526..d97e5c27bb 100644 --- a/sw/airborne/subsystems/navigation/common_nav.h +++ b/sw/airborne/subsystems/navigation/common_nav.h @@ -26,6 +26,7 @@ #define COMMON_NAV_H #include "std.h" +#include "subsystems/navigation/common_flight_plan.h" extern float max_dist_from_home; extern float dist2_to_home; @@ -49,13 +50,6 @@ extern const uint8_t nb_waypoint; extern struct point waypoints[]; /** size == nb_waypoint, waypoint 0 is a dummy waypoint */ -/** In s */ -extern uint16_t stage_time, block_time; - -extern uint8_t nav_stage, nav_block; -extern uint8_t last_block, last_stage; - - extern float ground_alt; /* m */ extern int32_t nav_utm_east0; /* m */ @@ -63,38 +57,12 @@ extern int32_t nav_utm_north0; /* m */ extern uint8_t nav_utm_zone0; -void nav_init_stage( void ); -void nav_init_block(void); -void nav_goto_block(uint8_t block_id); void compute_dist2_to_home(void); unit_t nav_reset_reference( void ) __attribute__ ((unused)); unit_t nav_update_waypoints_alt( void ) __attribute__ ((unused)); void common_nav_periodic_task_4Hz(void); -#define InitStage() nav_init_stage(); - -#define Block(x) case x: nav_block=x; -#define NextBlock() { nav_block++; nav_init_block(); } -#define GotoBlock(b) { nav_block=b; nav_init_block(); } - -#define Stage(s) case s: nav_stage=s; -#define NextStageAndBreak() { nav_stage++; InitStage(); break; } -#define NextStageAndBreakFrom(wp) { last_wp = wp; NextStageAndBreak(); } - -#define Label(x) label_ ## x: -#define Goto(x) { goto label_ ## x; } -#define Return() ({ nav_block=last_block; nav_stage=last_stage; block_time=0; FALSE;}) - -#define And(x, y) ((x) && (y)) -#define Or(x, y) ((x) || (y)) -#define Min(x,y) (x < y ? x : y) -#define Max(x,y) (x > y ? x : y) -#define LessThan(_x, _y) ((_x) < (_y)) - -/** Time in s since the entrance in the current block */ -#define NavBlockTime() (block_time) - #define NavSetGroundReferenceHere() ({ nav_reset_reference(); nav_update_waypoints_alt(); FALSE; }) #define NavSetWaypointHere(_wp) ({ \ diff --git a/sw/airborne/subsystems/navigation/discsurvey.c b/sw/airborne/subsystems/navigation/discsurvey.c index 1bd817d43e..dc20e9a558 100644 --- a/sw/airborne/subsystems/navigation/discsurvey.c +++ b/sw/airborne/subsystems/navigation/discsurvey.c @@ -39,7 +39,7 @@ bool_t disc_survey( uint8_t center, float radius) { c1.x = estimator_x; c1.y = estimator_y; - float d = ScalarProduct(upwind_x, upwind_y, estimator_x-waypoints[center].x, estimator_y-waypoints[center].y); + float d = ScalarProduct(upwind_x, upwind_y, estimator_x-WaypointX(center), estimator_y-WaypointY(center)); if (d > radius) { status = DOWNWIND; } else { @@ -48,8 +48,8 @@ bool_t disc_survey( uint8_t center, float radius) { float crosswind_x = - upwind_y; float crosswind_y = upwind_x; - c2.x = waypoints[center].x+d*upwind_x-w*sign*crosswind_x; - c2.y = waypoints[center].y+d*upwind_y-w*sign*crosswind_y; + c2.x = WaypointX(center)+d*upwind_x-w*sign*crosswind_x; + c2.y = WaypointY(center)+d*upwind_y-w*sign*crosswind_y; status = SEGMENT; } @@ -58,8 +58,8 @@ bool_t disc_survey( uint8_t center, float radius) { break; case DOWNWIND: - c2.x = waypoints[center].x - upwind_x * radius; - c2.y = waypoints[center].y - upwind_y * radius; + c2.x = WaypointX(center) - upwind_x * radius; + c2.y = WaypointY(center) - upwind_y * radius; status = SEGMENT; /* No break; */ diff --git a/sw/airborne/subsystems/navigation/nav_cube.c b/sw/airborne/subsystems/navigation/nav_cube.c index 827d94bf99..3f34f49c96 100644 --- a/sw/airborne/subsystems/navigation/nav_cube.c +++ b/sw/airborne/subsystems/navigation/nav_cube.c @@ -84,12 +84,12 @@ bool_t nav_cube_init(uint8_t center, uint8_t tb, uint8_t te) { sin_alpha = sin(alpha); /* calculate lower left start begin/end x coord */ - start_bx = waypoints[center].x - (((cube_nline_x_t-1) * cube_grid_x)/2) + start_bx = WaypointX(center) - (((cube_nline_x_t-1) * cube_grid_x)/2) + cube_offs_x; start_ex = start_bx; /* calculate lower left start end point y coord */ - start_ey = waypoints[center].y - cube_offs_y; + start_ey = WaypointY(center) - cube_offs_y; /* calculate lower left start begin point y coord */ start_by = start_ey - cube_size_y; @@ -101,26 +101,26 @@ bool_t nav_cube_init(uint8_t center, uint8_t tb, uint8_t te) { /* reset all waypoints to the standby position */ for (j=0; j < MAX_LINES_X; j++) { - waypoints[tb+j].x = waypoints[center].x + STBY_OFFSET; - waypoints[tb+j].y = waypoints[center].y; - waypoints[te+j].x = waypoints[center].x + STBY_OFFSET; - waypoints[te+j].y = waypoints[center].y; + waypoints[tb+j].x = WaypointX(center) + STBY_OFFSET; + waypoints[tb+j].y = WaypointY(center); + waypoints[te+j].x = WaypointX(center) + STBY_OFFSET; + waypoints[te+j].y = WaypointY(center); } /* set used waypoints */ for (j=0; j < cube_nline_x; j++) { int i = cube_line_x_start+j; /* set waypoints and vectorize in regard to center */ - bx = (start_bx + i*cube_grid_x) - waypoints[center].x; - by = start_by - waypoints[center].y; - ex = (start_ex + i*cube_grid_x) - waypoints[center].x; - ey = start_ey - waypoints[center].y; + bx = (start_bx + i*cube_grid_x) - WaypointX(center); + by = start_by - WaypointY(center); + ex = (start_ex + i*cube_grid_x) - WaypointX(center); + ey = start_ey - WaypointY(center); /* rotate clockwise with alpha and un-vectorize*/ - waypoints[tb+j].x = bx * cos_alpha - by * sin_alpha + waypoints[center].x; - waypoints[tb+j].y = bx * sin_alpha + by * cos_alpha + waypoints[center].y; + waypoints[tb+j].x = bx * cos_alpha - by * sin_alpha + WaypointX(center); + waypoints[tb+j].y = bx * sin_alpha + by * cos_alpha + WaypointY(center); waypoints[tb+j].a = start_bz; - waypoints[te+j].x = ex * cos_alpha - ey * sin_alpha + waypoints[center].x; - waypoints[te+j].y = ex * sin_alpha + ey * cos_alpha + waypoints[center].y; + waypoints[te+j].x = ex * cos_alpha - ey * sin_alpha + WaypointX(center); + waypoints[te+j].y = ex * sin_alpha + ey * cos_alpha + WaypointY(center); waypoints[te+j].a = start_ez; } diff --git a/sw/airborne/subsystems/navigation/nav_line.c b/sw/airborne/subsystems/navigation/nav_line.c index 961d39a683..d80738ab59 100644 --- a/sw/airborne/subsystems/navigation/nav_line.c +++ b/sw/airborne/subsystems/navigation/nav_line.c @@ -45,8 +45,8 @@ bool_t nav_line(uint8_t l1, uint8_t l2, float radius) { float alt = waypoints[l1].a; waypoints[l2].a = alt; - float l2_l1_x = waypoints[l1].x - waypoints[l2].x; - float l2_l1_y = waypoints[l1].y - waypoints[l2].y; + float l2_l1_x = WaypointX(l1) - WaypointX(l2); + float l2_l1_y = WaypointY(l1) - WaypointY(l2); float d = sqrt(l2_l1_x*l2_l1_x+l2_l1_y*l2_l1_y); /* Unit vector from l1 to l2 */ @@ -54,24 +54,24 @@ bool_t nav_line(uint8_t l1, uint8_t l2, float radius) { float u_y = l2_l1_y / d; /* The half circle centers and the other leg */ - struct point l2_c1 = { waypoints[l1].x + radius * u_y, - waypoints[l1].y + radius * -u_x, + struct point l2_c1 = { WaypointX(l1) + radius * u_y, + WaypointY(l1) + radius * -u_x, alt }; - struct point l2_c2 = { waypoints[l1].x + 1.732*radius * u_x, - waypoints[l1].y + 1.732*radius * u_y, + struct point l2_c2 = { WaypointX(l1) + 1.732*radius * u_x, + WaypointY(l1) + 1.732*radius * u_y, alt }; - struct point l2_c3 = { waypoints[l1].x + radius * -u_y, - waypoints[l1].y + radius * u_x, + struct point l2_c3 = { WaypointX(l1) + radius * -u_y, + WaypointY(l1) + radius * u_x, alt }; - struct point l1_c1 = { waypoints[l2].x + radius * -u_y, - waypoints[l2].y + radius * u_x, + struct point l1_c1 = { WaypointX(l2) + radius * -u_y, + WaypointY(l2) + radius * u_x, alt }; - struct point l1_c2 = { waypoints[l2].x +1.732*radius * -u_x, - waypoints[l2].y + 1.732*radius * -u_y, + struct point l1_c2 = { WaypointX(l2) +1.732*radius * -u_x, + WaypointY(l2) + 1.732*radius * -u_y, alt }; - struct point l1_c3 = { waypoints[l2].x + radius * u_y, - waypoints[l2].y + radius * -u_x, + struct point l1_c3 = { WaypointX(l2) + radius * u_y, + WaypointY(l2) + radius * -u_x, alt }; float qdr_out_2_1 = M_PI/3. - atan2(u_y, u_x); diff --git a/sw/airborne/subsystems/navigation/nav_survey_rectangle.c b/sw/airborne/subsystems/navigation/nav_survey_rectangle.c index ef50181871..6779748ce7 100644 --- a/sw/airborne/subsystems/navigation/nav_survey_rectangle.c +++ b/sw/airborne/subsystems/navigation/nav_survey_rectangle.c @@ -22,10 +22,10 @@ static survey_orientation_t survey_orientation = NS; void nav_survey_rectangle_init(uint8_t wp1, uint8_t wp2, float grid, survey_orientation_t so) { - nav_survey_west = Min(waypoints[wp1].x, waypoints[wp2].x); - nav_survey_east = Max(waypoints[wp1].x, waypoints[wp2].x); - nav_survey_south = Min(waypoints[wp1].y, waypoints[wp2].y); - nav_survey_north = Max(waypoints[wp1].y, waypoints[wp2].y); + nav_survey_west = Min(WaypointX(wp1), WaypointX(wp2)); + nav_survey_east = Max(WaypointX(wp1), WaypointX(wp2)); + nav_survey_south = Min(WaypointY(wp1), WaypointY(wp2)); + nav_survey_north = Max(WaypointY(wp1), WaypointY(wp2)); survey_orientation = so; if (survey_orientation == NS) { @@ -58,10 +58,10 @@ void nav_survey_rectangle(uint8_t wp1, uint8_t wp2) { nav_survey_active = TRUE; - nav_survey_west = Min(waypoints[wp1].x, waypoints[wp2].x); - nav_survey_east = Max(waypoints[wp1].x, waypoints[wp2].x); - nav_survey_south = Min(waypoints[wp1].y, waypoints[wp2].y); - nav_survey_north = Max(waypoints[wp1].y, waypoints[wp2].y); + nav_survey_west = Min(WaypointX(wp1), WaypointX(wp2)); + nav_survey_east = Max(WaypointX(wp1), WaypointX(wp2)); + nav_survey_south = Min(WaypointY(wp1), WaypointY(wp2)); + nav_survey_north = Max(WaypointY(wp1), WaypointY(wp2)); /* Update the current segment from corners' coordinates*/ if (SurveyGoingNorth()) { diff --git a/sw/airborne/subsystems/navigation/snav.c b/sw/airborne/subsystems/navigation/snav.c index 1d08b189b7..b4a659a0e7 100644 --- a/sw/airborne/subsystems/navigation/snav.c +++ b/sw/airborne/subsystems/navigation/snav.c @@ -6,7 +6,7 @@ #include "subsystems/navigation/snav.h" #include "estimator.h" #include "subsystems/nav.h" -#include "gps.h" +#include "subsystems/gps.h" #define Sign(_x) ((_x) > 0 ? 1 : (-1)) #define Norm2Pi(x) ({ uint8_t _i=1; float _x = x; while (_i && _x < 0.) { _i++;_x += 2*M_PI; } while (_i && _x > 2*M_PI) { _i++; _x -= 2*M_PI; } _x; }) @@ -150,7 +150,7 @@ bool_t snav_on_time(float nominal_radius) { float current_qdr = M_PI_2 - atan2(estimator_y-wp_ca.y, estimator_x-wp_ca.x); float remaining_angle = Norm2Pi(Sign(a_radius)*(qdr_a - current_qdr)); - float remaining_time = snav_desired_tow - gps_itow / 1000.; + float remaining_time = snav_desired_tow - gps.tow / 1000.; /* Use the nominal airspeed if the estimated one is not realistic */ float airspeed = estimator_airspeed; diff --git a/sw/airborne/subsystems/navigation/spiral.c b/sw/airborne/subsystems/navigation/spiral.c index 9ab41fb2b1..219e778603 100644 --- a/sw/airborne/subsystems/navigation/spiral.c +++ b/sw/airborne/subsystems/navigation/spiral.c @@ -6,7 +6,7 @@ IMPORTANT: numer of segments has to be larger than 2! */ -#include "spiral.h" +#include "subsystems/navigation/spiral.h" #include "subsystems/nav.h" #include "estimator.h" @@ -37,7 +37,7 @@ static float SRad; static float IRad; static float Alphalimit; static float Segmente; -static float CamAngle; +// static float CamAngle; static float ZPoint; static float nav_radius_min; @@ -57,19 +57,19 @@ bool_t InitializeSpiral(uint8_t CenterWP, uint8_t EdgeWP, float StartRad, float if (SRad < nav_radius_min) SRad = nav_radius_min; IRad = IncRad; // multiplier for increasing the spiral - EdgeCurrentX = waypoints[Edge].x - waypoints[Center].x; - EdgeCurrentY = waypoints[Edge].y - waypoints[Center].y; + EdgeCurrentX = WaypointX(Edge) - WaypointX(Center); + EdgeCurrentY = WaypointY(Edge) - WaypointY(Center); Spiralradius = sqrt(EdgeCurrentX*EdgeCurrentX+EdgeCurrentY*EdgeCurrentY); - TransCurrentX = estimator_x - waypoints[Center].x; - TransCurrentY = estimator_y - waypoints[Center].y; + TransCurrentX = estimator_x - WaypointX(Center); + TransCurrentY = estimator_y - WaypointY(Center); TransCurrentZ = estimator_z - ZPoint; DistanceFromCenter = sqrt(TransCurrentX*TransCurrentX+TransCurrentY*TransCurrentY); // SpiralTheta = atan2(TransCurrentY,TransCurrentX); - // Fly2X = Spiralradius*cos(SpiralTheta+3.14)+waypoints[Center].x; - // Fly2Y = Spiralradius*sin(SpiralTheta+3.14)+waypoints[Center].y; + // Fly2X = Spiralradius*cos(SpiralTheta+3.14)+WaypointX(Center); + // Fly2Y = Spiralradius*sin(SpiralTheta+3.14)+WaypointY(Center); // Alphalimit denotes angle, where the radius will be increased Alphalimit = 2*3.14 / Segments; @@ -84,11 +84,13 @@ bool_t InitializeSpiral(uint8_t CenterWP, uint8_t EdgeWP, float StartRad, float bool_t SpiralNav(void) { - TransCurrentX = estimator_x - waypoints[Center].x; - TransCurrentY = estimator_y - waypoints[Center].y; + TransCurrentX = estimator_x - WaypointX(Center); + TransCurrentY = estimator_y - WaypointY(Center); DistanceFromCenter = sqrt(TransCurrentX*TransCurrentX+TransCurrentY*TransCurrentY); bool_t InCircle = TRUE; + float DistanceStartEstim; + float CircleAlpha; if(DistanceFromCenter > Spiralradius) InCircle = FALSE; @@ -97,9 +99,9 @@ bool_t SpiralNav(void) { case Outside: //flys until center of the helix is reached an start helix - nav_route_xy(FlyFromX,FlyFromY,waypoints[Center].x, waypoints[Center].y); + nav_route_xy(FlyFromX,FlyFromY,WaypointX(Center), WaypointY(Center)); // center reached? - if (nav_approaching_xy(waypoints[Center].x, waypoints[Center].y, FlyFromX, FlyFromY, 0)) { + if (nav_approaching_xy(WaypointX(Center), WaypointY(Center), FlyFromX, FlyFromY, 0)) { // nadir image //dc_shutter(); CSpiralStatus = StartCircle; @@ -109,7 +111,7 @@ bool_t SpiralNav(void) // Starts helix // storage of current coordinates // calculation needed, State switch to Circle - nav_circle_XY(waypoints[Center].x, waypoints[Center].y, SRad); + nav_circle_XY(WaypointX(Center), WaypointY(Center), SRad); if(DistanceFromCenter >= SRad){ LastCircleX = estimator_x; LastCircleY = estimator_y; @@ -119,16 +121,16 @@ bool_t SpiralNav(void) } break; case Circle: { - nav_circle_XY(waypoints[Center].x, waypoints[Center].y, SRad); + nav_circle_XY(WaypointX(Center), WaypointY(Center), SRad); // Trigonometrische Berechnung des bereits geflogenen Winkels alpha // equation: // alpha = 2 * asin ( |Starting position angular - current positon| / (2* SRad) // if alphamax already reached, increase radius. //DistanceStartEstim = |Starting position angular - current positon| - float DistanceStartEstim = sqrt (((LastCircleX-estimator_x)*(LastCircleX-estimator_x)) + DistanceStartEstim = sqrt (((LastCircleX-estimator_x)*(LastCircleX-estimator_x)) + ((LastCircleY-estimator_y)*(LastCircleY-estimator_y))); - float CircleAlpha = (2.0 * asin (DistanceStartEstim / (2 * SRad))); + CircleAlpha = (2.0 * asin (DistanceStartEstim / (2 * SRad))); if (CircleAlpha >= Alphalimit) { LastCircleX = estimator_x; LastCircleY = estimator_y; diff --git a/sw/airborne/test/test_actuators.c b/sw/airborne/test/test_actuators.c index ce99e5c72f..567e227433 100644 --- a/sw/airborne/test/test_actuators.c +++ b/sw/airborne/test/test_actuators.c @@ -27,7 +27,7 @@ #include "led.h" #include "mcu_periph/i2c.h" -#include "booz/booz2_commands.h" +#include "firmwares/rotorcraft/commands.h" #include "firmwares/rotorcraft/actuators.h" static inline void main_init( void ); @@ -56,10 +56,10 @@ static inline void main_init( void ) { static inline void main_periodic_task( void ) { - booz2_commands[COMMAND_ROLL]=0; - booz2_commands[COMMAND_PITCH]=0; - booz2_commands[COMMAND_YAW]=0; - booz2_commands[COMMAND_THRUST]=1; + commands[COMMAND_ROLL]=0; + commands[COMMAND_PITCH]=0; + commands[COMMAND_YAW]=0; + commands[COMMAND_THRUST]=1; actuators_set(TRUE); diff --git a/sw/airborne/test/test_geodetic.c b/sw/airborne/test/test_geodetic.c index 774d353d1f..e4c24a983e 100644 --- a/sw/airborne/test/test_geodetic.c +++ b/sw/airborne/test/test_geodetic.c @@ -29,12 +29,12 @@ static void test_enu_to_ecef_to_enu( void ); int main(int argc, char** argv) { - test_floats(); - test_doubles(); + //test_floats(); + //test_doubles(); test_lla_of_utm(); - // test_enu_of_ecef_int(); + //test_enu_of_ecef_int(); // test_ned_to_ecef_to_ned(); // test_enu_to_ecef_to_enu(); @@ -45,13 +45,23 @@ int main(int argc, char** argv) { static void test_lla_of_utm(void) { printf("\n--- lla of UTM double ---\n"); - struct UTMCoor_d u = { .east=348805.71, .north=4759354.89, .zone=31 }; - struct LlaCoor_d l; - struct LlaCoor_d l_ref = {.lat=0.749999999392454875, + struct UtmCoor_d utm_d = { .east=348805.71, .north=4759354.89, .zone=31 }; + struct LlaCoor_d lla_d; + struct LlaCoor_d l_ref_d = {.lat=0.749999999392454875, .lon=0.019999999054505127}; - lla_of_utm(&l, &u); + lla_of_utm_d(&lla_d, &utm_d); printf(" lat=%.16f lon=%.16f\nref_lat=%.16f ref_lon=%.16f\n", - l.lat, l.lon, l_ref.lat, l_ref.lon); + lla_d.lat, lla_d.lon, l_ref_d.lat, l_ref_d.lon); + + printf("\n--- lla of UTM float ---\n"); + + struct UtmCoor_f utm_f = { .east=348805.71, .north=4759354.89, .zone=31 }; + struct LlaCoor_f lla_f; + struct LlaCoor_f l_ref_f = {.lat=0.749999999392454875, + .lon=0.019999999054505127}; + lla_of_utm_f(&lla_f, &utm_f); + printf(" lat=%.16f lon=%.16f\nref_lat=%.16f ref_lon=%.16f\n", + lla_f.lat, lla_f.lon, l_ref_f.lat, l_ref_f.lon); } static void test_floats(void) { @@ -121,10 +131,10 @@ static void test_enu_of_ecef_int(void) { printf("ecef0 : (%d,%d,%d)\n", ref_coor_i.x, ref_coor_i.y, ref_coor_i.z); struct LtpDef_i ltp_def_i; ltp_def_from_ecef_i(<p_def_i, &ref_coor_i); - printf("lla0 : (%d %d %d) (%f,%f,%f)\n", ltp_def_i.lla.lat, ltp_def_i.lla.lon, ltp_def_i.lla.alt, + printf("lla0 : (%f deg, %f deg, %f m) (%f,%f,%f)\n", DegOfRad(ltp_def_f.lla.lat), DegOfRad(ltp_def_f.lla.lon), ltp_def_f.lla.alt, DegOfRad(RAD_OF_EM7RAD((double)ltp_def_i.lla.lat)), DegOfRad(RAD_OF_EM7RAD((double)ltp_def_i.lla.lon)), - M_OF_CM((double)ltp_def_i.lla.alt)); + M_OF_MM((double)ltp_def_i.lla.alt)); #define STEP 1000. #define RANGE 100000. diff --git a/sw/ground_segment/cockpit/live.ml b/sw/ground_segment/cockpit/live.ml index 1f33553984..e9f70f6463 100644 --- a/sw/ground_segment/cockpit/live.ml +++ b/sw/ground_segment/cockpit/live.ml @@ -691,11 +691,11 @@ let create_ac = fun alert (geomap:G.widget) (acs_notebook:GPack.notebook) (ac_id (** Connect the GPS reset button *) begin try - let gps_reset_id = settings_tab#assoc "gps_reset" in + let gps_reset_id = settings_tab#assoc "gps.reset" in gps_page#connect_reset (fun x -> dl_setting_callback gps_reset_id (float x)) with Not_found -> - prerr_endline "Warning: GPS reset not setable from GCS (i.e. 'gps_reset' not listed in the xml settings file)" + prerr_endline "Warning: GPS reset not setable from GCS (i.e. 'gps.reset' not listed in the xml settings file)" end | None -> () end; diff --git a/sw/ground_segment/tmtc/rotorcraft_server.ml b/sw/ground_segment/tmtc/rotorcraft_server.ml index 7217143aac..a43aa5db96 100644 --- a/sw/ground_segment/tmtc/rotorcraft_server.ml +++ b/sw/ground_segment/tmtc/rotorcraft_server.ml @@ -146,7 +146,7 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values -> if not (msg.Pprz.name = "DOWNLINK_STATUS") then a.last_msg_date <- U.gettimeofday (); match msg.Pprz.name with - "BOOZ2_FP" -> + "ROTORCRAFT_FP" -> begin match a.nav_ref with None -> (); (* No nav_ref yet *) | Some nav_ref -> @@ -191,8 +191,8 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values -> let x = foi32value "ecef_x0" /. 100. and y = foi32value "ecef_y0" /. 100. and z = foi32value "ecef_z0" /. 100. - and alt = foi32value "alt0" /. 100. - and hmsl = foi32value "hmsl0" /. 100. in + and alt = foi32value "alt0" /. 1000. + and hmsl = foi32value "hmsl0" /. 1000. in let nav_ref_ecef = LL.make_ecef [| x; y; z |] in a.nav_ref <- Some (Ltp nav_ref_ecef); a.d_hmsl <- hmsl -. alt; diff --git a/sw/in_progress/python/attitude_viz.py b/sw/in_progress/python/attitude_viz.py index 9710330a9d..72458cb197 100755 --- a/sw/in_progress/python/attitude_viz.py +++ b/sw/in_progress/python/attitude_viz.py @@ -2,8 +2,6 @@ # Tool for visualizing quaternion as rotated cube -import wx -import wx.glcanvas from OpenGL.GLUT import * from OpenGL.GLU import * from OpenGL.GL import * @@ -13,13 +11,13 @@ from ivy.std_api import * import logging import getopt +import pygame +import time + _NAME = 'attitude_viz' -DEFAULT_X = 800 -DEFAULT_Y = 600 - class TelemetryQuat: - def __init__(self, message_name, index, name): + def __init__(self, message_name, index, name, integer): self.message_name = message_name self.index = index self.name = name @@ -27,6 +25,11 @@ class TelemetryQuat: self.qx = 0 self.qy = 0 self.qz = 0 + # optional scaling for fixed point telemetry + if integer: + self.scale = 0.00003051757812 + else: + self.scale = 1.0 class TelemetryValue: def __init__(self, message_name, index, name, offset, scale, max): @@ -38,241 +41,198 @@ class TelemetryValue: self.max = max self.value = 0 -class MyGLCanvas(wx.glcanvas.GLCanvas): - def __init__(self, parent): - wx.glcanvas.GLCanvas.__init__(self, parent,-1) - self.Bind( wx.EVT_PAINT, self.OnPaint) - self.init = False - self.quats = [] - self.graph_values = [] - self.throttle = 0.0 - self.mode = 0.0 - self.airspeed = 0.0 - for message_name, index, name in VEHICLE_QUATS: - self.quats.append(TelemetryQuat(message_name, index, name)) - for message_name, index, name, offset, scale, max in BAR_VALUES: - self.graph_values.append(TelemetryValue(message_name, index, name, offset, scale, max)) +class Visualization: + def __init__(self, parent): + self.quats = [] + self.graph_values = [] + self.throttle = 0.0 + self.mode = 0.0 + self.airspeed = 0.0 + self.display_list = None + self.display_dirty = True + for message_name, index, name, bfp in VEHICLE_QUATS: + self.quats.append(TelemetryQuat(message_name, index, name, bfp)) + for message_name, index, name, offset, scale, max in BAR_VALUES: + self.graph_values.append(TelemetryValue(message_name, index, name, offset, scale, max)) + def onmsgproc(self, agent, *larg): + data = str(larg[0]).split(' ') + for telemetry_quat in self.quats: + if (telemetry_quat.message_name == data[1]): + self.display_dirty = True + telemetry_quat.qi = float(data[telemetry_quat.index + 0]) + telemetry_quat.qx = float(data[telemetry_quat.index + 1]) + telemetry_quat.qy = float(data[telemetry_quat.index + 2]) + telemetry_quat.qz = float(data[telemetry_quat.index + 3]) - def onmsgproc(self, agent, *larg): - data = str(larg[0]).split(' ') - for telemetry_quat in self.quats: - if (telemetry_quat.message_name == data[1]): - telemetry_quat.qi = float(data[telemetry_quat.index + 0]) - telemetry_quat.qx = float(data[telemetry_quat.index + 1]) - telemetry_quat.qy = float(data[telemetry_quat.index + 2]) - telemetry_quat.qz = float(data[telemetry_quat.index + 3]) - - for graph_value in self.graph_values: - if (graph_value.message_name == data[1]): - graph_value.value = (float(data[graph_value.index + 0]) + graph_value.offset) / graph_value.scale + for graph_value in self.graph_values: + if (graph_value.message_name == data[1]): + self.display_dirty = True + graph_value.value = (float(data[graph_value.index + 0]) + graph_value.offset) / graph_value.scale - def OnPaint(self,event): - if not self.init: - self.SetCurrent() - self.InitGL() + def DrawCircle(self, radius): + glBegin(GL_TRIANGLE_FAN) + glVertex3f(0, 0, 0) + for angle in range (0, 361, 12): + glVertex3f( math.sin(math.radians(angle)) * radius, math.cos(math.radians(angle)) * radius, 0) + glEnd() - try: - self.Draw() - except: - raise - finally: - self.SwapBuffers() - self.Refresh() - - def InitGL(self): - self.init = True - - glutInit() - glEnable(GL_LINE_SMOOTH) - glEnable(GL_DEPTH_TEST) - glEnable(GL_LIGHTING) - glEnable(GL_LIGHT0) - glEnable(GL_BLEND) - glShadeModel (GL_SMOOTH) - glClearColor(1.0, 1.0, 1.0, 1.0) - glClearDepth(1.0) - - glPointSize(3.0) + # draw quad centered at origin, z = 0 + def DrawQuad(self, width, height): + glBegin (GL_QUADS) + glVertex3f( width, height, 0) + glVertex3f( -width, height, 0) + glVertex3f( -width, -height, 0) + glVertex3f( width, -height, 0) + glEnd() - glMatrixMode(GL_PROJECTION) - glLoadIdentity() - gluPerspective(7.0, 1.0, 95.0, 105.0) + def DrawBox(self, width, height, depth): + glPushMatrix() + glTranslate(0, 0, depth) + self.DrawQuad(width, height) + glTranslate(0, 0, -2 * depth) + self.DrawQuad(width, height) + glPopMatrix() - glMatrixMode(GL_MODELVIEW) + glPushMatrix() + glRotate(90, 1, 0, 0) + glTranslate(0, 0, height) + self.DrawQuad(width, depth) + glTranslate(0, 0, -2 * height) + self.DrawQuad(width, depth) + glPopMatrix() - glLight(GL_LIGHT0, GL_POSITION, [5, 30, -20]) - glLight(GL_LIGHT0, GL_AMBIENT, [0.5, 0.5, 0.5]) - glLight(GL_LIGHT0, GL_SPECULAR, [0.0, 0.0, 0.0]) - glLight(GL_LIGHT0, GL_DIFFUSE, [0.8, 0.8, 0.8]) - glEnable(GL_COLOR_MATERIAL) - glColorMaterial(GL_FRONT, GL_AMBIENT_AND_DIFFUSE) + glPushMatrix() + glRotate(90, 0, 1, 0) + glTranslate(0, 0, width) + self.DrawQuad(depth, height) + glTranslate(0, 0, -2 * width) + self.DrawQuad(depth, height) + glPopMatrix() - def DrawCircle(self, radius): - glBegin(GL_TRIANGLE_FAN) - glVertex3f(0, 0, 0) - for angle in range (0, 361, 8): - glVertex3f( math.sin(math.radians(angle)) * radius, math.cos(math.radians(angle)) * radius, 0) - glEnd() - + def DrawVehicle(self, name): + wingspan = 2.7 + separation = 0.7 + chord = 0.35 + thickness = 0.08 + strutcount = 5 + discradius = 0.45 + discseparation = 0.01 + + #wings + glColor3f(0.1, 0.1, 0.9) + glPushMatrix() + glTranslate(0, 0, separation) + self.DrawBox(wingspan, chord, thickness) + glColor3f(0.0, 0.0, 0.0) + glTranslate(-wingspan, -0.2, thickness + 0.01) + glScale(0.004, 0.004, 0.004) + glutStrokeString(GLUT_STROKE_ROMAN, name) + glPopMatrix() - # draw quad centered at origin, z = 0 - def DrawQuad(self, width, height): - glBegin (GL_QUADS) - glVertex3f( width, height, 0) - glVertex3f( -width, height, 0) - glVertex3f( -width, -height, 0) - glVertex3f( width, -height, 0) - glEnd() + glPushMatrix() + glTranslate(0, 0, -separation) + glColor3f(0.6, 0.6, 0.2) + self.DrawBox(wingspan, chord, thickness) + glColor3f(0.0, 0.0, 0.0) + glTranslate(wingspan, -0.2, -0.01 - thickness) + glScale(0.004, 0.004, 0.004) + glRotate(180, 0, 1, 0) + glutStrokeString(GLUT_STROKE_ROMAN, name) + glPopMatrix() - def DrawBox(self, width, height, depth): + if self.display_list is None: + self.display_list = glGenLists(1) + glNewList(self.display_list, GL_COMPILE) + # struts + glColor3f(0.4, 0.4, 0.4) glPushMatrix() - glTranslate(0, 0, depth) - self.DrawQuad(width, height) - glTranslate(0, 0, -2 * depth) - self.DrawQuad(width, height) + glTranslate(-wingspan, 0, 0) + glRotate(90, 0, 1, 0) + for x in range (0, strutcount): + self.DrawBox(separation, chord - .01, thickness) + glTranslate(0, 0, 2 * wingspan/(strutcount - 1)) + glPopMatrix() + + #rotors + glColor3f(0.9, 0.1, 0.1) + glPushMatrix() + glRotate(90, 1, 0, 0) + glTranslate(-wingspan, separation, -(chord + .01)) + for x in range (0, strutcount): + if (x != strutcount/2): + self.DrawCircle(discradius) + glTranslate(2 * wingspan/(strutcount - 1), 0, 0) glPopMatrix() glPushMatrix() glRotate(90, 1, 0, 0) - glTranslate(0, 0, height) - self.DrawQuad(width, depth) - glTranslate(0, 0, -2 * height) - self.DrawQuad(width, depth) + glTranslate(-wingspan, -separation, -(chord + .01)) + for x in range (0, strutcount): + if (x != strutcount/2): + self.DrawCircle(discradius) + glTranslate(2 * wingspan/(strutcount - 1), 0, 0) glPopMatrix() + glEndList() + glCallList(self.display_list) + + def DrawBar(self, name, value): + bar_height = 0.12 + bar_length = 3 + glPushMatrix() + glColor3f(0, 0, 0) + glTranslate(-bar_length, -0.09, 0.02) + glScale(0.0015, 0.0015, 0.0015) + glutStrokeString(GLUT_STROKE_ROMAN, name) + glPopMatrix() + glColor3f(0.92, 0.92, 0.92) + glPushMatrix() + glTranslate(0, 0, 0) + self.DrawQuad(bar_length, bar_height) + glPopMatrix() + glPushMatrix() + glTranslate(bar_length * value - bar_length, 0, 0.01) + glColor3f(0.6, 0.6, 0.6) + self.DrawQuad(bar_length * value, bar_height) + glPopMatrix() + + def Draw(self): + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT) + + glPushMatrix() + + height = 5 + + glDisable(GL_LIGHTING) + glPushMatrix() + for graph_value in self.graph_values: + self.DrawBar(graph_value.name % (graph_value.value), graph_value.value / graph_value.max) + glTranslate(0, 0.35, 0) + glPopMatrix() + glEnable(GL_LIGHTING) + + glTranslate(0, -height + (height / len(self.quats) + 1), 0) + for telemetry_quat in self.quats: glPushMatrix() - glRotate(90, 0, 1, 0) - glTranslate(0, 0, width) - self.DrawQuad(depth, height) - glTranslate(0, 0, -2 * width) - self.DrawQuad(depth, height) - glPopMatrix() - - def DrawVehicle(self, name): - wingspan = 2.7 - separation = 0.7 - chord = 0.35 - thickness = 0.08 - strutcount = 5 - discradius = 0.45 - discseparation = 0.01 - - #wings - glColor3f(0.1, 0.1, 0.9) - glPushMatrix() - glTranslate(0, 0, separation) - self.DrawBox(wingspan, chord, thickness) - glColor3f(0.0, 0.0, 0.0) - glTranslate(-wingspan, -0.2, thickness + 0.01) - glScale(0.004, 0.004, 0.004) - glutStrokeString(GLUT_STROKE_ROMAN, name) - glPopMatrix() - - glPushMatrix() - glTranslate(0, 0, -separation) - glColor3f(0.6, 0.6, 0.2) - self.DrawBox(wingspan, chord, thickness) - glColor3f(0.0, 0.0, 0.0) - glTranslate(wingspan, -0.2, -0.01 - thickness) - glScale(0.004, 0.004, 0.004) - glRotate(180, 0, 1, 0) - glutStrokeString(GLUT_STROKE_ROMAN, name) - glPopMatrix() - - # struts - glColor3f(0.4, 0.4, 0.4) - glPushMatrix() - glTranslate(-wingspan, 0, 0) - glRotate(90, 0, 1, 0) - for x in range (0, strutcount): - self.DrawBox(separation, chord - .01, thickness) - glTranslate(0, 0, 2 * wingspan/(strutcount - 1)) - glPopMatrix() - - #rotors - glColor3f(0.9, 0.1, 0.1) - glPushMatrix() - glRotate(90, 1, 0, 0) - glTranslate(-wingspan, separation, -(chord + .01)) - for x in range (0, strutcount): - if (x != strutcount/2): - self.DrawCircle(discradius) - glTranslate(2 * wingspan/(strutcount - 1), 0, 0) - glPopMatrix() - - glPushMatrix() - glRotate(90, 1, 0, 0) - glTranslate(-wingspan, -separation, -(chord + .01)) - for x in range (0, strutcount): - if (x != strutcount/2): - self.DrawCircle(discradius) - glTranslate(2 * wingspan/(strutcount - 1), 0, 0) - glPopMatrix() - - def DrawBar(self, name, value): - bar_height = 0.12 - bar_length = 3 - glPushMatrix() - glColor3f(0, 0, 0) - glTranslate(-bar_length, -0.09, 0.02) - glScale(0.0015, 0.0015, 0.0015) - glutStrokeString(GLUT_STROKE_ROMAN, name) - glPopMatrix() - glColor3f(0.92, 0.92, 0.92) - glPushMatrix() - glTranslate(0, 0, 0) - self.DrawQuad(bar_length, bar_height) - glPopMatrix() - glPushMatrix() - glTranslate(bar_length * value - bar_length, 0, 0.01) - glColor3f(0.6, 0.6, 0.6) - self.DrawQuad(bar_length * value, bar_height) - glPopMatrix() - - def Draw(self): - glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT) - - glMatrixMode(GL_MODELVIEW) - glLoadIdentity() - gluLookAt(0.0, 0.0, 100.0, - 0.0, 0.0, 0.0, - 0.0, 1.0, 0.0) - - height = 5 - - glDisable(GL_LIGHTING) - glPushMatrix() - for graph_value in self.graph_values: - self.DrawBar(graph_value.name % (graph_value.value), graph_value.value / graph_value.max) - glTranslate(0, 0.35, 0) - glPopMatrix() - glEnable(GL_LIGHTING) - - glTranslate(0, -height + (height / len(self.quats) + 1), 0) - for telemetry_quat in self.quats: - glPushMatrix() - glRotate(360 * math.acos(telemetry_quat.qi) / math.pi, telemetry_quat.qy, -telemetry_quat.qz, -telemetry_quat.qx) - glRotate(-90, 1, 0, 0) - self.DrawVehicle(telemetry_quat.name) - glPopMatrix() + try: + telemetry_quat.qi = telemetry_quat.qi * telemetry_quat.scale + telemetry_quat.qx = telemetry_quat.qx * telemetry_quat.scale + telemetry_quat.qy = telemetry_quat.qy * telemetry_quat.scale + telemetry_quat.qz = telemetry_quat.qz * telemetry_quat.scale + glRotate(360 * math.acos(telemetry_quat.qi ) / math.pi, telemetry_quat.qy, -telemetry_quat.qz, -telemetry_quat.qx) + glRotate(-90, 1, 0, 0) + self.DrawVehicle(telemetry_quat.name) + except Exception: + pass + finally: + glPopMatrix() glTranslate(0, 2 * height / (len(self.quats)), 0) + glPopMatrix() -class MainWindow(wx.Frame): - """ simple wx.Frame derived class. """ - def __init__(self, parent, id, title): - wx.Frame.__init__(self, parent, wx.ID_ANY, title, size=(200,100), - style=wx.DEFAULT_FRAME_STYLE|wx.NO_FULL_REPAINT_ON_RESIZE) - - self.GenerateGui() - self.InitLayout() - - # bind painting events - self.Bind( wx.EVT_CLOSE, self.OnClose) - self.Bind( wx.EVT_PAINT, self.OnPaint) - - # display the window - self.Show( True) +class Visualizer: + def __init__(self): + self.visualization = Visualization(self) # listen to Ivy logging.getLogger('Ivy').setLevel(logging.WARN) @@ -297,46 +257,89 @@ class MainWindow(wx.Frame): # bind to set of messages (ie, only bind each message once) for message_name in set(messages): bind_string = "(^.*" + message_name + ".*$)" - IvyBindMsg(self.panel.onmsgproc, bind_string) - def OnPaint(self, event): - pass - - def GenerateGui( self): - # generate panel and edit box - self.size_x = DEFAULT_X - self.size_y = DEFAULT_X + IvyBindMsg(self.visualization.onmsgproc, bind_string) - self.panel = MyGLCanvas(self) - self.panel.SetClientSize( (self.size_x, self.size_y)) - def InitLayout( self): - box = wx.BoxSizer(wx.VERTICAL) - box.Add( self.panel, 0, wx.EXPAND) - box.Fit( self) - self.SetSizer( box) - self.Fit() - def OnClose(self, event): - IvyStop() - self.Destroy() + def Draw(self): + if self.visualization.display_dirty: + self.visualization.Draw() + self.visualization.display_dirty = False + + def OnClose(self): + IvyStop() - -if __name__ == "__main__": +SCREEN_SIZE = (800, 800) + +def resize(width, height): + glViewport(0, 0, width, height) + glMatrixMode(GL_PROJECTION) + glLoadIdentity() + gluPerspective(60.0, float(width/height), .1, 100.) + glMatrixMode(GL_MODELVIEW) + glLoadIdentity() + +def init(): + glutInit() + glEnable(GL_LINE_SMOOTH) + glEnable(GL_DEPTH_TEST) + glEnable(GL_LIGHTING) + glEnable(GL_LIGHT0) + glEnable(GL_BLEND) + glShadeModel (GL_SMOOTH) + glClearColor(1.0, 1.0, 1.0, 1.0) + glClearDepth(1.0) + + glPointSize(3.0) + + glMatrixMode(GL_PROJECTION) + glLoadIdentity() + gluPerspective(7.0, 1.0, 95.0, 105.0) + + glMatrixMode(GL_MODELVIEW) + + glLight(GL_LIGHT0, GL_POSITION, [5, 30, -20]) + glLight(GL_LIGHT0, GL_AMBIENT, [0.5, 0.5, 0.5]) + glLight(GL_LIGHT0, GL_SPECULAR, [0.0, 0.0, 0.0]) + glLight(GL_LIGHT0, GL_DIFFUSE, [0.8, 0.8, 0.8]) + glEnable(GL_COLOR_MATERIAL) + glColorMaterial(GL_FRONT, GL_AMBIENT_AND_DIFFUSE) + + glMatrixMode(GL_MODELVIEW) + glLoadIdentity() + gluLookAt(0.0, 0.0, 100.0, + 0.0, 0.0, 0.0, + 0.0, 1.0, 0.0) + +def run(): global VEHICLE_QUATS, BAR_VALUES - VEHICLE_QUATS = [ ["AP_EST2USER_0", 11, "Loose Ins Gps"], ["BOOZ2_AHRS_REF_QUAT", 2, "Reference"]] - BAR_VALUES = [ ["AIRSPEED", 3, "Airspeed (m/s) %i", 0, 1, 40], ["BOOZ2_RADIO_CONTROL", 5, "Throttle (%%) %i", 9600, 96 * 2, 100], ["AP_RC", 6, "RC_MODE %i", 0, 1, 2],["AP_RC", 7, "RC_AUX2 %i", 0, 1, 2]] + VEHICLE_QUATS = [ ["AHRS_REF_QUAT", 6, "Estimate", True], ["AHRS_REF_QUAT", 2, "Reference", True]] + BAR_VALUES = [ ["BOOZ2_RADIO_CONTROL", 5, "Throttle (%%) %i", 9600, 96 * 2, 100] ] window_title = "Attitude_Viz" try: - opts, args = getopt.getopt(sys.argv[1:], "t:", - ["title"]) + opts, args = getopt.getopt(sys.argv[1:], "t:", ["title"]) for o,a in opts: if o in ("-t", "--title"): - window_title = a + window_title = a except getopt.error, msg: - print msg - print """usage: + print msg + print """usage: -t, --title set window title """ + pygame.init() + screen = pygame.display.set_mode(SCREEN_SIZE, pygame.OPENGL|pygame.DOUBLEBUF) + #resize(*SCREEN_SIZE) + init() + visualizer = Visualizer() + while True: + for event in pygame.event.get(): + if event.type == pygame.QUIT: + visualizer.OnClose() + return + if event.type == pygame.KEYUP and event.key == pygame.K_ESCAPE: + visualizer.OnClose() + return + visualizer.Draw() + pygame.display.flip() + time.sleep(.02) - app = wx.PySimpleApp() - frame = MainWindow( None, -1, window_title) - frame.Center() - app.MainLoop() +if __name__ == "__main__": + run() diff --git a/sw/include/std.h b/sw/include/std.h index a0dbe2e2cd..a359db6361 100644 --- a/sw/include/std.h +++ b/sw/include/std.h @@ -18,7 +18,7 @@ * 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. + * Boston, MA 02111-1307, USA. * * * a couple of fundamentals used in the avr code diff --git a/sw/lib/ocaml/pprz.ml b/sw/lib/ocaml/pprz.ml index 774b7f1ea7..ba2c64c93a 100644 --- a/sw/lib/ocaml/pprz.ml +++ b/sw/lib/ocaml/pprz.ml @@ -168,6 +168,8 @@ let alt_unit_coef_of_xml = function xml -> | ("rad", "deg") | ("rad/s", "deg/s") -> string_of_float (180. /. pi) | ("m", "cm") | ("m/s", "cm/s") -> "100." | ("cm", "m") | ("cm/s", "m/s") -> "0.01" + | ("m", "mm") | ("m/s", "mm/s") -> "1000." + | ("mm", "m") | ("mm/s", "m/s") -> "0.001" | ("decideg", "deg") -> "0.1" | (_, _) -> "1." diff --git a/sw/lib/ocaml/pprz.mli b/sw/lib/ocaml/pprz.mli index 855dd2938c..b52124d3d5 100644 --- a/sw/lib/ocaml/pprz.mli +++ b/sw/lib/ocaml/pprz.mli @@ -93,8 +93,12 @@ val alt_unit_coef_of_xml : Xml.xml -> string rad -> deg m -> cm cm -> m + m -> mm + mm -> m m/s -> cm/s cm/s -> m/s + m/s -> mm/s + mm/s -> m/s decideg -> deg *) diff --git a/sw/logalizer/export.ml b/sw/logalizer/export.ml index 5be9a3e57c..7707590f2b 100644 --- a/sw/logalizer/export.ml +++ b/sw/logalizer/export.ml @@ -93,14 +93,14 @@ let get_last_geo_pos = fun lookup -> and utm_north = float_of_string (lookup "GPS" "utm_north") /. 100. and utm_zone = int_of_string (lookup "GPS" "utm_zone") in Latlong.of_utm WGS84 {utm_x=utm_east; utm_y=utm_north; utm_zone=utm_zone} - else if lookup "NAV_REF" "x" <>"" && lookup "BOOZ2_FP" "east" <>"" then + else if lookup "NAV_REF" "x" <>"" && lookup "ROTORCRAFT_FP" "east" <>"" then let getf = fun m f -> float_of_string (lookup m f) in let x0 = getf "NAV_REF" "x" /. 100. and y0 = getf "NAV_REF" "y" /. 100. and z0 = getf "NAV_REF" "z" /. 100. - and e = getf "BOOZ2_FP" "east" /. 256. - and n = getf "BOOZ2_FP" "north" /. 256. - and u = getf "BOOZ2_FP" "up" /. 256. in + and e = getf "ROTORCRAFT_FP" "east" /. 256. + and n = getf "ROTORCRAFT_FP" "north" /. 256. + and u = getf "ROTORCRAFT_FP" "up" /. 256. in let ecef0 = make_ecef [|x0; y0; z0 |] and ned = make_ned [|n; e; -.u|] in fst (geo_of_ecef WGS84 (ecef_of_ned ecef0 ned)) diff --git a/sw/simulator/nps/nps_autopilot_booz.c b/sw/simulator/nps/nps_autopilot_booz.c index c82a02c3bf..0d085d547a 100644 --- a/sw/simulator/nps/nps_autopilot_booz.c +++ b/sw/simulator/nps/nps_autopilot_booz.c @@ -33,7 +33,7 @@ void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, cha } #include -#include "booz_gps.h" +#include "subsystems/gps.h" void nps_autopilot_run_step(double time __attribute__ ((unused))) { @@ -58,7 +58,7 @@ void nps_autopilot_run_step(double time __attribute__ ((unused))) { } if (nps_sensors_gps_available()) { - booz_gps_feed_value(); + gps_feed_value(); main_event(); } diff --git a/sw/tools/gen_flight_plan.ml b/sw/tools/gen_flight_plan.ml index 8cee6ff50b..bf24fff948 100644 --- a/sw/tools/gen_flight_plan.ml +++ b/sw/tools/gen_flight_plan.ml @@ -751,8 +751,8 @@ let () = Xml2h.define "NAV_UTM_ZONE0" (sprintf "%d" utm0.utm_zone); Xml2h.define "NAV_LAT0" (sprintf "%d /* 1e7deg */" (convert_angle wgs84.posn_lat)); Xml2h.define "NAV_LON0" (sprintf "%d /* 1e7deg */" (convert_angle wgs84.posn_long)); - Xml2h.define "NAV_ALT0" (sprintf "%.0f /* cm from msl */" (100. *. !ground_alt)); - Xml2h.define "NAV_HMSL0" (sprintf "%.0f /* cm, msl from ellipsoid (EGM96) */" (100. *. Egm96.of_wgs84 wgs84)); + Xml2h.define "NAV_ALT0" (sprintf "%.0f /* mm above msl */" (1000. *. !ground_alt)); + Xml2h.define "NAV_MSL0" (sprintf "%.0f /* mm, EGM96 geoid-height (msl) over ellipsoid */" (1000. *. Egm96.of_wgs84 wgs84)); Xml2h.define "QFU" (sprintf "%.1f" qfu);