rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-30 15:52:12 +01:00
Code Issues Releases Activity

LP-107 Replace the scaling function in scaleMotors

Browse Source 
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.
This commit is contained in:
Stefan Karlsson 2015-08-31 21:29:48 +02:00
parent d207f848ee
commit f7b8a57e42
2 changed files with 97 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

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

1
shared/uavobjectdefinition/flightmodesettings.xml
View File

@ -76,6 +76,7 @@
%NE:POI:AutoCruise;" />
<field name="AlwaysStabilizeWhenArmed" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="FALSE" description="For Multirotors. Always stabilize no matter the throttle setting when vehicle is armed. Does not work when vehicle is set to Always Armed."/>
<field name="MotorScalingMode" units="" type="enum" elements="1" options="NoScaling,AddAndSubtract,ElevateAndCompress" defaultvalue="ElevateAndCompress" description="Function to use to scale motors when the motors want to go beyond the end points.one motor wants to go beyond its end points."/>
<field name="ArmedTimeout" units="ms" type="uint16" elements="1" defaultvalue="30000"/>
<field name="ArmingSequenceTime" units="ms" type="uint16" elements="1" defaultvalue="1000"/>
<field name="DisarmingSequenceTime" units="ms" type="uint16" elements="1" defaultvalue="1000"/>