rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-18 08:54:15 +01:00
Code Issues Releases Activity

OP-163 Flight/Guidance: Add variations on position hold to try out. Only one will end

Browse Source 
up in the final code.

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2110 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2010-11-12 16:57:13 +00:00 committed by peabody124
parent 5850c1804d
commit 6ebc8ccd57
8 changed files with 145 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

113
flight/OpenPilot/Modules/Guidance/guidance.c
View File

@ -69,7 +69,9 @@ static void guidanceTask(void *parameters);
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
@ -107,13 +109,22 @@ static void guidanceTask(void *parameters)
ManualControlCommandGet(&manualControl);
SystemSettingsGet(&systemSettings);
GuidanceSettingsGet(&guidanceSettings);
if ((manualControl.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO) &&
((systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_VTOL) ||
(systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADP) ||
(systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADX))){
updateVtolDesiredVelocity();
updateVtolDesiredAttitude();
(systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADX)))
{
if(guidanceSettings.GuidanceMode == GUIDANCESETTINGS_GUIDANCEMODE_POSITION_PID) {
positionPIDcontrol();
} else {
if(guidanceSettings.GuidanceMode == GUIDANCESETTINGS_GUIDANCEMODE_DUAL_LOOP)
updateVtolDesiredVelocity();
else
manualSetDesiredVelocity();
updateVtolDesiredAttitude();
}
} else {
// Be cleaner and get rid of global variables
northIntegral = 0;
@ -178,9 +189,11 @@ static void updateVtolDesiredAttitude()
float northError;
float northDerivative;
float northCommand;
float eastError;
float eastDerivative;
float eastCommand;
float downError;
float downDerivative;
@ -245,6 +258,98 @@ static void updateVtolDesiredAttitude()
AttitudeDesiredSet(&attitudeDesired);
}
static void manualSetDesiredVelocity()
{
ManualControlCommandData cmd;
VelocityDesiredData velocityDesired;
ManualControlCommandGet(&cmd);
VelocityDesiredGet(&velocityDesired);
velocityDesired.North = -200 * cmd.Pitch;
velocityDesired.East = 200 * cmd.Roll;
velocityDesired.Down = 0;
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;
StabilizationSettingsData stabSettings;
SystemSettingsData systemSettings;
PositionActualData positionActual;
PositionDesiredData positionDesired;
float northError;
float northDerivative;
float northCommand;
float eastError;
float eastDerivative;
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);
attitudeDesired.Yaw = 0; // try and face north
// Yaw and pitch output from ground speed PID loop
northError = positionDesired.North - positionActual.North;
northDerivative = (northError - northErrorLast) / dT;
northIntegral =
bound(northIntegral + northError * dT, -guidanceSettings.MaxVelIntegral,
guidanceSettings.MaxVelIntegral);
northErrorLast = northError;
northCommand =
northError * guidanceSettings.VelP + northDerivative * guidanceSettings.VelD + northIntegral * guidanceSettings.VelI;
eastError = positionDesired.East - positionActual.East;
eastDerivative = (eastError - eastErrorLast) / dT;
eastIntegral = bound(eastIntegral + eastError * dT,
-guidanceSettings.MaxVelIntegral,
guidanceSettings.MaxVelIntegral);
eastErrorLast = eastError;
eastCommand = eastError * guidanceSettings.VelP + eastDerivative * guidanceSettings.VelD + eastIntegral * guidanceSettings.VelI;
// 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
flight/OpenPilot/UAVObjects/guidancesettings.c
View File

@ -80,6 +80,7 @@ static void setDefaults(UAVObjHandle obj, uint16_t instId)
// Initialize object fields to their default values
UAVObjGetInstanceData(obj, instId, &data);
memset(&data, 0, sizeof(GuidanceSettingsData));
data.GuidanceMode = 0;
data.MaxGroundspeed = 100;
data.GroundVelocityP = 0.1;
data.MaxVerticalSpeed = 100;

6
flight/OpenPilot/UAVObjects/inc/guidancesettings.h
View File

@ -41,7 +41,7 @@
#define GUIDANCESETTINGS_H
// Object constants
#define GUIDANCESETTINGS_OBJID 2428005802U
#define GUIDANCESETTINGS_OBJID 2071403670U
#define GUIDANCESETTINGS_NAME "GuidanceSettings"
#define GUIDANCESETTINGS_METANAME "GuidanceSettingsMeta"
#define GUIDANCESETTINGS_ISSINGLEINST 1
@ -71,6 +71,7 @@
// Object data
typedef struct {
uint8_t GuidanceMode;
int32_t MaxGroundspeed;
float GroundVelocityP;
int32_t MaxVerticalSpeed;
@ -89,6 +90,9 @@ typedef struct {
} __attribute__((packed)) GuidanceSettingsData;
// Field information
// Field GuidanceMode information
/* Enumeration options for field GuidanceMode */
typedef enum { GUIDANCESETTINGS_GUIDANCEMODE_DUAL_LOOP=0, GUIDANCESETTINGS_GUIDANCEMODE_VELOCITY_CONTROL=1, GUIDANCESETTINGS_GUIDANCEMODE_POSITION_PID=2 } GuidanceSettingsGuidanceModeOptions;
// Field MaxGroundspeed information
// Field GroundVelocityP information
// Field MaxVerticalSpeed information

2
flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj
View File

@ -38,6 +38,7 @@
65322D3012283CD60046CD7C /* NMEA.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = NMEA.h; sourceTree = "<group>"; };
65322D3B122841F60046CD7C /* gpstime.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = gpstime.xml; sourceTree = "<group>"; };
65322D77122897210046CD7C /* MagOrAccelSensorCal.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = MagOrAccelSensorCal.c; path = ../../AHRS/MagOrAccelSensorCal.c; sourceTree = SOURCE_ROOT; };
65345C871288668B00A5E4E8 /* guidancesettings.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = guidancesettings.xml; sourceTree = "<group>"; };
654330231218E9780063F913 /* insgps.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = insgps.c; path = ../../AHRS/insgps.c; sourceTree = SOURCE_ROOT; };
6543304F121980300063F913 /* insgps.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = insgps.h; sourceTree = "<group>"; };
6549E0D21279B3C800C5476F /* fifo_buffer.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = fifo_buffer.c; sourceTree = "<group>"; };
@ -6797,6 +6798,7 @@
65B367F3121C2620003EAD18 /* gcstelemetrystats.xml */,
65209A1912208B0600453371 /* gpsposition.xml */,
65322D3B122841F60046CD7C /* gpstime.xml */,
65345C871288668B00A5E4E8 /* guidancesettings.xml */,
657CEEAD121DB6C8007A1FBE /* homelocation.xml */,
65B367F4121C2620003EAD18 /* manualcontrolcommand.xml */,
65B367F5121C2620003EAD18 /* manualcontrolsettings.xml */,

8
ground/src/plugins/uavobjects/guidancesettings.cpp
View File

@ -42,6 +42,13 @@ GuidanceSettings::GuidanceSettings(): UAVDataObject(OBJID, ISSINGLEINST, ISSETTI
{
// 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()) );
@ -117,6 +124,7 @@ UAVObject::Metadata GuidanceSettings::getDefaultMetadata()
*/
void GuidanceSettings::setDefaultFieldValues()
{
data.GuidanceMode = 0;
data.MaxGroundspeed = 100;
data.GroundVelocityP = 0.1;
data.MaxVerticalSpeed = 100;

6
ground/src/plugins/uavobjects/guidancesettings.h
View File

@ -43,6 +43,7 @@ class UAVOBJECTS_EXPORT GuidanceSettings: public UAVDataObject
public:
// Field structure
typedef struct {
quint8 GuidanceMode;
qint32 MaxGroundspeed;
float GroundVelocityP;
qint32 MaxVerticalSpeed;
@ -61,6 +62,9 @@ public:
} __attribute__((packed)) DataFields;
// Field information
// Field GuidanceMode information
/* Enumeration options for field GuidanceMode */
typedef enum { GUIDANCEMODE_DUAL_LOOP=0, GUIDANCEMODE_VELOCITY_CONTROL=1, GUIDANCEMODE_POSITION_PID=2 } GuidanceModeOptions;
// Field MaxGroundspeed information
// Field GroundVelocityP information
// Field MaxVerticalSpeed information
@ -78,7 +82,7 @@ public:
// Constants
static const quint32 OBJID = 2428005802U;
static const quint32 OBJID = 2071403670U;
static const QString NAME;
static const bool ISSINGLEINST = 1;
static const bool ISSETTINGS = 1;

15
ground/src/plugins/uavobjects/guidancesettings.py
View File

@ -37,6 +37,19 @@ from collections import namedtuple
# This is a list of instances of the data fields contained in this object
_fields = [ \
uavobject.UAVObjectField(
'GuidanceMode',
'b',
1,
[
'0',
],
{
'0' : 'DUAL_LOOP',
'1' : 'VELOCITY_CONTROL',
'2' : 'POSITION_PID',
}
),
uavobject.UAVObjectField(
'MaxGroundspeed',
'i',
@ -182,7 +195,7 @@ _fields = [ \
class GuidanceSettings(uavobject.UAVObject):
## Object constants
OBJID = 2428005802
OBJID = 2071403670
NAME = "GuidanceSettings"
METANAME = "GuidanceSettingsMeta"
ISSINGLEINST = 1

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

@ -1,6 +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="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"/>