Problem need some help for an autopilot to controll ypr...

Topper

Addon Developer
Addon Developer
Donator
Joined
Mar 28, 2008
Messages
666
Reaction score
20
Points
33
Hello,
I have some Problems by developeing the new baseland add on:
I noticed that using the inline graphic client, there are always some little "thruster activities", i guess because the values for the allingment are never 100% corect after one timestep. I know it's a bit complicate but if somebody wants to help me I can even share the whole project, how ever I try to show you the important parts here. Maybe somebody know what I'm doing wrong or how to solve this issue...

I don't realy belive that anyone could help me this way but I will try it because I'm out of ideas... :)

So this is the method to controll Yaw, Pitch and Roll.
It calls the method "getYPRSpeedSetpointsByDeltaPhy3" to getting the delta (see later) and the method "setYPRSpeed" to set the thrusters.

Ah and this method works very good if the setpoint is constant, but not when the setpoint is changing...
And the goal is to have such a method which works for each vessel, so this should be an "universal" method with good capibilities.
So it needs to use the API to get the maximum angular accelerations and so on (as it does).

Code:
void autopilot::ctrlYPR()
{    
    //masterChannel = true;
    quaternion q_setpoints;
    q_setpoints.yprToQuat(ypr_setpoint);
    MATRIX3 mq = q_setpoints.getMatrix();
    MATRIX3 v_matrix = ORBITERTOOLS::getLocalRotationMatrix2(v);
    MATRIX3 dm = mul(mq ,MYMATH::invertMatrix(v_matrix));

    dv.x = asin(dm.m21);
    dv.y = -atan2(-dm.m23, dm.m22);
    dv.z = atan2(-dm.m31,dm.m11);    

    currentYPR.x = ypr_setpoint.x + dv.x;
    currentYPR.y = ypr_setpoint.y - dv.y;
    currentYPR.z = ypr_setpoint.z + dv.z;

    //currentYPR.x = NAN;

    double yawSpeed, pitchSpeed, bankSpeed;
    
    if (ORBITERTOOLS::checkForTouchDown(v))
    {
        masterChannel = false;
        yawChannel = false;
        pitchChannel = false;
        rollChannel = false;
        vaChannel = false;    
    }

    if (masterChannel_last && ! masterChannel)
    {
        resetThruster();    
        thrusterResetet = true;
    }
    
    if (masterChannel) // If Autopilot is active...
    {            
        VECTOR3 yprSpeedSetPoint = getYPRSpeedSetpointsByDeltaPhy3(dv);     // Getting the delta between current values and setpoints
        // The "if" also calls the method setYPRSpeed. It should return false on a NAN exception.
        if (!setYPRSpeed(yprSpeedSetPoint, yawChannel, pitchChannel, rollChannel) || currentYPR.x > PI2 ||currentYPR.y > PI2 || currentYPR.z > PI2 || ypr_setpoint.x > PI2 || ypr_setpoint.y > PI2 || ypr_setpoint.z > PI2 || currentYPR.x < -PI2 ||currentYPR.y < -PI2 || currentYPR.z < -PI2 || ypr_setpoint.x < -PI2 || ypr_setpoint.y < -PI2 || ypr_setpoint.z < -PI2)
        {
            ypr_setpoint_last.x = 0;
            ypr_setpoint_last.y = 0;
            ypr_setpoint_last.z = 0;

            currentYPR.x = 0;
            currentYPR.y = 0;
            currentYPR.z = 0;

            ypr_setpoint.x = 0;
            ypr_setpoint.y = 0;
            ypr_setpoint.z = 0;

            dv.x = 0;
            dv.y = 0;
            dv.z = 0;

        }
        thrusterResetet = false;        
    }
    masterChannel_last = masterChannel;
    ypr_setpoint_last = ypr_setpoint;    
}



Code:
VECTOR3 autopilot::getYPRSpeedSetpointsByDeltaPhy3(VECTOR3 deltaPhy)
{
    
    VECTOR3 angularSpeedSetPoint = { 0,0,0 };
    VECTOR3 vMaxAccAcceleration = ORBITERTOOLS::GetMaxAngAcc(v);  //Returns the maximum angular acceleration (Omega)
                                                                  //"i" stores if deltaPhy is positiv or negativ to set angularSpeedSetPoint positiv or negative in the end
    VECTOR3 i = { 0,0,0 };

    VECTOR3 vMaxAccXYZ;
    double maxYawAcceleration = vMaxAccAcceleration.y * abs(deltaPhy.x) * 0.9;
    double maxPitchAcceleration = vMaxAccAcceleration.x * abs(deltaPhy.y) * 0.9;
    double maxBankAcceleration = vMaxAccAcceleration.z * abs(deltaPhy.z) * 0.9;

    i.x = deltaPhy.x < 0 ? -1 : 1;
    i.y = deltaPhy.y < 0 ? -1 : 1;
    i.z = deltaPhy.z < 0 ? -1 : 1;

    VECTOR3 j;
    j.x = sqrt(pow(maxYawAcceleration, 3.0) / (6.0*abs(deltaPhy.x)));
    j.y = sqrt(pow(maxPitchAcceleration, 3.0) / (6.0*abs(deltaPhy.y)));
    j.z = sqrt(pow(maxBankAcceleration, 3.0) / (6.0*abs(deltaPhy.z)));

    const double frac_1_3 = 1.0 / 3.0;
    const double frac_9_2 = 4.5;

    angularSpeedSetPoint.x = pow(frac_9_2*abs(deltaPhy.x)*j.x, frac_1_3);
    angularSpeedSetPoint.y = pow(frac_9_2*abs(deltaPhy.y)*j.y, frac_1_3);
    angularSpeedSetPoint.z = pow(frac_9_2*abs(deltaPhy.z)*j.z, frac_1_3);
    
    angularSpeedSetPoint.x *= i.x;
    angularSpeedSetPoint.y *= i.y;
    angularSpeedSetPoint.z *= i.z;

    yprSpeedSetPointOld = angularSpeedSetPoint;

    return angularSpeedSetPoint;
}

Code:
        bool setYPRSpeed(VECTOR3 YPRSpeedSetpoint, bool yaw, bool pitch, bool bank)
        {                            
            resetThruster();
            VECTOR3 v3_angularVel;
            v->GetAngularVel(v3_angularVel);  // X= Pitch  Y= Yaw  Z= Bank

            VECTOR3 deltaAngularVel;        

            deltaAngularVel.x = (YPRSpeedSetpoint.x - v3_angularVel.y);  // x and y for VESSEL->GetPMI() (used byGet MaxAngAcc) and VESSEL->GetAngularVel are swapped
            deltaAngularVel.y = (YPRSpeedSetpoint.y - v3_angularVel.x);
            deltaAngularVel.z = (YPRSpeedSetpoint.z - v3_angularVel.z);
            VECTOR3 deltaDeltaAngularVel = deltaAngularVel - deltaAngularVelOld;
            //VECTOR3 angularAccRequired = (deltaAngularVel + deltaDeltaAngularVel) ;
            VECTOR3 angularAccRequired = (deltaAngularVel + (deltaDeltaAngularVel * oapiGetSimStep()));
            
            VECTOR3 maxAngularAcc = ORBITERTOOLS::GetMaxAngAcc(v);  //Returns the maxumum angular acceleration in °/s²            
            //maxAngularAcc = maxAngularAcc * 0.5;            

            VECTOR3 thrust;
            thrust.x = ((angularAccRequired.x) / (maxAngularAcc.y));    //Calculating the "percentage thrust" for the thruster groups.
            thrust.y = ((angularAccRequired.y) / (maxAngularAcc.x));    // x and y for VESSEL->GetPMI() (used byGet MaxAngAcc) and VESSEL->GetAngularVel are swapped
            thrust.z = ((angularAccRequired.z) / (maxAngularAcc.z));
            
            //On a NAN exception, var == var is false.
            // However, this should not occur anymore...
            if (thrust.x != thrust.x || thrust.y != thrust.y || thrust.z != thrust.z) return false;

            if (yaw)
            {
                
                v->SetThrusterGroupLevel(THGROUP_ATT_YAWLEFT, thrust.x);
                v->SetThrusterGroupLevel(THGROUP_ATT_YAWRIGHT, -thrust.x);
            }
            if (pitch)
            {
                v->SetThrusterGroupLevel(THGROUP_ATT_PITCHUP, thrust.y);
                v->SetThrusterGroupLevel(THGROUP_ATT_PITCHDOWN, -thrust.y);
            }
            if (bank)
            {
                v->SetThrusterGroupLevel(THGROUP_ATT_BANKRIGHT, thrust.z);
                v->SetThrusterGroupLevel(THGROUP_ATT_BANKLEFT, -thrust.z);
            }
            deltaAngularVelOld = deltaAngularVel;
            return true;
            
        }

Code:
class quaternion
{
	public:
		double x, y, z, w;
		void yprToQuat(VECTOR3 ypr)
		{
			// Create quaternian from setpoints
			double c1 = cos((ypr.z - PI05)/2);
			double s1 = sin((ypr.z - PI05)/2);
			double c2 = cos(ypr.y/2);
			double s2 = sin(ypr.y/2);
			double c3 = cos(-ypr.x/2);
			double s3 = sin(-ypr.x/2);
			double c1c2 = c1*c2;
			double s1s2 = s1*s2;

			w =c1c2*c3 - s1s2*s3;
  			x = c1c2*s3 + s1s2*c3;
			y = s1*c2*c3 + c1*s2*s3;
			z = c1*s2*c3 - s1*c2*s3;
		}

		MATRIX3 getMatrix()
		{
			// create matrix from the set point quaternions
			double sqw = w*w;
			double sqx = x*x;
			double sqy = y*y;
			double sqz = z*z;

			// invers (inverse square length) is only required if quaternion is not already normalised
			double invs = 1 / (sqx + sqy + sqz + sqw);

			MATRIX3 mq;
			mq.m11 = ( sqx - sqy - sqz + sqw)*invs ; // since sqw + sqx + sqy + sqz =1/invs*invs
			mq.m22 = (-sqx + sqy - sqz + sqw)*invs ;
			mq.m33 = (-sqx - sqy + sqz + sqw)*invs ;
    
			double tmp1 = x*y;
			double tmp2 = z*w;
			mq.m21 = 2.0 * (tmp1 + tmp2)*invs ;
			mq.m12 = 2.0 * (tmp1 - tmp2)*invs ;
    
			tmp1 = x*z;
			tmp2 = y*w;
			mq.m31 = 2.0 * (tmp1 - tmp2)*invs ;
			mq.m13 = 2.0 * (tmp1 + tmp2)*invs ;
			tmp1 = y*z;
			tmp2 = x*w;
			mq.m32 = 2.0 * (tmp1 + tmp2)*invs ;
			mq.m23 = 2.0 * (tmp1 - tmp2)*invs ; 
			return mq;
		}

};

Code:
class ORBITERTOOLS
{
	public:
		
		//Normaly the right one...
		static MATRIX3 getLocalRotationMatrix(VESSEL* v)
		{
			OBJHANDLE hVessel = v->GetHandle();
			OBJHANDLE hMoon;// = oapiGetObjectByName("Moon");
			hMoon = v->GetGravityRef();
			MATRIX3 m3Vessel = getRotationMatrixFromHandle(hVessel);
			MATRIX3 m3Moon  = getRotationMatrixFromHandle(hMoon);
			
			MATRIX3 m3rel = mul(MYMATH::invertMatrix(m3Vessel), m3Moon);
			double ln,lt,r;
			oapiGetEquPos(hVessel,&ln,&lt,&r);	
		
			VECTOR3 ve = {0,1,0};
			m3rel = MYMATH::rotateMatrix(ve, m3rel, -ln);
			ve.x = 0;	ve.y = 0;	ve.z = 1;
			m3rel = MYMATH::rotateMatrix(ve, m3rel, lt);
			
				/*
			ve.x = 0;	ve.y = 1;	ve.z = 0;
			m3rel = MYMATH::rotateMatrix(ve, m3rel, - PI05);
			*/
			return m3rel;

		}
		//Works corect for autopilot.cpp
		static MATRIX3 getLocalRotationMatrix2(VESSEL* v)
		{
			OBJHANDLE hVessel = v->GetHandle();
			OBJHANDLE hMoon;// = oapiGetObjectByName("Moon");
			hMoon = v->GetGravityRef();
			MATRIX3 m3Vessel = getRotationMatrixFromHandle(hVessel);
			MATRIX3 m3Moon  = getRotationMatrixFromHandle(hMoon);
			VECTOR3 ve;
			ve.x = 0;	ve.y = 0;	ve.z = -1;
			
			m3Vessel = MYMATH::rotateMatrix(ve,m3Vessel,PI);
			ve.x = -1;	ve.y = 0;	ve.z = 0;
			m3Vessel = MYMATH::rotateMatrix(ve,m3Vessel,-PI05);
			MATRIX3 m3rel = mul(MYMATH::invertMatrix(m3Vessel), m3Moon);
			double ln,lt,r;
			oapiGetEquPos(hVessel,&ln,&lt,&r);	
			ve.x = 0;	ve.y = 1; ve.z = 0;
			m3rel = MYMATH::rotateMatrix(ve, m3rel, -ln);
			ve.x = 0;	ve.y = 0;	ve.z = 1;
			m3rel = MYMATH::rotateMatrix(ve, m3rel, lt);
			return m3rel;
		}
				
		static VECTOR3 getThrGroupDir(VESSEL *v,THGROUP_TYPE thgt)
		{
			VECTOR3 dirTotal = {0,0,0};
			for(unsigned int i=0; i<v->GetGroupThrusterCount(thgt); i++) 
			{						
				THRUSTER_HANDLE th=v->GetGroupThruster(thgt,i);			
				VECTOR3 dir;
				v->GetThrusterDir(th,dir);
				dirTotal = dirTotal + dir;
			}
			dirTotal = dirTotal / v->GetGroupThrusterCount(thgt);
			return dirTotal;			
		}


		static double GetMaxTorqueFromTHGroup( VESSEL * v, THGROUP_TYPE thGroupType)
		{
			double torqueSum = 0;
			for (unsigned i = 0; i < v->GetGroupThrusterCount(thGroupType); ++i) 
			{
				THRUSTER_HANDLE th = v->GetGroupThruster(thGroupType, i);
				VECTOR3 thPos;
				v->GetThrusterRef(th, thPos);
				double torque = v->GetThrusterMax(th) * length(thPos);
				torqueSum += torque;
			}
			return torqueSum;
		}

		static VECTOR3 GetMaxAngAcc(VESSEL * v )
		{
			VECTOR3 pmi;
			v->GetPMI(pmi);
			double pitchTorque = GetMaxTorqueFromTHGroup( v, THGROUP_ATT_PITCHUP );
			double yawTorque = GetMaxTorqueFromTHGroup( v, THGROUP_ATT_YAWLEFT );
			double bankTorque = GetMaxTorqueFromTHGroup( v, THGROUP_ATT_BANKLEFT );
    
			double mass = v->GetMass();
			double pitchAcc = pitchTorque / (mass * pmi.x);
			double yawAcc = yawTorque / (mass * pmi.y);
			double bankAcc = bankTorque / (mass * pmi.z);
    
			VECTOR3 ret;
			ret.x = pitchAcc;
			ret.y = yawAcc;
			ret.z = bankAcc;

			return ret;
		}
		static MATRIX3 getRotationMatrixFromHandle(OBJHANDLE object)
		{
			MATRIX3 m;
			oapiGetRotationMatrix(object,&m);
			return m;
		}

		static double getEngineVectorForward(VESSEL *v,THGROUP_TYPE thgt)
		{			
			VECTOR3 edirLocal =  getThrGroupDir(v, thgt);
			VECTOR3 edirGlobal = {0,0,0};
			v->HorizonRot(edirLocal,edirGlobal);
			return -edirGlobal.z;
		}


		static double getEngineVectorDownward(VESSEL *v,THGROUP_TYPE thgt)
		{
			
			VECTOR3 edirLocal =  getThrGroupDir(v, thgt);
			VECTOR3 edirGlobal = {0,0,0};
			v->HorizonRot(edirLocal,edirGlobal);
			return -edirGlobal.y;
		}
		/*
		static double getEngineVectorDownward(VESSEL *v,THRUSTER_HANDLE th)
		{
			VECTOR3 edirLocal;
			v->GetThrusterDir(th, edirLocal);
			VECTOR3 edirGlobal = {0,0,0};
			v->HorizonRot(edirLocal,edirGlobal);
			return -edirGlobal.y;
		}
		*/
		static double getFlightVectorHeading(VESSEL *v)
		{			
			VECTOR3 shipAirspeedVector;
			v->GetHorizonAirspeedVector(shipAirspeedVector);
			//normalise(shipAirspeedVector);
			double vector = atan2(shipAirspeedVector.x, shipAirspeedVector.z);
			return vector;
		}

		static double getEngineHeading(VESSEL *v, THGROUP_TYPE thGroupType)
		{
			VECTOR3 engineDir = getThrGroupDir(v, thGroupType);
			VECTOR3 horzDir;
			v->HorizonRot(engineDir,horzDir);
			horzDir.x = -horzDir.x;
			horzDir.z = -horzDir.z;
			horzDir.y = 0;
			normalise(horzDir);
			
			//return acos(horzDir.z/sqrt((horzDir.x*horzDir.x)+(horzDir.z*horzDir.z)));

			return atan2(horzDir.x, horzDir.z);
		}

		/*
			ISSUE: WORKS FOR HOVER ENGINE ONLY WITHOUT GIMBAL!
		*/

		static double getMaxHorizontalAcceleration(VESSEL *v, THGROUP_TYPE thGroupType, double bank, double level=1)
		{
			double maxThrust = 0;
			int count = v->GetGroupThrusterCount(thGroupType);
			for (int i = 0; i < count; i++)
			{
				THRUSTER_HANDLE th = v->GetGroupThruster(thGroupType,i);
				maxThrust += v->GetThrusterMax(th);
			}
			double maxa = (level*maxThrust) / v->GetMass();
			double engineAngle = getEngineHeading(v,thGroupType );			
			maxa = maxa * cos(bank);			
			return maxa ;
		}



		
		//*NEW
		/*
		static double getPitchForVerticalAcceleration(VESSEL *v, THGROUP_TYPE thGroupType, double bank, double setPoint, double level=0)
		{


			
			if (level == 0) level = v->GetThrusterGroupLevel(thGroupType);
			
			double maxa = getMaxHorizontalAcceleration(v,thGroupType,bank,level);			

			if (abs(maxa) ==0) return PI05;
			if (abs(setPoint) / abs(maxa) >= 1) return PI05;
			if (abs(setPoint) / abs(maxa) <= 0) return PI05;
			double r = acos((setPoint) / (maxa));	
			return r - PI05;
		}
		*/

		static double getPitchForHorizontalAcceleration(VESSEL *v, THGROUP_TYPE thGroupType, double bank, double setPoint, double level=0)
		{
			if (level == 0) level = v->GetThrusterGroupLevel(thGroupType);
			double maxa = getMaxHorizontalAcceleration(v,thGroupType,bank,level);						
			if (abs(maxa) ==0) return PI05;
			if (abs(setPoint) / abs(maxa) >= 1) return PI05;
			if (abs(setPoint) / abs(maxa) <= 0) return PI05;
			double r = acos((setPoint) / (maxa));	
			return r - PI05;
		}

		static bool checkForTouchDown(VESSEL *v)
		{
			//V = _VESSEL;
			VECTOR3 tp1,tp2,tp3; 
			
			double Alt = v->GetAltitude(AltitudeMode::ALTMODE_GROUND);
			//double Alt = v->GetAltitude();
			
			v->GetTouchdownPoints(tp1,tp2,tp3);
			const double ShutDownAlt = 0.25;
			Alt = Alt - ShutDownAlt;
			double td1 = Alt-fabs(tp1.y);
			double td2 = Alt-fabs(tp2.y);
			double td3 = Alt-fabs(tp3.y);
			if (td1 <= 0  || td2 <=0  || td3 <=0) return true;
			return false;
		}


		static void pointRadialDistance(double lat1, double lon1, double bearing, double distance, VESSEL * v, double *lat2, double *lon2)
		{
			double rdistance = distance / oapiGetSize(v->GetGravityRef());
			* lat2 = asin(sin(lat1) * cos(rdistance) + cos(lat1) * sin (rdistance) * cos(bearing) );
			* lon2 = lon1 + atan2( sin (bearing) * sin (rdistance) * cos (lat1), cos (rdistance) - sin(lat1) * sin (* lat2 ));
		}

		static double getElevationAtDistance(VESSEL* v, double distance)
		{
			double lng, lat, rad;
			v->GetEquPos(lng,lat,rad);
			double lng2, lat2;	
			double heading = 0;
			//heading = ORBITERTOOLS::getFlightVectorHeading(v);

			VECTOR3 shipAirspeedVector;
			v->GetHorizonAirspeedVector(shipAirspeedVector);
			heading = atan2(shipAirspeedVector.x, shipAirspeedVector.z);
			
			
			if (heading != heading) return 0;
			pointRadialDistance(lat,lng,heading,distance,v,&lat2,&lng2);
			
			if (lat2 != lat2) return 0;	
			if (lng2 != lng2) return 0;

			if (oapiIsVessel(v->GetHandle()))
			{
				OBJHANDLE gbHandle = v->GetGravityRef();
				return oapiSurfaceElevation(gbHandle, lng2, lat2);
			}
			else return 0;
		}

		static double getElevationAtDistance(VESSEL* v, double distanceX, double distanceZ)
		{
			double lng, lat, rad;
			v->GetEquPos(lng,lat,rad);
			double lng2, lat2;	
			double lng3, lat3;	
			double heading = 0;
			heading = ORBITERTOOLS::getFlightVectorHeading(v);		
			pointRadialDistance(lat,lng,heading,distanceZ,v,&lat2,&lng2);
			pointRadialDistance(lat2,lng2,heading+90,distanceX,v,&lat3,&lng3);


			if (lat3 != lat3) return 0;
			if (lng3 != lng3) return 0;

			if (oapiIsVessel(v->GetHandle()))
			{
				OBJHANDLE gbHandle = v->GetGravityRef();
				return oapiSurfaceElevation(v->GetGravityRef(), lng3, lat3);
			}
			else return 0;
			
		}
};
For some math this post may be important: http://orbiter-forum.com/showthread.php?t=36209
 
Last edited:

Thorsten

Active member
Joined
Dec 7, 2013
Messages
785
Reaction score
56
Points
43
I noticed that using the inline graphic client, there are always some little "thruster activities", i guess because the values for the allingment are never 100% corect after one timestep.

Without delving into the details - do you realize that it's perfectly normal to have residual attitude and rate errors, and that real spacecraft use deadbands to inhibit thruster firings unless the errors grow beyond a pre-defined threshold?

If I require very tight deadbands for my automatic Shuttle pointing routings, I get a constant flurry of RCS firings as a reward (because even tiny effects like the FES exhaust cause attitude drifts which require corrections) - if I define permissive deadbands, the activity is essentially suppressed.

In the real Shuttle, these things were configurable and dependent on the precision you needed in the mission phase (for an OMS burn, 10 deg attitude offset is usually acceptable for instance, for docking, you may want very small rate errors,...).

So (you may not like this answer) - conceptually I think the guidance logic (what's the current attitude target and what are acceptable errors) and the control logic (what thrusters to fire to achieve that) should be separate, and while guidance can be (to some degree) universal, control ought to be very craft-specific.

In other words, if you're sending good guidance targets to a craft that has a poor control logic, you get poor results.
 
Top