- 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).
For some math this post may be important: http://orbiter-forum.com/showthread.php?t=36209
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,<,&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,<,&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;
}
};
Last edited: