From 6409dfb2df943d2e6141f2057e3a5414d7838121 Mon Sep 17 00:00:00 2001
From: Corvus Corax <corvuscorax@cybertrench.com>
Date: Sun, 28 Oct 2012 21:49:22 +0100
Subject: [PATCH] Further cleanup in PathFollower. Should now be safe to fly
 again

---
 .../fixedwingpathfollower.c                   | 94 ++++++-------------
 .../fixedwingpathfollowersettings.xml         | 11 +--
 .../fixedwingpathfollowerstatus.xml           |  6 +-
 3 files changed, 34 insertions(+), 77 deletions(-)

diff --git a/flight/Modules/FixedWingPathFollower/fixedwingpathfollower.c b/flight/Modules/FixedWingPathFollower/fixedwingpathfollower.c
index 7439c57ec..0b345d1ac 100644
--- a/flight/Modules/FixedWingPathFollower/fixedwingpathfollower.c
+++ b/flight/Modules/FixedWingPathFollower/fixedwingpathfollower.c
@@ -282,7 +282,6 @@ static void updatePathVelocity()
 		case PATHDESIRED_MODE_FLYCIRCLELEFT:
 		case PATHDESIRED_MODE_DRIVECIRCLELEFT:
 			groundspeed = pathDesired.EndingVelocity;
-//			groundspeed = fixedwingpathfollowerSettings.BestClimbRateSpeed;
 			altitudeSetpoint = pathDesired.End[2];
 			break;
 		case PATHDESIRED_MODE_FLYENDPOINT:
@@ -291,16 +290,19 @@ static void updatePathVelocity()
 		case PATHDESIRED_MODE_DRIVEVECTOR:
 		default:
 			groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) *
-			bound(progress.fractional_progress,0,1);
+				bound(progress.fractional_progress,0,1);
 			altitudeSetpoint = pathDesired.Start[2] + (pathDesired.End[2] - pathDesired.Start[2]) *
-			bound(progress.fractional_progress,0,1);
+				bound(progress.fractional_progress,0,1);
 			break;
 	}
+	// this ensures a significant forward component at least close to the real trajectory
+	if (groundspeed<fixedwingpathfollowerSettings.BestClimbRateSpeed/10.)
+		groundspeed=fixedwingpathfollowerSettings.BestClimbRateSpeed/10.;
 	
 	// calculate velocity - can be zero if waypoints are too close
 	VelocityDesiredData velocityDesired;
-	velocityDesired.North = progress.path_direction[0] * fmaxf(groundspeed,1e-6);
-	velocityDesired.East = progress.path_direction[1] * fmaxf(groundspeed,1e-6);
+	velocityDesired.North = progress.path_direction[0] * groundspeed;
+	velocityDesired.East = progress.path_direction[1] * groundspeed;
 	
 	float error_speed = progress.error * fixedwingpathfollowerSettings.HorizontalPosP;
 
@@ -366,10 +368,6 @@ static uint8_t updateFixedDesiredAttitude()
 	float indicatedAirspeedActual;
 	float indicatedAirspeedDesired;
 	float airspeedError;
-	float accelActual;
-	float accelDesired;
-	float accelError;
-	float accelCommand;
 	
 	float pitchCommand;
 
@@ -522,66 +520,28 @@ static uint8_t updateFixedDesiredAttitude()
 	/**
 	 * Compute desired pitch command
 	 */
-	// compute desired acceleration
-	if(0){
-		accelDesired = bound( (airspeedError/indicatedAirspeedActual) * fixedwingpathfollowerSettings.SpeedP[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDP_KP],
-			-fixedwingpathfollowerSettings.SpeedP[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDP_MAX],
-			fixedwingpathfollowerSettings.SpeedP[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDP_MAX]);
-		fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_SPEED] = airspeedError;
-		fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_SPEED] = 0.0f;
-		fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_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;
-		if (fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_KI] > 0){
-			accelIntegral = bound(accelIntegral + accelError * dT, 
-				-fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_ILIMIT]/fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_KI],
-				fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_ILIMIT]/fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_KI]);
-		}
 		
-		accelCommand = (accelError * fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_KP] + 
-			accelIntegral*fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_KI]);
+	if (fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI] > 0){
+		//Integrate with saturation
+		airspeedErrorInt=bound(airspeedErrorInt + airspeedError * dT, 
+				-fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_ILIMIT]/fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI],
+				fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_ILIMIT]/fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI]);
+	}				
 	
-		fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_ACCEL] = accelError;
-		fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_ACCEL] = accelIntegral;
-		fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_ACCEL] = accelCommand;
-		
-		pitchCommand= -accelCommand + bound ( (-descentspeedError/indicatedAirspeedActual) * fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_KP],
-			-fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX],
-			fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX]
-			);
-	}
-	else {
-		
-		if (fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_KI] > 0){
-			//Integrate with saturation
-			airspeedErrorInt=bound(airspeedErrorInt + airspeedError * dT, 
-					-fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_ILIMIT]/fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_KI],
-					fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_ILIMIT]/fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_KI]);
-		}				
-		
-		//Compute the cross feed from vertical speed to pitch, with saturation
-		float verticalSpeedToPitchCommandComponent=bound (-descentspeedError * fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_KP],
-				 -fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX],
-				 fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX]
-				 );
-		
-		//Compute the pitch command as err*Kp + errInt*Ki + X_feed.
-		pitchCommand= -(airspeedError*fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_KP] 
-							 + airspeedErrorInt*fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_KI]
-							 )	+ verticalSpeedToPitchCommandComponent;
-		
-		fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_SPEED] = airspeedError;
-		fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_SPEED] = airspeedErrorInt;
-		fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_SPEED] = indicatedAirspeedDesired;
-		
-		fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_ACCEL] = -123;
-		fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_ACCEL] = -123;
-		fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_ACCEL] = pitchCommand;
-		
-	}
+	//Compute the cross feed from vertical speed to pitch, with saturation
+	float verticalSpeedToPitchCommandComponent=bound (-descentspeedError * fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_KP],
+			 -fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX],
+			 fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX]
+			 );
+	
+	//Compute the pitch command as err*Kp + errInt*Ki + X_feed.
+	pitchCommand= -(airspeedError*fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KP] 
+						 + airspeedErrorInt*fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI]
+						 )	+ verticalSpeedToPitchCommandComponent;
+	
+	fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_SPEED] = airspeedError;
+	fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_SPEED] = airspeedErrorInt;
+	fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_SPEED] = pitchCommand;
 
 	stabDesired.Pitch = bound(fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_NEUTRAL] +
 		pitchCommand,
diff --git a/shared/uavobjectdefinition/fixedwingpathfollowersettings.xml b/shared/uavobjectdefinition/fixedwingpathfollowersettings.xml
index e2e01e00a..163a06687 100644
--- a/shared/uavobjectdefinition/fixedwingpathfollowersettings.xml
+++ b/shared/uavobjectdefinition/fixedwingpathfollowersettings.xml
@@ -6,9 +6,9 @@
         <field name="AirSpeedMax" units="m/s" type="float" elements="1" defaultvalue="20"/>
                 <!-- Vne, i.e. maximum airspeed the airframe can handle -->
         <field name="CruiseSpeed" units="m/s" type="float" elements="1" defaultvalue="20"/>
-                <!-- Cruise speed the in level flight - leave some safety margin -->
+                <!-- Maximum speed the autopilot will try to achieve, usually for long distances -->
         <field name="BestClimbRateSpeed" units="m/s" type="float" elements="1" defaultvalue="10"/>
-                <!-- V_y, i.e. airspeed at which plane climbs the best -->
+                <!-- V_y, Minimum speed the autopilot will try to fly, for example when loitering -->
         <field name="StallSpeedClean" units="m/s" type="float" elements="1" defaultvalue="8"/>
                 <!-- Vs1, i.e. stall speed in clean configuration- leave some safety margin -->
         <field name="StallSpeedDirty" units="m/s" type="float" elements="1" defaultvalue="8"/>
@@ -28,12 +28,9 @@
         <field name="BearingPI" 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 bearing 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, .15, 20"/>
+        <field name="SpeedPI" units="deg / (m/s)" type="float" elements="3" elementnames="Kp,Ki,ILimit" defaultvalue="1.5, .15, 20"/>
                 <!-- coefficients for desired pitch
-                     in relation to acceleration error -->
+                     in relation to relative speed error (IASerror/IASactual) -->
         <field name="VerticalToPitchCrossFeed" units="deg / ((m/s)/(m/s))" type="float" elementnames="Kp,Max" defaultvalue="5, 10"/>
                 <!-- coefficients for adjusting desired pitch
                      in relation to "vertical speed error relative to airspeed" (verror/IASactual) -->
diff --git a/shared/uavobjectdefinition/fixedwingpathfollowerstatus.xml b/shared/uavobjectdefinition/fixedwingpathfollowerstatus.xml
index fd70ce3b1..a8425dbd0 100644
--- a/shared/uavobjectdefinition/fixedwingpathfollowerstatus.xml
+++ b/shared/uavobjectdefinition/fixedwingpathfollowerstatus.xml
@@ -1,9 +1,9 @@
 <xml>
     <object name="FixedWingPathFollowerStatus" singleinstance="true" settings="false">
         <description>Object Storing Debugging Information on PID internals</description>
-	<field name="Error" units="" type="float" elementnames="Bearing,Speed,Accel,Power" />
-	<field name="ErrorInt" units="" type="float" elementnames="Bearing,Speed,Accel,Power" />
-	<field name="Command" units="" type="float" elementnames="Bearing,Speed,Accel,Power" />
+	<field name="Error" units="" type="float" elementnames="Bearing,Speed,Power" />
+	<field name="ErrorInt" units="" type="float" elementnames="Bearing,Speed,Power" />
+	<field name="Command" units="" type="float" elementnames="Bearing,Speed,Power" />
 	<field name="Errors" units="" type="uint8" elementnames="Wind,Stallspeed,Lowspeed,Highspeed,Overspeed,Lowpower,Highpower,Pitchcontrol" />
         <access gcs="readwrite" flight="readwrite"/>
         <telemetrygcs acked="false" updatemode="manual" period="0"/>