rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-18 08:54:15 +01:00
Code Issues Releases Activity

Merge branch 'next' into kfinisterre/GMapsAndGeoCodeSearchFix

Browse Source
This commit is contained in:
abeck70 2015-05-13 17:46:05 +10:00
commit e8c948c45b
341 changed files with 27833 additions and 36476 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, ..."

19
WHATSNEW.txt
View File

@ -1,3 +1,22 @@
--- 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

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

@ -62,11 +62,7 @@ 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
@ -78,11 +74,35 @@ void plan_setup_assistedcontrol(uint8_t timeout_occurred);
#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

375
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(false);
}
}
// 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);
@ -528,12 +830,16 @@ void plan_setup_assistedcontrol(uint8_t timeout_occurred)
PositionStateGet(&positionState);
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
FlightStatusAssistedControlStateOptions assistedControlFlightMode;
FlightStatusAssistedControlStateGet(&assistedControlFlightMode);
if (timeout_occurred) {
FlightStatusFlightModeOptions flightMode;
FlightStatusFlightModeGet(&flightMode);
if (flightMode == FLIGHTSTATUS_FLIGHTMODE_LAND) {
plan_setup_land_helper(&pathDesired);
} else {
pathDesired.End.North = positionState.North;
pathDesired.End.East = positionState.East;
pathDesired.End.Down = positionState.Down;
@ -542,7 +848,8 @@ void plan_setup_assistedcontrol(uint8_t timeout_occurred)
pathDesired.Start.Down = positionState.Down;
pathDesired.StartingVelocity = 0.0f;
pathDesired.EndingVelocity = 0.0f;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
pathDesired.Mode = PATHDESIRED_MODE_GOTOENDPOINT;
}
assistedControlFlightMode = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD;
} else {
VelocityStateData velocityState;

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/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

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

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

@ -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
@ -366,9 +397,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 +419,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 +486,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 +521,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
}

33
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,8 +69,9 @@ 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
if ((flightModeAssist != FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) &&
(assistedControlFlightMode == FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY)) {
// Switch from primary (just entered this PH flight mode) into brake
plan_setup_assistedcontrol(false);
} 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();
@ -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)
{}

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

@ -0,0 +1,357 @@
/*
******************************************************************************
*
* @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);
}
}
// 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.

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

@ -41,10 +41,12 @@
#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 +74,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 +87,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);
@ -124,6 +128,7 @@ int32_t ReceiverStart()
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL);
#endif
SettingsUpdatedCb(NULL);
return 0;
}
@ -141,13 +146,43 @@ int32_t ReceiverInitialize()
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
*/
@ -243,22 +278,16 @@ 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
// 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 +300,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 +311,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 +374,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 +458,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
@ -552,6 +612,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;

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

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);

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;
}
/**

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);

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);

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 */

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

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 */
/**
* @}
* @}
*/

3
flight/pios/inc/pios_usb_defs.h
View File

@ -353,7 +353,6 @@ enum usb_cdc_notification {
enum usb_product_ids {
USB_PRODUCT_ID_OPENPILOT_MAIN = 0x415A,
USB_PRODUCT_ID_COPTERCONTROL = 0x415B,
USB_PRODUCT_ID_OPLINK = 0x415C,
USB_PRODUCT_ID_CC3D = 0x415D,
USB_PRODUCT_ID_REVOLUTION = 0x415E,
@ -365,7 +364,7 @@ enum usb_op_board_ids {
USB_OP_BOARD_ID_OPENPILOT_MAIN = 1,
/* Board ID 2 may be unused or AHRS */
USB_OP_BOARD_ID_OPLINK = 3,
USB_OP_BOARD_ID_COPTERCONTROL = 4,
/* USB_OP_BOARD_ID_COPTERCONTROL = 4, */
USB_OP_BOARD_ID_REVOLUTION = 5,
USB_OP_BOARD_ID_OSD = 6,
} __attribute__((packed));

4
flight/pios/pios.h
View File

@ -228,6 +228,10 @@ extern "C" {
#include <pios_sbus.h>
#endif
#ifdef PIOS_INCLUDE_SRXL
#include <pios_srxl.h>
#endif
/* PIOS abstract receiver interface */
#ifdef PIOS_INCLUDE_RCVR
#include <pios_rcvr.h>

29
flight/pios/stm32f4xx/pios_adc.c
View File

@ -99,10 +99,11 @@ static void init_dma(void);
static void init_adc(void);
#endif
struct dma_config {
struct pios_adc_pin_config {
GPIO_TypeDef *port;
uint32_t pin;
uint32_t channel;
bool initialize;
};
struct adc_accumulator {
@ -111,7 +112,7 @@ struct adc_accumulator {
};
#if defined(PIOS_INCLUDE_ADC)
static const struct dma_config config[] = PIOS_DMA_PIN_CONFIG;
static const struct pios_adc_pin_config config[] = PIOS_DMA_PIN_CONFIG;
#define PIOS_ADC_NUM_PINS (sizeof(config) / sizeof(config[0]))
static struct adc_accumulator accumulator[PIOS_ADC_NUM_PINS];
@ -123,18 +124,11 @@ static uint16_t adc_raw_buffer[2][PIOS_ADC_MAX_SAMPLES][PIOS_ADC_NUM_PINS];
#if defined(PIOS_INCLUDE_ADC)
static void init_pins(void)
{
/* Setup analog pins */
GPIO_InitTypeDef GPIO_InitStructure;
GPIO_StructInit(&GPIO_InitStructure);
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AN;
for (uint32_t i = 0; i < PIOS_ADC_NUM_PINS; ++i) {
if (config[i].port == NULL) {
if (!config[i].initialize) {
continue;
}
GPIO_InitStructure.GPIO_Pin = config[i].pin;
GPIO_Init(config[i].port, &GPIO_InitStructure);
PIOS_ADC_PinSetup(i);
}
}
@ -454,6 +448,19 @@ void PIOS_ADC_DMA_Handler(void)
#endif
}
void PIOS_ADC_PinSetup(uint32_t pin)
{
if (config[pin].port != NULL && pin < PIOS_ADC_NUM_PINS) {
/* Setup analog pin */
GPIO_InitTypeDef GPIO_InitStructure;
GPIO_StructInit(&GPIO_InitStructure);
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AN;
GPIO_InitStructure.GPIO_Pin = config[pin].pin;
GPIO_Init(config[pin].port, &GPIO_InitStructure);
}
}
#endif /* PIOS_INCLUDE_ADC */
/**

23
flight/targets/boards/coptercontrol/board-info.mk
View File

@ -1,23 +0,0 @@
BOARD_TYPE := 0x04
BOARD_REVISION := 0x02
BOOTLOADER_VERSION := 0x04
HW_TYPE := 0x01
MCU := cortex-m3
CHIP := STM32F103CBT
BOARD := STM32103CB_CC_Rev1
MODEL := MD
MODEL_SUFFIX := _CC
OPENOCD_JTAG_CONFIG := foss-jtag.revb.cfg
OPENOCD_CONFIG := stm32f1x.cfg
# Note: These must match the values in link_$(BOARD)_memory.ld
BL_BANK_BASE := 0x08000000 # Start of bootloader flash
BL_BANK_SIZE := 0x00003000 # Should include BD_INFO region
FW_BANK_BASE := 0x08003000 # Start of firmware flash
FW_BANK_SIZE := 0x0001D000 # Should include FW_DESC_SIZE
FW_DESC_SIZE := 0x00000064
OSCILLATOR_FREQ := 8000000

1525
flight/targets/boards/coptercontrol/board_hw_defs.c
View File

@ -1,1525 +0,0 @@
/**
******************************************************************************
* @file board_hw_defs.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @author PhoenixPilot, http://github.com/PhoenixPilot, Copyright (C) 2012
* @addtogroup OpenPilotSystem OpenPilot System
* @{
* @addtogroup OpenPilotCore OpenPilot Core
* @{
* @brief Defines board specific static initializers for hardware for the CopterControl board.
*****************************************************************************/
/*
* 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
*/
#define BOARD_REVISION_CC 1
#define BOARD_REVISION_CC3D 2
#if defined(PIOS_INCLUDE_LED)
#include <pios_led_priv.h>
static const struct pios_gpio pios_leds_cc[] = {
[PIOS_LED_HEARTBEAT] = {
.pin = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_6,
.GPIO_Mode = GPIO_Mode_Out_PP,
.GPIO_Speed = GPIO_Speed_50MHz,
},
},
.active_low = true
},
};
static const struct pios_gpio_cfg pios_led_cfg_cc = {
.gpios = pios_leds_cc,
.num_gpios = NELEMENTS(pios_leds_cc),
};
static const struct pios_gpio pios_leds_cc3d[] = {
[PIOS_LED_HEARTBEAT] = {
.pin = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_3,
.GPIO_Mode = GPIO_Mode_Out_PP,
.GPIO_Speed = GPIO_Speed_50MHz,
},
},
.remap = GPIO_Remap_SWJ_JTAGDisable,
.active_low = true
},
};
static const struct pios_gpio_cfg pios_led_cfg_cc3d = {
.gpios = pios_leds_cc3d,
.num_gpios = NELEMENTS(pios_leds_cc3d),
};
const struct pios_gpio_cfg *PIOS_BOARD_HW_DEFS_GetLedCfg(uint32_t board_revision)
{
switch (board_revision) {
case BOARD_REVISION_CC: return &pios_led_cfg_cc;
case BOARD_REVISION_CC3D: return &pios_led_cfg_cc3d;
default: return NULL;
}
}
#endif /* PIOS_INCLUDE_LED */
#if defined(PIOS_INCLUDE_SPI)
#include <pios_spi_priv.h>
/* Gyro interface */
void PIOS_SPI_gyro_irq_handler(void);
void DMA1_Channel2_IRQHandler() __attribute__((alias("PIOS_SPI_gyro_irq_handler")));
void DMA1_Channel3_IRQHandler() __attribute__((alias("PIOS_SPI_gyro_irq_handler")));
static const struct pios_spi_cfg pios_spi_gyro_cfg = {
.regs = SPI1,
.init = {
.SPI_Mode = SPI_Mode_Master,
.SPI_Direction = SPI_Direction_2Lines_FullDuplex,
.SPI_DataSize = SPI_DataSize_8b,
.SPI_NSS = SPI_NSS_Soft,
.SPI_FirstBit = SPI_FirstBit_MSB,
.SPI_CRCPolynomial = 7,
.SPI_CPOL = SPI_CPOL_High,
.SPI_CPHA = SPI_CPHA_2Edge,
.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_16, /* 10 Mhz */
},
.use_crc = false,
.dma = {
.ahb_clk = RCC_AHBPeriph_DMA1,
.irq = {
.flags = (DMA1_FLAG_TC2 | DMA1_FLAG_TE2 | DMA1_FLAG_HT2 | DMA1_FLAG_GL2),
.init = {
.NVIC_IRQChannel = DMA1_Channel2_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.channel = DMA1_Channel2,
.init = {
.DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR),
.DMA_DIR = DMA_DIR_PeripheralSRC,
.DMA_PeripheralInc = DMA_PeripheralInc_Disable,
.DMA_MemoryInc = DMA_MemoryInc_Enable,
.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte,
.DMA_Mode = DMA_Mode_Normal,
.DMA_Priority = DMA_Priority_Medium,
.DMA_M2M = DMA_M2M_Disable,
},
},
.tx = {
.channel = DMA1_Channel3,
.init = {
.DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR),
.DMA_DIR = DMA_DIR_PeripheralDST,
.DMA_PeripheralInc = DMA_PeripheralInc_Disable,
.DMA_MemoryInc = DMA_MemoryInc_Enable,
.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte,
.DMA_Mode = DMA_Mode_Normal,
.DMA_Priority = DMA_Priority_Medium,
.DMA_M2M = DMA_M2M_Disable,
},
},
},
.sclk = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_5,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_AF_PP,
},
},
.miso = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_6,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_IPU,
},
},
.mosi = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_7,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_AF_PP,
},
},
.slave_count = 1,
.ssel = {
{
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_4,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_Out_PP,
},
}
},
};
static uint32_t pios_spi_gyro_id;
void PIOS_SPI_gyro_irq_handler(void)
{
/* Call into the generic code to handle the IRQ for this specific device */
PIOS_SPI_IRQ_Handler(pios_spi_gyro_id);
}
/* Flash/Accel Interface
*
* NOTE: Leave this declared as const data so that it ends up in the
* .rodata section (ie. Flash) rather than in the .bss section (RAM).
*/
void PIOS_SPI_flash_accel_irq_handler(void);
void DMA1_Channel4_IRQHandler() __attribute__((alias("PIOS_SPI_flash_accel_irq_handler")));
void DMA1_Channel5_IRQHandler() __attribute__((alias("PIOS_SPI_flash_accel_irq_handler")));
static const struct pios_spi_cfg pios_spi_flash_accel_cfg_cc3d = {
.regs = SPI2,
.init = {
.SPI_Mode = SPI_Mode_Master,
.SPI_Direction = SPI_Direction_2Lines_FullDuplex,
.SPI_DataSize = SPI_DataSize_8b,
.SPI_NSS = SPI_NSS_Soft,
.SPI_FirstBit = SPI_FirstBit_MSB,
.SPI_CRCPolynomial = 7,
.SPI_CPOL = SPI_CPOL_High,
.SPI_CPHA = SPI_CPHA_2Edge,
.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8,
},
.use_crc = false,
.dma = {
.ahb_clk = RCC_AHBPeriph_DMA1,
.irq = {
.flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4),
.init = {
.NVIC_IRQChannel = DMA1_Channel4_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.channel = DMA1_Channel4,
.init = {
.DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR),
.DMA_DIR = DMA_DIR_PeripheralSRC,
.DMA_PeripheralInc = DMA_PeripheralInc_Disable,
.DMA_MemoryInc = DMA_MemoryInc_Enable,
.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte,
.DMA_Mode = DMA_Mode_Normal,
.DMA_Priority = DMA_Priority_High,
.DMA_M2M = DMA_M2M_Disable,
},
},
.tx = {
.channel = DMA1_Channel5,
.init = {
.DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR),
.DMA_DIR = DMA_DIR_PeripheralDST,
.DMA_PeripheralInc = DMA_PeripheralInc_Disable,
.DMA_MemoryInc = DMA_MemoryInc_Enable,
.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte,
.DMA_Mode = DMA_Mode_Normal,
.DMA_Priority = DMA_Priority_High,
.DMA_M2M = DMA_M2M_Disable,
},
},
},
.sclk = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_13,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_AF_PP,
},
},
.miso = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_14,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_IN_FLOATING,
},
},
.mosi = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_15,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_AF_PP,
},
},
.slave_count = 2,
.ssel = {
{
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_12,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_Out_PP,
}
},{
.gpio = GPIOC,
.init = {
.GPIO_Pin = GPIO_Pin_15,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_Out_PP,
},
}
},
};
static const struct pios_spi_cfg pios_spi_flash_accel_cfg_cc = {
.regs = SPI2,
.init = {
.SPI_Mode = SPI_Mode_Master,
.SPI_Direction = SPI_Direction_2Lines_FullDuplex,
.SPI_DataSize = SPI_DataSize_8b,
.SPI_NSS = SPI_NSS_Soft,
.SPI_FirstBit = SPI_FirstBit_MSB,
.SPI_CRCPolynomial = 7,
.SPI_CPOL = SPI_CPOL_High,
.SPI_CPHA = SPI_CPHA_2Edge,
.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8,
},
.use_crc = false,
.dma = {
.ahb_clk = RCC_AHBPeriph_DMA1,
.irq = {
.flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4),
.init = {
.NVIC_IRQChannel = DMA1_Channel4_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.channel = DMA1_Channel4,
.init = {
.DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR),
.DMA_DIR = DMA_DIR_PeripheralSRC,
.DMA_PeripheralInc = DMA_PeripheralInc_Disable,
.DMA_MemoryInc = DMA_MemoryInc_Enable,
.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte,
.DMA_Mode = DMA_Mode_Normal,
.DMA_Priority = DMA_Priority_High,
.DMA_M2M = DMA_M2M_Disable,
},
},
.tx = {
.channel = DMA1_Channel5,
.init = {
.DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR),
.DMA_DIR = DMA_DIR_PeripheralDST,
.DMA_PeripheralInc = DMA_PeripheralInc_Disable,
.DMA_MemoryInc = DMA_MemoryInc_Enable,
.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte,
.DMA_Mode = DMA_Mode_Normal,
.DMA_Priority = DMA_Priority_High,
.DMA_M2M = DMA_M2M_Disable,
},
},
},
.sclk = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_13,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_AF_PP,
},
},
.miso = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_14,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_IN_FLOATING,
},
},
.mosi = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_15,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_AF_PP,
},
},
.slave_count = 2,
.ssel = {
{
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_12,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_Out_PP,
}
},{
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_7,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_Out_PP,
},
}
},
};
static uint32_t pios_spi_flash_accel_id;
void PIOS_SPI_flash_accel_irq_handler(void)
{
/* Call into the generic code to handle the IRQ for this specific device */
PIOS_SPI_IRQ_Handler(pios_spi_flash_accel_id);
}
#endif /* PIOS_INCLUDE_SPI */
#if defined(PIOS_INCLUDE_FLASH)
#include "pios_flashfs_logfs_priv.h"
#include "pios_flash_jedec_priv.h"
static const struct flashfs_logfs_cfg flashfs_w25x_cfg = {
.fs_magic = 0x99abcdef,
.total_fs_size = 0x00080000, /* 512K bytes (128 sectors = entire chip) */
.arena_size = 0x00010000, /* 256 * slot size */
.slot_size = 0x00000100, /* 256 bytes */
.start_offset = 0, /* start at the beginning of the chip */
.sector_size = 0x00001000, /* 4K bytes */
.page_size = 0x00000100, /* 256 bytes */
};
static const struct flashfs_logfs_cfg flashfs_m25p_cfg = {
.fs_magic = 0x99abceef,
.total_fs_size = 0x00200000, /* 2M bytes (32 sectors = entire chip) */
.arena_size = 0x00010000, /* 256 * slot size */
.slot_size = 0x00000100, /* 256 bytes */
.start_offset = 0, /* start at the beginning of the chip */
.sector_size = 0x00010000, /* 64K bytes */
.page_size = 0x00000100, /* 256 bytes */
};
#include "pios_flash.h"
#endif /* PIOS_INCLUDE_FLASH */
/*
* ADC system
*/
#if defined(PIOS_INCLUDE_ADC)
#include "pios_adc_priv.h"
extern void PIOS_ADC_handler(void);
void DMA1_Channel1_IRQHandler() __attribute__((alias("PIOS_ADC_handler")));
// Remap the ADC DMA handler to this one
static const struct pios_adc_cfg pios_adc_cfg = {
.dma = {
.ahb_clk = RCC_AHBPeriph_DMA1,
.irq = {
.flags = (DMA1_FLAG_TC1 | DMA1_FLAG_TE1 | DMA1_FLAG_HT1 | DMA1_FLAG_GL1),
.init = {
.NVIC_IRQChannel = DMA1_Channel1_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.channel = DMA1_Channel1,
.init = {
.DMA_PeripheralBaseAddr = (uint32_t)&ADC1->DR,
.DMA_DIR = DMA_DIR_PeripheralSRC,
.DMA_PeripheralInc = DMA_PeripheralInc_Disable,
.DMA_MemoryInc = DMA_MemoryInc_Enable,
.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word,
.DMA_MemoryDataSize = DMA_MemoryDataSize_Word,
.DMA_Mode = DMA_Mode_Circular,
.DMA_Priority = DMA_Priority_High,
.DMA_M2M = DMA_M2M_Disable,
},
}
},
.half_flag = DMA1_IT_HT1,
.full_flag = DMA1_IT_TC1,
};
void PIOS_ADC_handler()
{
PIOS_ADC_DMA_Handler();
}
#endif /* if defined(PIOS_INCLUDE_ADC) */
#include "pios_tim_priv.h"
static const TIM_TimeBaseInitTypeDef tim_1_2_3_4_time_base = {
.TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1,
.TIM_ClockDivision = TIM_CKD_DIV1,
.TIM_CounterMode = TIM_CounterMode_Up,
.TIM_Period = ((1000000 / PIOS_SERVO_UPDATE_HZ) - 1),
.TIM_RepetitionCounter = 0x0000,
};
static const struct pios_tim_clock_cfg tim_1_cfg = {
.timer = TIM1,
.time_base_init = &tim_1_2_3_4_time_base,
.irq = {
.init = {
.NVIC_IRQChannel = TIM1_CC_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
};
static const struct pios_tim_clock_cfg tim_2_cfg = {
.timer = TIM2,
.time_base_init = &tim_1_2_3_4_time_base,
.irq = {
.init = {
.NVIC_IRQChannel = TIM2_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
};
static const struct pios_tim_clock_cfg tim_3_cfg = {
.timer = TIM3,
.time_base_init = &tim_1_2_3_4_time_base,
.irq = {
.init = {
.NVIC_IRQChannel = TIM3_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
};
static const struct pios_tim_clock_cfg tim_4_cfg = {
.timer = TIM4,
.time_base_init = &tim_1_2_3_4_time_base,
.irq = {
.init = {
.NVIC_IRQChannel = TIM4_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
};
static const struct pios_tim_channel pios_tim_rcvrport_all_channels[] = {
{
.timer = TIM4,
.timer_chan = TIM_Channel_1,
.pin = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_6,
.GPIO_Mode = GPIO_Mode_IPD,
.GPIO_Speed = GPIO_Speed_2MHz,
},
},
},
{
.timer = TIM3,
.timer_chan = TIM_Channel_2,
.pin = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_5,
.GPIO_Mode = GPIO_Mode_IPD,
.GPIO_Speed = GPIO_Speed_2MHz,
},
},
.remap = GPIO_PartialRemap_TIM3,
},
{
.timer = TIM3,
.timer_chan = TIM_Channel_3,
.pin = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_0,
.GPIO_Mode = GPIO_Mode_IPD,
.GPIO_Speed = GPIO_Speed_2MHz,
},
},
},
{
.timer = TIM3,
.timer_chan = TIM_Channel_4,
.pin = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_1,
.GPIO_Mode = GPIO_Mode_IPD,
.GPIO_Speed = GPIO_Speed_2MHz,
},
},
},
{
.timer = TIM2,
.timer_chan = TIM_Channel_1,
.pin = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_0,
.GPIO_Mode = GPIO_Mode_IPD,
.GPIO_Speed = GPIO_Speed_2MHz,
},
},
},
{
.timer = TIM2,
.timer_chan = TIM_Channel_2,
.pin = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_1,
.GPIO_Mode = GPIO_Mode_IPD,
.GPIO_Speed = GPIO_Speed_2MHz,
},
},
},
};
static const struct pios_tim_channel pios_tim_servoport_all_pins[] = {
{
.timer = TIM4,
.timer_chan = TIM_Channel_4,
.pin = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_9,
.GPIO_Mode = GPIO_Mode_AF_PP,
.GPIO_Speed = GPIO_Speed_2MHz,
},
},
},
{
.timer = TIM4,
.timer_chan = TIM_Channel_3,
.pin = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_8,
.GPIO_Mode = GPIO_Mode_AF_PP,
.GPIO_Speed = GPIO_Speed_2MHz,
},
},
},
{
.timer = TIM4,
.timer_chan = TIM_Channel_2,
.pin = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_7,
.GPIO_Mode = GPIO_Mode_AF_PP,
.GPIO_Speed = GPIO_Speed_2MHz,
},
},
},
{
.timer = TIM1,
.timer_chan = TIM_Channel_1,
.pin = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_8,
.GPIO_Mode = GPIO_Mode_AF_PP,
.GPIO_Speed = GPIO_Speed_2MHz,
},
},
},
{
.timer = TIM3,
.timer_chan = TIM_Channel_1,
.pin = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_4,
.GPIO_Mode = GPIO_Mode_AF_PP,
.GPIO_Speed = GPIO_Speed_2MHz,
},
},
.remap = GPIO_PartialRemap_TIM3,
},
{
.timer = TIM2,
.timer_chan = TIM_Channel_3,
.pin = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_2,
.GPIO_Mode = GPIO_Mode_AF_PP,
.GPIO_Speed = GPIO_Speed_2MHz,
},
},
},
};
static const struct pios_tim_channel pios_tim_servoport_rcvrport_pins[] = {
{
.timer = TIM4,
.timer_chan = TIM_Channel_4,
.pin = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_9,
.GPIO_Mode = GPIO_Mode_AF_PP,
.GPIO_Speed = GPIO_Speed_2MHz,
},
},
},
{
.timer = TIM4,
.timer_chan = TIM_Channel_3,
.pin = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_8,
.GPIO_Mode = GPIO_Mode_AF_PP,
.GPIO_Speed = GPIO_Speed_2MHz,
},
},
},
{
.timer = TIM4,
.timer_chan = TIM_Channel_2,
.pin = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_7,
.GPIO_Mode = GPIO_Mode_AF_PP,
.GPIO_Speed = GPIO_Speed_2MHz,
},
},
},
{
.timer = TIM1,
.timer_chan = TIM_Channel_1,
.pin = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_8,
.GPIO_Mode = GPIO_Mode_AF_PP,
.GPIO_Speed = GPIO_Speed_2MHz,
},
},
},
{
.timer = TIM3,
.timer_chan = TIM_Channel_1,
.pin = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_4,
.GPIO_Mode = GPIO_Mode_AF_PP,
.GPIO_Speed = GPIO_Speed_2MHz,
},
},
.remap = GPIO_PartialRemap_TIM3,
},
{
.timer = TIM2,
.timer_chan = TIM_Channel_3,
.pin = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_2,
.GPIO_Mode = GPIO_Mode_AF_PP,
.GPIO_Speed = GPIO_Speed_2MHz,
},
},
},
// Receiver port pins
// S3-S6 inputs are used as outputs in this case
{
.timer = TIM3,
.timer_chan = TIM_Channel_3,
.pin = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_0,
.GPIO_Mode = GPIO_Mode_AF_PP,
.GPIO_Speed = GPIO_Speed_2MHz,
},
},
},
{
.timer = TIM3,
.timer_chan = TIM_Channel_4,
.pin = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_1,
.GPIO_Mode = GPIO_Mode_AF_PP,
.GPIO_Speed = GPIO_Speed_2MHz,
},
},
},
{
.timer = TIM2,
.timer_chan = TIM_Channel_1,
.pin = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_0,
.GPIO_Mode = GPIO_Mode_AF_PP,
.GPIO_Speed = GPIO_Speed_2MHz,
},
},
},
{
.timer = TIM2,
.timer_chan = TIM_Channel_2,
.pin = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_1,
.GPIO_Mode = GPIO_Mode_AF_PP,
.GPIO_Speed = GPIO_Speed_2MHz,
},
},
},
};
static const struct pios_tim_channel pios_tim_ppm_flexi_port = {
.timer = TIM2,
.timer_chan = TIM_Channel_4,
.pin = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_11,
.GPIO_Mode = GPIO_Mode_IPD,
.GPIO_Speed = GPIO_Speed_2MHz,
},
},
.remap = GPIO_PartialRemap2_TIM2,
};
#if defined(PIOS_INCLUDE_USART)
#include "pios_usart_priv.h"
static const struct pios_usart_cfg pios_usart_generic_main_cfg = {
.regs = USART1,
.init = {
.USART_BaudRate = 57600,
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1,
.USART_HardwareFlowControl = USART_HardwareFlowControl_None,
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
},
.irq = {
.init = {
.NVIC_IRQChannel = USART1_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_IPU,
},
},
.tx = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_9,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF_PP,
},
},
};
static const struct pios_usart_cfg pios_usart_generic_flexi_cfg = {
.regs = USART3,
.init = {
.USART_BaudRate = 57600,
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1,
.USART_HardwareFlowControl = USART_HardwareFlowControl_None,
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
},
.irq = {
.init = {
.NVIC_IRQChannel = USART3_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_11,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_IPU,
},
},
.tx = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF_PP,
},
},
};
#if defined(PIOS_INCLUDE_DSM)
/*
* Spektrum/JR DSM USART
*/
#include <pios_dsm_priv.h>
static const struct pios_usart_cfg pios_usart_dsm_main_cfg = {
.regs = USART1,
.init = {
.USART_BaudRate = 115200,
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1,
.USART_HardwareFlowControl = USART_HardwareFlowControl_None,
.USART_Mode = USART_Mode_Rx,
},
.irq = {
.init = {
.NVIC_IRQChannel = USART1_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_IPU,
},
},
.tx = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_9,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_IN_FLOATING,
},
},
};
static const struct pios_dsm_cfg pios_dsm_main_cfg = {
.bind = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_Out_PP,
},
},
};
static const struct pios_usart_cfg pios_usart_dsm_flexi_cfg = {
.regs = USART3,
.init = {
.USART_BaudRate = 115200,
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1,
.USART_HardwareFlowControl = USART_HardwareFlowControl_None,
.USART_Mode = USART_Mode_Rx,
},
.irq = {
.init = {
.NVIC_IRQChannel = USART3_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_11,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_IPU,
},
},
.tx = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_IN_FLOATING,
},
},
};
static const struct pios_dsm_cfg pios_dsm_flexi_cfg = {
.bind = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_11,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_Out_PP,
},
},
};
#endif /* PIOS_INCLUDE_DSM */
#if defined(PIOS_INCLUDE_SBUS)
/*
* S.Bus USART
*/
#include <pios_sbus_priv.h>
static const struct pios_usart_cfg pios_usart_sbus_main_cfg = {
.regs = USART1,
.init = {
.USART_BaudRate = 100000,
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_Even,
.USART_StopBits = USART_StopBits_2,
.USART_HardwareFlowControl = USART_HardwareFlowControl_None,
.USART_Mode = USART_Mode_Rx,
},
.irq = {
.init = {
.NVIC_IRQChannel = USART1_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_IPU,
},
},
.tx = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_9,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_IN_FLOATING,
},
},
};
static const struct pios_sbus_cfg pios_sbus_cfg = {
/* Inverter configuration */
.inv = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_2,
.GPIO_Mode = GPIO_Mode_Out_PP,
.GPIO_Speed = GPIO_Speed_2MHz,
},
},
.gpio_clk_func = RCC_APB2PeriphClockCmd,
.gpio_clk_periph = RCC_APB2Periph_GPIOB,
.gpio_inv_enable = Bit_SET,
};
#endif /* PIOS_INCLUDE_SBUS */
/*
* HK OSD
*/
static const struct pios_usart_cfg pios_usart_hkosd_main_cfg = {
.regs = USART1,
.init = {
.USART_BaudRate = 57600,
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1,
.USART_HardwareFlowControl = USART_HardwareFlowControl_None,
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
},
.irq = {
.init = {
.NVIC_IRQChannel = USART1_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_IPU,
},
},
.tx = {
.gpio = GPIOA,
.init = {
.GPIO_Pin = GPIO_Pin_9,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF_PP,
},
},
};
static const struct pios_usart_cfg pios_usart_hkosd_flexi_cfg = {
.regs = USART3,
.init = {
.USART_BaudRate = 57600,
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1,
.USART_HardwareFlowControl = USART_HardwareFlowControl_None,
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
},
.irq = {
.init = {
.NVIC_IRQChannel = USART3_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_11,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_IPU,
},
},
.tx = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF_PP,
},
},
};
#endif /* PIOS_INCLUDE_USART */
#if defined(PIOS_INCLUDE_COM)
#include "pios_com_priv.h"
#endif /* PIOS_INCLUDE_COM */
#if defined(PIOS_INCLUDE_RTC)
/*
* Realtime Clock (RTC)
*/
#include <pios_rtc_priv.h>
void PIOS_RTC_IRQ_Handler(void);
void RTC_IRQHandler() __attribute__((alias("PIOS_RTC_IRQ_Handler")));
static const struct pios_rtc_cfg pios_rtc_main_cfg = {
.clksrc = RCC_RTCCLKSource_HSE_Div128,
.prescaler = 100,
.irq = {
.init = {
.NVIC_IRQChannel = RTC_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
};
void PIOS_RTC_IRQ_Handler(void)
{
PIOS_RTC_irq_handler();
}
#endif /* if defined(PIOS_INCLUDE_RTC) */
#if defined(PIOS_INCLUDE_SERVO) && defined(PIOS_INCLUDE_TIM)
/*
* Servo outputs
*/
#include <pios_servo_priv.h>
const struct pios_servo_cfg pios_servo_cfg = {
.tim_oc_init = {
.TIM_OCMode = TIM_OCMode_PWM1,
.TIM_OutputState = TIM_OutputState_Enable,
.TIM_OutputNState = TIM_OutputNState_Disable,
.TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION,
.TIM_OCPolarity = TIM_OCPolarity_High,
.TIM_OCNPolarity = TIM_OCPolarity_High,
.TIM_OCIdleState = TIM_OCIdleState_Reset,
.TIM_OCNIdleState = TIM_OCNIdleState_Reset,
},
.channels = pios_tim_servoport_all_pins,
.num_channels = NELEMENTS(pios_tim_servoport_all_pins),
};
const struct pios_servo_cfg pios_servo_rcvr_cfg = {
.tim_oc_init = {
.TIM_OCMode = TIM_OCMode_PWM1,
.TIM_OutputState = TIM_OutputState_Enable,
.TIM_OutputNState = TIM_OutputNState_Disable,
.TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION,
.TIM_OCPolarity = TIM_OCPolarity_High,
.TIM_OCNPolarity = TIM_OCPolarity_High,
.TIM_OCIdleState = TIM_OCIdleState_Reset,
.TIM_OCNIdleState = TIM_OCNIdleState_Reset,
},
.channels = pios_tim_servoport_rcvrport_pins,
.num_channels = NELEMENTS(pios_tim_servoport_rcvrport_pins),
};
#endif /* PIOS_INCLUDE_SERVO && PIOS_INCLUDE_TIM */
/*
* PPM Inputs
*/
#if defined(PIOS_INCLUDE_PPM)
#include <pios_ppm_priv.h>
const struct pios_ppm_cfg pios_ppm_cfg = {
.tim_ic_init = {
.TIM_ICPolarity = TIM_ICPolarity_Rising,
.TIM_ICSelection = TIM_ICSelection_DirectTI,
.TIM_ICPrescaler = TIM_ICPSC_DIV1,
.TIM_ICFilter = 0x0,
},
/* Use only the first channel for ppm */
.channels = &pios_tim_rcvrport_all_channels[0],
.num_channels = 1,
};
const struct pios_ppm_cfg pios_ppm_pin8_cfg = {
.tim_ic_init = {
.TIM_ICPolarity = TIM_ICPolarity_Rising,
.TIM_ICSelection = TIM_ICSelection_DirectTI,
.TIM_ICPrescaler = TIM_ICPSC_DIV1,
.TIM_ICFilter = 0x0,
},
/* Use only the first channel for ppm */
.channels = &pios_tim_rcvrport_all_channels[5],
.num_channels = 1,
};
#endif /* PIOS_INCLUDE_PPM */
#if defined(PIOS_INCLUDE_PPM_FLEXI)
#include <pios_ppm_priv.h>
const struct pios_ppm_cfg pios_ppm_flexi_cfg = {
.tim_ic_init = {
.TIM_ICPolarity = TIM_ICPolarity_Rising,
.TIM_ICSelection = TIM_ICSelection_DirectTI,
.TIM_ICPrescaler = TIM_ICPSC_DIV1,
.TIM_ICFilter = 0x0,
},
.channels = &pios_tim_ppm_flexi_port,
.num_channels = 1,
};
#endif /* PIOS_INCLUDE_PPM_FLEXI */
/*
* PWM Inputs
*/
#if defined(PIOS_INCLUDE_PWM)
#include <pios_pwm_priv.h>
const struct pios_pwm_cfg pios_pwm_cfg = {
.tim_ic_init = {
.TIM_ICPolarity = TIM_ICPolarity_Rising,
.TIM_ICSelection = TIM_ICSelection_DirectTI,
.TIM_ICPrescaler = TIM_ICPSC_DIV1,
.TIM_ICFilter = 0x0,
},
.channels = pios_tim_rcvrport_all_channels,
.num_channels = NELEMENTS(pios_tim_rcvrport_all_channels),
};
const struct pios_pwm_cfg pios_pwm_with_ppm_cfg = {
.tim_ic_init = {
.TIM_ICPolarity = TIM_ICPolarity_Rising,
.TIM_ICSelection = TIM_ICSelection_DirectTI,
.TIM_ICPrescaler = TIM_ICPSC_DIV1,
.TIM_ICFilter = 0x0,
},
/* Leave the first channel for PPM use and use the rest for PWM */
.channels = &pios_tim_rcvrport_all_channels[1],
.num_channels = NELEMENTS(pios_tim_rcvrport_all_channels) - 1,
};
#endif /* if defined(PIOS_INCLUDE_PWM) */
/*
* SONAR Inputs
*/
#if defined(PIOS_INCLUDE_HCSR04)
#include <pios_hcsr04_priv.h>
static const struct pios_tim_channel pios_tim_hcsr04_port_all_channels[] = {
{
.timer = TIM3,
.timer_chan = TIM_Channel_2,
.pin = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_5,
.GPIO_Mode = GPIO_Mode_IPD,
.GPIO_Speed = GPIO_Speed_2MHz,
},
},
.remap = GPIO_PartialRemap_TIM3,
},
};
const struct pios_hcsr04_cfg pios_hcsr04_cfg = {
.tim_ic_init = {
.TIM_ICPolarity = TIM_ICPolarity_Rising,
.TIM_ICSelection = TIM_ICSelection_DirectTI,
.TIM_ICPrescaler = TIM_ICPSC_DIV1,
.TIM_ICFilter = 0x0,
},
.channels = pios_tim_hcsr04_port_all_channels,
.num_channels = NELEMENTS(pios_tim_hcsr04_port_all_channels),
.trigger = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_6,
.GPIO_Mode = GPIO_Mode_Out_PP,
.GPIO_Speed = GPIO_Speed_2MHz,
},
},
};
#endif /* if defined(PIOS_INCLUDE_HCSR04) */
#if defined(PIOS_INCLUDE_I2C)
#include <pios_i2c_priv.h>
/*
* I2C Adapters
*/
void PIOS_I2C_flexi_adapter_ev_irq_handler(void);
void PIOS_I2C_flexi_adapter_er_irq_handler(void);
void I2C2_EV_IRQHandler() __attribute__((alias("PIOS_I2C_flexi_adapter_ev_irq_handler")));
void I2C2_ER_IRQHandler() __attribute__((alias("PIOS_I2C_flexi_adapter_er_irq_handler")));
static const struct pios_i2c_adapter_cfg pios_i2c_flexi_adapter_cfg = {
.regs = I2C2,
.init = {
.I2C_Mode = I2C_Mode_I2C,
.I2C_OwnAddress1 = 0,
.I2C_Ack = I2C_Ack_Enable,
.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit,
.I2C_DutyCycle = I2C_DutyCycle_2,
.I2C_ClockSpeed = 400000, /* bits/s */
},
.transfer_timeout_ms = 50,
.scl = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_10MHz,
.GPIO_Mode = GPIO_Mode_AF_OD,
},
},
.sda = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_11,
.GPIO_Speed = GPIO_Speed_10MHz,
.GPIO_Mode = GPIO_Mode_AF_OD,
},
},
.event = {
.flags = 0, /* FIXME: check this */
.init = {
.NVIC_IRQChannel = I2C2_EV_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.error = {
.flags = 0, /* FIXME: check this */
.init = {
.NVIC_IRQChannel = I2C2_ER_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
};
uint32_t pios_i2c_flexi_adapter_id;
void PIOS_I2C_flexi_adapter_ev_irq_handler(void)
{
/* Call into the generic code to handle the IRQ for this specific device */
PIOS_I2C_EV_IRQ_Handler(pios_i2c_flexi_adapter_id);
}
void PIOS_I2C_flexi_adapter_er_irq_handler(void)
{
/* Call into the generic code to handle the IRQ for this specific device */
PIOS_I2C_ER_IRQ_Handler(pios_i2c_flexi_adapter_id);
}
#endif /* PIOS_INCLUDE_I2C */
#if defined(PIOS_INCLUDE_GCSRCVR)
#include "pios_gcsrcvr_priv.h"
#endif /* PIOS_INCLUDE_GCSRCVR */
#if defined(PIOS_INCLUDE_RCVR)
#include "pios_rcvr_priv.h"
#endif /* PIOS_INCLUDE_RCVR */
#if defined(PIOS_INCLUDE_USB)
#include "pios_usb_priv.h"
static const struct pios_usb_cfg pios_usb_main_cfg_cc = {
.irq = {
.init = {
.NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.vsense = {
.gpio = GPIOC,
.init = {
.GPIO_Pin = GPIO_Pin_15,
.GPIO_Speed = GPIO_Speed_10MHz,
.GPIO_Mode = GPIO_Mode_AF_OD,
},
},
.vsense_active_low = false
};
static const struct pios_usb_cfg pios_usb_main_cfg_cc3d = {
.irq = {
.init = {
.NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.vsense = {
.gpio = GPIOC,
.init = {
.GPIO_Pin = GPIO_Pin_14,
.GPIO_Speed = GPIO_Speed_10MHz,
.GPIO_Mode = GPIO_Mode_AF_OD,
},
},
.vsense_active_low = false
};
#include "pios_usb_board_data_priv.h"
#include "pios_usb_desc_hid_cdc_priv.h"
#include "pios_usb_desc_hid_only_priv.h"
#endif /* PIOS_INCLUDE_USB */
#if defined(PIOS_INCLUDE_COM_MSG)
#include <pios_com_msg_priv.h>
#endif /* PIOS_INCLUDE_COM_MSG */
#if defined(PIOS_INCLUDE_USB_HID)
#include <pios_usb_hid_priv.h>
const struct pios_usb_hid_cfg pios_usb_hid_cfg = {
.data_if = 2,
.data_rx_ep = 1,
.data_tx_ep = 1,
};
#endif /* PIOS_INCLUDE_USB_HID */
#if defined(PIOS_INCLUDE_USB_RCTX)
#include <pios_usb_rctx_priv.h>
const struct pios_usb_rctx_cfg pios_usb_rctx_cfg = {
.data_if = 2,
.data_tx_ep = 1,
};
#endif /* PIOS_INCLUDE_USB_RCTX */
#if defined(PIOS_INCLUDE_USB_CDC)
#include <pios_usb_cdc_priv.h>
const struct pios_usb_cdc_cfg pios_usb_cdc_cfg = {
.ctrl_if = 0,
.ctrl_tx_ep = 2,
.data_if = 1,
.data_rx_ep = 3,
.data_tx_ep = 3,
};
#endif /* PIOS_INCLUDE_USB_CDC */

32
flight/targets/boards/coptercontrol/bootloader/Makefile
View File

@ -1,32 +0,0 @@
#
# Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org
#
# 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 OPENPILOT_IS_COOL
$(error Top level Makefile must be used to build this target)
endif
## The standard CMSIS startup
SRC += $(CMSIS_DEVICEDIR)/system_stm32f10x.c
include ../board-info.mk
include $(ROOT_DIR)/make/firmware-defs.mk
include $(ROOT_DIR)/make/boot-defs.mk
include $(ROOT_DIR)/make/common-defs.mk
$(info Making bootloader for CopterControl, board revision $(BOARD_REVISION))
$(info Use BOARD_REVISION=1 for CC, BOARD_REVISION=2 for CC3D (default))

115
flight/targets/boards/coptercontrol/bootloader/inc/common.h
View File

@ -1,115 +0,0 @@
/**
******************************************************************************
* @addtogroup CopterControlBL CopterControl BootLoader
* @brief These files contain the code to the CopterControl Bootloader.
*
* @{
* @file common.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief This file contains various common defines for the BootLoader
* @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 COMMON_H_
#define COMMON_H_
// #include "board.h"
typedef enum {
start, keepgoing,
} DownloadAction;
/**************************************************/
/* OP_DFU states */
/**************************************************/
typedef enum {
DFUidle, // 0
uploading, // 1
wrong_packet_received, // 2
too_many_packets, // 3
too_few_packets, // 4
Last_operation_Success, // 5
downloading, // 6
BLidle, // 7
Last_operation_failed, // 8
uploadingStarting, // 9
outsideDevCapabilities, // 10
CRC_Fail, // 11
failed_jump,
// 12
} DFUStates;
/**************************************************/
/* OP_DFU commands */
/**************************************************/
typedef enum {
Reserved, // 0
Req_Capabilities, // 1
Rep_Capabilities, // 2
EnterDFU, // 3
JumpFW, // 4
Reset, // 5
Abort_Operation, // 6
Upload, // 7
Op_END, // 8
Download_Req, // 9
Download, // 10
Status_Request, // 11
Status_Rep
// 12
} DFUCommands;
typedef enum {
High_Density, Medium_Density
} DeviceType;
/**************************************************/
/* OP_DFU transfer types */
/**************************************************/
typedef enum {
FW, // 0
Descript
// 2
} DFUTransfer;
/**************************************************/
/* OP_DFU transfer port */
/**************************************************/
typedef enum {
Usb, // 0
Serial
// 2
} DFUPort;
/**************************************************/
/* OP_DFU programable programable HW types */
/**************************************************/
typedef enum {
Self_flash, // 0
Remote_flash_via_spi
// 1
} DFUProgType;
/**************************************************/
/* OP_DFU programable sources */
/**************************************************/
#define USB 0
#define SPI 1
#define DownloadDelay 100000
#define MAX_DEL_RETRYS 3
#define MAX_WRI_RETRYS 3
#endif /* COMMON_H_ */

53
flight/targets/boards/coptercontrol/bootloader/inc/pios_usb_board_data.h
View File

@ -1,53 +0,0 @@
/**
******************************************************************************
* @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
* @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_USB_BOARD_DATA_H
#define PIOS_USB_BOARD_DATA_H
// Note : changing below length will require changes to the USB buffer setup
#define PIOS_USB_BOARD_HID_DATA_LENGTH 64
#define PIOS_USB_BOARD_EP_NUM 2
#include <pios_usb_defs.h> /* struct usb_* */
#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_BL)
#define PIOS_USB_BOARD_SN_SUFFIX "+BL"
/*
* The bootloader uses a simplified report structure
* BL: <REPORT_ID><DATA>...<DATA>
* FW: <REPORT_ID><LENGTH><DATA>...<DATA>
* This define changes the behaviour in pios_usb_hid.c
*/
#define PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE
#endif /* PIOS_USB_BOARD_DATA_H */

223
flight/targets/boards/coptercontrol/bootloader/main.c
View File

@ -1,223 +0,0 @@
/**
******************************************************************************
* @addtogroup CopterControlBL CopterControl BootLoader
* @brief These files contain the code to the CopterControl Bootloader.
*
* @{
* @file main.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief This is the file with the main function of the OpenPilot BootLoader
* @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>
#include <pios_board_info.h>
#include <op_dfu.h>
#include <usb_lib.h>
#include <pios_iap.h>
#include <fifo_buffer.h>
#include <pios_com_msg.h>
/* Prototype of PIOS_Board_Init() function */
extern void PIOS_Board_Init(void);
extern void FLASH_Download();
#define BSL_HOLD_STATE ((PIOS_USB_DETECT_GPIO_PORT->IDR & PIOS_USB_DETECT_GPIO_PIN) ? 0 : 1)
/* Private typedef -----------------------------------------------------------*/
typedef void (*pFunction)(void);
/* Private define ------------------------------------------------------------*/
/* Private macro -------------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
pFunction Jump_To_Application;
uint32_t JumpAddress;
/// LEDs PWM
uint32_t period1 = 5000; // 5 mS
uint32_t sweep_steps1 = 100; // * 5 mS -> 500 mS
uint32_t period2 = 5000; // 5 mS
uint32_t sweep_steps2 = 100; // * 5 mS -> 500 mS
////////////////////////////////////////
uint8_t tempcount = 0;
/* Extern variables ----------------------------------------------------------*/
DFUStates DeviceState;
int16_t status = 0;
uint8_t JumpToApp = FALSE;
uint8_t GO_dfu = FALSE;
uint8_t USB_connected = FALSE;
uint8_t User_DFU_request = FALSE;
static uint8_t mReceive_Buffer[63];
/* Private function prototypes -----------------------------------------------*/
uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count);
uint8_t processRX();
void jump_to_app();
int main()
{
PIOS_SYS_Init();
PIOS_Board_Init();
PIOS_IAP_Init();
USB_connected = PIOS_USB_CableConnected(0);
if (PIOS_IAP_CheckRequest() == TRUE) {
PIOS_DELAY_WaitmS(1000);
User_DFU_request = TRUE;
PIOS_IAP_ClearRequest();
}
GO_dfu = (USB_connected == TRUE) || (User_DFU_request == TRUE);
if (GO_dfu == TRUE) {
PIOS_Board_Init();
if (User_DFU_request == TRUE) {
DeviceState = DFUidle;
} else {
DeviceState = BLidle;
}
} else {
JumpToApp = TRUE;
}
uint32_t stopwatch = 0;
uint32_t prev_ticks = PIOS_DELAY_GetuS();
while (TRUE) {
/* Update the stopwatch */
uint32_t elapsed_ticks = PIOS_DELAY_GetuSSince(prev_ticks);
prev_ticks += elapsed_ticks;
stopwatch += elapsed_ticks;
if (JumpToApp == TRUE) {
jump_to_app();
}
switch (DeviceState) {
case Last_operation_Success:
case uploadingStarting:
case DFUidle:
period1 = 5000;
sweep_steps1 = 100;
PIOS_LED_Off(PIOS_LED_HEARTBEAT);
period2 = 0;
break;
case uploading:
period1 = 5000;
sweep_steps1 = 100;
period2 = 2500;
sweep_steps2 = 50;
break;
case downloading:
period1 = 2500;
sweep_steps1 = 50;
PIOS_LED_Off(PIOS_LED_HEARTBEAT);
period2 = 0;
break;
case BLidle:
period1 = 0;
PIOS_LED_On(PIOS_LED_HEARTBEAT);
period2 = 0;
break;
default: // error
period1 = 5000;
sweep_steps1 = 100;
period2 = 5000;
sweep_steps2 = 100;
}
if (period1 != 0) {
if (LedPWM(period1, sweep_steps1, stopwatch)) {
PIOS_LED_On(PIOS_LED_HEARTBEAT);
} else {
PIOS_LED_Off(PIOS_LED_HEARTBEAT);
}
} else {
PIOS_LED_On(PIOS_LED_HEARTBEAT);
}
if (period2 != 0) {
if (LedPWM(period2, sweep_steps2, stopwatch)) {
PIOS_LED_On(PIOS_LED_HEARTBEAT);
} else {
PIOS_LED_Off(PIOS_LED_HEARTBEAT);
}
} else {
PIOS_LED_Off(PIOS_LED_HEARTBEAT);
}
if (stopwatch > 50 * 1000 * 1000) {
stopwatch = 0;
}
if ((stopwatch > 6 * 1000 * 1000) && ((DeviceState == BLidle) || (DeviceState == DFUidle && !USB_connected))) {
JumpToApp = TRUE;
}
processRX();
DataDownload(start);
}
}
void jump_to_app()
{
const struct pios_board_info *bdinfo = &pios_board_info_blob;
if (((*(__IO uint32_t *)bdinfo->fw_base) & 0x2FFE0000) == 0x20000000) { /* Jump to user application */
FLASH_Lock();
RCC_APB2PeriphResetCmd(0xffffffff, ENABLE);
RCC_APB1PeriphResetCmd(0xffffffff, ENABLE);
RCC_APB2PeriphResetCmd(0xffffffff, DISABLE);
RCC_APB1PeriphResetCmd(0xffffffff, DISABLE);
_SetCNTR(0); // clear interrupt mask
_SetISTR(0); // clear all requests
JumpAddress = *(__IO uint32_t *)(bdinfo->fw_base + 4);
Jump_To_Application = (pFunction)JumpAddress;
/* Initialize user application's Stack Pointer */
__set_MSP(*(__IO uint32_t *)bdinfo->fw_base);
Jump_To_Application();
} else {
DeviceState = failed_jump;
return;
}
}
uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count)
{
uint32_t curr_step = (count / pwm_period) % pwm_sweep_steps; /* 0 - pwm_sweep_steps */
uint32_t pwm_duty = pwm_period * curr_step / pwm_sweep_steps; /* fraction of pwm_period */
uint32_t curr_sweep = (count / (pwm_period * pwm_sweep_steps)); /* ticks once per full sweep */
if (curr_sweep & 1) {
pwm_duty = pwm_period - pwm_duty; /* reverse direction in odd sweeps */
}
return ((count % pwm_period) > pwm_duty) ? 1 : 0;
}
uint8_t processRX()
{
if (PIOS_COM_MSG_Receive(PIOS_COM_TELEM_USB, mReceive_Buffer, sizeof(mReceive_Buffer))) {
processComand(mReceive_Buffer);
}
return TRUE;
}
int32_t platform_senddata(const uint8_t *msg, uint16_t msg_len)
{
return PIOS_COM_MSG_Send(PIOS_COM_TELEM_USB, msg, msg_len);
}

106
flight/targets/boards/coptercontrol/bootloader/pios_board.c
View File

@ -1,106 +0,0 @@
/**
******************************************************************************
*
* @file pios_board.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Defines board specific static initializers for hardware for the CopterControl board.
* @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>
#include <pios_board_info.h>
/*
* Pull in the board-specific static HW definitions.
* Including .c files is a bit ugly but this allows all of
* the HW definitions to be const and static to limit their
* scope.
*
* NOTE: THIS IS THE ONLY PLACE THAT SHOULD EVER INCLUDE THIS FILE
*/
#include "../board_hw_defs.c"
uint32_t pios_com_telem_usb_id;
/**
* PIOS_Board_Init()
* initializes all the core subsystems on this specific hardware
* called from System/openpilot.c
*/
static bool board_init_complete = false;
void PIOS_Board_Init(void)
{
if (board_init_complete) {
return;
}
/* Enable Prefetch Buffer */
FLASH_PrefetchBufferCmd(FLASH_PrefetchBuffer_Enable);
/* Flash 2 wait state */
FLASH_SetLatency(FLASH_Latency_2);
/* Delay system */
PIOS_DELAY_Init();
const struct pios_board_info *bdinfo = &pios_board_info_blob;
#if defined(PIOS_INCLUDE_LED)
const struct pios_gpio_cfg *led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev);
PIOS_Assert(led_cfg);
PIOS_LED_Init(led_cfg);
#endif /* PIOS_INCLUDE_LED */
#if defined(PIOS_INCLUDE_USB)
/* Initialize board specific USB data */
PIOS_USB_BOARD_DATA_Init();
/* Activate the HID-only USB configuration */
PIOS_USB_DESC_HID_ONLY_Init();
uint32_t pios_usb_id;
switch (bdinfo->board_rev) {
case BOARD_REVISION_CC:
PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc);
break;
case BOARD_REVISION_CC3D:
PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc3d);
break;
default:
PIOS_Assert(0);
}
#if defined(PIOS_INCLUDE_USB_HID) && defined(PIOS_INCLUDE_COM_MSG)
uint32_t pios_usb_hid_id;
if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) {
PIOS_Assert(0);
}
if (PIOS_COM_MSG_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id)) {
PIOS_Assert(0);
}
#endif /* PIOS_INCLUDE_USB_HID && PIOS_INCLUDE_COM_MSG */
#endif /* PIOS_INCLUDE_USB */
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE); // TODO Tirar
board_init_complete = true;
}
void PIOS_ADC_DMA_Handler()
{}

148
flight/targets/boards/coptercontrol/firmware/Makefile
View File

@ -1,148 +0,0 @@
#
# Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org
# Copyright (c) 2012, PhoenixPilot, http://github.com/PhoenixPilot
#
# 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 OPENPILOT_IS_COOL
$(error Top level Makefile must be used to build this target)
endif
include ../board-info.mk
include $(ROOT_DIR)/make/firmware-defs.mk
# ARM DSP library
USE_DSP_LIB ?= NO
# Set to YES to build a FW version that will erase data flash memory
ERASE_FLASH ?= NO
# Set to yes to include Gcsreceiver module
GCSRECEIVER ?= NO
# Enable Diag tasks ?
DIAG_TASKS ?= NO
# List of mandatory modules to include
MODULES += Attitude
MODULES += Stabilization
MODULES += Actuator
MODULES += Receiver
MODULES += ManualControl
MODULES += FirmwareIAP
MODULES += Telemetry
# List of optional modules to include
OPTMODULES += CameraStab
OPTMODULES += ComUsbBridge
OPTMODULES += GPS
OPTMODULES += TxPID
OPTMODULES += Osd/osdoutput
#OPTMODULES += Altitude
#OPTMODULES += Fault
SRC += $(FLIGHTLIB)/notification.c
# Include all camera options
CDEFS += -DUSE_INPUT_LPF -DUSE_GIMBAL_LPF -DUSE_GIMBAL_FF
# Erase flash firmware should be buildable from command line
ifeq ($(ERASE_FLASH), YES)
CDEFS += -DERASE_FLASH
endif
# List C source files here (C dependencies are automatically generated).
# Use file-extension c for "c-only"-files
ifndef TESTAPP
## The standard CMSIS startup
SRC += $(CMSIS_DEVICEDIR)/system_stm32f10x.c
## Application Core
SRC += ../pios_usb_board_data.c
SRC += $(OPMODULEDIR)/System/systemmod.c
SRC += $(OPSYSTEM)/coptercontrol.c
SRC += $(OPSYSTEM)/pios_board.c
SRC += $(FLIGHTLIB)/alarms.c
SRC += $(FLIGHTLIB)/instrumentation.c
SRC += $(OPUAVTALK)/uavtalk.c
SRC += $(OPUAVOBJ)/uavobjectmanager.c
SRC += $(OPUAVOBJ)/uavobjectpersistence.c
SRC += $(OPUAVOBJ)/eventdispatcher.c
## UAVObjects
SRC += $(OPUAVSYNTHDIR)/accessorydesired.c
SRC += $(OPUAVSYNTHDIR)/objectpersistence.c
SRC += $(OPUAVSYNTHDIR)/gcstelemetrystats.c
SRC += $(OPUAVSYNTHDIR)/flighttelemetrystats.c
SRC += $(OPUAVSYNTHDIR)/faultsettings.c
SRC += $(OPUAVSYNTHDIR)/flightstatus.c
SRC += $(OPUAVSYNTHDIR)/systemstats.c
SRC += $(OPUAVSYNTHDIR)/systemalarms.c
SRC += $(OPUAVSYNTHDIR)/systemsettings.c
SRC += $(OPUAVSYNTHDIR)/stabilizationdesired.c
SRC += $(OPUAVSYNTHDIR)/stabilizationsettings.c
SRC += $(OPUAVSYNTHDIR)/stabilizationsettingsbank1.c
SRC += $(OPUAVSYNTHDIR)/stabilizationsettingsbank2.c
SRC += $(OPUAVSYNTHDIR)/stabilizationsettingsbank3.c
SRC += $(OPUAVSYNTHDIR)/stabilizationstatus.c
SRC += $(OPUAVSYNTHDIR)/stabilizationbank.c
SRC += $(OPUAVSYNTHDIR)/actuatorcommand.c
SRC += $(OPUAVSYNTHDIR)/actuatordesired.c
SRC += $(OPUAVSYNTHDIR)/actuatorsettings.c
SRC += $(OPUAVSYNTHDIR)/accelstate.c
SRC += $(OPUAVSYNTHDIR)/accelgyrosettings.c
SRC += $(OPUAVSYNTHDIR)/gyrostate.c
SRC += $(OPUAVSYNTHDIR)/attitudestate.c
SRC += $(OPUAVSYNTHDIR)/manualcontrolcommand.c
SRC += $(OPUAVSYNTHDIR)/watchdogstatus.c
SRC += $(OPUAVSYNTHDIR)/manualcontrolsettings.c
SRC += $(OPUAVSYNTHDIR)/flightmodesettings.c
SRC += $(OPUAVSYNTHDIR)/mixersettings.c
SRC += $(OPUAVSYNTHDIR)/firmwareiapobj.c
SRC += $(OPUAVSYNTHDIR)/attitudesettings.c
SRC += $(OPUAVSYNTHDIR)/camerastabsettings.c
SRC += $(OPUAVSYNTHDIR)/cameradesired.c
SRC += $(OPUAVSYNTHDIR)/gpspositionsensor.c
SRC += $(OPUAVSYNTHDIR)/gpsvelocitysensor.c
SRC += $(OPUAVSYNTHDIR)/gpssettings.c
SRC += $(OPUAVSYNTHDIR)/hwsettings.c
SRC += $(OPUAVSYNTHDIR)/receiveractivity.c
SRC += $(OPUAVSYNTHDIR)/mixerstatus.c
SRC += $(OPUAVSYNTHDIR)/ratedesired.c
SRC += $(OPUAVSYNTHDIR)/txpidsettings.c
SRC += $(OPUAVSYNTHDIR)/mpu6000settings.c
# Command line option for Gcsreceiver module
ifeq ($(GCSRECEIVER), YES)
SRC += $(OPUAVSYNTHDIR)/gcsreceiver.c
endif
# Enable Diag tasks and UAVOs needed
ifeq ($(DIAG_TASKS), YES)
CDEFS += -DDIAG_TASKS
SRC += $(OPUAVSYNTHDIR)/taskinfo.c
SRC += $(OPUAVSYNTHDIR)/callbackinfo.c
SRC += $(OPUAVSYNTHDIR)/perfcounter.c
SRC += $(OPUAVSYNTHDIR)/i2cstats.c
endif
else
## Test Code
SRC += $(OPTESTS)/test_common.c
SRC += $(OPTESTS)/$(TESTAPP).c
endif
include $(ROOT_DIR)/make/apps-defs.mk
include $(ROOT_DIR)/make/common-defs.mk

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