Stuck decompiling ARM64 function

Question

I am trying to figure out how this function squashes quaternion (double[4], dvar5..6 in the disassembly) data into the 2 int outputs vertexX and vertexY. As far as I can tell, it reads the raw sensor data into quat as 4 longs, then converts those to doubles, which makes sense for a Quaternion. What it does to then pack that into vertexX and vertexY I am stuck on.

/* angle_reader.c:3198
   DWARF DIE: 36e4 */

int XfmtReadAngle(int *vertexX,int *vertexY)

{
  int iVar1;
  undefined4 uVar2;
  undefined4 uVar3;
  undefined4 uVar4;
  undefined4 extraout_r1;
  undefined4 extraout_r1_00;
  undefined4 extraout_r1_01;
  double dVar5;
  double dVar6;
  double dVar7;
  double dVar8;
  uchar *more_00;
  undefined4 in_stack_ffffff7c;
  long quat [4];
  uchar more;
  short sensors;
  short accel [3];
  short gyro [3];
  double yaw;
  double roll;
  double pitch;
  double q3;
  double q2;
  double q1;
  double q0;
  double q30;
  int i;
  
  i = 0;
  do {
    if (9 < i) {
      return -1;
    }
    more_00 = &more;
    iVar1 = dmp_read_fifo(gyro,accel,quat,&sensors,more_00);
    if (-1 < iVar1) {
      if ((sensors & 0x100U) != 0) {
        dVar5 = (double)(longlong)quat[0] / 1073741824.0;
        dVar6 = (double)(longlong)quat[1] / 1073741824.0;
        dVar7 = (double)(longlong)quat[2] / 1073741824.0;
        dVar8 = (double)(longlong)quat[3] / 1073741824.0;
        uVar2 = SUB84(dVar6 * -2.0 * dVar8 + (dVar5 + dVar5) * dVar7,0);
        asin((double)CONCAT44(in_stack_ffffff7c,more_00));
        uVar3 = SUB84((dVar7 + dVar7) * dVar8 + (dVar5 + dVar5) * dVar6,0);
        atan2((double)CONCAT44(in_stack_ffffff7c,more_00),(double)CONCAT44(vertexX,vertexY));
        dVar5 = dVar6 * dVar7 + dVar5 * dVar8;
        uVar4 = SUB84(dVar5 + dVar5,0);
        atan2((double)CONCAT44(in_stack_ffffff7c,more_00),(double)CONCAT44(vertexX,vertexY));
        dVar5 = (double)CONCAT44(extraout_r1,uVar2) * 100000000.0 +
                (double)CONCAT44(extraout_r1_01,uVar4) * 1e-09;
        *vertexX = (uint)(0.0 < dVar5) * (int)(longlong)dVar5;
        dVar5 = (double)CONCAT44(extraout_r1_00,uVar3) * 100000000.0;
        *vertexY = (uint)(0.0 < dVar5) * (int)(longlong)dVar5;
        return 0;
      }
      fprintf((FILE *)g_xfmtDebug,"%s:%d: INV_WXYZ_QUAT error\n","XfmtReadAngle",0xc9f);
    }
    delay_ms(5);
    i = i + 1;
  } while( true );
}

                             **************************************************************
                             * angle_reader.c:3198                                        *
                             * DWARF DIE: 36e4                                            *
                             **************************************************************
                             int __stdcall XfmtReadAngle(int * vertexX, int * vertexY)
             int               r0:4           <RETURN>                                XREF[1]:     0001c438(W)  
             int *             r0:4           vertexX                                 XREF[1]:     0001c438(W)  
             int *             r1:4           vertexY
             undefined4        r0:4           uVar2                                   XREF[1]:     0001c438(W)  
             undefined4        Stack[-0x4]:4  local_4                                 XREF[1]:     0001c33c(W)  
             undefined4        Stack[-0x8]:4  local_8                                 XREF[2]:     0001c338(W), 
                                                                                                   0001c5b0(R)  
             int               Stack[-0xc]:4  i                                       XREF[4]:     0001c364(W), 
                                                                                                   0001c58c(R), 
                                                                                                   0001c594(W), 
                                                                                                   0001c598(R)  
             double            Stack[-0x18]:8 q30                                     XREF[1]:     0001c35c(W)  
             double            Stack[-0x20]:8 q0
             double            Stack[-0x28]:8 q1
             double            Stack[-0x30]:8 q2
             double            Stack[-0x38]:8 q3
             double            Stack[-0x40]:8 pitch                                   XREF[1]:     0001c43c(W)  
             double            Stack[-0x48]:8 roll                                    XREF[1]:     0001c4a0(W)  
             double            Stack[-0x50]:8 yaw                                     XREF[1]:     0001c50c(W)  
             short[3]          Stack[-0x58]:6 gyro
             short[3]          Stack[-0x60]:6 accel
             short             Stack[-0x62]:2 sensors                                 XREF[1]:     0001c398(R)  
             uchar             Stack[-0x63]:1 more
             long[4]           Stack[-0x74]   quat                                    XREF[1,3]:   0001c3ac(R), 
                                                                                                   0001c3c4(R), 
                                                                                                   0001c3dc(R), 
                                                                                                   0001c3f4(R)  
             undefined4        Stack[-0x7c]:4 local_7c                                XREF[2]:     0001c348(W), 
                                                                                                   0001c534(R)  
             undefined4        Stack[-0x80]:4 local_80                                XREF[2]:     0001c34c(W), 
                                                                                                   0001c550(R)  
             undefined4        Stack[-0x88]:4 local_88                                XREF[1]:     0001c380(W)  
                             XfmtReadAngle                                   XREF[4]:     Entry Point(*), 
                                                                                          XfmtCapture:00021fd0(c), 
                                                                                          XfmtHandleControl:0002460c(c), 
                                                                                          .debug_frame::00000cf4(*)  
        0001c338 08 b0 2d e5     str        r11,[sp,#local_8]!
        0001c33c 04 e0 8d e5     str        lr,[sp,#local_4]
        0001c340 04 b0 8d e2     add        r11,sp,#0x4
        0001c344 80 d0 4d e2     sub        sp,sp,#0x80
        0001c348 78 00 0b e5     str        vertexX,[r11,#local_7c]
        0001c34c 7c 10 0b e5     str        vertexY,[r11,#local_80]
        0001c350 00 20 a0 e3     mov        r2,#0x0
        0001c354 00 30 a0 e3     mov        r3,#0x0
        0001c358 d0 31 44 e3     movt       r3,#0x41d0
        0001c35c f4 21 4b e1     strd       r2,r3,[r11,#q30]
        0001c360 00 30 a0 e3     mov        r3,#0x0
        0001c364 08 30 0b e5     str        r3,[r11,#i]
        0001c368 8a 00 00 ea     b          LAB_0001c598
                             LAB_0001c36c                                    XREF[1]:     0001c5a0(j)  
        0001c36c 5e c0 4b e2     sub        r12,r11,#0x5e
        0001c370 70 20 4b e2     sub        r2,r11,#0x70
        0001c374 5c 10 4b e2     sub        vertexY,r11,#0x5c
        0001c378 54 00 4b e2     sub        vertexX,r11,#0x54
        0001c37c 5f 30 4b e2     sub        r3,r11,#0x5f
        0001c380 00 30 8d e5     str        r3,[sp,#0x0]=>local_88
        0001c384 0c 30 a0 e1     cpy        r3,r12
        0001c388 77 fd ff eb     bl         dmp_read_fifo                                    int dmp_read_fifo(short * gyro, 
        0001c38c 00 30 a0 e1     cpy        r3,vertexX
        0001c390 00 00 53 e3     cmp        r3,#0x0
        0001c394 7a 00 00 ba     blt        LAB_0001c584
        0001c398 be 35 5b e1     ldrh       r3,[r11,#sensors]
        0001c39c 73 30 ff e6     uxth       r3,r3
        0001c3a0 01 3c 03 e2     and        r3,r3,#0x100
        0001c3a4 00 00 53 e3     cmp        r3,#0x0
        0001c3a8 6c 00 00 0a     beq        LAB_0001c560
        0001c3ac 70 30 1b e5     ldr        r3,[r11,#quat[0]]
        0001c3b0 90 3a 07 ee     vmov       s15,r3
        0001c3b4 e7 1b f8 ee     vcvt.f64   d17,s15
        0001c3b8 05 0b 5b ed     vldr.64    d16,[r11,#-0x14]
        0001c3bc a0 0b c1 ee     vdiv.f64   d16,d17,d16
        0001c3c0 07 0b 4b ed     vstr.64    d16,[r11,#-0x1c]
        0001c3c4 6c 30 1b e5     ldr        r3,[r11,#quat[1]]
        0001c3c8 90 3a 07 ee     vmov       s15,r3
        0001c3cc e7 1b f8 ee     vcvt.f64   d17,s15
        0001c3d0 05 0b 5b ed     vldr.64    d16,[r11,#-0x14]
        0001c3d4 a0 0b c1 ee     vdiv.f64   d16,d17,d16
        0001c3d8 09 0b 4b ed     vstr.64    d16,[r11,#-0x24]
        0001c3dc 68 30 1b e5     ldr        r3,[r11,#quat[2]]
        0001c3e0 90 3a 07 ee     vmov       s15,r3
        0001c3e4 e7 1b f8 ee     vcvt.f64   d17,s15
        0001c3e8 05 0b 5b ed     vldr.64    d16,[r11,#-0x14]
        0001c3ec a0 0b c1 ee     vdiv.f64   d16,d17,d16
        0001c3f0 0b 0b 4b ed     vstr.64    d16,[r11,#-0x2c]
        0001c3f4 64 30 1b e5     ldr        r3,[r11,#quat[3]]
        0001c3f8 90 3a 07 ee     vmov       s15,r3
        0001c3fc e7 1b f8 ee     vcvt.f64   d17,s15
        0001c400 05 0b 5b ed     vldr.64    d16,[r11,#-0x14]
        0001c404 a0 0b c1 ee     vdiv.f64   d16,d17,d16
        0001c408 0d 0b 4b ed     vstr.64    d16,[r11,#-0x34]
        0001c40c 09 0b 5b ed     vldr.64    d16,[r11,#-0x24]
        0001c410 00 1b f8 ee     vmov.f64   d17,-0x4000000000000000
        0001c414 a1 1b 60 ee     vmul.f64   d17,d16,d17
        0001c418 0d 0b 5b ed     vldr.64    d16,[r11,#-0x34]
        0001c41c a0 1b 61 ee     vmul.f64   d17,d17,d16
        0001c420 07 0b 5b ed     vldr.64    d16,[r11,#-0x1c]
        0001c424 a0 2b 70 ee     vadd.f64   d18,d16,d16
        0001c428 0b 0b 5b ed     vldr.64    d16,[r11,#-0x2c]
        0001c42c a0 0b 62 ee     vmul.f64   d16,d18,d16
        0001c430 a0 0b 71 ee     vadd.f64   d16,d17,d16
        0001c434 30 0b 51 ec     vmov       vertexX,vertexY,d16
        0001c438 e0 dd ff eb     bl         libm.so.6::asin                                  double asin(double __x)
        0001c43c fc 03 4b e1     strd       uVar2,vertexY,[r11,#pitch]
        0001c440 0b 0b 5b ed     vldr.64    d16,[r11,#-0x2c]
        0001c444 a0 1b 70 ee     vadd.f64   d17,d16,d16
        0001c448 0d 0b 5b ed     vldr.64    d16,[r11,#-0x34]
        0001c44c a0 1b 61 ee     vmul.f64   d17,d17,d16
        0001c450 07 0b 5b ed     vldr.64    d16,[r11,#-0x1c]
        0001c454 a0 2b 70 ee     vadd.f64   d18,d16,d16
        0001c458 09 0b 5b ed     vldr.64    d16,[r11,#-0x24]
        0001c45c a0 0b 62 ee     vmul.f64   d16,d18,d16
        0001c460 a0 3b 71 ee     vadd.f64   d19,d17,d16
        0001c464 09 0b 5b ed     vldr.64    d16,[r11,#-0x24]
        0001c468 00 1b f8 ee     vmov.f64   d17,-0x4000000000000000
        0001c46c a1 1b 60 ee     vmul.f64   d17,d16,d17
        0001c470 09 0b 5b ed     vldr.64    d16,[r11,#-0x24]
        0001c474 a0 1b 61 ee     vmul.f64   d17,d17,d16
        0001c478 0b 0b 5b ed     vldr.64    d16,[r11,#-0x2c]
        0001c47c a0 2b 70 ee     vadd.f64   d18,d16,d16
        0001c480 0b 0b 5b ed     vldr.64    d16,[r11,#-0x2c]
        0001c484 a0 0b 62 ee     vmul.f64   d16,d18,d16
        0001c488 e0 0b 71 ee     vsub.f64   d16,d17,d16
        0001c48c 00 1b f7 ee     vmov.f64   d17,0x3ff0000000000000
        0001c490 a1 0b 70 ee     vadd.f64   d16,d16,d17
        0001c494 30 2b 53 ec     vmov       r2,r3,d16
        0001c498 33 0b 51 ec     vmov       uVar2,vertexY,d19
        0001c49c 21 de ff eb     bl         libm.so.6::atan2                                 double atan2(double __y, double 
        0001c4a0 f4 04 4b e1     strd       uVar2,vertexY,[r11,#roll]
        0001c4a4 09 1b 5b ed     vldr.64    d17,[r11,#-0x24]
        0001c4a8 0b 0b 5b ed     vldr.64    d16,[r11,#-0x2c]
        0001c4ac a0 1b 61 ee     vmul.f64   d17,d17,d16
        0001c4b0 07 2b 5b ed     vldr.64    d18,[r11,#-0x1c]
        0001c4b4 0d 0b 5b ed     vldr.64    d16,[r11,#-0x34]
        0001c4b8 a0 0b 62 ee     vmul.f64   d16,d18,d16
        0001c4bc a0 0b 71 ee     vadd.f64   d16,d17,d16
        0001c4c0 a0 3b 70 ee     vadd.f64   d19,d16,d16
        0001c4c4 07 1b 5b ed     vldr.64    d17,[r11,#-0x1c]
        0001c4c8 07 0b 5b ed     vldr.64    d16,[r11,#-0x1c]
        0001c4cc a0 1b 61 ee     vmul.f64   d17,d17,d16
        0001c4d0 09 2b 5b ed     vldr.64    d18,[r11,#-0x24]
        0001c4d4 09 0b 5b ed     vldr.64    d16,[r11,#-0x24]
        0001c4d8 a0 0b 62 ee     vmul.f64   d16,d18,d16
        0001c4dc a0 1b 71 ee     vadd.f64   d17,d17,d16
        0001c4e0 0b 2b 5b ed     vldr.64    d18,[r11,#-0x2c]
        0001c4e4 0b 0b 5b ed     vldr.64    d16,[r11,#-0x2c]
        0001c4e8 a0 0b 62 ee     vmul.f64   d16,d18,d16
        0001c4ec e0 1b 71 ee     vsub.f64   d17,d17,d16
        0001c4f0 0d 2b 5b ed     vldr.64    d18,[r11,#-0x34]
        0001c4f4 0d 0b 5b ed     vldr.64    d16,[r11,#-0x34]
        0001c4f8 a0 0b 62 ee     vmul.f64   d16,d18,d16
        0001c4fc e0 0b 71 ee     vsub.f64   d16,d17,d16
        0001c500 30 2b 53 ec     vmov       r2,r3,d16
        0001c504 33 0b 51 ec     vmov       uVar2,vertexY,d19
        0001c508 06 de ff eb     bl         libm.so.6::atan2                                 double atan2(double __y, double 
        0001c50c fc 04 4b e1     strd       uVar2,vertexY,[r11,#yaw]
        0001c510 0f 0b 5b ed     vldr.64    d16,[r11,#-0x3c]
        0001c514 29 1b df ed     vldr.64    d17,[pc,#0xa4]=>DOUBLE_0001c5c0                  = 1.0E8
        0001c518 a1 1b 60 ee     vmul.f64   d17,d16,d17
        0001c51c 13 0b 5b ed     vldr.64    d16,[r11,#-0x4c]
        0001c520 28 2b df ed     vldr.64    d18,[pc,#0xa0]=>DOUBLE_0001c5c8                  = 1.0E-9
        0001c524 a2 0b 60 ee     vmul.f64   d16,d16,d18
        0001c528 a0 0b 71 ee     vadd.f64   d16,d17,d16
        0001c52c e0 7b fd ee     vcvt.s32   s15,d16
        0001c530 90 2a 17 ee     vmov       r2,s15
        0001c534 78 30 1b e5     ldr        r3,[r11,#local_7c]
        0001c538 00 20 83 e5     str        r2,[r3,#0x0]
        0001c53c 11 0b 5b ed     vldr.64    d16,[r11,#-0x44]
        0001c540 1e 1b df ed     vldr.64    d17,[pc,#0x78]=>DOUBLE_0001c5c0                  = 1.0E8
        0001c544 a1 0b 60 ee     vmul.f64   d16,d16,d17
        0001c548 e0 7b fd ee     vcvt.s32   s15,d16
        0001c54c 90 2a 17 ee     vmov       r2,s15
        0001c550 7c 30 1b e5     ldr        r3,[r11,#local_80]
        0001c554 00 20 83 e5     str        r2,[r3,#0x0]
        0001c558 00 30 a0 e3     mov        r3,#0x0
        0001c55c 11 00 00 ea     b          LAB_0001c5a8
                             LAB_0001c560                                    XREF[1]:     0001c3a8(j)  
        0001c560 a0 3b 0c e3     movw       r3,#0xcba0
        0001c564 42 30 40 e3     movt       r3,#0x42
        0001c568 00 00 93 e5     ldr        uVar2,[r3,#0x0]=>g_xfmtDebug                     = 00000000
        0001c56c 9f 3c 00 e3     movw       r3,#0xc9f
        0001c570 a8 2b 09 e3     movw       r2,#0x9ba8
        0001c574 1c 20 40 e3     movt       r2,#0x1c
        0001c578 70 1b 09 e3     movw       vertexY,#0x9b70
        0001c57c 1c 10 40 e3     movt       vertexY=>s_%s:%d:_INV_WXYZ_QUAT_error_001c9b70   = "%s:%d: INV_WXYZ_QUAT error\n"
        0001c580 5b dd ff eb     bl         libc.so.6::fprintf                               int fprintf(FILE * __stream, cha
                             LAB_0001c584                                    XREF[1]:     0001c394(j)  
        0001c584 05 00 a0 e3     mov        uVar2,#0x5
        0001c588 4a ea ff eb     bl         delay_ms                                         void delay_ms(int ms)
        0001c58c 08 30 1b e5     ldr        r3,[r11,#i]
        0001c590 01 30 83 e2     add        r3,r3,#0x1
        0001c594 08 30 0b e5     str        r3,[r11,#i]
                             LAB_0001c598                                    XREF[1]:     0001c368(j)  
        0001c598 08 30 1b e5     ldr        r3,[r11,#i]
        0001c59c 09 00 53 e3     cmp        r3,#0x9
        0001c5a0 71 ff ff da     ble        LAB_0001c36c
        0001c5a4 00 30 e0 e3     mvn        r3,#0x0
                             LAB_0001c5a8                                    XREF[1]:     0001c55c(j)  
        0001c5a8 03 00 a0 e1     cpy        uVar2,r3
        0001c5ac 04 d0 4b e2     sub        sp,r11,#0x4
        0001c5b0 00 b0 9d e5     ldr        r11,[sp,#0x0]=>local_8
        0001c5b4 04 d0 8d e2     add        sp,sp,#0x4
        0001c5b8 04 f0 9d e4     ldr        pc,[sp],#0x4
                             -- Flow Override: RETURN (TERMINATOR)
        0001c5bc 00              ??         00h
        0001c5bd f0              ??         F0h
        0001c5be 20              ??         20h     
        0001c5bf e3              ??         E3h
                             DOUBLE_0001c5c0                                 XREF[2]:     XfmtReadAngle:0001c514(R), 
                                                                                          XfmtReadAngle:0001c540(R)  
        0001c5c0 00 00 00        double     1.0E8
                 00 84 d7 
                 97 41
                             DOUBLE_0001c5c8                                 XREF[1]:     XfmtReadAngle:0001c520(R)  
        0001c5c8 95 d6 26        double     1.0E-9
                 e8 0b 2e 
                 11 3e

Answer 1

It looks like the code for this function is mainly taken from here. So I think if you fix up the prototypes of asin and atan2 to return angles, you should see the roll, pitch, and yaw values being written to.

This should also make the calculation of vertexX and vertexY easier to follow.

It's a bit hard to follow but from what I can tell from the assembly, *vertexX = pitch * 1e8 + yaw * 1e-9 and *vertexY = roll * 1e8. This basically comes out to *vertexX = pitch * 1e8 so I'm not quite sure what the point of adding yaw * 1e-9 is.

My guess is that they are doing something similar to the calculation here while assuming the angles are small. (See https://en.wikipedia.org/wiki/Small-angle_approximation) I'm not that familiar with the actual calculations being done here so I can't say for sure.