From da1a7620723f5e82e4bad7fb362e598a754eaa0d Mon Sep 17 00:00:00 2001
From: andrew <andrew>
Date: Fri, 17 Sep 2010 05:52:34 +0000
Subject: [PATCH] Added basic code for Yaw stabilisation in manual mode for
 Helis. This is functionally the same as having a heading hold gyro in
 hardware. Code is copied from the simple stabilisation routine and is a bit
 rough at the minute, but initial testing looks good.

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1670 ebee16cc-31ac-478f-84a7-5cbb03baadba
---
 flight/OpenPilot/Modules/Actuator/actuator.c | 58 ++++++++++++++++++++
 1 file changed, 58 insertions(+)

diff --git a/flight/OpenPilot/Modules/Actuator/actuator.c b/flight/OpenPilot/Modules/Actuator/actuator.c
index fa4f73fa5..335512736 100644
--- a/flight/OpenPilot/Modules/Actuator/actuator.c
+++ b/flight/OpenPilot/Modules/Actuator/actuator.c
@@ -40,6 +40,12 @@
 #include "actuatorcommand.h"
 #include "manualcontrolcommand.h"
 
+//yaw stabilisation for helis...
+#include "stabilizationsettings.h"
+#include "attitudeactual.h"
+//yaw stabilisation for helis...
+
+
 // Private constants
 #define MAX_QUEUE_SIZE 2
 #define STACK_SIZE configMINIMAL_STACK_SIZE
@@ -47,6 +53,18 @@
 #define FAILSAFE_TIMEOUT_MS 100
 
 // Private types
+		//yaw stabilisation for helis...
+		//currently just coppied from simple stabilisation
+		#define YAW_INTEGRAL_LIMIT 0.5
+		AttitudeActualData attitudeActual;
+		StabilizationSettingsData stabSettings;
+		float ccpmYawError, ccpmYawErrorLast;
+		float ccpmYawDerivative;
+		float ccpmYawIntegral, ccpmYawIntegralLimit;
+		float desiredYaw;
+		static float bound(float val, float min, float max);
+		//yaw stabilisation for helis...
+
 
 // Private variables
 static xQueueHandle queue;
@@ -76,6 +94,10 @@ int32_t ActuatorInitialize()
 	// Listen for ExampleObject1 updates
 	ActuatorDesiredConnectQueue(queue);
 
+	ccpmYawErrorLast = 0.0;
+
+
+
 	// Start main task
 	xTaskCreate(actuatorTask, (signed char*)"Actuator", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle);
 
@@ -563,10 +585,46 @@ static int32_t mixerCCPM( ActuatorSettingsData* settings, const ActuatorDesiredD
 		{
 			configOK=0;
 		}
+		ccpmYawErrorLast = 0.0;
+
 	}
 	else
 	{//TODO need to implement the stabilization PID loop to provide heading hold gyro functionality in manual control mode
+		//currently just coppied from simple stabilisation
 
+		AttitudeActualGet(&attitudeActual);
+		StabilizationSettingsGet(&stabSettings);
+
+		if (desired->Yaw<0)
+		{
+			desiredYaw = 360 + (desired->Yaw*180.0);
+		}
+		else
+		{
+			desiredYaw = (desired->Yaw*180.0);
+		}
+
+		// Yaw stabilization control loop
+			ccpmYawError = desiredYaw - attitudeActual.Yaw;
+			//this should make it take the quickest path to reach the desired yaw
+			if (ccpmYawError>180.0)ccpmYawError -= 360;
+			if (ccpmYawError<-180.0)ccpmYawError += 360;
+			ccpmYawDerivative = ccpmYawError - ccpmYawErrorLast;
+			ccpmYawIntegralLimit = YAW_INTEGRAL_LIMIT / stabSettings.YawKi;
+			ccpmYawIntegral = bound(ccpmYawIntegral+ccpmYawError*stabSettings.UpdatePeriod, -ccpmYawIntegralLimit, ccpmYawIntegralLimit);
+			desiredYaw = stabSettings.YawKp*ccpmYawError + stabSettings.YawKi*ccpmYawIntegral + stabSettings.YawKd*ccpmYawDerivative;;
+			desiredYaw = bound(desiredYaw, -1.0, 1.0);
+			ccpmYawErrorLast = ccpmYawError;
+			if ( settings->CCPMTailRotor != ACTUATORSETTINGS_CCPMTAILROTOR_NONE )
+			{
+				cmd->Channel[ settings->CCPMTailRotor ] = scaleChannel(desiredYaw, settings->ChannelMax[ settings->CCPMTailRotor ],
+																	  settings->ChannelMin[ settings->CCPMTailRotor ],
+																	  settings->ChannelNeutral[ settings->CCPMTailRotor ]);
+			}
+			else
+			{
+				configOK=0;
+			}
 	}
 	if (configOK<2)return -1;
 	return 0;