-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathmap.c
2150 lines (1899 loc) · 70.6 KB
/
map.c
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
/*
* c't-Bot
*
* This program is free software; you can redistribute it
* and/or modify it under the terms of the GNU General
* Public License as published by the Free Software
* Foundation; either version 2 of the License, or (at your
* option) any later version.
* This program is distributed in the hope that it will be
* useful, but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
* PURPOSE. See the GNU General Public License for more details.
* You should have received a copy of the GNU General Public
* License along with this program; if not, write to the Free
* Software Foundation, Inc., 59 Temple Place, Suite 330, Boston,
* MA 02111-1307, USA.
*
*/
/**
* \file map.c
* \brief Karte
* \author Benjamin Benz ([email protected])
* \date 19.09.2006
*/
#include "ct-Bot.h"
#ifdef MAP_AVAILABLE
#include "ui/available_screens.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <ctype.h>
#include "sensor_correction.h"
#include "map.h"
#include "mmc.h"
#include "rc5-codes.h"
#include "sensor.h"
#include "display.h"
#include "log.h"
#include "timer.h"
#include "fifo.h"
#include "os_thread.h"
#include "math_utils.h"
#include "command.h"
#include "motor.h"
#include "init.h"
#include "uart.h"
#if !defined MMC_AVAILABLE && defined MCU
#error "Map geht auf dem MCU nicht ohne MMC"
#endif
#ifndef SDFAT_AVAILABLE
#error "Map braucht SDFAT_AVAILABLE"
#endif
#ifndef OS_AVAILABLE
#error "Map braucht OS_AVAILABLE"
#endif
//#define DEBUG_MAP // Schalter um recht viel Debug-Code anzumachen
//#define DEBUG_MAP_TIMES // Schalter um Performance-Messungen fuer MMC anzumachen
//#define DEBUG_STORAGE // Noch mehr Ausgaben zum Thema Organisation der Kartenstruktur, Macroblocks, Sections
//#define DEBUG_SCAN_OTF // Debug-Infos des Update-Threads an
//#define DEBUG_GET_RATIO // zeichnet Debug-Infos in die Map-Anzeige des Sim, gruen: Bereich komplett innerhalb des gewuenschten Intervalls, rot: Bereich nicht (komplett) innerhalb des gewuenschten Intervalls
//#define DEBUG_GET_RATIO_VERBOSE // zeichnet detaillierte Infos in die Map-Anzeige, gruen: Felder innerhalb des gewuenschten Intervalls, rot: Felder ausserhalb des gewuenschten Intervalls
//#define DEBUG_MAP_GET_AVERAGE // zeichnet Debug-Infos in die Map-Anzeige des Sim fuer map_get_average(), gruen: Durchschnitt des Feldes >= MAP_OBSTACLE_THRESHOLD, rot: sonst
//#define DEBUG_MAP_GET_AVERAGE_VERBOSE // zeichnet belegte Map-Felder, die map_get_average() auswertet rot und freie gruen
//#define MAP_TESTS_AVAILABLE // Schalter um Test-Code zu aktivieren
#define MAP_INFO_AVAILABLE // Schalter um Info-Code zu aktivieren
#ifdef MCU
// Soll auch der echte Bot Infos ausgeben, kommentiert man die folgende Zeile aus
#undef MAP_INFO_AVAILABLE // spart Flash
#endif
#ifndef MAP_2_SIM_AVAILABLE
#undef DEBUG_GET_RATIO
#undef DEBUG_GET_RATIO_VERBOSE
#endif
#ifndef LOG_AVAILABLE
#undef DEBUG_MAP
#undef DEBUG_STORAGE
#endif
#ifndef DEBUG_MAP
#undef DEBUG_STORAGE
#undef LOG_DEBUG
#define LOG_DEBUG(a, ...) {}
#endif
/*
* Eine Karte ist wie folgt organisiert:
* Es gibt Sektionen zu je MAP_SECTION_POINTS * MAP_SECTION_POINTS.
* Diese Sektionen enthalten direkt die Pixel-Daten
* Auf dem PC liegen die Daten genauso im RAM, wie sie auf der MMC
* angeordnet sind.
*
* Auf dem MCU passt eine Sektion in einen Flashblock, bzw immer 2
* Sektionen in einen Block Es stehen immer 2 Sections also 1
* Flash-Block im SRAM und werden bei Bedarf gewechselt.
*
* Felder sind vom Typ int8_t und haben einen Wertebereich von -128
* bis 127.
* 0 bedeutet: wir wissen nichts ueber das Feld
* negative Werte bedeuten: Hindernis
* positive Werte bedeuten: freier Weg
* Je groesser der Betrag ist, desto sicherer die Aussage ueber das
* Feld.
* Der Wert -128 ist Loechern vorbehalten und wird dann auch nicht
* durch die Abstandssensoren veraendert.
*
* Felder werden wie folgt aktualisiert:
* Wird ein Punkt als frei betrachtet, erhoehen wir den Wert des
* Feldes um MAP_STEP_FREE.
* Wird ein Punkt als belegt erkannt, ziehen wir um ihn einen
* Streukreis mit dem Radius MAP_RADIUS.
* Das Zentrum des Kreises wird um MAP_STEP_OCCUPIED dekrementiert,
* nach aussen hin immer weniger.
* Wird ein Feld als Loch erkannt, setzen wir den Wert fest auf -128.
*/
/** Anzahl der Sections in der Map */
#define MAP_SECTIONS (MAP_SIZE_MM * MAP_RESOLUTION / 1000 / MAP_SECTION_POINTS)
#define MAP_STEP_FREE_SENSOR 2 /**< Um diesen Wert wird ein Feld inkrementiert, wenn es vom Sensor als frei erkannt wird */
#define MAP_STEP_FREE_LOCATION 20 /**< Um diesen Wert wird ein Feld inkrementiert, wenn der Bot drueber faehrt */
#define MAP_STEP_OCCUPIED 5 /**< Um diesen Wert wird ein Feld dekrementiert, wenn es als belegt erkannt wird */
#define MAP_RADIUS 50 /**< Umkreis eines Messpunktes, der als besetzt aktualisiert wird (Streukreis) [mm] */
/** Umkreis einen Messpunkt, der als besetzt aktualisiert wird (Streukreis) [Felder] */
#define MAP_RADIUS_FIELDS (MAP_RESOLUTION * MAP_RADIUS / 1000)
#define MAP_PRINT_SCALE /**< Soll das PGM eine Skala erhalten? */
#define MAP_SCALE (MAP_RESOLUTION / 2) /**< Alle wieviel Punkte kommt ein Skalen-Strich */
#define MACRO_BLOCK_LENGTH 512U /**< Kantenlaenge eines Makroblocks in Punkten/Byte */
#define MAP_LENGTH_IN_MACRO_BLOCKS ((uint8_t) (MAP_SIZE_MM * MAP_RESOLUTION / 1000 / MACRO_BLOCK_LENGTH)) /**< Kantenlaenge der Karte in Makrobloecken */
#define MAP_FILE_SIZE ((uint16_t) ((uint32_t) ((uint32_t) MAP_SECTION_POINTS * MAP_SECTION_POINTS) * MAP_SECTIONS * MAP_SECTIONS \
/ MAP_BLOCK_SIZE)) /**< Dateigroesse der Map in Bloecken */
#define MAP_ALIGNMENT_MASK (2UL * MACRO_BLOCK_LENGTH * MACRO_BLOCK_LENGTH / MAP_BLOCK_SIZE - 1) /**< fuer die Ausrichtung der Karte an einer Sektorgrenze zu Optimierungszwecken */
#define MAP_FILENAME "ctbot.map" /**< Dateiname der Karte */
#define MAP_FILE_ALIGNMENT (512UL * 1024UL / MAP_BLOCK_SIZE) /**< Alingment der Map-Datei (512 KB) */
int16_t map_min_x = MAP_SIZE * MAP_RESOLUTION / 2; /**< belegter Bereich der Karte [Kartenindex]: kleinste X-Koordinate */
int16_t map_max_x = MAP_SIZE * MAP_RESOLUTION / 2; /**< belegter Bereich der Karte [Kartenindex]: groesste X-Koordinate */
int16_t map_min_y = MAP_SIZE * MAP_RESOLUTION / 2; /**< belegter Bereich der Karte [Kartenindex]: kleinste Y-Koordinate */
int16_t map_max_y = MAP_SIZE * MAP_RESOLUTION / 2; /**< belegter Bereich der Karte [Kartenindex]: groesste Y-Koordinate */
static uint8_t min_max_updated = False; /**< wurden die Min- / Max-Werte veraendert? */
uint16_t alignment_offset = 0;
/** Datentyp fuer die Elementarfelder einer Gruppe */
typedef struct {
int8_t section[MAP_SECTION_POINTS][MAP_SECTION_POINTS]; /**< Einzelne Punkte */
} PACKED_FORCE map_section_t;
static pFatFile map_file_desc; /**< Datei-Deskriptor der Map */
static uint8_t map_update_fifo_buffer[MAP_UPDATE_CACHE_SIZE]; /**< Puffer fuer Map-Cache-Indizes / FiFo */
fifo_t map_update_fifo; /**< Fifo fuer Map-Cache */
map_cache_t map_update_cache[MAP_UPDATE_CACHE_SIZE]; /**< Map-Cache */
uint8_t map_update_stack[MAP_UPDATE_STACK_SIZE]; /**< Stack des Update-Threads */
static Tcb_t* map_update_thread; /**< Thread fuer Map-Update */
static os_signal_t lock_signal = OS_SIGNAL_INITIALIZER; /**< Signal zur Synchronisation von Kartenzugriffen */
void map_update_main(void) OS_TASK_ATTR;
#define map_buffer GET_MMC_BUFFER(map_buffer) /**< Map-Puffer */
static map_section_t* map[2]; /**< Array mit den Zeigern auf die Elemente, es passen immer 2 Sektionen in den Puffer */
static struct {
uint16_t block; /**< Block, der aktuell im Puffer steht. Nur bis 32 MByte adressierbar */
uint8_t updated; /**< markiert, ob der aktuelle Block gegenueber der MMC-Karte veraendert wurde */
int16_t x; /**< X-Koordinate des Blocks */
int16_t y; /**< Y-Koordinate des Blocks */
} map_current_block = { 0, False, 0, 0 }; /**< Daten des aktuellen Blocks */
static uint8_t init_state = 0; /**< Status der Initialisierung (0 (nicht initialisiert), 1 (alles OK)) */
#ifdef MAP_2_SIM_AVAILABLE
void map_2_sim_main(void) OS_TASK_ATTR;
static fifo_t map_2_sim_fifo; /**< Fifo fuer Map-2-Sim-Daten */
static uint16_t map_2_sim_cache[MAP_2_SIM_BUFFER_SIZE]; /**< Speicher fuer Map-2-Sim-Daten (Adressen der geaenderten Bloecke) */
static struct {
position_t pos; /**< letzte Bot-Position */
int16_t heading; /**< letzte Bot-Ausrichtung */
#ifdef MEASURE_POSITION_ERRORS_AVAILABLE
int16_t error; /**< letzter Fehlerradius */
#endif
} map_2_sim_data = {
{MAP_SIZE * MAP_RESOLUTION / 2, MAP_SIZE * MAP_RESOLUTION / 2}, 0
#ifdef MEASURE_POSITION_ERRORS_AVAILABLE
, 0
#endif
};
static Tcb_t* map_2_sim_worker; /**< Worker-Thread fuer die Map-2-Sim-Anzeige */
uint8_t map_2_sim_worker_stack[MAP_2_SIM_STACK_SIZE]; /**< Stack des Map-2-Sim-Threads */
static pFatFile map_2_sim_file_desc; /**< File-Deskriptor fuer Map-2-Sim */
static uint8_t map_2_sim_buffer[512]; /**< Puffer fuer zur Map-2-Sim-Kommunikation */
static os_signal_t map_2_sim_signal = OS_SIGNAL_INITIALIZER; /**< Signal, um gleichzeitiges Senden von Map-Daten zu verhindern */
#endif // MAP_2_SIM_AVAILABLE
#ifdef PC
char* map_file = "sim.map"; /**< Dateiname fuer Ex- / Import */
#endif // PC
static uint16_t get_block(int16_t x, int16_t y) {
/* Sicherheitscheck */
if (((uint16_t) x >= (uint16_t) (MAP_SIZE * MAP_RESOLUTION)) || ((uint16_t) y >= (uint16_t) (MAP_SIZE * MAP_RESOLUTION))) {
LOG_ERROR("Zugriff ausserhalb der Karte: x=%u y=%u", x, y);
return 0xffff;
}
/* Berechne den gesuchten Block */
uint16_t local_x = (uint16_t) x % MACRO_BLOCK_LENGTH;
// wenn MACRO_BLOCK_LENGTH eine 2er Potenz ist, optimiert der Compiler hier
uint16_t local_y = (uint16_t) y % MACRO_BLOCK_LENGTH;
#ifdef DEBUG_STORAGE
LOG_DEBUG("local_x=%d, local_y=%d", local_x, local_y);
#endif
uint16_t block = local_x / MAP_SECTION_POINTS + (local_y / MAP_SECTION_POINTS) * (MACRO_BLOCK_LENGTH / MAP_SECTION_POINTS);
block = block >> 1; // es passen immer 2 Sections in einen Block
/* Makroblock berechnen */
uint16_t macroblock = (uint16_t) x / MACRO_BLOCK_LENGTH + ((uint16_t) y / MACRO_BLOCK_LENGTH) * MAP_LENGTH_IN_MACRO_BLOCKS;
#ifdef DEBUG_STORAGE
LOG_DEBUG("Macroblock=%u", macroblock);
#endif
block += (uint16_t) (macroblock * MACRO_BLOCK_LENGTH * (MACRO_BLOCK_LENGTH / MAP_BLOCK_SIZE)); // noch in den richtigen Makroblock springen
#ifdef DEBUG_STORAGE
LOG_DEBUG("block=%u", block);
#endif
return block;
}
/**
* Initialisiert die Karte
* \param clean_map True: Karte wird geloescht, False: Karte bleibt erhalten
* \return 0 wenn alles ok
*/
static int8_t init(uint8_t clean_map) {
if (init_state == 1) {
return 0;
}
#ifdef BEHAVIOUR_SCAN_AVAILABLE
deactivateBehaviour(bot_scan_onthefly_behaviour);
#endif
display_cursor(1, 1);
display_printf("Bitte warten...");
display_cursor(2, 1);
display_printf("MAP wird");
display_cursor(3, 1);
display_printf("initialisiert.");
// Die Karte auf den Puffer biegen
map[0] = (map_section_t*) map_buffer;
map[1] = (map_section_t*) (map_buffer + sizeof(map_section_t));
map_current_block.updated = 0xff; // Die MMC-Karte ist erstmal nicht verfuegbar
LOG_DEBUG("map::init(): sdfat_open(\"%s\")...", MAP_FILENAME);
const uint8_t mode = SDFAT_O_RDWR | SDFAT_O_CREAT;
uint8_t res = sdfat_open(MAP_FILENAME, &map_file_desc, mode);
LOG_DEBUG("map::init(): sdfat_open()=%d", res);
if (res != 0) {
LOG_DEBUG("map::init(): Dateioeffnen lieferte %d", res);
LOG_ERROR("map::init(): Fehler beim Oeffnen / Anlegen der Datei");
return 2;
}
const uint32_t data_offset = sdfat_get_first_block(map_file_desc) + (sizeof(map_header_t) / MAP_BLOCK_SIZE); // Blockadresse in Bloecken
const uint32_t alignment_mask = (512UL * 1024UL) / MAP_BLOCK_SIZE - 1; // in Bloecken
alignment_offset = (uint16_t) (((data_offset + alignment_mask) & ~alignment_mask) - data_offset); // in Bloecken
const uint32_t map_offset = (data_offset + alignment_offset) * MAP_BLOCK_SIZE; // in Byte
(void) map_offset;
LOG_INFO("map::init(): data_offset=0x%x%04x blocks", (uint16_t) (data_offset >> 16), (uint16_t) data_offset);
LOG_INFO("map::init(): alignment_mask=0x%x%04x blocks", (uint16_t) (alignment_mask >> 16), (uint16_t) alignment_mask);
LOG_INFO("map::init(): alignment_offset=0x%04x blocks", alignment_offset);
LOG_INFO("map::init(): map_offset=0x%x%04x byte", (uint16_t) (map_offset >> 16), (uint16_t) map_offset);
map_header_t* p_head_data = (map_header_t*) map_buffer;
/* Min- / Max-Werte initialisieren */
p_head_data->map_min_x = 0;
p_head_data->map_max_x = MAP_SIZE * MAP_RESOLUTION - 1;
p_head_data->map_min_y = 0;
p_head_data->map_max_y = MAP_SIZE * MAP_RESOLUTION - 1;
p_head_data->alignment_offset = alignment_offset;
if (sdfat_get_filesize(map_file_desc) < ((uint32_t) (MAP_FILE_SIZE + alignment_offset) * MAP_BLOCK_SIZE + sizeof(map_header_t))) {
LOG_DEBUG("map::init(): Datei zu klein, neu initialisieren");
clean_map = True;
} else {
LOG_DEBUG("map::init(): Dateigroesse passt, lese Header ein");
sdfat_rewind(map_file_desc);
if (sdfat_read(map_file_desc, p_head_data, sizeof(map_header_t)) != sizeof(map_header_t)) {
LOG_DEBUG("map::init(): sdfat_read(head) failed");
return 5;
}
}
if (clean_map) {
LOG_DEBUG("map::init(): Map wird geloescht/neu initialisiert");
map_min_x = p_head_data->map_min_x;
map_max_x = p_head_data->map_max_x;
map_min_y = p_head_data->map_min_y;
map_max_y = p_head_data->map_max_y;
LOG_DEBUG("map::init(): map_min_x=%d map_min_y=%d", map_min_x, map_min_y);
LOG_DEBUG("map::init(): map_max_x=%d map_max_y=%d", map_max_x, map_max_y);
LOG_DEBUG("map::init(): alignment_offset=0x%04x blocks", p_head_data->alignment_offset);
/* Min/Max Werte auf default setzen */
p_head_data->map_min_x = MAP_SIZE * MAP_RESOLUTION / 2;
p_head_data->map_max_x = MAP_SIZE * MAP_RESOLUTION / 2;
p_head_data->map_min_y = MAP_SIZE * MAP_RESOLUTION / 2;
p_head_data->map_max_y = MAP_SIZE * MAP_RESOLUTION / 2;
p_head_data->alignment_offset = alignment_offset;
sdfat_rewind(map_file_desc);
if (sdfat_write(map_file_desc, map_buffer, sizeof(map_header_t)) != sizeof(map_header_t)) {
LOG_DEBUG("map::init(): sdfat_write(head) failed");
return 6;
}
memset(map_buffer, 0, MAP_BLOCK_SIZE);
const uint16_t min_block = sdfat_get_filesize(map_file_desc) / MAP_BLOCK_SIZE < alignment_offset ? 0 : get_block(map_min_x, map_min_y) + alignment_offset;
const uint16_t max_block = get_block(map_max_x, map_max_y) + alignment_offset;
LOG_DEBUG("map::init(): min_block=%u max_block=%u", min_block, max_block);
if (sdfat_seek(map_file_desc, (int32_t) (min_block * MAP_BLOCK_SIZE) + sizeof(map_header_t), SEEK_SET)) {
LOG_DEBUG("map::init(): sdfat_seek(0x%ld) failed", min_block * MAP_BLOCK_SIZE);
return 7;
}
uint16_t i;
for (i = min_block; i <= max_block; ++i) {
if (sdfat_write(map_file_desc, map_buffer, MAP_BLOCK_SIZE) != MAP_BLOCK_SIZE) {
LOG_DEBUG("map::init(): sdfat_write(0x%x) failed", i);
return 8;
}
}
p_head_data->map_min_x = MAP_SIZE * MAP_RESOLUTION / 2;
p_head_data->map_max_x = MAP_SIZE * MAP_RESOLUTION / 2;
p_head_data->map_min_y = MAP_SIZE * MAP_RESOLUTION / 2;
p_head_data->map_max_y = MAP_SIZE * MAP_RESOLUTION / 2;
p_head_data->alignment_offset = alignment_offset;
}
/* Min- / Max-Werte laden */
map_min_x = p_head_data->map_min_x;
map_max_x = p_head_data->map_max_x;
map_min_y = p_head_data->map_min_y;
map_max_y = p_head_data->map_max_y;
#ifdef DEBUG_STORAGE
LOG_DEBUG("map::init(): map_min_x=0x%x, map_max_x=0x%x, map_min_y=0x%x, map_max_y=0x%x\n", map_min_x, map_max_x, map_min_y, map_max_y);
#endif
#ifdef MAP_2_SIM_AVAILABLE
if (sdfat_open(MAP_FILENAME, &map_2_sim_file_desc, SDFAT_O_READ)) {
LOG_DEBUG("map::init(): Mapdatei konnte nicht fuer Map-2-Sim geoeffnet werden");
return 9;
}
#endif // MAP_2_SIM_AVAILABLE
map_current_block.updated = False;
map_current_block.block = 0;
if (clean_map) {
memset(map_buffer, 0, sizeof(map_buffer));
} else {
/* Block 0 laden */
if (sdfat_seek(map_file_desc, (int32_t) ((map_current_block.block + alignment_offset) * MAP_BLOCK_SIZE) + sizeof(map_header_t), SEEK_SET)) {
LOG_DEBUG("map::init(): sdfat_seek(0x%x) failed", map_current_block.block + alignment_offset);
return 10;
}
if (sdfat_read(map_file_desc, map_buffer, MAP_BLOCK_SIZE) != MAP_BLOCK_SIZE) {
LOG_DEBUG("map::init(): sdfat_read(0x%x) failed", map_current_block.block + alignment_offset);
return 11;
}
}
/* Thread-Setup */
if (init_state == 0) {
/* Update-Thread initialisieren */
#ifdef OS_DEBUG
os_mask_stack(map_update_stack, MAP_UPDATE_STACK_SIZE);
#ifdef MAP_2_SIM_AVAILABLE
os_mask_stack(map_2_sim_worker_stack, MAP_2_SIM_STACK_SIZE);
#endif
#endif // OS_DEBUG
fifo_init(&map_update_fifo, map_update_fifo_buffer, (uint8_t) sizeof(map_update_fifo_buffer));
#ifdef MAP_2_SIM_AVAILABLE
fifo_init(&map_2_sim_fifo, map_2_sim_cache, sizeof(map_2_sim_cache));
#endif
map_update_thread = os_create_thread(&map_update_stack[MAP_UPDATE_STACK_SIZE - 1], map_update_main);
#ifdef MAP_2_SIM_AVAILABLE
map_2_sim_worker = os_create_thread(&map_2_sim_worker_stack[MAP_2_SIM_STACK_SIZE - 1], map_2_sim_main);
#endif // MAP_2_SIM_AVAILABLE
}
init_state = 1;
#ifdef BEHAVIOUR_SCAN_AVAILABLE
activateBehaviour(NULL, bot_scan_onthefly_behaviour);
#endif
return 0;
}
/**
* Initialisiert die Karte
* \return 0 wenn alles ok ist
*/
int8_t map_init(void) {
#ifdef MAP_CLEAR_ON_INIT
return init(True);
#else
return init(False);
#endif
}
/**
* Haelt den Bot an und schreibt den Map-Update-Cache komplett zurueck
*/
void map_flush_cache(void) {
#ifdef MCU
if (map_locked() == 1) {
motor_set(BOT_SPEED_STOP, BOT_SPEED_STOP);
}
#endif // MCU
/* Warten, bis Update fertig */
os_signal_set(&lock_signal);
/* Sperre sofort wieder freigeben */
os_signal_release(&lock_signal);
sdfat_rewind(map_file_desc);
if (sdfat_read(map_file_desc, map_buffer, sizeof(map_header_t)) != sizeof(map_header_t)) {
LOG_ERROR("map_flush_cache(): sdfat_read(head) failed");
return;
}
map_header_t* p_head_data = (map_header_t*) map_buffer;
p_head_data->map_min_x = map_min_x;
p_head_data->map_max_x = map_max_x;
p_head_data->map_min_y = map_min_y;
p_head_data->map_max_y = map_max_y;
sdfat_rewind(map_file_desc);
if (sdfat_write(map_file_desc, p_head_data, sizeof(map_header_t)) != sizeof(map_header_t)) {
LOG_ERROR("map_flush_cache(): sdfat_write(head) failed");
}
if (sdfat_seek(map_file_desc, (int32_t) ((map_current_block.block + alignment_offset) * MAP_BLOCK_SIZE) + sizeof(map_header_t), SEEK_SET)) {
LOG_ERROR("map_flush_cache(): sdfat_seek(0x%x) failed", map_current_block.block + alignment_offset);
return;
}
if (sdfat_read(map_file_desc, map_buffer, MAP_BLOCK_SIZE) != MAP_BLOCK_SIZE) {
LOG_ERROR("map_flush_cache(): sdfat_read(0x%x) failed", map_current_block.block + alignment_offset);
}
sdfat_flush(map_file_desc);
}
/**
* Konvertiert eine Weltkoordinate in eine Kartenkoordinate
* \param koord Weltkoordiante
* \return Kartenkoordinate
*/
int16_t world_to_map(int16_t koord) {
#if (1000 / MAP_RESOLUTION) * MAP_RESOLUTION != 1000
#error "MAP_RESOLUTION ist kein Teiler von 1000, Code in world_to_map() anpassen!"
#endif
#if defined MCU && MAP_RESOLUTION == 125
__asm__ __volatile__(
"lsr %B0 \n\t"
"ror %A0 \n\t"
"lsr %B0 \n\t"
"ror %A0 \n\t"
"lsr %B0 \n\t"
"ror %A0 \n\t"
"sbrc %B0,4 \n\t"
"ori %B0,224 \n\t"
"subi %A0,lo8(-(768)) \n\t"
"sbci %B0,hi8(-(768)) "
: "+d" (koord)
);
return koord;
#else
int32_t tmp = koord + (int16_t) (MAP_SIZE * MAP_RESOLUTION * 4);
return (int16_t) (tmp / (1000 / MAP_RESOLUTION));
#endif
}
/**
* Liefert einen Zeiger auf die Section zurueck, in der der Punkt liegt.
* Auf dem MCU kuemmert sie sich darum, die entsprechende Karte aus der MMC-Karte zu laden
* \param x X-Ordinate der Karte (nicht der Welt!!!)
* \param y Y-Ordinate der Karte (nicht der Welt!!!)
* \return Zeiger auf die Section
*/
static map_section_t* get_section(int16_t x, int16_t y) {
const uint16_t block = get_block(x, y);
if (block == 0xffff) {
return NULL;
}
// Da immer 2 Sections in einem Block stehen: richtige der beiden Sections raussuchen
const uint8_t index = (uint8_t) ((x / MAP_SECTION_POINTS) & 0x1);
/* Ist der Block schon geladen? */
if (map_current_block.block == block) {
#ifdef DEBUG_STORAGE
LOG_DEBUG("ist noch im Puffer");
#endif
return map[index];
}
/* Block ist also nicht im Puffer */
#ifdef DEBUG_STORAGE
LOG_DEBUG("ist nicht im Puffer");
#endif
/* Wurde der Block im RAM veraendert? */
if (map_current_block.updated == True) {
/* Shrinking */
if (map_current_block.x < map_min_x) {
map_min_x = map_current_block.x;
min_max_updated = True;
} else if (map_current_block.x > map_max_x) {
map_max_x = map_current_block.x + ((MAP_SECTION_POINTS * 2) - 1);
min_max_updated = True;
}
if (map_current_block.y < map_min_y) {
map_min_y = map_current_block.y;
min_max_updated = True;
} else if (map_current_block.y > map_max_y) {
map_max_y = map_current_block.y + (MAP_SECTION_POINTS - 1);
min_max_updated = True;
}
/* Dann erstmal sichern */
#ifdef DEBUG_MAP_TIMES
LOG_INFO("writing block 0%x", map_current_block.block + alignment_offset);
uint16_t start_ticks = TIMER_GET_TICKCOUNT_16;
#endif
if (sdfat_seek(map_file_desc, (int32_t) ((map_current_block.block + alignment_offset) * MAP_BLOCK_SIZE) + sizeof(map_header_t), SEEK_SET)) {
LOG_DEBUG("map::get_section(): sdfat_seek(0x%x) failed", map_current_block.block + alignment_offset);
map_current_block.updated = False;
return NULL;
}
if (sdfat_write(map_file_desc, map_buffer, MAP_BLOCK_SIZE) != MAP_BLOCK_SIZE) {
LOG_DEBUG("map::get_section(): sdfat_write(0x%x) failed", map_current_block.block + alignment_offset);
map_current_block.updated = False;
return NULL;
}
#ifdef MAP_2_SIM_AVAILABLE
map_2_sim_data.pos.x = world_to_map(x_pos);
map_2_sim_data.pos.y = world_to_map(y_pos);
map_2_sim_data.heading = heading_int;
#ifdef MEASURE_POSITION_ERRORS_AVAILABLE
map_2_sim_data.error = pos_error_radius / (1000 / MAP_RESOLUTION) + (BOT_DIAMETER / 2 / (1000 / MAP_RESOLUTION));
#endif
fifo_put_data(&map_2_sim_fifo, &map_current_block.block, sizeof(map_current_block.block), True);
#endif // MAP_2_SIM_AVAILABLE
#ifdef DEBUG_MAP_TIMES
uint16_t end_ticks = TIMER_GET_TICKCOUNT_16;
LOG_INFO("swapout took %u ms", (end_ticks - start_ticks) * 176 / 1000);
(void) start_ticks;
(void) end_ticks;
#endif
}
/* Statusvariablen anpassen */
map_current_block.block = block;
map_current_block.x = x & ~((MAP_SECTION_POINTS * 2) - 1); // 32 Einheiten in X-Richtung und
map_current_block.y = y & ~(MAP_SECTION_POINTS - 1); // 16 Einheiten in Y-Richtung pro Block
map_current_block.updated = False;
/* Lade den neuen Block */
#ifdef DEBUG_MAP_TIMES
LOG_INFO("reading block 0x%x", block + alignment_offset);
uint16_t start_ticks = TIMER_GET_TICKCOUNT_16;
#endif
if (sdfat_seek(map_file_desc, (int32_t) ((block + alignment_offset) * MAP_BLOCK_SIZE) + sizeof(map_header_t), SEEK_SET)) {
LOG_DEBUG("map::get_section(): sdfat_seek(0x%x) failed", block + alignment_offset);
return NULL;
}
if (sdfat_read(map_file_desc, map_buffer, MAP_BLOCK_SIZE) != MAP_BLOCK_SIZE) {
LOG_DEBUG("map::get_section(): sdfat_read(0x%x) failed", block + alignment_offset);
return NULL;
}
#ifdef DEBUG_MAP_TIMES
uint16_t end_ticks = TIMER_GET_TICKCOUNT_16;
LOG_INFO("swapin took %u ms", (end_ticks - start_ticks) * 176 / 1000);
(void) start_ticks;
(void) end_ticks;
#endif
return map[index];
}
/**
* Prueft, ob die Karte zurzeit gesperrt ist.
* \return 1, falls Karte gesperrt, 0 sonst
*/
uint8_t map_locked(void) {
return lock_signal.value;
}
/**
* Zugriff auf ein Feld der Karte. Kann lesend oder schreibend sein.
* \param x X-Ordinate der Karte
* \param y Y-Ordinate der Karte
* \param value Neuer Wert des Feldes (> 0 heisst frei, <0 heisst belegt)
* \param set 0 zum Lesen, 1 zum Schreiben
* \return Wert, der jetzt an (x|y) steht
*/
static int8_t access_field(int16_t x, int16_t y, int8_t value, uint8_t set) {
// Suche die Section heraus
map_section_t* p_section = get_section(x, y);
if (! p_section) {
LOG_DEBUG("map::access_field(%d,%d,%d,%u): get_section failed", x, y, value, set);
return 0;
}
// Berechne den Index innerhalb der Section
const uint16_t index_x = (uint16_t) x % MAP_SECTION_POINTS;
const uint16_t index_y = (uint16_t) y % MAP_SECTION_POINTS;
int8_t* data = &p_section->section[index_x][index_y];
if (set) {
*data = value;
map_current_block.updated = True;
}
return *data;
}
/**
* liefert den Durschnittswert um einen Punkt der Karte herum
* \param x x-Ordinate der Karte
* \param y y-Ordinate der Karte
* \param radius gewuenschter Radius
* \return Wert des Durchschnitts um das Feld (>0 heisst frei, <0 heisst belegt)
*/
static int8_t get_average_fields(int16_t x, int16_t y, int8_t radius) {
int32_t avg = 0;
int8_t dX, dY;
int16_t count = 0;
const int16_t h = muls8(radius, radius);
/* Daten auslesen */
for (dX = (int8_t) -radius; dX <= radius; dX++) {
for (dY = (int8_t) -radius; dY <= radius; dY++) {
if (muls8(dX, dX) + muls8(dY, dY) <= h) {
int8_t tmp = access_field(x + dX, y + dY, 0, 0);
avg += tmp;
count++;
#ifdef DEBUG_MAP_GET_AVERAGE_VERBOSE
uint8_t color = tmp < MAP_OBSTACLE_THRESHOLD ? 1 : 0;
position_t pos;
pos.x = x + dX;
pos.y = y + dY;
map_draw_line(pos, pos, color);
#endif // DEBUG_MAP_GET_AVERAGE
}
}
}
int8_t result = (int8_t)(count > 0 ? avg / count : 0);
#if defined DEBUG_MAP_GET_AVERAGE && !defined DEBUG_MAP_GET_AVERAGE_VERBOSE
uint8_t color = result < MAP_OBSTACLE_THRESHOLD ? 1 : 0;
for (dX =- radius; dX <= radius; dX++) {
for (dY =- radius; dY <= radius; dY++) {
if (muls8(dX, dX) + muls8(dY, dY) <= h) {
position_t pos;
pos.x = x + dX;
pos.y = y + dY;
map_draw_line(pos, pos, color);
}
}
}
#endif // DEBUG_MAP_GET_AVERAGE
return result;
}
/**
* liefert den Durschnittswert um einen Ort herum
* \param x x-Ordinate der Welt
* \param y y-Ordinate der Welt
* \param radius Radius der Umgebung, die beruecksichtigt wird [mm]; 0 fuer ein Map-Feld (Punkt)
* \return Durchschnitsswert im Umkreis um den Ort (>0 heisst frei, <0 heisst belegt)
*/
int8_t map_get_average(int16_t x, int16_t y, int16_t radius) {
// Ort in Kartenkoordinaten
int16_t X = world_to_map(x);
int16_t Y = world_to_map(y);
int8_t R = (int8_t) (radius / (1000 / MAP_RESOLUTION));
/* warten bis Karte frei ist */
map_flush_cache();
int8_t result = get_average_fields(X, Y, R);
return result;
}
/**
* Aendert den Wert eines Feldes um den angegebenen Betrag
* \param x x-Ordinate der Karte (nicht der Welt!!!)
* \param y y-Ordinate der Karte (nicht der Welt!!!)
* \param value Betrag um den das Feld veraendert wird (>0 heisst freier, <0 heisst belegter)
*/
static void update_field(int16_t x, int16_t y, int8_t value) {
int8_t tmp = access_field(x, y, 0, 0);
if (tmp == -128) {
// Nicht aktualiseren, wenn es sich um ein Loch handelt
return;
}
int8_t new_value = (int8_t) (tmp + value);
/* Saturation */
if (value > 0) {
if (new_value < tmp) {
new_value = 127;
}
} else {
if (new_value > tmp || new_value == -128) {
new_value = -127;
}
}
access_field(x, y, new_value, 1);
}
/**
* Aendert den Wert eines Kreises um den angegebenen Betrag
* \param x x-Ordinate der Karte (nicht der Welt!!!)
* \param y y-Ordinate der Karte (nicht der Welt!!!)
* \param radius Radius in Kartenpunkten
* \param value Betrag um den das Feld veraendert wird (>0 heisst freier, <0 heisst belegter)
*/
static void update_field_circle(int16_t x, int16_t y, int8_t radius, int8_t value) {
const int16_t square = muls8(radius, radius);
int16_t sec_x_max = (x + radius) / MAP_SECTION_POINTS;
int16_t sec_x_min = (x - radius) / MAP_SECTION_POINTS;
int16_t sec_y_max = (y + radius) / MAP_SECTION_POINTS;
int16_t sec_y_min = (y - radius) / MAP_SECTION_POINTS;
// LOG_DEBUG("Betroffene Sektionen X: %d-%d, Y:%d-%d", sec_x_min, sec_x_max, sec_y_min, sec_y_max);
int16_t sec_x, sec_y, X, Y, dX, dY;
int16_t starty, startx, stopx, stopy;
// Gehe ueber alle betroffenen Sektionen
for (sec_y = sec_y_min; sec_y <= sec_y_max; sec_y++) {
// Bereich weiter eingrenzen
if (sec_y * MAP_SECTION_POINTS > (y - radius)) {
starty = sec_y * MAP_SECTION_POINTS;
} else {
starty = y - radius;
}
if ((sec_y + 1) * MAP_SECTION_POINTS < (y + radius)) {
stopy = (sec_y + 1) * MAP_SECTION_POINTS;
} else {
stopy = y + radius;
}
for (sec_x = sec_x_min; sec_x <= sec_x_max; sec_x++) {
if (sec_x * MAP_SECTION_POINTS > (x - radius)) {
startx = sec_x * MAP_SECTION_POINTS;
} else {
startx = x - radius;
}
if ((sec_x + 1) * MAP_SECTION_POINTS < (x + radius)) {
stopx = (sec_x + 1) * MAP_SECTION_POINTS;
} else {
stopx = x + radius;
}
for (Y = starty; Y < stopy; Y++) {
int8_t tmp = (int8_t) (y - Y); // Distanz berechnen
dY = muls8(tmp, tmp); // Quadrat vorberechnen
for (X = startx; X < stopx; X++) {
tmp = (int8_t) (x - X); // Distanz berechnen
dX = muls8(tmp, tmp); // Quadrat vorberechnen
if (dX + dY <= square) { // wenn Punkt unter Bot
update_field(X, Y, value); // dann aktualisieren
}
}
}
}
}
}
/**
* Markiert ein Feld als belegt -- drueckt den Feldwert etwas mehr in Richtung "belegt"
* \param x x-Ordinate der Karte (nicht der Welt!!!)
* \param y y-Ordinate der Karte (nicht der Welt!!!)
* \param location_prob Gibt an, wie sicher wir ueber die Position sind [0; 255]
*/
static void update_occupied(int16_t x, int16_t y, uint8_t location_prob) {
#ifdef MEASURE_POSITION_ERRORS_AVAILABLE
const uint8_t prob = location_prob;
#else
(void) location_prob;
const uint8_t prob = 255;
#endif
const int8_t r = MAP_RADIUS_FIELDS;
const int8_t r_2 = r * r;
int16_t X, Y;
int8_t dX = r;
for (X = x - r; X <= x + r; ++X, --dX) {
const int8_t dX_2 = (int8_t) (dX * dX);
int8_t dY = r;
for (Y = y - r; Y <= y + r; ++Y, --dY) {
int8_t h = (int8_t) (dY * dY + dX_2);
if (h <= r_2) {
h /= 2;
if (h < MAP_STEP_OCCUPIED) {
h = MAP_STEP_OCCUPIED;
}
update_field(X, Y, (int8_t) ((((- MAP_STEP_OCCUPIED * MAP_STEP_OCCUPIED) / h) + 1) * prob / 255 - 1));
}
}
}
}
/**
* Map-Umfeldaktualisierung mit einem bestimmten Wert ab einer Position xy mit Radius r bei
* \param x Map-Koordinate
* \param y Map-Koordinate
* \param radius Radius des Umfeldes
* \param value Mapwert; nur eingetragen wenn aktueller Mapwert groesser value ist
*/
static void set_value_field_circle(int16_t x, int16_t y, int8_t radius, int8_t value) {
int8_t dX, dY;
int16_t h = muls8(radius, radius);
for (dX = (int8_t) -radius; dX <= radius; dX++) {
int16_t dX2 = muls8(dX, dX);
for (dY = (int8_t) -radius; dY <= radius; dY++) {
if (dX2 + muls8(dY, dY) <= h) {
// nur innerhalb des Umkreises
const int16_t x_tmp = x + dX;
const int16_t y_tmp = y + dY;
if (access_field(x_tmp, y_tmp, 0, 0) > value) {
// Mapwert hoeher Richtung frei
access_field (x_tmp, y_tmp, value, 1); // dann Wert eintragen
}
}
}
}
}
/**
* setzt ein Map-Feld auf einen Wert mit Umfeldaktualisierung;
* Hindernis wird mit MAP_RADIUS_FIELDS eingetragen
* \param x Map-Koordinate
* \param y Map-Koordinate
* \param val im Umkreis einzutragender Wert
*/
static void set_value_occupied(int16_t x, int16_t y, int8_t val) {
int8_t r;
// in Map mit dem Radius um x/y eintragen
for (r = 1; r <= MAP_RADIUS_FIELDS; r++) {
set_value_field_circle(x, y, r, val);
}
}
/**
* Aktualisiert die Karte mit den Daten eines Distanz-Sensors
* \param x X-Achse der Position des Sensors
* \param y Y-Achse der Position des Sensors
* \param h_sin sin(Blickrichtung)
* \param h_cos cos(Blickrichtung)
* \param dist Sensorwert
* \param location_prob Gibt an, wie sicher wir ueber die Position sind [0; 255]
*/
static void update_sensor_distance(int16_t x, int16_t y, float h_sin, float h_cos, int16_t dist, uint8_t location_prob) {
// Ort des Sensors in Kartenkoordinaten
int16_t X = world_to_map(x);
int16_t Y = world_to_map(y);
int16_t d;
if (dist == SENS_IR_INFINITE) {
d = SENS_IR_MAX_DIST;
} else {
d = dist;
}
// liefert die Mapkoordinaten des Hindernisses / Ende des Frei-Strahls
int16_t PH_X = world_to_map(x + (int16_t) (d * h_cos));
int16_t PH_Y = world_to_map(y + (int16_t) (d * h_sin));
// Nun markiere alle Felder vor dem Hindernis als frei
int8_t i;
int16_t lX = X; // Beginne mit dem Feld, in dem der Sensor steht
int16_t lY = Y;
const int8_t sX = (int8_t) (PH_X < X ? -1 : 1);
int8_t dX = (int8_t) abs(PH_X - X); // Laenge der Linie in X-Richtung
const int8_t sY = (int8_t) (PH_Y < Y ? -1 : 1);
int8_t dY = (int8_t) abs(PH_Y - Y); // Laenge der Linie in Y-Richtung
#ifdef MEASURE_POSITION_ERRORS_AVAILABLE
const int8_t step_value = (int8_t) ((MAP_STEP_FREE_SENSOR - 1) * location_prob / 255 + 1);
#else
const int8_t step_value = MAP_STEP_FREE_SENSOR;
#endif
if (dX >= dY) { // Hangle Dich an der laengeren Achse entlang
if (dY > 0) dY--; // stoppe ein Feld vor dem Hindernis
int8_t lh = dX / 2;
for (i = 0; i < dX; ++i) {
update_field(lX, lY, step_value);
lX += sX;
lh = (int8_t) (lh + dY);
if (lh >= dX) {
lh = (int8_t) (lh - dX);
lY += sY;
}
}
} else {
if (dX > 0) dX--; // stoppe ein Feld vor dem Hindernis
int8_t lh = dY / 2;
for (i = 0; i < dY; ++i) {
update_field(lX, lY, step_value);
lY += sY;
lh = (int8_t) (lh + dX);
if (lh >= dY) {
lh = (int8_t) (lh - dY);
lX += sX;
}
}
}
/* Hindernis eintragen */
if (dist <= SENS_IR_MAX_DIST) {
update_occupied(PH_X, PH_Y, location_prob);
}
}
/**
* Aktualisiert die interne Karte anhand der Sensordaten
* \param x X-Achse der Position in Weltkoordinaten
* \param y Y-Achse der Position in Weltkoordinaten
* \param sin_head sin(Blickrichtung)
* \param cos_head cos(Blickrichtung)
* \param distL Sensorwert links
* \param distR Sensorwert rechts
* \param location_prob Gibt an, wie sicher wir ueber die Position sind [0; 255]
*/
static void update_distance(int16_t x, int16_t y, float sin_head, float cos_head, int16_t distL, int16_t distR, uint8_t location_prob) {
// Ort des rechten Sensors in Weltkoordinaten
int16_t Pr_x = x + (int16_t)(DISTSENSOR_POS_SW * sin_head + DISTSENSOR_POS_FW * cos_head);
int16_t Pr_y = y - (int16_t)(DISTSENSOR_POS_SW * cos_head - DISTSENSOR_POS_FW * sin_head);
// Ort des linken Sensors in Weltkoordinaten
int16_t Pl_x = x - (int16_t)(DISTSENSOR_POS_SW * sin_head - DISTSENSOR_POS_FW * cos_head);
int16_t Pl_y = y + (int16_t)(DISTSENSOR_POS_SW * cos_head + DISTSENSOR_POS_FW * sin_head);
update_sensor_distance(Pl_x, Pl_y, sin_head, cos_head, distL, location_prob);
update_sensor_distance(Pr_x, Pr_y, sin_head, cos_head, distR, location_prob);
}
/**
* Aktualisiert den Standkreis der internen Karte
* \param x X-Achse der Position in Weltkoordinaten
* \param y Y-Achse der Position in Weltkoordinaten
* \param location_prob Gibt an, wie sicher wir ueber die Position sind [0; 255]
*/
static void update_location(int16_t x, int16_t y, uint8_t location_prob) {
int16_t x_map = world_to_map(x);
int16_t y_map = world_to_map(y);
// Aktualisiere die vom Bot selbst belegte Flaeche
update_field_circle(x_map, y_map, BOT_DIAMETER / 20 * MAP_RESOLUTION / 100,