AGV / agv_gelinmei · Files

user_slam.txt 47.6 KB
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389 390 391 392 393 394 395 396 397 398 399 400 401 402 403 404 405 406 407 408 409 410 411 412 413 414 415 416 417 418 419 420 421 422 423 424 425 426 427 428 429 430 431 432 433 434 435 436 437 438 439 440 441 442 443 444 445 446 447 448 449 450 451 452 453 454 455 456 457 458 459 460 461 462 463 464 465 466 467 468 469 470 471 472 473 474 475 476 477 478 479 480 481 482 483 484 485 486 487 488 489 490 491 492 493 494 495 496 497 498 499 500 501 502 503 504 505 506 507 508 509 510 511 512 513 514 515 516 517 518 519 520 521 522 523 524 525 526 527 528 529 530 531 532 533 534 535 536 537 538 539 540 541 542 543 544 545 546 547 548 549 550 551 552 553 554 555 556 557 558 559 560 561 562 563 564 565 566 567 568 569 570 571 572 573 574 575 576 577 578 579 580 581 582 583 584 585 586 587 588 589 590 591 592 593 594 595 596 597 598 599 600 601 602 603 604 605 606 607 608 609 610 611 612 613 614 615 616 617 618 619 620 621 622 623 624 625 626 627 628 629 630 631 632 633 634 635 636 637 638 639 640 641 642 643 644 645 646 647 648 649 650 651 652 653 654 655 656 657 658 659 660 661 662 663 664 665 666 667 668 669 670 671 672 673 674 675 676 677 678 679 680 681 682 683 684 685 686 687 688 689 690 691 692 693 694 695 696 697 698 699 700 701 702 703 704 705 706 707 708 709 710 711 712 713 714 715 716 717 718 719 720 721 722 723 724 725 726 727 728 729 730 731 732 733 734 735 736 737 738 739 740 741 742 743 744 745 746 747 748 749 750 751 752 753 754 755 756 757 758 759 760 761 762 763 764 765 766 767 768 769 770 771 772 773 774 775 776 777 778 779 780 781 782 783 784 785 786 787 788 789 790 791 792 793 794 795 796 797 798 799 800 801 802 803 804 805 806 807 808 809 810 811 812 813 814 815 816 817 818 819 820 821 822 823 824 825 826 827 828 829 830 831 832 833 834 835 836 837 838 839 840 841 842 843 844 845 846 847 848 849 850 851 852 853 854 855 856 857 858 859 860 861 862 863 864 865 866 867 868 869 870 871 872 873 874 875 876 877 878 879 880 881 882 883 884 885 886 887 888 889 890 891 892 893 894 895 896 897 898 899 900 901 902 903 904 905 906 907 908 909 910 911 912 913 914 915 916 917 918 919 920 921 922 923 924 925 926 927 928 929 930 931 932 933 934 935 936 937 938 939 940 941 942 943 944 945 946 947 948 949 950 951 952 953 954 955 956 957 958 959 960 961 962 963 964 965 966 967 968 969 970 971 972 973 974 975 976 977 978 979 980 981 982 983 984 985 986 987 988 989 990 991 992 993 994 995 996 997 998 999 1000 1001 1002 1003 1004 1005 1006 1007 1008 1009 1010 1011 1012 1013 1014 1015 1016 1017 1018 1019 1020 1021 1022 1023 1024 1025 1026 1027 1028 1029 1030 1031 1032 1033 1034 1035 1036 1037 1038 1039 1040 1041 1042 1043 1044 1045 1046 1047 1048 1049 1050 1051 1052 1053 1054 1055 1056 1057 1058 1059 1060 1061 1062 1063 1064 1065 1066 1067 1068 1069 1070 1071 1072 1073 1074 1075 1076 1077 1078 1079 1080 1081 1082 1083 1084 1085 1086 1087 1088 1089 1090 1091 1092 1093 1094 1095 1096 1097 1098 1099 1100 1101 1102 1103 1104 1105 1106 1107 1108 1109 1110 1111 1112 1113 1114 1115 1116 1117 1118 
; generated by Component: ARM Compiler 5.06 update 6 (build 750) Tool: ArmCC [4d3637]
; commandline ArmCC [--c99 --list --split_sections --debug -c --asm --interleave -o.\flash\obj\user_slam.o --asm_dir=.\Flash\List\ --list_dir=.\Flash\List\ --depend=.\flash\obj\user_slam.d --cpu=Cortex-M4.fp --apcs=interwork -O1 --diag_suppress=9931,870 -I..\..\Libraries\CMSIS\Include -I..\..\Libraries\CMSIS\Device\ST\STM32F4xx\Include -I..\..\Libraries\STM32F4xx_StdPeriph_Driver\inc -I..\..\uCOS-III\uC-CPU -I..\..\uCOS-III\uC-LIB -I..\..\uCOS-III\uCOS-III\Ports -I..\..\uCOS-III\uCOS-III\Source -I..\..\uCOS-III\uC-CPU\ARM-Cortex-M4\RealView -I..\..\uCOS-III\uC-LIB\Ports\ARM-Cortex-M4\RealView -I..\..\uCOS-III\uCOS-III\Ports\ARM-Cortex-M4\Generic\RealView -I..\..\User -I..\..\User\bsp -I..\..\User\bsp\inc -I..\..\User\libapp -I..\..\RL-ARM\Config -I..\..\RL-ARM\Driver -I..\..\RL-ARM\RL-RTX\inc -I..\..\User\bsp\BSP -I..\..\RL-ARM\RL-CAN -I..\..\Libraries\DSP_LIB\Include -I..\..\MODBUS\modbus\rtu -I..\..\MODBUS\BARE\port -I..\..\MODBUS\modbus\include -I..\..\User\bsp\BSP -I..\..\PLC -I..\..\Avoid -I..\..\User\parameter -I..\..\User\LaserMotionCtr -I..\..\User\W5100S -I..\..\User\bsp -I.\RTE\_Flash -IC:\Users\软件部\AppData\Local\Arm\Packs\ARM\CMSIS\5.5.1\CMSIS\Core\Include -IC:\Users\软件部\AppData\Local\Arm\Packs\Keil\STM32F4xx_DFP\2.13.0\Drivers\CMSIS\Device\ST\STM32F4xx\Include -D__UVISION_VERSION=527 -D_RTE_ -DSTM32F407xx -DUSE_STDPERIPH_DRIVER -DSTM32F40_41xxx -D__RTX -D__FPU_USED=1 --omf_browse=.\flash\obj\user_slam.crf ..\..\User\parameter\user_Slam.c]
                          THUMB

                          AREA ||i.DataInteractionInterface||, CODE, READONLY, ALIGN=1

                  DataInteractionInterface PROC
;;;14     
;;;15     void DataInteractionInterface()
000000  b510              PUSH     {r4,lr}
;;;16     {
;;;17     //	if(ControlInputValue.Direction == 1)
;;;18     //	{
;;;19     //		agvEncoderIntegrate(ControlInputValue.Wheelbase,&Tail_X,&Tail_Y,&BaseAngleOne,(float)ControlInputValue.AgvActualFirstSpeed*0.0001f,ControlInputValue.AgvFirstRudderWheelAngle/180.0f*PI);
;;;20     //		
;;;21     //		integral_X = Tail_X + cos(BaseAngleOne) * ControlInputValue.Wheelbase / 2.0f;
;;;22     //		integral_Y = Tail_Y + sin(BaseAngleOne) * ControlInputValue.Wheelbase / 2.0f;
;;;23     //		
;;;24     //		Head_X = Tail_X + cos(BaseAngleOne) * ControlInputValue.Wheelbase;
;;;25     //		Head_Y = Tail_Y + sin(BaseAngleOne) * ControlInputValue.Wheelbase;
;;;26     //		
;;;27     //		BaseAngleTwo = BaseAngleOne + PI;
;;;28     //		BaseAngleThree = BaseAngleOne + PI/2.0f;
;;;29     //		BaseAngleFour = BaseAngleOne + 3.0f * PI/2.0f;
;;;30     //		
;;;31     //		integral_W = BaseAngleOne;
;;;32     //	}
;;;33     //	else if(ControlInputValue.Direction == 2)
;;;34     //	{
;;;35     //		agvEncoderIntegrate(ControlInputValue.Wheelbase,&Head_X,&Head_Y,&BaseAngleTwo,(float)ControlInputValue.AgvActualSecondSpeed*0.0001f,ControlInputValue.AgvSecondRudderWheelAngle/180.0f*PI);
;;;36     //		
;;;37     //		integral_X = Head_X + cos(BaseAngleTwo) * ControlInputValue.Wheelbase / 2.0f;
;;;38     //		integral_Y = Head_Y + sin(BaseAngleTwo) * ControlInputValue.Wheelbase / 2.0f;
;;;39     //		
;;;40     //		Tail_X = Head_X + cos(BaseAngleTwo) * ControlInputValue.Wheelbase;
;;;41     //		Tail_Y = Head_Y + sin(BaseAngleTwo) * ControlInputValue.Wheelbase;	
;;;42     
;;;43     
;;;44     //		
;;;45     //		BaseAngleOne = BaseAngleTwo - PI;
;;;46     //		BaseAngleThree = BaseAngleTwo + PI / 2.0f;
;;;47     //		BaseAngleFour = BaseAngleTwo + 3.0f * PI / 2.0f;	
;;;48     
;;;49     //		integral_W = BaseAngleTwo;	
;;;50     //	}
;;;51     //	else if(ControlInputValue.Direction == 3)
;;;52     //	{
;;;53     //		_CalculateXBias((float)ControlInputValue.AgvActualFirstSpeed*0.0001f,(float)ControlInputValue.AgvActualSecondSpeed*0.0001f,ControlInputValue.Wheelbase,&integral_X,&integral_Y,&BaseAngleThree);
;;;54     //		
;;;55     //		BaseAngleOne = BaseAngleThree - PI /2.0f;
;;;56     //		BaseAngleTwo = BaseAngleThree + PI / 2.0f;
;;;57     //		BaseAngleFour = BaseAngleThree + PI;	
;;;58     //			
;;;59     //		integral_W = BaseAngleThree;	
;;;60     
;;;61     //	}
;;;62     //	else if(ControlInputValue.Direction == 4)
;;;63     //	{
;;;64     //		_CalculateXBias((float)ControlInputValue.AgvActualFirstSpeed*0.0001f,(float)ControlInputValue.AgvActualSecondSpeed*0.0001f,ControlInputValue.Wheelbase,&integral_X,&integral_Y,&BaseAngleFour);	
;;;65     //	
;;;66     //		BaseAngleOne = BaseAngleFour + PI / 2.0f;
;;;67     //		BaseAngleTwo = BaseAngleFour - PI / 2.0f;
;;;68     //		BaseAngleThree = BaseAngleFour - PI;
;;;69     //		
;;;70     //		integral_W = BaseAngleFour;	
;;;71     //		
;;;72     //	}
;;;73     ////	static u8 i = 6;
;;;74     ////	if(i++ > 5)
;;;75     ////	{
;;;76     ////		Uart_Printf(COM1,"dir=%d	speed1=%.1f	speed2=%.1f	angle1=%f	angle2=%f\r\n",ControlInputValue.Direction,(float)ControlInputValue.AgvActualFirstSpeed*0.1f,
;;;77     ////		(float)ControlInputValue.AgvActualSecondSpeed*0.1f,ControlInputValue.AgvFirstRudderWheelAngle,ControlInputValue.AgvSecondRudderWheelAngle);
;;;78     ////		i = 0;
;;;79     ////	}
;;;80     ////
;;;81     //		
;;;82     //	if(BaseAngleOne >= 1.99f*PI)BaseAngleOne -= 2.0f * PI;
;;;83     //	else if(BaseAngleOne <= -0.001f*PI)BaseAngleOne += 2.0f * PI;	
;;;84     //	
;;;85     //	if(BaseAngleTwo >= 1.99f*PI)BaseAngleTwo -= 2.0f * PI;
;;;86     //	else if(BaseAngleTwo <= -0.001f)BaseAngleTwo += 2.0f * PI;
;;;87     
;;;88     //	if(BaseAngleThree >= 1.99f*PI)BaseAngleThree -= 2.0f * PI;
;;;89     //	else if(BaseAngleThree <= -0.001f)BaseAngleThree += 2.0f * PI;
;;;90     //	
;;;91     //	if(BaseAngleFour >= 1.99f*PI)BaseAngleFour -= 2.0f * PI;
;;;92     //	else if(BaseAngleFour <= -0.001f)BaseAngleFour += 2.0f * PI;
;;;93     //	
;;;94     //	if(integral_W >= 1.99f*PI)integral_W -= 2.0f * PI;
;;;95     //	else if(integral_W <= -0.001f)integral_W += 2.0f * PI;
;;;96     
;;;97     //	
;;;98     	
;;;99     //	static u8 ii = 6;
;;;100    //	if(ii++ > 5)
;;;101    //	{
;;;102    //		Uart_Printf(COM1,"dir=%d	H_x=%.1f	y=%.1f	T_x=%.1f	y=%.1f	C_x=%.1f	y=%.1f	integral_W=%f	BaseAngleOne=%f	BaseAngleTwo=%f\r\n",
;;;103    //		ControlInputValue.Direction,Head_X*1000.0f,Head_Y*1000.0f,Tail_X*1000.0f,Tail_Y*1000.0f,integral_X*1000.0f,integral_Y*1000.0f,
;;;104    //		integral_W*180.0f/PI,BaseAngleOne*180.0f/PI,BaseAngleTwo*180.0f/PI);
;;;105    //		ii = 0;
;;;106    //	}
;;;107    //	//SLAM需求数据
;;;108    	UartToROS_Send_Info_To_Server();
000002  f7fffffe          BL       UartToROS_Send_Info_To_Server
;;;109    //	//读SLAM坐标值
;;;110    	ProcessDataFormUartSlam();	
000006  e8bd4010          POP      {r4,lr}
00000a  f7ffbffe          B.W      ProcessDataFormUartSlam
;;;111    }
;;;112    
                          ENDP


                          AREA ||i.Prepare_UartToROS_Send||, CODE, READONLY, ALIGN=2

                          REQUIRE _printf_percent
                          REQUIRE _printf_return_value
                          REQUIRE _printf_d
                          REQUIRE _printf_f
                          REQUIRE _printf_int_dec
                          REQUIRE _printf_fp_dec
                          REQUIRE _printf_pre_padding
                          REQUIRE _printf_flags
                          REQUIRE _printf_widthprec
                          REQUIRE _printf_x
                          REQUIRE _printf_longlong_hex
                  Prepare_UartToROS_Send PROC
;;;115    
;;;116    unsigned int Prepare_UartToROS_Send(unsigned char string_out[])
000000  e92d47f0          PUSH     {r4-r10,lr}
;;;117    {
000004  ed2d8b06          VPUSH    {d8-d10}
000008  b086              SUB      sp,sp,#0x18
00000a  4681              MOV      r9,r0
;;;118    	static int iii = 0;
;;;119    	short i = 0;
;;;120    	unsigned char sum = 0;
00000c  2400              MOVS     r4,#0
;;;121    	static unsigned char string_len = 0;
;;;122    	float speedOne = 0;
00000e  ed9f8a84          VLDR     s16,|L2.544|
;;;123    	float speedTwo = 0;
000012  eef08a48          VMOV.F32 s17,s16
;;;124    	
;;;125    	double Time = ((double)TIM_GetCounter(TIM7)) / 2000.0f;//得到时间单位为秒
000016  4f83              LDR      r7,|L2.548|
000018  4638              MOV      r0,r7
00001a  f7fffffe          BL       TIM_GetCounter
00001e  f7fffffe          BL       __aeabi_ui2d
000022  ed9f1b81          VLDR     d1,|L2.552|
000026  ec532b11          VMOV     r2,r3,d1
00002a  f7fffffe          BL       __aeabi_ddiv
00002e  460e              MOV      r6,r1
000030  4605              MOV      r5,r0
;;;126    	TIM_SetCounter(TIM7, 0);
000032  2100              MOVS     r1,#0
000034  4638              MOV      r0,r7
000036  f7fffffe          BL       TIM_SetCounter
;;;127    
;;;128    //	speedOne = (float)(ControlInputValue.AgvActualFirstSpeed)/1000.0f;
;;;129    //	speedTwo = (float)(ControlInputValue.AgvActualSecondSpeed)/1000.0f;	
;;;130    
;;;131    	if(ControlInputValue.OmnidirectionalWalking)
00003a  4f7d              LDR      r7,|L2.560|
00003c  f897007e          LDRB     r0,[r7,#0x7e]  ; ControlInputValue
;;;132    	{
;;;133    		if(ControlInputValue.Direction == 1)
;;;134    		{
;;;135    			speedOne = -(float)(ControlInputValue.AgvActualSecondSpeed)/1000.0f;
;;;136    			speedTwo = 0;
;;;137    		}
;;;138    		else if(ControlInputValue.Direction == 2)
;;;139    		{
;;;140    			speedOne = (float)(ControlInputValue.AgvActualSecondSpeed)/1000.0f;
000040  ed9faa7c          VLDR     s20,|L2.564|
000044  b3e8              CBZ      r0,|L2.194|
000046  f89700ba          LDRB     r0,[r7,#0xba]         ;133  ; ControlInputValue
00004a  2801              CMP      r0,#1                 ;133
00004c  d006              BEQ      |L2.92|
00004e  2802              CMP      r0,#2                 ;138
000050  d00f              BEQ      |L2.114|
;;;141    			speedTwo = 0;		
;;;142    		}
;;;143    		else if(ControlInputValue.Direction == 3)
000052  2803              CMP      r0,#3
000054  d016              BEQ      |L2.132|
;;;144    		{
;;;145    			speedOne = 0;
;;;146    			speedTwo = -fabs(((float)(ControlInputValue.AgvActualFirstSpeed)/1000.0f) + ((float)(ControlInputValue.AgvActualSecondSpeed)/1000.0f)) / 2.0f;
;;;147    		}
;;;148    		else if(ControlInputValue.Direction == 4)
000056  2804              CMP      r0,#4
000058  d03d              BEQ      |L2.214|
00005a  e0a8              B        |L2.430|
                  |L2.92|
00005c  f9b7000e          LDRSH    r0,[r7,#0xe]          ;135  ; ControlInputValue
000060  ee000a10          VMOV     s0,r0                 ;135
000064  eeb80ac0          VCVT.F32.S32 s0,s0                 ;135
000068  eddf0a73          VLDR     s1,|L2.568|
00006c  ee808a20          VDIV.F32 s16,s0,s1             ;135
000070  e09d              B        |L2.430|
                  |L2.114|
000072  f9b7000e          LDRSH    r0,[r7,#0xe]          ;140  ; ControlInputValue
000076  ee000a10          VMOV     s0,r0                 ;140
00007a  eeb80ac0          VCVT.F32.S32 s0,s0                 ;140
00007e  ee808a0a          VDIV.F32 s16,s0,s20            ;140
000082  e094              B        |L2.430|
                  |L2.132|
000084  f9b7000c          LDRSH    r0,[r7,#0xc]          ;146  ; ControlInputValue
000088  ee000a10          VMOV     s0,r0                 ;146
00008c  f9b7000e          LDRSH    r0,[r7,#0xe]          ;146  ; ControlInputValue
000090  eeb80ac0          VCVT.F32.S32 s0,s0                 ;146
000094  eec00a0a          VDIV.F32 s1,s0,s20             ;146
000098  ee000a10          VMOV     s0,r0                 ;146
00009c  eeb80ac0          VCVT.F32.S32 s0,s0                 ;146
0000a0  ee801a0a          VDIV.F32 s2,s0,s20             ;146
0000a4  ee300a81          VADD.F32 s0,s1,s2              ;146
0000a8  ee100a10          VMOV     r0,s0                 ;146
0000ac  f7fffffe          BL       __aeabi_f2d
0000b0  ec410b10          VMOV     d0,r0,r1              ;146
0000b4  f7fffffe          BL       __hardfp_fabs
0000b8  ed9f1b60          VLDR     d1,|L2.572|
0000bc  ec532b11          VMOV     r2,r3,d1              ;146
0000c0  e000              B        |L2.196|
                  |L2.194|
0000c2  e02f              B        |L2.292|
                  |L2.196|
0000c4  ec510b10          VMOV     r0,r1,d0              ;146
0000c8  f7fffffe          BL       __aeabi_dmul
0000cc  f7fffffe          BL       __aeabi_d2f
0000d0  ee080a90          VMOV     s17,r0                ;146
0000d4  e06b              B        |L2.430|
                  |L2.214|
;;;149    		{
;;;150    			speedOne = 0;
;;;151    			speedTwo = fabs(((float)(ControlInputValue.AgvActualFirstSpeed)/1000.0f) + ((float)(ControlInputValue.AgvActualSecondSpeed)/1000.0f)) / 2.0f;	
0000d6  f9b7000c          LDRSH    r0,[r7,#0xc]  ; ControlInputValue
0000da  ee000a10          VMOV     s0,r0
0000de  f9b7000e          LDRSH    r0,[r7,#0xe]  ; ControlInputValue
0000e2  eeb80ac0          VCVT.F32.S32 s0,s0
0000e6  eec00a0a          VDIV.F32 s1,s0,s20
0000ea  ee000a10          VMOV     s0,r0
0000ee  eeb80ac0          VCVT.F32.S32 s0,s0
0000f2  ee801a0a          VDIV.F32 s2,s0,s20
0000f6  ee300a81          VADD.F32 s0,s1,s2
0000fa  ee100a10          VMOV     r0,s0
0000fe  f7fffffe          BL       __aeabi_f2d
000102  ec410b10          VMOV     d0,r0,r1
000106  f7fffffe          BL       __hardfp_fabs
00010a  ed9f1b4e          VLDR     d1,|L2.580|
00010e  ec510b10          VMOV     r0,r1,d0
000112  ec532b11          VMOV     r2,r3,d1
000116  f7fffffe          BL       __aeabi_dmul
00011a  f7fffffe          BL       __aeabi_d2f
00011e  ee080a90          VMOV     s17,r0
000122  e044              B        |L2.430|
                  |L2.292|
;;;152    		}
;;;153    	}
;;;154    	else 
;;;155    	{
;;;156    		if(ControlInputValue.TurnningMode == 2)
000124  f897003c          LDRB     r0,[r7,#0x3c]  ; ControlInputValue
000128  2802              CMP      r0,#2
00012a  d127              BNE      |L2.380|
;;;157    		{
;;;158    			speedOne = ((float)(ControlInputValue.AgvActualFirstSpeed) * cos(ControlInputValue.AgvFirstRudderWheelAngle /180.0f * PI))/1000.0f;	
00012c  edd70a09          VLDR     s1,[r7,#0x24]
000130  ed9f1a46          VLDR     s2,|L2.588|
000134  ee800a81          VDIV.F32 s0,s1,s2
000138  eddf0a45          VLDR     s1,|L2.592|
00013c  ee200a20          VMUL.F32 s0,s0,s1
000140  ee100a10          VMOV     r0,s0
000144  f7fffffe          BL       __aeabi_f2d
000148  ec410b10          VMOV     d0,r0,r1
00014c  f7fffffe          BL       __hardfp_cos
000150  eeb09a40          VMOV.F32 s18,s0
000154  eef09a60          VMOV.F32 s19,s1
000158  f9b7000c          LDRSH    r0,[r7,#0xc]  ; ControlInputValue
00015c  f7fffffe          BL       __aeabi_i2d
000160  ec532b19          VMOV     r2,r3,d9
000164  f7fffffe          BL       __aeabi_dmul
000168  ed9f1b3a          VLDR     d1,|L2.596|
00016c  ec532b11          VMOV     r2,r3,d1
000170  f7fffffe          BL       __aeabi_ddiv
000174  f7fffffe          BL       __aeabi_d2f
000178  ee080a10          VMOV     s16,r0
                  |L2.380|
;;;159    		}
;;;160    		if(ControlInputValue.TurnningMode == 0)
00017c  f897003c          LDRB     r0,[r7,#0x3c]  ; ControlInputValue
000180  b9a8              CBNZ     r0,|L2.430|
;;;161    		{
;;;162    			speedOne = ((float)(ControlInputValue.AgvActualFirstSpeed)/1000.0f + (float)(ControlInputValue.AgvActualSecondSpeed)/1000.0f) / 2.0f;
000182  f9b7000c          LDRSH    r0,[r7,#0xc]  ; ControlInputValue
000186  ee000a10          VMOV     s0,r0
00018a  f9b7000e          LDRSH    r0,[r7,#0xe]  ; ControlInputValue
00018e  eeb80ac0          VCVT.F32.S32 s0,s0
000192  eec00a0a          VDIV.F32 s1,s0,s20
000196  ee000a10          VMOV     s0,r0
00019a  eeb80ac0          VCVT.F32.S32 s0,s0
00019e  ee801a0a          VDIV.F32 s2,s0,s20
0001a2  ee300a81          VADD.F32 s0,s1,s2
0001a6  eef60a00          VMOV.F32 s1,#0.50000000
0001aa  ee208a20          VMUL.F32 s16,s0,s1
                  |L2.430|
;;;163    			speedTwo = 0;
;;;164    		}
;;;165    	}
;;;166    //	if(iii++ >= 20)
;;;167    //	{
;;;168    //		Uart_Printf(COM1,"SSS=%d	angle=%f	dir=%d	speed1=%.1f	speed2=%.1f	time=%f\r\n",
;;;169    //		ControlInputValue.AgvActualFirstSpeed,ControlInputValue.AgvFirstRudderWheelAngle,
;;;170    //		ControlInputValue.Direction,speedOne*1000,speedTwo*1000,Time*1000);
;;;171    //		iii = 0;
;;;172    //	}
;;;173    	
;;;174    	HandlePlcData.RecvPlcData.ChangeMap = 1;
0001ae  482b              LDR      r0,|L2.604|
0001b0  f04f0a01          MOV      r10,#1
0001b4  f8c0a050          STR      r10,[r0,#0x50]  ; HandlePlcData
;;;175    	string_len = sprintf((char*)string_out, "$%f,%f,%f,%d*",speedOne,speedTwo,Time,HandlePlcData.RecvPlcData.ChangeMap);
0001b8  ee180a90          VMOV     r0,s17
0001bc  f7fffffe          BL       __aeabi_f2d
0001c0  4607              MOV      r7,r0
0001c2  4688              MOV      r8,r1
0001c4  ee180a10          VMOV     r0,s16
0001c8  f7fffffe          BL       __aeabi_f2d
0001cc  e9cd6a03          STRD     r6,r10,[sp,#0xc]
0001d0  ec410b10          VMOV     d0,r0,r1
0001d4  e9cd7800          STRD     r7,r8,[sp,#0]
0001d8  ec532b10          VMOV     r2,r3,d0
0001dc  a120              ADR      r1,|L2.608|
0001de  4648              MOV      r0,r9
0001e0  9502              STR      r5,[sp,#8]
0001e2  f7fffffe          BL       __2sprintf
0001e6  4d22              LDR      r5,|L2.624|
0001e8  b2c2              UXTB     r2,r0
0001ea  702a              STRB     r2,[r5,#0]
;;;176    	for (i = 1; i < string_len - 1; i++)//只校验$与*之间的数据
0001ec  2001              MOVS     r0,#1
0001ee  1e51              SUBS     r1,r2,#1
0001f0  e005              B        |L2.510|
                  |L2.498|
;;;177    	{
;;;178    		sum += string_out[i];
0001f2  f8193000          LDRB     r3,[r9,r0]
0001f6  4423              ADD      r3,r3,r4
0001f8  b2dc              UXTB     r4,r3
0001fa  1c40              ADDS     r0,r0,#1              ;176
0001fc  b200              SXTH     r0,r0                 ;176
                  |L2.510|
0001fe  4288              CMP      r0,r1                 ;176
000200  dbf7              BLT      |L2.498|
;;;179    	}
;;;180    	string_len += sprintf((char*)string_out + string_len, "%02X#\r\n", sum);
000202  eb090002          ADD      r0,r9,r2
000206  4622              MOV      r2,r4
000208  a11a              ADR      r1,|L2.628|
00020a  f7fffffe          BL       __2sprintf
00020e  7829              LDRB     r1,[r5,#0]  ; string_len
000210  4408              ADD      r0,r0,r1
000212  b2c0              UXTB     r0,r0
000214  7028              STRB     r0,[r5,#0]
;;;181    	
;;;182    	return string_len;
;;;183    }
000216  b006              ADD      sp,sp,#0x18
000218  ecbd8b06          VPOP     {d8-d10}
00021c  e8bd87f0          POP      {r4-r10,pc}
;;;184    
                          ENDP

                  |L2.544|
000220  00000000          DCFS     0x00000000 ; 0
                  |L2.548|
                          DCD      0x40001400
                  |L2.552|
000228  00000000          DCFD     0x409f400000000000 ; 2000
00022c  409f4000
                  |L2.560|
                          DCD      ControlInputValue
                  |L2.564|
000234  447a0000          DCFS     0x447a0000 ; 1000
                  |L2.568|
000238  c47a0000          DCFS     0xc47a0000 ; -1000
                  |L2.572|
00023c  00000000          DCFD     0xbfe0000000000000 ; -0.5
000240  bfe00000
                  |L2.580|
000244  00000000          DCFD     0x3fe0000000000000 ; 0.5
000248  3fe00000
                  |L2.588|
00024c  43340000          DCFS     0x43340000 ; 180
                  |L2.592|
000250  40490fda          DCFS     0x40490fda ; 3.1415925025939941
                  |L2.596|
000254  00000000          DCFD     0x408f400000000000 ; 1000
000258  408f4000
                  |L2.604|
                          DCD      HandlePlcData
                  |L2.608|
000260  2425662c          DCB      "$$%f,%f,%f,%d*",0
000264  25662c25
000268  662c2564
00026c  2a00    
00026e  00                DCB      0
00026f  00                DCB      0
                  |L2.624|
                          DCD      ||area_number.27||
                  |L2.628|
000274  25303258          DCB      "%02X#\r\n",0
000278  230d0a00

                          AREA ||i.Prepare_UartToROS_Send222||, CODE, READONLY, ALIGN=2

                          REQUIRE _printf_percent
                          REQUIRE _printf_return_value
                          REQUIRE _printf_f
                          REQUIRE _printf_fp_dec
                          REQUIRE _printf_pre_padding
                          REQUIRE _printf_flags
                          REQUIRE _printf_widthprec
                          REQUIRE _printf_x
                          REQUIRE _printf_longlong_hex
                  Prepare_UartToROS_Send222 PROC
;;;184    
;;;185    unsigned int Prepare_UartToROS_Send222(unsigned char string_out[])
000000  e92d47ff          PUSH     {r0-r10,lr}
;;;186    {
000004  4681              MOV      r9,r0
;;;187    	short i = 0;
;;;188    	unsigned char sum = 0;
000006  2400              MOVS     r4,#0
;;;189    	static unsigned char string_len = 0;
;;;190    	
;;;191    	string_len = sprintf((char*)string_out, "@%f,%f,%f*",PowOffList.PowOffCoordinatePoints.xPoint,PowOffList.PowOffCoordinatePoints.yPoint,PowOffList.PowOffCoordinatePoints.W);
000008  f8dfa070          LDR      r10,|L3.124|
00000c  f8da0008          LDR      r0,[r10,#8]  ; PowOffList
000010  f7fffffe          BL       __aeabi_f2d
000014  4605              MOV      r5,r0
000016  460e              MOV      r6,r1
000018  f8da0004          LDR      r0,[r10,#4]  ; PowOffList
00001c  f7fffffe          BL       __aeabi_f2d
000020  4607              MOV      r7,r0
000022  4688              MOV      r8,r1
000024  f8da0000          LDR      r0,[r10,#0]  ; PowOffList
000028  f7fffffe          BL       __aeabi_f2d
00002c  ec410b10          VMOV     d0,r0,r1
000030  e9cd7800          STRD     r7,r8,[sp,#0]
000034  e9cd5602          STRD     r5,r6,[sp,#8]
000038  ec532b10          VMOV     r2,r3,d0
00003c  a110              ADR      r1,|L3.128|
00003e  4648              MOV      r0,r9
000040  f7fffffe          BL       __2sprintf
000044  4d11              LDR      r5,|L3.140|
000046  b2c2              UXTB     r2,r0
000048  706a              STRB     r2,[r5,#1]
;;;192    	for (i = 1; i < string_len - 1; i++)//只校验$与*之间的数据
00004a  2001              MOVS     r0,#1
00004c  1e51              SUBS     r1,r2,#1
00004e  e005              B        |L3.92|
                  |L3.80|
;;;193    	{
;;;194    		sum += string_out[i];
000050  f8193000          LDRB     r3,[r9,r0]
000054  4423              ADD      r3,r3,r4
000056  b2dc              UXTB     r4,r3
000058  1c40              ADDS     r0,r0,#1              ;192
00005a  b200              SXTH     r0,r0                 ;192
                  |L3.92|
00005c  4288              CMP      r0,r1                 ;192
00005e  dbf7              BLT      |L3.80|
;;;195    	}
;;;196    	string_len += sprintf((char*)string_out + string_len, "%02X#\r\n", sum);
000060  eb090002          ADD      r0,r9,r2
000064  4622              MOV      r2,r4
000066  a10a              ADR      r1,|L3.144|
000068  f7fffffe          BL       __2sprintf
00006c  7869              LDRB     r1,[r5,#1]  ; string_len
00006e  4408              ADD      r0,r0,r1
000070  b2c0              UXTB     r0,r0
000072  7068              STRB     r0,[r5,#1]
;;;197    	
;;;198    	return string_len;
;;;199    }
000074  b004              ADD      sp,sp,#0x10
000076  e8bd87f0          POP      {r4-r10,pc}
;;;200    
                          ENDP

00007a  0000              DCW      0x0000
                  |L3.124|
                          DCD      PowOffList
                  |L3.128|
000080  4025662c          DCB      "@%f,%f,%f*",0
000084  25662c25
000088  662a00  
00008b  00                DCB      0
                  |L3.140|
                          DCD      ||area_number.27||
                  |L3.144|
000090  25303258          DCB      "%02X#\r\n",0
000094  230d0a00

                          AREA ||i.ProccessAGVDATAInfo||, CODE, READONLY, ALIGN=2

                  ProccessAGVDATAInfo PROC
;;;299    
;;;300    void ProccessAGVDATAInfo(unsigned char *Res)
000000  b570              PUSH     {r4-r6,lr}
;;;301    {
;;;302    	static unsigned char Buffer2[120];
;;;303    	static unsigned short iii = 0;
;;;304    
;;;305    	if(iii > 100)
000002  4c0c              LDR      r4,|L4.52|
000004  2500              MOVS     r5,#0
000006  8861              LDRH     r1,[r4,#2]  ; iii
000008  2964              CMP      r1,#0x64
00000a  d900              BLS      |L4.14|
;;;306    	{
;;;307    		iii = 0;
00000c  8065              STRH     r5,[r4,#2]
                  |L4.14|
;;;308    	}
;;;309    	if(*Res == 0x24)  					// $
00000e  7803              LDRB     r3,[r0,#0]
000010  2b24              CMP      r3,#0x24
000012  d100              BNE      |L4.22|
;;;310    	{
;;;311    		iii = 0;  
000014  8065              STRH     r5,[r4,#2]
                  |L4.22|
;;;312    	}
;;;313    	Buffer2[iii++] = *Res;
000016  8862              LDRH     r2,[r4,#2]  ; iii
000018  4e07              LDR      r6,|L4.56|
00001a  1c51              ADDS     r1,r2,#1
00001c  b289              UXTH     r1,r1
00001e  8061              STRH     r1,[r4,#2]
000020  54b3              STRB     r3,[r6,r2]
;;;314    	if(*Res == 0x23)   					// # 
000022  7800              LDRB     r0,[r0,#0]
000024  2823              CMP      r0,#0x23
000026  d103              BNE      |L4.48|
;;;315    	{
;;;316    		ProccessAGVInfo((unsigned char *)Buffer2,iii);
000028  4630              MOV      r0,r6
00002a  f7fffffe          BL       ProccessAGVInfo
;;;317    		iii = 0;
00002e  8065              STRH     r5,[r4,#2]
                  |L4.48|
;;;318    	}
;;;319    }
000030  bd70              POP      {r4-r6,pc}
;;;320    
                          ENDP

000032  0000              DCW      0x0000
                  |L4.52|
                          DCD      ||area_number.27||
                  |L4.56|
                          DCD      ||.bss||

                          AREA ||i.ProccessAGVInfo||, CODE, READONLY, ALIGN=2

                          REQUIRE _scanf_int
                          REQUIRE _scanf_real
                  ProccessAGVInfo PROC
;;;221    
;;;222    void ProccessAGVInfo(unsigned char *RecvBuffer,unsigned short BufferLength)
000000  b570              PUSH     {r4-r6,lr}
;;;223    {
000002  ed2d8b04          VPUSH    {d8-d9}
000006  b08a              SUB      sp,sp,#0x28
000008  4606              MOV      r6,r0
;;;224    	unsigned char i = 1;
00000a  2401              MOVS     r4,#1
;;;225    	float XbiasFromros = 0,YbiasFromros = 0,TheataFromros = 0;
00000c  ed9f0a79          VLDR     s0,|L5.500|
000010  ed8d0a09          VSTR     s0,[sp,#0x24]
000014  ed8d0a08          VSTR     s0,[sp,#0x20]
000018  ed8d0a07          VSTR     s0,[sp,#0x1c]
;;;226    	int radararea = 0;
;;;227    	unsigned char Location_State_ = 0;
;;;228    	
;;;229    	if(strncmp("$", (const char*)RecvBuffer, 1) == 0)//地标动作命令
00001c  2201              MOVS     r2,#1
00001e  4631              MOV      r1,r6
000020  a075              ADR      r0,|L5.504|
000022  f7fffffe          BL       strncmp
000026  2800              CMP      r0,#0
000028  d17e              BNE      |L5.296|
;;;230    	{
;;;231    		unsigned char Sum = 0;
00002a  2500              MOVS     r5,#0
;;;232    		unsigned char GetSum = 0;
00002c  9006              STR      r0,[sp,#0x18]
;;;233    	
;;;234    		int	Location_State_ = 0;
00002e  9005              STR      r0,[sp,#0x14]
;;;235    		sscanf((const char *)RecvBuffer + 1, "%f,%f,%f,%d,%d*%02X#",&XbiasFromros,&YbiasFromros,&TheataFromros,&Location_State_,&HandlePlcData.SendPlcData.SLAMerror,&GetSum);
000030  aa05              ADD      r2,sp,#0x14
000032  ab07              ADD      r3,sp,#0x1c
000034  e9cd3200          STRD     r3,r2,[sp,#0]
000038  a906              ADD      r1,sp,#0x18
00003a  4870              LDR      r0,|L5.508|
00003c  e9cd0102          STRD     r0,r1,[sp,#8]
000040  ab08              ADD      r3,sp,#0x20
000042  aa09              ADD      r2,sp,#0x24
000044  a16e              ADR      r1,|L5.512|
000046  1c70              ADDS     r0,r6,#1
000048  f7fffffe          BL       __0sscanf
;;;236    		
;;;237    
;;;238    		while(RecvBuffer[i] != '*')/*校验起始到结束信号*/
00004c  e003              B        |L5.86|
                  |L5.78|
;;;239    		{
;;;240    			Sum += RecvBuffer[i];
00004e  1950              ADDS     r0,r2,r5
000050  b2c5              UXTB     r5,r0
;;;241    			i++;
000052  1c64              ADDS     r4,r4,#1
000054  b2e4              UXTB     r4,r4
                  |L5.86|
000056  5d32              LDRB     r2,[r6,r4]            ;238
000058  2a2a              CMP      r2,#0x2a              ;238
00005a  d1f8              BNE      |L5.78|
;;;242    		}
;;;243    
;;;244    //		static int iii = 0;
;;;245    //		if(iii++>=100)
;;;246    //		{
;;;247    //			Uart_Printf(COM1,"Sum=%d	GetSum=%d	x=%f	y=%f	w=%f	status=%d	error=%d\r\n",
;;;248    //			Sum,GetSum,XbiasFromros*1000,YbiasFromros*1000,TheataFromros,Location_State_,HandlePlcData.SendPlcData.SLAMerror);
;;;249    //			iii=0;
;;;250    //		}
;;;251    //		
;;;252    		if(Location_State_)
00005c  9805              LDR      r0,[sp,#0x14]
00005e  b160              CBZ      r0,|L5.122|
;;;253    		{
;;;254    			PowOffList.PowOffCoordinatePoints.xPoint =	XbiasFromros;
000060  486d              LDR      r0,|L5.536|
000062  ed9d0a09          VLDR     s0,[sp,#0x24]
000066  ed800a00          VSTR     s0,[r0,#0]
;;;255    			PowOffList.PowOffCoordinatePoints.yPoint =	YbiasFromros;
00006a  ed9d0a08          VLDR     s0,[sp,#0x20]
00006e  ed800a01          VSTR     s0,[r0,#4]
;;;256    			PowOffList.PowOffCoordinatePoints.W =	TheataFromros;		
000072  ed9d0a07          VLDR     s0,[sp,#0x1c]
000076  ed800a02          VSTR     s0,[r0,#8]
                  |L5.122|
;;;257    		}
;;;258    		
;;;259     		if(Sum == GetSum)
00007a  f89d0018          LDRB     r0,[sp,#0x18]
00007e  4285              CMP      r5,r0
000080  d152              BNE      |L5.296|
;;;260    		{
;;;261    			//1.计算到运动中心的距离
;;;262    			ControlInputValue.W_coordinates = (int)((TheataFromros) * 1000 + ControlInputValue.AgvWoffset);
000082  4c66              LDR      r4,|L5.540|
000084  eddd0a07          VLDR     s1,[sp,#0x1c]
000088  ed940a16          VLDR     s0,[r4,#0x58]
00008c  ed9f8a64          VLDR     s16,|L5.544|
000090  ee000a88          VMLA.F32 s0,s1,s16
000094  eefd8ac0          VCVT.S32.F32 s17,s0
000098  edc48a02          VSTR     s17,[r4,#8]
;;;263    			
;;;264    
;;;265    
;;;266    			ControlInputValue.X_coordinates = (int)(XbiasFromros*1000 - sin((float)ControlInputValue.W_coordinates / 1000.0f) * ControlInputValue.AgvYoffset);
00009c  6d60              LDR      r0,[r4,#0x54]  ; ControlInputValue
00009e  f7fffffe          BL       __aeabi_f2d
0000a2  eef80ae8          VCVT.F32.S32 s1,s17
0000a6  ec410b19          VMOV     d9,r0,r1
0000aa  ee800a88          VDIV.F32 s0,s1,s16
0000ae  ee100a10          VMOV     r0,s0
0000b2  f7fffffe          BL       __aeabi_f2d
0000b6  ec410b10          VMOV     d0,r0,r1
0000ba  f7fffffe          BL       __hardfp_sin
0000be  ec532b19          VMOV     r2,r3,d9
0000c2  ec510b10          VMOV     r0,r1,d0
0000c6  f7fffffe          BL       __aeabi_dmul
0000ca  ed9d0a09          VLDR     s0,[sp,#0x24]
0000ce  ec410b19          VMOV     d9,r0,r1
0000d2  ee200a08          VMUL.F32 s0,s0,s16
0000d6  ee100a10          VMOV     r0,s0
0000da  f7fffffe          BL       __aeabi_f2d
0000de  ec532b19          VMOV     r2,r3,d9
0000e2  f7fffffe          BL       __aeabi_dsub
0000e6  f7fffffe          BL       __aeabi_d2iz
0000ea  6020              STR      r0,[r4,#0]  ; ControlInputValue
;;;267    			ControlInputValue.Y_coordinates = (int)(YbiasFromros*1000 + cos((float)ControlInputValue.W_coordinates / 1000.0f) * ControlInputValue.AgvYoffset);
0000ec  6d60              LDR      r0,[r4,#0x54]  ; ControlInputValue
0000ee  f7fffffe          BL       __aeabi_f2d
0000f2  ed940a02          VLDR     s0,[r4,#8]
0000f6  ec410b19          VMOV     d9,r0,r1
0000fa  eef80ac0          VCVT.F32.S32 s1,s0
0000fe  ee800a88          VDIV.F32 s0,s1,s16
000102  ee100a10          VMOV     r0,s0
000106  f7fffffe          BL       __aeabi_f2d
00010a  ec410b10          VMOV     d0,r0,r1
00010e  f7fffffe          BL       __hardfp_cos
000112  ec532b19          VMOV     r2,r3,d9
000116  ec510b10          VMOV     r0,r1,d0
00011a  f7fffffe          BL       __aeabi_dmul
00011e  ec410b19          VMOV     d9,r0,r1
000122  ed9d0a08          VLDR     s0,[sp,#0x20]
000126  e000              B        |L5.298|
                  |L5.296|
000128  e060              B        |L5.492|
                  |L5.298|
00012a  ee200a08          VMUL.F32 s0,s0,s16
00012e  ee100a10          VMOV     r0,s0
000132  f7fffffe          BL       __aeabi_f2d
000136  ec532b19          VMOV     r2,r3,d9
00013a  f7fffffe          BL       __aeabi_dadd
00013e  f7fffffe          BL       __aeabi_d2iz
000142  6060              STR      r0,[r4,#4]  ; ControlInputValue
;;;268    
;;;269    			ControlInputValue.X_coordinates = (int)(ControlInputValue.X_coordinates + cos((float)ControlInputValue.W_coordinates / 1000.0f) * ControlInputValue.AgvXoffset);
000144  6d20              LDR      r0,[r4,#0x50]  ; ControlInputValue
000146  f7fffffe          BL       __aeabi_f2d
00014a  ed940a02          VLDR     s0,[r4,#8]
00014e  ec410b19          VMOV     d9,r0,r1
000152  eef80ac0          VCVT.F32.S32 s1,s0
000156  ee800a88          VDIV.F32 s0,s1,s16
00015a  ee100a10          VMOV     r0,s0
00015e  f7fffffe          BL       __aeabi_f2d
000162  ec410b10          VMOV     d0,r0,r1
000166  f7fffffe          BL       __hardfp_cos
00016a  ec532b19          VMOV     r2,r3,d9
00016e  ec510b10          VMOV     r0,r1,d0
000172  f7fffffe          BL       __aeabi_dmul
000176  ec410b19          VMOV     d9,r0,r1
00017a  6820              LDR      r0,[r4,#0]  ; ControlInputValue
00017c  f7fffffe          BL       __aeabi_i2d
000180  ec532b19          VMOV     r2,r3,d9
000184  f7fffffe          BL       __aeabi_dadd
000188  f7fffffe          BL       __aeabi_d2iz
00018c  6020              STR      r0,[r4,#0]  ; ControlInputValue
;;;270    			ControlInputValue.Y_coordinates = (int)(ControlInputValue.Y_coordinates + sin((float)ControlInputValue.W_coordinates / 1000.0f) * ControlInputValue.AgvXoffset);
00018e  6d20              LDR      r0,[r4,#0x50]  ; ControlInputValue
000190  f7fffffe          BL       __aeabi_f2d
000194  ed940a02          VLDR     s0,[r4,#8]
000198  ec410b19          VMOV     d9,r0,r1
00019c  eef80ac0          VCVT.F32.S32 s1,s0
0001a0  ee800a88          VDIV.F32 s0,s1,s16
0001a4  ee100a10          VMOV     r0,s0
0001a8  f7fffffe          BL       __aeabi_f2d
0001ac  ec410b10          VMOV     d0,r0,r1
0001b0  f7fffffe          BL       __hardfp_sin
0001b4  ec532b19          VMOV     r2,r3,d9
0001b8  ec510b10          VMOV     r0,r1,d0
0001bc  f7fffffe          BL       __aeabi_dmul
0001c0  ec410b18          VMOV     d8,r0,r1
0001c4  6860              LDR      r0,[r4,#4]  ; ControlInputValue
0001c6  f7fffffe          BL       __aeabi_i2d
0001ca  ec532b18          VMOV     r2,r3,d8
0001ce  f7fffffe          BL       __aeabi_dadd
0001d2  f7fffffe          BL       __aeabi_d2iz
0001d6  6060              STR      r0,[r4,#4]  ; ControlInputValue
;;;271    		
;;;272    //			static int i_time = 30;
;;;273    //			if(i_time++ >= 20)
;;;274    //			{
;;;275    //				Uart_Printf(COM1,"X=%d	y=%d	w=%d	x1=%.1f	y=%.1f	w=%f	Angle=%.2f	speed=%d	%d\r\n",
;;;276    //				ControlInputValue.X_coordinates,ControlInputValue.Y_coordinates,ControlInputValue.W_coordinates,
;;;277    //				XbiasFromros*1000,YbiasFromros*1000,TheataFromros*180.0f/PI,ControlInputValue.W_coordinates *180.0f /PI /1000,
;;;278    //				ControlInputValue.AgvActualFirstSpeed,ControlInputValue.AgvActualSecondSpeed);
;;;279    //				i_time = 0;
;;;280    //			}
;;;281    			//AGV共线平移
;;;282    //			
;;;283    //			ControlInputValue.X_coordinates = (int)(ControlInputValue.X_coordinates - sin((float)ControlInputValue.W_coordinates / 1000.0f) * PinYI_X);
;;;284    //			ControlInputValue.Y_coordinates = (int)(ControlInputValue.Y_coordinates + cos((float)ControlInputValue.W_coordinates / 1000.0f) * PinYI_X);
;;;285    
;;;286    //			ControlInputValue.X_coordinates = (int)(ControlInputValue.X_coordinates - cos((float)ControlInputValue.W_coordinates / 1000.0f) * PinYI_Y);
;;;287    //			ControlInputValue.Y_coordinates = (int)(ControlInputValue.Y_coordinates - sin((float)ControlInputValue.W_coordinates / 1000.0f) * PinYI_Y);
;;;288    //				
;;;289    		//	Uart_Printf(COM1,"X=%d	y=%d\r\n",ControlInputValue.X_coordinates,ControlInputValue.Y_coordinates);
;;;290    			
;;;291    			HandlePlcData.SendPlcData.QR_X = ControlInputValue.X_coordinates;
0001d8  4908              LDR      r1,|L5.508|
0001da  6822              LDR      r2,[r4,#0]  ; ControlInputValue
0001dc  3108              ADDS     r1,r1,#8
0001de  600a              STR      r2,[r1,#0]  ; HandlePlcData
;;;292    			HandlePlcData.SendPlcData.QR_Y = ControlInputValue.Y_coordinates;
0001e0  6048              STR      r0,[r1,#4]  ; HandlePlcData
;;;293    			HandlePlcData.SendPlcData.QR_W = ControlInputValue.W_coordinates;			
0001e2  8920              LDRH     r0,[r4,#8]  ; ControlInputValue
0001e4  8108              STRH     r0,[r1,#8]
;;;294    			
;;;295    			ControlInputValue.SlamOnline = Location_State_;
0001e6  9805              LDR      r0,[sp,#0x14]
0001e8  f884007d          STRB     r0,[r4,#0x7d]
                  |L5.492|
;;;296    		}
;;;297    	}
;;;298    }
0001ec  b00a              ADD      sp,sp,#0x28
0001ee  ecbd8b04          VPOP     {d8-d9}
0001f2  bd70              POP      {r4-r6,pc}
;;;299    
                          ENDP

                  |L5.500|
0001f4  00000000          DCFS     0x00000000 ; 0
                  |L5.504|
0001f8  2400              DCB      "$$",0
0001fa  00                DCB      0
0001fb  00                DCB      0
                  |L5.508|
                          DCD      HandlePlcData+0xfc
                  |L5.512|
000200  25662c25          DCB      "%f,%f,%f,%d,%d*%02X#",0
000204  662c2566
000208  2c25642c
00020c  25642a25
000210  30325823
000214  00      
000215  00                DCB      0
000216  00                DCB      0
000217  00                DCB      0
                  |L5.536|
                          DCD      PowOffList
                  |L5.540|
                          DCD      ControlInputValue
                  |L5.544|
000220  447a0000          DCFS     0x447a0000 ; 1000

                          AREA ||i.ProcessDataFormUartSlam||, CODE, READONLY, ALIGN=2

                  ProcessDataFormUartSlam PROC
;;;320    
;;;321    void ProcessDataFormUartSlam()
000000  e92d43f0          PUSH     {r4-r9,lr}
;;;322    {
000004  b091              SUB      sp,sp,#0x44
;;;323    	static int ConnectSlamTime = 0;
;;;324        unsigned char buff[64];
;;;325        unsigned int len;
;;;326        unsigned i;
;;;327    	static unsigned char ii = 0;
;;;328        len = ReadUart(COM3,buff,64);
000006  2240              MOVS     r2,#0x40
000008  a901              ADD      r1,sp,#4
00000a  2003              MOVS     r0,#3
00000c  f7fffffe          BL       ReadUart
000010  4605              MOV      r5,r0
;;;329    		
;;;330        if( len > 0)   
;;;331    	{
;;;332    //		if(ii++ >= 20)
;;;333    //		{
;;;334    //			EthernetPrintf("len=%d	%s\r\n",len,buff);
;;;335    //			ii = 0;
;;;336    //		}
;;;337            for(i = 0; i < len; i++)
;;;338            {
;;;339    			ProccessAGVDATAInfo(buff+i);
;;;340            }
;;;341    		ConnectSlamTime = 0;
;;;342    		SySAlarmStatus.SLAMComInter = 0;
000012  f8df8040          LDR      r8,|L6.84|
000016  4e10              LDR      r6,|L6.88|
000018  2700              MOVS     r7,#0                 ;330
00001a  b185              CBZ      r5,|L6.62|
00001c  2400              MOVS     r4,#0                 ;337
00001e  f10d0904          ADD      r9,sp,#4              ;324
000022  e004              B        |L6.46|
                  |L6.36|
000024  eb090004          ADD      r0,r9,r4              ;339
000028  f7fffffe          BL       ProccessAGVDATAInfo
00002c  1c64              ADDS     r4,r4,#1              ;337
                  |L6.46|
00002e  42ac              CMP      r4,r5                 ;337
000030  d3f8              BCC      |L6.36|
000032  6077              STR      r7,[r6,#4]            ;341  ; ConnectSlamTime
000034  f8887005          STRB     r7,[r8,#5]
                  |L6.56|
;;;343    
;;;344    	}
;;;345    	else
;;;346    	{
;;;347    		ConnectSlamTime++;
;;;348    		
;;;349    		if(ConnectSlamTime >= 100)
;;;350    		{
;;;351    			ConnectSlamTime = 0;
;;;352    			SySAlarmStatus.SLAMComInter = 1;
;;;353    //			Uart_Printf(COM1,"SLAM通讯报警\r\n");
;;;354    		}
;;;355    	}
;;;356    }
000038  b011              ADD      sp,sp,#0x44
00003a  e8bd83f0          POP      {r4-r9,pc}
                  |L6.62|
00003e  6870              LDR      r0,[r6,#4]            ;347  ; ConnectSlamTime
000040  1c40              ADDS     r0,r0,#1              ;347
000042  6070              STR      r0,[r6,#4]            ;347  ; ConnectSlamTime
000044  2864              CMP      r0,#0x64              ;349
000046  dbf7              BLT      |L6.56|
000048  6077              STR      r7,[r6,#4]            ;351  ; ConnectSlamTime
00004a  2001              MOVS     r0,#1                 ;352
00004c  f8880005          STRB     r0,[r8,#5]            ;352
000050  e7f2              B        |L6.56|
                          ENDP

000052  0000              DCW      0x0000
                  |L6.84|
                          DCD      SySAlarmStatus
                  |L6.88|
                          DCD      ||area_number.27||

                          AREA ||i.UartToROS_Send_Info_To_Server||, CODE, READONLY, ALIGN=1

                  UartToROS_Send_Info_To_Server PROC
;;;200    
;;;201    void UartToROS_Send_Info_To_Server()
000000  b500              PUSH     {lr}
;;;202    {
000002  b0e5              SUB      sp,sp,#0x194
;;;203    	unsigned char atSendBuf[400] = {0};
000004  f44f71c8          MOV      r1,#0x190
000008  a801              ADD      r0,sp,#4
00000a  f7fffffe          BL       __aeabi_memclr4
;;;204    	unsigned int len;
;;;205    	
;;;206    	len = Prepare_UartToROS_Send(atSendBuf);
00000e  a801              ADD      r0,sp,#4
000010  f7fffffe          BL       Prepare_UartToROS_Send
000014  4602              MOV      r2,r0
;;;207    	WriteUart(COM3, atSendBuf, len);
000016  a901              ADD      r1,sp,#4
000018  2003              MOVS     r0,#3
00001a  f7fffffe          BL       WriteUart
;;;208    }
00001e  b065              ADD      sp,sp,#0x194
000020  bd00              POP      {pc}
;;;209    
                          ENDP


                          AREA ||i.UartToROS_Send_Info_To_Server22222||, CODE, READONLY, ALIGN=1

                  UartToROS_Send_Info_To_Server22222 PROC
;;;209    
;;;210    void UartToROS_Send_Info_To_Server22222()
000000  b500              PUSH     {lr}
;;;211    {
000002  b0e5              SUB      sp,sp,#0x194
;;;212    	unsigned char atSendBuf[400] = {0};
000004  f44f71c8          MOV      r1,#0x190
000008  a801              ADD      r0,sp,#4
00000a  f7fffffe          BL       __aeabi_memclr4
;;;213    	unsigned int len;
;;;214    	
;;;215    	len = Prepare_UartToROS_Send222(atSendBuf);
00000e  a801              ADD      r0,sp,#4
000010  f7fffffe          BL       Prepare_UartToROS_Send222
000014  4602              MOV      r2,r0
;;;216    	WriteUart(COM3, atSendBuf, len);
000016  a901              ADD      r1,sp,#4
000018  2003              MOVS     r0,#3
00001a  f7fffffe          BL       WriteUart
;;;217    }
00001e  b065              ADD      sp,sp,#0x194
000020  bd00              POP      {pc}
;;;218    
                          ENDP


                          AREA ||.bss||, DATA, NOINIT, ALIGN=0

                  Buffer2
                          %        120

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

                  integral_X
000000  00000000          DCFS     0x00000000 ; 0

                          AREA ||area_number.13||, DATA, ALIGN=2

                          EXPORTAS ||area_number.13||, ||.data||
                  integral_Y
000000  00000000          DCFS     0x00000000 ; 0

                          AREA ||area_number.14||, DATA, ALIGN=2

                          EXPORTAS ||area_number.14||, ||.data||
                  integral_W
000000  00000000          DCFS     0x00000000 ; 0

                          AREA ||area_number.15||, DATA, ALIGN=2

                          EXPORTAS ||area_number.15||, ||.data||
                  X_sPEED
000000  00000000          DCFS     0x00000000 ; 0

                          AREA ||area_number.16||, DATA, ALIGN=2

                          EXPORTAS ||area_number.16||, ||.data||
                  Y_sPEED
000000  00000000          DCFS     0x00000000 ; 0

                          AREA ||area_number.17||, DATA, ALIGN=2

                          EXPORTAS ||area_number.17||, ||.data||
                  BaseAngleOne
000000  00000000          DCFS     0x00000000 ; 0

                          AREA ||area_number.18||, DATA, ALIGN=2

                          EXPORTAS ||area_number.18||, ||.data||
                  BaseAngleTwo
000000  40490fda          DCFS     0x40490fda ; 3.1415925025939941

                          AREA ||area_number.19||, DATA, ALIGN=2

                          EXPORTAS ||area_number.19||, ||.data||
                  BaseAngleThree
000000  3fc90fda          DCFS     0x3fc90fda ; 1.5707962512969971

                          AREA ||area_number.20||, DATA, ALIGN=2

                          EXPORTAS ||area_number.20||, ||.data||
                  BaseAngleFour
000000  4096cbe4          DCFS     0x4096cbe4 ; 4.7123889923095703

                          AREA ||area_number.21||, DATA, ALIGN=2

                          EXPORTAS ||area_number.21||, ||.data||
                  Tail_X
000000  bf3851ec          DCFS     0xbf3851ec ; -0.72000002861022949

                          AREA ||area_number.22||, DATA, ALIGN=2

                          EXPORTAS ||area_number.22||, ||.data||
                  Tail_Y
000000  00000000          DCFS     0x00000000 ; 0

                          AREA ||area_number.23||, DATA, ALIGN=2

                          EXPORTAS ||area_number.23||, ||.data||
                  Head_X
000000  3f3851ec          DCFS     0x3f3851ec ; 0.72000002861022949

                          AREA ||area_number.24||, DATA, ALIGN=2

                          EXPORTAS ||area_number.24||, ||.data||
                  Head_Y
000000  00000000          DCFS     0x00000000 ; 0

                          AREA ||area_number.25||, DATA, ALIGN=2

                          EXPORTAS ||area_number.25||, ||.data||
                  PinYI_X
                          DCD      0x00000000

                          AREA ||area_number.26||, DATA, ALIGN=2

                          EXPORTAS ||area_number.26||, ||.data||
                  PinYI_Y
                          DCD      0x00000000

                          AREA ||area_number.27||, DATA, ALIGN=2

                          EXPORTAS ||area_number.27||, ||.data||
                  string_len
000000  00                DCB      0x00
                  |symbol_number.43|
000001  00                DCB      0x00
                  iii
000002  0000              DCW      0x0000
                  ConnectSlamTime
                          DCD      0x00000000

;*** Start embedded assembler ***

#line 1 "..\\..\\User\\parameter\\user_Slam.c"
	AREA ||.rev16_text||, CODE
	THUMB
	EXPORT |__asm___11_user_Slam_c_a76a57e0____REV16|
#line 129 "..\\..\\Libraries\\CMSIS\\Include\\core_cmInstr.h"
|__asm___11_user_Slam_c_a76a57e0____REV16| PROC
#line 130

 rev16 r0, r0
 bx lr
	ENDP
	AREA ||.revsh_text||, CODE
	THUMB
	EXPORT |__asm___11_user_Slam_c_a76a57e0____REVSH|
#line 144
|__asm___11_user_Slam_c_a76a57e0____REVSH| PROC
#line 145

 revsh r0, r0
 bx lr
	ENDP
	AREA ||.rrx_text||, CODE
	THUMB
	EXPORT |__asm___11_user_Slam_c_a76a57e0____RRX|
#line 300
|__asm___11_user_Slam_c_a76a57e0____RRX| PROC
#line 301

 rrx r0, r0
 bx lr
	ENDP

;*** End   embedded assembler ***