rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

OP-1848 altvario

Browse Source 
Speed up velocity control loop to original speed
This commit is contained in:
abeck70 2015-05-05 22:39:15 +10:00
parent 48f3922a8d
commit 0c77d8a21d
1 changed files with 20 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -60,6 +60,7 @@ extern "C" {
// Private types // Private types
// Private variables // Private variables
static DelayedCallbackInfo *altitudeHoldCBInfo;
static PIDControlDown controlDown; static PIDControlDown controlDown;
static AltitudeHoldSettingsData altitudeHoldSettings; static AltitudeHoldSettingsData altitudeHoldSettings;
static ThrustModeType thrustMode; static ThrustModeType thrustMode;
@ -68,6 +69,8 @@ static float thrustDemand = 0.0f;
// Private functions // Private functions
static void SettingsUpdatedCb(UAVObjEvent *ev); static void SettingsUpdatedCb(UAVObjEvent *ev);
static void altitudeHoldTask(void);
static void VelocityStateUpdatedCb(UAVObjEvent *ev);
/** /**
* Setup mode and setpoint * Setup mode and setpoint
@ -113,7 +116,17 @@ float stabilizationAltitudeHold(float setpoint, ThrustModeType mode, bool reinit
newaltitude = false; newaltitude = false;
} }
return thrustDemand;
}
/**
* Module thread, should not return.
*/
static void altitudeHoldTask(void)
{
AltitudeHoldStatusData altitudeHoldStatus; AltitudeHoldStatusData altitudeHoldStatus;
AltitudeHoldStatusGet(&altitudeHoldStatus); AltitudeHoldStatusGet(&altitudeHoldStatus);
float velocityStateDown; float velocityStateDown;
@ -129,8 +142,6 @@ float stabilizationAltitudeHold(float setpoint, ThrustModeType mode, bool reinit
controlDown.ControlPosition(); controlDown.ControlPosition();
altitudeHoldStatus.VelocityDesired = controlDown.GetVelocityDesired(); altitudeHoldStatus.VelocityDesired = controlDown.GetVelocityDesired();
thrustDemand = controlDown.GetDownCommand(); thrustDemand = controlDown.GetDownCommand();
// if thrust demand is high and we are below altitude by 2m, back off pitch
} }
break; break;
@ -145,8 +156,11 @@ float stabilizationAltitudeHold(float setpoint, ThrustModeType mode, bool reinit
} }
AltitudeHoldStatusSet(&altitudeHoldStatus); AltitudeHoldStatusSet(&altitudeHoldStatus);
}
return thrustDemand; static void VelocityStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
PIOS_CALLBACKSCHEDULER_Dispatch(altitudeHoldCBInfo);
} }
/** /**
@ -163,6 +177,9 @@ void stabilizationAltitudeloopInit()
AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb); AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb);
VtolSelfTuningStatsConnectCallback(&SettingsUpdatedCb); VtolSelfTuningStatsConnectCallback(&SettingsUpdatedCb);
SettingsUpdatedCb(NULL); SettingsUpdatedCb(NULL);
altitudeHoldCBInfo = PIOS_CALLBACKSCHEDULER_Create(&altitudeHoldTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_ALTITUDEHOLD, STACK_SIZE_BYTES);
VelocityStateConnectCallback(&VelocityStateUpdatedCb);
} }