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; } }