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-1599 add low pass filter to control input in vario mode to possibly reduce rubber-band effect - needs test and tuning

Browse Source
This commit is contained in:
Corvus Corax 2014-11-08 13:32:40 +01:00
parent 7d2ca3d92f
commit db0ba763bf
2 changed files with 19 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
flight/libraries/plans.c
View File

@ -167,11 +167,16 @@ void plan_run_land()
*/ */
static bool vario_hold = true; static bool vario_hold = true;
static float hold_position[3]; static float hold_position[3];
static float vario_course = 0; static float vario_control_lowpass[4];
static float vario_course = 0.0f;
static void plan_setup_PositionVario() static void plan_setup_PositionVario()
{ {
vario_hold = true; vario_hold = true;
vario_control_lowpass[0] = 0.0f;
vario_control_lowpass[1] = 0.0f;
vario_control_lowpass[2] = 0.0f;
vario_control_lowpass[3] = 0.0f;
AttitudeStateYawGet(&vario_course); AttitudeStateYawGet(&vario_course);
plan_setup_positionHold(); plan_setup_positionHold();
} }
@ -293,6 +298,7 @@ static void getVector(float controlVector[4], vario_type type)
static void plan_run_PositionVario(vario_type type) static void plan_run_PositionVario(vario_type type)
{ {
float controlVector[4]; float controlVector[4];
float alpha;
PathDesiredData pathDesired; PathDesiredData pathDesired;
PathDesiredGet(&pathDesired); PathDesiredGet(&pathDesired);
@ -305,6 +311,17 @@ static void plan_run_PositionVario(vario_type type)
ManualControlCommandYawGet(&controlVector[2]); ManualControlCommandYawGet(&controlVector[2]);
ManualControlCommandThrustGet(&controlVector[3]); ManualControlCommandThrustGet(&controlVector[3]);
FlightModeSettingsVarioControlLowPassAlphaGet(&alpha);
vario_control_lowpass[0] = alpha * vario_control_lowpass[0] + (1.0f - alpha) * controlVector[0];
vario_control_lowpass[1] = alpha * vario_control_lowpass[1] + (1.0f - alpha) * controlVector[1];
vario_control_lowpass[2] = alpha * vario_control_lowpass[2] + (1.0f - alpha) * controlVector[2];
vario_control_lowpass[3] = alpha * vario_control_lowpass[3] + (1.0f - alpha) * controlVector[3];
controlVector[0] = vario_control_lowpass[0];
controlVector[1] = vario_control_lowpass[1];
controlVector[2] = vario_control_lowpass[2];
controlVector[3] = vario_control_lowpass[3];
// check if movement is desired // check if movement is desired
if (normalizeDeadband(controlVector) == false) { if (normalizeDeadband(controlVector) == false) {
// no movement desired, re-enter positionHold at current start-position // no movement desired, re-enter positionHold at current start-position

1
shared/uavobjectdefinition/flightmodesettings.xml
View File

@ -114,6 +114,7 @@
<field name="PositionHoldOffset" units="m" type="float" elementnames="Horizontal,Vertical" defaultvalue="10,2"/> <field name="PositionHoldOffset" units="m" type="float" elementnames="Horizontal,Vertical" defaultvalue="10,2"/>
<!-- optimized for current vtolpathfollower, <!-- optimized for current vtolpathfollower,
for fixed wing pathfollower set to Horizontal=500,Vertical=5 --> for fixed wing pathfollower set to Horizontal=500,Vertical=5 -->
<field name="VarioControlLowPassAlpha" units="" type="float" elements="1" defaultvalue="0.9"/>
<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"/>