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

OP-1022 add position lowpass, rework setting uavo, cleanup

Browse Source
This commit is contained in:
Alessio Morale 2013-07-19 11:28:48 +00:00
parent e2a7c6cb25
commit 6b27ff1e27
4 changed files with 21 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
flight/modules/AltitudeHold/altitudehold.c
View File

@ -66,7 +66,7 @@
#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) #define TASK_PRIORITY (tskIDLE_PRIORITY + 1)
#define ACCEL_DOWNSAMPLE 4 #define ACCEL_DOWNSAMPLE 4
#define TIMEOUT_TRESHOLD 200000 #define TIMEOUT_TRESHOLD 200000
#define DESIRED_UPDATE_RATE_MS 20 // milliseconds #define DESIRED_UPDATE_RATE_MS 100 // milliseconds
// Private types // Private types
// Private variables // Private variables
@ -113,7 +113,7 @@ int32_t AltitudeHoldInitialize()
MODULE_INITCALL(AltitudeHoldInitialize, AltitudeHoldStart); MODULE_INITCALL(AltitudeHoldInitialize, AltitudeHoldStart);
float tau; float tau;
float accelAlpha; float positionAlpha;
float velAlpha; float velAlpha;
bool running = false; bool running = false;
@ -137,14 +137,13 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
VelocityStateData velocityData; VelocityStateData velocityData;
float dT; float dT;
float fblimit = 0; float fblimit = 0;
float lastVertVelocity;
portTickType thisTime, lastUpdateTime; portTickType thisTime, lastUpdateTime;
UAVObjEvent ev; UAVObjEvent ev;
dT = 0; dT = 0;
lastVertVelocity = 0;
timeval = 0; timeval = 0;
lastUpdateTime = 0; lastUpdateTime = 0;
// Force update of the settings // Force update of the settings
SettingsUpdatedCb(&ev); SettingsUpdatedCb(&ev);
// Failsafe handling // Failsafe handling
@ -215,15 +214,10 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
VelocityStateGet(&velocityData); VelocityStateGet(&velocityData);
altHold.Velocity = -(velAlpha * altHold.Velocity + (1 - velAlpha) * velocityData.Down); altHold.Velocity = -(velAlpha * altHold.Velocity + (1 - velAlpha) * velocityData.Down);
float vertAccel = (velocityData.Down - lastVertVelocity) / dT;
lastVertVelocity = velocityData.Down; float position;
PositionStateDownGet(&position);
altHold.Accel = -(accelAlpha * altHold.Accel + (1 - accelAlpha) * vertAccel); altHold.Altitude = -(positionAlpha * position) + (1 - positionAlpha) * altHold.Altitude;
altHold.StateUpdateInterval = dT;
PositionStateDownGet(&altHold.Altitude);
altHold.Altitude = -altHold.Altitude;
AltHoldSmoothedSet(&altHold); AltHoldSmoothedSet(&altHold);
@ -246,8 +240,8 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
velError = altitudeHoldDesired.Velocity - altHold.Velocity; velError = altitudeHoldDesired.Velocity - altHold.Velocity;
// Compute altitude and velocity integral // Compute altitude and velocity integral
altitudeIntegral += (error - fblimit) * altitudeHoldSettings.AltitudeKi * dT; altitudeIntegral += (error - fblimit) * altitudeHoldSettings.AltitudePID[ALTITUDEHOLDSETTINGS_ALTITUDEPID_KI] * dT;
velocityIntegral += (velError - fblimit) * altitudeHoldSettings.VelocityKi * dT; velocityIntegral += (velError - fblimit) * altitudeHoldSettings.VelocityPI[ALTITUDEHOLDSETTINGS_VELOCITYPI_KI] * dT;
thisTime = xTaskGetTickCount(); thisTime = xTaskGetTickCount();
@ -255,17 +249,15 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
if ((thisTime - lastUpdateTime) * 1000 / configTICK_RATE_HZ < DESIRED_UPDATE_RATE_MS) { if ((thisTime - lastUpdateTime) * 1000 / configTICK_RATE_HZ < DESIRED_UPDATE_RATE_MS) {
continue; continue;
} }
altHold.ThrottleUpdateInterval = thisTime - lastUpdateTime;
lastUpdateTime = thisTime; lastUpdateTime = thisTime;
// Instead of explicit limit on integral you output limit feedback // Instead of explicit limit on integral you output limit feedback
StabilizationDesiredGet(&stabilizationDesired); StabilizationDesiredGet(&stabilizationDesired);
if (!enterFailSafe) { if (!enterFailSafe) {
stabilizationDesired.Throttle = altitudeIntegral + velocityIntegral stabilizationDesired.Throttle = altitudeIntegral + velocityIntegral
+ error * fabsf(error) * altitudeHoldSettings.Altitude2Kp + error * altitudeHoldSettings.AltitudePID[ALTITUDEHOLDSETTINGS_ALTITUDEPID_KP]
+ error * altitudeHoldSettings.AltitudeKp + velError * altitudeHoldSettings.VelocityPI[ALTITUDEHOLDSETTINGS_VELOCITYPI_KP]
+ velError * altitudeHoldSettings.VelocityKp + derivative * altitudeHoldSettings.AltitudePID[ALTITUDEHOLDSETTINGS_ALTITUDEPID_KD];
+ derivative * altitudeHoldSettings.AltitudeKd;
// scale up throttle to compensate for roll/pitch angle but limit this to 60 deg (cos(60) == 0.5) to prevent excessive scaling // 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; AttitudeStateData attitudeState;
@ -312,8 +304,8 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{ {
AltitudeHoldSettingsGet(&altitudeHoldSettings); 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); 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) // don't use throttle filter if specified cutoff frequency is too low or above nyquist criteria (half the sampling frequency)
if (altitudeHoldSettings.ThrottleFilterCutoff > 0.001f && altitudeHoldSettings.ThrottleFilterCutoff < 2000.0f / DESIRED_UPDATE_RATE_MS) { if (altitudeHoldSettings.ThrottleFilterCutoff > 0.001f && altitudeHoldSettings.ThrottleFilterCutoff < 2000.0f / DESIRED_UPDATE_RATE_MS) {

8
flight/modules/ManualControl/manualcontrol.c
View File

@ -861,16 +861,16 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed)
} else if (cmd->Throttle > DEADBAND_HIGH && zeroed) { } else if (cmd->Throttle > DEADBAND_HIGH && zeroed) {
// being the two band symmetrical I can divide by DEADBAND_LOW to scale it to a value betweeon 0 and 1 // 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 // then apply an "exp" f(x,k) = (k*x*x*x + (255-k)*x) / 255
altitudeHoldDesiredData.Velocity = (throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - throttleExp) * (cmd->Throttle - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * throttleRate; altitudeHoldDesiredData.Velocity = (throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - throttleExp) * (cmd->Throttle - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * throttleRate;
altitudeHoldDesiredData.Altitude = altHold.Altitude; altitudeHoldDesiredData.Altitude = altHold.Altitude;
} else if (cmd->Throttle < DEADBAND_LOW && zeroed) { } else if (cmd->Throttle < DEADBAND_LOW && zeroed) {
altitudeHoldDesiredData.Velocity = -(throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (255 - throttleExp) * (DEADBAND_LOW - cmd->Throttle) / DEADBAND_LOW) / 255 * throttleRate; altitudeHoldDesiredData.Velocity = -(throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (255 - throttleExp) * (DEADBAND_LOW - cmd->Throttle) / DEADBAND_LOW) / 255 * throttleRate;
altitudeHoldDesiredData.Altitude = altHold.Altitude; altitudeHoldDesiredData.Altitude = altHold.Altitude;
} else if (cmd->Throttle >= DEADBAND_LOW && cmd->Throttle <= DEADBAND_HIGH && (throttleRate != 0)) { } 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 // Require the stick to enter the dead band before they can move height
// Vario is not "engaged" when throttleRate == 0 // Vario is not "engaged" when throttleRate == 0
if(fabsf(altitudeHoldDesiredData.Velocity)> 1e-3f) if (fabsf(altitudeHoldDesiredData.Velocity) > 1e-3f) {
{; ;
altitudeHoldDesiredData.Velocity = 0; altitudeHoldDesiredData.Velocity = 0;
altitudeHoldDesiredData.Altitude = altHold.Altitude; altitudeHoldDesiredData.Altitude = altHold.Altitude;
} }

3
shared/uavobjectdefinition/altholdsmoothed.xml
View File

@ -3,9 +3,6 @@
<description>The output on the kalman filter on altitude.</description> <description>The output on the kalman filter on altitude.</description>
<field name="Altitude" units="m" type="float" elements="1"/> <field name="Altitude" units="m" type="float" elements="1"/>
<field name="Velocity" units="m/s" 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"/> <access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/> <telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/> <telemetryflight acked="false" updatemode="periodic" period="1000"/>

10
shared/uavobjectdefinition/altitudeholdsettings.xml
View File

@ -1,14 +1,10 @@
<xml> <xml>
<object name="AltitudeHoldSettings" singleinstance="true" settings="true" category="Control"> <object name="AltitudeHoldSettings" singleinstance="true" settings="true" category="Control">
<description>Settings for the @ref AltitudeHold module</description> <description>Settings for the @ref AltitudeHold module</description>
<field name="AltitudeKp" units="throttle/m" type="float" elements="1" defaultvalue="0"/> <field name="AltitudePID" units="throttle/m" type="float" elementnames="Kp,Ki,Kd" defaultvalue="0.18,0.06,0.01"/>
<field name="Altitude2Kp" units="throttle/m^2" type="float" elements="1" defaultvalue="0.08"/> <field name="VelocityPI" units="throttle/m" type="float" elementnames="Kp,Ki" defaultvalue="0.1,0.1"/>
<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="VelocityTau" units="" type="float" elements="1" defaultvalue="0.5"/> <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="ThrottleFilterCutoff" units="Hz" type="float" elements="1" defaultvalue="2"/>
<field name="ThrottleExp" units="" type="uint8" elements="1" defaultvalue="128"/> <field name="ThrottleExp" units="" type="uint8" elements="1" defaultvalue="128"/>
<field name="ThrottleRate" units="m/s" type="uint8" elements="1" defaultvalue="5"/> <field name="ThrottleRate" units="m/s" type="uint8" elements="1" defaultvalue="5"/>