From 51f0b74e5cc437a33557d7157c357f0fa446360f Mon Sep 17 00:00:00 2001 From: Stefan Karlsson Date: Thu, 24 Sep 2015 23:02:09 +0200 Subject: [PATCH] LP-129 Remove broken motor scaling modes --- flight/modules/Actuator/actuator.c | 86 +------------------ .../flightmodesettings.xml | 1 - 2 files changed, 4 insertions(+), 83 deletions(-) diff --git a/flight/modules/Actuator/actuator.c b/flight/modules/Actuator/actuator.c index d3864a19d..c0adc82f1 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, FlightModeSettingsMotorScalingModeOptions scalingMode); +static int16_t scaleMotor(float value, int16_t max, int16_t min, int16_t neutral, float maxMotor, float minMotor, bool armed, bool AlwaysStabilizeWhenArmed, float throttleDesired); static void setFailsafe(); static float MixerCurveFullRangeProportional(const float input, const float *curve, uint8_t elements, bool multirotor); static float MixerCurveFullRangeAbsolute(const float input, const float *curve, uint8_t elements, bool multirotor); @@ -499,8 +499,7 @@ static void actuatorTask(__attribute__((unused)) void *parameters) minMotor, armed, AlwaysStabilizeWhenArmed, - throttleDesired, - settings.MotorScalingMode); + throttleDesired); } else { // else we scale the channel command.Channel[i] = scaleChannel(status[i], actuatorSettings.ChannelMax[i], @@ -688,36 +687,6 @@ 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; -} - /** * Move and compress all motor outputs so that none goes below neutral, * and all motors are below or equal to max. @@ -774,62 +743,15 @@ static inline int16_t scaleMotorMoveAndCompress(float valueMotor, int16_t max, i 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, FlightModeSettingsMotorScalingModeOptions scalingMode) +static int16_t scaleMotor(float value, int16_t max, int16_t min, int16_t neutral, float maxMotor, float minMotor, bool armed, bool AlwaysStabilizeWhenArmed, float throttleDesired) { int16_t valueScaled; if (max > min) { - switch (scalingMode) { - case FLIGHTMODESETTINGS_MOTORSCALINGMODE_NOSCALING: - valueScaled = scaleChannel(value, max, min, neutral); - break; - case FLIGHTMODESETTINGS_MOTORSCALINGMODE_MOVEANDCOMPRESS: - valueScaled = scaleMotorMoveAndCompress(value, max, neutral, maxMotor, minMotor); - 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); - } + valueScaled = scaleMotorMoveAndCompress(value, max, neutral, maxMotor, minMotor); } else { // not sure what to do about reversed polarity right now. Why would anyone do this? valueScaled = scaleChannel(value, max, min, neutral); diff --git a/shared/uavobjectdefinition/flightmodesettings.xml b/shared/uavobjectdefinition/flightmodesettings.xml index 35e66f4bb..162f96d6c 100644 --- a/shared/uavobjectdefinition/flightmodesettings.xml +++ b/shared/uavobjectdefinition/flightmodesettings.xml @@ -76,7 +76,6 @@ %NE:POI:AutoCruise;" /> -