; generated by Component: ARM Compiler 5.06 update 6 (build 750) Tool: ArmCC [4d3637]
; commandline ArmCC [--list --debug -c --asm --interleave -o.\obj\inv_mpu.o --asm_dir=.\list\ --list_dir=.\list\ --depend=.\obj\inv_mpu.d --cpu=Cortex-M3 --apcs=interwork -O0 --diag_suppress=9931 -I.\FWlib\inc -I.\user -I.\CM3 -I.\dmp -I.\RTE\_STM32-FD -If:\Users\Administrator\AppData\Local\Arm\Packs\ARM\CMSIS\5.7.0\CMSIS\Core\Include -If:\Users\Administrator\AppData\Local\Arm\Packs\Keil\STM32F1xx_DFP\2.4.1\Device\Include -D__MICROLIB -D__UVISION_VERSION=530 -D_RTE_ -DSTM32F10X_MD -D_RTE_ -DUSE_STDPERIPH_DRIVER -DSTM32F10X_MD --omf_browse=.\obj\inv_mpu.crf dmp\inv_mpu.c]
                          THUMB

                          AREA ||.text||, CODE, READONLY, ALIGN=2

                          REQUIRE _printf_pre_padding
                          REQUIRE _printf_percent
                          REQUIRE _printf_flags
                          REQUIRE _printf_widthprec
                          REQUIRE _printf_x
                          REQUIRE _printf_longlong_hex
                          REQUIRE _printf_d
                          REQUIRE _printf_int_dec
                  set_int_enable PROC
;;;684     */
;;;685    static int set_int_enable(unsigned char enable)
000000  b538              PUSH     {r3-r5,lr}
;;;686    {
000002  4604              MOV      r4,r0
;;;687        unsigned char tmp;
;;;688    
;;;689        if (st.chip_cfg.dmp_on) {
000004  48f7              LDR      r0,|L1.996|
000006  f8900024          LDRB     r0,[r0,#0x24]
00000a  b1c0              CBZ      r0,|L1.62|
;;;690            if (enable)
00000c  b114              CBZ      r4,|L1.20|
;;;691                tmp = BIT_DMP_INT_EN;
00000e  2002              MOVS     r0,#2
000010  9000              STR      r0,[sp,#0]
000012  e001              B        |L1.24|
                  |L1.20|
;;;692            else
;;;693                tmp = 0x00;
000014  2000              MOVS     r0,#0
000016  9000              STR      r0,[sp,#0]
                  |L1.24|
;;;694            if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &tmp))
000018  4af2              LDR      r2,|L1.996|
00001a  6812              LDR      r2,[r2,#0]  ; st
00001c  7bd1              LDRB     r1,[r2,#0xf]
00001e  4af1              LDR      r2,|L1.996|
000020  6852              LDR      r2,[r2,#4]  ; st
000022  7810              LDRB     r0,[r2,#0]
000024  466b              MOV      r3,sp
000026  2201              MOVS     r2,#1
000028  f7fffffe          BL       MPU_Write_Len
00002c  b110              CBZ      r0,|L1.52|
;;;695                return -1;
00002e  f04f30ff          MOV      r0,#0xffffffff
                  |L1.50|
;;;696            st.chip_cfg.int_enable = tmp;
;;;697        } else {
;;;698            if (!st.chip_cfg.sensors)
;;;699                return -1;
;;;700            if (enable && st.chip_cfg.int_enable)
;;;701                return 0;
;;;702            if (enable)
;;;703                tmp = BIT_DATA_RDY_EN;
;;;704            else
;;;705                tmp = 0x00;
;;;706            if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &tmp))
;;;707                return -1;
;;;708            st.chip_cfg.int_enable = tmp;
;;;709        }
;;;710        return 0;
;;;711    }
000032  bd38              POP      {r3-r5,pc}
                  |L1.52|
000034  f89d1000          LDRB     r1,[sp,#0]            ;696
000038  48ea              LDR      r0,|L1.996|
00003a  7441              STRB     r1,[r0,#0x11]         ;696
00003c  e023              B        |L1.134|
                  |L1.62|
00003e  48e9              LDR      r0,|L1.996|
000040  7a80              LDRB     r0,[r0,#0xa]          ;698
000042  b910              CBNZ     r0,|L1.74|
000044  f04f30ff          MOV      r0,#0xffffffff        ;699
000048  e7f3              B        |L1.50|
                  |L1.74|
00004a  b124              CBZ      r4,|L1.86|
00004c  48e5              LDR      r0,|L1.996|
00004e  7c40              LDRB     r0,[r0,#0x11]         ;700
000050  b108              CBZ      r0,|L1.86|
000052  2000              MOVS     r0,#0                 ;701
000054  e7ed              B        |L1.50|
                  |L1.86|
000056  b114              CBZ      r4,|L1.94|
000058  2001              MOVS     r0,#1                 ;703
00005a  9000              STR      r0,[sp,#0]            ;703
00005c  e001              B        |L1.98|
                  |L1.94|
00005e  2000              MOVS     r0,#0                 ;705
000060  9000              STR      r0,[sp,#0]            ;705
                  |L1.98|
000062  4ae0              LDR      r2,|L1.996|
000064  6812              LDR      r2,[r2,#0]            ;706  ; st
000066  7bd1              LDRB     r1,[r2,#0xf]          ;706
000068  4ade              LDR      r2,|L1.996|
00006a  6852              LDR      r2,[r2,#4]            ;706  ; st
00006c  7810              LDRB     r0,[r2,#0]            ;706
00006e  466b              MOV      r3,sp                 ;706
000070  2201              MOVS     r2,#1                 ;706
000072  f7fffffe          BL       MPU_Write_Len
000076  b110              CBZ      r0,|L1.126|
000078  f04f30ff          MOV      r0,#0xffffffff        ;707
00007c  e7d9              B        |L1.50|
                  |L1.126|
00007e  f89d1000          LDRB     r1,[sp,#0]            ;708
000082  48d8              LDR      r0,|L1.996|
000084  7441              STRB     r1,[r0,#0x11]         ;708
                  |L1.134|
000086  2000              MOVS     r0,#0                 ;710
000088  e7d3              B        |L1.50|
;;;712    
                          ENDP

                  mpu_reg_dump PROC
;;;716     */
;;;717    int mpu_reg_dump(void)
00008a  b538              PUSH     {r3-r5,lr}
;;;718    {
;;;719        unsigned char ii;
;;;720        unsigned char data;
;;;721    
;;;722        for (ii = 0; ii < st.hw->num_reg; ii++) {
00008c  2400              MOVS     r4,#0
00008e  e01f              B        |L1.208|
                  |L1.144|
;;;723            if (ii == st.reg->fifo_r_w || ii == st.reg->mem_r_w)
000090  48d4              LDR      r0,|L1.996|
000092  6800              LDR      r0,[r0,#0]  ; st
000094  7ac0              LDRB     r0,[r0,#0xb]
000096  42a0              CMP      r0,r4
000098  d004              BEQ      |L1.164|
00009a  48d2              LDR      r0,|L1.996|
00009c  6800              LDR      r0,[r0,#0]  ; st
00009e  7d40              LDRB     r0,[r0,#0x15]
0000a0  42a0              CMP      r0,r4
0000a2  d100              BNE      |L1.166|
                  |L1.164|
;;;724                continue;
0000a4  e012              B        |L1.204|
                  |L1.166|
;;;725            if (i2c_read(st.hw->addr, ii, 1, &data))
0000a6  49cf              LDR      r1,|L1.996|
0000a8  6849              LDR      r1,[r1,#4]  ; st
0000aa  7808              LDRB     r0,[r1,#0]
0000ac  466b              MOV      r3,sp
0000ae  2201              MOVS     r2,#1
0000b0  4621              MOV      r1,r4
0000b2  f7fffffe          BL       MPU_Read_Len
0000b6  b110              CBZ      r0,|L1.190|
;;;726                return -1;
0000b8  f04f30ff          MOV      r0,#0xffffffff
                  |L1.188|
;;;727            log_i("%#5x: %#5x\r\n", ii, data);
;;;728        }
;;;729        return 0;
;;;730    }
0000bc  bd38              POP      {r3-r5,pc}
                  |L1.190|
0000be  f89d2000          LDRB     r2,[sp,#0]            ;727
0000c2  4621              MOV      r1,r4                 ;727
0000c4  a0c8              ADR      r0,|L1.1000|
0000c6  f7fffffe          BL       __2printf
0000ca  bf00              NOP                            ;724
                  |L1.204|
0000cc  1c60              ADDS     r0,r4,#1              ;722
0000ce  b2c4              UXTB     r4,r0                 ;722
                  |L1.208|
0000d0  48c4              LDR      r0,|L1.996|
0000d2  6840              LDR      r0,[r0,#4]            ;722  ; st
0000d4  7900              LDRB     r0,[r0,#4]            ;722
0000d6  42a0              CMP      r0,r4                 ;722
0000d8  dcda              BGT      |L1.144|
0000da  2000              MOVS     r0,#0                 ;729
0000dc  e7ee              B        |L1.188|
;;;731    
                          ENDP

                  mpu_read_reg PROC
;;;738     */
;;;739    int mpu_read_reg(unsigned char reg, unsigned char *data)
0000de  b570              PUSH     {r4-r6,lr}
;;;740    {
0000e0  4604              MOV      r4,r0
0000e2  460d              MOV      r5,r1
;;;741        if (reg == st.reg->fifo_r_w || reg == st.reg->mem_r_w)
0000e4  48bf              LDR      r0,|L1.996|
0000e6  6800              LDR      r0,[r0,#0]  ; st
0000e8  7ac0              LDRB     r0,[r0,#0xb]
0000ea  42a0              CMP      r0,r4
0000ec  d004              BEQ      |L1.248|
0000ee  48bd              LDR      r0,|L1.996|
0000f0  6800              LDR      r0,[r0,#0]  ; st
0000f2  7d40              LDRB     r0,[r0,#0x15]
0000f4  42a0              CMP      r0,r4
0000f6  d102              BNE      |L1.254|
                  |L1.248|
;;;742            return -1;
0000f8  f04f30ff          MOV      r0,#0xffffffff
                  |L1.252|
;;;743        if (reg >= st.hw->num_reg)
;;;744            return -1;
;;;745        return i2c_read(st.hw->addr, reg, 1, data);
;;;746    }
0000fc  bd70              POP      {r4-r6,pc}
                  |L1.254|
0000fe  48b9              LDR      r0,|L1.996|
000100  6840              LDR      r0,[r0,#4]            ;743  ; st
000102  7900              LDRB     r0,[r0,#4]            ;743
000104  42a0              CMP      r0,r4                 ;743
000106  dc02              BGT      |L1.270|
000108  f04f30ff          MOV      r0,#0xffffffff        ;744
00010c  e7f6              B        |L1.252|
                  |L1.270|
00010e  49b5              LDR      r1,|L1.996|
000110  6849              LDR      r1,[r1,#4]            ;745  ; st
000112  7808              LDRB     r0,[r1,#0]            ;745
000114  462b              MOV      r3,r5                 ;745
000116  2201              MOVS     r2,#1                 ;745
000118  4621              MOV      r1,r4                 ;745
00011a  f7fffffe          BL       MPU_Read_Len
00011e  e7ed              B        |L1.252|
;;;747    
                          ENDP

                  mpu_set_int_latched PROC
;;;1883    */
;;;1884   int mpu_set_int_latched(unsigned char enable)
000120  b538              PUSH     {r3-r5,lr}
;;;1885   {
000122  4604              MOV      r4,r0
;;;1886       unsigned char tmp;
;;;1887       if (st.chip_cfg.latched_int == enable)
000124  48af              LDR      r0,|L1.996|
000126  f8900023          LDRB     r0,[r0,#0x23]
00012a  42a0              CMP      r0,r4
00012c  d101              BNE      |L1.306|
;;;1888           return 0;
00012e  2000              MOVS     r0,#0
                  |L1.304|
;;;1889   
;;;1890       if (enable)
;;;1891           tmp = BIT_LATCH_EN | BIT_ANY_RD_CLR;
;;;1892       else
;;;1893           tmp = 0;
;;;1894       if (st.chip_cfg.bypass_mode)
;;;1895           tmp |= BIT_BYPASS_EN;
;;;1896       if (st.chip_cfg.active_low_int)
;;;1897           tmp |= BIT_ACTL;
;;;1898       if (i2c_write(st.hw->addr, st.reg->int_pin_cfg, 1, &tmp))
;;;1899           return -1;
;;;1900       st.chip_cfg.latched_int = enable;
;;;1901       return 0;
;;;1902   }
000130  bd38              POP      {r3-r5,pc}
                  |L1.306|
000132  b114              CBZ      r4,|L1.314|
000134  2030              MOVS     r0,#0x30              ;1891
000136  9000              STR      r0,[sp,#0]            ;1891
000138  e001              B        |L1.318|
                  |L1.314|
00013a  2000              MOVS     r0,#0                 ;1893
00013c  9000              STR      r0,[sp,#0]            ;1893
                  |L1.318|
00013e  48a9              LDR      r0,|L1.996|
000140  7c80              LDRB     r0,[r0,#0x12]         ;1894
000142  b120              CBZ      r0,|L1.334|
000144  f89d0000          LDRB     r0,[sp,#0]            ;1895
000148  f0400002          ORR      r0,r0,#2              ;1895
00014c  9000              STR      r0,[sp,#0]            ;1895
                  |L1.334|
00014e  48a5              LDR      r0,|L1.996|
000150  f8900022          LDRB     r0,[r0,#0x22]         ;1896
000154  b120              CBZ      r0,|L1.352|
000156  f89d0000          LDRB     r0,[sp,#0]            ;1897
00015a  f0400080          ORR      r0,r0,#0x80           ;1897
00015e  9000              STR      r0,[sp,#0]            ;1897
                  |L1.352|
000160  4aa0              LDR      r2,|L1.996|
000162  6812              LDR      r2,[r2,#0]            ;1898  ; st
000164  7d11              LDRB     r1,[r2,#0x14]         ;1898
000166  4a9f              LDR      r2,|L1.996|
000168  6852              LDR      r2,[r2,#4]            ;1898  ; st
00016a  7810              LDRB     r0,[r2,#0]            ;1898
00016c  466b              MOV      r3,sp                 ;1898
00016e  2201              MOVS     r2,#1                 ;1898
000170  f7fffffe          BL       MPU_Write_Len
000174  b110              CBZ      r0,|L1.380|
000176  f04f30ff          MOV      r0,#0xffffffff        ;1899
00017a  e7d9              B        |L1.304|
                  |L1.380|
00017c  4899              LDR      r0,|L1.996|
00017e  f8804023          STRB     r4,[r0,#0x23]         ;1900
000182  2000              MOVS     r0,#0                 ;1901
000184  e7d4              B        |L1.304|
;;;1903   
                          ENDP

                  mpu_set_sensors PROC
;;;1596    */
;;;1597   int mpu_set_sensors(unsigned char sensors)
000186  b538              PUSH     {r3-r5,lr}
;;;1598   {
000188  4604              MOV      r4,r0
;;;1599       unsigned char data;
;;;1600   #ifdef AK89xx_SECONDARY
;;;1601       unsigned char user_ctrl;
;;;1602   #endif
;;;1603   
;;;1604       if (sensors & INV_XYZ_GYRO)
00018a  f0040070          AND      r0,r4,#0x70
00018e  b110              CBZ      r0,|L1.406|
;;;1605           data = INV_CLK_PLL;
000190  2001              MOVS     r0,#1
000192  9000              STR      r0,[sp,#0]
000194  e005              B        |L1.418|
                  |L1.406|
;;;1606       else if (sensors)
000196  b114              CBZ      r4,|L1.414|
;;;1607           data = 0;
000198  2000              MOVS     r0,#0
00019a  9000              STR      r0,[sp,#0]
00019c  e001              B        |L1.418|
                  |L1.414|
;;;1608       else
;;;1609           data = BIT_SLEEP;
00019e  2040              MOVS     r0,#0x40
0001a0  9000              STR      r0,[sp,#0]
                  |L1.418|
;;;1610       if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, &data)) {
0001a2  4a90              LDR      r2,|L1.996|
0001a4  6812              LDR      r2,[r2,#0]  ; st
0001a6  7c91              LDRB     r1,[r2,#0x12]
0001a8  4a8e              LDR      r2,|L1.996|
0001aa  6852              LDR      r2,[r2,#4]  ; st
0001ac  7810              LDRB     r0,[r2,#0]
0001ae  466b              MOV      r3,sp
0001b0  2201              MOVS     r2,#1
0001b2  f7fffffe          BL       MPU_Write_Len
0001b6  b120              CBZ      r0,|L1.450|
;;;1611           st.chip_cfg.sensors = 0;
0001b8  2100              MOVS     r1,#0
0001ba  488a              LDR      r0,|L1.996|
0001bc  7281              STRB     r1,[r0,#0xa]
;;;1612           return -1;
0001be  1e48              SUBS     r0,r1,#1
                  |L1.448|
;;;1613       }
;;;1614       st.chip_cfg.clk_src = data & ~BIT_SLEEP;
;;;1615   
;;;1616       data = 0;
;;;1617       if (!(sensors & INV_X_GYRO))
;;;1618           data |= BIT_STBY_XG;
;;;1619       if (!(sensors & INV_Y_GYRO))
;;;1620           data |= BIT_STBY_YG;
;;;1621       if (!(sensors & INV_Z_GYRO))
;;;1622           data |= BIT_STBY_ZG;
;;;1623       if (!(sensors & INV_XYZ_ACCEL))
;;;1624           data |= BIT_STBY_XYZA;
;;;1625       if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_2, 1, &data)) {
;;;1626           st.chip_cfg.sensors = 0;
;;;1627           return -1;
;;;1628       }
;;;1629   
;;;1630       if (sensors && (sensors != INV_XYZ_ACCEL))
;;;1631           /* Latched interrupts only used in LP accel mode. */
;;;1632           mpu_set_int_latched(0);
;;;1633   
;;;1634   #ifdef AK89xx_SECONDARY
;;;1635   #ifdef AK89xx_BYPASS
;;;1636       if (sensors & INV_XYZ_COMPASS)
;;;1637           mpu_set_bypass(1);
;;;1638       else
;;;1639           mpu_set_bypass(0);
;;;1640   #else
;;;1641       if (i2c_read(st.hw->addr, st.reg->user_ctrl, 1, &user_ctrl))
;;;1642           return -1;
;;;1643       /* Handle AKM power management. */
;;;1644       if (sensors & INV_XYZ_COMPASS) {
;;;1645           data = AKM_SINGLE_MEASUREMENT;
;;;1646           user_ctrl |= BIT_AUX_IF_EN;
;;;1647       } else {
;;;1648           data = AKM_POWER_DOWN;
;;;1649           user_ctrl &= ~BIT_AUX_IF_EN;
;;;1650       }
;;;1651       if (st.chip_cfg.dmp_on)
;;;1652           user_ctrl |= BIT_DMP_EN;
;;;1653       else
;;;1654           user_ctrl &= ~BIT_DMP_EN;
;;;1655       if (i2c_write(st.hw->addr, st.reg->s1_do, 1, &data))
;;;1656           return -1;
;;;1657       /* Enable/disable I2C master mode. */
;;;1658       if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &user_ctrl))
;;;1659           return -1;
;;;1660   #endif
;;;1661   #endif
;;;1662   
;;;1663       st.chip_cfg.sensors = sensors;
;;;1664       st.chip_cfg.lp_accel_mode = 0;
;;;1665       delay_ms(50);
;;;1666       return 0;
;;;1667   }
0001c0  bd38              POP      {r3-r5,pc}
                  |L1.450|
0001c2  f89d0000          LDRB     r0,[sp,#0]            ;1614
0001c6  f0200140          BIC      r1,r0,#0x40           ;1614
0001ca  4886              LDR      r0,|L1.996|
0001cc  7301              STRB     r1,[r0,#0xc]          ;1614
0001ce  2000              MOVS     r0,#0                 ;1616
0001d0  9000              STR      r0,[sp,#0]            ;1616
0001d2  f0040040          AND      r0,r4,#0x40           ;1617
0001d6  b920              CBNZ     r0,|L1.482|
0001d8  f89d0000          LDRB     r0,[sp,#0]            ;1618
0001dc  f0400004          ORR      r0,r0,#4              ;1618
0001e0  9000              STR      r0,[sp,#0]            ;1618
                  |L1.482|
0001e2  f0040020          AND      r0,r4,#0x20           ;1619
0001e6  b920              CBNZ     r0,|L1.498|
0001e8  f89d0000          LDRB     r0,[sp,#0]            ;1620
0001ec  f0400002          ORR      r0,r0,#2              ;1620
0001f0  9000              STR      r0,[sp,#0]            ;1620
                  |L1.498|
0001f2  f0040010          AND      r0,r4,#0x10           ;1621
0001f6  b920              CBNZ     r0,|L1.514|
0001f8  f89d0000          LDRB     r0,[sp,#0]            ;1622
0001fc  f0400001          ORR      r0,r0,#1              ;1622
000200  9000              STR      r0,[sp,#0]            ;1622
                  |L1.514|
000202  f0040008          AND      r0,r4,#8              ;1623
000206  b920              CBNZ     r0,|L1.530|
000208  f89d0000          LDRB     r0,[sp,#0]            ;1624
00020c  f0400038          ORR      r0,r0,#0x38           ;1624
000210  9000              STR      r0,[sp,#0]            ;1624
                  |L1.530|
000212  4a74              LDR      r2,|L1.996|
000214  6812              LDR      r2,[r2,#0]            ;1625  ; st
000216  7cd1              LDRB     r1,[r2,#0x13]         ;1625
000218  4a72              LDR      r2,|L1.996|
00021a  6852              LDR      r2,[r2,#4]            ;1625  ; st
00021c  7810              LDRB     r0,[r2,#0]            ;1625
00021e  466b              MOV      r3,sp                 ;1625
000220  2201              MOVS     r2,#1                 ;1625
000222  f7fffffe          BL       MPU_Write_Len
000226  b120              CBZ      r0,|L1.562|
000228  2100              MOVS     r1,#0                 ;1626
00022a  486e              LDR      r0,|L1.996|
00022c  7281              STRB     r1,[r0,#0xa]          ;1626
00022e  1e48              SUBS     r0,r1,#1              ;1627
000230  e7c6              B        |L1.448|
                  |L1.562|
000232  b124              CBZ      r4,|L1.574|
000234  2c08              CMP      r4,#8                 ;1630
000236  d002              BEQ      |L1.574|
000238  2000              MOVS     r0,#0                 ;1632
00023a  f7fffffe          BL       mpu_set_int_latched
                  |L1.574|
00023e  4869              LDR      r0,|L1.996|
000240  7284              STRB     r4,[r0,#0xa]          ;1663
000242  2100              MOVS     r1,#0                 ;1664
000244  7501              STRB     r1,[r0,#0x14]         ;1664
000246  2032              MOVS     r0,#0x32              ;1665
000248  f7fffffe          BL       delay_ms
00024c  2000              MOVS     r0,#0                 ;1666
00024e  e7b7              B        |L1.448|
;;;1668   
                          ENDP

                  mpu_set_bypass PROC
;;;1821    */
;;;1822   int mpu_set_bypass(unsigned char bypass_on)
000250  b538              PUSH     {r3-r5,lr}
;;;1823   {
000252  4604              MOV      r4,r0
;;;1824       unsigned char tmp;
;;;1825   
;;;1826       if (st.chip_cfg.bypass_mode == bypass_on)
000254  4863              LDR      r0,|L1.996|
000256  7c80              LDRB     r0,[r0,#0x12]
000258  42a0              CMP      r0,r4
00025a  d101              BNE      |L1.608|
;;;1827           return 0;
00025c  2000              MOVS     r0,#0
                  |L1.606|
;;;1828   
;;;1829       if (bypass_on) {
;;;1830           if (i2c_read(st.hw->addr, st.reg->user_ctrl, 1, &tmp))
;;;1831               return -1;
;;;1832           tmp &= ~BIT_AUX_IF_EN;
;;;1833           if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &tmp))
;;;1834               return -1;
;;;1835           delay_ms(3);
;;;1836           tmp = BIT_BYPASS_EN;
;;;1837           if (st.chip_cfg.active_low_int)
;;;1838               tmp |= BIT_ACTL;
;;;1839           if (st.chip_cfg.latched_int)
;;;1840               tmp |= BIT_LATCH_EN | BIT_ANY_RD_CLR;
;;;1841           if (i2c_write(st.hw->addr, st.reg->int_pin_cfg, 1, &tmp))
;;;1842               return -1;
;;;1843       } else {
;;;1844           /* Enable I2C master mode if compass is being used. */
;;;1845           if (i2c_read(st.hw->addr, st.reg->user_ctrl, 1, &tmp))
;;;1846               return -1;
;;;1847           if (st.chip_cfg.sensors & INV_XYZ_COMPASS)
;;;1848               tmp |= BIT_AUX_IF_EN;
;;;1849           else
;;;1850               tmp &= ~BIT_AUX_IF_EN;
;;;1851           if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &tmp))
;;;1852               return -1;
;;;1853           delay_ms(3);
;;;1854           if (st.chip_cfg.active_low_int)
;;;1855               tmp = BIT_ACTL;
;;;1856           else
;;;1857               tmp = 0;
;;;1858           if (st.chip_cfg.latched_int)
;;;1859               tmp |= BIT_LATCH_EN | BIT_ANY_RD_CLR;
;;;1860           if (i2c_write(st.hw->addr, st.reg->int_pin_cfg, 1, &tmp))
;;;1861               return -1;
;;;1862       }
;;;1863       st.chip_cfg.bypass_mode = bypass_on;
;;;1864       return 0;
;;;1865   }
00025e  bd38              POP      {r3-r5,pc}
                  |L1.608|
000260  2c00              CMP      r4,#0                 ;1829
000262  d046              BEQ      |L1.754|
000264  4a5f              LDR      r2,|L1.996|
000266  6812              LDR      r2,[r2,#0]            ;1830  ; st
000268  7911              LDRB     r1,[r2,#4]            ;1830
00026a  4a5e              LDR      r2,|L1.996|
00026c  6852              LDR      r2,[r2,#4]            ;1830  ; st
00026e  7810              LDRB     r0,[r2,#0]            ;1830
000270  466b              MOV      r3,sp                 ;1830
000272  2201              MOVS     r2,#1                 ;1830
000274  f7fffffe          BL       MPU_Read_Len
000278  b110              CBZ      r0,|L1.640|
00027a  f04f30ff          MOV      r0,#0xffffffff        ;1831
00027e  e7ee              B        |L1.606|
                  |L1.640|
000280  f89d0000          LDRB     r0,[sp,#0]            ;1832
000284  f0200020          BIC      r0,r0,#0x20           ;1832
000288  9000              STR      r0,[sp,#0]            ;1832
00028a  4a56              LDR      r2,|L1.996|
00028c  6812              LDR      r2,[r2,#0]            ;1833  ; st
00028e  7911              LDRB     r1,[r2,#4]            ;1833
000290  4a54              LDR      r2,|L1.996|
000292  6852              LDR      r2,[r2,#4]            ;1833  ; st
000294  7810              LDRB     r0,[r2,#0]            ;1833
000296  466b              MOV      r3,sp                 ;1833
000298  2201              MOVS     r2,#1                 ;1833
00029a  f7fffffe          BL       MPU_Write_Len
00029e  b110              CBZ      r0,|L1.678|
0002a0  f04f30ff          MOV      r0,#0xffffffff        ;1834
0002a4  e7db              B        |L1.606|
                  |L1.678|
0002a6  2003              MOVS     r0,#3                 ;1835
0002a8  f7fffffe          BL       delay_ms
0002ac  2002              MOVS     r0,#2                 ;1836
0002ae  9000              STR      r0,[sp,#0]            ;1836
0002b0  484c              LDR      r0,|L1.996|
0002b2  f8900022          LDRB     r0,[r0,#0x22]         ;1837
0002b6  b120              CBZ      r0,|L1.706|
0002b8  f89d0000          LDRB     r0,[sp,#0]            ;1838
0002bc  f0400080          ORR      r0,r0,#0x80           ;1838
0002c0  9000              STR      r0,[sp,#0]            ;1838
                  |L1.706|
0002c2  4848              LDR      r0,|L1.996|
0002c4  f8900023          LDRB     r0,[r0,#0x23]         ;1839
0002c8  b120              CBZ      r0,|L1.724|
0002ca  f89d0000          LDRB     r0,[sp,#0]            ;1840
0002ce  f0400030          ORR      r0,r0,#0x30           ;1840
0002d2  9000              STR      r0,[sp,#0]            ;1840
                  |L1.724|
0002d4  4a43              LDR      r2,|L1.996|
0002d6  6812              LDR      r2,[r2,#0]            ;1841  ; st
0002d8  7d11              LDRB     r1,[r2,#0x14]         ;1841
0002da  4a42              LDR      r2,|L1.996|
0002dc  6852              LDR      r2,[r2,#4]            ;1841  ; st
0002de  7810              LDRB     r0,[r2,#0]            ;1841
0002e0  466b              MOV      r3,sp                 ;1841
0002e2  2201              MOVS     r2,#1                 ;1841
0002e4  f7fffffe          BL       MPU_Write_Len
0002e8  2800              CMP      r0,#0                 ;1841
0002ea  d051              BEQ      |L1.912|
0002ec  f04f30ff          MOV      r0,#0xffffffff        ;1842
0002f0  e7b5              B        |L1.606|
                  |L1.754|
0002f2  4a3c              LDR      r2,|L1.996|
0002f4  6812              LDR      r2,[r2,#0]            ;1845  ; st
0002f6  7911              LDRB     r1,[r2,#4]            ;1845
0002f8  4a3a              LDR      r2,|L1.996|
0002fa  6852              LDR      r2,[r2,#4]            ;1845  ; st
0002fc  7810              LDRB     r0,[r2,#0]            ;1845
0002fe  466b              MOV      r3,sp                 ;1845
000300  2201              MOVS     r2,#1                 ;1845
000302  f7fffffe          BL       MPU_Read_Len
000306  b110              CBZ      r0,|L1.782|
000308  f04f30ff          MOV      r0,#0xffffffff        ;1846
00030c  e7a7              B        |L1.606|
                  |L1.782|
00030e  4835              LDR      r0,|L1.996|
000310  7a80              LDRB     r0,[r0,#0xa]          ;1847
000312  f0000001          AND      r0,r0,#1              ;1847
000316  b128              CBZ      r0,|L1.804|
000318  f89d0000          LDRB     r0,[sp,#0]            ;1848
00031c  f0400020          ORR      r0,r0,#0x20           ;1848
000320  9000              STR      r0,[sp,#0]            ;1848
000322  e004              B        |L1.814|
                  |L1.804|
000324  f89d0000          LDRB     r0,[sp,#0]            ;1850
000328  f0200020          BIC      r0,r0,#0x20           ;1850
00032c  9000              STR      r0,[sp,#0]            ;1850
                  |L1.814|
00032e  4a2d              LDR      r2,|L1.996|
000330  6812              LDR      r2,[r2,#0]            ;1851  ; st
000332  7911              LDRB     r1,[r2,#4]            ;1851
000334  4a2b              LDR      r2,|L1.996|
000336  6852              LDR      r2,[r2,#4]            ;1851  ; st
000338  7810              LDRB     r0,[r2,#0]            ;1851
00033a  466b              MOV      r3,sp                 ;1851
00033c  2201              MOVS     r2,#1                 ;1851
00033e  f7fffffe          BL       MPU_Write_Len
000342  b110              CBZ      r0,|L1.842|
000344  f04f30ff          MOV      r0,#0xffffffff        ;1852
000348  e789              B        |L1.606|
                  |L1.842|
00034a  2003              MOVS     r0,#3                 ;1853
00034c  f7fffffe          BL       delay_ms
000350  4824              LDR      r0,|L1.996|
000352  f8900022          LDRB     r0,[r0,#0x22]         ;1854
000356  b110              CBZ      r0,|L1.862|
000358  2080              MOVS     r0,#0x80              ;1855
00035a  9000              STR      r0,[sp,#0]            ;1855
00035c  e001              B        |L1.866|
                  |L1.862|
00035e  2000              MOVS     r0,#0                 ;1857
000360  9000              STR      r0,[sp,#0]            ;1857
                  |L1.866|
000362  4820              LDR      r0,|L1.996|
000364  f8900023          LDRB     r0,[r0,#0x23]         ;1858
000368  b120              CBZ      r0,|L1.884|
00036a  f89d0000          LDRB     r0,[sp,#0]            ;1859
00036e  f0400030          ORR      r0,r0,#0x30           ;1859
000372  9000              STR      r0,[sp,#0]            ;1859
                  |L1.884|
000374  4a1b              LDR      r2,|L1.996|
000376  6812              LDR      r2,[r2,#0]            ;1860  ; st
000378  7d11              LDRB     r1,[r2,#0x14]         ;1860
00037a  4a1a              LDR      r2,|L1.996|
00037c  6852              LDR      r2,[r2,#4]            ;1860  ; st
00037e  7810              LDRB     r0,[r2,#0]            ;1860
000380  466b              MOV      r3,sp                 ;1860
000382  2201              MOVS     r2,#1                 ;1860
000384  f7fffffe          BL       MPU_Write_Len
000388  b110              CBZ      r0,|L1.912|
00038a  f04f30ff          MOV      r0,#0xffffffff        ;1861
00038e  e766              B        |L1.606|
                  |L1.912|
000390  4814              LDR      r0,|L1.996|
000392  7484              STRB     r4,[r0,#0x12]         ;1863
000394  2000              MOVS     r0,#0                 ;1864
000396  e762              B        |L1.606|
;;;1866   
                          ENDP

                  mpu_reset_fifo PROC
;;;1096    */
;;;1097   int mpu_reset_fifo(void)
000398  b508              PUSH     {r3,lr}
;;;1098   {
;;;1099       unsigned char data;
;;;1100   
;;;1101       if (!(st.chip_cfg.sensors))
00039a  4812              LDR      r0,|L1.996|
00039c  7a80              LDRB     r0,[r0,#0xa]
00039e  b910              CBNZ     r0,|L1.934|
;;;1102           return -1;
0003a0  f04f30ff          MOV      r0,#0xffffffff
                  |L1.932|
;;;1103   
;;;1104       data = 0;
;;;1105       if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &data))
;;;1106           return -1;
;;;1107       if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, &data))
;;;1108           return -1;
;;;1109       if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data))
;;;1110           return -1;
;;;1111        
;;;1112       if (st.chip_cfg.dmp_on) {
;;;1113           data = BIT_FIFO_RST | BIT_DMP_RST;
;;;1114           if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data))
;;;1115               return -1;
;;;1116           delay_ms(50);
;;;1117           data = BIT_DMP_EN | BIT_FIFO_EN;
;;;1118           if (st.chip_cfg.sensors & INV_XYZ_COMPASS)
;;;1119               data |= BIT_AUX_IF_EN;
;;;1120           if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data))
;;;1121               return -1;
;;;1122           if (st.chip_cfg.int_enable)
;;;1123               data = BIT_DMP_INT_EN;
;;;1124           else
;;;1125               data = 0;
;;;1126           if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &data))
;;;1127               return -1;
;;;1128           data = 0;
;;;1129           if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, &data))
;;;1130               return -1;//USART_SendData(USART1,55);	
;;;1131       } else {
;;;1132           data = BIT_FIFO_RST;
;;;1133           if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data))
;;;1134               return -1;
;;;1135           if (st.chip_cfg.bypass_mode || !(st.chip_cfg.sensors & INV_XYZ_COMPASS))
;;;1136               data = BIT_FIFO_EN;
;;;1137           else
;;;1138               data = BIT_FIFO_EN | BIT_AUX_IF_EN;
;;;1139           if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &data))
;;;1140               return -1;
;;;1141           delay_ms(50);
;;;1142           if (st.chip_cfg.int_enable)
;;;1143               data = BIT_DATA_RDY_EN;
;;;1144           else
;;;1145               data = 0;
;;;1146           if (i2c_write(st.hw->addr, st.reg->int_enable, 1, &data))
;;;1147               return -1;
;;;1148           if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, &st.chip_cfg.fifo_enable))
;;;1149               return -1;
;;;1150       }
;;;1151       return 0;
;;;1152   }
0003a4  bd08              POP      {r3,pc}
                  |L1.934|
0003a6  2000              MOVS     r0,#0                 ;1104
0003a8  9000              STR      r0,[sp,#0]            ;1104
0003aa  4a0e              LDR      r2,|L1.996|
0003ac  6812              LDR      r2,[r2,#0]            ;1105  ; st
0003ae  7bd1              LDRB     r1,[r2,#0xf]          ;1105
0003b0  4a0c              LDR      r2,|L1.996|
0003b2  6852              LDR      r2,[r2,#4]            ;1105  ; st
0003b4  7810              LDRB     r0,[r2,#0]            ;1105
0003b6  466b              MOV      r3,sp                 ;1105
0003b8  2201              MOVS     r2,#1                 ;1105
0003ba  f7fffffe          BL       MPU_Write_Len
0003be  b110              CBZ      r0,|L1.966|
0003c0  f04f30ff          MOV      r0,#0xffffffff        ;1106
0003c4  e7ee              B        |L1.932|
                  |L1.966|
0003c6  4a07              LDR      r2,|L1.996|
0003c8  6812              LDR      r2,[r2,#0]            ;1107  ; st
0003ca  7951              LDRB     r1,[r2,#5]            ;1107
0003cc  4a05              LDR      r2,|L1.996|
0003ce  6852              LDR      r2,[r2,#4]            ;1107  ; st
0003d0  7810              LDRB     r0,[r2,#0]            ;1107
0003d2  466b              MOV      r3,sp                 ;1107
0003d4  2201              MOVS     r2,#1                 ;1107
0003d6  f7fffffe          BL       MPU_Write_Len
0003da  b168              CBZ      r0,|L1.1016|
0003dc  f04f30ff          MOV      r0,#0xffffffff        ;1108
0003e0  e7e0              B        |L1.932|
0003e2  0000              DCW      0x0000
                  |L1.996|
                          DCD      ||st||
                  |L1.1000|
0003e8  25233578          DCB      "%#5x: %#5x\r\n",0
0003ec  3a202523
0003f0  35780d0a
0003f4  00      
0003f5  00                DCB      0
0003f6  00                DCB      0
0003f7  00                DCB      0
                  |L1.1016|
0003f8  4af8              LDR      r2,|L1.2012|
0003fa  6812              LDR      r2,[r2,#0]            ;1109  ; st
0003fc  7911              LDRB     r1,[r2,#4]            ;1109
0003fe  4af7              LDR      r2,|L1.2012|
000400  6852              LDR      r2,[r2,#4]            ;1109  ; st
000402  7810              LDRB     r0,[r2,#0]            ;1109
000404  466b              MOV      r3,sp                 ;1109
000406  2201              MOVS     r2,#1                 ;1109
000408  f7fffffe          BL       MPU_Write_Len
00040c  b110              CBZ      r0,|L1.1044|
00040e  f04f30ff          MOV      r0,#0xffffffff        ;1110
000412  e7c7              B        |L1.932|
                  |L1.1044|
000414  48f1              LDR      r0,|L1.2012|
000416  f8900024          LDRB     r0,[r0,#0x24]         ;1112
00041a  2800              CMP      r0,#0                 ;1112
00041c  d053              BEQ      |L1.1222|
00041e  200c              MOVS     r0,#0xc               ;1113
000420  9000              STR      r0,[sp,#0]            ;1113
000422  4aee              LDR      r2,|L1.2012|
000424  6812              LDR      r2,[r2,#0]            ;1114  ; st
000426  7911              LDRB     r1,[r2,#4]            ;1114
000428  4aec              LDR      r2,|L1.2012|
00042a  6852              LDR      r2,[r2,#4]            ;1114  ; st
00042c  7810              LDRB     r0,[r2,#0]            ;1114
00042e  466b              MOV      r3,sp                 ;1114
000430  2201              MOVS     r2,#1                 ;1114
000432  f7fffffe          BL       MPU_Write_Len
000436  b110              CBZ      r0,|L1.1086|
000438  f04f30ff          MOV      r0,#0xffffffff        ;1115
00043c  e7b2              B        |L1.932|
                  |L1.1086|
00043e  2032              MOVS     r0,#0x32              ;1116
000440  f7fffffe          BL       delay_ms
000444  20c0              MOVS     r0,#0xc0              ;1117
000446  9000              STR      r0,[sp,#0]            ;1117
000448  48e4              LDR      r0,|L1.2012|
00044a  7a80              LDRB     r0,[r0,#0xa]          ;1118
00044c  f0000001          AND      r0,r0,#1              ;1118
000450  b120              CBZ      r0,|L1.1116|
000452  f89d0000          LDRB     r0,[sp,#0]            ;1119
000456  f0400020          ORR      r0,r0,#0x20           ;1119
00045a  9000              STR      r0,[sp,#0]            ;1119
                  |L1.1116|
00045c  4adf              LDR      r2,|L1.2012|
00045e  6812              LDR      r2,[r2,#0]            ;1120  ; st
000460  7911              LDRB     r1,[r2,#4]            ;1120
000462  4ade              LDR      r2,|L1.2012|
000464  6852              LDR      r2,[r2,#4]            ;1120  ; st
000466  7810              LDRB     r0,[r2,#0]            ;1120
000468  466b              MOV      r3,sp                 ;1120
00046a  2201              MOVS     r2,#1                 ;1120
00046c  f7fffffe          BL       MPU_Write_Len
000470  b110              CBZ      r0,|L1.1144|
000472  f04f30ff          MOV      r0,#0xffffffff        ;1121
000476  e795              B        |L1.932|
                  |L1.1144|
000478  48d8              LDR      r0,|L1.2012|
00047a  7c40              LDRB     r0,[r0,#0x11]         ;1122
00047c  b110              CBZ      r0,|L1.1156|
00047e  2002              MOVS     r0,#2                 ;1123
000480  9000              STR      r0,[sp,#0]            ;1123
000482  e001              B        |L1.1160|
                  |L1.1156|
000484  2000              MOVS     r0,#0                 ;1125
000486  9000              STR      r0,[sp,#0]            ;1125
                  |L1.1160|
000488  4ad4              LDR      r2,|L1.2012|
00048a  6812              LDR      r2,[r2,#0]            ;1126  ; st
00048c  7bd1              LDRB     r1,[r2,#0xf]          ;1126
00048e  4ad3              LDR      r2,|L1.2012|
000490  6852              LDR      r2,[r2,#4]            ;1126  ; st
000492  7810              LDRB     r0,[r2,#0]            ;1126
000494  466b              MOV      r3,sp                 ;1126
000496  2201              MOVS     r2,#1                 ;1126
000498  f7fffffe          BL       MPU_Write_Len
00049c  b110              CBZ      r0,|L1.1188|
00049e  f04f30ff          MOV      r0,#0xffffffff        ;1127
0004a2  e77f              B        |L1.932|
                  |L1.1188|
0004a4  2000              MOVS     r0,#0                 ;1128
0004a6  9000              STR      r0,[sp,#0]            ;1128
0004a8  4acc              LDR      r2,|L1.2012|
0004aa  6812              LDR      r2,[r2,#0]            ;1129  ; st
0004ac  7951              LDRB     r1,[r2,#5]            ;1129
0004ae  4acb              LDR      r2,|L1.2012|
0004b0  6852              LDR      r2,[r2,#4]            ;1129  ; st
0004b2  7810              LDRB     r0,[r2,#0]            ;1129
0004b4  466b              MOV      r3,sp                 ;1129
0004b6  2201              MOVS     r2,#1                 ;1129
0004b8  f7fffffe          BL       MPU_Write_Len
0004bc  2800              CMP      r0,#0                 ;1129
0004be  d055              BEQ      |L1.1388|
0004c0  f04f30ff          MOV      r0,#0xffffffff        ;1130
0004c4  e76e              B        |L1.932|
                  |L1.1222|
0004c6  2004              MOVS     r0,#4                 ;1132
0004c8  9000              STR      r0,[sp,#0]            ;1132
0004ca  4ac4              LDR      r2,|L1.2012|
0004cc  6812              LDR      r2,[r2,#0]            ;1133  ; st
0004ce  7911              LDRB     r1,[r2,#4]            ;1133
0004d0  4ac2              LDR      r2,|L1.2012|
0004d2  6852              LDR      r2,[r2,#4]            ;1133  ; st
0004d4  7810              LDRB     r0,[r2,#0]            ;1133
0004d6  466b              MOV      r3,sp                 ;1133
0004d8  2201              MOVS     r2,#1                 ;1133
0004da  f7fffffe          BL       MPU_Write_Len
0004de  b110              CBZ      r0,|L1.1254|
0004e0  f04f30ff          MOV      r0,#0xffffffff        ;1134
0004e4  e75e              B        |L1.932|
                  |L1.1254|
0004e6  48bd              LDR      r0,|L1.2012|
0004e8  7c80              LDRB     r0,[r0,#0x12]         ;1135
0004ea  b920              CBNZ     r0,|L1.1270|
0004ec  48bb              LDR      r0,|L1.2012|
0004ee  7a80              LDRB     r0,[r0,#0xa]          ;1135
0004f0  f0000001          AND      r0,r0,#1              ;1135
0004f4  b910              CBNZ     r0,|L1.1276|
                  |L1.1270|
0004f6  2040              MOVS     r0,#0x40              ;1136
0004f8  9000              STR      r0,[sp,#0]            ;1136
0004fa  e001              B        |L1.1280|
                  |L1.1276|
0004fc  2060              MOVS     r0,#0x60              ;1138
0004fe  9000              STR      r0,[sp,#0]            ;1138
                  |L1.1280|
000500  4ab6              LDR      r2,|L1.2012|
000502  6812              LDR      r2,[r2,#0]            ;1139  ; st
000504  7911              LDRB     r1,[r2,#4]            ;1139
000506  4ab5              LDR      r2,|L1.2012|
000508  6852              LDR      r2,[r2,#4]            ;1139  ; st
00050a  7810              LDRB     r0,[r2,#0]            ;1139
00050c  466b              MOV      r3,sp                 ;1139
00050e  2201              MOVS     r2,#1                 ;1139
000510  f7fffffe          BL       MPU_Write_Len
000514  b110              CBZ      r0,|L1.1308|
000516  f04f30ff          MOV      r0,#0xffffffff        ;1140
00051a  e743              B        |L1.932|
                  |L1.1308|
00051c  2032              MOVS     r0,#0x32              ;1141
00051e  f7fffffe          BL       delay_ms
000522  48ae              LDR      r0,|L1.2012|
000524  7c40              LDRB     r0,[r0,#0x11]         ;1142
000526  b110              CBZ      r0,|L1.1326|
000528  2001              MOVS     r0,#1                 ;1143
00052a  9000              STR      r0,[sp,#0]            ;1143
00052c  e001              B        |L1.1330|
                  |L1.1326|
00052e  2000              MOVS     r0,#0                 ;1145
000530  9000              STR      r0,[sp,#0]            ;1145
                  |L1.1330|
000532  4aaa              LDR      r2,|L1.2012|
000534  6812              LDR      r2,[r2,#0]            ;1146  ; st
000536  7bd1              LDRB     r1,[r2,#0xf]          ;1146
000538  4aa8              LDR      r2,|L1.2012|
00053a  6852              LDR      r2,[r2,#4]            ;1146  ; st
00053c  7810              LDRB     r0,[r2,#0]            ;1146
00053e  466b              MOV      r3,sp                 ;1146
000540  2201              MOVS     r2,#1                 ;1146
000542  f7fffffe          BL       MPU_Write_Len
000546  b110              CBZ      r0,|L1.1358|
000548  f04f30ff          MOV      r0,#0xffffffff        ;1147
00054c  e72a              B        |L1.932|
                  |L1.1358|
00054e  4aa3              LDR      r2,|L1.2012|
000550  6812              LDR      r2,[r2,#0]            ;1148  ; st
000552  7951              LDRB     r1,[r2,#5]            ;1148
000554  4aa1              LDR      r2,|L1.2012|
000556  6852              LDR      r2,[r2,#4]            ;1148  ; st
000558  7810              LDRB     r0,[r2,#0]            ;1148
00055a  4ba0              LDR      r3,|L1.2012|
00055c  3310              ADDS     r3,r3,#0x10           ;1148
00055e  2201              MOVS     r2,#1                 ;1148
000560  f7fffffe          BL       MPU_Write_Len
000564  b110              CBZ      r0,|L1.1388|
000566  f04f30ff          MOV      r0,#0xffffffff        ;1149
00056a  e71b              B        |L1.932|
                  |L1.1388|
00056c  2000              MOVS     r0,#0                 ;1151
00056e  e719              B        |L1.932|
;;;1153   
                          ENDP

                  mpu_configure_fifo PROC
;;;1535    */
;;;1536   int mpu_configure_fifo(unsigned char sensors)
000570  b570              PUSH     {r4-r6,lr}
;;;1537   {
000572  4604              MOV      r4,r0
;;;1538       unsigned char prev;
;;;1539       int result = 0;
000574  2600              MOVS     r6,#0
;;;1540   
;;;1541       /* Compass data isn't going into the FIFO. Stop trying. */
;;;1542       sensors &= ~INV_XYZ_COMPASS;
000576  f0240401          BIC      r4,r4,#1
;;;1543   
;;;1544       if (st.chip_cfg.dmp_on)
00057a  4898              LDR      r0,|L1.2012|
00057c  f8900024          LDRB     r0,[r0,#0x24]
000580  b108              CBZ      r0,|L1.1414|
;;;1545           return 0;
000582  2000              MOVS     r0,#0
                  |L1.1412|
;;;1546       else {
;;;1547           if (!(st.chip_cfg.sensors))
;;;1548               return -1;
;;;1549           prev = st.chip_cfg.fifo_enable;
;;;1550           st.chip_cfg.fifo_enable = sensors & st.chip_cfg.sensors;
;;;1551           if (st.chip_cfg.fifo_enable != sensors)
;;;1552               /* You're not getting what you asked for. Some sensors are
;;;1553                * asleep.
;;;1554                */
;;;1555               result = -1;
;;;1556           else
;;;1557               result = 0;
;;;1558           if (sensors || st.chip_cfg.lp_accel_mode)
;;;1559               set_int_enable(1);
;;;1560           else
;;;1561               set_int_enable(0);
;;;1562           if (sensors) {
;;;1563               if (mpu_reset_fifo()) {
;;;1564                   st.chip_cfg.fifo_enable = prev;
;;;1565                   return -1;
;;;1566               }
;;;1567           }
;;;1568       }
;;;1569   
;;;1570       return result;
;;;1571   }
000584  bd70              POP      {r4-r6,pc}
                  |L1.1414|
000586  4895              LDR      r0,|L1.2012|
000588  7a80              LDRB     r0,[r0,#0xa]          ;1547
00058a  b910              CBNZ     r0,|L1.1426|
00058c  f04f30ff          MOV      r0,#0xffffffff        ;1548
000590  e7f8              B        |L1.1412|
                  |L1.1426|
000592  4892              LDR      r0,|L1.2012|
000594  7c05              LDRB     r5,[r0,#0x10]         ;1549
000596  7a80              LDRB     r0,[r0,#0xa]          ;1550
000598  4020              ANDS     r0,r0,r4              ;1550
00059a  4990              LDR      r1,|L1.2012|
00059c  7408              STRB     r0,[r1,#0x10]         ;1550
00059e  4608              MOV      r0,r1                 ;1551
0005a0  7c00              LDRB     r0,[r0,#0x10]         ;1551
0005a2  42a0              CMP      r0,r4                 ;1551
0005a4  d002              BEQ      |L1.1452|
0005a6  f04f36ff          MOV      r6,#0xffffffff        ;1555
0005aa  e000              B        |L1.1454|
                  |L1.1452|
0005ac  2600              MOVS     r6,#0                 ;1557
                  |L1.1454|
0005ae  b914              CBNZ     r4,|L1.1462|
0005b0  488a              LDR      r0,|L1.2012|
0005b2  7d00              LDRB     r0,[r0,#0x14]         ;1558
0005b4  b118              CBZ      r0,|L1.1470|
                  |L1.1462|
0005b6  2001              MOVS     r0,#1                 ;1559
0005b8  f7fffffe          BL       set_int_enable
0005bc  e002              B        |L1.1476|
                  |L1.1470|
0005be  2000              MOVS     r0,#0                 ;1561
0005c0  f7fffffe          BL       set_int_enable
                  |L1.1476|
0005c4  b13c              CBZ      r4,|L1.1494|
0005c6  f7fffffe          BL       mpu_reset_fifo
0005ca  b120              CBZ      r0,|L1.1494|
0005cc  4883              LDR      r0,|L1.2012|
0005ce  7405              STRB     r5,[r0,#0x10]         ;1564
0005d0  f04f30ff          MOV      r0,#0xffffffff        ;1565
0005d4  e7d6              B        |L1.1412|
                  |L1.1494|
0005d6  4630              MOV      r0,r6                 ;1570
0005d8  e7d4              B        |L1.1412|
;;;1572   
                          ENDP

                  mpu_set_lpf PROC
;;;1323    */
;;;1324   int mpu_set_lpf(unsigned short lpf)
0005da  b538              PUSH     {r3-r5,lr}
;;;1325   {
0005dc  4604              MOV      r4,r0
;;;1326       unsigned char data;
;;;1327   
;;;1328       if (!(st.chip_cfg.sensors))
0005de  487f              LDR      r0,|L1.2012|
0005e0  7a80              LDRB     r0,[r0,#0xa]
0005e2  b910              CBNZ     r0,|L1.1514|
;;;1329           return -1;
0005e4  f04f30ff          MOV      r0,#0xffffffff
                  |L1.1512|
;;;1330   
;;;1331       if (lpf >= 188)
;;;1332           data = INV_FILTER_188HZ;
;;;1333       else if (lpf >= 98)
;;;1334           data = INV_FILTER_98HZ;
;;;1335       else if (lpf >= 42)
;;;1336           data = INV_FILTER_42HZ;
;;;1337       else if (lpf >= 20)
;;;1338           data = INV_FILTER_20HZ;
;;;1339       else if (lpf >= 10)
;;;1340           data = INV_FILTER_10HZ;
;;;1341       else
;;;1342           data = INV_FILTER_5HZ;
;;;1343   
;;;1344       if (st.chip_cfg.lpf == data)
;;;1345           return 0;
;;;1346       if (i2c_write(st.hw->addr, st.reg->lpf, 1, &data))
;;;1347           return -1;
;;;1348       st.chip_cfg.lpf = data;
;;;1349       return 0;
;;;1350   }
0005e8  bd38              POP      {r3-r5,pc}
                  |L1.1514|
0005ea  2cbc              CMP      r4,#0xbc              ;1331
0005ec  db02              BLT      |L1.1524|
0005ee  2001              MOVS     r0,#1                 ;1332
0005f0  9000              STR      r0,[sp,#0]            ;1332
0005f2  e015              B        |L1.1568|
                  |L1.1524|
0005f4  2c62              CMP      r4,#0x62              ;1333
0005f6  db02              BLT      |L1.1534|
0005f8  2002              MOVS     r0,#2                 ;1334
0005fa  9000              STR      r0,[sp,#0]            ;1334
0005fc  e010              B        |L1.1568|
                  |L1.1534|
0005fe  2c2a              CMP      r4,#0x2a              ;1335
000600  db02              BLT      |L1.1544|
000602  2003              MOVS     r0,#3                 ;1336
000604  9000              STR      r0,[sp,#0]            ;1336
000606  e00b              B        |L1.1568|
                  |L1.1544|
000608  2c14              CMP      r4,#0x14              ;1337
00060a  db02              BLT      |L1.1554|
00060c  2004              MOVS     r0,#4                 ;1338
00060e  9000              STR      r0,[sp,#0]            ;1338
000610  e006              B        |L1.1568|
                  |L1.1554|
000612  2c0a              CMP      r4,#0xa               ;1339
000614  db02              BLT      |L1.1564|
000616  2005              MOVS     r0,#5                 ;1340
000618  9000              STR      r0,[sp,#0]            ;1340
00061a  e001              B        |L1.1568|
                  |L1.1564|
00061c  2006              MOVS     r0,#6                 ;1342
00061e  9000              STR      r0,[sp,#0]            ;1342
                  |L1.1568|
000620  486e              LDR      r0,|L1.2012|
000622  7ac0              LDRB     r0,[r0,#0xb]          ;1344
000624  f89d1000          LDRB     r1,[sp,#0]            ;1344
000628  4288              CMP      r0,r1                 ;1344
00062a  d101              BNE      |L1.1584|
00062c  2000              MOVS     r0,#0                 ;1345
00062e  e7db              B        |L1.1512|
                  |L1.1584|
000630  4a6a              LDR      r2,|L1.2012|
000632  6812              LDR      r2,[r2,#0]            ;1346  ; st
000634  7891              LDRB     r1,[r2,#2]            ;1346
000636  4a69              LDR      r2,|L1.2012|
000638  6852              LDR      r2,[r2,#4]            ;1346  ; st
00063a  7810              LDRB     r0,[r2,#0]            ;1346
00063c  466b              MOV      r3,sp                 ;1346
00063e  2201              MOVS     r2,#1                 ;1346
000640  f7fffffe          BL       MPU_Write_Len
000644  b110              CBZ      r0,|L1.1612|
000646  f04f30ff          MOV      r0,#0xffffffff        ;1347
00064a  e7cd              B        |L1.1512|
                  |L1.1612|
00064c  f89d1000          LDRB     r1,[sp,#0]            ;1348
000650  4862              LDR      r0,|L1.2012|
000652  72c1              STRB     r1,[r0,#0xb]          ;1348
000654  2000              MOVS     r0,#0                 ;1349
000656  e7c7              B        |L1.1512|
;;;1351   
                          ENDP

                  mpu_lp_accel_mode PROC
;;;892     */
;;;893    int mpu_lp_accel_mode(unsigned char rate)
000658  b538              PUSH     {r3-r5,lr}
;;;894    {
00065a  4604              MOV      r4,r0
;;;895        unsigned char tmp[2];
;;;896    
;;;897        if (rate > 40)
00065c  2c28              CMP      r4,#0x28
00065e  dd02              BLE      |L1.1638|
;;;898            return -1;
000660  f04f30ff          MOV      r0,#0xffffffff
                  |L1.1636|
;;;899    
;;;900        if (!rate) {
;;;901            mpu_set_int_latched(0);
;;;902            tmp[0] = 0;
;;;903            tmp[1] = BIT_STBY_XYZG;
;;;904            if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, tmp))
;;;905                return -1;
;;;906            st.chip_cfg.lp_accel_mode = 0;
;;;907            return 0;
;;;908        }
;;;909        /* For LP accel, we automatically configure the hardware to produce latched
;;;910         * interrupts. In LP accel mode, the hardware cycles into sleep mode before
;;;911         * it gets a chance to deassert the interrupt pin; therefore, we shift this
;;;912         * responsibility over to the MCU.
;;;913         *
;;;914         * Any register read will clear the interrupt.
;;;915         */
;;;916        mpu_set_int_latched(1);
;;;917    #if defined MPU6050
;;;918        tmp[0] = BIT_LPA_CYCLE;
;;;919        if (rate == 1) {
;;;920            tmp[1] = INV_LPA_1_25HZ;
;;;921            mpu_set_lpf(5);
;;;922        } else if (rate <= 5) {
;;;923            tmp[1] = INV_LPA_5HZ;
;;;924            mpu_set_lpf(5);
;;;925        } else if (rate <= 20) {
;;;926            tmp[1] = INV_LPA_20HZ;
;;;927            mpu_set_lpf(10);
;;;928        } else {
;;;929            tmp[1] = INV_LPA_40HZ;
;;;930            mpu_set_lpf(20);
;;;931        }
;;;932        tmp[1] = (tmp[1] << 6) | BIT_STBY_XYZG;
;;;933        if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, tmp))
;;;934            return -1;
;;;935    #elif defined MPU6500
;;;936        /* Set wake frequency. */
;;;937        if (rate == 1)
;;;938            tmp[0] = INV_LPA_1_25HZ;
;;;939        else if (rate == 2)
;;;940            tmp[0] = INV_LPA_2_5HZ;
;;;941        else if (rate <= 5)
;;;942            tmp[0] = INV_LPA_5HZ;
;;;943        else if (rate <= 10)
;;;944            tmp[0] = INV_LPA_10HZ;
;;;945        else if (rate <= 20)
;;;946            tmp[0] = INV_LPA_20HZ;
;;;947        else if (rate <= 40)
;;;948            tmp[0] = INV_LPA_40HZ;
;;;949        else if (rate <= 80)
;;;950            tmp[0] = INV_LPA_80HZ;
;;;951        else if (rate <= 160)
;;;952            tmp[0] = INV_LPA_160HZ;
;;;953        else if (rate <= 320)
;;;954            tmp[0] = INV_LPA_320HZ;
;;;955        else
;;;956            tmp[0] = INV_LPA_640HZ;
;;;957        if (i2c_write(st.hw->addr, st.reg->lp_accel_odr, 1, tmp))
;;;958            return -1;
;;;959        tmp[0] = BIT_LPA_CYCLE;
;;;960        if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, tmp))
;;;961            return -1;
;;;962    #endif
;;;963        st.chip_cfg.sensors = INV_XYZ_ACCEL;
;;;964        st.chip_cfg.clk_src = 0;
;;;965        st.chip_cfg.lp_accel_mode = 1;
;;;966        mpu_configure_fifo(0);
;;;967    
;;;968        return 0;
;;;969    }
000664  bd38              POP      {r3-r5,pc}
                  |L1.1638|
000666  b9dc              CBNZ     r4,|L1.1696|
000668  2000              MOVS     r0,#0                 ;901
00066a  f7fffffe          BL       mpu_set_int_latched
00066e  2000              MOVS     r0,#0                 ;902
000670  f88d0000          STRB     r0,[sp,#0]            ;902
000674  2007              MOVS     r0,#7                 ;903
000676  f88d0001          STRB     r0,[sp,#1]            ;903
00067a  4a58              LDR      r2,|L1.2012|
00067c  6812              LDR      r2,[r2,#0]            ;904  ; st
00067e  7c91              LDRB     r1,[r2,#0x12]         ;904
000680  4a56              LDR      r2,|L1.2012|
000682  6852              LDR      r2,[r2,#4]            ;904  ; st
000684  7810              LDRB     r0,[r2,#0]            ;904
000686  466b              MOV      r3,sp                 ;904
000688  2202              MOVS     r2,#2                 ;904
00068a  f7fffffe          BL       MPU_Write_Len
00068e  b110              CBZ      r0,|L1.1686|
000690  f04f30ff          MOV      r0,#0xffffffff        ;905
000694  e7e6              B        |L1.1636|
                  |L1.1686|
000696  2100              MOVS     r1,#0                 ;906
000698  4850              LDR      r0,|L1.2012|
00069a  7501              STRB     r1,[r0,#0x14]         ;906
00069c  2000              MOVS     r0,#0                 ;907
00069e  e7e1              B        |L1.1636|
                  |L1.1696|
0006a0  2001              MOVS     r0,#1                 ;916
0006a2  f7fffffe          BL       mpu_set_int_latched
0006a6  2020              MOVS     r0,#0x20              ;918
0006a8  f88d0000          STRB     r0,[sp,#0]            ;918
0006ac  2c01              CMP      r4,#1                 ;919
0006ae  d106              BNE      |L1.1726|
0006b0  2000              MOVS     r0,#0                 ;920
0006b2  f88d0001          STRB     r0,[sp,#1]            ;920
0006b6  2005              MOVS     r0,#5                 ;921
0006b8  f7fffffe          BL       mpu_set_lpf
0006bc  e017              B        |L1.1774|
                  |L1.1726|
0006be  2c05              CMP      r4,#5                 ;922
0006c0  dc06              BGT      |L1.1744|
0006c2  2001              MOVS     r0,#1                 ;923
0006c4  f88d0001          STRB     r0,[sp,#1]            ;923
0006c8  2005              MOVS     r0,#5                 ;924
0006ca  f7fffffe          BL       mpu_set_lpf
0006ce  e00e              B        |L1.1774|
                  |L1.1744|
0006d0  2c14              CMP      r4,#0x14              ;925
0006d2  dc06              BGT      |L1.1762|
0006d4  2002              MOVS     r0,#2                 ;926
0006d6  f88d0001          STRB     r0,[sp,#1]            ;926
0006da  200a              MOVS     r0,#0xa               ;927
0006dc  f7fffffe          BL       mpu_set_lpf
0006e0  e005              B        |L1.1774|
                  |L1.1762|
0006e2  2003              MOVS     r0,#3                 ;929
0006e4  f88d0001          STRB     r0,[sp,#1]            ;929
0006e8  2014              MOVS     r0,#0x14              ;930
0006ea  f7fffffe          BL       mpu_set_lpf
                  |L1.1774|
0006ee  f89d0001          LDRB     r0,[sp,#1]            ;932
0006f2  2107              MOVS     r1,#7                 ;932
0006f4  eb011080          ADD      r0,r1,r0,LSL #6       ;932
0006f8  b2c0              UXTB     r0,r0                 ;932
0006fa  f88d0001          STRB     r0,[sp,#1]            ;932
0006fe  4a37              LDR      r2,|L1.2012|
000700  6812              LDR      r2,[r2,#0]            ;933  ; st
000702  7c91              LDRB     r1,[r2,#0x12]         ;933
000704  4a35              LDR      r2,|L1.2012|
000706  6852              LDR      r2,[r2,#4]            ;933  ; st
000708  7810              LDRB     r0,[r2,#0]            ;933
00070a  466b              MOV      r3,sp                 ;933
00070c  2202              MOVS     r2,#2                 ;933
00070e  f7fffffe          BL       MPU_Write_Len
000712  b110              CBZ      r0,|L1.1818|
000714  f04f30ff          MOV      r0,#0xffffffff        ;934
000718  e7a4              B        |L1.1636|
                  |L1.1818|
00071a  2108              MOVS     r1,#8                 ;963
00071c  482f              LDR      r0,|L1.2012|
00071e  7281              STRB     r1,[r0,#0xa]          ;963
000720  2100              MOVS     r1,#0                 ;964
000722  7301              STRB     r1,[r0,#0xc]          ;964
000724  2101              MOVS     r1,#1                 ;965
000726  7501              STRB     r1,[r0,#0x14]         ;965
000728  2000              MOVS     r0,#0                 ;966
00072a  f7fffffe          BL       mpu_configure_fifo
00072e  2000              MOVS     r0,#0                 ;968
000730  e798              B        |L1.1636|
;;;970    
                          ENDP

                  mpu_set_sample_rate PROC
;;;1371    */
;;;1372   int mpu_set_sample_rate(unsigned short rate)
000732  b538              PUSH     {r3-r5,lr}
;;;1373   {
000734  4604              MOV      r4,r0
;;;1374       unsigned char data;
;;;1375   
;;;1376       if (!(st.chip_cfg.sensors))
000736  4829              LDR      r0,|L1.2012|
000738  7a80              LDRB     r0,[r0,#0xa]
00073a  b910              CBNZ     r0,|L1.1858|
;;;1377           return -1;
00073c  f04f30ff          MOV      r0,#0xffffffff
                  |L1.1856|
;;;1378   
;;;1379       if (st.chip_cfg.dmp_on)
;;;1380           return -1;
;;;1381       else {
;;;1382           if (st.chip_cfg.lp_accel_mode) {
;;;1383               if (rate && (rate <= 40)) {
;;;1384                   /* Just stay in low-power accel mode. */
;;;1385                   mpu_lp_accel_mode(rate);
;;;1386                   return 0;
;;;1387               }
;;;1388               /* Requested rate exceeds the allowed frequencies in LP accel mode,
;;;1389                * switch back to full-power mode.
;;;1390                */
;;;1391               mpu_lp_accel_mode(0);
;;;1392           }
;;;1393           if (rate < 4)
;;;1394               rate = 4;
;;;1395           else if (rate > 1000)
;;;1396               rate = 1000;
;;;1397   
;;;1398           data = 1000 / rate - 1;
;;;1399           if (i2c_write(st.hw->addr, st.reg->rate_div, 1, &data))
;;;1400               return -1;
;;;1401   
;;;1402           st.chip_cfg.sample_rate = 1000 / (1 + data);
;;;1403   
;;;1404   #ifdef AK89xx_SECONDARY
;;;1405           mpu_set_compass_sample_rate(min(st.chip_cfg.compass_sample_rate, MAX_COMPASS_SAMPLE_RATE));
;;;1406   #endif
;;;1407   
;;;1408           /* Automatically set LPF to 1/2 sampling rate. */
;;;1409           mpu_set_lpf(st.chip_cfg.sample_rate >> 1);
;;;1410           return 0;
;;;1411       }
;;;1412   }
000740  bd38              POP      {r3-r5,pc}
                  |L1.1858|
000742  4826              LDR      r0,|L1.2012|
000744  f8900024          LDRB     r0,[r0,#0x24]         ;1379
000748  b110              CBZ      r0,|L1.1872|
00074a  f04f30ff          MOV      r0,#0xffffffff        ;1380
00074e  e7f7              B        |L1.1856|
                  |L1.1872|
000750  4822              LDR      r0,|L1.2012|
000752  7d00              LDRB     r0,[r0,#0x14]         ;1382
000754  b150              CBZ      r0,|L1.1900|
000756  b134              CBZ      r4,|L1.1894|
000758  2c28              CMP      r4,#0x28              ;1383
00075a  dc04              BGT      |L1.1894|
00075c  b2e0              UXTB     r0,r4                 ;1385
00075e  f7fffffe          BL       mpu_lp_accel_mode
000762  2000              MOVS     r0,#0                 ;1386
000764  e7ec              B        |L1.1856|
                  |L1.1894|
000766  2000              MOVS     r0,#0                 ;1391
000768  f7fffffe          BL       mpu_lp_accel_mode
                  |L1.1900|
00076c  2c04              CMP      r4,#4                 ;1393
00076e  da01              BGE      |L1.1908|
000770  2404              MOVS     r4,#4                 ;1394
000772  e004              B        |L1.1918|
                  |L1.1908|
000774  f5b47f7a          CMP      r4,#0x3e8             ;1395
000778  dd01              BLE      |L1.1918|
00077a  f44f747a          MOV      r4,#0x3e8             ;1396
                  |L1.1918|
00077e  f44f707a          MOV      r0,#0x3e8             ;1398
000782  fb90f0f4          SDIV     r0,r0,r4              ;1398
000786  1e40              SUBS     r0,r0,#1              ;1398
000788  b2c0              UXTB     r0,r0                 ;1398
00078a  9000              STR      r0,[sp,#0]            ;1398
00078c  4a13              LDR      r2,|L1.2012|
00078e  6812              LDR      r2,[r2,#0]            ;1399  ; st
000790  7851              LDRB     r1,[r2,#1]            ;1399
000792  4a12              LDR      r2,|L1.2012|
000794  6852              LDR      r2,[r2,#4]            ;1399  ; st
000796  7810              LDRB     r0,[r2,#0]            ;1399
000798  466b              MOV      r3,sp                 ;1399
00079a  2201              MOVS     r2,#1                 ;1399
00079c  f7fffffe          BL       MPU_Write_Len
0007a0  b110              CBZ      r0,|L1.1960|
0007a2  f04f30ff          MOV      r0,#0xffffffff        ;1400
0007a6  e7cb              B        |L1.1856|
                  |L1.1960|
0007a8  f89d0000          LDRB     r0,[sp,#0]            ;1402
0007ac  1c40              ADDS     r0,r0,#1              ;1402
0007ae  f44f717a          MOV      r1,#0x3e8             ;1402
0007b2  fb91f0f0          SDIV     r0,r1,r0              ;1402
0007b6  b281              UXTH     r1,r0                 ;1402
0007b8  4808              LDR      r0,|L1.2012|
0007ba  81c1              STRH     r1,[r0,#0xe]          ;1402
0007bc  4601              MOV      r1,r0                 ;1409
0007be  89c9              LDRH     r1,[r1,#0xe]          ;1409
0007c0  1048              ASRS     r0,r1,#1              ;1409
0007c2  f7fffffe          BL       mpu_set_lpf
0007c6  2000              MOVS     r0,#0                 ;1410
0007c8  e7ba              B        |L1.1856|
;;;1413   
                          ENDP

                  mpu_set_accel_fsr PROC
;;;1250    */
;;;1251   int mpu_set_accel_fsr(unsigned char fsr)
0007ca  b538              PUSH     {r3-r5,lr}
;;;1252   {
0007cc  4604              MOV      r4,r0
;;;1253       unsigned char data;
;;;1254   
;;;1255       if (!(st.chip_cfg.sensors))
0007ce  4803              LDR      r0,|L1.2012|
0007d0  7a80              LDRB     r0,[r0,#0xa]
0007d2  b928              CBNZ     r0,|L1.2016|
;;;1256           return -1;
0007d4  f04f30ff          MOV      r0,#0xffffffff
                  |L1.2008|
;;;1257   
;;;1258       switch (fsr) {
;;;1259       case 2:
;;;1260           data = INV_FSR_2G << 3;
;;;1261           break;
;;;1262       case 4:
;;;1263           data = INV_FSR_4G << 3;
;;;1264           break;
;;;1265       case 8:
;;;1266           data = INV_FSR_8G << 3;
;;;1267           break;
;;;1268       case 16:
;;;1269           data = INV_FSR_16G << 3;
;;;1270           break;
;;;1271       default:
;;;1272           return -1;
;;;1273       }
;;;1274   
;;;1275       if (st.chip_cfg.accel_fsr == (data >> 3))
;;;1276           return 0;
;;;1277       if (i2c_write(st.hw->addr, st.reg->accel_cfg, 1, &data))
;;;1278           return -1;
;;;1279       st.chip_cfg.accel_fsr = data >> 3;
;;;1280       return 0;
;;;1281   }
0007d8  bd38              POP      {r3-r5,pc}
0007da  0000              DCW      0x0000
                  |L1.2012|
                          DCD      ||st||
                  |L1.2016|
0007e0  2c02              CMP      r4,#2                 ;1258
0007e2  d006              BEQ      |L1.2034|
0007e4  2c04              CMP      r4,#4                 ;1258
0007e6  d007              BEQ      |L1.2040|
0007e8  2c08              CMP      r4,#8                 ;1258
0007ea  d008              BEQ      |L1.2046|
0007ec  2c10              CMP      r4,#0x10              ;1258
0007ee  d10c              BNE      |L1.2058|
0007f0  e008              B        |L1.2052|
                  |L1.2034|
0007f2  2000              MOVS     r0,#0                 ;1260
0007f4  9000              STR      r0,[sp,#0]            ;1260
0007f6  e00b              B        |L1.2064|
                  |L1.2040|
0007f8  2008              MOVS     r0,#8                 ;1263
0007fa  9000              STR      r0,[sp,#0]            ;1263
0007fc  e008              B        |L1.2064|
                  |L1.2046|
0007fe  2010              MOVS     r0,#0x10              ;1266
000800  9000              STR      r0,[sp,#0]            ;1266
000802  e005              B        |L1.2064|
                  |L1.2052|
000804  2018              MOVS     r0,#0x18              ;1269
000806  9000              STR      r0,[sp,#0]            ;1269
000808  e002              B        |L1.2064|
                  |L1.2058|
00080a  f04f30ff          MOV      r0,#0xffffffff        ;1272
00080e  e7e3              B        |L1.2008|
                  |L1.2064|
000810  bf00              NOP                            ;1261
000812  48f9              LDR      r0,|L1.3064|
000814  7a40              LDRB     r0,[r0,#9]            ;1275
000816  f89d1000          LDRB     r1,[sp,#0]            ;1275
00081a  ebb00fe1          CMP      r0,r1,ASR #3          ;1275
00081e  d101              BNE      |L1.2084|
000820  2000              MOVS     r0,#0                 ;1276
000822  e7d9              B        |L1.2008|
                  |L1.2084|
000824  4af4              LDR      r2,|L1.3064|
000826  6812              LDR      r2,[r2,#0]            ;1277  ; st
000828  79d1              LDRB     r1,[r2,#7]            ;1277
00082a  4af3              LDR      r2,|L1.3064|
00082c  6852              LDR      r2,[r2,#4]            ;1277  ; st
00082e  7810              LDRB     r0,[r2,#0]            ;1277
000830  466b              MOV      r3,sp                 ;1277
000832  2201              MOVS     r2,#1                 ;1277
000834  f7fffffe          BL       MPU_Write_Len
000838  b110              CBZ      r0,|L1.2112|
00083a  f04f30ff          MOV      r0,#0xffffffff        ;1278
00083e  e7cb              B        |L1.2008|
                  |L1.2112|
000840  f89d0000          LDRB     r0,[sp,#0]            ;1279
000844  10c1              ASRS     r1,r0,#3              ;1279
000846  48ec              LDR      r0,|L1.3064|
000848  7241              STRB     r1,[r0,#9]            ;1279
00084a  2000              MOVS     r0,#0                 ;1280
00084c  e7c4              B        |L1.2008|
;;;1282   
                          ENDP

                  mpu_set_gyro_fsr PROC
;;;1185    */
;;;1186   int mpu_set_gyro_fsr(unsigned short fsr)
00084e  b538              PUSH     {r3-r5,lr}
;;;1187   {
000850  4604              MOV      r4,r0
;;;1188       unsigned char data;
;;;1189   
;;;1190       if (!(st.chip_cfg.sensors))
000852  48e9              LDR      r0,|L1.3064|
000854  7a80              LDRB     r0,[r0,#0xa]
000856  b910              CBNZ     r0,|L1.2142|
;;;1191           return -1;
000858  f04f30ff          MOV      r0,#0xffffffff
                  |L1.2140|
;;;1192   
;;;1193       switch (fsr) {
;;;1194       case 250:
;;;1195           data = INV_FSR_250DPS << 3;
;;;1196           break;
;;;1197       case 500:
;;;1198           data = INV_FSR_500DPS << 3;
;;;1199           break;
;;;1200       case 1000:
;;;1201           data = INV_FSR_1000DPS << 3;
;;;1202           break;
;;;1203       case 2000:
;;;1204           data = INV_FSR_2000DPS << 3;
;;;1205           break;
;;;1206       default:
;;;1207           return -1;
;;;1208       }
;;;1209   
;;;1210       if (st.chip_cfg.gyro_fsr == (data >> 3))
;;;1211           return 0;
;;;1212       if (i2c_write(st.hw->addr, st.reg->gyro_cfg, 1, &data))
;;;1213           return -1;
;;;1214       st.chip_cfg.gyro_fsr = data >> 3;
;;;1215       return 0;
;;;1216   }
00085c  bd38              POP      {r3-r5,pc}
                  |L1.2142|
00085e  2cfa              CMP      r4,#0xfa              ;1193
000860  d009              BEQ      |L1.2166|
000862  f5b47ffa          CMP      r4,#0x1f4             ;1193
000866  d009              BEQ      |L1.2172|
000868  f5b47f7a          CMP      r4,#0x3e8             ;1193
00086c  d009              BEQ      |L1.2178|
00086e  f5b46ffa          CMP      r4,#0x7d0             ;1193
000872  d10c              BNE      |L1.2190|
000874  e008              B        |L1.2184|
                  |L1.2166|
000876  2000              MOVS     r0,#0                 ;1195
000878  9000              STR      r0,[sp,#0]            ;1195
00087a  e00b              B        |L1.2196|
                  |L1.2172|
00087c  2008              MOVS     r0,#8                 ;1198
00087e  9000              STR      r0,[sp,#0]            ;1198
000880  e008              B        |L1.2196|
                  |L1.2178|
000882  2010              MOVS     r0,#0x10              ;1201
000884  9000              STR      r0,[sp,#0]            ;1201
000886  e005              B        |L1.2196|
                  |L1.2184|
000888  2018              MOVS     r0,#0x18              ;1204
00088a  9000              STR      r0,[sp,#0]            ;1204
00088c  e002              B        |L1.2196|
                  |L1.2190|
00088e  f04f30ff          MOV      r0,#0xffffffff        ;1207
000892  e7e3              B        |L1.2140|
                  |L1.2196|
000894  bf00              NOP                            ;1196
000896  48d8              LDR      r0,|L1.3064|
000898  7a00              LDRB     r0,[r0,#8]            ;1210  ; st
00089a  f89d1000          LDRB     r1,[sp,#0]            ;1210
00089e  ebb00fe1          CMP      r0,r1,ASR #3          ;1210
0008a2  d101              BNE      |L1.2216|
0008a4  2000              MOVS     r0,#0                 ;1211
0008a6  e7d9              B        |L1.2140|
                  |L1.2216|
0008a8  4ad3              LDR      r2,|L1.3064|
0008aa  6812              LDR      r2,[r2,#0]            ;1212  ; st
0008ac  7991              LDRB     r1,[r2,#6]            ;1212
0008ae  4ad2              LDR      r2,|L1.3064|
0008b0  6852              LDR      r2,[r2,#4]            ;1212  ; st
0008b2  7810              LDRB     r0,[r2,#0]            ;1212
0008b4  466b              MOV      r3,sp                 ;1212
0008b6  2201              MOVS     r2,#1                 ;1212
0008b8  f7fffffe          BL       MPU_Write_Len
0008bc  b110              CBZ      r0,|L1.2244|
0008be  f04f30ff          MOV      r0,#0xffffffff        ;1213
0008c2  e7cb              B        |L1.2140|
                  |L1.2244|
0008c4  f89d0000          LDRB     r0,[sp,#0]            ;1214
0008c8  10c0              ASRS     r0,r0,#3              ;1214
0008ca  49cb              LDR      r1,|L1.3064|
0008cc  7208              STRB     r0,[r1,#8]            ;1214
0008ce  2000              MOVS     r0,#0                 ;1215
0008d0  e7c4              B        |L1.2140|
;;;1217   
                          ENDP

                  mpu_init PROC
;;;760     */
;;;761    int mpu_init(void)
0008d2  b51c              PUSH     {r2-r4,lr}
;;;762    {
;;;763        unsigned char data[6], rev;
;;;764    
;;;765        /* Reset device. */
;;;766        data[0] = BIT_RESET;
0008d4  2080              MOVS     r0,#0x80
0008d6  f88d0000          STRB     r0,[sp,#0]
;;;767        if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data))
0008da  4ac7              LDR      r2,|L1.3064|
0008dc  6812              LDR      r2,[r2,#0]  ; st
0008de  7c91              LDRB     r1,[r2,#0x12]
0008e0  4ac5              LDR      r2,|L1.3064|
0008e2  6852              LDR      r2,[r2,#4]  ; st
0008e4  7810              LDRB     r0,[r2,#0]
0008e6  466b              MOV      r3,sp
0008e8  2201              MOVS     r2,#1
0008ea  f7fffffe          BL       MPU_Write_Len
0008ee  b110              CBZ      r0,|L1.2294|
;;;768    			 //uprintf(USART1," ϵͳɹ!\r\n");  
;;;769            return -1;
0008f0  f04f30ff          MOV      r0,#0xffffffff
                  |L1.2292|
;;;770        delay_ms(100);
;;;771        //
;;;772        /* Wake up chip. */
;;;773        data[0] = 0x00;
;;;774        if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data))
;;;775            return -1;
;;;776    
;;;777    #if defined MPU6050
;;;778        /* Check product revision. */
;;;779        if (i2c_read(st.hw->addr, st.reg->accel_offs, 6, data))
;;;780            return -1;
;;;781        rev = ((data[5] & 0x01) << 2) | ((data[3] & 0x01) << 1) |
;;;782            (data[1] & 0x01);
;;;783    
;;;784        if (rev) {
;;;785            /* Congrats, these parts are better. */
;;;786            if (rev == 1)
;;;787                st.chip_cfg.accel_half = 1;
;;;788            else if (rev == 2)
;;;789                st.chip_cfg.accel_half = 0;
;;;790            else {
;;;791                log_e("Unsupported software product rev %d.\n", rev);
;;;792                return -1;
;;;793            }
;;;794        } else {
;;;795            if (i2c_read(st.hw->addr, st.reg->prod_id, 1, data))
;;;796                return -1;
;;;797            rev = data[0] & 0x0F;
;;;798            if (!rev) {
;;;799                log_e("Product ID read as 0 indicates device is either "
;;;800                    "incompatible or an MPU3050.\n");
;;;801                return -1;
;;;802            } else if (rev == 4) {
;;;803                log_i("Half sensitivity part found.\n");
;;;804                st.chip_cfg.accel_half = 1;
;;;805            } else
;;;806                st.chip_cfg.accel_half = 0;
;;;807        }
;;;808    #elif defined MPU6500
;;;809    #define MPU6500_MEM_REV_ADDR    (0x17)
;;;810        if (mpu_read_mem(MPU6500_MEM_REV_ADDR, 1, &rev))
;;;811            return -1;
;;;812        if (rev == 0x1)
;;;813            st.chip_cfg.accel_half = 0;
;;;814        else {
;;;815            log_e("Unsupported software product rev %d.\n", rev);
;;;816            return -1;
;;;817        }
;;;818    
;;;819        /* MPU6500 shares 4kB of memory between the DMP and the FIFO. Since the
;;;820         * first 3kB are needed by the DMP, we'll use the last 1kB for the FIFO.
;;;821         */
;;;822        data[0] = BIT_FIFO_SIZE_1024 | 0x8;
;;;823        if (i2c_write(st.hw->addr, st.reg->accel_cfg2, 1, data))
;;;824            return -1;
;;;825    #endif
;;;826    
;;;827        /* Set to invalid values to ensure no I2C writes are skipped. */
;;;828        st.chip_cfg.sensors = 0xFF;
;;;829        st.chip_cfg.gyro_fsr = 0xFF;
;;;830        st.chip_cfg.accel_fsr = 0xFF;
;;;831        st.chip_cfg.lpf = 0xFF;
;;;832        st.chip_cfg.sample_rate = 0xFFFF;
;;;833        st.chip_cfg.fifo_enable = 0xFF;
;;;834        st.chip_cfg.bypass_mode = 0xFF;
;;;835    #ifdef AK89xx_SECONDARY
;;;836        st.chip_cfg.compass_sample_rate = 0xFFFF;
;;;837    #endif
;;;838        /* mpu_set_sensors always preserves this setting. */
;;;839        st.chip_cfg.clk_src = INV_CLK_PLL;
;;;840        /* Handled in next call to mpu_set_bypass. */
;;;841        st.chip_cfg.active_low_int = 1;
;;;842        st.chip_cfg.latched_int = 0;
;;;843        st.chip_cfg.int_motion_only = 0;
;;;844        st.chip_cfg.lp_accel_mode = 0;
;;;845        memset(&st.chip_cfg.cache, 0, sizeof(st.chip_cfg.cache));
;;;846        st.chip_cfg.dmp_on = 0;
;;;847        st.chip_cfg.dmp_loaded = 0;
;;;848        st.chip_cfg.dmp_sample_rate = 0;
;;;849    
;;;850        if (mpu_set_gyro_fsr(2000))
;;;851            return -1;
;;;852        if (mpu_set_accel_fsr(2))
;;;853            return -1;
;;;854        if (mpu_set_lpf(42))
;;;855            return -1;
;;;856        if (mpu_set_sample_rate(50))
;;;857            return -1;
;;;858        if (mpu_configure_fifo(0))
;;;859            return -1;
;;;860    
;;;861    //    if (int_param)
;;;862    //        reg_int_cb(int_param);
;;;863    
;;;864    #ifdef AK89xx_SECONDARY
;;;865        setup_compass();
;;;866        if (mpu_set_compass_sample_rate(10))
;;;867            return -1;
;;;868    #else
;;;869        /* Already disabled by setup_compass. */
;;;870        if (mpu_set_bypass(0))
;;;871            return -1;
;;;872    #endif
;;;873    
;;;874        mpu_set_sensors(0);
;;;875        return 0;
;;;876    }
0008f4  bd1c              POP      {r2-r4,pc}
                  |L1.2294|
0008f6  2064              MOVS     r0,#0x64              ;770
0008f8  f7fffffe          BL       delay_ms
0008fc  2000              MOVS     r0,#0                 ;773
0008fe  f88d0000          STRB     r0,[sp,#0]            ;773
000902  4abd              LDR      r2,|L1.3064|
000904  6812              LDR      r2,[r2,#0]            ;774  ; st
000906  7c91              LDRB     r1,[r2,#0x12]         ;774
000908  4abb              LDR      r2,|L1.3064|
00090a  6852              LDR      r2,[r2,#4]            ;774  ; st
00090c  7810              LDRB     r0,[r2,#0]            ;774
00090e  466b              MOV      r3,sp                 ;774
000910  2201              MOVS     r2,#1                 ;774
000912  f7fffffe          BL       MPU_Write_Len
000916  b110              CBZ      r0,|L1.2334|
000918  f04f30ff          MOV      r0,#0xffffffff        ;775
00091c  e7ea              B        |L1.2292|
                  |L1.2334|
00091e  4ab6              LDR      r2,|L1.3064|
000920  6812              LDR      r2,[r2,#0]            ;779  ; st
000922  7d91              LDRB     r1,[r2,#0x16]         ;779
000924  4ab4              LDR      r2,|L1.3064|
000926  6852              LDR      r2,[r2,#4]            ;779  ; st
000928  7810              LDRB     r0,[r2,#0]            ;779
00092a  466b              MOV      r3,sp                 ;779
00092c  2206              MOVS     r2,#6                 ;779
00092e  f7fffffe          BL       MPU_Read_Len
000932  b110              CBZ      r0,|L1.2362|
000934  f04f30ff          MOV      r0,#0xffffffff        ;780
000938  e7dc              B        |L1.2292|
                  |L1.2362|
00093a  f89d0005          LDRB     r0,[sp,#5]            ;781
00093e  07c0              LSLS     r0,r0,#31             ;781
000940  0f40              LSRS     r0,r0,#29             ;781
000942  f89d1003          LDRB     r1,[sp,#3]            ;781
000946  f0010101          AND      r1,r1,#1              ;781
00094a  ea400041          ORR      r0,r0,r1,LSL #1       ;781
00094e  f89d1001          LDRB     r1,[sp,#1]            ;781
000952  f0010101          AND      r1,r1,#1              ;781
000956  ea400401          ORR      r4,r0,r1              ;781
00095a  b194              CBZ      r4,|L1.2434|
00095c  2c01              CMP      r4,#1                 ;786
00095e  d103              BNE      |L1.2408|
000960  2101              MOVS     r1,#1                 ;787
000962  48a5              LDR      r0,|L1.3064|
000964  74c1              STRB     r1,[r0,#0x13]         ;787
000966  e031              B        |L1.2508|
                  |L1.2408|
000968  2c02              CMP      r4,#2                 ;788
00096a  d103              BNE      |L1.2420|
00096c  2100              MOVS     r1,#0                 ;789
00096e  48a2              LDR      r0,|L1.3064|
000970  74c1              STRB     r1,[r0,#0x13]         ;789
000972  e02b              B        |L1.2508|
                  |L1.2420|
000974  4621              MOV      r1,r4                 ;791
000976  a0a1              ADR      r0,|L1.3068|
000978  f7fffffe          BL       __2printf
00097c  f04f30ff          MOV      r0,#0xffffffff        ;792
000980  e7b8              B        |L1.2292|
                  |L1.2434|
000982  4a9d              LDR      r2,|L1.3064|
000984  6812              LDR      r2,[r2,#0]            ;795  ; st
000986  78d1              LDRB     r1,[r2,#3]            ;795
000988  4a9b              LDR      r2,|L1.3064|
00098a  6852              LDR      r2,[r2,#4]            ;795  ; st
00098c  7810              LDRB     r0,[r2,#0]            ;795
00098e  466b              MOV      r3,sp                 ;795
000990  2201              MOVS     r2,#1                 ;795
000992  f7fffffe          BL       MPU_Read_Len
000996  b110              CBZ      r0,|L1.2462|
000998  f04f30ff          MOV      r0,#0xffffffff        ;796
00099c  e7aa              B        |L1.2292|
                  |L1.2462|
00099e  f89d0000          LDRB     r0,[sp,#0]            ;797
0009a2  f000040f          AND      r4,r0,#0xf            ;797
0009a6  b92c              CBNZ     r4,|L1.2484|
0009a8  489e              LDR      r0,|L1.3108|
0009aa  f7fffffe          BL       __2printf
0009ae  f04f30ff          MOV      r0,#0xffffffff        ;801
0009b2  e79f              B        |L1.2292|
                  |L1.2484|
0009b4  2c04              CMP      r4,#4                 ;802
0009b6  d106              BNE      |L1.2502|
0009b8  a09b              ADR      r0,|L1.3112|
0009ba  f7fffffe          BL       __2printf
0009be  2101              MOVS     r1,#1                 ;804
0009c0  488d              LDR      r0,|L1.3064|
0009c2  74c1              STRB     r1,[r0,#0x13]         ;804
0009c4  e002              B        |L1.2508|
                  |L1.2502|
0009c6  2100              MOVS     r1,#0                 ;806
0009c8  488b              LDR      r0,|L1.3064|
0009ca  74c1              STRB     r1,[r0,#0x13]         ;806
                  |L1.2508|
0009cc  21ff              MOVS     r1,#0xff              ;828
0009ce  488a              LDR      r0,|L1.3064|
0009d0  7281              STRB     r1,[r0,#0xa]          ;828
0009d2  20ff              MOVS     r0,#0xff              ;829
0009d4  4988              LDR      r1,|L1.3064|
0009d6  7208              STRB     r0,[r1,#8]            ;829
0009d8  21ff              MOVS     r1,#0xff              ;830
0009da  4887              LDR      r0,|L1.3064|
0009dc  7241              STRB     r1,[r0,#9]            ;830
0009de  72c1              STRB     r1,[r0,#0xb]          ;831
0009e0  f64f71ff          MOV      r1,#0xffff            ;832
0009e4  81c1              STRH     r1,[r0,#0xe]          ;832
0009e6  21ff              MOVS     r1,#0xff              ;833
0009e8  7401              STRB     r1,[r0,#0x10]         ;833
0009ea  7481              STRB     r1,[r0,#0x12]         ;834
0009ec  2101              MOVS     r1,#1                 ;839
0009ee  7301              STRB     r1,[r0,#0xc]          ;839
0009f0  f8801022          STRB     r1,[r0,#0x22]         ;841
0009f4  2100              MOVS     r1,#0                 ;842
0009f6  f8801023          STRB     r1,[r0,#0x23]         ;842
0009fa  7541              STRB     r1,[r0,#0x15]         ;843
0009fc  7501              STRB     r1,[r0,#0x14]         ;844
0009fe  3016              ADDS     r0,r0,#0x16           ;845
000a00  6001              STR      r1,[r0,#0]            ;845
000a02  6041              STR      r1,[r0,#4]            ;845
000a04  6081              STR      r1,[r0,#8]            ;845
000a06  3816              SUBS     r0,r0,#0x16           ;846
000a08  f8801024          STRB     r1,[r0,#0x24]         ;846
000a0c  f8801025          STRB     r1,[r0,#0x25]         ;847
000a10  84c1              STRH     r1,[r0,#0x26]         ;848
000a12  f44f60fa          MOV      r0,#0x7d0             ;850
000a16  f7fffffe          BL       mpu_set_gyro_fsr
000a1a  b110              CBZ      r0,|L1.2594|
000a1c  f04f30ff          MOV      r0,#0xffffffff        ;851
000a20  e768              B        |L1.2292|
                  |L1.2594|
000a22  2002              MOVS     r0,#2                 ;852
000a24  f7fffffe          BL       mpu_set_accel_fsr
000a28  b110              CBZ      r0,|L1.2608|
000a2a  f04f30ff          MOV      r0,#0xffffffff        ;853
000a2e  e761              B        |L1.2292|
                  |L1.2608|
000a30  202a              MOVS     r0,#0x2a              ;854
000a32  f7fffffe          BL       mpu_set_lpf
000a36  b110              CBZ      r0,|L1.2622|
000a38  f04f30ff          MOV      r0,#0xffffffff        ;855
000a3c  e75a              B        |L1.2292|
                  |L1.2622|
000a3e  2032              MOVS     r0,#0x32              ;856
000a40  f7fffffe          BL       mpu_set_sample_rate
000a44  b110              CBZ      r0,|L1.2636|
000a46  f04f30ff          MOV      r0,#0xffffffff        ;857
000a4a  e753              B        |L1.2292|
                  |L1.2636|
000a4c  2000              MOVS     r0,#0                 ;858
000a4e  f7fffffe          BL       mpu_configure_fifo
000a52  b110              CBZ      r0,|L1.2650|
000a54  f04f30ff          MOV      r0,#0xffffffff        ;859
000a58  e74c              B        |L1.2292|
                  |L1.2650|
000a5a  2000              MOVS     r0,#0                 ;870
000a5c  f7fffffe          BL       mpu_set_bypass
000a60  b110              CBZ      r0,|L1.2664|
000a62  f04f30ff          MOV      r0,#0xffffffff        ;871
000a66  e745              B        |L1.2292|
                  |L1.2664|
000a68  2000              MOVS     r0,#0                 ;874
000a6a  f7fffffe          BL       mpu_set_sensors
000a6e  2000              MOVS     r0,#0                 ;875
000a70  e740              B        |L1.2292|
;;;877    
                          ENDP

                  mget_ms PROC
;;;2947   
;;;2948   void mget_ms(unsigned long *time)
000a72  4770              BX       lr
;;;2949   {
;;;2950   
;;;2951   }
;;;2952   //mpu6050,dmpʼ
                          ENDP

                  mpu_get_gyro_reg PROC
;;;976     */
;;;977    int mpu_get_gyro_reg(short *data, unsigned long *timestamp)
000a74  b57c              PUSH     {r2-r6,lr}
;;;978    {
000a76  4604              MOV      r4,r0
000a78  460d              MOV      r5,r1
;;;979        unsigned char tmp[6];
;;;980    
;;;981        if (!(st.chip_cfg.sensors & INV_XYZ_GYRO))
000a7a  485f              LDR      r0,|L1.3064|
000a7c  7a80              LDRB     r0,[r0,#0xa]
000a7e  f0000070          AND      r0,r0,#0x70
000a82  b910              CBNZ     r0,|L1.2698|
;;;982            return -1;
000a84  f04f30ff          MOV      r0,#0xffffffff
                  |L1.2696|
;;;983    
;;;984        if (i2c_read(st.hw->addr, st.reg->raw_gyro, 6, tmp))
;;;985            return -1;
;;;986        data[0] = (tmp[0] << 8) | tmp[1];
;;;987        data[1] = (tmp[2] << 8) | tmp[3];
;;;988        data[2] = (tmp[4] << 8) | tmp[5];
;;;989        if (timestamp)
;;;990            get_ms(timestamp);
;;;991        return 0;
;;;992    }
000a88  bd7c              POP      {r2-r6,pc}
                  |L1.2698|
000a8a  4a5b              LDR      r2,|L1.3064|
000a8c  6812              LDR      r2,[r2,#0]            ;984  ; st
000a8e  7b11              LDRB     r1,[r2,#0xc]          ;984
000a90  4a59              LDR      r2,|L1.3064|
000a92  6852              LDR      r2,[r2,#4]            ;984  ; st
000a94  7810              LDRB     r0,[r2,#0]            ;984
000a96  466b              MOV      r3,sp                 ;984
000a98  2206              MOVS     r2,#6                 ;984
000a9a  f7fffffe          BL       MPU_Read_Len
000a9e  b110              CBZ      r0,|L1.2726|
000aa0  f04f30ff          MOV      r0,#0xffffffff        ;985
000aa4  e7f0              B        |L1.2696|
                  |L1.2726|
000aa6  f89d0001          LDRB     r0,[sp,#1]            ;986
000aaa  f89d1000          LDRB     r1,[sp,#0]            ;986
000aae  ea402001          ORR      r0,r0,r1,LSL #8       ;986
000ab2  b200              SXTH     r0,r0                 ;986
000ab4  8020              STRH     r0,[r4,#0]            ;986
000ab6  f89d0003          LDRB     r0,[sp,#3]            ;987
000aba  f89d1002          LDRB     r1,[sp,#2]            ;987
000abe  ea402001          ORR      r0,r0,r1,LSL #8       ;987
000ac2  b200              SXTH     r0,r0                 ;987
000ac4  8060              STRH     r0,[r4,#2]            ;987
000ac6  f89d0005          LDRB     r0,[sp,#5]            ;988
000aca  f89d1004          LDRB     r1,[sp,#4]            ;988
000ace  ea402001          ORR      r0,r0,r1,LSL #8       ;988
000ad2  b200              SXTH     r0,r0                 ;988
000ad4  80a0              STRH     r0,[r4,#4]            ;988
000ad6  b115              CBZ      r5,|L1.2782|
000ad8  4628              MOV      r0,r5                 ;990
000ada  f7fffffe          BL       mget_ms
                  |L1.2782|
000ade  2000              MOVS     r0,#0                 ;991
000ae0  e7d2              B        |L1.2696|
;;;993    
                          ENDP

                  mpu_get_accel_reg PROC
;;;999     */
;;;1000   int mpu_get_accel_reg(short *data, unsigned long *timestamp)
000ae2  b57c              PUSH     {r2-r6,lr}
;;;1001   {
000ae4  4604              MOV      r4,r0
000ae6  460d              MOV      r5,r1
;;;1002       unsigned char tmp[6];
;;;1003   
;;;1004       if (!(st.chip_cfg.sensors & INV_XYZ_ACCEL))
000ae8  4843              LDR      r0,|L1.3064|
000aea  7a80              LDRB     r0,[r0,#0xa]
000aec  f0000008          AND      r0,r0,#8
000af0  b910              CBNZ     r0,|L1.2808|
;;;1005           return -1;
000af2  f04f30ff          MOV      r0,#0xffffffff
                  |L1.2806|
;;;1006   
;;;1007       if (i2c_read(st.hw->addr, st.reg->raw_accel, 6, tmp))
;;;1008           return -1;
;;;1009       data[0] = (tmp[0] << 8) | tmp[1];
;;;1010       data[1] = (tmp[2] << 8) | tmp[3];
;;;1011       data[2] = (tmp[4] << 8) | tmp[5];
;;;1012       if (timestamp)
;;;1013           get_ms(timestamp);
;;;1014       return 0;
;;;1015   }
000af6  bd7c              POP      {r2-r6,pc}
                  |L1.2808|
000af8  4a3f              LDR      r2,|L1.3064|
000afa  6812              LDR      r2,[r2,#0]            ;1007  ; st
000afc  7b51              LDRB     r1,[r2,#0xd]          ;1007
000afe  4a3e              LDR      r2,|L1.3064|
000b00  6852              LDR      r2,[r2,#4]            ;1007  ; st
000b02  7810              LDRB     r0,[r2,#0]            ;1007
000b04  466b              MOV      r3,sp                 ;1007
000b06  2206              MOVS     r2,#6                 ;1007
000b08  f7fffffe          BL       MPU_Read_Len
000b0c  b110              CBZ      r0,|L1.2836|
000b0e  f04f30ff          MOV      r0,#0xffffffff        ;1008
000b12  e7f0              B        |L1.2806|
                  |L1.2836|
000b14  f89d0001          LDRB     r0,[sp,#1]            ;1009
000b18  f89d1000          LDRB     r1,[sp,#0]            ;1009
000b1c  ea402001          ORR      r0,r0,r1,LSL #8       ;1009
000b20  b200              SXTH     r0,r0                 ;1009
000b22  8020              STRH     r0,[r4,#0]            ;1009
000b24  f89d0003          LDRB     r0,[sp,#3]            ;1010
000b28  f89d1002          LDRB     r1,[sp,#2]            ;1010
000b2c  ea402001          ORR      r0,r0,r1,LSL #8       ;1010
000b30  b200              SXTH     r0,r0                 ;1010
000b32  8060              STRH     r0,[r4,#2]            ;1010
000b34  f89d0005          LDRB     r0,[sp,#5]            ;1011
000b38  f89d1004          LDRB     r1,[sp,#4]            ;1011
000b3c  ea402001          ORR      r0,r0,r1,LSL #8       ;1011
000b40  b200              SXTH     r0,r0                 ;1011
000b42  80a0              STRH     r0,[r4,#4]            ;1011
000b44  b115              CBZ      r5,|L1.2892|
000b46  4628              MOV      r0,r5                 ;1013
000b48  f7fffffe          BL       mget_ms
                  |L1.2892|
000b4c  2000              MOVS     r0,#0                 ;1014
000b4e  e7d2              B        |L1.2806|
;;;1016   
                          ENDP

                  mpu_get_temperature PROC
;;;1022    */
;;;1023   int mpu_get_temperature(long *data, unsigned long *timestamp)
000b50  e92d4ffe          PUSH     {r1-r11,lr}
;;;1024   {
000b54  4605              MOV      r5,r0
000b56  460c              MOV      r4,r1
;;;1025       unsigned char tmp[2];
;;;1026       short raw;
;;;1027   
;;;1028       if (!(st.chip_cfg.sensors))
000b58  4827              LDR      r0,|L1.3064|
000b5a  7a80              LDRB     r0,[r0,#0xa]
000b5c  b918              CBNZ     r0,|L1.2918|
;;;1029           return -1;
000b5e  f04f30ff          MOV      r0,#0xffffffff
                  |L1.2914|
;;;1030   
;;;1031       if (i2c_read(st.hw->addr, st.reg->temp, 2, tmp))
;;;1032           return -1;
;;;1033       raw = (tmp[0] << 8) | tmp[1];
;;;1034       if (timestamp)
;;;1035           get_ms(timestamp);
;;;1036   
;;;1037       data[0] = (long)((35 + ((raw - (float)st.hw->temp_offset) / st.hw->temp_sens)) * 65536L);
;;;1038       return 0;
;;;1039   }
000b62  e8bd8ffe          POP      {r1-r11,pc}
                  |L1.2918|
000b66  4a24              LDR      r2,|L1.3064|
000b68  6812              LDR      r2,[r2,#0]            ;1031  ; st
000b6a  7b91              LDRB     r1,[r2,#0xe]          ;1031
000b6c  4a22              LDR      r2,|L1.3064|
000b6e  6852              LDR      r2,[r2,#4]            ;1031  ; st
000b70  7810              LDRB     r0,[r2,#0]            ;1031
000b72  ab02              ADD      r3,sp,#8              ;1031
000b74  2202              MOVS     r2,#2                 ;1031
000b76  f7fffffe          BL       MPU_Read_Len
000b7a  b110              CBZ      r0,|L1.2946|
000b7c  f04f30ff          MOV      r0,#0xffffffff        ;1032
000b80  e7ef              B        |L1.2914|
                  |L1.2946|
000b82  f89d0009          LDRB     r0,[sp,#9]            ;1033
000b86  f89d1008          LDRB     r1,[sp,#8]            ;1033
000b8a  ea402001          ORR      r0,r0,r1,LSL #8       ;1033
000b8e  b206              SXTH     r6,r0                 ;1033
000b90  b114              CBZ      r4,|L1.2968|
000b92  4620              MOV      r0,r4                 ;1035
000b94  f7fffffe          BL       mget_ms
                  |L1.2968|
000b98  4917              LDR      r1,|L1.3064|
000b9a  6849              LDR      r1,[r1,#4]            ;1037  ; st
000b9c  88c8              LDRH     r0,[r1,#6]            ;1037
000b9e  f7fffffe          BL       __aeabi_ui2f
000ba2  4915              LDR      r1,|L1.3064|
000ba4  9001              STR      r0,[sp,#4]            ;1037
000ba6  6849              LDR      r1,[r1,#4]            ;1037  ; st
000ba8  f9b10008          LDRSH    r0,[r1,#8]            ;1037
000bac  f7fffffe          BL       __aeabi_i2f
000bb0  4683              MOV      r11,r0                ;1037
000bb2  4630              MOV      r0,r6                 ;1037
000bb4  f7fffffe          BL       __aeabi_i2f
000bb8  4659              MOV      r1,r11                ;1037
000bba  9000              STR      r0,[sp,#0]            ;1037
000bbc  f7fffffe          BL       __aeabi_fsub
000bc0  4682              MOV      r10,r0                ;1037
000bc2  9901              LDR      r1,[sp,#4]            ;1037
000bc4  f7fffffe          BL       __aeabi_fdiv
000bc8  4681              MOV      r9,r0                 ;1037
000bca  491f              LDR      r1,|L1.3144|
000bcc  f7fffffe          BL       __aeabi_fadd
000bd0  4680              MOV      r8,r0                 ;1037
000bd2  f04f418f          MOV      r1,#0x47800000        ;1037
000bd6  f7fffffe          BL       __aeabi_fmul
000bda  4607              MOV      r7,r0                 ;1037
000bdc  f7fffffe          BL       __aeabi_f2iz
000be0  6028              STR      r0,[r5,#0]            ;1037
000be2  2000              MOVS     r0,#0                 ;1038
000be4  e7bd              B        |L1.2914|
;;;1040   
                          ENDP

                  mpu_set_accel_bias PROC
;;;1047    */
;;;1048   int mpu_set_accel_bias(const long *accel_bias)
000be6  b510              PUSH     {r4,lr}
;;;1049   {
000be8  b088              SUB      sp,sp,#0x20
000bea  4604              MOV      r4,r0
;;;1050       unsigned char data[6];
;;;1051       short accel_hw[3];
;;;1052       short got_accel[3];
;;;1053       short fg[3];
;;;1054   
;;;1055       if (!accel_bias)
000bec  b91c              CBNZ     r4,|L1.3062|
;;;1056           return -1;
000bee  f04f30ff          MOV      r0,#0xffffffff
;;;1057       if (!accel_bias[0] && !accel_bias[1] && !accel_bias[2])
;;;1058           return 0;
;;;1059   
;;;1060       if (i2c_read(st.hw->addr, 3, 3, data))
;;;1061           return -1;
;;;1062       fg[0] = ((data[0] >> 4) + 8) & 0xf;
;;;1063       fg[1] = ((data[1] >> 4) + 8) & 0xf;
;;;1064       fg[2] = ((data[2] >> 4) + 8) & 0xf;
;;;1065   
;;;1066       accel_hw[0] = (short)(accel_bias[0] * 2 / (64 + fg[0]));
;;;1067       accel_hw[1] = (short)(accel_bias[1] * 2 / (64 + fg[1]));
;;;1068       accel_hw[2] = (short)(accel_bias[2] * 2 / (64 + fg[2]));
;;;1069   
;;;1070       if (i2c_read(st.hw->addr, 0x06, 6, data))
;;;1071           return -1;
;;;1072   
;;;1073       got_accel[0] = ((short)data[0] << 8) | data[1];
;;;1074       got_accel[1] = ((short)data[2] << 8) | data[3];
;;;1075       got_accel[2] = ((short)data[4] << 8) | data[5];
;;;1076   
;;;1077       accel_hw[0] += got_accel[0];
;;;1078       accel_hw[1] += got_accel[1];
;;;1079       accel_hw[2] += got_accel[2];
;;;1080   
;;;1081       data[0] = (accel_hw[0] >> 8) & 0xff;
;;;1082       data[1] = (accel_hw[0]) & 0xff;
;;;1083       data[2] = (accel_hw[1] >> 8) & 0xff;
;;;1084       data[3] = (accel_hw[1]) & 0xff;
;;;1085       data[4] = (accel_hw[2] >> 8) & 0xff;
;;;1086       data[5] = (accel_hw[2]) & 0xff;
;;;1087   
;;;1088       if (i2c_write(st.hw->addr, 0x06, 6, data))
;;;1089           return -1;
;;;1090       return 0;
;;;1091   }
000bf2  b008              ADD      sp,sp,#0x20
000bf4  bd10              POP      {r4,pc}
                  |L1.3062|
000bf6  e029              B        |L1.3148|
                  |L1.3064|
                          DCD      ||st||
                  |L1.3068|
000bfc  556e7375          DCB      "Unsupported software product rev %d.\n",0
000c00  70706f72
000c04  74656420
000c08  736f6674
000c0c  77617265
000c10  2070726f
000c14  64756374
000c18  20726576
000c1c  2025642e
000c20  0a00    
000c22  00                DCB      0
000c23  00                DCB      0
                  |L1.3108|
                          DCD      ||.conststring||
                  |L1.3112|
000c28  48616c66          DCB      "Half sensitivity part found.\n",0
000c2c  2073656e
000c30  73697469
000c34  76697479
000c38  20706172
000c3c  7420666f
000c40  756e642e
000c44  0a00    
000c46  00                DCB      0
000c47  00                DCB      0
                  |L1.3144|
                          DCD      0x420c0000
                  |L1.3148|
000c4c  6820              LDR      r0,[r4,#0]            ;1057
000c4e  b930              CBNZ     r0,|L1.3166|
000c50  6860              LDR      r0,[r4,#4]            ;1057
000c52  b920              CBNZ     r0,|L1.3166|
000c54  68a0              LDR      r0,[r4,#8]            ;1057
000c56  b910              CBNZ     r0,|L1.3166|
000c58  2000              MOVS     r0,#0                 ;1058
                  |L1.3162|
000c5a  b008              ADD      sp,sp,#0x20
000c5c  bd10              POP      {r4,pc}
                  |L1.3166|
000c5e  49fa              LDR      r1,|L1.4168|
000c60  6849              LDR      r1,[r1,#4]            ;1060  ; st
000c62  7808              LDRB     r0,[r1,#0]            ;1060
000c64  ab06              ADD      r3,sp,#0x18           ;1060
000c66  2203              MOVS     r2,#3                 ;1060
000c68  4611              MOV      r1,r2                 ;1060
000c6a  f7fffffe          BL       MPU_Read_Len
000c6e  b110              CBZ      r0,|L1.3190|
000c70  f04f30ff          MOV      r0,#0xffffffff        ;1061
000c74  e7f1              B        |L1.3162|
                  |L1.3190|
000c76  f89d0018          LDRB     r0,[sp,#0x18]         ;1062
000c7a  2108              MOVS     r1,#8                 ;1062
000c7c  eb011020          ADD      r0,r1,r0,ASR #4       ;1062
000c80  f000000f          AND      r0,r0,#0xf            ;1062
000c84  f8ad0000          STRH     r0,[sp,#0]            ;1062
000c88  f89d0019          LDRB     r0,[sp,#0x19]         ;1063
000c8c  eb011020          ADD      r0,r1,r0,ASR #4       ;1063
000c90  f000000f          AND      r0,r0,#0xf            ;1063
000c94  f8ad0002          STRH     r0,[sp,#2]            ;1063
000c98  f89d001a          LDRB     r0,[sp,#0x1a]         ;1064
000c9c  eb011020          ADD      r0,r1,r0,ASR #4       ;1064
000ca0  f000000f          AND      r0,r0,#0xf            ;1064
000ca4  f8ad0004          STRH     r0,[sp,#4]            ;1064
000ca8  6820              LDR      r0,[r4,#0]            ;1066
000caa  0041              LSLS     r1,r0,#1              ;1066
000cac  f9bd0000          LDRSH    r0,[sp,#0]            ;1066
000cb0  3040              ADDS     r0,r0,#0x40           ;1066
000cb2  fb91f0f0          SDIV     r0,r1,r0              ;1066
000cb6  b200              SXTH     r0,r0                 ;1066
000cb8  f8ad0010          STRH     r0,[sp,#0x10]         ;1066
000cbc  6860              LDR      r0,[r4,#4]            ;1067
000cbe  0041              LSLS     r1,r0,#1              ;1067
000cc0  f9bd0002          LDRSH    r0,[sp,#2]            ;1067
000cc4  3040              ADDS     r0,r0,#0x40           ;1067
000cc6  fb91f0f0          SDIV     r0,r1,r0              ;1067
000cca  b200              SXTH     r0,r0                 ;1067
000ccc  f8ad0012          STRH     r0,[sp,#0x12]         ;1067
000cd0  68a0              LDR      r0,[r4,#8]            ;1068
000cd2  0041              LSLS     r1,r0,#1              ;1068
000cd4  f9bd0004          LDRSH    r0,[sp,#4]            ;1068
000cd8  3040              ADDS     r0,r0,#0x40           ;1068
000cda  fb91f0f0          SDIV     r0,r1,r0              ;1068
000cde  b200              SXTH     r0,r0                 ;1068
000ce0  f8ad0014          STRH     r0,[sp,#0x14]         ;1068
000ce4  49d8              LDR      r1,|L1.4168|
000ce6  6849              LDR      r1,[r1,#4]            ;1070  ; st
000ce8  7808              LDRB     r0,[r1,#0]            ;1070
000cea  ab06              ADD      r3,sp,#0x18           ;1070
000cec  2206              MOVS     r2,#6                 ;1070
000cee  4611              MOV      r1,r2                 ;1070
000cf0  f7fffffe          BL       MPU_Read_Len
000cf4  b110              CBZ      r0,|L1.3324|
000cf6  f04f30ff          MOV      r0,#0xffffffff        ;1071
000cfa  e7ae              B        |L1.3162|
                  |L1.3324|
000cfc  f89d0019          LDRB     r0,[sp,#0x19]         ;1073
000d00  f89d1018          LDRB     r1,[sp,#0x18]         ;1073
000d04  ea402001          ORR      r0,r0,r1,LSL #8       ;1073
000d08  b200              SXTH     r0,r0                 ;1073
000d0a  f8ad0008          STRH     r0,[sp,#8]            ;1073
000d0e  f89d001b          LDRB     r0,[sp,#0x1b]         ;1074
000d12  f89d101a          LDRB     r1,[sp,#0x1a]         ;1074
000d16  ea402001          ORR      r0,r0,r1,LSL #8       ;1074
000d1a  b200              SXTH     r0,r0                 ;1074
000d1c  f8ad000a          STRH     r0,[sp,#0xa]          ;1074
000d20  f89d001d          LDRB     r0,[sp,#0x1d]         ;1075
000d24  f89d101c          LDRB     r1,[sp,#0x1c]         ;1075
000d28  ea402001          ORR      r0,r0,r1,LSL #8       ;1075
000d2c  b200              SXTH     r0,r0                 ;1075
000d2e  f8ad000c          STRH     r0,[sp,#0xc]          ;1075
000d32  f8bd0010          LDRH     r0,[sp,#0x10]         ;1077
000d36  f8bd1008          LDRH     r1,[sp,#8]            ;1077
000d3a  4408              ADD      r0,r0,r1              ;1077
000d3c  b200              SXTH     r0,r0                 ;1077
000d3e  f8ad0010          STRH     r0,[sp,#0x10]         ;1077
000d42  f8bd0012          LDRH     r0,[sp,#0x12]         ;1078
000d46  f8bd100a          LDRH     r1,[sp,#0xa]          ;1078
000d4a  4408              ADD      r0,r0,r1              ;1078
000d4c  b200              SXTH     r0,r0                 ;1078
000d4e  f8ad0012          STRH     r0,[sp,#0x12]         ;1078
000d52  f8bd0014          LDRH     r0,[sp,#0x14]         ;1079
000d56  f8bd100c          LDRH     r1,[sp,#0xc]          ;1079
000d5a  4408              ADD      r0,r0,r1              ;1079
000d5c  b200              SXTH     r0,r0                 ;1079
000d5e  f8ad0014          STRH     r0,[sp,#0x14]         ;1079
000d62  f8bd0010          LDRH     r0,[sp,#0x10]         ;1081
000d66  0a00              LSRS     r0,r0,#8              ;1081
000d68  f88d0018          STRB     r0,[sp,#0x18]         ;1081
000d6c  f8bd0010          LDRH     r0,[sp,#0x10]         ;1082
000d70  b2c0              UXTB     r0,r0                 ;1082
000d72  f88d0019          STRB     r0,[sp,#0x19]         ;1082
000d76  f8bd0012          LDRH     r0,[sp,#0x12]         ;1083
000d7a  0a00              LSRS     r0,r0,#8              ;1083
000d7c  f88d001a          STRB     r0,[sp,#0x1a]         ;1083
000d80  f8bd0012          LDRH     r0,[sp,#0x12]         ;1084
000d84  b2c0              UXTB     r0,r0                 ;1084
000d86  f88d001b          STRB     r0,[sp,#0x1b]         ;1084
000d8a  f8bd0014          LDRH     r0,[sp,#0x14]         ;1085
000d8e  0a00              LSRS     r0,r0,#8              ;1085
000d90  f88d001c          STRB     r0,[sp,#0x1c]         ;1085
000d94  f8bd0014          LDRH     r0,[sp,#0x14]         ;1086
000d98  b2c0              UXTB     r0,r0                 ;1086
000d9a  f88d001d          STRB     r0,[sp,#0x1d]         ;1086
000d9e  49aa              LDR      r1,|L1.4168|
000da0  6849              LDR      r1,[r1,#4]            ;1088  ; st
000da2  7808              LDRB     r0,[r1,#0]            ;1088
000da4  ab06              ADD      r3,sp,#0x18           ;1088
000da6  2206              MOVS     r2,#6                 ;1088
000da8  4611              MOV      r1,r2                 ;1088
000daa  f7fffffe          BL       MPU_Write_Len
000dae  b110              CBZ      r0,|L1.3510|
000db0  f04f30ff          MOV      r0,#0xffffffff        ;1089
000db4  e751              B        |L1.3162|
                  |L1.3510|
000db6  2000              MOVS     r0,#0                 ;1090
000db8  e74f              B        |L1.3162|
;;;1092   
                          ENDP

                  mpu_get_gyro_fsr PROC
;;;1158    */
;;;1159   int mpu_get_gyro_fsr(unsigned short *fsr)
000dba  4601              MOV      r1,r0
;;;1160   {
;;;1161       switch (st.chip_cfg.gyro_fsr) {
000dbc  48a2              LDR      r0,|L1.4168|
000dbe  7a00              LDRB     r0,[r0,#8]  ; st
000dc0  b130              CBZ      r0,|L1.3536|
000dc2  2801              CMP      r0,#1
000dc4  d007              BEQ      |L1.3542|
000dc6  2802              CMP      r0,#2
000dc8  d009              BEQ      |L1.3550|
000dca  2803              CMP      r0,#3
000dcc  d10f              BNE      |L1.3566|
000dce  e00a              B        |L1.3558|
                  |L1.3536|
;;;1162       case INV_FSR_250DPS:
;;;1163           fsr[0] = 250;
000dd0  20fa              MOVS     r0,#0xfa
000dd2  8008              STRH     r0,[r1,#0]
;;;1164           break;
000dd4  e00e              B        |L1.3572|
                  |L1.3542|
;;;1165       case INV_FSR_500DPS:
;;;1166           fsr[0] = 500;
000dd6  f44f70fa          MOV      r0,#0x1f4
000dda  8008              STRH     r0,[r1,#0]
;;;1167           break;
000ddc  e00a              B        |L1.3572|
                  |L1.3550|
;;;1168       case INV_FSR_1000DPS:
;;;1169           fsr[0] = 1000;
000dde  f44f707a          MOV      r0,#0x3e8
000de2  8008              STRH     r0,[r1,#0]
;;;1170           break;
000de4  e006              B        |L1.3572|
                  |L1.3558|
;;;1171       case INV_FSR_2000DPS:
;;;1172           fsr[0] = 2000;
000de6  f44f60fa          MOV      r0,#0x7d0
000dea  8008              STRH     r0,[r1,#0]
;;;1173           break;
000dec  e002              B        |L1.3572|
                  |L1.3566|
;;;1174       default:
;;;1175           fsr[0] = 0;
000dee  2000              MOVS     r0,#0
000df0  8008              STRH     r0,[r1,#0]
;;;1176           break;
000df2  bf00              NOP      
                  |L1.3572|
000df4  bf00              NOP                            ;1164
;;;1177       }
;;;1178       return 0;
000df6  2000              MOVS     r0,#0
;;;1179   }
000df8  4770              BX       lr
;;;1180   
                          ENDP

                  mpu_get_accel_fsr PROC
;;;1222    */
;;;1223   int mpu_get_accel_fsr(unsigned char *fsr)
000dfa  4601              MOV      r1,r0
;;;1224   {
;;;1225       switch (st.chip_cfg.accel_fsr) {
000dfc  4892              LDR      r0,|L1.4168|
000dfe  7a40              LDRB     r0,[r0,#9]
000e00  b130              CBZ      r0,|L1.3600|
000e02  2801              CMP      r0,#1
000e04  d007              BEQ      |L1.3606|
000e06  2802              CMP      r0,#2
000e08  d008              BEQ      |L1.3612|
000e0a  2803              CMP      r0,#3
000e0c  d10c              BNE      |L1.3624|
000e0e  e008              B        |L1.3618|
                  |L1.3600|
;;;1226       case INV_FSR_2G:
;;;1227           fsr[0] = 2;
000e10  2002              MOVS     r0,#2
000e12  7008              STRB     r0,[r1,#0]
;;;1228           break;
000e14  e00b              B        |L1.3630|
                  |L1.3606|
;;;1229       case INV_FSR_4G:
;;;1230           fsr[0] = 4;
000e16  2004              MOVS     r0,#4
000e18  7008              STRB     r0,[r1,#0]
;;;1231           break;
000e1a  e008              B        |L1.3630|
                  |L1.3612|
;;;1232       case INV_FSR_8G:
;;;1233           fsr[0] = 8;
000e1c  2008              MOVS     r0,#8
000e1e  7008              STRB     r0,[r1,#0]
;;;1234           break;
000e20  e005              B        |L1.3630|
                  |L1.3618|
;;;1235       case INV_FSR_16G:
;;;1236           fsr[0] = 16;
000e22  2010              MOVS     r0,#0x10
000e24  7008              STRB     r0,[r1,#0]
;;;1237           break;
000e26  e002              B        |L1.3630|
                  |L1.3624|
;;;1238       default:
;;;1239           return -1;
000e28  f04f30ff          MOV      r0,#0xffffffff
                  |L1.3628|
;;;1240       }
;;;1241       if (st.chip_cfg.accel_half)
;;;1242           fsr[0] <<= 1;
;;;1243       return 0;
;;;1244   }
000e2c  4770              BX       lr
                  |L1.3630|
000e2e  bf00              NOP                            ;1228
000e30  4885              LDR      r0,|L1.4168|
000e32  7cc0              LDRB     r0,[r0,#0x13]         ;1241
000e34  b118              CBZ      r0,|L1.3646|
000e36  7808              LDRB     r0,[r1,#0]            ;1242
000e38  0640              LSLS     r0,r0,#25             ;1242
000e3a  0e00              LSRS     r0,r0,#24             ;1242
000e3c  7008              STRB     r0,[r1,#0]            ;1242
                  |L1.3646|
000e3e  2000              MOVS     r0,#0                 ;1243
000e40  e7f4              B        |L1.3628|
;;;1245   
                          ENDP

                  mpu_get_lpf PROC
;;;1287    */
;;;1288   int mpu_get_lpf(unsigned short *lpf)
000e42  4601              MOV      r1,r0
;;;1289   {
;;;1290       switch (st.chip_cfg.lpf) {
000e44  4880              LDR      r0,|L1.4168|
000e46  7ac0              LDRB     r0,[r0,#0xb]
000e48  2808              CMP      r0,#8
000e4a  d217              BCS      |L1.3708|
000e4c  e8dff000          TBB      [pc,r0]
000e50  1704070a          DCB      0x17,0x04,0x07,0x0a
000e54  0d101318          DCB      0x0d,0x10,0x13,0x18
;;;1291       case INV_FILTER_188HZ:
;;;1292           lpf[0] = 188;
000e58  20bc              MOVS     r0,#0xbc
000e5a  8008              STRH     r0,[r1,#0]
;;;1293           break;
000e5c  e013              B        |L1.3718|
;;;1294       case INV_FILTER_98HZ:
;;;1295           lpf[0] = 98;
000e5e  2062              MOVS     r0,#0x62
000e60  8008              STRH     r0,[r1,#0]
;;;1296           break;
000e62  e010              B        |L1.3718|
;;;1297       case INV_FILTER_42HZ:
;;;1298           lpf[0] = 42;
000e64  202a              MOVS     r0,#0x2a
000e66  8008              STRH     r0,[r1,#0]
;;;1299           break;
000e68  e00d              B        |L1.3718|
;;;1300       case INV_FILTER_20HZ:
;;;1301           lpf[0] = 20;
000e6a  2014              MOVS     r0,#0x14
000e6c  8008              STRH     r0,[r1,#0]
;;;1302           break;
000e6e  e00a              B        |L1.3718|
;;;1303       case INV_FILTER_10HZ:
;;;1304           lpf[0] = 10;
000e70  200a              MOVS     r0,#0xa
000e72  8008              STRH     r0,[r1,#0]
;;;1305           break;
000e74  e007              B        |L1.3718|
;;;1306       case INV_FILTER_5HZ:
;;;1307           lpf[0] = 5;
000e76  2005              MOVS     r0,#5
000e78  8008              STRH     r0,[r1,#0]
;;;1308           break;
000e7a  e004              B        |L1.3718|
                  |L1.3708|
;;;1309       case INV_FILTER_256HZ_NOLPF2:
000e7c  bf00              NOP      
;;;1310       case INV_FILTER_2100HZ_NOLPF:
000e7e  bf00              NOP      
;;;1311       default:
;;;1312           lpf[0] = 0;
000e80  2000              MOVS     r0,#0
000e82  8008              STRH     r0,[r1,#0]
;;;1313           break;
000e84  bf00              NOP      
                  |L1.3718|
000e86  bf00              NOP                            ;1293
;;;1314       }
;;;1315       return 0;
000e88  2000              MOVS     r0,#0
;;;1316   }
000e8a  4770              BX       lr
;;;1317   
                          ENDP

                  mpu_get_sample_rate PROC
;;;1356    */
;;;1357   int mpu_get_sample_rate(unsigned short *rate)
000e8c  4601              MOV      r1,r0
;;;1358   {
;;;1359       if (st.chip_cfg.dmp_on)
000e8e  486e              LDR      r0,|L1.4168|
000e90  f8900024          LDRB     r0,[r0,#0x24]
000e94  b110              CBZ      r0,|L1.3740|
;;;1360           return -1;
000e96  f04f30ff          MOV      r0,#0xffffffff
                  |L1.3738|
;;;1361       else
;;;1362           rate[0] = st.chip_cfg.sample_rate;
;;;1363       return 0;
;;;1364   }
000e9a  4770              BX       lr
                  |L1.3740|
000e9c  486a              LDR      r0,|L1.4168|
000e9e  89c0              LDRH     r0,[r0,#0xe]          ;1362
000ea0  8008              STRH     r0,[r1,#0]            ;1362
000ea2  2000              MOVS     r0,#0                 ;1363
000ea4  e7f9              B        |L1.3738|
;;;1365   
                          ENDP

                  mpu_get_compass_sample_rate PROC
;;;1418    */
;;;1419   int mpu_get_compass_sample_rate(unsigned short *rate)
000ea6  4601              MOV      r1,r0
;;;1420   {
;;;1421   #ifdef AK89xx_SECONDARY
;;;1422       rate[0] = st.chip_cfg.compass_sample_rate;
;;;1423       return 0;
;;;1424   #else
;;;1425       rate[0] = 0;
000ea8  2000              MOVS     r0,#0
000eaa  8008              STRH     r0,[r1,#0]
;;;1426       return -1;
000eac  1e40              SUBS     r0,r0,#1
;;;1427   #endif
;;;1428   }
000eae  4770              BX       lr
;;;1429   
                          ENDP

                  mpu_set_compass_sample_rate PROC
;;;1440    */
;;;1441   int mpu_set_compass_sample_rate(unsigned short rate)
000eb0  4601              MOV      r1,r0
;;;1442   {
;;;1443   #ifdef AK89xx_SECONDARY
;;;1444       unsigned char div;
;;;1445       if (!rate || rate > st.chip_cfg.sample_rate || rate > MAX_COMPASS_SAMPLE_RATE)
;;;1446           return -1;
;;;1447   
;;;1448       div = st.chip_cfg.sample_rate / rate - 1;
;;;1449       if (i2c_write(st.hw->addr, st.reg->s4_ctrl, 1, &div))
;;;1450           return -1;
;;;1451       st.chip_cfg.compass_sample_rate = st.chip_cfg.sample_rate / (div + 1);
;;;1452       return 0;
;;;1453   #else
;;;1454       return -1;
000eb2  f04f30ff          MOV      r0,#0xffffffff
;;;1455   #endif
;;;1456   }
000eb6  4770              BX       lr
;;;1457   
                          ENDP

                  mpu_get_gyro_sens PROC
;;;1462    */
;;;1463   int mpu_get_gyro_sens(float *sens)
000eb8  4601              MOV      r1,r0
;;;1464   {
;;;1465       switch (st.chip_cfg.gyro_fsr) {
000eba  4863              LDR      r0,|L1.4168|
000ebc  7a00              LDRB     r0,[r0,#8]  ; st
000ebe  b130              CBZ      r0,|L1.3790|
000ec0  2801              CMP      r0,#1
000ec2  d007              BEQ      |L1.3796|
000ec4  2802              CMP      r0,#2
000ec6  d008              BEQ      |L1.3802|
000ec8  2803              CMP      r0,#3
000eca  d10c              BNE      |L1.3814|
000ecc  e008              B        |L1.3808|
                  |L1.3790|
;;;1466       case INV_FSR_250DPS:
;;;1467           sens[0] = 131.f;
000ece  485f              LDR      r0,|L1.4172|
000ed0  6008              STR      r0,[r1,#0]
;;;1468           break;
000ed2  e00b              B        |L1.3820|
                  |L1.3796|
;;;1469       case INV_FSR_500DPS:
;;;1470           sens[0] = 65.5f;
000ed4  485e              LDR      r0,|L1.4176|
000ed6  6008              STR      r0,[r1,#0]
;;;1471           break;
000ed8  e008              B        |L1.3820|
                  |L1.3802|
;;;1472       case INV_FSR_1000DPS:
;;;1473           sens[0] = 32.8f;
000eda  485e              LDR      r0,|L1.4180|
000edc  6008              STR      r0,[r1,#0]
;;;1474           break;
000ede  e005              B        |L1.3820|
                  |L1.3808|
;;;1475       case INV_FSR_2000DPS:
;;;1476           sens[0] = 16.4f;
000ee0  485d              LDR      r0,|L1.4184|
000ee2  6008              STR      r0,[r1,#0]
;;;1477           break;
000ee4  e002              B        |L1.3820|
                  |L1.3814|
;;;1478       default:
;;;1479           return -1;
000ee6  f04f30ff          MOV      r0,#0xffffffff
                  |L1.3818|
;;;1480       }
;;;1481       return 0;
;;;1482   }
000eea  4770              BX       lr
                  |L1.3820|
000eec  bf00              NOP                            ;1468
000eee  2000              MOVS     r0,#0                 ;1481
000ef0  e7fb              B        |L1.3818|
;;;1483   
                          ENDP

                  mpu_get_accel_sens PROC
;;;1488    */
;;;1489   int mpu_get_accel_sens(unsigned short *sens)
000ef2  4601              MOV      r1,r0
;;;1490   {
;;;1491       switch (st.chip_cfg.accel_fsr) {
000ef4  4854              LDR      r0,|L1.4168|
000ef6  7a40              LDRB     r0,[r0,#9]
000ef8  b130              CBZ      r0,|L1.3848|
000efa  2801              CMP      r0,#1
000efc  d008              BEQ      |L1.3856|
000efe  2802              CMP      r0,#2
000f00  d00a              BEQ      |L1.3864|
000f02  2803              CMP      r0,#3
000f04  d110              BNE      |L1.3880|
000f06  e00b              B        |L1.3872|
                  |L1.3848|
;;;1492       case INV_FSR_2G:
;;;1493           sens[0] = 16384;
000f08  f44f4080          MOV      r0,#0x4000
000f0c  8008              STRH     r0,[r1,#0]
;;;1494           break;
000f0e  e00e              B        |L1.3886|
                  |L1.3856|
;;;1495       case INV_FSR_4G:
;;;1496           sens[0] = 8092;
000f10  f641709c          MOV      r0,#0x1f9c
000f14  8008              STRH     r0,[r1,#0]
;;;1497           break;
000f16  e00a              B        |L1.3886|
                  |L1.3864|
;;;1498       case INV_FSR_8G:
;;;1499           sens[0] = 4096;
000f18  f44f5080          MOV      r0,#0x1000
000f1c  8008              STRH     r0,[r1,#0]
;;;1500           break;
000f1e  e006              B        |L1.3886|
                  |L1.3872|
;;;1501       case INV_FSR_16G:
;;;1502           sens[0] = 2048;
000f20  f44f6000          MOV      r0,#0x800
000f24  8008              STRH     r0,[r1,#0]
;;;1503           break;
000f26  e002              B        |L1.3886|
                  |L1.3880|
;;;1504       default:
;;;1505           return -1;
000f28  f04f30ff          MOV      r0,#0xffffffff
                  |L1.3884|
;;;1506       }
;;;1507       if (st.chip_cfg.accel_half)
;;;1508           sens[0] >>= 1;
;;;1509       return 0;
;;;1510   }
000f2c  4770              BX       lr
                  |L1.3886|
000f2e  bf00              NOP                            ;1494
000f30  4845              LDR      r0,|L1.4168|
000f32  7cc0              LDRB     r0,[r0,#0x13]         ;1507
000f34  b110              CBZ      r0,|L1.3900|
000f36  8808              LDRH     r0,[r1,#0]            ;1508
000f38  1040              ASRS     r0,r0,#1              ;1508
000f3a  8008              STRH     r0,[r1,#0]            ;1508
                  |L1.3900|
000f3c  2000              MOVS     r0,#0                 ;1509
000f3e  e7f5              B        |L1.3884|
;;;1511   
                          ENDP

                  mpu_get_fifo_config PROC
;;;1520    */
;;;1521   int mpu_get_fifo_config(unsigned char *sensors)
000f40  4601              MOV      r1,r0
;;;1522   {
;;;1523       sensors[0] = st.chip_cfg.fifo_enable;
000f42  4841              LDR      r0,|L1.4168|
000f44  7c00              LDRB     r0,[r0,#0x10]
000f46  7008              STRB     r0,[r1,#0]
;;;1524       return 0;
000f48  2000              MOVS     r0,#0
;;;1525   }
000f4a  4770              BX       lr
;;;1526   
                          ENDP

                  mpu_get_power_state PROC
;;;1577    */
;;;1578   int mpu_get_power_state(unsigned char *power_on)
000f4c  4601              MOV      r1,r0
;;;1579   {
;;;1580       if (st.chip_cfg.sensors)
000f4e  483e              LDR      r0,|L1.4168|
000f50  7a80              LDRB     r0,[r0,#0xa]
000f52  b110              CBZ      r0,|L1.3930|
;;;1581           power_on[0] = 1;
000f54  2001              MOVS     r0,#1
000f56  7008              STRB     r0,[r1,#0]
000f58  e001              B        |L1.3934|
                  |L1.3930|
;;;1582       else
;;;1583           power_on[0] = 0;
000f5a  2000              MOVS     r0,#0
000f5c  7008              STRB     r0,[r1,#0]
                  |L1.3934|
;;;1584       return 0;
000f5e  2000              MOVS     r0,#0
;;;1585   }
000f60  4770              BX       lr
;;;1586   
                          ENDP

                  mpu_get_int_status PROC
;;;1673    */
;;;1674   int mpu_get_int_status(short *status)
000f62  b538              PUSH     {r3-r5,lr}
;;;1675   {
000f64  4604              MOV      r4,r0
;;;1676       unsigned char tmp[2];
;;;1677       if (!st.chip_cfg.sensors)
000f66  4838              LDR      r0,|L1.4168|
000f68  7a80              LDRB     r0,[r0,#0xa]
000f6a  b910              CBNZ     r0,|L1.3954|
;;;1678           return -1;
000f6c  f04f30ff          MOV      r0,#0xffffffff
                  |L1.3952|
;;;1679       if (i2c_read(st.hw->addr, st.reg->dmp_int_status, 2, tmp))
;;;1680           return -1;
;;;1681       status[0] = (tmp[0] << 8) | tmp[1];
;;;1682       return 0;
;;;1683   }
000f70  bd38              POP      {r3-r5,pc}
                  |L1.3954|
000f72  4a35              LDR      r2,|L1.4168|
000f74  6812              LDR      r2,[r2,#0]            ;1679  ; st
000f76  7c11              LDRB     r1,[r2,#0x10]         ;1679
000f78  4a33              LDR      r2,|L1.4168|
000f7a  6852              LDR      r2,[r2,#4]            ;1679  ; st
000f7c  7810              LDRB     r0,[r2,#0]            ;1679
000f7e  466b              MOV      r3,sp                 ;1679
000f80  2202              MOVS     r2,#2                 ;1679
000f82  f7fffffe          BL       MPU_Read_Len
000f86  b110              CBZ      r0,|L1.3982|
000f88  f04f30ff          MOV      r0,#0xffffffff        ;1680
000f8c  e7f0              B        |L1.3952|
                  |L1.3982|
000f8e  f89d0001          LDRB     r0,[sp,#1]            ;1681
000f92  f89d1000          LDRB     r1,[sp,#0]            ;1681
000f96  ea402001          ORR      r0,r0,r1,LSL #8       ;1681
000f9a  b200              SXTH     r0,r0                 ;1681
000f9c  8020              STRH     r0,[r4,#0]            ;1681
000f9e  2000              MOVS     r0,#0                 ;1682
000fa0  e7e6              B        |L1.3952|
;;;1684   
                          ENDP

                  mpu_read_fifo PROC
;;;1702    */
;;;1703   int mpu_read_fifo(short *gyro, short *accel, unsigned long *timestamp,
000fa2  e92d4ffe          PUSH     {r1-r11,lr}
;;;1704           unsigned char *sensors, unsigned char *more)
;;;1705   {
000fa6  4607              MOV      r7,r0
000fa8  4688              MOV      r8,r1
000faa  4692              MOV      r10,r2
000fac  461d              MOV      r5,r3
000fae  f8ddb030          LDR      r11,[sp,#0x30]
;;;1706       /* Assumes maximum packet size is gyro (6) + accel (6). */
;;;1707       unsigned char data[MAX_PACKET_LENGTH];
;;;1708       unsigned char packet_size = 0;
000fb2  2600              MOVS     r6,#0
;;;1709       unsigned short fifo_count, index = 0;
000fb4  2400              MOVS     r4,#0
;;;1710   
;;;1711       if (st.chip_cfg.dmp_on)
000fb6  4824              LDR      r0,|L1.4168|
000fb8  f8900024          LDRB     r0,[r0,#0x24]
000fbc  b110              CBZ      r0,|L1.4036|
;;;1712           return -1;
000fbe  1e60              SUBS     r0,r4,#1
                  |L1.4032|
;;;1713   
;;;1714       sensors[0] = 0;
;;;1715       if (!st.chip_cfg.sensors)
;;;1716           return -1;
;;;1717       if (!st.chip_cfg.fifo_enable)
;;;1718           return -1;
;;;1719   
;;;1720       if (st.chip_cfg.fifo_enable & INV_X_GYRO)
;;;1721           packet_size += 2;
;;;1722       if (st.chip_cfg.fifo_enable & INV_Y_GYRO)
;;;1723           packet_size += 2;
;;;1724       if (st.chip_cfg.fifo_enable & INV_Z_GYRO)
;;;1725           packet_size += 2;
;;;1726       if (st.chip_cfg.fifo_enable & INV_XYZ_ACCEL)
;;;1727           packet_size += 6;
;;;1728   
;;;1729       if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, data))
;;;1730           return -1;
;;;1731       fifo_count = (data[0] << 8) | data[1];
;;;1732       if (fifo_count < packet_size)
;;;1733           return 0;
;;;1734   //    log_i("FIFO count: %hd\n", fifo_count);
;;;1735       if (fifo_count > (st.hw->max_fifo >> 1)) {
;;;1736           /* FIFO is 50% full, better check overflow bit. */
;;;1737           if (i2c_read(st.hw->addr, st.reg->int_status, 1, data))
;;;1738               return -1;
;;;1739           if (data[0] & BIT_FIFO_OVERFLOW) {
;;;1740               mpu_reset_fifo();
;;;1741               return -2;
;;;1742           }
;;;1743       }
;;;1744       get_ms((unsigned long*)timestamp);
;;;1745   
;;;1746       if (i2c_read(st.hw->addr, st.reg->fifo_r_w, packet_size, data))
;;;1747           return -1;
;;;1748       more[0] = fifo_count / packet_size - 1;
;;;1749       sensors[0] = 0;
;;;1750   
;;;1751       if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_XYZ_ACCEL) {
;;;1752           accel[0] = (data[index+0] << 8) | data[index+1];
;;;1753           accel[1] = (data[index+2] << 8) | data[index+3];
;;;1754           accel[2] = (data[index+4] << 8) | data[index+5];
;;;1755           sensors[0] |= INV_XYZ_ACCEL;
;;;1756           index += 6;
;;;1757       }
;;;1758       if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_X_GYRO) {
;;;1759           gyro[0] = (data[index+0] << 8) | data[index+1];
;;;1760           sensors[0] |= INV_X_GYRO;
;;;1761           index += 2;
;;;1762       }
;;;1763       if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_Y_GYRO) {
;;;1764           gyro[1] = (data[index+0] << 8) | data[index+1];
;;;1765           sensors[0] |= INV_Y_GYRO;
;;;1766           index += 2;
;;;1767       }
;;;1768       if ((index != packet_size) && st.chip_cfg.fifo_enable & INV_Z_GYRO) {
;;;1769           gyro[2] = (data[index+0] << 8) | data[index+1];
;;;1770           sensors[0] |= INV_Z_GYRO;
;;;1771           index += 2;
;;;1772       }
;;;1773   
;;;1774       return 0;
;;;1775   }
000fc0  e8bd8ffe          POP      {r1-r11,pc}
                  |L1.4036|
000fc4  2000              MOVS     r0,#0                 ;1714
000fc6  7028              STRB     r0,[r5,#0]            ;1714
000fc8  481f              LDR      r0,|L1.4168|
000fca  7a80              LDRB     r0,[r0,#0xa]          ;1715
000fcc  b910              CBNZ     r0,|L1.4052|
000fce  f04f30ff          MOV      r0,#0xffffffff        ;1716
000fd2  e7f5              B        |L1.4032|
                  |L1.4052|
000fd4  481c              LDR      r0,|L1.4168|
000fd6  7c00              LDRB     r0,[r0,#0x10]         ;1717
000fd8  b910              CBNZ     r0,|L1.4064|
000fda  f04f30ff          MOV      r0,#0xffffffff        ;1718
000fde  e7ef              B        |L1.4032|
                  |L1.4064|
000fe0  4819              LDR      r0,|L1.4168|
000fe2  7c00              LDRB     r0,[r0,#0x10]         ;1720
000fe4  f0000040          AND      r0,r0,#0x40           ;1720
000fe8  b108              CBZ      r0,|L1.4078|
000fea  1cb0              ADDS     r0,r6,#2              ;1721
000fec  b2c6              UXTB     r6,r0                 ;1721
                  |L1.4078|
000fee  4816              LDR      r0,|L1.4168|
000ff0  7c00              LDRB     r0,[r0,#0x10]         ;1722
000ff2  f0000020          AND      r0,r0,#0x20           ;1722
000ff6  b108              CBZ      r0,|L1.4092|
000ff8  1cb0              ADDS     r0,r6,#2              ;1723
000ffa  b2c6              UXTB     r6,r0                 ;1723
                  |L1.4092|
000ffc  4812              LDR      r0,|L1.4168|
000ffe  7c00              LDRB     r0,[r0,#0x10]         ;1724
001000  f0000010          AND      r0,r0,#0x10           ;1724
001004  b108              CBZ      r0,|L1.4106|
001006  1cb0              ADDS     r0,r6,#2              ;1725
001008  b2c6              UXTB     r6,r0                 ;1725
                  |L1.4106|
00100a  480f              LDR      r0,|L1.4168|
00100c  7c00              LDRB     r0,[r0,#0x10]         ;1726
00100e  f0000008          AND      r0,r0,#8              ;1726
001012  b108              CBZ      r0,|L1.4120|
001014  1db0              ADDS     r0,r6,#6              ;1727
001016  b2c6              UXTB     r6,r0                 ;1727
                  |L1.4120|
001018  4a0b              LDR      r2,|L1.4168|
00101a  6812              LDR      r2,[r2,#0]            ;1729  ; st
00101c  7a91              LDRB     r1,[r2,#0xa]          ;1729
00101e  4a0a              LDR      r2,|L1.4168|
001020  6852              LDR      r2,[r2,#4]            ;1729  ; st
001022  7810              LDRB     r0,[r2,#0]            ;1729
001024  466b              MOV      r3,sp                 ;1729
001026  2202              MOVS     r2,#2                 ;1729
001028  f7fffffe          BL       MPU_Read_Len
00102c  b110              CBZ      r0,|L1.4148|
00102e  f04f30ff          MOV      r0,#0xffffffff        ;1730
001032  e7c5              B        |L1.4032|
                  |L1.4148|
001034  f89d0001          LDRB     r0,[sp,#1]            ;1731
001038  f89d1000          LDRB     r1,[sp,#0]            ;1731
00103c  ea402901          ORR      r9,r0,r1,LSL #8       ;1731
001040  45b1              CMP      r9,r6                 ;1732
001042  da0b              BGE      |L1.4188|
001044  2000              MOVS     r0,#0                 ;1733
001046  e7bb              B        |L1.4032|
                  |L1.4168|
                          DCD      ||st||
                  |L1.4172|
                          DCD      0x43030000
                  |L1.4176|
                          DCD      0x42830000
                  |L1.4180|
                          DCD      0x42033333
                  |L1.4184|
                          DCD      0x41833333
                  |L1.4188|
00105c  48fc              LDR      r0,|L1.5200|
00105e  6840              LDR      r0,[r0,#4]            ;1735  ; st
001060  8840              LDRH     r0,[r0,#2]            ;1735
001062  ebb90f60          CMP      r9,r0,ASR #1          ;1735
001066  dd17              BLE      |L1.4248|
001068  4af9              LDR      r2,|L1.5200|
00106a  6812              LDR      r2,[r2,#0]            ;1737  ; st
00106c  7c51              LDRB     r1,[r2,#0x11]         ;1737
00106e  4af8              LDR      r2,|L1.5200|
001070  6852              LDR      r2,[r2,#4]            ;1737  ; st
001072  7810              LDRB     r0,[r2,#0]            ;1737
001074  466b              MOV      r3,sp                 ;1737
001076  2201              MOVS     r2,#1                 ;1737
001078  f7fffffe          BL       MPU_Read_Len
00107c  b110              CBZ      r0,|L1.4228|
00107e  f04f30ff          MOV      r0,#0xffffffff        ;1738
001082  e79d              B        |L1.4032|
                  |L1.4228|
001084  f89d0000          LDRB     r0,[sp,#0]            ;1739
001088  f0000010          AND      r0,r0,#0x10           ;1739
00108c  b120              CBZ      r0,|L1.4248|
00108e  f7fffffe          BL       mpu_reset_fifo
001092  f06f0001          MVN      r0,#1                 ;1741
001096  e793              B        |L1.4032|
                  |L1.4248|
001098  4650              MOV      r0,r10                ;1744
00109a  f7fffffe          BL       mget_ms
00109e  4aec              LDR      r2,|L1.5200|
0010a0  6812              LDR      r2,[r2,#0]            ;1746  ; st
0010a2  7ad1              LDRB     r1,[r2,#0xb]          ;1746
0010a4  4aea              LDR      r2,|L1.5200|
0010a6  6852              LDR      r2,[r2,#4]            ;1746  ; st
0010a8  7810              LDRB     r0,[r2,#0]            ;1746
0010aa  466b              MOV      r3,sp                 ;1746
0010ac  4632              MOV      r2,r6                 ;1746
0010ae  f7fffffe          BL       MPU_Read_Len
0010b2  b110              CBZ      r0,|L1.4282|
0010b4  f04f30ff          MOV      r0,#0xffffffff        ;1747
0010b8  e782              B        |L1.4032|
                  |L1.4282|
0010ba  fb99f0f6          SDIV     r0,r9,r6              ;1748
0010be  1e40              SUBS     r0,r0,#1              ;1748
0010c0  f88b0000          STRB     r0,[r11,#0]           ;1748
0010c4  2000              MOVS     r0,#0                 ;1749
0010c6  7028              STRB     r0,[r5,#0]            ;1749
0010c8  42b4              CMP      r4,r6                 ;1751
0010ca  d02a              BEQ      |L1.4386|
0010cc  48e0              LDR      r0,|L1.5200|
0010ce  7c00              LDRB     r0,[r0,#0x10]         ;1751
0010d0  f0000008          AND      r0,r0,#8              ;1751
0010d4  b328              CBZ      r0,|L1.4386|
0010d6  1c60              ADDS     r0,r4,#1              ;1752
0010d8  f81d0000          LDRB     r0,[sp,r0]            ;1752
0010dc  f81d1004          LDRB     r1,[sp,r4]            ;1752
0010e0  ea402001          ORR      r0,r0,r1,LSL #8       ;1752
0010e4  b200              SXTH     r0,r0                 ;1752
0010e6  f8a80000          STRH     r0,[r8,#0]            ;1752
0010ea  1ce0              ADDS     r0,r4,#3              ;1753
0010ec  f81d1000          LDRB     r1,[sp,r0]            ;1753
0010f0  1ca0              ADDS     r0,r4,#2              ;1753
0010f2  f81d0000          LDRB     r0,[sp,r0]            ;1753
0010f6  ea412000          ORR      r0,r1,r0,LSL #8       ;1753
0010fa  b200              SXTH     r0,r0                 ;1753
0010fc  f8a80002          STRH     r0,[r8,#2]            ;1753
001100  1d60              ADDS     r0,r4,#5              ;1754
001102  f81d1000          LDRB     r1,[sp,r0]            ;1754
001106  1d20              ADDS     r0,r4,#4              ;1754
001108  f81d0000          LDRB     r0,[sp,r0]            ;1754
00110c  ea412000          ORR      r0,r1,r0,LSL #8       ;1754
001110  b200              SXTH     r0,r0                 ;1754
001112  f8a80004          STRH     r0,[r8,#4]            ;1754
001116  7828              LDRB     r0,[r5,#0]            ;1755
001118  f0400008          ORR      r0,r0,#8              ;1755
00111c  7028              STRB     r0,[r5,#0]            ;1755
00111e  1da0              ADDS     r0,r4,#6              ;1756
001120  b284              UXTH     r4,r0                 ;1756
                  |L1.4386|
001122  42b4              CMP      r4,r6                 ;1758
001124  d013              BEQ      |L1.4430|
001126  48ca              LDR      r0,|L1.5200|
001128  7c00              LDRB     r0,[r0,#0x10]         ;1758
00112a  f0000040          AND      r0,r0,#0x40           ;1758
00112e  b170              CBZ      r0,|L1.4430|
001130  1c60              ADDS     r0,r4,#1              ;1759
001132  f81d0000          LDRB     r0,[sp,r0]            ;1759
001136  f81d1004          LDRB     r1,[sp,r4]            ;1759
00113a  ea402001          ORR      r0,r0,r1,LSL #8       ;1759
00113e  b200              SXTH     r0,r0                 ;1759
001140  8038              STRH     r0,[r7,#0]            ;1759
001142  7828              LDRB     r0,[r5,#0]            ;1760
001144  f0400040          ORR      r0,r0,#0x40           ;1760
001148  7028              STRB     r0,[r5,#0]            ;1760
00114a  1ca0              ADDS     r0,r4,#2              ;1761
00114c  b284              UXTH     r4,r0                 ;1761
                  |L1.4430|
00114e  42b4              CMP      r4,r6                 ;1763
001150  d013              BEQ      |L1.4474|
001152  48bf              LDR      r0,|L1.5200|
001154  7c00              LDRB     r0,[r0,#0x10]         ;1763
001156  f0000020          AND      r0,r0,#0x20           ;1763
00115a  b170              CBZ      r0,|L1.4474|
00115c  1c60              ADDS     r0,r4,#1              ;1764
00115e  f81d0000          LDRB     r0,[sp,r0]            ;1764
001162  f81d1004          LDRB     r1,[sp,r4]            ;1764
001166  ea402001          ORR      r0,r0,r1,LSL #8       ;1764
00116a  b200              SXTH     r0,r0                 ;1764
00116c  8078              STRH     r0,[r7,#2]            ;1764
00116e  7828              LDRB     r0,[r5,#0]            ;1765
001170  f0400020          ORR      r0,r0,#0x20           ;1765
001174  7028              STRB     r0,[r5,#0]            ;1765
001176  1ca0              ADDS     r0,r4,#2              ;1766
001178  b284              UXTH     r4,r0                 ;1766
                  |L1.4474|
00117a  42b4              CMP      r4,r6                 ;1768
00117c  d013              BEQ      |L1.4518|
00117e  48b4              LDR      r0,|L1.5200|
001180  7c00              LDRB     r0,[r0,#0x10]         ;1768
001182  f0000010          AND      r0,r0,#0x10           ;1768
001186  b170              CBZ      r0,|L1.4518|
001188  1c60              ADDS     r0,r4,#1              ;1769
00118a  f81d0000          LDRB     r0,[sp,r0]            ;1769
00118e  f81d1004          LDRB     r1,[sp,r4]            ;1769
001192  ea402001          ORR      r0,r0,r1,LSL #8       ;1769
001196  b200              SXTH     r0,r0                 ;1769
001198  80b8              STRH     r0,[r7,#4]            ;1769
00119a  7828              LDRB     r0,[r5,#0]            ;1770
00119c  f0400010          ORR      r0,r0,#0x10           ;1770
0011a0  7028              STRB     r0,[r5,#0]            ;1770
0011a2  1ca0              ADDS     r0,r4,#2              ;1771
0011a4  b284              UXTH     r4,r0                 ;1771
                  |L1.4518|
0011a6  2000              MOVS     r0,#0                 ;1774
0011a8  e70a              B        |L1.4032|
;;;1776   
                          ENDP

                  mpu_read_fifo_stream PROC
;;;1783    */
;;;1784   int mpu_read_fifo_stream(unsigned short length, unsigned char *data,
0011aa  b5f8              PUSH     {r3-r7,lr}
;;;1785       unsigned char *more)
;;;1786   {
0011ac  4604              MOV      r4,r0
0011ae  460f              MOV      r7,r1
0011b0  4616              MOV      r6,r2
;;;1787       unsigned char tmp[2];
;;;1788       unsigned short fifo_count;
;;;1789       if (!st.chip_cfg.dmp_on)
0011b2  48a7              LDR      r0,|L1.5200|
0011b4  f8900024          LDRB     r0,[r0,#0x24]
0011b8  b910              CBNZ     r0,|L1.4544|
;;;1790           return -1;
0011ba  f04f30ff          MOV      r0,#0xffffffff
                  |L1.4542|
;;;1791       if (!st.chip_cfg.sensors)
;;;1792           return -1;
;;;1793   
;;;1794       if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, tmp))
;;;1795           return -1;
;;;1796       fifo_count = (tmp[0] << 8) | tmp[1];
;;;1797       if (fifo_count < length) {
;;;1798           more[0] = 0;
;;;1799           return -1;
;;;1800       }
;;;1801       if (fifo_count > (st.hw->max_fifo >> 1)) {
;;;1802           /* FIFO is 50% full, better check overflow bit. */
;;;1803           if (i2c_read(st.hw->addr, st.reg->int_status, 1, tmp))
;;;1804               return -1;
;;;1805           if (tmp[0] & BIT_FIFO_OVERFLOW) {
;;;1806               mpu_reset_fifo();
;;;1807               return -2;
;;;1808           }
;;;1809       }
;;;1810   
;;;1811       if (i2c_read(st.hw->addr, st.reg->fifo_r_w, length, data))
;;;1812           return -1;
;;;1813       more[0] = fifo_count / length - 1;
;;;1814       return 0;
;;;1815   }
0011be  bdf8              POP      {r3-r7,pc}
                  |L1.4544|
0011c0  48a3              LDR      r0,|L1.5200|
0011c2  7a80              LDRB     r0,[r0,#0xa]          ;1791
0011c4  b910              CBNZ     r0,|L1.4556|
0011c6  f04f30ff          MOV      r0,#0xffffffff        ;1792
0011ca  e7f8              B        |L1.4542|
                  |L1.4556|
0011cc  4aa0              LDR      r2,|L1.5200|
0011ce  6812              LDR      r2,[r2,#0]            ;1794  ; st
0011d0  7a91              LDRB     r1,[r2,#0xa]          ;1794
0011d2  4a9f              LDR      r2,|L1.5200|
0011d4  6852              LDR      r2,[r2,#4]            ;1794  ; st
0011d6  7810              LDRB     r0,[r2,#0]            ;1794
0011d8  466b              MOV      r3,sp                 ;1794
0011da  2202              MOVS     r2,#2                 ;1794
0011dc  f7fffffe          BL       MPU_Read_Len
0011e0  b110              CBZ      r0,|L1.4584|
0011e2  f04f30ff          MOV      r0,#0xffffffff        ;1795
0011e6  e7ea              B        |L1.4542|
                  |L1.4584|
0011e8  f89d0001          LDRB     r0,[sp,#1]            ;1796
0011ec  f89d1000          LDRB     r1,[sp,#0]            ;1796
0011f0  ea402501          ORR      r5,r0,r1,LSL #8       ;1796
0011f4  42a5              CMP      r5,r4                 ;1797
0011f6  da03              BGE      |L1.4608|
0011f8  2000              MOVS     r0,#0                 ;1798
0011fa  7030              STRB     r0,[r6,#0]            ;1798
0011fc  1e40              SUBS     r0,r0,#1              ;1799
0011fe  e7de              B        |L1.4542|
                  |L1.4608|
001200  4893              LDR      r0,|L1.5200|
001202  6840              LDR      r0,[r0,#4]            ;1801  ; st
001204  8840              LDRH     r0,[r0,#2]            ;1801
001206  ebb50f60          CMP      r5,r0,ASR #1          ;1801
00120a  dd17              BLE      |L1.4668|
00120c  4a90              LDR      r2,|L1.5200|
00120e  6812              LDR      r2,[r2,#0]            ;1803  ; st
001210  7c51              LDRB     r1,[r2,#0x11]         ;1803
001212  4a8f              LDR      r2,|L1.5200|
001214  6852              LDR      r2,[r2,#4]            ;1803  ; st
001216  7810              LDRB     r0,[r2,#0]            ;1803
001218  466b              MOV      r3,sp                 ;1803
00121a  2201              MOVS     r2,#1                 ;1803
00121c  f7fffffe          BL       MPU_Read_Len
001220  b110              CBZ      r0,|L1.4648|
001222  f04f30ff          MOV      r0,#0xffffffff        ;1804
001226  e7ca              B        |L1.4542|
                  |L1.4648|
001228  f89d0000          LDRB     r0,[sp,#0]            ;1805
00122c  f0000010          AND      r0,r0,#0x10           ;1805
001230  b120              CBZ      r0,|L1.4668|
001232  f7fffffe          BL       mpu_reset_fifo
001236  f06f0001          MVN      r0,#1                 ;1807
00123a  e7c0              B        |L1.4542|
                  |L1.4668|
00123c  b2e2              UXTB     r2,r4                 ;1811
00123e  4b84              LDR      r3,|L1.5200|
001240  681b              LDR      r3,[r3,#0]            ;1811  ; st
001242  7ad9              LDRB     r1,[r3,#0xb]          ;1811
001244  4b82              LDR      r3,|L1.5200|
001246  685b              LDR      r3,[r3,#4]            ;1811  ; st
001248  7818              LDRB     r0,[r3,#0]            ;1811
00124a  463b              MOV      r3,r7                 ;1811
00124c  f7fffffe          BL       MPU_Read_Len
001250  b110              CBZ      r0,|L1.4696|
001252  f04f30ff          MOV      r0,#0xffffffff        ;1812
001256  e7b2              B        |L1.4542|
                  |L1.4696|
001258  fb95f0f4          SDIV     r0,r5,r4              ;1813
00125c  1e40              SUBS     r0,r0,#1              ;1813
00125e  7030              STRB     r0,[r6,#0]            ;1813
001260  2000              MOVS     r0,#0                 ;1814
001262  e7ac              B        |L1.4542|
;;;1816   
                          ENDP

                  mpu_set_int_level PROC
;;;1871    */
;;;1872   int mpu_set_int_level(unsigned char active_low)
001264  4601              MOV      r1,r0
;;;1873   {
;;;1874       st.chip_cfg.active_low_int = active_low;
001266  487a              LDR      r0,|L1.5200|
001268  f8801022          STRB     r1,[r0,#0x22]
;;;1875       return 0;
00126c  2000              MOVS     r0,#0
;;;1876   }
00126e  4770              BX       lr
;;;1877   
                          ENDP

                  get_accel_prod_shift PROC
;;;1904   #ifdef MPU6050
;;;1905   static int get_accel_prod_shift(float *st_shift)
001270  b57c              PUSH     {r2-r6,lr}
;;;1906   {
001272  4605              MOV      r5,r0
;;;1907       unsigned char tmp[4], shift_code[3], ii;
;;;1908   
;;;1909       if (i2c_read(st.hw->addr, 0x0D, 4, tmp))
001274  4976              LDR      r1,|L1.5200|
001276  6849              LDR      r1,[r1,#4]  ; st
001278  7808              LDRB     r0,[r1,#0]
00127a  ab01              ADD      r3,sp,#4
00127c  2204              MOVS     r2,#4
00127e  210d              MOVS     r1,#0xd
001280  f7fffffe          BL       MPU_Read_Len
001284  b108              CBZ      r0,|L1.4746|
;;;1910           return 0x07;
001286  2007              MOVS     r0,#7
                  |L1.4744|
;;;1911   
;;;1912       shift_code[0] = ((tmp[0] & 0xE0) >> 3) | ((tmp[3] & 0x30) >> 4);
;;;1913       shift_code[1] = ((tmp[1] & 0xE0) >> 3) | ((tmp[3] & 0x0C) >> 2);
;;;1914       shift_code[2] = ((tmp[2] & 0xE0) >> 3) | (tmp[3] & 0x03);
;;;1915       for (ii = 0; ii < 3; ii++) {
;;;1916           if (!shift_code[ii]) {
;;;1917               st_shift[ii] = 0.f;
;;;1918               continue;
;;;1919           }
;;;1920           /* Equivalent to..
;;;1921            * st_shift[ii] = 0.34f * powf(0.92f/0.34f, (shift_code[ii]-1) / 30.f)
;;;1922            */
;;;1923           st_shift[ii] = 0.34f;
;;;1924           while (--shift_code[ii])
;;;1925               st_shift[ii] *= 1.034f;
;;;1926       }
;;;1927       return 0;
;;;1928   }
001288  bd7c              POP      {r2-r6,pc}
                  |L1.4746|
00128a  f89d0007          LDRB     r0,[sp,#7]            ;1912
00128e  f3c01001          UBFX     r0,r0,#4,#2           ;1912
001292  f89d1004          LDRB     r1,[sp,#4]            ;1912
001296  f00101e0          AND      r1,r1,#0xe0           ;1912
00129a  ea4000e1          ORR      r0,r0,r1,ASR #3       ;1912
00129e  f88d0000          STRB     r0,[sp,#0]            ;1912
0012a2  f89d0007          LDRB     r0,[sp,#7]            ;1913
0012a6  f3c00081          UBFX     r0,r0,#2,#2           ;1913
0012aa  f89d1005          LDRB     r1,[sp,#5]            ;1913
0012ae  f00101e0          AND      r1,r1,#0xe0           ;1913
0012b2  ea4000e1          ORR      r0,r0,r1,ASR #3       ;1913
0012b6  f88d0001          STRB     r0,[sp,#1]            ;1913
0012ba  f89d0007          LDRB     r0,[sp,#7]            ;1914
0012be  f0000003          AND      r0,r0,#3              ;1914
0012c2  f89d1006          LDRB     r1,[sp,#6]            ;1914
0012c6  f00101e0          AND      r1,r1,#0xe0           ;1914
0012ca  ea4000e1          ORR      r0,r0,r1,ASR #3       ;1914
0012ce  f88d0002          STRB     r0,[sp,#2]            ;1914
0012d2  2400              MOVS     r4,#0                 ;1915
0012d4  e01c              B        |L1.4880|
                  |L1.4822|
0012d6  f81d0004          LDRB     r0,[sp,r4]            ;1916
0012da  b918              CBNZ     r0,|L1.4836|
0012dc  2000              MOVS     r0,#0                 ;1917
0012de  f8450024          STR      r0,[r5,r4,LSL #2]     ;1917
0012e2  e013              B        |L1.4876|
                  |L1.4836|
0012e4  485b              LDR      r0,|L1.5204|
0012e6  f8450024          STR      r0,[r5,r4,LSL #2]     ;1923
0012ea  e006              B        |L1.4858|
                  |L1.4844|
0012ec  f8550024          LDR      r0,[r5,r4,LSL #2]     ;1925
0012f0  4959              LDR      r1,|L1.5208|
0012f2  f7fffffe          BL       __aeabi_fmul
0012f6  f8450024          STR      r0,[r5,r4,LSL #2]     ;1925
                  |L1.4858|
0012fa  f81d0004          LDRB     r0,[sp,r4]            ;1924
0012fe  1e40              SUBS     r0,r0,#1              ;1924
001300  b2c0              UXTB     r0,r0                 ;1924
001302  f80d0004          STRB     r0,[sp,r4]            ;1924
001306  2800              CMP      r0,#0                 ;1924
001308  d1f0              BNE      |L1.4844|
00130a  bf00              NOP                            ;1918
                  |L1.4876|
00130c  1c60              ADDS     r0,r4,#1              ;1915
00130e  b2c4              UXTB     r4,r0                 ;1915
                  |L1.4880|
001310  2c03              CMP      r4,#3                 ;1915
001312  dbe0              BLT      |L1.4822|
001314  2000              MOVS     r0,#0                 ;1927
001316  e7b7              B        |L1.4744|
;;;1929   
                          ENDP

                  accel_self_test PROC
;;;1930   static int accel_self_test(long *bias_regular, long *bias_st)
001318  e92d4ffe          PUSH     {r1-r11,lr}
;;;1931   {
00131c  4606              MOV      r6,r0
00131e  460f              MOV      r7,r1
;;;1932       int jj, result = 0;
001320  2500              MOVS     r5,#0
;;;1933       float st_shift[3], st_shift_cust, st_shift_var;
;;;1934   
;;;1935       get_accel_prod_shift(st_shift);
001322  4668              MOV      r0,sp
001324  f7fffffe          BL       get_accel_prod_shift
;;;1936       for(jj = 0; jj < 3; jj++) {
001328  2400              MOVS     r4,#0
00132a  e042              B        |L1.5042|
                  |L1.4908|
;;;1937           st_shift_cust = labs(bias_regular[jj] - bias_st[jj]) / 65536.f;
00132c  f8560024          LDR      r0,[r6,r4,LSL #2]
001330  f8571024          LDR      r1,[r7,r4,LSL #2]
001334  eba00a01          SUB      r10,r0,r1
001338  f1ba0f00          CMP      r10,#0
00133c  db01              BLT      |L1.4930|
00133e  4651              MOV      r1,r10
001340  e001              B        |L1.4934|
                  |L1.4930|
001342  f1ca0100          RSB      r1,r10,#0
                  |L1.4934|
001346  4608              MOV      r0,r1
001348  f7fffffe          BL       __aeabi_i2f
00134c  4683              MOV      r11,r0
00134e  f04f418f          MOV      r1,#0x47800000
001352  f7fffffe          BL       __aeabi_fdiv
001356  4680              MOV      r8,r0
;;;1938           if (st_shift[jj]) {
001358  f85d0024          LDR      r0,[sp,r4,LSL #2]
00135c  2100              MOVS     r1,#0
00135e  f7fffffe          BL       __aeabi_cfcmpeq
001362  d016              BEQ      |L1.5010|
;;;1939               st_shift_var = st_shift_cust / st_shift[jj] - 1.f;
001364  f85d1024          LDR      r1,[sp,r4,LSL #2]
001368  4640              MOV      r0,r8
00136a  f7fffffe          BL       __aeabi_fdiv
00136e  4682              MOV      r10,r0
001370  f04f517e          MOV      r1,#0x3f800000
001374  f7fffffe          BL       __aeabi_fsub
001378  4681              MOV      r9,r0
;;;1940               if (fabs(st_shift_var) > test.max_accel_var)
00137a  f0294a00          BIC      r10,r9,#0x80000000
00137e  4837              LDR      r0,|L1.5212|
001380  6a41              LDR      r1,[r0,#0x24]  ; test
001382  4650              MOV      r0,r10
001384  f7fffffe          BL       __aeabi_cfrcmple
001388  d212              BCS      |L1.5040|
;;;1941                   result |= 1 << jj;
00138a  2001              MOVS     r0,#1
00138c  40a0              LSLS     r0,r0,r4
00138e  4305              ORRS     r5,r5,r0
001390  e00e              B        |L1.5040|
                  |L1.5010|
;;;1942           } else if ((st_shift_cust < test.min_g) ||
001392  4832              LDR      r0,|L1.5212|
001394  69c1              LDR      r1,[r0,#0x1c]  ; test
001396  4640              MOV      r0,r8
001398  f7fffffe          BL       __aeabi_cfcmple
00139c  d305              BCC      |L1.5034|
;;;1943               (st_shift_cust > test.max_g))
00139e  482f              LDR      r0,|L1.5212|
0013a0  6a01              LDR      r1,[r0,#0x20]  ; test
0013a2  4640              MOV      r0,r8
0013a4  f7fffffe          BL       __aeabi_cfrcmple
0013a8  d202              BCS      |L1.5040|
                  |L1.5034|
;;;1944               result |= 1 << jj;
0013aa  2001              MOVS     r0,#1
0013ac  40a0              LSLS     r0,r0,r4
0013ae  4305              ORRS     r5,r5,r0
                  |L1.5040|
0013b0  1c64              ADDS     r4,r4,#1              ;1936
                  |L1.5042|
0013b2  2c03              CMP      r4,#3                 ;1936
0013b4  dbba              BLT      |L1.4908|
;;;1945       }
;;;1946   
;;;1947       return result;
0013b6  4628              MOV      r0,r5
;;;1948   }
0013b8  e8bd8ffe          POP      {r1-r11,pc}
;;;1949   
                          ENDP

                  gyro_self_test PROC
;;;1950   static int gyro_self_test(long *bias_regular, long *bias_st)
0013bc  e92d5ffc          PUSH     {r2-r12,lr}
;;;1951   {
0013c0  4606              MOV      r6,r0
0013c2  460f              MOV      r7,r1
;;;1952       int jj, result = 0;
0013c4  2500              MOVS     r5,#0
;;;1953       unsigned char tmp[3];
;;;1954       float st_shift, st_shift_cust, st_shift_var;
;;;1955   
;;;1956       if (i2c_read(st.hw->addr, 0x0D, 3, tmp))
0013c6  4922              LDR      r1,|L1.5200|
0013c8  6849              LDR      r1,[r1,#4]  ; st
0013ca  7808              LDRB     r0,[r1,#0]
0013cc  ab01              ADD      r3,sp,#4
0013ce  2203              MOVS     r2,#3
0013d0  210d              MOVS     r1,#0xd
0013d2  f7fffffe          BL       MPU_Read_Len
0013d6  b110              CBZ      r0,|L1.5086|
;;;1957           return 0x07;
0013d8  2007              MOVS     r0,#7
                  |L1.5082|
;;;1958   
;;;1959       tmp[0] &= 0x1F;
;;;1960       tmp[1] &= 0x1F;
;;;1961       tmp[2] &= 0x1F;
;;;1962   
;;;1963       for (jj = 0; jj < 3; jj++) {
;;;1964           st_shift_cust = labs(bias_regular[jj] - bias_st[jj]) / 65536.f;
;;;1965           if (tmp[jj]) {
;;;1966               st_shift = 3275.f / test.gyro_sens;
;;;1967               while (--tmp[jj])
;;;1968                   st_shift *= 1.046f;
;;;1969               st_shift_var = st_shift_cust / st_shift - 1.f;
;;;1970               if (fabs(st_shift_var) > test.max_gyro_var)
;;;1971                   result |= 1 << jj;
;;;1972           } else if ((st_shift_cust < test.min_dps) ||
;;;1973               (st_shift_cust > test.max_dps))
;;;1974               result |= 1 << jj;
;;;1975       }
;;;1976       return result;
;;;1977   }
0013da  e8bd9ffc          POP      {r2-r12,pc}
                  |L1.5086|
0013de  f89d0004          LDRB     r0,[sp,#4]            ;1959
0013e2  f000001f          AND      r0,r0,#0x1f           ;1959
0013e6  f88d0004          STRB     r0,[sp,#4]            ;1959
0013ea  f89d0005          LDRB     r0,[sp,#5]            ;1960
0013ee  f000001f          AND      r0,r0,#0x1f           ;1960
0013f2  f88d0005          STRB     r0,[sp,#5]            ;1960
0013f6  f89d0006          LDRB     r0,[sp,#6]            ;1961
0013fa  f000001f          AND      r0,r0,#0x1f           ;1961
0013fe  f88d0006          STRB     r0,[sp,#6]            ;1961
001402  2400              MOVS     r4,#0                 ;1963
001404  e062              B        |L1.5324|
                  |L1.5126|
001406  f8560024          LDR      r0,[r6,r4,LSL #2]     ;1964
00140a  f8571024          LDR      r1,[r7,r4,LSL #2]     ;1964
00140e  eba00b01          SUB      r11,r0,r1             ;1964
001412  f1bb0f00          CMP      r11,#0                ;1964
001416  db01              BLT      |L1.5148|
001418  4659              MOV      r1,r11                ;1964
00141a  e001              B        |L1.5152|
                  |L1.5148|
00141c  f1cb0100          RSB      r1,r11,#0             ;1964
                  |L1.5152|
001420  4608              MOV      r0,r1                 ;1964
001422  f7fffffe          BL       __aeabi_i2f
001426  f04f418f          MOV      r1,#0x47800000        ;1964
00142a  9000              STR      r0,[sp,#0]            ;1964
00142c  f7fffffe          BL       __aeabi_fdiv
001430  4681              MOV      r9,r0                 ;1964
001432  a801              ADD      r0,sp,#4              ;1965
001434  5d00              LDRB     r0,[r0,r4]            ;1965
001436  b3c0              CBZ      r0,|L1.5290|
001438  4908              LDR      r1,|L1.5212|
00143a  6808              LDR      r0,[r1,#0]            ;1966  ; test
00143c  f7fffffe          BL       __aeabi_ui2f
001440  4683              MOV      r11,r0                ;1966
001442  4659              MOV      r1,r11                ;1966
001444  4806              LDR      r0,|L1.5216|
001446  f7fffffe          BL       __aeabi_fdiv
00144a  4680              MOV      r8,r0                 ;1966
00144c  e00f              B        |L1.5230|
00144e  0000              DCW      0x0000
                  |L1.5200|
                          DCD      ||st||
                  |L1.5204|
                          DCD      0x3eae147b
                  |L1.5208|
                          DCD      0x3f845a1d
                  |L1.5212|
                          DCD      test
                  |L1.5216|
                          DCD      0x454cb000
                  |L1.5220|
001464  49fe              LDR      r1,|L1.6240|
001466  4640              MOV      r0,r8                 ;1968
001468  f7fffffe          BL       __aeabi_fmul
00146c  4680              MOV      r8,r0                 ;1968
                  |L1.5230|
00146e  a801              ADD      r0,sp,#4              ;1967
001470  5d00              LDRB     r0,[r0,r4]            ;1967
001472  1e40              SUBS     r0,r0,#1              ;1967
001474  b2c0              UXTB     r0,r0                 ;1967
001476  a901              ADD      r1,sp,#4              ;1967
001478  5508              STRB     r0,[r1,r4]            ;1967
00147a  2800              CMP      r0,#0                 ;1967
00147c  d1f2              BNE      |L1.5220|
00147e  4641              MOV      r1,r8                 ;1969
001480  4648              MOV      r0,r9                 ;1969
001482  f7fffffe          BL       __aeabi_fdiv
001486  4683              MOV      r11,r0                ;1969
001488  f04f517e          MOV      r1,#0x3f800000        ;1969
00148c  f7fffffe          BL       __aeabi_fsub
001490  4682              MOV      r10,r0                ;1969
001492  f02a4b00          BIC      r11,r10,#0x80000000   ;1970
001496  48f3              LDR      r0,|L1.6244|
001498  6981              LDR      r1,[r0,#0x18]         ;1970  ; test
00149a  4658              MOV      r0,r11                ;1970
00149c  f7fffffe          BL       __aeabi_cfrcmple
0014a0  d213              BCS      |L1.5322|
0014a2  2001              MOVS     r0,#1                 ;1971
0014a4  40a0              LSLS     r0,r0,r4              ;1971
0014a6  4305              ORRS     r5,r5,r0              ;1971
0014a8  e00f              B        |L1.5322|
                  |L1.5290|
0014aa  e7ff              B        |L1.5292|
                  |L1.5292|
0014ac  48ed              LDR      r0,|L1.6244|
0014ae  6901              LDR      r1,[r0,#0x10]         ;1972  ; test
0014b0  4648              MOV      r0,r9                 ;1972
0014b2  f7fffffe          BL       __aeabi_cfcmple
0014b6  d305              BCC      |L1.5316|
0014b8  48ea              LDR      r0,|L1.6244|
0014ba  6941              LDR      r1,[r0,#0x14]         ;1973  ; test
0014bc  4648              MOV      r0,r9                 ;1973
0014be  f7fffffe          BL       __aeabi_cfrcmple
0014c2  d202              BCS      |L1.5322|
                  |L1.5316|
0014c4  2001              MOVS     r0,#1                 ;1974
0014c6  40a0              LSLS     r0,r0,r4              ;1974
0014c8  4305              ORRS     r5,r5,r0              ;1974
                  |L1.5322|
0014ca  1c64              ADDS     r4,r4,#1              ;1963
                  |L1.5324|
0014cc  2c03              CMP      r4,#3                 ;1963
0014ce  db9a              BLT      |L1.5126|
0014d0  4628              MOV      r0,r5                 ;1976
0014d2  e782              B        |L1.5082|
;;;1978   
                          ENDP

                  get_st_biases PROC
;;;2033   
;;;2034   static int get_st_biases(long *gyro, long *accel, unsigned char hw_test)
0014d4  e92d4ff0          PUSH     {r4-r11,lr}
;;;2035   {
0014d8  b087              SUB      sp,sp,#0x1c
0014da  4605              MOV      r5,r0
0014dc  460c              MOV      r4,r1
0014de  4691              MOV      r9,r2
;;;2036       unsigned char data[MAX_PACKET_LENGTH];
;;;2037       unsigned char packet_count, ii;
;;;2038       unsigned short fifo_count;
;;;2039   
;;;2040       data[0] = 0x01;
0014e0  2001              MOVS     r0,#1
0014e2  f88d0010          STRB     r0,[sp,#0x10]
;;;2041       data[1] = 0;
0014e6  2000              MOVS     r0,#0
0014e8  f88d0011          STRB     r0,[sp,#0x11]
;;;2042       if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, data))
0014ec  4ade              LDR      r2,|L1.6248|
0014ee  6812              LDR      r2,[r2,#0]  ; st
0014f0  7c91              LDRB     r1,[r2,#0x12]
0014f2  4add              LDR      r2,|L1.6248|
0014f4  6852              LDR      r2,[r2,#4]  ; st
0014f6  7810              LDRB     r0,[r2,#0]
0014f8  ab04              ADD      r3,sp,#0x10
0014fa  2202              MOVS     r2,#2
0014fc  f7fffffe          BL       MPU_Write_Len
001500  b120              CBZ      r0,|L1.5388|
;;;2043           return -1;
001502  f04f30ff          MOV      r0,#0xffffffff
                  |L1.5382|
;;;2044       delay_ms(200);
;;;2045       data[0] = 0;
;;;2046       if (i2c_write(st.hw->addr, st.reg->int_enable, 1, data))
;;;2047           return -1;
;;;2048       if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data))
;;;2049           return -1;
;;;2050       if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data))
;;;2051           return -1;
;;;2052       if (i2c_write(st.hw->addr, st.reg->i2c_mst, 1, data))
;;;2053           return -1;
;;;2054       if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data))
;;;2055           return -1;
;;;2056       data[0] = BIT_FIFO_RST | BIT_DMP_RST;
;;;2057       if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data))
;;;2058           return -1;
;;;2059       delay_ms(15);
;;;2060       data[0] = st.test->reg_lpf;
;;;2061       if (i2c_write(st.hw->addr, st.reg->lpf, 1, data))
;;;2062           return -1;
;;;2063       data[0] = st.test->reg_rate_div;
;;;2064       if (i2c_write(st.hw->addr, st.reg->rate_div, 1, data))
;;;2065           return -1;
;;;2066       if (hw_test)
;;;2067           data[0] = st.test->reg_gyro_fsr | 0xE0;
;;;2068       else
;;;2069           data[0] = st.test->reg_gyro_fsr;
;;;2070       if (i2c_write(st.hw->addr, st.reg->gyro_cfg, 1, data))
;;;2071           return -1;
;;;2072   
;;;2073       if (hw_test)
;;;2074           data[0] = st.test->reg_accel_fsr | 0xE0;
;;;2075       else
;;;2076           data[0] = test.reg_accel_fsr;
;;;2077       if (i2c_write(st.hw->addr, st.reg->accel_cfg, 1, data))
;;;2078           return -1;
;;;2079       if (hw_test)
;;;2080           delay_ms(200);
;;;2081   
;;;2082       /* Fill FIFO for test.wait_ms milliseconds. */
;;;2083       data[0] = BIT_FIFO_EN;
;;;2084       if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, data))
;;;2085           return -1;
;;;2086   
;;;2087       data[0] = INV_XYZ_GYRO | INV_XYZ_ACCEL;
;;;2088       if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data))
;;;2089           return -1;
;;;2090       delay_ms(test.wait_ms);
;;;2091       data[0] = 0;
;;;2092       if (i2c_write(st.hw->addr, st.reg->fifo_en, 1, data))
;;;2093           return -1;
;;;2094   
;;;2095       if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, data))
;;;2096           return -1;
;;;2097   
;;;2098       fifo_count = (data[0] << 8) | data[1];
;;;2099       packet_count = fifo_count / MAX_PACKET_LENGTH;
;;;2100       gyro[0] = gyro[1] = gyro[2] = 0;
;;;2101       accel[0] = accel[1] = accel[2] = 0;
;;;2102   
;;;2103       for (ii = 0; ii < packet_count; ii++) {
;;;2104           short accel_cur[3], gyro_cur[3];
;;;2105           if (i2c_read(st.hw->addr, st.reg->fifo_r_w, MAX_PACKET_LENGTH, data))
;;;2106               return -1;
;;;2107           accel_cur[0] = ((short)data[0] << 8) | data[1];
;;;2108           accel_cur[1] = ((short)data[2] << 8) | data[3];
;;;2109           accel_cur[2] = ((short)data[4] << 8) | data[5];
;;;2110           accel[0] += (long)accel_cur[0];
;;;2111           accel[1] += (long)accel_cur[1];
;;;2112           accel[2] += (long)accel_cur[2];
;;;2113           gyro_cur[0] = (((short)data[6] << 8) | data[7]);
;;;2114           gyro_cur[1] = (((short)data[8] << 8) | data[9]);
;;;2115           gyro_cur[2] = (((short)data[10] << 8) | data[11]);
;;;2116           gyro[0] += (long)gyro_cur[0];
;;;2117           gyro[1] += (long)gyro_cur[1];
;;;2118           gyro[2] += (long)gyro_cur[2];
;;;2119       }
;;;2120   #ifdef EMPL_NO_64BIT
;;;2121       gyro[0] = (long)(((float)gyro[0]*65536.f) / test.gyro_sens / packet_count);
;;;2122       gyro[1] = (long)(((float)gyro[1]*65536.f) / test.gyro_sens / packet_count);
;;;2123       gyro[2] = (long)(((float)gyro[2]*65536.f) / test.gyro_sens / packet_count);
;;;2124       if (has_accel) {
;;;2125           accel[0] = (long)(((float)accel[0]*65536.f) / test.accel_sens /
;;;2126               packet_count);
;;;2127           accel[1] = (long)(((float)accel[1]*65536.f) / test.accel_sens /
;;;2128               packet_count);
;;;2129           accel[2] = (long)(((float)accel[2]*65536.f) / test.accel_sens /
;;;2130               packet_count);
;;;2131           /* Don't remove gravity! */
;;;2132           accel[2] -= 65536L;
;;;2133       }
;;;2134   #else
;;;2135       gyro[0] = (long)(((long long)gyro[0]<<16) / test.gyro_sens / packet_count);
;;;2136       gyro[1] = (long)(((long long)gyro[1]<<16) / test.gyro_sens / packet_count);
;;;2137       gyro[2] = (long)(((long long)gyro[2]<<16) / test.gyro_sens / packet_count);
;;;2138       accel[0] = (long)(((long long)accel[0]<<16) / test.accel_sens /
;;;2139           packet_count);
;;;2140       accel[1] = (long)(((long long)accel[1]<<16) / test.accel_sens /
;;;2141           packet_count);
;;;2142       accel[2] = (long)(((long long)accel[2]<<16) / test.accel_sens /
;;;2143           packet_count);
;;;2144       /* Don't remove gravity! */
;;;2145       if (accel[2] > 0L)
;;;2146           accel[2] -= 65536L;
;;;2147       else
;;;2148           accel[2] += 65536L;
;;;2149   #endif
;;;2150   
;;;2151       return 0;
;;;2152   }
001506  b007              ADD      sp,sp,#0x1c
001508  e8bd8ff0          POP      {r4-r11,pc}
                  |L1.5388|
00150c  20c8              MOVS     r0,#0xc8              ;2044
00150e  f7fffffe          BL       delay_ms
001512  2000              MOVS     r0,#0                 ;2045
001514  f88d0010          STRB     r0,[sp,#0x10]         ;2045
001518  4ad3              LDR      r2,|L1.6248|
00151a  6812              LDR      r2,[r2,#0]            ;2046  ; st
00151c  7bd1              LDRB     r1,[r2,#0xf]          ;2046
00151e  4ad2              LDR      r2,|L1.6248|
001520  6852              LDR      r2,[r2,#4]            ;2046  ; st
001522  7810              LDRB     r0,[r2,#0]            ;2046
001524  ab04              ADD      r3,sp,#0x10           ;2046
001526  2201              MOVS     r2,#1                 ;2046
001528  f7fffffe          BL       MPU_Write_Len
00152c  b110              CBZ      r0,|L1.5428|
00152e  f04f30ff          MOV      r0,#0xffffffff        ;2047
001532  e7e8              B        |L1.5382|
                  |L1.5428|
001534  4acc              LDR      r2,|L1.6248|
001536  6812              LDR      r2,[r2,#0]            ;2048  ; st
001538  7951              LDRB     r1,[r2,#5]            ;2048
00153a  4acb              LDR      r2,|L1.6248|
00153c  6852              LDR      r2,[r2,#4]            ;2048  ; st
00153e  7810              LDRB     r0,[r2,#0]            ;2048
001540  ab04              ADD      r3,sp,#0x10           ;2048
001542  2201              MOVS     r2,#1                 ;2048
001544  f7fffffe          BL       MPU_Write_Len
001548  b110              CBZ      r0,|L1.5456|
00154a  f04f30ff          MOV      r0,#0xffffffff        ;2049
00154e  e7da              B        |L1.5382|
                  |L1.5456|
001550  4ac5              LDR      r2,|L1.6248|
001552  6812              LDR      r2,[r2,#0]            ;2050  ; st
001554  7c91              LDRB     r1,[r2,#0x12]         ;2050
001556  4ac4              LDR      r2,|L1.6248|
001558  6852              LDR      r2,[r2,#4]            ;2050  ; st
00155a  7810              LDRB     r0,[r2,#0]            ;2050
00155c  ab04              ADD      r3,sp,#0x10           ;2050
00155e  2201              MOVS     r2,#1                 ;2050
001560  f7fffffe          BL       MPU_Write_Len
001564  b110              CBZ      r0,|L1.5484|
001566  f04f30ff          MOV      r0,#0xffffffff        ;2051
00156a  e7cc              B        |L1.5382|
                  |L1.5484|
00156c  4abe              LDR      r2,|L1.6248|
00156e  6812              LDR      r2,[r2,#0]            ;2052  ; st
001570  7dd1              LDRB     r1,[r2,#0x17]         ;2052
001572  4abd              LDR      r2,|L1.6248|
001574  6852              LDR      r2,[r2,#4]            ;2052  ; st
001576  7810              LDRB     r0,[r2,#0]            ;2052
001578  ab04              ADD      r3,sp,#0x10           ;2052
00157a  2201              MOVS     r2,#1                 ;2052
00157c  f7fffffe          BL       MPU_Write_Len
001580  b110              CBZ      r0,|L1.5512|
001582  f04f30ff          MOV      r0,#0xffffffff        ;2053
001586  e7be              B        |L1.5382|
                  |L1.5512|
001588  4ab7              LDR      r2,|L1.6248|
00158a  6812              LDR      r2,[r2,#0]            ;2054  ; st
00158c  7911              LDRB     r1,[r2,#4]            ;2054
00158e  4ab6              LDR      r2,|L1.6248|
001590  6852              LDR      r2,[r2,#4]            ;2054  ; st
001592  7810              LDRB     r0,[r2,#0]            ;2054
001594  ab04              ADD      r3,sp,#0x10           ;2054
001596  2201              MOVS     r2,#1                 ;2054
001598  f7fffffe          BL       MPU_Write_Len
00159c  b110              CBZ      r0,|L1.5540|
00159e  f04f30ff          MOV      r0,#0xffffffff        ;2055
0015a2  e7b0              B        |L1.5382|
                  |L1.5540|
0015a4  200c              MOVS     r0,#0xc               ;2056
0015a6  f88d0010          STRB     r0,[sp,#0x10]         ;2056
0015aa  4aaf              LDR      r2,|L1.6248|
0015ac  6812              LDR      r2,[r2,#0]            ;2057  ; st
0015ae  7911              LDRB     r1,[r2,#4]            ;2057
0015b0  4aad              LDR      r2,|L1.6248|
0015b2  6852              LDR      r2,[r2,#4]            ;2057  ; st
0015b4  7810              LDRB     r0,[r2,#0]            ;2057
0015b6  ab04              ADD      r3,sp,#0x10           ;2057
0015b8  2201              MOVS     r2,#1                 ;2057
0015ba  f7fffffe          BL       MPU_Write_Len
0015be  b110              CBZ      r0,|L1.5574|
0015c0  f04f30ff          MOV      r0,#0xffffffff        ;2058
0015c4  e79f              B        |L1.5382|
                  |L1.5574|
0015c6  200f              MOVS     r0,#0xf               ;2059
0015c8  f7fffffe          BL       delay_ms
0015cc  48a6              LDR      r0,|L1.6248|
0015ce  6a80              LDR      r0,[r0,#0x28]         ;2060  ; st
0015d0  7a40              LDRB     r0,[r0,#9]            ;2060
0015d2  f88d0010          STRB     r0,[sp,#0x10]         ;2060
0015d6  4aa4              LDR      r2,|L1.6248|
0015d8  6812              LDR      r2,[r2,#0]            ;2061  ; st
0015da  7891              LDRB     r1,[r2,#2]            ;2061
0015dc  4aa2              LDR      r2,|L1.6248|
0015de  6852              LDR      r2,[r2,#4]            ;2061  ; st
0015e0  7810              LDRB     r0,[r2,#0]            ;2061
0015e2  ab04              ADD      r3,sp,#0x10           ;2061
0015e4  2201              MOVS     r2,#1                 ;2061
0015e6  f7fffffe          BL       MPU_Write_Len
0015ea  b110              CBZ      r0,|L1.5618|
0015ec  f04f30ff          MOV      r0,#0xffffffff        ;2062
0015f0  e789              B        |L1.5382|
                  |L1.5618|
0015f2  489d              LDR      r0,|L1.6248|
0015f4  6a80              LDR      r0,[r0,#0x28]         ;2063  ; st
0015f6  7a00              LDRB     r0,[r0,#8]            ;2063
0015f8  f88d0010          STRB     r0,[sp,#0x10]         ;2063
0015fc  4a9a              LDR      r2,|L1.6248|
0015fe  6812              LDR      r2,[r2,#0]            ;2064  ; st
001600  7851              LDRB     r1,[r2,#1]            ;2064
001602  4a99              LDR      r2,|L1.6248|
001604  6852              LDR      r2,[r2,#4]            ;2064  ; st
001606  7810              LDRB     r0,[r2,#0]            ;2064
001608  ab04              ADD      r3,sp,#0x10           ;2064
00160a  2201              MOVS     r2,#1                 ;2064
00160c  f7fffffe          BL       MPU_Write_Len
001610  b110              CBZ      r0,|L1.5656|
001612  f04f30ff          MOV      r0,#0xffffffff        ;2065
001616  e776              B        |L1.5382|
                  |L1.5656|
001618  f1b90f00          CMP      r9,#0                 ;2066
00161c  d007              BEQ      |L1.5678|
00161e  4892              LDR      r0,|L1.6248|
001620  6a80              LDR      r0,[r0,#0x28]         ;2067  ; st
001622  7a80              LDRB     r0,[r0,#0xa]          ;2067
001624  f04000e0          ORR      r0,r0,#0xe0           ;2067
001628  f88d0010          STRB     r0,[sp,#0x10]         ;2067
00162c  e004              B        |L1.5688|
                  |L1.5678|
00162e  488e              LDR      r0,|L1.6248|
001630  6a80              LDR      r0,[r0,#0x28]         ;2069  ; st
001632  7a80              LDRB     r0,[r0,#0xa]          ;2069
001634  f88d0010          STRB     r0,[sp,#0x10]         ;2069
                  |L1.5688|
001638  4a8b              LDR      r2,|L1.6248|
00163a  6812              LDR      r2,[r2,#0]            ;2070  ; st
00163c  7991              LDRB     r1,[r2,#6]            ;2070
00163e  4a8a              LDR      r2,|L1.6248|
001640  6852              LDR      r2,[r2,#4]            ;2070  ; st
001642  7810              LDRB     r0,[r2,#0]            ;2070
001644  ab04              ADD      r3,sp,#0x10           ;2070
001646  2201              MOVS     r2,#1                 ;2070
001648  f7fffffe          BL       MPU_Write_Len
00164c  b110              CBZ      r0,|L1.5716|
00164e  f04f30ff          MOV      r0,#0xffffffff        ;2071
001652  e758              B        |L1.5382|
                  |L1.5716|
001654  f1b90f00          CMP      r9,#0                 ;2073
001658  d007              BEQ      |L1.5738|
00165a  4883              LDR      r0,|L1.6248|
00165c  6a80              LDR      r0,[r0,#0x28]         ;2074  ; st
00165e  7ac0              LDRB     r0,[r0,#0xb]          ;2074
001660  f04000e0          ORR      r0,r0,#0xe0           ;2074
001664  f88d0010          STRB     r0,[sp,#0x10]         ;2074
001668  e003              B        |L1.5746|
                  |L1.5738|
00166a  487e              LDR      r0,|L1.6244|
00166c  7ac0              LDRB     r0,[r0,#0xb]          ;2076  ; test
00166e  f88d0010          STRB     r0,[sp,#0x10]         ;2076
                  |L1.5746|
001672  4a7d              LDR      r2,|L1.6248|
001674  6812              LDR      r2,[r2,#0]            ;2077  ; st
001676  79d1              LDRB     r1,[r2,#7]            ;2077
001678  4a7b              LDR      r2,|L1.6248|
00167a  6852              LDR      r2,[r2,#4]            ;2077  ; st
00167c  7810              LDRB     r0,[r2,#0]            ;2077
00167e  ab04              ADD      r3,sp,#0x10           ;2077
001680  2201              MOVS     r2,#1                 ;2077
001682  f7fffffe          BL       MPU_Write_Len
001686  b110              CBZ      r0,|L1.5774|
001688  f04f30ff          MOV      r0,#0xffffffff        ;2078
00168c  e73b              B        |L1.5382|
                  |L1.5774|
00168e  f1b90f00          CMP      r9,#0                 ;2079
001692  d002              BEQ      |L1.5786|
001694  20c8              MOVS     r0,#0xc8              ;2080
001696  f7fffffe          BL       delay_ms
                  |L1.5786|
00169a  2040              MOVS     r0,#0x40              ;2083
00169c  f88d0010          STRB     r0,[sp,#0x10]         ;2083
0016a0  4a71              LDR      r2,|L1.6248|
0016a2  6812              LDR      r2,[r2,#0]            ;2084  ; st
0016a4  7911              LDRB     r1,[r2,#4]            ;2084
0016a6  4a70              LDR      r2,|L1.6248|
0016a8  6852              LDR      r2,[r2,#4]            ;2084  ; st
0016aa  7810              LDRB     r0,[r2,#0]            ;2084
0016ac  ab04              ADD      r3,sp,#0x10           ;2084
0016ae  2201              MOVS     r2,#1                 ;2084
0016b0  f7fffffe          BL       MPU_Write_Len
0016b4  b110              CBZ      r0,|L1.5820|
0016b6  f04f30ff          MOV      r0,#0xffffffff        ;2085
0016ba  e724              B        |L1.5382|
                  |L1.5820|
0016bc  2078              MOVS     r0,#0x78              ;2087
0016be  f88d0010          STRB     r0,[sp,#0x10]         ;2087
0016c2  4a69              LDR      r2,|L1.6248|
0016c4  6812              LDR      r2,[r2,#0]            ;2088  ; st
0016c6  7951              LDRB     r1,[r2,#5]            ;2088
0016c8  4a67              LDR      r2,|L1.6248|
0016ca  6852              LDR      r2,[r2,#4]            ;2088  ; st
0016cc  7810              LDRB     r0,[r2,#0]            ;2088
0016ce  ab04              ADD      r3,sp,#0x10           ;2088
0016d0  2201              MOVS     r2,#1                 ;2088
0016d2  f7fffffe          BL       MPU_Write_Len
0016d6  b110              CBZ      r0,|L1.5854|
0016d8  f04f30ff          MOV      r0,#0xffffffff        ;2089
0016dc  e713              B        |L1.5382|
                  |L1.5854|
0016de  4961              LDR      r1,|L1.6244|
0016e0  8988              LDRH     r0,[r1,#0xc]          ;2090  ; test
0016e2  f7fffffe          BL       delay_ms
0016e6  2000              MOVS     r0,#0                 ;2091
0016e8  f88d0010          STRB     r0,[sp,#0x10]         ;2091
0016ec  4a5e              LDR      r2,|L1.6248|
0016ee  6812              LDR      r2,[r2,#0]            ;2092  ; st
0016f0  7951              LDRB     r1,[r2,#5]            ;2092
0016f2  4a5d              LDR      r2,|L1.6248|
0016f4  6852              LDR      r2,[r2,#4]            ;2092  ; st
0016f6  7810              LDRB     r0,[r2,#0]            ;2092
0016f8  ab04              ADD      r3,sp,#0x10           ;2092
0016fa  2201              MOVS     r2,#1                 ;2092
0016fc  f7fffffe          BL       MPU_Write_Len
001700  b110              CBZ      r0,|L1.5896|
001702  f04f30ff          MOV      r0,#0xffffffff        ;2093
001706  e6fe              B        |L1.5382|
                  |L1.5896|
001708  4a57              LDR      r2,|L1.6248|
00170a  6812              LDR      r2,[r2,#0]            ;2095  ; st
00170c  7a91              LDRB     r1,[r2,#0xa]          ;2095
00170e  4a56              LDR      r2,|L1.6248|
001710  6852              LDR      r2,[r2,#4]            ;2095  ; st
001712  7810              LDRB     r0,[r2,#0]            ;2095
001714  ab04              ADD      r3,sp,#0x10           ;2095
001716  2202              MOVS     r2,#2                 ;2095
001718  f7fffffe          BL       MPU_Read_Len
00171c  b110              CBZ      r0,|L1.5924|
00171e  f04f30ff          MOV      r0,#0xffffffff        ;2096
001722  e6f0              B        |L1.5382|
                  |L1.5924|
001724  f89d0011          LDRB     r0,[sp,#0x11]         ;2098
001728  f89d1010          LDRB     r1,[sp,#0x10]         ;2098
00172c  ea402b01          ORR      r11,r0,r1,LSL #8      ;2098
001730  200c              MOVS     r0,#0xc               ;2099
001732  fb9bf0f0          SDIV     r0,r11,r0             ;2099
001736  b2c6              UXTB     r6,r0                 ;2099
001738  2000              MOVS     r0,#0                 ;2100
00173a  60a8              STR      r0,[r5,#8]            ;2100
00173c  6068              STR      r0,[r5,#4]            ;2100
00173e  6028              STR      r0,[r5,#0]            ;2100
001740  60a0              STR      r0,[r4,#8]            ;2101
001742  6060              STR      r0,[r4,#4]            ;2101
001744  6020              STR      r0,[r4,#0]            ;2101
001746  4682              MOV      r10,r0                ;2103
001748  e065              B        |L1.6166|
                  |L1.5962|
00174a  4a47              LDR      r2,|L1.6248|
00174c  6812              LDR      r2,[r2,#0]            ;2105  ; st
00174e  7ad1              LDRB     r1,[r2,#0xb]          ;2105
001750  4a45              LDR      r2,|L1.6248|
001752  6852              LDR      r2,[r2,#4]            ;2105  ; st
001754  7810              LDRB     r0,[r2,#0]            ;2105
001756  ab04              ADD      r3,sp,#0x10           ;2105
001758  220c              MOVS     r2,#0xc               ;2105
00175a  f7fffffe          BL       MPU_Read_Len
00175e  b110              CBZ      r0,|L1.5990|
001760  f04f30ff          MOV      r0,#0xffffffff        ;2106
001764  e6cf              B        |L1.5382|
                  |L1.5990|
001766  f89d0011          LDRB     r0,[sp,#0x11]         ;2107
00176a  f89d1010          LDRB     r1,[sp,#0x10]         ;2107
00176e  ea402001          ORR      r0,r0,r1,LSL #8       ;2107
001772  b200              SXTH     r0,r0                 ;2107
001774  f8ad0008          STRH     r0,[sp,#8]            ;2107
001778  f89d0013          LDRB     r0,[sp,#0x13]         ;2108
00177c  f89d1012          LDRB     r1,[sp,#0x12]         ;2108
001780  ea402001          ORR      r0,r0,r1,LSL #8       ;2108
001784  b200              SXTH     r0,r0                 ;2108
001786  f8ad000a          STRH     r0,[sp,#0xa]          ;2108
00178a  f89d0015          LDRB     r0,[sp,#0x15]         ;2109
00178e  f89d1014          LDRB     r1,[sp,#0x14]         ;2109
001792  ea402001          ORR      r0,r0,r1,LSL #8       ;2109
001796  b200              SXTH     r0,r0                 ;2109
001798  f8ad000c          STRH     r0,[sp,#0xc]          ;2109
00179c  f9bd1008          LDRSH    r1,[sp,#8]            ;2110
0017a0  6820              LDR      r0,[r4,#0]            ;2110
0017a2  4408              ADD      r0,r0,r1              ;2110
0017a4  6020              STR      r0,[r4,#0]            ;2110
0017a6  f9bd100a          LDRSH    r1,[sp,#0xa]          ;2111
0017aa  6860              LDR      r0,[r4,#4]            ;2111
0017ac  4408              ADD      r0,r0,r1              ;2111
0017ae  6060              STR      r0,[r4,#4]            ;2111
0017b0  f9bd100c          LDRSH    r1,[sp,#0xc]          ;2112
0017b4  68a0              LDR      r0,[r4,#8]            ;2112
0017b6  4408              ADD      r0,r0,r1              ;2112
0017b8  60a0              STR      r0,[r4,#8]            ;2112
0017ba  f89d0017          LDRB     r0,[sp,#0x17]         ;2113
0017be  f89d1016          LDRB     r1,[sp,#0x16]         ;2113
0017c2  ea402001          ORR      r0,r0,r1,LSL #8       ;2113
0017c6  b200              SXTH     r0,r0                 ;2113
0017c8  f8ad0000          STRH     r0,[sp,#0]            ;2113
0017cc  f89d0019          LDRB     r0,[sp,#0x19]         ;2114
0017d0  f89d1018          LDRB     r1,[sp,#0x18]         ;2114
0017d4  ea402001          ORR      r0,r0,r1,LSL #8       ;2114
0017d8  b200              SXTH     r0,r0                 ;2114
0017da  f8ad0002          STRH     r0,[sp,#2]            ;2114
0017de  f89d001b          LDRB     r0,[sp,#0x1b]         ;2115
0017e2  f89d101a          LDRB     r1,[sp,#0x1a]         ;2115
0017e6  ea402001          ORR      r0,r0,r1,LSL #8       ;2115
0017ea  b200              SXTH     r0,r0                 ;2115
0017ec  f8ad0004          STRH     r0,[sp,#4]            ;2115
0017f0  f9bd1000          LDRSH    r1,[sp,#0]            ;2116
0017f4  6828              LDR      r0,[r5,#0]            ;2116
0017f6  4408              ADD      r0,r0,r1              ;2116
0017f8  6028              STR      r0,[r5,#0]            ;2116
0017fa  f9bd1002          LDRSH    r1,[sp,#2]            ;2117
0017fe  6868              LDR      r0,[r5,#4]            ;2117
001800  4408              ADD      r0,r0,r1              ;2117
001802  6068              STR      r0,[r5,#4]            ;2117
001804  f9bd1004          LDRSH    r1,[sp,#4]            ;2118
001808  68a8              LDR      r0,[r5,#8]            ;2118
00180a  4408              ADD      r0,r0,r1              ;2118
00180c  60a8              STR      r0,[r5,#8]            ;2118
00180e  f10a0001          ADD      r0,r10,#1             ;2103
001812  f0000aff          AND      r10,r0,#0xff          ;2103
                  |L1.6166|
001816  45b2              CMP      r10,r6                ;2103
001818  db97              BLT      |L1.5962|
00181a  4812              LDR      r0,|L1.6244|
00181c  f8d0c000          LDR      r12,[r0,#0]           ;2135  ; test
001820  6828              LDR      r0,[r5,#0]            ;2135
001822  17c1              ASRS     r1,r0,#31             ;2135
001824  0409              LSLS     r1,r1,#16             ;2135
001826  ea414110          ORR      r1,r1,r0,LSR #16      ;2135
00182a  0407              LSLS     r7,r0,#16             ;2135
00182c  4662              MOV      r2,r12                ;2135
00182e  2300              MOVS     r3,#0                 ;2135
001830  4638              MOV      r0,r7                 ;2135
001832  f7fffffe          BL       __aeabi_ldivmod
001836  4632              MOV      r2,r6                 ;2135
001838  2300              MOVS     r3,#0                 ;2135
00183a  e9cd0102          STRD     r0,r1,[sp,#8]         ;2135
00183e  f7fffffe          BL       __aeabi_ldivmod
001842  6028              STR      r0,[r5,#0]            ;2135
001844  4807              LDR      r0,|L1.6244|
001846  f8d0c000          LDR      r12,[r0,#0]           ;2136  ; test
00184a  6868              LDR      r0,[r5,#4]            ;2136
00184c  17c1              ASRS     r1,r0,#31             ;2136
00184e  0409              LSLS     r1,r1,#16             ;2136
001850  ea414110          ORR      r1,r1,r0,LSR #16      ;2136
001854  0407              LSLS     r7,r0,#16             ;2136
001856  4662              MOV      r2,r12                ;2136
001858  2300              MOVS     r3,#0                 ;2136
00185a  4638              MOV      r0,r7                 ;2136
00185c  e006              B        |L1.6252|
00185e  0000              DCW      0x0000
                  |L1.6240|
                          DCD      0x3f85e354
                  |L1.6244|
                          DCD      test
                  |L1.6248|
                          DCD      ||st||
                  |L1.6252|
00186c  f7fffffe          BL       __aeabi_ldivmod
001870  4632              MOV      r2,r6                 ;2136
001872  2300              MOVS     r3,#0                 ;2136
001874  e9cd0102          STRD     r0,r1,[sp,#8]         ;2136
001878  f7fffffe          BL       __aeabi_ldivmod
00187c  6068              STR      r0,[r5,#4]            ;2136
00187e  48fe              LDR      r0,|L1.7288|
001880  f8d0c000          LDR      r12,[r0,#0]           ;2137  ; test
001884  68a8              LDR      r0,[r5,#8]            ;2137
001886  17c1              ASRS     r1,r0,#31             ;2137
001888  0409              LSLS     r1,r1,#16             ;2137
00188a  ea414110          ORR      r1,r1,r0,LSR #16      ;2137
00188e  0407              LSLS     r7,r0,#16             ;2137
001890  4662              MOV      r2,r12                ;2137
001892  2300              MOVS     r3,#0                 ;2137
001894  4638              MOV      r0,r7                 ;2137
001896  f7fffffe          BL       __aeabi_ldivmod
00189a  4632              MOV      r2,r6                 ;2137
00189c  2300              MOVS     r3,#0                 ;2137
00189e  e9cd0102          STRD     r0,r1,[sp,#8]         ;2137
0018a2  f7fffffe          BL       __aeabi_ldivmod
0018a6  60a8              STR      r0,[r5,#8]            ;2137
0018a8  48f3              LDR      r0,|L1.7288|
0018aa  f8d0c004          LDR      r12,[r0,#4]           ;2138  ; test
0018ae  6821              LDR      r1,[r4,#0]            ;2138
0018b0  17c8              ASRS     r0,r1,#31             ;2138
0018b2  0400              LSLS     r0,r0,#16             ;2138
0018b4  ea404811          ORR      r8,r0,r1,LSR #16      ;2138
0018b8  040f              LSLS     r7,r1,#16             ;2138
0018ba  4662              MOV      r2,r12                ;2138
0018bc  2300              MOVS     r3,#0                 ;2138
0018be  4638              MOV      r0,r7                 ;2138
0018c0  4641              MOV      r1,r8                 ;2138
0018c2  f7fffffe          BL       __aeabi_ldivmod
0018c6  4632              MOV      r2,r6                 ;2138
0018c8  2300              MOVS     r3,#0                 ;2138
0018ca  e9cd0102          STRD     r0,r1,[sp,#8]         ;2138
0018ce  f7fffffe          BL       __aeabi_ldivmod
0018d2  6020              STR      r0,[r4,#0]            ;2138
0018d4  48e8              LDR      r0,|L1.7288|
0018d6  f8d0c004          LDR      r12,[r0,#4]           ;2140  ; test
0018da  6860              LDR      r0,[r4,#4]            ;2140
0018dc  17c1              ASRS     r1,r0,#31             ;2140
0018de  0409              LSLS     r1,r1,#16             ;2140
0018e0  ea414110          ORR      r1,r1,r0,LSR #16      ;2140
0018e4  0407              LSLS     r7,r0,#16             ;2140
0018e6  4662              MOV      r2,r12                ;2140
0018e8  2300              MOVS     r3,#0                 ;2140
0018ea  4638              MOV      r0,r7                 ;2140
0018ec  f7fffffe          BL       __aeabi_ldivmod
0018f0  4632              MOV      r2,r6                 ;2140
0018f2  2300              MOVS     r3,#0                 ;2140
0018f4  e9cd0102          STRD     r0,r1,[sp,#8]         ;2140
0018f8  f7fffffe          BL       __aeabi_ldivmod
0018fc  6060              STR      r0,[r4,#4]            ;2140
0018fe  48de              LDR      r0,|L1.7288|
001900  f8d0c004          LDR      r12,[r0,#4]           ;2142  ; test
001904  68a0              LDR      r0,[r4,#8]            ;2142
001906  17c1              ASRS     r1,r0,#31             ;2142
001908  0409              LSLS     r1,r1,#16             ;2142
00190a  ea414110          ORR      r1,r1,r0,LSR #16      ;2142
00190e  0407              LSLS     r7,r0,#16             ;2142
001910  4662              MOV      r2,r12                ;2142
001912  2300              MOVS     r3,#0                 ;2142
001914  4638              MOV      r0,r7                 ;2142
001916  f7fffffe          BL       __aeabi_ldivmod
00191a  4632              MOV      r2,r6                 ;2142
00191c  2300              MOVS     r3,#0                 ;2142
00191e  e9cd0102          STRD     r0,r1,[sp,#8]         ;2142
001922  f7fffffe          BL       __aeabi_ldivmod
001926  60a0              STR      r0,[r4,#8]            ;2142
001928  68a0              LDR      r0,[r4,#8]            ;2145
00192a  2800              CMP      r0,#0                 ;2145
00192c  dd04              BLE      |L1.6456|
00192e  68a0              LDR      r0,[r4,#8]            ;2146
001930  f5a03080          SUB      r0,r0,#0x10000        ;2146
001934  60a0              STR      r0,[r4,#8]            ;2146
001936  e003              B        |L1.6464|
                  |L1.6456|
001938  68a0              LDR      r0,[r4,#8]            ;2148
00193a  f5003080          ADD      r0,r0,#0x10000        ;2148
00193e  60a0              STR      r0,[r4,#8]            ;2148
                  |L1.6464|
001940  2000              MOVS     r0,#0                 ;2151
001942  e5e0              B        |L1.5382|
;;;2153   
                          ENDP

                  mpu_set_dmp_state PROC
;;;2381    */
;;;2382   int mpu_set_dmp_state(unsigned char enable)
001944  b538              PUSH     {r3-r5,lr}
;;;2383   {
001946  4604              MOV      r4,r0
;;;2384       unsigned char tmp;
;;;2385       if (st.chip_cfg.dmp_on == enable)
001948  48cc              LDR      r0,|L1.7292|
00194a  f8900024          LDRB     r0,[r0,#0x24]
00194e  42a0              CMP      r0,r4
001950  d101              BNE      |L1.6486|
;;;2386           return 0;
001952  2000              MOVS     r0,#0
                  |L1.6484|
;;;2387       
;;;2388       if (enable) {
;;;2389           if (!st.chip_cfg.dmp_loaded)
;;;2390               return -1;
;;;2391           /* Disable data ready interrupt. */
;;;2392           set_int_enable(0);
;;;2393           /* Disable bypass mode. */
;;;2394           mpu_set_bypass(0);
;;;2395           /* Keep constant sample rate, FIFO rate controlled by DMP. */
;;;2396           mpu_set_sample_rate(st.chip_cfg.dmp_sample_rate);
;;;2397           /* Remove FIFO elements. */
;;;2398           tmp = 0;
;;;2399           i2c_write(st.hw->addr, 0x23, 1, &tmp); 
;;;2400           st.chip_cfg.dmp_on = 1;
;;;2401           /* Enable DMP interrupt. */
;;;2402           set_int_enable(1);//USART_SendData(USART1,66);	
;;;2403           mpu_reset_fifo();
;;;2404       } else {
;;;2405           /* Disable DMP interrupt. */
;;;2406           set_int_enable(0);
;;;2407           /* Restore FIFO settings. */
;;;2408           tmp = st.chip_cfg.fifo_enable;
;;;2409           i2c_write(st.hw->addr, 0x23, 1, &tmp);
;;;2410           st.chip_cfg.dmp_on = 0;
;;;2411           mpu_reset_fifo();
;;;2412       }
;;;2413       return 0;
;;;2414   }
001954  bd38              POP      {r3-r5,pc}
                  |L1.6486|
001956  b324              CBZ      r4,|L1.6562|
001958  48c8              LDR      r0,|L1.7292|
00195a  f8900025          LDRB     r0,[r0,#0x25]         ;2389
00195e  b910              CBNZ     r0,|L1.6502|
001960  f04f30ff          MOV      r0,#0xffffffff        ;2390
001964  e7f6              B        |L1.6484|
                  |L1.6502|
001966  2000              MOVS     r0,#0                 ;2392
001968  f7fffffe          BL       set_int_enable
00196c  2000              MOVS     r0,#0                 ;2394
00196e  f7fffffe          BL       mpu_set_bypass
001972  49c2              LDR      r1,|L1.7292|
001974  8cc8              LDRH     r0,[r1,#0x26]         ;2396
001976  f7fffffe          BL       mpu_set_sample_rate
00197a  2000              MOVS     r0,#0                 ;2398
00197c  9000              STR      r0,[sp,#0]            ;2398
00197e  49bf              LDR      r1,|L1.7292|
001980  6849              LDR      r1,[r1,#4]            ;2399  ; st
001982  7808              LDRB     r0,[r1,#0]            ;2399
001984  466b              MOV      r3,sp                 ;2399
001986  2201              MOVS     r2,#1                 ;2399
001988  2123              MOVS     r1,#0x23              ;2399
00198a  f7fffffe          BL       MPU_Write_Len
00198e  2101              MOVS     r1,#1                 ;2400
001990  48ba              LDR      r0,|L1.7292|
001992  f8801024          STRB     r1,[r0,#0x24]         ;2400
001996  2001              MOVS     r0,#1                 ;2402
001998  f7fffffe          BL       set_int_enable
00199c  f7fffffe          BL       mpu_reset_fifo
0019a0  e013              B        |L1.6602|
                  |L1.6562|
0019a2  2000              MOVS     r0,#0                 ;2406
0019a4  f7fffffe          BL       set_int_enable
0019a8  48b4              LDR      r0,|L1.7292|
0019aa  7c00              LDRB     r0,[r0,#0x10]         ;2408
0019ac  9000              STR      r0,[sp,#0]            ;2408
0019ae  49b3              LDR      r1,|L1.7292|
0019b0  6849              LDR      r1,[r1,#4]            ;2409  ; st
0019b2  7808              LDRB     r0,[r1,#0]            ;2409
0019b4  466b              MOV      r3,sp                 ;2409
0019b6  2201              MOVS     r2,#1                 ;2409
0019b8  2123              MOVS     r1,#0x23              ;2409
0019ba  f7fffffe          BL       MPU_Write_Len
0019be  2100              MOVS     r1,#0                 ;2410
0019c0  48ae              LDR      r0,|L1.7292|
0019c2  f8801024          STRB     r1,[r0,#0x24]         ;2410
0019c6  f7fffffe          BL       mpu_reset_fifo
                  |L1.6602|
0019ca  2000              MOVS     r0,#0                 ;2413
0019cc  e7c2              B        |L1.6484|
;;;2415   
                          ENDP

                  mpu_run_self_test PROC
;;;2173    */
;;;2174   int mpu_run_self_test(long *gyro, long *accel)
0019ce  e92d4ff0          PUSH     {r4-r11,lr}
;;;2175   {
0019d2  b08d              SUB      sp,sp,#0x34
0019d4  4683              MOV      r11,r0
0019d6  460f              MOV      r7,r1
;;;2176   #ifdef MPU6050
;;;2177       const unsigned char tries = 2;
0019d8  2002              MOVS     r0,#2
0019da  900c              STR      r0,[sp,#0x30]
;;;2178       long gyro_st[3], accel_st[3];
;;;2179       unsigned char accel_result, gyro_result;
;;;2180   #ifdef AK89xx_SECONDARY
;;;2181       unsigned char compass_result;
;;;2182   #endif
;;;2183       int ii;
;;;2184   #endif
;;;2185       int result;
;;;2186       unsigned char accel_fsr, fifo_sensors, sensors_on;
;;;2187       unsigned short gyro_fsr, sample_rate, lpf;
;;;2188       unsigned char dmp_was_on;
;;;2189   
;;;2190       if (st.chip_cfg.dmp_on) {
0019dc  48a7              LDR      r0,|L1.7292|
0019de  f8900024          LDRB     r0,[r0,#0x24]
0019e2  b120              CBZ      r0,|L1.6638|
;;;2191           mpu_set_dmp_state(0);
0019e4  2000              MOVS     r0,#0
0019e6  f7fffffe          BL       mpu_set_dmp_state
;;;2192           dmp_was_on = 1;
0019ea  2601              MOVS     r6,#1
0019ec  e000              B        |L1.6640|
                  |L1.6638|
;;;2193       } else
;;;2194           dmp_was_on = 0;
0019ee  2600              MOVS     r6,#0
                  |L1.6640|
;;;2195   
;;;2196       /* Get initial settings. */
;;;2197       mpu_get_gyro_fsr(&gyro_fsr);
0019f0  a803              ADD      r0,sp,#0xc
0019f2  f7fffffe          BL       mpu_get_gyro_fsr
;;;2198       mpu_get_accel_fsr(&accel_fsr);
0019f6  a805              ADD      r0,sp,#0x14
0019f8  f7fffffe          BL       mpu_get_accel_fsr
;;;2199       mpu_get_lpf(&lpf);
0019fc  a801              ADD      r0,sp,#4
0019fe  f7fffffe          BL       mpu_get_lpf
;;;2200       mpu_get_sample_rate(&sample_rate);
001a02  a802              ADD      r0,sp,#8
001a04  f7fffffe          BL       mpu_get_sample_rate
;;;2201       sensors_on = st.chip_cfg.sensors;
001a08  489c              LDR      r0,|L1.7292|
001a0a  f890a00a          LDRB     r10,[r0,#0xa]
;;;2202       mpu_get_fifo_config(&fifo_sensors);
001a0e  a804              ADD      r0,sp,#0x10
001a10  f7fffffe          BL       mpu_get_fifo_config
;;;2203   
;;;2204       /* For older chips, the self-test will be different. */
;;;2205   #if defined MPU6050
;;;2206       for (ii = 0; ii < tries; ii++)
001a14  2400              MOVS     r4,#0
001a16  e007              B        |L1.6696|
                  |L1.6680|
;;;2207           if (!get_st_biases(gyro, accel, 0))
001a18  2200              MOVS     r2,#0
001a1a  4639              MOV      r1,r7
001a1c  4658              MOV      r0,r11
001a1e  f7fffffe          BL       get_st_biases
001a22  b900              CBNZ     r0,|L1.6694|
;;;2208               break;
001a24  e002              B        |L1.6700|
                  |L1.6694|
001a26  1c64              ADDS     r4,r4,#1              ;2206
                  |L1.6696|
001a28  2c02              CMP      r4,#2                 ;2206
001a2a  dbf5              BLT      |L1.6680|
                  |L1.6700|
001a2c  bf00              NOP      
;;;2209       if (ii == tries) {
001a2e  2c02              CMP      r4,#2
001a30  d101              BNE      |L1.6710|
;;;2210           /* If we reach this point, we most likely encountered an I2C error.
;;;2211            * We'll just report an error for all three sensors.
;;;2212            */
;;;2213           result = 0;
001a32  2500              MOVS     r5,#0
;;;2214           goto restore;
001a34  e028              B        |L1.6792|
                  |L1.6710|
;;;2215       }
;;;2216       for (ii = 0; ii < tries; ii++)
001a36  2400              MOVS     r4,#0
001a38  e007              B        |L1.6730|
                  |L1.6714|
;;;2217           if (!get_st_biases(gyro_st, accel_st, 1))
001a3a  2201              MOVS     r2,#1
001a3c  a906              ADD      r1,sp,#0x18
001a3e  a809              ADD      r0,sp,#0x24
001a40  f7fffffe          BL       get_st_biases
001a44  b900              CBNZ     r0,|L1.6728|
;;;2218               break;
001a46  e002              B        |L1.6734|
                  |L1.6728|
001a48  1c64              ADDS     r4,r4,#1              ;2216
                  |L1.6730|
001a4a  2c02              CMP      r4,#2                 ;2216
001a4c  dbf5              BLT      |L1.6714|
                  |L1.6734|
001a4e  bf00              NOP      
;;;2219       if (ii == tries) {
001a50  2c02              CMP      r4,#2
001a52  d101              BNE      |L1.6744|
;;;2220           /* Again, probably an I2C error. */
;;;2221           result = 0;
001a54  2500              MOVS     r5,#0
;;;2222           goto restore;
001a56  e017              B        |L1.6792|
                  |L1.6744|
;;;2223       }
;;;2224       accel_result = accel_self_test(accel, accel_st);
001a58  a906              ADD      r1,sp,#0x18
001a5a  4638              MOV      r0,r7
001a5c  f7fffffe          BL       accel_self_test
001a60  f00008ff          AND      r8,r0,#0xff
;;;2225       gyro_result = gyro_self_test(gyro, gyro_st);
001a64  a909              ADD      r1,sp,#0x24
001a66  4658              MOV      r0,r11
001a68  f7fffffe          BL       gyro_self_test
001a6c  f00009ff          AND      r9,r0,#0xff
;;;2226   
;;;2227       result = 0;
001a70  2500              MOVS     r5,#0
;;;2228       if (!gyro_result)
001a72  f1b90f00          CMP      r9,#0
001a76  d101              BNE      |L1.6780|
;;;2229           result |= 0x01;
001a78  f0450501          ORR      r5,r5,#1
                  |L1.6780|
;;;2230       if (!accel_result)
001a7c  f1b80f00          CMP      r8,#0
001a80  d101              BNE      |L1.6790|
;;;2231           result |= 0x02;
001a82  f0450502          ORR      r5,r5,#2
                  |L1.6790|
;;;2232   
;;;2233   #ifdef AK89xx_SECONDARY
;;;2234       compass_result = compass_self_test();
;;;2235       if (!compass_result)
;;;2236           result |= 0x04;
;;;2237   #endif
;;;2238   restore:
001a86  bf00              NOP      
                  |L1.6792|
;;;2239   #elif defined MPU6500
;;;2240       /* For now, this function will return a "pass" result for all three sensors
;;;2241        * for compatibility with current test applications.
;;;2242        */
;;;2243       get_st_biases(gyro, accel, 0);
;;;2244       result = 0x7;
;;;2245   #endif
;;;2246       /* Set to invalid values to ensure no I2C writes are skipped. */
;;;2247       st.chip_cfg.gyro_fsr = 0xFF;
001a88  20ff              MOVS     r0,#0xff
001a8a  497c              LDR      r1,|L1.7292|
001a8c  7208              STRB     r0,[r1,#8]
;;;2248       st.chip_cfg.accel_fsr = 0xFF;
001a8e  21ff              MOVS     r1,#0xff
001a90  487a              LDR      r0,|L1.7292|
001a92  7241              STRB     r1,[r0,#9]
;;;2249       st.chip_cfg.lpf = 0xFF;
001a94  72c1              STRB     r1,[r0,#0xb]
;;;2250       st.chip_cfg.sample_rate = 0xFFFF;
001a96  f64f71ff          MOV      r1,#0xffff
001a9a  81c1              STRH     r1,[r0,#0xe]
;;;2251       st.chip_cfg.sensors = 0xFF;
001a9c  21ff              MOVS     r1,#0xff
001a9e  7281              STRB     r1,[r0,#0xa]
;;;2252       st.chip_cfg.fifo_enable = 0xFF;
001aa0  7401              STRB     r1,[r0,#0x10]
;;;2253       st.chip_cfg.clk_src = INV_CLK_PLL;
001aa2  2101              MOVS     r1,#1
001aa4  7301              STRB     r1,[r0,#0xc]
;;;2254       mpu_set_gyro_fsr(gyro_fsr);
001aa6  f8bd000c          LDRH     r0,[sp,#0xc]
001aaa  f7fffffe          BL       mpu_set_gyro_fsr
;;;2255       mpu_set_accel_fsr(accel_fsr);
001aae  f89d0014          LDRB     r0,[sp,#0x14]
001ab2  f7fffffe          BL       mpu_set_accel_fsr
;;;2256       mpu_set_lpf(lpf);
001ab6  f8bd0004          LDRH     r0,[sp,#4]
001aba  f7fffffe          BL       mpu_set_lpf
;;;2257       mpu_set_sample_rate(sample_rate);
001abe  f8bd0008          LDRH     r0,[sp,#8]
001ac2  f7fffffe          BL       mpu_set_sample_rate
;;;2258       mpu_set_sensors(sensors_on);
001ac6  4650              MOV      r0,r10
001ac8  f7fffffe          BL       mpu_set_sensors
;;;2259       mpu_configure_fifo(fifo_sensors);
001acc  f89d0010          LDRB     r0,[sp,#0x10]
001ad0  f7fffffe          BL       mpu_configure_fifo
;;;2260   
;;;2261       if (dmp_was_on)
001ad4  b116              CBZ      r6,|L1.6876|
;;;2262           mpu_set_dmp_state(1);
001ad6  2001              MOVS     r0,#1
001ad8  f7fffffe          BL       mpu_set_dmp_state
                  |L1.6876|
;;;2263   
;;;2264       return result;
001adc  4628              MOV      r0,r5
;;;2265   }
001ade  b00d              ADD      sp,sp,#0x34
001ae0  e8bd8ff0          POP      {r4-r11,pc}
;;;2266   
                          ENDP

                  mpu_write_mem PROC
;;;2275    */
;;;2276   int mpu_write_mem(unsigned short mem_addr, unsigned short length,
001ae4  b5f8              PUSH     {r3-r7,lr}
;;;2277           unsigned char *data)
;;;2278   {
001ae6  4604              MOV      r4,r0
001ae8  460d              MOV      r5,r1
001aea  4616              MOV      r6,r2
;;;2279       unsigned char tmp[2];
;;;2280   
;;;2281       if (!data)
001aec  b916              CBNZ     r6,|L1.6900|
;;;2282           return -1;
001aee  f04f30ff          MOV      r0,#0xffffffff
                  |L1.6898|
;;;2283       if (!st.chip_cfg.sensors)
;;;2284           return -1;
;;;2285   
;;;2286       tmp[0] = (unsigned char)(mem_addr >> 8);
;;;2287       tmp[1] = (unsigned char)(mem_addr & 0xFF);
;;;2288   
;;;2289       /* Check bank boundaries. */
;;;2290       if (tmp[1] + length > st.hw->bank_size)
;;;2291           return -1;
;;;2292   
;;;2293       if (i2c_write(st.hw->addr, st.reg->bank_sel, 2, tmp))
;;;2294           return -1;
;;;2295       if (i2c_write(st.hw->addr, st.reg->mem_r_w, length, data))
;;;2296           return -1;
;;;2297       return 0;
;;;2298   }
001af2  bdf8              POP      {r3-r7,pc}
                  |L1.6900|
001af4  4861              LDR      r0,|L1.7292|
001af6  7a80              LDRB     r0,[r0,#0xa]          ;2283
001af8  b910              CBNZ     r0,|L1.6912|
001afa  f04f30ff          MOV      r0,#0xffffffff        ;2284
001afe  e7f8              B        |L1.6898|
                  |L1.6912|
001b00  1220              ASRS     r0,r4,#8              ;2286
001b02  f88d0000          STRB     r0,[sp,#0]            ;2286
001b06  b2e0              UXTB     r0,r4                 ;2287
001b08  f88d0001          STRB     r0,[sp,#1]            ;2287
001b0c  f89d0001          LDRB     r0,[sp,#1]            ;2290
001b10  4428              ADD      r0,r0,r5              ;2290
001b12  495a              LDR      r1,|L1.7292|
001b14  6849              LDR      r1,[r1,#4]            ;2290  ; st
001b16  8949              LDRH     r1,[r1,#0xa]          ;2290
001b18  4288              CMP      r0,r1                 ;2290
001b1a  dd02              BLE      |L1.6946|
001b1c  f04f30ff          MOV      r0,#0xffffffff        ;2291
001b20  e7e7              B        |L1.6898|
                  |L1.6946|
001b22  4a56              LDR      r2,|L1.7292|
001b24  6812              LDR      r2,[r2,#0]            ;2293  ; st
001b26  7e11              LDRB     r1,[r2,#0x18]         ;2293
001b28  4a54              LDR      r2,|L1.7292|
001b2a  6852              LDR      r2,[r2,#4]            ;2293  ; st
001b2c  7810              LDRB     r0,[r2,#0]            ;2293
001b2e  466b              MOV      r3,sp                 ;2293
001b30  2202              MOVS     r2,#2                 ;2293
001b32  f7fffffe          BL       MPU_Write_Len
001b36  b110              CBZ      r0,|L1.6974|
001b38  f04f30ff          MOV      r0,#0xffffffff        ;2294
001b3c  e7d9              B        |L1.6898|
                  |L1.6974|
001b3e  b2ea              UXTB     r2,r5                 ;2295
001b40  4b4e              LDR      r3,|L1.7292|
001b42  681b              LDR      r3,[r3,#0]            ;2295  ; st
001b44  7d59              LDRB     r1,[r3,#0x15]         ;2295
001b46  4b4d              LDR      r3,|L1.7292|
001b48  685b              LDR      r3,[r3,#4]            ;2295  ; st
001b4a  7818              LDRB     r0,[r3,#0]            ;2295
001b4c  4633              MOV      r3,r6                 ;2295
001b4e  f7fffffe          BL       MPU_Write_Len
001b52  b110              CBZ      r0,|L1.7002|
001b54  f04f30ff          MOV      r0,#0xffffffff        ;2296
001b58  e7cb              B        |L1.6898|
                  |L1.7002|
001b5a  2000              MOVS     r0,#0                 ;2297
001b5c  e7c9              B        |L1.6898|
;;;2299   
                          ENDP

                  mpu_read_mem PROC
;;;2308    */
;;;2309   int mpu_read_mem(unsigned short mem_addr, unsigned short length,
001b5e  b5f8              PUSH     {r3-r7,lr}
;;;2310           unsigned char *data)
;;;2311   {
001b60  4604              MOV      r4,r0
001b62  460d              MOV      r5,r1
001b64  4616              MOV      r6,r2
;;;2312       unsigned char tmp[2];
;;;2313   
;;;2314       if (!data)
001b66  b916              CBNZ     r6,|L1.7022|
;;;2315           return -1;
001b68  f04f30ff          MOV      r0,#0xffffffff
                  |L1.7020|
;;;2316       if (!st.chip_cfg.sensors)
;;;2317           return -1;
;;;2318   
;;;2319       tmp[0] = (unsigned char)(mem_addr >> 8);
;;;2320       tmp[1] = (unsigned char)(mem_addr & 0xFF);
;;;2321   
;;;2322       /* Check bank boundaries. */
;;;2323       if (tmp[1] + length > st.hw->bank_size)
;;;2324           return -1;
;;;2325   
;;;2326       if (i2c_write(st.hw->addr, st.reg->bank_sel, 2, tmp))
;;;2327           return -1;
;;;2328       if (i2c_read(st.hw->addr, st.reg->mem_r_w, length, data))
;;;2329           return -1;
;;;2330       return 0;
;;;2331   }
001b6c  bdf8              POP      {r3-r7,pc}
                  |L1.7022|
001b6e  4843              LDR      r0,|L1.7292|
001b70  7a80              LDRB     r0,[r0,#0xa]          ;2316
001b72  b910              CBNZ     r0,|L1.7034|
001b74  f04f30ff          MOV      r0,#0xffffffff        ;2317
001b78  e7f8              B        |L1.7020|
                  |L1.7034|
001b7a  1220              ASRS     r0,r4,#8              ;2319
001b7c  f88d0000          STRB     r0,[sp,#0]            ;2319
001b80  b2e0              UXTB     r0,r4                 ;2320
001b82  f88d0001          STRB     r0,[sp,#1]            ;2320
001b86  f89d0001          LDRB     r0,[sp,#1]            ;2323
001b8a  4428              ADD      r0,r0,r5              ;2323
001b8c  493b              LDR      r1,|L1.7292|
001b8e  6849              LDR      r1,[r1,#4]            ;2323  ; st
001b90  8949              LDRH     r1,[r1,#0xa]          ;2323
001b92  4288              CMP      r0,r1                 ;2323
001b94  dd02              BLE      |L1.7068|
001b96  f04f30ff          MOV      r0,#0xffffffff        ;2324
001b9a  e7e7              B        |L1.7020|
                  |L1.7068|
001b9c  4a37              LDR      r2,|L1.7292|
001b9e  6812              LDR      r2,[r2,#0]            ;2326  ; st
001ba0  7e11              LDRB     r1,[r2,#0x18]         ;2326
001ba2  4a36              LDR      r2,|L1.7292|
001ba4  6852              LDR      r2,[r2,#4]            ;2326  ; st
001ba6  7810              LDRB     r0,[r2,#0]            ;2326
001ba8  466b              MOV      r3,sp                 ;2326
001baa  2202              MOVS     r2,#2                 ;2326
001bac  f7fffffe          BL       MPU_Write_Len
001bb0  b110              CBZ      r0,|L1.7096|
001bb2  f04f30ff          MOV      r0,#0xffffffff        ;2327
001bb6  e7d9              B        |L1.7020|
                  |L1.7096|
001bb8  b2ea              UXTB     r2,r5                 ;2328
001bba  4b30              LDR      r3,|L1.7292|
001bbc  681b              LDR      r3,[r3,#0]            ;2328  ; st
001bbe  7d59              LDRB     r1,[r3,#0x15]         ;2328
001bc0  4b2e              LDR      r3,|L1.7292|
001bc2  685b              LDR      r3,[r3,#4]            ;2328  ; st
001bc4  7818              LDRB     r0,[r3,#0]            ;2328
001bc6  4633              MOV      r3,r6                 ;2328
001bc8  f7fffffe          BL       MPU_Read_Len
001bcc  b110              CBZ      r0,|L1.7124|
001bce  f04f30ff          MOV      r0,#0xffffffff        ;2329
001bd2  e7cb              B        |L1.7020|
                  |L1.7124|
001bd4  2000              MOVS     r0,#0                 ;2330
001bd6  e7c9              B        |L1.7020|
;;;2332   
                          ENDP

                  mpu_load_firmware PROC
;;;2340    */
;;;2341   int mpu_load_firmware(unsigned short length, const unsigned char *firmware,
001bd8  e92d43f0          PUSH     {r4-r9,lr}
;;;2342       unsigned short start_addr, unsigned short sample_rate)
;;;2343   {
001bdc  b085              SUB      sp,sp,#0x14
001bde  4605              MOV      r5,r0
001be0  460e              MOV      r6,r1
001be2  4690              MOV      r8,r2
001be4  4699              MOV      r9,r3
;;;2344       unsigned short ii;
;;;2345       unsigned short this_write;
;;;2346       /* Must divide evenly into st.hw->bank_size to avoid bank crossings. */
;;;2347   #define LOAD_CHUNK  (16)
;;;2348       unsigned char cur[LOAD_CHUNK], tmp[2];
;;;2349   
;;;2350       if (st.chip_cfg.dmp_loaded)
001be6  4825              LDR      r0,|L1.7292|
001be8  f8900025          LDRB     r0,[r0,#0x25]
001bec  b120              CBZ      r0,|L1.7160|
;;;2351           /* DMP should only be loaded once. */
;;;2352           return -1;
001bee  f04f30ff          MOV      r0,#0xffffffff
                  |L1.7154|
;;;2353       
;;;2354       if (!firmware)
;;;2355           return -1;
;;;2356       for (ii = 0; ii < length; ii += this_write) {
;;;2357           this_write = min(LOAD_CHUNK, length - ii);
;;;2358           if (mpu_write_mem(ii, this_write, (unsigned char*)&firmware[ii]))
;;;2359               return -1;
;;;2360           if (mpu_read_mem(ii, this_write, cur))
;;;2361               return -1;
;;;2362           if (memcmp(firmware+ii, cur, this_write))
;;;2363               return -2;//uprintf(USART1," 111111111!\r\n");
;;;2364       }
;;;2365   
;;;2366       /* Set program start address. */
;;;2367       tmp[0] = start_addr >> 8;
;;;2368       tmp[1] = start_addr & 0xFF;
;;;2369       if (i2c_write(st.hw->addr, st.reg->prgm_start_h, 2, tmp))
;;;2370           return -1;
;;;2371   
;;;2372       st.chip_cfg.dmp_loaded = 1;
;;;2373       st.chip_cfg.dmp_sample_rate = sample_rate;
;;;2374       return 0;
;;;2375   }
001bf2  b005              ADD      sp,sp,#0x14
001bf4  e8bd83f0          POP      {r4-r9,pc}
                  |L1.7160|
001bf8  b916              CBNZ     r6,|L1.7168|
001bfa  f04f30ff          MOV      r0,#0xffffffff        ;2355
001bfe  e7f8              B        |L1.7154|
                  |L1.7168|
001c00  2400              MOVS     r4,#0                 ;2356
001c02  e023              B        |L1.7244|
                  |L1.7172|
001c04  1b28              SUBS     r0,r5,r4              ;2357
001c06  2810              CMP      r0,#0x10              ;2357
001c08  dd01              BLE      |L1.7182|
001c0a  2010              MOVS     r0,#0x10              ;2357
001c0c  e000              B        |L1.7184|
                  |L1.7182|
001c0e  1b28              SUBS     r0,r5,r4              ;2357
                  |L1.7184|
001c10  b287              UXTH     r7,r0                 ;2357
001c12  1932              ADDS     r2,r6,r4              ;2358
001c14  4639              MOV      r1,r7                 ;2358
001c16  4620              MOV      r0,r4                 ;2358
001c18  f7fffffe          BL       mpu_write_mem
001c1c  b110              CBZ      r0,|L1.7204|
001c1e  f04f30ff          MOV      r0,#0xffffffff        ;2359
001c22  e7e6              B        |L1.7154|
                  |L1.7204|
001c24  aa01              ADD      r2,sp,#4              ;2360
001c26  4639              MOV      r1,r7                 ;2360
001c28  4620              MOV      r0,r4                 ;2360
001c2a  f7fffffe          BL       mpu_read_mem
001c2e  b110              CBZ      r0,|L1.7222|
001c30  f04f30ff          MOV      r0,#0xffffffff        ;2361
001c34  e7dd              B        |L1.7154|
                  |L1.7222|
001c36  1930              ADDS     r0,r6,r4              ;2362
001c38  463a              MOV      r2,r7                 ;2362
001c3a  a901              ADD      r1,sp,#4              ;2362
001c3c  f7fffffe          BL       memcmp
001c40  b110              CBZ      r0,|L1.7240|
001c42  f06f0001          MVN      r0,#1                 ;2363
001c46  e7d4              B        |L1.7154|
                  |L1.7240|
001c48  19e0              ADDS     r0,r4,r7              ;2356
001c4a  b284              UXTH     r4,r0                 ;2356
                  |L1.7244|
001c4c  42ac              CMP      r4,r5                 ;2356
001c4e  dbd9              BLT      |L1.7172|
001c50  ea4f2028          ASR      r0,r8,#8              ;2367
001c54  f88d0000          STRB     r0,[sp,#0]            ;2367
001c58  f00800ff          AND      r0,r8,#0xff           ;2368
001c5c  f88d0001          STRB     r0,[sp,#1]            ;2368
001c60  4a06              LDR      r2,|L1.7292|
001c62  6812              LDR      r2,[r2,#0]            ;2369  ; st
001c64  7e91              LDRB     r1,[r2,#0x1a]         ;2369
001c66  4a05              LDR      r2,|L1.7292|
001c68  6852              LDR      r2,[r2,#4]            ;2369  ; st
001c6a  7810              LDRB     r0,[r2,#0]            ;2369
001c6c  466b              MOV      r3,sp                 ;2369
001c6e  2202              MOVS     r2,#2                 ;2369
001c70  f7fffffe          BL       MPU_Write_Len
001c74  b138              CBZ      r0,|L1.7302|
001c76  e003              B        |L1.7296|
                  |L1.7288|
                          DCD      test
                  |L1.7292|
                          DCD      ||st||
                  |L1.7296|
001c80  f04f30ff          MOV      r0,#0xffffffff        ;2370
001c84  e7b5              B        |L1.7154|
                  |L1.7302|
001c86  2101              MOVS     r1,#1                 ;2372
001c88  48fe              LDR      r0,|L1.8324|
001c8a  f8801025          STRB     r1,[r0,#0x25]         ;2372
001c8e  f8a09026          STRH     r9,[r0,#0x26]         ;2373
001c92  2000              MOVS     r0,#0                 ;2374
001c94  e7ad              B        |L1.7154|
;;;2376   
                          ENDP

                  mpu_get_dmp_state PROC
;;;2420    */
;;;2421   int mpu_get_dmp_state(unsigned char *enabled)
001c96  4601              MOV      r1,r0
;;;2422   {
;;;2423       enabled[0] = st.chip_cfg.dmp_on;
001c98  48fa              LDR      r0,|L1.8324|
001c9a  f8900024          LDRB     r0,[r0,#0x24]
001c9e  7008              STRB     r0,[r1,#0]
;;;2424       return 0;
001ca0  2000              MOVS     r0,#0
;;;2425   }
001ca2  4770              BX       lr
;;;2426   
                          ENDP

                  setup_compass PROC
;;;2428   /* This initialization is similar to the one in ak8975.c. */
;;;2429   int setup_compass(void)
001ca4  f04f30ff          MOV      r0,#0xffffffff
;;;2430   {
;;;2431   #ifdef AK89xx_SECONDARY
;;;2432       unsigned char data[4], akm_addr;
;;;2433   
;;;2434       mpu_set_bypass(1);
;;;2435   
;;;2436       /* Find compass. Possible addresses range from 0x0C to 0x0F. */
;;;2437       for (akm_addr = 0x0C; akm_addr <= 0x0F; akm_addr++) {
;;;2438           int result;
;;;2439           result = i2c_read(akm_addr, AKM_REG_WHOAMI, 1, data);
;;;2440           if (!result && (data[0] == AKM_WHOAMI))
;;;2441               break;
;;;2442       }
;;;2443   
;;;2444       if (akm_addr > 0x0F) {
;;;2445           /* TODO: Handle this case in all compass-related functions. */
;;;2446           log_e("Compass not found.\n");
;;;2447           return -1;
;;;2448       }
;;;2449   
;;;2450       st.chip_cfg.compass_addr = akm_addr;
;;;2451   
;;;2452       data[0] = AKM_POWER_DOWN;
;;;2453       if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data))
;;;2454           return -1;
;;;2455       delay_ms(1);
;;;2456   
;;;2457       data[0] = AKM_FUSE_ROM_ACCESS;
;;;2458       if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data))
;;;2459           return -1;
;;;2460       delay_ms(1);
;;;2461   
;;;2462       /* Get sensitivity adjustment data from fuse ROM. */
;;;2463       if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_ASAX, 3, data))
;;;2464           return -1;
;;;2465       st.chip_cfg.mag_sens_adj[0] = (long)data[0] + 128;
;;;2466       st.chip_cfg.mag_sens_adj[1] = (long)data[1] + 128;
;;;2467       st.chip_cfg.mag_sens_adj[2] = (long)data[2] + 128;
;;;2468   
;;;2469       data[0] = AKM_POWER_DOWN;
;;;2470       if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data))
;;;2471           return -1;
;;;2472       delay_ms(1);
;;;2473   
;;;2474       mpu_set_bypass(0);
;;;2475   
;;;2476       /* Set up master mode, master clock, and ES bit. */
;;;2477       data[0] = 0x40;
;;;2478       if (i2c_write(st.hw->addr, st.reg->i2c_mst, 1, data))
;;;2479           return -1;
;;;2480   
;;;2481       /* Slave 0 reads from AKM data registers. */
;;;2482       data[0] = BIT_I2C_READ | st.chip_cfg.compass_addr;
;;;2483       if (i2c_write(st.hw->addr, st.reg->s0_addr, 1, data))
;;;2484           return -1;
;;;2485   
;;;2486       /* Compass reads start at this register. */
;;;2487       data[0] = AKM_REG_ST1;
;;;2488       if (i2c_write(st.hw->addr, st.reg->s0_reg, 1, data))
;;;2489           return -1;
;;;2490   
;;;2491       /* Enable slave 0, 8-byte reads. */
;;;2492       data[0] = BIT_SLAVE_EN | 8;
;;;2493       if (i2c_write(st.hw->addr, st.reg->s0_ctrl, 1, data))
;;;2494           return -1;
;;;2495   
;;;2496       /* Slave 1 changes AKM measurement mode. */
;;;2497       data[0] = st.chip_cfg.compass_addr;
;;;2498       if (i2c_write(st.hw->addr, st.reg->s1_addr, 1, data))
;;;2499           return -1;
;;;2500   
;;;2501       /* AKM measurement mode register. */
;;;2502       data[0] = AKM_REG_CNTL;
;;;2503       if (i2c_write(st.hw->addr, st.reg->s1_reg, 1, data))
;;;2504           return -1;
;;;2505   
;;;2506       /* Enable slave 1, 1-byte writes. */
;;;2507       data[0] = BIT_SLAVE_EN | 1;
;;;2508       if (i2c_write(st.hw->addr, st.reg->s1_ctrl, 1, data))
;;;2509           return -1;
;;;2510   
;;;2511       /* Set slave 1 data. */
;;;2512       data[0] = AKM_SINGLE_MEASUREMENT;
;;;2513       if (i2c_write(st.hw->addr, st.reg->s1_do, 1, data))
;;;2514           return -1;
;;;2515   
;;;2516       /* Trigger slave 0 and slave 1 actions at each sample. */
;;;2517       data[0] = 0x03;
;;;2518       if (i2c_write(st.hw->addr, st.reg->i2c_delay_ctrl, 1, data))
;;;2519           return -1;
;;;2520   
;;;2521   #ifdef MPU9150
;;;2522       /* For the MPU9150, the auxiliary I2C bus needs to be set to VDD. */
;;;2523       data[0] = BIT_I2C_MST_VDDIO;
;;;2524       if (i2c_write(st.hw->addr, st.reg->yg_offs_tc, 1, data))
;;;2525           return -1;
;;;2526   #endif
;;;2527   
;;;2528       return 0;
;;;2529   #else
;;;2530       return -1;
;;;2531   #endif
;;;2532   }
001ca8  4770              BX       lr
;;;2533   
                          ENDP

                  mpu_get_compass_reg PROC
;;;2539    */
;;;2540   int mpu_get_compass_reg(short *data, unsigned long *timestamp)
001caa  4602              MOV      r2,r0
;;;2541   {
;;;2542   #ifdef AK89xx_SECONDARY
;;;2543       unsigned char tmp[9];
;;;2544   
;;;2545       if (!(st.chip_cfg.sensors & INV_XYZ_COMPASS))
;;;2546           return -1;
;;;2547   
;;;2548   #ifdef AK89xx_BYPASS
;;;2549       if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_ST1, 8, tmp))
;;;2550           return -1;
;;;2551       tmp[8] = AKM_SINGLE_MEASUREMENT;
;;;2552       if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, tmp+8))
;;;2553           return -1;
;;;2554   #else
;;;2555       if (i2c_read(st.hw->addr, st.reg->raw_compass, 8, tmp))
;;;2556           return -1;
;;;2557   #endif
;;;2558   
;;;2559   #if defined AK8975_SECONDARY
;;;2560       /* AK8975 doesn't have the overrun error bit. */
;;;2561       if (!(tmp[0] & AKM_DATA_READY))
;;;2562           return -2;
;;;2563       if ((tmp[7] & AKM_OVERFLOW) || (tmp[7] & AKM_DATA_ERROR))
;;;2564           return -3;
;;;2565   #elif defined AK8963_SECONDARY
;;;2566       /* AK8963 doesn't have the data read error bit. */
;;;2567       if (!(tmp[0] & AKM_DATA_READY) || (tmp[0] & AKM_DATA_OVERRUN))
;;;2568           return -2;
;;;2569       if (tmp[7] & AKM_OVERFLOW)
;;;2570           return -3;
;;;2571   #endif
;;;2572       data[0] = (tmp[2] << 8) | tmp[1];
;;;2573       data[1] = (tmp[4] << 8) | tmp[3];
;;;2574       data[2] = (tmp[6] << 8) | tmp[5];
;;;2575   
;;;2576       data[0] = ((long)data[0] * st.chip_cfg.mag_sens_adj[0]) >> 8;
;;;2577       data[1] = ((long)data[1] * st.chip_cfg.mag_sens_adj[1]) >> 8;
;;;2578       data[2] = ((long)data[2] * st.chip_cfg.mag_sens_adj[2]) >> 8;
;;;2579   
;;;2580       if (timestamp)
;;;2581           get_ms(timestamp);
;;;2582       return 0;
;;;2583   #else
;;;2584       return -1;
001cac  f04f30ff          MOV      r0,#0xffffffff
;;;2585   #endif
;;;2586   }
001cb0  4770              BX       lr
;;;2587   
                          ENDP

                  mpu_get_compass_fsr PROC
;;;2592    */
;;;2593   int mpu_get_compass_fsr(unsigned short *fsr)
001cb2  4601              MOV      r1,r0
;;;2594   {
;;;2595   #ifdef AK89xx_SECONDARY
;;;2596       fsr[0] = st.hw->compass_fsr;
;;;2597       return 0;
;;;2598   #else
;;;2599       return -1;
001cb4  f04f30ff          MOV      r0,#0xffffffff
;;;2600   #endif
;;;2601   }
001cb8  4770              BX       lr
;;;2602   
                          ENDP

                  mpu_lp_motion_interrupt PROC
;;;2646    */
;;;2647   int mpu_lp_motion_interrupt(unsigned short thresh, unsigned char time,
001cba  b5f8              PUSH     {r3-r7,lr}
;;;2648       unsigned char lpa_freq)
;;;2649   {
001cbc  4605              MOV      r5,r0
001cbe  460f              MOV      r7,r1
001cc0  4614              MOV      r4,r2
;;;2650       unsigned char data[3];
;;;2651   
;;;2652       if (lpa_freq) {
001cc2  2c00              CMP      r4,#0
001cc4  d070              BEQ      |L1.7592|
;;;2653           unsigned char thresh_hw;
;;;2654   
;;;2655   #if defined MPU6050
;;;2656           /* TODO: Make these const/#defines. */
;;;2657           /* 1LSb = 32mg. */
;;;2658           if (thresh > 8160)
001cc6  f5b55fff          CMP      r5,#0x1fe0
001cca  dd01              BLE      |L1.7376|
;;;2659               thresh_hw = 255;
001ccc  26ff              MOVS     r6,#0xff
001cce  e005              B        |L1.7388|
                  |L1.7376|
;;;2660           else if (thresh < 32)
001cd0  2d20              CMP      r5,#0x20
001cd2  da01              BGE      |L1.7384|
;;;2661               thresh_hw = 1;
001cd4  2601              MOVS     r6,#1
001cd6  e001              B        |L1.7388|
                  |L1.7384|
;;;2662           else
;;;2663               thresh_hw = thresh >> 5;
001cd8  f3c51647          UBFX     r6,r5,#5,#8
                  |L1.7388|
;;;2664   #elif defined MPU6500
;;;2665           /* 1LSb = 4mg. */
;;;2666           if (thresh > 1020)
;;;2667               thresh_hw = 255;
;;;2668           else if (thresh < 4)
;;;2669               thresh_hw = 1;
;;;2670           else
;;;2671               thresh_hw = thresh >> 2;
;;;2672   #endif
;;;2673   
;;;2674           if (!time)
001cdc  b907              CBNZ     r7,|L1.7392|
;;;2675               /* Minimum duration must be 1ms. */
;;;2676               time = 1;
001cde  2701              MOVS     r7,#1
                  |L1.7392|
;;;2677   
;;;2678   #if defined MPU6050
;;;2679           if (lpa_freq > 40)
001ce0  2c28              CMP      r4,#0x28
001ce2  dd02              BLE      |L1.7402|
;;;2680   #elif defined MPU6500
;;;2681           if (lpa_freq > 640)
;;;2682   #endif
;;;2683               /* At this point, the chip has not been re-configured, so the
;;;2684                * function can safely exit.
;;;2685                */
;;;2686               return -1;
001ce4  f04f30ff          MOV      r0,#0xffffffff
                  |L1.7400|
;;;2687   
;;;2688           if (!st.chip_cfg.int_motion_only) {
;;;2689               /* Store current settings for later. */
;;;2690               if (st.chip_cfg.dmp_on) {
;;;2691                   mpu_set_dmp_state(0);
;;;2692                   st.chip_cfg.cache.dmp_on = 1;
;;;2693               } else
;;;2694                   st.chip_cfg.cache.dmp_on = 0;
;;;2695               mpu_get_gyro_fsr(&st.chip_cfg.cache.gyro_fsr);
;;;2696               mpu_get_accel_fsr(&st.chip_cfg.cache.accel_fsr);
;;;2697               mpu_get_lpf(&st.chip_cfg.cache.lpf);
;;;2698               mpu_get_sample_rate(&st.chip_cfg.cache.sample_rate);
;;;2699               st.chip_cfg.cache.sensors_on = st.chip_cfg.sensors;
;;;2700               mpu_get_fifo_config(&st.chip_cfg.cache.fifo_sensors);
;;;2701           }
;;;2702   
;;;2703   #ifdef MPU6050
;;;2704           /* Disable hardware interrupts for now. */
;;;2705           set_int_enable(0);
;;;2706   
;;;2707           /* Enter full-power accel-only mode. */
;;;2708           mpu_lp_accel_mode(0);
;;;2709   
;;;2710           /* Override current LPF (and HPF) settings to obtain a valid accel
;;;2711            * reading.
;;;2712            */
;;;2713           data[0] = INV_FILTER_256HZ_NOLPF2;
;;;2714           if (i2c_write(st.hw->addr, st.reg->lpf, 1, data))
;;;2715               return -1;
;;;2716   
;;;2717           /* NOTE: Digital high pass filter should be configured here. Since this
;;;2718            * driver doesn't modify those bits anywhere, they should already be
;;;2719            * cleared by default.
;;;2720            */
;;;2721   
;;;2722           /* Configure the device to send motion interrupts. */
;;;2723           /* Enable motion interrupt. */
;;;2724           data[0] = BIT_MOT_INT_EN;
;;;2725           if (i2c_write(st.hw->addr, st.reg->int_enable, 1, data))
;;;2726               goto lp_int_restore;
;;;2727   
;;;2728           /* Set motion interrupt parameters. */
;;;2729           data[0] = thresh_hw;
;;;2730           data[1] = time;
;;;2731           if (i2c_write(st.hw->addr, st.reg->motion_thr, 2, data))
;;;2732               goto lp_int_restore;
;;;2733   
;;;2734           /* Force hardware to "lock" current accel sample. */
;;;2735           delay_ms(5);
;;;2736           data[0] = (st.chip_cfg.accel_fsr << 3) | BITS_HPF;
;;;2737           if (i2c_write(st.hw->addr, st.reg->accel_cfg, 1, data))
;;;2738               goto lp_int_restore;
;;;2739   
;;;2740           /* Set up LP accel mode. */
;;;2741           data[0] = BIT_LPA_CYCLE;
;;;2742           if (lpa_freq == 1)
;;;2743               data[1] = INV_LPA_1_25HZ;
;;;2744           else if (lpa_freq <= 5)
;;;2745               data[1] = INV_LPA_5HZ;
;;;2746           else if (lpa_freq <= 20)
;;;2747               data[1] = INV_LPA_20HZ;
;;;2748           else
;;;2749               data[1] = INV_LPA_40HZ;
;;;2750           data[1] = (data[1] << 6) | BIT_STBY_XYZG;
;;;2751           if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 2, data))
;;;2752               goto lp_int_restore;
;;;2753   
;;;2754           st.chip_cfg.int_motion_only = 1;
;;;2755           return 0;
;;;2756   #elif defined MPU6500
;;;2757           /* Disable hardware interrupts. */
;;;2758           set_int_enable(0);
;;;2759   
;;;2760           /* Enter full-power accel-only mode, no FIFO/DMP. */
;;;2761           data[0] = 0;
;;;2762           data[1] = 0;
;;;2763           data[2] = BIT_STBY_XYZG;
;;;2764           if (i2c_write(st.hw->addr, st.reg->user_ctrl, 3, data))
;;;2765               goto lp_int_restore;
;;;2766   
;;;2767           /* Set motion threshold. */
;;;2768           data[0] = thresh_hw;
;;;2769           if (i2c_write(st.hw->addr, st.reg->motion_thr, 1, data))
;;;2770               goto lp_int_restore;
;;;2771   
;;;2772           /* Set wake frequency. */
;;;2773           if (lpa_freq == 1)
;;;2774               data[0] = INV_LPA_1_25HZ;
;;;2775           else if (lpa_freq == 2)
;;;2776               data[0] = INV_LPA_2_5HZ;
;;;2777           else if (lpa_freq <= 5)
;;;2778               data[0] = INV_LPA_5HZ;
;;;2779           else if (lpa_freq <= 10)
;;;2780               data[0] = INV_LPA_10HZ;
;;;2781           else if (lpa_freq <= 20)
;;;2782               data[0] = INV_LPA_20HZ;
;;;2783           else if (lpa_freq <= 40)
;;;2784               data[0] = INV_LPA_40HZ;
;;;2785           else if (lpa_freq <= 80)
;;;2786               data[0] = INV_LPA_80HZ;
;;;2787           else if (lpa_freq <= 160)
;;;2788               data[0] = INV_LPA_160HZ;
;;;2789           else if (lpa_freq <= 320)
;;;2790               data[0] = INV_LPA_320HZ;
;;;2791           else
;;;2792               data[0] = INV_LPA_640HZ;
;;;2793           if (i2c_write(st.hw->addr, st.reg->lp_accel_odr, 1, data))
;;;2794               goto lp_int_restore;
;;;2795   
;;;2796           /* Enable motion interrupt (MPU6500 version). */
;;;2797           data[0] = BITS_WOM_EN;
;;;2798           if (i2c_write(st.hw->addr, st.reg->accel_intel, 1, data))
;;;2799               goto lp_int_restore;
;;;2800   
;;;2801           /* Enable cycle mode. */
;;;2802           data[0] = BIT_LPA_CYCLE;
;;;2803           if (i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, data))
;;;2804               goto lp_int_restore;
;;;2805   
;;;2806           /* Enable interrupt. */
;;;2807           data[0] = BIT_MOT_INT_EN;
;;;2808           if (i2c_write(st.hw->addr, st.reg->int_enable, 1, data))
;;;2809               goto lp_int_restore;
;;;2810   
;;;2811           st.chip_cfg.int_motion_only = 1;
;;;2812           return 0;
;;;2813   #endif
;;;2814       } else {
;;;2815           /* Don't "restore" the previous state if no state has been saved. */
;;;2816           int ii;
;;;2817           char *cache_ptr = (char*)&st.chip_cfg.cache;
;;;2818           for (ii = 0; ii < sizeof(st.chip_cfg.cache); ii++) {
;;;2819               if (cache_ptr[ii] != 0)
;;;2820                   goto lp_int_restore;
;;;2821           }
;;;2822           /* If we reach this point, motion interrupt mode hasn't been used yet. */
;;;2823           return -1;
;;;2824       }
;;;2825   lp_int_restore:
;;;2826       /* Set to invalid values to ensure no I2C writes are skipped. */
;;;2827       st.chip_cfg.gyro_fsr = 0xFF;
;;;2828       st.chip_cfg.accel_fsr = 0xFF;
;;;2829       st.chip_cfg.lpf = 0xFF;
;;;2830       st.chip_cfg.sample_rate = 0xFFFF;
;;;2831       st.chip_cfg.sensors = 0xFF;
;;;2832       st.chip_cfg.fifo_enable = 0xFF;
;;;2833       st.chip_cfg.clk_src = INV_CLK_PLL;
;;;2834       mpu_set_sensors(st.chip_cfg.cache.sensors_on);
;;;2835       mpu_set_gyro_fsr(st.chip_cfg.cache.gyro_fsr);
;;;2836       mpu_set_accel_fsr(st.chip_cfg.cache.accel_fsr);
;;;2837       mpu_set_lpf(st.chip_cfg.cache.lpf);
;;;2838       mpu_set_sample_rate(st.chip_cfg.cache.sample_rate);
;;;2839       mpu_configure_fifo(st.chip_cfg.cache.fifo_sensors);
;;;2840   
;;;2841       if (st.chip_cfg.cache.dmp_on)
;;;2842           mpu_set_dmp_state(1);
;;;2843   
;;;2844   #ifdef MPU6500
;;;2845       /* Disable motion interrupt (MPU6500 version). */
;;;2846       data[0] = 0;
;;;2847       if (i2c_write(st.hw->addr, st.reg->accel_intel, 1, data))
;;;2848           goto lp_int_restore;
;;;2849   #endif
;;;2850   
;;;2851       st.chip_cfg.int_motion_only = 0;
;;;2852       return 0;
;;;2853   }
001ce8  bdf8              POP      {r3-r7,pc}
                  |L1.7402|
001cea  48e6              LDR      r0,|L1.8324|
001cec  7d40              LDRB     r0,[r0,#0x15]         ;2688
001cee  bb28              CBNZ     r0,|L1.7484|
001cf0  48e4              LDR      r0,|L1.8324|
001cf2  f8900024          LDRB     r0,[r0,#0x24]         ;2690
001cf6  b138              CBZ      r0,|L1.7432|
001cf8  2000              MOVS     r0,#0                 ;2691
001cfa  f7fffffe          BL       mpu_set_dmp_state
001cfe  2101              MOVS     r1,#1                 ;2692
001d00  48e0              LDR      r0,|L1.8324|
001d02  f8801020          STRB     r1,[r0,#0x20]         ;2692
001d06  e003              B        |L1.7440|
                  |L1.7432|
001d08  2100              MOVS     r1,#0                 ;2694
001d0a  48de              LDR      r0,|L1.8324|
001d0c  f8801020          STRB     r1,[r0,#0x20]         ;2694
                  |L1.7440|
001d10  48dc              LDR      r0,|L1.8324|
001d12  3016              ADDS     r0,r0,#0x16           ;2695
001d14  f7fffffe          BL       mpu_get_gyro_fsr
001d18  48da              LDR      r0,|L1.8324|
001d1a  3018              ADDS     r0,r0,#0x18           ;2696
001d1c  f7fffffe          BL       mpu_get_accel_fsr
001d20  48d8              LDR      r0,|L1.8324|
001d22  301a              ADDS     r0,r0,#0x1a           ;2697
001d24  f7fffffe          BL       mpu_get_lpf
001d28  48d6              LDR      r0,|L1.8324|
001d2a  301c              ADDS     r0,r0,#0x1c           ;2698
001d2c  f7fffffe          BL       mpu_get_sample_rate
001d30  48d4              LDR      r0,|L1.8324|
001d32  7a81              LDRB     r1,[r0,#0xa]          ;2699
001d34  7781              STRB     r1,[r0,#0x1e]         ;2699
001d36  301f              ADDS     r0,r0,#0x1f           ;2700
001d38  f7fffffe          BL       mpu_get_fifo_config
                  |L1.7484|
001d3c  2000              MOVS     r0,#0                 ;2705
001d3e  f7fffffe          BL       set_int_enable
001d42  2000              MOVS     r0,#0                 ;2708
001d44  f7fffffe          BL       mpu_lp_accel_mode
001d48  2000              MOVS     r0,#0                 ;2713
001d4a  f88d0000          STRB     r0,[sp,#0]            ;2713
001d4e  4acd              LDR      r2,|L1.8324|
001d50  6812              LDR      r2,[r2,#0]            ;2714  ; st
001d52  7891              LDRB     r1,[r2,#2]            ;2714
001d54  4acb              LDR      r2,|L1.8324|
001d56  6852              LDR      r2,[r2,#4]            ;2714  ; st
001d58  7810              LDRB     r0,[r2,#0]            ;2714
001d5a  466b              MOV      r3,sp                 ;2714
001d5c  2201              MOVS     r2,#1                 ;2714
001d5e  f7fffffe          BL       MPU_Write_Len
001d62  b110              CBZ      r0,|L1.7530|
001d64  f04f30ff          MOV      r0,#0xffffffff        ;2715
001d68  e7be              B        |L1.7400|
                  |L1.7530|
001d6a  2040              MOVS     r0,#0x40              ;2724
001d6c  f88d0000          STRB     r0,[sp,#0]            ;2724
001d70  4ac4              LDR      r2,|L1.8324|
001d72  6812              LDR      r2,[r2,#0]            ;2725  ; st
001d74  7bd1              LDRB     r1,[r2,#0xf]          ;2725
001d76  4ac3              LDR      r2,|L1.8324|
001d78  6852              LDR      r2,[r2,#4]            ;2725  ; st
001d7a  7810              LDRB     r0,[r2,#0]            ;2725
001d7c  466b              MOV      r3,sp                 ;2725
001d7e  2201              MOVS     r2,#1                 ;2725
001d80  f7fffffe          BL       MPU_Write_Len
001d84  b100              CBZ      r0,|L1.7560|
001d86  e065              B        |L1.7764|
                  |L1.7560|
001d88  f88d6000          STRB     r6,[sp,#0]            ;2729
001d8c  f88d7001          STRB     r7,[sp,#1]            ;2730
001d90  4abc              LDR      r2,|L1.8324|
001d92  6812              LDR      r2,[r2,#0]            ;2731  ; st
001d94  7a11              LDRB     r1,[r2,#8]            ;2731
001d96  4abb              LDR      r2,|L1.8324|
001d98  6852              LDR      r2,[r2,#4]            ;2731  ; st
001d9a  7810              LDRB     r0,[r2,#0]            ;2731
001d9c  466b              MOV      r3,sp                 ;2731
001d9e  2202              MOVS     r2,#2                 ;2731
001da0  f7fffffe          BL       MPU_Write_Len
001da4  b108              CBZ      r0,|L1.7594|
001da6  e055              B        |L1.7764|
                  |L1.7592|
001da8  e047              B        |L1.7738|
                  |L1.7594|
001daa  2005              MOVS     r0,#5                 ;2735
001dac  f7fffffe          BL       delay_ms
001db0  48b4              LDR      r0,|L1.8324|
001db2  7a40              LDRB     r0,[r0,#9]            ;2736
001db4  2107              MOVS     r1,#7                 ;2736
001db6  eb0100c0          ADD      r0,r1,r0,LSL #3       ;2736
001dba  b2c0              UXTB     r0,r0                 ;2736
001dbc  f88d0000          STRB     r0,[sp,#0]            ;2736
001dc0  4ab0              LDR      r2,|L1.8324|
001dc2  6812              LDR      r2,[r2,#0]            ;2737  ; st
001dc4  79d1              LDRB     r1,[r2,#7]            ;2737
001dc6  4aaf              LDR      r2,|L1.8324|
001dc8  6852              LDR      r2,[r2,#4]            ;2737  ; st
001dca  7810              LDRB     r0,[r2,#0]            ;2737
001dcc  466b              MOV      r3,sp                 ;2737
001dce  2201              MOVS     r2,#1                 ;2737
001dd0  f7fffffe          BL       MPU_Write_Len
001dd4  b100              CBZ      r0,|L1.7640|
001dd6  e03d              B        |L1.7764|
                  |L1.7640|
001dd8  2020              MOVS     r0,#0x20              ;2741
001dda  f88d0000          STRB     r0,[sp,#0]            ;2741
001dde  2c01              CMP      r4,#1                 ;2742
001de0  d103              BNE      |L1.7658|
001de2  2000              MOVS     r0,#0                 ;2743
001de4  f88d0001          STRB     r0,[sp,#1]            ;2743
001de8  e00e              B        |L1.7688|
                  |L1.7658|
001dea  2c05              CMP      r4,#5                 ;2744
001dec  dc03              BGT      |L1.7670|
001dee  2001              MOVS     r0,#1                 ;2745
001df0  f88d0001          STRB     r0,[sp,#1]            ;2745
001df4  e008              B        |L1.7688|
                  |L1.7670|
001df6  2c14              CMP      r4,#0x14              ;2746
001df8  dc03              BGT      |L1.7682|
001dfa  2002              MOVS     r0,#2                 ;2747
001dfc  f88d0001          STRB     r0,[sp,#1]            ;2747
001e00  e002              B        |L1.7688|
                  |L1.7682|
001e02  2003              MOVS     r0,#3                 ;2749
001e04  f88d0001          STRB     r0,[sp,#1]            ;2749
                  |L1.7688|
001e08  f89d0001          LDRB     r0,[sp,#1]            ;2750
001e0c  2107              MOVS     r1,#7                 ;2750
001e0e  eb011080          ADD      r0,r1,r0,LSL #6       ;2750
001e12  b2c0              UXTB     r0,r0                 ;2750
001e14  f88d0001          STRB     r0,[sp,#1]            ;2750
001e18  4a9a              LDR      r2,|L1.8324|
001e1a  6812              LDR      r2,[r2,#0]            ;2751  ; st
001e1c  7c91              LDRB     r1,[r2,#0x12]         ;2751
001e1e  4a99              LDR      r2,|L1.8324|
001e20  6852              LDR      r2,[r2,#4]            ;2751  ; st
001e22  7810              LDRB     r0,[r2,#0]            ;2751
001e24  466b              MOV      r3,sp                 ;2751
001e26  2202              MOVS     r2,#2                 ;2751
001e28  f7fffffe          BL       MPU_Write_Len
001e2c  b100              CBZ      r0,|L1.7728|
001e2e  e011              B        |L1.7764|
                  |L1.7728|
001e30  2101              MOVS     r1,#1                 ;2754
001e32  4894              LDR      r0,|L1.8324|
001e34  7541              STRB     r1,[r0,#0x15]         ;2754
001e36  2000              MOVS     r0,#0                 ;2755
001e38  e756              B        |L1.7400|
                  |L1.7738|
001e3a  4a92              LDR      r2,|L1.8324|
001e3c  3216              ADDS     r2,r2,#0x16           ;2817
001e3e  2100              MOVS     r1,#0                 ;2818
001e40  e003              B        |L1.7754|
                  |L1.7746|
001e42  5c50              LDRB     r0,[r2,r1]            ;2819
001e44  b100              CBZ      r0,|L1.7752|
001e46  e005              B        |L1.7764|
                  |L1.7752|
001e48  1c49              ADDS     r1,r1,#1              ;2818
                  |L1.7754|
001e4a  290c              CMP      r1,#0xc               ;2818
001e4c  d3f9              BCC      |L1.7746|
001e4e  f04f30ff          MOV      r0,#0xffffffff        ;2823
001e52  e749              B        |L1.7400|
                  |L1.7764|
001e54  20ff              MOVS     r0,#0xff              ;2827
001e56  498b              LDR      r1,|L1.8324|
001e58  7208              STRB     r0,[r1,#8]            ;2827
001e5a  21ff              MOVS     r1,#0xff              ;2828
001e5c  4889              LDR      r0,|L1.8324|
001e5e  7241              STRB     r1,[r0,#9]            ;2828
001e60  72c1              STRB     r1,[r0,#0xb]          ;2829
001e62  f64f71ff          MOV      r1,#0xffff            ;2830
001e66  81c1              STRH     r1,[r0,#0xe]          ;2830
001e68  21ff              MOVS     r1,#0xff              ;2831
001e6a  7281              STRB     r1,[r0,#0xa]          ;2831
001e6c  7401              STRB     r1,[r0,#0x10]         ;2832
001e6e  2101              MOVS     r1,#1                 ;2833
001e70  7301              STRB     r1,[r0,#0xc]          ;2833
001e72  4601              MOV      r1,r0                 ;2834
001e74  7f88              LDRB     r0,[r1,#0x1e]         ;2834
001e76  f7fffffe          BL       mpu_set_sensors
001e7a  4982              LDR      r1,|L1.8324|
001e7c  8ac8              LDRH     r0,[r1,#0x16]         ;2835
001e7e  f7fffffe          BL       mpu_set_gyro_fsr
001e82  4980              LDR      r1,|L1.8324|
001e84  7e08              LDRB     r0,[r1,#0x18]         ;2836
001e86  f7fffffe          BL       mpu_set_accel_fsr
001e8a  497e              LDR      r1,|L1.8324|
001e8c  8b48              LDRH     r0,[r1,#0x1a]         ;2837
001e8e  f7fffffe          BL       mpu_set_lpf
001e92  497c              LDR      r1,|L1.8324|
001e94  8b88              LDRH     r0,[r1,#0x1c]         ;2838
001e96  f7fffffe          BL       mpu_set_sample_rate
001e9a  497a              LDR      r1,|L1.8324|
001e9c  7fc8              LDRB     r0,[r1,#0x1f]         ;2839
001e9e  f7fffffe          BL       mpu_configure_fifo
001ea2  4878              LDR      r0,|L1.8324|
001ea4  f8900020          LDRB     r0,[r0,#0x20]         ;2841
001ea8  b110              CBZ      r0,|L1.7856|
001eaa  2001              MOVS     r0,#1                 ;2842
001eac  f7fffffe          BL       mpu_set_dmp_state
                  |L1.7856|
001eb0  2100              MOVS     r1,#0                 ;2851
001eb2  4874              LDR      r0,|L1.8324|
001eb4  7541              STRB     r1,[r0,#0x15]         ;2851
001eb6  2000              MOVS     r0,#0                 ;2852
001eb8  e716              B        |L1.7400|
;;;2854   //////////////////////////////////////////////////////////////////////////////////
                          ENDP

                  run_self_test PROC
;;;2878   //    ,ʧ
;;;2879   u8 run_self_test(void)
001eba  b570              PUSH     {r4-r6,lr}
;;;2880   {
001ebc  b088              SUB      sp,sp,#0x20
;;;2881   	int result;
;;;2882   	//char test_packet[4] = {0};
;;;2883   	long gyro[3], accel[3]; 
;;;2884   	result = mpu_run_self_test(gyro, accel);
001ebe  a902              ADD      r1,sp,#8
001ec0  a805              ADD      r0,sp,#0x14
001ec2  f7fffffe          BL       mpu_run_self_test
001ec6  4604              MOV      r4,r0
;;;2885   	if (result == 0x3) 
001ec8  2c03              CMP      r4,#3
001eca  d13e              BNE      |L1.8010|
;;;2886   	{
;;;2887   		/* Test passed. We can trust the gyro data here, so let's push it down
;;;2888   		* to the DMP.
;;;2889   		*/
;;;2890   		float sens;
;;;2891   		unsigned short accel_sens;
;;;2892   		mpu_get_gyro_sens(&sens);
001ecc  a801              ADD      r0,sp,#4
001ece  f7fffffe          BL       mpu_get_gyro_sens
;;;2893   		gyro[0] = (long)(gyro[0] * sens);
001ed2  9805              LDR      r0,[sp,#0x14]
001ed4  f7fffffe          BL       __aeabi_i2f
001ed8  4606              MOV      r6,r0
001eda  9901              LDR      r1,[sp,#4]
001edc  f7fffffe          BL       __aeabi_fmul
001ee0  4605              MOV      r5,r0
001ee2  f7fffffe          BL       __aeabi_f2iz
001ee6  9005              STR      r0,[sp,#0x14]
;;;2894   		gyro[1] = (long)(gyro[1] * sens);
001ee8  9806              LDR      r0,[sp,#0x18]
001eea  f7fffffe          BL       __aeabi_i2f
001eee  4606              MOV      r6,r0
001ef0  9901              LDR      r1,[sp,#4]
001ef2  f7fffffe          BL       __aeabi_fmul
001ef6  4605              MOV      r5,r0
001ef8  f7fffffe          BL       __aeabi_f2iz
001efc  9006              STR      r0,[sp,#0x18]
;;;2895   		gyro[2] = (long)(gyro[2] * sens);
001efe  9807              LDR      r0,[sp,#0x1c]
001f00  f7fffffe          BL       __aeabi_i2f
001f04  4606              MOV      r6,r0
001f06  9901              LDR      r1,[sp,#4]
001f08  f7fffffe          BL       __aeabi_fmul
001f0c  4605              MOV      r5,r0
001f0e  f7fffffe          BL       __aeabi_f2iz
001f12  9007              STR      r0,[sp,#0x1c]
;;;2896   		dmp_set_gyro_bias(gyro);
001f14  a805              ADD      r0,sp,#0x14
001f16  f7fffffe          BL       dmp_set_gyro_bias
;;;2897   		mpu_get_accel_sens(&accel_sens);
001f1a  4668              MOV      r0,sp
001f1c  f7fffffe          BL       mpu_get_accel_sens
;;;2898   		accel[0] *= accel_sens;
001f20  f8bd1000          LDRH     r1,[sp,#0]
001f24  9802              LDR      r0,[sp,#8]
001f26  4348              MULS     r0,r1,r0
001f28  9002              STR      r0,[sp,#8]
;;;2899   		accel[1] *= accel_sens;
001f2a  f8bd1000          LDRH     r1,[sp,#0]
001f2e  9803              LDR      r0,[sp,#0xc]
001f30  4348              MULS     r0,r1,r0
001f32  9003              STR      r0,[sp,#0xc]
;;;2900   		accel[2] *= accel_sens;
001f34  f8bd1000          LDRH     r1,[sp,#0]
001f38  9804              LDR      r0,[sp,#0x10]
001f3a  4348              MULS     r0,r1,r0
001f3c  9004              STR      r0,[sp,#0x10]
;;;2901   		dmp_set_accel_bias(accel);
001f3e  a802              ADD      r0,sp,#8
001f40  f7fffffe          BL       dmp_set_accel_bias
;;;2902   		return 0;
001f44  2000              MOVS     r0,#0
                  |L1.8006|
;;;2903   	}else return 1;
;;;2904   }
001f46  b008              ADD      sp,sp,#0x20
001f48  bd70              POP      {r4-r6,pc}
                  |L1.8010|
001f4a  2001              MOVS     r0,#1                 ;2903
001f4c  e7fb              B        |L1.8006|
;;;2905   //Ƿ
                          ENDP

                  inv_row_2_scale PROC
;;;2926   //ת
;;;2927   unsigned short inv_row_2_scale(const signed char *row)
001f4e  4601              MOV      r1,r0
;;;2928   {
;;;2929       unsigned short b;
;;;2930   
;;;2931       if (row[0] > 0)
001f50  f9912000          LDRSB    r2,[r1,#0]
001f54  2a00              CMP      r2,#0
001f56  dd01              BLE      |L1.8028|
;;;2932           b = 0;
001f58  2000              MOVS     r0,#0
001f5a  e01e              B        |L1.8090|
                  |L1.8028|
;;;2933       else if (row[0] < 0)
001f5c  f9912000          LDRSB    r2,[r1,#0]
001f60  2a00              CMP      r2,#0
001f62  da01              BGE      |L1.8040|
;;;2934           b = 4;
001f64  2004              MOVS     r0,#4
001f66  e018              B        |L1.8090|
                  |L1.8040|
;;;2935       else if (row[1] > 0)
001f68  f9912001          LDRSB    r2,[r1,#1]
001f6c  2a00              CMP      r2,#0
001f6e  dd01              BLE      |L1.8052|
;;;2936           b = 1;
001f70  2001              MOVS     r0,#1
001f72  e012              B        |L1.8090|
                  |L1.8052|
;;;2937       else if (row[1] < 0)
001f74  f9912001          LDRSB    r2,[r1,#1]
001f78  2a00              CMP      r2,#0
001f7a  da01              BGE      |L1.8064|
;;;2938           b = 5;
001f7c  2005              MOVS     r0,#5
001f7e  e00c              B        |L1.8090|
                  |L1.8064|
;;;2939       else if (row[2] > 0)
001f80  f9912002          LDRSB    r2,[r1,#2]
001f84  2a00              CMP      r2,#0
001f86  dd01              BLE      |L1.8076|
;;;2940           b = 2;
001f88  2002              MOVS     r0,#2
001f8a  e006              B        |L1.8090|
                  |L1.8076|
;;;2941       else if (row[2] < 0)
001f8c  f9912002          LDRSB    r2,[r1,#2]
001f90  2a00              CMP      r2,#0
001f92  da01              BGE      |L1.8088|
;;;2942           b = 6;
001f94  2006              MOVS     r0,#6
001f96  e000              B        |L1.8090|
                  |L1.8088|
;;;2943       else
;;;2944           b = 7;      // error
001f98  2007              MOVS     r0,#7
                  |L1.8090|
;;;2945       return b;
;;;2946   }
001f9a  4770              BX       lr
;;;2947   
                          ENDP

                  inv_orientation_matrix_to_scalar PROC
;;;2905   //Ƿ
;;;2906   unsigned short inv_orientation_matrix_to_scalar(
001f9c  b510              PUSH     {r4,lr}
;;;2907       const signed char *mtx)
;;;2908   {
001f9e  4604              MOV      r4,r0
;;;2909       unsigned short scalar; 
;;;2910       /*
;;;2911          XYZ  010_001_000 Identity Matrix
;;;2912          XZY  001_010_000
;;;2913          YXZ  010_000_001
;;;2914          YZX  000_010_001
;;;2915          ZXY  001_000_010
;;;2916          ZYX  000_001_010
;;;2917        */
;;;2918   
;;;2919       scalar = inv_row_2_scale(mtx);
001fa0  4620              MOV      r0,r4
001fa2  f7fffffe          BL       inv_row_2_scale
001fa6  4603              MOV      r3,r0
;;;2920       scalar |= inv_row_2_scale(mtx + 3) << 3;
001fa8  1ce0              ADDS     r0,r4,#3
001faa  f7fffffe          BL       inv_row_2_scale
001fae  ea4300c0          ORR      r0,r3,r0,LSL #3
001fb2  b283              UXTH     r3,r0
;;;2921       scalar |= inv_row_2_scale(mtx + 6) << 6;
001fb4  1da0              ADDS     r0,r4,#6
001fb6  f7fffffe          BL       inv_row_2_scale
001fba  ea431080          ORR      r0,r3,r0,LSL #6
001fbe  b283              UXTH     r3,r0
;;;2922   
;;;2923   
;;;2924       return scalar;
001fc0  4618              MOV      r0,r3
;;;2925   }
001fc2  bd10              POP      {r4,pc}
;;;2926   //ת
                          ENDP

                  mpu_dmp_init PROC
;;;2954   //    ,ʧ
;;;2955   u8 mpu_dmp_init(void)
001fc4  b570              PUSH     {r4-r6,lr}
;;;2956   {
;;;2957   	u8 res=0;
001fc6  2400              MOVS     r4,#0
;;;2958   	
;;;2959   	MPU_I2C_Init(); 	//ʼI2C
001fc8  f7fffffe          BL       MPU_I2C_Init
;;;2960   	
;;;2961       delay_ms(1000);
001fcc  f44f707a          MOV      r0,#0x3e8
001fd0  f7fffffe          BL       delay_ms
;;;2962   	
;;;2963   	if(mpu_init()==0)	//ʼMPU6050
001fd4  f7fffffe          BL       mpu_init
001fd8  2800              CMP      r0,#0
001fda  d13e              BNE      |L1.8282|
;;;2964   	{	//GPIO_SetBits(GPIOB, GPIO_Pin_5);
;;;2965   		res=mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL);//ҪĴ
001fdc  2078              MOVS     r0,#0x78
001fde  f7fffffe          BL       mpu_set_sensors
001fe2  b2c4              UXTB     r4,r0
;;;2966   		if(res)return 1;
001fe4  b10c              CBZ      r4,|L1.8170|
001fe6  2001              MOVS     r0,#1
                  |L1.8168|
;;;2967   		res=mpu_configure_fifo(INV_XYZ_GYRO|INV_XYZ_ACCEL);//FIFO
;;;2968   		if(res)return 2; 
;;;2969   		MPU_I2C_Delay();
;;;2970   		res=mpu_set_sample_rate(DEFAULT_MPU_HZ);	//ò
;;;2971   		if(res)return 3; 
;;;2972   		MPU_I2C_Delay();
;;;2973   		res=dmp_load_motion_driver_firmware();		//dmp̼
;;;2974   		if(res)return 4;  //uprintf(USART1," ϵͳɹ!\r\n");   
;;;2975   		res=dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));//Ƿ
;;;2976   		if(res)return 5; 
;;;2977   		res=dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_TAP|	//dmp
;;;2978   		    DMP_FEATURE_ANDROID_ORIENT|DMP_FEATURE_SEND_RAW_ACCEL|DMP_FEATURE_SEND_CAL_GYRO|
;;;2979   		    DMP_FEATURE_GYRO_CAL);
;;;2980   		if(res)return 6; 
;;;2981   		res=dmp_set_fifo_rate(DEFAULT_MPU_HZ);	//DMP(󲻳200Hz)
;;;2982   		if(res)return 7;  
;;;2983   		//res=run_self_test();		//Լ
;;;2984   		//if(res)return 8;    
;;;2985   		res=mpu_set_dmp_state(1);	//ʹDMP
;;;2986   		
;;;2987   		if(res)return 9;     
;;;2988   	}else return 10;
;;;2989   	return 0;
;;;2990   }
001fe8  bd70              POP      {r4-r6,pc}
                  |L1.8170|
001fea  2078              MOVS     r0,#0x78              ;2967
001fec  f7fffffe          BL       mpu_configure_fifo
001ff0  b2c4              UXTB     r4,r0                 ;2967
001ff2  b10c              CBZ      r4,|L1.8184|
001ff4  2002              MOVS     r0,#2                 ;2968
001ff6  e7f7              B        |L1.8168|
                  |L1.8184|
001ff8  f7fffffe          BL       MPU_I2C_Delay
001ffc  2064              MOVS     r0,#0x64              ;2970
001ffe  f7fffffe          BL       mpu_set_sample_rate
002002  b2c4              UXTB     r4,r0                 ;2970
002004  b10c              CBZ      r4,|L1.8202|
002006  2003              MOVS     r0,#3                 ;2971
002008  e7ee              B        |L1.8168|
                  |L1.8202|
00200a  f7fffffe          BL       MPU_I2C_Delay
00200e  f7fffffe          BL       dmp_load_motion_driver_firmware
002012  b2c4              UXTB     r4,r0                 ;2973
002014  b10c              CBZ      r4,|L1.8218|
002016  2004              MOVS     r0,#4                 ;2974
002018  e7e6              B        |L1.8168|
                  |L1.8218|
00201a  481b              LDR      r0,|L1.8328|
00201c  f7fffffe          BL       inv_orientation_matrix_to_scalar
002020  4605              MOV      r5,r0                 ;2975
002022  f7fffffe          BL       dmp_set_orientation
002026  b2c4              UXTB     r4,r0                 ;2975
002028  b10c              CBZ      r4,|L1.8238|
00202a  2005              MOVS     r0,#5                 ;2976
00202c  e7dc              B        |L1.8168|
                  |L1.8238|
00202e  f2401073          MOV      r0,#0x173             ;2977
002032  f7fffffe          BL       dmp_enable_feature
002036  b2c4              UXTB     r4,r0                 ;2977
002038  b10c              CBZ      r4,|L1.8254|
00203a  2006              MOVS     r0,#6                 ;2980
00203c  e7d4              B        |L1.8168|
                  |L1.8254|
00203e  2064              MOVS     r0,#0x64              ;2981
002040  f7fffffe          BL       dmp_set_fifo_rate
002044  b2c4              UXTB     r4,r0                 ;2981
002046  b10c              CBZ      r4,|L1.8268|
002048  2007              MOVS     r0,#7                 ;2982
00204a  e7cd              B        |L1.8168|
                  |L1.8268|
00204c  2001              MOVS     r0,#1                 ;2985
00204e  f7fffffe          BL       mpu_set_dmp_state
002052  b2c4              UXTB     r4,r0                 ;2985
002054  b11c              CBZ      r4,|L1.8286|
002056  2009              MOVS     r0,#9                 ;2987
002058  e7c6              B        |L1.8168|
                  |L1.8282|
00205a  200a              MOVS     r0,#0xa               ;2988
00205c  e7c4              B        |L1.8168|
                  |L1.8286|
00205e  2000              MOVS     r0,#0                 ;2989
002060  e7c2              B        |L1.8168|
;;;2991   
                          ENDP

                  mpu_dmp_get_data PROC
;;;2997   //    ,ʧ
;;;2998   unsigned char mpu_dmp_get_data(float *pitch,float *roll,float *yaw)
002062  e92d4ff0          PUSH     {r4-r11,lr}
;;;2999   {
002066  b099              SUB      sp,sp,#0x64
002068  4604              MOV      r4,r0
00206a  460d              MOV      r5,r1
00206c  4616              MOV      r6,r2
;;;3000   	float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
00206e  f04f577e          MOV      r7,#0x3f800000
002072  f04f0800          MOV      r8,#0
002076  f04f0900          MOV      r9,#0
00207a  f04f0a00          MOV      r10,#0
;;;3001   	unsigned long sensor_timestamp;
;;;3002   	short gyro[3], accel[3], sensors;
;;;3003   	unsigned char more;
;;;3004   	long quat[4]; 
;;;3005   	//GPIO_SetBits(GPIOB, GPIO_Pin_5);	     
;;;3006   	if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more))return 1;	 
00207e  a812              ADD      r0,sp,#0x48
002080  a913              ADD      r1,sp,#0x4c
002082  e003              B        |L1.8332|
                  |L1.8324|
                          DCD      ||st||
                  |L1.8328|
                          DCD      gyro_orientation
                  |L1.8332|
00208c  ab18              ADD      r3,sp,#0x60
00208e  aa0e              ADD      r2,sp,#0x38
002090  e9cd1000          STRD     r1,r0,[sp,#0]
002094  a914              ADD      r1,sp,#0x50
002096  a816              ADD      r0,sp,#0x58
002098  f7fffffe          BL       dmp_read_fifo
00209c  b118              CBZ      r0,|L1.8358|
00209e  2001              MOVS     r0,#1
                  |L1.8352|
;;;3007   	/* Gyro and accel data are written to the FIFO by the DMP in chip frame and hardware units.
;;;3008   	 * This behavior is convenient because it keeps the gyro and accel outputs of dmp_read_fifo and mpu_read_fifo consistent.
;;;3009   	**/
;;;3010   	/*if (sensors & INV_XYZ_GYRO )
;;;3011   	send_packet(PACKET_TYPE_GYRO, gyro);
;;;3012   	if (sensors & INV_XYZ_ACCEL)
;;;3013   	send_packet(PACKET_TYPE_ACCEL, accel); */
;;;3014   	/* Unlike gyro and accel, quaternions are written to the FIFO in the body frame, q30.
;;;3015   	 * The orientation is set by the scalar passed to dmp_set_orientation during initialization. 
;;;3016   	**/
;;;3017   	if(sensors&INV_WXYZ_QUAT) 
;;;3018   	{
;;;3019   		q0 = quat[0] / q30;	//q30ʽתΪ
;;;3020   		q1 = quat[1] / q30;
;;;3021   		q2 = quat[2] / q30;
;;;3022   		q3 = quat[3] / q30; 
;;;3023   		//õ//
;;;3024   		*pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;	// pitch
;;;3025   		*roll  = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3;	// roll
;;;3026   		*yaw   = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;	//yaw
;;;3027   	}else return 2;
;;;3028   	return 0;
;;;3029   }
0020a0  b019              ADD      sp,sp,#0x64
0020a2  e8bd8ff0          POP      {r4-r11,pc}
                  |L1.8358|
0020a6  f8bd004c          LDRH     r0,[sp,#0x4c]         ;3017
0020aa  f4007080          AND      r0,r0,#0x100          ;3017
0020ae  2800              CMP      r0,#0                 ;3017
0020b0  d07e              BEQ      |L1.8624|
0020b2  980e              LDR      r0,[sp,#0x38]         ;3019
0020b4  f7fffffe          BL       __aeabi_i2f
0020b8  4683              MOV      r11,r0                ;3019
0020ba  f04f419d          MOV      r1,#0x4e800000        ;3019
0020be  f7fffffe          BL       __aeabi_fdiv
0020c2  4607              MOV      r7,r0                 ;3019
0020c4  980f              LDR      r0,[sp,#0x3c]         ;3020
0020c6  f7fffffe          BL       __aeabi_i2f
0020ca  4683              MOV      r11,r0                ;3020
0020cc  f04f419d          MOV      r1,#0x4e800000        ;3020
0020d0  f7fffffe          BL       __aeabi_fdiv
0020d4  4680              MOV      r8,r0                 ;3020
0020d6  9810              LDR      r0,[sp,#0x40]         ;3021
0020d8  f7fffffe          BL       __aeabi_i2f
0020dc  4683              MOV      r11,r0                ;3021
0020de  f04f419d          MOV      r1,#0x4e800000        ;3021
0020e2  f7fffffe          BL       __aeabi_fdiv
0020e6  4681              MOV      r9,r0                 ;3021
0020e8  9811              LDR      r0,[sp,#0x44]         ;3022
0020ea  f7fffffe          BL       __aeabi_i2f
0020ee  4683              MOV      r11,r0                ;3022
0020f0  f04f419d          MOV      r1,#0x4e800000        ;3022
0020f4  f7fffffe          BL       __aeabi_fdiv
0020f8  4682              MOV      r10,r0                ;3022
0020fa  4639              MOV      r1,r7                 ;3024
0020fc  f04f4080          MOV      r0,#0x40000000        ;3024
002100  f7fffffe          BL       __aeabi_fmul
002104  4649              MOV      r1,r9                 ;3024
002106  9005              STR      r0,[sp,#0x14]         ;3024
002108  f7fffffe          BL       __aeabi_fmul
00210c  4641              MOV      r1,r8                 ;3024
00210e  9007              STR      r0,[sp,#0x1c]         ;3024
002110  f04f4040          MOV      r0,#0xc0000000        ;3024
002114  f7fffffe          BL       __aeabi_fmul
002118  4651              MOV      r1,r10                ;3024
00211a  9005              STR      r0,[sp,#0x14]         ;3024
00211c  f7fffffe          BL       __aeabi_fmul
002120  9006              STR      r0,[sp,#0x18]         ;3024
002122  9907              LDR      r1,[sp,#0x1c]         ;3024
002124  f7fffffe          BL       __aeabi_fadd
002128  4683              MOV      r11,r0                ;3024
00212a  f7fffffe          BL       __aeabi_f2d
00212e  e9cd0108          STRD     r0,r1,[sp,#0x20]      ;3024
002132  f7fffffe          BL       asin
002136  f04f3266          MOV      r2,#0x66666666        ;3024
00213a  4b54              LDR      r3,|L1.8844|
00213c  e9cd010a          STRD     r0,r1,[sp,#0x28]      ;3024
002140  f7fffffe          BL       __aeabi_dmul
002144  e9cd010c          STRD     r0,r1,[sp,#0x30]      ;3024
002148  f7fffffe          BL       __aeabi_d2f
00214c  6020              STR      r0,[r4,#0]            ;3024
00214e  4649              MOV      r1,r9                 ;3025
002150  f04f4080          MOV      r0,#0x40000000        ;3025
002154  f7fffffe          BL       __aeabi_fmul
002158  4649              MOV      r1,r9                 ;3025
00215a  9002              STR      r0,[sp,#8]            ;3025
00215c  f7fffffe          BL       __aeabi_fmul
002160  4641              MOV      r1,r8                 ;3025
002162  9004              STR      r0,[sp,#0x10]         ;3025
002164  f04f4040          MOV      r0,#0xc0000000        ;3025
002168  f7fffffe          BL       __aeabi_fmul
00216c  4641              MOV      r1,r8                 ;3025
00216e  9002              STR      r0,[sp,#8]            ;3025
002170  f7fffffe          BL       __aeabi_fmul
002174  9003              STR      r0,[sp,#0xc]          ;3025
002176  9904              LDR      r1,[sp,#0x10]         ;3025
002178  f7fffffe          BL       __aeabi_fsub
00217c  4683              MOV      r11,r0                ;3025
00217e  f04f517e          MOV      r1,#0x3f800000        ;3025
002182  f7fffffe          BL       __aeabi_fadd
002186  9005              STR      r0,[sp,#0x14]         ;3025
002188  f7fffffe          BL       __aeabi_f2d
00218c  e9cd0108          STRD     r0,r1,[sp,#0x20]      ;3025
002190  4639              MOV      r1,r7                 ;3025
002192  f04f4080          MOV      r0,#0x40000000        ;3025
002196  f7fffffe          BL       __aeabi_fmul
00219a  4641              MOV      r1,r8                 ;3025
00219c  9003              STR      r0,[sp,#0xc]          ;3025
00219e  f7fffffe          BL       __aeabi_fmul
0021a2  4649              MOV      r1,r9                 ;3025
0021a4  9004              STR      r0,[sp,#0x10]         ;3025
0021a6  f04f4080          MOV      r0,#0x40000000        ;3025
0021aa  f7fffffe          BL       __aeabi_fmul
0021ae  e000              B        |L1.8626|
                  |L1.8624|
0021b0  e068              B        |L1.8836|
                  |L1.8626|
0021b2  4651              MOV      r1,r10                ;3025
0021b4  9003              STR      r0,[sp,#0xc]          ;3025
0021b6  f7fffffe          BL       __aeabi_fmul
0021ba  4683              MOV      r11,r0                ;3025
0021bc  9904              LDR      r1,[sp,#0x10]         ;3025
0021be  f7fffffe          BL       __aeabi_fadd
0021c2  9005              STR      r0,[sp,#0x14]         ;3025
0021c4  f7fffffe          BL       __aeabi_f2d
0021c8  e9cd0106          STRD     r0,r1,[sp,#0x18]      ;3025
0021cc  e9dd2308          LDRD     r2,r3,[sp,#0x20]      ;3025
0021d0  f7fffffe          BL       atan2
0021d4  f04f3266          MOV      r2,#0x66666666        ;3025
0021d8  4b2c              LDR      r3,|L1.8844|
0021da  e9cd010a          STRD     r0,r1,[sp,#0x28]      ;3025
0021de  f7fffffe          BL       __aeabi_dmul
0021e2  e9cd010c          STRD     r0,r1,[sp,#0x30]      ;3025
0021e6  f7fffffe          BL       __aeabi_d2f
0021ea  6028              STR      r0,[r5,#0]            ;3025
0021ec  4651              MOV      r1,r10                ;3026
0021ee  4650              MOV      r0,r10                ;3026
0021f0  f7fffffe          BL       __aeabi_fmul
0021f4  4649              MOV      r1,r9                 ;3026
0021f6  9004              STR      r0,[sp,#0x10]         ;3026
0021f8  4648              MOV      r0,r9                 ;3026
0021fa  f7fffffe          BL       __aeabi_fmul
0021fe  4641              MOV      r1,r8                 ;3026
002200  9003              STR      r0,[sp,#0xc]          ;3026
002202  4640              MOV      r0,r8                 ;3026
002204  f7fffffe          BL       __aeabi_fmul
002208  4639              MOV      r1,r7                 ;3026
00220a  9001              STR      r0,[sp,#4]            ;3026
00220c  4638              MOV      r0,r7                 ;3026
00220e  f7fffffe          BL       __aeabi_fmul
002212  9000              STR      r0,[sp,#0]            ;3026
002214  9901              LDR      r1,[sp,#4]            ;3026
002216  f7fffffe          BL       __aeabi_fadd
00221a  9002              STR      r0,[sp,#8]            ;3026
00221c  9903              LDR      r1,[sp,#0xc]          ;3026
00221e  f7fffffe          BL       __aeabi_fsub
002222  4683              MOV      r11,r0                ;3026
002224  9904              LDR      r1,[sp,#0x10]         ;3026
002226  f7fffffe          BL       __aeabi_fsub
00222a  9005              STR      r0,[sp,#0x14]         ;3026
00222c  f7fffffe          BL       __aeabi_f2d
002230  e9cd0108          STRD     r0,r1,[sp,#0x20]      ;3026
002234  4651              MOV      r1,r10                ;3026
002236  4638              MOV      r0,r7                 ;3026
002238  f7fffffe          BL       __aeabi_fmul
00223c  4649              MOV      r1,r9                 ;3026
00223e  9004              STR      r0,[sp,#0x10]         ;3026
002240  4640              MOV      r0,r8                 ;3026
002242  f7fffffe          BL       __aeabi_fmul
002246  9003              STR      r0,[sp,#0xc]          ;3026
002248  9904              LDR      r1,[sp,#0x10]         ;3026
00224a  f7fffffe          BL       __aeabi_fadd
00224e  f04f4180          MOV      r1,#0x40000000        ;3026
002252  9005              STR      r0,[sp,#0x14]         ;3026
002254  f7fffffe          BL       __aeabi_fmul
002258  4683              MOV      r11,r0                ;3026
00225a  f7fffffe          BL       __aeabi_f2d
00225e  e9cd0106          STRD     r0,r1,[sp,#0x18]      ;3026
002262  e9dd2308          LDRD     r2,r3,[sp,#0x20]      ;3026
002266  f7fffffe          BL       atan2
00226a  f04f3266          MOV      r2,#0x66666666        ;3026
00226e  4b07              LDR      r3,|L1.8844|
002270  e9cd010a          STRD     r0,r1,[sp,#0x28]      ;3026
002274  f7fffffe          BL       __aeabi_dmul
002278  e9cd010c          STRD     r0,r1,[sp,#0x30]      ;3026
00227c  f7fffffe          BL       __aeabi_d2f
002280  6030              STR      r0,[r6,#0]            ;3026
002282  e001              B        |L1.8840|
                  |L1.8836|
002284  2002              MOVS     r0,#2                 ;3027
002286  e70b              B        |L1.8352|
                  |L1.8840|
002288  2000              MOVS     r0,#0                 ;3028
00228a  e709              B        |L1.8352|
;;;3030   
                          ENDP

                  |L1.8844|
                          DCD      0x404ca666

                          AREA ||.constdata||, DATA, READONLY, ALIGN=2

                  ||reg||
000000  75191a0c          DCB      0x75,0x19,0x1a,0x0c
000004  6a231b1c          DCB      0x6a,0x23,0x1b,0x1c
000008  1f207274          DCB      0x1f,0x20,0x72,0x74
00000c  433b4138          DCB      0x43,0x3b,0x41,0x38
000010  393a6b6c          DCB      0x39,0x3a,0x6b,0x6c
000014  376f0624          DCB      0x37,0x6f,0x06,0x24
000018  6d6e7000          DCB      0x6d,0x6e,0x70,0x00
                  ||hw||
00001c  6800              DCB      0x68,0x00
00001e  0400              DCW      0x0400
000020  7600              DCB      0x76,0x00
000022  0154              DCW      0x0154
000024  fdf70100          DCW      0xfdf7,0x0100
                  test
                          DCD      0x00000083
                          DCD      0x00000800
000030  00010018          DCB      0x00,0x01,0x00,0x18
000034  0032              DCW      0x0032
000036  0500              DCB      0x05,0x00
000038  41200000          DCFS     0x41200000 ; 10
00003c  42d20000          DCFS     0x42d20000 ; 105
000040  3e0f5c29          DCFS     0x3e0f5c29 ; 0.14000000059604645
000044  3e99999a          DCFS     0x3e99999a ; 0.30000001192092896
000048  3f733333          DCFS     0x3f733333 ; 0.94999998807907104
00004c  3e0f5c29          DCFS     0x3e0f5c29 ; 0.14000000059604645

                          AREA ||.conststring||, DATA, READONLY, MERGE=1, STRINGS, ALIGN=2

000000  50726f64          DCB      "Product ID read as 0 indicates device is either incompa"
000004  75637420
000008  49442072
00000c  65616420
000010  61732030
000014  20696e64
000018  69636174
00001c  65732064
000020  65766963
000024  65206973
000028  20656974
00002c  68657220
000030  696e636f
000034  6d7061  
000037  7469626c          DCB      "tible or an MPU3050.\n",0
00003b  65206f72
00003f  20616e20
000043  4d505533
000047  3035302e
00004b  0a00    

                          AREA ||.data||, DATA, ALIGN=2

                  ||st||
                          DCD      ||reg||
                          DCD      ||hw||
000008  00000000          DCB      0x00,0x00,0x00,0x00
00000c  0000              DCB      0x00,0x00
00000e  0000              DCW      0x0000
000010  00000000          DCB      0x00,0x00,0x00,0x00
000014  00000000          DCB      0x00,0x00,0x00,0x00
                          %        8
000020  00000000          DCB      0x00,0x00,0x00,0x00
000024  0000              DCB      0x00,0x00
000026  0000              DCW      0x0000
                          DCD      test
                  gyro_orientation
00002c  01000000          DCB      0x01,0x00,0x00,0x00
000030  01000000          DCB      0x01,0x00,0x00,0x00
000034  01                DCB      0x01
