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 !