mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-11-29 07:24:13 +01:00
OP-1022 add position lowpass, rework setting uavo, cleanup
This commit is contained in:
Alessio Morale
parent
e2a7c6cb25
commit
6b27ff1e27
@ -66,7 +66,7 @@
|
||||
#define TASK_PRIORITY (tskIDLE_PRIORITY + 1)
|
||||
#define ACCEL_DOWNSAMPLE 4
|
||||
#define TIMEOUT_TRESHOLD 200000
|
||||
#define DESIRED_UPDATE_RATE_MS 20 // milliseconds
|
||||
#define DESIRED_UPDATE_RATE_MS 100 // milliseconds
|
||||
// Private types
|
||||
|
||||
// Private variables
|
||||
@ -113,7 +113,7 @@ int32_t AltitudeHoldInitialize()
|
||||
MODULE_INITCALL(AltitudeHoldInitialize, AltitudeHoldStart);
|
||||
|
||||
float tau;
|
||||
float accelAlpha;
|
||||
float positionAlpha;
|
||||
float velAlpha;
|
||||
bool running = false;
|
||||
|
||||
@ -137,12 +137,11 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
|
||||
VelocityStateData velocityData;
|
||||
float dT;
|
||||
float fblimit = 0;
|
||||
float lastVertVelocity;
|
||||
|
||||
portTickType thisTime, lastUpdateTime;
|
||||
UAVObjEvent ev;
|
||||
|
||||
dT = 0;
|
||||
lastVertVelocity = 0;
|
||||
timeval = 0;
|
||||
lastUpdateTime = 0;
|
||||
// Force update of the settings
|
||||
@ -215,15 +214,10 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
|
||||
VelocityStateGet(&velocityData);
|
||||
|
||||
altHold.Velocity = -(velAlpha * altHold.Velocity + (1 - velAlpha) * velocityData.Down);
|
||||
float vertAccel = (velocityData.Down - lastVertVelocity) / dT;
|
||||
|
||||
lastVertVelocity = velocityData.Down;
|
||||
|
||||
altHold.Accel = -(accelAlpha * altHold.Accel + (1 - accelAlpha) * vertAccel);
|
||||
altHold.StateUpdateInterval = dT;
|
||||
|
||||
PositionStateDownGet(&altHold.Altitude);
|
||||
altHold.Altitude = -altHold.Altitude;
|
||||
float position;
|
||||
PositionStateDownGet(&position);
|
||||
altHold.Altitude = -(positionAlpha * position) + (1 - positionAlpha) * altHold.Altitude;
|
||||
|
||||
AltHoldSmoothedSet(&altHold);
|
||||
|
||||
@ -246,8 +240,8 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
|
||||
velError = altitudeHoldDesired.Velocity - altHold.Velocity;
|
||||
|
||||
// Compute altitude and velocity integral
|
||||
altitudeIntegral += (error - fblimit) * altitudeHoldSettings.AltitudeKi * dT;
|
||||
velocityIntegral += (velError - fblimit) * altitudeHoldSettings.VelocityKi * dT;
|
||||
altitudeIntegral += (error - fblimit) * altitudeHoldSettings.AltitudePID[ALTITUDEHOLDSETTINGS_ALTITUDEPID_KI] * dT;
|
||||
velocityIntegral += (velError - fblimit) * altitudeHoldSettings.VelocityPI[ALTITUDEHOLDSETTINGS_VELOCITYPI_KI] * dT;
|
||||
|
||||
|
||||
thisTime = xTaskGetTickCount();
|
||||
@ -255,17 +249,15 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
|
||||
if ((thisTime - lastUpdateTime) * 1000 / configTICK_RATE_HZ < DESIRED_UPDATE_RATE_MS) {
|
||||
continue;
|
||||
}
|
||||
altHold.ThrottleUpdateInterval = thisTime - lastUpdateTime;
|
||||
lastUpdateTime = thisTime;
|
||||
|
||||
// Instead of explicit limit on integral you output limit feedback
|
||||
StabilizationDesiredGet(&stabilizationDesired);
|
||||
if (!enterFailSafe) {
|
||||
stabilizationDesired.Throttle = altitudeIntegral + velocityIntegral
|
||||
+ error * fabsf(error) * altitudeHoldSettings.Altitude2Kp
|
||||
+ error * altitudeHoldSettings.AltitudeKp
|
||||
+ velError * altitudeHoldSettings.VelocityKp
|
||||
+ derivative * altitudeHoldSettings.AltitudeKd;
|
||||
+ error * altitudeHoldSettings.AltitudePID[ALTITUDEHOLDSETTINGS_ALTITUDEPID_KP]
|
||||
+ velError * altitudeHoldSettings.VelocityPI[ALTITUDEHOLDSETTINGS_VELOCITYPI_KP]
|
||||
+ derivative * altitudeHoldSettings.AltitudePID[ALTITUDEHOLDSETTINGS_ALTITUDEPID_KD];
|
||||
|
||||
// scale up throttle to compensate for roll/pitch angle but limit this to 60 deg (cos(60) == 0.5) to prevent excessive scaling
|
||||
AttitudeStateData attitudeState;
|
||||
@ -312,7 +304,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
|
||||
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
|
||||
{
|
||||
AltitudeHoldSettingsGet(&altitudeHoldSettings);
|
||||
accelAlpha = expf(-(1000.0f / 666.0f * ACCEL_DOWNSAMPLE) / altitudeHoldSettings.AccelTau);
|
||||
positionAlpha = expf(-(1000.0f / 666.0f * ACCEL_DOWNSAMPLE) / altitudeHoldSettings.PositionTau);
|
||||
velAlpha = expf(-(1000.0f / 666.0f * ACCEL_DOWNSAMPLE) / altitudeHoldSettings.VelocityTau);
|
||||
|
||||
// don't use throttle filter if specified cutoff frequency is too low or above nyquist criteria (half the sampling frequency)
|
||||
|
||||
@ -869,8 +869,8 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed)
|
||||
} else if (cmd->Throttle >= DEADBAND_LOW && cmd->Throttle <= DEADBAND_HIGH && (throttleRate != 0)) {
|
||||
// Require the stick to enter the dead band before they can move height
|
||||
// Vario is not "engaged" when throttleRate == 0
|
||||
if(fabsf(altitudeHoldDesiredData.Velocity)> 1e-3f)
|
||||
{;
|
||||
if (fabsf(altitudeHoldDesiredData.Velocity) > 1e-3f) {
|
||||
;
|
||||
altitudeHoldDesiredData.Velocity = 0;
|
||||
altitudeHoldDesiredData.Altitude = altHold.Altitude;
|
||||
}
|
||||
|
||||
@ -3,9 +3,6 @@
|
||||
<description>The output on the kalman filter on altitude.</description>
|
||||
<field name="Altitude" units="m" type="float" elements="1"/>
|
||||
<field name="Velocity" units="m/s" type="float" elements="1"/>
|
||||
<field name="Accel" units="m/s^2" type="float" elements="1"/>
|
||||
<field name="StateUpdateInterval" units="ms" type="float" elements="1"/>
|
||||
<field name="ThrottleUpdateInterval" units="ms" type="float" elements="1"/>
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
<telemetrygcs acked="false" updatemode="manual" period="0"/>
|
||||
<telemetryflight acked="false" updatemode="periodic" period="1000"/>
|
||||
|
||||
@ -1,14 +1,10 @@
|
||||
<xml>
|
||||
<object name="AltitudeHoldSettings" singleinstance="true" settings="true" category="Control">
|
||||
<description>Settings for the @ref AltitudeHold module</description>
|
||||
<field name="AltitudeKp" units="throttle/m" type="float" elements="1" defaultvalue="0"/>
|
||||
<field name="Altitude2Kp" units="throttle/m^2" type="float" elements="1" defaultvalue="0.08"/>
|
||||
<field name="VelocityKp" units="throttle/(m/s)" type="float" elements="1" defaultvalue="0.1"/>
|
||||
<field name="AltitudeKi" units="throttle/m" type="float" elements="1" defaultvalue="0.1"/>
|
||||
<field name="VelocityKi" units="throttle/m" type="float" elements="1" defaultvalue="0.2"/>
|
||||
<field name="AltitudeKd" units="throttle/m" type="float" elements="1" defaultvalue="0.18"/>
|
||||
<field name="AltitudePID" units="throttle/m" type="float" elementnames="Kp,Ki,Kd" defaultvalue="0.18,0.06,0.01"/>
|
||||
<field name="VelocityPI" units="throttle/m" type="float" elementnames="Kp,Ki" defaultvalue="0.1,0.1"/>
|
||||
<field name="VelocityTau" units="" type="float" elements="1" defaultvalue="0.5"/>
|
||||
<field name="AccelTau" units="" type="float" elements="1" defaultvalue="0.5"/>
|
||||
<field name="PositionTau" units="" type="float" elements="1" defaultvalue="0.5"/>
|
||||
<field name="ThrottleFilterCutoff" units="Hz" type="float" elements="1" defaultvalue="2"/>
|
||||
<field name="ThrottleExp" units="" type="uint8" elements="1" defaultvalue="128"/>
|
||||
<field name="ThrottleRate" units="m/s" type="uint8" elements="1" defaultvalue="5"/>
|
||||
|
||||
Loading…
Reference in New Issue
Block a user