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

Flight/Stabilization: Use the system clock to determine time from previous

Browse Source 
update since now drive by events.  This means StabilizationSettings object
changing so write down your settings.

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1819 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2010-10-01 12:33:29 +00:00 committed by peabody124
parent 4461aba6d6
commit 2dc40d11a1
7 changed files with 18 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
flight/OpenPilot/Modules/Stabilization/simple/Stabilization/stabilization.c
View File

@ -72,6 +72,7 @@ int32_t StabilizationInitialize()
return 0;
}
float dT = 1;
/**
* Module task
@ -87,6 +88,7 @@ static void stabilizationTask(void* parameters)
ManualControlCommandData manualControl;
SystemSettingsData systemSettings;
portTickType lastSysTime;
portTickType thisSysTime;
float pitchError, pitchErrorLast;
float rollError, rollErrorLast;
float yawError, yawErrorLast;
@ -115,9 +117,14 @@ static void stabilizationTask(void* parameters)
if ( xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE )
{
AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION,SYSTEMALARMS_ALARM_WARNING);
}
// Check how long since last update
thisSysTime = xTaskGetTickCount();
if(thisSysTime > lastSysTime) // reuse dt in case of wraparound
dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;
lastSysTime = thisSysTime;
// Read settings and other objects
StabilizationSettingsGet(&stabSettings);
SystemSettingsGet(&systemSettings);
@ -127,16 +134,16 @@ static void stabilizationTask(void* parameters)
// Pitch stabilization control loop
pitchError = attitudeDesired.Pitch - attitudeActual.Pitch;
pitchDerivative = ((pitchError - pitchErrorLast) * 1000) / stabSettings.UpdatePeriod;
pitchIntegral = bound(pitchIntegral+(pitchError * stabSettings.UpdatePeriod) / 1000, -stabSettings.PitchIntegralLimit, stabSettings.PitchIntegralLimit);
pitchDerivative = (pitchError - pitchErrorLast) / dT;
pitchIntegral = bound(pitchIntegral + pitchError * dT, -stabSettings.PitchIntegralLimit, stabSettings.PitchIntegralLimit);
actuatorDesired.Pitch = stabSettings.PitchKp*pitchError + stabSettings.PitchKi*pitchIntegral + stabSettings.PitchKd*pitchDerivative;
actuatorDesired.Pitch = bound(actuatorDesired.Pitch, -1.0, 1.0);
pitchErrorLast = pitchError;
// Roll stabilization control loop
rollError = attitudeDesired.Roll - attitudeActual.Roll;
rollDerivative = ((rollError - rollErrorLast) * 1000) / stabSettings.UpdatePeriod;
rollIntegral = bound(rollIntegral+(rollError * stabSettings.UpdatePeriod) / 1000, -stabSettings.RollIntegralLimit, stabSettings.RollIntegralLimit);
rollDerivative = (rollError - rollErrorLast) / dT;
rollIntegral = bound(rollIntegral + rollError * dT, -stabSettings.RollIntegralLimit, stabSettings.RollIntegralLimit);
actuatorDesired.Roll = stabSettings.RollKp*rollError + stabSettings.RollKi*rollIntegral + stabSettings.RollKd*rollDerivative;
actuatorDesired.Roll = bound(actuatorDesired.Roll, -1.0, 1.0);
rollErrorLast = rollError;
@ -145,7 +152,7 @@ static void stabilizationTask(void* parameters)
if (( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_VTOL )||( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HELICP))
{
if(stabSettings.YawMode == STABILIZATIONSETTINGS_YAWMODE_RATE) { // rate stabilization on yaw
yawChange = ((attitudeActual.Yaw - yawPrevious) * 1000) / stabSettings.UpdatePeriod;
yawChange = (attitudeActual.Yaw - yawPrevious) / dT;
yawPrevious = attitudeActual.Yaw;
yawError = bound(attitudeDesired.Yaw, -stabSettings.YawMax, stabSettings.YawMax) - yawChange;
} else { // heading stabilization
@ -155,8 +162,8 @@ static void stabilizationTask(void* parameters)
//this should make it take the quickest path to reach the desired yaw
if (yawError>180.0)yawError -= 360;
if (yawError<-180.0)yawError += 360;
yawDerivative = ((yawError - yawErrorLast) * 1000) / stabSettings.UpdatePeriod;
yawIntegral = bound(yawIntegral+(yawError * stabSettings.UpdatePeriod) / 1000, -stabSettings.YawIntegralLimit, stabSettings.YawIntegralLimit);
yawDerivative = (yawError - yawErrorLast) / dT;
yawIntegral = bound(yawIntegral + yawError * dT, -stabSettings.YawIntegralLimit, stabSettings.YawIntegralLimit);
actuatorDesired.Yaw = stabSettings.YawKp*yawError + stabSettings.YawKi*yawIntegral + stabSettings.YawKd*yawDerivative;;
actuatorDesired.Yaw = bound(actuatorDesired.Yaw, -1.0, 1.0);
yawErrorLast = yawError;

4
flight/OpenPilot/UAVObjects/inc/stabilizationsettings.h
View File

@ -41,7 +41,7 @@
#define STABILIZATIONSETTINGS_H
// Object constants
#define STABILIZATIONSETTINGS_OBJID 117768092U
#define STABILIZATIONSETTINGS_OBJID 1346414844U
#define STABILIZATIONSETTINGS_NAME "StabilizationSettings"
#define STABILIZATIONSETTINGS_METANAME "StabilizationSettingsMeta"
#define STABILIZATIONSETTINGS_ISSINGLEINST 1
@ -71,7 +71,6 @@
// Object data
typedef struct {
uint8_t UpdatePeriod;
uint8_t RollMax;
uint8_t PitchMax;
uint8_t YawMax;
@ -94,7 +93,6 @@ typedef struct {
} __attribute__((packed)) StabilizationSettingsData;
// Field information
// Field UpdatePeriod information
// Field RollMax information
// Field PitchMax information
// Field YawMax information

1
flight/OpenPilot/UAVObjects/stabilizationsettings.c
View File

@ -80,7 +80,6 @@ static void setDefaults(UAVObjHandle obj, uint16_t instId)
// Initialize object fields to their default values
UAVObjGetInstanceData(obj, instId, &data);
memset(&data, 0, sizeof(StabilizationSettingsData));
data.UpdatePeriod = 10;
data.RollMax = 35;
data.PitchMax = 35;
data.YawMax = 35;

4
ground/src/plugins/uavobjects/stabilizationsettings.cpp
View File

@ -42,9 +42,6 @@ StabilizationSettings::StabilizationSettings(): UAVDataObject(OBJID, ISSINGLEINS
{
// Create fields
QList<UAVObjectField*> fields;
QStringList UpdatePeriodElemNames;
UpdatePeriodElemNames.append("0");
fields.append( new UAVObjectField(QString("UpdatePeriod"), QString("ms"), UAVObjectField::UINT8, UpdatePeriodElemNames, QStringList()) );
QStringList RollMaxElemNames;
RollMaxElemNames.append("0");
fields.append( new UAVObjectField(QString("RollMax"), QString("degrees"), UAVObjectField::UINT8, RollMaxElemNames, QStringList()) );
@ -135,7 +132,6 @@ UAVObject::Metadata StabilizationSettings::getDefaultMetadata()
*/
void StabilizationSettings::setDefaultFieldValues()
{
data.UpdatePeriod = 10;
data.RollMax = 35;
data.PitchMax = 35;
data.YawMax = 35;

4
ground/src/plugins/uavobjects/stabilizationsettings.h
View File

@ -43,7 +43,6 @@ class UAVOBJECTS_EXPORT StabilizationSettings: public UAVDataObject
public:
// Field structure
typedef struct {
quint8 UpdatePeriod;
quint8 RollMax;
quint8 PitchMax;
quint8 YawMax;
@ -66,7 +65,6 @@ public:
} __attribute__((packed)) DataFields;
// Field information
// Field UpdatePeriod information
// Field RollMax information
// Field PitchMax information
// Field YawMax information
@ -90,7 +88,7 @@ public:
// Constants
static const quint32 OBJID = 117768092U;
static const quint32 OBJID = 1346414844U;
static const QString NAME;
static const bool ISSINGLEINST = 1;
static const bool ISSETTINGS = 1;

12
ground/src/plugins/uavobjects/stabilizationsettings.py
View File

@ -37,16 +37,6 @@ from collections import namedtuple
# This is a list of instances of the data fields contained in this object
_fields = [ \
uavobject.UAVObjectField(
'UpdatePeriod',
'B',
1,
[
'0',
],
{
}
),
uavobject.UAVObjectField(
'RollMax',
'B',
@ -234,7 +224,7 @@ _fields = [ \
class StabilizationSettings(uavobject.UAVObject):
## Object constants
OBJID = 117768092
OBJID = 1346414844
NAME = "StabilizationSettings"
METANAME = "StabilizationSettingsMeta"
ISSINGLEINST = 1

1
ground/src/shared/uavobjectdefinition/stabilizationsettings.xml
View File

@ -1,7 +1,6 @@
<xml>
<object name="StabilizationSettings" singleinstance="true" settings="true">
<description>PID settings used by the Stabilization module to combine the @ref AttitudeActual and @ref AttitudeDesired to compute @ref ActuatorDesired</description>
<field name="UpdatePeriod" units="ms" type="uint8" elements="1" defaultvalue="10"/>
<field name="RollMax" units="degrees" type="uint8" elements="1" defaultvalue="35"/>
<field name="PitchMax" units="degrees" type="uint8" elements="1" defaultvalue="35"/>
<field name="YawMax" units="degrees" type="uint8" elements="1" defaultvalue="35"/>