diff --git a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml index dc2fddc2bdf..e2428d84fef 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 aaa4ec37d51..b542d440ec0 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/TU_Delft/MicrojetCDW.xml b/conf/airframes/TU_Delft/MicrojetCDW.xml index 34d651058fe..baba44fc6f7 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 @@
- + @@ -81,9 +81,9 @@ - - - + + + @@ -92,7 +92,7 @@ - + @@ -161,7 +161,7 @@ - +
diff --git a/conf/airframes/TU_Delft/skywalker.xml b/conf/airframes/TU_Delft/skywalker.xml new file mode 100644 index 00000000000..226be3e4272 --- /dev/null +++ b/conf/airframes/TU_Delft/skywalker.xml @@ -0,0 +1,240 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + + + + + + + + +
+ + + +
+ +
+ + + +
+ +
+ + +
+ +
+ + + + + +
+ +
+ + +
+ +
+ + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + +
+ +
+ + + + + + + +
+ +
+ + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/airframes/TU_Delft/yapa_xsens.xml b/conf/airframes/TU_Delft/yapa_xsens.xml index d9e489ef66d..f27b129aeb4 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/booz2_flixr.xml b/conf/airframes/booz2_flixr.xml index 323dc620f45..ce88da0d18b 100644 --- a/conf/airframes/booz2_flixr.xml +++ b/conf/airframes/booz2_flixr.xml @@ -4,7 +4,7 @@ - + diff --git a/conf/autopilot/lisa_l_test_progs.makefile b/conf/autopilot/lisa_l_test_progs.makefile index 95aef22c047..22f35e00810 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 a61cf4bfe12..05526f258af 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 00fa88a447b..0574420200a 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 1a48c121c41..233820d1e67 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 c3506ccafa4..708eff5803b 100644 --- a/conf/autopilot/rotorcraft.makefile +++ b/conf/autopilot/rotorcraft.makefile @@ -101,7 +101,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 @@ -216,7 +216,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/subsystems/fixedwing/autopilot.makefile b/conf/autopilot/subsystems/fixedwing/autopilot.makefile index 7d491dd3382..a1994bcd334 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 + ###################################################################### ## ## COMMON FOR ALL NON-SIMULATION TARGETS diff --git a/conf/autopilot/subsystems/fixedwing/gps_ublox_lea4p.makefile b/conf/autopilot/subsystems/fixedwing/gps_ublox_lea4p.makefile index 2e3249f6795..368d4e76a82 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 de753df714d..d8d5723be07 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 4cde327f951..40338ab345d 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 5042fbd81ef..ed0607f397c 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_booz.makefile b/conf/autopilot/subsystems/fixedwing/imu_booz.makefile index e6ad633f17a..c6ee27d0c1f 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) diff --git a/conf/autopilot/subsystems/fixedwing/navigation.makefile b/conf/autopilot/subsystems/fixedwing/navigation.makefile index b1eb8b1dad7..5f699bc858b 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 60cc01c1af8..0ddcd7ca86f 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/rotorcraft/fdm_nps.makefile b/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile index 592c196ea31..b63ecbc6789 100644 --- a/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile +++ b/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile @@ -82,7 +82,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 @@ -169,3 +169,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 9a06840e50b..3e6049af234 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 784099f03a6..14073950e1d 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/shared/ahrs_ic.makefile b/conf/autopilot/subsystems/shared/ahrs_ic.makefile index bda8b08365d..3c3feb18fa9 100644 --- a/conf/autopilot/subsystems/shared/ahrs_ic.makefile +++ b/conf/autopilot/subsystems/shared/ahrs_ic.makefile @@ -10,9 +10,10 @@ 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 +AHRS_SRCS += math/pprz_trig_int.c ap.CFLAGS += $(AHRS_CFLAGS) ap.srcs += $(AHRS_SRCS) @@ -20,3 +21,23 @@ 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/messages.xml b/conf/messages.xml index 0009e3d02f4..692914de170 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -19,15 +19,7 @@ - - - - - - - - - + @@ -48,10 +40,10 @@ - + - + @@ -442,7 +434,7 @@ - + @@ -753,16 +745,40 @@ - + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - @@ -1045,7 +1061,7 @@ - + @@ -1108,25 +1124,26 @@ - - - - + + + + - - - - - - - - - - - - + + + + + + + + + + + + + @@ -1574,7 +1591,14 @@ - + + + + + + + + @@ -1862,9 +1886,9 @@ - - - + + + diff --git a/conf/modules/baro_ms5611_i2c.xml b/conf/modules/baro_ms5611_i2c.xml new file mode 100644 index 00000000000..d3bbf48f7ef --- /dev/null +++ b/conf/modules/baro_ms5611_i2c.xml @@ -0,0 +1,21 @@ + + + + + +
+ +
+ + + + + + + + + + diff --git a/conf/modules/gps_ubx_uart.xml b/conf/modules/gps_ubx_uart.xml new file mode 100644 index 00000000000..55ea724d27e --- /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 00000000000..a3e520d880f --- /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 00000000000..d5f93cd3273 --- /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 27e67847cd5..6ff09118062 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 00000000000..c8439d8e3e0 --- /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 00000000000..1886310cfab --- /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 35991b8d98a..64147a71a15 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 9facc3d61b4..f717d6034ff 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 4f83d2daaa0..22ac1e1d7cf 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 f7ccad596f2..911ed87aa4b 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 b052c9be538..0d84278f267 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 08250f98b9b..2cf8fe880b9 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 b2cd6984f41..eb6a88c2fe8 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 ba8e605fe34..474c6bd9d77 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 71eba1ade6d..623d46e0279 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 7e8ad038e53..b178ddbb479 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 4852249e86f..36de95539bc 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/xsens_MTi-G.xml b/conf/xsens_MTi-G.xml index 4db7955d4ce..63bf030e13a 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 8a0c7bc43f1..34bedf56790 100644 --- a/sw/airborne/ap_downlink.h +++ b/sw/airborne/ap_downlink.h @@ -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 edf4f713fc8..00000000000 --- 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 00000000000..4e401891331 --- /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 00000000000..4345e8b14c1 --- /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/sim/gps_hw.c b/sw/airborne/arch/sim/gps_hw.c deleted file mode 100644 index 45112478032..00000000000 --- 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 61e2085b6be..00000000000 --- 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 1df8a10cd95..f489ac35a4c 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.; + + //TODO set alt above ellipsoid and hmsl - 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; + 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 88ed9404a7f..4963f2ffb23 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 1625f6000f8..9e8bf97d86d 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/sim_ap.c b/sw/airborne/arch/sim/sim_ap.c index f344d8f74f0..a37a453baae 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 e117d97a18f..923961727b6 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 6bfe4c90587..0930c39f344 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 f81a5bcb415..00000000000 --- 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/stm32/gps_hw.h b/sw/airborne/arch/stm32/gps_hw.h deleted file mode 100644 index edf4f713fc8..00000000000 --- 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/booz/test/booz2_test_gps.c b/sw/airborne/booz/test/booz2_test_gps.c index a85f6f30a0c..fa6f92a64df 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 ead159baa5c..2450122ea10 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 4cc32206021..72b1ea4c8a9 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 af09ab6a0ac..61978e26a05 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 4b9b584a2a7..60d871d922a 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 72b1d92359c..7248878cba0 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 00c3c879a73..106156e5649 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 a12f4e873a9..4e98435640b 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/rotorcraft/actuators/actuators_asctec.c b/sw/airborne/firmwares/rotorcraft/actuators/actuators_asctec.c index ae69747eccc..c0b602915bd 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 59b3f9538e0..19512529290 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 07602f1430b..b9d7a04b980 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= 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 dff7396961b..742f1b9c106 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/telemetry.h b/sw/airborne/firmwares/rotorcraft/telemetry.h index 65a755cbb62..d2a18a63d30 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 */ @@ -624,11 +626,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 +648,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_xsens.c b/sw/airborne/gps_xsens.c deleted file mode 100644 index 077d2353af5..00000000000 --- 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 2a23a7c7056..96a5c5e13c9 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 bc3e4c8f64c..50ad516aad6 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_geodetic_double.c b/sw/airborne/math/pprz_geodetic_double.c index 5a411f62ffc..371eac115ae 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 a0c7128fee7..69df6f8e446 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 9d5f4fdb5ec..4f0ef308308 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 e40e502fb5a..9d021880adb 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 99153cc38c4..4d332d53638 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 91a5770fd63..7813bff7a7f 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 00000000000..83ef1db2822 --- /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 00000000000..640bd7be309 --- /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 848fcbad741..554fc6b9aa8 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 1e6ddb8dd0a..608b9b242cb 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 94fd11d943d..5e40d64ad62 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 99543a6f560..9589bda2f54 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 bed149f856b..85170fc0a44 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 00000000000..3349680f443 --- /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 591b36e3c2f..23a312312ec 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 7229bdc344b..def37babcec 100644 --- a/sw/airborne/modules/gsm/gsm.c +++ b/sw/airborne/modules/gsm/gsm.c @@ -41,7 +41,7 @@ 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 @@ #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 f102d183cd4..61694472e87 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 0591bc57bcc..6fade8a21e0 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 c4e14dd7cd6..dbea8c92f2d 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 57eafb72697..6d7dd584aef 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 3929155ec45..7f6453795b4 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 49a053b314e..c8be89d68c8 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); - - 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(); + + // 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; + } 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 446faabad8f..790e65bf5a5 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/humid_htm_b71.c b/sw/airborne/modules/meteo/humid_htm_b71.c new file mode 100644 index 00000000000..30af8b3dc40 --- /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 00000000000..84a03ecbbc7 --- /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 8166f16192c..3515b49673d 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 f9ecd9aceda..cf3508ea3b6 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 5e356d55361..24e3a2538c3 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 1bce893c59a..48c2f53d110 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 cd6ae6a8b67..8572bb909ed 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 6030bb3f7d2..a86db7aae77 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 520eac4f2fa..736b3782c2b 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 00000000000..a77eeba8b25 --- /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 00000000000..6acef006a73 --- /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 00000000000..cc3965c14de --- /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 00000000000..d21dbb50379 --- /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 635ff50df67..3f0b0643be0 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 94c53cf09a5..49386c806e1 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 "booz/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 da14c6bcdbf..c5bc91e44d2 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 e201bc4980c..e455e30633e 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 d5209968314..188b3d18b07 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,24 +30,32 @@ 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) { @@ -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 2601dae09d3..a198ddab116 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 4a3e9abce3f..4438c4d0637 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c @@ -29,7 +29,7 @@ #include "subsystems/ahrs/ahrs_float_dcm_algebra.h" #ifdef USE_GPS -#include "gps.h" +#include "subsystems/gps.h" #endif #include @@ -73,7 +73,8 @@ 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); @@ -113,8 +114,8 @@ void ahrs_update_fw_estimator( void ) estimator_psi = ahrs_float.ltp_to_imu_euler.psi; estimator_p = Omega_Vector[0]; - - RunOnceEvery(6,DOWNLINK_SEND_RMAT_DEBUG(DefaultChannel, +/* + RunOnceEvery(6,DOWNLINK_SEND_RMAT_DEBUG(DefaultChannel, &(DCM_Matrix[0][0]), &(DCM_Matrix[0][1]), &(DCM_Matrix[0][2]), @@ -128,7 +129,7 @@ void ahrs_update_fw_estimator( void ) &(DCM_Matrix[2][2]) )); - +*/ } @@ -219,19 +220,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) @@ -365,9 +416,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 +430,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 a74f631cb38..f398bdd891d 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,27 @@ __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); } + +// 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_imu_euler); + + estimator_phi = att.phi; + estimator_theta = att.theta; + estimator_psi = att.psi; + + //estimator_p = Omega_Vector[0]; +} diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.h b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.h index f1f833b656f..ab0a7d939dc 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.h +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.h @@ -40,4 +40,12 @@ struct AhrsIntCmpl { extern struct AhrsIntCmpl ahrs_impl; + +// 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 /* AHRS_INT_CMPL_H */ diff --git a/sw/airborne/gps_xsens.h b/sw/airborne/subsystems/gps.c similarity index 65% rename from sw/airborne/gps_xsens.h rename to sw/airborne/subsystems/gps.c index 2395793e5a0..4d50a634dca 100644 --- a/sw/airborne/gps_xsens.h +++ b/sw/airborne/subsystems/gps.c @@ -1,7 +1,5 @@ /* - * Paparazzi autopilot $Id: gps_ubx.h 4659 2010-03-10 16:55:04Z mmm $ - * - * Copyright (C) 2004-2006 Pascal Brisset, Antoine Drouin + * Copyright (C) 2008-2011 The Paparazzi Team * * This file is part of paparazzi. * @@ -19,30 +17,24 @@ * 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 - * -*/ - - -#ifndef XSENS_GPS_H -#define XSENS_GPS_H - -extern uint16_t gps_reset; - -#define GPS_NB_CHANNELS 16 +#include "subsystems/gps.h" -extern void reset_gps_watchdog(void); +#include "led.h" +struct GpsState gps; -#define GpsFixValid() (gps_mode > 0) - -#define gps_xsens_Reset(_val) { \ - gps_reset = _val; \ -} - +#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 00000000000..21a30c6bb1f --- /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_sim.c b/sw/airborne/subsystems/gps/gps_sim.c new file mode 100644 index 00000000000..6ac86699645 --- /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 00000000000..7794ea9c248 --- /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 00000000000..55c935af604 --- /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 00000000000..c14e3f8f946 --- /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 00000000000..30ab9674fb2 --- /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 00000000000..fea8398af0c --- /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 00000000000..6a8d3fd61cc --- /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 00000000000..e1f85d4d410 --- /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/ins.c b/sw/airborne/subsystems/ins.c index 0e8e560273c..d25b367a84e 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 126a35c4763..88e2c658571 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 7e644c24789..12aadf70d6b 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 1594b9f81f4..9688560c8c4 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 efd1c72099d..0fa98b71be8 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 e98813e858c..4cb346a2884 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/subsystems/navigation/common_flight_plan.c b/sw/airborne/subsystems/navigation/common_flight_plan.c new file mode 100644 index 00000000000..25f33c990b9 --- /dev/null +++ b/sw/airborne/subsystems/navigation/common_flight_plan.c @@ -0,0 +1,54 @@ +/* + * $Id$ + * + * Copyright (C) 2007-2009 ENAC, 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. + * + */ + +#include "subsystems/navigation/common_flight_plan.h" + +#include "generated/flight_plan.h" + + +/** 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; + } + 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 00000000000..b0394c7e459 --- /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 8a6551fa5bf..c2609a9a1c2 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 9c9dbcc526b..d97e5c27bbb 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 1bd817d43e0..dc20e9a5588 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 827d94bf994..3f34f49c96e 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 961d39a6837..d80738ab593 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 ef50181871a..6779748ce7e 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 1d08b189b70..b4a659a0e7b 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 9ab41fb2b11..219e7786032 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 ce99e5c72f3..567e2274339 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 774d353d1fc..e4c24a983e4 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/tmtc/rotorcraft_server.ml b/sw/ground_segment/tmtc/rotorcraft_server.ml index 7217143aac9..a43aa5db96d 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/include/std.h b/sw/include/std.h index a0dbe2e2cd5..a359db63614 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 774b7f1ea7f..ba2c64c93a3 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 855dd2938cf..b52124d3d55 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 5be9a3e57c4..7707590f2bc 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 c82a02c3bf6..0d085d547ab 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 8cee6ff50b7..bf24fff948a 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);