Branch data MC/DC data Line data Source code
1 : : : #include <stdbool.h>
2 : : : #include <stdint.h>
3 : : :
4 : : : #include "common.h"
5 : : :
6 : : : #include "can.h"
7 : : : #include "message_types.h"
8 : : : #include "msg_common.h"
9 : : : #include "msg_gps.h"
10 : : :
11 : : 1 : void build_gps_time_msg(can_msg_prio_t prio, uint16_t timestamp, uint8_t utc_hours,
12 : : : uint8_t utc_mins, uint8_t utc_secs, uint8_t utc_dsecs, can_msg_t *output) {
13 [ + - ]: [ T f ]: 1 : w_assert(output);
14 : : :
15 : : 1 : output->sid = build_sid(prio, MSG_GPS_TIMESTAMP, 0);
16 : : 1 : write_timestamp(timestamp, output);
17 : : :
18 : : 1 : output->data[2] = utc_hours;
19 : : 1 : output->data[3] = utc_mins;
20 : : 1 : output->data[4] = utc_secs;
21 : : 1 : output->data[5] = utc_dsecs;
22 : : :
23 : : 1 : output->data_len = 6;
24 : : 1 : }
25 : : :
26 : : 1 : void build_gps_lat_msg(can_msg_prio_t prio, uint16_t timestamp, uint8_t degrees, uint8_t minutes,
27 : : : uint16_t dminutes, uint8_t direction, can_msg_t *output) {
28 [ + - ]: [ T f ]: 1 : w_assert(output);
29 : : :
30 : : 1 : output->sid = build_sid(prio, MSG_GPS_LATITUDE, 0);
31 : : 1 : write_timestamp(timestamp, output);
32 : : :
33 : : 1 : output->data[2] = degrees;
34 : : 1 : output->data[3] = minutes;
35 : : 1 : output->data[4] = dminutes >> 8;
36 : : 1 : output->data[5] = dminutes & 0xFF;
37 : : 1 : output->data[6] = direction;
38 : : :
39 : : 1 : output->data_len = 7;
40 : : 1 : }
41 : : :
42 : : 1 : void build_gps_lon_msg(can_msg_prio_t prio, uint16_t timestamp, uint8_t degrees, uint8_t minutes,
43 : : : uint16_t dminutes, uint8_t direction, can_msg_t *output) {
44 [ + - ]: [ T f ]: 1 : w_assert(output);
45 : : :
46 : : 1 : output->sid = build_sid(prio, MSG_GPS_LONGITUDE, 0);
47 : : 1 : write_timestamp(timestamp, output);
48 : : :
49 : : 1 : output->data[2] = degrees;
50 : : 1 : output->data[3] = minutes;
51 : : 1 : output->data[4] = dminutes >> 8;
52 : : 1 : output->data[5] = dminutes & 0xFF;
53 : : 1 : output->data[6] = direction;
54 : : :
55 : : 1 : output->data_len = 7;
56 : : 1 : }
57 : : :
58 : : 1 : void build_gps_alt_msg(can_msg_prio_t prio, uint16_t timestamp, uint16_t altitude,
59 : : : uint8_t daltitude, uint8_t units, can_msg_t *output) {
60 [ + - ]: [ T f ]: 1 : w_assert(output);
61 : : :
62 : : 1 : output->sid = build_sid(prio, MSG_GPS_ALTITUDE, 0);
63 : : 1 : write_timestamp(timestamp, output);
64 : : :
65 : : 1 : output->data[2] = (altitude >> 8) & 0xff;
66 : : 1 : output->data[3] = (altitude >> 0) & 0xff;
67 : : 1 : output->data[4] = daltitude;
68 : : 1 : output->data[5] = units;
69 : : :
70 : : 1 : output->data_len = 6;
71 : : 1 : }
72 : : :
73 : : 1 : void build_gps_info_msg(can_msg_prio_t prio, uint16_t timestamp, uint8_t num_sat, uint8_t quality,
74 : : : can_msg_t *output) {
75 [ + - ]: [ T f ]: 1 : w_assert(output);
76 : : :
77 : : 1 : output->sid = build_sid(prio, MSG_GPS_INFO, 0);
78 : : 1 : write_timestamp(timestamp, output);
79 : : :
80 : : 1 : output->data[2] = num_sat;
81 : : 1 : output->data[3] = quality;
82 : : :
83 : : 1 : output->data_len = 4;
84 : : 1 : }
85 : : :
86 : : 1 : bool get_gps_time(const can_msg_t *msg, uint8_t *utc_hours, uint8_t *utc_mins, uint8_t *utc_secs,
87 : : : uint8_t *utc_dsecs) {
88 [ + - ]: [ T f ]: 1 : w_assert(msg);
89 [ + - ]: [ T f ]: 1 : w_assert(utc_hours);
90 [ + - ]: [ T f ]: 1 : w_assert(utc_mins);
91 [ + - ]: [ T f ]: 1 : w_assert(utc_secs);
92 [ + - ]: [ T f ]: 1 : w_assert(utc_dsecs);
93 : : :
94 [ + - ]: [ t F ]: 1 : if (get_message_type(msg) != MSG_GPS_TIMESTAMP) {
95 : : : return false;
96 : : : }
97 : : :
98 : : 1 : *utc_hours = msg->data[2];
99 : : 1 : *utc_mins = msg->data[3];
100 : : 1 : *utc_secs = msg->data[4];
101 : : 1 : *utc_dsecs = msg->data[5];
102 : : :
103 : : 1 : return true;
104 : : : }
105 : : :
106 : : 1 : bool get_gps_lat(const can_msg_t *msg, uint8_t *degrees, uint8_t *minutes, uint16_t *dminutes,
107 : : : uint8_t *direction) {
108 [ + - ]: [ T f ]: 1 : w_assert(msg);
109 [ + - ]: [ T f ]: 1 : w_assert(degrees);
110 [ + - ]: [ T f ]: 1 : w_assert(minutes);
111 [ + - ]: [ T f ]: 1 : w_assert(dminutes);
112 [ + - ]: [ T f ]: 1 : w_assert(direction);
113 : : :
114 [ + - ]: [ t F ]: 1 : if (get_message_type(msg) != MSG_GPS_LATITUDE) {
115 : : : return false;
116 : : : }
117 : : :
118 : : 1 : *degrees = msg->data[2];
119 : : 1 : *minutes = msg->data[3];
120 : : 1 : *dminutes = ((uint16_t)msg->data[4] << 8) | msg->data[5];
121 : : 1 : *direction = msg->data[6];
122 : : :
123 : : 1 : return true;
124 : : : }
125 : : :
126 : : 1 : bool get_gps_lon(const can_msg_t *msg, uint8_t *degrees, uint8_t *minutes, uint16_t *dminutes,
127 : : : uint8_t *direction) {
128 [ + - ]: [ T f ]: 1 : w_assert(msg);
129 [ + - ]: [ T f ]: 1 : w_assert(degrees);
130 [ + - ]: [ T f ]: 1 : w_assert(minutes);
131 [ + - ]: [ T f ]: 1 : w_assert(dminutes);
132 [ + - ]: [ T f ]: 1 : w_assert(direction);
133 : : :
134 [ + - ]: [ t F ]: 1 : if (get_message_type(msg) != MSG_GPS_LONGITUDE) {
135 : : : return false;
136 : : : }
137 : : :
138 : : 1 : *degrees = msg->data[2];
139 : : 1 : *minutes = msg->data[3];
140 : : 1 : *dminutes = ((uint16_t)msg->data[4] << 8) | msg->data[5];
141 : : 1 : *direction = msg->data[6];
142 : : :
143 : : 1 : return true;
144 : : : }
145 : : :
146 : : 1 : bool get_gps_alt(const can_msg_t *msg, uint16_t *altitude, uint8_t *daltitude, uint8_t *units) {
147 [ + - ]: [ T f ]: 1 : w_assert(msg);
148 [ + - ]: [ T f ]: 1 : w_assert(altitude);
149 [ + - ]: [ T f ]: 1 : w_assert(daltitude);
150 [ + - ]: [ T f ]: 1 : w_assert(units);
151 : : :
152 [ + - ]: [ t F ]: 1 : if (get_message_type(msg) != MSG_GPS_ALTITUDE) {
153 : : : return false;
154 : : : }
155 : : :
156 : : 1 : *altitude = ((uint16_t)msg->data[2] << 8) | msg->data[3];
157 : : 1 : *daltitude = msg->data[4];
158 : : 1 : *units = msg->data[5];
159 : : :
160 : : 1 : return true;
161 : : : }
162 : : :
163 : : 1 : bool get_gps_info(const can_msg_t *msg, uint8_t *num_sat, uint8_t *quality) {
164 [ + - ]: [ T f ]: 1 : w_assert(msg);
165 [ + - ]: [ T f ]: 1 : w_assert(num_sat);
166 [ + - ]: [ T f ]: 1 : w_assert(quality);
167 : : :
168 [ + - ]: [ t F ]: 1 : if (get_message_type(msg) != MSG_GPS_INFO) {
169 : : : return false;
170 : : : }
171 : : :
172 : : 1 : *num_sat = msg->data[2];
173 : : 1 : *quality = msg->data[3];
174 : : :
175 : : 1 : return true;
176 : : : }
|
