Branch data Line data Source code
1 : : #include <stdbool.h>
2 : : #include <stdint.h>
3 : :
4 : : #include "can.h"
5 : : #include "message_types.h"
6 : : #include "msg_common.h"
7 : : #include "msg_gps.h"
8 : :
9 : 0 : bool build_gps_time_msg(can_msg_prio_t prio, uint16_t timestamp, uint8_t utc_hours,
10 : : uint8_t utc_mins, uint8_t utc_secs, uint8_t utc_dsecs, can_msg_t *output) {
11 [ # # ]: 0 : if (!output) {
12 : : return false;
13 : : }
14 : :
15 : 0 : output->sid = build_sid(prio, MSG_GPS_TIMESTAMP, 0);
16 : 0 : write_timestamp(timestamp, output);
17 : :
18 : 0 : output->data[2] = utc_hours;
19 : 0 : output->data[3] = utc_mins;
20 : 0 : output->data[4] = utc_secs;
21 : 0 : output->data[5] = utc_dsecs;
22 : :
23 : 0 : output->data_len = 6;
24 : :
25 : 0 : return true;
26 : : }
27 : :
28 : 0 : bool build_gps_lat_msg(can_msg_prio_t prio, uint16_t timestamp, uint8_t degrees, uint8_t minutes,
29 : : uint16_t dminutes, uint8_t direction, can_msg_t *output) {
30 [ # # ]: 0 : if (!output) {
31 : : return false;
32 : : }
33 : :
34 : 0 : output->sid = build_sid(prio, MSG_GPS_LATITUDE, 0);
35 : 0 : write_timestamp(timestamp, output);
36 : :
37 : 0 : output->data[2] = degrees;
38 : 0 : output->data[3] = minutes;
39 : 0 : output->data[4] = dminutes >> 8;
40 : 0 : output->data[5] = dminutes & 0xFF;
41 : 0 : output->data[6] = direction;
42 : :
43 : 0 : output->data_len = 7;
44 : :
45 : 0 : return true;
46 : : }
47 : :
48 : 0 : bool build_gps_lon_msg(can_msg_prio_t prio, uint16_t timestamp, uint8_t degrees, uint8_t minutes,
49 : : uint16_t dminutes, uint8_t direction, can_msg_t *output) {
50 [ # # ]: 0 : if (!output) {
51 : : return false;
52 : : }
53 : :
54 : 0 : output->sid = build_sid(prio, MSG_GPS_LONGITUDE, 0);
55 : 0 : write_timestamp(timestamp, output);
56 : :
57 : 0 : output->data[2] = degrees;
58 : 0 : output->data[3] = minutes;
59 : 0 : output->data[4] = dminutes >> 8;
60 : 0 : output->data[5] = dminutes & 0xFF;
61 : 0 : output->data[6] = direction;
62 : :
63 : 0 : output->data_len = 7;
64 : :
65 : 0 : return true;
66 : : }
67 : :
68 : 0 : bool build_gps_alt_msg(can_msg_prio_t prio, uint16_t timestamp, uint16_t altitude,
69 : : uint8_t daltitude, uint8_t units, can_msg_t *output) {
70 [ # # ]: 0 : if (!output) {
71 : : return false;
72 : : }
73 : :
74 : 0 : output->sid = build_sid(prio, MSG_GPS_ALTITUDE, 0);
75 : 0 : write_timestamp(timestamp, output);
76 : :
77 : 0 : output->data[2] = (altitude >> 8) & 0xff;
78 : 0 : output->data[3] = (altitude >> 0) & 0xff;
79 : 0 : output->data[4] = daltitude;
80 : 0 : output->data[5] = units;
81 : :
82 : 0 : output->data_len = 6;
83 : :
84 : 0 : return true;
85 : : }
86 : :
87 : 0 : bool build_gps_info_msg(can_msg_prio_t prio, uint16_t timestamp, uint8_t num_sat, uint8_t quality,
88 : : can_msg_t *output) {
89 [ # # ]: 0 : if (!output) {
90 : : return false;
91 : : }
92 : :
93 : 0 : output->sid = build_sid(prio, MSG_GPS_INFO, 0);
94 : 0 : write_timestamp(timestamp, output);
95 : :
96 : 0 : output->data[2] = num_sat;
97 : 0 : output->data[3] = quality;
98 : :
99 : 0 : output->data_len = 4;
100 : :
101 : 0 : return true;
102 : : }
103 : :
104 : 0 : bool get_gps_time(const can_msg_t *msg, uint8_t *utc_hours, uint8_t *utc_mins, uint8_t *utc_secs,
105 : : uint8_t *utc_dsecs) {
106 [ # # ]: 0 : if (!msg) {
107 : : return false;
108 : : }
109 [ # # ]: 0 : if (!utc_hours) {
110 : : return false;
111 : : }
112 [ # # ]: 0 : if (!utc_mins) {
113 : : return false;
114 : : }
115 [ # # ]: 0 : if (!utc_secs) {
116 : : return false;
117 : : }
118 [ # # ]: 0 : if (!utc_dsecs) {
119 : : return false;
120 : : }
121 [ # # ]: 0 : if (get_message_type(msg) != MSG_GPS_TIMESTAMP) {
122 : : return false;
123 : : }
124 : :
125 : 0 : *utc_hours = msg->data[2];
126 : 0 : *utc_mins = msg->data[3];
127 : 0 : *utc_secs = msg->data[4];
128 : 0 : *utc_dsecs = msg->data[5];
129 : :
130 : 0 : return true;
131 : : }
132 : :
133 : 0 : bool get_gps_lat(const can_msg_t *msg, uint8_t *degrees, uint8_t *minutes, uint16_t *dminutes,
134 : : uint8_t *direction) {
135 [ # # ]: 0 : if (!msg) {
136 : : return false;
137 : : }
138 [ # # ]: 0 : if (!degrees) {
139 : : return false;
140 : : }
141 [ # # ]: 0 : if (!minutes) {
142 : : return false;
143 : : }
144 [ # # ]: 0 : if (!dminutes) {
145 : : return false;
146 : : }
147 [ # # ]: 0 : if (!direction) {
148 : : return false;
149 : : }
150 [ # # ]: 0 : if (get_message_type(msg) != MSG_GPS_LATITUDE) {
151 : : return false;
152 : : }
153 : :
154 : 0 : *degrees = msg->data[2];
155 : 0 : *minutes = msg->data[3];
156 : 0 : *dminutes = ((uint16_t)msg->data[4] << 8) | msg->data[5];
157 : 0 : *direction = msg->data[6];
158 : :
159 : 0 : return true;
160 : : }
161 : :
162 : 0 : bool get_gps_lon(const can_msg_t *msg, uint8_t *degrees, uint8_t *minutes, uint16_t *dminutes,
163 : : uint8_t *direction) {
164 [ # # ]: 0 : if (!msg) {
165 : : return false;
166 : : }
167 [ # # ]: 0 : if (!degrees) {
168 : : return false;
169 : : }
170 [ # # ]: 0 : if (!minutes) {
171 : : return false;
172 : : }
173 [ # # ]: 0 : if (!dminutes) {
174 : : return false;
175 : : }
176 [ # # ]: 0 : if (!direction) {
177 : : return false;
178 : : }
179 [ # # ]: 0 : if (get_message_type(msg) != MSG_GPS_LONGITUDE) {
180 : : return false;
181 : : }
182 : :
183 : 0 : *degrees = msg->data[2];
184 : 0 : *minutes = msg->data[3];
185 : 0 : *dminutes = ((uint16_t)msg->data[4] << 8) | msg->data[5];
186 : 0 : *direction = msg->data[6];
187 : :
188 : 0 : return true;
189 : : }
190 : :
191 : 0 : bool get_gps_alt(const can_msg_t *msg, uint16_t *altitude, uint8_t *daltitude, uint8_t *units) {
192 [ # # ]: 0 : if (!msg) {
193 : : return false;
194 : : }
195 [ # # ]: 0 : if (!altitude) {
196 : : return false;
197 : : }
198 [ # # ]: 0 : if (!daltitude) {
199 : : return false;
200 : : }
201 [ # # ]: 0 : if (!units) {
202 : : return false;
203 : : }
204 [ # # ]: 0 : if (get_message_type(msg) != MSG_GPS_ALTITUDE) {
205 : : return false;
206 : : }
207 : :
208 : 0 : *altitude = ((uint16_t)msg->data[2] << 8) | msg->data[3];
209 : 0 : *daltitude = msg->data[4];
210 : 0 : *units = msg->data[5];
211 : :
212 : 0 : return true;
213 : : }
214 : :
215 : 0 : bool get_gps_info(const can_msg_t *msg, uint8_t *num_sat, uint8_t *quality) {
216 [ # # ]: 0 : if (!msg) {
217 : : return false;
218 : : }
219 [ # # ]: 0 : if (!num_sat) {
220 : : return false;
221 : : }
222 [ # # ]: 0 : if (!quality) {
223 : : return false;
224 : : }
225 [ # # ]: 0 : if (get_message_type(msg) != MSG_GPS_INFO) {
226 : : return false;
227 : : }
228 : :
229 : 0 : *num_sat = msg->data[2];
230 : 0 : *quality = msg->data[3];
231 : :
232 : 0 : return true;
233 : : }
|
