diff --git a/.cproject b/.cproject
new file mode 100644
index 0000000..acc842d
--- /dev/null
+++ b/.cproject
@@ -0,0 +1,134 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ make
+
+ sketch
+ true
+ true
+ true
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/.project b/.project
new file mode 100644
index 0000000..ded277f
--- /dev/null
+++ b/.project
@@ -0,0 +1,82 @@
+
+
+ tricopter
+
+
+
+
+
+ org.eclipse.cdt.managedbuilder.core.genmakebuilder
+
+
+ ?name?
+
+
+
+ org.eclipse.cdt.make.core.append_environment
+ true
+
+
+ org.eclipse.cdt.make.core.autoBuildTarget
+ sketch
+
+
+ org.eclipse.cdt.make.core.buildArguments
+
+
+
+ org.eclipse.cdt.make.core.buildCommand
+ make
+
+
+ org.eclipse.cdt.make.core.buildLocation
+ ${workspace_loc:/tricopter}
+
+
+ org.eclipse.cdt.make.core.cleanBuildTarget
+ clean
+
+
+ org.eclipse.cdt.make.core.contents
+ org.eclipse.cdt.make.core.activeConfigSettings
+
+
+ org.eclipse.cdt.make.core.enableAutoBuild
+ true
+
+
+ org.eclipse.cdt.make.core.enableCleanBuild
+ true
+
+
+ org.eclipse.cdt.make.core.enableFullBuild
+ true
+
+
+ org.eclipse.cdt.make.core.fullBuildTarget
+ sketch
+
+
+ org.eclipse.cdt.make.core.stopOnError
+ true
+
+
+ org.eclipse.cdt.make.core.useDefaultBuildCmd
+ true
+
+
+
+
+ org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder
+ full,incremental,
+
+
+
+
+
+ org.eclipse.cdt.core.cnature
+ org.eclipse.cdt.core.ccnature
+ org.eclipse.cdt.managedbuilder.core.managedBuildNature
+ org.eclipse.cdt.managedbuilder.core.ScannerConfigNature
+
+
diff --git a/.settings/org.eclipse.cdt.codan.core.prefs b/.settings/org.eclipse.cdt.codan.core.prefs
new file mode 100644
index 0000000..491e29f
--- /dev/null
+++ b/.settings/org.eclipse.cdt.codan.core.prefs
@@ -0,0 +1,66 @@
+#Fri Dec 02 09:54:29 CST 2011
+eclipse.preferences.version=1
+org.eclipse.cdt.codan.checkers.errnoreturn=Warning
+org.eclipse.cdt.codan.checkers.errnoreturn.params={implicit\=>false}
+org.eclipse.cdt.codan.checkers.errreturnvalue=Error
+org.eclipse.cdt.codan.checkers.errreturnvalue.params={}
+org.eclipse.cdt.codan.checkers.noreturn=Error
+org.eclipse.cdt.codan.checkers.noreturn.params={implicit\=>false}
+org.eclipse.cdt.codan.internal.checkers.AbstractClassCreation=Error
+org.eclipse.cdt.codan.internal.checkers.AbstractClassCreation.params={launchModes\=>{RUN_ON_FULL_BUILD\=>false,RUN_ON_INC_BUILD\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}}
+org.eclipse.cdt.codan.internal.checkers.AmbiguousProblem=Error
+org.eclipse.cdt.codan.internal.checkers.AmbiguousProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>false,RUN_ON_INC_BUILD\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}}
+org.eclipse.cdt.codan.internal.checkers.AssignmentInConditionProblem=Warning
+org.eclipse.cdt.codan.internal.checkers.AssignmentInConditionProblem.params={}
+org.eclipse.cdt.codan.internal.checkers.AssignmentToItselfProblem=Error
+org.eclipse.cdt.codan.internal.checkers.AssignmentToItselfProblem.params={}
+org.eclipse.cdt.codan.internal.checkers.CaseBreakProblem=Warning
+org.eclipse.cdt.codan.internal.checkers.CaseBreakProblem.params={no_break_comment\=>"no break",last_case_param\=>true,empty_case_param\=>false}
+org.eclipse.cdt.codan.internal.checkers.CatchByReference=Warning
+org.eclipse.cdt.codan.internal.checkers.CatchByReference.params={unknown\=>false,exceptions\=>()}
+org.eclipse.cdt.codan.internal.checkers.CircularReferenceProblem=Error
+org.eclipse.cdt.codan.internal.checkers.CircularReferenceProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>false,RUN_ON_INC_BUILD\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}}
+org.eclipse.cdt.codan.internal.checkers.FieldResolutionProblem=Error
+org.eclipse.cdt.codan.internal.checkers.FieldResolutionProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>false,RUN_ON_INC_BUILD\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}}
+org.eclipse.cdt.codan.internal.checkers.FunctionResolutionProblem=Error
+org.eclipse.cdt.codan.internal.checkers.FunctionResolutionProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>false,RUN_ON_INC_BUILD\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}}
+org.eclipse.cdt.codan.internal.checkers.InvalidArguments=Error
+org.eclipse.cdt.codan.internal.checkers.InvalidArguments.params={launchModes\=>{RUN_ON_FULL_BUILD\=>false,RUN_ON_INC_BUILD\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}}
+org.eclipse.cdt.codan.internal.checkers.InvalidTemplateArgumentsProblem=Error
+org.eclipse.cdt.codan.internal.checkers.InvalidTemplateArgumentsProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>false,RUN_ON_INC_BUILD\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}}
+org.eclipse.cdt.codan.internal.checkers.LabelStatementNotFoundProblem=Error
+org.eclipse.cdt.codan.internal.checkers.LabelStatementNotFoundProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>false,RUN_ON_INC_BUILD\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}}
+org.eclipse.cdt.codan.internal.checkers.MemberDeclarationNotFoundProblem=Error
+org.eclipse.cdt.codan.internal.checkers.MemberDeclarationNotFoundProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>false,RUN_ON_INC_BUILD\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}}
+org.eclipse.cdt.codan.internal.checkers.MethodResolutionProblem=Error
+org.eclipse.cdt.codan.internal.checkers.MethodResolutionProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>false,RUN_ON_INC_BUILD\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}}
+org.eclipse.cdt.codan.internal.checkers.NamingConventionFunctionChecker=-Info
+org.eclipse.cdt.codan.internal.checkers.NamingConventionFunctionChecker.params={pattern\=>"^[a-z]",macro\=>true,exceptions\=>()}
+org.eclipse.cdt.codan.internal.checkers.NonVirtualDestructorProblem=Warning
+org.eclipse.cdt.codan.internal.checkers.NonVirtualDestructorProblem.params={}
+org.eclipse.cdt.codan.internal.checkers.OverloadProblem=Error
+org.eclipse.cdt.codan.internal.checkers.OverloadProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>false,RUN_ON_INC_BUILD\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}}
+org.eclipse.cdt.codan.internal.checkers.RedeclarationProblem=Error
+org.eclipse.cdt.codan.internal.checkers.RedeclarationProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>false,RUN_ON_INC_BUILD\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}}
+org.eclipse.cdt.codan.internal.checkers.RedefinitionProblem=Error
+org.eclipse.cdt.codan.internal.checkers.RedefinitionProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>false,RUN_ON_INC_BUILD\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}}
+org.eclipse.cdt.codan.internal.checkers.ReturnStyleProblem=-Warning
+org.eclipse.cdt.codan.internal.checkers.ReturnStyleProblem.params={}
+org.eclipse.cdt.codan.internal.checkers.ScanfFormatStringSecurityProblem=-Warning
+org.eclipse.cdt.codan.internal.checkers.ScanfFormatStringSecurityProblem.params={}
+org.eclipse.cdt.codan.internal.checkers.StatementHasNoEffectProblem=Warning
+org.eclipse.cdt.codan.internal.checkers.StatementHasNoEffectProblem.params={macro\=>true,exceptions\=>()}
+org.eclipse.cdt.codan.internal.checkers.SuggestedParenthesisProblem=Warning
+org.eclipse.cdt.codan.internal.checkers.SuggestedParenthesisProblem.params={paramNot\=>false}
+org.eclipse.cdt.codan.internal.checkers.SuspiciousSemicolonProblem=Warning
+org.eclipse.cdt.codan.internal.checkers.SuspiciousSemicolonProblem.params={else\=>false,afterelse\=>false}
+org.eclipse.cdt.codan.internal.checkers.TypeResolutionProblem=Error
+org.eclipse.cdt.codan.internal.checkers.TypeResolutionProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>false,RUN_ON_INC_BUILD\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}}
+org.eclipse.cdt.codan.internal.checkers.UnusedFunctionDeclarationProblem=Warning
+org.eclipse.cdt.codan.internal.checkers.UnusedFunctionDeclarationProblem.params={macro\=>true}
+org.eclipse.cdt.codan.internal.checkers.UnusedStaticFunctionProblem=Warning
+org.eclipse.cdt.codan.internal.checkers.UnusedStaticFunctionProblem.params={macro\=>true}
+org.eclipse.cdt.codan.internal.checkers.UnusedVariableDeclarationProblem=Warning
+org.eclipse.cdt.codan.internal.checkers.UnusedVariableDeclarationProblem.params={macro\=>true,exceptions\=>("@(\#)","$Id")}
+org.eclipse.cdt.codan.internal.checkers.VariableResolutionProblem=Error
+org.eclipse.cdt.codan.internal.checkers.VariableResolutionProblem.params={launchModes\=>{RUN_ON_FULL_BUILD\=>false,RUN_ON_INC_BUILD\=>false,RUN_AS_YOU_TYPE\=>true,RUN_ON_DEMAND\=>true}}
diff --git a/Makefile b/Makefile
index 7b1fc21..7b5cae0 100644
--- a/Makefile
+++ b/Makefile
@@ -2,9 +2,6 @@
.DEFAULT_GOAL := sketch
-##
-## Useful paths, constants, etc.
-##
ifeq ($(LIB_MAPLE_HOME),)
SRCROOT := .
@@ -12,6 +9,7 @@ else
SRCROOT := $(LIB_MAPLE_HOME)
endif
BUILD_PATH = build
+APP_OBJ_PATH=${BUILD_PATH}/app
LIBMAPLE_PATH := $(SRCROOT)/libmaple
WIRISH_PATH := $(SRCROOT)/wirish
SUPPORT_PATH := $(SRCROOT)/support
@@ -24,6 +22,18 @@ MAKEDIR := $(SUPPORT_PATH)/make
VENDOR_ID := 1EAF
PRODUCT_ID := 0003
+
+##
+## Useful paths, constants, etc.
+##
+SRCS= main.cpp \
+ utils.cpp \
+ esc-control.cpp \
+ # ppm-decode.cpp
+
+INCLUDES=${SRCS:.cpp=.h}
+OBJS=$(addprefix $(APP_OBJ_PATH)/, $(SRCS:.cpp=.o))
+
##
## Target-specific configuration. This determines some compiler and
## linker options/flags.
@@ -31,7 +41,7 @@ PRODUCT_ID := 0003
# Try "make help" for more information on BOARD and MEMORY_TARGET;
# these default to a Maple Flash build.
-BOARD ?= maple
+BOARD ?= maple_mini
MEMORY_TARGET ?= flash
# $(BOARD)- and $(MEMORY_TARGET)-specific configuration
@@ -85,6 +95,10 @@ LIBMAPLE_MODULES += $(SRCROOT)/libraries/FreeRTOS
# Call each module's rules.mk:
$(foreach m,$(LIBMAPLE_MODULES),$(eval $(call LIBMAPLE_MODULE_template,$(m))))
+# app obj folder
+BUILDDIRS+=${APP_OBJ_PATH}
+
+
##
## Targets
##
@@ -117,7 +131,7 @@ ifneq ($(PREV_BUILD_TYPE), $(MEMORY_TARGET))
endif
sketch: build-check MSG_INFO $(BUILD_PATH)/$(BOARD).bin
-
+
clean:
rm -rf build
diff --git a/build-targets.mk b/build-targets.mk
index fb8edc0..e012650 100644
--- a/build-targets.mk
+++ b/build-targets.mk
@@ -1,6 +1,6 @@
# main project target
-$(BUILD_PATH)/main.o: main.cpp
- $(SILENT_CXX) $(CXX) $(CFLAGS) $(CXXFLAGS) $(LIBMAPLE_INCLUDES) $(WIRISH_INCLUDES) -o $@ -c $<
+$(APP_OBJ_PATH)/%.o:%.cpp
+ @echo " [CXX] $(<)" ; $(CXX) $(CFLAGS) $(CXXFLAGS) $(LIBMAPLE_INCLUDES) $(WIRISH_INCLUDES) -o $@ -c $<
$(BUILD_PATH)/libmaple.a: $(BUILDDIRS) $(TGT_BIN)
- rm -f $@
@@ -10,8 +10,8 @@ library: $(BUILD_PATH)/libmaple.a
.PHONY: library
-$(BUILD_PATH)/$(BOARD).elf: $(BUILDDIRS) $(TGT_BIN) $(BUILD_PATH)/main.o
- $(SILENT_LD) $(CXX) $(LDFLAGS) -o $@ $(TGT_BIN) $(BUILD_PATH)/main.o -Wl,-Map,$(BUILD_PATH)/$(BOARD).map
+$(BUILD_PATH)/$(BOARD).elf: $(BUILDDIRS) $(TGT_BIN) $(OBJS)
+ $(SILENT_LD) $(CXX) $(LDFLAGS) -o $@ $(TGT_BIN) $(OBJS) -Wl,-Map,$(BUILD_PATH)/$(BOARD).map
$(BUILD_PATH)/$(BOARD).bin: $(BUILD_PATH)/$(BOARD).elf
$(SILENT_OBJCOPY) $(OBJCOPY) -v -Obinary $(BUILD_PATH)/$(BOARD).elf $@ 1>/dev/null
@@ -40,3 +40,4 @@ MSG_INFO:
@echo ""
@echo "================================================================================"
@echo ""
+
diff --git a/esc-control.cpp b/esc-control.cpp
new file mode 100644
index 0000000..928ae83
--- /dev/null
+++ b/esc-control.cpp
@@ -0,0 +1,171 @@
+#include "wirish.h"
+#include "esc-control.h"
+
+// ASCII escape character
+#define ESC ((uint8)27)
+
+// Default USART baud rate
+#define BAUD 9600
+
+// Servo constants
+#define SERVO_MIN 3430
+#define SERVO_MAX 6800
+#define PPM_CNTS_TO_DEG 0.09
+#define SERVO_ANGLE_TO_DUTY 37.44444
+#define SERVO_PIN 16
+
+uint8 gpio_state[BOARD_NR_GPIO_PINS];
+
+const char* dummy_data = ("qwertyuiopasdfghjklzxcvbnmmmmmm,./1234567890-="
+ "qwertyuiopasdfghjklzxcvbnm,./1234567890");
+
+
+
+void ppm_decode(void){
+ SerialUSB.println("Decoding DIY Drones PPM encoder on pin D15");
+
+ int pin = 15;
+ uint8 prev_state;
+ int time_elapsed = 0;
+ int time_start = 0;
+ int i = 0;
+ int channels[8];
+ float angle;
+
+ pinMode(pin, INPUT);
+ prev_state = (uint8)digitalRead(pin);
+
+ while(!SerialUSB.available()){
+
+ uint8 current_state = (uint8)digitalRead(pin);
+ if (current_state != prev_state) {
+ if (current_state) {
+
+ time_elapsed = (micros() - time_start);
+ time_start = micros();
+
+ }else{
+#ifdef VERBOSE
+ SerialUSB.print(i);
+ SerialUSB.print(":");
+ SerialUSB.print(time_elapsed);
+#endif
+ if(time_elapsed < 2500 && i < 8){
+ if(i==2){
+ if(time_elapsed < 1000){
+ angle = 0.0;
+ }else if(time_elapsed > 2000){
+ angle = 90.0;
+ }else{
+ angle = (float)((time_elapsed-1000)*PPM_CNTS_TO_DEG);
+ }
+ SerialUSB.print(":");
+ SerialUSB.print(angle);
+ set_servo_angle(angle);
+ }
+ channels[i++] = time_elapsed;
+ }else{
+#ifdef VERBOSE
+ SerialUSB.println("");
+#endif
+ i=0;
+ }
+#ifdef VERBOSE
+ SerialUSB.print("\t");
+#endif
+
+ time_elapsed = 0;
+ }
+ prev_state = current_state;
+ }
+
+ }
+ SerialUSB.println("Done!");
+
+}
+
+void set_servo_angle(float angle)
+{
+ //SERVO_MIN = 3430
+ //SERVO_MAX = 6800
+ //SERVO_CNTS_TO_DEG=0.0267
+ int duty;
+
+ disable_usarts();
+
+ duty = SERVO_MIN + (int)(angle * SERVO_ANGLE_TO_DUTY);
+ if(duty > SERVO_MAX) duty = SERVO_MAX;
+ pwmWrite(SERVO_PIN, duty);
+}
+
+void cmd_servo_sweep(void) {
+ SerialUSB.println("Testing D16 PWM header with a servo sweep. "
+ "Press any key to stop.");
+ SerialUSB.println();
+
+ disable_usarts();
+ init_all_timers(21);
+
+ for (uint32 i = 0; i < BOARD_NR_PWM_PINS; i++) {
+ if (boardUsesPin(i))
+ continue;
+ pinMode(boardPWMPins[i], PWM);
+ pwmWrite(boardPWMPins[i], 4000);
+ }
+
+ // 1.25ms = 4096counts = 0deg
+ // 1.50ms = 4915counts = 90deg
+ // 1.75ms = 5734counts = 180deg
+ //int rate = 4096;
+ int rate = 3430; // 1 ms
+ while (!SerialUSB.available()) {
+ rate += 20;
+ if (rate > 6800)//5734) 6800 = 2ms
+ rate = 3430;//4096;
+ //for (uint32 i = 0; i < BOARD_NR_PWM_PINS; i++) {
+ // if (boardUsesPin(i))
+ // continue;
+ // pwmWrite(boardPWMPins[i], rate);
+ //}
+ pwmWrite(16, rate);
+ delay(20);
+ }
+
+ for (uint32 i = 0; i < BOARD_NR_PWM_PINS; i++) {
+ if (boardUsesPin(i))
+ continue;
+ pinMode(boardPWMPins[i], OUTPUT);
+ }
+ init_all_timers(1);
+ enable_usarts();
+}
+
+void cmd_board_info(void) { // TODO print more information
+ SerialUSB.println("Board information");
+ SerialUSB.println("=================");
+
+ SerialUSB.print("* Clock speed (MHz): ");
+ SerialUSB.println(CYCLES_PER_MICROSECOND);
+
+ SerialUSB.print("* BOARD_LED_PIN: ");
+ SerialUSB.println(BOARD_LED_PIN);
+
+ SerialUSB.print("* BOARD_BUTTON_PIN: ");
+ SerialUSB.println(BOARD_BUTTON_PIN);
+
+ SerialUSB.print("* GPIO information (BOARD_NR_GPIO_PINS = ");
+ SerialUSB.print(BOARD_NR_GPIO_PINS);
+ SerialUSB.println("):");
+ print_board_array("ADC pins", boardADCPins, BOARD_NR_ADC_PINS);
+ print_board_array("PWM pins", boardPWMPins, BOARD_NR_PWM_PINS);
+ print_board_array("Used pins", boardUsedPins, BOARD_NR_USED_PINS);
+}
+
+// -- Helper functions --------------------------------------------------------
+
+static uint16 init_all_timers_prescale = 0;
+
+static void set_prescale(timer_dev *dev) {
+ timer_set_prescaler(dev, init_all_timers_prescale);
+}
+
diff --git a/esc-control.h b/esc-control.h
new file mode 100644
index 0000000..f7c485c
--- /dev/null
+++ b/esc-control.h
@@ -0,0 +1,15 @@
+#include "wirish.h"
+
+// Commands
+void cmd_print_help(void);
+void cmd_servo_sweep(void);
+void cmd_board_info(void);
+void ppm_decode(void);
+void set_servo_angle(float angle);
+
+// Helper functions
+void init_all_timers(uint16 prescale);
+void enable_usarts(void);
+void disable_usarts(void);
+void print_board_array(const char* msg, const uint8 arr[], int len);
+
diff --git a/input-capture.mk b/input-capture.mk
new file mode 100644
index 0000000..7b1fc21
--- /dev/null
+++ b/input-capture.mk
@@ -0,0 +1,181 @@
+# Try "make help" first
+
+.DEFAULT_GOAL := sketch
+
+##
+## Useful paths, constants, etc.
+##
+
+ifeq ($(LIB_MAPLE_HOME),)
+SRCROOT := .
+else
+SRCROOT := $(LIB_MAPLE_HOME)
+endif
+BUILD_PATH = build
+LIBMAPLE_PATH := $(SRCROOT)/libmaple
+WIRISH_PATH := $(SRCROOT)/wirish
+SUPPORT_PATH := $(SRCROOT)/support
+# Support files for linker
+LDDIR := $(SUPPORT_PATH)/ld
+# Support files for this Makefile
+MAKEDIR := $(SUPPORT_PATH)/make
+
+# USB ID for DFU upload
+VENDOR_ID := 1EAF
+PRODUCT_ID := 0003
+
+##
+## Target-specific configuration. This determines some compiler and
+## linker options/flags.
+##
+
+# Try "make help" for more information on BOARD and MEMORY_TARGET;
+# these default to a Maple Flash build.
+BOARD ?= maple
+MEMORY_TARGET ?= flash
+
+# $(BOARD)- and $(MEMORY_TARGET)-specific configuration
+include $(MAKEDIR)/target-config.mk
+
+##
+## Compilation flags
+##
+
+GLOBAL_FLAGS := -D$(VECT_BASE_ADDR) \
+ -DBOARD_$(BOARD) -DMCU_$(MCU) \
+ -DERROR_LED_PORT=$(ERROR_LED_PORT) \
+ -DERROR_LED_PIN=$(ERROR_LED_PIN) \
+ -D$(DENSITY)
+GLOBAL_CFLAGS := -Os -g3 -gdwarf-2 -mcpu=cortex-m3 -mthumb -march=armv7-m \
+ -nostdlib -ffunction-sections -fdata-sections \
+ -Wl,--gc-sections $(GLOBAL_FLAGS)
+GLOBAL_CXXFLAGS := -fno-rtti -fno-exceptions -Wall $(GLOBAL_FLAGS)
+GLOBAL_ASFLAGS := -mcpu=cortex-m3 -march=armv7-m -mthumb \
+ -x assembler-with-cpp $(GLOBAL_FLAGS)
+LDFLAGS = -T$(LDDIR)/$(LDSCRIPT) -L$(LDDIR) \
+ -mcpu=cortex-m3 -mthumb -Xlinker \
+ --gc-sections --print-gc-sections --march=armv7-m -Wall
+
+##
+## Build rules and useful templates
+##
+
+include $(SUPPORT_PATH)/make/build-rules.mk
+include $(SUPPORT_PATH)/make/build-templates.mk
+
+##
+## Set all submodules here
+##
+
+# Try to keep LIBMAPLE_MODULES a simply-expanded variable
+ifeq ($(LIBMAPLE_MODULES),)
+ LIBMAPLE_MODULES := $(SRCROOT)/libmaple
+else
+ LIBMAPLE_MODULES += $(SRCROOT)/libmaple
+endif
+LIBMAPLE_MODULES += $(SRCROOT)/wirish
+# Official libraries:
+LIBMAPLE_MODULES += $(SRCROOT)/libraries/Servo
+LIBMAPLE_MODULES += $(SRCROOT)/libraries/LiquidCrystal
+LIBMAPLE_MODULES += $(SRCROOT)/libraries/Wire
+
+# Experimental libraries:
+LIBMAPLE_MODULES += $(SRCROOT)/libraries/FreeRTOS
+
+# Call each module's rules.mk:
+$(foreach m,$(LIBMAPLE_MODULES),$(eval $(call LIBMAPLE_MODULE_template,$(m))))
+
+##
+## Targets
+##
+
+# main target
+include $(SRCROOT)/build-targets.mk
+
+.PHONY: install sketch clean help debug cscope tags ctags ram flash jtag doxygen mrproper
+
+# Target upload commands
+UPLOAD_ram := $(SUPPORT_PATH)/scripts/reset.py && \
+ sleep 1 && \
+ $(DFU) -a0 -d $(VENDOR_ID):$(PRODUCT_ID) -D $(BUILD_PATH)/$(BOARD).bin -R
+UPLOAD_flash := $(SUPPORT_PATH)/scripts/reset.py && \
+ sleep 1 && \
+ $(DFU) -a1 -d $(VENDOR_ID):$(PRODUCT_ID) -D $(BUILD_PATH)/$(BOARD).bin -R
+UPLOAD_jtag := $(OPENOCD_WRAPPER) flash
+
+# Conditionally upload to whatever the last build was
+install: INSTALL_TARGET = $(shell cat $(BUILD_PATH)/build-type 2>/dev/null)
+install: $(BUILD_PATH)/$(BOARD).bin
+ @echo "Install target:" $(INSTALL_TARGET)
+ $(UPLOAD_$(INSTALL_TARGET))
+
+# Force a rebuild if the target changed
+PREV_BUILD_TYPE = $(shell cat $(BUILD_PATH)/build-type 2>/dev/null)
+build-check:
+ifneq ($(PREV_BUILD_TYPE), $(MEMORY_TARGET))
+ $(shell rm -rf $(BUILD_PATH))
+endif
+
+sketch: build-check MSG_INFO $(BUILD_PATH)/$(BOARD).bin
+
+clean:
+ rm -rf build
+
+mrproper: clean
+ rm -rf doxygen
+
+help:
+ @echo ""
+ @echo " libmaple Makefile help"
+ @echo " ----------------------"
+ @echo " "
+ @echo " Programming targets:"
+ @echo " sketch: Compile for BOARD to MEMORY_TARGET (default)."
+ @echo " install: Compile and upload code over USB, using Maple bootloader"
+ @echo " "
+ @echo " You *must* set BOARD if not compiling for Maple (e.g."
+ @echo " use BOARD=maple_mini for mini, etc.), and MEMORY_TARGET"
+ @echo " if not compiling to Flash."
+ @echo " "
+ @echo " Valid BOARDs:"
+ @echo " maple, maple_mini, maple_RET6, maple_native"
+ @echo " "
+ @echo " Valid MEMORY_TARGETs (default=flash):"
+ @echo " ram: Compile sketch code to ram"
+ @echo " flash: Compile sketch code to flash"
+ @echo " jtag: Compile sketch code for jtag; overwrites bootloader"
+ @echo " "
+ @echo " Other targets:"
+ @echo " debug: Start OpenOCD gdb server on port 3333, telnet on port 4444"
+ @echo " clean: Remove all build and object files"
+ @echo " help: Show this message"
+ @echo " doxygen: Build Doxygen HTML and XML documentation"
+ @echo " mrproper: Remove all generated files"
+ @echo " "
+
+debug:
+ $(OPENOCD_WRAPPER) debug
+
+cscope:
+ rm -rf *.cscope
+ find . -name '*.[hcS]' -o -name '*.cpp' | xargs cscope -b
+
+tags:
+ etags `find . -name "*.c" -o -name "*.cpp" -o -name "*.h"`
+ @echo "Made TAGS file for EMACS code browsing"
+
+ctags:
+ ctags-exuberant -R .
+ @echo "Made tags file for VIM code browsing"
+
+ram:
+ @$(MAKE) MEMORY_TARGET=ram --no-print-directory sketch
+
+flash:
+ @$(MAKE) MEMORY_TARGET=flash --no-print-directory sketch
+
+jtag:
+ @$(MAKE) MEMORY_TARGET=jtag --no-print-directory sketch
+
+doxygen:
+ doxygen $(SUPPORT_PATH)/doxygen/Doxyfile
diff --git a/main.cpp b/main.cpp
new file mode 100644
index 0000000..bd28b99
--- /dev/null
+++ b/main.cpp
@@ -0,0 +1,96 @@
+// Comment out to disable USB printouts
+#define USB_VERBOSE
+
+// Include files
+#include "wirish.h"
+#include "utils.h"
+#include "esc-control.h"
+//#include "ppm-decode.h"
+
+
+// ASCII escape character
+#define ESC ((uint8)27)
+
+
+// Servo constants
+#define SERVO_MIN 3430
+#define SERVO_MAX 6800
+#define PPM_CNTS_TO_DEG 0.09
+#define SERVO_ANGLE_TO_DUTY 37.44444
+#define SERVO_PIN 16
+
+// Globals
+HardwareTimer timer4(4);
+
+// -- setup() and loop() ------------------------------------------------------
+
+void setup() {
+ // Set up the LED to blink
+ pinMode(BOARD_LED_PIN, OUTPUT);
+
+ // Setup timer4 for PWM
+ timer4.setMode(TIMER_CH1, TIMER_PWM);
+ timer4.setPrescaleFactor(21);
+ pinMode(SERVO_PIN, PWM);
+
+ //ppm decode setup
+ SerialUSB.begin();
+ while(!isConnected()); //wait till console attaches.
+ SerialUSB.println("Hello!");
+}
+
+
+// Force init to be called *first*, i.e. before static object allocation.
+// Otherwise, statically allocated objects that need libmaple may fail.
+__attribute__((constructor)) void premain() {
+ init();
+}
+
+int main(void) {
+ // init
+ setup();
+
+ while (1) {
+ toggleLED();
+ delay(250);
+
+ while (SerialUSB.available()) {
+ uint8 input = SerialUSB.read();
+ SerialUSB.println((char)input);
+
+ switch(input) {
+ case '\r':
+ break;
+
+ case ' ':
+ SerialUSB.println("spacebar, nice!");
+ break;
+
+ case '?':
+ case 'h':
+ cmd_print_help();
+ break;
+
+ case 't':
+ ppm_decode();
+ break;
+
+ case 's':
+ cmd_servo_sweep();
+ break;
+
+ case 'b':
+ cmd_board_info();
+ break;
+
+ default: // -------------------------------
+ SerialUSB.print("Unexpected byte: 0x");
+ SerialUSB.print((int)input, HEX);
+ SerialUSB.println(", press h for help.");
+ }
+
+ SerialUSB.print("> ");
+ }
+ }
+ return 0;
+}
diff --git a/main.h b/main.h
new file mode 100644
index 0000000..e69de29
diff --git a/ppm-decode.cpp b/ppm-decode.cpp
new file mode 100644
index 0000000..6da36ce
--- /dev/null
+++ b/ppm-decode.cpp
@@ -0,0 +1,370 @@
+///**********************************************************************
+// Various Maple tests.. including timer capture to memory via dma =)
+// */
+//#include "wirish.h"
+//#include "usb.h"
+//#include "timer.h"
+//#include "dma.h"
+//
+//
+///**********************************************************************
+// timer tests.. seeing if the timer clock really does run at 72mhz..
+// */
+//
+//volatile int overflowCount=0;
+//void countOverflows(){
+// overflowCount++;
+//}
+//
+//int PRESCALE=128;
+//int WAIT=50;
+//int results[16];
+//int overflow[16];
+//void testHardwareTimer(HardwareTimer t){
+// t.attachInterrupt(1,countOverflows);
+// for(int i=0; i<16; i++){
+// t.setCount(0);
+// overflowCount=0;
+// t.resume();
+// delay(WAIT);
+// t.pause();
+// results[i]=t.getCount();
+// overflow[i]=overflowCount;
+// }
+// t.detachInterrupt(1);
+//}
+//void printResults(){
+// SerialUSB.print("Timing results: ");
+// for(int i=0; i<16; i++){
+// SerialUSB.print(results[i]);
+// SerialUSB.print(" ");
+// }
+// SerialUSB.println();
+// SerialUSB.print("Overflows: ");
+// for(int i=0; i<16; i++){
+// SerialUSB.print(overflow[i]);
+// SerialUSB.print(" ");
+// }
+// SerialUSB.println();
+// SerialUSB.print("Each count worth approx (ns): ");
+// for(int i=0; i<16; i++){
+// SerialUSB.print( waitToNanos(overflow[i], results[i]) );
+// SerialUSB.print(" ");
+// }
+// SerialUSB.println();
+//}
+//double expectedTimePeriod(){
+// //in nanos.. so 72mhz = 72000khz = 72000000hz 1/72000000hz = tick in seconds
+// // 1/72000 = tick in ms, 1/72 = tick in us (1/72) * 1000 = tick in ns
+// //tick in ns * prescale == time we're supposed to see
+// return ((double)1.0 / ((double)72.0)) * (double)1000.0 * (double)PRESCALE;
+//}
+//double waitToNanos( int overflows, int count ){
+// //wait is in millis, *1000 for micros, *1000 for nanos
+// double time = (((double)WAIT * (double)1000.0 * (double)1000.0) ) ;
+// time = time / ((double)count + ((double)65535*(double)overflows));
+// return time;
+//}
+//
+//int readInt(char terminator){
+// char current;
+// int output=0;
+// while(SerialUSB.available() && (current=SerialUSB.read())!=terminator){
+// if(current>='0' && current<='9'){
+// output=output*10;
+// output+=(current-'0');
+// }else{
+// output=-1;
+// break;
+// }
+// }
+// return output;
+//}
+//
+//HardwareTimer timer2 = HardwareTimer(2);
+//HardwareTimer timer1 = HardwareTimer(1);
+//void timingTest(){
+// SerialUSB.println("Starting Timing test");
+// SerialUSB.print(" Prescale: ");
+// SerialUSB.println(PRESCALE);
+// SerialUSB.print(" Wait Period (ms): ");
+// SerialUSB.println(WAIT);
+// SerialUSB.print(" Expected value for each tick :");
+// SerialUSB.println(expectedTimePeriod());
+// timer1.pause();
+// timer2.pause();
+//
+// timer2.setMode(TIMER_CH1, TIMER_OUTPUT_COMPARE);
+// timer2.setPrescaleFactor(PRESCALE);
+// timer2.setOverflow(65535);
+// timer2.setCompare(1,65535);
+// timer2.setCount(0);
+// timer2.refresh();
+//
+// timer1.setMode(TIMER_CH1, TIMER_OUTPUT_COMPARE);
+// timer1.setPrescaleFactor(PRESCALE);
+// timer1.setOverflow(65535);
+// timer1.setCompare(1,65535);
+// timer1.setCount(0);
+// timer1.refresh();
+//
+// testHardwareTimer(timer1);
+// printResults();
+//
+// testHardwareTimer(timer2);
+// printResults();
+//}
+//
+///******************************************************************************
+// DMA from Timer Capture to array test..
+// */
+//
+////number of captures to do..
+//#define TIMERS 512
+//volatile int exit=0; //set to 1 when dma complete.
+//volatile uint16 data[TIMERS]; //place to put the data via dma
+//
+////dump routine to show content of captured data.
+//void printData(){
+// SerialUSB.print("DATA: ");
+// for(int i=0; i ");
+// SerialUSB.println(" eg. e 128 200 for a prescale of 128 and wait of 200");
+//done:
+// break;
+// }
+// case 'f':{
+// timer_dev *t = TIMER1;
+//
+// timer1.pause();
+// timer1.setPrescaleFactor(128);
+// timer1.setOverflow(65535);
+// timer1.setCount(0);
+// timer1.refresh();
+//
+// timer_reg_map r = t->regs;
+//
+// //using timer1, channel1, maps to pin d6
+// //according to maple master pin map.
+// pinMode(6,INPUT_PULLUP);
+//
+// //capture compare regs TIMx_CCRx used to hold val after a transition on corresponding ICx
+//
+// //when cap occurs, flag CCXIF (TIMx_SR register) is set,
+// //and interrupt, or dma req can be sent if they are enabled.
+//
+// //if cap occurs while flag is already high CCxOF (overcapture) flag is set..
+//
+// //CCIX can be cleared by writing 0, or by reading the capped data from TIMx_CCRx
+// //CCxOF is cleared by writing 0 to it.
+//
+// //Capture/Compare 1 Selection
+// // set CC1S bits to 01 in the capture compare mode register.
+// // 01 selects TI1 as the input to use. (page 336 stm32 reference)
+// // (assuming here that TI1 is D6, according to maple master pin map)
+// //CC1S bits are bits 0,1
+// bitSet(r.adv->CCMR1, 0);
+// bitClear(r.adv->CCMR1, 1);
+//
+//
+// //Input Capture 1 Filter.
+// // need to set IC1F bits according to a table saying how long
+// // we should wait for a signal to be 'stable' to validate a transition
+// // on the input.
+// // (page 336 stm32 reference)
+// //IC1F bits are bits 7,6,5,4
+// bitClear(r.adv->CCMR1, 7);
+// bitClear(r.adv->CCMR1, 6);
+// bitSet(r.adv->CCMR1, 5);
+// bitSet(r.adv->CCMR1, 4);
+//
+// //sort out the input capture prescaler..
+// //00 no prescaler.. capture is done as soon as edge is detected
+// bitClear(r.adv->CCMR1, 3);
+// bitClear(r.adv->CCMR1, 2);
+//
+// //select the edge for the transition on TI1 channel using CC1P in CCER
+// //CC1P is bit 1 of CCER (page 339)
+// // 0 = falling
+// // 1 = rising
+// bitClear(r.adv->CCER,1);
+//
+// //set the CC1E bit to enable capture from the counter.
+// //CCE1 is bit 0 of CCER (page 339)
+// bitSet(r.adv->CCER,0);
+//
+// //enable dma for this timer..
+// //sets the Capture/Compare 1 DMA request enable bit on the DMA/interrupt enable register.
+// //bit 9 is CC1DE as defined on page 329.
+// bitSet(r.adv->DIER,9);
+//
+// dma_init(DMA1);
+// dma_setup_transfer( DMA1, //dma device, dma1 here because that's the only one we have
+// DMA_CH2, //dma channel, channel2, because it looks after tim1_ch1 (timer1, channel1)
+// &(r.adv->CCR1), //peripheral address
+// DMA_SIZE_16BITS, //peripheral size
+// data, //memory address
+// DMA_SIZE_16BITS, //memory transfer size
+// (0
+// //| DMA_FROM_MEM //set if going from memory, don't set if going to memory.
+// | DMA_MINC_MODE //auto inc where the data does in memory (uses size_16bits to know how much)
+// | DMA_TRNS_ERR //tell me if it's fubar
+// //| DMA_HALF_TRNS //tell me half way (actually, don't as I spend so long there, I dont see 'complete')
+// | DMA_TRNS_CMPLT //tell me at the end
+// )
+// );
+//
+// dma_attach_interrupt(DMA1, DMA_CH2, dma_isr); //hook up an isr for the dma chan to tell us if things happen.
+// dma_set_num_transfers(DMA1, DMA_CH2, TIMERS); //only allow it to transfer TIMERS number of times.
+// dma_enable(DMA1, DMA_CH2); //enable it..
+//
+// SerialUSB.println("Starting timer.. falling edge of D6 (hopefully)");
+// //start the timer counting.
+// timer1.resume();
+// //the timer is now counting up, and any falling edges on D6
+// //will trigger a DMA request to clone the timercapture to the array
+//
+// //If we weren't using DMA, we could busy wait on the CC1IF bit in SR
+// //
+// //when the timer captures, the CC1IF bit will be set on the timer SR register.
+// //CC1IF bit is bit 1 (page 332)
+// //while(!bitRead(r.adv->SR, 1)){
+// //}
+// //SerialUSB.println("Timer triggered : ");
+// //SerialUSB.println(r.adv->CCR1);
+//
+// SerialUSB.println("Waiting for exit flag from dma...");
+// //busy wait on the exit flag
+// //we could do real work here if wanted..
+// while(!exit);
+// //dump the data
+// printData();
+//
+//
+// //turn off the timer & tidy up before we leave this cmd.
+// timer1.pause();
+// dma_disable(DMA1, DMA_CH2);
+// dma_detach_interrupt(DMA1, DMA_CH2);
+//
+// break;
+// }
+// }
+//}
+//
+//
+//
+//
diff --git a/utils.cpp b/utils.cpp
new file mode 100644
index 0000000..93283d8
--- /dev/null
+++ b/utils.cpp
@@ -0,0 +1,64 @@
+#include "utils.h"
+
+
+static uint16 init_all_timers_prescale;
+
+
+void cmd_print_help(void) {
+ SerialUSB.println("");
+ SerialUSB.println("Command Listing");
+ SerialUSB.println("\t?: print this menu");
+ SerialUSB.println("\tb: print information about the board.");
+ SerialUSB.println("\ts: servo sweep on pin D16.");
+ SerialUSB.println("\tt: ppm decode on pin D15.");
+}
+
+
+
+void set_prescale(timer_dev *dev) {
+ timer_set_prescaler(dev, init_all_timers_prescale);
+}
+
+void init_all_timers(uint16 prescale) {
+ init_all_timers_prescale = prescale;
+ timer_foreach(set_prescale);
+}
+
+void enable_usarts(void) {
+ Serial1.begin(BAUD);
+ Serial2.begin(BAUD);
+ Serial3.begin(BAUD);
+#if defined(STM32_HIGH_DENSITY) && !defined(BOARD_maple_RET6)
+ Serial4.begin(BAUD);
+ Serial5.begin(BAUD);
+#endif
+}
+
+void disable_usarts(void) {
+ Serial1.end();
+ Serial2.end();
+ Serial3.end();
+#if defined(STM32_HIGH_DENSITY) && !defined(BOARD_maple_RET6)
+ Serial4.end();
+ Serial5.end();
+#endif
+}
+
+void print_board_array(const char* msg, const uint8 arr[], int len) {
+ SerialUSB.print("\t");
+ SerialUSB.print(msg);
+ SerialUSB.print(" (");
+ SerialUSB.print(len);
+ SerialUSB.print("): ");
+ for (int i = 0; i < len; i++) {
+ SerialUSB.print(arr[i], DEC);
+ if (i < len - 1) SerialUSB.print(", ");
+ }
+ SerialUSB.println();
+}
+
+//quick method to return if there's anyone on the other end of the serialusb link yet.
+boolean isConnected(){
+ return (SerialUSB.isConnected() && (SerialUSB.getDTR() || SerialUSB.getRTS()));
+}
+
diff --git a/utils.h b/utils.h
new file mode 100644
index 0000000..1722a98
--- /dev/null
+++ b/utils.h
@@ -0,0 +1,14 @@
+#include "wirish.h"
+
+
+// Default USART baud rate
+#define BAUD 9600
+
+
+void cmd_print_help(void);
+void set_prescale(timer_dev *dev);
+void init_all_timers(uint16 prescale);
+void enable_usarts(void);
+void disable_usarts(void);
+void print_board_array(const char* msg, const uint8 arr[], int len);
+boolean isConnected();