--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/systemswstubs/tiltcompensationstub/src/tiltcompensation.cpp	Mon Jan 18 20:06:52 2010 +0200
@@ -0,0 +1,211 @@
+/*
+* Copyright (c) 2007 Nokia Corporation and/or its subsidiary(-ies). 
+* All rights reserved.
+* This component and the accompanying materials are made available
+* under the terms of "Eclipse Public License v1.0"
+* which accompanies this distribution, and is available
+* at the URL "http://www.eclipse.org/legal/epl-v10.html".
+*
+* Initial Contributors:
+* Nokia Corporation - initial contribution.
+*
+* Contributors:
+*
+* Description:  Compensate function definition.
+*
+*/
+
+
+#include <e32math.h>
+#include <tiltcompensation.h>
+#include <e32debug.h>
+
+//-----------------------------------------------------------------------------
+// Direction6D
+//-----------------------------------------------------------------------------
+//
+EXPORT_C TInt Compensate(
+    const TTiltCompensationInput& aInput,
+    TTiltCompensationOutput& aOutput,
+    const TInt16 aPreviousTheta
+     )
+    {
+    TReal target;
+    TReal source( 0 );
+    TReal inputY( aInput.iMagneticVector.iY );
+    TReal inputZ( aInput.iMagneticVector.iZ );
+    
+    if( aInput.iMagneticVector.iY > 0 && aInput.iMagneticVector.iZ > 0 )
+    // 0 - 90 degrees
+        {
+        source = inputY / inputZ;
+        }
+    if( aInput.iMagneticVector.iY > 0 && aInput.iMagneticVector.iZ < 0 )
+    // 90 - 180 degrees
+        {
+        source = inputZ / inputY * -1;
+        }
+    if( aInput.iMagneticVector.iY < 0 && aInput.iMagneticVector.iZ < 0 )
+    // 180 - 270 degrees
+        {
+        source = inputY / inputZ;
+        }
+    if( aInput.iMagneticVector.iY < 0 && aInput.iMagneticVector.iZ > 0 )
+    // 270 - 360 degrees
+        {
+        source = inputZ / inputY * -1;
+        }
+    
+    Math::ATan( target, source );
+    TInt16 declination( ( target * 180 ) / 3.14 );
+    
+    if( aInput.iMagneticVector.iY > 0 && aInput.iMagneticVector.iZ > 0 )
+    // 0 - 90 degrees
+        {
+        }
+    if( aInput.iMagneticVector.iY > 0 && aInput.iMagneticVector.iZ < 0 )
+    // 90 - 180 degrees
+        {
+        declination = declination + 90;
+        }
+    if( aInput.iMagneticVector.iY < 0 && aInput.iMagneticVector.iZ < 0 )
+    // 180 - 270 degrees
+        {
+        declination = declination + 180;
+        }
+    if( aInput.iMagneticVector.iY < 0 && aInput.iMagneticVector.iZ > 0 )
+    // 270 - 360 degrees
+        {
+        declination = declination + 270;
+        }
+    
+    if( inputY == 0 && inputZ > 0 )
+        {
+        declination = 0;
+        }
+    if( inputY > 0 && inputZ == 0 )
+        {
+        declination = 90;
+        }
+    if( inputY == 0 && inputZ < 0 )
+        {
+        declination = 180;
+        }
+    if( inputY < 0 && inputZ == 0 )
+        {
+        declination = 270;
+        }
+    
+    if( ( aPreviousTheta < ( aOutput.iTheta + 5 ) ) && ( aPreviousTheta > ( aOutput.iTheta - 5 ) ) )
+        {
+        aOutput.iTheta = aPreviousTheta;
+        }
+    
+    aOutput.iTheta = declination;
+    return KErrNone;
+    }
+    
+//-----------------------------------------------------------------------------
+// Direction6D
+//-----------------------------------------------------------------------------
+//
+EXPORT_C TInt Compensate(
+    const TTiltCompensationInput& aInput,
+    TTiltCompensationOutput& aOutput,
+    const TInt16 aPreviousTheta,
+    const RParamsArray& aParamsArray )
+    {
+    TReal target;
+    TReal source( 0 );
+    TReal inputY( aInput.iMagneticVector.iY );
+    TReal inputX( aInput.iMagneticVector.iX );
+    
+    // Get parameters
+    for( TInt i = 0; i != aParamsArray.Count(); i++ )
+        {
+        TInt parameter = aParamsArray[ i ];
+        }
+    
+    if( aInput.iMagneticVector.iY > 0 && aInput.iMagneticVector.iX > 0 )
+    // 0 - 90 degrees
+        {
+        source = inputX / inputY;
+        }
+    if( aInput.iMagneticVector.iY > 0 && aInput.iMagneticVector.iX < 0 )
+    // 90 - 180 degrees
+        {
+        source = inputX / inputY * -1;
+        }
+    if( aInput.iMagneticVector.iY < 0 && aInput.iMagneticVector.iX < 0 )
+    // 180 - 270 degrees
+        {
+        source = inputY / inputX;
+        }
+    if( aInput.iMagneticVector.iY < 0 && aInput.iMagneticVector.iX > 0 )
+    // 270 - 360 degrees
+        {
+        source = inputX / inputY * -1;
+        }
+    
+    Math::ATan( target, source );
+    TInt16 declination( ( target * 180 ) / 3.14 );
+    
+    if( aInput.iMagneticVector.iY > 0 && aInput.iMagneticVector.iX > 0 )
+    // 0 - 90 degrees
+        {
+        }
+    if( aInput.iMagneticVector.iY > 0 && aInput.iMagneticVector.iX < 0 )
+    // 90 - 180 degrees
+        {
+        declination = declination + 90;
+        }
+    if( aInput.iMagneticVector.iY < 0 && aInput.iMagneticVector.iX < 0 )
+    // 180 - 270 degrees
+        {
+        declination = declination + 180;
+        }
+    if( aInput.iMagneticVector.iY < 0 && aInput.iMagneticVector.iX > 0 )
+    // 270 - 360 degrees
+        {
+        declination = declination + 270;
+        }
+    
+    if( inputY == 0 && inputX > 0 )
+        {
+        declination = 0;
+        }
+    if( inputY > 0 && inputX == 0 )
+        {
+        declination = 90;
+        }
+    if( inputY == 0 && inputX < 0 )
+        {
+        declination = 180;
+        }
+    if( inputY < 0 && inputX == 0 )
+        {
+        declination = 270;
+        }
+    
+    if( ( aPreviousTheta < ( aOutput.iTheta + 5 ) ) && ( aPreviousTheta > ( aOutput.iTheta - 5 ) ) )
+        {
+        aOutput.iTheta = aPreviousTheta;
+        }
+    
+    // Filter data...
+    
+    // For testing purposes change angle with fixed values from input parameters
+    aOutput.iTheta = declination;
+    const TInt KAngleChange = 30;
+    aOutput.iTheta += ( aParamsArray[ 0 ] * KAngleChange );
+    aOutput.iTheta += ( aParamsArray[ 1 ] * KAngleChange );
+    aOutput.iTheta += ( aParamsArray[ 2 ] * KAngleChange );
+    aOutput.iTheta += ( aParamsArray[ 3 ] * KAngleChange );
+    aOutput.iTheta %= 360; // take modulo to avoid angle larger than 360
+    // ... end for testing purposes
+    
+
+    return KErrNone;
+    }    
+
+// End of File