rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-18 03:52:11 +01:00
Code Issues Releases Activity

OP-261 Clean up GuidanceSettings to be more intuitive

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2414 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2011-01-14 00:52:00 +00:00 committed by peabody124
parent c141b02339
commit 982f2dd8af
3 changed files with 38 additions and 130 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -126,11 +126,12 @@ static void guidanceTask(void *parameters)
while (1) { while (1) {
// Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe // Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe
if ( xQueueReceive(queue, &ev, guidanceSettings.VelUpdatePeriod / portTICK_RATE_MS) != pdTRUE ) if ( xQueueReceive(queue, &ev, guidanceSettings.UpdatePeriod / portTICK_RATE_MS) != pdTRUE )
{ {
AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION,SYSTEMALARMS_ALARM_WARNING); AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_WARNING);
continue; } else {
} AlarmsClear(SYSTEMALARMS_ALARM_GUIDANCE);
}
// Collect downsampled attitude data // Collect downsampled attitude data
AttitudeRawData attitudeRaw; AttitudeRawData attitudeRaw;
@ -142,7 +143,7 @@ static void guidanceTask(void *parameters)
// Continue collecting data if not enough time // Continue collecting data if not enough time
thisTime = xTaskGetTickCount(); thisTime = xTaskGetTickCount();
if( (thisTime - lastUpdateTime) < (guidanceSettings.VelUpdatePeriod / portTICK_RATE_MS) ) if( (thisTime - lastUpdateTime) < (guidanceSettings.UpdatePeriod / portTICK_RATE_MS) )
continue; continue;
lastUpdateTime = xTaskGetTickCount(); lastUpdateTime = xTaskGetTickCount();
@ -238,16 +239,16 @@ void updateVtolDesiredVelocity()
float dEast = positionDesired.East - positionActual.East; float dEast = positionDesired.East - positionActual.East;
float distance = sqrt(pow(dNorth, 2) + pow(dEast, 2)); float distance = sqrt(pow(dNorth, 2) + pow(dEast, 2));
float heading = atan2f(dEast, dNorth); float heading = atan2f(dEast, dNorth);
float groundspeed = bound(guidanceSettings.GroundVelocityP * distance, float groundspeed = bound(distance * guidanceSettings.HorizontalP[GUIDANCESETTINGS_HORIZONTALP_KP],
0, guidanceSettings.MaxGroundspeed); 0, guidanceSettings.HorizontalP[GUIDANCESETTINGS_HORIZONTALP_MAX]);
velocityDesired.North = groundspeed * cosf(heading); velocityDesired.North = groundspeed * cosf(heading);
velocityDesired.East = groundspeed * sinf(heading); velocityDesired.East = groundspeed * sinf(heading);
float dDown = positionDesired.Down - positionActual.Down; float dDown = positionDesired.Down - positionActual.Down;
velocityDesired.Down = bound(guidanceSettings.VertVelocityP * dDown, velocityDesired.Down = bound(dDown * guidanceSettings.VerticalP[GUIDANCESETTINGS_VERTICALP_KP],
-guidanceSettings.MaxVerticalSpeed, -guidanceSettings.VerticalP[GUIDANCESETTINGS_VERTICALP_MAX],
guidanceSettings.MaxVerticalSpeed); guidanceSettings.VerticalP[GUIDANCESETTINGS_VERTICALP_MAX]);
VelocityDesiredSet(&velocityDesired); VelocityDesiredSet(&velocityDesired);
} }
@ -304,42 +305,40 @@ static void updateVtolDesiredAttitude()
// Compute desired north command // Compute desired north command
northError = velocityDesired.North - velocityActual.North; northError = velocityDesired.North - velocityActual.North;
northIntegral = bound(northIntegral + northError * dT, northIntegral = bound(northIntegral + northError * dT,
-guidanceSettings.MaxVelIntegral, -guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT],
guidanceSettings.MaxVelIntegral); guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT]);
northCommand = (northError * guidanceSettings.VelP + northCommand = (northError * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KP] +
northIntegral * guidanceSettings.VelI - northIntegral * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KI] -
nedAccel.North * guidanceSettings.VelD); nedAccel.North * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KD]);
// Compute desired east command // Compute desired east command
eastError = velocityDesired.East - velocityActual.East; eastError = velocityDesired.East - velocityActual.East;
eastIntegral = bound(eastIntegral + eastError * dT, eastIntegral = bound(eastIntegral + eastError * dT,
-guidanceSettings.MaxVelIntegral, -guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT],
guidanceSettings.MaxVelIntegral); guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT]);
eastCommand = (eastError * guidanceSettings.VelP + eastCommand = (eastError * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KP] +
eastIntegral * guidanceSettings.VelI - eastIntegral * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KI] -
nedAccel.East * guidanceSettings.VelD); nedAccel.East * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KD]);
// Compute desired down command // Compute desired down command
downError = velocityDesired.Down - velocityActual.Down; downError = velocityDesired.Down - velocityActual.Down;
downIntegral = bound(downIntegral + downError * guidanceSettings.VelPIDUpdatePeriod, downIntegral = bound(downIntegral + downError * dT,
-guidanceSettings.MaxThrottleIntegral, -guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_ILIMIT],
guidanceSettings.MaxThrottleIntegral); guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_ILIMIT]);
downCommand = (downError * guidanceSettings.DownP + downCommand = (downError * guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_KP] +
downIntegral * guidanceSettings.DownI - downIntegral * guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_KI] -
nedAccel.Down * guidanceSettings.DownD); nedAccel.Down * guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_KD]);
attitudeDesired.Throttle = bound(downError * guidanceSettings.DownP + attitudeDesired.Throttle = bound(downCommand, 0, 1);
downIntegral * guidanceSettings.DownI -
nedAccel.Down * guidanceSettings.DownD, 0, 1);
// Project the north and east command signals into the pitch and roll based on yaw. For this to behave well the // 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 // craft should move similarly for 5 deg roll versus 5 deg pitch
attitudeDesired.Pitch = bound(-northCommand * cosf(attitudeActual.Yaw * M_PI / 180) + attitudeDesired.Pitch = bound(-northCommand * cosf(attitudeActual.Yaw * M_PI / 180) +
-eastCommand * sinf(attitudeActual.Yaw * M_PI / 180), -eastCommand * sinf(attitudeActual.Yaw * M_PI / 180),
-stabSettings.PitchMax, stabSettings.PitchMax); -guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch);
attitudeDesired.Roll = bound(-northCommand * sinf(attitudeActual.Yaw * M_PI / 180) + attitudeDesired.Roll = bound(-northCommand * sinf(attitudeActual.Yaw * M_PI / 180) +
eastCommand * cosf(attitudeActual.Yaw * M_PI / 180), eastCommand * cosf(attitudeActual.Yaw * M_PI / 180),
-stabSettings.RollMax, stabSettings.RollMax); -guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch);
if(guidanceSettings.ThrottleControl == GUIDANCESETTINGS_THROTTLECONTROL_FALSE) { if(guidanceSettings.ThrottleControl == GUIDANCESETTINGS_THROTTLECONTROL_FALSE) {
// For now override throttle with manual control. Disable at your risk, quad goes to China. // For now override throttle with manual control. Disable at your risk, quad goes to China.
@ -365,8 +364,8 @@ static void manualSetDesiredVelocity()
GuidanceSettingsData guidanceSettings; GuidanceSettingsData guidanceSettings;
GuidanceSettingsGet(&guidanceSettings); GuidanceSettingsGet(&guidanceSettings);
velocityDesired.North = -guidanceSettings.MaxGroundspeed * cmd.Pitch; velocityDesired.North = -guidanceSettings.HorizontalP[GUIDANCESETTINGS_HORIZONTALP_MAX] * cmd.Pitch;
velocityDesired.East = guidanceSettings.MaxGroundspeed * cmd.Roll; velocityDesired.East = guidanceSettings.HorizontalP[GUIDANCESETTINGS_HORIZONTALP_MAX] * cmd.Roll;
velocityDesired.Down = 0; velocityDesired.Down = 0;
VelocityDesiredSet(&velocityDesired); VelocityDesiredSet(&velocityDesired);

83
ground/src/plugins/uavobjects/velocitydesired.h
View File

@ -1,83 +0,0 @@
/**
******************************************************************************
*
* @file velocitydesired.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: velocitydesired.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 VELOCITYDESIRED_H
#define VELOCITYDESIRED_H
#include "uavdataobject.h"
#include "uavobjectmanager.h"
class UAVOBJECTS_EXPORT VelocityDesired: public UAVDataObject
{
Q_OBJECT
public:
// Field structure
typedef struct {
qint32 North;
qint32 East;
qint32 Down;
} __attribute__((packed)) DataFields;
// Field information
// Field North information
// Field East information
// Field Down information
// Constants
static const quint32 OBJID = 305094202U;
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
VelocityDesired();
DataFields getData();
void setData(const DataFields& data);
Metadata getDefaultMetadata();
UAVDataObject* clone(quint32 instID);
static VelocityDesired* GetInstance(UAVObjectManager* objMngr, quint32 instID = 0);
private:
DataFields data;
void setDefaultFieldValues();
};
#endif // VELOCITYDESIRED_H

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

@ -2,21 +2,13 @@
<object name="GuidanceSettings" singleinstance="true" settings="true"> <object name="GuidanceSettings" singleinstance="true" settings="true">
<description>Settings for the @ref GuidanceModule</description> <description>Settings for the @ref GuidanceModule</description>
<field name="GuidanceMode" units="" type="enum" elements="1" options="DUAL_LOOP,VELOCITY_CONTROL" 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="HorizontalP" units="" type="float" elements="1" elementnames="Kp,Max" defaultvalue="0.1,200"/>
<field name="GroundVelocityP" units="" type="float" elements="1" defaultvalue="0.1"/> <field name="HorizontalVelPID" units="" type="float" elements="4" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.1,0,0,0"/>
<field name="MaxVerticalSpeed" units="cm/s" type="int32" elements="1" defaultvalue="100"/> <field name="VerticalP" units="" type="float" elements="1" elementnames="Kp,Max" defaultvalue="0.1,200"/>
<field name="VertVelocityP" units="" type="float" elements="1" defaultvalue="0.1"/> <field name="VerticalVelPID" units="" type="float" elements="4" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.1,0,0,0"/>
<field name="VelP" units="" type="float" elements="1" defaultvalue="0.1"/>
<field name="VelI" units="" type="float" elements="1" defaultvalue="0.1"/>
<field name="VelD" units="" type="float" elements="1" defaultvalue="0.0"/>
<field name="DownP" units="" type="float" elements="1" defaultvalue="0.0"/>
<field name="DownI" units="" type="float" elements="1" defaultvalue="0.0"/>
<field name="DownD" units="" type="float" elements="1" defaultvalue="0.0"/>
<field name="MaxVelIntegral" units="deg" type="float" elements="1" defaultvalue="2"/>
<field name="MaxThrottleIntegral" units="deg" type="float" elements="1" defaultvalue="1"/>
<field name="ThrottleControl" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="FALSE"/> <field name="ThrottleControl" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="FALSE"/>
<field name="VelUpdatePeriod" units="" type="int32" elements="1" defaultvalue="100"/> <field name="MaxRollPitch" units="deg" type="float" elements="1" defaultvalue="10"/>
<field name="VelPIDUpdatePeriod" units="" type="int32" elements="1" defaultvalue="20"/> <field name="UpdatePeriod" units="" type="int32" elements="1" defaultvalue="100"/>
<access gcs="readwrite" flight="readwrite"/> <access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/> <telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/> <telemetryflight acked="true" updatemode="onchange" period="0"/>