Merge branch 'sim' into testing

Conflicts:
	ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro

flight/Modules/Attitude/revolution/attitude.c

@@ -58,6 +58,7 @@
 #include "baroaltitude.h"
 #include "flightstatus.h"
 #include "gpsposition.h"
+#include "gpsvelocity.h"
 #include "gyros.h"
 #include "gyrosbias.h"
 #include "homelocation.h"
@@ -86,6 +87,7 @@ static xQueueHandle accelQueue;
 static xQueueHandle magQueue;
 static xQueueHandle baroQueue;
 static xQueueHandle gpsQueue;
+static xQueueHandle gpsVelQueue;
 
 static AttitudeSettingsData attitudeSettings;
 static HomeLocationData homeLocation;
@@ -165,6 +167,7 @@ int32_t AttitudeStart(void)
 	magQueue = xQueueCreate(1, sizeof(UAVObjEvent));
 	baroQueue = xQueueCreate(1, sizeof(UAVObjEvent));
 	gpsQueue = xQueueCreate(1, sizeof(UAVObjEvent));
+	gpsVelQueue = xQueueCreate(1, sizeof(UAVObjEvent));
 
 	// Start main task
 	xTaskCreate(AttitudeTask, (signed char *)"Attitude", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &attitudeTaskHandle);
@@ -176,7 +179,8 @@ int32_t AttitudeStart(void)
 	MagnetometerConnectQueue(magQueue);
 	BaroAltitudeConnectQueue(baroQueue);
 	GPSPositionConnectQueue(gpsQueue);
-	
+	GPSVelocityConnectQueue(gpsVelQueue);
+
 	return 0;
 }
 
@@ -468,12 +472,14 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
 	MagnetometerData magData;
 	BaroAltitudeData baroData;
 	GPSPositionData gpsData;
+	GPSVelocityData gpsVelData;
 	GyrosBiasData gyrosBias;
 	HomeLocationData home;
 
 	static bool mag_updated;
 	static bool baro_updated;
 	static bool gps_updated;
+	static bool gps_vel_updated;
 
 	static uint32_t ins_last_time = 0;
 	static bool inited;
@@ -508,6 +514,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
 	mag_updated |= (xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && homeLocation.Set == HOMELOCATION_SET_TRUE;
 	baro_updated |= xQueueReceive(baroQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE;
 	gps_updated |= (xQueueReceive(gpsQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode;
+	gps_vel_updated |= (xQueueReceive(gpsVelQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode;
 
 	// Get most recent data
 	GyrosGet(&gyrosData);
@@ -515,6 +522,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
 	MagnetometerGet(&magData);
 	BaroAltitudeGet(&baroData);
 	GPSPositionGet(&gpsData);
+	GPSVelocityGet(&gpsVelData);
 	HomeLocationGet(&home);
 
 	// Have a minimum requirement for gps usage
@@ -617,9 +625,9 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
 	// Because the sensor module remove the bias we need to add it
 	// back in here so that the INS algorithm can track it correctly 
 	GyrosBiasGet(&gyrosBias);
-	float gyros[3] = {(gyrosData.x + gyrosBias.x) * F_PI / 180.0f, 
-		(gyrosData.y + gyrosBias.y) * F_PI / 180.0f, 
-		(gyrosData.z + gyrosBias.z) * F_PI / 180.0f};
+	float gyros[3] = {(gyrosData.x - gyrosBias.x) * F_PI / 180.0f, 
+		(gyrosData.y - gyrosBias.y) * F_PI / 180.0f, 
+		(gyrosData.z - gyrosBias.z) * F_PI / 180.0f};
 
 	// Advance the state estimate
 	INSStatePrediction(gyros, &accelsData.x, dT);
@@ -635,9 +643,9 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
 	AttitudeActualSet(&attitude);
 
 	// Copy the gyro bias into the UAVO
-	gyrosBias.x = Nav.gyro_bias[0];
-	gyrosBias.y = Nav.gyro_bias[1];
-	gyrosBias.z = Nav.gyro_bias[2];
+	gyrosBias.x = -Nav.gyro_bias[0] * 180.0f / F_PI;
+	gyrosBias.y = -Nav.gyro_bias[1] * 180.0f / F_PI;
+	gyrosBias.z = -Nav.gyro_bias[2] * 180.0f / F_PI;
 	GyrosBiasSet(&gyrosBias);
 
 	// Advance the covariance estimate
@@ -658,10 +666,15 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
 		GPSPositionData gpsPosition;
 		GPSPositionGet(&gpsPosition);
 
-		vel[0] = gpsPosition.Groundspeed * cosf(gpsPosition.Heading * F_PI / 180.0f);
-		vel[1] = gpsPosition.Groundspeed * sinf(gpsPosition.Heading * F_PI / 180.0f);
-		vel[2] = 0;
-
+		if (0) {
+			vel[0] = gpsPosition.Groundspeed * cosf(gpsPosition.Heading * F_PI / 180.0f);
+			vel[1] = gpsPosition.Groundspeed * sinf(gpsPosition.Heading * F_PI / 180.0f);
+			vel[2] = 0;
+		} else {
+			vel[0] = gpsVelData.North;
+			vel[1] = gpsVelData.East;
+			vel[2] = gpsVelData.Down;
+		}
 		// Transform the GPS position into NED coordinates
 		getNED(&gpsPosition, NED);
 

flight/Modules/GPS/GPS.c

@@ -36,11 +36,14 @@
 #include <stdbool.h>
 
 #include "NMEA.h"
+#include "UBX.h"
+
 
 #include "gpsposition.h"
 #include "homelocation.h"
 #include "gpstime.h"
 #include "gpssatellites.h"
+#include "gpsvelocity.h"
 #include "WorldMagModel.h"
 #include "CoordinateConversions.h"
 #include "hwsettings.h"
@@ -135,8 +138,20 @@ int32_t GPSInitialize(void)
 		gpsEnabled = false;
 #endif
 
+#if defined(REVOLUTION)
+	// Revolution expects these objects to always be defined.  Not doing so will fail some
+	// queue connections in navigation
+	GPSPositionInitialize();
+	GPSVelocityInitialize();
+	GPSTimeInitialize();
+	GPSSatellitesInitialize();
+	HomeLocationInitialize();
+	updateSettings();
+
+#else
 	if (gpsPort && gpsEnabled) {
 		GPSPositionInitialize();
+		GPSVelocityInitialize();
 #if !defined(PIOS_GPS_MINIMAL)
 		GPSTimeInitialize();
 		GPSSatellitesInitialize();
@@ -145,7 +160,10 @@ int32_t GPSInitialize(void)
 		HomeLocationInitialize();
 #endif
 		updateSettings();
+	}
+#endif
 
+	if (gpsPort && gpsEnabled) {
 		gps_rx_buffer = pvPortMalloc(NMEA_MAX_PACKET_LENGTH);
 		PIOS_Assert(gps_rx_buffer);
 
@@ -167,10 +185,14 @@ static void gpsTask(void *parameters)
 	portTickType xDelay = 100 / portTICK_RATE_MS;
 	uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;;
 	GPSPositionData GpsData;
+	UBXPacket *ubx = (UBXPacket *)gps_rx_buffer;
 	
 	uint8_t rx_count = 0;
-	bool start_flag = false;
+//	bool start_flag = false;
 	bool found_cr = false;
+	enum proto_states {START,NMEA,UBX_SY2,UBX_CLASS,UBX_ID,UBX_LEN1,
+		UBX_LEN2,UBX_PAYLOAD,UBX_CHK1,UBX_CHK2};
+	enum proto_states proto_state = START;
 	int32_t gpsRxOverflow = 0;
 	
 	numUpdates = 0;
@@ -184,6 +206,7 @@ static void gpsTask(void *parameters)
 	while (1)
 	{
 		uint8_t c;
+
 		// NMEA or SINGLE-SENTENCE GPS mode
 
 		// This blocks the task until there is something on the buffer
@@ -191,22 +214,94 @@ static void gpsTask(void *parameters)
 		{
 		
 			// detect start while acquiring stream
-			if (!start_flag && (c == '$'))
+			switch (proto_state)
 			{
-				start_flag = true;
-				found_cr = false;
-				rx_count = 0;
+				case START: // detect protocol
+					switch (c)
+					{
+						case UBX_SYNC1: // first UBX sync char found
+							proto_state = UBX_SY2;
+							continue;
+						case '$': // NMEA identifier found
+							proto_state = NMEA;
+							found_cr = false;
+							rx_count = 0;
+							break;
+						default:
+							continue;
+					}
+					break;
+				case UBX_SY2:
+					if (c == UBX_SYNC2) // second UBX sync char found
+					{
+						proto_state = UBX_CLASS;
+						found_cr = false;
+						rx_count = 0;
+					}
+					else
+					{
+						proto_state = START; // reset state
+					}
+					continue;
+				case UBX_CLASS:
+					ubx->header.class = c;
+					proto_state = UBX_ID;
+					continue;
+				case UBX_ID:
+					ubx->header.id = c;
+					proto_state = UBX_LEN1;
+					continue;
+				case UBX_LEN1:
+					ubx->header.len = c;
+					proto_state = UBX_LEN2;
+					continue;
+				case UBX_LEN2:
+					ubx->header.len += (c << 8);
+					if ((sizeof (UBXHeader)) + ubx->header.len > NMEA_MAX_PACKET_LENGTH)
+					{
+						gpsRxOverflow++;
+						proto_state = START;
+						found_cr = false;
+						rx_count = 0;
+					}
+					else
+					{
+						proto_state = UBX_PAYLOAD;
+					}
+					continue;
+				case UBX_PAYLOAD:
+					if (rx_count < ubx->header.len)
+					{
+						ubx->payload.payload[rx_count] = c;
+						if (++rx_count == ubx->header.len)
+							proto_state = UBX_CHK1;
+					}
+					else
+						proto_state = START;
+					continue;
+				case UBX_CHK1:
+					ubx->header.ck_a = c;
+					proto_state = UBX_CHK2;
+					continue;
+				case UBX_CHK2:
+					ubx->header.ck_b = c;
+					if (checksum_ubx_message(ubx))
+					{
+						parse_ubx_message(ubx);
+					}
+					proto_state = START;
+					continue;
+				case NMEA:
+					break;
 			}
-			else
-			if (!start_flag)
-				continue;
-		
+
+
 			if (rx_count >= NMEA_MAX_PACKET_LENGTH)
 			{
 				// The buffer is already full and we haven't found a valid NMEA sentence.
 				// Flush the buffer and note the overflow event.
 				gpsRxOverflow++;
-				start_flag = false;
+				proto_state = START;
 				found_cr = false;
 				rx_count = 0;
 			}
@@ -230,7 +325,7 @@ static void gpsTask(void *parameters)
 				gps_rx_buffer[rx_count-2] = 0;
 
 				// prepare to parse next sentence
-				start_flag = false;
+				proto_state = START;
 				found_cr = false;
 				rx_count = 0;
 				// Our rxBuffer must look like this now:

flight/Modules/GPS/UBX.c

@@ -0,0 +1,97 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup GSPModule GPS Module
+ * @brief Process GPS information (UBX binary format)
+ * @{
+ *
+ * @file       UBX.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
+ * @brief      GPS module, handles GPS and NMEA stream
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * 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 "openpilot.h"
+#include "pios.h"
+#include "UBX.h"
+#include "gpsvelocity.h"
+
+bool checksum_ubx_message (UBXPacket *ubx)
+{
+	int i;
+	uint8_t ck_a, ck_b;
+
+	ck_a = ubx->header.class;
+	ck_b = ck_a;
+
+	ck_a += ubx->header.id;
+	ck_b += ck_a;
+
+	ck_a += ubx->header.len & 0xff;
+	ck_b += ck_a;
+
+	ck_a += ubx->header.len >> 8;
+	ck_b += ck_a;
+
+
+
+	for (i = 0; i < ubx->header.len; i++)
+	{
+		ck_a += ubx->payload.payload[i];
+		ck_b += ck_a;
+	}
+
+	if (ubx->header.ck_a == ck_a &&
+			ubx->header.ck_b == ck_b)
+	{
+		return true;
+	}
+	else
+		return false;
+
+}
+
+void parse_ubx_nav_velned (UBXPayload payload)
+{
+	GPSVelocityData GpsVelocity;
+	GPSVelocityGet(&GpsVelocity);
+
+	GpsVelocity.North	= (float)payload.nav_velned.velN/100.0;
+	GpsVelocity.East	= (float)payload.nav_velned.velE/100.0;
+	GpsVelocity.Down	= (float)payload.nav_velned.velD/100.0;
+
+	GPSVelocitySet(&GpsVelocity);
+}
+
+void parse_ubx_message (UBXPacket *ubx)
+{
+	switch (ubx->header.class)
+	{
+		case UBX_CLASS_NAV:
+			switch (ubx->header.id)
+			{
+				case UBX_ID_VELNED:
+					parse_ubx_nav_velned (ubx->payload);
+			}
+			break;
+	}
+}
+
+

flight/Modules/GPS/inc/UBX.h

@@ -0,0 +1,80 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup GSPModule GPS Module
+ * @brief Process GPS information
+ * @{
+ *
+ * @file       UBX.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      GPS module, handles GPS and NMEA stream
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * 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 UBX_H
+#define UBX_H
+#include "openpilot.h"
+
+#define UBX_SYNC1						0xb5 // UBX protocol synchronization characters
+#define UBX_SYNC2						0x62
+
+// From u-blox6 receiver protocol specification
+
+// Messages classes
+#define UBX_CLASS_NAV	0x01
+
+// Message IDs
+#define	UBX_ID_VELNED	0x12
+
+// private structures
+
+typedef struct  {
+	uint32_t	iTOW;	// ms GPS Millisecond Time of Week
+	int32_t		velN;	// cm/s NED north velocity
+	int32_t		velE;	// cm/s NED east velocity
+	int32_t		velD;	// cm/s NED down velocity
+	uint32_t	speed;	// cm/s Speed (3-D)
+	uint32_t	gSpeed;	// cm/s Ground Speed (2-D)
+	int32_t		heading;	// 1e-5 *deg Heading of motion 2-D
+	uint32_t	sAcc;	// cm/s Speed Accuracy Estimate
+	uint32_t	cAcc;	// 1e-5 *deg Course / Heading Accuracy Estimate
+} UBX_NAV_VELNED;
+
+typedef union {		// add more message types later
+	uint8_t		payload[0];
+	UBX_NAV_VELNED	nav_velned;
+} UBXPayload;
+
+typedef struct {
+	uint8_t 	class;
+	uint8_t 	id;
+	uint16_t	len;
+	uint8_t 	ck_a;
+	uint8_t 	ck_b;
+} UBXHeader;
+
+typedef struct {
+	UBXHeader	header;
+	UBXPayload	payload;
+} UBXPacket;
+
+bool checksum_ubx_message(UBXPacket *);
+void parse_ubx_message(UBXPacket *);
+#endif /* UBX_H */

flight/Modules/Guidance/guidance.c

@@ -54,6 +54,8 @@
 #include "positionactual.h"
 #include "manualcontrol.h"
 #include "flightstatus.h"
+#include "gpsvelocity.h"
+#include "gpsposition.h"
 #include "guidancesettings.h"
 #include "nedaccel.h"
 #include "stabilizationdesired.h"
@@ -67,6 +69,7 @@
 #define MAX_QUEUE_SIZE 4
 #define STACK_SIZE_BYTES 1548
 #define TASK_PRIORITY (tskIDLE_PRIORITY+2)
+#define F_PI 3.14159265358979323846f
 // Private types
 
 // Private variables
@@ -297,7 +300,6 @@ void updateVtolDesiredVelocity()
 	float northCommand;
 	float eastCommand;
 	float downCommand;
-	
 
 	// Check how long since last update
 	if(thisSysTime > lastSysTime) // reuse dt in case of wraparound
@@ -321,7 +323,7 @@ void updateVtolDesiredVelocity()
 		       eastPosIntegral);
 	
 	
-	float total_vel = sqrtf(powf(velocityDesired.North,2) + powf(velocityDesired.East,2));
+	float total_vel = sqrtf(powf(northCommand,2) + powf(eastCommand,2));
 	float scale = 1;
 	if(total_vel > guidanceSettings.HorizontalVelMax)
 		scale = guidanceSettings.HorizontalVelMax / total_vel;
@@ -388,31 +390,60 @@ static void updateVtolDesiredAttitude()
 	StabilizationSettingsGet(&stabSettings);
 	NedAccelGet(&nedAccel);
 	
+	float northVel, eastVel, downVel;
+	switch (guidanceSettings.VelocitySource) {
+		case GUIDANCESETTINGS_VELOCITYSOURCE_EKF:
+			northVel = velocityActual.North;
+			eastVel = velocityActual.East;
+			downVel = velocityActual.Down;
+			break;
+		case GUIDANCESETTINGS_VELOCITYSOURCE_NEDVEL:
+		{
+			GPSVelocityData gpsVelocity;
+			GPSVelocityGet(&gpsVelocity);
+			northVel = gpsVelocity.North;
+			eastVel = gpsVelocity.East;
+			downVel = gpsVelocity.Down;
+		}
+		case GUIDANCESETTINGS_VELOCITYSOURCE_GPSPOS:
+		{
+			GPSPositionData gpsPosition;
+			GPSPositionGet(&gpsPosition);
+			northVel = gpsPosition.Groundspeed * cosf(gpsPosition.Heading * F_PI / 180.0f);
+			eastVel = gpsPosition.Groundspeed * sinf(gpsPosition.Heading * F_PI / 180.0f);
+			downVel = velocityActual.Down;
+		}
+		default:
+			break;
+	}
+	
 	// Testing code - refactor into manual control command
 	ManualControlCommandData manualControlData;
 	ManualControlCommandGet(&manualControlData);
 	stabDesired.Yaw = stabSettings.MaximumRate[STABILIZATIONSETTINGS_MAXIMUMRATE_YAW] * manualControlData.Yaw;	
 	
 	// Compute desired north command
-	northError = velocityDesired.North - velocityActual.North;
+	northError = velocityDesired.North - northVel;
 	northVelIntegral = bound(northVelIntegral + northError * dT * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KI], 
 			      -guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT],
 			      guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT]);
 	northCommand = (northError * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KP] +
 			northVelIntegral -
-			nedAccel.North * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KD]);
+			nedAccel.North * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KD] +
+			velocityDesired.North * guidanceSettings.VelocityFeedforward);
 	
 	// Compute desired east command
-	eastError = velocityDesired.East - velocityActual.East;
+	eastError = velocityDesired.East - eastVel;
 	eastVelIntegral = bound(eastVelIntegral + eastError * dT * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KI], 
 			     -guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT],
 			     guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT]);
 	eastCommand = (eastError * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KP] + 
 		       eastVelIntegral - 
-		       nedAccel.East * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KD]);
+		       nedAccel.East * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KD] +
+			   velocityDesired.East * guidanceSettings.VelocityFeedforward);
 	
 	// Compute desired down command
-	downError = velocityDesired.Down - velocityActual.Down;
+	downError = velocityDesired.Down - downVel;
 	// Must flip this sign 
 	downError = -downError;
 	downVelIntegral = bound(downVelIntegral + downError * dT * guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_KI], 

flight/Modules/Sensors/simulated/sensors.c

@@ -59,6 +59,7 @@
 #include "gyrosbias.h"
 #include "flightstatus.h"
 #include "gpsposition.h"
+#include "gpsvelocity.h"
 #include "homelocation.h"
 #include "magnetometer.h"
 #include "ratedesired.h"
@@ -100,6 +101,7 @@ int32_t SensorsInitialize(void)
 	GyrosInitialize();
 	GyrosBiasInitialize();
 	GPSPositionInitialize();
+	GPSVelocityInitialize();
 	MagnetometerInitialize();
 	RevoCalibrationInitialize();
 
@@ -467,6 +469,13 @@ static void simulateModelQuadcopter()
 		gpsPosition.PDOP = 1;
 		GPSPositionSet(&gpsPosition);
 		last_gps_time = PIOS_DELAY_GetRaw();
+
+		GPSVelocityData gpsVelocity;
+		GPSVelocityGet(&gpsVelocity);
+		gpsVelocity.North = vel[0] + gps_vel_drift[0];
+		gpsVelocity.East = vel[1] + gps_vel_drift[1];
+		gpsVelocity.Down = vel[2] + gps_vel_drift[2];
+		GPSVelocitySet(&gpsVelocity);
 	}
 
 	// Update mag periodically

flight/Revolution/UAVObjects.inc

@@ -47,6 +47,7 @@ UAVOBJSRCFILENAMES += gcstelemetrystats
 UAVOBJSRCFILENAMES += gpsposition
 UAVOBJSRCFILENAMES += gpssatellites
 UAVOBJSRCFILENAMES += gpstime
+UAVOBJSRCFILENAMES += gpsvelocity
 UAVOBJSRCFILENAMES += guidancesettings
 UAVOBJSRCFILENAMES += homelocation
 UAVOBJSRCFILENAMES += i2cstats

ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro

@@ -55,6 +55,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
     $$UAVOBJECT_SYNTHETICS/gpstime.h \
     $$UAVOBJECT_SYNTHETICS/gpssatellites.h \
     $$UAVOBJECT_SYNTHETICS/pathdesired.h \
+    $$UAVOBJECT_SYNTHETICS/gpsvelocity.h \
     $$UAVOBJECT_SYNTHETICS/positionactual.h \
     $$UAVOBJECT_SYNTHETICS/flightbatterystate.h \
     $$UAVOBJECT_SYNTHETICS/homelocation.h \
@@ -119,6 +120,7 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
     $$UAVOBJECT_SYNTHETICS/gpstime.cpp \
     $$UAVOBJECT_SYNTHETICS/gpssatellites.cpp \
     $$UAVOBJECT_SYNTHETICS/pathdesired.cpp \
+    $$UAVOBJECT_SYNTHETICS/gpsvelocity.cpp \
     $$UAVOBJECT_SYNTHETICS/positionactual.cpp \
     $$UAVOBJECT_SYNTHETICS/flightbatterystate.cpp \
     $$UAVOBJECT_SYNTHETICS/homelocation.cpp \

shared/uavobjectdefinition/gpsvelocity.xml

@@ -0,0 +1,12 @@
+<xml>
+    <object name="GPSVelocity" singleinstance="true" settings="false">
+        <description>Raw GPS data from @ref GPSModule.  Should only be used by @ref AHRSCommsModule.</description>
+        <field name="North" units="m/s" type="float" elements="1"/>
+        <field name="East" units="m/s" type="float" elements="1"/>
+        <field name="Down" units="m/s" type="float" elements="1"/>
+        <access gcs="readwrite" flight="readwrite"/>
+        <telemetrygcs acked="false" updatemode="manual" period="0"/>
+        <telemetryflight acked="false" updatemode="periodic" period="1000"/>
+        <logging updatemode="periodic" period="1000"/>
+    </object>
+</xml>

shared/uavobjectdefinition/guidancesettings.xml

@@ -6,10 +6,13 @@
 	<field name="HorizontalVelMax" units="m/s" type="uint16" elements="1" defaultvalue="10"/>
         <field name="VerticalVelMax" units="m/s" type="uint16" elements="1" defaultvalue="1"/>
         <field name="HorizontalPosPI" units="(m/s)/m" type="float" elementnames="Kp,Ki,ILimit" defaultvalue="1,0,0"/>
-        <field name="HorizontalVelPID" units="deg/(m/s)" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="10,0,1,0"/>
+        <field name="HorizontalVelPID" units="deg/(m/s)" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="5,0,1,0"/>
         <field name="VerticalPosPI" units="" type="float" elementnames="Kp,Ki,ILimit" defaultvalue="0.1,0.001,200"/>
         <field name="VerticalVelPID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.1,0,0,0"/>
-        <field name="ThrottleControl" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="FALSE"/>
+	<field name="VelocityFeedforward" units="deg/(m/s)" type="float" elements="1" defaultvalue="0"/>
+	<field name="ThrottleControl" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="FALSE"/>
+        <field name="VelocitySource" units="" type="enum" elements="1" options="EKF,NEDVEL,GPSPOS" defaultvalue="EKF"/>
+        <field name="PositionSource" units="" type="enum" elements="1" options="EKF,GPSPOS" defaultvalue="EKF"/>
         <field name="MaxRollPitch" units="deg" type="float" elements="1" defaultvalue="20"/>
         <field name="UpdatePeriod" units="" type="int32" elements="1" defaultvalue="100"/>
         <access gcs="readwrite" flight="readwrite"/>