template<typename FixedType> std::string toStringImpl(FixedType f){
uint32_t N = sizeof(f.words)/sizeof(uint32_t);
uint32_t numOfFractionDigits = ((N-1) * 32)/3.32; // 3.32 = log(10)/log(2)
std::string s = (f.sign? "-" : "") + std::to_string(f.words[0]);
f.words[0] = 0;
FixedType ten{
10,0
};
std::stringstream str;
//fractional part
int nonZeroIndex = 0;
for(uint32_t i = 0; i < numOfFractionDigits; i++){
f = tmul(f,ten);
char newDigit = f.words[0] % 10 + '0';
str << newDigit;
if(newDigit != '0'){
nonZeroIndex = i;
}
f.words[0] = 0;
}
return s + '.' + str.str().substr(0,nonZeroIndex+1);
}
bool AttitudeReference::GetTgtEulerAngles (VECTOR3 &tgt_euler) const
{
if (!valid_tgteuler) {
switch (tgtmode) {
case 1: // fixed
tgteuler.y = tgteuler.z = 0.0;
tgteuler += tgt_offs;
have_tgteuler = true;
break;
case 2: // direction of NAV source
case 3: { // relative velocity of NAV source
NAVHANDLE hNav;
if (mode >= 4 && (hNav = v->GetNavSource (navid))) {
VECTOR3 dir, sdir;
if (tgtmode == 2) {
oapiGetNavPos (hNav, &dir);
v->GetGlobalPos (sdir);
dir = tmul (GetFrameRotMatrix(), unit (dir-sdir));
} else {
v->GetGlobalVel (sdir);
dir = tmul (GetFrameRotMatrix(), unit (tgt_rvel));
}
if (projmode == 0) {
tgteuler.y = asin (dir.y);
tgteuler.z = atan2 (dir.x, dir.z);
} else {
tgteuler.y = atan2 (dir.y, dir.z);
tgteuler.z = asin (dir.x);
}
tgteuler += tgt_offs;
//tgteuler += GetEulerAngles (projection_mode);
// frame-relative --> ship-relative; is this a good idea?
tgteuler.y = posangle(tgteuler.y);
tgteuler.z = posangle(tgteuler.z);
have_tgteuler = true;
} else
have_tgteuler = false;
} break;
default:
have_tgteuler = false;
break;
}
}
valid_tgteuler = true;
tgt_euler = tgteuler;
return have_tgteuler;;
}
template<typename FixedType> void fromStringImpl(std::string s,FixedType& f){
int N = sizeof(f.words)/sizeof(uint32_t);
for(int i = 0; i < N; i++){
f.words[i] = 0;
}
bool sign = s[0] == '-';
if(s[0] == '-' || s[0] == '+'){
s = s.substr(1);
}
auto pointPos = s.find('.');
// ____
//approximate 0.1 in FixedType, its 0.00011.
FixedType invertedBase{};
invertedBase.sign = 0;
invertedBase.words[0] = 0;
invertedBase.words[1] = 0b00011001100110011001100110011001;
for(int i = 2; i < N-1; i++){
invertedBase.words[i] = 0b10011001100110011001100110011001;
}
invertedBase.words[N-1] = 0b10011001100110011001100110011010; //<-- round up,
//find fractional part
if(pointPos != s.npos){
for(uint32_t i = s.size()-1; i > pointPos; i--){
char c = s.at(i);
if(isdigit(c)){
f.words[0] += c - '0';
f = tmul(f,invertedBase);
}else if(c == ' '){
continue; //just ignore
}else{
throw std::runtime_error("number format wrong");
}
}
}
//find integer part
uint32_t intPart = 0;
pointPos = pointPos == s.npos ? s.size() : pointPos;
for(uint32_t i = 0; i < pointPos; i++){
char c = s.at(i);
if(isdigit(c)){
intPart *= 10;
intPart += c - '0';
}else if(c == ' '){
continue; //just ignore
}else{
throw std::runtime_error("number format wrong");
}
}
f.words[0] = intPart;
f.sign = sign;
}
double AscentAP::CalcTargetAzimuth () const
{
if (!vessel->status) return launch_azimuth;
VECTOR3 pos, equ, ep, dir, hdir;
MATRIX3 pR, vR;
const OBJHANDLE hRef = vessel->GetGravityRef();
oapiGetRotationMatrix (hRef, &pR);
vessel->GetGlobalPos(pos);
oapiGlobalToLocal (hRef, &pos, &equ); // vessel position in planet frame
normalise(equ);
ep = tmul(tgt.R,equ); // rotate to equator plane
double elng = atan2(ep.z, ep.x); // longitude of rotated position
dir = _V(-sin(elng),0,cos(elng)); // rotated target direction
dir = mul(tgt.R,dir); // target direction in planet frame
dir = mul(pR, dir); // target direction in global frame
vessel->GetRotationMatrix (vR);
dir = tmul (vR, dir); // target direction in vessel frame
vessel->HorizonRot (dir, hdir); // target direction in local horizon frame
double az = atan2 (hdir.x,hdir.z); // target azimuth
return az;
}
static Triangles tvgen(const Obj obj)
{
const int scale = vmaxlen(obj.vsv);
Triangles tv = tsnew(obj.fs.count);
for(int i = 0; i < obj.fs.count; i++)
{
const Triangle t = {
obj.vsv.vertex[obj.fs.face[i].va],
obj.vsv.vertex[obj.fs.face[i].vb],
obj.vsv.vertex[obj.fs.face[i].vc],
};
tv.triangle[i] = tmul(t, 1.0f / scale);
}
return tv;
}
const VECTOR3 &AttitudeReference::GetEulerAngles () const
{
if (!valid_euler) {
// Update the axes of the reference frame
const MATRIX3 &Rref = GetFrameRotMatrix();
// Rotation matrix ship->global
MATRIX3 srot;
v->GetRotationMatrix (srot);
// map ship's local axes into reference frame
VECTOR3 shipx = {srot.m11, srot.m21, srot.m31};
VECTOR3 shipy = {srot.m12, srot.m22, srot.m32};
VECTOR3 shipz = {srot.m13, srot.m23, srot.m33};
shipx = tmul (Rref, shipx);
shipy = tmul (Rref, shipy);
shipz = tmul (Rref, shipz);
if (projmode == 0) {
euler.x = atan2(shipx.y, shipy.y); // roll angle
euler.y = asin(shipz.y); // pitch angle
euler.z = atan2(shipz.x, shipz.z); // yaw angle
} else {
euler.x = -atan2(shipy.x, shipx.x); // roll angle
euler.y = atan2(shipz.y,shipz.z); // pitch angle
euler.z = asin(shipz.x); // yaw angle
}
euler += euler_offs;
for (int i = 0; i < 3; i++)
euler.data[i] = posangle (euler.data[i]);
valid_euler = true;
}
return euler;
}
void LVIMU::Timestep(double simt)
{
double deltaTime, pulses;
if (!Operate) {
if (IsPowered())
TurnOn();
else
return;
}
else if (!IsPowered()) {
TurnOff();
return;
}
//
// If we get here, we're powered up.
//
if (!TurnedOn) {
return;
}
// fill OrbiterData
VESSELSTATUS vs;
OurVessel->GetStatus(vs);
Orbiter.Attitude.X = vs.arot.x;
Orbiter.Attitude.Y = vs.arot.y;
Orbiter.Attitude.Z = vs.arot.z;
// Vessel to Orbiter global transformation
MATRIX3 tinv = getRotationMatrixZ(-vs.arot.z);
tinv = mul(getRotationMatrixY(-vs.arot.y), tinv);
tinv = mul(getRotationMatrixX(-vs.arot.x), tinv);
if (!Initialized) {
SetOrbiterAttitudeReference();
// Get current weight vector in vessel coordinates
VECTOR3 w;
OurVessel->GetWeightVector(w);
// Transform to Orbiter global and calculate weight acceleration
w = mul(tinv, w) / OurVessel->GetMass();
LastWeightAcceleration = w;
OurVessel->GetGlobalVel(LastGlobalVel);
LastTime = simt;
Initialized = true;
}
else {
deltaTime = (simt - LastTime);
// Calculate accelerations
VECTOR3 w, vel;
OurVessel->GetWeightVector(w);
// Transform to Orbiter global and calculate accelerations
w = mul(tinv, w) / OurVessel->GetMass();
OurVessel->GetGlobalVel(vel);
VECTOR3 dvel = (vel - LastGlobalVel) / deltaTime;
// Measurements with the 2006-P1 version showed that the average of the weight
// vector of this and the last step match the force vector while in free fall
// The force vector matches the global velocity change of the last timestep exactly,
// but isn't used because GetForceVector isn't working while docked
VECTOR3 dw1 = w - dvel;
VECTOR3 dw2 = LastWeightAcceleration - dvel;
VECTOR3 accel = -(dw1 + dw2) / 2.0;
LastWeightAcceleration = w;
LastGlobalVel = vel;
// orbiter earth rotation
// imuState->Orbiter.Y = imuState->Orbiter.Y + (deltaTime * TwoPI / 86164.09);
// sprintf(oapiDebugString(), "accel x %.10f y %.10f z %.10f DT %f", accel.x, accel.y, accel.z, deltaTime);
// Process channel bits
if (ZeroIMUCDUFlag) {
ZeroIMUCDUs();
}
else if (CoarseAlignEnableFlag) {
SetOrbiterAttitudeReference();
}
else if (Caged) {
SetOrbiterAttitudeReference();
}
else {
// Gimbals
MATRIX3 t = Orbiter.AttitudeReference;
t = mul(getRotationMatrixX(Orbiter.Attitude.X), t);
t = mul(getRotationMatrixY(Orbiter.Attitude.Y), t);
t = mul(getRotationMatrixZ(Orbiter.Attitude.Z), t);
t = mul(getOrbiterLocalToNavigationBaseTransformation(), t);
// calculate the new gimbal angles
VECTOR3 newAngles = getRotationAnglesXZY(t);
// drive gimbals to new angles
// CAUTION: gimbal angles are left-handed
DriveGimbalX(-newAngles.x - Gimbal.X);
DriveGimbalY(-newAngles.y - Gimbal.Y);
DriveGimbalZ(-newAngles.z - Gimbal.Z);
// PIPAs
accel = tmul(Orbiter.AttitudeReference, accel);
// sprintf(oapiDebugString(), "accel x %.10f y %.10f z %.10f DT %f", accel.x, accel.y, accel.z, deltaTime);
// pulse PIPAs
//pulses = RemainingPIPA.X + (accel.x * deltaTime / 0.0585);
//PulsePIPA(LVRegPIPAX, (int) pulses);
//RemainingPIPA.X = pulses - (int) pulses;
//pulses = RemainingPIPA.Y + (accel.y * deltaTime / 0.0585);
//PulsePIPA(LVRegPIPAY, (int) pulses);
//RemainingPIPA.Y = pulses - (int) pulses;
//pulses = RemainingPIPA.Z + (accel.z * deltaTime / 0.0585);
//PulsePIPA(LVRegPIPAZ, (int) pulses);
//RemainingPIPA.Z = pulses - (int) pulses;
pulses = (accel.x * deltaTime);
PulsePIPA(LVRegPIPAX, pulses);
pulses = (accel.y * deltaTime);
PulsePIPA(LVRegPIPAY, pulses);
pulses = (accel.z * deltaTime);
PulsePIPA(LVRegPIPAZ, pulses);
}
LastTime = simt;
}
}
