diff --git a/.gitignore b/.gitignore
index 4a11d007..5acf36fb 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,5 +1,6 @@
 .tags*
 *.o
+*.d
 cscope*
 *.bin
 *.elf
diff --git a/.travis.yml b/.travis.yml
index dc1239a0..bea8a8b8 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -9,6 +9,5 @@ before_install:
   - sudo apt-get install gcc-arm-none-eabi
 
 script:
-  - cd program/
   - make 
   - arm-none-eabi-size firmware.elf
diff --git a/Makefile b/Makefile
new file mode 100644
index 00000000..8ad9e712
--- /dev/null
+++ b/Makefile
@@ -0,0 +1,73 @@
+export WORKSPACE_DIR=./program
+
+#Primary firmware target
+export FIRMWARE=./firmware
+
+#
+#include .mk
+include $(WORKSPACE_DIR)/makefiles/toolchain.mk
+include $(WORKSPACE_DIR)/makefiles/workspace.mk
+
+#
+#object file dir
+include $(WORKSPACE_DIR)/board/vertigo-v2/board_config.mk
+
+OBJS = $(sort $(patsubst %c, %o, $(SRCS) ))
+
+
+
+#
+#make target
+all: $(FIRMWARE).bin $(FIRMWARE).elf
+
+include $(WORKSPACE_DIR)/makefiles/rules.mk
+
+clean:
+	rm -rf $(STARTUP_OBJ)
+	rm -rf $(FIRMWARE).elf
+	rm -rf $(FIRMWARE).bin
+	rm -f $(OBJS)
+	rm -f $(DEPS)
+#
+#upload firmware through st-flash
+flash:
+	st-flash write $(FIRMWARE).bin 0x8000000
+
+#
+#create gdb server through openocd
+openocd: flash
+	openocd -f ../debug/openocd.cfg
+
+#
+#execute cgdb 
+cgdb:
+	cgdb -d $(GDB) -x ./st_util_init.gdb
+
+#
+#execute gdbtui
+gdbtui:
+	$(GDB) -tui -x ./st_util_init.gdb
+
+#
+#upload firmware through black magic probe
+flash_bmp:
+	$(GDB) firmware.elf -x ./gdb_black_magic.gdb
+#
+#execute and connect to black magic gdb server, no needs to open a
+#local sever in PC
+cgdb_bmp:
+	cgdb -d $(GDB) firmware.elf -x ./bmp_gdbinit.gdb
+flash_openocd:
+	openocd -f interface/stlink-v2.cfg \
+	-f target/stm32f4x_stlink.cfg \
+	-c "init" \
+	-c "reset init" \
+	-c "halt" \
+	-c "flash write_image erase $(PROJECT).elf" \
+	-c "verify_image $(FIRMWARE).elf" \
+	-c "reset run" -c shutdown
+#automatically formate
+astyle: 
+	astyle -r --exclude=lib  *.c *.h
+
+.PHONY:all clean flash openocd gdbauto gdbtui cgdb astyle
diff --git a/program/Makefile b/program/Makefile
deleted file mode 100644
index 353003b3..00000000
--- a/program/Makefile
+++ /dev/null
@@ -1,223 +0,0 @@
-#Output files
-PROJECT=firmware
-EXECUTABLE=$(PROJECT).elf
-BIN_IMAGE=$(PROJECT).bin
-TEST_EXE=run_test
-#============================================================================#
-HOST_CC=gcc
-#Cross Compiler
-CC=arm-none-eabi-gcc
-OBJCOPY=arm-none-eabi-objcopy
-GDB=arm-none-eabi-gdb
-LD=arm-none-eabi-gcc
-CMSIS=./lib/CMSIS
-ST=./lib/STM32F4xx_StdPeriph_Driver
-EXTERNAL_DEVICE=./ext_device
-MCU_PERIPH=./mcu_periph
-ESTIMATOR=./estimator
-ACTUATORS=./actuators
-COMMON=./common
-RADIO_CONTROLLER=./radio_controller
-CONTROLLER =./controller
-MAVLINK=./mavlink
-FREERTOS=./lib/FreeRTOS
-MAVLINK_LIB=./lib/mavlink
-TEST=./test
-#============================================================================#
-BOARD_CONFIG=./vertigo_v2_config.h
-DEBUG_CONFIG=./debug_config.h
-CFLAGS_INCLUDE=-I./ \
-	-I$(TEST) \
-	-I$(ACTUATORS) \
-	-I$(CONTROLLER) \
-	-I$(MCU_PERIPH) \
-	-I$(EXTERNAL_DEVICE) \
-	-I$(ESTIMATOR)\
-	-I$(COMMON)\
-	-I$(RADIO_CONTROLLER) \
-	-I$(FREERTOS)/Source/include \
-	-I$(FREERTOS)/Source/portable/GCC/ARM_CM4F \
-	-I$(CMSIS) \
-	-I$(ST)/inc \
-	-I$(MAVLINK) \
-	-I$(MAVLINK_LIB) \
-	-I$(MAVLINK_LIB)/common
-CFLAGS_DEFINE= \
-        -D USE_STDPERIPH_DRIVER \
-        -D __FPU_PRESENT=1 \
-        -D ARM_MATH_CM4 \
-        -D __FPU_USED=1 \
-        -include $(BOARD_CONFIG) \
-        -include $(DEBUG_CONFIG) \
-		-U printf -D printf=printf_base
-
-        #__CC_ARM
-CFLAGS_OPTIMIZE= \
-	-O2
-CFLAGS_NEW_LIB_NANO= \
-	--specs=nano.specs --specs=nosys.specs  -u _printf_float
-CFLAGS_WARNING= \
-	-Wall \
-	-Wextra \
-	-Wdouble-promotion \
-	-Wshadow \
-	-Werror=array-bounds \
-	-Wfatal-errors \
-	-Wmissing-prototypes \
-	-Wbad-function-cast  \
-	-Wstrict-prototypes \
-	-Wmissing-parameter-type
-
-ARCH_FLAGS=-mlittle-endian -mthumb -mcpu=cortex-m4 \
-	-mfpu=fpv4-sp-d16 -mfloat-abi=hard
-
-CFLAGS=-g $(ARCH_FLAGS)\
-	${CFLAGS_INCLUDE} ${CFLAGS_DEFINE} \
-	${CFLAGS_WARNING}
-
-
-LDFLAGS +=$(CFLAGS_NEW_LIB_NANO) --static -Wl,--gc-sections
-
-LDFLAGS +=-T STM32F427VI_FLASH.ld
-LDLIBS +=-Wl,--start-group -lm -Wl,--end-group
-
-#============================================================================#
-
-STARTUP=./startup_stm32f427xx.o
-EXTERNAL_DEVICE_SRC = $(EXTERNAL_DEVICE)/AT24C04C.o \
-			$(EXTERNAL_DEVICE)/mpu9250.o \
-			$(EXTERNAL_DEVICE)/hmc5983.o \
-			$(EXTERNAL_DEVICE)/lea6h_ubx.o \
-			$(EXTERNAL_DEVICE)/ADS1246_MPX6115A.o
-
-MCU_PERIPH_SRC =  \
-	$(MCU_PERIPH)/i2c.o \
-	$(MCU_PERIPH)/spi.o \
-	$(MCU_PERIPH)/gpio.o \
-	$(MCU_PERIPH)/tim.o \
-	$(MCU_PERIPH)/led.o \
-	$(MCU_PERIPH)/usart.o \
-	$(MCU_PERIPH)/input_capture.o \
-	$(MCU_PERIPH)/system_time.o
-ESTIMATOR_SRC =	$(ESTIMATOR)/attitude_estimator.o \
-	$(ESTIMATOR)/vertical_estimator.o \
-	$(ESTIMATOR)/estimator.o 
-
-ACTUATORS_SRC = $(ACTUATORS)/pwm.o
-RADIO_CONTROLLER_SRC = $(RADIO_CONTROLLER)/radio_control.o \
-	$(RADIO_CONTROLLER)/pwm_decoder.o
-COMMON_SRC =$(COMMON)/test_common.o \
-	$(COMMON)/memory.o \
-	$(COMMON)/io.o \
-	$(COMMON)/std.o
-
-CONTROLLER_SRC = $(CONTROLLER)/attitude_stabilizer.o \
-				 $(CONTROLLER)/vertical_stabilizer.o \
-				 $(CONTROLLER)/navigation.o \
-				 $(CONTROLLER)/flight_controller.o \
-				 $(CONTROLLER)/controller.o
-
-FREERTOS_SRC=$(FREERTOS)/Source/croutine.o \
-	$(FREERTOS)/Source/list.o \
-	$(FREERTOS)/Source/queue.o \
-	$(FREERTOS)/Source/tasks.o \
-	$(FREERTOS)/Source/timers.o \
-	$(FREERTOS)/Source/portable/MemMang/heap_1.o \
-	$(FREERTOS)/Source/portable/GCC/ARM_CM4F/port.o
-
-MAVLINK_SRC=$(MAVLINK)/communication.o \
-	$(MAVLINK)/mission.o \
-	$(MAVLINK)/parameter.o \
-	$(MAVLINK)/command_parser.o \
-	$(MAVLINK)/global.o 
-
-OBJS=	./system_stm32f4xx.o \
-	$(CMSIS)/FastMathFunctions/arm_cos_f32.o \
-	$(CMSIS)/FastMathFunctions/arm_sin_f32.o \
-        $(ST)/src/misc.o \
-        $(ST)/src/stm32f4xx_rcc.o \
-        $(ST)/src/stm32f4xx_dma.o \
-        $(ST)/src/stm32f4xx_flash.o \
-        $(ST)/src/stm32f4xx_gpio.o \
-        $(ST)/src/stm32f4xx_usart.o \
-        $(ST)/src/stm32f4xx_tim.o \
-        $(ST)/src/stm32f4xx_spi.o \
-        $(ST)/src/stm32f4xx_i2c.o \
-        $(ST)/src/stm32f4xx_sdio.o \
-        ./interrupt.o \
-        ./main.o \
-        $(STARTUP) \
-        $(EXTERNAL_DEVICE_SRC) $(MCU_PERIPH_SRC) \
-	$(ESTIMATOR_SRC) $(ACTUATORS_SRC) $(RADIO_CONTROLLER_SRC) \
-	$(COMMON_SRC) $(CONTROLLER_SRC) $(FREERTOS_SRC) \
-	$(MAVLINK_SRC)
-	
-
-
-#Make all
-all:$(BIN_IMAGE)
-
-$(BIN_IMAGE):$(EXECUTABLE)
-	@$(OBJCOPY) -O binary $^ $@
-	@echo 'OBJCOPY $(BIN_IMAGE)'
-
-$(EXECUTABLE): $(OBJS)
-	@$(LD) $(LDFLAGS) $(ARCH_FLAGS) $(OBJS) $(LDLIBS) -o $@ 
-	@echo 'LD $(EXECUTABLE)'
-
-%.o: %.c
-	@$(CC) $(CFLAGS) -c $< -o $@
-	@echo 'CC $<'
-
-%.o: %.s
-	@$(CC) $(CFLAGS) -c $< -o $@
-	@echo 'CC $<'
-
-PC_SIM:$(TEST_EXE)
-
-$(TEST_EXE):$(HOST_SRC)
-	$(HOST_CC) $(HOST_CFLAG) $^ -o $@
-#Make clean
-clean:
-	rm -rf $(STARTUP_OBJ)
-	rm -rf $(EXECUTABLE)
-	rm -rf $(BIN_IMAGE)
-	rm -f $(OBJS)
-
-#Make flash
-flash:
-	st-flash write $(BIN_IMAGE) 0x8000000
-
-#Make openocd
-openocd: flash
-	openocd -f ../debug/openocd.cfg
-
-#Make cgdb
-cgdb:
-	cgdb -d $(GDB) -x ./st_util_init.gdb
-
-#Make gdbtui
-gdbtui:
-	$(GDB) -tui -x ./st_util_init.gdb
-#Make gdbauto
-gdbauto: cgdb
-
-flash_bmp:
-	$(GDB) firmware.elf -x ./gdb_black_magic.gdb
-cgdb_bmp: 
-	cgdb -d $(GDB) firmware.elf -x ./bmp_gdbinit.gdb
-flash_openocd:
-	openocd -f interface/stlink-v2.cfg \
-	-f target/stm32f4x_stlink.cfg \
-	-c "init" \
-	-c "reset init" \
-	-c "halt" \
-	-c "flash write_image erase $(PROJECT).elf" \
-	-c "verify_image $(PROJECT).elf" \
-	-c "reset run" -c shutdown
-#automatically formate
-astyle: 
-	astyle -r --exclude=lib  *.c *.h
-#============================================================================#
-
-.PHONY:all clean flash openocd gdbauto gdbtui cgdb astyle
diff --git a/program/board/vertigo-v2/board_config.mk b/program/board/vertigo-v2/board_config.mk
index 9105a388..615152d9 100644
--- a/program/board/vertigo-v2/board_config.mk
+++ b/program/board/vertigo-v2/board_config.mk
@@ -1 +1,86 @@
 TARGET_BOARD='Vertigo v2.0'
+
+STARTUP_SRCS=$(WORKSPACE_DIR)/startup_stm32f427xx.c
+EXTERNAL_DEVICE_SRCS = $(EXTERNAL_DEVICE)/AT24C04C.c \
+			$(EXTERNAL_DEVICE)/mpu9250.c \
+			$(EXTERNAL_DEVICE)/hmc5983.c \
+			$(EXTERNAL_DEVICE)/lea6h_ubx.c \
+			$(EXTERNAL_DEVICE)/ADS1246_MPX6115A.c
+
+MCU_PERIPH_SRCS =  \
+	$(MCU_PERIPH)/i2c.c \
+	$(MCU_PERIPH)/spi.c \
+	$(MCU_PERIPH)/gpio.c \
+	$(MCU_PERIPH)/tim.c \
+	$(MCU_PERIPH)/led.c \
+	$(MCU_PERIPH)/usart.c \
+	$(MCU_PERIPH)/input_capture.c \
+	$(MCU_PERIPH)/system_time.c
+
+ACTUATORS_SRCS = $(ACTUATORS)/pwm.c
+
+RADIO_CONTROLLER_SRCS = $(RADIO_CONTROLLER)/radio_control.c \
+	$(RADIO_CONTROLLER)/pwm_decoder.c
+
+COMMON_SRCS =$(COMMON)/test_common.c \
+	$(COMMON)/memory.c \
+	$(COMMON)/io.c \
+	$(COMMON)/std.c
+
+CONTROLLER_SRCS = $(CONTROLLER)/attitude_stabilizer.c \
+		$(CONTROLLER)/vertical_stabilizer.c \
+		$(CONTROLLER)/navigation.c \
+		$(CONTROLLER)/flight_controller.c \
+		$(CONTROLLER)/controller.c
+
+FREERTOS_SRCS=$(FREERTOS)/Source/croutine.c \
+	$(FREERTOS)/Source/list.c \
+	$(FREERTOS)/Source/queue.c \
+	$(FREERTOS)/Source/tasks.c \
+	$(FREERTOS)/Source/timers.c \
+	$(FREERTOS)/Source/portable/MemMang/heap_1.c \
+	$(FREERTOS)/Source/portable/GCC/ARM_CM4F/port.c
+
+MAVLINK_SRCS=$(MAVLINK)/communication.c \
+	$(MAVLINK)/mission.c \
+	$(MAVLINK)/parameter.c \
+	$(MAVLINK)/command_parser.c \
+	$(MAVLINK)/global.c
+
+CMSIS_SRCS= \
+	$(CMSIS)/FastMathFunctions/arm_cos_f32.c \
+	$(CMSIS)/FastMathFunctions/arm_sin_f32.c
+
+STMF4_STD_DRIVER_SRCS= \
+	$(WORKSPACE_DIR)/system_stm32f4xx.c \
+        $(ST)/src/misc.c \
+        $(ST)/src/stm32f4xx_rcc.c \
+        $(ST)/src/stm32f4xx_dma.c \
+        $(ST)/src/stm32f4xx_flash.c \
+        $(ST)/src/stm32f4xx_gpio.c \
+        $(ST)/src/stm32f4xx_usart.c \
+        $(ST)/src/stm32f4xx_tim.c \
+        $(ST)/src/stm32f4xx_spi.c \
+        $(ST)/src/stm32f4xx_i2c.c \
+        $(ST)/src/stm32f4xx_sdio.c
+
+BASIC_SRCS= \
+        $(WORKSPACE_DIR)/interrupt.c \
+        $(WORKSPACE_DIR)/main.c \
+
+
+#
+#generate the source file list
+#
+SRCS += $(STARTUP_SRCS)
+SRCS += $(EXTERNAL_DEVICE_SRCS)
+SRCS += $(MCU_PERIPH_SRCS)
+SRCS += $(ACTUATORS_SRCS)
+SRCS += $(RADIO_CONTROLLER_SRCS)
+SRCS += $(COMMON_SRCS)
+SRCS += $(CONTROLLER_SRCS)
+SRCS += $(FREERTOS_SRCS)
+SRCS += $(MAVLINK_SRCS)
+SRCS += $(CMSIS_SRCS)
+SRCS += $(STMF4_STD_DRIVER_SRCS)
+SRCS += $(BASIC_SRCS)
diff --git a/program/estimator/Makefile b/program/estimator/Makefile
new file mode 100644
index 00000000..4be77d8e
--- /dev/null
+++ b/program/estimator/Makefile
@@ -0,0 +1,20 @@
+export WORKSPACE_DIR=..
+#
+#include makefiles
+include $(WORKSPACE_DIR)/makefiles/toolchain.mk
+include $(WORKSPACE_DIR)/makefiles/workspace.mk
+include $(WORKSPACE_DIR)/makefiles/rules.mk
+
+LIB_ESTIMATOR=libestimator.a
+ESTIMATOR_SRC = $(WORKSPACE_DIR)/estimator
+ESTIMATOR_OBJS=	$(ESTIMATOR_SRC)/attitude_estimator.o \
+	$(ESTIMATOR_SRC)/vertical_estimator.o \
+	$(ESTIMATOR_SRC)/estimator.o
+#make target
+all: $(LIB_ESTIMATOR)
+
+$(LIB_ESTIMATOR): $(ESTIMATOR_OBJS)
+	$(AR) -r $(LIB_ESTIMATOR) $(ESTIMATOR_OBJS)
+	$(RANLIB) $(LIB_ESTIMATOR)
+clean:
+	rm -rf $(LIB_ESTIMATOR)
\ No newline at end of file
diff --git a/program/estimator/libestimator.a b/program/estimator/libestimator.a
new file mode 100644
index 00000000..728ec4e2
Binary files /dev/null and b/program/estimator/libestimator.a differ
diff --git a/program/makefiles/rules.mk b/program/makefiles/rules.mk
new file mode 100644
index 00000000..3fd5005a
--- /dev/null
+++ b/program/makefiles/rules.mk
@@ -0,0 +1,22 @@
+#
+#makefile rules
+%.bin: %.elf
+	@$(OBJCOPY) -O binary $^ $@
+	@echo 'OBJCOPY $<'
+
+%.elf: $(OBJS)
+	@$(LD) $(LDFLAGS) $(ARCH_FLAGS) $(OBJS) $(LDLIBS) -o $@
+	@echo 'LD $^'
+
+DEPS = $(OBJS:.o=.d)
+-include $(DEPS)
+
+%.o: %.c
+	@$(CC) $(CFLAGS) -c $< -o $@
+	@echo 'CC $<'
+
+%.o: %.s
+	@$(CC) $(CFLAGS) -c $< -o $@
+	@echo 'CC $<'
+
+.PRECIOUS: $(OBJS) %.bin %.elf
\ No newline at end of file
diff --git a/program/makefiles/toolchain.mk b/program/makefiles/toolchain.mk
new file mode 100644
index 00000000..d96a4c24
--- /dev/null
+++ b/program/makefiles/toolchain.mk
@@ -0,0 +1,64 @@
+
+HOST_CC=gcc
+CC=arm-none-eabi-gcc
+OBJCOPY=arm-none-eabi-objcopy
+GDB=arm-none-eabi-gdb
+LD=arm-none-eabi-gcc
+AR =arm-none-eabi-ar
+RANLIB =arm-none-eabi-ranlib
+
+CFLAGS_OPTIMIZE= \
+	-O2
+
+CFLAGS_NEW_LIB_NANO= \
+	--specs=nano.specs --specs=nosys.specs  -u _printf_float
+
+CFLAGS_WARNING= \
+	-Wall \
+	-Wextra \
+	-Wdouble-promotion \
+	-Wshadow \
+	-Werror=array-bounds \
+	-Wfatal-errors \
+	-Wmissing-prototypes \
+	-Wbad-function-cast  \
+	-Wstrict-prototypes \
+	-Wmissing-parameter-type
+
+ARCH_FLAGS=-mlittle-endian -mthumb -mcpu=cortex-m4 \
+	-mfpu=fpv4-sp-d16 -mfloat-abi=hard
+
+LDFLAGS =$(CFLAGS_NEW_LIB_NANO) --static -Wl,--gc-sections \
+	-T $(WORKSPACE_DIR)/STM32F427VI_FLASH.ld
+
+LDLIBS = -Wl,--start-group -lm  -L$(ESTIMATOR) -lestimator -Wl,--end-group
+
+CFLAGS_INCLUDE=-I$(WORKSPACE_DIR)/ \
+	-I$(TEST) \
+	-I$(ACTUATORS) \
+	-I$(CONTROLLER) \
+	-I$(MCU_PERIPH) \
+	-I$(EXTERNAL_DEVICE) \
+	-I$(ESTIMATOR)\
+	-I$(COMMON)\
+	-I$(RADIO_CONTROLLER) \
+	-I$(FREERTOS)/Source/include \
+	-I$(FREERTOS)/Source/portable/GCC/ARM_CM4F \
+	-I$(CMSIS) \
+	-I$(ST)/inc \
+	-I$(MAVLINK) \
+	-I$(MAVLINK_LIB) \
+	-I$(MAVLINK_LIB)/common
+
+CFLAGS_DEFINE= \
+        -D USE_STDPERIPH_DRIVER \
+        -D __FPU_PRESENT=1 \
+        -D ARM_MATH_CM4 \
+        -D __FPU_USED=1 \
+        -include $(BOARD_CONFIG) \
+        -include $(DEBUG_CONFIG) \
+	-U printf -D printf=printf_base
+
+CFLAGS=-g $(ARCH_FLAGS)\
+	${CFLAGS_INCLUDE} ${CFLAGS_DEFINE} \
+	${CFLAGS_WARNING} -MMD -MP
\ No newline at end of file
diff --git a/program/makefiles/workspace.mk b/program/makefiles/workspace.mk
new file mode 100644
index 00000000..080617d9
--- /dev/null
+++ b/program/makefiles/workspace.mk
@@ -0,0 +1,20 @@
+#
+#export abs path
+export CMSIS=$(abspath $(WORKSPACE_DIR)/lib/CMSIS)
+export ST=$(abspath $(WORKSPACE_DIR)/lib/STM32F4xx_StdPeriph_Driver)
+export EXTERNAL_DEVICE=$(abspath $(WORKSPACE_DIR)/ext_device)
+export MCU_PERIPH=$(abspath $(WORKSPACE_DIR)/mcu_periph)
+export ESTIMATOR=$(abspath $(WORKSPACE_DIR)/estimator)
+export ACTUATORS=$(abspath $(WORKSPACE_DIR)/actuators)
+export COMMON=$(abspath $(WORKSPACE_DIR)/common)
+export RADIO_CONTROLLER=$(abspath $(WORKSPACE_DIR)/radio_controller)
+export CONTROLLER =$(abspath $(WORKSPACE_DIR)/controller)
+export MAVLINK=$(abspath $(WORKSPACE_DIR)/mavlink)
+export FREERTOS=$(abspath $(WORKSPACE_DIR)/lib/FreeRTOS)
+export MAVLINK_LIB=$(abspath $(WORKSPACE_DIR)/lib/mavlink)
+export TEST=$(abspath $(WORKSPACE_DIR)/test)
+
+#
+#inclue dir
+BOARD_CONFIG=$(WORKSPACE_DIR)/vertigo_v2_config.h
+DEBUG_CONFIG=$(WORKSPACE_DIR)/debug_config.h