From 2dc40d11a1ea850bd058941b5efedd48857ef59a Mon Sep 17 00:00:00 2001 From: peabody124 Date: Fri, 1 Oct 2010 12:33:29 +0000 Subject: [PATCH] Flight/Stabilization: Use the system clock to determine time from previous 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 --- .../simple/Stabilization/stabilization.c | 23 ++++++++++++------- .../UAVObjects/inc/stabilizationsettings.h | 4 +--- .../UAVObjects/stabilizationsettings.c | 1 - .../uavobjects/stabilizationsettings.cpp | 4 ---- .../uavobjects/stabilizationsettings.h | 4 +--- .../uavobjects/stabilizationsettings.py | 12 +--------- .../stabilizationsettings.xml | 1 - 7 files changed, 18 insertions(+), 31 deletions(-) diff --git a/flight/OpenPilot/Modules/Stabilization/simple/Stabilization/stabilization.c b/flight/OpenPilot/Modules/Stabilization/simple/Stabilization/stabilization.c index e6c21a761..07526c689 100644 --- a/flight/OpenPilot/Modules/Stabilization/simple/Stabilization/stabilization.c +++ b/flight/OpenPilot/Modules/Stabilization/simple/Stabilization/stabilization.c @@ -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; diff --git a/flight/OpenPilot/UAVObjects/inc/stabilizationsettings.h b/flight/OpenPilot/UAVObjects/inc/stabilizationsettings.h index 5e4685acc..61d2b32a7 100644 --- a/flight/OpenPilot/UAVObjects/inc/stabilizationsettings.h +++ b/flight/OpenPilot/UAVObjects/inc/stabilizationsettings.h @@ -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 diff --git a/flight/OpenPilot/UAVObjects/stabilizationsettings.c b/flight/OpenPilot/UAVObjects/stabilizationsettings.c index 02c5e9f7c..bc95eae14 100644 --- a/flight/OpenPilot/UAVObjects/stabilizationsettings.c +++ b/flight/OpenPilot/UAVObjects/stabilizationsettings.c @@ -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; diff --git a/ground/src/plugins/uavobjects/stabilizationsettings.cpp b/ground/src/plugins/uavobjects/stabilizationsettings.cpp index 611a8e9d4..b24e78737 100644 --- a/ground/src/plugins/uavobjects/stabilizationsettings.cpp +++ b/ground/src/plugins/uavobjects/stabilizationsettings.cpp @@ -42,9 +42,6 @@ StabilizationSettings::StabilizationSettings(): UAVDataObject(OBJID, ISSINGLEINS { // Create fields QList 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; diff --git a/ground/src/plugins/uavobjects/stabilizationsettings.h b/ground/src/plugins/uavobjects/stabilizationsettings.h index de2a95eac..097189065 100644 --- a/ground/src/plugins/uavobjects/stabilizationsettings.h +++ b/ground/src/plugins/uavobjects/stabilizationsettings.h @@ -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; diff --git a/ground/src/plugins/uavobjects/stabilizationsettings.py b/ground/src/plugins/uavobjects/stabilizationsettings.py index d09fc0c98..3adb9f8a4 100644 --- a/ground/src/plugins/uavobjects/stabilizationsettings.py +++ b/ground/src/plugins/uavobjects/stabilizationsettings.py @@ -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 diff --git a/ground/src/shared/uavobjectdefinition/stabilizationsettings.xml b/ground/src/shared/uavobjectdefinition/stabilizationsettings.xml index 4d6e02197..f34ffaa91 100644 --- a/ground/src/shared/uavobjectdefinition/stabilizationsettings.xml +++ b/ground/src/shared/uavobjectdefinition/stabilizationsettings.xml @@ -1,7 +1,6 @@ PID settings used by the Stabilization module to combine the @ref AttitudeActual and @ref AttitudeDesired to compute @ref ActuatorDesired -