mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-02-20 10:54:14 +01:00
OP-1838 altvario
Fixes and tidyup. TODO: 1. Fix UI for sliders 2. Disable vtol tuning on stabi input 3. Altitude loss feedback
This commit is contained in:
abeck70
parent
443b3742cc
commit
e36ab50040
@ -64,8 +64,6 @@ static PIDControlDown controlDown;
|
||||
static AltitudeHoldSettingsData altitudeHoldSettings;
|
||||
static ThrustModeType thrustMode;
|
||||
static float thrustDemand = 0.0f;
|
||||
static float thrustRate;
|
||||
static uint8_t thrustExp;
|
||||
|
||||
|
||||
// Private functions
|
||||
@ -99,11 +97,11 @@ float stabilizationAltitudeHold(float setpoint, ThrustModeType mode, bool reinit
|
||||
} else if (mode == ALTITUDEVARIO && setpoint > DEADBAND_HIGH) {
|
||||
// being the two band symmetrical I can divide by DEADBAND_LOW to scale it to a value betweeon 0 and 1
|
||||
// then apply an "exp" f(x,k) = (k*x*x*x + (255-k)*x) / 255
|
||||
controlDown.UpdateVelocitySetpoint(-((thrustExp * powf((setpoint - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - thrustExp) * (setpoint - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * thrustRate));
|
||||
controlDown.UpdateVelocitySetpoint(-((altitudeHoldSettings.ThrustExp * powf((setpoint - DEADBAND_HIGH) / (DEADBAND_LOW), 3.0f) + (255.0f - altitudeHoldSettings.ThrustExp) * (setpoint - DEADBAND_HIGH) / DEADBAND_LOW) / 255.0f * altitudeHoldSettings.ThrustRate));
|
||||
thrustMode = ALTITUDEVARIO;
|
||||
newaltitude = true;
|
||||
} else if (mode == ALTITUDEVARIO && setpoint < DEADBAND_LOW) {
|
||||
controlDown.UpdateVelocitySetpoint(-(-(thrustExp * powf((DEADBAND_LOW - (setpoint < 0 ? 0 : setpoint)) / DEADBAND_LOW, 3) + (255 - thrustExp) * (DEADBAND_LOW - setpoint) / DEADBAND_LOW) / 255 * thrustRate));
|
||||
controlDown.UpdateVelocitySetpoint(-(-(altitudeHoldSettings.ThrustExp * powf((DEADBAND_LOW - (setpoint < 0 ? 0 : setpoint)) / DEADBAND_LOW, 3.0f) + (255.0f - altitudeHoldSettings.ThrustExp) * (DEADBAND_LOW - setpoint) / DEADBAND_LOW) / 255.0f * altitudeHoldSettings.ThrustRate));
|
||||
thrustMode = ALTITUDEVARIO;
|
||||
newaltitude = true;
|
||||
} else if (newaltitude == true) {
|
||||
@ -131,6 +129,8 @@ float stabilizationAltitudeHold(float setpoint, ThrustModeType mode, bool reinit
|
||||
controlDown.ControlPosition();
|
||||
altitudeHoldStatus.VelocityDesired = controlDown.GetVelocityDesired();
|
||||
thrustDemand = controlDown.GetDownCommand();
|
||||
|
||||
// if thrust demand is high and we are below altitude by 2m, back off pitch
|
||||
}
|
||||
break;
|
||||
|
||||
@ -140,7 +140,7 @@ float stabilizationAltitudeHold(float setpoint, ThrustModeType mode, bool reinit
|
||||
break;
|
||||
|
||||
case DIRECT:
|
||||
altitudeHoldStatus.VelocityDesired = 0;
|
||||
altitudeHoldStatus.VelocityDesired = 0.0f;
|
||||
break;
|
||||
}
|
||||
|
||||
@ -160,8 +160,8 @@ void stabilizationAltitudeloopInit()
|
||||
VelocityStateInitialize();
|
||||
VtolSelfTuningStatsInitialize();
|
||||
|
||||
// Create object queue
|
||||
AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb);
|
||||
VtolSelfTuningStatsConnectCallback(&SettingsUpdatedCb);
|
||||
SettingsUpdatedCb(NULL);
|
||||
}
|
||||
|
||||
@ -175,7 +175,7 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
|
||||
altitudeHoldSettings.VerticalVelPID.Kd,
|
||||
altitudeHoldSettings.VerticalVelPID.Beta,
|
||||
(float)(OUTERLOOP_SKIPCOUNT * UPDATE_EXPECTED),
|
||||
altitudeHoldSettings.VerticalVelMax);
|
||||
altitudeHoldSettings.ThrustRate);
|
||||
|
||||
controlDown.UpdatePositionalParameters(altitudeHoldSettings.VerticalPosP);
|
||||
|
||||
@ -186,8 +186,6 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
|
||||
// initialise limits on thrust but note the FSM can override.
|
||||
controlDown.SetThrustLimits(altitudeHoldSettings.ThrustLimits.Min, altitudeHoldSettings.ThrustLimits.Max);
|
||||
|
||||
AltitudeHoldSettingsThrustExpGet(&thrustExp);
|
||||
AltitudeHoldSettingsThrustRateGet(&thrustRate);
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -8244,8 +8244,8 @@ border-radius: 5;</string>
|
||||
<rect>
|
||||
<x>0</x>
|
||||
<y>0</y>
|
||||
<width>884</width>
|
||||
<height>736</height>
|
||||
<width>950</width>
|
||||
<height>773</height>
|
||||
</rect>
|
||||
</property>
|
||||
<layout class="QVBoxLayout" name="verticalLayout_29">
|
||||
@ -18236,8 +18236,8 @@ border-radius: 5;</string>
|
||||
<rect>
|
||||
<x>0</x>
|
||||
<y>0</y>
|
||||
<width>839</width>
|
||||
<height>670</height>
|
||||
<width>950</width>
|
||||
<height>773</height>
|
||||
</rect>
|
||||
</property>
|
||||
<layout class="QVBoxLayout" name="verticalLayout_8" stretch="0,0,0,0,0,0">
|
||||
@ -25298,10 +25298,10 @@ border-radius: 5;</string>
|
||||
<string><html><head/><body><p>How fast the vehicle should adjust its neutral throttle estimation. Altitude assumes that when engaged the throttle is in the range required to hover. If the throttle is a lot higher or lower, it needs to adjust this &quot;throttle trim&quot; Higher values make it do this adjustment faster, but this could lead to ugly oscillations. Leave at default unless you know what you are doing.</p></body></html></string>
|
||||
</property>
|
||||
<property name="maximum">
|
||||
<number>1000</number>
|
||||
<number>10</number>
|
||||
</property>
|
||||
<property name="value">
|
||||
<number>50</number>
|
||||
<number>10</number>
|
||||
</property>
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
@ -25351,10 +25351,10 @@ border-radius: 5;</string>
|
||||
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
|
||||
</property>
|
||||
<property name="maximum">
|
||||
<number>1000</number>
|
||||
<number>10</number>
|
||||
</property>
|
||||
<property name="value">
|
||||
<number>51</number>
|
||||
<number>10</number>
|
||||
</property>
|
||||
<property name="objrelation" stdset="0">
|
||||
<stringlist>
|
||||
@ -25458,10 +25458,10 @@ border-radius: 5;</string>
|
||||
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
|
||||
</property>
|
||||
<property name="maximum">
|
||||
<number>1000</number>
|
||||
<number>1</number>
|
||||
</property>
|
||||
<property name="value">
|
||||
<number>51</number>
|
||||
<number>1</number>
|
||||
</property>
|
||||
<property name="objrelation" stdset="0">
|
||||
<stringlist>
|
||||
@ -25512,10 +25512,10 @@ border-radius: 5;</string>
|
||||
<string><html><head/><body><p>How fast the vehicle should adjust its neutral throttle estimation. Altitude assumes that when engaged the throttle is in the range required to hover. If the throttle is a lot higher or lower, it needs to adjust this &quot;throttle trim&quot; Higher values make it do this adjustment faster, but this could lead to ugly oscillations. Leave at default unless you know what you are doing.</p></body></html></string>
|
||||
</property>
|
||||
<property name="maximum">
|
||||
<number>1000</number>
|
||||
<number>1</number>
|
||||
</property>
|
||||
<property name="value">
|
||||
<number>50</number>
|
||||
<number>1</number>
|
||||
</property>
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
@ -25565,10 +25565,10 @@ border-radius: 5;</string>
|
||||
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
|
||||
</property>
|
||||
<property name="maximum">
|
||||
<number>1000</number>
|
||||
<number>1</number>
|
||||
</property>
|
||||
<property name="value">
|
||||
<number>51</number>
|
||||
<number>1</number>
|
||||
</property>
|
||||
<property name="objrelation" stdset="0">
|
||||
<stringlist>
|
||||
|
||||
@ -5,7 +5,6 @@
|
||||
<field name="ThrustExp" units="" type="uint8" elements="1" defaultvalue="128" />
|
||||
<field name="ThrustRate" units="m/s" type="float" elements="1" defaultvalue="5" />
|
||||
<field name="ThrustLimits" units="" type="float" elementnames="Min,Neutral,Max" defaultvalue="0.2, 0.5, 0.9"/>
|
||||
<field name="VerticalVelMax" units="m/s" type="float" elements="1" defaultvalue="4.0" description="maximum allowed climb/dive velocity"/>
|
||||
<field name="VerticalPosP" units="(m/s)/m" type="float" elements="1" defaultvalue="0.4"/>
|
||||
<field name="VerticalVelPID" units="(m/s^2)/(m/s)" type="float" elementnames="Kp,Ki,Kd,Beta" defaultvalue="0.35, 3.0, 0.05, 0.9"/>
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user