1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00

Added vertical velocity as a control loop input to improve dynamics during

commanded ascend/decent.

Conflicts:
	flight/modules/AltitudeHold/altitudehold.c
	flight/modules/ManualControl/manualcontrol.c
	shared/uavobjectdefinition/altitudeholdsettings.xml
This commit is contained in:
Werner Backes 2013-07-11 18:17:00 +02:00 committed by Alessio Morale
parent ed233efde2
commit 7708aab313
5 changed files with 57 additions and 55 deletions

View File

@ -64,7 +64,7 @@
#define MAX_QUEUE_SIZE 2
#define STACK_SIZE_BYTES 1024
#define TASK_PRIORITY (tskIDLE_PRIORITY + 1)
#define ACCEL_DOWNSAMPLE 10
#define ACCEL_DOWNSAMPLE 4
#define TIMEOUT_TRESHOLD 200000
// Private types
@ -109,8 +109,11 @@ int32_t AltitudeHoldInitialize()
}
MODULE_INITCALL(AltitudeHoldInitialize, AltitudeHoldStart);
float throttleIntegral;
float switchThrottle;
float tau;
float altitudeIntegral;
float velocityIntegral;
float decay;
float velocity_decay;
float velocity;
float accelAlpha;
float velAlpha;
@ -131,7 +134,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
AttitudeStateData attitudeState;
VelocityStateData velocityData;
float dT;
float q[4], Rbe[3][3];
float q[4], Rbe[3][3], fblimit = 0;
float lastVertVelocity;
portTickType thisTime, lastUpdateTime;
UAVObjEvent ev;
@ -164,7 +167,7 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
// Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe
if (xQueueReceive(queue, &ev, 100 / portTICK_RATE_MS) != pdTRUE) {
if (!running) {
throttleIntegral = 0;
altitudeIntegral = 0;
}
// Todo: Add alarm if it should be running
@ -180,17 +183,19 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
q[3] = attitudeState.q4;
Quaternion2R(q, Rbe);
// Copy the current throttle as a starting point for integral
StabilizationDesiredThrottleGet(&throttleIntegral);
switchThrottle = throttleIntegral;
throttleIntegral *= Rbe[2][2]; // rotate into earth frame
if (throttleIntegral > 1) {
throttleIntegral = 1;
} else if (throttleIntegral < 0) {
throttleIntegral = 0;
float initThrottle;
StabilizationDesiredThrottleGet(&initThrottle);
initThrottle *= Rbe[2][2]; // rotate into earth frame
if (initThrottle > 1) {
initThrottle = 1;
} else if (initThrottle < 0) {
initThrottle = 0;
}
error = 0;
velocity = 0;
altitudeHoldDesired.Velocity = 0;
altitudeHoldDesired.Altitude = altHold.Altitude;
altitudeIntegral = altHold.Altitude * altitudeHoldSettings.Kp + initThrottle;
velocityIntegral = 0;
running = true;
} else if (!altitudeHoldFlightMode) {
running = false;
@ -230,14 +235,10 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
continue;
}
// Compute the altitude error
error = altitudeHoldDesired.Altitude - altHold.Altitude;
velError = altitudeHoldDesired.Velocity - altHold.Velocity;
// Compute altitude and velocity integral
altitudeIntegral += (altitudeHoldDesired.Altitude - altHold.Altitude - fblimit) * altitudeHoldSettings.AltitudeKi * dT;
velocityIntegral += (altitudeHoldDesired.Velocity - altHold.Velocity - fblimit) * altitudeHoldSettings.VelocityKi * dT;
if (fabsf(altitudeHoldDesired.Velocity) < 1e-3f) {
// Compute integral off altitude error
throttleIntegral += error * altitudeHoldSettings.Ki * dT;
}
thisTime = xTaskGetTickCount();
// Only update stabilizationDesired less frequently
if ((thisTime - lastUpdateTime) < 20) {
@ -249,31 +250,28 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
// Instead of explicit limit on integral you output limit feedback
StabilizationDesiredGet(&stabilizationDesired);
if (!enterFailSafe) {
if (fabsf(altitudeHoldDesired.Velocity) < 1e-3f) {
stabilizationDesired.Throttle = error * altitudeHoldSettings.Kp
+ error * fabsf(error) * altitudeHoldSettings.Kp2
+ 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
AttitudeStateGet(&attitudeState);
q[0] = attitudeState.q1;
q[1] = attitudeState.q2;
q[2] = attitudeState.q3;
q[3] = attitudeState.q4;
Quaternion2R(q, Rbe);
float throttlescale = Rbe[2][2] < 0.5f ? 0.5f : Rbe[2][2];
stabilizationDesired.Throttle /= throttlescale;
} else {
stabilizationDesired.Throttle = velError * altitudeHoldSettings.Kv + throttleIntegral;
}
stabilizationDesired.Throttle = altitudeIntegral + velocityIntegral
+ error * fabsf(error) * altitudeHoldSettings.Kp2
- altHold.Altitude * altitudeHoldSettings.Kp
- 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
AttitudeStateGet(&attitudeState);
q[0] = attitudeState.q1;
q[1] = attitudeState.q2;
q[2] = attitudeState.q3;
q[3] = attitudeState.q4;
Quaternion2R(q, Rbe);
float throttlescale = Rbe[2][2] < 0.5f ? 0.5f : Rbe[2][2];
stabilizationDesired.Throttle /= throttlescale;
fblimit = 0;
if (stabilizationDesired.Throttle > 1) {
throttleIntegral -= (stabilizationDesired.Throttle - 1);
fblimit = stabilizationDesired.Throttle - 1;
stabilizationDesired.Throttle = 1;
} else if (stabilizationDesired.Throttle < 0) {
throttleIntegral -= stabilizationDesired.Throttle;
fblimit = stabilizationDesired.Throttle;
stabilizationDesired.Throttle = 0;
}
} else {

View File

@ -827,7 +827,10 @@ 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);
@ -844,8 +847,10 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed)
StabilizationSettingsData stabSettings;
StabilizationSettingsGet(&stabSettings);
AltHoldSmoothedData altHoldSmoothed;
AltHoldSmoothedGet(&altHoldSmoothed);
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;
@ -853,24 +858,22 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed)
if (changed) {
// After not being in this mode for a while init at current height
altitudeHoldDesiredData.Altitude = altHoldSmoothed.Altitude;
altitudeHoldDesiredData.Velocity = 0.0f;
AltHoldSmoothedData altHold;
AltHoldSmoothedGet(&altHold);
altitudeHoldDesiredData.Altitude = altHold.Altitude;
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.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 += altitudeHoldDesiredData.Velocity * dT;
} 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 += altitudeHoldDesiredData.Velocity * dT;
} 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) {
altitudeHoldDesiredData.Altitude = altHoldSmoothed.Altitude;
altitudeHoldDesiredData.Velocity = 0.0f;
}
altitudeHoldDesiredData.Velocity = 0;
zeroed = true;
}

View File

@ -133,7 +133,7 @@ static const struct pios_hmc5883_cfg pios_hmc5883_cfg = {
#if defined(PIOS_INCLUDE_MS5611)
#include "pios_ms5611.h"
static const struct pios_ms5611_cfg pios_ms5611_cfg = {
.oversampling = MS5611_OSR_512,
.oversampling = MS5611_OSR_4096,
};
#endif /* PIOS_INCLUDE_MS5611 */

View File

@ -1,8 +1,8 @@
<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="Velocity" units="m/s" type="float" elements="1"/>
<field name="Roll" units="deg" type="float" elements="1"/>
<field name="Pitch" units="deg" type="float" elements="1"/>
<field name="Yaw" units="deg/s" type="float" elements="1"/>

View File

@ -4,7 +4,8 @@
<field name="Kp" units="throttle/m" type="float" elements="1" defaultvalue="0"/>
<field name="Kp2" units="throttle/m^2" 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="AltitudeKi" units="throttle/m" type="float" elements="1" defaultvalue="0"/>
<field name="VelocityKi" units="throttle/m" type="float" elements="1" defaultvalue="0"/>
<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="VelocityTau" units="" type="float" elements="1" defaultvalue="0.05"/>