// File: DavidLBruce[P]sim3dof/build/../src/FcTseO.cpp
// Function: FcTseO::FrameUpdate()
void FcTseO::FrameUpdate()
{
  double angSig;
  double measAng;
  double measRng;
  double rngSig;

  simTime = GetSimTime();

  measAng = pRdr->measAng;
  angSig = pRdr->rinAng; 

  measRng = pRdr->measRng;
  rngSig = pRdr->rinRng;

  measPosY = measRng * measAng;
  rinPosY = measRng * angSig + measAng * rngSig; 

  if ( !initialized )
  {
    initialized = true;

    stateVec(0) = initPosY;
    stateVec(1) = initVelY;
    stateVec(2) = initOmega;

    PP[0][0] = SQ( initPosYsigScale * rinPosY );
    PP[1][1] = SQ( initVelYsig );
    PP[2][2] = SQ( initWsig );
  }

  PropStates( frameTimeStep );

  UpdateStates( measPosY );

  estPosY = stateVec(0);
  estPosYdot = stateVec(1);
  estOmega2 = stateVec(2);
  estOmega = sqrt( estOmega2 );

}

// Assembly:
pushq %rbp
movq %rsp, %rbp
subq $0x60, %rsp
movq %rdi, -0x8(%rbp)
movq -0x8(%rbp), %rdi
movq %rdi, -0x30(%rbp)
callq 0x3c40
movq -0x30(%rbp), %rax
movsd %xmm0, 0x240(%rax)
movq 0x268(%rax), %rcx
movsd 0x128(%rcx), %xmm0
movsd %xmm0, -0x18(%rbp)
movq 0x268(%rax), %rcx
movsd 0x110(%rcx), %xmm0
movsd %xmm0, -0x10(%rbp)
movq 0x268(%rax), %rcx
movsd 0x138(%rcx), %xmm0
movsd %xmm0, -0x20(%rbp)
movq 0x268(%rax), %rcx
movsd 0x120(%rcx), %xmm0
movsd %xmm0, -0x28(%rbp)
movsd -0x20(%rbp), %xmm0
mulsd -0x18(%rbp), %xmm0
movsd %xmm0, 0x1d8(%rax)
movsd -0x20(%rbp), %xmm0
movsd -0x10(%rbp), %xmm2
movsd -0x18(%rbp), %xmm1
mulsd -0x28(%rbp), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x218(%rax)
testb $0x1, 0x1a0(%rax)
jne 0x4db9
movq -0x30(%rbp), %rdi
movb $0x1, 0x1a0(%rdi)
movsd 0x108(%rdi), %xmm0
movsd %xmm0, -0x60(%rbp)
addq $0x248, %rdi            # imm = 0x248
xorl %esi, %esi
callq 0x1fd40
movsd -0x60(%rbp), %xmm0
movq -0x30(%rbp), %rdi
movsd %xmm0, (%rax)
movsd 0x118(%rdi), %xmm0
movsd %xmm0, -0x58(%rbp)
addq $0x248, %rdi            # imm = 0x248
movl $0x1, %esi
callq 0x1fd40
movsd -0x58(%rbp), %xmm0
movq -0x30(%rbp), %rdi
movsd %xmm0, (%rax)
movsd 0x100(%rdi), %xmm0
movsd %xmm0, -0x50(%rbp)
addq $0x248, %rdi            # imm = 0x248
movl $0x2, %esi
callq 0x1fd40
movsd -0x50(%rbp), %xmm0
movq %rax, %rcx
movq -0x30(%rbp), %rax
movsd %xmm0, (%rcx)
movsd 0x110(%rax), %xmm0
mulsd 0x218(%rax), %xmm0
callq 0x4450
movq -0x30(%rbp), %rdi
movsd %xmm0, -0x48(%rbp)
addq $0x1f8, %rdi            # imm = 0x1F8
xorl %esi, %esi
callq 0x1fe60
movsd -0x48(%rbp), %xmm0
movq %rax, %rcx
movq -0x30(%rbp), %rax
movsd %xmm0, (%rcx)
movsd 0x120(%rax), %xmm0
callq 0x4450
movq -0x30(%rbp), %rdi
movsd %xmm0, -0x40(%rbp)
addq $0x1f8, %rdi            # imm = 0x1F8
movl $0x1, %esi
callq 0x1fe60
movsd -0x40(%rbp), %xmm0
movq %rax, %rcx
movq -0x30(%rbp), %rax
movsd %xmm0, 0x8(%rcx)
movsd 0x128(%rax), %xmm0
callq 0x4450
movq -0x30(%rbp), %rdi
movsd %xmm0, -0x38(%rbp)
addq $0x1f8, %rdi            # imm = 0x1F8
movl $0x2, %esi
callq 0x1fe60
movsd -0x38(%rbp), %xmm0
movsd %xmm0, 0x10(%rax)
movq -0x30(%rbp), %rdi
movsd 0x8(%rdi), %xmm0
callq 0x4e60
movq -0x30(%rbp), %rdi
movsd 0x1d8(%rdi), %xmm0
callq 0x5510
movq -0x30(%rbp), %rdi
addq $0x248, %rdi            # imm = 0x248
xorl %esi, %esi
callq 0x1fd40
movq -0x30(%rbp), %rdi
movsd (%rax), %xmm0
movsd %xmm0, 0x138(%rdi)
addq $0x248, %rdi            # imm = 0x248
movl $0x1, %esi
callq 0x1fd40
movq -0x30(%rbp), %rdi
movsd (%rax), %xmm0
movsd %xmm0, 0x140(%rdi)
addq $0x248, %rdi            # imm = 0x248
movl $0x2, %esi
callq 0x1fd40
movq %rax, %rcx
movq -0x30(%rbp), %rax
movsd (%rcx), %xmm0
movsd %xmm0, 0x150(%rax)
movsd 0x150(%rax), %xmm0
callq 0x3190
movq -0x30(%rbp), %rax
movsd %xmm0, 0x148(%rax)
addq $0x60, %rsp
popq %rbp
retq
nop