rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-18 03:52:11 +01:00
Code Issues Releases Activity

OP-1022: Added a velocity loop in AH module, used when throttle is not in central position.

Browse Source
This commit is contained in:
Alessio Morale 2013-07-14 07:46:42 +00:00
parent 23b5b37480
commit 133ad414f8
4 changed files with 30 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -114,6 +114,8 @@ float decay;
float velocity_decay;
bool running = false;
float error;
float velError;
float switchThrottle;
float smoothed_altitude;
float starting_altitude;
@ -364,9 +366,12 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
// Compute the altitude error
error = altitudeHoldDesired.Altitude - altHold.Altitude;
velError = altitudeHoldDesired.Velocity - altHold.Velocity;
// Compute integral off altitude error
throttleIntegral += error * altitudeHoldSettings.Ki * dT;
if(fabsf(altitudeHoldDesired.Velocity) < 1e-3f) {
// Compute integral off altitude error
throttleIntegral += error * altitudeHoldSettings.Ki * dT;
}
// Only update stabilizationDesired less frequently
if ((thisTime - lastUpdateTime) < 20) {
@ -378,12 +383,15 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
// Instead of explicit limit on integral you output limit feedback
StabilizationDesiredGet(&stabilizationDesired);
if (!enterFailSafe) {
stabilizationDesired.Throttle = error * altitudeHoldSettings.Kp + throttleIntegral -
altHold.Velocity * altitudeHoldSettings.Kd - altHold.Accel * altitudeHoldSettings.Ka;
// scale up throttle to compensate for roll/pitch angle but limit this to 60 deg (cos(60) == 0.5) to prevent excessive scaling
float throttlescale = Rbe[2][2] < 0.5f ? 0.5f : Rbe[2][2];
stabilizationDesired.Throttle /= throttlescale;
if(fabsf(altitudeHoldDesired.Velocity) < 1e-3f) {
stabilizationDesired.Throttle = error * altitudeHoldSettings.Kp + throttleIntegral -
altHold.Velocity * altitudeHoldSettings.Kd - altHold.Accel * altitudeHoldSettings.Ka;
// scale up throttle to compensate for roll/pitch angle but limit this to 60 deg (cos(60) == 0.5) to prevent excessive scaling
float throttlescale = Rbe[2][2] < 0.5f ? 0.5f : Rbe[2][2];
stabilizationDesired.Throttle /= throttlescale;
} else {
stabilizationDesired.Throttle = velError * altitudeHoldSettings.Kv + throttleIntegral;
}
if (stabilizationDesired.Throttle > 1) {
throttleIntegral -= (stabilizationDesired.Throttle - 1);
stabilizationDesired.Throttle = 1;

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

@ -827,10 +827,7 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed)
uint8_t throttleExp;
static uint8_t flightMode;
static portTickType lastSysTimeAH;
static bool zeroed = false;
portTickType thisSysTime;
float dT;
FlightStatusFlightModeGet(&flightMode);
@ -848,29 +845,30 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed)
StabilizationSettingsData stabSettings;
StabilizationSettingsGet(&stabSettings);
thisSysTime = xTaskGetTickCount();
dT = ((thisSysTime == lastSysTimeAH) ? 0.001f : (thisSysTime - lastSysTimeAH) * portTICK_RATE_MS * 0.001f);
lastSysTimeAH = thisSysTime;
altitudeHoldDesiredData.Roll = cmd->Roll * stabSettings.RollMax;
altitudeHoldDesiredData.Pitch = cmd->Pitch * stabSettings.PitchMax;
altitudeHoldDesiredData.Yaw = cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW];
if (changed) {
// After not being in this mode for a while init at current height
AltHoldSmoothedData altHold;
AltHoldSmoothedGet(&altHold);
altitudeHoldDesiredData.Altitude = altHold.Altitude;
AltHoldSmoothedAltitudeGet(&altitudeHoldDesiredData.Altitude);
altitudeHoldDesiredData.Velocity = 0.0f;
zeroed = false;
} 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
// then apply an "exp" f(x,k) = (k*x*x*x + (255-k)*x) / 255
altitudeHoldDesiredData.Altitude += (throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - throttleExp) * (cmd->Throttle - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * throttleRate * dT;
altitudeHoldDesiredData.Velocity = ((throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - throttleExp) * (cmd->Throttle - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * throttleRate);
} else if (cmd->Throttle < DEADBAND_LOW && zeroed) {
altitudeHoldDesiredData.Altitude -= (throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (255 - throttleExp) * (DEADBAND_LOW - cmd->Throttle) / DEADBAND_LOW) / 255 * throttleRate * dT;
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);
} 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) {
AltHoldSmoothedAltitudeGet(&altitudeHoldDesiredData.Altitude);
altitudeHoldDesiredData.Velocity = 0.0f;
}
zeroed = true;
}

1
shared/uavobjectdefinition/altitudeholddesired.xml
View File

@ -1,6 +1,7 @@
<xml>
<object name="AltitudeHoldDesired" singleinstance="true" settings="false" category="Control">
<description>Holds the desired altitude (from manual control) as well as the desired attitude to pass through</description>
<field name="Velocity" units="m/s" type ="float" elements="1"/>
<field name="Altitude" units="m" type="float" elements="1"/>
<field name="Roll" units="deg" type="float" elements="1"/>
<field name="Pitch" units="deg" type="float" elements="1"/>

5
shared/uavobjectdefinition/altitudeholdsettings.xml
View File

@ -1,9 +1,10 @@
<xml>
<object name="AltitudeHoldSettings" singleinstance="true" settings="true" category="Control">
<description>Settings for the @ref AltitudeHold module</description>
<field name="Kp" units="throttle/m" type="float" elements="1" defaultvalue="0.0001"/>
<field name="Kp" units="throttle/m" type="float" elements="1" defaultvalue="0.03"/>
<field name="Kv" units="throttle/(m/s)" type="float" elements="1" defaultvalue="0.01"/>
<field name="Ki" units="throttle/m" type="float" elements="1" defaultvalue="0"/>
<field name="Kd" units="throttle/m" type="float" elements="1" defaultvalue="0.05"/>
<field name="Kd" units="throttle/m" type="float" elements="1" defaultvalue="0.03"/>
<field name="Ka" units="throttle/(m/s^2)" type="float" elements="1" defaultvalue="0.005"/>
<field name="PressureNoise" units="m" type="float" elements="1" defaultvalue="0.4"/>
<field name="AccelNoise" units="m/s^2" type="float" elements="1" defaultvalue="5"/>