Hlynkacg
Aspiring rocket scientist
And continues to add thrusters a-la our DG loop conundrum
---------- Post added at 16:11 ---------- Previous post was at 16:08 ----------
Code:
bool AAPO::FCS::FindSolution ()
{
// Get desired torque and normalize it.
VECTOR3 tVector = DesiredTorque; // Direction of torque
double tMag = length (tVector); // Magnitude of torque
if (tMag > 0) normalise (tVector); // Normalise vector
// Do same for force.
VECTOR3 fVector = DesiredForce; // Direction of force
double fMag = length (fVector); // Magnitude of force
if (fMag > 0) normalise (fVector); // Normalise vector
if (tMag + fMag <= 0) return false; // Sanity Check: Is there a solution to find?
// Debuggery
sprintf (oapiDebugString(), "Desired Torque x %0.1f, y %0.1f, z %0.1f, Desired Force x %0.1f, y %0.1f, z %0.1f", DesiredTorque.x, DesiredTorque.y, DesiredTorque.z, DesiredForce.x, DesiredForce.y, DesiredForce.z);
// Initialize variables
VECTOR3 ResultantTorque = NULL_VECTOR;
VECTOR3 ResultantForce = NULL_VECTOR;
double tolerance = 0.1;
int iterations = 0;
int MaxIterations = thrusters.size ();
bool solution = false;
// Create a list of thrusters to be used
std::vector <std::pair <THRUSTER_DATA, double>> ThrusterList;
// Get to work
while ((solution == false) && (iterations < MaxIterations))
{
VECTOR3 TorqueRemaining = DesiredTorque - ResultantTorque;
VECTOR3 ForceRemaining = DesiredForce - ResultantTorque;
bool GoodTorque = (length (TorqueRemaining) < tolerance);
bool GoodForce = (length (ForceRemaining) < tolerance);
if ((GoodTorque) && (GoodForce)) solution = true;
else // we need to iterate
{
VECTOR3 tr = NULL_VECTOR;
VECTOR3 fr = NULL_VECTOR;
if (length (TorqueRemaining) > 0)
{
tr = TorqueRemaining;
normalise (tr);
}
if (length (ForceRemaining) > 0)
{
fr = ForceRemaining;
normalise (fr);
}
// find all thrusters that positively contribute to desired torque
int best = -1;
double highscore = 0;
std::vector<THRUSTER_DATA>::iterator i = thrusters.begin ();
for (int i = 0; i < thrusters.size(); i++)
{
double score = WeighThruster (thrusters[i], DesiredTorque, DesiredForce);
if (score > 0)
{
double lvl = score / length (thrusters[i].F);
ThrusterList.push_back (std::make_pair (thrusters[i], lvl));
}
i++;
}
if (best != -1)
// Debuggery
sprintf (oapiDebugString(), "%i Thrusters on list", ThrusterList.size ());
//sprintf (oapiDebugString(), "Desired Torque x %0.1f, y %0.1f, z %0.1f, Desired Force x %0.1f, y %0.1f, z %0.1f, %i Thrusters on list", DesiredTorque.x, DesiredTorque.y, DesiredTorque.z, DesiredForce.x, DesiredForce.y, DesiredForce.z, ThrusterList.size ());
VECTOR3 T = NULL_VECTOR;
VECTOR3 F = NULL_VECTOR;
// determine total torque and force of all thrusters on list..
for (int i = 0; i < ThrusterList.size(); i++)
{
//we don't care about pos, only the force/torque values
F += ThrusterList[i].first.F * ThrusterList[i].second;
T += ThrusterList[i].first.T * ThrusterList[i].second;
}
// Scale to magnitude of desired force and torque
double tMod = 1;
if (length (T) > tMag) tMod = tMag / length (T);
double fMod = 1;
if (length (F) > fMag) fMod = fMag / length (F);
double mod = (fMag > tMag ? fMod : tMod);
// Update list and resultants
for (int i = 0; i < ThrusterList.size(); i++)
{
ThrusterList[i].second *= mod;
ResultantTorque += ThrusterList[i].first.T * ThrusterList[i].second;
ResultantForce += ThrusterList[i].first.F * ThrusterList[i].second;
}
// Debuggery
sprintf (oapiDebugString(), "Desired Torque x %0.1f, y %0.1f, z %0.1f, Desired Force x %0.1f, y %0.1f, z %0.1f, Torque x %0.1f, y %0.1f, z %0.1f, Force x %0.1f, y %0.1f, z %0.1f, %i Thrusters on list", DesiredTorque.x, DesiredTorque.y, DesiredTorque.z, DesiredForce.x, DesiredForce.y, DesiredForce.z, T.x, T.y, T.z, F.x, F.y, F.z, ThrusterList.size ());
} // End iteration back to the top;
iterations++;
// Debuggery
sprintf (oapiDebugString(), "%i iterations", iterations);
} // End "while ((solution == false) && (iterations < MaxIterations))"
// Note: neet to make some sort of scaling scheme to prevent excessivly high torques or forces from breaking the solver
// Use "ThrusterList" to generate firing commands
//if ((solution) && (ThrusterList.size() > 0))
if (ThrusterList.size() > 0)
{
//double simdt = oapiGetSimStep ();
for (int i = 0; i < ThrusterList.size(); i++)
{
double lvl = ThrusterList[i].second;
FCS::FireThruster (ThrusterList[i].first.th, lvl);
}
return true;
}
else return false;
}Ok most recent code iteration.
Loop issue fixed but now yaw axis is non responsive and inverted control inputs are back.
Pretty sure it's just a swapped sign somewhere but not sure where.
air <THRUSTER_DATA, double>>" ThrusterLists and the magnitude of force or torque that it produces, ".mag". 
