mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-01-17 02:52:12 +01:00
OP-261 Drop PositionPID mode from GuidanceSettings
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2413 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124
peabody124
parent
aa8d9eb178
commit
c141b02339
@ -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) {
|
||||
positionPIDcontrol();
|
||||
} else {
|
||||
if(guidanceSettings.GuidanceMode == GUIDANCESETTINGS_GUIDANCEMODE_DUAL_LOOP)
|
||||
updateVtolDesiredVelocity();
|
||||
else
|
||||
manualSetDesiredVelocity();
|
||||
|
||||
updateVtolDesiredAttitude();
|
||||
}
|
||||
if(guidanceSettings.GuidanceMode == GUIDANCESETTINGS_GUIDANCEMODE_DUAL_LOOP)
|
||||
updateVtolDesiredVelocity();
|
||||
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
|
||||
*/
|
||||
|
||||
@ -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));
|
||||
}
|
||||
@ -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
|
||||
@ -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"/>
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user