rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

OP-1022 Adding a proportional term on the square of the altitude error.

Browse Source
This commit is contained in:
Alessio Morale 2013-07-16 20:01:16 +00:00
parent 5b4d46819e
commit 0203e2c6e2
3 changed files with 12 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -250,6 +250,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
if (!enterFailSafe) { if (!enterFailSafe) {
if(fabsf(altitudeHoldDesired.Velocity) < 1e-3f) { if(fabsf(altitudeHoldDesired.Velocity) < 1e-3f) {
stabilizationDesired.Throttle = error * altitudeHoldSettings.Kp stabilizationDesired.Throttle = error * altitudeHoldSettings.Kp
+ error * fabsf(error) * altitudeHoldSettings.Kp2
+ throttleIntegral + throttleIntegral
- altHold.Velocity * altitudeHoldSettings.Kd - altHold.Velocity * altitudeHoldSettings.Kd
- altHold.Accel * altitudeHoldSettings.Ka; - altHold.Accel * altitudeHoldSettings.Ka;

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

@ -813,7 +813,7 @@ static void updateLandDesired(__attribute__((unused)) ManualControlCommandData *
*/ */
static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed)
{ {
const float DEADBAND = 0.25f; const float DEADBAND = 0.20f;
const float DEADBAND_HIGH = 1.0f / 2 + DEADBAND / 2; const float DEADBAND_HIGH = 1.0f / 2 + DEADBAND / 2;
const float DEADBAND_LOW = 1.0f / 2 - DEADBAND / 2; const float DEADBAND_LOW = 1.0f / 2 - DEADBAND / 2;

19
shared/uavobjectdefinition/altitudeholdsettings.xml
View File

@ -1,15 +1,16 @@
<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="Kp" units="throttle/m" type="float" elements="1" defaultvalue="0.03"/> <field name="Kp" units="throttle/m" type="float" elements="1" defaultvalue="0"/>
<field name="Kv" units="throttle/(m/s)" type="float" elements="1" defaultvalue="0.01"/> <field name="Kp2" units="throttle/m^2" type="float" elements="1" defaultvalue="0.03"/>
<field name="Ki" units="throttle/m" type="float" elements="1" defaultvalue="0"/> <field name="Kv" units="throttle/(m/s)" type="float" elements="1" defaultvalue="0.01"/>
<field name="Kd" units="throttle/m" type="float" elements="1" defaultvalue="0.03"/> <field name="Ki" units="throttle/m" type="float" elements="1" defaultvalue="0"/>
<field name="Ka" units="throttle/(m/s^2)" type="float" elements="1" defaultvalue="0.005"/> <field name="Kd" units="throttle/m" type="float" elements="1" defaultvalue="0.03"/>
<field name="VelocityTau" units="" type="float" elements="1" defaultvalue="0.05"/> <field name="Ka" units="throttle/(m/s^2)" type="float" elements="1" defaultvalue="0.005"/>
<field name="AccelTau" units="" type="float" elements="1" defaultvalue="0.05"/> <field name="VelocityTau" units="" type="float" elements="1" defaultvalue="0.05"/>
<field name="ThrottleExp" units="" type="uint8" elements="1" defaultvalue="128"/> <field name="AccelTau" units="" type="float" elements="1" defaultvalue="0.05"/>
<field name="ThrottleRate" units="m/s" type="uint8" elements="1" defaultvalue="5"/> <field name="ThrottleExp" units="" type="uint8" elements="1" defaultvalue="128"/>
<field name="ThrottleRate" units="m/s" type="uint8" elements="1" defaultvalue="5"/>
<access gcs="readwrite" flight="readwrite"/> <access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/> <telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/> <telemetryflight acked="true" updatemode="onchange" period="0"/>