diff --git a/flight/OpenPilot/Modules/Guidance/guidance.c b/flight/OpenPilot/Modules/Guidance/guidance.c index 146bc9119..afc4fd9a8 100644 --- a/flight/OpenPilot/Modules/Guidance/guidance.c +++ b/flight/OpenPilot/Modules/Guidance/guidance.c @@ -126,11 +126,12 @@ static void guidanceTask(void *parameters) while (1) { // 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); - continue; - } + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_WARNING); + } else { + AlarmsClear(SYSTEMALARMS_ALARM_GUIDANCE); + } // Collect downsampled attitude data AttitudeRawData attitudeRaw; @@ -142,7 +143,7 @@ static void guidanceTask(void *parameters) // Continue collecting data if not enough time thisTime = xTaskGetTickCount(); - if( (thisTime - lastUpdateTime) < (guidanceSettings.VelUpdatePeriod / portTICK_RATE_MS) ) + if( (thisTime - lastUpdateTime) < (guidanceSettings.UpdatePeriod / portTICK_RATE_MS) ) continue; lastUpdateTime = xTaskGetTickCount(); @@ -238,16 +239,16 @@ void updateVtolDesiredVelocity() float dEast = positionDesired.East - positionActual.East; float distance = sqrt(pow(dNorth, 2) + pow(dEast, 2)); float heading = atan2f(dEast, dNorth); - float groundspeed = bound(guidanceSettings.GroundVelocityP * distance, - 0, guidanceSettings.MaxGroundspeed); + float groundspeed = bound(distance * guidanceSettings.HorizontalP[GUIDANCESETTINGS_HORIZONTALP_KP], + 0, guidanceSettings.HorizontalP[GUIDANCESETTINGS_HORIZONTALP_MAX]); velocityDesired.North = groundspeed * cosf(heading); velocityDesired.East = groundspeed * sinf(heading); float dDown = positionDesired.Down - positionActual.Down; - velocityDesired.Down = bound(guidanceSettings.VertVelocityP * dDown, - -guidanceSettings.MaxVerticalSpeed, - guidanceSettings.MaxVerticalSpeed); + velocityDesired.Down = bound(dDown * guidanceSettings.VerticalP[GUIDANCESETTINGS_VERTICALP_KP], + -guidanceSettings.VerticalP[GUIDANCESETTINGS_VERTICALP_MAX], + guidanceSettings.VerticalP[GUIDANCESETTINGS_VERTICALP_MAX]); VelocityDesiredSet(&velocityDesired); } @@ -304,42 +305,40 @@ static void updateVtolDesiredAttitude() // Compute desired north command northError = velocityDesired.North - velocityActual.North; northIntegral = bound(northIntegral + northError * dT, - -guidanceSettings.MaxVelIntegral, - guidanceSettings.MaxVelIntegral); - northCommand = (northError * guidanceSettings.VelP + - northIntegral * guidanceSettings.VelI - - nedAccel.North * guidanceSettings.VelD); + -guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT], + guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT]); + northCommand = (northError * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KP] + + northIntegral * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KI] - + nedAccel.North * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KD]); // Compute desired east command eastError = velocityDesired.East - velocityActual.East; eastIntegral = bound(eastIntegral + eastError * dT, - -guidanceSettings.MaxVelIntegral, - guidanceSettings.MaxVelIntegral); - eastCommand = (eastError * guidanceSettings.VelP + - eastIntegral * guidanceSettings.VelI - - nedAccel.East * guidanceSettings.VelD); + -guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT], + guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT]); + eastCommand = (eastError * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KP] + + eastIntegral * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KI] - + nedAccel.East * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KD]); // Compute desired down command downError = velocityDesired.Down - velocityActual.Down; - downIntegral = bound(downIntegral + downError * guidanceSettings.VelPIDUpdatePeriod, - -guidanceSettings.MaxThrottleIntegral, - guidanceSettings.MaxThrottleIntegral); - downCommand = (downError * guidanceSettings.DownP + - downIntegral * guidanceSettings.DownI - - nedAccel.Down * guidanceSettings.DownD); + downIntegral = bound(downIntegral + downError * dT, + -guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_ILIMIT], + guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_ILIMIT]); + downCommand = (downError * guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_KP] + + downIntegral * guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_KI] - + nedAccel.Down * guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_KD]); - attitudeDesired.Throttle = bound(downError * guidanceSettings.DownP + - downIntegral * guidanceSettings.DownI - - nedAccel.Down * guidanceSettings.DownD, 0, 1); + attitudeDesired.Throttle = bound(downCommand, 0, 1); // 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); + -guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch); attitudeDesired.Roll = bound(-northCommand * sinf(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) { // For now override throttle with manual control. Disable at your risk, quad goes to China. @@ -365,8 +364,8 @@ static void manualSetDesiredVelocity() GuidanceSettingsData guidanceSettings; GuidanceSettingsGet(&guidanceSettings); - velocityDesired.North = -guidanceSettings.MaxGroundspeed * cmd.Pitch; - velocityDesired.East = guidanceSettings.MaxGroundspeed * cmd.Roll; + velocityDesired.North = -guidanceSettings.HorizontalP[GUIDANCESETTINGS_HORIZONTALP_MAX] * cmd.Pitch; + velocityDesired.East = guidanceSettings.HorizontalP[GUIDANCESETTINGS_HORIZONTALP_MAX] * cmd.Roll; velocityDesired.Down = 0; VelocityDesiredSet(&velocityDesired); diff --git a/ground/src/plugins/uavobjects/velocitydesired.h b/ground/src/plugins/uavobjects/velocitydesired.h deleted file mode 100644 index 9144a11de..000000000 --- a/ground/src/plugins/uavobjects/velocitydesired.h +++ /dev/null @@ -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 diff --git a/ground/src/shared/uavobjectdefinition/guidancesettings.xml b/ground/src/shared/uavobjectdefinition/guidancesettings.xml index a74040dcf..3aa4c003a 100644 --- a/ground/src/shared/uavobjectdefinition/guidancesettings.xml +++ b/ground/src/shared/uavobjectdefinition/guidancesettings.xml @@ -2,21 +2,13 @@ Settings for the @ref GuidanceModule - - - - - - - - - - - - + + + + - - + +