rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-27 16:54:15 +01:00
Code Issues Releases Activity

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

Browse Source 
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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

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

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

@ -827,7 +827,10 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed)
uint8_t throttleExp; uint8_t throttleExp;
static uint8_t flightMode; static uint8_t flightMode;
static portTickType lastSysTimeAH;
static bool zeroed = false; static bool zeroed = false;
portTickType thisSysTime;
float dT;
FlightStatusFlightModeGet(&flightMode); FlightStatusFlightModeGet(&flightMode);
@ -844,8 +847,10 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed)
StabilizationSettingsData stabSettings; StabilizationSettingsData stabSettings;
StabilizationSettingsGet(&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.Roll = cmd->Roll * stabSettings.RollMax;
altitudeHoldDesiredData.Pitch = cmd->Pitch * stabSettings.PitchMax; altitudeHoldDesiredData.Pitch = cmd->Pitch * stabSettings.PitchMax;
@ -853,24 +858,22 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed)
if (changed) { if (changed) {
// After not being in this mode for a while init at current height // After not being in this mode for a while init at current height
AltHoldSmoothedData altHold;
altitudeHoldDesiredData.Altitude = altHoldSmoothed.Altitude; AltHoldSmoothedGet(&altHold);
altitudeHoldDesiredData.Velocity = 0.0f; altitudeHoldDesiredData.Altitude = altHold.Altitude;
zeroed = false; zeroed = false;
} else if (cmd->Throttle > DEADBAND_HIGH && zeroed) { } 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 // 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 // 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) { } 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)) { } 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 // Require the stick to enter the dead band before they can move height
// Vario is not "engaged" when throttleRate == 0 // Vario is not "engaged" when throttleRate == 0
if (fabsf(altitudeHoldDesiredData.Velocity) > 1e-3f) { altitudeHoldDesiredData.Velocity = 0;
altitudeHoldDesiredData.Altitude = altHoldSmoothed.Altitude;
altitudeHoldDesiredData.Velocity = 0.0f;
}
zeroed = true; zeroed = true;
} }

2
flight/targets/boards/revolution/firmware/pios_board.c
View File

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

2
shared/uavobjectdefinition/altitudeholddesired.xml
View File

@ -1,8 +1,8 @@
<xml> <xml>
<object name="AltitudeHoldDesired" singleinstance="true" settings="false" category="Control"> <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> <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="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="Roll" units="deg" type="float" elements="1"/>
<field name="Pitch" 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"/> <field name="Yaw" units="deg/s" type="float" elements="1"/>

3
shared/uavobjectdefinition/altitudeholdsettings.xml
View File

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