-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathsdk.lst
2245 lines (2244 loc) · 96 KB
/
sdk.lst
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
1 .file "sdk.c"
9 .Ltext0:
10 .align 2
11 .global hovering_test
13 hovering_test:
14 .LFB3:
15 .file 1 "sdk.c"
1:sdk.c **** /*
2:sdk.c ****
3:sdk.c **** AscTec SDK 3.0
4:sdk.c ****
5:sdk.c **** Copyright (c) 2011, Ascending Technologies GmbH
6:sdk.c **** All rights reserved.
7:sdk.c ****
8:sdk.c **** Redistribution and use in source and binary forms, with or without
9:sdk.c **** modification, are permitted provided that the following conditions are met:
10:sdk.c ****
11:sdk.c **** * Redistributions of source code must retain the above copyright notice,
12:sdk.c **** this list of conditions and the following disclaimer.
13:sdk.c **** * Redistributions in binary form must reproduce the above copyright
14:sdk.c **** notice, this list of conditions and the following disclaimer in the
15:sdk.c **** documentation and/or other materials provided with the distribution.
16:sdk.c ****
17:sdk.c **** THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND ANY
18:sdk.c **** EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
19:sdk.c **** WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
20:sdk.c **** DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE FOR ANY
21:sdk.c **** DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
22:sdk.c **** (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
23:sdk.c **** SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
24:sdk.c **** CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
25:sdk.c **** LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
26:sdk.c **** OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
27:sdk.c **** DAMAGE.
28:sdk.c ****
29:sdk.c **** */
30:sdk.c ****
31:sdk.c **** #include "main.h"
32:sdk.c **** #include "sdk.h"
33:sdk.c **** #include "LL_HL_comm.h"
34:sdk.c **** #include "gpsmath.h"
35:sdk.c **** #include "sdk_telemetry.h"
36:sdk.c **** #include "uart.h"
37:sdk.c **** #include "system.h"
38:sdk.c **** #include "lpc_aci_eeprom.h"
39:sdk.c **** #ifdef MATLAB
40:sdk.c **** #include "..\custom_mdl\onboard_matlab_ert_rtw\onboard_matlab.h"
41:sdk.c **** #endif
42:sdk.c ****
43:sdk.c **** struct WO_SDK_STRUCT WO_SDK;
44:sdk.c **** struct WO_CTRL_INPUT WO_CTRL_Input;
45:sdk.c **** struct RO_RC_DATA RO_RC_Data;
46:sdk.c **** struct RO_ALL_DATA RO_ALL_Data;
47:sdk.c **** struct WO_DIRECT_MOTOR_CONTROL WO_Direct_Motor_Control;
48:sdk.c **** struct WO_DIRECT_INDIVIDUAL_MOTOR_CONTROL WO_Direct_Individual_Motor_Control;
49:sdk.c ****
50:sdk.c **** //waypoint example global variables for jeti display
51:sdk.c **** unsigned char wpExampleWpNr=0;
52:sdk.c **** unsigned char wpExampleActive=0;
53:sdk.c ****
54:sdk.c **** //emergency mode variables
55:sdk.c **** unsigned char emergencyMode;
56:sdk.c **** unsigned char emergencyModeUpdate=0;
57:sdk.c ****
58:sdk.c **** #ifdef MATLAB
59:sdk.c **** unsigned char xbee_send_flag=0;
60:sdk.c **** unsigned char triggerSaveMatlabParams=0; //trigger command to save matlab parameters to flash
61:sdk.c **** struct MATLAB_DEBUG matlab_debug;
62:sdk.c **** struct MATLAB_UART matlab_uart, matlab_uart_tmp;
63:sdk.c **** struct MATLAB_PARAMS matlab_params, matlab_params_tmp;
64:sdk.c ****
65:sdk.c **** void SDK_matlabMainLoop(void);
66:sdk.c **** #endif
67:sdk.c **** void SDK_EXAMPLE_direct_individual_motor_commands(void);
68:sdk.c **** void SDK_EXAMPLE_direct_motor_commands_with_standard_output_mapping(void);
69:sdk.c **** void SDK_EXAMPLE_attitude_commands(void);
70:sdk.c **** void SDK_EXAMPLE_gps_waypoint_control(void);
71:sdk.c **** int SDK_EXAMPLE_turn_motors_on(void);
72:sdk.c **** int SDK_EXAMPLE_turn_motors_off(void);
73:sdk.c ****
74:sdk.c **** /*****self define function declare*****/
75:sdk.c **** void hovering_test(void);
76:sdk.c **** int lock_motors(void);
77:sdk.c **** int unlock_motors(void);
78:sdk.c ****
79:sdk.c **** /****self define global variable****/
80:sdk.c **** static int locker_timer = 0;
81:sdk.c **** int lock_flag = 0;
82:sdk.c ****
83:sdk.c **** /******** SDK in general ************
84:sdk.c **** *
85:sdk.c **** * You can find further information about the AscTec SDK in our AscTec Wiki: http://wiki.asctec.de
86:sdk.c **** *
87:sdk.c **** * Scroll down for information about how to change the Eclipse settings to use the SDK with the Asc
88:sdk.c **** *
89:sdk.c **** *
90:sdk.c **** * SDK_mainloop(void) is triggered @ 1kHz.
91:sdk.c **** *
92:sdk.c **** * RO_(Read Only) data is updated before entering this function
93:sdk.c **** * and can be read to obtain information for supervision or control
94:sdk.c **** *
95:sdk.c **** * WO_(Write Only) data is written to the LL processor after
96:sdk.c **** * execution of this function.
97:sdk.c **** *
98:sdk.c **** * WO_ and RO_ structs are defined in sdk.h
99:sdk.c **** *
100:sdk.c **** * The struct RO_ALL_Data (defined in sdk.h)
101:sdk.c **** * is used to read all sensor data, results of the data fusion
102:sdk.c **** * and R/C inputs transmitted from the LL-processor. This struct is
103:sdk.c **** * automatically updated at 1 kHz.
104:sdk.c **** *
105:sdk.c **** */
106:sdk.c ****
107:sdk.c **** /******* How to flash the high level processor ********
108:sdk.c **** *
109:sdk.c **** * The easiest way to get your code on the high level processor is to use the JTAG-adapter.
110:sdk.c **** *
111:sdk.c **** * It needs three steps to get your code on the high level processor.
112:sdk.c **** * 1. Build your code ("Build Project")
113:sdk.c **** * 2. Connect your JTAG adapter and start the JTAG connection (Run "OpenOCD Asctec-JTAG")
114:sdk.c **** * 3. Flash the processor (Run "Asctec JTAG Debug")
115:sdk.c **** *
116:sdk.c **** * In the menu "Run -> External Tools -> External Tools Configuration..." you
117:sdk.c **** * will find "OpenOCD Asctec-JTAG". If the JTAG connection was activated
118:sdk.c **** * correctly, the console will show only the following line:
119:sdk.c **** * "Info: openocd.c:92 main(): Open On-Chip Debugger (2007-08-10 22:30 CEST)"
120:sdk.c **** *
121:sdk.c **** * Do not launch more than ONE JTAG-connection at the same time!
122:sdk.c **** *
123:sdk.c **** * In the menu "Run -> Debug Configurations..." you will find "Asctec JTAG Debug"
124:sdk.c **** * If the code was successfully flashed on the processor, the program will switch
125:sdk.c **** * to the Debug window.
126:sdk.c **** *
127:sdk.c **** * If you want to flash the high level processor using a serial interface (AscTec USB adapter)
128:sdk.c **** * and bootloader software like "Flash Magic", you can find the main.hex in your workspace folder.
129:sdk.c **** *
130:sdk.c **** */
131:sdk.c ****
132:sdk.c **** /********* Debugging and testing your code ************
133:sdk.c **** *
134:sdk.c **** * After flashing the HL, your code can be debugged online via JTAG. The ARM7 supports ONE hardware
135:sdk.c **** * You can monitor the CPU-load by looking at RO_ALL_Data.HL_cpu_load. As long as this value is bel
136:sdk.c **** * code in SDK_mainloop() is executed at 1 kHz. Example: 0.021 means the HL code uses 21% of the av
137:sdk.c **** *
138:sdk.c **** * To activate the SDK controls, the serial interface switch on your R/C (channel 5) needs to be in
139:sdk.c **** * If you use our standard RC Futaba FF7, the black marked switch on the right hand side needs to b
140:sdk.c **** * the pilot to enable the serial interface.
141:sdk.c **** *
142:sdk.c **** * To test if the flashing of the HLP worked and how to enable the serial interface, you can run th
143:sdk.c **** * motors on and off every 2 seconds" in the SDK_mainloop.
144:sdk.c **** *
145:sdk.c **** */
146:sdk.c ****
147:sdk.c **** /********** Serial communication **********
148:sdk.c **** *
149:sdk.c **** * If your project needs communication via HL serial 0 port, the easiest way is to use the AscTec C
150:sdk.c **** * (information and tutorials can be found in the AscTec Wiki) -
151:sdk.c **** * or you can directly program the serial port (you find an example of how to do so in main.c, line
152:sdk.c **** *
153:sdk.c **** */
154:sdk.c ****
155:sdk.c **** /********** Simulink Toolkit ***********
156:sdk.c **** *
157:sdk.c **** * To use this SDK in combination with the AscTec Simulink Toolkit you need to change the Build Con
158:sdk.c **** * Click on "Project - Build Configuration - Set Active - AscTec Simulink Toolkit" in the menu bar.
159:sdk.c **** *
160:sdk.c **** * Please carefully follow the complete instructions of the Simulink Toolkit manual before flashing
161:sdk.c **** * You have to go through all steps until you can receive any data with the UART_Communication mode
162:sdk.c **** *
163:sdk.c **** * If you want to combine the automatically generated C-code from the Simulink Toolkit with additio
164:sdk.c **** * please put the additional C-code into SDK_matlabMainLoop().
165:sdk.c **** */
166:sdk.c ****
167:sdk.c **** /********** Emergency Modes ************
168:sdk.c **** *
169:sdk.c **** * Now you can set the Emergency Modes directly from the HLP. For more information about the EMs, p
170:sdk.c **** * look at the AscTec Wiki. Please set an emergency mode according to the flight path of your fligh
171:sdk.c **** * with SDK_SetEmergencyMode(). If non was set, Direct Landing is activated.
172:sdk.c **** */
173:sdk.c ****
174:sdk.c **** void SDK_mainloop(void)
175:sdk.c **** {
176:sdk.c **** #ifdef MATLAB
177:sdk.c **** SDK_matlabMainLoop(); //this runs only in combination with the AscTec Simulink Toolkit
178:sdk.c ****
179:sdk.c **** //jeti telemetry can always be activated. You may deactivate this call if you don't have the AscTe
180:sdk.c **** SDK_jetiAscTecExampleRun();
181:sdk.c ****
182:sdk.c **** #else //write your own C-code within this function
183:sdk.c ****
184:sdk.c **** /****hovering test****/
185:sdk.c **** // if(lock_flag == 0)
186:sdk.c **** // hovering_test();
187:sdk.c **** // if(RO_ALL_Data.flying == 1 && lock_flag == 1)
188:sdk.c **** // lock_motors();
189:sdk.c ****
190:sdk.c **** //you can select an example by using ONE of the functions below.
191:sdk.c **** //CAUTION! Read the code of the examples before you test them on your UAV!
192:sdk.c ****
193:sdk.c **** //example to turn motors on and off every 2 seconds
194:sdk.c **** /*
195:sdk.c **** static int timer=0;
196:sdk.c **** if(++timer<1000) SDK_EXAMPLE_turn_motors_on();
197:sdk.c **** else if(timer<2000) SDK_EXAMPLE_turn_motors_off();
198:sdk.c **** else timer=0;
199:sdk.c **** */
200:sdk.c ****
201:sdk.c ****
202:sdk.c **** //examples which show the different control modes
203:sdk.c ****
204:sdk.c **** //SDK_EXAMPLE_direct_individual_motor_commands();
205:sdk.c **** //SDK_EXAMPLE_direct_motor_commands_with_standard_output_mapping();
206:sdk.c **** //SDK_EXAMPLE_attitude_commands();
207:sdk.c **** //SDK_EXAMPLE_gps_waypoint_control();
208:sdk.c ****
209:sdk.c **** //jeti telemetry can always be activated. You may deactivate this call if you don't use the AscTec
210:sdk.c **** SDK_jetiAscTecExampleRun();
211:sdk.c ****
212:sdk.c **** if (wpExampleActive) //this is used to activate the waypoint example via the jeti telemetry displa
213:sdk.c **** SDK_EXAMPLE_gps_waypoint_control();
214:sdk.c ****
215:sdk.c **** #endif
216:sdk.c **** }
217:sdk.c ****
218:sdk.c **** void hovering_test()
219:sdk.c **** {
16 @ Function supports interworking.
17 @ args = 0, pretend = 0, frame = 0
18 @ frame_needed = 0, uses_anonymous_args = 0
19 stmfd sp!, {r4, r5, lr}
20 0000 30402DE9 .LCFI0:
21 .loc 1 233 0
220:sdk.c **** static int hovering_timer = 0;
221:sdk.c **** WO_SDK.ctrl_mode = 0x02; //0x02: attitude and throttle control: commands are input for standard at
222:sdk.c **** WO_SDK.ctrl_enabled = 1; //1: enable control by HL processor
223:sdk.c **** // WO_SDK.disable_motor_onoff_by_stick=0;
224:sdk.c ****
225:sdk.c ****
226:sdk.c **** WO_CTRL_Input.ctrl = 0x1f; //control height, thrust and attitude(pitch roll and yaw)
227:sdk.c **** WO_CTRL_Input.pitch = 50;
228:sdk.c **** WO_CTRL_Input.roll = 5;
229:sdk.c **** WO_CTRL_Input.yaw = 0;
230:sdk.c ****
231:sdk.c **** // WO_CTRL_Input.thrust = 2000; //10% throttle command
232:sdk.c ****
233:sdk.c **** if(RO_ALL_Data.flying == 1)
22 3, .L16
23 0004 E4309FE5 .loc 1 221 0
24 ldr r1, .L16+4
25 0008 E4109FE5 .loc 1 226 0
26 ldr r0, .L16+8
27 000c E4009FE5 .loc 1 233 0
28 ldrb r4, [r3, #2] @ zero_extendqisi2
29 0010 0240D3E5 .loc 1 222 0
30 mov r3, #1
31 0014 0130A0E3 strb r3, [r1, #1]
32 0018 0130C1E5 .loc 1 226 0
33 mov r3, #31 @ movhi
34 001c 1F30A0E3 strh r3, [r0, #8] @ movhi
35 0020 B830C0E1 .loc 1 227 0
36 mov r3, #50 @ movhi
37 0024 3230A0E3 .loc 1 233 0
38 cmp r4, #1
39 0028 010054E3 .loc 1 227 0
40 strh r3, [r0, #0] @ movhi
41 002c B030C0E1 .loc 1 228 0
42 mov r3, #5 @ movhi
43 0030 0530A0E3 strh r3, [r0, #2] @ movhi
44 0034 B230C0E1 .loc 1 252 0
234:sdk.c **** {
235:sdk.c **** hovering_timer ++;
236:sdk.c **** if(hovering_timer<5000)
237:sdk.c **** WO_CTRL_Input.thrust = 2500;
238:sdk.c **** else if(hovering_timer>=5000 && hovering_timer<7000)
239:sdk.c **** WO_CTRL_Input.thrust = 2100;
240:sdk.c **** else if(hovering_timer>=7000 && hovering_timer<9000)
241:sdk.c **** WO_CTRL_Input.thrust = 9000-hovering_timer;
242:sdk.c **** else if(hovering_timer>=9000 && hovering_timer<10000)
243:sdk.c **** WO_CTRL_Input.thrust = 0;
244:sdk.c **** else
245:sdk.c **** {
246:sdk.c **** lock_flag = 1;
247:sdk.c **** // lock_motors();
248:sdk.c **** hovering_timer = 10000;
249:sdk.c **** }
250:sdk.c **** }
251:sdk.c **** else
252:sdk.c **** hovering_timer = 0;
45 c 1 229 0
46 0038 BC309F15 mov ip, #0
47 .loc 1 221 0
48 003c 00C0A0E3 mov r2, #2
49 strb r2, [r1, #0]
50 0040 0220A0E3 .loc 1 229 0
51 0044 0020C1E5 strh ip, [r0, #4] @ movhi
52 .loc 1 252 0
53 0048 B4C0C0E1 strne ip, [r3, #0]
54 .loc 1 233 0
55 004c 00C08315 beq .L15
56 .L13:
57 0050 0100000A .loc 1 253 0
58 ldmfd sp!, {r4, r5, lr}
253:sdk.c **** }
59 .loc 1 235 0
60 0054 3040BDE8 ldr r5, .L16+12
61 0058 1EFF2FE1 ldr r1, [r5, #0]
62 .loc 1 236 0
63 mov r3, #4992
64 005c 98509FE5 add r3, r3, #7
65 0060 001095E5 .loc 1 235 0
66 add r2, r1, #1
67 0064 4E3DA0E3 .loc 1 236 0
68 0068 073083E2 cmp r2, r3
69 .loc 1 237 0
70 006c 012081E2 movle r3, #2496
71 addle r3, r3, #4
72 0070 030052E1 .loc 1 235 0
73 str r2, [r5, #0]
74 0074 273DA0D3 .loc 1 237 0
75 0078 043083D2 strleh r3, [r0, #6] @ movhi
76 .loc 1 236 0
77 007c 002085E5 ble .L13
78 .loc 1 238 0
79 0080 B630C0D1 sub r3, r1, #4992
80 mov lr, #1984
81 0084 F2FFFFDA sub r3, r3, #7
82 add lr, lr, #15
83 0088 4E3D41E2 cmp r3, lr
84 008c 1FEDA0E3 .loc 1 239 0
85 0090 073043E2 movls r3, #2096
86 0094 0FE08EE2 addls r3, r3, #4
87 0098 0E0053E1 strlsh r3, [r0, #6] @ movhi
88 .loc 1 238 0
89 009c 833EA093 bls .L13
90 00a0 04308392 .loc 1 240 0
91 00a4 B630C091 sub r3, r1, #6976
92 sub r3, r3, #23
93 00a8 E9FFFF9A cmp r3, lr
94 .loc 1 241 0
95 00ac 6D3D41E2 rsbls r3, r2, #8960
96 00b0 173043E2 addls r3, r3, #40
97 00b4 0E0053E1 strlsh r3, [r0, #6] @ movhi
98 .loc 1 240 0
99 00b8 233C6292 bls .L13
100 00bc 28308392 .loc 1 242 0
101 00c0 B630C091 sub r3, r1, #8960
102 sub r3, r3, #39
103 00c4 E2FFFF9A cmp r3, #1000
104 .loc 1 248 0
105 00c8 233C41E2 movcs r3, #9984
106 00cc 273043E2 .loc 1 246 0
107 00d0 FA0F53E3 ldrcs r2, .L16+16
108 .loc 1 248 0
109 00d4 273CA023 addcs r3, r3, #16
110 .loc 1 243 0
111 00d8 20209F25 strcch ip, [r0, #6] @ movhi
112 .loc 1 246 0
113 00dc 10308322 strcs r4, [r2, #0]
114 .loc 1 248 0
115 00e0 B6C0C031 strcs r3, [r5, #0]
116 b .L13
117 00e4 00408225 .L17:
118 .align 2
119 00e8 00308525 .L16:
120 00ec D8FFFFEA .word RO_ALL_Data
121 .word WO_SDK
122 .word WO_CTRL_Input
123 .word hovering_timer.1780
124 00f0 00000000 .word lock_flag
125 00f4 00000000 .LFE3:
127 00fc 28000000 .align 2
128 0100 00000000 .global unlock_motors
130 unlock_motors:
131 .LFB4:
132 .loc 1 256 0
133 @ Function supports interworking.
134 @ args = 0, pretend = 0, frame = 0
135 @ frame_needed = 0, uses_anonymous_args = 0
254:sdk.c ****
255:sdk.c **** int unlock_motors()
256:sdk.c **** {
136 1 276 0
137 ldrh ip, [r1, #8]
138 .loc 1 262 0
139 ldr r3, .L20+4
257:sdk.c **** WO_SDK.ctrl_mode = 0x02; //0x02: attitude and throttle control: commands are input for standard at
258:sdk.c **** WO_SDK.ctrl_enabled = 1; //1: enable control by HL processor
259:sdk.c **** WO_SDK.disable_motor_onoff_by_stick=0;
260:sdk.c ****
261:sdk.c **** locker_timer = 0;
262:sdk.c **** if(++locker_timer >= 2000)
263:sdk.c **** {
264:sdk.c **** locker_timer = 0;
265:sdk.c **** return(1); //1 => start sequence completed => motors running => user can stop calling this functi
266:sdk.c **** }
267:sdk.c **** else if(locker_timer > 1000) //neutral stick command for 500 ms
268:sdk.c **** {
269:sdk.c **** WO_CTRL_Input.ctrl |= 0x0C; //0x0C: enable throttle control and yaw control
270:sdk.c **** WO_CTRL_Input.thrust = 0;
271:sdk.c **** WO_CTRL_Input.yaw = 0;
272:sdk.c **** return(0);
273:sdk.c **** }
274:sdk.c **** else //hold stick command for 500 ms
275:sdk.c **** {
276:sdk.c **** WO_CTRL_Input.ctrl |= 0x0C; //0x0C: enable throttle control and yaw control
140 c 1 257 0
141 0104 48109FE5 ldr lr, .L20+8
142 .loc 1 258 0
143 0108 10402DE9 mov r4, #1
144 .loc 1 278 0
145 mvn r2, #2032
146 010c B8C0D1E1 .loc 1 259 0
147 mov r0, #0
148 0110 40309FE5 .loc 1 276 0
149 orr ip, ip, #12
150 0114 40E09FE5 .loc 1 278 0
151 sub r2, r2, #14
152 0118 0140A0E3 .loc 1 262 0
277:sdk.c **** WO_CTRL_Input.thrust = 0;
278:sdk.c **** WO_CTRL_Input.yaw = -2047;
153 #0]
154 011c 7F2EE0E3 .loc 1 257 0
155 mov r3, #2
156 0120 0000A0E3 .loc 1 278 0
157 strh r2, [r1, #4] @ movhi
158 0124 0CC08CE3 .loc 1 257 0
159 strb r3, [lr, #0]
160 0128 0E2042E2 .loc 1 259 0
161 strb r0, [lr, #2]
162 012c 004083E5 .loc 1 276 0
163 strh ip, [r1, #8] @ movhi
164 0130 0230A0E3 .loc 1 258 0
165 strb r4, [lr, #1]
166 0134 B420C1E1 .loc 1 277 0
167 strh r0, [r1, #6] @ movhi
168 0138 0030CEE5 .loc 1 281 0
169 ldmfd sp!, {r4, lr}
170 013c 0200CEE5 bx lr
171 .L21:
172 0140 B8C0C1E1 .align 2
173 .L20:
174 0144 0140CEE5 .word WO_CTRL_Input
175 .word locker_timer
176 0148 B600C1E1 .word WO_SDK
279:sdk.c **** return(0);
280:sdk.c **** }
281:sdk.c **** }
177 ors, .-unlock_motors
178 014c 1040BDE8 .align 2
179 0150 1EFF2FE1 .global lock_motors
181 lock_motors:
182 .LFB5:
183 0154 00000000 .loc 1 284 0
184 0158 24000000 @ Function supports interworking.
185 015c 00000000 @ args = 0, pretend = 0, frame = 0
186 @ frame_needed = 0, uses_anonymous_args = 0
187 .loc 1 304 0
188 ldr r1, .L24
189 .loc 1 284 0
190 stmfd sp!, {r4, lr}
191 .LCFI2:
192 .loc 1 304 0
282:sdk.c ****
283:sdk.c **** int lock_motors()
284:sdk.c **** {
193 8]
194 .loc 1 290 0
195 ldr r3, .L24+4
196 .loc 1 285 0
285:sdk.c **** WO_SDK.ctrl_mode = 0x02; //0x02: attitude and throttle control: commands are input for standard at
286:sdk.c **** WO_SDK.ctrl_enabled = 1; //1: enable control by HL processor
287:sdk.c **** WO_SDK.disable_motor_onoff_by_stick=0;
288:sdk.c ****
289:sdk.c **** locker_timer = 0;
290:sdk.c **** if(++locker_timer >= 2000)
291:sdk.c **** {
292:sdk.c **** locker_timer = 0;
293:sdk.c **** return(1); //1 => stop sequence completed => motors turned off => user can stop calling this func
294:sdk.c **** }
295:sdk.c **** else if(locker_timer > 1000) //neutral stick command for 500 ms
296:sdk.c **** {
297:sdk.c **** WO_CTRL_Input.ctrl |= 0x0C; //0x0C: enable throttle control and yaw control
298:sdk.c **** WO_CTRL_Input.thrust = 0;
299:sdk.c **** WO_CTRL_Input.yaw = 0;
300:sdk.c **** return(0);
301:sdk.c **** }
302:sdk.c **** else //hold stick command for 500 ms
303:sdk.c **** {
304:sdk.c **** WO_CTRL_Input.ctrl |= 0x0C; //0x0C: enable throttle control and yaw control
197 lr, .L24+8
198 0160 48109FE5 .loc 1 286 0
199 mov r4, #1
200 0164 10402DE9 .loc 1 306 0
201 mvn r2, #2032
202 .loc 1 287 0
203 0168 B8C0D1E1 mov r0, #0
204 .loc 1 304 0
205 016c 40309FE5 orr ip, ip, #12
206 .loc 1 306 0
207 0170 40E09FE5 sub r2, r2, #14
208 .loc 1 290 0
209 0174 0140A0E3 str r4, [r3, #0]
305:sdk.c **** WO_CTRL_Input.thrust = 0;
306:sdk.c **** WO_CTRL_Input.yaw = -2047;
210
211 0178 7F2EE0E3 mov r3, #2
212 .loc 1 306 0
213 017c 0000A0E3 strh r2, [r1, #4] @ movhi
214 .loc 1 285 0
215 0180 0CC08CE3 strb r3, [lr, #0]
216 .loc 1 287 0
217 0184 0E2042E2 strb r0, [lr, #2]
218 .loc 1 304 0
219 0188 004083E5 strh ip, [r1, #8] @ movhi
220 .loc 1 286 0
221 018c 0230A0E3 strb r4, [lr, #1]
222 .loc 1 305 0
223 0190 B420C1E1 strh r0, [r1, #6] @ movhi
224 .loc 1 309 0
225 0194 0030CEE5 ldmfd sp!, {r4, lr}
226 bx lr
227 0198 0200CEE5 .L25:
228 .align 2
229 019c B8C0C1E1 .L24:
230 .word WO_CTRL_Input
231 01a0 0140CEE5 .word locker_timer
232 .word WO_SDK
233 01a4 B600C1E1 .LFE5:
307:sdk.c **** return(0);
308:sdk.c **** }
309:sdk.c **** }
234 ock_motors
235 01a8 1040BDE8 .align 2
236 01ac 1EFF2FE1 .global SDK_SetEmergencyMode
238 SDK_SetEmergencyMode:
239 .LFB6:
240 01b0 00000000 .loc 1 316 0
241 01b4 24000000 @ Function supports interworking.
242 01b8 00000000 @ args = 0, pretend = 0, frame = 0
243 @ frame_needed = 0, uses_anonymous_args = 0
244 @ link register save eliminated.
245 .LVL0:
246 and r0, r0, #255
247 .loc 1 317 0
248 sub r3, r0, #1
249 cmp r0, #8
310:sdk.c ****
311:sdk.c **** /*
312:sdk.c **** *
313:sdk.c **** * Sets emergency mode on LowLevel processor. Select one of the EM_ defines as mode option. See EM_
314:sdk.c **** */
315:sdk.c ****
316:sdk.c **** void SDK_SetEmergencyMode(unsigned char mode) {
250 loc 1 316 0
251 @ lr needed for prologue
252 .loc 1 317 0
253 bls .L27
254 cmp r0, #4
255 bxne lr
256 01bc FF0000E2 .L27:
317:sdk.c **** if ((mode != EM_SAVE_EXTENDED_WAITING_TIME) && (mode != EM_SAVE) && (mode
257 321 0
258 01c0 013040E2 ldr r3, .L31
259 01c4 080050E3 strb r0, [r3, #0]
260 01c8 01005313 .loc 1 322 0
261 ldr r3, .L31+4
262 mov r2, #1
263 strb r2, [r3, #0]
264 01cc 0100009A bx lr
265 01d0 040050E3 .L32:
266 01d4 1EFF2F11 .align 2
267 .L31:
318:sdk.c **** != EM_RETURN_AT_MISSION_SUMMIT) && (mode
319:sdk.c **** != EM_RETURN_AT_PREDEFINED_HEIGHT))
320:sdk.c **** return;
321:sdk.c **** emergencyMode = mode;
268 gencyMode
269 01d8 10309FE5 .word emergencyModeUpdate
270 01dc 0000C3E5 .LFE6:
322:sdk.c **** emergencyModeUpdate = 1;
271 ize SDK_SetEmergencyMode, .-SDK_SetEmergencyMode
272 01e0 0C309FE5 .align 2
273 01e4 0120A0E3 .global SDK_EXAMPLE_direct_individual_motor_commands
275 01ec 1EFF2FE1 SDK_EXAMPLE_direct_individual_motor_commands:
276 .LFB7:
277 .loc 1 329 0
278 @ Function supports interworking.
279 01f0 00000000 @ args = 0, pretend = 0, frame = 0
280 01f4 00000000 @ frame_needed = 0, uses_anonymous_args = 0
281 @ link register save eliminated.
282 .loc 1 344 0
283 ldr r0, .L47
284 ldr r2, .L47+4
285 ldrh r3, [r0, #16]
286 umull ip, r1, r2, r3
287 rsb r3, r1, r3
323:sdk.c **** }
324:sdk.c ****
325:sdk.c **** /*
326:sdk.c **** * the following example shows the direct motor command usage by mapping the stick directly to the
327:sdk.c **** */
328:sdk.c **** void SDK_EXAMPLE_direct_individual_motor_commands(void)
329:sdk.c **** {
288 rh ip, [r0, #24]
289 .loc 1 344 0
290 add r1, r1, r3, lsr #1
291 .loc 1 351 0
292 mov r3, #2496
330:sdk.c ****
331:sdk.c **** WO_SDK.ctrl_mode=0x00; //0x00: direct individual motor control: individual commands for motors 0..
332:sdk.c **** //0x01: direct motor control using standard output mapping: commands are interpreted as pitc
333:sdk.c **** //0x02: attitude and throttle control: commands are input for standard attitude controller
334:sdk.c **** //0x03: GPS waypoint control
335:sdk.c ****
336:sdk.c **** WO_SDK.ctrl_enabled=1; //0: disable control by HL processor
337:sdk.c **** //1: enable control by HL processor
338:sdk.c ****
339:sdk.c **** WO_SDK.disable_motor_onoff_by_stick=0;
340:sdk.c ****
341:sdk.c **** unsigned int i;
342:sdk.c ****
343:sdk.c **** //scale throttle stick to [0..200] and map it to all motors
344:sdk.c **** WO_Direct_Individual_Motor_Control.motor[0]=RO_ALL_Data.channel[2]/21;
293 1 344 0
294 01f8 B0009FE5 ldr r0, .L47+8
295 01fc B0209FE5 .loc 1 331 0
296 0200 B031D0E1 ldr r2, .L47+12
297 0204 92C381E0 .loc 1 351 0
298 0208 033061E0 add r3, r3, #4
345:sdk.c **** WO_Direct_Individual_Motor_Control.motor[1]=RO_ALL_Data.channel[2]/21;
346:sdk.c **** WO_Direct_Individual_Motor_Control.motor[2]=RO_ALL_Data.channel[2]/21;
347:sdk.c **** WO_Direct_Individual_Motor_Control.motor[3]=RO_ALL_Data.channel[2]/21;
348:sdk.c **** WO_Direct_Individual_Motor_Control.motor[4]=RO_ALL_Data.channel[2]/21;
349:sdk.c **** WO_Direct_Individual_Motor_Control.motor[5]=RO_ALL_Data.channel[2]/21;
350:sdk.c ****
351:sdk.c **** if (RO_ALL_Data.channel[6]>2500)
299 1 344 0
300 020c B8C1D0E1 mov r1, r1, lsr #4
301 and r1, r1, #255
302 0210 A31081E0 .loc 1 351 0
303 cmp ip, r3
304 0214 273DA0E3 .loc 1 331 0
305 mov r3, #0
306 0218 98009FE5 .loc 1 336 0
307 mov ip, #1
308 021c 98209FE5 .loc 1 329 0
309 @ lr needed for prologue
310 0220 043083E2 .loc 1 339 0
311 strb r3, [r2, #2]
312 0224 2112A0E1 .loc 1 349 0
313 0228 FF1001E2 strb r1, [r0, #5]
314 .loc 1 331 0
315 022c 03005CE1 strb r3, [r2, #0]
316 .loc 1 336 0
317 0230 0030A0E3 strb ip, [r2, #1]
318 .loc 1 344 0
319 0234 01C0A0E3 strb r1, [r0, #0]
320 .loc 1 345 0
321 strb r1, [r0, #1]
322 .loc 1 346 0
323 0238 0230C2E5 strb r1, [r0, #2]
324 .loc 1 347 0
325 023c 0510C0E5 strb r1, [r0, #3]
326 .loc 1 348 0
327 0240 0030C2E5 strb r1, [r0, #4]
328 .loc 1 358 0
329 0244 01C0C2E5 strlsb r3, [r0, #8]
330 .loc 1 351 0
331 0248 0010C0E5 bls .L36
332 .loc 1 355 0
333 024c 0110C0E5 ldrb r3, [r0, #0] @ zero_extendqisi2
334 cmp r3, #80
335 0250 0210C0E5 .loc 1 356 0
336 movhi r3, #80
337 0254 0310C0E5 .loc 1 353 0
338 strb ip, [r0, #8]
339 0258 0410C0E5 .loc 1 356 0
352:sdk.c **** {
353:sdk.c **** WO_Direct_Individual_Motor_Control.motorReverseMask=0x01; //invert motor 0 if AUX switch is enabl
354:sdk.c **** //limit inverted speed (IMPORTANT! THIS IS NOT DONE AUTOMATICALLY!)
355:sdk.c **** if (WO_Direct_Individual_Motor_Control.motor[0]>80)
356:sdk.c **** WO_Direct_Individual_Motor_Control.motor[0]=80;
357:sdk.c **** }else
358:sdk.c **** WO_Direct_Individual_Motor_Control.motorReverseMask=0x00;
340 ldr r2, .L47+8
341 025c 0830C095 mov r0, #1
342 add r1, r2, #6
343 0260 0400009A mvn ip, #55
344 .L38:
345 0264 0030D0E5 .loc 1 365 0
346 0268 500053E3 ldrb r3, [r2, #0] @ zero_extendqisi2
347 cmp r3, #0
348 026c 5030A083 streqb r0, [r2, #0]
349 beq .L41
350 0270 08C0C0E5 .loc 1 366 0
351 cmp r3, #200
352 0274 0030C085 strhib ip, [r2, #0]
353 .L41:
354 add r2, r2, #1
355 0278 38209FE5 .loc 1 363 0
356 027c 0100A0E3 cmp r2, r1
357 0280 061082E2 bne .L38
358 0284 37C0E0E3 .loc 1 368 0
359 bx lr
359:sdk.c ****
360:sdk.c ****
361:sdk.c **** //make sure commands are never 0 so that motors will always keep spinning
362:sdk.c **** //also make sure that commands stay within range
363:sdk.c **** for(i=0;i<6;i++)
364:sdk.c **** {
365:sdk.c **** if(!WO_Direct_Individual_Motor_Control.motor[i]) WO_Direct_Individual_Motor_Control.motor[i]=1
360 :
361 0288 0030D2E5 .word RO_ALL_Data
362 028c 000053E3 .word -2045222521
363 0290 0000C205 .word WO_Direct_Individual_Motor_Control
364 0294 0100000A .word WO_SDK
366:sdk.c **** else if (WO_Direct_Individual_Motor_Control.motor[i]>200) WO_Direct_Individual_Motor_Control.m
365 :
367 029c 00C0C285 .align 2
368 .global SDK_EXAMPLE_direct_motor_commands_with_standard_output_mapping
370 SDK_EXAMPLE_direct_motor_commands_with_standard_output_mapping:
371 02a4 010052E1 .LFB8:
372 02a8 F6FFFF1A .loc 1 372 0
367:sdk.c **** }
368:sdk.c **** }
373 ion supports interworking.
374 02ac 1EFF2FE1 @ args = 0, pretend = 0, frame = 0
375 @ frame_needed = 0, uses_anonymous_args = 0
376 .loc 1 385 0
377 ldr r3, .L51
378 02b0 00000000 .loc 1 372 0
379 02b4 87611886 stmfd sp!, {r4, r5, r6, r7, lr}
380 02b8 00000000 .LCFI3:
381 02bc 00000000 .loc 1 387 0
382 ldrh ip, [r3, #16]
383 .loc 1 386 0
384 ldr r4, .L51+4
385 ldrh r0, [r3, #14]
386 .loc 1 385 0
387 ldrh r6, [r3, #12]
388 .loc 1 388 0
369:sdk.c ****
370:sdk.c ****
371:sdk.c **** void SDK_EXAMPLE_direct_motor_commands_with_standard_output_mapping(void)
372:sdk.c **** {
389 18]
390 .loc 1 385 0
391 mov r2, #4080
392 .loc 1 387 0
373:sdk.c **** WO_SDK.ctrl_mode=0x01; //0x00: direct individual motor control: individual commands for motors 0..
374:sdk.c **** //0x01: direct motor control using standard output mapping: commands are interpreted as pitc
375:sdk.c **** //0x02: attitude and throttle control: commands are input for standard attitude controller
376:sdk.c **** //0x03: GPS waypoint control
377:sdk.c ****
378:sdk.c **** WO_SDK.ctrl_enabled=1; //0: disable control by HL processor
379:sdk.c **** //1: enable control by HL processor
380:sdk.c ****
381:sdk.c **** /*
382:sdk.c **** * Stick commands directly mapped to motors, NO attitude control! Do NOT try to fly!
383:sdk.c **** * */
384:sdk.c ****
385:sdk.c **** WO_Direct_Motor_Control.pitch=(4095-RO_ALL_Data.channel[0])/21;
393 ll r3, r5, r4, ip
394 02c0 94309FE5 .loc 1 385 0
395 mov r3, #817889280
396 02c4 F0402DE9 .loc 1 386 0
397 umull r7, lr, r4, r0
386:sdk.c **** WO_Direct_Motor_Control.roll=RO_ALL_Data.channel[1]/21;
387:sdk.c **** WO_Direct_Motor_Control.thrust=RO_ALL_Data.channel[2]/21;
398 1 385 0
399 02c8 B0C1D3E1 add r2, r2, #15
400 add r3, r3, #199680
401 02cc 8C409FE5 .loc 1 388 0
402 02d0 BE00D3E1 rsb r1, r1, r2
403 .loc 1 385 0
404 02d4 BC60D3E1 add r3, r3, #49
388:sdk.c **** WO_Direct_Motor_Control.yaw=(4095-RO_ALL_Data.channel[3])/21;
405 , r6, r2
406 02d8 B211D3E1 .loc 1 388 0
407 smull r4, r6, r3, r1
408 02dc FF2EA0E3 .loc 1 385 0
409 smull r7, r4, r3, r2
410 02e0 943C85E0 .loc 1 386 0
411 rsb r0, lr, r0
412 02e4 C335A0E3 .loc 1 387 0
413 rsb ip, r5, ip
414 02e8 94708EE0 .loc 1 385 0
415 ldr r3, .L51+8
416 02ec 0F2082E2 .loc 1 386 0
417 02f0 C33B83E2 add lr, lr, r0, lsr #1
418 .loc 1 387 0
419 02f4 021061E0 add r5, r5, ip, lsr #1
420 .loc 1 385 0
421 02f8 313083E2 mov r2, r2, asr #31
422 02fc 022066E0 .loc 1 373 0
423 ldr ip, .L51+12
424 0300 9341C6E0 .loc 1 388 0
425 mov r1, r1, asr #31
426 0304 9372C4E0 .loc 1 373 0
427 mov r0, #1
428 0308 00006EE0 .loc 1 385 0
429 rsb r2, r2, r4, asr #2
430 030c 0CC065E0 .loc 1 388 0
431 rsb r1, r1, r6, asr #2
432 0310 4C309FE5 .loc 1 386 0
433 mov lr, lr, lsr #4
434 0314 A0E08EE0 .loc 1 387 0
435 mov r5, r5, lsr #4
436 0318 AC5085E0 .loc 1 378 0
437 strb r0, [ip, #1]
438 031c C22FA0E1 .loc 1 388 0
439 strb r1, [r3, #2]
440 0320 40C09FE5 .loc 1 385 0
441 strb r2, [r3, #0]
442 0324 C11FA0E1 .loc 1 386 0
443 strb lr, [r3, #1]
444 0328 0100A0E3 .loc 1 387 0
445 strb r5, [r3, #3]
446 032c 442162E0 .loc 1 373 0
447 strb r0, [ip, #0]
448 0330 461161E0 .loc 1 390 0
449 ldmfd sp!, {r4, r5, r6, r7, lr}
450 0334 2EE2A0E1 bx lr
451 .L52:
452 0338 2552A0E1 .align 2
453 .L51:
454 033c 0100CCE5 .word RO_ALL_Data
455 .word -2045222521
456 0340 0210C3E5 .word WO_Direct_Motor_Control
457 .word WO_SDK
458 0344 0020C3E5 .LFE8:
460 0348 01E0C3E5 .align 2
461 .global SDK_EXAMPLE_attitude_commands
463 SDK_EXAMPLE_attitude_commands:
464 0350 0000CCE5 .LFB9:
389:sdk.c ****
390:sdk.c **** }
465 , pretend = 0, frame = 0
466 0354 F040BDE8 @ frame_needed = 0, uses_anonymous_args = 0
467 0358 1EFF2FE1 @ link register save eliminated.
468 .loc 1 395 0
469 ldr r1, .L55
470 .loc 1 404 0
471 035c 00000000 ldr r0, .L55+4
472 0360 87611886 .loc 1 400 0
473 0364 00000000 mov r3, #1
474 0368 00000000 strb r3, [r1, #1]
475 .loc 1 407 0
476 mov r3, #400 @ movhi
477 .loc 1 395 0
478 mov r2, #2
479 .loc 1 407 0
480 strh r3, [r0, #6] @ movhi
481 .loc 1 404 0
391:sdk.c ****
392:sdk.c ****
393:sdk.c **** void SDK_EXAMPLE_attitude_commands(void)
394:sdk.c **** {
482 hi
483 .loc 1 394 0
484 @ lr needed for prologue
485 .loc 1 395 0
486 strb r2, [r1, #0]
395:sdk.c **** WO_SDK.ctrl_mode=0x02; //0x00: direct individual motor control: individual commands for motors 0..
487 1 404 0
488 036c 24109FE5 strh r3, [r0, #8] @ movhi
396:sdk.c **** //0x01: direct motor control using standard output mapping: commands are interpreted as pitc
397:sdk.c **** //0x02: attitude and throttle control: commands are input for standard attitude controller
398:sdk.c **** //0x03: GPS waypoint control
399:sdk.c ****
400:sdk.c **** WO_SDK.ctrl_enabled=1; //0: disable control by HL processor
401:sdk.c **** //1: enable control by HL processor
402:sdk.c ****
403:sdk.c **** //with this example the UAV will go to ~10% throttle when SDK control is activated
404:sdk.c **** WO_CTRL_Input.ctrl=0x08; //0x08: enable throttle control by HL. Height control and GPS are deactiv
489 loc 1 410 0
490 0370 24009FE5 bx lr
491 .L56:
492 0374 0130A0E3 .align 2
493 0378 0130C1E5 .L55:
405:sdk.c **** //pitch, roll and yaw are still commanded via the remote control
406:sdk.c ****
407:sdk.c **** WO_CTRL_Input.thrust=400; //10% throttle command
494 d WO_SDK
495 037c 193EA0E3 .word WO_CTRL_Input
496 .LFE9:
498 .align 2
499 0384 B630C0E1 .global SDK_EXAMPLE_turn_motors_on
501 0388 0830A0E3 SDK_EXAMPLE_turn_motors_on:
502 .LFB11:
503 .loc 1 691 0
504 @ Function supports interworking.
505 038c 0020C1E5 @ args = 0, pretend = 0, frame = 0
506 @ frame_needed = 0, uses_anonymous_args = 0
507 0390 B830C0E1 .loc 1 704 0
408:sdk.c ****
409:sdk.c ****
410:sdk.c **** }
508 ldr r3, [r0, #0]
509 0394 1EFF2FE1 .loc 1 694 0
510 ldr r2, .L64+4
511 .loc 1 704 0
512 add ip, r3, #1
513 0398 00000000 .loc 1 691 0
514 039c 00000000 stmfd sp!, {r4, lr}
515 .LCFI4:
516 .loc 1 699 0
517 mov r1, #1
518 .loc 1 702 0
519 mov lr, #0
520 .loc 1 704 0
521 cmp ip, #1000
411:sdk.c ****
412:sdk.c ****
413:sdk.c ****
414:sdk.c **** /* This function demonstrates a simple waypoint command generation. The switch on Channel 7 is used
415:sdk.c **** * to activate a 15m by 15m square. Therefore a waypoint is calculated from the current position an
416:sdk.c **** * height and is transmitted to the low level processor. The waypoint status is monitored to switch
417:sdk.c **** * the next waypoint after the current one is reached.
418:sdk.c **** *
419:sdk.c **** * wpCtrlWpCmd is used to send a command to the low level processor. Different options like waypoin
420:sdk.c **** * are available. See LL_HL_comm.h for WP_CMD_* defines
421:sdk.c **** *
422:sdk.c **** * wpCtrlWpCmdUpdated has to be set to 1 to send the command. When the cmd is sent it is set back t
423:sdk.c **** *
424:sdk.c **** * wpCtrlAckTrigger is set to 1 when the LL accepts the waypoint
425:sdk.c **** *
426:sdk.c **** * wpCtrlNavStatus gives you a navigation status. See WP_NAVSTAT_* defines in SDK.h for options
427:sdk.c **** *
428:sdk.c **** * wpCtrlDistToWp gives you the current distance to the current waypoint in dm (= 10 cm)
429:sdk.c **** */
430:sdk.c **** void SDK_EXAMPLE_gps_waypoint_control()
431:sdk.c **** {
432:sdk.c **** static unsigned char wpExampleState=0;
433:sdk.c **** static double originLat,originLon;
434:sdk.c ****
435:sdk.c ****
436:sdk.c **** WO_SDK.ctrl_mode=0x03;
437:sdk.c ****
438:sdk.c **** WO_SDK.ctrl_enabled=1; //0: disable control by HL processor
439:sdk.c **** //1: enable control by HL processor
440:sdk.c ****
441:sdk.c **** switch (wpExampleState)
442:sdk.c **** {
443:sdk.c **** //prior to start, the lever on channel 7 has to be in "OFF" position
444:sdk.c **** case 0:
445:sdk.c **** if ((RO_ALL_Data.channel[6]<1600) || (wpExampleActive))
446:sdk.c **** wpExampleState=1;
447:sdk.c **** break;
448:sdk.c ****
449:sdk.c **** case 1:
450:sdk.c **** if ((RO_ALL_Data.channel[6]>2400) || (wpExampleActive))
451:sdk.c **** {
452:sdk.c **** double lat,lon;
453:sdk.c **** //lever was set to "ON" state -> calculate and send first waypoint and switch state
454:sdk.c ****
455:sdk.c **** //fill waypoint structure
456:sdk.c **** wpToLL.max_speed=100;
457:sdk.c **** wpToLL.pos_acc=3000; //3m accuracy
458:sdk.c **** wpToLL.time=400; //4 seconds waiting time at each waypoint
459:sdk.c **** wpToLL.wp_activated=1;
460:sdk.c ****
461:sdk.c **** //see LL_HL_comm.h for WPPROP defines
462:sdk.c **** wpToLL.properties=WPPROP_ABSCOORDS|WPPROP_AUTOMATICGOTO|WPPROP_HEIGHTENABLED|WPPROP_YAWENABLED;
463:sdk.c ****
464:sdk.c **** //use current height and yaw
465:sdk.c **** wpToLL.yaw=RO_ALL_Data.angle_yaw; //use current yaw
466:sdk.c **** wpToLL.height=RO_ALL_Data.fusion_height; //use current height
467:sdk.c ****
468:sdk.c **** originLat=(double)GPS_Data.latitude/10000000.0;
469:sdk.c **** originLon=(double)GPS_Data.longitude/10000000.0;
470:sdk.c ****
471:sdk.c **** //calculate a position 15m north of us
472:sdk.c **** xy2latlon(originLat,originLon,0.0,15.0,&lat,&lon);
473:sdk.c ****
474:sdk.c **** wpToLL.X=lon*10000000;
475:sdk.c **** wpToLL.Y=lat*10000000;
476:sdk.c ****
477:sdk.c **** //calc chksum
478:sdk.c **** wpToLL.chksum = 0xAAAA
479:sdk.c **** + wpToLL.yaw
480:sdk.c **** + wpToLL.height
481:sdk.c **** + wpToLL.time
482:sdk.c **** + wpToLL.X
483:sdk.c **** + wpToLL.Y
484:sdk.c **** + wpToLL.max_speed
485:sdk.c **** + wpToLL.pos_acc
486:sdk.c **** + wpToLL.properties
487:sdk.c **** + wpToLL.wp_activated;
488:sdk.c ****
489:sdk.c **** //send waypoint
490:sdk.c **** wpCtrlAckTrigger=0;
491:sdk.c **** wpCtrlWpCmd=WP_CMD_SINGLE_WP;
492:sdk.c **** wpCtrlWpCmdUpdated=1;
493:sdk.c **** wpExampleWpNr=0;
494:sdk.c **** wpExampleState=2;
495:sdk.c ****
496:sdk.c **** }
497:sdk.c **** break;
498:sdk.c ****