OP-261 Magic Waypoint: Accel feedback for position hold

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2412 ebee16cc-31ac-478f-84a7-5cbb03baadba

flight/OpenPilot/Makefile

@@ -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)

flight/OpenPilot/Modules/Guidance/guidance.c

@@ -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, 
-	northIntegral =
+			      -guidanceSettings.MaxVelIntegral,
-	bound(northIntegral + northError * dT, -guidanceSettings.MaxVelIntegral,
 			      guidanceSettings.MaxVelIntegral);
-	northErrorLast = northError;
+	northCommand = (northError * guidanceSettings.VelP +
-	northCommand =
+			northIntegral * guidanceSettings.VelI -
-	northError * guidanceSettings.VelP + northDerivative * guidanceSettings.VelD + 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 + 
-	eastCommand = eastError * guidanceSettings.VelP + eastDerivative * guidanceSettings.VelD + eastIntegral * guidanceSettings.VelI;
+		       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;
+	if(guidanceSettings.ThrottleControl == GUIDANCESETTINGS_THROTTLECONTROL_FALSE) {
-	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;
+	}
 	
 	AttitudeDesiredSet(&attitudeDesired);
 }

flight/OpenPilot/Modules/Stabilization/stabilization.c

@@ -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

flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj

@@ -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>";

ground/src/plugins/uavobjects/guidancesettings.cpp

@@ -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));
+}

ground/src/plugins/uavobjects/guidancesettings.h

@@ -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

ground/src/plugins/uavobjects/nedaccel.cpp

@@ -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));
+}

ground/src/plugins/uavobjects/systemalarms.h

@@ -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

ground/src/plugins/uavobjects/uavobjects.pro

@@ -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

ground/src/shared/uavobjectdefinition/guidancesettings.xml

@@ -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"/>

ground/src/shared/uavobjectdefinition/nedaccel.xml

@@ -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>

ground/src/shared/uavobjectdefinition/systemalarms.xml

@@ -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"/>