simple autopilot for iss sim spacex

gistfile1.txt

let a = setInterval(() => { 
      // retrieve values from simulation
      let roll = fixedRotationZ;
      let pitch = fixedRotationX;
      let yaw = fixedRotationY;
      let rpy = new THREE.Vector3(roll, pitch, yaw);
  
      let rollRate = -rateRotationZ/10.0;
      let pitchRate = -rateRotationX/10.0;
      let yawRate = -rateRotationY/10.0;
  
      let rpyRate = new THREE.Vector3(rollRate, pitchRate, yawRate);
  
      let pos = camera.position.clone();
      let posRate = motionVector.clone();
      let targetPos = issObject.position.clone();
  
      let targetRpy = new THREE.Vector3(0,0,0);
      let dRpy = targetRpy.clone().sub(rpy);
  
      // P-controller controls target roll,pitch,yaw-rate (targetRpyRate) to reach target orientation
      let targetRpyRate = new THREE.Vector3(
          dRpy.x/1.0, 
          dRpy.y/1.0, 
          dRpy.z/1.0
      );
  
      let minRpyRate = new THREE.Vector3(-1.5, -1.5, -1.5);
      let maxRpyRate = new THREE.Vector3(+1.5, +1.5, +1.5);
      targetRpyRate.clamp(minRpyRate, maxRpyRate)
  
      let dRpyRate = targetRpyRate.clone().sub(rpyRate);
   
      // don't calculate with displayed x,y,z but the internal coordinates 
      // (z-axis is forward and backward)
      let dPos = targetPos.clone().sub(pos);
      let d = dPos.length();
      // P-controller controls target motion (targetPosRate) to reach target position
      let targetPosRate = new THREE.Vector3(
          dPos.x / 100.0,
          dPos.y / 100.0,
          dPos.z / 100.0
      );
      let minPosRate = new THREE.Vector3(-0.1, -0.1, -0.01);
      let maxPosRate = new THREE.Vector3(+0.1, +0.1, +0.01);
  
      // thresholds for bang contRol
      let dRpyRateControlThreshold = new THREE.Vector3(0.0, 0.0, 0.0);
      let dPosRateControlThreshold = new THREE.Vector3(0.001, 0.001, 0.001);
  
      // define some phases where we want to have different movement behaviour
  
      if (dRpyRate.length() > 0.1)
      {
          // not correctly oriented, stop movement
          targetPosRate.x = 0;
          targetPosRate.y = 0;
          targetPosRate.z = 0;
      }
      else if (Math.abs(dPos.z) < 20)
      {
          // we are very close, slow approach, larger P-control in lateral
          // directions to correct remaining errors
          minPosRate.z = -0.005;
          maxPosRate.z = +0.005;
          dPosRateControlThreshold.x = 0.01;
          dPosRateControlThreshold.y = 0.01;
          targetPosRate.x = dPos.x;
          targetPosRate.y = dPos.y;
      }
      else if (Math.abs(dPos.z) < 50)
      {
          // we are getting, close, slow down
          minPosRate.z = -0.01;
          maxPosRate.z = +0.01;
  
          minPosRate.x = -0.01;
          minPosRate.y = -0.01;
          maxPosRate.x = +0.01;
          maxPosRate.y = +0.01;
      }
      else if (Math.abs(dPos.z) < 100)
      {
          // slow down
          minPosRate.z = -0.1;
          maxPosRate.z = +0.1;
      }
      else if ((Math.abs(dPos.x) < 0.05) && (Math.abs(dPos.y) < 0.05))
      {
          // lateral position correct, use maximum approaching speed
          minPosRate.z = -0.2;
          maxPosRate.z = +0.2;
      }
      targetPosRate.clamp(minPosRate, maxPosRate);
      let dPosRate = targetPosRate.clone().sub(posRate);
          
      // bang control to reach target roll pitch and yaw rate
      if (dRpyRate.x < -dRpyRateControlThreshold.x)
          rollRight();
      else if (dRpyRate.x > +dRpyRateControlThreshold.x)
          rollLeft();
      if (dRpyRate.y < -dRpyRateControlThreshold.y)
          pitchDown();
      else if (dRpyRate.y > +dRpyRateControlThreshold.y)
          pitchUp();
      if (dRpyRate.z < -dRpyRateControlThreshold.z)
          yawRight();
      else if (dRpyRate.z > +dRpyRateControlThreshold.z)
          yawLeft();
  
      // bang control to reach target motion
      if (dPosRate.x < -dPosRateControlThreshold.x)
          translateLeft();
      else if (dPosRate.x > +dPosRateControlThreshold.x)
          translateRight();
      if (dPosRate.y < -dPosRateControlThreshold.y)
          translateDown();
      else if (dPosRate.y > +dPosRateControlThreshold.y)
          translateUp();
      if (dPosRate.z < -dPosRateControlThreshold.z)
          translateForward();
      else if (dPosRate.z > +dPosRateControlThreshold.z)
          translateBackward();
  
  },100);