rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

Merge remote branch 'origin/next' into OP-536_CorvusCorax_ObjectOrientedFirmwareUAVTalk

Browse Source 
Conflicts:
	flight/Modules/Telemetry/telemetry.c
This commit is contained in:
Corvus Corax 2011-08-17 08:30:32 +02:00
commit 6fe495c088
311 changed files with 23286 additions and 12174 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
CREDITS.txt
View File

@ -27,6 +27,10 @@ N: Pedro Assuncao
E: pedro (dot) agda (plus) openpilot (at) gmail (dot) com
D: Initial GCS Settings Gadget work
N: Werner Backes
E: werner (at) bit-1 (dot) de
D: Port of CopterControl to PS3 Move Controller (MoveCopter)
N: Jose Barros
E: josembarros (at) hotmail (dot) com
D: Next-Gen OP Map Lib, Y-Modem Library, Uploader Plugin

30
HISTORY.txt Normal file
View File

@ -0,0 +1,30 @@
Short summary of changes. For a complete list see the git log.
2011-08-04
Fixed packaging aesthetic issues. Also avoid runtime issues on OSX Lion by
disabling the ModelView and Notify plugins for now (sorry).
2011-07-29
Added support for PPM receivers from James W. Now all 4 interfaces (R/C
standard PWM, combined PPM (MK), Spektrum satellite, Futaba S.Bus) are
supported and configurable through the GCS hardware configuration tab.
2011-07-17
Updated module initialization from Mathieu which separates the initialization
from the task startup. Also implements a method to reclaim unused ram from
initialization and end of memory for the FreeRTOS heap.
2011-07-12
Improvements to the stabilization code. Included a LPF on the gyros to smooth
out noise in high vibration environments. Also two new modes: axis-lock and
weak leveling. Axis-lock will try and hold an axis at a fixed position and
reject any disturbances. This is like heading-hold on a heli for the tail but
can be useful for other axes. Weak leveling is rate mode with a weak
correction to self level the craft - good for easier rate mode flying.
2011-07-07
Dynamic hardware configuration from Stac. The input type is now
selected from ManualControlSettings.InputMode and the aircraft must be rebooted
after changing this. Also for CopterControl the HwSettings object must
indicate which modules are connected to which ports. PPM currently not
working.

39
MILESTONES.txt
View File

@ -117,11 +117,45 @@ C: Sami Korhonen (Sambas)
D: May 2011
V: http://vimeo.com/24258192
M: First CopterControl flip on a Flybarless Heli
M: First Y6 CopterControl flight
C: Michel Pet
D: June 2011
V: http://www.youtube.com/watch?v=QsE2MQELPZY
M: First MoveCopter flight
C: Werner Backes
D: July 2011
V: http://vimeo.com/25983655
M: First Y4 CopterControl flight
C: Mat Wellington
D: July 2011
V: http://www.youtube.com/watch?v=TxZ4MDGIj1o
M: First V-Tail4 CopterControl flight
C: Mat Wellington
D: July 2011
V: http://www.youtube.com/watch?v=YE4Fd9vdg1I
M: First Altitude Hold using Sonar
C:
D:
V:
M: First CopterControl Navigation on RC Ground Vechicle
C:
D:
V:
M: First CopterControl Navigation on RC Water Vechicle
C:
D:
V:
M: First CopterControl flip on a Flybarless Heli
C:
D:
V:
An incomplete list of some future Milestones is below:
@ -131,8 +165,7 @@ An incomplete list of some future Milestones is below:
* First successful flight using the GCS only and no RC TX
* First use of Magic Waypoint
* First Flybarless Helicopter flight with OpenPilot
* First flight with CopterControl
* First fixed wing navigation flight
* First fixed wing navigation flight
* First Multirotor navigation flight
* First Helicopter navigation flight
* First over 1km navigation flight

75
Makefile
View File

@ -71,6 +71,7 @@ help:
@echo " qt_sdk_install - Install the QT v4.6.2 tools"
@echo " arm_sdk_install - Install the Code Sourcery ARM gcc toolchain"
@echo " openocd_install - Install the OpenOCD JTAG daemon"
@echo " stm32flash_install - Install the stm32flash tool for unbricking boards"
@echo
@echo " [Big Hammer]"
@echo " all - Generate UAVObjects, build openpilot firmware and gcs"
@ -92,21 +93,25 @@ help:
@echo " <board> - Build firmware for <board>"
@echo " supported boards are ($(ALL_BOARDS))"
@echo " fw_<board> - Build firmware for <board>"
@echo " supported boards are ($(FW_TARGETS))"
@echo " supported boards are ($(FW_BOARDS))"
@echo " fw_<board>_clean - Remove firmware for <board>"
@echo " fw_<board>_program - Use OpenOCD + JTAG to write firmware to <board>"
@echo
@echo " [Bootloader]"
@echo " bl_<board> - Build bootloader for <board>"
@echo " supported boards are ($(BL_TARGETS))"
@echo " supported boards are ($(BL_BOARDS))"
@echo " bl_<board>_clean - Remove bootloader for <board>"
@echo " bl_<board>_program - Use OpenOCD + JTAG to write bootloader to <board>"
@echo
@echo " [Bootloader Updater]"
@echo " bu_<board> - Build bootloader updater for <board>"
@echo " supported boards are ($(BU_TARGETS))"
@echo " supported boards are ($(BU_BOARDS))"
@echo " bu_<board>_clean - Remove bootloader updater for <board>"
@echo
@echo " [Unbrick a board]"
@echo " unbrick_<board> - Use the STM32's built in boot ROM to write a bootloader to <board>"
@echo " supported boards are ($(BL_BOARDS))"
@echo
@echo " [Simulation]"
@echo " sim_posix - Build OpenPilot simulation firmware for"
@echo " a POSIX compatible system (Linux, Mac OS X, ...)"
@ -175,10 +180,10 @@ qt_sdk_clean:
$(V1) [ ! -d "$(QT_SDK_DIR)" ] || $(RM) -rf $(QT_SDK_DIR)
# Set up ARM (STM32) SDK
ARM_SDK_DIR := $(TOOLS_DIR)/arm-2009q3
ARM_SDK_DIR := $(TOOLS_DIR)/arm-2011.03
.PHONY: arm_sdk_install
arm_sdk_install: ARM_SDK_URL := http://www.codesourcery.com/sgpp/lite/arm/portal/package5353/public/arm-none-eabi/arm-2009q3-68-arm-none-eabi-i686-pc-linux-gnu.tar.bz2
arm_sdk_install: ARM_SDK_URL := http://www.codesourcery.com/sgpp/lite/arm/portal/package8734/public/arm-none-eabi/arm-2011.03-42-arm-none-eabi-i686-pc-linux-gnu.tar.bz2
arm_sdk_install: ARM_SDK_FILE := $(notdir $(ARM_SDK_URL))
# order-only prereq on directory existance:
arm_sdk_install: | $(DL_DIR) $(TOOLS_DIR)
@ -226,6 +231,25 @@ openocd_install: openocd_clean
openocd_clean:
$(V1) [ ! -d "$(OPENOCD_DIR)" ] || $(RM) -r "$(OPENOCD_DIR)"
STM32FLASH_DIR := $(TOOLS_DIR)/stm32flash
.PHONY: stm32flash_install
stm32flash_install: STM32FLASH_URL := http://stm32flash.googlecode.com/svn/trunk
stm32flash_install: STM32FLASH_REV := 52
stm32flash_install: stm32flash_clean
# download the source
$(V0) @echo " DOWNLOAD $(STM32FLASH_URL) @ r$(STM32FLASH_REV)"
$(V1) svn export -q -r "$(STM32FLASH_REV)" "$(STM32FLASH_URL)" "$(STM32FLASH_DIR)"
# build
$(V0) @echo " BUILD $(STM32FLASH_DIR)"
$(V1) $(MAKE) --silent -C $(STM32FLASH_DIR) all
.PHONY: stm32flash_clean
stm32flash_clean:
$(V0) @echo " CLEAN $(STM32FLASH_DIR)"
$(V1) [ ! -d "$(STM32FLASH_DIR)" ] || $(RM) -r "$(STM32FLASH_DIR)"
##############################
#
# Set up paths to tools
@ -277,7 +301,7 @@ openpilotgcs: uavobjects_gcs
.PHONY: openpilotgcs_clean
openpilotgcs_clean:
$(V0) @echo " CLEAN $@"
$(V0) @echo " CLEAN $@"
$(V1) [ ! -d "$(BUILD_DIR)/ground/openpilotgcs" ] || $(RM) -r "$(BUILD_DIR)/ground/openpilotgcs"
.PHONY: uavobjgenerator
@ -307,7 +331,7 @@ uavobjects_test: $(UAVOBJ_OUT_DIR) uavobjgenerator
$(V1) $(UAVOBJGENERATOR) -v -none $(UAVOBJ_XML_DIR) $(ROOT_DIR)
uavobjects_clean:
$(V0) @echo " CLEAN $@"
$(V0) @echo " CLEAN $@"
$(V1) [ ! -d "$(UAVOBJ_OUT_DIR)" ] || $(RM) -r "$(UAVOBJ_OUT_DIR)"
##############################
@ -335,7 +359,7 @@ fw_$(1)_%: uavobjects_flight
.PHONY: $(1)_clean
$(1)_clean: fw_$(1)_clean
fw_$(1)_clean:
$(V0) @echo " CLEAN $$@"
$(V0) @echo " CLEAN $$@"
$(V1) $(RM) -fr $(BUILD_DIR)/fw_$(1)
endef
@ -355,9 +379,23 @@ bl_$(1)_%:
REMOVE_CMD="$(RM)" OOCD_EXE="$(OPENOCD)" \
$$*
.PHONY: unbrick_$(1)
unbrick_$(1): bl_$(1)_hex
$(if $(filter-out undefined,$(origin UNBRICK_TTY)),
$(V0) @echo " UNBRICK $(1) via $$(UNBRICK_TTY)"
$(V1) $(STM32FLASH_DIR)/stm32flash \
-w $(BUILD_DIR)/bl_$(1)/bl_$(1).hex \
-g 0x0 \
$$(UNBRICK_TTY)
,
$(V0) @echo
$(V0) @echo "ERROR: You must specify UNBRICK_TTY=<serial-device> to use for unbricking."
$(V0) @echo " eg. $$(MAKE) $$@ UNBRICK_TTY=/dev/ttyUSB0"
)
.PHONY: bl_$(1)_clean
bl_$(1)_clean:
$(V0) @echo " CLEAN $$@"
$(V0) @echo " CLEAN $$@"
$(V1) $(RM) -fr $(BUILD_DIR)/bl_$(1)
endef
@ -377,7 +415,7 @@ bu_$(1)_%: bl_$(1)_bino
.PHONY: bu_$(1)_clean
bu_$(1)_clean:
$(V0) @echo " CLEAN $$@"
$(V0) @echo " CLEAN $$@"
$(V1) $(RM) -fr $(BUILD_DIR)/bu_$(1)
endef
@ -403,14 +441,20 @@ pipxtreme_friendly := PipXtreme
ins_friendly := INS
ahrs_friendly := AHRS
FW_TARGETS := $(addprefix fw_, $(ALL_BOARDS))
BL_TARGETS := $(addprefix bl_, $(ALL_BOARDS))
BU_TARGETS := $(addprefix bu_, $(ALL_BOARDS))
# Start out assuming that we'll build fw, bl and bu for all boards
FW_BOARDS := $(ALL_BOARDS)
BL_BOARDS := $(ALL_BOARDS)
BU_BOARDS := $(ALL_BOARDS)
# FIXME: The INS build doesn't have a bootloader or bootloader
# updater yet so we need to filter them out to prevent errors.
BL_TARGETS := $(filter-out bl_ins, $(BL_TARGETS))
BU_TARGETS := $(filter-out bu_ins, $(BU_TARGETS))
BL_BOARDS := $(filter-out ins, $(ALL_BOARDS))
BU_BOARDS := $(filter-out ins, $(ALL_BOARDS))
# Generate the targets for whatever boards are left in each list
FW_TARGETS := $(addprefix fw_, $(FW_BOARDS))
BL_TARGETS := $(addprefix bl_, $(BL_BOARDS))
BU_TARGETS := $(addprefix bu_, $(BU_BOARDS))
.PHONY: all_fw all_fw_clean
all_fw: $(addsuffix _opfw, $(FW_TARGETS))
@ -428,6 +472,7 @@ all_bu_clean: $(addsuffix _clean, $(BU_TARGETS))
all_flight: all_fw all_bl all_bu
all_flight_clean: all_fw_clean all_bl_clean all_bu_clean
# Expand the groups of targets for each board
$(foreach board, $(ALL_BOARDS), $(eval $(call BOARD_PHONY_TEMPLATE,$(board))))
# Expand the bootloader updater rules

BIN
artwork/3D Model/multi/joes_cnc/CC.PNG Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 96 KiB

BIN
artwork/3D Model/multi/joes_cnc/J14-QT_+.3DS Normal file
View File

Binary file not shown.

BIN
artwork/3D Model/multi/joes_cnc/J14-QT_+.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 125 KiB

BIN
artwork/3D Model/multi/joes_cnc/J14-QT_X.3DS Normal file
View File

Binary file not shown.

BIN
artwork/3D Model/multi/joes_cnc/J14-QT_X.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 132 KiB

BIN
artwork/3D Model/multi/joes_cnc/J14-Q_+.3DS Normal file
View File

Binary file not shown.

BIN
artwork/3D Model/multi/joes_cnc/J14-Q_+.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 84 KiB

BIN
artwork/3D Model/multi/joes_cnc/J14-Q_X.3DS Normal file
View File

Binary file not shown.

BIN
artwork/3D Model/multi/joes_cnc/J14-Q_X.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 84 KiB

BIN
artwork/3D Model/multi/joes_cnc/TEXTURE.PNG Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 1.1 KiB

1329
artwork/Dials/deluxe/lineardial-horizontal.svg Normal file
View File

@ -0,0 +1,1329 @@
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
<!-- Created with Inkscape (http://www.inkscape.org/) -->
<svg
xmlns:dc="http://purl.org/dc/elements/1.1/"
xmlns:cc="http://creativecommons.org/ns#"
xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#"
xmlns:svg="http://www.w3.org/2000/svg"
xmlns="http://www.w3.org/2000/svg"
xmlns:xlink="http://www.w3.org/1999/xlink"
xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
width="280.76797"
height="70.597504"
id="svg10068"
version="1.1"
inkscape:version="0.48.1 "
sodipodi:docname="lineardial-horizontal.svg"
inkscape:export-filename="H:\Documents\Hobbies\W433\My Gauges\vbat-001.png"
inkscape:export-xdpi="103.61"
inkscape:export-ydpi="103.61"
style="display:inline">
<defs
id="defs10070">
<linearGradient
id="linearGradient4439">
<stop
style="stop-color:#1a1a1a;stop-opacity:1"
offset="0"
id="stop4441" />
<stop
id="stop4443"
offset="0.19742694"
style="stop-color:#808080;stop-opacity:1" />
<stop
style="stop-color:#000000;stop-opacity:1"
offset="1"
id="stop4445" />
</linearGradient>
<linearGradient
id="linearGradient4413">
<stop
id="stop4415"
offset="0"
style="stop-color:#1a1a1a;stop-opacity:1" />
<stop
style="stop-color:#4d4d4d;stop-opacity:1"
offset="0.19742694"
id="stop4417" />
<stop
id="stop4419"
offset="1"
style="stop-color:#000000;stop-opacity:1" />
</linearGradient>
<linearGradient
id="linearGradient4387">
<stop
id="stop4389"
offset="0"
style="stop-color:#4d4d4d;stop-opacity:1" />
<stop
style="stop-color:#000000;stop-opacity:1"
offset="0.60976541"
id="stop4391" />
<stop
id="stop4393"
offset="1"
style="stop-color:#4d4d4d;stop-opacity:1" />
</linearGradient>
<linearGradient
id="linearGradient4361">
<stop
style="stop-color:#4d4d4d;stop-opacity:1"
offset="0"
id="stop4363" />
<stop
id="stop4365"
offset="0.60976541"
style="stop-color:#000000;stop-opacity:1" />
<stop
style="stop-color:#4d4d4d;stop-opacity:1"
offset="1"
id="stop4367" />
</linearGradient>
<linearGradient
id="linearGradient4317">
<stop
id="stop4319"
offset="0"
style="stop-color:#4d4d4d;stop-opacity:1" />
<stop
style="stop-color:#000000;stop-opacity:1"
offset="0.60976541"
id="stop4321" />
<stop
id="stop4323"
offset="1"
style="stop-color:#b3b3b3;stop-opacity:1" />
</linearGradient>
<linearGradient
id="linearGradient4265">
<stop
style="stop-color:#333333;stop-opacity:1"
offset="0"
id="stop4267" />
<stop
id="stop4269"
offset="0.60976541"
style="stop-color:#000000;stop-opacity:1" />
<stop
style="stop-color:#333333;stop-opacity:1"
offset="1"
id="stop4271" />
</linearGradient>
<linearGradient
id="linearGradient4239">
<stop
id="stop4241"
offset="0"
style="stop-color:#000000;stop-opacity:1" />
<stop
style="stop-color:#808080;stop-opacity:1"
offset="0.60976541"
id="stop4243" />
<stop
id="stop4245"
offset="1"
style="stop-color:#000000;stop-opacity:1" />
</linearGradient>
<linearGradient
id="linearGradient4215">
<stop
style="stop-color:#1a1a1a;stop-opacity:1"
offset="0"
id="stop4217" />
<stop
id="stop4219"
offset="0.3051295"
style="stop-color:#808080;stop-opacity:1" />
<stop
style="stop-color:#000000;stop-opacity:1"
offset="1"
id="stop4221" />
</linearGradient>
<linearGradient
id="linearGradient4193">
<stop
id="stop4195"
offset="0"
style="stop-color:#1a1a1a;stop-opacity:1" />
<stop
style="stop-color:#999999;stop-opacity:1"
offset="0.3051295"
id="stop4197" />
<stop
id="stop4199"
offset="1"
style="stop-color:#000000;stop-opacity:1" />
</linearGradient>
<linearGradient
id="linearGradient4171">
<stop
style="stop-color:#1a1a1a;stop-opacity:1"
offset="0"
id="stop4173" />
<stop
id="stop4175"
offset="0.3051295"
style="stop-color:#666666;stop-opacity:1" />
<stop
style="stop-color:#1a1a1a;stop-opacity:1"
offset="1"
id="stop4177" />
</linearGradient>
<linearGradient
id="linearGradient4121">
<stop
id="stop4123"
offset="0"
style="stop-color:#1a1a1a;stop-opacity:1" />
<stop
style="stop-color:#666666;stop-opacity:1"
offset="0.2984421"
id="stop4125" />
<stop
id="stop4127"
offset="1"
style="stop-color:#1a1a1a;stop-opacity:1" />
</linearGradient>
<linearGradient
id="linearGradient4085">
<stop
style="stop-color:#1a1a1a;stop-opacity:1"
offset="0"
id="stop4087" />
<stop
id="stop4089"
offset="0.2984421"
style="stop-color:#999999;stop-opacity:1" />
<stop
style="stop-color:#1a1a1a;stop-opacity:1"
offset="1"
id="stop4091" />
</linearGradient>
<linearGradient
id="linearGradient4063">
<stop
id="stop4065"
offset="0"
style="stop-color:#1a1a1a;stop-opacity:1" />
<stop
style="stop-color:#b3b3b3;stop-opacity:1"
offset="0.39999998"
id="stop4067" />
<stop
id="stop4069"
offset="1"
style="stop-color:#1a1a1a;stop-opacity:1" />
</linearGradient>
<linearGradient
id="linearGradient4027">
<stop
style="stop-color:#000000;stop-opacity:1"
offset="0"
id="stop4029" />
<stop
id="stop4031"
offset="0.31880337"
style="stop-color:#b3b3b3;stop-opacity:1" />
<stop
style="stop-color:#000000;stop-opacity:1"
offset="1"
id="stop4033" />
</linearGradient>
<linearGradient
id="linearGradient3903">
<stop
id="stop3905"
offset="0"
style="stop-color:#000000;stop-opacity:1" />
<stop
style="stop-color:#ffffff;stop-opacity:1"
offset="0.3220683"
id="stop3907" />
<stop
id="stop3909"
offset="1"
style="stop-color:#000000;stop-opacity:1" />
</linearGradient>
<linearGradient
id="linearGradient5375">
<stop
style="stop-color:#00000c;stop-opacity:1"
offset="0"
id="stop5377" />
<stop
id="stop5383"
offset="0.34210527"
style="stop-color:#0044d8;stop-opacity:1" />
<stop
style="stop-color:#00000c;stop-opacity:1"
offset="1"
id="stop5379" />
</linearGradient>
<linearGradient
id="linearGradient5367">
<stop
style="stop-color:#000020;stop-opacity:1"
offset="0"
id="stop5369" />
<stop
id="stop5371"
offset="1"
style="stop-color:#000018;stop-opacity:1" />
</linearGradient>
<linearGradient
id="linearGradient5359">
<stop
style="stop-color:#00002c;stop-opacity:1"
offset="0"
id="stop5361" />
<stop
id="stop5363"
offset="0.32456139"
style="stop-color:#00ffff;stop-opacity:1" />
<stop
style="stop-color:#00c8fc;stop-opacity:1"
offset="1"
id="stop5365" />
</linearGradient>
<linearGradient
id="linearGradient5346">
<stop
id="stop5348"
offset="0"
style="stop-color:#1a1a1a;stop-opacity:1" />
<stop
style="stop-color:#666666;stop-opacity:1"
offset="0.37595931"
id="stop5350" />
<stop
id="stop5352"
offset="1"
style="stop-color:#1a1a1a;stop-opacity:1" />
</linearGradient>
<linearGradient
id="linearGradient5326">
<stop
style="stop-color:#000000;stop-opacity:1"
offset="0"
id="stop5328" />
<stop
id="stop5330"
offset="0.35277387"
style="stop-color:#4d4d4d;stop-opacity:1" />
<stop
style="stop-color:#000000;stop-opacity:1"
offset="1"
id="stop5332" />
</linearGradient>
<linearGradient
id="linearGradient5306">
<stop
id="stop5308"
offset="0"
style="stop-color:#000000;stop-opacity:1" />
<stop
style="stop-color:#808080;stop-opacity:1"
offset="0.35277387"
id="stop5310" />
<stop
id="stop5312"
offset="1"
style="stop-color:#000000;stop-opacity:1" />
</linearGradient>
<linearGradient
id="linearGradient5271">
<stop
id="stop5273"
offset="0"
style="stop-color:#000080;stop-opacity:1" />
<stop
style="stop-color:#00ffff;stop-opacity:1"
offset="0.32456139"
id="stop5275" />
<stop
id="stop5277"
offset="1"
style="stop-color:#000080;stop-opacity:1" />
</linearGradient>
<linearGradient
id="linearGradient5248">
<stop
style="stop-color:#000080;stop-opacity:1"
offset="0"
id="stop5250" />
<stop
id="stop5256"
offset="0.32456139"
style="stop-color:#00ffff;stop-opacity:1" />
<stop
style="stop-color:#000080;stop-opacity:1"
offset="1"
id="stop5252" />
</linearGradient>
<linearGradient
id="linearGradient5236">
<stop
style="stop-color:#000000;stop-opacity:1;"
offset="0"
id="stop5238" />
<stop
id="stop5244"
offset="0.5"
style="stop-color:#000000;stop-opacity:0.49803922;" />
<stop
style="stop-color:#000000;stop-opacity:0;"
offset="1"
id="stop5240" />
</linearGradient>
<linearGradient
id="linearGradient5219">
<stop
style="stop-color:#000000;stop-opacity:1"
offset="0"
id="stop5221" />
<stop
id="stop5223"
offset="0.35277387"
style="stop-color:#b3b3b3;stop-opacity:1" />
<stop
style="stop-color:#1a1a1a;stop-opacity:1"
offset="1"
id="stop5225" />
</linearGradient>
<linearGradient
id="linearGradient5151">
<stop
id="stop5153"
offset="0"
style="stop-color:#000000;stop-opacity:1" />
<stop
style="stop-color:#b3b3b3;stop-opacity:1"
offset="0.38512576"
id="stop5155" />
<stop
id="stop5157"
offset="1"
style="stop-color:#000000;stop-opacity:1" />
</linearGradient>
<linearGradient
id="linearGradient5129">
<stop
style="stop-color:#1a1a1a;stop-opacity:1"
offset="0"
id="stop5131" />
<stop
id="stop5137"
offset="0.38512576"
style="stop-color:#cccccc;stop-opacity:1" />
<stop
style="stop-color:#4d4d4d;stop-opacity:1"
offset="1"
id="stop5133" />
</linearGradient>
<linearGradient
id="linearGradient5116">
<stop
style="stop-color:#000000;stop-opacity:1"
offset="0"
id="stop5118" />
<stop
id="stop5124"
offset="0.35911319"
style="stop-color:#dcaf28;stop-opacity:1" />
<stop
style="stop-color:#000000;stop-opacity:1"
offset="1"
id="stop5120" />
</linearGradient>
<linearGradient
id="linearGradient5106">
<stop
style="stop-color:#000000;stop-opacity:1"
offset="0"
id="stop5108" />
<stop
id="stop5114"
offset="0.36023793"
style="stop-color:#00a000;stop-opacity:1" />
<stop
style="stop-color:#000000;stop-opacity:1"
offset="1"
id="stop5110" />
</linearGradient>
<linearGradient
id="linearGradient5096">
<stop
style="stop-color:#100000;stop-opacity:1"
offset="0"
id="stop5098" />
<stop
id="stop5104"
offset="0.36453304"
style="stop-color:#aa0000;stop-opacity:1" />
<stop
style="stop-color:#0c0000;stop-opacity:1"
offset="1"
id="stop5100" />
</linearGradient>
<marker
inkscape:stockid="Arrow2Sstart"
orient="auto"
refY="0"
refX="0"
id="Arrow2Sstart"
style="overflow:visible">
<path
id="path4640"
style="font-size:12px;fill-rule:evenodd;stroke-width:0.625;stroke-linejoin:round"
d="M 8.7185878,4.0337352 -2.2072895,0.01601326 8.7185884,-4.0017078 c -1.7454984,2.3720609 -1.7354408,5.6174519 -6e-7,8.035443 z"
transform="matrix(0.3,0,0,0.3,-0.69,0)"
inkscape:connector-curvature="0" />
</marker>
<inkscape:perspective
sodipodi:type="inkscape:persp3d"
inkscape:vp_x="0 : 526.18109 : 1"
inkscape:vp_y="0 : 1000 : 0"
inkscape:vp_z="744.09448 : 526.18109 : 1"
inkscape:persp3d-origin="372.04724 : 350.78739 : 1"
id="perspective10076" />
<inkscape:perspective
id="perspective9987"
inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
inkscape:vp_z="1 : 0.5 : 1"
inkscape:vp_y="0 : 1000 : 0"
inkscape:vp_x="0 : 0.5 : 1"
sodipodi:type="inkscape:persp3d" />
<inkscape:perspective
id="perspective10250"
inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
inkscape:vp_z="1 : 0.5 : 1"
inkscape:vp_y="0 : 1000 : 0"
inkscape:vp_x="0 : 0.5 : 1"
sodipodi:type="inkscape:persp3d" />
<inkscape:perspective
id="perspective10279"
inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
inkscape:vp_z="1 : 0.5 : 1"
inkscape:vp_y="0 : 1000 : 0"
inkscape:vp_x="0 : 0.5 : 1"
sodipodi:type="inkscape:persp3d" />
<inkscape:perspective
id="perspective10517"
inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
inkscape:vp_z="1 : 0.5 : 1"
inkscape:vp_y="0 : 1000 : 0"
inkscape:vp_x="0 : 0.5 : 1"
sodipodi:type="inkscape:persp3d" />
<inkscape:perspective
id="perspective11202"
inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
inkscape:vp_z="1 : 0.5 : 1"
inkscape:vp_y="0 : 1000 : 0"
inkscape:vp_x="0 : 0.5 : 1"
sodipodi:type="inkscape:persp3d" />
<inkscape:perspective
id="perspective3720"
inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
inkscape:vp_z="1 : 0.5 : 1"
inkscape:vp_y="0 : 1000 : 0"
inkscape:vp_x="0 : 0.5 : 1"
sodipodi:type="inkscape:persp3d" />
<inkscape:perspective
id="perspective3735"
inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
inkscape:vp_z="1 : 0.5 : 1"
inkscape:vp_y="0 : 1000 : 0"
inkscape:vp_x="0 : 0.5 : 1"
sodipodi:type="inkscape:persp3d" />
<inkscape:perspective
id="perspective3757"
inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
inkscape:vp_z="1 : 0.5 : 1"
inkscape:vp_y="0 : 1000 : 0"
inkscape:vp_x="0 : 0.5 : 1"
sodipodi:type="inkscape:persp3d" />
<inkscape:perspective
id="perspective3828"
inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
inkscape:vp_z="1 : 0.5 : 1"
inkscape:vp_y="0 : 1000 : 0"
inkscape:vp_x="0 : 0.5 : 1"
sodipodi:type="inkscape:persp3d" />
<inkscape:perspective
id="perspective3889"
inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
inkscape:vp_z="1 : 0.5 : 1"
inkscape:vp_y="0 : 1000 : 0"
inkscape:vp_x="0 : 0.5 : 1"
sodipodi:type="inkscape:persp3d" />
<inkscape:perspective
id="perspective3925"
inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
inkscape:vp_z="1 : 0.5 : 1"
inkscape:vp_y="0 : 1000 : 0"
inkscape:vp_x="0 : 0.5 : 1"
sodipodi:type="inkscape:persp3d" />
<inkscape:perspective
id="perspective3968"
inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
inkscape:vp_z="1 : 0.5 : 1"
inkscape:vp_y="0 : 1000 : 0"
inkscape:vp_x="0 : 0.5 : 1"
sodipodi:type="inkscape:persp3d" />
<inkscape:perspective
id="perspective3982"
inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
inkscape:vp_z="1 : 0.5 : 1"
inkscape:vp_y="0 : 1000 : 0"
inkscape:vp_x="0 : 0.5 : 1"
sodipodi:type="inkscape:persp3d" />
<inkscape:perspective
id="perspective4682"
inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
inkscape:vp_z="1 : 0.5 : 1"
inkscape:vp_y="0 : 1000 : 0"
inkscape:vp_x="0 : 0.5 : 1"
sodipodi:type="inkscape:persp3d" />
<inkscape:perspective
id="perspective3347"
inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
inkscape:vp_z="1 : 0.5 : 1"
inkscape:vp_y="0 : 1000 : 0"
inkscape:vp_x="0 : 0.5 : 1"
sodipodi:type="inkscape:persp3d" />
<inkscape:perspective
id="perspective5659"
inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
inkscape:vp_z="1 : 0.5 : 1"
inkscape:vp_y="0 : 1000 : 0"
inkscape:vp_x="0 : 0.5 : 1"
sodipodi:type="inkscape:persp3d" />
<inkscape:perspective
id="perspective5719"
inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
inkscape:vp_z="1 : 0.5 : 1"
inkscape:vp_y="0 : 1000 : 0"
inkscape:vp_x="0 : 0.5 : 1"
sodipodi:type="inkscape:persp3d" />
<inkscape:perspective
id="perspective5810"
inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
inkscape:vp_z="1 : 0.5 : 1"
inkscape:vp_y="0 : 1000 : 0"
inkscape:vp_x="0 : 0.5 : 1"
sodipodi:type="inkscape:persp3d" />
<inkscape:perspective
id="perspective3546"
inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
inkscape:vp_z="1 : 0.5 : 1"
inkscape:vp_y="0 : 1000 : 0"
inkscape:vp_x="0 : 0.5 : 1"
sodipodi:type="inkscape:persp3d" />
<inkscape:perspective
id="perspective4328"
inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
inkscape:vp_z="1 : 0.5 : 1"
inkscape:vp_y="0 : 1000 : 0"
inkscape:vp_x="0 : 0.5 : 1"
sodipodi:type="inkscape:persp3d" />
<linearGradient
inkscape:collect="always"
xlink:href="#linearGradient8928"
id="linearGradient5318"
gradientUnits="userSpaceOnUse"
gradientTransform="matrix(2.2532936,0,0,2.206809,446.89077,-285.33499)"
x1="-29.837337"
y1="285.0596"
x2="-29.837337"
y2="342.60553" />
<linearGradient
id="linearGradient8928">
<stop
style="stop-color:#ffffff;stop-opacity:0.89156628;"
offset="0"
id="stop8930" />
<stop
style="stop-color:#ffffff;stop-opacity:0;"
offset="1"
id="stop8932" />
</linearGradient>
<inkscape:perspective
id="perspective4367"
inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
inkscape:vp_z="1 : 0.5 : 1"
inkscape:vp_y="0 : 1000 : 0"
inkscape:vp_x="0 : 0.5 : 1"
sodipodi:type="inkscape:persp3d" />
<inkscape:perspective
id="perspective4402"
inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
inkscape:vp_z="1 : 0.5 : 1"
inkscape:vp_y="0 : 1000 : 0"
inkscape:vp_x="0 : 0.5 : 1"
sodipodi:type="inkscape:persp3d" />
<inkscape:perspective
id="perspective2940"
inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
inkscape:vp_z="1 : 0.5 : 1"
inkscape:vp_y="0 : 1000 : 0"
inkscape:vp_x="0 : 0.5 : 1"
sodipodi:type="inkscape:persp3d" />
<inkscape:perspective
id="perspective2864"
inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
inkscape:vp_z="1 : 0.5 : 1"
inkscape:vp_y="0 : 1000 : 0"
inkscape:vp_x="0 : 0.5 : 1"
sodipodi:type="inkscape:persp3d" />
<linearGradient
inkscape:collect="always"
xlink:href="#linearGradient5096"
id="linearGradient5102"
x1="117.41457"
y1="168.83261"
x2="87.074203"
y2="168.83261"
gradientUnits="userSpaceOnUse"
gradientTransform="matrix(0,0.99999995,-0.99999975,0,-11.23354,-270.8763)" />
<linearGradient
inkscape:collect="always"
xlink:href="#linearGradient5106"
id="linearGradient5112"
x1="117.57944"
y1="173.46591"
x2="86.644958"
y2="173.46591"
gradientUnits="userSpaceOnUse"
gradientTransform="matrix(0,0.99999997,-1,0,-11.23354,-270.8763)" />
<linearGradient
inkscape:collect="always"
xlink:href="#linearGradient5116"
id="linearGradient5122"
x1="117.77806"
y1="168.82289"
x2="86.547356"
y2="168.82289"
gradientUnits="userSpaceOnUse"
gradientTransform="matrix(0,1,-1,0,-11.23354,-270.8763)" />
<linearGradient
inkscape:collect="always"
xlink:href="#linearGradient5129"
id="linearGradient5135"
x1="-246.82069"
y1="353.9455"
x2="-246.82069"
y2="408.26157"
gradientUnits="userSpaceOnUse" />
<linearGradient
inkscape:collect="always"
xlink:href="#linearGradient5129"
id="linearGradient5144"
gradientUnits="userSpaceOnUse"
x1="-246.82069"
y1="353.9455"
x2="-246.82069"
y2="408.26157" />
<linearGradient
inkscape:collect="always"
xlink:href="#linearGradient5151"
id="linearGradient5149"
gradientUnits="userSpaceOnUse"
x1="-246.82069"
y1="353.9455"
x2="-246.82069"
y2="408.26157" />
<linearGradient
inkscape:collect="always"
xlink:href="#linearGradient5151"
id="linearGradient5164"
gradientUnits="userSpaceOnUse"
x1="-246.82069"
y1="353.9455"
x2="-246.82069"
y2="408.26157" />
<linearGradient
inkscape:collect="always"
xlink:href="#linearGradient5151"
id="linearGradient5169"
gradientUnits="userSpaceOnUse"
x1="-246.82069"
y1="353.9455"
x2="-246.82069"
y2="408.26157" />
<linearGradient
inkscape:collect="always"
xlink:href="#linearGradient5151"
id="linearGradient5176"
gradientUnits="userSpaceOnUse"
x1="-246.82069"
y1="353.9455"
x2="-246.82069"
y2="408.26157" />
<linearGradient
inkscape:collect="always"
xlink:href="#linearGradient5151"
id="linearGradient5181"
gradientUnits="userSpaceOnUse"
x1="-246.82069"
y1="353.9455"
x2="-246.82069"
y2="408.26157" />
<linearGradient
inkscape:collect="always"
xlink:href="#linearGradient5151"
id="linearGradient5188"
gradientUnits="userSpaceOnUse"
x1="-246.82069"
y1="353.9455"
x2="-246.82069"
y2="408.26157" />
<linearGradient
inkscape:collect="always"
xlink:href="#linearGradient5151"
id="linearGradient5193"
gradientUnits="userSpaceOnUse"
x1="-246.82069"
y1="353.9455"
x2="-246.82069"
y2="408.26157" />
<linearGradient
inkscape:collect="always"
xlink:href="#linearGradient5151"
id="linearGradient5200"
gradientUnits="userSpaceOnUse"
x1="-246.82069"
y1="353.9455"
x2="-246.82069"
y2="408.26157" />
<linearGradient
inkscape:collect="always"
xlink:href="#linearGradient5151"
id="linearGradient5205"
gradientUnits="userSpaceOnUse"
x1="-246.82069"
y1="353.9455"
x2="-246.82069"
y2="408.26157" />
<linearGradient
inkscape:collect="always"
xlink:href="#linearGradient5151"
id="linearGradient5212"
gradientUnits="userSpaceOnUse"
x1="-246.82069"
y1="353.9455"
x2="-246.82069"
y2="408.26157" />
<linearGradient
inkscape:collect="always"
xlink:href="#linearGradient5219"
id="linearGradient5217"
gradientUnits="userSpaceOnUse"
x1="-246.82069"
y1="353.9455"
x2="-247.44464"
y2="412.00528" />
<linearGradient
inkscape:collect="always"
xlink:href="#linearGradient5236"
id="linearGradient5242"
x1="3.7395172"
y1="59.899364"
x2="42.432911"
y2="59.899364"
gradientUnits="userSpaceOnUse"
gradientTransform="translate(129.62194,-129.52019)" />
<linearGradient
inkscape:collect="always"
xlink:href="#linearGradient5248"
id="linearGradient5254"
x1="-172.83463"
y1="74.562233"
x2="-137.26938"
y2="74.562233"
gradientUnits="userSpaceOnUse" />
<linearGradient
inkscape:collect="always"
xlink:href="#linearGradient5271"
id="linearGradient5285"
x1="-172.83463"
y1="74.562233"
x2="-137.26938"
y2="74.562233"
gradientUnits="userSpaceOnUse"
gradientTransform="matrix(1,0,0,0.58333333,-2.7039997e-8,32.705471)" />
<linearGradient
inkscape:collect="always"
xlink:href="#linearGradient5219"
id="linearGradient5287"
gradientUnits="userSpaceOnUse"
x1="-246.82069"
y1="353.9455"
x2="-247.44464"
y2="412.00528" />
<linearGradient
inkscape:collect="always"
xlink:href="#linearGradient5219"
id="linearGradient5292"
gradientUnits="userSpaceOnUse"
x1="-246.82069"
y1="353.9455"
x2="-247.44464"
y2="412.00528" />
<linearGradient
inkscape:collect="always"
xlink:href="#linearGradient5219"
id="linearGradient5299"
gradientUnits="userSpaceOnUse"
x1="-246.82069"
y1="353.9455"
x2="-247.44464"
y2="412.00528" />
<linearGradient
inkscape:collect="always"
xlink:href="#linearGradient5306"
id="linearGradient5304"
gradientUnits="userSpaceOnUse"
x1="-246.82069"
y1="353.9455"
x2="-247.44464"
y2="412.00528" />
<linearGradient
inkscape:collect="always"
xlink:href="#linearGradient5306"
id="linearGradient5319"
gradientUnits="userSpaceOnUse"
x1="-246.82069"
y1="353.9455"
x2="-247.44464"
y2="412.00528" />
<linearGradient
inkscape:collect="always"
xlink:href="#linearGradient5326"
id="linearGradient5324"
gradientUnits="userSpaceOnUse"
x1="-246.82069"
y1="353.9455"
x2="-247.44464"
y2="412.00528" />
<linearGradient
inkscape:collect="always"
xlink:href="#linearGradient5326"
id="linearGradient5339"
gradientUnits="userSpaceOnUse"
x1="-246.82069"
y1="353.9455"
x2="-247.44464"
y2="412.00528" />
<linearGradient
inkscape:collect="always"
xlink:href="#linearGradient5375"
id="linearGradient5381"
x1="-172.83463"
y1="74.562233"
x2="-137.26938"
y2="74.562233"
gradientUnits="userSpaceOnUse"
gradientTransform="translate(128.37404,-129.52019)" />
<linearGradient
inkscape:collect="always"
xlink:href="#linearGradient3903"
id="linearGradient3899"
gradientUnits="userSpaceOnUse"
gradientTransform="matrix(0,-0.99999988,1.7142859,0,35.373449,-317.43383)"
x1="-150.75359"
y1="68.860146"
x2="-150.75359"
y2="87.776077" />
<linearGradient
inkscape:collect="always"
xlink:href="#linearGradient4317"
id="linearGradient4337"
gradientUnits="userSpaceOnUse"
x1="-384.0242"
y1="-558.1579"
x2="-384.0242"
y2="-510.8558" />
<linearGradient
inkscape:collect="always"
xlink:href="#linearGradient4215"
id="linearGradient4340"
gradientUnits="userSpaceOnUse"
gradientTransform="matrix(0,0.99999998,-0.99999991,0,-11.233538,-270.87634)"
x1="-235.30438"
y1="369.2117"
x2="-299.45587"
y2="369.2117" />
<linearGradient
inkscape:collect="always"
xlink:href="#linearGradient4215"
id="linearGradient4349"
gradientUnits="userSpaceOnUse"
gradientTransform="matrix(0,0.99999998,-0.99999991,0,-11.233538,-270.87634)"
x1="-235.30438"
y1="369.2117"
x2="-299.45587"
y2="369.2117" />
<linearGradient
inkscape:collect="always"
xlink:href="#linearGradient4317"
id="linearGradient4351"
gradientUnits="userSpaceOnUse"
x1="-384.0242"
y1="-558.1579"
x2="-384.0242"
y2="-510.8558" />
<linearGradient
inkscape:collect="always"
xlink:href="#linearGradient4361"
id="linearGradient4355"
gradientUnits="userSpaceOnUse"
x1="-384.0242"
y1="-558.1579"
x2="-383.71014"
y2="-510.8558" />
<linearGradient
inkscape:collect="always"
xlink:href="#linearGradient4215"
id="linearGradient4358"
gradientUnits="userSpaceOnUse"
gradientTransform="matrix(0,0.99999998,-0.99999991,0,-11.233538,-270.87634)"
x1="-235.30438"
y1="369.2117"
x2="-299.45587"
y2="369.2117" />
<linearGradient
inkscape:collect="always"
xlink:href="#linearGradient4215"
id="linearGradient4375"
gradientUnits="userSpaceOnUse"
gradientTransform="matrix(0,0.99999998,-0.99999991,0,-11.233538,-270.87634)"
x1="-235.30438"
y1="369.2117"
x2="-299.45587"
y2="369.2117" />
<linearGradient
inkscape:collect="always"
xlink:href="#linearGradient4361"
id="linearGradient4377"
gradientUnits="userSpaceOnUse"
x1="-384.0242"
y1="-558.1579"
x2="-383.71014"
y2="-510.8558" />
<linearGradient
inkscape:collect="always"
xlink:href="#linearGradient4387"
id="linearGradient4381"
gradientUnits="userSpaceOnUse"
x1="-383.39609"
y1="-561.92657"
x2="-383.71014"
y2="-510.8558" />
<linearGradient
inkscape:collect="always"
xlink:href="#linearGradient4215"
id="linearGradient4384"
gradientUnits="userSpaceOnUse"
gradientTransform="matrix(0,0.99999998,-0.99999991,0,-11.233538,-270.87634)"
x1="-235.30438"
y1="369.2117"
x2="-299.45587"
y2="369.2117" />
<linearGradient
inkscape:collect="always"
xlink:href="#linearGradient4215"
id="linearGradient4401"
gradientUnits="userSpaceOnUse"
gradientTransform="matrix(0,0.99999998,-0.99999991,0,-11.233536,-270.87635)"
x1="-235.30438"
y1="369.2117"
x2="-299.45587"
y2="369.2117" />
<linearGradient
inkscape:collect="always"
xlink:href="#linearGradient4387"
id="linearGradient4403"
gradientUnits="userSpaceOnUse"
x1="-383.39609"
y1="-561.92657"
x2="-383.71014"
y2="-510.8558"
gradientTransform="translate(2.3925782e-6,-1.3232422e-5)" />
<linearGradient
inkscape:collect="always"
xlink:href="#linearGradient4387"
id="linearGradient4407"
gradientUnits="userSpaceOnUse"
gradientTransform="translate(2.3925782e-6,-1.3232422e-5)"
x1="-383.39609"
y1="-561.92657"
x2="-383.71014"
y2="-510.8558" />
<linearGradient
inkscape:collect="always"
xlink:href="#linearGradient4413"
id="linearGradient4410"
gradientUnits="userSpaceOnUse"
gradientTransform="matrix(0,0.99999998,-0.99999991,0,-11.233536,-270.87635)"
x1="-235.30438"
y1="369.2117"
x2="-299.45587"
y2="369.2117" />
<linearGradient
inkscape:collect="always"
xlink:href="#linearGradient4413"
id="linearGradient4427"
gradientUnits="userSpaceOnUse"
gradientTransform="matrix(0,0.99999998,-0.99999991,0,-11.233536,-270.87635)"
x1="-235.30438"
y1="369.2117"
x2="-299.45587"
y2="369.2117" />
<linearGradient
inkscape:collect="always"
xlink:href="#linearGradient4387"
id="linearGradient4429"
gradientUnits="userSpaceOnUse"
gradientTransform="translate(2.3925782e-6,-1.3232422e-5)"
x1="-383.39609"
y1="-561.92657"
x2="-383.71014"
y2="-510.8558" />
<linearGradient
inkscape:collect="always"
xlink:href="#linearGradient4387"
id="linearGradient4433"
gradientUnits="userSpaceOnUse"
gradientTransform="translate(2.3925782e-6,-1.3232422e-5)"
x1="-383.39609"
y1="-561.92657"
x2="-383.71014"
y2="-510.8558" />
<linearGradient
inkscape:collect="always"
xlink:href="#linearGradient4439"
id="linearGradient4436"
gradientUnits="userSpaceOnUse"
gradientTransform="matrix(0,0.99999998,-0.99999991,0,-11.233536,-270.87635)"
x1="-235.30438"
y1="369.2117"
x2="-299.45587"
y2="369.2117" />
</defs>
<sodipodi:namedview
id="base"
pagecolor="#ffffff"
bordercolor="#666666"
borderopacity="1.0"
inkscape:pageopacity="0.0"
inkscape:pageshadow="2"
inkscape:zoom="3.1841238"
inkscape:cx="140.38399"
inkscape:cy="35.455778"
inkscape:document-units="px"
inkscape:current-layer="layer5"
showgrid="false"
inkscape:window-width="1366"
inkscape:window-height="706"
inkscape:window-x="-8"
inkscape:window-y="-8"
inkscape:window-maximized="1"
inkscape:object-paths="true"
showguides="true"
inkscape:guide-bbox="true"
inkscape:snap-to-guides="false"
inkscape:snap-grids="false"
inkscape:snap-global="false"
fit-margin-top="0"
fit-margin-left="0"
fit-margin-right="0"
fit-margin-bottom="0" />
<metadata
id="metadata10073">
<rdf:RDF>
<cc:Work
rdf:about="">
<dc:format>image/svg+xml</dc:format>
<dc:type
rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
<dc:title></dc:title>
<dc:creator>
<cc:Agent>
<dc:title>Edouard Lafargue</dc:title>
</cc:Agent>
</dc:creator>
</cc:Work>
</rdf:RDF>
</metadata>
<g
style="display:inline"
inkscape:label="Dark background"
id="g2932"
inkscape:groupmode="layer"
transform="translate(-368.2988,-507.08981)"
sodipodi:insensitive="true">
<g
id="background"
inkscape:label="#g4447">
<rect
inkscape:export-ydpi="88.809998"
inkscape:export-xdpi="88.809998"
inkscape:export-filename="H:\Documents\Hobbies\W433\g9905.png"
ry="4.5346842"
y="-577.68732"
x="-616.67365"
height="61.448738"
width="226.5451"
id="rect3145"
style="fill:none;stroke:none"
transform="scale(-1,-1)" />
<rect
transform="scale(-1,-1)"
style="fill:url(#linearGradient4436);fill-opacity:1;stroke:none"
id="rect2936"
width="280.76797"
height="53.465179"
x="-649.06677"
y="-560.55499"
ry="3.9455285"
inkscape:export-filename="H:\Documents\Hobbies\W433\g9905.png"
inkscape:export-xdpi="88.809998"
inkscape:export-ydpi="88.809998" />
<rect
transform="scale(-1,-1)"
inkscape:label="#rect4388"
inkscape:export-ydpi="88.809998"
inkscape:export-xdpi="88.809998"
inkscape:export-filename="H:\Documents\Hobbies\W433\g9905.png"
ry="2.9134333"
y="-554.10626"
x="-642.96082"
height="39.479435"
width="267.02957"
id="bargraph-outer"
style="fill:url(#linearGradient4433);fill-opacity:1;stroke:none" />
<rect
transform="scale(-1,-1)"
style="fill:#000000;fill-opacity:1;stroke:none"
id="bargraph"
width="262.18338"
height="32.956219"
x="-640.57104"
y="-550.64001"
ry="2.432044"
inkscape:export-filename="H:\Documents\Hobbies\W433\g9905.png"
inkscape:export-xdpi="88.809998"
inkscape:export-ydpi="88.809998"
inkscape:label="#rect4388" />
</g>
</g>
<g
inkscape:groupmode="layer"
id="layer5"
inkscape:label="Green Zone"
style="display:inline"
transform="translate(-140.85549,-141.35611)"
sodipodi:insensitive="true">
<rect
inkscape:label="#rect5741"
style="fill:url(#linearGradient5112);fill-opacity:1;stroke:none;display:inline"
id="green"
width="260.53882"
height="32.20755"
x="-411.77084"
y="-184.00433"
ry="2.3767958"
inkscape:export-filename="H:\Documents\Hobbies\W433\g9905.png"
inkscape:export-xdpi="88.809998"
inkscape:export-ydpi="88.809998"
transform="scale(-1,-1)" />
</g>
<g
inkscape:groupmode="layer"
id="layer4"
inkscape:label="Yellow Zone"
style="display:none"
transform="translate(-140.85549,-141.35611)"
sodipodi:insensitive="true">
<rect
inkscape:export-ydpi="88.809998"
inkscape:export-xdpi="88.809998"
inkscape:export-filename="H:\Documents\Hobbies\W433\g9905.png"
ry="2.4231479"
y="-184.63248"
x="-411.45679"
height="32.835663"
width="260.53882"
id="yellow"
style="fill:url(#linearGradient5122);fill-opacity:1;stroke:none;display:inline"
inkscape:label="#rect5741"
transform="scale(-1,-1)" />
</g>
<g
inkscape:groupmode="layer"
id="layer3"
inkscape:label="Red zone"
style="display:none"
transform="translate(-140.85549,-141.35611)"
sodipodi:insensitive="true">
<rect
inkscape:label="#rect5741"
style="fill:url(#linearGradient5102);fill-opacity:1;stroke:none;display:inline"
id="red"
width="260.23901"
height="32.519711"
x="-411.38739"
y="-184.35484"
ry="2.399832"
inkscape:export-filename="H:\Documents\Hobbies\W433\g9905.png"
inkscape:export-xdpi="88.809998"
inkscape:export-ydpi="88.809998"
transform="scale(-1,-1)" />
</g>
<g
inkscape:groupmode="layer"
id="layer6"
inkscape:label="Indicator"
style="display:inline"
transform="translate(-140.85549,-141.35611)"
sodipodi:insensitive="true">
<rect
style="fill:url(#linearGradient3899);fill-opacity:1;stroke:#000000;stroke-width:0.2964696;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none"
id="needle"
width="32.035065"
height="6.0045304"
x="152.63882"
y="-157.12926"
inkscape:label="#rect5246"
transform="matrix(0,1,-1,0,0,0)" />
</g>
<g
inkscape:groupmode="layer"
id="layer1"
inkscape:label="Text"
style="display:inline"
transform="translate(-140.85549,-141.35611)"
sodipodi:insensitive="true">
<rect
style="fill:#ffffff;fill-opacity:1;stroke:none"
id="field"
width="50.321022"
height="13.236827"
x="-201.80493"
y="-209.93616"
inkscape:label="#rect2878"
transform="scale(-1,-1)" />
<rect
inkscape:label="#rect2878"
y="-209.5061"
x="-405.05276"
height="13.236827"
width="51.71294"
id="value"
style="fill:#ffffff;fill-opacity:1;stroke:none"
transform="scale(-1,-1)" />
</g>
</svg>
Side by Side

After

Width:  |  Height:  |  Size: 39 KiB

25
artwork/Dials/deluxe/lineardial-vertical.svg
View File

@ -30,7 +30,7 @@
style="stop-color:#000000;stop-opacity:1" />
<stop
style="stop-color:#ffffff;stop-opacity:1"
offset="0.32793573"
offset="0.39386007"
id="stop3907" />
<stop
id="stop3909"
@ -222,7 +222,7 @@
<stop
id="stop5124"
offset="0.37640449"
style="stop-color:#ffff00;stop-opacity:1" />
style="stop-color:#dcaf28;stop-opacity:1" />
<stop
style="stop-color:#000000;stop-opacity:1"
offset="1"
@ -236,8 +236,8 @@
id="stop5108" />
<stop
id="stop5114"
offset="0.33917606"
style="stop-color:#00ff00;stop-opacity:1" />
offset="0.38184431"
style="stop-color:#00a000;stop-opacity:1" />
<stop
style="stop-color:#000000;stop-opacity:1"
offset="1"
@ -252,7 +252,7 @@
<stop
id="stop5104"
offset="0.39717463"
style="stop-color:#ff2a2a;stop-opacity:1" />
style="stop-color:#aa0000;stop-opacity:1" />
<stop
style="stop-color:#0c0000;stop-opacity:1"
offset="1"
@ -740,9 +740,9 @@
borderopacity="1.0"
inkscape:pageopacity="0.0"
inkscape:pageshadow="2"
inkscape:zoom="7.9373969"
inkscape:cx="10.296503"
inkscape:cy="298.23206"
inkscape:zoom="1.9843492"
inkscape:cx="13.888309"
inkscape:cy="302.67927"
inkscape:document-units="px"
inkscape:current-layer="layer6"
showgrid="false"
@ -849,7 +849,7 @@
inkscape:groupmode="layer"
id="layer4"
inkscape:label="Yellow Zone"
style="display:inline"
style="display:none"
transform="translate(-129.62194,129.52019)"
sodipodi:insensitive="true">
<rect
@ -891,11 +891,10 @@
inkscape:groupmode="layer"
id="layer6"
inkscape:label="Indicator"
style="display:none"
transform="translate(-129.62194,129.52019)"
sodipodi:insensitive="true">
style="display:inline"
transform="translate(-129.62194,129.52019)">
<rect
style="fill:url(#linearGradient3899);fill-opacity:1;stroke:#000000;stroke-width:0.30000001;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none"
style="fill:url(#linearGradient3899);fill-opacity:1;stroke:#000000;stroke-width:0.30000000999999998;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none"
id="needle"
width="33.162418"
height="5.9393759"

Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 28 KiB

After

Width:  |  Height:  |  Size: 28 KiB

35
artwork/GCS Icons/dialog-warning.svg Normal file
View File

@ -0,0 +1,35 @@
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
<!-- Created with Inkscape (http://www.inkscape.org/) -->
<svg id="svg3247" xmlns="http://www.w3.org/2000/svg" height="48" width="48" version="1.0" xmlns:xlink="http://www.w3.org/1999/xlink">
<defs id="defs3249">
<linearGradient id="linearGradient2411" y2="5.4676" gradientUnits="userSpaceOnUse" x2="63.397" gradientTransform="matrix(2.1154 0 0 2.1153 -107.58 32.427)" y1="-12.489" x1="63.397">
<stop id="stop4875" style="stop-color:#fff" offset="0"/>
<stop id="stop4877" style="stop-color:#fff;stop-opacity:0" offset="1"/>
</linearGradient>
<linearGradient id="linearGradient2416" y2="3.0816" gradientUnits="userSpaceOnUse" x2="18.379" gradientTransform="matrix(.95844 0 0 .95844 .99752 1.9975)" y1="44.98" x1="18.379">
<stop id="stop2492" style="stop-color:#791235" offset="0"/>
<stop id="stop2494" style="stop-color:#dd3b27" offset="1"/>
</linearGradient>
<radialGradient id="radialGradient2414" gradientUnits="userSpaceOnUse" cy="3.99" cx="23.896" gradientTransform="matrix(0 2.2875 -3.0194 0 36.047 -50.63)" r="20.397">
<stop id="stop3244" style="stop-color:#f8b17e" offset="0"/>
<stop id="stop3246" style="stop-color:#e35d4f" offset=".26238"/>
<stop id="stop3248" style="stop-color:#c6262e" offset=".66094"/>
<stop id="stop3250" style="stop-color:#690b54" offset="1"/>
</radialGradient>
<radialGradient id="radialGradient2419" gradientUnits="userSpaceOnUse" cy="4.625" cx="62.625" gradientTransform="matrix(2.1647 0 0 .75294 -111.56 36.518)" r="10.625">
<stop id="stop8840" offset="0"/>
<stop id="stop8842" style="stop-opacity:0" offset="1"/>
</radialGradient>
</defs>
<g id="layer1">
<g id="g3275">
<path id="path8836" style="opacity:.3;fill-rule:evenodd;fill:url(#radialGradient2419)" d="m47 40c0 4.418-10.297 8-23 8s-23-3.582-23-8 10.297-8 23-8 23 3.582 23 8z"/>
<path id="path2555" style="stroke-linejoin:round;stroke:url(#linearGradient2416);stroke-linecap:round;stroke-width:1.0037;fill:url(#radialGradient2414)" d="m24 5.5018c-10.758 0-19.498 8.7402-19.498 19.498-0.0002 10.758 8.74 19.498 19.498 19.498s19.498-8.74 19.498-19.498-8.74-19.498-19.498-19.498z"/>
<path id="path8655" style="opacity:.4;stroke:url(#linearGradient2411);fill:none" d="m42.5 24.999c0 10.218-8.283 18.501-18.5 18.501s-18.5-8.283-18.5-18.501c0-10.217 8.283-18.499 18.5-18.499s18.5 8.282 18.5 18.499z"/>
</g>
<g id="g3243" transform="translate(51.075 .56862)">
<path id="path3295" style="opacity:.2" d="m-29.451 12.554c0.563 5.5 1.208 10.961 1.687 16.482h1.53c0.397-5.302 1.038-10.571 1.501-15.867 0.236-1.254-0.408-2.742-1.732-3.047-1.308-0.3824-2.77 0.565-2.944 1.918-0.029 0.17-0.042 0.342-0.042 0.514zm-0.167 22.359c-0.059 1.637 1.742 2.92 3.28 2.401 1.489-0.38 2.274-2.252 1.51-3.583-0.683-1.375-2.687-1.829-3.84-0.776-0.582 0.479-0.968 1.194-0.95 1.958z"/>
<path id="text2315" style="fill:#fff" d="m-29.451 13.555c0.563 5.499 1.208 10.96 1.687 16.481h1.53c0.397-5.301 1.038-10.571 1.501-15.866 0.236-1.254-0.408-2.743-1.732-3.048-1.308-0.382-2.77 0.565-2.944 1.918-0.029 0.17-0.042 0.342-0.042 0.515zm-0.167 22.358c-0.059 1.637 1.742 2.921 3.28 2.402 1.489-0.381 2.274-2.253 1.51-3.584-0.683-1.375-2.687-1.828-3.84-0.776-0.582 0.479-0.968 1.194-0.95 1.958z"/>
</g>
</g>
</svg>
Side by Side

After

Width:  |  Height:  |  Size: 3.2 KiB

44
flight/AHRS/pios_board.c
View File

@ -54,7 +54,6 @@ static const struct pios_spi_cfg pios_spi_op_cfg = {
.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),
@ -153,11 +152,10 @@ void PIOS_SPI_op_irq_handler(void)
extern void PIOS_ADC_handler(void);
void DMA1_Channel1_IRQHandler() __attribute__ ((alias("PIOS_ADC_handler")));
// Remap the ADC DMA handler to this one
const struct pios_adc_cfg pios_adc_cfg = {
static const struct pios_adc_cfg pios_adc_cfg = {
.dma = {
.ahb_clk = RCC_AHBPeriph_DMA1,
.irq = {
.handler = PIOS_ADC_DMA_Handler,
.flags = (DMA1_FLAG_TC1 | DMA1_FLAG_TE1 | DMA1_FLAG_HT1 | DMA1_FLAG_GL1),
.init = {
.NVIC_IRQChannel = DMA1_Channel1_IRQn,
@ -205,17 +203,10 @@ void PIOS_ADC_handler() {
/*
* AUX USART
*/
void PIOS_USART_aux_irq_handler(void);
void USART3_IRQHandler()
__attribute__ ((alias("PIOS_USART_aux_irq_handler")));
const struct pios_usart_cfg pios_usart_aux_cfg = {
static const struct pios_usart_cfg pios_usart_aux_cfg = {
.regs = USART3,
.init = {
#if defined (PIOS_USART_BAUDRATE)
.USART_BaudRate = PIOS_USART_BAUDRATE,
#else
.USART_BaudRate = 57600,
#endif
.USART_BaudRate = 230400,
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1,
@ -224,7 +215,6 @@ const struct pios_usart_cfg pios_usart_aux_cfg = {
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
},
.irq = {
.handler = PIOS_USART_aux_irq_handler,
.init = {
.NVIC_IRQChannel = USART3_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
@ -250,18 +240,15 @@ const struct pios_usart_cfg pios_usart_aux_cfg = {
},
};
static uint32_t pios_usart_aux_id;
void PIOS_USART_aux_irq_handler(void)
{
PIOS_USART_IRQ_Handler(pios_usart_aux_id);
}
#endif /* PIOS_INCLUDE_USART */
#if defined(PIOS_INCLUDE_COM)
#include <pios_com_priv.h>
#define PIOS_COM_AUX_TX_BUF_LEN 192
static uint8_t pios_com_aux_tx_buffer[PIOS_COM_AUX_TX_BUF_LEN];
#endif /* PIOS_INCLUDE_COM */
#if defined(PIOS_INCLUDE_I2C)
@ -279,7 +266,7 @@ void I2C1_EV_IRQHandler()
void I2C1_ER_IRQHandler()
__attribute__ ((alias("PIOS_I2C_main_adapter_er_irq_handler")));
const struct pios_i2c_adapter_cfg pios_i2c_main_adapter_cfg = {
static const struct pios_i2c_adapter_cfg pios_i2c_main_adapter_cfg = {
.regs = I2C1,
.init = {
.I2C_Mode = I2C_Mode_I2C,
@ -307,7 +294,6 @@ const struct pios_i2c_adapter_cfg pios_i2c_main_adapter_cfg = {
},
},
.event = {
.handler = PIOS_I2C_main_adapter_ev_irq_handler,
.flags = 0, /* FIXME: check this */
.init = {
.NVIC_IRQChannel = I2C1_EV_IRQn,
@ -317,7 +303,6 @@ const struct pios_i2c_adapter_cfg pios_i2c_main_adapter_cfg = {
},
},
.error = {
.handler = PIOS_I2C_main_adapter_er_irq_handler,
.flags = 0, /* FIXME: check this */
.init = {
.NVIC_IRQChannel = I2C1_ER_IRQn,
@ -388,11 +373,16 @@ void PIOS_Board_Init(void) {
/* Communication system */
#if !defined(PIOS_ENABLE_DEBUG_PINS)
#if defined(PIOS_INCLUDE_COM)
if (PIOS_USART_Init(&pios_usart_aux_id, &pios_usart_aux_cfg)) {
PIOS_DEBUG_Assert(0);
}
if (PIOS_COM_Init(&pios_com_aux_id, &pios_usart_com_driver, pios_usart_aux_id)) {
PIOS_DEBUG_Assert(0);
{
uint32_t pios_usart_aux_id;
if (PIOS_USART_Init(&pios_usart_aux_id, &pios_usart_aux_cfg)) {
PIOS_DEBUG_Assert(0);
}
if (PIOS_COM_Init(&pios_com_aux_id, &pios_usart_com_driver, pios_usart_aux_id,
NULL, 0,
pios_com_aux_tx_buffer, sizeof(pios_com_aux_tx_buffer))) {
PIOS_DEBUG_Assert(0);
}
}
#endif /* PIOS_INCLUDE_COM */
#endif

8
flight/Bootloaders/AHRS/pios_board.c
View File

@ -55,7 +55,6 @@ static const struct pios_spi_cfg
.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,
@ -133,11 +132,18 @@ void PIOS_SPI_op_irq_handler(void) {
#include "bl_fsm.h" /* lfsm_* */
static bool board_init_complete = false;
void PIOS_Board_Init() {
if (board_init_complete) {
return;
}
/* 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();
board_init_complete = true;
}

3
flight/Bootloaders/CopterControl/Makefile
View File

@ -94,8 +94,6 @@ DOXYGENDIR = ../Doc/Doxygen
SRC += $(OPSYSTEM)/main.c
SRC += $(OPSYSTEM)/pios_board.c
SRC += $(OPSYSTEM)/op_dfu.c
SRC += $(FLIGHTLIB)/stopwatch.c
## PIOS Hardware (STM32F10x)
SRC += $(PIOSSTM32F10X)/pios_sys.c
@ -139,7 +137,6 @@ SRC += $(STMSPDSRCDIR)/stm32f10x_pwr.c
SRC += $(STMSPDSRCDIR)/stm32f10x_rcc.c
SRC += $(STMSPDSRCDIR)/stm32f10x_rtc.c
SRC += $(STMSPDSRCDIR)/stm32f10x_spi.c
SRC += $(STMSPDSRCDIR)/stm32f10x_tim.c
SRC += $(STMSPDSRCDIR)/stm32f10x_usart.c
SRC += $(STMSPDSRCDIR)/stm32f10x_dbgmcu.c
SRC += $(STMSPDSRCDIR)/misc.c

55
flight/Bootloaders/CopterControl/main.c
View File

@ -28,7 +28,6 @@
/* Bootloader Includes */
#include <pios.h>
#include <pios_board_info.h>
#include "stopwatch.h"
#include "op_dfu.h"
#include "usb_lib.h"
#include "pios_iap.h"
@ -47,9 +46,9 @@ pFunction Jump_To_Application;
uint32_t JumpAddress;
/// LEDs PWM
uint32_t period1 = 50; // *100 uS -> 5 mS
uint32_t period1 = 5000; // 5 mS
uint32_t sweep_steps1 = 100; // * 5 mS -> 500 mS
uint32_t period2 = 50; // *100 uS -> 5 mS
uint32_t period2 = 5000; // 5 mS
uint32_t sweep_steps2 = 100; // * 5 mS -> 500 mS
@ -70,7 +69,6 @@ uint8_t processRX();
void jump_to_app();
#define BLUE LED1
#define LED_PWM_TIMER TIM1
int main() {
PIOS_SYS_Init();
if (BSL_HOLD_STATE == 0)
@ -93,13 +91,17 @@ int main() {
DeviceState = DFUidle;
else
DeviceState = BLidle;
STOPWATCH_Init(100, LED_PWM_TIMER);
} else
JumpToApp = TRUE;
STOPWATCH_Reset(LED_PWM_TIMER);
uint32_t stopwatch = 0;
uint32_t prev_ticks = PIOS_DELAY_GetuS();
while (TRUE) {
/* Update the stopwatch */
uint32_t elapsed_ticks = PIOS_DELAY_GetuSSince(prev_ticks);
prev_ticks += elapsed_ticks;
stopwatch += elapsed_ticks;
if (JumpToApp == TRUE)
jump_to_app();
@ -107,19 +109,19 @@ int main() {
case Last_operation_Success:
case uploadingStarting:
case DFUidle:
period1 = 50;
period1 = 5000;
sweep_steps1 = 100;
PIOS_LED_Off(BLUE);
period2 = 0;
break;
case uploading:
period1 = 50;
period1 = 5000;
sweep_steps1 = 100;
period2 = 25;
period2 = 2500;
sweep_steps2 = 50;
break;
case downloading:
period1 = 25;
period1 = 2500;
sweep_steps1 = 50;
PIOS_LED_Off(BLUE);
period2 = 0;
@ -130,14 +132,14 @@ int main() {
period2 = 0;
break;
default://error
period1 = 50;
period1 = 5000;
sweep_steps1 = 100;
period2 = 50;
period2 = 5000;
sweep_steps2 = 100;
}
if (period1 != 0) {
if (LedPWM(period1, sweep_steps1, STOPWATCH_ValueGet(LED_PWM_TIMER)))
if (LedPWM(period1, sweep_steps1, stopwatch))
PIOS_LED_On(BLUE);
else
PIOS_LED_Off(BLUE);
@ -145,16 +147,16 @@ int main() {
PIOS_LED_On(BLUE);
if (period2 != 0) {
if (LedPWM(period2, sweep_steps2, STOPWATCH_ValueGet(LED_PWM_TIMER)))
if (LedPWM(period2, sweep_steps2, stopwatch))
PIOS_LED_On(BLUE);
else
PIOS_LED_Off(BLUE);
} else
PIOS_LED_Off(BLUE);
if (STOPWATCH_ValueGet(LED_PWM_TIMER) > 100 * 50 * 100)
STOPWATCH_Reset(LED_PWM_TIMER);
if ((STOPWATCH_ValueGet(LED_PWM_TIMER) > 60000) && (DeviceState
if (stopwatch > 50 * 1000 * 1000)
stopwatch = 0;
if ((stopwatch > 6 * 1000 * 1000) && (DeviceState
== BLidle))
JumpToApp = TRUE;
@ -185,20 +187,21 @@ void jump_to_app() {
}
}
uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count) {
uint32_t pwm_duty = ((count / pwm_period) % pwm_sweep_steps)
/ (pwm_sweep_steps / pwm_period);
if ((count % (2 * pwm_period * pwm_sweep_steps)) > pwm_period
* pwm_sweep_steps)
pwm_duty = pwm_period - pwm_duty; // negative direction each 50*100 ticks
uint32_t curr_step = (count / pwm_period) % pwm_sweep_steps; /* 0 - pwm_sweep_steps */
uint32_t pwm_duty = pwm_period * curr_step / pwm_sweep_steps; /* fraction of pwm_period */
uint32_t curr_sweep = (count / (pwm_period * pwm_sweep_steps)); /* ticks once per full sweep */
if (curr_sweep & 1) {
pwm_duty = pwm_period - pwm_duty; /* reverse direction in odd sweeps */
}
return ((count % pwm_period) > pwm_duty) ? 1 : 0;
}
uint8_t processRX() {
while (PIOS_COM_ReceiveBufferUsed(PIOS_COM_TELEM_USB) >= 63) {
for (int32_t x = 0; x < 63; ++x) {
mReceive_Buffer[x] = PIOS_COM_ReceiveBuffer(PIOS_COM_TELEM_USB);
if (PIOS_COM_ReceiveBuffer(PIOS_COM_TELEM_USB, mReceive_Buffer, 63, 0) == 63) {
processComand(mReceive_Buffer);
}
processComand(mReceive_Buffer);
}
return TRUE;
}

41
flight/Bootloaders/CopterControl/pios_board.c
View File

@ -31,10 +31,31 @@
#include <pios_com_priv.h>
#define PIOS_COM_TELEM_USB_RX_BUF_LEN 192
#define PIOS_COM_TELEM_USB_TX_BUF_LEN 192
static uint8_t pios_com_telem_usb_rx_buffer[PIOS_COM_TELEM_USB_RX_BUF_LEN];
static uint8_t pios_com_telem_usb_tx_buffer[PIOS_COM_TELEM_USB_TX_BUF_LEN];
#endif /* PIOS_INCLUDE_COM */
// ***********************************************************************************
#if defined(PIOS_INCLUDE_USB_HID)
#include "pios_usb_hid_priv.h"
static const struct pios_usb_hid_cfg pios_usb_hid_main_cfg = {
.irq = {
.init = {
.NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
};
#endif /* PIOS_INCLUDE_USB_HID */
extern const struct pios_com_driver pios_usb_com_driver;
uint32_t pios_com_telem_usb_id;
@ -44,7 +65,12 @@ uint32_t pios_com_telem_usb_id;
* initializes all the core subsystems on this specific hardware
* called from System/openpilot.c
*/
static bool board_init_complete = false;
void PIOS_Board_Init(void) {
if (board_init_complete) {
return;
}
/* Enable Prefetch Buffer */
FLASH_PrefetchBufferCmd(FLASH_PrefetchBuffer_Enable);
@ -58,13 +84,20 @@ void PIOS_Board_Init(void) {
PIOS_GPIO_Init();
#if defined(PIOS_INCLUDE_USB_HID)
PIOS_USB_HID_Init(0);
uint32_t pios_usb_hid_id;
if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_main_cfg)) {
PIOS_Assert(0);
}
#if defined(PIOS_INCLUDE_COM)
if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_com_driver, 0)) {
PIOS_DEBUG_Assert(0);
if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_com_driver, pios_usb_hid_id,
pios_com_telem_usb_rx_buffer, sizeof(pios_com_telem_usb_rx_buffer),
pios_com_telem_usb_tx_buffer, sizeof(pios_com_telem_usb_tx_buffer))) {
PIOS_Assert(0);
}
#endif /* PIOS_INCLUDE_COM */
#endif /* PIOS_INCLUDE_USB_HID */
#endif /* PIOS_INCLUDE_USB_HID */
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE);//TODO Tirar
board_init_complete = true;
}

12
flight/Bootloaders/OpenPilot/main.c
View File

@ -250,10 +250,9 @@ uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count) {
uint8_t processRX() {
if (ProgPort == Usb) {
while (PIOS_COM_ReceiveBufferUsed(PIOS_COM_TELEM_USB) >= 63) {
for (int32_t x = 0; x < 63; ++x) {
mReceive_Buffer[x] = PIOS_COM_ReceiveBuffer(PIOS_COM_TELEM_USB);
if (PIOS_COM_ReceiveBuffer(PIOS_COM_TELEM_USB, mReceive_Buffer, 63, 0) == 63) {
processComand(mReceive_Buffer);
}
processComand(mReceive_Buffer);
}
} else if (ProgPort == Serial) {
@ -279,7 +278,12 @@ void SSP_CallBack(uint8_t *buf, uint16_t len) {
}
int16_t SSP_SerialRead(void) {
if (PIOS_COM_ReceiveBufferUsed(PIOS_COM_TELEM_RF) > 0) {
return PIOS_COM_ReceiveBuffer(PIOS_COM_TELEM_RF);
uint8_t byte;
if (PIOS_COM_ReceiveBuffer(PIOS_COM_TELEM_RF, &byte, 1, 0) == 1) {
return byte;
} else {
return -1;
}
} else
return -1;
}

62
flight/Bootloaders/OpenPilot/pios_board.c
View File

@ -60,7 +60,6 @@ const struct pios_spi_cfg
.ahb_clk = RCC_AHBPeriph_DMA1,
.irq = {
.handler = PIOS_SPI_ahrs_irq_handler,
.flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4),
.init = {
.NVIC_IRQChannel = DMA1_Channel4_IRQn,
@ -143,9 +142,9 @@ void PIOS_SPI_ahrs_irq_handler(void) {
/*
* Telemetry USART
*/
void PIOS_USART_telem_irq_handler(void);
void USART2_IRQHandler() __attribute__ ((alias ("PIOS_USART_telem_irq_handler")));
const struct pios_usart_cfg pios_usart_telem_cfg = { .regs = USART2, .init = {
const struct pios_usart_cfg pios_usart_telem_cfg = {
.regs = USART2,
.init = {
#if defined (PIOS_COM_TELEM_BAUDRATE)
.USART_BaudRate = PIOS_COM_TELEM_BAUDRATE,
#else
@ -157,7 +156,6 @@ const struct pios_usart_cfg pios_usart_telem_cfg = { .regs = USART2, .init = {
.USART_HardwareFlowControl = USART_HardwareFlowControl_None,
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
}, .irq = {
.handler = PIOS_USART_telem_irq_handler,
.init = {
.NVIC_IRQChannel = USART2_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
@ -180,19 +178,41 @@ const struct pios_usart_cfg pios_usart_telem_cfg = { .regs = USART2, .init = {
},
}, };
static uint32_t pios_usart_telem_rf_id;
void PIOS_USART_telem_irq_handler(void) {
PIOS_USART_IRQ_Handler(pios_usart_telem_rf_id);
}
#endif /* PIOS_INCLUDE_USART */
#if defined(PIOS_INCLUDE_COM)
#include "pios_com_priv.h"
#define PIOS_COM_TELEM_USB_RX_BUF_LEN 192
#define PIOS_COM_TELEM_USB_TX_BUF_LEN 192
static uint8_t pios_com_telem_usb_rx_buffer[PIOS_COM_TELEM_USB_RX_BUF_LEN];
static uint8_t pios_com_telem_usb_tx_buffer[PIOS_COM_TELEM_USB_TX_BUF_LEN];
#define PIOS_COM_TELEM_RF_RX_BUF_LEN 192
#define PIOS_COM_TELEM_RF_TX_BUF_LEN 192
static uint8_t pios_com_telem_rf_rx_buffer[PIOS_COM_TELEM_RF_RX_BUF_LEN];
static uint8_t pios_com_telem_rf_tx_buffer[PIOS_COM_TELEM_RF_TX_BUF_LEN];
#endif /* PIOS_INCLUDE_COM */
#if defined(PIOS_INCLUDE_USB_HID)
#include "pios_usb_hid_priv.h"
static const struct pios_usb_hid_cfg pios_usb_hid_main_cfg = {
.irq = {
.init = {
.NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
};
#endif /* PIOS_INCLUDE_USB_HID */
extern const struct pios_com_driver pios_usb_com_driver;
uint32_t pios_com_telem_rf_id;
@ -205,7 +225,11 @@ uint32_t pios_com_telem_usb_id;
* initializes all the core subsystems on this specific hardware
* called from System/openpilot.c
*/
static bool board_init_complete = false;
void PIOS_Board_Init(void) {
if (board_init_complete) {
return;
}
/* Enable Prefetch Buffer */
FLASH_PrefetchBufferCmd(FLASH_PrefetchBuffer_Enable);
@ -218,11 +242,14 @@ void PIOS_Board_Init(void) {
/* Initialize the PiOS library */
#if defined(PIOS_INCLUDE_COM)
uint32_t pios_usart_telem_rf_id;
if (PIOS_USART_Init(&pios_usart_telem_rf_id, &pios_usart_telem_cfg)) {
PIOS_DEBUG_Assert(0);
}
if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver,
pios_usart_telem_rf_id)) {
pios_usart_telem_rf_id,
pios_com_telem_rf_rx_buffer, sizeof(pios_com_telem_rf_rx_buffer),
pios_com_telem_rf_tx_buffer, sizeof(pios_com_telem_rf_tx_buffer))) {
PIOS_DEBUG_Assert(0);
}
#endif /* PIOS_INCLUDE_COM */
@ -230,13 +257,16 @@ void PIOS_Board_Init(void) {
PIOS_GPIO_Init();
#if defined(PIOS_INCLUDE_USB_HID)
PIOS_USB_HID_Init(0);
uint32_t pios_usb_hid_id;
PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_main_cfg);
#if defined(PIOS_INCLUDE_COM)
if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_com_driver, 0)) {
PIOS_DEBUG_Assert(0);
if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_com_driver, pios_usb_hid_id,
pios_com_telem_usb_rx_buffer, sizeof(pios_com_telem_usb_rx_buffer),
pios_com_telem_usb_tx_buffer, sizeof(pios_com_telem_usb_tx_buffer))) {
PIOS_Assert(0);
}
#endif /* PIOS_INCLUDE_COM */
#endif /* PIOS_INCLUDE_USB_HID */
#endif /* PIOS_INCLUDE_USB_HID */
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE);//TODO Tirar
@ -247,6 +277,8 @@ void PIOS_Board_Init(void) {
/* Bind the AHRS comms layer to the AHRS SPI link */
PIOS_OPAHRS_Attach(pios_spi_ahrs_id);
board_init_complete = true;
}
/**

3
flight/Bootloaders/PipXtreme/Makefile
View File

@ -94,8 +94,6 @@ DOXYGENDIR = ../Doc/Doxygen
SRC += $(OPSYSTEM)/main.c
SRC += $(OPSYSTEM)/pios_board.c
SRC += $(OPSYSTEM)/op_dfu.c
SRC += $(FLIGHTLIB)/stopwatch.c
## PIOS Hardware (STM32F10x)
SRC += $(PIOSSTM32F10X)/pios_sys.c
@ -138,7 +136,6 @@ SRC += $(STMSPDSRCDIR)/stm32f10x_pwr.c
SRC += $(STMSPDSRCDIR)/stm32f10x_rcc.c
SRC += $(STMSPDSRCDIR)/stm32f10x_rtc.c
SRC += $(STMSPDSRCDIR)/stm32f10x_spi.c
SRC += $(STMSPDSRCDIR)/stm32f10x_tim.c
SRC += $(STMSPDSRCDIR)/stm32f10x_usart.c
SRC += $(STMSPDSRCDIR)/stm32f10x_dbgmcu.c
SRC += $(STMSPDSRCDIR)/misc.c

57
flight/Bootloaders/PipXtreme/main.c
View File

@ -28,7 +28,6 @@
/* Bootloader Includes */
#include <pios.h>
#include <pios_board_info.h>
#include "stopwatch.h"
#include "op_dfu.h"
#include "usb_lib.h"
#include "pios_iap.h"
@ -47,9 +46,9 @@ pFunction Jump_To_Application;
uint32_t JumpAddress;
/// LEDs PWM
uint32_t period1 = 50; // *100 uS -> 5 mS
uint32_t period1 = 5000; // 5 mS
uint32_t sweep_steps1 = 100; // * 5 mS -> 500 mS
uint32_t period2 = 50; // *100 uS -> 5 mS
uint32_t period2 = 5000; // 5 mS
uint32_t sweep_steps2 = 100; // * 5 mS -> 500 mS
@ -71,7 +70,6 @@ void jump_to_app();
#define BLUE LED1
#define RED LED4
#define LED_PWM_TIMER TIM3
int main() {
/* NOTE: Do NOT modify the following start-up sequence */
/* Any new initialization functions should be added in OpenPilotInit() */
@ -98,35 +96,37 @@ int main() {
DeviceState = DFUidle;
else
DeviceState = BLidle;
STOPWATCH_Init(100, LED_PWM_TIMER);
} else
JumpToApp = TRUE;
STOPWATCH_Reset(LED_PWM_TIMER);
uint32_t stopwatch = 0;
uint32_t prev_ticks = PIOS_DELAY_GetuS();
while (TRUE) {
/* Update the stopwatch */
uint32_t elapsed_ticks = PIOS_DELAY_GetuSSince(prev_ticks);
prev_ticks += elapsed_ticks;
stopwatch += elapsed_ticks;
if (JumpToApp == TRUE)
jump_to_app();
//pwm_period = 50; // *100 uS -> 5 mS
//pwm_sweep_steps =100; // * 5 mS -> 500 mS
switch (DeviceState) {
case Last_operation_Success:
case uploadingStarting:
case DFUidle:
period1 = 50;
period1 = 5000;
sweep_steps1 = 100;
PIOS_LED_Off(RED);
period2 = 0;
break;
case uploading:
period1 = 50;
period1 = 5000;
sweep_steps1 = 100;
period2 = 25;
period2 = 2500;
sweep_steps2 = 50;
break;
case downloading:
period1 = 25;
period1 = 2500;
sweep_steps1 = 50;
PIOS_LED_Off(RED);
period2 = 0;
@ -137,14 +137,14 @@ int main() {
period2 = 0;
break;
default://error
period1 = 50;
period1 = 5000;
sweep_steps1 = 100;
period2 = 50;
period2 = 5000;
sweep_steps2 = 100;
}
if (period1 != 0) {
if (LedPWM(period1, sweep_steps1, STOPWATCH_ValueGet(LED_PWM_TIMER)))
if (LedPWM(period1, sweep_steps1, stopwatch))
PIOS_LED_On(BLUE);
else
PIOS_LED_Off(BLUE);
@ -152,16 +152,16 @@ int main() {
PIOS_LED_On(BLUE);
if (period2 != 0) {
if (LedPWM(period2, sweep_steps2, STOPWATCH_ValueGet(LED_PWM_TIMER)))
if (LedPWM(period2, sweep_steps2, stopwatch))
PIOS_LED_On(RED);
else
PIOS_LED_Off(RED);
} else
PIOS_LED_Off(RED);
if (STOPWATCH_ValueGet(LED_PWM_TIMER) > 100 * 50 * 100)
STOPWATCH_Reset(LED_PWM_TIMER);
if ((STOPWATCH_ValueGet(LED_PWM_TIMER) > 60000) && (DeviceState
if (stopwatch > 50 * 1000 * 1000)
stopwatch = 0;
if ((stopwatch > 6 * 1000 * 1000) && (DeviceState
== BLidle))
JumpToApp = TRUE;
@ -193,20 +193,21 @@ void jump_to_app() {
}
}
uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count) {
uint32_t pwm_duty = ((count / pwm_period) % pwm_sweep_steps)
/ (pwm_sweep_steps / pwm_period);
if ((count % (2 * pwm_period * pwm_sweep_steps)) > pwm_period
* pwm_sweep_steps)
pwm_duty = pwm_period - pwm_duty; // negative direction each 50*100 ticks
uint32_t curr_step = (count / pwm_period) % pwm_sweep_steps; /* 0 - pwm_sweep_steps */
uint32_t pwm_duty = pwm_period * curr_step / pwm_sweep_steps; /* fraction of pwm_period */
uint32_t curr_sweep = (count / (pwm_period * pwm_sweep_steps)); /* ticks once per full sweep */
if (curr_sweep & 1) {
pwm_duty = pwm_period - pwm_duty; /* reverse direction in odd sweeps */
}
return ((count % pwm_period) > pwm_duty) ? 1 : 0;
}
uint8_t processRX() {
while (PIOS_COM_ReceiveBufferUsed(PIOS_COM_TELEM_USB) >= 63) {
for (int32_t x = 0; x < 63; ++x) {
mReceive_Buffer[x] = PIOS_COM_ReceiveBuffer(PIOS_COM_TELEM_USB);
if (PIOS_COM_ReceiveBuffer(PIOS_COM_TELEM_USB, mReceive_Buffer, 63, 0) == 63) {
processComand(mReceive_Buffer);
}
processComand(mReceive_Buffer);
}
return TRUE;
}

39
flight/Bootloaders/PipXtreme/pios_board.c
View File

@ -31,10 +31,31 @@
#include <pios_com_priv.h>
#define PIOS_COM_TELEM_USB_RX_BUF_LEN 192
#define PIOS_COM_TELEM_USB_TX_BUF_LEN 192
static uint8_t pios_com_telem_usb_rx_buffer[PIOS_COM_TELEM_USB_RX_BUF_LEN];
static uint8_t pios_com_telem_usb_tx_buffer[PIOS_COM_TELEM_USB_TX_BUF_LEN];
#endif /* PIOS_INCLUDE_COM */
// ***********************************************************************************
#if defined(PIOS_INCLUDE_USB_HID)
#include "pios_usb_hid_priv.h"
static const struct pios_usb_hid_cfg pios_usb_hid_main_cfg = {
.irq = {
.init = {
.NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
};
#endif /* PIOS_INCLUDE_USB_HID */
extern const struct pios_com_driver pios_usb_com_driver;
uint32_t pios_com_telem_usb_id;
@ -44,7 +65,12 @@ uint32_t pios_com_telem_usb_id;
* initializes all the core subsystems on this specific hardware
* called from System/openpilot.c
*/
static bool board_init_complete = false;
void PIOS_Board_Init(void) {
if (board_init_complete) {
return;
}
/* Enable Prefetch Buffer */
FLASH_PrefetchBufferCmd(FLASH_PrefetchBuffer_Enable);
@ -58,13 +84,18 @@ void PIOS_Board_Init(void) {
PIOS_GPIO_Init();
#if defined(PIOS_INCLUDE_USB_HID)
PIOS_USB_HID_Init(0);
uint32_t pios_usb_hid_id;
PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_main_cfg);
#if defined(PIOS_INCLUDE_COM)
if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_com_driver, 0)) {
PIOS_DEBUG_Assert(0);
if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_com_driver, pios_usb_hid_id,
pios_com_telem_usb_rx_buffer, sizeof(pios_com_telem_usb_rx_buffer),
pios_com_telem_usb_tx_buffer, sizeof(pios_com_telem_usb_tx_buffer))) {
PIOS_Assert(0);
}
#endif /* PIOS_INCLUDE_COM */
#endif /* PIOS_INCLUDE_USB_HID */
#endif /* PIOS_INCLUDE_USB_HID */
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE);//TODO Tirar
board_init_complete = true;
}

30
flight/CopterControl/Makefile
View File

@ -45,7 +45,7 @@ ENABLE_DEBUG_PINS ?= NO
# Set to Yes to enable the AUX UART which is mapped on the S1 (Tx) and S2 (Rx) servo outputs
ENABLE_AUX_UART ?= NO
USE_SPEKTRUM ?= NO
USE_GPS ?= NO
USE_I2C ?= NO
@ -113,12 +113,9 @@ OPUAVSYNTHDIR = $(OUTDIR)/../uavobject-synthetics/flight
# List C source files here. (C dependencies are automatically generated.)
# use file-extension c for "c-only"-files
MODNAMES = $(notdir ${MODULES})
ifndef TESTAPP
## MODULES
SRC += ${foreach MOD, ${MODULES}, ${wildcard ${OPMODULEDIR}/${MOD}/*.c}}
SRC += ${OUTDIR}/InitMods.c
## OPENPILOT CORE:
SRC += ${OPMODULEDIR}/System/systemmod.c
SRC += $(OPSYSTEM)/coptercontrol.c
@ -166,6 +163,7 @@ SRC += $(OPUAVSYNTHDIR)/mixersettings.c
SRC += $(OPUAVSYNTHDIR)/mixerstatus.c
SRC += $(OPUAVSYNTHDIR)/firmwareiapobj.c
SRC += $(OPUAVSYNTHDIR)/attitudesettings.c
SRC += $(OPUAVSYNTHDIR)/hwsettings.c
#${wildcard ${OBJ}/$(shell echo $(VAR) | tr A-Z a-z)/*.c}
#SRC += ${foreach OBJ, ${UAVOBJECTS}, $(UAVOBJECTS)/$(OBJ).c}
# Cant use until i can automatically generate list of UAVObjects
@ -185,6 +183,7 @@ SRC += $(PIOSSTM32F10X)/pios_spi.c
SRC += $(PIOSSTM32F10X)/pios_ppm.c
SRC += $(PIOSSTM32F10X)/pios_pwm.c
SRC += $(PIOSSTM32F10X)/pios_spektrum.c
SRC += $(PIOSSTM32F10X)/pios_sbus.c
SRC += $(PIOSSTM32F10X)/pios_debug.c
SRC += $(PIOSSTM32F10X)/pios_gpio.c
SRC += $(PIOSSTM32F10X)/pios_exti.c
@ -207,6 +206,7 @@ SRC += $(PIOSCOMMON)/pios_com.c
SRC += $(PIOSCOMMON)/pios_i2c_esc.c
SRC += $(PIOSCOMMON)/pios_iap.c
SRC += $(PIOSCOMMON)/pios_bl_helper.c
SRC += $(PIOSCOMMON)/pios_rcvr.c
SRC += $(PIOSCOMMON)/printf-stdarg.c
## Libraries for flight calculations
SRC += $(FLIGHTLIB)/fifo_buffer.c
@ -379,15 +379,15 @@ endif
ifeq ($(ERASE_FLASH), YES)
CDEFS += -DERASE_FLASH
endif
ifeq ($(USE_SPEKTRUM), YES)
CDEFS += -DUSE_SPEKTRUM
ifneq ($(USE_GPS), NO)
CDEFS += -DUSE_GPS
endif
ifeq ($(USE_I2C), YES)
CDEFS += -DUSE_I2C
endif
# Place project-specific -D and/or -U options for
# Assembler with preprocessor here.
#ADEFS = -DUSE_IRQ_ASM_WRAPPER
@ -487,7 +487,7 @@ LSTFILES = $(addprefix $(OUTDIR)/, $(addsuffix .lst, $(ALLSRCBASE)))
DEPFILES = $(addprefix $(OUTDIR)/dep/, $(addsuffix .o.d, $(ALLSRCBASE)))
# Default target.
all: gencode build
all: build
ifeq ($(LOADFORMAT),ihex)
build: elf hex lss sym
@ -503,18 +503,6 @@ endif
endif
endif
# Generate intermediate code for objects
gencode: ${OUTDIR}/InitMods.c
# Generate code for module initialization
${OUTDIR}/InitMods.c: Makefile
@echo $(MSG_MODINIT) $(call toprel, $@)
@echo ${quote}// Autogenerated file${quote} > ${OUTDIR}/InitMods.c
@echo ${quote}${foreach MOD, ${MODNAMES}, extern unsigned int ${MOD}Initialize(void);}${quote} >> ${OUTDIR}/InitMods.c
@echo ${quote}void InitModules() {${quote} >> ${OUTDIR}/InitMods.c
@echo ${quote}${foreach MOD, ${MODNAMES}, ${MOD}Initialize();}${quote} >> ${OUTDIR}/InitMods.c
@echo ${quote}}${quote} >> ${OUTDIR}/InitMods.c
# Generate code for PyMite
#$(OUTDIR)/pmlib_img.c $(OUTDIR)/pmlib_nat.c $(OUTDIR)/pmlibusr_img.c $(OUTDIR)/pmlibusr_nat.c $(OUTDIR)/pmfeatures.h: $(wildcard $(PYMITELIB)/*.py) $(wildcard $(PYMITEPLAT)/*.py) $(wildcard $(FLIGHTPLANLIB)/*.py) $(wildcard $(FLIGHTPLANS)/*.py)
# @echo $(MSG_PYMITEINIT) $(call toprel, $@)
@ -621,4 +609,4 @@ else
endif
# Listing of phony targets.
.PHONY : all build clean clean_list gencode install
.PHONY : all build clean clean_list install

50
flight/CopterControl/System/coptercontrol.c
View File

@ -42,18 +42,16 @@
/* Global Variables */
/* Prototype of generated InitModules() function */
extern void InitModules(void);
/* Prototype of PIOS_Board_Init() function */
extern void PIOS_Board_Init(void);
extern void Stack_Change(void);
/**
* OpenPilot Main function:
*
* Initialize PiOS<BR>
* Create the "System" task (SystemModInitializein Modules/System/systemmod.c) <BR>
* Start FreeRTOS Scheduler (vTaskStartScheduler)<BR>
* Start FreeRTOS Scheduler (vTaskStartScheduler) (Now handled by caller)
* If something goes wrong, blink LED1 and LED2 every 100ms
*
*/
@ -65,40 +63,32 @@ int main()
/* Brings up System using CMSIS functions, enables the LEDs. */
PIOS_SYS_Init();
/* Initialize the system thread */
SystemModInitialize();
/* Start the FreeRTOS scheduler */
vTaskStartScheduler();
/* If all is well we will never reach here as the scheduler will now be running. */
/* If we do get here, it will most likely be because we ran out of heap space. */
return 0;
}
/**
* Initialize the hardware, libraries and modules (called by the System thread in systemmod.c)
*/
void OpenPilotInit()
{
/* Architecture dependant Hardware and
* core subsystem initialisation
* (see pios_board.c for your arch)
* */
PIOS_Board_Init();
#ifdef ERASE_FLASH
PIOS_Flash_W25X_EraseChip();
while(TRUE){};
#endif
/* Initialize modules */
InitModules();
}
MODULE_INITIALISE_ALL
/* swap the stack to use the IRQ stack */
Stack_Change();
/* Start the FreeRTOS scheduler which should never returns.*/
vTaskStartScheduler();
/* If all is well we will never reach here as the scheduler will now be running. */
/* Do some indication to user that something bad just happened */
PIOS_LED_Off(LED1); \
for(;;) { \
PIOS_LED_Toggle(LED1); \
PIOS_DELAY_WaitmS(100); \
};
return 0;
}
/**
* @}

11
flight/CopterControl/System/inc/FreeRTOSConfig.h
View File

@ -9,7 +9,7 @@
* application requirements.
*
* THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE
* FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE.
* FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE.
*
* See http://www.freertos.org/a00110.html.
*----------------------------------------------------------*/
@ -29,8 +29,8 @@
#define configCPU_CLOCK_HZ ( ( unsigned long ) 72000000 )
#define configTICK_RATE_HZ ( ( portTickType ) 1000 )
#define configMAX_PRIORITIES ( ( unsigned portBASE_TYPE ) 5 )
#define configMINIMAL_STACK_SIZE ( ( unsigned short ) 128 )
#define configTOTAL_HEAP_SIZE ( ( size_t ) ( 14 * 1024 ) )
#define configMINIMAL_STACK_SIZE ( ( unsigned short ) 48 )
#define configTOTAL_HEAP_SIZE ( ( size_t ) ( 54 * 256) )
#define configMAX_TASK_NAME_LEN ( 16 )
#define configUSE_TRACE_FACILITY 0
#define configUSE_16_BIT_TICKS 0
@ -71,6 +71,10 @@ configKERNEL_INTERRUPT_PRIORITY setting. Here 15 corresponds to the lowest
NVIC value of 255. */
#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY 15
#if !defined(ARCH_POSIX) && !defined(ARCH_WIN32)
#define CHECK_IRQ_STACK
#endif
/* Enable run time stats collection */
//#if defined(DEBUG)
#define configGENERATE_RUN_TIME_STATS 1
@ -83,6 +87,7 @@ do {\
#define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004)/* DWT_CYCCNT */
//#endif
/**
* @}
*/

220
flight/CopterControl/System/inc/pios_config.h
View File

@ -1,107 +1,113 @@
/**
******************************************************************************
* @addtogroup OpenPilotSystem OpenPilot System
* @{
* @addtogroup OpenPilotCore OpenPilot Core
* @{
*
* @file pios_config.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief PiOS configuration header.
* Central compile time config for the project.
* In particular, pios_config.h is where you define which PiOS libraries
* and features are included in the firmware.
* @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 PIOS_CONFIG_H
#define PIOS_CONFIG_H
/* Enable/Disable PiOS Modules */
#define PIOS_INCLUDE_ADC
#define PIOS_INCLUDE_DELAY
#if defined(USE_I2C)
#define PIOS_INCLUDE_I2C
#define PIOS_INCLUDE_I2C_ESC
#endif
#define PIOS_INCLUDE_IRQ
#define PIOS_INCLUDE_LED
#if defined(USE_SPEKTRUM)
#define PIOS_INCLUDE_SPEKTRUM
#else
#define PIOS_INCLUDE_GPS
//#define PIOS_INCLUDE_PPM
#define PIOS_INCLUDE_PWM
#endif
#define PIOS_INCLUDE_SERVO
#define PIOS_INCLUDE_SPI
#define PIOS_INCLUDE_SYS
#define PIOS_INCLUDE_USART
#define PIOS_INCLUDE_USB_HID
#define PIOS_INCLUDE_COM
#define PIOS_INCLUDE_SETTINGS
#define PIOS_INCLUDE_FREERTOS
#define PIOS_INCLUDE_GPIO
#define PIOS_INCLUDE_EXTI
#define PIOS_INCLUDE_RTC
#define PIOS_INCLUDE_WDG
#define PIOS_INCLUDE_BL_HELPER
#define PIOS_INCLUDE_ADXL345
#define PIOS_INCLUDE_FLASH
/* A really shitty setting saving implementation */
#define PIOS_INCLUDE_FLASH_SECTOR_SETTINGS
/* Defaults for Logging */
#define LOG_FILENAME "PIOS.LOG"
#define STARTUP_LOG_ENABLED 1
/* COM Module */
#define GPS_BAUDRATE 19200
#define TELEM_BAUDRATE 19200
#define AUXUART_ENABLED 0
#define AUXUART_BAUDRATE 19200
/* Alarm Thresholds */
#define HEAP_LIMIT_WARNING 220
#define HEAP_LIMIT_CRITICAL 150
#define CPULOAD_LIMIT_WARNING 80
#define CPULOAD_LIMIT_CRITICAL 95
/* Task stack sizes */
#define PIOS_ACTUATOR_STACK_SIZE 1020
#define PIOS_MANUAL_STACK_SIZE 724
#define PIOS_SYSTEM_STACK_SIZE 560
#define PIOS_STABILIZATION_STACK_SIZE 524
#define PIOS_TELEM_STACK_SIZE 500
#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 1995998
//#define PIOS_QUATERNION_STABILIZATION
#endif /* PIOS_CONFIG_H */
/**
* @}
* @}
*/
/**
******************************************************************************
* @addtogroup OpenPilotSystem OpenPilot System
* @{
* @addtogroup OpenPilotCore OpenPilot Core
* @{
*
* @file pios_config.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief PiOS configuration header.
* Central compile time config for the project.
* In particular, pios_config.h is where you define which PiOS libraries
* and features are included in the firmware.
* @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 PIOS_CONFIG_H
#define PIOS_CONFIG_H
/* Enable/Disable PiOS Modules */
#define PIOS_INCLUDE_ADC
#define PIOS_INCLUDE_DELAY
#if defined(USE_I2C)
#define PIOS_INCLUDE_I2C
#define PIOS_INCLUDE_I2C_ESC
#endif
#define PIOS_INCLUDE_IRQ
#define PIOS_INCLUDE_LED
#define PIOS_INCLUDE_RCVR
/* Supported receiver interfaces */
#define PIOS_INCLUDE_SPEKTRUM
#define PIOS_INCLUDE_SBUS
//#define PIOS_INCLUDE_PPM
#define PIOS_INCLUDE_PWM
/* Supported USART-based PIOS modules */
#define PIOS_INCLUDE_TELEMETRY_RF
//#define PIOS_INCLUDE_GPS
#define PIOS_INCLUDE_SERVO
#define PIOS_INCLUDE_SPI
#define PIOS_INCLUDE_SYS
#define PIOS_INCLUDE_USART
#define PIOS_INCLUDE_USB_HID
#define PIOS_INCLUDE_COM
#define PIOS_INCLUDE_SETTINGS
#define PIOS_INCLUDE_FREERTOS
#define PIOS_INCLUDE_GPIO
#define PIOS_INCLUDE_EXTI
#define PIOS_INCLUDE_RTC
#define PIOS_INCLUDE_WDG
#define PIOS_INCLUDE_BL_HELPER
#define PIOS_INCLUDE_ADXL345
#define PIOS_INCLUDE_FLASH
/* A really shitty setting saving implementation */
#define PIOS_INCLUDE_FLASH_SECTOR_SETTINGS
/* Defaults for Logging */
#define LOG_FILENAME "PIOS.LOG"
#define STARTUP_LOG_ENABLED 1
/* COM Module */
#define GPS_BAUDRATE 19200
#define TELEM_BAUDRATE 19200
#define AUXUART_ENABLED 0
#define AUXUART_BAUDRATE 19200
/* Alarm Thresholds */
#define HEAP_LIMIT_WARNING 220
#define HEAP_LIMIT_CRITICAL 40
#define IRQSTACK_LIMIT_WARNING 100
#define IRQSTACK_LIMIT_CRITICAL 60
#define CPULOAD_LIMIT_WARNING 85
#define CPULOAD_LIMIT_CRITICAL 95
/* Task stack sizes */
#define PIOS_ACTUATOR_STACK_SIZE 1020
#define PIOS_MANUAL_STACK_SIZE 724
#define PIOS_SYSTEM_STACK_SIZE 460
#define PIOS_STABILIZATION_STACK_SIZE 524
#define PIOS_TELEM_STACK_SIZE 500
#define PIOS_EVENTDISPATCHER_STACK_SIZE 130
#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 1995998
//#define PIOS_QUATERNION_STABILIZATION
// This can't be too high to stop eventdispatcher thread overflowing
#define PIOS_EVENTDISAPTCHER_QUEUE 10
#endif /* PIOS_CONFIG_H */
/**
* @}
* @}
*/

700
flight/CopterControl/System/pios_board.c
View File

@ -30,10 +30,11 @@
#include <pios.h>
#include <openpilot.h>
#include <uavobjectsinit.h>
#include <hwsettings.h>
#include <manualcontrolsettings.h>
#if defined(PIOS_INCLUDE_SPI)
#include <pios_spi_priv.h>
/* Flash/Accel Interface
@ -44,7 +45,7 @@
void PIOS_SPI_flash_accel_irq_handler(void);
void DMA1_Channel4_IRQHandler() __attribute__ ((alias ("PIOS_SPI_flash_accel_irq_handler")));
void DMA1_Channel5_IRQHandler() __attribute__ ((alias ("PIOS_SPI_flash_accel_irq_handler")));
const struct pios_spi_cfg pios_spi_flash_accel_cfg = {
static const struct pios_spi_cfg pios_spi_flash_accel_cfg = {
.regs = SPI2,
.init = {
.SPI_Mode = SPI_Mode_Master,
@ -62,7 +63,6 @@ const struct pios_spi_cfg pios_spi_flash_accel_cfg = {
.ahb_clk = RCC_AHBPeriph_DMA1,
.irq = {
.handler = PIOS_SPI_flash_accel_irq_handler,
.flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4),
.init = {
.NVIC_IRQChannel = DMA1_Channel4_IRQn,
@ -151,11 +151,10 @@ void PIOS_SPI_flash_accel_irq_handler(void)
extern void PIOS_ADC_handler(void);
void DMA1_Channel1_IRQHandler() __attribute__ ((alias("PIOS_ADC_handler")));
// Remap the ADC DMA handler to this one
const struct pios_adc_cfg pios_adc_cfg = {
static const struct pios_adc_cfg pios_adc_cfg = {
.dma = {
.ahb_clk = RCC_AHBPeriph_DMA1,
.irq = {
.handler = PIOS_ADC_DMA_Handler,
.flags = (DMA1_FLAG_TC1 | DMA1_FLAG_TE1 | DMA1_FLAG_HT1 | DMA1_FLAG_GL1),
.init = {
.NVIC_IRQChannel = DMA1_Channel1_IRQn,
@ -200,19 +199,14 @@ void PIOS_ADC_handler() {
#include "pios_usart_priv.h"
#if defined(PIOS_INCLUDE_TELEMETRY_RF)
/*
* Telemetry USART
*/
void PIOS_USART_telem_irq_handler(void);
void USART1_IRQHandler() __attribute__ ((alias ("PIOS_USART_telem_irq_handler")));
const struct pios_usart_cfg pios_usart_telem_cfg = {
static const struct pios_usart_cfg pios_usart_telem_main_cfg = {
.regs = USART1,
.init = {
#if defined (PIOS_COM_TELEM_BAUDRATE)
.USART_BaudRate = PIOS_COM_TELEM_BAUDRATE,
#else
.USART_BaudRate = 57600,
#endif
.USART_BaudRate = 57600,
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1,
@ -220,7 +214,6 @@ const struct pios_usart_cfg pios_usart_telem_cfg = {
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
},
.irq = {
.handler = PIOS_USART_telem_irq_handler,
.init = {
.NVIC_IRQChannel = USART1_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
@ -246,20 +239,10 @@ const struct pios_usart_cfg pios_usart_telem_cfg = {
},
};
#if defined(PIOS_INCLUDE_GPS)
/*
* GPS USART
*/
void PIOS_USART_gps_irq_handler(void);
void USART3_IRQHandler() __attribute__ ((alias ("PIOS_USART_gps_irq_handler")));
const struct pios_usart_cfg pios_usart_gps_cfg = {
.regs = USART3,
static const struct pios_usart_cfg pios_usart_telem_flexi_cfg = {
.regs = USART3,
.init = {
#if defined (PIOS_COM_GPS_BAUDRATE)
.USART_BaudRate = PIOS_COM_GPS_BAUDRATE,
#else
.USART_BaudRate = 57600,
#endif
.USART_BaudRate = 57600,
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1,
@ -267,7 +250,6 @@ const struct pios_usart_cfg pios_usart_gps_cfg = {
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
},
.irq = {
.handler = PIOS_USART_gps_irq_handler,
.init = {
.NVIC_IRQChannel = USART3_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
@ -292,22 +274,143 @@ const struct pios_usart_cfg pios_usart_gps_cfg = {
},
},
};
#endif
#endif /* PIOS_INCLUDE_TELEMETRY_RF */
#if defined(PIOS_INCLUDE_GPS)
/*
* GPS USART
*/
static const struct pios_usart_cfg pios_usart_gps_main_cfg = {
.regs = USART1,
.init = {
.USART_BaudRate = 57600,
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1,
.USART_HardwareFlowControl = USART_HardwareFlowControl_None,
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
},
.irq = {
.init = {
.NVIC_IRQChannel = USART1_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_IPU,
},
},
.tx = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_9,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF_PP,
},
},
};
static const struct pios_usart_cfg pios_usart_gps_flexi_cfg = {
.regs = USART3,
.init = {
.USART_BaudRate = 57600,
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1,
.USART_HardwareFlowControl = USART_HardwareFlowControl_None,
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
},
.irq = {
.init = {
.NVIC_IRQChannel = USART3_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_11,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_IPU,
},
},
.tx = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF_PP,
},
},
};
#endif /* PIOS_INCLUDE_GPS */
#if defined(PIOS_INCLUDE_SPEKTRUM)
/*
* SPEKTRUM USART
*/
void PIOS_USART_spektrum_irq_handler(void);
void USART3_IRQHandler() __attribute__ ((alias ("PIOS_USART_spektrum_irq_handler")));
const struct pios_usart_cfg pios_usart_spektrum_cfg = {
.regs = USART3,
#include <pios_spektrum_priv.h>
static const struct pios_usart_cfg pios_usart_spektrum_main_cfg = {
.regs = USART1,
.init = {
#if defined (PIOS_COM_SPEKTRUM_BAUDRATE)
.USART_BaudRate = PIOS_COM_SPEKTRUM_BAUDRATE,
#else
.USART_BaudRate = 115200,
#endif
.USART_BaudRate = 115200,
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1,
.USART_HardwareFlowControl = USART_HardwareFlowControl_None,
.USART_Mode = USART_Mode_Rx,
},
.irq = {
.init = {
.NVIC_IRQChannel = USART1_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_IPU,
},
},
.tx = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_9,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_IN_FLOATING,
},
},
};
static const struct pios_spektrum_cfg pios_spektrum_main_cfg = {
.bind = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_Out_PP,
},
},
.remap = 0,
};
static const struct pios_usart_cfg pios_usart_spektrum_flexi_cfg = {
.regs = USART3,
.init = {
.USART_BaudRate = 115200,
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1,
@ -315,7 +418,6 @@ const struct pios_usart_cfg pios_usart_spektrum_cfg = {
.USART_Mode = USART_Mode_Rx,
},
.irq = {
.handler = PIOS_USART_spektrum_irq_handler,
.init = {
.NVIC_IRQChannel = USART3_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
@ -341,75 +443,128 @@ const struct pios_usart_cfg pios_usart_spektrum_cfg = {
},
};
static uint32_t pios_usart_spektrum_id;
void PIOS_USART_spektrum_irq_handler(void)
{
SPEKTRUM_IRQHandler(pios_usart_spektrum_id);
}
#include <pios_spektrum_priv.h>
void RTC_IRQHandler();
void RTC_IRQHandler() __attribute__ ((alias ("PIOS_SUPV_irq_handler")));
const struct pios_spektrum_cfg pios_spektrum_cfg = {
.pios_usart_spektrum_cfg = &pios_usart_spektrum_cfg,
.gpio_init = { //used for bind feature
.GPIO_Mode = GPIO_Mode_Out_PP,
.GPIO_Speed = GPIO_Speed_2MHz,
},
.remap = 0,
.irq = {
.handler = RTC_IRQHandler,
.init = {
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
static const struct pios_spektrum_cfg pios_spektrum_flexi_cfg = {
.bind = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_11,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_Out_PP,
},
},
.port = GPIOB,
.pin = GPIO_Pin_11,
.remap = 0,
};
void PIOS_SUPV_irq_handler() {
if (RTC_GetITStatus(RTC_IT_SEC))
{
/* Call the right handler */
PIOS_SPEKTRUM_irq_handler(pios_usart_spektrum_id);
/* Wait until last write operation on RTC registers has finished */
RTC_WaitForLastTask();
/* Clear the RTC Second interrupt */
RTC_ClearITPendingBit(RTC_IT_SEC);
}
}
#endif /* PIOS_INCLUDE_SPEKTRUM */
static uint32_t pios_usart_telem_rf_id;
void PIOS_USART_telem_irq_handler(void)
{
PIOS_USART_IRQ_Handler(pios_usart_telem_rf_id);
}
#if defined(PIOS_INCLUDE_SBUS)
/*
* SBUS USART
*/
#include <pios_sbus_priv.h>
#if defined(PIOS_INCLUDE_GPS)
static uint32_t pios_usart_gps_id;
void PIOS_USART_gps_irq_handler(void)
{
PIOS_USART_IRQ_Handler(pios_usart_gps_id);
}
#endif /* PIOS_INCLUDE_GPS */
static const struct pios_usart_cfg pios_usart_sbus_main_cfg = {
.regs = USART1,
.init = {
.USART_BaudRate = 100000,
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_Even,
.USART_StopBits = USART_StopBits_2,
.USART_HardwareFlowControl = USART_HardwareFlowControl_None,
.USART_Mode = USART_Mode_Rx,
},
.irq = {
.init = {
.NVIC_IRQChannel = USART1_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_IPU,
},
},
.tx = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_9,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_IN_FLOATING,
},
},
};
#endif /* PIOS_INCLUDE_USART */
static const struct pios_sbus_cfg pios_sbus_cfg = {
/* Inverter configuration */
.inv = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_2,
.GPIO_Mode = GPIO_Mode_Out_PP,
.GPIO_Speed = GPIO_Speed_2MHz,
},
},
.gpio_clk_func = RCC_APB2PeriphClockCmd,
.gpio_clk_periph = RCC_APB2Periph_GPIOB,
.gpio_inv_enable = Bit_SET,
};
#endif /* PIOS_INCLUDE_SBUS */
#endif /* PIOS_INCLUDE_USART */
#if defined(PIOS_INCLUDE_COM)
#include "pios_com_priv.h"
#define PIOS_COM_TELEM_RF_RX_BUF_LEN 192
#define PIOS_COM_TELEM_RF_TX_BUF_LEN 192
#define PIOS_COM_GPS_RX_BUF_LEN 96
#define PIOS_COM_TELEM_USB_RX_BUF_LEN 192
#define PIOS_COM_TELEM_USB_TX_BUF_LEN 192
#endif /* PIOS_INCLUDE_COM */
#if defined(PIOS_INCLUDE_RTC)
/*
* Realtime Clock (RTC)
*/
#include <pios_rtc_priv.h>
void PIOS_RTC_IRQ_Handler (void);
void RTC_IRQHandler() __attribute__ ((alias ("PIOS_RTC_IRQ_Handler")));
static const struct pios_rtc_cfg pios_rtc_main_cfg = {
.clksrc = RCC_RTCCLKSource_HSE_Div128,
.prescaler = 100,
.irq = {
.init = {
.NVIC_IRQChannel = RTC_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
};
void PIOS_RTC_IRQ_Handler (void)
{
PIOS_RTC_irq_handler ();
}
#endif
/*
* Servo outputs
*/
#include <pios_servo_priv.h>
const struct pios_servo_channel pios_servo_channels[] = {
static const struct pios_servo_channel pios_servo_channels[] = {
{
.timer = TIM4,
.port = GPIOB,
@ -475,13 +630,64 @@ const struct pios_servo_cfg pios_servo_cfg = {
.num_channels = NELEMENTS(pios_servo_channels),
};
#if defined(PIOS_INCLUDE_PWM) && defined(PIOS_INCLUDE_PPM)
#error Cannot define both PIOS_INCLUDE_PWM and PIOS_INCLUDE_PPM at the same time (yet)
#endif
/*
* PPM Inputs
*/
#if defined(PIOS_INCLUDE_PPM)
#include <pios_ppm_priv.h>
void TIM4_IRQHandler();
void TIM4_IRQHandler() __attribute__ ((alias ("PIOS_TIM4_irq_handler")));
const struct pios_ppm_cfg pios_ppm_cfg = {
.tim_base_init = {
.TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1, /* For 1 uS accuracy */
.TIM_ClockDivision = TIM_CKD_DIV1,
.TIM_CounterMode = TIM_CounterMode_Up,
.TIM_Period = 0xFFFF, /* shared timer, make sure init correctly in outputs */
.TIM_RepetitionCounter = 0x0000,
},
.tim_ic_init = {
.TIM_Channel = TIM_Channel_1,
.TIM_ICPolarity = TIM_ICPolarity_Rising,
.TIM_ICSelection = TIM_ICSelection_DirectTI,
.TIM_ICPrescaler = TIM_ICPSC_DIV1,
.TIM_ICFilter = 0x0,
},
.gpio_init = {
.GPIO_Pin = GPIO_Pin_6,
.GPIO_Mode = GPIO_Mode_IPD,
.GPIO_Speed = GPIO_Speed_2MHz,
},
.remap = 0,
.irq = {
.init = {
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.timer = TIM4,
.port = GPIOB,
.ccr = TIM_IT_CC1,
};
void PIOS_TIM4_irq_handler()
{
PIOS_PPM_irq_handler();
}
#endif /* PIOS_INCLUDE_PPM */
/*
* PWM Inputs
*/
#if defined(PIOS_INCLUDE_PWM)
#include <pios_pwm_priv.h>
const struct pios_pwm_channel pios_pwm_channels[] = {
static const struct pios_pwm_channel pios_pwm_channels[] = {
{
.timer = TIM4,
.port = GPIOB,
@ -552,7 +758,6 @@ const struct pios_pwm_cfg pios_pwm_cfg = {
},
.remap = 0,
.irq = {
.handler = TIM2_IRQHandler,
.init = {
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
@ -589,7 +794,7 @@ void PIOS_I2C_main_adapter_er_irq_handler(void);
void I2C2_EV_IRQHandler() __attribute__ ((alias ("PIOS_I2C_main_adapter_ev_irq_handler")));
void I2C2_ER_IRQHandler() __attribute__ ((alias ("PIOS_I2C_main_adapter_er_irq_handler")));
const struct pios_i2c_adapter_cfg pios_i2c_main_adapter_cfg = {
static const struct pios_i2c_adapter_cfg pios_i2c_main_adapter_cfg = {
.regs = I2C2,
.init = {
.I2C_Mode = I2C_Mode_I2C,
@ -617,7 +822,6 @@ const struct pios_i2c_adapter_cfg pios_i2c_main_adapter_cfg = {
},
},
.event = {
.handler = PIOS_I2C_main_adapter_ev_irq_handler,
.flags = 0, /* FIXME: check this */
.init = {
.NVIC_IRQChannel = I2C2_EV_IRQn,
@ -627,7 +831,6 @@ const struct pios_i2c_adapter_cfg pios_i2c_main_adapter_cfg = {
},
},
.error = {
.handler = PIOS_I2C_main_adapter_er_irq_handler,
.flags = 0, /* FIXME: check this */
.init = {
.NVIC_IRQChannel = I2C2_ER_IRQn,
@ -653,12 +856,33 @@ void PIOS_I2C_main_adapter_er_irq_handler(void)
#endif /* PIOS_INCLUDE_I2C */
#if defined(PIOS_INCLUDE_RCVR)
#include "pios_rcvr_priv.h"
struct pios_rcvr_channel_map pios_rcvr_channel_to_id_map[PIOS_RCVR_MAX_CHANNELS];
uint32_t pios_rcvr_max_channel;
#endif /* PIOS_INCLUDE_RCVR */
#if defined(PIOS_INCLUDE_USB_HID)
#include "pios_usb_hid_priv.h"
static const struct pios_usb_hid_cfg pios_usb_hid_main_cfg = {
.irq = {
.init = {
.NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
};
#endif /* PIOS_INCLUDE_USB_HID */
extern const struct pios_com_driver pios_usb_com_driver;
uint32_t pios_com_telem_rf_id;
uint32_t pios_com_telem_usb_id;
uint32_t pios_com_gps_id;
uint32_t pios_com_spektrum_id;
/**
* PIOS_Board_Init()
@ -668,57 +892,257 @@ uint32_t pios_com_spektrum_id;
void PIOS_Board_Init(void) {
/* Delay system */
PIOS_DELAY_Init();
PIOS_DELAY_Init();
/* Set up the SPI interface to the serial flash */
if (PIOS_SPI_Init(&pios_spi_flash_accel_id, &pios_spi_flash_accel_cfg)) {
PIOS_DEBUG_Assert(0);
PIOS_Assert(0);
}
PIOS_Flash_W25X_Init(pios_spi_flash_accel_id);
PIOS_ADXL345_Attach(pios_spi_flash_accel_id);
PIOS_FLASHFS_Init();
#if defined(PIOS_INCLUDE_SPEKTRUM)
/* SPEKTRUM init must come before comms */
PIOS_SPEKTRUM_Init();
if (PIOS_USART_Init(&pios_usart_spektrum_id, &pios_usart_spektrum_cfg)) {
PIOS_DEBUG_Assert(0);
}
if (PIOS_COM_Init(&pios_com_spektrum_id, &pios_usart_com_driver, pios_usart_spektrum_id)) {
PIOS_DEBUG_Assert(0);
}
#endif
/* Initialize UAVObject libraries */
EventDispatcherInitialize();
UAVObjInitialize();
UAVObjectsInitializeAll();
#if defined(PIOS_INCLUDE_RTC)
/* Initialize the real-time clock and its associated tick */
PIOS_RTC_Init(&pios_rtc_main_cfg);
#endif
/* Initialize the alarms library */
AlarmsInitialize();
/* Initialize the task monitor library */
TaskMonitorInitialize();
/* Initialize the PiOS library */
#if defined(PIOS_INCLUDE_COM)
if (PIOS_USART_Init(&pios_usart_telem_rf_id, &pios_usart_telem_cfg)) {
PIOS_DEBUG_Assert(0);
}
if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_telem_rf_id)) {
PIOS_DEBUG_Assert(0);
}
/* Configure the main IO port */
uint8_t hwsettings_cc_mainport;
HwSettingsCC_MainPortGet(&hwsettings_cc_mainport);
switch (hwsettings_cc_mainport) {
case HWSETTINGS_CC_MAINPORT_DISABLED:
break;
case HWSETTINGS_CC_MAINPORT_TELEMETRY:
#if defined(PIOS_INCLUDE_TELEMETRY_RF)
{
uint32_t pios_usart_telem_rf_id;
if (PIOS_USART_Init(&pios_usart_telem_rf_id, &pios_usart_telem_main_cfg)) {
PIOS_Assert(0);
}
uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN);
uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN);
PIOS_Assert(rx_buffer);
PIOS_Assert(tx_buffer);
if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_telem_rf_id,
rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN,
tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) {
PIOS_Assert(0);
}
}
#endif /* PIOS_INCLUDE_TELEMETRY_RF */
break;
case HWSETTINGS_CC_MAINPORT_SBUS:
#if defined(PIOS_INCLUDE_SBUS)
{
uint32_t pios_usart_sbus_id;
if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_usart_sbus_main_cfg)) {
PIOS_Assert(0);
}
uint32_t pios_sbus_id;
if (PIOS_SBUS_Init(&pios_sbus_id, &pios_sbus_cfg, &pios_usart_com_driver, pios_usart_sbus_id)) {
PIOS_Assert(0);
}
}
#endif /* PIOS_INCLUDE_SBUS */
break;
case HWSETTINGS_CC_MAINPORT_GPS:
#if defined(PIOS_INCLUDE_GPS)
if (PIOS_USART_Init(&pios_usart_gps_id, &pios_usart_gps_cfg)) {
PIOS_DEBUG_Assert(0);
}
if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id)) {
PIOS_DEBUG_Assert(0);
}
{
uint32_t pios_usart_gps_id;
if (PIOS_USART_Init(&pios_usart_gps_id, &pios_usart_gps_main_cfg)) {
PIOS_Assert(0);
}
uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN);
PIOS_Assert(rx_buffer);
if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id,
rx_buffer, PIOS_COM_GPS_RX_BUF_LEN,
NULL, 0)) {
PIOS_Assert(0);
}
}
#endif /* PIOS_INCLUDE_GPS */
#endif /* PIOS_INCLUDE_COM */
break;
case HWSETTINGS_CC_MAINPORT_SPEKTRUM:
#if defined(PIOS_INCLUDE_SPEKTRUM)
{
uint32_t pios_usart_spektrum_id;
if (PIOS_USART_Init(&pios_usart_spektrum_id, &pios_usart_spektrum_main_cfg)) {
PIOS_Assert(0);
}
uint32_t pios_spektrum_id;
if (PIOS_SPEKTRUM_Init(&pios_spektrum_id, &pios_spektrum_main_cfg, &pios_usart_com_driver, pios_usart_spektrum_id, false)) {
PIOS_Assert(0);
}
}
#endif /* PIOS_INCLUDE_SPEKTRUM */
break;
case HWSETTINGS_CC_MAINPORT_COMAUX:
break;
}
/* Configure the flexi port */
uint8_t hwsettings_cc_flexiport;
HwSettingsCC_FlexiPortGet(&hwsettings_cc_flexiport);
switch (hwsettings_cc_flexiport) {
case HWSETTINGS_CC_FLEXIPORT_DISABLED:
break;
case HWSETTINGS_CC_FLEXIPORT_TELEMETRY:
#if defined(PIOS_INCLUDE_TELEMETRY_RF)
{
uint32_t pios_usart_telem_rf_id;
if (PIOS_USART_Init(&pios_usart_telem_rf_id, &pios_usart_telem_flexi_cfg)) {
PIOS_Assert(0);
}
uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN);
uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN);
PIOS_Assert(rx_buffer);
PIOS_Assert(tx_buffer);
if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_telem_rf_id,
rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN,
tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) {
PIOS_Assert(0);
}
}
#endif /* PIOS_INCLUDE_TELEMETRY_RF */
break;
case HWSETTINGS_CC_FLEXIPORT_GPS:
#if defined(PIOS_INCLUDE_GPS)
{
uint32_t pios_usart_gps_id;
if (PIOS_USART_Init(&pios_usart_gps_id, &pios_usart_gps_flexi_cfg)) {
PIOS_Assert(0);
}
uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN);
PIOS_Assert(rx_buffer);
if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id,
rx_buffer, PIOS_COM_GPS_RX_BUF_LEN,
NULL, 0)) {
PIOS_Assert(0);
}
}
#endif /* PIOS_INCLUDE_GPS */
break;
case HWSETTINGS_CC_FLEXIPORT_SPEKTRUM:
#if defined(PIOS_INCLUDE_SPEKTRUM)
{
uint32_t pios_usart_spektrum_id;
if (PIOS_USART_Init(&pios_usart_spektrum_id, &pios_usart_spektrum_flexi_cfg)) {
PIOS_Assert(0);
}
uint32_t pios_spektrum_id;
if (PIOS_SPEKTRUM_Init(&pios_spektrum_id, &pios_spektrum_flexi_cfg, &pios_usart_com_driver, pios_usart_spektrum_id, false)) {
PIOS_Assert(0);
}
}
#endif /* PIOS_INCLUDE_SPEKTRUM */
break;
case HWSETTINGS_CC_FLEXIPORT_COMAUX:
break;
case HWSETTINGS_CC_FLEXIPORT_I2C:
#if defined(PIOS_INCLUDE_I2C)
{
if (PIOS_I2C_Init(&pios_i2c_main_adapter_id, &pios_i2c_main_adapter_cfg)) {
PIOS_Assert(0);
}
}
#endif /* PIOS_INCLUDE_I2C */
break;
}
/* Configure the selected receiver */
uint8_t manualcontrolsettings_inputmode;
ManualControlSettingsInputModeGet(&manualcontrolsettings_inputmode);
switch (manualcontrolsettings_inputmode) {
case MANUALCONTROLSETTINGS_INPUTMODE_PWM:
#if defined(PIOS_INCLUDE_PWM)
PIOS_PWM_Init();
uint32_t pios_pwm_rcvr_id;
if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, 0)) {
PIOS_Assert(0);
}
for (uint8_t i = 0;
i < PIOS_PWM_NUM_INPUTS && pios_rcvr_max_channel < NELEMENTS(pios_rcvr_channel_to_id_map);
i++) {
pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].id = pios_pwm_rcvr_id;
pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].channel = i;
pios_rcvr_max_channel++;
}
#endif /* PIOS_INCLUDE_PWM */
break;
case MANUALCONTROLSETTINGS_INPUTMODE_PPM:
#if defined(PIOS_INCLUDE_PPM)
PIOS_PPM_Init();
uint32_t pios_ppm_rcvr_id;
if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, 0)) {
PIOS_Assert(0);
}
for (uint8_t i = 0;
i < PIOS_PPM_NUM_INPUTS && pios_rcvr_max_channel < NELEMENTS(pios_rcvr_channel_to_id_map);
i++) {
pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].id = pios_ppm_rcvr_id;
pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].channel = i;
pios_rcvr_max_channel++;
}
#endif /* PIOS_INCLUDE_PPM */
break;
case MANUALCONTROLSETTINGS_INPUTMODE_SPEKTRUM:
#if defined(PIOS_INCLUDE_SPEKTRUM)
if (hwsettings_cc_mainport == HWSETTINGS_CC_MAINPORT_SPEKTRUM ||
hwsettings_cc_flexiport == HWSETTINGS_CC_FLEXIPORT_SPEKTRUM) {
uint32_t pios_spektrum_rcvr_id;
if (PIOS_RCVR_Init(&pios_spektrum_rcvr_id, &pios_spektrum_rcvr_driver, 0)) {
PIOS_Assert(0);
}
for (uint8_t i = 0;
i < PIOS_SPEKTRUM_NUM_INPUTS && pios_rcvr_max_channel < NELEMENTS(pios_rcvr_channel_to_id_map);
i++) {
pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].id = pios_spektrum_rcvr_id;
pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].channel = i;
pios_rcvr_max_channel++;
}
}
#endif /* PIOS_INCLUDE_SPEKTRUM */
break;
case MANUALCONTROLSETTINGS_INPUTMODE_SBUS:
#if defined(PIOS_INCLUDE_SBUS)
if (hwsettings_cc_mainport == HWSETTINGS_CC_MAINPORT_SBUS) {
uint32_t pios_sbus_rcvr_id;
if (PIOS_RCVR_Init(&pios_sbus_rcvr_id, &pios_sbus_rcvr_driver, 0)) {
PIOS_Assert(0);
}
for (uint8_t i = 0;
i < SBUS_NUMBER_OF_CHANNELS && pios_rcvr_max_channel < NELEMENTS(pios_rcvr_channel_to_id_map);
i++) {
pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].id = pios_sbus_rcvr_id;
pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].channel = i;
pios_rcvr_max_channel++;
}
}
#endif /* PIOS_INCLUDE_SBUS */
break;
}
/* Remap AFIO pin */
GPIO_PinRemapConfig( GPIO_Remap_SWJ_NoJTRST, ENABLE);
@ -727,26 +1151,22 @@ void PIOS_Board_Init(void) {
PIOS_ADC_Init();
PIOS_GPIO_Init();
#if defined(PIOS_INCLUDE_PWM)
PIOS_PWM_Init();
#endif
#if defined(PIOS_INCLUDE_PPM)
PIOS_PPM_Init();
#endif
#if defined(PIOS_INCLUDE_USB_HID)
PIOS_USB_HID_Init(0);
uint32_t pios_usb_hid_id;
PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_main_cfg);
#if defined(PIOS_INCLUDE_COM)
if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_com_driver, 0)) {
PIOS_DEBUG_Assert(0);
uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN);
uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_TX_BUF_LEN);
PIOS_Assert(rx_buffer);
PIOS_Assert(tx_buffer);
if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_com_driver, pios_usb_hid_id,
rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN,
tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) {
PIOS_Assert(0);
}
#endif /* PIOS_INCLUDE_COM */
#endif
#endif /* PIOS_INCLUDE_USB_HID */
#if defined(PIOS_INCLUDE_I2C)
if (PIOS_I2C_Init(&pios_i2c_main_adapter_id, &pios_i2c_main_adapter_cfg)) {
PIOS_DEBUG_Assert(0);
}
#endif /* PIOS_INCLUDE_I2C */
PIOS_IAP_Init();
PIOS_WDG_Init();
}

62
flight/INS/pios_board.c
View File

@ -61,7 +61,6 @@ static const struct pios_spi_cfg pios_spi_op_mag_cfg = {
.ahb_clk = RCC_AHBPeriph_DMA1,
.irq = {
.handler = PIOS_SPI_op_mag_irq_handler,
.flags =
(DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 |
DMA1_FLAG_GL4),
@ -175,7 +174,6 @@ static const struct pios_spi_cfg pios_spi_accel_cfg = {
.ahb_clk = RCC_AHBPeriph_DMA1,
.irq = {
.handler = PIOS_SPI_accel_irq_handler,
.flags = (DMA1_FLAG_TC2 | DMA1_FLAG_TE2 | DMA1_FLAG_HT2 | DMA1_FLAG_GL2),
.init = {
.NVIC_IRQChannel = DMA1_Channel2_IRQn,
@ -265,17 +263,10 @@ void PIOS_SPI_accel_irq_handler(void)
/*
* GPS USART
*/
void PIOS_USART_gps_irq_handler(void);
void USART1_IRQHandler()
__attribute__ ((alias("PIOS_USART_gps_irq_handler")));
const struct pios_usart_cfg pios_usart_gps_cfg = {
static const struct pios_usart_cfg pios_usart_gps_cfg = {
.regs = USART1,
.init = {
#if defined (PIOS_USART_BAUDRATE)
.USART_BaudRate = PIOS_USART_BAUDRATE,
#else
.USART_BaudRate = 57600,
#endif
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1,
@ -284,7 +275,6 @@ const struct pios_usart_cfg pios_usart_gps_cfg = {
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
},
.irq = {
.handler = PIOS_USART_gps_irq_handler,
.init = {
.NVIC_IRQChannel = USART1_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
@ -310,29 +300,16 @@ const struct pios_usart_cfg pios_usart_gps_cfg = {
},
};
static uint32_t pios_usart_gps_id;
void PIOS_USART_gps_irq_handler(void)
{
PIOS_USART_IRQ_Handler(pios_usart_gps_id);
}
#endif /* PIOS_INCLUDE_GPS */
#ifdef PIOS_COM_AUX
/*
* AUX USART
*/
void PIOS_USART_aux_irq_handler(void);
void USART4_IRQHandler()
__attribute__ ((alias("PIOS_USART_aux_irq_handler")));
const struct pios_usart_cfg pios_usart_aux_cfg = {
static const struct pios_usart_cfg pios_usart_aux_cfg = {
.regs = USART4,
.init = {
#if defined (PIOS_USART_BAUDRATE)
.USART_BaudRate = PIOS_USART_BAUDRATE,
#else
.USART_BaudRate = 57600,
#endif
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1,
@ -341,7 +318,6 @@ const struct pios_usart_cfg pios_usart_aux_cfg = {
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
},
.irq = {
.handler = PIOS_USART_aux_irq_handler,
.init = {
.NVIC_IRQChannel = USART4_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
@ -367,15 +343,24 @@ const struct pios_usart_cfg pios_usart_aux_cfg = {
},
};
static uint32_t pios_usart_aux_id;
void PIOS_USART_aux_irq_handler(void)
{
PIOS_USART_IRQ_Handler(pios_usart_aux_id);
}
#endif /* PIOS_COM_AUX */
#if defined(PIOS_INCLUDE_COM)
#include <pios_com_priv.h>
#if 0
#define PIOS_COM_AUX_TX_BUF_LEN 192
static uint8_t pios_com_aux_tx_buffer[PIOS_COM_AUX_TX_BUF_LEN];
#endif
#define PIOS_COM_GPS_RX_BUF_LEN 96
static uint8_t pios_com_gps_rx_buffer[PIOS_COM_GPS_RX_BUF_LEN];
#endif /* PIOS_INCLUDE_COM */
#if defined(PIOS_INCLUDE_I2C)
#include <pios_i2c_priv.h>
@ -391,7 +376,7 @@ void I2C1_EV_IRQHandler()
void I2C1_ER_IRQHandler()
__attribute__ ((alias("PIOS_I2C_pres_mag_adapter_er_irq_handler")));
const struct pios_i2c_adapter_cfg pios_i2c_pres_mag_adapter_cfg = {
static const struct pios_i2c_adapter_cfg pios_i2c_pres_mag_adapter_cfg = {
.regs = I2C1,
.init = {
.I2C_Mode = I2C_Mode_I2C,
@ -419,7 +404,6 @@ const struct pios_i2c_adapter_cfg pios_i2c_pres_mag_adapter_cfg = {
},
},
.event = {
.handler = PIOS_I2C_pres_mag_adapter_ev_irq_handler,
.flags = 0, /* FIXME: check this */
.init = {
.NVIC_IRQChannel = I2C1_EV_IRQn,
@ -429,7 +413,6 @@ const struct pios_i2c_adapter_cfg pios_i2c_pres_mag_adapter_cfg = {
},
},
.error = {
.handler = PIOS_I2C_pres_mag_adapter_er_irq_handler,
.flags = 0, /* FIXME: check this */
.init = {
.NVIC_IRQChannel = I2C1_ER_IRQn,
@ -459,7 +442,7 @@ void PIOS_I2C_gyro_adapter_er_irq_handler(void);
void I2C2_EV_IRQHandler() __attribute__ ((alias ("PIOS_I2C_gyro_adapter_ev_irq_handler")));
void I2C2_ER_IRQHandler() __attribute__ ((alias ("PIOS_I2C_gyro_adapter_er_irq_handler")));
const struct pios_i2c_adapter_cfg pios_i2c_gyro_adapter_cfg = {
static const struct pios_i2c_adapter_cfg pios_i2c_gyro_adapter_cfg = {
.regs = I2C2,
.init = {
.I2C_Mode = I2C_Mode_I2C,
@ -487,7 +470,6 @@ const struct pios_i2c_adapter_cfg pios_i2c_gyro_adapter_cfg = {
},
},
.event = {
.handler = PIOS_I2C_gyro_adapter_ev_irq_handler,
.flags = 0, /* FIXME: check this */
.init = {
.NVIC_IRQChannel = I2C2_EV_IRQn,
@ -497,7 +479,6 @@ const struct pios_i2c_adapter_cfg pios_i2c_gyro_adapter_cfg = {
},
},
.error = {
.handler = PIOS_I2C_gyro_adapter_er_irq_handler,
.flags = 0, /* FIXME: check this */
.init = {
.NVIC_IRQChannel = I2C2_ER_IRQn,
@ -547,10 +528,13 @@ void PIOS_Board_Init(void) {
#if defined(PIOS_INCLUDE_COM)
#if defined(PIOS_INCLUDE_GPS)
uint32_t pios_usart_gps_id;
if (PIOS_USART_Init(&pios_usart_gps_id, &pios_usart_gps_cfg)) {
PIOS_DEBUG_Assert(0);
}
if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id)) {
if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id,
pios_com_gps_rx_buffer, sizeof(pios_com_gps_rx_buffer),
NULL, 0)) {
PIOS_DEBUG_Assert(0);
}
#endif /* PIOS_INCLUDE_GPS */

2
flight/Libraries/inc/fifo_buffer.h
View File

@ -26,7 +26,7 @@
#ifndef _FIFO_BUFFER_H_
#define _FIFO_BUFFER_H_
#include "stm32f10x.h"
#include <stdint.h>
// *********************

13
flight/Modules/AHRSComms/ahrs_comms.c
View File

@ -69,7 +69,7 @@ static void ahrscommsTask(void *parameters);
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t AHRSCommsInitialize(void)
int32_t AHRSCommsStart(void)
{
// Start main task
xTaskCreate(ahrscommsTask, (signed char *)"AHRSComms", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle);
@ -79,6 +79,17 @@ int32_t AHRSCommsInitialize(void)
return 0;
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t AHRSCommsInitialize(void)
{
return 0;
}
MODULE_INITCALL(AHRSCommsInitialize, AHRSCommsStart)
/**
* Module thread, should not return.
*/

21
flight/Modules/Actuator/actuator.c
View File

@ -85,6 +85,19 @@ typedef struct {
int8_t matrix[5];
} __attribute__((packed)) Mixer_t;
/**
* @brief Module initialization
* @return 0
*/
int32_t ActuatorStart()
{
// Start main task
xTaskCreate(actuatorTask, (signed char*)"Actuator", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_ACTUATOR, taskHandle);
PIOS_WDG_RegisterFlag(PIOS_WDG_ACTUATOR);
return 0;
}
/**
* @brief Module initialization
@ -100,14 +113,10 @@ int32_t ActuatorInitialize()
// If settings change, update the output rate
ActuatorSettingsConnectCallback(actuator_update_rate);
// Start main task
xTaskCreate(actuatorTask, (signed char*)"Actuator", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_ACTUATOR, taskHandle);
PIOS_WDG_RegisterFlag(PIOS_WDG_ACTUATOR);
return 0;
}
MODULE_INITCALL(ActuatorInitialize, ActuatorStart)
/**
* @brief Main Actuator module task

15
flight/Modules/Altitude/altitude.c
View File

@ -37,6 +37,7 @@
*/
#include "openpilot.h"
#include "altitude.h"
#include "baroaltitude.h" // object that will be updated by the module
#if defined(PIOS_INCLUDE_HCSR04)
#include "sonaraltitude.h" // object that will be updated by the module
@ -66,12 +67,22 @@ static void altitudeTask(void *parameters);
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t AltitudeInitialize()
int32_t AltitudeStart()
{
// Start main task
xTaskCreate(altitudeTask, (signed char *)"Altitude", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_ALTITUDE, taskHandle);
return 0;
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t AltitudeInitialize()
{
// init down-sampling data
alt_ds_temp = 0;
alt_ds_pres = 0;
@ -79,7 +90,7 @@ int32_t AltitudeInitialize()
return 0;
}
MODULE_INITCALL(AltitudeInitialize, AltitudeStart)
/**
* Module thread, should not return.
*/

230
flight/Modules/Attitude/attitude.c
View File

@ -3,7 +3,7 @@
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup Attitude Copter Control Attitude Estimation
* @brief Acquires sensor data and computes attitude estimate
* @brief Acquires sensor data and computes attitude estimate
* Specifically updates the the @ref AttitudeActual "AttitudeActual" and @ref AttitudeRaw "AttitudeRaw" settings objects
* @{
*
@ -76,7 +76,7 @@ static void AttitudeTask(void *parameters);
static float gyro_correct_int[3] = {0,0,0};
static xQueueHandle gyro_queue;
static void updateSensors(AttitudeRawData *);
static int8_t updateSensors(AttitudeRawData *);
static void updateAttitude(AttitudeRawData *);
static void settingsUpdatedCb(UAVObjEvent * objEv);
@ -89,6 +89,22 @@ static float q[4] = {1,0,0,0};
static float R[3][3];
static int8_t rotate = 0;
static bool zero_during_arming = false;
static bool bias_correct_gyro = true;
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t AttitudeStart(void)
{
// Start main task
xTaskCreate(AttitudeTask, (signed char *)"Attitude", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_ATTITUDE, taskHandle);
PIOS_WDG_RegisterFlag(PIOS_WDG_ATTITUDE);
return 0;
}
/**
* Initialise the module, called on startup
@ -104,91 +120,125 @@ int32_t AttitudeInitialize(void)
attitude.q3 = 0;
attitude.q4 = 0;
AttitudeActualSet(&attitude);
// Cannot trust the values to init right above if BL runs
gyro_correct_int[0] = 0;
gyro_correct_int[1] = 0;
gyro_correct_int[2] = 0;
q[0] = 1;
q[1] = 0;
q[2] = 0;
q[3] = 0;
for(uint8_t i = 0; i < 3; i++)
for(uint8_t j = 0; j < 3; j++)
R[i][j] = 0;
// Create queue for passing gyro data, allow 2 back samples in case
gyro_queue = xQueueCreate(1, sizeof(float) * 4);
if(gyro_queue == NULL)
if(gyro_queue == NULL)
return -1;
PIOS_ADC_SetQueue(gyro_queue);
AttitudeSettingsConnectCallback(&settingsUpdatedCb);
// Start main task
xTaskCreate(AttitudeTask, (signed char *)"Attitude", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_ATTITUDE, taskHandle);
PIOS_WDG_RegisterFlag(PIOS_WDG_ATTITUDE);
return 0;
}
MODULE_INITCALL(AttitudeInitialize, AttitudeStart)
/**
* Module thread, should not return.
*/
static void AttitudeTask(void *parameters)
{
uint8_t init = 0;
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
PIOS_ADC_Config((PIOS_ADC_RATE / 1000.0f) * UPDATE_RATE);
// Keep flash CS pin high while talking accel
PIOS_FLASH_DISABLE;
PIOS_FLASH_DISABLE;
PIOS_ADXL345_Init();
zero_during_arming = false;
// Set critical error and wait until the accel is producing data
while(PIOS_ADXL345_FifoElements() == 0) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL);
PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
}
// Force settings update to make sure rotation loaded
settingsUpdatedCb(AttitudeSettingsHandle());
// Main task loop
while (1) {
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if(xTaskGetTickCount() < 7000) {
// Force settings update to make sure rotation loaded
settingsUpdatedCb(AttitudeSettingsHandle());
if((xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) {
// For first 7 seconds use accels to get gyro bias
accelKp = 1;
accelKi = 0.9;
yawBiasRate = 0.23;
init = 0;
}
}
else if (zero_during_arming && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
accelKp = 1;
accelKi = 0.9;
yawBiasRate = 0.23;
init = 0;
init = 0;
} else if (init == 0) {
settingsUpdatedCb(AttitudeSettingsHandle());
// Reload settings (all the rates)
AttitudeSettingsAccelKiGet(&accelKi);
AttitudeSettingsAccelKpGet(&accelKp);
AttitudeSettingsYawBiasRateGet(&yawBiasRate);
init = 1;
}
}
PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
AttitudeRawData attitudeRaw;
AttitudeRawGet(&attitudeRaw);
updateSensors(&attitudeRaw);
updateAttitude(&attitudeRaw);
AttitudeRawSet(&attitudeRaw);
AttitudeRawGet(&attitudeRaw);
if(updateSensors(&attitudeRaw) != 0)
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
else {
// Only update attitude when sensor data is good
updateAttitude(&attitudeRaw);
AttitudeRawSet(&attitudeRaw);
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
}
}
}
static void updateSensors(AttitudeRawData * attitudeRaw)
{
/**
* Get an update from the sensors
* @param[in] attitudeRaw Populate the UAVO instead of saving right here
* @return 0 if successfull, -1 if not
*/
static int8_t updateSensors(AttitudeRawData * attitudeRaw)
{
struct pios_adxl345_data accel_data;
float gyro[4];
// Only wait the time for two nominal updates before setting an alarm
if(xQueueReceive(gyro_queue, (void * const) gyro, UPDATE_RATE * 2) == errQUEUE_EMPTY) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
return;
return -1;
}
// No accel data available
if(PIOS_ADXL345_FifoElements() == 0)
return -1;
// First sample is temperature
attitudeRaw->gyros[ATTITUDERAW_GYROS_X] = -(gyro[1] - GYRO_NEUTRAL) * gyroGain;
attitudeRaw->gyros[ATTITUDERAW_GYROS_Y] = (gyro[2] - GYRO_NEUTRAL) * gyroGain;
attitudeRaw->gyros[ATTITUDERAW_GYROS_Z] = -(gyro[3] - GYRO_NEUTRAL) * gyroGain;
int32_t x = 0;
int32_t y = 0;
int32_t z = 0;
@ -203,9 +253,9 @@ static void updateSensors(AttitudeRawData * attitudeRaw)
} while ( (i < 32) && (samples_remaining > 0) );
attitudeRaw->gyrotemp[0] = samples_remaining;
attitudeRaw->gyrotemp[1] = i;
float accel[3] = {(float) x / i, (float) y / i, (float) z / i};
if(rotate) {
// TODO: rotate sensors too so stabilization is well behaved
float vec_out[3];
@ -221,68 +271,71 @@ static void updateSensors(AttitudeRawData * attitudeRaw)
attitudeRaw->accels[0] = accel[0];
attitudeRaw->accels[1] = accel[1];
attitudeRaw->accels[2] = accel[2];
}
}
// Scale accels and correct bias
attitudeRaw->accels[ATTITUDERAW_ACCELS_X] = (attitudeRaw->accels[ATTITUDERAW_ACCELS_X] - accelbias[0]) * 0.004f * 9.81f;
attitudeRaw->accels[ATTITUDERAW_ACCELS_Y] = (attitudeRaw->accels[ATTITUDERAW_ACCELS_Y] - accelbias[1]) * 0.004f * 9.81f;
attitudeRaw->accels[ATTITUDERAW_ACCELS_Z] = (attitudeRaw->accels[ATTITUDERAW_ACCELS_Z] - accelbias[2]) * 0.004f * 9.81f;
if(bias_correct_gyro) {
// Applying integral component here so it can be seen on the gyros and correct bias
attitudeRaw->gyros[ATTITUDERAW_GYROS_X] += gyro_correct_int[0];
attitudeRaw->gyros[ATTITUDERAW_GYROS_Y] += gyro_correct_int[1];
attitudeRaw->gyros[ATTITUDERAW_GYROS_Z] += gyro_correct_int[2];
}
// Because most crafts wont get enough information from gravity to zero yaw gyro, we try
// and make it average zero (weakly)
gyro_correct_int[2] += - attitudeRaw->gyros[ATTITUDERAW_GYROS_Z] * yawBiasRate;
// Applying integral component here so it can be seen on the gyros and correct bias
attitudeRaw->gyros[ATTITUDERAW_GYROS_X] += gyro_correct_int[0];
attitudeRaw->gyros[ATTITUDERAW_GYROS_Y] += gyro_correct_int[1];
// Because most crafts wont get enough information from gravity to zero yaw gyro
attitudeRaw->gyros[ATTITUDERAW_GYROS_Z] += gyro_correct_int[2];
gyro_correct_int[2] += - attitudeRaw->gyros[ATTITUDERAW_GYROS_Z] * yawBiasRate;
return 0;
}
static void updateAttitude(AttitudeRawData * attitudeRaw)
{
float dT;
portTickType thisSysTime = xTaskGetTickCount();
static portTickType lastSysTime = 0;
static portTickType thisSysTime;
static float dT = 0;
thisSysTime = xTaskGetTickCount();
if(thisSysTime > lastSysTime) // reuse dt in case of wraparound
dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;
dT = (thisSysTime == lastSysTime) ? 0.001 : (portMAX_DELAY & (thisSysTime - lastSysTime)) / portTICK_RATE_MS / 1000.0f;
lastSysTime = thisSysTime;
// Bad practice to assume structure order, but saves memory
float gyro[3];
gyro[0] = attitudeRaw->gyros[0];
gyro[1] = attitudeRaw->gyros[1];
gyro[2] = attitudeRaw->gyros[2];
{
float * accels = attitudeRaw->accels;
float grot[3];
float accel_err[3];
// Rotate gravity to body frame and cross with accels
grot[0] = -(2 * (q[1] * q[3] - q[0] * q[2]));
grot[1] = -(2 * (q[2] * q[3] + q[0] * q[1]));
grot[2] = -(q[0] * q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]);
CrossProduct((const float *) accels, (const float *) grot, accel_err);
// Account for accel magnitude
// Account for accel magnitude
float accel_mag = sqrt(accels[0]*accels[0] + accels[1]*accels[1] + accels[2]*accels[2]);
accel_err[0] /= accel_mag;
accel_err[1] /= accel_mag;
accel_err[2] /= accel_mag;
// Accumulate integral of error. Scale here so that units are (rad/s) but Ki has units of s
// Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s
gyro_correct_int[0] += accel_err[0] * accelKi;
gyro_correct_int[1] += accel_err[1] * accelKi;
//gyro_correct_int[2] += accel_err[2] * settings.AccelKI * dT;
// Correct rates based on error, integral component dealt with in updateSensors
gyro[0] += accel_err[0] * accelKp / dT;
gyro[1] += accel_err[1] * accelKp / dT;
gyro[2] += accel_err[2] * accelKp / dT;
}
{ // scoping variables to save memory
// Work out time derivative from INSAlgo writeup
// Also accounts for the fact that gyros are in deg/s
@ -291,28 +344,44 @@ static void updateAttitude(AttitudeRawData * attitudeRaw)
qdot[1] = (q[0] * gyro[0] - q[3] * gyro[1] + q[2] * gyro[2]) * dT * M_PI / 180 / 2;
qdot[2] = (q[3] * gyro[0] + q[0] * gyro[1] - q[1] * gyro[2]) * dT * M_PI / 180 / 2;
qdot[3] = (-q[2] * gyro[0] + q[1] * gyro[1] + q[0] * gyro[2]) * dT * M_PI / 180 / 2;
// Take a time step
q[0] = q[0] + qdot[0];
q[1] = q[1] + qdot[1];
q[2] = q[2] + qdot[2];
q[3] = q[3] + qdot[3];
if(q[0] < 0) {
q[0] = -q[0];
q[1] = -q[1];
q[2] = -q[2];
q[3] = -q[3];
}
}
// Renomalize
float qmag = sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
q[0] = q[0] / qmag;
q[1] = q[1] / qmag;
q[2] = q[2] / qmag;
q[3] = q[3] / qmag;
// If quaternion has become inappropriately short or is nan reinit.
// THIS SHOULD NEVER ACTUALLY HAPPEN
if((fabs(qmag) < 1e-3) || (qmag != qmag)) {
q[0] = 1;
q[1] = 0;
q[2] = 0;
q[3] = 0;
}
AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual);
quat_copy(q, &attitudeActual.q1);
// Convert into eueler degrees (makes assumptions about RPY order)
Quaternion2RPY(&attitudeActual.q1,&attitudeActual.Roll);
Quaternion2RPY(&attitudeActual.q1,&attitudeActual.Roll);
AttitudeActualSet(&attitudeActual);
}
@ -320,36 +389,41 @@ static void updateAttitude(AttitudeRawData * attitudeRaw)
static void settingsUpdatedCb(UAVObjEvent * objEv) {
AttitudeSettingsData attitudeSettings;
AttitudeSettingsGet(&attitudeSettings);
accelKp = attitudeSettings.AccelKp;
accelKi = attitudeSettings.AccelKi;
accelKi = attitudeSettings.AccelKi;
yawBiasRate = attitudeSettings.YawBiasRate;
gyroGain = attitudeSettings.GyroGain;
zero_during_arming = attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE;
bias_correct_gyro = attitudeSettings.BiasCorrectGyro == ATTITUDESETTINGS_BIASCORRECTGYRO_TRUE;
accelbias[0] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_X];
accelbias[1] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Y];
accelbias[2] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Z];
gyro_correct_int[0] = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_X] / 100.0f;
gyro_correct_int[1] = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_Y] / 100.0f;
gyro_correct_int[2] = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_Z] / 100.0f;
// Indicates not to expend cycles on rotation
if(attitudeSettings.BoardRotation[0] == 0 && attitudeSettings.BoardRotation[1] == 0 &&
attitudeSettings.BoardRotation[2] == 0) {
rotate = 0;
// Shouldn't be used but to be safe
float rotationQuat[4] = {1,0,0,0};
Quaternion2R(rotationQuat, R);
} else {
float rotationQuat[4];
const float rpy[3] = {attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_ROLL],
attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_PITCH],
const float rpy[3] = {attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_ROLL],
attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_PITCH],
attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_YAW]};
RPY2Quaternion(rpy, rotationQuat);
Quaternion2R(rotationQuat, R);
rotate = 1;
}
}
}
/**
* @}

2
flight/Modules/Battery/battery.c
View File

@ -75,6 +75,8 @@ static void onTimer(UAVObjEvent* ev);
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
MODULE_INITCALL(BatteryInitialize, 0)
int32_t BatteryInitialize(void)
{
static UAVObjEvent ev;

9
flight/Modules/Example/example.c
View File

@ -49,9 +49,14 @@
#include "examplemodperiodic.h"
#include "examplemodthread.h"
void ExampleInitialize(void)
void ExampleStart(void)
{
ExampleModEventInitialize();
ExampleModPeriodicInitialize();
ExampleModThreadInitialize();
}
void ExampleInitialize(void)
{
ExampleModEventInitialize();
}
MODULE_INITCALL(ExampleInitialize, ExampleStart)

4
flight/Modules/FirmwareIAP/firmwareiap.c
View File

@ -88,7 +88,7 @@ static void resetTask(UAVObjEvent *);
* \note
*
*/
MODULE_INITCALL(FirmwareIAPInitialize, 0)
int32_t FirmwareIAPInitialize()
{
const struct pios_board_info * bdinfo = &pios_board_info_blob;
@ -100,7 +100,7 @@ int32_t FirmwareIAPInitialize()
data.ArmReset=0;
data.crc = 0;
FirmwareIAPObjSet( &data );
FirmwareIAPObjConnectCallback( &FirmwareIAPCallback );
if(bdinfo->magic==PIOS_BOARD_INFO_BLOB_MAGIC) FirmwareIAPObjConnectCallback( &FirmwareIAPCallback );
return 0;
}

21
flight/Modules/FlightPlan/flightplan.c
View File

@ -31,6 +31,7 @@
#include "openpilot.h"
#include "pm.h"
#include "flightplan.h"
#include "flightplanstatus.h"
#include "flightplancontrol.h"
#include "flightplansettings.h"
@ -53,6 +54,20 @@ static void objectUpdatedCb(UAVObjEvent * ev);
// External variables (temporary, TODO: this will be loaded from the SD card)
extern unsigned char usrlib_img[];
/**
* Module initialization
*/
int32_t FlightPlanStart()
{
taskHandle = NULL;
// Start VM thread
xTaskCreate(flightPlanTask, (signed char *)"FlightPlan", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_FLIGHTPLAN, taskHandle);
return 0;
}
/**
* Module initialization
*/
@ -69,13 +84,9 @@ int32_t FlightPlanInitialize()
// Listen for FlightPlanControl updates
FlightPlanControlConnectQueue(queue);
// Start VM thread
xTaskCreate(flightPlanTask, (signed char *)"FlightPlan", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_FLIGHTPLAN, taskHandle);
return 0;
}
MODULE_INITCALL(FlightPlanInitialize, FlightPlanStart)
/**
* Module task
*/

31
flight/Modules/GPS/GPS.c
View File

@ -109,19 +109,27 @@ static uint32_t numParsingErrors;
* \return 0 on success
*/
int32_t GPSInitialize(void)
int32_t GPSStart(void)
{
signed portBASE_TYPE xReturn;
// TODO: Get gps settings object
gpsPort = PIOS_COM_GPS;
// Start gps task
xReturn = xTaskCreate(gpsTask, (signed char *)"GPS", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &gpsTaskHandle);
xTaskCreate(gpsTask, (signed char *)"GPS", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &gpsTaskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_GPS, gpsTaskHandle);
return 0;
}
/**
* Initialise the gps module
* \return -1 if initialisation failed
* \return 0 on success
*/
int32_t GPSInitialize(void)
{
// TODO: Get gps settings object
gpsPort = PIOS_COM_GPS;
return 0;
}
MODULE_INITCALL(GPSInitialize, GPSStart)
// ****************
/**
@ -188,8 +196,10 @@ static void gpsTask(void *parameters)
while (PIOS_COM_ReceiveBufferUsed(gpsPort) > 0)
{
int res = GTOP_BIN_update_position(PIOS_COM_ReceiveBuffer(gpsPort), &numChecksumErrors, &numParsingErrors);
if (res >= 0)
uint8_t c;
PIOS_COM_ReceiveBuffer(gpsPort, &c, 1, 0);
if (GTOP_BIN_update_position(c, &numChecksumErrors, &numParsingErrors) >= 0)
{
numUpdates++;
@ -205,7 +215,8 @@ static void gpsTask(void *parameters)
// This blocks the task until there is something on the buffer
while (PIOS_COM_ReceiveBufferUsed(gpsPort) > 0)
{
char c = PIOS_COM_ReceiveBuffer(gpsPort);
uint8_t c;
PIOS_COM_ReceiveBuffer(gpsPort, &c, 1, 0);
// detect start while acquiring stream
if (!start_flag && (c == '$'))

18
flight/Modules/Guidance/guidance.c
View File

@ -78,6 +78,19 @@ static void updateVtolDesiredVelocity();
static void manualSetDesiredVelocity();
static void updateVtolDesiredAttitude();
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t GuidanceStart()
{
// Start main task
xTaskCreate(guidanceTask, (signed char *)"Guidance", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &guidanceTaskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_GUIDANCE, guidanceTaskHandle);
return 0;
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
@ -90,12 +103,9 @@ int32_t GuidanceInitialize()
// Listen for updates.
AttitudeRawConnectQueue(queue);
// Start main task
xTaskCreate(guidanceTask, (signed char *)"Guidance", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &guidanceTaskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_GUIDANCE, guidanceTaskHandle);
return 0;
}
MODULE_INITCALL(GuidanceInitialize, GuidanceStart)
static float northVelIntegral = 0;
static float eastVelIntegral = 0;

2
flight/Modules/Guidance/inc/guidance.h
View File

@ -27,5 +27,5 @@
#define EXAMPLEMODPERIODIC_H
int32_t ExampleModPeriodicInitialize();
int32_t GuidanceInitialize(void);
#endif // EXAMPLEMODPERIODIC_H

62
flight/Modules/ManualControl/inc/manualcontrol.h
View File

@ -1,7 +1,7 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @{
* @addtogroup ManualControlModule Manual Control Module
* @{
*
@ -45,4 +45,64 @@ typedef enum {FLIGHTMODE_UNDEFINED = 0, FLIGHTMODE_MANUAL = 1, FLIGHTMODE_STABIL
int32_t ManualControlInitialize();
/*
* These are assumptions we make in the flight code about the order of settings and their consistency between
* objects. Please keep this synchronized to the UAVObjects
*/
#define assumptions1 ( \
((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \
)
#define assumptions3 ( \
((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \
)
#define assumptions5 ( \
((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \
)
#define ARMING_CHANNEL_ROLL 0
#define ARMING_CHANNEL_PITCH 1
#define ARMING_CHANNEL_YAW 2
#define assumptions7 ( \
( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_ROLL) && \
( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_ROLL) && \
( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHFORWARD -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_PITCH) && \
( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHAFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_PITCH) && \
( ((int)MANUALCONTROLSETTINGS_ARMING_YAWLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_YAW) && \
( ((int)MANUALCONTROLSETTINGS_ARMING_YAWRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_YAW) \
)
#define assumptions8 ( \
( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 == 0) && \
( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 != 0) && \
( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHFORWARD -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 == 0) && \
( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHAFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 != 0) && \
( ((int)MANUALCONTROLSETTINGS_ARMING_YAWLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 == 0) && \
( ((int)MANUALCONTROLSETTINGS_ARMING_YAWRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 != 0) \
)
#define assumptions_flightmode ( \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_MANUAL == (int) FLIGHTSTATUS_FLIGHTMODE_MANUAL) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED1 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED1) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED2 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED2) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED3 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED3) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL == (int) FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD == (int) FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) \
)
#endif // MANUALCONTROL_H

1215
flight/Modules/ManualControl/manualcontrol.c
View File

@ -1,628 +1,587 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup ManualControlModule Manual Control Module
* @brief Provide manual control or allow it alter flight mode.
* @{
*
* Reads in the ManualControlCommand FlightMode setting from receiver then either
* pass the settings straght to ActuatorDesired object (manual mode) or to
* AttitudeDesired object (stabilized mode)
*
* @file manualcontrol.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief ManualControl module. Handles safety R/C link and flight mode.
*
* @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
*/
#include "openpilot.h"
#include "manualcontrol.h"
#include "manualcontrolsettings.h"
#include "stabilizationsettings.h"
#include "manualcontrolcommand.h"
#include "actuatordesired.h"
#include "stabilizationdesired.h"
#include "flighttelemetrystats.h"
#include "flightstatus.h"
#include "accessorydesired.h"
// Private constants
#if defined(PIOS_MANUAL_STACK_SIZE)
#define STACK_SIZE_BYTES PIOS_MANUAL_STACK_SIZE
#else
#define STACK_SIZE_BYTES 824
#endif
#define TASK_PRIORITY (tskIDLE_PRIORITY+4)
#define UPDATE_PERIOD_MS 20
#define THROTTLE_FAILSAFE -0.1
#define FLIGHT_MODE_LIMIT 1.0/3.0
#define ARMED_TIME_MS 1000
#define ARMED_THRESHOLD 0.50
//safe band to allow a bit of calibration error or trim offset (in microseconds)
#define CONNECTION_OFFSET 150
// Private types
typedef enum
{
ARM_STATE_DISARMED,
ARM_STATE_ARMING_MANUAL,
ARM_STATE_ARMED,
ARM_STATE_DISARMING_MANUAL,
ARM_STATE_DISARMING_TIMEOUT
} ArmState_t;
// Private variables
static xTaskHandle taskHandle;
static ArmState_t armState;
static portTickType lastSysTime;
// Private functions
static void updateActuatorDesired(ManualControlCommandData * cmd);
static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings);
static void processFlightMode(ManualControlSettingsData * settings, float flightMode);
static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings);
static void manualControlTask(void *parameters);
static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral);
static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time);
static bool okToArm(void);
static bool validInputRange(int16_t min, int16_t max, uint16_t value);
#define assumptions1 ( \
((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \
)
#define assumptions3 ( \
((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \
)
#define assumptions5 ( \
((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \
((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \
)
#define ARMING_CHANNEL_ROLL 0
#define ARMING_CHANNEL_PITCH 1
#define ARMING_CHANNEL_YAW 2
#define assumptions7 ( \
( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_ROLL) && \
( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_ROLL) && \
( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHFORWARD -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_PITCH) && \
( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHAFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_PITCH) && \
( ((int)MANUALCONTROLSETTINGS_ARMING_YAWLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_YAW) && \
( ((int)MANUALCONTROLSETTINGS_ARMING_YAWRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_YAW) \
)
#define assumptions8 ( \
( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 == 0) && \
( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 != 0) && \
( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHFORWARD -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 == 0) && \
( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHAFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 != 0) && \
( ((int)MANUALCONTROLSETTINGS_ARMING_YAWLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 == 0) && \
( ((int)MANUALCONTROLSETTINGS_ARMING_YAWRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 != 0) \
)
#define assumptions_flightmode ( \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_MANUAL == (int) FLIGHTSTATUS_FLIGHTMODE_MANUAL) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED1 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED1) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED2 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED2) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED3 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED3) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL == (int) FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) && \
( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD == (int) FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) \
)
#define assumptions (assumptions1 && assumptions3 && assumptions5 && assumptions7 && assumptions8 && assumptions_flightmode)
/**
* Module initialization
*/
int32_t ManualControlInitialize()
{
/* Check the assumptions about uavobject enum's are correct */
if(!assumptions)
return -1;
// Start main task
xTaskCreate(manualControlTask, (signed char *)"ManualControl", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_MANUALCONTROL, taskHandle);
PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL);
return 0;
}
/**
* Module task
*/
static void manualControlTask(void *parameters)
{
ManualControlSettingsData settings;
ManualControlCommandData cmd;
FlightStatusData flightStatus;
float flightMode = 0;
uint8_t disconnected_count = 0;
uint8_t connected_count = 0;
// For now manual instantiate extra instances of Accessory Desired. In future should be done dynamically
// this includes not even registering it if not used
AccessoryDesiredCreateInstance();
AccessoryDesiredCreateInstance();
// Make sure unarmed on power up
ManualControlCommandGet(&cmd);
FlightStatusGet(&flightStatus);
flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED;
armState = ARM_STATE_DISARMED;
// Main task loop
lastSysTime = xTaskGetTickCount();
while (1) {
float scaledChannel[MANUALCONTROLCOMMAND_CHANNEL_NUMELEM];
// Wait until next update
vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS);
PIOS_WDG_UpdateFlag(PIOS_WDG_MANUAL);
// Read settings
ManualControlSettingsGet(&settings);
if (ManualControlCommandReadOnly(&cmd)) {
FlightTelemetryStatsData flightTelemStats;
FlightTelemetryStatsGet(&flightTelemStats);
if(flightTelemStats.Status != FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
/* trying to fly via GCS and lost connection. fall back to transmitter */
UAVObjMetadata metadata;
UAVObjGetMetadata(&cmd, &metadata);
metadata.access = ACCESS_READWRITE;
UAVObjSetMetadata(&cmd, &metadata);
}
}
if (!ManualControlCommandReadOnly(&cmd)) {
// Read channel values in us
// TODO: settings.InputMode is currently ignored because PIOS will not allow runtime
// selection of PWM and PPM. The configuration is currently done at compile time in
// the pios_config.h file.
for (int n = 0; n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) {
#if defined(PIOS_INCLUDE_PWM)
cmd.Channel[n] = PIOS_PWM_Get(n);
#elif defined(PIOS_INCLUDE_PPM)
cmd.Channel[n] = PIOS_PPM_Get(n);
#elif defined(PIOS_INCLUDE_SPEKTRUM)
cmd.Channel[n] = PIOS_SPEKTRUM_Get(n);
#endif
scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n], settings.ChannelMin[n], settings.ChannelNeutral[n]);
}
// Check settings, if error raise alarm
if (settings.Roll >= MANUALCONTROLSETTINGS_ROLL_NONE ||
settings.Pitch >= MANUALCONTROLSETTINGS_PITCH_NONE ||
settings.Yaw >= MANUALCONTROLSETTINGS_YAW_NONE ||
settings.Throttle >= MANUALCONTROLSETTINGS_THROTTLE_NONE ||
settings.FlightMode >= MANUALCONTROLSETTINGS_FLIGHTMODE_NONE) {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
ManualControlCommandSet(&cmd);
continue;
}
// decide if we have valid manual input or not
bool valid_input_detected = validInputRange(settings.ChannelMin[settings.Throttle], settings.ChannelMax[settings.Throttle], cmd.Channel[settings.Throttle]) &&
validInputRange(settings.ChannelMin[settings.Roll], settings.ChannelMax[settings.Roll], cmd.Channel[settings.Roll]) &&
validInputRange(settings.ChannelMin[settings.Yaw], settings.ChannelMax[settings.Yaw], cmd.Channel[settings.Yaw]) &&
validInputRange(settings.ChannelMin[settings.Pitch], settings.ChannelMax[settings.Pitch], cmd.Channel[settings.Pitch]);
// Implement hysteresis loop on connection status
if (valid_input_detected && (++connected_count > 10)) {
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE;
connected_count = 0;
disconnected_count = 0;
} else if (!valid_input_detected && (++disconnected_count > 10)) {
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
connected_count = 0;
disconnected_count = 0;
}
if (cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) {
cmd.Throttle = -1; // Shut down engine with no control
cmd.Roll = 0;
cmd.Yaw = 0;
cmd.Pitch = 0;
//cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO; // don't do until AUTO implemented and functioning
// Important: Throttle < 0 will reset Stabilization coefficients among other things. Either change this,
// or leave throttle at IDLE speed or above when going into AUTO-failsafe.
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
ManualControlCommandSet(&cmd);
} else {
AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL);
// Scale channels to -1 -> +1 range
cmd.Roll = scaledChannel[settings.Roll];
cmd.Pitch = scaledChannel[settings.Pitch];
cmd.Yaw = scaledChannel[settings.Yaw];
cmd.Throttle = scaledChannel[settings.Throttle];
flightMode = scaledChannel[settings.FlightMode];
AccessoryDesiredData accessory;
// Set Accessory 0
if(settings.Accessory0 != MANUALCONTROLSETTINGS_ACCESSORY0_NONE) {
accessory.AccessoryVal = scaledChannel[settings.Accessory0];
if(AccessoryDesiredInstSet(0, &accessory) != 0)
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
// Set Accessory 1
if(settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY1_NONE) {
accessory.AccessoryVal = scaledChannel[settings.Accessory1];
if(AccessoryDesiredInstSet(1, &accessory) != 0)
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
// Set Accsesory 2
if(settings.Accessory2 != MANUALCONTROLSETTINGS_ACCESSORY2_NONE) {
accessory.AccessoryVal = scaledChannel[settings.Accessory2];
if(AccessoryDesiredInstSet(2, &accessory) != 0)
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
processFlightMode(&settings, flightMode);
processArm(&cmd, &settings);
// Update cmd object
ManualControlCommandSet(&cmd);
}
} else {
ManualControlCommandGet(&cmd); /* Under GCS control */
}
FlightStatusGet(&flightStatus);
// Depending on the mode update the Stabilization or Actuator objects
switch(PARSE_FLIGHT_MODE(flightStatus.FlightMode)) {
case FLIGHTMODE_UNDEFINED:
// This reflects a bug in the code architecture!
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
break;
case FLIGHTMODE_MANUAL:
updateActuatorDesired(&cmd);
break;
case FLIGHTMODE_STABILIZED:
updateStabilizationDesired(&cmd, &settings);
break;
case FLIGHTMODE_GUIDANCE:
// TODO: Implement
break;
}
}
}
static void updateActuatorDesired(ManualControlCommandData * cmd)
{
ActuatorDesiredData actuator;
ActuatorDesiredGet(&actuator);
actuator.Roll = cmd->Roll;
actuator.Pitch = cmd->Pitch;
actuator.Yaw = cmd->Yaw;
actuator.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle;
ActuatorDesiredSet(&actuator);
}
static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings)
{
StabilizationDesiredData stabilization;
StabilizationDesiredGet(&stabilization);
StabilizationSettingsData stabSettings;
StabilizationSettingsGet(&stabSettings);
uint8_t * stab_settings;
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
switch(flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
stab_settings = settings->Stabilization1Settings;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
stab_settings = settings->Stabilization2Settings;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
stab_settings = settings->Stabilization3Settings;
break;
default:
// Major error, this should not occur because only enter this block when one of these is true
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
return;
}
// TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order
stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = stab_settings[0];
stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = stab_settings[1];
stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = stab_settings[2];
stabilization.Roll = (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax :
0; // this is an invalid mode
;
stabilization.Pitch = (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax :
0; // this is an invalid mode
stabilization.Yaw = (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? fmod(cmd->Yaw * 180.0, 360) :
0; // this is an invalid mode
stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle;
StabilizationDesiredSet(&stabilization);
}
/**
* Convert channel from servo pulse duration (microseconds) to scaled -1/+1 range.
*/
static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral)
{
float valueScaled;
// Scale
if ((max > min && value >= neutral) || (min > max && value <= neutral))
{
if (max != neutral)
valueScaled = (float)(value - neutral) / (float)(max - neutral);
else
valueScaled = 0;
}
else
{
if (min != neutral)
valueScaled = (float)(value - neutral) / (float)(neutral - min);
else
valueScaled = 0;
}
// Bound
if (valueScaled > 1.0) valueScaled = 1.0;
else
if (valueScaled < -1.0) valueScaled = -1.0;
return valueScaled;
}
static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time) {
if(end_time > start_time)
return (end_time - start_time) * portTICK_RATE_MS;
return ((((portTICK_RATE_MS) -1) - start_time) + end_time) * portTICK_RATE_MS;
}
/**
* @brief Determine if the aircraft is safe to arm
* @returns True if safe to arm, false otherwise
*/
static bool okToArm(void)
{
// read alarms
SystemAlarmsData alarms;
SystemAlarmsGet(&alarms);
// Check each alarm
for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++)
{
if (alarms.Alarm[i] >= SYSTEMALARMS_ALARM_ERROR)
{ // found an alarm thats set
if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY)
continue;
return false;
}
}
return true;
}
/**
* @brief Update the flightStatus object only if value changed. Reduces callbacks
* @param[in] val The new value
*/
static void setArmedIfChanged(uint8_t val) {
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if(flightStatus.Armed != val) {
flightStatus.Armed = val;
FlightStatusSet(&flightStatus);
}
}
/**
* @brief Process the inputs and determine whether to arm or not
* @param[out] cmd The structure to set the armed in
* @param[in] settings Settings indicating the necessary position
*/
static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings)
{
bool lowThrottle = cmd->Throttle <= 0;
if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSDISARMED) {
// In this configuration we always disarm
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
} else {
// Not really needed since this function not called when disconnected
if (cmd->Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE)
return;
// The throttle is not low, in case we where arming or disarming, abort
if (!lowThrottle) {
switch(armState) {
case ARM_STATE_DISARMING_MANUAL:
case ARM_STATE_DISARMING_TIMEOUT:
armState = ARM_STATE_ARMED;
break;
case ARM_STATE_ARMING_MANUAL:
armState = ARM_STATE_DISARMED;
break;
default:
// Nothing needs to be done in the other states
break;
}
return;
}
// The rest of these cases throttle is low
if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSARMED) {
// In this configuration, we go into armed state as soon as the throttle is low, never disarm
setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED);
return;
}
// When the configuration is not "Always armed" and no "Always disarmed",
// the state will not be changed when the throttle is not low
static portTickType armedDisarmStart;
float armingInputLevel = 0;
// Calc channel see assumptions7
int8_t sign = ((settings->Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2) ? -1 : 1;
switch ( (settings->Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 ) {
case ARMING_CHANNEL_ROLL: armingInputLevel = sign * cmd->Roll; break;
case ARMING_CHANNEL_PITCH: armingInputLevel = sign * cmd->Pitch; break;
case ARMING_CHANNEL_YAW: armingInputLevel = sign * cmd->Yaw; break;
}
bool manualArm = false;
bool manualDisarm = false;
if (armingInputLevel <= -ARMED_THRESHOLD)
manualArm = true;
else if (armingInputLevel >= +ARMED_THRESHOLD)
manualDisarm = true;
switch(armState) {
case ARM_STATE_DISARMED:
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
// only allow arming if it's OK too
if (manualArm && okToArm()) {
armedDisarmStart = lastSysTime;
armState = ARM_STATE_ARMING_MANUAL;
}
break;
case ARM_STATE_ARMING_MANUAL:
setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMING);
if (manualArm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS))
armState = ARM_STATE_ARMED;
else if (!manualArm)
armState = ARM_STATE_DISARMED;
break;
case ARM_STATE_ARMED:
// When we get here, the throttle is low,
// we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled
armedDisarmStart = lastSysTime;
armState = ARM_STATE_DISARMING_TIMEOUT;
setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED);
break;
case ARM_STATE_DISARMING_TIMEOUT:
// We get here when armed while throttle low, even when the arming timeout is not enabled
if ((settings->ArmedTimeout != 0) && (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings->ArmedTimeout))
armState = ARM_STATE_DISARMED;
// Switch to disarming due to manual control when needed
if (manualDisarm) {
armedDisarmStart = lastSysTime;
armState = ARM_STATE_DISARMING_MANUAL;
}
break;
case ARM_STATE_DISARMING_MANUAL:
if (manualDisarm &&(timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS))
armState = ARM_STATE_DISARMED;
else if (!manualDisarm)
armState = ARM_STATE_ARMED;
break;
} // End Switch
}
}
/**
* @brief Determine which of three positions the flight mode switch is in and set flight mode accordingly
* @param[out] cmd Pointer to the command structure to set the flight mode in
* @param[in] settings The settings which indicate which position is which mode
* @param[in] flightMode the value of the switch position
*/
static void processFlightMode(ManualControlSettingsData * settings, float flightMode)
{
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
uint8_t newMode;
// Note here the code is ass
if (flightMode < -FLIGHT_MODE_LIMIT)
newMode = settings->FlightModePosition[0];
else if (flightMode > FLIGHT_MODE_LIMIT)
newMode = settings->FlightModePosition[2];
else
newMode = settings->FlightModePosition[1];
if(flightStatus.FlightMode != newMode) {
flightStatus.FlightMode = newMode;
FlightStatusSet(&flightStatus);
}
}
/**
* @brief Determine if the manual input value is within acceptable limits
* @returns return TRUE if so, otherwise return FALSE
*/
bool validInputRange(int16_t min, int16_t max, uint16_t value)
{
if (min > max)
{
int16_t tmp = min;
min = max;
max = tmp;
}
return (value >= min - CONNECTION_OFFSET && value <= max + CONNECTION_OFFSET);
}
/**
* @}
* @}
*/
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup ManualControlModule Manual Control Module
* @brief Provide manual control or allow it alter flight mode.
* @{
*
* Reads in the ManualControlCommand FlightMode setting from receiver then either
* pass the settings straght to ActuatorDesired object (manual mode) or to
* AttitudeDesired object (stabilized mode)
*
* @file manualcontrol.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief ManualControl module. Handles safety R/C link and flight mode.
*
* @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
*/
#include "openpilot.h"
#include "manualcontrol.h"
#include "manualcontrolsettings.h"
#include "stabilizationsettings.h"
#include "manualcontrolcommand.h"
#include "actuatordesired.h"
#include "stabilizationdesired.h"
#include "flighttelemetrystats.h"
#include "flightstatus.h"
#include "accessorydesired.h"
// Private constants
#if defined(PIOS_MANUAL_STACK_SIZE)
#define STACK_SIZE_BYTES PIOS_MANUAL_STACK_SIZE
#else
#define STACK_SIZE_BYTES 824
#endif
#define TASK_PRIORITY (tskIDLE_PRIORITY+4)
#define UPDATE_PERIOD_MS 20
#define THROTTLE_FAILSAFE -0.1
#define FLIGHT_MODE_LIMIT 1.0/3.0
#define ARMED_TIME_MS 1000
#define ARMED_THRESHOLD 0.50
//safe band to allow a bit of calibration error or trim offset (in microseconds)
#define CONNECTION_OFFSET 150
// Private types
typedef enum
{
ARM_STATE_DISARMED,
ARM_STATE_ARMING_MANUAL,
ARM_STATE_ARMED,
ARM_STATE_DISARMING_MANUAL,
ARM_STATE_DISARMING_TIMEOUT
} ArmState_t;
// Private variables
static xTaskHandle taskHandle;
static ArmState_t armState;
static portTickType lastSysTime;
// Private functions
static void updateActuatorDesired(ManualControlCommandData * cmd);
static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings);
static void processFlightMode(ManualControlSettingsData * settings, float flightMode);
static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings);
static void manualControlTask(void *parameters);
static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral);
static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time);
static bool okToArm(void);
static bool validInputRange(int16_t min, int16_t max, uint16_t value);
#define assumptions (assumptions1 && assumptions3 && assumptions5 && assumptions7 && assumptions8 && assumptions_flightmode)
/**
* Module initialization
*/
int32_t ManualControlStart()
{
/* Check the assumptions about uavobject enum's are correct */
if(!assumptions)
return -1;
// Start main task
xTaskCreate(manualControlTask, (signed char *)"ManualControl", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_MANUALCONTROL, taskHandle);
PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL);
return 0;
}
/**
* Module initialization
*/
int32_t ManualControlInitialize()
{
return 0;
}
MODULE_INITCALL(ManualControlInitialize, ManualControlStart)
/**
* Module task
*/
static void manualControlTask(void *parameters)
{
ManualControlSettingsData settings;
ManualControlCommandData cmd;
FlightStatusData flightStatus;
float flightMode = 0;
uint8_t disconnected_count = 0;
uint8_t connected_count = 0;
// For now manual instantiate extra instances of Accessory Desired. In future should be done dynamically
// this includes not even registering it if not used
AccessoryDesiredCreateInstance();
AccessoryDesiredCreateInstance();
// Make sure unarmed on power up
ManualControlCommandGet(&cmd);
FlightStatusGet(&flightStatus);
flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED;
armState = ARM_STATE_DISARMED;
// Main task loop
lastSysTime = xTaskGetTickCount();
while (1) {
float scaledChannel[MANUALCONTROLCOMMAND_CHANNEL_NUMELEM];
// Wait until next update
vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS);
PIOS_WDG_UpdateFlag(PIOS_WDG_MANUAL);
// Read settings
ManualControlSettingsGet(&settings);
if (ManualControlCommandReadOnly(&cmd)) {
FlightTelemetryStatsData flightTelemStats;
FlightTelemetryStatsGet(&flightTelemStats);
if(flightTelemStats.Status != FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
/* trying to fly via GCS and lost connection. fall back to transmitter */
UAVObjMetadata metadata;
UAVObjGetMetadata(&cmd, &metadata);
metadata.access = ACCESS_READWRITE;
UAVObjSetMetadata(&cmd, &metadata);
}
}
if (!ManualControlCommandReadOnly(&cmd)) {
// Read channel values in us
for (int n = 0; n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) {
if (pios_rcvr_channel_to_id_map[n].id) {
cmd.Channel[n] = PIOS_RCVR_Read(pios_rcvr_channel_to_id_map[n].id,
pios_rcvr_channel_to_id_map[n].channel);
} else {
cmd.Channel[n] = -1;
}
scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n], settings.ChannelMin[n], settings.ChannelNeutral[n]);
}
// Check settings, if error raise alarm
if (settings.Roll >= MANUALCONTROLSETTINGS_ROLL_NONE ||
settings.Pitch >= MANUALCONTROLSETTINGS_PITCH_NONE ||
settings.Yaw >= MANUALCONTROLSETTINGS_YAW_NONE ||
settings.Throttle >= MANUALCONTROLSETTINGS_THROTTLE_NONE ||
settings.FlightMode >= MANUALCONTROLSETTINGS_FLIGHTMODE_NONE) {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
ManualControlCommandSet(&cmd);
continue;
}
// decide if we have valid manual input or not
bool valid_input_detected = validInputRange(settings.ChannelMin[settings.Throttle], settings.ChannelMax[settings.Throttle], cmd.Channel[settings.Throttle]) &&
validInputRange(settings.ChannelMin[settings.Roll], settings.ChannelMax[settings.Roll], cmd.Channel[settings.Roll]) &&
validInputRange(settings.ChannelMin[settings.Yaw], settings.ChannelMax[settings.Yaw], cmd.Channel[settings.Yaw]) &&
validInputRange(settings.ChannelMin[settings.Pitch], settings.ChannelMax[settings.Pitch], cmd.Channel[settings.Pitch]);
// Implement hysteresis loop on connection status
if (valid_input_detected && (++connected_count > 10)) {
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE;
connected_count = 0;
disconnected_count = 0;
} else if (!valid_input_detected && (++disconnected_count > 10)) {
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
connected_count = 0;
disconnected_count = 0;
}
if (cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) {
cmd.Throttle = -1; // Shut down engine with no control
cmd.Roll = 0;
cmd.Yaw = 0;
cmd.Pitch = 0;
//cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO; // don't do until AUTO implemented and functioning
// Important: Throttle < 0 will reset Stabilization coefficients among other things. Either change this,
// or leave throttle at IDLE speed or above when going into AUTO-failsafe.
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
ManualControlCommandSet(&cmd);
} else {
AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL);
// Scale channels to -1 -> +1 range
cmd.Roll = scaledChannel[settings.Roll];
cmd.Pitch = scaledChannel[settings.Pitch];
cmd.Yaw = scaledChannel[settings.Yaw];
cmd.Throttle = scaledChannel[settings.Throttle];
flightMode = scaledChannel[settings.FlightMode];
AccessoryDesiredData accessory;
// Set Accessory 0
if(settings.Accessory0 != MANUALCONTROLSETTINGS_ACCESSORY0_NONE) {
accessory.AccessoryVal = scaledChannel[settings.Accessory0];
if(AccessoryDesiredInstSet(0, &accessory) != 0)
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
// Set Accessory 1
if(settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY1_NONE) {
accessory.AccessoryVal = scaledChannel[settings.Accessory1];
if(AccessoryDesiredInstSet(1, &accessory) != 0)
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
// Set Accsesory 2
if(settings.Accessory2 != MANUALCONTROLSETTINGS_ACCESSORY2_NONE) {
accessory.AccessoryVal = scaledChannel[settings.Accessory2];
if(AccessoryDesiredInstSet(2, &accessory) != 0)
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
processFlightMode(&settings, flightMode);
processArm(&cmd, &settings);
// Update cmd object
ManualControlCommandSet(&cmd);
}
} else {
ManualControlCommandGet(&cmd); /* Under GCS control */
}
FlightStatusGet(&flightStatus);
// Depending on the mode update the Stabilization or Actuator objects
switch(PARSE_FLIGHT_MODE(flightStatus.FlightMode)) {
case FLIGHTMODE_UNDEFINED:
// This reflects a bug in the code architecture!
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
break;
case FLIGHTMODE_MANUAL:
updateActuatorDesired(&cmd);
break;
case FLIGHTMODE_STABILIZED:
updateStabilizationDesired(&cmd, &settings);
break;
case FLIGHTMODE_GUIDANCE:
// TODO: Implement
break;
}
}
}
static void updateActuatorDesired(ManualControlCommandData * cmd)
{
ActuatorDesiredData actuator;
ActuatorDesiredGet(&actuator);
actuator.Roll = cmd->Roll;
actuator.Pitch = cmd->Pitch;
actuator.Yaw = cmd->Yaw;
actuator.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle;
ActuatorDesiredSet(&actuator);
}
static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings)
{
StabilizationDesiredData stabilization;
StabilizationDesiredGet(&stabilization);
StabilizationSettingsData stabSettings;
StabilizationSettingsGet(&stabSettings);
uint8_t * stab_settings;
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
switch(flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
stab_settings = settings->Stabilization1Settings;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
stab_settings = settings->Stabilization2Settings;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
stab_settings = settings->Stabilization3Settings;
break;
default:
// Major error, this should not occur because only enter this block when one of these is true
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
return;
}
// TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order
stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = stab_settings[0];
stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = stab_settings[1];
stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = stab_settings[2];
stabilization.Roll = (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
0; // this is an invalid mode
;
stabilization.Pitch = (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
0; // this is an invalid mode
stabilization.Yaw = (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
0; // this is an invalid mode
stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle;
StabilizationDesiredSet(&stabilization);
}
/**
* Convert channel from servo pulse duration (microseconds) to scaled -1/+1 range.
*/
static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral)
{
float valueScaled;
// Scale
if ((max > min && value >= neutral) || (min > max && value <= neutral))
{
if (max != neutral)
valueScaled = (float)(value - neutral) / (float)(max - neutral);
else
valueScaled = 0;
}
else
{
if (min != neutral)
valueScaled = (float)(value - neutral) / (float)(neutral - min);
else
valueScaled = 0;
}
// Bound
if (valueScaled > 1.0) valueScaled = 1.0;
else
if (valueScaled < -1.0) valueScaled = -1.0;
return valueScaled;
}
static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time) {
if(end_time > start_time)
return (end_time - start_time) * portTICK_RATE_MS;
return ((((portTICK_RATE_MS) -1) - start_time) + end_time) * portTICK_RATE_MS;
}
/**
* @brief Determine if the aircraft is safe to arm
* @returns True if safe to arm, false otherwise
*/
static bool okToArm(void)
{
// read alarms
SystemAlarmsData alarms;
SystemAlarmsGet(&alarms);
// Check each alarm
for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++)
{
if (alarms.Alarm[i] >= SYSTEMALARMS_ALARM_ERROR)
{ // found an alarm thats set
if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY)
continue;
return false;
}
}
return true;
}
/**
* @brief Update the flightStatus object only if value changed. Reduces callbacks
* @param[in] val The new value
*/
static void setArmedIfChanged(uint8_t val) {
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if(flightStatus.Armed != val) {
flightStatus.Armed = val;
FlightStatusSet(&flightStatus);
}
}
/**
* @brief Process the inputs and determine whether to arm or not
* @param[out] cmd The structure to set the armed in
* @param[in] settings Settings indicating the necessary position
*/
static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings)
{
bool lowThrottle = cmd->Throttle <= 0;
if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSDISARMED) {
// In this configuration we always disarm
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
} else {
// Not really needed since this function not called when disconnected
if (cmd->Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE)
return;
// The throttle is not low, in case we where arming or disarming, abort
if (!lowThrottle) {
switch(armState) {
case ARM_STATE_DISARMING_MANUAL:
case ARM_STATE_DISARMING_TIMEOUT:
armState = ARM_STATE_ARMED;
break;
case ARM_STATE_ARMING_MANUAL:
armState = ARM_STATE_DISARMED;
break;
default:
// Nothing needs to be done in the other states
break;
}
return;
}
// The rest of these cases throttle is low
if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSARMED) {
// In this configuration, we go into armed state as soon as the throttle is low, never disarm
setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED);
return;
}
// When the configuration is not "Always armed" and no "Always disarmed",
// the state will not be changed when the throttle is not low
static portTickType armedDisarmStart;
float armingInputLevel = 0;
// Calc channel see assumptions7
int8_t sign = ((settings->Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2) ? -1 : 1;
switch ( (settings->Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 ) {
case ARMING_CHANNEL_ROLL: armingInputLevel = sign * cmd->Roll; break;
case ARMING_CHANNEL_PITCH: armingInputLevel = sign * cmd->Pitch; break;
case ARMING_CHANNEL_YAW: armingInputLevel = sign * cmd->Yaw; break;
}
bool manualArm = false;
bool manualDisarm = false;
if (armingInputLevel <= -ARMED_THRESHOLD)
manualArm = true;
else if (armingInputLevel >= +ARMED_THRESHOLD)
manualDisarm = true;
switch(armState) {
case ARM_STATE_DISARMED:
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
// only allow arming if it's OK too
if (manualArm && okToArm()) {
armedDisarmStart = lastSysTime;
armState = ARM_STATE_ARMING_MANUAL;
}
break;
case ARM_STATE_ARMING_MANUAL:
setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMING);
if (manualArm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS))
armState = ARM_STATE_ARMED;
else if (!manualArm)
armState = ARM_STATE_DISARMED;
break;
case ARM_STATE_ARMED:
// When we get here, the throttle is low,
// we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled
armedDisarmStart = lastSysTime;
armState = ARM_STATE_DISARMING_TIMEOUT;
setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED);
break;
case ARM_STATE_DISARMING_TIMEOUT:
// We get here when armed while throttle low, even when the arming timeout is not enabled
if ((settings->ArmedTimeout != 0) && (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings->ArmedTimeout))
armState = ARM_STATE_DISARMED;
// Switch to disarming due to manual control when needed
if (manualDisarm) {
armedDisarmStart = lastSysTime;
armState = ARM_STATE_DISARMING_MANUAL;
}
break;
case ARM_STATE_DISARMING_MANUAL:
if (manualDisarm &&(timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS))
armState = ARM_STATE_DISARMED;
else if (!manualDisarm)
armState = ARM_STATE_ARMED;
break;
} // End Switch
}
}
/**
* @brief Determine which of three positions the flight mode switch is in and set flight mode accordingly
* @param[out] cmd Pointer to the command structure to set the flight mode in
* @param[in] settings The settings which indicate which position is which mode
* @param[in] flightMode the value of the switch position
*/
static void processFlightMode(ManualControlSettingsData * settings, float flightMode)
{
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
uint8_t newMode;
// Note here the code is ass
if (flightMode < -FLIGHT_MODE_LIMIT)
newMode = settings->FlightModePosition[0];
else if (flightMode > FLIGHT_MODE_LIMIT)
newMode = settings->FlightModePosition[2];
else
newMode = settings->FlightModePosition[1];
if(flightStatus.FlightMode != newMode) {
flightStatus.FlightMode = newMode;
FlightStatusSet(&flightStatus);
}
}
/**
* @brief Determine if the manual input value is within acceptable limits
* @returns return TRUE if so, otherwise return FALSE
*/
bool validInputRange(int16_t min, int16_t max, uint16_t value)
{
if (min > max)
{
int16_t tmp = min;
min = max;
max = tmp;
}
return (value >= min - CONNECTION_OFFSET && value <= max + CONNECTION_OFFSET);
}
/**
* @}
* @}
*/

246
flight/Modules/Stabilization/stabilization.c
View File

@ -77,6 +77,15 @@ static xTaskHandle taskHandle;
static StabilizationSettingsData settings;
static xQueueHandle queue;
float dT = 1;
float gyro_alpha = 0;
float gyro_filtered[3] = {0,0,0};
float axis_lock_accum[3] = {0,0,0};
uint8_t max_axis_lock = 0;
uint8_t max_axislock_rate = 0;
float weak_leveling_kp = 0;
uint8_t weak_leveling_max = 0;
bool lowThrottleZeroIntegral;
pid_type pids[PID_MAX];
// Private functions
@ -89,27 +98,40 @@ static void SettingsUpdatedCb(UAVObjEvent * ev);
/**
* Module initialization
*/
int32_t StabilizationInitialize()
int32_t StabilizationStart()
{
// Initialize variables
// Create object queue
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
// Listen for updates.
// AttitudeActualConnectQueue(queue);
AttitudeRawConnectQueue(queue);
StabilizationSettingsConnectCallback(SettingsUpdatedCb);
SettingsUpdatedCb(StabilizationSettingsHandle());
// Start main task
xTaskCreate(stabilizationTask, (signed char*)"Stabilization", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_STABILIZATION, taskHandle);
PIOS_WDG_RegisterFlag(PIOS_WDG_STABILIZATION);
return 0;
}
/**
* Module initialization
*/
int32_t StabilizationInitialize()
{
// Initialize variables
// Create object queue
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
// Listen for updates.
// AttitudeActualConnectQueue(queue);
AttitudeRawConnectQueue(queue);
StabilizationSettingsConnectCallback(SettingsUpdatedCb);
SettingsUpdatedCb(StabilizationSettingsHandle());
// Start main task
return 0;
}
MODULE_INITCALL(StabilizationInitialize, StabilizationStart)
/**
* Module task
*/
@ -118,8 +140,8 @@ static void stabilizationTask(void* parameters)
portTickType lastSysTime;
portTickType thisSysTime;
UAVObjEvent ev;
ActuatorDesiredData actuatorDesired;
StabilizationDesiredData stabDesired;
RateDesiredData rateDesired;
@ -127,28 +149,28 @@ static void stabilizationTask(void* parameters)
AttitudeRawData attitudeRaw;
SystemSettingsData systemSettings;
FlightStatusData flightStatus;
SettingsUpdatedCb((UAVObjEvent *) NULL);
// Main task loop
lastSysTime = xTaskGetTickCount();
ZeroPids();
while(1) {
PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION);
// Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe
if ( xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE )
{
AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION,SYSTEMALARMS_ALARM_WARNING);
continue;
}
}
// Check how long since last update
thisSysTime = xTaskGetTickCount();
if(thisSysTime > lastSysTime) // reuse dt in case of wraparound
dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;
lastSysTime = thisSysTime;
FlightStatusGet(&flightStatus);
StabilizationDesiredGet(&stabDesired);
AttitudeActualGet(&attitudeActual);
@ -162,11 +184,11 @@ static void stabilizationTask(void* parameters)
float q_desired[4];
float q_error[4];
float local_error[3];
// Essentially zero errors for anything in rate or none
if(stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE)
rpy_desired[0] = stabDesired.Roll;
else
rpy_desired[0] = stabDesired.Roll;
else
rpy_desired[0] = attitudeActual.Roll;
if(stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE)
@ -178,13 +200,13 @@ static void stabilizationTask(void* parameters)
rpy_desired[2] = stabDesired.Yaw;
else
rpy_desired[2] = attitudeActual.Yaw;
RPY2Quaternion(rpy_desired, q_desired);
quat_inverse(q_desired);
quat_mult(q_desired, &attitudeActual.q1, q_error);
quat_inverse(q_error);
Quaternion2RPY(q_error, local_error);
#else
// Simpler algorithm for CC, less memory
float local_error[3] = {stabDesired.Roll - attitudeActual.Roll,
@ -192,49 +214,83 @@ static void stabilizationTask(void* parameters)
stabDesired.Yaw - attitudeActual.Yaw};
local_error[2] = fmod(local_error[2] + 180, 360) - 180;
#endif
for(uint8_t i = 0; i < MAX_AXES; i++) {
gyro_filtered[i] = gyro_filtered[i] * gyro_alpha + attitudeRaw.gyros[i] * (1 - gyro_alpha);
}
float *attitudeDesiredAxis = &stabDesired.Roll;
float *actuatorDesiredAxis = &actuatorDesired.Roll;
float *rateDesiredAxis = &rateDesired.Roll;
//Calculate desired rate
for(int8_t ct=0; ct< MAX_AXES; ct++)
for(uint8_t i=0; i< MAX_AXES; i++)
{
switch(stabDesired.StabilizationMode[ct])
switch(stabDesired.StabilizationMode[i])
{
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE:
rateDesiredAxis[ct] = attitudeDesiredAxis[ct];
rateDesiredAxis[i] = attitudeDesiredAxis[i];
axis_lock_accum[i] = 0;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING:
{
float weak_leveling = local_error[i] * weak_leveling_kp;
if(weak_leveling > weak_leveling_max)
weak_leveling = weak_leveling_max;
if(weak_leveling < -weak_leveling_max)
weak_leveling = -weak_leveling_max;
rateDesiredAxis[i] = attitudeDesiredAxis[i] + weak_leveling;
axis_lock_accum[i] = 0;
break;
}
case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
rateDesiredAxis[ct] = ApplyPid(&pids[PID_ROLL + ct], local_error[ct]);
rateDesiredAxis[i] = ApplyPid(&pids[PID_ROLL + i], local_error[i]);
axis_lock_accum[i] = 0;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK:
if(fabs(attitudeDesiredAxis[i]) > max_axislock_rate) {
// While getting strong commands act like rate mode
rateDesiredAxis[i] = attitudeDesiredAxis[i];
axis_lock_accum[i] = 0;
} else {
// For weaker commands or no command simply attitude lock (almost) on no gyro change
axis_lock_accum[i] += (attitudeDesiredAxis[i] - gyro_filtered[i]) * dT;
if(axis_lock_accum[i] > max_axis_lock)
axis_lock_accum[i] = max_axis_lock;
else if(axis_lock_accum[i] < -max_axis_lock)
axis_lock_accum[i] = -max_axis_lock;
rateDesiredAxis[i] = ApplyPid(&pids[PID_ROLL + i], axis_lock_accum[i]);
}
break;
}
}
uint8_t shouldUpdate = 1;
RateDesiredSet(&rateDesired);
ActuatorDesiredGet(&actuatorDesired);
//Calculate desired command
for(int8_t ct=0; ct< MAX_AXES; ct++)
{
if(fabs(rateDesiredAxis[ct]) > settings.MaximumRate[ct])
{
if(rateDesiredAxis[ct] > 0)
{
rateDesiredAxis[ct] = settings.MaximumRate[ct];
}else
{
rateDesiredAxis[ct] = -settings.MaximumRate[ct];
}
}
if(rateDesiredAxis[ct] > settings.MaximumRate[ct])
rateDesiredAxis[ct] = settings.MaximumRate[ct];
else if(rateDesiredAxis[ct] < -settings.MaximumRate[ct])
rateDesiredAxis[ct] = -settings.MaximumRate[ct];
switch(stabDesired.StabilizationMode[ct])
{
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE:
case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK:
case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING:
{
float command = ApplyPid(&pids[PID_RATE_ROLL + ct], rateDesiredAxis[ct]-attitudeRaw.gyros[ct]);
float command = ApplyPid(&pids[PID_RATE_ROLL + ct], rateDesiredAxis[ct] - gyro_filtered[ct]);
actuatorDesiredAxis[ct] = bound(command);
break;
}
@ -255,16 +311,16 @@ static void stabilizationTask(void* parameters)
break;
}
break;
}
}
// Save dT
actuatorDesired.UpdateTime = dT * 1000;
if(PARSE_FLIGHT_MODE(flightStatus.FlightMode) == FLIGHTMODE_MANUAL)
shouldUpdate = 0;
if(shouldUpdate)
{
actuatorDesired.Throttle = stabDesired.Throttle;
@ -272,16 +328,17 @@ static void stabilizationTask(void* parameters)
actuatorDesired.NumLongUpdates++;
ActuatorDesiredSet(&actuatorDesired);
}
if(flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED ||
!shouldUpdate || (stabDesired.Throttle < 0))
(lowThrottleZeroIntegral && stabDesired.Throttle < 0) ||
!shouldUpdate)
{
ZeroPids();
}
// Clear alarms
AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION);
AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION);
}
}
@ -289,15 +346,15 @@ float ApplyPid(pid_type * pid, const float err)
{
float diff = (err - pid->lastErr);
pid->lastErr = err;
pid->iAccumulator += err * pid->i * dT;
if(fabs(pid->iAccumulator) > pid->iLim) {
if(pid->iAccumulator >0) {
pid->iAccumulator = pid->iLim;
} else {
pid->iAccumulator = -pid->iLim;
}
// Scale up accumulator by 1000 while computing to avoid losing precision
pid->iAccumulator += err * (pid->i * dT * 1000);
if(pid->iAccumulator > (pid->iLim * 1000)) {
pid->iAccumulator = pid->iLim * 1000;
} else if (pid->iAccumulator < -(pid->iLim * 1000)) {
pid->iAccumulator = -pid->iLim * 1000;
}
return ((err * pid->p) + pid->iAccumulator + (diff * pid->d / dT));
return ((err * pid->p) + pid->iAccumulator / 1000 + (diff * pid->d / dT));
}
@ -307,6 +364,8 @@ static void ZeroPids(void)
pids[ct].iAccumulator = 0;
pids[ct].lastErr = 0;
}
for(uint8_t i = 0; i < 3; i++)
axis_lock_accum[i] = 0;
}
@ -328,14 +387,61 @@ static void SettingsUpdatedCb(UAVObjEvent * ev)
{
memset(pids,0,sizeof (pid_type) * PID_MAX);
StabilizationSettingsGet(&settings);
float * data = settings.RollRatePI;
for(int8_t pid=0; pid < PID_MAX; pid++)
{
pids[pid].p = *data++;
pids[pid].i = *data++;
pids[pid].iLim = *data++;
}
// Set the roll rate PID constants
pids[0].p = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP];
pids[0].i = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI];
pids[0].d = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD];
pids[0].iLim = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT];
// Set the pitch rate PID constants
pids[1].p = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP];
pids[1].i = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI];
pids[1].d = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD];
pids[1].iLim = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT];
// Set the yaw rate PID constants
pids[2].p = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KP];
pids[2].i = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KI];
pids[2].d = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KD];
pids[2].iLim = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_ILIMIT];
// Set the roll attitude PI constants
pids[3].p = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP];
pids[3].i = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI];
pids[3].iLim = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT];
// Set the pitch attitude PI constants
pids[4].p = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP];
pids[4].i = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI];
pids[4].iLim = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT];
// Set the yaw attitude PI constants
pids[5].p = settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KP];
pids[5].i = settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KI];
pids[5].iLim = settings.YawPI[STABILIZATIONSETTINGS_YAWPI_ILIMIT];
// Maximum deviation to accumulate for axis lock
max_axis_lock = settings.MaxAxisLock;
max_axislock_rate = settings.MaxAxisLockRate;
// Settings for weak leveling
weak_leveling_kp = settings.WeakLevelingKp;
weak_leveling_max = settings.MaxWeakLevelingRate;
// Whether to zero the PID integrals while throttle is low
lowThrottleZeroIntegral = settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE;
// The dT has some jitter iteration to iteration that we don't want to
// make thie result unpredictable. Still, it's nicer to specify the constant
// based on a time (in ms) rather than a fixed multiplier. The error between
// update rates on OP (~300 Hz) and CC (~475 Hz) is negligible for this
// calculation
const float fakeDt = 0.0025;
if(settings.GyroTau < 0.0001)
gyro_alpha = 0; // not trusting this to resolve to 0
else
gyro_alpha = exp(-fakeDt / settings.GyroTau);
}

87
flight/Modules/System/systemmod.c
View File

@ -84,18 +84,34 @@ static void updateSystemAlarms();
static void systemTask(void *parameters);
/**
* Initialise the module, called on startup.
* \returns 0 on success or -1 if initialisation failed
* Create the module task.
* \returns 0 on success or -1 if initialization failed
*/
int32_t SystemModInitialize(void)
int32_t SystemModStart(void)
{
// Initialize vars
stackOverflow = 0;
// Create system task
xTaskCreate(systemTask, (signed char *)"System", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &systemTaskHandle);
// Register task
TaskMonitorAdd(TASKINFO_RUNNING_SYSTEM, systemTaskHandle);
return 0;
}
/**
* Initialize the module, called on startup.
* \returns 0 on success or -1 if initialization failed
*/
int32_t SystemModInitialize(void)
{
SystemModStart();
return 0;
}
MODULE_INITCALL(SystemModInitialize, 0)
/**
* System task, periodically executes every SYSTEM_UPDATE_PERIOD_MS
*/
@ -103,11 +119,8 @@ static void systemTask(void *parameters)
{
portTickType lastSysTime;
// System initialization
OpenPilotInit();
// Register task
TaskMonitorAdd(TASKINFO_RUNNING_SYSTEM, systemTaskHandle);
/* create all modules thread */
MODULE_TASKCREATE_ALL
// Initialize vars
idleCounter = 0;
@ -118,7 +131,7 @@ static void systemTask(void *parameters)
ObjectPersistenceConnectCallback(&objectUpdatedCb);
// Main system loop
while (1) {
while (1) {
// Update the system statistics
updateStats();
@ -260,6 +273,48 @@ static void updateWDGstats()
WatchdogStatusSet(&watchdogStatus);
}
/**
* Called periodically to update the system stats
*/
static uint16_t GetFreeIrqStackSize(void)
{
uint32_t i = 0x200;
#if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) && defined(CHECK_IRQ_STACK)
extern uint32_t _irq_stack_top;
extern uint32_t _irq_stack_end;
uint32_t pattern = 0x0000A5A5;
uint32_t *ptr = &_irq_stack_end;
#if 1 /* the ugly way accurate but takes more time, useful for debugging */
uint32_t stack_size = (((uint32_t)&_irq_stack_top - (uint32_t)&_irq_stack_end) & ~3 ) / 4;
for (i=0; i< stack_size; i++)
{
if (ptr[i] != pattern)
{
i=i*4;
break;
}
}
#else /* faster way but not accurate */
if (*(volatile uint32_t *)((uint32_t)ptr + IRQSTACK_LIMIT_CRITICAL) != pattern)
{
i = IRQSTACK_LIMIT_CRITICAL - 1;
}
else if (*(volatile uint32_t *)((uint32_t)ptr + IRQSTACK_LIMIT_WARNING) != pattern)
{
i = IRQSTACK_LIMIT_WARNING - 1;
}
else
{
i = IRQSTACK_LIMIT_WARNING;
}
#endif
#endif
return i;
}
/**
* Called periodically to update the system stats
*/
@ -278,6 +333,9 @@ static void updateStats()
stats.HeapRemaining = xPortGetFreeHeapSize();
#endif
// Get Irq stack status
stats.IRQStackRemaining = GetFreeIrqStackSize();
// When idleCounterClear was not reset by the idle-task, it means the idle-task did not run
if (idleCounterClear) {
idleCounter = 0;
@ -320,6 +378,17 @@ static void updateSystemAlarms()
AlarmsClear(SYSTEMALARMS_ALARM_OUTOFMEMORY);
}
#if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) && defined(CHECK_IRQ_STACK)
// Check IRQ stack
if (stats.IRQStackRemaining < IRQSTACK_LIMIT_CRITICAL) {
AlarmsSet(SYSTEMALARMS_ALARM_OUTOFMEMORY, SYSTEMALARMS_ALARM_CRITICAL);
} else if (stats.IRQStackRemaining < IRQSTACK_LIMIT_WARNING) {
AlarmsSet(SYSTEMALARMS_ALARM_OUTOFMEMORY, SYSTEMALARMS_ALARM_WARNING);
} else {
AlarmsClear(SYSTEMALARMS_ALARM_OUTOFMEMORY);
}
#endif
// Check CPU load
if (stats.CPULoad > CPULOAD_LIMIT_CRITICAL) {
AlarmsSet(SYSTEMALARMS_ALARM_CPUOVERLOAD, SYSTEMALARMS_ALARM_CRITICAL);

87
flight/Modules/Telemetry/telemetry.c
View File

@ -31,6 +31,7 @@
*/
#include "openpilot.h"
#include "telemetry.h"
#include "flighttelemetrystats.h"
#include "gcstelemetrystats.h"
#include "telemetrysettings.h"
@ -81,6 +82,28 @@ static void updateTelemetryStats();
static void gcsTelemetryStatsUpdated();
static void updateSettings();
/**
* Initialise the telemetry module
* \return -1 if initialisation failed
* \return 0 on success
*/
int32_t TelemetryStart(void)
{
// Start telemetry tasks
xTaskCreate(telemetryTxTask, (signed char *)"TelTx", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY_TX, &telemetryTxTaskHandle);
xTaskCreate(telemetryRxTask, (signed char *)"TelRx", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY_RX, &telemetryRxTaskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_TELEMETRYTX, telemetryTxTaskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_TELEMETRYRX, telemetryRxTaskHandle);
#if defined(PIOS_TELEM_PRIORITY_QUEUE)
xTaskCreate(telemetryTxPriTask, (signed char *)"TelPriTx", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY_TXPRI, &telemetryTxPriTaskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_TELEMETRYTXPRI, telemetryTxPriTaskHandle);
#endif
return 0;
}
/**
* Initialise the telemetry module
* \return -1 if initialisation failed
@ -118,20 +141,11 @@ int32_t TelemetryInitialize(void)
GCSTelemetryStatsConnectQueue(priorityQueue);
TelemetrySettingsConnectQueue(priorityQueue);
// Start telemetry tasks
xTaskCreate(telemetryTxTask, (signed char *)"TelTx", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY_TX, &telemetryTxTaskHandle);
xTaskCreate(telemetryRxTask, (signed char *)"TelRx", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY_RX, &telemetryRxTaskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_TELEMETRYTX, telemetryTxTaskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_TELEMETRYRX, telemetryRxTaskHandle);
#if defined(PIOS_TELEM_PRIORITY_QUEUE)
xTaskCreate(telemetryTxPriTask, (signed char *)"TelPriTx", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY_TXPRI, &telemetryTxPriTaskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_TELEMETRYTXPRI, telemetryTxPriTaskHandle);
#endif
return 0;
}
MODULE_INITCALL(TelemetryInitialize, TelemetryStart)
/**
* Register a new object, adds object to local list and connects the queue depending on the object's
* telemetry settings.
@ -292,7 +306,6 @@ static void telemetryTxPriTask(void *parameters)
static void telemetryRxTask(void *parameters)
{
uint32_t inputPort;
int32_t len;
// Task loop
while (1) {
@ -306,14 +319,20 @@ static void telemetryRxTask(void *parameters)
inputPort = telemetryPort;
}
// Block until data are available
// TODO: Currently we periodically check the buffer for data, update once the PIOS_COM is made blocking
len = PIOS_COM_ReceiveBufferUsed(inputPort);
for (int32_t n = 0; n < len; ++n) {
UAVTalkProcessInputStream(uavTalkCon, PIOS_COM_ReceiveBuffer(inputPort));
}
vTaskDelay(5); // <- remove when blocking calls are implemented
if (inputPort) {
// Block until data are available
uint8_t serial_data[1];
uint16_t bytes_to_process;
bytes_to_process = PIOS_COM_ReceiveBuffer(inputPort, serial_data, sizeof(serial_data), 500);
if (bytes_to_process > 0) {
for (uint8_t i = 0; i < bytes_to_process; i++) {
UAVTalkProcessInputStream(uavTalkCon,serial_data[i]);
}
}
} else {
vTaskDelay(5);
}
}
}
@ -337,7 +356,11 @@ static int32_t transmitData(uint8_t * data, int32_t length)
outputPort = telemetryPort;
}
return PIOS_COM_SendBufferNonBlocking(outputPort, data, length);
if (outputPort) {
return PIOS_COM_SendBufferNonBlocking(outputPort, data, length);
} else {
return -1;
}
}
/**
@ -493,20 +516,16 @@ static void updateSettings()
// Retrieve settings
TelemetrySettingsGet(&settings);
// Set port speed
if (settings.Speed == TELEMETRYSETTINGS_SPEED_2400) PIOS_COM_ChangeBaud(telemetryPort, 2400);
else
if (settings.Speed == TELEMETRYSETTINGS_SPEED_4800) PIOS_COM_ChangeBaud(telemetryPort, 4800);
else
if (settings.Speed == TELEMETRYSETTINGS_SPEED_9600) PIOS_COM_ChangeBaud(telemetryPort, 9600);
else
if (settings.Speed == TELEMETRYSETTINGS_SPEED_19200) PIOS_COM_ChangeBaud(telemetryPort, 19200);
else
if (settings.Speed == TELEMETRYSETTINGS_SPEED_38400) PIOS_COM_ChangeBaud(telemetryPort, 38400);
else
if (settings.Speed == TELEMETRYSETTINGS_SPEED_57600) PIOS_COM_ChangeBaud(telemetryPort, 57600);
else
if (settings.Speed == TELEMETRYSETTINGS_SPEED_115200) PIOS_COM_ChangeBaud(telemetryPort, 115200);
if (telemetryPort) {
// Set port speed
if (settings.Speed == TELEMETRYSETTINGS_SPEED_2400) PIOS_COM_ChangeBaud(telemetryPort, 2400);
else if (settings.Speed == TELEMETRYSETTINGS_SPEED_4800) PIOS_COM_ChangeBaud(telemetryPort, 4800);
else if (settings.Speed == TELEMETRYSETTINGS_SPEED_9600) PIOS_COM_ChangeBaud(telemetryPort, 9600);
else if (settings.Speed == TELEMETRYSETTINGS_SPEED_19200) PIOS_COM_ChangeBaud(telemetryPort, 19200);
else if (settings.Speed == TELEMETRYSETTINGS_SPEED_38400) PIOS_COM_ChangeBaud(telemetryPort, 38400);
else if (settings.Speed == TELEMETRYSETTINGS_SPEED_57600) PIOS_COM_ChangeBaud(telemetryPort, 57600);
else if (settings.Speed == TELEMETRYSETTINGS_SPEED_115200) PIOS_COM_ChangeBaud(telemetryPort, 115200);
}
}
/**

24
flight/OpenPilot/Makefile
View File

@ -43,9 +43,6 @@ ENABLE_DEBUG_PINS ?= NO
# Set to Yes to enable the AUX UART which is mapped on the S1 (Tx) and S2 (Rx) servo outputs
ENABLE_AUX_UART ?= NO
USE_SPEKTRUM ?= NO
# Set to YES when using Code Sourcery toolchain
CODE_SOURCERY ?= YES
@ -115,8 +112,6 @@ UAVOBJSYNTHDIR = $(OUTDIR)/../uavobject-synthetics/flight
# List C source files here. (C dependencies are automatically generated.)
# use file-extension c for "c-only"-files
MODNAMES = $(notdir ${MODULES} ${PYMODULES})
ifndef TESTAPP
## PyMite files and modules
@ -131,7 +126,6 @@ SRC += $(PYSRC)
## MODULES
SRC += ${foreach MOD, ${MODULES}, ${wildcard ${OPMODULEDIR}/${MOD}/*.c}}
SRC += ${OUTDIR}/InitMods.c
## OPENPILOT CORE:
SRC += ${OPMODULEDIR}/System/systemmod.c
SRC += $(OPSYSTEM)/openpilot.c
@ -170,6 +164,7 @@ SRC += $(PIOSSTM32F10X)/pios_spi.c
SRC += $(PIOSSTM32F10X)/pios_ppm.c
SRC += $(PIOSSTM32F10X)/pios_pwm.c
SRC += $(PIOSSTM32F10X)/pios_spektrum.c
SRC += $(PIOSSTM32F10X)/pios_sbus.c
SRC += $(PIOSSTM32F10X)/pios_debug.c
SRC += $(PIOSSTM32F10X)/pios_gpio.c
SRC += $(PIOSSTM32F10X)/pios_exti.c
@ -194,6 +189,7 @@ SRC += $(PIOSCOMMON)/pios_hcsr04.c
SRC += $(PIOSCOMMON)/pios_i2c_esc.c
SRC += $(PIOSCOMMON)/pios_iap.c
SRC += $(PIOSCOMMON)/pios_bl_helper.c
SRC += $(PIOSCOMMON)/pios_rcvr.c
SRC += $(PIOSCOMMON)/printf-stdarg.c
SRC += $(FLIGHTLIB)/ahrs_spi_comm.c
SRC += $(FLIGHTLIB)/ahrs_comm_objects.c
@ -363,11 +359,6 @@ ifeq ($(ENABLE_AUX_UART), YES)
CDEFS += -DPIOS_ENABLE_AUX_UART
endif
ifeq ($(USE_SPEKTRUM), YES)
CDEFS += -DUSE_SPEKTRUM
endif
# Place project-specific -D and/or -U options for
# Assembler with preprocessor here.
#ADEFS = -DUSE_IRQ_ASM_WRAPPER
@ -478,19 +469,10 @@ endif
endif
# Generate intermediate code
gencode: ${OUTDIR}/InitMods.c ${OUTDIR}/pmlib_img.c ${OUTDIR}/pmlib_nat.c ${OUTDIR}/pmlibusr_img.c ${OUTDIR}/pmlibusr_nat.c ${OUTDIR}/pmfeatures.h
gencode: ${OUTDIR}/pmlib_img.c ${OUTDIR}/pmlib_nat.c ${OUTDIR}/pmlibusr_img.c ${OUTDIR}/pmlibusr_nat.c ${OUTDIR}/pmfeatures.h
$(PYSRC): gencode
# Generate code for module initialization
${OUTDIR}/InitMods.c: Makefile
@echo $(MSG_MODINIT) $(call toprel, $@)
@echo ${quote}// Autogenerated file${quote} > ${OUTDIR}/InitMods.c
@echo ${quote}${foreach MOD, ${MODNAMES}, extern unsigned int ${MOD}Initialize(void);}${quote} >> ${OUTDIR}/InitMods.c
@echo ${quote}void InitModules() {${quote} >> ${OUTDIR}/InitMods.c
@echo ${quote}${foreach MOD, ${MODNAMES}, ${MOD}Initialize();}${quote} >> ${OUTDIR}/InitMods.c
@echo ${quote}}${quote} >> ${OUTDIR}/InitMods.c
# Generate code for PyMite
${OUTDIR}/pmlib_img.c ${OUTDIR}/pmlib_nat.c ${OUTDIR}/pmlibusr_img.c ${OUTDIR}/pmlibusr_nat.c ${OUTDIR}/pmfeatures.h: $(wildcard ${PYMITELIB}/*.py) $(wildcard ${PYMITEPLAT}/*.py) $(wildcard ${FLIGHTPLANLIB}/*.py) $(wildcard ${FLIGHTPLANS}/*.py)
@echo $(MSG_PYMITEINIT) $(call toprel, $@)

9
flight/OpenPilot/Makefile.posix
View File

@ -165,6 +165,7 @@ endif
SRC += $(PIOSPOSIX)/pios_crc.c
SRC += $(PIOSPOSIX)/pios_sys.c
SRC += $(PIOSPOSIX)/pios_led.c
SRC += $(PIOSPOSIX)/pios_irq.c
SRC += $(PIOSPOSIX)/pios_delay.c
SRC += $(PIOSPOSIX)/pios_sdcard.c
SRC += $(PIOSPOSIX)/pios_udp.c
@ -173,8 +174,10 @@ SRC += $(PIOSPOSIX)/pios_servo.c
SRC += $(PIOSPOSIX)/pios_wdg.c
SRC += $(PIOSPOSIX)/pios_debug.c
SRC += $(PIOSPOSIX)/pios_rcvr.c
## Libraries for flight calculations
#SRC += $(FLIGHTLIB)/fifo_buffer.c
SRC += $(FLIGHTLIB)/fifo_buffer.c
SRC += $(FLIGHTLIB)/WorldMagModel.c
SRC += $(FLIGHTLIB)/CoordinateConversions.c
## RTOS and RTOS Portable
@ -441,9 +444,13 @@ ${OUTDIR}/InitMods.c: Makefile.posix
@echo ${MSG_MODINIT}
@echo ${quote}// Autogenerated file${quote} > ${OUTDIR}/InitMods.c
@echo ${quote}${foreach MOD, ${MODNAMES}, extern unsigned int ${MOD}Initialize(void);}${quote} >> ${OUTDIR}/InitMods.c
@echo ${quote}${foreach MOD, ${MODNAMES}, extern unsigned int ${MOD}Start(void);}${quote} >> ${OUTDIR}/InitMods.c
@echo ${quote}void InitModules() {${quote} >> ${OUTDIR}/InitMods.c
@echo ${quote}${foreach MOD, ${MODNAMES}, ${MOD}Initialize();}${quote} >> ${OUTDIR}/InitMods.c
@echo ${quote}}${quote} >> ${OUTDIR}/InitMods.c
@echo ${quote}void StartModules() {${quote} >> ${OUTDIR}/InitMods.c
@echo ${quote}${foreach MOD, ${MODNAMES}, ${MOD}Start();}${quote} >> ${OUTDIR}/InitMods.c
@echo ${quote}}${quote} >> ${OUTDIR}/InitMods.c
# Generate code for PyMite
${OUTDIR}/pmlib_img.c ${OUTDIR}/pmlib_nat.c ${OUTDIR}/pmlibusr_img.c ${OUTDIR}/pmlibusr_nat.c ${OUTDIR}/pmfeatures.h: $(wildcard ${PYMITELIB}/*.py) $(wildcard ${PYMITEPLAT}/*.py) $(wildcard ${FLIGHTPLANLIB}/*.py) $(wildcard ${FLIGHTPLANS}/*.py) $(wildcard $(UAVOBJPYTHONSYNTHDIR)/*.py)

1
flight/OpenPilot/Makefile.win32
View File

@ -166,6 +166,7 @@ SRC += $(PIOSWIN32)/pios_udp.c
SRC += $(PIOSWIN32)/pios_com.c
SRC += $(PIOSWIN32)/pios_servo.c
SRC += $(PIOSWIN32)/pios_wdg.c
SRC += $(PIOSWIN32)/pios_crc.c
#
## RTOS
SRC += $(RTOSSRCDIR)/list.c

3
flight/OpenPilot/System/inc/FreeRTOSConfig.h
View File

@ -83,6 +83,9 @@ do {\
#define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004)/* DWT_CYCCNT */
#endif
#if !defined(ARCH_POSIX) && !defined(ARCH_WIN32)
#define CHECK_IRQ_STACK
#endif
/**
* @}

17
flight/OpenPilot/System/inc/pios_board_posix.h
View File

@ -52,15 +52,22 @@
//-------------------------
//#define PIOS_USART_TX_BUFFER_SIZE 256
#define PIOS_COM_BUFFER_SIZE 1024
#define PIOS_COM_MAX_DEVS 256
#define PIOS_UDP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE
#define PIOS_COM_TELEM_RF 0
#define PIOS_COM_GPS 1
#define PIOS_COM_TELEM_USB 2
extern uint32_t pios_com_telem_rf_id;
extern uint32_t pios_com_telem_usb_id;
extern uint32_t pios_com_gps_id;
extern uint32_t pios_com_aux_id;
extern uint32_t pios_com_spectrum_id;
#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id)
#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id)
#define PIOS_COM_GPS (pios_com_gps_id)
#ifdef PIOS_ENABLE_AUX_UART
#define PIOS_COM_AUX 3
#define PIOS_COM_DEBUG PIOS_COM_AUX
#define PIOS_COM_AUX (pios_com_aux_id)
#define PIOS_COM_DEBUG (PIOS_COM_AUX
#endif
/**

12
flight/OpenPilot/System/inc/pios_config.h
View File

@ -42,12 +42,14 @@
#define PIOS_INCLUDE_IRQ
#define PIOS_INCLUDE_LED
#if defined(USE_SPEKTRUM)
#define PIOS_INCLUDE_RCVR
#define PIOS_INCLUDE_SPEKTRUM
#else
//#define PIOS_INCLUDE_PPM
//#define PIOS_INCLUDE_SBUS
#define PIOS_INCLUDE_PWM
#endif
//#define PIOS_INCLUDE_PPM
#define PIOS_INCLUDE_TELEMETRY_RF
#define PIOS_INCLUDE_SERVO
#define PIOS_INCLUDE_SPI
@ -84,6 +86,8 @@
/* Alarm Thresholds */
#define HEAP_LIMIT_WARNING 4000
#define HEAP_LIMIT_CRITICAL 1000
#define IRQSTACK_LIMIT_WARNING 150
#define IRQSTACK_LIMIT_CRITICAL 80
#define CPULOAD_LIMIT_WARNING 80
#define CPULOAD_LIMIT_CRITICAL 95

5
flight/OpenPilot/System/inc/pios_config_posix.h
View File

@ -36,9 +36,14 @@
#define PIOS_INCLUDE_SDCARD
#define PIOS_INCLUDE_FREERTOS
#define PIOS_INCLUDE_COM
#define PIOS_INCLUDE_IRQ
#define PIOS_INCLUDE_TELEMETRY_RF
#define PIOS_INCLUDE_UDP
#define PIOS_INCLUDE_SERVO
#define PIOS_INCLUDE_RCVR
#define PIOS_RCVR_MAX_CHANNELS 12
#define PIOS_RCVR_MAX_DEVS 3
/* Defaults for Logging */
#define LOG_FILENAME "PIOS.LOG"

66
flight/OpenPilot/System/openpilot.c
View File

@ -62,11 +62,10 @@ static void TaskSDCard(void *pvParameters);
int32_t CONSOLE_Parse(uint8_t port, char c);
void OP_ADC_NotifyChange(uint32_t pin, uint32_t pin_value);
/* Prototype of generated InitModules() function */
extern void InitModules(void);
/* Prototype of PIOS_Board_Init() function */
extern void PIOS_Board_Init(void);
extern void Stack_Change(void);
static void Stack_Change_Weak () __attribute__ ((weakref ("Stack_Change")));
/**
* OpenPilot Main function:
@ -85,48 +84,42 @@ int main()
/* Brings up System using CMSIS functions, enables the LEDs. */
PIOS_SYS_Init();
/* Initialize the system thread */
SystemModInitialize();
/* Start the FreeRTOS scheduler */
vTaskStartScheduler();
/* If all is well we will never reach here as the scheduler will now be running. */
/* If we do get here, it will most likely be because we ran out of heap space. */
PIOS_LED_Off(LED1);
PIOS_LED_Off(LED2);
for(;;) {
PIOS_LED_Toggle(LED1);
PIOS_LED_Toggle(LED2);
PIOS_DELAY_WaitmS(100);
}
return 0;
}
/**
* Initialize the hardware, libraries and modules (called by the System thread in systemmod.c)
*/
void OpenPilotInit()
{
/* Architecture dependant Hardware and
* core subsystem initialisation
* (see pios_board.c for your arch)
* */
PIOS_Board_Init();
/* Initialize modules */
InitModules();
MODULE_INITIALISE_ALL
#if INCLUDE_TEST_TASKS
/* Create test tasks */
//xTaskCreate(TaskTesting, (signed portCHAR *)"Testing", configMINIMAL_STACK_SIZE , NULL, 4, NULL);
//xTaskCreate(TaskHIDTest, (signed portCHAR *)"HIDTest", configMINIMAL_STACK_SIZE , NULL, 3, NULL);
//xTaskCreate(TaskServos, (signed portCHAR *)"Servos", configMINIMAL_STACK_SIZE , NULL, 3, NULL);
//xTaskCreate(TaskSDCard, (signed portCHAR *)"SDCard", configMINIMAL_STACK_SIZE, NULL, (tskIDLE_PRIORITY + 2), NULL);
xTaskCreate(TaskTesting, (signed portCHAR *)"Testing", configMINIMAL_STACK_SIZE , NULL, 4, NULL);
xTaskCreate(TaskHIDTest, (signed portCHAR *)"HIDTest", configMINIMAL_STACK_SIZE , NULL, 3, NULL);
xTaskCreate(TaskServos, (signed portCHAR *)"Servos", configMINIMAL_STACK_SIZE , NULL, 3, NULL);
xTaskCreate(TaskSDCard, (signed portCHAR *)"SDCard", configMINIMAL_STACK_SIZE, NULL, (tskIDLE_PRIORITY + 2), NULL);
#endif
/* swap the stack to use the IRQ stack (does nothing in sim mode) */
Stack_Change_Weak();
/* Start the FreeRTOS scheduler which should never returns.*/
vTaskStartScheduler();
/* If all is well we will never reach here as the scheduler will now be running. */
/* Do some indication to user that something bad just happened */
PIOS_LED_Off(LED1); \
for(;;) { \
PIOS_LED_Toggle(LED1); \
PIOS_DELAY_WaitmS(100); \
};
return 0;
}
#if INCLUDE_TEST_TASKS
static void TaskTesting(void *pvParameters)
{
@ -153,6 +146,9 @@ static void TaskTesting(void *pvParameters)
#if defined(PIOS_INCLUDE_SPEKTRUM)
PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "%u,%u,%u,%u,%u,%u,%u,%u\r", PIOS_SPEKTRUM_Get(0), PIOS_SPEKTRUM_Get(1), PIOS_SPEKTRUM_Get(2), PIOS_SPEKTRUM_Get(3), PIOS_SPEKTRUM_Get(4), PIOS_SPEKTRUM_Get(5), PIOS_SPEKTRUM_Get(6), PIOS_SPEKTRUM_Get(7));
#endif
#if defined(PIOS_INCLUDE_SBUS)
PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "%u,%u,%u,%u,%u,%u,%u,%u\r", PIOS_SBUS_Get(0), PIOS_SBUS_Get(1), PIOS_SBUS_Get(2), PIOS_SBUS_Get(3), PIOS_SBUS_Get(4), PIOS_SBUS_Get(5), PIOS_SBUS_Get(6), PIOS_SBUS_Get(7));
#endif
#if defined(PIOS_INCLUDE_PWM)
PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "%u,%u,%u,%u,%u,%u,%u,%u uS\r", PIOS_PWM_Get(0), PIOS_PWM_Get(1), PIOS_PWM_Get(2), PIOS_PWM_Get(3), PIOS_PWM_Get(4), PIOS_PWM_Get(5), PIOS_PWM_Get(6), PIOS_PWM_Get(7));
#endif
@ -339,5 +335,5 @@ static void TaskSDCard(void *pvParameters)
/**
* @}
* @}
*/
*/

363
flight/OpenPilot/System/pios_board.c
View File

@ -30,6 +30,7 @@
#include <pios.h>
#include <openpilot.h>
#include <uavobjectsinit.h>
#include "manualcontrolsettings.h"
//#define I2C_DEBUG_PIN 0
//#define USART_GPS_DEBUG_PIN 1
@ -46,7 +47,7 @@
void PIOS_SPI_sdcard_irq_handler(void);
void DMA1_Channel2_IRQHandler() __attribute__ ((alias ("PIOS_SPI_sdcard_irq_handler")));
void DMA1_Channel3_IRQHandler() __attribute__ ((alias ("PIOS_SPI_sdcard_irq_handler")));
const struct pios_spi_cfg pios_spi_sdcard_cfg = {
static const struct pios_spi_cfg pios_spi_sdcard_cfg = {
.regs = SPI1,
.init = {
.SPI_Mode = SPI_Mode_Master,
@ -63,7 +64,6 @@ const struct pios_spi_cfg pios_spi_sdcard_cfg = {
.ahb_clk = RCC_AHBPeriph_DMA1,
.irq = {
.handler = PIOS_SPI_sdcard_irq_handler,
.flags = (DMA1_FLAG_TC2 | DMA1_FLAG_TE2 | DMA1_FLAG_HT2 | DMA1_FLAG_GL2),
.init = {
.NVIC_IRQChannel = DMA1_Channel2_IRQn,
@ -144,7 +144,7 @@ const struct pios_spi_cfg pios_spi_sdcard_cfg = {
void PIOS_SPI_ahrs_irq_handler(void);
void DMA1_Channel4_IRQHandler() __attribute__ ((alias ("PIOS_SPI_ahrs_irq_handler")));
void DMA1_Channel5_IRQHandler() __attribute__ ((alias ("PIOS_SPI_ahrs_irq_handler")));
const struct pios_spi_cfg pios_spi_ahrs_cfg = {
static const struct pios_spi_cfg pios_spi_ahrs_cfg = {
.regs = SPI2,
.init = {
.SPI_Mode = SPI_Mode_Master,
@ -162,7 +162,6 @@ const struct pios_spi_cfg pios_spi_ahrs_cfg = {
.ahb_clk = RCC_AHBPeriph_DMA1,
.irq = {
.handler = PIOS_SPI_ahrs_irq_handler,
.flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4),
.init = {
.NVIC_IRQChannel = DMA1_Channel4_IRQn,
@ -258,11 +257,10 @@ void PIOS_SPI_ahrs_irq_handler(void)
extern void PIOS_ADC_handler(void);
void DMA1_Channel1_IRQHandler() __attribute__ ((alias("PIOS_ADC_handler")));
// Remap the ADC DMA handler to this one
const struct pios_adc_cfg pios_adc_cfg = {
static const struct pios_adc_cfg pios_adc_cfg = {
.dma = {
.ahb_clk = RCC_AHBPeriph_DMA1,
.irq = {
.handler = PIOS_ADC_DMA_Handler,
.flags = (DMA1_FLAG_TC1 | DMA1_FLAG_TE1 | DMA1_FLAG_HT1 | DMA1_FLAG_GL1),
.init = {
.NVIC_IRQChannel = DMA1_Channel1_IRQn,
@ -310,16 +308,10 @@ void PIOS_ADC_handler() {
/*
* Telemetry USART
*/
void PIOS_USART_telem_irq_handler(void);
void USART2_IRQHandler() __attribute__ ((alias ("PIOS_USART_telem_irq_handler")));
const struct pios_usart_cfg pios_usart_telem_cfg = {
static const struct pios_usart_cfg pios_usart_telem_cfg = {
.regs = USART2,
.init = {
#if defined (PIOS_COM_TELEM_BAUDRATE)
.USART_BaudRate = PIOS_COM_TELEM_BAUDRATE,
#else
.USART_BaudRate = 57600,
#endif
.USART_BaudRate = 57600,
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1,
@ -327,7 +319,6 @@ const struct pios_usart_cfg pios_usart_telem_cfg = {
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
},
.irq = {
.handler = PIOS_USART_telem_irq_handler,
.init = {
.NVIC_IRQChannel = USART2_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
@ -356,17 +347,11 @@ const struct pios_usart_cfg pios_usart_telem_cfg = {
/*
* GPS USART
*/
void PIOS_USART_gps_irq_handler(void);
void USART3_IRQHandler() __attribute__ ((alias ("PIOS_USART_gps_irq_handler")));
const struct pios_usart_cfg pios_usart_gps_cfg = {
static const struct pios_usart_cfg pios_usart_gps_cfg = {
.regs = USART3,
.remap = GPIO_PartialRemap_USART3,
.init = {
#if defined (PIOS_COM_GPS_BAUDRATE)
.USART_BaudRate = PIOS_COM_GPS_BAUDRATE,
#else
.USART_BaudRate = 57600,
#endif
.USART_BaudRate = 57600,
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1,
@ -374,7 +359,6 @@ const struct pios_usart_cfg pios_usart_gps_cfg = {
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
},
.irq = {
.handler = PIOS_USART_gps_irq_handler,
.init = {
.NVIC_IRQChannel = USART3_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
@ -404,16 +388,9 @@ const struct pios_usart_cfg pios_usart_gps_cfg = {
/*
* AUX USART
*/
void PIOS_USART_aux_irq_handler(void);
void USART1_IRQHandler() __attribute__ ((alias ("PIOS_USART_aux_irq_handler")));
const struct pios_usart_cfg pios_usart_aux_cfg = {
static const struct pios_usart_cfg pios_usart_aux_cfg = {
.regs = USART1,
.init = {
#if defined (PIOS_COM_AUX_BAUDRATE)
.USART_BaudRate = PIOS_COM_AUX_BAUDRATE,
#else
.USART_BaudRate = 57600,
#endif
.USART_BaudRate = 57600,
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
@ -422,7 +399,6 @@ const struct pios_usart_cfg pios_usart_aux_cfg = {
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
},
.irq = {
.handler = PIOS_USART_aux_irq_handler,
.init = {
.NVIC_IRQChannel = USART1_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
@ -450,20 +426,44 @@ const struct pios_usart_cfg pios_usart_aux_cfg = {
};
#endif
#ifdef PIOS_COM_SPEKTRUM
#if defined(PIOS_INCLUDE_RTC)
/*
* Realtime Clock (RTC)
*/
#include <pios_rtc_priv.h>
void PIOS_RTC_IRQ_Handler (void);
void RTC_IRQHandler() __attribute__ ((alias ("PIOS_RTC_IRQ_Handler")));
static const struct pios_rtc_cfg pios_rtc_main_cfg = {
.clksrc = RCC_RTCCLKSource_HSE_Div128,
.prescaler = 100,
.irq = {
.init = {
.NVIC_IRQChannel = RTC_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
};
void PIOS_RTC_IRQ_Handler (void)
{
PIOS_RTC_irq_handler ();
}
#endif
#if defined(PIOS_INCLUDE_SPEKTRUM)
/*
* SPEKTRUM USART
*/
void PIOS_USART_spektrum_irq_handler(void);
void USART1_IRQHandler() __attribute__ ((alias ("PIOS_USART_spektrum_irq_handler")));
const struct pios_usart_cfg pios_usart_spektrum_cfg = {
#include <pios_spektrum_priv.h>
static const struct pios_usart_cfg pios_usart_spektrum_cfg = {
.regs = USART1,
.init = {
#if defined (PIOS_COM_SPEKTRUM_BAUDRATE)
.USART_BaudRate = PIOS_COM_SPEKTRUM_BAUDRATE,
#else
.USART_BaudRate = 115200,
#endif
.USART_BaudRate = 115200,
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1,
@ -471,7 +471,6 @@ const struct pios_usart_cfg pios_usart_spektrum_cfg = {
.USART_Mode = USART_Mode_Rx,
},
.irq = {
.handler = PIOS_USART_spektrum_irq_handler,
.init = {
.NVIC_IRQChannel = USART1_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
@ -497,74 +496,24 @@ const struct pios_usart_cfg pios_usart_spektrum_cfg = {
},
};
static uint32_t pios_usart_spektrum_id;
void PIOS_USART_spektrum_irq_handler(void)
{
SPEKTRUM_IRQHandler(pios_usart_spektrum_id);
}
#include <pios_spektrum_priv.h>
void RTC_IRQHandler();
void RTC_IRQHandler() __attribute__ ((alias ("PIOS_SUPV_irq_handler")));
const struct pios_spektrum_cfg pios_spektrum_cfg = {
.pios_usart_spektrum_cfg = &pios_usart_spektrum_cfg,
.gpio_init = { //used for bind feature
.GPIO_Mode = GPIO_Mode_Out_PP,
.GPIO_Speed = GPIO_Speed_2MHz,
},
.remap = 0,
.irq = {
.handler = RTC_IRQHandler,
.init = {
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
static const struct pios_spektrum_cfg pios_spektrum_cfg = {
.bind = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_Out_PP,
},
},
.port = GPIOA,
.pin = GPIO_Pin_10,
.remap = 0,
};
void PIOS_SUPV_irq_handler() {
if (RTC_GetITStatus(RTC_IT_SEC))
{
/* Call the right handler */
PIOS_SPEKTRUM_irq_handler(pios_usart_spektrum_id);
/* Wait until last write operation on RTC registers has finished */
RTC_WaitForLastTask();
/* Clear the RTC Second interrupt */
RTC_ClearITPendingBit(RTC_IT_SEC);
}
}
#endif /* PIOS_COM_SPEKTRUM */
static uint32_t pios_usart_telem_rf_id;
void PIOS_USART_telem_irq_handler(void)
{
PIOS_USART_IRQ_Handler(pios_usart_telem_rf_id);
}
static uint32_t pios_usart_gps_id;
void PIOS_USART_gps_irq_handler(void)
{
#ifdef USART_GPS_DEBUG_PIN
PIOS_DEBUG_PinHigh(USART_GPS_DEBUG_PIN);
#endif
PIOS_USART_IRQ_Handler(pios_usart_gps_id);
#ifdef USART_GPS_DEBUG_PIN
PIOS_DEBUG_PinLow(USART_GPS_DEBUG_PIN);
#endif
}
#ifdef PIOS_COM_AUX
static uint32_t pios_usart_aux_id;
void PIOS_USART_aux_irq_handler(void)
{
PIOS_USART_IRQ_Handler(pios_usart_aux_id);
}
#endif
#if defined(PIOS_INCLUDE_SBUS)
#error PIOS_INCLUDE_SBUS not implemented
#endif /* PIOS_INCLUDE_SBUS */
#endif /* PIOS_INCLUDE_USART */
@ -572,13 +521,21 @@ void PIOS_USART_aux_irq_handler(void)
#include "pios_com_priv.h"
#define PIOS_COM_TELEM_RF_RX_BUF_LEN 192
#define PIOS_COM_TELEM_RF_TX_BUF_LEN 192
#define PIOS_COM_GPS_RX_BUF_LEN 96
#define PIOS_COM_TELEM_USB_RX_BUF_LEN 192
#define PIOS_COM_TELEM_USB_TX_BUF_LEN 192
#endif /* PIOS_INCLUDE_COM */
/**
* Pios servo configuration structures
*/
#include <pios_servo_priv.h>
const struct pios_servo_channel pios_servo_channels[] = {
static const struct pios_servo_channel pios_servo_channels[] = {
{
.timer = TIM4,
.port = GPIOB,
@ -662,7 +619,7 @@ const struct pios_servo_cfg pios_servo_cfg = {
*/
#if defined(PIOS_INCLUDE_PWM)
#include <pios_pwm_priv.h>
const struct pios_pwm_channel pios_pwm_channels[] = {
static const struct pios_pwm_channel pios_pwm_channels[] = {
{
.timer = TIM1,
.port = GPIOA,
@ -747,7 +704,6 @@ const struct pios_pwm_cfg pios_pwm_cfg = {
},
.remap = GPIO_PartialRemap_TIM3,
.irq = {
.handler = TIM1_CC_IRQHandler,
.init = {
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
@ -778,7 +734,7 @@ void PIOS_TIM5_irq_handler()
#include <pios_ppm_priv.h>
void TIM6_IRQHandler();
void TIM6_IRQHandler() __attribute__ ((alias ("PIOS_TIM6_irq_handler")));
const struct pios_ppmsv_cfg pios_ppmsv_cfg = {
static const struct pios_ppmsv_cfg pios_ppmsv_cfg = {
.tim_base_init = {
.TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1, /* For 1 uS accuracy */
.TIM_ClockDivision = TIM_CKD_DIV1,
@ -787,7 +743,6 @@ const struct pios_ppmsv_cfg pios_ppmsv_cfg = {
.TIM_RepetitionCounter = 0x0000,
},
.irq = {
.handler = TIM6_IRQHandler,
.init = {
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
@ -798,14 +753,14 @@ const struct pios_ppmsv_cfg pios_ppmsv_cfg = {
.ccr = TIM_IT_Update,
};
void PIOS_TIM6_irq_handler()
void PIOS_TIM6_irq_handler(void)
{
PIOS_PPMSV_irq_handler();
}
void TIM1_CC_IRQHandler();
void TIM1_CC_IRQHandler() __attribute__ ((alias ("PIOS_TIM1_CC_irq_handler")));
const struct pios_ppm_cfg pios_ppm_cfg = {
static const struct pios_ppm_cfg pios_ppm_cfg = {
.tim_base_init = {
.TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1, /* For 1 uS accuracy */
.TIM_ClockDivision = TIM_CKD_DIV1,
@ -827,7 +782,6 @@ const struct pios_ppm_cfg pios_ppm_cfg = {
},
.remap = 0,
.irq = {
.handler = TIM1_CC_IRQHandler,
.init = {
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
@ -840,7 +794,7 @@ const struct pios_ppm_cfg pios_ppm_cfg = {
.ccr = TIM_IT_CC2,
};
void PIOS_TIM1_CC_irq_handler()
void PIOS_TIM1_CC_irq_handler(void)
{
PIOS_PPM_irq_handler();
}
@ -860,7 +814,7 @@ void PIOS_I2C_main_adapter_er_irq_handler(void);
void I2C2_EV_IRQHandler() __attribute__ ((alias ("PIOS_I2C_main_adapter_ev_irq_handler")));
void I2C2_ER_IRQHandler() __attribute__ ((alias ("PIOS_I2C_main_adapter_er_irq_handler")));
const struct pios_i2c_adapter_cfg pios_i2c_main_adapter_cfg = {
static const struct pios_i2c_adapter_cfg pios_i2c_main_adapter_cfg = {
.regs = I2C2,
.init = {
.I2C_Mode = I2C_Mode_I2C,
@ -888,7 +842,6 @@ const struct pios_i2c_adapter_cfg pios_i2c_main_adapter_cfg = {
},
},
.event = {
.handler = PIOS_I2C_main_adapter_ev_irq_handler,
.flags = 0, /* FIXME: check this */
.init = {
.NVIC_IRQChannel = I2C2_EV_IRQn,
@ -898,7 +851,6 @@ const struct pios_i2c_adapter_cfg pios_i2c_main_adapter_cfg = {
},
},
.error = {
.handler = PIOS_I2C_main_adapter_er_irq_handler,
.flags = 0, /* FIXME: check this */
.init = {
.NVIC_IRQChannel = I2C2_ER_IRQn,
@ -1009,6 +961,28 @@ static const struct stm32_gpio pios_debug_pins[] = {
#endif /* PIOS_ENABLE_DEBUG_PINS */
#if defined(PIOS_INCLUDE_RCVR)
#include "pios_rcvr_priv.h"
struct pios_rcvr_channel_map pios_rcvr_channel_to_id_map[PIOS_RCVR_MAX_CHANNELS];
uint32_t pios_rcvr_max_channel;
#endif /* PIOS_INCLUDE_RCVR */
#if defined(PIOS_INCLUDE_USB_HID)
#include "pios_usb_hid_priv.h"
static const struct pios_usb_hid_cfg pios_usb_hid_main_cfg = {
.irq = {
.init = {
.NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
};
#endif /* PIOS_INCLUDE_USB_HID */
extern const struct pios_com_driver pios_usb_com_driver;
uint32_t pios_com_telem_rf_id;
@ -1038,7 +1012,7 @@ void PIOS_Board_Init(void) {
#if defined(PIOS_INCLUDE_SPI)
/* Set up the SPI interface to the SD card */
if (PIOS_SPI_Init(&pios_spi_sdcard_id, &pios_spi_sdcard_cfg)) {
PIOS_DEBUG_Assert(0);
PIOS_Assert(0);
}
/* Enable and mount the SDCard */
@ -1046,23 +1020,16 @@ void PIOS_Board_Init(void) {
PIOS_SDCARD_MountFS(0);
#endif /* PIOS_INCLUDE_SPI */
#if defined(PIOS_INCLUDE_SPEKTRUM)
/* SPEKTRUM init must come before comms */
PIOS_RTC_Init(); // Spektrum uses RTC to check for frame failures
PIOS_SPEKTRUM_Init();
if (PIOS_USART_Init(&pios_usart_spektrum_id, &pios_usart_spektrum_cfg)) {
PIOS_DEBUG_Assert(0);
}
if (PIOS_COM_Init(&pios_com_spektrum_id, &pios_usart_com_driver, pios_usart_spektrum_id)) {
PIOS_DEBUG_Assert(0);
}
#endif
/* Initialize UAVObject libraries */
EventDispatcherInitialize();
UAVObjInitialize();
UAVObjectsInitializeAll();
#if defined(PIOS_INCLUDE_RTC)
/* Initialize the real-time clock and its associated tick */
PIOS_RTC_Init(&pios_rtc_main_cfg);
#endif
/* Initialize the alarms library */
AlarmsInitialize();
@ -1074,7 +1041,7 @@ void PIOS_Board_Init(void) {
/* Set up the SPI interface to the AHRS */
if (PIOS_SPI_Init(&pios_spi_ahrs_id, &pios_spi_ahrs_cfg)) {
PIOS_DEBUG_Assert(0);
PIOS_Assert(0);
}
/* Bind the AHRS comms layer to the AHRS SPI link */
@ -1082,43 +1049,145 @@ void PIOS_Board_Init(void) {
/* Initialize the PiOS library */
#if defined(PIOS_INCLUDE_COM)
if (PIOS_USART_Init(&pios_usart_telem_rf_id, &pios_usart_telem_cfg)) {
PIOS_DEBUG_Assert(0);
}
if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_telem_rf_id)) {
PIOS_DEBUG_Assert(0);
}
#if defined(PIOS_INCLUDE_TELEMETRY_RF)
{
uint32_t pios_usart_telem_rf_id;
if (PIOS_USART_Init(&pios_usart_telem_rf_id, &pios_usart_telem_cfg)) {
PIOS_Assert(0);
}
if (PIOS_USART_Init(&pios_usart_gps_id, &pios_usart_gps_cfg)) {
PIOS_DEBUG_Assert(0);
uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN);
uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN);
PIOS_Assert(rx_buffer);
PIOS_Assert(tx_buffer);
if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_telem_rf_id,
rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN,
tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) {
PIOS_Assert(0);
}
}
if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id)) {
PIOS_DEBUG_Assert(0);
#endif /* PIOS_INCLUDE_TELEMETRY_RF */
#if defined(PIOS_INCLUDE_GPS)
{
uint32_t pios_usart_gps_id;
if (PIOS_USART_Init(&pios_usart_gps_id, &pios_usart_gps_cfg)) {
PIOS_Assert(0);
}
uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN);
PIOS_Assert(rx_buffer);
if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id,
rx_buffer, PIOS_COM_GPS_RX_BUF_LEN,
NULL, 0)) {
PIOS_Assert(0);
}
}
#endif /* PIOS_INCLUDE_GPS */
#endif
PIOS_Servo_Init();
PIOS_ADC_Init();
PIOS_GPIO_Init();
/* Configure the selected receiver */
uint8_t manualcontrolsettings_inputmode;
ManualControlSettingsInputModeGet(&manualcontrolsettings_inputmode);
switch (manualcontrolsettings_inputmode) {
case MANUALCONTROLSETTINGS_INPUTMODE_PWM:
#if defined(PIOS_INCLUDE_PWM)
PIOS_PWM_Init();
#if (PIOS_PWM_NUM_INPUTS > PIOS_RCVR_MAX_CHANNELS)
#error More receiver inputs than available devices
#endif
PIOS_PWM_Init();
uint32_t pios_pwm_rcvr_id;
if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, 0)) {
PIOS_Assert(0);
}
for (uint8_t i = 0;
i < PIOS_PWM_NUM_INPUTS && pios_rcvr_max_channel < NELEMENTS(pios_rcvr_channel_to_id_map);
i++) {
pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].id = pios_pwm_rcvr_id;
pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].channel = i;
pios_rcvr_max_channel++;
}
#endif /* PIOS_INCLUDE_PWM */
break;
case MANUALCONTROLSETTINGS_INPUTMODE_PPM:
#if defined(PIOS_INCLUDE_PPM)
PIOS_PPM_Init();
#if (PIOS_PPM_NUM_INPUTS > PIOS_RCVR_MAX_CHANNELS)
#error More receiver inputs than available devices
#endif
PIOS_PPM_Init();
uint32_t pios_ppm_rcvr_id;
if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, 0)) {
PIOS_Assert(0);
}
for (uint8_t i = 0;
i < PIOS_PPM_NUM_INPUTS && pios_rcvr_max_channel < NELEMENTS(pios_rcvr_channel_to_id_map);
i++) {
pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].id = pios_ppm_rcvr_id;
pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].channel = i;
pios_rcvr_max_channel++;
}
#endif /* PIOS_INCLUDE_PPM */
break;
case MANUALCONTROLSETTINGS_INPUTMODE_SPEKTRUM:
#if defined(PIOS_INCLUDE_SPEKTRUM)
#if (PIOS_SPEKTRUM_NUM_INPUTS > PIOS_RCVR_MAX_CHANNELS)
#error More receiver inputs than available devices
#endif
{
uint32_t pios_usart_spektrum_id;
if (PIOS_USART_Init(&pios_usart_spektrum_id, &pios_usart_spektrum_cfg)) {
PIOS_Assert(0);
}
uint32_t pios_spektrum_id;
if (PIOS_SPEKTRUM_Init(&pios_spektrum_id, &pios_spektrum_cfg, &pios_usart_com_driver, pios_usart_spektrum_id, false)) {
PIOS_Assert(0);
}
uint32_t pios_spektrum_rcvr_id;
if (PIOS_RCVR_Init(&pios_spektrum_rcvr_id, &pios_spektrum_rcvr_driver, 0)) {
PIOS_Assert(0);
}
for (uint8_t i = 0;
i < PIOS_SPEKTRUM_NUM_INPUTS && pios_rcvr_max_channel < NELEMENTS(pios_rcvr_channel_to_id_map);
i++) {
pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].id = pios_spektrum_rcvr_id;
pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].channel = i;
pios_rcvr_max_channel++;
}
}
#endif
break;
case MANUALCONTROLSETTINGS_INPUTMODE_SBUS:
#if defined(PIOS_INCLUDE_SBUS)
#error SBUS NOT ON OP YET
#endif /* PIOS_INCLUDE_SBUS */
break;
}
#if defined(PIOS_INCLUDE_USB_HID)
PIOS_USB_HID_Init(0);
uint32_t pios_usb_hid_id;
PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_main_cfg);
#if defined(PIOS_INCLUDE_COM)
if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_com_driver, 0)) {
PIOS_DEBUG_Assert(0);
uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN);
uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_TX_BUF_LEN);
PIOS_Assert(rx_buffer);
PIOS_Assert(tx_buffer);
if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_com_driver, pios_usb_hid_id,
rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN,
tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) {
PIOS_Assert(0);
}
#endif /* PIOS_INCLUDE_COM */
#endif /* PIOS_INCLUDE_USB_HID */
#endif /* PIOS_INCLUDE_USB_HID */
#if defined(PIOS_INCLUDE_I2C)
if (PIOS_I2C_Init(&pios_i2c_main_adapter_id, &pios_i2c_main_adapter_cfg)) {
PIOS_DEBUG_Assert(0);
PIOS_Assert(0);
}
#endif /* PIOS_INCLUDE_I2C */
PIOS_IAP_Init();

127
flight/OpenPilot/System/pios_board_posix.c
View File

@ -29,42 +29,27 @@
#include <openpilot.h>
#include <uavobjectsinit.h>
/**
* PIOS_Board_Init()
* initializes all the core systems on this specific hardware
* called from System/openpilot.c
*/
void PIOS_Board_Init(void) {
#include "pios_rcvr_priv.h"
/* Delay system */
PIOS_DELAY_Init();
struct pios_rcvr_channel_map pios_rcvr_channel_to_id_map[PIOS_RCVR_MAX_CHANNELS];
uint32_t pios_rcvr_max_channel;
/* Initialize UAVObject libraries */
EventDispatcherInitialize();
UAVObjInitialize();
UAVObjectsInitializeAll();
/* Initialize the alarms library */
AlarmsInitialize();
/* Initialize the task monitor library */
TaskMonitorInitialize();
/* Initialize the PiOS library */
PIOS_COM_Init();
void Stack_Change() {
}
void Stack_Change_Weak() {
}
const struct pios_udp_cfg pios_udp0_cfg = {
const struct pios_udp_cfg pios_udp_telem_cfg = {
.ip = "0.0.0.0",
.port = 9000,
};
const struct pios_udp_cfg pios_udp1_cfg = {
const struct pios_udp_cfg pios_udp_gps_cfg = {
.ip = "0.0.0.0",
.port = 9001,
};
const struct pios_udp_cfg pios_udp2_cfg = {
const struct pios_udp_cfg pios_udp_debug_cfg = {
.ip = "0.0.0.0",
.port = 9002,
};
@ -73,15 +58,19 @@ const struct pios_udp_cfg pios_udp2_cfg = {
/*
* AUX USART
*/
const struct pios_udp_cfg pios_udp3_cfg = {
const struct pios_udp_cfg pios_udp_aux_cfg = {
.ip = "0.0.0.0",
.port = 9003,
};
#endif
#define PIOS_COM_TELEM_RF_RX_BUF_LEN 192
#define PIOS_COM_TELEM_RF_TX_BUF_LEN 192
/*
* Board specific number of devices.
*/
/*
struct pios_udp_dev pios_udp_devs[] = {
#define PIOS_UDP_TELEM 0
{
@ -104,7 +93,7 @@ struct pios_udp_dev pios_udp_devs[] = {
};
uint8_t pios_udp_num_devices = NELEMENTS(pios_udp_devs);
*/
/*
* COM devices
*/
@ -115,28 +104,72 @@ uint8_t pios_udp_num_devices = NELEMENTS(pios_udp_devs);
extern const struct pios_com_driver pios_serial_com_driver;
extern const struct pios_com_driver pios_udp_com_driver;
struct pios_com_dev pios_com_devs[] = {
{
.id = PIOS_UDP_TELEM,
.driver = &pios_udp_com_driver,
},
{
.id = PIOS_UDP_GPS,
.driver = &pios_udp_com_driver,
},
{
.id = PIOS_UDP_LOCAL,
.driver = &pios_udp_com_driver,
},
#ifdef PIOS_COM_AUX
{
.id = PIOS_UDP_AUX,
.driver = &pios_udp_com_driver,
},
#endif
};
uint32_t pios_com_telem_rf_id;
uint32_t pios_com_telem_usb_id;
uint32_t pios_com_gps_id;
uint32_t pios_com_aux_id;
uint32_t pios_com_spectrum_id;
const uint8_t pios_com_num_devices = NELEMENTS(pios_com_devs);
/**
* PIOS_Board_Init()
* initializes all the core systems on this specific hardware
* called from System/openpilot.c
*/
void PIOS_Board_Init(void) {
/* Delay system */
PIOS_DELAY_Init();
/* Initialize UAVObject libraries */
EventDispatcherInitialize();
UAVObjInitialize();
UAVObjectsInitializeAll();
/* Initialize the alarms library */
AlarmsInitialize();
/* Initialize the task monitor library */
TaskMonitorInitialize();
#if defined(PIOS_INCLUDE_COM)
#if defined(PIOS_INCLUDE_TELEMETRY_RF)
{
uint32_t pios_udp_telem_rf_id;
if (PIOS_UDP_Init(&pios_udp_telem_rf_id, &pios_udp_telem_cfg)) {
PIOS_Assert(0);
}
uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN);
uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN);
PIOS_Assert(rx_buffer);
PIOS_Assert(tx_buffer);
if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_udp_com_driver, pios_udp_telem_rf_id,
rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN,
tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) {
PIOS_Assert(0);
}
}
#endif /* PIOS_INCLUDE_TELEMETRY_RF */
#if defined(PIOS_INCLUDE_GPS)
{
uint32_t pios_udp_gps_id;
if (PIOS_USART_Init(&pios_udp_gps_id, &pios_udp_gps_cfg)) {
PIOS_Assert(0);
}
uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN);
PIOS_Assert(rx_buffer);
if (PIOS_COM_Init(&pios_com_gps_id, &pios_udp_com_driver, pios_udp_gps_id,
rx_buffer, PIOS_COM_GPS_RX_BUF_LEN,
NULL, 0)) {
PIOS_Assert(0);
}
}
#endif /* PIOS_INCLUDE_GPS */
#endif
}
/**
* @}

120
flight/PiOS.posix/inc/pios_com.h
View File

@ -1,55 +1,65 @@
/**
******************************************************************************
*
* @file pios_com.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* Parts by Thorsten Klose (tk@midibox.org)
* @brief COM layer functions 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 PIOS_COM_H
#define PIOS_COM_H
/* Public Functions */
extern int32_t PIOS_COM_Init(void);
extern int32_t PIOS_COM_ChangeBaud(uint8_t port, uint32_t baud);
extern int32_t PIOS_COM_SendCharNonBlocking(uint8_t port, char c);
extern int32_t PIOS_COM_SendChar(uint8_t port, char c);
extern int32_t PIOS_COM_SendBufferNonBlocking(uint8_t port, uint8_t *buffer, uint16_t len);
extern int32_t PIOS_COM_SendBuffer(uint8_t port, uint8_t *buffer, uint16_t len);
extern int32_t PIOS_COM_SendStringNonBlocking(uint8_t port, char *str);
extern int32_t PIOS_COM_SendString(uint8_t port, char *str);
extern int32_t PIOS_COM_SendFormattedStringNonBlocking(uint8_t port, char *format, ...);
extern int32_t PIOS_COM_SendFormattedString(uint8_t port, char *format, ...);
extern uint8_t PIOS_COM_ReceiveBuffer(uint8_t port);
extern int32_t PIOS_COM_ReceiveBufferUsed(uint8_t port);
extern int32_t PIOS_COM_ReceiveHandler(void);
struct pios_com_driver {
void (*init)(uint8_t id);
void (*set_baud)(uint8_t id, uint32_t baud);
int32_t (*tx_nb)(uint8_t id, uint8_t *buffer, uint16_t len);
int32_t (*tx)(uint8_t id, uint8_t *buffer, uint16_t len);
int32_t (*rx)(uint8_t id);
int32_t (*rx_avail)(uint8_t id);
};
#endif /* PIOS_COM_H */
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_COM COM layer functions
* @brief Hardware communication layer
* @{
*
* @file pios_com.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* Parts by Thorsten Klose (tk@midibox.org)
* @brief COM layer functions 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 PIOS_COM_H
#define PIOS_COM_H
typedef uint16_t (*pios_com_callback)(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * task_woken);
struct pios_com_driver {
void (*init)(uint32_t id);
void (*set_baud)(uint32_t id, uint32_t baud);
void (*tx_start)(uint32_t id, uint16_t tx_bytes_avail);
void (*rx_start)(uint32_t id, uint16_t rx_bytes_avail);
void (*bind_rx_cb)(uint32_t id, pios_com_callback rx_in_cb, uint32_t context);
void (*bind_tx_cb)(uint32_t id, pios_com_callback tx_out_cb, uint32_t context);
};
/* Public Functions */
extern int32_t PIOS_COM_Init(uint32_t * com_id, const struct pios_com_driver * driver, uint32_t lower_id, uint8_t * rx_buffer, uint16_t rx_buffer_len, uint8_t * tx_buffer, uint16_t tx_buffer_len);
extern int32_t PIOS_COM_ChangeBaud(uint32_t com_id, uint32_t baud);
extern int32_t PIOS_COM_SendCharNonBlocking(uint32_t com_id, char c);
extern int32_t PIOS_COM_SendChar(uint32_t com_id, char c);
extern int32_t PIOS_COM_SendBufferNonBlocking(uint32_t com_id, const uint8_t *buffer, uint16_t len);
extern int32_t PIOS_COM_SendBuffer(uint32_t com_id, const uint8_t *buffer, uint16_t len);
extern int32_t PIOS_COM_SendStringNonBlocking(uint32_t com_id, const char *str);
extern int32_t PIOS_COM_SendString(uint32_t com_id, const char *str);
extern int32_t PIOS_COM_SendFormattedStringNonBlocking(uint32_t com_id, const char *format, ...);
extern int32_t PIOS_COM_SendFormattedString(uint32_t com_id, const char *format, ...);
extern uint16_t PIOS_COM_ReceiveBuffer(uint32_t com_id, uint8_t * buf, uint16_t buf_len, uint32_t timeout_ms);
extern int32_t PIOS_COM_ReceiveBufferUsed(uint32_t com_id);
#endif /* PIOS_COM_H */
/**
* @}
* @}
*/

17
flight/PiOS.posix/inc/pios_com_priv.h
View File

@ -1,5 +1,10 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_COM COM layer functions
* @brief Hardware communication layer
* @{
*
* @file pios_com_priv.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
@ -29,13 +34,11 @@
#include <pios.h>
struct pios_com_dev {
uint8_t id;
const struct pios_com_driver * const driver;
};
extern struct pios_com_dev pios_com_devs[];
extern const uint8_t pios_com_num_devices;
extern int32_t PIOS_COM_ReceiveHandler(uint32_t com_id);
#endif /* PIOS_COM_PRIV_H */
/**
* @}
* @}
*/

27
flight/PiOS.posix/inc/pios_initcall.h
View File

@ -38,7 +38,32 @@
* and we cannot define a linker script for each of them atm
*/
#define uavobj_initcall(fn)
typedef int32_t (*initcall_t)(void);
typedef struct {
initcall_t fn_minit;
initcall_t fn_tinit;
} initmodule_t;
/* Init module section */
extern initmodule_t __module_initcall_start[], __module_initcall_end[];
extern void InitModules();
extern void StartModules();
#define UAVOBJ_INITCALL(fn)
#define MODULE_INITCALL(ifn, sfn)
#define MODULE_TASKCREATE_ALL { \
/* Start all module threads */ \
StartModules(); \
}
#define MODULE_INITIALISE_ALL { \
/* Initialize modules */ \
InitModules(); \
/* Initialize the system thread */ \
SystemModInitialize();}
#endif /* PIOS_INITCALL_H */

38
flight/PiOS.posix/inc/pios_irq.h Normal file
View File

@ -0,0 +1,38 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_IRQ IRQ Setup Functions
* @{
*
* @file pios_irq.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* Parts by Thorsten Klose (tk@midibox.org)
* @brief IRQ functions 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 PIOS_IRQ_H
#define PIOS_IRQ_H
/* Public Functions */
extern int32_t PIOS_IRQ_Disable(void);
extern int32_t PIOS_IRQ_Enable(void);
#endif /* PIOS_IRQ_H */

54
flight/PiOS.posix/inc/pios_rcvr.h Normal file
View File

@ -0,0 +1,54 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_RCVR RCVR layer functions
* @brief Hardware communication layer
* @{
*
* @file pios_rcvr.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief RCVR layer functions 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 PIOS_RCVR_H
#define PIOS_RCVR_H
struct pios_rcvr_channel_map {
uint32_t id;
uint8_t channel;
};
extern struct pios_rcvr_channel_map pios_rcvr_channel_to_id_map[];
struct pios_rcvr_driver {
void (*init)(uint32_t id);
int32_t (*read)(uint32_t id, uint8_t channel);
};
/* Public Functions */
extern int32_t PIOS_RCVR_Read(uint32_t rcvr_id, uint8_t channel);
#endif /* PIOS_RCVR_H */
/**
* @}
* @}
*/

48
flight/PiOS.posix/inc/pios_rcvr_priv.h Normal file
View File

@ -0,0 +1,48 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_RCVR RCVR Functions
* @brief PIOS interface for RCVR drivers
* @{
*
* @file pios_rcvr_priv.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* Parts by Thorsten Klose (tk@midibox.org)
* @brief USART private definitions.
* @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 PIOS_RCVR_PRIV_H
#define PIOS_RCVR_PRIV_H
#include <pios.h>
extern uint32_t pios_rcvr_max_channel;
extern int32_t PIOS_RCVR_Init(uint32_t * rcvr_id, const struct pios_rcvr_driver * driver, const uint32_t lower_id);
extern void PIOS_RCVR_IRQ_Handler(uint32_t rcvr_id);
#endif /* PIOS_RCVR_PRIV_H */
/**
* @}
* @}
*/

16
flight/PiOS.posix/inc/pios_udp.h
View File

@ -31,21 +31,5 @@
/* Global Types */
/* Public Functions */
//extern void PIOS_UDP_Init(void);
void PIOS_UDP_Init(void);
extern void PIOS_UDP_ChangeBaud(uint8_t usart, uint32_t baud);
extern int32_t PIOS_UDP_RxBufferFree(uint8_t usart);
extern int32_t PIOS_UDP_RxBufferUsed(uint8_t usart);
extern int32_t PIOS_UDP_RxBufferGet(uint8_t usart);
extern int32_t PIOS_UDP_RxBufferPeek(uint8_t usart);
extern int32_t PIOS_UDP_RxBufferPut(uint8_t usart, uint8_t b);
extern int32_t PIOS_UDP_TxBufferFree(uint8_t usart);
extern int32_t PIOS_UDP_TxBufferGet(uint8_t usart);
extern int32_t PIOS_UDP_TxBufferPutMoreNonBlocking(uint8_t usart, uint8_t *buffer, uint16_t len);
extern int32_t PIOS_UDP_TxBufferPutMore(uint8_t usart, uint8_t *buffer, uint16_t len);
extern int32_t PIOS_UDP_TxBufferPutNonBlocking(uint8_t usart, uint8_t b);
extern int32_t PIOS_UDP_TxBufferPut(uint8_t usart, uint8_t b);
#endif /* PIOS_UDP_H */

33
flight/PiOS.posix/inc/pios_udp_priv.h
View File

@ -29,6 +29,7 @@
#include <pios.h>
#include <stdio.h>
#include <pthread.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <stdlib.h>
@ -37,30 +38,34 @@
#include <fcntl.h>
#include <netinet/in.h>
struct pios_udp_cfg {
const char * ip;
uint16_t port;
};
struct pios_udp_buffer {
uint8_t buf[PIOS_UDP_RX_BUFFER_SIZE];
uint16_t head;
uint16_t tail;
uint16_t size;
};
typedef struct {
const struct pios_udp_cfg * cfg;
pthread_t rxThread;
struct pios_udp_dev {
const struct pios_udp_cfg * const cfg;
struct pios_udp_buffer rx;
int socket;
struct sockaddr_in server;
struct sockaddr_in client;
uint32_t clientLength;
};
extern struct pios_udp_dev pios_udp_devs[];
extern uint8_t pios_udp_num_devices;
pthread_cond_t cond;
pthread_mutex_t mutex;
pios_com_callback tx_out_cb;
uint32_t tx_out_context;
pios_com_callback rx_in_cb;
uint32_t rx_in_context;
uint8_t rx_buffer[PIOS_UDP_RX_BUFFER_SIZE];
uint8_t tx_buffer[PIOS_UDP_RX_BUFFER_SIZE];
} pios_udp_dev;
extern int32_t PIOS_UDP_Init(uint32_t * udp_id, const struct pios_udp_cfg * cfg);
#endif /* PIOS_UDP_PRIV_H */

2
flight/PiOS.posix/pios.h
View File

@ -58,6 +58,7 @@
#include <pios_sys.h>
#include <pios_delay.h>
#include <pios_led.h>
#include <pios_irq.h>
#include <pios_sdcard.h>
#include <pios_udp.h>
#include <pios_com.h>
@ -65,6 +66,7 @@
#include <pios_wdg.h>
#include <pios_debug.h>
#include <pios_crc.h>
#include <pios_rcvr.h>
#define NELEMENTS(x) (sizeof(x) / sizeof(*(x)))

743
flight/PiOS.posix/posix/pios_com.c
View File

@ -1,285 +1,522 @@
/**
******************************************************************************
*
* @file pios_com.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* Parts by Thorsten Klose (tk@midibox.org)
* @brief COM layer functions
* @see The GNU Public License (GPL) Version 3
* @defgroup PIOS_COM COM layer functions
* @{
*
*****************************************************************************/
/*
* 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
*/
/* Project Includes */
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_COM COM layer functions
* @brief Hardware communication layer
* @{
*
* @file pios_com.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* Parts by Thorsten Klose (tk@midibox.org)
* @brief COM layer functions
* @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
*/
/* Project Includes */
#include "pios.h"
#if defined(PIOS_INCLUDE_COM)
#include "fifo_buffer.h"
#include <pios_com_priv.h>
static struct pios_com_dev * find_com_dev_by_id (uint8_t port)
{
if (port >= pios_com_num_devices) {
/* Undefined COM port for this board (see pios_board.c) */
return NULL;
}
#if !defined(PIOS_INCLUDE_FREERTOS)
#include "pios_delay.h" /* PIOS_DELAY_WaitmS */
#endif
/* Get a handle for the device configuration */
return &(pios_com_devs[port]);
enum pios_com_dev_magic {
PIOS_COM_DEV_MAGIC = 0xaa55aa55,
};
struct pios_com_dev {
enum pios_com_dev_magic magic;
uint32_t lower_id;
const struct pios_com_driver * driver;
#if defined(PIOS_INCLUDE_FREERTOS)
xSemaphoreHandle tx_sem;
xSemaphoreHandle rx_sem;
#endif
bool has_rx;
bool has_tx;
t_fifo_buffer rx;
t_fifo_buffer tx;
};
static bool PIOS_COM_validate(struct pios_com_dev * com_dev)
{
return (com_dev && (com_dev->magic == PIOS_COM_DEV_MAGIC));
}
/**
* Initialises COM layer
* \param[in] mode currently only mode 0 supported
* \return < 0 if initialisation failed
*/
int32_t PIOS_COM_Init(void)
{
int32_t ret = 0;
/* If any COM assignment: */
#if defined(PIOS_INCLUDE_SERIAL)
PIOS_SERIAL_Init();
#endif
#if defined(PIOS_INCLUDE_UDP)
PIOS_UDP_Init();
#endif
return ret;
}
/**
* Change the port speed without re-initializing
* \param[in] port COM port
* \param[in] baud Requested baud rate
* \return -1 if port not available
* \return 0 on success
*/
int32_t PIOS_COM_ChangeBaud(uint8_t port, uint32_t baud)
#if defined(PIOS_INCLUDE_FREERTOS) && 0
// static struct pios_com_dev * PIOS_COM_alloc(void)
//{
// struct pios_com_dev * com_dev;
//
// com_dev = (struct pios_com_dev *)malloc(sizeof(*com_dev));
// if (!com_dev) return (NULL);
//
// com_dev->magic = PIOS_COM_DEV_MAGIC;
// return(com_dev);
//}
#else
static struct pios_com_dev pios_com_devs[PIOS_COM_MAX_DEVS];
static uint8_t pios_com_num_devs;
static uint32_t PIOS_COM_create(void)
{
struct pios_com_dev * com_dev;
struct pios_com_dev * com_dev;
com_dev = find_com_dev_by_id (port);
if (pios_com_num_devs >= PIOS_COM_MAX_DEVS) {
return (PIOS_COM_MAX_DEVS+1);
}
if (!com_dev) {
/* Undefined COM port for this board (see pios_board.c) */
return -1;
}
com_dev = &pios_com_devs[pios_com_num_devs++];
com_dev->magic = PIOS_COM_DEV_MAGIC;
/* Invoke the driver function if it exists */
if (com_dev->driver->set_baud) {
com_dev->driver->set_baud(com_dev->id, baud);
}
return 0;
return (pios_com_num_devs);
}
/**
* Sends a package over given port
* \param[in] port COM port
* \param[in] buffer character buffer
* \param[in] len buffer length
* \return -1 if port not available
* \return -2 if non-blocking mode activated: buffer is full
* caller should retry until buffer is free again
* \return 0 on success
*/
int32_t PIOS_COM_SendBufferNonBlocking(uint8_t port, uint8_t *buffer, uint16_t len)
static struct pios_com_dev * PIOS_COM_find_dev(uint32_t com_dev_id)
{
struct pios_com_dev * com_dev;
if (!com_dev_id) return NULL;
if (com_dev_id>pios_com_num_devs+1) return NULL;
return &pios_com_devs[com_dev_id-1];
}
#endif
com_dev = find_com_dev_by_id (port);
static uint16_t PIOS_COM_TxOutCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield);
static uint16_t PIOS_COM_RxInCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield);
static void PIOS_COM_UnblockRx(struct pios_com_dev * com_dev, bool * need_yield);
static void PIOS_COM_UnblockTx(struct pios_com_dev * com_dev, bool * need_yield);
if (!com_dev) {
/* Undefined COM port for this board (see pios_board.c) */
return -1;
}
/**
* Initialises COM layer
* \param[out] handle
* \param[in] driver
* \param[in] id
* \return < 0 if initialisation failed
*/
int32_t PIOS_COM_Init(uint32_t * com_id, const struct pios_com_driver * driver, uint32_t lower_id, uint8_t * rx_buffer, uint16_t rx_buffer_len, uint8_t * tx_buffer, uint16_t tx_buffer_len)
{
PIOS_Assert(com_id);
PIOS_Assert(driver);
/* Invoke the driver function if it exists */
if (com_dev->driver->tx_nb) {
return com_dev->driver->tx_nb(com_dev->id, buffer, len);
}
bool has_rx = (rx_buffer && rx_buffer_len > 0);
bool has_tx = (tx_buffer && tx_buffer_len > 0);
PIOS_Assert(has_rx || has_tx);
PIOS_Assert(driver->bind_tx_cb || !has_tx);
PIOS_Assert(driver->bind_rx_cb || !has_rx);
return 0;
uint32_t com_dev_id;
struct pios_com_dev * com_dev;
com_dev_id = PIOS_COM_create();
com_dev = PIOS_COM_find_dev(com_dev_id);
if (!com_dev) goto out_fail;
com_dev->driver = driver;
com_dev->lower_id = lower_id;
com_dev->has_rx = has_rx;
com_dev->has_tx = has_tx;
if (has_rx) {
fifoBuf_init(&com_dev->rx, rx_buffer, rx_buffer_len);
#if defined(PIOS_INCLUDE_FREERTOS)
vSemaphoreCreateBinary(com_dev->rx_sem);
#endif /* PIOS_INCLUDE_FREERTOS */
(com_dev->driver->bind_rx_cb)(lower_id, PIOS_COM_RxInCallback, com_dev_id);
if (com_dev->driver->rx_start) {
/* Start the receiver */
(com_dev->driver->rx_start)(com_dev->lower_id,
fifoBuf_getFree(&com_dev->rx));
}
}
if (has_tx) {
fifoBuf_init(&com_dev->tx, tx_buffer, tx_buffer_len);
#if defined(PIOS_INCLUDE_FREERTOS)
vSemaphoreCreateBinary(com_dev->tx_sem);
#endif /* PIOS_INCLUDE_FREERTOS */
(com_dev->driver->bind_tx_cb)(lower_id, PIOS_COM_TxOutCallback, com_dev_id);
}
*com_id = com_dev_id;
return(0);
out_fail:
return(-1);
}
/**
* Sends a package over given port
* (blocking function)
* \param[in] port COM port
* \param[in] buffer character buffer
* \param[in] len buffer length
* \return -1 if port not available
* \return 0 on success
*/
int32_t PIOS_COM_SendBuffer(uint8_t port, uint8_t *buffer, uint16_t len)
static void PIOS_COM_UnblockRx(struct pios_com_dev * com_dev, bool * need_yield)
{
struct pios_com_dev * com_dev;
#if defined(PIOS_INCLUDE_FREERTOS)
static signed portBASE_TYPE xHigherPriorityTaskWoken;
xSemaphoreGiveFromISR(com_dev->rx_sem, &xHigherPriorityTaskWoken);
com_dev = find_com_dev_by_id (port);
if (!com_dev) {
/* Undefined COM port for this board (see pios_board.c) */
return -1;
}
/* Invoke the driver function if it exists */
if (com_dev->driver->tx) {
return com_dev->driver->tx(com_dev->id, buffer, len);
}
return 0;
if (xHigherPriorityTaskWoken != pdFALSE) {
*need_yield = true;
} else {
*need_yield = false;
}
#else
*need_yield = false;
#endif
}
/**
* Sends a single character over given port
* \param[in] port COM port
* \param[in] c character
* \return -1 if port not available
* \return -2 buffer is full
* caller should retry until buffer is free again
* \return 0 on success
*/
int32_t PIOS_COM_SendCharNonBlocking(uint8_t port, char c)
{
return PIOS_COM_SendBufferNonBlocking(port, (uint8_t *)&c, 1);
}
/**
* Sends a single character over given port
* (blocking function)
* \param[in] port COM port
* \param[in] c character
* \return -1 if port not available
* \return 0 on success
*/
int32_t PIOS_COM_SendChar(uint8_t port, char c)
{
return PIOS_COM_SendBuffer(port, (uint8_t *)&c, 1);
}
/**
* Sends a string over given port
* \param[in] port COM port
* \param[in] str zero-terminated string
* \return -1 if port not available
* \return -2 buffer is full
* caller should retry until buffer is free again
* \return 0 on success
*/
int32_t PIOS_COM_SendStringNonBlocking(uint8_t port, char *str)
{
return PIOS_COM_SendBufferNonBlocking(port, (uint8_t *)str, (uint16_t)strlen(str));
}
/**
* Sends a string over given port
* (blocking function)
* \param[in] port COM port
* \param[in] str zero-terminated string
* \return -1 if port not available
* \return 0 on success
*/
int32_t PIOS_COM_SendString(uint8_t port, char *str)
{
return PIOS_COM_SendBuffer(port, (uint8_t *)str, strlen(str));
}
/**
* Sends a formatted string (-> printf) over given port
* \param[in] port COM port
* \param[in] *format zero-terminated format string - 128 characters supported maximum!
* \param[in] ... optional arguments,
* 128 characters supported maximum!
* \return -2 if non-blocking mode activated: buffer is full
* caller should retry until buffer is free again
* \return 0 on success
*/
int32_t PIOS_COM_SendFormattedStringNonBlocking(uint8_t port, char *format, ...)
{
uint8_t buffer[128]; // TODO: tmp!!! Provide a streamed COM method later!
va_list args;
va_start(args, format);
vsprintf((char *)buffer, format, args);
return PIOS_COM_SendBufferNonBlocking(port, buffer, (uint16_t)strlen((char *)buffer));
}
/**
* Sends a formatted string (-> printf) over given port
* (blocking function)
* \param[in] port COM port
* \param[in] *format zero-terminated format string - 128 characters supported maximum!
* \param[in] ... optional arguments,
* \return -1 if port not available
* \return 0 on success
*/
int32_t PIOS_COM_SendFormattedString(uint8_t port, char *format, ...)
{
uint8_t buffer[128]; // TODO: tmp!!! Provide a streamed COM method later!
va_list args;
va_start(args, format);
vsprintf((char *)buffer, format, args);
return PIOS_COM_SendBuffer(port, buffer, (uint16_t)strlen((char *)buffer));
}
/**
* Transfer bytes from port buffers into another buffer
* \param[in] port COM port
static void PIOS_COM_UnblockTx(struct pios_com_dev * com_dev, bool * need_yield)
{
#if defined(PIOS_INCLUDE_FREERTOS)
static signed portBASE_TYPE xHigherPriorityTaskWoken;
xSemaphoreGiveFromISR(com_dev->tx_sem, &xHigherPriorityTaskWoken);
if (xHigherPriorityTaskWoken != pdFALSE) {
*need_yield = true;
} else {
*need_yield = false;
}
#else
*need_yield = false;
#endif
}
static uint16_t PIOS_COM_RxInCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield)
{
struct pios_com_dev * com_dev = PIOS_COM_find_dev(context);
bool valid = PIOS_COM_validate(com_dev);
PIOS_Assert(valid);
PIOS_Assert(com_dev->has_rx);
PIOS_IRQ_Disable();
uint16_t bytes_into_fifo = fifoBuf_putData(&com_dev->rx, buf, buf_len);
PIOS_IRQ_Enable();
if (bytes_into_fifo > 0) {
/* Data has been added to the buffer */
PIOS_COM_UnblockRx(com_dev, need_yield);
}
if (headroom) {
*headroom = fifoBuf_getFree(&com_dev->rx);
}
return (bytes_into_fifo);
}
static uint16_t PIOS_COM_TxOutCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield)
{
struct pios_com_dev * com_dev = PIOS_COM_find_dev(context);
bool valid = PIOS_COM_validate(com_dev);
PIOS_Assert(valid);
PIOS_Assert(buf);
PIOS_Assert(buf_len);
PIOS_Assert(com_dev->has_tx);
PIOS_IRQ_Disable();
uint16_t bytes_from_fifo = fifoBuf_getData(&com_dev->tx, buf, buf_len);
PIOS_IRQ_Enable();
if (bytes_from_fifo > 0) {
/* More space has been made in the buffer */
PIOS_COM_UnblockTx(com_dev, need_yield);
}
if (headroom) {
*headroom = fifoBuf_getUsed(&com_dev->tx);
}
return (bytes_from_fifo);
}
/**
* Change the port speed without re-initializing
* \param[in] port COM port
* \param[in] baud Requested baud rate
* \return -1 if port not available
* \return 0 on success
*/
int32_t PIOS_COM_ChangeBaud(uint32_t com_id, uint32_t baud)
{
struct pios_com_dev * com_dev = PIOS_COM_find_dev(com_id);
if (!PIOS_COM_validate(com_dev)) {
/* Undefined COM port for this board (see pios_board.c) */
return -1;
}
/* Invoke the driver function if it exists */
if (com_dev->driver->set_baud) {
com_dev->driver->set_baud(com_dev->lower_id, baud);
}
return 0;
}
/**
* Sends a package over given port
* \param[in] port COM port
* \param[in] buffer character buffer
* \param[in] len buffer length
* \return -1 if port not available
* \return -2 if non-blocking mode activated: buffer is full
* caller should retry until buffer is free again
* \return 0 on success
*/
int32_t PIOS_COM_SendBufferNonBlocking(uint32_t com_id, const uint8_t *buffer, uint16_t len)
{
struct pios_com_dev * com_dev = PIOS_COM_find_dev(com_id);
if (!PIOS_COM_validate(com_dev)) {
/* Undefined COM port for this board (see pios_board.c) */
return -1;
}
PIOS_Assert(com_dev->has_tx);
if (len >= fifoBuf_getFree(&com_dev->tx)) {
/* Buffer cannot accept all requested bytes (retry) */
return -2;
}
PIOS_IRQ_Disable();
uint16_t bytes_into_fifo = fifoBuf_putData(&com_dev->tx, buffer, len);
PIOS_IRQ_Enable();
if (bytes_into_fifo > 0) {
/* More data has been put in the tx buffer, make sure the tx is started */
if (com_dev->driver->tx_start) {
com_dev->driver->tx_start(com_dev->lower_id,
fifoBuf_getUsed(&com_dev->tx));
}
}
return (0);
}
/**
* Sends a package over given port
* (blocking function)
* \param[in] port COM port
* \param[in] buffer character buffer
* \param[in] len buffer length
* \return -1 if port not available
* \return 0 on success
*/
int32_t PIOS_COM_SendBuffer(uint32_t com_id, const uint8_t *buffer, uint16_t len)
{
struct pios_com_dev * com_dev = PIOS_COM_find_dev(com_id);
if (!PIOS_COM_validate(com_dev)) {
/* Undefined COM port for this board (see pios_board.c) */
return -1;
}
PIOS_Assert(com_dev->has_tx);
int32_t rc;
do {
rc = PIOS_COM_SendBufferNonBlocking(com_id, buffer, len);
#if defined(PIOS_INCLUDE_FREERTOS)
if (rc == -2) {
/* Make sure the transmitter is running while we wait */
if (com_dev->driver->tx_start) {
(com_dev->driver->tx_start)(com_dev->lower_id,
fifoBuf_getUsed(&com_dev->tx));
}
if (xSemaphoreTake(com_dev->tx_sem, portMAX_DELAY) != pdTRUE) {
return -3;
}
}
#endif
} while (rc == -2);
return rc;
}
/**
* Sends a single character over given port
* \param[in] port COM port
* \param[in] c character
* \return -1 if port not available
* \return -2 buffer is full
* caller should retry until buffer is free again
* \return 0 on success
*/
int32_t PIOS_COM_SendCharNonBlocking(uint32_t com_id, char c)
{
return PIOS_COM_SendBufferNonBlocking(com_id, (uint8_t *)&c, 1);
}
/**
* Sends a single character over given port
* (blocking function)
* \param[in] port COM port
* \param[in] c character
* \return -1 if port not available
* \return 0 on success
*/
int32_t PIOS_COM_SendChar(uint32_t com_id, char c)
{
return PIOS_COM_SendBuffer(com_id, (uint8_t *)&c, 1);
}
/**
* Sends a string over given port
* \param[in] port COM port
* \param[in] str zero-terminated string
* \return -1 if port not available
* \return -2 buffer is full
* caller should retry until buffer is free again
* \return 0 on success
*/
int32_t PIOS_COM_SendStringNonBlocking(uint32_t com_id, const char *str)
{
return PIOS_COM_SendBufferNonBlocking(com_id, (uint8_t *)str, (uint16_t)strlen(str));
}
/**
* Sends a string over given port
* (blocking function)
* \param[in] port COM port
* \param[in] str zero-terminated string
* \return -1 if port not available
* \return 0 on success
*/
int32_t PIOS_COM_SendString(uint32_t com_id, const char *str)
{
return PIOS_COM_SendBuffer(com_id, (uint8_t *)str, strlen(str));
}
/**
* Sends a formatted string (-> printf) over given port
* \param[in] port COM port
* \param[in] *format zero-terminated format string - 128 characters supported maximum!
* \param[in] ... optional arguments,
* 128 characters supported maximum!
* \return -2 if non-blocking mode activated: buffer is full
* caller should retry until buffer is free again
* \return 0 on success
*/
int32_t PIOS_COM_SendFormattedStringNonBlocking(uint32_t com_id, const char *format, ...)
{
uint8_t buffer[128]; // TODO: tmp!!! Provide a streamed COM method later!
va_list args;
va_start(args, format);
vsprintf((char *)buffer, format, args);
return PIOS_COM_SendBufferNonBlocking(com_id, buffer, (uint16_t)strlen((char *)buffer));
}
/**
* Sends a formatted string (-> printf) over given port
* (blocking function)
* \param[in] port COM port
* \param[in] *format zero-terminated format string - 128 characters supported maximum!
* \param[in] ... optional arguments,
* \return -1 if port not available
* \return 0 on success
*/
int32_t PIOS_COM_SendFormattedString(uint32_t com_id, const char *format, ...)
{
uint8_t buffer[128]; // TODO: tmp!!! Provide a streamed COM method later!
va_list args;
va_start(args, format);
vsprintf((char *)buffer, format, args);
return PIOS_COM_SendBuffer(com_id, buffer, (uint16_t)strlen((char *)buffer));
}
/**
* Transfer bytes from port buffers into another buffer
* \param[in] port COM port
* \returns Byte from buffer
*/
uint8_t PIOS_COM_ReceiveBuffer(uint8_t port)
{
struct pios_com_dev * com_dev;
com_dev = find_com_dev_by_id (port);
//PIOS_DEBUG_Assert(com_dev);
//PIOS_DEBUG_Assert(com_dev->driver->rx);
return com_dev->driver->rx(com_dev->id);
}
/**
* Get the number of bytes waiting in the buffer
* \param[in] port COM port
* \return Number of bytes used in buffer
*/
int32_t PIOS_COM_ReceiveBufferUsed(uint8_t port)
*/
uint16_t PIOS_COM_ReceiveBuffer(uint32_t com_id, uint8_t * buf, uint16_t buf_len, uint32_t timeout_ms)
{
struct pios_com_dev * com_dev;
PIOS_Assert(buf);
PIOS_Assert(buf_len);
com_dev = find_com_dev_by_id (port);
struct pios_com_dev * com_dev = PIOS_COM_find_dev(com_id);
if (!com_dev) {
/* Undefined COM port for this board (see pios_board.c) */
return 0;
}
if (!PIOS_COM_validate(com_dev)) {
/* Undefined COM port for this board (see pios_board.c) */
PIOS_Assert(0);
}
PIOS_Assert(com_dev->has_rx);
if (!com_dev->driver->rx_avail) {
return 0;
}
check_again:
PIOS_IRQ_Disable();
uint16_t bytes_from_fifo = fifoBuf_getData(&com_dev->rx, buf, buf_len);
PIOS_IRQ_Enable();
return com_dev->driver->rx_avail(com_dev->id);
if (bytes_from_fifo == 0 && timeout_ms > 0) {
/* No more bytes in receive buffer */
/* Make sure the receiver is running while we wait */
if (com_dev->driver->rx_start) {
/* Notify the lower layer that there is now room in the rx buffer */
(com_dev->driver->rx_start)(com_dev->lower_id,
fifoBuf_getFree(&com_dev->rx));
}
#if defined(PIOS_INCLUDE_FREERTOS)
if (xSemaphoreTake(com_dev->rx_sem, timeout_ms / portTICK_RATE_MS) == pdTRUE) {
/* Make sure we don't come back here again */
timeout_ms = 0;
goto check_again;
}
#else
PIOS_DELAY_WaitmS(1);
timeout_ms--;
goto check_again;
#endif
}
/* Return received byte */
return (bytes_from_fifo);
}
#endif
/**
* Get the number of bytes waiting in the buffer
* \param[in] port COM port
* \return Number of bytes used in buffer
*/
int32_t PIOS_COM_ReceiveBufferUsed(uint32_t com_id)
{
struct pios_com_dev * com_dev = PIOS_COM_find_dev(com_id);
if (!PIOS_COM_validate(com_dev)) {
/* Undefined COM port for this board (see pios_board.c) */
PIOS_Assert(0);
}
PIOS_Assert(com_dev->has_rx);
return (fifoBuf_getUsed(&com_dev->rx));
}
#endif
/**
* @}
* @}
*/

4
flight/PiOS.posix/posix/pios_delay.c
View File

@ -67,10 +67,6 @@ int32_t PIOS_DELAY_WaituS(uint16_t uS)
while (!nanosleep(&wait,&rest)) {
wait=rest;
}
//uint16_t start = PIOS_DELAY_TIMER->CNT;
/* Note that this event works on 16bit counter wrap-arounds */
//while((uint16_t)(PIOS_DELAY_TIMER->CNT - start) <= uS);
/* No error */
return 0;

69
flight/PiOS.posix/posix/pios_irq.c Normal file
View File

@ -0,0 +1,69 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_IRQ IRQ Setup Functions
* @brief STM32 Hardware code to enable and disable interrupts
* @{
*
* @file pios_irq.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org)
* @brief IRQ Enable/Disable routines
* @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
*/
/* Project Includes */
#include "pios.h"
#if defined(PIOS_INCLUDE_IRQ)
/* Private Function Prototypes */
/**
* Disables all interrupts (nested)
* \return < 0 On errors
*/
int32_t PIOS_IRQ_Disable(void)
{
#if defined(PIOS_INCLUDE_FREERTOS)
taskENTER_CRITICAL();
#endif
return 0;
}
/**
* Enables all interrupts (nested)
* \return < 0 on errors
* \return -1 on nesting errors (PIOS_IRQ_Disable() hasn't been called before)
*/
int32_t PIOS_IRQ_Enable(void)
{
#if defined(PIOS_INCLUDE_FREERTOS)
taskEXIT_CRITICAL();
#endif
return 0;
}
#endif
/**
* @}
* @}
*/

100
flight/PiOS.posix/posix/pios_rcvr.c Normal file
View File

@ -0,0 +1,100 @@
/* Project Includes */
#include "pios.h"
#if defined(PIOS_INCLUDE_RCVR)
#include <pios_rcvr_priv.h>
enum pios_rcvr_dev_magic {
PIOS_RCVR_DEV_MAGIC = 0x99aabbcc,
};
struct pios_rcvr_dev {
enum pios_rcvr_dev_magic magic;
uint32_t lower_id;
const struct pios_rcvr_driver * driver;
};
static bool PIOS_RCVR_validate(struct pios_rcvr_dev * rcvr_dev)
{
return (rcvr_dev->magic == PIOS_RCVR_DEV_MAGIC);
}
#if defined(PIOS_INCLUDE_FREERTOS) && 0
static struct pios_rcvr_dev * PIOS_RCVR_alloc(void)
{
struct pios_rcvr_dev * rcvr_dev;
rcvr_dev = (struct pios_rcvr_dev *)malloc(sizeof(*rcvr_dev));
if (!rcvr_dev) return (NULL);
rcvr_dev->magic = PIOS_RCVR_DEV_MAGIC;
return(rcvr_dev);
}
#else
static struct pios_rcvr_dev pios_rcvr_devs[PIOS_RCVR_MAX_DEVS];
static uint8_t pios_rcvr_num_devs;
static struct pios_rcvr_dev * PIOS_RCVR_alloc(void)
{
struct pios_rcvr_dev * rcvr_dev;
if (pios_rcvr_num_devs >= PIOS_RCVR_MAX_DEVS) {
return (NULL);
}
rcvr_dev = &pios_rcvr_devs[pios_rcvr_num_devs++];
rcvr_dev->magic = PIOS_RCVR_DEV_MAGIC;
return (rcvr_dev);
}
#endif
/**
* Initialises RCVR layer
* \param[out] handle
* \param[in] driver
* \param[in] id
* \return < 0 if initialisation failed
*/
int32_t PIOS_RCVR_Init(uint32_t * rcvr_id, const struct pios_rcvr_driver * driver, const uint32_t lower_id)
{
PIOS_DEBUG_Assert(rcvr_id);
PIOS_DEBUG_Assert(driver);
struct pios_rcvr_dev * rcvr_dev;
rcvr_dev = (struct pios_rcvr_dev *) PIOS_RCVR_alloc();
if (!rcvr_dev) goto out_fail;
rcvr_dev->driver = driver;
rcvr_dev->lower_id = lower_id;
*rcvr_id = pios_rcvr_num_devs - 1;
return(0);
out_fail:
return(-1);
}
int32_t PIOS_RCVR_Read(uint32_t rcvr_id, uint8_t channel)
{
struct pios_rcvr_dev * rcvr_dev = &pios_rcvr_devs[rcvr_id];
if (!PIOS_RCVR_validate(rcvr_dev)) {
/* Undefined RCVR port for this board (see pios_board.c) */
PIOS_DEBUG_Assert(0);
}
PIOS_DEBUG_Assert(rcvr_dev->driver->read);
return rcvr_dev->driver->read(rcvr_dev->lower_id, channel);
}
#endif
/**
* @}
* @}
*/

533
flight/PiOS.posix/posix/pios_udp.c
View File

@ -32,18 +32,35 @@
#if defined(PIOS_INCLUDE_UDP)
#include <signal.h>
#include <pios_udp_priv.h>
/* We need a list of UDP devices */
#define PIOS_UDP_MAX_DEV 256
static int8_t pios_udp_num_devices = 0;
static pios_udp_dev pios_udp_devices[PIOS_UDP_MAX_DEV];
/* Provide a COM driver */
static void PIOS_UDP_ChangeBaud(uint32_t udp_id, uint32_t baud);
static void PIOS_UDP_RegisterRxCallback(uint32_t udp_id, pios_com_callback rx_in_cb, uint32_t context);
static void PIOS_UDP_RegisterTxCallback(uint32_t udp_id, pios_com_callback tx_out_cb, uint32_t context);
static void PIOS_UDP_TxStart(uint32_t udp_id, uint16_t tx_bytes_avail);
static void PIOS_UDP_RxStart(uint32_t udp_id, uint16_t rx_bytes_avail);
const struct pios_com_driver pios_udp_com_driver = {
.set_baud = PIOS_UDP_ChangeBaud,
.tx_nb = PIOS_UDP_TxBufferPutMoreNonBlocking,
.tx = PIOS_UDP_TxBufferPutMore,
.rx = PIOS_UDP_RxBufferGet,
.rx_avail = PIOS_UDP_RxBufferUsed,
.set_baud = PIOS_UDP_ChangeBaud,
.tx_start = PIOS_UDP_TxStart,
.rx_start = PIOS_UDP_RxStart,
.bind_tx_cb = PIOS_UDP_RegisterTxCallback,
.bind_rx_cb = PIOS_UDP_RegisterRxCallback,
};
static struct pios_udp_dev * find_udp_dev_by_id (uint8_t udp)
static pios_udp_dev * find_udp_dev_by_id (uint8_t udp)
{
if (udp >= pios_udp_num_devices) {
/* Undefined UDP port for this board (see pios_board.c) */
@ -51,410 +68,174 @@ static struct pios_udp_dev * find_udp_dev_by_id (uint8_t udp)
}
/* Get a handle for the device configuration */
return &(pios_udp_devs[udp]);
return &(pios_udp_devices[udp]);
}
/**
* Open some UDP sockets
*/
void PIOS_UDP_Init(void)
{
struct pios_udp_dev * udp_dev;
uint8_t i;
for (i = 0; i < pios_udp_num_devices; i++) {
/* Get a handle for the device configuration */
udp_dev = find_udp_dev_by_id(i);
//PIOS_DEBUG_Assert(udp_dev);
/* Clear buffer counters */
udp_dev->rx.head = udp_dev->rx.tail = udp_dev->rx.size = 0;
/* assign socket */
udp_dev->socket = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
memset(&udp_dev->server,0,sizeof(udp_dev->server));
memset(&udp_dev->client,0,sizeof(udp_dev->client));
udp_dev->server.sin_family = AF_INET;
udp_dev->server.sin_addr.s_addr = inet_addr(udp_dev->cfg->ip);
udp_dev->server.sin_port = htons(udp_dev->cfg->port);
int res= bind(udp_dev->socket, (struct sockaddr *)&udp_dev->server,sizeof(udp_dev->server));
/* use nonblocking IO */
int flags = fcntl(udp_dev->socket, F_GETFL, 0);
fcntl(udp_dev->socket, F_SETFL, flags | O_NONBLOCK);
printf("udp dev %i - socket %i opened - result %i\n",i,udp_dev->socket,res);
/* TODO do some error handling - wait no, we can't - we are void anyway ;) */
}
}
/**
* Changes the baud rate of the UDP peripheral without re-initialising.
* \param[in] udp UDP name (GPS, TELEM, AUX)
* \param[in] baud Requested baud rate
*/
void PIOS_UDP_ChangeBaud(uint8_t udp, uint32_t baud)
{
}
/**
* puts a byte onto the receive buffer
* \param[in] UDP UDP name
* \param[in] b byte which should be put into Rx buffer
* \return 0 if no error
* \return -1 if UDP not available
* \return -2 if buffer full (retry)
* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions
*/
int32_t PIOS_UDP_RxBufferPut(uint8_t udp, uint8_t b)
{
struct pios_udp_dev * udp_dev;
/* Get a handle for the device configuration */
udp_dev = find_udp_dev_by_id(udp);
if (!udp_dev) {
/* Undefined UDP port for this board (see pios_board.c) */
return -1;
}
if (udp_dev->rx.size >= sizeof(udp_dev->rx.buf)) {
/* Buffer full (retry) */
return -2;
}
/* Copy received byte into receive buffer */
udp_dev->rx.buf[udp_dev->rx.head++] = b;
if (udp_dev->rx.head >= sizeof(udp_dev->rx.buf)) {
udp_dev->rx.head = 0;
}
udp_dev->rx.size++;
/* No error */
return 0;
}
/**
* attempt to receive
* RxThread
*/
void PIOS_UDP_RECV(uint8_t udp) {
struct pios_udp_dev * udp_dev;
unsigned char localbuffer[PIOS_UDP_RX_BUFFER_SIZE];
/* Get a handle for the device configuration */
udp_dev = find_udp_dev_by_id(udp);
if (!udp_dev) {
/* Undefined UDP port for this board (see pios_board.c) */
return;
}
/* use nonblocking IO */
int flags = fcntl(udp_dev->socket, F_GETFL, 0);
fcntl(udp_dev->socket, F_SETFL, flags | O_NONBLOCK);
/* receive data */
int received;
udp_dev->clientLength=sizeof(udp_dev->client);
if ((received = recvfrom(udp_dev->socket,
localbuffer,
(PIOS_UDP_RX_BUFFER_SIZE - udp_dev->rx.size),
0,
(struct sockaddr *) &udp_dev->client,
(socklen_t*)&udp_dev->clientLength)) < 0) {
return;
}
/* copy received data to buffer */
int t;
for (t=0;t<received;t++) {
PIOS_UDP_RxBufferPut(udp,localbuffer[t]);
}
}
/**
* Returns number of free bytes in receive buffer
* \param[in] UDP UDP name
* \return UDP number of free bytes
* \return 1: UDP available
* \return 0: UDP not available
*/
int32_t PIOS_UDP_RxBufferFree(uint8_t udp)
void * PIOS_UDP_RxThread(void * udp_dev_n)
{
struct pios_udp_dev * udp_dev;
/* Get a handle for the device configuration */
udp_dev = find_udp_dev_by_id(udp);
/* needed because of FreeRTOS.posix scheduling */
sigset_t set;
sigfillset(&set);
sigprocmask(SIG_BLOCK, &set, NULL);
if (!udp_dev) {
/* Undefined UDP port for this board (see pios_board.c) */
return -2;
}
pios_udp_dev * udp_dev = (pios_udp_dev*) udp_dev_n;
/* fill buffer */
PIOS_UDP_RECV(udp);
/**
* com devices never get closed except by application "reboot"
* we also never give up our mutex except for waiting
*/
while(1) {
return (sizeof(udp_dev->rx.buf) - udp_dev->rx.size);
}
/**
* receive
*/
int received;
udp_dev->clientLength=sizeof(udp_dev->client);
if ((received = recvfrom(udp_dev->socket,
&udp_dev->rx_buffer,
PIOS_UDP_RX_BUFFER_SIZE,
0,
(struct sockaddr *) &udp_dev->client,
(socklen_t*)&udp_dev->clientLength)) >= 0)
{
/**
* Returns number of used bytes in receive buffer
* \param[in] UDP UDP name
* \return > 0: number of used bytes
* \return 0 if UDP not available
* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions
*/
int32_t PIOS_UDP_RxBufferUsed(uint8_t udp)
{
struct pios_udp_dev * udp_dev;
/* copy received data to buffer if possible */
/* we do NOT buffer data locally. If the com buffer can't receive, data is discarded! */
/* (thats what the USART driver does too!) */
bool rx_need_yield = false;
if (udp_dev->rx_in_cb) {
(void) (udp_dev->rx_in_cb)(udp_dev->rx_in_context, udp_dev->rx_buffer, received, NULL, &rx_need_yield);
}
/* Get a handle for the device configuration */
udp_dev = find_udp_dev_by_id(udp);
#if defined(PIOS_INCLUDE_FREERTOS)
if (rx_need_yield) {
vPortYieldFromISR();
}
#endif /* PIOS_INCLUDE_FREERTOS */
if (!udp_dev) {
/* Undefined UDP port for this board (see pios_board.c) */
return -2;
}
}
/* fill buffer */
PIOS_UDP_RECV(udp);
return (udp_dev->rx.size);
}
/**
* Gets a byte from the receive buffer
* \param[in] UDP UDP name
* \return -1 if UDP not available
* \return -2 if no new byte available
* \return >= 0: number of received bytes
* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions
*/
int32_t PIOS_UDP_RxBufferGet(uint8_t udp)
{
struct pios_udp_dev * udp_dev;
/* Get a handle for the device configuration */
udp_dev = find_udp_dev_by_id(udp);
if (!udp_dev) {
/* Undefined UDP port for this board (see pios_board.c) */
return -2;
}
/* fill buffer */
PIOS_UDP_RECV(udp);
if (!udp_dev->rx.size) {
/* Nothing new in the buffer */
return -1;
}
/* get byte */
uint8_t b = udp_dev->rx.buf[udp_dev->rx.tail++];
if (udp_dev->rx.tail >= sizeof(udp_dev->rx.buf)) {
udp_dev->rx.tail = 0;
}
udp_dev->rx.size--;
/* Return received byte */
return b;
}
/**
* Returns the next byte of the receive buffer without taking it
* \param[in] UDP UDP name
* \return -1 if UDP not available
* \return -2 if no new byte available
* \return >= 0: number of received bytes
* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions
*/
int32_t PIOS_UDP_RxBufferPeek(uint8_t udp)
{
struct pios_udp_dev * udp_dev;
/* Get a handle for the device configuration */
udp_dev = find_udp_dev_by_id(udp);
if (!udp_dev) {
/* Undefined UDP port for this board (see pios_board.c) */
return -2;
}
/* fill buffer */
PIOS_UDP_RECV(udp);
if (!udp_dev->rx.size) {
/* Nothing new in the buffer */
return -1;
}
/* get byte */
uint8_t b = udp_dev->rx.buf[udp_dev->rx.tail];
/* Return received byte */
return b;
}
/**
* returns number of free bytes in transmit buffer
* \param[in] UDP UDP name
* \return number of free bytes
* \return 0 if UDP not available
* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions
*/
int32_t PIOS_UDP_TxBufferFree(uint8_t udp)
{
struct pios_udp_dev * udp_dev;
/* Get a handle for the device configuration */
udp_dev = find_udp_dev_by_id(udp);
if (!udp_dev) {
/* Undefined UDP port for this board (see pios_board.c) */
return 0;
}
return PIOS_UDP_RX_BUFFER_SIZE;
}
/**
* returns number of used bytes in transmit buffer
* \param[in] UDP UDP name
* \return number of used bytes
* \return 0 if UDP not available
* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions
*/
int32_t PIOS_UDP_TxBufferUsed(uint8_t udp)
{
struct pios_udp_dev * udp_dev;
/* Get a handle for the device configuration */
udp_dev = find_udp_dev_by_id(udp);
if (!udp_dev) {
/* Undefined UDP port for this board (see pios_board.c) */
return 0;
}
return 0;
}
}
/**
* puts more than one byte onto the transmit buffer (used for atomic sends)
* \param[in] UDP UDP name
* \param[in] *buffer pointer to buffer to be sent
* \param[in] len number of bytes to be sent
* \return 0 if no error
* \return -1 if UDP not available
* \return -2 if buffer full or cannot get all requested bytes (retry)
* \return -3 if UDP not supported by UDPTxBufferPut Routine
* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions
* Open UDP socket
*/
int32_t PIOS_UDP_TxBufferPutMoreNonBlocking(uint8_t udp, uint8_t *buffer, uint16_t len)
int32_t PIOS_UDP_Init(uint32_t * udp_id, const struct pios_udp_cfg * cfg)
{
struct pios_udp_dev * udp_dev;
/* Get a handle for the device configuration */
udp_dev = find_udp_dev_by_id(udp);
pios_udp_dev * udp_dev = &pios_udp_devices[pios_udp_num_devices];
if (!udp_dev) {
/* Undefined UDP port for this board (see pios_board.c) */
return -1;
}
if (len >= PIOS_UDP_RX_BUFFER_SIZE) {
/* Buffer cannot accept all requested bytes (retry) */
return -2;
}
/* send data to client - non blocking*/
/* use nonblocking IO */
int flags = fcntl(udp_dev->socket, F_GETFL, 0);
fcntl(udp_dev->socket, F_SETFL, flags | O_NONBLOCK);
sendto(udp_dev->socket, buffer, len, 0,
(struct sockaddr *) &udp_dev->client,
(socklen_t)sizeof(udp_dev->client));
pios_udp_num_devices++;
/* No error */
return 0;
/* initialize */
udp_dev->rx_in_cb = NULL;
udp_dev->tx_out_cb = NULL;
udp_dev->cfg=cfg;
/* assign socket */
udp_dev->socket = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
memset(&udp_dev->server,0,sizeof(udp_dev->server));
memset(&udp_dev->client,0,sizeof(udp_dev->client));
udp_dev->server.sin_family = AF_INET;
udp_dev->server.sin_addr.s_addr = inet_addr(udp_dev->cfg->ip);
udp_dev->server.sin_port = htons(udp_dev->cfg->port);
int res= bind(udp_dev->socket, (struct sockaddr *)&udp_dev->server,sizeof(udp_dev->server));
/* Create transmit thread for this connection */
pthread_create(&udp_dev->rxThread, NULL, PIOS_UDP_RxThread, (void*)udp_dev);
printf("udp dev %i - socket %i opened - result %i\n",pios_udp_num_devices-1,udp_dev->socket,res);
return res;
}
/**
* puts more than one byte onto the transmit buffer (used for atomic sends)<BR>
* (blocking function)
* \param[in] UDP UDP name
* \param[in] *buffer pointer to buffer to be sent
* \param[in] len number of bytes to be sent
* \return 0 if no error
* \return -1 if UDP not available
* \return -3 if UDP not supported by UDPTxBufferPut Routine
* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions
*/
int32_t PIOS_UDP_TxBufferPutMore(uint8_t udp, uint8_t *buffer, uint16_t len)
void PIOS_UDP_ChangeBaud(uint32_t udp_id, uint32_t baud)
{
struct pios_udp_dev * udp_dev;
/* Get a handle for the device configuration */
udp_dev = find_udp_dev_by_id(udp);
if (!udp_dev) {
/* Undefined UDP port for this board (see pios_board.c) */
return -1;
}
if (len >= PIOS_UDP_RX_BUFFER_SIZE) {
/* Buffer cannot accept all requested bytes (retry) */
return -2;
}
/* send data to client - blocking*/
/* use blocking IO */
int flags = fcntl(udp_dev->socket, F_GETFL, 0);
fcntl(udp_dev->socket, F_SETFL, flags & ~O_NONBLOCK);
sendto(udp_dev->socket, buffer, len, 0,
(struct sockaddr *) &udp_dev->client,
sizeof(udp_dev->client));
/* No error */
return 0;
/**
* doesn't apply!
*/
}
/**
* puts a byte onto the transmit buffer
* \param[in] UDP UDP name
* \param[in] b byte which should be put into Tx buffer
* \return 0 if no error
* \return -1 if UDP not available
* \return -2 if buffer full (retry)
* \return -3 if UDP not supported by UDPTxBufferPut Routine
* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions
*/
int32_t PIOS_UDP_TxBufferPut_NonBlocking(uint8_t udp, uint8_t b)
static void PIOS_UDP_RxStart(uint32_t udp_id, uint16_t rx_bytes_avail)
{
return PIOS_UDP_TxBufferPutMoreNonBlocking(udp, &b, 1);
/**
* lazy!
*/
}
/**
* puts a byte onto the transmit buffer<BR>
* (blocking function)
* \param[in] UDP UDP name
* \param[in] b byte which should be put into Tx buffer
* \return 0 if no error
* \return -1 if UDP not available
* \return -3 if UDP not supported by UDPTxBufferPut Routine
* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions
*/
int32_t PIOS_UDP_TxBufferPut(uint8_t udp, uint8_t b)
static void PIOS_UDP_TxStart(uint32_t udp_id, uint16_t tx_bytes_avail)
{
return PIOS_UDP_TxBufferPutMore(udp, &b, 1);
pios_udp_dev * udp_dev = find_udp_dev_by_id(udp_id);
PIOS_Assert(udp_dev);
int32_t length,len,rem;
/**
* we send everything directly whenever notified of data to send (lazy!)
*/
if (udp_dev->tx_out_cb) {
while (tx_bytes_avail>0) {
bool tx_need_yield = false;
length = (udp_dev->tx_out_cb)(udp_dev->tx_out_context, udp_dev->tx_buffer, PIOS_UDP_RX_BUFFER_SIZE, NULL, &tx_need_yield);
rem = length;
while (rem>0) {
len = sendto(udp_dev->socket, udp_dev->tx_buffer, length, 0,
(struct sockaddr *) &udp_dev->client,
sizeof(udp_dev->client));
if (len<=0) {
rem=0;
} else {
rem -= len;
}
}
tx_bytes_avail -= length;
}
}
}
static void PIOS_UDP_RegisterRxCallback(uint32_t udp_id, pios_com_callback rx_in_cb, uint32_t context)
{
pios_udp_dev * udp_dev = find_udp_dev_by_id(udp_id);
PIOS_Assert(udp_dev);
/*
* Order is important in these assignments since ISR uses _cb
* field to determine if it's ok to dereference _cb and _context
*/
udp_dev->rx_in_context = context;
udp_dev->rx_in_cb = rx_in_cb;
}
static void PIOS_UDP_RegisterTxCallback(uint32_t udp_id, pios_com_callback tx_out_cb, uint32_t context)
{
pios_udp_dev * udp_dev = find_udp_dev_by_id(udp_id);
PIOS_Assert(udp_dev);
/*
* Order is important in these assignments since ISR uses _cb
* field to determine if it's ok to dereference _cb and _context
*/
udp_dev->tx_out_context = context;
udp_dev->tx_out_cb = tx_out_cb;
}
#endif

31
flight/PiOS.win32/inc/pios_crc.h Normal file
View File

@ -0,0 +1,31 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_CRC CRC Functions
* @{
*
* @file pios_crc.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief CRC functions 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
*/
uint8_t PIOS_CRC_updateByte(uint8_t crc, const uint8_t data);
uint8_t PIOS_CRC_updateCRC(uint8_t crc, const uint8_t* data, int32_t length);

13
flight/PiOS.win32/inc/pios_initcall.h
View File

@ -38,7 +38,18 @@
* and we cannot define a linker script for each of them atm
*/
#define uavobj_initcall(fn)
#define UAVOBJ_INITCALL(fn)
#define MODULE_INITCALL(ifn, iparam, sfn, sparam, flags)
#define MODULE_TASKCREATE_ALL
#define MODULE_INITIALISE_ALL { \
/* Initialize modules */ \
InitModules(); \
/* Start the FreeRTOS scheduler which never returns.*/ \
/* Initialize the system thread */ \
SystemModInitialize();}
#endif /* PIOS_INITCALL_H */

1
flight/PiOS.win32/pios.h
View File

@ -63,6 +63,7 @@
#include <pios_com.h>
#include <pios_servo.h>
#include <pios_wdg.h>
#include <pios_crc.h>
#define NELEMENTS(x) (sizeof(x) / sizeof(*(x)))

5
flight/PiOS.win32/win32/Libraries/FreeRTOS/Source/portable/GCC/Win32/port.c
View File

@ -941,3 +941,8 @@ void vPortExitCritical( void )
}
}
}
unsigned long ulPortGetTimerValue( void )
{
return (unsigned long) clock();
}

74
flight/PiOS.win32/win32/pios_crc.c Normal file
View File

@ -0,0 +1,74 @@
/*
* pios_crc.c
* OpenPilotOSX
*
* Created by James Cotton on 6/4/11.
* Copyright 2011 OpenPilot. All rights reserved.
*
*/
#include "pios.h"
// CRC lookup table
static const uint8_t crc_table[256] = {
0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15, 0x38, 0x3f, 0x36, 0x31, 0x24, 0x23, 0x2a, 0x2d,
0x70, 0x77, 0x7e, 0x79, 0x6c, 0x6b, 0x62, 0x65, 0x48, 0x4f, 0x46, 0x41, 0x54, 0x53, 0x5a, 0x5d,
0xe0, 0xe7, 0xee, 0xe9, 0xfc, 0xfb, 0xf2, 0xf5, 0xd8, 0xdf, 0xd6, 0xd1, 0xc4, 0xc3, 0xca, 0xcd,
0x90, 0x97, 0x9e, 0x99, 0x8c, 0x8b, 0x82, 0x85, 0xa8, 0xaf, 0xa6, 0xa1, 0xb4, 0xb3, 0xba, 0xbd,
0xc7, 0xc0, 0xc9, 0xce, 0xdb, 0xdc, 0xd5, 0xd2, 0xff, 0xf8, 0xf1, 0xf6, 0xe3, 0xe4, 0xed, 0xea,
0xb7, 0xb0, 0xb9, 0xbe, 0xab, 0xac, 0xa5, 0xa2, 0x8f, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9d, 0x9a,
0x27, 0x20, 0x29, 0x2e, 0x3b, 0x3c, 0x35, 0x32, 0x1f, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0d, 0x0a,
0x57, 0x50, 0x59, 0x5e, 0x4b, 0x4c, 0x45, 0x42, 0x6f, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7d, 0x7a,
0x89, 0x8e, 0x87, 0x80, 0x95, 0x92, 0x9b, 0x9c, 0xb1, 0xb6, 0xbf, 0xb8, 0xad, 0xaa, 0xa3, 0xa4,
0xf9, 0xfe, 0xf7, 0xf0, 0xe5, 0xe2, 0xeb, 0xec, 0xc1, 0xc6, 0xcf, 0xc8, 0xdd, 0xda, 0xd3, 0xd4,
0x69, 0x6e, 0x67, 0x60, 0x75, 0x72, 0x7b, 0x7c, 0x51, 0x56, 0x5f, 0x58, 0x4d, 0x4a, 0x43, 0x44,
0x19, 0x1e, 0x17, 0x10, 0x05, 0x02, 0x0b, 0x0c, 0x21, 0x26, 0x2f, 0x28, 0x3d, 0x3a, 0x33, 0x34,
0x4e, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5c, 0x5b, 0x76, 0x71, 0x78, 0x7f, 0x6a, 0x6d, 0x64, 0x63,
0x3e, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2c, 0x2b, 0x06, 0x01, 0x08, 0x0f, 0x1a, 0x1d, 0x14, 0x13,
0xae, 0xa9, 0xa0, 0xa7, 0xb2, 0xb5, 0xbc, 0xbb, 0x96, 0x91, 0x98, 0x9f, 0x8a, 0x8d, 0x84, 0x83,
0xde, 0xd9, 0xd0, 0xd7, 0xc2, 0xc5, 0xcc, 0xcb, 0xe6, 0xe1, 0xe8, 0xef, 0xfa, 0xfd, 0xf4, 0xf3
};
/**
* Update the crc value with new data.
*
* Generated by pycrc v0.7.5, http://www.tty1.net/pycrc/
* using the configuration:
* Width = 8
* Poly = 0x07
* XorIn = 0x00
* ReflectIn = False
* XorOut = 0x00
* ReflectOut = False
* Algorithm = table-driven
*
* \param crc The current crc value.
* \param data Pointer to a buffer of \a data_len bytes.
* \param length Number of bytes in the \a data buffer.
* \return The updated crc value.
*/
uint8_t PIOS_CRC_updateByte(uint8_t crc, const uint8_t data)
{
return crc_table[crc ^ data];
}
/*
* @brief Update a CRC with a data buffer
* @param[in] crc Starting CRC value
* @param[in] data Data buffer
* @param[in] length Number of bytes to process
* @returns Updated CRC
*/
uint8_t PIOS_CRC_updateCRC(uint8_t crc, const uint8_t* data, int32_t length)
{
// use registers for speed
register int32_t len = length;
register uint8_t crc8 = crc;
register const uint8_t *p = data;
while (len--)
crc8 = crc_table[crc8 ^ *p++];
return crc8;
}

4
flight/PiOS.win32/win32/pios_delay.c
View File

@ -62,11 +62,7 @@ int32_t PIOS_DELAY_Init(void)
int32_t PIOS_DELAY_WaituS(uint16_t uS)
{
Sleep(uS/1000);
//uint16_t start = PIOS_DELAY_TIMER->CNT;
/* Note that this event works on 16bit counter wrap-arounds */
//while((uint16_t)(PIOS_DELAY_TIMER->CNT - start) <= uS);
/* No error */
return 0;
}

10
flight/PiOS/Boards/STM32103CB_AHRS.h
View File

@ -79,12 +79,6 @@ TIM8 | | | |
#define PIOS_LED_PINS { PIOS_LED_LED1_GPIO_PIN }
#define PIOS_LED_CLKS { PIOS_LED_LED1_GPIO_CLK }
//-------------------------
// Delay Timer
//-------------------------
#define PIOS_DELAY_TIMER TIM2
#define PIOS_DELAY_TIMER_RCC_FUNC RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE)
//-------------------------
// System Settings
//-------------------------
@ -118,9 +112,6 @@ extern uint32_t pios_i2c_main_adapter_id;
// PIOS_USART
//-------------------------
#define PIOS_USART_MAX_DEVS 2
#define PIOS_USART_RX_BUFFER_SIZE 256
#define PIOS_USART_TX_BUFFER_SIZE 256
#define PIOS_USART_BAUDRATE 230400
//-------------------------
// PIOS_COM
@ -128,6 +119,7 @@ extern uint32_t pios_i2c_main_adapter_id;
// See also pios_board.c
//-------------------------
#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

49
flight/PiOS/Boards/STM32103CB_CC_Rev1.h
View File

@ -34,7 +34,7 @@
Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4
------+-----------+-----------+-----------+----------
TIM1 | Servo 4 | | |
TIM2 | RC In 5 | RC In 6 | Servo 6 |
TIM2 | RC In 5 | RC In 6 | Servo 6 |
TIM3 | Servo 5 | RC In 2 | RC In 3 | RC In 4
TIM4 | RC In 1 | Servo 3 | Servo 2 | Servo 1
------+-----------+-----------+-----------+----------
@ -76,7 +76,7 @@ TIM4 | RC In 1 | Servo 3 | Servo 2 | Servo 1
#define PIOS_WDG_MANUAL 0x0008
//------------------------
// TELEMETRY
// TELEMETRY
//------------------------
#define TELEM_QUEUE_SIZE 20
@ -91,12 +91,6 @@ TIM4 | RC In 1 | Servo 3 | Servo 2 | Servo 1
#define PIOS_LED_PINS { PIOS_LED_LED1_GPIO_PIN }
#define PIOS_LED_CLKS { PIOS_LED_LED1_GPIO_CLK }
//-------------------------
// Delay Timer
//-------------------------
#define PIOS_DELAY_TIMER TIM3
#define PIOS_DELAY_TIMER_RCC_FUNC RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE)
//-------------------------
// System Settings
//-------------------------
@ -131,23 +125,18 @@ extern uint32_t pios_i2c_main_adapter_id;
//-------------------------
#define PIOS_USART_MAX_DEVS 2
#define PIOS_USART_RX_BUFFER_SIZE 256
#define PIOS_USART_TX_BUFFER_SIZE 256
//-------------------------
// PIOS_COM
//
// See also pios_board.c
//-------------------------
#define PIOS_COM_MAX_DEVS 4
#define PIOS_COM_MAX_DEVS 3
#define PIOS_COM_TELEM_BAUDRATE 57600
extern uint32_t pios_com_telem_rf_id;
#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id)
#define PIOS_COM_DEBUG PIOS_COM_TELEM_RF
#if defined(PIOS_INCLUDE_GPS)
#define PIOS_COM_GPS_BAUDRATE 57600
extern uint32_t pios_com_gps_id;
#define PIOS_COM_GPS (pios_com_gps_id)
#endif /* PIOS_INCLUDE_GPS */
@ -155,12 +144,6 @@ extern uint32_t pios_com_gps_id;
extern uint32_t pios_com_telem_usb_id;
#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id)
#ifdef PIOS_INCLUDE_SPEKTRUM
#define PIOS_COM_SPEKTRUM_BAUDRATE 115200
extern uint32_t pios_com_spektrum_id;
#define PIOS_COM_SPEKTRUM (pios_com_spektrum_id)
#endif
//-------------------------
// ADC
// PIOS_ADC_PinGet(0) = Gyro Z
@ -197,7 +180,7 @@ extern uint32_t pios_com_spektrum_id;
#define PIOS_ADC_CHANNELS { PIOS_ADC_PIN1_GPIO_CHANNEL, PIOS_ADC_PIN2_GPIO_CHANNEL, PIOS_ADC_PIN3_GPIO_CHANNEL }
#define PIOS_ADC_MAPPING { PIOS_ADC_PIN1_ADC, PIOS_ADC_PIN2_ADC, PIOS_ADC_PIN3_ADC }
#define PIOS_ADC_CHANNEL_MAPPING { PIOS_ADC_PIN1_ADC_NUMBER, PIOS_ADC_PIN2_ADC_NUMBER, PIOS_ADC_PIN3_ADC_NUMBER }
#define PIOS_ADC_NUM_CHANNELS (PIOS_ADC_NUM_PINS + PIOS_ADC_USE_TEMP_SENSOR)
#define PIOS_ADC_NUM_CHANNELS (PIOS_ADC_NUM_PINS + PIOS_ADC_USE_TEMP_SENSOR)
#define PIOS_ADC_NUM_ADC_CHANNELS 2
#define PIOS_ADC_USE_ADC2 1
#define PIOS_ADC_CLOCK_FUNCTION RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1 | RCC_APB2Periph_ADC2, ENABLE)
@ -219,10 +202,27 @@ extern uint32_t pios_com_spektrum_id;
#define PIOS_ADC_RATE (72.0e6 / 1.0 / 8.0 / 252.0 / (PIOS_ADC_NUM_CHANNELS >> PIOS_ADC_USE_ADC2))
#define PIOS_ADC_MAX_OVERSAMPLING 36
//------------------------
// PIOS_RCVR
// See also pios_board.c
//------------------------
#define PIOS_RCVR_MAX_DEVS 1
#define PIOS_RCVR_MAX_CHANNELS 12
//-------------------------
// Receiver PWM inputs
// Receiver PPM input
//-------------------------
#define PIOS_PWM_MAX_INPUTS 6
#define PIOS_PPM_NUM_INPUTS 12
//-------------------------
// Receiver PWM input
//-------------------------
#define PIOS_PWM_NUM_INPUTS 6
//-------------------------
// Receiver SPEKTRUM input
//-------------------------
#define PIOS_SPEKTRUM_NUM_INPUTS 12
//-------------------------
// Servo outputs
@ -252,10 +252,9 @@ extern uint32_t pios_com_spektrum_id;
// USB
//-------------------------
#define PIOS_USB_ENABLED 1
#define PIOS_USB_HID_MAX_DEVS 1
#define PIOS_USB_DETECT_GPIO_PORT GPIOC
#define PIOS_USB_DETECT_GPIO_PIN GPIO_Pin_15
#define PIOS_USB_DETECT_EXTI_LINE EXTI_Line15
#define PIOS_IRQ_USB_PRIORITY PIOS_IRQ_PRIO_MID
#define PIOS_USB_RX_BUFFER_SIZE 128
#define PIOS_USB_TX_BUFFER_SIZE 128
#endif /* STM32103CB_AHRS_H_ */

14
flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h
View File

@ -117,14 +117,6 @@ TIM4 | STOPWATCH |
#define TX_LED_OFF PIOS_LED_Off(LED4)
#define TX_LED_TOGGLE PIOS_LED_Toggle(LED4)
// *****************************************************************
// Delay Timer
//#define PIOS_DELAY_TIMER TIM2
//#define PIOS_DELAY_TIMER_RCC_FUNC RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE)
#define PIOS_DELAY_TIMER TIM1
#define PIOS_DELAY_TIMER_RCC_FUNC RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE)
// *****************************************************************
// Timer interrupt
@ -152,9 +144,6 @@ extern uint32_t pios_spi_port_id;
//-------------------------
#define PIOS_USART_MAX_DEVS 1
#define PIOS_USART_RX_BUFFER_SIZE 512
#define PIOS_USART_TX_BUFFER_SIZE 512
//-------------------------
// PIOS_COM
//
@ -404,12 +393,11 @@ extern uint32_t pios_com_telem_usb_id;
#if defined(PIOS_INCLUDE_USB_HID)
#define PIOS_USB_ENABLED 1
#define PIOS_USB_HID_MAX_DEVS 1
#define PIOS_USB_DETECT_GPIO_PORT GPIO_IN_2_PORT
#define PIOS_USB_DETECT_GPIO_PIN GPIO_IN_2_PIN
#define PIOS_USB_DETECT_EXTI_LINE EXTI_Line4
#define PIOS_IRQ_USB_PRIORITY 8
#define PIOS_USB_RX_BUFFER_SIZE 512
#define PIOS_USB_TX_BUFFER_SIZE 512
#endif
// *****************************************************************

11
flight/PiOS/Boards/STM3210E_INS.h
View File

@ -125,9 +125,6 @@ extern uint32_t pios_i2c_gyro_adapter_id;
//-------------------------
#define PIOS_USART_MAX_DEVS 2
#define PIOS_USART_RX_BUFFER_SIZE 256
#define PIOS_USART_TX_BUFFER_SIZE 256
//-------------------------
// PIOS_COM
//
@ -135,23 +132,15 @@ extern uint32_t pios_i2c_gyro_adapter_id;
//-------------------------
#define PIOS_COM_MAX_DEVS 2
#define PIOS_COM_GPS_BAUDRATE 57600
extern uint32_t pios_com_gps_id;
#define PIOS_COM_GPS (pios_com_gps_id)
#ifdef PIOS_ENABLE_AUX_UART
#define PIOS_COM_AUX_BAUDRATE 57600
extern uint32_t pios_com_aux_id;
#define PIOS_COM_AUX (pios_com_aux_id)
#define PIOS_COM_DEBUG PIOS_COM_AUX
#endif
//-------------------------
// Delay Timer
//-------------------------
#define PIOS_DELAY_TIMER TIM2
#define PIOS_DELAY_TIMER_RCC_FUNC RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE)
//-------------------------
// System Settings
//-------------------------

55
flight/PiOS/Boards/STM3210E_OP.h
View File

@ -139,9 +139,6 @@ extern uint32_t pios_i2c_main_adapter_id;
//-------------------------
#define PIOS_USART_MAX_DEVS 3
#define PIOS_USART_RX_BUFFER_SIZE 512
#define PIOS_USART_TX_BUFFER_SIZE 512
//-------------------------
// PIOS_COM
//
@ -149,11 +146,9 @@ extern uint32_t pios_i2c_main_adapter_id;
//-------------------------
#define PIOS_COM_MAX_DEVS 4
#define PIOS_COM_TELEM_BAUDRATE 57600
extern uint32_t pios_com_telem_rf_id;
#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id)
#define PIOS_COM_GPS_BAUDRATE 57600
extern uint32_t pios_com_gps_id;
#define PIOS_COM_GPS (pios_com_gps_id)
@ -161,24 +156,11 @@ extern uint32_t pios_com_telem_usb_id;
#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id)
#ifdef PIOS_ENABLE_AUX_UART
#define PIOS_COM_AUX_BAUDRATE 57600
extern uint32_t pios_com_aux_id;
#define PIOS_COM_AUX (pios_com_aux_id)
#define PIOS_COM_DEBUG PIOS_COM_AUX
#endif
#ifdef PIOS_INCLUDE_SPEKTRUM
#define PIOS_COM_SPEKTRUM_BAUDRATE 115200
extern uint32_t pios_com_spektrum_id;
#define PIOS_COM_SPEKTRUM (pios_com_spektrum_id)
#endif
//-------------------------
// Delay Timer
//-------------------------
#define PIOS_DELAY_TIMER TIM2
#define PIOS_DELAY_TIMER_RCC_FUNC RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE)
//-------------------------
// System Settings
//-------------------------
@ -193,37 +175,33 @@ extern uint32_t pios_com_spektrum_id;
#define PIOS_IRQ_PRIO_HIGH 5 // for SPI, ADC, I2C etc...
#define PIOS_IRQ_PRIO_HIGHEST 4 // for USART etc...
//-------------------------
// Receiver PWM inputs
//-------------------------
/*#define PIOS_PWM_SUPV_ENABLED 1
#define PIOS_PWM_SUPV_TIMER TIM6
#define PIOS_PWM_SUPV_TIMER_RCC_FUNC RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM6, ENABLE)
#define PIOS_PWM_SUPV_HZ 25
#define PIOS_PWM_SUPV_IRQ_CHANNEL TIM6_IRQn
#define PIOS_PWM_SUPV_IRQ_FUNC void TIM6_IRQHandler(void)*/
//------------------------
// PIOS_RCVR
// See also pios_board.c
//------------------------
#define PIOS_RCVR_MAX_DEVS 1
#define PIOS_RCVR_MAX_CHANNELS 12
//-------------------------
// Receiver PPM input
//-------------------------
#define PIOS_PPM_NUM_INPUTS 8 //Could be more if needed
#define PIOS_PPM_SUPV_ENABLED 1
#define PIOS_PPM_NUM_INPUTS 12
//-------------------------
// SPEKTRUM input
// Receiver PWM input
//-------------------------
//#define PIOS_SPEKTRUM_SUPV_ENABLED 1
//#define PIOS_SPEKTRUM_SUPV_TIMER TIM6
//#define PIOS_SPEKTRUM_SUPV_TIMER_RCC_FUNC RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM6, ENABLE)
//#define PIOS_SPEKTRUM_SUPV_HZ 60 // 1/22ms
//#define PIOS_SPEKTRUM_SUPV_IRQ_CHANNEL TIM6_IRQn
//#define PIOS_SPEKTRUM_SUPV_IRQ_FUNC void TIM6_IRQHandler(void)
#define PIOS_PWM_NUM_INPUTS 8
//-------------------------
// Receiver SPEKTRUM input
//-------------------------
#define PIOS_SPEKTRUM_NUM_INPUTS 12
//-------------------------
// Servo outputs
//-------------------------
#define PIOS_SERVO_UPDATE_HZ 50
#define PIOS_SERVOS_INITIAL_POSITION 0 /* dont want to start motors, have no pulse till settings loaded */
#define PIOS_PWM_MAX_INPUTS 8
//-------------------------
// ADC
@ -317,12 +295,11 @@ extern uint32_t pios_com_spektrum_id;
// USB
//-------------------------
#define PIOS_USB_ENABLED 1
#define PIOS_USB_HID_MAX_DEVS 1
#define PIOS_USB_DETECT_GPIO_PORT GPIOC
#define PIOS_USB_DETECT_GPIO_PIN GPIO_Pin_4
#define PIOS_USB_DETECT_EXTI_LINE EXTI_Line4
#define PIOS_IRQ_USB_PRIORITY PIOS_IRQ_PRIO_MID
#define PIOS_USB_RX_BUFFER_SIZE 512
#define PIOS_USB_TX_BUFFER_SIZE 512
/**
* glue macros for file IO

16
flight/PiOS/Common/pios_adxl345.c
View File

@ -96,6 +96,22 @@ void PIOS_ADXL345_Init()
PIOS_ADXL345_SetMeasure(1);
}
/**
* @brief Return number of entries in the fifo
*/
uint8_t PIOS_ADXL345_FifoElements()
{
uint8_t buf[2] = {0,0};
uint8_t rec[2] = {0,0};
buf[0] = ADXL_FIFOSTATUS_ADDR | ADXL_READ_BIT ; // Read fifo status
PIOS_ADXL345_ClaimBus();
PIOS_SPI_TransferBlock(PIOS_SPI_ACCEL,&buf[0],&rec[0],sizeof(buf),NULL);
PIOS_ADXL345_ReleaseBus();
return rec[1] & 0x3f;
}
/**
* @brief Read a single set of values from the x y z channels
* @returns The number of samples remaining in the fifo

252
flight/PiOS/Common/pios_com.c
View File

@ -34,11 +34,37 @@
#if defined(PIOS_INCLUDE_COM)
#include "fifo_buffer.h"
#include <pios_com_priv.h>
#if !defined(PIOS_INCLUDE_FREERTOS)
#include "pios_delay.h" /* PIOS_DELAY_WaitmS */
#endif
enum pios_com_dev_magic {
PIOS_COM_DEV_MAGIC = 0xaa55aa55,
};
struct pios_com_dev {
enum pios_com_dev_magic magic;
uint32_t lower_id;
const struct pios_com_driver * driver;
#if defined(PIOS_INCLUDE_FREERTOS)
xSemaphoreHandle tx_sem;
xSemaphoreHandle rx_sem;
#endif
bool has_rx;
bool has_tx;
t_fifo_buffer rx;
t_fifo_buffer tx;
};
static bool PIOS_COM_validate(struct pios_com_dev * com_dev)
{
return (com_dev->magic == PIOS_COM_DEV_MAGIC);
return (com_dev && (com_dev->magic == PIOS_COM_DEV_MAGIC));
}
#if defined(PIOS_INCLUDE_FREERTOS) && 0
@ -70,6 +96,11 @@ static struct pios_com_dev * PIOS_COM_alloc(void)
}
#endif
static uint16_t PIOS_COM_TxOutCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield);
static uint16_t PIOS_COM_RxInCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield);
static void PIOS_COM_UnblockRx(struct pios_com_dev * com_dev, bool * need_yield);
static void PIOS_COM_UnblockTx(struct pios_com_dev * com_dev, bool * need_yield);
/**
* Initialises COM layer
* \param[out] handle
@ -77,18 +108,48 @@ static struct pios_com_dev * PIOS_COM_alloc(void)
* \param[in] id
* \return < 0 if initialisation failed
*/
int32_t PIOS_COM_Init(uint32_t * com_id, const struct pios_com_driver * driver, const uint32_t lower_id)
int32_t PIOS_COM_Init(uint32_t * com_id, const struct pios_com_driver * driver, uint32_t lower_id, uint8_t * rx_buffer, uint16_t rx_buffer_len, uint8_t * tx_buffer, uint16_t tx_buffer_len)
{
PIOS_DEBUG_Assert(com_id);
PIOS_DEBUG_Assert(driver);
PIOS_Assert(com_id);
PIOS_Assert(driver);
bool has_rx = (rx_buffer && rx_buffer_len > 0);
bool has_tx = (tx_buffer && tx_buffer_len > 0);
PIOS_Assert(has_rx || has_tx);
PIOS_Assert(driver->bind_tx_cb || !has_tx);
PIOS_Assert(driver->bind_rx_cb || !has_rx);
struct pios_com_dev * com_dev;
com_dev = (struct pios_com_dev *) PIOS_COM_alloc();
if (!com_dev) goto out_fail;
com_dev->driver = driver;
com_dev->id = lower_id;
com_dev->driver = driver;
com_dev->lower_id = lower_id;
com_dev->has_rx = has_rx;
com_dev->has_tx = has_tx;
if (has_rx) {
fifoBuf_init(&com_dev->rx, rx_buffer, rx_buffer_len);
#if defined(PIOS_INCLUDE_FREERTOS)
vSemaphoreCreateBinary(com_dev->rx_sem);
#endif /* PIOS_INCLUDE_FREERTOS */
(com_dev->driver->bind_rx_cb)(lower_id, PIOS_COM_RxInCallback, (uint32_t)com_dev);
if (com_dev->driver->rx_start) {
/* Start the receiver */
(com_dev->driver->rx_start)(com_dev->lower_id,
fifoBuf_getFree(&com_dev->rx));
}
}
if (has_tx) {
fifoBuf_init(&com_dev->tx, tx_buffer, tx_buffer_len);
#if defined(PIOS_INCLUDE_FREERTOS)
vSemaphoreCreateBinary(com_dev->tx_sem);
#endif /* PIOS_INCLUDE_FREERTOS */
(com_dev->driver->bind_tx_cb)(lower_id, PIOS_COM_TxOutCallback, (uint32_t)com_dev);
}
*com_id = (uint32_t)com_dev;
return(0);
@ -97,6 +158,88 @@ out_fail:
return(-1);
}
static void PIOS_COM_UnblockRx(struct pios_com_dev * com_dev, bool * need_yield)
{
#if defined(PIOS_INCLUDE_FREERTOS)
static signed portBASE_TYPE xHigherPriorityTaskWoken;
xSemaphoreGiveFromISR(com_dev->rx_sem, &xHigherPriorityTaskWoken);
if (xHigherPriorityTaskWoken != pdFALSE) {
*need_yield = true;
} else {
*need_yield = false;
}
#else
*need_yield = false;
#endif
}
static void PIOS_COM_UnblockTx(struct pios_com_dev * com_dev, bool * need_yield)
{
#if defined(PIOS_INCLUDE_FREERTOS)
static signed portBASE_TYPE xHigherPriorityTaskWoken;
xSemaphoreGiveFromISR(com_dev->tx_sem, &xHigherPriorityTaskWoken);
if (xHigherPriorityTaskWoken != pdFALSE) {
*need_yield = true;
} else {
*need_yield = false;
}
#else
*need_yield = false;
#endif
}
static uint16_t PIOS_COM_RxInCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield)
{
struct pios_com_dev * com_dev = (struct pios_com_dev *)context;
bool valid = PIOS_COM_validate(com_dev);
PIOS_Assert(valid);
PIOS_Assert(com_dev->has_rx);
PIOS_IRQ_Disable();
uint16_t bytes_into_fifo = fifoBuf_putData(&com_dev->rx, buf, buf_len);
PIOS_IRQ_Enable();
if (bytes_into_fifo > 0) {
/* Data has been added to the buffer */
PIOS_COM_UnblockRx(com_dev, need_yield);
}
if (headroom) {
*headroom = fifoBuf_getFree(&com_dev->rx);
}
return (bytes_into_fifo);
}
static uint16_t PIOS_COM_TxOutCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield)
{
struct pios_com_dev * com_dev = (struct pios_com_dev *)context;
bool valid = PIOS_COM_validate(com_dev);
PIOS_Assert(valid);
PIOS_Assert(buf);
PIOS_Assert(buf_len);
PIOS_Assert(com_dev->has_tx);
PIOS_IRQ_Disable();
uint16_t bytes_from_fifo = fifoBuf_getData(&com_dev->tx, buf, buf_len);
PIOS_IRQ_Enable();
if (bytes_from_fifo > 0) {
/* More space has been made in the buffer */
PIOS_COM_UnblockTx(com_dev, need_yield);
}
if (headroom) {
*headroom = fifoBuf_getUsed(&com_dev->tx);
}
return (bytes_from_fifo);
}
/**
* Change the port speed without re-initializing
* \param[in] port COM port
@ -115,7 +258,7 @@ int32_t PIOS_COM_ChangeBaud(uint32_t com_id, uint32_t baud)
/* Invoke the driver function if it exists */
if (com_dev->driver->set_baud) {
com_dev->driver->set_baud(com_dev->id, baud);
com_dev->driver->set_baud(com_dev->lower_id, baud);
}
return 0;
@ -140,12 +283,26 @@ int32_t PIOS_COM_SendBufferNonBlocking(uint32_t com_id, const uint8_t *buffer, u
return -1;
}
/* Invoke the driver function if it exists */
if (com_dev->driver->tx_nb) {
return com_dev->driver->tx_nb(com_dev->id, buffer, len);
PIOS_Assert(com_dev->has_tx);
if (len >= fifoBuf_getFree(&com_dev->tx)) {
/* Buffer cannot accept all requested bytes (retry) */
return -2;
}
return 0;
PIOS_IRQ_Disable();
uint16_t bytes_into_fifo = fifoBuf_putData(&com_dev->tx, buffer, len);
PIOS_IRQ_Enable();
if (bytes_into_fifo > 0) {
/* More data has been put in the tx buffer, make sure the tx is started */
if (com_dev->driver->tx_start) {
com_dev->driver->tx_start(com_dev->lower_id,
fifoBuf_getUsed(&com_dev->tx));
}
}
return (0);
}
/**
@ -166,12 +323,27 @@ int32_t PIOS_COM_SendBuffer(uint32_t com_id, const uint8_t *buffer, uint16_t len
return -1;
}
/* Invoke the driver function if it exists */
if (com_dev->driver->tx) {
return com_dev->driver->tx(com_dev->id, buffer, len);
}
PIOS_Assert(com_dev->has_tx);
return 0;
int32_t rc;
do {
rc = PIOS_COM_SendBufferNonBlocking(com_id, buffer, len);
#if defined(PIOS_INCLUDE_FREERTOS)
if (rc == -2) {
/* Make sure the transmitter is running while we wait */
if (com_dev->driver->tx_start) {
(com_dev->driver->tx_start)(com_dev->lower_id,
fifoBuf_getUsed(&com_dev->tx));
}
if (xSemaphoreTake(com_dev->tx_sem, portMAX_DELAY) != pdTRUE) {
return -3;
}
}
#endif
} while (rc == -2);
return rc;
}
/**
@ -273,18 +445,47 @@ int32_t PIOS_COM_SendFormattedString(uint32_t com_id, const char *format, ...)
* \param[in] port COM port
* \returns Byte from buffer
*/
uint8_t PIOS_COM_ReceiveBuffer(uint32_t com_id)
uint16_t PIOS_COM_ReceiveBuffer(uint32_t com_id, uint8_t * buf, uint16_t buf_len, uint32_t timeout_ms)
{
PIOS_Assert(buf);
PIOS_Assert(buf_len);
struct pios_com_dev * com_dev = (struct pios_com_dev *)com_id;
if (!PIOS_COM_validate(com_dev)) {
/* Undefined COM port for this board (see pios_board.c) */
PIOS_DEBUG_Assert(0);
PIOS_Assert(0);
}
PIOS_Assert(com_dev->has_rx);
check_again:
PIOS_IRQ_Disable();
uint16_t bytes_from_fifo = fifoBuf_getData(&com_dev->rx, buf, buf_len);
PIOS_IRQ_Enable();
if (bytes_from_fifo == 0 && timeout_ms > 0) {
/* No more bytes in receive buffer */
/* Make sure the receiver is running while we wait */
if (com_dev->driver->rx_start) {
/* Notify the lower layer that there is now room in the rx buffer */
(com_dev->driver->rx_start)(com_dev->lower_id,
fifoBuf_getFree(&com_dev->rx));
}
#if defined(PIOS_INCLUDE_FREERTOS)
if (xSemaphoreTake(com_dev->rx_sem, timeout_ms / portTICK_RATE_MS) == pdTRUE) {
/* Make sure we don't come back here again */
timeout_ms = 0;
goto check_again;
}
#else
PIOS_DELAY_WaitmS(1);
timeout_ms--;
goto check_again;
#endif
}
PIOS_DEBUG_Assert(com_dev->driver->rx);
return com_dev->driver->rx(com_dev->id);
/* Return received byte */
return (bytes_from_fifo);
}
/**
@ -298,14 +499,11 @@ int32_t PIOS_COM_ReceiveBufferUsed(uint32_t com_id)
if (!PIOS_COM_validate(com_dev)) {
/* Undefined COM port for this board (see pios_board.c) */
PIOS_DEBUG_Assert(0);
PIOS_Assert(0);
}
if (!com_dev->driver->rx_avail) {
return 0;
}
return com_dev->driver->rx_avail(com_dev->id);
PIOS_Assert(com_dev->has_rx);
return (fifoBuf_getUsed(&com_dev->rx));
}
#endif

85
flight/PiOS/Common/pios_flash_w25x.c
View File

@ -41,11 +41,11 @@ static uint8_t PIOS_Flash_W25X_Busy() ;
static uint32_t PIOS_SPI_FLASH;
/**
/**
* @brief Claim the SPI bus for flash use and assert CS pin
* @return 0 for sucess, -1 for failure to get semaphore
*/
static int8_t PIOS_Flash_W25X_ClaimBus()
static int8_t PIOS_Flash_W25X_ClaimBus()
{
int8_t ret = PIOS_SPI_ClaimBus(PIOS_SPI_FLASH);
PIOS_FLASH_ENABLE;
@ -55,7 +55,7 @@ static int8_t PIOS_Flash_W25X_ClaimBus()
/**
* @brief Release the SPI bus sempahore and ensure flash chip not using bus
*/
static void PIOS_Flash_W25X_ReleaseBus()
static void PIOS_Flash_W25X_ReleaseBus()
{
PIOS_FLASH_DISABLE;
PIOS_SPI_ReleaseBus(PIOS_SPI_FLASH);
@ -64,8 +64,8 @@ static void PIOS_Flash_W25X_ReleaseBus()
/**
* @brief Returns if the flash chip is busy
*/
static uint8_t PIOS_Flash_W25X_Busy()
{
static uint8_t PIOS_Flash_W25X_Busy()
{
return PIOS_Flash_W25X_ReadStatus() & W25X_STATUS_BUSY;
}
@ -73,12 +73,12 @@ static uint8_t PIOS_Flash_W25X_Busy()
* @brief Execute the write enable instruction and returns the status
* @returns 0 if successful, -1 if unable to claim bus
*/
static uint8_t PIOS_Flash_W25X_WriteEnable()
static uint8_t PIOS_Flash_W25X_WriteEnable()
{
uint8_t out[] = {W25X_WRITE_ENABLE};
if(PIOS_Flash_W25X_ClaimBus() != 0)
return -1;
PIOS_SPI_TransferBlock(PIOS_SPI_FLASH,out,NULL,sizeof(out),NULL);
PIOS_SPI_TransferBlock(PIOS_SPI_FLASH,out,NULL,sizeof(out),NULL);
PIOS_Flash_W25X_ReleaseBus();
return 0;
}
@ -97,27 +97,27 @@ int8_t PIOS_Flash_W25X_Init(uint32_t spi_id)
/**
* @brief Read the status register from flash chip and return it
* @brief Read the status register from flash chip and return it
*/
uint8_t PIOS_Flash_W25X_ReadStatus()
uint8_t PIOS_Flash_W25X_ReadStatus()
{
uint8_t out[2] = {W25X_READ_STATUS, 0};
uint8_t in[2] = {0,0};
PIOS_Flash_W25X_ClaimBus();
PIOS_SPI_TransferBlock(PIOS_SPI_FLASH,out,in,sizeof(out),NULL);
PIOS_SPI_TransferBlock(PIOS_SPI_FLASH,out,in,sizeof(out),NULL);
PIOS_Flash_W25X_ReleaseBus();
return in[1];
}
/**
* @brief Read the status register from flash chip and return it
* @brief Read the status register from flash chip and return it
*/
uint8_t PIOS_Flash_W25X_ReadID()
uint8_t PIOS_Flash_W25X_ReadID()
{
uint8_t out[] = {W25X_DEVICE_ID, 0, 0, 0, 0, 0};
uint8_t in[6];
PIOS_Flash_W25X_ClaimBus();
PIOS_SPI_TransferBlock(PIOS_SPI_FLASH,out,in,sizeof(out),NULL);
PIOS_SPI_TransferBlock(PIOS_SPI_FLASH,out,in,sizeof(out),NULL);
PIOS_Flash_W25X_ReleaseBus();
return in[5];
}
@ -127,28 +127,27 @@ uint8_t PIOS_Flash_W25X_ReadID()
* @param[in] add Address of flash to erase
* @returns 0 if successful
* @retval -1 if unable to claim bus
* @retval
* @retval
*/
int8_t PIOS_Flash_W25X_EraseSector(uint32_t addr)
{
uint8_t ret;
uint8_t out[] = {W25X_SECTOR_ERASE, (addr >> 16) & 0xff, (addr >> 8) & 0xff , addr & 0xff};
if((ret = PIOS_Flash_W25X_WriteEnable()) != 0)
return ret;
if(PIOS_Flash_W25X_ClaimBus() != 0)
return -1;
return -1;
PIOS_SPI_TransferBlock(PIOS_SPI_FLASH,out,NULL,sizeof(out),NULL);
PIOS_Flash_W25X_ReleaseBus();
uint32_t i = 1;
while(PIOS_Flash_W25X_Busy()) {
//TODO: Fail on timeout
#if defined(PIOS_INCLUDE_FREERTOS)
vTaskDelay(1);
#endif
if(++i == 0)
return -1;
}
return 0;
}
@ -160,25 +159,26 @@ int8_t PIOS_Flash_W25X_EraseChip()
{
uint8_t ret;
uint8_t out[] = {W25X_CHIP_ERASE};
if((ret = PIOS_Flash_W25X_WriteEnable()) != 0)
return ret;
if(PIOS_Flash_W25X_ClaimBus() != 0)
return -1;
PIOS_SPI_TransferBlock(PIOS_SPI_FLASH,out,NULL,sizeof(out),NULL);
PIOS_SPI_TransferBlock(PIOS_SPI_FLASH,out,NULL,sizeof(out),NULL);
PIOS_Flash_W25X_ReleaseBus();
uint32_t i = 1;
while(PIOS_Flash_W25X_Busy()) {
//TODO: Fail on timeout
#if defined(PIOS_INCLUDE_FREERTOS)
vTaskDelay(1);
#endif
if(++i == 0)
return -1;
}
return 0;
}
/**
/**
* @brief Write one page of data (up to 256 bytes) aligned to a page start
* @param[in] addr Address in flash to write to
* @param[in] data Pointer to data to write to flash
@ -192,37 +192,38 @@ int8_t PIOS_Flash_W25X_WriteData(uint32_t addr, uint8_t * data, uint16_t len)
{
uint8_t ret;
uint8_t out[4] = {W25X_PAGE_WRITE, (addr >> 16) & 0xff, (addr >> 8) & 0xff , addr & 0xff};
/* Can only write one page at a time */
if(len > 0x100)
if(len > 0x100)
return -2;
/* Ensure number of bytes fits after starting address before end of page */
if(((addr & 0xff) + len) > 0x100)
if(((addr & 0xff) + len) > 0x100)
return -3;
if((ret = PIOS_Flash_W25X_WriteEnable()) != 0)
return ret;
/* Execute write page command and clock in address. Keep CS asserted */
if(PIOS_Flash_W25X_ClaimBus() != 0)
return -1;
PIOS_SPI_TransferBlock(PIOS_SPI_FLASH,out,NULL,sizeof(out),NULL);
/* Clock out data to flash */
PIOS_SPI_TransferBlock(PIOS_SPI_FLASH,data,NULL,len,NULL);
PIOS_Flash_W25X_ReleaseBus();
uint32_t i = 1;
while(PIOS_Flash_W25X_Busy()) {
#if defined(PIOS_INCLUDE_FREERTOS)
vTaskDelay(1);
#endif
if(++i == 0)
return -1;
}
return 0;
}
/**
/**
* @brief Read data from a location in flash memory
* @param[in] addr Address in flash to write to
* @param[in] data Pointer to data to write from flash
@ -243,6 +244,6 @@ int8_t PIOS_Flash_W25X_ReadData(uint32_t addr, uint8_t * data, uint16_t len)
PIOS_SPI_TransferBlock(PIOS_SPI_FLASH,NULL,data,len,NULL);
PIOS_Flash_W25X_ReleaseBus();
return 0;
}

141
flight/PiOS/Common/pios_flashfs_objlist.c
View File

@ -12,19 +12,19 @@
* @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
/*
* 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
*
* 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.,
*
* 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
*/
@ -33,7 +33,7 @@
#include "uavobjectmanager.h"
// Private functions
static int32_t PIOS_FLASHFS_CleabObjectTableHeader();
static int32_t PIOS_FLASHFS_ClearObjectTableHeader();
static int32_t PIOS_FLASHFS_GetObjAddress(uint32_t objId, uint16_t instId);
static int32_t PIOS_FLASHFS_GetNewAddress(uint32_t objId, uint16_t instId);
@ -56,11 +56,12 @@ struct fileHeader {
} __attribute__((packed));
#define OBJECT_TABLE_MAGIC 0x85FB3C34
#define OBJECT_TABLE_MAGIC 0x85FB3C35
#define OBJ_MAGIC 0x3015AE71
#define OBJECT_TABLE_START 0x00000010
#define OBJECT_TABLE_END 0x00001000
#define SECTOR_SIZE 0x00001000
#define MAX_BADMAGIC 4
/**
* @brief Initialize the flash object setting FS
@ -68,34 +69,50 @@ struct fileHeader {
*/
int32_t PIOS_FLASHFS_Init()
{
// Check for valid object table or create one
uint32_t object_table_magic;
if (PIOS_Flash_W25X_ReadData(0, (uint8_t *)&object_table_magic, sizeof(object_table_magic)) != 0)
return -1;
if(object_table_magic != OBJECT_TABLE_MAGIC) {
if(PIOS_FLASHFS_CleabObjectTableHeader() < 0)
uint8_t magic_fail_count = 0;
bool magic_good = false;
while(!magic_good) {
if (PIOS_Flash_W25X_ReadData(0, (uint8_t *)&object_table_magic, sizeof(object_table_magic)) != 0)
return -1;
if(object_table_magic != OBJECT_TABLE_MAGIC) {
if(magic_fail_count++ > MAX_BADMAGIC) {
PIOS_FLASHFS_ClearObjectTableHeader();
PIOS_LED_Toggle(LED1);
magic_fail_count = 0;
magic_good = true;
} else {
PIOS_DELAY_WaituS(100);
}
}
else {
magic_good = true;
}
}
int32_t addr = OBJECT_TABLE_START;
struct objectHeader header;
numObjects = 0;
// Loop through header area while objects detect to count how many saved
while(addr < OBJECT_TABLE_END) {
// Read the instance data
if (PIOS_Flash_W25X_ReadData(addr, (uint8_t *)&header, sizeof(header)) != 0)
return -1;
// Counting number of valid headers
if(header.objMagic != OBJ_MAGIC)
// Counting number of valid headers
if(header.objMagic != OBJ_MAGIC)
break;
numObjects++;
addr += sizeof(header);
}
return 0;
}
@ -103,7 +120,7 @@ int32_t PIOS_FLASHFS_Init()
* @brief Erase the headers for all objects in the flash chip
* @return 0 if successful, -1 if not
*/
static int32_t PIOS_FLASHFS_CleabObjectTableHeader()
static int32_t PIOS_FLASHFS_ClearObjectTableHeader()
{
if(PIOS_Flash_W25X_EraseSector(0) != 0)
return -1;
@ -121,26 +138,26 @@ static int32_t PIOS_FLASHFS_CleabObjectTableHeader()
* @parma instId Instance id for that object
* @return address if successful, -1 if not found
*/
static int32_t PIOS_FLASHFS_GetObjAddress(uint32_t objId, uint16_t instId)
static int32_t PIOS_FLASHFS_GetObjAddress(uint32_t objId, uint16_t instId)
{
int32_t addr = OBJECT_TABLE_START;
struct objectHeader header;
// Loop through header area while objects detect to count how many saved
while(addr < OBJECT_TABLE_END) {
// Read the instance data
if (PIOS_Flash_W25X_ReadData(addr, (uint8_t *) &header, sizeof(header)) != 0)
return -1;
if(header.objMagic != OBJ_MAGIC)
if(header.objMagic != OBJ_MAGIC)
break; // stop searching once hit first non-object header
else if (header.objId == objId && header.instId == instId)
break;
addr += sizeof(header);
}
if (header.objId == objId && header.instId == instId)
return header.address;
return -1;
}
@ -150,7 +167,7 @@ static int32_t PIOS_FLASHFS_GetObjAddress(uint32_t objId, uint16_t instId)
* @param[in] instId The instance id of object to be saved
* @return 0 if success or error code
* @retval -1 Object not found
* @retval -2 No room in object table
* @retval -2 No room in object table
* @retval -3 Unable to write entry into object table
* @retval -4 FS not initialized
*/
@ -160,26 +177,26 @@ int32_t PIOS_FLASHFS_GetNewAddress(uint32_t objId, uint16_t instId)
if(numObjects < 0)
return -4;
// Don't worry about max size of flash chip here, other code will catch that
header.objMagic = OBJ_MAGIC;
header.objId = objId;
header.instId = instId;
header.address = OBJECT_TABLE_END + SECTOR_SIZE * numObjects;
int32_t addr = OBJECT_TABLE_START + sizeof(header) * numObjects;
// No room for this header in object table
if((addr + sizeof(header)) > OBJECT_TABLE_END)
return -2;
if((addr + sizeof(header)) > OBJECT_TABLE_END)
return -2;
if(PIOS_Flash_W25X_WriteData(addr, (uint8_t *) &header, sizeof(header)) != 0)
return -3;
// This numObejcts value must stay consistent or there will be a break in the table
// and later the table will have bad values in it
numObjects++;
return header.address;
numObjects++;
return header.address;
}
@ -191,38 +208,38 @@ int32_t PIOS_FLASHFS_GetNewAddress(uint32_t objId, uint16_t instId)
* @note This uses one sector on the flash chip per object so that no buffering in ram
* must be done when erasing the sector before a save
*/
int32_t PIOS_FLASHFS_ObjSave(UAVObjHandle obj, uint16_t instId, uint8_t * data)
int32_t PIOS_FLASHFS_ObjSave(UAVObjHandle obj, uint16_t instId, uint8_t * data)
{
uint32_t objId = UAVObjGetID(obj);
uint8_t crc = 0;
int32_t addr = PIOS_FLASHFS_GetObjAddress(objId, instId);
// Object currently not saved
if(addr < 0)
if(addr < 0)
addr = PIOS_FLASHFS_GetNewAddress(objId, instId);
// Could not allocate a sector
if(addr < 0)
return -1;
struct fileHeader header = {
.id = objId,
.instId = instId,
.size = UAVObjGetNumBytes(obj)
};
if(PIOS_Flash_W25X_EraseSector(addr) != 0)
return -2;
// Save header
// This information IS redundant with the object table id. Oh well. Better safe than sorry.
// This information IS redundant with the object table id. Oh well. Better safe than sorry.
if(PIOS_Flash_W25X_WriteData(addr, (uint8_t *) &header, sizeof(header)) != 0)
return -3;
// Update CRC
crc = PIOS_CRC_updateCRC(0, (uint8_t *) &header, sizeof(header));
// Save data
if(PIOS_Flash_W25X_WriteData(addr + sizeof(header), data, UAVObjGetNumBytes(obj)) != 0)
return -4;
@ -233,7 +250,7 @@ int32_t PIOS_FLASHFS_ObjSave(UAVObjHandle obj, uint16_t instId, uint8_t * data)
// Save CRC (written so will work when CRC changes to uint16)
if(PIOS_Flash_W25X_WriteData(addr + sizeof(header) + UAVObjGetNumBytes(obj), (uint8_t *) &crc, sizeof(crc)) != 0)
return -4;
return 0;
}
@ -259,20 +276,20 @@ int32_t PIOS_FLASHFS_ObjLoad(UAVObjHandle obj, uint16_t instId, uint8_t * data)
uint8_t crcFlash = 0;
const uint8_t crc_read_step = 8;
uint8_t crc_read_buffer[crc_read_step];
int32_t addr = PIOS_FLASHFS_GetObjAddress(objId, instId);
// Object currently not saved
if(addr < 0)
if(addr < 0)
return -1;
struct fileHeader header;
// Load header
// This information IS redundant with the object table id. Oh well. Better safe than sorry.
if(PIOS_Flash_W25X_ReadData(addr, (uint8_t *) &header, sizeof(header)) != 0)
return -2;
// Update CRC
crc = PIOS_CRC_updateCRC(0, (uint8_t *) &header, sizeof(header));
@ -290,7 +307,7 @@ int32_t PIOS_FLASHFS_ObjLoad(UAVObjHandle obj, uint16_t instId, uint8_t * data)
// Read CRC (written so will work when CRC changes to uint16)
if(PIOS_Flash_W25X_ReadData(addr + sizeof(header) + objSize, (uint8_t *) &crcFlash, sizeof(crcFlash)) != 0)
return -5;
if(crc != crcFlash)
return -6;
@ -308,22 +325,22 @@ int32_t PIOS_FLASHFS_ObjLoad(UAVObjHandle obj, uint16_t instId, uint8_t * data)
* @return 0 if success or error code
* @retval -1 if object not in file table
* @retval -2 Erase failed
* @note To avoid buffering the file table (1k ram!) the entry in the file table
* remains but destination sector is erased. This will make the load fail as the
* @note To avoid buffering the file table (1k ram!) the entry in the file table
* remains but destination sector is erased. This will make the load fail as the
* file header won't match the object. At next save it goes back there.
*/
int32_t PIOS_FLASHFS_ObjDelete(UAVObjHandle obj, uint16_t instId)
{
uint32_t objId = UAVObjGetID(obj);
int32_t addr = PIOS_FLASHFS_GetObjAddress(objId, instId);
// Object currently not saved
if(addr < 0)
if(addr < 0)
return -1;
if(PIOS_Flash_W25X_EraseSector(addr) != 0)
return -2;
return 0;
}
}

4
flight/PiOS/Common/pios_hcsr04.c
View File

@ -32,8 +32,8 @@
#include "pios.h"
#if defined(PIOS_INCLUDE_HCSR04)
#if !defined(PIOS_INCLUDE_SPEKTRUM)
#error Only supported with spektrum interface!
#if !(defined(PIOS_INCLUDE_SPEKTRUM) || defined(PIOS_INCLUDE_SBUS))
#error Only supported with Spektrum or S.Bus interface!
#endif
/* Local Variables */

98
flight/PiOS/Common/pios_rcvr.c Normal file
View File

@ -0,0 +1,98 @@
/* Project Includes */
#include "pios.h"
#if defined(PIOS_INCLUDE_RCVR)
#include <pios_rcvr_priv.h>
enum pios_rcvr_dev_magic {
PIOS_RCVR_DEV_MAGIC = 0x99aabbcc,
};
struct pios_rcvr_dev {
enum pios_rcvr_dev_magic magic;
uint32_t lower_id;
const struct pios_rcvr_driver * driver;
};
static bool PIOS_RCVR_validate(struct pios_rcvr_dev * rcvr_dev)
{
return (rcvr_dev->magic == PIOS_RCVR_DEV_MAGIC);
}
#if defined(PIOS_INCLUDE_FREERTOS) && 0
static struct pios_rcvr_dev * PIOS_RCVR_alloc(void)
{
struct pios_rcvr_dev * rcvr_dev;
rcvr_dev = (struct pios_rcvr_dev *)malloc(sizeof(*rcvr_dev));
if (!rcvr_dev) return (NULL);
rcvr_dev->magic = PIOS_RCVR_DEV_MAGIC;
return(rcvr_dev);
}
#else
static struct pios_rcvr_dev pios_rcvr_devs[PIOS_RCVR_MAX_DEVS];
static uint8_t pios_rcvr_num_devs;
static struct pios_rcvr_dev * PIOS_RCVR_alloc(void)
{
struct pios_rcvr_dev * rcvr_dev;
if (pios_rcvr_num_devs >= PIOS_RCVR_MAX_DEVS) {
return (NULL);
}
rcvr_dev = &pios_rcvr_devs[pios_rcvr_num_devs++];
rcvr_dev->magic = PIOS_RCVR_DEV_MAGIC;
return (rcvr_dev);
}
#endif
/**
* Initialises RCVR layer
* \param[out] handle
* \param[in] driver
* \param[in] id
* \return < 0 if initialisation failed
*/
int32_t PIOS_RCVR_Init(uint32_t * rcvr_id, const struct pios_rcvr_driver * driver, uint32_t lower_id)
{
PIOS_DEBUG_Assert(rcvr_id);
PIOS_DEBUG_Assert(driver);
struct pios_rcvr_dev * rcvr_dev;
rcvr_dev = (struct pios_rcvr_dev *) PIOS_RCVR_alloc();
if (!rcvr_dev) goto out_fail;
rcvr_dev->driver = driver;
rcvr_dev->lower_id = lower_id;
*rcvr_id = (uint32_t)rcvr_dev;
return(0);
out_fail:
return(-1);
}
int32_t PIOS_RCVR_Read(uint32_t rcvr_id, uint8_t channel)
{
struct pios_rcvr_dev * rcvr_dev = (struct pios_rcvr_dev *)rcvr_id;
if (!PIOS_RCVR_validate(rcvr_dev)) {
/* Undefined RCVR port for this board (see pios_board.c) */
PIOS_Assert(0);
}
PIOS_DEBUG_Assert(rcvr_dev->driver->read);
return rcvr_dev->driver->read(rcvr_dev->lower_id, channel);
}
#endif
/**
* @}
* @}
*/

36
flight/PiOS/STM32F10x/Libraries/FreeRTOS/Source/portable/MemMang/heap_1.c
View File

@ -1,6 +1,6 @@
/*
FreeRTOS V7.0.1 - Copyright (C) 2011 Real Time Engineers Ltd.
***************************************************************************
* *
@ -79,16 +79,18 @@ static union xRTOS_HEAP
volatile portDOUBLE dDummy;
#else
volatile unsigned long ulDummy;
#endif
#endif
unsigned char ucHeap[ configTOTAL_HEAP_SIZE ];
} xHeap;
} xHeap __attribute__ ((section (".heap")));
static size_t xNextFreeByte = ( size_t ) 0;
static size_t currentTOTAL_HEAP_SIZE = configTOTAL_HEAP_SIZE;
/*-----------------------------------------------------------*/
void *pvPortMalloc( size_t xWantedSize )
{
void *pvReturn = NULL;
void *pvReturn = NULL;
/* Ensure that blocks are always aligned to the required number of bytes. */
#if portBYTE_ALIGNMENT != 1
@ -102,17 +104,17 @@ void *pvReturn = NULL;
vTaskSuspendAll();
{
/* Check there is enough room left for the allocation. */
if( ( ( xNextFreeByte + xWantedSize ) < configTOTAL_HEAP_SIZE ) &&
if( ( ( xNextFreeByte + xWantedSize ) < currentTOTAL_HEAP_SIZE ) &&
( ( xNextFreeByte + xWantedSize ) > xNextFreeByte ) )/* Check for overflow. */
{
/* Return the next free byte then increment the index past this
block. */
pvReturn = &( xHeap.ucHeap[ xNextFreeByte ] );
xNextFreeByte += xWantedSize;
}
xNextFreeByte += xWantedSize;
}
}
xTaskResumeAll();
#if( configUSE_MALLOC_FAILED_HOOK == 1 )
{
if( pvReturn == NULL )
@ -121,7 +123,7 @@ void *pvReturn = NULL;
vApplicationMallocFailedHook();
}
}
#endif
#endif
return pvReturn;
}
@ -129,8 +131,8 @@ void *pvReturn = NULL;
void vPortFree( void *pv )
{
/* Memory cannot be freed using this scheme. See heap_2.c and heap_3.c
for alternative implementations, and the memory management pages of
/* Memory cannot be freed using this scheme. See heap_2.c and heap_3.c
for alternative implementations, and the memory management pages of
http://www.FreeRTOS.org for more information. */
( void ) pv;
}
@ -145,8 +147,14 @@ void vPortInitialiseBlocks( void )
size_t xPortGetFreeHeapSize( void )
{
return ( configTOTAL_HEAP_SIZE - xNextFreeByte );
return ( currentTOTAL_HEAP_SIZE - xNextFreeByte );
}
/*-----------------------------------------------------------*/
void xPortIncreaseHeapSize( size_t bytes )
{
vTaskSuspendAll();
currentTOTAL_HEAP_SIZE = configTOTAL_HEAP_SIZE + bytes;
xTaskResumeAll();
}
/*-----------------------------------------------------------*/

28
flight/PiOS/STM32F10x/Libraries/FreeRTOS/Source/portable/MemMang/heap_2.c
View File

@ -1,6 +1,6 @@
/*
FreeRTOS V7.0.1 - Copyright (C) 2011 Real Time Engineers Ltd.
***************************************************************************
* *
@ -81,7 +81,7 @@ static union xRTOS_HEAP
volatile unsigned long ulDummy;
#endif
unsigned char ucHeap[ configTOTAL_HEAP_SIZE ];
} xHeap;
} xHeap __attribute__ ((section (".heap")));
/* Define the linked list structure. This is used to link free blocks in order
of their size. */
@ -101,6 +101,7 @@ static xBlockLink xStart, xEnd;
/* Keeps track of the number of free bytes remaining, but says nothing about
fragmentation. */
static size_t xFreeBytesRemaining = configTOTAL_HEAP_SIZE;
static size_t currentTOTAL_HEAP_SIZE = configTOTAL_HEAP_SIZE;
/* STATIC FUNCTIONS ARE DEFINED AS MACROS TO MINIMIZE THE FUNCTION CALL DEPTH. */
@ -140,13 +141,13 @@ xBlockLink *pxFirstFreeBlock; \
xStart.xBlockSize = ( size_t ) 0; \
\
/* xEnd is used to mark the end of the list of free blocks. */ \
xEnd.xBlockSize = configTOTAL_HEAP_SIZE; \
xEnd.xBlockSize = currentTOTAL_HEAP_SIZE; \
xEnd.pxNextFreeBlock = NULL; \
\
/* To start with there is a single free block that is sized to take up the \
entire heap space. */ \
pxFirstFreeBlock = ( void * ) xHeap.ucHeap; \
pxFirstFreeBlock->xBlockSize = configTOTAL_HEAP_SIZE; \
pxFirstFreeBlock->xBlockSize = currentTOTAL_HEAP_SIZE; \
pxFirstFreeBlock->pxNextFreeBlock = &xEnd; \
}
/*-----------------------------------------------------------*/
@ -181,7 +182,7 @@ void *pvReturn = NULL;
}
}
if( ( xWantedSize > 0 ) && ( xWantedSize < configTOTAL_HEAP_SIZE ) )
if( ( xWantedSize > 0 ) && ( xWantedSize < currentTOTAL_HEAP_SIZE ) )
{
/* Blocks are stored in byte order - traverse the list from the start
(smallest) block until one of adequate size is found. */
@ -220,7 +221,7 @@ void *pvReturn = NULL;
/* Insert the new block into the list of free blocks. */
prvInsertBlockIntoFreeList( ( pxNewBlockLink ) );
}
xFreeBytesRemaining -= pxBlock->xBlockSize;
}
}
@ -276,3 +277,18 @@ void vPortInitialiseBlocks( void )
{
/* This just exists to keep the linker quiet. */
}
void xPortIncreaseHeapSize( size_t bytes )
{
xBlockLink *pxNewBlockLink;
vTaskSuspendAll();
currentTOTAL_HEAP_SIZE = configTOTAL_HEAP_SIZE + bytes;
xEnd.xBlockSize = currentTOTAL_HEAP_SIZE;
xFreeBytesRemaining += bytes;
/* Insert the new block into the list of free blocks. */
pxNewBlockLink = ( void * ) &xHeap.ucHeap[ configTOTAL_HEAP_SIZE ];
pxNewBlockLink->xBlockSize = bytes;
prvInsertBlockIntoFreeList( ( pxNewBlockLink ) );
xTaskResumeAll();
}
/*-----------------------------------------------------------*/

21
flight/PiOS/STM32F10x/link_STM32103CB_CC_Rev1_BL_sections.ld
View File

@ -3,7 +3,8 @@ PROVIDE ( vPortSVCHandler = 0 ) ;
PROVIDE ( xPortPendSVHandler = 0 ) ;
PROVIDE ( xPortSysTickHandler = 0 ) ;
_estack = 0x20004FF0;
/* This is the size of the stack for early init and for all FreeRTOS IRQs */
_irq_stack_size = 0x400;
/* Section Definitions */
SECTIONS
@ -59,6 +60,24 @@ SECTIONS
_ebss = . ;
} > SRAM
/*
* This stack is used both as the initial sp during early init as well as ultimately
* being used as the STM32's MSP (Main Stack Pointer) which is the same stack that
* is used for _all_ interrupt handlers. The end of this stack should be placed
* against the lowest address in RAM so that a stack overrun results in a hard fault
* at the first access beyond the end of the stack.
*/
.irq_stack :
{
. = ALIGN(4);
_irq_stack_end = . ;
. = . + _irq_stack_size ;
. = ALIGN(4);
_irq_stack_top = . - 4 ;
_init_stack_top = _irq_stack_top;
. = ALIGN(4);
} > SRAM
. = ALIGN(4);
_end = . ;

73
flight/PiOS/STM32F10x/link_STM32103CB_CC_Rev1_sections.ld
View File

@ -1,3 +1,8 @@
/* This is the size of the stack for all FreeRTOS IRQs */
_irq_stack_size = 0x180;
/* This is the size of the stack for early init: life span is until scheduler starts */
_init_stack_size = 0x100;
/* Stub out these functions since we don't use them anyway */
PROVIDE ( vPortSVCHandler = 0 ) ;
PROVIDE ( xPortPendSVHandler = 0 ) ;
@ -5,8 +10,6 @@ PROVIDE ( xPortSysTickHandler = 0 ) ;
PROVIDE(pios_board_info_blob = ORIGIN(BD_INFO));
_estack = 0x20004FF0;
/* Section Definitions */
SECTIONS
{
@ -29,6 +32,16 @@ SECTIONS
__uavobj_initcall_end = .;
} >FLASH
/* module sections */
.initcallmodule.init :
{
. = ALIGN(4);
__module_initcall_start = .;
KEEP(*(.initcallmodule.init))
. = ALIGN(4);
__module_initcall_end = .;
} >FLASH
.ARM.extab :
{
*(.ARM.extab* .gnu.linkonce.armextab.*)
@ -43,6 +56,23 @@ SECTIONS
_etext = .;
_sidata = .;
/*
* This stack is used both as the initial sp during early init as well as ultimately
* being used as the STM32's MSP (Main Stack Pointer) which is the same stack that
* is used for _all_ interrupt handlers. The end of this stack should be placed
* against the lowest address in RAM so that a stack overrun results in a hard fault
* at the first access beyond the end of the stack.
*/
.irq_stack :
{
. = ALIGN(4);
_irq_stack_end = . ;
. = . + _irq_stack_size ;
. = ALIGN(4);
_irq_stack_top = . - 4 ;
. = ALIGN(4);
} > SRAM
.data : AT (_etext)
{
_sdata = .;
@ -51,18 +81,49 @@ SECTIONS
_edata = . ;
} > SRAM
/* .bss section which is used for uninitialized data */
.bss (NOLOAD) :
{
_sbss = . ;
*(.bss .bss.*)
*(COMMON)
. = ALIGN(4);
_ebss = . ;
} > SRAM
. = ALIGN(4);
_end = . ;
.heap (NOLOAD) :
{
. = ALIGN(4);
_sheap = . ;
_sheap_pre_rtos = . ;
*(.heap)
. = ALIGN(4);
_eheap = . ;
_eheap_pre_rtos = . ;
_init_stack_end = . ;
_sheap_post_rtos = . ;
. = . + _init_stack_size ;
. = ALIGN(4);
_eheap_post_rtos = . ;
_init_stack_top = . - 4 ;
} > SRAM
_free_ram = . ;
.free_ram (NOLOAD) :
{
. = ORIGIN(SRAM) + LENGTH(SRAM) - _free_ram ;
/* This is used by the startup in order to initialize the .bss section */
_ebss = . ;
_eram = . ;
} > SRAM
/* keep the heap section at the end of the SRAM
* this will allow to claim the remaining bytes not used
* at run time! (done by the reset vector).
*/
PROVIDE ( _end = _ebss ) ;
/* Stabs debugging sections. */
.stab 0 : { *(.stab) }

1
flight/PiOS/STM32F10x/link_STM3210E_INS_sections.ld
View File

@ -250,6 +250,7 @@ SECTIONS
. = . + _irq_stack_size ;
. = ALIGN(4);
_irq_stack_top = . - 4 ;
_init_stack_top = _irq_stack_top;
. = ALIGN(4);
} >RAM

3
flight/PiOS/STM32F10x/link_STM3210E_OP_BL_sections.ld
View File

@ -223,6 +223,7 @@ SECTIONS
. = . + _irq_stack_size ;
. = ALIGN(4);
_irq_stack_top = . - 4 ;
_init_stack_top = _irq_stack_top;
. = ALIGN(4);
} >RAM
@ -364,5 +365,3 @@ SECTIONS
.debug_typenames 0 : { *(.debug_typenames) }
.debug_varnames 0 : { *(.debug_varnames) }
}

49
flight/PiOS/STM32F10x/link_STM3210E_OP_sections.ld
View File

@ -1,5 +1,7 @@
/* This is the size of the stack for early init and for all FreeRTOS IRQs */
_irq_stack_size = 0x400;
/* This is the size of the stack for early init: life span is until scheduler starts */
_init_stack_size = 0x400;
/* Check valid alignment for VTOR */
ASSERT(ORIGIN(FLASH) == ALIGN(ORIGIN(FLASH), 0x80), "Start of memory region flash not aligned for startup vector table");
@ -191,6 +193,16 @@ SECTIONS
__uavobj_initcall_end = .;
} >FLASH
/* module sections */
.initcallmodule.init :
{
. = ALIGN(4);
__module_initcall_start = .;
KEEP(*(.initcallmodule.init))
. = ALIGN(4);
__module_initcall_end = .;
} >FLASH
/* the program code is stored in the .text section, which goes to Flash */
.text :
{
@ -250,17 +262,44 @@ SECTIONS
.bss :
{
. = ALIGN(4);
/* This is used by the startup in order to initialize the .bss secion */
/* This is used by the startup in order to initialize the .bss section */
_sbss = .;
*(.bss)
*(COMMON)
. = ALIGN(4);
/* This is used by the startup in order to initialize the .bss secion */
_ebss = . ;
} >RAM
.heap (NOLOAD) :
{
. = ALIGN(4);
_sheap = . ;
_sheap_pre_rtos = . ;
*(.heap)
. = ALIGN(4);
_eheap = . ;
_eheap_pre_rtos = . ;
_init_stack_end = . ;
_sheap_post_rtos = . ;
. = . + _init_stack_size ;
. = ALIGN(4);
_eheap_post_rtos = . ;
_init_stack_top = . - 4 ;
} > RAM
_free_ram = . ;
.free_ram (NOLOAD) :
{
. = ORIGIN(RAM) + LENGTH(RAM) - _free_ram ;
/* This is used by the startup in order to initialize the .bss section */
_ebss = . ;
_eram = . ;
} > RAM
/* keep the heap section at the end of the SRAM
* this will allow to claim the remaining bytes not used
* at run time! (done by the reset vector).
*/
PROVIDE ( end = _ebss );
PROVIDE ( _end = _ebss );

148
flight/PiOS/STM32F10x/pios_delay.c
View File

@ -7,10 +7,10 @@
* @{
*
* @file pios_delay.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org)
* @author Michael Smith Copyright (C) 2011
* @brief Delay Functions
* - Provides a micro-second granular delay using a TIM
* - Provides a micro-second granular delay using the CPU
* cycle counter.
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
@ -31,72 +31,97 @@
*/
/* Project Includes */
#include "pios.h"
#include <pios.h>
#if defined(PIOS_INCLUDE_DELAY)
/* these should be defined by CMSIS, but they aren't */
#define DWT_CTRL (*(volatile uint32_t *)0xe0001000)
#define CYCCNTENA (1<<0)
#define DWT_CYCCNT (*(volatile uint32_t *)0xe0001004)
/* cycles per microsecond */
static uint32_t us_ticks;
/**
* Initialises the Timer used by PIOS_DELAY functions<BR>
* This is called from pios.c as part of the main() function
* at system start up.
* \return < 0 if initialisation failed
*/
* Initialises the Timer used by PIOS_DELAY functions.
*
* \return always zero (success)
*/
int32_t PIOS_DELAY_Init(void)
{
/* Enable timer clock */
PIOS_DELAY_TIMER_RCC_FUNC;
RCC_ClocksTypeDef clocks;
/* Time base configuration */
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
TIM_TimeBaseStructure.TIM_Period = 65535; // maximum value
TIM_TimeBaseStructure.TIM_Prescaler = 72 - 1; // for 1 uS accuracy fixed to 72Mhz
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(PIOS_DELAY_TIMER, &TIM_TimeBaseStructure);
/* compute the number of system clocks per microsecond */
RCC_GetClocksFreq(&clocks);
us_ticks = clocks.SYSCLK_Frequency / 1000000;
PIOS_DEBUG_Assert(us_ticks > 1);
/* Enable counter */
TIM_Cmd(PIOS_DELAY_TIMER, ENABLE);
/* turn on access to the DWT registers */
CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
/* enable the CPU cycle counter */
DWT_CTRL |= CYCCNTENA;
return 0;
}
/**
* Waits for a specific number of uS
*
* Example:<BR>
* \code
* // Wait for 500 uS
* PIOS_DELAY_Wait_uS(500);
* \endcode
* \param[in] uS delay
* \return < 0 on errors
*/
int32_t PIOS_DELAY_WaituS(uint32_t uS)
{
uint32_t elapsed = 0;
uint32_t last_count = DWT_CYCCNT;
for (;;) {
uint32_t current_count = DWT_CYCCNT;
uint32_t elapsed_uS;
/* measure the time elapsed since the last time we checked */
elapsed += current_count - last_count;
last_count = current_count;
/* convert to microseconds */
elapsed_uS = elapsed / us_ticks;
if (elapsed_uS >= uS)
break;
/* reduce the delay by the elapsed time */
uS -= elapsed_uS;
/* keep fractional microseconds for the next iteration */
elapsed %= us_ticks;
}
/* No error */
return 0;
}
/**
* Waits for a specific number of uS<BR>
* Example:<BR>
* \code
* // Wait for 500 uS
* PIOS_DELAY_Wait_uS(500);
* \endcode
* \param[in] uS delay (1..65535 microseconds)
* \return < 0 on errors
*/
int32_t PIOS_DELAY_WaituS(uint16_t uS)
* Waits for a specific number of mS
*
* Example:<BR>
* \code
* // Wait for 500 mS
* PIOS_DELAY_Wait_mS(500);
* \endcode
* \param[in] mS delay
* \return < 0 on errors
*/
int32_t PIOS_DELAY_WaitmS(uint32_t mS)
{
uint16_t start = PIOS_DELAY_TIMER->CNT;
/* Note that this event works on 16bit counter wrap-arounds */
while ((uint16_t) (PIOS_DELAY_TIMER->CNT - start) <= uS) ;
/* No error */
return 0;
}
/**
* Waits for a specific number of mS<BR>
* Example:<BR>
* \code
* // Wait for 500 mS
* PIOS_DELAY_Wait_mS(500);
* \endcode
* \param[in] mS delay (1..65535 milliseconds)
* \return < 0 on errors
*/
int32_t PIOS_DELAY_WaitmS(uint16_t mS)
{
for (int i = 0; i < mS; i++) {
while (mS--) {
PIOS_DELAY_WaituS(1000);
}
@ -108,22 +133,19 @@ int32_t PIOS_DELAY_WaitmS(uint16_t mS)
* @brief Query the Delay timer for the current uS
* @return A microsecond value
*/
uint16_t PIOS_DELAY_GetuS()
uint32_t PIOS_DELAY_GetuS(void)
{
return PIOS_DELAY_TIMER->CNT;
return DWT_CYCCNT / us_ticks;
}
/**
* @brief Compute the difference between now and a reference time
* @param[in] the reference time to compare now to
* @return The number of uS since the delay
*
* @note the user is responsible for worrying about rollover on the 16 bit uS counter
* @brief Calculate time in microseconds since a previous time
* @param[in] t previous time
* @return time in us since previous time t.
*/
int32_t PIOS_DELAY_DiffuS(uint16_t ref)
uint32_t PIOS_DELAY_GetuSSince(uint32_t t)
{
int32_t ret_t = ref;
return (int16_t) (PIOS_DELAY_GetuS() - ret_t);
return (PIOS_DELAY_GetuS() - t);
}
#endif

290
flight/PiOS/STM32F10x/pios_ppm.c
View File

@ -34,34 +34,59 @@
#if defined(PIOS_INCLUDE_PPM)
/* Local Variables */
/* Provide a RCVR driver */
static int32_t PIOS_PPM_Get(uint32_t rcvr_id, uint8_t channel);
const struct pios_rcvr_driver pios_ppm_rcvr_driver = {
.read = PIOS_PPM_Get,
};
#define PIOS_PPM_IN_MIN_NUM_CHANNELS 4
#define PIOS_PPM_IN_MAX_NUM_CHANNELS PIOS_PPM_NUM_INPUTS
#define PIOS_PPM_STABLE_CHANNEL_COUNT 25 // frames
#define PIOS_PPM_IN_MIN_SYNC_PULSE_US 3800 // microseconds
#define PIOS_PPM_IN_MIN_CHANNEL_PULSE_US 750 // microseconds
#define PIOS_PPM_IN_MAX_CHANNEL_PULSE_US 2250 // microseconds
#define PIOS_PPM_INPUT_INVALID 0
/* Local Variables */
static TIM_ICInitTypeDef TIM_ICInitStructure;
static uint8_t PulseIndex;
static uint32_t PreviousValue;
static uint32_t CurrentValue;
static uint32_t CapturedValue;
static uint32_t CaptureValue[PIOS_PPM_NUM_INPUTS];
static uint32_t PreviousTime;
static uint32_t CurrentTime;
static uint32_t DeltaTime;
static uint32_t CaptureValue[PIOS_PPM_IN_MAX_NUM_CHANNELS];
static uint32_t CaptureValueNewFrame[PIOS_PPM_IN_MAX_NUM_CHANNELS];
static uint32_t LargeCounter;
static int8_t NumChannels;
static int8_t NumChannelsPrevFrame;
static uint8_t NumChannelCounter;
static uint8_t SupervisorState = 0;
static uint32_t CapCounter[PIOS_PPM_NUM_INPUTS];
static uint32_t CapCounterPrev[PIOS_PPM_NUM_INPUTS];
static uint8_t supv_timer = 0;
static bool Tracking;
static bool Fresh;
static void PIOS_PPM_Supervisor(uint32_t ppm_id);
/**
* Initialises all the LED's
*/
void PIOS_PPM_Init(void)
{
/* Flush counter variables */
int32_t i;
PulseIndex = 0;
PreviousValue = 0;
CurrentValue = 0;
CapturedValue = 0;
PreviousTime = 0;
CurrentTime = 0;
DeltaTime = 0;
LargeCounter = 0;
NumChannels = -1;
NumChannelsPrevFrame = -1;
NumChannelCounter = 0;
Tracking = FALSE;
Fresh = FALSE;
for (i = 0; i < PIOS_PPM_NUM_INPUTS; i++) {
for (i = 0; i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) {
CaptureValue[i] = 0;
CaptureValueNewFrame[i] = 0;
}
NVIC_InitTypeDef NVIC_InitStructure = pios_ppm_cfg.irq.init;
@ -85,7 +110,6 @@ void PIOS_PPM_Init(void)
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
break;
#ifdef STM32F10X_HD
case (int32_t)TIM5:
NVIC_InitStructure.NVIC_IRQChannel = TIM5_IRQn;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5, ENABLE);
@ -121,85 +145,20 @@ void PIOS_PPM_Init(void)
TIM_TimeBaseInit(pios_ppm_cfg.timer, &TIM_TimeBaseStructure);
/* Enable the Capture Compare Interrupt Request */
TIM_ITConfig(pios_ppm_cfg.timer, pios_ppm_cfg.ccr, ENABLE);
TIM_ITConfig(pios_ppm_cfg.timer, pios_ppm_cfg.ccr | TIM_IT_Update, ENABLE);
/* Enable timers */
TIM_Cmd(pios_ppm_cfg.timer, ENABLE);
/* Supervisor Setup */
#if (PIOS_PPM_SUPV_ENABLED)
/* Flush counter variables */
for (i = 0; i < PIOS_PPM_NUM_INPUTS; i++) {
CapCounter[i] = 0;
}
for (i = 0; i < PIOS_PPM_NUM_INPUTS; i++) {
CapCounterPrev[i] = 0;
}
NVIC_InitStructure = pios_ppmsv_cfg.irq.init;
/* Enable appropriate clock to timer module */
switch((int32_t) pios_ppmsv_cfg.timer) {
case (int32_t)TIM1:
NVIC_InitStructure.NVIC_IRQChannel = TIM1_CC_IRQn;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);
break;
case (int32_t)TIM2:
NVIC_InitStructure.NVIC_IRQChannel = TIM2_IRQn;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
break;
case (int32_t)TIM3:
NVIC_InitStructure.NVIC_IRQChannel = TIM3_IRQn;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
break;
case (int32_t)TIM4:
NVIC_InitStructure.NVIC_IRQChannel = TIM4_IRQn;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
break;
#ifdef STM32F10X_HD
case (int32_t)TIM5:
NVIC_InitStructure.NVIC_IRQChannel = TIM5_IRQn;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5, ENABLE);
break;
case (int32_t)TIM6:
NVIC_InitStructure.NVIC_IRQChannel = TIM6_IRQn;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM6, ENABLE);
break;
case (int32_t)TIM7:
NVIC_InitStructure.NVIC_IRQChannel = TIM7_IRQn;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM7, ENABLE);
break;
case (int32_t)TIM8:
NVIC_InitStructure.NVIC_IRQChannel = TIM8_CC_IRQn;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8, ENABLE);
break;
#endif
}
/* Configure interrupts */
NVIC_Init(&NVIC_InitStructure);
/* Time base configuration */
TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
TIM_TimeBaseStructure = pios_ppmsv_cfg.tim_base_init;
TIM_TimeBaseInit(pios_ppmsv_cfg.timer, &TIM_TimeBaseStructure);
/* Enable the CCx Interrupt Request */
TIM_ITConfig(pios_ppmsv_cfg.timer, pios_ppmsv_cfg.ccr, ENABLE);
/* Clear update pending flag */
TIM_ClearFlag(pios_ppmsv_cfg.timer, TIM_FLAG_Update);
/* Enable counter */
TIM_Cmd(pios_ppmsv_cfg.timer, ENABLE);
#endif
/* Setup local variable which stays in this scope */
/* Doing this here and using a local variable saves doing it in the ISR */
TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
TIM_ICInitStructure.TIM_ICFilter = 0x0;
if (!PIOS_RTC_RegisterTickCallback(PIOS_PPM_Supervisor, 0)) {
PIOS_DEBUG_Assert(0);
}
}
/**
@ -208,13 +167,13 @@ void PIOS_PPM_Init(void)
* \output -1 Channel not available
* \output >0 Channel value
*/
int32_t PIOS_PPM_Get(int8_t Channel)
static int32_t PIOS_PPM_Get(uint32_t rcvr_id, uint8_t channel)
{
/* Return error if channel not available */
if (Channel >= PIOS_PPM_NUM_INPUTS) {
if (channel >= PIOS_PPM_IN_MAX_NUM_CHANNELS) {
return -1;
}
return CaptureValue[Channel];
return CaptureValue[channel];
}
/**
@ -224,75 +183,128 @@ int32_t PIOS_PPM_Get(int8_t Channel)
*/
void PIOS_PPM_irq_handler(void)
{
/* Do this as it's more efficient */
/* Timer Overflow Interrupt
* The time between timer overflows must be greater than the PPM
* frame period. If a full frame has not decoded in the between timer
* overflows then capture values should be cleared.
*/
if (TIM_GetITStatus(pios_ppm_cfg.timer, TIM_IT_Update) == SET) {
/* Clear TIMx overflow interrupt pending bit */
TIM_ClearITPendingBit(pios_ppm_cfg.timer, TIM_IT_Update);
/* If sharing a timer with a servo output the ARR register will
be set according to the PWM period. When timer reaches the
ARR value a timer overflow interrupt will fire. We use the
interrupt accumulate a 32-bit timer. */
LargeCounter = LargeCounter + pios_ppm_cfg.timer->ARR;
}
/* Signal edge interrupt */
if (TIM_GetITStatus(pios_ppm_cfg.timer, pios_ppm_cfg.ccr) == SET) {
PreviousValue = CurrentValue;
PreviousTime = CurrentTime;
switch((int32_t) pios_ppm_cfg.ccr) {
case (int32_t)TIM_IT_CC1:
CurrentValue = TIM_GetCapture1(pios_ppm_cfg.timer);
CurrentTime = TIM_GetCapture1(pios_ppm_cfg.timer);
break;
case (int32_t)TIM_IT_CC2:
CurrentValue = TIM_GetCapture2(pios_ppm_cfg.timer);
CurrentTime = TIM_GetCapture2(pios_ppm_cfg.timer);
break;
case (int32_t)TIM_IT_CC3:
CurrentValue = TIM_GetCapture3(pios_ppm_cfg.timer);
CurrentTime = TIM_GetCapture3(pios_ppm_cfg.timer);
break;
case (int32_t)TIM_IT_CC4:
CurrentValue = TIM_GetCapture4(pios_ppm_cfg.timer);
CurrentTime = TIM_GetCapture4(pios_ppm_cfg.timer);
break;
}
}
/* Clear TIMx Capture compare interrupt pending bit */
TIM_ClearITPendingBit(pios_ppm_cfg.timer, pios_ppm_cfg.ccr);
/* Clear TIMx Capture compare interrupt pending bit */
TIM_ClearITPendingBit(pios_ppm_cfg.timer, pios_ppm_cfg.ccr);
/* Capture computation */
if (CurrentValue > PreviousValue) {
CapturedValue = (CurrentValue - PreviousValue);
} else {
CapturedValue = ((0xFFFF - PreviousValue) + CurrentValue);
}
/* Convert to 32-bit timer result */
CurrentTime = CurrentTime + LargeCounter;
/* sync pulse */
if (CapturedValue > 8000) {
PulseIndex = 0;
/* trying to detect bad pulses, not sure this is working correctly yet. I need a scope :P */
} else if (CapturedValue > 750 && CapturedValue < 2500) {
if (PulseIndex < PIOS_PPM_NUM_INPUTS) {
CaptureValue[PulseIndex] = CapturedValue;
CapCounter[PulseIndex]++;
PulseIndex++;
/* Capture computation */
DeltaTime = CurrentTime - PreviousTime;
PreviousTime = CurrentTime;
/* Sync pulse detection */
if (DeltaTime > PIOS_PPM_IN_MIN_SYNC_PULSE_US) {
if (PulseIndex == NumChannelsPrevFrame
&& PulseIndex >= PIOS_PPM_IN_MIN_NUM_CHANNELS
&& PulseIndex <= PIOS_PPM_IN_MAX_NUM_CHANNELS)
{
/* If we see n simultaneous frames of the same
number of channels we save it as our frame size */
if (NumChannelCounter < PIOS_PPM_STABLE_CHANNEL_COUNT)
NumChannelCounter++;
else
NumChannels = PulseIndex;
} else {
NumChannelCounter = 0;
}
/* Check if the last frame was well formed */
if (PulseIndex == NumChannels && Tracking) {
/* The last frame was well formed */
for (uint32_t i = 0; i < NumChannels; i++) {
CaptureValue[i] = CaptureValueNewFrame[i];
}
for (uint32_t i = NumChannels;
i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) {
CaptureValue[i] = PIOS_PPM_INPUT_INVALID;
}
}
Fresh = TRUE;
Tracking = TRUE;
NumChannelsPrevFrame = PulseIndex;
PulseIndex = 0;
/* We rely on the supervisor to set CaptureValue to invalid
if no valid frame is found otherwise we ride over it */
} else if (Tracking) {
/* Valid pulse duration 0.75 to 2.5 ms*/
if (DeltaTime > PIOS_PPM_IN_MIN_CHANNEL_PULSE_US
&& DeltaTime < PIOS_PPM_IN_MAX_CHANNEL_PULSE_US
&& PulseIndex < PIOS_PPM_IN_MAX_NUM_CHANNELS) {
CaptureValueNewFrame[PulseIndex] = DeltaTime;
PulseIndex++;
} else {
/* Not a valid pulse duration */
Tracking = FALSE;
for (uint32_t i = 0; i < PIOS_PPM_IN_MAX_NUM_CHANNELS ; i++) {
CaptureValueNewFrame[i] = PIOS_PPM_INPUT_INVALID;
}
}
}
}
}
/**
* This function handles TIM3 global interrupt request.
*/
void PIOS_PPMSV_irq_handler(void) {
/* Clear timer interrupt pending bit */
TIM_ClearITPendingBit(pios_ppmsv_cfg.timer, pios_ppmsv_cfg.ccr);
/* Simple state machine */
if (SupervisorState == 0) {
/* Save this states values */
for (int32_t i = 0; i < PIOS_PPM_NUM_INPUTS; i++) {
CapCounterPrev[i] = CapCounter[i];
}
/* Move to next state */
SupervisorState = 1;
} else {
/* See what channels have been updated */
for (int32_t i = 0; i < PIOS_PPM_NUM_INPUTS; i++) {
if (CapCounter[i] == CapCounterPrev[i]) {
CaptureValue[i] = 0;
}
}
/* Move to next state */
SupervisorState = 0;
static void PIOS_PPM_Supervisor(uint32_t ppm_id) {
/*
* RTC runs at 625Hz so divide down the base rate so
* that this loop runs at 25Hz.
*/
if(++supv_timer < 25) {
return;
}
supv_timer = 0;
if (!Fresh) {
Tracking = FALSE;
for (int32_t i = 0; i < PIOS_PPM_IN_MAX_NUM_CHANNELS ; i++) {
CaptureValue[i] = PIOS_PPM_INPUT_INVALID;
CaptureValueNewFrame[i] = PIOS_PPM_INPUT_INVALID;
}
}
Fresh = FALSE;
}
#endif

Some files were not shown because too many files have changed in this diff Show More