diff --git a/flight/modules/Actuator/actuator.c b/flight/modules/Actuator/actuator.c index 8a7bd3266..9a4a9a7f9 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], @@ -687,51 +688,92 @@ static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutr 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 = scaleChannel(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. - } + 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. 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; } /** diff --git a/ground/gcs/src/libs/osgearth/copydata.pro b/ground/gcs/src/libs/osgearth/copydata.pro index 34959bf03..85e77482c 100644 --- a/ground/gcs/src/libs/osgearth/copydata.pro +++ b/ground/gcs/src/libs/osgearth/copydata.pro @@ -10,10 +10,10 @@ equals(copyosg, 1) { data_copy.commands += $(MKDIR) $${GCS_LIBRARY_PATH}/osg $$addNewline() exists( $${OSG_SDK_DIR}/lib64 ) { - data_copy.commands += $(COPY_DIR) $${OSG_SDK_DIR}/lib64/* $${GCS_LIBRARY_PATH}/osg/ $$addNewline() + data_copy.commands += $(COPY_DIR) $$shell_quote($$OSG_SDK_DIR/lib64/)* $$shell_quote($$GCS_LIBRARY_PATH/osg/) } else { - data_copy.commands += $(COPY_DIR) $${OSG_SDK_DIR}/lib/* $${GCS_LIBRARY_PATH}/osg/ $$addNewline() + data_copy.commands += $(COPY_DIR) $$shell_quote($$OSG_SDK_DIR/lib/)* $$shell_quote($$GCS_LIBRARY_PATH/osg/) } # add make target @@ -25,7 +25,7 @@ equals(copyosg, 1) { macx { - 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 { 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 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;" /> +