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 'samguns/zh_CN_GCS' of ssh://git.openpilot.org/OpenPilot into samguns/zh_CN_GCS

Browse Source
This commit is contained in:
samguns 2015-04-27 13:18:05 +08:00
commit 11a8268949
185 changed files with 13149 additions and 4185 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

181
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
@ -140,20 +142,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 $@
##############################
#
@ -167,10 +155,12 @@ 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)/$@ && \
uavobjgenerator: | $(UAVOBJGENERATOR_DIR)
$(V1) ( cd $(UAVOBJGENERATOR_DIR) && \
$(QMAKE) $(ROOT_DIR)/ground/uavobjgenerator/uavobjgenerator.pro -spec $(QT_SPEC) -r CONFIG+="$(UAVOGEN_BUILD_CONF) $(UAVOGEN_SILENT)" && \
$(MAKE) --no-print-directory -w ; \
)
@ -183,8 +173,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) && \
@ -214,6 +203,8 @@ 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
@ -225,7 +216,7 @@ osd_short := 'osd '
revoproto_short := 'revp'
simposix_short := 'posx'
discoveryf4bare_short := 'df4b'
gpsplatinum_short := 'gps9 '
gpsplatinum_short := 'gps9'
# SimPosix only builds on Linux so drop it from the list for
# all other platforms.
@ -460,8 +451,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)
@ -470,32 +462,25 @@ 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) && \
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) $(GCS_SILENT)" $(GCS_QMAKE_OPTS) \
)
else
@$(ECHO) "skipping qmake"
endif
.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)"
################################
#
@ -503,77 +488,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) && \
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) $(GCS_SILENT)" $(GCS_QMAKE_OPTS) \
)
else
@$(ECHO) "skipping qmake"
endif
.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
@ -695,8 +630,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)))
@ -771,9 +705,8 @@ 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
@ -792,18 +725,10 @@ PACKAGE_NAME := OpenPilot
PACKAGE_SEP := -
PACKAGE_FULL_NAME := $(PACKAGE_NAME)$(PACKAGE_SEP)$(PACKAGE_LBL)
# Source distribution is never dirty because it uses git archive
DIST_NAME := $(DIST_DIR)/$(subst dirty-,,$(PACKAGE_FULL_NAME)).tar
.PHONY: package
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
##############################
#
@ -872,9 +797,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" \
@ -903,6 +827,17 @@ $(DIST_NAME).gz: $(DIST_VER_INFO) .git/index | $(DIST_DIR)
.PHONY: dist
dist: $(DIST_NAME).gz
##############################
#
# Directories
#
##############################
$(DIRS):
$(V1) $(MKDIR) -p $@
##############################
#
# Help message, the default Makefile goal
@ -936,7 +871,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)"
@ -1008,26 +942,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, ..."

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

197
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],
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],
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 SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
float Y[NUMV], float P[NUMX][NUMX], float X[NUMX],
uint16_t SensorsUsed);
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]);
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) {
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,60 +436,78 @@ 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];
float *Firow = F[i];
float *Pirow = P[i];
float *Dirow = Dummy[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 (k = Fistart; k <= Fiend; k++) {
for (j = 0; j < NUMX; j++) {
Dirow[j] += Firow[k] * P[k][j]; // [] + F * P
}
}
}
for (i = 0; i < NUMX; i++) { // Calculate Pnew = (T^2) [Dummy/T + Dummy*F' + G*Qw*G']
float *Dirow = Dummy[i];
float *Girow = G[i];
float *Pirow = P[i];
int8_t Gistart = GrowMin[i];
int8_t Giend = GrowMax[i];
float *Dirow = Dummy[i];
float *Girow = G[i];
float *Pirow = P[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];
int8_t Fjstart = FrowMin[j];
int8_t Fjend = FrowMax[j];
int8_t k;
for (k = Fjstart; k <= Fjend; k++) {
Ptmp += Dirow[k] * Fjrow[k]; // [] + Dummy*F' ...
}
for (j = i; j < NUMX; j++) { // Use symmetry, ie only find upper triangular
float Ptmp = Dirow[j] * dT1; // Pnew = Dummy / T ...
const float *Fjrow = F[j];
int8_t Fjstart = FrowMin[j];
int8_t Fjend = FrowMax[j];
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++) {
Ptmp += Q[k] * Girow[k] * Gjrow[k]; // [] + G*Q*G' ...
float *Gjrow = G[j];
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 (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);

387
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,48 +156,201 @@ void plan_setup_returnToBase()
pathDesired.StartingVelocity = 0.0f;
pathDesired.EndingVelocity = 0.0f;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
uint8_t 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;
void plan_setup_land()
// 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)
{
float descendspeed;
PositionStateData positionState;
plan_setup_positionHold();
PositionStateGet(&positionState);
float velocity_down;
float autotakeoff_height;
FlightModeSettingsLandingVelocityGet(&descendspeed);
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
pathDesired.StartingVelocity = descendspeed;
pathDesired.EndingVelocity = descendspeed;
PathDesiredSet(&pathDesired);
PIOS_DELTATIME_Init(&landdT, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
}
/**
* @brief execute land
*/
void plan_run_land()
{
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);
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;
}
PathDesiredEndSet(&pathDesiredEnd);
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:
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()
{
PathDesiredData pathDesired;
plan_setup_land_helper(&pathDesired);
PathDesiredSet(&pathDesired);
}
static void plan_setup_land_from_velocityroam()
{
plan_setup_land();
FlightStatusAssistedControlStateOptions assistedControlFlightMode;
assistedControlFlightMode = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD;
FlightStatusAssistedControlStateSet(&assistedControlFlightMode);
}
/**
* @brief positionvario functionality
@ -197,6 +380,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 +531,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 +575,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 +731,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,22 +822,27 @@ void plan_setup_assistedcontrol(uint8_t timeout_occurred)
PositionStateGet(&positionState);
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
FlightStatusAssistedControlStateOptions assistedControlFlightMode;
FlightStatusAssistedControlStateGet(&assistedControlFlightMode);
if (timeout_occurred) {
pathDesired.End.North = positionState.North;
pathDesired.End.East = positionState.East;
pathDesired.End.Down = positionState.Down;
pathDesired.Start.North = positionState.North;
pathDesired.Start.East = positionState.East;
pathDesired.Start.Down = positionState.Down;
pathDesired.StartingVelocity = 0.0f;
pathDesired.EndingVelocity = 0.0f;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
assistedControlFlightMode = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD;
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;
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;
}
assistedControlFlightMode = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD;
} else {
VelocityStateData velocityState;
VelocityStateGet(&velocityState);

13
flight/libraries/sanitycheck.c
View File

@ -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:

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

@ -367,13 +367,15 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
}
if ((mixer_type == MIXERSETTINGS_MIXER1TYPE_MOTOR)) {
if (curve1 < 0.0f) {
curve1 = 0.0f;
float nonreversible_curve1 = curve1;
float nonreversible_curve2 = curve2;
if (nonreversible_curve1 < 0.0f) {
nonreversible_curve1 = 0.0f;
}
if (curve2 < 0.0f) {
curve2 = 0.0f;
if (nonreversible_curve2 < 0.0f) {
nonreversible_curve2 = 0.0f;
}
status[ct] = ProcessMixer(ct, curve1, curve2, &desired, dTSeconds);
status[ct] = ProcessMixer(ct, nonreversible_curve1, nonreversible_curve2, &desired, dTSeconds);
// If not armed or motors aren't meant to spin all the time
if (!armed ||
(!spinWhileArmed && !positiveThrottle)) {
@ -586,11 +588,7 @@ static float MixerCurveFullRangeAbsolute(const float input, const float *curve,
scale -= (float)idx1; // remainder
if (curve[0] < -1) {
return input;
}
if (idx1 < 0) {
idx1 = 0; // clamp to lowest entry in table
scale = 0;
return abs_input;
}
int idx2 = idx1 + 1;
if (idx2 >= elements) {

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

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

@ -275,6 +275,12 @@ static void gpsTask(__attribute__((unused)) void *parameters)
ubx_autoconfig_run(&buffer, &count, status != GPSPOSITIONSENSOR_STATUS_NOGPS);
// 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);
}
}
@ -479,6 +485,7 @@ void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev)
uint8_t ubxSbasMode;
ubx_autoconfig_settings_t newconfig;
uint8_t ubxSbasSats;
uint8_t ubxGnssMode;
GPSSettingsUbxRateGet(&newconfig.navRate);
@ -541,6 +548,41 @@ void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev)
ubxSbasSats == GPSSETTINGS_UBXSBASSATS_GAGAN ? UBX_SBAS_SATS_GAGAN :
ubxSbasSats == GPSSETTINGS_UBXSBASSATS_SDCM ? UBX_SBAS_SATS_SDCM : UBX_SBAS_SATS_AUTOSCAN;
GPSSettingsUbxGNSSModeGet(&ubxGnssMode);
switch (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);
}
#endif /* if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) */

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

@ -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,8 +124,8 @@ 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;
struct UBXPacket *ubx = (struct UBXPacket *)gps_rx_buffer;
static uint16_t rx_count = 0;
struct UBXPacket *ubx = (struct UBXPacket *)gps_rx_buffer;
for (int i = 0; i < len; i++) {
c = rx[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;

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

@ -102,6 +102,7 @@ typedef enum {
UBX_ID_CFG_MSG = 0x01,
UBX_ID_CFG_CFG = 0x09,
UBX_ID_CFG_SBAS = 0x16,
UBX_ID_CFG_GNSS = 0x3E
} ubx_class_cfg_id;
typedef enum {
@ -310,7 +311,7 @@ struct UBX_NAV_SVINFO_SV {
};
// SV information message
#define MAX_SVS 16
#define MAX_SVS 32
struct UBX_NAV_SVINFO {
uint32_t iTOW; // GPS Millisecond Time of Week (ms)
@ -404,7 +405,7 @@ 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 */

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

@ -88,6 +88,10 @@ 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
@ -164,6 +168,44 @@ typedef struct {
uint32_t scanmode1;
} __attribute__((packed)) ubx_cfg_sbas_t;
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;
#define UBX_CFG_GNSS_FLAGS_ENABLED 0x01
#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
typedef struct {
uint8_t gnssId;
uint8_t resTrkCh;
uint8_t maxTrkCh;
uint8_t resvd;
uint32_t flags;
} __attribute__((packed)) ubx_cfg_gnss_cfgblock_t;
typedef struct {
uint8_t msgVer;
uint8_t numTrkChHw;
uint8_t numTrkChUse;
uint8_t numConfigBlocks;
ubx_cfg_gnss_cfgblock_t cfgBlocks[UBX_GNSS_ID_MAX];
} __attribute__((packed)) ubx_cfg_gnss_t;
typedef struct {
uint8_t prolog[2];
uint8_t class;
@ -181,6 +223,7 @@ typedef union {
ubx_cfg_nav5_t cfg_nav5;
ubx_cfg_rate_t cfg_rate;
ubx_cfg_sbas_t cfg_sbas;
ubx_cfg_gnss_t cfg_gnss;
} payload;
uint8_t resvd[2]; // added space for checksum bytes
} message;

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

@ -224,6 +224,65 @@ void config_sbas(uint16_t *bytes_to_send)
status->requiredAck.msgID = UBX_ID_CFG_SBAS;
}
void config_gnss(uint16_t *bytes_to_send)
{
int32_t i;
memset(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 (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(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_GNSS, sizeof(ubx_cfg_gnss_t));
status->requiredAck.clsID = UBX_CLASS_CFG;
status->requiredAck.msgID = UBX_ID_CFG_GNSS;
}
void config_save(uint16_t *bytes_to_send)
{
memset(status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_cfg_t));
@ -244,13 +303,21 @@ static void configure(uint16_t *bytes_to_send)
config_nav(bytes_to_send);
break;
case LAST_CONFIG_SENT_START + 2:
if (status->currentSettings.enableGLONASS || status->currentSettings.enableGPS) {
config_gnss(bytes_to_send);
break;
} else {
// Skip and fall through to next step
status->lastConfigSent++;
}
case LAST_CONFIG_SENT_START + 3:
config_sbas(bytes_to_send);
if (!status->currentSettings.storeSettings) {
// skips saving
status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED;
}
break;
case LAST_CONFIG_SENT_START + 3:
case LAST_CONFIG_SENT_START + 4:
config_save(bytes_to_send);
status->lastConfigSent = LAST_CONFIG_SENT_COMPLETED;
return;
@ -308,22 +375,12 @@ void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send, bool gps_connec
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;
@ -366,15 +423,12 @@ void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send, bool gps_connec
{
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
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)) {
// timeout or NAK, resend the message or abort
status->retryCount++;
if (status->retryCount > UBX_MAX_RETRIES) {
status->currentStep = INIT_STEP_ERROR;

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

@ -35,6 +35,9 @@
#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
@ -310,6 +313,11 @@ static bool okToArm(void)
return true;
break;
case FLIGHTSTATUS_FLIGHTMODE_LAND:
return false;
case FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF:
return true;
default:
return false;
@ -334,6 +342,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;
}

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

@ -397,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;
@ -412,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;
@ -528,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
}

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

@ -57,7 +57,9 @@ void pathFollowerHandler(bool newinit)
uint8_t flightMode;
uint8_t assistedControlFlightMode;
uint8_t 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:
plan_setup_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:
plan_setup_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:
plan_run_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();

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

50
flight/modules/PathFollower/inc/pathfollowercontrol.h Normal file
View File

@ -0,0 +1,50 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @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
*
*****************************************************************************/
/*
* 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 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;
};
#endif // PATHFOLLOWERCONTROL_H

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

@ -0,0 +1,80 @@
/**
******************************************************************************
* @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>
}
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:
// 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

95
flight/modules/PathFollower/inc/pidcontroldown.h Normal file
View File

@ -0,0 +1,95 @@
/**
******************************************************************************
* @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 "pathfollowerfsm.h"
class PIDControlDown {
public:
PIDControlDown();
~PIDControlDown();
void Initialize(PathFollowerFSM *fsm);
void SetThrustLimits(float min_thrust, float max_thrust);
void Deactivate();
void Activate();
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);
private:
void setup_neutralThrustCalc();
void run_neutralThrustCalc();
struct pid2 PID;
float deltaTime;
float mVelocitySetpointTarget;
float mVelocitySetpointCurrent;
float mVelocityState;
float mDownCommand;
PathFollowerFSM *mFSM;
float mNeutral;
float mVelocityMax;
struct pid PIDpos;
float mPositionSetpointTarget;
float mPositionState;
float mMinThrust;
float mMaxThrust;
uint8_t mActive;
struct NeutralThrustEstimation {
uint32_t count;
float sum;
float average;
float correction;
float min;
float max;
bool start_sampling;
bool have_correction;
};
struct NeutralThrustEstimation neutralThrustEst;
};
#endif // PIDCONTROLDOWN_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(uint8_t 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;
PathFollowerFSM_AutoTakeoffState_T 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(PathFollowerFSM_AutoTakeoffState_T 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

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

@ -0,0 +1,75 @@
/**
******************************************************************************
* @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);
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;
PathFollowerFSM_LandState_T 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(PathFollowerFSM_LandState_T 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);
}

52
flight/modules/PathFollower/pathfollowercontrol.cpp Normal file
View File

@ -0,0 +1,52 @@
/*
******************************************************************************
*
* @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
*
*****************************************************************************/
/*
* 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 <flightstatus.h>
#include <pathstatus.h>
#include <pathdesired.h>
}
// C++ includes
#include "pathfollowercontrol.h"
PathDesiredData *PathFollowerControl::pathDesired = 0;
FlightStatusData *PathFollowerControl::flightStatus = 0;
PathStatusData *PathFollowerControl::pathStatus = 0;
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);
pathDesired = ptr_pathDesired;
flightStatus = ptr_flightStatus;
pathStatus = ptr_pathStatus;
return 0;
}

374
flight/modules/PathFollower/pidcontroldown.cpp Normal file
View File

@ -0,0 +1,374 @@
/**
******************************************************************************
* @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 "pathfollowerfsm.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),
mFSM(NULL), mNeutral(0.5f), mVelocityMax(1.0f), mPositionSetpointTarget(0.0f), mPositionState(0.0f),
mMinThrust(0.1f), mMaxThrust(0.6f), mActive(false)
{
Deactivate();
}
PIDControlDown::~PIDControlDown() {}
void PIDControlDown::Initialize(PathFollowerFSM *fsm)
{
mFSM = fsm;
}
void PIDControlDown::SetThrustLimits(float min_thrust, float max_thrust)
{
mMinThrust = min_thrust;
mMaxThrust = max_thrust;
}
void PIDControlDown::Deactivate()
{
// pid_zero(&PID);
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 (mFSM) {
// The FSM controls the actual descent velocity and introduces step changes as required
float velocitySetpointDesired = mFSM->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 (mFSM) {
mFSM->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;
}

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];
}

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

@ -0,0 +1,308 @@
/*
******************************************************************************
*
* @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
if (flightStatus->ControlChain.PathPlanner != FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
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]);
} else {
float velocity_down;
FlightModeSettingsAutoTakeOffVelocityGet(&velocity_down);
controlDown.UpdateVelocitySetpoint(-velocity_down);
controlNE.UpdateVelocitySetpoint(0.0f, 0.0f);
// pathplanner mode the waypoint provides the final destination including altitude. Takeoff would
// often be waypoint 0, in which case the starting location is the current location, and the end location
// is the waypoint location which really needs to be the same as the end in north and east. If it is not,
// it will fly to that location the during ascent.
// Note in pathplanner mode we use the start location which is the current initial location as the
// target NE to control. Takeoff only ascends vertically.
controlNE.UpdatePositionSetpoint(pathDesired->Start.North, pathDesired->Start.East);
// Sanity check that the end location is at least a reasonable height above the start location in the down direction
float autotakeoff_height = pathDesired->Start.Down - pathDesired->End.Down;
if (autotakeoff_height < 2.0f) {
pathDesired->End.Down = pathDesired->Start.Down - 2.0f;
}
controlDown.UpdatePositionSetpoint(pathDesired->End.Down); // the altitude is set by the end location.
fsm->setControlState(STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_INITIATE);
}
}
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.ILimit,
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(uint8_t 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(AUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
} else {
mAutoTakeoffData->currentState = AUTOTAKEOFF_STATE_INACTIVE;
}
}
void VtolAutoTakeoffFSM::Activate()
{
memset(mAutoTakeoffData, 0, sizeof(VtolAutoTakeoffFSMData_T));
mAutoTakeoffData->currentState = AUTOTAKEOFF_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[AUTOTAKEOFF_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(AUTOTAKEOFF_STATE_HOLD, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
return;
}
// initially inactive and wait for a change in controlstate.
setState(AUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
}
void VtolAutoTakeoffFSM::Abort(void)
{
setState(AUTOTAKEOFF_STATE_ABORT, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
}
PathFollowerFSMState_T VtolAutoTakeoffFSM::GetCurrentState(void)
{
switch (mAutoTakeoffData->currentState) {
case AUTOTAKEOFF_STATE_INACTIVE:
return PFFSM_STATE_INACTIVE;
break;
case AUTOTAKEOFF_STATE_ABORT:
return PFFSM_STATE_ABORT;
break;
case AUTOTAKEOFF_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(PathFollowerFSM_AutoTakeoffState_T newState, StatusVtolAutoTakeoffStateExitReasonOptions reason)
{
mAutoTakeoffData->fsmAutoTakeoffStatus.StateExitReason[mAutoTakeoffData->currentState] = reason;
if (mAutoTakeoffData->currentState == newState) {
return;
}
mAutoTakeoffData->currentState = newState;
if (newState != AUTOTAKEOFF_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(AUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
break;
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE:
setState(AUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
break;
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_REQUIREUNARMEDFIRST:
setState(AUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
break;
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_INITIATE:
setState(AUTOTAKEOFF_STATE_CHECKSTATE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
break;
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_POSITIONHOLD:
setState(AUTOTAKEOFF_STATE_HOLD, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
break;
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_ABORT:
setState(AUTOTAKEOFF_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(AUTOTAKEOFF_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(AUTOTAKEOFF_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(AUTOTAKEOFF_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(AUTOTAKEOFF_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(AUTOTAKEOFF_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(AUTOTAKEOFF_STATE_THRUSTDOWN, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_POSITIONERROR);
return;
}
if (fabsf(down_error) < 0.5f) {
setState(AUTOTAKEOFF_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(AUTOTAKEOFF_STATE_THRUSTOFF, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_ZEROTHRUST);
}
if (flTimeout) {
setState(AUTOTAKEOFF_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(AUTOTAKEOFF_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)
{}

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

@ -0,0 +1,355 @@
/*
******************************************************************************
*
* @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.ILimit,
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;
}
stabDesired.StabilizationMode.Thrust = 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;
}

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

@ -0,0 +1,547 @@
/*
******************************************************************************
*
* @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.ILimit,
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.ILimit, // TODO Change to 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);
}
/**
* 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.ILimit,
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(LAND_STATE_INACTIVE, STATUSVTOLLAND_STATEEXITREASON_NONE);
} else {
mLandData->currentState = LAND_STATE_INACTIVE;
}
}
void VtolLandFSM::Activate()
{
memset(mLandData, 0, sizeof(VtolLandFSMData_T));
mLandData->currentState = LAND_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[LAND_STATE_INACTIVE] = 0.0f;
assessAltitude();
if (pathDesired->Mode == PATHDESIRED_MODE_LAND) {
#ifndef DEBUG_GROUNDIMPACT
setState(LAND_STATE_INIT_ALTHOLD, STATUSVTOLLAND_STATEEXITREASON_NONE);
#else
setState(LAND_STATE_WTG_FOR_GROUNDEFFECT, STATUSVTOLLAND_STATEEXITREASON_NONE);
#endif
} else {
// move to error state and callback to position hold
setState(LAND_STATE_ABORT, STATUSVTOLLAND_STATEEXITREASON_NONE);
}
}
void VtolLandFSM::Abort(void)
{
setState(LAND_STATE_ABORT, STATUSVTOLLAND_STATEEXITREASON_NONE);
}
PathFollowerFSMState_T VtolLandFSM::GetCurrentState(void)
{
switch (mLandData->currentState) {
case LAND_STATE_INACTIVE:
return PFFSM_STATE_INACTIVE;
break;
case LAND_STATE_ABORT:
return PFFSM_STATE_ABORT;
break;
case LAND_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(PathFollowerFSM_LandState_T newState, StatusVtolLandStateExitReasonOptions reason)
{
mLandData->fsmLandStatus.StateExitReason[mLandData->currentState] = reason;
if (mLandData->currentState == newState) {
return;
}
mLandData->currentState = newState;
if (newState != LAND_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(LAND_STATE_WTG_FOR_DESCENTRATE, 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(LAND_STATE_AT_DESCENTRATE, STATUSVTOLLAND_STATEEXITREASON_DESCENTRATEOK);
return;
}
}
if (flTimeout) {
setState(LAND_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(LAND_STATE_WTG_FOR_GROUNDEFFECT, 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(LAND_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(LAND_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(LAND_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(LAND_STATE_THRUSTDOWN, STATUSVTOLLAND_STATEEXITREASON_POSITIONERROR);
return;
}
if (flTimeout) {
setState(LAND_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(LAND_STATE_THRUSTOFF, STATUSVTOLLAND_STATEEXITREASON_ZEROTHRUST);
}
if (flTimeout) {
setState(LAND_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(LAND_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(LAND_STATE_WTG_FOR_GROUNDEFFECT, 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.ILimit,
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_AXISLOCK;
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);
}

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

@ -43,8 +43,11 @@
#include "waypoint.h"
#include "waypointactive.h"
#include "flightmodesettings.h"
#include <systemsettings.h>
#include "paths.h"
#include "plans.h"
#include <sanitycheck.h>
#include <vtolpathfollowersettings.h>
// Private constants
#define STACK_SIZE_BYTES 1024
@ -73,6 +76,7 @@ static uint8_t conditionAboveSpeed();
static uint8_t conditionPointingTowardsNext();
static uint8_t conditionPythonScript();
static uint8_t conditionImmediate();
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
// Private variables
@ -82,7 +86,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 +102,9 @@ int32_t PathPlannerStart()
WaypointActiveConnectCallback(commandUpdated);
PathActionConnectCallback(commandUpdated);
PathStatusConnectCallback(statusUpdated);
SettingsUpdatedCb(NULL);
SystemSettingsConnectCallback(&SettingsUpdatedCb);
VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
// Start main task callback
PIOS_CALLBACKSCHEDULER_Dispatch(pathPlannerHandle);
@ -125,6 +135,39 @@ int32_t PathPlannerInitialize()
MODULE_INITCALL(PathPlannerInitialize, PathPlannerStart);
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
uint8_t 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;
}
}
/**
* Module task
*/
@ -160,22 +203,17 @@ 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);
@ -365,6 +403,8 @@ 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];*/
// 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;
@ -515,18 +555,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 +582,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;
}

151
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,42 +593,45 @@ 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;
if (state == UAVTALK_STATE_COMPLETE) {
// We only want to unpack certain telemetry objects
uint32_t objId = UAVTalkGetPacketObjId(inConnectionHandle);
switch (objId) {
case OPLINKSTATUS_OBJID:
case OPLINKSETTINGS_OBJID:
case OPLINKRECEIVER_OBJID:
case MetaObjectId(OPLINKSTATUS_OBJID):
case MetaObjectId(OPLINKSETTINGS_OBJID):
case MetaObjectId(OPLINKRECEIVER_OBJID):
UAVTalkReceiveObject(inConnectionHandle);
break;
case OBJECTPERSISTENCE_OBJID:
case MetaObjectId(OBJECTPERSISTENCE_OBJID):
// receive object locally
// some objects will send back a response to telemetry
// FIXME:
// OPLM will ack or nack all objects requests and acked object sends
// Receiver will probably also ack / nack the same messages
// This has some consequences like :
// Second ack/nack will not match an open transaction or will apply to wrong transaction
// Question : how does GCS handle receiving the same object twice
// The OBJECTPERSISTENCE logic can be broken too if for example OPLM nacks and then REVO acks...
UAVTalkReceiveObject(inConnectionHandle);
// relay packet to remote modem
UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle);
break;
default:
// all other packets are relayed to the remote modem
UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle);
break;
// 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);
switch (objId) {
case OPLINKSTATUS_OBJID:
case OPLINKSETTINGS_OBJID:
case OPLINKRECEIVER_OBJID:
case MetaObjectId(OPLINKSTATUS_OBJID):
case MetaObjectId(OPLINKSETTINGS_OBJID):
case MetaObjectId(OPLINKRECEIVER_OBJID):
UAVTalkReceiveObject(inConnectionHandle);
break;
case OBJECTPERSISTENCE_OBJID:
case MetaObjectId(OBJECTPERSISTENCE_OBJID):
// receive object locally
// some objects will send back a response to telemetry
// FIXME:
// OPLM will ack or nack all objects requests and acked object sends
// Receiver will probably also ack / nack the same messages
// This has some consequences like :
// Second ack/nack will not match an open transaction or will apply to wrong transaction
// Question : how does GCS handle receiving the same object twice
// The OBJECTPERSISTENCE logic can be broken too if for example OPLM nacks and then REVO acks...
UAVTalkReceiveObject(inConnectionHandle);
// relay packet to remote modem
UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle);
break;
default:
// all other packets are relayed to the remote modem
UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle);
break;
}
}
}
}
@ -642,39 +641,43 @@ static void ProcessTelemetryStream(UAVTalkConnection inConnectionHandle, UAVTalk
*
* @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;
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
uint32_t objId = UAVTalkGetPacketObjId(inConnectionHandle);
switch (objId) {
case OPLINKSTATUS_OBJID:
case OPLINKSETTINGS_OBJID:
case MetaObjectId(OPLINKSTATUS_OBJID):
case MetaObjectId(OPLINKSETTINGS_OBJID):
// Ignore object...
// These objects are shadowed by the modem and are not transmitted to the telemetry port
// - OPLINKSTATUS_OBJID : ground station will receive the OPLM link status instead
// - OPLINKSETTINGS_OBJID : ground station will read and write the OPLM settings instead
break;
case OPLINKRECEIVER_OBJID:
case MetaObjectId(OPLINKRECEIVER_OBJID):
// Receive object locally
// These objects are received by the modem and are not transmitted to the telemetry port
// - OPLINKRECEIVER_OBJID : not sure why
// some objects will send back a response to the remote modem
UAVTalkReceiveObject(inConnectionHandle);
break;
default:
// all other packets are relayed to the telemetry port
UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle);
break;
// 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
uint32_t objId = UAVTalkGetPacketObjId(inConnectionHandle);
switch (objId) {
case OPLINKSTATUS_OBJID:
case OPLINKSETTINGS_OBJID:
case MetaObjectId(OPLINKSTATUS_OBJID):
case MetaObjectId(OPLINKSETTINGS_OBJID):
// Ignore object...
// These objects are shadowed by the modem and are not transmitted to the telemetry port
// - OPLINKSTATUS_OBJID : ground station will receive the OPLM link status instead
// - OPLINKSETTINGS_OBJID : ground station will read and write the OPLM settings instead
break;
case OPLINKRECEIVER_OBJID:
case MetaObjectId(OPLINKRECEIVER_OBJID):
// Receive object locally
// These objects are received by the modem and are not transmitted to the telemetry port
// - OPLINKRECEIVER_OBJID : not sure why
// some objects will send back a response to the remote modem
UAVTalkReceiveObject(inConnectionHandle);
break;
default:
// all other packets are relayed to the telemetry port
UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle);
break;
}
}
}
}

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

@ -300,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))) {

19
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];
@ -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));
}
/**
* @}

20
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()
@ -241,6 +244,10 @@ static void stabilizationInnerloopTask()
float dT;
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 +255,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:
@ -322,7 +338,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);

39
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
#define STACK_REQUIRED 64
// Private types
// Private types
struct data {
uint32_t lastUpdate;
};
// Private variables
// Private functions
@ -49,27 +53,34 @@ 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)
{
state->pos[0] = 0.0f;
state->pos[1] = 0.0f;
state->pos[2] = 0.0f;
state->updated |= SENSORUPDATES_pos;
struct data *this = (struct data *)self->localdata;
state->vel[0] = 0.0f;
state->vel[1] = 0.0f;
state->vel[2] = 0.0f;
state->updated |= SENSORUPDATES_vel;
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;
state->updated |= SENSORUPDATES_pos;
state->vel[0] = 0.0f;
state->vel[1] = 0.0f;
state->vel[2] = 0.0f;
state->updated |= SENSORUPDATES_vel;
}
return FILTERRESULT_OK;
}

321
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,193 +360,171 @@ static void StateEstimationCb(void)
return;
}
switch (runState) {
case RUNSTATE_LOAD:
alarm = FILTERRESULT_OK;
alarm = FILTERRESULT_OK;
// set alarm to warning if called through timeout
if (updatedSensors == 0) {
if (PIOS_DELAY_DiffuS(last_time) > 1000 * TIMEOUT_MS) {
alarm = FILTERRESULT_WARNING;
}
} else {
last_time = PIOS_DELAY_GetRaw();
// set alarm to warning if called through timeout
if (updatedSensors == 0) {
if (PIOS_DELAY_DiffuS(last_time) > 1000 * TIMEOUT_MS) {
alarm = FILTERRESULT_WARNING;
}
} else {
last_time = PIOS_DELAY_GetRaw();
}
// check if a new filter chain should be initialized
if (fusionAlgorithm != revoSettings.FusionAlgorithm) {
FlightStatusData fs;
FlightStatusGet(&fs);
if (fs.Armed == FLIGHTSTATUS_ARMED_DISARMED || fusionAlgorithm == FILTER_INIT_FORCE) {
const filterPipeline *newFilterChain;
switch (revoSettings.FusionAlgorithm) {
case REVOSETTINGS_FUSIONALGORITHM_BASICCOMPLEMENTARY:
newFilterChain = cfQueue;
break;
case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYMAG:
newFilterChain = cfmiQueue;
break;
case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYMAGGPSOUTDOOR:
newFilterChain = cfmQueue;
break;
case REVOSETTINGS_FUSIONALGORITHM_INS13INDOOR:
newFilterChain = ekf13iQueue;
break;
case REVOSETTINGS_FUSIONALGORITHM_GPSNAVIGATIONINS13:
newFilterChain = ekf13Queue;
break;
default:
newFilterChain = NULL;
}
// initialize filters in chain
current = newFilterChain;
bool error = 0;
while (current != NULL) {
int32_t result = current->filter->init((stateFilter *)current->filter);
if (result != 0) {
error = 1;
break;
}
current = current->next;
}
if (error) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
return;
} else {
// set new fusion algortithm
filterChain = newFilterChain;
fusionAlgorithm = revoSettings.FusionAlgorithm;
}
}
}
// read updated sensor UAVObjects and set initial state
states.updated = updatedSensors;
updatedSensors = 0;
// fetch sensors, check values, and load into state struct
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(GyroSensor, gyro, x, y, z);
if (IS_SET(states.updated, SENSORUPDATES_gyro)) {
gyroRaw[0] = states.gyro[0];
gyroRaw[1] = states.gyro[1];
gyroRaw[2] = states.gyro[2];
}
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(AccelSensor, accel, x, y, z);
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(MagSensor, boardMag, x, y, z);
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(AuxMagSensor, auxMag, x, y, z);
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(GPSVelocitySensor, vel, North, East, Down);
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_1_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(BaroSensor, baro, Altitude, true);
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_2_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(AirspeedSensor, airspeed, CalibratedAirspeed, TrueAirspeed, s.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE);
// GPS position data (LLA) is not fetched here since it does not contain floats. The filter must do all checks itself
// at this point sensor state is stored in "states" with some rudimentary filtering applied
// apply all filters in the current filter chain
current = filterChain;
// we are not done, re-dispatch self execution
runState = RUNSTATE_FILTER;
PIOS_CALLBACKSCHEDULER_Dispatch(stateEstimationCallback);
break;
case RUNSTATE_FILTER:
if (current != NULL) {
filterResult result = current->filter->filter((stateFilter *)current->filter, &states);
if (result > alarm) {
alarm = result;
}
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)) {
gyroDelta[0] = states.gyro[0] - gyroRaw[0];
gyroDelta[1] = states.gyro[1] - gyroRaw[1];
gyroDelta[2] = states.gyro[2] - gyroRaw[2];
}
EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(AccelState, accel, x, y, z);
if (IS_SET(states.updated, SENSORUPDATES_mag)) {
MagStateData s;
MagStateGet(&s);
s.x = states.mag[0];
s.y = states.mag[1];
s.z = states.mag[2];
switch (states.magStatus) {
case MAGSTATUS_OK:
s.Source = MAGSTATE_SOURCE_ONBOARD;
// check if a new filter chain should be initialized
if (fusionAlgorithm != revoSettings.FusionAlgorithm) {
FlightStatusData fs;
FlightStatusGet(&fs);
if (fs.Armed == FLIGHTSTATUS_ARMED_DISARMED || fusionAlgorithm == FILTER_INIT_FORCE) {
const filterPipeline *newFilterChain;
switch ((RevoSettingsFusionAlgorithmOptions)revoSettings.FusionAlgorithm) {
case REVOSETTINGS_FUSIONALGORITHM_BASICCOMPLEMENTARY:
newFilterChain = cfQueue;
break;
case MAGSTATUS_AUX:
s.Source = MAGSTATE_SOURCE_AUX;
case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYMAG:
newFilterChain = cfmiQueue;
break;
case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYMAGGPSOUTDOOR:
newFilterChain = cfmQueue;
break;
case REVOSETTINGS_FUSIONALGORITHM_INS13INDOOR:
newFilterChain = ekf13iQueue;
break;
case REVOSETTINGS_FUSIONALGORITHM_GPSNAVIGATIONINS13:
newFilterChain = ekf13Queue;
break;
default:
s.Source = MAGSTATE_SOURCE_INVALID;
newFilterChain = NULL;
}
// initialize filters in chain
current = newFilterChain;
bool error = 0;
while (current != NULL) {
int32_t result = current->filter->init((stateFilter *)current->filter);
if (result != 0) {
error = 1;
break;
}
current = current->next;
}
if (error) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
return;
} else {
// set new fusion algortithm
filterChain = newFilterChain;
fusionAlgorithm = revoSettings.FusionAlgorithm;
}
MagStateSet(&s);
}
}
EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(PositionState, pos, North, East, Down);
EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(VelocityState, vel, North, East, Down);
EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_2_DIMENSIONS(AirspeedState, airspeed, CalibratedAirspeed, TrueAirspeed);
// attitude nees manual conversion from quaternion to euler
if (IS_SET(states.updated, SENSORUPDATES_attitude)) { \
AttitudeStateData s;
AttitudeStateGet(&s);
s.q1 = states.attitude[0];
s.q2 = states.attitude[1];
s.q3 = states.attitude[2];
s.q4 = states.attitude[3];
Quaternion2RPY(&s.q1, &s.Roll);
AttitudeStateSet(&s);
// read updated sensor UAVObjects and set initial state
states.updated = updatedSensors;
updatedSensors = 0;
// fetch sensors, check values, and load into state struct
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(GyroSensor, gyro, x, y, z);
if (IS_SET(states.updated, SENSORUPDATES_gyro)) {
gyroRaw[0] = states.gyro[0];
gyroRaw[1] = states.gyro[1];
gyroRaw[2] = states.gyro[2];
}
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(AccelSensor, accel, x, y, z);
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(MagSensor, boardMag, x, y, z);
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(AuxMagSensor, auxMag, x, y, z);
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(GPSVelocitySensor, vel, North, East, Down);
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_1_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(BaroSensor, baro, Altitude, true);
FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_2_DIMENSION_WITH_CUSTOM_EXTRA_CHECK(AirspeedSensor, airspeed, CalibratedAirspeed, TrueAirspeed, s.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE);
// GPS position data (LLA) is not fetched here since it does not contain floats. The filter must do all checks itself
// at this point sensor state is stored in "states" with some rudimentary filtering applied
// apply all filters in the current filter chain
current = filterChain;
// we are not done, re-dispatch self execution
while (current) {
filterResult result = current->filter->filter((stateFilter *)current->filter, &states);
if (result > alarm) {
alarm = result;
}
current = current->next;
}
// throttle alarms, raise alarm flags immediately
// but require system to run for a while before decreasing
// to prevent alarm flapping
if (alarm >= lastAlarm) {
// 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)) {
gyroDelta[0] = states.gyro[0] - gyroRaw[0];
gyroDelta[1] = states.gyro[1] - gyroRaw[1];
gyroDelta[2] = states.gyro[2] - gyroRaw[2];
}
EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(AccelState, accel, x, y, z);
if (IS_SET(states.updated, SENSORUPDATES_mag)) {
MagStateData s;
MagStateGet(&s);
s.x = states.mag[0];
s.y = states.mag[1];
s.z = states.mag[2];
switch (states.magStatus) {
case MAGSTATUS_OK:
s.Source = MAGSTATE_SOURCE_ONBOARD;
break;
case MAGSTATUS_AUX:
s.Source = MAGSTATE_SOURCE_AUX;
break;
default:
s.Source = MAGSTATE_SOURCE_INVALID;
}
MagStateSet(&s);
}
EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(PositionState, pos, North, East, Down);
EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_3_DIMENSIONS(VelocityState, vel, North, East, Down);
EXPORT_STATE_TO_UAVOBJECT_IF_UPDATED_2_DIMENSIONS(AirspeedState, airspeed, CalibratedAirspeed, TrueAirspeed);
// attitude nees manual conversion from quaternion to euler
if (IS_SET(states.updated, SENSORUPDATES_attitude)) { \
AttitudeStateData s;
AttitudeStateGet(&s);
s.q1 = states.attitude[0];
s.q2 = states.attitude[1];
s.q3 = states.attitude[2];
s.q4 = states.attitude[3];
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
if (alarm >= lastAlarm) {
lastAlarm = alarm;
alarmcounter = 0;
} else {
if (alarmcounter < 100) {
alarmcounter++;
} else {
lastAlarm = alarm;
alarmcounter = 0;
} else {
if (alarmcounter < 100) {
alarmcounter++;
} else {
lastAlarm = alarm;
alarmcounter = 0;
}
}
}
// clear alarms if everything is alright, then schedule callback execution after timeout
if (lastAlarm == FILTERRESULT_WARNING) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING);
} else if (lastAlarm == FILTERRESULT_CRITICAL) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL);
} else if (lastAlarm >= FILTERRESULT_ERROR) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
} else {
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
}
// clear alarms if everything is alright, then schedule callback execution after timeout
if (lastAlarm == FILTERRESULT_WARNING) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING);
} else if (lastAlarm == FILTERRESULT_CRITICAL) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL);
} else if (lastAlarm >= FILTERRESULT_ERROR) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
} else {
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;
if (updatedSensors) {
PIOS_CALLBACKSCHEDULER_Dispatch(stateEstimationCallback);
} else {
PIOS_CALLBACKSCHEDULER_Schedule(stateEstimationCallback, TIMEOUT_MS, CALLBACK_UPDATEMODE_SOONER);
}
}

561
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
#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,
&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);
}
// Start the telemetry tasks associated with Radio/USB
UAVObjIterate(&registerRadioObject);
// Listen to objects of interest
GCSTelemetryStatsConnectQueue(priorityQueue);
#ifdef PIOS_TELEM_PRIORITY_QUEUE
GCSTelemetryStatsConnectQueue(radioChannel.priorityQueue);
#else /* PIOS_TELEM_PRIORITY_QUEUE */
GCSTelemetryStatsConnectQueue(radioChannel.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);
#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
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;
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]);
}
}
} else {
vTaskDelay(5);
}
}
return PIOS_COM_TELEM_RF;
}
/**
* 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;
}
/**
* 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,9 +882,11 @@ 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;
HwSettingsTelemetrySpeedGet(&speed);
@ -693,58 +894,30 @@ static void updateSettings()
// 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;
}
}
/**
* @}

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

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

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

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

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

2
flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc
View File

@ -19,6 +19,7 @@
# These are the UAVObjects supposed to be build as part of the OpenPilot target
# (all architectures)
UAVOBJSRCFILENAMES =
UAVOBJSRCFILENAMES += statusvtolland
UAVOBJSRCFILENAMES += vtolselftuningstats
UAVOBJSRCFILENAMES += accelgyrosettings
UAVOBJSRCFILENAMES += accessorydesired
@ -61,6 +62,7 @@ UAVOBJSRCFILENAMES += gpsextendedstatus
UAVOBJSRCFILENAMES += fixedwingpathfollowersettings
UAVOBJSRCFILENAMES += fixedwingpathfollowerstatus
UAVOBJSRCFILENAMES += vtolpathfollowersettings
UAVOBJSRCFILENAMES += groundpathfollowersettings
UAVOBJSRCFILENAMES += homelocation
UAVOBJSRCFILENAMES += i2cstats
UAVOBJSRCFILENAMES += manualcontrolcommand

11
flight/targets/boards/discoveryf4bare/firmware/pios_board.c
View File

@ -909,6 +909,17 @@ void PIOS_Board_Init(void)
#include <pios_ws2811.h>
PIOS_WS2811_Init(&pios_ws2811_cfg, &pios_ws2811_pin_cfg);
#endif // PIOS_INCLUDE_WS2811
#ifdef PIOS_INCLUDE_ADC
{
uint8_t adc_config[HWSETTINGS_ADCROUTING_NUMELEM];
HwSettingsADCRoutingArrayGet(adc_config);
for (uint32_t i = 0; i < HWSETTINGS_ADCROUTING_NUMELEM; i++) {
if (adc_config[i] != HWSETTINGS_ADCROUTING_DISABLED) {
PIOS_ADC_PinSetup(i);
}
}
}
#endif // PIOS_INCLUDE_ADC
}
/**

8
flight/targets/boards/discoveryf4bare/pios_board.h
View File

@ -264,10 +264,10 @@ extern uint32_t pios_packet_handler;
// -------------------------
#define PIOS_DMA_PIN_CONFIG \
{ \
{ GPIOC, GPIO_Pin_1, ADC_Channel_11 }, \
{ GPIOC, GPIO_Pin_2, ADC_Channel_12 }, \
{ NULL, 0, ADC_Channel_Vrefint }, /* Voltage reference */ \
{ NULL, 0, ADC_Channel_TempSensor }, /* Temperature sensor */ \
{ GPIOC, GPIO_Pin_1, ADC_Channel_11, false }, \
{ GPIOC, GPIO_Pin_2, ADC_Channel_12, false }, \
{ NULL, 0, ADC_Channel_Vrefint, false }, /* Voltage reference */ \
{ NULL, 0, ADC_Channel_TempSensor, false }, /* Temperature sensor */ \
}
/* we have to do all this to satisfy the PIOS_ADC_MAX_SAMPLES define in pios_adc.h */

11
flight/targets/boards/osd/firmware/pios_board.c
View File

@ -449,6 +449,17 @@ void PIOS_Board_Init(void)
// PIOS_TIM_InitClock(&pios_tim4_cfg);
PIOS_Video_Init(&pios_video_cfg);
#endif
#ifdef PIOS_INCLUDE_ADC
{
uint8_t adc_config[HWSETTINGS_ADCROUTING_NUMELEM];
HwSettingsADCRoutingArrayGet(adc_config);
for (uint32_t i = 0; i < HWSETTINGS_ADCROUTING_NUMELEM; i++) {
if (adc_config[i] != HWSETTINGS_ADCROUTING_DISABLED) {
PIOS_ADC_PinSetup(i);
}
}
}
#endif // PIOS_INCLUDE_ADC
}
uint16_t supv_timer = 0;

14
flight/targets/boards/osd/pios_board.h
View File

@ -208,13 +208,13 @@ extern uint32_t pios_com_telem_usb_id;
#define PIOS_DMA_PIN_CONFIG \
{ \
{ GPIOC, GPIO_Pin_0, ADC_Channel_10 }, \
{ GPIOC, GPIO_Pin_1, ADC_Channel_11 }, \
{ GPIOC, GPIO_Pin_2, ADC_Channel_12 }, \
{ NULL, 0, ADC_Channel_TempSensor }, /* Temperature sensor */ \
{ GPIOC, GPIO_Pin_3, ADC_Channel_13 }, \
{ GPIOA, GPIO_Pin_7, ADC_Channel_7 }, \
{ NULL, 0, ADC_Channel_Vrefint } /* Voltage reference */ \
{ GPIOC, GPIO_Pin_0, ADC_Channel_10, true }, \
{ GPIOC, GPIO_Pin_1, ADC_Channel_11, true }, \
{ GPIOC, GPIO_Pin_2, ADC_Channel_12, true }, \
{ NULL, 0, ADC_Channel_TempSensor, true }, /* Temperature sensor */ \
{ GPIOC, GPIO_Pin_3, ADC_Channel_13, true }, \
{ GPIOA, GPIO_Pin_7, ADC_Channel_7, true }, \
{ NULL, 0, ADC_Channel_Vrefint, true } /* Voltage reference */ \
}
/* we have to do all this to satisfy the PIOS_ADC_MAX_SAMPLES define in pios_adc.h */

5
flight/targets/boards/revolution/firmware/UAVObjects.inc
View File

@ -19,6 +19,10 @@
# These are the UAVObjects supposed to be build as part of the OpenPilot target
# (all architectures)
UAVOBJSRCFILENAMES =
UAVOBJSRCFILENAMES += statusgrounddrive
UAVOBJSRCFILENAMES += statusvtolautotakeoff
UAVOBJSRCFILENAMES += pidstatus
UAVOBJSRCFILENAMES += statusvtolland
UAVOBJSRCFILENAMES += vtolselftuningstats
UAVOBJSRCFILENAMES += accelgyrosettings
UAVOBJSRCFILENAMES += accessorydesired
@ -61,6 +65,7 @@ UAVOBJSRCFILENAMES += gpsextendedstatus
UAVOBJSRCFILENAMES += fixedwingpathfollowersettings
UAVOBJSRCFILENAMES += fixedwingpathfollowerstatus
UAVOBJSRCFILENAMES += vtolpathfollowersettings
UAVOBJSRCFILENAMES += groundpathfollowersettings
UAVOBJSRCFILENAMES += homelocation
UAVOBJSRCFILENAMES += i2cstats
UAVOBJSRCFILENAMES += manualcontrolcommand

17
flight/targets/boards/revolution/firmware/pios_board.c
View File

@ -741,10 +741,7 @@ void PIOS_Board_Init(void)
tx_buffer, PIOS_COM_RFM22B_RF_TX_BUF_LEN)) {
PIOS_Assert(0);
}
/* Set Telemetry to use OPLinkMini if no other telemetry is configured (USB always overrides anyway) */
if (!pios_com_telem_rf_id) {
pios_com_telem_rf_id = pios_com_rf_id;
}
oplinkStatus.LinkState = OPLINKSTATUS_LINKSTATE_ENABLED;
// Set the RF data rate on the modem to ~2X the selected buad rate because the modem is half duplex.
@ -947,8 +944,18 @@ void PIOS_Board_Init(void)
if (ws2811_pin_settings != HWSETTINGS_WS2811LED_OUT_DISABLED && ws2811_pin_settings < NELEMENTS(pios_ws2811_pin_cfg)) {
PIOS_WS2811_Init(&pios_ws2811_cfg, &pios_ws2811_pin_cfg[ws2811_pin_settings]);
}
#endif // PIOS_INCLUDE_WS2811
#ifdef PIOS_INCLUDE_ADC
{
uint8_t adc_config[HWSETTINGS_ADCROUTING_NUMELEM];
HwSettingsADCRoutingArrayGet(adc_config);
for (uint32_t i = 0; i < HWSETTINGS_ADCROUTING_NUMELEM; i++) {
if (adc_config[i] != HWSETTINGS_ADCROUTING_DISABLED) {
PIOS_ADC_PinSetup(i);
}
}
}
#endif // PIOS_INCLUDE_ADC
}
/**

20
flight/targets/boards/revolution/pios_board.h
View File

@ -281,23 +281,27 @@ extern uint32_t pios_packet_handler;
// PIOS_ADC_PinGet(4) = VREF
// PIOS_ADC_PinGet(5) = Temperature sensor
// -------------------------
#define PIOS_DMA_PIN_CONFIG \
{ \
{ GPIOC, GPIO_Pin_1, ADC_Channel_11 }, \
{ GPIOC, GPIO_Pin_2, ADC_Channel_12 }, \
{ NULL, 0, ADC_Channel_Vrefint }, /* Voltage reference */ \
{ NULL, 0, ADC_Channel_TempSensor }, /* Temperature sensor */ \
#define PIOS_DMA_PIN_CONFIG \
{ \
{ GPIOC, GPIO_Pin_1, ADC_Channel_11, false }, /* batt/sonar pin 3 */ \
{ GPIOC, GPIO_Pin_2, ADC_Channel_12, false }, /* batt/sonar pin 4 */ \
{ GPIOA, GPIO_Pin_3, ADC_Channel_3, false }, /* Servo pin 3 */ \
{ GPIOA, GPIO_Pin_2, ADC_Channel_2, false }, /* Servo pin 4 */ \
{ GPIOA, GPIO_Pin_1, ADC_Channel_1, false }, /* Servo pin 5 */ \
{ GPIOA, GPIO_Pin_0, ADC_Channel_9, false }, /* Servo pin 6 */ \
{ NULL, 0, ADC_Channel_Vrefint, false }, /* Voltage reference */ \
{ NULL, 0, ADC_Channel_TempSensor, false }, /* Temperature sensor */ \
}
/* we have to do all this to satisfy the PIOS_ADC_MAX_SAMPLES define in pios_adc.h */
/* which is annoying because this then determines the rate at which we generate buffer turnover events */
/* the objective here is to get enough buffer space to support 100Hz averaging rate */
#define PIOS_ADC_NUM_CHANNELS 4
#define PIOS_ADC_NUM_CHANNELS 8
#define PIOS_ADC_MAX_OVERSAMPLING 2
#define PIOS_ADC_USE_ADC2 0
#define PIOS_ADC_USE_TEMP_SENSOR
#define PIOS_ADC_TEMPERATURE_PIN 3
#define PIOS_ADC_TEMPERATURE_PIN 7
// -------------------------
// USB

5
flight/targets/boards/revoproto/firmware/UAVObjects.inc
View File

@ -19,6 +19,10 @@
# These are the UAVObjects supposed to be build as part of the OpenPilot target
# (all architectures)
UAVOBJSRCFILENAMES =
UAVOBJSRCFILENAMES += statusgrounddrive
UAVOBJSRCFILENAMES += statusvtolland
UAVOBJSRCFILENAMES += statusvtolautotakeoff
UAVOBJSRCFILENAMES += pidstatus
UAVOBJSRCFILENAMES += vtolselftuningstats
UAVOBJSRCFILENAMES += accelgyrosettings
UAVOBJSRCFILENAMES += accessorydesired
@ -61,6 +65,7 @@ UAVOBJSRCFILENAMES += gpsextendedstatus
UAVOBJSRCFILENAMES += fixedwingpathfollowersettings
UAVOBJSRCFILENAMES += fixedwingpathfollowerstatus
UAVOBJSRCFILENAMES += vtolpathfollowersettings
UAVOBJSRCFILENAMES += groundpathfollowersettings
UAVOBJSRCFILENAMES += homelocation
UAVOBJSRCFILENAMES += i2cstats
UAVOBJSRCFILENAMES += manualcontrolcommand

11
flight/targets/boards/revoproto/firmware/pios_board.c
View File

@ -926,6 +926,17 @@ void PIOS_Board_Init(void)
default:
PIOS_DEBUG_Assert(0);
}
#ifdef PIOS_INCLUDE_ADC
{
uint8_t adc_config[HWSETTINGS_ADCROUTING_NUMELEM];
HwSettingsADCRoutingArrayGet(adc_config);
for (uint32_t i = 0; i < HWSETTINGS_ADCROUTING_NUMELEM; i++) {
if (adc_config[i] != HWSETTINGS_ADCROUTING_DISABLED) {
PIOS_ADC_PinSetup(i);
}
}
}
#endif // PIOS_INCLUDE_ADC
}
/**

10
flight/targets/boards/revoproto/pios_board.h
View File

@ -242,11 +242,11 @@ extern uint32_t pios_com_hkosd_id;
// -------------------------
#define PIOS_DMA_PIN_CONFIG \
{ \
{ GPIOC, GPIO_Pin_0, ADC_Channel_10 }, \
{ GPIOC, GPIO_Pin_1, ADC_Channel_11 }, \
{ NULL, 0, ADC_Channel_Vrefint }, /* Voltage reference */ \
{ NULL, 0, ADC_Channel_TempSensor }, /* Temperature sensor */ \
{ GPIOC, GPIO_Pin_2, ADC_Channel_12 } \
{ GPIOC, GPIO_Pin_0, ADC_Channel_10, true }, \
{ GPIOC, GPIO_Pin_1, ADC_Channel_11, true }, \
{ NULL, 0, ADC_Channel_Vrefint, true }, /* Voltage reference */ \
{ NULL, 0, ADC_Channel_TempSensor, true }, /* Temperature sensor */ \
{ GPIOC, GPIO_Pin_2, ADC_Channel_12, true } \
}
/* we have to do all this to satisfy the PIOS_ADC_MAX_SAMPLES define in pios_adc.h */

11
flight/targets/boards/simposix/firmware/Makefile
View File

@ -20,6 +20,9 @@
# 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
#####
# REVO C++ support
USE_CXX = YES
override ARM_SDK_PREFIX :=
override THUMB :=
@ -83,10 +86,11 @@ endif
## MODULES
SRC += ${foreach MOD, ${MODULES}, ${wildcard ${OPMODULEDIR}/${MOD}/*.c}}
CPPSRC += ${foreach MOD, ${MODULES}, ${wildcard ${OPMODULEDIR}/${MOD}/*.cpp}}
SRC += ${OUTDIR}/InitMods.c
## OPENPILOT CORE:
SRC += ${OPMODULEDIR}/System/systemmod.c
SRC += $(OPSYSTEM)/simposix.c
CPPSRC += $(OPSYSTEM)/simposix.cpp
SRC += $(OPSYSTEM)/pios_board.c
SRC += $(FLIGHTLIB)/alarms.c
SRC += $(OPUAVTALK)/uavtalk.c
@ -124,6 +128,7 @@ SRC += $(PIOSCORECOMMON)/pios_mem.c
## PIOS Hardware
include $(PIOS)/posix/library.mk
include ./UAVObjects.inc
SRC += $(UAVOBJSRC)
@ -315,7 +320,7 @@ endif
# Link: create ELF output file from object files.
$(eval $(call LINK_TEMPLATE, $(OUTDIR)/$(TARGET).elf, $(ALLOBJ)))
$(eval $(call LINK_CXX_TEMPLATE, $(OUTDIR)/$(TARGET).elf, $(ALLOBJ)))
# Assemble: create object files from assembler source files.
$(foreach src, $(ASRC), $(eval $(call ASSEMBLE_TEMPLATE, $(src))))
@ -330,7 +335,7 @@ $(foreach src, $(SRC), $(eval $(call COMPILE_C_TEMPLATE, $(src))))
$(foreach src, $(SRCARM), $(eval $(call COMPILE_C_ARM_TEMPLATE, $(src))))
# Compile: create object files from C++ source files.
$(foreach src, $(CPPSRC), $(eval $(call COMPILE_CPP_TEMPLATE, $(src))))
$(foreach src, $(CPPSRC), $(eval $(call COMPILE_CXX_TEMPLATE, $(src))))
# Compile: create object files from C++ source files. ARM-only
$(foreach src, $(CPPSRCARM), $(eval $(call COMPILE_CPP_ARM_TEMPLATE, $(src))))

5
flight/targets/boards/simposix/firmware/UAVObjects.inc
View File

@ -24,6 +24,10 @@
# (all architectures)
UAVOBJSRCFILENAMES =
UAVOBJSRCFILENAMES += statusgrounddrive
UAVOBJSRCFILENAMES += statusvtolautotakeoff
UAVOBJSRCFILENAMES += pidstatus
UAVOBJSRCFILENAMES += statusvtolland
UAVOBJSRCFILENAMES += vtolselftuningstats
UAVOBJSRCFILENAMES += accessorydesired
UAVOBJSRCFILENAMES += actuatorcommand
@ -62,6 +66,7 @@ UAVOBJSRCFILENAMES += gpstime
UAVOBJSRCFILENAMES += gpsvelocitysensor
UAVOBJSRCFILENAMES += gpssettings
UAVOBJSRCFILENAMES += vtolpathfollowersettings
UAVOBJSRCFILENAMES += groundpathfollowersettings
UAVOBJSRCFILENAMES += fixedwingpathfollowersettings
UAVOBJSRCFILENAMES += fixedwingpathfollowerstatus
UAVOBJSRCFILENAMES += homelocation

2
flight/targets/boards/simposix/firmware/simposix.c → flight/targets/boards/simposix/firmware/simposix.cpp
View File

@ -31,6 +31,7 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
extern "C" {
#include "inc/openpilot.h"
#include <systemmod.h>
#include <uavobjectsinit.h>
@ -74,6 +75,7 @@ static void initTask(void *parameters);
/* Prototype of generated InitModules() function */
extern void InitModules(void);
}
/**
* OpenPilot Main function:

4
flight/uavtalk/inc/uavtalk.h
View File

@ -57,8 +57,8 @@ UAVTalkOutputStream UAVTalkGetOutputStream(UAVTalkConnection connection);
int32_t UAVTalkSendObject(UAVTalkConnection connection, UAVObjHandle obj, uint16_t instId, uint8_t acked, int32_t timeoutMs);
int32_t UAVTalkSendObjectTimestamped(UAVTalkConnection connectionHandle, UAVObjHandle obj, uint16_t instId, uint8_t acked, int32_t timeoutMs);
int32_t UAVTalkSendObjectRequest(UAVTalkConnection connection, UAVObjHandle obj, uint16_t instId, int32_t timeoutMs);
UAVTalkRxState UAVTalkProcessInputStream(UAVTalkConnection connection, uint8_t rxbyte);
UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connection, uint8_t rxbyte);
UAVTalkRxState UAVTalkProcessInputStream(UAVTalkConnection connectionHandle, uint8_t *rxbuffer, uint8_t length);
UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle, uint8_t *rxbuffer, uint8_t length, uint8_t *position);
int32_t UAVTalkRelayPacket(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle);
int32_t UAVTalkReceiveObject(UAVTalkConnection connectionHandle);
void UAVTalkGetStats(UAVTalkConnection connection, UAVTalkStats *stats, bool reset);

470
flight/uavtalk/uavtalk.c
View File

@ -53,7 +53,15 @@ static int32_t sendObject(UAVTalkConnectionData *connection, uint8_t type, uint3
static int32_t sendSingleObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, UAVObjHandle obj);
static int32_t receiveObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, uint8_t *data);
static void updateAck(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId);
// UavTalk Process FSM functions
static bool UAVTalkProcess_SYNC(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position);
static bool UAVTalkProcess_TYPE(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position);
static bool UAVTalkProcess_OBJID(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position);
static bool UAVTalkProcess_INSTID(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position);
static bool UAVTalkProcess_SIZE(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position);
static bool UAVTalkProcess_TIMESTAMP(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position);
static bool UAVTalkProcess_DATA(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position);
static bool UAVTalkProcess_CS(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position);
/**
* Initialize the UAVTalk library
* \param[in] connection UAVTalkConnection to be used
@ -350,10 +358,12 @@ static int32_t objectTransaction(UAVTalkConnectionData *connection, uint8_t type
/**
* Process an byte from the telemetry stream.
* \param[in] connectionHandle UAVTalkConnection to be used
* \param[in] rxbyte Received byte
* \param[in] rxbuffer Received buffer
* \param[in/out] Length in bytes of received buffer
* \param[in/out] position Next item to be read inside rxbuffer
* \return UAVTalkRxState
*/
UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle, uint8_t rxbyte)
UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle, uint8_t *rxbuffer, uint8_t length, uint8_t *position)
{
UAVTalkConnectionData *connection;
@ -361,231 +371,83 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle
UAVTalkInputProcessor *iproc = &connection->iproc;
++connection->stats.rxBytes;
if (iproc->state == UAVTALK_STATE_ERROR || iproc->state == UAVTALK_STATE_COMPLETE) {
iproc->state = UAVTALK_STATE_SYNC;
}
if (iproc->rxPacketLength < 0xffff) {
// update packet byte count
iproc->rxPacketLength++;
}
uint8_t processedBytes = (*position);
uint8_t count = 0;
// Receive state machine
switch (iproc->state) {
case UAVTALK_STATE_SYNC:
if (rxbyte != UAVTALK_SYNC_VAL) {
connection->stats.rxSyncErrors++;
// stop processing as soon as a complete packet is received, error is encountered or buffer is processed entirely
while ((count = length - (*position)) > 0
&& iproc->state != UAVTALK_STATE_COMPLETE
&& iproc->state != UAVTALK_STATE_ERROR) {
// Receive state machine
if (iproc->state == UAVTALK_STATE_SYNC &&
!UAVTalkProcess_SYNC(connection, iproc, rxbuffer, length, position)) {
break;
}
// Initialize and update the CRC
iproc->cs = PIOS_CRC_updateByte(0, rxbyte);
iproc->rxPacketLength = 1;
iproc->rxCount = 0;
iproc->type = 0;
iproc->state = UAVTALK_STATE_TYPE;
break;
case UAVTALK_STATE_TYPE:
if ((rxbyte & UAVTALK_TYPE_MASK) != UAVTALK_TYPE_VER) {
connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_SYNC;
if (iproc->state == UAVTALK_STATE_TYPE &&
!UAVTalkProcess_TYPE(connection, iproc, rxbuffer, length, position)) {
break;
}
// update the CRC
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
iproc->type = rxbyte;
iproc->packet_size = 0;
iproc->state = UAVTALK_STATE_SIZE;
break;
case UAVTALK_STATE_SIZE:
// update the CRC
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
if (iproc->rxCount == 0) {
iproc->packet_size += rxbyte;
iproc->rxCount++;
break;
}
iproc->packet_size += rxbyte << 8;
iproc->rxCount = 0;
if (iproc->packet_size < UAVTALK_MIN_HEADER_LENGTH || iproc->packet_size > UAVTALK_MAX_HEADER_LENGTH + UAVTALK_MAX_PAYLOAD_LENGTH) {
// incorrect packet size
connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_ERROR;
if (iproc->state == UAVTALK_STATE_SIZE &&
!UAVTalkProcess_SIZE(connection, iproc, rxbuffer, length, position)) {
break;
}
iproc->objId = 0;
iproc->state = UAVTALK_STATE_OBJID;
break;
case UAVTALK_STATE_OBJID:
// update the CRC
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
iproc->objId += rxbyte << (8 * (iproc->rxCount++));
if (iproc->rxCount < 4) {
break;
}
iproc->rxCount = 0;
iproc->instId = 0;
iproc->state = UAVTALK_STATE_INSTID;
break;
case UAVTALK_STATE_INSTID:
// update the CRC
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
iproc->instId += rxbyte << (8 * (iproc->rxCount++));
if (iproc->rxCount < 2) {
break;
}
iproc->rxCount = 0;
UAVObjHandle obj = UAVObjGetByID(iproc->objId);
// Determine data length
if (iproc->type == UAVTALK_TYPE_OBJ_REQ || iproc->type == UAVTALK_TYPE_ACK || iproc->type == UAVTALK_TYPE_NACK) {
iproc->length = 0;
iproc->timestampLength = 0;
} else {
iproc->timestampLength = (iproc->type & UAVTALK_TIMESTAMPED) ? 2 : 0;
if (obj) {
iproc->length = UAVObjGetNumBytes(obj);
} else {
iproc->length = iproc->packet_size - iproc->rxPacketLength - iproc->timestampLength;
}
}
// Check length
if (iproc->length >= UAVTALK_MAX_PAYLOAD_LENGTH) {
// packet error - exceeded payload max length
connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_ERROR;
if (iproc->state == UAVTALK_STATE_OBJID &&
!UAVTalkProcess_OBJID(connection, iproc, rxbuffer, length, position)) {
break;
}
// Check the lengths match
if ((iproc->rxPacketLength + iproc->timestampLength + iproc->length) != iproc->packet_size) {
// packet error - mismatched packet size
connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_ERROR;
if (iproc->state == UAVTALK_STATE_INSTID &&
!UAVTalkProcess_INSTID(connection, iproc, rxbuffer, length, position)) {
break;
}
// Determine next state
if (iproc->type & UAVTALK_TIMESTAMPED) {
// If there is a timestamp get it
iproc->timestamp = 0;
iproc->state = UAVTALK_STATE_TIMESTAMP;
} else {
// If there is a payload get it, otherwise receive checksum
if (iproc->length > 0) {
iproc->state = UAVTALK_STATE_DATA;
} else {
iproc->state = UAVTALK_STATE_CS;
}
}
break;
case UAVTALK_STATE_TIMESTAMP:
// update the CRC
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
iproc->timestamp += rxbyte << (8 * (iproc->rxCount++));
if (iproc->rxCount < 2) {
break;
}
iproc->rxCount = 0;
// If there is a payload get it, otherwise receive checksum
if (iproc->length > 0) {
iproc->state = UAVTALK_STATE_DATA;
} else {
iproc->state = UAVTALK_STATE_CS;
}
break;
case UAVTALK_STATE_DATA:
// update the CRC
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
connection->rxBuffer[iproc->rxCount++] = rxbyte;
if (iproc->rxCount < iproc->length) {
break;
}
iproc->rxCount = 0;
iproc->state = UAVTALK_STATE_CS;
break;
case UAVTALK_STATE_CS:
// Check the CRC byte
if (rxbyte != iproc->cs) {
// packet error - faulty CRC
UAVT_DEBUGLOG_PRINTF("BAD CRC");
connection->stats.rxCrcErrors++;
connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_ERROR;
if (iproc->state == UAVTALK_STATE_TIMESTAMP &&
!UAVTalkProcess_TIMESTAMP(connection, iproc, rxbuffer, length, position)) {
break;
}
if (iproc->rxPacketLength != (iproc->packet_size + UAVTALK_CHECKSUM_LENGTH)) {
// packet error - mismatched packet size
connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_ERROR;
if (iproc->state == UAVTALK_STATE_DATA &&
!UAVTalkProcess_DATA(connection, iproc, rxbuffer, length, position)) {
break;
}
connection->stats.rxObjects++;
connection->stats.rxObjectBytes += iproc->length;
iproc->state = UAVTALK_STATE_COMPLETE;
break;
default:
iproc->state = UAVTALK_STATE_ERROR;
break;
if (iproc->state == UAVTALK_STATE_CS &&
!UAVTalkProcess_CS(connection, iproc, rxbuffer, length, position)) {
break;
}
}
// Done
processedBytes = (*position) - processedBytes;
connection->stats.rxBytes += processedBytes;
return iproc->state;
}
/**
* Process an byte from the telemetry stream.
* Process a buffer from the telemetry stream.
* \param[in] connection UAVTalkConnection to be used
* \param[in] rxbyte Received byte
* \param[in] rxbuffer Received buffer
* \param[in] count bytes inside rxbuffer
* \return UAVTalkRxState
*/
UAVTalkRxState UAVTalkProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rxbyte)
UAVTalkRxState UAVTalkProcessInputStream(UAVTalkConnection connectionHandle, uint8_t *rxbuffer, uint8_t length)
{
UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(connectionHandle, rxbyte);
uint8_t position = 0;
UAVTalkRxState state = UAVTALK_STATE_ERROR;
if (state == UAVTALK_STATE_COMPLETE) {
UAVTalkReceiveObject(connectionHandle);
while (position < length) {
state = UAVTalkProcessInputStreamQuiet(connectionHandle, rxbuffer, length, &position);
if (state == UAVTALK_STATE_COMPLETE) {
UAVTalkReceiveObject(connectionHandle);
}
}
return state;
}
@ -1000,6 +862,236 @@ static int32_t sendSingleObject(UAVTalkConnectionData *connection, uint8_t type,
return 0;
}
/*
* Functions that implements the UAVTalk Process FSM. return false to break out of current cycle
*/
static bool UAVTalkProcess_SYNC(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, __attribute__((unused)) uint8_t length, uint8_t *position)
{
uint8_t rxbyte = rxbuffer[(*position)++];
if (rxbyte != UAVTALK_SYNC_VAL) {
connection->stats.rxSyncErrors++;
return false;
}
// Initialize and update the CRC
iproc->cs = PIOS_CRC_updateByte(0, rxbyte);
iproc->rxPacketLength = 1;
iproc->rxCount = 0;
iproc->type = 0;
iproc->state = UAVTALK_STATE_TYPE;
return true;
}
static bool UAVTalkProcess_TYPE(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, __attribute__((unused)) uint8_t length, uint8_t *position)
{
uint8_t rxbyte = rxbuffer[(*position)++];
if ((rxbyte & UAVTALK_TYPE_MASK) != UAVTALK_TYPE_VER) {
connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_SYNC;
return false;
}
// update the CRC
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
iproc->type = rxbyte;
iproc->rxPacketLength++;
iproc->packet_size = 0;
iproc->state = UAVTALK_STATE_SIZE;
return true;
}
static bool UAVTalkProcess_SIZE(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position)
{
while (iproc->rxCount < 2 && length > (*position)) {
uint8_t rxbyte = rxbuffer[(*position)++];
// update the CRC
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
iproc->packet_size += rxbyte << 8 * iproc->rxCount;
iproc->rxCount++;
}
if (iproc->rxCount < 2) {
return false;;
}
iproc->rxCount = 0;
if (iproc->packet_size < UAVTALK_MIN_HEADER_LENGTH || iproc->packet_size > UAVTALK_MAX_HEADER_LENGTH + UAVTALK_MAX_PAYLOAD_LENGTH) {
// incorrect packet size
connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_ERROR;
return false;
}
iproc->rxPacketLength += 2;
iproc->objId = 0;
iproc->state = UAVTALK_STATE_OBJID;
return true;
}
static bool UAVTalkProcess_OBJID(__attribute__((unused)) UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position)
{
while (iproc->rxCount < 4 && length > (*position)) {
uint8_t rxbyte = rxbuffer[(*position)++];
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
iproc->objId += rxbyte << (8 * (iproc->rxCount++));
}
if (iproc->rxCount < 4) {
return false;
}
iproc->rxCount = 0;
iproc->rxPacketLength += 4;
iproc->instId = 0;
iproc->state = UAVTALK_STATE_INSTID;
return true;
}
static bool UAVTalkProcess_INSTID(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position)
{
while (iproc->rxCount < 2 && length > (*position)) {
uint8_t rxbyte = rxbuffer[(*position)++];
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
iproc->instId += rxbyte << (8 * (iproc->rxCount++));
}
if (iproc->rxCount < 2) {
return false;
}
iproc->rxPacketLength += 2;
iproc->rxCount = 0;
UAVObjHandle obj = UAVObjGetByID(iproc->objId);
// Determine data length
if (iproc->type == UAVTALK_TYPE_OBJ_REQ || iproc->type == UAVTALK_TYPE_ACK || iproc->type == UAVTALK_TYPE_NACK) {
iproc->length = 0;
iproc->timestampLength = 0;
} else {
iproc->timestampLength = (iproc->type & UAVTALK_TIMESTAMPED) ? 2 : 0;
if (obj) {
iproc->length = UAVObjGetNumBytes(obj);
} else {
iproc->length = iproc->packet_size - iproc->rxPacketLength - iproc->timestampLength;
}
}
// Check length
if (iproc->length >= UAVTALK_MAX_PAYLOAD_LENGTH) {
// packet error - exceeded payload max length
connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_ERROR;
return false;
}
// Check the lengths match
if ((iproc->rxPacketLength + iproc->timestampLength + iproc->length) != iproc->packet_size) {
// packet error - mismatched packet size
connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_ERROR;
return false;
}
// Determine next state
if (iproc->type & UAVTALK_TIMESTAMPED) {
// If there is a timestamp get it
iproc->timestamp = 0;
iproc->state = UAVTALK_STATE_TIMESTAMP;
} else {
// If there is a payload get it, otherwise receive checksum
if (iproc->length > 0) {
iproc->state = UAVTALK_STATE_DATA;
} else {
iproc->state = UAVTALK_STATE_CS;
}
}
return true;
}
static bool UAVTalkProcess_TIMESTAMP(__attribute__((unused)) UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position)
{
while (iproc->rxCount < 2 && length > (*position)) {
uint8_t rxbyte = rxbuffer[(*position)++];
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
iproc->timestamp += rxbyte << (8 * (iproc->rxCount++));
}
if (iproc->rxCount < 2) {
return false;;
}
iproc->rxCount = 0;
iproc->rxPacketLength += 2;
// If there is a payload get it, otherwise receive checksum
if (iproc->length > 0) {
iproc->state = UAVTALK_STATE_DATA;
} else {
iproc->state = UAVTALK_STATE_CS;
}
return true;
}
static bool UAVTalkProcess_DATA(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position)
{
uint8_t toCopy = iproc->length - iproc->rxCount;
if (toCopy > length - (*position)) {
toCopy = length - (*position);
}
memcpy(&connection->rxBuffer[iproc->rxCount], &rxbuffer[(*position)], toCopy);
(*position) += toCopy;
// update the CRC
iproc->cs = PIOS_CRC_updateCRC(iproc->cs, &connection->rxBuffer[iproc->rxCount], toCopy);
iproc->rxCount += toCopy;
iproc->rxPacketLength += toCopy;
if (iproc->rxCount < iproc->length) {
return false;
}
iproc->rxCount = 0;
iproc->state = UAVTALK_STATE_CS;
return true;
}
static bool UAVTalkProcess_CS(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, __attribute__((unused)) uint8_t length, uint8_t *position)
{
// Check the CRC byte
uint8_t rxbyte = rxbuffer[(*position)++];
if (rxbyte != iproc->cs) {
// packet error - faulty CRC
UAVT_DEBUGLOG_PRINTF("BAD CRC");
connection->stats.rxCrcErrors++;
connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_ERROR;
return false;;
}
iproc->rxPacketLength++;
if (iproc->rxPacketLength != (iproc->packet_size + UAVTALK_CHECKSUM_LENGTH)) {
// packet error - mismatched packet size
connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_ERROR;
return false;;
}
connection->stats.rxObjects++;
connection->stats.rxObjectBytes += iproc->length;
iproc->state = UAVTALK_STATE_COMPLETE;
return true;
}
/**
* @}
* @}

233
ground/openpilotgcs/copydata.pro
View File

@ -1,14 +1,10 @@
include(openpilotgcs.pri)
TEMPLATE = subdirs
TEMPLATE = aux
# Copy Qt runtime libraries into the build directory (to run or package)
equals(copyqt, 1) {
GCS_LIBRARY_PATH
linux-* {
linux {
QT_LIBS = libQt5Core.so.5 \
libQt5Gui.so.5 \
libQt5Widgets.so.5 \
@ -34,87 +30,21 @@ GCS_LIBRARY_PATH
libicui18n.so.53 \
libicuuc.so.53 \
libicudata.so.53
data_copy.commands += -@$(MKDIR) $$targetPath(\"$$GCS_QT_LIBRARY_PATH\") $$addNewline()
for(lib, QT_LIBS) {
data_copy.commands += $(COPY_FILE) $$targetPath(\"$$[QT_INSTALL_LIBS]/$$lib\") $$targetPath(\"$$GCS_QT_LIBRARY_PATH/$$lib\") $$addNewline()
addCopyFileTarget($${lib},$$[QT_INSTALL_LIBS],$${GCS_QT_LIBRARY_PATH})
}
# create Qt plugin directories
QT_PLUGIN_DIRS = iconengines \
imageformats \
platforms \
mediaservice \
sqldrivers
for(dir, QT_PLUGIN_DIRS) {
data_copy.commands += -@$(MKDIR) $$targetPath(\"$$GCS_QT_PLUGINS_PATH/$$dir\") $$addNewline()
}
QT_PLUGIN_LIBS = iconengines/libqsvgicon.so \
imageformats/libqgif.so \
imageformats/libqico.so \
imageformats/libqjpeg.so \
imageformats/libqmng.so \
imageformats/libqsvg.so \
imageformats/libqtiff.so \
mediaservice/libgstaudiodecoder.so \
mediaservice/libgstmediaplayer.so \
platforms/libqxcb.so \
sqldrivers/libqsqlite.so
for(lib, QT_PLUGIN_LIBS) {
data_copy.commands += $(COPY_FILE) $$targetPath(\"$$[QT_INSTALL_PLUGINS]/$$lib\") $$targetPath(\"$$GCS_QT_PLUGINS_PATH/$$lib\") $$addNewline()
}
# create QtQuick2 plugin directories
QT_QUICK2_DIRS = QtQuick \
QtQuick.2 \
QtQuick/Layouts \
QtQuick/LocalStorage \
QtQuick/Particles.2 \
QtQuick/PrivateWidgets \
QtQuick/Window.2 \
QtQuick/XmlListModel
for(dir, QT_QUICK2_DIRS) {
data_copy.commands += -@$(MKDIR) $$targetPath(\"$$GCS_QT_QML_PATH/$$dir\") $$addNewline()
}
# Copy QtQuick2 complete directories
# These directories have a lot of files
# Easier to copy everything
QTQ_WHOLE_DIRS = QtQuick/Controls \
QtQuick/Dialogs
for(dir, QTQ_WHOLE_DIRS) {
data_copy.commands += $(COPY_DIR) $$targetPath(\"$$[QT_INSTALL_QML]/$$dir\") $$targetPath(\"$$GCS_QT_QML_PATH/$$dir\") $$addNewline()
}
# Remaining QtQuick plugin libs
QT_QUICK2_DLLS = QtQuick.2/libqtquick2plugin.so \
QtQuick.2/plugins.qmltypes \
QtQuick.2/qmldir \
QtQuick/Layouts/libqquicklayoutsplugin.so \
QtQuick/Layouts/plugins.qmltypes \
QtQuick/Layouts/qmldir \
QtQuick/LocalStorage/libqmllocalstorageplugin.so \
QtQuick/LocalStorage/plugins.qmltypes \
QtQuick/LocalStorage/qmldir \
QtQuick/Particles.2/libparticlesplugin.so \
QtQuick/Particles.2/plugins.qmltypes \
QtQuick/Particles.2/qmldir \
QtQuick/PrivateWidgets/libwidgetsplugin.so \
QtQuick/PrivateWidgets/plugins.qmltypes \
QtQuick/PrivateWidgets/qmldir \
QtQuick/Window.2/libwindowplugin.so \
QtQuick/Window.2/plugins.qmltypes \
QtQuick/Window.2/qmldir \
QtQuick/XmlListModel/libqmlxmllistmodelplugin.so \
QtQuick/XmlListModel/plugins.qmltypes \
QtQuick/XmlListModel/qmldir
for(lib, QT_QUICK2_DLLS) {
data_copy.commands += $(COPY_FILE) $$targetPath(\"$$[QT_INSTALL_QML]/$$lib\") $$targetPath(\"$$GCS_QT_QML_PATH/$$lib\") $$addNewline()
}
data_copy.target = FORCE
QMAKE_EXTRA_TARGETS += data_copy
QT_PLUGINS = iconengines/libqsvgicon.so \
imageformats/libqgif.so \
imageformats/libqico.so \
imageformats/libqjpeg.so \
imageformats/libqmng.so \
imageformats/libqsvg.so \
imageformats/libqtiff.so \
mediaservice/libgstaudiodecoder.so \
mediaservice/libgstmediaplayer.so \
platforms/libqxcb.so \
sqldrivers/libqsqlite.so
}
win32 {
@ -136,7 +66,6 @@ GCS_LIBRARY_PATH
Qt5Script$${DS}.dll \
Qt5Concurrent$${DS}.dll \
Qt5PrintSupport$${DS}.dll \
Qt5OpenGL$${DS}.dll \
Qt5SerialPort$${DS}.dll \
Qt5Multimedia$${DS}.dll \
Qt5MultimediaWidgets$${DS}.dll \
@ -150,82 +79,7 @@ GCS_LIBRARY_PATH
libstdc++-6.dll \
libwinpthread-1.dll
for(dll, QT_DLLS) {
data_copy.commands += $(COPY_FILE) $$targetPath(\"$$[QT_INSTALL_BINS]/$$dll\") $$targetPath(\"$$GCS_APP_PATH/$$dll\") $$addNewline()
}
# create Qt plugin directories
QT_PLUGIN_DIRS = iconengines \
imageformats \
platforms \
mediaservice \
sqldrivers \
opengl32_32
for(dir, QT_PLUGIN_DIRS) {
data_copy.commands += -@$(MKDIR) $$targetPath(\"$$GCS_APP_PATH/$$dir\") $$addNewline()
}
# copy Qt plugin DLLs
QT_PLUGIN_DLLS = iconengines/qsvgicon$${DS}.dll \
imageformats/qgif$${DS}.dll \
imageformats/qico$${DS}.dll \
imageformats/qjpeg$${DS}.dll \
imageformats/qmng$${DS}.dll \
imageformats/qsvg$${DS}.dll \
imageformats/qtiff$${DS}.dll \
platforms/qwindows$${DS}.dll \
mediaservice/dsengine$${DS}.dll \
sqldrivers/qsqlite$${DS}.dll
for(dll, QT_PLUGIN_DLLS) {
data_copy.commands += $(COPY_FILE) $$targetPath(\"$$[QT_INSTALL_PLUGINS]/$$dll\") $$targetPath(\"$$GCS_APP_PATH/$$dll\") $$addNewline()
}
# create QtQuick2 plugin directories
QT_QUICK2_DIRS = qtquick \
qtquick.2 \
qtquick/layouts \
qtquick/localstorage \
qtquick/particles.2 \
qtquick/privatewidgets \
qtquick/window.2 \
qtquick/xmllistmodel
for(dir, QT_QUICK2_DIRS) {
data_copy.commands += -@$(MKDIR) $$targetPath(\"$$GCS_APP_PATH/$$dir\") $$addNewline()
}
# Copy QtQuick2 complete directories
# These directories have a lot of files
# Easier to copy everything
QTQ_WHOLE_DIRS = qtquick/controls \
qtquick/dialogs
for(dir, QTQ_WHOLE_DIRS) {
data_copy.commands += $(COPY_DIR) $$targetPath(\"$$[QT_INSTALL_QML]/$$dir\") $$targetPath(\"$$GCS_APP_PATH/$$dir\") $$addNewline()
}
# Remaining QtQuick plugin DLLs
QT_QUICK2_DLLS = QtQuick.2/qtquick2plugin$${DS}.dll \
QtQuick.2/plugins.qmltypes \
QtQuick.2/qmldir \
qtquick/layouts/qquicklayoutsplugin$${DS}.dll \
qtquick/layouts/plugins.qmltypes \
qtquick/layouts/qmldir \
qtquick/localstorage/qmllocalstorageplugin$${DS}.dll \
qtquick/localstorage/plugins.qmltypes \
qtquick/localstorage/qmldir \
qtquick/particles.2/particlesplugin$${DS}.dll \
qtquick/particles.2/plugins.qmltypes \
qtquick/particles.2/qmldir \
qtquick/privatewidgets/widgetsplugin$${DS}.dll \
qtquick/privatewidgets/plugins.qmltypes \
qtquick/privatewidgets/qmldir \
qtquick/window.2/windowplugin$${DS}.dll \
qtquick/window.2/plugins.qmltypes \
qtquick/window.2/qmldir \
qtquick/XmlListModel/qmlxmllistmodelplugin$${DS}.dll \
qtquick/XmlListModel/plugins.qmltypes \
qtquick/XmlListModel/qmldir
for(dll, QT_QUICK2_DLLS) {
data_copy.commands += $(COPY_FILE) $$targetPath(\"$$[QT_INSTALL_QML]/$$dll\") $$targetPath(\"$$GCS_APP_PATH/$$dll\") $$addNewline()
addCopyFileTarget($${dll},$$[QT_INSTALL_BINS],$${GCS_APP_PATH})
}
# copy OpenSSL DLLs
@ -233,44 +87,45 @@ GCS_LIBRARY_PATH
ssleay32.dll \
libeay32.dll
for(dll, OPENSSL_DLLS) {
data_copy.commands += $(COPY_FILE) $$targetPath(\"$${OPENSSL_DIR}/$$dll\") $$targetPath(\"$$GCS_APP_PATH/$$dll\") $$addNewline()
addCopyFileTarget($${dll},$${OPENSSL_DIR},$${GCS_APP_PATH})
}
# copy OpenGL DLL
OPENGL_DLLS = \
opengl32_32/opengl32.dll
for(dll, OPENGL_DLLS) {
data_copy.commands += $(COPY_FILE) $$targetPath(\"$${MESAWIN_DIR}/$$dll\") $$targetPath(\"$$GCS_APP_PATH/$$dll\") $$addNewline()
addCopyFileTarget($${dll},$${MESAWIN_DIR},$${GCS_APP_PATH})
}
data_copy.target = FORCE
QMAKE_EXTRA_TARGETS += data_copy
QT_PLUGINS = iconengines/qsvgicon$${DS}.dll \
imageformats/qgif$${DS}.dll \
imageformats/qico$${DS}.dll \
imageformats/qjpeg$${DS}.dll \
imageformats/qmng$${DS}.dll \
imageformats/qsvg$${DS}.dll \
imageformats/qtiff$${DS}.dll \
platforms/qwindows$${DS}.dll \
mediaservice/dsengine$${DS}.dll \
sqldrivers/qsqlite$${DS}.dll
}
for(plugin, QT_PLUGINS) {
addCopyFileTarget($${plugin},$$[QT_INSTALL_PLUGINS],$${GCS_QT_PLUGINS_PATH})
}
macx{
#NOTE: debug dylib can be copied as they will be cleaned out with packaging scripts
#standard plugins directory (will copy just dylib, plugins.qmltypes and qmldir
QT_QUICK2_PLUGINS = QtQuick.2 QtQuick/Layouts QtQuick/LocalStorage QtQuick/Particles.2 QtQuick/PrivateWidgets QtQuick/Window.2 QtQuick/XmlListModel
#those directories will be fully copied to dest
QT_QUICK2_FULL_DIRS = QtQuick/Controls QtQuick/Dialogs
#create QtQuick dir (that will host all subdirs)
data_copy.commands += -@$(MKDIR) $$targetPath(\"$$GCS_QT_QML_PATH/QtQuick\") $$addNewline()
for(dir, QT_QUICK2_FULL_DIRS) {
#data_copy.commands += -@$(MKDIR) $$targetPath(\"$$GCS_QT_QML_PATH/$$dir\") $$addNewline()
data_copy.commands += $(COPY_DIR) $$targetPath(\"$$[QT_INSTALL_QML]/$$dir\") $$targetPath(\"$$GCS_QT_QML_PATH/$$dir\") $$addNewline()
}
for(lib, QT_QUICK2_PLUGINS) {
data_copy.commands += $(MKDIR) $$targetPath(\"$$GCS_QT_QML_PATH/$$lib\") $$addNewline()
data_copy.commands += $(COPY_FILE) $$targetPath(\"$$[QT_INSTALL_QML]/$$lib/\"*.dylib) $$targetPath(\"$$GCS_QT_QML_PATH/$$lib/\") $$addNewline()
data_copy.commands += $(COPY_FILE) $$targetPath(\"$$[QT_INSTALL_QML]/$$lib/plugins.qmltypes\") $$targetPath(\"$$GCS_QT_QML_PATH/$$lib/plugins.qmltypes\") $$addNewline()
data_copy.commands += $(COPY_FILE) $$targetPath(\"$$[QT_INSTALL_QML]/$$lib/qmldir\") $$targetPath(\"$$GCS_QT_QML_PATH/$$lib/qmldir\") $$addNewline()
}
data_copy.target = FORCE
QMAKE_EXTRA_TARGETS += data_copy
# Copy QtQuick2 complete directories
# Some of these directories have a lot of files
# Easier to copy everything
QT_QUICK2_DIRS = QtQuick/Controls \
QtQuick/Dialogs \
QtQuick/Layouts \
QtQuick/LocalStorage \
QtQuick/Particles.2 \
QtQuick/PrivateWidgets \
QtQuick/Window.2 \
QtQuick/XmlListModel \
QtQuick.2
for(dir, QT_QUICK2_DIRS) {
addCopyDirTarget($${dir},$$[QT_INSTALL_QML],$${GCS_QT_QML_PATH})
}
}

55
ground/openpilotgcs/openpilotgcs.pri
View File

@ -32,6 +32,57 @@ defineReplace(qtLibraryName) {
return($$RET)
}
defineTest(addCopyFileTarget) {
file = $$1
src = $$2/$$1
dest = $$3/$$1
$${file}.target = $$dest
$${file}.depends = $$src
# create directory. Better would be an order only dependency
$${file}.commands = -@$(MKDIR) \"$$targetPath($$dirname(dest))\" $$addNewline()
$${file}.commands += $(COPY_FILE) \"$$targetPath($$src)\" \"$$targetPath($$dest)\"
QMAKE_EXTRA_TARGETS += $$file
POST_TARGETDEPS += $$eval($${file}.target)
export($${file}.target)
export($${file}.depends)
export($${file}.commands)
export(QMAKE_EXTRA_TARGETS)
export(POST_TARGETDEPS)
return(true)
}
defineTest(addCopyDirTarget) {
dir = $$1
src = $$2/$$1
dest = $$3/$$1
$${dir}.target = $$dest
$${dir}.depends = $$src
# Windows does not update directory timestamp if files are modified
win32: $${dir}.depends += FORCE
$${dir}.commands = @rm -rf \"$$targetPath($$dest)\" $$addNewline()
# create directory. Better would be an order only dependency
$${dir}.commands += -@$(MKDIR) \"$$targetPath($$dirname(dest))\" $$addNewline()
$${dir}.commands += $(COPY_DIR) \"$$targetPath($$src)\" \"$$targetPath($$dest)\"
QMAKE_EXTRA_TARGETS += $$dir
POST_TARGETDEPS += $$eval($${dir}.target)
export($${dir}.target)
export($${dir}.depends)
export($${dir}.commands)
export(QMAKE_EXTRA_TARGETS)
export(POST_TARGETDEPS)
return(true)
}
# For use in custom compilers which just copy files
win32:i_flag = i
defineReplace(stripSrcDir) {
@ -119,6 +170,10 @@ macx {
contains(TEMPLATE, vc.*)|contains(TEMPLATE_PREFIX, vc):vcproj = 1
GCS_APP_TARGET = openpilotgcs
GCS_QT_PLUGINS_PATH = $$GCS_APP_PATH
GCS_QT_QML_PATH = $$GCS_APP_PATH
copyqt = $$copydata
} else {
GCS_APP_TARGET = openpilotgcs

13
ground/openpilotgcs/share/copydata.pro Normal file
View File

@ -0,0 +1,13 @@
include(../openpilotgcs.pri)
TEMPLATE = aux
DATACOLLECTIONS = cloudconfig default_configurations dials models pfd sounds diagrams mapicons stylesheets
equals(copydata, 1) {
for(dir, DATACOLLECTIONS) {
exists($$GCS_SOURCE_TREE/share/openpilotgcs/$$dir) {
addCopyDirTarget($$dir, $$GCS_SOURCE_TREE/share/openpilotgcs, $$GCS_DATA_PATH)
}
}
}

10
ground/openpilotgcs/share/openpilotgcs/pfd/default/Info.qml
View File

@ -245,8 +245,7 @@ Item {
visible: SystemAlarms.Alarm_PathPlan == 1
Text {
text: ["FLY END POINT","FLY VECTOR","FLY CIRCLE RIGHT","FLY CIRCLE LEFT","DRIVE END POINT","DRIVE VECTOR","DRIVE CIRCLE LEFT",
"DRIVE CIRCLE RIGHT","FIXED ATTITUDE","SET ACCESSORY","LAND","DISARM ALARM"][PathDesired.Mode]
text: ["GOTO ENDPOINT","FOLLOW VECTOR","CIRCLE RIGHT","CIRCLE LEFT","FIXED ATTITUDE","SET ACCESSORY","DISARM ALARM","LAND","BRAKE","VELOCITY","AUTO TAKEOFF"][PathDesired.Mode]
anchors.centerIn: parent
color: "cyan"
@ -355,6 +354,13 @@ Item {
Rectangle {
anchors.fill: parent
MouseArea {
id: reset_consumed_energy_mouseArea;
anchors.fill: parent;
cursorShape: Qt.PointingHandCursor;
onClicked: qmlWidget.resetConsumedEnergy();
}
// Alarm based on FlightBatteryState.EstimatedFlightTime < 120s orange, < 60s red
color: (FlightBatteryState.EstimatedFlightTime <= 120 && FlightBatteryState.EstimatedFlightTime > 60 ? "orange" :
(FlightBatteryState.EstimatedFlightTime <= 60 ? "red": info.batColors[SystemAlarms.Alarm_Battery]))

14
ground/openpilotgcs/share/openpilotgcs/pfd/default/Panels.qml
View File

@ -480,6 +480,13 @@ Item {
Rectangle {
anchors.fill: parent
MouseArea {
id: reset_panel_consumed_energy_mouseArea;
anchors.fill: parent;
cursorShape: Qt.PointingHandCursor;
onClicked: qmlWidget.resetConsumedEnergy();
}
// Alarm based on FlightBatteryState.EstimatedFlightTime < 120s orange, < 60s red
color: (FlightBatteryState.EstimatedFlightTime <= 120 && FlightBatteryState.EstimatedFlightTime > 60 ? "orange" :
(FlightBatteryState.EstimatedFlightTime <= 60 ? "red": panels.batColors[SystemAlarms.Alarm_Battery]))
@ -526,6 +533,13 @@ Item {
anchors.fill: parent
//color: panels.batColors[SystemAlarms.Alarm_Battery]
MouseArea {
id: reset_panel_consumed_energy_mouseArea2;
anchors.fill: parent;
cursorShape: Qt.PointingHandCursor;
onClicked: qmlWidget.resetConsumedEnergy();
}
// Alarm based on FlightBatteryState.EstimatedFlightTime < 120s orange, < 60s red
color: (FlightBatteryState.EstimatedFlightTime <= 120 && FlightBatteryState.EstimatedFlightTime > 60 ? "orange" :
(FlightBatteryState.EstimatedFlightTime <= 60 ? "red": panels.batColors[SystemAlarms.Alarm_Battery]))

344
ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_fr.ts
View File

@ -52,7 +52,7 @@
<translation></translation>
</message>
<message>
<location filename="../../../src/plugins/coreplugin/generalsettings.cpp" line="+62"/>
<location filename="../../../src/plugins/coreplugin/generalsettings.cpp" line="+64"/>
<source>General</source>
<translation>Général</translation>
</message>
@ -68,7 +68,7 @@
<translation>&lt;Langue Système&gt;</translation>
</message>
<message>
<location line="+112"/>
<location line="+120"/>
<source>Variables</source>
<translation>Variables</translation>
</message>
@ -103,22 +103,35 @@
<translation>Sélectionner automatiquement un périphérique USB OpenPilot :</translation>
</message>
<message>
<location/>
<source>Use UDP Mirror</source>
<translatorcomment>Typo need &quot;:&quot; or remove on others</translatorcomment>
<translation>Utiliser Miroir UDP</translation>
<translation type="vanished">Utiliser Miroir UDP</translation>
</message>
<message>
<location/>
<source>Expert Mode</source>
<translatorcomment>Typo need &quot;:&quot; or remove on others</translatorcomment>
<translation>Mode Expert</translation>
<translation type="vanished">Mode Expert</translation>
</message>
<message>
<location/>
<source>Language:</source>
<translation>Langue :</translation>
</message>
<message>
<location/>
<source>Expert Mode:</source>
<translation>Mode Expert :</translation>
</message>
<message>
<location/>
<source>Contribute usage statistics:</source>
<translation type="unfinished">Contribuer aux statistiques d&apos;utilisation :</translation>
</message>
<message>
<location/>
<source>Use UDP Mirror:</source>
<translation>Utiliser Miroir UDP :</translation>
</message>
</context>
<context>
<name>Core::Internal::MainWindow</name>
@ -3792,9 +3805,8 @@ uniquement lorsque les valeurs changent</translation>
<translation>Flot Données GPS</translation>
</message>
<message>
<location/>
<source>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Displays the SNR for each detected sat. Satellite number (PRN) is displayed inside the green bar (GPS) or orange bar (SBAS). Sat SNR is displayed above (in dBHz)&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</source>
<translation>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Affiche le SNR pour chaque satellite détecté. Le numéro du satellite (PRN) est affiché sur une barre verte (GPS) ou orange (SBAS). Le SNR du satellite est affiché au-dessus (en dBHz)</translation>
<translation type="vanished">&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Affiche le SNR pour chaque satellite détecté. Le numéro du satellite (PRN) est affiché sur une barre verte (GPS) ou orange (SBAS). Le SNR du satellite est affiché au-dessus (en dBHz)</translation>
</message>
<message>
<location/>
@ -3807,6 +3819,11 @@ uniquement lorsque les valeurs changent</translation>
<extracomment>Location of GCS on the Earth</extracomment>
<translation>Position sur la terre</translation>
</message>
<message>
<location/>
<source>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Displays the SNR for each detected sat. GPS satellites are shown in green, GLONASS in cyan, BeiDou in red and SBAS/QZSS in orange. Satellite number (PRN) is displayed inside the bar. Sat SNR is displayed above (in dBHz)&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</source>
<translation>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Affiche le SNR pour chaque satellite détecté. Les satellites GPS sont affichés en vert, GLONASS en bleu clair, Beidou en rouge et SBAS/QZSS en orange. Le numéro du satellite (PRN) est affiché sur la barre Le SNR du satellite est affiché au-dessus (en dBHz)</translation>
</message>
</context>
<context>
<name>IPconnectionOptionsPage</name>
@ -9611,32 +9628,32 @@ Veuillez sélectionner le type de multirotor désiré pour la configuration ci-d
</message>
<message>
<location/>
<location filename="../../../src/plugins/setupwizard/pages/outputcalibrationpage.cpp" line="+435"/>
<location line="+113"/>
<location filename="../../../src/plugins/setupwizard/pages/outputcalibrationpage.cpp" line="+561"/>
<location line="+131"/>
<source>Start</source>
<translation>Démarrer</translation>
</message>
<message>
<location filename="../../../src/plugins/setupwizard/pages/outputcalibrationpage.cpp" line="-207"/>
<location filename="../../../src/plugins/setupwizard/pages/outputcalibrationpage.cpp" line="-268"/>
<location line="+8"/>
<location line="+187"/>
<location line="+25"/>
<location line="+243"/>
<location line="+38"/>
<source>Output value : &lt;b&gt;%1&lt;/b&gt; µs</source>
<translation>Valeur de sortie : &lt;b&gt;%1&lt;/b&gt; µs</translation>
</message>
<message>
<location line="-215"/>
<location line="-284"/>
<source>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;&lt;span style=&quot; font-size:10pt;&quot;&gt;To find &lt;/span&gt;&lt;span style=&quot; font-size:10pt; font-weight:600;&quot;&gt;the neutral rate for this reversable motor&lt;/span&gt;&lt;span style=&quot; font-size:10pt;&quot;&gt;, press the Start button below and slide the slider to the right or left until you find the value where the motor doesn&apos;t start. &lt;br/&gt;&lt;br/&gt;When done press button again to stop.&lt;/span&gt;&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</source>
<translation>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;&lt;span style=&quot; font-size:10pt;&quot;&gt;Pour trouver &lt;/span&gt;&lt;span style=&quot; font-size:10pt; font-weight:600;&quot;&gt;la valeur de neutre de ce moteur inversable&lt;/span&gt;&lt;span style=&quot; font-size:10pt;&quot;&gt;, appuyez sur le bouton Démarrer et bouger le curseur à gauche ou à droite jusqu&apos;à trouver la position centrale où le moteur ne démarre pas. &lt;br/&gt;&lt;br/&gt;Lorsque c&apos;est terminé, appuyer à nouveau sur le bouton pour arrêter.&lt;/span&gt;&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</translation>
</message>
<message>
<location line="+89"/>
<location line="+113"/>
<location line="+132"/>
<location line="+131"/>
<source>Stop</source>
<translation>Arrêter</translation>
</message>
<message>
<location line="-52"/>
<location line="-59"/>
<source>The actuator module is in an error state.
Please make sure the correct firmware version is used then restart the wizard and try again. If the problem persists please consult the openpilot.org support forum.</source>
@ -9644,6 +9661,16 @@ Please make sure the correct firmware version is used then restart the wizard an
Soyez certain d&apos;utiliser la bonne version de firmware puis redémarrer l&apos;assistant et essayez à nouveau. Si le problème persiste, consultez le forum openpilot.org.</translation>
</message>
<message>
<location line="+132"/>
<source>Output value : &lt;b&gt;%1&lt;/b&gt; µs (Min)</source>
<translation>Valeur de sortie : &lt;b&gt;%1&lt;/b&gt; µs (Min)</translation>
</message>
<message>
<location line="+31"/>
<source>Output value : &lt;b&gt;%1&lt;/b&gt; µs (Max)</source>
<translation>Valeur de sortie : &lt;b&gt;%1&lt;/b&gt; µs (Max)</translation>
</message>
<message>
<location filename="../../../src/plugins/setupwizard/pages/outputcalibrationpage.ui"/>
<source>Reverse</source>
@ -9735,6 +9762,11 @@ p, li { white-space: pre-wrap; }
<source>Output value: 1000µs</source>
<translation>Valeur de sortie : &lt;b&gt;1000&lt;/b&gt; µs</translation>
</message>
<message>
<location/>
<source>Calibrate all motor outputs at the same time</source>
<translation>Calibrer toutes les sorties moteurs en même temps</translation>
</message>
</context>
<context>
<name>RebootPage</name>
@ -10680,7 +10712,7 @@ Voulez-vous toujours continuer ?</translation>
<context>
<name>ConfigInputWidget</name>
<message>
<location filename="../../../src/plugins/config/configinputwidget.cpp" line="+388"/>
<location filename="../../../src/plugins/config/configinputwidget.cpp" line="+393"/>
<source>http://wiki.openpilot.org/x/04Cf</source>
<translation></translation>
</message>
@ -10697,7 +10729,7 @@ Voulez-vous toujours continuer ?</translation>
<translation>Vous devrez reconfigurer manuellement les paramètres d&apos;armement lorsque l&apos;assistant sera terminé. Après la dernière étape de l&apos;assistant, vous serez redirigé vers l&apos;écran des Paramètres d&apos;Armement.</translation>
</message>
<message>
<location line="+212"/>
<location line="+222"/>
<source>Next</source>
<translation>Suivant</translation>
</message>
@ -10738,7 +10770,7 @@ Vous pouvez appuyer à tout moment sur &apos;Précédent&apos; pour revenir à l
<translation type="vanished">Veuillez sélectionner votre mode de pilotage :</translation>
</message>
<message>
<location line="+45"/>
<location line="+47"/>
<source>Mode 1: Fore/Aft Cyclic and Yaw on the left, Throttle/Collective and Left/Right Cyclic on the right</source>
<translatorcomment>Mode pilotage Hélicoptères: (Notice Graupner)
1: Longitudinal, Anti-couple, à gauche et Moteur/Pas, Latéral, à droite
@ -10835,7 +10867,16 @@ IMPORTANT: These new settings have not been saved to the board yet. After pressi
IMPORTANT : Ces nouveaux paramètres ne sont pas encore enregistrés sur la carte. Après avoir appuyé sur Suivant vous serez dirigé vers l&apos;onglet Paramètres d&apos;Armement où vous pourrez choisir votre séquence d&apos;armement et enregistrer la configuration.</translation>
</message>
<message>
<location line="+191"/>
<location line="+23"/>
<source>Please center all controls and trims and press Next when ready.
For a ground vehicle, this center position will be used as neutral value of each channel.</source>
<translation>Veuillez positionner tous les manches/trims en position centrale et appuyer sur Suivant lorsque vous êtes prêt.
Pour un véhicule terrestre, ces positions centrales seront utilisées comme neutre de chaque canal.</translation>
</message>
<message>
<location line="+178"/>
<source>Please enable throttle hold mode.
Move the Collective Pitch stick.</source>
@ -10883,7 +10924,7 @@ Bougez le manche %1.</translation>
<translation> Vous avez la possibilité d&apos;appuyer sur Suivant pour ignorer ce canal.</translation>
</message>
<message>
<location line="+725"/>
<location line="+748"/>
<source>Stop Manual Calibration</source>
<translation>Arrêter Calibration Manuelle</translation>
</message>
@ -10898,12 +10939,22 @@ Bougez le manche %1.</translation>
<translation>Vous devrez reconfigurer les paramètres d&apos;armement manuellement lorsque la calibration manuelle sera terminée.</translation>
</message>
<message>
<location line="+51"/>
<location line="+34"/>
<source>Ground vehicle</source>
<translation>Véhicule terrestre</translation>
</message>
<message>
<location line="+1"/>
<source>&lt;p&gt;Are you configuring a transmitter for your &lt;b&gt;ground vehicle&lt;/b&gt; with reversible motor&lt;br&gt;controlled by throttle stick?&lt;/p&gt;&lt;p&gt;If so, please make sure you&apos;ve centered throttle control and press &lt;b&gt;Yes&lt;/b&gt; button. Otherwise, press No.&lt;/p&gt;&lt;p&gt;Attention, if you press &lt;b&gt;Yes&lt;/b&gt;, then the &lt;b&gt;Flight Mode Count&lt;/b&gt; will be set to 1.&lt;/p&gt;</source>
<translation>&lt;p&gt;Confirmez-vous que vous utilisez une radio pour votre &lt;b&gt;véhicule terrestre&lt;/b&gt; avec un moteur inversable contrôlé avec le manche Throttle ?&lt;/p&gt;Si c&apos;est le cas, soyez certain d&apos;avoir centré le manche throttle et appuyez sur le bouton &lt;b&gt;Oui&lt;/b&gt;, sinon appuyez sur Non&lt;/p&gt;&lt;p&gt;Attention, si vous appuyez sur &lt;b&gt;Oui&lt;/b&gt;, le nombre de &lt;b&gt;Modes de Vol&lt;/b&gt; sera fixé à 1.&lt;/p&gt;</translation>
</message>
<message>
<location line="+39"/>
<source>Start Manual Calibration</source>
<translation>Démarrer Calibration Manuelle</translation>
</message>
<message>
<location line="+28"/>
<location line="+35"/>
<source>Warning</source>
<translation>Attention</translation>
</message>
@ -11506,14 +11557,22 @@ Double clic sur la légende ou le tracé pour afficher/cacher la légende.</tran
<context>
<name>SetupWizardPlugin</name>
<message>
<location filename="../../../src/plugins/setupwizard/setupwizardplugin.cpp" line="+62"/>
<source>Vehicle Setup Wizard</source>
<translation>Assistant Configuration Véhicule</translation>
<translation type="vanished">Assistant Configuration Véhicule</translation>
</message>
<message>
<source>Export Wizard Vehicle Template</source>
<translation type="vanished">Exporter Modèle Assistant Véhicule</translation>
</message>
<message>
<location filename="../../../src/plugins/setupwizard/setupwizardplugin.cpp" line="+61"/>
<source>Vehicle Setup Wizard...</source>
<translation>Assistant Configuration Véhicule...</translation>
</message>
<message>
<location line="+12"/>
<source>Export Wizard Vehicle Template</source>
<translation>Exporter Modèle Assistant Véhicule</translation>
<source>Export/Import Vehicle Template...</source>
<translation>Export / Import Modèle Assistant Véhicule...</translation>
</message>
</context>
<context>
@ -11584,7 +11643,7 @@ Double clic sur la légende ou le tracé pour afficher/cacher la légende.</tran
<translation>Écriture paramètres de stabilisation</translation>
</message>
<message>
<location line="+146"/>
<location line="+144"/>
<source>Writing mixer settings</source>
<translation>Écriture paramètres mixeur</translation>
</message>
@ -15102,81 +15161,67 @@ et même conduire au crash. A utiliser avec prudence.</translation>
<translation>Page d&apos;Assistant</translation>
</message>
<message>
<location/>
<source>Information about the Vehicle in short.</source>
<translation>Résumé d&apos;informations sur le Véhicule.</translation>
<translation type="vanished">Résumé d&apos;informations sur le Véhicule.</translation>
</message>
<message>
<location filename="../../../src/plugins/setupwizard/pages/airframeinitialtuningpage.cpp" line="+120"/>
<source>Name of Vehicle: </source>
<translation>Nom du Véhicule : </translation>
<translation type="vanished">Nom du Véhicule : </translation>
</message>
<message>
<location line="+1"/>
<source>Name of Owner: </source>
<translation>Nom du Propriétaire : </translation>
<translation type="vanished">Nom du Propriétaire : </translation>
</message>
<message>
<location line="+5"/>
<source>Size: </source>
<translation>Taille : </translation>
<translation type="vanished">Taille : </translation>
</message>
<message>
<location line="+1"/>
<source>Weight: </source>
<translation>Masse : </translation>
<translation type="vanished">Masse : </translation>
</message>
<message>
<location line="+1"/>
<source>Motor(s): </source>
<translation>Moteur(s) : </translation>
<translation type="vanished">Moteur(s) : </translation>
</message>
<message>
<location line="+1"/>
<source>ESC(s): </source>
<translation>Esc(s) : </translation>
<translation type="vanished">Esc(s) : </translation>
</message>
<message>
<location line="+1"/>
<source>Servo(s): </source>
<translation>Servo(s) : </translation>
<translation type="vanished">Servo(s) : </translation>
</message>
<message>
<location line="+1"/>
<source>Battery: </source>
<translation>Batterie : </translation>
<translation type="vanished">Batterie : </translation>
</message>
<message>
<location line="+1"/>
<source>Propellers(s): </source>
<translation>Hélice(s) : </translation>
<translation type="vanished">Hélice(s) : </translation>
</message>
<message>
<location line="+1"/>
<source>Controller: </source>
<translation>Carte Contrôleur : </translation>
<translation type="vanished">Carte Contrôleur : </translation>
</message>
<message>
<location line="+1"/>
<source>Comments: </source>
<translation>Commentaires : </translation>
<translation type="vanished">Commentaires : </translation>
</message>
<message>
<location line="+3"/>
<source>This option will use the current tuning settings saved on the controller, if your controller is currently unconfigured, then the OpenPilot firmware defaults will be used.
It is suggested that if this is a first time configuration of your controller, rather than use this option, instead select a tuning set that matches your own airframe as close as possible from the list above or if you are not able to fine one, then select the generic item from the list.</source>
<translation>Cette option utilise les paramètres de réglage actuels enregistrés dans la carte, si votre contrôleur n&apos;est pas actuellement configuré, alors les paramètres par défaut du firmware OpenPilot seront utilisés.
<translation type="vanished">Cette option utilise les paramètres de réglage actuels enregistrés dans la carte, si votre contrôleur n&apos;est pas actuellement configuré, alors les paramètres par défaut du firmware OpenPilot seront utilisés.
Il est suggéré que si cela est une première configuration de votre contrôleur, plutôt que d&apos;utiliser cette option, sélectionnez à la place un ensemble de réglages qui correspond le mieux à votre propre appareil dans la liste ci-dessus. Si vous n&apos;êtes pas en mesure d&apos;en choisir un, sélectionnez l&apos;élément générique de la liste.</translation>
</message>
<message>
<location line="+93"/>
<source>Current Tuning</source>
<translation>Réglages Actuels</translation>
<translation type="vanished">Réglages Actuels</translation>
</message>
<message>
<location filename="../../../src/plugins/setupwizard/pages/airframeinitialtuningpage.ui"/>
<location/>
<source>&lt;!DOCTYPE HTML PUBLIC &quot;-//W3C//DTD HTML 4.0//EN&quot; &quot;http://www.w3.org/TR/REC-html40/strict.dtd&quot;&gt;
&lt;html&gt;&lt;head&gt;&lt;meta name=&quot;qrichtext&quot; content=&quot;1&quot; /&gt;&lt;style type=&quot;text/css&quot;&gt;
p, li { white-space: pre-wrap; }
@ -15315,12 +15360,11 @@ p, li { white-space: pre-wrap; }
<context>
<name>VehicleTemplateExportDialog</name>
<message>
<location filename="../../../src/plugins/setupwizard/vehicletemplateexportdialog.ui"/>
<source>Vehicle Template Export</source>
<translation>Exportation Modèle Véhicule</translation>
<translation type="vanished">Exportation Modèle Véhicule</translation>
</message>
<message>
<location/>
<location filename="../../../src/plugins/setupwizard/vehicletemplateexportdialog.ui"/>
<source>Weight:</source>
<translation>Masse :</translation>
</message>
@ -15475,12 +15519,11 @@ p, li { white-space: pre-wrap; }
<translation>Sélectionner image...</translation>
</message>
<message>
<location/>
<source>Cancel</source>
<translation>Annuler</translation>
<translation type="vanished">Annuler</translation>
</message>
<message>
<location filename="../../../src/plugins/setupwizard/vehicletemplateexportdialog.cpp" line="+88"/>
<location filename="../../../src/plugins/setupwizard/vehicletemplateexportdialog.cpp" line="+109"/>
<source>Fixed Wing - Aileron</source>
<translation>Voilure Fixe - Aileron</translation>
</message>
@ -15569,7 +15612,7 @@ p, li { white-space: pre-wrap; }
<translation>Non Supporté</translation>
</message>
<message>
<location line="+75"/>
<location line="+56"/>
<source>Export settings</source>
<translation>Exporter réglages</translation>
</message>
@ -15579,15 +15622,16 @@ p, li { white-space: pre-wrap; }
<translation></translation>
</message>
<message>
<location line="+11"/>
<location line="+14"/>
<source>Settings could not be exported to
%1(%2).
Please try again.</source>
<translation>Les réglages ne peuvent être exportés vers
%1(%2).</translation>
%1(%2).
Veuillez essayer à nouveau.</translation>
</message>
<message>
<location line="+9"/>
<location line="+61"/>
<source>Import Image</source>
<translation>Importer Image</translation>
</message>
@ -15604,7 +15648,37 @@ Please try again.</source>
<message>
<location/>
<source>Put comments here that don&apos;t fit in the categories above</source>
<translation>Indiquez ici les commentaires ne correspondant pas aux catégories ci-dessus</translation>
<translation>Indiquez ici les commentaires ne correspondant pas aux catégories ci-dessus</translation>
</message>
<message>
<location/>
<source>Vehicle Templates</source>
<translation>Modèles Véhicules</translation>
</message>
<message>
<location/>
<source>Export template</source>
<translation>Export modèle</translation>
</message>
<message>
<location/>
<source>Close</source>
<translation>Fermer</translation>
</message>
<message>
<location/>
<source>Save as</source>
<translation>Enregistrer sous</translation>
</message>
<message>
<location/>
<source>Import template</source>
<translation>Import modèle</translation>
</message>
<message>
<location/>
<source>Import</source>
<translation>Importer</translation>
</message>
</context>
<context>
@ -16013,9 +16087,8 @@ Vous pouvez appuyer à tout moment sur &apos;Précédent&apos; pour revenir à l
<translation>Hélicoptère : avec pas collectif et commande des gaz</translation>
</message>
<message>
<location/>
<source>If selecting the Helicopter option, please engage throttle hold now.</source>
<translation>Si vous avez sélectionné l&apos;option Hélicoptère, veuillez enclencher et maintenir les gaz.</translation>
<translation type="vanished">Si vous avez sélectionné l&apos;option Hélicoptère, veuillez enclencher et maintenir les gaz.</translation>
</message>
<message>
<location/>
@ -16050,7 +16123,7 @@ Vous pouvez appuyer à tout moment sur &apos;Précédent&apos; pour revenir à l
<message>
<location/>
<source>Identify sticks instructions</source>
<translation type="unfinished">Identifiez les instructions de manches</translation>
<translation>Identifiez les instructions de manches</translation>
</message>
<message>
<location/>
@ -16099,11 +16172,21 @@ IMPORTANT : Ces nouveaux paramètres ne sont pas encore enregistrés sur la cart
<source>Cancel</source>
<translation>Annuler</translation>
</message>
<message>
<location/>
<source>Surface: has reversible motor controlled by throttle stick, plus yaw control (2 channels)</source>
<translation>Terrestre : A un moteur inversable contrôlé par le manche des gaz plus une direction Yaw (2 canaux)</translation>
</message>
<message>
<location/>
<source>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;If selecting the Helicopter option, please engage throttle hold now.&lt;/p&gt;&lt;p&gt;If selecting the Surface option, the &lt;b&gt;Flight Mode Count&lt;/b&gt; will be set to be 1.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</source>
<translation type="unfinished">&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Si l&apos;option Hélicoptère est sélectionnée, veuillez maintenir le manche des gaz maintenant.&lt;/p&gt;&lt;p&gt;Si l&apos;option Terrestre est sélectionnée, le nombre de &lt;b&gt;Modes de Vol&lt;/b&gt; sera fixé à 1.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</translation>
</message>
</context>
<context>
<name>ConfigCcpmWidget</name>
<message>
<location filename="../../../src/plugins/config/cfg_vehicletypes/configccpmwidget.cpp" line="+1081"/>
<location filename="../../../src/plugins/config/cfg_vehicletypes/configccpmwidget.cpp" line="+1080"/>
<source>&lt;h1&gt;Swashplate Leveling Routine&lt;/h1&gt;</source>
<translation type="unfinished"></translation>
</message>
@ -16168,4 +16251,119 @@ IMPORTANT : Ces nouveaux paramètres ne sont pas encore enregistrés sur la cart
<translation type="unfinished"></translation>
</message>
</context>
<context>
<name>VehicleTemplateSelectorWidget</name>
<message>
<location filename="../../../src/plugins/setupwizard/vehicletemplateselectorwidget.ui"/>
<source>Form</source>
<translation>Formulaire</translation>
</message>
<message>
<location/>
<source>Information about the Vehicle in short.</source>
<translation>Résumé d&apos;informations sur le Véhicule.</translation>
</message>
<message>
<location filename="../../../src/plugins/setupwizard/vehicletemplateselectorwidget.cpp" line="+100"/>
<source>Name of Vehicle: </source>
<translation>Nom du Véhicule : </translation>
</message>
<message>
<location line="+1"/>
<source>Name of Owner: </source>
<translation>Nom du Propriétaire : </translation>
</message>
<message>
<location line="+5"/>
<source>Size: </source>
<translation>Taille : </translation>
</message>
<message>
<location line="+1"/>
<source>Weight: </source>
<translation>Masse : </translation>
</message>
<message>
<location line="+1"/>
<source>Motor(s): </source>
<translation>Moteur(s) : </translation>
</message>
<message>
<location line="+1"/>
<source>ESC(s): </source>
<translation>Esc(s) : </translation>
</message>
<message>
<location line="+1"/>
<source>Servo(s): </source>
<translation>Servo(s) : </translation>
</message>
<message>
<location line="+1"/>
<source>Battery: </source>
<translation>Batterie : </translation>
</message>
<message>
<location line="+1"/>
<source>Propellers(s): </source>
<translation>Hélice(s) : </translation>
</message>
<message>
<location line="+1"/>
<source>Controller: </source>
<translation>Carte Contrôleur : </translation>
</message>
<message>
<location line="+1"/>
<source>Comments: </source>
<translation>Commentaires : </translation>
</message>
<message>
<location line="+3"/>
<source>This option will use the current tuning settings saved on the controller, if your controller is currently unconfigured, then the OpenPilot firmware defaults will be used.
It is suggested that if this is a first time configuration of your controller, rather than use this option, instead select a tuning set that matches your own airframe as close as possible from the list above or if you are not able to fine one, then select the generic item from the list.</source>
<translation>Cette option utilise les paramètres de réglage actuels enregistrés dans la carte, si votre contrôleur n&apos;est pas actuellement configuré, alors les paramètres par défaut du firmware OpenPilot seront utilisés.
Il est suggéré que si cela est une première configuration de votre contrôleur, plutôt que d&apos;utiliser cette option, sélectionnez à la place un ensemble de réglages qui correspond le mieux à votre propre appareil dans la liste ci-dessus. Si vous n&apos;êtes pas en mesure d&apos;en choisir un, sélectionnez l&apos;élément générique de la liste.</translation>
</message>
<message>
<location line="+105"/>
<source>Current Tuning</source>
<translation>Réglages Actuels</translation>
</message>
</context>
<context>
<name>UsageTrackerPlugin</name>
<message>
<location filename="../../../src/plugins/usagetracker/usagetrackerplugin.cpp" line="+80"/>
<source>Usage feedback</source>
<translation>Retour d&apos;utilisation</translation>
</message>
<message>
<location line="+2"/>
<source>Yes, count me in</source>
<translation>Oui, comptez sur moi</translation>
</message>
<message>
<location line="+1"/>
<source>No, I will not help</source>
<translation>Non, je ne souhaite pas aider</translation>
</message>
<message>
<location line="+1"/>
<source>Openpilot GCS has a function to collect limited anonymous information about the usage of the application itself and the OpenPilot hardware connected to it.&lt;p&gt;The intention is to not include anything that can be considered sensitive or a threat to the users integrity. The collected information will be sent using a secure protocol to an OpenPilot web service and stored in a database for later analysis and statistical purposes.&lt;br&gt;No information will be sold or given to any third party. The sole purpose is to collect statistics about the usage of our software and hardware to enable us to make things better for you.&lt;p&gt;The following things are collected:&lt;ul&gt;&lt;li&gt;Bootloader version&lt;/li&gt;&lt;li&gt;Firmware version, tag and git hash&lt;/li&gt;&lt;li&gt;OP Hardware type, revision and mcu serial number&lt;/li&gt;&lt;li&gt;Selected configuration parameters&lt;/li&gt;&lt;li&gt;GCS version&lt;/li&gt;&lt;li&gt;Operating system version and architecture&lt;/li&gt;&lt;li&gt;Current local time&lt;/li&gt;&lt;/ul&gt;The information is collected only at the time when a board is connecting to GCS.&lt;p&gt;It is possible to enable or disable this functionality in the general settings part of the options for the GCS application at any time.&lt;p&gt;We need your help, with your feedback we know where to improve things and what platforms are in use. This is a community project that depends on people being involved.&lt;br&gt;Thank You for helping us making things better and for supporting OpenPilot!</source>
<translation type="unfinished">Openpilot GCS possède une fonction qui permet de collecter les informations de manière anonyme sur l&apos;utilisation de l&apos;application en elle-même ainsi que le matériel OpenPilot connecté dessus.&lt;p&gt;Il n&apos;est pas question de collecter des informations sensibles ou pouvant représenter une menace pour l&apos;intégrité des utilisateurs. Les informations collectées seront envoyées vers un site web OpenPilot en utilisant un protocole sécurisé et stockées dans une base de données pour une analyse et des statistiques ultérieures.&lt;br&gt;Aucune information ne sera vendue ou donnée à une quelconque tierce partie. Le seul but est de collecter des informations à propos de l&apos;utilisation de notre logiciel / matériel pour nous permettre de l&apos;améliorer.&lt;p&gt;Les éléments suivants sont collectés :&lt;ul&gt;&lt;li&gt;Version bootloader&lt;/li&gt;&lt;li&gt;Version firmware, tag et git hash&lt;/li&gt;&lt;li&gt;Type de matériel OP, révision et numéro de série CPU&lt;/li&gt;&lt;li&gt;Paramètres de configuration sélectionnés&lt;/li&gt;&lt;li&gt;Version GCS&lt;/li&gt;&lt;li&gt;Système d&apos;exploitation et architecture&lt;/li&gt;&lt;li&gt;Fuseau horaire&lt;/li&gt;&lt;/ul&gt;Les informations sont collectées au moment de la connexion de la carte avec GCS.&lt;p&gt;Il est possible d&apos;activer ou de désactiver cette fonctionnalité à tout moment dans le menu Options &gt; Paramètres généraux de GCS.&lt;p&gt;Nous avons besoin de votre aide, avec votre participation nous connaîtrons où apporter des améliorations et quelle plateforme vous utilisez. C&apos;est un projet communautaire qui dépend de l&apos;implication des utilisateurs.&lt;br&gt;Merci de nous aider à rendre les choses meilleures et soutenir OpenPilot !</translation>
</message>
<message>
<location line="+23"/>
<source>&amp;Don&apos;t show this message again.</source>
<translation>&amp;Ne pas afficher ce message à nouveau.</translation>
</message>
<message>
<location line="+148"/>
<source>Unknown</source>
<translation>Inconnu</translation>
</message>
</context>
</TS>

25
ground/openpilotgcs/share/share.pro
View File

@ -1,26 +1,5 @@
include(../openpilotgcs.pri)
TEMPLATE = subdirs
SUBDIRS = openpilotgcs/translations
DATACOLLECTIONS = cloudconfig default_configurations dials models pfd sounds diagrams mapicons stylesheets
SUBDIRS = openpilotgcs/translations copydata
equals(copydata, 1) {
for(dir, DATACOLLECTIONS) {
exists($$GCS_SOURCE_TREE/share/openpilotgcs/$$dir) {
# Qt make macros (CHK_DIR_EXISTS, COPY_DIR, etc) have different syntax. They cannot be used
# reliably to copy subdirectories in two different Windows environments (bash and cmd/QtCreator).
# So undocumented QMAKE_SH variable is used to find out the real environment.
!isEmpty(QMAKE_SH) {
# sh environment (including Windows bash)
data_copy.commands += $(MKDIR) $$targetPath(\"$$GCS_DATA_PATH/$$dir\") $$addNewline()
data_copy.commands += $(COPY_DIR) $$targetPath(\"$$GCS_SOURCE_TREE/share/openpilotgcs/$$dir\") $$targetPath(\"$$GCS_DATA_PATH/\") $$addNewline()
} else {
# native Windows cmd environment
data_copy.commands += $(COPY_DIR) $$targetPath(\"$$GCS_SOURCE_TREE/share/openpilotgcs/$$dir\") $$targetPath(\"$$GCS_DATA_PATH/$$dir\") $$addNewline()
}
}
}
data_copy.target = FORCE
QMAKE_EXTRA_TARGETS += data_copy
}
copydata.file = copydata.pro

10
ground/openpilotgcs/src/libs/sdlgamepad/copydata.pro
View File

@ -1,18 +1,10 @@
equals(copydata, 1) {
win32 {
# copy SDL DLL
SDL_DLLS = \
SDL.dll
for(dll, SDL_DLLS) {
data_copy.commands += $(COPY_FILE) $$targetPath(\"$${SDL_DIR}/bin/$$dll\") $$targetPath(\"$$GCS_APP_PATH/$$dll\") $$addNewline()
addCopyFileTarget($${dll},$${SDL_DIR}/bin,$${GCS_APP_PATH})
}
# add make target
POST_TARGETDEPS += copydata
data_copy.target = copydata
QMAKE_EXTRA_TARGETS += data_copy
}
}

9
ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configccpmwidget.cpp
View File

@ -674,7 +674,6 @@ void ConfigCcpmWidget::UpdateMixer()
if (throwConfigError(QString("HeliCP"))) {
return;
}
GUIConfigDataUnion config = getConfigData();
useCCPM = !(config.heli.ccpmCollectivePassthroughState || !config.heli.ccpmLinkCyclicState);
@ -1567,28 +1566,28 @@ bool ConfigCcpmWidget::throwConfigError(QString airframeType)
bool error = false;
if ((m_aircraft->ccpmServoWChannel->currentIndex() == 0) && (m_aircraft->ccpmServoWChannel->isEnabled())) {
if ((m_aircraft->ccpmServoWChannel->currentIndex() == 0) && (m_aircraft->ccpmServoWChannel->isVisible())) {
m_aircraft->ccpmServoWLabel->setText("<font color=red>" + m_aircraft->ccpmServoWLabel->text() + "</font>");
error = true;
} else {
m_aircraft->ccpmServoWLabel->setText(QTextEdit(m_aircraft->ccpmServoWLabel->text()).toPlainText());
}
if ((m_aircraft->ccpmServoXChannel->currentIndex() == 0) && (m_aircraft->ccpmServoXChannel->isEnabled())) {
if ((m_aircraft->ccpmServoXChannel->currentIndex() == 0) && (m_aircraft->ccpmServoXChannel->isVisible())) {
m_aircraft->ccpmServoXLabel->setText("<font color=red>" + m_aircraft->ccpmServoXLabel->text() + "</font>");
error = true;
} else {
m_aircraft->ccpmServoXLabel->setText(QTextEdit(m_aircraft->ccpmServoXLabel->text()).toPlainText());
}
if ((m_aircraft->ccpmServoYChannel->currentIndex() == 0) && (m_aircraft->ccpmServoYChannel->isEnabled())) {
if ((m_aircraft->ccpmServoYChannel->currentIndex() == 0) && (m_aircraft->ccpmServoYChannel->isVisible())) {
m_aircraft->ccpmServoYLabel->setText("<font color=red>" + m_aircraft->ccpmServoYLabel->text() + "</font>");
error = true;
} else {
m_aircraft->ccpmServoYLabel->setText(QTextEdit(m_aircraft->ccpmServoYLabel->text()).toPlainText());
}
if ((m_aircraft->ccpmServoZChannel->currentIndex() == 0) && (m_aircraft->ccpmServoZChannel->isEnabled())) {
if ((m_aircraft->ccpmServoZChannel->currentIndex() == 0) && (m_aircraft->ccpmServoZChannel->isVisible())) {
m_aircraft->ccpmServoZLabel->setText("<font color=red>" + m_aircraft->ccpmServoZLabel->text() + "</font>");
error = true;
} else {

153
ground/openpilotgcs/src/plugins/config/configinputwidget.cpp
View File

@ -42,10 +42,13 @@
#include <utils/stylehelper.h>
#include <QMessageBox>
#define ACCESS_MIN_MOVE -3
#define ACCESS_MAX_MOVE 3
#define STICK_MIN_MOVE -8
#define STICK_MAX_MOVE 8
#define ACCESS_MIN_MOVE -3
#define ACCESS_MAX_MOVE 3
#define STICK_MIN_MOVE -8
#define STICK_MAX_MOVE 8
#define CHANNEL_NUMBER_NONE 0
#define DEFAULT_FLIGHT_MODE_NUMBER 3
ConfigInputWidget::ConfigInputWidget(QWidget *parent) :
ConfigTaskWidget(parent),
@ -72,6 +75,7 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) :
accessoryDesiredObj1 = AccessoryDesired::GetInstance(getObjectManager(), 1);
accessoryDesiredObj2 = AccessoryDesired::GetInstance(getObjectManager(), 2);
actuatorSettingsObj = ActuatorSettings::GetInstance(getObjectManager());
systemSettingsObj = SystemSettings::GetInstance(getObjectManager());
// Only instance 0 is present if the board is not connected.
// The other instances are populated lazily.
@ -349,6 +353,10 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) :
ManualControlSettings::CHANNELGROUPS_ACCESSORY1 <<
ManualControlSettings::CHANNELGROUPS_ACCESSORY2;
groundChannelOrder << ManualControlSettings::CHANNELGROUPS_THROTTLE <<
ManualControlSettings::CHANNELGROUPS_YAW <<
ManualControlSettings::CHANNELGROUPS_ACCESSORY0;
updateEnableControls();
}
@ -411,14 +419,18 @@ void ConfigInputWidget::goToWizard()
// chooses a different TX type (which could otherwise result in
// unexpected TX channels being enabled)
manualSettingsData = manualSettingsObj->getData();
previousManualSettingsData = manualSettingsData;
memento.manualSettingsData = manualSettingsData;
flightModeSettingsData = flightModeSettingsObj->getData();
previousFlightModeSettingsData = flightModeSettingsData;
memento.flightModeSettingsData = flightModeSettingsData;
flightModeSettingsData.Arming = FlightModeSettings::ARMING_ALWAYSDISARMED;
flightModeSettingsObj->setData(flightModeSettingsData);
// Stash actuatorSettings
actuatorSettingsData = actuatorSettingsObj->getData();
previousActuatorSettingsData = actuatorSettingsData;
memento.actuatorSettingsData = actuatorSettingsData;
// Stash systemSettings
systemSettingsData = systemSettingsObj->getData();
memento.systemSettingsData = systemSettingsData;
// Now reset channel and actuator settings (disable outputs)
resetChannelSettings();
@ -460,9 +472,10 @@ void ConfigInputWidget::wzCancel()
ui->stackedWidget->setCurrentIndex(0);
// Load settings back from beginning of wizard
manualSettingsObj->setData(previousManualSettingsData);
flightModeSettingsObj->setData(previousFlightModeSettingsData);
actuatorSettingsObj->setData(previousActuatorSettingsData);
manualSettingsObj->setData(memento.manualSettingsData);
flightModeSettingsObj->setData(memento.flightModeSettingsData);
actuatorSettingsObj->setData(memento.actuatorSettingsData);
systemSettingsObj->setData(memento.systemSettingsData);
}
void ConfigInputWidget::registerControlActivity()
@ -545,13 +558,18 @@ void ConfigInputWidget::wzNext()
restoreMdata();
// Load actuator settings back from beginning of wizard
actuatorSettingsObj->setData(previousActuatorSettingsData);
actuatorSettingsObj->setData(memento.actuatorSettingsData);
// Force flight mode neutral to middle and Throttle neutral at 4%
adjustSpecialNeutrals();
throttleError = false;
checkThrottleRange();
// Force flight mode number to be 1 if 2 CH ground vehicle was selected
if (transmitterType == ground) {
forceOneFlightMode();
}
manualSettingsObj->setData(manualSettingsData);
// move to Arming Settings tab
ui->stackedWidget->setCurrentIndex(0);
@ -629,6 +647,8 @@ void ConfigInputWidget::wizardSetUpStep(enum wizardSteps step)
wizardUi->wzBack->setEnabled(true);
if (transmitterType == heli) {
wizardUi->typeHeli->setChecked(true);
} else if (transmitterType == ground) {
wizardUi->typeGround->setChecked(true);
} else {
wizardUi->typeAcro->setChecked(true);
}
@ -687,6 +707,10 @@ void ConfigInputWidget::wizardSetUpStep(enum wizardSteps step)
case wizardIdentifyCenter:
setTxMovement(centerAll);
wizardUi->pagesStack->setCurrentWidget(wizardUi->identifyCenterPage);
if (transmitterType == ground) {
wizardUi->identifyCenterInstructions->setText(QString(tr("Please center all controls and trims and press Next when ready.\n\n"
"For a ground vehicle, this center position will be used as neutral value of each channel.")));
}
break;
case wizardIdentifyLimits:
{
@ -752,6 +776,12 @@ void ConfigInputWidget::wizardTearDownStep(enum wizardSteps step)
case wizardChooseType:
if (wizardUi->typeAcro->isChecked()) {
transmitterType = acro;
} else if (wizardUi->typeGround->isChecked()) {
transmitterType = ground;
/* Make sure to tell controller, this is really a ground vehicle. */
systemSettingsData = systemSettingsObj->getData();
systemSettingsData.AirframeType = SystemSettings::AIRFRAMETYPE_GROUNDVEHICLECAR;
systemSettingsObj->setData(systemSettingsData);
} else {
transmitterType = heli;
}
@ -775,6 +805,12 @@ void ConfigInputWidget::wizardTearDownStep(enum wizardSteps step)
disconnect(receiverActivityObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(identifyControls()));
wizardUi->wzNext->setEnabled(true);
setTxMovement(nothing);
/* If flight mode stick isn't identified, force flight mode number to be 1 */
manualSettingsData = manualSettingsObj->getData();
if (manualSettingsData.ChannelGroups[ManualControlSettings::CHANNELNUMBER_FLIGHTMODE] ==
ManualControlSettings::CHANNELGROUPS_NONE) {
forceOneFlightMode();
}
break;
case wizardIdentifyCenter:
manualCommandData = manualCommandObj->getData();
@ -855,21 +891,32 @@ void ConfigInputWidget::restoreMdata()
*/
void ConfigInputWidget::setChannel(int newChan)
{
bool canBeSkipped = false;
if (newChan == ManualControlSettings::CHANNELGROUPS_COLLECTIVE) {
wizardUi->identifyStickInstructions->setText(QString(tr("Please enable throttle hold mode.\n\nMove the Collective Pitch stick.")));
wizardUi->identifyStickInstructions->setText(QString(tr("<p>Please enable throttle hold mode.</p>"
"<p>Move the Collective Pitch stick.</p>")));
} else if (newChan == ManualControlSettings::CHANNELGROUPS_FLIGHTMODE) {
wizardUi->identifyStickInstructions->setText(QString(tr("Please toggle the Flight Mode switch.\n\nFor switches you may have to repeat this rapidly.")));
wizardUi->identifyStickInstructions->setText(QString(tr("<p>Please toggle the Flight Mode switch.</p>"
"<p>For switches you may have to repeat this rapidly.</p>"
"<p>Alternatively, you can click Next to skip this channel, but you will get only <b>ONE</b> Flight Mode.</p>")));
canBeSkipped = true;
} else if ((transmitterType == heli) && (newChan == ManualControlSettings::CHANNELGROUPS_THROTTLE)) {
wizardUi->identifyStickInstructions->setText(QString(tr("Please disable throttle hold mode.\n\nMove the Throttle stick.")));
wizardUi->identifyStickInstructions->setText(QString(tr("<p>Please disable throttle hold mode.</p>"
"<p>Move the Throttle stick.</p>")));
} else {
wizardUi->identifyStickInstructions->setText(QString(tr("Please move each control one at a time according to the instructions and picture below.\n\n"
"Move the %1 stick.")).arg(manualSettingsObj->getField("ChannelGroups")->getElementNames().at(newChan)));
wizardUi->identifyStickInstructions->setText(QString(tr("<p>Please move each control one at a time according to the instructions and picture below.</p>"
"<p>Move the %1 stick.</p>")).arg(manualSettingsObj->getField("ChannelGroups")->getElementNames().at(newChan)));
}
if (manualSettingsObj->getField("ChannelGroups")->getElementNames().at(newChan).contains("Accessory")) {
wizardUi->identifyStickInstructions->setText(wizardUi->identifyStickInstructions->text() + tr("<p>Alternatively, click Next to skip this channel.</p>"));
canBeSkipped = true;
}
if (canBeSkipped) {
wizardUi->wzNext->setEnabled(true);
wizardUi->wzNext->setText(tr("Next / Skip"));
wizardUi->identifyStickInstructions->setText(wizardUi->identifyStickInstructions->text() + tr(" Alternatively, click Next to skip this channel."));
} else {
wizardUi->wzNext->setEnabled(false);
}
@ -885,7 +932,18 @@ void ConfigInputWidget::setChannel(int newChan)
*/
void ConfigInputWidget::nextChannel()
{
QList <int> order = (transmitterType == heli) ? heliChannelOrder : acroChannelOrder;
QList <int> order;
switch (transmitterType) {
case heli:
order = heliChannelOrder;
break;
case ground:
order = groundChannelOrder;
break;
default:
order = acroChannelOrder;
break;
}
if (currentChannelNum == -1) {
setChannel(order[0]);
@ -906,7 +964,18 @@ void ConfigInputWidget::nextChannel()
*/
void ConfigInputWidget::prevChannel()
{
QList <int> order = transmitterType == heli ? heliChannelOrder : acroChannelOrder;
QList <int> order;
switch (transmitterType) {
case heli:
order = heliChannelOrder;
break;
case ground:
order = groundChannelOrder;
break;
default:
order = acroChannelOrder;
break;
}
// No previous from unset channel or next state
if (currentChannelNum == -1) {
@ -1014,6 +1083,7 @@ void ConfigInputWidget::identifyLimits()
}
manualSettingsObj->setData(manualSettingsData);
}
void ConfigInputWidget::setMoveFromCommand(int command)
{
// ManualControlSettings::ChannelNumberElem:
@ -1624,7 +1694,7 @@ void ConfigInputWidget::simpleCalibration(bool enable)
// Stash actuatorSettings
actuatorSettingsData = actuatorSettingsObj->getData();
previousActuatorSettingsData = actuatorSettingsData;
memento.actuatorSettingsData = actuatorSettingsData;
// Disable all actuators
resetActuatorSettings();
@ -1634,8 +1704,31 @@ void ConfigInputWidget::simpleCalibration(bool enable)
manualCommandData = manualCommandObj->getData();
manualSettingsData = manualSettingsObj->getData();
QMessageBox::StandardButton reply;
reply = QMessageBox::question(this, tr("Ground vehicle"),
tr("<p>Are you configuring a transmitter for your <b>ground vehicle</b> with reversible motor<br>"
"controlled by throttle stick?</p>"
"<p>If so, please make sure you've centered throttle control and press <b>Yes</b> button. Otherwise, press No.</p>"
"<p>Attention, if you press <b>Yes</b>, then the <b>Flight Mode Count</b> will be set to 1.</p>"),
QMessageBox::Yes | QMessageBox::No);
if (reply == QMessageBox::Yes) {
transmitterType = ground;
manualSettingsData.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_THROTTLE] =
manualCommandData.Channel[ManualControlSettings::CHANNELNUMBER_THROTTLE];
/* Make sure to tell controller, this is really a ground vehicle. */
systemSettingsData = systemSettingsObj->getData();
systemSettingsData.AirframeType = SystemSettings::AIRFRAMETYPE_GROUNDVEHICLECAR;
systemSettingsObj->setData(systemSettingsData);
}
restoreMdataSingle(manualCommandObj, &manualControlMdata);
// Force flight mode number to be 1 if 2 channel ground vehicle was confirmed
if (transmitterType == ground) {
forceOneFlightMode();
}
for (unsigned int i = 0; i < ManualControlCommand::CHANNEL_NUMELEM; i++) {
if ((i == ManualControlSettings::CHANNELNUMBER_FLIGHTMODE) || (i == ManualControlSettings::CHANNELNUMBER_THROTTLE)) {
adjustSpecialNeutrals();
@ -1647,7 +1740,7 @@ void ConfigInputWidget::simpleCalibration(bool enable)
manualSettingsObj->setData(manualSettingsData);
// Load actuator settings back from beginning of manual calibration
actuatorSettingsObj->setData(previousActuatorSettingsData);
actuatorSettingsObj->setData(memento.actuatorSettingsData);
ui->configurationWizard->setEnabled(true);
ui->saveRCInputToRAM->setEnabled(true);
@ -1667,6 +1760,12 @@ void ConfigInputWidget::adjustSpecialNeutrals()
(manualSettingsData.ChannelMax[ManualControlSettings::CHANNELMAX_FLIGHTMODE] +
manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_FLIGHTMODE]) / 2;
// A ground vehicle has a reversible motor, the center position of throttle is the neutral setting.
// So do not have to set a special neutral value for it.
if (transmitterType == ground) {
return;
}
// Force throttle to be near min, add 4% from total range to avoid arming issues
manualSettingsData.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_THROTTLE] =
manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_THROTTLE] +
@ -1699,9 +1798,10 @@ void ConfigInputWidget::resetChannelSettings()
{
manualSettingsData = manualSettingsObj->getData();
// Clear all channel data : Channel Type (PPM,PWM..) and Number
for (unsigned int channel = 0; channel < 9; channel++) {
for (unsigned int channel = 0; channel < ManualControlSettings::CHANNELNUMBER_NUMELEM; channel++) {
manualSettingsData.ChannelGroups[channel] = ManualControlSettings::CHANNELGROUPS_NONE;
manualSettingsData.ChannelNumber[channel] = 0;
manualSettingsData.ChannelNumber[channel] = CHANNEL_NUMBER_NONE;
manualSettingsData.FlightModeNumber = DEFAULT_FLIGHT_MODE_NUMBER;
manualSettingsObj->setData(manualSettingsData);
}
}
@ -1737,3 +1837,10 @@ void ConfigInputWidget::resetActuatorSettings()
actuatorSettingsObj->setData(actuatorSettingsData);
}
}
void ConfigInputWidget::forceOneFlightMode()
{
manualSettingsData = manualSettingsObj->getData();
manualSettingsData.FlightModeNumber = 1;
manualSettingsObj->setData(manualSettingsData);
}

19
ground/openpilotgcs/src/plugins/config/configinputwidget.h
View File

@ -50,6 +50,7 @@
#include "flightstatus.h"
#include "accessorydesired.h"
#include <QPointer>
#include "systemsettings.h"
class Ui_InputWidget;
@ -62,7 +63,7 @@ public:
enum txMode { mode1, mode2, mode3, mode4 };
enum txMovements { moveLeftVerticalStick, moveRightVerticalStick, moveLeftHorizontalStick, moveRightHorizontalStick, moveAccess0, moveAccess1, moveAccess2, moveFlightMode, centerAll, moveAll, nothing };
enum txMovementType { vertical, horizontal, jump, mix };
enum txType { acro, heli };
enum txType { acro, heli, ground };
void startInputWizard()
{
goToWizard();
@ -111,6 +112,7 @@ private:
int currentChannelNum;
QList<int> heliChannelOrder;
QList<int> acroChannelOrder;
QList<int> groundChannelOrder;
UAVObject::Metadata manualControlMdata;
ManualControlCommand *manualCommandObj;
@ -126,18 +128,26 @@ private:
ManualControlSettings *manualSettingsObj;
ManualControlSettings::DataFields manualSettingsData;
ManualControlSettings::DataFields previousManualSettingsData;
ActuatorSettings *actuatorSettingsObj;
ActuatorSettings::DataFields actuatorSettingsData;
ActuatorSettings::DataFields previousActuatorSettingsData;
FlightModeSettings *flightModeSettingsObj;
FlightModeSettings::DataFields flightModeSettingsData;
FlightModeSettings::DataFields previousFlightModeSettingsData;
ReceiverActivity *receiverActivityObj;
ReceiverActivity::DataFields receiverActivityData;
SystemSettings *systemSettingsObj;
SystemSettings::DataFields systemSettingsData;
typedef struct {
ManualControlSettings::DataFields manualSettingsData;
ActuatorSettings::DataFields actuatorSettingsData;
FlightModeSettings::DataFields flightModeSettingsData;
SystemSettings::DataFields systemSettingsData;
} Memento;
Memento memento;
QSvgRenderer *m_renderer;
// Background: background
@ -204,6 +214,7 @@ private slots:
void updateCalibration();
void resetChannelSettings();
void resetActuatorSettings();
void forceOneFlightMode();
protected:
void resizeEvent(QResizeEvent *event);

11
ground/openpilotgcs/src/plugins/config/input_wizard.ui
View File

@ -64,6 +64,13 @@ You can press 'back' at any time to return to the previous screen or press 'Canc
</property>
</widget>
</item>
<item>
<widget class="QRadioButton" name="typeGround">
<property name="text">
<string>Surface: has reversible motor controlled by throttle stick, plus yaw control (2 channels)</string>
</property>
</widget>
</item>
<item>
<widget class="QRadioButton" name="typeHeli">
<property name="text">
@ -74,7 +81,7 @@ You can press 'back' at any time to return to the previous screen or press 'Canc
<item>
<widget class="QLabel" name="typePageFooter">
<property name="text">
<string>If selecting the Helicopter option, please engage throttle hold now.</string>
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;If selecting the Helicopter option, please engage throttle hold now.&lt;/p&gt;&lt;p&gt;If selecting the Surface option, the &lt;b&gt;Flight Mode Count&lt;/b&gt; will be set to be 1.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@ -89,7 +96,7 @@ You can press 'back' at any time to return to the previous screen or press 'Canc
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>40</height>
<height>23</height>
</size>
</property>
</spacer>

53
ground/openpilotgcs/src/plugins/coreplugin/generalsettings.cpp
View File

@ -49,6 +49,8 @@ GeneralSettings::GeneralSettings() :
m_autoSelect(true),
m_useUDPMirror(false),
m_useExpertMode(false),
m_collectUsageData(true),
m_showUsageDataDisclaimer(true),
m_dialog(0)
{}
@ -125,6 +127,7 @@ QWidget *GeneralSettings::createPage(QWidget *parent)
m_page->checkAutoSelect->setChecked(m_autoSelect);
m_page->cbUseUDPMirror->setChecked(m_useUDPMirror);
m_page->cbExpertMode->setChecked(m_useExpertMode);
m_page->cbUsageData->setChecked(m_collectUsageData);
m_page->colorButton->setColor(StyleHelper::baseColor());
connect(m_page->resetButton, SIGNAL(clicked()), this, SLOT(resetInterfaceColor()));
@ -145,6 +148,7 @@ void GeneralSettings::apply()
m_useExpertMode = m_page->cbExpertMode->isChecked();
m_autoConnect = m_page->checkAutoConnect->isChecked();
m_autoSelect = m_page->checkAutoSelect->isChecked();
setCollectUsageData(m_page->cbUsageData->isChecked());
}
void GeneralSettings::finish()
@ -155,12 +159,15 @@ void GeneralSettings::finish()
void GeneralSettings::readSettings(QSettings *qs)
{
qs->beginGroup(QLatin1String("General"));
m_language = qs->value(QLatin1String("OverrideLanguage"), QLocale::system().name()).toString();
m_language = qs->value(QLatin1String("OverrideLanguage"), QLocale::system().name()).toString();
m_saveSettingsOnExit = qs->value(QLatin1String("SaveSettingsOnExit"), m_saveSettingsOnExit).toBool();
m_autoConnect = qs->value(QLatin1String("AutoConnect"), m_autoConnect).toBool();
m_autoSelect = qs->value(QLatin1String("AutoSelect"), m_autoSelect).toBool();
m_useUDPMirror = qs->value(QLatin1String("UDPMirror"), m_useUDPMirror).toBool();
m_useExpertMode = qs->value(QLatin1String("ExpertMode"), m_useExpertMode).toBool();
m_autoConnect = qs->value(QLatin1String("AutoConnect"), m_autoConnect).toBool();
m_autoSelect = qs->value(QLatin1String("AutoSelect"), m_autoSelect).toBool();
m_useUDPMirror = qs->value(QLatin1String("UDPMirror"), m_useUDPMirror).toBool();
m_useExpertMode = qs->value(QLatin1String("ExpertMode"), m_useExpertMode).toBool();
m_collectUsageData = qs->value(QLatin1String("CollectUsageData"), m_collectUsageData).toBool();
m_showUsageDataDisclaimer = qs->value(QLatin1String("ShowUsageDataDisclaimer"), m_showUsageDataDisclaimer).toBool();
m_lastUsageHash = qs->value(QLatin1String("LastUsageHash"), m_lastUsageHash).toString();
qs->endGroup();
}
@ -179,6 +186,9 @@ void GeneralSettings::saveSettings(QSettings *qs)
qs->setValue(QLatin1String("AutoSelect"), m_autoSelect);
qs->setValue(QLatin1String("UDPMirror"), m_useUDPMirror);
qs->setValue(QLatin1String("ExpertMode"), m_useExpertMode);
qs->setValue(QLatin1String("CollectUsageData"), m_collectUsageData);
qs->setValue(QLatin1String("ShowUsageDataDisclaimer"), m_showUsageDataDisclaimer);
qs->setValue(QLatin1String("LastUsageHash"), m_lastUsageHash);
qs->endGroup();
}
@ -249,11 +259,44 @@ bool GeneralSettings::useUDPMirror() const
return m_useUDPMirror;
}
bool GeneralSettings::collectUsageData() const
{
return m_collectUsageData;
}
bool GeneralSettings::showUsageDataDisclaimer() const
{
return m_showUsageDataDisclaimer;
}
QString GeneralSettings::lastUsageHash() const
{
return m_lastUsageHash;
}
bool GeneralSettings::useExpertMode() const
{
return m_useExpertMode;
}
bool GeneralSettings::setCollectUsageData(bool collect)
{
if (collect && collect != m_collectUsageData) {
setShowUsageDataDisclaimer(true);
}
m_collectUsageData = collect;
}
bool GeneralSettings::setShowUsageDataDisclaimer(bool show)
{
m_showUsageDataDisclaimer = show;
}
void GeneralSettings::setLastUsageHash(QString hash)
{
m_lastUsageHash = hash;
}
void GeneralSettings::slotAutoConnect(int value)
{
if (value == Qt::Checked) {

10
ground/openpilotgcs/src/plugins/coreplugin/generalsettings.h
View File

@ -57,10 +57,15 @@ public:
bool autoConnect() const;
bool autoSelect() const;
bool useUDPMirror() const;
bool collectUsageData() const;
bool showUsageDataDisclaimer() const;
QString lastUsageHash() const;
void readSettings(QSettings *qs);
void saveSettings(QSettings *qs);
bool useExpertMode() const;
signals:
bool setCollectUsageData(bool collect);
bool setShowUsageDataDisclaimer(bool show);
void setLastUsageHash(QString hash);
private slots:
void resetInterfaceColor();
@ -79,6 +84,9 @@ private:
bool m_autoSelect;
bool m_useUDPMirror;
bool m_useExpertMode;
bool m_collectUsageData;
bool m_showUsageDataDisclaimer;
QString m_lastUsageHash;
QPointer<QWidget> m_dialog;
QList<QTextCodec *> m_codecs;
};

210
ground/openpilotgcs/src/plugins/coreplugin/generalsettings.ui
View File

@ -11,7 +11,16 @@
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_2">
<property name="margin">
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number>
</property>
<item>
@ -20,17 +29,73 @@
<string>General settings</string>
</property>
<layout class="QGridLayout" name="gridLayout">
<item row="1" column="0">
<widget class="QLabel" name="colorLabel">
<item row="0" column="0">
<widget class="QLabel" name="label_3">
<property name="text">
<string>User interface color:</string>
<string>Language:</string>
</property>
</widget>
</item>
<item row="1" column="1">
<item row="14" column="0">
<widget class="QLabel" name="labelExpert">
<property name="text">
<string>Expert Mode:</string>
</property>
</widget>
</item>
<item row="13" column="2">
<widget class="QCheckBox" name="cbUseUDPMirror">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="12" column="2">
<widget class="QCheckBox" name="checkAutoSelect">
<property name="enabled">
<bool>false</bool>
</property>
<property name="text">
<string/>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
<item row="11" column="0">
<widget class="QLabel" name="label_5">
<property name="text">
<string>Automatically connect an OpenPilot USB device:</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="10" column="2">
<widget class="QCheckBox" name="checkBoxSaveOnExit">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
<item row="1" column="2">
<widget class="QWidget" name="widget" native="true">
<layout class="QGridLayout" name="gridLayout_2">
<property name="margin">
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number>
</property>
<item row="0" column="1">
@ -82,46 +147,46 @@
</layout>
</widget>
</item>
<item row="9" column="1">
<item row="1" column="0">
<widget class="QLabel" name="colorLabel">
<property name="text">
<string>User interface color:</string>
</property>
</widget>
</item>
<item row="15" column="0">
<widget class="QLabel" name="labelExpert_2">
<property name="text">
<string>Contribute usage statistics:</string>
</property>
</widget>
</item>
<item row="9" column="2">
<widget class="QWidget" name="widget_2" native="true">
<layout class="QGridLayout" name="gridLayout_3">
<property name="margin">
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number>
</property>
</layout>
</widget>
</item>
<item row="10" column="1">
<widget class="QCheckBox" name="checkBoxSaveOnExit">
<item row="14" column="2">
<widget class="QCheckBox" name="cbExpertMode">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
<item row="10" column="0">
<widget class="QLabel" name="label_4">
<property name="text">
<string>Save configuration settings on exit:</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="11" column="0">
<widget class="QLabel" name="label_5">
<property name="text">
<string>Automatically connect an OpenPilot USB device:</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="11" column="1">
<item row="11" column="2">
<widget class="QCheckBox" name="checkAutoConnect">
<property name="text">
<string/>
@ -131,58 +196,14 @@
</property>
</widget>
</item>
<item row="12" column="0">
<widget class="QLabel" name="label_6">
<property name="text">
<string>Automatically select an OpenPilot USB device:</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="12" column="1">
<widget class="QCheckBox" name="checkAutoSelect">
<property name="enabled">
<bool>false</bool>
</property>
<property name="text">
<string/>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
<item row="13" column="1">
<widget class="QCheckBox" name="cbUseUDPMirror">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="13" column="0">
<widget class="QLabel" name="labelUDP">
<property name="text">
<string>Use UDP Mirror</string>
<string>Use UDP Mirror:</string>
</property>
</widget>
</item>
<item row="14" column="0">
<widget class="QLabel" name="labelExpert">
<property name="text">
<string>Expert Mode</string>
</property>
</widget>
</item>
<item row="14" column="1">
<widget class="QCheckBox" name="cbExpertMode">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="0" column="1">
<item row="0" column="2">
<layout class="QHBoxLayout" name="horizontalLayout">
<item>
<widget class="QComboBox" name="languageBox"/>
@ -202,10 +223,33 @@
</item>
</layout>
</item>
<item row="0" column="0">
<widget class="QLabel" name="label_3">
<item row="10" column="0">
<widget class="QLabel" name="label_4">
<property name="text">
<string>Language:</string>
<string>Save configuration settings on exit:</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="12" column="0">
<widget class="QLabel" name="label_6">
<property name="text">
<string>Automatically select an OpenPilot USB device:</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="15" column="2">
<widget class="QCheckBox" name="cbUsageData">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>

12
ground/openpilotgcs/src/plugins/coreplugin/uavgadgetdecorator.cpp
View File

@ -44,7 +44,7 @@ UAVGadgetDecorator::UAVGadgetDecorator(IUAVGadget *gadget, QList<IUAVGadgetConfi
m_toolbar->setMinimumContentsLength(15);
foreach(IUAVGadgetConfiguration * config, *m_configurations)
m_toolbar->addItem(config->name());
connect(m_toolbar, SIGNAL(activated(int)), this, SLOT(loadConfiguration(int)));
connect(m_toolbar, SIGNAL(currentIndexChanged(int)), this, SLOT(loadConfiguration(int)));
updateToolbar();
}
@ -63,15 +63,22 @@ void UAVGadgetDecorator::loadConfiguration(int index)
void UAVGadgetDecorator::loadConfiguration(IUAVGadgetConfiguration *config)
{
if (m_activeConfiguration == config) {
return;
}
m_activeConfiguration = config;
int index = m_toolbar->findText(config->name());
m_toolbar->setCurrentIndex(index);
if (m_toolbar->currentIndex() != index) {
m_toolbar->setCurrentIndex(index);
}
m_gadget->loadConfiguration(config);
}
void UAVGadgetDecorator::configurationChanged(IUAVGadgetConfiguration *config)
{
if (config == m_activeConfiguration) {
// force a configuration reload
m_activeConfiguration = NULL;
loadConfiguration(config);
}
}
@ -133,7 +140,6 @@ void UAVGadgetDecorator::restoreState(QSettings *qSettings)
foreach(IUAVGadgetConfiguration * config, *m_configurations) {
if (config->name() == configName) {
m_activeConfiguration = config;
loadConfiguration(config);
break;
}

2
ground/openpilotgcs/src/plugins/coreplugin/uavgadgetinstancemanager.cpp
View File

@ -166,7 +166,7 @@ void UAVGadgetInstanceManager::readConfigs_1_1_0(QSettings *qs)
configs = qs->childGroups();
foreach(QString configName, configs) {
qDebug() << "Loading config: " << classId << "," << configName;
qDebug().nospace() << "Loading config: " << classId << ", " << configName;
qs->beginGroup(configName);
bool locked = qs->value("config.locked").toBool();
configInfo.setNameOfConfigurable(classId + "-" + configName);

209
ground/openpilotgcs/src/plugins/coreplugin/uavgadgetmanager/splitterorview.cpp
View File

@ -34,7 +34,6 @@
#include <QtCore/QDebug>
#ifdef Q_WS_MAC
#include <qmacstyle_mac.h>
#endif
@ -46,22 +45,28 @@ SplitterOrView::SplitterOrView(Core::UAVGadgetManager *uavGadgetManager, Core::I
m_uavGadgetManager(uavGadgetManager),
m_splitter(0)
{
m_view = new UAVGadgetView(m_uavGadgetManager, uavGadget, this);
m_layout = new QStackedLayout(this);
m_layout->addWidget(m_view);
m_view = new UAVGadgetView(m_uavGadgetManager, uavGadget, this);
setLayout(new QStackedLayout());
layout()->addWidget(m_view);
}
SplitterOrView::SplitterOrView(SplitterOrView &splitterOrView, QWidget *parent) :
QWidget(parent),
m_uavGadgetManager(splitterOrView.m_uavGadgetManager),
m_view(splitterOrView.m_view),
m_splitter(splitterOrView.m_splitter)
{
Q_ASSERT((m_view || m_splitter) && !(m_view && m_splitter));
setLayout(new QStackedLayout());
if (m_view) {
layout()->addWidget(m_view);
} else if (m_splitter) {
layout()->addWidget(m_splitter);
}
}
SplitterOrView::~SplitterOrView()
{
if (m_view) {
delete m_view;
m_view = 0;
}
if (m_splitter) {
delete m_splitter;
m_splitter = 0;
}
}
{}
void SplitterOrView::mousePressEvent(QMouseEvent *e)
{
@ -221,7 +226,7 @@ QSplitter *SplitterOrView::takeSplitter()
QSplitter *oldSplitter = m_splitter;
if (m_splitter) {
m_layout->removeWidget(m_splitter);
layout()->removeWidget(m_splitter);
}
m_splitter = 0;
return oldSplitter;
@ -232,7 +237,7 @@ UAVGadgetView *SplitterOrView::takeView()
UAVGadgetView *oldView = m_view;
if (m_view) {
m_layout->removeWidget(m_view);
layout()->removeWidget(m_splitter);
}
m_view = 0;
return oldView;
@ -255,35 +260,6 @@ QList<IUAVGadget *> SplitterOrView::gadgets()
return g;
}
void SplitterOrView::split(Qt::Orientation orientation)
{
Q_ASSERT(m_view);
Q_ASSERT(!m_splitter);
m_splitter = new MiniSplitter(this);
m_splitter->setOrientation(orientation);
connect(m_splitter, SIGNAL(splitterMoved(int, int)), this, SLOT(onSplitterMoved(int, int)));
m_layout->addWidget(m_splitter);
Core::IUAVGadget *ourGadget = m_view->gadget();
if (ourGadget) {
// Give our gadget to the new left or top SplitterOrView.
m_view->removeGadget();
m_splitter->addWidget(new SplitterOrView(m_uavGadgetManager, ourGadget));
m_splitter->addWidget(new SplitterOrView(m_uavGadgetManager));
} else {
m_splitter->addWidget(new SplitterOrView(m_uavGadgetManager));
m_splitter->addWidget(new SplitterOrView(m_uavGadgetManager));
}
m_layout->setCurrentWidget(m_splitter);
if (m_view) {
m_uavGadgetManager->emptyView(m_view);
delete m_view;
m_view = 0;
}
}
void SplitterOrView::onSplitterMoved(int pos, int index)
{
Q_UNUSED(pos);
@ -292,66 +268,107 @@ void SplitterOrView::onSplitterMoved(int pos, int index)
m_sizes = m_splitter->sizes();
}
void SplitterOrView::unsplitAll(IUAVGadget *currentGadget)
void SplitterOrView::split(Qt::Orientation orientation)
{
Q_ASSERT(m_splitter);
Q_ASSERT(!m_view);
m_splitter->hide();
m_layout->removeWidget(m_splitter); // workaround Qt bug
unsplitAll_helper();
delete m_splitter;
m_splitter = 0;
Q_ASSERT(m_view);
Q_ASSERT(!m_splitter);
m_view = new UAVGadgetView(m_uavGadgetManager, currentGadget, this);
m_layout->addWidget(m_view);
MiniSplitter *splitter = new MiniSplitter(this);
splitter->setOrientation(orientation);
layout()->addWidget(splitter);
// [OP-1586] make sure that the view never becomes parent less otherwise a rendering bug happens
// in osgearth QML views (not all kind of scenes are affected but those containing terrain are)
// Making the view parent less will destroy the OpenGL context used by the QQuickFramebufferObject used OSGViewport
// A new OpenGL context will be created but for some reason, osgearth does not switch to it gracefully.
// Enabling the stats overlay (by pressing the 's' key in the view) will restore proper rendering (?).
// Note : avoiding to make the view parent less is a workaround... the real cause of the rendering bug needs to be
// understood and fixed (the same workaround is also need in unsplit and unsplitAll)
// Important : the changes also apparently make splitting and un-splitting more reactive and less jumpy!
// Give our view to the new left or top SplitterOrView.
splitter->addWidget(new SplitterOrView(*this, splitter));
splitter->addWidget(new SplitterOrView(m_uavGadgetManager));
m_view = 0;
m_splitter = splitter;
connect(m_splitter, SIGNAL(splitterMoved(int, int)), this, SLOT(onSplitterMoved(int, int)));
}
void SplitterOrView::unsplitAll_helper()
{
if (m_view) {
m_uavGadgetManager->emptyView(m_view);
}
if (m_splitter) {
for (int i = 0; i < m_splitter->count(); ++i) {
if (SplitterOrView * splitterOrView = qobject_cast<SplitterOrView *>(m_splitter->widget(i))) {
splitterOrView->unsplitAll_helper();
}
}
}
}
void SplitterOrView::unsplit()
void SplitterOrView::unsplit(IUAVGadget *gadget)
{
if (!m_splitter) {
return;
}
Q_ASSERT(m_splitter->count() == 1);
SplitterOrView *childSplitterOrView = qobject_cast<SplitterOrView *>(m_splitter->widget(0));
QSplitter *oldSplitter = m_splitter;
m_splitter = 0;
if (childSplitterOrView->isSplitter()) {
Q_ASSERT(childSplitterOrView->view() == 0);
m_splitter = childSplitterOrView->takeSplitter();
m_layout->addWidget(m_splitter);
m_layout->setCurrentWidget(m_splitter);
} else {
UAVGadgetView *childView = childSplitterOrView->view();
Q_ASSERT(childView);
if (m_view) {
if (IUAVGadget * e = childView->gadget()) {
childView->removeGadget();
m_view->setGadget(e);
}
m_uavGadgetManager->emptyView(childView);
} else {
m_view = childSplitterOrView->takeView();
m_layout->addWidget(m_view);
}
m_layout->setCurrentWidget(m_view);
SplitterOrView *view = findView(gadget);
if (!view || view == this) {
return;
}
// find the other gadgets
// TODO handle case where m_splitter->count() > 2
SplitterOrView *splitterOrView = NULL;
for (int i = 0; i < m_splitter->count(); ++i) {
splitterOrView = qobject_cast<SplitterOrView *>(m_splitter->widget(i));
if (splitterOrView && (splitterOrView != view)) {
break;
}
}
if (splitterOrView) {
if (splitterOrView->isView()) {
layout()->addWidget(splitterOrView->m_view);
} else {
layout()->addWidget(splitterOrView->m_splitter);
}
layout()->removeWidget(m_splitter);
m_uavGadgetManager->emptyView(view->m_view);
delete view;
delete m_splitter;
m_view = splitterOrView->m_view;
m_splitter = splitterOrView->m_splitter;
}
}
void SplitterOrView::unsplitAll(Core::IUAVGadget *gadget)
{
Q_ASSERT(m_splitter);
Q_ASSERT(!m_view);
SplitterOrView *splitterOrView = findView(gadget);
if (!splitterOrView || splitterOrView == this) {
return;
}
// first re-parent the gadget (see split for an explanation)
m_view = splitterOrView->m_view;
layout()->addWidget(m_view);
layout()->removeWidget(m_splitter);
// make sure the old m_view is not emptied...
splitterOrView->m_view = NULL;
// cleanup
unsplitAll_helper(m_uavGadgetManager, m_splitter);
delete m_splitter;
m_splitter = 0;
}
void SplitterOrView::unsplitAll_helper(UAVGadgetManager *uavGadgetManager, QSplitter *splitter)
{
for (int i = 0; i < splitter->count(); ++i) {
if (SplitterOrView * splitterOrView = qobject_cast<SplitterOrView *>(splitter->widget(i))) {
if (splitterOrView->m_view) {
uavGadgetManager->emptyView(splitterOrView->m_view);
}
if (splitterOrView->m_splitter) {
unsplitAll_helper(uavGadgetManager, splitterOrView->m_splitter);
}
delete splitterOrView;
}
}
delete oldSplitter;
m_uavGadgetManager->setCurrentGadget(findFirstView()->gadget());
}
void SplitterOrView::saveState(QSettings *qSettings) const

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