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

OP-261 Magic Waypoint: Accel feedback for position hold

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2412 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2011-01-14 00:51:54 +00:00 committed by peabody124
parent db9bc06a08
commit aa8d9eb178
12 changed files with 468 additions and 236 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
flight/OpenPilot/Makefile
View File

@ -202,6 +202,7 @@ SRC += $(OPUAVOBJ)/flightplanstatus.c
SRC += $(OPUAVOBJ)/flightplansettings.c
SRC += $(OPUAVOBJ)/taskinfo.c
SRC += $(OPUAVOBJ)/watchdogstatus.c
SRC += $(OPUAVOBJ)/nedaccel.c
endif
## PIOS Hardware (STM32F10x)

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

@ -46,23 +46,28 @@
#include "openpilot.h"
#include "guidance.h"
#include "guidancesettings.h"
#include "attituderaw.h"
#include "attitudeactual.h"
#include "attitudedesired.h"
#include "positiondesired.h" // object that will be updated by the module
#include "positionactual.h"
#include "manualcontrolcommand.h"
#include "nedaccel.h"
#include "stabilizationsettings.h"
#include "systemsettings.h"
#include "velocitydesired.h"
#include "velocityactual.h"
#include "CoordinateConversions.h"
// Private constants
#define MAX_QUEUE_SIZE 1
#define STACK_SIZE_BYTES 824
#define TASK_PRIORITY (tskIDLE_PRIORITY+2)
// Private types
// Private variables
static xTaskHandle guidanceTaskHandle;
static xQueueHandle queue;
// Private functions
static void guidanceTask(void *parameters);
@ -79,6 +84,12 @@ static void positionPIDcontrol();
*/
int32_t GuidanceInitialize()
{
// Create object queue
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
// Listen for updates.
AttitudeRawConnectQueue(queue);
// Start main task
xTaskCreate(guidanceTask, (signed char *)"Guidance", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &guidanceTaskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_GUIDANCE, guidanceTaskHandle);
@ -87,11 +98,8 @@ int32_t GuidanceInitialize()
}
static float northIntegral = 0;
static float northErrorLast = 0;
static float eastIntegral = 0;
static float eastErrorLast = 0;
static float downIntegral = 0;
static float downErrorLast = 0;
static uint8_t positionHoldLast = 0;
/**
@ -103,11 +111,70 @@ static void guidanceTask(void *parameters)
GuidanceSettingsData guidanceSettings;
ManualControlCommandData manualControl;
portTickType lastSysTime;
portTickType thisTime;
portTickType lastUpdateTime;
UAVObjEvent ev;
float accel[3] = {0,0,0};
uint32_t accel_accum = 0;
float q[4];
float Rbe[3][3];
float accel_ned[3];
// Main task loop
lastSysTime = xTaskGetTickCount();
lastUpdateTime = xTaskGetTickCount();
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 )
{
AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION,SYSTEMALARMS_ALARM_WARNING);
continue;
}
// Collect downsampled attitude data
AttitudeRawData attitudeRaw;
AttitudeRawGet(&attitudeRaw);
accel[0] += attitudeRaw.gyros_filtered[0];
accel[1] += attitudeRaw.gyros_filtered[1];
accel[2] += attitudeRaw.gyros_filtered[2];
accel_accum++;
// Continue collecting data if not enough time
thisTime = xTaskGetTickCount();
if( (thisTime - lastUpdateTime) < (guidanceSettings.VelUpdatePeriod / portTICK_RATE_MS) )
continue;
lastUpdateTime = xTaskGetTickCount();
accel[0] /= accel_accum;
accel[1] /= accel_accum;
accel[2] /= accel_accum;
//rotate avg accels into earth frame and store it
AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual);
q[0]=attitudeActual.q1;
q[1]=attitudeActual.q2;
q[2]=attitudeActual.q3;
q[3]=attitudeActual.q4;
Quaternion2R(q, Rbe);
for (uint8_t i=0; i<3; i++){
accel_ned[i]=0;
for (uint8_t j=0; j<3; j++)
accel_ned[i] += Rbe[j][i]*accel[j];
}
accel_ned[2] += 9.81;
NedAccelData accelData;
NedAccelGet(&accelData);
// Convert from m/s to cm/s
accelData.North = accel[0] * 100;
accelData.East = accel[1] * 100;
accelData.North = accel[2] * 100;
NedAccelSet(&accelData);
ManualControlCommandGet(&manualControl);
SystemSettingsGet(&systemSettings);
GuidanceSettingsGet(&guidanceSettings);
@ -143,14 +210,13 @@ static void guidanceTask(void *parameters)
} else {
// Be cleaner and get rid of global variables
northIntegral = 0;
northErrorLast = 0;
eastIntegral = 0;
eastErrorLast = 0;
downIntegral = 0;
downErrorLast = 0;
positionHoldLast = 0;
}
vTaskDelayUntil(&lastSysTime, guidanceSettings.VelUpdatePeriod / portTICK_RATE_MS);
}
accel[0] = accel[1] = accel[2] = 0;
accel_accum = 0;
}
}
@ -198,20 +264,19 @@ static void updateVtolDesiredAttitude()
VelocityActualData velocityActual;
AttitudeDesiredData attitudeDesired;
AttitudeActualData attitudeActual;
NedAccelData nedAccel;
GuidanceSettingsData guidanceSettings;
StabilizationSettingsData stabSettings;
SystemSettingsData systemSettings;
float northError;
float northDerivative;
float northCommand;
float eastError;
float eastDerivative;
float eastCommand;
float downError;
float downDerivative;
float downCommand;
// Check how long since last update
if(thisSysTime > lastSysTime) // reuse dt in case of wraparound
@ -227,26 +292,40 @@ static void updateVtolDesiredAttitude()
VelocityDesiredGet(&velocityDesired);
AttitudeActualGet(&attitudeActual);
StabilizationSettingsGet(&stabSettings);
NedAccelGet(&nedAccel);
attitudeDesired.Yaw = 0; // try and face north
// Yaw and pitch output from ground speed PID loop
// Compute desired north command
northError = velocityDesired.North - velocityActual.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;
northIntegral = bound(northIntegral + northError * dT,
-guidanceSettings.MaxVelIntegral,
guidanceSettings.MaxVelIntegral);
northCommand = (northError * guidanceSettings.VelP +
northIntegral * guidanceSettings.VelI -
nedAccel.North * guidanceSettings.VelD);
// Compute desired east command
eastError = velocityDesired.East - velocityActual.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;
eastCommand = (eastError * guidanceSettings.VelP +
eastIntegral * guidanceSettings.VelI -
nedAccel.East * guidanceSettings.VelD);
// 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);
attitudeDesired.Throttle = bound(downError * guidanceSettings.DownP +
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
// craft should move similarly for 5 deg roll versus 5 deg pitch
@ -257,19 +336,12 @@ static void updateVtolDesiredAttitude()
eastCommand * cosf(attitudeActual.Yaw * M_PI / 180),
-stabSettings.RollMax, stabSettings.RollMax);
downError = velocityDesired.Down - velocityActual.Down;
downDerivative = (downError - downErrorLast) / guidanceSettings.VelPIDUpdatePeriod;
downIntegral = bound(downIntegral + downError * guidanceSettings.VelPIDUpdatePeriod,
-guidanceSettings.MaxThrottleIntegral,
guidanceSettings.MaxThrottleIntegral);
downErrorLast = downError;
attitudeDesired.Throttle = bound(downError * guidanceSettings.DownP + downDerivative * guidanceSettings.DownD +
downIntegral * guidanceSettings.DownI, 0, 1);
// For now override throttle with manual control. Disable at your risk, quad goes to China.
ManualControlCommandData manualControl;
ManualControlCommandGet(&manualControl);
attitudeDesired.Throttle = manualControl.Throttle;
if(guidanceSettings.ThrottleControl == GUIDANCESETTINGS_THROTTLECONTROL_FALSE) {
// 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);
}

2
flight/OpenPilot/Modules/Stabilization/stabilization.c
View File

@ -44,7 +44,7 @@
#include "ahrssettings.h"
// Private constants
#define MAX_QUEUE_SIZE 2
#define MAX_QUEUE_SIZE 1
#define STACK_SIZE_BYTES 724
#define TASK_PRIORITY (tskIDLE_PRIORITY+4)
#define FAILSAFE_TIMEOUT_MS 30

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

@ -41,6 +41,7 @@
6549E0D21279B3C800C5476F /* fifo_buffer.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = fifo_buffer.c; sourceTree = "<group>"; };
6549E0D31279B3CF00C5476F /* fifo_buffer.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = fifo_buffer.h; sourceTree = "<group>"; };
655268BC121FBD2900410C6E /* ahrscalibration.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = ahrscalibration.xml; sourceTree = "<group>"; };
656268C612DC1923007B0A0F /* nedaccel.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = nedaccel.xml; sourceTree = "<group>"; };
65632CBF124E09D900469B77 /* guidance.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = guidance.c; sourceTree = "<group>"; };
65632CC1124E09D900469B77 /* guidance.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = guidance.h; sourceTree = "<group>"; };
65632DF51251650300469B77 /* pios_board.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_board.h; sourceTree = "<group>"; };
@ -6783,6 +6784,7 @@
65B367FD121C2620003EAD18 /* systemstats.xml */,
65B367FE121C2620003EAD18 /* telemetrysettings.xml */,
65408AA812BB1648004DACC5 /* i2cstats.xml */,
656268C612DC1923007B0A0F /* nedaccel.xml */,
);
path = uavobjectdefinition;
sourceTree = "<group>";

199
ground/src/plugins/uavobjects/guidancesettings.cpp Normal file
View File

@ -0,0 +1,199 @@
/**
******************************************************************************
*
* @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));
}

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

@ -1,109 +0,0 @@
/**
******************************************************************************
*
* @file guidancesettings.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: 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
*/
#ifndef GUIDANCESETTINGS_H
#define GUIDANCESETTINGS_H
#include "uavdataobject.h"
#include "uavobjectmanager.h"
class UAVOBJECTS_EXPORT GuidanceSettings: public UAVDataObject
{
Q_OBJECT
public:
// Field structure
typedef struct {
quint8 GuidanceMode;
qint32 MaxGroundspeed;
float GroundVelocityP;
qint32 MaxVerticalSpeed;
float VertVelocityP;
float VelP;
float VelI;
float VelD;
float DownP;
float DownI;
float DownD;
float MaxVelIntegral;
float MaxThrottleIntegral;
qint32 VelUpdatePeriod;
qint32 VelPIDUpdatePeriod;
} __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
// Field VertVelocityP information
// Field VelP information
// Field VelI information
// Field VelD information
// Field DownP information
// Field DownI information
// Field DownD information
// Field MaxVelIntegral information
// Field MaxThrottleIntegral information
// Field VelUpdatePeriod information
// Field VelPIDUpdatePeriod information
// Constants
static const quint32 OBJID = 2071403670U;
static const QString NAME;
static const QString DESCRIPTION;
static const bool ISSINGLEINST = 1;
static const bool ISSETTINGS = 1;
static const quint32 NUMBYTES = sizeof(DataFields);
// Functions
GuidanceSettings();
DataFields getData();
void setData(const DataFields& data);
Metadata getDefaultMetadata();
UAVDataObject* clone(quint32 instID);
static GuidanceSettings* GetInstance(UAVObjectManager* objMngr, quint32 instID = 0);
private:
DataFields data;
void setDefaultFieldValues();
};
#endif // GUIDANCESETTINGS_H

137
ground/src/plugins/uavobjects/nedaccel.cpp Normal file
View File

@ -0,0 +1,137 @@
/**
******************************************************************************
*
* @file nedaccel.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: nedaccel.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 "nedaccel.h"
#include "uavobjectfield.h"
const QString NedAccel::NAME = QString("NedAccel");
const QString NedAccel::DESCRIPTION = QString("The projection of acceleration in the NED reference frame used by @ref Guidance.");
/**
* Constructor
*/
NedAccel::NedAccel(): UAVDataObject(OBJID, ISSINGLEINST, ISSETTINGS, NAME)
{
// Create fields
QList<UAVObjectField*> fields;
QStringList NorthElemNames;
NorthElemNames.append("0");
fields.append( new UAVObjectField(QString("North"), QString("cm/s"), UAVObjectField::FLOAT32, NorthElemNames, QStringList()) );
QStringList EastElemNames;
EastElemNames.append("0");
fields.append( new UAVObjectField(QString("East"), QString("cm/s"), UAVObjectField::FLOAT32, EastElemNames, QStringList()) );
QStringList DownElemNames;
DownElemNames.append("0");
fields.append( new UAVObjectField(QString("Down"), QString("cm/s"), UAVObjectField::FLOAT32, DownElemNames, 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 NedAccel::getDefaultMetadata()
{
UAVObject::Metadata metadata;
metadata.flightAccess = ACCESS_READWRITE;
metadata.gcsAccess = ACCESS_READWRITE;
metadata.gcsTelemetryAcked = 0;
metadata.gcsTelemetryUpdateMode = UAVObject::UPDATEMODE_MANUAL;
metadata.gcsTelemetryUpdatePeriod = 0;
metadata.flightTelemetryAcked = 0;
metadata.flightTelemetryUpdateMode = UAVObject::UPDATEMODE_PERIODIC;
metadata.flightTelemetryUpdatePeriod = 10001;
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 NedAccel::setDefaultFieldValues()
{
}
/**
* Get the object data fields
*/
NedAccel::DataFields NedAccel::getData()
{
QMutexLocker locker(mutex);
return data;
}
/**
* Set the object data fields
*/
void NedAccel::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* NedAccel::clone(quint32 instID)
{
NedAccel* obj = new NedAccel();
obj->initialize(instID, this->getMetaObject());
return obj;
}
/**
* Static function to retrieve an instance of the object.
*/
NedAccel* NedAccel::GetInstance(UAVObjectManager* objMngr, quint32 instID)
{
return dynamic_cast<NedAccel*>(objMngr->getObject(NedAccel::OBJID, instID));
}

85
ground/src/plugins/uavobjects/systemalarms.h
View File

@ -1,85 +0,0 @@
/**
******************************************************************************
*
* @file systemalarms.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: systemalarms.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 SYSTEMALARMS_H
#define SYSTEMALARMS_H
#include "uavdataobject.h"
#include "uavobjectmanager.h"
class UAVOBJECTS_EXPORT SystemAlarms: public UAVDataObject
{
Q_OBJECT
public:
// Field structure
typedef struct {
quint8 Alarm[14];
} __attribute__((packed)) DataFields;
// Field information
// Field Alarm information
/* Enumeration options for field Alarm */
typedef enum { ALARM_OK=0, ALARM_WARNING=1, ALARM_ERROR=2, ALARM_CRITICAL=3, ALARM_UNINITIALISED=4 } AlarmOptions;
/* Array element names for field Alarm */
typedef enum { ALARM_OUTOFMEMORY=0, ALARM_STACKOVERFLOW=1, ALARM_CPUOVERLOAD=2, ALARM_EVENTSYSTEM=3, ALARM_SDCARD=4, ALARM_TELEMETRY=5, ALARM_MANUALCONTROL=6, ALARM_ACTUATOR=7, ALARM_STABILIZATION=8, ALARM_AHRSCOMMS=9, ALARM_BATTERY=10, ALARM_FLIGHTTIME=11, ALARM_I2C=12, ALARM_GPS=13 } AlarmElem;
/* Number of elements for field Alarm */
static const quint32 ALARM_NUMELEM = 14;
// Constants
static const quint32 OBJID = 2311314672U;
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
SystemAlarms();
DataFields getData();
void setData(const DataFields& data);
Metadata getDefaultMetadata();
UAVDataObject* clone(quint32 instID);
static SystemAlarms* GetInstance(UAVObjectManager* objMngr, quint32 instID = 0);
private:
DataFields data;
void setDefaultFieldValues();
};
#endif // SYSTEMALARMS_H

6
ground/src/plugins/uavobjects/uavobjects.pro
View File

@ -53,7 +53,8 @@ HEADERS += uavobjects_global.h \
flightplanstatus.h \
flightplansettings.h \
flightplancontrol.h \
watchdogstatus.h
watchdogstatus.h \
nedaccel.h
SOURCES += uavobject.cpp \
uavmetaobject.cpp \
@ -104,6 +105,7 @@ SOURCES += uavobject.cpp \
flightplanstatus.cpp \
flightplansettings.cpp \
flightplancontrol.cpp \
watchdogstatus.cpp
watchdogstatus.cpp \
nedaccel.cpp
OTHER_FILES += UAVObjects.pluginspec

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

@ -14,6 +14,7 @@
<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="VelUpdatePeriod" units="" type="int32" elements="1" defaultvalue="100"/>
<field name="VelPIDUpdatePeriod" units="" type="int32" elements="1" defaultvalue="20"/>
<access gcs="readwrite" flight="readwrite"/>

12
ground/src/shared/uavobjectdefinition/nedaccel.xml Normal file
View File

@ -0,0 +1,12 @@
<xml>
<object name="NedAccel" singleinstance="true" settings="false">
<description>The projection of acceleration in the NED reference frame used by @ref Guidance.</description>
<field name="North" units="cm/s" type="float" elements="1"/>
<field name="East" units="cm/s" type="float" elements="1"/>
<field name="Down" units="cm/s" type="float" elements="1"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="10001"/>
<logging updatemode="never" period="0"/>
</object>
</xml>

2
ground/src/shared/uavobjectdefinition/systemalarms.xml
View File

@ -2,7 +2,7 @@
<object name="SystemAlarms" singleinstance="true" settings="false">
<description>Alarms from OpenPilot to indicate failure conditions or warnings. Set by various modules.</description>
<field name="Alarm" units="" type="enum" options="OK,Warning,Error,Critical,Uninitialised"
elementnames="OutOfMemory,StackOverflow,CPUOverload,EventSystem,SDCard,Telemetry,ManualControl,Actuator,Stabilization,AHRSComms,Battery,FlightTime,I2C,GPS" defaultvalue="Uninitialised"/>
elementnames="OutOfMemory,StackOverflow,CPUOverload,EventSystem,SDCard,Telemetry,ManualControl,Actuator,Stabilization,Guidance,AHRSComms,Battery,FlightTime,I2C,GPS" defaultvalue="Uninitialised"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="periodic" period="4000"/>