rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

Merged in skarlsso/librepilot/skarlsso/LP-128_elevate_and_compress_is_broken (pull request #72)

Browse Source 
LP-128 Fix scaleMotor attitude problems
This commit is contained in:
Alessio Morale 2015-09-24 21:48:41 +02:00
commit 98c74e6f2a
2 changed files with 60 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -718,6 +718,62 @@ static inline int16_t scaleMotorElevateAndCompress(float value, int16_t max, int
return valueScaled;
}
/**
* Move and compress all motor outputs so that none goes below neutral,
* and all motors are below or equal to max.
*/
static inline int16_t scaleMotorMoveAndCompress(float valueMotor, int16_t max, int16_t neutral, float maxMotor, float minMotor)
{
// The valueMotor parameter is the desired motor value somewhere in the
// [minMotor, maxMotor] range, which is [< -1.00, > 1.00].
//
// Before converting valueMotor to the [neutral, max] range, we scale
// valueMotor to a value in the [0.0f, 1.0f] range.
//
// This is done by, first, conceptually moving all three values valueMotor,
// minMotor, and maxMotor, equally so that the [minMotor, maxMotor] range,
// are contained or overlaps with the [0.0f, 1.0f] range.
//
// Then if the [minMotor, maxMotor] range is larger than 1.0f, the values
// are compressed enough to shrink the [minMotor + move, maxMotor + move]
// range to fit within the [0.0f, 1.0f] range.
// First move the values so that the source range [minMotor, maxMotor]
// covers the target range [0.0f, 1.0f] as much as possible.
float moveValue = 0.0f;
if (minMotor <= 0.0f) {
// Negative minMotor always adjust to 0.
moveValue = -minMotor;
} else if (maxMotor > 1.0f) {
// A too large maxMotor value adjust the range down towards, but not past, the minMotor value.
float beyondMax = maxMotor - 1.0f;
moveValue = -(beyondMax < minMotor ? beyondMax : minMotor);
}
// Then calculate the compress value, if the source range is greater than 1.0f.
float compressValue = 1.0f;
float rangeMotor = maxMotor - minMotor;
if (rangeMotor > 1.0f) {
compressValue = rangeMotor;
}
// Combine the movement and compression, to get the value within [0.0f, 1.0f]
float movedAndCompressedValue = (valueMotor + moveValue) / compressValue;
// And last, convert the value into the [neutral, max] range.
int16_t valueScaled = movedAndCompressedValue * ((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
@ -763,6 +819,9 @@ static int16_t scaleMotor(float value, int16_t max, int16_t min, int16_t neutral
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;

2
shared/uavobjectdefinition/flightmodesettings.xml
View File

@ -76,7 +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="MotorScalingMode" units="" type="enum" elements="1" options="NoScaling,AddAndSubtract,ElevateAndCompress,MoveAndCompress" defaultvalue="MoveAndCompress" description="Function to use to scale motors when the motors want to go beyond the 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"/>