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

Add a configurable lowpass filter to smooth throttle commands in

Browse Source 
AltitudeHold mode.

Conflicts:
	flight/modules/AltitudeHold/altitudehold.c
	shared/uavobjectdefinition/altitudeholdsettings.xml
This commit is contained in:
Werner Backes 2013-07-16 14:08:59 +02:00 committed by Alessio Morale
parent 7708aab313
commit 45cebda628
2 changed files with 20 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -66,12 +66,15 @@
#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
// Private types // Private types
// Private variables // Private variables
static xTaskHandle altitudeHoldTaskHandle; static xTaskHandle altitudeHoldTaskHandle;
static xQueueHandle queue; static xQueueHandle queue;
static AltitudeHoldSettingsData altitudeHoldSettings; static AltitudeHoldSettingsData altitudeHoldSettings;
static float throttleAlpha = 1.0f;
static float throttle_old = 0.0f;
// Private functions // Private functions
static void altitudeHoldTask(void *parameters); static void altitudeHoldTask(void *parameters);
@ -119,6 +122,9 @@ float accelAlpha;
float velAlpha; float velAlpha;
bool running = false; bool running = false;
float error; float error;
float switchThrottle;
float smoothed_altitude;
float starting_altitude;
float velError; float velError;
uint32_t timeval; uint32_t timeval;
bool posUpdated; bool posUpdated;
@ -241,7 +247,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
thisTime = xTaskGetTickCount(); thisTime = xTaskGetTickCount();
// Only update stabilizationDesired less frequently // Only update stabilizationDesired less frequently
if ((thisTime - lastUpdateTime) < 20) { if ((thisTime - lastUpdateTime)*1000/configTICK_RATE_HZ < DESIRED_UPDATE_RATE_MS) {
continue; continue;
} }
altHold.ThrottleUpdateInterval = thisTime - lastUpdateTime; altHold.ThrottleUpdateInterval = thisTime - lastUpdateTime;
@ -265,6 +271,8 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
Quaternion2R(q, Rbe); Quaternion2R(q, Rbe);
float throttlescale = Rbe[2][2] < 0.5f ? 0.5f : Rbe[2][2]; float throttlescale = Rbe[2][2] < 0.5f ? 0.5f : Rbe[2][2];
stabilizationDesired.Throttle /= throttlescale; stabilizationDesired.Throttle /= throttlescale;
stabilizationDesired.Throttle = stabilizationDesired.Throttle * throttleAlpha + throttle_old * (1.0f - throttleAlpha);
throttle_old = stabilizationDesired.Throttle;
fblimit = 0; fblimit = 0;
if (stabilizationDesired.Throttle > 1) { if (stabilizationDesired.Throttle > 1) {
@ -299,4 +307,14 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
AltitudeHoldSettingsGet(&altitudeHoldSettings); AltitudeHoldSettingsGet(&altitudeHoldSettings);
accelAlpha = expf(-(1000.0f / 666.0f * ACCEL_DOWNSAMPLE) / altitudeHoldSettings.AccelTau); accelAlpha = expf(-(1000.0f / 666.0f * ACCEL_DOWNSAMPLE) / altitudeHoldSettings.AccelTau);
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)
if (altitudeHoldSettings.ThrottleFilterCutoff > 0.001f && altitudeHoldSettings.ThrottleFilterCutoff < 2000.0f/DESIRED_UPDATE_RATE_MS)
{
throttleAlpha = (float)DESIRED_UPDATE_RATE_MS/((float)DESIRED_UPDATE_RATE_MS + 1000.0f/(2.0f*M_PI_F*altitudeHoldSettings.ThrottleFilterCutoff));
}
else
{
throttleAlpha = 1.0f;
}
} }

1
shared/uavobjectdefinition/altitudeholdsettings.xml
View File

@ -10,6 +10,7 @@
<field name="Ka" units="throttle/(m/s^2)" type="float" elements="1" defaultvalue="0.005"/> <field name="Ka" units="throttle/(m/s^2)" type="float" elements="1" defaultvalue="0.005"/>
<field name="VelocityTau" units="" type="float" elements="1" defaultvalue="0.05"/> <field name="VelocityTau" units="" type="float" elements="1" defaultvalue="0.05"/>
<field name="AccelTau" units="" type="float" elements="1" defaultvalue="0.05"/> <field name="AccelTau" units="" type="float" elements="1" defaultvalue="0.05"/>
<field name="ThrottleFilterCutoff" units="Hz" type="float" elements="1" defaultvalue="5"/>
<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"/>
<access gcs="readwrite" flight="readwrite"/> <access gcs="readwrite" flight="readwrite"/>