rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

Merge branch 'next' of ssh://git.openpilot.org/OpenPilot into kfinisterre/WiresharkPlugin

Browse Source
This commit is contained in:
abeck70 2015-05-18 23:00:29 +10:00
commit 9bfc68ca62
377 changed files with 30230 additions and 38002 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.gitignore vendored
View File

@ -2,6 +2,7 @@
/downloads
/tools
/build
/3rdparty
# Exclude temporary and system files
Thumbs.db
@ -11,6 +12,7 @@ GRTAGS
GSYMS
GTAGS
core
*~
# flight
/flight/*.pnproj

227
Makefile
View File

@ -49,6 +49,8 @@ export BUILD_DIR := $(ROOT_DIR)/build
export PACKAGE_DIR := $(ROOT_DIR)/build/package
export DIST_DIR := $(ROOT_DIR)/build/dist
DIRS = $(DL_DIR) $(TOOLS_DIR) $(BUILD_DIR) $(PACKAGE_DIR) $(DIST_DIR)
# Set up default build configurations (debug | release)
GCS_BUILD_CONF := release
UAVOGEN_BUILD_CONF := release
@ -81,10 +83,12 @@ $(foreach var, $(SANITIZE_DEPRECATED_VARS), $(eval $(call SANITIZE_VAR,$(var),de
# Make sure this isn't being run as root unless installing (no whoami on Windows, but that is ok here)
ifeq ($(shell whoami 2>/dev/null),root)
ifeq ($(filter install all_clean,$(MAKECMDGOALS)),)
ifeq ($(filter install,$(MAKECMDGOALS)),)
ifndef FAKEROOTKEY
$(error You should not be running this as root)
endif
endif
endif
# Decide on a verbosity level based on the V= parameter
export AT := @
@ -109,6 +113,9 @@ endif
# Include tools installers
include $(ROOT_DIR)/make/tools.mk
# Include third party builders if available
-include $(ROOT_DIR)/make/3rdparty/3rdparty.mk
# We almost need to consider autoconf/automake instead of this
ifeq ($(UNAME), Linux)
QT_SPEC = linux-g++
@ -138,20 +145,6 @@ all_clean:
.PONY: clean
clean: all_clean
$(DL_DIR):
$(MKDIR) -p $@
$(TOOLS_DIR):
$(MKDIR) -p $@
$(BUILD_DIR):
$(MKDIR) -p $@
$(PACKAGE_DIR):
$(MKDIR) -p $@
$(DIST_DIR):
$(MKDIR) -p $@
##############################
#
@ -165,13 +158,15 @@ else
UAVOGEN_SILENT := silent
endif
UAVOBJGENERATOR_DIR = $(BUILD_DIR)/uavobjgenerator
DIRS += $(UAVOBJGENERATOR_DIR)
.PHONY: uavobjgenerator
uavobjgenerator:
$(V1) $(MKDIR) -p $(BUILD_DIR)/$@
$(V1) ( cd $(BUILD_DIR)/$@ && \
$(QMAKE) $(ROOT_DIR)/ground/uavobjgenerator/uavobjgenerator.pro -spec $(QT_SPEC) -r CONFIG+="$(UAVOGEN_BUILD_CONF) $(UAVOGEN_SILENT)" && \
$(MAKE) --no-print-directory -w ; \
)
uavobjgenerator: | $(UAVOBJGENERATOR_DIR)
$(V1) cd $(UAVOBJGENERATOR_DIR) && \
$(QMAKE) $(ROOT_DIR)/ground/uavobjgenerator/uavobjgenerator.pro \
-spec $(QT_SPEC) -r CONFIG+=$(UAVOGEN_BUILD_CONF) CONFIG+=$(UAVOGEN_SILENT) && \
$(MAKE) --no-print-directory -w
UAVOBJ_TARGETS := gcs flight python matlab java wireshark
@ -181,8 +176,7 @@ uavobjects: $(addprefix uavobjects_, $(UAVOBJ_TARGETS))
UAVOBJ_XML_DIR := $(ROOT_DIR)/shared/uavobjectdefinition
UAVOBJ_OUT_DIR := $(BUILD_DIR)/uavobject-synthetics
$(UAVOBJ_OUT_DIR):
$(V1) $(MKDIR) -p $@
DIRS += $(UAVOBJ_OUT_DIR)
uavobjects_%: $(UAVOBJ_OUT_DIR) uavobjgenerator
$(V1) ( cd $(UAVOBJ_OUT_DIR) && \
@ -212,11 +206,12 @@ export OPUAVTALK := $(ROOT_DIR)/flight/uavtalk
export OPUAVSYNTHDIR := $(BUILD_DIR)/uavobject-synthetics/flight
export OPGCSSYNTHDIR := $(BUILD_DIR)/openpilotgcs-synthetics
DIRS += $(OPGCSSYNTHDIR)
# Define supported board lists
ALL_BOARDS := coptercontrol oplinkmini revolution osd revoproto simposix discoveryf4bare gpsplatinum
ALL_BOARDS := oplinkmini revolution osd revoproto simposix discoveryf4bare gpsplatinum
# Short names of each board (used to display board name in parallel builds)
coptercontrol_short := 'cc '
oplinkmini_short := 'oplm'
revolution_short := 'revo'
osd_short := 'osd '
@ -458,8 +453,9 @@ sim_osx_%: uavobjects_flight
all_ground: openpilotgcs uploader
# Convenience target for the GCS
.PHONY: gcs gcs_clean
.PHONY: gcs gcs_qmake gcs_clean
gcs: openpilotgcs
gcs_qmake: openpilotgcs_qmake
gcs_clean: openpilotgcs_clean
ifeq ($(V), 1)
@ -468,32 +464,27 @@ else
GCS_SILENT := silent
endif
.NOTPARALLEL:
.PHONY: openpilotgcs
openpilotgcs: uavobjects_gcs openpilotgcs_qmake openpilotgcs_make
OPENPILOTGCS_DIR := $(BUILD_DIR)/openpilotgcs_$(GCS_BUILD_CONF)
DIRS += $(OPENPILOTGCS_DIR)
OPENPILOTGCS_MAKEFILE := $(OPENPILOTGCS_DIR)/Makefile
.PHONY: openpilotgcs_qmake
openpilotgcs_qmake:
ifeq ($(QMAKE_SKIP),)
$(V1) $(MKDIR) -p $(BUILD_DIR)/openpilotgcs_$(GCS_BUILD_CONF)
$(V1) ( cd $(BUILD_DIR)/openpilotgcs_$(GCS_BUILD_CONF) && \
$(QMAKE) $(ROOT_DIR)/ground/openpilotgcs/openpilotgcs.pro -spec $(QT_SPEC) -r CONFIG+="$(GCS_BUILD_CONF) $(GCS_SILENT)" $(GCS_QMAKE_OPTS) \
)
else
@$(ECHO) "skipping qmake"
endif
openpilotgcs_qmake $(OPENPILOTGCS_MAKEFILE): | $(OPENPILOTGCS_DIR)
$(V1) cd $(OPENPILOTGCS_DIR) && \
$(QMAKE) $(ROOT_DIR)/ground/openpilotgcs/openpilotgcs.pro \
-spec $(QT_SPEC) -r CONFIG+=$(GCS_BUILD_CONF) CONFIG+=$(GCS_SILENT) $(GCS_QMAKE_OPTS)
.PHONY: openpilotgcs_make
openpilotgcs_make:
$(V1) $(MKDIR) -p $(BUILD_DIR)/openpilotgcs_$(GCS_BUILD_CONF)
$(V1) ( cd $(BUILD_DIR)/openpilotgcs_$(GCS_BUILD_CONF)/$(MAKE_DIR) && \
$(MAKE) -w ; \
)
.PHONY: openpilotgcs
openpilotgcs: uavobjects_gcs $(OPENPILOTGCS_MAKEFILE)
$(V1) $(MAKE) -w -C $(OPENPILOTGCS_DIR)/$(MAKE_DIR);
.PHONY: openpilotgcs_clean
openpilotgcs_clean:
@$(ECHO) " CLEAN $(call toprel, $(BUILD_DIR)/openpilotgcs_$(GCS_BUILD_CONF))"
$(V1) [ ! -d "$(BUILD_DIR)/openpilotgcs_$(GCS_BUILD_CONF)" ] || $(RM) -r "$(BUILD_DIR)/openpilotgcs_$(GCS_BUILD_CONF)"
@$(ECHO) " CLEAN $(call toprel, $(OPENPILOTGCS_DIR))"
$(V1) [ ! -d "$(OPENPILOTGCS_DIR)" ] || $(RM) -r "$(OPENPILOTGCS_DIR)"
################################
#
@ -501,77 +492,27 @@ openpilotgcs_clean:
#
################################
.NOTPARALLEL:
.PHONY: uploader
uploader: uploader_qmake uploader_make
UPLOADER_DIR := $(BUILD_DIR)/uploader_$(GCS_BUILD_CONF)
DIRS += $(UPLOADER_DIR)
UPLOADER_MAKEFILE := $(UPLOADER_DIR)/Makefile
.PHONY: uploader_qmake
uploader_qmake:
ifeq ($(QMAKE_SKIP),)
$(V1) $(MKDIR) -p $(BUILD_DIR)/uploader_$(GCS_BUILD_CONF)
$(V1) ( cd $(BUILD_DIR)/uploader_$(GCS_BUILD_CONF) && \
$(QMAKE) $(ROOT_DIR)/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/upload.pro -spec $(QT_SPEC) -r CONFIG+="$(GCS_BUILD_CONF) $(GCS_SILENT)" $(GCS_QMAKE_OPTS) \
)
else
@$(ECHO) "skipping qmake"
endif
uploader_qmake $(UPLOADER_MAKEFILE): | $(UPLOADER_DIR)
$(V1) cd $(UPLOADER_DIR) && \
$(QMAKE) $(ROOT_DIR)/ground/openpilotgcs/src/experimental/USB_UPLOAD_TOOL/upload.pro \
-spec $(QT_SPEC) -r CONFIG+=$(GCS_BUILD_CONF) CONFIG+=$(GCS_SILENT) $(GCS_QMAKE_OPTS)
.PHONY: uploader_make
uploader_make:
$(V1) $(MKDIR) -p $(BUILD_DIR)/uploader_$(GCS_BUILD_CONF)
$(V1) ( cd $(BUILD_DIR)/uploader_$(GCS_BUILD_CONF)/$(MAKE_DIR) && \
$(MAKE) -w ; \
)
.PHONY: uploader
uploader: $(UPLOADER_MAKEFILE)
$(V1) $(MAKE) -w -C $(UPLOADER_DIR)
.PHONY: uploader_clean
uploader_clean:
@$(ECHO) " CLEAN $(call toprel, $(BUILD_DIR)/uploader_$(GCS_BUILD_CONF))"
$(V1) [ ! -d "$(BUILD_DIR)/uploader_$(GCS_BUILD_CONF)" ] || $(RM) -r "$(BUILD_DIR)/uploader_$(GCS_BUILD_CONF)"
@$(ECHO) " CLEAN $(call toprel, $(UPLOADER_DIR))"
$(V1) [ ! -d "$(UPLOADER_DIR)" ] || $(RM) -r "$(UPLOADER_DIR)"
################################
#
# Android GCS related components
#
################################
# Build the output directory for the Android GCS build
ANDROIDGCS_OUT_DIR := $(BUILD_DIR)/androidgcs
$(ANDROIDGCS_OUT_DIR):
$(V1) $(MKDIR) -p $@
# Build the asset directory for the android assets
ANDROIDGCS_ASSETS_DIR := $(ANDROIDGCS_OUT_DIR)/assets
$(ANDROIDGCS_ASSETS_DIR)/uavos:
$(V1) $(MKDIR) -p $@
ifeq ($(V), 1)
ANT_QUIET :=
ANDROID_SILENT :=
else
ANT_QUIET := -q
ANDROID_SILENT := -s
endif
.PHONY: androidgcs
androidgcs: uavo-collections_java
$(V0) @$(ECHO) " ANDROID $(call toprel, $(ANDROIDGCS_OUT_DIR))"
$(V1) $(MKDIR) -p $(ANDROIDGCS_OUT_DIR)
$(V1) $(ANDROID) $(ANDROID_SILENT) update project \
--target "Google Inc.:Google APIs:$(GOOGLE_API_VERSION)" \
--name androidgcs \
--path ./androidgcs
$(V1) $(ANT) -f ./androidgcs/build.xml \
$(ANT_QUIET) \
-Dout.dir="../$(call toprel, $(ANDROIDGCS_OUT_DIR)/bin)" \
-Dgen.absolute.dir="$(ANDROIDGCS_OUT_DIR)/gen" \
$(ANDROIDGCS_BUILD_CONF)
.PHONY: androidgcs_clean
androidgcs_clean:
@$(ECHO) " CLEAN $(call toprel, $(ANDROIDGCS_OUT_DIR))"
$(V1) [ ! -d "$(ANDROIDGCS_OUT_DIR)" ] || $(RM) -r "$(ANDROIDGCS_OUT_DIR)"
# We want to take snapshots of the UAVOs at each point that they change
# to allow the GCS to be compatible with as many versions as possible.
# We always include a pseudo collection called "srctree" which represents
@ -693,8 +634,7 @@ ALL_UNITTESTS := logfs math lednotification
# Build the directory for the unit tests
UT_OUT_DIR := $(BUILD_DIR)/unit_tests
$(UT_OUT_DIR):
$(V1) $(MKDIR) -p $@
DIRS += $(UT_OUT_DIR)
.PHONY: all_ut
all_ut: $(addsuffix _elf, $(addprefix ut_, $(ALL_UNITTESTS)))
@ -769,14 +709,13 @@ OPFW_CONTENTS := \
.PHONY: opfw_resource
opfw_resource: $(OPFW_RESOURCE)
$(OPFW_RESOURCE): $(FW_TARGETS)
$(OPFW_RESOURCE): $(FW_TARGETS) | $(OPGCSSYNTHDIR)
@$(ECHO) Generating OPFW resource file $(call toprel, $@)
$(V1) $(MKDIR) -p $(dir $@)
$(V1) $(ECHO) $(QUOTE)$(OPFW_CONTENTS)$(QUOTE) > $@
# If opfw_resource or all firmware are requested, GCS should depend on the resource
ifneq ($(strip $(filter opfw_resource all all_fw all_flight package,$(MAKECMDGOALS))),)
$(eval openpilotgcs_qmake: $(OPFW_RESOURCE))
$(OPENPILOTGCS_MAKEFILE): $(OPFW_RESOURCE)
endif
# Packaging targets: package
@ -785,19 +724,15 @@ endif
# - calls paltform-specific packaging script
# Define some variables
export PACKAGE_LBL := $(shell $(VERSION_INFO) --format=\$${LABEL})
export PACKAGE_NAME := OpenPilot
export PACKAGE_SEP := -
.PHONY: package
PACKAGE_LBL := $(shell $(VERSION_INFO) --format=\$${LABEL})
PACKAGE_NAME := OpenPilot
PACKAGE_SEP := -
PACKAGE_FULL_NAME := $(PACKAGE_NAME)$(PACKAGE_SEP)$(PACKAGE_LBL)
include $(ROOT_DIR)/package/$(UNAME).mk
package: all_fw all_ground uavobjects_matlab $(PACKAGE_DIR)
ifneq ($(GCS_BUILD_CONF),release)
# We can only package release builds
$(error Packaging is currently supported for release builds only)
endif
# Source distribution is never dirty because it uses git archive
DIST_NAME := $(DIST_DIR)/$(subst dirty-,,$(PACKAGE_FULL_NAME)).tar
##############################
#
@ -866,9 +801,8 @@ docs_all_clean:
##############################
.PHONY: build-info
build-info:
build-info: | $(BUILD_DIR)
@$(ECHO) " BUILD-INFO $(call toprel, $(BUILD_DIR)/$@.txt)"
$(V1) $(MKDIR) -p $(BUILD_DIR)
$(V1) $(VERSION_INFO) \
--uavodir=$(ROOT_DIR)/shared/uavobjectdefinition \
--template="make/templates/$@.txt" \
@ -882,20 +816,31 @@ build-info:
DIST_VER_INFO := $(DIST_DIR)/version-info.json
.PHONY: $(DIST_VER_INFO) # Because to many deps to list
$(DIST_VER_INFO): $(DIST_DIR)
$(DIST_VER_INFO): .git/index | $(DIST_DIR)
$(V1) $(VERSION_INFO) --jsonpath="$(DIST_DIR)"
.PHONY: dist
dist: $(DIST_DIR) $(DIST_VER_INFO)
@$(ECHO) " SOURCE FOR DISTRIBUTION $(call toprel, $(DIST_DIR))"
$(eval DIST_NAME := $(call toprel, "$(DIST_DIR)/OpenPilot-$(shell git describe).tar"))
$(V1) git archive --prefix="OpenPilot/" -o "$(DIST_NAME)" HEAD
$(DIST_NAME).gz: $(DIST_VER_INFO) .git/index | $(DIST_DIR)
@$(ECHO) " SOURCE FOR DISTRIBUTION $(call toprel, $(DIST_NAME).gz)"
$(V1) git archive --prefix="$(PACKAGE_NAME)/" -o "$(DIST_NAME)" HEAD
$(V1) tar --append --file="$(DIST_NAME)" \
--transform='s,.*version-info.json,OpenPilot/version-info.json,' \
--transform='s,.*version-info.json,$(PACKAGE_NAME)/version-info.json,' \
$(call toprel, "$(DIST_VER_INFO)")
$(V1) gzip -f "$(DIST_NAME)"
.PHONY: dist
dist: $(DIST_NAME).gz
##############################
#
# Directories
#
##############################
$(DIRS):
$(V1) $(MKDIR) -p $@
##############################
#
@ -930,7 +875,6 @@ help:
@$(ECHO) " openocd_install - Install the OpenOCD JTAG daemon"
@$(ECHO) " stm32flash_install - Install the stm32flash tool for unbricking F1-based boards"
@$(ECHO) " dfuutil_install - Install the dfu-util tool for unbricking F4-based boards"
@$(ECHO) " android_sdk_install - Install the Android SDK tools"
@$(ECHO) " Install all available tools:"
@$(ECHO) " all_sdk_install - Install all of above (platform-dependent)"
@$(ECHO) " build_sdk_install - Install only essential for build tools (platform-dependent)"
@ -1002,26 +946,19 @@ help:
@$(ECHO)
@$(ECHO) " [GCS]"
@$(ECHO) " gcs - Build the Ground Control System (GCS) application (debug|release)"
@$(ECHO) " Skip qmake: QMAKE_SKIP=1"
@$(ECHO) " Compile specific directory: MAKE_DIR=<dir>"
@$(ECHO) " Example: make gcs QMAKE_SKIP=1 MAKE_DIR=src/plugins/coreplugin"
@$(ECHO) " Example: make gcs MAKE_DIR=src/plugins/coreplugin"
@$(ECHO) " gcs_qmake - Run qmake for the Ground Control System (GCS) application (debug|release)"
@$(ECHO) " gcs_clean - Remove the Ground Control System (GCS) application (debug|release)"
@$(ECHO) " Supported build configurations: GCS_BUILD_CONF=debug|release (default is $(GCS_BUILD_CONF))"
@$(ECHO)
@$(ECHO) " [Uploader Tool]"
@$(ECHO) " uploader - Build the serial uploader tool (debug|release)"
@$(ECHO) " Skip qmake: QMAKE_SKIP=1"
@$(ECHO) " Example: make uploader QMAKE_SKIP=1"
@$(ECHO) " uploader_qmake - Run qmake for the serial uploader tool (debug|release)"
@$(ECHO) " uploader_clean - Remove the serial uploader tool (debug|release)"
@$(ECHO) " Supported build configurations: GCS_BUILD_CONF=debug|release (default is $(GCS_BUILD_CONF))"
@$(ECHO)
@$(ECHO)
@$(ECHO) " [AndroidGCS]"
@$(ECHO) " androidgcs - Build the Android Ground Control System (GCS) application"
@$(ECHO) " androidgcs_install - Use ADB to install the Android GCS application"
@$(ECHO) " androidgcs_run - Run the Android GCS application"
@$(ECHO) " androidgcs_clean - Remove the Android GCS application"
@$(ECHO)
@$(ECHO) " [UAVObjects]"
@$(ECHO) " uavobjects - Generate source files from the UAVObject definition XML files"
@$(ECHO) " uavobjects_test - Parse xml-files - check for valid, duplicate ObjId's, ..."

83
WHATSNEW.txt
View File

@ -1,3 +1,86 @@
Release Notes - OpenPilot - Version RELEASE-15.05
Fully autonomous flight is now possible with autotakeoff and landing as flight modes available to path plans.
An all new implementation of altitude vario provides improved altitude maintenance and smoother flight.
A new RateTrainer mode for beginners and aerial photography make it easier to learn this mode my limiting the pitch and roll extents.
4CH transmitters for multirotor and 2CH for ground is now supported.
It is now easier to takeoff with an improved axislock on yaw implementation.
Full speed flight just got faster with new motor constraints that maintain your ability to enact roll without requiring a upper throttle limit.
Setup on GPS just got easier. See the wiki for details.
New control protocols and support for due telemtry and OSD programing have been added.
The full list of bugfixes in this release is accessible here:
** Bug
* [OP-1691] - PIOS_DELAY_WaitmS() in RFM22B causes jitter
* [OP-1756] - Add option to Vehicle Setup Wizard to calibrate all motor outputs at the same time.
* [OP-1778] - resolve win_sdk_install.sh issue with https://
* [OP-1793] - Fixes for Sensor module
* [OP-1794] - AxisLock windup not cleared with low throttle while armed
* [OP-1834] - Piro Comp - adverse effect in Atti/Ratti Modes
* [OP-1841] - Serial telemetry is not reliable
* [OP-1855] - Limit parsing error in logs when starting GCS
* [OP-1858] - PathPlanner AutoTakeoff fixes
* [OP-1867] - PathPlanner AutoLand simplification
* [OP-1869] - Allow Analog Airspeed scale
* [OP-1872] - Vehicle Wiz Tricopter tail servo settings don't save
** Improvement
* [OP-1289] - Need Revo to send two telemetry streams for OSD and GCS
* [OP-1734] - Clarify the need of reversing servo in FW vehicle wizard
* [OP-1736] - Make package label something more meaningful than the date.
* [OP-1739] - Add GNSS (GPS/GLONASS) selection to UBX autoconf
* [OP-1750] - Revo state estimation CPU optimization
* [OP-1776] - Performance optimizations for UAVTalk telemetry
* [OP-1783] - Fall back PathPlanner flight mode to PH with config warning if no plan
* [OP-1791] - Change the description of the stabilization modes
* [OP-1796] - Upgrade GCS to qt 5.4.1
* [OP-1797] - Improve GCS workspace layout management reactivity
* [OP-1798] - GCS ophid plugin is too verbose
* [OP-1802] - Throttle filterstationary fake pos/vel data rate
* [OP-1808] - Make the flight mode switch step in transmitter setup wizard to be optional
* [OP-1814] - Reset Button for mAH used
* [OP-1828] - Changes ADC module to support other pins as optional analog inputs
* [OP-1835] - Add motor constraints in place of overhead throttle buffer for enhanced stability and power.
* [OP-1837] - Add support for Multiplex SRXL protocol
* [OP-1840] - GPS serlal port needed features
* [OP-1844] - Create a vagrant environment that contains all the bits for Android development, including Android.
* [OP-1848] - Rewrite AltVario/Hold in C++ for functional improvements
* [OP-1852] - Include version number in window title bar.
* [OP-1874] - Various improvements to led notifications
** New Feature
* [OP-1696] - PathFollower C++ Rewrite: Autonomous Landing, Velocity Roam, RTBL, GroundPathFollower
* [OP-1760] - AutoTakeoff
* [OP-1769] - Support Ground Vehicles with 2CH receiver and reversible motor
* [OP-1781] - Ground Input Channel Configuration
* [OP-1803] - Create UAVTalk objects for receiver signal quality
* [OP-1818] - Update OP toolchain for compiling osgearth
* [OP-1832] - Need method to get default UAV Object settings in Java UAVObjects
* [OP-1849] - Support programming/update of minimosd using USB VCP port
* [OP-1863] - RateTrainer mode - add maxpitch for beginners and aerial photography
--- RELEASE-15.02.02 ---
This release fixes a bug that prevents revo onboard mag to work correctly.
Release Notes - OpenPilot - Version RELEASE-15.02.02
The full list of bugfixes in this release is accessible here:
https://progress.openpilot.org/issues/?filter=12262
** Bug
* [OP-1820] - fix onboard mag orientation
* [OP-1821] - Tricopter tail servo wrong speed on wizard
* [OP-1827] - Version ID wrong in Windows uninstaller
* [OP-1857] - PPM on Flexi does not work on CC/CC3D
** Task
* [OP-1831] - due to oneshot higher pid values ki now shows "red" warning in stabilization page
--- RELEASE-15.02.01 ---
This release fixes an in important bug that may prevent failsafe to work correctly using CC3D/CC with a PWM receiver.

8
flight/libraries/inc/CoordinateConversions.h
View File

@ -112,7 +112,7 @@ static inline void matrix_mult_3x3f(float a[3][3], float b[3][3], float result[3
result[2][2] = a[0][2] * b[2][0] + a[1][2] * b[2][1] + a[2][2] * b[2][2];
}
inline void matrix_inline_scale_3f(float a[3][3], float scale)
static inline void matrix_inline_scale_3f(float a[3][3], float scale)
{
a[0][0] *= scale;
a[0][1] *= scale;
@ -127,7 +127,7 @@ inline void matrix_inline_scale_3f(float a[3][3], float scale)
a[2][2] *= scale;
}
inline void rot_about_axis_x(const float rotation, float R[3][3])
static inline void rot_about_axis_x(const float rotation, float R[3][3])
{
float s = sinf(rotation);
float c = cosf(rotation);
@ -145,7 +145,7 @@ inline void rot_about_axis_x(const float rotation, float R[3][3])
R[2][2] = c;
}
inline void rot_about_axis_y(const float rotation, float R[3][3])
static inline void rot_about_axis_y(const float rotation, float R[3][3])
{
float s = sinf(rotation);
float c = cosf(rotation);
@ -163,7 +163,7 @@ inline void rot_about_axis_y(const float rotation, float R[3][3])
R[2][2] = c;
}
inline void rot_about_axis_z(const float rotation, float R[3][3])
static inline void rot_about_axis_z(const float rotation, float R[3][3])
{
float s = sinf(rotation);
float c = cosf(rotation);

2
flight/libraries/inc/paths.h
View File

@ -34,6 +34,6 @@ struct path_status {
float correction_vector[3];
};
void path_progress(PathDesiredData *path, float *cur_point, struct path_status *status);
void path_progress(PathDesiredData *path, float *cur_point, struct path_status *status, bool mode3D);
#endif

34
flight/libraries/inc/plans.h
View File

@ -62,27 +62,47 @@ void plan_setup_returnToBase();
* @brief setup pathplanner/pathfollower for land
*/
void plan_setup_land();
/**
* @brief execute land
*/
void plan_run_land();
void plan_setup_AutoTakeoff();
/**
* @brief setup pathplanner/pathfollower for braking
*/
void plan_setup_assistedcontrol(uint8_t timeout_occurred);
void plan_setup_assistedcontrol();
#define PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_NORTH 0
#define PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_EAST 1
#define PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_DOWN 2
#define PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT 3
#define PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_NORTH 0
#define PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_EAST 1
#define PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_DOWN 2
#define PATHDESIRED_MODEPARAMETER_VELOCITY_UNUSED 3
#define PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_NORTH 0
#define PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_EAST 1
#define PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_DOWN 2
#define PATHDESIRED_MODEPARAMETER_LAND_OPTIONS 3
#define PATHDESIRED_MODEPARAMETER_LAND_OPTION_NONE 0
#define PATHDESIRED_MODEPARAMETER_LAND_OPTION_HORIZONTAL_PH 1
#define PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_NEXTCOMMAND 0
#define PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_UNUSED1 1
#define PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_UNUSED2 2
#define PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_UNUSED3 3
#define PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_NORTH 0
#define PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_EAST 1
#define PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_DOWN 2
#define PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_CONTROLSTATE 3
/**
* @brief setup pathfollower for positionvario
*/
void plan_setup_CourseLock();
void plan_setup_PositionRoam();
void plan_setup_VelocityRoam();
void plan_setup_HomeLeash();
void plan_setup_AbsolutePosition();
@ -91,8 +111,10 @@ void plan_setup_AbsolutePosition();
*/
void plan_run_CourseLock();
void plan_run_PositionRoam();
void plan_run_VelocityRoam();
void plan_run_HomeLeash();
void plan_run_AbsolutePosition();
void plan_run_AutoTakeoff();
/**
* @brief setup pathplanner/pathfollower for AutoCruise

6
flight/libraries/inc/stopwatch.h
View File

@ -47,7 +47,7 @@ typedef struct {
/////////////////////////////////////////////////////////////////////////////
inline int32_t STOPWATCH_Init(uint32_t resolution, stopwatch_t *stopwatch)
static inline int32_t STOPWATCH_Init(uint32_t resolution, stopwatch_t *stopwatch)
{
stopwatch->raw = PIOS_DELAY_GetRaw();
stopwatch->resolution = resolution;
@ -58,7 +58,7 @@ inline int32_t STOPWATCH_Init(uint32_t resolution, stopwatch_t *stopwatch)
// ! Resets the stopwatch
// ! \return < 0 on errors
/////////////////////////////////////////////////////////////////////////////
inline int32_t STOPWATCH_Reset(stopwatch_t *stopwatch)
static inline int32_t STOPWATCH_Reset(stopwatch_t *stopwatch)
{
stopwatch->raw = PIOS_DELAY_GetRaw();
return 0; // no error
@ -68,7 +68,7 @@ inline int32_t STOPWATCH_Reset(stopwatch_t *stopwatch)
// ! Returns current value of stopwatch
// ! \return stopwatch value
/////////////////////////////////////////////////////////////////////////////
inline uint32_t STOPWATCH_ValueGet(stopwatch_t *stopwatch)
static inline uint32_t STOPWATCH_ValueGet(stopwatch_t *stopwatch)
{
uint32_t value = PIOS_DELAY_GetuSSince(stopwatch->raw);

157
flight/libraries/insgps13state.c
View File

@ -33,25 +33,26 @@
#include <math.h>
#include <stdint.h>
#include <pios_math.h>
#include <mathmisc.h>
// constants/macros/typdefs
#define NUMX 13 // number of states, X is the state vector
#define NUMW 9 // number of plant noise inputs, w is disturbance noise vector
#define NUMV 10 // number of measurements, v is the measurement noise vector
#define NUMU 6 // number of deterministic inputs, U is the input vector
#pragma GCC optimize "O3"
// Private functions
void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
float Q[NUMW], float dT, float P[NUMX][NUMX]);
void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
static void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
float Y[NUMV], float P[NUMX][NUMX], float X[NUMX],
uint16_t SensorsUsed);
void RungeKutta(float X[NUMX], float U[NUMU], float dT);
void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX]);
void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX],
static void RungeKutta(float X[NUMX], float U[NUMU], float dT);
static void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX]);
static void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX],
float G[NUMX][NUMW]);
void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV]);
void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX]);
static void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV]);
static void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX]);
// Private variables
@ -60,29 +61,29 @@ void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX]);
// LinearizeFG() and LinearizeH():
//
// usage F: usage G: usage H:
// 0123456789abc 012345678 0123456789abc
// -0123456789abc 012345678 0123456789abc
// 0...X......... ......... X............
// 1....X........ ......... .X...........
// 2.....X....... ......... ..X..........
// 3......XXXX... ...XXX... ...X.........
// 4......XXXX... ...XXX... ....X........
// 5......XXXX... ...XXX... .....X.......
// 6.......XXXXXX XXX...... ......XXXX...
// 7......X.XXXXX XXX...... ......XXXX...
// 8......XX.XXXX XXX...... ......XXXX...
// 9......XXX.XXX XXX...... ..X..........
// a............. ......X..
// b............. .......X.
// c............. ........X
// 6.....ooXXXXXX XXX...... ......XXXX...
// 7.....oXoXXXXX XXX...... ......XXXX...
// 8.....oXXoXXXX XXX...... ......XXXX...
// 9.....oXXXoXXX XXX...... ..X..........
// a............. ......Xoo
// b............. ......oXo
// c............. ......ooX
static const int8_t FrowMin[NUMX] = { 3, 4, 5, 6, 6, 6, 7, 6, 6, 6, 13, 13, 13 };
static const int8_t FrowMax[NUMX] = { 3, 4, 5, 9, 9, 9, 12, 12, 12, 12, -1, -1, -1 };
static int8_t FrowMin[NUMX] = { 3, 4, 5, 6, 6, 6, 5, 5, 5, 5, 13, 13, 13 };
static int8_t FrowMax[NUMX] = { 3, 4, 5, 9, 9, 9, 12, 12, 12, 12, -1, -1, -1 };
static const int8_t GrowMin[NUMX] = { 9, 9, 9, 3, 3, 3, 0, 0, 0, 0, 6, 7, 8 };
static const int8_t GrowMax[NUMX] = { -1, -1, -1, 5, 5, 5, 2, 2, 2, 2, 6, 7, 8 };
static int8_t GrowMin[NUMX] = { 9, 9, 9, 3, 3, 3, 0, 0, 0, 0, 6, 7, 8 };
static int8_t GrowMax[NUMX] = { -1, -1, -1, 5, 5, 5, 2, 2, 2, 2, 6, 7, 8 };
static const int8_t HrowMin[NUMV] = { 0, 1, 2, 3, 4, 5, 6, 6, 6, 2 };
static const int8_t HrowMax[NUMV] = { 0, 1, 2, 3, 4, 5, 9, 9, 9, 2 };
static int8_t HrowMin[NUMV] = { 0, 1, 2, 3, 4, 5, 6, 6, 6, 2 };
static int8_t HrowMax[NUMV] = { 0, 1, 2, 3, 4, 5, 9, 9, 9, 2 };
static struct EKFData {
// linearized system matrices
@ -182,8 +183,8 @@ void INSGetP(float PDiag[NUMX])
uint8_t i;
// retrieve diagonal elements (aka state variance)
for (i = 0; i < NUMX; i++) {
if (PDiag != 0) {
for (i = 0; i < NUMX; i++) {
PDiag[i] = ekf.P[i][i];
}
}
@ -289,7 +290,7 @@ void INSSetMagNorth(float B[3])
void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT)
{
float U[6];
float qmag;
float invqmag;
// rate gyro inputs in units of rad/s
U[0] = gyro_data[0];
@ -304,11 +305,11 @@ void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT)
// EKF prediction step
LinearizeFG(ekf.X, U, ekf.F, ekf.G);
RungeKutta(ekf.X, U, dT);
qmag = sqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]);
ekf.X[6] /= qmag;
ekf.X[7] /= qmag;
ekf.X[8] /= qmag;
ekf.X[9] /= qmag;
invqmag = fast_invsqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]);
ekf.X[6] *= invqmag;
ekf.X[7] *= invqmag;
ekf.X[8] *= invqmag;
ekf.X[9] *= invqmag;
// CovariancePrediction(ekf.F,ekf.G,ekf.Q,dT,ekf.P);
// Update Nav solution structure
@ -373,8 +374,8 @@ void VelBaroCorrection(float Vel[3], float BaroAlt)
void INSCorrection(float mag_data[3], float Pos[3], float Vel[3],
float BaroAlt, uint16_t SensorsUsed)
{
float Z[10], Y[10];
float Bmag, qmag;
float Z[10] = { 0 };
float Y[10] = { 0 };
// GPS Position in meters and in local NED frame
Z[0] = Pos[0];
@ -385,14 +386,15 @@ void INSCorrection(float mag_data[3], float Pos[3], float Vel[3],
Z[3] = Vel[0];
Z[4] = Vel[1];
Z[5] = Vel[2];
// magnetometer data in any units (use unit vector) and in body frame
Bmag =
sqrtf(mag_data[0] * mag_data[0] + mag_data[1] * mag_data[1] +
mag_data[2] * mag_data[2]);
Z[6] = mag_data[0] / Bmag;
Z[7] = mag_data[1] / Bmag;
Z[8] = mag_data[2] / Bmag;
if (SensorsUsed & MAG_SENSORS) {
float invBmag = fast_invsqrtf(mag_data[0] * mag_data[0] + mag_data[1] * mag_data[1] + mag_data[2] * mag_data[2]);
Z[6] = mag_data[0] * invBmag;
Z[7] = mag_data[1] * invBmag;
Z[8] = mag_data[2] * invBmag;
}
// barometric altimeter in meters and in local NED frame
Z[9] = BaroAlt;
@ -401,12 +403,12 @@ void INSCorrection(float mag_data[3], float Pos[3], float Vel[3],
LinearizeH(ekf.X, ekf.Be, ekf.H);
MeasurementEq(ekf.X, ekf.Be, Y);
SerialUpdate(ekf.H, ekf.R, Z, Y, ekf.P, ekf.X, SensorsUsed);
qmag = sqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]);
ekf.X[6] /= qmag;
ekf.X[7] /= qmag;
ekf.X[8] /= qmag;
ekf.X[9] /= qmag;
float invqmag = fast_invsqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]);
ekf.X[6] *= invqmag;
ekf.X[7] *= invqmag;
ekf.X[8] *= invqmag;
ekf.X[9] *= invqmag;
// Update Nav solution structure
Nav.Pos[0] = ekf.X[0];
Nav.Pos[1] = ekf.X[1];
@ -434,29 +436,31 @@ void INSCorrection(float mag_data[3], float Pos[3], float Vel[3],
// The first Method is very specific to this implementation
// ************************************************
__attribute__((optimize("O3")))
void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
float Q[NUMW], float dT, float P[NUMX][NUMX])
{
// Pnew = (I+F*T)*P*(I+F*T)' + (T^2)*G*Q*G' = (T^2)[(P/T + F*P)*(I/T + F') + G*Q*G')]
float dT1 = 1.0f / dT; // multiplication is faster than division on fpu.
float dTsq = dT * dT;
const float dT1 = 1.0f / dT; // multiplication is faster than division on fpu.
const float dTsq = dT * dT;
float Dummy[NUMX][NUMX];
int8_t i;
int8_t k;
for (i = 0; i < NUMX; i++) { // Calculate Dummy = (P/T +F*P)
float *Firow = F[i];
float *Pirow = P[i];
float *Dirow = Dummy[i];
int8_t Fistart = FrowMin[i];
int8_t Fiend = FrowMax[i];
const int8_t Fistart = FrowMin[i];
const int8_t Fiend = FrowMax[i];
int8_t j;
for (j = 0; j < NUMX; j++) {
Dirow[j] = Pirow[j] * dT1; // Dummy = P / T ...
int8_t k;
}
for (k = Fistart; k <= Fiend; k++) {
for (j = 0; j < NUMX; j++) {
Dirow[j] += Firow[k] * P[k][j]; // [] + F * P
}
}
@ -465,29 +469,45 @@ void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
float *Dirow = Dummy[i];
float *Girow = G[i];
float *Pirow = P[i];
int8_t Gistart = GrowMin[i];
int8_t Giend = GrowMax[i];
const int8_t Gistart = GrowMin[i];
const int8_t Giend = GrowMax[i];
int8_t j;
for (j = i; j < NUMX; j++) { // Use symmetry, ie only find upper triangular
float Ptmp = Dirow[j] * dT1; // Pnew = Dummy / T ...
{
float *Fjrow = F[j];
const float *Fjrow = F[j];
int8_t Fjstart = FrowMin[j];
int8_t Fjend = FrowMax[j];
int8_t k;
for (k = Fjstart; k <= Fjend; k++) {
k = Fjstart;
while (k <= Fjend - 3) {
Ptmp += Dirow[k] * Fjrow[k]; // [] + Dummy*F' ...
Ptmp += Dirow[k + 1] * Fjrow[k + 1];
Ptmp += Dirow[k + 2] * Fjrow[k + 2];
Ptmp += Dirow[k + 3] * Fjrow[k + 3];
k += 4;
}
while (k <= Fjend) {
Ptmp += Dirow[k] * Fjrow[k];
k++;
}
{
float *Gjrow = G[j];
int8_t Gjstart = MAX(Gistart, GrowMin[j]);
int8_t Gjend = MIN(Giend, GrowMax[j]);
int8_t k;
for (k = Gjstart; k <= Gjend; k++) {
const int8_t Gjstart = MAX(Gistart, GrowMin[j]);
const int8_t Gjend = MIN(Giend, GrowMax[j]);
k = Gjstart;
while (k <= Gjend - 2) {
Ptmp += Q[k] * Girow[k] * Gjrow[k]; // [] + G*Q*G' ...
Ptmp += Q[k + 1] * Girow[k + 1] * Gjrow[k + 1];
Ptmp += Q[k + 2] * Girow[k + 2] * Gjrow[k + 2];
k += 3;
}
if (k <= Gjend) {
Ptmp += Q[k] * Girow[k] * Gjrow[k];
if (k <= Gjend - 1) {
Ptmp += Q[k + 1] * Girow[k + 1] * Gjrow[k + 1];
}
}
@ -511,7 +531,6 @@ void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
// The SensorsUsed variable is a bitwise mask indicating which sensors
// should be used in the update.
// ************************************************
void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
float Y[NUMV], float P[NUMX][NUMX], float X[NUMX],
uint16_t SensorsUsed)
@ -524,7 +543,10 @@ void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
if (SensorsUsed & (0x01 << m)) { // use this sensor for update
for (j = 0; j < NUMX; j++) { // Find Hp = H*P
HP[j] = 0;
}
for (k = HrowMin[m]; k <= HrowMax[m]; k++) {
for (j = 0; j < NUMX; j++) { // Find Hp = H*P
HP[j] += H[m][k] * P[k][j];
}
}
@ -532,14 +554,13 @@ void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
for (k = HrowMin[m]; k <= HrowMax[m]; k++) {
HPHR += HP[k] * H[m][k];
}
float invHPHR = 1.0f / HPHR;
for (k = 0; k < NUMX; k++) {
Km[k] = HP[k] / HPHR; // find K = HP/HPHR
Km[k] = HP[k] * invHPHR; // find K = HP/HPHR
}
for (i = 0; i < NUMX; i++) { // Find P(m)= P(m-1) + K*HP
for (j = i; j < NUMX; j++) {
P[i][j] = P[j][i] =
P[i][j] - Km[i] * HP[j];
P[i][j] = P[j][i] = P[i][j] - Km[i] * HP[j];
}
}
@ -560,8 +581,8 @@ void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
void RungeKutta(float X[NUMX], float U[NUMU], float dT)
{
float dT2 =
dT / 2.0f, K1[NUMX], K2[NUMX], K3[NUMX], K4[NUMX], Xlast[NUMX];
const float dT2 = dT / 2.0f;
float K1[NUMX], K2[NUMX], K3[NUMX], K4[NUMX], Xlast[NUMX];
uint8_t i;
for (i = 0; i < NUMX; i++) {
@ -585,7 +606,7 @@ void RungeKutta(float X[NUMX], float U[NUMU], float dT)
for (i = 0; i < NUMX; i++) {
X[i] =
Xlast[i] + dT * (K1[i] + 2.0f * K2[i] + 2.0f * K3[i] +
K4[i]) / 6.0f;
K4[i]) * (1.0f / 6.0f);
}
}
@ -608,7 +629,7 @@ void RungeKutta(float X[NUMX], float U[NUMU], float dT)
// H is output of LinearizeH(), all elements not set should be zero
// ************************************************
void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX])
static void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX])
{
float ax, ay, az, wx, wy, wz, q0, q1, q2, q3;

92
flight/libraries/math/pid.c
View File

@ -143,3 +143,95 @@ void pid_configure(struct pid *pid, float p, float i, float d, float iLim)
pid->d = d;
pid->iLim = iLim;
}
/**
* Configure the settings for a pid2 structure
* @param[out] pid The PID2 structure to configure
* @param[in] kp proportional gain
* @param[in] ki integral gain. Time constant Ti = kp/ki
* @param[in] kd derivative gain. Time constant Td = kd/kp
* @param[in] Tf filtering time = (kd/k)/N, N is in the range of 2 to 20
* @param[in] kt tracking gain for anti-windup. Tt = √TiTd and Tt = (Ti + Td)/2
* @param[in] dt delta time increment
* @param[in] beta setpoint weight on setpoint in P component. beta=1 error feedback. beta=0 smoothes out response to changes in setpoint
* @param[in] u0 initial output for r=y at activation to achieve bumpless transfer
* @param[in] va constant for compute of actuator output for check against limits for antiwindup
* @param[in] vb multiplier for compute of actuator output for check against limits for anti-windup
*/
void pid2_configure(struct pid2 *pid, float kp, float ki, float kd, float Tf, float kt, float dT, float beta, float u0, float va, float vb)
{
pid->reconfigure = true;
pid->u0 = u0;
pid->va = va;
pid->vb = vb;
pid->kp = kp;
pid->beta = beta; // setpoint weight on proportional term
pid->bi = ki * dT;
pid->br = kt * dT / vb;
pid->ad = Tf / (Tf + dT);
pid->bd = kd / (Tf + dT);
}
/**
* Achieve a bumpless transfer and trigger initialisation of I term
* @param[out] pid The PID structure to configure
* @param[in] u0 initial output for r=y at activation to achieve bumpless transfer
*/
void pid2_transfer(struct pid2 *pid, float u0)
{
pid->reconfigure = true;
pid->u0 = u0;
}
/**
* pid controller with setpoint weighting, anti-windup, with a low-pass filtered derivative on the process variable
* See "Feedback Systems" for an explanation
* @param[out] pid The PID structure to configure
* @param[in] r setpoint
* @param[in] y process variable
* @param[in] ulow lower limit on actuator
* @param[in] uhigh upper limit on actuator
*/
float pid2_apply(
struct pid2 *pid,
const float r,
const float y,
const float ulow,
const float uhigh)
{
// on reconfigure ensure bumpless transfer
// http://www.controlguru.com/2008/021008.html
if (pid->reconfigure) {
pid->reconfigure = false;
// initialise derivative terms
pid->yold = y;
pid->D = 0.0f;
// t=0, u=u0, y=y0, v=u
pid->I = (pid->u0 - pid->va) / pid->vb - pid->kp * (pid->beta * r - y);
}
// compute proportional part
pid->P = pid->kp * (pid->beta * r - y);
// update derivative part
pid->D = pid->ad * pid->D - pid->bd * (y - pid->yold);
// compute temporary output
float v = pid->va + pid->vb * (pid->P + pid->I + pid->D);
// simulate actuator saturation
float u = boundf(v, ulow, uhigh);
// update integral
pid->I = pid->I + pid->bi * (r - y) + pid->br * (u - v);
// update old process output
pid->yold = y;
return u;
}

25
flight/libraries/math/pid.h
View File

@ -44,7 +44,25 @@ struct pid {
float lastDer;
};
typedef struct pid_scaler {
// pid2 structure for a PID+setpoint weighting, anti-windup and filtered derivative control
struct pid2 {
float u0;
float va;
float vb;
float kp;
float bi;
float ad;
float bd;
float br;
float beta;
float yold;
float P;
float I;
float D;
uint8_t reconfigure;
};
typedef struct pid_scaler_s {
float p;
float i;
float d;
@ -57,4 +75,9 @@ void pid_zero(struct pid *pid);
void pid_configure(struct pid *pid, float p, float i, float d, float iLim);
void pid_configure_derivative(float cutoff, float gamma);
// Methods for use with pid2 structure
void pid2_configure(struct pid2 *pid, float kp, float ki, float kd, float Tf, float kt, float dT, float beta, float u0, float va, float vb);
void pid2_transfer(struct pid2 *pid, float u0);
float pid2_apply(struct pid2 *pid, const float r, const float y, float ulow, float uhigh);
#endif /* PID_H */

28
flight/libraries/paths.c
View File

@ -44,33 +44,27 @@ static void path_circle(PathDesiredData *path, float *cur_point, struct path_sta
* @param[in] cur_point Current location
* @param[out] status Structure containing progress along path and deviation
*/
void path_progress(PathDesiredData *path, float *cur_point, struct path_status *status)
void path_progress(PathDesiredData *path, float *cur_point, struct path_status *status, bool mode3D)
{
switch (path->Mode) {
case PATHDESIRED_MODE_BRAKE: // should never get here...
case PATHDESIRED_MODE_FLYVECTOR:
return path_vector(path, cur_point, status, true);
case PATHDESIRED_MODE_BRAKE:
case PATHDESIRED_MODE_FOLLOWVECTOR:
return path_vector(path, cur_point, status, mode3D);
break;
case PATHDESIRED_MODE_DRIVEVECTOR:
return path_vector(path, cur_point, status, false);
case PATHDESIRED_MODE_CIRCLERIGHT:
return path_circle(path, cur_point, status, true);
break;
case PATHDESIRED_MODE_FLYCIRCLERIGHT:
case PATHDESIRED_MODE_DRIVECIRCLERIGHT:
return path_circle(path, cur_point, status, 1);
case PATHDESIRED_MODE_CIRCLELEFT:
return path_circle(path, cur_point, status, false);
break;
case PATHDESIRED_MODE_FLYCIRCLELEFT:
case PATHDESIRED_MODE_DRIVECIRCLELEFT:
return path_circle(path, cur_point, status, 0);
case PATHDESIRED_MODE_GOTOENDPOINT:
return path_endpoint(path, cur_point, status, mode3D);
break;
case PATHDESIRED_MODE_FLYENDPOINT:
return path_endpoint(path, cur_point, status, true);
break;
case PATHDESIRED_MODE_DRIVEENDPOINT:
case PATHDESIRED_MODE_LAND:
default:
// use the endpoint as default failsafe if called in unknown modes
return path_endpoint(path, cur_point, status, false);

372
flight/libraries/pid/pidcontroldown.cpp Normal file
View File

@ -0,0 +1,372 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup PathFollower PID interface class
* @brief PID loop for down control
* @{
*
* @file pidcontroldown.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Executes PID control for down direction
*
* @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
*/
extern "C" {
#include <openpilot.h>
#include <callbackinfo.h>
#include <math.h>
#include <pid.h>
#include <CoordinateConversions.h>
#include <sin_lookup.h>
#include <pathdesired.h>
#include <paths.h>
#include "plans.h"
#include <stabilizationdesired.h>
#include <pidstatus.h>
#include <vtolselftuningstats.h>
}
#include "pidcontroldown.h"
#define NEUTRALTHRUST_PH_POSITIONAL_ERROR_LIMIT 0.5f
#define NEUTRALTHRUST_PH_VEL_DESIRED_LIMIT 0.2f
#define NEUTRALTHRUST_PH_VEL_STATE_LIMIT 0.2f
#define NEUTRALTHRUST_PH_VEL_ERROR_LIMIT 0.1f
#define NEUTRALTHRUST_START_DELAY (2 * 20) // 2 seconds at rate of 20Hz (50ms update rate)
#define NEUTRALTHRUST_END_COUNT (NEUTRALTHRUST_START_DELAY + (4 * 20)) // 4 second sample
PIDControlDown::PIDControlDown()
: deltaTime(0.0f), mVelocitySetpointTarget(0.0f), mVelocitySetpointCurrent(0.0f), mVelocityState(0.0f), mDownCommand(0.0f),
mCallback(NULL), mNeutral(0.5f), mVelocityMax(1.0f), mPositionSetpointTarget(0.0f), mPositionState(0.0f),
mMinThrust(0.1f), mMaxThrust(0.6f), mActive(false), mAllowNeutralThrustCalc(true)
{
Deactivate();
}
PIDControlDown::~PIDControlDown() {}
void PIDControlDown::Initialize(PIDControlDownCallback *callback)
{
mCallback = callback;
}
void PIDControlDown::SetThrustLimits(float min_thrust, float max_thrust)
{
mMinThrust = min_thrust;
mMaxThrust = max_thrust;
}
void PIDControlDown::Deactivate()
{
mActive = false;
}
void PIDControlDown::Activate()
{
float currentThrust;
StabilizationDesiredThrustGet(&currentThrust);
pid2_transfer(&PID, currentThrust);
mActive = true;
}
void PIDControlDown::UpdateParameters(float kp, float ki, float kd, float beta, float dT, float velocityMax)
{
// pid_configure(&PID, kp, ki, kd, ilimit);
float Ti = kp / ki;
float Td = kd / kp;
float Tt = (Ti + Td) / 2.0f;
float kt = 1.0f / Tt;
float N = 10.0f;
float Tf = Td / N;
if (ki < 1e-6f) {
// Avoid Ti being infinite
Ti = 1e6f;
// Tt antiwindup time constant - we don't need antiwindup with no I term
Tt = 1e6f;
kt = 0.0f;
}
if (kd < 1e-6f) {
// PI Controller or P Controller
Tf = Ti / N;
}
if (beta > 1.0f) {
beta = 1.0f;
} else if (beta < 0.4f) {
beta = 0.4f;
}
pid2_configure(&PID, kp, ki, kd, Tf, kt, dT, beta, mNeutral, mNeutral, -1.0f);
deltaTime = dT;
mVelocityMax = velocityMax;
}
void PIDControlDown::UpdatePositionalParameters(float kp)
{
pid_configure(&PIDpos, kp, 0.0f, 0.0f, 0.0f);
}
void PIDControlDown::UpdatePositionSetpoint(float setpointDown)
{
mPositionSetpointTarget = setpointDown;
}
void PIDControlDown::UpdatePositionState(float pvDown)
{
mPositionState = pvDown;
setup_neutralThrustCalc();
}
// This is a pure position hold position control
void PIDControlDown::ControlPosition()
{
// Current progress location relative to end
float velDown = 0.0f;
velDown = pid_apply(&PIDpos, mPositionSetpointTarget - mPositionState, deltaTime);
UpdateVelocitySetpoint(velDown);
run_neutralThrustCalc();
}
void PIDControlDown::ControlPositionWithPath(struct path_status *progress)
{
// Current progress location relative to end
float velDown = progress->path_vector[2];
velDown += pid_apply(&PIDpos, progress->correction_vector[2], deltaTime);
UpdateVelocitySetpoint(velDown);
}
void PIDControlDown::run_neutralThrustCalc(void)
{
// if auto thrust and we have not run a correction calc check for PH and stability to then run an assessment
// note that arming into this flight mode is not allowed, so assumption here is that
// we have not landed. If GPSAssist+Manual/Cruise control thrust mode is used, a user overriding the
// altitude maintaining PID will have moved the throttle state to FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL.
// In manualcontrol.c the state will stay in manual throttle until the throttle command exceeds the vtol thrust min,
// avoiding auto-takeoffs. Therefore no need to check that here.
if (neutralThrustEst.have_correction != true) {
// Make FSM specific
bool stable = (fabsf(mPositionSetpointTarget - mPositionState) < NEUTRALTHRUST_PH_POSITIONAL_ERROR_LIMIT &&
fabsf(mVelocitySetpointCurrent) < NEUTRALTHRUST_PH_VEL_DESIRED_LIMIT &&
fabsf(mVelocityState) < NEUTRALTHRUST_PH_VEL_STATE_LIMIT &&
fabsf(mVelocitySetpointCurrent - mVelocityState) < NEUTRALTHRUST_PH_VEL_ERROR_LIMIT);
if (stable) {
if (neutralThrustEst.start_sampling) {
neutralThrustEst.count++;
// delay count for 2 seconds into hold allowing for stablisation
if (neutralThrustEst.count > NEUTRALTHRUST_START_DELAY) {
neutralThrustEst.sum += PID.I;
if (PID.I < neutralThrustEst.min) {
neutralThrustEst.min = PID.I;
}
if (PID.I > neutralThrustEst.max) {
neutralThrustEst.max = PID.I;
}
}
if (neutralThrustEst.count >= NEUTRALTHRUST_END_COUNT) {
// 6 seconds have past
// lets take an average
neutralThrustEst.average = neutralThrustEst.sum / (float)(NEUTRALTHRUST_END_COUNT - NEUTRALTHRUST_START_DELAY);
neutralThrustEst.correction = neutralThrustEst.average;
PID.I -= neutralThrustEst.average;
neutralThrustEst.start_sampling = false;
neutralThrustEst.have_correction = true;
// Write a new adjustment value
// vtolSelfTuningStats.NeutralThrustOffset was incremental adjusted above
VtolSelfTuningStatsData vtolSelfTuningStats;
VtolSelfTuningStatsGet(&vtolSelfTuningStats);
// add the average remaining i value to the
vtolSelfTuningStats.NeutralThrustOffset += neutralThrustEst.correction;
vtolSelfTuningStats.NeutralThrustCorrection = neutralThrustEst.correction; // the i term thrust correction value applied
vtolSelfTuningStats.NeutralThrustAccumulator = PID.I; // the actual iaccumulator value after correction
vtolSelfTuningStats.NeutralThrustRange = neutralThrustEst.max - neutralThrustEst.min;
VtolSelfTuningStatsSet(&vtolSelfTuningStats);
}
} else {
// start a tick count
neutralThrustEst.start_sampling = true;
neutralThrustEst.count = 0;
neutralThrustEst.sum = 0.0f;
neutralThrustEst.max = 0.0f;
neutralThrustEst.min = 0.0f;
}
} else {
// reset sampling as we did't get 6 continuous seconds
neutralThrustEst.start_sampling = false;
}
} // else we already have a correction for this PH run
}
void PIDControlDown::setup_neutralThrustCalc(void)
{
// reset neutral thrust assessment.
// and do once for each position hold engagement
neutralThrustEst.start_sampling = false;
neutralThrustEst.count = 0;
neutralThrustEst.sum = 0.0f;
neutralThrustEst.have_correction = false;
neutralThrustEst.average = 0.0f;
neutralThrustEst.correction = 0.0f;
neutralThrustEst.min = 0.0f;
neutralThrustEst.max = 0.0f;
}
void PIDControlDown::UpdateNeutralThrust(float neutral)
{
if (mActive) {
// adjust neutral and achieve bumpless transfer
PID.va = neutral;
pid2_transfer(&PID, mDownCommand);
}
mNeutral = neutral;
}
void PIDControlDown::UpdateVelocitySetpoint(float setpoint)
{
mVelocitySetpointTarget = setpoint;
if (fabsf(mVelocitySetpointTarget) > mVelocityMax) {
// maintain sign but set to max
mVelocitySetpointTarget *= mVelocityMax / fabsf(mVelocitySetpointTarget);
}
}
void PIDControlDown::RateLimit(float *spDesired, float *spCurrent, float rateLimit)
{
float velocity_delta = *spDesired - *spCurrent;
if (fabsf(velocity_delta) < 1e-6f) {
*spCurrent = *spDesired;
return;
}
// Calculate the rate of change
float accelerationDesired = velocity_delta / deltaTime;
if (fabsf(accelerationDesired) > rateLimit) {
accelerationDesired *= rateLimit / accelerationDesired;
}
if (fabsf(accelerationDesired) < 0.1f) {
*spCurrent = *spDesired;
} else {
*spCurrent += accelerationDesired * deltaTime;
}
}
void PIDControlDown::UpdateBrakeVelocity(float startingVelocity, float dT, float brakeRate, float currentVelocity, float *updatedVelocity)
{
if (startingVelocity >= 0.0f) {
*updatedVelocity = startingVelocity - dT * brakeRate;
if (*updatedVelocity > currentVelocity) {
*updatedVelocity = currentVelocity;
}
if (*updatedVelocity < 0.0f) {
*updatedVelocity = 0.0f;
}
} else {
*updatedVelocity = startingVelocity + dT * brakeRate;
if (*updatedVelocity < currentVelocity) {
*updatedVelocity = currentVelocity;
}
if (*updatedVelocity > 0.0f) {
*updatedVelocity = 0.0f;
}
}
}
void PIDControlDown::UpdateVelocityStateWithBrake(float pvDown, float path_time, float brakeRate)
{
mVelocityState = pvDown;
float velocitySetpointDesired;
UpdateBrakeVelocity(mVelocitySetpointTarget, path_time, brakeRate, pvDown, &velocitySetpointDesired);
// Calculate the rate of change
// RateLimit(velocitySetpointDesired[iaxis], mVelocitySetpointCurrent[iaxis], 2.0f );
mVelocitySetpointCurrent = velocitySetpointDesired;
}
// Update velocity state called per dT. Also update current
// desired velocity
void PIDControlDown::UpdateVelocityState(float pv)
{
mVelocityState = pv;
if (mCallback) {
// The FSM controls the actual descent velocity and introduces step changes as required
float velocitySetpointDesired = mCallback->BoundVelocityDown(mVelocitySetpointTarget);
// RateLimit(velocitySetpointDesired, mVelocitySetpointCurrent, 2.0f );
mVelocitySetpointCurrent = velocitySetpointDesired;
} else {
mVelocitySetpointCurrent = mVelocitySetpointTarget;
}
}
float PIDControlDown::GetVelocityDesired(void)
{
return mVelocitySetpointCurrent;
}
float PIDControlDown::GetDownCommand(void)
{
PIDStatusData pidStatus;
float ulow = mMinThrust;
float uhigh = mMaxThrust;
if (mCallback) {
mCallback->BoundThrust(ulow, uhigh);
}
float downCommand = pid2_apply(&PID, mVelocitySetpointCurrent, mVelocityState, ulow, uhigh);
pidStatus.setpoint = mVelocitySetpointCurrent;
pidStatus.actual = mVelocityState;
pidStatus.error = mVelocitySetpointCurrent - mVelocityState;
pidStatus.setpoint = mVelocitySetpointCurrent;
pidStatus.ulow = ulow;
pidStatus.uhigh = uhigh;
pidStatus.command = downCommand;
pidStatus.P = PID.P;
pidStatus.I = PID.I;
pidStatus.D = PID.D;
PIDStatusSet(&pidStatus);
mDownCommand = downCommand;
return mDownCommand;
}

108
flight/libraries/pid/pidcontroldown.h Normal file
View File

@ -0,0 +1,108 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup PathFollower PID Control implementation
* @brief PID Controller for down direction
* @{
*
* @file PIDControlDown.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Executes control loop for down direction
*
* @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 PIDCONTROLDOWN_H
#define PIDCONTROLDOWN_H
extern "C" {
#include <pid.h>
#include <stabilizationdesired.h>
}
#include "pidcontroldowncallback.h"
class PIDControlDown {
public:
PIDControlDown();
~PIDControlDown();
void Initialize(PIDControlDownCallback *callback);
void SetThrustLimits(float min_thrust, float max_thrust);
void Deactivate();
void Activate();
bool IsActive()
{
return mActive;
}
void UpdateParameters(float kp, float ki, float kd, float beta, float dT, float velocityMax);
void UpdateNeutralThrust(float neutral);
void UpdateVelocitySetpoint(float setpoint);
void RateLimit(float *spDesired, float *spCurrent, float rateLimit);
void UpdateVelocityState(float pv);
float GetVelocityDesired(void);
float GetDownCommand(void);
void UpdatePositionalParameters(float kp);
void UpdatePositionState(float pvDown);
void UpdatePositionSetpoint(float setpointDown);
void ControlPosition();
void ControlPositionWithPath(struct path_status *progress);
void UpdateBrakeVelocity(float startingVelocity, float dT, float brakeRate, float currentVelocity, float *updatedVelocity);
void UpdateVelocityStateWithBrake(float pvDown, float path_time, float brakeRate);
void DisableNeutralThrustCalc()
{
mAllowNeutralThrustCalc = false;
}
void EnableNeutralThrustCalc()
{
mAllowNeutralThrustCalc = true;
}
private:
void setup_neutralThrustCalc();
void run_neutralThrustCalc();
struct pid2 PID;
float deltaTime;
float mVelocitySetpointTarget;
float mVelocitySetpointCurrent;
float mVelocityState;
float mDownCommand;
PIDControlDownCallback *mCallback;
float mNeutral;
float mVelocityMax;
struct pid PIDpos;
float mPositionSetpointTarget;
float mPositionState;
float mMinThrust;
float mMaxThrust;
struct NeutralThrustEstimation {
uint32_t count;
float sum;
float average;
float correction;
float min;
float max;
bool start_sampling;
bool have_correction;
};
struct NeutralThrustEstimation neutralThrustEst;
bool mActive;
bool mAllowNeutralThrustCalc;
};
#endif // PIDCONTROLDOWN_H

44
flight/targets/boards/coptercontrol/firmware/inc/pios_config_posix.h → flight/libraries/pid/pidcontroldowncallback.h
View File

@ -1,10 +1,15 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup PID Library
* @brief Thrust control callback pure virtual
* @{
*
* @file pidcontroldowncallback.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Interface class for PathFollower FSMs
*
* @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.
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
@ -23,26 +28,15 @@
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef PIDCONTROLDOWNCALLBACK_H
#define PIDCONTROLDOWNCALLBACK_H
class PIDControlDownCallback {
public:
// PIDControlDownCalback() {};
virtual void BoundThrust(__attribute__((unused)) float &ulow, __attribute__((unused)) float &uhigh) = 0;
virtual float BoundVelocityDown(float velocity) = 0;
// virtual ~PIDControlDownCalback() = 0;
};
#ifndef PIOS_CONFIG_POSIX_H
#define PIOS_CONFIG_POSIX_H
/* Enable/Disable PiOS Modules */
#define PIOS_INCLUDE_SYS
#define PIOS_INCLUDE_DELAY
#define PIOS_INCLUDE_LED
#define PIOS_INCLUDE_FREERTOS
#define PIOS_INCLUDE_CALLBACKSCHEDULER
#define PIOS_INCLUDE_TASK_MONITOR
#define PIOS_INCLUDE_COM
#define PIOS_INCLUDE_UDP
#define PIOS_INCLUDE_SERVO
/* Defaults for Logging */
#define LOG_FILENAME "PIOS.LOG"
#define STARTUP_LOG_ENABLED 1
#endif /* PIOS_CONFIG_POSIX_H */
#endif // PIDCONTROLDOWNCALLBACK_H

383
flight/libraries/plans.c
View File

@ -40,13 +40,41 @@
#include <manualcontrolcommand.h>
#include <attitudestate.h>
#include <vtolpathfollowersettings.h>
#include <stabilizationbank.h>
#include <stabilizationdesired.h>
#include <sin_lookup.h>
#include <statusvtolautotakeoff.h>
#define UPDATE_EXPECTED 0.02f
#define UPDATE_MIN 1.0e-6f
#define UPDATE_MAX 1.0f
#define UPDATE_ALPHA 1.0e-2f
static float applyExpo(float value, float expo);
static float applyExpo(float value, float expo)
{
// note: fastPow makes a small error, therefore result needs to be bound
float exp = boundf(fastPow(1.00695f, expo), 0.5f, 2.0f);
// magic number scales expo
// so that
// expo=100 yields value**10
// expo=0 yields value**1
// expo=-100 yields value**(1/10)
// (pow(2.0,1/100)~=1.00695)
if (value > 0.0f) {
return boundf(fastPow(value, exp), 0.0f, 1.0f);
} else if (value < -0.0f) {
return boundf(-fastPow(-value, exp), -1.0f, 0.0f);
} else {
return 0.0f;
}
}
/**
* @brief initialize UAVOs and structs used by this library
*/
@ -61,6 +89,8 @@ void plan_initialize()
ManualControlCommandInitialize();
VelocityStateInitialize();
VtolPathFollowerSettingsInitialize();
StabilizationBankInitialize();
StabilizationDesiredInitialize();
}
/**
@ -85,7 +115,7 @@ void plan_setup_positionHold()
pathDesired.Start.Down = positionState.Down;
pathDesired.StartingVelocity = 0.0f;
pathDesired.EndingVelocity = 0.0f;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
pathDesired.Mode = PATHDESIRED_MODE_GOTOENDPOINT;
PathDesiredSet(&pathDesired);
}
@ -126,49 +156,210 @@ void plan_setup_returnToBase()
pathDesired.StartingVelocity = 0.0f;
pathDesired.EndingVelocity = 0.0f;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
FlightModeSettingsReturnToBaseNextCommandOptions ReturnToBaseNextCommand;
FlightModeSettingsReturnToBaseNextCommandGet(&ReturnToBaseNextCommand);
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_NEXTCOMMAND] = (float)ReturnToBaseNextCommand;
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_UNUSED1] = 0.0f;
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_UNUSED2] = 0.0f;
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_UNUSED3] = 0.0f;
pathDesired.Mode = PATHDESIRED_MODE_GOTOENDPOINT;
PathDesiredSet(&pathDesired);
}
static PiOSDeltatimeConfig landdT;
// Vtol AutoTakeoff invocation from flight mode requires the following sequence:
// 1. Arming must be done whilst in the AutoTakeOff flight mode
// 2. If the AutoTakeoff flight mode is selected and already armed, requires disarming first
// 3. Wait for armed state
// 4. Once the user increases the throttle position to above 50%, then and only then initiate auto-takeoff.
// 5. Whilst the throttle is < 50% before takeoff, all stick inputs are being ignored.
// 6. If during the autotakeoff sequence, at any stage, if the throttle stick position reduces to less than 10%, landing is initiated.
static StatusVtolAutoTakeoffControlStateOptions autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED;
#define AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MIN 2.0f
#define AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MAX 50.0f
static void plan_setup_AutoTakeoff_helper(PathDesiredData *pathDesired)
{
PositionStateData positionState;
PositionStateGet(&positionState);
float velocity_down;
float autotakeoff_height;
FlightModeSettingsAutoTakeOffVelocityGet(&velocity_down);
FlightModeSettingsAutoTakeOffHeightGet(&autotakeoff_height);
autotakeoff_height = fabsf(autotakeoff_height);
if (autotakeoff_height < AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MIN) {
autotakeoff_height = AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MIN;
} else if (autotakeoff_height > AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MAX) {
autotakeoff_height = AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MAX;
}
pathDesired->Start.North = positionState.North;
pathDesired->Start.East = positionState.East;
pathDesired->Start.Down = positionState.Down;
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_NORTH] = 0.0f;
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_EAST] = 0.0f;
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_DOWN] = -velocity_down;
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_CONTROLSTATE] = (float)autotakeoffState;
pathDesired->End.North = positionState.North;
pathDesired->End.East = positionState.East;
pathDesired->End.Down = positionState.Down - autotakeoff_height;
pathDesired->StartingVelocity = 0.0f;
pathDesired->EndingVelocity = 0.0f;
pathDesired->Mode = PATHDESIRED_MODE_AUTOTAKEOFF;
}
#define AUTOTAKEOFF_INFLIGHT_THROTTLE_CHECK_LIMIT 0.2f
void plan_setup_AutoTakeoff()
{
autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED;
// We only allow takeoff if the state transition of disarmed to armed occurs
// whilst in the autotake flight mode
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
StabilizationDesiredData stabiDesired;
StabilizationDesiredGet(&stabiDesired);
// Are we inflight?
if (flightStatus.Armed && stabiDesired.Thrust > AUTOTAKEOFF_INFLIGHT_THROTTLE_CHECK_LIMIT) {
// ok assume already in flight and just enter position hold
// if we are not actually inflight this will just be a violent autotakeoff
autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_POSITIONHOLD;
plan_setup_positionHold();
} else {
if (flightStatus.Armed) {
autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_REQUIREUNARMEDFIRST;
// Note that if this mode was invoked unintentionally whilst in flight, effectively
// all inputs get ignored and the vtol continues to fly to its previous
// stabi command.
}
PathDesiredData pathDesired;
plan_setup_AutoTakeoff_helper(&pathDesired);
PathDesiredSet(&pathDesired);
}
}
#define AUTOTAKEOFF_THROTTLE_LIMIT_TO_ALLOW_TAKEOFF_START 0.3f
#define AUTOTAKEOFF_THROTTLE_ABORT_LIMIT 0.1f
void plan_run_AutoTakeoff()
{
StatusVtolAutoTakeoffControlStateOptions priorState = autotakeoffState;
switch (autotakeoffState) {
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_REQUIREUNARMEDFIRST:
{
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if (!flightStatus.Armed) {
autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE;
}
}
break;
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED:
{
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if (flightStatus.Armed) {
autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE;
}
}
break;
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE:
{
ManualControlCommandData cmd;
ManualControlCommandGet(&cmd);
if (cmd.Throttle > AUTOTAKEOFF_THROTTLE_LIMIT_TO_ALLOW_TAKEOFF_START) {
autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_INITIATE;
}
}
break;
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_INITIATE:
{
ManualControlCommandData cmd;
ManualControlCommandGet(&cmd);
if (cmd.Throttle < AUTOTAKEOFF_THROTTLE_ABORT_LIMIT) {
autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_ABORT;
plan_setup_land();
}
}
break;
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_ABORT:
{
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if (!flightStatus.Armed) {
autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED;
}
}
break;
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_POSITIONHOLD:
// nothing to do. land has been requested. stay here for forever until mode change.
default:
break;
}
if (autotakeoffState != STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_ABORT &&
autotakeoffState != STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_POSITIONHOLD) {
if (priorState != autotakeoffState) {
PathDesiredData pathDesired;
plan_setup_AutoTakeoff_helper(&pathDesired);
PathDesiredSet(&pathDesired);
}
}
}
static void plan_setup_land_helper(PathDesiredData *pathDesired)
{
PositionStateData positionState;
PositionStateGet(&positionState);
float velocity_down;
FlightModeSettingsLandingVelocityGet(&velocity_down);
pathDesired->Start.North = positionState.North;
pathDesired->Start.East = positionState.East;
pathDesired->Start.Down = positionState.Down;
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_NORTH] = 0.0f;
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_EAST] = 0.0f;
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_DOWN] = velocity_down;
pathDesired->End.North = positionState.North;
pathDesired->End.East = positionState.East;
pathDesired->End.Down = positionState.Down;
pathDesired->StartingVelocity = 0.0f;
pathDesired->EndingVelocity = 0.0f;
pathDesired->Mode = PATHDESIRED_MODE_LAND;
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_OPTIONS] = (float)PATHDESIRED_MODEPARAMETER_LAND_OPTION_HORIZONTAL_PH;
}
void plan_setup_land()
{
float descendspeed;
plan_setup_positionHold();
FlightModeSettingsLandingVelocityGet(&descendspeed);
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
pathDesired.StartingVelocity = descendspeed;
pathDesired.EndingVelocity = descendspeed;
plan_setup_land_helper(&pathDesired);
PathDesiredSet(&pathDesired);
PIOS_DELTATIME_Init(&landdT, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
}
/**
* @brief execute land
*/
void plan_run_land()
static void plan_setup_land_from_velocityroam()
{
float downPos, descendspeed;
PathDesiredEndData pathDesiredEnd;
PositionStateDownGet(&downPos); // current down position
PathDesiredEndGet(&pathDesiredEnd); // desired position
PathDesiredEndingVelocityGet(&descendspeed);
// desired position is updated to match the desired descend speed but don't run ahead
// too far if the current position can't keep up. This normaly means we have landed.
if (pathDesiredEnd.Down - downPos < 10) {
pathDesiredEnd.Down += descendspeed * PIOS_DELTATIME_GetAverageSeconds(&landdT);
plan_setup_land();
FlightStatusAssistedControlStateOptions assistedControlFlightMode;
assistedControlFlightMode = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD;
FlightStatusAssistedControlStateSet(&assistedControlFlightMode);
}
PathDesiredEndSet(&pathDesiredEnd);
}
/**
* @brief positionvario functionality
*/
@ -197,6 +388,14 @@ void plan_setup_PositionRoam()
plan_setup_PositionVario();
}
void plan_setup_VelocityRoam()
{
vario_control_lowpass[0] = 0.0f;
vario_control_lowpass[1] = 0.0f;
vario_control_lowpass[2] = 0.0f;
AttitudeStateYawGet(&vario_course);
}
void plan_setup_HomeLeash()
{
plan_setup_PositionVario();
@ -340,6 +539,9 @@ static void plan_run_PositionVario(vario_type type)
pathDesired.Start.North = pathDesired.End.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter
pathDesired.Start.East = pathDesired.End.East;
pathDesired.Start.Down = pathDesired.End.Down;
// set mode explicitly
PathDesiredSet(&pathDesired);
}
} else {
@ -381,6 +583,106 @@ static void plan_run_PositionVario(vario_type type)
}
}
void plan_run_VelocityRoam()
{
// float alpha;
PathDesiredData pathDesired;
FlightStatusAssistedControlStateOptions assistedControlFlightMode;
FlightStatusFlightModeOptions flightMode;
PathDesiredGet(&pathDesired);
FlightModeSettingsPositionHoldOffsetData offset;
FlightModeSettingsPositionHoldOffsetGet(&offset);
FlightStatusAssistedControlStateGet(&assistedControlFlightMode);
FlightStatusFlightModeGet(&flightMode);
StabilizationBankData stabSettings;
StabilizationBankGet(&stabSettings);
ManualControlCommandData cmd;
ManualControlCommandGet(&cmd);
cmd.Roll = applyExpo(cmd.Roll, stabSettings.StickExpo.Roll);
cmd.Pitch = applyExpo(cmd.Pitch, stabSettings.StickExpo.Pitch);
cmd.Yaw = applyExpo(cmd.Yaw, stabSettings.StickExpo.Yaw);
bool flagRollPitchHasInput = (fabsf(cmd.Roll) > 0.0f || fabsf(cmd.Pitch) > 0.0f);
if (!flagRollPitchHasInput) {
// no movement desired, re-enter positionHold at current start-position
if (assistedControlFlightMode == FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY) {
// initiate braking and change assisted control flight mode to braking
if (flightMode == FLIGHTSTATUS_FLIGHTMODE_LAND) {
// avoid brake then hold sequence to continue descent.
plan_setup_land_from_velocityroam();
} else {
plan_setup_assistedcontrol();
}
}
// otherwise nothing to do in braking/hold modes
} else {
PositionStateData positionState;
PositionStateGet(&positionState);
// Revert assist control state to primary, which in this case implies
// we are in roaming state (a GPS vector assisted velocity roam)
assistedControlFlightMode = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY;
// Calculate desired velocity in each direction
float angle;
AttitudeStateYawGet(&angle);
angle = DEG2RAD(angle);
float cos_angle = cosf(angle);
float sine_angle = sinf(angle);
float rotated[2] = {
-cmd.Pitch * cos_angle - cmd.Roll * sine_angle,
-cmd.Pitch * sine_angle + cmd.Roll * cos_angle
};
// flip pitch to have pitch down (away) point north
float horizontalVelMax;
float verticalVelMax;
VtolPathFollowerSettingsHorizontalVelMaxGet(&horizontalVelMax);
VtolPathFollowerSettingsVerticalVelMaxGet(&verticalVelMax);
float velocity_north = rotated[0] * horizontalVelMax;
float velocity_east = rotated[1] * horizontalVelMax;
float velocity_down = 0.0f;
if (flightMode == FLIGHTSTATUS_FLIGHTMODE_LAND) {
FlightModeSettingsLandingVelocityGet(&velocity_down);
}
float velocity = velocity_north * velocity_north + velocity_east * velocity_east;
velocity = sqrtf(velocity);
// if one stick input (pitch or roll) should we use fly by vector? set arbitrary distance of say 20m after which we
// expect new stick input
// if two stick input pilot is fighting wind manually and we use fly by velocity
// in reality setting velocity desired to zero will fight wind anyway.
pathDesired.Start.North = positionState.North;
pathDesired.Start.East = positionState.East;
pathDesired.Start.Down = positionState.Down;
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_NORTH] = velocity_north;
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_EAST] = velocity_east;
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_DOWN] = velocity_down;
pathDesired.End.North = positionState.North;
pathDesired.End.East = positionState.East;
pathDesired.End.Down = positionState.Down;
pathDesired.StartingVelocity = velocity;
pathDesired.EndingVelocity = velocity;
pathDesired.Mode = PATHDESIRED_MODE_VELOCITY;
if (flightMode == FLIGHTSTATUS_FLIGHTMODE_LAND) {
pathDesired.Mode = PATHDESIRED_MODE_LAND;
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_OPTIONS] = (float)PATHDESIRED_MODEPARAMETER_LAND_OPTION_NONE;
} else {
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_UNUSED] = 0.0f;
}
PathDesiredSet(&pathDesired);
FlightStatusAssistedControlStateSet(&assistedControlFlightMode);
}
}
void plan_run_CourseLock()
{
plan_run_PositionVario(COURSE);
@ -437,7 +739,7 @@ void plan_setup_AutoCruise()
pathDesired.Start.Down = pathDesired.End.Down;
pathDesired.StartingVelocity = 0.0f;
pathDesired.EndingVelocity = 0.0f;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
pathDesired.Mode = PATHDESIRED_MODE_GOTOENDPOINT;
PathDesiredSet(&pathDesired);
@ -522,29 +824,15 @@ void plan_run_AutoCruise()
#define ASSISTEDCONTROL_TIMETOSTOP_MAXIMUM 9.0f // seconds
#define ASSISTEDCONTROL_DELAY_TO_BRAKE 1.0f // seconds
#define ASSISTEDCONTROL_TIMEOUT_MULTIPLIER 2.0f // actual deceleration rate can be 50% of desired...timeouts need to cater for this
void plan_setup_assistedcontrol(uint8_t timeout_occurred)
void plan_setup_assistedcontrol()
{
PositionStateData positionState;
PositionStateGet(&positionState);
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
FlightStatusAssistedControlStateOptions assistedControlFlightMode;
FlightStatusAssistedControlStateGet(&assistedControlFlightMode);
if (timeout_occurred) {
pathDesired.End.North = positionState.North;
pathDesired.End.East = positionState.East;
pathDesired.End.Down = positionState.Down;
pathDesired.Start.North = positionState.North;
pathDesired.Start.East = positionState.East;
pathDesired.Start.Down = positionState.Down;
pathDesired.StartingVelocity = 0.0f;
pathDesired.EndingVelocity = 0.0f;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
assistedControlFlightMode = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD;
} else {
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
float brakeRate;
@ -593,7 +881,6 @@ void plan_setup_assistedcontrol(uint8_t timeout_occurred)
pathDesired.Mode = PATHDESIRED_MODE_BRAKE;
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT] = time_to_stopped * ASSISTEDCONTROL_TIMEOUT_MULTIPLIER;
assistedControlFlightMode = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE;
}
FlightStatusAssistedControlStateSet(&assistedControlFlightMode);
PathDesiredSet(&pathDesired);
}

35
flight/libraries/sanitycheck.c
View File

@ -74,7 +74,7 @@ int32_t configuration_check()
// Classify navigation capability
#ifdef REVOLUTION
RevoSettingsInitialize();
uint8_t revoFusion;
RevoSettingsFusionAlgorithmOptions revoFusion;
RevoSettingsFusionAlgorithmGet(&revoFusion);
bool navCapableFusion;
switch (revoFusion) {
@ -104,8 +104,8 @@ int32_t configuration_check()
// For each available flight mode position sanity check the available
// modes
uint8_t num_modes;
uint8_t modes[FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM];
uint8_t FlightModeAssistMap[STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM];
FlightModeSettingsFlightModePositionOptions modes[FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM];
StabilizationSettingsFlightModeAssistMapOptions FlightModeAssistMap[STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM];
ManualControlSettingsFlightModeNumberGet(&num_modes);
StabilizationSettingsFlightModeAssistMapGet(FlightModeAssistMap);
FlightModeSettingsFlightModePositionGet(modes);
@ -118,7 +118,7 @@ int32_t configuration_check()
ADDSEVERITY(navCapableFusion);
}
switch (modes[i]) {
switch ((FlightModeSettingsFlightModePositionOptions)modes[i]) {
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_MANUAL:
ADDSEVERITY(!gps_assisted);
ADDSEVERITY(!multirotor);
@ -143,24 +143,19 @@ int32_t configuration_check()
break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_PATHPLANNER:
{
// Revo supports PathPlanner and that must be OK or we are not sane
// PathPlan alarm is uninitialized if not running
// PathPlan alarm is warning or error if the flightplan is invalid
SystemAlarmsAlarmData alarms;
SystemAlarmsAlarmGet(&alarms);
ADDSEVERITY(alarms.PathPlan == SYSTEMALARMS_ALARM_OK);
ADDSEVERITY(!gps_assisted);
}
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONROAM:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_LAND:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTAKEOFF:
ADDSEVERITY(!coptercontrol);
ADDSEVERITY(navCapableFusion);
break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_COURSELOCK:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONROAM:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_HOMELEASH:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_ABSOLUTEPOSITION:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_LAND:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POI:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOCRUISE:
@ -214,7 +209,7 @@ int32_t configuration_check()
}
}
uint8_t checks_disabled;
FlightModeSettingsDisableSanityChecksOptions checks_disabled;
FlightModeSettingsDisableSanityChecksGet(&checks_disabled);
if (checks_disabled == FLIGHTMODESETTINGS_DISABLESANITYCHECKS_TRUE) {
severity = SYSTEMALARMS_ALARM_WARNING;
@ -242,22 +237,22 @@ static bool check_stabilization_settings(int index, bool multirotor, bool copter
// Get the different axis modes for this switch position
switch (index) {
case 1:
FlightModeSettingsStabilization1SettingsArrayGet(modes);
FlightModeSettingsStabilization1SettingsArrayGet((FlightModeSettingsStabilization1SettingsOptions *)modes);
break;
case 2:
FlightModeSettingsStabilization2SettingsArrayGet(modes);
FlightModeSettingsStabilization2SettingsArrayGet((FlightModeSettingsStabilization2SettingsOptions *)modes);
break;
case 3:
FlightModeSettingsStabilization3SettingsArrayGet(modes);
FlightModeSettingsStabilization3SettingsArrayGet((FlightModeSettingsStabilization3SettingsOptions *)modes);
break;
case 4:
FlightModeSettingsStabilization4SettingsArrayGet(modes);
FlightModeSettingsStabilization4SettingsArrayGet((FlightModeSettingsStabilization4SettingsOptions *)modes);
break;
case 5:
FlightModeSettingsStabilization5SettingsArrayGet(modes);
FlightModeSettingsStabilization5SettingsArrayGet((FlightModeSettingsStabilization5SettingsOptions *)modes);
break;
case 6:
FlightModeSettingsStabilization6SettingsArrayGet(modes);
FlightModeSettingsStabilization6SettingsArrayGet((FlightModeSettingsStabilization6SettingsOptions *)modes);
break;
default:
return false;
@ -331,7 +326,7 @@ static bool check_stabilization_settings(int index, bool multirotor, bool copter
FrameType_t GetCurrentFrameType()
{
uint8_t airframe_type;
SystemSettingsAirframeTypeOptions airframe_type;
SystemSettingsAirframeTypeGet(&airframe_type);
switch ((SystemSettingsAirframeTypeOptions)airframe_type) {

460
flight/modules/Actuator/actuator.c
View File

@ -40,11 +40,17 @@
#include "actuatordesired.h"
#include "actuatorcommand.h"
#include "flightstatus.h"
#include <flightmodesettings.h>
#include "mixersettings.h"
#include "mixerstatus.h"
#include "cameradesired.h"
#include "manualcontrolcommand.h"
#include "taskinfo.h"
#include <systemsettings.h>
#include <sanitycheck.h>
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
#include <vtolpathfollowersettings.h>
#endif
#undef PIOS_INCLUDE_INSTRUMENTATION
#ifdef PIOS_INCLUDE_INSTRUMENTATION
#include <pios_instrumentation.h>
@ -76,27 +82,35 @@ static int8_t counter;
// Private variables
static xQueueHandle queue;
static xTaskHandle taskHandle;
static FrameType_t frameType = FRAME_TYPE_MULTIROTOR;
static SystemSettingsThrustControlOptions thrustType = SYSTEMSETTINGS_THRUSTCONTROL_THROTTLE;
static float lastResult[MAX_MIX_ACTUATORS] = { 0 };
static float filterAccumulator[MAX_MIX_ACTUATORS] = { 0 };
static uint8_t pinsMode[MAX_MIX_ACTUATORS];
// used to inform the actuator thread that actuator update rate is changed
static volatile bool actuator_settings_updated;
static ActuatorSettingsData actuatorSettings;
static bool spinWhileArmed;
// used to inform the actuator thread that mixer settings are changed
static volatile bool mixer_settings_updated;
static MixerSettingsData mixerSettings;
static int mixer_settings_count = 2;
// Private functions
static void actuatorTask(void *parameters);
static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutral);
static void setFailsafe(const ActuatorSettingsData *actuatorSettings, const MixerSettingsData *mixerSettings);
static float MixerCurve(const float throttle, const float *curve, uint8_t elements);
static bool set_channel(uint8_t mixer_channel, uint16_t value, const ActuatorSettingsData *actuatorSettings);
static void actuator_update_rate_if_changed(const ActuatorSettingsData *actuatorSettings, bool force_update);
static int16_t scaleMotor(float value, int16_t max, int16_t min, int16_t neutral, float maxMotor, float minMotor, bool armed, bool AlwaysStabilizeWhenArmed, float throttleDesired);
static void setFailsafe();
static float MixerCurveFullRangeProportional(const float input, const float *curve, uint8_t elements, bool multirotor);
static float MixerCurveFullRangeAbsolute(const float input, const float *curve, uint8_t elements, bool multirotor);
static bool set_channel(uint8_t mixer_channel, uint16_t value);
static void actuator_update_rate_if_changed(bool force_update);
static void MixerSettingsUpdatedCb(UAVObjEvent *ev);
static void ActuatorSettingsUpdatedCb(UAVObjEvent *ev);
static void SettingsUpdatedCb(UAVObjEvent *ev);
float ProcessMixer(const int index, const float curve1, const float curve2,
const MixerSettingsData *mixerSettings, ActuatorDesiredData *desired,
const float period);
ActuatorDesiredData *desired,
const float period, bool multirotor);
// this structure is equivalent to the UAVObjects for one mixer.
typedef struct {
@ -116,6 +130,9 @@ int32_t ActuatorStart()
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_RegisterFlag(PIOS_WDG_ACTUATOR);
#endif
SettingsUpdatedCb(NULL);
MixerSettingsUpdatedCb(NULL);
ActuatorSettingsUpdatedCb(NULL);
return 0;
}
@ -149,6 +166,13 @@ int32_t ActuatorInitialize()
MixerStatusInitialize();
#endif
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
VtolPathFollowerSettingsInitialize();
VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
#endif
SystemSettingsInitialize();
SystemSettingsConnectCallback(&SettingsUpdatedCb);
return 0;
}
MODULE_INITCALL(ActuatorInitialize, ActuatorStart);
@ -177,8 +201,8 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
ActuatorCommandData command;
ActuatorDesiredData desired;
MixerStatusData mixerStatus;
FlightModeSettingsData settings;
FlightStatusData flightStatus;
SystemSettingsThrustControlOptions thrustType;
float throttleDesired;
float collectiveDesired;
@ -186,21 +210,17 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
counter = PIOS_Instrumentation_CreateCounter(0xAC700001);
#endif
/* Read initial values of ActuatorSettings */
ActuatorSettingsData actuatorSettings;
actuator_settings_updated = false;
ActuatorSettingsGet(&actuatorSettings);
/* Read initial values of MixerSettings */
MixerSettingsData mixerSettings;
mixer_settings_updated = false;
MixerSettingsGet(&mixerSettings);
/* Force an initial configuration of the actuator update rates */
actuator_update_rate_if_changed(&actuatorSettings, true);
actuator_update_rate_if_changed(true);
// Go to the neutral (failsafe) values until an ActuatorDesired update is received
setFailsafe(&actuatorSettings, &mixerSettings);
setFailsafe();
// Main task loop
lastSysTime = xTaskGetTickCount();
@ -214,20 +234,10 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
#ifdef PIOS_INCLUDE_INSTRUMENTATION
PIOS_Instrumentation_TimeStart(counter);
#endif
/* Process settings updated events even in timeout case so we always act on the latest settings */
if (actuator_settings_updated) {
actuator_settings_updated = false;
ActuatorSettingsGet(&actuatorSettings);
actuator_update_rate_if_changed(&actuatorSettings, false);
}
if (mixer_settings_updated) {
mixer_settings_updated = false;
MixerSettingsGet(&mixerSettings);
}
if (rc != pdTRUE) {
/* Update of ActuatorDesired timed out. Go to failsafe */
setFailsafe(&actuatorSettings, &mixerSettings);
setFailsafe();
continue;
}
@ -238,9 +248,9 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
dTSeconds = dTMilliseconds * 0.001f;
FlightStatusGet(&flightStatus);
FlightModeSettingsGet(&settings);
ActuatorDesiredGet(&desired);
ActuatorCommandGet(&command);
SystemSettingsThrustControlGet(&thrustType);
// read in throttle and collective -demultiplex thrust
switch (thrustType) {
@ -258,67 +268,94 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
}
bool armed = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED;
bool activeThrottle = (throttleDesired < -0.001f || throttleDesired > 0.001f); // for ground and reversible motors
bool positiveThrottle = (throttleDesired > 0.00f);
bool multirotor = (GetCurrentFrameType() == FRAME_TYPE_MULTIROTOR); // check if frame is a multirotor.
bool alwaysArmed = settings.Arming == FLIGHTMODESETTINGS_ARMING_ALWAYSARMED;
bool AlwaysStabilizeWhenArmed = settings.AlwaysStabilizeWhenArmed == FLIGHTMODESETTINGS_ALWAYSSTABILIZEWHENARMED_TRUE;
if (alwaysArmed) {
AlwaysStabilizeWhenArmed = false; // Do not allow always stabilize when alwaysArmed is active. This is dangerous.
}
// safety settings
if (!armed) {
throttleDesired = 0;
throttleDesired = 0.00f; // this also happens in scaleMotors as a per axis check
}
if (throttleDesired <= 0.00f || !armed) {
if ((frameType == FRAME_TYPE_GROUND && !activeThrottle) || (frameType != FRAME_TYPE_GROUND && throttleDesired <= 0.00f) || !armed) {
// throttleDesired should never be 0 or go below 0.
// force set all other controls to zero if throttle is cut (previously set in Stabilization)
// todo: can probably remove this
if (!(multirotor && AlwaysStabilizeWhenArmed && armed)) { // we don't do this if this is a multirotor AND AlwaysStabilizeWhenArmed is true and the model is armed
if (actuatorSettings.LowThrottleZeroAxis.Roll == ACTUATORSETTINGS_LOWTHROTTLEZEROAXIS_TRUE) {
desired.Roll = 0;
desired.Roll = 0.00f;
}
if (actuatorSettings.LowThrottleZeroAxis.Pitch == ACTUATORSETTINGS_LOWTHROTTLEZEROAXIS_TRUE) {
desired.Pitch = 0;
desired.Pitch = 0.00f;
}
if (actuatorSettings.LowThrottleZeroAxis.Yaw == ACTUATORSETTINGS_LOWTHROTTLEZEROAXIS_TRUE) {
desired.Yaw = 0;
desired.Yaw = 0.00f;
}
}
}
#ifdef DIAG_MIXERSTATUS
MixerStatusGet(&mixerStatus);
#endif
int nMixers = 0;
Mixer_t *mixers = (Mixer_t *)&mixerSettings.Mixer1Type;
for (int ct = 0; ct < MAX_MIX_ACTUATORS; ct++) {
if (mixers[ct].type != MIXERSETTINGS_MIXER1TYPE_DISABLED) {
nMixers++;
}
}
if ((nMixers < 2) && !ActuatorCommandReadOnly()) { // Nothing can fly with less than two mixers.
setFailsafe(&actuatorSettings, &mixerSettings); // So that channels like PWM buzzer keep working
if ((mixer_settings_count < 2) && !ActuatorCommandReadOnly()) { // Nothing can fly with less than two mixers.
setFailsafe();
continue;
}
AlarmsClear(SYSTEMALARMS_ALARM_ACTUATOR);
bool activeThrottle = (throttleDesired < 0.00f || throttleDesired > 0.00f);
bool positiveThrottle = (throttleDesired > 0.00f);
bool spinWhileArmed = actuatorSettings.MotorsSpinWhileArmed == ACTUATORSETTINGS_MOTORSSPINWHILEARMED_TRUE;
float curve1 = 0.0f; // curve 1 is the throttle curve applied to all motors.
float curve2 = 0.0f;
float curve1 = MixerCurve(throttleDesired, mixerSettings.ThrottleCurve1, MIXERSETTINGS_THROTTLECURVE1_NUMELEM);
// Interpolate curve 1 from throttleDesired as input.
// assume reversible motor/mixer initially. We can later reverse this. The difference is simply that -ve throttleDesired values
// map differently
curve1 = MixerCurveFullRangeProportional(throttleDesired, mixerSettings.ThrottleCurve1, MIXERSETTINGS_THROTTLECURVE1_NUMELEM, multirotor);
// The source for the secondary curve is selectable
float curve2 = 0;
AccessoryDesiredData accessory;
switch (mixerSettings.Curve2Source) {
uint8_t curve2Source = mixerSettings.Curve2Source;
switch (curve2Source) {
case MIXERSETTINGS_CURVE2SOURCE_THROTTLE:
curve2 = MixerCurve(throttleDesired, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
// assume reversible motor/mixer initially
curve2 = MixerCurveFullRangeProportional(throttleDesired, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor);
break;
case MIXERSETTINGS_CURVE2SOURCE_ROLL:
curve2 = MixerCurve(desired.Roll, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
// Throttle curve contribution the same for +ve vs -ve roll
if (multirotor) {
curve2 = MixerCurveFullRangeProportional(desired.Roll, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor);
} else {
curve2 = MixerCurveFullRangeAbsolute(desired.Roll, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor);
}
break;
case MIXERSETTINGS_CURVE2SOURCE_PITCH:
curve2 = MixerCurve(desired.Pitch, mixerSettings.ThrottleCurve2,
MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
// Throttle curve contribution the same for +ve vs -ve pitch
if (multirotor) {
curve2 = MixerCurveFullRangeProportional(desired.Pitch, mixerSettings.ThrottleCurve2,
MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor);
} else {
curve2 = MixerCurveFullRangeAbsolute(desired.Pitch, mixerSettings.ThrottleCurve2,
MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor);
}
break;
case MIXERSETTINGS_CURVE2SOURCE_YAW:
curve2 = MixerCurve(desired.Yaw, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
// Throttle curve contribution the same for +ve vs -ve yaw
if (multirotor) {
curve2 = MixerCurveFullRangeProportional(desired.Yaw, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor);
} else {
curve2 = MixerCurveFullRangeAbsolute(desired.Yaw, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor);
}
break;
case MIXERSETTINGS_CURVE2SOURCE_COLLECTIVE:
curve2 = MixerCurve(collectiveDesired, mixerSettings.ThrottleCurve2,
MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
// assume reversible motor/mixer initially
curve2 = MixerCurveFullRangeProportional(collectiveDesired, mixerSettings.ThrottleCurve2,
MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor);
break;
case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY0:
case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY1:
@ -327,14 +364,21 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY4:
case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY5:
if (AccessoryDesiredInstGet(mixerSettings.Curve2Source - MIXERSETTINGS_CURVE2SOURCE_ACCESSORY0, &accessory) == 0) {
curve2 = MixerCurve(accessory.AccessoryVal, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
// Throttle curve contribution the same for +ve vs -ve accessory....maybe not want we want.
curve2 = MixerCurveFullRangeAbsolute(accessory.AccessoryVal, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM, multirotor);
} else {
curve2 = 0;
curve2 = 0.0f;
}
break;
default:
curve2 = 0.0f;
break;
}
float *status = (float *)&mixerStatus; // access status objects as an array of floats
Mixer_t *mixers = (Mixer_t *)&mixerSettings.Mixer1Type;
float maxMotor = -1.0f; // highest motor value. Addition method needs this to be -1.0f, division method needs this to be 1.0f
float minMotor = 1.0f; // lowest motor value Addition method needs this to be 1.0f, division method needs this to be -1.0f
for (int ct = 0; ct < MAX_MIX_ACTUATORS; ct++) {
// During boot all camera actuators should be completely disabled (PWM pulse = 0).
@ -343,20 +387,26 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
// Setting it to 1 by default means "Rescale this channel and enable PWM on its output".
command.Channel[ct] = 1;
if (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_DISABLED) {
uint8_t mixer_type = mixers[ct].type;
if (mixer_type == MIXERSETTINGS_MIXER1TYPE_DISABLED) {
// Set to minimum if disabled. This is not the same as saying PWM pulse = 0 us
status[ct] = -1;
continue;
}
if ((mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) || (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_REVERSABLEMOTOR) || (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_SERVO)) {
status[ct] = ProcessMixer(ct, curve1, curve2, &mixerSettings, &desired, dTSeconds);
} else {
status[ct] = -1;
if ((mixer_type == MIXERSETTINGS_MIXER1TYPE_MOTOR)) {
float nonreversible_curve1 = curve1;
float nonreversible_curve2 = curve2;
if (nonreversible_curve1 < 0.0f) {
nonreversible_curve1 = 0.0f;
}
// Motors have additional protection for when to be on
if (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) {
if (nonreversible_curve2 < 0.0f) {
if (!multirotor) { // allow negative throttle if multirotor. function scaleMotors handles the sanity checks.
nonreversible_curve2 = 0.0f;
}
}
status[ct] = ProcessMixer(ct, nonreversible_curve1, nonreversible_curve2, &desired, dTSeconds, multirotor);
// If not armed or motors aren't meant to spin all the time
if (!armed ||
(!spinWhileArmed && !positiveThrottle)) {
@ -367,19 +417,25 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
// If armed meant to keep spinning,
else if ((spinWhileArmed && !positiveThrottle) ||
(status[ct] < 0)) {
if (!multirotor) {
status[ct] = 0;
// allow throttle values lower than 0 if multirotor.
// Values will be scaled to 0 if they need to be in the scaleMotor function
}
}
} else if (mixer_type == MIXERSETTINGS_MIXER1TYPE_REVERSABLEMOTOR) {
status[ct] = ProcessMixer(ct, curve1, curve2, &desired, dTSeconds, multirotor);
// Reversable Motors are like Motors but go to neutral instead of minimum
if (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_REVERSABLEMOTOR) {
// If not armed or motor is inactive - no "spinwhilearmed" for this engine type
if (!armed || !activeThrottle) {
filterAccumulator[ct] = 0;
lastResult[ct] = 0;
status[ct] = 0; // force neutral throttle
}
}
} else if (mixer_type == MIXERSETTINGS_MIXER1TYPE_SERVO) {
status[ct] = ProcessMixer(ct, curve1, curve2, &desired, dTSeconds, multirotor);
} else {
status[ct] = -1;
// If an accessory channel is selected for direct bypass mode
// In this configuration the accessory channel is scaled and mapped
@ -387,20 +443,20 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
// these also will not be updated in failsafe mode. I'm not sure what
// the correct behavior is since it seems domain specific. I don't love
// this code
if ((mixers[ct].type >= MIXERSETTINGS_MIXER1TYPE_ACCESSORY0) &&
(mixers[ct].type <= MIXERSETTINGS_MIXER1TYPE_ACCESSORY5)) {
if (AccessoryDesiredInstGet(mixers[ct].type - MIXERSETTINGS_MIXER1TYPE_ACCESSORY0, &accessory) == 0) {
if ((mixer_type >= MIXERSETTINGS_MIXER1TYPE_ACCESSORY0) &&
(mixer_type <= MIXERSETTINGS_MIXER1TYPE_ACCESSORY5)) {
if (AccessoryDesiredInstGet(mixer_type - MIXERSETTINGS_MIXER1TYPE_ACCESSORY0, &accessory) == 0) {
status[ct] = accessory.AccessoryVal;
} else {
status[ct] = -1;
}
}
if ((mixers[ct].type >= MIXERSETTINGS_MIXER1TYPE_CAMERAROLLORSERVO1) &&
(mixers[ct].type <= MIXERSETTINGS_MIXER1TYPE_CAMERAYAW)) {
if ((mixer_type >= MIXERSETTINGS_MIXER1TYPE_CAMERAROLLORSERVO1) &&
(mixer_type <= MIXERSETTINGS_MIXER1TYPE_CAMERAYAW)) {
CameraDesiredData cameraDesired;
if (CameraDesiredGet(&cameraDesired) == 0) {
switch (mixers[ct].type) {
switch (mixer_type) {
case MIXERSETTINGS_MIXER1TYPE_CAMERAROLLORSERVO1:
status[ct] = cameraDesired.RollOrServo1;
break;
@ -424,16 +480,40 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
}
}
// If mixer type is motor we need to find which motor has the highest value and which motor has the lowest value.
// For use in function scaleMotor
if (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) {
if (maxMotor < status[ct]) {
maxMotor = status[ct];
}
if (minMotor > status[ct]) {
minMotor = status[ct];
}
}
}
// Set real actuator output values scaling them from mixers. All channels
// will be set except explicitly disabled (which will have PWM pulse = 0).
for (int i = 0; i < MAX_MIX_ACTUATORS; i++) {
if (command.Channel[i]) {
if (mixers[i].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) { // If mixer is for a motor we need to find the highest value of all motors
command.Channel[i] = scaleMotor(status[i],
actuatorSettings.ChannelMax[i],
actuatorSettings.ChannelMin[i],
actuatorSettings.ChannelNeutral[i],
maxMotor,
minMotor,
armed,
AlwaysStabilizeWhenArmed,
throttleDesired);
} else { // else we scale the channel
command.Channel[i] = scaleChannel(status[i],
actuatorSettings.ChannelMax[i],
actuatorSettings.ChannelMin[i],
actuatorSettings.ChannelNeutral[i]);
}
}
}
// Store update time
command.UpdateTime = dTMilliseconds;
@ -454,7 +534,7 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
bool success = true;
for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) {
success &= set_channel(n, command.Channel[n], &actuatorSettings);
success &= set_channel(n, command.Channel[n]);
}
PIOS_Servo_Update();
@ -475,10 +555,10 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
* Process mixing for one actuator
*/
float ProcessMixer(const int index, const float curve1, const float curve2,
const MixerSettingsData *mixerSettings, ActuatorDesiredData *desired, const float period)
ActuatorDesiredData *desired, const float period, bool multirotor)
{
static float lastFilteredResult[MAX_MIX_ACTUATORS];
const Mixer_t *mixers = (Mixer_t *)&mixerSettings->Mixer1Type; // pointer to array of mixers in UAVObjects
const Mixer_t *mixers = (Mixer_t *)&mixerSettings.Mixer1Type; // pointer to array of mixers in UAVObjects
const Mixer_t *mixer = &mixers[index];
float result = ((((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_THROTTLECURVE1]) * curve1) +
@ -489,24 +569,26 @@ float ProcessMixer(const int index, const float curve1, const float curve2,
// note: no feedforward for reversable motors yet for safety reasons
if (mixer->type == MIXERSETTINGS_MIXER1TYPE_MOTOR) {
if (result < 0.0f) { // idle throttle
if (!multirotor) { // we allow negative throttle with a multirotor
if (result < 0.0f) { // zero throttle
result = 0.0f;
}
}
// feed forward
float accumulator = filterAccumulator[index];
accumulator += (result - lastResult[index]) * mixerSettings->FeedForward;
accumulator += (result - lastResult[index]) * mixerSettings.FeedForward;
lastResult[index] = result;
result += accumulator;
if (period > 0.0f) {
if (accumulator > 0.0f) {
float invFilter = period / mixerSettings->AccelTime;
float invFilter = period / mixerSettings.AccelTime;
if (invFilter > 1) {
invFilter = 1;
}
accumulator -= accumulator * invFilter;
} else {
float invFilter = period / mixerSettings->DecelTime;
float invFilter = period / mixerSettings.DecelTime;
if (invFilter > 1) {
invFilter = 1;
}
@ -518,7 +600,7 @@ float ProcessMixer(const int index, const float curve1, const float curve2,
// acceleration limit
float dt = result - lastFilteredResult[index];
float maxDt = mixerSettings->MaxAccel * period;
float maxDt = mixerSettings.MaxAccel * period;
if (dt > maxDt) { // we are accelerating too hard
result = lastFilteredResult[index] + maxDt;
}
@ -530,30 +612,64 @@ float ProcessMixer(const int index, const float curve1, const float curve2,
/**
* Interpolate a throttle curve. Throttle input should be in the range 0 to 1.
* Output is in the range 0 to 1.
* Interpolate a throttle curve
* Full range input (-1 to 1) for yaw, roll, pitch
* Output range (-1 to 1) reversible motor/throttle curve
*
* Input of -1 -> -lookup(1)
* Input of 0 -> lookup(0)
* Input of 1 -> lookup(1)
*/
static float MixerCurve(const float throttle, const float *curve, uint8_t elements)
static float MixerCurveFullRangeProportional(const float input, const float *curve, uint8_t elements, bool multirotor)
{
float scale = throttle * (float)(elements - 1);
float unsigned_value = MixerCurveFullRangeAbsolute(input, curve, elements, multirotor);
if (input < 0.0f) {
return -unsigned_value;
} else {
return unsigned_value;
}
}
/**
* Interpolate a throttle curve
* Full range input (-1 to 1) for yaw, roll, pitch
* Output range (0 to 1) non-reversible motor/throttle curve
*
* Input of -1 -> lookup(1)
* Input of 0 -> lookup(0)
* Input of 1 -> lookup(1)
*/
static float MixerCurveFullRangeAbsolute(const float input, const float *curve, uint8_t elements, bool multirotor)
{
float abs_input = fabsf(input);
float scale = abs_input * (float)(elements - 1);
int idx1 = scale;
scale -= (float)idx1; // remainder
if (curve[0] < -1) {
return throttle;
}
if (idx1 < 0) {
idx1 = 0; // clamp to lowest entry in table
scale = 0;
return abs_input;
}
int idx2 = idx1 + 1;
if (idx2 >= elements) {
idx2 = elements - 1; // clamp to highest entry in table
if (idx1 >= elements) {
if (multirotor) {
// if multirotor frame we can return throttle values higher than 100%.
// Since the we don't have elements in the curve higher than 100% we return
// the last element multiplied by the throttle float
if (input < 2.0f) { // this limits positive throttle to 200% of max value in table (Maybe this is too much allowance)
return curve[idx2] * input;
} else {
return curve[idx2] * 2.0f; // return 200% of max value in table
}
}
idx1 = elements - 1;
}
}
return curve[idx1] * (1.0f - scale) + curve[idx2] * scale;
float unsigned_value = curve[idx1] * (1.0f - scale) + curve[idx2] * scale;
return unsigned_value;
}
@ -590,24 +706,92 @@ static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutr
return valueScaled;
}
/**
* Constrain motor values to keep any one motor value from going too far out of range of another motor
*/
static int16_t scaleMotor(float value, int16_t max, int16_t min, int16_t neutral, float maxMotor, float minMotor, bool armed, bool AlwaysStabilizeWhenArmed, float throttleDesired)
{
int16_t valueScaled;
int16_t maxMotorScaled;
int16_t minMotorScaled;
int16_t diff;
// Scale
valueScaled = (int16_t)(value * ((float)(max - neutral))) + neutral;
maxMotorScaled = (int16_t)(maxMotor * ((float)(max - neutral))) + neutral;
minMotorScaled = (int16_t)(minMotor * ((float)(max - neutral))) + neutral;
if (max > min) {
diff = max - maxMotorScaled; // difference between max allowed and actual max motor
if (diff < 0) { // if the difference is smaller than 0 we add it to the scaled value
valueScaled += diff;
}
diff = neutral - minMotorScaled; // difference between min allowed and actual min motor
if (diff > 0) { // if the difference is larger than 0 we add it to the scaled value
valueScaled += diff;
}
// todo: make this flow easier to understand
if (valueScaled > max) {
valueScaled = max; // clamp to max value only after scaling is done.
}
if ((valueScaled < neutral) && (spinWhileArmed) && AlwaysStabilizeWhenArmed) {
valueScaled = neutral; // clamp to neutral value only after scaling is done.
} else if ((valueScaled < neutral) && (!spinWhileArmed) && AlwaysStabilizeWhenArmed) {
valueScaled = neutral; // clamp to neutral value only after scaling is done. //throttle goes to min if throttledesired is equal to or less than 0 below
} else if (valueScaled < neutral) {
valueScaled = min; // clamp to min value only after scaling is done.
}
} else {
// not sure what to do about reversed polarity right now. Why would anyone do this?
if (valueScaled < max) {
valueScaled = max; // clamp to max value only after scaling is done.
}
if (valueScaled > min) {
valueScaled = min; // clamp to min value only after scaling is done.
}
}
// I've added the bool AlwaysStabilizeWhenArmed to this function. Right now we command the motors at min or a range between neutral and max.
// NEVER should a motor be command at between min and neutral. I don't like the idea of stabilization ever commanding a motor to min, but we give people the option
// This prevents motors startup sync issues causing possible ESC failures.
// safety checks
if (!armed) {
// if not armed return min EVERYTIME!
valueScaled = min;
} else if (!AlwaysStabilizeWhenArmed && (throttleDesired <= 0.0f) && spinWhileArmed) {
// all motors idle is AlwaysStabilizeWhenArmed is false, throttle is less than or equal to neutral and spin while armed
// stabilize when armed?
valueScaled = neutral;
} else if (!spinWhileArmed && (throttleDesired <= 0.0f)) {
// soft disarm
valueScaled = min;
}
return valueScaled;
}
/**
* Set actuator output to the neutral values (failsafe)
*/
static void setFailsafe(const ActuatorSettingsData *actuatorSettings, const MixerSettingsData *mixerSettings)
static void setFailsafe()
{
/* grab only the parts that we are going to use */
int16_t Channel[ACTUATORCOMMAND_CHANNEL_NUMELEM];
ActuatorCommandChannelGet(Channel);
const Mixer_t *mixers = (Mixer_t *)&mixerSettings->Mixer1Type; // pointer to array of mixers in UAVObjects
const Mixer_t *mixers = (Mixer_t *)&mixerSettings.Mixer1Type; // pointer to array of mixers in UAVObjects
// Reset ActuatorCommand to safe values
for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) {
if (mixers[n].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) {
Channel[n] = actuatorSettings->ChannelMin[n];
Channel[n] = actuatorSettings.ChannelMin[n];
} else if (mixers[n].type == MIXERSETTINGS_MIXER1TYPE_SERVO || mixers[n].type == MIXERSETTINGS_MIXER1TYPE_REVERSABLEMOTOR) {
Channel[n] = actuatorSettings->ChannelNeutral[n];
// reversible motors need calibration wizard that allows channel neutral to be the 0 velocity point
Channel[n] = actuatorSettings.ChannelNeutral[n];
} else {
Channel[n] = 0;
}
@ -618,7 +802,7 @@ static void setFailsafe(const ActuatorSettingsData *actuatorSettings, const Mixe
// Update servo outputs
for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) {
set_channel(n, Channel[n], actuatorSettings);
set_channel(n, Channel[n]);
}
// Send the updated command
PIOS_Servo_Update();
@ -713,39 +897,39 @@ static inline bool buzzerState(buzzertype type)
#if defined(ARCH_POSIX) || defined(ARCH_WIN32)
static bool set_channel(uint8_t mixer_channel, uint16_t value, const ActuatorSettingsData *actuatorSettings)
static bool set_channel(uint8_t mixer_channel, uint16_t value)
{
return true;
}
#else
static bool set_channel(uint8_t mixer_channel, uint16_t value, const ActuatorSettingsData *actuatorSettings)
static bool set_channel(uint8_t mixer_channel, uint16_t value)
{
switch (actuatorSettings->ChannelType[mixer_channel]) {
switch (actuatorSettings.ChannelType[mixer_channel]) {
case ACTUATORSETTINGS_CHANNELTYPE_PWMALARMBUZZER:
PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel],
buzzerState(BUZZ_BUZZER) ? actuatorSettings->ChannelMax[mixer_channel] : actuatorSettings->ChannelMin[mixer_channel]);
PIOS_Servo_Set(actuatorSettings.ChannelAddr[mixer_channel],
buzzerState(BUZZ_BUZZER) ? actuatorSettings.ChannelMax[mixer_channel] : actuatorSettings.ChannelMin[mixer_channel]);
return true;
case ACTUATORSETTINGS_CHANNELTYPE_ARMINGLED:
PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel],
buzzerState(BUZZ_ARMING) ? actuatorSettings->ChannelMax[mixer_channel] : actuatorSettings->ChannelMin[mixer_channel]);
PIOS_Servo_Set(actuatorSettings.ChannelAddr[mixer_channel],
buzzerState(BUZZ_ARMING) ? actuatorSettings.ChannelMax[mixer_channel] : actuatorSettings.ChannelMin[mixer_channel]);
return true;
case ACTUATORSETTINGS_CHANNELTYPE_INFOLED:
PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel],
buzzerState(BUZZ_INFO) ? actuatorSettings->ChannelMax[mixer_channel] : actuatorSettings->ChannelMin[mixer_channel]);
PIOS_Servo_Set(actuatorSettings.ChannelAddr[mixer_channel],
buzzerState(BUZZ_INFO) ? actuatorSettings.ChannelMax[mixer_channel] : actuatorSettings.ChannelMin[mixer_channel]);
return true;
case ACTUATORSETTINGS_CHANNELTYPE_PWM:
{
uint8_t mode = pinsMode[actuatorSettings->ChannelAddr[mixer_channel]];
uint8_t mode = pinsMode[actuatorSettings.ChannelAddr[mixer_channel]];
switch (mode) {
case ACTUATORSETTINGS_BANKMODE_ONESHOT125:
// Remap 1000-2000 range to 125-250
PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel], value / ACTUATOR_ONESHOT125_PULSE_SCALE);
PIOS_Servo_Set(actuatorSettings.ChannelAddr[mixer_channel], value / ACTUATOR_ONESHOT125_PULSE_SCALE);
break;
default:
PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel], value);
PIOS_Servo_Set(actuatorSettings.ChannelAddr[mixer_channel], value);
break;
}
return true;
@ -770,12 +954,12 @@ static bool set_channel(uint8_t mixer_channel, uint16_t value, const ActuatorSet
/**
* @brief Update the servo update rate
*/
static void actuator_update_rate_if_changed(const ActuatorSettingsData *actuatorSettings, bool force_update)
static void actuator_update_rate_if_changed(bool force_update)
{
static uint16_t prevBankUpdateFreq[ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM];
static uint8_t prevBankMode[ACTUATORSETTINGS_BANKMODE_NUMELEM];
bool updateMode = force_update || (memcmp(prevBankMode, actuatorSettings->BankMode, sizeof(prevBankMode)) != 0);
bool updateFreq = force_update || (memcmp(prevBankUpdateFreq, actuatorSettings->BankUpdateFreq, sizeof(prevBankUpdateFreq)) != 0);
bool updateMode = force_update || (memcmp(prevBankMode, actuatorSettings.BankMode, sizeof(prevBankMode)) != 0);
bool updateFreq = force_update || (memcmp(prevBankUpdateFreq, actuatorSettings.BankUpdateFreq, sizeof(prevBankUpdateFreq)) != 0);
// check if any setting is changed
if (updateMode || updateFreq) {
@ -784,15 +968,15 @@ static void actuator_update_rate_if_changed(const ActuatorSettingsData *actuator
uint16_t freq[ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM];
uint32_t clock[ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM] = { 0 };
for (uint8_t i = 0; i < ACTUATORSETTINGS_BANKMODE_NUMELEM; i++) {
if (force_update || (actuatorSettings->BankMode[i] != prevBankMode[i])) {
if (force_update || (actuatorSettings.BankMode[i] != prevBankMode[i])) {
PIOS_Servo_SetBankMode(i,
actuatorSettings->BankMode[i] ==
actuatorSettings.BankMode[i] ==
ACTUATORSETTINGS_BANKMODE_PWM ?
PIOS_SERVO_BANK_MODE_PWM :
PIOS_SERVO_BANK_MODE_SINGLE_PULSE
);
}
switch (actuatorSettings->BankMode[i]) {
switch (actuatorSettings.BankMode[i]) {
case ACTUATORSETTINGS_BANKMODE_ONESHOT125:
freq[i] = 100; // Value must be small enough so CCr isn't update until the PIOS_Servo_Update is triggered
clock[i] = ACTUATOR_ONESHOT125_CLOCK; // Setup an 2MHz timer clock
@ -802,37 +986,73 @@ static void actuator_update_rate_if_changed(const ActuatorSettingsData *actuator
clock[i] = ACTUATOR_PWM_CLOCK;
break;
default: // PWM
freq[i] = actuatorSettings->BankUpdateFreq[i];
freq[i] = actuatorSettings.BankUpdateFreq[i];
clock[i] = ACTUATOR_PWM_CLOCK;
break;
}
}
memcpy(prevBankMode,
actuatorSettings->BankMode,
actuatorSettings.BankMode,
sizeof(prevBankMode));
PIOS_Servo_SetHz(freq, clock, ACTUATORSETTINGS_BANKUPDATEFREQ_NUMELEM);
memcpy(prevBankUpdateFreq,
actuatorSettings->BankUpdateFreq,
actuatorSettings.BankUpdateFreq,
sizeof(prevBankUpdateFreq));
// retrieve mode from related bank
for (uint8_t i = 0; i < MAX_MIX_ACTUATORS; i++) {
uint8_t bank = PIOS_Servo_GetPinBank(i);
pinsMode[i] = actuatorSettings->BankMode[bank];
pinsMode[i] = actuatorSettings.BankMode[bank];
}
}
}
static void ActuatorSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
actuator_settings_updated = true;
ActuatorSettingsGet(&actuatorSettings);
spinWhileArmed = actuatorSettings.MotorsSpinWhileArmed == ACTUATORSETTINGS_MOTORSSPINWHILEARMED_TRUE;
if (frameType == FRAME_TYPE_GROUND) {
spinWhileArmed = false;
}
actuator_update_rate_if_changed(false);
}
static void MixerSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
mixer_settings_updated = true;
MixerSettingsGet(&mixerSettings);
mixer_settings_count = 0;
Mixer_t *mixers = (Mixer_t *)&mixerSettings.Mixer1Type;
for (int ct = 0; ct < MAX_MIX_ACTUATORS; ct++) {
if (mixers[ct].type != MIXERSETTINGS_MIXER1TYPE_DISABLED) {
mixer_settings_count++;
}
}
}
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
frameType = GetCurrentFrameType();
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
uint8_t TreatCustomCraftAs;
VtolPathFollowerSettingsTreatCustomCraftAsGet(&TreatCustomCraftAs);
if (frameType == FRAME_TYPE_CUSTOM) {
switch (TreatCustomCraftAs) {
case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_FIXEDWING:
frameType = FRAME_TYPE_FIXED_WING;
break;
case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_VTOL:
frameType = FRAME_TYPE_MULTIROTOR;
break;
case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_GROUND:
frameType = FRAME_TYPE_GROUND;
break;
}
}
#endif
SystemSettingsThrustControlGet(&thrustType);
}
/**

6
flight/modules/Airspeed/airspeed.c
View File

@ -98,7 +98,7 @@ int32_t AirspeedInitialize()
#else
HwSettingsInitialize();
uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
HwSettingsOptionalModulesOptions optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
HwSettingsOptionalModulesArrayGet(optionalModules);
@ -110,7 +110,7 @@ int32_t AirspeedInitialize()
}
#endif
uint8_t adcRouting[HWSETTINGS_ADCROUTING_NUMELEM];
HwSettingsADCRoutingOptions adcRouting[HWSETTINGS_ADCROUTING_NUMELEM];
HwSettingsADCRoutingArrayGet(adcRouting);
// Determine if the barometric airspeed sensor is routed to an ADC pin
@ -169,6 +169,8 @@ static void airspeedTask(__attribute__((unused)) void *parameters)
imu_airspeedInitialize(&airspeedSettings);
}
break;
default:
break;
}
}
switch (airspeedSettings.AirspeedSensorType) {

7
flight/modules/Airspeed/baro_airspeed_mpxv.c
View File

@ -46,7 +46,7 @@
#define CALIBRATION_IDLE_MS 2000 // Time to wait before calibrating, in [ms]
#define CALIBRATION_COUNT_MS 2000 // Time to spend calibrating, in [ms]
#define ANALOG_BARO_AIRSPEED_TIME_CONSTANT_MS 100 // low pass filter
#define ANALOG_BARO_AIRSPEED_TIME_CONSTANT_MS 256 // low pass filter
// Private types
@ -104,9 +104,10 @@ void baro_airspeedGetMPXV(AirspeedSensorData *airspeedSensor, AirspeedSettingsDa
}
}
sensor.zeroPoint = airspeedSettings->ZeroPoint;
sensor.scale = airspeedSettings->Scale;
// Filter CAS
float alpha = airspeedSettings->SamplePeriod / (airspeedSettings->SamplePeriod + ANALOG_BARO_AIRSPEED_TIME_CONSTANT_MS); // Low pass filter.
// Filter CAS : airspeedSettings->SamplePeriod value is 0 - 255 range
float alpha = 1 - (airspeedSettings->SamplePeriod / ANALOG_BARO_AIRSPEED_TIME_CONSTANT_MS); // Low pass filter.
airspeedSensor->CalibratedAirspeed = PIOS_MPXV_CalcAirspeed(&sensor, airspeedSensor->SensorValue) * (alpha) + airspeedSensor->CalibratedAirspeed * (1.0f - alpha);
airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE;

7
flight/modules/Battery/battery.c
View File

@ -138,6 +138,13 @@ static void onTimer(__attribute__((unused)) UAVObjEvent *ev)
const float dT = SAMPLE_PERIOD_MS / 1000.0f;
float energyRemaining;
// Reset ConsumedEnergy counter
if (batterySettings.ResetConsumedEnergy) {
flightBatteryData.ConsumedEnergy = 0;
batterySettings.ResetConsumedEnergy = false;
FlightBatterySettingsSet(&batterySettings);
}
// calculate the battery parameters
if (voltageADCPin >= 0) {
flightBatteryData.Voltage = (PIOS_ADC_PinGetVolt(voltageADCPin) - batterySettings.SensorCalibrations.VoltageZero) * batterySettings.SensorCalibrations.VoltageFactor; // in Volts

17
flight/modules/ComUsbBridge/ComUsbBridge.c
View File

@ -43,6 +43,7 @@
static void com2UsbBridgeTask(void *parameters);
static void usb2ComBridgeTask(void *parameters);
static void updateSettings(UAVObjEvent *ev);
static void usb2ComBridgeSetCtrlLine(uint32_t com_id, uint32_t mask, uint32_t state);
// ****************
// Private constants
@ -98,6 +99,14 @@ static int32_t comUsbBridgeInitialize(void)
usart_port = PIOS_COM_BRIDGE;
vcp_port = PIOS_COM_VCP;
// Register the call back handler for USB control line changes to simply
// pass these onto any handler registered on the USART
if (vcp_port) {
PIOS_COM_RegisterCtrlLineCallback(vcp_port,
usb2ComBridgeSetCtrlLine,
usart_port);
}
#ifdef MODULE_COMUSBBRIDGE_BUILTIN
bridge_enabled = true;
#else
@ -168,6 +177,14 @@ static void usb2ComBridgeTask(__attribute__((unused)) void *parameters)
}
}
/* This routine is registered with the USB driver and will be called in the
* event of a control line state change. It will then call down to the USART
* driver to drive the required control line state.
*/
static void usb2ComBridgeSetCtrlLine(uint32_t com_id, uint32_t mask, uint32_t state)
{
PIOS_COM_SetCtrlLine(com_id, mask, state);
}
static void updateSettings(__attribute__((unused)) UAVObjEvent *ev)
{

296
flight/modules/GPS/GPS.c
View File

@ -50,6 +50,7 @@
#include "UBX.h"
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
#include "inc/ubx_autoconfig.h"
// #include "../../libraries/inc/fifo_buffer.h"
#endif
#include <pios_instrumentation_helper.h>
@ -60,7 +61,7 @@ PERF_DEFINE_COUNTER(counterParse);
// Private functions
static void gpsTask(void *parameters);
static void updateHwSettings();
static void updateHwSettings(UAVObjEvent *ev);
#ifdef PIOS_GPS_SETS_HOMELOCATION
static void setHomeLocation(GPSPositionSensorData *gpsData);
@ -111,6 +112,8 @@ void updateGpsSettings(UAVObjEvent *ev);
// ****************
// Private variables
static GPSSettingsData gpsSettings;
static uint32_t gpsPort;
static bool gpsEnabled = false;
@ -150,15 +153,15 @@ int32_t GPSStart(void)
* \return -1 if initialisation failed
* \return 0 on success
*/
int32_t GPSInitialize(void)
{
gpsPort = PIOS_COM_GPS;
uint8_t gpsProtocol;
HwSettingsInitialize();
#ifdef MODULE_GPS_BUILTIN
gpsEnabled = true;
#else
HwSettingsInitialize();
HwSettingsOptionalModulesData optionalModules;
HwSettingsOptionalModulesGet(&optionalModules);
@ -188,8 +191,12 @@ int32_t GPSInitialize(void)
AuxMagSettingsUpdatedCb(NULL);
AuxMagSettingsConnectCallback(AuxMagSettingsUpdatedCb);
#endif
updateHwSettings();
#else
GPSSettingsInitialize();
// updateHwSettings() uses gpsSettings
GPSSettingsGet(&gpsSettings);
// must updateHwSettings() before updateGpsSettings() so baud rate is set before GPS serial code starts running
updateHwSettings(0);
#else /* if defined(REVOLUTION) */
if (gpsPort && gpsEnabled) {
GPSPositionSensorInitialize();
GPSVelocitySensorInitialize();
@ -200,27 +207,29 @@ int32_t GPSInitialize(void)
#if defined(PIOS_GPS_SETS_HOMELOCATION)
HomeLocationInitialize();
#endif
updateHwSettings();
GPSSettingsInitialize();
// updateHwSettings() uses gpsSettings
GPSSettingsGet(&gpsSettings);
// must updateHwSettings() before updateGpsSettings() so baud rate is set before GPS serial code starts running
updateHwSettings(0);
}
#endif /* if defined(REVOLUTION) */
if (gpsPort && gpsEnabled) {
GPSSettingsInitialize();
GPSSettingsDataProtocolGet(&gpsProtocol);
switch (gpsProtocol) {
#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER)
case GPSSETTINGS_DATAPROTOCOL_NMEA:
gps_rx_buffer = pios_malloc(NMEA_MAX_PACKET_LENGTH);
break;
#endif
case GPSSETTINGS_DATAPROTOCOL_UBX:
#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) && defined(PIOS_INCLUDE_GPS_UBX_PARSER)
gps_rx_buffer = pios_malloc((sizeof(struct UBXPacket) > NMEA_MAX_PACKET_LENGTH) ? sizeof(struct UBXPacket) : NMEA_MAX_PACKET_LENGTH);
#elif defined(PIOS_INCLUDE_GPS_UBX_PARSER)
gps_rx_buffer = pios_malloc(sizeof(struct UBXPacket));
break;
default:
#elif defined(PIOS_INCLUDE_GPS_NMEA_PARSER)
gps_rx_buffer = pios_malloc(NMEA_MAX_PACKET_LENGTH);
#else
gps_rx_buffer = NULL;
}
#endif
#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER)
PIOS_Assert(gps_rx_buffer);
#endif
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
HwSettingsConnectCallback(updateHwSettings); // allow changing baud rate even after startup
GPSSettingsConnectCallback(updateGpsSettings);
#endif
return 0;
@ -245,15 +254,13 @@ static void gpsTask(__attribute__((unused)) void *parameters)
portTickType homelocationSetDelay = 0;
#endif
GPSPositionSensorData gpspositionsensor;
GPSSettingsData gpsSettings;
GPSSettingsGet(&gpsSettings);
timeOfLastUpdateMs = timeNowMs;
timeOfLastCommandMs = timeNowMs;
GPSPositionSensorGet(&gpspositionsensor);
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
// this should be done in the task because it calls out to actually start the GPS serial reads
updateGpsSettings(0);
#endif
@ -270,18 +277,41 @@ static void gpsTask(__attribute__((unused)) void *parameters)
if (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX) {
char *buffer = 0;
uint16_t count = 0;
uint8_t status;
GPSPositionSensorStatusGet(&status);
ubx_autoconfig_run(&buffer, &count, status != GPSPOSITIONSENSOR_STATUS_NOGPS);
ubx_autoconfig_run(&buffer, &count);
// Something to send?
if (count) {
// clear ack/nak
ubxLastAck.clsID = 0x00;
ubxLastAck.msgID = 0x00;
ubxLastNak.clsID = 0x00;
ubxLastNak.msgID = 0x00;
PIOS_COM_SendBuffer(gpsPort, (uint8_t *)buffer, count);
}
}
#endif
// This blocks the task until there is something on the buffer
// need to do this whether there is received data or not, or the status (e.g. gcs) is not always correct
int32_t ac_status = ubx_autoconfig_get_status();
static uint8_t lastStatus = GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DISABLED
+ GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_RUNNING
+ GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DONE
+ GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR;
gpspositionsensor.AutoConfigStatus =
ac_status == UBX_AUTOCONFIG_STATUS_DISABLED ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DISABLED :
ac_status == UBX_AUTOCONFIG_STATUS_DONE ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DONE :
ac_status == UBX_AUTOCONFIG_STATUS_ERROR ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR :
GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_RUNNING;
if (gpspositionsensor.AutoConfigStatus != lastStatus) {
GPSPositionSensorAutoConfigStatusSet(&gpspositionsensor.AutoConfigStatus);
lastStatus = gpspositionsensor.AutoConfigStatus;
}
#endif /* if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) */
// This blocks the task until there is something on the buffer (or 100ms? passes)
uint16_t cnt;
while ((cnt = PIOS_COM_ReceiveBuffer(gpsPort, c, GPS_READ_BUFFER, xDelay)) > 0) {
cnt = PIOS_COM_ReceiveBuffer(gpsPort, c, GPS_READ_BUFFER, xDelay);
if (cnt > 0) {
PERF_TIMED_SECTION_START(counterParse);
PERF_TRACK_VALUE(counterBytesIn, cnt);
PERF_MEASURE_PERIOD(counterRate);
@ -294,23 +324,7 @@ static void gpsTask(__attribute__((unused)) void *parameters)
#endif
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
case GPSSETTINGS_DATAPROTOCOL_UBX:
{
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
int32_t ac_status = ubx_autoconfig_get_status();
static uint8_t lastStatus = 0;
gpspositionsensor.AutoConfigStatus =
ac_status == UBX_AUTOCONFIG_STATUS_DISABLED ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DISABLED :
ac_status == UBX_AUTOCONFIG_STATUS_DONE ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DONE :
ac_status == UBX_AUTOCONFIG_STATUS_ERROR ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR :
GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_RUNNING;
if (gpspositionsensor.AutoConfigStatus != lastStatus) {
GPSPositionSensorAutoConfigStatusSet(&gpspositionsensor.AutoConfigStatus);
lastStatus = gpspositionsensor.AutoConfigStatus;
}
#endif
res = parse_ubx_stream(c, cnt, gps_rx_buffer, &gpspositionsensor, &gpsRxStats);
}
break;
#endif
default:
@ -332,7 +346,7 @@ static void gpsTask(__attribute__((unused)) void *parameters)
(gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX && gpspositionsensor.AutoConfigStatus == GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR)) {
// we have not received any valid GPS sentences for a while.
// either the GPS is not plugged in or a hardware problem or the GPS has locked up.
uint8_t status = GPSPOSITIONSENSOR_STATUS_NOGPS;
GPSPositionSensorStatusOptions status = GPSPOSITIONSENSOR_STATUS_NOGPS;
GPSPositionSensorStatusSet(&status);
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR);
} else {
@ -429,14 +443,98 @@ static void setHomeLocation(GPSPositionSensorData *gpsData)
* like protocol, etc. Thus the HwSettings object which contains the
* GPS port speed is used for now.
*/
static void updateHwSettings()
{
if (gpsPort) {
// Retrieve settings
uint8_t speed;
HwSettingsGPSSpeedGet(&speed);
// Set port speed
// must updateHwSettings() before updateGpsSettings() so baud rate is set before GPS serial code starts running
static void updateHwSettings(UAVObjEvent __attribute__((unused)) *ev)
{
// with these changes, no one (not even OP board makers) should ever need u-center (the complex UBlox configuration program)
// no booting of Revo should be required during any setup or testing, just Send the settings you want to play with
// with autoconfig enabled, just change the baud rate in HwSettings and it will change the GPS internal baud and then the Revo port
// with autoconfig disabled, it will only change the Revo port, so you can try to find the current GPS baud rate if you don't know it
// GPSPositionSensor.SensorType and .UbxAutoConfigStatus now give correct values at all times, so use .SensorType to prove it is
// connected, even to an incorrectly configured (e.g. factory default) GPS
// Use cases:
// - the user can plug in a default GPS, use autoconfig-store once and then always use autoconfig-disabled
// - the user can plug in a default GPS that can't store settings (defaults to 9600 baud) and always use autoconfig-nostore
// - - this is one reason for coding this: cheap eBay GPS's that loose their settings sometimes
// - the user can plug in a default GPS that _can_ store settings and always use autoconfig-nostore with that too
// - - if settings change in newer releases of OP, this will make sure to use the correct settings with whatever code you are running
// - the user can plug in a correctly configured GPS and use autoconfig-disabled
// - the user can recover an old or incorrectly configured GPS (internal GPS baud rate or other GPS settings wrong)
// - - disable UBX GPS autoconfig
// - - set HwSettings GPS baud rate to the current GPS internal baud rate to get it to connect at current (especially non-9600) baud rate
// - - - you can tell the baud rate is correct when GPSPositionSensor.SensorType is known (not "Unknown") (e.g. UBX6)
// - - - the SensorType and AutoConfigStatus may fail at 9600 and will fail at 4800 or lower if the GPS is configured to send OP data
// - - - (way more than default) at 4800 baud or slower
// - - enable autoconfig-nostore GPSSettings to set correct messages and enable further magic baud rate settings
// - - change the baud rate to what you want in HwSettings (it will change the baud rate in the GPS and then in the Revo port)
// - - wait for the baud rate change to complete, GPSPositionSensor.AutoConfigStatus will say "DONE"
// - - enable autoconfig-store and it will save the new baud rate and the correct message configuration
// - - wait for the store to complete, GPSPositionSensor.AutoConfigStatus will say "DONE"
// - - for the new baud rate, the user should either choose 9600 for use with autoconfig-nostore or can choose 57600 for use with autoconfig-disabled
// - the user (Dave :)) can configure a whole bunch of default GPS's with no intervention by using autoconfig-store as a saved Revo setting
// - - just plug the default (9600 baud) GPS in to an unpowered Revo, power the Revo/GPS through the servo connector, wait some time, unplug
// - - or with this code, OP could and even should just ship GPS's with default settings
// if we add an "initial baud rate" instead of assuming 9600 at boot up for autoconfig-nostore/store
// - the user can use the same GPS with both an older release of OP (e.g. GPS settings at 38400) and the current release (autoconfig-nostore)
// - the 57600 could be fixed instead of the 9600 as part of autoconfig-store/nostore and the HwSettings.GPSSpeed be the initial baud rate?
// About using UBlox GPS's with default settings (9600 baud and NEMA data):
// - the default uBlox settings (different than OP settings) are NEMA and 9600 baud
// - - and that is OK and you use autoconfig-nostore
// - - I personally feel that this is the best way to set it up because if OP dev changes GPS settings,
// - - you get them automatically changed to match whatever version of OP code you are running
// - - but 9600 is only OK for this autoconfig startup
// - by my testing, the 9600 initial to 57600 final baud startup takes:
// - - 0:10 if the GPS has been configured to send OP data at 9600
// - - 0:06 if the GPS has default data settings (NEMA) at 9600
// - - reminder that even 0:10 isn't that bad. You need to wait for the GPS to acquire satellites,
// Some things you want to think about if you want to play around with this:
// - don't play with low baud rates, with OP data settings (lots of data) it can take a long time to auto-configure
// - - at 2400 it could take a long time to autoconfig or error out
// - - at 9600 or lower the autoconfig is skipped and only the baud rate is changed
// - if you autoconfigure an OP configuration at 9600 baud or lower
// - - it will actually make a factory default configuration (not an OP configuration) at that baud rate
// - remember that there are two baud rates (inside the GPS and the Revo port) and you can get them out of sync
// - - rebooting the Revo from the Ground Control Station (without powering the GPS down too)
// - - can leave the baud rates out of sync if you are using autoconfig
// - - just power off and on both the Revo and the GPS
// - my OP GPS #2 will NOT save 115200 baud or higher, but it will use all bauds, even 230400
// - - so you could use autoconfig.nostore with this high baud rate, but not autoconfig.store (once) followed by autoconfig.disabled
// - - I haven't tested other GPS's in regard to this
// since 9600 baud and lower are not usable, and are best left at NEMA, I could have coded it to do a factory reset
// - if set to 9600 baud (or lower)
if (gpsPort) {
HwSettingsGPSSpeedOptions speed;
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
// use 9600 baud during startup if autoconfig is enabled
// that is the flag that the code should assumes a factory default baud rate
if (ev == NULL && gpsSettings.UbxAutoConfig != GPSSETTINGS_UBXAUTOCONFIG_DISABLED) {
speed = HWSETTINGS_GPSSPEED_9600;
} else
#endif
{
// Retrieve settings
HwSettingsGPSSpeedGet(&speed);
}
// must always set the baud here, even if it looks like it is the same
// e.g. startup sets 9600 here, ubx_autoconfig sets 57600 there, then the user selects a change back to 9600 here
// on startup (ev == NULL) we must set the Revo port baud rate
// after startup (ev != NULL) we must set the Revo port baud rate only if autoconfig is disabled
// always we must set the Revo port baud rate if not using UBX
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
if (ev == NULL || gpsSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_DISABLED || gpsSettings.DataProtocol != GPSSETTINGS_DATAPROTOCOL_UBX)
#endif
{
// Set Revo port speed
switch (speed) {
case HWSETTINGS_GPSSPEED_2400:
PIOS_COM_ChangeBaud(gpsPort, 2400);
@ -463,6 +561,18 @@ static void updateHwSettings()
PIOS_COM_ChangeBaud(gpsPort, 230400);
break;
}
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
// even changing the baud rate will make it re-verify the sensor type
// that way the user can just try some baud rates and it when the sensor type is valid, the baud rate is correct
ubx_reset_sensor_type();
#endif
}
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
else {
// it will never do this during startup because ev == NULL
ubx_autoconfig_set(NULL);
}
#endif
}
}
@ -474,31 +584,26 @@ void AuxMagSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev)
{
uint8_t ubxAutoConfig;
uint8_t ubxDynamicModel;
uint8_t ubxSbasMode;
ubx_autoconfig_settings_t newconfig;
uint8_t ubxSbasSats;
GPSSettingsUbxRateGet(&newconfig.navRate);
GPSSettingsGet(&gpsSettings);
GPSSettingsUbxAutoConfigGet(&ubxAutoConfig);
newconfig.autoconfigEnabled = ubxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_DISABLED ? false : true;
newconfig.storeSettings = ubxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_CONFIGUREANDSTORE;
newconfig.navRate = gpsSettings.UbxRate;
GPSSettingsUbxDynamicModelGet(&ubxDynamicModel);
newconfig.dynamicModel = ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_PORTABLE ? UBX_DYNMODEL_PORTABLE :
ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_STATIONARY ? UBX_DYNMODEL_STATIONARY :
ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_PEDESTRIAN ? UBX_DYNMODEL_PEDESTRIAN :
ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AUTOMOTIVE ? UBX_DYNMODEL_AUTOMOTIVE :
ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_SEA ? UBX_DYNMODEL_SEA :
ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE1G ? UBX_DYNMODEL_AIRBORNE1G :
ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE2G ? UBX_DYNMODEL_AIRBORNE2G :
ubxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE4G ? UBX_DYNMODEL_AIRBORNE4G :
newconfig.autoconfigEnabled = gpsSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_DISABLED ? false : true;
newconfig.storeSettings = gpsSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_CONFIGUREANDSTORE;
newconfig.dynamicModel = gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_PORTABLE ? UBX_DYNMODEL_PORTABLE :
gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_STATIONARY ? UBX_DYNMODEL_STATIONARY :
gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_PEDESTRIAN ? UBX_DYNMODEL_PEDESTRIAN :
gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AUTOMOTIVE ? UBX_DYNMODEL_AUTOMOTIVE :
gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_SEA ? UBX_DYNMODEL_SEA :
gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE1G ? UBX_DYNMODEL_AIRBORNE1G :
gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE2G ? UBX_DYNMODEL_AIRBORNE2G :
gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_AIRBORNE4G ? UBX_DYNMODEL_AIRBORNE4G :
UBX_DYNMODEL_AIRBORNE1G;
GPSSettingsUbxSBASModeGet(&ubxSbasMode);
switch ((GPSSettingsUbxSBASModeOptions)ubxSbasMode) {
switch ((GPSSettingsUbxSBASModeOptions)gpsSettings.UbxSBASMode) {
case GPSSETTINGS_UBXSBASMODE_RANGINGCORRECTION:
case GPSSETTINGS_UBXSBASMODE_CORRECTION:
case GPSSETTINGS_UBXSBASMODE_RANGINGCORRECTIONINTEGRITY:
@ -509,7 +614,7 @@ void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev)
newconfig.SBASCorrection = false;
}
switch ((GPSSettingsUbxSBASModeOptions)ubxSbasMode) {
switch ((GPSSettingsUbxSBASModeOptions)gpsSettings.UbxSBASMode) {
case GPSSETTINGS_UBXSBASMODE_RANGING:
case GPSSETTINGS_UBXSBASMODE_RANGINGCORRECTION:
case GPSSETTINGS_UBXSBASMODE_RANGINGINTEGRITY:
@ -520,7 +625,7 @@ void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev)
newconfig.SBASRanging = false;
}
switch ((GPSSettingsUbxSBASModeOptions)ubxSbasMode) {
switch ((GPSSettingsUbxSBASModeOptions)gpsSettings.UbxSBASMode) {
case GPSSETTINGS_UBXSBASMODE_INTEGRITY:
case GPSSETTINGS_UBXSBASMODE_RANGINGINTEGRITY:
case GPSSETTINGS_UBXSBASMODE_RANGINGCORRECTIONINTEGRITY:
@ -531,17 +636,48 @@ void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev)
newconfig.SBASIntegrity = false;
}
GPSSettingsUbxSBASChannelsUsedGet(&newconfig.SBASChannelsUsed);
newconfig.SBASChannelsUsed = gpsSettings.UbxSBASChannelsUsed;
GPSSettingsUbxSBASSatsGet(&ubxSbasSats);
newconfig.SBASSats = gpsSettings.UbxSBASSats == GPSSETTINGS_UBXSBASSATS_WAAS ? UBX_SBAS_SATS_WAAS :
gpsSettings.UbxSBASSats == GPSSETTINGS_UBXSBASSATS_EGNOS ? UBX_SBAS_SATS_EGNOS :
gpsSettings.UbxSBASSats == GPSSETTINGS_UBXSBASSATS_MSAS ? UBX_SBAS_SATS_MSAS :
gpsSettings.UbxSBASSats == GPSSETTINGS_UBXSBASSATS_GAGAN ? UBX_SBAS_SATS_GAGAN :
gpsSettings.UbxSBASSats == GPSSETTINGS_UBXSBASSATS_SDCM ? UBX_SBAS_SATS_SDCM : UBX_SBAS_SATS_AUTOSCAN;
newconfig.SBASSats = ubxSbasSats == GPSSETTINGS_UBXSBASSATS_WAAS ? UBX_SBAS_SATS_WAAS :
ubxSbasSats == GPSSETTINGS_UBXSBASSATS_EGNOS ? UBX_SBAS_SATS_EGNOS :
ubxSbasSats == GPSSETTINGS_UBXSBASSATS_MSAS ? UBX_SBAS_SATS_MSAS :
ubxSbasSats == GPSSETTINGS_UBXSBASSATS_GAGAN ? UBX_SBAS_SATS_GAGAN :
ubxSbasSats == GPSSETTINGS_UBXSBASSATS_SDCM ? UBX_SBAS_SATS_SDCM : UBX_SBAS_SATS_AUTOSCAN;
switch (gpsSettings.UbxGNSSMode) {
case GPSSETTINGS_UBXGNSSMODE_GPSGLONASS:
newconfig.enableGPS = true;
newconfig.enableGLONASS = true;
newconfig.enableBeiDou = false;
break;
case GPSSETTINGS_UBXGNSSMODE_GLONASS:
newconfig.enableGPS = false;
newconfig.enableGLONASS = true;
newconfig.enableBeiDou = false;
break;
case GPSSETTINGS_UBXGNSSMODE_GPS:
newconfig.enableGPS = true;
newconfig.enableGLONASS = false;
newconfig.enableBeiDou = false;
break;
case GPSSETTINGS_UBXGNSSMODE_GPSBEIDOU:
newconfig.enableGPS = true;
newconfig.enableGLONASS = false;
newconfig.enableBeiDou = true;
break;
case GPSSETTINGS_UBXGNSSMODE_GLONASSBEIDOU:
newconfig.enableGPS = false;
newconfig.enableGLONASS = true;
newconfig.enableBeiDou = true;
break;
default:
newconfig.enableGPS = false;
newconfig.enableGLONASS = false;
newconfig.enableBeiDou = false;
break;
}
ubx_autoconfig_set(newconfig);
ubx_autoconfig_set(&newconfig);
}
#endif /* if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) */
/**

26
flight/modules/GPS/UBX.c
View File

@ -43,7 +43,7 @@
static bool useMag = false;
#endif
static GPSPositionSensorSensorTypeOptions sensorType = GPSPOSITIONSENSOR_SENSORTYPE_UNKNOWN;
GPSPositionSensorSensorTypeOptions sensorType = GPSPOSITIONSENSOR_SENSORTYPE_UNKNOWN;
static bool usePvt = false;
static uint32_t lastPvtTime = 0;
@ -107,7 +107,7 @@ struct UBX_ACK_NAK ubxLastNak;
#define UBX_PVT_TIMEOUT (1000)
// parse incoming character stream for messages in UBX binary format
int parse_ubx_stream(uint8_t *rx, uint8_t len, char *gps_rx_buffer, GPSPositionSensorData *GpsData, struct GPS_RX_STATS *gpsRxStats)
int parse_ubx_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPositionSensorData *GpsData, struct GPS_RX_STATS *gpsRxStats)
{
int ret = PARSER_INCOMPLETE; // message not (yet) complete
enum proto_states {
@ -124,7 +124,7 @@ int parse_ubx_stream(uint8_t *rx, uint8_t len, char *gps_rx_buffer, GPSPositionS
};
uint8_t c;
static enum proto_states proto_state = START;
static uint8_t rx_count = 0;
static uint16_t rx_count = 0;
struct UBXPacket *ubx = (struct UBXPacket *)gps_rx_buffer;
for (int i = 0; i < len; i++) {
@ -413,8 +413,10 @@ static void parse_ubx_nav_svinfo(struct UBXPacket *ubx, __attribute__((unused))
struct UBX_NAV_SVINFO *svinfo = &ubx->payload.nav_svinfo;
svdata.SatsInView = 0;
// First, use slots for SVs actually being received
for (chan = 0; chan < svinfo->numCh; chan++) {
if (svdata.SatsInView < GPSSATELLITES_PRN_NUMELEM) {
if (svdata.SatsInView < GPSSATELLITES_PRN_NUMELEM && svinfo->sv[chan].cno > 0) {
svdata.Azimuth[svdata.SatsInView] = svinfo->sv[chan].azim;
svdata.Elevation[svdata.SatsInView] = svinfo->sv[chan].elev;
svdata.PRN[svdata.SatsInView] = svinfo->sv[chan].svid;
@ -422,6 +424,18 @@ static void parse_ubx_nav_svinfo(struct UBXPacket *ubx, __attribute__((unused))
svdata.SatsInView++;
}
}
// Now try to add the rest
for (chan = 0; chan < svinfo->numCh; chan++) {
if (svdata.SatsInView < GPSSATELLITES_PRN_NUMELEM && 0 == svinfo->sv[chan].cno) {
svdata.Azimuth[svdata.SatsInView] = svinfo->sv[chan].azim;
svdata.Elevation[svdata.SatsInView] = svinfo->sv[chan].elev;
svdata.PRN[svdata.SatsInView] = svinfo->sv[chan].svid;
svdata.SNR[svdata.SatsInView] = svinfo->sv[chan].cno;
svdata.SatsInView++;
}
}
// fill remaining slots (if any)
for (chan = svdata.SatsInView; chan < GPSSATELLITES_PRN_NUMELEM; chan++) {
svdata.Azimuth[chan] = 0;
@ -452,9 +466,11 @@ static void parse_ubx_mon_ver(struct UBXPacket *ubx, __attribute__((unused)) GPS
struct UBX_MON_VER *mon_ver = &ubx->payload.mon_ver;
ubxHwVersion = atoi(mon_ver->hwVersion);
sensorType = (ubxHwVersion >= 80000) ? GPSPOSITIONSENSOR_SENSORTYPE_UBX8 :
((ubxHwVersion >= 70000) ? GPSPOSITIONSENSOR_SENSORTYPE_UBX7 : GPSPOSITIONSENSOR_SENSORTYPE_UBX);
// send sensor type right now because on UBX NEMA we don't get a full set of messages
// and we want to be able to see sensor type even on UBX NEMA GPS's
GPSPositionSensorSensorTypeSet((uint8_t *)&sensorType);
}
static void parse_ubx_op_sys(struct UBXPacket *ubx, __attribute__((unused)) GPSPositionSensorData *GpsPosition)

323
flight/modules/GPS/inc/UBX.h
View File

@ -102,6 +102,8 @@ typedef enum {
UBX_ID_CFG_MSG = 0x01,
UBX_ID_CFG_CFG = 0x09,
UBX_ID_CFG_SBAS = 0x16,
UBX_ID_CFG_GNSS = 0x3E,
UBX_ID_CFG_PRT = 0x00
} ubx_class_cfg_id;
typedef enum {
@ -128,6 +130,39 @@ typedef enum {
UBX_ID_RXM_SFRB = 0x11,
UBX_ID_RXM_SVSI = 0x20,
} ubx_class_rxm_id;
typedef enum {
UBX_GNSS_ID_GPS = 0,
UBX_GNSS_ID_SBAS = 1,
UBX_GNSS_ID_GALILEO = 2,
UBX_GNSS_ID_BEIDOU = 3,
UBX_GNSS_ID_IMES = 4,
UBX_GNSS_ID_QZSS = 5,
UBX_GNSS_ID_GLONASS = 6,
UBX_GNSS_ID_MAX
} ubx_config_gnss_id_t;
// Enumeration options for field UBXDynamicModel
typedef enum {
UBX_DYNMODEL_PORTABLE = 0,
UBX_DYNMODEL_STATIONARY = 2,
UBX_DYNMODEL_PEDESTRIAN = 3,
UBX_DYNMODEL_AUTOMOTIVE = 4,
UBX_DYNMODEL_SEA = 5,
UBX_DYNMODEL_AIRBORNE1G = 6,
UBX_DYNMODEL_AIRBORNE2G = 7,
UBX_DYNMODEL_AIRBORNE4G = 8
} ubx_config_dynamicmodel_t;
typedef enum {
UBX_SBAS_SATS_AUTOSCAN = 0,
UBX_SBAS_SATS_WAAS = 1,
UBX_SBAS_SATS_EGNOS = 2,
UBX_SBAS_SATS_MSAS = 3,
UBX_SBAS_SATS_GAGAN = 4,
UBX_SBAS_SATS_SDCM = 5
} ubx_config_sats_t;
// private structures
// Geodetic Position Solution
@ -139,10 +174,9 @@ struct UBX_NAV_POSLLH {
int32_t hMSL; // Height above mean sea level (mm)
uint32_t hAcc; // Horizontal Accuracy Estimate (mm)
uint32_t vAcc; // Vertical Accuracy Estimate (mm)
};
} __attribute__((packed));
// Receiver Navigation Status
#define STATUS_GPSFIX_NOFIX 0x00
#define STATUS_GPSFIX_DRONLY 0x01
#define STATUS_GPSFIX_2DFIX 0x02
@ -163,7 +197,7 @@ struct UBX_NAV_STATUS {
uint8_t flags2; // Additional navigation output information
uint32_t ttff; // Time to first fix (ms)
uint32_t msss; // Milliseconds since startup/reset (ms)
};
} __attribute__((packed));
// Dilution of precision
struct UBX_NAV_DOP {
@ -175,10 +209,9 @@ struct UBX_NAV_DOP {
uint16_t hDOP; // Horizontal DOP
uint16_t nDOP; // Northing DOP
uint16_t eDOP; // Easting DOP
};
} __attribute__((packed));
// Navigation solution
struct UBX_NAV_SOL {
uint32_t iTOW; // GPS Millisecond Time of Week (ms)
int32_t fTOW; // fractional nanoseconds (ns)
@ -197,41 +230,9 @@ struct UBX_NAV_SOL {
uint8_t reserved1; // Reserved
uint8_t numSV; // Number of SVs used in Nav Solution
uint32_t reserved2; // Reserved
};
// North/East/Down velocity
struct UBX_NAV_VELNED {
uint32_t iTOW; // ms GPS Millisecond Time of Week
int32_t velN; // cm/s NED north velocity
int32_t velE; // cm/s NED east velocity
int32_t velD; // cm/s NED down velocity
uint32_t speed; // cm/s Speed (3-D)
uint32_t gSpeed; // cm/s Ground Speed (2-D)
int32_t heading; // 1e-5 *deg Heading of motion 2-D
uint32_t sAcc; // cm/s Speed Accuracy Estimate
uint32_t cAcc; // 1e-5 *deg Course / Heading Accuracy Estimate
};
// UTC Time Solution
#define TIMEUTC_VALIDTOW (1 << 0)
#define TIMEUTC_VALIDWKN (1 << 1)
#define TIMEUTC_VALIDUTC (1 << 2)
struct UBX_NAV_TIMEUTC {
uint32_t iTOW; // GPS Millisecond Time of Week (ms)
uint32_t tAcc; // Time Accuracy Estimate (ns)
int32_t nano; // Nanoseconds of second
uint16_t year;
uint8_t month;
uint8_t day;
uint8_t hour;
uint8_t min;
uint8_t sec;
uint8_t valid; // Validity Flags
};
} __attribute__((packed));
// PVT Navigation Position Velocity Time Solution
#define PVT_VALID_VALIDDATE 0x01
#define PVT_VALID_VALIDTIME 0x02
#define PVT_VALID_FULLYRESOLVED 0x04
@ -251,7 +252,6 @@ struct UBX_NAV_TIMEUTC {
#define PVT_FLAGS_PSMSTATE_PO_TRACKING (4 << 2)
#define PVT_FLAGS_PSMSTATE_INACTIVE (5 << 2)
// PVT Navigation Position Velocity Time Solution
struct UBX_NAV_PVT {
uint32_t iTOW;
uint16_t year;
@ -285,10 +285,39 @@ struct UBX_NAV_PVT {
uint32_t reserved3;
} __attribute__((packed));
// North/East/Down velocity
struct UBX_NAV_VELNED {
uint32_t iTOW; // ms GPS Millisecond Time of Week
int32_t velN; // cm/s NED north velocity
int32_t velE; // cm/s NED east velocity
int32_t velD; // cm/s NED down velocity
uint32_t speed; // cm/s Speed (3-D)
uint32_t gSpeed; // cm/s Ground Speed (2-D)
int32_t heading; // 1e-5 *deg Heading of motion 2-D
uint32_t sAcc; // cm/s Speed Accuracy Estimate
uint32_t cAcc; // 1e-5 *deg Course / Heading Accuracy Estimate
} __attribute__((packed));
// UTC Time Solution
#define TIMEUTC_VALIDTOW (1 << 0)
#define TIMEUTC_VALIDWKN (1 << 1)
#define TIMEUTC_VALIDUTC (1 << 2)
struct UBX_NAV_TIMEUTC {
uint32_t iTOW; // GPS Millisecond Time of Week (ms)
uint32_t tAcc; // Time Accuracy Estimate (ns)
int32_t nano; // Nanoseconds of second
uint16_t year;
uint8_t month;
uint8_t day;
uint8_t hour;
uint8_t min;
uint8_t sec;
uint8_t valid; // Validity Flags
} __attribute__((packed));
// Space Vehicle (SV) Information
// Single SV information block
#define SVUSED (1 << 0) // This SV is used for navigation
#define DIFFCORR (1 << 1) // Differential correction available
#define ORBITAVAIL (1 << 2) // Orbit information available
@ -307,10 +336,10 @@ struct UBX_NAV_SVINFO_SV {
int8_t elev; // Elevation (integer degrees)
int16_t azim; // Azimuth (integer degrees)
int32_t prRes; // Pseudo range residual (cm)
};
} __attribute__((packed));
// SV information message
#define MAX_SVS 16
#define MAX_SVS 32
struct UBX_NAV_SVINFO {
uint32_t iTOW; // GPS Millisecond Time of Week (ms)
@ -318,19 +347,180 @@ struct UBX_NAV_SVINFO {
uint8_t globalFlags; //
uint16_t reserved2; // Reserved
struct UBX_NAV_SVINFO_SV sv[MAX_SVS]; // Repeated 'numCh' times
};
} __attribute__((packed));
// ACK message class
struct UBX_ACK_ACK {
uint8_t clsID; // ClassID
uint8_t msgID; // MessageID
};
} __attribute__((packed));
struct UBX_ACK_NAK {
uint8_t clsID; // ClassID
uint8_t msgID; // MessageID
};
} __attribute__((packed));
// Communication port information
// default cfg_prt packet at 9600,N,8,1 (from u-center)
// 0000 B5 62 06 00 14 00-01 00
// 0008 00 00 D0 08 00 00 80 25
// 0010 00 00 07 00 03 00 00 00
// 0018 00 00-A2 B5
#define UBX_CFG_PRT_PORTID_DDC 0
#define UBX_CFG_PRT_PORTID_UART1 1
#define UBX_CFG_PRT_PORTID_UART2 2
#define UBX_CFG_PRT_PORTID_USB 3
#define UBX_CFG_PRT_PORTID_SPI 4
#define UBX_CFG_PRT_MODE_DATABITS5 0x00
#define UBX_CFG_PRT_MODE_DATABITS6 0x40
#define UBX_CFG_PRT_MODE_DATABITS7 0x80
#define UBX_CFG_PRT_MODE_DATABITS8 0xC0
#define UBX_CFG_PRT_MODE_EVENPARITY 0x000
#define UBX_CFG_PRT_MODE_ODDPARITY 0x200
#define UBX_CFG_PRT_MODE_NOPARITY 0x800
#define UBX_CFG_PRT_MODE_STOPBITS1_0 0x0000
#define UBX_CFG_PRT_MODE_STOPBITS1_5 0x1000
#define UBX_CFG_PRT_MODE_STOPBITS2_0 0x2000
#define UBX_CFG_PRT_MODE_STOPBITS0_5 0x3000
#define UBX_CFG_PRT_MODE_RESERVED 0x10
#define UBX_CFG_PRT_MODE_DEFAULT (UBX_CFG_PRT_MODE_DATABITS8 | UBX_CFG_PRT_MODE_NOPARITY | UBX_CFG_PRT_MODE_STOPBITS1_0 | UBX_CFG_PRT_MODE_RESERVED)
struct UBX_CFG_PRT {
uint8_t portID; // 1 or 2 for UART ports
uint8_t res0; // reserved
uint16_t res1; // reserved
uint32_t mode; // bit masks for databits, stopbits, parity, and non-zero reserved
uint32_t baudRate; // bits per second, 9600 means 9600
uint16_t inProtoMask; // bit 0 on = UBX, bit 1 on = NEMA
uint16_t outProtoMask; // bit 0 on = UBX, bit 1 on = NEMA
uint16_t flags; // reserved
uint16_t pad; // reserved
} __attribute__((packed));
struct UBX_CFG_MSG {
uint8_t msgClass;
uint8_t msgID;
uint8_t rate;
} __attribute__((packed));
struct UBX_CFG_RATE {
uint16_t measRate;
uint16_t navRate;
uint16_t timeRef;
} __attribute__((packed));
// Mask for "all supported devices": battery backed RAM, Flash, EEPROM, SPI Flash
#define UBX_CFG_CFG_DEVICE_BBR 0x01
#define UBX_CFG_CFG_DEVICE_FLASH 0x02
#define UBX_CFG_CFG_DEVICE_EEPROM 0x04
#define UBX_CFG_CFG_DEVICE_SPIFLASH 0x10
#define UBX_CFG_CFG_DEVICE_ALL (UBX_CFG_CFG_DEVICE_BBR | UBX_CFG_CFG_DEVICE_FLASH | UBX_CFG_CFG_DEVICE_EEPROM | UBX_CFG_CFG_DEVICE_SPIFLASH)
#define UBX_CFG_CFG_SETTINGS_NONE 0x000
#define UBX_CFG_CFG_SETTINGS_IOPORT 0x001
#define UBX_CFG_CFG_SETTINGS_MSGCONF 0x002
#define UBX_CFG_CFG_SETTINGS_INFMSG 0x004
#define UBX_CFG_CFG_SETTINGS_NAVCONF 0x008
#define UBX_CFG_CFG_SETTINGS_TPCONF 0x010
#define UBX_CFG_CFG_SETTINGS_SFDRCONF 0x100
#define UBX_CFG_CFG_SETTINGS_RINVCONF 0x200
#define UBX_CFG_CFG_SETTINGS_ANTCONF 0x400
#define UBX_CFG_CFG_SETTINGS_ALL \
(UBX_CFG_CFG_SETTINGS_IOPORT | \
UBX_CFG_CFG_SETTINGS_MSGCONF | \
UBX_CFG_CFG_SETTINGS_INFMSG | \
UBX_CFG_CFG_SETTINGS_NAVCONF | \
UBX_CFG_CFG_SETTINGS_TPCONF | \
UBX_CFG_CFG_SETTINGS_SFDRCONF | \
UBX_CFG_CFG_SETTINGS_RINVCONF | \
UBX_CFG_CFG_SETTINGS_ANTCONF)
// Sent messages for configuration support
struct UBX_CFG_CFG {
uint32_t clearMask;
uint32_t saveMask;
uint32_t loadMask;
uint8_t deviceMask;
} __attribute__((packed));
#define UBX_CFG_SBAS_MODE_ENABLED 0x01
#define UBX_CFG_SBAS_MODE_TEST 0x02
#define UBX_CFG_SBAS_USAGE_RANGE 0x01
#define UBX_CFG_SBAS_USAGE_DIFFCORR 0x02
#define UBX_CFG_SBAS_USAGE_INTEGRITY 0x04
// SBAS used satellite PNR bitmask (120-151)
// -------------------------------------1---------1---------1---------1
// -------------------------------------5---------4---------3---------2
// ------------------------------------10987654321098765432109876543210
// WAAS 122, 133, 134, 135, 138---------|---------|---------|---------|
#define UBX_CFG_SBAS_SCANMODE1_WAAS 0b00000000000001001110000000000100
// EGNOS 120, 124, 126, 131-------------|---------|---------|---------|
#define UBX_CFG_SBAS_SCANMODE1_EGNOS 0b00000000000000000000100001010001
// MSAS 129, 137------------------------|---------|---------|---------|
#define UBX_CFG_SBAS_SCANMODE1_MSAS 0b00000000000000100000001000000000
// GAGAN 127, 128-----------------------|---------|---------|---------|
#define UBX_CFG_SBAS_SCANMODE1_GAGAN 0b00000000000000000000000110000000
// SDCM 125, 140, 141-------------------|---------|---------|---------|
#define UBX_CFG_SBAS_SCANMODE1_SDCM 0b00000000001100000000000000100000
#define UBX_CFG_SBAS_SCANMODE2 0x00
struct UBX_CFG_SBAS {
uint8_t mode;
uint8_t usage;
uint8_t maxSBAS;
uint8_t scanmode2;
uint32_t scanmode1;
} __attribute__((packed));
#define UBX_CFG_GNSS_FLAGS_ENABLED 0x000001
#define UBX_CFG_GNSS_FLAGS_GPS_L1CA 0x010000
#define UBX_CFG_GNSS_FLAGS_SBAS_L1CA 0x010000
#define UBX_CFG_GNSS_FLAGS_BEIDOU_B1I 0x010000
#define UBX_CFG_GNSS_FLAGS_QZSS_L1CA 0x010000
#define UBX_CFG_GNSS_FLAGS_QZSS_L1SAIF 0x040000
#define UBX_CFG_GNSS_FLAGS_GLONASS_L1OF 0x010000
#define UBX_CFG_GNSS_NUMCH_VER7 22
#define UBX_CFG_GNSS_NUMCH_VER8 32
struct UBX_CFG_GNSS_CFGBLOCK {
uint8_t gnssId;
uint8_t resTrkCh;
uint8_t maxTrkCh;
uint8_t resvd;
uint32_t flags;
} __attribute__((packed));
struct UBX_CFG_GNSS {
uint8_t msgVer;
uint8_t numTrkChHw;
uint8_t numTrkChUse;
uint8_t numConfigBlocks;
struct UBX_CFG_GNSS_CFGBLOCK cfgBlocks[UBX_GNSS_ID_MAX];
} __attribute__((packed));
struct UBX_CFG_NAV5 {
uint16_t mask;
uint8_t dynModel;
uint8_t fixMode;
int32_t fixedAlt;
uint32_t fixedAltVar;
int8_t minElev;
uint8_t drLimit;
uint16_t pDop;
uint16_t tDop;
uint16_t pAcc;
uint16_t tAcc;
uint8_t staticHoldThresh;
uint8_t dgpsTimeOut;
uint8_t cnoThreshNumSVs;
uint8_t cnoThresh;
uint16_t reserved2;
uint32_t reserved3;
uint32_t reserved4;
} __attribute__((packed));
// MON message Class
#define UBX_MON_MAX_EXT 5
@ -340,8 +530,7 @@ struct UBX_MON_VER {
#if UBX_MON_MAX_EXT > 0
char extension[UBX_MON_MAX_EXT][30];
#endif
};
} __attribute__((packed));
// OP custom messages
struct UBX_OP_SYSINFO {
@ -359,7 +548,7 @@ struct UBX_OP_MAG {
int16_t y;
int16_t z;
uint16_t Status;
};
} __attribute__((packed));
typedef union {
uint8_t payload[0];
@ -373,6 +562,7 @@ typedef union {
struct UBX_NAV_PVT nav_pvt;
struct UBX_NAV_TIMEUTC nav_timeutc;
struct UBX_NAV_SVINFO nav_svinfo;
struct UBX_CFG_PRT cfg_prt;
// Ack Class
struct UBX_ACK_ACK ack_ack;
struct UBX_ACK_NAK ack_nak;
@ -389,22 +579,47 @@ struct UBXHeader {
uint16_t len;
uint8_t ck_a;
uint8_t ck_b;
};
} __attribute__((packed));
struct UBXPacket {
struct UBXHeader header;
UBXPayload payload;
};
} __attribute__((packed));
struct UBXSENTHEADER {
uint8_t prolog[2];
uint8_t class;
uint8_t id;
uint16_t len;
} __attribute__((packed));
union UBXSENTPACKET {
uint8_t buffer[0];
struct {
struct UBXSENTHEADER header;
union {
struct UBX_CFG_CFG cfg_cfg;
struct UBX_CFG_MSG cfg_msg;
struct UBX_CFG_NAV5 cfg_nav5;
struct UBX_CFG_PRT cfg_prt;
struct UBX_CFG_RATE cfg_rate;
struct UBX_CFG_SBAS cfg_sbas;
struct UBX_CFG_GNSS cfg_gnss;
} payload;
uint8_t resvd[2]; // added space for checksum bytes
} message;
} __attribute__((packed));
// Used by AutoConfig code
extern int32_t ubxHwVersion;
extern GPSPositionSensorSensorTypeOptions sensorType;
extern struct UBX_ACK_ACK ubxLastAck;
extern struct UBX_ACK_NAK ubxLastNak;
bool checksum_ubx_message(struct UBXPacket *);
uint32_t parse_ubx_message(struct UBXPacket *, GPSPositionSensorData *);
int parse_ubx_stream(uint8_t *rx, uint8_t len, char *, GPSPositionSensorData *, struct GPS_RX_STATS *);
int parse_ubx_stream(uint8_t *rx, uint16_t len, char *, GPSPositionSensorData *, struct GPS_RX_STATS *);
void load_mag_settings();
#endif /* UBX_H */

172
flight/modules/GPS/inc/ubx_autoconfig.h
View File

@ -37,16 +37,44 @@
#define UBX_MAX_RATE_VER7 10
#define UBX_MAX_RATE 5
// time to wait before reinitializing the fsm due to disconnection
#define UBX_CONNECTION_TIMEOUT (2000 * 1000)
// times between retries in case an error does occurs
#define UBX_ERROR_RETRY_TIMEOUT (1000 * 1000)
// reset to factory (and 9600 baud), and baud changes are not acked
// it could be 31 (full PIOS buffer) + 60 (gnss) + overhead bytes at 240 bytes per second
// = .42 seconds for the longest sentence to be fully transmitted
// and then it may have to do something like erase flash before replying...
// at low baud rates and high data rates the ubx gps simply must drop some outgoing data
// and when a lot of data is being dropped, the MON VER reply often gets dropped
// on the other hand, uBlox documents that some versions discard data that is over a second old
// implying that it could be over 1 second before a reply is received
// later versions dropped this and drop data when the send buffer is full and that could be even longer
// rather than have long timeouts, we will let timeouts * retries handle that if it happens
// timeout for ack reception
#define UBX_REPLY_TIMEOUT (500 * 1000)
// timeout for a settings save, in case it has to erase flash
#define UBX_REPLY_TO_SAVE_TIMEOUT (3000 * 1000)
// max retries in case of timeout
#define UBX_MAX_RETRIES 5
// pause between each configuration step
#define UBX_STEP_WAIT_TIME (10 * 1000)
// pause between each verifiably correct configuration step
#define UBX_VERIFIED_STEP_WAIT_TIME (50 * 1000)
// pause between each unverifiably correct configuration step
#define UBX_UNVERIFIED_STEP_WAIT_TIME (500 * 1000)
#define UBX_CFG_CFG_OP_STORE_SETTINGS \
(UBX_CFG_CFG_SETTINGS_IOPORT | \
UBX_CFG_CFG_SETTINGS_MSGCONF | \
UBX_CFG_CFG_SETTINGS_NAVCONF)
#define UBX_CFG_CFG_OP_CLEAR_SETTINGS ((~UBX_CFG_CFG_OP_STORE_SETTINGS) & UBX_CFG_CFG_SETTINGS_ALL)
// don't clear rinv as that is just text as configured by the owner
#define UBX_CFG_CFG_OP_RESET_SETTINGS \
(UBX_CFG_CFG_SETTINGS_IOPORT | \
UBX_CFG_CFG_SETTINGS_MSGCONF | \
UBX_CFG_CFG_SETTINGS_INFMSG | \
UBX_CFG_CFG_SETTINGS_NAVCONF | \
UBX_CFG_CFG_SETTINGS_TPCONF | \
UBX_CFG_CFG_SETTINGS_SFDRCONF | \
UBX_CFG_CFG_SETTINGS_ANTCONF)
// types
typedef enum {
UBX_AUTOCONFIG_STATUS_DISABLED = 0,
@ -54,26 +82,6 @@ typedef enum {
UBX_AUTOCONFIG_STATUS_DONE,
UBX_AUTOCONFIG_STATUS_ERROR
} ubx_autoconfig_status_t;
// Enumeration options for field UBXDynamicModel
typedef enum {
UBX_DYNMODEL_PORTABLE = 0,
UBX_DYNMODEL_STATIONARY = 2,
UBX_DYNMODEL_PEDESTRIAN = 3,
UBX_DYNMODEL_AUTOMOTIVE = 4,
UBX_DYNMODEL_SEA = 5,
UBX_DYNMODEL_AIRBORNE1G = 6,
UBX_DYNMODEL_AIRBORNE2G = 7,
UBX_DYNMODEL_AIRBORNE4G = 8
} ubx_config_dynamicmodel_t;
typedef enum {
UBX_SBAS_SATS_AUTOSCAN = 0,
UBX_SBAS_SATS_WAAS = 1,
UBX_SBAS_SATS_EGNOS = 2,
UBX_SBAS_SATS_MSAS = 3,
UBX_SBAS_SATS_GAGAN = 4,
UBX_SBAS_SATS_SDCM = 5
} ubx_config_sats_t;
#define UBX_
typedef struct {
@ -88,105 +96,27 @@ typedef struct {
int8_t navRate;
ubx_config_dynamicmodel_t dynamicModel;
bool enableGPS;
bool enableGLONASS;
bool enableBeiDou;
} ubx_autoconfig_settings_t;
// Mask for "all supported devices": battery backed RAM, Flash, EEPROM, SPI Flash
#define UBX_CFG_CFG_ALL_DEVICES_MASK (0x01 | 0x02 | 0x04 | 0x10)
// Sent messages for configuration support
typedef struct {
uint32_t clearMask;
uint32_t saveMask;
uint32_t loadMask;
uint8_t deviceMask;
} __attribute__((packed)) ubx_cfg_cfg_t;
typedef struct UBX_CFG_CFG ubx_cfg_cfg_t;
typedef struct UBX_CFG_NAV5 ubx_cfg_nav5_t;
typedef struct UBX_CFG_RATE ubx_cfg_rate_t;
typedef struct UBX_CFG_MSG ubx_cfg_msg_t;
typedef struct UBX_CFG_PRT ubx_cfg_prt_t;
typedef struct UBX_CFG_SBAS ubx_cfg_sbas_t;
typedef struct UBX_CFG_GNSS_CFGBLOCK ubx_cfg_gnss_cfgblock_t;
typedef struct UBX_CFG_GNSS ubx_cfg_gnss_t;
typedef struct UBXSENTHEADER UBXSentHeader_t;
typedef union UBXSENTPACKET UBXSentPacket_t;
typedef struct {
uint16_t mask;
uint8_t dynModel;
uint8_t fixMode;
int32_t fixedAlt;
uint32_t fixedAltVar;
int8_t minElev;
uint8_t drLimit;
uint16_t pDop;
uint16_t tDop;
uint16_t pAcc;
uint16_t tAcc;
uint8_t staticHoldThresh;
uint8_t dgpsTimeOut;
uint8_t cnoThreshNumSVs;
uint8_t cnoThresh;
uint16_t reserved2;
uint32_t reserved3;
uint32_t reserved4;
} __attribute__((packed)) ubx_cfg_nav5_t;
void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send);
void ubx_autoconfig_set(ubx_autoconfig_settings_t *config);
void ubx_reset_sensor_type();
typedef struct {
uint16_t measRate;
uint16_t navRate;
uint16_t timeRef;
} __attribute__((packed)) ubx_cfg_rate_t;
typedef struct {
uint8_t msgClass;
uint8_t msgID;
uint8_t rate;
} __attribute__((packed)) ubx_cfg_msg_t;
#define UBX_CFG_SBAS_MODE_ENABLED 0x01
#define UBX_CFG_SBAS_MODE_TEST 0x02
#define UBX_CFG_SBAS_USAGE_RANGE 0x01
#define UBX_CFG_SBAS_USAGE_DIFFCORR 0x02
#define UBX_CFG_SBAS_USAGE_INTEGRITY 0x04
// SBAS used satellite PNR bitmask (120-151)
// -------------------------------------1---------1---------1---------1
// -------------------------------------5---------4---------3---------2
// ------------------------------------10987654321098765432109876543210
// WAAS 122, 133, 134, 135, 138---------|---------|---------|---------|
#define UBX_CFG_SBAS_SCANMODE1_WAAS 0b00000000000001001110000000000100
// EGNOS 120, 124, 126, 131-------------|---------|---------|---------|
#define UBX_CFG_SBAS_SCANMODE1_EGNOS 0b00000000000000000000100001010001
// MSAS 129, 137------------------------|---------|---------|---------|
#define UBX_CFG_SBAS_SCANMODE1_MSAS 0b00000000000000100000001000000000
// GAGAN 127, 128-----------------------|---------|---------|---------|
#define UBX_CFG_SBAS_SCANMODE1_GAGAN 0b00000000000000000000000110000000
// SDCM 125, 140, 141-------------------|---------|---------|---------|
#define UBX_CFG_SBAS_SCANMODE1_SDCM 0b00000000001100000000000000100000
#define UBX_CFG_SBAS_SCANMODE2 0x00
typedef struct {
uint8_t mode;
uint8_t usage;
uint8_t maxSBAS;
uint8_t scanmode2;
uint32_t scanmode1;
} __attribute__((packed)) ubx_cfg_sbas_t;
typedef struct {
uint8_t prolog[2];
uint8_t class;
uint8_t id;
uint16_t len;
} __attribute__((packed)) UBXSentHeader_t;
typedef union {
uint8_t buffer[0];
struct {
UBXSentHeader_t header;
union {
ubx_cfg_cfg_t cfg_cfg;
ubx_cfg_msg_t cfg_msg;
ubx_cfg_nav5_t cfg_nav5;
ubx_cfg_rate_t cfg_rate;
ubx_cfg_sbas_t cfg_sbas;
} payload;
uint8_t resvd[2]; // added space for checksum bytes
} message;
} __attribute__((packed)) UBXSentPacket_t;
void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send, bool gps_connected);
void ubx_autoconfig_set(ubx_autoconfig_settings_t config);
int32_t ubx_autoconfig_get_status();
#endif /* UBX_AUTOCONFIG_H_ */

756
flight/modules/GPS/ubx_autoconfig.c
View File

@ -27,28 +27,47 @@
* 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 "hwsettings.h"
#include "inc/ubx_autoconfig.h"
#include <pios_mem.h>
// private type definitions
typedef enum {
INIT_STEP_DISABLED = 0,
INIT_STEP_START,
INIT_STEP_ASK_VER,
INIT_STEP_WAIT_VER,
INIT_STEP_RESET_GPS,
INIT_STEP_REVO_9600_BAUD,
INIT_STEP_GPS_BAUD,
INIT_STEP_REVO_BAUD,
INIT_STEP_ENABLE_SENTENCES,
INIT_STEP_ENABLE_SENTENCES_WAIT_ACK,
INIT_STEP_CONFIGURE,
INIT_STEP_CONFIGURE_WAIT_ACK,
INIT_STEP_SAVE,
INIT_STEP_SAVE_WAIT_ACK,
INIT_STEP_DONE,
INIT_STEP_ERROR,
INIT_STEP_ERROR
} initSteps_t;
typedef struct {
initSteps_t currentStep; // Current configuration "fsm" status
initSteps_t currentStepSave; // Current configuration "fsm" status
uint32_t lastStepTimestampRaw; // timestamp of last operation
uint32_t lastConnectedRaw; // timestamp of last time gps was connected
struct {
UBXSentPacket_t working_packet; // outbound "buffer"
ubx_autoconfig_settings_t currentSettings;
// bufferPaddingForPiosBugAt2400Baud must exist for baud rate change to work at 2400 or 4800
// failure mode otherwise:
// - send message with baud rate change
// - wait 1 second (even at 2400, the baud rate change command should clear even an initially full 31 byte PIOS buffer much more quickly)
// - change Revo port baud rate
// sometimes fails (much worse for lowest baud rates)
uint8_t bufferPaddingForPiosBugAt2400Baud[2]; // must be at least 2 for 2400 to work, probably 1 for 4800 and 0 for 9600+
} __attribute__((packed));
volatile ubx_autoconfig_settings_t currentSettings;
int8_t lastConfigSent; // index of last configuration string sent
struct UBX_ACK_ACK requiredAck; // Class and id of the message we are waiting for an ACK from GPS
uint8_t retryCount;
@ -114,15 +133,24 @@ ubx_cfg_msg_t msg_config_ubx7[] = {
};
// private defines
#define LAST_CONFIG_SENT_START (-1)
#define LAST_CONFIG_SENT_COMPLETED (-2)
// always reset the stored GPS configuration, even when doing autoconfig.nostore
// that is required to do a 100% correct configuration
// but is unexpected because it changes the stored configuration when doing autoconfig.nostore
// note that a reset is always done with autoconfig.store
// #define ALWAYS_RESET
// private variables
// enable the autoconfiguration system
static bool enabled;
static volatile bool enabled = false;
static volatile bool current_step_touched = false;
// both the pointer and what it points to are volatile. Yuk.
static volatile status_t *volatile status = 0;
static uint8_t hwsettings_baud;
static status_t *status = 0;
static void append_checksum(UBXSentPacket_t *packet)
{
@ -139,29 +167,152 @@ static void append_checksum(UBXSentPacket_t *packet)
packet->buffer[len] = ck_a;
packet->buffer[len + 1] = ck_b;
}
/**
* prepare a packet to be sent, fill the header and appends the checksum.
* return the total packet lenght comprising header and checksum
*/
static uint16_t prepare_packet(UBXSentPacket_t *packet, uint8_t classID, uint8_t messageID, uint16_t len)
{
memset((uint8_t *)status->working_packet.buffer + len + sizeof(UBXSentHeader_t) + 2, 0, sizeof(status->bufferPaddingForPiosBugAt2400Baud));
packet->message.header.prolog[0] = UBX_SYNC1;
packet->message.header.prolog[1] = UBX_SYNC2;
packet->message.header.class = classID;
packet->message.header.id = messageID;
packet->message.header.len = len;
append_checksum(packet);
return packet->message.header.len + sizeof(UBXSentHeader_t) + 2; // header + payload + checksum
status->requiredAck.clsID = classID;
status->requiredAck.msgID = messageID;
return len + sizeof(UBXSentHeader_t) + 2 + sizeof(status->bufferPaddingForPiosBugAt2400Baud); // payload + header + checksum + extra bytes
}
static void build_request(UBXSentPacket_t *packet, uint8_t classID, uint8_t messageID, uint16_t *bytes_to_send)
{
*bytes_to_send = prepare_packet(packet, classID, messageID, 0);
}
void config_rate(uint16_t *bytes_to_send)
static void set_current_step_if_untouched(initSteps_t new_steps)
{
memset(status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_rate_t));
// assume this one byte initSteps_t is atomic
// take care of some but not all concurrency issues
if (!current_step_touched) {
status->currentStep = new_steps;
}
if (current_step_touched) {
status->currentStep = status->currentStepSave;
}
}
void ubx_reset_sensor_type()
{
ubxHwVersion = -1;
sensorType = GPSPOSITIONSENSOR_SENSORTYPE_UNKNOWN;
GPSPositionSensorSensorTypeSet((uint8_t *)&sensorType);
}
static void config_reset(uint16_t *bytes_to_send)
{
memset((uint8_t *)status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_cfg_t));
// mask LSB=ioPort|msgConf|infMsg|navConf|rxmConf|||||rinvConf|antConf|....|= MSB
// ioPort=1, msgConf=2, infMsg=4, navConf=8, tpConf=0x10, sfdrConf=0x100, rinvConf=0x200, antConf=0x400
// first: reset (permanent settings to default) all but rinv = e.g. owner name
status->working_packet.message.payload.cfg_cfg.clearMask = UBX_CFG_CFG_OP_RESET_SETTINGS;
// then: don't store any current settings to permanent
status->working_packet.message.payload.cfg_cfg.saveMask = UBX_CFG_CFG_SETTINGS_NONE;
// lastly: load (immediately start to use) all but rinv = e.g. owner name
status->working_packet.message.payload.cfg_cfg.loadMask = UBX_CFG_CFG_OP_RESET_SETTINGS;
// all devices
status->working_packet.message.payload.cfg_cfg.deviceMask = UBX_CFG_CFG_DEVICE_ALL;
*bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_CFG, sizeof(ubx_cfg_cfg_t));
}
// set the GPS baud rate to the user specified baud rate
// because we may have started up with 9600 baud (for a GPS with no permanent settings)
static void config_gps_baud(uint16_t *bytes_to_send)
{
memset((uint8_t *)status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_prt_t));
status->working_packet.message.payload.cfg_prt.mode = UBX_CFG_PRT_MODE_DEFAULT; // 8databits, 1stopbit, noparity, and non-zero reserved
status->working_packet.message.payload.cfg_prt.portID = 1; // 1 = UART1, 2 = UART2
status->working_packet.message.payload.cfg_prt.inProtoMask = 1; // 1 = UBX only (bit 0)
status->working_packet.message.payload.cfg_prt.outProtoMask = 1; // 1 = UBX only (bit 0)
// Ask GPS to change it's speed
switch (hwsettings_baud) {
case HWSETTINGS_GPSSPEED_2400:
status->working_packet.message.payload.cfg_prt.baudRate = 2400;
break;
case HWSETTINGS_GPSSPEED_4800:
status->working_packet.message.payload.cfg_prt.baudRate = 4800;
break;
case HWSETTINGS_GPSSPEED_9600:
status->working_packet.message.payload.cfg_prt.baudRate = 9600;
break;
case HWSETTINGS_GPSSPEED_19200:
status->working_packet.message.payload.cfg_prt.baudRate = 19200;
break;
case HWSETTINGS_GPSSPEED_38400:
status->working_packet.message.payload.cfg_prt.baudRate = 38400;
break;
case HWSETTINGS_GPSSPEED_57600:
status->working_packet.message.payload.cfg_prt.baudRate = 57600;
break;
case HWSETTINGS_GPSSPEED_115200:
status->working_packet.message.payload.cfg_prt.baudRate = 115200;
break;
case HWSETTINGS_GPSSPEED_230400:
status->working_packet.message.payload.cfg_prt.baudRate = 230400;
break;
}
*bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_PRT, sizeof(ubx_cfg_prt_t));
}
// having already set the GPS's baud rate with a serial command, set the local Revo port baud rate
static void config_baud(uint8_t baud)
{
// Set Revo port hwsettings_baud
switch (baud) {
case HWSETTINGS_GPSSPEED_2400:
PIOS_COM_ChangeBaud(PIOS_COM_GPS, 2400);
break;
case HWSETTINGS_GPSSPEED_4800:
PIOS_COM_ChangeBaud(PIOS_COM_GPS, 4800);
break;
case HWSETTINGS_GPSSPEED_9600:
PIOS_COM_ChangeBaud(PIOS_COM_GPS, 9600);
break;
case HWSETTINGS_GPSSPEED_19200:
PIOS_COM_ChangeBaud(PIOS_COM_GPS, 19200);
break;
case HWSETTINGS_GPSSPEED_38400:
PIOS_COM_ChangeBaud(PIOS_COM_GPS, 38400);
break;
case HWSETTINGS_GPSSPEED_57600:
PIOS_COM_ChangeBaud(PIOS_COM_GPS, 57600);
break;
case HWSETTINGS_GPSSPEED_115200:
PIOS_COM_ChangeBaud(PIOS_COM_GPS, 115200);
break;
case HWSETTINGS_GPSSPEED_230400:
PIOS_COM_ChangeBaud(PIOS_COM_GPS, 230400);
break;
}
}
static void config_rate(uint16_t *bytes_to_send)
{
memset((uint8_t *)status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_rate_t));
// if rate is less than 1 uses the highest rate for current hardware
uint16_t rate = status->currentSettings.navRate > 0 ? status->currentSettings.navRate : 99;
if (ubxHwVersion < UBX_HW_VERSION_7 && rate > UBX_MAX_RATE) {
@ -172,36 +323,31 @@ void config_rate(uint16_t *bytes_to_send)
rate = UBX_MAX_RATE_VER8;
}
uint16_t period = 1000 / rate;
status->working_packet.message.payload.cfg_rate.measRate = period;
status->working_packet.message.payload.cfg_rate.navRate = 1; // must be set to 1
status->working_packet.message.payload.cfg_rate.timeRef = 1; // 0 = UTC Time, 1 = GPS Time
*bytes_to_send = prepare_packet(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_RATE, sizeof(ubx_cfg_rate_t));
status->requiredAck.clsID = UBX_CLASS_CFG;
status->requiredAck.msgID = UBX_ID_CFG_RATE;
*bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_RATE, sizeof(ubx_cfg_rate_t));
}
void config_nav(uint16_t *bytes_to_send)
{
memset(status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_nav5_t));
static void config_nav(uint16_t *bytes_to_send)
{
memset((uint8_t *)status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_nav5_t));
status->working_packet.message.payload.cfg_nav5.dynModel = status->currentSettings.dynamicModel;
status->working_packet.message.payload.cfg_nav5.fixMode = 2; // 1=2D only, 2=3D only, 3=Auto 2D/3D
// mask LSB=dyn|minEl|posFixMode|drLim|posMask|statisticHoldMask|dgpsMask|......|reservedBit0 = MSB
status->working_packet.message.payload.cfg_nav5.mask = 0x01 + 0x04; // Dyn Model | posFixMode configuration
*bytes_to_send = prepare_packet(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_NAV5, sizeof(ubx_cfg_nav5_t));
status->requiredAck.clsID = UBX_CLASS_CFG;
status->requiredAck.msgID = UBX_ID_CFG_NAV5;
*bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_NAV5, sizeof(ubx_cfg_nav5_t));
}
void config_sbas(uint16_t *bytes_to_send)
static void config_sbas(uint16_t *bytes_to_send)
{
memset(status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_sbas_t));
status->working_packet.message.payload.cfg_sbas.maxSBAS = status->currentSettings.SBASChannelsUsed < 4 ?
status->currentSettings.SBASChannelsUsed : 3;
memset((uint8_t *)status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_sbas_t));
status->working_packet.message.payload.cfg_sbas.maxSBAS =
status->currentSettings.SBASChannelsUsed < 4 ? status->currentSettings.SBASChannelsUsed : 3;
status->working_packet.message.payload.cfg_sbas.usage =
(status->currentSettings.SBASCorrection ? UBX_CFG_SBAS_USAGE_DIFFCORR : 0) |
(status->currentSettings.SBASIntegrity ? UBX_CFG_SBAS_USAGE_INTEGRITY : 0) |
@ -209,58 +355,119 @@ void config_sbas(uint16_t *bytes_to_send)
// If sbas is used for anything then set mode as enabled
status->working_packet.message.payload.cfg_sbas.mode =
status->working_packet.message.payload.cfg_sbas.usage != 0 ? UBX_CFG_SBAS_MODE_ENABLED : 0;
status->working_packet.message.payload.cfg_sbas.scanmode1 =
status->currentSettings.SBASSats == UBX_SBAS_SATS_WAAS ? UBX_CFG_SBAS_SCANMODE1_WAAS :
status->currentSettings.SBASSats == UBX_SBAS_SATS_EGNOS ? UBX_CFG_SBAS_SCANMODE1_EGNOS :
status->currentSettings.SBASSats == UBX_SBAS_SATS_MSAS ? UBX_CFG_SBAS_SCANMODE1_MSAS :
status->currentSettings.SBASSats == UBX_SBAS_SATS_GAGAN ? UBX_CFG_SBAS_SCANMODE1_GAGAN :
status->currentSettings.SBASSats == UBX_SBAS_SATS_SDCM ? UBX_CFG_SBAS_SCANMODE1_SDCM : UBX_SBAS_SATS_AUTOSCAN;
status->working_packet.message.payload.cfg_sbas.scanmode2 =
UBX_CFG_SBAS_SCANMODE2;
status->working_packet.message.payload.cfg_sbas.scanmode2 = UBX_CFG_SBAS_SCANMODE2;
*bytes_to_send = prepare_packet(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_SBAS, sizeof(ubx_cfg_sbas_t));
status->requiredAck.clsID = UBX_CLASS_CFG;
status->requiredAck.msgID = UBX_ID_CFG_SBAS;
*bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_SBAS, sizeof(ubx_cfg_sbas_t));
}
void config_save(uint16_t *bytes_to_send)
static void config_gnss(uint16_t *bytes_to_send)
{
memset(status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_cfg_t));
// mask LSB=ioPort|msgConf|infMsg|navConf|rxmConf|||||rinvConf|antConf|....|= MSB
status->working_packet.message.payload.cfg_cfg.saveMask = 0x02 | 0x08; // msgConf + navConf
status->working_packet.message.payload.cfg_cfg.deviceMask = UBX_CFG_CFG_ALL_DEVICES_MASK;
*bytes_to_send = prepare_packet(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_CFG, sizeof(ubx_cfg_cfg_t));
status->requiredAck.clsID = UBX_CLASS_CFG;
status->requiredAck.msgID = UBX_ID_CFG_CFG;
memset((uint8_t *)status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_gnss_t));
status->working_packet.message.payload.cfg_gnss.numConfigBlocks = UBX_GNSS_ID_MAX;
status->working_packet.message.payload.cfg_gnss.numTrkChHw = (ubxHwVersion > UBX_HW_VERSION_7) ? UBX_CFG_GNSS_NUMCH_VER8 : UBX_CFG_GNSS_NUMCH_VER7;
status->working_packet.message.payload.cfg_gnss.numTrkChUse = status->working_packet.message.payload.cfg_gnss.numTrkChHw;
for (int32_t i = 0; i < UBX_GNSS_ID_MAX; i++) {
status->working_packet.message.payload.cfg_gnss.cfgBlocks[i].gnssId = i;
switch (i) {
case UBX_GNSS_ID_GPS:
if (status->currentSettings.enableGPS) {
status->working_packet.message.payload.cfg_gnss.cfgBlocks[i].flags = UBX_CFG_GNSS_FLAGS_ENABLED | UBX_CFG_GNSS_FLAGS_GPS_L1CA;
status->working_packet.message.payload.cfg_gnss.cfgBlocks[i].maxTrkCh = 16;
status->working_packet.message.payload.cfg_gnss.cfgBlocks[i].resTrkCh = 8;
}
break;
case UBX_GNSS_ID_QZSS:
if (status->currentSettings.enableGPS) {
status->working_packet.message.payload.cfg_gnss.cfgBlocks[i].flags = UBX_CFG_GNSS_FLAGS_ENABLED | UBX_CFG_GNSS_FLAGS_QZSS_L1CA;
status->working_packet.message.payload.cfg_gnss.cfgBlocks[i].maxTrkCh = 3;
status->working_packet.message.payload.cfg_gnss.cfgBlocks[i].resTrkCh = 0;
}
break;
case UBX_GNSS_ID_SBAS:
if (status->currentSettings.SBASCorrection || status->currentSettings.SBASIntegrity || status->currentSettings.SBASRanging) {
status->working_packet.message.payload.cfg_gnss.cfgBlocks[i].flags = UBX_CFG_GNSS_FLAGS_ENABLED | UBX_CFG_GNSS_FLAGS_SBAS_L1CA;
status->working_packet.message.payload.cfg_gnss.cfgBlocks[i].maxTrkCh = status->currentSettings.SBASChannelsUsed < 4 ? status->currentSettings.SBASChannelsUsed : 3;
status->working_packet.message.payload.cfg_gnss.cfgBlocks[i].resTrkCh = 1;
}
break;
case UBX_GNSS_ID_GLONASS:
if (status->currentSettings.enableGLONASS) {
status->working_packet.message.payload.cfg_gnss.cfgBlocks[i].flags = UBX_CFG_GNSS_FLAGS_ENABLED | UBX_CFG_GNSS_FLAGS_GLONASS_L1OF;
status->working_packet.message.payload.cfg_gnss.cfgBlocks[i].maxTrkCh = 14;
status->working_packet.message.payload.cfg_gnss.cfgBlocks[i].resTrkCh = 8;
}
break;
case UBX_GNSS_ID_BEIDOU:
if (status->currentSettings.enableBeiDou) {
status->working_packet.message.payload.cfg_gnss.cfgBlocks[i].flags = UBX_CFG_GNSS_FLAGS_ENABLED | UBX_CFG_GNSS_FLAGS_BEIDOU_B1I;
status->working_packet.message.payload.cfg_gnss.cfgBlocks[i].maxTrkCh = 14;
status->working_packet.message.payload.cfg_gnss.cfgBlocks[i].resTrkCh = 8;
}
break;
default:
break;
}
}
*bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_GNSS, sizeof(ubx_cfg_gnss_t));
}
static void config_save(uint16_t *bytes_to_send)
{
memset((uint8_t *)status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_cfg_t));
// mask LSB=ioPort|msgConf|infMsg|navConf|rxmConf|||||rinvConf|antConf|....|= MSB
// ioPort=1, msgConf=2, infMsg=4, navConf=8, tpConf=0x10, sfdrConf=0x100, rinvConf=0x200, antConf=0x400
status->working_packet.message.payload.cfg_cfg.saveMask = UBX_CFG_CFG_OP_STORE_SETTINGS; // a list of settings we just set
status->working_packet.message.payload.cfg_cfg.clearMask = UBX_CFG_CFG_OP_CLEAR_SETTINGS; // everything else gets factory default
status->working_packet.message.payload.cfg_cfg.deviceMask = UBX_CFG_CFG_DEVICE_ALL;
*bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_CFG, sizeof(ubx_cfg_cfg_t));
}
static void configure(uint16_t *bytes_to_send)
{
switch (status->lastConfigSent) {
case LAST_CONFIG_SENT_START:
// increase message rates to 5 fixes per second
config_rate(bytes_to_send);
break;
case LAST_CONFIG_SENT_START + 1:
config_nav(bytes_to_send);
break;
case LAST_CONFIG_SENT_START + 2:
config_sbas(bytes_to_send);
if (!status->currentSettings.storeSettings) {
// skips saving
status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED;
}
if (status->currentSettings.enableGLONASS || status->currentSettings.enableGPS) {
config_gnss(bytes_to_send);
break;
} else {
// Skip and fall through to next step
status->lastConfigSent++;
}
// in the else case we must fall through because we must send something each time because successful send is tested externally
case LAST_CONFIG_SENT_START + 3:
config_save(bytes_to_send);
status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED;
return;
config_sbas(bytes_to_send);
break;
default:
status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED;
return;
break;
}
}
static void enable_sentences(__attribute__((unused)) uint16_t *bytes_to_send)
{
int8_t msg = status->lastConfigSent + 1;
@ -271,76 +478,303 @@ static void enable_sentences(__attribute__((unused)) uint16_t *bytes_to_send)
if (msg >= 0 && msg < msg_count) {
status->working_packet.message.payload.cfg_msg = msg_config[msg];
*bytes_to_send = prepare_packet(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_MSG, sizeof(ubx_cfg_msg_t));
status->requiredAck.clsID = UBX_CLASS_CFG;
status->requiredAck.msgID = UBX_ID_CFG_MSG;
*bytes_to_send = prepare_packet((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_MSG, sizeof(ubx_cfg_msg_t));
} else {
status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED;
}
}
void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send, bool gps_connected)
// End User Documentation
// There are two baud rates of interest
// The baud rate the GPS is talking at
// The baud rate Revo is talking at
// These two must match for the GPS to work
// You only have direct control of the Revo baud rate
// The two baud rates must be the same for the Revo to send a command to the GPS
// to tell the GPS to change it's baud rate
// So you start out by changing Revo's baud rate to match the GPS's
// and then enable UbxAutoConfig to tell Revo to change the GPS baud every time, just before it changes the Revo baud
// That is the basis of these instructions
// There are microprocessors and they each have internal settings
// Revo
// GPS
// and each of these settings can be temporary or permanent
// To change a Revo setting
// Use the System tab in the GCS for all the following
// Example: in Settings->GPSSettings click on the VALUE for UbxAutoConfig and change it to Disabled
// Click on UbxAutoConfig itself and the line will turn green and blue
// To change this setting permanently, press the red up arrow (Save) at the top of the screen
// Permanently means that it uses this setting, even if you reboot Revo, e.g. power off and on
// To change this setting temporarily, press the green up arrow (Send) at the top of the screen
// Temporarily means that it overrides the permanent setting, but it goes back to the permanent setting when you reboot Revo, e.g. power off and on
// To change an internal GPS setting you use the OP GCS System tab to tell Revo to make the GPS changes
// This only works correctly after you have matching baud rates so Revo and GPS can talk together
// "Settings->GPSSettings->UbxAutoConfig = Configure" sets the internal GPS setting temporarily
// "Settings->GPSSettings->UbxAutoConfig = ConfigureAndStore" sets the internal GPS setting permanently
// You want to wind up with a set of permanent settings that work together
// There are two different sets of permanent settings that work together
// GPS at 9600 baud and factory defaults
// Revo configured to start out at 9600 baud, but then completely configure the GPS and switch both to 57600 baud
// (takes 6 seconds at boot up while you are waiting for it to acquire satellites anyway)
// This is the preferred way so that if we change the settings in the future, the new release will automatically use the correct settings
// GPS at 57600 baud with all the settings for the current release stored in the GPS
// Revo configured to disable UbxAutoConfig since all the GPS settings are permanently stored correctly
// May require reconfiguring in a future release
// Changable settings of interest
// AutoConfig mode
// Settings->GPSSettings->UbxAutoConfig (Disabled, Configure, ConfigureAndStore, default=Configure)
// Disabled means that changes to the GPS baud setting only affect the Revo port
// It doesn't try to change the GPS's internal baud rate setting
// Configure means change the GPS's internal baud setting temporarily (GPS settings revert to the permanent values when GPS is powered off/on)
// ConfigureAndStore means change the GPS's internal baud setting permanently (even after the GPS is powered off/on)
// GPS baud rate
// Settings->HwSettings->GPSSpeed
// If the baud rates are the same and an AutoConfig mode is enabled this will change both the GPS baud rate and the Revo baud rate
// If the baud rates are not the same and an AutoConfig mode is enabled it will fail
// If AutoConfig mode is disabled this will only change the Revo baud rate
// View only settings of interest
// Detected GPS type
// Data Objects -> GPSPositionSensor -> SensorType (Unknown, NMEA, UBX, UBX7, UBX8)
// When it says something other than Unknown, the GPS and Revo baud rates are synced and talking
// Real time progress of the GPS detection process
// Data Objects -> GPSPositionSensor -> AutoConfigStatus (DISABLED, RUNNING, DONE, ERROR)
// Syncing the baud rates means that the GPS's internal baud rate setting is the same as the Revo port setting
// This is necessary for the GPS to work with Revo
// To sync to and find out an unknown GPS baud rate (or sync to and use a known GPS baud rate)
// Temporarily change the AutoConfig mode to Disabled
// Temporarily change the GPS baud rate to a value you think it might be (or go up the list)
// See if that baud rate is correct (Data Objects->GPSPositionSensor->SensorType will be something besides Unknown)
// Repeat, changing the GPS baud rate, until found
// Some very important facts:
// For 9600 baud or lower, the autoconfig will configure it to factory default settings
// For 19200 baud or higher, the autoconfig will configure it to OP required settings
// If autoconfig is enabled permanently in Revo, it will assume that the GPS is configured to power up at 9600 baud
// 57600 baud is recommended for the current release
// That can be achieved either by
// autoconfiging the GPS from a permanent 9600 baud (and factory settings) to a temporary 57600 (with OP settings) on each power up
// or by configuring the GPS with a permanent 57600 (with OP settings) and then permanently disabling autoconfig
// Some previous releases used 38400 and had some other settings differences
// The user should either:
// Permanently configure their GPS to 9600 baud factory settings and tell the Revo configuration to load volatile settings at each startup by:
// (Recommended method because new versions could require new settings and this handles future changes automatically)
// Syncing the baud rates
// Setting it to autoconfig.nostore and waiting for it to complete
// Setting HwSettings.GPSSpeed to 9600 and waiting for it to complete
// Setting it to autoconfig.store and waiting for it to complete (this tells the GPS to store the 9600 permanently)
// Permanently setting it to autoconfig.nostore and waiting for it to complete
// Permanently setting HwSettings.GPSSpeed to 57600 and waiting for it to complete
// Permanently configure their GPS to 57600 baud, including OpenPilot settings and telling the Revo configuration to just set the baud to 57600 at each startup by:
// (Less recommended method because new versions could require new settings so you would have to do this again)
// Syncing the baud rates
// Setting it to autoconfig.nostore and waiting for it to complete
// Permanently setting HwSettings.GPSSpeed to 57600 and waiting for it to complete
// Setting it to autoconfig.store
// Permanently setting it to autoconfig.disabled
// The algorithm is:
// If autoconfig is enabled at all
// It will assume that the GPS boot up baud rate is 9600 and the user wants that changed to HwSettings.GPSSpeed
// and that change can be either volatile (must be done each boot up) or non-volatile (stored in GPS's non-volatile settings storage)
// according to whether CONFIGURE is used or CONFIGUREANDSTORE is used
// The only user who should need CONFIGUREANDSTORE stored permanently in Revo is Dave, who configures many OP GPS's before shipping
// plug a factory default GPS in to a Revo, power up, wait for it to configure and permanently store in the GPS, power down, ship
// If autoconfig is not enabled
// it will use HwSettings.GPSSpeed for the baud rate and not do any configuration changes
// If GPSSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_CONFIGUREANDSTORE it will
// 1 Reset the permanent configuration back to factory default
// 2 Disable NEMA message settings
// 3 Add some volatile UBX settings to the copies of the non-volatile ones that are currently running
// 4 Save the current volatile settings to non-volatile storage
// If GPSSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_CONFIGURE it will
// 2 Disable NEMA message settings
// 3 Add some volatile UBX settings to the copies of the non-volatile ones that are currently running
// If the requested baud rate is 9600 or less it skips the step (3) of adding some volatile UBX settings
// Talking points to point out:
// U-center is no longer needed for any use case with this code
// 9600 is factory default for GPS's
// Some GPS can't even permanently store settings and must start at 9600 baud?
// I have a GPS that sometimes looses settings and reverts to 9600 and this is a fix for that too :)
// This code handles a GPS configured either way (9600 with factory default settings or e.g. 57600 with OP settings)
// Autoconfig.nostore at each boot for 9600, autoconfig.disabled for the 57600 with OP settings (or custom settings and baud)
// This code can permanently configure a GPS to be e.g. 9600 with factory default settings or 57600 with OP settings
// GPS's with 9600 baud and factory default settings would be a good default for future OP releases
// Changing the GPS internal settings multiple times in the future is handled automatically
// This code is written to do a configure from 9600 to 57600
// (actually 9600 to whatever is stored in HwSettings.GPSSpeed)
// if autoconfig is enabled at boot up
// When autoconfiging to 9600 baud or lower, the autoconfig will configure it to factory default settings, not OP settings
// That is because 9600 baud drops many of the OP messages and because 9600 baud is factory default
// For 19200 baud or higher, the autoconfig will configure it to OP required settings
// If autoconfig is enabled permanently in Revo, it will assume that the GPS is configured to power up at 9600 baud
// This is good for factory default GPS's
// This is good in case we change some settings in a future release
void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send)
{
*bytes_to_send = 0;
*buffer = (char *)status->working_packet.buffer;
if (!status || !enabled || status->currentStep == INIT_STEP_DISABLED) {
current_step_touched = false;
// autoconfig struct not yet allocated
if (!status) {
return;
}
// smallest delay between each step
if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_VERIFIED_STEP_WAIT_TIME) {
return;
}
// get UBX version whether autoconfig is enabled or not
// this allows the user to try some baud rates and visibly see when it works
// ubxHwVersion is a global set externally by the caller of this function
if (ubxHwVersion <= 0) {
// at low baud rates and high data rates the ubx gps simply must drop some outgoing data
// this isn't really an error
// and when a lot of data is being dropped, the MON VER reply often gets dropped
// on the other hand, uBlox documents that some versions discard data that is over 1 second old
// implying a 1 second send buffer and that it could be over 1 second before a reply is received
// later uBlox versions dropped this 1 second constraint and drop data when the send buffer is full
// and that could be even longer than 1 second
// send this more quickly and it will get a reply more quickly if a fixed percentage of replies are being dropped
// wait for the normal reply timeout before sending it over and over
if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_REPLY_TIMEOUT) {
return;
}
build_request((UBXSentPacket_t *)&status->working_packet, UBX_CLASS_MON, UBX_ID_MON_VER, bytes_to_send);
// keep timeouts running properly, we (will have) just sent a packet that generates a reply
status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
return;
}
if (!enabled) {
// keep resetting the timeouts here if we are not actually going to run the configure code
// not really necessary, but it keeps the timer from wrapping every 50 seconds
status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
return; // autoconfig not enabled
}
if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_STEP_WAIT_TIME) {
// replaying constantly could wear the settings memory out
// don't allow constant reconfiging when offline
// don't even allow program bugs that could constantly toggle between connected and disconnected to cause configuring
if (status->currentStep == INIT_STEP_DONE || status->currentStep == INIT_STEP_ERROR) {
return;
}
// when gps is disconnected it will replay the autoconfig sequence.
if (!gps_connected) {
if (status->currentStep == INIT_STEP_DONE) {
// if some operation is in progress and it is not running into issues it maybe that
// the disconnection is only due to wrong message rates, so reinit only when done.
// errors caused by disconnection are handled by error retry logic
if (PIOS_DELAY_DiffuS(status->lastConnectedRaw) > UBX_CONNECTION_TIMEOUT) {
status->currentStep = INIT_STEP_START;
return;
}
}
} else {
// reset connection timer
status->lastConnectedRaw = PIOS_DELAY_GetRaw();
}
switch (status->currentStep) {
case INIT_STEP_ERROR:
if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) > UBX_ERROR_RETRY_TIMEOUT) {
status->currentStep = INIT_STEP_START;
status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
}
return;
case INIT_STEP_DISABLED:
case INIT_STEP_DONE:
return;
case INIT_STEP_START:
case INIT_STEP_ASK_VER:
// clear ack
ubxLastAck.clsID = 0x00;
ubxLastAck.msgID = 0x00;
status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
build_request(&status->working_packet, UBX_CLASS_MON, UBX_ID_MON_VER, bytes_to_send);
status->currentStep = INIT_STEP_WAIT_VER;
// we should look for the GPS version again
ubx_reset_sensor_type();
// do not fall through to next state
// or it might try to get the sensor type when the baud rate is half changed
set_current_step_if_untouched(INIT_STEP_RESET_GPS);
// allow it to get the sensor type immmediately by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
break;
case INIT_STEP_WAIT_VER:
if (ubxHwVersion > 0) {
status->lastConfigSent = LAST_CONFIG_SENT_START;
status->currentStep = INIT_STEP_ENABLE_SENTENCES;
status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
return;
}
case INIT_STEP_RESET_GPS:
// make sure we don't change the baud rate too soon and garble the packet being sent
// even after pios says the buffer is empty, the serial port buffer still has data in it
// and changing the baud will screw it up
// when the GPS is configured to send a lot of data, but has a low baud rate
// it has way too many messages to send and has to drop most of them
if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) > UBX_REPLY_TIMEOUT) {
status->currentStep = INIT_STEP_ASK_VER;
// Retrieve desired GPS baud rate once for use throughout this module
HwSettingsGPSSpeedGet(&hwsettings_baud);
#if !defined(ALWAYS_RESET)
// ALWAYS_RESET is undefined because it causes stored settings to change even with autoconfig.nostore
// but with it off, some settings may be enabled that should really be disabled (but aren't) after autoconfig.nostore
// if user requests a low baud rate then we just reset and leave it set to NEMA
// because low baud and high OP data rate doesn't play nice
// if user requests that settings be saved, we will reset here too
// that makes sure that all strange settings are reset to factory default
// else these strange settings may persist because we don't reset all settings by table
if (status->currentSettings.storeSettings)
#endif
{
// reset all GPS parameters to factory default (configure low rate NEMA for low baud rates)
// this is not usable by OP code for either baud rate or types of messages sent
// but it starts up very quickly for use with autoconfig-nostore (which sets a high baud and enables all the necessary messages)
config_reset(bytes_to_send);
status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
}
// else allow it enter the next state immmediately by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
set_current_step_if_untouched(INIT_STEP_REVO_9600_BAUD);
break;
case INIT_STEP_REVO_9600_BAUD:
#if !defined(ALWAYS_RESET)
// if user requests a low baud rate then we just reset and leave it set to NEMA
// because low baud and high OP data rate doesn't play nice
// if user requests that settings be saved, we will reset here too
// that makes sure that all strange settings are reset to factory default
// else these strange settings may persist because we don't reset all settings by hand
if (status->currentSettings.storeSettings)
#endif
{
// wait for previous step
if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_UNVERIFIED_STEP_WAIT_TIME) {
return;
}
// set the Revo GPS port to 9600 baud to match the reset to factory default that has already been done
config_baud(HWSETTINGS_GPSSPEED_9600);
}
// at most, we just set Revo baud and that doesn't send any data
// fall through to next state
// we can do that if we choose because we haven't sent any data in this state
// set_current_step_if_untouched(INIT_STEP_GPS_BAUD);
// allow it enter the next state immmediately by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
// break;
case INIT_STEP_GPS_BAUD:
// https://www.u-blox.com/images/downloads/Product_Docs/u-bloxM8_ReceiverDescriptionProtocolSpec_%28UBX-13003221%29_Public.pdf
// It is possible to change the current communications port settings using a UBX-CFG-CFG message. This could
// affect baud rate and other transmission parameters. Because there may be messages queued for transmission
// there may be uncertainty about which protocol applies to such messages. In addition a message currently in
// transmission may be corrupted by a protocol change. Host data reception parameters may have to be changed to
// be able to receive future messages, including the acknowledge message associated with the UBX-CFG-CFG message.
// so the message that changes the baud rate will send it's acknowledgement back at the new baud rate; this is not good.
// if your message was corrupted, you didn't change the baud rate and you have to guess; try pinging at both baud rates.
// also, you would have to change the baud rate instantly after the last byte of the sentence was sent,
// and you would have to poll the port in real time for that, and there may be messages ahead of the baud rate change.
//
// so we ignore the ack from this. it has proven to be reliable (with the addition of two dummy bytes after the packet)
// set the GPS internal baud rate to the user configured value
config_gps_baud(bytes_to_send);
set_current_step_if_untouched(INIT_STEP_REVO_BAUD);
status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
break;
case INIT_STEP_REVO_BAUD:
// wait for previous step
if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_UNVERIFIED_STEP_WAIT_TIME) {
return;
}
// set the Revo GPS port baud rate to the (same) user configured value
config_baud(hwsettings_baud);
status->lastConfigSent = LAST_CONFIG_SENT_START;
status->retryCount = 0;
// skip enabling UBX sentences for low baud rates
// low baud rates are not usable, and higher data rates just makes it harder for this code to change the configuration
if (hwsettings_baud <= HWSETTINGS_GPSSPEED_9600) {
set_current_step_if_untouched(INIT_STEP_SAVE);
} else {
set_current_step_if_untouched(INIT_STEP_ENABLE_SENTENCES);
}
// allow it enter the next state immmediately by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
break;
case INIT_STEP_ENABLE_SENTENCES:
case INIT_STEP_CONFIGURE:
@ -352,64 +786,136 @@ void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send, bool gps_connec
enable_sentences(bytes_to_send);
}
status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
// for some branches, allow it enter the next state immmediately by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
if (status->lastConfigSent == LAST_CONFIG_SENT_COMPLETED) {
status->lastConfigSent = LAST_CONFIG_SENT_START;
status->currentStep = step_configure ? INIT_STEP_DONE : INIT_STEP_CONFIGURE;
if (step_configure) {
// zero retries for the next state that needs it (INIT_STEP_SAVE)
status->retryCount = 0;
set_current_step_if_untouched(INIT_STEP_SAVE);
} else {
status->currentStep = step_configure ? INIT_STEP_CONFIGURE_WAIT_ACK : INIT_STEP_ENABLE_SENTENCES_WAIT_ACK;
// finished enabling sentences, now configure() needs to start at the beginning
status->lastConfigSent = LAST_CONFIG_SENT_START;
set_current_step_if_untouched(INIT_STEP_CONFIGURE);
}
return;
} else {
set_current_step_if_untouched(step_configure ? INIT_STEP_CONFIGURE_WAIT_ACK : INIT_STEP_ENABLE_SENTENCES_WAIT_ACK);
status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
}
break;
}
case INIT_STEP_ENABLE_SENTENCES_WAIT_ACK:
case INIT_STEP_CONFIGURE_WAIT_ACK: // Wait for an ack from GPS
{
bool step_configure = (status->currentStep == INIT_STEP_CONFIGURE_WAIT_ACK);
if (ubxLastAck.clsID == status->requiredAck.clsID && ubxLastAck.msgID == status->requiredAck.msgID) {
// clear ack
ubxLastAck.clsID = 0x00;
ubxLastAck.msgID = 0x00;
// Continue with next configuration option
// start retries over for the next setting to be sent
status->retryCount = 0;
status->lastConfigSent++;
} else if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) > UBX_REPLY_TIMEOUT) {
// timeout, resend the message or abort
} else if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_REPLY_TIMEOUT &&
(ubxLastNak.clsID != status->requiredAck.clsID || ubxLastNak.msgID != status->requiredAck.msgID)) {
// allow timeouts to count up by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
break;
} else {
// timeout or NAK, resend the message or abort
status->retryCount++;
if (status->retryCount > UBX_MAX_RETRIES) {
status->currentStep = INIT_STEP_ERROR;
set_current_step_if_untouched(INIT_STEP_ERROR);
status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
return;
break;
}
}
// no abort condition, continue or retries.,
// success or failure here, retries are handled elsewhere
if (step_configure) {
status->currentStep = INIT_STEP_CONFIGURE;
set_current_step_if_untouched(INIT_STEP_CONFIGURE);
} else {
status->currentStep = INIT_STEP_ENABLE_SENTENCES;
set_current_step_if_untouched(INIT_STEP_ENABLE_SENTENCES);
}
return;
break;
}
case INIT_STEP_SAVE:
if (status->currentSettings.storeSettings) {
config_save(bytes_to_send);
set_current_step_if_untouched(INIT_STEP_SAVE_WAIT_ACK);
status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
} else {
set_current_step_if_untouched(INIT_STEP_DONE);
// allow it enter INIT_STEP_DONE immmediately by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
}
break;
// we could remove this state
// if we retry, it writes to settings storage a few more times
// and it is probably the ack that was dropped, with the save actually performed correctly
case INIT_STEP_SAVE_WAIT_ACK:
if (ubxLastAck.clsID == status->requiredAck.clsID && ubxLastAck.msgID == status->requiredAck.msgID) {
// Continue with next configuration option
set_current_step_if_untouched(INIT_STEP_DONE);
// note that we increase the reply timeout in case the GPS must do a flash erase
} else if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_REPLY_TO_SAVE_TIMEOUT &&
(ubxLastNak.clsID != status->requiredAck.clsID || ubxLastNak.msgID != status->requiredAck.msgID)) {
// allow timeouts to count up by not setting status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
break;
} else {
// timeout or NAK, resend the message or abort
status->retryCount++;
if (status->retryCount > UBX_MAX_RETRIES / 2) {
// give up on the retries
set_current_step_if_untouched(INIT_STEP_ERROR);
status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
} else {
// retry a few times
set_current_step_if_untouched(INIT_STEP_SAVE);
}
}
break;
case INIT_STEP_ERROR:
// on error we should get the GPS version immediately
ubx_reset_sensor_type();
// fall through
case INIT_STEP_DISABLED:
case INIT_STEP_DONE:
break;
}
}
void ubx_autoconfig_set(ubx_autoconfig_settings_t config)
void ubx_autoconfig_set(ubx_autoconfig_settings_t *config)
{
initSteps_t new_step;
enabled = false;
if (config.autoconfigEnabled) {
if (!status) {
status = (status_t *)pios_malloc(sizeof(status_t));
PIOS_Assert(status);
memset(status, 0, sizeof(status_t));
status->currentStep = INIT_STEP_DISABLED;
memset((status_t *)status, 0, sizeof(status_t));
}
status->currentSettings = config;
status->currentStep = INIT_STEP_START;
enabled = true;
// if caller used NULL, just use current settings to restart autoconfig process
if (config != NULL) {
status->currentSettings = *config;
}
if (status->currentSettings.autoconfigEnabled) {
new_step = INIT_STEP_START;
} else {
if (status) {
status->currentStep = INIT_STEP_DISABLED;
new_step = INIT_STEP_DISABLED;
}
// assume this one byte initSteps_t is atomic
// take care of some but not all concurrency issues
status->currentStep = new_step;
status->currentStepSave = new_step;
current_step_touched = true;
status->currentStep = new_step;
status->currentStepSave = new_step;
if (status->currentSettings.autoconfigEnabled) {
enabled = true;
}
}

2
flight/modules/Logging/Logging.c
View File

@ -134,7 +134,7 @@ static void ControlUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
}
DebugLogEntrySet(entry);
} else if (control.Operation == DEBUGLOGCONTROL_OPERATION_FORMATFLASH) {
uint8_t armed;
FlightStatusArmedOptions armed;
FlightStatusArmedGet(&armed);
if (armed == FLIGHTSTATUS_ARMED_DISARMED) {
PIOS_DEBUGLOG_Format();

32
flight/modules/ManualControl/armhandler.c
View File

@ -35,9 +35,13 @@
#include <flightstatus.h>
#include <flightmodesettings.h>
#include <stabilizationdesired.h>
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
#include <statusvtolland.h>
#endif
// Private constants
#define ARMED_THRESHOLD 0.50f
#define GROUND_LOW_THROTTLE 0.01f
// Private types
typedef enum {
@ -62,7 +66,7 @@ static bool forcedDisArm(void);
* @input: ManualControlCommand, AccessoryDesired
* @output: FlightStatus.Arming
*/
void armHandler(bool newinit)
void armHandler(bool newinit, FrameType_t frameType)
{
static ArmState_t armState;
@ -82,6 +86,11 @@ void armHandler(bool newinit)
bool lowThrottle = cmd.Throttle < 0;
if (frameType == FRAME_TYPE_GROUND) {
// Deadbanding applied in receiver.c typically at 2% but we don't assume its enabled.
lowThrottle = fabsf(cmd.Throttle) < GROUND_LOW_THROTTLE;
}
bool armSwitch = false;
switch (settings.Arming) {
@ -173,6 +182,8 @@ void armHandler(bool newinit)
case FLIGHTMODESETTINGS_ARMING_ACCESSORY2:
armingInputLevel = -1.0f * acc.AccessoryVal;
break;
default:
break;
}
bool manualArm = false;
@ -304,6 +315,12 @@ static bool okToArm(void)
return true;
break;
case FLIGHTSTATUS_FLIGHTMODE_LAND:
return false;
case FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF:
case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
return true;
default:
return false;
@ -328,6 +345,19 @@ static bool forcedDisArm(void)
if (alarms.Receiver == SYSTEMALARMS_ALARM_CRITICAL) {
return true;
}
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
// check landing state if active
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if (flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_LAND) {
StatusVtolLandData statusland;
StatusVtolLandGet(&statusland);
if (statusland.State == STATUSVTOLLAND_STATE_DISARMED) {
return true;
}
}
#endif
return false;
}

3
flight/modules/ManualControl/inc/manualcontrol.h
View File

@ -32,6 +32,7 @@
#include <openpilot.h>
#include <flightstatus.h>
#include <sanitycheck.h>
typedef void (*handlerFunc)(bool newinit);
@ -45,7 +46,7 @@ typedef struct controlHandlerStruct {
* @input: ManualControlCommand, AccessoryDesired
* @output: FlightStatus.Arming
*/
void armHandler(bool newinit);
void armHandler(bool newinit, FrameType_t frameType);
/**
* @brief Handler to control Manual flightmode - input directly steers actuators

83
flight/modules/ManualControl/manualcontrol.c
View File

@ -59,8 +59,8 @@
#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL
#define ASSISTEDCONTROL_NEUTRALTHROTTLERANGE_FACTOR 0.2f
#define ASSISTEDCONTROL_BRAKETHRUST_DEADBAND_FACTOR_LO 0.92f
#define ASSISTEDCONTROL_BRAKETHRUST_DEADBAND_FACTOR_HI 1.08f
#define ASSISTEDCONTROL_BRAKETHRUST_DEADBAND_FACTOR_LO 0.96f
#define ASSISTEDCONTROL_BRAKETHRUST_DEADBAND_FACTOR_HI 1.04f
// defined handlers
@ -108,6 +108,7 @@ static float thrustHi = 0.0f;
#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
// Private variables
static DelayedCallbackInfo *callbackHandle;
static FrameType_t frameType = FRAME_TYPE_MULTIROTOR;
// Private functions
static void configurationUpdatedCb(UAVObjEvent *ev);
@ -116,6 +117,7 @@ static void manualControlTask(void);
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
static uint8_t isAssistedFlightMode(uint8_t position, uint8_t flightMode, FlightModeSettingsData *modeSettings);
#endif
static void SettingsUpdatedCb(UAVObjEvent *ev);
#define assumptions (assumptions1 && assumptions2 && assumptions3 && assumptions4 && assumptions5 && assumptions6 && assumptions7 && assumptions_flightmode)
@ -135,11 +137,14 @@ int32_t ManualControlStart()
// clear alarms
AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL);
SettingsUpdatedCb(NULL);
// Make sure unarmed on power up
armHandler(true);
armHandler(true, frameType);
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
takeOffLocationHandlerInit();
#endif
// Start main task
PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle);
@ -164,6 +169,8 @@ int32_t ManualControlInitialize()
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
VtolSelfTuningStatsInitialize();
VtolPathFollowerSettingsInitialize();
VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
SystemSettingsConnectCallback(&SettingsUpdatedCb);
#endif
callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&manualControlTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_MANUALCONTROL, STACK_SIZE_BYTES);
@ -171,13 +178,37 @@ int32_t ManualControlInitialize()
}
MODULE_INITCALL(ManualControlInitialize, ManualControlStart);
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
frameType = GetCurrentFrameType();
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
VtolPathFollowerSettingsTreatCustomCraftAsOptions TreatCustomCraftAs;
VtolPathFollowerSettingsTreatCustomCraftAsGet(&TreatCustomCraftAs);
if (frameType == FRAME_TYPE_CUSTOM) {
switch (TreatCustomCraftAs) {
case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_FIXEDWING:
frameType = FRAME_TYPE_FIXED_WING;
break;
case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_VTOL:
frameType = FRAME_TYPE_MULTIROTOR;
break;
case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_GROUND:
frameType = FRAME_TYPE_GROUND;
break;
}
}
#endif
}
/**
* Module task
*/
static void manualControlTask(void)
{
// Process Arming
armHandler(false);
armHandler(false, frameType);
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
takeOffLocationHandler();
#endif
@ -211,7 +242,7 @@ static void manualControlTask(void)
// set assist mode to none to avoid an assisted flight mode position
// carrying over and impacting a newly selected non-assisted flight mode pos
newFlightModeAssist = FLIGHTSTATUS_FLIGHTMODEASSIST_NONE;
// The following are eqivalent to none effectively. Code should always
// The following are equivalent to none effectively. Code should always
// check the flightmodeassist state.
newAssistedControlState = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY;
newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL;
@ -233,6 +264,16 @@ static void manualControlTask(void)
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
newFlightModeAssist = isAssistedFlightMode(position, newMode, &modeSettings);
if (newFlightModeAssist != flightStatus.FlightModeAssist) {
// On change of assist mode reinitialise control state. This is required
// for the scenario where a flight position change reuses a flight mode
// but adds assistedcontrol.
newAssistedControlState = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY;
newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL;
}
if (newFlightModeAssist) {
// assess roll/pitch state
bool flagRollPitchHasInput = (fabsf(cmd.Roll) > 0.0f || fabsf(cmd.Pitch) > 0.0f);
@ -270,7 +311,7 @@ static void manualControlTask(void)
// retain thrust cmd for later comparison with actual in braking
thrustAtBrakeStart = cmd.Thrust;
// calculate hi and low value of +-8% as a mini-deadband
// calculate hi and low value of +-4% as a mini-deadband
// for use in auto-override in brake sequence
thrustLo = ASSISTEDCONTROL_BRAKETHRUST_DEADBAND_FACTOR_LO * thrustAtBrakeStart;
thrustHi = ASSISTEDCONTROL_BRAKETHRUST_DEADBAND_FACTOR_HI * thrustAtBrakeStart;
@ -366,9 +407,16 @@ static void manualControlTask(void)
break;
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
// During development the assistedcontrol implementation is optional and set
// set in stabi settings. Once if we decide to always have this on, it can
// can be directly set here...i.e. set the flight mode assist as required.
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
case FLIGHTSTATUS_FLIGHTMODE_POSITIONROAM:
case FLIGHTSTATUS_FLIGHTMODE_LAND:
case FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF:
newFlightModeAssist = isAssistedFlightMode(position, newMode, &modeSettings);
if (newFlightModeAssist) {
// Set the default thrust state
switch (newFlightModeAssist) {
case FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST_PRIMARYTHRUST:
newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL;
@ -381,28 +429,14 @@ static void manualControlTask(void)
newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL; // Effectively None
break;
}
switch (newAssistedControlState) {
case FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE:
newAssistedControlState = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE;
break;
case FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY:
newAssistedControlState = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE;
break;
case FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD:
newAssistedControlState = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD;
break;
}
}
handler = &handler_PATHFOLLOWER;
break;
case FLIGHTSTATUS_FLIGHTMODE_COURSELOCK:
case FLIGHTSTATUS_FLIGHTMODE_POSITIONROAM:
case FLIGHTSTATUS_FLIGHTMODE_HOMELEASH:
case FLIGHTSTATUS_FLIGHTMODE_ABSOLUTEPOSITION:
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
case FLIGHTSTATUS_FLIGHTMODE_LAND:
case FLIGHTSTATUS_FLIGHTMODE_POI:
case FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE:
handler = &handler_PATHFOLLOWER;
@ -462,7 +496,7 @@ static uint8_t isAssistedFlightMode(uint8_t position, uint8_t flightMode, Flight
{
uint8_t flightModeAssistOption = STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NONE;
uint8_t isAssistedFlag = FLIGHTSTATUS_FLIGHTMODEASSIST_NONE;
uint8_t FlightModeAssistMap[STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM];
StabilizationSettingsFlightModeAssistMapOptions FlightModeAssistMap[STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM];
StabilizationSettingsFlightModeAssistMapGet(FlightModeAssistMap);
if (position < STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM) {
@ -497,11 +531,16 @@ static uint8_t isAssistedFlightMode(uint8_t position, uint8_t flightMode, Flight
thrustMode = modeSettings->Stabilization6Settings.Thrust;
break;
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
// we hard code the "GPS Assisted" PostionHold to use alt-vario which
case FLIGHTSTATUS_FLIGHTMODE_POSITIONROAM:
// we hard code the "GPS Assisted" PostionHold/Roam to use alt-vario which
// is a more appropriate throttle mode. "GPSAssist" adds braking
// and a better throttle management to the standard Position Hold.
thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO;
break;
case FLIGHTSTATUS_FLIGHTMODE_LAND:
case FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF:
thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL;
break;
// other modes will use cruisecontrol as default
}

37
flight/modules/ManualControl/pathfollowerhandler.c
View File

@ -55,9 +55,11 @@ void pathFollowerHandler(bool newinit)
plan_initialize();
}
uint8_t flightMode;
uint8_t assistedControlFlightMode;
FlightStatusFlightModeOptions flightMode;
FlightStatusAssistedControlStateOptions assistedControlFlightMode;
FlightStatusFlightModeAssistOptions flightModeAssist;
FlightStatusFlightModeGet(&flightMode);
FlightStatusFlightModeAssistGet(&flightModeAssist);
FlightStatusAssistedControlStateGet(&assistedControlFlightMode);
if (newinit) {
@ -67,9 +69,10 @@ void pathFollowerHandler(bool newinit)
plan_setup_returnToBase();
break;
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
if (assistedControlFlightMode == FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE) {
// Just initiated braking after returning from stabi control
plan_setup_assistedcontrol(false);
if ((flightModeAssist != FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) &&
(assistedControlFlightMode == FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY)) {
// Switch from primary (just entered this PH flight mode) into brake
plan_setup_assistedcontrol();
} else {
plan_setup_positionHold();
}
@ -78,7 +81,11 @@ void pathFollowerHandler(bool newinit)
plan_setup_CourseLock();
break;
case FLIGHTSTATUS_FLIGHTMODE_POSITIONROAM:
if (flightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) {
plan_setup_PositionRoam();
} else {
plan_setup_VelocityRoam();
}
break;
case FLIGHTSTATUS_FLIGHTMODE_HOMELEASH:
plan_setup_HomeLeash();
@ -88,7 +95,14 @@ void pathFollowerHandler(bool newinit)
break;
case FLIGHTSTATUS_FLIGHTMODE_LAND:
if (flightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) {
plan_setup_land();
} else {
plan_setup_VelocityRoam();
}
break;
case FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF:
plan_setup_AutoTakeoff();
break;
case FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE:
plan_setup_AutoCruise();
@ -102,7 +116,7 @@ void pathFollowerHandler(bool newinit)
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
if (assistedControlFlightMode == FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE) {
// Just initiated braking after returning from stabi control
plan_setup_assistedcontrol(false);
plan_setup_assistedcontrol();
}
break;
@ -117,7 +131,11 @@ void pathFollowerHandler(bool newinit)
plan_run_CourseLock();
break;
case FLIGHTSTATUS_FLIGHTMODE_POSITIONROAM:
if (flightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) {
plan_run_PositionRoam();
} else {
plan_run_VelocityRoam();
}
break;
case FLIGHTSTATUS_FLIGHTMODE_HOMELEASH:
plan_run_HomeLeash();
@ -126,7 +144,12 @@ void pathFollowerHandler(bool newinit)
plan_run_AbsolutePosition();
break;
case FLIGHTSTATUS_FLIGHTMODE_LAND:
plan_run_land();
if (flightModeAssist != FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) {
plan_run_VelocityRoam();
}
break;
case FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF:
plan_run_AutoTakeoff();
break;
case FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE:
plan_run_AutoCruise();

17
flight/modules/ManualControl/stabilizedhandler.c
View File

@ -96,33 +96,34 @@ void stabilizedHandler(bool newinit)
switch (flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
stab_settings = FlightModeSettingsStabilization1SettingsToArray(settings.Stabilization1Settings);
stab_settings = (uint8_t *)FlightModeSettingsStabilization1SettingsToArray(settings.Stabilization1Settings);
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
stab_settings = FlightModeSettingsStabilization2SettingsToArray(settings.Stabilization2Settings);
stab_settings = (uint8_t *)FlightModeSettingsStabilization2SettingsToArray(settings.Stabilization2Settings);
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
stab_settings = FlightModeSettingsStabilization3SettingsToArray(settings.Stabilization3Settings);
stab_settings = (uint8_t *)FlightModeSettingsStabilization3SettingsToArray(settings.Stabilization3Settings);
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4:
stab_settings = FlightModeSettingsStabilization4SettingsToArray(settings.Stabilization4Settings);
stab_settings = (uint8_t *)FlightModeSettingsStabilization4SettingsToArray(settings.Stabilization4Settings);
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5:
stab_settings = FlightModeSettingsStabilization5SettingsToArray(settings.Stabilization5Settings);
stab_settings = (uint8_t *)FlightModeSettingsStabilization5SettingsToArray(settings.Stabilization5Settings);
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
stab_settings = FlightModeSettingsStabilization6SettingsToArray(settings.Stabilization6Settings);
stab_settings = (uint8_t *)FlightModeSettingsStabilization6SettingsToArray(settings.Stabilization6Settings);
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);
stab_settings = FlightModeSettingsStabilization1SettingsToArray(settings.Stabilization1Settings);
stab_settings = (uint8_t *)FlightModeSettingsStabilization1SettingsToArray(settings.Stabilization1Settings);
return;
}
stabilization.Roll =
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) ? cmd.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Roll * stabSettings.ManualRate.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATETRAINER) ? cmd.Roll * stabSettings.ManualRate.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Roll * stabSettings.RollMax :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Roll * stabSettings.RollMax :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Roll * stabSettings.ManualRate.Roll :
@ -134,6 +135,7 @@ void stabilizedHandler(bool newinit)
stabilization.Pitch =
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) ? cmd.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATETRAINER) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Pitch * stabSettings.PitchMax :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Pitch * stabSettings.PitchMax :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
@ -155,6 +157,7 @@ void stabilizedHandler(bool newinit)
stabilization.Yaw =
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) ? cmd.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATETRAINER) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Yaw * stabSettings.YawMax :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Yaw * stabSettings.YawMax :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Yaw * stabSettings.ManualRate.Yaw :

12
flight/modules/ManualControl/takeofflocationhandler.c
View File

@ -42,8 +42,8 @@ void takeOffLocationHandlerInit()
{
TakeOffLocationInitialize();
// check whether there is a preset/valid takeoff location
uint8_t mode;
uint8_t status;
TakeOffLocationModeOptions mode;
TakeOffLocationStatusOptions status;
TakeOffLocationModeGet(&mode);
TakeOffLocationStatusGet(&status);
// preset with invalid location will actually behave like FirstTakeoff
@ -61,8 +61,8 @@ void takeOffLocationHandlerInit()
*/
void takeOffLocationHandler()
{
uint8_t armed;
uint8_t status;
FlightStatusArmedOptions armed;
TakeOffLocationStatusOptions status;
FlightStatusArmedGet(&armed);
@ -77,7 +77,7 @@ void takeOffLocationHandler()
case FLIGHTSTATUS_ARMED_ARMING:
case FLIGHTSTATUS_ARMED_ARMED:
if (!locationSet || status != TAKEOFFLOCATION_STATUS_VALID) {
uint8_t mode;
TakeOffLocationModeOptions mode;
TakeOffLocationModeGet(&mode);
if ((mode != TAKEOFFLOCATION_MODE_PRESET) || (status == TAKEOFFLOCATION_STATUS_INVALID)) {
@ -91,7 +91,7 @@ void takeOffLocationHandler()
case FLIGHTSTATUS_ARMED_DISARMED:
// unset if location is to be acquired at each arming
if (locationSet) {
uint8_t mode;
TakeOffLocationModeOptions mode;
TakeOffLocationModeGet(&mode);
if (mode == TAKEOFFLOCATION_MODE_ARMINGLOCATION) {
locationSet = false;

618
flight/modules/PathFollower/fixedwingflycontroller.cpp Normal file
View File

@ -0,0 +1,618 @@
/*
******************************************************************************
*
* @file FixedWingFlyController.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Fixed wing fly controller implementation
* @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
*/
extern "C" {
#include <openpilot.h>
#include <callbackinfo.h>
#include <math.h>
#include <pid.h>
#include <CoordinateConversions.h>
#include <sin_lookup.h>
#include <pathdesired.h>
#include <paths.h>
#include "plans.h"
#include <sanitycheck.h>
#include <homelocation.h>
#include <accelstate.h>
#include <fixedwingpathfollowersettings.h>
#include <fixedwingpathfollowerstatus.h>
#include <flightstatus.h>
#include <flightmodesettings.h>
#include <pathstatus.h>
#include <positionstate.h>
#include <velocitystate.h>
#include <velocitydesired.h>
#include <stabilizationdesired.h>
#include <airspeedstate.h>
#include <attitudestate.h>
#include <takeofflocation.h>
#include <poilocation.h>
#include <manualcontrolcommand.h>
#include <systemsettings.h>
#include <stabilizationbank.h>
#include <stabilizationdesired.h>
#include <vtolselftuningstats.h>
#include <pathsummary.h>
}
// C++ includes
#include "fixedwingflycontroller.h"
// Private constants
// pointer to a singleton instance
FixedWingFlyController *FixedWingFlyController::p_inst = 0;
FixedWingFlyController::FixedWingFlyController()
: fixedWingSettings(NULL), mActive(false), mMode(0), indicatedAirspeedStateBias(0.0f)
{}
// Called when mode first engaged
void FixedWingFlyController::Activate(void)
{
if (!mActive) {
mActive = true;
SettingsUpdated();
resetGlobals();
mMode = pathDesired->Mode;
}
}
uint8_t FixedWingFlyController::IsActive(void)
{
return mActive;
}
uint8_t FixedWingFlyController::Mode(void)
{
return mMode;
}
// Objective updated in pathdesired
void FixedWingFlyController::ObjectiveUpdated(void)
{}
void FixedWingFlyController::Deactivate(void)
{
if (mActive) {
mActive = false;
resetGlobals();
}
}
void FixedWingFlyController::SettingsUpdated(void)
{
// fixed wing PID only
pid_configure(&PIDposH[0], fixedWingSettings->HorizontalPosP, 0.0f, 0.0f, 0.0f);
pid_configure(&PIDposH[1], fixedWingSettings->HorizontalPosP, 0.0f, 0.0f, 0.0f);
pid_configure(&PIDposV, fixedWingSettings->VerticalPosP, 0.0f, 0.0f, 0.0f);
pid_configure(&PIDcourse, fixedWingSettings->CoursePI.Kp, fixedWingSettings->CoursePI.Ki, 0.0f, fixedWingSettings->CoursePI.ILimit);
pid_configure(&PIDspeed, fixedWingSettings->SpeedPI.Kp, fixedWingSettings->SpeedPI.Ki, 0.0f, fixedWingSettings->SpeedPI.ILimit);
pid_configure(&PIDpower, fixedWingSettings->PowerPI.Kp, fixedWingSettings->PowerPI.Ki, 0.0f, fixedWingSettings->PowerPI.ILimit);
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t FixedWingFlyController::Initialize(FixedWingPathFollowerSettingsData *ptr_fixedWingSettings)
{
PIOS_Assert(ptr_fixedWingSettings);
fixedWingSettings = ptr_fixedWingSettings;
resetGlobals();
return 0;
}
/**
* reset integrals
*/
void FixedWingFlyController::resetGlobals()
{
pid_zero(&PIDposH[0]);
pid_zero(&PIDposH[1]);
pid_zero(&PIDposV);
pid_zero(&PIDcourse);
pid_zero(&PIDspeed);
pid_zero(&PIDpower);
pathStatus->path_time = 0.0f;
}
void FixedWingFlyController::UpdateAutoPilot()
{
uint8_t result = updateAutoPilotFixedWing();
if (result) {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
} else {
pathStatus->Status = PATHSTATUS_STATUS_CRITICAL;
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
}
PathStatusSet(pathStatus);
}
/**
* fixed wing autopilot:
* straight forward:
* 1. update path velocity for limited motion crafts
* 2. update attitude according to default fixed wing pathfollower algorithm
*/
uint8_t FixedWingFlyController::updateAutoPilotFixedWing()
{
updatePathVelocity(fixedWingSettings->CourseFeedForward, true);
return updateFixedDesiredAttitude();
}
/**
* Compute desired velocity from the current position and path
*/
void FixedWingFlyController::updatePathVelocity(float kFF, bool limited)
{
PositionStateData positionState;
PositionStateGet(&positionState);
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
VelocityDesiredData velocityDesired;
const float dT = fixedWingSettings->UpdatePeriod / 1000.0f;
// look ahead kFF seconds
float cur[3] = { positionState.North + (velocityState.North * kFF),
positionState.East + (velocityState.East * kFF),
positionState.Down + (velocityState.Down * kFF) };
struct path_status progress;
path_progress(pathDesired, cur, &progress, true);
// calculate velocity - can be zero if waypoints are too close
velocityDesired.North = progress.path_vector[0];
velocityDesired.East = progress.path_vector[1];
velocityDesired.Down = progress.path_vector[2];
if (limited &&
// if a plane is crossing its desired flightpath facing the wrong way (away from flight direction)
// it would turn towards the flightpath to get on its desired course. This however would reverse the correction vector
// once it crosses the flightpath again, which would make it again turn towards the flightpath (but away from its desired heading)
// leading to an S-shape snake course the wrong way
// this only happens especially if HorizontalPosP is too high, as otherwise the angle between velocity desired and path_direction won't
// turn steep unless there is enough space complete the turn before crossing the flightpath
// in this case the plane effectively needs to be turned around
// indicators:
// difference between correction_direction and velocitystate >90 degrees and
// difference between path_direction and velocitystate >90 degrees ( 4th sector, facing away from everything )
// fix: ignore correction, steer in path direction until the situation has become better (condition doesn't apply anymore)
// calculating angles < 90 degrees through dot products
(vector_lengthf(progress.path_vector, 2) > 1e-6f) &&
((progress.path_vector[0] * velocityState.North + progress.path_vector[1] * velocityState.East) < 0.0f) &&
((progress.correction_vector[0] * velocityState.North + progress.correction_vector[1] * velocityState.East) < 0.0f)) {
;
} else {
// calculate correction
velocityDesired.North += pid_apply(&PIDposH[0], progress.correction_vector[0], dT);
velocityDesired.East += pid_apply(&PIDposH[1], progress.correction_vector[1], dT);
}
velocityDesired.Down += pid_apply(&PIDposV, progress.correction_vector[2], dT);
// update pathstatus
pathStatus->error = progress.error;
pathStatus->fractional_progress = progress.fractional_progress;
pathStatus->path_direction_north = progress.path_vector[0];
pathStatus->path_direction_east = progress.path_vector[1];
pathStatus->path_direction_down = progress.path_vector[2];
pathStatus->correction_direction_north = progress.correction_vector[0];
pathStatus->correction_direction_east = progress.correction_vector[1];
pathStatus->correction_direction_down = progress.correction_vector[2];
VelocityDesiredSet(&velocityDesired);
}
/**
* Compute desired attitude from the desired velocity for fixed wing craft
*/
uint8_t FixedWingFlyController::updateFixedDesiredAttitude()
{
uint8_t result = 1;
const float dT = fixedWingSettings->UpdatePeriod / 1000.0f;
VelocityDesiredData velocityDesired;
VelocityStateData velocityState;
StabilizationDesiredData stabDesired;
AttitudeStateData attitudeState;
FixedWingPathFollowerStatusData fixedWingPathFollowerStatus;
AirspeedStateData airspeedState;
SystemSettingsData systemSettings;
float groundspeedProjection;
float indicatedAirspeedState;
float indicatedAirspeedDesired;
float airspeedError;
float pitchCommand;
float descentspeedDesired;
float descentspeedError;
float powerCommand;
float airspeedVector[2];
float fluidMovement[2];
float courseComponent[2];
float courseError;
float courseCommand;
FixedWingPathFollowerStatusGet(&fixedWingPathFollowerStatus);
VelocityStateGet(&velocityState);
StabilizationDesiredGet(&stabDesired);
VelocityDesiredGet(&velocityDesired);
AttitudeStateGet(&attitudeState);
AirspeedStateGet(&airspeedState);
SystemSettingsGet(&systemSettings);
/**
* Compute speed error and course
*/
// missing sensors for airspeed-direction we have to assume within
// reasonable error that measured airspeed is actually the airspeed
// component in forward pointing direction
// airspeedVector is normalized
airspeedVector[0] = cos_lookup_deg(attitudeState.Yaw);
airspeedVector[1] = sin_lookup_deg(attitudeState.Yaw);
// current ground speed projected in forward direction
groundspeedProjection = velocityState.North * airspeedVector[0] + velocityState.East * airspeedVector[1];
// note that airspeedStateBias is ( calibratedAirspeed - groundspeedProjection ) at the time of measurement,
// but thanks to accelerometers, groundspeedProjection reacts faster to changes in direction
// than airspeed and gps sensors alone
indicatedAirspeedState = groundspeedProjection + indicatedAirspeedStateBias;
// fluidMovement is a vector describing the aproximate movement vector of
// the surrounding fluid in 2d space (aka wind vector)
fluidMovement[0] = velocityState.North - (indicatedAirspeedState * airspeedVector[0]);
fluidMovement[1] = velocityState.East - (indicatedAirspeedState * airspeedVector[1]);
// calculate the movement vector we need to fly to reach velocityDesired -
// taking fluidMovement into account
courseComponent[0] = velocityDesired.North - fluidMovement[0];
courseComponent[1] = velocityDesired.East - fluidMovement[1];
indicatedAirspeedDesired = boundf(sqrtf(courseComponent[0] * courseComponent[0] + courseComponent[1] * courseComponent[1]),
fixedWingSettings->HorizontalVelMin,
fixedWingSettings->HorizontalVelMax);
// if we could fly at arbitrary speeds, we'd just have to move towards the
// courseComponent vector as previously calculated and we'd be fine
// unfortunately however we are bound by min and max air speed limits, so
// we need to recalculate the correct course to meet at least the
// velocityDesired vector direction at our current speed
// this overwrites courseComponent
bool valid = correctCourse(courseComponent, (float *)&velocityDesired.North, fluidMovement, indicatedAirspeedDesired);
// Error condition: wind speed too high, we can't go where we want anymore
fixedWingPathFollowerStatus.Errors.Wind = 0;
if ((!valid) &&
fixedWingSettings->Safetymargins.Wind > 0.5f) { // alarm switched on
fixedWingPathFollowerStatus.Errors.Wind = 1;
result = 0;
}
// Airspeed error
airspeedError = indicatedAirspeedDesired - indicatedAirspeedState;
// Vertical speed error
descentspeedDesired = boundf(
velocityDesired.Down,
-fixedWingSettings->VerticalVelMax,
fixedWingSettings->VerticalVelMax);
descentspeedError = descentspeedDesired - velocityState.Down;
// Error condition: plane too slow or too fast
fixedWingPathFollowerStatus.Errors.Highspeed = 0;
fixedWingPathFollowerStatus.Errors.Lowspeed = 0;
if (indicatedAirspeedState > systemSettings.AirSpeedMax * fixedWingSettings->Safetymargins.Overspeed) {
fixedWingPathFollowerStatus.Errors.Overspeed = 1;
result = 0;
}
if (indicatedAirspeedState > fixedWingSettings->HorizontalVelMax * fixedWingSettings->Safetymargins.Highspeed) {
fixedWingPathFollowerStatus.Errors.Highspeed = 1;
result = 0;
}
if (indicatedAirspeedState < fixedWingSettings->HorizontalVelMin * fixedWingSettings->Safetymargins.Lowspeed) {
fixedWingPathFollowerStatus.Errors.Lowspeed = 1;
result = 0;
}
if (indicatedAirspeedState < systemSettings.AirSpeedMin * fixedWingSettings->Safetymargins.Stallspeed) {
fixedWingPathFollowerStatus.Errors.Stallspeed = 1;
result = 0;
}
/**
* Compute desired thrust command
*/
// Compute the cross feed from vertical speed to pitch, with saturation
float speedErrorToPowerCommandComponent = boundf(
(airspeedError / fixedWingSettings->HorizontalVelMin) * fixedWingSettings->AirspeedToPowerCrossFeed.Kp,
-fixedWingSettings->AirspeedToPowerCrossFeed.Max,
fixedWingSettings->AirspeedToPowerCrossFeed.Max
);
// Compute final thrust response
powerCommand = pid_apply(&PIDpower, -descentspeedError, dT) +
speedErrorToPowerCommandComponent;
// Output internal state to telemetry
fixedWingPathFollowerStatus.Error.Power = descentspeedError;
fixedWingPathFollowerStatus.ErrorInt.Power = PIDpower.iAccumulator;
fixedWingPathFollowerStatus.Command.Power = powerCommand;
// set thrust
stabDesired.Thrust = boundf(fixedWingSettings->ThrustLimit.Neutral + powerCommand,
fixedWingSettings->ThrustLimit.Min,
fixedWingSettings->ThrustLimit.Max);
// Error condition: plane cannot hold altitude at current speed.
fixedWingPathFollowerStatus.Errors.Lowpower = 0;
if (fixedWingSettings->ThrustLimit.Neutral + powerCommand >= fixedWingSettings->ThrustLimit.Max && // thrust at maximum
velocityState.Down > 0.0f && // we ARE going down
descentspeedDesired < 0.0f && // we WANT to go up
airspeedError > 0.0f && // we are too slow already
fixedWingSettings->Safetymargins.Lowpower > 0.5f) { // alarm switched on
fixedWingPathFollowerStatus.Errors.Lowpower = 1;
result = 0;
}
// Error condition: plane keeps climbing despite minimum thrust (opposite of above)
fixedWingPathFollowerStatus.Errors.Highpower = 0;
if (fixedWingSettings->ThrustLimit.Neutral + powerCommand <= fixedWingSettings->ThrustLimit.Min && // thrust at minimum
velocityState.Down < 0.0f && // we ARE going up
descentspeedDesired > 0.0f && // we WANT to go down
airspeedError < 0.0f && // we are too fast already
fixedWingSettings->Safetymargins.Highpower > 0.5f) { // alarm switched on
fixedWingPathFollowerStatus.Errors.Highpower = 1;
result = 0;
}
/**
* Compute desired pitch command
*/
// Compute the cross feed from vertical speed to pitch, with saturation
float verticalSpeedToPitchCommandComponent = boundf(-descentspeedError * fixedWingSettings->VerticalToPitchCrossFeed.Kp,
-fixedWingSettings->VerticalToPitchCrossFeed.Max,
fixedWingSettings->VerticalToPitchCrossFeed.Max
);
// Compute the pitch command as err*Kp + errInt*Ki + X_feed.
pitchCommand = -pid_apply(&PIDspeed, airspeedError, dT) + verticalSpeedToPitchCommandComponent;
fixedWingPathFollowerStatus.Error.Speed = airspeedError;
fixedWingPathFollowerStatus.ErrorInt.Speed = PIDspeed.iAccumulator;
fixedWingPathFollowerStatus.Command.Speed = pitchCommand;
stabDesired.Pitch = boundf(fixedWingSettings->PitchLimit.Neutral + pitchCommand,
fixedWingSettings->PitchLimit.Min,
fixedWingSettings->PitchLimit.Max);
// Error condition: high speed dive
fixedWingPathFollowerStatus.Errors.Pitchcontrol = 0;
if (fixedWingSettings->PitchLimit.Neutral + pitchCommand >= fixedWingSettings->PitchLimit.Max && // pitch demand is full up
velocityState.Down > 0.0f && // we ARE going down
descentspeedDesired < 0.0f && // we WANT to go up
airspeedError < 0.0f && // we are too fast already
fixedWingSettings->Safetymargins.Pitchcontrol > 0.5f) { // alarm switched on
fixedWingPathFollowerStatus.Errors.Pitchcontrol = 1;
result = 0;
}
/**
* Compute desired roll command
*/
courseError = RAD2DEG(atan2f(courseComponent[1], courseComponent[0])) - attitudeState.Yaw;
if (courseError < -180.0f) {
courseError += 360.0f;
}
if (courseError > 180.0f) {
courseError -= 360.0f;
}
// overlap calculation. Theres a dead zone behind the craft where the
// counter-yawing of some craft while rolling could render a desired right
// turn into a desired left turn. Making the turn direction based on
// current roll angle keeps the plane committed to a direction once chosen
if (courseError < -180.0f + (fixedWingSettings->ReverseCourseOverlap * 0.5f)
&& attitudeState.Roll > 0.0f) {
courseError += 360.0f;
}
if (courseError > 180.0f - (fixedWingSettings->ReverseCourseOverlap * 0.5f)
&& attitudeState.Roll < 0.0f) {
courseError -= 360.0f;
}
courseCommand = pid_apply(&PIDcourse, courseError, dT);
fixedWingPathFollowerStatus.Error.Course = courseError;
fixedWingPathFollowerStatus.ErrorInt.Course = PIDcourse.iAccumulator;
fixedWingPathFollowerStatus.Command.Course = courseCommand;
stabDesired.Roll = boundf(fixedWingSettings->RollLimit.Neutral +
courseCommand,
fixedWingSettings->RollLimit.Min,
fixedWingSettings->RollLimit.Max);
// TODO: find a check to determine loss of directional control. Likely needs some check of derivative
/**
* Compute desired yaw command
*/
// TODO implement raw control mode for yaw and base on Accels.Y
stabDesired.Yaw = 0.0f;
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
StabilizationDesiredSet(&stabDesired);
FixedWingPathFollowerStatusSet(&fixedWingPathFollowerStatus);
return result;
}
/**
* Function to calculate course vector C based on airspeed s, fluid movement F
* and desired movement vector V
* parameters in: V,F,s
* parameters out: C
* returns true if a valid solution could be found for V,F,s, false if not
* C will be set to a best effort attempt either way
*/
bool FixedWingFlyController::correctCourse(float *C, float *V, float *F, float s)
{
// Approach:
// Let Sc be a circle around origin marking possible movement vectors
// of the craft with airspeed s (all possible options for C)
// Let Vl be a line through the origin along movement vector V where fr any
// point v on line Vl v = k * (V / |V|) = k' * V
// Let Wl be a line parallel to Vl where for any point v on line Vl exists
// a point w on WL with w = v - F
// Then any intersection between circle Sc and line Wl represents course
// vector which would result in a movement vector
// V' = k * ( V / |V|) = k' * V
// If there is no intersection point, S is insufficient to compensate
// for F and we can only try to fly in direction of V (thus having wind drift
// but at least making progress orthogonal to wind)
s = fabsf(s);
float f = vector_lengthf(F, 2);
// normalize Cn=V/|V|, |V| must be >0
float v = vector_lengthf(V, 2);
if (v < 1e-6f) {
// if |V|=0, we aren't supposed to move, turn into the wind
// (this allows hovering)
C[0] = -F[0];
C[1] = -F[1];
// if desired airspeed matches fluidmovement a hover is actually
// intended so return true
return fabsf(f - s) < 1e-3f;
}
float Vn[2] = { V[0] / v, V[1] / v };
// project F on V
float fp = F[0] * Vn[0] + F[1] * Vn[1];
// find component Fo of F that is orthogonal to V
// (which is exactly the distance between Vl and Wl)
float Fo[2] = { F[0] - (fp * Vn[0]), F[1] - (fp * Vn[1]) };
float fo2 = Fo[0] * Fo[0] + Fo[1] * Fo[1];
// find k where k * Vn = C - Fo
// |C|=s is the hypothenuse in any rectangular triangle formed by k * Vn and Fo
// so k^2 + fo^2 = s^2 (since |Vn|=1)
float k2 = s * s - fo2;
if (k2 <= -1e-3f) {
// there is no solution, we will be drifted off either way
// fallback: fly stupidly in direction of V and hope for the best
C[0] = V[0];
C[1] = V[1];
return false;
} else if (k2 <= 1e-3f) {
// there is exactly one solution: -Fo
C[0] = -Fo[0];
C[1] = -Fo[1];
return true;
}
// we have two possible solutions k positive and k negative as there are
// two intersection points between Wl and Sc
// which one is better? two criteria:
// 1. we MUST move in the right direction, if any k leads to -v its invalid
// 2. we should minimize the speed error
float k = sqrt(k2);
float C1[2] = { -k * Vn[0] - Fo[0], -k * Vn[1] - Fo[1] };
float C2[2] = { k *Vn[0] - Fo[0], k * Vn[1] - Fo[1] };
// project C+F on Vn to find signed resulting movement vector length
float vp1 = (C1[0] + F[0]) * Vn[0] + (C1[1] + F[1]) * Vn[1];
float vp2 = (C2[0] + F[0]) * Vn[0] + (C2[1] + F[1]) * Vn[1];
if (vp1 >= 0.0f && fabsf(v - vp1) < fabsf(v - vp2)) {
// in this case the angle between course and resulting movement vector
// is greater than 90 degrees - so we actually fly backwards
C[0] = C1[0];
C[1] = C1[1];
return true;
}
C[0] = C2[0];
C[1] = C2[1];
if (vp2 >= 0.0f) {
// in this case the angle between course and movement vector is less than
// 90 degrees, but we do move in the right direction
return true;
} else {
// in this case we actually get driven in the opposite direction of V
// with both solutions for C
// this might be reached in headwind stronger than maximum allowed
// airspeed.
return false;
}
}
void FixedWingFlyController::AirspeedStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
AirspeedStateData airspeedState;
VelocityStateData velocityState;
AirspeedStateGet(&airspeedState);
VelocityStateGet(&velocityState);
float airspeedVector[2];
float yaw;
AttitudeStateYawGet(&yaw);
airspeedVector[0] = cos_lookup_deg(yaw);
airspeedVector[1] = sin_lookup_deg(yaw);
// vector projection of groundspeed on airspeed vector to handle both forward and backwards movement
float groundspeedProjection = velocityState.North * airspeedVector[0] + velocityState.East * airspeedVector[1];
indicatedAirspeedStateBias = airspeedState.CalibratedAirspeed - groundspeedProjection;
// note - we do fly by Indicated Airspeed (== calibrated airspeed) however
// since airspeed is updated less often than groundspeed, we use sudden
// changes to groundspeed to offset the airspeed by the same measurement.
// This has a side effect that in the absence of any airspeed updates, the
// pathfollower will fly using groundspeed.
}

303
flight/modules/PathFollower/grounddrivecontroller.cpp Normal file
View File

@ -0,0 +1,303 @@
/*
******************************************************************************
*
* @file grounddrivecontroller.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Ground drive controller
* the required PathDesired LAND 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
*/
extern "C" {
#include <openpilot.h>
#include <callbackinfo.h>
#include <math.h>
#include <pid.h>
#include <CoordinateConversions.h>
#include <sin_lookup.h>
#include <pathdesired.h>
#include <paths.h>
#include "plans.h"
#include <sanitycheck.h>
#include <homelocation.h>
#include <accelstate.h>
#include <groundpathfollowersettings.h>
#include <flightstatus.h>
#include <flightmodesettings.h>
#include <pathstatus.h>
#include <positionstate.h>
#include <velocitystate.h>
#include <velocitydesired.h>
#include <stabilizationdesired.h>
#include <airspeedstate.h>
#include <attitudestate.h>
#include <takeofflocation.h>
#include <poilocation.h>
#include <manualcontrolcommand.h>
#include <systemsettings.h>
#include <stabilizationbank.h>
#include <stabilizationdesired.h>
#include <pathsummary.h>
#include <statusgrounddrive.h>
}
// C++ includes
#include "grounddrivecontroller.h"
// Private constants
// pointer to a singleton instance
GroundDriveController *GroundDriveController::p_inst = 0;
GroundDriveController::GroundDriveController()
: groundSettings(0), mActive(false), mMode(0)
{}
// Called when mode first engaged
void GroundDriveController::Activate(void)
{
if (!mActive) {
mActive = true;
SettingsUpdated();
controlNE.Activate();
mMode = pathDesired->Mode;
}
}
uint8_t GroundDriveController::IsActive(void)
{
return mActive;
}
uint8_t GroundDriveController::Mode(void)
{
return mMode;
}
// Objective updated in pathdesired
void GroundDriveController::ObjectiveUpdated(void)
{}
void GroundDriveController::Deactivate(void)
{
if (mActive) {
mActive = false;
controlNE.Deactivate();
}
}
void GroundDriveController::SettingsUpdated(void)
{
const float dT = groundSettings->UpdatePeriod / 1000.0f;
controlNE.UpdatePositionalParameters(groundSettings->HorizontalPosP);
controlNE.UpdateParameters(groundSettings->SpeedPI.Kp,
groundSettings->SpeedPI.Ki,
groundSettings->SpeedPI.Kd,
groundSettings->SpeedPI.Beta,
dT,
groundSettings->HorizontalVelMax);
// max/min are NE command values equivalent to thrust but must be symmetrical as this is NE not forward/reverse.
controlNE.UpdateCommandParameters(-groundSettings->ThrustLimit.Max, groundSettings->ThrustLimit.Max, groundSettings->VelocityFeedForward);
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t GroundDriveController::Initialize(GroundPathFollowerSettingsData *ptr_groundSettings)
{
PIOS_Assert(ptr_groundSettings);
groundSettings = ptr_groundSettings;
return 0;
}
void GroundDriveController::UpdateAutoPilot()
{
uint8_t result = updateAutoPilotGround();
if (result) {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
} else {
pathStatus->Status = PATHSTATUS_STATUS_CRITICAL;
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
}
PathStatusSet(pathStatus);
}
/**
* fixed wing autopilot:
* straight forward:
* 1. update path velocity for limited motion crafts
* 2. update attitude according to default fixed wing pathfollower algorithm
*/
uint8_t GroundDriveController::updateAutoPilotGround()
{
updatePathVelocity(groundSettings->CourseFeedForward);
return updateGroundDesiredAttitude();
}
/**
* Compute desired velocity from the current position and path
*/
void GroundDriveController::updatePathVelocity(float kFF)
{
PositionStateData positionState;
PositionStateGet(&positionState);
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
VelocityDesiredData velocityDesired;
controlNE.UpdateVelocityState(velocityState.North, velocityState.East);
// look ahead kFF seconds
float cur[3] = { positionState.North + (velocityState.North * kFF),
positionState.East + (velocityState.East * kFF),
positionState.Down + (velocityState.Down * kFF) };
struct path_status progress;
path_progress(pathDesired, cur, &progress, false);
// GOTOENDPOINT: correction_vector is distance array to endpoint, path_vector is velocity vector
// FOLLOWVECTOR: correct_vector is distance to vector path, path_vector is the desired velocity vector
// Calculate the desired velocity from the lateral vector path errors (correct_vector) and the desired velocity vector (path_vector)
controlNE.ControlPositionWithPath(&progress);
float north, east;
controlNE.GetVelocityDesired(&north, &east);
velocityDesired.North = north;
velocityDesired.East = east;
velocityDesired.Down = 0.0f;
// update pathstatus
pathStatus->error = progress.error;
pathStatus->fractional_progress = progress.fractional_progress;
// FOLLOWVECTOR: desired velocity vector
pathStatus->path_direction_north = progress.path_vector[0];
pathStatus->path_direction_east = progress.path_vector[1];
pathStatus->path_direction_down = progress.path_vector[2];
// FOLLOWVECTOR: correction distance to vector path
pathStatus->correction_direction_north = progress.correction_vector[0];
pathStatus->correction_direction_east = progress.correction_vector[1];
pathStatus->correction_direction_down = progress.correction_vector[2];
VelocityDesiredSet(&velocityDesired);
}
/**
* Compute desired attitude for ground vehicles
*/
uint8_t GroundDriveController::updateGroundDesiredAttitude()
{
StatusGroundDriveData statusGround;
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
AttitudeStateData attitudeState;
AttitudeStateGet(&attitudeState);
statusGround.State.Yaw = attitudeState.Yaw;
statusGround.State.Velocity = sqrtf(velocityState.North * velocityState.North + velocityState.East * velocityState.East);
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
// estimate a north/east command value to control the velocity error.
// ThrustLimits.Max(+-) limits the range. Think of this as a command unit vector
// of the ultimate direction to reduce lateral error and achieve the target direction (desired angle).
float northCommand, eastCommand;
controlNE.GetNECommand(&northCommand, &eastCommand);
// Get current vehicle orientation
float angle_radians = DEG2RAD(attitudeState.Yaw); // (+-pi)
float cos_angle = cosf(angle_radians);
float sine_angle = sinf(angle_radians);
float courseCommand = 0.0f;
float speedCommand = 0.0f;
float lateralCommand = boundf(-northCommand * sine_angle + eastCommand * cos_angle, -groundSettings->ThrustLimit.Max, groundSettings->ThrustLimit.Max);
float forwardCommand = boundf(northCommand * cos_angle + eastCommand * sine_angle, -groundSettings->ThrustLimit.Max, groundSettings->ThrustLimit.Max);
// +ve facing correct direction, lateral command should just correct angle,
if (forwardCommand > 0.0f) {
// if +ve forward command, -+ lateralCommand drives steering to manage lateral error and angular error
courseCommand = boundf(lateralCommand, -1.0f, 1.0f);
speedCommand = boundf(forwardCommand, groundSettings->ThrustLimit.SlowForward, groundSettings->ThrustLimit.Max);
// reinstate max thrust
controlNE.UpdateCommandParameters(-groundSettings->ThrustLimit.Max, groundSettings->ThrustLimit.Max, groundSettings->VelocityFeedForward);
statusGround.ControlState = STATUSGROUNDDRIVE_CONTROLSTATE_ONTRACK;
} else {
// -ve facing opposite direction, lateral command irrelevant, need to turn to change direction and do so slowly.
// Reduce steering angle based on current velocity
float steeringReductionFactor = 1.0f;
if (stabDesired.Thrust > 0.3f) {
steeringReductionFactor = (groundSettings->HorizontalVelMax - statusGround.State.Velocity) / groundSettings->HorizontalVelMax;
steeringReductionFactor = boundf(steeringReductionFactor, 0.05f, 1.0f);
}
// should we turn left or right?
if (lateralCommand >= 0.1f) {
courseCommand = 1.0f * steeringReductionFactor;
statusGround.ControlState = STATUSGROUNDDRIVE_CONTROLSTATE_TURNAROUNDRIGHT;
} else {
courseCommand = -1.0f * steeringReductionFactor;
statusGround.ControlState = STATUSGROUNDDRIVE_CONTROLSTATE_TURNAROUNDLEFT;
}
// Impose limits to slow down.
controlNE.UpdateCommandParameters(-groundSettings->ThrustLimit.SlowForward, groundSettings->ThrustLimit.SlowForward, 0.0f);
speedCommand = groundSettings->ThrustLimit.SlowForward;
}
stabDesired.Roll = 0.0f;
stabDesired.Pitch = 0.0f;
stabDesired.Yaw = courseCommand;
// Mix yaw into thrust limit TODO
stabDesired.Thrust = boundf(speedCommand, groundSettings->ThrustLimit.Min, groundSettings->ThrustLimit.Max);
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
StabilizationDesiredSet(&stabDesired);
statusGround.NECommand.North = northCommand;
statusGround.NECommand.East = eastCommand;
statusGround.State.Thrust = stabDesired.Thrust;
statusGround.BodyCommand.Forward = forwardCommand;
statusGround.BodyCommand.Right = lateralCommand;
statusGround.ControlCommand.Course = courseCommand;
statusGround.ControlCommand.Speed = speedCommand;
StatusGroundDriveSet(&statusGround);
return 1;
}

80
flight/modules/PathFollower/inc/fixedwingflycontroller.h Normal file
View File

@ -0,0 +1,80 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup FixedWing CONTROL interface class
* @brief CONTROL interface class for pathfollower fixed wing fly controller
* @{
*
* @file FixedWingCONTROL.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Executes CONTROL for fixed wing fly objectives
*
* @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 FIXEDWINGFLYCONTROLLER_H
#define FIXEDWINGFLYCONTROLLER_H
#include "pathfollowercontrol.h"
class FixedWingFlyController : public PathFollowerControl {
private:
static FixedWingFlyController *p_inst;
FixedWingFlyController();
public:
static FixedWingFlyController *instance()
{
if (!p_inst) {
p_inst = new FixedWingFlyController();
}
return p_inst;
}
int32_t Initialize(FixedWingPathFollowerSettingsData *fixedWingSettings);
void Activate(void);
void Deactivate(void);
void SettingsUpdated(void);
void UpdateAutoPilot(void);
void ObjectiveUpdated(void);
uint8_t IsActive(void);
uint8_t Mode(void);
void AirspeedStateUpdatedCb(__attribute__((unused)) UAVObjEvent * ev);
private:
void resetGlobals();
uint8_t updateAutoPilotFixedWing();
void updatePathVelocity(float kFF, bool limited);
uint8_t updateFixedDesiredAttitude();
bool correctCourse(float *C, float *V, float *F, float s);
FixedWingPathFollowerSettingsData *fixedWingSettings;
uint8_t mActive;
uint8_t mMode;
struct pid PIDposH[2];
struct pid PIDposV;
struct pid PIDcourse;
struct pid PIDspeed;
struct pid PIDpower;
// correct speed by measured airspeed
float indicatedAirspeedStateBias;
};
#endif // FIXEDWINGFLYCONTROLLER_H

71
flight/modules/PathFollower/inc/grounddrivecontroller.h Normal file
View File

@ -0,0 +1,71 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup Ground CONTROL interface class
* @brief CONTROL interface class for ground drive controller
* @{
*
* @file grounddrivecontroller.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Class definition for ground drive controller implementation
*
* @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 GROUNDDRIVECONTROLLER_H
#define GROUNDDRIVECONTROLLER_H
#include "pathfollowercontrol.h"
#include "pidcontrolne.h"
class GroundDriveController : public PathFollowerControl {
private:
static GroundDriveController *p_inst;
GroundDriveController();
public:
static GroundDriveController *instance()
{
if (!p_inst) {
p_inst = new GroundDriveController();
}
return p_inst;
}
int32_t Initialize(GroundPathFollowerSettingsData *groundSettings);
void Activate(void);
void Deactivate(void);
void SettingsUpdated(void);
void UpdateAutoPilot(void);
void ObjectiveUpdated(void);
uint8_t IsActive(void);
uint8_t Mode(void);
private:
uint8_t updateAutoPilotGround();
void updatePathVelocity(float kFF);
uint8_t updateGroundDesiredAttitude();
GroundPathFollowerSettingsData *groundSettings;
uint8_t mActive;
uint8_t mMode;
PIDControlNE controlNE;
};
#endif // GROUNDDRIVECONTROLLER_H

59
flight/targets/boards/coptercontrol/bootloader/inc/pios_config.h → flight/modules/PathFollower/inc/pathfollowercontrol.h
View File

@ -1,14 +1,15 @@
/**
******************************************************************************
* @addtogroup CopterControlBL CopterControl BootLoader
* @brief These files contain the code to the CopterControl Bootloader.
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @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.
* @addtogroup PathFollower CONTROL interface class
* @brief CONTROL interface class for pathfollower goal implementations
* @{
*
* @file pathfollowercontrol.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Interface class for controllers
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
@ -27,27 +28,23 @@
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef PATHFOLLOWERCONTROL_H
#define PATHFOLLOWERCONTROL_H
class PathFollowerControl {
public:
virtual void Activate(void) = 0;
virtual void Deactivate(void) = 0;
virtual void SettingsUpdated(void) = 0;
virtual void UpdateAutoPilot(void) = 0;
virtual void ObjectiveUpdated(void) = 0;
virtual uint8_t Mode(void) = 0;
static int32_t Initialize(PathDesiredData *ptr_pathDesired,
FlightStatusData *ptr_flightStatus,
PathStatusData *ptr_pathStatus);
protected:
static PathDesiredData *pathDesired;
static FlightStatusData *flightStatus;
static PathStatusData *pathStatus;
};
#ifndef PIOS_CONFIG_H
#define PIOS_CONFIG_H
/* Enable/Disable PiOS modules */
#define PIOS_INCLUDE_DELAY
#define PIOS_INCLUDE_SYS
#define PIOS_INCLUDE_IRQ
#define PIOS_INCLUDE_GPIO
#define PIOS_INCLUDE_USB
#define PIOS_INCLUDE_USB_HID
#define PIOS_INCLUDE_LED
#define PIOS_INCLUDE_IAP
#define PIOS_INCLUDE_COM
#define PIOS_INCLUDE_COM_MSG
#define PIOS_INCLUDE_BL_HELPER
#define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT
#endif /* PIOS_CONFIG_H */
/**
* @}
* @}
*/
#endif // PATHFOLLOWERCONTROL_H

81
flight/modules/PathFollower/inc/pathfollowerfsm.h Normal file
View File

@ -0,0 +1,81 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup PathFollower FSM interface class
* @brief FSM interface class for pathfollower goal fsm implementations
* @{
*
* @file pathfollowerfsm.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Interface class for PathFollower FSMs
*
* @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 PATHFOLLOWERFSM_H
#define PATHFOLLOWERFSM_H
extern "C" {
#include <stabilizationdesired.h>
}
#include <pidcontroldowncallback.h>
typedef enum {
PFFSM_STATE_INACTIVE = 0, // Inactive state is the initialised state on startup
PFFSM_STATE_ACTIVE,
PFFSM_STATE_DISARMED,
PFFSM_STATE_ABORT // Abort on error
} PathFollowerFSMState_T;
class PathFollowerFSM : public PIDControlDownCallback {
public:
// PathFollowerFSM() {};
virtual void Inactive(void) {}
virtual void Activate(void) {}
virtual void Update(void) {}
virtual void SettingsUpdated(void) {}
virtual void BoundThrust(__attribute__((unused)) float &ulow, __attribute__((unused)) float &uhigh) {}
virtual PathFollowerFSMState_T GetCurrentState(void)
{
return PFFSM_STATE_INACTIVE;
}
virtual void ConstrainStabiDesired(__attribute__((unused)) StabilizationDesiredData *stabDesired) {}
virtual float BoundVelocityDown(float velocity)
{
return velocity;
}
virtual void CheckPidScaler(__attribute__((unused)) pid_scaler *scaler) {}
virtual void GetYaw(bool &yaw_attitude, float &yaw_direction)
{
yaw_attitude = false; yaw_direction = 0.0f;
}
virtual void Abort(void) {}
virtual uint8_t ManualThrust(void)
{
return false;
}
virtual uint8_t PositionHoldState(void)
{
return false;
}
// virtual ~PathFollowerFSM() = 0;
};
#endif // PATHFOLLOWERFSM_H

80
flight/modules/PathFollower/inc/pidcontrolne.h Normal file
View File

@ -0,0 +1,80 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup PathFollower CONTROL interface class
* @brief PID Controler for NE Class definition
* @{
*
* @file pidcontrolne.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Executes pid control loop for NE
*
* @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 PIDCONTROLNE_H
#define PIDCONTROLNE_H
extern "C" {
#include <pid.h>
}
#include "pathfollowerfsm.h"
class PIDControlNE {
public:
PIDControlNE();
~PIDControlNE();
void Initialize();
void Deactivate();
void Activate();
void UpdateParameters(float kp, float ki, float kd, __attribute__((unused)) float ilimit, float dT, float velocityMax);
void UpdatePositionalParameters(float kp);
void UpdatePositionState(float pvNorth, float pvEast);
void UpdatePositionSetpoint(float setpointNorth, float setpointEast);
void UpdateVelocitySetpoint(float setpointNorth, float setpointEast);
// void RateLimit(float *spDesired, float *spCurrent, float rateLimit);
void UpdateVelocityState(float pvNorth, float pvEast);
void UpdateCommandParameters(float minCommand, float maxCommand, float velocityFeedforward);
void ControlPosition();
void ControlPositionWithPath(struct path_status *progress);
void GetNECommand(float *northCommand, float *eastCommand);
void GetVelocityDesired(float *north, float *east);
void UpdateBrakeVelocity(float startingVelocity, float dT, float brakeRate, float currentVelocity, float *updatedVelocity);
void UpdateVelocityStateWithBrake(float pvNorth, float pvEast, float path_time, float brakeRate);
private:
struct pid2 PIDvel[2]; // North, East
float mVelocitySetpointTarget[2];
float mVelocityState[2];
struct pid PIDposH[2];
float mPositionSetpointTarget[2];
float mPositionState[2];
float deltaTime;
float mVelocitySetpointCurrent[2];
float mNECommand;
float mNeutral;
float mVelocityMax;
float mMinCommand;
float mMaxCommand;
float mVelocityFeedforward;
uint8_t mActive;
};
#endif // PIDCONTROLNE_H

76
flight/modules/PathFollower/inc/vtolautotakeoffcontroller.h Normal file
View File

@ -0,0 +1,76 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup PathFollower CONTROL interface class
* @brief vtol land controller class
* @{
*
* @file vtollandcontroller.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Executes CONTROL for landing sequence
*
* @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 VTOLAUTOTAKEOFFCONTROLLER_H
#define VTOLAUTOTAKEOFFCONTROLLER_H
#include "pathfollowercontrol.h"
#include "pidcontroldown.h"
#include "pidcontrolne.h"
// forward decl
class VtolAutoTakeoffFSM;
class VtolAutoTakeoffController : public PathFollowerControl {
private:
static VtolAutoTakeoffController *p_inst;
VtolAutoTakeoffController();
public:
static VtolAutoTakeoffController *instance()
{
if (!p_inst) {
p_inst = new VtolAutoTakeoffController();
}
return p_inst;
}
int32_t Initialize(VtolPathFollowerSettingsData *vtolPathFollowerSettings);
void Activate(void);
void Deactivate(void);
void SettingsUpdated(void);
void UpdateAutoPilot(void);
void ObjectiveUpdated(void);
uint8_t IsActive(void);
uint8_t Mode(void);
private:
void UpdateVelocityDesired(void);
int8_t UpdateStabilizationDesired(bool yaw_attitude, float yaw_direction);
void setArmedIfChanged(FlightStatusArmedOptions val);
VtolAutoTakeoffFSM *fsm;
VtolPathFollowerSettingsData *vtolPathFollowerSettings;
PIDControlDown controlDown;
PIDControlNE controlNE;
uint8_t mActive;
};
#endif // VTOLAUTOTAKEOFFCONTROLLER_H

158
flight/modules/PathFollower/inc/vtolautotakeofffsm.h Normal file
View File

@ -0,0 +1,158 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup PathFollower FSM
* @brief Executes landing sequence via an FSM
* @{
*
* @file vtollandfsm.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Executes FSM for landing sequence
*
* @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 VTOLAUTOTAKEOFFFSM_H
#define VTOLAUTOTAKEOFFFSM_H
extern "C" {
#include "statusvtolautotakeoff.h"
}
#include "pathfollowerfsm.h"
// AutoTakeoffing state machine
typedef enum {
AUTOTAKEOFF_STATE_INACTIVE = 0, // Inactive state is the initialised state on startup
AUTOTAKEOFF_STATE_CHECKSTATE, // Initial condition checks
AUTOTAKEOFF_STATE_SLOWSTART, // Slow start motors
AUTOTAKEOFF_STATE_THRUSTUP, // Ramp motors up to neutral thrust
AUTOTAKEOFF_STATE_TAKEOFF, // Ascend to target velocity
AUTOTAKEOFF_STATE_HOLD, // Hold position as completion of the sequence
AUTOTAKEOFF_STATE_THRUSTDOWN, // Thrust down sequence
AUTOTAKEOFF_STATE_THRUSTOFF, // Thrust is now off
AUTOTAKEOFF_STATE_DISARMED, // Disarmed
AUTOTAKEOFF_STATE_ABORT, // Abort on error triggerig fallback to hold
AUTOTAKEOFF_STATE_SIZE
} PathFollowerFSM_AutoTakeoffState_T;
class VtolAutoTakeoffFSM : public PathFollowerFSM {
private:
static VtolAutoTakeoffFSM *p_inst;
VtolAutoTakeoffFSM();
public:
static VtolAutoTakeoffFSM *instance()
{
if (!p_inst) {
p_inst = new VtolAutoTakeoffFSM();
}
return p_inst;
}
int32_t Initialize(VtolPathFollowerSettingsData *vtolPathFollowerSettings,
PathDesiredData *pathDesired,
FlightStatusData *flightStatus);
void Inactive(void);
void Activate(void);
void Update(void);
void BoundThrust(float &ulow, float &uhigh);
PathFollowerFSMState_T GetCurrentState(void);
void ConstrainStabiDesired(StabilizationDesiredData *stabDesired);
void Abort(void);
uint8_t PositionHoldState(void);
void setControlState(StatusVtolAutoTakeoffControlStateOptions controlState);
protected:
// FSM instance data type
typedef struct {
StatusVtolAutoTakeoffData fsmAutoTakeoffStatus;
StatusVtolAutoTakeoffStateOptions currentState;
TakeOffLocationData takeOffLocation;
uint32_t stateRunCount;
uint32_t stateTimeoutCount;
float sum1;
float sum2;
float expectedAutoTakeoffPositionNorth;
float expectedAutoTakeoffPositionEast;
float thrustLimit;
float boundThrustMin;
float boundThrustMax;
uint8_t observationCount;
uint8_t observation2Count;
uint8_t flZeroStabiHorizontal;
uint8_t flConstrainThrust;
uint8_t flLowAltitude;
uint8_t flAltitudeHold;
} VtolAutoTakeoffFSMData_T;
// FSM state structure
typedef struct {
void(VtolAutoTakeoffFSM::*setup) (void); // Called to initialise the state
void(VtolAutoTakeoffFSM::*run) (uint8_t); // Run the event detection code for a state
} PathFollowerFSM_AutoTakeoffStateHandler_T;
// Private variables
VtolAutoTakeoffFSMData_T *mAutoTakeoffData;
VtolPathFollowerSettingsData *vtolPathFollowerSettings;
PathDesiredData *pathDesired;
FlightStatusData *flightStatus;
void setup_inactive(void);
void setup_checkstate(void);
void setup_slowstart(void);
void run_slowstart(uint8_t);
void setup_takeoff(void);
void run_takeoff(uint8_t);
void setup_hold(void);
void run_hold(uint8_t);
void setup_thrustup(void);
void run_thrustup(uint8_t);
void setup_thrustdown(void);
void run_thrustdown(uint8_t);
void setup_thrustoff(void);
void run_thrustoff(uint8_t);
void setup_disarmed(void);
void run_disarmed(uint8_t);
void setup_abort(void);
void run_abort(uint8_t);
void initFSM(void);
void setState(StatusVtolAutoTakeoffStateOptions newState, StatusVtolAutoTakeoffStateExitReasonOptions reason);
int32_t runState();
int32_t runAlways();
void updateVtolAutoTakeoffFSMStatus();
void assessAltitude(void);
void setStateTimeout(int32_t count);
void fallback_to_hold(void);
static PathFollowerFSM_AutoTakeoffStateHandler_T sAutoTakeoffStateTable[AUTOTAKEOFF_STATE_SIZE];
};
#endif // VTOLAUTOTAKEOFFFSM_H

76
flight/modules/PathFollower/inc/vtolbrakecontroller.h Normal file
View File

@ -0,0 +1,76 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup PathFollower CONTROL interface class
* @brief CONTROL interface class for brake controller
* @{
*
* @file vtolbrakecontroller.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Executes brake controller for vtol
*
* @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 VTOLBRAKECONTROLLER_H
#define VTOLBRAKECONTROLLER_H
#include "pathfollowercontrol.h"
#include "pidcontroldown.h"
#include "pidcontrolne.h"
// forward decl
class PathFollowerFSM;
class VtolBrakeController : public PathFollowerControl {
private:
static VtolBrakeController *p_inst;
VtolBrakeController();
public:
static VtolBrakeController *instance()
{
if (!p_inst) {
p_inst = new VtolBrakeController();
}
return p_inst;
}
int32_t Initialize(VtolPathFollowerSettingsData *vtolPathFollowerSettings);
void Activate(void);
void Deactivate(void);
void SettingsUpdated(void);
void UpdateAutoPilot(void);
void ObjectiveUpdated(void);
uint8_t IsActive(void);
uint8_t Mode(void);
private:
void UpdateVelocityDesired(void);
int8_t UpdateStabilizationDesired(void);
void updateBrakeVelocity(float startingVelocity, float dT, float brakeRate, float currentVelocity, float *updatedVelocity);
PathFollowerFSM *fsm;
VtolPathFollowerSettingsData *vtolPathFollowerSettings;
PIDControlDown controlDown;
PIDControlNE controlNE;
uint8_t mActive;
uint8_t mHoldActive;
uint8_t mManualThrust;
};
#endif // VTOLBRAKECONTROLLER_H

111
flight/modules/PathFollower/inc/vtolbrakefsm.h Normal file
View File

@ -0,0 +1,111 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup PathFollower FSM Brake
* @brief Executes brake seqeuence
* @{
*
* @file vtolbrakfsm.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Executes brake sequence fsm
*
* @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 VTOLBRAKEFSM_H
#define VTOLBRAKEFSM_H
#include "pathfollowerfsm.h"
// Brakeing state machine
typedef enum {
BRAKE_STATE_INACTIVE = 0, // Inactive state is the initialised state on startup
BRAKE_STATE_BRAKE, // Initiate altitude hold before starting descent
BRAKE_STATE_HOLD, // Waiting for attainment of landing descent rate
BRAKE_STATE_SIZE
} PathFollowerFSM_BrakeState_T;
typedef enum {
FSMBRAKESTATUS_STATEEXITREASON_NONE = 0
} VtolBrakeFSMStatusStateExitReasonOptions;
class VtolBrakeFSM : public PathFollowerFSM {
private:
static VtolBrakeFSM *p_inst;
VtolBrakeFSM();
public:
static VtolBrakeFSM *instance()
{
if (!p_inst) {
p_inst = new VtolBrakeFSM();
}
return p_inst;
}
int32_t Initialize(VtolPathFollowerSettingsData *vtolPathFollowerSettings,
PathDesiredData *pathDesired,
FlightStatusData *flightStatus,
PathStatusData *ptr_pathStatus);
void Inactive(void);
void Activate(void);
void Update(void);
PathFollowerFSMState_T GetCurrentState(void);
void Abort(void);
uint8_t PositionHoldState(void);
protected:
// FSM instance data type
typedef struct {
PathFollowerFSM_BrakeState_T currentState;
uint32_t stateRunCount;
uint32_t stateTimeoutCount;
float sum1;
float sum2;
uint8_t observationCount;
uint8_t observation2Count;
} VtolBrakeFSMData_T;
// FSM state structure
typedef struct {
void(VtolBrakeFSM::*setup) (void); // Called to initialise the state
void(VtolBrakeFSM::*run) (uint8_t); // Run the event detection code for a state
} PathFollowerFSM_BrakeStateHandler_T;
// Private variables
VtolBrakeFSMData_T *mBrakeData;
VtolPathFollowerSettingsData *vtolPathFollowerSettings;
PathDesiredData *pathDesired;
PathStatusData *pathStatus;
FlightStatusData *flightStatus;
void setup_brake(void);
void run_brake(uint8_t);
void initFSM(void);
void setState(PathFollowerFSM_BrakeState_T newState, VtolBrakeFSMStatusStateExitReasonOptions reason);
int32_t runState();
// void updateVtolBrakeFSMStatus();
void setStateTimeout(int32_t count);
void fallback_to_hold(void);
static PathFollowerFSM_BrakeStateHandler_T sBrakeStateTable[BRAKE_STATE_SIZE];
};
#endif // VTOLBRAKEFSM_H

84
flight/modules/PathFollower/inc/vtolflycontroller.h Normal file
View File

@ -0,0 +1,84 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup PathFollower CONTROL interface class
* @brief vtol fly controller class definition
* @{
*
* @file vtolflycontroller.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Executes fly control for vtols
*
* @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 VTOLFLYCONTROLLER_H
#define VTOLFLYCONTROLLER_H
#include "pathfollowercontrol.h"
#include "pidcontrolne.h"
#include "pidcontroldown.h"
class VtolFlyController : public PathFollowerControl {
private:
static VtolFlyController *p_inst;
VtolFlyController();
public:
static VtolFlyController *instance()
{
if (!p_inst) {
p_inst = new VtolFlyController();
}
return p_inst;
}
int32_t Initialize(VtolPathFollowerSettingsData *vtolPathFollowerSettings);
void Activate(void);
void Deactivate(void);
void SettingsUpdated(void);
void UpdateAutoPilot(void);
void ObjectiveUpdated(void);
uint8_t IsActive(void);
uint8_t Mode(void);
private:
void UpdateVelocityDesired(void);
int8_t UpdateStabilizationDesired(bool yaw_attitude, float yaw_direction);
void UpdateDesiredAttitudeEmergencyFallback();
void fallback_to_hold(void);
float updateTailInBearing();
float updateCourseBearing();
float updatePathBearing();
float updatePOIBearing();
uint8_t RunAutoPilot();
VtolPathFollowerSettingsData *vtolPathFollowerSettings;
PIDControlNE controlNE;
PIDControlDown controlDown;
uint8_t mActive;
uint8_t mManualThrust;
uint8_t mMode;
float vtolEmergencyFallback;
bool vtolEmergencyFallbackSwitch;
};
#endif // VTOLFLYCONTROLLER_H

76
flight/modules/PathFollower/inc/vtollandcontroller.h Normal file
View File

@ -0,0 +1,76 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup PathFollower CONTROL interface class
* @brief vtol land controller class
* @{
*
* @file vtollandcontroller.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Executes CONTROL for landing sequence
*
* @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 VTOLLANDCONTROLLER_H
#define VTOLLANDCONTROLLER_H
#include "pathfollowercontrol.h"
#include "pidcontroldown.h"
#include "pidcontrolne.h"
// forward decl
class PathFollowerFSM;
class VtolLandController : public PathFollowerControl {
private:
static VtolLandController *p_inst;
VtolLandController();
public:
static VtolLandController *instance()
{
if (!p_inst) {
p_inst = new VtolLandController();
}
return p_inst;
}
int32_t Initialize(VtolPathFollowerSettingsData *vtolPathFollowerSettings);
void Activate(void);
void Deactivate(void);
void SettingsUpdated(void);
void UpdateAutoPilot(void);
void ObjectiveUpdated(void);
uint8_t IsActive(void);
uint8_t Mode(void);
private:
void UpdateVelocityDesired(void);
int8_t UpdateStabilizationDesired(bool yaw_attitude, float yaw_direction);
void setArmedIfChanged(FlightStatusArmedOptions val);
PathFollowerFSM *fsm;
VtolPathFollowerSettingsData *vtolPathFollowerSettings;
PIDControlDown controlDown;
PIDControlNE controlNE;
uint8_t mActive;
};
#endif // VTOLLANDCONTROLLER_H

148
flight/modules/PathFollower/inc/vtollandfsm.h Normal file
View File

@ -0,0 +1,148 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup PathFollower FSM
* @brief Executes landing sequence via an FSM
* @{
*
* @file vtollandfsm.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Executes FSM for landing sequence
*
* @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 VTOLLANDFSM_H
#define VTOLLANDFSM_H
extern "C" {
#include "statusvtolland.h"
}
#include "pathfollowerfsm.h"
// Landing state machine
typedef enum {
LAND_STATE_INACTIVE = 0, // Inactive state is the initialised state on startup
LAND_STATE_INIT_ALTHOLD, // Initiate altitude hold before starting descent
LAND_STATE_WTG_FOR_DESCENTRATE, // Waiting for attainment of landing descent rate
LAND_STATE_AT_DESCENTRATE, // Landing descent rate achieved
LAND_STATE_WTG_FOR_GROUNDEFFECT, // Waiting for group effect to be detected
LAND_STATE_GROUNDEFFECT, // Ground effect detected
LAND_STATE_THRUSTDOWN, // Thrust down sequence
LAND_STATE_THRUSTOFF, // Thrust is now off
LAND_STATE_DISARMED, // Disarmed
LAND_STATE_ABORT, // Abort on error triggerig fallback to hold
LAND_STATE_SIZE
} PathFollowerFSM_LandState_T;
class VtolLandFSM : public PathFollowerFSM {
private:
static VtolLandFSM *p_inst;
VtolLandFSM();
public:
static VtolLandFSM *instance()
{
if (!p_inst) {
p_inst = new VtolLandFSM();
}
return p_inst;
}
int32_t Initialize(VtolPathFollowerSettingsData *vtolPathFollowerSettings,
PathDesiredData *pathDesired,
FlightStatusData *flightStatus);
void Inactive(void);
void Activate(void);
void Update(void);
void BoundThrust(float &ulow, float &uhigh);
PathFollowerFSMState_T GetCurrentState(void);
void ConstrainStabiDesired(StabilizationDesiredData *stabDesired);
float BoundVelocityDown(float);
void CheckPidScaler(pid_scaler *scaler);
void Abort(void);
protected:
// FSM instance data type
typedef struct {
StatusVtolLandData fsmLandStatus;
StatusVtolLandStateOptions currentState;
TakeOffLocationData takeOffLocation;
uint32_t stateRunCount;
uint32_t stateTimeoutCount;
float sum1;
float sum2;
float expectedLandPositionNorth;
float expectedLandPositionEast;
float thrustLimit;
float boundThrustMin;
float boundThrustMax;
uint8_t observationCount;
uint8_t observation2Count;
uint8_t flZeroStabiHorizontal;
uint8_t flConstrainThrust;
uint8_t flLowAltitude;
uint8_t flAltitudeHold;
} VtolLandFSMData_T;
// FSM state structure
typedef struct {
void(VtolLandFSM::*setup) (void); // Called to initialise the state
void(VtolLandFSM::*run) (uint8_t); // Run the event detection code for a state
} PathFollowerFSM_LandStateHandler_T;
// Private variables
VtolLandFSMData_T *mLandData;
VtolPathFollowerSettingsData *vtolPathFollowerSettings;
PathDesiredData *pathDesired;
FlightStatusData *flightStatus;
void setup_inactive(void);
void setup_init_althold(void);
void setup_wtg_for_descentrate(void);
void setup_at_descentrate(void);
void setup_wtg_for_groundeffect(void);
void run_init_althold(uint8_t);
void run_wtg_for_descentrate(uint8_t);
void run_at_descentrate(uint8_t);
void run_wtg_for_groundeffect(uint8_t);
void setup_groundeffect(void);
void run_groundeffect(uint8_t);
void setup_thrustdown(void);
void run_thrustdown(uint8_t);
void setup_thrustoff(void);
void run_thrustoff(uint8_t);
void setup_disarmed(void);
void run_disarmed(uint8_t);
void setup_abort(void);
void run_abort(uint8_t);
void initFSM(void);
void setState(StatusVtolLandStateOptions newState, StatusVtolLandStateExitReasonOptions reason);
int32_t runState();
int32_t runAlways();
void updateVtolLandFSMStatus();
void assessAltitude(void);
void setStateTimeout(int32_t count);
void fallback_to_hold(void);
static PathFollowerFSM_LandStateHandler_T sLandStateTable[LAND_STATE_SIZE];
};
#endif // VTOLLANDFSM_H

72
flight/modules/PathFollower/inc/vtolvelocitycontroller.h Normal file
View File

@ -0,0 +1,72 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup PathFollower Controller
* @brief Controller for Vtol velocity roam
* @{
*
* @file vtolvelocitycontroller.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Executes velocity roam control
*
* @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 PATHFOLLOWERCONTROLVELOCITYROAM_H
#define PATHFOLLOWERCONTROLVELOCITYROAM_H
#include "pathfollowercontrol.h"
#include "pidcontrolne.h"
class VtolVelocityController : public PathFollowerControl {
private:
static VtolVelocityController *p_inst;
VtolVelocityController();
public:
static VtolVelocityController *instance()
{
if (!p_inst) {
p_inst = new VtolVelocityController();
}
return p_inst;
}
int32_t Initialize(VtolPathFollowerSettingsData *vtolPathFollowerSettings);
void Activate(void);
void Deactivate(void);
void SettingsUpdated(void);
void UpdateAutoPilot(void);
void ObjectiveUpdated(void);
uint8_t IsActive(void);
uint8_t Mode(void);
private:
void UpdateVelocityDesired(void);
int8_t UpdateStabilizationDesired(bool yaw_attitude, float yaw_direction);
void fallback_to_hold(void);
VtolPathFollowerSettingsData *vtolPathFollowerSettings;
PIDControlNE controlNE;
uint8_t mActive;
};
#endif // ifndef PATHFOLLOWERCONTROLVELOCITYROAM_H

1544
flight/modules/PathFollower/pathfollower.c
View File

@ -1,1544 +0,0 @@
/**
******************************************************************************
*
* @file pathfollower.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief This module compared @ref PositionActuatl to @ref ActiveWaypoint
* and sets @ref AttitudeDesired. It only does this when the FlightMode field
* of @ref ManualControlCommand is Auto.
*
* @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
*/
/**
* Input object: PathDesired
* Input object: PositionState
* Input object: ManualControlCommand
* Output object: StabilizationDesired
*
* This module acts as "autopilot" - it controls the setpoints of stabilization
* based on current flight situation and desired flight path (PathDesired) as
* directed by flightmode selection or pathplanner
* This is a periodic delayed callback module
*
* Modules have no API, all communication to other modules is done through UAVObjects.
* However modules may use the API exposed by shared libraries.
* See the OpenPilot wiki for more details.
* http://www.openpilot.org/OpenPilot_Application_Architecture
*
*/
#include <openpilot.h>
#include <callbackinfo.h>
#include <math.h>
#include <pid.h>
#include <CoordinateConversions.h>
#include <sin_lookup.h>
#include <pathdesired.h>
#include <paths.h>
#include "plans.h"
#include <sanitycheck.h>
#include <fixedwingpathfollowersettings.h>
#include <fixedwingpathfollowerstatus.h>
#include <vtolpathfollowersettings.h>
#include <flightstatus.h>
#include <flightmodesettings.h>
#include <pathstatus.h>
#include <positionstate.h>
#include <velocitystate.h>
#include <velocitydesired.h>
#include <stabilizationdesired.h>
#include <airspeedstate.h>
#include <attitudestate.h>
#include <takeofflocation.h>
#include <poilocation.h>
#include <manualcontrolcommand.h>
#include <systemsettings.h>
#include <stabilizationbank.h>
#include <vtolselftuningstats.h>
#include <pathsummary.h>
// Private constants
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW
#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL
#define PF_IDLE_UPDATE_RATE_MS 100
#define STACK_SIZE_BYTES 2048
#define DEADBAND_HIGH 0.10f
#define DEADBAND_LOW -0.10f
#define BRAKE_FRACTIONALPROGRESS_STARTVELOCITYCHECK 0.95f
#define BRAKE_EXIT_VELOCITY_LIMIT 0.2f
#define BRAKE_RATE_MINIMUM 0.2f
#define NEUTRALTHRUST_PH_POSITIONAL_ERROR_LIMIT 0.5f
#define NEUTRALTHRUST_PH_VEL_DESIRED_LIMIT 0.2f
#define NEUTRALTHRUST_PH_VEL_STATE_LIMIT 0.2f
#define NEUTRALTHRUST_PH_VEL_ERROR_LIMIT 0.1f
#define NEUTRALTHRUST_START_DELAY (2 * 20) // 2 seconds at rate of 20Hz (50ms update rate)
#define NEUTRALTHRUST_END_COUNT (NEUTRALTHRUST_START_DELAY + (4 * 20)) // 4 second sample
// Private types
struct Globals {
struct pid PIDposH[2];
struct pid PIDposV;
struct pid PIDvel[3]; // North, East, Down
struct pid BrakePIDvel[2]; // North, East
struct pid PIDcourse;
struct pid PIDspeed;
struct pid PIDpower;
float poiRadius;
float vtolEmergencyFallback;
bool vtolEmergencyFallbackSwitch;
};
struct NeutralThrustEstimation {
uint32_t count;
float sum;
float average;
float correction;
float algo_erro_check;
float min;
float max;
bool start_sampling;
bool have_correction;
};
static struct NeutralThrustEstimation neutralThrustEst;
// Private variables
static DelayedCallbackInfo *pathFollowerCBInfo;
static uint32_t updatePeriod = PF_IDLE_UPDATE_RATE_MS;
static struct Globals global;
static PathStatusData pathStatus;
static PathDesiredData pathDesired;
static FixedWingPathFollowerSettingsData fixedWingPathFollowerSettings;
static VtolPathFollowerSettingsData vtolPathFollowerSettings;
static FlightStatusData flightStatus;
static PathSummaryData pathSummary;
// correct speed by measured airspeed
static float indicatedAirspeedStateBias = 0.0f;
// Private functions
static void pathFollowerTask(void);
static void resetGlobals();
static void SettingsUpdatedCb(UAVObjEvent *ev);
static uint8_t updateAutoPilotByFrameType();
static uint8_t updateAutoPilotFixedWing();
static uint8_t updateAutoPilotVtol();
static float updateTailInBearing();
static float updateCourseBearing();
static float updatePathBearing();
static float updatePOIBearing();
static void processPOI();
static void updateBrakeVelocity(float startingVelocity, float dT, float brakeRate, float currentVelocity, float *updatedVelocity);
static void updatePathVelocity(float kFF, bool limited);
static uint8_t updateFixedDesiredAttitude();
static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction);
static void updateFixedAttitude();
static void updateVtolDesiredAttitudeEmergencyFallback();
static void airspeedStateUpdatedCb(UAVObjEvent *ev);
static bool correctCourse(float *C, float *V, float *F, float s);
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t PathFollowerStart()
{
// Start main task
PathStatusGet(&pathStatus);
SettingsUpdatedCb(NULL);
PIOS_CALLBACKSCHEDULER_Dispatch(pathFollowerCBInfo);
return 0;
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t PathFollowerInitialize()
{
// initialize objects
FixedWingPathFollowerSettingsInitialize();
FixedWingPathFollowerStatusInitialize();
VtolPathFollowerSettingsInitialize();
FlightStatusInitialize();
FlightModeSettingsInitialize();
PathStatusInitialize();
PathSummaryInitialize();
PathDesiredInitialize();
PositionStateInitialize();
VelocityStateInitialize();
VelocityDesiredInitialize();
StabilizationDesiredInitialize();
AirspeedStateInitialize();
AttitudeStateInitialize();
TakeOffLocationInitialize();
PoiLocationInitialize();
ManualControlCommandInitialize();
SystemSettingsInitialize();
StabilizationBankInitialize();
VtolSelfTuningStatsInitialize();
// reset integrals
resetGlobals();
// Create object queue
pathFollowerCBInfo = PIOS_CALLBACKSCHEDULER_Create(&pathFollowerTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_PATHFOLLOWER, STACK_SIZE_BYTES);
FixedWingPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
PathDesiredConnectCallback(SettingsUpdatedCb);
AirspeedStateConnectCallback(airspeedStateUpdatedCb);
return 0;
}
MODULE_INITCALL(PathFollowerInitialize, PathFollowerStart);
/**
* Module thread, should not return.
*/
static void pathFollowerTask(void)
{
FlightStatusGet(&flightStatus);
if (flightStatus.ControlChain.PathFollower != FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
resetGlobals();
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_UNINITIALISED);
PIOS_CALLBACKSCHEDULER_Schedule(pathFollowerCBInfo, PF_IDLE_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
return;
}
if (flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_POI) { // TODO Hack from vtolpathfollower, move into manualcontrol!
processPOI();
}
int16_t old_uid = pathStatus.UID;
pathStatus.UID = pathDesired.UID;
pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
if (pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT] > 0.0f) {
if (old_uid != pathStatus.UID) {
pathStatus.path_time = 0.0f;
} else {
pathStatus.path_time += updatePeriod / 1000.0f;
}
}
switch (pathDesired.Mode) {
case PATHDESIRED_MODE_FLYENDPOINT:
case PATHDESIRED_MODE_FLYVECTOR:
case PATHDESIRED_MODE_BRAKE:
case PATHDESIRED_MODE_FLYCIRCLERIGHT:
case PATHDESIRED_MODE_FLYCIRCLELEFT:
{
uint8_t result = updateAutoPilotByFrameType();
if (result) {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
} else {
pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
}
}
break;
case PATHDESIRED_MODE_FIXEDATTITUDE:
updateFixedAttitude(pathDesired.ModeParameters);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
break;
case PATHDESIRED_MODE_DISARMALARM:
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
break;
default:
pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
break;
}
PathStatusSet(&pathStatus);
PIOS_CALLBACKSCHEDULER_Schedule(pathFollowerCBInfo, updatePeriod, CALLBACK_UPDATEMODE_SOONER);
}
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
FixedWingPathFollowerSettingsGet(&fixedWingPathFollowerSettings);
pid_configure(&global.PIDcourse, fixedWingPathFollowerSettings.CoursePI.Kp, fixedWingPathFollowerSettings.CoursePI.Ki, 0.0f, fixedWingPathFollowerSettings.CoursePI.ILimit);
pid_configure(&global.PIDspeed, fixedWingPathFollowerSettings.SpeedPI.Kp, fixedWingPathFollowerSettings.SpeedPI.Ki, 0.0f, fixedWingPathFollowerSettings.SpeedPI.ILimit);
pid_configure(&global.PIDpower, fixedWingPathFollowerSettings.PowerPI.Kp, fixedWingPathFollowerSettings.PowerPI.Ki, 0.0f, fixedWingPathFollowerSettings.PowerPI.ILimit);
VtolPathFollowerSettingsGet(&vtolPathFollowerSettings);
pid_configure(&global.PIDvel[0], vtolPathFollowerSettings.HorizontalVelPID.Kp, vtolPathFollowerSettings.HorizontalVelPID.Ki, vtolPathFollowerSettings.HorizontalVelPID.Kd, vtolPathFollowerSettings.HorizontalVelPID.ILimit);
pid_configure(&global.PIDvel[1], vtolPathFollowerSettings.HorizontalVelPID.Kp, vtolPathFollowerSettings.HorizontalVelPID.Ki, vtolPathFollowerSettings.HorizontalVelPID.Kd, vtolPathFollowerSettings.HorizontalVelPID.ILimit);
pid_configure(&global.PIDvel[2], vtolPathFollowerSettings.VerticalVelPID.Kp, vtolPathFollowerSettings.VerticalVelPID.Ki, vtolPathFollowerSettings.VerticalVelPID.Kd, vtolPathFollowerSettings.VerticalVelPID.ILimit);
pid_configure(&global.BrakePIDvel[0], vtolPathFollowerSettings.BrakeHorizontalVelPID.Kp, vtolPathFollowerSettings.BrakeHorizontalVelPID.Ki, vtolPathFollowerSettings.BrakeHorizontalVelPID.Kd, vtolPathFollowerSettings.BrakeHorizontalVelPID.ILimit);
pid_configure(&global.BrakePIDvel[1], vtolPathFollowerSettings.BrakeHorizontalVelPID.Kp, vtolPathFollowerSettings.BrakeHorizontalVelPID.Ki, vtolPathFollowerSettings.BrakeHorizontalVelPID.Kd, vtolPathFollowerSettings.BrakeHorizontalVelPID.ILimit);
PathDesiredGet(&pathDesired);
}
static void airspeedStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
AirspeedStateData airspeedState;
VelocityStateData velocityState;
AirspeedStateGet(&airspeedState);
VelocityStateGet(&velocityState);
float airspeedVector[2];
float yaw;
AttitudeStateYawGet(&yaw);
airspeedVector[0] = cos_lookup_deg(yaw);
airspeedVector[1] = sin_lookup_deg(yaw);
// vector projection of groundspeed on airspeed vector to handle both forward and backwards movement
float groundspeedProjection = velocityState.North * airspeedVector[0] + velocityState.East * airspeedVector[1];
indicatedAirspeedStateBias = airspeedState.CalibratedAirspeed - groundspeedProjection;
// note - we do fly by Indicated Airspeed (== calibrated airspeed) however
// since airspeed is updated less often than groundspeed, we use sudden
// changes to groundspeed to offset the airspeed by the same measurement.
// This has a side effect that in the absence of any airspeed updates, the
// pathfollower will fly using groundspeed.
}
/**
* reset integrals
*/
static void resetGlobals()
{
pid_zero(&global.PIDposH[0]);
pid_zero(&global.PIDposH[1]);
pid_zero(&global.PIDposV);
pid_zero(&global.PIDvel[0]);
pid_zero(&global.PIDvel[1]);
pid_zero(&global.PIDvel[2]);
pid_zero(&global.BrakePIDvel[0]);
pid_zero(&global.BrakePIDvel[1]);
pid_zero(&global.PIDcourse);
pid_zero(&global.PIDspeed);
pid_zero(&global.PIDpower);
global.poiRadius = 0.0f;
global.vtolEmergencyFallback = 0;
global.vtolEmergencyFallbackSwitch = false;
// reset neutral thrust assessment. We restart this process
// and do once for each position hold engagement
neutralThrustEst.start_sampling = false;
neutralThrustEst.count = 0;
neutralThrustEst.sum = 0.0f;
neutralThrustEst.have_correction = false;
neutralThrustEst.average = 0.0f;
neutralThrustEst.correction = 0.0f;
neutralThrustEst.min = 0.0f;
neutralThrustEst.max = 0.0f;
pathStatus.path_time = 0.0f;
}
static uint8_t updateAutoPilotByFrameType()
{
FrameType_t frameType = GetCurrentFrameType();
if (frameType == FRAME_TYPE_CUSTOM || frameType == FRAME_TYPE_GROUND) {
switch (vtolPathFollowerSettings.TreatCustomCraftAs) {
case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_FIXEDWING:
frameType = FRAME_TYPE_FIXED_WING;
break;
case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_VTOL:
frameType = FRAME_TYPE_MULTIROTOR;
break;
}
}
switch (frameType) {
case FRAME_TYPE_MULTIROTOR:
case FRAME_TYPE_HELI:
updatePeriod = vtolPathFollowerSettings.UpdatePeriod;
return updateAutoPilotVtol();
break;
case FRAME_TYPE_FIXED_WING:
default:
updatePeriod = fixedWingPathFollowerSettings.UpdatePeriod;
return updateAutoPilotFixedWing();
break;
}
}
/**
* fixed wing autopilot:
* straight forward:
* 1. update path velocity for limited motion crafts
* 2. update attitude according to default fixed wing pathfollower algorithm
*/
static uint8_t updateAutoPilotFixedWing()
{
pid_configure(&global.PIDposH[0], fixedWingPathFollowerSettings.HorizontalPosP, 0.0f, 0.0f, 0.0f);
pid_configure(&global.PIDposH[1], fixedWingPathFollowerSettings.HorizontalPosP, 0.0f, 0.0f, 0.0f);
pid_configure(&global.PIDposV, fixedWingPathFollowerSettings.VerticalPosP, 0.0f, 0.0f, 0.0f);
updatePathVelocity(fixedWingPathFollowerSettings.CourseFeedForward, true);
return updateFixedDesiredAttitude();
}
/**
* vtol autopilot
* use hover capable algorithm with unlimeted movement calculation. if that fails (flyaway situation due to compass failure)
* fall back to emergency fallback autopilot to keep minimum amount of flight control
*/
static uint8_t updateAutoPilotVtol()
{
enum { RETURN_0 = 0, RETURN_1 = 1, RETURN_RESULT } returnmode;
enum { FOLLOWER_REGULAR, FOLLOWER_FALLBACK } followermode;
uint8_t result = 0;
// decide on behaviour based on settings and system state
if (global.vtolEmergencyFallbackSwitch) {
returnmode = RETURN_0;
followermode = FOLLOWER_FALLBACK;
} else {
if (vtolPathFollowerSettings.FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_ALWAYS) {
returnmode = RETURN_1;
followermode = FOLLOWER_FALLBACK;
} else {
returnmode = RETURN_RESULT;
followermode = FOLLOWER_REGULAR;
}
}
// vertical positon control PID loops works the same in both regular and fallback modes, setup according to settings
pid_configure(&global.PIDposV, vtolPathFollowerSettings.VerticalPosP, 0.0f, 0.0f, 0.0f);
switch (followermode) {
case FOLLOWER_REGULAR:
{
// horizontal position control PID loop works according to settings in regular mode, allowing integral terms
pid_configure(&global.PIDposH[0], vtolPathFollowerSettings.HorizontalPosP, 0.0f, 0.0f, 0.0f);
pid_configure(&global.PIDposH[1], vtolPathFollowerSettings.HorizontalPosP, 0.0f, 0.0f, 0.0f);
updatePathVelocity(vtolPathFollowerSettings.CourseFeedForward, false);
// yaw behaviour is configurable in vtolpathfollower, select yaw control algorithm
bool yaw_attitude = true;
float yaw = 0.0f;
if (pathDesired.Mode == PATHDESIRED_MODE_BRAKE) {
yaw_attitude = false;
} else {
switch (vtolPathFollowerSettings.YawControl) {
case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_MANUAL:
yaw_attitude = false;
break;
case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_TAILIN:
yaw = updateTailInBearing();
break;
case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_MOVEMENTDIRECTION:
yaw = updateCourseBearing();
break;
case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_PATHDIRECTION:
yaw = updatePathBearing();
break;
case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_POI:
yaw = updatePOIBearing();
break;
}
}
result = updateVtolDesiredAttitude(yaw_attitude, yaw);
if (!result) {
if (pathDesired.Mode == PATHDESIRED_MODE_BRAKE) {
plan_setup_assistedcontrol(true); // revert braking to position hold, user can always stick override
} else if (vtolPathFollowerSettings.FlyawayEmergencyFallback != VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_DISABLED) {
// switch to emergency follower if follower indicates problems
global.vtolEmergencyFallbackSwitch = true;
}
}
}
break;
case FOLLOWER_FALLBACK:
{
// fallback loop only cares about intended horizontal flight direction, simplify control behaviour accordingly
pid_configure(&global.PIDposH[0], 1.0f, 0.0f, 0.0f, 0.0f);
pid_configure(&global.PIDposH[1], 1.0f, 0.0f, 0.0f, 0.0f);
updatePathVelocity(vtolPathFollowerSettings.CourseFeedForward, true);
// emergency follower has no return value
updateVtolDesiredAttitudeEmergencyFallback();
}
break;
}
// Brake mode end condition checks
if (pathDesired.Mode == PATHDESIRED_MODE_BRAKE) {
bool exit_brake = false;
VelocityStateData velocityState;
if (pathStatus.path_time > pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT]) { // enter hold on timeout
pathSummary.brake_exit_reason = PATHSUMMARY_BRAKE_EXIT_REASON_TIMEOUT;
exit_brake = true;
} else if (pathStatus.fractional_progress > BRAKE_FRACTIONALPROGRESS_STARTVELOCITYCHECK) {
VelocityStateGet(&velocityState);
if (fabsf(velocityState.East) < BRAKE_EXIT_VELOCITY_LIMIT && fabsf(velocityState.North) < BRAKE_EXIT_VELOCITY_LIMIT) {
pathSummary.brake_exit_reason = PATHSUMMARY_BRAKE_EXIT_REASON_PATHCOMPLETED;
exit_brake = true;
}
}
if (exit_brake) {
// Calculate the distance error between the originally desired
// stopping point and the actual brake-exit point.
PositionStateData p;
PositionStateGet(&p);
float north_offset = pathDesired.End.North - p.North;
float east_offset = pathDesired.End.East - p.East;
float down_offset = pathDesired.End.Down - p.Down;
pathSummary.brake_distance_offset = sqrtf(north_offset * north_offset + east_offset * east_offset + down_offset * down_offset);
pathSummary.time_remaining = pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT] - pathStatus.path_time;
pathSummary.fractional_progress = pathStatus.fractional_progress;
float cur_velocity = velocityState.North * velocityState.North + velocityState.East * velocityState.East + velocityState.Down * velocityState.Down;
cur_velocity = sqrtf(cur_velocity);
pathSummary.decelrate = (pathDesired.StartingVelocity - cur_velocity) / pathStatus.path_time;
pathSummary.brakeRateActualDesiredRatio = pathSummary.decelrate / vtolPathFollowerSettings.BrakeRate;
pathSummary.velocityIntoHold = cur_velocity;
pathSummary.UID = pathStatus.UID;
PathSummarySet(&pathSummary);
plan_setup_assistedcontrol(true); // braking timeout true
// having re-entered hold allow reassessment of neutral thrust
neutralThrustEst.have_correction = false;
}
}
switch (returnmode) {
case RETURN_RESULT:
return result;
default:
// returns either 0 or 1 according to enum definition above
return returnmode;
}
}
/**
* Compute bearing of current takeoff location
*/
static float updateTailInBearing()
{
PositionStateData p;
PositionStateGet(&p);
TakeOffLocationData t;
TakeOffLocationGet(&t);
// atan2f always returns in between + and - 180 degrees
return RAD2DEG(atan2f(p.East - t.East, p.North - t.North));
}
/**
* Compute bearing of current movement direction
*/
static float updateCourseBearing()
{
VelocityStateData v;
VelocityStateGet(&v);
// atan2f always returns in between + and - 180 degrees
return RAD2DEG(atan2f(v.East, v.North));
}
/**
* Compute bearing of current path direction
*/
static float updatePathBearing()
{
PositionStateData positionState;
PositionStateGet(&positionState);
float cur[3] = { positionState.North,
positionState.East,
positionState.Down };
struct path_status progress;
path_progress(&pathDesired, cur, &progress);
// atan2f always returns in between + and - 180 degrees
return RAD2DEG(atan2f(progress.path_vector[1], progress.path_vector[0]));
}
/**
* Compute bearing between current position and POI
*/
static float updatePOIBearing()
{
PoiLocationData poi;
PoiLocationGet(&poi);
PositionStateData positionState;
PositionStateGet(&positionState);
const float dT = updatePeriod / 1000.0f;
float dLoc[3];
float yaw = 0;
/*float elevation = 0;*/
dLoc[0] = positionState.North - poi.North;
dLoc[1] = positionState.East - poi.East;
dLoc[2] = positionState.Down - poi.Down;
if (dLoc[1] < 0) {
yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) + 180.0f;
} else {
yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) - 180.0f;
}
ManualControlCommandData manualControlData;
ManualControlCommandGet(&manualControlData);
float pathAngle = 0;
if (manualControlData.Roll > DEADBAND_HIGH) {
pathAngle = -(manualControlData.Roll - DEADBAND_HIGH) * dT * 300.0f;
} else if (manualControlData.Roll < DEADBAND_LOW) {
pathAngle = -(manualControlData.Roll - DEADBAND_LOW) * dT * 300.0f;
}
return yaw + (pathAngle / 2.0f);
}
/**
* process POI control logic TODO: this should most likely go into manualcontrol!!!!
* TODO: the whole process of POI handling likely needs cleanup and rethinking, might be broken since manualcontrol was refactored currently
**/
static void processPOI()
{
const float dT = updatePeriod / 1000.0f;
PositionStateData positionState;
PositionStateGet(&positionState);
// TODO put commented out camera feature code back in place either
// permanently or optionally or remove it
// CameraDesiredData cameraDesired;
// CameraDesiredGet(&cameraDesired);
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
PoiLocationData poi;
PoiLocationGet(&poi);
float dLoc[3];
float yaw = 0;
// TODO camera feature
/*float elevation = 0;*/
dLoc[0] = positionState.North - poi.North;
dLoc[1] = positionState.East - poi.East;
dLoc[2] = positionState.Down - poi.Down;
if (dLoc[1] < 0) {
yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) + 180.0f;
} else {
yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) - 180.0f;
}
// distance
float distance = sqrtf(powf(dLoc[0], 2.0f) + powf(dLoc[1], 2.0f));
ManualControlCommandData manualControlData;
ManualControlCommandGet(&manualControlData);
float changeRadius = 0;
// Move closer or further, radially
if (manualControlData.Pitch > DEADBAND_HIGH) {
changeRadius = (manualControlData.Pitch - DEADBAND_HIGH) * dT * 100.0f;
} else if (manualControlData.Pitch < DEADBAND_LOW) {
changeRadius = (manualControlData.Pitch - DEADBAND_LOW) * dT * 100.0f;
}
// move along circular path
float pathAngle = 0;
if (manualControlData.Roll > DEADBAND_HIGH) {
pathAngle = -(manualControlData.Roll - DEADBAND_HIGH) * dT * 300.0f;
} else if (manualControlData.Roll < DEADBAND_LOW) {
pathAngle = -(manualControlData.Roll - DEADBAND_LOW) * dT * 300.0f;
} else if (manualControlData.Roll >= DEADBAND_LOW && manualControlData.Roll <= DEADBAND_HIGH) {
// change radius only when not circling
global.poiRadius = distance + changeRadius;
}
// don't try to move any closer
if (global.poiRadius >= 3.0f || changeRadius > 0) {
if (fabsf(pathAngle) > 0.0f || fabsf(changeRadius) > 0.0f) {
pathDesired.End.North = poi.North + (global.poiRadius * cosf(DEG2RAD(pathAngle + yaw - 180.0f)));
pathDesired.End.East = poi.East + (global.poiRadius * sinf(DEG2RAD(pathAngle + yaw - 180.0f)));
pathDesired.StartingVelocity = 1.0f;
pathDesired.EndingVelocity = 0.0f;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
PathDesiredSet(&pathDesired);
}
}
// not above
if (distance >= 3.0f) {
// TODO camera feature
// You can feed this into camerastabilization
/*elevation = RAD2DEG(atan2f(dLoc[2],distance));*/
// cameraDesired.Yaw=yaw;
// cameraDesired.PitchOrServo2=elevation;
// CameraDesiredSet(&cameraDesired);
}
}
static void updateBrakeVelocity(float startingVelocity, float dT, float brakeRate, float currentVelocity, float *updatedVelocity)
{
if (startingVelocity >= 0.0f) {
*updatedVelocity = startingVelocity - dT * brakeRate;
if (*updatedVelocity > currentVelocity) {
*updatedVelocity = currentVelocity;
}
if (*updatedVelocity < 0.0f) {
*updatedVelocity = 0.0f;
}
} else {
*updatedVelocity = startingVelocity + dT * brakeRate;
if (*updatedVelocity < currentVelocity) {
*updatedVelocity = currentVelocity;
}
if (*updatedVelocity > 0.0f) {
*updatedVelocity = 0.0f;
}
}
}
/**
* Compute desired velocity from the current position and path
*/
static void updatePathVelocity(float kFF, bool limited)
{
PositionStateData positionState;
PositionStateGet(&positionState);
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
VelocityDesiredData velocityDesired;
const float dT = updatePeriod / 1000.0f;
if (pathDesired.Mode == PATHDESIRED_MODE_BRAKE) {
float brakeRate = vtolPathFollowerSettings.BrakeRate;
if (brakeRate < BRAKE_RATE_MINIMUM) {
brakeRate = BRAKE_RATE_MINIMUM; // set a minimum to avoid a divide by zero potential below
}
updateBrakeVelocity(pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_NORTH], pathStatus.path_time, brakeRate, velocityState.North, &velocityDesired.North);
updateBrakeVelocity(pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_EAST], pathStatus.path_time, brakeRate, velocityState.East, &velocityDesired.East);
updateBrakeVelocity(pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_DOWN], pathStatus.path_time, brakeRate, velocityState.Down, &velocityDesired.Down);
float cur_velocity = velocityState.North * velocityState.North + velocityState.East * velocityState.East + velocityState.Down * velocityState.Down;
cur_velocity = sqrtf(cur_velocity);
float desired_velocity = velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East + velocityDesired.Down * velocityDesired.Down;
desired_velocity = sqrtf(desired_velocity);
// update pathstatus
pathStatus.error = cur_velocity - desired_velocity;
pathStatus.fractional_progress = 1.0f;
if (pathDesired.StartingVelocity > 0.0f) {
pathStatus.fractional_progress = (pathDesired.StartingVelocity - cur_velocity) / pathDesired.StartingVelocity;
// sometimes current velocity can exceed starting velocity due to initial acceleration
if (pathStatus.fractional_progress < 0.0f) {
pathStatus.fractional_progress = 0.0f;
}
}
pathStatus.path_direction_north = velocityDesired.North;
pathStatus.path_direction_east = velocityDesired.East;
pathStatus.path_direction_down = velocityDesired.Down;
pathStatus.correction_direction_north = velocityDesired.North - velocityState.North;
pathStatus.correction_direction_east = velocityDesired.East - velocityState.East;
pathStatus.correction_direction_down = velocityDesired.Down - velocityState.Down;
} else {
// look ahead kFF seconds
float cur[3] = { positionState.North + (velocityState.North * kFF),
positionState.East + (velocityState.East * kFF),
positionState.Down + (velocityState.Down * kFF) };
struct path_status progress;
path_progress(&pathDesired, cur, &progress);
// calculate velocity - can be zero if waypoints are too close
velocityDesired.North = progress.path_vector[0];
velocityDesired.East = progress.path_vector[1];
velocityDesired.Down = progress.path_vector[2];
if (limited &&
// if a plane is crossing its desired flightpath facing the wrong way (away from flight direction)
// it would turn towards the flightpath to get on its desired course. This however would reverse the correction vector
// once it crosses the flightpath again, which would make it again turn towards the flightpath (but away from its desired heading)
// leading to an S-shape snake course the wrong way
// this only happens especially if HorizontalPosP is too high, as otherwise the angle between velocity desired and path_direction won't
// turn steep unless there is enough space complete the turn before crossing the flightpath
// in this case the plane effectively needs to be turned around
// indicators:
// difference between correction_direction and velocitystate >90 degrees and
// difference between path_direction and velocitystate >90 degrees ( 4th sector, facing away from everything )
// fix: ignore correction, steer in path direction until the situation has become better (condition doesn't apply anymore)
// calculating angles < 90 degrees through dot products
(vector_lengthf(progress.path_vector, 2) > 1e-6f) &&
((progress.path_vector[0] * velocityState.North + progress.path_vector[1] * velocityState.East) < 0.0f) &&
((progress.correction_vector[0] * velocityState.North + progress.correction_vector[1] * velocityState.East) < 0.0f)) {
;
} else {
// calculate correction
velocityDesired.North += pid_apply(&global.PIDposH[0], progress.correction_vector[0], dT);
velocityDesired.East += pid_apply(&global.PIDposH[1], progress.correction_vector[1], dT);
}
velocityDesired.Down += pid_apply(&global.PIDposV, progress.correction_vector[2], dT);
// update pathstatus
pathStatus.error = progress.error;
pathStatus.fractional_progress = progress.fractional_progress;
pathStatus.path_direction_north = progress.path_vector[0];
pathStatus.path_direction_east = progress.path_vector[1];
pathStatus.path_direction_down = progress.path_vector[2];
pathStatus.correction_direction_north = progress.correction_vector[0];
pathStatus.correction_direction_east = progress.correction_vector[1];
pathStatus.correction_direction_down = progress.correction_vector[2];
}
VelocityDesiredSet(&velocityDesired);
}
/**
* Compute desired attitude from the desired velocity for fixed wing craft
*/
static uint8_t updateFixedDesiredAttitude()
{
uint8_t result = 1;
const float dT = updatePeriod / 1000.0f; // Convert from [ms] to [s]
VelocityDesiredData velocityDesired;
VelocityStateData velocityState;
StabilizationDesiredData stabDesired;
AttitudeStateData attitudeState;
FixedWingPathFollowerStatusData fixedWingPathFollowerStatus;
AirspeedStateData airspeedState;
SystemSettingsData systemSettings;
float groundspeedProjection;
float indicatedAirspeedState;
float indicatedAirspeedDesired;
float airspeedError;
float pitchCommand;
float descentspeedDesired;
float descentspeedError;
float powerCommand;
float airspeedVector[2];
float fluidMovement[2];
float courseComponent[2];
float courseError;
float courseCommand;
FixedWingPathFollowerStatusGet(&fixedWingPathFollowerStatus);
VelocityStateGet(&velocityState);
StabilizationDesiredGet(&stabDesired);
VelocityDesiredGet(&velocityDesired);
AttitudeStateGet(&attitudeState);
AirspeedStateGet(&airspeedState);
SystemSettingsGet(&systemSettings);
/**
* Compute speed error and course
*/
// missing sensors for airspeed-direction we have to assume within
// reasonable error that measured airspeed is actually the airspeed
// component in forward pointing direction
// airspeedVector is normalized
airspeedVector[0] = cos_lookup_deg(attitudeState.Yaw);
airspeedVector[1] = sin_lookup_deg(attitudeState.Yaw);
// current ground speed projected in forward direction
groundspeedProjection = velocityState.North * airspeedVector[0] + velocityState.East * airspeedVector[1];
// note that airspeedStateBias is ( calibratedAirspeed - groundspeedProjection ) at the time of measurement,
// but thanks to accelerometers, groundspeedProjection reacts faster to changes in direction
// than airspeed and gps sensors alone
indicatedAirspeedState = groundspeedProjection + indicatedAirspeedStateBias;
// fluidMovement is a vector describing the aproximate movement vector of
// the surrounding fluid in 2d space (aka wind vector)
fluidMovement[0] = velocityState.North - (indicatedAirspeedState * airspeedVector[0]);
fluidMovement[1] = velocityState.East - (indicatedAirspeedState * airspeedVector[1]);
// calculate the movement vector we need to fly to reach velocityDesired -
// taking fluidMovement into account
courseComponent[0] = velocityDesired.North - fluidMovement[0];
courseComponent[1] = velocityDesired.East - fluidMovement[1];
indicatedAirspeedDesired = boundf(sqrtf(courseComponent[0] * courseComponent[0] + courseComponent[1] * courseComponent[1]),
fixedWingPathFollowerSettings.HorizontalVelMin,
fixedWingPathFollowerSettings.HorizontalVelMax);
// if we could fly at arbitrary speeds, we'd just have to move towards the
// courseComponent vector as previously calculated and we'd be fine
// unfortunately however we are bound by min and max air speed limits, so
// we need to recalculate the correct course to meet at least the
// velocityDesired vector direction at our current speed
// this overwrites courseComponent
bool valid = correctCourse(courseComponent, (float *)&velocityDesired.North, fluidMovement, indicatedAirspeedDesired);
// Error condition: wind speed too high, we can't go where we want anymore
fixedWingPathFollowerStatus.Errors.Wind = 0;
if ((!valid) &&
fixedWingPathFollowerSettings.Safetymargins.Wind > 0.5f) { // alarm switched on
fixedWingPathFollowerStatus.Errors.Wind = 1;
result = 0;
}
// Airspeed error
airspeedError = indicatedAirspeedDesired - indicatedAirspeedState;
// Vertical speed error
descentspeedDesired = boundf(
velocityDesired.Down,
-fixedWingPathFollowerSettings.VerticalVelMax,
fixedWingPathFollowerSettings.VerticalVelMax);
descentspeedError = descentspeedDesired - velocityState.Down;
// Error condition: plane too slow or too fast
fixedWingPathFollowerStatus.Errors.Highspeed = 0;
fixedWingPathFollowerStatus.Errors.Lowspeed = 0;
if (indicatedAirspeedState > systemSettings.AirSpeedMax * fixedWingPathFollowerSettings.Safetymargins.Overspeed) {
fixedWingPathFollowerStatus.Errors.Overspeed = 1;
result = 0;
}
if (indicatedAirspeedState > fixedWingPathFollowerSettings.HorizontalVelMax * fixedWingPathFollowerSettings.Safetymargins.Highspeed) {
fixedWingPathFollowerStatus.Errors.Highspeed = 1;
result = 0;
}
if (indicatedAirspeedState < fixedWingPathFollowerSettings.HorizontalVelMin * fixedWingPathFollowerSettings.Safetymargins.Lowspeed) {
fixedWingPathFollowerStatus.Errors.Lowspeed = 1;
result = 0;
}
if (indicatedAirspeedState < systemSettings.AirSpeedMin * fixedWingPathFollowerSettings.Safetymargins.Stallspeed) {
fixedWingPathFollowerStatus.Errors.Stallspeed = 1;
result = 0;
}
/**
* Compute desired thrust command
*/
// Compute the cross feed from vertical speed to pitch, with saturation
float speedErrorToPowerCommandComponent = boundf(
(airspeedError / fixedWingPathFollowerSettings.HorizontalVelMin) * fixedWingPathFollowerSettings.AirspeedToPowerCrossFeed.Kp,
-fixedWingPathFollowerSettings.AirspeedToPowerCrossFeed.Max,
fixedWingPathFollowerSettings.AirspeedToPowerCrossFeed.Max
);
// Compute final thrust response
powerCommand = pid_apply(&global.PIDpower, -descentspeedError, dT) +
speedErrorToPowerCommandComponent;
// Output internal state to telemetry
fixedWingPathFollowerStatus.Error.Power = descentspeedError;
fixedWingPathFollowerStatus.ErrorInt.Power = global.PIDpower.iAccumulator;
fixedWingPathFollowerStatus.Command.Power = powerCommand;
// set thrust
stabDesired.Thrust = boundf(fixedWingPathFollowerSettings.ThrustLimit.Neutral + powerCommand,
fixedWingPathFollowerSettings.ThrustLimit.Min,
fixedWingPathFollowerSettings.ThrustLimit.Max);
// Error condition: plane cannot hold altitude at current speed.
fixedWingPathFollowerStatus.Errors.Lowpower = 0;
if (fixedWingPathFollowerSettings.ThrustLimit.Neutral + powerCommand >= fixedWingPathFollowerSettings.ThrustLimit.Max && // thrust at maximum
velocityState.Down > 0.0f && // we ARE going down
descentspeedDesired < 0.0f && // we WANT to go up
airspeedError > 0.0f && // we are too slow already
fixedWingPathFollowerSettings.Safetymargins.Lowpower > 0.5f) { // alarm switched on
fixedWingPathFollowerStatus.Errors.Lowpower = 1;
result = 0;
}
// Error condition: plane keeps climbing despite minimum thrust (opposite of above)
fixedWingPathFollowerStatus.Errors.Highpower = 0;
if (fixedWingPathFollowerSettings.ThrustLimit.Neutral + powerCommand <= fixedWingPathFollowerSettings.ThrustLimit.Min && // thrust at minimum
velocityState.Down < 0.0f && // we ARE going up
descentspeedDesired > 0.0f && // we WANT to go down
airspeedError < 0.0f && // we are too fast already
fixedWingPathFollowerSettings.Safetymargins.Highpower > 0.5f) { // alarm switched on
fixedWingPathFollowerStatus.Errors.Highpower = 1;
result = 0;
}
/**
* Compute desired pitch command
*/
// Compute the cross feed from vertical speed to pitch, with saturation
float verticalSpeedToPitchCommandComponent = boundf(-descentspeedError * fixedWingPathFollowerSettings.VerticalToPitchCrossFeed.Kp,
-fixedWingPathFollowerSettings.VerticalToPitchCrossFeed.Max,
fixedWingPathFollowerSettings.VerticalToPitchCrossFeed.Max
);
// Compute the pitch command as err*Kp + errInt*Ki + X_feed.
pitchCommand = -pid_apply(&global.PIDspeed, airspeedError, dT) + verticalSpeedToPitchCommandComponent;
fixedWingPathFollowerStatus.Error.Speed = airspeedError;
fixedWingPathFollowerStatus.ErrorInt.Speed = global.PIDspeed.iAccumulator;
fixedWingPathFollowerStatus.Command.Speed = pitchCommand;
stabDesired.Pitch = boundf(fixedWingPathFollowerSettings.PitchLimit.Neutral + pitchCommand,
fixedWingPathFollowerSettings.PitchLimit.Min,
fixedWingPathFollowerSettings.PitchLimit.Max);
// Error condition: high speed dive
fixedWingPathFollowerStatus.Errors.Pitchcontrol = 0;
if (fixedWingPathFollowerSettings.PitchLimit.Neutral + pitchCommand >= fixedWingPathFollowerSettings.PitchLimit.Max && // pitch demand is full up
velocityState.Down > 0.0f && // we ARE going down
descentspeedDesired < 0.0f && // we WANT to go up
airspeedError < 0.0f && // we are too fast already
fixedWingPathFollowerSettings.Safetymargins.Pitchcontrol > 0.5f) { // alarm switched on
fixedWingPathFollowerStatus.Errors.Pitchcontrol = 1;
result = 0;
}
/**
* Compute desired roll command
*/
courseError = RAD2DEG(atan2f(courseComponent[1], courseComponent[0])) - attitudeState.Yaw;
if (courseError < -180.0f) {
courseError += 360.0f;
}
if (courseError > 180.0f) {
courseError -= 360.0f;
}
// overlap calculation. Theres a dead zone behind the craft where the
// counter-yawing of some craft while rolling could render a desired right
// turn into a desired left turn. Making the turn direction based on
// current roll angle keeps the plane committed to a direction once chosen
if (courseError < -180.0f + (fixedWingPathFollowerSettings.ReverseCourseOverlap * 0.5f)
&& attitudeState.Roll > 0.0f) {
courseError += 360.0f;
}
if (courseError > 180.0f - (fixedWingPathFollowerSettings.ReverseCourseOverlap * 0.5f)
&& attitudeState.Roll < 0.0f) {
courseError -= 360.0f;
}
courseCommand = pid_apply(&global.PIDcourse, courseError, dT);
fixedWingPathFollowerStatus.Error.Course = courseError;
fixedWingPathFollowerStatus.ErrorInt.Course = global.PIDcourse.iAccumulator;
fixedWingPathFollowerStatus.Command.Course = courseCommand;
stabDesired.Roll = boundf(fixedWingPathFollowerSettings.RollLimit.Neutral +
courseCommand,
fixedWingPathFollowerSettings.RollLimit.Min,
fixedWingPathFollowerSettings.RollLimit.Max);
// TODO: find a check to determine loss of directional control. Likely needs some check of derivative
/**
* Compute desired yaw command
*/
// TODO implement raw control mode for yaw and base on Accels.Y
stabDesired.Yaw = 0.0f;
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
StabilizationDesiredSet(&stabDesired);
FixedWingPathFollowerStatusSet(&fixedWingPathFollowerStatus);
return result;
}
/**
* Function to calculate course vector C based on airspeed s, fluid movement F
* and desired movement vector V
* parameters in: V,F,s
* parameters out: C
* returns true if a valid solution could be found for V,F,s, false if not
* C will be set to a best effort attempt either way
*/
static bool correctCourse(float *C, float *V, float *F, float s)
{
// Approach:
// Let Sc be a circle around origin marking possible movement vectors
// of the craft with airspeed s (all possible options for C)
// Let Vl be a line through the origin along movement vector V where fr any
// point v on line Vl v = k * (V / |V|) = k' * V
// Let Wl be a line parallel to Vl where for any point v on line Vl exists
// a point w on WL with w = v - F
// Then any intersection between circle Sc and line Wl represents course
// vector which would result in a movement vector
// V' = k * ( V / |V|) = k' * V
// If there is no intersection point, S is insufficient to compensate
// for F and we can only try to fly in direction of V (thus having wind drift
// but at least making progress orthogonal to wind)
s = fabsf(s);
float f = vector_lengthf(F, 2);
// normalize Cn=V/|V|, |V| must be >0
float v = vector_lengthf(V, 2);
if (v < 1e-6f) {
// if |V|=0, we aren't supposed to move, turn into the wind
// (this allows hovering)
C[0] = -F[0];
C[1] = -F[1];
// if desired airspeed matches fluidmovement a hover is actually
// intended so return true
return fabsf(f - s) < 1e-3f;
}
float Vn[2] = { V[0] / v, V[1] / v };
// project F on V
float fp = F[0] * Vn[0] + F[1] * Vn[1];
// find component Fo of F that is orthogonal to V
// (which is exactly the distance between Vl and Wl)
float Fo[2] = { F[0] - (fp * Vn[0]), F[1] - (fp * Vn[1]) };
float fo2 = Fo[0] * Fo[0] + Fo[1] * Fo[1];
// find k where k * Vn = C - Fo
// |C|=s is the hypothenuse in any rectangular triangle formed by k * Vn and Fo
// so k^2 + fo^2 = s^2 (since |Vn|=1)
float k2 = s * s - fo2;
if (k2 <= -1e-3f) {
// there is no solution, we will be drifted off either way
// fallback: fly stupidly in direction of V and hope for the best
C[0] = V[0];
C[1] = V[1];
return false;
} else if (k2 <= 1e-3f) {
// there is exactly one solution: -Fo
C[0] = -Fo[0];
C[1] = -Fo[1];
return true;
}
// we have two possible solutions k positive and k negative as there are
// two intersection points between Wl and Sc
// which one is better? two criteria:
// 1. we MUST move in the right direction, if any k leads to -v its invalid
// 2. we should minimize the speed error
float k = sqrt(k2);
float C1[2] = { -k * Vn[0] - Fo[0], -k * Vn[1] - Fo[1] };
float C2[2] = { k *Vn[0] - Fo[0], k * Vn[1] - Fo[1] };
// project C+F on Vn to find signed resulting movement vector length
float vp1 = (C1[0] + F[0]) * Vn[0] + (C1[1] + F[1]) * Vn[1];
float vp2 = (C2[0] + F[0]) * Vn[0] + (C2[1] + F[1]) * Vn[1];
if (vp1 >= 0.0f && fabsf(v - vp1) < fabsf(v - vp2)) {
// in this case the angle between course and resulting movement vector
// is greater than 90 degrees - so we actually fly backwards
C[0] = C1[0];
C[1] = C1[1];
return true;
}
C[0] = C2[0];
C[1] = C2[1];
if (vp2 >= 0.0f) {
// in this case the angle between course and movement vector is less than
// 90 degrees, but we do move in the right direction
return true;
} else {
// in this case we actually get driven in the opposite direction of V
// with both solutions for C
// this might be reached in headwind stronger than maximum allowed
// airspeed.
return false;
}
}
/**
* Compute desired attitude from the desired velocity
*
* Takes in @ref NedState which has the acceleration in the
* NED frame as the feedback term and then compares the
* @ref VelocityState against the @ref VelocityDesired
*/
static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction)
{
const float dT = updatePeriod / 1000.0f;
uint8_t result = 1;
bool manual_thrust = false;
VelocityDesiredData velocityDesired;
VelocityStateData velocityState;
StabilizationDesiredData stabDesired;
AttitudeStateData attitudeState;
StabilizationBankData stabSettings;
SystemSettingsData systemSettings;
VtolSelfTuningStatsData vtolSelfTuningStats;
float northError;
float northCommand;
float eastError;
float eastCommand;
float downError;
float downCommand;
SystemSettingsGet(&systemSettings);
VelocityStateGet(&velocityState);
VelocityDesiredGet(&velocityDesired);
StabilizationDesiredGet(&stabDesired);
VelocityDesiredGet(&velocityDesired);
AttitudeStateGet(&attitudeState);
StabilizationBankGet(&stabSettings);
VtolSelfTuningStatsGet(&vtolSelfTuningStats);
if (pathDesired.Mode != PATHDESIRED_MODE_BRAKE) {
// scale velocity if it is above configured maximum
// for braking, we can not help it if initial velocity was greater
float velH = sqrtf(velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East);
if (velH > vtolPathFollowerSettings.HorizontalVelMax) {
velocityDesired.North *= vtolPathFollowerSettings.HorizontalVelMax / velH;
velocityDesired.East *= vtolPathFollowerSettings.HorizontalVelMax / velH;
}
if (fabsf(velocityDesired.Down) > vtolPathFollowerSettings.VerticalVelMax) {
velocityDesired.Down *= vtolPathFollowerSettings.VerticalVelMax / fabsf(velocityDesired.Down);
}
}
// calculate the velocity errors between desired and actual
northError = velocityDesired.North - velocityState.North;
eastError = velocityDesired.East - velocityState.East;
downError = velocityDesired.Down - velocityState.Down;
// Must flip this sign
downError = -downError;
// Compute desired commands
if (pathDesired.Mode != PATHDESIRED_MODE_BRAKE) {
northCommand = pid_apply(&global.PIDvel[0], northError, dT) + velocityDesired.North * vtolPathFollowerSettings.VelocityFeedforward;
eastCommand = pid_apply(&global.PIDvel[1], eastError, dT) + velocityDesired.East * vtolPathFollowerSettings.VelocityFeedforward;
} else {
northCommand = pid_apply(&global.BrakePIDvel[0], northError, dT) + velocityDesired.North * vtolPathFollowerSettings.BrakeVelocityFeedforward;
eastCommand = pid_apply(&global.BrakePIDvel[1], eastError, dT) + velocityDesired.East * vtolPathFollowerSettings.BrakeVelocityFeedforward;
}
downCommand = pid_apply(&global.PIDvel[2], downError, dT);
if ((vtolPathFollowerSettings.ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_MANUAL &&
flightStatus.FlightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) ||
(flightStatus.FlightModeAssist && flightStatus.AssistedThrottleState == FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL)) {
manual_thrust = true;
}
// if auto thrust and we have not run a correction calc check for PH and stability to then run an assessment
// note that arming into this flight mode is not allowed, so assumption here is that
// we have not landed. If GPSAssist+Manual/Cruise control thrust mode is used, a user overriding the
// altitude maintaining PID will have moved the throttle state to FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL.
// In manualcontrol.c the state will stay in manual throttle until the throttle command exceeds the vtol thrust min,
// avoiding auto-takeoffs. Therefore no need to check that here.
if (!manual_thrust && neutralThrustEst.have_correction != true) {
// Assess if position hold state running. This can be normal position hold or
// another mode with assist-hold active.
bool ph_active =
((flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD &&
flightStatus.FlightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) ||
(flightStatus.FlightModeAssist != FLIGHTSTATUS_FLIGHTMODEASSIST_NONE &&
flightStatus.AssistedControlState == FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD));
bool stable = (fabsf(pathStatus.correction_direction_down) < NEUTRALTHRUST_PH_POSITIONAL_ERROR_LIMIT &&
fabsf(velocityDesired.Down) < NEUTRALTHRUST_PH_VEL_DESIRED_LIMIT &&
fabsf(velocityState.Down) < NEUTRALTHRUST_PH_VEL_STATE_LIMIT &&
fabsf(downError) < NEUTRALTHRUST_PH_VEL_ERROR_LIMIT);
if (ph_active && stable) {
if (neutralThrustEst.start_sampling) {
neutralThrustEst.count++;
// delay count for 2 seconds into hold allowing for stablisation
if (neutralThrustEst.count > NEUTRALTHRUST_START_DELAY) {
neutralThrustEst.sum += global.PIDvel[2].iAccumulator;
if (global.PIDvel[2].iAccumulator < neutralThrustEst.min) {
neutralThrustEst.min = global.PIDvel[2].iAccumulator;
}
if (global.PIDvel[2].iAccumulator > neutralThrustEst.max) {
neutralThrustEst.max = global.PIDvel[2].iAccumulator;
}
}
if (neutralThrustEst.count >= NEUTRALTHRUST_END_COUNT) {
// 6 seconds have past
// lets take an average
neutralThrustEst.average = neutralThrustEst.sum / (float)(NEUTRALTHRUST_END_COUNT - NEUTRALTHRUST_START_DELAY);
neutralThrustEst.correction = neutralThrustEst.average / 1000.0f;
global.PIDvel[2].iAccumulator -= neutralThrustEst.average;
neutralThrustEst.start_sampling = false;
neutralThrustEst.have_correction = true;
// Write a new adjustment value
// vtolSelfTuningStats.NeutralThrustOffset was incremental adjusted above
VtolSelfTuningStatsData new_vtolSelfTuningStats;
// add the average remaining i value to the
new_vtolSelfTuningStats.NeutralThrustOffset = vtolSelfTuningStats.NeutralThrustOffset + neutralThrustEst.correction;
new_vtolSelfTuningStats.NeutralThrustCorrection = neutralThrustEst.correction; // the i term thrust correction value applied
new_vtolSelfTuningStats.NeutralThrustAccumulator = global.PIDvel[2].iAccumulator; // the actual iaccumulator value after correction
new_vtolSelfTuningStats.NeutralThrustRange = neutralThrustEst.max - neutralThrustEst.min;
VtolSelfTuningStatsSet(&new_vtolSelfTuningStats);
}
} else {
// start a tick count
neutralThrustEst.start_sampling = true;
neutralThrustEst.count = 0;
neutralThrustEst.sum = 0.0f;
neutralThrustEst.max = 0.0f;
neutralThrustEst.min = 0.0f;
}
} else {
// reset sampling as we did't get 6 continuous seconds
neutralThrustEst.start_sampling = false;
}
} // else we already have a correction for this PH run
// Generally in braking the downError will be an increased altitude. We really will rely on cruisecontrol to backoff.
stabDesired.Thrust = boundf(vtolSelfTuningStats.NeutralThrustOffset + downCommand + vtolPathFollowerSettings.ThrustLimits.Neutral, vtolPathFollowerSettings.ThrustLimits.Min, vtolPathFollowerSettings.ThrustLimits.Max);
// DEBUG HACK: allow user to skew compass on purpose to see if emergency failsafe kicks in
if (vtolPathFollowerSettings.FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_DEBUGTEST) {
attitudeState.Yaw += 120.0f;
if (attitudeState.Yaw > 180.0f) {
attitudeState.Yaw -= 360.0f;
}
}
if ( // emergency flyaway detection
( // integral already at its limit
vtolPathFollowerSettings.HorizontalVelPID.ILimit - fabsf(global.PIDvel[0].iAccumulator) < 1e-6f ||
vtolPathFollowerSettings.HorizontalVelPID.ILimit - fabsf(global.PIDvel[1].iAccumulator) < 1e-6f
) &&
// angle between desired and actual velocity >90 degrees (by dot product)
(velocityDesired.North * velocityState.North + velocityDesired.East * velocityState.East < 0.0f) &&
// quad is moving at significant speed (during flyaway it would keep speeding up)
squaref(velocityState.North) + squaref(velocityState.East) > 1.0f
) {
global.vtolEmergencyFallback += dT;
if (global.vtolEmergencyFallback >= vtolPathFollowerSettings.FlyawayEmergencyFallbackTriggerTime) {
// after emergency timeout, trigger alarm - everything else is handled by callers
// (switch to emergency algorithm, switch to emergency waypoint in pathplanner, alarms, ...)
result = 0;
}
} else {
global.vtolEmergencyFallback = 0.0f;
}
// Project the north and east command signals into the pitch and roll based on yaw. For this to behave well the
// craft should move similarly for 5 deg roll versus 5 deg pitch
float maxPitch = vtolPathFollowerSettings.MaxRollPitch;
if (pathDesired.Mode == PATHDESIRED_MODE_BRAKE) {
maxPitch = vtolPathFollowerSettings.BrakeMaxPitch;
}
stabDesired.Pitch = boundf(-northCommand * cosf(DEG2RAD(attitudeState.Yaw)) +
-eastCommand * sinf(DEG2RAD(attitudeState.Yaw)),
-maxPitch, maxPitch);
stabDesired.Roll = boundf(-northCommand * sinf(DEG2RAD(attitudeState.Yaw)) +
eastCommand * cosf(DEG2RAD(attitudeState.Yaw)),
-maxPitch, maxPitch);
ManualControlCommandData manualControl;
ManualControlCommandGet(&manualControl);
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
if (yaw_attitude) {
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.Yaw = yaw_direction;
} else {
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw;
}
// default thrust mode to cruise control
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL;
// when flight mode assist is active but in primary-thrust mode, the thrust mode must be set to the same as per the primary mode.
if (flightStatus.FlightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST_PRIMARYTHRUST) {
FlightModeSettingsData settings;
FlightModeSettingsGet(&settings);
FlightModeSettingsStabilization1SettingsOptions thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL;
switch (flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
thrustMode = settings.Stabilization1Settings.Thrust;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
thrustMode = settings.Stabilization2Settings.Thrust;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
thrustMode = settings.Stabilization3Settings.Thrust;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4:
thrustMode = settings.Stabilization4Settings.Thrust;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5:
thrustMode = settings.Stabilization5Settings.Thrust;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
thrustMode = settings.Stabilization6Settings.Thrust;
break;
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
// we hard code the "GPS Assisted" PostionHold to use alt-vario which
// is a more appropriate throttle mode. "GPSAssist" adds braking
// and a better throttle management to the standard Position Hold.
thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO;
break;
}
stabDesired.StabilizationMode.Thrust = thrustMode;
stabDesired.Thrust = manualControl.Thrust;
} else if (manual_thrust) {
stabDesired.Thrust = manualControl.Thrust;
}
// else thrust is set by the PID controller
StabilizationDesiredSet(&stabDesired);
return result;
}
/**
* Compute desired attitude for vtols - emergency fallback
*/
static void updateVtolDesiredAttitudeEmergencyFallback()
{
const float dT = updatePeriod / 1000.0f;
VelocityDesiredData velocityDesired;
VelocityStateData velocityState;
StabilizationDesiredData stabDesired;
float courseError;
float courseCommand;
float downError;
float downCommand;
VelocityStateGet(&velocityState);
VelocityDesiredGet(&velocityDesired);
ManualControlCommandData manualControlData;
ManualControlCommandGet(&manualControlData);
courseError = RAD2DEG(atan2f(velocityDesired.East, velocityDesired.North) - atan2f(velocityState.East, velocityState.North));
if (courseError < -180.0f) {
courseError += 360.0f;
}
if (courseError > 180.0f) {
courseError -= 360.0f;
}
courseCommand = (courseError * vtolPathFollowerSettings.EmergencyFallbackYawRate.kP);
stabDesired.Yaw = boundf(courseCommand, -vtolPathFollowerSettings.EmergencyFallbackYawRate.Max, vtolPathFollowerSettings.EmergencyFallbackYawRate.Max);
// Compute desired down command
downError = velocityDesired.Down - velocityState.Down;
// Must flip this sign
downError = -downError;
downCommand = pid_apply(&global.PIDvel[2], downError, dT);
stabDesired.Thrust = boundf(downCommand + vtolPathFollowerSettings.ThrustLimits.Neutral, vtolPathFollowerSettings.ThrustLimits.Min, vtolPathFollowerSettings.ThrustLimits.Max);
stabDesired.Roll = vtolPathFollowerSettings.EmergencyFallbackAttitude.Roll;
stabDesired.Pitch = vtolPathFollowerSettings.EmergencyFallbackAttitude.Pitch;
if (vtolPathFollowerSettings.ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_MANUAL) {
// For now override thrust with manual control. Disable at your risk, quad goes to China.
ManualControlCommandData manualControl;
ManualControlCommandGet(&manualControl);
stabDesired.Thrust = manualControl.Thrust;
}
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL;
StabilizationDesiredSet(&stabDesired);
}
/**
* Compute desired attitude from a fixed preset
*
*/
static void updateFixedAttitude(float *attitude)
{
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
stabDesired.Roll = attitude[0];
stabDesired.Pitch = attitude[1];
stabDesired.Yaw = attitude[2];
stabDesired.Thrust = attitude[3];
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
StabilizationDesiredSet(&stabDesired);
}

470
flight/modules/PathFollower/pathfollower.cpp Normal file
View File

@ -0,0 +1,470 @@
/**
******************************************************************************
*
* @file pathfollower.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief This module compared @ref PositionActuatl to @ref ActiveWaypoint
* and sets @ref AttitudeDesired. It only does this when the FlightMode field
* of @ref ManualControlCommand is Auto.
*
* @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
*/
/**
* Input object: PathDesired
* Input object: PositionState
* Input object: ManualControlCommand
* Output object: StabilizationDesired
*
* This module acts as "autopilot" - it controls the setpoints of stabilization
* based on current flight situation and desired flight path (PathDesired) as
* directed by flightmode selection or pathplanner
* This is a periodic delayed callback module
*
* Modules have no API, all communication to other modules is done through UAVObjects.
* However modules may use the API exposed by shared libraries.
* See the OpenPilot wiki for more details.
* http://www.openpilot.org/OpenPilot_Application_Architecture
*
*/
extern "C" {
#include <openpilot.h>
#include <callbackinfo.h>
#include <math.h>
#include <pid.h>
#include <CoordinateConversions.h>
#include <sin_lookup.h>
#include <pathdesired.h>
#include <paths.h>
#include "plans.h"
#include <sanitycheck.h>
#include <groundpathfollowersettings.h>
#include <fixedwingpathfollowersettings.h>
#include <fixedwingpathfollowerstatus.h>
#include <vtolpathfollowersettings.h>
#include <flightstatus.h>
#include <flightmodesettings.h>
#include <pathstatus.h>
#include <positionstate.h>
#include <velocitystate.h>
#include <velocitydesired.h>
#include <stabilizationdesired.h>
#include <airspeedstate.h>
#include <attitudestate.h>
#include <takeofflocation.h>
#include <poilocation.h>
#include <manualcontrolcommand.h>
#include <systemsettings.h>
#include <stabilizationbank.h>
#include <vtolselftuningstats.h>
#include <pathsummary.h>
#include <pidstatus.h>
#include <homelocation.h>
#include <accelstate.h>
#include <statusvtolautotakeoff.h>
#include <statusvtolland.h>
#include <statusgrounddrive.h>
}
#include "pathfollowercontrol.h"
#include "vtollandcontroller.h"
#include "vtolautotakeoffcontroller.h"
#include "vtolvelocitycontroller.h"
#include "vtolbrakecontroller.h"
#include "vtolflycontroller.h"
#include "fixedwingflycontroller.h"
#include "grounddrivecontroller.h"
// Private constants
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW
#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL
#define PF_IDLE_UPDATE_RATE_MS 100
#define STACK_SIZE_BYTES 2048
// Private types
// Private variables
static DelayedCallbackInfo *pathFollowerCBInfo;
static uint32_t updatePeriod = PF_IDLE_UPDATE_RATE_MS;
static FrameType_t frameType = FRAME_TYPE_MULTIROTOR;
static PathStatusData pathStatus;
static PathDesiredData pathDesired;
static FixedWingPathFollowerSettingsData fixedWingPathFollowerSettings;
static GroundPathFollowerSettingsData groundPathFollowerSettings;
static VtolPathFollowerSettingsData vtolPathFollowerSettings;
static FlightStatusData flightStatus;
static PathFollowerControl *activeController = 0;
// Private functions
static void pathFollowerTask(void);
static void SettingsUpdatedCb(UAVObjEvent *ev);
static void airspeedStateUpdatedCb(UAVObjEvent *ev);
static void pathFollowerObjectiveUpdatedCb(UAVObjEvent *ev);
static void flightStatusUpdatedCb(UAVObjEvent *ev);
static void updateFixedAttitude(float *attitude);
static void pathFollowerInitializeControllersForFrameType();
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
extern "C" int32_t PathFollowerStart()
{
// Start main task
PathStatusGet(&pathStatus);
SettingsUpdatedCb(NULL);
PIOS_CALLBACKSCHEDULER_Dispatch(pathFollowerCBInfo);
return 0;
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
extern "C" int32_t PathFollowerInitialize()
{
// initialize objects
GroundPathFollowerSettingsInitialize();
FixedWingPathFollowerSettingsInitialize();
FixedWingPathFollowerStatusInitialize();
VtolPathFollowerSettingsInitialize();
FlightStatusInitialize();
FlightModeSettingsInitialize();
PathStatusInitialize();
PathSummaryInitialize();
PathDesiredInitialize();
PositionStateInitialize();
VelocityStateInitialize();
VelocityDesiredInitialize();
StabilizationDesiredInitialize();
AirspeedStateInitialize();
AttitudeStateInitialize();
TakeOffLocationInitialize();
PoiLocationInitialize();
ManualControlCommandInitialize();
SystemSettingsInitialize();
StabilizationBankInitialize();
VtolSelfTuningStatsInitialize();
PIDStatusInitialize();
StatusVtolLandInitialize();
StatusGroundDriveInitialize();
StatusVtolAutoTakeoffInitialize();
// VtolLandFSM additional objects
HomeLocationInitialize();
AccelStateInitialize();
// Init references to controllers
PathFollowerControl::Initialize(&pathDesired, &flightStatus, &pathStatus);
// Create object queue
pathFollowerCBInfo = PIOS_CALLBACKSCHEDULER_Create(&pathFollowerTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_PATHFOLLOWER, STACK_SIZE_BYTES);
FixedWingPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
GroundPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
PathDesiredConnectCallback(&pathFollowerObjectiveUpdatedCb);
FlightStatusConnectCallback(&flightStatusUpdatedCb);
SystemSettingsConnectCallback(&SettingsUpdatedCb);
AirspeedStateConnectCallback(&airspeedStateUpdatedCb);
VtolSelfTuningStatsConnectCallback(&SettingsUpdatedCb);
return 0;
}
MODULE_INITCALL(PathFollowerInitialize, PathFollowerStart);
void pathFollowerInitializeControllersForFrameType()
{
static uint8_t multirotor_initialised = 0;
static uint8_t fixedwing_initialised = 0;
static uint8_t ground_initialised = 0;
switch (frameType) {
case FRAME_TYPE_MULTIROTOR:
case FRAME_TYPE_HELI:
if (!multirotor_initialised) {
VtolLandController::instance()->Initialize(&vtolPathFollowerSettings);
VtolAutoTakeoffController::instance()->Initialize(&vtolPathFollowerSettings);
VtolVelocityController::instance()->Initialize(&vtolPathFollowerSettings);
VtolFlyController::instance()->Initialize(&vtolPathFollowerSettings);
VtolBrakeController::instance()->Initialize(&vtolPathFollowerSettings);
multirotor_initialised = 1;
}
break;
case FRAME_TYPE_FIXED_WING:
if (!fixedwing_initialised) {
FixedWingFlyController::instance()->Initialize(&fixedWingPathFollowerSettings);
fixedwing_initialised = 1;
}
break;
case FRAME_TYPE_GROUND:
if (!ground_initialised) {
GroundDriveController::instance()->Initialize(&groundPathFollowerSettings);
ground_initialised = 1;
}
break;
default:
break;
}
}
static void pathFollowerSetActiveController(void)
{
// Init controllers for the frame type
if (activeController == 0) {
// Initialise
pathFollowerInitializeControllersForFrameType();
switch (frameType) {
case FRAME_TYPE_MULTIROTOR:
case FRAME_TYPE_HELI:
switch (pathDesired.Mode) {
case PATHDESIRED_MODE_BRAKE: // brake then hold sequence controller
activeController = VtolBrakeController::instance();
activeController->Activate();
break;
case PATHDESIRED_MODE_VELOCITY: // velocity roam controller
activeController = VtolVelocityController::instance();
activeController->Activate();
break;
case PATHDESIRED_MODE_GOTOENDPOINT:
case PATHDESIRED_MODE_FOLLOWVECTOR:
case PATHDESIRED_MODE_CIRCLERIGHT:
case PATHDESIRED_MODE_CIRCLELEFT:
activeController = VtolFlyController::instance();
activeController->Activate();
break;
case PATHDESIRED_MODE_LAND: // land with optional velocity roam option
activeController = VtolLandController::instance();
activeController->Activate();
break;
case PATHDESIRED_MODE_AUTOTAKEOFF:
activeController = VtolAutoTakeoffController::instance();
activeController->Activate();
break;
default:
activeController = 0;
break;
}
break;
case FRAME_TYPE_FIXED_WING:
switch (pathDesired.Mode) {
case PATHDESIRED_MODE_GOTOENDPOINT:
case PATHDESIRED_MODE_FOLLOWVECTOR:
case PATHDESIRED_MODE_CIRCLERIGHT:
case PATHDESIRED_MODE_CIRCLELEFT:
activeController = FixedWingFlyController::instance();
activeController->Activate();
break;
default:
activeController = 0;
break;
}
break;
case FRAME_TYPE_GROUND:
switch (pathDesired.Mode) {
case PATHDESIRED_MODE_GOTOENDPOINT:
case PATHDESIRED_MODE_FOLLOWVECTOR:
case PATHDESIRED_MODE_CIRCLERIGHT:
case PATHDESIRED_MODE_CIRCLELEFT:
activeController = GroundDriveController::instance();
activeController->Activate();
break;
default:
activeController = 0;
break;
}
break;
default:
activeController = 0;
break;
}
}
}
/**
* Module thread, should not return.
*/
static void pathFollowerTask(void)
{
FlightStatusGet(&flightStatus);
if (flightStatus.ControlChain.PathFollower != FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_UNINITIALISED);
PIOS_CALLBACKSCHEDULER_Schedule(pathFollowerCBInfo, PF_IDLE_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
return;
}
pathStatus.UID = pathDesired.UID;
pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
pathFollowerSetActiveController();
if (activeController) {
activeController->UpdateAutoPilot();
PIOS_CALLBACKSCHEDULER_Schedule(pathFollowerCBInfo, updatePeriod, CALLBACK_UPDATEMODE_SOONER);
return;
}
switch (pathDesired.Mode) {
case PATHDESIRED_MODE_FIXEDATTITUDE:
updateFixedAttitude(pathDesired.ModeParameters);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
break;
case PATHDESIRED_MODE_DISARMALARM:
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
break;
default:
pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
break;
}
PathStatusSet(&pathStatus);
PIOS_CALLBACKSCHEDULER_Schedule(pathFollowerCBInfo, updatePeriod, CALLBACK_UPDATEMODE_SOONER);
}
static void pathFollowerObjectiveUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
PathDesiredGet(&pathDesired);
if (activeController && pathDesired.Mode != activeController->Mode()) {
activeController->Deactivate();
activeController = 0;
}
pathFollowerSetActiveController();
if (activeController) {
activeController->ObjectiveUpdated();
}
}
// we use this to deactivate active controllers as the pathdesired
// objective never gets updated with switching to a non-pathfollower flight mode
static void flightStatusUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
if (!activeController) {
return;
}
FlightStatusControlChainData controlChain;
FlightStatusControlChainGet(&controlChain);
if (controlChain.PathFollower != FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
activeController->Deactivate();
activeController = 0;
}
}
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
VtolPathFollowerSettingsGet(&vtolPathFollowerSettings);
FrameType_t previous_frameType = frameType;
frameType = GetCurrentFrameType();
if (frameType == FRAME_TYPE_CUSTOM) {
switch (vtolPathFollowerSettings.TreatCustomCraftAs) {
case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_FIXEDWING:
frameType = FRAME_TYPE_FIXED_WING;
break;
case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_VTOL:
frameType = FRAME_TYPE_MULTIROTOR;
break;
case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_GROUND:
frameType = FRAME_TYPE_GROUND;
break;
}
}
// If frame type changes, initialise the frame type controllers
if (frameType != previous_frameType) {
pathFollowerInitializeControllersForFrameType();
}
switch (frameType) {
case FRAME_TYPE_MULTIROTOR:
case FRAME_TYPE_HELI:
updatePeriod = vtolPathFollowerSettings.UpdatePeriod;
break;
case FRAME_TYPE_FIXED_WING:
FixedWingPathFollowerSettingsGet(&fixedWingPathFollowerSettings);
updatePeriod = fixedWingPathFollowerSettings.UpdatePeriod;
break;
case FRAME_TYPE_GROUND:
GroundPathFollowerSettingsGet(&groundPathFollowerSettings);
updatePeriod = groundPathFollowerSettings.UpdatePeriod;
break;
default:
updatePeriod = vtolPathFollowerSettings.UpdatePeriod;
break;
}
if (activeController) {
activeController->SettingsUpdated();
}
}
static void airspeedStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
FixedWingFlyController::instance()->AirspeedStateUpdatedCb(ev);
}
/**
* Compute desired attitude from a fixed preset
*
*/
static void updateFixedAttitude(float *attitude)
{
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
stabDesired.Roll = attitude[0];
stabDesired.Pitch = attitude[1];
stabDesired.Yaw = attitude[2];
stabDesired.Thrust = attitude[3];
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
StabilizationDesiredSet(&stabDesired);
}

49
flight/targets/boards/coptercontrol/firmware/inc/pios_usb_board_data.h → flight/modules/PathFollower/pathfollowercontrol.cpp
View File

@ -1,14 +1,9 @@
/**
/*
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_USB_BOARD Board specific USB definitions
* @brief Board specific USB definitions
* @{
*
* @file pios_usb_board_data.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Board specific USB definitions
* @file PathFollowerControl.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Controller interface class implementation
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
@ -28,20 +23,30 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef PIOS_USB_BOARD_DATA_H
#define PIOS_USB_BOARD_DATA_H
extern "C" {
#include <openpilot.h>
#include <flightstatus.h>
#include <pathstatus.h>
#include <pathdesired.h>
}
// Note : changing below length will require changes to the USB buffer setup
#define PIOS_USB_BOARD_CDC_DATA_LENGTH 64
#define PIOS_USB_BOARD_CDC_MGMT_LENGTH 32
#define PIOS_USB_BOARD_HID_DATA_LENGTH 64
// C++ includes
#include "pathfollowercontrol.h"
#define PIOS_USB_BOARD_EP_NUM 4
PathDesiredData *PathFollowerControl::pathDesired = 0;
FlightStatusData *PathFollowerControl::flightStatus = 0;
PathStatusData *PathFollowerControl::pathStatus = 0;
#include <pios_usb_defs.h> /* USB_* macros */
int32_t PathFollowerControl::Initialize(PathDesiredData *ptr_pathDesired,
FlightStatusData *ptr_flightStatus,
PathStatusData *ptr_pathStatus)
{
PIOS_Assert(ptr_pathDesired);
PIOS_Assert(ptr_flightStatus);
PIOS_Assert(ptr_pathStatus);
#define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_COPTERCONTROL
#define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_COPTERCONTROL, USB_OP_BOARD_MODE_FW)
#define PIOS_USB_BOARD_SN_SUFFIX "+FW"
#endif /* PIOS_USB_BOARD_DATA_H */
pathDesired = ptr_pathDesired;
flightStatus = ptr_flightStatus;
pathStatus = ptr_pathStatus;
return 0;
}

244
flight/modules/PathFollower/pidcontrolne.cpp Normal file
View File

@ -0,0 +1,244 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup PathFollower CONTROL interface class
* @brief PID controller for NE
* @{
*
* @file PIDControlNE.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Executes PID control loops for NE directions
*
* @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
*/
extern "C" {
#include <openpilot.h>
#include <callbackinfo.h>
#include <math.h>
#include <pid.h>
#include <CoordinateConversions.h>
#include <sin_lookup.h>
#include <pathdesired.h>
#include <paths.h>
#include "plans.h"
#include <pidstatus.h>
}
#include "pathfollowerfsm.h"
#include "pidcontrolne.h"
PIDControlNE::PIDControlNE()
: deltaTime(0), mNECommand(0), mNeutral(0), mVelocityMax(0), mMinCommand(0), mMaxCommand(0), mVelocityFeedforward(0), mActive(false)
{}
PIDControlNE::~PIDControlNE() {}
void PIDControlNE::Initialize()
{}
void PIDControlNE::Deactivate()
{
mActive = false;
}
void PIDControlNE::Activate()
{
mActive = true;
}
void PIDControlNE::UpdateParameters(float kp, float ki, float kd, float beta, float dT, float velocityMax)
{
// pid_configure(&PID, kp, ki, kd, ilimit);
float Ti = kp / ki;
float Td = kd / kp;
float Tt = (Ti + Td) / 2.0f;
float kt = 1.0f / Tt;
float u0 = 0.0f;
float N = 10.0f;
float Tf = Td / N;
if (ki < 1e-6f) {
// Avoid Ti being infinite
Ti = 1e6f;
// Tt antiwindup time constant - we don't need antiwindup with no I term
Tt = 1e6f;
kt = 0.0f;
}
if (kd < 1e-6f) {
// PI Controller
Tf = Ti / N;
}
if (beta > 1.0f) {
beta = 1.0f;
} else if (beta < 0.4f) {
beta = 0.4f;
}
pid2_configure(&PIDvel[0], kp, ki, kd, Tf, kt, dT, beta, u0, 0.0f, 1.0f);
pid2_configure(&PIDvel[1], kp, ki, kd, Tf, kt, dT, beta, u0, 0.0f, 1.0f);
deltaTime = dT;
mVelocityMax = velocityMax;
}
void PIDControlNE::UpdatePositionalParameters(float kp)
{
pid_configure(&PIDposH[0], kp, 0.0f, 0.0f, 0.0f);
pid_configure(&PIDposH[1], kp, 0.0f, 0.0f, 0.0f);
}
void PIDControlNE::UpdatePositionSetpoint(float setpointNorth, float setpointEast)
{
mPositionSetpointTarget[0] = setpointNorth;
mPositionSetpointTarget[1] = setpointEast;
}
void PIDControlNE::UpdatePositionState(float pvNorth, float pvEast)
{
mPositionState[0] = pvNorth;
mPositionState[1] = pvEast;
}
// This is a pure position hold position control
void PIDControlNE::ControlPosition()
{
// Current progress location relative to end
float velNorth = 0.0f;
float velEast = 0.0f;
velNorth = pid_apply(&PIDposH[0], mPositionSetpointTarget[0] - mPositionState[0], deltaTime);
velEast = pid_apply(&PIDposH[1], mPositionSetpointTarget[1] - mPositionState[1], deltaTime);
UpdateVelocitySetpoint(velNorth, velEast);
}
void PIDControlNE::ControlPositionWithPath(struct path_status *progress)
{
// Current progress location relative to end
float velNorth = progress->path_vector[0];
float velEast = progress->path_vector[1];
velNorth += pid_apply(&PIDposH[0], progress->correction_vector[0], deltaTime);
velEast += pid_apply(&PIDposH[1], progress->correction_vector[1], deltaTime);
UpdateVelocitySetpoint(velNorth, velEast);
}
void PIDControlNE::UpdateVelocitySetpoint(float setpointNorth, float setpointEast)
{
// scale velocity if it is above configured maximum
// for braking, we can not help it if initial velocity was greater
float velH = sqrtf(setpointNorth * setpointNorth + setpointEast * setpointEast);
if (velH > mVelocityMax) {
setpointNorth *= mVelocityMax / velH;
setpointEast *= mVelocityMax / velH;
}
mVelocitySetpointTarget[0] = setpointNorth;
mVelocitySetpointTarget[1] = setpointEast;
}
void PIDControlNE::UpdateBrakeVelocity(float startingVelocity, float dT, float brakeRate, float currentVelocity, float *updatedVelocity)
{
if (startingVelocity >= 0.0f) {
*updatedVelocity = startingVelocity - dT * brakeRate;
if (*updatedVelocity > currentVelocity) {
*updatedVelocity = currentVelocity;
}
if (*updatedVelocity < 0.0f) {
*updatedVelocity = 0.0f;
}
} else {
*updatedVelocity = startingVelocity + dT * brakeRate;
if (*updatedVelocity < currentVelocity) {
*updatedVelocity = currentVelocity;
}
if (*updatedVelocity > 0.0f) {
*updatedVelocity = 0.0f;
}
}
}
void PIDControlNE::UpdateVelocityStateWithBrake(float pvNorth, float pvEast, float path_time, float brakeRate)
{
mVelocityState[0] = pvNorth;
mVelocityState[1] = pvEast;
float velocitySetpointDesired[2];
UpdateBrakeVelocity(mVelocitySetpointTarget[0], path_time, brakeRate, pvNorth, &velocitySetpointDesired[0]);
UpdateBrakeVelocity(mVelocitySetpointTarget[1], path_time, brakeRate, pvEast, &velocitySetpointDesired[1]);
// If rate of change limits required, add here
for (int iaxis = 0; iaxis < 2; iaxis++) {
mVelocitySetpointCurrent[iaxis] = velocitySetpointDesired[iaxis];
}
}
void PIDControlNE::UpdateVelocityState(float pvNorth, float pvEast)
{
mVelocityState[0] = pvNorth;
mVelocityState[1] = pvEast;
// The FSM controls the actual descent velocity and introduces step changes as required
float velocitySetpointDesired[2];
velocitySetpointDesired[0] = mVelocitySetpointTarget[0];
velocitySetpointDesired[1] = mVelocitySetpointTarget[1];
// If rate of change limits required, add here
for (int iaxis = 0; iaxis < 2; iaxis++) {
mVelocitySetpointCurrent[iaxis] = velocitySetpointDesired[iaxis];
}
}
void PIDControlNE::UpdateCommandParameters(float minCommand, float maxCommand, float velocityFeedforward)
{
mMinCommand = minCommand;
mMaxCommand = maxCommand;
mVelocityFeedforward = velocityFeedforward;
}
void PIDControlNE::GetNECommand(float *northCommand, float *eastCommand)
{
PIDvel[0].va = mVelocitySetpointCurrent[0] * mVelocityFeedforward;
*northCommand = pid2_apply(&(PIDvel[0]), mVelocitySetpointCurrent[0], mVelocityState[0], mMinCommand, mMaxCommand);
PIDvel[1].va = mVelocitySetpointCurrent[1] * mVelocityFeedforward;
*eastCommand = pid2_apply(&(PIDvel[1]), mVelocitySetpointCurrent[1], mVelocityState[1], mMinCommand, mMaxCommand);
PIDStatusData pidStatus;
pidStatus.setpoint = mVelocitySetpointCurrent[0];
pidStatus.actual = mVelocityState[0];
pidStatus.error = mVelocitySetpointCurrent[0] - mVelocityState[0];
pidStatus.setpoint = mVelocitySetpointCurrent[0];
pidStatus.ulow = mMinCommand;
pidStatus.uhigh = mMaxCommand;
pidStatus.command = *northCommand;
pidStatus.P = PIDvel[0].P;
pidStatus.I = PIDvel[0].I;
pidStatus.D = PIDvel[0].D;
PIDStatusSet(&pidStatus);
}
void PIDControlNE::GetVelocityDesired(float *north, float *east)
{
*north = mVelocitySetpointCurrent[0];
*east = mVelocitySetpointCurrent[1];
}

287
flight/modules/PathFollower/vtolautotakeoffcontroller.cpp Normal file
View File

@ -0,0 +1,287 @@
/*
******************************************************************************
*
* @file vtollandcontroller.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Vtol landing controller loop
* @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
*/
extern "C" {
#include <openpilot.h>
#include <callbackinfo.h>
#include <math.h>
#include <pid.h>
#include <CoordinateConversions.h>
#include <sin_lookup.h>
#include <pathdesired.h>
#include <paths.h>
#include "plans.h"
#include <sanitycheck.h>
#include <homelocation.h>
#include <accelstate.h>
#include <vtolpathfollowersettings.h>
#include <flightstatus.h>
#include <flightmodesettings.h>
#include <pathstatus.h>
#include <positionstate.h>
#include <velocitystate.h>
#include <velocitydesired.h>
#include <stabilizationdesired.h>
#include <airspeedstate.h>
#include <attitudestate.h>
#include <takeofflocation.h>
#include <poilocation.h>
#include <manualcontrolcommand.h>
#include <systemsettings.h>
#include <stabilizationbank.h>
#include <stabilizationdesired.h>
#include <vtolselftuningstats.h>
#include <pathsummary.h>
}
// C++ includes
#include "vtolautotakeoffcontroller.h"
#include "pathfollowerfsm.h"
#include "vtolautotakeofffsm.h"
#include "pidcontroldown.h"
// Private constants
// pointer to a singleton instance
VtolAutoTakeoffController *VtolAutoTakeoffController::p_inst = 0;
VtolAutoTakeoffController::VtolAutoTakeoffController()
: fsm(0), vtolPathFollowerSettings(0), mActive(false)
{}
// Called when mode first engaged
void VtolAutoTakeoffController::Activate(void)
{
if (!mActive) {
mActive = true;
SettingsUpdated();
fsm->Activate();
controlDown.Activate();
controlNE.Activate();
}
}
uint8_t VtolAutoTakeoffController::IsActive(void)
{
return mActive;
}
uint8_t VtolAutoTakeoffController::Mode(void)
{
return PATHDESIRED_MODE_AUTOTAKEOFF;
}
// Objective updated in pathdesired, e.g. same flight mode but new target velocity
void VtolAutoTakeoffController::ObjectiveUpdated(void)
{
// Set the objective's target velocity
controlDown.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_DOWN]);
controlNE.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_NORTH],
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_EAST]);
controlNE.UpdatePositionSetpoint(pathDesired->End.North, pathDesired->End.East);
controlDown.UpdatePositionSetpoint(pathDesired->End.Down);
fsm->setControlState((StatusVtolAutoTakeoffControlStateOptions)pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_CONTROLSTATE]);
}
void VtolAutoTakeoffController::Deactivate(void)
{
if (mActive) {
mActive = false;
fsm->Inactive();
controlDown.Deactivate();
controlNE.Deactivate();
}
}
// AutoTakeoff Uses different vertical velocity PID.
void VtolAutoTakeoffController::SettingsUpdated(void)
{
const float dT = vtolPathFollowerSettings->UpdatePeriod / 1000.0f;
controlNE.UpdateParameters(vtolPathFollowerSettings->HorizontalVelPID.Kp,
vtolPathFollowerSettings->HorizontalVelPID.Ki,
vtolPathFollowerSettings->HorizontalVelPID.Kd,
vtolPathFollowerSettings->HorizontalVelPID.Beta,
dT,
vtolPathFollowerSettings->HorizontalVelMax);
controlNE.UpdatePositionalParameters(vtolPathFollowerSettings->HorizontalPosP);
controlNE.UpdateCommandParameters(-vtolPathFollowerSettings->MaxRollPitch, vtolPathFollowerSettings->MaxRollPitch, vtolPathFollowerSettings->VelocityFeedforward);
controlDown.UpdateParameters(vtolPathFollowerSettings->AutoTakeoffVerticalVelPID.Kp,
vtolPathFollowerSettings->AutoTakeoffVerticalVelPID.Ki,
vtolPathFollowerSettings->AutoTakeoffVerticalVelPID.Kd,
vtolPathFollowerSettings->AutoTakeoffVerticalVelPID.Beta,
dT,
vtolPathFollowerSettings->VerticalVelMax);
controlDown.UpdatePositionalParameters(vtolPathFollowerSettings->VerticalPosP);
VtolSelfTuningStatsData vtolSelfTuningStats;
VtolSelfTuningStatsGet(&vtolSelfTuningStats);
controlDown.UpdateNeutralThrust(vtolSelfTuningStats.NeutralThrustOffset + vtolPathFollowerSettings->ThrustLimits.Neutral);
controlDown.SetThrustLimits(vtolPathFollowerSettings->ThrustLimits.Min, vtolPathFollowerSettings->ThrustLimits.Max);
fsm->SettingsUpdated();
}
// AutoTakeoff Uses a different FSM to its parent
int32_t VtolAutoTakeoffController::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollowerSettings)
{
PIOS_Assert(ptr_vtolPathFollowerSettings);
vtolPathFollowerSettings = ptr_vtolPathFollowerSettings;
if (fsm == 0) {
fsm = VtolAutoTakeoffFSM::instance();
VtolAutoTakeoffFSM::instance()->Initialize(vtolPathFollowerSettings, pathDesired, flightStatus);
controlDown.Initialize(fsm);
}
return 0;
}
void VtolAutoTakeoffController::UpdateVelocityDesired()
{
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
VelocityDesiredData velocityDesired;
PositionStateData positionState;
PositionStateGet(&positionState);
if (fsm->PositionHoldState()) {
controlDown.UpdatePositionState(positionState.Down);
controlDown.ControlPosition();
}
controlDown.UpdateVelocityState(velocityState.Down);
controlNE.UpdateVelocityState(velocityState.North, velocityState.East);
controlNE.UpdatePositionState(positionState.North, positionState.East);
controlNE.ControlPosition();
velocityDesired.Down = controlDown.GetVelocityDesired();
float north, east;
controlNE.GetVelocityDesired(&north, &east);
velocityDesired.North = north;
velocityDesired.East = east;
// update pathstatus
pathStatus->error = 0.0f;
pathStatus->fractional_progress = 0.0f;
if (fsm->PositionHoldState()) {
pathStatus->fractional_progress = 1.0f;
// note if the takeoff waypoint and the launch position are significantly different
// the above check might need to expand to assessment of north and east.
}
pathStatus->path_direction_north = velocityDesired.North;
pathStatus->path_direction_east = velocityDesired.East;
pathStatus->path_direction_down = velocityDesired.Down;
pathStatus->correction_direction_north = velocityDesired.North - velocityState.North;
pathStatus->correction_direction_east = velocityDesired.East - velocityState.East;
pathStatus->correction_direction_down = velocityDesired.Down - velocityState.Down;
VelocityDesiredSet(&velocityDesired);
}
int8_t VtolAutoTakeoffController::UpdateStabilizationDesired(bool yaw_attitude, float yaw_direction)
{
uint8_t result = 1;
StabilizationDesiredData stabDesired;
AttitudeStateData attitudeState;
StabilizationBankData stabSettings;
float northCommand;
float eastCommand;
StabilizationDesiredGet(&stabDesired);
AttitudeStateGet(&attitudeState);
StabilizationBankGet(&stabSettings);
controlNE.GetNECommand(&northCommand, &eastCommand);
stabDesired.Thrust = controlDown.GetDownCommand();
float angle_radians = DEG2RAD(attitudeState.Yaw);
float cos_angle = cosf(angle_radians);
float sine_angle = sinf(angle_radians);
float maxPitch = vtolPathFollowerSettings->MaxRollPitch;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.Pitch = boundf(-northCommand * cos_angle - eastCommand * sine_angle, -maxPitch, maxPitch);
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.Roll = boundf(-northCommand * sine_angle + eastCommand * cos_angle, -maxPitch, maxPitch);
ManualControlCommandData manualControl;
ManualControlCommandGet(&manualControl);
if (yaw_attitude) {
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.Yaw = yaw_direction;
} else {
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw;
}
// default thrust mode to cruise control
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL;
fsm->ConstrainStabiDesired(&stabDesired); // excludes thrust
StabilizationDesiredSet(&stabDesired);
return result;
}
void VtolAutoTakeoffController::UpdateAutoPilot()
{
fsm->Update();
UpdateVelocityDesired();
// yaw behaviour is configurable in vtolpathfollower, select yaw control algorithm
bool yaw_attitude = true;
float yaw = 0.0f;
fsm->GetYaw(yaw_attitude, yaw);
int8_t result = UpdateStabilizationDesired(yaw_attitude, yaw);
if (!result) {
fsm->Abort();
}
if (fsm->GetCurrentState() == PFFSM_STATE_DISARMED) {
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
}
PathStatusSet(pathStatus);
}
void VtolAutoTakeoffController::setArmedIfChanged(FlightStatusArmedOptions val)
{
if (flightStatus->Armed != val) {
flightStatus->Armed = val;
FlightStatusSet(flightStatus);
}
}

590
flight/modules/PathFollower/vtolautotakeofffsm.cpp Normal file
View File

@ -0,0 +1,590 @@
/*
******************************************************************************
*
* @file vtolautotakeofffsm.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief This autotakeoff state machine is a helper state machine to the
* VtolAutoTakeoffController.
* @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
*/
extern "C" {
#include <openpilot.h>
#include <callbackinfo.h>
#include <math.h>
#include <pid.h>
#include <pathdesired.h>
#include <paths.h>
#include "plans.h"
#include <sanitycheck.h>
#include <homelocation.h>
#include <accelstate.h>
#include <vtolpathfollowersettings.h>
#include <flightstatus.h>
#include <flightmodesettings.h>
#include <pathstatus.h>
#include <positionstate.h>
#include <velocitystate.h>
#include <velocitydesired.h>
#include <stabilizationdesired.h>
#include <attitudestate.h>
#include <takeofflocation.h>
#include <manualcontrolcommand.h>
#include <systemsettings.h>
#include <stabilizationbank.h>
#include <stabilizationdesired.h>
#include <vtolselftuningstats.h>
#include <statusvtolautotakeoff.h>
#include <pathsummary.h>
}
// C++ includes
#include <vtolautotakeofffsm.h>
// Private constants
#define TIMER_COUNT_PER_SECOND (1000 / vtolPathFollowerSettings->UpdatePeriod)
#define TIMEOUT_SLOWSTART (2 * TIMER_COUNT_PER_SECOND)
#define TIMEOUT_THRUSTUP (1 * TIMER_COUNT_PER_SECOND)
#define TIMEOUT_THRUSTDOWN (2 * TIMER_COUNT_PER_SECOND)
#define AUTOTAKEOFFING_SLOWDOWN_HEIGHT -5.0f
VtolAutoTakeoffFSM::PathFollowerFSM_AutoTakeoffStateHandler_T VtolAutoTakeoffFSM::sAutoTakeoffStateTable[AUTOTAKEOFF_STATE_SIZE] = {
[AUTOTAKEOFF_STATE_INACTIVE] = { .setup = &VtolAutoTakeoffFSM::setup_inactive, .run = 0 },
[AUTOTAKEOFF_STATE_CHECKSTATE] = { .setup = &VtolAutoTakeoffFSM::setup_checkstate, .run = 0 },
[AUTOTAKEOFF_STATE_SLOWSTART] = { .setup = &VtolAutoTakeoffFSM::setup_slowstart, .run = &VtolAutoTakeoffFSM::run_slowstart },
[AUTOTAKEOFF_STATE_THRUSTUP] = { .setup = &VtolAutoTakeoffFSM::setup_thrustup, .run = &VtolAutoTakeoffFSM::run_thrustup },
[AUTOTAKEOFF_STATE_TAKEOFF] = { .setup = &VtolAutoTakeoffFSM::setup_takeoff, .run = &VtolAutoTakeoffFSM::run_takeoff },
[AUTOTAKEOFF_STATE_HOLD] = { .setup = &VtolAutoTakeoffFSM::setup_hold, .run = &VtolAutoTakeoffFSM::run_hold },
[AUTOTAKEOFF_STATE_THRUSTDOWN] = { .setup = &VtolAutoTakeoffFSM::setup_thrustdown, .run = &VtolAutoTakeoffFSM::run_thrustdown },
[AUTOTAKEOFF_STATE_THRUSTOFF] = { .setup = &VtolAutoTakeoffFSM::setup_thrustoff, .run = &VtolAutoTakeoffFSM::run_thrustoff },
[AUTOTAKEOFF_STATE_DISARMED] = { .setup = &VtolAutoTakeoffFSM::setup_disarmed, .run = &VtolAutoTakeoffFSM::run_disarmed },
[AUTOTAKEOFF_STATE_ABORT] = { .setup = &VtolAutoTakeoffFSM::setup_abort, .run = &VtolAutoTakeoffFSM::run_abort }
};
// pointer to a singleton instance
VtolAutoTakeoffFSM *VtolAutoTakeoffFSM::p_inst = 0;
VtolAutoTakeoffFSM::VtolAutoTakeoffFSM()
: mAutoTakeoffData(0), vtolPathFollowerSettings(0), pathDesired(0), flightStatus(0)
{}
// Private types
// Private functions
// Public API methods
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t VtolAutoTakeoffFSM::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollowerSettings,
PathDesiredData *ptr_pathDesired,
FlightStatusData *ptr_flightStatus)
{
PIOS_Assert(ptr_vtolPathFollowerSettings);
PIOS_Assert(ptr_pathDesired);
PIOS_Assert(ptr_flightStatus);
if (mAutoTakeoffData == 0) {
mAutoTakeoffData = (VtolAutoTakeoffFSMData_T *)pios_malloc(sizeof(VtolAutoTakeoffFSMData_T));
PIOS_Assert(mAutoTakeoffData);
}
memset(mAutoTakeoffData, 0, sizeof(VtolAutoTakeoffFSMData_T));
vtolPathFollowerSettings = ptr_vtolPathFollowerSettings;
pathDesired = ptr_pathDesired;
flightStatus = ptr_flightStatus;
initFSM();
return 0;
}
void VtolAutoTakeoffFSM::Inactive(void)
{
memset(mAutoTakeoffData, 0, sizeof(VtolAutoTakeoffFSMData_T));
initFSM();
}
// Initialise the FSM
void VtolAutoTakeoffFSM::initFSM(void)
{
if (vtolPathFollowerSettings != 0) {
setState(STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
} else {
mAutoTakeoffData->currentState = STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE;
}
}
void VtolAutoTakeoffFSM::Activate()
{
memset(mAutoTakeoffData, 0, sizeof(VtolAutoTakeoffFSMData_T));
mAutoTakeoffData->currentState = STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE;
mAutoTakeoffData->flLowAltitude = true;
mAutoTakeoffData->flAltitudeHold = false;
mAutoTakeoffData->boundThrustMin = 0.0f;
mAutoTakeoffData->boundThrustMax = 0.0f;
mAutoTakeoffData->flZeroStabiHorizontal = true; // turn off positional controllers
TakeOffLocationGet(&(mAutoTakeoffData->takeOffLocation));
mAutoTakeoffData->fsmAutoTakeoffStatus.AltitudeAtState[STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE] = 0.0f;
mAutoTakeoffData->fsmAutoTakeoffStatus.ControlState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED;
assessAltitude();
// Check if we are already flying. This can happen in pathplanner mode
// going into a second loop of the waypoints.
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
if (stabDesired.Thrust > vtolPathFollowerSettings->ThrustLimits.Min) {
setState(STATUSVTOLAUTOTAKEOFF_STATE_HOLD, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
return;
}
// initially inactive and wait for a change in controlstate.
setState(STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
}
void VtolAutoTakeoffFSM::Abort(void)
{
setState(STATUSVTOLAUTOTAKEOFF_STATE_ABORT, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
}
PathFollowerFSMState_T VtolAutoTakeoffFSM::GetCurrentState(void)
{
switch (mAutoTakeoffData->currentState) {
case STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE:
return PFFSM_STATE_INACTIVE;
break;
case STATUSVTOLAUTOTAKEOFF_STATE_ABORT:
return PFFSM_STATE_ABORT;
break;
case STATUSVTOLAUTOTAKEOFF_STATE_DISARMED:
return PFFSM_STATE_DISARMED;
break;
default:
return PFFSM_STATE_ACTIVE;
break;
}
}
void VtolAutoTakeoffFSM::Update()
{
runState();
if (GetCurrentState() != PFFSM_STATE_INACTIVE) {
runAlways();
}
}
int32_t VtolAutoTakeoffFSM::runState(void)
{
uint8_t flTimeout = false;
mAutoTakeoffData->stateRunCount++;
if (mAutoTakeoffData->stateTimeoutCount > 0 && mAutoTakeoffData->stateRunCount > mAutoTakeoffData->stateTimeoutCount) {
flTimeout = true;
}
// If the current state has a static function, call it
if (sAutoTakeoffStateTable[mAutoTakeoffData->currentState].run) {
(this->*sAutoTakeoffStateTable[mAutoTakeoffData->currentState].run)(flTimeout);
}
return 0;
}
int32_t VtolAutoTakeoffFSM::runAlways(void)
{
void assessAltitude(void);
return 0;
}
// PathFollower implements the PID scheme and has a objective
// set by a PathDesired object. Based on the mode, pathfollower
// uses FSM's as helper functions that manage state and event detection.
// PathFollower calls into FSM methods to alter its commands.
void VtolAutoTakeoffFSM::BoundThrust(float &ulow, float &uhigh)
{
ulow = mAutoTakeoffData->boundThrustMin;
uhigh = mAutoTakeoffData->boundThrustMax;
if (mAutoTakeoffData->flConstrainThrust) {
uhigh = mAutoTakeoffData->thrustLimit;
}
}
void VtolAutoTakeoffFSM::ConstrainStabiDesired(StabilizationDesiredData *stabDesired)
{
if (mAutoTakeoffData->flZeroStabiHorizontal && stabDesired) {
stabDesired->Pitch = 0.0f;
stabDesired->Roll = 0.0f;
stabDesired->Yaw = 0.0f;
}
}
// Set the new state and perform setup for subsequent state run calls
// This is called by state run functions on event detection that drive
// state transitions.
void VtolAutoTakeoffFSM::setState(StatusVtolAutoTakeoffStateOptions newState, StatusVtolAutoTakeoffStateExitReasonOptions reason)
{
mAutoTakeoffData->fsmAutoTakeoffStatus.StateExitReason[mAutoTakeoffData->currentState] = reason;
if (mAutoTakeoffData->currentState == newState) {
return;
}
mAutoTakeoffData->currentState = newState;
if (newState != STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE) {
PositionStateData positionState;
PositionStateGet(&positionState);
float takeOffDown = 0.0f;
if (mAutoTakeoffData->takeOffLocation.Status == TAKEOFFLOCATION_STATUS_VALID) {
takeOffDown = mAutoTakeoffData->takeOffLocation.Down;
}
mAutoTakeoffData->fsmAutoTakeoffStatus.AltitudeAtState[newState] = positionState.Down - takeOffDown;
assessAltitude();
}
// Restart state timer counter
mAutoTakeoffData->stateRunCount = 0;
// Reset state timeout to disabled/zero
mAutoTakeoffData->stateTimeoutCount = 0;
if (sAutoTakeoffStateTable[mAutoTakeoffData->currentState].setup) {
(this->*sAutoTakeoffStateTable[mAutoTakeoffData->currentState].setup)();
}
updateVtolAutoTakeoffFSMStatus();
}
// Timeout utility function for use by state init implementations
void VtolAutoTakeoffFSM::setStateTimeout(int32_t count)
{
mAutoTakeoffData->stateTimeoutCount = count;
}
void VtolAutoTakeoffFSM::updateVtolAutoTakeoffFSMStatus()
{
mAutoTakeoffData->fsmAutoTakeoffStatus.State = mAutoTakeoffData->currentState;
if (mAutoTakeoffData->flLowAltitude) {
mAutoTakeoffData->fsmAutoTakeoffStatus.AltitudeState = STATUSVTOLAUTOTAKEOFF_ALTITUDESTATE_LOW;
} else {
mAutoTakeoffData->fsmAutoTakeoffStatus.AltitudeState = STATUSVTOLAUTOTAKEOFF_ALTITUDESTATE_HIGH;
}
StatusVtolAutoTakeoffSet(&mAutoTakeoffData->fsmAutoTakeoffStatus);
}
void VtolAutoTakeoffFSM::assessAltitude(void)
{
float positionDown;
PositionStateDownGet(&positionDown);
float takeOffDown = 0.0f;
if (mAutoTakeoffData->takeOffLocation.Status == TAKEOFFLOCATION_STATUS_VALID) {
takeOffDown = mAutoTakeoffData->takeOffLocation.Down;
}
float positionDownRelativeToTakeoff = positionDown - takeOffDown;
if (positionDownRelativeToTakeoff < AUTOTAKEOFFING_SLOWDOWN_HEIGHT) {
mAutoTakeoffData->flLowAltitude = false;
} else {
mAutoTakeoffData->flLowAltitude = true;
}
}
// Action the required state from plans.c
void VtolAutoTakeoffFSM::setControlState(StatusVtolAutoTakeoffControlStateOptions controlState)
{
mAutoTakeoffData->fsmAutoTakeoffStatus.ControlState = controlState;
switch (controlState) {
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED:
setState(STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
break;
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE:
setState(STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
break;
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_REQUIREUNARMEDFIRST:
setState(STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
break;
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_INITIATE:
setState(STATUSVTOLAUTOTAKEOFF_STATE_CHECKSTATE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
break;
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_POSITIONHOLD:
setState(STATUSVTOLAUTOTAKEOFF_STATE_HOLD, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
break;
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_ABORT:
setState(STATUSVTOLAUTOTAKEOFF_STATE_ABORT, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
break;
}
}
// State: INACTIVE
void VtolAutoTakeoffFSM::setup_inactive(void)
{
// Re-initialise local variables
mAutoTakeoffData->flZeroStabiHorizontal = false;
mAutoTakeoffData->flConstrainThrust = false;
}
// State: CHECKSTATE
void VtolAutoTakeoffFSM::setup_checkstate(void)
{
// Assumptions that do not need to be checked if flight mode is AUTOTAKEOFF
// 1. Already armed
// 2. Not in flight. This was checked in plans.c
// 3. User has placed throttle position to more than 50% to allow autotakeoff
// If pathplanner, we need additional checks
// E.g. if inflight, this mode is just positon hol
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
if (stabDesired.Thrust > vtolPathFollowerSettings->ThrustLimits.Min) {
setState(STATUSVTOLAUTOTAKEOFF_STATE_HOLD, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
return;
}
// Start from a enforced thrust off condition
mAutoTakeoffData->flConstrainThrust = false;
mAutoTakeoffData->boundThrustMin = -0.1f;
mAutoTakeoffData->boundThrustMax = 0.0f;
setState(STATUSVTOLAUTOTAKEOFF_STATE_SLOWSTART, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_TIMEOUT);
}
// STATE: SLOWSTART spools up motors to vtol min over 2 seconds for effect.
// PID loops may be cumulating I terms but that problem needs to be solved
#define SLOWSTART_INITIAL_THRUST 0.05f // assumed to be less than vtol min
void VtolAutoTakeoffFSM::setup_slowstart(void)
{
setStateTimeout(TIMEOUT_SLOWSTART);
mAutoTakeoffData->flZeroStabiHorizontal = true; // turn off positional controllers
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
float vtol_thrust_min = vtolPathFollowerSettings->ThrustLimits.Min;
if (vtol_thrust_min < SLOWSTART_INITIAL_THRUST) {
vtol_thrust_min = SLOWSTART_INITIAL_THRUST;
}
mAutoTakeoffData->sum1 = (vtol_thrust_min - SLOWSTART_INITIAL_THRUST) / (float)TIMEOUT_SLOWSTART;
mAutoTakeoffData->sum2 = vtol_thrust_min;
mAutoTakeoffData->boundThrustMin = SLOWSTART_INITIAL_THRUST;
mAutoTakeoffData->boundThrustMax = SLOWSTART_INITIAL_THRUST;
PositionStateData positionState;
PositionStateGet(&positionState);
mAutoTakeoffData->expectedAutoTakeoffPositionNorth = positionState.North;
mAutoTakeoffData->expectedAutoTakeoffPositionEast = positionState.East;
}
void VtolAutoTakeoffFSM::run_slowstart(__attribute__((unused)) uint8_t flTimeout)
{
// increase thrust setpoint step by step
if (mAutoTakeoffData->boundThrustMin < mAutoTakeoffData->sum2) {
mAutoTakeoffData->boundThrustMin += mAutoTakeoffData->sum1;
}
mAutoTakeoffData->boundThrustMax += mAutoTakeoffData->sum1;
if (mAutoTakeoffData->boundThrustMax > mAutoTakeoffData->sum2) {
mAutoTakeoffData->boundThrustMax = mAutoTakeoffData->sum2;
}
if (flTimeout) {
setState(STATUSVTOLAUTOTAKEOFF_STATE_THRUSTUP, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_TIMEOUT);
}
}
// STATE: THRUSTUP spools up motors to vtol min over 5 seconds for effect.
// PID loops may be cumulating I terms but that problem needs to be solved
#define THRUSTUP_FINAL_THRUST_AS_RATIO_OF_VTOLMAX 0.8f
void VtolAutoTakeoffFSM::setup_thrustup(void)
{
setStateTimeout(TIMEOUT_THRUSTUP);
mAutoTakeoffData->flZeroStabiHorizontal = false;
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
mAutoTakeoffData->sum2 = THRUSTUP_FINAL_THRUST_AS_RATIO_OF_VTOLMAX * vtolPathFollowerSettings->ThrustLimits.Max;
mAutoTakeoffData->sum1 = (mAutoTakeoffData->sum2 - mAutoTakeoffData->boundThrustMax) / (float)TIMEOUT_THRUSTUP;
mAutoTakeoffData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min;
}
void VtolAutoTakeoffFSM::run_thrustup(__attribute__((unused)) uint8_t flTimeout)
{
// increase thrust setpoint step by step
mAutoTakeoffData->boundThrustMax += mAutoTakeoffData->sum1;
if (mAutoTakeoffData->boundThrustMax > mAutoTakeoffData->sum2) {
mAutoTakeoffData->boundThrustMax = mAutoTakeoffData->sum2;
}
if (flTimeout) {
setState(STATUSVTOLAUTOTAKEOFF_STATE_TAKEOFF, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_TIMEOUT);
}
}
// STATE: TAKEOFF
void VtolAutoTakeoffFSM::setup_takeoff(void)
{
mAutoTakeoffData->flZeroStabiHorizontal = false;
}
void VtolAutoTakeoffFSM::run_takeoff(__attribute__((unused)) uint8_t flTimeout)
{
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
if (stabDesired.Thrust < 0.0f) {
setState(STATUSVTOLAUTOTAKEOFF_STATE_THRUSTOFF, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_ZEROTHRUST);
return;
}
// detect broad sideways drift.
PositionStateData positionState;
PositionStateGet(&positionState);
float north_error = mAutoTakeoffData->expectedAutoTakeoffPositionNorth - positionState.North;
float east_error = mAutoTakeoffData->expectedAutoTakeoffPositionEast - positionState.East;
float down_error = pathDesired->End.Down - positionState.Down;
float positionError = sqrtf(north_error * north_error + east_error * east_error);
if (positionError > 3.0f) {
setState(STATUSVTOLAUTOTAKEOFF_STATE_THRUSTDOWN, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_POSITIONERROR);
return;
}
if (fabsf(down_error) < 0.5f) {
setState(STATUSVTOLAUTOTAKEOFF_STATE_HOLD, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_ARRIVEDATALT);
return;
}
}
// STATE: HOLD
void VtolAutoTakeoffFSM::setup_hold(void)
{
mAutoTakeoffData->flZeroStabiHorizontal = false;
mAutoTakeoffData->flAltitudeHold = true;
}
void VtolAutoTakeoffFSM::run_hold(__attribute__((unused)) uint8_t flTimeout)
{}
uint8_t VtolAutoTakeoffFSM::PositionHoldState(void)
{
return mAutoTakeoffData->flAltitudeHold;
}
// STATE: THRUSTDOWN
void VtolAutoTakeoffFSM::setup_thrustdown(void)
{
setStateTimeout(TIMEOUT_THRUSTDOWN);
mAutoTakeoffData->flZeroStabiHorizontal = true;
mAutoTakeoffData->flConstrainThrust = true;
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
mAutoTakeoffData->thrustLimit = stabDesired.Thrust;
mAutoTakeoffData->sum1 = stabDesired.Thrust / (float)TIMEOUT_THRUSTDOWN;
mAutoTakeoffData->boundThrustMin = -0.1f;
mAutoTakeoffData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Neutral;
}
void VtolAutoTakeoffFSM::run_thrustdown(__attribute__((unused)) uint8_t flTimeout)
{
// reduce thrust setpoint step by step
mAutoTakeoffData->thrustLimit -= mAutoTakeoffData->sum1;
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
if (stabDesired.Thrust < 0.0f || mAutoTakeoffData->thrustLimit < 0.0f) {
setState(STATUSVTOLAUTOTAKEOFF_STATE_THRUSTOFF, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_ZEROTHRUST);
}
if (flTimeout) {
setState(STATUSVTOLAUTOTAKEOFF_STATE_THRUSTOFF, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_TIMEOUT);
}
}
// STATE: THRUSTOFF
void VtolAutoTakeoffFSM::setup_thrustoff(void)
{
mAutoTakeoffData->thrustLimit = -1.0f;
mAutoTakeoffData->flConstrainThrust = true;
mAutoTakeoffData->boundThrustMin = -0.1f;
mAutoTakeoffData->boundThrustMax = 0.0f;
}
void VtolAutoTakeoffFSM::run_thrustoff(__attribute__((unused)) uint8_t flTimeout)
{
setState(STATUSVTOLAUTOTAKEOFF_STATE_DISARMED, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
}
// STATE: DISARMED
void VtolAutoTakeoffFSM::setup_disarmed(void)
{
// nothing to do
mAutoTakeoffData->flConstrainThrust = false;
mAutoTakeoffData->flZeroStabiHorizontal = false;
mAutoTakeoffData->observationCount = 0;
mAutoTakeoffData->boundThrustMin = -0.1f;
mAutoTakeoffData->boundThrustMax = 0.0f;
}
void VtolAutoTakeoffFSM::run_disarmed(__attribute__((unused)) uint8_t flTimeout)
{
#ifdef DEBUG_GROUNDIMPACT
if (mAutoTakeoffData->observationCount++ > 100) {
setState(AUTOTAKEOFF_STATE_WTG_FOR_GROUNDEFFECT, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
}
#endif
}
void VtolAutoTakeoffFSM::fallback_to_hold(void)
{
PositionStateData positionState;
PositionStateGet(&positionState);
pathDesired->End.North = positionState.North;
pathDesired->End.East = positionState.East;
pathDesired->End.Down = positionState.Down;
pathDesired->Start.North = positionState.North;
pathDesired->Start.East = positionState.East;
pathDesired->Start.Down = positionState.Down;
pathDesired->StartingVelocity = 0.0f;
pathDesired->EndingVelocity = 0.0f;
pathDesired->Mode = PATHDESIRED_MODE_GOTOENDPOINT;
PathDesiredSet(pathDesired);
}
// abort repeatedly overwrites pathfollower's objective on a landing abort and
// continues to do so until a flight mode change.
void VtolAutoTakeoffFSM::setup_abort(void)
{
mAutoTakeoffData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min;
mAutoTakeoffData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max;
mAutoTakeoffData->flConstrainThrust = false;
mAutoTakeoffData->flZeroStabiHorizontal = false;
fallback_to_hold();
}
void VtolAutoTakeoffFSM::run_abort(__attribute__((unused)) uint8_t flTimeout)
{}

372
flight/modules/PathFollower/vtolbrakecontroller.cpp Normal file
View File

@ -0,0 +1,372 @@
/*
******************************************************************************
*
* @file vtolbrakecontroller.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief This landing state machine is a helper state machine to the
* pathfollower task/thread to implement detailed landing controls.
* This is to be called only from the pathfollower task.
* Note that initiation of the land occurs in the manual control
* command thread calling plans.c plan_setup_land which writes
* the required PathDesired LAND 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
*/
extern "C" {
#include <openpilot.h>
#include <callbackinfo.h>
#include <math.h>
#include <pid.h>
#include <CoordinateConversions.h>
#include <sin_lookup.h>
#include <pathdesired.h>
#include <paths.h>
#include "plans.h"
#include <sanitycheck.h>
#include <accelstate.h>
#include <vtolpathfollowersettings.h>
#include <flightstatus.h>
#include <flightmodesettings.h>
#include <pathstatus.h>
#include <positionstate.h>
#include <velocitystate.h>
#include <velocitydesired.h>
#include <stabilizationdesired.h>
#include <attitudestate.h>
#include <takeofflocation.h>
#include <manualcontrolcommand.h>
#include <systemsettings.h>
#include <stabilizationbank.h>
#include <stabilizationdesired.h>
#include <vtolselftuningstats.h>
#include <pathsummary.h>
}
// C++ includes
#include "vtolbrakecontroller.h"
#include "pathfollowerfsm.h"
#include "vtolbrakefsm.h"
#include "pidcontroldown.h"
// Private constants
#define BRAKE_RATE_MINIMUM 0.2f
// pointer to a singleton instance
VtolBrakeController *VtolBrakeController::p_inst = 0;
VtolBrakeController::VtolBrakeController()
: fsm(0), vtolPathFollowerSettings(0), mActive(false), mHoldActive(false), mManualThrust(false)
{}
// Called when mode first engaged
void VtolBrakeController::Activate(void)
{
if (!mActive) {
mActive = true;
mHoldActive = false;
mManualThrust = false;
SettingsUpdated();
fsm->Activate();
controlDown.Activate();
controlNE.Activate();
}
}
uint8_t VtolBrakeController::IsActive(void)
{
return mActive;
}
uint8_t VtolBrakeController::Mode(void)
{
return PATHDESIRED_MODE_BRAKE;
}
// Objective updated in pathdesired
void VtolBrakeController::ObjectiveUpdated(void)
{
// Set the objective's target velocity
controlDown.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_DOWN]);
controlNE.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_NORTH],
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_EAST]);
if (pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT] > 0.0f) {
pathStatus->path_time = 0.0f;
}
}
void VtolBrakeController::Deactivate(void)
{
if (mActive) {
mActive = false;
mHoldActive = false;
mManualThrust = false;
fsm->Inactive();
controlDown.Deactivate();
controlNE.Deactivate();
}
}
void VtolBrakeController::SettingsUpdated(void)
{
const float dT = vtolPathFollowerSettings->UpdatePeriod / 1000.0f;
controlNE.UpdateParameters(vtolPathFollowerSettings->BrakeHorizontalVelPID.Kp,
vtolPathFollowerSettings->BrakeHorizontalVelPID.Ki,
vtolPathFollowerSettings->BrakeHorizontalVelPID.Kd,
vtolPathFollowerSettings->BrakeHorizontalVelPID.Beta,
dT,
10.0f * vtolPathFollowerSettings->HorizontalVelMax); // avoid constraining initial fast entry into brake
controlNE.UpdatePositionalParameters(vtolPathFollowerSettings->HorizontalPosP);
controlNE.UpdateCommandParameters(-vtolPathFollowerSettings->BrakeMaxPitch, vtolPathFollowerSettings->BrakeMaxPitch, vtolPathFollowerSettings->BrakeVelocityFeedforward);
controlDown.UpdateParameters(vtolPathFollowerSettings->LandVerticalVelPID.Kp,
vtolPathFollowerSettings->LandVerticalVelPID.Ki,
vtolPathFollowerSettings->LandVerticalVelPID.Kd,
vtolPathFollowerSettings->LandVerticalVelPID.Beta,
dT,
10.0f * vtolPathFollowerSettings->VerticalVelMax); // avoid constraining initial fast entry into brake
controlDown.UpdatePositionalParameters(vtolPathFollowerSettings->VerticalPosP);
controlDown.SetThrustLimits(vtolPathFollowerSettings->ThrustLimits.Min, vtolPathFollowerSettings->ThrustLimits.Max);
VtolSelfTuningStatsData vtolSelfTuningStats;
VtolSelfTuningStatsGet(&vtolSelfTuningStats);
controlDown.UpdateNeutralThrust(vtolSelfTuningStats.NeutralThrustOffset + vtolPathFollowerSettings->ThrustLimits.Neutral);
fsm->SettingsUpdated();
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t VtolBrakeController::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollowerSettings)
{
PIOS_Assert(ptr_vtolPathFollowerSettings);
vtolPathFollowerSettings = ptr_vtolPathFollowerSettings;
if (fsm == 0) {
VtolBrakeFSM::instance()->Initialize(vtolPathFollowerSettings, pathDesired, flightStatus, pathStatus);
fsm = (PathFollowerFSM *)VtolBrakeFSM::instance();
controlDown.Initialize(fsm);
}
return 0;
}
void VtolBrakeController::UpdateVelocityDesired()
{
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
VelocityDesiredData velocityDesired;
float brakeRate = vtolPathFollowerSettings->BrakeRate;
if (brakeRate < BRAKE_RATE_MINIMUM) {
brakeRate = BRAKE_RATE_MINIMUM; // set a minimum to avoid a divide by zero potential below
}
if (fsm->PositionHoldState()) {
PositionStateData positionState;
PositionStateGet(&positionState);
// On first engagement set the position setpoint
if (!mHoldActive) {
mHoldActive = true;
controlNE.UpdatePositionSetpoint(positionState.North, positionState.East);
if (!mManualThrust) {
controlDown.UpdatePositionSetpoint(positionState.Down);
}
FlightStatusFlightModeAssistOptions flightModeAssist;
FlightStatusFlightModeAssistGet(&flightModeAssist);
if (flightModeAssist != FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) {
// Notify manualcommand via setting hold state in flightstatus assistedcontrolstate
FlightStatusAssistedControlStateOptions assistedControlFlightMode;
FlightStatusAssistedControlStateGet(&assistedControlFlightMode);
// sanity check that we are in brake state according to flight status, which means
// we are being used for gpsassist
if (assistedControlFlightMode == FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE) {
assistedControlFlightMode = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD;
FlightStatusAssistedControlStateSet(&assistedControlFlightMode);
}
}
}
// Update position state and control position to create inputs to velocity control
controlNE.UpdatePositionState(positionState.North, positionState.East);
controlNE.ControlPosition();
if (!mManualThrust) {
controlDown.UpdatePositionState(positionState.Down);
controlDown.ControlPosition();
}
controlNE.UpdateVelocityState(velocityState.North, velocityState.East);
if (!mManualThrust) {
controlDown.UpdateVelocityState(velocityState.Down);
}
} else {
controlNE.UpdateVelocityStateWithBrake(velocityState.North, velocityState.East, pathStatus->path_time, brakeRate);
if (!mManualThrust) {
controlDown.UpdateVelocityStateWithBrake(velocityState.Down, pathStatus->path_time, brakeRate);
}
}
if (!mManualThrust) {
velocityDesired.Down = controlDown.GetVelocityDesired();
} else { velocityDesired.Down = 0.0f; }
float north, east;
controlNE.GetVelocityDesired(&north, &east);
velocityDesired.North = north;
velocityDesired.East = east;
VelocityDesiredSet(&velocityDesired);
// update pathstatus
float cur_velocity = velocityState.North * velocityState.North + velocityState.East * velocityState.East + velocityState.Down * velocityState.Down;
cur_velocity = sqrtf(cur_velocity);
float desired_velocity = velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East + velocityDesired.Down * velocityDesired.Down;
desired_velocity = sqrtf(desired_velocity);
pathStatus->error = cur_velocity - desired_velocity;
pathStatus->fractional_progress = 1.0f;
if (pathDesired->StartingVelocity > 0.0f) {
pathStatus->fractional_progress = (pathDesired->StartingVelocity - cur_velocity) / pathDesired->StartingVelocity;
// sometimes current velocity can exceed starting velocity due to initial acceleration
if (pathStatus->fractional_progress < 0.0f) {
pathStatus->fractional_progress = 0.0f;
}
}
pathStatus->path_direction_north = velocityDesired.North;
pathStatus->path_direction_east = velocityDesired.East;
pathStatus->path_direction_down = velocityDesired.Down;
pathStatus->correction_direction_north = velocityDesired.North - velocityState.North;
pathStatus->correction_direction_east = velocityDesired.East - velocityState.East;
}
int8_t VtolBrakeController::UpdateStabilizationDesired(void)
{
uint8_t result = 1;
StabilizationDesiredData stabDesired;
AttitudeStateData attitudeState;
StabilizationBankData stabSettings;
float northCommand;
float eastCommand;
StabilizationDesiredGet(&stabDesired);
AttitudeStateGet(&attitudeState);
StabilizationBankGet(&stabSettings);
controlNE.GetNECommand(&northCommand, &eastCommand);
float angle_radians = DEG2RAD(attitudeState.Yaw);
float cos_angle = cosf(angle_radians);
float sine_angle = sinf(angle_radians);
float maxPitch = vtolPathFollowerSettings->BrakeMaxPitch;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.Pitch = boundf(-northCommand * cos_angle - eastCommand * sine_angle, -maxPitch, maxPitch); // this should be in the controller
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.Roll = boundf(-northCommand * sine_angle + eastCommand * cos_angle, -maxPitch, maxPitch);
ManualControlCommandData manualControl;
ManualControlCommandGet(&manualControl);
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw;
// default thrust mode to cruise control
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL;
// when flight mode assist is active but in primary-thrust mode, the thrust mode must be set to the same as per the primary mode.
if (flightStatus->FlightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST_PRIMARYTHRUST) {
FlightModeSettingsData settings;
FlightModeSettingsGet(&settings);
uint8_t thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL;
switch (flightStatus->FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
thrustMode = settings.Stabilization1Settings.Thrust;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
thrustMode = settings.Stabilization2Settings.Thrust;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
thrustMode = settings.Stabilization3Settings.Thrust;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4:
thrustMode = settings.Stabilization4Settings.Thrust;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5:
thrustMode = settings.Stabilization5Settings.Thrust;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
thrustMode = settings.Stabilization6Settings.Thrust;
break;
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
// we hard code the "GPS Assisted" PostionHold/Roam to use alt-vario which
// is a more appropriate throttle mode. "GPSAssist" adds braking
// and a better throttle management to the standard Position Hold.
thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO;
break;
default:
break;
}
stabDesired.StabilizationMode.Thrust = (StabilizationDesiredStabilizationModeOptions)thrustMode;
}
// set the thrust value
if (mManualThrust) {
stabDesired.Thrust = manualControl.Thrust;
} else {
stabDesired.Thrust = controlDown.GetDownCommand();
}
StabilizationDesiredSet(&stabDesired);
return result;
}
void VtolBrakeController::UpdateAutoPilot()
{
if (pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT] > 0.0f) {
pathStatus->path_time += vtolPathFollowerSettings->UpdatePeriod / 1000.0f;
}
if (flightStatus->FlightModeAssist && flightStatus->AssistedThrottleState == FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL) {
mManualThrust = true;
} else {
mManualThrust = false;
}
fsm->Update(); // This will run above end condition checks
UpdateVelocityDesired();
int8_t result = UpdateStabilizationDesired();
if (!result) {
fsm->Abort(); // enter PH
}
PathStatusSet(pathStatus);
}

261
flight/modules/PathFollower/vtolbrakefsm.cpp Normal file
View File

@ -0,0 +1,261 @@
/*
******************************************************************************
*
* @file vtolbrakefsm.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Vtol brake finate state machine to regulate behaviour of the
* brake controller.
* @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
*/
extern "C" {
#include <openpilot.h>
#include <callbackinfo.h>
#include <math.h>
#include <pid.h>
#include <CoordinateConversions.h>
#include <sin_lookup.h>
#include <pathdesired.h>
#include <paths.h>
#include "plans.h"
#include <sanitycheck.h>
#include <vtolpathfollowersettings.h>
#include <flightstatus.h>
#include <flightmodesettings.h>
#include <pathstatus.h>
#include <positionstate.h>
#include <velocitystate.h>
#include <velocitydesired.h>
#include <stabilizationdesired.h>
#include <attitudestate.h>
#include <manualcontrolcommand.h>
#include <systemsettings.h>
#include <stabilizationbank.h>
#include <stabilizationdesired.h>
#include <vtolselftuningstats.h>
#include <pathsummary.h>
}
// C++ includes
#include <vtolbrakefsm.h>
// Private constants
#define TIMER_COUNT_PER_SECOND (1000 / vtolPathFollowerSettings->UpdatePeriod)
#define BRAKE_FRACTIONALPROGRESS_STARTVELOCITYCHECK 0.95f
#define BRAKE_EXIT_VELOCITY_LIMIT 0.2f
VtolBrakeFSM::PathFollowerFSM_BrakeStateHandler_T VtolBrakeFSM::sBrakeStateTable[BRAKE_STATE_SIZE] = {
[BRAKE_STATE_INACTIVE] = { .setup = 0, .run = 0 },
[BRAKE_STATE_BRAKE] = { .setup = &VtolBrakeFSM::setup_brake, .run = &VtolBrakeFSM::run_brake },
[BRAKE_STATE_HOLD] = { .setup = 0, .run = 0 }
};
// pointer to a singleton instance
VtolBrakeFSM *VtolBrakeFSM::p_inst = 0;
VtolBrakeFSM::VtolBrakeFSM()
: mBrakeData(0), vtolPathFollowerSettings(0), pathDesired(0), flightStatus(0)
{}
// Private types
// Private functions
// Public API methods
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t VtolBrakeFSM::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollowerSettings,
PathDesiredData *ptr_pathDesired,
FlightStatusData *ptr_flightStatus,
PathStatusData *ptr_pathStatus)
{
PIOS_Assert(ptr_vtolPathFollowerSettings);
PIOS_Assert(ptr_pathDesired);
PIOS_Assert(ptr_flightStatus);
// allow for Initialize being called more than once.
if (!mBrakeData) {
mBrakeData = (VtolBrakeFSMData_T *)pios_malloc(sizeof(VtolBrakeFSMData_T));
PIOS_Assert(mBrakeData);
}
memset(mBrakeData, 0, sizeof(VtolBrakeFSMData_T));
vtolPathFollowerSettings = ptr_vtolPathFollowerSettings;
pathDesired = ptr_pathDesired;
flightStatus = ptr_flightStatus;
pathStatus = ptr_pathStatus;
initFSM();
return 0;
}
void VtolBrakeFSM::Inactive(void)
{
memset(mBrakeData, 0, sizeof(VtolBrakeFSMData_T));
initFSM();
}
// Initialise the FSM
void VtolBrakeFSM::initFSM(void)
{
mBrakeData->currentState = BRAKE_STATE_INACTIVE;
}
void VtolBrakeFSM::Activate()
{
memset(mBrakeData, 0, sizeof(VtolBrakeFSMData_T));
mBrakeData->currentState = BRAKE_STATE_INACTIVE;
setState(BRAKE_STATE_BRAKE, FSMBRAKESTATUS_STATEEXITREASON_NONE);
}
void VtolBrakeFSM::Abort(void)
{
setState(BRAKE_STATE_HOLD, FSMBRAKESTATUS_STATEEXITREASON_NONE);
}
PathFollowerFSMState_T VtolBrakeFSM::GetCurrentState(void)
{
switch (mBrakeData->currentState) {
case BRAKE_STATE_INACTIVE:
return PFFSM_STATE_INACTIVE;
break;
default:
return PFFSM_STATE_ACTIVE;
break;
}
}
void VtolBrakeFSM::Update()
{
runState();
}
int32_t VtolBrakeFSM::runState(void)
{
uint8_t flTimeout = false;
mBrakeData->stateRunCount++;
if (mBrakeData->stateTimeoutCount > 0 && mBrakeData->stateRunCount > mBrakeData->stateTimeoutCount) {
flTimeout = true;
}
// If the current state has a static function, call it
if (sBrakeStateTable[mBrakeData->currentState].run) {
(this->*sBrakeStateTable[mBrakeData->currentState].run)(flTimeout);
}
return 0;
}
// Set the new state and perform setup for subsequent state run calls
// This is called by state run functions on event detection that drive
// state transitions.
void VtolBrakeFSM::setState(PathFollowerFSM_BrakeState_T newState, __attribute__((unused)) VtolBrakeFSMStatusStateExitReasonOptions reason)
{
// mBrakeData->fsmBrakeStatus.StateExitReason[mBrakeData->currentState] = reason;
if (mBrakeData->currentState == newState) {
return;
}
mBrakeData->currentState = newState;
// Restart state timer counter
mBrakeData->stateRunCount = 0;
// Reset state timeout to disabled/zero
mBrakeData->stateTimeoutCount = 0;
if (sBrakeStateTable[mBrakeData->currentState].setup) {
(this->*sBrakeStateTable[mBrakeData->currentState].setup)();
}
}
// Timeout utility function for use by state init implementations
void VtolBrakeFSM::setStateTimeout(int32_t count)
{
mBrakeData->stateTimeoutCount = count;
}
// FSM Setup and Run method implementation
// State: WAITING FOR DESCENT RATE
void VtolBrakeFSM::setup_brake(void)
{
setStateTimeout(TIMER_COUNT_PER_SECOND * pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT]);
mBrakeData->observationCount = 0;
mBrakeData->observation2Count = 0;
}
void VtolBrakeFSM::run_brake(uint8_t flTimeout)
{
// Brake mode end condition checks
bool exit_brake = false;
VelocityStateData velocityState;
PathSummaryData pathSummary;
if (flTimeout) {
pathSummary.brake_exit_reason = PATHSUMMARY_BRAKE_EXIT_REASON_TIMEOUT;
exit_brake = true;
} else if (pathStatus->fractional_progress > BRAKE_FRACTIONALPROGRESS_STARTVELOCITYCHECK) {
VelocityStateGet(&velocityState);
if (fabsf(velocityState.East) < BRAKE_EXIT_VELOCITY_LIMIT && fabsf(velocityState.North) < BRAKE_EXIT_VELOCITY_LIMIT) {
pathSummary.brake_exit_reason = PATHSUMMARY_BRAKE_EXIT_REASON_PATHCOMPLETED;
exit_brake = true;
}
}
if (exit_brake) {
// Calculate the distance error between the originally desired
// stopping point and the actual brake-exit point.
PositionStateData p;
PositionStateGet(&p);
float north_offset = pathDesired->End.North - p.North;
float east_offset = pathDesired->End.East - p.East;
float down_offset = pathDesired->End.Down - p.Down;
pathSummary.brake_distance_offset = sqrtf(north_offset * north_offset + east_offset * east_offset + down_offset * down_offset);
pathSummary.time_remaining = pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT] - pathStatus->path_time;
pathSummary.fractional_progress = pathStatus->fractional_progress;
float cur_velocity = velocityState.North * velocityState.North + velocityState.East * velocityState.East + velocityState.Down * velocityState.Down;
cur_velocity = sqrtf(cur_velocity);
pathSummary.decelrate = (pathDesired->StartingVelocity - cur_velocity) / pathStatus->path_time;
pathSummary.brakeRateActualDesiredRatio = pathSummary.decelrate / vtolPathFollowerSettings->BrakeRate;
pathSummary.velocityIntoHold = cur_velocity;
pathSummary.Mode = PATHSUMMARY_MODE_BRAKE;
pathSummary.UID = pathStatus->UID;
PathSummarySet(&pathSummary);
setState(BRAKE_STATE_HOLD, FSMBRAKESTATUS_STATEEXITREASON_NONE);
}
}
uint8_t VtolBrakeFSM::PositionHoldState(void)
{
return mBrakeData->currentState == BRAKE_STATE_HOLD;
}

550
flight/modules/PathFollower/vtolflycontroller.cpp Normal file
View File

@ -0,0 +1,550 @@
/*
******************************************************************************
*
* @file vtolflycontroller.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Class implements the fly controller for vtols
* @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
*/
extern "C" {
#include <openpilot.h>
#include <callbackinfo.h>
#include <math.h>
#include <pid.h>
#include <CoordinateConversions.h>
#include <sin_lookup.h>
#include <pathdesired.h>
#include <paths.h>
#include "plans.h"
#include <sanitycheck.h>
#include <accelstate.h>
#include <vtolpathfollowersettings.h>
#include <flightstatus.h>
#include <flightmodesettings.h>
#include <pathstatus.h>
#include <positionstate.h>
#include <velocitystate.h>
#include <velocitydesired.h>
#include <stabilizationdesired.h>
#include <attitudestate.h>
#include <takeofflocation.h>
#include <poilocation.h>
#include <manualcontrolcommand.h>
#include <systemsettings.h>
#include <stabilizationbank.h>
#include <stabilizationdesired.h>
#include <vtolselftuningstats.h>
#include <pathsummary.h>
}
// C++ includes
#include "vtolflycontroller.h"
#include "pathfollowerfsm.h"
#include "pidcontroldown.h"
#include "pidcontrolne.h"
// Private constants
#define DEADBAND_HIGH 0.10f
#define DEADBAND_LOW -0.10f
#define RTB_LAND_FRACTIONAL_PROGRESS_START_CHECKS 0.95f
#define RTB_LAND_NE_DISTANCE_REQUIRED_TO_START_LAND_SEQUENCE 2.0f
// pointer to a singleton instance
VtolFlyController *VtolFlyController::p_inst = 0;
VtolFlyController::VtolFlyController()
: vtolPathFollowerSettings(NULL), mActive(false), mManualThrust(false), mMode(0), vtolEmergencyFallback(0.0f), vtolEmergencyFallbackSwitch(false)
{}
// Called when mode first engaged
void VtolFlyController::Activate(void)
{
if (!mActive) {
mActive = true;
mManualThrust = false;
SettingsUpdated();
controlDown.Activate();
controlNE.Activate();
mMode = pathDesired->Mode;
vtolEmergencyFallback = 0.0f;
vtolEmergencyFallbackSwitch = false;
}
}
uint8_t VtolFlyController::IsActive(void)
{
return mActive;
}
uint8_t VtolFlyController::Mode(void)
{
return mMode;
}
// Objective updated in pathdesired
void VtolFlyController::ObjectiveUpdated(void)
{}
void VtolFlyController::Deactivate(void)
{
if (mActive) {
mActive = false;
mManualThrust = false;
controlDown.Deactivate();
controlNE.Deactivate();
vtolEmergencyFallback = 0.0f;
vtolEmergencyFallbackSwitch = false;
}
}
void VtolFlyController::SettingsUpdated(void)
{
const float dT = vtolPathFollowerSettings->UpdatePeriod / 1000.0f;
controlNE.UpdateParameters(vtolPathFollowerSettings->HorizontalVelPID.Kp,
vtolPathFollowerSettings->HorizontalVelPID.Ki,
vtolPathFollowerSettings->HorizontalVelPID.Kd,
vtolPathFollowerSettings->HorizontalVelPID.Beta,
dT,
vtolPathFollowerSettings->HorizontalVelMax);
controlNE.UpdatePositionalParameters(vtolPathFollowerSettings->HorizontalPosP);
controlNE.UpdateCommandParameters(-vtolPathFollowerSettings->MaxRollPitch, vtolPathFollowerSettings->MaxRollPitch, vtolPathFollowerSettings->VelocityFeedforward);
controlDown.UpdateParameters(vtolPathFollowerSettings->VerticalVelPID.Kp,
vtolPathFollowerSettings->VerticalVelPID.Ki,
vtolPathFollowerSettings->VerticalVelPID.Kd,
vtolPathFollowerSettings->VerticalVelPID.Beta,
dT,
vtolPathFollowerSettings->VerticalVelMax);
controlDown.UpdatePositionalParameters(vtolPathFollowerSettings->VerticalPosP);
VtolSelfTuningStatsData vtolSelfTuningStats;
VtolSelfTuningStatsGet(&vtolSelfTuningStats);
controlDown.UpdateNeutralThrust(vtolSelfTuningStats.NeutralThrustOffset + vtolPathFollowerSettings->ThrustLimits.Neutral);
controlDown.SetThrustLimits(vtolPathFollowerSettings->ThrustLimits.Min, vtolPathFollowerSettings->ThrustLimits.Max);
// disable neutral thrust calcs which should only be done in a hold mode.
controlDown.DisableNeutralThrustCalc();
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t VtolFlyController::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollowerSettings)
{
PIOS_Assert(ptr_vtolPathFollowerSettings);
vtolPathFollowerSettings = ptr_vtolPathFollowerSettings;
return 0;
}
/**
* Compute desired velocity from the current position and path
*/
void VtolFlyController::UpdateVelocityDesired()
{
PositionStateData positionState;
PositionStateGet(&positionState);
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
controlNE.UpdateVelocityState(velocityState.North, velocityState.East);
controlDown.UpdateVelocityState(velocityState.Down);
VelocityDesiredData velocityDesired;
// look ahead kFF seconds
float cur[3] = { positionState.North + (velocityState.North * vtolPathFollowerSettings->CourseFeedForward),
positionState.East + (velocityState.East * vtolPathFollowerSettings->CourseFeedForward),
positionState.Down + (velocityState.Down * vtolPathFollowerSettings->CourseFeedForward) };
struct path_status progress;
path_progress(pathDesired, cur, &progress, true);
controlNE.ControlPositionWithPath(&progress);
if (!mManualThrust) {
controlDown.ControlPositionWithPath(&progress);
}
float north, east;
controlNE.GetVelocityDesired(&north, &east);
velocityDesired.North = north;
velocityDesired.East = east;
if (!mManualThrust) {
velocityDesired.Down = controlDown.GetVelocityDesired();
} else { velocityDesired.Down = 0.0f; }
// update pathstatus
pathStatus->error = progress.error;
pathStatus->fractional_progress = progress.fractional_progress;
pathStatus->path_direction_north = progress.path_vector[0];
pathStatus->path_direction_east = progress.path_vector[1];
pathStatus->path_direction_down = progress.path_vector[2];
pathStatus->correction_direction_north = progress.correction_vector[0];
pathStatus->correction_direction_east = progress.correction_vector[1];
pathStatus->correction_direction_down = progress.correction_vector[2];
VelocityDesiredSet(&velocityDesired);
}
int8_t VtolFlyController::UpdateStabilizationDesired(bool yaw_attitude, float yaw_direction)
{
uint8_t result = 1;
StabilizationDesiredData stabDesired;
AttitudeStateData attitudeState;
StabilizationBankData stabSettings;
float northCommand;
float eastCommand;
StabilizationDesiredGet(&stabDesired);
AttitudeStateGet(&attitudeState);
StabilizationBankGet(&stabSettings);
controlNE.GetNECommand(&northCommand, &eastCommand);
float angle_radians = DEG2RAD(attitudeState.Yaw);
float cos_angle = cosf(angle_radians);
float sine_angle = sinf(angle_radians);
float maxPitch = vtolPathFollowerSettings->MaxRollPitch;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.Pitch = boundf(-northCommand * cos_angle - eastCommand * sine_angle, -maxPitch, maxPitch); // this should be in the controller
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.Roll = boundf(-northCommand * sine_angle + eastCommand * cos_angle, -maxPitch, maxPitch);
ManualControlCommandData manualControl;
ManualControlCommandGet(&manualControl);
// TODO The below need to be rewritten because the PID implementation has changed.
#if 0
// DEBUG HACK: allow user to skew compass on purpose to see if emergency failsafe kicks in
if (vtolPathFollowerSettings->FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_DEBUGTEST) {
attitudeState.Yaw += 120.0f;
if (attitudeState.Yaw > 180.0f) {
attitudeState.Yaw -= 360.0f;
}
}
if ( // emergency flyaway detection
( // integral already at its limit
vtolPathFollowerSettings.HorizontalVelPID.ILimit - fabsf(global.PIDvel[0].iAccumulator) < 1e-6f ||
vtolPathFollowerSettings.HorizontalVelPID.ILimit - fabsf(global.PIDvel[1].iAccumulator) < 1e-6f
) &&
// angle between desired and actual velocity >90 degrees (by dot product)
(velocityDesired.North * velocityState.North + velocityDesired.East * velocityState.East < 0.0f) &&
// quad is moving at significant speed (during flyaway it would keep speeding up)
squaref(velocityState.North) + squaref(velocityState.East) > 1.0f
) {
vtolEmergencyFallback += dT;
if (vtolEmergencyFallback >= vtolPathFollowerSettings->FlyawayEmergencyFallbackTriggerTime) {
// after emergency timeout, trigger alarm - everything else is handled by callers
// (switch to emergency algorithm, switch to emergency waypoint in pathplanner, alarms, ...)
result = 0;
}
} else {
vtolEmergencyFallback = 0.0f;
}
#endif // if 0
if (yaw_attitude) {
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.Yaw = yaw_direction;
} else {
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw;
}
// default thrust mode to cruise control
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL;
if (mManualThrust) {
stabDesired.Thrust = manualControl.Thrust;
} else {
stabDesired.Thrust = controlDown.GetDownCommand();
}
StabilizationDesiredSet(&stabDesired);
return result;
}
/**
* Compute desired attitude for vtols - emergency fallback
*/
void VtolFlyController::UpdateDesiredAttitudeEmergencyFallback()
{
VelocityDesiredData velocityDesired;
VelocityStateData velocityState;
StabilizationDesiredData stabDesired;
float courseError;
float courseCommand;
VelocityStateGet(&velocityState);
VelocityDesiredGet(&velocityDesired);
ManualControlCommandData manualControlData;
ManualControlCommandGet(&manualControlData);
courseError = RAD2DEG(atan2f(velocityDesired.East, velocityDesired.North) - atan2f(velocityState.East, velocityState.North));
if (courseError < -180.0f) {
courseError += 360.0f;
}
if (courseError > 180.0f) {
courseError -= 360.0f;
}
courseCommand = (courseError * vtolPathFollowerSettings->EmergencyFallbackYawRate.kP);
stabDesired.Yaw = boundf(courseCommand, -vtolPathFollowerSettings->EmergencyFallbackYawRate.Max, vtolPathFollowerSettings->EmergencyFallbackYawRate.Max);
controlDown.UpdateVelocitySetpoint(velocityDesired.Down);
controlDown.UpdateVelocityState(velocityState.Down);
stabDesired.Thrust = controlDown.GetDownCommand();
stabDesired.Roll = vtolPathFollowerSettings->EmergencyFallbackAttitude.Roll;
stabDesired.Pitch = vtolPathFollowerSettings->EmergencyFallbackAttitude.Pitch;
if (vtolPathFollowerSettings->ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_MANUAL) {
stabDesired.Thrust = manualControlData.Thrust;
}
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL;
StabilizationDesiredSet(&stabDesired);
}
void VtolFlyController::UpdateAutoPilot()
{
if (vtolPathFollowerSettings->ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_MANUAL) {
mManualThrust = true;
}
uint8_t result = RunAutoPilot();
if (result) {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
} else {
pathStatus->Status = PATHSTATUS_STATUS_CRITICAL;
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
}
PathStatusSet(pathStatus);
// If rtbl, detect arrival at the endpoint and then triggers a change
// to the pathDesired to initiate a Landing sequence. This is the simpliest approach. plans.c
// can't manage this. And pathplanner whilst similar does not manage this as it is not a
// waypoint traversal and is not aware of flight modes other than path plan.
if ((uint8_t)pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_NEXTCOMMAND] == FLIGHTMODESETTINGS_RETURNTOBASENEXTCOMMAND_LAND) {
if (pathStatus->fractional_progress > RTB_LAND_FRACTIONAL_PROGRESS_START_CHECKS) {
if (fabsf(pathStatus->correction_direction_north) < RTB_LAND_NE_DISTANCE_REQUIRED_TO_START_LAND_SEQUENCE && fabsf(pathStatus->correction_direction_east) < RTB_LAND_NE_DISTANCE_REQUIRED_TO_START_LAND_SEQUENCE) {
plan_setup_land();
}
}
}
}
/**
* vtol autopilot
* use hover capable algorithm with unlimeted movement calculation. if that fails (flyaway situation due to compass failure)
* fall back to emergency fallback autopilot to keep minimum amount of flight control
*/
uint8_t VtolFlyController::RunAutoPilot()
{
enum { RETURN_0 = 0, RETURN_1 = 1, RETURN_RESULT } returnmode;
enum { FOLLOWER_REGULAR, FOLLOWER_FALLBACK } followermode;
uint8_t result = 0;
// decide on behaviour based on settings and system state
if (vtolEmergencyFallbackSwitch) {
returnmode = RETURN_0;
followermode = FOLLOWER_FALLBACK;
} else {
if (vtolPathFollowerSettings->FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_ALWAYS) {
returnmode = RETURN_1;
followermode = FOLLOWER_FALLBACK;
} else {
returnmode = RETURN_RESULT;
followermode = FOLLOWER_REGULAR;
}
}
switch (followermode) {
case FOLLOWER_REGULAR:
{
// horizontal position control PID loop works according to settings in regular mode, allowing integral terms
UpdateVelocityDesired();
// yaw behaviour is configurable in vtolpathfollower, select yaw control algorithm
bool yaw_attitude = true;
float yaw = 0.0f;
switch (vtolPathFollowerSettings->YawControl) {
case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_MANUAL:
yaw_attitude = false;
break;
case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_TAILIN:
yaw = updateTailInBearing();
break;
case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_MOVEMENTDIRECTION:
yaw = updateCourseBearing();
break;
case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_PATHDIRECTION:
yaw = updatePathBearing();
break;
case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_POI:
yaw = updatePOIBearing();
break;
}
result = UpdateStabilizationDesired(yaw_attitude, yaw);
if (!result) {
if (vtolPathFollowerSettings->FlyawayEmergencyFallback != VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_DISABLED) {
// switch to emergency follower if follower indicates problems
vtolEmergencyFallbackSwitch = true;
}
}
}
break;
case FOLLOWER_FALLBACK:
{
// fallback loop only cares about intended horizontal flight direction, simplify control behaviour accordingly
controlNE.UpdatePositionalParameters(1.0f);
UpdateVelocityDesired();
// emergency follower has no return value
UpdateDesiredAttitudeEmergencyFallback();
}
break;
}
switch (returnmode) {
case RETURN_RESULT:
return result;
default:
// returns either 0 or 1 according to enum definition above
return returnmode;
}
}
/**
* Compute bearing of current takeoff location
*/
float VtolFlyController::updateTailInBearing()
{
PositionStateData p;
PositionStateGet(&p);
TakeOffLocationData t;
TakeOffLocationGet(&t);
// atan2f always returns in between + and - 180 degrees
return RAD2DEG(atan2f(p.East - t.East, p.North - t.North));
}
/**
* Compute bearing of current movement direction
*/
float VtolFlyController::updateCourseBearing()
{
VelocityStateData v;
VelocityStateGet(&v);
// atan2f always returns in between + and - 180 degrees
return RAD2DEG(atan2f(v.East, v.North));
}
/**
* Compute bearing of current path direction
*/
float VtolFlyController::updatePathBearing()
{
PositionStateData positionState;
PositionStateGet(&positionState);
float cur[3] = { positionState.North,
positionState.East,
positionState.Down };
struct path_status progress;
path_progress(pathDesired, cur, &progress, true);
// atan2f always returns in between + and - 180 degrees
return RAD2DEG(atan2f(progress.path_vector[1], progress.path_vector[0]));
}
/**
* Compute bearing between current position and POI
*/
float VtolFlyController::updatePOIBearing()
{
PoiLocationData poi;
PoiLocationGet(&poi);
PositionStateData positionState;
PositionStateGet(&positionState);
const float dT = vtolPathFollowerSettings->UpdatePeriod / 1000.0f;
float dLoc[3];
float yaw = 0;
/*float elevation = 0;*/
dLoc[0] = positionState.North - poi.North;
dLoc[1] = positionState.East - poi.East;
dLoc[2] = positionState.Down - poi.Down;
if (dLoc[1] < 0) {
yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) + 180.0f;
} else {
yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) - 180.0f;
}
ManualControlCommandData manualControlData;
ManualControlCommandGet(&manualControlData);
float pathAngle = 0;
if (manualControlData.Roll > DEADBAND_HIGH) {
pathAngle = -(manualControlData.Roll - DEADBAND_HIGH) * dT * 300.0f;
} else if (manualControlData.Roll < DEADBAND_LOW) {
pathAngle = -(manualControlData.Roll - DEADBAND_LOW) * dT * 300.0f;
}
return yaw + (pathAngle / 2.0f);
}

275
flight/modules/PathFollower/vtollandcontroller.cpp Normal file
View File

@ -0,0 +1,275 @@
/*
******************************************************************************
*
* @file vtollandcontroller.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Vtol landing controller loop
* @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
*/
extern "C" {
#include <openpilot.h>
#include <callbackinfo.h>
#include <math.h>
#include <pid.h>
#include <CoordinateConversions.h>
#include <sin_lookup.h>
#include <pathdesired.h>
#include <paths.h>
#include "plans.h"
#include <sanitycheck.h>
#include <accelstate.h>
#include <vtolpathfollowersettings.h>
#include <flightstatus.h>
#include <flightmodesettings.h>
#include <pathstatus.h>
#include <positionstate.h>
#include <velocitystate.h>
#include <velocitydesired.h>
#include <stabilizationdesired.h>
#include <attitudestate.h>
#include <takeofflocation.h>
#include <manualcontrolcommand.h>
#include <systemsettings.h>
#include <stabilizationbank.h>
#include <stabilizationdesired.h>
#include <vtolselftuningstats.h>
#include <pathsummary.h>
}
// C++ includes
#include "vtollandcontroller.h"
#include "pathfollowerfsm.h"
#include "vtollandfsm.h"
#include "pidcontroldown.h"
// Private constants
// pointer to a singleton instance
VtolLandController *VtolLandController::p_inst = 0;
VtolLandController::VtolLandController()
: fsm(NULL), vtolPathFollowerSettings(NULL), mActive(false)
{}
// Called when mode first engaged
void VtolLandController::Activate(void)
{
if (!mActive) {
mActive = true;
SettingsUpdated();
fsm->Activate();
controlDown.Activate();
controlNE.Activate();
}
}
uint8_t VtolLandController::IsActive(void)
{
return mActive;
}
uint8_t VtolLandController::Mode(void)
{
return PATHDESIRED_MODE_LAND;
}
// Objective updated in pathdesired, e.g. same flight mode but new target velocity
void VtolLandController::ObjectiveUpdated(void)
{
// Set the objective's target velocity
controlDown.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_DOWN]);
controlNE.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_NORTH],
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_EAST]);
controlNE.UpdatePositionSetpoint(pathDesired->End.North, pathDesired->End.East);
}
void VtolLandController::Deactivate(void)
{
if (mActive) {
mActive = false;
fsm->Inactive();
controlDown.Deactivate();
controlNE.Deactivate();
}
}
void VtolLandController::SettingsUpdated(void)
{
const float dT = vtolPathFollowerSettings->UpdatePeriod / 1000.0f;
controlNE.UpdateParameters(vtolPathFollowerSettings->HorizontalVelPID.Kp,
vtolPathFollowerSettings->HorizontalVelPID.Ki,
vtolPathFollowerSettings->HorizontalVelPID.Kd,
vtolPathFollowerSettings->HorizontalVelPID.Beta,
dT,
vtolPathFollowerSettings->HorizontalVelMax);
controlNE.UpdatePositionalParameters(vtolPathFollowerSettings->HorizontalPosP);
controlNE.UpdateCommandParameters(-vtolPathFollowerSettings->MaxRollPitch, vtolPathFollowerSettings->MaxRollPitch, vtolPathFollowerSettings->VelocityFeedforward);
controlDown.UpdateParameters(vtolPathFollowerSettings->LandVerticalVelPID.Kp,
vtolPathFollowerSettings->LandVerticalVelPID.Ki,
vtolPathFollowerSettings->LandVerticalVelPID.Kd,
vtolPathFollowerSettings->LandVerticalVelPID.Beta,
dT,
vtolPathFollowerSettings->VerticalVelMax);
// The following is not currently used in the landing control.
controlDown.UpdatePositionalParameters(vtolPathFollowerSettings->VerticalPosP);
VtolSelfTuningStatsData vtolSelfTuningStats;
VtolSelfTuningStatsGet(&vtolSelfTuningStats);
controlDown.UpdateNeutralThrust(vtolSelfTuningStats.NeutralThrustOffset + vtolPathFollowerSettings->ThrustLimits.Neutral);
// initialise limits on thrust but note the FSM can override.
controlDown.SetThrustLimits(vtolPathFollowerSettings->ThrustLimits.Min, vtolPathFollowerSettings->ThrustLimits.Max);
fsm->SettingsUpdated();
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t VtolLandController::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollowerSettings)
{
PIOS_Assert(ptr_vtolPathFollowerSettings);
if (fsm == 0) {
fsm = (PathFollowerFSM *)VtolLandFSM::instance();
VtolLandFSM::instance()->Initialize(ptr_vtolPathFollowerSettings, pathDesired, flightStatus);
vtolPathFollowerSettings = ptr_vtolPathFollowerSettings;
controlDown.Initialize(fsm);
}
return 0;
}
void VtolLandController::UpdateVelocityDesired()
{
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
VelocityDesiredData velocityDesired;
controlDown.UpdateVelocityState(velocityState.Down);
controlNE.UpdateVelocityState(velocityState.North, velocityState.East);
// Implement optional horizontal position hold.
if ((((uint8_t)pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_OPTIONS]) == PATHDESIRED_MODEPARAMETER_LAND_OPTION_HORIZONTAL_PH) ||
(flightStatus->ControlChain.PathPlanner == FLIGHTSTATUS_CONTROLCHAIN_TRUE)) {
// landing flight mode has stored original horizontal position in pathdesired
PositionStateData positionState;
PositionStateGet(&positionState);
controlNE.UpdatePositionState(positionState.North, positionState.East);
controlNE.ControlPosition();
}
velocityDesired.Down = controlDown.GetVelocityDesired();
float north, east;
controlNE.GetVelocityDesired(&north, &east);
velocityDesired.North = north;
velocityDesired.East = east;
// update pathstatus
pathStatus->error = 0.0f;
pathStatus->fractional_progress = 0.0f;
if (fsm->GetCurrentState() == PFFSM_STATE_DISARMED) {
pathStatus->fractional_progress = 1.0f;
}
pathStatus->path_direction_north = velocityDesired.North;
pathStatus->path_direction_east = velocityDesired.East;
pathStatus->path_direction_down = velocityDesired.Down;
pathStatus->correction_direction_north = velocityDesired.North - velocityState.North;
pathStatus->correction_direction_east = velocityDesired.East - velocityState.East;
pathStatus->correction_direction_down = velocityDesired.Down - velocityState.Down;
VelocityDesiredSet(&velocityDesired);
}
int8_t VtolLandController::UpdateStabilizationDesired(bool yaw_attitude, float yaw_direction)
{
uint8_t result = 1;
StabilizationDesiredData stabDesired;
AttitudeStateData attitudeState;
StabilizationBankData stabSettings;
float northCommand;
float eastCommand;
StabilizationDesiredGet(&stabDesired);
AttitudeStateGet(&attitudeState);
StabilizationBankGet(&stabSettings);
controlNE.GetNECommand(&northCommand, &eastCommand);
stabDesired.Thrust = controlDown.GetDownCommand();
float angle_radians = DEG2RAD(attitudeState.Yaw);
float cos_angle = cosf(angle_radians);
float sine_angle = sinf(angle_radians);
float maxPitch = vtolPathFollowerSettings->MaxRollPitch;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.Pitch = boundf(-northCommand * cos_angle - eastCommand * sine_angle, -maxPitch, maxPitch);
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.Roll = boundf(-northCommand * sine_angle + eastCommand * cos_angle, -maxPitch, maxPitch);
ManualControlCommandData manualControl;
ManualControlCommandGet(&manualControl);
if (yaw_attitude) {
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.Yaw = yaw_direction;
} else {
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw;
}
// default thrust mode to cruise control
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL;
fsm->ConstrainStabiDesired(&stabDesired); // excludes thrust
StabilizationDesiredSet(&stabDesired);
return result;
}
void VtolLandController::UpdateAutoPilot()
{
fsm->Update();
UpdateVelocityDesired();
// yaw behaviour is configurable in vtolpathfollower, select yaw control algorithm
bool yaw_attitude = false;
float yaw = 0.0f;
fsm->GetYaw(yaw_attitude, yaw);
int8_t result = UpdateStabilizationDesired(yaw_attitude, yaw);
if (!result) {
fsm->Abort();
}
PathStatusSet(pathStatus);
}

701
flight/modules/PathFollower/vtollandfsm.cpp Normal file
View File

@ -0,0 +1,701 @@
/*
******************************************************************************
*
* @file vtollandfsm.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief This landing state machine is a helper state machine to the
* VtolLandController.
* @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
*/
extern "C" {
#include <openpilot.h>
#include <callbackinfo.h>
#include <math.h>
#include <pid.h>
#include <CoordinateConversions.h>
#include <sin_lookup.h>
#include <pathdesired.h>
#include <paths.h>
#include "plans.h"
#include <sanitycheck.h>
#include <homelocation.h>
#include <accelstate.h>
#include <vtolpathfollowersettings.h>
#include <flightstatus.h>
#include <flightmodesettings.h>
#include <pathstatus.h>
#include <positionstate.h>
#include <velocitystate.h>
#include <velocitydesired.h>
#include <stabilizationdesired.h>
#include <airspeedstate.h>
#include <attitudestate.h>
#include <takeofflocation.h>
#include <poilocation.h>
#include <manualcontrolcommand.h>
#include <systemsettings.h>
#include <stabilizationbank.h>
#include <stabilizationdesired.h>
#include <vtolselftuningstats.h>
#include <statusvtolland.h>
#include <pathsummary.h>
}
// C++ includes
#include <vtollandfsm.h>
// Private constants
#define TIMER_COUNT_PER_SECOND (1000 / vtolPathFollowerSettings->UpdatePeriod)
#define MIN_LANDRATE 0.1f
#define MAX_LANDRATE 0.6f
#define LOW_ALT_DESCENT_REDUCTION_FACTOR 0.7f // TODO Need to make the transition smooth
#define LANDRATE_LOWLIMIT_FACTOR 0.5f
#define LANDRATE_HILIMIT_FACTOR 1.5f
#define TIMEOUT_INIT_ALTHOLD (3 * TIMER_COUNT_PER_SECOND)
#define TIMEOUT_WTG_FOR_DESCENTRATE (10 * TIMER_COUNT_PER_SECOND)
#define WTG_FOR_DESCENTRATE_COUNT_LIMIT 10
#define TIMEOUT_AT_DESCENTRATE 10
#define TIMEOUT_GROUNDEFFECT (1 * TIMER_COUNT_PER_SECOND)
#define TIMEOUT_THRUSTDOWN (2 * TIMER_COUNT_PER_SECOND)
#define LANDING_PID_SCALAR_P 2.0f
#define LANDING_PID_SCALAR_I 10.0f
#define LANDING_SLOWDOWN_HEIGHT -5.0f
#define BOUNCE_VELOCITY_TRIGGER_LIMIT -0.3f
#define BOUNCE_ACCELERATION_TRIGGER_LIMIT -9.0f // -6.0 found to be too sensitive
#define BOUNCE_TRIGGER_COUNT 4
#define GROUNDEFFECT_SLOWDOWN_FACTOR 0.3f
#define GROUNDEFFECT_SLOWDOWN_COUNT 4
VtolLandFSM::PathFollowerFSM_LandStateHandler_T VtolLandFSM::sLandStateTable[LAND_STATE_SIZE] = {
[LAND_STATE_INACTIVE] = { .setup = &VtolLandFSM::setup_inactive, .run = 0 },
[LAND_STATE_INIT_ALTHOLD] = { .setup = &VtolLandFSM::setup_init_althold, .run = &VtolLandFSM::run_init_althold },
[LAND_STATE_WTG_FOR_DESCENTRATE] = { .setup = &VtolLandFSM::setup_wtg_for_descentrate, .run = &VtolLandFSM::run_wtg_for_descentrate },
[LAND_STATE_AT_DESCENTRATE] = { .setup = &VtolLandFSM::setup_at_descentrate, .run = &VtolLandFSM::run_at_descentrate },
[LAND_STATE_WTG_FOR_GROUNDEFFECT] = { .setup = &VtolLandFSM::setup_wtg_for_groundeffect, .run = &VtolLandFSM::run_wtg_for_groundeffect },
[LAND_STATE_GROUNDEFFECT] = { .setup = &VtolLandFSM::setup_groundeffect, .run = &VtolLandFSM::run_groundeffect },
[LAND_STATE_THRUSTDOWN] = { .setup = &VtolLandFSM::setup_thrustdown, .run = &VtolLandFSM::run_thrustdown },
[LAND_STATE_THRUSTOFF] = { .setup = &VtolLandFSM::setup_thrustoff, .run = &VtolLandFSM::run_thrustoff },
[LAND_STATE_DISARMED] = { .setup = &VtolLandFSM::setup_disarmed, .run = &VtolLandFSM::run_disarmed },
[LAND_STATE_ABORT] = { .setup = &VtolLandFSM::setup_abort, .run = &VtolLandFSM::run_abort }
};
// pointer to a singleton instance
VtolLandFSM *VtolLandFSM::p_inst = 0;
VtolLandFSM::VtolLandFSM()
: mLandData(0), vtolPathFollowerSettings(0), pathDesired(0), flightStatus(0)
{}
// Private types
// Private functions
// Public API methods
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t VtolLandFSM::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollowerSettings,
PathDesiredData *ptr_pathDesired,
FlightStatusData *ptr_flightStatus)
{
PIOS_Assert(ptr_vtolPathFollowerSettings);
PIOS_Assert(ptr_pathDesired);
PIOS_Assert(ptr_flightStatus);
if (mLandData == 0) {
mLandData = (VtolLandFSMData_T *)pios_malloc(sizeof(VtolLandFSMData_T));
PIOS_Assert(mLandData);
}
memset(mLandData, 0, sizeof(VtolLandFSMData_T));
vtolPathFollowerSettings = ptr_vtolPathFollowerSettings;
pathDesired = ptr_pathDesired;
flightStatus = ptr_flightStatus;
initFSM();
return 0;
}
void VtolLandFSM::Inactive(void)
{
memset(mLandData, 0, sizeof(VtolLandFSMData_T));
initFSM();
}
// Initialise the FSM
void VtolLandFSM::initFSM(void)
{
if (vtolPathFollowerSettings != 0) {
setState(STATUSVTOLLAND_STATE_INACTIVE, STATUSVTOLLAND_STATEEXITREASON_NONE);
} else {
mLandData->currentState = STATUSVTOLLAND_STATE_INACTIVE;
}
}
void VtolLandFSM::Activate()
{
memset(mLandData, 0, sizeof(VtolLandFSMData_T));
mLandData->currentState = STATUSVTOLLAND_STATE_INACTIVE;
mLandData->flLowAltitude = false;
mLandData->flAltitudeHold = false;
mLandData->fsmLandStatus.averageDescentRate = MIN_LANDRATE;
mLandData->fsmLandStatus.averageDescentThrust = vtolPathFollowerSettings->ThrustLimits.Neutral;
mLandData->fsmLandStatus.calculatedNeutralThrust = vtolPathFollowerSettings->ThrustLimits.Neutral;
mLandData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min;
mLandData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max;
TakeOffLocationGet(&(mLandData->takeOffLocation));
mLandData->fsmLandStatus.AltitudeAtState[STATUSVTOLLAND_STATE_INACTIVE] = 0.0f;
assessAltitude();
if (pathDesired->Mode == PATHDESIRED_MODE_LAND) {
#ifndef DEBUG_GROUNDIMPACT
setState(STATUSVTOLLAND_STATE_INITALTHOLD, STATUSVTOLLAND_STATEEXITREASON_NONE);
#else
setState(STATUSVTOLLAND_STATE_WTGFORGROUNDEFFECT, STATUSVTOLLAND_STATEEXITREASON_NONE);
#endif
} else {
// move to error state and callback to position hold
setState(STATUSVTOLLAND_STATE_ABORT, STATUSVTOLLAND_STATEEXITREASON_NONE);
}
}
void VtolLandFSM::Abort(void)
{
setState(STATUSVTOLLAND_STATE_ABORT, STATUSVTOLLAND_STATEEXITREASON_NONE);
}
PathFollowerFSMState_T VtolLandFSM::GetCurrentState(void)
{
switch (mLandData->currentState) {
case STATUSVTOLLAND_STATE_INACTIVE:
return PFFSM_STATE_INACTIVE;
break;
case STATUSVTOLLAND_STATE_ABORT:
return PFFSM_STATE_ABORT;
break;
case STATUSVTOLLAND_STATE_DISARMED:
return PFFSM_STATE_DISARMED;
break;
default:
return PFFSM_STATE_ACTIVE;
break;
}
}
void VtolLandFSM::Update()
{
runState();
if (GetCurrentState() != PFFSM_STATE_INACTIVE) {
runAlways();
}
}
int32_t VtolLandFSM::runState(void)
{
uint8_t flTimeout = false;
mLandData->stateRunCount++;
if (mLandData->stateTimeoutCount > 0 && mLandData->stateRunCount > mLandData->stateTimeoutCount) {
flTimeout = true;
}
// If the current state has a static function, call it
if (sLandStateTable[mLandData->currentState].run) {
(this->*sLandStateTable[mLandData->currentState].run)(flTimeout);
}
return 0;
}
int32_t VtolLandFSM::runAlways(void)
{
void assessAltitude(void);
return 0;
}
// PathFollower implements the PID scheme and has a objective
// set by a PathDesired object. Based on the mode, pathfollower
// uses FSM's as helper functions that manage state and event detection.
// PathFollower calls into FSM methods to alter its commands.
void VtolLandFSM::BoundThrust(float &ulow, float &uhigh)
{
ulow = mLandData->boundThrustMin;
uhigh = mLandData->boundThrustMax;
if (mLandData->flConstrainThrust) {
uhigh = mLandData->thrustLimit;
}
}
void VtolLandFSM::ConstrainStabiDesired(StabilizationDesiredData *stabDesired)
{
if (mLandData->flZeroStabiHorizontal && stabDesired) {
stabDesired->Pitch = 0.0f;
stabDesired->Roll = 0.0f;
stabDesired->Yaw = 0.0f;
}
}
void VtolLandFSM::CheckPidScaler(pid_scaler *local_scaler)
{
if (mLandData->flLowAltitude) {
local_scaler->p = LANDING_PID_SCALAR_P;
local_scaler->i = LANDING_PID_SCALAR_I;
}
}
// Set the new state and perform setup for subsequent state run calls
// This is called by state run functions on event detection that drive
// state transitions.
void VtolLandFSM::setState(StatusVtolLandStateOptions newState, StatusVtolLandStateExitReasonOptions reason)
{
mLandData->fsmLandStatus.StateExitReason[mLandData->currentState] = reason;
if (mLandData->currentState == newState) {
return;
}
mLandData->currentState = newState;
if (newState != STATUSVTOLLAND_STATE_INACTIVE) {
PositionStateData positionState;
PositionStateGet(&positionState);
float takeOffDown = 0.0f;
if (mLandData->takeOffLocation.Status == TAKEOFFLOCATION_STATUS_VALID) {
takeOffDown = mLandData->takeOffLocation.Down;
}
mLandData->fsmLandStatus.AltitudeAtState[newState] = positionState.Down - takeOffDown;
assessAltitude();
}
// Restart state timer counter
mLandData->stateRunCount = 0;
// Reset state timeout to disabled/zero
mLandData->stateTimeoutCount = 0;
if (sLandStateTable[mLandData->currentState].setup) {
(this->*sLandStateTable[mLandData->currentState].setup)();
}
updateVtolLandFSMStatus();
}
// Timeout utility function for use by state init implementations
void VtolLandFSM::setStateTimeout(int32_t count)
{
mLandData->stateTimeoutCount = count;
}
void VtolLandFSM::updateVtolLandFSMStatus()
{
mLandData->fsmLandStatus.State = mLandData->currentState;
if (mLandData->flLowAltitude) {
mLandData->fsmLandStatus.AltitudeState = STATUSVTOLLAND_ALTITUDESTATE_LOW;
} else {
mLandData->fsmLandStatus.AltitudeState = STATUSVTOLLAND_ALTITUDESTATE_HIGH;
}
StatusVtolLandSet(&mLandData->fsmLandStatus);
}
float VtolLandFSM::BoundVelocityDown(float velocity_down)
{
velocity_down = boundf(velocity_down, MIN_LANDRATE, MAX_LANDRATE);
if (mLandData->flLowAltitude) {
velocity_down *= LOW_ALT_DESCENT_REDUCTION_FACTOR;
}
mLandData->fsmLandStatus.targetDescentRate = velocity_down;
if (mLandData->flAltitudeHold) {
return 0.0f;
} else {
return velocity_down;
}
}
void VtolLandFSM::assessAltitude(void)
{
float positionDown;
PositionStateDownGet(&positionDown);
float takeOffDown = 0.0f;
if (mLandData->takeOffLocation.Status == TAKEOFFLOCATION_STATUS_VALID) {
takeOffDown = mLandData->takeOffLocation.Down;
}
float positionDownRelativeToTakeoff = positionDown - takeOffDown;
if (positionDownRelativeToTakeoff < LANDING_SLOWDOWN_HEIGHT) {
mLandData->flLowAltitude = false;
} else {
mLandData->flLowAltitude = true;
}
}
// FSM Setup and Run method implementation
// State: INACTIVE
void VtolLandFSM::setup_inactive(void)
{
// Re-initialise local variables
mLandData->flZeroStabiHorizontal = false;
mLandData->flConstrainThrust = false;
}
// State: INIT ALTHOLD
void VtolLandFSM::setup_init_althold(void)
{
setStateTimeout(TIMEOUT_INIT_ALTHOLD);
// get target descent velocity
mLandData->flZeroStabiHorizontal = false;
mLandData->fsmLandStatus.targetDescentRate = BoundVelocityDown(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_DOWN]);
mLandData->flConstrainThrust = false;
mLandData->flAltitudeHold = true;
mLandData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min;
mLandData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max;
}
void VtolLandFSM::run_init_althold(uint8_t flTimeout)
{
if (flTimeout) {
mLandData->flAltitudeHold = false;
setState(STATUSVTOLLAND_STATE_WTGFORDESCENTRATE, STATUSVTOLLAND_STATEEXITREASON_TIMEOUT);
}
}
// State: WAITING FOR DESCENT RATE
void VtolLandFSM::setup_wtg_for_descentrate(void)
{
setStateTimeout(TIMEOUT_WTG_FOR_DESCENTRATE);
// get target descent velocity
mLandData->flZeroStabiHorizontal = false;
mLandData->observationCount = 0;
mLandData->observation2Count = 0;
mLandData->flConstrainThrust = false;
mLandData->flAltitudeHold = false;
mLandData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min;
mLandData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max;
}
void VtolLandFSM::run_wtg_for_descentrate(uint8_t flTimeout)
{
// Look at current actual thrust...are we already shutdown??
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
// We don't expect PID to get exactly the target descent rate, so have a lower
// water mark but need to see 5 observations to be confident that we have semi-stable
// descent achieved
// we need to see velocity down within a range of control before we proceed, without which we
// really don't have confidence to allow later states to run.
if (velocityState.Down > (LANDRATE_LOWLIMIT_FACTOR * mLandData->fsmLandStatus.targetDescentRate) &&
velocityState.Down < (LANDRATE_HILIMIT_FACTOR * mLandData->fsmLandStatus.targetDescentRate)) {
if (mLandData->observationCount++ > WTG_FOR_DESCENTRATE_COUNT_LIMIT) {
setState(STATUSVTOLLAND_STATE_ATDESCENTRATE, STATUSVTOLLAND_STATEEXITREASON_DESCENTRATEOK);
return;
}
}
if (flTimeout) {
setState(STATUSVTOLLAND_STATE_ABORT, STATUSVTOLLAND_STATEEXITREASON_TIMEOUT);
}
}
// State: AT DESCENT RATE
void VtolLandFSM::setup_at_descentrate(void)
{
setStateTimeout(TIMEOUT_AT_DESCENTRATE);
mLandData->flZeroStabiHorizontal = false;
mLandData->observationCount = 0;
mLandData->sum1 = 0.0f;
mLandData->sum2 = 0.0f;
mLandData->flConstrainThrust = false;
mLandData->fsmLandStatus.averageDescentRate = MIN_LANDRATE;
mLandData->fsmLandStatus.averageDescentThrust = vtolPathFollowerSettings->ThrustLimits.Neutral;
mLandData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min;
mLandData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max;
}
void VtolLandFSM::run_at_descentrate(uint8_t flTimeout)
{
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
mLandData->sum1 += velocityState.Down;
mLandData->sum2 += stabDesired.Thrust;
mLandData->observationCount++;
if (flTimeout) {
mLandData->fsmLandStatus.averageDescentRate = boundf((mLandData->sum1 / (float)(mLandData->observationCount)), 0.5f * MIN_LANDRATE, 1.5f * MAX_LANDRATE);
mLandData->fsmLandStatus.averageDescentThrust = boundf((mLandData->sum2 / (float)(mLandData->observationCount)), vtolPathFollowerSettings->ThrustLimits.Min, vtolPathFollowerSettings->ThrustLimits.Max);
// We need to calculate a neutral limit to use later to constrain upper thrust range during states where we are close to the ground
// As our battery gets flat, ThrustLimits.Neutral needs to constrain us too much and we get too fast a descent rate. We can
// detect this by the fact that the descent rate will exceed the target and the required thrust will exceed the neutral value
mLandData->fsmLandStatus.calculatedNeutralThrust = mLandData->fsmLandStatus.averageDescentRate / mLandData->fsmLandStatus.targetDescentRate * mLandData->fsmLandStatus.averageDescentThrust;
mLandData->fsmLandStatus.calculatedNeutralThrust = boundf(mLandData->fsmLandStatus.calculatedNeutralThrust, vtolPathFollowerSettings->ThrustLimits.Neutral, vtolPathFollowerSettings->ThrustLimits.Max);
setState(STATUSVTOLLAND_STATE_WTGFORGROUNDEFFECT, STATUSVTOLLAND_STATEEXITREASON_DESCENTRATEOK);
}
}
// State: WAITING FOR GROUND EFFECT
void VtolLandFSM::setup_wtg_for_groundeffect(void)
{
// No timeout
mLandData->flZeroStabiHorizontal = false;
mLandData->observationCount = 0;
mLandData->observation2Count = 0;
mLandData->sum1 = 0.0f;
mLandData->sum2 = 0.0f;
mLandData->flConstrainThrust = false;
mLandData->fsmLandStatus.WtgForGroundEffect.BounceVelocity = 0.0f;
mLandData->fsmLandStatus.WtgForGroundEffect.BounceAccel = 0.0f;
mLandData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min;
mLandData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max;
}
void VtolLandFSM::run_wtg_for_groundeffect(__attribute__((unused)) uint8_t flTimeout)
{
// detect material downrating in thrust for 1 second.
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
AccelStateData accelState;
AccelStateGet(&accelState);
// +ve 9.8 expected
float g_e;
HomeLocationg_eGet(&g_e);
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
// detect bounce
uint8_t flBounce = (velocityState.Down < BOUNCE_VELOCITY_TRIGGER_LIMIT);
if (flBounce) {
mLandData->fsmLandStatus.WtgForGroundEffect.BounceVelocity = velocityState.Down;
} else {
mLandData->fsmLandStatus.WtgForGroundEffect.BounceVelocity = 0.0f;
}
// invert sign of accel to the standard convention of down is +ve and subtract the gravity to get
// a relative acceleration term.
float bounceAccel = -accelState.z - g_e;
uint8_t flBounceAccel = (bounceAccel < BOUNCE_ACCELERATION_TRIGGER_LIMIT);
if (flBounceAccel) {
mLandData->fsmLandStatus.WtgForGroundEffect.BounceAccel = bounceAccel;
} else {
mLandData->fsmLandStatus.WtgForGroundEffect.BounceAccel = 0.0f;
}
if (flBounce) { // || flBounceAccel) { // accel trigger can occur due to vibration and is too sensitive
mLandData->observation2Count++;
if (mLandData->observation2Count > BOUNCE_TRIGGER_COUNT) {
setState(STATUSVTOLLAND_STATE_GROUNDEFFECT, (flBounce ? STATUSVTOLLAND_STATEEXITREASON_BOUNCEVELOCITY : STATUSVTOLLAND_STATEEXITREASON_BOUNCEACCEL));
return;
}
} else {
mLandData->observation2Count = 0;
}
// detect low descent rate
uint8_t flDescentRateLow = (velocityState.Down < (GROUNDEFFECT_SLOWDOWN_FACTOR * mLandData->fsmLandStatus.averageDescentRate));
if (flDescentRateLow) {
mLandData->boundThrustMax = mLandData->fsmLandStatus.calculatedNeutralThrust;
mLandData->observationCount++;
if (mLandData->observationCount > GROUNDEFFECT_SLOWDOWN_COUNT) {
#ifndef DEBUG_GROUNDIMPACT
setState(STATUSVTOLLAND_STATE_GROUNDEFFECT, STATUSVTOLLAND_STATEEXITREASON_LOWDESCENTRATE);
#endif
return;
}
} else {
mLandData->observationCount = 0;
}
updateVtolLandFSMStatus();
}
// STATE: GROUNDEFFET
void VtolLandFSM::setup_groundeffect(void)
{
setStateTimeout(TIMEOUT_GROUNDEFFECT);
mLandData->flZeroStabiHorizontal = false;
PositionStateData positionState;
PositionStateGet(&positionState);
mLandData->expectedLandPositionNorth = positionState.North;
mLandData->expectedLandPositionEast = positionState.East;
mLandData->flConstrainThrust = false;
// now that we have ground effect limit max thrust to neutral
mLandData->boundThrustMin = -0.1f;
mLandData->boundThrustMax = mLandData->fsmLandStatus.calculatedNeutralThrust;
}
void VtolLandFSM::run_groundeffect(__attribute__((unused)) uint8_t flTimeout)
{
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
if (stabDesired.Thrust < 0.0f) {
setState(STATUSVTOLLAND_STATE_THRUSTOFF, STATUSVTOLLAND_STATEEXITREASON_ZEROTHRUST);
return;
}
// Stay in this state until we get a low altitude flag.
if (mLandData->flLowAltitude == false) {
// worst case scenario is that we land and the pid brings thrust down to zero.
return;
}
// detect broad sideways drift. If for some reason we have a hard landing that the bounce detection misses, this will kick in
PositionStateData positionState;
PositionStateGet(&positionState);
float north_error = mLandData->expectedLandPositionNorth - positionState.North;
float east_error = mLandData->expectedLandPositionEast - positionState.East;
float positionError = sqrtf(north_error * north_error + east_error * east_error);
if (positionError > 1.5f) {
setState(STATUSVTOLLAND_STATE_THRUSTDOWN, STATUSVTOLLAND_STATEEXITREASON_POSITIONERROR);
return;
}
if (flTimeout) {
setState(STATUSVTOLLAND_STATE_THRUSTDOWN, STATUSVTOLLAND_STATEEXITREASON_TIMEOUT);
}
}
// STATE: THRUSTDOWN
void VtolLandFSM::setup_thrustdown(void)
{
setStateTimeout(TIMEOUT_THRUSTDOWN);
mLandData->flZeroStabiHorizontal = true;
mLandData->flConstrainThrust = true;
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
mLandData->thrustLimit = stabDesired.Thrust;
mLandData->sum1 = stabDesired.Thrust / (float)TIMEOUT_THRUSTDOWN;
mLandData->boundThrustMin = -0.1f;
mLandData->boundThrustMax = mLandData->fsmLandStatus.calculatedNeutralThrust;
}
void VtolLandFSM::run_thrustdown(__attribute__((unused)) uint8_t flTimeout)
{
// reduce thrust setpoint step by step
mLandData->thrustLimit -= mLandData->sum1;
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
if (stabDesired.Thrust < 0.0f || mLandData->thrustLimit < 0.0f) {
setState(STATUSVTOLLAND_STATE_THRUSTOFF, STATUSVTOLLAND_STATEEXITREASON_ZEROTHRUST);
}
if (flTimeout) {
setState(STATUSVTOLLAND_STATE_THRUSTOFF, STATUSVTOLLAND_STATEEXITREASON_TIMEOUT);
}
}
// STATE: THRUSTOFF
void VtolLandFSM::setup_thrustoff(void)
{
mLandData->thrustLimit = -1.0f;
mLandData->flConstrainThrust = true;
mLandData->boundThrustMin = -0.1f;
mLandData->boundThrustMax = 0.0f;
}
void VtolLandFSM::run_thrustoff(__attribute__((unused)) uint8_t flTimeout)
{
setState(STATUSVTOLLAND_STATE_DISARMED, STATUSVTOLLAND_STATEEXITREASON_NONE);
}
// STATE: DISARMED
void VtolLandFSM::setup_disarmed(void)
{
// nothing to do
mLandData->flConstrainThrust = false;
mLandData->flZeroStabiHorizontal = false;
mLandData->observationCount = 0;
mLandData->boundThrustMin = -0.1f;
mLandData->boundThrustMax = 0.0f;
}
void VtolLandFSM::run_disarmed(__attribute__((unused)) uint8_t flTimeout)
{
#ifdef DEBUG_GROUNDIMPACT
if (mLandData->observationCount++ > 100) {
setState(STATUSVTOLLAND_STATE_WTGFORGROUNDEFFECT, STATUSVTOLLAND_STATEEXITREASON_NONE);
}
#endif
}
void VtolLandFSM::fallback_to_hold(void)
{
PositionStateData positionState;
PositionStateGet(&positionState);
pathDesired->End.North = positionState.North;
pathDesired->End.East = positionState.East;
pathDesired->End.Down = positionState.Down;
pathDesired->Start.North = positionState.North;
pathDesired->Start.East = positionState.East;
pathDesired->Start.Down = positionState.Down;
pathDesired->StartingVelocity = 0.0f;
pathDesired->EndingVelocity = 0.0f;
pathDesired->Mode = PATHDESIRED_MODE_GOTOENDPOINT;
PathDesiredSet(pathDesired);
}
// abort repeatedly overwrites pathfollower's objective on a landing abort and
// continues to do so until a flight mode change.
void VtolLandFSM::setup_abort(void)
{
mLandData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min;
mLandData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max;
mLandData->flConstrainThrust = false;
mLandData->flZeroStabiHorizontal = false;
fallback_to_hold();
}
void VtolLandFSM::run_abort(__attribute__((unused)) uint8_t flTimeout)
{}

227
flight/modules/PathFollower/vtolvelocitycontroller.cpp Normal file
View File

@ -0,0 +1,227 @@
/*
******************************************************************************
*
* @file VtolVelocityController.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Velocity roam controller for vtols
* @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
*/
extern "C" {
#include <openpilot.h>
#include <callbackinfo.h>
#include <math.h>
#include <pid.h>
#include <CoordinateConversions.h>
#include <sin_lookup.h>
#include <pathdesired.h>
#include <paths.h>
#include "plans.h"
#include <sanitycheck.h>
#include <vtolpathfollowersettings.h>
#include <flightmodesettings.h>
#include <pathstatus.h>
#include <positionstate.h>
#include <velocitystate.h>
#include <velocitydesired.h>
#include <stabilizationdesired.h>
#include <attitudestate.h>
#include <flightstatus.h>
#include <manualcontrolcommand.h>
#include <systemsettings.h>
#include <stabilizationbank.h>
#include <stabilizationdesired.h>
#include <pathsummary.h>
}
// C++ includes
#include "vtolvelocitycontroller.h"
#include "pidcontrolne.h"
// Private constants
// pointer to a singleton instance
VtolVelocityController *VtolVelocityController::p_inst = 0;
VtolVelocityController::VtolVelocityController()
: vtolPathFollowerSettings(0), mActive(false)
{}
// Called when mode first engaged
void VtolVelocityController::Activate(void)
{
if (!mActive) {
mActive = true;
SettingsUpdated();
controlNE.Activate();
}
}
uint8_t VtolVelocityController::IsActive(void)
{
return mActive;
}
uint8_t VtolVelocityController::Mode(void)
{
return PATHDESIRED_MODE_VELOCITY;
}
void VtolVelocityController::ObjectiveUpdated(void)
{
controlNE.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_NORTH],
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_EAST]);
}
void VtolVelocityController::Deactivate(void)
{
if (mActive) {
mActive = false;
controlNE.Deactivate();
}
}
void VtolVelocityController::SettingsUpdated(void)
{
const float dT = vtolPathFollowerSettings->UpdatePeriod / 1000.0f;
controlNE.UpdateParameters(vtolPathFollowerSettings->HorizontalVelPID.Kp,
vtolPathFollowerSettings->HorizontalVelPID.Ki,
vtolPathFollowerSettings->HorizontalVelPID.Kd,
vtolPathFollowerSettings->HorizontalVelPID.Beta,
dT,
vtolPathFollowerSettings->HorizontalVelMax);
controlNE.UpdatePositionalParameters(vtolPathFollowerSettings->HorizontalPosP);
controlNE.UpdateCommandParameters(-vtolPathFollowerSettings->MaxRollPitch, vtolPathFollowerSettings->MaxRollPitch, vtolPathFollowerSettings->VelocityFeedforward);
}
int32_t VtolVelocityController::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollowerSettings)
{
PIOS_Assert(ptr_vtolPathFollowerSettings);
vtolPathFollowerSettings = ptr_vtolPathFollowerSettings;
return 0;
}
void VtolVelocityController::UpdateVelocityDesired()
{
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
VelocityDesiredData velocityDesired;
controlNE.UpdateVelocityState(velocityState.North, velocityState.East);
velocityDesired.Down = 0.0f;
float north, east;
controlNE.GetVelocityDesired(&north, &east);
velocityDesired.North = north;
velocityDesired.East = east;
// update pathstatus
pathStatus->error = 0.0f;
pathStatus->fractional_progress = 0.0f;
pathStatus->path_direction_north = velocityDesired.North;
pathStatus->path_direction_east = velocityDesired.East;
pathStatus->path_direction_down = velocityDesired.Down;
pathStatus->correction_direction_north = velocityDesired.North - velocityState.North;
pathStatus->correction_direction_east = velocityDesired.East - velocityState.East;
pathStatus->correction_direction_down = 0.0f;
VelocityDesiredSet(&velocityDesired);
}
int8_t VtolVelocityController::UpdateStabilizationDesired(__attribute__((unused)) bool yaw_attitude, __attribute__((unused)) float yaw_direction)
{
uint8_t result = 1;
StabilizationDesiredData stabDesired;
AttitudeStateData attitudeState;
StabilizationBankData stabSettings;
float northCommand;
float eastCommand;
StabilizationDesiredGet(&stabDesired);
AttitudeStateGet(&attitudeState);
StabilizationBankGet(&stabSettings);
controlNE.GetNECommand(&northCommand, &eastCommand);
float angle_radians = DEG2RAD(attitudeState.Yaw);
float cos_angle = cosf(angle_radians);
float sine_angle = sinf(angle_radians);
float maxPitch = vtolPathFollowerSettings->MaxRollPitch;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.Pitch = boundf(-northCommand * cos_angle - eastCommand * sine_angle, -maxPitch, maxPitch);
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.Roll = boundf(-northCommand * sine_angle + eastCommand * cos_angle, -maxPitch, maxPitch);
ManualControlCommandData manualControl;
ManualControlCommandGet(&manualControl);
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw;
// default thrust mode to altvario
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEVARIO;
StabilizationDesiredSet(&stabDesired);
return result;
}
void VtolVelocityController::UpdateAutoPilot()
{
UpdateVelocityDesired();
bool yaw_attitude = false;
float yaw = 0.0f;
int8_t result = UpdateStabilizationDesired(yaw_attitude, yaw);
if (!result) {
fallback_to_hold();
}
PathStatusSet(pathStatus);
}
void VtolVelocityController::fallback_to_hold(void)
{
PositionStateData positionState;
PositionStateGet(&positionState);
pathDesired->End.North = positionState.North;
pathDesired->End.East = positionState.East;
pathDesired->End.Down = positionState.Down;
pathDesired->Start.North = positionState.North;
pathDesired->Start.East = positionState.East;
pathDesired->Start.Down = positionState.Down;
pathDesired->StartingVelocity = 0.0f;
pathDesired->EndingVelocity = 0.0f;
pathDesired->Mode = PATHDESIRED_MODE_GOTOENDPOINT;
PathDesiredSet(pathDesired);
}

259
flight/modules/PathPlanner/pathplanner.c
View File

@ -43,8 +43,14 @@
#include "waypoint.h"
#include "waypointactive.h"
#include "flightmodesettings.h"
#include <systemsettings.h>
#include "paths.h"
#include "plans.h"
#include <sanitycheck.h>
#include <vtolpathfollowersettings.h>
#include <statusvtolautotakeoff.h>
#include <statusvtolland.h>
#include <manualcontrolcommand.h>
// Private constants
#define STACK_SIZE_BYTES 1024
@ -73,6 +79,10 @@ static uint8_t conditionAboveSpeed();
static uint8_t conditionPointingTowardsNext();
static uint8_t conditionPythonScript();
static uint8_t conditionImmediate();
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
static void planner_setup_pathdesired_land(PathDesiredData *pathDesired);
static void planner_setup_pathdesired_takeoff(PathDesiredData *pathDesired);
static void planner_setup_pathdesired(PathDesiredData *pathDesired, bool overwrite_start_position);
// Private variables
@ -82,7 +92,10 @@ static WaypointActiveData waypointActive;
static WaypointData waypoint;
static PathActionData pathAction;
static bool pathplanner_active = false;
static FrameType_t frameType;
static bool mode3D;
extern FrameType_t GetCurrentFrameType();
/**
* Module initialization
@ -95,6 +108,9 @@ int32_t PathPlannerStart()
WaypointActiveConnectCallback(commandUpdated);
PathActionConnectCallback(commandUpdated);
PathStatusConnectCallback(statusUpdated);
SettingsUpdatedCb(NULL);
SystemSettingsConnectCallback(&SettingsUpdatedCb);
VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
// Start main task callback
PIOS_CALLBACKSCHEDULER_Dispatch(pathPlannerHandle);
@ -116,6 +132,8 @@ int32_t PathPlannerInitialize()
VelocityStateInitialize();
WaypointInitialize();
WaypointActiveInitialize();
StatusVtolAutoTakeoffInitialize();
StatusVtolLandInitialize();
pathPlannerHandle = PIOS_CALLBACKSCHEDULER_Create(&pathPlannerTask, CALLBACK_PRIORITY_REGULAR, TASK_PRIORITY, CALLBACKINFO_RUNNING_PATHPLANNER0, STACK_SIZE_BYTES);
pathDesiredUpdaterHandle = PIOS_CALLBACKSCHEDULER_Create(&updatePathDesired, CALLBACK_PRIORITY_CRITICAL, TASK_PRIORITY, CALLBACKINFO_RUNNING_PATHPLANNER1, STACK_SIZE_BYTES);
@ -125,6 +143,40 @@ int32_t PathPlannerInitialize()
MODULE_INITCALL(PathPlannerInitialize, PathPlannerStart);
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
VtolPathFollowerSettingsTreatCustomCraftAsOptions TreatCustomCraftAs;
VtolPathFollowerSettingsTreatCustomCraftAsGet(&TreatCustomCraftAs);
frameType = GetCurrentFrameType();
if (frameType == FRAME_TYPE_CUSTOM) {
switch (TreatCustomCraftAs) {
case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_FIXEDWING:
frameType = FRAME_TYPE_FIXED_WING;
break;
case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_VTOL:
frameType = FRAME_TYPE_MULTIROTOR;
break;
case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_GROUND:
frameType = FRAME_TYPE_GROUND;
break;
}
}
switch (frameType) {
case FRAME_TYPE_GROUND:
mode3D = false;
break;
default:
mode3D = true;
}
}
#define AUTOTAKEOFF_THROTTLE_LIMIT_TO_ALLOW_TAKEOFF_START 0.3f
/**
* Module task
*/
@ -160,27 +212,33 @@ static void pathPlannerTask()
static uint8_t failsafeRTHset = 0;
if (!validPathPlan) {
// At this point the craft is in PathPlanner mode, so pilot has no manual control capability.
// Failsafe: behave as if in return to base mode
// what to do in this case is debatable, it might be better to just
// make a forced disarming but rth has a higher chance of survival when
// in flight.
pathplanner_active = false;
if (!failsafeRTHset) {
failsafeRTHset = 1;
// copy pasta: same calculation as in manualcontrol, set return to home coordinates
plan_setup_positionHold();
}
AlarmsSet(SYSTEMALARMS_ALARM_PATHPLAN, SYSTEMALARMS_ALARM_CRITICAL);
return;
}
failsafeRTHset = 0;
AlarmsClear(SYSTEMALARMS_ALARM_PATHPLAN);
WaypointActiveGet(&waypointActive);
// with the introduction of takeoff, we allow for arming
// whilst in pathplanner mode. Previously it was just an assumption that
// a user never armed in pathplanner mode. This check allows a user to select
// pathplanner, to upload waypoints, and then arm in pathplanner.
if (!flightStatus.Armed) {
return;
}
// the transition from pathplanner to another flightmode back to pathplanner
// triggers a reset back to 0 index in the waypoint list
if (pathplanner_active == false) {
pathplanner_active = true;
@ -207,6 +265,22 @@ static void pathPlannerTask()
return;
}
// check start conditions
// autotakeoff requires midpoint thrust if we are in a pending takeoff situation
if (pathAction.Mode == PATHACTION_MODE_AUTOTAKEOFF) {
pathAction.EndCondition = PATHACTION_ENDCONDITION_LEGREMAINING;
if ((uint8_t)pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_CONTROLSTATE] == STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE) {
ManualControlCommandData cmd;
ManualControlCommandGet(&cmd);
if (cmd.Throttle > AUTOTAKEOFF_THROTTLE_LIMIT_TO_ALLOW_TAKEOFF_START) {
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_CONTROLSTATE] = (float)STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_INITIATE;
PathDesiredSet(&pathDesired);
}
return;
}
}
// check if condition has been met
endCondition = pathConditionCheck();
// decide what to do
@ -245,6 +319,40 @@ static void pathPlannerTask()
}
}
// callback function when waypoints changed in any way, update pathDesired
void updatePathDesired()
{
// only ever touch pathDesired if pathplanner is enabled
if (!pathplanner_active) {
return;
}
// find out current waypoint
WaypointActiveGet(&waypointActive);
WaypointInstGet(waypointActive.Index, &waypoint);
// Capture if current mode is takeoff
bool autotakeoff = (pathAction.Mode == PATHACTION_MODE_AUTOTAKEOFF);
PathActionInstGet(waypoint.Action, &pathAction);
PathDesiredData pathDesired;
switch (pathAction.Mode) {
case PATHACTION_MODE_AUTOTAKEOFF:
planner_setup_pathdesired_takeoff(&pathDesired);
break;
case PATHACTION_MODE_LAND:
planner_setup_pathdesired_land(&pathDesired);
break;
default:
planner_setup_pathdesired(&pathDesired, autotakeoff);
break;
}
PathDesiredSet(&pathDesired);
}
// safety checks for path plan integrity
static uint8_t checkPathPlan()
{
@ -329,35 +437,22 @@ void statusUpdated(__attribute__((unused)) UAVObjEvent *ev)
PIOS_CALLBACKSCHEDULER_Dispatch(pathPlannerHandle);
}
// callback function when waypoints changed in any way, update pathDesired
void updatePathDesired()
// Standard setup of a pathDesired command from the waypoint path plan
static void planner_setup_pathdesired(PathDesiredData *pathDesired, bool overwrite_start_position)
{
// only ever touch pathDesired if pathplanner is enabled
if (!pathplanner_active) {
return;
}
pathDesired->End.North = waypoint.Position.North;
pathDesired->End.East = waypoint.Position.East;
pathDesired->End.Down = waypoint.Position.Down;
pathDesired->EndingVelocity = waypoint.Velocity;
pathDesired->Mode = pathAction.Mode;
pathDesired->ModeParameters[0] = pathAction.ModeParameters[0];
pathDesired->ModeParameters[1] = pathAction.ModeParameters[1];
pathDesired->ModeParameters[2] = pathAction.ModeParameters[2];
pathDesired->ModeParameters[3] = pathAction.ModeParameters[3];
pathDesired->UID = waypointActive.Index;
PathDesiredData pathDesired;
// find out current waypoint
WaypointActiveGet(&waypointActive);
WaypointInstGet(waypointActive.Index, &waypoint);
PathActionInstGet(waypoint.Action, &pathAction);
pathDesired.End.North = waypoint.Position.North;
pathDesired.End.East = waypoint.Position.East;
pathDesired.End.Down = waypoint.Position.Down;
pathDesired.EndingVelocity = waypoint.Velocity;
pathDesired.Mode = pathAction.Mode;
pathDesired.ModeParameters[0] = pathAction.ModeParameters[0];
pathDesired.ModeParameters[1] = pathAction.ModeParameters[1];
pathDesired.ModeParameters[2] = pathAction.ModeParameters[2];
pathDesired.ModeParameters[3] = pathAction.ModeParameters[3];
pathDesired.UID = waypointActive.Index;
if (waypointActive.Index == 0) {
if (waypointActive.Index == 0 || overwrite_start_position) {
PositionStateData positionState;
PositionStateGet(&positionState);
// First waypoint has itself as start point (used to be home position but that proved dangerous when looping)
@ -365,23 +460,90 @@ void updatePathDesired()
/*pathDesired.Start[PATHDESIRED_START_NORTH] = waypoint.Position[WAYPOINT_POSITION_NORTH];
pathDesired.Start[PATHDESIRED_START_EAST] = waypoint.Position[WAYPOINT_POSITION_EAST];
pathDesired.Start[PATHDESIRED_START_DOWN] = waypoint.Position[WAYPOINT_POSITION_DOWN];*/
pathDesired.Start.North = positionState.North;
pathDesired.Start.East = positionState.East;
pathDesired.Start.Down = positionState.Down;
pathDesired.StartingVelocity = pathDesired.EndingVelocity;
// note takeoff relies on the start being the current location as it merely ascends and using
// the start as assumption current NE location
pathDesired->Start.North = positionState.North;
pathDesired->Start.East = positionState.East;
pathDesired->Start.Down = positionState.Down;
pathDesired->StartingVelocity = pathDesired->EndingVelocity;
} else {
// Get previous waypoint as start point
WaypointData waypointPrev;
WaypointInstGet(waypointActive.Index - 1, &waypointPrev);
pathDesired.Start.North = waypointPrev.Position.North;
pathDesired.Start.East = waypointPrev.Position.East;
pathDesired.Start.Down = waypointPrev.Position.Down;
pathDesired.StartingVelocity = waypointPrev.Velocity;
pathDesired->Start.North = waypointPrev.Position.North;
pathDesired->Start.East = waypointPrev.Position.East;
pathDesired->Start.Down = waypointPrev.Position.Down;
pathDesired->StartingVelocity = waypointPrev.Velocity;
}
PathDesiredSet(&pathDesired);
}
#define AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MIN 2.0f
#define AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MAX 50.0f
static void planner_setup_pathdesired_takeoff(PathDesiredData *pathDesired)
{
PositionStateData positionState;
PositionStateGet(&positionState);
float velocity_down;
float autotakeoff_height;
FlightModeSettingsAutoTakeOffVelocityGet(&velocity_down);
FlightModeSettingsAutoTakeOffHeightGet(&autotakeoff_height);
autotakeoff_height = fabsf(autotakeoff_height);
if (autotakeoff_height < AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MIN) {
autotakeoff_height = AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MIN;
} else if (autotakeoff_height > AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MAX) {
autotakeoff_height = AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MAX;
}
pathDesired->Start.North = positionState.North;
pathDesired->Start.East = positionState.East;
pathDesired->Start.Down = positionState.Down;
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_NORTH] = 0.0f;
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_EAST] = 0.0f;
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_DOWN] = -velocity_down;
// initially halt takeoff.
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_CONTROLSTATE] = (float)STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE;
pathDesired->End.North = positionState.North;
pathDesired->End.East = positionState.East;
pathDesired->End.Down = positionState.Down - autotakeoff_height;
pathDesired->StartingVelocity = 0.0f;
pathDesired->EndingVelocity = 0.0f;
pathDesired->Mode = PATHDESIRED_MODE_AUTOTAKEOFF;
pathDesired->UID = waypointActive.Index;
}
static void planner_setup_pathdesired_land(PathDesiredData *pathDesired)
{
PositionStateData positionState;
PositionStateGet(&positionState);
float velocity_down;
FlightModeSettingsLandingVelocityGet(&velocity_down);
pathDesired->Start.North = positionState.North;
pathDesired->Start.East = positionState.East;
pathDesired->Start.Down = positionState.Down;
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_NORTH] = 0.0f;
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_EAST] = 0.0f;
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_DOWN] = velocity_down;
pathDesired->End.North = positionState.North;
pathDesired->End.East = positionState.East;
pathDesired->End.Down = positionState.Down;
pathDesired->StartingVelocity = 0.0f;
pathDesired->EndingVelocity = 0.0f;
pathDesired->Mode = PATHDESIRED_MODE_LAND;
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_OPTIONS] = (float)PATHDESIRED_MODEPARAMETER_LAND_OPTION_HORIZONTAL_PH;
}
// helper function to go to a specific waypoint
static void setWaypoint(uint16_t num)
{
@ -515,18 +677,11 @@ static uint8_t conditionDistanceToTarget()
*/
static uint8_t conditionLegRemaining()
{
PathDesiredData pathDesired;
PositionStateData positionState;
PathStatusData pathStatus;
PathDesiredGet(&pathDesired);
PositionStateGet(&positionState);
PathStatusGet(&pathStatus);
float cur[3] = { positionState.North, positionState.East, positionState.Down };
struct path_status progress;
path_progress(&pathDesired,
cur, &progress);
if (progress.fractional_progress >= 1.0f - pathAction.ConditionParameters[0]) {
if (pathStatus.fractional_progress >= (1.0f - pathAction.ConditionParameters[0])) {
return true;
}
return false;
@ -549,7 +704,7 @@ static uint8_t conditionBelowError()
struct path_status progress;
path_progress(&pathDesired,
cur, &progress);
cur, &progress, mode3D);
if (progress.error <= pathAction.ConditionParameters[0]) {
return true;
}

37
flight/modules/RadioComBridge/RadioComBridge.c
View File

@ -108,8 +108,8 @@ static void radioRxTask(void *parameters);
static void PPMInputTask(void *parameters);
static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length);
static int32_t RadioSendHandler(uint8_t *buf, int32_t length);
static void ProcessTelemetryStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t rxbyte);
static void ProcessRadioStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t rxbyte);
static void ProcessTelemetryStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t *rxbuffer, uint8_t count);
static void ProcessRadioStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t *rxbuffer, uint8_t count);
static void objectPersistenceUpdatedCb(UAVObjEvent *objEv);
static void registerObject(UAVObjHandle obj);
@ -403,14 +403,12 @@ static void radioRxTask(__attribute__((unused)) void *parameters)
PIOS_WDG_UpdateFlag(PIOS_WDG_RADIORX);
#endif
if (PIOS_COM_RADIO) {
uint8_t serial_data[1];
uint8_t serial_data[16];
uint16_t bytes_to_process = PIOS_COM_ReceiveBuffer(PIOS_COM_RADIO, serial_data, sizeof(serial_data), MAX_PORT_DELAY);
if (bytes_to_process > 0) {
if (data->parseUAVTalk) {
// Pass the data through the UAVTalk parser.
for (uint8_t i = 0; i < bytes_to_process; i++) {
ProcessRadioStream(data->radioUAVTalkCon, data->telemUAVTalkCon, serial_data[i]);
}
ProcessRadioStream(data->radioUAVTalkCon, data->telemUAVTalkCon, serial_data, bytes_to_process);
} else if (PIOS_COM_TELEMETRY) {
// Send the data straight to the telemetry port.
// Following call can fail with -2 error code (buffer full) or -3 error code (could not acquire send mutex)
@ -448,12 +446,10 @@ static void telemetryRxTask(__attribute__((unused)) void *parameters)
}
#endif /* PIOS_INCLUDE_USB */
if (inputPort) {
uint8_t serial_data[1];
uint8_t serial_data[16];
uint16_t bytes_to_process = PIOS_COM_ReceiveBuffer(inputPort, serial_data, sizeof(serial_data), MAX_PORT_DELAY);
if (bytes_to_process > 0) {
for (uint8_t i = 0; i < bytes_to_process; i++) {
ProcessTelemetryStream(data->telemUAVTalkCon, data->radioUAVTalkCon, serial_data[i]);
}
ProcessTelemetryStream(data->telemUAVTalkCon, data->radioUAVTalkCon, serial_data, bytes_to_process);
}
} else {
vTaskDelay(5);
@ -597,11 +593,13 @@ static int32_t RadioSendHandler(uint8_t *buf, int32_t length)
* @param[in] outConnectionHandle The UAVTalk connection handle on the radio port.
* @param[in] rxbyte The received byte.
*/
static void ProcessTelemetryStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t rxbyte)
static void ProcessTelemetryStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t *rxbuffer, uint8_t length)
{
// Keep reading until we receive a completed packet.
UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(inConnectionHandle, rxbyte);
uint8_t position = 0;
// Keep reading until we receive a completed packet.
while (position < length) {
UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(inConnectionHandle, rxbuffer, length, &position);
if (state == UAVTALK_STATE_COMPLETE) {
// We only want to unpack certain telemetry objects
uint32_t objId = UAVTalkGetPacketObjId(inConnectionHandle);
@ -636,19 +634,23 @@ static void ProcessTelemetryStream(UAVTalkConnection inConnectionHandle, UAVTalk
}
}
}
}
/**
* @brief Process a byte of data received on the radio data stream.
*
* @param[in] inConnectionHandle The UAVTalk connection handle on the radio port.
* @param[in] outConnectionHandle The UAVTalk connection handle on the telemetry port.
* @param[in] rxbyte The received byte.
* @param[in] rxbuffer The received buffer.
* @param[in] length buffer length
*/
static void ProcessRadioStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t rxbyte)
static void ProcessRadioStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t *rxbuffer, uint8_t length)
{
// Keep reading until we receive a completed packet.
UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(inConnectionHandle, rxbyte);
uint8_t position = 0;
// Keep reading until we receive a completed packet.
while (position < length) {
UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(inConnectionHandle, rxbuffer, length, &position);
if (state == UAVTALK_STATE_COMPLETE) {
// We only want to unpack certain objects from the remote modem
// Similarly we only want to relay certain objects to the telemetry port
@ -678,6 +680,7 @@ static void ProcessRadioStream(UAVTalkConnection inConnectionHandle, UAVTalkConn
}
}
}
}
/**
* @brief Callback that is called when the ObjectPersistence UAVObject is changed.

140
flight/modules/Receiver/receiver.c
View File

@ -37,14 +37,17 @@
#include <manualcontrolsettings.h>
#include <manualcontrolcommand.h>
#include <receiveractivity.h>
#include <receiverstatus.h>
#include <flightstatus.h>
#include <flighttelemetrystats.h>
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
#include <stabilizationsettings.h>
#include <vtolpathfollowersettings.h>
#endif
#include <flightmodesettings.h>
#include <systemsettings.h>
#include <taskinfo.h>
#include <sanitycheck.h>
#if defined(PIOS_INCLUDE_USB_RCTX)
@ -72,6 +75,7 @@
// Private variables
static xTaskHandle taskHandle;
static portTickType lastSysTime;
static FrameType_t frameType = FRAME_TYPE_MULTIROTOR;
#ifdef USE_INPUT_LPF
static portTickType lastSysTimeLPF;
@ -84,6 +88,7 @@ static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutr
static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time);
static bool validInputRange(int16_t min, int16_t max, uint16_t value);
static void applyDeadband(float *value, float deadband);
static void SettingsUpdatedCb(UAVObjEvent *ev);
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
static uint8_t isAssistedFlightMode(uint8_t position);
@ -99,11 +104,16 @@ struct rcvr_activity_fsm {
ManualControlSettingsChannelGroupsOptions group;
uint16_t prev[RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP];
uint8_t sample_count;
uint8_t quality;
};
static struct rcvr_activity_fsm activity_fsm;
static void resetRcvrActivity(struct rcvr_activity_fsm *fsm);
static bool updateRcvrActivity(struct rcvr_activity_fsm *fsm);
static void resetRcvrStatus(struct rcvr_activity_fsm *fsm);
static bool updateRcvrStatus(
struct rcvr_activity_fsm *fsm,
ManualControlSettingsChannelGroupsOptions group);
#define assumptions \
( \
@ -124,6 +134,7 @@ int32_t ReceiverStart()
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL);
#endif
SettingsUpdatedCb(NULL);
return 0;
}
@ -138,16 +149,47 @@ int32_t ReceiverInitialize()
AccessoryDesiredInitialize();
ManualControlCommandInitialize();
ReceiverActivityInitialize();
ReceiverStatusInitialize();
ManualControlSettingsInitialize();
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
StabilizationSettingsInitialize();
VtolPathFollowerSettingsInitialize();
VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
#endif
SystemSettingsInitialize();
SystemSettingsConnectCallback(&SettingsUpdatedCb);
return 0;
}
MODULE_INITCALL(ReceiverInitialize, ReceiverStart);
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
frameType = GetCurrentFrameType();
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
uint8_t TreatCustomCraftAs;
VtolPathFollowerSettingsTreatCustomCraftAsGet(&TreatCustomCraftAs);
if (frameType == FRAME_TYPE_CUSTOM) {
switch (TreatCustomCraftAs) {
case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_FIXEDWING:
frameType = FRAME_TYPE_FIXED_WING;
break;
case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_VTOL:
frameType = FRAME_TYPE_MULTIROTOR;
break;
case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_GROUND:
frameType = FRAME_TYPE_GROUND;
break;
}
}
#endif
}
/**
* Module task
*/
@ -173,6 +215,7 @@ static void receiverTask(__attribute__((unused)) void *parameters)
/* Initialize the RcvrActivty FSM */
portTickType lastActivityTime = xTaskGetTickCount();
resetRcvrActivity(&activity_fsm);
resetRcvrStatus(&activity_fsm);
// Main task loop
lastSysTime = xTaskGetTickCount();
@ -197,9 +240,13 @@ static void receiverTask(__attribute__((unused)) void *parameters)
/* Reset the aging timer because activity was detected */
lastActivityTime = lastSysTime;
}
/* Read signal quality from the group used for the throttle */
(void)updateRcvrStatus(&activity_fsm,
settings.ChannelGroups.Throttle);
}
if (timeDifferenceMs(lastActivityTime, lastSysTime) > 5000) {
resetRcvrActivity(&activity_fsm);
resetRcvrStatus(&activity_fsm);
lastActivityTime = lastSysTime;
}
@ -243,22 +290,20 @@ static void receiverTask(__attribute__((unused)) void *parameters)
}
}
// Check settings, if error raise alarm
if (settings.ChannelGroups.Roll >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| settings.ChannelGroups.Pitch >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| settings.ChannelGroups.Yaw >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
/* Read signal quality from the group used for the throttle */
(void)updateRcvrStatus(&activity_fsm,
settings.ChannelGroups.Throttle);
// Sanity Check Throttle and Yaw
if (settings.ChannelGroups.Yaw >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| settings.ChannelGroups.Throttle >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
||
// Check all channel mappings are valid
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t)PIOS_RCVR_INVALID
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t)PIOS_RCVR_INVALID
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t)PIOS_RCVR_INVALID
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t)PIOS_RCVR_INVALID
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t)PIOS_RCVR_INVALID
||
// Check the driver exists
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t)PIOS_RCVR_NODRIVER
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t)PIOS_RCVR_NODRIVER
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t)PIOS_RCVR_NODRIVER
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t)PIOS_RCVR_NODRIVER
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t)PIOS_RCVR_NODRIVER
||
// Check collective if required
@ -271,7 +316,7 @@ static void receiverTask(__attribute__((unused)) void *parameters)
settings.FlightModeNumber < 1 || settings.FlightModeNumber > FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM
||
// Similar checks for FlightMode channel but only if more than one flight mode has been set. Otherwise don't care
((settings.FlightModeNumber > 1)
((settings.FlightModeNumber > 1) && (frameType != FRAME_TYPE_GROUND)
&& (settings.ChannelGroups.FlightMode >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t)PIOS_RCVR_INVALID
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t)PIOS_RCVR_NODRIVER))) {
@ -282,15 +327,39 @@ static void receiverTask(__attribute__((unused)) void *parameters)
continue;
}
if (frameType != FRAME_TYPE_GROUND) {
// Sanity Check Pitch and Roll
if (settings.ChannelGroups.Roll >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| settings.ChannelGroups.Pitch >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
||
// Check all channel mappings are valid
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t)PIOS_RCVR_INVALID
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t)PIOS_RCVR_INVALID
||
// Check the driver exists
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t)PIOS_RCVR_NODRIVER
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t)PIOS_RCVR_NODRIVER) {
AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_CRITICAL);
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
ManualControlCommandSet(&cmd);
continue;
}
}
// decide if we have valid manual input or not
valid_input_detected &= validInputRange(settings.ChannelMin.Throttle,
settings.ChannelMax.Throttle, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE])
&& validInputRange(settings.ChannelMin.Roll,
settings.ChannelMax.Roll, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL])
&& validInputRange(settings.ChannelMin.Yaw,
settings.ChannelMax.Yaw, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW])
settings.ChannelMax.Yaw, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]);
if (frameType != FRAME_TYPE_GROUND) {
valid_input_detected &= validInputRange(settings.ChannelMin.Roll,
settings.ChannelMax.Roll, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL])
&& validInputRange(settings.ChannelMin.Pitch,
settings.ChannelMax.Pitch, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]);
}
if (settings.ChannelGroups.Collective != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
valid_input_detected &= validInputRange(settings.ChannelMin.Collective,
@ -321,7 +390,11 @@ static void receiverTask(__attribute__((unused)) void *parameters)
}
if (cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) {
if (frameType != FRAME_TYPE_GROUND) {
cmd.Throttle = settings.FailsafeChannel.Throttle;
} else {
cmd.Throttle = 0.0f;
}
cmd.Roll = settings.FailsafeChannel.Roll;
cmd.Pitch = settings.FailsafeChannel.Pitch;
cmd.Yaw = settings.FailsafeChannel.Yaw;
@ -401,6 +474,9 @@ static void receiverTask(__attribute__((unused)) void *parameters)
applyDeadband(&cmd.Roll, deadband_checked);
applyDeadband(&cmd.Pitch, deadband_checked);
applyDeadband(&cmd.Yaw, deadband_checked);
if (frameType == FRAME_TYPE_GROUND) { // assumes reversible motors
applyDeadband(&cmd.Throttle, deadband_checked);
}
}
#ifdef USE_INPUT_LPF
// Apply Low Pass Filter to input channels, time delta between calls in ms
@ -508,6 +584,12 @@ static void resetRcvrActivity(struct rcvr_activity_fsm *fsm)
fsm->sample_count = 0;
}
static void resetRcvrStatus(struct rcvr_activity_fsm *fsm)
{
/* Reset the state */
fsm->quality = 0;
}
static void updateRcvrActivitySample(uint32_t rcvr_id, uint16_t samples[], uint8_t max_channels)
{
for (uint8_t channel = 1; channel <= max_channels; channel++) {
@ -552,6 +634,9 @@ static bool updateRcvrActivityCompare(uint32_t rcvr_id, struct rcvr_activity_fsm
case MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS:
group = RECEIVERACTIVITY_ACTIVEGROUP_SBUS;
break;
case MANUALCONTROLSETTINGS_CHANNELGROUPS_SRXL:
group = RECEIVERACTIVITY_ACTIVEGROUP_SRXL;
break;
case MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS:
group = RECEIVERACTIVITY_ACTIVEGROUP_GCS;
break;
@ -578,6 +663,7 @@ static bool updateRcvrActivity(struct rcvr_activity_fsm *fsm)
if (fsm->group >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
/* We're out of range, reset things */
resetRcvrActivity(fsm);
resetRcvrStatus(fsm);
}
extern uint32_t pios_rcvr_group_map[];
@ -623,6 +709,32 @@ group_completed:
return activity_updated;
}
/* Read signal quality from the specified group */
static bool updateRcvrStatus(
struct rcvr_activity_fsm *fsm,
ManualControlSettingsChannelGroupsOptions group)
{
extern uint32_t pios_rcvr_group_map[];
bool activity_updated = false;
int8_t quality;
quality = PIOS_RCVR_GetQuality(pios_rcvr_group_map[group]);
/* If no driver is detected or any other error then return */
if (quality < 0) {
return activity_updated;
}
/* Compare with previous sample */
if (quality != fsm->quality) {
fsm->quality = quality;
ReceiverStatusQualitySet(&fsm->quality);
activity_updated = true;
}
return activity_updated;
}
/**
* Convert channel from servo pulse duration (microseconds) to scaled -1/+1 range.
*/

23
flight/modules/Sensors/sensors.c
View File

@ -411,9 +411,9 @@ static void handleAccel(float *samples, float temperature)
AccelSensorData accelSensorData;
updateAccelTempBias(temperature);
float accels_out[3] = { samples[0] * agcal.accel_scale.X - agcal.accel_bias.X - accel_temp_bias[0],
samples[1] * agcal.accel_scale.Y - agcal.accel_bias.Y - accel_temp_bias[1],
samples[2] * agcal.accel_scale.Z - agcal.accel_bias.Z - accel_temp_bias[2] };
float accels_out[3] = { (samples[0] - agcal.accel_bias.X) * agcal.accel_scale.X - accel_temp_bias[0],
(samples[1] - agcal.accel_bias.Y) * agcal.accel_scale.Y - accel_temp_bias[1],
(samples[2] - agcal.accel_bias.Z) * agcal.accel_scale.Z - accel_temp_bias[2] };
rot_mult(R, accels_out, samples);
accelSensorData.x = samples[0];
@ -444,8 +444,8 @@ static void handleGyro(float *samples, float temperature)
static void handleMag(float *samples, float temperature)
{
MagSensorData mag;
float mags[3] = { (float)samples[1] - mag_bias[0],
(float)samples[0] - mag_bias[1],
float mags[3] = { (float)samples[0] - mag_bias[0],
(float)samples[1] - mag_bias[1],
(float)samples[2] - mag_bias[2] };
rot_mult(mag_transform, mags, samples);
@ -485,7 +485,10 @@ static void updateAccelTempBias(float temperature)
if ((accel_temp_calibrated) && !accel_temp_calibration_count) {
accel_temp_calibration_count = TEMP_CALIB_INTERVAL;
if (accel_temp_calibrated) {
float ctemp = boundf(accel_temperature, agcal.temp_calibrated_extent.max, agcal.temp_calibrated_extent.min);
float ctemp = boundf(accel_temperature,
agcal.temp_calibrated_extent.max,
agcal.temp_calibrated_extent.min);
accel_temp_bias[0] = agcal.accel_temp_coeff.X * ctemp;
accel_temp_bias[1] = agcal.accel_temp_coeff.Y * ctemp;
accel_temp_bias[2] = agcal.accel_temp_coeff.Z * ctemp;
@ -588,8 +591,12 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
RevoSettingsBaroTempCorrectionPolynomialGet(&baroCorrection);
RevoSettingsBaroTempCorrectionExtentGet(&baroCorrectionExtent);
baro_temp_correction_enabled = !(baroCorrectionExtent.max - baroCorrectionExtent.min < 0.1f ||
(baroCorrection.a < 1e-9f && baroCorrection.b < 1e-9f && baroCorrection.c < 1e-9f && baroCorrection.d < 1e-9f));
baro_temp_correction_enabled =
(baroCorrectionExtent.max - baroCorrectionExtent.min > 0.1f &&
(fabsf(baroCorrection.a) > 1e-9f ||
fabsf(baroCorrection.b) > 1e-9f ||
fabsf(baroCorrection.c) > 1e-9f ||
fabsf(baroCorrection.d) > 1e-9f));
}
/**
* @}

198
flight/modules/Stabilization/altitudeloop.c → flight/modules/Stabilization/altitudeloop.cpp
View File

@ -1,7 +1,7 @@
/**
******************************************************************************
*
* @file altitudeloop.c
* @file altitudeloop.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief This module compared @ref PositionActuatl to @ref ActiveWaypoint
* and sets @ref AttitudeDesired. It only does this when the FlightMode field
@ -26,6 +26,7 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
extern "C" {
#include <openpilot.h>
#include <callbackinfo.h>
@ -37,6 +38,12 @@
#include <altitudeholdstatus.h>
#include <velocitystate.h>
#include <positionstate.h>
#include <vtolselftuningstats.h>
#include <stabilization.h>
}
#include <pidcontroldown.h>
// Private constants
@ -50,78 +57,134 @@
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW
#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL
#define STACK_SIZE_BYTES 512
// Private types
// Private variables
static DelayedCallbackInfo *altitudeHoldCBInfo;
static PIDControlDown controlDown;
static AltitudeHoldSettingsData altitudeHoldSettings;
static struct pid pid0, pid1;
static ThrustModeType thrustMode;
static PiOSDeltatimeConfig timeval;
static float thrustSetpoint = 0.0f;
static float thrustDemand = 0.0f;
static float startThrust = 0.5f;
// Private functions
static void altitudeHoldTask(void);
static void SettingsUpdatedCb(UAVObjEvent *ev);
static void altitudeHoldTask(void);
static void VelocityStateUpdatedCb(UAVObjEvent *ev);
/**
* Setup mode and setpoint
*
* reinit: true when althold/vario mode selected over a previous alternate thrust mode
*/
float stabilizationAltitudeHold(float setpoint, ThrustModeType mode, bool reinit)
{
static bool newaltitude = true;
if (reinit) {
startThrust = setpoint;
pid_zero(&pid0);
pid_zero(&pid1);
if (reinit || !controlDown.IsActive()) {
controlDown.Activate();
newaltitude = true;
// calculate a thrustDemand on reinit only
altitudeHoldTask();
}
const float DEADBAND = 0.20f;
const float DEADBAND_HIGH = 1.0f / 2 + DEADBAND / 2;
const float DEADBAND_LOW = 1.0f / 2 - DEADBAND / 2;
// this is the max speed in m/s at the extents of thrust
float thrustRate;
uint8_t thrustExp;
AltitudeHoldSettingsThrustExpGet(&thrustExp);
AltitudeHoldSettingsThrustRateGet(&thrustRate);
PositionStateData posState;
PositionStateGet(&posState);
if (altitudeHoldSettings.CutThrustWhenZero && setpoint <= 0) {
// Cut thrust if desired
thrustSetpoint = 0.0f;
controlDown.UpdateVelocitySetpoint(0.0f);
thrustDemand = 0.0f;
thrustMode = DIRECT;
newaltitude = true;
return thrustDemand;
} else if (mode == ALTITUDEVARIO && setpoint > DEADBAND_HIGH) {
// being the two band symmetrical I can divide by DEADBAND_LOW to scale it to a value betweeon 0 and 1
// then apply an "exp" f(x,k) = (k*x*x*x + (255-k)*x) / 255
thrustSetpoint = -((thrustExp * powf((setpoint - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - thrustExp) * (setpoint - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * thrustRate);
controlDown.UpdateVelocitySetpoint(-((altitudeHoldSettings.ThrustExp * powf((setpoint - DEADBAND_HIGH) / (DEADBAND_LOW), 3.0f) + (255.0f - altitudeHoldSettings.ThrustExp) * (setpoint - DEADBAND_HIGH) / DEADBAND_LOW) / 255.0f * altitudeHoldSettings.ThrustRate));
thrustMode = ALTITUDEVARIO;
newaltitude = true;
} else if (mode == ALTITUDEVARIO && setpoint < DEADBAND_LOW) {
thrustSetpoint = -(-(thrustExp * powf((DEADBAND_LOW - (setpoint < 0 ? 0 : setpoint)) / DEADBAND_LOW, 3) + (255 - thrustExp) * (DEADBAND_LOW - setpoint) / DEADBAND_LOW) / 255 * thrustRate);
controlDown.UpdateVelocitySetpoint(-(-(altitudeHoldSettings.ThrustExp * powf((DEADBAND_LOW - (setpoint < 0 ? 0 : setpoint)) / DEADBAND_LOW, 3.0f) + (255.0f - altitudeHoldSettings.ThrustExp) * (DEADBAND_LOW - setpoint) / DEADBAND_LOW) / 255.0f * altitudeHoldSettings.ThrustRate));
thrustMode = ALTITUDEVARIO;
newaltitude = true;
} else if (newaltitude == true) {
thrustSetpoint = posState.Down;
controlDown.UpdateVelocitySetpoint(0.0f);
PositionStateData posState;
PositionStateGet(&posState);
controlDown.UpdatePositionSetpoint(posState.Down);
thrustMode = ALTITUDEHOLD;
newaltitude = false;
}
thrustDemand = boundf(thrustDemand, altitudeHoldSettings.ThrustLimits.Min, altitudeHoldSettings.ThrustLimits.Max);
return thrustDemand;
}
/**
* Disable the alt hold task loop velocity controller to save cpu cycles
*/
void stabilizationDisableAltitudeHold(void)
{
controlDown.Deactivate();
}
/**
* Module thread, should not return.
*/
static void altitudeHoldTask(void)
{
if (!controlDown.IsActive()) {
return;
}
AltitudeHoldStatusData altitudeHoldStatus;
AltitudeHoldStatusGet(&altitudeHoldStatus);
float velocityStateDown;
VelocityStateDownGet(&velocityStateDown);
controlDown.UpdateVelocityState(velocityStateDown);
float local_thrustDemand = 0.0f;
switch (thrustMode) {
case ALTITUDEHOLD:
{
float positionStateDown;
PositionStateDownGet(&positionStateDown);
controlDown.UpdatePositionState(positionStateDown);
controlDown.ControlPosition();
altitudeHoldStatus.VelocityDesired = controlDown.GetVelocityDesired();
altitudeHoldStatus.State = ALTITUDEHOLDSTATUS_STATE_ALTITUDEHOLD;
local_thrustDemand = controlDown.GetDownCommand();
}
break;
case ALTITUDEVARIO:
altitudeHoldStatus.VelocityDesired = controlDown.GetVelocityDesired();
altitudeHoldStatus.State = ALTITUDEHOLDSTATUS_STATE_ALTITUDEVARIO;
local_thrustDemand = controlDown.GetDownCommand();
break;
case DIRECT:
altitudeHoldStatus.VelocityDesired = 0.0f;
altitudeHoldStatus.State = ALTITUDEHOLDSTATUS_STATE_DIRECT;
break;
}
AltitudeHoldStatusSet(&altitudeHoldStatus);
thrustDemand = local_thrustDemand;
}
static void VelocityStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
PIOS_CALLBACKSCHEDULER_Dispatch(altitudeHoldCBInfo);
}
/**
* Initialise the module, called on startup
*/
@ -131,82 +194,39 @@ void stabilizationAltitudeloopInit()
AltitudeHoldStatusInitialize();
PositionStateInitialize();
VelocityStateInitialize();
VtolSelfTuningStatsInitialize();
PIOS_DELTATIME_Init(&timeval, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
// Create object queue
AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb);
VtolSelfTuningStatsConnectCallback(&SettingsUpdatedCb);
SettingsUpdatedCb(NULL);
altitudeHoldCBInfo = PIOS_CALLBACKSCHEDULER_Create(&altitudeHoldTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_ALTITUDEHOLD, STACK_SIZE_BYTES);
AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb);
VelocityStateConnectCallback(&VelocityStateUpdatedCb);
// Start main task
SettingsUpdatedCb(NULL);
}
/**
* Module thread, should not return.
*/
static void altitudeHoldTask(void)
{
AltitudeHoldStatusData altitudeHoldStatus;
AltitudeHoldStatusGet(&altitudeHoldStatus);
// do the actual control loop(s)
float positionStateDown;
PositionStateDownGet(&positionStateDown);
float velocityStateDown;
VelocityStateDownGet(&velocityStateDown);
float dT;
dT = PIOS_DELTATIME_GetAverageSeconds(&timeval);
switch (thrustMode) {
case ALTITUDEHOLD:
{
// altitude control loop
// No scaling.
const pid_scaler scaler = { .p = 1.0f, .i = 1.0f, .d = 1.0f };
altitudeHoldStatus.VelocityDesired = pid_apply_setpoint(&pid0, &scaler, thrustSetpoint, positionStateDown, dT);
}
break;
case ALTITUDEVARIO:
altitudeHoldStatus.VelocityDesired = thrustSetpoint;
break;
default:
altitudeHoldStatus.VelocityDesired = 0;
break;
}
AltitudeHoldStatusSet(&altitudeHoldStatus);
switch (thrustMode) {
case DIRECT:
thrustDemand = thrustSetpoint;
break;
default:
{
// velocity control loop
// No scaling.
const pid_scaler scaler = { .p = 1.0f, .i = 1.0f, .d = 1.0f };
thrustDemand = startThrust - pid_apply_setpoint(&pid1, &scaler, altitudeHoldStatus.VelocityDesired, velocityStateDown, dT);
}
break;
}
}
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
AltitudeHoldSettingsGet(&altitudeHoldSettings);
pid_configure(&pid0, altitudeHoldSettings.AltitudePI.Kp, altitudeHoldSettings.AltitudePI.Ki, 0, altitudeHoldSettings.AltitudePI.Ilimit);
pid_zero(&pid0);
pid_configure(&pid1, altitudeHoldSettings.VelocityPI.Kp, altitudeHoldSettings.VelocityPI.Ki, 0, altitudeHoldSettings.VelocityPI.Ilimit);
pid_zero(&pid1);
}
static void VelocityStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
PIOS_CALLBACKSCHEDULER_Dispatch(altitudeHoldCBInfo);
controlDown.UpdateParameters(altitudeHoldSettings.VerticalVelPID.Kp,
altitudeHoldSettings.VerticalVelPID.Ki,
altitudeHoldSettings.VerticalVelPID.Kd,
altitudeHoldSettings.VerticalVelPID.Beta,
(float)(UPDATE_EXPECTED),
altitudeHoldSettings.ThrustRate);
controlDown.UpdatePositionalParameters(altitudeHoldSettings.VerticalPosP);
VtolSelfTuningStatsData vtolSelfTuningStats;
VtolSelfTuningStatsGet(&vtolSelfTuningStats);
controlDown.UpdateNeutralThrust(vtolSelfTuningStats.NeutralThrustOffset + altitudeHoldSettings.ThrustLimits.Neutral);
// initialise limits on thrust but note the FSM can override.
controlDown.SetThrustLimits(altitudeHoldSettings.ThrustLimits.Min, altitudeHoldSettings.ThrustLimits.Max);
// disable neutral thrust calcs which should only be done in a hold mode.
controlDown.DisableNeutralThrustCalc();
}

1
flight/modules/Stabilization/inc/altitudeloop.h
View File

@ -37,5 +37,6 @@ typedef enum { ALTITUDEHOLD = 0, ALTITUDEVARIO = 1, DIRECT = 2 } ThrustModeType;
void stabilizationAltitudeloopInit();
float stabilizationAltitudeHold(float setpoint, ThrustModeType mode, bool reinit);
void stabilizationDisableAltitudeHold(void);
#endif /* ALTITUDELOOP_H */

28
flight/modules/Stabilization/innerloop.c
View File

@ -49,7 +49,7 @@
#include <stabilization.h>
#include <virtualflybar.h>
#include <cruisecontrol.h>
#include <sanitycheck.h>
// Private constants
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_CRITICAL
@ -66,6 +66,7 @@ static float axis_lock_accum[3] = { 0, 0, 0 };
static uint8_t previous_mode[AXES] = { 255, 255, 255, 255 };
static PiOSDeltatimeConfig timeval;
static float speedScaleFactor = 1.0f;
static bool frame_is_multirotor;
// Private functions
static void stabilizationInnerloopTask();
@ -95,6 +96,8 @@ void stabilizationInnerloopInit()
// schedule dead calls every FAILSAFE_TIMEOUT_MS to have the watchdog cleared
PIOS_CALLBACKSCHEDULER_Schedule(callbackHandle, FAILSAFE_TIMEOUT_MS, CALLBACK_UPDATEMODE_LATER);
frame_is_multirotor = (GetCurrentFrameType() == FRAME_TYPE_MULTIROTOR);
}
static float get_pid_scale_source_value()
@ -239,8 +242,13 @@ static void stabilizationInnerloopTask()
float *actuatorDesiredAxis = &actuator.Roll;
int t;
float dT;
bool multirotor = (GetCurrentFrameType() == FRAME_TYPE_MULTIROTOR); // check if frame is a multirotor
dT = PIOS_DELTATIME_GetAverageSeconds(&timeval);
StabilizationStatusOuterLoopData outerLoop;
StabilizationStatusOuterLoopGet(&outerLoop);
bool allowPiroComp = true;
for (t = 0; t < AXES; t++) {
bool reinit = (StabilizationStatusInnerLoopToArray(enabled)[t] != previous_mode[t]);
previous_mode[t] = StabilizationStatusInnerLoopToArray(enabled)[t];
@ -248,6 +256,15 @@ static void stabilizationInnerloopTask()
if (t < STABILIZATIONSTATUS_INNERLOOP_THRUST) {
if (reinit) {
stabSettings.innerPids[t].iAccumulator = 0;
if (frame_is_multirotor) {
// Multirotors should dump axis lock accumulators when unarmed or throttle is low.
// Fixed wing or ground vehicles can fly/drive with low throttle.
axis_lock_accum[t] = 0;
}
}
// Any self leveling on roll or pitch must prevent pirouette compensation
if (t < STABILIZATIONSTATUS_INNERLOOP_YAW && StabilizationStatusOuterLoopToArray(outerLoop)[t] != STABILIZATIONSTATUS_OUTERLOOP_DIRECT) {
allowPiroComp = false;
}
switch (StabilizationStatusInnerLoopToArray(enabled)[t]) {
case STABILIZATIONSTATUS_INNERLOOP_VIRTUALFLYBAR:
@ -308,8 +325,13 @@ static void stabilizationInnerloopTask()
}
}
if (!multirotor) {
// we only need to clamp the desired axis to a sane range if the frame is not a multirotor type
// we don't want to do any clamping until after the motors are calculated and scaled.
// need to figure out what to do with a tricopter tail servo.
actuatorDesiredAxis[t] = boundf(actuatorDesiredAxis[t], -1.0f, 1.0f);
}
}
actuator.UpdateTime = dT * 1000;
@ -322,7 +344,7 @@ static void stabilizationInnerloopTask()
}
}
if (stabSettings.stabBank.EnablePiroComp == STABILIZATIONBANK_ENABLEPIROCOMP_TRUE && stabSettings.innerPids[0].iLim > 1e-3f && stabSettings.innerPids[1].iLim > 1e-3f) {
if (allowPiroComp && stabSettings.stabBank.EnablePiroComp == STABILIZATIONBANK_ENABLEPIROCOMP_TRUE && stabSettings.innerPids[0].iLim > 1e-3f && stabSettings.innerPids[1].iLim > 1e-3f) {
// attempted piro compensation - rotate pitch and yaw integrals (experimental)
float angleYaw = DEG2RAD(gyro_filtered[2] * dT);
float sinYaw = sinf(angleYaw);
@ -334,7 +356,7 @@ static void stabilizationInnerloopTask()
}
{
uint8_t armed;
FlightStatusArmedOptions armed;
FlightStatusArmedGet(&armed);
float throttleDesired;
ManualControlCommandThrottleGet(&throttleDesired);

86
flight/modules/Stabilization/outerloop.c
View File

@ -105,6 +105,32 @@ static void stabilizationOuterloopTask()
int t;
float dT = PIOS_DELTATIME_GetAverageSeconds(&timeval);
bool reinit = (StabilizationStatusOuterLoopToArray(enabled)[STABILIZATIONSTATUS_OUTERLOOP_THRUST] != previous_mode[STABILIZATIONSTATUS_OUTERLOOP_THRUST]);
previous_mode[STABILIZATIONSTATUS_OUTERLOOP_THRUST] = StabilizationStatusOuterLoopToArray(enabled)[STABILIZATIONSTATUS_OUTERLOOP_THRUST];
#ifdef REVOLUTION
if (reinit && (previous_mode[STABILIZATIONSTATUS_OUTERLOOP_THRUST] == STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE ||
previous_mode[STABILIZATIONSTATUS_OUTERLOOP_THRUST] == STABILIZATIONSTATUS_OUTERLOOP_ALTITUDEVARIO)) {
// disable the altvario velocity control loop
stabilizationDisableAltitudeHold();
}
#endif /* REVOLUTION */
switch (StabilizationStatusOuterLoopToArray(enabled)[STABILIZATIONSTATUS_OUTERLOOP_THRUST]) {
#ifdef REVOLUTION
case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE:
rateDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST] = stabilizationAltitudeHold(stabilizationDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST], ALTITUDEHOLD, reinit);
break;
case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDEVARIO:
rateDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST] = stabilizationAltitudeHold(stabilizationDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST], ALTITUDEVARIO, reinit);
break;
#endif /* REVOLUTION */
case STABILIZATIONSTATUS_OUTERLOOP_DIRECT:
case STABILIZATIONSTATUS_OUTERLOOP_DIRECTWITHLIMITS:
default:
rateDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST] = stabilizationDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST];
break;
}
float local_error[3];
{
#if defined(PIOS_QUATERNION_STABILIZATION)
@ -120,6 +146,7 @@ static void stabilizationOuterloopTask()
case STABILIZATIONSTATUS_OUTERLOOP_WEAKLEVELING:
rpy_desired[t] = stabilizationDesiredAxis[t];
break;
case STABILIZATIONSTATUS_OUTERLOOP_DIRECTWITHLIMITS:
case STABILIZATIONSTATUS_OUTERLOOP_DIRECT:
default:
rpy_desired[t] = ((float *)&attitudeState.Roll)[t];
@ -148,11 +175,12 @@ static void stabilizationOuterloopTask()
}
#endif /* if defined(PIOS_QUATERNION_STABILIZATION) */
}
for (t = 0; t < AXES; t++) {
bool reinit = (StabilizationStatusOuterLoopToArray(enabled)[t] != previous_mode[t]);
for (t = STABILIZATIONSTATUS_OUTERLOOP_ROLL; t < STABILIZATIONSTATUS_OUTERLOOP_THRUST; t++) {
reinit = (StabilizationStatusOuterLoopToArray(enabled)[t] != previous_mode[t]);
previous_mode[t] = StabilizationStatusOuterLoopToArray(enabled)[t];
if (t < STABILIZATIONSTATUS_OUTERLOOP_THRUST) {
if (reinit) {
stabSettings.outerPids[t].iAccumulator = 0;
}
@ -239,32 +267,50 @@ static void stabilizationOuterloopTask()
rateDesiredAxis[t] = rate_input + weak_leveling;
}
break;
case STABILIZATIONSTATUS_OUTERLOOP_DIRECTWITHLIMITS:
rateDesiredAxis[t] = stabilizationDesiredAxis[t]; // default for all axes
// now test limits for pitch and/or roll
if (t == 1) { // pitch
if (attitudeState.Pitch < -stabSettings.stabBank.PitchMax) {
// attitude exceeds pitch max.
// zero rate desired if also -ve
if (rateDesiredAxis[t] < 0.0f) {
rateDesiredAxis[t] = 0.0f;
}
} else if (attitudeState.Pitch > stabSettings.stabBank.PitchMax) {
// attitude exceeds pitch max
// zero rate desired if also +ve
if (rateDesiredAxis[t] > 0.0f) {
rateDesiredAxis[t] = 0.0f;
}
}
} else if (t == 0) { // roll
if (attitudeState.Roll < -stabSettings.stabBank.RollMax) {
// attitude exceeds roll max.
// zero rate desired if also -ve
if (rateDesiredAxis[t] < 0.0f) {
rateDesiredAxis[t] = 0.0f;
}
} else if (attitudeState.Roll > stabSettings.stabBank.RollMax) {
// attitude exceeds roll max
// zero rate desired if also +ve
if (rateDesiredAxis[t] > 0.0f) {
rateDesiredAxis[t] = 0.0f;
}
}
}
break;
case STABILIZATIONSTATUS_OUTERLOOP_DIRECT:
default:
rateDesiredAxis[t] = stabilizationDesiredAxis[t];
break;
}
} else {
switch (StabilizationStatusOuterLoopToArray(enabled)[t]) {
#ifdef REVOLUTION
case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE:
rateDesiredAxis[t] = stabilizationAltitudeHold(stabilizationDesiredAxis[t], ALTITUDEHOLD, reinit);
break;
case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDEVARIO:
rateDesiredAxis[t] = stabilizationAltitudeHold(stabilizationDesiredAxis[t], ALTITUDEVARIO, reinit);
break;
#endif /* REVOLUTION */
case STABILIZATIONSTATUS_OUTERLOOP_DIRECT:
default:
rateDesiredAxis[t] = stabilizationDesiredAxis[t];
break;
}
}
}
RateDesiredSet(&rateDesired);
{
uint8_t armed;
FlightStatusArmedOptions armed;
FlightStatusArmedGet(&armed);
float throttleDesired;
ManualControlCommandThrottleGet(&throttleDesired);

4
flight/modules/Stabilization/stabilization.c
View File

@ -137,6 +137,10 @@ static void StabilizationDesiredUpdatedCb(__attribute__((unused)) UAVObjEvent *e
StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATETRAINER:
StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECTWITHLIMITS;
StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE;
StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;

23
flight/modules/StateEstimation/filterstationary.c
View File

@ -30,13 +30,17 @@
*/
#include "inc/stateestimation.h"
// Fake pos rate in uS
#define FILTERSTATIONARY_FAKEPOSRATE 100000
// Private constants
#define STACK_REQUIRED 64
// Private types
// Private types
struct data {
uint32_t lastUpdate;
};
// Private variables
// Private functions
@ -49,17 +53,24 @@ int32_t filterStationaryInitialize(stateFilter *handle)
{
handle->init = &init;
handle->filter = &filter;
handle->localdata = NULL;
handle->localdata = pios_malloc(sizeof(struct data));
return STACK_REQUIRED;
}
static int32_t init(__attribute__((unused)) stateFilter *self)
static int32_t init(stateFilter *self)
{
struct data *this = (struct data *)self->localdata;
this->lastUpdate = PIOS_DELAY_GetRaw();
return 0;
}
static filterResult filter(__attribute__((unused)) stateFilter *self, stateEstimation *state)
static filterResult filter(stateFilter *self, stateEstimation *state)
{
struct data *this = (struct data *)self->localdata;
if (PIOS_DELAY_DiffuS(this->lastUpdate) > FILTERSTATIONARY_FAKEPOSRATE) {
this->lastUpdate = PIOS_DELAY_GetRaw();
state->pos[0] = 0.0f;
state->pos[1] = 0.0f;
state->pos[2] = 0.0f;
@ -69,7 +80,7 @@ static filterResult filter(__attribute__((unused)) stateFilter *self, stateEstim
state->vel[1] = 0.0f;
state->vel[2] = 0.0f;
state->updated |= SENSORUPDATES_vel;
}
return FILTERRESULT_OK;
}

27
flight/modules/StateEstimation/stateestimation.c
View File

@ -345,7 +345,6 @@ MODULE_INITCALL(StateEstimationInitialize, StateEstimationStart);
*/
static void StateEstimationCb(void)
{
static enum { RUNSTATE_LOAD = 0, RUNSTATE_FILTER = 1, RUNSTATE_SAVE = 2 } runState = RUNSTATE_LOAD;
static filterResult alarm = FILTERRESULT_OK;
static filterResult lastAlarm = FILTERRESULT_UNINITIALISED;
static uint16_t alarmcounter = 0;
@ -361,9 +360,6 @@ static void StateEstimationCb(void)
return;
}
switch (runState) {
case RUNSTATE_LOAD:
alarm = FILTERRESULT_OK;
// set alarm to warning if called through timeout
@ -381,7 +377,7 @@ static void StateEstimationCb(void)
FlightStatusGet(&fs);
if (fs.Armed == FLIGHTSTATUS_ARMED_DISARMED || fusionAlgorithm == FILTER_INIT_FORCE) {
const filterPipeline *newFilterChain;
switch (revoSettings.FusionAlgorithm) {
switch ((RevoSettingsFusionAlgorithmOptions)revoSettings.FusionAlgorithm) {
case REVOSETTINGS_FUSIONALGORITHM_BASICCOMPLEMENTARY:
newFilterChain = cfQueue;
break;
@ -448,13 +444,8 @@ static void StateEstimationCb(void)
current = filterChain;
// we are not done, re-dispatch self execution
runState = RUNSTATE_FILTER;
PIOS_CALLBACKSCHEDULER_Dispatch(stateEstimationCallback);
break;
case RUNSTATE_FILTER:
if (current != NULL) {
while (current) {
filterResult result = current->filter->filter((stateFilter *)current->filter, &states);
if (result > alarm) {
alarm = result;
@ -462,15 +453,6 @@ static void StateEstimationCb(void)
current = current->next;
}
// we are not done, re-dispatch self execution
if (!current) {
runState = RUNSTATE_SAVE;
}
PIOS_CALLBACKSCHEDULER_Dispatch(stateEstimationCallback);
break;
case RUNSTATE_SAVE:
// the final output of filters is saved in state variables
// EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(GyroState, gyro, x, y, z) // replaced by performance shortcut
if (IS_SET(states.updated, SENSORUPDATES_gyro)) {
@ -513,7 +495,6 @@ static void StateEstimationCb(void)
Quaternion2RPY(&s.q1, &s.Roll);
AttitudeStateSet(&s);
}
// throttle alarms, raise alarm flags immediately
// but require system to run for a while before decreasing
// to prevent alarm flapping
@ -540,15 +521,11 @@ static void StateEstimationCb(void)
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
}
// we are done, re-schedule next self execution
runState = RUNSTATE_LOAD;
if (updatedSensors) {
PIOS_CALLBACKSCHEDULER_Dispatch(stateEstimationCallback);
} else {
PIOS_CALLBACKSCHEDULER_Schedule(stateEstimationCallback, TIMEOUT_MS, CALLBACK_UPDATEMODE_SOONER);
}
break;
}
}

2
flight/modules/System/systemmod.c
View File

@ -101,7 +101,9 @@ static enum { STACKOVERFLOW_NONE = 0, STACKOVERFLOW_WARNING = 1, STACKOVERFLOW_C
static bool mallocFailed;
static HwSettingsData bootHwSettings;
static FrameType_t bootFrameType;
#if !defined(ARCH_POSIX) && !defined(ARCH_WIN32)
static struct PIOS_FLASHFS_Stats fsStats;
#endif
// Private functions
static void objectUpdatedCb(UAVObjEvent *ev);

553
flight/modules/Telemetry/telemetry.c
View File

@ -9,7 +9,7 @@
* @{
*
* @file telemetry.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Telemetry module, handles telemetry and UAVObject updates
* @see The GNU Public License (GPL) Version 3
*
@ -30,6 +30,34 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
/* Telemetry uses four tasks. Two are created for the main telemetry
* stream called "TelTx" and "TelRx". Two are created to handle the OPLink
* radio connection, called "RadioTx" and "RadioRx", the latter being
* overridden by USB if connected.
*
* The following code uses a "local" prefix to refer to the telemetry channel
* associated with a port on the FC and a "radio" prefix to refer to the
* telemetry channel associated with the OPLink/USB.
*
* The "local" telemetry port to use is defined by PIOS_COM_TELEM_RF in
* PIOS_Board_Init().
*
* A UAVTalk connection instance, telemUavTalkCon, is associated with the
* "local" channel and another, radioUavTalkCon, with the "radio" channel.
* Associated with each instance is a transmit routine which will send data
* to the appropriate port.
*
* Data is passed on the telemetry channels using queues. If
* PIOS_TELEM_PRIORITY_QUEUE is defined then two queues are created, one normal
* priority and the other high priority.
*
* The "Tx" tasks read events first from the priority queue and then from
* the normal queue, passing each event to processObjEvent() which ultimately
* passes each event to the UAVTalk library which results in the appropriate
* transmit routine being called to send the data back to the recipient on
* the "local" or "radio" link.
*/
#include <openpilot.h>
#include "telemetry.h"
@ -53,62 +81,74 @@
#ifdef PIOS_TELEM_RADIO_RX_STACK_SIZE
#define STACK_SIZE_RADIO_RX_BYTES PIOS_TELEM_RADIO_RX_STACK_SIZE
#define STACK_SIZE_RADIO_TX_BYTES PIOS_TELEM_RADIO_TX_STACK_SIZE
#else
#define STACK_SIZE_RADIO_RX_BYTES STACK_SIZE_RX_BYTES
#define STACK_SIZE_RADIO_TX_BYTES STACK_SIZE_TX_BYTES
#endif
#define TASK_PRIORITY_RX (tskIDLE_PRIORITY + 2)
#define TASK_PRIORITY_TX (tskIDLE_PRIORITY + 2)
#define TASK_PRIORITY_RADRX (tskIDLE_PRIORITY + 2)
#define TASK_PRIORITY_RADTX (tskIDLE_PRIORITY + 2)
#define REQ_TIMEOUT_MS 250
#define MAX_RETRIES 2
#define STATS_UPDATE_PERIOD_MS 4000
#define CONNECTION_TIMEOUT_MS 8000
// Private types
typedef struct {
// Determine port on which to communicate telemetry information
uint32_t (*getPort)();
// Main telemetry queue
xQueueHandle queue;
#ifdef PIOS_TELEM_PRIORITY_QUEUE
// Priority telemetry queue
xQueueHandle priorityQueue;
#endif /* PIOS_TELEM_PRIORITY_QUEUE */
// Transmit/receive task handles
xTaskHandle txTaskHandle;
xTaskHandle rxTaskHandle;
// Telemetry stream
UAVTalkConnection uavTalkCon;
} channelContext;
// Private variables
static uint32_t telemetryPort;
#ifdef PIOS_INCLUDE_RFM22B
static uint32_t radioPort;
#endif
static xQueueHandle queue;
// Main telemetry channel
static channelContext localChannel;
static int32_t transmitLocalData(uint8_t *data, int32_t length);
static void registerLocalObject(UAVObjHandle obj);
static uint32_t localPort();
#if defined(PIOS_TELEM_PRIORITY_QUEUE)
static xQueueHandle priorityQueue;
#else
#define priorityQueue queue
#endif
// OPLink telemetry channel
static channelContext radioChannel;
static int32_t transmitRadioData(uint8_t *data, int32_t length);
static void registerRadioObject(UAVObjHandle obj);
static uint32_t radioPort();
static xTaskHandle telemetryTxTaskHandle;
static xTaskHandle telemetryRxTaskHandle;
#ifdef PIOS_INCLUDE_RFM22B
static xTaskHandle radioRxTaskHandle;
#endif
// Telemetry stats
static uint32_t txErrors;
static uint32_t txRetries;
static uint32_t timeOfLastObjectUpdate;
static UAVTalkConnection uavTalkCon;
#ifdef PIOS_INCLUDE_RFM22B
static UAVTalkConnection radioUavTalkCon;
#endif
// Private functions
static void telemetryTxTask(void *parameters);
static void telemetryRxTask(void *parameters);
#ifdef PIOS_INCLUDE_RFM22B
static void radioRxTask(void *parameters);
static int32_t transmitRadioData(uint8_t *data, int32_t length);
#endif
static int32_t transmitData(uint8_t *data, int32_t length);
static void registerObject(UAVObjHandle obj);
static void updateObject(UAVObjHandle obj, int32_t eventType);
static int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs);
static int32_t setLoggingPeriod(UAVObjHandle obj, int32_t updatePeriodMs);
static void processObjEvent(UAVObjEvent *ev);
static void updateObject(
channelContext *channel,
UAVObjHandle obj,
int32_t eventType);
static void processObjEvent(
channelContext *channel,
UAVObjEvent *ev);
static int32_t setUpdatePeriod(
channelContext *channel,
UAVObjHandle obj,
int32_t updatePeriodMs);
static int32_t setLoggingPeriod(
channelContext *channel,
UAVObjHandle obj,
int32_t updatePeriodMs);
static void updateTelemetryStats();
static void gcsTelemetryStatsUpdated();
static void updateSettings();
static uint32_t getComPort(bool input);
static void updateSettings(channelContext *channel);
/**
* Initialise the telemetry module
@ -117,26 +157,94 @@ static uint32_t getComPort(bool input);
*/
int32_t TelemetryStart(void)
{
// Process all registered objects and connect queue for updates
UAVObjIterate(&registerObject);
// Only start the local telemetry tasks if needed
if (localPort()) {
UAVObjIterate(&registerLocalObject);
// Listen to objects of interest
GCSTelemetryStatsConnectQueue(priorityQueue);
#ifdef PIOS_TELEM_PRIORITY_QUEUE
GCSTelemetryStatsConnectQueue(localChannel.priorityQueue);
#else /* PIOS_TELEM_PRIORITY_QUEUE */
GCSTelemetryStatsConnectQueue(localChannel.queue);
#endif /* PIOS_TELEM_PRIORITY_QUEUE */
// Start telemetry tasks
xTaskCreate(telemetryTxTask, "TelTx", STACK_SIZE_TX_BYTES / 4, NULL, TASK_PRIORITY_TX, &telemetryTxTaskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYTX, telemetryTxTaskHandle);
xTaskCreate(telemetryRxTask, "TelRx", STACK_SIZE_RX_BYTES / 4, NULL, TASK_PRIORITY_RX, &telemetryRxTaskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYRX, telemetryRxTaskHandle);
xTaskCreate(telemetryTxTask,
"TelTx",
STACK_SIZE_TX_BYTES / 4,
&localChannel,
TASK_PRIORITY_TX,
&localChannel.txTaskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYTX,
localChannel.txTaskHandle);
xTaskCreate(telemetryRxTask,
"TelRx",
STACK_SIZE_RX_BYTES / 4,
&localChannel,
TASK_PRIORITY_RX,
&localChannel.rxTaskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYRX,
localChannel.rxTaskHandle);
}
#ifdef PIOS_INCLUDE_RFM22B
xTaskCreate(radioRxTask, "RadioRx", STACK_SIZE_RADIO_RX_BYTES / 4, NULL, TASK_PRIORITY_RADRX, &radioRxTaskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_RADIORX, radioRxTaskHandle);
#endif
// Start the telemetry tasks associated with Radio/USB
UAVObjIterate(&registerRadioObject);
// Listen to objects of interest
#ifdef PIOS_TELEM_PRIORITY_QUEUE
GCSTelemetryStatsConnectQueue(radioChannel.priorityQueue);
#else /* PIOS_TELEM_PRIORITY_QUEUE */
GCSTelemetryStatsConnectQueue(radioChannel.queue);
#endif /* PIOS_TELEM_PRIORITY_QUEUE */
xTaskCreate(telemetryTxTask,
"RadioTx",
STACK_SIZE_RADIO_TX_BYTES / 4,
&radioChannel,
TASK_PRIORITY_RADTX,
&radioChannel.txTaskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_RADIOTX,
radioChannel.txTaskHandle);
xTaskCreate(telemetryRxTask,
"RadioRx",
STACK_SIZE_RADIO_RX_BYTES / 4,
&radioChannel,
TASK_PRIORITY_RADRX,
&radioChannel.rxTaskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_RADIORX,
radioChannel.rxTaskHandle);
return 0;
}
/* Intialise a telemetry channel */
void TelemetryInitializeChannel(channelContext *channel)
{
// Create object queues
channel->queue = xQueueCreate(MAX_QUEUE_SIZE,
sizeof(UAVObjEvent));
#if defined(PIOS_TELEM_PRIORITY_QUEUE)
channel->priorityQueue = xQueueCreate(MAX_QUEUE_SIZE,
sizeof(UAVObjEvent));
#endif /* PIOS_TELEM_PRIORITY_QUEUE */
// Initialise UAVTalk
channel->uavTalkCon = UAVTalkInitialize(&transmitLocalData);
// Create periodic event that will be used to update the telemetry stats
UAVObjEvent ev;
memset(&ev, 0, sizeof(UAVObjEvent));
#ifdef PIOS_TELEM_PRIORITY_QUEUE
EventPeriodicQueueCreate(&ev,
channel->priorityQueue,
STATS_UPDATE_PERIOD_MS);
#else /* PIOS_TELEM_PRIORITY_QUEUE */
EventPeriodicQueueCreate(&ev,
channel->queue,
STATS_UPDATE_PERIOD_MS);
#endif /* PIOS_TELEM_PRIORITY_QUEUE */
}
/**
* Initialise the telemetry module
* \return -1 if initialisation failed
@ -144,39 +252,37 @@ int32_t TelemetryStart(void)
*/
int32_t TelemetryInitialize(void)
{
HwSettingsInitialize();
FlightTelemetryStatsInitialize();
GCSTelemetryStatsInitialize();
// Initialize vars
timeOfLastObjectUpdate = 0;
// Create object queues
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
#if defined(PIOS_TELEM_PRIORITY_QUEUE)
priorityQueue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
#endif
// Update telemetry settings
telemetryPort = PIOS_COM_TELEM_RF;
#ifdef PIOS_INCLUDE_RFM22B
radioPort = PIOS_COM_RF;
#endif
HwSettingsInitialize();
updateSettings();
// Initialise UAVTalk
uavTalkCon = UAVTalkInitialize(&transmitData);
#ifdef PIOS_INCLUDE_RFM22B
radioUavTalkCon = UAVTalkInitialize(&transmitRadioData);
#endif
// Create periodic event that will be used to update the telemetry stats
// FIXME STATS_UPDATE_PERIOD_MS is 4000ms while FlighTelemetryStats update period is 5000ms...
// Reset link stats
txErrors = 0;
txRetries = 0;
UAVObjEvent ev;
memset(&ev, 0, sizeof(UAVObjEvent));
EventPeriodicQueueCreate(&ev, priorityQueue, STATS_UPDATE_PERIOD_MS);
// Set channel port handlers
localChannel.getPort = localPort;
radioChannel.getPort = radioPort;
// Set the local telemetry baud rate
updateSettings(&localChannel);
// Only initialise local channel if telemetry port enabled
if (localPort()) {
// Initialise channel
TelemetryInitializeChannel(&localChannel);
// Initialise UAVTalk
localChannel.uavTalkCon = UAVTalkInitialize(&transmitLocalData);
}
// Initialise channel
TelemetryInitializeChannel(&radioChannel);
// Initialise UAVTalk
radioChannel.uavTalkCon = UAVTalkInitialize(&transmitRadioData);
return 0;
}
@ -188,22 +294,51 @@ MODULE_INITCALL(TelemetryInitialize, TelemetryStart);
* telemetry settings.
* \param[in] obj Object to connect
*/
static void registerObject(UAVObjHandle obj)
static void registerLocalObject(UAVObjHandle obj)
{
if (UAVObjIsMetaobject(obj)) {
// Only connect change notifications for meta objects. No periodic updates
UAVObjConnectQueue(obj, priorityQueue, EV_MASK_ALL_UPDATES);
#ifdef PIOS_TELEM_PRIORITY_QUEUE
UAVObjConnectQueue(obj, localChannel.priorityQueue, EV_MASK_ALL_UPDATES);
#else /* PIOS_TELEM_PRIORITY_QUEUE */
UAVObjConnectQueue(obj, localChannel.queue, EV_MASK_ALL_UPDATES);
#endif /* PIOS_TELEM_PRIORITY_QUEUE */
} else {
// Setup object for periodic updates
updateObject(obj, EV_NONE);
updateObject(
&localChannel,
obj,
EV_NONE);
}
}
static void registerRadioObject(UAVObjHandle obj)
{
if (UAVObjIsMetaobject(obj)) {
// Only connect change notifications for meta objects. No periodic updates
#ifdef PIOS_TELEM_PRIORITY_QUEUE
UAVObjConnectQueue(obj, radioChannel.priorityQueue, EV_MASK_ALL_UPDATES);
#else /* PIOS_TELEM_PRIORITY_QUEUE */
UAVObjConnectQueue(obj, radioChannel.queue, EV_MASK_ALL_UPDATES);
#endif /* PIOS_TELEM_PRIORITY_QUEUE */
} else {
// Setup object for periodic updates
updateObject(
&radioChannel,
obj,
EV_NONE);
}
}
/**
* Update object's queue connections and timer, depending on object's settings
* \param[in] telemetry channel context
* \param[in] obj Object to updates
*/
static void updateObject(UAVObjHandle obj, int32_t eventType)
static void updateObject(
channelContext *channel,
UAVObjHandle obj,
int32_t eventType)
{
UAVObjMetadata metadata;
UAVObjUpdateMode updateMode, loggingMode;
@ -226,13 +361,15 @@ static void updateObject(UAVObjHandle obj, int32_t eventType)
switch (updateMode) {
case UPDATEMODE_PERIODIC:
// Set update period
setUpdatePeriod(obj, metadata.telemetryUpdatePeriod);
setUpdatePeriod(channel,
obj,
metadata.telemetryUpdatePeriod);
// Connect queue
eventMask |= EV_UPDATED_PERIODIC | EV_UPDATED_MANUAL | EV_UPDATE_REQ;
break;
case UPDATEMODE_ONCHANGE:
// Set update period
setUpdatePeriod(obj, 0);
setUpdatePeriod(channel, obj, 0);
// Connect queue
eventMask |= EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ;
break;
@ -242,7 +379,9 @@ static void updateObject(UAVObjHandle obj, int32_t eventType)
eventMask |= EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ;
// Set update period on initialization and metadata change
if (eventType == EV_NONE) {
setUpdatePeriod(obj, metadata.telemetryUpdatePeriod);
setUpdatePeriod(channel,
obj,
metadata.telemetryUpdatePeriod);
}
} else {
// Otherwise, we just received an object update, so switch to periodic for the timeout period to prevent more updates
@ -251,7 +390,7 @@ static void updateObject(UAVObjHandle obj, int32_t eventType)
break;
case UPDATEMODE_MANUAL:
// Set update period
setUpdatePeriod(obj, 0);
setUpdatePeriod(channel, obj, 0);
// Connect queue
eventMask |= EV_UPDATED_MANUAL | EV_UPDATE_REQ;
break;
@ -259,13 +398,13 @@ static void updateObject(UAVObjHandle obj, int32_t eventType)
switch (loggingMode) {
case UPDATEMODE_PERIODIC:
// Set update period
setLoggingPeriod(obj, metadata.loggingUpdatePeriod);
setLoggingPeriod(channel, obj, metadata.loggingUpdatePeriod);
// Connect queue
eventMask |= EV_LOGGING_PERIODIC | EV_LOGGING_MANUAL;
break;
case UPDATEMODE_ONCHANGE:
// Set update period
setLoggingPeriod(obj, 0);
setLoggingPeriod(channel, obj, 0);
// Connect queue
eventMask |= EV_UPDATED | EV_LOGGING_MANUAL;
break;
@ -275,7 +414,9 @@ static void updateObject(UAVObjHandle obj, int32_t eventType)
eventMask |= EV_UPDATED | EV_LOGGING_MANUAL;
// Set update period on initialization and metadata change
if (eventType == EV_NONE) {
setLoggingPeriod(obj, metadata.loggingUpdatePeriod);
setLoggingPeriod(channel,
obj,
metadata.loggingUpdatePeriod);
}
} else {
// Otherwise, we just received an object update, so switch to periodic for the timeout period to prevent more updates
@ -284,23 +425,28 @@ static void updateObject(UAVObjHandle obj, int32_t eventType)
break;
case UPDATEMODE_MANUAL:
// Set update period
setLoggingPeriod(obj, 0);
setLoggingPeriod(channel, obj, 0);
// Connect queue
eventMask |= EV_LOGGING_MANUAL;
break;
}
// note that all setting objects have implicitly IsPriority=true
#ifdef PIOS_TELEM_PRIORITY_QUEUE
if (UAVObjIsPriority(obj)) {
UAVObjConnectQueue(obj, priorityQueue, eventMask);
} else {
UAVObjConnectQueue(obj, queue, eventMask);
}
UAVObjConnectQueue(obj, channel->priorityQueue, eventMask);
} else
#endif /* PIOS_TELEM_PRIORITY_QUEUE */
UAVObjConnectQueue(obj, channel->queue, eventMask);
}
/**
* Processes queue events
*/
static void processObjEvent(UAVObjEvent *ev)
static void processObjEvent(
channelContext *channel,
UAVObjEvent *ev)
{
UAVObjMetadata metadata;
UAVObjUpdateMode updateMode;
@ -325,7 +471,10 @@ static void processObjEvent(UAVObjEvent *ev)
// Send update to GCS (with retries)
while (retries < MAX_RETRIES && success == -1) {
// call blocks until ack is received or timeout
success = UAVTalkSendObject(uavTalkCon, ev->obj, ev->instId, UAVObjGetTelemetryAcked(&metadata), REQ_TIMEOUT_MS);
success = UAVTalkSendObject(channel->uavTalkCon,
ev->obj,
ev->instId,
UAVObjGetTelemetryAcked(&metadata), REQ_TIMEOUT_MS);
if (success == -1) {
++retries;
}
@ -339,7 +488,10 @@ static void processObjEvent(UAVObjEvent *ev)
// Request object update from GCS (with retries)
while (retries < MAX_RETRIES && success == -1) {
// call blocks until update is received or timeout
success = UAVTalkSendObjectRequest(uavTalkCon, ev->obj, ev->instId, REQ_TIMEOUT_MS);
success = UAVTalkSendObjectRequest(channel->uavTalkCon,
ev->obj,
ev->instId,
REQ_TIMEOUT_MS);
if (success == -1) {
++retries;
}
@ -353,11 +505,17 @@ static void processObjEvent(UAVObjEvent *ev)
// If this is a metaobject then make necessary telemetry updates
if (UAVObjIsMetaobject(ev->obj)) {
// linked object will be the actual object the metadata are for
updateObject(UAVObjGetLinkedObj(ev->obj), EV_NONE);
updateObject(
channel,
UAVObjGetLinkedObj(ev->obj),
EV_NONE);
} else {
if (updateMode == UPDATEMODE_THROTTLED) {
// If this is UPDATEMODE_THROTTLED, the event mask changes on every event.
updateObject(ev->obj, ev->event);
updateObject(
channel,
ev->obj,
ev->event);
}
}
}
@ -378,7 +536,10 @@ static void processObjEvent(UAVObjEvent *ev)
}
if (updateMode == UPDATEMODE_THROTTLED) {
// If this is UPDATEMODE_THROTTLED, the event mask changes on every event.
updateObject(ev->obj, ev->event);
updateObject(
channel,
ev->obj,
ev->event);
}
}
}
@ -386,37 +547,43 @@ static void processObjEvent(UAVObjEvent *ev)
/**
* Telemetry transmit task, regular priority
*/
static void telemetryTxTask(__attribute__((unused)) void *parameters)
static void telemetryTxTask(void *parameters)
{
channelContext *channel = (channelContext *)parameters;
UAVObjEvent ev;
/* Check for a bad context */
if (!channel) {
return;
}
// Loop forever
while (1) {
/**
* Tries to empty the high priority queue before handling any standard priority item
*/
#if defined(PIOS_TELEM_PRIORITY_QUEUE)
#ifdef PIOS_TELEM_PRIORITY_QUEUE
// empty priority queue, non-blocking
while (xQueueReceive(priorityQueue, &ev, 0) == pdTRUE) {
while (xQueueReceive(channel->priorityQueue, &ev, 0) == pdTRUE) {
// Process event
processObjEvent(&ev);
processObjEvent(channel, &ev);
}
// check regular queue and process update - non-blocking
if (xQueueReceive(queue, &ev, 0) == pdTRUE) {
if (xQueueReceive(channel->queue, &ev, 0) == pdTRUE) {
// Process event
processObjEvent(&ev);
processObjEvent(channel, &ev);
// if both queues are empty, wait on priority queue for updates (1 tick) then repeat cycle
} else if (xQueueReceive(priorityQueue, &ev, 1) == pdTRUE) {
} else if (xQueueReceive(channel->priorityQueue, &ev, 1) == pdTRUE) {
// Process event
processObjEvent(&ev);
processObjEvent(channel, &ev);
}
#else
// wait on queue for updates (1 tick) then repeat cycle
if (xQueueReceive(queue, &ev, 1) == pdTRUE) {
if (xQueueReceive(channel->queue, &ev, 1) == pdTRUE) {
// Process event
processObjEvent(&ev);
processObjEvent(channel, &ev);
}
#endif /* if defined(PIOS_TELEM_PRIORITY_QUEUE) */
#endif /* PIOS_TELEM_PRIORITY_QUEUE */
}
}
@ -424,22 +591,27 @@ static void telemetryTxTask(__attribute__((unused)) void *parameters)
/**
* Telemetry receive task. Processes queue events and periodic updates.
*/
static void telemetryRxTask(__attribute__((unused)) void *parameters)
static void telemetryRxTask(void *parameters)
{
channelContext *channel = (channelContext *)parameters;
/* Check for a bad context */
if (!channel) {
return;
}
// Task loop
while (1) {
uint32_t inputPort = getComPort(true);
uint32_t inputPort = channel->getPort();
if (inputPort) {
// Block until data are available
uint8_t serial_data[1];
uint8_t serial_data[16];
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]);
}
UAVTalkProcessInputStream(channel->uavTalkCon, serial_data, bytes_to_process);
}
} else {
vTaskDelay(5);
@ -447,30 +619,58 @@ static void telemetryRxTask(__attribute__((unused)) void *parameters)
}
}
#ifdef PIOS_INCLUDE_RFM22B
/**
* Radio telemetry receive task. Processes queue events and periodic updates.
* Determine the port to be used for communication on the telemetry channel
* \return com port number
*/
static void radioRxTask(__attribute__((unused)) void *parameters)
static uint32_t localPort()
{
// Task loop
while (1) {
if (radioPort) {
// Block until data are available
uint8_t serial_data[1];
uint16_t bytes_to_process;
return PIOS_COM_TELEM_RF;
}
bytes_to_process = PIOS_COM_ReceiveBuffer(radioPort, serial_data, sizeof(serial_data), 500);
if (bytes_to_process > 0) {
for (uint8_t i = 0; i < bytes_to_process; i++) {
UAVTalkProcessInputStream(radioUavTalkCon, serial_data[i]);
/**
* Determine the port to be used for communication on the radio channel
* \return com port number
*/
static uint32_t radioPort()
{
#ifdef PIOS_INCLUDE_RFM22B
uint32_t port = PIOS_COM_RF;
#else /* PIOS_INCLUDE_RFM22B */
uint32_t port = 0;
#endif /* PIOS_INCLUDE_RFM22B */
#ifdef PIOS_INCLUDE_USB
// if USB is connected, USB takes precedence for telemetry
if (PIOS_COM_Available(PIOS_COM_TELEM_USB)) {
port = PIOS_COM_TELEM_USB;
}
#endif /* PIOS_INCLUDE_USB */
return port;
}
} else {
vTaskDelay(5);
}
/**
* Transmit data buffer to the modem or USB port.
* \param[in] data Data buffer to send
* \param[in] length Length of buffer
* \return -1 on failure
* \return number of bytes transmitted on success
*/
static int32_t transmitLocalData(uint8_t *data, int32_t length)
{
uint32_t outputPort = localChannel.getPort();
if (outputPort) {
return PIOS_COM_SendBuffer(outputPort, data, length);
}
return -1;
}
/**
* Transmit data buffer to the radioport.
* \param[in] data Data buffer to send
@ -480,24 +680,7 @@ static void radioRxTask(__attribute__((unused)) void *parameters)
*/
static int32_t transmitRadioData(uint8_t *data, int32_t length)
{
if (radioPort) {
return PIOS_COM_SendBuffer(radioPort, data, length);
}
return -1;
}
#endif /* PIOS_INCLUDE_RFM22B */
/**
* Transmit data buffer to the modem or USB port.
* \param[in] data Data buffer to send
* \param[in] length Length of buffer
* \return -1 on failure
* \return number of bytes transmitted on success
*/
static int32_t transmitData(uint8_t *data, int32_t length)
{
uint32_t outputPort = getComPort(false);
uint32_t outputPort = radioChannel.getPort();
if (outputPort) {
return PIOS_COM_SendBuffer(outputPort, data, length);
@ -508,12 +691,16 @@ static int32_t transmitData(uint8_t *data, int32_t length)
/**
* Set update period of object (it must be already setup for periodic updates)
* \param[in] telemetry channel context
* \param[in] obj The object to update
* \param[in] updatePeriodMs The update period in ms, if zero then periodic updates are disabled
* \return 0 Success
* \return -1 Failure
*/
static int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs)
static int32_t setUpdatePeriod(
channelContext *channel,
UAVObjHandle obj,
int32_t updatePeriodMs)
{
UAVObjEvent ev;
int32_t ret;
@ -524,7 +711,12 @@ static int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs)
ev.event = EV_UPDATED_PERIODIC;
ev.lowPriority = true;
xQueueHandle targetQueue = UAVObjIsPriority(obj) ? priorityQueue : queue;
#ifdef PIOS_TELEM_PRIORITY_QUEUE
xQueueHandle targetQueue = UAVObjIsPriority(obj) ? channel->priorityQueue :
channel->queue;
#else /* PIOS_TELEM_PRIORITY_QUEUE */
xQueueHandle targetQueue = channel->queue;
#endif /* PIOS_TELEM_PRIORITY_QUEUE */
ret = EventPeriodicQueueUpdate(&ev, targetQueue, updatePeriodMs);
if (ret == -1) {
@ -535,12 +727,16 @@ static int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs)
/**
* Set logging update period of object (it must be already setup for periodic updates)
* \param[in] telemetry channel context
* \param[in] obj The object to update
* \param[in] updatePeriodMs The update period in ms, if zero then periodic updates are disabled
* \return 0 Success
* \return -1 Failure
*/
static int32_t setLoggingPeriod(UAVObjHandle obj, int32_t updatePeriodMs)
static int32_t setLoggingPeriod(
channelContext *channel,
UAVObjHandle obj,
int32_t updatePeriodMs)
{
UAVObjEvent ev;
int32_t ret;
@ -551,7 +747,12 @@ static int32_t setLoggingPeriod(UAVObjHandle obj, int32_t updatePeriodMs)
ev.event = EV_LOGGING_PERIODIC;
ev.lowPriority = true;
xQueueHandle targetQueue = UAVObjIsPriority(obj) ? priorityQueue : queue;
#ifdef PIOS_TELEM_PRIORITY_QUEUE
xQueueHandle targetQueue = UAVObjIsPriority(obj) ? channel->priorityQueue :
channel->queue;
#else /* PIOS_TELEM_PRIORITY_QUEUE */
xQueueHandle targetQueue = channel->queue;
#endif /* PIOS_TELEM_PRIORITY_QUEUE */
ret = EventPeriodicQueueUpdate(&ev, targetQueue, updatePeriodMs);
if (ret == -1) {
@ -590,10 +791,8 @@ static void updateTelemetryStats()
uint32_t timeNow;
// Get stats
UAVTalkGetStats(uavTalkCon, &utalkStats, true);
#ifdef PIOS_INCLUDE_RFM22B
UAVTalkAddStats(radioUavTalkCon, &utalkStats, true);
#endif
UAVTalkGetStats(localChannel.uavTalkCon, &utalkStats, true);
UAVTalkAddStats(radioChannel.uavTalkCon, &utalkStats, true);
// Get object data
FlightTelemetryStatsGet(&flightStats);
@ -683,68 +882,42 @@ static void updateTelemetryStats()
* settings, etc. Thus the HwSettings object which contains the
* telemetry port speed is used for now.
*/
static void updateSettings()
static void updateSettings(channelContext *channel)
{
if (telemetryPort) {
uint32_t port = channel->getPort();
if (port) {
// Retrieve settings
uint8_t speed;
HwSettingsTelemetrySpeedOptions speed;
HwSettingsTelemetrySpeedGet(&speed);
// Set port speed
switch (speed) {
case HWSETTINGS_TELEMETRYSPEED_2400:
PIOS_COM_ChangeBaud(telemetryPort, 2400);
PIOS_COM_ChangeBaud(port, 2400);
break;
case HWSETTINGS_TELEMETRYSPEED_4800:
PIOS_COM_ChangeBaud(telemetryPort, 4800);
PIOS_COM_ChangeBaud(port, 4800);
break;
case HWSETTINGS_TELEMETRYSPEED_9600:
PIOS_COM_ChangeBaud(telemetryPort, 9600);
PIOS_COM_ChangeBaud(port, 9600);
break;
case HWSETTINGS_TELEMETRYSPEED_19200:
PIOS_COM_ChangeBaud(telemetryPort, 19200);
PIOS_COM_ChangeBaud(port, 19200);
break;
case HWSETTINGS_TELEMETRYSPEED_38400:
PIOS_COM_ChangeBaud(telemetryPort, 38400);
PIOS_COM_ChangeBaud(port, 38400);
break;
case HWSETTINGS_TELEMETRYSPEED_57600:
PIOS_COM_ChangeBaud(telemetryPort, 57600);
PIOS_COM_ChangeBaud(port, 57600);
break;
case HWSETTINGS_TELEMETRYSPEED_115200:
PIOS_COM_ChangeBaud(telemetryPort, 115200);
PIOS_COM_ChangeBaud(port, 115200);
break;
}
}
}
/**
* Determine input/output com port as highest priority available
* @param[in] input Returns the approproate input com port if true, else the appropriate output com port
*/
#ifdef PIOS_INCLUDE_RFM22B
static uint32_t getComPort(bool input)
#else
static uint32_t getComPort(__attribute__((unused)) bool input)
#endif
{
#if defined(PIOS_INCLUDE_USB)
// if USB is connected, USB takes precedence for telemetry
if (PIOS_COM_Available(PIOS_COM_TELEM_USB)) {
return PIOS_COM_TELEM_USB;
} else
#endif /* PIOS_INCLUDE_USB */
#ifdef PIOS_INCLUDE_RFM22B
// PIOS_COM_RF input is handled by a separate RX thread and therefore must be ignored
if (input && telemetryPort == PIOS_COM_RF) {
return 0;
} else
#endif /* PIOS_INCLUDE_RFM22B */
if (PIOS_COM_Available(telemetryPort)) {
return telemetryPort;
} else {
return 0;
}
}
/**
* @}

36
flight/modules/TxPID/txpid.c
View File

@ -55,6 +55,9 @@
#include "accessorydesired.h"
#include "manualcontrolcommand.h"
#include "stabilizationsettings.h"
#ifdef REVOLUTION
#include "altitudeholdsettings.h"
#endif
#include "stabilizationbank.h"
#include "stabilizationsettingsbank1.h"
#include "stabilizationsettingsbank2.h"
@ -95,6 +98,10 @@ int32_t TxPIDInitialize(void)
bool txPIDEnabled;
HwSettingsOptionalModulesData optionalModules;
#ifdef REVOLUTION
AltitudeHoldSettingsInitialize();
#endif
HwSettingsInitialize();
HwSettingsOptionalModulesGet(&optionalModules);
@ -188,10 +195,17 @@ static void updatePIDs(UAVObjEvent *ev)
}
StabilizationSettingsData stab;
StabilizationSettingsGet(&stab);
#ifdef REVOLUTION
AltitudeHoldSettingsData altitude;
AltitudeHoldSettingsGet(&altitude);
#endif
AccessoryDesiredData accessory;
uint8_t needsUpdateBank = 0;
uint8_t needsUpdateStab = 0;
#ifdef REVOLUTION
uint8_t needsUpdateAltitude = 0;
#endif
// Loop through every enabled instance
for (uint8_t i = 0; i < TXPIDSETTINGS_PIDS_NUMELEM; i++) {
@ -351,6 +365,23 @@ static void updatePIDs(UAVObjEvent *ev)
case TXPIDSETTINGS_PIDS_ACROPLUSFACTOR:
needsUpdateBank |= update(&bank.AcroInsanityFactor, value);
break;
#ifdef REVOLUTION
case TXPIDSETTINGS_PIDS_ALTITUDEPOSKP:
needsUpdateAltitude |= update(&altitude.VerticalPosP, value);
break;
case TXPIDSETTINGS_PIDS_ALTITUDEVELOCITYKP:
needsUpdateAltitude |= update(&altitude.VerticalVelPID.Kp, value);
break;
case TXPIDSETTINGS_PIDS_ALTITUDEVELOCITYKI:
needsUpdateAltitude |= update(&altitude.VerticalVelPID.Ki, value);
break;
case TXPIDSETTINGS_PIDS_ALTITUDEVELOCITYKD:
needsUpdateAltitude |= update(&altitude.VerticalVelPID.Kd, value);
break;
case TXPIDSETTINGS_PIDS_ALTITUDEVELOCITYBETA:
needsUpdateAltitude |= update(&altitude.VerticalVelPID.Beta, value);
break;
#endif
default:
PIOS_Assert(0);
}
@ -359,6 +390,11 @@ static void updatePIDs(UAVObjEvent *ev)
if (needsUpdateStab) {
StabilizationSettingsSet(&stab);
}
#ifdef REVOLUTION
if (needsUpdateAltitude) {
AltitudeHoldSettingsSet(&altitude);
}
#endif
if (needsUpdateBank) {
switch (inst.BankNumber) {
case 0:

5
flight/modules/gpsp/gps9maghandler.c
View File

@ -49,9 +49,8 @@ void handleMag()
if (PIOS_HMC5x83_ReadMag(onboard_mag, mag) == 0) {
MagUbxPkt magPkt;
// swap axis so that if side with connector is aligned to revo side with connectors, mags data are aligned
magPkt.fragments.data.X = -mag[1];
magPkt.fragments.data.Y = mag[0];
magPkt.fragments.data.X = mag[0];
magPkt.fragments.data.Y = mag[1];
magPkt.fragments.data.Z = mag[2];
magPkt.fragments.data.status = 1;
ubx_buildPacket(&magPkt.packet, UBX_OP_CUST_CLASS, UBX_OP_MAG, sizeof(MagData));

55
flight/pios/common/libraries/msheap/msheap.c
View File

@ -194,13 +194,18 @@ msheap_init(heap_handle_t *heap, void *base, void *limit)
}
void *
msheap_alloc(heap_handle_t *heap, uint32_t size)
msheap_alloc(heap_handle_t *heap, void *ptr, uint32_t size)
{
marker_t cursor;
marker_t best;
uint32_t copy_data = 0;
uint16_t old_size = 0;
ASSERT(3, msheap_check(heap));
if (size == 0)
return 0;
/* convert the passed-in size to the number of marker-size units we need to allocate */
size += marker_size;
size = round_up(size, marker_size);
@ -210,6 +215,39 @@ msheap_alloc(heap_handle_t *heap, uint32_t size)
if (size > heap->heap_free)
return 0;
/* realloc */
if (ptr != 0) {
best = (marker_t)ptr - 1;
ASSERT(0, region_check(heap, best));
ASSERT(3, msheap_check(heap));
#ifdef HEAP_REALLOC_FREE_UNUSED_AREA
if (best->next.size == size)
goto done;
if (best->next.size > size) {
/* this region is free, mark it accordingly */
best->next.free = 1;
(best + best->next.size)->prev.free = 1;
traceFREE( ptr, best->next.size );
/* account for space we are freeing */
heap->heap_free += best->next.size;
goto split;
}
#else
if (best->next.size >= size)
goto done;
#endif
old_size = best->next.size;
msheap_free(heap, ptr);
copy_data = 1;
}
/* simple single-pass best-fit search */
restart:
cursor = heap->free_hint;
@ -242,13 +280,28 @@ restart:
/* no space */
return 0;
}
#ifdef HEAP_REALLOC_FREE_UNUSED_AREA
split:
#endif
/* split the free region to make space */
split_region(heap, best, size);
/* update free space counter */
heap->heap_free -= size;
done:
traceMALLOC( (void *)(best + 1), size );
/* Copy data that might be reused */
if (copy_data && ptr) {
size = old_size;
size = size - 1;
size *= marker_size;
for(uint32_t i=0 ; i < size; i++)
((uint8_t *)(best + 1))[i] = ((uint8_t *)ptr)[i];
}
/* and return a pointer to the allocated region */
return (void *)(best + 1);
}

2
flight/pios/common/libraries/msheap/msheap.h
View File

@ -116,7 +116,7 @@ extern void msheap_init(heap_handle_t *heap, void *base, void *limit);
*
* @param size The number of bytes required (more may be allocated).
*/
extern void *msheap_alloc(heap_handle_t *heap, uint32_t size);
extern void *msheap_alloc(heap_handle_t *heap, void *ptr, uint32_t size);
/**
* Free memory back to the heap.

12
flight/pios/common/libraries/msheap/pios_msheap.c
View File

@ -74,15 +74,15 @@ heap_handle_t fast_heap;
extern void vApplicationMallocFailedHook(void) __attribute__((weak));
void *
pios_general_malloc(size_t s, bool use_fast_heap)
pios_general_malloc(void *ptr, size_t s, bool use_fast_heap)
{
void *p;
vPortEnterCritical();
if(use_fast_heap){
p = msheap_alloc(&fast_heap, s);
p = msheap_alloc(&fast_heap, ptr, s);
} else {
p = msheap_alloc(&sram_heap, s);
p = msheap_alloc(&sram_heap, ptr, s);
}
vPortExitCritical();
@ -95,13 +95,13 @@ pios_general_malloc(size_t s, bool use_fast_heap)
void *
pvPortMalloc(size_t s)
{
return pios_general_malloc(s, true);
return pios_general_malloc(NULL, s, true);
}
void *
pvPortMallocStack(size_t s)
{
return pios_general_malloc(s, false);
return pios_general_malloc(NULL, s, false);
}
void
@ -154,7 +154,7 @@ malloc(size_t size)
heap_init_done = 1;
}
return msheap_alloc(sram_heap, size);
return msheap_alloc(NULL, sram_heap, size);
}
void

50
flight/pios/common/pios_com.c
View File

@ -283,6 +283,56 @@ int32_t PIOS_COM_ChangeBaud(uint32_t com_id, uint32_t baud)
return 0;
}
/**
* Set control lines associated with the port
* \param[in] port COM port
* \param[in] mask Lines to change
* \param[in] state New state for lines
* \return -1 if port not available
* \return 0 on success
*/
int32_t PIOS_COM_SetCtrlLine(uint32_t com_id, uint32_t mask, uint32_t state)
{
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) */
return -1;
}
/* Invoke the driver function if it exists */
if (com_dev->driver->set_ctrl_line) {
com_dev->driver->set_ctrl_line(com_dev->lower_id, mask, state);
}
return 0;
}
/**
* Set control lines associated with the port
* \param[in] port COM port
* \param[in] ctrl_line_cb Callback function
* \param[in] context context to pass to the callback function
* \return -1 if port not available
* \return 0 on success
*/
int32_t PIOS_COM_RegisterCtrlLineCallback(uint32_t com_id, pios_com_callback_ctrl_line ctrl_line_cb, uint32_t context)
{
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) */
return -1;
}
/* Invoke the driver function if it exists */
if (com_dev->driver->bind_ctrl_line_cb) {
com_dev->driver->bind_ctrl_line_cb(com_dev->lower_id, ctrl_line_cb, context);
}
return 0;
}
static int32_t PIOS_COM_SendBufferNonBlockingInternal(struct pios_com_dev *com_dev, const uint8_t *buffer, uint16_t len)
{

54
flight/pios/common/pios_hmc5x83.c
View File

@ -224,7 +224,7 @@ int32_t PIOS_HMC5x83_ReadMag(pios_hmc5x83_dev_t handler, int16_t out[3])
dev->data_ready = false;
uint8_t buffer[6];
int32_t temp;
int16_t temp[3];
int32_t sensitivity;
if (dev->cfg->Driver->Read(handler, PIOS_HMC5x83_DATAOUT_XMSB_REG, buffer, 6) != 0) {
@ -259,16 +259,54 @@ int32_t PIOS_HMC5x83_ReadMag(pios_hmc5x83_dev_t handler, int16_t out[3])
default:
PIOS_Assert(0);
}
for (int i = 0; i < 3; i++) {
temp = ((int16_t)((uint16_t)buffer[2 * i] << 8)
int16_t v = ((int16_t)((uint16_t)buffer[2 * i] << 8)
+ buffer[2 * i + 1]) * 1000 / sensitivity;
out[i] = temp;
temp[i] = v;
}
switch (dev->cfg->Orientation) {
case PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP:
out[0] = temp[2];
out[1] = temp[0];
out[2] = -temp[1];
break;
case PIOS_HMC5X83_ORIENTATION_SOUTH_EAST_UP:
out[0] = -temp[0];
out[1] = temp[2];
out[2] = -temp[1];
break;
case PIOS_HMC5X83_ORIENTATION_WEST_SOUTH_UP:
out[0] = -temp[2];
out[1] = -temp[0];
out[2] = -temp[1];
break;
case PIOS_HMC5X83_ORIENTATION_NORTH_WEST_UP:
out[0] = temp[0];
out[1] = -temp[2];
out[2] = -temp[1];
break;
case PIOS_HMC5X83_ORIENTATION_EAST_SOUTH_DOWN:
out[0] = temp[2];
out[1] = -temp[0];
out[2] = temp[1];
break;
case PIOS_HMC5X83_ORIENTATION_SOUTH_WEST_DOWN:
out[0] = -temp[0];
out[1] = -temp[2];
out[2] = temp[1];
break;
case PIOS_HMC5X83_ORIENTATION_WEST_NORTH_DOWN:
out[0] = -temp[2];
out[1] = temp[0];
out[2] = temp[1];
break;
case PIOS_HMC5X83_ORIENTATION_NORTH_EAST_DOWN:
out[0] = temp[0];
out[1] = temp[2];
out[2] = temp[1];
break;
}
// Data reads out as X,Z,Y
temp = out[2];
out[2] = out[1];
out[1] = temp;
// This should not be necessary but for some reason it is coming out of continuous conversion mode
dev->cfg->Driver->Write(handler, PIOS_HMC5x83_MODE_REG, PIOS_HMC5x83_MODE_CONTINUOUS);

38
flight/pios/common/pios_mem.c
View File

@ -29,17 +29,32 @@
#ifdef PIOS_TARGET_PROVIDES_FAST_HEAP
// relies on pios_general_malloc to perform the allocation (i.e. pios_msheap.c)
extern void *pios_general_malloc(size_t size, bool fastheap);
extern void *pios_general_malloc(void *ptr, size_t size, bool fastheap);
void *pios_fastheapmalloc(size_t size)
{
return pios_general_malloc(size, true);
return pios_general_malloc(NULL, size, true);
}
void *pios_malloc(size_t size)
{
return pios_general_malloc(size, false);
return pios_general_malloc(NULL, size, false);
}
void *pios_realloc(__attribute__((unused)) void *ptr, __attribute__((unused)) size_t size)
{
#ifdef PIOS_INCLUDE_REALLOC
return pios_general_malloc(ptr, size, false);
#else
// Not allowed to use realloc without the proper define PIOS_INCLUDE_REALLOC set
while (1) {
;
}
return NULL;
#endif
}
void pios_free(void *p)
@ -47,7 +62,7 @@ void pios_free(void *p)
vPortFree(p);
}
#else
#else /* ifdef PIOS_TARGET_PROVIDES_FAST_HEAP */
// demand to pvPortMalloc implementation
void *pios_fastheapmalloc(size_t size)
{
@ -60,6 +75,21 @@ void *pios_malloc(size_t size)
return pvPortMalloc(size);
}
void *pios_realloc(__attribute__((unused)) void *ptr, __attribute__((unused)) size_t size)
{
#ifdef PIOS_INCLUDE_REALLOC
return pvPortMalloc(size);
#else
// Not allowed to use realloc without the proper define PIOS_INCLUDE_REALLOC set
while (1) {
;
}
return NULL;
#endif
}
void pios_free(void *p)
{
vPortFree(p);

4
flight/pios/common/pios_mpxv.c
View File

@ -68,8 +68,8 @@ uint16_t PIOS_MPXV_Calibrate(PIOS_MPXV_descriptor *desc, uint16_t measurement)
*/
float PIOS_MPXV_CalcAirspeed(PIOS_MPXV_descriptor *desc, uint16_t measurement)
{
// Calculate dynamic pressure, as per docs
float Qc = 3.3f / 4096.0f * (float)(measurement - desc->zeroPoint);
// Calculate dynamic pressure, as per docs - Apply scale factor (voltage divider)
float Qc = 3.3f / 4096.0f * (float)((measurement - desc->zeroPoint) * desc->scale);
// Saturate Qc on the lower bound, in order to make sure we don't have negative airspeeds. No need
// to saturate on the upper bound, we'll handle that later with calibratedAirspeed.

21
flight/pios/common/pios_ms5611.c
View File

@ -42,7 +42,9 @@
#ifndef PIOS_MS5611_SLOW_TEMP_RATE
#define PIOS_MS5611_SLOW_TEMP_RATE 1
#endif
// Running moving average smoothing factor
#define PIOS_MS5611_TEMP_SMOOTHING 10
//
/* Local Types */
typedef struct {
uint16_t C[6];
@ -72,6 +74,7 @@ static uint32_t RawTemperature;
static uint32_t RawPressure;
static int64_t Pressure;
static int64_t Temperature;
static int64_t FilteredTemperature = INT32_MIN;
static int32_t lastConversionStart;
static uint32_t conversionDelayMs;
@ -247,6 +250,12 @@ int32_t PIOS_MS5611_ReadADC(void)
// Actual temperature (-40…85°C with 0.01°C resolution)
// TEMP = 20°C + dT * TEMPSENS = 2000 + dT * C6 / 2^23
Temperature = 2000l + ((deltaTemp * CalibData.C[5]) / POW2(23));
if (FilteredTemperature != INT32_MIN) {
FilteredTemperature = (FilteredTemperature * (PIOS_MS5611_TEMP_SMOOTHING - 1)
+ Temperature) / PIOS_MS5611_TEMP_SMOOTHING;
} else {
FilteredTemperature = Temperature;
}
} else {
int64_t Offset;
int64_t Sens;
@ -259,21 +268,21 @@ int32_t PIOS_MS5611_ReadADC(void)
return -2;
}
// check if temperature is less than 20°C
if (Temperature < 2000) {
if (FilteredTemperature < 2000) {
// Apply compensation
// T2 = dT^2 / 2^31
// OFF2 = 5 ⋅ (TEMP – 2000)^2/2
// SENS2 = 5 ⋅ (TEMP – 2000)^2/2^2
int64_t tcorr = (Temperature - 2000) * (Temperature - 2000);
int64_t tcorr = (FilteredTemperature - 2000) * (FilteredTemperature - 2000);
Offset2 = (5 * tcorr) / 2;
Sens2 = (5 * tcorr) / 4;
compensation_t2 = (deltaTemp * deltaTemp) >> 31;
// Apply the "Very low temperature compensation" when temp is less than -15°C
if (Temperature < -1500) {
if (FilteredTemperature < -1500) {
// OFF2 = OFF2 + 7 ⋅ (TEMP + 1500)^2
// SENS2 = SENS2 + 11 ⋅ (TEMP + 1500)^2 / 2
int64_t tcorr2 = (Temperature + 1500) * (Temperature + 1500);
int64_t tcorr2 = (FilteredTemperature + 1500) * (FilteredTemperature + 1500);
Offset2 += 7 * tcorr2;
Sens2 += (11 * tcorr2) / 2;
}
@ -302,7 +311,7 @@ int32_t PIOS_MS5611_ReadADC(void)
static float PIOS_MS5611_GetTemperature(void)
{
// Apply the second order low and very low temperature compensation offset
return ((float)(Temperature - compensation_t2)) / 100.0f;
return ((float)(FilteredTemperature - compensation_t2)) / 100.0f;
}
/**

13
flight/pios/common/pios_oplinkrcvr.c
View File

@ -34,6 +34,7 @@
#include <uavobjectmanager.h>
#include <oplinkreceiver.h>
#include <oplinkstatus.h>
#include <pios_oplinkrcvr_priv.h>
static OPLinkReceiverData oplinkreceiverdata;
@ -41,9 +42,11 @@ static OPLinkReceiverData oplinkreceiverdata;
/* Provide a RCVR driver */
static int32_t PIOS_OPLinkRCVR_Get(uint32_t rcvr_id, uint8_t channel);
static void PIOS_oplinkrcvr_Supervisor(uint32_t ppm_id);
static uint8_t PIOS_OPLinkRCVR_Quality_Get(uint32_t oplinkrcvr_id);
const struct pios_rcvr_driver pios_oplinkrcvr_rcvr_driver = {
.read = PIOS_OPLinkRCVR_Get,
.get_quality = PIOS_OPLinkRCVR_Quality_Get
};
/* Local Variables */
@ -186,6 +189,16 @@ static void PIOS_oplinkrcvr_Supervisor(uint32_t oplinkrcvr_id)
oplinkrcvr_dev->Fresh = false;
}
static uint8_t PIOS_OPLinkRCVR_Quality_Get(__attribute__((unused)) uint32_t oplinkrcvr_id)
{
uint8_t oplink_quality;
OPLinkStatusLinkQualityGet(&oplink_quality);
/* link_status is in the range 0-128, so scale to a % */
return oplink_quality * 100 / 128;
}
#endif /* PIOS_INCLUDE_OPLINKRCVR */
/**

28
flight/pios/common/pios_rcvr.c
View File

@ -113,6 +113,34 @@ int32_t PIOS_RCVR_Read(uint32_t rcvr_id, uint8_t channel)
return rcvr_dev->driver->read(rcvr_dev->lower_id, channel);
}
/**
* @brief Reads input quality from the appropriate driver
* @param[in] rcvr_id driver to read from
* @returns received signal quality expressed as a %
* @retval PIOS_RCVR_NODRIVER driver was not initialized
*/
uint8_t PIOS_RCVR_GetQuality(uint32_t rcvr_id)
{
if (rcvr_id == 0) {
return PIOS_RCVR_NODRIVER;
}
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) */
/* As no receiver is available assume min */
return 0;
}
if (!rcvr_dev->driver->get_quality) {
/* If no quality is available assume max */
return 100;
}
return rcvr_dev->driver->get_quality(rcvr_dev->lower_id);
}
/**
* @brief Get a semaphore that signals when a new sample is available.
* @param[in] rcvr_id driver to read from

14
flight/pios/common/pios_rfm22b.c
View File

@ -1376,7 +1376,7 @@ static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev)
}
// Wait 1ms if not.
PIOS_DELAY_WaitmS(1);
vTaskDelay(1 + (1 / (portTICK_RATE_MS + 1)));
}
// ****************
@ -1481,6 +1481,15 @@ static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev)
// x-nibbles rx preamble detection
rfm22_write(rfm22b_dev, RFM22_preamble_detection_ctrl1, RX_PREAMBLE_NIBBLES << 3);
// Release the bus
rfm22_releaseBus(rfm22b_dev);
// Yield the CPU.
vTaskDelay(1 + (1 / (portTICK_RATE_MS + 1)));
// Claim the SPI bus.
rfm22_claimBus(rfm22b_dev);
// header control - using a 4 by header with broadcast of 0xffffffff
rfm22_write(rfm22b_dev, RFM22_header_control1,
RFM22_header_cntl1_bcen_0 |
@ -1530,6 +1539,9 @@ static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev)
// Release the bus
rfm22_releaseBus(rfm22b_dev);
// Yield the CPU.
vTaskDelay(1 + (1 / (portTICK_RATE_MS + 1)));
// Initialize the frequency and datarate to te default.
rfm22_setNominalCarrierFrequency(rfm22b_dev, 0);
pios_rfm22_setDatarate(rfm22b_dev);

59
flight/pios/common/pios_sbus.c
View File

@ -32,6 +32,10 @@
#ifdef PIOS_INCLUDE_SBUS
// Define to report number of frames since last dropped instead of weighted ave
#undef SBUS_GOOD_FRAME_COUNT
#include <uavobjectmanager.h>
#include "pios_sbus_priv.h"
/* Forward Declarations */
@ -42,11 +46,12 @@ static uint16_t PIOS_SBus_RxInCallback(uint32_t context,
uint16_t *headroom,
bool *need_yield);
static void PIOS_SBus_Supervisor(uint32_t sbus_id);
static uint8_t PIOS_SBus_Quality_Get(uint32_t rcvr_id);
/* Local Variables */
const struct pios_rcvr_driver pios_sbus_rcvr_driver = {
.read = PIOS_SBus_Get,
.get_quality = PIOS_SBus_Quality_Get
};
enum pios_sbus_dev_magic {
@ -60,8 +65,17 @@ struct pios_sbus_state {
uint8_t failsafe_timer;
uint8_t frame_found;
uint8_t byte_count;
float quality;
#ifdef SBUS_GOOD_FRAME_COUNT
uint8_t frame_count;
#endif /* SBUS_GOOD_FRAME_COUNT */
};
/* With an S.Bus frame rate of 7ms (130Hz) averaging over 26 samples
* gives about a 200ms response.
*/
#define SBUS_FL_WEIGHTED_AVE 26
struct pios_sbus_dev {
enum pios_sbus_dev_magic magic;
const struct pios_sbus_cfg *cfg;
@ -120,6 +134,10 @@ static void PIOS_SBus_ResetState(struct pios_sbus_state *state)
state->receive_timer = 0;
state->failsafe_timer = 0;
state->frame_found = 0;
state->quality = 0.0f;
#ifdef SBUS_GOOD_FRAME_COUNT
state->frame_count = 0;
#endif /* SBUS_GOOD_FRAME_COUNT */
PIOS_SBus_ResetChannels(state);
}
@ -251,11 +269,29 @@ static void PIOS_SBus_UpdateState(struct pios_sbus_state *state, uint8_t b)
state->byte_count++;
} else {
if (b == SBUS_EOF_BYTE || (b & SBUS_R7008SB_EOF_COUNTER_MASK) == 0) {
#ifndef SBUS_GOOD_FRAME_COUNT
/* Quality trend is towards 0% by default*/
uint8_t quality_trend = 0;
#endif /* SBUS_GOOD_FRAME_COUNT */
/* full frame received */
uint8_t flags = state->received_data[SBUS_FRAME_LENGTH - 3];
if (flags & SBUS_FLAG_FL) {
/* frame lost, do not update */
} else if (flags & SBUS_FLAG_FS) {
#ifdef SBUS_GOOD_FRAME_COUNT
state->quality = state->frame_count;
state->frame_count = 0;
#endif /* SBUS_GOOD_FRAME_COUNT */
} else {
#ifdef SBUS_GOOD_FRAME_COUNT
if (++state->frame_count == 255) {
state->quality = state->frame_count--;
}
#else /* SBUS_GOOD_FRAME_COUNT */
/* Quality trend is towards 100% */
quality_trend = 100;
#endif /* SBUS_GOOD_FRAME_COUNT */
if (flags & SBUS_FLAG_FS) {
/* failsafe flag active */
PIOS_SBus_ResetChannels(state);
} else {
@ -263,6 +299,12 @@ static void PIOS_SBus_UpdateState(struct pios_sbus_state *state, uint8_t b)
PIOS_SBus_UnrollChannels(state);
state->failsafe_timer = 0;
}
}
#ifndef SBUS_GOOD_FRAME_COUNT
/* Present quality as a weighted average of good frames */
state->quality = ((state->quality * (SBUS_FL_WEIGHTED_AVE - 1)) +
quality_trend) / SBUS_FL_WEIGHTED_AVE;
#endif /* SBUS_GOOD_FRAME_COUNT */
} else {
/* discard whole frame */
}
@ -341,6 +383,19 @@ static void PIOS_SBus_Supervisor(uint32_t sbus_id)
}
}
static uint8_t PIOS_SBus_Quality_Get(uint32_t sbus_id)
{
struct pios_sbus_dev *sbus_dev = (struct pios_sbus_dev *)sbus_id;
bool valid = PIOS_SBus_Validate(sbus_dev);
PIOS_Assert(valid);
struct pios_sbus_state *state = &(sbus_dev->state);
return (uint8_t)(state->quality + 0.5f);
}
#endif /* PIOS_INCLUDE_SBUS */
/**

365
flight/pios/common/pios_srxl.c Normal file
View File

@ -0,0 +1,365 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_SRXL Multiplex SRXL receiver functions
* @brief Code to read Multiplex SRXL receiver serial stream
* @{
*
* @file pios_srxl.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Code to read Multiplex SRXL receiver serial stream
* @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 "pios.h"
#ifdef PIOS_INCLUDE_SRXL
#include "pios_srxl_priv.h"
// #define PIOS_INSTRUMENT_MODULE
#include <pios_instrumentation_helper.h>
PERF_DEFINE_COUNTER(crcFailureCount);
PERF_DEFINE_COUNTER(failsafeCount);
PERF_DEFINE_COUNTER(successfulCount);
PERF_DEFINE_COUNTER(messageUnrollTimer);
PERF_DEFINE_COUNTER(messageReceiveRate);
PERF_DEFINE_COUNTER(receivedBytesCount);
PERF_DEFINE_COUNTER(frameStartCount);
PERF_DEFINE_COUNTER(frameAbortCount);
PERF_DEFINE_COUNTER(completeMessageCount);
/* Forward Declarations */
static int32_t PIOS_SRXL_Get(uint32_t rcvr_id, uint8_t channel);
static uint16_t PIOS_SRXL_RxInCallback(uint32_t context,
uint8_t *buf,
uint16_t buf_len,
uint16_t *headroom,
bool *need_yield);
static void PIOS_SRXL_Supervisor(uint32_t srxl_id);
/* Local Variables */
const struct pios_rcvr_driver pios_srxl_rcvr_driver = {
.read = PIOS_SRXL_Get,
};
enum pios_srxl_dev_magic {
PIOS_SRXL_DEV_MAGIC = 0x55545970,
};
struct pios_srxl_state {
uint16_t channel_data[PIOS_SRXL_NUM_INPUTS];
uint8_t received_data[SRXL_FRAME_LENGTH];
uint8_t receive_timer;
uint8_t failsafe_timer;
uint8_t frame_found;
uint8_t byte_count;
uint8_t data_bytes;
};
struct pios_srxl_dev {
enum pios_srxl_dev_magic magic;
struct pios_srxl_state state;
};
/* Allocate S.Bus device descriptor */
#if defined(PIOS_INCLUDE_FREERTOS)
static struct pios_srxl_dev *PIOS_SRXL_Alloc(void)
{
struct pios_srxl_dev *srxl_dev;
srxl_dev = (struct pios_srxl_dev *)pios_malloc(sizeof(*srxl_dev));
if (!srxl_dev) {
return NULL;
}
srxl_dev->magic = PIOS_SRXL_DEV_MAGIC;
return srxl_dev;
}
#else
static struct pios_srxl_dev pios_srxl_devs[PIOS_SRXL_MAX_DEVS];
static uint8_t pios_srxl_num_devs;
static struct pios_srxl_dev *PIOS_SRXL_Alloc(void)
{
struct pios_srxl_dev *srxl_dev;
if (pios_srxl_num_devs >= PIOS_SRXL_MAX_DEVS) {
return NULL;
}
srxl_dev = &pios_srxl_devs[pios_srxl_num_devs++];
srxl_dev->magic = PIOS_SRXL_DEV_MAGIC;
return srxl_dev;
}
#endif /* if defined(PIOS_INCLUDE_FREERTOS) */
/* Validate SRXL device descriptor */
static bool PIOS_SRXL_Validate(struct pios_srxl_dev *srxl_dev)
{
return srxl_dev->magic == PIOS_SRXL_DEV_MAGIC;
}
/* Reset channels in case of lost signal or explicit failsafe receiver flag */
static void PIOS_SRXL_ResetChannels(struct pios_srxl_state *state)
{
for (int i = 0; i < PIOS_SRXL_NUM_INPUTS; i++) {
state->channel_data[i] = PIOS_RCVR_TIMEOUT;
}
}
/* Reset SRXL receiver state */
static void PIOS_SRXL_ResetState(struct pios_srxl_state *state)
{
state->receive_timer = 0;
state->failsafe_timer = 0;
state->frame_found = 0;
state->data_bytes = 0;
PIOS_SRXL_ResetChannels(state);
}
/* Initialize SRXL receiver interface */
int32_t PIOS_SRXL_Init(uint32_t *srxl_id,
const struct pios_com_driver *driver,
uint32_t lower_id)
{
PIOS_DEBUG_Assert(srxl_id);
PIOS_DEBUG_Assert(driver);
struct pios_srxl_dev *srxl_dev;
srxl_dev = (struct pios_srxl_dev *)PIOS_SRXL_Alloc();
if (!srxl_dev) {
goto out_fail;
}
PIOS_SRXL_ResetState(&(srxl_dev->state));
*srxl_id = (uint32_t)srxl_dev;
/* Set comm driver callback */
(driver->bind_rx_cb)(lower_id, PIOS_SRXL_RxInCallback, *srxl_id);
if (!PIOS_RTC_RegisterTickCallback(PIOS_SRXL_Supervisor, *srxl_id)) {
PIOS_DEBUG_Assert(0);
}
PERF_INIT_COUNTER(crcFailureCount, 0x5551);
PERF_INIT_COUNTER(failsafeCount, 0x5552);
PERF_INIT_COUNTER(successfulCount, 0x5553);
PERF_INIT_COUNTER(messageUnrollTimer, 0x5554);
PERF_INIT_COUNTER(messageReceiveRate, 0x5555);
PERF_INIT_COUNTER(receivedBytesCount, 0x5556);
PERF_INIT_COUNTER(frameStartCount, 0x5557);
PERF_INIT_COUNTER(frameAbortCount, 0x5558);
PERF_INIT_COUNTER(completeMessageCount, 0x5559);
return 0;
out_fail:
return -1;
}
/**
* Get the value of an input channel
* \param[in] channel Number of the channel desired (zero based)
* \output PIOS_RCVR_INVALID channel not available
* \output PIOS_RCVR_TIMEOUT failsafe condition or missing receiver
* \output >=0 channel value
*/
static int32_t PIOS_SRXL_Get(uint32_t rcvr_id, uint8_t channel)
{
struct pios_srxl_dev *srxl_dev = (struct pios_srxl_dev *)rcvr_id;
if (!PIOS_SRXL_Validate(srxl_dev)) {
return PIOS_RCVR_INVALID;
}
/* return error if channel is not available */
if (channel >= PIOS_SRXL_NUM_INPUTS) {
return PIOS_RCVR_INVALID;
}
return srxl_dev->state.channel_data[channel];
}
static void PIOS_SRXL_UnrollChannels(struct pios_srxl_state *state)
{
PERF_TIMED_SECTION_START(messageUnrollTimer);
uint8_t *received_data = state->received_data;
uint8_t channel;
uint16_t channel_value;
for (channel = 0; channel < (state->data_bytes / 2); channel++) {
channel_value = ((uint16_t)received_data[SRXL_HEADER_LENGTH + (channel * 2)]) << 8;
channel_value = channel_value + ((uint16_t)received_data[SRXL_HEADER_LENGTH + (channel * 2) + 1]);
state->channel_data[channel] = (800 + ((channel_value * 1400) >> 12));
}
PERF_TIMED_SECTION_END(messageUnrollTimer);
}
static bool PIOS_SRXL_Validate_Checksum(struct pios_srxl_state *state)
{
// Check the CRC16 checksum. The provided checksum is immediately after the channel data.
// All data including start byte and version byte is included in crc calculation.
uint8_t i = 0;
uint16_t crc = 0;
for (i = 0; i < SRXL_HEADER_LENGTH + state->data_bytes; i++) {
crc = crc ^ (int16_t)state->received_data[i] << 8;
uint8_t j = 0;
for (j = 0; j < 8; j++) {
if (crc & 0x8000) {
crc = crc << 1 ^ 0x1021;
} else {
crc = crc << 1;
}
}
}
uint16_t checksum = (((uint16_t)(state->received_data[i] << 8) |
(uint16_t)state->received_data[i + 1]));
return crc == checksum;
}
/* Update decoder state processing input byte from the SRXL stream */
static void PIOS_SRXL_UpdateState(struct pios_srxl_state *state, uint8_t b)
{
/* should not process any data until new frame is found */
if (!state->frame_found) {
return;
}
if (state->byte_count < (SRXL_HEADER_LENGTH + state->data_bytes + SRXL_CHECKSUM_LENGTH)) {
if (state->byte_count == 0) {
// Set up the length of the channel data according to version received
PERF_INCREMENT_VALUE(frameStartCount);
if (b == SRXL_V1_HEADER) {
state->data_bytes = SRXL_V1_CHANNEL_DATA_BYTES;
} else if (b == SRXL_V2_HEADER) {
state->data_bytes = SRXL_V2_CHANNEL_DATA_BYTES;
} else {
/* discard the whole frame if the 1st byte is not correct */
state->frame_found = 0;
PERF_INCREMENT_VALUE(frameAbortCount);
return;
}
}
/* store next byte */
state->received_data[state->byte_count] = b;
state->byte_count++;
if (state->byte_count == (SRXL_HEADER_LENGTH + state->data_bytes + SRXL_CHECKSUM_LENGTH)) {
PERF_INCREMENT_VALUE(completeMessageCount);
// We have a complete message, lets decode it
if (PIOS_SRXL_Validate_Checksum(state)) {
/* data looking good */
PIOS_SRXL_UnrollChannels(state);
state->failsafe_timer = 0;
PERF_INCREMENT_VALUE(successfulCount);
} else {
/* discard whole frame */
PERF_INCREMENT_VALUE(crcFailureCount);
}
/* prepare for the next frame */
state->frame_found = 0;
PERF_MEASURE_PERIOD(messageReceiveRate);
}
}
}
/* Comm byte received callback */
static uint16_t PIOS_SRXL_RxInCallback(uint32_t context,
uint8_t *buf,
uint16_t buf_len,
uint16_t *headroom,
bool *need_yield)
{
struct pios_srxl_dev *srxl_dev = (struct pios_srxl_dev *)context;
bool valid = PIOS_SRXL_Validate(srxl_dev);
PIOS_Assert(valid);
struct pios_srxl_state *state = &(srxl_dev->state);
/* process byte(s) and clear receive timer */
for (uint8_t i = 0; i < buf_len; i++) {
PIOS_SRXL_UpdateState(state, buf[i]);
state->receive_timer = 0;
PERF_INCREMENT_VALUE(receivedBytesCount);
}
/* Always signal that we can accept another byte */
if (headroom) {
*headroom = SRXL_FRAME_LENGTH;
}
/* We never need a yield */
*need_yield = false;
/* Always indicate that all bytes were consumed */
return buf_len;
}
/**
* Input data supervisor is called periodically and provides
* two functions: frame syncing and failsafe triggering.
*
* Multiplex SRXL frames come at 14ms (FastResponse ON) or 21ms (FastResponse OFF)
* rate at 115200bps.
* RTC timer is running at 625Hz (1.6ms). So with divider 2 it gives
* 3.2ms pause between frames which is good for both SRXL frame rates.
*
* Data receive function must clear the receive_timer to confirm new
* data reception. If no new data received in 100ms, we must call the
* failsafe function which clears all channels.
*/
static void PIOS_SRXL_Supervisor(uint32_t srxl_id)
{
struct pios_srxl_dev *srxl_dev = (struct pios_srxl_dev *)srxl_id;
bool valid = PIOS_SRXL_Validate(srxl_dev);
PIOS_Assert(valid);
struct pios_srxl_state *state = &(srxl_dev->state);
/* waiting for new frame if no bytes were received in 6.4ms */
if (++state->receive_timer > 4) {
state->frame_found = 1;
state->byte_count = 0;
state->receive_timer = 0;
}
/* activate failsafe if no frames have arrived in 102.4ms */
if (++state->failsafe_timer > 64) {
PIOS_SRXL_ResetChannels(state);
state->failsafe_timer = 0;
PERF_INCREMENT_VALUE(failsafeCount);
}
}
#endif /* PIOS_INCLUDE_SRXL */
/**
* @}
* @}
*/

1
flight/pios/inc/pios_adc.h
View File

@ -38,6 +38,7 @@ typedef void (*ADCCallback)(float *data);
/* Public Functions */
void PIOS_ADC_Config(uint32_t oversampling);
void PIOS_ADC_PinSetup(uint32_t pin);
int32_t PIOS_ADC_PinGet(uint32_t pin);
float PIOS_ADC_PinGetVolt(uint32_t pin);
int16_t *PIOS_ADC_GetRawBuffer(void);

11
flight/pios/inc/pios_com.h
View File

@ -8,6 +8,7 @@
*
* @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
*
@ -35,19 +36,29 @@
#include <stdbool.h> /* bool */
typedef uint16_t (*pios_com_callback)(uint32_t context, uint8_t *buf, uint16_t buf_len, uint16_t *headroom, bool *task_woken);
typedef void (*pios_com_callback_ctrl_line)(uint32_t context, uint32_t mask, uint32_t state);
struct pios_com_driver {
void (*init)(uint32_t id);
void (*set_baud)(uint32_t id, uint32_t baud);
void (*set_ctrl_line)(uint32_t id, uint32_t mask, uint32_t state);
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);
void (*bind_ctrl_line_cb)(uint32_t id, pios_com_callback_ctrl_line ctrl_line_cb, uint32_t context);
bool (*available)(uint32_t id);
};
/* Control line definitions */
#define COM_CTRL_LINE_DTR_MASK 0x01
#define COM_CTRL_LINE_RTS_MASK 0x02
/* 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_SetCtrlLine(uint32_t com_id, uint32_t mask, uint32_t state);
extern int32_t PIOS_COM_RegisterCtrlLineCallback(uint32_t usart_id, pios_com_callback_ctrl_line ctrl_line_cb, uint32_t context);
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);

13
flight/pios/inc/pios_hmc5x83.h
View File

@ -109,6 +109,18 @@ extern const struct pios_hmc5x83_io_driver PIOS_HMC5x83_SPI_DRIVER;
#ifdef PIOS_INCLUDE_I2C
extern const struct pios_hmc5x83_io_driver PIOS_HMC5x83_I2C_DRIVER;
#endif
// xyz axis orientation
enum PIOS_HMC5X83_ORIENTATION {
PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP,
PIOS_HMC5X83_ORIENTATION_SOUTH_EAST_UP,
PIOS_HMC5X83_ORIENTATION_WEST_SOUTH_UP,
PIOS_HMC5X83_ORIENTATION_NORTH_WEST_UP,
PIOS_HMC5X83_ORIENTATION_EAST_SOUTH_DOWN,
PIOS_HMC5X83_ORIENTATION_SOUTH_WEST_DOWN,
PIOS_HMC5X83_ORIENTATION_WEST_NORTH_DOWN,
PIOS_HMC5X83_ORIENTATION_NORTH_EAST_DOWN,
};
struct pios_hmc5x83_cfg {
#ifdef PIOS_HMC5X83_HAS_GPIOS
@ -119,6 +131,7 @@ struct pios_hmc5x83_cfg {
uint8_t Gain; // Gain Configuration, select the full scale --> here below the relative define (See datasheet page 11 for more details) */
uint8_t Mode;
bool TempCompensation; // enable temperature sensor on HMC5983 for temperature gain compensation
enum PIOS_HMC5X83_ORIENTATION Orientation;
const struct pios_hmc5x83_io_driver *Driver;
};

31
flight/pios/inc/pios_instrumentation.h
View File

@ -47,7 +47,7 @@ extern int8_t pios_instrumentation_last_used_counter;
* @param counter_handle handle of the counter to update @see PIOS_Instrumentation_SearchCounter @see PIOS_Instrumentation_CreateCounter
* @param newValue the updated value.
*/
inline void PIOS_Instrumentation_updateCounter(pios_counter_t counter_handle, int32_t newValue)
static inline void PIOS_Instrumentation_updateCounter(pios_counter_t counter_handle, int32_t newValue)
{
PIOS_Assert(pios_instrumentation_perf_counters && counter_handle);
vPortEnterCritical();
@ -69,7 +69,7 @@ inline void PIOS_Instrumentation_updateCounter(pios_counter_t counter_handle, in
* Used to determine the time duration of a code block, mark the begin of the block. @see PIOS_Instrumentation_TimeEnd
* @param counter_handle handle of the counter @see PIOS_Instrumentation_SearchCounter @see PIOS_Instrumentation_CreateCounter
*/
inline void PIOS_Instrumentation_TimeStart(pios_counter_t counter_handle)
static inline void PIOS_Instrumentation_TimeStart(pios_counter_t counter_handle)
{
PIOS_Assert(pios_instrumentation_perf_counters && counter_handle);
vPortEnterCritical();
@ -83,7 +83,7 @@ inline void PIOS_Instrumentation_TimeStart(pios_counter_t counter_handle)
* Used to determine the time duration of a code block, mark the end of the block. @see PIOS_Instrumentation_TimeStart
* @param counter_handle handle of the counter @see PIOS_Instrumentation_SearchCounter @see PIOS_Instrumentation_CreateCounter
*/
inline void PIOS_Instrumentation_TimeEnd(pios_counter_t counter_handle)
static inline void PIOS_Instrumentation_TimeEnd(pios_counter_t counter_handle)
{
PIOS_Assert(pios_instrumentation_perf_counters && counter_handle);
vPortEnterCritical();
@ -106,7 +106,7 @@ inline void PIOS_Instrumentation_TimeEnd(pios_counter_t counter_handle)
* Used to determine the mean period between each call to the function
* @param counter_handle handle of the counter @see PIOS_Instrumentation_SearchCounter @see PIOS_Instrumentation_CreateCounter
*/
inline void PIOS_Instrumentation_TrackPeriod(pios_counter_t counter_handle)
static inline void PIOS_Instrumentation_TrackPeriod(pios_counter_t counter_handle)
{
PIOS_Assert(pios_instrumentation_perf_counters && counter_handle);
pios_perf_counter_t *counter = (pios_perf_counter_t *)counter_handle;
@ -127,6 +127,29 @@ inline void PIOS_Instrumentation_TrackPeriod(pios_counter_t counter_handle)
counter->lastUpdateTS = PIOS_DELAY_GetRaw();
}
/**
* Increment a counter with a value
* @param counter_handle handle of the counter to update @see PIOS_Instrumentation_SearchCounter @see PIOS_Instrumentation_CreateCounter
* @param increment the value to increment counter with.
*/
static inline void PIOS_Instrumentation_incrementCounter(pios_counter_t counter_handle, int32_t increment)
{
PIOS_Assert(pios_instrumentation_perf_counters && counter_handle);
vPortEnterCritical();
pios_perf_counter_t *counter = (pios_perf_counter_t *)counter_handle;
counter->value += increment;
counter->max--;
if (counter->value > counter->max) {
counter->max = counter->value;
}
counter->min++;
if (counter->value < counter->min) {
counter->min = counter->value;
}
counter->lastUpdateTS = PIOS_DELAY_GetRaw();
vPortExitCritical();
}
/**
* Initialize the Instrumentation infrastructure
* @param maxCounters maximum number of allowed counters

4
flight/pios/inc/pios_instrumentation_helper.h
View File

@ -98,6 +98,8 @@
#define PERF_TIMED_SECTION_END(x) PIOS_Instrumentation_TimeEnd(x)
#define PERF_MEASURE_PERIOD(x) PIOS_Instrumentation_TrackPeriod(x)
#define PERF_TRACK_VALUE(x, y) PIOS_Instrumentation_updateCounter(x, y)
#define PERF_INCREMENT_VALUE(x) PIOS_Instrumentation_incrementCounter(x, 1)
#define PERF_DECREMENT_VALUE(x) PIOS_Instrumentation_incrementCounter(x, -1)
#else
@ -107,5 +109,7 @@
#define PERF_TIMED_SECTION_END(x)
#define PERF_MEASURE_PERIOD(x)
#define PERF_TRACK_VALUE(x, y)
#define PERF_INCREMENT_VALUE(x)
#define PERF_DECREMENT_VALUE(x)
#endif /* PIOS_INCLUDE_INSTRUMENTATION */
#endif /* PIOS_INSTRUMENTATION_HELPER_H */

2
flight/pios/inc/pios_mem.h
View File

@ -32,6 +32,8 @@ void *pios_fastheapmalloc(size_t size);
void *pios_malloc(size_t size);
void *pios_realloc(void *ptr, size_t size);
void pios_free(void *p);
#endif /* PIOS_MEM_H */

1
flight/pios/inc/pios_mpxv.h
View File

@ -38,6 +38,7 @@ typedef struct {
uint16_t calibrationCount;
uint32_t calibrationSum;
uint16_t zeroPoint;
float scale;
float maxSpeed;
} PIOS_MPXV_descriptor;

2
flight/pios/inc/pios_posix.h
View File

@ -28,7 +28,9 @@
#include <stdint.h>
#ifndef __cplusplus
typedef enum { FALSE = 0, TRUE = !FALSE } bool;
#endif
#ifndef false
#define false FALSE

2
flight/pios/inc/pios_rcvr.h
View File

@ -35,10 +35,12 @@ struct pios_rcvr_driver {
void (*init)(uint32_t id);
int32_t (*read)(uint32_t id, uint8_t channel);
xSemaphoreHandle (*get_semaphore)(uint32_t id, uint8_t channel);
uint8_t (*get_quality)(uint32_t id);
};
/* Public Functions */
extern int32_t PIOS_RCVR_Read(uint32_t rcvr_id, uint8_t channel);
extern uint8_t PIOS_RCVR_GetQuality(uint32_t rcvr_id);
extern xSemaphoreHandle PIOS_RCVR_GetSemaphore(uint32_t rcvr_id, uint8_t channel);
/*! Define error codes for PIOS_RCVR_Get */

33
flight/targets/boards/coptercontrol/firmware/inc/openpilot.h → flight/pios/inc/pios_srxl.h
View File

@ -1,12 +1,14 @@
/**
******************************************************************************
* @addtogroup OpenPilotSystem OpenPilot System
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup OpenPilotCore OpenPilot Core
* @addtogroup PIOS_SRXL Multiplex SRXL receiver functions
* @brief Code to read Multiplex SRXL receiver serial stream
* @{
* @file openpilot.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Main OpenPilot header.
*
* @file pios_srxl.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Code to read Multiplex SRXL receiver serial stream
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
@ -26,25 +28,14 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef OPENPILOT_H
#define OPENPILOT_H
#ifndef PIOS_SRXL_H
#define PIOS_SRXL_H
/* PIOS Includes */
#include <pios.h>
/* Global Types */
/* OpenPilot Libraries */
#include <utlist.h>
#include <uavobjectmanager.h>
#include <eventdispatcher.h>
#include <uavtalk.h>
/* Public Functions */
#include "alarms.h"
#include <mathmisc.h>
/* Global Functions */
void OpenPilotInit(void);
#endif /* OPENPILOT_H */
#endif /* PIOS_SRXL_H */
/**
* @}

95
flight/pios/inc/pios_srxl_priv.h Normal file
View File

@ -0,0 +1,95 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_SRXL Multiplex SRXL receiver functions
* @brief Code to read Multiplex SRXL receiver serial stream
* @{
*
* @file pios_srxl_priv.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Code to read Multiplex SRXL receiver serial stream
* @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_SRXL_PRIV_H
#define PIOS_SRXL_PRIV_H
#include <pios.h>
#include <pios_stm32.h>
#include <pios_usart_priv.h>
/*
* Multiplex SRXL serial port settings:
* 115200bps inverted serial stream, 8 bits, no parity, 1 stop bits
* frame period is 14ms (FastResponse ON) or 21ms (FastResponse OFF)
*
* Frame structure:
* 1 byte - start and version
* 0xa1 (v1 12-channels)
* 0xa2 (v2 16-channels)
* 24/32 bytes - channel data (4 + 12 bit/channel, 12/16 channels, MSB first)
* 16 bits per channel. 4 first reserved/not used. 12 bits channel data in
* 4095 steps, 0x000(800µs) - 0x800(1500µs) - 0xfff(2200µs)
* 2 bytes checksum (calculated over all bytes including start and version)
*
* Checksum calculation:
* u16 CRC16(u16 crc, u8 value) {
* u8 i;
* crc = crc ^ (s16)value << 8;
* for(i = 0; i < 8; i++) {
* if(crc & 0x8000) {
* crc = crc << 1 ^ 0x1021;
* } else {
* crc = crc << 1;
* }
* }
* return crc;
* }
*/
#define SRXL_V1_HEADER 0xa1
#define SRXL_V2_HEADER 0xa2
#define SRXL_HEADER_LENGTH 1
#define SRXL_CHECKSUM_LENGTH 2
#define SRXL_V1_CHANNEL_DATA_BYTES (12 * 2)
#define SRXL_V2_CHANNEL_DATA_BYTES (16 * 2)
#define SRXL_FRAME_LENGTH (SRXL_HEADER_LENGTH + SRXL_V2_CHANNEL_DATA_BYTES + SRXL_CHECKSUM_LENGTH)
/*
* Multiplex SRXL protocol provides 16 proportional channels.
* Do not change unless driver code is updated accordingly.
*/
#if (PIOS_SRXL_NUM_INPUTS != 16)
#error "Multiplex SRXL protocol provides 16 proportional channels."
#endif
extern const struct pios_rcvr_driver pios_srxl_rcvr_driver;
extern int32_t PIOS_SRXL_Init(uint32_t *srxl_id,
const struct pios_com_driver *driver,
uint32_t lower_id);
#endif /* PIOS_SRXL_PRIV_H */
/**
* @}
* @}
*/

1
flight/pios/inc/pios_usart_priv.h
View File

@ -44,6 +44,7 @@ struct pios_usart_cfg {
USART_InitTypeDef init;
struct stm32_gpio rx;
struct stm32_gpio tx;
struct stm32_gpio dtr;
struct stm32_irq irq;
};

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