Add path navigation mode to guidance. Works well in simulation.

flight/Libraries/inc/paths.h

@@ -0,0 +1,39 @@
+/**
+ ******************************************************************************
+ *
+ * @file       paths.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
+ * @brief      Header for path manipulation library 
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#ifndef PATHS_H_
+#define PATHS_H_
+
+struct path_status {
+	float fractional_progress;
+	float error;
+	float correction_direction[2];
+	float path_direction[2];
+};
+
+void path_progress(float * start_point, float * end_point, float * cur_point, struct path_status * status);
+
+#endif

flight/Libraries/paths.c

@@ -0,0 +1,72 @@
+/**
+ ******************************************************************************
+ *
+ * @file       paths.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
+ * @brief      Library path manipulation 
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#include "pios.h"
+#include "paths.h"
+
+/**
+ * @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[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)
+{
+	float path_north, path_east, diff_north, diff_east;
+	float dist_path2;
+	float dot;
+	float normal[2];
+
+	// Distance to go
+	path_north = end_point[0] - start_point[0];
+	path_east = end_point[1] - start_point[1];
+
+	// Current progress location relative to start
+	diff_north = cur_point[0] - start_point[0];
+	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;
+
+	// Compute the normal to the path
+	normal[0] = -path_east / sqrtf(dist_path2);
+	normal[1] = path_north / sqrtf(dist_path2);
+
+	status->fractional_progress = dot / dist_path2;
+	status->error = normal[0] * diff_north + normal[1] * diff_east;
+
+	// Compute direction to correct error
+	status->correction_direction[0] = -status->error * normal[0];
+	status->correction_direction[1] = -status->error * normal[1];
+
+	// Compute direction to travel
+	status->path_direction[0] = path_north / sqrtf(dist_path2);
+	status->path_direction[1] = path_east / sqrtf(dist_path2);
+
+	status->error = fabs(status->error);
+}
+

flight/Modules/Guidance/guidance.c

@@ -44,9 +44,12 @@
  */
 
 #include "openpilot.h"
+#include "paths.h"
+
 #include "guidance.h"
 #include "accels.h"
 #include "attitudeactual.h"
+#include "pathdesired.h"	// object that will be updated by the module
 #include "positiondesired.h"	// object that will be updated by the module
 #include "positionactual.h"
 #include "manualcontrol.h"
@@ -74,10 +77,14 @@ static xQueueHandle queue;
 static void guidanceTask(void *parameters);
 static float bound(float val, float min, float max);
 
+static void updateNedAccel();
+static void updatePathVelocity();
 static void updateVtolDesiredVelocity();
 static void manualSetDesiredVelocity();
 static void updateVtolDesiredAttitude();
 
+static GuidanceSettingsData guidanceSettings;
+
 /**
  * Initialise the module, called on startup
  * \returns 0 on success or -1 if initialisation failed
@@ -98,8 +105,9 @@ int32_t GuidanceStart()
 int32_t GuidanceInitialize()
 {
 	GuidanceSettingsInitialize();
-	PositionDesiredInitialize();
 	NedAccelInitialize();
+	PathDesiredInitialize();
+	PositionDesiredInitialize();
 	VelocityDesiredInitialize();
 
 	// Create object queue
@@ -128,20 +136,12 @@ static uint8_t positionHoldLast = 0;
 static void guidanceTask(void *parameters)
 {
 	SystemSettingsData systemSettings;
-	GuidanceSettingsData guidanceSettings;
 	FlightStatusData flightStatus;
 
 	portTickType thisTime;
 	portTickType lastUpdateTime;
 	UAVObjEvent ev;
 	
-	float accel[3] = {0,0,0};
-	uint32_t accel_accum = 0;
-	
-	float q[4];
-	float Rbe[3][3];
-	float accel_ned[3];
-	
 	// Main task loop
 	lastUpdateTime = xTaskGetTickCount();
 	while (1) {
@@ -155,78 +155,99 @@ static void guidanceTask(void *parameters)
 			AlarmsClear(SYSTEMALARMS_ALARM_GUIDANCE);
 		}
 
-		// Collect downsampled attitude data
-		AccelsData accels;
-		AccelsGet(&accels);		
-		accel[0] += accels.x;
-		accel[1] += accels.y;
-		accel[2] += accels.z;
-		accel_accum++;
-
 		// Continue collecting data if not enough time
 		thisTime = xTaskGetTickCount();
 		if( (thisTime - lastUpdateTime) < (guidanceSettings.UpdatePeriod / portTICK_RATE_MS) )
 			continue;
 
-		lastUpdateTime = xTaskGetTickCount();
+		// Convert the accels into the NED frame
-		accel[0] /= accel_accum;
+		updateNedAccel();
-		accel[1] /= accel_accum;
-		accel[2] /= accel_accum;
-		accel[0] = accel[1] = accel[2] = 0;
-		accel_accum = 0;
-		
-		//rotate avg accels into earth frame and store it
-		AttitudeActualData attitudeActual;
-		AttitudeActualGet(&attitudeActual);
-		q[0]=attitudeActual.q1;
-		q[1]=attitudeActual.q2;
-		q[2]=attitudeActual.q3;
-		q[3]=attitudeActual.q4;
-		Quaternion2R(q, Rbe);
-		for (uint8_t i=0; i<3; i++){
-			accel_ned[i]=0;
-			for (uint8_t j=0; j<3; j++)
-				accel_ned[i] += Rbe[j][i]*accel[j];
-		}
-		accel_ned[2] += 9.81f;
-		
-		NedAccelData accelData;
-		NedAccelGet(&accelData);
-		accelData.North = accel_ned[0];
-		accelData.East = accel_ned[1];
-		accelData.Down = accel_ned[2];
-		NedAccelSet(&accelData);
 		
 		FlightStatusGet(&flightStatus);
 		SystemSettingsGet(&systemSettings);
 		GuidanceSettingsGet(&guidanceSettings);
 
-		if ((flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD ||
+		if ( (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_VTOL) &&
-			 flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) &&
+		     (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADP) &&
-		    ((systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_VTOL) ||
+		     (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADX) &&
-		     (systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADP) ||
+		     (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) )
-		     (systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADX) ||
-		     (systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) ))
 		{
-			if( flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD ||
+			AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_WARNING);
-			   flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) 
+			continue;
+		}
+		
+		switch(flightStatus.FlightMode) {
+			case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
 				updateVtolDesiredVelocity();
-			else 
+				updateVtolDesiredAttitude();
+				break;
+			case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
+				if (guidanceSettings.PathMode == GUIDANCESETTINGS_PATHMODE_ENDPOINT)
+					updateVtolDesiredVelocity();
+				else
+					updatePathVelocity();
+				updateVtolDesiredAttitude();
+				break;
+			default:
+				// Be cleaner and get rid of global variables
+				northVelIntegral = 0;
+				eastVelIntegral = 0;
+				downVelIntegral = 0;
+				northPosIntegral = 0;
+				eastPosIntegral = 0;
+				downPosIntegral = 0;
+				positionHoldLast = 0;
+				
 				manualSetDesiredVelocity();
-			updateVtolDesiredAttitude();
+				break;
-		} else {
-			// Be cleaner and get rid of global variables
-			northVelIntegral = 0;
-			eastVelIntegral = 0;
-			downVelIntegral = 0;
-			northPosIntegral = 0;
-			eastPosIntegral = 0;
-			downPosIntegral = 0;
-			positionHoldLast = 0;
 		}
 	}
 }
 
+/**
+ * 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()
+{
+	PathDesiredData pathDesired;
+	PathDesiredGet(&pathDesired);
+
+	PositionActualData positionActual;
+	PositionActualGet(&positionActual);
+	
+	float cur[3] = {positionActual.North, positionActual.East, positionActual.Down};
+	struct path_status progress;
+	
+	path_progress(pathDesired.Start, pathDesired.End, cur, &progress);
+	
+	float groundspeed = pathDesired.StartingVelocity + 
+	    (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * progress.fractional_progress;
+	if(progress.fractional_progress > 1)
+		groundspeed = 0;
+	
+	VelocityDesiredData velocityDesired;
+	velocityDesired.North = progress.path_direction[0] * groundspeed;
+	velocityDesired.East = progress.path_direction[1] * groundspeed;
+	velocityDesired.Down = 0;
+	
+	float error_speed = progress.error * guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_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)
+		scale = guidanceSettings.HorizontalVelMax / total_vel;
+
+	velocityDesired.North += progress.correction_direction[0] * error_speed * scale;
+	velocityDesired.East += progress.correction_direction[1] * error_speed * scale;
+	
+	VelocityDesiredSet(&velocityDesired);
+}
+
 /**
  * Compute desired velocity from the current position
  *
@@ -405,6 +426,46 @@ static void updateVtolDesiredAttitude()
 	StabilizationDesiredSet(&stabDesired);
 }
 
+/**
+ * Keep a running filtered version of the acceleration in the NED frame
+ */
+static void updateNedAccel()
+{
+	float accel[3];
+	float q[4];
+	float Rbe[3][3];
+	float accel_ned[3];
+
+	// Collect downsampled attitude data
+	AccelsData accels;
+	AccelsGet(&accels);		
+	accel[0] = accels.x;
+	accel[1] = accels.y;
+	accel[2] = accels.z;
+	
+	//rotate avg accels into earth frame and store it
+	AttitudeActualData attitudeActual;
+	AttitudeActualGet(&attitudeActual);
+	q[0]=attitudeActual.q1;
+	q[1]=attitudeActual.q2;
+	q[2]=attitudeActual.q3;
+	q[3]=attitudeActual.q4;
+	Quaternion2R(q, Rbe);
+	for (uint8_t i=0; i<3; i++){
+		accel_ned[i]=0;
+		for (uint8_t j=0; j<3; j++)
+			accel_ned[i] += Rbe[j][i]*accel[j];
+	}
+	accel_ned[2] += 9.81f;
+	
+	NedAccelData accelData;
+	NedAccelGet(&accelData);
+	accelData.North = accel_ned[0];
+	accelData.East = accel_ned[1];
+	accelData.Down = accel_ned[2];
+	NedAccelSet(&accelData);
+}
+
 /**
  * Set the desired velocity from the input sticks
  */

flight/Revolution/Makefile

@@ -133,6 +133,7 @@ SRC += $(OPSYSTEM)/cm3_fault_handlers.c
 #endif
 
 SRC += $(FLIGHTLIB)/CoordinateConversions.c
+SRC += $(FLIGHTLIB)/paths.c
 SRC += $(FLIGHTLIB)/fifo_buffer.c
 SRC += $(FLIGHTLIB)/WorldMagModel.c
 SRC += $(FLIGHTLIB)/insgps13state.c

flight/Revolution/Makefile.osx

@@ -186,6 +186,7 @@ SRC += $(PIOSPOSIX)/pios_rcvr.c
 SRC += $(FLIGHTLIB)/fifo_buffer.c
 SRC += $(FLIGHTLIB)/WorldMagModel.c
 SRC += $(FLIGHTLIB)/CoordinateConversions.c
+SRC += $(FLIGHTLIB)/paths.c
 SRC += $(FLIGHTLIB)/insgps13state.c
 SRC += $(FLIGHTLIB)/taskmonitor.c
 

flight/Revolution/UAVObjects.inc

@@ -58,6 +58,7 @@ UAVOBJSRCFILENAMES += nedaccel
 UAVOBJSRCFILENAMES += nedposition
 UAVOBJSRCFILENAMES += objectpersistence
 UAVOBJSRCFILENAMES += overosyncstats
+UAVOBJSRCFILENAMES += pathdesired
 UAVOBJSRCFILENAMES += positionactual
 UAVOBJSRCFILENAMES += positiondesired
 UAVOBJSRCFILENAMES += ratedesired

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

@@ -54,6 +54,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
     $$UAVOBJECT_SYNTHETICS/gpsposition.h \
     $$UAVOBJECT_SYNTHETICS/gpstime.h \
     $$UAVOBJECT_SYNTHETICS/gpssatellites.h \
+    $$UAVOBJECT_SYNTHETICS/pathdesired.h \
     $$UAVOBJECT_SYNTHETICS/positionactual.h \
     $$UAVOBJECT_SYNTHETICS/flightbatterystate.h \
     $$UAVOBJECT_SYNTHETICS/homelocation.h \
@@ -117,6 +118,7 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
     $$UAVOBJECT_SYNTHETICS/gpsposition.cpp \
     $$UAVOBJECT_SYNTHETICS/gpstime.cpp \
     $$UAVOBJECT_SYNTHETICS/gpssatellites.cpp \
+    $$UAVOBJECT_SYNTHETICS/pathdesired.cpp \
     $$UAVOBJECT_SYNTHETICS/positionactual.cpp \
     $$UAVOBJECT_SYNTHETICS/flightbatterystate.cpp \
     $$UAVOBJECT_SYNTHETICS/homelocation.cpp \

shared/uavobjectdefinition/guidancesettings.xml

@@ -2,7 +2,8 @@
     <object name="GuidanceSettings" singleinstance="true" settings="true">
         <description>Settings for the @ref GuidanceModule</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="PathMode" units="" type="enum" elements="1" options="ENDPOINT,PATH" defaultvalue="ENDPOINT"/>
+	<field name="HorizontalVelMax" units="m/s" type="uint16" elements="1" defaultvalue="10"/>
         <field name="VerticalVelMax" units="m/s" type="uint16" elements="1" defaultvalue="1"/>
         <field name="HorizontalPosPI" units="(m/s)/m" type="float" elementnames="Kp,Ki,ILimit" defaultvalue="1,0,0"/>
         <field name="HorizontalVelPID" units="deg/(m/s)" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="10,0,1,0"/>

shared/uavobjectdefinition/pathdesired.xml

@@ -0,0 +1,13 @@
+<xml>
+    <object name="PathDesired" singleinstance="true" settings="false">
+        <description>The position the craft is trying t achieve.  Can come from GCS or @ref PathPlanner </description>
+	<field name="Start" units="m" type="float" elementnames="North,East,Down" default="0"/>
+        <field name="End" units="m" type="float" elementnames="North,East,Down" default="0"/>
+	<field name="StartingVelocity" units="m/s" type="float" elements="1" default="0"/>
+	<field name="EndingVelocity" units="m/s" type="float" elements="1" default="0"/>
+        <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>