diff --git a/.gitignore b/.gitignore index a721f0a463..8864c07043 100644 --- a/.gitignore +++ b/.gitignore @@ -113,6 +113,10 @@ /sw/tools/fp_parser.ml /sw/tools/wiki_gen/wiki_gen +# /sw/ground_segment/misc +/sw/ground_segment/misc/davis2ivy + + # /sw/airborne/arch/lpc21/test/bootloader /sw/airborne/arch/lpc21/test/bootloader/bl.dmp /sw/airborne/arch/lpc21/test/bootloader/bl.hex diff --git a/Makefile b/Makefile index 516f73a2ec..5e928b0fa7 100644 --- a/Makefile +++ b/Makefile @@ -43,6 +43,7 @@ AIRBORNE=sw/airborne COCKPIT=sw/ground_segment/cockpit TMTC=sw/ground_segment/tmtc MULTIMON=sw/ground_segment/multimon +MISC=sw/ground_segment/misc LOGALIZER=sw/logalizer SIMULATOR=sw/simulator MAKE=make PAPARAZZI_SRC=$(PAPARAZZI_SRC) PAPARAZZI_HOME=$(PAPARAZZI_HOME) @@ -69,7 +70,7 @@ OCAMLRUN=$(shell which ocamlrun) all: commands static conf -static : lib center tools cockpit multimon tmtc logalizer lpc21iap sim_static static_h usb_lib +static : lib center tools cockpit multimon tmtc misc logalizer lpc21iap sim_static static_h usb_lib conf: conf/conf.xml conf/control_panel.xml @@ -98,13 +99,16 @@ cockpit: lib tmtc: lib cockpit cd $(TMTC); $(MAKE) all +misc: + cd $(MISC); $(MAKE) all + multimon: cd $(MULTIMON); $(MAKE) static_h: $(MESSAGES_H) $(MESSAGES2_H) $(UBX_PROTOCOL_H) $(XSENS_PROTOCOL_H) $(DL_PROTOCOL_H) $(DL_PROTOCOL2_H) usb_lib: - @[ -d sw/airborne/arch/lpc21/lpcusb ] && ((test -x $(ARMGCC) && (cd sw/airborne/arch/lpc21/lpcusb; $(MAKE))) || echo "Not building usb_lib: ARMGCC=$(ARMGCC) not found") || echo "Not building usb_lib: sw/airborne/arch/lpc21/lpcusb directory missing" + @[ -d sw/airborne/arch/lpc21/lpcusb ] && ((test -x "$(ARMGCC)" && (cd sw/airborne/arch/lpc21/lpcusb; $(MAKE))) || echo "Not building usb_lib: ARMGCC=$(ARMGCC) not found") || echo "Not building usb_lib: sw/airborne/arch/lpc21/lpcusb directory missing" $(MESSAGES_H) : $(MESSAGES_XML) $(CONF_XML) tools $(Q)test -d $(STATICINCLUDE) || mkdir -p $(STATICINCLUDE) diff --git a/conf/Makefile.stm32 b/conf/Makefile.stm32 index fb7fe7ea20..be9a360e33 100644 --- a/conf/Makefile.stm32 +++ b/conf/Makefile.stm32 @@ -138,7 +138,7 @@ else LDFLAGS = -D__thumb2__ -T$(LDSCRIPT) -nostartfiles -L$(GCC_LIB_DIR) -O$(OPT) --gc-sections endif LDFLAGS += -Wl,-Map=$(OBJDIR)/$(TARGET).map,--cref,--gc-sections -LDLIBS += -lc -lm -lgcc -lcmsis -lstm32 +LDLIBS += -lc -lm -lgcc -lcmsis -lstm32 -lopencm3_stm32 CPFLAGS = -j .isr_vector -j .text -j .data CPFLAGS_BIN = -Obinary diff --git a/conf/airframes/Poine/booz2_a7.xml b/conf/airframes/Poine/booz2_a7.xml index 091401ebed..61428ed284 100644 --- a/conf/airframes/Poine/booz2_a7.xml +++ b/conf/airframes/Poine/booz2_a7.xml @@ -61,14 +61,6 @@ - @@ -77,7 +69,7 @@
- +
@@ -124,7 +116,7 @@ - + + --> + + + + + + + + + + + + + + + + + + +
+ + + + +
+ +
- +
@@ -155,21 +173,29 @@ + + + + + + - +
- + - + +
@@ -182,15 +208,19 @@
- + +--> - - + + + + + + @@ -201,29 +231,35 @@ - + + + + + - - - - - - - - - - - - - + + + + + + + + + + + + + + + diff --git a/conf/airframes/Poine/booz2_a6.xml b/conf/airframes/Poine/h_hex.xml similarity index 100% rename from conf/airframes/Poine/booz2_a6.xml rename to conf/airframes/Poine/h_hex.xml diff --git a/conf/airframes/Poine/test_settings.xml b/conf/airframes/Poine/test_settings.xml index 72186562de..644d4e99a7 100644 --- a/conf/airframes/Poine/test_settings.xml +++ b/conf/airframes/Poine/test_settings.xml @@ -7,6 +7,8 @@ test settings + + diff --git a/conf/airframes/booz2_flixr.xml b/conf/airframes/booz2_flixr.xml index 38255e1429..323dc620f4 100644 --- a/conf/airframes/booz2_flixr.xml +++ b/conf/airframes/booz2_flixr.xml @@ -4,28 +4,21 @@ - - + - - + + - - - - - + + - - - @@ -33,6 +26,8 @@ + + @@ -213,8 +208,7 @@ - - +
diff --git a/conf/airframes/esden/calib/aspirin_012.xml b/conf/airframes/esden/calib/aspirin_012.xml new file mode 100644 index 0000000000..484ce882c4 --- /dev/null +++ b/conf/airframes/esden/calib/aspirin_012.xml @@ -0,0 +1,32 @@ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
diff --git a/conf/airframes/esden/lisa_m_pwm.xml b/conf/airframes/esden/lisa_m_pwm.xml index 20ef5ac15f..3f78a121e1 100644 --- a/conf/airframes/esden/lisa_m_pwm.xml +++ b/conf/airframes/esden/lisa_m_pwm.xml @@ -158,7 +158,7 @@
- +
@@ -167,14 +167,14 @@ - - - + + + - +
@@ -230,8 +230,8 @@ - - + @@ -38,7 +38,7 @@
- + @@ -51,46 +51,19 @@
+ +
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - +
- +
@@ -137,16 +110,17 @@ + - - - + + + - - - + + + - + @@ -154,7 +128,33 @@ + --> + + + + + + + + + + + + + + + + + + + + +
+ + + +
@@ -167,17 +167,17 @@ - - - + + + + - +
-
@@ -220,8 +220,11 @@ - - + + + + + diff --git a/conf/airframes/mm/extra/probe_t.xml b/conf/airframes/mm/extra/probe_t.xml index b9120d8065..217b5f9bbd 100644 --- a/conf/airframes/mm/extra/probe_t.xml +++ b/conf/airframes/mm/extra/probe_t.xml @@ -14,14 +14,10 @@ - - - + - - - + diff --git a/conf/airframes/mm/extra/turbine_trigger.xml b/conf/airframes/mm/extra/turbine_trigger.xml index c02889cc5d..91c6cad9a1 100644 --- a/conf/airframes/mm/extra/turbine_trigger.xml +++ b/conf/airframes/mm/extra/turbine_trigger.xml @@ -7,14 +7,10 @@ - - - + - - - + diff --git a/conf/airframes/mm/fixed-wing/funjetmm.xml b/conf/airframes/mm/fixed-wing/funjetmm.xml index aaef743332..7249478bc8 100644 --- a/conf/airframes/mm/fixed-wing/funjetmm.xml +++ b/conf/airframes/mm/fixed-wing/funjetmm.xml @@ -10,12 +10,14 @@ + + + + + + + - - - - - @@ -25,30 +27,22 @@ - - - - - - - - + - - - + + @@ -60,16 +54,14 @@ - - diff --git a/conf/airframes/mm/fixed-wing/fw_ins_arduimu.xml b/conf/airframes/mm/fixed-wing/fw_ins_arduimu.xml index ced804cc9e..144fa3b68f 100644 --- a/conf/airframes/mm/fixed-wing/fw_ins_arduimu.xml +++ b/conf/airframes/mm/fixed-wing/fw_ins_arduimu.xml @@ -29,17 +29,13 @@ - - - + - - - + diff --git a/conf/airframes/twinjet_example.xml b/conf/airframes/twinjet_example.xml index 70fd68b0b7..b5974575d7 100644 --- a/conf/airframes/twinjet_example.xml +++ b/conf/airframes/twinjet_example.xml @@ -36,7 +36,7 @@ - + diff --git a/conf/autopilot/booz2_test_progs.makefile b/conf/autopilot/booz2_test_progs.makefile index ec7cc603ac..ff3604c885 100644 --- a/conf/autopilot/booz2_test_progs.makefile +++ b/conf/autopilot/booz2_test_progs.makefile @@ -131,6 +131,29 @@ tunnel_bb.CFLAGS += -DUSE_LED tunnel_bb.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c +# +# usb tunnels +# +usb_tunnel_0.ARCHDIR = $(ARCH) +usb_tunnel_0.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) +usb_tunnel_0.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B38400 -DPERIPHERALS_AUTO_INIT +usb_tunnel_0.CFLAGS += -DUSE_USB_LINE_CODING -DUSE_USB_SERIAL -DUSE_LED +usb_tunnel_0.srcs += $(SRC_ARCH)/usb_tunnel.c $(SRC_ARCH)/usb_ser_hw.c $(SRC_ARCH)/mcu_periph/uart_arch.c +usb_tunnel_0.srcs += $(SRC_ARCH)/lpcusb/usbhw_lpc.c $(SRC_ARCH)/lpcusb/usbinit.c +usb_tunnel_0.srcs += $(SRC_ARCH)/lpcusb/usbcontrol.c $(SRC_ARCH)/lpcusb/usbstdreq.c +usb_tunnel_0.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c +usb_tunnel_0.srcs += mcu.c $(SRC_ARCH)/mcu_arch.c + +usb_tunnel_1.ARCHDIR = $(ARCH) +usb_tunnel_1.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) +usb_tunnel_1.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B38400 -DPERIPHERALS_AUTO_INIT +usb_tunnel_1.CFLAGS += -DUSE_USB_LINE_CODING -DUSE_USB_SERIAL -DUSE_LED +usb_tunnel_1.srcs += $(SRC_ARCH)/usb_tunnel.c $(SRC_ARCH)/usb_ser_hw.c $(SRC_ARCH)/mcu_periph/uart_arch.c +usb_tunnel_1.srcs += $(SRC_ARCH)/lpcusb/usbhw_lpc.c $(SRC_ARCH)/lpcusb/usbinit.c +usb_tunnel_1.srcs += $(SRC_ARCH)/lpcusb/usbcontrol.c $(SRC_ARCH)/lpcusb/usbstdreq.c +usb_tunnel_1.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c +usb_tunnel_1.srcs += mcu.c $(SRC_ARCH)/mcu_arch.c + # # test GPS @@ -189,6 +212,7 @@ test_usb.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))' # -DTIME_LED=1 test_usb.CFLAGS += -DUSE_LED test_usb.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c +test_usb.srcs += mcu.c $(SRC_ARCH)/mcu_arch.c #test_usb.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600 #test_usb.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c diff --git a/conf/autopilot/lisa_l_test_progs.makefile b/conf/autopilot/lisa_l_test_progs.makefile index 8a89e49b72..95aef22c04 100644 --- a/conf/autopilot/lisa_l_test_progs.makefile +++ b/conf/autopilot/lisa_l_test_progs.makefile @@ -735,7 +735,7 @@ tunnel_hw.srcs += $(SRC_ARCH)/led_hw.c tunnel_hw.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=$(SYS_TIME_LED) tunnel_hw.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)' tunnel_hw.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c -tunnel_hw.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B38400 +tunnel_hw.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600 tunnel_hw.CFLAGS += -DUSE_UART2 -DUART2_BAUD=B57600 tunnel_hw.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c @@ -769,4 +769,5 @@ test_settings.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK test_settings.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c test_settings.CFLAGS += -DDATALINK=PPRZ -DPPRZ_UART=$(MODEM_PORT) test_settings.srcs += subsystems/settings.c - +test_settings.srcs += $(SRC_ARCH)/subsystems/settings_arch.c +test_settings.CFLAGS += -DUSE_PERSISTENT_SETTINGS diff --git a/conf/autopilot/lisa_m_test_progs.makefile b/conf/autopilot/lisa_m_test_progs.makefile index 310718dda6..9fa3bd820f 100644 --- a/conf/autopilot/lisa_m_test_progs.makefile +++ b/conf/autopilot/lisa_m_test_progs.makefile @@ -156,27 +156,27 @@ test_telemetry.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c ## MODEM_PORT ## MODEM_BAUD ## -#test_baro.ARCHDIR = $(ARCH) -#test_baro.CFLAGS = -I$(SRC_LISA) -I$(SRC_ARCH) -I$(SRC_BOARD) -DPERIPHERALS_AUTO_INIT -#test_baro.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) -#test_baro.srcs = $(SRC_AIRBORNE)/mcu.c \ -# $(SRC_ARCH)/mcu_arch.c \ -# $(SRC_BOARD)/test_baro.c \ -# $(SRC_ARCH)/stm32_exceptions.c \ -# $(SRC_ARCH)/stm32_vector_table.c -#test_baro.CFLAGS += -DUSE_LED -#test_baro.srcs += $(SRC_ARCH)/led_hw.c -#test_baro.CFLAGS += -DUSE_SYS_TIME -#test_baro.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)' -#test_baro.CFLAGS += -DSYS_TIME_LED=$(SYS_TIME_LED) -#test_baro.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c -#test_baro.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT) -#test_baro.srcs += downlink.c pprz_transport.c -#test_baro.CFLAGS += -DUSE_$(MODEM_PORT) -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD) -#test_baro.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c -#test_baro.srcs += $(SRC_BOARD)/baro_board.c -#test_baro.CFLAGS += -DUSE_I2C2 -#test_baro.srcs += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c +test_baro.ARCHDIR = $(ARCH) +test_baro.CFLAGS = -I$(SRC_LISA) -I$(SRC_ARCH) -I$(SRC_BOARD) -DPERIPHERALS_AUTO_INIT +test_baro.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) +test_baro.srcs = $(SRC_AIRBORNE)/mcu.c \ + $(SRC_ARCH)/mcu_arch.c \ + $(SRC_BOARD)/test_baro.c \ + $(SRC_ARCH)/stm32_exceptions.c \ + $(SRC_ARCH)/stm32_vector_table.c +test_baro.CFLAGS += -DUSE_LED +test_baro.srcs += $(SRC_ARCH)/led_hw.c +test_baro.CFLAGS += -DUSE_SYS_TIME +test_baro.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)' +test_baro.CFLAGS += -DSYS_TIME_LED=$(SYS_TIME_LED) +test_baro.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c +test_baro.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT) +test_baro.srcs += downlink.c pprz_transport.c +test_baro.CFLAGS += -DUSE_$(MODEM_PORT) -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD) +test_baro.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c +test_baro.srcs += $(SRC_BOARD)/baro_board.c +test_baro.CFLAGS += -DUSE_I2C2 +test_baro.srcs += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c # # ## diff --git a/conf/autopilot/rotorcraft.makefile b/conf/autopilot/rotorcraft.makefile index 69a38e4fa8..c3506ccafa 100644 --- a/conf/autopilot/rotorcraft.makefile +++ b/conf/autopilot/rotorcraft.makefile @@ -93,6 +93,8 @@ endif # or # include subsystems/rotorcraft/telemetry_xbee_api.makefile # +ap.srcs += subsystems/settings.c +ap.srcs += $(SRC_ARCH)/subsystems/settings_arch.c ap.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c # I2C is needed for speed controllers and barometers on lisa @@ -153,7 +155,6 @@ ap.srcs += subsystems/electrical.c ap.CFLAGS += -DUSE_DAC ap.srcs += $(SRC_ARCH)/mcu_periph/dac_arch.c else ifeq ($(ARCH), stm32) -#ap.srcs += lisa/lisa_analog_plug.c ap.CFLAGS += -DUSE_ADC ap.CFLAGS += -DUSE_AD1 -DUSE_AD1_1 -DUSE_AD1_2 -DUSE_AD1_3 -DUSE_AD1_4 ap.CFLAGS += -DUSE_ADC1_2_IRQ_HANDLER diff --git a/conf/autopilot/setup.makefile b/conf/autopilot/setup.makefile index b71b547167..bd1757dbb9 100644 --- a/conf/autopilot/setup.makefile +++ b/conf/autopilot/setup.makefile @@ -23,6 +23,7 @@ tunnel.srcs += mcu.c $(SRC_ARCH)/mcu_arch.c # for the usb_tunnel we need to set PCLK higher with the flag USE_USB_HIGH_PCLK # a configuration program to access both uart through usb +ifeq ($(ARCH), lpc21) usb_tunnel_0.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B115200 -DPERIPHERALS_AUTO_INIT usb_tunnel_0.CFLAGS += -DUSE_USB_LINE_CODING -DUSE_USB_SERIAL -DUSE_LED -DUSE_USB_HIGH_PCLK usb_tunnel_0.srcs += $(SRC_ARCH)/usb_tunnel.c $(SRC_ARCH)/usb_ser_hw.c $(SRC_ARCH)/mcu_periph/uart_arch.c @@ -38,6 +39,9 @@ usb_tunnel_1.srcs += $(SRC_ARCH)/lpcusb/usbhw_lpc.c $(SRC_ARCH)/lpcusb/usbinit.c usb_tunnel_1.srcs += $(SRC_ARCH)/lpcusb/usbcontrol.c $(SRC_ARCH)/lpcusb/usbstdreq.c usb_tunnel_1.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c usb_tunnel_1.srcs += mcu.c $(SRC_ARCH)/mcu_arch.c +else +$(error usb_tunnel currently only implemented for the lpc21) +endif diff --git a/conf/autopilot/subsystems/fixedwing/autopilot.makefile b/conf/autopilot/subsystems/fixedwing/autopilot.makefile index 4881ed55f9..47d2fd97df 100644 --- a/conf/autopilot/subsystems/fixedwing/autopilot.makefile +++ b/conf/autopilot/subsystems/fixedwing/autopilot.makefile @@ -133,6 +133,8 @@ ns_srcs += $(SRC_ARCH)/sys_time_hw.c # ns_srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c +ns_srcs += subsystems/settings.c +ns_srcs += $(SRC_ARCH)/subsystems/settings_arch.c # # ANALOG @@ -186,6 +188,9 @@ sim.srcs += $(SRC_ARCH)/sim_ap.c sim.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport sim.srcs += downlink.c $(SRC_FIRMWARE)/datalink.c $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/ivy_transport.c $(SRC_ARCH)/sim_adc_generic.c +sim.srcs += subsystems/settings.c +sim.srcs += $(SRC_ARCH)/subsystems/settings_arch.c + ###################################################################### ## ## JSBSIM THREAD SPECIFIC diff --git a/conf/autopilot/subsystems/fixedwing/telemetry_transparent_usb.makefile b/conf/autopilot/subsystems/fixedwing/telemetry_transparent_usb.makefile index fdc2d31baf..4221fce81d 100644 --- a/conf/autopilot/subsystems/fixedwing/telemetry_transparent_usb.makefile +++ b/conf/autopilot/subsystems/fixedwing/telemetry_transparent_usb.makefile @@ -1,9 +1,15 @@ #serial USB (e.g. /dev/ttyACM0) +ifeq ($(ARCH), lpc21) ap.CFLAGS += -DDOWNLINK -DDOWNLINK_FBW_DEVICE=UsbS -DDOWNLINK_AP_DEVICE=UsbS -DPPRZ_UART=UsbS ap.CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=PPRZ -DUSE_USB_SERIAL -DUSE_USB_HIGH_PCLK ap.srcs += $(SRC_FIXEDWING)/downlink.c $(SRC_FIRMWARE)/datalink.c $(SRC_FIXEDWING)/pprz_transport.c ap.srcs += $(SRC_ARCH)/usb_ser_hw.c $(SRC_ARCH)/lpcusb/usbhw_lpc.c $(SRC_ARCH)/lpcusb/usbcontrol.c ap.srcs += $(SRC_ARCH)/lpcusb/usbstdreq.c $(SRC_ARCH)/lpcusb/usbinit.c +else +ifneq ($(ARCH), sim) +$(error telemetry_transparent_usb currently only implemented for the lpc21) +endif +endif diff --git a/conf/autopilot/subsystems/lisa_passthrough/ahrs_cmpl.makefile b/conf/autopilot/subsystems/lisa_passthrough/ahrs_cmpl.makefile index f13e52aeba..423f48203e 100644 --- a/conf/autopilot/subsystems/lisa_passthrough/ahrs_cmpl.makefile +++ b/conf/autopilot/subsystems/lisa_passthrough/ahrs_cmpl.makefile @@ -2,7 +2,7 @@ # Complementary filter for attitude estimation # -ap.CFLAGS += -DUSE_AHRS_CMPL -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) -DAHRS_FIXED_POINT +ap.CFLAGS += -DUSE_AHRS_CMPL -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) stm_passthrough.srcs += $(SRC_SUBSYSTEMS)/ahrs.c stm_passthrough.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c stm_passthrough.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_int_cmpl_euler.c diff --git a/conf/autopilot/subsystems/rotorcraft/actuators_asctec.makefile b/conf/autopilot/subsystems/rotorcraft/actuators_asctec.makefile index be29427092..e31987e894 100644 --- a/conf/autopilot/subsystems/rotorcraft/actuators_asctec.makefile +++ b/conf/autopilot/subsystems/rotorcraft/actuators_asctec.makefile @@ -14,7 +14,7 @@ endif # Simulator sim.srcs += $(SRC_FIRMWARE)/actuators/actuators_asctec.c -sim.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=150 -DI2C0_SCLH=150 -DI2C0_VIC_SLOT=10 +sim.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=150 -DI2C0_SCLH=150 -DI2C0_VIC_SLOT=11 sim.srcs += mcu_periph/i2c.c sim.srcs += $(SRC_ARCH)/mcu_periph/i2c_arch.c diff --git a/conf/autopilot/subsystems/rotorcraft/actuators_asctec_v2.makefile b/conf/autopilot/subsystems/rotorcraft/actuators_asctec_v2.makefile index 6b9d300c2e..30451deebb 100644 --- a/conf/autopilot/subsystems/rotorcraft/actuators_asctec_v2.makefile +++ b/conf/autopilot/subsystems/rotorcraft/actuators_asctec_v2.makefile @@ -22,7 +22,7 @@ ap.srcs += $(SRC_FIRMWARE)/actuators/actuators_asctec.c ifeq ($(ARCH), lpc21) ap.CFLAGS += -DACTUATORS_ASCTEC_DEVICE=i2c0 -ap.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=150 -DI2C0_SCLH=150 -DI2C0_VIC_SLOT=10 +ap.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=150 -DI2C0_SCLH=150 -DI2C0_VIC_SLOT=11 endif ifeq ($(ARCH), stm32) diff --git a/conf/autopilot/subsystems/rotorcraft/actuators_dummy.makefile b/conf/autopilot/subsystems/rotorcraft/actuators_dummy.makefile new file mode 100644 index 0000000000..5c6b93839a --- /dev/null +++ b/conf/autopilot/subsystems/rotorcraft/actuators_dummy.makefile @@ -0,0 +1,5 @@ +# +# empty dummy actuators for testing +# + +ap.srcs += $(SRC_FIRMWARE)/actuators/actuators_dummy.c diff --git a/conf/autopilot/subsystems/rotorcraft/actuators_mkk.makefile b/conf/autopilot/subsystems/rotorcraft/actuators_mkk.makefile index 3529c03923..a55d0e4313 100644 --- a/conf/autopilot/subsystems/rotorcraft/actuators_mkk.makefile +++ b/conf/autopilot/subsystems/rotorcraft/actuators_mkk.makefile @@ -41,7 +41,7 @@ ap.srcs += $(SRC_FIRMWARE)/actuators/actuators_mkk.c ifeq ($(ARCH), lpc21) ap.CFLAGS += -DACTUATORS_MKK_DEVICE=i2c0 -ap.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=$(MKK_I2C_SCL_TIME) -DI2C0_SCLH=$(MKK_I2C_SCL_TIME) -DI2C0_VIC_SLOT=10 +ap.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=$(MKK_I2C_SCL_TIME) -DI2C0_SCLH=$(MKK_I2C_SCL_TIME) -DI2C0_VIC_SLOT=11 else ifeq ($(ARCH), stm32) ap.CFLAGS += -DACTUATORS_MKK_DEVICE=i2c1 ap.CFLAGS += -DUSE_I2C1 diff --git a/conf/autopilot/subsystems/rotorcraft/ahrs_cmpl.makefile b/conf/autopilot/subsystems/rotorcraft/ahrs_cmpl.makefile index 9482514737..457f08f575 100644 --- a/conf/autopilot/subsystems/rotorcraft/ahrs_cmpl.makefile +++ b/conf/autopilot/subsystems/rotorcraft/ahrs_cmpl.makefile @@ -3,15 +3,14 @@ # ifdef AHRS_ALIGNER_LED -ap.CFLAGS += -DUSE_AHRS_CMPL -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) -DAHRS_FIXED_POINT -else -ap.CFLAGS += -DUSE_AHRS_CMPL -DAHRS_FIXED_POINT +ap.CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) endif +ap.CFLAGS += -DUSE_AHRS_CMPL ap.srcs += $(SRC_SUBSYSTEMS)/ahrs.c ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_int_cmpl_euler.c -sim.CFLAGS += -DUSE_AHRS_CMPL -DAHRS_ALIGNER_LED=3 -DAHRS_FIXED_POINT +sim.CFLAGS += -DUSE_AHRS_CMPL -DAHRS_ALIGNER_LED=3 sim.srcs += $(SRC_SUBSYSTEMS)/ahrs.c sim.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c sim.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_int_cmpl_euler.c diff --git a/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile b/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile index 5c0fe15c78..592c196ea3 100644 --- a/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile +++ b/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile @@ -22,7 +22,7 @@ sim.ARCHDIR = $(ARCH) sim.CFLAGS += -DSITL -DNPS sim.CFLAGS += `pkg-config glib-2.0 --cflags` -I /usr/include/meschach -sim.LDFLAGS += `pkg-config glib-2.0 --libs` -lm -lmeschach -lpcre -lglibivy +sim.LDFLAGS += `pkg-config glib-2.0 --libs` -lm -lpcre -lglibivy -lgsl -lgslcblas sim.CFLAGS += -I$(NPSDIR) -I$(SRC_FIRMWARE) -I$(SRC_BOOZ) -I$(SRC_BOOZ_SIM) -I$(SRC_BOARD) -I../simulator -I$(PAPARAZZI_HOME)/conf/simulator/nps # use the paparazzi-jsbsim package if it is installed, otherwise look for JSBsim under /opt/jsbsim @@ -74,6 +74,8 @@ sim.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))' #sim.CFLAGS += -DUSE_LED sim.srcs += sys_time.c +sim.srcs += subsystems/settings.c +sim.srcs += $(SRC_ARCH)/subsystems/settings_arch.c sim.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport sim.srcs += $(SRC_FIRMWARE)/telemetry.c \ diff --git a/conf/autopilot/subsystems/rotorcraft/gps_skytraq.makefile b/conf/autopilot/subsystems/rotorcraft/gps_skytraq.makefile index e3f8faf488..9a06840e50 100644 --- a/conf/autopilot/subsystems/rotorcraft/gps_skytraq.makefile +++ b/conf/autopilot/subsystems/rotorcraft/gps_skytraq.makefile @@ -10,10 +10,5 @@ ifneq ($(GPS_LED),none) ap.CFLAGS += -DGPS_LED=$(GPS_LED) endif -ifeq ($(ARCH), lpc21) -ap.CFLAGS += -D$(GPS_PORT)_VIC_SLOT=5 -endif - - sim.CFLAGS += -DUSE_GPS sim.srcs += $(SRC_BOOZ)/booz_gps.c diff --git a/conf/autopilot/subsystems/rotorcraft/gps_ublox.makefile b/conf/autopilot/subsystems/rotorcraft/gps_ublox.makefile index 1e476c2c29..784099f03a 100644 --- a/conf/autopilot/subsystems/rotorcraft/gps_ublox.makefile +++ b/conf/autopilot/subsystems/rotorcraft/gps_ublox.makefile @@ -10,9 +10,5 @@ ifneq ($(GPS_LED),none) ap.CFLAGS += -DGPS_LED=$(GPS_LED) endif -ifeq ($(ARCH), lpc21) -ap.CFLAGS += -D$(GPS_PORT)_VIC_SLOT=5 -endif - sim.CFLAGS += -DUSE_GPS sim.srcs += $(SRC_BOOZ)/booz_gps.c diff --git a/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.0.makefile b/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.0.makefile index f3074ca2bb..0e435f803d 100644 --- a/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.0.makefile +++ b/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.0.makefile @@ -50,7 +50,7 @@ imu_CFLAGS += -DMAX1168_EOC_VIC_SLOT=8 imu_CFLAGS += -DUSE_AMI601 imu_srcs += peripherals/ami601.c -imu_CFLAGS += -DUSE_I2C1 -DI2C1_SCLL=150 -DI2C1_SCLH=150 -DI2C1_VIC_SLOT=11 +imu_CFLAGS += -DUSE_I2C1 -DI2C1_SCLL=150 -DI2C1_SCLH=150 -DI2C1_VIC_SLOT=12 # Keep CFLAGS/Srcs for imu in separate expression so we can assign it to other targets diff --git a/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.1.makefile b/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.1.makefile index 18db231194..afea1237dd 100644 --- a/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.1.makefile +++ b/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.1.makefile @@ -57,7 +57,7 @@ imu_srcs += $(SRC_ARCH)/peripherals/ms2100_arch.c ifeq ($(ARCH), lpc21) imu_CFLAGS += -DSSP_VIC_SLOT=9 imu_CFLAGS += -DMAX1168_EOC_VIC_SLOT=8 -imu_CFLAGS += -DMS2100_DRDY_VIC_SLOT=11 +imu_CFLAGS += -DMS2100_DRDY_VIC_SLOT=12 else ifeq ($(ARCH), stm32) imu_CFLAGS += -DUSE_SPI2 -DUSE_DMA1_C4_IRQ -DUSE_EXTI2_IRQ -DUSE_SPI2_IRQ imu_CFLAGS += -DMAX_1168_DRDY_PORT=$(MAX_1168_DRDY_PORT) diff --git a/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.2.makefile b/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.2.makefile index e208c41664..91ba3d16d4 100644 --- a/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.2.makefile +++ b/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.2.makefile @@ -58,7 +58,7 @@ ifeq ($(ARCH), lpc21) imu_CFLAGS += -DSSP_VIC_SLOT=9 imu_CFLAGS += -DMAX1168_EOC_VIC_SLOT=8 #FIXME ms2100 not used on this imu -imu_CFLAGS += -DMS2100_DRDY_VIC_SLOT=11 +imu_CFLAGS += -DMS2100_DRDY_VIC_SLOT=12 else ifeq ($(ARCH), stm32) imu_CFLAGS += -DUSE_SPI2 -DUSE_DMA1_C4_IRQ -DUSE_EXTI2_IRQ -DUSE_SPI2_IRQ imu_CFLAGS += -DMAX_1168_DRDY_PORT=$(MAX_1168_DRDY_PORT) diff --git a/conf/autopilot/subsystems/rotorcraft/imu_crista.makefile b/conf/autopilot/subsystems/rotorcraft/imu_crista.makefile index 449da2f909..aa1dfb9cc9 100644 --- a/conf/autopilot/subsystems/rotorcraft/imu_crista.makefile +++ b/conf/autopilot/subsystems/rotorcraft/imu_crista.makefile @@ -70,7 +70,7 @@ imu_srcs += peripherals/ami601.c imu_CFLAGS += -DUSE_I2C1 ifeq ($(ARCH), lpc21) -imu_CFLAGS += -DI2C1_SCLL=150 -DI2C1_SCLH=150 -DI2C1_VIC_SLOT=11 -DI2C1_BUF_LEN=16 +imu_CFLAGS += -DI2C1_SCLL=150 -DI2C1_SCLH=150 -DI2C1_VIC_SLOT=12 -DI2C1_BUF_LEN=16 else ifeq ($(ARCH), stm32) #FIXME endif diff --git a/conf/autopilot/subsystems/rotorcraft/telemetry_transparent.makefile b/conf/autopilot/subsystems/rotorcraft/telemetry_transparent.makefile index fbc1b89c2e..920b031d66 100644 --- a/conf/autopilot/subsystems/rotorcraft/telemetry_transparent.makefile +++ b/conf/autopilot/subsystems/rotorcraft/telemetry_transparent.makefile @@ -12,7 +12,3 @@ ap.CFLAGS += -DDOWNLINK -DDOWNLINK_DEVICE=$(MODEM_PORT) -DPPRZ_UART=$(MODEM_PORT ap.CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=PPRZ ap.srcs += downlink.c pprz_transport.c ap.srcs += $(SRC_FIRMWARE)/datalink.c $(SRC_FIRMWARE)/telemetry.c - -ifeq ($(ARCH), lpc21) -ap.CFLAGS += -D$(MODEM_PORT)_VIC_SLOT=6 -endif diff --git a/conf/autopilot/subsystems/rotorcraft/telemetry_transparent_usb.makefile b/conf/autopilot/subsystems/rotorcraft/telemetry_transparent_usb.makefile new file mode 100644 index 0000000000..a15920babb --- /dev/null +++ b/conf/autopilot/subsystems/rotorcraft/telemetry_transparent_usb.makefile @@ -0,0 +1,16 @@ + +#serial USB (e.g. /dev/ttyACM0) + +ifeq ($(ARCH), lpc21) +ap.CFLAGS += -DDOWNLINK -DDOWNLINK_DEVICE=UsbS -DPPRZ_UART=UsbS +ap.CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=PPRZ -DUSE_USB_SERIAL +ap.srcs += downlink.c pprz_transport.c +ap.srcs += $(SRC_FIRMWARE)/datalink.c $(SRC_FIRMWARE)/telemetry.c +ap.srcs += $(SRC_ARCH)/usb_ser_hw.c $(SRC_ARCH)/lpcusb/usbhw_lpc.c $(SRC_ARCH)/lpcusb/usbcontrol.c +ap.srcs += $(SRC_ARCH)/lpcusb/usbstdreq.c $(SRC_ARCH)/lpcusb/usbinit.c +else +ifneq ($(ARCH), sim) +$(error telemetry_transparent_usb currently only implemented for the lpc21) +endif +endif + diff --git a/conf/autopilot/subsystems/rotorcraft/telemetry_xbee_api.makefile b/conf/autopilot/subsystems/rotorcraft/telemetry_xbee_api.makefile index a10f3fa95e..d083461928 100644 --- a/conf/autopilot/subsystems/rotorcraft/telemetry_xbee_api.makefile +++ b/conf/autopilot/subsystems/rotorcraft/telemetry_xbee_api.makefile @@ -13,7 +13,3 @@ ap.CFLAGS += -DDOWNLINK -DDOWNLINK_DEVICE=$(MODEM_PORT) -DXBEE_UART=$(MODEM_PORT ap.CFLAGS += -DDOWNLINK_TRANSPORT=XBeeTransport -DDATALINK=XBEE ap.srcs += downlink.c xbee.c ap.srcs += $(SRC_FIRMWARE)/datalink.c $(SRC_FIRMWARE)/telemetry.c - -ifeq ($(ARCH), lpc21) -ap.CFLAGS += -D$(MODEM_PORT)_VIC_SLOT=6 -endif diff --git a/conf/autopilot/subsystems/shared/ahrs_ic.makefile b/conf/autopilot/subsystems/shared/ahrs_ic.makefile index a683b25d66..bda8b08365 100644 --- a/conf/autopilot/subsystems/shared/ahrs_ic.makefile +++ b/conf/autopilot/subsystems/shared/ahrs_ic.makefile @@ -16,3 +16,7 @@ AHRS_SRCS += subsystems/ahrs/ahrs_aligner.c ap.CFLAGS += $(AHRS_CFLAGS) ap.srcs += $(AHRS_SRCS) + +sim.CFLAGS += $(AHRS_CFLAGS) +sim.srcs += $(AHRS_SRCS) + diff --git a/conf/boards/lisa_m_1.0.makefile b/conf/boards/lisa_m_1.0.makefile index 9bb238e36a..0a05903f17 100644 --- a/conf/boards/lisa_m_1.0.makefile +++ b/conf/boards/lisa_m_1.0.makefile @@ -14,10 +14,6 @@ $(TARGET).ARCHDIR = $(ARCH) $(TARGET).OOCD_INTERFACE=flossjtag #$(TARGET).OOCD_INTERFACE=jtagkey-tiny -# ---------------------------------------------------------------------- -# add the opencm3_stm32 lib -LDLIBS += -lopencm3_stm32 - # ----------------------------------------------------------------------- ifndef FLASH_MODE diff --git a/conf/control_panel.xml.example b/conf/control_panel.xml.example index e9a2ea99a1..36f12526fa 100644 --- a/conf/control_panel.xml.example +++ b/conf/control_panel.xml.example @@ -63,6 +63,11 @@ + + + + +
diff --git a/conf/flight_plans/EMAV2008.xml b/conf/flight_plans/EMAV2008.xml index 61457ec38c..2cadcfc0a2 100644 --- a/conf/flight_plans/EMAV2008.xml +++ b/conf/flight_plans/EMAV2008.xml @@ -84,7 +84,7 @@ - + @@ -130,7 +130,7 @@ - + @@ -149,7 +149,7 @@ - + diff --git a/conf/flight_plans/IS.xml b/conf/flight_plans/IS.xml index 9c264b7f37..dddd2b9cb5 100644 --- a/conf/flight_plans/IS.xml +++ b/conf/flight_plans/IS.xml @@ -148,7 +148,7 @@ diff --git a/conf/flight_plans/basic.xml b/conf/flight_plans/basic.xml index 57f3531d50..f9ead2281c 100644 --- a/conf/flight_plans/basic.xml +++ b/conf/flight_plans/basic.xml @@ -78,7 +78,7 @@ - + diff --git a/conf/flight_plans/ccc_gl.xml b/conf/flight_plans/ccc_gl.xml index 47a16f5a85..1a7656c6f6 100644 --- a/conf/flight_plans/ccc_gl.xml +++ b/conf/flight_plans/ccc_gl.xml @@ -112,7 +112,7 @@ - + diff --git a/conf/flight_plans/fp_tp_auto.xml b/conf/flight_plans/fp_tp_auto.xml index d9f0b4f940..3f61b05c1b 100644 --- a/conf/flight_plans/fp_tp_auto.xml +++ b/conf/flight_plans/fp_tp_auto.xml @@ -111,7 +111,7 @@ - + diff --git a/conf/flight_plans/grosslobke_demo.xml b/conf/flight_plans/grosslobke_demo.xml index 03e7dee1a1..92cecfe5f7 100755 --- a/conf/flight_plans/grosslobke_demo.xml +++ b/conf/flight_plans/grosslobke_demo.xml @@ -77,7 +77,7 @@ - + diff --git a/conf/flight_plans/joystick.xml b/conf/flight_plans/joystick.xml index 55b6b6e0cd..585a5542e4 100644 --- a/conf/flight_plans/joystick.xml +++ b/conf/flight_plans/joystick.xml @@ -80,7 +80,7 @@ - + diff --git a/conf/flight_plans/landing.xml b/conf/flight_plans/landing.xml index bcbc1cf8b0..953b26464b 100644 --- a/conf/flight_plans/landing.xml +++ b/conf/flight_plans/landing.xml @@ -18,7 +18,7 @@ - + diff --git a/conf/flight_plans/mav07.xml b/conf/flight_plans/mav07.xml index fb2a87092a..d4e3429ae0 100644 --- a/conf/flight_plans/mav07.xml +++ b/conf/flight_plans/mav07.xml @@ -130,7 +130,7 @@ - + diff --git a/conf/flight_plans/mav08.xml b/conf/flight_plans/mav08.xml index d418ee1cf7..084723612c 100644 --- a/conf/flight_plans/mav08.xml +++ b/conf/flight_plans/mav08.xml @@ -98,7 +98,7 @@ - + diff --git a/conf/flight_plans/nordlys.xml b/conf/flight_plans/nordlys.xml index 61fa5a0fa6..9e279fe850 100644 --- a/conf/flight_plans/nordlys.xml +++ b/conf/flight_plans/nordlys.xml @@ -85,7 +85,7 @@ - + diff --git a/conf/flight_plans/poles.xml b/conf/flight_plans/poles.xml index fad085ec5e..e1476ff1d8 100644 --- a/conf/flight_plans/poles.xml +++ b/conf/flight_plans/poles.xml @@ -70,7 +70,7 @@ - + diff --git a/conf/flight_plans/basic_booz.xml b/conf/flight_plans/rotorcraft_basic.xml similarity index 100% rename from conf/flight_plans/basic_booz.xml rename to conf/flight_plans/rotorcraft_basic.xml diff --git a/conf/flight_plans/slayer_training.xml b/conf/flight_plans/slayer_training.xml index 16773c0183..92d84fe58c 100644 --- a/conf/flight_plans/slayer_training.xml +++ b/conf/flight_plans/slayer_training.xml @@ -75,7 +75,7 @@ - + diff --git a/conf/flight_plans/snav.xml b/conf/flight_plans/snav.xml index 866073c27e..d22bd1c962 100644 --- a/conf/flight_plans/snav.xml +++ b/conf/flight_plans/snav.xml @@ -55,7 +55,7 @@ - + diff --git a/conf/flight_plans/standard.xml b/conf/flight_plans/standard.xml index 906defbb70..ffa5e8dbda 100644 --- a/conf/flight_plans/standard.xml +++ b/conf/flight_plans/standard.xml @@ -68,7 +68,7 @@ - + diff --git a/conf/flight_plans/tcas.xml b/conf/flight_plans/tcas.xml index c0b327457c..9a3c2ce1ba 100644 --- a/conf/flight_plans/tcas.xml +++ b/conf/flight_plans/tcas.xml @@ -72,7 +72,7 @@ - + diff --git a/conf/flight_plans/versatile.xml b/conf/flight_plans/versatile.xml index c5d5e9becc..7544e5dda3 100644 --- a/conf/flight_plans/versatile.xml +++ b/conf/flight_plans/versatile.xml @@ -119,7 +119,7 @@ - + diff --git a/conf/flight_plans/xsens_cachejunction.xml b/conf/flight_plans/xsens_cachejunction.xml index 84aa6cf7bf..2179cf4094 100644 --- a/conf/flight_plans/xsens_cachejunction.xml +++ b/conf/flight_plans/xsens_cachejunction.xml @@ -89,7 +89,7 @@ - + diff --git a/conf/gps/rotorcraft_ublox6.txt b/conf/gps/rotorcraft_ublox6.txt new file mode 100644 index 0000000000..ed3c8389d5 --- /dev/null +++ b/conf/gps/rotorcraft_ublox6.txt @@ -0,0 +1,65 @@ +MON-VER - 0A 04 46 00 37 2E 30 31 20 28 34 34 31 37 39 29 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 30 30 30 34 30 30 30 37 00 00 36 2E 30 32 20 28 33 36 30 32 33 29 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 +CFG-ANT - 06 13 04 00 1B 00 8B A9 +CFG-DAT - 06 06 02 00 00 00 +CFG-FXN - 06 0E 24 00 0C 00 00 00 00 00 00 00 00 00 00 00 10 27 00 00 10 27 00 00 D0 07 00 00 18 FC FF FF 00 00 00 00 00 00 00 00 +CFG-INF - 06 02 0A 00 00 00 00 00 00 00 00 00 00 00 +CFG-INF - 06 02 0A 00 01 00 00 00 87 87 87 87 87 87 +CFG-INF - 06 02 0A 00 03 00 00 00 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 01 01 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 01 02 00 05 00 00 00 00 +CFG-MSG - 06 01 08 00 01 03 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 01 04 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 01 06 00 01 00 00 00 00 +CFG-MSG - 06 01 08 00 01 11 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 01 12 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 01 20 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 01 21 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 01 22 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 01 30 00 04 00 00 00 00 +CFG-MSG - 06 01 08 00 01 31 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 01 32 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 02 20 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 02 23 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0A 02 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0A 05 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0A 06 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0A 07 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0A 08 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0A 09 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0A 0A 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0A 20 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0A 21 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0B 00 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0B 30 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0B 31 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0B 32 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0D 01 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 0D 03 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 F0 00 01 01 01 01 01 01 +CFG-MSG - 06 01 08 00 F0 01 01 01 01 01 01 01 +CFG-MSG - 06 01 08 00 F0 02 01 01 01 01 01 01 +CFG-MSG - 06 01 08 00 F0 03 01 01 01 01 01 01 +CFG-MSG - 06 01 08 00 F0 04 01 01 01 01 01 01 +CFG-MSG - 06 01 08 00 F0 05 01 01 01 01 01 01 +CFG-MSG - 06 01 08 00 F0 06 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 F0 07 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 F0 08 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 F0 09 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 F0 0A 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 F1 00 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 F1 03 00 00 00 00 00 00 +CFG-MSG - 06 01 08 00 F1 04 00 00 00 00 00 00 +CFG-NAV5 - 06 24 24 00 FF FF 08 02 00 00 00 00 10 27 00 00 05 00 FA 00 FA 00 64 00 2C 01 00 3C 00 00 00 00 00 00 00 00 00 00 00 00 +CFG-NAVX5 - 06 23 28 00 00 00 FF FF 03 00 00 00 03 02 03 10 07 00 00 01 00 00 43 06 00 00 00 00 01 01 00 00 00 64 78 00 00 00 00 00 00 00 00 00 +CFG-NMEA - 06 17 04 00 00 23 00 02 +CFG-PM - 06 32 18 00 00 06 00 00 04 90 00 00 E8 03 00 00 10 27 00 00 00 00 00 00 02 00 00 00 +CFG-PRT - 06 00 14 00 00 00 00 00 84 00 00 00 00 00 00 00 07 00 07 00 00 00 00 00 +CFG-PRT - 06 00 14 00 01 00 00 00 C0 08 00 00 00 E1 00 00 07 00 01 00 00 00 00 00 +CFG-PRT - 06 00 14 00 02 00 00 00 C0 08 00 00 80 25 00 00 00 00 00 00 00 00 00 00 +CFG-PRT - 06 00 14 00 03 00 00 00 00 00 00 00 00 00 00 00 07 00 07 00 00 00 00 00 +CFG-RATE - 06 08 06 00 7D 00 01 00 01 00 +CFG-RXM - 06 11 02 00 08 00 +CFG-SBAS - 06 16 08 00 03 03 03 00 51 08 00 00 +CFG-TP - 06 07 14 00 40 42 0F 00 A0 86 01 00 01 01 00 00 32 00 00 00 00 00 00 00 +CFG-TP5 - 06 31 20 00 00 E9 03 00 32 00 00 00 40 42 0F 00 40 42 0F 00 00 00 00 00 A0 86 01 00 00 00 00 00 F7 00 00 00 +CFG-USB - 06 1B 6C 00 46 15 A6 01 00 00 00 00 64 00 00 01 75 2D 62 6C 6F 78 20 41 47 20 2D 20 77 77 77 2E 75 2D 62 6C 6F 78 2E 63 6F 6D 00 00 00 00 00 00 75 2D 62 6C 6F 78 20 36 20 20 2D 20 20 47 50 53 20 52 65 63 65 69 76 65 72 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 diff --git a/conf/messages.xml b/conf/messages.xml index 896f8cae8d..45a085f936 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -451,7 +451,7 @@ - + @@ -834,19 +834,19 @@ - + - + - + @@ -1327,7 +1327,13 @@ - + + + + + + + @@ -1597,7 +1603,12 @@ - + + + + + + diff --git a/conf/modules/ins_arduimu_basic.xml b/conf/modules/ins_arduimu_basic.xml index b1d4493404..bc29ed822e 100644 --- a/conf/modules/ins_arduimu_basic.xml +++ b/conf/modules/ins_arduimu_basic.xml @@ -6,7 +6,7 @@ - + diff --git a/conf/modules/servo_switch.xml b/conf/modules/servo_switch.xml index f6d93e2a34..9255184d55 100644 --- a/conf/modules/servo_switch.xml +++ b/conf/modules/servo_switch.xml @@ -6,7 +6,7 @@ - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/telemetry/telemetry_booz2.xml b/conf/telemetry/telemetry_booz2.xml index ee9f58aac8..4852249e86 100644 --- a/conf/telemetry/telemetry_booz2.xml +++ b/conf/telemetry/telemetry_booz2.xml @@ -40,9 +40,9 @@ - - - + + + diff --git a/conf/telemetry/telemetry_booz2_flixr.xml b/conf/telemetry/telemetry_booz2_flixr.xml deleted file mode 100644 index 441e1d228f..0000000000 --- a/conf/telemetry/telemetry_booz2_flixr.xml +++ /dev/null @@ -1,124 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/doc/pprz_algebra/quaternion.tex b/doc/pprz_algebra/quaternion.tex index 7977222283..3cb937575f 100644 --- a/doc/pprz_algebra/quaternion.tex +++ b/doc/pprz_algebra/quaternion.tex @@ -162,8 +162,8 @@ It is also possible to directly normalise the quaternion \begin{equation} \quat{} := \frac{\quat{}}{\norm{\quat{}}} \end{equation} -\inHfile{INT32\_QUAT\_NORMALISE(q)}{pprz\_algebra\_int} -\inHfile{FLOAT\_QUAT\_NORMALISE(q)}{pprz\_algebra\_float} +\inHfile{INT32\_QUAT\_NORMALIZE(q)}{pprz\_algebra\_int} +\inHfile{FLOAT\_QUAT\_NORMALIZE(q)}{pprz\_algebra\_float} \subsection*{Making the real value positive} It is possible to invert the quaternion if its real value is negative diff --git a/sw/airborne/arch/lpc21/LPC2148-ROM-bl.ld b/sw/airborne/arch/lpc21/LPC2148-ROM-bl.ld index 625af790cd..730c1632e3 100644 --- a/sw/airborne/arch/lpc21/LPC2148-ROM-bl.ld +++ b/sw/airborne/arch/lpc21/LPC2148-ROM-bl.ld @@ -7,8 +7,13 @@ STACK_SIZE = 0x1000; /* Memory Definitions */ MEMORY { - ROM (rx) : ORIGIN = 0x00004000, LENGTH = 484k - RAM (rw) : ORIGIN = 0x40000000, LENGTH = 32k + /* 0x00000000: Paparazzi bootloader (16k) */ + /* 0x00004000: Paparazzi code (480k) */ + /* 0x0007C000: persistent settings (4k) */ + /* 0x0007D000: Philips/NXP bootloader (12k) */ + ROM (rx) : ORIGIN = 0x00004000, LENGTH = 480k + /* topmost 32 bytes are reserved for IAP operations */ + RAM (rw) : ORIGIN = 0x40000000, LENGTH = 32736 } /* Section Definitions */ diff --git a/sw/airborne/arch/lpc21/subsystems/settings_arch.c b/sw/airborne/arch/lpc21/subsystems/settings_arch.c new file mode 100644 index 0000000000..c493cb6521 --- /dev/null +++ b/sw/airborne/arch/lpc21/subsystems/settings_arch.c @@ -0,0 +1,272 @@ +/* + * Paparazzi persistent settings low level flash routines lpc21 + * + * 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. + * + */ + +/* + LPC2148 flash data is located in the last available page + + 0x00000000: Paparazzi bootloader (16k) + 0x00004000: Paparazzi code (480k) + 0x0007C000: persistent settings (4k) + 0x0007D000: Philips/NXP bootloader (12k) + + data flash_addr + data_size flash_end - FSIZ + checksum flash_end - FCHK + + LPC21: minimum write size 256 bytes, endurance 100k cycles, + max sector erase time 400ms, max prog time 1ms per 256 bytes +*/ + +#include "subsystems/settings.h" + +#define IAP_LOCATION 0x7FFFFFF1 + +#define IAP_PREPARE_SECTORS 50 +#define IAP_COPY_RAM_TO_FLASH 51 +#define IAP_ERASE_SECTORS 52 +#define IAP_BLANK_CHECK_SECTORS 53 +#define IAP_READ_PART_ID 54 +#define IAP_COMPARE 56 + +/* we have to operate on 256 byte flash boundaries */ +#define BOUND 256 + +#define FSIZ 8 +#define FCHK 4 + +typedef void (*IAP)(uint32_t[], uint32_t[]); + +typedef struct { + uint32_t addr; + uint32_t total_size; + uint32_t page_nr; + uint32_t page_size; +} FlashInfo; + +static uint32_t pflash_checksum(uint32_t ptr, uint32_t size); +static int32_t flash_detect(FlashInfo* flash); +static int32_t pflash_erase_page(FlashInfo* flash); +static int32_t pflash_program_array(FlashInfo* flash, + uint32_t dest, + uint32_t src); +static int32_t pflash_program_bytes(FlashInfo* flash, + uint32_t src, + uint32_t size, + uint32_t chksum); + + +static uint32_t pflash_checksum(uint32_t ptr, uint32_t size) { + uint32_t i, sum = 0; + + /* do it cheap for now */ + for (i=0; ipage_size = 0x1000; + flash->page_nr = 26; + flash->addr = 0x7C000; + break; + } + default: return -1; + } + + return 0; +} + +static int32_t pflash_erase_page(FlashInfo* flash) { + uint32_t command[5], result[3]; + IAP iap_entry; + + iap_entry = (IAP) IAP_LOCATION; + + /* prepare page/sector */ + command[0] = IAP_PREPARE_SECTORS; + command[1] = flash->page_nr; + command[2] = flash->page_nr; + disableIRQ(); + iap_entry(command, result); + enableIRQ(); + if (result[0] != 0) return result[0]; + + /* erase page/sector */ + command[0] = IAP_ERASE_SECTORS; + command[1] = flash->page_nr; + command[2] = flash->page_nr; + disableIRQ(); + iap_entry(command, result); + enableIRQ(); + if (result[0] != 0) return result[0]; + + /* verify erase */ + command[0] = IAP_BLANK_CHECK_SECTORS; + command[1] = flash->page_nr; + command[2] = flash->page_nr; + iap_entry(command, result); + if (result[0] != 0) return result[0]; + + return 0; +} + +static int32_t pflash_program_array(FlashInfo* flash, + uint32_t dest, + uint32_t src) { + uint32_t command[5], result[3]; + IAP iap_entry; + + iap_entry = (IAP) IAP_LOCATION; + + /* prepare page/sector */ + command[0] = IAP_PREPARE_SECTORS; + command[1] = flash->page_nr; + command[2] = flash->page_nr; + disableIRQ(); + iap_entry(command, result); + enableIRQ(); + if (result[0] != 0) return result[0]; + + /* flash from ram */ + command[0] = IAP_COPY_RAM_TO_FLASH; + command[1] = dest; + command[2] = src; + command[3] = BOUND; + command[4] = CCLK/1000; + disableIRQ(); + iap_entry(command, result); + enableIRQ(); + if (result[0] != 0) return result[0]; + + return 0; +} + +static int32_t pflash_program_bytes(FlashInfo* flash, + uint32_t src, + uint32_t size, + uint32_t chksum) { + uint32_t data[BOUND/4], i, j, ret; + uint32_t ptr = (uint32_t) &data; + + /* erase */ + if ((ret = pflash_erase_page(flash))) return ret; + + /* write data in arrays */ + for (i=0; ipage_size - BOUND) { + data[(BOUND-FSIZ)/4] = size; + data[(BOUND-FCHK)/4] = chksum; + } + if ((ret = pflash_program_array(flash, flash->addr+i, ptr))) return ret; + } + + /* last array */ + if (i <= flash->page_size - BOUND) { + data[(BOUND-FSIZ)/4] = size; + data[(BOUND-FCHK)/4] = chksum; + if ((ret = pflash_program_array(flash, + flash->addr+flash->page_size-BOUND, + ptr))) + return ret; + } + + /* verify data */ + for (i=0; iaddr+i)) != (*(uint8_t*) (src+i))) return -2; + } + if (*(uint32_t*) (flash->addr+flash->page_size-FSIZ) != size) return -3; + if (*(uint32_t*) (flash->addr+flash->page_size-FCHK) != chksum) return -4; + + return 0; +} + +int32_t persistent_write(uint32_t ptr, uint32_t size) { + FlashInfo flash_info; + + if (flash_detect(&flash_info)) return -1; + if ((size > flash_info.page_size-FSIZ) || (size == 0)) return -2; + + return pflash_program_bytes(&flash_info, + ptr, + size, + pflash_checksum(ptr, size)); +} + +int32_t persistent_read(uint32_t ptr, uint32_t size) { + FlashInfo flash; + uint32_t i; + + /* check parameters */ + if (flash_detect(&flash)) return -1; + if ((size > flash.page_size-FSIZ) || (size == 0)) return -2; + + /* check consistency */ + if (size != *(uint32_t*)(flash.addr+flash.page_size-FSIZ)) return -3; + if (pflash_checksum(flash.addr, size) != + *(uint32_t*)(flash.addr+flash.page_size-FCHK)) + return -4; + + /* copy data */ + for (i=0; i + * + * This file is part of Paparazzi. + * + * Paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * Paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + * + */ + + +#ifndef LPC21_SUBSYSTEMS_SETTINGS_H +#define LPC21_SUBSYSTEMS_SETTINGS_H + + + +#endif /* LPC21_SUBSYSTEMS_SETTINGS_H */ diff --git a/sw/airborne/arch/lpc21/vic_slots.txt b/sw/airborne/arch/lpc21/vic_slots.txt deleted file mode 100644 index c75e66ef1e..0000000000 --- a/sw/airborne/arch/lpc21/vic_slots.txt +++ /dev/null @@ -1,15 +0,0 @@ - 1 TIMER0_VIC_SLOT - 2 AD0_VIC_SLOT - 4 AD1_VIC_SLOT - 5 UART0_VIC_SLOT - 6 UART1_VIC_SLOT - 9 I2C0_VIC_SLOT - I2C1_VIC_SLOT -11 SSP_VIC_SLOT -11 MS2100_DRDY_VIC_SLOT - -12 MICROMAG_DRDY_VIC_SLOT -12 MAX11040_DRDY_VIC_SLOT - -?? MAX1167_EOC_VIC_SLOT - diff --git a/sw/airborne/arch/lpc21/vic_slots_fw.txt b/sw/airborne/arch/lpc21/vic_slots_fw.txt new file mode 100644 index 0000000000..6c94aa7bfe --- /dev/null +++ b/sw/airborne/arch/lpc21/vic_slots_fw.txt @@ -0,0 +1,22 @@ +VIC slots used for fixedwings with the LPC2148 + + +define name slot (default) used for +------------------------------------------------------------------ +TIMER0_VIC_SLOT 1 (1) system timer +AD0_VIC_SLOT 2 (2) was for adc battery (not needed anymore?) +hardcoded, no define 3 PWM_ISR in servos_4015 +AD1_VIC_SLOT 4 (4) was for adc baro (not needed anymore?) +UART0_VIC_SLOT 5 (5) uart_arch, e.g. gps +UART1_VIC_SLOT 6 (6) uart_arch, e.g. modem +hardcoded, no define 7 SPI1 in mcu_periph/spi_arch.c, imu_crista_arch, max3100 module, baro_scp module, lcd_dogm module +MAX1168_EOC_VIC_SLOT 8 imu_b2_v1.0, imu_b2_v1.1, imu_b2_v1.2 +hardcoded, no define 8 EXTINT in max3100 module +I2C0_VIC_SLOT ? (9) (9 is default in mcu_periph/i2c_arch.c) +SSP_VIC_SLOT 9 imu_b2_v1.0, imu_b2_v1.1, imu_b2_v1.2 +MICROMAG_DRDY_VIC_SLOT 9 micromag +hardcoded, no define 10 usb, e.g. telemetry_transparent_usb +hardcoded, no define 11 EXTINT in baro_scp module +I2C1_VIC_SLOT 12 ami601 in imu_b2_v1.0, imu_crista +MS2100_DRDY_VIC_SLOT 12 ms2100 mag in imu_b2_v1.1 +MAX11040_DRDY_VIC_SLOT ? max11040 adc module diff --git a/sw/airborne/arch/lpc21/vic_slots_rotorcrafts.txt b/sw/airborne/arch/lpc21/vic_slots_rotorcrafts.txt new file mode 100644 index 0000000000..0acf1429f4 --- /dev/null +++ b/sw/airborne/arch/lpc21/vic_slots_rotorcrafts.txt @@ -0,0 +1,18 @@ +VIC slots used for rotorcrafts with the LPC2148 + +define name slot (default) used for +------------------------------------------------------------------ +TIMER0_VIC_SLOT 1 (1) system timer +AD0_VIC_SLOT 2 (2) was for adc battery (not needed anymore?) + +AD1_VIC_SLOT 4 (4) was for adc baro (not needed anymore?) +UART0_VIC_SLOT 5 (5) uart_arch, e.g. gps +UART1_VIC_SLOT 6 (6) uart_arch, e.g. modem +hardcoded, no define 7 SPI1 in mcu_periph/spi_arch.c, imu_crista_arch +MAX1168_EOC_VIC_SLOT 8 imu_b2_v1.0, imu_b2_v1.1, imu_b2_v1.2 +SSP_VIC_SLOT 9 imu_b2_v1.0, imu_b2_v1.1, imu_b2_v1.2 +hardcoded, no define 10 usb, e.g. telemetry_transparent_usb +I2C0_VIC_SLOT 11 (9) actuators_acstec, actuators_acstec_v2, actuators_mkk, (9 is default in mcu_periph/i2c_arch.c) +I2C1_VIC_SLOT 12 ami601 in imu_b2_v1.0, imu_crista +MS2100_DRDY_VIC_SLOT 12 ms2100 mag in imu_b2_v1.1 + diff --git a/sw/airborne/arch/sim/subsystems/settings_arch.c b/sw/airborne/arch/sim/subsystems/settings_arch.c new file mode 100644 index 0000000000..1ed8a6a601 --- /dev/null +++ b/sw/airborne/arch/sim/subsystems/settings_arch.c @@ -0,0 +1,14 @@ +#include "subsystems/settings.h" + + +int32_t persistent_write(uint32_t ptr, uint32_t size) { + ptr=ptr; + size=size; + return 0; +} + +int32_t persistent_read(uint32_t ptr, uint32_t size) { + ptr=ptr; + size=size; + return 0; +} diff --git a/sw/airborne/arch/stm32/mcu_arch.c b/sw/airborne/arch/stm32/mcu_arch.c index 9a6fb255ec..494d252322 100644 --- a/sw/airborne/arch/stm32/mcu_arch.c +++ b/sw/airborne/arch/stm32/mcu_arch.c @@ -87,5 +87,12 @@ void mcu_arch_init(void) { /* Set the Vector Table base location at 0x08000000 */ NVIC_SetVectorTable(NVIC_VectTab_FLASH, 0x0); +#ifdef STM32_FORCE_ALL_CLOCK_ON + RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB | + RCC_APB2Periph_GPIOC | RCC_APB2Periph_GPIOD | + RCC_APB2Periph_GPIOE | RCC_APB2Periph_AFIO, ENABLE); +#endif + + } diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.new.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.new.c new file mode 100644 index 0000000000..59dfcf823a --- /dev/null +++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.new.c @@ -0,0 +1,616 @@ +#include "mcu_periph/i2c.h" + +#include +#include +#include +#include + + +static void start_transaction(struct i2c_periph* p); +static inline void end_of_transaction(struct i2c_periph *p); +static inline void i2c_hard_reset(struct i2c_periph *p); +static inline void i2c_reset_init(struct i2c_periph *p); + +#define I2C_BUSY 0x20 + +#ifdef DEBUG_I2C +#define SPURIOUS_INTERRUPT(_periph, _status, _event) { while(1); } +#define OUT_OF_SYNC_STATE_MACHINE(_periph, _status, _event) { while(1); } +#else +//#define SPURIOUS_INTERRUPT(_periph, _status, _event) { periph->errors->unexpected_event_cnt++; abort_and_reset(_periph);} +#define SPURIOUS_INTERRUPT(_periph, _status, _event) { if (_status == I2CAddrWrSent) abort_and_reset(_periph);} +#define OUT_OF_SYNC_STATE_MACHINE(_periph, _status, _event) { abort_and_reset(_periph);} +#endif + +#ifdef USE_I2C1 +static I2C_InitTypeDef I2C1_InitStruct = { + .I2C_Mode = I2C_Mode_I2C, + .I2C_DutyCycle = I2C_DutyCycle_2, + .I2C_OwnAddress1 = 0x00, + .I2C_Ack = I2C_Ack_Enable, + .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, + .I2C_ClockSpeed = 200000 +}; +#endif + +#ifdef USE_I2C2 +static I2C_InitTypeDef I2C2_InitStruct = { + .I2C_Mode = I2C_Mode_I2C, + .I2C_DutyCycle = I2C_DutyCycle_2, + .I2C_OwnAddress1 = 0x00, + .I2C_Ack = I2C_Ack_Enable, + .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, + .I2C_ClockSpeed = 300000 +}; +#endif + +static inline void i2c_delay(void) +{ + for (__IO int j = 0; j < 50; j++); +} + +static inline void i2c_apply_config(struct i2c_periph *p) +{ + I2C_Init(p->reg_addr, p->init_struct); +} + +static inline void end_of_transaction(struct i2c_periph *p) +{ + p->trans_extract_idx++; + if (p->trans_extract_idx >= I2C_TRANSACTION_QUEUE_LEN) + p->trans_extract_idx = 0; + /* if we have no more transaction to process, stop here */ + if (p->trans_extract_idx == p->trans_insert_idx) + p->status = I2CIdle; + /* if not, start next transaction */ + else + start_transaction(p); +} + +static inline void abort_and_reset(struct i2c_periph *p) { + struct i2c_transaction* trans = p->trans[p->trans_extract_idx]; + trans->status = I2CTransFailed; + I2C_ITConfig(p->reg_addr, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE); + i2c_hard_reset(p); + I2C_ITConfig(p->reg_addr, I2C_IT_ERR, ENABLE); + end_of_transaction(p); +} + +#ifdef USE_I2C2 +static inline void on_status_start_requested(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event); +static inline void on_status_addr_wr_sent(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event); +static inline void on_status_sending_byte(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event); +static inline void on_status_stop_requested(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event); +static inline void on_status_addr_rd_sent(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event); +static inline void on_status_reading_byte(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event); +static inline void on_status_reading_last_byte(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event); +static inline void on_status_restart_requested(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event); + +/* + * Start Requested + * + */ +static inline void on_status_start_requested(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event) { + if (event & I2C_FLAG_SB) { + if(trans->type == I2CTransRx) { + I2C_Send7bitAddress(periph->reg_addr, trans->slave_addr, I2C_Direction_Receiver); + periph->status = I2CAddrRdSent; + } + else { + I2C_Send7bitAddress(periph->reg_addr, trans->slave_addr, I2C_Direction_Transmitter); + periph->status = I2CAddrWrSent; + } + } + // else + // SPURIOUS_INTERRUPT(periph, I2CStartRequested, event); + // FIXME: this one seems to get called all the time with mkk controllers +} + +/* + * Addr WR sent + * + */ +static inline void on_status_addr_wr_sent(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event) { + if ((event & I2C_FLAG_ADDR) && (event & I2C_FLAG_TRA)) { + I2C_SendData(periph->reg_addr, trans->buf[0]); + if (trans->len_w > 1) { + I2C_SendData(periph->reg_addr, trans->buf[1]); + periph->idx_buf = 2; + I2C_ITConfig(periph->reg_addr, I2C_IT_BUF, ENABLE); + periph->status = I2CSendingByte; + } + else { + periph->idx_buf = 1; + if (trans->type == I2CTransTx) { + I2C_GenerateSTOP(periph->reg_addr, ENABLE); + periph->status = I2CStopRequested; + } + else { + I2C_GenerateSTART(periph->reg_addr, ENABLE); + periph->status = I2CRestartRequested; + } + } + } + else { + SPURIOUS_INTERRUPT(periph, I2CAddrWrSent, event); + // FIXME: this was where the code would break with mkk controllers on april 10 2011 + // now have SPURIOUS_INTERRUPT call abort_and_reset + } +} + +/* + * Sending Byte + * + */ +static inline void on_status_sending_byte(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event) { + I2C_TypeDef *regs = (I2C_TypeDef *) periph->reg_addr; + if (event & I2C_FLAG_TXE) { + if (periph->idx_buf < trans->len_w) { + I2C_SendData(periph->reg_addr, trans->buf[periph->idx_buf]); + periph->idx_buf++; + } + else { + I2C_ITConfig(periph->reg_addr, I2C_IT_BUF, DISABLE); + if (trans->type == I2CTransTx) { + I2C_GenerateSTOP(periph->reg_addr, ENABLE); + /* Make sure that the STOP bit is cleared by Hardware */ + static __IO uint8_t counter = 0; + while ((regs->CR1 & 0x200) == 0x200) { + counter++; + if (counter > 100) break; + } + periph->status = I2CStopRequested; + } + else { + I2C_GenerateSTART(periph->reg_addr, ENABLE); + periph->status = I2CRestartRequested; + } + } + } + else + SPURIOUS_INTERRUPT(periph, I2CSendingByte, event); +} + +/* + * Stop Requested + * + */ +static inline void on_status_stop_requested(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event) { + /* bummer.... */ + if (event & I2C_FLAG_RXNE) { + uint8_t read_byte = I2C_ReceiveData(periph->reg_addr); + if (periph->idx_buf < trans->len_r) { + trans->buf[periph->idx_buf] = read_byte; + } + } + I2C_ITConfig(periph->reg_addr, I2C_IT_EVT|I2C_IT_BUF, DISABLE); // should only need to disable evt, buf already disabled + trans->status = I2CTransSuccess; + end_of_transaction(periph); +} + +/* + * Addr RD sent + * + */ +static inline void on_status_addr_rd_sent(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event) { + I2C_TypeDef *regs = (I2C_TypeDef *) periph->reg_addr; + + if ((event & I2C_FLAG_ADDR) && !(event & I2C_FLAG_TRA)) { + periph->idx_buf = 0; + if(trans->len_r == 1) { // If we're going to read only one byte + I2C_AcknowledgeConfig(periph->reg_addr, DISABLE); // make sure it's gonna be nacked + I2C_GenerateSTOP(periph->reg_addr, ENABLE); // and followed by a stop + /* Make sure that the STOP bit is cleared by Hardware */ + static __IO uint8_t counter = 0; + while ((regs->CR1 & 0x200) == 0x200) { + counter++; + if (counter > 100) break; + } + periph->status = I2CReadingLastByte; // and remember we did + } + else { + I2C_AcknowledgeConfig(periph->reg_addr, ENABLE); // if it's more than one byte, ack it + I2C_ITConfig(periph->reg_addr, I2C_IT_BUF, ENABLE); + periph->status = I2CReadingByte; // and remember we did + } + } + else + SPURIOUS_INTERRUPT(periph, I2CAddrRdSent, event); +} + + +/* + * Reading byte + * + */ +static inline void on_status_reading_byte(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event) { + I2C_TypeDef *regs = (I2C_TypeDef *) periph->reg_addr; + if (event & I2C_FLAG_RXNE) { + uint8_t read_byte = I2C_ReceiveData(periph->reg_addr); + if (periph->idx_buf < trans->len_r) { + trans->buf[periph->idx_buf] = read_byte; + periph->idx_buf++; + if (periph->idx_buf >= trans->len_r-1) { // We're reading our last byte + I2C_AcknowledgeConfig(periph->reg_addr, DISABLE); // give them a nack once it's done + I2C_GenerateSTOP(periph->reg_addr, ENABLE); // and follow with a stop + /* Make sure that the STOP bit is cleared by Hardware */ + static __IO uint8_t counter = 0; + while ((regs->CR1 & 0x200) == 0x200) { + counter++; + if (counter > 100) break; + } + periph->status = I2CStopRequested; // remember we already trigered the stop + } + } // else { something very wrong has happened } + } + else + SPURIOUS_INTERRUPT(periph, I2CReadingByte, event); +} + +/* + * Reading last byte + * + */ +static inline void on_status_reading_last_byte(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event) { + if (event & I2C_FLAG_BTF) { + uint8_t read_byte = I2C_ReceiveData(periph->reg_addr); + trans->buf[periph->idx_buf] = read_byte; + I2C_GenerateSTOP(periph->reg_addr, ENABLE); + periph->status = I2CStopRequested; + } + else if (event & I2C_FLAG_RXNE) { // should really be BTF ? + uint8_t read_byte = I2C_ReceiveData(periph->reg_addr); + trans->buf[periph->idx_buf] = read_byte; + periph->status = I2CStopRequested; + } + else + SPURIOUS_INTERRUPT(periph, I2CReadingLastByte, event); +} + +/* + * Restart requested + * + */ +static inline void on_status_restart_requested(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event) { + if (event & I2C_FLAG_SB) { + I2C_Send7bitAddress(periph->reg_addr, trans->slave_addr, I2C_Direction_Receiver); + periph->status = I2CAddrRdSent; + } +} + + + +static inline void i2c_event(struct i2c_periph *p, uint32_t event) +{ + struct i2c_transaction* trans = p->trans[p->trans_extract_idx]; + switch (p->status) { + case I2CStartRequested: + on_status_start_requested(p, trans, event); + break; + case I2CAddrWrSent: + on_status_addr_wr_sent(p, trans, event); + break; + case I2CSendingByte: + on_status_sending_byte(p, trans, event); + break; + case I2CStopRequested: + on_status_stop_requested(p, trans, event); + break; + case I2CAddrRdSent: + on_status_addr_rd_sent(p, trans, event); + break; + case I2CReadingByte: + on_status_reading_byte(p, trans, event); + break; + case I2CReadingLastByte: + on_status_reading_last_byte(p, trans, event); + break; + case I2CRestartRequested: + on_status_restart_requested(p, trans, event); + break; + default: + OUT_OF_SYNC_STATE_MACHINE(p, p->status, event); + break; + } +} + +static inline void i2c_error(struct i2c_periph *p) +{ + p->errors->er_irq_cnt; + if (I2C_GetITStatus(p->reg_addr, I2C_IT_AF)) { /* Acknowledge failure */ + p->errors->ack_fail_cnt++; + I2C_ClearITPendingBit(p->reg_addr, I2C_IT_AF); + } + if (I2C_GetITStatus(p->reg_addr, I2C_IT_BERR)) { /* Misplaced Start or Stop condition */ + p->errors->miss_start_stop_cnt++; + I2C_ClearITPendingBit(p->reg_addr, I2C_IT_BERR); + } + if (I2C_GetITStatus(p->reg_addr, I2C_IT_ARLO)) { /* Arbitration lost */ + p->errors->arb_lost_cnt++; + I2C_ClearITPendingBit(p->reg_addr, I2C_IT_ARLO); + // I2C_AcknowledgeConfig(I2C2, DISABLE); + // uint8_t dummy __attribute__ ((unused)) = I2C_ReceiveData(I2C2); + // I2C_GenerateSTOP(I2C2, ENABLE); + } + if (I2C_GetITStatus(p->reg_addr, I2C_IT_OVR)) { /* Overrun/Underrun */ + p->errors->over_under_cnt++; + I2C_ClearITPendingBit(p->reg_addr, I2C_IT_OVR); + } + if (I2C_GetITStatus(p->reg_addr, I2C_IT_PECERR)) { /* PEC Error in reception */ + p->errors->pec_recep_cnt++; + I2C_ClearITPendingBit(p->reg_addr, I2C_IT_PECERR); + } + if (I2C_GetITStatus(p->reg_addr, I2C_IT_TIMEOUT)) { /* Timeout or Tlow error */ + p->errors->timeout_tlow_cnt++; + I2C_ClearITPendingBit(p->reg_addr, I2C_IT_TIMEOUT); + } + if (I2C_GetITStatus(p->reg_addr, I2C_IT_SMBALERT)) { /* SMBus alert */ + p->errors->smbus_alert_cnt++; + I2C_ClearITPendingBit(p->reg_addr, I2C_IT_SMBALERT); + } + + abort_and_reset(p); +} + + +static inline void i2c_hard_reset(struct i2c_periph *p) +{ + I2C_TypeDef *regs = (I2C_TypeDef *) p->reg_addr; + + I2C_DeInit(p->reg_addr); + + GPIO_InitTypeDef GPIO_InitStructure; + GPIO_InitStructure.GPIO_Pin = p->scl_pin | p->sda_pin; + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_10MHz; + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD; + GPIO_SetBits(GPIOB, p->scl_pin | p->sda_pin); + GPIO_Init(GPIOB, &GPIO_InitStructure); + + while(GPIO_ReadInputDataBit(GPIOB, p->sda_pin) == Bit_RESET) { + // Raise SCL, wait until SCL is high (in case of clock stretching) + GPIO_SetBits(GPIOB, p->scl_pin); + while (GPIO_ReadInputDataBit(GPIOB, p->scl_pin) == Bit_RESET); + i2c_delay(); + + // Lower SCL, wait + GPIO_ResetBits(GPIOB, p->scl_pin); + i2c_delay(); + + // Raise SCL, wait + GPIO_SetBits(GPIOB, p->scl_pin); + i2c_delay(); + } + + // Generate a start condition followed by a stop condition + GPIO_SetBits(GPIOB, p->scl_pin); + i2c_delay(); + GPIO_ResetBits(GPIOB, p->sda_pin); + i2c_delay(); + GPIO_ResetBits(GPIOB, p->sda_pin); + i2c_delay(); + + // Raise both SCL and SDA and wait for SCL high (in case of clock stretching) + GPIO_SetBits(GPIOB, p->scl_pin | p->sda_pin); + while (GPIO_ReadInputDataBit(GPIOB, p->scl_pin) == Bit_RESET); + + // Wait for SDA to be high + while (GPIO_ReadInputDataBit(GPIOB, p->sda_pin) != Bit_SET); + + // SCL and SDA should be high at this point, bus should be free + // Return the GPIO pins to the alternate function + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD; + GPIO_Init(GPIOB, &GPIO_InitStructure); + + I2C_DeInit(p->reg_addr); + + i2c_apply_config(p); + + if (regs->SR2 & I2C_BUSY) { + // Reset the I2C block + I2C_SoftwareResetCmd(p->reg_addr, ENABLE); + I2C_SoftwareResetCmd(p->reg_addr, DISABLE); + } +} + +static inline void i2c_reset_init(struct i2c_periph *p) +{ + // Reset bus and configure GPIO pins + i2c_hard_reset(p); + + // enable peripheral + I2C_Cmd(p->reg_addr, ENABLE); + + // enable error interrupts + I2C_ITConfig(p->reg_addr, I2C_IT_ERR, ENABLE); +} +#endif /* USE_I2C2 */ + +#ifdef USE_I2C1 + +struct i2c_errors i2c1_errors; + +#include "my_debug_servo.h" + +void i2c1_hw_init(void) { + + i2c1.reg_addr = I2C1; + i2c1.init_struct = &I2C1_InitStruct; + i2c1.scl_pin = GPIO_Pin_6; + i2c1.sda_pin = GPIO_Pin_7; + i2c1.errors = &i2c1_errors; + + /* zeros error counter */ + ZEROS_ERR_COUNTER(i2c1_errors); + + /* reset peripheral to default state ( sometimes not achieved on reset :( ) */ + I2C_DeInit(I2C1); + + NVIC_PriorityGroupConfig(NVIC_PriorityGroup_0); + NVIC_InitTypeDef NVIC_InitStructure; + + /* Configure and enable I2C1 event interrupt -------------------------------*/ + NVIC_InitStructure.NVIC_IRQChannel = I2C1_EV_IRQn; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; + NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1; + NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; + NVIC_Init(&NVIC_InitStructure); + + /* Configure and enable I2C1 err interrupt -------------------------------*/ + NVIC_InitStructure.NVIC_IRQChannel = I2C1_ER_IRQn; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; + NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; + NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; + NVIC_Init(&NVIC_InitStructure); + + /* Enable peripheral clocks --------------------------------------------------*/ + /* Enable I2C1 clock */ + RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE); + /* Enable GPIOB clock */ + RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); + + /* Configure I2C1 pins: SCL and SDA ------------------------------------------*/ + GPIO_InitTypeDef GPIO_InitStructure; + GPIO_StructInit(&GPIO_InitStructure); + GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7; + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD; + GPIO_Init(GPIOB, &GPIO_InitStructure); + + /* I2C configuration ----------------------------------------------------------*/ + + + /* Reset and initialize I2C HW */ + i2c_reset_init(&i2c1); + +} + + +void i2c1_ev_irq_handler(void) { + + uint32_t event = I2C_GetLastEvent(I2C1); + i2c_event(&i2c1, event); + +} + + +void i2c1_er_irq_handler(void) { + i2c_error(&i2c1); +} + +#endif /* USE_I2C1 */ + + + + + +#ifdef USE_I2C2 + +// dec hex +// 196609 30001 BUSY MSL | SB +// 458882 70082 TRA BUSY MSL | TXE ADDR +// 458884 70084 TRA BUSY MSL | TXE BTF +// 196609 30001 BUSY MSL | SB +// 196610 30002 BUSY MSL | ADDR +// + + +struct i2c_errors i2c2_errors; + +#include "my_debug_servo.h" + +void i2c2_hw_init(void) { + + i2c2.reg_addr = I2C2; + i2c2.init_struct = &I2C2_InitStruct; + i2c2.scl_pin = GPIO_Pin_10; + i2c2.sda_pin = GPIO_Pin_11; + i2c2.errors = &i2c2_errors; + + /* zeros error counter */ + ZEROS_ERR_COUNTER(i2c2_errors); + + /* reset peripheral to default state ( sometimes not achieved on reset :( ) */ + I2C_DeInit(I2C2); + + NVIC_PriorityGroupConfig(NVIC_PriorityGroup_0); + NVIC_InitTypeDef NVIC_InitStructure; + + /* Configure and enable I2C2 event interrupt --------------------------------*/ + NVIC_InitStructure.NVIC_IRQChannel = I2C2_EV_IRQn; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; + NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; + NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; + NVIC_Init(&NVIC_InitStructure); + + /* Configure and enable I2C2 err interrupt ----------------------------------*/ + NVIC_InitStructure.NVIC_IRQChannel = I2C2_ER_IRQn; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; + NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1; + NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; + NVIC_Init(&NVIC_InitStructure); + + /* Enable peripheral clocks -------------------------------------------------*/ + /* Enable I2C2 clock */ + RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C2, ENABLE); + /* Enable GPIOB clock */ + RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); + + // Reset and initialize I2C HW + i2c_reset_init(&i2c2); + +} + + + + +void i2c2_ev_irq_handler(void) { + uint32_t event = I2C_GetLastEvent(I2C2); + i2c_event(&i2c2, event); +} + +void i2c2_er_irq_handler(void) { + i2c_error(&i2c2); + +} + +#endif /* USE_I2C2 */ + + + +bool_t i2c_idle(struct i2c_periph* p) +{ + return !I2C_GetFlagStatus(p->reg_addr, I2C_FLAG_BUSY); +} + +bool_t i2c_submit(struct i2c_periph* p, struct i2c_transaction* t) { + + uint8_t temp; + temp = p->trans_insert_idx + 1; + if (temp >= I2C_TRANSACTION_QUEUE_LEN) temp = 0; + if (temp == p->trans_extract_idx) + return FALSE; // queue full + + t->status = I2CTransPending; + + + __disable_irq(); + /* put transacation in queue */ + p->trans[p->trans_insert_idx] = t; + p->trans_insert_idx = temp; + + /* if peripheral is idle, start the transaction */ + if (p->status == I2CIdle) + start_transaction(p); + /* else it will be started by the interrupt handler when the previous transactions completes */ + __enable_irq(); + + return TRUE; +} + + +static void start_transaction(struct i2c_periph* p) { + p->idx_buf = 0; + p->status = I2CStartRequested; + I2C_ITConfig(p->reg_addr, I2C_IT_EVT, ENABLE); + I2C_GenerateSTART(p->reg_addr, ENABLE); +} diff --git a/sw/airborne/arch/stm32/stm32f103rb_flash.ld b/sw/airborne/arch/stm32/stm32f103rb_flash.ld index 7e1575e460..95181a8db5 100644 --- a/sw/airborne/arch/stm32/stm32f103rb_flash.ld +++ b/sw/airborne/arch/stm32/stm32f103rb_flash.ld @@ -25,7 +25,8 @@ MEMORY { RAM (xrw): ORIGIN = 0x20000000, LENGTH = 20K - FLASH (rx) : ORIGIN = 0x8000000, LENGTH = 128K + /* last page (1k) flash for persistent settings */ + FLASH (rx) : ORIGIN = 0x8000000, LENGTH = 127K FLASHB1 (rx) : ORIGIN = 0x00000000, LENGTH = 0 } diff --git a/sw/airborne/arch/stm32/stm32f103re_flash.ld b/sw/airborne/arch/stm32/stm32f103re_flash.ld index 3df6455c4c..94fcdbdbd3 100644 --- a/sw/airborne/arch/stm32/stm32f103re_flash.ld +++ b/sw/airborne/arch/stm32/stm32f103re_flash.ld @@ -25,7 +25,8 @@ MEMORY { RAM (xrw): ORIGIN = 0x20000000, LENGTH = 64K - FLASH (rx) : ORIGIN = 0x8000000, LENGTH = 512K + /* last page (2k) flash for persistent settings */ + FLASH (rx) : ORIGIN = 0x8000000, LENGTH = 510K FLASHB1 (rx) : ORIGIN = 0x00000000, LENGTH = 0 } diff --git a/sw/airborne/arch/stm32/subsystems/settings_arch.c b/sw/airborne/arch/stm32/subsystems/settings_arch.c new file mode 100644 index 0000000000..ee5f21a598 --- /dev/null +++ b/sw/airborne/arch/stm32/subsystems/settings_arch.c @@ -0,0 +1,270 @@ +/* + * Paparazzi persistent settings low level flash routines stm32 + * + * 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. + * + */ + +/* + flash data is located in the last page/sector of flash + + data flash_addr + data_size flash_end - FSIZ + checksum flash_end - FCHK + + STM32: minimum write size 2 bytes, endurance 10k cycles, + max sector erase time 40ms, max prog time 70us per 2 bytes +*/ + +#include "subsystems/settings.h" + +#include +#include +#include + +struct FlashInfo { + uint32_t addr; + uint32_t total_size; + uint32_t page_nr; + uint32_t page_size; +}; + + +static uint32_t pflash_checksum(uint32_t ptr, uint32_t size); +static int32_t flash_detect(struct FlashInfo* flash); +static int32_t pflash_program_bytes(struct FlashInfo* flash, + uint32_t src, + uint32_t size, + uint32_t chksum); + +#define FLASH_SIZE_ MMIO16(0x1FFFF7E0) + +#define FLASH_BEGIN 0x08000000 +#define FSIZ 8 +#define FCHK 4 + + +static uint32_t pflash_checksum(uint32_t ptr, uint32_t size) { + uint32_t i; + + /* reset crc */ + CRC_CR = CRC_CR_RESET; + + if (ptr % 4) { + /* calc in 8bit chunks */ + for (i=0; i<(size & ~3); i+=4) { + CRC_DR = (*(uint8_t*) (ptr+i)) | + (*(uint8_t*) (ptr+i+1)) << 8 | + (*(uint8_t*) (ptr+i+2)) << 16 | + (*(uint8_t*) (ptr+i+3)) << 24; + } + } else { + /* calc in 32bit */ + for (i=0; i<(size & ~3); i+=4) { + CRC_DR = *(uint32_t*) (ptr+i); + } + } + + /* remaining bytes */ + switch (size % 4) { + case 1: + CRC_DR = *(uint8_t*) (ptr+i); + break; + case 2: + CRC_DR = (*(uint8_t*) (ptr+i)) | + (*(uint8_t*) (ptr+i+1)) << 8; + break; + case 3: + CRC_DR = (*(uint8_t*) (ptr+i)) | + (*(uint8_t*) (ptr+i+1)) << 8 | + (*(uint8_t*) (ptr+i+2)) << 16; + break; + default: + break; + } + + return CRC_DR; +} + +static int32_t flash_detect(struct FlashInfo* flash) { + uint32_t device_id; + + flash->total_size = FLASH_SIZE_ * 0x400; + +#if 1 + /* FIXME This will not work for connectivity line (needs ID, see below), but + device ID is only readable when freshly loaded through JTAG?! */ + + switch (flash->total_size) { + /* low density */ + case 0x00004000: /* 16 kBytes */ + case 0x00008000: /* 32 kBytes */ + /* medium density, e.g. STM32F103RBT6 (Olimex STM32-H103) */ + case 0x00010000: /* 64 kBytes */ + case 0x00200000: /* 128 kBytes */ + { + flash->page_size = 0x400; + break; + } + /* high density, e.g. STM32F103RE (Joby Lisa/M, Lisa/L) */ + case 0x00040000: /* 256 kBytes */ + case 0x00080000: /* 512 kBytes */ + /* XL density */ + case 0x000C0000: /* 768 kBytes */ + case 0x00100000: /* 1 MByte */ + { + flash->page_size = 0x800; + break; + } + default: {return -1;} + } + +#else /* this is the correct way of detecting page sizes */ + + /* read device id */ + device_id = DBGMCU_IDCODE & DBGMCU_IDCODE_DEV_ID_MASK; + + switch (device_id) { + /* low density */ + case 0x412: + /* medium density, e.g. STM32F103RB (Olimex STM32-H103) */ + case 0x410: + { + flash->page_size = 0x400; + break; + } + /* high density, e.g. STM32F103RE (Joby Lisa/L) */ + case 0x414: + /* XL density */ + case 0x430: + /* connectivity line */ + case 0x418: + { + flash->page_size = 0x800; + break; + } + default: return -1; + } + + switch (flash->total_size) { + case 0x00004000: /* 16 kBytes */ + case 0x00008000: /* 32 kBytes */ + case 0x00010000: /* 64 kBytes */ + case 0x00200000: /* 128 kBytes */ + case 0x00040000: /* 256 kBytes */ + case 0x00080000: /* 512 kBytes */ + case 0x000C0000: /* 768 kBytes */ + case 0x00100000: /* 1 MByte */ + break; + default: return -1; + } +#endif + + flash->page_nr = (flash->total_size / flash->page_size) - 1; + flash->addr = FLASH_BEGIN + flash->page_nr * flash->page_size; + + return 0; +} + +// (gdb) p *flash +// $1 = {addr = 134739968, total_size = 524288, page_nr = 255, page_size = 2048} +// 0x807F800 0x80000 + +static int32_t pflash_program_bytes(struct FlashInfo* flash, + uint32_t src, + uint32_t size, + uint32_t chksum) { + uint32_t i; + + /* erase */ + flash_unlock(); + flash_erase_page(flash->addr); + flash_lock(); + + /* verify erase */ + for (i=0; ipage_size; i+=4) { + if ((*(uint32_t*) (flash->addr + i)) != 0xFFFFFFFF) return -1; + } + + flash_unlock(); + /* write full 16 bit words */ + for (i=0; i<(size & ~1); i+=2) { + flash_program_half_word(flash->addr+i, + (uint16_t)(*(uint8_t*)(src+i) | (*(uint8_t*)(src+i+1)) << 8)); + } + /* fill bytes with a zero */ + if (size & 1) { + flash_program_half_word(flash->addr+i, (uint16_t)(*(uint8_t*)(src+i))); + } + /* write size */ + flash_program_half_word(flash->addr+flash->page_size-FSIZ, + (uint16_t)(size & 0xFFFF)); + flash_program_half_word(flash->addr+flash->page_size-FSIZ+2, + (uint16_t)((size >> 16) & 0xFFFF)); + /* write checksum */ + flash_program_half_word(flash->addr+flash->page_size-FCHK, + (uint16_t)(chksum & 0xFFFF)); + flash_program_half_word(flash->addr+flash->page_size-FCHK+2, + (uint16_t)((chksum >> 16) & 0xFFFF)); + flash_lock(); + + /* verify data */ + for (i=0; iaddr+i)) != (*(uint8_t*) (src+i))) return -2; + } + if (*(uint32_t*) (flash->addr+flash->page_size-FSIZ) != size) return -3; + if (*(uint32_t*) (flash->addr+flash->page_size-FCHK) != chksum) return -4; + + return 0; +} + +int32_t persistent_write(uint32_t ptr, uint32_t size) { + struct FlashInfo flash_info; + if (flash_detect(&flash_info)) return -1; + if ((size > flash_info.page_size-FSIZ) || (size == 0)) return -2; + + return pflash_program_bytes(&flash_info, + ptr, + size, + pflash_checksum(ptr, size)); +} + +int32_t persistent_read(uint32_t ptr, uint32_t size) { + struct FlashInfo flash; + uint32_t i; + + /* check parameters */ + if (flash_detect(&flash)) return -1; + if ((size > flash.page_size-FSIZ) || (size == 0)) return -2; + + /* check consistency */ + if (size != *(uint32_t*)(flash.addr+flash.page_size-FSIZ)) return -3; + if (pflash_checksum(flash.addr, size) != + *(uint32_t*)(flash.addr+flash.page_size-FCHK)) + return -4; + + /* copy data */ + for (i=0; i + * + * This file is part of Paparazzi. + * + * Paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * Paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + * + */ + + +#ifndef STM32_SUBSYSTEMS_SETTINGS_H +#define STM32_SUBSYSTEMS_SETTINGS_H + + + +#endif /* STM32_SUBSYSTEMS_SETTINGS_H */ diff --git a/sw/airborne/boards/lisa_m/baro_board.c b/sw/airborne/boards/lisa_m/baro_board.c index 11829b5acf..8e686c004e 100644 --- a/sw/airborne/boards/lisa_m/baro_board.c +++ b/sw/airborne/boards/lisa_m/baro_board.c @@ -1,63 +1,143 @@ #include "subsystems/sensors/baro.h" +#include struct Baro baro; struct BaroBoard baro_board; struct i2c_transaction baro_trans; +struct bmp085_baro_calibration calibration; +#define BMP085_SAMPLE_PERIOD_MS (3 + (2 << BMP085_OSS) * 3) +#define BMP085_SAMPLE_PERIOD (BMP075_SAMPLE_PERIOD_MS >> 1) -static inline void baro_board_write_to_register(uint8_t baro_addr, uint8_t reg_addr, uint8_t val_msb, uint8_t val_lsb); -static inline void baro_board_read_from_register(uint8_t baro_addr, uint8_t reg_addr); -static inline void baro_board_set_current_register(uint8_t baro_addr, uint8_t reg_addr); -static inline void baro_board_read_from_current_register(uint8_t baro_addr); +// FIXME: BARO DRDY connected to PB0 for lisa/m -// absolute -#define BARO_ABS_ADDR 0x90 -// differential -#define BARO_DIFF_ADDR 0x92 +static inline void bmp085_write_reg(uint8_t addr, uint8_t value) +{ + baro_trans.type = I2CTransTx; + baro_trans.slave_addr = BMP085_ADDR; + baro_trans.len_w = 2; + baro_trans.buf[0] = addr; + baro_trans.buf[1] = value; + i2c_submit(&i2c2, &baro_trans); + while (baro_trans.status == I2CTransPending || baro_trans.status == I2CTransRunning); +} + +static inline void bmp085_read_reg16(uint8_t addr) +{ + baro_trans.type = I2CTransTxRx; + baro_trans.slave_addr = BMP085_ADDR; + baro_trans.len_w = 1; + baro_trans.len_r = 2; + baro_trans.buf[0] = addr; + i2c_submit(&i2c2, &baro_trans); +} + +static inline int16_t bmp085_read_reg16_blocking(uint8_t addr) +{ + bmp085_read_reg16(addr); + + while (baro_trans.status == I2CTransPending || baro_trans.status == I2CTransRunning); + + return ((baro_trans.buf[0] << 8) | baro_trans.buf[1]); +} + +static inline void bmp085_read_reg24(uint8_t addr) +{ + baro_trans.type = I2CTransTxRx; + baro_trans.slave_addr = BMP085_ADDR; + baro_trans.len_w = 1; + baro_trans.len_r = 3; + baro_trans.buf[0] = addr; + i2c_submit(&i2c2, &baro_trans); +} + +static void bmp085_baro_read_calibration(void) +{ + calibration.ac1 = bmp085_read_reg16_blocking(0xAA); // AC1 + calibration.ac2 = bmp085_read_reg16_blocking(0xAC); // AC2 + calibration.ac3 = bmp085_read_reg16_blocking(0xAE); // AC3 + calibration.ac4 = bmp085_read_reg16_blocking(0xB0); // AC4 + calibration.ac5 = bmp085_read_reg16_blocking(0xB2); // AC5 + calibration.ac6 = bmp085_read_reg16_blocking(0xB4); // AC6 + calibration.b1 = bmp085_read_reg16_blocking(0xB6); // B1 + calibration.b2 = bmp085_read_reg16_blocking(0xB8); // B2 + calibration.mb = bmp085_read_reg16_blocking(0xBA); // MB + calibration.mc = bmp085_read_reg16_blocking(0xBC); // MC + calibration.md = bmp085_read_reg16_blocking(0xBE); // MD +} void baro_init(void) { baro.status = BS_UNINITIALIZED; baro.absolute = 0; baro.differential = 0; baro_board.status = LBS_UNINITIALIZED; + bmp085_baro_read_calibration(); + + /* STM32 specific (maybe this is a LISA/M specific driver anyway?) */ + GPIO_InitTypeDef GPIO_InitStructure; + GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0; + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU; + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; + GPIO_Init(GPIOB, &GPIO_InitStructure); } +static inline int baro_eoc(void) +{ + return GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_0); +} + +static inline void bmp085_request_pressure(void) +{ + bmp085_write_reg(0xF4, 0x34 + (BMP085_OSS << 6)); +} + +static inline void bmp085_request_temp(void) +{ + bmp085_write_reg(0xF4, 0x2E); +} + +static inline void bmp085_read_pressure(void) +{ + bmp085_read_reg24(0xF6); +} + +static inline void bmp085_read_temp(void) +{ + bmp085_read_reg16(0xF6); +} void baro_periodic(void) { - // check i2c_done + // check that nothing is in progress + if (baro_trans.status == I2CTransPending) return; + if (baro_trans.status == I2CTransRunning) return; if (!i2c_idle(&i2c2)) return; + switch (baro_board.status) { case LBS_UNINITIALIZED: baro_board_send_reset(); - baro_board.status = LBS_RESETED; - break; - case LBS_RESETED: - baro_board_send_config_abs(); - baro_board.status = LBS_INITIALIZING_ABS; - break; - case LBS_INITIALIZING_ABS: - baro_board_set_current_register(BARO_ABS_ADDR, 0x00); - baro_board.status = LBS_INITIALIZING_ABS_1; - break; - case LBS_INITIALIZING_ABS_1: - baro_board_send_config_diff(); - baro_board.status = LBS_INITIALIZING_DIFF; - break; - case LBS_INITIALIZING_DIFF: - baro_board_set_current_register(BARO_DIFF_ADDR, 0x00); - baro_board.status = LBS_INITIALIZING_DIFF_1; - // baro_board.status = LBS_UNINITIALIZED; - break; - case LBS_INITIALIZING_DIFF_1: + baro_board.status = LBS_REQUEST; baro.status = BS_RUNNING; - case LBS_READ_DIFF: - baro_board_read_from_current_register(BARO_ABS_ADDR); - baro_board.status = LBS_READING_ABS; break; - case LBS_READ_ABS: - baro_board_read_from_current_register(BARO_DIFF_ADDR); - baro_board.status = LBS_READING_DIFF; + case LBS_REQUEST: + bmp085_request_pressure(); + baro_board.status = LBS_READ; + break; + case LBS_READ: + if (baro_eoc()) { + bmp085_read_pressure(); + baro_board.status = LBS_READING; + } + break; + case LBS_REQUEST_TEMP: + bmp085_request_temp(); + baro_board.status = LBS_READ_TEMP; + break; + case LBS_READ_TEMP: + if (baro_eoc()) { + bmp085_read_temp(); + baro_board.status = LBS_READING_TEMP; + } break; default: break; @@ -65,58 +145,6 @@ void baro_periodic(void) { } - -void baro_board_send_config_abs(void) { - baro_board_write_to_register(BARO_ABS_ADDR, 0x01, 0x86, 0x83); -} - -void baro_board_send_config_diff(void) { - baro_board_write_to_register(BARO_DIFF_ADDR, 0x01, 0x84, 0x83); -} - void baro_board_send_reset(void) { - baro_trans.type = I2CTransTx; - baro_trans.slave_addr = 0x00; - baro_trans.len_w = 1; - baro_trans.buf[0] = 0x06; - i2c_submit(&i2c2,&baro_trans); -} - -static inline void baro_board_write_to_register(uint8_t baro_addr, uint8_t reg_addr, uint8_t val_msb, uint8_t val_lsb) { - baro_trans.type = I2CTransTx; - baro_trans.slave_addr = baro_addr; - baro_trans.len_w = 3; - baro_trans.buf[0] = reg_addr; - baro_trans.buf[1] = val_msb; - baro_trans.buf[2] = val_lsb; - i2c_submit(&i2c2,&baro_trans); -} - -static inline void baro_board_read_from_register(uint8_t baro_addr, uint8_t reg_addr) { - baro_trans.type = I2CTransTxRx; - baro_trans.slave_addr = baro_addr; - baro_trans.len_w = 1; - baro_trans.len_r = 2; - baro_trans.buf[0] = reg_addr; - i2c_submit(&i2c2,&baro_trans); - // i2c2.buf[0] = reg_addr; - // i2c2_transceive(baro_addr, 1, 2, &baro_board.i2c_done); -} - -static inline void baro_board_set_current_register(uint8_t baro_addr, uint8_t reg_addr) { - baro_trans.type = I2CTransTx; - baro_trans.slave_addr = baro_addr; - baro_trans.len_w = 1; - baro_trans.buf[0] = reg_addr; - i2c_submit(&i2c2,&baro_trans); - // i2c2.buf[0] = reg_addr; - // i2c2_transmit(baro_addr, 1, &baro_board.i2c_done); -} - -static inline void baro_board_read_from_current_register(uint8_t baro_addr) { - baro_trans.type = I2CTransRx; - baro_trans.slave_addr = baro_addr; - baro_trans.len_r = 2; - i2c_submit(&i2c2,&baro_trans); - // i2c2_receive(baro_addr, 2, &baro_board.i2c_done); + // This is a NOP at the moment } diff --git a/sw/airborne/boards/lisa_m/baro_board.h b/sw/airborne/boards/lisa_m/baro_board.h index e2d61a79db..cbe3454983 100644 --- a/sw/airborne/boards/lisa_m/baro_board.h +++ b/sw/airborne/boards/lisa_m/baro_board.h @@ -4,59 +4,103 @@ * */ -#ifndef BOARDS_LISA_L_BARO_H -#define BOARDS_LISA_L_BARO_H +#ifndef BOARDS_LISA_M_BARO_H +#define BOARDS_LISA_M_BARO_H #include "std.h" #include "mcu_periph/i2c.h" +// absolute addr +#define BMP085_ADDR 0xEE +// Over sample setting (0-3) +#define BMP085_OSS 3 + enum LisaBaroStatus { LBS_UNINITIALIZED, - LBS_RESETED, - LBS_INITIALIZING_ABS, - LBS_INITIALIZING_ABS_1, - LBS_INITIALIZING_DIFF, - LBS_INITIALIZING_DIFF_1, - LBS_IDLE, - LBS_READING_ABS, - LBS_READ_ABS, - LBS_READING_DIFF, - LBS_READ_DIFF + LBS_REQUEST, + LBS_READING, + LBS_READ, + LBS_REQUEST_TEMP, + LBS_READING_TEMP, + LBS_READ_TEMP, }; struct BaroBoard { enum LisaBaroStatus status; }; +struct bmp085_baro_calibration { + // These values come from EEPROM on sensor + int16_t ac1; + int16_t ac2; + int16_t ac3; + uint16_t ac4; + uint16_t ac5; + uint16_t ac6; + int16_t b1; + int16_t b2; + int16_t mb; + int16_t mc; + int16_t md; + + // These values are calculated + int32_t b5; +}; + extern struct BaroBoard baro_board; extern struct i2c_transaction baro_trans; +extern struct bmp085_baro_calibration calibration; extern void baro_board_send_reset(void); -extern void baro_board_send_config_abs(void); -extern void baro_board_send_config_diff(void); +extern void baro_board_send_config(void); -#define BaroEvent(_b_abs_handler, _b_diff_handler) { \ - if (baro_board.status == LBS_READING_ABS && \ - baro_trans.status != I2CTransPending) { \ - baro_board.status = LBS_READ_ABS; \ - if (baro_trans.status == I2CTransSuccess) { \ - int16_t tmp = baro_trans.buf[0]<<8 | baro_trans.buf[1]; \ - baro.absolute = tmp; \ - _b_abs_handler(); \ - } \ - } \ - else if (baro_board.status == LBS_READING_DIFF && \ - baro_trans.status != I2CTransPending) { \ - baro_board.status = LBS_READ_DIFF; \ - if (baro_trans.status == I2CTransSuccess) { \ - int16_t tmp = baro_trans.buf[0]<<8 | baro_trans.buf[1]; \ - baro.differential = tmp; \ - _b_diff_handler(); \ - } \ - } \ +// Apply temp calibration and sensor calibration to raw measurement to get Pa (from BMP085 datasheet) +static inline int32_t baro_apply_calibration(int32_t raw) +{ + int32_t b6 = calibration.b5 - 4000; + int x1 = (calibration.b2 * (b6 * b6 >> 12)) >> 11; + int x2 = calibration.ac2 * b6 >> 11; + int32_t x3 = x1 + x2; + int32_t b3 = (((calibration.ac1 * 4 + x3) << BMP085_OSS) + 2)/4; + x1 = calibration.ac3 * b6 >> 13; + x2 = (calibration.b1 * (b6 * b6 >> 12)) >> 16; + x3 = ((x1 + x2) + 2) >> 2; + uint32_t b4 = (calibration.ac4 * (uint32_t) (x3 + 32768)) >> 15; + uint32_t b7 = (raw - b3) * (50000 >> BMP085_OSS); + int32_t p = b7 < 0x80000000 ? (b7 * 2) / b4 : (b7 / b4) * 2; + x1 = (p >> 8) * (p >> 8); + x1 = (x1 * 3038) >> 16; + x2 = (-7357 * p) >> 16; + return p + ((x1 + x2 + 3791) >> 4); +} + +static inline void baro_event(void (*b_abs_handler)(void), void (*b_diff_handler)(void)) +{ + if (baro_board.status == LBS_READING && + baro_trans.status != I2CTransPending && baro_trans.status != I2CTransRunning) { + baro_board.status = LBS_REQUEST_TEMP; + if (baro_trans.status == I2CTransSuccess) { + int32_t tmp = (baro_trans.buf[0]<<16) | (baro_trans.buf[1] << 8) | baro_trans.buf[2]; + tmp = tmp >> (8 - BMP085_OSS); + baro.absolute = baro_apply_calibration(tmp); + b_abs_handler(); + } } + if (baro_board.status == LBS_READING_TEMP && + baro_trans.status != I2CTransPending && baro_trans.status != I2CTransRunning) { + baro_board.status = LBS_REQUEST; + if (baro_trans.status == I2CTransSuccess) { + // abuse differential to store temp in 0.1C for now + int32_t tmp = (baro_trans.buf[0] << 8) | baro_trans.buf[1]; + int32_t x1 = ((tmp - calibration.ac6) * calibration.ac5) >> 15; + int32_t x2 = (calibration.mc << 11) / (x1 + calibration.md); + calibration.b5 = x1 + x2; + baro.differential = (calibration.b5 + 8) >> 4; + b_diff_handler(); + } + } +} +#define BaroEvent(_b_abs_handler, _b_diff_handler) baro_event(_b_abs_handler,_b_diff_handler) - - -#endif /* BOARDS_LISA_L_BARO_H */ +#endif /* BOARDS_LISA_M_BARO_H */ diff --git a/sw/airborne/boards/lisa_m/test_baro.c b/sw/airborne/boards/lisa_m/test_baro.c new file mode 100644 index 0000000000..8a24041f53 --- /dev/null +++ b/sw/airborne/boards/lisa_m/test_baro.c @@ -0,0 +1,108 @@ +/* + * $Id$ + * + * Copyright (C) 2009 Antoine Drouin + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/* + * + * test baro using interrupts + * + */ + +#include BOARD_CONFIG + +#include "mcu.h" +#include "sys_time.h" +#include "mcu_periph/uart.h" + +#include "downlink.h" + +#include "subsystems/sensors/baro.h" +//#include "my_debug_servo.h" + +static inline void main_init( void ); +static inline void main_periodic_task( void ); +static inline void main_event_task( void ); + +static inline void main_on_baro_diff(void); +static inline void main_on_baro_abs(void); + + +int main(void) { + main_init(); + + while(1) { + if (sys_time_periodic()) + main_periodic_task(); + main_event_task(); + } + + return 0; +} + +static inline void main_init( void ) { + mcu_init(); + sys_time_init(); + baro_init(); + + // DEBUG_SERVO1_INIT(); + // DEBUG_SERVO2_INIT(); + + +} + + + +static inline void main_periodic_task( void ) { + + RunOnceEvery(2, {baro_periodic();}); + LED_PERIODIC(); + RunOnceEvery(256, {DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);}); + RunOnceEvery(256, + { + DOWNLINK_SEND_I2C_ERRORS(DefaultChannel, + &i2c2_errors.ack_fail_cnt, + &i2c2_errors.miss_start_stop_cnt, + &i2c2_errors.arb_lost_cnt, + &i2c2_errors.over_under_cnt, + &i2c2_errors.pec_recep_cnt, + &i2c2_errors.timeout_tlow_cnt, + &i2c2_errors.smbus_alert_cnt, + &i2c2_errors.unexpected_event_cnt, + &i2c2_errors.last_unexpected_event); + }); +} + + + +static inline void main_event_task( void ) { + BaroEvent(main_on_baro_abs, main_on_baro_diff); +} + + + +static inline void main_on_baro_diff(void) { + +} + +static inline void main_on_baro_abs(void) { + RunOnceEvery(5,{DOWNLINK_SEND_BARO_RAW(DefaultChannel, &baro.absolute, &baro.differential);}); +} diff --git a/sw/airborne/booz/test/booz2_test_crista.c b/sw/airborne/booz/test/booz2_test_crista.c index 82f990afe4..1231b7e6ab 100644 --- a/sw/airborne/booz/test/booz2_test_crista.c +++ b/sw/airborne/booz/test/booz2_test_crista.c @@ -108,11 +108,11 @@ static inline void on_imu_event(void) { &imu_accel_unscaled.z); } else if (cnt == 7) { - DOWNLINK_SEND_BOOZ2_GYRO(&imu_gyro.x, + DOWNLINK_SEND_IMU_GYRO_SCALED(&imu_gyro.x, &imu_gyro.y, &imu_gyro.z); - DOWNLINK_SEND_BOOZ2_ACCEL(&imu_accel.x, + DOWNLINK_SEND_IMU_ACCEL_SCALED(&imu_accel.x, &imu_accel.y, &imu_accel.z); } diff --git a/sw/airborne/booz/test/booz_test_imu.c b/sw/airborne/booz/test/booz_test_imu.c index 262013388a..19146b63cb 100644 --- a/sw/airborne/booz/test/booz_test_imu.c +++ b/sw/airborne/booz/test/booz_test_imu.c @@ -118,7 +118,7 @@ static inline void on_accel_event(void) { &imu.accel_unscaled.z); } else if (cnt == 7) { - DOWNLINK_SEND_BOOZ2_ACCEL(DefaultChannel, + DOWNLINK_SEND_IMU_ACCEL_SCALED(DefaultChannel, &imu.accel.x, &imu.accel.y, &imu.accel.z); @@ -140,7 +140,7 @@ static inline void on_gyro_accel_event(void) { &imu.gyro_unscaled.r); } else if (cnt == 7) { - DOWNLINK_SEND_BOOZ2_GYRO(DefaultChannel, + DOWNLINK_SEND_IMU_GYRO_SCALED(DefaultChannel, &imu.gyro.p, &imu.gyro.q, &imu.gyro.r); @@ -155,7 +155,7 @@ static inline void on_mag_event(void) { if (cnt > 10) cnt = 0; if (cnt == 0) { - DOWNLINK_SEND_BOOZ2_MAG(DefaultChannel, + DOWNLINK_SEND_IMU_MAG_SCALED(DefaultChannel, &imu.mag.x, &imu.mag.y, &imu.mag.z); diff --git a/sw/airborne/csc/mercury_xsens.h b/sw/airborne/csc/mercury_xsens.h index 9b04d79737..a2f1857a97 100644 --- a/sw/airborne/csc/mercury_xsens.h +++ b/sw/airborne/csc/mercury_xsens.h @@ -64,20 +64,20 @@ extern int xsens_setzero; #include "subsystems/ahrs.h" -#define PERIODIC_SEND_BOOZ2_GYRO() { \ - DOWNLINK_SEND_BOOZ2_GYRO(&imu.gyro.p, \ +#define PERIODIC_SEND_IMU_GYRO_SCALED() { \ + DOWNLINK_SEND_IMU_GYRO_SCALED(&imu.gyro.p, \ &imu.gyro.q, \ &imu.gyro.r); \ } -#define PERIODIC_SEND_BOOZ2_ACCEL() { \ - DOWNLINK_SEND_BOOZ2_ACCEL(&imu.accel.x, \ +#define PERIODIC_SEND_IMU_ACCEL_SCALED() { \ + DOWNLINK_SEND_IMU_ACCEL_SCALED(&imu.accel.x, \ &imu.accel.y, \ &imu.accel.z); \ } -#define PERIODIC_SEND_BOOZ2_MAG() { \ - DOWNLINK_SEND_BOOZ2_MAG(&imu.mag.x, \ +#define PERIODIC_SEND_IMU_MAG_SCALED() { \ + DOWNLINK_SEND_IMU_MAG_SCALED(&imu.mag.x, \ &imu.mag.y, \ &imu.mag.z); \ } diff --git a/sw/airborne/downlink.c b/sw/airborne/downlink.c index dbd07749aa..003b816beb 100644 --- a/sw/airborne/downlink.c +++ b/sw/airborne/downlink.c @@ -29,6 +29,7 @@ #include "std.h" +#include "generated/airframe.h" #ifdef FBW #ifndef TELEMETRY_MODE_FBW diff --git a/sw/airborne/downlink.h b/sw/airborne/downlink.h index 17052669ed..26dcab2988 100644 --- a/sw/airborne/downlink.h +++ b/sw/airborne/downlink.h @@ -51,6 +51,9 @@ #include "pprz_transport.h" #include "modem.h" #include "xbee.h" +#ifdef USE_USB_SERIAL +#include "mcu_periph/usb_serial.h" +#endif #endif /** !SITL */ #ifndef DefaultChannel diff --git a/sw/airborne/firmwares/beth/main_overo.c b/sw/airborne/firmwares/beth/main_overo.c index 6a52bc090a..b24958321f 100644 --- a/sw/airborne/firmwares/beth/main_overo.c +++ b/sw/airborne/firmwares/beth/main_overo.c @@ -159,13 +159,13 @@ static void main_periodic(int my_sig_num) { RunOnceEvery(10, {DOWNLINK_SEND_IMU_ACCEL_RAW(gcs_com.udp_transport, //&msg_in.payload.msg_up.accel.x,&msg_in.payload.msg_up.accel.y,&msg_in.payload.msg_up.accel.z &imu.accel_unscaled.x,&imu.accel_unscaled.y,&imu.accel_unscaled.z);}) - RunOnceEvery(50, {DOWNLINK_SEND_BOOZ2_GYRO(gcs_com.udp_transport, + RunOnceEvery(50, {DOWNLINK_SEND_IMU_GYRO_SCALED(gcs_com.udp_transport, //&msg_in.payload.msg_up.gyro.p,&msg_in.payload.msg_up.gyro.q,&msg_in.payload.msg_up.gyro.r) &imu.gyro.p,&imu.gyro.q,&imu.gyro.r);}); RunOnceEvery(50, {DOWNLINK_SEND_AHRS_EULER(gcs_com.udp_transport, &estimator.tilt, &estimator.elevation, &estimator.azimuth );}); - RunOnceEvery(50, {DOWNLINK_SEND_BOOZ2_ACCEL(DefaultChannel, + RunOnceEvery(50, {DOWNLINK_SEND_IMU_ACCEL_SCALED(DefaultChannel, //&msg_in.payload.msg_up.accel.x,&msg_in.payload.msg_up.accel.y,&msg_in.payload.msg_up.accel.z &imu.accel.x,&imu.accel.y,&imu.accel.z);});*/ diff --git a/sw/airborne/firmwares/beth/main_stm32.c b/sw/airborne/firmwares/beth/main_stm32.c index ee5aca10d2..af09ab6a0a 100644 --- a/sw/airborne/firmwares/beth/main_stm32.c +++ b/sw/airborne/firmwares/beth/main_stm32.c @@ -176,12 +176,12 @@ static inline void on_gyro_accel_event(void) { &imu.accel_unscaled.z); } else if (cnt == 7) { - DOWNLINK_SEND_BOOZ2_GYRO(DefaultChannel, + DOWNLINK_SEND_IMU_GYRO_SCALED(DefaultChannel, &imu.gyro.p, &imu.gyro.q, &imu.gyro.r); - DOWNLINK_SEND_BOOZ2_ACCEL(DefaultChannel, + DOWNLINK_SEND_IMU_ACCEL_SCALED(DefaultChannel, &imu.accel.x, &imu.accel.y, &imu.accel.z); @@ -196,7 +196,7 @@ static inline void on_mag_event(void) { if (cnt > 1) cnt = 0; if (cnt%2) { - DOWNLINK_SEND_BOOZ2_MAG(DefaultChannel, + DOWNLINK_SEND_IMU_MAG_SCALED(DefaultChannel, &imu.mag.x, &imu.mag.y, &imu.mag.z); diff --git a/sw/airborne/firmwares/fixedwing/datalink.c b/sw/airborne/firmwares/fixedwing/datalink.c index cc0320864c..1178a0ef0b 100644 --- a/sw/airborne/firmwares/fixedwing/datalink.c +++ b/sw/airborne/firmwares/fixedwing/datalink.c @@ -47,10 +47,6 @@ #include "joystick.h" #endif -#ifdef USE_USB_SERIAL -#include "mcu_periph/usb_serial.h" -#endif - #ifdef HITL #include "gps.h" #endif diff --git a/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c b/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c index 02ecaaf826..fcc14344f1 100644 --- a/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c +++ b/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c @@ -198,7 +198,7 @@ static inline void v_ctl_set_pitch ( void ) { if (v_ctl_auto_pitch_igain < 0.) { v_ctl_auto_pitch_sum_err += err*(1./60.); - BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR / v_ctl_auto_pitch_igain); + BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR / (-v_ctl_auto_pitch_igain)); } // PI loop + feedforward ctl @@ -223,7 +223,7 @@ static inline void v_ctl_set_throttle( void ) { if (v_ctl_auto_throttle_igain < 0.) { v_ctl_auto_throttle_sum_err += err*(1./60.); - BoundAbs(v_ctl_auto_throttle_sum_err, V_CTL_AUTO_THROTTLE_MAX_SUM_ERR / v_ctl_auto_throttle_igain); + BoundAbs(v_ctl_auto_throttle_sum_err, V_CTL_AUTO_THROTTLE_MAX_SUM_ERR / (-v_ctl_auto_throttle_igain)); } // PID loop + feedforward ctl diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c index 9ff099da95..5f571449e1 100644 --- a/sw/airborne/firmwares/fixedwing/main_ap.c +++ b/sw/airborne/firmwares/fixedwing/main_ap.c @@ -52,6 +52,7 @@ #include "sys_time.h" #include "generated/flight_plan.h" #include "datalink.h" +#include "subsystems/settings.h" #include "xbee.h" #include "gpio.h" @@ -551,6 +552,8 @@ void init_ap( void ) { modules_init(); + settings_init(); + /** - start interrupt task */ mcu_int_enable(); diff --git a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/ADC.pde b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/ADC.pde index 3d4728d168..9f8473b7e2 100644 --- a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/ADC.pde +++ b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/ADC.pde @@ -31,29 +31,9 @@ float read_adc(int select) float temp; if (SENSOR_SIGN[select]<0){ temp = (AN_OFFSET[select]-AN[select]); - if (abs(temp)>900) { -#if PRINT_DEBUG != 0 - Serial.print("!!!ADC:1,VAL:"); - Serial.print (temp); - Serial.println("***"); -#endif -#if PERFORMANCE_REPORTING == 1 - adc_constraints++; -#endif - } return constrain(temp,-900,900); //Throw out nonsensical values } else { temp = (AN[select]-AN_OFFSET[select]); - if (abs(temp)>900) { -#if PRINT_DEBUG != 0 - Serial.print("!!!ADC:2,VAL:"); - Serial.print (temp); - Serial.println("***"); -#endif -#if PERFORMANCE_REPORTING == 1 - adc_constraints++; -#endif - } return constrain(temp,-900,900); } } @@ -61,8 +41,8 @@ float read_adc(int select) //Activating the ADC interrupts. void Analog_Init(void) { - ADCSRA|=(1<=8) MuxSel=0; diff --git a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/DCM.pde b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/DCM.pde index 305c2bac57..ed30a70190 100644 --- a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/DCM.pde +++ b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/DCM.pde @@ -21,27 +21,13 @@ void Normalize(void) renorm= .5 * (3-renorm); //eq.21 } else if (renorm < 100.0f && renorm > 0.01f) { renorm= 1. / sqrt(renorm); -#if PERFORMANCE_REPORTING == 1 +#if PERFORMANCE_REPORTING == 1 renorm_sqrt_count++; -#endif -#if PRINT_DEBUG != 0 - Serial.print("???SQT:1,RNM:"); - Serial.print (renorm); - Serial.print (",ERR:"); - Serial.print (error); - Serial.println("***"); #endif } else { problem = TRUE; #if PERFORMANCE_REPORTING == 1 renorm_blowup_count++; -#endif -#if PRINT_DEBUG != 0 - Serial.print("???PRB:1,RNM:"); - Serial.print (renorm); - Serial.print (",ERR:"); - Serial.print (error); - Serial.println("***"); #endif } Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm); @@ -53,25 +39,11 @@ void Normalize(void) renorm= 1. / sqrt(renorm); #if PERFORMANCE_REPORTING == 1 renorm_sqrt_count++; -#endif -#if PRINT_DEBUG != 0 - Serial.print("???SQT:2,RNM:"); - Serial.print (renorm); - Serial.print (",ERR:"); - Serial.print (error); - Serial.println("***"); #endif } else { problem = TRUE; #if PERFORMANCE_REPORTING == 1 renorm_blowup_count++; -#endif -#if PRINT_DEBUG != 0 - Serial.print("???PRB:2,RNM:"); - Serial.print (renorm); - Serial.print (",ERR:"); - Serial.print (error); - Serial.println("***"); #endif } Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm); @@ -83,23 +55,11 @@ void Normalize(void) renorm= 1. / sqrt(renorm); #if PERFORMANCE_REPORTING == 1 renorm_sqrt_count++; -#endif -#if PRINT_DEBUG != 0 - Serial.print("???SQT:3,RNM:"); - Serial.print (renorm); - Serial.print (",ERR:"); - Serial.print (error); - Serial.println("***"); #endif } else { problem = TRUE; #if PERFORMANCE_REPORTING == 1 renorm_blowup_count++; -#endif -#if PRINT_DEBUG != 0 - Serial.print("???PRB:3,RNM:"); - Serial.print (renorm); - Serial.println("***"); #endif } Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm); @@ -170,23 +130,18 @@ void Drift_correction(void) Integrator_magnitude = sqrt(Vector_Dot_Product(Omega_I,Omega_I)); if (Integrator_magnitude > ToRad(300)) { Vector_Scale(Omega_I,Omega_I,0.5f*ToRad(300)/Integrator_magnitude); -#if PRINT_DEBUG != 0 - Serial.print("!!!INT:1,MAG:"); - Serial.print (ToDeg(Integrator_magnitude)); - - Serial.println("***"); -#endif } } + /**************************************************/ void Accel_adjust(void) { Accel_Vector[1] += Accel_Scale(speed_3d*Omega[2]); // Centrifugal force on Acc_y = GPS_speed*GyroZ Accel_Vector[2] -= Accel_Scale(speed_3d*Omega[1]); // Centrifugal force on Acc_z = GPS_speed*GyroY } -/**************************************************/ +/**************************************************/ void Matrix_update(void) { Gyro_Vector[0]=Gyro_Scaled_X(read_adc(0)); //gyro x roll @@ -202,7 +157,6 @@ void Matrix_update(void) Accel_adjust(); //Remove centrifugal acceleration. -#if OUTPUTMODE==1 Update_Matrix[0][0]=0; Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y @@ -212,17 +166,6 @@ void Matrix_update(void) Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x Update_Matrix[2][2]=0; -#else // Uncorrected data (no drift correction) - Update_Matrix[0][0]=0; - Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z - Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y - Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z - Update_Matrix[1][1]=0; - Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0]; - Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1]; - Update_Matrix[2][1]=G_Dt*Gyro_Vector[0]; - Update_Matrix[2][2]=0; -#endif Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c @@ -235,16 +178,11 @@ void Matrix_update(void) } } +/**************************************************/ void Euler_angles(void) { -#if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes) - roll = atan2(Accel_Vector[1],Accel_Vector[2]); // atan2(acc_y,acc_z) - pitch = -asin((Accel_Vector[0])/(double)GRAVITY); // asin(acc_x) - yaw = 0; -#else pitch = -asin(DCM_Matrix[2][0]); roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]); yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]); -#endif } diff --git a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/Output.pde b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/Output.pde index 49170c774c..2ee09a3d7c 100644 --- a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/Output.pde +++ b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/Output.pde @@ -1,24 +1,28 @@ //*****I2C Output************************************************************ - -void requestEvent(){ -#if PRINT_DEBUG != 0 - Serial.println("Sending IMU Data"); -#endif - +void fill_I2C_message() { // Message Array : Roll; Pitch; YaW; GyroX; GyroY; GyroZ; ACCX; ACCY; ACCZ // Float Number is multipited with 10000 and converted to an Integer, for sending via I2C. // Resolution for angles: 12, rates: 12, accel:10 (from pprza BFP math lib) I2C_Message_ar[0] = int(roll*(1<<12)); I2C_Message_ar[1] = int(pitch*(1<<12)); I2C_Message_ar[2] = int(yaw*(1<<12)); - I2C_Message_ar[3] = int(Gyro_Vector[0]*(1<<12)); - I2C_Message_ar[4] = int(Gyro_Vector[1]*(1<<12)); - I2C_Message_ar[5] = int(Gyro_Vector[2]*(1<<12)); + I2C_Message_ar[3] = int(Omega_Vector[0]*(1<<12)); + I2C_Message_ar[4] = int(Omega_Vector[1]*(1<<12)); + I2C_Message_ar[5] = int(Omega_Vector[2]*(1<<12)); I2C_Message_ar[6] = int((9.81*Accel_Vector[0]/GRAVITY)*(1<<10)); I2C_Message_ar[7] = int((9.81*Accel_Vector[1]/GRAVITY)*(1<<10)); I2C_Message_ar[8] = int((9.81*Accel_Vector[2]/GRAVITY)*(1<<10)); +} + +void requestEvent(){ +#if PRINT_DEBUG != 0 + Serial.println("Sending IMU Data"); +#endif + + fill_I2C_message(); + byte* pointer; pointer = (byte*) &I2C_Message_ar; Wire.send(pointer, 18); @@ -51,6 +55,7 @@ void receiveEvent(int howMany){ void printdata(void){ #if PRINT_I2C_MSG == 1 + fill_I2C_message(); Serial.print("Time "); Serial.print(millis()); Serial.print("; Roll "); @@ -89,8 +94,7 @@ void printdata(void){ Serial.print (",AN4:"); Serial.print(read_adc(4)); Serial.print (",AN5:"); - Serial.print(read_adc(5)); - Serial.print (","); + Serial.println(read_adc(5)); #endif #if PRINT_DCM == 1 @@ -111,8 +115,7 @@ void printdata(void){ Serial.print (",EX7:"); Serial.print(convert_to_dec(DCM_Matrix[2][1])); Serial.print (",EX8:"); - Serial.print(convert_to_dec(DCM_Matrix[2][2])); - Serial.print (","); + Serial.println(convert_to_dec(DCM_Matrix[2][2])); #endif #if PRINT_EULER == 1 @@ -123,8 +126,7 @@ void printdata(void){ Serial.print(",YAW:"); Serial.print(ToDeg(yaw)); Serial.print(",IMUH:"); - Serial.print((imu_health>>8)&0xff); - Serial.print (","); + Serial.println((imu_health>>8)&0xff); #endif #if PRINT_GPS == 1 @@ -141,6 +143,7 @@ void printdata(void){ gps_messages_sent++; #endif } + Serial.println(""); #endif } @@ -174,7 +177,7 @@ void printPerfData(long time) Serial.print(gps_messages_sent,DEC); Serial.print(",imu:"); Serial.print((imu_health>>8),DEC); - Serial.print(",***"); + Serial.println(",***"); // Reset counters mainLoop_count = 0; diff --git a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/arduimu_basic.pde b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/arduimu_basic.pde index df41fd5778..7c5f550e0a 100644 --- a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/arduimu_basic.pde +++ b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/arduimu_basic.pde @@ -30,9 +30,6 @@ /*For debugging propurses*/ #define PRINT_DEBUG 0 //Will print Debug messages -//OUTPUTMODE=1 will print the corrected data, 0 will print uncorrected data of the gyros (with drift), 2 will print accelerometer only data -#define OUTPUTMODE 1 - #define PRINT_I2C_MSG 0 //Will print the I2C output buffer #define PRINT_DCM 0 //Will print the whole direction cosine matrix #define PRINT_ANALOGS 0 //Will print the analog raw data @@ -49,7 +46,7 @@ int I2C_Message_ar[9]; // *** NOTE! To use ArduIMU with ArduPilot you must select binary output messages (change to 1 here) -#define PRINT_BINARY 0 //Will print binary message and suppress ASCII messages (above) +#define PRINT_BINARY 1 //Will print binary message and suppress ASCII messages (above) // *** NOTE! Performance reporting is only supported for Ublox. Set to 0 for others #define PERFORMANCE_REPORTING 1 //Will include performance reports in the binary output ~ 1/2 min @@ -70,10 +67,9 @@ int I2C_Message_ar[9]; #define ToDeg(x) (x*57.2957795131) // *180/pi // LPR530 & LY530 Sensitivity (from datasheet) => 3.33mV/ยบ/s, 3.22mV/ADC step => 1.03 -// Tested values : 0.96,0.96,0.94 -#define Gyro_Gain_X 0.92 //X axis Gyro gain -#define Gyro_Gain_Y 0.92 //Y axis Gyro gain -#define Gyro_Gain_Z 0.94 //Z axis Gyro gain +#define Gyro_Gain_X 1.0 //X axis Gyro gain +#define Gyro_Gain_Y 1.0 //Y axis Gyro gain +#define Gyro_Gain_Z 1.0 //Z axis Gyro gain #define Gyro_Scaled_X(x) x*ToRad(Gyro_Gain_X) //Return the scaled ADC raw data of the gyro in radians for second #define Gyro_Scaled_Y(x) x*ToRad(Gyro_Gain_Y) //Return the scaled ADC raw data of the gyro in radians for second #define Gyro_Scaled_Z(x) x*ToRad(Gyro_Gain_Z) //Return the scaled ADC raw data of the gyro in radians for second @@ -84,7 +80,7 @@ int I2C_Message_ar[9]; //#define Kp_YAW 2.5 //High yaw drift correction gain - use with caution! #define Ki_YAW 0.00005 -#define ADC_WARM_CYCLES 75 +#define ADC_WARM_CYCLES 100 #define FALSE 0 #define TRUE 1 @@ -268,7 +264,10 @@ void loop() //Main Loop { timeNow = millis(); - if((timeNow-timer)>=20) // Main loop runs at 50Hz + // 20 -> Main loop runs at 50Hz + // 5 -> Main loop runs at 200Hz + // Max measured duty time around 3ms + if((timeNow-timer)>=5) { timer_old = timer; timer = timeNow; diff --git a/sw/airborne/firmwares/rotorcraft/battery.c b/sw/airborne/firmwares/rotorcraft/actuators/actuators_dummy.c similarity index 87% rename from sw/airborne/firmwares/rotorcraft/battery.c rename to sw/airborne/firmwares/rotorcraft/actuators/actuators_dummy.c index 357cc448d2..3204727e9f 100644 --- a/sw/airborne/firmwares/rotorcraft/battery.c +++ b/sw/airborne/firmwares/rotorcraft/actuators/actuators_dummy.c @@ -21,10 +21,15 @@ * Boston, MA 02111-1307, USA. */ -#include "firmwares/rotorcraft/battery.h" +#include "firmwares/rotorcraft/actuators.h" -uint8_t battery_voltage; -void battery_init(void) { - battery_voltage = 0; + + +void actuators_init(void) { + +} + + +void actuators_set(bool_t motors_on) { } diff --git a/sw/airborne/firmwares/rotorcraft/actuators/actuators_pwm_supervision.c b/sw/airborne/firmwares/rotorcraft/actuators/actuators_pwm_supervision.c index 06fb68f5ad..9debad1cfc 100644 --- a/sw/airborne/firmwares/rotorcraft/actuators/actuators_pwm_supervision.c +++ b/sw/airborne/firmwares/rotorcraft/actuators/actuators_pwm_supervision.c @@ -46,6 +46,7 @@ void actuators_set(bool_t motors_on) { booz2_commands[COMMAND_PITCH] = booz2_commands[COMMAND_PITCH] * PWM_GAIN_SCALE; booz2_commands[COMMAND_ROLL] = booz2_commands[COMMAND_ROLL] * PWM_GAIN_SCALE; booz2_commands[COMMAND_YAW] = booz2_commands[COMMAND_YAW] * PWM_GAIN_SCALE; + booz2_commands[COMMAND_THRUST] = (booz2_commands[COMMAND_THRUST] * ((SUPERVISION_MAX_MOTOR - SUPERVISION_MIN_MOTOR) / 200)) + SUPERVISION_MIN_MOTOR; supervision_run(motors_on, FALSE, booz2_commands); diff --git a/sw/airborne/firmwares/rotorcraft/battery.h b/sw/airborne/firmwares/rotorcraft/battery.h deleted file mode 100644 index 41aa171692..0000000000 --- a/sw/airborne/firmwares/rotorcraft/battery.h +++ /dev/null @@ -1,43 +0,0 @@ -/* - * $Id$ - * - * Copyright (C) 2008-2009 Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - */ - -#ifndef BATTERY_H -#define BATTERY_H - -#include "std.h" - -#include "generated/airframe.h" - -/* decivolts */ -extern uint8_t battery_voltage; - -static inline void BatteryISRHandler(uint16_t _val) { - uint32_t cal_v = (uint32_t)(_val) * BATTERY_SENS_NUM / BATTERY_SENS_DEN; - uint32_t sum = (uint32_t)battery_voltage + cal_v; - battery_voltage = (uint8_t)(sum/2); -} - - -extern void battery_init(void); - -#endif /* BATTERY_H */ diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c index 03a4b251b2..8dbdf59828 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c @@ -111,7 +111,7 @@ void guidance_v_read_rc(void) { // used in RC_DIRECT directly and as saturation in CLIMB and HOVER #ifndef USE_HELI - guidance_v_rc_delta_t = (int32_t)radio_control.values[RADIO_THROTTLE] * (SUPERVISION_MAX_MOTOR - SUPERVISION_MIN_MOTOR) / MAX_PPRZ + SUPERVISION_MIN_MOTOR; + guidance_v_rc_delta_t = (int32_t)radio_control.values[RADIO_THROTTLE] * 200 / MAX_PPRZ; #else guidance_v_rc_delta_t = (int32_t)radio_control.values[RADIO_THROTTLE] * 4 / 5; #endif diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c index fd6ce9a3ee..1a960eda0c 100644 --- a/sw/airborne/firmwares/rotorcraft/main.c +++ b/sw/airborne/firmwares/rotorcraft/main.c @@ -32,6 +32,7 @@ #include "downlink.h" #include "firmwares/rotorcraft/telemetry.h" #include "datalink.h" +#include "subsystems/settings.h" #include "xbee.h" #include "booz2_commands.h" @@ -46,7 +47,6 @@ #include "subsystems/electrical.h" -// #include "booz_fms.h" // FIXME #include "firmwares/rotorcraft/autopilot.h" #include "firmwares/rotorcraft/stabilization.h" @@ -108,7 +108,6 @@ STATIC_INLINE void main_init( void ) { baro_init(); imu_init(); - // booz_fms_init(); // FIXME autopilot_init(); nav_init(); guidance_h_init(); @@ -126,6 +125,8 @@ STATIC_INLINE void main_init( void ) { modules_init(); + settings_init(); + mcu_int_enable(); } diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c index 2680feef43..36f6dd9df7 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c @@ -200,7 +200,7 @@ void stabilization_attitude_run(bool_t enable_integrator) { scaled_att_err.qy = att_err.qy / IERROR_SCALE; scaled_att_err.qz = att_err.qz / IERROR_SCALE; FLOAT_QUAT_COMP_INV(new_sum_err, stabilization_att_sum_err_quat, scaled_att_err); - FLOAT_QUAT_NORMALISE(new_sum_err); + FLOAT_QUAT_NORMALIZE(new_sum_err); FLOAT_QUAT_COPY(stabilization_att_sum_err_quat, new_sum_err); FLOAT_EULERS_OF_QUAT(stabilization_att_sum_err_eulers, stabilization_att_sum_err_quat); } else { diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c index 7799d9995b..c6478d5b40 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c @@ -117,7 +117,7 @@ void stabilization_attitude_ref_update() { FLOAT_QUAT_DERIVATIVE(qdot, stab_att_ref_rate, stab_att_ref_quat); QUAT_SMUL(qdot, qdot, DT_UPDATE); QUAT_ADD(stab_att_ref_quat, qdot); - FLOAT_QUAT_NORMALISE(stab_att_ref_quat); + FLOAT_QUAT_NORMALIZE(stab_att_ref_quat); /* integrate reference rotational speeds */ struct FloatRates delta_rate; diff --git a/sw/airborne/firmwares/rotorcraft/telemetry.h b/sw/airborne/firmwares/rotorcraft/telemetry.h index e9cbe76fae..65a755cbb6 100644 --- a/sw/airborne/firmwares/rotorcraft/telemetry.h +++ b/sw/airborne/firmwares/rotorcraft/telemetry.h @@ -130,22 +130,22 @@ extern uint8_t telemetry_mode_Main_DefaultChannel; #define PERIODIC_SEND_PPM(_chan) {} #endif -#define PERIODIC_SEND_BOOZ2_GYRO(_chan) { \ - DOWNLINK_SEND_BOOZ2_GYRO(_chan, \ +#define PERIODIC_SEND_IMU_GYRO_SCALED(_chan) { \ + DOWNLINK_SEND_IMU_GYRO_SCALED(_chan, \ &imu.gyro.p, \ &imu.gyro.q, \ &imu.gyro.r); \ } -#define PERIODIC_SEND_BOOZ2_ACCEL(_chan) { \ - DOWNLINK_SEND_BOOZ2_ACCEL(_chan, \ +#define PERIODIC_SEND_IMU_ACCEL_SCALED(_chan) { \ + DOWNLINK_SEND_IMU_ACCEL_SCALED(_chan, \ &imu.accel.x, \ &imu.accel.y, \ &imu.accel.z); \ } -#define PERIODIC_SEND_BOOZ2_MAG(_chan) { \ - DOWNLINK_SEND_BOOZ2_MAG(_chan, \ +#define PERIODIC_SEND_IMU_MAG_SCALED(_chan) { \ + DOWNLINK_SEND_IMU_MAG_SCALED(_chan, \ &imu.mag.x, \ &imu.mag.y, \ &imu.mag.z); \ diff --git a/sw/airborne/fms/fms_spi_autopilot_msg.c b/sw/airborne/fms/fms_spi_autopilot_msg.c index 33c0a53ee1..8df37b8ba3 100644 --- a/sw/airborne/fms/fms_spi_autopilot_msg.c +++ b/sw/airborne/fms/fms_spi_autopilot_msg.c @@ -106,7 +106,7 @@ int spi_ap_link_init() FLOAT_QUAT_OF_AXIS_ANGLE(imuFloat.body_to_imu_quat, x_axis, QUAT_SETPOINT_HOVER_PITCH); #endif - FLOAT_QUAT_NORMALISE(imuFloat.body_to_imu_quat); + FLOAT_QUAT_NORMALIZE(imuFloat.body_to_imu_quat); FLOAT_EULERS_OF_QUAT(imuFloat.body_to_imu_eulers, imuFloat.body_to_imu_quat); FLOAT_RMAT_OF_QUAT(imuFloat.body_to_imu_rmat, imuFloat.body_to_imu_quat); diff --git a/sw/airborne/lisa/lisa_analog_plug.c b/sw/airborne/lisa/lisa_analog_plug.c deleted file mode 100644 index 3e49d689d9..0000000000 --- a/sw/airborne/lisa/lisa_analog_plug.c +++ /dev/null @@ -1,10 +0,0 @@ -#include "std.h" - -uint8_t battery_voltage; - -void booz2_analog_init(void); -void battery_init(void); - -void booz2_analog_init(void) {} -void battery_init(void) {} - diff --git a/sw/airborne/math/pprz_algebra_float.h b/sw/airborne/math/pprz_algebra_float.h index 4e07955bc2..5a31345f6c 100644 --- a/sw/airborne/math/pprz_algebra_float.h +++ b/sw/airborne/math/pprz_algebra_float.h @@ -494,7 +494,7 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) { #define FLOAT_QUAT_NORM(_q) (sqrtf(SQUARE(_q.qi) + SQUARE(_q.qx)+ \ SQUARE(_q.qy) + SQUARE(_q.qz))) \ -#define FLOAT_QUAT_NORMALISE(q) { \ +#define FLOAT_QUAT_NORMALIZE(q) { \ float norm = FLOAT_QUAT_NORM(q); \ if (norm > FLT_MIN) { \ q.qi = q.qi / norm; \ @@ -515,7 +515,7 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) { #define FLOAT_QUAT_COMP_NORM_SHORTEST(_a2c, _a2b, _b2c) { \ FLOAT_QUAT_COMP(_a2c, _a2b, _b2c); \ FLOAT_QUAT_WRAP_SHORTEST(_a2c); \ - FLOAT_QUAT_NORMALISE(_a2c); \ + FLOAT_QUAT_NORMALIZE(_a2c); \ } /* _a2c = _a2b comp _b2c , aka _a2c = _a2b * _b2c */ @@ -532,7 +532,7 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) { #define FLOAT_QUAT_COMP_INV_NORM_SHORTEST(_a2b, _a2c, _b2c) { \ FLOAT_QUAT_COMP_INV(_a2b, _a2c, _b2c); \ FLOAT_QUAT_WRAP_SHORTEST(_a2b); \ - FLOAT_QUAT_NORMALISE(_a2b); \ + FLOAT_QUAT_NORMALIZE(_a2b); \ } /* _a2b = _a2c comp_inv _b2c , aka _a2b = _a2c * inv(_b2c) */ @@ -547,7 +547,7 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) { #define FLOAT_QUAT_INV_COMP_NORM_SHORTEST(_b2c, _a2b, _a2c) { \ FLOAT_QUAT_INV_COMP(_b2c, _a2b, _a2c); \ FLOAT_QUAT_WRAP_SHORTEST(_b2c); \ - FLOAT_QUAT_NORMALISE(_b2c); \ + FLOAT_QUAT_NORMALIZE(_b2c); \ } /* _b2c = _a2b inv_comp _a2c , aka _b2c = inv(_a2b) * _a2c */ diff --git a/sw/airborne/modules/ins/ins_arduimu_basic.c b/sw/airborne/modules/ins/ins_arduimu_basic.c index 1ff1c7151a..18b9fe654b 100644 --- a/sw/airborne/modules/ins/ins_arduimu_basic.c +++ b/sw/airborne/modules/ins/ins_arduimu_basic.c @@ -132,7 +132,7 @@ void ArduIMU_event( void ) { estimator_p = arduimu_rates.p; ardu_ins_trans.status = I2CTransDone; - RunOnceEvery(15, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, &arduimu_eulers.phi, &arduimu_eulers.theta, &arduimu_eulers.psi)); + //RunOnceEvery(15, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, &arduimu_eulers.phi, &arduimu_eulers.theta, &arduimu_eulers.psi)); RunOnceEvery(15, DOWNLINK_SEND_IMU_GYRO(DefaultChannel, &arduimu_rates.p, &arduimu_rates.q, &arduimu_rates.r)); RunOnceEvery(15, DOWNLINK_SEND_IMU_ACCEL(DefaultChannel, &arduimu_accel.x, &arduimu_accel.y, &arduimu_accel.z)); } diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_rmat.c b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_rmat.c index 8e91105add..6eed92bc4d 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_rmat.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_rmat.c @@ -119,7 +119,7 @@ void ahrs_propagate(void) { #endif #ifdef AHRS_PROPAGATE_QUAT FLOAT_QUAT_INTEGRATE(ahrs_float.ltp_to_imu_quat, omega, dt); - FLOAT_QUAT_NORMALISE(ahrs_float.ltp_to_imu_quat); + FLOAT_QUAT_NORMALIZE(ahrs_float.ltp_to_imu_quat); compute_imu_rmat_and_euler_from_quat(); #endif compute_body_orientation_and_rates(); diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_lkf.c b/sw/airborne/subsystems/ahrs/ahrs_float_lkf.c index 50e2c6f6df..f1e781a36a 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_lkf.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_lkf.c @@ -315,7 +315,7 @@ void ahrs_propagate(void) { bafl_qnorm = FLOAT_QUAT_NORM(bafl_quat); //TODO check if broot force normalization is good, use lagrange normalization? - FLOAT_QUAT_NORMALISE(bafl_quat); + FLOAT_QUAT_NORMALIZE(bafl_quat); /* diff --git a/sw/airborne/subsystems/imu.c b/sw/airborne/subsystems/imu.c index d5412ff5f8..515adc6b38 100644 --- a/sw/airborne/subsystems/imu.c +++ b/sw/airborne/subsystems/imu.c @@ -64,7 +64,7 @@ void imu_float_init(struct ImuFloat* imuf) { EULERS_ASSIGN(imuf->body_to_imu_eulers, IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI); FLOAT_QUAT_OF_EULERS(imuf->body_to_imu_quat, imuf->body_to_imu_eulers); - FLOAT_QUAT_NORMALISE(imuf->body_to_imu_quat); + FLOAT_QUAT_NORMALIZE(imuf->body_to_imu_quat); FLOAT_RMAT_OF_EULERS(imuf->body_to_imu_rmat, imuf->body_to_imu_eulers); #else EULERS_ASSIGN(imuf->body_to_imu_eulers, 0., 0., 0.); diff --git a/sw/airborne/subsystems/navigation/spiral.c b/sw/airborne/subsystems/navigation/spiral.c index b25ef937e2..9ab41fb2b1 100644 --- a/sw/airborne/subsystems/navigation/spiral.c +++ b/sw/airborne/subsystems/navigation/spiral.c @@ -118,7 +118,7 @@ bool_t SpiralNav(void) //dc_Circle(360/Segmente); } break; - case Circle: + case Circle: { nav_circle_XY(waypoints[Center].x, waypoints[Center].y, SRad); // Trigonometrische Berechnung des bereits geflogenen Winkels alpha // equation: @@ -135,6 +135,7 @@ bool_t SpiralNav(void) CSpiralStatus = IncSpiral; } break; + } case IncSpiral: // increasing circle radius as long as it is smaller than max helix radius if(SRad + IRad < Spiralradius) diff --git a/sw/airborne/subsystems/settings.c b/sw/airborne/subsystems/settings.c index 452c0edcef..04754e2ead 100644 --- a/sw/airborne/subsystems/settings.c +++ b/sw/airborne/subsystems/settings.c @@ -4,14 +4,18 @@ struct PersistentSettings pers_settings; bool_t settings_store_now; + void settings_init(void) { - // READ SETTINGS FROM FLASH +#ifdef USE_PERSISTENT_SETTINGS + if (persistent_read((uint32_t)&pers_settings, sizeof(struct PersistentSettings))) + return; // return -1 ? persitent_settings_load(); +#endif } void settings_store(void) { persitent_settings_store(); - // WRITE SETTINGS TO FLASH + persistent_write((uint32_t)&pers_settings, sizeof(struct PersistentSettings)); } diff --git a/sw/airborne/subsystems/settings.h b/sw/airborne/subsystems/settings.h index 16008c51fb..8c39b680eb 100644 --- a/sw/airborne/subsystems/settings.h +++ b/sw/airborne/subsystems/settings.h @@ -8,14 +8,13 @@ extern void settings_store(void); extern bool_t settings_store_now; -#define settings_StoreSettings(_v) { \ - if (_v) { \ - settings_store(); \ - settings_store_now = FALSE; \ - } \ - } +#define settings_StoreSettings(_v) { settings_store_now = _v; settings_store(); } #include "generated/settings.h" +/* implemented in arch dependant code */ +int32_t persistent_write(uint32_t ptr, uint32_t size); +int32_t persistent_read(uint32_t ptr, uint32_t size); + #endif /* SUBSYSTEMS_SETTINGS_H */ diff --git a/sw/airborne/test/subsystems/test_ahrs.c b/sw/airborne/test/subsystems/test_ahrs.c index 26456a7bb7..ac42d4b344 100644 --- a/sw/airborne/test/subsystems/test_ahrs.c +++ b/sw/airborne/test/subsystems/test_ahrs.c @@ -136,20 +136,20 @@ static inline void main_report(void) { &imu.mag_unscaled.z); }, { - DOWNLINK_SEND_BOOZ2_ACCEL(DefaultChannel, + DOWNLINK_SEND_IMU_ACCEL_SCALED(DefaultChannel, &imu.accel.x, &imu.accel.y, &imu.accel.z); }, { - DOWNLINK_SEND_BOOZ2_GYRO(DefaultChannel, + DOWNLINK_SEND_IMU_GYRO_SCALED(DefaultChannel, &imu.gyro.p, &imu.gyro.q, &imu.gyro.r); }, { - DOWNLINK_SEND_BOOZ2_MAG(DefaultChannel, + DOWNLINK_SEND_IMU_MAG_SCALED(DefaultChannel, &imu.mag.x, &imu.mag.y, &imu.mag.z); diff --git a/sw/airborne/test/subsystems/test_imu.c b/sw/airborne/test/subsystems/test_imu.c index 012d16865d..9591438b63 100644 --- a/sw/airborne/test/subsystems/test_imu.c +++ b/sw/airborne/test/subsystems/test_imu.c @@ -108,7 +108,7 @@ static inline void on_accel_event(void) { &imu.accel_unscaled.z); } else if (cnt == 7) { - DOWNLINK_SEND_BOOZ2_ACCEL(DefaultChannel, + DOWNLINK_SEND_IMU_ACCEL_SCALED(DefaultChannel, &imu.accel.x, &imu.accel.y, &imu.accel.z); @@ -129,7 +129,7 @@ static inline void on_gyro_accel_event(void) { &imu.gyro_unscaled.r); } else if (cnt == 7) { - DOWNLINK_SEND_BOOZ2_GYRO(DefaultChannel, + DOWNLINK_SEND_IMU_GYRO_SCALED(DefaultChannel, &imu.gyro.p, &imu.gyro.q, &imu.gyro.r); @@ -144,7 +144,7 @@ static inline void on_mag_event(void) { if (cnt > 10) cnt = 0; if (cnt == 0) { - DOWNLINK_SEND_BOOZ2_MAG(DefaultChannel, + DOWNLINK_SEND_IMU_MAG_SCALED(DefaultChannel, &imu.mag.x, &imu.mag.y, &imu.mag.z); diff --git a/sw/airborne/test/subsystems/test_settings.c b/sw/airborne/test/subsystems/test_settings.c index cf37af0782..1f33f4aad1 100644 --- a/sw/airborne/test/subsystems/test_settings.c +++ b/sw/airborne/test/subsystems/test_settings.c @@ -34,6 +34,7 @@ #include "mcu_periph/uart.h" #include "messages.h" +#include "my_debug_servo.h" static inline void main_init( void ); static inline void main_periodic( void ); @@ -61,13 +62,22 @@ static inline void main_init( void ) { mcu_init(); sys_time_init(); settings_init(); + // DEBUG_SERVO2_INIT(); + // LED_ON(1); + // LED_ON(2); + // DEBUG_S4_ON(); + // DEBUG_S5_ON(); + // DEBUG_S6_ON(); mcu_int_enable(); } static inline void main_periodic( void ) { - RunOnceEvery(100, {DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);}); + RunOnceEvery(100, { + DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM); + PeriodicSendDlValue(DefaultChannel); + }); } diff --git a/sw/airborne/test/test_algebra.c b/sw/airborne/test/test_algebra.c index b8b7ce4d8b..ee67f3dffe 100644 --- a/sw/airborne/test/test_algebra.c +++ b/sw/airborne/test/test_algebra.c @@ -108,7 +108,7 @@ static void test_2(void) { struct Int32Quat quat_i; INT32_QUAT_OF_EULERS(quat_i, euler_i); DISPLAY_INT32_QUAT("quat_i", quat_i); - INT32_QUAT_NORMALISE(quat_i); + INT32_QUAT_NORMALIZE(quat_i); DISPLAY_INT32_QUAT("quat_i_n", quat_i); struct Int32Vect3 v2; @@ -155,7 +155,7 @@ static void test_3(void) { struct Int32Quat b2i_q; INT32_QUAT_OF_EULERS(b2i_q, b2i_e); DISPLAY_INT32_QUAT_AS_EULERS_DEG("b2i_q", b2i_q); - // INT32_QUAT_NORMALISE(b2i_q); + // INT32_QUAT_NORMALIZE(b2i_q); // DISPLAY_INT32_QUAT_AS_EULERS_DEG("b2i_q_n", b2i_q); /* Compute BODY to IMU rotation matrix */ @@ -232,7 +232,7 @@ static void test_4_int(void) { struct Int32Quat _q; INT32_QUAT_OF_EULERS(_q, _e); DISPLAY_INT32_QUAT_AS_EULERS_DEG("quat1 ", _q); - // INT32_QUAT_NORMALISE(_q); + // INT32_QUAT_NORMALIZE(_q); // DISPLAY_INT32_QUAT_2("_q_n", _q); /* back to eulers */ @@ -257,7 +257,7 @@ static void test_4_float(void) { struct FloatQuat q; FLOAT_QUAT_OF_EULERS(q, e); // DISPLAY_FLOAT_QUAT("q", q); - FLOAT_QUAT_NORMALISE(q); + FLOAT_QUAT_NORMALIZE(q); DISPLAY_FLOAT_QUAT("q_n", q); DISPLAY_FLOAT_QUAT_AS_INT("q_n as int", q); /* back to eulers */ diff --git a/sw/ground_segment/joystick/input2ivy.ml b/sw/ground_segment/joystick/input2ivy.ml index 6e653b9e9c..356bd99cd6 100644 --- a/sw/ground_segment/joystick/input2ivy.ml +++ b/sw/ground_segment/joystick/input2ivy.ml @@ -347,7 +347,8 @@ let eval_expr = fun buttons axis inputs expr -> | Syntax.Call (ident, exprs) | Syntax.CallOperator (ident, exprs) -> eval_call ident (List.map eval exprs) | Syntax.Index _ -> failwith "eval_expr: index" - | Syntax.Field _ -> failwith "eval_expr: Field" in + | Syntax.Field _ -> failwith "eval_expr: Field" + | Syntax.Deref _ -> failwith "eval_expr: deref" in eval expr diff --git a/sw/ground_segment/lpc21iap/lpc21iap.c b/sw/ground_segment/lpc21iap/lpc21iap.c index 26dbd652a4..9cd73ff3be 100644 --- a/sw/ground_segment/lpc21iap/lpc21iap.c +++ b/sw/ground_segment/lpc21iap/lpc21iap.c @@ -24,7 +24,7 @@ #define LPC21IAP_VER_MAJ 1 -#define LPC21IAP_VER_MIN 1 +#define LPC21IAP_VER_MIN 2 #if defined(_WIN32) && !defined(__CYGWIN__) @@ -156,9 +156,10 @@ int main(int argc, char *argv[]) tIntermediateBuffer* actBuf = NULL; tIntermediateBuffer* startBuf = NULL; + printf("lpc21iap v%d.%d\n", LPC21IAP_VER_MAJ, LPC21IAP_VER_MIN); + if ((argc < 2) || (argc > 3)) { - printf("lpc21iap version v%d.%d, ", LPC21IAP_VER_MAJ, LPC21IAP_VER_MIN); printf("usage: %s file.elf [usb_serial_number]\n", argv[0]); exit(1); } @@ -353,6 +354,12 @@ int main(int argc, char *argv[]) exit(2); } + /* flashing code: persistent settings needs highest sector to be erased */ + if (highFlash >= BOOTLOAD_SECT*MAX_SECT) + { + highFlash = maxFlash-1; + } + /* anything to flash erase? */ if (lowFlash != maxFlash) { diff --git a/sw/ground_segment/misc/Makefile b/sw/ground_segment/misc/Makefile new file mode 100644 index 0000000000..018ab40905 --- /dev/null +++ b/sw/ground_segment/misc/Makefile @@ -0,0 +1,7 @@ +all: davis2ivy + +davis2ivy: davis2ivy.o + g++ -o davis2ivy davis2ivy.o -s -livy + +%.o : %.c + gcc -c -O2 -Wall $< diff --git a/sw/ground_segment/misc/davis2ivy.c b/sw/ground_segment/misc/davis2ivy.c new file mode 100644 index 0000000000..f167bb7815 --- /dev/null +++ b/sw/ground_segment/misc/davis2ivy.c @@ -0,0 +1,269 @@ +/* + * Paparazzi $Id$ + * + * Copyright (C) 2011 Andreas Gaeb + * + * 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 davis2ivy.c + * \brief Connect a Davis VantagePro weather station to the Paparazzi system + * + * The program communicates with a Davis VantagePro(2) weather station connected + * to a serial port. It asks for new data (Davis' LOOP command) in the + * specified intervals, extracts the relevant data (ambient pressure and + * temperature, wind speed and direction) and broadcasts this via the Ivy bus. + * + * At the moment, the Ivy messages should be sent with the ID of the actually + * flying aircraft, which integrates them into the log file, as long as the + * aircraft sends its alive message. + * + * Useful links: + * - Weather Stations + * - Communication docs + * + */ + + + #include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + + +typedef enum { FALSE = 0, TRUE } BOOL; + +#define PACKET_LENGTH 99 + + +// global variables +int fd, ac_id = 1; +const char *device; +unsigned char packet[PACKET_LENGTH]; +TimerId tid; +BOOL want_alive_msg = FALSE; + + +/// Handler for Ctrl-C, exits the main loop +void sigint_handler(int sig) { + IvyStop(); + TimerRemove(tid); + close(fd); +} + +/// open the serial port with the appropiate settings +void open_port(const char* device) { + fd = open(device, O_RDWR | O_NOCTTY | O_NDELAY); + if (fd == -1) { + fprintf(stderr, "open_port: unable to open device %s - ", device); + perror(NULL); + exit(EXIT_FAILURE); + } + // setup connection options + struct termios options; + + // get the current options + tcgetattr(fd, &options); + + // set local mode, enable receiver, set comm. options: + // 8 data bits, 1 stop bit, no parity, 19200 Baud + options.c_cflag = CLOCAL | CREAD | CS8 | B19200; + + // write options back to port + tcsetattr(fd, TCSANOW, &options); + +} + +/// disable transactions and empty queue +void reset_station() { + char newline = '\n', bytes = 0; + fprintf(stderr, "Resetting communication\n"); + // send a \n (wakeup and cancel all running transmits) + bytes = write(fd, &newline, 1); + // read and discard everything that might be left in the queue + close(fd); + sleep(1); + open_port(device); +} + +/// send a wakeup call to the station +BOOL wakeup(int tries) { + int loops = tries, bytes; + BOOL woken = FALSE; + char buf[] = {0, 0}; + char newline = '\n'; + do { + // send a \n + bytes = write(fd, &newline, 1); + // wait until station answers with \n\r + usleep(30000); + bytes = read(fd, buf, sizeof(buf)); + woken = (buf[0] == 10) && (buf[1] == 13); + } while (!woken && loops-- > 0); + if (!woken) { + fprintf(stderr, "Could not wake up station: "); + if (bytes < 1) fprintf(stderr, "no bytes received\n"); + else fprintf(stderr, "received %02x:%02x instead of \\n\\r\n", buf[0], buf[1]); + reset_station(); + } + return woken; +} + +/// send a LOOP command (read sensor data) to the station and get the packet back +BOOL send_loop() { + char msg[32], ack; + // TODO maybe ask for more packets? + snprintf(msg, sizeof(msg), "LOOP %i\n", 1); + int bytes = write(fd, msg, strlen(msg)); + usleep(120000); + bytes = read(fd, &ack, 1); + if (bytes < 1 || ack != 0x06) { + fprintf(stderr, "Failed to receive ACK from station\n"); + reset_station(); + return FALSE; + } + bytes = read(fd, packet, PACKET_LENGTH); + if (bytes < PACKET_LENGTH) { + fprintf(stderr, "Received packet is incomplete, only %i of %i bytes\n", + bytes, PACKET_LENGTH); + reset_station(); + return FALSE; + } else { + return TRUE; + } +} + +/// get the relevant data from the packet and sent it as Ivy message +void decode_and_send_to_ivy() { + + // check packet integrity + char expected[] = "LOO"; + if (strncmp((char *)packet, expected, 3) != 0) { + fprintf(stderr, "Received packet from the weather station which does not match the expected format\n"); + reset_station(); + return; + } + + // TODO CRC checking (is rather involved for the Davis protocol) + + // get relevant data and convert to SI units + // see chapter IX.1 of the protocol definition + float + pstatic_Pa = (packet[7] | packet[8] << 8)*3.386388640341, // original is inches Hg / 1000 + temp_degC = ((packet[12] | packet[13] << 8)/10.0 - 32.0)*5.0/9.0, // original is deg F / 10 + windspeed_mps = packet[14]*0.44704, // original is miles per hour + winddir_deg = packet[16] | packet[17] << 8; + + + // TODO get the real MD5 for the aircraft id + if (want_alive_msg) + IvySendMsg("%d ALIVE 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0\n", ac_id); + + // format has to match declaration in conf/messages.xml + IvySendMsg("%d WEATHER %f %f %f %f\n", + ac_id, pstatic_Pa, temp_degC, windspeed_mps, winddir_deg); +} + +/// Get data from the station and send it via Ivy +/** This function is executed by the timer + */ +void handle_timer (TimerId id, void *data, unsigned long delta) { + if (wakeup(3) && send_loop()) decode_and_send_to_ivy(); +} + +void print_usage(int argc, char ** argv) { + fprintf(stderr, "Usage: %s [-a] [-b ] [-d ] [-i ] [-s ]\n", + argv[0]); +}; + +/// Main function +int main(int argc, char **argv) { + // default values for options + const char + *defaultbus = "127.255.255.255:2010", + *bus = defaultbus, + *defaultdevice = "/dev/ttyUSB1"; + device = defaultdevice; + long delay = 1000; + + // parse options + char c; + while ((c = getopt (argc, argv, "hab:d:i:s:")) != EOF) { + switch (c) { + case 'h': + print_usage(argc, argv); + exit(EXIT_SUCCESS); + break; + case 'a': + want_alive_msg = TRUE; + break; + case 'b': + bus = optarg; + break; + case 'd': + device = optarg; + break; + case 'i': + ac_id = atoi(optarg); + break; + case 's': + delay = atoi(optarg)*1000; + break; + case '?': + if (optopt == 'a' || optopt == 'b' || optopt == 'd' || optopt == 's') + fprintf (stderr, "Option -%c requires an argument.\n", optopt); + else if (isprint (optopt)) + fprintf (stderr, "Unknown option `-%c'.\n", optopt); + else + fprintf (stderr, "Unknown option character `\\x%x'.\n", optopt); + print_usage(argc, argv); + exit(EXIT_FAILURE); + default: + abort (); + } + } + + + // make Ctrl-C stop the main loop and clean up properly + signal(SIGINT, sigint_handler); + + bzero (packet, PACKET_LENGTH); + open_port(device); + + // setup Ivy communication + IvyInit("davis2ivy", "READY", 0, 0, 0, 0); + IvyStart(bus); + + // create timer + tid = TimerRepeatAfter (0, delay, handle_timer, 0); + + IvyMainLoop(); + + return 0; +} diff --git a/sw/ground_segment/misc/readme.txt b/sw/ground_segment/misc/readme.txt new file mode 100644 index 0000000000..5221c38c9f --- /dev/null +++ b/sw/ground_segment/misc/readme.txt @@ -0,0 +1,3 @@ +davis2ivy: + A wrapper to communicate with a Davis VantagePro/VantagePro2 weather + station and integrate weather data into the telemetry link. diff --git a/sw/lib/ocaml/expr_lexer.mll b/sw/lib/ocaml/expr_lexer.mll index 695409d5a9..dbfa4fab10 100644 --- a/sw/lib/ocaml/expr_lexer.mll +++ b/sw/lib/ocaml/expr_lexer.mll @@ -42,6 +42,7 @@ rule token = parse | '}' { RC } | '[' { LB } | ']' { RB } + | "->" { DEREF } | "==" { EQ } | "&&" { AND } | "||" { OR } diff --git a/sw/lib/ocaml/expr_parser.mly b/sw/lib/ocaml/expr_parser.mly index c2f63b4a00..7aa963ce37 100644 --- a/sw/lib/ocaml/expr_parser.mly +++ b/sw/lib/ocaml/expr_parser.mly @@ -29,7 +29,7 @@ open Expr_syntax %token FLOAT %token IDENT %token EOF -%token DOT COMMA SEMICOLON LP RP LC RC LB RB AND COLON OR +%token DOT COMMA SEMICOLON LP RP LC RC LB RB DEREF AND COLON OR %token EQ GT ASSIGN GEQ NOT %token PLUS MINUS %token MULT DIV MOD @@ -39,7 +39,8 @@ open Expr_syntax %left PLUS MINUS %left MULT DIV MOD %nonassoc NOT -%nonassoc UMINUS /* highest precedence */ +%nonassoc UMINUS +%left DEREF /* highest precedence */ %start expression /* the entry point */ %type expression @@ -63,6 +64,7 @@ expression: | FLOAT { Float $1 } | IDENT { Ident $1 } | IDENT DOT IDENT { Field ($1,$3) } + | expression DEREF IDENT { Deref($1, $3) } | IDENT LP Args RP { Call ($1, $3) } | LP expression RP { $2 } | IDENT LB expression RB { Index ($1, $3) } diff --git a/sw/lib/ocaml/expr_syntax.ml b/sw/lib/ocaml/expr_syntax.ml index be15604c8d..5f8f5034b5 100644 --- a/sw/lib/ocaml/expr_syntax.ml +++ b/sw/lib/ocaml/expr_syntax.ml @@ -37,6 +37,7 @@ type expression = | CallOperator of ident * (expression list) | Index of ident * expression | Field of ident * ident + | Deref of expression * ident let c_var_of_ident = fun x -> "_var_" ^ x @@ -55,6 +56,7 @@ let rec sprint = function sprintf "%s(%s)" i (String.concat "," ses) | Index (i,e) -> sprintf "%s[%s]" i (sprint e) | Field (i,f) -> sprintf "%s.%s" i f + | Deref (e,f) -> sprintf "(%s)->%s" (sprint e) f (* Valid functions : FIXME *) let functions = [ @@ -113,3 +115,5 @@ let rec check_expression = fun e -> | Field (i, _field) -> if not (List.mem i variables) then unexpected "ident" i + | Deref (e, _field) -> + check_expression e diff --git a/sw/lib/ocaml/expr_syntax.mli b/sw/lib/ocaml/expr_syntax.mli index 0d266c2354..7cb9619094 100644 --- a/sw/lib/ocaml/expr_syntax.mli +++ b/sw/lib/ocaml/expr_syntax.mli @@ -34,6 +34,7 @@ type expression = | CallOperator of ident * expression list | Index of ident * expression | Field of ident * ident + | Deref of expression * ident val c_var_of_ident : ident -> string (** Encapsulate a user ident into a C variable *) diff --git a/sw/lib/ocaml/papget.ml b/sw/lib/ocaml/papget.ml index 23ed642836..586f428724 100644 --- a/sw/lib/ocaml/papget.ml +++ b/sw/lib/ocaml/papget.ml @@ -92,6 +92,7 @@ let hash_vars = fun expr -> | E.Int _ | E.Float _ -> () | E.Call (_id, list) | E.CallOperator (_id, list) -> List.iter loop list | E.Index (_id, e) -> loop e + | E.Deref (_e, _f) as deref -> fprintf stderr "Warning: Deref operator is not allowed in Papgets expressions (%s)" (E.sprint deref) | E.Field (i, f) -> if not (Hashtbl.mem htable (i,f)) then let msg_obj = new message_field i f in @@ -121,6 +122,7 @@ let eval_expr = fun (extra_functions:(string * (string list -> string)) list) h | E.Call (ident, _l) | E.CallOperator (ident, _l) -> failwith (sprintf "Papget.eval_expr '%s(...)'" ident) | E.Index (ident, _e) -> failwith (sprintf "Papget.eval_expr '%s[...]'" ident) + | E.Deref (_e, _f) as deref -> failwith (sprintf "Papget.eval_expr Deref operator is not allowed in Papgets expressions (%s)" (E.sprint deref)) | E.Field (i, f) -> try (Hashtbl.find h (i,f))#last_value diff --git a/sw/tools/fp_proc.ml b/sw/tools/fp_proc.ml index 2a2e70dec4..93f37bab4c 100644 --- a/sw/tools/fp_proc.ml +++ b/sw/tools/fp_proc.ml @@ -59,7 +59,8 @@ let subst_expression = fun env e -> | Int _ | Float _ | Field _ -> e | Call (i, es) -> Call (i, List.map sub es) | CallOperator (i, es) -> CallOperator (i, List.map sub es) - | Index (i,e) -> Index (i,sub e) in + | Index (i,e) -> Index (i,sub e) + | Deref (e,f) -> Deref (sub e, f) in sub e diff --git a/sw/tools/gen_settings.ml b/sw/tools/gen_settings.ml index 7bc25f6424..a537587827 100644 --- a/sw/tools/gen_settings.ml +++ b/sw/tools/gen_settings.ml @@ -134,11 +134,67 @@ let print_dl_settings = fun settings -> lprintf "default: return 0.;\n"; lprintf "}\n"; left (); - lprintf "}\n" - - + lprintf "}\n"; + left() + +(* + Generate code for persitent settings +*) +let print_persistent_settings = fun settings -> + let settings = flatten settings [] in + let pers_settings = + List.filter (fun x -> try let _ = Xml.attrib x "persistent" in true with _ -> false) settings in + (* structure declaration *) +(* if List.length pers_settings > 0 then begin *) + lprintf "\n/* Persistent Settings */\n"; + lprintf "struct PersistentSettings {\n"; + right(); + let idx = ref 0 in + List.iter + (fun s -> + let v = ExtXml.attrib s "var" in + lprintf "float s_%d; /* %s */\n" !idx v; incr idx) + pers_settings; + left(); + lprintf "};\n\n"; + lprintf "extern struct PersistentSettings pers_settings;\n\n"; + (* Inline function to store persistent settings *) + idx := 0; + lprintf "static inline void persitent_settings_store( void ) {\n"; + right(); + List.iter + (fun s -> + let v = ExtXml.attrib s "var" in + lprintf "pers_settings.s_%d = %s;\n" !idx v; incr idx) + pers_settings; + left(); + lprintf "}\n\n"; + (* Inline function to load persistent settings *) + idx := 0; + lprintf "static inline void persitent_settings_load( void ) {\n"; + right(); + List.iter + (fun s -> + let v = ExtXml.attrib s "var" in + begin + try + let h = ExtXml.attrib s "handler" and + m = ExtXml.attrib s "module" in + lprintf "%s_%s( pers_settings.s_%d );\n" (Filename.basename m) h !idx ; +(* lprintf "%s = pers_settings.s_%d;\n" v !idx *) (* do we want to set the value too or just call the handler ? *) + with + ExtXml.Error e -> lprintf "%s = pers_settings.s_%d;\n" v !idx + end; + incr idx) + pers_settings; + left(); + lprintf "};\n" +(* end *) +(* + Blaaaaaa2 +*) let calib_mode_of_rc = function "gain_1_up" -> 1, "up" | "gain_1_down" -> 1, "down" @@ -233,6 +289,9 @@ let _ = left (); lprintf "}\n"; print_dl_settings dl_settings; + + print_persistent_settings dl_settings; + finish h_name with Xml.Error e -> prerr_endline (Xml.error e); exit 1