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-1848 altvario improvements

Browse Source 
1. Disable velocity pid loop when altvario is not active
2. Add sanity boundf to ensure max min are inforced.
This commit is contained in:
abeck70 2015-05-12 17:40:28 +10:00
parent 43ec464737
commit 8d2f4722d7
5 changed files with 32 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
flight/libraries/pid/pidcontroldown.h
View File

@ -44,6 +44,10 @@ public:
void SetThrustLimits(float min_thrust, float max_thrust);
void Deactivate();
void Activate();
bool IsActive()
{
return mActive;
}
void UpdateParameters(float kp, float ki, float kd, float beta, float dT, float velocityMax);
void UpdateNeutralThrust(float neutral);
void UpdateVelocitySetpoint(float setpoint);

25
flight/modules/Stabilization/altitudeloop.cpp
View File

@ -81,7 +81,7 @@ float stabilizationAltitudeHold(float setpoint, ThrustModeType mode, bool reinit
{
static bool newaltitude = true;
if (reinit) {
if (reinit || !controlDown.IsActive()) {
controlDown.Activate();
newaltitude = true;
}
@ -97,6 +97,7 @@ float stabilizationAltitudeHold(float setpoint, ThrustModeType mode, bool reinit
thrustDemand = 0.0f;
thrustMode = DIRECT;
newaltitude = true;
return thrustDemand;
} else if (mode == ALTITUDEVARIO && setpoint > DEADBAND_HIGH) {
// 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
@ -116,23 +117,38 @@ float stabilizationAltitudeHold(float setpoint, ThrustModeType mode, bool reinit
newaltitude = false;
}
thrustDemand = boundf(thrustDemand, altitudeHoldSettings.ThrustLimits.Min, altitudeHoldSettings.ThrustLimits.Max);
return thrustDemand;
}
/**
* Disable the alt hold task loop velocity controller to save cpu cycles
*/
void stabilizationDisableAltitudeHold(void)
{
controlDown.Deactivate();
}
/**
* Module thread, should not return.
*/
static void altitudeHoldTask(void)
{
AltitudeHoldStatusData altitudeHoldStatus;
if (!controlDown.IsActive()) {
return;
}
AltitudeHoldStatusData altitudeHoldStatus;
AltitudeHoldStatusGet(&altitudeHoldStatus);
float velocityStateDown;
VelocityStateDownGet(&velocityStateDown);
controlDown.UpdateVelocityState(velocityStateDown);
float local_thrustDemand = 0.0f;
switch (thrustMode) {
case ALTITUDEHOLD:
{
@ -142,14 +158,14 @@ static void altitudeHoldTask(void)
controlDown.ControlPosition();
altitudeHoldStatus.VelocityDesired = controlDown.GetVelocityDesired();
altitudeHoldStatus.State = ALTITUDEHOLDSTATUS_STATE_ALTITUDEHOLD;
thrustDemand = controlDown.GetDownCommand();
local_thrustDemand = controlDown.GetDownCommand();
}
break;
case ALTITUDEVARIO:
altitudeHoldStatus.VelocityDesired = controlDown.GetVelocityDesired();
altitudeHoldStatus.State = ALTITUDEHOLDSTATUS_STATE_ALTITUDEVARIO;
thrustDemand = controlDown.GetDownCommand();
local_thrustDemand = controlDown.GetDownCommand();
break;
case DIRECT:
@ -159,6 +175,7 @@ static void altitudeHoldTask(void)
}
AltitudeHoldStatusSet(&altitudeHoldStatus);
thrustDemand = local_thrustDemand;
}
static void VelocityStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)

1
flight/modules/Stabilization/inc/altitudeloop.h
View File

@ -37,5 +37,6 @@ typedef enum { ALTITUDEHOLD = 0, ALTITUDEVARIO = 1, DIRECT = 2 } ThrustModeType;
void stabilizationAltitudeloopInit();
float stabilizationAltitudeHold(float setpoint, ThrustModeType mode, bool reinit);
void stabilizationDisableAltitudeHold(void);
#endif /* ALTITUDELOOP_H */

5
flight/modules/Stabilization/outerloop.c
View File

@ -107,6 +107,11 @@ static void stabilizationOuterloopTask()
bool reinit = (StabilizationStatusOuterLoopToArray(enabled)[STABILIZATIONSTATUS_OUTERLOOP_THRUST] != previous_mode[STABILIZATIONSTATUS_OUTERLOOP_THRUST]);
previous_mode[STABILIZATIONSTATUS_OUTERLOOP_THRUST] = StabilizationStatusOuterLoopToArray(enabled)[STABILIZATIONSTATUS_OUTERLOOP_THRUST];
if (reinit && (previous_mode[STABILIZATIONSTATUS_OUTERLOOP_THRUST] == STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE ||
previous_mode[STABILIZATIONSTATUS_OUTERLOOP_THRUST] == STABILIZATIONSTATUS_OUTERLOOP_ALTITUDEVARIO)) {
// disable the altvario velocity control loop
stabilizationDisableAltitudeHold();
}
switch (StabilizationStatusOuterLoopToArray(enabled)[STABILIZATIONSTATUS_OUTERLOOP_THRUST]) {
#ifdef REVOLUTION
case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE:

1
shared/uavobjectdefinition/altitudeholdstatus.xml
View File

@ -3,6 +3,7 @@
<description>Status Data from Altitude Hold Control Loops</description>
<field name="VelocityDesired" units="m/s" type="float" elements="1"/>
<field name="State" units="" type="enum" elements="1" options="Direct,AltitudeVario,AltitudeHold" defaultvalue="Direct"/>
<field name="ThrustDemand" units="" type="float" elements="1"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/>