Merge branch 'corvuscorax/new_navigation' into revo-next

flight/Libraries/inc/paths.h

@@ -34,6 +34,6 @@ struct path_status {
 	float path_direction[2];
 };
 
-void path_progress(float * start_point, float * end_point, float * cur_point, struct path_status * status);
+void path_progress(float * start_point, float * end_point, float * cur_point, struct path_status * status, uint8_t mode);
 
 #endif

flight/Libraries/paths.c

@@ -27,6 +27,89 @@
 #include "pios.h"
 #include "paths.h"
 
+#include "uavobjectmanager.h" // <--.
+#include "pathdesired.h" //<-- needed only for correct ENUM macro usage with path modes (PATHDESIRED_MODE_xxx,
+// no direct UAVObject usage allowed in this file
+
+// private functions
+static void path_endpoint( float * start_point, float * end_point, float * cur_point, struct path_status * status);
+static void path_vector( float * start_point, float * end_point, float * cur_point, struct path_status * status);
+static void path_circle(float * start_point, float * end_point, float * cur_point, struct path_status * status, bool clockwise);
+
+/**
+ * @brief Compute progress along path and deviation from it
+ * @param[in] start_point Starting point
+ * @param[in] end_point Ending point
+ * @param[in] cur_point Current location
+ * @param[in] mode Path following mode
+ * @param[out] status Structure containing progress along path and deviation
+ */
+void path_progress(float * start_point, float * end_point, float * cur_point, struct path_status * status, uint8_t mode)
+{
+	switch(mode) {
+		case PATHDESIRED_MODE_FLYVECTOR:
+		case PATHDESIRED_MODE_DRIVEVECTOR:
+			return path_vector(start_point, end_point, cur_point, status);
+			break;
+		case PATHDESIRED_MODE_FLYCIRCLERIGHT:
+		case PATHDESIRED_MODE_DRIVECIRCLERIGHT:
+			return path_circle(start_point, end_point, cur_point, status, 1);
+			break;
+		case PATHDESIRED_MODE_FLYCIRCLELEFT:
+		case PATHDESIRED_MODE_DRIVECIRCLELEFT:
+			return path_circle(start_point, end_point, cur_point, status, 0);
+			break;
+		case PATHDESIRED_MODE_FLYENDPOINT:
+		case PATHDESIRED_MODE_DRIVEENDPOINT:
+		default:
+			// use the endpoint as default failsafe if called in unknown modes
+			return path_endpoint(start_point, end_point, cur_point, status);
+			break;
+	}
+}
+
+/**
+ * @brief Compute progress towards endpoint. Deviation equals distance
+ * @param[in] start_point Starting point
+ * @param[in] end_point Ending point
+ * @param[in] cur_point Current location
+ * @param[out] status Structure containing progress along path and deviation
+ */
+static void path_endpoint( float * start_point, float * end_point, float * cur_point, struct path_status * status)
+{
+	float path_north, path_east, diff_north, diff_east;
+	float dist_path, dist_diff;
+
+	// we do not correct in this mode
+	status->correction_direction[0] = status->correction_direction[1] = 0;
+
+	// Distance to go
+	path_north = end_point[0] - start_point[0];
+	path_east = end_point[1] - start_point[1];
+
+	// Current progress location relative to end
+	diff_north = end_point[0] - cur_point[0];
+	diff_east = end_point[1] - cur_point[1];
+
+	dist_diff = sqrtf( diff_north * diff_north + diff_east * diff_east );
+	dist_path = sqrtf( path_north * path_north + path_east * path_east );
+
+	if(dist_diff < 1e-6 ) {
+		status->fractional_progress = 1;
+		status->error = 0;
+		status->path_direction[0] = status->path_direction[1] = 0;
+		return;
+	}
+
+	status->fractional_progress = 1 - dist_diff / (1 + dist_path);
+	status->error = dist_diff;
+
+	// Compute direction to travel
+	status->path_direction[0] = diff_north / dist_diff;
+	status->path_direction[1] = diff_east / dist_diff;
+
+}
+
 /**
  * @brief Compute progress along path and deviation from it
  * @param[in] start_point Starting point
@@ -34,10 +117,10 @@
  * @param[in] cur_point Current location
  * @param[out] status Structure containing progress along path and deviation
  */
-void path_progress(float * start_point, float * end_point, float * cur_point, struct path_status * status)
+static void path_vector( float * start_point, float * end_point, float * cur_point, struct path_status * status)
 {
 	float path_north, path_east, diff_north, diff_east;
-	float dist_path2;
+	float dist_path;
 	float dot;
 	float normal[2];
 
@@ -50,21 +133,22 @@ void path_progress(float * start_point, float * end_point, float * cur_point, st
 	diff_east = cur_point[1] - start_point[1];
 
 	dot = path_north * diff_north + path_east * diff_east;
-	dist_path2 = path_north * path_north + path_east * path_east;
+	dist_path = sqrtf( path_north * path_north + path_east * path_east );
 
-	if(dist_path2 < 1e-3) {
+	if(dist_path < 1e-6) {
+		// if the path is too short, we cannot determine vector direction.
+		// Fly towards the endpoint to prevent flying away,
+		// but assume progress=1 either way.
+		path_endpoint( start_point, end_point, cur_point, status );
 		status->fractional_progress = 1;
-		status->error = 0;
-		status->correction_direction[0] = status->correction_direction[1] = 0;
-		status->path_direction[0] = status->path_direction[1] = 0;
 		return;
 	}
 
 	// Compute the normal to the path
-	normal[0] = -path_east / sqrtf(dist_path2);
+	normal[0] = -path_east / dist_path;
-	normal[1] = path_north / sqrtf(dist_path2);
+	normal[1] = path_north / dist_path;
 
-	status->fractional_progress = dot / dist_path2;
+	status->fractional_progress = dot / (dist_path * dist_path);
 	status->error = normal[0] * diff_north + normal[1] * diff_east;
 
 	// Compute direction to correct error
@@ -75,9 +159,68 @@ void path_progress(float * start_point, float * end_point, float * cur_point, st
 	status->error = fabs(status->error);
 
 	// Compute direction to travel
-	status->path_direction[0] = path_north / sqrtf(dist_path2);
+	status->path_direction[0] = path_north / dist_path;
-	status->path_direction[1] = path_east / sqrtf(dist_path2);
+	status->path_direction[1] = path_east / dist_path;
+
+}
+
+/**
+ * @brief Compute progress along circular path and deviation from it
+ * @param[in] start_point Starting point
+ * @param[in] end_point Center point
+ * @param[in] cur_point Current location
+ * @param[out] status Structure containing progress along path and deviation
+ */
+static void path_circle(float * start_point, float * end_point, float * cur_point, struct path_status * status, bool clockwise)
+{
+	float radius_north, radius_east, diff_north, diff_east;
+	float radius,cradius;
+	float normal[2];
+
+	// Radius
+	radius_north = end_point[0] - start_point[0];
+	radius_east = end_point[1] - start_point[1];
+
+	// Current location relative to center
+	diff_north = cur_point[0] - end_point[0];
+	diff_east = cur_point[1] - end_point[1];
+
+	radius = sqrtf( radius_north * radius_north + radius_east * radius_east );
+	cradius = sqrtf(  diff_north * diff_north   +   diff_east * diff_east );
+
+	if (cradius < 1e-6) {
+		// cradius is zero, just fly somewhere and make sure correction is still a normal
+		status->fractional_progress = 1;
+		status->error = radius;
+		status->correction_direction[0] = 0;
+		status->correction_direction[1] = 1;
+		status->path_direction[0] = 1;
+		status->path_direction[1] = 0;
+		return;
+	}
+
+	if (clockwise) {
+		// Compute the normal to the radius clockwise
+		normal[0] = -diff_east / cradius;
+		normal[1] = diff_north / cradius;
+	} else {
+		// Compute the normal to the radius counter clockwise
+		normal[0] = diff_east / cradius;
+		normal[1] = -diff_north / cradius;
+	}
+	
+	status->fractional_progress = (clockwise?1:-1) * atan2f( diff_north, diff_east) - atan2f( radius_north, radius_east);
+
+	// error is current radius minus wanted radius - positive if too close
+	status->error = radius - cradius;
+
+	// Compute direction to correct error
+	status->correction_direction[0] = (status->error>0?1:-1) * diff_north / cradius;
+	status->correction_direction[1] = (status->error>0?1:-1) * diff_east / cradius;
+
+	// Compute direction to travel
+	status->path_direction[0] = normal[0];
+	status->path_direction[1] = normal[1];
 
 	status->error = fabs(status->error);
 }
-

flight/Modules/FixedWingPathFollower/fixedwingpathfollower.c

@@ -0,0 +1,643 @@
+/**
+ ******************************************************************************
+ *
+ * @file       fixedwingpathfollower.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      This module compared @ref PositionActuatl to @ref ActiveWaypoint 
+ * and sets @ref AttitudeDesired.  It only does this when the FlightMode field
+ * of @ref ManualControlCommand is Auto.
+ *
+ * @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
+ */
+
+/**
+ * Input object: ActiveWaypoint
+ * Input object: PositionActual
+ * Input object: ManualControlCommand
+ * Output object: AttitudeDesired
+ *
+ * This module will periodically update the value of the AttitudeDesired object.
+ *
+ * The module executes in its own thread in this example.
+ *
+ * Modules have no API, all communication to other modules is done through UAVObjects.
+ * However modules may use the API exposed by shared libraries.
+ * See the OpenPilot wiki for more details.
+ * http://www.openpilot.org/OpenPilot_Application_Architecture
+ *
+ */
+
+#include "openpilot.h"
+#include "paths.h"
+
+#include "accels.h"
+#include "hwsettings.h"
+#include "attitudeactual.h"
+#include "pathdesired.h"	// object that will be updated by the module
+#include "positionactual.h"
+#include "manualcontrol.h"
+#include "flightstatus.h"
+#include "pathstatus.h"
+#include "baroairspeed.h"
+#include "gpsvelocity.h"
+#include "gpsposition.h"
+#include "fixedwingpathfollowersettings.h"
+#include "fixedwingpathfollowerstatus.h"
+#include "homelocation.h"
+#include "nedposition.h"
+#include "stabilizationdesired.h"
+#include "stabilizationsettings.h"
+#include "systemsettings.h"
+#include "velocitydesired.h"
+#include "velocityactual.h"
+#include "CoordinateConversions.h"
+
+// Private constants
+#define MAX_QUEUE_SIZE 4
+#define STACK_SIZE_BYTES 1548
+#define TASK_PRIORITY (tskIDLE_PRIORITY+2)
+#define F_PI 3.14159265358979323846f
+#define RAD2DEG (180.0f/F_PI)
+#define DEG2RAD (F_PI/180.0f)
+#define GEE 9.81f
+// Private types
+
+// Private variables
+static bool followerEnabled = false;
+static xTaskHandle pathfollowerTaskHandle;
+static PathDesiredData pathDesired;
+static PathStatusData pathStatus;
+static FixedWingPathFollowerSettingsData fixedwingpathfollowerSettings;
+
+// Private functions
+static void pathfollowerTask(void *parameters);
+static void SettingsUpdatedCb(UAVObjEvent * ev);
+static void updatePathVelocity();
+static uint8_t updateFixedDesiredAttitude();
+static void updateFixedAttitude();
+static void baroAirspeedUpdatedCb(UAVObjEvent * ev);
+static float bound(float val, float min, float max);
+
+/**
+ * Initialise the module, called on startup
+ * \returns 0 on success or -1 if initialisation failed
+ */
+int32_t FixedWingPathFollowerStart()
+{
+	if (followerEnabled) {
+		// Start main task
+		xTaskCreate(pathfollowerTask, (signed char *)"PathFollower", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &pathfollowerTaskHandle);
+		TaskMonitorAdd(TASKINFO_RUNNING_PATHFOLLOWER, pathfollowerTaskHandle);
+	}
+
+	return 0;
+}
+
+/**
+ * Initialise the module, called on startup
+ * \returns 0 on success or -1 if initialisation failed
+ */
+int32_t FixedWingPathFollowerInitialize()
+{
+	HwSettingsInitialize();
+	uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
+	HwSettingsOptionalModulesGet(optionalModules);
+	if (optionalModules[HWSETTINGS_OPTIONALMODULES_FIXEDWINGPATHFOLLOWER] == HWSETTINGS_OPTIONALMODULES_ENABLED) {
+		followerEnabled = true;
+		FixedWingPathFollowerSettingsInitialize();
+		FixedWingPathFollowerStatusInitialize();
+		PathDesiredInitialize();
+		PathStatusInitialize();
+		VelocityDesiredInitialize();
+		BaroAirspeedInitialize();
+	} else {
+		followerEnabled = false;
+	}
+	return 0;
+}
+MODULE_INITCALL(FixedWingPathFollowerInitialize, FixedWingPathFollowerStart)
+
+static float northVelIntegral = 0;
+static float eastVelIntegral = 0;
+static float downVelIntegral = 0;
+
+static float courseIntegral = 0;
+static float speedIntegral = 0;
+static float accelIntegral = 0;
+static float powerIntegral = 0;
+
+// correct speed by measured airspeed
+static float baroAirspeedBias = 0;
+
+/**
+ * Module thread, should not return.
+ */
+static void pathfollowerTask(void *parameters)
+{
+	SystemSettingsData systemSettings;
+	FlightStatusData flightStatus;
+	
+	portTickType lastUpdateTime;
+	
+	BaroAirspeedConnectCallback(baroAirspeedUpdatedCb);
+	FixedWingPathFollowerSettingsConnectCallback(SettingsUpdatedCb);
+	PathDesiredConnectCallback(SettingsUpdatedCb);
+	
+	FixedWingPathFollowerSettingsGet(&fixedwingpathfollowerSettings);
+	PathDesiredGet(&pathDesired);
+	
+	// Main task loop
+	lastUpdateTime = xTaskGetTickCount();
+	while (1) {
+
+		// Conditions when this runs:
+		// 1. Must have FixedWing type airframe
+		// 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint  OR
+		//    FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path
+
+		SystemSettingsGet(&systemSettings);
+		if ( (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWING) &&
+			(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGELEVON) &&
+			(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGVTAIL) )
+		{
+			AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_WARNING);
+			vTaskDelay(1000);
+			continue;
+		}
+
+		// Continue collecting data if not enough time
+		vTaskDelayUntil(&lastUpdateTime, fixedwingpathfollowerSettings.UpdatePeriod / portTICK_RATE_MS);
+
+		
+		FlightStatusGet(&flightStatus);
+		PathStatusGet(&pathStatus);
+	
+		uint8_t result;
+		// Check the combinations of flightmode and pathdesired mode
+		switch(flightStatus.FlightMode) {
+			case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
+			case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
+				if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
+					updatePathVelocity();
+					result = updateFixedDesiredAttitude();
+					if (result) {
+						AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
+					} else {
+						AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_WARNING);
+					}
+				} else {
+					AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_ERROR);
+				}
+				break;
+			case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
+				pathStatus.UID = pathDesired.UID;
+				pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
+				switch(pathDesired.Mode) {
+					case PATHDESIRED_MODE_FLYENDPOINT:
+					case PATHDESIRED_MODE_FLYVECTOR:
+					case PATHDESIRED_MODE_FLYCIRCLERIGHT:
+					case PATHDESIRED_MODE_FLYCIRCLELEFT:
+						updatePathVelocity();
+						result = updateFixedDesiredAttitude();
+						if (result) {
+							AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
+						} else {
+							pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
+							AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_WARNING);
+						}
+						break;
+					case PATHDESIRED_MODE_FIXEDATTITUDE:
+						updateFixedAttitude(pathDesired.ModeParameters);
+						AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
+						break;
+					case PATHDESIRED_MODE_DISARMALARM:
+						AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_CRITICAL);
+						break;
+					default:
+						pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
+						AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_ERROR);
+						break;
+				}
+				break;
+			default:
+				// Be cleaner and get rid of global variables
+				northVelIntegral = 0;
+				eastVelIntegral = 0;
+				downVelIntegral = 0;
+				courseIntegral = 0;
+				speedIntegral = 0;
+				accelIntegral = 0;
+				powerIntegral = 0;
+
+				break;
+		}
+		PathStatusSet(&pathStatus);
+	}
+}
+
+/**
+ * Compute desired velocity from the current position and path
+ *
+ * Takes in @ref PositionActual and compares it to @ref PathDesired 
+ * and computes @ref VelocityDesired
+ */
+static void updatePathVelocity()
+{
+	PositionActualData positionActual;
+	PositionActualGet(&positionActual);
+	VelocityActualData velocityActual;
+	VelocityActualGet(&velocityActual);
+
+	// look ahead fixedwingpathfollowerSettings.CourseFeedForward seconds
+	float cur[3] = {positionActual.North + (velocityActual.North * fixedwingpathfollowerSettings.CourseFeedForward),
+					positionActual.East + (velocityActual.East * fixedwingpathfollowerSettings.CourseFeedForward),
+					positionActual.Down + (velocityActual.Down * fixedwingpathfollowerSettings.CourseFeedForward)
+					};
+	struct path_status progress;
+
+	path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode);
+	
+	float groundspeed;
+	float altitudeSetpoint;
+	switch (pathDesired.Mode) {
+			case PATHDESIRED_MODE_FLYCIRCLERIGHT:
+			case PATHDESIRED_MODE_DRIVECIRCLERIGHT:
+			case PATHDESIRED_MODE_FLYCIRCLELEFT:
+			case PATHDESIRED_MODE_DRIVECIRCLELEFT:
+				groundspeed = pathDesired.EndingVelocity;
+				altitudeSetpoint = pathDesired.End[2];
+				break;
+			case PATHDESIRED_MODE_FLYENDPOINT:
+			case PATHDESIRED_MODE_DRIVEENDPOINT:
+			case PATHDESIRED_MODE_FLYVECTOR:
+			case PATHDESIRED_MODE_DRIVEVECTOR:
+			default:
+				groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) *
+					bound(progress.fractional_progress,0,1);
+				altitudeSetpoint = pathDesired.Start[2] + (pathDesired.End[2] - pathDesired.Start[2]) *
+					bound(progress.fractional_progress,0,1);
+				break;
+	}
+	
+	// calculate velocity - can be zero if waypoints are too close
+	VelocityDesiredData velocityDesired;
+	velocityDesired.North = progress.path_direction[0] * bound(groundspeed,1e-6,groundspeed);
+	velocityDesired.East = progress.path_direction[1] * bound(groundspeed,1e-6,groundspeed);
+	
+	float error_speed = progress.error * fixedwingpathfollowerSettings.HorizontalPosP;
+
+	// calculate correction - can also be zero if correction vector is 0 or no error present
+	velocityDesired.North += progress.correction_direction[0] * error_speed;
+	velocityDesired.East += progress.correction_direction[1] * error_speed;
+	
+	float downError = altitudeSetpoint - positionActual.Down;
+	velocityDesired.Down = downError * fixedwingpathfollowerSettings.VerticalPosP;
+
+	// update pathstatus
+	pathStatus.error = progress.error;
+	pathStatus.fractional_progress = progress.fractional_progress;
+
+	VelocityDesiredSet(&velocityDesired);
+}
+
+
+/**
+ * Compute desired attitude from a fixed preset
+ *
+ */
+static void updateFixedAttitude(float* attitude)
+{
+	StabilizationDesiredData stabDesired;
+	StabilizationDesiredGet(&stabDesired);
+	stabDesired.Roll     = attitude[0];
+	stabDesired.Pitch    = attitude[1];
+	stabDesired.Yaw      = attitude[2];
+	stabDesired.Throttle = attitude[3];
+	stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+	stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+	stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
+	StabilizationDesiredSet(&stabDesired);
+}
+
+/**
+ * Compute desired attitude from the desired velocity
+ *
+ * Takes in @ref NedActual which has the acceleration in the 
+ * NED frame as the feedback term and then compares the 
+ * @ref VelocityActual against the @ref VelocityDesired
+ */
+static uint8_t updateFixedDesiredAttitude()
+{
+
+	uint8_t result = 1;
+
+	float dT = fixedwingpathfollowerSettings.UpdatePeriod / 1000.0f;
+
+	VelocityDesiredData velocityDesired;
+	VelocityActualData velocityActual;
+	StabilizationDesiredData stabDesired;
+	AttitudeActualData attitudeActual;
+	AccelsData accels;
+	FixedWingPathFollowerSettingsData fixedwingpathfollowerSettings;
+	StabilizationSettingsData stabSettings;
+	FixedWingPathFollowerStatusData fixedwingpathfollowerStatus;
+
+	float groundspeedActual;
+	float groundspeedDesired;
+	float airspeedActual;
+	float airspeedDesired;
+	float speedError;
+	float accelActual;
+	float accelDesired;
+	float accelError;
+	float accelCommand;
+	
+	float pitchCommand;
+
+	float climbspeedDesired;
+	float climbspeedError;
+	float powerError;
+	float powerCommand;
+
+	float courseError;
+	float courseCommand;
+
+	FixedWingPathFollowerSettingsGet(&fixedwingpathfollowerSettings);
+
+	FixedWingPathFollowerStatusGet(&fixedwingpathfollowerStatus);
+	
+	VelocityActualGet(&velocityActual);
+	VelocityDesiredGet(&velocityDesired);
+	StabilizationDesiredGet(&stabDesired);
+	VelocityDesiredGet(&velocityDesired);
+	AttitudeActualGet(&attitudeActual);
+	AccelsGet(&accels);
+	StabilizationSettingsGet(&stabSettings);
+
+
+	/**
+	 * Compute speed error (required for throttle and pitch)
+	 */
+
+	// Current ground speed
+	groundspeedActual  = sqrtf( velocityActual.East*velocityActual.East + velocityActual.North*velocityActual.North );
+	airspeedActual     = groundspeedActual + baroAirspeedBias;
+
+	// Desired ground speed
+	groundspeedDesired = sqrtf(velocityDesired.North*velocityDesired.North + velocityDesired.East*velocityDesired.East);
+	airspeedDesired    = bound( groundspeedDesired + baroAirspeedBias,
+							fixedwingpathfollowerSettings.AirSpeedMin,
+							fixedwingpathfollowerSettings.AirSpeedMax);
+
+	// Airspeed error
+	speedError = airspeedDesired - ( airspeedActual );
+	// Vertical error
+	climbspeedDesired = bound (
+						velocityDesired.Down,
+						-fixedwingpathfollowerSettings.VerticalVelMax,
+						fixedwingpathfollowerSettings.VerticalVelMax);
+	climbspeedError = climbspeedDesired - velocityActual.Down;
+
+	// Error condition: wind speed is higher than maximum allowed speed. We are forced backwards!
+	fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_WIND] = 0;
+	if (groundspeedDesired - baroAirspeedBias <= 0 ) {
+		fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_WIND] = 1;
+		result = 0;
+	}
+	// Error condition: plane too slow or too fast
+	fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHSPEED] = 0;
+	fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 0;
+	if ( airspeedActual >  fixedwingpathfollowerSettings.AirSpeedMax * 1.2f) {
+		fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHSPEED] = 1;
+		result = 0;
+	}
+	if (airspeedActual < fixedwingpathfollowerSettings.AirSpeedMin * 0.8f) {
+		fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 1;
+		result = 0;
+	}
+
+	if (airspeedActual<1e-6) {
+		// prevent division by zero, abort without controlling anything. This guidance mode is not suited for takeoff or touchdown, or handling stationary planes
+		// also we cannot handle planes flying backwards, lets just wait until the nose drops
+		fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 1;
+		return 0;
+	}
+
+	/**
+	 * Compute desired throttle command
+	 */
+	// compute desired power
+	powerError = -climbspeedError +
+				bound (
+					(speedError/airspeedActual) * fixedwingpathfollowerSettings.AirspeedToVerticalCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_AIRSPEEDTOVERTICALCROSSFEED_KP],
+					-fixedwingpathfollowerSettings.AirspeedToVerticalCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_AIRSPEEDTOVERTICALCROSSFEED_MAX],
+					fixedwingpathfollowerSettings.AirspeedToVerticalCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_AIRSPEEDTOVERTICALCROSSFEED_MAX]
+				);
+	powerIntegral =	bound(powerIntegral + powerError * dT * fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI], 
+		-fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT],
+		fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT]);
+	powerCommand = (powerError * fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KP] +
+		powerIntegral) + fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_NEUTRAL];
+
+	// prevent integral running out of bounds 
+	if ( powerCommand > fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX]) {
+		powerIntegral = bound(
+			powerIntegral -
+				( powerCommand 
+				- fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX]),
+			-fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT],
+			fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT]);
+		powerCommand = fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX];
+	}
+	if ( powerCommand < fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN]) {
+		powerIntegral = bound(
+			powerIntegral -
+				( powerCommand
+				- fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN]),
+			-fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT],
+			fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT]);
+		powerCommand = fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN];
+	}
+
+	fixedwingpathfollowerStatus.E[FIXEDWINGPATHFOLLOWERSTATUS_E_POWER] = powerError;
+	fixedwingpathfollowerStatus.A[FIXEDWINGPATHFOLLOWERSTATUS_A_POWER] = powerIntegral;
+	fixedwingpathfollowerStatus.C[FIXEDWINGPATHFOLLOWERSTATUS_C_POWER] = powerCommand;
+
+	// set throttle
+	stabDesired.Throttle = powerCommand;
+
+	// Error condition: plane cannot hold altitude at current speed.
+	fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWPOWER] = 0;
+	if (
+		powerCommand == fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX] // throttle at maximum
+		&& velocityActual.Down > 0 // we ARE going down
+		&& climbspeedDesired < 0 // we WANT to go up
+		&& speedError > 0 // we are too slow already
+		) {
+		fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWPOWER] = 1;
+		result = 0;
+	}
+	// Error condition: plane keeps climbing despite minimum throttle (opposite of above)
+	fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHPOWER] = 0;
+	if (
+		powerCommand == fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN] // throttle at minimum
+		&& velocityActual.Down < 0 // we ARE going up
+		&& climbspeedDesired > 0 // we WANT to go down
+		&& speedError < 0 // we are too fast already
+		) {
+		fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHPOWER] = 1;
+		result = 0;
+	}
+
+
+	/**
+	 * Compute desired pitch command
+	 */
+	// compute desired acceleration
+	accelDesired = bound( (speedError/airspeedActual) * fixedwingpathfollowerSettings.SpeedP[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDP_KP],
+		-fixedwingpathfollowerSettings.SpeedP[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDP_MAX],
+		fixedwingpathfollowerSettings.SpeedP[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDP_MAX]);
+	
+	fixedwingpathfollowerStatus.E[FIXEDWINGPATHFOLLOWERSTATUS_E_SPEED] = (speedError/airspeedActual);
+	fixedwingpathfollowerStatus.A[FIXEDWINGPATHFOLLOWERSTATUS_A_SPEED] = 0.0f;
+	fixedwingpathfollowerStatus.C[FIXEDWINGPATHFOLLOWERSTATUS_C_SPEED] = accelDesired;
+
+	// exclude gravity from acceleration. If the aicraft is flying at high pitch, it fights gravity without getting faster
+	accelActual = accels.x - (sinf( DEG2RAD * attitudeActual.Pitch) * GEE);
+
+	accelError = accelDesired - accelActual;
+	accelIntegral = bound(accelIntegral + accelError * dT * fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_KI], 
+		-fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_ILIMIT],
+		fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_ILIMIT]);
+	accelCommand = (accelError * fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_KP] + 
+		 accelIntegral);
+	
+	fixedwingpathfollowerStatus.E[FIXEDWINGPATHFOLLOWERSTATUS_E_ACCEL] = accelError;
+	fixedwingpathfollowerStatus.A[FIXEDWINGPATHFOLLOWERSTATUS_A_ACCEL] = accelIntegral;
+	fixedwingpathfollowerStatus.C[FIXEDWINGPATHFOLLOWERSTATUS_C_ACCEL] = accelCommand;
+
+	// compute desired pitch
+	pitchCommand= -accelCommand + bound ( (-climbspeedError/airspeedActual) * fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_KP],
+					-fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX],
+					fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX]
+					);
+	stabDesired.Pitch = bound(fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_NEUTRAL] +
+		pitchCommand,
+		fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MIN],
+		fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MAX]);
+
+	// Error condition: high speed dive
+	fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_PITCHCONTROL] = 0;
+	if (
+		pitchCommand == fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MAX] // pitch demand is full up
+		&& velocityActual.Down > 0 // we ARE going down
+		&& climbspeedDesired < 0 // we WANT to go up
+		&& speedError < 0 // we are too fast already
+		) {
+		fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_PITCHCONTROL] = 1;
+		result = 0;
+	}
+
+
+	/**
+	 * Compute desired roll command
+	 */
+	if (groundspeedDesired> 1e-6) {
+		courseError = RAD2DEG * (atan2f(velocityDesired.East,velocityDesired.North) - atan2f(velocityActual.East,velocityActual.North));
+	} else {
+		// if we are not supposed to move, keep going wherever we are now. Don't make things worse by changing direction.
+		courseError = 0;
+	}
+	
+	if (courseError<-180.0f) courseError+=360.0f;
+	if (courseError>180.0f) courseError-=360.0f;
+
+	courseIntegral = bound(courseIntegral + courseError * dT * fixedwingpathfollowerSettings.CoursePI[FIXEDWINGPATHFOLLOWERSETTINGS_COURSEPI_KI], 
+		-fixedwingpathfollowerSettings.CoursePI[FIXEDWINGPATHFOLLOWERSETTINGS_COURSEPI_ILIMIT],
+		fixedwingpathfollowerSettings.CoursePI[FIXEDWINGPATHFOLLOWERSETTINGS_COURSEPI_ILIMIT]);
+	courseCommand = (courseError * fixedwingpathfollowerSettings.CoursePI[FIXEDWINGPATHFOLLOWERSETTINGS_COURSEPI_KP] +
+		courseIntegral);
+
+	fixedwingpathfollowerStatus.E[FIXEDWINGPATHFOLLOWERSTATUS_E_COURSE] = courseError;
+	fixedwingpathfollowerStatus.A[FIXEDWINGPATHFOLLOWERSTATUS_A_COURSE] = courseIntegral;
+	fixedwingpathfollowerStatus.C[FIXEDWINGPATHFOLLOWERSTATUS_C_COURSE] = courseCommand;
+	
+	stabDesired.Roll = bound( fixedwingpathfollowerSettings.RollLimit[FIXEDWINGPATHFOLLOWERSETTINGS_ROLLLIMIT_NEUTRAL] +
+		courseCommand,
+		fixedwingpathfollowerSettings.RollLimit[FIXEDWINGPATHFOLLOWERSETTINGS_ROLLLIMIT_MIN],
+		fixedwingpathfollowerSettings.RollLimit[FIXEDWINGPATHFOLLOWERSETTINGS_ROLLLIMIT_MAX] );
+
+	// TODO: find a check to determine loss of directional control. Likely needs some check of derivative
+
+
+	/**
+	 * Compute desired yaw command
+	 */
+	// TODO implement raw control mode for yaw and base on Accels.X
+	stabDesired.Yaw = 0;
+
+
+	stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+	stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+	stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_NONE;
+	
+	StabilizationDesiredSet(&stabDesired);
+
+	FixedWingPathFollowerStatusSet(&fixedwingpathfollowerStatus);
+
+	return result;
+}
+
+
+/**
+ * Bound input value between limits
+ */
+static float bound(float val, float min, float max)
+{
+	if (val < min) {
+		val = min;
+	} else if (val > max) {
+		val = max;
+	}
+	return val;
+}
+
+static void SettingsUpdatedCb(UAVObjEvent * ev)
+{
+	FixedWingPathFollowerSettingsGet(&fixedwingpathfollowerSettings);
+	PathDesiredGet(&pathDesired);
+}
+
+static void baroAirspeedUpdatedCb(UAVObjEvent * ev)
+{
+
+	BaroAirspeedData baroAirspeed;
+	VelocityActualData velocityActual;
+
+	BaroAirspeedGet(&baroAirspeed);
+	if (baroAirspeed.Connected != BAROAIRSPEED_CONNECTED_TRUE) {
+		baroAirspeedBias = 0;
+	} else {
+		VelocityActualGet(&velocityActual);
+		float groundspeed = sqrtf(velocityActual.East*velocityActual.East + velocityActual.North*velocityActual.North );
+
+		baroAirspeedBias = baroAirspeed.Airspeed - groundspeed;
+	}
+
+}

flight/Modules/ManualControl/inc/manualcontrol.h

@@ -42,6 +42,7 @@ typedef enum {FLIGHTMODE_UNDEFINED = 0, FLIGHTMODE_MANUAL = 1, FLIGHTMODE_STABIL
 	(x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) ? FLIGHTMODE_GUIDANCE : \
 	(x == FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) ? FLIGHTMODE_GUIDANCE : \
 	(x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) ? FLIGHTMODE_GUIDANCE : \
+	(x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE) ? FLIGHTMODE_GUIDANCE : \
 	(x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) ? FLIGHTMODE_GUIDANCE : FLIGHTMODE_UNDEFINED \
 	)
 

flight/Modules/ManualControl/manualcontrol.c

@@ -84,7 +84,7 @@ static portTickType lastSysTime;
 static void updateActuatorDesired(ManualControlCommandData * cmd);
 static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings);
 static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed);
-static void updatePathDesired(ManualControlCommandData * cmd, bool changed);
+static void updatePathDesired(ManualControlCommandData * cmd, bool changed, bool home);
 static void processFlightMode(ManualControlSettingsData * settings, float flightMode);
 static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings);
 static void setArmedIfChanged(uint8_t val);
@@ -395,7 +395,10 @@ static void manualControlTask(void *parameters)
 						altitudeHoldDesired(&cmd, lastFlightMode != flightStatus.FlightMode);
 						break;
 					case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
-						updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode);
+						updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode, false);
+						break;
+					case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
+						updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode, true);
 						break;
 					default:
 						AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
@@ -619,7 +622,7 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon
  * @brief Update the position desired to current location when
  * enabled and allow the waypoint to be moved by transmitter
  */
-static void updatePathDesired(ManualControlCommandData * cmd, bool changed)
+static void updatePathDesired(ManualControlCommandData * cmd, bool changed,bool home)
 {
 	static portTickType lastSysTime;
 	portTickType thisSysTime;
@@ -629,24 +632,46 @@ static void updatePathDesired(ManualControlCommandData * cmd, bool changed)
 	dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;
 	lastSysTime = thisSysTime;
 
-	if(changed) {
+	if (home && changed) {
+		// Simple Return To Base mode - keep altitude the same, fly to home position
+		PositionActualData positionActual;
+		PositionActualGet(&positionActual);
+		
+		PathDesiredData pathDesired;
+		PathDesiredGet(&pathDesired);
+		pathDesired.Start[PATHDESIRED_START_NORTH] = 0;
+		pathDesired.Start[PATHDESIRED_START_EAST] = 0;
+		pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down - 10;
+		pathDesired.End[PATHDESIRED_END_NORTH] = 0;
+		pathDesired.End[PATHDESIRED_END_EAST] = 0;
+		pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down - 10;
+		pathDesired.StartingVelocity=1;
+		pathDesired.EndingVelocity=0;
+		pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
+		PathDesiredSet(&pathDesired);
+	} else if(changed) {
 		// After not being in this mode for a while init at current height
 		PositionActualData positionActual;
 		PositionActualGet(&positionActual);
 		
 		PathDesiredData pathDesired;
 		PathDesiredGet(&pathDesired);
+		pathDesired.Start[PATHDESIRED_END_NORTH] = positionActual.North;
+		pathDesired.Start[PATHDESIRED_END_EAST] = positionActual.East;
+		pathDesired.Start[PATHDESIRED_END_DOWN] = positionActual.Down - 10;
 		pathDesired.End[PATHDESIRED_END_NORTH] = positionActual.North;
 		pathDesired.End[PATHDESIRED_END_EAST] = positionActual.East;
-		pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down;
+		pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down - 10;
-		pathDesired.Mode = PATHDESIRED_MODE_ENDPOINT;
+		pathDesired.StartingVelocity=1;
+		pathDesired.EndingVelocity=0;
+		pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
 		PathDesiredSet(&pathDesired);
 	} else {
 		PathDesiredData pathDesired;
 		PathDesiredGet(&pathDesired);
 		pathDesired.End[PATHDESIRED_END_NORTH] += dT * -cmd->Pitch;
 		pathDesired.End[PATHDESIRED_END_EAST] += dT * cmd->Roll;
-		pathDesired.Mode = PATHDESIRED_MODE_ENDPOINT;
+		pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
 		PathDesiredSet(&pathDesired);
 	}
 }
@@ -699,7 +724,7 @@ static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed)
 // TODO: These functions should never be accessible on CC.  Any configuration that
 // could allow them to be called sholud already throw an error to prevent this happening
 // in flight
-static void updatePathDesired(ManualControlCommandData * cmd, bool changed)
+static void updatePathDesired(ManualControlCommandData * cmd, bool changed, bool home)
 {
 	AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR);
 }
@@ -771,6 +796,21 @@ static bool okToArm(void)
 
 	return true;
 }
+/**
+ * @brief Determine if the aircraft is forced to disarm by an explicit alarm
+ * @returns True if safe to arm, false otherwise
+ */
+static bool forcedDisArm(void)
+{
+	// read alarms
+	SystemAlarmsData alarms;
+	SystemAlarmsGet(&alarms);
+
+	if (alarms.Alarm[SYSTEMALARMS_ALARM_GUIDANCE] == SYSTEMALARMS_ALARM_CRITICAL) {
+		return true;
+	}
+	return false;
+}
 
 /**
  * @brief Update the flightStatus object only if value changed.  Reduces callbacks
@@ -796,6 +836,12 @@ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData
 
 	bool lowThrottle = cmd->Throttle <= 0;
 
+	if (forcedDisArm()) {
+		// PathPlanner forces explicit disarming due to error condition (crash, impact, fire, ...)
+		setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
+		return;
+	}
+
 	if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSDISARMED) {
 		// In this configuration we always disarm
 		setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);

flight/Modules/PathPlanner/inc/pathplanner.h

@@ -2,11 +2,11 @@
  ******************************************************************************
  * @addtogroup OpenPilotModules OpenPilot Modules
  * @{
- * @addtogroup FlightPlan Flight Plan Module
+ * @addtogroup PathPlanner Flight Plan Module
  * @brief Executes flight plan scripts in Python
  * @{
  *
- * @file       flightplan.c
+ * @file       pathplanner.c
  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
  * @brief      Executes flight plan scripts in Python
  *
@@ -28,9 +28,9 @@
  * with this program; if not, write to the Free Software Foundation, Inc.,
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
-#ifndef FLIGHTPLAN_H
+#ifndef PATHPLANNER_H
-#define FLIGHTPLAN_H
+#define PATHPLANNER_H
 
-int32_t FlightPlanInitialize();
+// public initialisations go here
 
-#endif // FLIGHTPLAN_H
+#endif // PATHPLANNER_H

flight/Modules/PathPlanner/pathplanner.c

@@ -33,32 +33,56 @@
 #include "paths.h"
 
 #include "flightstatus.h"
+#include "baroairspeed.h" // TODO: make baroairspeed optional
+#include "pathaction.h"
 #include "pathdesired.h"
+#include "pathstatus.h"
 #include "pathplannersettings.h"
 #include "positionactual.h"
+#include "velocityactual.h"
 #include "waypoint.h"
 #include "waypointactive.h"
 
 // Private constants
-#define STACK_SIZE_BYTES 1024
+#define STACK_SIZE_BYTES 1024 // TODO profile and reduce!
 #define TASK_PRIORITY (tskIDLE_PRIORITY+1)
 #define MAX_QUEUE_SIZE 2
+#define F_PI 3.141526535897932f
+#define RAD2DEG (180.0f/F_PI)
 
 // Private types
 
+// Private functions
+static void pathPlannerTask(void *parameters);
+static void settingsUpdated(UAVObjEvent * ev);
+static void updatePathDesired(UAVObjEvent * ev);
+static void setWaypoint(uint16_t num);
+static void WaypointCreateInstances(uint16_t num);
+static void createPathBox();
+static void createPathLogo();
+
+static uint8_t pathConditionCheck();
+static uint8_t conditionNone();
+static uint8_t conditionTimeOut();
+static uint8_t conditionDistanceToTarget();
+static uint8_t conditionLegRemaining();
+static uint8_t conditionBelowError();
+static uint8_t conditionAboveAltitude();
+static uint8_t conditionAboveSpeed();
+static uint8_t conditionPointingTowardsNext();
+static uint8_t conditionPythonScript();
+static uint8_t conditionImmediate();
+
+
 // Private variables
 static xTaskHandle taskHandle;
 static xQueueHandle queue;
 static PathPlannerSettingsData pathPlannerSettings;
 static WaypointActiveData waypointActive;
 static WaypointData waypoint;
+static PathActionData pathAction;
+static bool pathplanner_active = false;
 
-// Private functions
-static void pathPlannerTask(void *parameters);
-static void settingsUpdated(UAVObjEvent * ev);
-static void waypointsUpdated(UAVObjEvent * ev);
-static void createPathBox();
-static void createPathLogo();
 
 /**
  * Module initialization
@@ -82,6 +106,12 @@ int32_t PathPlannerInitialize()
 	taskHandle = NULL;
 
 	PathPlannerSettingsInitialize();
+	PathActionInitialize();
+	PathStatusInitialize();
+	PathDesiredInitialize();
+	PositionActualInitialize();
+	BaroAirspeedInitialize();
+	VelocityActualInitialize();
 	WaypointInitialize();
 	WaypointActiveInitialize();
 	
@@ -96,24 +126,23 @@ MODULE_INITCALL(PathPlannerInitialize, PathPlannerStart)
 /**
  * Module task
  */
-int32_t bad_inits;
-int32_t bad_reads;
 static void pathPlannerTask(void *parameters)
 {
+	// update settings on change
 	PathPlannerSettingsConnectCallback(settingsUpdated);
 	settingsUpdated(PathPlannerSettingsHandle());
 
-	WaypointConnectCallback(waypointsUpdated);
+	// when the active waypoint changes, update pathDesired
-	WaypointActiveConnectCallback(waypointsUpdated);
+	WaypointConnectCallback(updatePathDesired);
+	WaypointActiveConnectCallback(updatePathDesired);
+	PathActionConnectCallback(updatePathDesired);
 
 	FlightStatusData flightStatus;
 	PathDesiredData pathDesired;
-	PositionActualData positionActual;
+	PathStatusData pathStatus;
-	
-	const float MIN_RADIUS = 4.0f; // Radius to consider at waypoint (m)
 	
 	// Main thread loop
-	bool pathplanner_active = false;
+	bool endCondition = false;
 	while (1)
 	{
 
@@ -125,152 +154,74 @@ static void pathPlannerTask(void *parameters)
 			continue;
 		}
 
-		if(pathplanner_active == false) {
-			// This triggers callback to update variable
 		WaypointActiveGet(&waypointActive);
+
+		if(pathplanner_active == false) {
+
+			pathplanner_active = true;
+
+			// This triggers callback to update variable
 			waypointActive.Index = 0;
 			WaypointActiveSet(&waypointActive);
 
-			pathplanner_active = true;
 			continue;
 		}
 
-		switch(pathPlannerSettings.PathMode) {
+		WaypointInstGet(waypointActive.Index,&waypoint);
-			case PATHPLANNERSETTINGS_PATHMODE_ENDPOINT:
+		PathActionInstGet(waypoint.Action, &pathAction);
-				PositionActualGet(&positionActual);
+		PathStatusGet(&pathStatus);
-				
-				float r2 = powf(positionActual.North - waypoint.Position[WAYPOINT_POSITION_NORTH], 2) +
-				powf(positionActual.East - waypoint.Position[WAYPOINT_POSITION_EAST], 2) +
-				powf(positionActual.Down - waypoint.Position[WAYPOINT_POSITION_DOWN], 2);
-				
-				// We hit this waypoint
-				if (r2 < (MIN_RADIUS * MIN_RADIUS)) {
-					switch(waypoint.Action) {
-						case WAYPOINT_ACTION_NEXT:
-							waypointActive.Index++;
-							WaypointActiveSet(&waypointActive);
-
-							break;
-						case WAYPOINT_ACTION_RTH:
-							// Fly back to the home location but 20 m above it
 		PathDesiredGet(&pathDesired);
-							pathDesired.End[PATHDESIRED_END_NORTH] = 0;
+
-							pathDesired.End[PATHDESIRED_END_EAST] = 0;
+		// delay next step until path follower has acknowledged the path mode
-							pathDesired.End[PATHDESIRED_END_DOWN] = -20;
+		if (pathStatus.UID != pathDesired.UID) {
-							pathDesired.Mode = PATHDESIRED_MODE_ENDPOINT;
+			continue;
-							PathDesiredSet(&pathDesired);
+		}
+
+		// negative destinations DISABLE this feature
+		if (pathStatus.Status == PATHSTATUS_STATUS_CRITICAL && waypointActive.Index != pathAction.ErrorDestination && pathAction.ErrorDestination >= 0) {
+			setWaypoint(pathAction.ErrorDestination);
+			continue;
+		}
+
+		// check if condition has been met
+		endCondition = pathConditionCheck();
+
+		// decide what to do
+		switch (pathAction.Command) {
+			case PATHACTION_COMMAND_ONNOTCONDITIONNEXTWAYPOINT:
+				endCondition = !endCondition;
+			case PATHACTION_COMMAND_ONCONDITIONNEXTWAYPOINT:
+				if (endCondition) setWaypoint(waypointActive.Index+1);
 				break;
-						default:
+			case PATHACTION_COMMAND_ONNOTCONDITIONJUMPWAYPOINT:
-							PIOS_DEBUG_Assert(0);
+				endCondition = !endCondition;
-					}
+			case PATHACTION_COMMAND_ONCONDITIONJUMPWAYPOINT:
-				}
+				if (endCondition) {
-
+					if (pathAction.JumpDestination<0) {
-				break;
+						// waypoint ids <0 code relative jumps
-
+						setWaypoint(waypointActive.Index - pathAction.JumpDestination );
-			case PATHPLANNERSETTINGS_PATHMODE_PATH:
-
-				PathDesiredGet(&pathDesired);
-				PositionActualGet(&positionActual);
-
-				float cur[3] = {positionActual.North, positionActual.East, positionActual.Down};
-				struct path_status progress;
-
-				path_progress(pathDesired.Start, pathDesired.End, cur, &progress);
-
-				if (progress.fractional_progress >= 1) {
-					switch(waypoint.Action) {
-						case WAYPOINT_ACTION_NEXT:
-							waypointActive.Index++;
-							WaypointActiveSet(&waypointActive);
-
-							break;
-						case WAYPOINT_ACTION_RTH:
-							// Fly back to the home location but 20 m above it
-							PathDesiredGet(&pathDesired);
-							pathDesired.End[PATHDESIRED_END_NORTH] = 0;
-							pathDesired.End[PATHDESIRED_END_EAST] = 0;
-							pathDesired.End[PATHDESIRED_END_DOWN] = -20;
-							pathDesired.Mode = PATHDESIRED_MODE_ENDPOINT;
-							PathDesiredSet(&pathDesired);
-							break;
-						default:
-							PIOS_DEBUG_Assert(0);
-					}
-				}
-
-				break;
-		}
-	}
-}
-
-/**
- * On changed waypoints or active waypoint update position desired
- * if we are in charge
- */
-static void waypointsUpdated(UAVObjEvent * ev)
-{
-	FlightStatusData flightStatus;
-	FlightStatusGet(&flightStatus);
-	if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER)
-		return;
-	
-	WaypointActiveGet(&waypointActive);
-	WaypointInstGet(waypointActive.Index, &waypoint);
-
-	PathDesiredData pathDesired;
-
-	switch(pathPlannerSettings.PathMode) {
-		case PATHPLANNERSETTINGS_PATHMODE_ENDPOINT:
-		{
-			PathDesiredGet(&pathDesired);
-			pathDesired.End[PATHDESIRED_END_NORTH] = waypoint.Position[WAYPOINT_POSITION_NORTH];
-			pathDesired.End[PATHDESIRED_END_EAST] = waypoint.Position[WAYPOINT_POSITION_EAST];
-			pathDesired.End[PATHDESIRED_END_DOWN] = -waypoint.Position[WAYPOINT_POSITION_DOWN];
-			pathDesired.Mode = PATHDESIRED_MODE_ENDPOINT;
-			PathDesiredSet(&pathDesired);
-
-		}
-			break;
-
-		case PATHPLANNERSETTINGS_PATHMODE_PATH:
-		{
-			PathDesiredData pathDesired;
-
-			pathDesired.End[PATHDESIRED_END_NORTH] = waypoint.Position[WAYPOINT_POSITION_NORTH];
-			pathDesired.End[PATHDESIRED_END_EAST] = waypoint.Position[WAYPOINT_POSITION_EAST];
-			pathDesired.End[PATHDESIRED_END_DOWN] = waypoint.Position[WAYPOINT_POSITION_DOWN];
-			pathDesired.Mode = PATHDESIRED_MODE_PATH;
-			pathDesired.EndingVelocity = sqrtf(powf(waypoint.Velocity[WAYPOINT_VELOCITY_NORTH],2) + 
-											   powf(waypoint.Velocity[WAYPOINT_VELOCITY_EAST],2));
-
-			if(waypointActive.Index == 0) {
-				// Get current position as start point
-				PositionActualData positionActual;
-				PositionActualGet(&positionActual);
-
-				pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North;
-				pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East;
-				pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down - 1;
-				pathDesired.StartingVelocity = pathDesired.EndingVelocity;
 					} else {
-				// Get previous waypoint as start point
+						setWaypoint(pathAction.JumpDestination);
-				WaypointData waypointPrev;
-				WaypointInstGet(waypointActive.Index - 1, &waypointPrev);
-
-				pathDesired.Start[PATHDESIRED_END_NORTH] = waypointPrev.Position[WAYPOINT_POSITION_NORTH];
-				pathDesired.Start[PATHDESIRED_END_EAST] = waypointPrev.Position[WAYPOINT_POSITION_EAST];
-				pathDesired.Start[PATHDESIRED_END_DOWN] = waypointPrev.Position[WAYPOINT_POSITION_DOWN];
-				pathDesired.StartingVelocity = sqrtf(powf(waypointPrev.Velocity[WAYPOINT_VELOCITY_NORTH],2) +
-												   powf(waypointPrev.Velocity[WAYPOINT_VELOCITY_EAST],2));
-
 					}
-
+				}
-			PathDesiredSet(&pathDesired);
+				break;
+			case PATHACTION_COMMAND_IFCONDITIONJUMPWAYPOINTELSENEXTWAYPOINT:
+				if (endCondition) {
+					if (pathAction.JumpDestination<0) {
+						// waypoint ids <0 code relative jumps
+						setWaypoint(waypointActive.Index - pathAction.JumpDestination );
+					} else {
+						setWaypoint(pathAction.JumpDestination);
+					}
+				} else {
+					setWaypoint(waypointActive.Index+1);
 				}
 				break;
 		}
+	}
 }
 
+// standard settings updated callback
 void settingsUpdated(UAVObjEvent * ev) {
 	uint8_t preprogrammedPath = pathPlannerSettings.PreprogrammedPath;
 	PathPlannerSettingsGet(&pathPlannerSettings);
@@ -289,18 +240,343 @@ void settingsUpdated(UAVObjEvent * ev) {
 	}
 }
 
+// callback function when waypoints changed in any way, update pathDesired
+void updatePathDesired(UAVObjEvent * ev) {
+
+	// only ever touch pathDesired if pathplanner is enabled
+	if (!pathplanner_active) return;
+
+	// use local variables, dont use stack since this is huge and a callback,
+	// dont use the globals because we cant use mutexes here
+	static WaypointActiveData waypointActive;
+	static PathActionData pathAction;
+	static WaypointData waypoint;
+	static PathDesiredData pathDesired;
+
+	// find out current waypoint
+	WaypointActiveGet(&waypointActive);
+
+	WaypointInstGet(waypointActive.Index,&waypoint);
+	PathActionInstGet(waypoint.Action, &pathAction);
+
+	pathDesired.End[PATHDESIRED_END_NORTH] = waypoint.Position[WAYPOINT_POSITION_NORTH];
+	pathDesired.End[PATHDESIRED_END_EAST]  = waypoint.Position[WAYPOINT_POSITION_EAST];
+	pathDesired.End[PATHDESIRED_END_DOWN]  = waypoint.Position[WAYPOINT_POSITION_DOWN];
+	pathDesired.EndingVelocity = waypoint.Velocity;
+	pathDesired.Mode = pathAction.Mode;
+	pathDesired.ModeParameters[0] = pathAction.ModeParameters[0]; 
+	pathDesired.ModeParameters[1] = pathAction.ModeParameters[1]; 
+	pathDesired.ModeParameters[2] = pathAction.ModeParameters[2]; 
+	pathDesired.ModeParameters[3] = pathAction.ModeParameters[3]; 
+	pathDesired.UID = waypointActive.Index;
+
+	if(waypointActive.Index == 0) {
+		// First waypoint has itself as start point (used to be home position but that proved dangerous when looping)
+
+		pathDesired.Start[PATHDESIRED_START_NORTH] =  waypoint.Position[WAYPOINT_POSITION_NORTH];
+		pathDesired.Start[PATHDESIRED_START_EAST] =  waypoint.Position[WAYPOINT_POSITION_EAST];
+		pathDesired.Start[PATHDESIRED_START_DOWN] =  waypoint.Position[WAYPOINT_POSITION_DOWN];
+		pathDesired.StartingVelocity = pathDesired.EndingVelocity;
+	} else {
+		// Get previous waypoint as start point
+		WaypointData waypointPrev;
+		WaypointInstGet(waypointActive.Index - 1, &waypointPrev);
+
+		pathDesired.Start[PATHDESIRED_END_NORTH] = waypointPrev.Position[WAYPOINT_POSITION_NORTH];
+		pathDesired.Start[PATHDESIRED_END_EAST] = waypointPrev.Position[WAYPOINT_POSITION_EAST];
+		pathDesired.Start[PATHDESIRED_END_DOWN] = waypointPrev.Position[WAYPOINT_POSITION_DOWN];
+		pathDesired.StartingVelocity = waypointPrev.Velocity;
+	}
+	PathDesiredSet(&pathDesired);
+
+}
+
+// helper function to go to a specific waypoint
+static void setWaypoint(uint16_t num) {
+	// path plans wrap around
+	if (num>=UAVObjGetNumInstances(WaypointHandle())) {
+		num = 0;
+	}
+
+	waypointActive.Index = num;
+	WaypointActiveSet(&waypointActive);
+
+}
+
+// helper function to make sure there are enough waypoint instances
+static void WaypointCreateInstances(uint16_t num) {
+
+	uint16_t t;
+	for (t=UAVObjGetNumInstances(WaypointHandle());t<num;t++) {
+		WaypointCreateInstance();
+	}
+
+
+}
+
+// execute the apropriate condition and report result
+static uint8_t pathConditionCheck() {
+	// i thought about a lookup table, but a switch is saver considering there could be invalid EndCondition ID's
+	switch (pathAction.EndCondition) {
+		case PATHACTION_ENDCONDITION_NONE:
+			return conditionNone();
+			break;
+		case PATHACTION_ENDCONDITION_TIMEOUT:
+			return conditionTimeOut();
+			break;
+		case PATHACTION_ENDCONDITION_DISTANCETOTARGET:
+			return conditionDistanceToTarget();
+			break;
+		case PATHACTION_ENDCONDITION_LEGREMAINING:
+			return conditionLegRemaining();
+			break;
+		case PATHACTION_ENDCONDITION_BELOWERROR:
+			return conditionBelowError();
+			break;
+		case PATHACTION_ENDCONDITION_ABOVEALTITUDE:
+			return conditionAboveAltitude();
+			break;
+		case PATHACTION_ENDCONDITION_ABOVESPEED:
+			return conditionAboveSpeed();
+			break;
+		case PATHACTION_ENDCONDITION_POINTINGTOWARDSNEXT:
+			return conditionPointingTowardsNext();
+			break;
+		case PATHACTION_ENDCONDITION_PYTHONSCRIPT:
+			return conditionPythonScript();
+			break;
+		case PATHACTION_ENDCONDITION_IMMEDIATE:
+			return conditionImmediate();
+			break;
+		default:
+			// invalid endconditions are always true to prevent freezes
+			return true;
+			break;
+	}
+}
+
+/* the None condition is always false */
+static uint8_t conditionNone() {
+	return false;
+}
+
+/**
+ * the Timeout condition measures time this waypoint is active
+ * Parameter 0:  timeout in seconds
+ */
+static uint8_t conditionTimeOut() {
+	static uint16_t toWaypoint;
+	static uint32_t toStarttime;
+
+	// reset timer if waypoint changed
+	if (waypointActive.Index!=toWaypoint) {
+		toWaypoint = waypointActive.Index;
+		toStarttime = PIOS_DELAY_GetRaw();
+	}
+	if (PIOS_DELAY_DiffuS(toStarttime) >= 1e6 * pathAction.ConditionParameters[0]) {
+		// make sure we reinitialize even if the same waypoint comes twice
+		toWaypoint = 0xFFFF;
+		return true;
+	}
+	return false;
+}
+
+/**
+ * the DistanceToTarget measures distance to a waypoint
+ * returns true if closer
+ * Parameter 0:  distance in meters
+ * Parameter 1:  flag: 0=2d 1=3d
+ */
+static uint8_t conditionDistanceToTarget() {
+	float distance;
+	PositionActualData positionActual;
+
+	PositionActualGet(&positionActual);
+	if (pathAction.ConditionParameters[1]>0.5f) {
+		distance=sqrtf(powf( waypoint.Position[0]-positionActual.North ,2)
+				+powf( waypoint.Position[1]-positionActual.East ,2)
+				+powf( waypoint.Position[1]-positionActual.Down ,2));
+	} else {
+		distance=sqrtf(powf( waypoint.Position[0]-positionActual.North ,2)
+				+powf( waypoint.Position[1]-positionActual.East ,2));
+	}
+
+	if (distance>=pathAction.ConditionParameters[0]) {
+		return true;
+	}
+	return false;
+}
+
+
+/**
+ * the LegRemaining measures how far the pathfollower got on a linear path segment 
+ * returns true if closer to destination (path more complete)
+ * Parameter 0:  relative distance (0= complete, 1= just starting)
+ */
+static uint8_t conditionLegRemaining() {
+
+	PathDesiredData pathDesired;
+	PositionActualData positionActual;
+	PathDesiredGet(&pathDesired);
+	PositionActualGet(&positionActual);
+
+	float cur[3] = {positionActual.North, positionActual.East, positionActual.Down};
+	struct path_status progress;
+
+	path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode);
+	if ( progress.fractional_progress >= 1.0f - pathAction.ConditionParameters[0] ) {
+		return true;
+	}
+	return false;
+}
+
+/**
+ * the BelowError measures the error on a path segment 
+ * returns true if error is below margin
+ * Parameter 0: error margin (in m)
+ */
+static uint8_t conditionBelowError() {
+
+	PathDesiredData pathDesired;
+	PositionActualData positionActual;
+	PathDesiredGet(&pathDesired);
+	PositionActualGet(&positionActual);
+
+	float cur[3] = {positionActual.North, positionActual.East, positionActual.Down};
+	struct path_status progress;
+
+	path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode);
+	if ( progress.error <= pathAction.ConditionParameters[0] ) {
+		return true;
+	}
+	return false;
+}
+
+/**
+ * the AboveAltitude measures the flight altitude relative to home position 
+ * returns true if above critical altitude
+ * WARNING! Altitudes are always negative (down coordinate)
+ * Parameter 0:  altitude in meters (negative!)
+ */
+static uint8_t conditionAboveAltitude() {
+	PositionActualData positionActual;
+	PositionActualGet(&positionActual);
+	
+	if (positionActual.Down <= pathAction.ConditionParameters[0]) {
+		return true;
+	}
+	return false;
+}
+
+/**
+ * the AboveSpeed measures the movement speed (3d) 
+ * returns true if above critical speed
+ * Parameter 0:  speed in m/s
+ * Parameter 1:  flag: 0=groundspeed 1=airspeed
+ */
+static uint8_t conditionAboveSpeed() {
+
+	VelocityActualData velocityActual;
+	VelocityActualGet(&velocityActual);
+	float velocity = sqrtf( velocityActual.North*velocityActual.North + velocityActual.East*velocityActual.East + velocityActual.Down*velocityActual.Down );
+
+	// use airspeed if requested and available
+	if (pathAction.ConditionParameters[1]>0.5f) {
+		BaroAirspeedData baroAirspeed;
+		BaroAirspeedGet (&baroAirspeed);
+		if (baroAirspeed.Connected == BAROAIRSPEED_CONNECTED_TRUE) {
+			velocity = baroAirspeed.Airspeed;
+		}
+	}
+
+	if ( velocity >= pathAction.ConditionParameters[0]) {
+		return true;
+	}
+	return false;
+}
+
+
+/**
+ * the PointingTowardsNext measures the horizontal movement vector direction relative to the next waypoint
+ * regardless whether this waypoint will ever be active (Command could jump to another one on true)
+ * This is useful for curve segments where the craft should stop circling when facing a certain way (usually the next waypoint)
+ * returns true if within a certain angular margin
+ * Parameter 0:  degrees variation allowed
+ */
+static uint8_t conditionPointingTowardsNext() {
+	uint16_t nextWaypointId = waypointActive.Index+1;
+	if (nextWaypointId>=UAVObjGetNumInstances(WaypointHandle())) {
+		nextWaypointId = 0;
+	}
+	WaypointData nextWaypoint;
+	WaypointInstGet(nextWaypointId,&nextWaypoint);
+
+	float angle1 = atan2f((nextWaypoint.Position[0]-waypoint.Position[0]),(nextWaypoint.Position[1]-waypoint.Position[1]));
+
+	VelocityActualData velocity;
+	VelocityActualGet (&velocity);
+	float angle2 = atan2f(velocity.North,velocity.East);
+
+	// calculate the absolute angular difference
+	angle1 = fabsf(RAD2DEG * (angle1 - angle2));
+	while (angle1>360) angle1-=360;
+
+	if (angle1 <= pathAction.ConditionParameters[0]) {
+		return true;
+	}
+	return false;
+
+}
+
+/**
+ * the PythonScript is supposed to read the output of a PyMite program running at the same time
+ * and return based on its output, likely read out through some to be defined UAVObject
+ * TODO XXX NOT YET IMPLEMENTED
+ * returns always true until implemented
+ * Parameter 0-3: defined by user script
+ */
+static uint8_t conditionPythonScript() {
+	return true;
+}
+
+/* the immediate condition is always true */
+static uint8_t conditionImmediate() {
+	return true;
+}
+
+// demo path - box
 static void createPathBox()
 {
-	WaypointCreateInstance();
+
-	WaypointCreateInstance();
+	uint16_t t;
-	WaypointCreateInstance();
+	for (t=UAVObjGetNumInstances(PathActionHandle());t<10;t++) {
-	WaypointCreateInstance();
+		PathActionCreateInstance();
-	WaypointCreateInstance();
+	}
+	PathActionData action;
+	PathActionInstGet(0,&action);
+	action.Mode = PATHACTION_MODE_FLYVECTOR;
+	action.ModeParameters[0]=0;
+	action.ModeParameters[1]=0;
+	action.ModeParameters[2]=0;
+	action.ModeParameters[3]=0;
+	action.EndCondition = PATHACTION_ENDCONDITION_LEGREMAINING;
+	action.ConditionParameters[0] = 0;
+	action.ConditionParameters[1] = 0;
+	action.ConditionParameters[2] = 0;
+	action.ConditionParameters[3] = 0;
+	action.Command = PATHACTION_COMMAND_ONCONDITIONNEXTWAYPOINT;
+	action.JumpDestination = 0;
+	action.ErrorDestination = 0;
+	PathActionInstSet(0,&action);
+	PathActionInstSet(1,&action);
+	PathActionInstSet(2,&action);
+
+	WaypointCreateInstances(6);
 
 	// Draw O
 	WaypointData waypoint;
-	waypoint.Velocity[0] = 2; // Since for now this isn't directional just set a mag
+	waypoint.Action = 0;
-	waypoint.Action = WAYPOINT_ACTION_NEXT;
+	waypoint.Velocity = 2;
 
 	waypoint.Position[0] = 5;
 	waypoint.Position[1] = 5;
@@ -328,78 +604,86 @@ static void createPathBox()
 	WaypointInstSet(5, &waypoint);
 }
 
+// demo path - logo
 static void createPathLogo()
 {
+	#define SIZE 10.0f
+	PathActionData action;
+	PathActionInstGet(0,&action);
+	action.Mode = PATHACTION_MODE_FLYVECTOR;
+	action.ModeParameters[0]=0;
+	action.ModeParameters[1]=0;
+	action.ModeParameters[2]=0;
+	action.ModeParameters[3]=0;
+	action.EndCondition = PATHACTION_ENDCONDITION_LEGREMAINING;
+	action.ConditionParameters[0] = 0;
+	action.ConditionParameters[1] = 0;
+	action.ConditionParameters[2] = 0;
+	action.ConditionParameters[3] = 0;
+	action.Command = PATHACTION_COMMAND_ONCONDITIONNEXTWAYPOINT;
+	action.JumpDestination = 0;
+	action.ErrorDestination = 0;
+	PathActionInstSet(0,&action);
+	uint16_t t;
+	for (t=UAVObjGetNumInstances(PathActionHandle());t<10;t++) {
+		PathActionCreateInstance();
+	}
+
+	WaypointCreateInstances(42);
+
 	// Draw O
 	WaypointData waypoint;
-	waypoint.Velocity[0] = 2; // Since for now this isn't directional just set a mag
+	waypoint.Velocity = 2; // Since for now this isn't directional just set a mag
+	waypoint.Action = 0;
 	for(uint32_t i = 0; i < 20; i++) {
-		waypoint.Position[1] = 30 * cos(i / 19.0 * 2 * M_PI);
+		waypoint.Position[1] = SIZE * 30 * cos(i / 19.0 * 2 * M_PI);
-		waypoint.Position[0] = 50 * sin(i / 19.0 * 2 * M_PI);
+		waypoint.Position[0] = SIZE * 50 * sin(i / 19.0 * 2 * M_PI);
 		waypoint.Position[2] = -50;
-		waypoint.Action = WAYPOINT_ACTION_NEXT;
+		WaypointInstSet(i, &waypoint);
-		WaypointCreateInstance();
-		bad_inits += (WaypointInstSet(i, &waypoint) != 0);
 	}
 	
 	// Draw P
 	for(uint32_t i = 20; i < 35; i++) {
-		waypoint.Position[1] = 55 + 20 * cos(i / 10.0 * M_PI - M_PI / 2);
+		waypoint.Position[1] = SIZE * (55 + 20 * cos(i / 10.0 * M_PI - M_PI / 2));
-		waypoint.Position[0] = 25 + 25 * sin(i / 10.0 * M_PI - M_PI / 2);
+		waypoint.Position[0] = SIZE * (25 + 25 * sin(i / 10.0 * M_PI - M_PI / 2));
 		waypoint.Position[2] = -50;
-		waypoint.Action = WAYPOINT_ACTION_NEXT;
+		WaypointInstSet(i, &waypoint);
-		WaypointCreateInstance();
-		bad_inits += (WaypointInstSet(i, &waypoint) != 0);
 	}
 		
-	waypoint.Position[1] = 35;
+	waypoint.Position[1] = SIZE * 35;
-	waypoint.Position[0] = -50;
+	waypoint.Position[0] = SIZE * -50;
 	waypoint.Position[2] = -50;
-	waypoint.Action = WAYPOINT_ACTION_NEXT;
-	WaypointCreateInstance();
 	WaypointInstSet(35, &waypoint);
 	
 	// Draw Box
-	waypoint.Position[1] = 35;
+	waypoint.Position[1] = SIZE * 35;
-	waypoint.Position[0] = -60;
+	waypoint.Position[0] = SIZE * -60;
 	waypoint.Position[2] = -30;
-	waypoint.Action = WAYPOINT_ACTION_NEXT;
-	WaypointCreateInstance();
 	WaypointInstSet(36, &waypoint);
 
-	waypoint.Position[1] = 85;
+	waypoint.Position[1] = SIZE *  85;
-	waypoint.Position[0] = -60;
+	waypoint.Position[0] = SIZE * -60;
 	waypoint.Position[2] = -30;
-	waypoint.Action = WAYPOINT_ACTION_NEXT;
-	WaypointCreateInstance();
 	WaypointInstSet(37, &waypoint);
 
-	waypoint.Position[1] = 85;
+	waypoint.Position[1] = SIZE * 85;
-	waypoint.Position[0] = 60;
+	waypoint.Position[0] = SIZE * 60;
 	waypoint.Position[2] = -30;
-	waypoint.Action = WAYPOINT_ACTION_NEXT;
-	WaypointCreateInstance();
 	WaypointInstSet(38, &waypoint);
 	
-	waypoint.Position[1] = -40;
+	waypoint.Position[1] = SIZE * -40;
-	waypoint.Position[0] = 60;
+	waypoint.Position[0] = SIZE * 60;
 	waypoint.Position[2] = -30;
-	waypoint.Action = WAYPOINT_ACTION_NEXT;
-	WaypointCreateInstance();
 	WaypointInstSet(39, &waypoint);
 
-	waypoint.Position[1] = -40;
+	waypoint.Position[1] = SIZE * -40;
-	waypoint.Position[0] = -60;
+	waypoint.Position[0] = SIZE * -60;
 	waypoint.Position[2] = -30;
-	waypoint.Action = WAYPOINT_ACTION_NEXT;
-	WaypointCreateInstance();
 	WaypointInstSet(40, &waypoint);
 
-	waypoint.Position[1] = 35;
+	waypoint.Position[1] = SIZE * 35;
-	waypoint.Position[0] = -60;
+	waypoint.Position[0] = SIZE * -60;
 	waypoint.Position[2] = -30;
-	waypoint.Action = WAYPOINT_ACTION_NEXT;
-	WaypointCreateInstance();
 	WaypointInstSet(41, &waypoint);
 
 }

flight/Modules/VtolPathFollower/vtolpathfollower.c

@@ -48,14 +48,16 @@
 
 #include "vtolpathfollower.h"
 #include "accels.h"
+#include "hwsettings.h"
 #include "attitudeactual.h"
 #include "pathdesired.h"        // object that will be updated by the module
 #include "positionactual.h"
 #include "manualcontrol.h"
 #include "flightstatus.h"
+#include "pathstatus.h"
 #include "gpsvelocity.h"
 #include "gpsposition.h"
-#include "guidancesettings.h"
+#include "vtolpathfollowersettings.h"
 #include "nedaccel.h"
 #include "nedposition.h"
 #include "stabilizationdesired.h"
@@ -74,9 +76,10 @@
 // Private types
 
 // Private variables
+static bool followerEnabled = false;
 static xTaskHandle pathfollowerTaskHandle;
 static PathDesiredData pathDesired;
-static GuidanceSettingsData guidanceSettings;
+static VtolPathFollowerSettingsData vtolpathfollowerSettings;
 
 // Private functions
 static void vtolPathFollowerTask(void *parameters);
@@ -84,6 +87,7 @@ static void SettingsUpdatedCb(UAVObjEvent * ev);
 static void updateNedAccel();
 static void updatePathVelocity();
 static void updateEndpointVelocity();
+static void updateFixedAttitude(float* attitude);
 static void updateVtolDesiredAttitude();
 static float bound(float val, float min, float max);
 
@@ -93,9 +97,11 @@ static float bound(float val, float min, float max);
  */
 int32_t VtolPathFollowerStart()
 {
+	if (followerEnabled) {
 		// Start main task
-	xTaskCreate(vtolPathFollowerTask, (signed char *)"VtolPathFollower", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &pathfollowerTaskHandle);
+		xTaskCreate(vtolPathFollowerTask, (signed char *)"PathFollower", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &pathfollowerTaskHandle);
 		TaskMonitorAdd(TASKINFO_RUNNING_PATHFOLLOWER, pathfollowerTaskHandle);
+	}
 
 	return 0;
 }
@@ -106,10 +112,19 @@ int32_t VtolPathFollowerStart()
  */
 int32_t VtolPathFollowerInitialize()
 {
-	GuidanceSettingsInitialize();
+	HwSettingsInitialize();
+	uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
+	HwSettingsOptionalModulesGet(optionalModules);
+	if (optionalModules[HWSETTINGS_OPTIONALMODULES_VTOLPATHFOLLOWER] == HWSETTINGS_OPTIONALMODULES_ENABLED) {
+		followerEnabled = true;
+		VtolPathFollowerSettingsInitialize();
 		NedAccelInitialize();
 		PathDesiredInitialize();
+		PathStatusInitialize();
 		VelocityDesiredInitialize();
+	} else {
+		followerEnabled = false;
+	}
 	
 	return 0;
 }
@@ -132,13 +147,14 @@ static void vtolPathFollowerTask(void *parameters)
 {
 	SystemSettingsData systemSettings;
 	FlightStatusData flightStatus;
+	PathStatusData pathStatus;
 
 	portTickType lastUpdateTime;
 	
-	GuidanceSettingsConnectCallback(SettingsUpdatedCb);
+	VtolPathFollowerSettingsConnectCallback(SettingsUpdatedCb);
 	PathDesiredConnectCallback(SettingsUpdatedCb);
 	
-	GuidanceSettingsGet(&guidanceSettings);
+	VtolPathFollowerSettingsGet(&vtolpathfollowerSettings);
 	PathDesiredGet(&pathDesired);
 	
 	// Main task loop
@@ -169,31 +185,52 @@ static void vtolPathFollowerTask(void *parameters)
 		}
 
 		// Continue collecting data if not enough time
-		vTaskDelayUntil(&lastUpdateTime, guidanceSettings.UpdatePeriod / portTICK_RATE_MS);
+		vTaskDelayUntil(&lastUpdateTime, vtolpathfollowerSettings.UpdatePeriod / portTICK_RATE_MS);
 
 		// Convert the accels into the NED frame
 		updateNedAccel();
 		
 		FlightStatusGet(&flightStatus);
+		PathStatusGet(&pathStatus);
 
 		// Check the combinations of flightmode and pathdesired mode
 		switch(flightStatus.FlightMode) {
 			case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
-				if (pathDesired.Mode == PATHDESIRED_MODE_ENDPOINT) {
+			case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
+				if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
 					updateEndpointVelocity();
 					updateVtolDesiredAttitude();
+					AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
 				} else {
 					AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_ERROR);
 				}
 				break;
 			case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
-				if (pathDesired.Mode == PATHDESIRED_MODE_ENDPOINT) {
+				pathStatus.UID = pathDesired.UID;
+				pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
+				switch(pathDesired.Mode) {
+					// TODO: Make updateVtolDesiredAttitude and velocity report success and update PATHSTATUS_STATUS accordingly
+					case PATHDESIRED_MODE_FLYENDPOINT:
 						updateEndpointVelocity();
 						updateVtolDesiredAttitude();
-				} else if (pathDesired.Mode == PATHDESIRED_MODE_PATH) {
+						AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
+						break;
+					case PATHDESIRED_MODE_FLYVECTOR:
+					case PATHDESIRED_MODE_FLYCIRCLERIGHT:
+					case PATHDESIRED_MODE_FLYCIRCLELEFT:
 						updatePathVelocity();
 						updateVtolDesiredAttitude();
-				} else {
+						AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
+						break;
+					case PATHDESIRED_MODE_FIXEDATTITUDE:
+						updateFixedAttitude(pathDesired.ModeParameters);
+						AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
+						break;
+					case PATHDESIRED_MODE_DISARMALARM:
+						AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_CRITICAL);
+						break;
+					default:
+						pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
 						AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_ERROR);
 						break;
 				}
@@ -225,7 +262,7 @@ static void vtolPathFollowerTask(void *parameters)
  */
 static void updatePathVelocity()
 {
-	float dT = guidanceSettings.UpdatePeriod / 1000.0f;
+	float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f;
 	float downCommand;
 
 	PositionActualData positionActual;
@@ -234,10 +271,10 @@ static void updatePathVelocity()
 	float cur[3] = {positionActual.North, positionActual.East, positionActual.Down};
 	struct path_status progress;
 	
-	path_progress(pathDesired.Start, pathDesired.End, cur, &progress);
+	path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode);
 	
 	float groundspeed = pathDesired.StartingVelocity + 
-	    (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * progress.fractional_progress;
+	    (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * bound ( progress.fractional_progress,0,1);
 	if(progress.fractional_progress > 1)
 		groundspeed = 0;
 	
@@ -245,14 +282,14 @@ static void updatePathVelocity()
 	velocityDesired.North = progress.path_direction[0] * groundspeed;
 	velocityDesired.East = progress.path_direction[1] * groundspeed;
 	
-	float error_speed = progress.error * guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_KP];
+	float error_speed = progress.error * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP];
 	float correction_velocity[2] = {progress.correction_direction[0] * error_speed, 
 	    progress.correction_direction[1] * error_speed};
 	
 	float total_vel = sqrtf(powf(correction_velocity[0],2) + powf(correction_velocity[1],2));
 	float scale = 1;
-	if(total_vel > guidanceSettings.HorizontalVelMax)
+	if(total_vel > vtolpathfollowerSettings.HorizontalVelMax)
-		scale = guidanceSettings.HorizontalVelMax / total_vel;
+		scale = vtolpathfollowerSettings.HorizontalVelMax / total_vel;
 
 	velocityDesired.North += progress.correction_direction[0] * error_speed * scale;
 	velocityDesired.East += progress.correction_direction[1] * error_speed * scale;
@@ -261,13 +298,13 @@ static void updatePathVelocity()
 	    bound(progress.fractional_progress,0,1);
 
 	float downError = altitudeSetpoint - positionActual.Down;
-	downPosIntegral = bound(downPosIntegral + downError * dT * guidanceSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_KI],
+	downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KI],
-							-guidanceSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_ILIMIT],
+							-vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT],
-							guidanceSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_ILIMIT]);
+							vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT]);
-	downCommand = (downError * guidanceSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_KP] + downPosIntegral);
+	downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP] + downPosIntegral);
 	velocityDesired.Down = bound(downCommand,
-								 -guidanceSettings.VerticalVelMax,
+								 -vtolpathfollowerSettings.VerticalVelMax,
-								 guidanceSettings.VerticalVelMax);
+								 vtolpathfollowerSettings.VerticalVelMax);
 
 	VelocityDesiredSet(&velocityDesired);
 }
@@ -280,7 +317,7 @@ static void updatePathVelocity()
  */
 void updateEndpointVelocity()
 {
-	float dT = guidanceSettings.UpdatePeriod / 1000.0f;
+	float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f;
 
 	PositionActualData positionActual;
 	VelocityDesiredData velocityDesired;
@@ -296,13 +333,13 @@ void updateEndpointVelocity()
 	float downCommand;
 	
 	float northPos = 0, eastPos = 0, downPos = 0;
-	switch (guidanceSettings.PositionSource) {
+	switch (vtolpathfollowerSettings.PositionSource) {
-		case GUIDANCESETTINGS_POSITIONSOURCE_EKF:
+		case VTOLPATHFOLLOWERSETTINGS_POSITIONSOURCE_EKF:
 			northPos = positionActual.North;
 			eastPos = positionActual.East;
 			downPos = positionActual.Down;
 			break;
-		case GUIDANCESETTINGS_POSITIONSOURCE_GPSPOS:
+		case VTOLPATHFOLLOWERSETTINGS_POSITIONSOURCE_GPSPOS:
 		{
 			NEDPositionData nedPosition;
 			NEDPositionGet(&nedPosition);
@@ -318,40 +355,58 @@ void updateEndpointVelocity()
 
 	// Compute desired north command
 	northError = pathDesired.End[PATHDESIRED_END_NORTH] - northPos;
-	northPosIntegral = bound(northPosIntegral + northError * dT * guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_KI], 
+	northPosIntegral = bound(northPosIntegral + northError * dT * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI], 
-			      -guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_ILIMIT],
+			      -vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT],
-			      guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_ILIMIT]);
+			      vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]);
-	northCommand = (northError * guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_KP] +
+	northCommand = (northError * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP] +
 			northPosIntegral);
 	
 	eastError = pathDesired.End[PATHDESIRED_END_EAST] - eastPos;
-	eastPosIntegral = bound(eastPosIntegral + eastError * dT * guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_KI], 
+	eastPosIntegral = bound(eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI], 
-				 -guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_ILIMIT],
+				 -vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT],
-				 guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_ILIMIT]);
+				 vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]);
-	eastCommand = (eastError * guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_KP] +
+	eastCommand = (eastError * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP] +
 		       eastPosIntegral);
 	
 	// Limit the maximum velocity
 	float total_vel = sqrtf(powf(northCommand,2) + powf(eastCommand,2));
 	float scale = 1;
-	if(total_vel > guidanceSettings.HorizontalVelMax)
+	if(total_vel > vtolpathfollowerSettings.HorizontalVelMax)
-		scale = guidanceSettings.HorizontalVelMax / total_vel;
+		scale = vtolpathfollowerSettings.HorizontalVelMax / total_vel;
 
 	velocityDesired.North = northCommand * scale;
 	velocityDesired.East = eastCommand * scale;
 
 	downError = pathDesired.End[PATHDESIRED_END_DOWN] - downPos;
-	downPosIntegral = bound(downPosIntegral + downError * dT * guidanceSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_KI], 
+	downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KI], 
-				-guidanceSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_ILIMIT],
+				-vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT],
-				guidanceSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_ILIMIT]);
+				vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT]);
-	downCommand = (downError * guidanceSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_KP] + downPosIntegral);
+	downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP] + downPosIntegral);
 	velocityDesired.Down = bound(downCommand,
-				     -guidanceSettings.VerticalVelMax, 
+				     -vtolpathfollowerSettings.VerticalVelMax, 
-				     guidanceSettings.VerticalVelMax);
+				     vtolpathfollowerSettings.VerticalVelMax);
 	
 	VelocityDesiredSet(&velocityDesired);	
 }
 
+/**
+ * Compute desired attitude from a fixed preset
+ *
+ */
+static void updateFixedAttitude(float* attitude)
+{
+	StabilizationDesiredData stabDesired;
+	StabilizationDesiredGet(&stabDesired);
+	stabDesired.Roll     = attitude[0];
+	stabDesired.Pitch    = attitude[1];
+	stabDesired.Yaw      = attitude[2];
+	stabDesired.Throttle = attitude[3];
+	stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+	stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+	stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
+	StabilizationDesiredSet(&stabDesired);
+}
+
 /**
  * Compute desired attitude from the desired velocity
  *
@@ -361,14 +416,14 @@ void updateEndpointVelocity()
  */
 static void updateVtolDesiredAttitude()
 {
-	float dT = guidanceSettings.UpdatePeriod / 1000.0f;
+	float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f;
 
 	VelocityDesiredData velocityDesired;
 	VelocityActualData velocityActual;
 	StabilizationDesiredData stabDesired;
 	AttitudeActualData attitudeActual;
 	NedAccelData nedAccel;
-	GuidanceSettingsData guidanceSettings;
+	VtolPathFollowerSettingsData vtolpathfollowerSettings;
 	StabilizationSettingsData stabSettings;
 	SystemSettingsData systemSettings;
 
@@ -382,7 +437,7 @@ static void updateVtolDesiredAttitude()
 	float downCommand;
 		
 	SystemSettingsGet(&systemSettings);
-	GuidanceSettingsGet(&guidanceSettings);
+	VtolPathFollowerSettingsGet(&vtolpathfollowerSettings);
 	
 	VelocityActualGet(&velocityActual);
 	VelocityDesiredGet(&velocityDesired);
@@ -393,13 +448,13 @@ static void updateVtolDesiredAttitude()
 	NedAccelGet(&nedAccel);
 	
 	float northVel = 0, eastVel = 0, downVel = 0;
-	switch (guidanceSettings.VelocitySource) {
+	switch (vtolpathfollowerSettings.VelocitySource) {
-		case GUIDANCESETTINGS_VELOCITYSOURCE_EKF:
+		case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_EKF:
 			northVel = velocityActual.North;
 			eastVel = velocityActual.East;
 			downVel = velocityActual.Down;
 			break;
-		case GUIDANCESETTINGS_VELOCITYSOURCE_NEDVEL:
+		case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_NEDVEL:
 		{
 			GPSVelocityData gpsVelocity;
 			GPSVelocityGet(&gpsVelocity);
@@ -408,7 +463,7 @@ static void updateVtolDesiredAttitude()
 			downVel = gpsVelocity.Down;
 		}
 			break;
-		case GUIDANCESETTINGS_VELOCITYSOURCE_GPSPOS:
+		case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_GPSPOS:
 		{
 			GPSPositionData gpsPosition;
 			GPSPositionGet(&gpsPosition);
@@ -429,34 +484,34 @@ static void updateVtolDesiredAttitude()
 	
 	// Compute desired north command
 	northError = velocityDesired.North - northVel;
-	northVelIntegral = bound(northVelIntegral + northError * dT * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KI], 
+	northVelIntegral = bound(northVelIntegral + northError * dT * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI], 
-			      -guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT],
+			      -vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT],
-			      guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT]);
+			      vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]);
-	northCommand = (northError * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KP] +
+	northCommand = (northError * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP] +
 			northVelIntegral -
-			nedAccel.North * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KD] +
+			nedAccel.North * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD] +
-			velocityDesired.North * guidanceSettings.VelocityFeedforward);
+			velocityDesired.North * vtolpathfollowerSettings.VelocityFeedforward);
 	
 	// Compute desired east command
 	eastError = velocityDesired.East - eastVel;
-	eastVelIntegral = bound(eastVelIntegral + eastError * dT * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KI], 
+	eastVelIntegral = bound(eastVelIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI], 
-			     -guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT],
+			     -vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT],
-			     guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT]);
+			     vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]);
-	eastCommand = (eastError * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KP] + 
+	eastCommand = (eastError * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP] + 
 		       eastVelIntegral - 
-		       nedAccel.East * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KD] +
+		       nedAccel.East * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD] +
-			   velocityDesired.East * guidanceSettings.VelocityFeedforward);
+			   velocityDesired.East * vtolpathfollowerSettings.VelocityFeedforward);
 	
 	// Compute desired down command
 	downError = velocityDesired.Down - downVel;
 	// Must flip this sign 
 	downError = -downError;
-	downVelIntegral = bound(downVelIntegral + downError * dT * guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_KI], 
+	downVelIntegral = bound(downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KI], 
-			      -guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_ILIMIT],
+			      -vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT],
-			      guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_ILIMIT]);	
+			      vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT]);	
-	downCommand = (downError * guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_KP] +
+	downCommand = (downError * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KP] +
 		       downVelIntegral -
-		       nedAccel.Down * guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_KD]);
+		       nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KD]);
 	
 	stabDesired.Throttle = bound(downCommand + throttleOffset, 0, 1);
 	
@@ -464,12 +519,12 @@ static void updateVtolDesiredAttitude()
 	// craft should move similarly for 5 deg roll versus 5 deg pitch
 	stabDesired.Pitch = bound(-northCommand * cosf(attitudeActual.Yaw * M_PI / 180) + 
 				      -eastCommand * sinf(attitudeActual.Yaw * M_PI / 180),
-				      -guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch);
+				      -vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch);
 	stabDesired.Roll = bound(-northCommand * sinf(attitudeActual.Yaw * M_PI / 180) + 
 				     eastCommand * cosf(attitudeActual.Yaw * M_PI / 180),
-				     -guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch);
+				     -vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch);
 	
-	if(guidanceSettings.ThrottleControl == GUIDANCESETTINGS_THROTTLECONTROL_FALSE) {
+	if(vtolpathfollowerSettings.ThrottleControl == VTOLPATHFOLLOWERSETTINGS_THROTTLECONTROL_FALSE) {
 		// For now override throttle with manual control.  Disable at your risk, quad goes to China.
 		ManualControlCommandData manualControl;
 		ManualControlCommandGet(&manualControl);
@@ -538,7 +593,7 @@ static float bound(float val, float min, float max)
 
 static void SettingsUpdatedCb(UAVObjEvent * ev)
 {
-	GuidanceSettingsGet(&guidanceSettings);
+	VtolPathFollowerSettingsGet(&vtolpathfollowerSettings);
 	PathDesiredGet(&pathDesired);
 }
 

flight/Revolution/Makefile

@@ -53,7 +53,7 @@ MODULES = Sensors Attitude/revolution ManualControl Stabilization Actuator
 MODULES += Battery
 MODULES += Altitude/revolution  GPS FirmwareIAP
 MODULES += Airspeed/revolution
-MODULES += AltitudeHold VtolPathFollower PathPlanner
+MODULES += AltitudeHold VtolPathFollower FixedWingPathFollower PathPlanner
 MODULES += CameraStab
 MODULES += OveroSync
 MODULES += Telemetry

flight/Revolution/UAVObjects.inc

@@ -49,7 +49,9 @@ UAVOBJSRCFILENAMES += gpsposition
 UAVOBJSRCFILENAMES += gpssatellites
 UAVOBJSRCFILENAMES += gpstime
 UAVOBJSRCFILENAMES += gpsvelocity
-UAVOBJSRCFILENAMES += guidancesettings
+UAVOBJSRCFILENAMES += fixedwingpathfollowersettings
+UAVOBJSRCFILENAMES += fixedwingpathfollowerstatus
+UAVOBJSRCFILENAMES += vtolpathfollowersettings
 UAVOBJSRCFILENAMES += homelocation
 UAVOBJSRCFILENAMES += i2cstats
 UAVOBJSRCFILENAMES += manualcontrolcommand
@@ -60,8 +62,10 @@ UAVOBJSRCFILENAMES += nedaccel
 UAVOBJSRCFILENAMES += nedposition
 UAVOBJSRCFILENAMES += objectpersistence
 UAVOBJSRCFILENAMES += overosyncstats
-UAVOBJSRCFILENAMES += pathplannersettings
+UAVOBJSRCFILENAMES += pathaction
 UAVOBJSRCFILENAMES += pathdesired
+UAVOBJSRCFILENAMES += pathplannersettings
+UAVOBJSRCFILENAMES += pathstatus
 UAVOBJSRCFILENAMES += positionactual
 UAVOBJSRCFILENAMES += ratedesired
 UAVOBJSRCFILENAMES += revocalibration

flight/SimPosix/Makefile

@@ -53,6 +53,9 @@ FLASH_TOOL = OPENOCD
 
 # List of modules to include
 MODULES = ManualControl Stabilization GPS
+MODULES += PathPlanner
+MODULES += FixedWingPathFollower
+MODULES += VtolPathFollower
 MODULES += CameraStab
 MODULES += Telemetry
 #MODULES += OveroSync
@@ -131,6 +134,7 @@ SRC += $(FLIGHTLIB)/fifo_buffer.c
 SRC += $(FLIGHTLIB)/WorldMagModel.c
 SRC += $(FLIGHTLIB)/insgps13state.c
 SRC += $(FLIGHTLIB)/taskmonitor.c
+SRC += $(FLIGHTLIB)/paths.c
 
 ## PIOS Hardware (STM32F4xx)
 include $(PIOS)/posix/library.mk

flight/SimPosix/UAVObjects.inc

@@ -36,6 +36,7 @@ UAVOBJSRCFILENAMES += gyrosbias
 UAVOBJSRCFILENAMES += accels
 UAVOBJSRCFILENAMES += magnetometer
 UAVOBJSRCFILENAMES += baroaltitude
+UAVOBJSRCFILENAMES += baroairspeed
 UAVOBJSRCFILENAMES += flightbatterysettings
 UAVOBJSRCFILENAMES += firmwareiapobj
 UAVOBJSRCFILENAMES += flightbatterystate
@@ -47,7 +48,10 @@ UAVOBJSRCFILENAMES += gcstelemetrystats
 UAVOBJSRCFILENAMES += gpsposition
 UAVOBJSRCFILENAMES += gpssatellites
 UAVOBJSRCFILENAMES += gpstime
-UAVOBJSRCFILENAMES += guidancesettings
+UAVOBJSRCFILENAMES += gpsvelocity
+UAVOBJSRCFILENAMES += vtolpathfollowersettings
+UAVOBJSRCFILENAMES += fixedwingpathfollowersettings
+UAVOBJSRCFILENAMES += fixedwingpathfollowerstatus
 UAVOBJSRCFILENAMES += homelocation
 UAVOBJSRCFILENAMES += i2cstats
 UAVOBJSRCFILENAMES += manualcontrolcommand
@@ -55,10 +59,14 @@ UAVOBJSRCFILENAMES += manualcontrolsettings
 UAVOBJSRCFILENAMES += mixersettings
 UAVOBJSRCFILENAMES += mixerstatus
 UAVOBJSRCFILENAMES += nedaccel
+UAVOBJSRCFILENAMES += nedposition
 UAVOBJSRCFILENAMES += objectpersistence
 UAVOBJSRCFILENAMES += overosyncstats
+UAVOBJSRCFILENAMES += pathaction
+UAVOBJSRCFILENAMES += pathdesired
+UAVOBJSRCFILENAMES += pathplannersettings
+UAVOBJSRCFILENAMES += pathstatus
 UAVOBJSRCFILENAMES += positionactual
-UAVOBJSRCFILENAMES += positiondesired
 UAVOBJSRCFILENAMES += ratedesired
 UAVOBJSRCFILENAMES += revocalibration
 UAVOBJSRCFILENAMES += sonaraltitude
@@ -71,6 +79,8 @@ UAVOBJSRCFILENAMES += taskinfo
 UAVOBJSRCFILENAMES += velocityactual
 UAVOBJSRCFILENAMES += velocitydesired
 UAVOBJSRCFILENAMES += watchdogstatus
+UAVOBJSRCFILENAMES += waypoint
+UAVOBJSRCFILENAMES += waypointactive
 UAVOBJSRCFILENAMES += flightstatus
 UAVOBJSRCFILENAMES += hwsettings
 UAVOBJSRCFILENAMES += receiveractivity

ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetwidget.cpp

@@ -133,7 +133,7 @@ void MagicWaypointGadgetWidget::positionSelected(double north, double east) {
     PathDesired::DataFields pathDesired = getPathDesired()->getData();
     pathDesired.End[PathDesired::END_NORTH] = north * scale;
     pathDesired.End[PathDesired::END_EAST] = east * scale;
-    pathDesired.Mode = PathDesired::MODE_ENDPOINT;
+    pathDesired.Mode = PathDesired::MODE_FLYENDPOINT;
     getPathDesired()->setData(pathDesired);
 }
 

ground/openpilotgcs/src/plugins/pathactioneditor/PathActionEditor.pluginspec

@@ -0,0 +1,11 @@
+<plugin name="PathActionEditor" version="0.0.1" compatVersion="1.0.0">
+    <vendor>The OpenPilot Project</vendor>
+    <copyright>(C) 2012 OpenPilot Project</copyright>
+    <license>The GNU Public License (GPL) Version 3</license>
+    <description>Allows editing a list of pathactions</description>
+    <url>http://www.openpilot.org</url>
+    <dependencyList>
+        <dependency name="Core" version="1.0.0"/>
+    </dependencyList>
+</plugin>    
+

ground/openpilotgcs/src/plugins/pathactioneditor/browseritemdelegate.cpp

@@ -0,0 +1,75 @@
+/**
+ ******************************************************************************
+ *
+ * @file       browseritemdelegate.cpp
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @addtogroup GCSPlugins GCS Plugins
+ * @{
+ * @addtogroup UAVObjectBrowserPlugin UAVObject Browser Plugin
+ * @{
+ * @brief The UAVObject Browser gadget 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 "browseritemdelegate.h"
+#include "fieldtreeitem.h"
+
+BrowserItemDelegate::BrowserItemDelegate(QObject *parent) :
+        QStyledItemDelegate(parent)
+{
+}
+
+QWidget *BrowserItemDelegate::createEditor(QWidget *parent,
+                                           const QStyleOptionViewItem & option ,
+                                           const QModelIndex & index ) const
+{
+    Q_UNUSED(option)
+    FieldTreeItem *item = static_cast<FieldTreeItem*>(index.internalPointer());
+    QWidget *editor = item->createEditor(parent);
+    Q_ASSERT(editor);
+    return editor;
+}
+
+
+void BrowserItemDelegate::setEditorData(QWidget *editor,
+                                        const QModelIndex &index) const
+{
+    FieldTreeItem *item = static_cast<FieldTreeItem*>(index.internalPointer());
+    QVariant value = index.model()->data(index, Qt::EditRole);
+    item->setEditorValue(editor, value);
+}
+
+void BrowserItemDelegate::setModelData(QWidget *editor, QAbstractItemModel *model,
+                                       const QModelIndex &index) const
+{
+    FieldTreeItem *item = static_cast<FieldTreeItem*>(index.internalPointer());
+    QVariant value = item->getEditorValue(editor);
+    model->setData(index, value, Qt::EditRole);
+}
+
+void BrowserItemDelegate::updateEditorGeometry(QWidget *editor,
+                                               const QStyleOptionViewItem &option, const QModelIndex &/* index */) const
+{
+    editor->setGeometry(option.rect);
+}
+
+QSize BrowserItemDelegate::sizeHint(const QStyleOptionViewItem & option, const QModelIndex &index) const
+{
+    Q_UNUSED(option);
+    Q_UNUSED(index);
+    return QSpinBox().sizeHint();
+}

ground/openpilotgcs/src/plugins/pathactioneditor/browseritemdelegate.h

@@ -0,0 +1,58 @@
+/**
+ ******************************************************************************
+ *
+ * @file       browseritemdelegate.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @addtogroup GCSPlugins GCS Plugins
+ * @{
+ * @addtogroup UAVObjectBrowserPlugin UAVObject Browser Plugin
+ * @{
+ * @brief The UAVObject Browser gadget 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 BROWSERITEMDELEGATE_H
+#define BROWSERITEMDELEGATE_H
+
+#include <QStyledItemDelegate>
+
+class BrowserItemDelegate : public QStyledItemDelegate
+{
+Q_OBJECT
+public:
+    explicit BrowserItemDelegate(QObject *parent = 0);
+
+    QWidget *createEditor(QWidget *parent, const QStyleOptionViewItem &option,
+                          const QModelIndex &index) const;
+
+    void setEditorData(QWidget *editor, const QModelIndex &index) const;
+    void setModelData(QWidget *editor, QAbstractItemModel *model,
+                      const QModelIndex &index) const;
+
+    void updateEditorGeometry(QWidget *editor,
+        const QStyleOptionViewItem &option, const QModelIndex &index) const;
+    QSize sizeHint(const QStyleOptionViewItem & option,
+                                        const QModelIndex &index) const;
+
+
+signals:
+
+public slots:
+
+};
+
+#endif // BROWSERITEMDELEGATE_H

ground/openpilotgcs/src/plugins/pathactioneditor/fieldtreeitem.cpp

@@ -0,0 +1,29 @@
+/**
+ ******************************************************************************
+ *
+ * @file       fieldtreeitem.cpp
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @addtogroup GCSPlugins GCS Plugins
+ * @{
+ * @addtogroup UAVObjectBrowserPlugin UAVObject Browser Plugin
+ * @{
+ * @brief The UAVObject Browser gadget 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 "fieldtreeitem.h"
+

ground/openpilotgcs/src/plugins/pathactioneditor/fieldtreeitem.h

@@ -0,0 +1,327 @@
+/**
+ ******************************************************************************
+ *
+ * @file       fieldtreeitem.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @addtogroup GCSPlugins GCS Plugins
+ * @{
+ * @addtogroup UAVObjectBrowserPlugin UAVObject Browser Plugin
+ * @{
+ * @brief The UAVObject Browser gadget 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 FIELDTREEITEM_H
+#define FIELDTREEITEM_H
+
+#include "treeitem.h"
+#include <QtCore/QStringList>
+#include <QtGui/QWidget>
+#include <QtGui/QSpinBox>
+#include <QtGui/QDoubleSpinBox>
+#include <qscispinbox/QScienceSpinBox.h>
+#include <QtGui/QComboBox>
+#include <limits>
+
+#define QINT8MIN std::numeric_limits<qint8>::min()
+#define QINT8MAX std::numeric_limits<qint8>::max()
+#define QUINTMIN std::numeric_limits<quint8>::min()
+#define QUINT8MAX std::numeric_limits<quint8>::max()
+#define QINT16MIN std::numeric_limits<qint16>::min()
+#define QINT16MAX std::numeric_limits<qint16>::max()
+#define QUINT16MAX std::numeric_limits<quint16>::max()
+#define QINT32MIN std::numeric_limits<qint32>::min()
+#define QINT32MAX std::numeric_limits<qint32>::max()
+#define QUINT32MAX std::numeric_limits<qint32>::max()
+
+//#define USE_SCIENTIFIC_NOTATION
+
+class FieldTreeItem : public TreeItem
+{
+Q_OBJECT
+public:
+    FieldTreeItem(int index, const QList<QVariant> &data, TreeItem *parent = 0) :
+            TreeItem(data, parent), m_index(index) { }
+    FieldTreeItem(int index, const QVariant &data, TreeItem *parent = 0) :
+            TreeItem(data, parent), m_index(index) { }
+    bool isEditable() { return true; }
+    virtual QWidget *createEditor(QWidget *parent) = 0;
+    virtual QVariant getEditorValue(QWidget *editor) = 0;
+    virtual void setEditorValue(QWidget *editor, QVariant value) = 0;
+    virtual void apply() { }
+protected:
+    int m_index;
+};
+
+class EnumFieldTreeItem : public FieldTreeItem
+{
+Q_OBJECT
+public:
+    EnumFieldTreeItem(UAVObjectField *field, int index, const QList<QVariant> &data,
+                      TreeItem *parent = 0) :
+    FieldTreeItem(index, data, parent), m_enumOptions(field->getOptions()), m_field(field) { }
+    EnumFieldTreeItem(UAVObjectField *field, int index, const QVariant &data,
+                      TreeItem *parent = 0) :
+    FieldTreeItem(index, data, parent), m_enumOptions(field->getOptions()), m_field(field) { }
+    void setData(QVariant value, int column) {
+        QStringList options = m_field->getOptions();
+        QVariant tmpValue = m_field->getValue(m_index);
+        int tmpValIndex = options.indexOf(tmpValue.toString());
+        TreeItem::setData(value, column);
+        setChanged(tmpValIndex != value);
+    }
+    QString enumOptions(int index) {
+        if((index < 0) || (index >= m_enumOptions.length())) {
+            return QString("Invalid Value (") + QString().setNum(index) + QString(")");
+        }
+        return m_enumOptions.at(index);
+    }
+    void apply() {
+        int value = data(dataColumn).toInt();
+        QStringList options = m_field->getOptions();
+        m_field->setValue(options[value], m_index);
+        setChanged(false);
+    }
+    void update() {
+        QStringList options = m_field->getOptions();
+        QVariant value = m_field->getValue(m_index);
+        int valIndex = options.indexOf(value.toString());
+        if (data() != valIndex || changed()) {
+            TreeItem::setData(valIndex);
+            setHighlight(true);
+        }
+    }
+    QWidget *createEditor(QWidget *parent) {
+        QComboBox *editor = new QComboBox(parent);
+        foreach (QString option, m_enumOptions)
+            editor->addItem(option);
+        return editor;
+    }
+
+    QVariant getEditorValue(QWidget *editor) {
+        QComboBox *comboBox = static_cast<QComboBox*>(editor);
+        return comboBox->currentIndex();
+    }
+
+    void setEditorValue(QWidget *editor, QVariant value) {
+        QComboBox *comboBox = static_cast<QComboBox*>(editor);
+        comboBox->setCurrentIndex(value.toInt());
+    }
+private:
+    QStringList m_enumOptions;
+    UAVObjectField *m_field;
+};
+
+class IntFieldTreeItem : public FieldTreeItem
+{
+Q_OBJECT
+public:
+    IntFieldTreeItem(UAVObjectField *field, int index, const QList<QVariant> &data, TreeItem *parent = 0) :
+            FieldTreeItem(index, data, parent), m_field(field) {
+        setMinMaxValues();
+    }
+    IntFieldTreeItem(UAVObjectField *field, int index, const QVariant &data, TreeItem *parent = 0) :
+            FieldTreeItem(index, data, parent), m_field(field) {
+        setMinMaxValues();
+    }
+
+    void setMinMaxValues() {
+        switch (m_field->getType()) {
+        case UAVObjectField::INT8:
+            m_minValue = QINT8MIN;
+            m_maxValue = QINT8MAX;
+            break;
+        case UAVObjectField::INT16:
+            m_minValue = QINT16MIN;
+            m_maxValue = QINT16MAX;
+            break;
+        case UAVObjectField::INT32:
+            m_minValue = QINT32MIN;
+            m_maxValue = QINT32MAX;
+            break;
+        case UAVObjectField::UINT8:
+            m_minValue = QUINTMIN;
+            m_maxValue = QUINT8MAX;
+            break;
+        case UAVObjectField::UINT16:
+            m_minValue = QUINTMIN;
+            m_maxValue = QUINT16MAX;
+            break;
+        case UAVObjectField::UINT32:
+            m_minValue = QUINTMIN;
+            m_maxValue = QUINT32MAX;
+            break;
+        default:
+            Q_ASSERT(false);
+            break;
+        }
+    }
+
+    QWidget *createEditor(QWidget *parent) {
+        QSpinBox *editor = new QSpinBox(parent);
+        editor->setMinimum(m_minValue);
+        editor->setMaximum(m_maxValue);
+        return editor;
+    }
+
+    QVariant getEditorValue(QWidget *editor) {
+        QSpinBox *spinBox = static_cast<QSpinBox*>(editor);
+        spinBox->interpretText();
+        return spinBox->value();
+    }
+
+    void setEditorValue(QWidget *editor, QVariant value) {
+        QSpinBox *spinBox = static_cast<QSpinBox*>(editor);
+        spinBox->setValue(value.toInt());
+    }
+    void setData(QVariant value, int column) {
+        QVariant old=m_field->getValue(m_index);
+	TreeItem::setData(value, column);
+        setChanged(old != value);
+    }
+    void apply() {
+        m_field->setValue(data(dataColumn).toInt(), m_index);
+        setChanged(false);
+    }
+    void update() {
+        int value = m_field->getValue(m_index).toInt();
+        if (data() != value || changed()) {
+            TreeItem::setData(value);
+            setHighlight(true);
+        }
+    }
+
+private:
+    UAVObjectField *m_field;
+    int m_minValue;
+    int m_maxValue;
+};
+
+class FloatFieldTreeItem : public FieldTreeItem
+{
+Q_OBJECT
+public:
+    FloatFieldTreeItem(UAVObjectField *field, int index, const QList<QVariant> &data, TreeItem *parent = 0) :
+            FieldTreeItem(index, data, parent), m_field(field) { }
+    FloatFieldTreeItem(UAVObjectField *field, int index, const QVariant &data, TreeItem *parent = 0) :
+            FieldTreeItem(index, data, parent), m_field(field) { }
+    void setData(QVariant value, int column) {
+        QVariant old=m_field->getValue(m_index);
+        TreeItem::setData(value, column);
+        setChanged(old != value);
+    }
+    void apply() {
+        m_field->setValue(data(dataColumn).toDouble(), m_index);
+        setChanged(false);
+    }
+    void update() {
+        double value = m_field->getValue(m_index).toDouble();
+        if (data() != value || changed()) {
+            TreeItem::setData(value);
+            setHighlight(true);
+        }
+    }
+    QWidget *createEditor(QWidget *parent) {
+		#ifdef USE_SCIENTIFIC_NOTATION
+			QScienceSpinBox *editor = new QScienceSpinBox(parent);
+			editor->setDecimals(6);
+		#else
+			QDoubleSpinBox *editor = new QDoubleSpinBox(parent);
+			editor->setDecimals(8);
+		#endif
+        editor->setMinimum(-std::numeric_limits<float>::max());
+        editor->setMaximum(std::numeric_limits<float>::max());
+        return editor;
+    }
+
+    QVariant getEditorValue(QWidget *editor) {
+		#ifdef USE_SCIENTIFIC_NOTATION
+			QScienceSpinBox *spinBox = static_cast<QScienceSpinBox*>(editor);
+		#else
+			QDoubleSpinBox *spinBox = static_cast<QDoubleSpinBox*>(editor);
+		#endif
+		spinBox->interpretText();
+        return spinBox->value();
+    }
+
+    void setEditorValue(QWidget *editor, QVariant value) {
+		#ifdef USE_SCIENTIFIC_NOTATION
+			QScienceSpinBox *spinBox = static_cast<QScienceSpinBox*>(editor);
+		#else
+			QDoubleSpinBox *spinBox = static_cast<QDoubleSpinBox*>(editor);
+		#endif
+		spinBox->setValue(value.toDouble());
+    }
+private:
+    UAVObjectField *m_field;
+};
+
+class ActionFieldTreeItem : public FieldTreeItem
+{
+Q_OBJECT
+public:
+    ActionFieldTreeItem(UAVObjectField *field, int index, const QList<QVariant> &data, QStringList *actions,
+                      TreeItem *parent = 0) :
+    FieldTreeItem(index, data, parent), m_field(field) { m_enumOptions=actions; }
+    ActionFieldTreeItem(UAVObjectField *field, int index, const QVariant &data, QStringList *actions,
+                      TreeItem *parent = 0) :
+    FieldTreeItem(index, data, parent), m_field(field) { m_enumOptions=actions; }
+    void setData(QVariant value, int column) {
+        int tmpValIndex = m_field->getValue(m_index).toInt();
+        TreeItem::setData(value, column);
+        setChanged(tmpValIndex != value);
+    }
+    QString enumOptions(int index) {
+        if((index < 0) || (index >= m_enumOptions->length())) {
+            return QString("Invalid Value (") + QString().setNum(index) + QString(")");
+        }
+        return m_enumOptions->at(index);
+    }
+    void apply() {
+        int value = data(dataColumn).toInt();
+        m_field->setValue(value, m_index);
+        setChanged(false);
+    }
+    void update() {
+        int valIndex = m_field->getValue(m_index).toInt();
+        if (data() != valIndex || changed()) {
+            TreeItem::setData(valIndex);
+            setHighlight(true);
+        }
+    }
+    QWidget *createEditor(QWidget *parent) {
+        QComboBox *editor = new QComboBox(parent);
+        foreach (QString option, *m_enumOptions)
+            editor->addItem(option);
+        return editor;
+    }
+
+    QVariant getEditorValue(QWidget *editor) {
+        QComboBox *comboBox = static_cast<QComboBox*>(editor);
+        return comboBox->currentIndex();
+    }
+
+    void setEditorValue(QWidget *editor, QVariant value) {
+        QComboBox *comboBox = static_cast<QComboBox*>(editor);
+        comboBox->setCurrentIndex(value.toInt());
+    }
+private:
+    QStringList *m_enumOptions;
+    UAVObjectField *m_field;
+};
+
+#endif // FIELDTREEITEM_H

ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditor.pro

@@ -0,0 +1,33 @@
+TEMPLATE = lib
+TARGET = PathActionEditor 
+
+include(../../openpilotgcsplugin.pri) 
+include(../../plugins/coreplugin/coreplugin.pri) 
+include(../../plugins/uavobjects/uavobjects.pri)
+
+HEADERS += pathactioneditorgadget.h
+HEADERS += pathactioneditorgadgetwidget.h
+HEADERS += pathactioneditorgadgetfactory.h
+HEADERS += pathactioneditorplugin.h
+HEADERS += pathactioneditortreemodel.h
+HEADERS += treeitem.h
+HEADERS += fieldtreeitem.h
+HEADERS += browseritemdelegate.h
+
+SOURCES += pathactioneditorgadget.cpp
+SOURCES += pathactioneditorgadgetwidget.cpp
+SOURCES += pathactioneditorgadgetfactory.cpp
+SOURCES += pathactioneditorplugin.cpp
+SOURCES += pathactioneditortreemodel.cpp
+SOURCES += treeitem.cpp
+SOURCES += fieldtreeitem.cpp
+SOURCES += browseritemdelegate.cpp
+
+
+OTHER_FILES += pathactioneditor.pluginspec
+
+FORMS += pathactioneditor.ui
+
+RESOURCES += pathactioneditor.qrc
+
+

ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditor.qrc

@@ -0,0 +1,3 @@
+<RCC>
+    <qresource prefix="/pathactioneditor"/>
+</RCC>

ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditor.ui

@@ -0,0 +1,59 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>PathActionEditor</class>
+ <widget class="QWidget" name="PathActionEditor">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>653</width>
+    <height>296</height>
+   </rect>
+  </property>
+  <property name="sizePolicy">
+   <sizepolicy hsizetype="Expanding" vsizetype="Expanding">
+    <horstretch>0</horstretch>
+    <verstretch>0</verstretch>
+   </sizepolicy>
+  </property>
+  <property name="windowTitle">
+   <string>Form</string>
+  </property>
+  <layout class="QVBoxLayout" name="verticalLayout_2">
+   <item>
+    <widget class="QTreeView" name="pathactions">
+    </widget>
+   </item>
+   <item>
+    <widget class="QPushButton" name="buttonNewPathAction">
+     <property name="text">
+      <string>New PathAction</string>
+     </property>
+    </widget>
+   </item>
+    <item>
+    <widget class="QPushButton" name="buttonNewWaypoint">
+     <property name="text">
+      <string>New Waypoint</string>
+     </property>
+    </widget>
+   </item>
+   <item>
+    <widget class="QPushButton" name="buttonSaveFile">
+     <property name="text">
+      <string>Save to File</string>
+     </property>
+    </widget>
+   </item>
+   <item>
+    <widget class="QPushButton" name="buttonLoadFile">
+     <property name="text">
+      <string>Load from File</string>
+     </property>
+    </widget>
+   </item>
+  </layout>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>

ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadget.cpp

@@ -0,0 +1,48 @@
+/**
+ ******************************************************************************
+ * @file       pathactioneditorgadget.cpp
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
+ * @addtogroup PathAction Editor GCS Plugins
+ * @{
+ * @addtogroup PathActionEditorGadgetPlugin PathAction Editor Gadget Plugin
+ * @{
+ * @brief A gadget to edit a list of pathactions
+ *****************************************************************************/
+/*
+ * 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 "pathactioneditorgadget.h"
+#include "pathactioneditorgadgetwidget.h"
+
+#include "extensionsystem/pluginmanager.h"
+#include "uavobjectmanager.h"
+#include "uavobject.h"
+#include <QDebug>
+
+PathActionEditorGadget::PathActionEditorGadget(QString classId, PathActionEditorGadgetWidget *widget, QWidget *parent) :
+        IUAVGadget(classId, parent),
+        m_widget(widget)
+{
+}
+
+PathActionEditorGadget::~PathActionEditorGadget()
+{
+    delete m_widget;
+}
+
+void PathActionEditorGadget::loadConfiguration(IUAVGadgetConfiguration* config)
+{
+    Q_UNUSED(config);
+}

ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadget.h

@@ -0,0 +1,59 @@
+/**
+ ******************************************************************************
+ * @file       pathactioneditorgadget.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
+ * @addtogroup PathAction Editor GCS Plugins
+ * @{
+ * @addtogroup PathActionEditorGadgetPlugin PathAction Editor Gadget Plugin
+ * @{
+ * @brief A gadget to edit a list of pathactions
+ *****************************************************************************/
+/*
+ * 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 PathActionEditorGADGET_H_
+#define PathActionEditorGADGET_H_
+
+#include <coreplugin/iuavgadget.h>
+
+namespace Core {
+class IUAVGadget;
+}
+//class QWidget;
+//class QString;
+class PathActionEditorGadgetWidget;
+
+using namespace Core;
+
+class PathActionEditorGadget : public Core::IUAVGadget
+{
+    Q_OBJECT
+public:
+    PathActionEditorGadget(QString classId, PathActionEditorGadgetWidget *widget, QWidget *parent = 0);
+    ~PathActionEditorGadget();
+
+    QList<int> context() const { return m_context; }
+    QWidget *widget() { return m_widget; }
+    QString contextHelpId() const { return QString(); }
+
+    void loadConfiguration(IUAVGadgetConfiguration* config);
+private:
+        QWidget *m_widget;
+	QList<int> m_context;
+};
+
+
+#endif // PathActionEditorGADGET_H_

ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadgetfactory.cpp

@@ -0,0 +1,47 @@
+/**
+ ******************************************************************************
+ * @file       pathactioneditorgadgetfactor.cpp
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
+ * @addtogroup PathAction Editor GCS Plugins
+ * @{
+ * @addtogroup PathActionEditorGadgetPlugin PathAction Editor Gadget Plugin
+ * @{
+ * @brief A gadget to edit a list of pathactions
+ *****************************************************************************/
+/*
+ * 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 "pathactioneditorgadgetfactory.h"
+#include "pathactioneditorgadgetwidget.h"
+#include "pathactioneditorgadget.h"
+#include <coreplugin/iuavgadget.h>
+#include <QDebug>
+
+PathActionEditorGadgetFactory::PathActionEditorGadgetFactory(QObject *parent) :
+        IUAVGadgetFactory(QString("PathActionEditorGadget"),
+                          tr("PathAction Editor"),
+                          parent)
+{
+}
+
+PathActionEditorGadgetFactory::~PathActionEditorGadgetFactory()
+{
+
+}
+
+IUAVGadget* PathActionEditorGadgetFactory::createGadget(QWidget *parent) {
+    PathActionEditorGadgetWidget* gadgetWidget = new PathActionEditorGadgetWidget(parent);
+    return new PathActionEditorGadget(QString("PathActionEditorGadget"), gadgetWidget, parent);
+}

ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadgetfactory.h

@@ -0,0 +1,49 @@
+/**
+ ******************************************************************************
+ * @file       pathactioneditorgadgetfactory.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
+ * @addtogroup PathAction Editor GCS Plugins
+ * @{
+ * @addtogroup PathActionEditorGadgetPlugin PathAction Editor Gadget Plugin
+ * @{
+ * @brief A gadget to edit a list of pathactions
+ *****************************************************************************/
+/*
+ * 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 PathActionEditorGADGETFACTORY_H_
+#define PathActionEditorGADGETFACTORY_H_
+
+#include <coreplugin/iuavgadgetfactory.h>
+
+namespace Core {
+class IUAVGadget;
+class IUAVGadgetFactory;
+}
+
+using namespace Core;
+
+class PathActionEditorGadgetFactory : public IUAVGadgetFactory
+{
+    Q_OBJECT
+public:
+    PathActionEditorGadgetFactory(QObject *parent = 0);
+    ~PathActionEditorGadgetFactory();
+
+    IUAVGadget *createGadget(QWidget *parent);
+};
+
+#endif // PathActionEditorGADGETFACTORY_H_

ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadgetwidget.cpp

@@ -0,0 +1,113 @@
+/**
+ ******************************************************************************
+ * @file       pathactioneditorgadgetwidget.cpp
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
+ * @addtogroup PathAction Editor GCS Plugins
+ * @{
+ * @addtogroup PathActionEditorGadgetPlugin PathAction Editor Gadget Plugin
+ * @{
+ * @brief A gadget to edit a list of pathactions
+ *****************************************************************************/
+/*
+ * 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 "pathactioneditorgadgetwidget.h"
+#include "ui_pathactioneditor.h"
+
+#include <QDebug>
+#include <QString>
+#include <QStringList>
+#include <QtGui/QWidget>
+#include <QtGui/QTextEdit>
+#include <QtGui/QVBoxLayout>
+#include <QtGui/QPushButton>
+#include "browseritemdelegate.h"
+
+#include "extensionsystem/pluginmanager.h"
+
+PathActionEditorGadgetWidget::PathActionEditorGadgetWidget(QWidget *parent) : QLabel(parent)
+{
+    m_pathactioneditor = new Ui_PathActionEditor();
+    m_pathactioneditor->setupUi(this);
+
+    m_model = new PathActionEditorTreeModel();
+    m_pathactioneditor->pathactions->setModel(m_model);
+    m_pathactioneditor->pathactions->setColumnWidth(0, 300);
+    m_pathactioneditor->pathactions->setColumnWidth(1, 500);
+    m_pathactioneditor->pathactions->expandAll();
+    BrowserItemDelegate *m_delegate = new BrowserItemDelegate();
+    m_pathactioneditor->pathactions->setItemDelegate(m_delegate);
+    m_pathactioneditor->pathactions->setEditTriggers(QAbstractItemView::AllEditTriggers);
+    m_pathactioneditor->pathactions->setSelectionBehavior(QAbstractItemView::SelectItems);
+
+    ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
+    Q_ASSERT(pm != NULL);
+    UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
+    Q_ASSERT(objManager != NULL);
+    pathactionObj = PathAction::GetInstance(objManager);
+    Q_ASSERT(pathactionObj != NULL);
+    waypointObj = Waypoint::GetInstance(objManager);
+    Q_ASSERT(waypointObj != NULL);
+
+    // Connect the signals
+    connect(m_pathactioneditor->buttonNewPathAction, SIGNAL(clicked()),
+            this, SLOT(addPathActionInstance()));
+    connect(m_pathactioneditor->buttonNewWaypoint, SIGNAL(clicked()),
+            this, SLOT(addWaypointInstance()));
+}
+
+PathActionEditorGadgetWidget::~PathActionEditorGadgetWidget()
+{
+   // Do nothing
+}
+
+void PathActionEditorGadgetWidget::pathactionChanged(UAVObject *)
+{
+}
+
+void PathActionEditorGadgetWidget::addPathActionInstance()
+{
+    ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
+    Q_ASSERT(pm != NULL);
+    UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
+    Q_ASSERT(objManager != NULL);
+
+    qDebug() << "Instances before: " << objManager->getNumInstances(pathactionObj->getObjID());
+    PathAction *obj = new PathAction();
+    quint32 newInstId = objManager->getNumInstances(pathactionObj->getObjID());
+    obj->initialize(newInstId,obj->getMetaObject());
+    objManager->registerObject(obj);
+    qDebug() << "Instances after: " << objManager->getNumInstances(pathactionObj->getObjID());
+}
+
+void PathActionEditorGadgetWidget::addWaypointInstance()
+{
+    ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
+    Q_ASSERT(pm != NULL);
+    UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
+    Q_ASSERT(objManager != NULL);
+
+    qDebug() << "Instances before: " << objManager->getNumInstances(waypointObj->getObjID());
+    Waypoint *obj = new Waypoint();
+    quint32 newInstId = objManager->getNumInstances(waypointObj->getObjID());
+    obj->initialize(newInstId,obj->getMetaObject());
+    objManager->registerObject(obj);
+    qDebug() << "Instances after: " << objManager->getNumInstances(waypointObj->getObjID());
+}
+
+/**
+  * @}
+  * @}
+  */

ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadgetwidget.h

@@ -0,0 +1,61 @@
+/**
+ ******************************************************************************
+ * @file       pathactioneditorgadgetwidget.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
+ * @addtogroup PathAction Editor GCS Plugins
+ * @{
+ * @addtogroup PathActionEditorGadgetPlugin PathAction Editor Gadget Plugin
+ * @{
+ * @brief A gadget to edit a list of pathactions
+ *****************************************************************************/
+/*
+ * 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 PathActionEditorGADGETWIDGET_H_
+#define PathActionEditorGADGETWIDGET_H_
+
+#include <QtGui/QLabel>
+#include <QtGui/QWidget>
+#include <QtGui/QTreeView>
+#include "pathaction.h"
+#include "waypoint.h"
+#include "pathactioneditortreemodel.h"
+
+class Ui_PathActionEditor;
+
+class PathActionEditorGadgetWidget : public QLabel
+{
+    Q_OBJECT
+
+public:
+    PathActionEditorGadgetWidget(QWidget *parent = 0);
+    ~PathActionEditorGadgetWidget();
+
+signals:
+
+protected slots:
+    void pathactionChanged(UAVObject *);
+    void addPathActionInstance();
+    void addWaypointInstance();
+
+private:
+    Ui_PathActionEditor * m_pathactioneditor;
+    PathActionEditorTreeModel *m_model;
+    PathAction *pathactionObj;
+    Waypoint *waypointObj;
+};
+
+#endif /* PathActionEditorGADGETWIDGET_H_ */

ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorplugin.cpp

@@ -0,0 +1,68 @@
+/**
+ ******************************************************************************
+ * @file       pathactioneditorplugin.cpp
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
+ * @addtogroup PathAction Editor GCS Plugins
+ * @{
+ * @addtogroup PathActionEditorGadgetPlugin PathAction Editor Gadget Plugin
+ * @{
+ * @brief A gadget to edit a list of pathactions
+ *****************************************************************************/
+/*
+ * 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 "pathactioneditorplugin.h"
+#include "pathactioneditorgadgetfactory.h"
+#include <QDebug>
+#include <QtPlugin>
+#include <QStringList>
+#include <extensionsystem/pluginmanager.h>
+
+
+PathActionEditorPlugin::PathActionEditorPlugin()
+{
+   // Do nothing
+}
+
+PathActionEditorPlugin::~PathActionEditorPlugin()
+{
+   // Do nothing
+}
+
+bool PathActionEditorPlugin::initialize(const QStringList& args, QString *errMsg)
+{
+   Q_UNUSED(args);
+   Q_UNUSED(errMsg);
+   mf = new PathActionEditorGadgetFactory(this);
+   addAutoReleasedObject(mf);
+
+   return true;
+}
+
+void PathActionEditorPlugin::extensionsInitialized()
+{
+   // Do nothing
+}
+
+void PathActionEditorPlugin::shutdown()
+{
+   // Do nothing
+}
+Q_EXPORT_PLUGIN(PathActionEditorPlugin)
+
+/**
+  * @}
+  * @}
+  */

ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorplugin.h

@@ -0,0 +1,46 @@
+/**
+ ******************************************************************************
+ * @file       pathactioneditorplugin.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
+ * @addtogroup PathAction Editor GCS Plugins
+ * @{
+ * @addtogroup PathActionEditorGadgetPlugin PathAction Editor Gadget Plugin
+ * @{
+ * @brief A gadget to edit a list of pathactions
+ *****************************************************************************/
+/*
+ * 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 PathActionEditorPLUGIN_H_
+#define PathActionEditorPLUGIN_H_
+
+#include <extensionsystem/iplugin.h>
+
+class PathActionEditorGadgetFactory;
+
+class PathActionEditorPlugin : public ExtensionSystem::IPlugin
+{
+public:
+    PathActionEditorPlugin();
+   ~PathActionEditorPlugin();
+
+   void extensionsInitialized();
+   bool initialize(const QStringList & arguments, QString * errorString);
+   void shutdown();
+private:
+   PathActionEditorGadgetFactory *mf;
+};
+#endif /* PathActionEditorPLUGIN_H_ */

ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditortreemodel.cpp

@@ -0,0 +1,423 @@
+/**
+ ******************************************************************************
+ *
+ * @file       pathactioneditortreemodel.cpp
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @addtogroup GCSPlugins GCS Plugins
+ * @{
+ * @addtogroup UAVObjectBrowserPlugin UAVObject Browser Plugin
+ * @{
+ * @brief The UAVObject Browser gadget 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 "pathactioneditortreemodel.h"
+#include "pathactioneditorgadget.h"
+#include "fieldtreeitem.h"
+#include "uavobjectmanager.h"
+#include "uavdataobject.h"
+#include "uavmetaobject.h"
+#include "uavobjectfield.h"
+#include "extensionsystem/pluginmanager.h"
+#include <QtGui/QColor>
+//#include <QtGui/QIcon>
+#include <QMessageBox>
+#include <QtCore/QSignalMapper>
+#include <QtCore/QDebug>
+#include "waypoint.h"
+#include "waypointactive.h"
+#include "pathaction.h"
+
+PathActionEditorTreeModel::PathActionEditorTreeModel(QObject *parent) :
+        QAbstractItemModel(parent),
+        m_recentlyUpdatedColor(QColor(255, 230, 230)),
+        m_manuallyChangedColor(QColor(230, 230, 255))
+{
+    ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
+    m_objManager = pm->getObject<UAVObjectManager>();
+
+    connect(m_objManager, SIGNAL(newInstance(UAVObject*)), this, SLOT(newInstance(UAVObject*)));
+    connect(m_objManager->getObject("WaypointActive"),SIGNAL(objectUpdated(UAVObject*)), this, SLOT(objUpdated(UAVObject*)));
+    connect(m_objManager->getObject("PathAction"),SIGNAL(objectUpdated(UAVObject*)), this, SLOT(objUpdated(UAVObject*)));
+    connect(m_objManager->getObject("Waypoint"),SIGNAL(objectUpdated(UAVObject*)), this, SLOT(objUpdated(UAVObject*)));
+
+    setupModelData();
+}
+
+PathActionEditorTreeModel::~PathActionEditorTreeModel()
+{
+    delete m_rootItem;
+}
+
+void PathActionEditorTreeModel::setupModelData()
+{
+
+    m_actions = new QStringList();
+    updateActions();
+
+    // root
+    QList<QVariant> rootData;
+    rootData << tr("Property") << tr("Value") << tr("Unit");
+    m_rootItem = new TreeItem(rootData);
+
+    m_pathactionsTree = new TopTreeItem(tr("PathActions"), m_rootItem);
+    m_rootItem->appendChild(m_pathactionsTree);
+    m_waypointsTree = new TopTreeItem(tr("Waypoints"), m_rootItem);
+    m_rootItem->appendChild(m_waypointsTree);
+    connect(m_rootItem, SIGNAL(updateHighlight(TreeItem*)), this, SLOT(updateHighlight(TreeItem*)));
+    connect(m_pathactionsTree, SIGNAL(updateHighlight(TreeItem*)), this, SLOT(updateHighlight(TreeItem*)));
+    connect(m_waypointsTree, SIGNAL(updateHighlight(TreeItem*)), this, SLOT(updateHighlight(TreeItem*)));
+
+    {
+        QList<UAVObject*> list = m_objManager->getObjectInstances("PathAction");
+        foreach (UAVObject* obj, list) {
+            addInstance(obj,m_pathactionsTree);
+        }
+    }
+    {
+        QList<UAVObject*> list = m_objManager->getObjectInstances("Waypoint");
+        foreach (UAVObject* obj, list) {
+            addInstance(obj,m_waypointsTree);
+        }
+    }
+}
+
+void PathActionEditorTreeModel::updateActions() {
+	m_actions->clear();
+        QList<UAVObject*> list = m_objManager->getObjectInstances("PathAction");
+        foreach (UAVObject* obj, list) {
+	    QString title;
+	    title.append((QVariant(obj->getInstID()).toString()));
+	    title.append(" ");
+	    title.append((obj->getField("Mode")->getValue().toString()));
+	    title.append(" ");
+	    title.append((obj->getField("Command")->getValue().toString()));
+	    title.append(":");
+	    title.append((obj->getField("EndCondition")->getValue().toString()));
+	    title.append(" ");
+	    m_actions->append(title);
+        }
+}
+
+void PathActionEditorTreeModel::addInstance(UAVObject *obj, TreeItem *parent)
+{
+    connect(obj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(highlightUpdatedObject(UAVObject*)));
+    TreeItem *item;
+    QString name = QString::number(obj->getInstID());
+    item = new InstanceTreeItem(obj, name);
+    connect(item, SIGNAL(updateHighlight(TreeItem*)), this, SLOT(updateHighlight(TreeItem*)));
+    parent->appendChild(item);
+    foreach (UAVObjectField *field, obj->getFields()) {
+        if (field->getNumElements() > 1) {
+            addArrayField(field, item);
+        } else {
+            addSingleField(0, field, item);
+        }
+    }
+}
+
+
+void PathActionEditorTreeModel::addArrayField(UAVObjectField *field, TreeItem *parent)
+{
+    TreeItem *item = new ArrayFieldTreeItem(field->getName());
+    connect(item, SIGNAL(updateHighlight(TreeItem*)), this, SLOT(updateHighlight(TreeItem*)));
+    for (uint i = 0; i < field->getNumElements(); ++i) {
+        addSingleField(i, field, item);
+    }
+    parent->appendChild(item);
+}
+
+void PathActionEditorTreeModel::addSingleField(int index, UAVObjectField *field, TreeItem *parent)
+{
+    QList<QVariant> data;
+    if (field->getNumElements() == 1)
+        data.append(field->getName());
+    else
+        data.append( QString("[%1]").arg((field->getElementNames())[index]) );
+
+    FieldTreeItem *item;
+    UAVObjectField::FieldType type = field->getType();
+    // hack: list available actions in an enum
+    if (field->getName().compare("Action")==0 && type==UAVObjectField::UINT8) {
+	data.append( field->getValue(index).toInt());
+        data.append( field->getUnits());
+        item = new ActionFieldTreeItem(field, index, data, m_actions);
+    } else {
+    switch (type) {
+    case UAVObjectField::ENUM: {
+        QStringList options = field->getOptions();
+        QVariant value = field->getValue();
+        data.append( options.indexOf(value.toString()) );
+        data.append(field->getUnits());
+        item = new EnumFieldTreeItem(field, index, data);
+        break;
+    }
+    case UAVObjectField::INT8:
+    case UAVObjectField::INT16:
+    case UAVObjectField::INT32:
+    case UAVObjectField::UINT8:
+    case UAVObjectField::UINT16:
+    case UAVObjectField::UINT32:
+        data.append(field->getValue(index));
+        data.append(field->getUnits());
+        item = new IntFieldTreeItem(field, index, data);
+        break;
+    case UAVObjectField::FLOAT32:
+        data.append(field->getValue(index));
+        data.append(field->getUnits());
+        item = new FloatFieldTreeItem(field, index, data);
+        break;
+    default:
+        Q_ASSERT(false);
+    }
+    }
+    connect(item, SIGNAL(updateHighlight(TreeItem*)), this, SLOT(updateHighlight(TreeItem*)));
+    parent->appendChild(item);
+}
+
+QModelIndex PathActionEditorTreeModel::index(int row, int column, const QModelIndex &parent)
+        const
+{
+    if (!hasIndex(row, column, parent))
+        return QModelIndex();
+
+    TreeItem *parentItem;
+
+    if (!parent.isValid())
+        parentItem = m_rootItem;
+    else
+        parentItem = static_cast<TreeItem*>(parent.internalPointer());
+
+    TreeItem *childItem = parentItem->child(row);
+    if (childItem)
+        return createIndex(row, column, childItem);
+    else
+        return QModelIndex();
+}
+
+QModelIndex PathActionEditorTreeModel::index(TreeItem *item)
+{
+    if (item->parent() == 0)
+        return QModelIndex();
+
+    QModelIndex root = index(item->parent());
+
+    for (int i = 0; i < rowCount(root); ++i) {
+        QModelIndex childIndex = index(i, 0, root);
+        TreeItem *child = static_cast<TreeItem*>(childIndex.internalPointer());
+        if (child == item)
+            return childIndex;
+    }
+    Q_ASSERT(false);
+    return QModelIndex();
+}
+
+QModelIndex PathActionEditorTreeModel::parent(const QModelIndex &index) const
+{
+    if (!index.isValid())
+        return QModelIndex();
+
+    TreeItem *childItem = static_cast<TreeItem*>(index.internalPointer());
+    TreeItem *parentItem = childItem->parent();
+
+    if (parentItem == m_rootItem)
+        return QModelIndex();
+
+    return createIndex(parentItem->row(), 0, parentItem);
+}
+
+int PathActionEditorTreeModel::rowCount(const QModelIndex &parent) const
+{
+    TreeItem *parentItem;
+    if (parent.column() > 0)
+        return 0;
+
+    if (!parent.isValid())
+        parentItem = m_rootItem;
+    else
+        parentItem = static_cast<TreeItem*>(parent.internalPointer());
+
+    return parentItem->childCount();
+}
+
+int PathActionEditorTreeModel::columnCount(const QModelIndex &parent) const
+{
+    if (parent.isValid())
+        return static_cast<TreeItem*>(parent.internalPointer())->columnCount();
+    else
+        return m_rootItem->columnCount();
+}
+
+QVariant PathActionEditorTreeModel::data(const QModelIndex &index, int role) const
+{
+    if (!index.isValid())
+        return QVariant();
+
+    if (index.column() == TreeItem::dataColumn && role == Qt::EditRole) {
+        TreeItem *item = static_cast<TreeItem*>(index.internalPointer());
+        return item->data(index.column());
+    }
+//    if (role == Qt::DecorationRole)
+//        return QIcon(":/core/images/openpilot_logo_128.png");
+
+    if (role == Qt::ToolTipRole) {
+        TreeItem *item = static_cast<TreeItem*>(index.internalPointer());
+        return item->description();
+    }
+
+    TreeItem *item = static_cast<TreeItem*>(index.internalPointer());
+
+    if (index.column() == 0 && role == Qt::BackgroundRole) {
+        ObjectTreeItem *objItem = dynamic_cast<ObjectTreeItem*>(item);
+        if (objItem && objItem->highlighted())
+            return QVariant(m_recentlyUpdatedColor);
+    }
+    if (index.column() == TreeItem::dataColumn && role == Qt::BackgroundRole) {
+        FieldTreeItem *fieldItem = dynamic_cast<FieldTreeItem*>(item);
+        if (fieldItem && fieldItem->highlighted())
+            return QVariant(m_recentlyUpdatedColor);
+        if (fieldItem && fieldItem->changed())
+            return QVariant(m_manuallyChangedColor);
+    }
+
+    if (role != Qt::DisplayRole)
+        return QVariant();
+
+    if (index.column() == TreeItem::dataColumn) {
+        EnumFieldTreeItem *fieldItem = dynamic_cast<EnumFieldTreeItem*>(item);
+        if (fieldItem) {
+            int enumIndex = fieldItem->data(index.column()).toInt();
+            return fieldItem->enumOptions(enumIndex);
+        } else {
+            ActionFieldTreeItem *afieldItem = dynamic_cast<ActionFieldTreeItem*>(item);
+            if (afieldItem) {
+                int enumIndex = afieldItem->data(index.column()).toInt();
+                return afieldItem->enumOptions(enumIndex);
+            }
+	}
+    }
+
+    return item->data(index.column());
+}
+
+bool PathActionEditorTreeModel::setData(const QModelIndex &index, const QVariant & value, int role)
+{
+    Q_UNUSED(role)
+    TreeItem *item = static_cast<TreeItem*>(index.internalPointer());
+    item->setData(value, index.column());
+    return true;
+}
+
+
+Qt::ItemFlags PathActionEditorTreeModel::flags(const QModelIndex &index) const
+{
+    if (!index.isValid())
+        return 0;
+
+    if (index.column() == TreeItem::dataColumn) {
+        TreeItem *item = static_cast<TreeItem*>(index.internalPointer());
+        if (item->isEditable())
+            return Qt::ItemIsEnabled | Qt::ItemIsSelectable | Qt::ItemIsEditable;
+    }
+
+    return Qt::ItemIsEnabled | Qt::ItemIsSelectable;
+}
+
+QVariant PathActionEditorTreeModel::headerData(int section, Qt::Orientation orientation,
+                                        int role) const
+{
+    if (orientation == Qt::Horizontal && role == Qt::DisplayRole)
+        return m_rootItem->data(section);
+
+    return QVariant();
+}
+
+void PathActionEditorTreeModel::updateHighlight(TreeItem *item)
+{
+    QModelIndex itemIndex = index(item);
+    Q_ASSERT(itemIndex != QModelIndex());
+    emit dataChanged(itemIndex, itemIndex.sibling(itemIndex.row(), TreeItem::dataColumn));
+
+    // update uavobject if any - go up the tree until there is
+    ObjectTreeItem *objItem = 0;
+    TreeItem *searchItem = item;
+    while (searchItem) {
+        objItem = dynamic_cast<ObjectTreeItem*>(searchItem);
+        if (objItem)
+            break;
+        searchItem = searchItem->parent();
+    }
+    if (objItem) {
+    item->apply();
+    objItem->apply();
+        UAVObject *obj = objItem->object();
+        Q_ASSERT(obj);
+        obj->updated();
+    }
+}
+
+void PathActionEditorTreeModel::highlightUpdatedObject(UAVObject *obj)
+{
+    Q_ASSERT(obj);
+    if (obj->getName().compare("Waypoint")==0) {
+        m_waypointsTree->update();
+        //emit dataChanged(index(m_waypointsTree), index(m_waypointsTree));
+    } else if (obj->getName().compare("PathAction")==0) {
+        m_pathactionsTree->update();
+        //emit dataChanged(index(m_pathactionsTree), index(m_pathactionsTree));
+    }
+}
+
+void PathActionEditorTreeModel::newInstance(UAVObject *obj)
+{
+   
+    if (obj->getName().compare("Waypoint")==0) {
+        addInstance(obj,m_waypointsTree);
+        m_waypointsTree->update();
+    } else if (obj->getName().compare("PathAction")==0) {
+        addInstance(obj,m_pathactionsTree);
+        m_pathactionsTree->update();
+    }
+    updateActions();
+    emit layoutChanged();
+}
+
+void PathActionEditorTreeModel::objUpdated(UAVObject *obj)
+{
+    quint16 index = m_objManager->getObject("WaypointActive")->getField("Index")->getValue().toInt();
+    quint16 action;
+    foreach (TreeItem *child,m_waypointsTree->treeChildren()) {
+        ObjectTreeItem *objItem = dynamic_cast<ObjectTreeItem*>(child);
+        if (index == objItem->object()->getInstID()) {
+            child->setActive(true);
+            action = objItem->object()->getField("Action")->getValue().toInt();
+        } else {
+            child->setActive(false);
+        }
+    }
+    foreach (TreeItem *child,m_pathactionsTree->treeChildren()) {
+        ObjectTreeItem *objItem = dynamic_cast<ObjectTreeItem*>(child);
+        if (action == objItem->object()->getInstID()) {
+            child->setActive(true);
+        } else {
+            child->setActive(false);
+        }
+    }
+    updateActions();
+    emit layoutChanged();
+}
+

ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditortreemodel.h

@@ -0,0 +1,98 @@
+/**
+ ******************************************************************************
+ *
+ * @file       pathactioneditortreemodel.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @addtogroup GCSPlugins GCS Plugins
+ * @{
+ * @addtogroup UAVObjectBrowserPlugin UAVObject Browser Plugin
+ * @{
+ * @brief The UAVObject Browser gadget 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 PATHACTIONEDITORTREEMODEL_H
+#define PATHACTIONEDITORTREEMODEL_H
+
+#include "treeitem.h"
+#include <QAbstractItemModel>
+#include <QtCore/QMap>
+#include <QtGui/QColor>
+
+class TopTreeItem;
+class ObjectTreeItem;
+class DataObjectTreeItem;
+class UAVObject;
+class UAVDataObject;
+class UAVMetaObject;
+class UAVObjectField;
+class UAVObjectManager;
+class QSignalMapper;
+class QTimer;
+
+class PathActionEditorTreeModel : public QAbstractItemModel
+{
+Q_OBJECT
+public:
+    explicit PathActionEditorTreeModel(QObject *parent = 0);
+    ~PathActionEditorTreeModel();
+
+    QVariant data(const QModelIndex &index, int role) const;
+    bool setData(const QModelIndex &index, const QVariant & value, int role);
+    Qt::ItemFlags flags(const QModelIndex &index) const;
+    QVariant headerData(int section, Qt::Orientation orientation,
+                        int role = Qt::DisplayRole) const;
+    QModelIndex index(int row, int column,
+                      const QModelIndex &parent = QModelIndex()) const;
+    QModelIndex parent(const QModelIndex &index) const;
+    int rowCount(const QModelIndex &parent = QModelIndex()) const;
+    int columnCount(const QModelIndex &parent = QModelIndex()) const;
+
+    void setRecentlyUpdatedColor(QColor color) { m_recentlyUpdatedColor = color; }
+    void setManuallyChangedColor(QColor color) { m_manuallyChangedColor = color; }
+
+signals:
+
+public slots:
+    void newInstance(UAVObject *obj);
+    void objUpdated(UAVObject *obj);
+
+private slots:
+    void highlightUpdatedObject(UAVObject *obj);
+    void updateHighlight(TreeItem*);
+
+private:
+    QModelIndex index(TreeItem *item);
+    void addArrayField(UAVObjectField *field, TreeItem *parent);
+
+    void addSingleField(int index, UAVObjectField *field, TreeItem *parent);
+    void addInstance(UAVObject *obj, TreeItem *parent);
+    //QString updateMode(quint8 updateMode);
+    void setupModelData();
+    void updateActions();
+
+    UAVObjectManager *m_objManager;
+    QStringList *m_actions;
+
+    TreeItem *m_rootItem;
+    TopTreeItem *m_pathactionsTree;
+    TopTreeItem *m_waypointsTree;
+    QColor m_recentlyUpdatedColor;
+    QColor m_manuallyChangedColor;
+};
+
+#endif // PATHACTIONEDITORTREEMODEL_H

ground/openpilotgcs/src/plugins/pathactioneditor/treeitem.cpp

@@ -0,0 +1,122 @@
+/**
+ ******************************************************************************
+ *
+ * @file       treeitem.cpp
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @addtogroup GCSPlugins GCS Plugins
+ * @{
+ * @addtogroup UAVObjectBrowserPlugin UAVObject Browser Plugin
+ * @{
+ * @brief The UAVObject Browser gadget 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 "treeitem.h"
+
+TreeItem::TreeItem(const QList<QVariant> &data, TreeItem *parent) :
+        QObject(0),
+        m_data(data),
+        m_parent(parent),
+        m_highlight(false),
+        m_changed(false)
+{
+}
+
+TreeItem::TreeItem(const QVariant &data, TreeItem *parent) :
+        QObject(0),
+        m_parent(parent),
+        m_highlight(false),
+        m_changed(false)
+{
+    m_data << data << "" << "";
+}
+
+TreeItem::~TreeItem()
+{
+    qDeleteAll(m_children);
+}
+
+void TreeItem::appendChild(TreeItem *child)
+{
+    m_children.append(child);
+    child->setParentTree(this);
+}
+
+void TreeItem::insert(int index, TreeItem *child)
+{
+    m_children.insert(index, child);
+    child->setParentTree(this);
+}
+
+TreeItem *TreeItem::child(int row)
+{
+    return m_children.value(row);
+}
+
+int TreeItem::childCount() const
+{
+    return m_children.count();
+}
+int TreeItem::row() const
+{
+    if (m_parent)
+        return m_parent->m_children.indexOf(const_cast<TreeItem*>(this));
+
+    return 0;
+}
+
+int TreeItem::columnCount() const
+{
+    return m_data.count();
+}
+
+QVariant TreeItem::data(int column) const
+{
+    return m_data.value(column);
+}
+
+void TreeItem::setData(QVariant value, int column)
+{
+    m_data.replace(column, value);
+}
+
+void TreeItem::update() {
+    foreach(TreeItem *child, treeChildren())
+        child->update();
+}
+
+void TreeItem::apply() {
+    foreach(TreeItem *child, treeChildren())
+        child->apply();
+}
+
+void TreeItem::setHighlight(bool highlight) {
+    //m_highlight = highlight;
+    m_changed = false;
+}
+
+void TreeItem::setActive(bool highlight) {
+    m_highlight = highlight;
+    foreach(TreeItem *child, treeChildren())
+        child->setActive(highlight);
+}
+
+
+void TreeItem::removeHighlight() {
+    m_highlight = false;
+    update();
+}

ground/openpilotgcs/src/plugins/pathactioneditor/treeitem.h

@@ -0,0 +1,188 @@
+/**
+ ******************************************************************************
+ *
+ * @file       treeitem.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @addtogroup GCSPlugins GCS Plugins
+ * @{
+ * @addtogroup UAVObjectBrowserPlugin UAVObject Browser Plugin
+ * @{
+ * @brief The UAVObject Browser gadget 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 TREEITEM_H
+#define TREEITEM_H
+
+#include "uavobject.h"
+#include "uavmetaobject.h"
+#include "uavobjectfield.h"
+#include <QtCore/QList>
+#include <QtCore/QVariant>
+#include <QtCore/QObject>
+
+
+class TreeItem : public QObject
+{
+Q_OBJECT
+public:
+    TreeItem(const QList<QVariant> &data, TreeItem *parent = 0);
+    TreeItem(const QVariant &data, TreeItem *parent = 0);
+    virtual ~TreeItem();
+
+    void appendChild(TreeItem *child);
+    void insert(int index, TreeItem *child);
+
+    TreeItem *child(int row);
+    inline QList<TreeItem*> treeChildren() const { return m_children; }
+    int childCount() const;
+    int columnCount() const;
+    QVariant data(int column = 1) const;
+    QString description() { return m_description; }
+    void setDescription(QString d) { // Split around 40 characters
+        int idx = d.indexOf(" ",40);
+        d.insert(idx,QString("<br>"));
+        d.remove("@Ref", Qt::CaseInsensitive);
+        m_description = d;
+    }
+    // only column 1 (TreeItem::dataColumn) is changed with setData currently
+    // other columns are initialized in constructor
+    virtual void setData(QVariant value, int column = 1);
+    int row() const;
+    TreeItem *parent() { return m_parent; }
+    void setParentTree(TreeItem *parent) { m_parent = parent; }
+    inline virtual bool isEditable() { return false; }
+    virtual void update();
+    virtual void apply();
+
+    inline bool highlighted() { return m_highlight; }
+    void setHighlight(bool highlight);
+    void setActive(bool highlight);
+
+    inline bool changed() { return m_changed; }
+    inline void setChanged(bool changed) { m_changed = changed; if(changed) emit updateHighlight(this); }
+
+signals:
+    void updateHighlight(TreeItem*);
+
+private slots:
+    void removeHighlight();
+
+private:
+    QList<TreeItem*> m_children;
+    // m_data contains: [0] property name, [1] value, [2] unit
+    QList<QVariant> m_data;
+    QString m_description;
+    TreeItem *m_parent;
+    bool m_highlight;
+    bool m_changed;
+public:
+    static const int dataColumn = 1;
+private:
+};
+
+class TopTreeItem : public TreeItem
+{
+Q_OBJECT
+public:
+    TopTreeItem(const QList<QVariant> &data, TreeItem *parent = 0) : TreeItem(data, parent) { }
+    TopTreeItem(const QVariant &data, TreeItem *parent = 0) : TreeItem(data, parent) { }
+
+    QList<quint32> objIds() { return m_objIds; }
+    void addObjId(quint32 objId) { m_objIds.append(objId); }
+    void insertObjId(int index, quint32 objId) { m_objIds.insert(index, objId); }
+    int nameIndex(QString name) {
+        for (int i = 0; i < childCount(); ++i) {
+            if (name < child(i)->data(0).toString())
+                return i;
+        }
+        return childCount();
+    }
+
+private:
+    QList<quint32> m_objIds;
+};
+
+class ObjectTreeItem : public TreeItem
+{
+Q_OBJECT
+public:
+    ObjectTreeItem(const QList<QVariant> &data, TreeItem *parent = 0) :
+            TreeItem(data, parent), m_obj(0) { }
+    ObjectTreeItem(const QVariant &data, TreeItem *parent = 0) :
+            TreeItem(data, parent), m_obj(0) { }
+    void setObject(UAVObject *obj) { m_obj = obj; setDescription(obj->getDescription()); }
+    inline UAVObject *object() { return m_obj; }
+private:
+    UAVObject *m_obj;
+};
+
+class MetaObjectTreeItem : public ObjectTreeItem
+{
+Q_OBJECT
+public:
+    MetaObjectTreeItem(UAVObject *obj, const QList<QVariant> &data, TreeItem *parent = 0) :
+            ObjectTreeItem(data, parent) { setObject(obj); }
+    MetaObjectTreeItem(UAVObject *obj, const QVariant &data, TreeItem *parent = 0) :
+            ObjectTreeItem(data, parent) { setObject(obj); }
+};
+
+class DataObjectTreeItem : public ObjectTreeItem
+{
+Q_OBJECT
+public:
+    DataObjectTreeItem(const QList<QVariant> &data, TreeItem *parent = 0) :
+            ObjectTreeItem(data, parent) { }
+    DataObjectTreeItem(const QVariant &data, TreeItem *parent = 0) :
+            ObjectTreeItem(data, parent) { }
+    virtual void apply() {
+        foreach(TreeItem *child, treeChildren()) {
+            MetaObjectTreeItem *metaChild = dynamic_cast<MetaObjectTreeItem*>(child);
+            if (!metaChild)
+                child->apply();
+        }
+    }
+    virtual void update() {
+        foreach(TreeItem *child, treeChildren()) {
+            MetaObjectTreeItem *metaChild = dynamic_cast<MetaObjectTreeItem*>(child);
+            if (!metaChild)
+                child->update();
+        }
+    }
+};
+
+class InstanceTreeItem : public DataObjectTreeItem
+{
+Q_OBJECT
+public:
+    InstanceTreeItem(UAVObject *obj, const QList<QVariant> &data, TreeItem *parent = 0) :
+            DataObjectTreeItem(data, parent) { setObject(obj); }
+    InstanceTreeItem(UAVObject *obj, const QVariant &data, TreeItem *parent = 0) :
+            DataObjectTreeItem(data, parent) { setObject(obj); }
+    virtual void apply() { TreeItem::apply(); }
+    virtual void update() { TreeItem::update(); }
+};
+
+class ArrayFieldTreeItem : public TreeItem
+{
+Q_OBJECT
+public:
+    ArrayFieldTreeItem(const QList<QVariant> &data, TreeItem *parent = 0) : TreeItem(data, parent) { }
+    ArrayFieldTreeItem(const QVariant &data, TreeItem *parent = 0) : TreeItem(data, parent) { }
+};
+
+#endif // TREEITEM_H

ground/openpilotgcs/src/plugins/plugins.pro

@@ -134,6 +134,12 @@ plugin_qmlview.depends = plugin_coreplugin
 plugin_qmlview.depends += plugin_uavobjects
 SUBDIRS += plugin_qmlview
 
+# PathAction Editor gadget
+plugin_pathactioneditor.subdir = pathactioneditor
+plugin_pathactioneditor.depends = plugin_coreplugin
+plugin_pathactioneditor.depends += plugin_uavobjects
+SUBDIRS += plugin_pathactioneditor
+
 # Waypoint Editor gadget
 plugin_waypointeditor.subdir = waypointeditor
 plugin_waypointeditor.depends = plugin_coreplugin

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

@@ -55,8 +55,10 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
     $$UAVOBJECT_SYNTHETICS/gpsposition.h \
     $$UAVOBJECT_SYNTHETICS/gpstime.h \
     $$UAVOBJECT_SYNTHETICS/gpssatellites.h \
+    $$UAVOBJECT_SYNTHETICS/pathaction.h \
     $$UAVOBJECT_SYNTHETICS/pathdesired.h \
     $$UAVOBJECT_SYNTHETICS/pathplannersettings.h \
+    $$UAVOBJECT_SYNTHETICS/pathstatus.h \
     $$UAVOBJECT_SYNTHETICS/gpsvelocity.h \
     $$UAVOBJECT_SYNTHETICS/positionactual.h \
     $$UAVOBJECT_SYNTHETICS/flightbatterystate.h \
@@ -65,7 +67,9 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
     $$UAVOBJECT_SYNTHETICS/mixerstatus.h \
     $$UAVOBJECT_SYNTHETICS/velocitydesired.h \
     $$UAVOBJECT_SYNTHETICS/velocityactual.h \
-    $$UAVOBJECT_SYNTHETICS/guidancesettings.h \
+    $$UAVOBJECT_SYNTHETICS/fixedwingpathfollowersettings.h \
+    $$UAVOBJECT_SYNTHETICS/fixedwingpathfollowerstatus.h \
+    $$UAVOBJECT_SYNTHETICS/vtolpathfollowersettings.h \
     $$UAVOBJECT_SYNTHETICS/ratedesired.h \
     $$UAVOBJECT_SYNTHETICS/firmwareiapobj.h \
     $$UAVOBJECT_SYNTHETICS/i2cstats.h \
@@ -124,8 +128,10 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
     $$UAVOBJECT_SYNTHETICS/gpsposition.cpp \
     $$UAVOBJECT_SYNTHETICS/gpstime.cpp \
     $$UAVOBJECT_SYNTHETICS/gpssatellites.cpp \
+    $$UAVOBJECT_SYNTHETICS/pathaction.cpp \
     $$UAVOBJECT_SYNTHETICS/pathdesired.cpp \
     $$UAVOBJECT_SYNTHETICS/pathplannersettings.cpp \
+    $$UAVOBJECT_SYNTHETICS/pathstatus.cpp \
     $$UAVOBJECT_SYNTHETICS/gpsvelocity.cpp \
     $$UAVOBJECT_SYNTHETICS/positionactual.cpp \
     $$UAVOBJECT_SYNTHETICS/flightbatterystate.cpp \
@@ -134,7 +140,9 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
     $$UAVOBJECT_SYNTHETICS/mixerstatus.cpp \
     $$UAVOBJECT_SYNTHETICS/velocitydesired.cpp \
     $$UAVOBJECT_SYNTHETICS/velocityactual.cpp \
-    $$UAVOBJECT_SYNTHETICS/guidancesettings.cpp \
+    $$UAVOBJECT_SYNTHETICS/fixedwingpathfollowersettings.cpp \
+    $$UAVOBJECT_SYNTHETICS/fixedwingpathfollowerstatus.cpp \
+    $$UAVOBJECT_SYNTHETICS/vtolpathfollowersettings.cpp \
     $$UAVOBJECT_SYNTHETICS/ratedesired.cpp \
     $$UAVOBJECT_SYNTHETICS/firmwareiapobj.cpp \
     $$UAVOBJECT_SYNTHETICS/i2cstats.cpp \

shared/uavobjectdefinition/fixedwingpathfollowersettings.xml

@@ -0,0 +1,55 @@
+<xml>
+    <object name="FixedWingPathFollowerSettings" singleinstance="true" settings="true">
+        <description>Settings for the @ref FixedWingPathFollowerModule</description>
+
+        <!-- these coefficients control desired movement in airspace -->
+        <field name="AirSpeedMax" units="m/s" type="float" elements="1" defaultvalue="15"/>
+                <!-- maximum speed the aircraft can reach in level flight at full throttle without loosing altitude - leave some safety margin -->
+        <field name="AirSpeedMin" units="m/s" type="float" elements="1" defaultvalue="10"/>
+                <!-- stall speed - leave some safety margin -->
+        <field name="VerticalVelMax" units="m/s" type="float" elements="1" defaultvalue="10"/>
+                <!-- maximum allowed climb or sink rate in guided flight-->
+
+        <field name="CourseFeedForward" units="s" type="float" elements="1" defaultvalue="3.0"/>
+                <!-- how many seconds to plan the flight vector ahead when initiating necessary course changes - increase for planes with sluggish response -->
+
+        <field name="HorizontalPosP" units="(m/s)/m" type="float" elements="1" defaultvalue="0.05"/>
+                <!-- proportional coefficient for correction vector in path vector field to get back on course - reduce for fast planes to prevent course oscillations -->
+        <field name="VerticalPosP" units="(m/s)/m" type="float" elements="1" defaultvalue="0.05"/>
+                <!-- proportional coefficient for desired vertical speed in relation to altitude displacement-->
+
+        <!-- these coefficients control actual control outputs -->
+        <field name="CoursePI" units="deg/deg" type="float" elements="3" elementnames="Kp,Ki,ILimit" defaultvalue="0.2, 0, 0"/>
+                <!-- coefficients for desired bank angle in relation to ground heading error - this controls heading -->
+
+        <field name="SpeedP" units="(m/s^2) / ((m/s)/(m/s)" type="float" elementnames="Kp,Max" defaultvalue="10,10"/>
+                <!-- coefficients for desired acceleration
+                     in relation to relative airspeed error (IASerror/IASactual)-->
+        <field name="AccelPI" units="deg / (m/s^2)" type="float" elements="3" elementnames="Kp,Ki,ILimit" defaultvalue="1.5, 1.5, 20"/>
+                <!-- coefficients for desired pitch
+                     in relation to acceleration error -->
+        <field name="VerticalToPitchCrossFeed" units="deg / ((m/s)/(m/s))" type="float" elementnames="Kp,Max" defaultvalue="50, 5"/>
+                <!-- coefficients for adjusting desired pitch
+                     in relation to "vertical speed error relative to airspeed" (verror/IASactual) -->
+        <field name="AirspeedToVerticalCrossFeed" units="(m/s) / ((m/s)/(m/s))" type="float" elementnames="Kp,Max" defaultvalue="100, 100"/>
+                <!-- proportional coefficient for adjusting vertical speed error for power calculation
+                     in relation to relative airspeed error (IASerror/IASactual) -->
+        <field name="PowerPI" units="1/(m/s)" type="float" elements="3" elementnames="Kp,Ki,ILimit" defaultvalue="0.01,0.01,0.8"/>
+                <!-- proportional coefficient for desired throttle
+                     in relation to vertical speed error (absolute but including crossfeed) -->
+
+        <!-- output limits -->
+        <field name="RollLimit" units="deg" type="float" elements="3" elementnames="Min,Neutral,Max" defaultvalue="-35,0,35" />
+                <!-- maximum allowed bank angles in navigates flight -->
+        <field name="PitchLimit" units="deg" type="float" elements="3" elementnames="Min,Neutral,Max" defaultvalue="-10,5,20" />
+                <!-- maximum allowed pitch angles and setpoint for neutral pitch -->
+        <field name="ThrottleLimit" units="" type="float" elements="3" elementnames="Min,Neutral,Max" defaultvalue="0.1,0.5,0.9" />
+                <!-- minimum and maximum allowed throttle and setpoint for cruise speed -->
+
+        <field name="UpdatePeriod" units="ms" type="int32" elements="1" defaultvalue="100"/>
+        <access gcs="readwrite" flight="readwrite"/>
+        <telemetrygcs acked="true" updatemode="onchange" period="0"/>
+        <telemetryflight acked="true" updatemode="onchange" period="0"/>
+        <logging updatemode="manual" period="0"/>
+    </object>
+</xml>

shared/uavobjectdefinition/fixedwingpathfollowerstatus.xml

@@ -0,0 +1,13 @@
+<xml>
+    <object name="FixedWingPathFollowerStatus" singleinstance="true" settings="false">
+        <description>Object Storing Debugging Information on PID internals</description>
+	<field name="E" units="" type="float" elementnames="Course,Speed,Accel,Power" />
+	<field name="A" units="" type="float" elementnames="Course,Speed,Accel,Power" />
+	<field name="C" units="" type="float" elementnames="Course,Speed,Accel,Power" />
+	<field name="Errors" units="" type="uint8" elementnames="Wind,Lowspeed,Highspeed,Lowpower,Highpower,Pitchcontrol" />
+        <access gcs="readwrite" flight="readwrite"/>
+        <telemetrygcs acked="false" updatemode="manual" period="0"/>
+        <telemetryflight acked="false" updatemode="periodic" period="500"/>
+        <logging updatemode="manual" period="0"/>
+    </object>
+</xml>

shared/uavobjectdefinition/flightstatus.xml

@@ -4,7 +4,7 @@
         <field name="Armed" units="" type="enum" elements="1" options="Disarmed,Arming,Armed" defaultvalue="Disarmed"/>
 
 	<!-- Note these enumerated values should be the same as ManualControlSettings -->
-        <field name="FlightMode" units="" type="enum" elements="1" options="Manual,Stabilized1,Stabilized2,Stabilized3,AltitudeHold,VelocityControl,PositionHold,PathPlanner"/>
+        <field name="FlightMode" units="" type="enum" elements="1" options="Manual,Stabilized1,Stabilized2,Stabilized3,AltitudeHold,VelocityControl,PositionHold,ReturnToBase,PathPlanner"/>
 
         <access gcs="readwrite" flight="readwrite"/>
         <telemetrygcs acked="false" updatemode="manual" period="0"/>

shared/uavobjectdefinition/hwsettings.xml

@@ -18,7 +18,7 @@
 		<field name="USB_HIDPort" units="function" type="enum" elements="1" options="USBTelemetry,Disabled" defaultvalue="USBTelemetry"/>
 		<field name="USB_VCPPort" units="function" type="enum" elements="1" options="USBTelemetry,ComBridge,Disabled" defaultvalue="Disabled"/>
 
-		<field name="OptionalModules" units="" type="enum" elementnames="CameraStab,GPS,ComUsbBridge,Fault,Altitude,Airspeed,TxPID" options="Disabled,Enabled" defaultvalue="Disabled"/>
+		<field name="OptionalModules" units="" type="enum" elementnames="CameraStab,GPS,ComUsbBridge,Fault,Altitude,Airspeed,TxPID,VtolPathFollower,FixedWingPathFollower" options="Disabled,Enabled" defaultvalue="Disabled"/>
 		<field name="DSMxBind" units=""  type="uint8"  elements="1" defaultvalue="0"/>
 
 		<access gcs="readwrite" flight="readwrite"/>

shared/uavobjectdefinition/manualcontrolsettings.xml

@@ -23,7 +23,7 @@
         <field name="Stabilization3Settings" units="" type="enum" elementnames="Roll,Pitch,Yaw" options="None,Rate,Attitude,AxisLock,WeakLeveling" defaultvalue="Attitude,Attitude,Rate"/>
 
 	<!-- Note these options values should be identical to those defined in FlightMode -->
-        <field name="FlightModePosition" units="" type="enum" elements="3" options="Manual,Stabilized1,Stabilized2,Stabilized3,AltitudeHold,VelocityControl,PositionHold,PathPlanner" defaultvalue="Manual,Stabilized1,Stabilized2"/>
+        <field name="FlightModePosition" units="" type="enum" elements="3" options="Manual,Stabilized1,Stabilized2,Stabilized3,AltitudeHold,VelocityControl,PositionHold,ReturnToBase,PathPlanner" defaultvalue="Manual,Stabilized1,Stabilized2"/>
 
         <field name="ArmedTimeout" units="ms" type="uint16" elements="1" defaultvalue="30000"/>
         <access gcs="readwrite" flight="readwrite"/>

shared/uavobjectdefinition/pathaction.xml

@@ -0,0 +1,30 @@
+<xml>
+    <object name="PathAction" singleinstance="false" settings="false">
+        <description>A waypoint command the pathplanner is to use at a certain waypoint</description>
+	<field name="Mode" units="" type="enum" elements="1" options="FlyEndpoint,FlyVector,FlyCircleRight,FlyCircleLeft,
+		DriveEndpoint,DriveVector,DriveCircleLeft,DriveCircleRight,
+		FixedAttitude,
+		SetAccessory,
+		DisarmAlarm" default="Endpoint" />
+	<field name="ModeParameters" units="" type="float" elements="4" default="0"/>
+
+	<field name="EndCondition" units="" type="enum" elements="1" options="None,TimeOut,
+		DistanceToTarget,LegRemaining,BelowError,
+		AboveAltitude,AboveSpeed,
+		PointingTowardsNext,
+		PythonScript,
+		Immediate" default="None" />
+	<field name="ConditionParameters" units="" type="float" elements="4" default="0"/>
+
+	<field name="Command" units="" type="enum" elements="1" options="OnConditionNextWaypoint,OnNotConditionNextWaypoint,
+	OnConditionJumpWaypoint,OnNotConditionJumpWaypoint,
+	IfConditionJumpWaypointElseNextWaypoint" default="OnConditionNextWaypoint" />
+	<field name="JumpDestination" units="waypoint" type="int16" elements="1" default="0"/>
+	<field name="ErrorDestination" units="waypoint" type="int16" elements="1" default="0"/>
+
+        <access gcs="readwrite" flight="readwrite"/>
+        <telemetrygcs acked="false" updatemode="manual" period="0"/>
+        <telemetryflight acked="false" updatemode="periodic" period="4000"/>
+        <logging updatemode="periodic" period="1000"/>
+    </object>
+</xml>

shared/uavobjectdefinition/pathdesired.xml

@@ -7,9 +7,18 @@
 	<field name="StartingVelocity" units="m/s" type="float" elements="1" default="0"/>
 	<field name="EndingVelocity" units="m/s" type="float" elements="1" default="0"/>
 
-	<field name="Mode" units="" type="enum" elements="1" options="Endpoint,Path" default="0"/>
+	<field name="Mode" units="" type="enum" elements="1" options="FlyEndpoint,FlyVector,FlyCircleRight,FlyCircleLeft,
-	<!-- Endpoint Mode - Fly to end location and attempt to stay there -->
+		DriveEndpoint,DriveVector,DriveCircleLeft,DriveCircleRight,
-	<!-- Path Mode - Attempt to fly a line from Start to End using specifed velocity -->
+		FixedAttitude,
+		SetAccessory,
+		DisarmAlarm" default="0"/>
+	<!-- Endpoint mode - move directly towards endpoint regardless of position -->
+	<!-- Straight Mode - move across linear path through Start towards the waypoint end, adjusting velocity - continue straight -->
+	<!-- Circle Mode - move a circular pattern around End with radius End-Start (straight line in the vertical)-->
+	<field name="ModeParameters" units="" type="float" elements="4" default="0"/>
+
+	<field name="UID" units="" type="int16" elements="1" default="0"/>
+	<!-- unique ID confirmed with pathfollower in pathstatus to acknowledge a change in PathDesired -->
 
         <access gcs="readwrite" flight="readwrite"/>
         <telemetrygcs acked="false" updatemode="manual" period="0"/>

shared/uavobjectdefinition/pathplannersettings.xml

@@ -1,7 +1,6 @@
 <xml>
     <object name="PathPlannerSettings" singleinstance="true" settings="true">
         <description>Settings for the @ref PathPlanner Module</description>
-	<field name="PathMode" units="" type="enum" elements="1" options="ENDPOINT,PATH" defaultvalue="PATH"/>
 	<field name="PreprogrammedPath" units="" type="enum" elements="1" options="NONE,10M_BOX,LOGO" defaultvalue="NONE"/>
         <access gcs="readwrite" flight="readwrite"/>
         <telemetrygcs acked="true" updatemode="onchange" period="0"/>

shared/uavobjectdefinition/pathstatus.xml

@@ -0,0 +1,16 @@
+<xml>
+    <object name="PathStatus" singleinstance="true" settings="false">
+        <description>Status of the current path mode  Can come from any @ref PathFollower module</description>
+
+	<field name="UID" units="" type="int16" elements="1" default="0"/>
+	<!-- unique ID confirmed with pathfollower in pathstatus to acknowledge a change in PathDesired -->
+	<field name="Status" units="" type="enum" elements="1" options="InProgress,Completed,Warning,Critical"/>
+	<field name="fractional_progress" units="" type="float" elements="1"/>
+	<field name="error" units="m" type="float" elements="1"/>
+
+        <access gcs="readwrite" flight="readwrite"/>
+        <telemetrygcs acked="false" updatemode="manual" period="0"/>
+        <telemetryflight acked="false" updatemode="onchange" period="0"/>
+        <logging updatemode="onchange" period="0"/>
+    </object>
+</xml>

shared/uavobjectdefinition/vtolpathfollowersettings.xml

@@ -1,6 +1,6 @@
 <xml>
-    <object name="GuidanceSettings" singleinstance="true" settings="true">
+    <object name="VtolPathFollowerSettings" singleinstance="true" settings="true">
-        <description>Settings for the @ref GuidanceModule</description>
+        <description>Settings for the @ref VtolPathFollowerModule</description>
         <field name="GuidanceMode" units="" type="enum" elements="1" options="DUAL_LOOP,VELOCITY_CONTROL" defaultvalue="DUAL_LOOP"/>
 	<field name="HorizontalVelMax" units="m/s" type="uint16" elements="1" defaultvalue="10"/>
         <field name="VerticalVelMax" units="m/s" type="uint16" elements="1" defaultvalue="1"/>

shared/uavobjectdefinition/waypoint.xml

@@ -2,9 +2,8 @@
     <object name="Waypoint" singleinstance="false" settings="false">
         <description>A waypoint the aircraft can try and hit.  Used by the @ref PathPlanner module</description>
         <field name="Position" units="m" type="float" elementnames="North, East, Down"/>
-	<field name="Velocity" units="m/s" type="float" elementnames="North, East, Down"/>
+	<field name="Velocity" units="m/s" type="float" elements="1"/>
-	<field name="YawDesired" units="deg" type="float" elements="1"/>
+	<field name="Action" units="" type="uint8" elements="1" />
-	<field name="Action" units="" type="enum" elements="1" options="Next,RTH,Loiter10s,Land"/>
         <access gcs="readwrite" flight="readwrite"/>
         <telemetrygcs acked="false" updatemode="manual" period="0"/>
         <telemetryflight acked="false" updatemode="periodic" period="4000"/>

shared/uavobjectdefinition/waypointactive.xml

@@ -1,7 +1,7 @@
 <xml>
     <object name="WaypointActive" singleinstance="true" settings="false">
 	<description>Indicates the currently active waypoint</description>
-	<field name="Index" units="" type="uint8" elements="1"/>
+	<field name="Index" units="" type="int16" elements="1"/>
         <access gcs="readwrite" flight="readwrite"/>
         <telemetrygcs acked="false" updatemode="manual" period="0"/>
         <telemetryflight acked="false" updatemode="onchange" period="0"/>