-
Notifications
You must be signed in to change notification settings - Fork 0
/
path.cpp
132 lines (101 loc) · 3.42 KB
/
path.cpp
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
#include <Arduino.h>
#include <Adafruit_ST7735.h>
#include <SD.h>
#include <mem_syms.h>
#include "map.h"
#include "serial_handling.h"
#define DEBUG_PATH
/* path routine error code
0 no error
1
*/
int16_t path_errno;
extern Adafruit_ST7735 tft;
// read a path from the serial port and return the length of the
// path and a pointer to the array of coordinates. That array should
// be freed later.
// Returns 1 if the call was successful, 0 if not.
uint8_t read_path(uint16_t *length_p, coord_t *path_p[]) {
// the line to be read, and it size
const uint8_t line_size = 40;
char line[line_size];
uint16_t bytes_read;
// the field extracted
const uint8_t field_size = 20;
char field[field_size];
uint16_t field_index;
int32_t field_value;
*length_p = 0;
*path_p = 0;
// reset the error code
path_errno = 0;
while ( ! Serial.available() ) { };
uint16_t max_path_size = (AVAIL_MEM - 256) / sizeof(coord_t);
#ifdef DEBUG_PATH
Serial.print("Max path length ");
Serial.println(max_path_size);
#endif
bytes_read = serial_readline(line, line_size);
// read the number of points, first field
field_index = 0;
field_index =
string_read_field(line, field_index, field, field_size, " ");
field_value = string_get_int(field);
#ifdef DEBUG_PATH
Serial.print("Path length ");
Serial.print(field_value);
Serial.println();
#endif
// do a consistency check
if ( field_value < 0 || max_path_size < field_value ) {
path_errno = 1;
return 0;
}
uint8_t tmp_length = field_value;
*length_p = tmp_length;
// allocate the storage, see if we got it.
coord_t *tmp_path = (coord_t *) malloc( tmp_length * sizeof(coord_t));
if ( !tmp_path ) {
path_errno = 2;
return 0;
}
*path_p = tmp_path;
while ( tmp_length > 0 ) {
bytes_read = serial_readline(line, line_size);
// read the number of points, first field
field_index = 0;
field_index =
string_read_field(line, field_index, field, field_size, " ");
tmp_path->lat = string_get_int(field);
field_index =
string_read_field(line, field_index, field, field_size, " ");
tmp_path->lon = string_get_int(field);
tmp_length--;
tmp_path++;
}
return 1;
}
uint8_t is_coord_visible(coord_t point) {
// figure out the x and y positions on the current map of the
// given point
uint16_t point_map_y = latitude_to_y(current_map_num, point.lat);
uint16_t point_map_x = longitude_to_x(current_map_num, point.lon);
uint8_t r =
screen_map_x < point_map_x &&
point_map_x < screen_map_x + display_window_width &&
screen_map_y < point_map_y &&
point_map_y < screen_map_y + display_window_height;
return r;
}
void draw_path(uint16_t length, coord_t path[]) {
for (int i = 0; i < length-1; ++i) {
//get coords
int x1 = longitude_to_x(current_map_num, path[ i ].lon) - screen_map_x;
int y1 = latitude_to_y( current_map_num, path[ i ].lat) - screen_map_y;
int x2 = longitude_to_x(current_map_num, path[i+1].lon) - screen_map_x;
int y2 = latitude_to_y( current_map_num, path[i+1].lat) - screen_map_y;
//clipping
//draw
tft.drawLine(x1, y1, x2, y2, tft.Color565(255,0,0));
}
}