How to prevent a programmed drone to fully rotate when no more throttle?

Viewed 63

I'm trying to simulate drone in JavaScript and facing a problem with the LEVEL fly mode : When for example Throttle only is fully applied up and back to 0 suddenly, the drone rotate up side down until the ending position, same rotation when throttle down is applied from 0.

Here is a part of the code called every update (every frame) by the drone object:

  update (currentState, stickLeftX, stickLeftY, stickRightX, stickRightY) {
    const inputYaw = -stickLeftX
    const inputZ = stickLeftY // Pos. Up, Neg. Down
    const inputY = stickRightY // Pos. Forward, Neg. Backward
    const inputX = stickRightX // Pos. Right, Neg. Left

    const dT = natives.getFrameTime()

    this.updateEndState(inputX, inputY, inputZ, inputYaw, dT)

    this.posPID.update(currentState.pos, this._endPos, dT)
    const output = this.posPID.getOutput()

    let norm = Math.hypot(...output.toArray())

    output.divideScalar(norm) // normalize

    const trans1 = new THREE.Vector3(output.x, output.y, output.z)
    const trans2 = trans1.clone() // new THREE.Vector3(output.x, output.y, output.z)
    trans1.applyQuaternion(this._endRot.clone().conjugate())
    trans2.applyQuaternion(currentState.rot.clone().conjugate())
    const q1 = new THREE.Quaternion()
    const q2 = new THREE.Quaternion()
    const closeEnd = this._endRot.clone().multiply(q1.setFromUnitVectors(this.up.normalize(), trans1.normalize()))
    const closeCurr = currentState.rot.clone().multiply(q2.setFromUnitVectors(this.up.normalize(), trans2.normalize()))

    this._calcRot = closeCurr.slerp(closeEnd, 0.4) // Slowly go to closest idle quat

    const tr = this.up.clone().applyQuaternion(currentState.rot)
    const alignment = output.dot(tr)

    if (norm > 1.0) norm = 1.0

    this._calcThrottle = norm * alignment

    if (!Settings.drone3DFly && this._calcThrottle < 0.0) this._calcThrottle = 0.0
  }

  updateEndState (inputX, inputY, inputZ, inputYaw, dT) {

    const inVect = new THREE.Vector3(inputX, inputY, inputZ)

    const relDeltaPos = inVect.multiplyScalar(dT).multiplyScalar(Settings.droneMaxVel).multiplyScalar(0.8)

    const q = new THREE.Quaternion()
    this._endRot.multiply(q.setFromAxisAngle(this.up, degToRad(180.0) * inputYaw * dT))

    const tr2 = relDeltaPos.applyQuaternion(this._endRot)
    this._endPos.add(tr2)
  }

How to prevent this rotation ?

I have tried to Math.abs() trans1.z and trans2.z also to not conjugate() _endpos and currentState.rot when trans1.z and trans2.z are negative.

With this :

let dir = this.up
    if (output.z < 0) {
      dir = this.down
    }
    const closeEnd = this._endRot.clone().multiply(q1.setFromUnitVectors(dir.normalize(), trans1.normalize())) // Closest quat to end
    const closeCurr = currentState.rot.clone().multiply(q2.setFromUnitVectors(dir.normalize(), trans2.normalize())) // Closest quat to current

it's correct the problem BUT of course when the rotation would occur front/rear/left/right are reversed !

0 Answers
Related