mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-02 10:24:11 +01:00
AHRS: Yaw stabilization now supports rate stabilization or heading hold. Also need to cleanly support rate stabilization on all the other axis. Les suggested this be in manual mode which will require changing the logic somewhat (essentially for vtol it will always be running through the Stabilization module) in an aircraft dependent manner. An alternative would be a stabilization flag indicating to do full attitude hold, attitude + yaw stabilization, or all axis gyro stabilization. This would be my preference.
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1659 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124
peabody124
parent
64018c88c1
commit
a6e71177e4
@ -242,18 +242,24 @@ static void manualControlTask(void* parameters)
|
||||
{
|
||||
attitude.Roll = cmd.Roll*stabSettings.RollMax;
|
||||
attitude.Pitch = cmd.Pitch*stabSettings.PitchMax;
|
||||
if (cmd.Yaw<0)
|
||||
{
|
||||
attitude.Yaw = 360 + (cmd.Yaw*180.0);
|
||||
if(stabSettings.YawMode == STABILIZATIONSETTINGS_YAWMODE_RATE) {
|
||||
/* rate stabilization */
|
||||
attitude.Yaw = cmd.Yaw*stabSettings.YawMax;
|
||||
} else {
|
||||
/* heading stabilization */
|
||||
if (cmd.Yaw<0)
|
||||
{
|
||||
attitude.Yaw = 360 + (cmd.Yaw*180.0);
|
||||
}
|
||||
else
|
||||
{
|
||||
attitude.Yaw = (cmd.Yaw*180.0);
|
||||
}
|
||||
if(cmd.Throttle < 0)
|
||||
attitude.Throttle = -1;
|
||||
else
|
||||
attitude.Throttle = cmd.Throttle*stabSettings.ThrottleMax;
|
||||
}
|
||||
else
|
||||
{
|
||||
attitude.Yaw = (cmd.Yaw*180.0);
|
||||
}
|
||||
if(cmd.Throttle < 0)
|
||||
attitude.Throttle = -1;
|
||||
else
|
||||
attitude.Throttle = cmd.Throttle*stabSettings.ThrottleMax;
|
||||
AttitudeDesiredSet(&attitude);
|
||||
}
|
||||
|
||||
|
||||
@ -88,6 +88,8 @@ static void stabilizationTask(void* parameters)
|
||||
float pitchIntegral;
|
||||
float rollIntegral;
|
||||
float yawIntegral;
|
||||
float yawPrevious;
|
||||
float yawChange;
|
||||
|
||||
// Initialize
|
||||
pitchIntegral = 0.0;
|
||||
@ -127,7 +129,14 @@ static void stabilizationTask(void* parameters)
|
||||
// Yaw stabilization control loop (only enabled on VTOL airframes)
|
||||
if (( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_VTOL )||( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HELICP))
|
||||
{
|
||||
yawError = attitudeDesired.Yaw - attitudeActual.Yaw;
|
||||
if(stabSettings.YawMode == STABILIZATIONSETTINGS_YAWMODE_RATE) { // rate stabilization on yaw
|
||||
yawChange = attitudeActual.Yaw - yawPrevious;
|
||||
yawPrevious = attitudeActual.Yaw;
|
||||
yawError = bound(attitudeDesired.Yaw, -stabSettings.YawMax, stabSettings.YawMax) - yawChange;
|
||||
} else { // heading stabilization
|
||||
yawError = attitudeDesired.Yaw - attitudeActual.Yaw;
|
||||
}
|
||||
|
||||
//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;
|
||||
|
||||
@ -41,7 +41,7 @@
|
||||
#define STABILIZATIONSETTINGS_H
|
||||
|
||||
// Object constants
|
||||
#define STABILIZATIONSETTINGS_OBJID 3082215042U
|
||||
#define STABILIZATIONSETTINGS_OBJID 1244337598U
|
||||
#define STABILIZATIONSETTINGS_NAME "StabilizationSettings"
|
||||
#define STABILIZATIONSETTINGS_METANAME "StabilizationSettingsMeta"
|
||||
#define STABILIZATIONSETTINGS_ISSINGLEINST 1
|
||||
@ -74,6 +74,8 @@ typedef struct {
|
||||
uint16_t UpdatePeriod;
|
||||
float RollMax;
|
||||
float PitchMax;
|
||||
float YawMax;
|
||||
uint8_t YawMode;
|
||||
float ThrottleMax;
|
||||
float RollIntegralLimit;
|
||||
float PitchIntegralLimit;
|
||||
@ -94,6 +96,10 @@ typedef struct {
|
||||
// Field UpdatePeriod information
|
||||
// Field RollMax information
|
||||
// Field PitchMax information
|
||||
// Field YawMax information
|
||||
// Field YawMode information
|
||||
/* Enumeration options for field YawMode */
|
||||
typedef enum { STABILIZATIONSETTINGS_YAWMODE_RATE=0, STABILIZATIONSETTINGS_YAWMODE_HEADING=1 } StabilizationSettingsYawModeOptions;
|
||||
// Field ThrottleMax information
|
||||
// Field RollIntegralLimit information
|
||||
// Field PitchIntegralLimit information
|
||||
|
||||
@ -83,6 +83,8 @@ static void setDefaults(UAVObjHandle obj, uint16_t instId)
|
||||
data.UpdatePeriod = 10;
|
||||
data.RollMax = 35;
|
||||
data.PitchMax = 35;
|
||||
data.YawMax = 35;
|
||||
data.YawMode = 0;
|
||||
data.ThrottleMax = 1;
|
||||
data.RollIntegralLimit = 0.5;
|
||||
data.PitchIntegralLimit = 0.5;
|
||||
|
||||
@ -51,6 +51,15 @@ StabilizationSettings::StabilizationSettings(): UAVDataObject(OBJID, ISSINGLEINS
|
||||
QStringList PitchMaxElemNames;
|
||||
PitchMaxElemNames.append("0");
|
||||
fields.append( new UAVObjectField(QString("PitchMax"), QString("degrees"), UAVObjectField::FLOAT32, PitchMaxElemNames, QStringList()) );
|
||||
QStringList YawMaxElemNames;
|
||||
YawMaxElemNames.append("0");
|
||||
fields.append( new UAVObjectField(QString("YawMax"), QString("degrees"), UAVObjectField::FLOAT32, YawMaxElemNames, QStringList()) );
|
||||
QStringList YawModeElemNames;
|
||||
YawModeElemNames.append("0");
|
||||
QStringList YawModeEnumOptions;
|
||||
YawModeEnumOptions.append("rate");
|
||||
YawModeEnumOptions.append("heading");
|
||||
fields.append( new UAVObjectField(QString("YawMode"), QString("degrees"), UAVObjectField::ENUM, YawModeElemNames, YawModeEnumOptions) );
|
||||
QStringList ThrottleMaxElemNames;
|
||||
ThrottleMaxElemNames.append("0");
|
||||
fields.append( new UAVObjectField(QString("ThrottleMax"), QString("%"), UAVObjectField::FLOAT32, ThrottleMaxElemNames, QStringList()) );
|
||||
@ -126,6 +135,8 @@ void StabilizationSettings::setDefaultFieldValues()
|
||||
data.UpdatePeriod = 10;
|
||||
data.RollMax = 35;
|
||||
data.PitchMax = 35;
|
||||
data.YawMax = 35;
|
||||
data.YawMode = 0;
|
||||
data.ThrottleMax = 1;
|
||||
data.RollIntegralLimit = 0.5;
|
||||
data.PitchIntegralLimit = 0.5;
|
||||
|
||||
@ -46,6 +46,8 @@ public:
|
||||
quint16 UpdatePeriod;
|
||||
float RollMax;
|
||||
float PitchMax;
|
||||
float YawMax;
|
||||
quint8 YawMode;
|
||||
float ThrottleMax;
|
||||
float RollIntegralLimit;
|
||||
float PitchIntegralLimit;
|
||||
@ -66,6 +68,10 @@ public:
|
||||
// Field UpdatePeriod information
|
||||
// Field RollMax information
|
||||
// Field PitchMax information
|
||||
// Field YawMax information
|
||||
// Field YawMode information
|
||||
/* Enumeration options for field YawMode */
|
||||
typedef enum { YAWMODE_RATE=0, YAWMODE_HEADING=1 } YawModeOptions;
|
||||
// Field ThrottleMax information
|
||||
// Field RollIntegralLimit information
|
||||
// Field PitchIntegralLimit information
|
||||
@ -82,7 +88,7 @@ public:
|
||||
|
||||
|
||||
// Constants
|
||||
static const quint32 OBJID = 3082215042U;
|
||||
static const quint32 OBJID = 1244337598U;
|
||||
static const QString NAME;
|
||||
static const bool ISSINGLEINST = 1;
|
||||
static const bool ISSETTINGS = 1;
|
||||
|
||||
@ -67,6 +67,28 @@ _fields = [ \
|
||||
{
|
||||
}
|
||||
),
|
||||
uavobject.UAVObjectField(
|
||||
'YawMax',
|
||||
'f',
|
||||
1,
|
||||
[
|
||||
'0',
|
||||
],
|
||||
{
|
||||
}
|
||||
),
|
||||
uavobject.UAVObjectField(
|
||||
'YawMode',
|
||||
'b',
|
||||
1,
|
||||
[
|
||||
'0',
|
||||
],
|
||||
{
|
||||
'0' : 'rate',
|
||||
'1' : 'heading',
|
||||
}
|
||||
),
|
||||
uavobject.UAVObjectField(
|
||||
'ThrottleMax',
|
||||
'f',
|
||||
@ -202,7 +224,7 @@ _fields = [ \
|
||||
|
||||
class StabilizationSettings(uavobject.UAVObject):
|
||||
## Object constants
|
||||
OBJID = 3082215042
|
||||
OBJID = 1244337598
|
||||
NAME = "StabilizationSettings"
|
||||
METANAME = "StabilizationSettingsMeta"
|
||||
ISSINGLEINST = 1
|
||||
|
||||
@ -4,6 +4,8 @@
|
||||
<field name="UpdatePeriod" units="ms" type="uint16" elements="1" defaultvalue="10"/>
|
||||
<field name="RollMax" units="degrees" type="float" elements="1" defaultvalue="35"/>
|
||||
<field name="PitchMax" units="degrees" type="float" elements="1" defaultvalue="35"/>
|
||||
<field name="YawMax" units="degrees" type="float" elements="1" defaultvalue="35"/>
|
||||
<field name="YawMode" units="degrees" type="enum" elements="1" options="rate,heading" defaultvalue="rate"/>
|
||||
<field name="ThrottleMax" units="%" type="float" elements="1" defaultvalue="1.0"/>
|
||||
<field name="RollIntegralLimit" units="" type="float" elements="1" defaultvalue="0.5"/>
|
||||
<field name="PitchIntegralLimit" units="" type="float" elements="1" defaultvalue="0.5"/>
|
||||
|
||||
Loading…
Reference in New Issue
Block a user