From f7b8a57e42bf7b2953f874a356da6a01f33e248b Mon Sep 17 00:00:00 2001 From: Stefan Karlsson Date: Mon, 31 Aug 2015 21:29:48 +0200 Subject: [PATCH 1/7] LP-107 Replace the scaling function in scaleMotors The previous scaling function used in scaleMotor could end up with output values below the neutral point. This patch adds a new scaling mode that will try to scale the output proportionally while still keep all outputs within neutral and max. The user can select three different modes: 1) NoScaling - Only cap the output values between neutral and max. 2) AddAndSubtract - The previous output scaling method, which moves the output values directly proportionally to the amount the max/min motor is out of the limits. 3) ElevateAndCompress - The new mode, which elevates all motor values by the percentage needed to bring the min motor to neutral, and then compress all motor values by the percentage needed to bring the max motor down to max. The motor scaling mode can be selected by setting the FlightModeSettings.MotorScalingMode UAVO field. --- flight/modules/Actuator/actuator.c | 133 +++++++++++++----- .../flightmodesettings.xml | 1 + 2 files changed, 97 insertions(+), 37 deletions(-) diff --git a/flight/modules/Actuator/actuator.c b/flight/modules/Actuator/actuator.c index 7722408a3..d4ac15c89 100644 --- a/flight/modules/Actuator/actuator.c +++ b/flight/modules/Actuator/actuator.c @@ -98,7 +98,7 @@ static int mixer_settings_count = 2; // Private functions static void actuatorTask(void *parameters); static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutral); -static int16_t scaleMotor(float value, int16_t max, int16_t min, int16_t neutral, float maxMotor, float minMotor, bool armed, bool AlwaysStabilizeWhenArmed, float throttleDesired); +static int16_t scaleMotor(float value, int16_t max, int16_t min, int16_t neutral, float maxMotor, float minMotor, bool armed, bool AlwaysStabilizeWhenArmed, float throttleDesired, FlightModeSettingsMotorScalingModeOptions scalingMode); static void setFailsafe(); static float MixerCurveFullRangeProportional(const float input, const float *curve, uint8_t elements, bool multirotor); static float MixerCurveFullRangeAbsolute(const float input, const float *curve, uint8_t elements, bool multirotor); @@ -499,7 +499,8 @@ static void actuatorTask(__attribute__((unused)) void *parameters) minMotor, armed, AlwaysStabilizeWhenArmed, - throttleDesired); + throttleDesired, + settings.MotorScalingMode); } else { // else we scale the channel command.Channel[i] = scaleChannel(status[i], actuatorSettings.ChannelMax[i], @@ -688,51 +689,109 @@ static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutr return valueScaled; } +/** + * No motor scaling, only limit the values to min and max. + */ +static inline int16_t scaleMotorNoScaling(float value, int16_t max, int16_t min, int16_t neutral) +{ + int16_t valueScaled = (int16_t)(value * ((float)(max - neutral))) + neutral; + + if (valueScaled > max) { + valueScaled = max; // clamp to max value only after scaling is done. + } + if (valueScaled < min) { + valueScaled = min; // clamp to min value only after scaling is done. + } + return valueScaled; +} + +/** + * Elevate all motor outputs so that none goes below neutral. + * Compress all motors so that all motors are below or equal to max. + */ +static inline int16_t scaleMotorElevateAndCompress(float value, int16_t max, int16_t neutral, float maxMotor, float minMotor) +{ + float elevatedAndCompressedValue = value; + + if (minMotor < 0.0f) { + // Elevate the value. + elevatedAndCompressedValue += -minMotor; + } + + float rangeMotor = maxMotor - minMotor; + if (rangeMotor > 1.0f) { + // Compress the value. + elevatedAndCompressedValue /= rangeMotor; + } + + int16_t valueScaled = elevatedAndCompressedValue * ((float)(max - neutral)) + neutral; + + if (valueScaled > max) { + valueScaled = max; // clamp to max value only after scaling is done. + } + + PIOS_Assert(valueScaled >= neutral); + + return valueScaled; +} + +static inline int16_t scaleMotorAddAndSubtract(float value, int16_t max, int16_t min, int16_t neutral, float maxMotor, float minMotor, bool AlwaysStabilizeWhenArmed) +{ + // Scale + int16_t valueScaled = (int16_t)(value * ((float)(max - neutral))) + neutral; + int16_t maxMotorScaled = (int16_t)(maxMotor * ((float)(max - neutral))) + neutral; + int16_t minMotorScaled = (int16_t)(minMotor * ((float)(max - neutral))) + neutral; + + int16_t diff = max - maxMotorScaled; // difference between max allowed and actual max motor + + if (diff < 0) { // if the difference is smaller than 0 we add it to the scaled value + valueScaled += diff; + } + diff = neutral - minMotorScaled; // difference between min allowed and actual min motor + if (diff > 0) { // if the difference is larger than 0 we add it to the scaled value + valueScaled += diff; + } + + if ((valueScaled < neutral) && (spinWhileArmed) && AlwaysStabilizeWhenArmed) { + valueScaled = neutral; // clamp to neutral value only after scaling is done. + } else if ((valueScaled < neutral) && (!spinWhileArmed) && AlwaysStabilizeWhenArmed) { + valueScaled = neutral; // clamp to neutral value only after scaling is done. //throttle goes to min if throttledesired is equal to or less than 0 below + } else if (valueScaled < neutral) { + valueScaled = min; // clamp to min value only after scaling is done. + } + + if (valueScaled > max) { + valueScaled = max; // clamp to max value only after scaling is done. + } + + return valueScaled; +} + + /** * Constrain motor values to keep any one motor value from going too far out of range of another motor */ -static int16_t scaleMotor(float value, int16_t max, int16_t min, int16_t neutral, float maxMotor, float minMotor, bool armed, bool AlwaysStabilizeWhenArmed, float throttleDesired) +static int16_t scaleMotor(float value, int16_t max, int16_t min, int16_t neutral, float maxMotor, float minMotor, bool armed, bool AlwaysStabilizeWhenArmed, float throttleDesired, FlightModeSettingsMotorScalingModeOptions scalingMode) { int16_t valueScaled; - int16_t maxMotorScaled; - int16_t minMotorScaled; - int16_t diff; - - // Scale - valueScaled = (int16_t)(value * ((float)(max - neutral))) + neutral; - maxMotorScaled = (int16_t)(maxMotor * ((float)(max - neutral))) + neutral; - minMotorScaled = (int16_t)(minMotor * ((float)(max - neutral))) + neutral; - if (max > min) { - diff = max - maxMotorScaled; // difference between max allowed and actual max motor - if (diff < 0) { // if the difference is smaller than 0 we add it to the scaled value - valueScaled += diff; - } - diff = neutral - minMotorScaled; // difference between min allowed and actual min motor - if (diff > 0) { // if the difference is larger than 0 we add it to the scaled value - valueScaled += diff; - } - // todo: make this flow easier to understand - if (valueScaled > max) { - valueScaled = max; // clamp to max value only after scaling is done. - } - - if ((valueScaled < neutral) && (spinWhileArmed) && AlwaysStabilizeWhenArmed) { - valueScaled = neutral; // clamp to neutral value only after scaling is done. - } else if ((valueScaled < neutral) && (!spinWhileArmed) && AlwaysStabilizeWhenArmed) { - valueScaled = neutral; // clamp to neutral value only after scaling is done. //throttle goes to min if throttledesired is equal to or less than 0 below - } else if (valueScaled < neutral) { - valueScaled = min; // clamp to min value only after scaling is done. + switch (scalingMode) { + case FLIGHTMODESETTINGS_MOTORSCALINGMODE_NOSCALING: + valueScaled = scaleMotorNoScaling(value, max, min, neutral); + break; + case FLIGHTMODESETTINGS_MOTORSCALINGMODE_ELEVATEANDCOMPRESS: + valueScaled = scaleMotorElevateAndCompress(value, max, neutral, maxMotor, minMotor); + break; + case FLIGHTMODESETTINGS_MOTORSCALINGMODE_ADDANDSUBTRACT: + valueScaled = scaleMotorAddAndSubtract(value, max, min, neutral, maxMotor, minMotor, AlwaysStabilizeWhenArmed); + break; + default: PIOS_Assert(false); } } else { // not sure what to do about reversed polarity right now. Why would anyone do this? - if (valueScaled < max) { - valueScaled = max; // clamp to max value only after scaling is done. - } - if (valueScaled > min) { - valueScaled = min; // clamp to min value only after scaling is done. - } + // Note the reversal of the max and min arguments below. + valueScaled = scaleMotorNoScaling(value, min, max, neutral); } // I've added the bool AlwaysStabilizeWhenArmed to this function. Right now we command the motors at min or a range between neutral and max. diff --git a/shared/uavobjectdefinition/flightmodesettings.xml b/shared/uavobjectdefinition/flightmodesettings.xml index 162f96d6c..42496711f 100644 --- a/shared/uavobjectdefinition/flightmodesettings.xml +++ b/shared/uavobjectdefinition/flightmodesettings.xml @@ -76,6 +76,7 @@ %NE:POI:AutoCruise;" /> + From 71f6e8239974e89625922d9a7e00f27d62010890 Mon Sep 17 00:00:00 2001 From: Stefan Karlsson Date: Tue, 1 Sep 2015 21:17:52 +0200 Subject: [PATCH 2/7] LP-107 Use scaleChannel for unscaled motor outputs --- flight/modules/Actuator/actuator.c | 21 ++------------------- 1 file changed, 2 insertions(+), 19 deletions(-) diff --git a/flight/modules/Actuator/actuator.c b/flight/modules/Actuator/actuator.c index d4ac15c89..4bbb3a8cb 100644 --- a/flight/modules/Actuator/actuator.c +++ b/flight/modules/Actuator/actuator.c @@ -689,22 +689,6 @@ static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutr return valueScaled; } -/** - * No motor scaling, only limit the values to min and max. - */ -static inline int16_t scaleMotorNoScaling(float value, int16_t max, int16_t min, int16_t neutral) -{ - int16_t valueScaled = (int16_t)(value * ((float)(max - neutral))) + neutral; - - if (valueScaled > max) { - valueScaled = max; // clamp to max value only after scaling is done. - } - if (valueScaled < min) { - valueScaled = min; // clamp to min value only after scaling is done. - } - return valueScaled; -} - /** * Elevate all motor outputs so that none goes below neutral. * Compress all motors so that all motors are below or equal to max. @@ -778,7 +762,7 @@ static int16_t scaleMotor(float value, int16_t max, int16_t min, int16_t neutral if (max > min) { switch (scalingMode) { case FLIGHTMODESETTINGS_MOTORSCALINGMODE_NOSCALING: - valueScaled = scaleMotorNoScaling(value, max, min, neutral); + valueScaled = scaleChannel(value, max, min, neutral); break; case FLIGHTMODESETTINGS_MOTORSCALINGMODE_ELEVATEANDCOMPRESS: valueScaled = scaleMotorElevateAndCompress(value, max, neutral, maxMotor, minMotor); @@ -790,8 +774,7 @@ static int16_t scaleMotor(float value, int16_t max, int16_t min, int16_t neutral } } else { // not sure what to do about reversed polarity right now. Why would anyone do this? - // Note the reversal of the max and min arguments below. - valueScaled = scaleMotorNoScaling(value, min, max, neutral); + valueScaled = scaleChannel(value, max, min, neutral); } // I've added the bool AlwaysStabilizeWhenArmed to this function. Right now we command the motors at min or a range between neutral and max. From 9b495456e28839c79f6e8c1fe7ed1858fcbd0c3f Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Sun, 6 Sep 2015 17:10:52 +0200 Subject: [PATCH 3/7] LP-29 all_sdk_install will not install osg anymore, instead build_sdk_install will install if OS is windows --- make/tools.mk | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/make/tools.mk b/make/tools.mk index 4bfbbe45d..ea4fe4f73 100644 --- a/make/tools.mk +++ b/make/tools.mk @@ -149,9 +149,9 @@ QT_SDK_PREFIX := $(QT_SDK_DIR) BUILD_SDK_TARGETS := arm_sdk qt_sdk ifeq ($(UNAME), Windows) - BUILD_SDK_TARGETS += sdl nsis mesawin openssl ccache + BUILD_SDK_TARGETS += osg sdl nsis mesawin openssl ccache endif -ALL_SDK_TARGETS := $(BUILD_SDK_TARGETS) osg gtest uncrustify doxygen +ALL_SDK_TARGETS := $(BUILD_SDK_TARGETS) gtest uncrustify doxygen define GROUP_SDK_TEMPLATE .PHONY: $(1)_install $(1)_clean $(1)_distclean $(1)_version From a7c592b29454be4f6bc53fd0e05959d9f6596d94 Mon Sep 17 00:00:00 2001 From: Stefan Karlsson Date: Wed, 9 Sep 2015 01:27:20 +0200 Subject: [PATCH 4/7] LP-115 Fix TPS and Acroplus settings setup Both TPS and Acroplus had settings initialized in SettingsUpdatedCb. These values comes from stabSettings.stabBank, which isn't initialzed when SettingsUpdatedCb is first called. The patch moves the setup of the affected settings values to BankUpdatedCb, where other bank specific settings are updated. --- flight/modules/Stabilization/stabilization.c | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index ba841b196..d9c3c01cb 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -341,6 +341,14 @@ static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) && tps_for_pid[pid]; } } + + for (int i = 0; i < STABILIZATIONSETTINGSBANK1_THRUSTPIDSCALECURVE_NUMELEM; i++) { + stabSettings.floatThrustPIDScaleCurve[i] = (float)(stabSettings.stabBank.ThrustPIDScaleCurve[i]) * 0.01f; + } + + stabSettings.acroInsanityFactors[0] = (float)(stabSettings.stabBank.AcroInsanityFactor.Roll) * 0.01f; + stabSettings.acroInsanityFactors[1] = (float)(stabSettings.stabBank.AcroInsanityFactor.Pitch) * 0.01f; + stabSettings.acroInsanityFactors[2] = (float)(stabSettings.stabBank.AcroInsanityFactor.Yaw) * 0.01f; } @@ -381,13 +389,6 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) stabSettings.cruiseControl.power_trim = stabSettings.settings.CruiseControlPowerTrim / 100.0f; stabSettings.cruiseControl.half_power_delay = stabSettings.settings.CruiseControlPowerDelayComp / 2.0f; stabSettings.cruiseControl.max_power_factor_angle = RAD2DEG(acosf(1.0f / stabSettings.settings.CruiseControlMaxPowerFactor)); - - for (int i = 0; i < STABILIZATIONSETTINGSBANK1_THRUSTPIDSCALECURVE_NUMELEM; i++) { - stabSettings.floatThrustPIDScaleCurve[i] = (float)(stabSettings.stabBank.ThrustPIDScaleCurve[i]) * 0.01f; - } - stabSettings.acroInsanityFactors[0] = (float)(stabSettings.stabBank.AcroInsanityFactor.Roll) * 0.01f; - stabSettings.acroInsanityFactors[1] = (float)(stabSettings.stabBank.AcroInsanityFactor.Pitch) * 0.01f; - stabSettings.acroInsanityFactors[2] = (float)(stabSettings.stabBank.AcroInsanityFactor.Yaw) * 0.01f; } /** From 466f4bb69558afdf8015c8637bfc91c4b92c3179 Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Sat, 12 Sep 2015 18:04:16 +0200 Subject: [PATCH 5/7] LP-29 disabled warnings on mac (mac treats warnings are errors...) --- ground/gcs/src/libs/osgearth/osgearth.pro | 3 +++ 1 file changed, 3 insertions(+) diff --git a/ground/gcs/src/libs/osgearth/osgearth.pro b/ground/gcs/src/libs/osgearth/osgearth.pro index 47d15d7cf..7a8007e9a 100644 --- a/ground/gcs/src/libs/osgearth/osgearth.pro +++ b/ground/gcs/src/libs/osgearth/osgearth.pro @@ -12,6 +12,9 @@ contains(DEFINES, OSG_USE_QT_PRIVATE) { include(../../library.pri) include(../utils/utils.pri) +# disable all warnings on mac to avoid build failures +macx:CONFIG += warn_off + # osg and osgearth emit a lot of unused parameter warnings... QMAKE_CXXFLAGS += -Wno-unused-parameter From e7436823116b1eacb7742a0596ddb8ee93c4b981 Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Sat, 12 Sep 2015 18:19:57 +0200 Subject: [PATCH 6/7] LP-29 moved osg RPATHDIR definition to libs/osgearth/osgearth.pro --- ground/gcs/src/library.pri | 1 - ground/gcs/src/libs/osgearth/osgearth.pro | 7 ++++++- ground/gcs/src/plugin.pri | 1 - 3 files changed, 6 insertions(+), 3 deletions(-) diff --git a/ground/gcs/src/library.pri b/ground/gcs/src/library.pri index a57a8f308..3120d28ce 100644 --- a/ground/gcs/src/library.pri +++ b/ground/gcs/src/library.pri @@ -19,7 +19,6 @@ macx { } else { QMAKE_RPATHDIR = $$shell_quote(\$$ORIGIN) QMAKE_RPATHDIR += $$shell_quote(\$$ORIGIN/$$relative_path($$GCS_QT_LIBRARY_PATH, $$GCS_LIBRARY_PATH)) - QMAKE_RPATHDIR += $$shell_quote(\$$ORIGIN/$$relative_path($$GCS_LIBRARY_PATH/osg, $$GCS_LIBRARY_PATH)) include(rpath.pri) target.path = /$$GCS_LIBRARY_BASENAME/gcs diff --git a/ground/gcs/src/libs/osgearth/osgearth.pro b/ground/gcs/src/libs/osgearth/osgearth.pro index 7a8007e9a..0f0f3aae5 100644 --- a/ground/gcs/src/libs/osgearth/osgearth.pro +++ b/ground/gcs/src/libs/osgearth/osgearth.pro @@ -6,12 +6,17 @@ DEFINES += OSGEARTH_LIBRARY QT += widgets opengl qml quick contains(DEFINES, OSG_USE_QT_PRIVATE) { - QT += core-private gui-private + QT += core-private gui-private } include(../../library.pri) include(../utils/utils.pri) +linux { + QMAKE_RPATHDIR = $$shell_quote(\$$ORIGIN/$$relative_path($$GCS_LIBRARY_PATH/osg, $$GCS_LIBRARY_PATH)) + include(../../rpath.pri) +} + # disable all warnings on mac to avoid build failures macx:CONFIG += warn_off diff --git a/ground/gcs/src/plugin.pri b/ground/gcs/src/plugin.pri index 33548c23f..ea4764a9e 100644 --- a/ground/gcs/src/plugin.pri +++ b/ground/gcs/src/plugin.pri @@ -31,7 +31,6 @@ macx { QMAKE_RPATHDIR = $$shell_quote(\$$ORIGIN) QMAKE_RPATHDIR += $$shell_quote(\$$ORIGIN/$$relative_path($$GCS_LIBRARY_PATH, $$DESTDIR)) QMAKE_RPATHDIR += $$shell_quote(\$$ORIGIN/$$relative_path($$GCS_QT_LIBRARY_PATH, $$DESTDIR)) - QMAKE_RPATHDIR += $$shell_quote(\$$ORIGIN/$$relative_path($$GCS_LIBRARY_PATH/osg, $$DESTDIR)) include(rpath.pri) } From 66901c2be7921edc597892aa41f1f3ad0319b2b6 Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Sat, 12 Sep 2015 18:18:55 +0200 Subject: [PATCH 7/7] LP-29 fixed osgearth copydata.pro for mac and linux --- ground/gcs/src/libs/osgearth/copydata.pro | 21 +++++++++++++-------- ground/gcs/src/libs/osgearth/osgearth.pro | 1 - 2 files changed, 13 insertions(+), 9 deletions(-) diff --git a/ground/gcs/src/libs/osgearth/copydata.pro b/ground/gcs/src/libs/osgearth/copydata.pro index 9d22738bd..85e77482c 100644 --- a/ground/gcs/src/libs/osgearth/copydata.pro +++ b/ground/gcs/src/libs/osgearth/copydata.pro @@ -7,20 +7,25 @@ equals(copyosg, 1) { linux { # copy osg libraries + + data_copy.commands += $(MKDIR) $${GCS_LIBRARY_PATH}/osg $$addNewline() exists( $${OSG_SDK_DIR}/lib64 ) { - #addCopyDirFilesTargets($${OSG_SDK_DIR}/lib64,$${GCS_LIBRARY_PATH}/osg) - } else { - #addCopyDirFilesTargets($${OSG_SDK_DIR}/lib,$${GCS_LIBRARY_PATH}/osg) + data_copy.commands += $(COPY_DIR) $$shell_quote($$OSG_SDK_DIR/lib64/)* $$shell_quote($$GCS_LIBRARY_PATH/osg/) } + else { + data_copy.commands += $(COPY_DIR) $$shell_quote($$OSG_SDK_DIR/lib/)* $$shell_quote($$GCS_LIBRARY_PATH/osg/) + } + + # add make target + POST_TARGETDEPS += copydata + + data_copy.target = copydata + QMAKE_EXTRA_TARGETS += data_copy } macx { - isEmpty(PROVIDER) { - PROVIDER = OpenPilot - } - - data_copy.commands += $(COPY_DIR) $$targetPath(\"$$OSG_SDK_DIR/lib/\"*) $$targetPath(\"$$GCS_LIBRARY_PATH\") $$addNewline() + data_copy.commands += $(COPY_DIR) $$shell_quote($$OSG_SDK_DIR/lib/)* $$shell_quote($$GCS_LIBRARY_PATH/) # add make target POST_TARGETDEPS += copydata diff --git a/ground/gcs/src/libs/osgearth/osgearth.pro b/ground/gcs/src/libs/osgearth/osgearth.pro index 0f0f3aae5..f8c3e4423 100644 --- a/ground/gcs/src/libs/osgearth/osgearth.pro +++ b/ground/gcs/src/libs/osgearth/osgearth.pro @@ -84,7 +84,6 @@ macx { LIBS += -lOpenThreads LIBS += -losg -losgUtil -losgDB -losgGA -losgViewer -losgText -losgQt LIBS += -losgEarth -losgEarthUtil -losgEarthFeatures -losgEarthSymbology -losgEarthAnnotation -losgEarthQt - LIBS += -losgDB } win32 {