- Joined
- Jan 13, 2008
- Messages
- 2,302
- Reaction score
- 6
- Points
- 38
- Location
- Atlanta, GA, USA, North America
I'm a little stuck working on the autopilot for my Prometheus. I've made a little more progress in a combined vector system to work on all three axes at once. Here's the code I'm working with so far:
Right now I'm only focusing on the pitch axis, I'll add roll and yaw once I get this working. But basically I'm looking for the angle between the vector I want the launcher to point at (defined by the rotx and roty matrices) and the vector my vessel is currently at. I use both a dir and a rot vector since just a single vector would leave at least one axis (roll) undefined. The problem might be how I'm getting the angle, and thus the error, between the two vectors. Pitch takes place within the Y-Z plane so that's what I'm doing there after the dPitchError line. I read online that one way to get the angle between vectors in a plane is to take the dot product (y^2+z^2) then the arccosine of that. That somewhat seems to work, but as my vehicle pitches over, the error only grows. What am I missing here? Is there a better way entirely?
Thanks!
Code:
VECTOR3 attitude = _V(0, 0, 0);
double x = 70*RAD; //pitch
double y = 90*RAD; //heading
double z = attitude.z;
MATRIX3 rotxmatrix = _M(1, 0, 0,
0, cos(x), sin(x),
0, -sin(x), cos(x));
MATRIX3 rotymatrix = _M(cos(y), 0, sin(y),
0, 1, 0,
-sin(y), 0, cos(y));
VECTOR3 tgtDirVector = mul(rotxmatrix, _V(0, 0, 1));
tgtDirVector = mul(rotymatrix, _V(tgtDirVector.x, tgtDirVector.y, tgtDirVector.z));
VECTOR3 tgtRotVector = mul(rotxmatrix, _V(0, 1, 0));
tgtRotVector = mul(rotymatrix, _V(tgtRotVector.x, tgtRotVector.y, tgtRotVector.z));
VECTOR3 locTgtDirVector;
VECTOR3 locTgtRotVector;
HorizonInvRot(tgtDirVector, locTgtDirVector);
HorizonInvRot(tgtRotVector, locTgtRotVector);
//This is the core of the problem I think here
double dPitchError = 0.0;
VECTOR3 vPitch = _V(0, 0, 1);
dPitchError = acos((vPitch.y * locTgtDirVector.y) + (vPitch.z * locTgtDirVector.z));
//This is the PID loop, the PID itself works fine. This particular one just takes a current error value (in degrees for now)
double pitchval = pids[0]->updateError(dPitchError*DEG, time, true);
this->SetAttitudeRotLevel(0, pitchval);
Thanks!