OP-261 Drop PositionPID mode from GuidanceSettings

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2413 ebee16cc-31ac-478f-84a7-5cbb03baadba

flight/OpenPilot/Modules/Guidance/guidance.c

@@ -76,7 +76,6 @@ static float bound(float val, float min, float max);
 static void updateVtolDesiredVelocity();
 static void manualSetDesiredVelocity();
 static void updateVtolDesiredAttitude();
-static void positionPIDcontrol();
 
 /**
  * Initialise the module, called on startup
@@ -197,16 +196,12 @@ static void guidanceTask(void *parameters)
 				positionHoldLast = 1;
 			}
 			
-			if(guidanceSettings.GuidanceMode == GUIDANCESETTINGS_GUIDANCEMODE_POSITION_PID) {
+			if(guidanceSettings.GuidanceMode == GUIDANCESETTINGS_GUIDANCEMODE_DUAL_LOOP) 
-				positionPIDcontrol();
+				updateVtolDesiredVelocity();
-			} else {
+			else
-				if(guidanceSettings.GuidanceMode == GUIDANCESETTINGS_GUIDANCEMODE_DUAL_LOOP) 
+				manualSetDesiredVelocity();			
-					updateVtolDesiredVelocity();
+			updateVtolDesiredAttitude();
-				else
+			
-					manualSetDesiredVelocity();
-				
-				updateVtolDesiredAttitude();
-			}
 		} else {
 			// Be cleaner and get rid of global variables
 			northIntegral = 0;
@@ -220,6 +215,12 @@ static void guidanceTask(void *parameters)
 	}
 }
 
+/**
+ * Compute desired velocity from the current position
+ *
+ * Takes in @ref PositionActual and compares it to @ref PositionDesired 
+ * and computes @ref VelocityDesired
+ */
 void updateVtolDesiredVelocity()
 {
 	GuidanceSettingsData guidanceSettings;
@@ -252,7 +253,11 @@ void updateVtolDesiredVelocity()
 }
 
 /**
- * Module thread, should not return.
+ * Compute desired attitude from the desired velocity
+ *
+ * Takes in @ref NedActual which has the acceleration in the 
+ * NED frame as the feedback term and then compares the 
+ * @ref VelocityActual against the @ref VelocityDesired
  */
 static void updateVtolDesiredAttitude()
 {
@@ -346,6 +351,9 @@ static void updateVtolDesiredAttitude()
 	AttitudeDesiredSet(&attitudeDesired);
 }
 
+/**
+ * Set the desired velocity from the input sticks
+ */
 static void manualSetDesiredVelocity() 
 {
 	ManualControlCommandData cmd;
@@ -364,82 +372,6 @@ static void manualSetDesiredVelocity()
 	VelocityDesiredSet(&velocityDesired);	
 }
 
-/** 
- * Control attitude with direct PID on position error
- */
-static void positionPIDcontrol() 
-{
-	static portTickType lastSysTime;
-	portTickType thisSysTime = xTaskGetTickCount();;
-	float dT;
-	
-	AttitudeDesiredData attitudeDesired;
-	AttitudeActualData attitudeActual;
-	GuidanceSettingsData guidanceSettings;
-	VelocityActualData velocityActual;
-	StabilizationSettingsData stabSettings;
-	SystemSettingsData systemSettings;
-	PositionActualData positionActual;
-	PositionDesiredData positionDesired;
-	
-	float northError;
-	float northCommand;
-	
-	float eastError;
-	float eastCommand;
-	
-	// Check how long since last update
-	if(thisSysTime > lastSysTime) // reuse dt in case of wraparound
-		dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;		
-	lastSysTime = thisSysTime;
-	
-	SystemSettingsGet(&systemSettings);
-	GuidanceSettingsGet(&guidanceSettings);
-	
-	AttitudeDesiredGet(&attitudeDesired);
-	AttitudeActualGet(&attitudeActual);
-	StabilizationSettingsGet(&stabSettings);
-	PositionActualGet(&positionActual);
-	PositionDesiredGet(&positionDesired);
-	VelocityActualGet(&velocityActual);
-	
-	attitudeDesired.Yaw = 0;	// try and face north
-	
-	// Yaw and pitch output from ground speed PID loop
-	northError = positionDesired.North - positionActual.North;
-	northIntegral = bound(northIntegral + northError * dT, 
-			      -guidanceSettings.MaxVelIntegral,
-			      guidanceSettings.MaxVelIntegral);
-	northCommand = northError * guidanceSettings.VelP + 
-			northIntegral * guidanceSettings.VelI -
-			velocityActual.North * guidanceSettings.VelD;
-	
-	eastError = positionDesired.East - positionActual.East;
-	eastIntegral = bound(eastIntegral + eastError * dT, 
-			     -guidanceSettings.MaxVelIntegral,
-			     guidanceSettings.MaxVelIntegral);
-	eastCommand = eastError * guidanceSettings.VelP + 
-			eastIntegral * guidanceSettings.VelI  - 
-			velocityActual.East * guidanceSettings.VelD;
-	
-	// Project the north and east command signals into the pitch and roll based on yaw.  For this to behave well the
-	// craft should move similarly for 5 deg roll versus 5 deg pitch
-	attitudeDesired.Pitch = bound(-northCommand * cosf(attitudeActual.Yaw * M_PI / 180) + 
-				      eastCommand * sinf(attitudeActual.Yaw * M_PI / 180),
-				      -stabSettings.PitchMax, stabSettings.PitchMax);
-	attitudeDesired.Roll = bound(-northCommand * sinf(attitudeActual.Yaw * M_PI / 180) + 
-				     eastCommand * cosf(attitudeActual.Yaw * M_PI / 180),
-				     -stabSettings.RollMax, stabSettings.RollMax);
-		
-	// For now override throttle with manual control.  Disable at your risk, quad goes to China.
-	ManualControlCommandData manualControl;
-	ManualControlCommandGet(&manualControl);
-	attitudeDesired.Throttle = manualControl.Throttle;
-	
-	AttitudeDesiredSet(&attitudeDesired);
-	
-}
-
 /**
  * Bound input value between limits
  */

ground/src/plugins/uavobjects/guidancesettings.cpp

@@ -1,199 +0,0 @@
-/**
- ******************************************************************************
- *
- * @file       guidancesettings.cpp
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @see        The GNU Public License (GPL) Version 3
- * @addtogroup GCSPlugins GCS Plugins
- * @{
- * @addtogroup UAVObjectsPlugin UAVObjects Plugin
- * @{
- *   
- * @note       Object definition file: guidancesettings.xml. 
- *             This is an automatically generated file.
- *             DO NOT modify manually.
- *
- * @brief      The UAVUObjects GCS plugin
- *****************************************************************************/
-/* 
- * This program is free software; you can redistribute it and/or modify 
- * it under the terms of the GNU General Public License as published by 
- * the Free Software Foundation; either version 3 of the License, or 
- * (at your option) any later version.
- * 
- * This program is distributed in the hope that it will be useful, but 
- * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY 
- * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License 
- * for more details.
- * 
- * You should have received a copy of the GNU General Public License along 
- * with this program; if not, write to the Free Software Foundation, Inc., 
- * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- */
-#include "guidancesettings.h"
-#include "uavobjectfield.h"
-
-const QString GuidanceSettings::NAME = QString("GuidanceSettings");
-const QString GuidanceSettings::DESCRIPTION = QString("Settings for the @ref GuidanceModule");
-
-/**
- * Constructor
- */
-GuidanceSettings::GuidanceSettings(): UAVDataObject(OBJID, ISSINGLEINST, ISSETTINGS, NAME)
-{
-    // Create fields
-    QList<UAVObjectField*> fields;
-    QStringList GuidanceModeElemNames;
-    GuidanceModeElemNames.append("0");
-    QStringList GuidanceModeEnumOptions;
-    GuidanceModeEnumOptions.append("DUAL_LOOP");
-    GuidanceModeEnumOptions.append("VELOCITY_CONTROL");
-    GuidanceModeEnumOptions.append("POSITION_PID");
-    fields.append( new UAVObjectField(QString("GuidanceMode"), QString(""), UAVObjectField::ENUM, GuidanceModeElemNames, GuidanceModeEnumOptions) );
-    QStringList MaxGroundspeedElemNames;
-    MaxGroundspeedElemNames.append("0");
-    fields.append( new UAVObjectField(QString("MaxGroundspeed"), QString("cm/s"), UAVObjectField::INT32, MaxGroundspeedElemNames, QStringList()) );
-    QStringList GroundVelocityPElemNames;
-    GroundVelocityPElemNames.append("0");
-    fields.append( new UAVObjectField(QString("GroundVelocityP"), QString(""), UAVObjectField::FLOAT32, GroundVelocityPElemNames, QStringList()) );
-    QStringList MaxVerticalSpeedElemNames;
-    MaxVerticalSpeedElemNames.append("0");
-    fields.append( new UAVObjectField(QString("MaxVerticalSpeed"), QString("cm/s"), UAVObjectField::INT32, MaxVerticalSpeedElemNames, QStringList()) );
-    QStringList VertVelocityPElemNames;
-    VertVelocityPElemNames.append("0");
-    fields.append( new UAVObjectField(QString("VertVelocityP"), QString(""), UAVObjectField::FLOAT32, VertVelocityPElemNames, QStringList()) );
-    QStringList VelPElemNames;
-    VelPElemNames.append("0");
-    fields.append( new UAVObjectField(QString("VelP"), QString(""), UAVObjectField::FLOAT32, VelPElemNames, QStringList()) );
-    QStringList VelIElemNames;
-    VelIElemNames.append("0");
-    fields.append( new UAVObjectField(QString("VelI"), QString(""), UAVObjectField::FLOAT32, VelIElemNames, QStringList()) );
-    QStringList VelDElemNames;
-    VelDElemNames.append("0");
-    fields.append( new UAVObjectField(QString("VelD"), QString(""), UAVObjectField::FLOAT32, VelDElemNames, QStringList()) );
-    QStringList DownPElemNames;
-    DownPElemNames.append("0");
-    fields.append( new UAVObjectField(QString("DownP"), QString(""), UAVObjectField::FLOAT32, DownPElemNames, QStringList()) );
-    QStringList DownIElemNames;
-    DownIElemNames.append("0");
-    fields.append( new UAVObjectField(QString("DownI"), QString(""), UAVObjectField::FLOAT32, DownIElemNames, QStringList()) );
-    QStringList DownDElemNames;
-    DownDElemNames.append("0");
-    fields.append( new UAVObjectField(QString("DownD"), QString(""), UAVObjectField::FLOAT32, DownDElemNames, QStringList()) );
-    QStringList MaxVelIntegralElemNames;
-    MaxVelIntegralElemNames.append("0");
-    fields.append( new UAVObjectField(QString("MaxVelIntegral"), QString("deg"), UAVObjectField::FLOAT32, MaxVelIntegralElemNames, QStringList()) );
-    QStringList MaxThrottleIntegralElemNames;
-    MaxThrottleIntegralElemNames.append("0");
-    fields.append( new UAVObjectField(QString("MaxThrottleIntegral"), QString("deg"), UAVObjectField::FLOAT32, MaxThrottleIntegralElemNames, QStringList()) );
-    QStringList ThrottleControlElemNames;
-    ThrottleControlElemNames.append("0");
-    QStringList ThrottleControlEnumOptions;
-    ThrottleControlEnumOptions.append("FALSE");
-    ThrottleControlEnumOptions.append("TRUE");
-    fields.append( new UAVObjectField(QString("ThrottleControl"), QString(""), UAVObjectField::ENUM, ThrottleControlElemNames, ThrottleControlEnumOptions) );
-    QStringList VelUpdatePeriodElemNames;
-    VelUpdatePeriodElemNames.append("0");
-    fields.append( new UAVObjectField(QString("VelUpdatePeriod"), QString(""), UAVObjectField::INT32, VelUpdatePeriodElemNames, QStringList()) );
-    QStringList VelPIDUpdatePeriodElemNames;
-    VelPIDUpdatePeriodElemNames.append("0");
-    fields.append( new UAVObjectField(QString("VelPIDUpdatePeriod"), QString(""), UAVObjectField::INT32, VelPIDUpdatePeriodElemNames, QStringList()) );
-
-    // Initialize object
-    initializeFields(fields, (quint8*)&data, NUMBYTES);
-    // Set the default field values
-    setDefaultFieldValues();
-    // Set the object description
-    setDescription(DESCRIPTION);
-}
-
-/**
- * Get the default metadata for this object
- */
-UAVObject::Metadata GuidanceSettings::getDefaultMetadata()
-{
-    UAVObject::Metadata metadata;
-    metadata.flightAccess = ACCESS_READWRITE;
-    metadata.gcsAccess = ACCESS_READWRITE;
-    metadata.gcsTelemetryAcked = 1;
-    metadata.gcsTelemetryUpdateMode = UAVObject::UPDATEMODE_ONCHANGE;
-    metadata.gcsTelemetryUpdatePeriod = 0;
-    metadata.flightTelemetryAcked = 1;
-    metadata.flightTelemetryUpdateMode = UAVObject::UPDATEMODE_ONCHANGE;
-    metadata.flightTelemetryUpdatePeriod = 0;
-    metadata.loggingUpdateMode = UAVObject::UPDATEMODE_NEVER;
-    metadata.loggingUpdatePeriod = 0;
-    return metadata;
-}
-
-/**
- * Initialize object fields with the default values.
- * If a default value is not specified the object fields
- * will be initialized to zero.
- */
-void GuidanceSettings::setDefaultFieldValues()
-{
-    data.GuidanceMode = 0;
-    data.MaxGroundspeed = 100;
-    data.GroundVelocityP = 0.1;
-    data.MaxVerticalSpeed = 100;
-    data.VertVelocityP = 0.1;
-    data.VelP = 0.1;
-    data.VelI = 0.1;
-    data.VelD = 0;
-    data.DownP = 0;
-    data.DownI = 0;
-    data.DownD = 0;
-    data.MaxVelIntegral = 2;
-    data.MaxThrottleIntegral = 1;
-    data.ThrottleControl = 0;
-    data.VelUpdatePeriod = 100;
-    data.VelPIDUpdatePeriod = 20;
-
-}
-
-/**
- * Get the object data fields
- */
-GuidanceSettings::DataFields GuidanceSettings::getData()
-{
-    QMutexLocker locker(mutex);
-    return data;
-}
-
-/**
- * Set the object data fields
- */
-void GuidanceSettings::setData(const DataFields& data)
-{
-    QMutexLocker locker(mutex);
-    // Get metadata
-    Metadata mdata = getMetadata();
-    // Update object if the access mode permits
-    if ( mdata.gcsAccess == ACCESS_READWRITE )
-    {
-        this->data = data;
-        emit objectUpdatedAuto(this); // trigger object updated event
-        emit objectUpdated(this);
-    }
-}
-
-/**
- * Create a clone of this object, a new instance ID must be specified.
- * Do not use this function directly to create new instances, the
- * UAVObjectManager should be used instead.
- */
-UAVDataObject* GuidanceSettings::clone(quint32 instID)
-{
-    GuidanceSettings* obj = new GuidanceSettings();
-    obj->initialize(instID, this->getMetaObject());
-    return obj;
-}
-
-/**
- * Static function to retrieve an instance of the object.
- */
-GuidanceSettings* GuidanceSettings::GetInstance(UAVObjectManager* objMngr, quint32 instID)
-{
-    return dynamic_cast<GuidanceSettings*>(objMngr->getObject(GuidanceSettings::OBJID, instID));
-}

ground/src/plugins/uavobjects/watchdogstatus.h

@@ -1,81 +0,0 @@
-/**
- ******************************************************************************
- *
- * @file       watchdogstatus.h
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @see        The GNU Public License (GPL) Version 3
- * @addtogroup GCSPlugins GCS Plugins
- * @{
- * @addtogroup UAVObjectsPlugin UAVObjects Plugin
- * @{
- *   
- * @note       Object definition file: watchdogstatus.xml. 
- *             This is an automatically generated file.
- *             DO NOT modify manually.
- *
- * @brief      The UAVUObjects GCS plugin 
- *****************************************************************************/
-/* 
- * This program is free software; you can redistribute it and/or modify 
- * it under the terms of the GNU General Public License as published by 
- * the Free Software Foundation; either version 3 of the License, or 
- * (at your option) any later version.
- * 
- * This program is distributed in the hope that it will be useful, but 
- * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY 
- * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License 
- * for more details.
- * 
- * You should have received a copy of the GNU General Public License along 
- * with this program; if not, write to the Free Software Foundation, Inc., 
- * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- */
-#ifndef WATCHDOGSTATUS_H
-#define WATCHDOGSTATUS_H
-
-#include "uavdataobject.h"
-#include "uavobjectmanager.h"
-
-class UAVOBJECTS_EXPORT WatchdogStatus: public UAVDataObject
-{
-    Q_OBJECT
-
-public:
-    // Field structure
-    typedef struct {
-        quint16 BootupFlags;
-        quint16 ActiveFlags;
-
-    } __attribute__((packed)) DataFields;
-
-    // Field information
-    // Field BootupFlags information
-    // Field ActiveFlags information
-
-  
-    // Constants
-    static const quint32 OBJID = 3594971408U;
-    static const QString NAME;
-    static const QString DESCRIPTION;
-    static const bool ISSINGLEINST = 1;
-    static const bool ISSETTINGS = 0;
-    static const quint32 NUMBYTES = sizeof(DataFields);
-
-    // Functions
-    WatchdogStatus();
-
-    DataFields getData();
-    void setData(const DataFields& data);
-    Metadata getDefaultMetadata();
-    UAVDataObject* clone(quint32 instID);
-
-    static WatchdogStatus* GetInstance(UAVObjectManager* objMngr, quint32 instID = 0);
-	
-private:
-    DataFields data;
-
-    void setDefaultFieldValues();
-
-};
-
-#endif // WATCHDOGSTATUS_H

ground/src/shared/uavobjectdefinition/guidancesettings.xml

@@ -1,7 +1,7 @@
 <xml>
     <object name="GuidanceSettings" singleinstance="true" settings="true">
         <description>Settings for the @ref GuidanceModule</description>
-        <field name="GuidanceMode" units="" type="enum" elements="1" options="DUAL_LOOP,VELOCITY_CONTROL,POSITION_PID" defaultvalue="DUAL_LOOP"/>
+        <field name="GuidanceMode" units="" type="enum" elements="1" options="DUAL_LOOP,VELOCITY_CONTROL" defaultvalue="DUAL_LOOP"/>
         <field name="MaxGroundspeed" units="cm/s" type="int32" elements="1" defaultvalue="100"/>
         <field name="GroundVelocityP" units="" type="float" elements="1" defaultvalue="0.1"/>
         <field name="MaxVerticalSpeed" units="cm/s" type="int32" elements="1" defaultvalue="100"/>