From 1ccb9a194753671208a7f70758043f1a3462cbbb Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sat, 6 Aug 2011 19:46:41 -0500 Subject: [PATCH] Some changes to BL to get running on F2. Also disables it for now (flash interface is different). --- flight/Bootloaders/INS/Makefile | 117 ++- flight/Bootloaders/INS/ahrs_slave_test.c | 44 +- flight/Bootloaders/INS/ahrs_spi_program.c | 136 ++- .../Bootloaders/INS/ahrs_spi_program_master.c | 76 +- .../Bootloaders/INS/ahrs_spi_program_slave.c | 61 +- flight/Bootloaders/INS/bl_fsm.c | 835 +++++++++--------- flight/Bootloaders/INS/inc/ahrs_bl.h | 52 ++ flight/Bootloaders/INS/inc/ahrs_spi_program.h | 72 +- .../INS/inc/ahrs_spi_program_master.h | 33 +- .../INS/inc/ahrs_spi_program_slave.h | 8 +- flight/Bootloaders/INS/inc/bl_fsm.h | 89 +- flight/Bootloaders/INS/inc/pios_config.h | 10 +- flight/Bootloaders/INS/main.c | 45 +- flight/Bootloaders/INS/pios_board.c | 187 ++-- flight/PiOS/Boards/STM3210E_INS.h | 17 +- make/boards/ins/board-info.mk | 4 +- 16 files changed, 881 insertions(+), 905 deletions(-) create mode 100644 flight/Bootloaders/INS/inc/ahrs_bl.h diff --git a/flight/Bootloaders/INS/Makefile b/flight/Bootloaders/INS/Makefile index ca33f25b8..6e4b22ac5 100644 --- a/flight/Bootloaders/INS/Makefile +++ b/flight/Bootloaders/INS/Makefile @@ -1,10 +1,10 @@ ##### - # Project: OpenPilot INS_BOOTLOADER + # Project: OpenPilot AHRS_BOOTLOADER # # - # Makefile for OpenPilot INS_BOOTLOADER project + # Makefile for OpenPilot AHRS_BOOTLOADER project # - # The OpenPilot Team, http://www.openpilot.org, Copyright (C) 2011. + # The OpenPilot Team, http://www.openpilot.org, Copyright (C) 2009. # # # This program is free software; you can redistribute it and/or modify @@ -25,6 +25,13 @@ WHEREAMI := $(dir $(lastword $(MAKEFILE_LIST))) TOP := $(realpath $(WHEREAMI)/../../../) include $(TOP)/make/firmware-defs.mk +include $(TOP)/make/boards/$(BOARD_NAME)/board-info.mk + +# Target file name (without extension). +TARGET := bl_$(BOARD_NAME) + +# Directory for output files (lst, obj, dep, elf, sym, map, hex, bin etc.) +OUTDIR := $(TOP)/build/$(TARGET) # Set developer code and compile options # Set to YES for debugging @@ -41,37 +48,19 @@ endif FLASH_TOOL = OPENOCD -# MCU name, submodel and board -# - MCU used for compiler-option (-mcpu) -# - MODEL used for linker-script name (-T) and passed as define -# - BOARD just passed as define (optional) -MCU = cortex-m3 -CHIP = STM32F103RET -BOARD = STM3210E_INS -MODEL = HD - - - -# Directory for output files (lst, obj, dep, elf, sym, map, hex, bin etc.) -OUTDIR = $(TOP)/build/bl_ins - -# Target file name (without extension). -TARGET = INS_BL - # Paths -INS_BL = ./ -INS_BLINC = $(INS_BL)/inc +AHRS_BL = ./ +AHRS_BLINC = $(AHRS_BL)/inc PIOS = ../../PiOS PIOSINC = $(PIOS)/inc FLIGHTLIB = ../Libraries FLIGHTLIBINC = ../Libraries/inc -PIOSSTM32F10X = $(PIOS)/STM32F10x +PIOSSTM32FXX = $(PIOS)/STM32F2xx PIOSCOMMON = $(PIOS)/Common PIOSBOARDS = $(PIOS)/Boards -APPLIBDIR = $(PIOSSTM32F10X)/Libraries +APPLIBDIR = $(PIOSSTM32FXX)/Libraries STMLIBDIR = $(APPLIBDIR) -STMSPDDIR = $(STMLIBDIR)/STM32F10x_StdPeriph_Driver -STMUSBDIR = $(STMLIBDIR)/STM32_USB-FS-Device_Driver +STMSPDDIR = $(STMLIBDIR)/STM32F2xx_StdPeriph_Driver STMSPDSRCDIR = $(STMSPDDIR)/src STMSPDINCDIR = $(STMSPDDIR)/inc CMSISDIR = $(STMLIBDIR)/CMSIS/Core/CM3 @@ -84,17 +73,23 @@ CMSISDIR = $(STMLIBDIR)/CMSIS/Core/CM3 SRC = main.c SRC += pios_board.c SRC += bl_fsm.c +#SRC += insgps.c +#SRC += $(FLIGHTLIB)/CoordinateConversions.c ## PIOS Hardware (STM32F10x) -SRC += $(PIOSSTM32F10X)/pios_sys.c -SRC += $(PIOSSTM32F10X)/pios_led.c -SRC += $(PIOSSTM32F10X)/pios_delay.c -SRC += $(PIOSSTM32F10X)/pios_usart.c -SRC += $(PIOSSTM32F10X)/pios_irq.c -SRC += $(PIOSSTM32F10X)/pios_gpio.c -SRC += $(PIOSSTM32F10X)/pios_spi.c +SRC += $(PIOSSTM32FXX)/pios_sys.c +SRC += $(PIOSSTM32FXX)/pios_led.c +SRC += $(PIOSSTM32FXX)/pios_delay.c +SRC += $(PIOSSTM32FXX)/pios_usart.c +SRC += $(PIOSSTM32FXX)/pios_irq.c +#SRC += $(PIOSSTM32FXX)/pios_i2c.c +SRC += $(PIOSSTM32FXX)/pios_gpio.c +SRC += $(PIOSSTM32FXX)/pios_spi.c ## PIOS Hardware (Common) +#SRC += $(PIOSCOMMON)/pios_com.c +#SRC += $(PIOSCOMMON)/pios_hmc5843.c +SRC += $(PIOSCOMMON)/pios_board_info.c SRC += $(PIOSCOMMON)/pios_opahrs_proto.c SRC += $(PIOSCOMMON)/printf-stdarg.c SRC += $(PIOSCOMMON)/pios_bl_helper.c @@ -143,7 +138,7 @@ CPPSRCARM = # Even though the DOS/Win* filesystem matches both .s and .S the same, # it will preserve the spelling of the filenames, and gcc itself does # care about how the name is spelled on its command-line. -ASRC = $(PIOSSTM32F10X)/startup_stm32f10x_$(MODEL).S +ASRC = $(PIOSSTM32FXX)/startup_stm32f2xx.S # List Assembler source files here which must be assembled in ARM-Mode.. ASRCARM = @@ -153,12 +148,12 @@ ASRCARM = EXTRAINCDIRS += $(PIOS) EXTRAINCDIRS += $(PIOSINC) EXTRAINCDIRS += $(FLIGHTLIBINC) -EXTRAINCDIRS += $(PIOSSTM32F10X) +EXTRAINCDIRS += $(PIOSSTM32FXX) EXTRAINCDIRS += $(PIOSCOMMON) EXTRAINCDIRS += $(PIOSBOARDS) EXTRAINCDIRS += $(STMSPDINCDIR) EXTRAINCDIRS += $(CMSISDIR) -EXTRAINCDIRS += $(INS_BLINC) +EXTRAINCDIRS += $(AHRS_BLINC) # List any extra directories to look for library files here. # Also add directories where the linker should search for @@ -175,7 +170,7 @@ EXTRA_LIBDIRS = EXTRA_LIBS = # Path to Linker-Scripts -LINKERSCRIPTPATH = $(PIOSSTM32F10X) +LINKERSCRIPTPATH = $(PIOSSTM32FXX) # Optimization level, can be [0, 1, 2, 3, s]. # 0 = turn off optimization. s = optimize for size. @@ -202,6 +197,16 @@ DEBUGF = dwarf-2 CDEFS = -DSTM32F10X_$(MODEL) CDEFS += -DUSE_STDPERIPH_DRIVER CDEFS += -DUSE_$(BOARD) + +# Provide (only) the bootloader with board-specific defines +BLONLY_CDEFS += -DBOARD_TYPE=$(BOARD_TYPE) +BLONLY_CDEFS += -DBOARD_REVISION=$(BOARD_REVISION) +BLONLY_CDEFS += -DHW_TYPE=$(HW_TYPE) +BLONLY_CDEFS += -DBOOTLOADER_VERSION=$(BOOTLOADER_VERSION) +BLONLY_CDEFS += -DFW_BANK_BASE=$(FW_BANK_BASE) +BLONLY_CDEFS += -DFW_BANK_SIZE=$(FW_BANK_SIZE) +BLONLY_CDEFS += -DFW_DESC_SIZE=$(FW_DESC_SIZE) + # Place project-specific -D and/or -U options for # Assembler with preprocessor here. #ADEFS = -DUSE_IRQ_ASM_WRAPPER @@ -236,8 +241,9 @@ CFLAGS += -O$(OPT) ifeq ($(DEBUG),NO) CFLAGS += -fdata-sections -ffunction-sections endif -CFLAGS += -mcpu=$(MCU) -mthumb +CFLAGS += -mcpu=$(MCU) CFLAGS += $(CDEFS) +CFLAGS += $(BLONLY_CDEFS) CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) -I. CFLAGS += -mapcs-frame @@ -259,7 +265,7 @@ CONLYFLAGS += $(CSTANDARD) # Assembler flags. # -Wa,...: tell GCC to pass this to the assembler. # -ahlns: create listing -ASFLAGS = -mcpu=$(MCU) -mthumb -I. -x assembler-with-cpp +ASFLAGS = -mcpu=$(MCU) -I. -x assembler-with-cpp ASFLAGS += $(ADEFS) ASFLAGS += -Wa,-adhlns=$(addprefix $(OUTDIR)/, $(notdir $(addsuffix .lst, $(basename $<)))) ASFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) @@ -284,22 +290,8 @@ LDFLAGS += -lc -lgcc LDFLAGS += -T$(LINKERSCRIPTPATH)/link_$(BOARD)_memory.ld LDFLAGS += -T$(LINKERSCRIPTPATH)/link_$(BOARD)_BL_sections.ld -OOCD_LOADFILE+=$(OUTDIR)/$(TARGET).bin -# Program -OOCD_CL+=-c "flash write_image erase $(OOCD_LOADFILE) 0x08000000 bin" -# Verify -OOCD_CL+=-c "verify_image $(OOCD_LOADFILE) 0x08000000 bin" -# reset target -OOCD_CL+=-c "reset run" -# terminate OOCD after programming -OOCD_CL+=-c shutdown - - - # Define programs and commands. REMOVE = $(REMOVE_CMD) -f -###SHELL = sh -###COPY = cp # List of all source files. ALLSRC = $(ASRCARM) $(ASRC) $(SRCARM) $(SRC) $(CPPSRCARM) $(CPPSRC) @@ -331,14 +323,6 @@ endif endif endif -# Program the device. -ifeq ($(FLASH_TOOL),OPENOCD) -# Program the device with Dominic Rath's OPENOCD in "batch-mode", needs cfg and "reset-script". -program: $(OUTDIR)/$(TARGET).bin - @echo "Programming with OPENOCD" - $(OOCD_EXE) $(OOCD_CL) -endif - # Link: create ELF output file from object files. $(eval $(call LINK_TEMPLATE, $(OUTDIR)/$(TARGET).elf, $(ALLOBJ))) @@ -368,7 +352,10 @@ $(eval $(call PARTIAL_COMPILE_ARM_TEMPLATE, SRCARM)) $(OUTDIR)/$(TARGET).bin.o: $(OUTDIR)/$(TARGET).bin -.PHONY: elf lss sym hex bin +# Add jtag targets (program and wipe) +$(eval $(call JTAG_TEMPLATE_F2X,$(OUTDIR)/$(TARGET).bin,$(BL_BANK_BASE),$(BL_BANK_SIZE))) + +.PHONY: elf lss sym hex bin bino elf: $(OUTDIR)/$(TARGET).elf lss: $(OUTDIR)/$(TARGET).lss sym: $(OUTDIR)/$(TARGET).sym @@ -378,8 +365,6 @@ bino: $(OUTDIR)/$(TARGET).bin.o # Display sizes of sections. $(eval $(call SIZE_TEMPLATE, $(OUTDIR)/$(TARGET).elf)) -.PHONY: size -size: $(OUTDIR)/$(TARGET).elf_size # Generate Doxygen documents docs: @@ -406,7 +391,7 @@ clean_list : $(V1) $(REMOVE) $(OUTDIR)/$(TARGET).bin $(V1) $(REMOVE) $(OUTDIR)/$(TARGET).sym $(V1) $(REMOVE) $(OUTDIR)/$(TARGET).lss - $(V1) $(REMOVE) $(OUTDIR)/$(TARGET).bin.o + $(V1) $(REMOVE) $(OUTDIR)/$(TARGET).bin.o $(V1) $(REMOVE) $(ALLOBJ) $(V1) $(REMOVE) $(LSTFILES) $(V1) $(REMOVE) $(DEPFILES) @@ -432,4 +417,4 @@ else endif # Listing of phony targets. -.PHONY : all build clean clean_list program install +.PHONY : all build clean clean_list install diff --git a/flight/Bootloaders/INS/ahrs_slave_test.c b/flight/Bootloaders/INS/ahrs_slave_test.c index 9c2759c82..4ef55b975 100644 --- a/flight/Bootloaders/INS/ahrs_slave_test.c +++ b/flight/Bootloaders/INS/ahrs_slave_test.c @@ -1,38 +1,34 @@ -#include "ins_bl.h" +#include "ahrs_bl.h" #include "ahrs_spi_program.h" uint8_t buf[256]; -bool StartProgramming(void) -{ - PIOS_COM_SendFormattedString(PIOS_COM_AUX,"Started programming\r\n"); - return(true); +bool StartProgramming(void) { + PIOS_COM_SendFormattedString(PIOS_COM_AUX, "Started programming\r\n"); + return (true); } -bool WriteData(uint32_t offset, uint8_t *buffer, uint32_t size) -{ - if(size > SPI_MAX_PROGRAM_DATA_SIZE) - { -PIOS_COM_SendFormattedString(PIOS_COM_AUX,"oversize: %d\r\n", size); - return(false); +bool WriteData(uint32_t offset, uint8_t *buffer, uint32_t size) { + if (size > SPI_MAX_PROGRAM_DATA_SIZE) { + PIOS_COM_SendFormattedString(PIOS_COM_AUX, "oversize: %d\r\n", size); + return (false); } - PIOS_COM_SendFormattedString(PIOS_COM_AUX,"Wrote %d bytes to %d\r\n", size, offset); - memcpy(buf,buffer,size); + PIOS_COM_SendFormattedString(PIOS_COM_AUX, "Wrote %d bytes to %d\r\n", + size, offset); + memcpy(buf, buffer, size); PIOS_LED_Toggle(LED1); - return(true); + return (true); } -bool ReadData(uint32_t offset, uint8_t *buffer, uint32_t size) -{ - if(size > SPI_MAX_PROGRAM_DATA_SIZE) - { -PIOS_COM_SendFormattedString(PIOS_COM_AUX,"oversize: %d\r\n", size); - return(false); +bool ReadData(uint32_t offset, uint8_t *buffer, uint32_t size) { + if (size > SPI_MAX_PROGRAM_DATA_SIZE) { + PIOS_COM_SendFormattedString(PIOS_COM_AUX, "oversize: %d\r\n", size); + return (false); } - PIOS_COM_SendFormattedString(PIOS_COM_AUX,"Read %d bytes from %d\r\n", size, offset); - memcpy(buffer,buf,size); + PIOS_COM_SendFormattedString(PIOS_COM_AUX, "Read %d bytes from %d\r\n", + size, offset); + memcpy(buffer, buf, size); PIOS_LED_Toggle(LED1); - return(true); + return (true); } - diff --git a/flight/Bootloaders/INS/ahrs_spi_program.c b/flight/Bootloaders/INS/ahrs_spi_program.c index 22d48b1a8..c29e9c609 100644 --- a/flight/Bootloaders/INS/ahrs_spi_program.c +++ b/flight/Bootloaders/INS/ahrs_spi_program.c @@ -1,89 +1,65 @@ #include #include "ahrs_spi_program.h" -// Static CRC polynomial table -static uint32_t crcTable[256] = -{ - 0x00000000, 0x77073096, 0xEE0E612C, 0x990951BA, - 0x076DC419, 0x706AF48F, 0xE963A535, 0x9E6495A3, - 0x0EDB8832, 0x79DCB8A4, 0xE0D5E91E, 0x97D2D988, - 0x09B64C2B, 0x7EB17CBD, 0xE7B82D07, 0x90BF1D91, - 0x1DB71064, 0x6AB020F2, 0xF3B97148, 0x84BE41DE, - 0x1ADAD47D, 0x6DDDE4EB, 0xF4D4B551, 0x83D385C7, - 0x136C9856, 0x646BA8C0, 0xFD62F97A, 0x8A65C9EC, - 0x14015C4F, 0x63066CD9, 0xFA0F3D63, 0x8D080DF5, - 0x3B6E20C8, 0x4C69105E, 0xD56041E4, 0xA2677172, - 0x3C03E4D1, 0x4B04D447, 0xD20D85FD, 0xA50AB56B, - 0x35B5A8FA, 0x42B2986C, 0xDBBBC9D6, 0xACBCF940, - 0x32D86CE3, 0x45DF5C75, 0xDCD60DCF, 0xABD13D59, - 0x26D930AC, 0x51DE003A, 0xC8D75180, 0xBFD06116, - 0x21B4F4B5, 0x56B3C423, 0xCFBA9599, 0xB8BDA50F, - 0x2802B89E, 0x5F058808, 0xC60CD9B2, 0xB10BE924, - 0x2F6F7C87, 0x58684C11, 0xC1611DAB, 0xB6662D3D, - - 0x76DC4190, 0x01DB7106, 0x98D220BC, 0xEFD5102A, - 0x71B18589, 0x06B6B51F, 0x9FBFE4A5, 0xE8B8D433, - 0x7807C9A2, 0x0F00F934, 0x9609A88E, 0xE10E9818, - 0x7F6A0DBB, 0x086D3D2D, 0x91646C97, 0xE6635C01, - 0x6B6B51F4, 0x1C6C6162, 0x856530D8, 0xF262004E, - 0x6C0695ED, 0x1B01A57B, 0x8208F4C1, 0xF50FC457, - 0x65B0D9C6, 0x12B7E950, 0x8BBEB8EA, 0xFCB9887C, - 0x62DD1DDF, 0x15DA2D49, 0x8CD37CF3, 0xFBD44C65, - 0x4DB26158, 0x3AB551CE, 0xA3BC0074, 0xD4BB30E2, - 0x4ADFA541, 0x3DD895D7, 0xA4D1C46D, 0xD3D6F4FB, - 0x4369E96A, 0x346ED9FC, 0xAD678846, 0xDA60B8D0, - 0x44042D73, 0x33031DE5, 0xAA0A4C5F, 0xDD0D7CC9, - 0x5005713C, 0x270241AA, 0xBE0B1010, 0xC90C2086, - 0x5768B525, 0x206F85B3, 0xB966D409, 0xCE61E49F, - 0x5EDEF90E, 0x29D9C998, 0xB0D09822, 0xC7D7A8B4, - 0x59B33D17, 0x2EB40D81, 0xB7BD5C3B, 0xC0BA6CAD, - - 0xEDB88320, 0x9ABFB3B6, 0x03B6E20C, 0x74B1D29A, - 0xEAD54739, 0x9DD277AF, 0x04DB2615, 0x73DC1683, - 0xE3630B12, 0x94643B84, 0x0D6D6A3E, 0x7A6A5AA8, - 0xE40ECF0B, 0x9309FF9D, 0x0A00AE27, 0x7D079EB1, - 0xF00F9344, 0x8708A3D2, 0x1E01F268, 0x6906C2FE, - 0xF762575D, 0x806567CB, 0x196C3671, 0x6E6B06E7, - 0xFED41B76, 0x89D32BE0, 0x10DA7A5A, 0x67DD4ACC, - 0xF9B9DF6F, 0x8EBEEFF9, 0x17B7BE43, 0x60B08ED5, - 0xD6D6A3E8, 0xA1D1937E, 0x38D8C2C4, 0x4FDFF252, - 0xD1BB67F1, 0xA6BC5767, 0x3FB506DD, 0x48B2364B, - 0xD80D2BDA, 0xAF0A1B4C, 0x36034AF6, 0x41047A60, - 0xDF60EFC3, 0xA867DF55, 0x316E8EEF, 0x4669BE79, - 0xCB61B38C, 0xBC66831A, 0x256FD2A0, 0x5268E236, - 0xCC0C7795, 0xBB0B4703, 0x220216B9, 0x5505262F, - 0xC5BA3BBE, 0xB2BD0B28, 0x2BB45A92, 0x5CB36A04, - 0xC2D7FFA7, 0xB5D0CF31, 0x2CD99E8B, 0x5BDEAE1D, - - 0x9B64C2B0, 0xEC63F226, 0x756AA39C, 0x026D930A, - 0x9C0906A9, 0xEB0E363F, 0x72076785, 0x05005713, - 0x95BF4A82, 0xE2B87A14, 0x7BB12BAE, 0x0CB61B38, - 0x92D28E9B, 0xE5D5BE0D, 0x7CDCEFB7, 0x0BDBDF21, - 0x86D3D2D4, 0xF1D4E242, 0x68DDB3F8, 0x1FDA836E, - 0x81BE16CD, 0xF6B9265B, 0x6FB077E1, 0x18B74777, - 0x88085AE6, 0xFF0F6A70, 0x66063BCA, 0x11010B5C, - 0x8F659EFF, 0xF862AE69, 0x616BFFD3, 0x166CCF45, - 0xA00AE278, 0xD70DD2EE, 0x4E048354, 0x3903B3C2, - 0xA7672661, 0xD06016F7, 0x4969474D, 0x3E6E77DB, - 0xAED16A4A, 0xD9D65ADC, 0x40DF0B66, 0x37D83BF0, - 0xA9BCAE53, 0xDEBB9EC5, 0x47B2CF7F, 0x30B5FFE9, - 0xBDBDF21C, 0xCABAC28A, 0x53B39330, 0x24B4A3A6, - 0xBAD03605, 0xCDD70693, 0x54DE5729, 0x23D967BF, - 0xB3667A2E, 0xC4614AB8, 0x5D681B02, 0x2A6F2B94, - 0xB40BBE37, 0xC30C8EA1, 0x5A05DF1B, 0x2D02EF8D, -}; +// Static CRC polynomial table +static uint32_t crcTable[256] = { 0x00000000, 0x77073096, 0xEE0E612C, + 0x990951BA, 0x076DC419, 0x706AF48F, 0xE963A535, 0x9E6495A3, 0x0EDB8832, + 0x79DCB8A4, 0xE0D5E91E, 0x97D2D988, 0x09B64C2B, 0x7EB17CBD, 0xE7B82D07, + 0x90BF1D91, 0x1DB71064, 0x6AB020F2, 0xF3B97148, 0x84BE41DE, 0x1ADAD47D, + 0x6DDDE4EB, 0xF4D4B551, 0x83D385C7, 0x136C9856, 0x646BA8C0, 0xFD62F97A, + 0x8A65C9EC, 0x14015C4F, 0x63066CD9, 0xFA0F3D63, 0x8D080DF5, 0x3B6E20C8, + 0x4C69105E, 0xD56041E4, 0xA2677172, 0x3C03E4D1, 0x4B04D447, 0xD20D85FD, + 0xA50AB56B, 0x35B5A8FA, 0x42B2986C, 0xDBBBC9D6, 0xACBCF940, 0x32D86CE3, + 0x45DF5C75, 0xDCD60DCF, 0xABD13D59, 0x26D930AC, 0x51DE003A, 0xC8D75180, + 0xBFD06116, 0x21B4F4B5, 0x56B3C423, 0xCFBA9599, 0xB8BDA50F, 0x2802B89E, + 0x5F058808, 0xC60CD9B2, 0xB10BE924, 0x2F6F7C87, 0x58684C11, 0xC1611DAB, + 0xB6662D3D, + + 0x76DC4190, 0x01DB7106, 0x98D220BC, 0xEFD5102A, 0x71B18589, 0x06B6B51F, + 0x9FBFE4A5, 0xE8B8D433, 0x7807C9A2, 0x0F00F934, 0x9609A88E, 0xE10E9818, + 0x7F6A0DBB, 0x086D3D2D, 0x91646C97, 0xE6635C01, 0x6B6B51F4, 0x1C6C6162, + 0x856530D8, 0xF262004E, 0x6C0695ED, 0x1B01A57B, 0x8208F4C1, 0xF50FC457, + 0x65B0D9C6, 0x12B7E950, 0x8BBEB8EA, 0xFCB9887C, 0x62DD1DDF, 0x15DA2D49, + 0x8CD37CF3, 0xFBD44C65, 0x4DB26158, 0x3AB551CE, 0xA3BC0074, 0xD4BB30E2, + 0x4ADFA541, 0x3DD895D7, 0xA4D1C46D, 0xD3D6F4FB, 0x4369E96A, 0x346ED9FC, + 0xAD678846, 0xDA60B8D0, 0x44042D73, 0x33031DE5, 0xAA0A4C5F, 0xDD0D7CC9, + 0x5005713C, 0x270241AA, 0xBE0B1010, 0xC90C2086, 0x5768B525, 0x206F85B3, + 0xB966D409, 0xCE61E49F, 0x5EDEF90E, 0x29D9C998, 0xB0D09822, 0xC7D7A8B4, + 0x59B33D17, 0x2EB40D81, 0xB7BD5C3B, 0xC0BA6CAD, + + 0xEDB88320, 0x9ABFB3B6, 0x03B6E20C, 0x74B1D29A, 0xEAD54739, 0x9DD277AF, + 0x04DB2615, 0x73DC1683, 0xE3630B12, 0x94643B84, 0x0D6D6A3E, 0x7A6A5AA8, + 0xE40ECF0B, 0x9309FF9D, 0x0A00AE27, 0x7D079EB1, 0xF00F9344, 0x8708A3D2, + 0x1E01F268, 0x6906C2FE, 0xF762575D, 0x806567CB, 0x196C3671, 0x6E6B06E7, + 0xFED41B76, 0x89D32BE0, 0x10DA7A5A, 0x67DD4ACC, 0xF9B9DF6F, 0x8EBEEFF9, + 0x17B7BE43, 0x60B08ED5, 0xD6D6A3E8, 0xA1D1937E, 0x38D8C2C4, 0x4FDFF252, + 0xD1BB67F1, 0xA6BC5767, 0x3FB506DD, 0x48B2364B, 0xD80D2BDA, 0xAF0A1B4C, + 0x36034AF6, 0x41047A60, 0xDF60EFC3, 0xA867DF55, 0x316E8EEF, 0x4669BE79, + 0xCB61B38C, 0xBC66831A, 0x256FD2A0, 0x5268E236, 0xCC0C7795, 0xBB0B4703, + 0x220216B9, 0x5505262F, 0xC5BA3BBE, 0xB2BD0B28, 0x2BB45A92, 0x5CB36A04, + 0xC2D7FFA7, 0xB5D0CF31, 0x2CD99E8B, 0x5BDEAE1D, + + 0x9B64C2B0, 0xEC63F226, 0x756AA39C, 0x026D930A, 0x9C0906A9, 0xEB0E363F, + 0x72076785, 0x05005713, 0x95BF4A82, 0xE2B87A14, 0x7BB12BAE, 0x0CB61B38, + 0x92D28E9B, 0xE5D5BE0D, 0x7CDCEFB7, 0x0BDBDF21, 0x86D3D2D4, 0xF1D4E242, + 0x68DDB3F8, 0x1FDA836E, 0x81BE16CD, 0xF6B9265B, 0x6FB077E1, 0x18B74777, + 0x88085AE6, 0xFF0F6A70, 0x66063BCA, 0x11010B5C, 0x8F659EFF, 0xF862AE69, + 0x616BFFD3, 0x166CCF45, 0xA00AE278, 0xD70DD2EE, 0x4E048354, 0x3903B3C2, + 0xA7672661, 0xD06016F7, 0x4969474D, 0x3E6E77DB, 0xAED16A4A, 0xD9D65ADC, + 0x40DF0B66, 0x37D83BF0, 0xA9BCAE53, 0xDEBB9EC5, 0x47B2CF7F, 0x30B5FFE9, + 0xBDBDF21C, 0xCABAC28A, 0x53B39330, 0x24B4A3A6, 0xBAD03605, 0xCDD70693, + 0x54DE5729, 0x23D967BF, 0xB3667A2E, 0xC4614AB8, 0x5D681B02, 0x2A6F2B94, + 0xB40BBE37, 0xC30C8EA1, 0x5A05DF1B, 0x2D02EF8D, }; /**generate CRC32 from a program packet -this is slightly overkill but we want to be sure -*/ -uint32_t GenerateCRC (AhrsProgramPacket * packet) -{ - uint8_t * ptr = (uint8_t *)packet; - int size = ((int)&packet->crc) - (int)packet; + this is slightly overkill but we want to be sure + */ +uint32_t GenerateCRC(AhrsProgramPacket * packet) { + uint8_t * ptr = (uint8_t *) packet; + int size = ((int) &packet->crc) - (int) packet; uint32_t crc = 0xFFFFFFFF; - for(int ct=0; ct< size; ct++) - { + for (int ct = 0; ct < size; ct++) { crc = ((crc) >> 8) ^ crcTable[(*ptr++) ^ ((crc) & 0x000000FF)]; } - return(~crc); + return (~crc); } diff --git a/flight/Bootloaders/INS/ahrs_spi_program_master.c b/flight/Bootloaders/INS/ahrs_spi_program_master.c index 561c4435a..8227771c0 100644 --- a/flight/Bootloaders/INS/ahrs_spi_program_master.c +++ b/flight/Bootloaders/INS/ahrs_spi_program_master.c @@ -29,22 +29,22 @@ #include "ahrs_spi_program.h" #include "pios_spi.h" -PROGERR TransferPacket(uint32_t spi_id, AhrsProgramPacket *txBuf, AhrsProgramPacket *rxBuf); +PROGERR TransferPacket(uint32_t spi_id, AhrsProgramPacket *txBuf, + AhrsProgramPacket *rxBuf); #define MAX_CONNECT_TRIES 500 //half a second - -bool AhrsProgramConnect(uint32_t spi_id) -{ +bool AhrsProgramConnect(uint32_t spi_id) { AhrsProgramPacket rxBuf; AhrsProgramPacket txBuf; memset(&rxBuf, 0, sizeof(AhrsProgramPacket)); - memcpy(&txBuf,SPI_PROGRAM_REQUEST,SPI_PROGRAM_REQUEST_LENGTH); - for(int ct = 0; ct < MAX_CONNECT_TRIES; ct++) { + memcpy(&txBuf, SPI_PROGRAM_REQUEST, SPI_PROGRAM_REQUEST_LENGTH); + for (int ct = 0; ct < MAX_CONNECT_TRIES; ct++) { PIOS_SPI_RC_PinSet(spi_id, 0); uint32_t res = PIOS_SPI_TransferBlock(spi_id, (uint8_t *) &txBuf, - (uint8_t *) & rxBuf, SPI_PROGRAM_REQUEST_LENGTH +1, NULL); + (uint8_t *) &rxBuf, SPI_PROGRAM_REQUEST_LENGTH + 1, NULL); PIOS_SPI_RC_PinSet(spi_id, 1); - if(res == 0 && memcmp(&rxBuf, SPI_PROGRAM_ACK, SPI_PROGRAM_REQUEST_LENGTH) == 0) { + if (res == 0 && memcmp(&rxBuf, SPI_PROGRAM_ACK, + SPI_PROGRAM_REQUEST_LENGTH) == 0) { return (true); } @@ -53,25 +53,24 @@ bool AhrsProgramConnect(uint32_t spi_id) return (false); } - -PROGERR AhrsProgramWrite(uint32_t spi_id, uint32_t address, void * data, uint32_t size) -{ +PROGERR AhrsProgramWrite(uint32_t spi_id, uint32_t address, void * data, + uint32_t size) { AhrsProgramPacket rxBuf; AhrsProgramPacket txBuf; memset(&rxBuf, 0, sizeof(AhrsProgramPacket)); - memcpy(txBuf.data,data,size); + memcpy(txBuf.data, data, size); txBuf.size = size; txBuf.type = PROGRAM_WRITE; txBuf.address = address; PROGERR ret = TransferPacket(spi_id, &txBuf, &rxBuf); - if(ret != PROGRAM_ERR_OK) { - return(ret); + if (ret != PROGRAM_ERR_OK) { + return (ret); } - return(PROGRAM_ERR_OK); + return (PROGRAM_ERR_OK); } -PROGERR AhrsProgramRead(uint32_t spi_id, uint32_t address, void * data, uint32_t size) -{ +PROGERR AhrsProgramRead(uint32_t spi_id, uint32_t address, void * data, + uint32_t size) { AhrsProgramPacket rxBuf; AhrsProgramPacket txBuf; memset(&rxBuf, 0, sizeof(AhrsProgramPacket)); @@ -79,59 +78,54 @@ PROGERR AhrsProgramRead(uint32_t spi_id, uint32_t address, void * data, uint32_t txBuf.type = PROGRAM_READ; txBuf.address = address; PROGERR ret = TransferPacket(spi_id, &txBuf, &rxBuf); - if(ret != PROGRAM_ERR_OK) { - return(ret); + if (ret != PROGRAM_ERR_OK) { + return (ret); } memcpy(data, rxBuf.data, size); - return(PROGRAM_ERR_OK); + return (PROGRAM_ERR_OK); } -PROGERR AhrsProgramReboot(uint32_t spi_id) -{ +PROGERR AhrsProgramReboot(uint32_t spi_id) { AhrsProgramPacket rxBuf; AhrsProgramPacket txBuf; memset(&rxBuf, 0, sizeof(AhrsProgramPacket)); txBuf.type = PROGRAM_REBOOT; - memcpy(txBuf.data,REBOOT_CONFIRMATION,REBOOT_CONFIRMATION_LENGTH); + memcpy(txBuf.data, REBOOT_CONFIRMATION, REBOOT_CONFIRMATION_LENGTH); PROGERR ret = TransferPacket(spi_id, &txBuf, &rxBuf); //If AHRS has rebooted we will get comms errors - if(ret == PROGRAM_ERR_LINK) { - return(PROGRAM_ERR_OK); + if (ret == PROGRAM_ERR_LINK) { + return (PROGRAM_ERR_OK); } - return(PROGRAM_ERR_FUNCTION); + return (PROGRAM_ERR_FUNCTION); } -PROGERR TransferPacket(uint32_t spi_id, AhrsProgramPacket *txBuf, AhrsProgramPacket *rxBuf) -{ +PROGERR TransferPacket(uint32_t spi_id, AhrsProgramPacket *txBuf, + AhrsProgramPacket *rxBuf) { static uint32_t pktId = 0; pktId++; txBuf->packetId = pktId; txBuf->crc = GenerateCRC(txBuf); int ct = 0; - for(; ct < MAX_CONNECT_TRIES; ct++) { + for (; ct < MAX_CONNECT_TRIES; ct++) { PIOS_SPI_RC_PinSet(spi_id, 0); uint32_t res = PIOS_SPI_TransferBlock(spi_id, (uint8_t *) txBuf, - (uint8_t *) rxBuf, sizeof(AhrsProgramPacket), NULL); + (uint8_t *) rxBuf, sizeof(AhrsProgramPacket), NULL); PIOS_SPI_RC_PinSet(spi_id, 1); - if(res == 0) { - if(rxBuf->type != PROGRAM_NULL && - rxBuf->crc == GenerateCRC(rxBuf) && - rxBuf->packetId == pktId) { + if (res == 0) { + if (rxBuf->type != PROGRAM_NULL && rxBuf->crc == GenerateCRC(rxBuf) + && rxBuf->packetId == pktId) { break; } } vTaskDelay(1 / portTICK_RATE_MS); } - if(ct == MAX_CONNECT_TRIES) { + if (ct == MAX_CONNECT_TRIES) { return (PROGRAM_ERR_LINK); } - if(rxBuf->type != PROGRAM_ACK) { - return(PROGRAM_ERR_FUNCTION); + if (rxBuf->type != PROGRAM_ACK) { + return (PROGRAM_ERR_FUNCTION); } - return(PROGRAM_ERR_OK); + return (PROGRAM_ERR_OK); } - - - diff --git a/flight/Bootloaders/INS/ahrs_spi_program_slave.c b/flight/Bootloaders/INS/ahrs_spi_program_slave.c index a7d011448..8fc3de482 100644 --- a/flight/Bootloaders/INS/ahrs_spi_program_slave.c +++ b/flight/Bootloaders/INS/ahrs_spi_program_slave.c @@ -24,15 +24,14 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ - #include #include #include "pios_opahrs_proto.h" #include "pios_spi.h" -#include "STM3210E_INS.h" +#include "STM32103CB_AHRS.h" -#include "ins_bl.h" +#include "ahrs_bl.h" #include "ahrs_spi_program_slave.h" #include "ahrs_spi_program.h" @@ -43,53 +42,52 @@ static bool done = false; static void ProcessPacket(); #define WAIT_IF_RECEIVING() while(!(GPIOB->IDR & GPIO_Pin_12)){}; //NSS must be high - //Number of crc failures to allow before giving up #define PROGRAM_PACKET_TRIES 4 -void AhrsProgramReceive(uint32_t spi_id) -{ +void AhrsProgramReceive(uint32_t spi_id) { done = false; - memset(&txBuf,0,sizeof(AhrsProgramPacket)); + memset(&txBuf, 0, sizeof(AhrsProgramPacket)); //wait for a program request int count = PROGRAM_PACKET_TRIES; - while(1) { + while (1) { WAIT_IF_RECEIVING(); - while((PIOS_SPI_Busy(spi_id) != 0)){}; - memset(&rxBuf,'a',sizeof(AhrsProgramPacket)); + while ((PIOS_SPI_Busy(spi_id) != 0)) { + }; + memset(&rxBuf, 'a', sizeof(AhrsProgramPacket)); int32_t res = PIOS_SPI_TransferBlock(spi_id, NULL, (uint8_t*) &rxBuf, - SPI_PROGRAM_REQUEST_LENGTH + 1, NULL); + SPI_PROGRAM_REQUEST_LENGTH + 1, NULL); - if(res == 0 && - memcmp(&rxBuf, SPI_PROGRAM_REQUEST, SPI_PROGRAM_REQUEST_LENGTH) == 0) { + if (res == 0 && memcmp(&rxBuf, SPI_PROGRAM_REQUEST, + SPI_PROGRAM_REQUEST_LENGTH) == 0) { break; } - if(count-- == 0) { + if (count-- == 0) { return; } } - if(!StartProgramming()) { + if (!StartProgramming()) { //Couldn't erase FLASH. Nothing we can do. return; } //send ack - memcpy(&txBuf,SPI_PROGRAM_ACK,SPI_PROGRAM_REQUEST_LENGTH); + memcpy(&txBuf, SPI_PROGRAM_ACK, SPI_PROGRAM_REQUEST_LENGTH); WAIT_IF_RECEIVING(); - while(0 != PIOS_SPI_TransferBlock(spi_id,(uint8_t*) &txBuf, NULL, - SPI_PROGRAM_REQUEST_LENGTH + 1, NULL)) {}; + while (0 != PIOS_SPI_TransferBlock(spi_id, (uint8_t*) &txBuf, NULL, + SPI_PROGRAM_REQUEST_LENGTH + 1, NULL)) { + }; txBuf.type = PROGRAM_NULL; - while(!done) { + while (!done) { WAIT_IF_RECEIVING(); - if(0 == PIOS_SPI_TransferBlock(spi_id,(uint8_t*) &txBuf, - (uint8_t*) &rxBuf, sizeof(AhrsProgramPacket), NULL)) { + if (0 == PIOS_SPI_TransferBlock(spi_id, (uint8_t*) &txBuf, + (uint8_t*) &rxBuf, sizeof(AhrsProgramPacket), NULL)) { uint32_t crc = GenerateCRC(&rxBuf); - if(crc != rxBuf.crc || - txBuf.packetId == rxBuf.packetId) { + if (crc != rxBuf.crc || txBuf.packetId == rxBuf.packetId) { continue; } ProcessPacket(); @@ -99,18 +97,14 @@ void AhrsProgramReceive(uint32_t spi_id) } } - - - -void ProcessPacket() -{ - switch(rxBuf.type) { +void ProcessPacket() { + switch (rxBuf.type) { case PROGRAM_NULL: txBuf.type = PROGRAM_NULL; break; case PROGRAM_WRITE: - if(WriteData(rxBuf.address, rxBuf.data, rxBuf.size)) { + if (WriteData(rxBuf.address, rxBuf.data, rxBuf.size)) { txBuf.type = PROGRAM_ACK; txBuf.size = rxBuf.size; } else { @@ -119,7 +113,7 @@ void ProcessPacket() break; case PROGRAM_READ: - if(ReadData(rxBuf.address, txBuf.data, rxBuf.size)) { + if (ReadData(rxBuf.address, txBuf.data, rxBuf.size)) { txBuf.type = PROGRAM_ACK; txBuf.size = rxBuf.size; } else { @@ -128,10 +122,11 @@ void ProcessPacket() break; case PROGRAM_REBOOT: - if(0 == memcmp(rxBuf.data,REBOOT_CONFIRMATION,REBOOT_CONFIRMATION_LENGTH)) { + if (0 == memcmp(rxBuf.data, REBOOT_CONFIRMATION, + REBOOT_CONFIRMATION_LENGTH)) { done = true; txBuf.type = PROGRAM_ACK; - }else{ + } else { txBuf.type = PROGRAM_ERR; } break; diff --git a/flight/Bootloaders/INS/bl_fsm.c b/flight/Bootloaders/INS/bl_fsm.c index 48126037e..10e26a46e 100644 --- a/flight/Bootloaders/INS/bl_fsm.c +++ b/flight/Bootloaders/INS/bl_fsm.c @@ -8,34 +8,33 @@ #include "pios.h" struct lfsm_context { - enum lfsm_state curr_state; - enum opahrs_msg_link_state link_state; - enum opahrs_msg_type user_payload_type; - uint32_t user_payload_len; + enum lfsm_state curr_state; + enum opahrs_msg_link_state link_state; + enum opahrs_msg_type user_payload_type; + uint32_t user_payload_len; - uint32_t errors; + uint32_t errors; - uint8_t * rx; - uint8_t * tx; + uint8_t * rx; + uint8_t * tx; - uint8_t * link_rx; - uint8_t * link_tx; + uint8_t * link_rx; + uint8_t * link_tx; - uint8_t * user_rx; - uint8_t * user_tx; + uint8_t * user_rx; + uint8_t * user_tx; - struct lfsm_link_stats stats; + struct lfsm_link_stats stats; }; static struct lfsm_context context = { 0 }; -static void lfsm_update_link_tx (struct lfsm_context * context); -static void lfsm_init_rx (struct lfsm_context * context); +static void lfsm_update_link_tx(struct lfsm_context * context); +static void lfsm_init_rx(struct lfsm_context * context); static uint32_t PIOS_SPI_OP; -void lfsm_attach(uint32_t spi_id) -{ - PIOS_SPI_OP = spi_id; +void lfsm_attach(uint32_t spi_id) { + PIOS_SPI_OP = spi_id; } /* @@ -45,8 +44,8 @@ void lfsm_attach(uint32_t spi_id) */ struct lfsm_transition { - void (*entry_fn)(struct lfsm_context * context); - enum lfsm_state next_state[LFSM_EVENT_NUM_EVENTS]; + void (*entry_fn)(struct lfsm_context * context); + enum lfsm_state next_state[LFSM_EVENT_NUM_EVENTS]; }; static void go_faulted(struct lfsm_context * context); @@ -65,312 +64,292 @@ static void go_user_tx_active(struct lfsm_context * context); static void go_user_rxtx_active(struct lfsm_context * context); const static struct lfsm_transition lfsm_transitions[LFSM_STATE_NUM_STATES] = { - [LFSM_STATE_FAULTED] = { - .entry_fn = go_faulted, - }, - [LFSM_STATE_STOPPED] = { - .entry_fn = go_stopped, - .next_state = { - [LFSM_EVENT_INIT_LINK] = LFSM_STATE_INACTIVE, - [LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_STOPPED, - }, - }, - [LFSM_STATE_STOPPING] = { - .entry_fn = go_stopping, - .next_state = { - [LFSM_EVENT_RX_LINK] = LFSM_STATE_STOPPED, - [LFSM_EVENT_RX_USER] = LFSM_STATE_STOPPED, - [LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_STOPPED, - }, - }, - [LFSM_STATE_INACTIVE] = { - .entry_fn = go_inactive, - .next_state = { - [LFSM_EVENT_STOP] = LFSM_STATE_STOPPING, - [LFSM_EVENT_USER_SET_RX] = LFSM_STATE_USER_BUSY_RX_PENDING, - [LFSM_EVENT_USER_SET_TX] = LFSM_STATE_USER_BUSY_TX_PENDING, - [LFSM_EVENT_RX_LINK] = LFSM_STATE_INACTIVE, - [LFSM_EVENT_RX_USER] = LFSM_STATE_INACTIVE, - [LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_INACTIVE, - }, - }, - [LFSM_STATE_USER_BUSY] = { - .entry_fn = go_user_busy, - .next_state = { - [LFSM_EVENT_STOP] = LFSM_STATE_STOPPING, - [LFSM_EVENT_USER_SET_RX] = LFSM_STATE_USER_BUSY_RX_PENDING, - [LFSM_EVENT_USER_SET_TX] = LFSM_STATE_USER_BUSY_TX_PENDING, - [LFSM_EVENT_USER_DONE] = LFSM_STATE_INACTIVE, - [LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_BUSY, - [LFSM_EVENT_RX_USER] = LFSM_STATE_USER_BUSY, - [LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_BUSY, - }, - }, - [LFSM_STATE_USER_BUSY_RX_PENDING] = { - .entry_fn = go_user_busy_rx_pending, - .next_state = { - [LFSM_EVENT_USER_SET_TX] = LFSM_STATE_USER_BUSY_RXTX_PENDING, - [LFSM_EVENT_USER_DONE] = LFSM_STATE_USER_RX_PENDING, - [LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_BUSY_RX_PENDING, - [LFSM_EVENT_RX_USER] = LFSM_STATE_USER_BUSY_RX_PENDING, - [LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_BUSY_RX_PENDING, - }, - }, - [LFSM_STATE_USER_BUSY_TX_PENDING] = { - .entry_fn = go_user_busy_tx_pending, - .next_state = { - [LFSM_EVENT_USER_SET_RX] = LFSM_STATE_USER_BUSY_RXTX_PENDING, - [LFSM_EVENT_USER_DONE] = LFSM_STATE_USER_TX_PENDING, - [LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_BUSY_TX_PENDING, - [LFSM_EVENT_RX_USER] = LFSM_STATE_USER_BUSY_TX_PENDING, - [LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_BUSY_TX_PENDING, - }, - }, - [LFSM_STATE_USER_BUSY_RXTX_PENDING] = { - .entry_fn = go_user_busy_rxtx_pending, - .next_state = { - [LFSM_EVENT_USER_DONE] = LFSM_STATE_USER_RXTX_PENDING, - [LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_BUSY_RXTX_PENDING, - [LFSM_EVENT_RX_USER] = LFSM_STATE_USER_BUSY_RXTX_PENDING, - [LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_BUSY_RXTX_PENDING, - }, - }, - [LFSM_STATE_USER_RX_PENDING] = { - .entry_fn = go_user_rx_pending, - .next_state = { - [LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_RX_ACTIVE, - [LFSM_EVENT_RX_USER] = LFSM_STATE_USER_RX_ACTIVE, - [LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_RX_ACTIVE, - }, - }, - [LFSM_STATE_USER_TX_PENDING] = { - .entry_fn = go_user_tx_pending, - .next_state = { - [LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_TX_ACTIVE, - [LFSM_EVENT_RX_USER] = LFSM_STATE_USER_TX_ACTIVE, - [LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_TX_ACTIVE, - }, - }, - [LFSM_STATE_USER_RXTX_PENDING] = { - .entry_fn = go_user_rxtx_pending, - .next_state = { - [LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_RXTX_ACTIVE, - [LFSM_EVENT_RX_USER] = LFSM_STATE_USER_RXTX_ACTIVE, - [LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_RXTX_ACTIVE, - }, - }, - [LFSM_STATE_USER_RX_ACTIVE] = { - .entry_fn = go_user_rx_active, - .next_state = { - [LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_RX_ACTIVE, - [LFSM_EVENT_RX_USER] = LFSM_STATE_USER_BUSY, - [LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_RX_ACTIVE, - }, - }, - [LFSM_STATE_USER_TX_ACTIVE] = { - .entry_fn = go_user_tx_active, - .next_state = { - [LFSM_EVENT_RX_LINK] = LFSM_STATE_INACTIVE, - [LFSM_EVENT_RX_USER] = LFSM_STATE_INACTIVE, - [LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_INACTIVE, - }, - }, - [LFSM_STATE_USER_RXTX_ACTIVE] = { - .entry_fn = go_user_rxtx_active, - .next_state = { - [LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_RX_ACTIVE, - [LFSM_EVENT_RX_USER] = LFSM_STATE_USER_BUSY, - [LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_RX_ACTIVE, - }, - }, -}; + [LFSM_STATE_FAULTED] = { + .entry_fn = go_faulted, + }, [LFSM_STATE_STOPPED] = { + .entry_fn = go_stopped, + .next_state = { + [LFSM_EVENT_INIT_LINK] = LFSM_STATE_INACTIVE, + [LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_STOPPED, + }, + }, [LFSM_STATE_STOPPING] = { + .entry_fn = go_stopping, + .next_state = { + [LFSM_EVENT_RX_LINK] = LFSM_STATE_STOPPED, + [LFSM_EVENT_RX_USER] = LFSM_STATE_STOPPED, + [LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_STOPPED, + }, + }, [LFSM_STATE_INACTIVE] = { + .entry_fn = go_inactive, + .next_state = { + [LFSM_EVENT_STOP] = LFSM_STATE_STOPPING, + [LFSM_EVENT_USER_SET_RX] = LFSM_STATE_USER_BUSY_RX_PENDING, + [LFSM_EVENT_USER_SET_TX] = LFSM_STATE_USER_BUSY_TX_PENDING, + [LFSM_EVENT_RX_LINK] = LFSM_STATE_INACTIVE, + [LFSM_EVENT_RX_USER] = LFSM_STATE_INACTIVE, + [LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_INACTIVE, + }, + }, [LFSM_STATE_USER_BUSY] = { + .entry_fn = go_user_busy, + .next_state = { + [LFSM_EVENT_STOP] = LFSM_STATE_STOPPING, + [LFSM_EVENT_USER_SET_RX] = LFSM_STATE_USER_BUSY_RX_PENDING, + [LFSM_EVENT_USER_SET_TX] = LFSM_STATE_USER_BUSY_TX_PENDING, + [LFSM_EVENT_USER_DONE] = LFSM_STATE_INACTIVE, + [LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_BUSY, + [LFSM_EVENT_RX_USER] = LFSM_STATE_USER_BUSY, + [LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_BUSY, + }, + }, [LFSM_STATE_USER_BUSY_RX_PENDING] = { + .entry_fn = go_user_busy_rx_pending, + .next_state = { + [LFSM_EVENT_USER_SET_TX] = LFSM_STATE_USER_BUSY_RXTX_PENDING, + [LFSM_EVENT_USER_DONE] = LFSM_STATE_USER_RX_PENDING, + [LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_BUSY_RX_PENDING, + [LFSM_EVENT_RX_USER] = LFSM_STATE_USER_BUSY_RX_PENDING, + [LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_BUSY_RX_PENDING, + }, + }, [LFSM_STATE_USER_BUSY_TX_PENDING] = { + .entry_fn = go_user_busy_tx_pending, + .next_state = { + [LFSM_EVENT_USER_SET_RX] = LFSM_STATE_USER_BUSY_RXTX_PENDING, + [LFSM_EVENT_USER_DONE] = LFSM_STATE_USER_TX_PENDING, + [LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_BUSY_TX_PENDING, + [LFSM_EVENT_RX_USER] = LFSM_STATE_USER_BUSY_TX_PENDING, + [LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_BUSY_TX_PENDING, + }, + }, [LFSM_STATE_USER_BUSY_RXTX_PENDING] = { + .entry_fn = go_user_busy_rxtx_pending, + .next_state = { + [LFSM_EVENT_USER_DONE] = LFSM_STATE_USER_RXTX_PENDING, + [LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_BUSY_RXTX_PENDING, + [LFSM_EVENT_RX_USER] = LFSM_STATE_USER_BUSY_RXTX_PENDING, + [LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_BUSY_RXTX_PENDING, + }, + }, [LFSM_STATE_USER_RX_PENDING] = { + .entry_fn = go_user_rx_pending, + .next_state = { + [LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_RX_ACTIVE, + [LFSM_EVENT_RX_USER] = LFSM_STATE_USER_RX_ACTIVE, + [LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_RX_ACTIVE, + }, + }, [LFSM_STATE_USER_TX_PENDING] = { + .entry_fn = go_user_tx_pending, + .next_state = { + [LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_TX_ACTIVE, + [LFSM_EVENT_RX_USER] = LFSM_STATE_USER_TX_ACTIVE, + [LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_TX_ACTIVE, + }, + }, [LFSM_STATE_USER_RXTX_PENDING] = { + .entry_fn = go_user_rxtx_pending, + .next_state = { + [LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_RXTX_ACTIVE, + [LFSM_EVENT_RX_USER] = LFSM_STATE_USER_RXTX_ACTIVE, + [LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_RXTX_ACTIVE, + }, + }, [LFSM_STATE_USER_RX_ACTIVE] = { + .entry_fn = go_user_rx_active, + .next_state = { + [LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_RX_ACTIVE, + [LFSM_EVENT_RX_USER] = LFSM_STATE_USER_BUSY, + [LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_RX_ACTIVE, + }, + }, [LFSM_STATE_USER_TX_ACTIVE] = { + .entry_fn = go_user_tx_active, + .next_state = { + [LFSM_EVENT_RX_LINK] = LFSM_STATE_INACTIVE, + [LFSM_EVENT_RX_USER] = LFSM_STATE_INACTIVE, + [LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_INACTIVE, + }, + }, [LFSM_STATE_USER_RXTX_ACTIVE] = { + .entry_fn = go_user_rxtx_active, + .next_state = { + [LFSM_EVENT_RX_LINK] = LFSM_STATE_USER_RX_ACTIVE, + [LFSM_EVENT_RX_USER] = LFSM_STATE_USER_BUSY, + [LFSM_EVENT_RX_UNKNOWN] = LFSM_STATE_USER_RX_ACTIVE, + }, + }, }; /* * FSM State Entry Functions */ -static void go_faulted(struct lfsm_context * context) -{ - PIOS_DEBUG_Assert(0); +static void go_faulted(struct lfsm_context * context) { + PIOS_DEBUG_Assert(0); } -static void go_stopped(struct lfsm_context * context) -{ +static void go_stopped(struct lfsm_context * context) { #if 0 - PIOS_SPI_Stop(PIOS_SPI_OP); + PIOS_SPI_Stop(PIOS_SPI_OP); #endif } -static void go_stopping(struct lfsm_context * context) -{ - context->link_tx = NULL; - context->tx = NULL; +static void go_stopping(struct lfsm_context * context) { + context->link_tx = NULL; + context->tx = NULL; } -static void go_inactive(struct lfsm_context * context) -{ - context->link_state = OPAHRS_MSG_LINK_STATE_INACTIVE; - lfsm_update_link_tx(context); +static void go_inactive(struct lfsm_context * context) { + context->link_state = OPAHRS_MSG_LINK_STATE_INACTIVE; + lfsm_update_link_tx(context); - context->user_rx = NULL; - context->user_tx = NULL; + context->user_rx = NULL; + context->user_tx = NULL; - context->rx = context->link_rx; - context->tx = context->link_tx; + context->rx = context->link_rx; + context->tx = context->link_tx; - lfsm_init_rx(context); - PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, context->user_payload_len, lfsm_irq_callback); + lfsm_init_rx(context); + PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, + context->user_payload_len, lfsm_irq_callback); } -static void go_user_busy(struct lfsm_context * context) -{ - /* Sanity checks */ - PIOS_DEBUG_Assert(context->user_rx); +static void go_user_busy(struct lfsm_context * context) { + /* Sanity checks */ + PIOS_DEBUG_Assert(context->user_rx); - context->user_rx = NULL; - context->user_tx = NULL; + context->user_rx = NULL; + context->user_tx = NULL; - context->link_state = OPAHRS_MSG_LINK_STATE_BUSY; - lfsm_update_link_tx(context); + context->link_state = OPAHRS_MSG_LINK_STATE_BUSY; + lfsm_update_link_tx(context); - context->rx = context->link_rx; - context->tx = context->link_tx; + context->rx = context->link_rx; + context->tx = context->link_tx; - lfsm_init_rx(context); - PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, context->user_payload_len, lfsm_irq_callback); + lfsm_init_rx(context); + PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, + context->user_payload_len, lfsm_irq_callback); } -static void go_user_busy_rx_pending(struct lfsm_context * context) -{ - /* Sanity checks */ - PIOS_DEBUG_Assert(context->user_rx); +static void go_user_busy_rx_pending(struct lfsm_context * context) { + /* Sanity checks */ + PIOS_DEBUG_Assert(context->user_rx); - context->link_state = OPAHRS_MSG_LINK_STATE_BUSY; - lfsm_update_link_tx(context); + context->link_state = OPAHRS_MSG_LINK_STATE_BUSY; + lfsm_update_link_tx(context); - context->rx = context->link_rx; - context->tx = context->link_tx; + context->rx = context->link_rx; + context->tx = context->link_tx; - lfsm_init_rx(context); - PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, context->user_payload_len, lfsm_irq_callback); + lfsm_init_rx(context); + PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, + context->user_payload_len, lfsm_irq_callback); } -static void go_user_busy_tx_pending(struct lfsm_context * context) -{ - /* Sanity checks */ - PIOS_DEBUG_Assert(context->user_tx); +static void go_user_busy_tx_pending(struct lfsm_context * context) { + /* Sanity checks */ + PIOS_DEBUG_Assert(context->user_tx); - context->link_state = OPAHRS_MSG_LINK_STATE_BUSY; - lfsm_update_link_tx(context); + context->link_state = OPAHRS_MSG_LINK_STATE_BUSY; + lfsm_update_link_tx(context); - context->rx = context->link_rx; - context->tx = context->link_tx; + context->rx = context->link_rx; + context->tx = context->link_tx; - lfsm_init_rx(context); - PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, context->user_payload_len, lfsm_irq_callback); + lfsm_init_rx(context); + PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, + context->user_payload_len, lfsm_irq_callback); } -static void go_user_busy_rxtx_pending(struct lfsm_context * context) -{ - /* Sanity checks */ - PIOS_DEBUG_Assert(context->user_rx); - PIOS_DEBUG_Assert(context->user_tx); +static void go_user_busy_rxtx_pending(struct lfsm_context * context) { + /* Sanity checks */ + PIOS_DEBUG_Assert(context->user_rx); PIOS_DEBUG_Assert(context->user_tx); - context->link_state = OPAHRS_MSG_LINK_STATE_BUSY; - lfsm_update_link_tx(context); + context->link_state = OPAHRS_MSG_LINK_STATE_BUSY; + lfsm_update_link_tx(context); - context->rx = context->link_rx; - context->tx = context->link_tx; + context->rx = context->link_rx; + context->tx = context->link_tx; - lfsm_init_rx(context); - PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, context->user_payload_len, lfsm_irq_callback); + lfsm_init_rx(context); + PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, + context->user_payload_len, lfsm_irq_callback); } -static void go_user_rx_pending(struct lfsm_context * context) -{ - /* Sanity checks */ - PIOS_DEBUG_Assert(context->user_rx); +static void go_user_rx_pending(struct lfsm_context * context) { + /* Sanity checks */ + PIOS_DEBUG_Assert(context->user_rx); - context->link_state = OPAHRS_MSG_LINK_STATE_BUSY; - lfsm_update_link_tx(context); + context->link_state = OPAHRS_MSG_LINK_STATE_BUSY; + lfsm_update_link_tx(context); - context->rx = context->link_rx; - context->tx = context->link_tx; + context->rx = context->link_rx; + context->tx = context->link_tx; - lfsm_init_rx(context); - PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, context->user_payload_len, lfsm_irq_callback); + lfsm_init_rx(context); + PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, + context->user_payload_len, lfsm_irq_callback); } -static void go_user_tx_pending(struct lfsm_context * context) -{ - /* Sanity checks */ - PIOS_DEBUG_Assert(context->user_tx); +static void go_user_tx_pending(struct lfsm_context * context) { + /* Sanity checks */ + PIOS_DEBUG_Assert(context->user_tx); - context->link_state = OPAHRS_MSG_LINK_STATE_BUSY; - lfsm_update_link_tx(context); + context->link_state = OPAHRS_MSG_LINK_STATE_BUSY; + lfsm_update_link_tx(context); - context->rx = context->link_rx; - context->tx = context->link_tx; + context->rx = context->link_rx; + context->tx = context->link_tx; - lfsm_init_rx(context); - PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, context->user_payload_len, lfsm_irq_callback); + lfsm_init_rx(context); + PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, + context->user_payload_len, lfsm_irq_callback); } -static void go_user_rxtx_pending(struct lfsm_context * context) -{ - /* Sanity checks */ - PIOS_DEBUG_Assert(context->user_rx); - PIOS_DEBUG_Assert(context->user_tx); +static void go_user_rxtx_pending(struct lfsm_context * context) { + /* Sanity checks */ + PIOS_DEBUG_Assert(context->user_rx); PIOS_DEBUG_Assert(context->user_tx); - context->link_state = OPAHRS_MSG_LINK_STATE_BUSY; - lfsm_update_link_tx(context); + context->link_state = OPAHRS_MSG_LINK_STATE_BUSY; + lfsm_update_link_tx(context); - context->rx = context->link_rx; - context->tx = context->link_tx; + context->rx = context->link_rx; + context->tx = context->link_tx; - lfsm_init_rx(context); - PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, context->user_payload_len, lfsm_irq_callback); + lfsm_init_rx(context); + PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, + context->user_payload_len, lfsm_irq_callback); } -static void go_user_rx_active(struct lfsm_context * context) -{ - /* Sanity checks */ - PIOS_DEBUG_Assert(context->user_rx); +static void go_user_rx_active(struct lfsm_context * context) { + /* Sanity checks */ + PIOS_DEBUG_Assert(context->user_rx); - context->rx = context->user_rx; - context->tx = context->link_tx; - context->link_state = OPAHRS_MSG_LINK_STATE_READY; + context->rx = context->user_rx; + context->tx = context->link_tx; + context->link_state = OPAHRS_MSG_LINK_STATE_READY; - lfsm_update_link_tx(context); - lfsm_init_rx(context); - PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, context->user_payload_len, lfsm_irq_callback); + lfsm_update_link_tx(context); + lfsm_init_rx(context); + PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, + context->user_payload_len, lfsm_irq_callback); } -static void go_user_tx_active(struct lfsm_context * context) -{ - /* Sanity checks */ - PIOS_DEBUG_Assert(context->user_tx); +static void go_user_tx_active(struct lfsm_context * context) { + /* Sanity checks */ + PIOS_DEBUG_Assert(context->user_tx); - context->link_state = OPAHRS_MSG_LINK_STATE_BUSY; - context->rx = context->link_rx; - context->tx = context->user_tx; + context->link_state = OPAHRS_MSG_LINK_STATE_BUSY; + context->rx = context->link_rx; + context->tx = context->user_tx; - lfsm_init_rx(context); - PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, context->user_payload_len, lfsm_irq_callback); + lfsm_init_rx(context); + PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, + context->user_payload_len, lfsm_irq_callback); } -static void go_user_rxtx_active(struct lfsm_context * context) -{ - /* Sanity checks */ - PIOS_DEBUG_Assert(context->user_rx); - PIOS_DEBUG_Assert(context->user_tx); +static void go_user_rxtx_active(struct lfsm_context * context) { + /* Sanity checks */ + PIOS_DEBUG_Assert(context->user_rx); PIOS_DEBUG_Assert(context->user_tx); - context->link_state = OPAHRS_MSG_LINK_STATE_READY; - context->rx = context->user_rx; - context->tx = context->user_tx; + context->link_state = OPAHRS_MSG_LINK_STATE_READY; + context->rx = context->user_rx; + context->tx = context->user_tx; - lfsm_init_rx(context); - PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, context->user_payload_len, lfsm_irq_callback); + lfsm_init_rx(context); + PIOS_SPI_TransferBlock(PIOS_SPI_OP, context->tx, context->rx, + context->user_payload_len, lfsm_irq_callback); } /* @@ -379,52 +358,52 @@ static void go_user_rxtx_active(struct lfsm_context * context) * */ -static void lfsm_update_link_tx_v0 (struct opahrs_msg_v0 * msg, enum opahrs_msg_link_state state, uint16_t errors) -{ - opahrs_msg_v0_init_link_tx(msg, OPAHRS_MSG_LINK_TAG_NOP); +static void lfsm_update_link_tx_v0(struct opahrs_msg_v0 * msg, + enum opahrs_msg_link_state state, uint16_t errors) { + opahrs_msg_v0_init_link_tx(msg, OPAHRS_MSG_LINK_TAG_NOP); - msg->payload.link.state = state; - msg->payload.link.errors = errors; + msg->payload.link.state = state; + msg->payload.link.errors = errors; } -static void lfsm_update_link_tx_v1 (struct opahrs_msg_v1 * msg, enum opahrs_msg_link_state state, uint16_t errors) -{ - opahrs_msg_v1_init_link_tx(msg, OPAHRS_MSG_LINK_TAG_NOP); +static void lfsm_update_link_tx_v1(struct opahrs_msg_v1 * msg, + enum opahrs_msg_link_state state, uint16_t errors) { + opahrs_msg_v1_init_link_tx(msg, OPAHRS_MSG_LINK_TAG_NOP); - msg->payload.link.state = state; - msg->payload.link.errors = errors; + msg->payload.link.state = state; + msg->payload.link.errors = errors; } -static void lfsm_update_link_tx (struct lfsm_context * context) -{ - PIOS_DEBUG_Assert(context->link_tx); +static void lfsm_update_link_tx(struct lfsm_context * context) { + PIOS_DEBUG_Assert(context->link_tx); - switch (context->user_payload_type) { - case OPAHRS_MSG_TYPE_USER_V0: - lfsm_update_link_tx_v0 ((struct opahrs_msg_v0 *)context->link_tx, context->link_state, context->errors); - break; - case OPAHRS_MSG_TYPE_USER_V1: - lfsm_update_link_tx_v1 ((struct opahrs_msg_v1 *)context->link_tx, context->link_state, context->errors); - break; - case OPAHRS_MSG_TYPE_LINK: - PIOS_DEBUG_Assert(0); - } + switch (context->user_payload_type) { + case OPAHRS_MSG_TYPE_USER_V0: + lfsm_update_link_tx_v0((struct opahrs_msg_v0 *) context->link_tx, + context->link_state, context->errors); + break; + case OPAHRS_MSG_TYPE_USER_V1: + lfsm_update_link_tx_v1((struct opahrs_msg_v1 *) context->link_tx, + context->link_state, context->errors); + break; + case OPAHRS_MSG_TYPE_LINK: + PIOS_DEBUG_Assert(0); + } } -static void lfsm_init_rx (struct lfsm_context * context) -{ - PIOS_DEBUG_Assert(context->rx); +static void lfsm_init_rx(struct lfsm_context * context) { + PIOS_DEBUG_Assert(context->rx); - switch (context->user_payload_type) { - case OPAHRS_MSG_TYPE_USER_V0: - opahrs_msg_v0_init_rx ((struct opahrs_msg_v0 *)context->rx); - break; - case OPAHRS_MSG_TYPE_USER_V1: - opahrs_msg_v1_init_rx ((struct opahrs_msg_v1 *)context->rx); - break; - case OPAHRS_MSG_TYPE_LINK: - PIOS_DEBUG_Assert(0); - } + switch (context->user_payload_type) { + case OPAHRS_MSG_TYPE_USER_V0: + opahrs_msg_v0_init_rx((struct opahrs_msg_v0 *) context->rx); + break; + case OPAHRS_MSG_TYPE_USER_V1: + opahrs_msg_v1_init_rx((struct opahrs_msg_v1 *) context->rx); + break; + case OPAHRS_MSG_TYPE_LINK: + PIOS_DEBUG_Assert(0); + } } /* @@ -433,120 +412,107 @@ static void lfsm_init_rx (struct lfsm_context * context) * */ -void lfsm_inject_event(enum lfsm_event event) -{ - PIOS_IRQ_Disable(); +void lfsm_inject_event(enum lfsm_event event) { + PIOS_IRQ_Disable(); - /* - * Move to the next state - * - * This is done prior to calling the new state's entry function to - * guarantee that the entry function never depends on the previous - * state. This way, it cannot ever know what the previous state was. - */ - context.curr_state = lfsm_transitions[context.curr_state].next_state[event]; + /* + * Move to the next state + * + * This is done prior to calling the new state's entry function to + * guarantee that the entry function never depends on the previous + * state. This way, it cannot ever know what the previous state was. + */ + context.curr_state = lfsm_transitions[context.curr_state].next_state[event]; - /* Call the entry function (if any) for the next state. */ - if (lfsm_transitions[context.curr_state].entry_fn) { - lfsm_transitions[context.curr_state].entry_fn(&context); - } - PIOS_IRQ_Enable(); + /* Call the entry function (if any) for the next state. */ + if (lfsm_transitions[context.curr_state].entry_fn) { + lfsm_transitions[context.curr_state].entry_fn(&context); + } + PIOS_IRQ_Enable(); } -void lfsm_init(void) -{ - context.curr_state = LFSM_STATE_STOPPED; - go_stopped(&context); +void lfsm_init(void) { + context.curr_state = LFSM_STATE_STOPPED; + go_stopped(&context); } -void lfsm_set_link_proto_v0 (struct opahrs_msg_v0 * link_tx, struct opahrs_msg_v0 * link_rx) -{ - PIOS_DEBUG_Assert(link_tx); +void lfsm_set_link_proto_v0(struct opahrs_msg_v0 * link_tx, + struct opahrs_msg_v0 * link_rx) { + PIOS_DEBUG_Assert(link_tx); - context.link_tx = (uint8_t *)link_tx; - context.link_rx = (uint8_t *)link_rx; - context.user_payload_type = OPAHRS_MSG_TYPE_USER_V0; - context.user_payload_len = sizeof(*link_tx); + context.link_tx = (uint8_t *) link_tx; + context.link_rx = (uint8_t *) link_rx; + context.user_payload_type = OPAHRS_MSG_TYPE_USER_V0; + context.user_payload_len = sizeof(*link_tx); - lfsm_update_link_tx_v0(link_tx, context.link_state, context.errors); + lfsm_update_link_tx_v0(link_tx, context.link_state, context.errors); - lfsm_inject_event(LFSM_EVENT_INIT_LINK); + lfsm_inject_event(LFSM_EVENT_INIT_LINK); } -void lfsm_set_link_proto_v1 (struct opahrs_msg_v1 * link_tx, struct opahrs_msg_v1 * link_rx) -{ - PIOS_DEBUG_Assert(link_tx); +void lfsm_set_link_proto_v1(struct opahrs_msg_v1 * link_tx, + struct opahrs_msg_v1 * link_rx) { + PIOS_DEBUG_Assert(link_tx); - context.link_tx = (uint8_t *)link_tx; - context.link_rx = (uint8_t *)link_rx; - context.user_payload_type = OPAHRS_MSG_TYPE_USER_V1; - context.user_payload_len = sizeof(*link_tx); + context.link_tx = (uint8_t *) link_tx; + context.link_rx = (uint8_t *) link_rx; + context.user_payload_type = OPAHRS_MSG_TYPE_USER_V1; + context.user_payload_len = sizeof(*link_tx); - lfsm_update_link_tx_v1(link_tx, context.link_state, context.errors); + lfsm_update_link_tx_v1(link_tx, context.link_state, context.errors); - lfsm_inject_event(LFSM_EVENT_INIT_LINK); + lfsm_inject_event(LFSM_EVENT_INIT_LINK); } -void lfsm_user_set_tx_v0 (struct opahrs_msg_v0 * user_tx) -{ - PIOS_DEBUG_Assert(user_tx); +void lfsm_user_set_tx_v0(struct opahrs_msg_v0 * user_tx) { + PIOS_DEBUG_Assert(user_tx); - PIOS_DEBUG_Assert(context.user_payload_type == OPAHRS_MSG_TYPE_USER_V0); - context.user_tx = (uint8_t *)user_tx; + PIOS_DEBUG_Assert(context.user_payload_type == OPAHRS_MSG_TYPE_USER_V0); + context.user_tx = (uint8_t *) user_tx; - lfsm_inject_event(LFSM_EVENT_USER_SET_TX); + lfsm_inject_event(LFSM_EVENT_USER_SET_TX); } -void lfsm_user_set_rx_v0 (struct opahrs_msg_v0 * user_rx) -{ - PIOS_DEBUG_Assert(user_rx); - PIOS_DEBUG_Assert(context.user_payload_type == OPAHRS_MSG_TYPE_USER_V0); +void lfsm_user_set_rx_v0(struct opahrs_msg_v0 * user_rx) { + PIOS_DEBUG_Assert(user_rx); PIOS_DEBUG_Assert(context.user_payload_type == OPAHRS_MSG_TYPE_USER_V0); - context.user_rx = (uint8_t *)user_rx; + context.user_rx = (uint8_t *) user_rx; - lfsm_inject_event(LFSM_EVENT_USER_SET_RX); + lfsm_inject_event(LFSM_EVENT_USER_SET_RX); } -void lfsm_user_set_tx_v1 (struct opahrs_msg_v1 * user_tx) -{ - PIOS_DEBUG_Assert(user_tx); - PIOS_DEBUG_Assert(context.user_payload_type == OPAHRS_MSG_TYPE_USER_V1); +void lfsm_user_set_tx_v1(struct opahrs_msg_v1 * user_tx) { + PIOS_DEBUG_Assert(user_tx); PIOS_DEBUG_Assert(context.user_payload_type == OPAHRS_MSG_TYPE_USER_V1); - context.user_tx = (uint8_t *)user_tx; + context.user_tx = (uint8_t *) user_tx; - lfsm_inject_event(LFSM_EVENT_USER_SET_TX); + lfsm_inject_event(LFSM_EVENT_USER_SET_TX); } -void lfsm_user_set_rx_v1 (struct opahrs_msg_v1 * user_rx) -{ - PIOS_DEBUG_Assert(user_rx); - PIOS_DEBUG_Assert(context.user_payload_type == OPAHRS_MSG_TYPE_USER_V1); +void lfsm_user_set_rx_v1(struct opahrs_msg_v1 * user_rx) { + PIOS_DEBUG_Assert(user_rx); PIOS_DEBUG_Assert(context.user_payload_type == OPAHRS_MSG_TYPE_USER_V1); - context.user_rx = (uint8_t *)user_rx; + context.user_rx = (uint8_t *) user_rx; - lfsm_inject_event(LFSM_EVENT_USER_SET_RX); + lfsm_inject_event(LFSM_EVENT_USER_SET_RX); } -void lfsm_user_done (void) -{ - lfsm_inject_event(LFSM_EVENT_USER_DONE); +void lfsm_user_done(void) { + lfsm_inject_event(LFSM_EVENT_USER_DONE); } -void lfsm_stop (void) -{ - lfsm_inject_event(LFSM_EVENT_STOP); +void lfsm_stop(void) { + lfsm_inject_event(LFSM_EVENT_STOP); } -void lfsm_get_link_stats (struct lfsm_link_stats * stats) -{ - PIOS_DEBUG_Assert(stats); +void lfsm_get_link_stats(struct lfsm_link_stats * stats) { + PIOS_DEBUG_Assert(stats); - *stats = context.stats; + *stats = context.stats; } -enum lfsm_state lfsm_get_state (void) -{ - return context.curr_state; +enum lfsm_state lfsm_get_state(void) { + return context.curr_state; } /* @@ -555,72 +521,71 @@ enum lfsm_state lfsm_get_state (void) * */ -void lfsm_irq_callback(uint8_t crc_ok, uint8_t crc_val) -{ - if (!crc_ok) { - context.stats.rx_badcrc++; - lfsm_inject_event (LFSM_EVENT_RX_UNKNOWN); - return; - } +void lfsm_irq_callback(uint8_t crc_ok, uint8_t crc_val) { + if (!crc_ok) { + context.stats.rx_badcrc++; + lfsm_inject_event(LFSM_EVENT_RX_UNKNOWN); + return; + } - if (!context.rx) { - /* No way to know what we just received, assume invalid */ - lfsm_inject_event (LFSM_EVENT_RX_UNKNOWN); - return; - } + if (!context.rx) { + /* No way to know what we just received, assume invalid */ + lfsm_inject_event(LFSM_EVENT_RX_UNKNOWN); + return; + } - /* Recover the head and tail pointers from the message */ - struct opahrs_msg_link_head * head=0; - struct opahrs_msg_link_tail * tail=0; + /* Recover the head and tail pointers from the message */ + struct opahrs_msg_link_head * head = 0; + struct opahrs_msg_link_tail * tail = 0; - switch (context.user_payload_type) { - case OPAHRS_MSG_TYPE_USER_V0: - head = &((struct opahrs_msg_v0 *)context.rx)->head; - tail = &((struct opahrs_msg_v0 *)context.rx)->tail; - break; - case OPAHRS_MSG_TYPE_USER_V1: - head = &((struct opahrs_msg_v1 *)context.rx)->head; - tail = &((struct opahrs_msg_v1 *)context.rx)->tail; - break; - case OPAHRS_MSG_TYPE_LINK: - /* Should never be rx'ing before the link protocol version is known */ - PIOS_DEBUG_Assert(0); - break; - } + switch (context.user_payload_type) { + case OPAHRS_MSG_TYPE_USER_V0: + head = &((struct opahrs_msg_v0 *) context.rx)->head; + tail = &((struct opahrs_msg_v0 *) context.rx)->tail; + break; + case OPAHRS_MSG_TYPE_USER_V1: + head = &((struct opahrs_msg_v1 *) context.rx)->head; + tail = &((struct opahrs_msg_v1 *) context.rx)->tail; + break; + case OPAHRS_MSG_TYPE_LINK: + /* Should never be rx'ing before the link protocol version is known */ + PIOS_DEBUG_Assert(0); + break; + } - /* Check for bad magic */ - if ((head->magic != OPAHRS_MSG_MAGIC_HEAD) || - (tail->magic != OPAHRS_MSG_MAGIC_TAIL)) { - if (head->magic != OPAHRS_MSG_MAGIC_HEAD) { - context.stats.rx_badmagic_head++; - } - if (tail->magic != OPAHRS_MSG_MAGIC_TAIL) { - context.stats.rx_badmagic_tail++; - } - lfsm_inject_event (LFSM_EVENT_RX_UNKNOWN); - return; - } + /* Check for bad magic */ + if ((head->magic != OPAHRS_MSG_MAGIC_HEAD) || (tail->magic + != OPAHRS_MSG_MAGIC_TAIL)) { + if (head->magic != OPAHRS_MSG_MAGIC_HEAD) { + context.stats.rx_badmagic_head++; + } + if (tail->magic != OPAHRS_MSG_MAGIC_TAIL) { + context.stats.rx_badmagic_tail++; + } + lfsm_inject_event(LFSM_EVENT_RX_UNKNOWN); + return; + } - /* Good magic, find out what type of payload we've got */ - switch (head->type) { - case OPAHRS_MSG_TYPE_LINK: - context.stats.rx_link++; - lfsm_inject_event (LFSM_EVENT_RX_LINK); - break; - case OPAHRS_MSG_TYPE_USER_V0: - case OPAHRS_MSG_TYPE_USER_V1: - if (head->type == context.user_payload_type) { - context.stats.rx_user++; - lfsm_inject_event (LFSM_EVENT_RX_USER); - } else { - /* Mismatched user payload type */ - context.stats.rx_badver++; - lfsm_inject_event (LFSM_EVENT_RX_UNKNOWN); - } - break; - default: - /* Unidentifiable payload type */ - context.stats.rx_badtype++; - lfsm_inject_event (LFSM_EVENT_RX_UNKNOWN); - } + /* Good magic, find out what type of payload we've got */ + switch (head->type) { + case OPAHRS_MSG_TYPE_LINK: + context.stats.rx_link++; + lfsm_inject_event(LFSM_EVENT_RX_LINK); + break; + case OPAHRS_MSG_TYPE_USER_V0: + case OPAHRS_MSG_TYPE_USER_V1: + if (head->type == context.user_payload_type) { + context.stats.rx_user++; + lfsm_inject_event(LFSM_EVENT_RX_USER); + } else { + /* Mismatched user payload type */ + context.stats.rx_badver++; + lfsm_inject_event(LFSM_EVENT_RX_UNKNOWN); + } + break; + default: + /* Unidentifiable payload type */ + context.stats.rx_badtype++; + lfsm_inject_event(LFSM_EVENT_RX_UNKNOWN); + } } diff --git a/flight/Bootloaders/INS/inc/ahrs_bl.h b/flight/Bootloaders/INS/inc/ahrs_bl.h new file mode 100644 index 000000000..2d96385ab --- /dev/null +++ b/flight/Bootloaders/INS/inc/ahrs_bl.h @@ -0,0 +1,52 @@ +/** + ****************************************************************************** + * + * @file ahrs_bl.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Main AHRS_BL header. + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program 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 3 of the License, or + * (at your option) any later version. + * + * This program 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 this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#ifndef AHRS_BL_H +#define AHRS_BL_H + +/* PIOS Includes */ +#include + +/** Start programming + returns: true if FLASH erased and ready to program + */ +bool +StartProgramming(void); + +/** Write a block to FLASH + buffer contains the data to be written + returns: true if FLASH programmed correctly + */ +bool +WriteData(uint32_t offset, uint8_t *buffer, uint32_t size); + +/** Read a block from FLASH + returns: true if FLASH read correctly. + Buffer is set to the read data + */ +bool +ReadData(uint32_t offset, uint8_t *buffer, uint32_t size); + +#endif /* AHRS_BL_H */ diff --git a/flight/Bootloaders/INS/inc/ahrs_spi_program.h b/flight/Bootloaders/INS/inc/ahrs_spi_program.h index dcb285416..024d1b061 100644 --- a/flight/Bootloaders/INS/inc/ahrs_spi_program.h +++ b/flight/Bootloaders/INS/inc/ahrs_spi_program.h @@ -2,58 +2,64 @@ #define AHRS_SPI_PROGRAM_H /* Special packets to enter programming mode. -Note: these must both be SPI_PROGRAM_REQUEST_LENGTH long. -Pad with spaces if needed. -*/ + Note: these must both be SPI_PROGRAM_REQUEST_LENGTH long. + Pad with spaces if needed. + */ #define SPI_PROGRAM_REQUEST "AHRS START PROGRAMMING " #define SPI_PROGRAM_ACK "AHRS PROGRAMMING STARTED" #define SPI_PROGRAM_REQUEST_LENGTH 24 /**Proposed programming protocol: -In the master: -1) Send a AhrsProgramPacket containing the relevant data. -Note crc is a CRC32 as the CRC8 used in hardware can be fooled. -2) Keep sending PROGRAM_NULL packets and wait for an ack. - Time out if we waited too long. -3) Compare ack packet with transmitted packet. The data - should be the bitwise inverse of the data transmitted. - packetId should correspond to the transmitted packet. -4) repeat for next packet until finished -5) Repeat using verify packets - Returned data should be exactly as read from memory + In the master: + 1) Send a AhrsProgramPacket containing the relevant data. + Note crc is a CRC32 as the CRC8 used in hardware can be fooled. + 2) Keep sending PROGRAM_NULL packets and wait for an ack. + Time out if we waited too long. + 3) Compare ack packet with transmitted packet. The data + should be the bitwise inverse of the data transmitted. + packetId should correspond to the transmitted packet. + 4) repeat for next packet until finished + 5) Repeat using verify packets + Returned data should be exactly as read from memory -In the slave: -1) Wait for an AhrsProgramPacket -2) Check CRC then write to memory -3) Bitwise invert data and add it to the return packet -4) Copy packetId from received packet -5) Transmit packet. -6) repeat until we receive a read packet -7) read memory as requested until we receive a reboot packet -Reboot packets had better have some sort of magic number in the data, -just to be absolutely sure + In the slave: + 1) Wait for an AhrsProgramPacket + 2) Check CRC then write to memory + 3) Bitwise invert data and add it to the return packet + 4) Copy packetId from received packet + 5) Transmit packet. + 6) repeat until we receive a read packet + 7) read memory as requested until we receive a reboot packet + Reboot packets had better have some sort of magic number in the data, + just to be absolutely sure -*/ + */ -typedef enum { PROGRAM_NULL, PROGRAM_WRITE, PROGRAM_READ, PROGRAM_ACK, PROGRAM_REBOOT, PROGRAM_ERR} ProgramType; +typedef enum { + PROGRAM_NULL, + PROGRAM_WRITE, + PROGRAM_READ, + PROGRAM_ACK, + PROGRAM_REBOOT, + PROGRAM_ERR +} ProgramType; #define SPI_MAX_PROGRAM_DATA_SIZE (14 * 4) //USB comms uses 14x 32 bit words - #define REBOOT_CONFIRMATION "AHRS REBOOT" #define REBOOT_CONFIRMATION_LENGTH 11 /** Proposed program packet defintion -*/ + */ typedef struct { ProgramType type; - uint32_t packetId; //Transmission packet ID - uint32_t address; //base address to place data - uint32_t size; //Size of data (0 to SPI_MAX_PROGRAM_DATA_SIZE) + uint32_t packetId; //Transmission packet ID + uint32_t address; //base address to place data + uint32_t size; //Size of data (0 to SPI_MAX_PROGRAM_DATA_SIZE) uint8_t data[SPI_MAX_PROGRAM_DATA_SIZE]; - uint32_t crc; //CRC32 - hardware CRC8 can be fooled + uint32_t crc; //CRC32 - hardware CRC8 can be fooled uint8_t dummy; //for some reason comms trashes the last byte sent } AhrsProgramPacket; -uint32_t GenerateCRC (AhrsProgramPacket * packet); +uint32_t GenerateCRC(AhrsProgramPacket * packet); #endif diff --git a/flight/Bootloaders/INS/inc/ahrs_spi_program_master.h b/flight/Bootloaders/INS/inc/ahrs_spi_program_master.h index 2e93b805c..87a3f9ac7 100644 --- a/flight/Bootloaders/INS/inc/ahrs_spi_program_master.h +++ b/flight/Bootloaders/INS/inc/ahrs_spi_program_master.h @@ -27,34 +27,37 @@ #ifndef AHRS_PROGRAM_MASTER_H #define AHRS_PROGRAM_MASTER_H -typedef enum {PROGRAM_ERR_OK, //OK +typedef enum { + PROGRAM_ERR_OK, //OK PROGRAM_ERR_LINK, //comms error - PROGRAM_ERR_FUNCTION, //function failed + PROGRAM_ERR_FUNCTION, +//function failed } PROGERR; - /** Connect to AHRS and request programming mode -* returns: false if failed. -*/ + * returns: false if failed. + */ bool AhrsProgramConnect(uint32_t spi_id); /** Write data to AHRS -* size must be between 1 and SPI_MAX_PROGRAM_DATA_SIZE -* returns: error status -*/ + * size must be between 1 and SPI_MAX_PROGRAM_DATA_SIZE + * returns: error status + */ -PROGERR AhrsProgramWrite(uint32_t spi_id, uint32_t address, void * data, uint32_t size); +PROGERR AhrsProgramWrite(uint32_t spi_id, uint32_t address, void * data, + uint32_t size); /** Read data from AHRS -* size must be between 1 and SPI_MAX_PROGRAM_DATA_SIZE -* returns: error status -*/ + * size must be between 1 and SPI_MAX_PROGRAM_DATA_SIZE + * returns: error status + */ -PROGERR AhrsProgramRead(uint32_t spi_id, uint32_t address, void * data, uint32_t size); +PROGERR AhrsProgramRead(uint32_t spi_id, uint32_t address, void * data, + uint32_t size); /** reboot AHRS -* returns: error status -*/ + * returns: error status + */ PROGERR AhrsProgramReboot(uint32_t spi_id); diff --git a/flight/Bootloaders/INS/inc/ahrs_spi_program_slave.h b/flight/Bootloaders/INS/inc/ahrs_spi_program_slave.h index d13b44334..683bfa0f6 100644 --- a/flight/Bootloaders/INS/inc/ahrs_spi_program_slave.h +++ b/flight/Bootloaders/INS/inc/ahrs_spi_program_slave.h @@ -24,12 +24,12 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ - #ifndef AHRS_SPI_PROGRAM_SLAVE_H +#ifndef AHRS_SPI_PROGRAM_SLAVE_H #define AHRS_SPI_PROGRAM_SLAVE_H /** Check if OpenPilot is trying to program AHRS -* If so, it will program the FLASH then return -* If not it just returns. -*/ + * If so, it will program the FLASH then return + * If not it just returns. + */ void AhrsProgramReceive(uint32_t spi_id); #endif //AHRS_PROGRAM_SLAVE_H diff --git a/flight/Bootloaders/INS/inc/bl_fsm.h b/flight/Bootloaders/INS/inc/bl_fsm.h index 7ab3369e7..7c443edb7 100644 --- a/flight/Bootloaders/INS/inc/bl_fsm.h +++ b/flight/Bootloaders/INS/inc/bl_fsm.h @@ -1,4 +1,4 @@ - /** +/** ****************************************************************************** * * @file ahrs_fsm.h @@ -23,53 +23,54 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ - #ifndef BL_FSM_H #define BL_FSM_H #include "pios_opahrs_proto.h" enum lfsm_state { - LFSM_STATE_FAULTED = 0, /* Must be zero so undefined transitions land here */ - LFSM_STATE_STOPPED, - LFSM_STATE_STOPPING, - LFSM_STATE_INACTIVE, - LFSM_STATE_USER_BUSY, - LFSM_STATE_USER_BUSY_RX_PENDING, - LFSM_STATE_USER_BUSY_TX_PENDING, - LFSM_STATE_USER_BUSY_RXTX_PENDING, - LFSM_STATE_USER_RX_PENDING, - LFSM_STATE_USER_TX_PENDING, - LFSM_STATE_USER_RXTX_PENDING, - LFSM_STATE_USER_RX_ACTIVE, - LFSM_STATE_USER_TX_ACTIVE, - LFSM_STATE_USER_RXTX_ACTIVE, + LFSM_STATE_FAULTED = 0, /* Must be zero so undefined transitions land here */ + LFSM_STATE_STOPPED, + LFSM_STATE_STOPPING, + LFSM_STATE_INACTIVE, + LFSM_STATE_USER_BUSY, + LFSM_STATE_USER_BUSY_RX_PENDING, + LFSM_STATE_USER_BUSY_TX_PENDING, + LFSM_STATE_USER_BUSY_RXTX_PENDING, + LFSM_STATE_USER_RX_PENDING, + LFSM_STATE_USER_TX_PENDING, + LFSM_STATE_USER_RXTX_PENDING, + LFSM_STATE_USER_RX_ACTIVE, + LFSM_STATE_USER_TX_ACTIVE, + LFSM_STATE_USER_RXTX_ACTIVE, - LFSM_STATE_NUM_STATES /* Must be last */ + LFSM_STATE_NUM_STATES +/* Must be last */ }; enum lfsm_event { - LFSM_EVENT_INIT_LINK, - LFSM_EVENT_STOP, - LFSM_EVENT_USER_SET_RX, - LFSM_EVENT_USER_SET_TX, - LFSM_EVENT_USER_DONE, - LFSM_EVENT_RX_LINK, - LFSM_EVENT_RX_USER, - LFSM_EVENT_RX_UNKNOWN, + LFSM_EVENT_INIT_LINK, + LFSM_EVENT_STOP, + LFSM_EVENT_USER_SET_RX, + LFSM_EVENT_USER_SET_TX, + LFSM_EVENT_USER_DONE, + LFSM_EVENT_RX_LINK, + LFSM_EVENT_RX_USER, + LFSM_EVENT_RX_UNKNOWN, - LFSM_EVENT_NUM_EVENTS /* Must be last */ + LFSM_EVENT_NUM_EVENTS +/* Must be last */ }; struct lfsm_link_stats { - uint32_t rx_badcrc; - uint32_t rx_badmagic_head; - uint32_t rx_badmagic_tail; - uint32_t rx_link; - uint32_t rx_user; - uint32_t tx_user; - uint32_t rx_badtype; - uint32_t rx_badver; + uint32_t rx_badcrc; + uint32_t rx_badmagic_head; + uint32_t rx_badmagic_tail; + uint32_t rx_link; + uint32_t rx_user; + uint32_t tx_user; + uint32_t rx_badtype; + uint32_t rx_badver; }; extern void lfsm_attach(uint32_t spi_id); @@ -78,17 +79,19 @@ extern void lfsm_inject_event(enum lfsm_event event); extern void lfsm_irq_callback(uint8_t crc_ok, uint8_t crc_val); -extern void lfsm_get_link_stats (struct lfsm_link_stats * stats); -extern enum lfsm_state lfsm_get_state (void); +extern void lfsm_get_link_stats(struct lfsm_link_stats * stats); +extern enum lfsm_state lfsm_get_state(void); -extern void lfsm_set_link_proto_v0 (struct opahrs_msg_v0 * link_tx, struct opahrs_msg_v0 * link_rx); -extern void lfsm_user_set_rx_v0 (struct opahrs_msg_v0 * user_rx); -extern void lfsm_user_set_tx_v0 (struct opahrs_msg_v0 * user_tx); +extern void lfsm_set_link_proto_v0(struct opahrs_msg_v0 * link_tx, + struct opahrs_msg_v0 * link_rx); +extern void lfsm_user_set_rx_v0(struct opahrs_msg_v0 * user_rx); +extern void lfsm_user_set_tx_v0(struct opahrs_msg_v0 * user_tx); -extern void lfsm_set_link_proto_v1 (struct opahrs_msg_v1 * link_tx, struct opahrs_msg_v1 * link_rx); -extern void lfsm_user_set_rx_v1 (struct opahrs_msg_v1 * user_rx); -extern void lfsm_user_set_tx_v1 (struct opahrs_msg_v1 * user_tx); +extern void lfsm_set_link_proto_v1(struct opahrs_msg_v1 * link_tx, + struct opahrs_msg_v1 * link_rx); +extern void lfsm_user_set_rx_v1(struct opahrs_msg_v1 * user_rx); +extern void lfsm_user_set_tx_v1(struct opahrs_msg_v1 * user_tx); -extern void lfsm_user_done (void); +extern void lfsm_user_done(void); #endif /* BL_FSM_H */ diff --git a/flight/Bootloaders/INS/inc/pios_config.h b/flight/Bootloaders/INS/inc/pios_config.h index 488d99b83..19aed3401 100644 --- a/flight/Bootloaders/INS/inc/pios_config.h +++ b/flight/Bootloaders/INS/inc/pios_config.h @@ -24,18 +24,20 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ - #ifndef PIOS_CONFIG_H #define PIOS_CONFIG_H +#define STM32F2XX +#define HSE_VALUE 8000000 + /* Enable/Disable PiOS Modules */ #define PIOS_INCLUDE_DELAY #define PIOS_INCLUDE_IRQ #define PIOS_INCLUDE_LED -#define PIOS_INCLUDE_SPI +//#define PIOS_INCLUDE_SPI #define PIOS_INCLUDE_SYS -#define PIOS_INCLUDE_BL_HELPER -#define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT +//#define PIOS_INCLUDE_BL_HELPER +//#define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT #define PIOS_INCLUDE_GPIO #endif /* PIOS_CONFIG_H */ diff --git a/flight/Bootloaders/INS/main.c b/flight/Bootloaders/INS/main.c index adcd1085b..3cab01ab2 100644 --- a/flight/Bootloaders/INS/main.c +++ b/flight/Bootloaders/INS/main.c @@ -32,10 +32,11 @@ */ /* OpenPilot Includes */ -#include "ins_bl.h" +#include "ahrs_bl.h" +#include #include "pios_opahrs_proto.h" #include "bl_fsm.h" /* lfsm_state */ -#include "stm32f10x_flash.h" +#include "stm32f2xx_flash.h" extern void PIOS_Board_Init(void); @@ -47,20 +48,20 @@ typedef void pFunction Jump_To_Application; uint32_t JumpAddress; /* Function Prototypes */ -void -process_spi_request(void); +//void +//process_spi_request(void); void jump_to_app(); -uint8_t jumpFW = FALSE; uint32_t Fw_crc; /** * @brief Bootloader Main function */ int main() { - - uint8_t GO_dfu = false; /* Brings up System using CMSIS functions, enables the LEDs. */ PIOS_SYS_Init(); +#if 0 + uint8_t GO_dfu = false; + /* Enable Prefetch Buffer */ FLASH_PrefetchBufferCmd(FLASH_PrefetchBuffer_Enable); @@ -100,14 +101,18 @@ int main() { while (1) { process_spi_request(); } +#endif + jump_to_app(); return 0; } +#if 0 static struct opahrs_msg_v0 link_tx_v0; static struct opahrs_msg_v0 link_rx_v0; static struct opahrs_msg_v0 user_tx_v0; static struct opahrs_msg_v0 user_rx_v0; void process_spi_request(void) { + const struct pios_board_info * bdinfo = &pios_board_info_blob; bool msg_to_process = FALSE; PIOS_IRQ_Disable(); @@ -166,15 +171,15 @@ void process_spi_request(void) { break; case OPAHRS_MSG_V0_REQ_MEM_MAP: opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_MEM_MAP); - user_tx_v0.payload.user.v.rsp.mem_map.density = HW_TYPE; + user_tx_v0.payload.user.v.rsp.mem_map.density = bdinfo->hw_type; user_tx_v0.payload.user.v.rsp.mem_map.rw_flags = (BOARD_READABLE - | (BOARD_WRITABLA << 1)); + | (BOARD_WRITABLE << 1)); user_tx_v0.payload.user.v.rsp.mem_map.size_of_code_memory - = SIZE_OF_CODE; + = bdinfo->fw_size; user_tx_v0.payload.user.v.rsp.mem_map.size_of_description - = SIZE_OF_DESCRIPTION; + = bdinfo->desc_size; user_tx_v0.payload.user.v.rsp.mem_map.start_of_user_code - = START_OF_USER_CODE; + = bdinfo->fw_base; lfsm_user_set_tx_v0(&user_tx_v0); break; case OPAHRS_MSG_V0_REQ_SERIAL: @@ -192,7 +197,7 @@ void process_spi_request(void) { PIOS_LED_On(LED1); opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_FWUP_STATUS); if (!(user_rx_v0.payload.user.v.req.fwup_data.adress - < START_OF_USER_CODE)) { + < bdinfo->fw_base)) { for (uint8_t x = 0; x < user_rx_v0.payload.user.v.req.fwup_data.size; ++x) { if (FLASH_ProgramWord( @@ -249,14 +254,12 @@ void process_spi_request(void) { lfsm_user_done(); return; } +#endif void jump_to_app() { - //while(TRUE) - //{ - // PIOS_LED_Toggle(LED1); - // PIOS_DELAY_WaitmS(1000); - //} + const struct pios_board_info * bdinfo = &pios_board_info_blob; + PIOS_LED_On(LED1); - if (((*(__IO uint32_t*) START_OF_USER_CODE) & 0x2FFE0000) == 0x20000000) { /* Jump to user application */ + if (((*(__IO uint32_t*) bdinfo->fw_base) & 0x2FFE0000) == 0x20000000) { /* Jump to user application */ FLASH_Lock(); RCC_APB2PeriphResetCmd(0xffffffff, ENABLE); RCC_APB1PeriphResetCmd(0xffffffff, ENABLE); @@ -265,10 +268,10 @@ void jump_to_app() { //_SetCNTR(0); // clear interrupt mask //_SetISTR(0); // clear all requests - JumpAddress = *(__IO uint32_t*) (START_OF_USER_CODE + 4); + JumpAddress = *(__IO uint32_t*) (bdinfo->fw_base + 4); Jump_To_Application = (pFunction) JumpAddress; /* Initialize user application's Stack Pointer */ - __set_MSP(*(__IO uint32_t*) START_OF_USER_CODE); + __set_MSP(*(__IO uint32_t*) bdinfo->fw_base); Jump_To_Application(); } else { boot_status = jump_failed; diff --git a/flight/Bootloaders/INS/pios_board.c b/flight/Bootloaders/INS/pios_board.c index 0b1cd6fd0..9d0b40647 100644 --- a/flight/Bootloaders/INS/pios_board.c +++ b/flight/Bootloaders/INS/pios_board.c @@ -29,7 +29,6 @@ #include - /* OP Interface * * NOTE: Leave this declared as const data so that it ends up in the @@ -38,99 +37,93 @@ void PIOS_SPI_op_irq_handler(void); void DMA1_Channel5_IRQHandler() __attribute__ ((alias ("PIOS_SPI_op_irq_handler"))); void DMA1_Channel4_IRQHandler() __attribute__ ((alias ("PIOS_SPI_op_irq_handler"))); -static const struct pios_spi_cfg pios_spi_op_cfg = { - .regs = SPI2, - .init = { - .SPI_Mode = SPI_Mode_Slave, - .SPI_Direction = SPI_Direction_2Lines_FullDuplex, - .SPI_DataSize = SPI_DataSize_8b, - .SPI_NSS = SPI_NSS_Hard, - .SPI_FirstBit = SPI_FirstBit_MSB, - .SPI_CRCPolynomial = 7, - .SPI_CPOL = SPI_CPOL_High, - .SPI_CPHA = SPI_CPHA_2Edge, - }, - .use_crc = TRUE, - .dma = { - .ahb_clk = RCC_AHBPeriph_DMA1, - - .irq = { - .handler = PIOS_SPI_op_irq_handler, - .flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4), - .init = { - .NVIC_IRQChannel = DMA1_Channel4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, +static const struct pios_spi_cfg + pios_spi_op_cfg = { + .regs = SPI2, + .init = { + .SPI_Mode = SPI_Mode_Slave, + .SPI_Direction = SPI_Direction_2Lines_FullDuplex, + .SPI_DataSize = SPI_DataSize_8b, + .SPI_NSS = SPI_NSS_Hard, + .SPI_FirstBit = SPI_FirstBit_MSB, + .SPI_CRCPolynomial = 7, + .SPI_CPOL = SPI_CPOL_High, + .SPI_CPHA = SPI_CPHA_2Edge, + }, + .use_crc = TRUE, + .dma = { + .ahb_clk = RCC_AHBPeriph_DMA1, - .rx = { - .channel = DMA1_Channel4, - .init = { - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), - .DMA_DIR = DMA_DIR_PeripheralSRC, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_Medium, - .DMA_M2M = DMA_M2M_Disable, - }, - }, - .tx = { - .channel = DMA1_Channel5, - .init = { - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), - .DMA_DIR = DMA_DIR_PeripheralDST, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_Medium, - .DMA_M2M = DMA_M2M_Disable, - }, - }, - }, - .ssel = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_12, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, - .sclk = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_13, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, - .miso = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_14, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, - }, - }, - .mosi = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_15, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, -}; + .irq = { + .flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4), + .init = { + .NVIC_IRQChannel = DMA1_Channel4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + + .rx = { + .channel = DMA1_Channel4, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), + .DMA_DIR = DMA_DIR_PeripheralSRC, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_Medium, + .DMA_M2M = DMA_M2M_Disable, + }, + }, + .tx = { + .channel = DMA1_Channel5, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), + .DMA_DIR = DMA_DIR_PeripheralDST, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_Medium, + .DMA_M2M = DMA_M2M_Disable, + }, + }, + }, .ssel = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_12, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_IN_FLOATING, + }, + }, .sclk = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_13, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_IN_FLOATING, + }, + }, .miso = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_14, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, + }, .mosi = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_IN_FLOATING, + }, + }, }; uint32_t pios_spi_op_id; -void PIOS_SPI_op_irq_handler(void) -{ +void PIOS_SPI_op_irq_handler(void) { /* Call into the generic code to handle the IRQ for this specific device */ PIOS_SPI_IRQ_Handler(pios_spi_op_id); } @@ -139,12 +132,20 @@ void PIOS_SPI_op_irq_handler(void) #include "bl_fsm.h" /* lfsm_* */ -void PIOS_Board_Init() -{ +static bool board_init_complete = false; +void PIOS_Board_Init() { + if (board_init_complete) { + return; + } + +#if defined(PIOS_INCLUDE_SPI) /* Set up the SPI interface to the OP board */ if (PIOS_SPI_Init(&pios_spi_op_id, &pios_spi_op_cfg)) { PIOS_DEBUG_Assert(0); } lfsm_attach(pios_spi_op_id); lfsm_init(); +#endif + + board_init_complete = true; } diff --git a/flight/PiOS/Boards/STM3210E_INS.h b/flight/PiOS/Boards/STM3210E_INS.h index 4678fe2d9..e1b68c97a 100644 --- a/flight/PiOS/Boards/STM3210E_INS.h +++ b/flight/PiOS/Boards/STM3210E_INS.h @@ -132,17 +132,12 @@ extern uint32_t pios_i2c_gyro_adapter_id; // // See also pios_board.c //------------------------- -#define PIOS_COM_MAX_DEVS 2 - -extern uint32_t pios_com_gps_id; -#define PIOS_COM_GPS (pios_com_gps_id) - -#ifdef PIOS_ENABLE_AUX_UART -extern uint32_t pios_com_aux_id; -#endif -#define PIOS_COM_AUX (pios_com_aux_id) -#define PIOS_COM_DEBUG PIOS_COM_AUX - +#define PIOS_COM_MAX_DEVS 2 + +extern uint32_t pios_com_aux_id; +#define PIOS_COM_AUX (pios_com_aux_id) +#define PIOS_COM_DEBUG PIOS_COM_AUX + //------------------------- // System Settings //------------------------- diff --git a/make/boards/ins/board-info.mk b/make/boards/ins/board-info.mk index 8a57661bf..b05ae5398 100644 --- a/make/boards/ins/board-info.mk +++ b/make/boards/ins/board-info.mk @@ -4,10 +4,10 @@ BOOTLOADER_VERSION := 0x00 HW_TYPE := 0x00 MCU := cortex-m3 -CHIP := STM32F103RET +CHIP := STM32F205RGT BOARD := STM3210E_INS MODEL := HD -MODEL_SUFFIX := _OP +MODEL_SUFFIX := # Note: These must match the values in link_$(BOARD)_memory.ld BL_BANK_BASE := 0x08000000 # Start of bootloader flash