rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-18 08:54:15 +01:00
Code Issues Releases Activity

Merge remote-tracking branch 'origin/next' into filnet/LP-29_osgearth_integration

Browse Source 
Conflicts:
	ground/gcs/src/libs/osgearth/copydata.pro
This commit is contained in:
Philippe Renon 2015-09-16 22:55:54 +02:00
commit 24a5bdfa49
6 changed files with 93 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

116
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],
@ -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.

15
flight/modules/Stabilization/stabilization.c
View File

@ -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;
}
/**

6
ground/gcs/src/libs/osgearth/copydata.pro
View File

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

1
ground/gcs/src/libs/osgearth/osgearth.pro
View File

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

4
make/tools.mk
View File

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

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"/>