mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-01 09:24:10 +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
Alessio Morale
parent
ed233efde2
commit
7708aab313
@ -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 {
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
|
||||
@ -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 */
|
||||
|
||||
|
||||
@ -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"/>
|
||||
|
||||
@ -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"/>
|
||||
|
||||
Loading…
Reference in New Issue
Block a user