Line data Source code
1 : #include "SIM_config.h"
2 :
3 : #if AP_SIM_GPS_TRIMBLE_ENABLED
4 :
5 : #include "SIM_GPS_Trimble.h"
6 :
7 : #include <SITL/SITL.h>
8 : #include <AP_HAL/utility/sparse-endian.h>
9 : #include <AP_InternalError/AP_InternalError.h>
10 :
11 : #include <assert.h>
12 :
13 : using namespace SITL;
14 :
15 82 : void GPS_Trimble::publish(const GPS_Data *d)
16 : {
17 : // This logic is to populate output buffer only with enabled channels.
18 : // It also only sends each channel at the configured rate.
19 82 : const uint64_t now = AP_HAL::millis();
20 82 : uint8_t buf[MAX_PAYLOAD_SIZE] = {};
21 82 : uint8_t payload_sz = 0;
22 82 : uint8_t offset = 0;
23 82 : if (channel_rates[uint8_t(Gsof_Msg_Record_Type::POSITION_TIME)] != Output_Rate::OFF){
24 62 : const auto last_time = last_publish_ms[uint8_t(Gsof_Msg_Record_Type::POSITION_TIME)];
25 62 : const auto desired_rate = channel_rates[uint8_t(Gsof_Msg_Record_Type::POSITION_TIME)];
26 :
27 62 : if (now - last_time > RateToPeriodMs(desired_rate)) {
28 :
29 : // https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_TIME.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____25
30 62 : constexpr uint8_t GSOF_POS_TIME_LEN { 0x0A };
31 : // TODO magic number until SITL supports GPS bootcount based on GPSN_ENABLE
32 62 : const uint8_t bootcount = 17;
33 :
34 : // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Position%20flags%201
35 : enum class POS_FLAGS_1 : uint8_t {
36 : NEW_POSITION = 1U << 0,
37 : CLOCK_FIX_CALULATED = 1U << 1,
38 : HORIZ_FROM_THIS_POS = 1U << 2,
39 : HEIGHT_FROM_THIS_POS = 1U << 3,
40 : RESERVED_4 = 1U << 4,
41 : LEAST_SQ_POSITION = 1U << 5,
42 : RESERVED_6 = 1U << 6,
43 : POSITION_L1_PSEUDORANGES = 1U << 7
44 : };
45 62 : const uint8_t pos_flags_1 {
46 : uint8_t(POS_FLAGS_1::NEW_POSITION) |
47 : uint8_t(POS_FLAGS_1::CLOCK_FIX_CALULATED) |
48 : uint8_t(POS_FLAGS_1::HORIZ_FROM_THIS_POS) |
49 : uint8_t(POS_FLAGS_1::HEIGHT_FROM_THIS_POS) |
50 : uint8_t(POS_FLAGS_1::RESERVED_4) |
51 : uint8_t(POS_FLAGS_1::LEAST_SQ_POSITION) |
52 : uint8_t(POS_FLAGS_1::POSITION_L1_PSEUDORANGES)
53 : };
54 :
55 : // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Position%20flags%202
56 : enum class POS_FLAGS_2 : uint8_t {
57 : DIFFERENTIAL_POS = 1U << 0,
58 : DIFFERENTIAL_POS_PHASE_RTK = 1U << 1,
59 : POSITION_METHOD_FIXED_PHASE = 1U << 2,
60 : OMNISTAR_ACTIVE = 1U << 3,
61 : DETERMINED_WITH_STATIC_CONSTRAINT = 1U << 4,
62 : NETWORK_RTK = 1U << 5,
63 : DITHERED_RTK = 1U << 6,
64 : BEACON_DGNSS = 1U << 7,
65 : };
66 :
67 : // Simulate a GPS without RTK in SIM since there is no RTK SIM params.
68 : // This means these flags are unset:
69 : // NETWORK_RTK, DITHERED_RTK, BEACON_DGNSS
70 62 : uint8_t pos_flags_2 {0};
71 62 : if(d->have_lock) {
72 62 : pos_flags_2 |= uint8_t(POS_FLAGS_2::DIFFERENTIAL_POS);
73 62 : pos_flags_2 |= uint8_t(POS_FLAGS_2::DIFFERENTIAL_POS_PHASE_RTK);
74 62 : pos_flags_2 |= uint8_t(POS_FLAGS_2::POSITION_METHOD_FIXED_PHASE);
75 62 : pos_flags_2 |= uint8_t(POS_FLAGS_2::OMNISTAR_ACTIVE);
76 62 : pos_flags_2 |= uint8_t(POS_FLAGS_2::DETERMINED_WITH_STATIC_CONSTRAINT);
77 : }
78 :
79 62 : const auto gps_tow = gps_time();
80 : const struct PACKED gsof_pos_time {
81 : const uint8_t OUTPUT_RECORD_TYPE;
82 : const uint8_t RECORD_LEN;
83 : uint32_t time_week_ms;
84 : uint16_t time_week;
85 : uint8_t num_sats;
86 : // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Position%20flags%201
87 : uint8_t pos_flags_1;
88 : // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Position%20flags%202
89 : uint8_t pos_flags_2;
90 : uint8_t initialized_num;
91 62 : } pos_time {
92 : uint8_t(Gsof_Msg_Record_Type::POSITION_TIME),
93 : GSOF_POS_TIME_LEN,
94 62 : htobe32(gps_tow.ms),
95 62 : htobe16(gps_tow.week),
96 62 : d->have_lock ? d->num_sats : uint8_t(3),
97 : pos_flags_1,
98 : pos_flags_2,
99 : bootcount
100 62 : };
101 : static_assert(sizeof(gsof_pos_time) - (sizeof(gsof_pos_time::OUTPUT_RECORD_TYPE) + sizeof(gsof_pos_time::RECORD_LEN)) == GSOF_POS_TIME_LEN, "Trimble size check failed");
102 :
103 62 : payload_sz += sizeof(pos_time);
104 62 : memcpy(&buf[offset], &pos_time, sizeof(pos_time));
105 62 : offset += sizeof(pos_time);
106 : }
107 : }
108 :
109 82 : if (channel_rates[uint8_t(Gsof_Msg_Record_Type::LLH)] != Output_Rate::OFF){
110 61 : const auto last_time = last_publish_ms[uint8_t(Gsof_Msg_Record_Type::LLH)];
111 61 : const auto desired_rate = channel_rates[uint8_t(Gsof_Msg_Record_Type::LLH)];
112 :
113 61 : if (now - last_time > RateToPeriodMs(desired_rate)) {
114 : // https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_LLH.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____20
115 61 : constexpr uint8_t GSOF_POS_LEN = 0x18;
116 :
117 : const struct PACKED gsof_pos {
118 : const uint8_t OUTPUT_RECORD_TYPE;
119 : const uint8_t RECORD_LEN;
120 : uint64_t lat;
121 : uint64_t lng;
122 : uint64_t alt;
123 61 : } pos {
124 : uint8_t(Gsof_Msg_Record_Type::LLH),
125 : GSOF_POS_LEN,
126 61 : gsof_pack_double(d->latitude * DEG_TO_RAD_DOUBLE),
127 122 : gsof_pack_double(d->longitude * DEG_TO_RAD_DOUBLE),
128 122 : gsof_pack_double(static_cast<double>(d->altitude))
129 61 : };
130 : static_assert(sizeof(gsof_pos) - (sizeof(gsof_pos::OUTPUT_RECORD_TYPE) + sizeof(gsof_pos::RECORD_LEN)) == GSOF_POS_LEN, "Trimble size check failed");
131 :
132 61 : payload_sz += sizeof(pos);
133 61 : memcpy(&buf[offset], &pos, sizeof(pos));
134 61 : offset += sizeof(pos);
135 : }
136 : }
137 :
138 82 : if (channel_rates[uint8_t(Gsof_Msg_Record_Type::VELOCITY_DATA)] != Output_Rate::OFF){
139 60 : const auto last_time = last_publish_ms[uint8_t(Gsof_Msg_Record_Type::VELOCITY_DATA)];
140 60 : const auto desired_rate = channel_rates[uint8_t(Gsof_Msg_Record_Type::VELOCITY_DATA)];
141 :
142 60 : if (now - last_time > RateToPeriodMs(desired_rate)) {
143 : // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Velocity.html
144 : // use the smaller packet by ignoring local coordinate system
145 60 : constexpr uint8_t GSOF_VEL_LEN = 0x0D;
146 :
147 : // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Velocity%20flags
148 : enum class VEL_FIELDS : uint8_t {
149 : VALID = 1U << 0,
150 : CONSECUTIVE_MEASUREMENTS = 1U << 1,
151 : HEADING_VALID = 1U << 2,
152 : RESERVED_3 = 1U << 3,
153 : RESERVED_4 = 1U << 4,
154 : RESERVED_5 = 1U << 5,
155 : RESERVED_6 = 1U << 6,
156 : RESERVED_7 = 1U << 7,
157 : };
158 60 : uint8_t vel_flags {0};
159 60 : if(d->have_lock) {
160 60 : vel_flags |= uint8_t(VEL_FIELDS::VALID);
161 60 : vel_flags |= uint8_t(VEL_FIELDS::CONSECUTIVE_MEASUREMENTS);
162 60 : vel_flags |= uint8_t(VEL_FIELDS::HEADING_VALID);
163 : }
164 :
165 : const struct PACKED gsof_vel {
166 : const uint8_t OUTPUT_RECORD_TYPE;
167 : const uint8_t RECORD_LEN;
168 : // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Velocity%20flags
169 : uint8_t flags;
170 : uint32_t horiz_m_p_s;
171 : uint32_t heading_rad;
172 : uint32_t vertical_m_p_s;
173 60 : } vel {
174 : uint8_t(Gsof_Msg_Record_Type::VELOCITY_DATA),
175 : GSOF_VEL_LEN,
176 : vel_flags,
177 60 : gsof_pack_float(d->speed_2d()),
178 60 : gsof_pack_float(d->ground_track_rad()),
179 : // Trimble API has ambiguous direction here.
180 : // Intentionally narrow from double.
181 120 : gsof_pack_float(static_cast<float>(d->speedD))
182 180 : };
183 : static_assert(sizeof(gsof_vel) - (sizeof(gsof_vel::OUTPUT_RECORD_TYPE) + sizeof(gsof_vel::RECORD_LEN)) == GSOF_VEL_LEN, "Trimble size check failed");
184 :
185 60 : payload_sz += sizeof(vel);
186 60 : memcpy(&buf[offset], &vel, sizeof(vel));
187 60 : offset += sizeof(vel);
188 : }
189 : }
190 82 : if (channel_rates[uint8_t(Gsof_Msg_Record_Type::PDOP_INFO)] != Output_Rate::OFF){
191 60 : const auto last_time = last_publish_ms[uint8_t(Gsof_Msg_Record_Type::PDOP_INFO)];
192 60 : const auto desired_rate = channel_rates[uint8_t(Gsof_Msg_Record_Type::PDOP_INFO)];
193 :
194 60 : if (now - last_time > RateToPeriodMs(desired_rate)) {
195 : // https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_PDOP.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____12
196 60 : constexpr uint8_t GSOF_DOP_LEN = 0x10;
197 : const struct PACKED gsof_dop {
198 : const uint8_t OUTPUT_RECORD_TYPE { uint8_t(Gsof_Msg_Record_Type::PDOP_INFO) };
199 : const uint8_t RECORD_LEN { GSOF_DOP_LEN };
200 : uint32_t pdop = htobe32(1);
201 : uint32_t hdop = htobe32(1);
202 : uint32_t vdop = htobe32(1);
203 : uint32_t tdop = htobe32(1);
204 60 : } dop {};
205 : // Check the payload size calculation in the compiler
206 60 : constexpr auto dop_size = sizeof(gsof_dop);
207 : static_assert(dop_size == 18, "gsof_dop must be 8 bytes");
208 60 : constexpr auto dop_record_type_size = sizeof(gsof_dop::OUTPUT_RECORD_TYPE);
209 : static_assert(dop_record_type_size == 1, "gsof_dop::OUTPUT_RECORD_TYPE must be 1 byte");
210 60 : constexpr auto len_size = sizeof(gsof_dop::RECORD_LEN);
211 : static_assert(len_size == 1, "gsof_dop::RECORD_LEN must be 1 bytes");
212 60 : constexpr auto dop_payload_size = dop_size - (dop_record_type_size + len_size);
213 : static_assert(dop_payload_size == GSOF_DOP_LEN, "dop_payload_size must be GSOF_DOP_LEN bytes");
214 :
215 60 : payload_sz += sizeof(dop);
216 60 : memcpy(&buf[offset], &dop, sizeof(dop));
217 60 : offset += sizeof(dop);
218 : }
219 : }
220 82 : if (channel_rates[uint8_t(Gsof_Msg_Record_Type::POSITION_SIGMA_INFO)] != Output_Rate::OFF){
221 59 : const auto last_time = last_publish_ms[uint8_t(Gsof_Msg_Record_Type::POSITION_SIGMA_INFO)];
222 59 : const auto desired_rate = channel_rates[uint8_t(Gsof_Msg_Record_Type::POSITION_SIGMA_INFO)];
223 59 : if (now - last_time > RateToPeriodMs(desired_rate)) {
224 : // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_SIGMA.html
225 59 : constexpr uint8_t GSOF_POS_SIGMA_LEN = 0x26;
226 : const struct PACKED gsof_pos_sigma {
227 : const uint8_t OUTPUT_RECORD_TYPE { uint8_t(Gsof_Msg_Record_Type::POSITION_SIGMA_INFO) };
228 : const uint8_t RECORD_LEN { GSOF_POS_SIGMA_LEN };
229 : uint32_t pos_rms = htobe32(0);
230 : uint32_t sigma_e = htobe32(0);
231 : uint32_t sigma_n = htobe32(0);
232 : uint32_t cov_en = htobe32(0);
233 : uint32_t sigma_up = htobe32(0);
234 : uint32_t semi_major_axis = htobe32(0);
235 : uint32_t semi_minor_axis = htobe32(0);
236 : uint32_t orientation = htobe32(0);
237 : uint32_t unit_variance = htobe32(0);
238 : uint16_t n_epocs = htobe32(1); // Always 1 for kinematic.
239 59 : } pos_sigma {};
240 : static_assert(sizeof(gsof_pos_sigma) - (sizeof(gsof_pos_sigma::OUTPUT_RECORD_TYPE) + sizeof(gsof_pos_sigma::RECORD_LEN)) == GSOF_POS_SIGMA_LEN, "Trimble size check failed");
241 59 : payload_sz += sizeof(pos_sigma);
242 59 : memcpy(&buf[offset], &pos_sigma, sizeof(pos_sigma));
243 59 : offset += sizeof(pos_sigma);
244 : }
245 : }
246 :
247 82 : assert(offset == payload_sz);
248 :
249 : // Don't send empy frames when all channels are disabled;
250 82 : if (payload_sz > 0) {
251 62 : send_gsof(buf, payload_sz);
252 : }
253 :
254 82 : }
255 :
256 124 : bool DCOL_Parser::dcol_parse(const char data_in) {
257 124 : bool ret = false;
258 124 : switch (parse_state) {
259 6 : case Parse_State::WAITING_ON_STX:
260 6 : if (data_in == STX) {
261 6 : reset();
262 6 : parse_state = Parse_State::WAITING_ON_STATUS;
263 : }
264 6 : break;
265 6 : case Parse_State::WAITING_ON_STATUS:
266 6 : if (data_in != (uint8_t)Status::OK) {
267 : // Invalid, status should always be OK.
268 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
269 : } else {
270 6 : status = static_cast<Status>(data_in);
271 6 : parse_state = Parse_State::WAITING_ON_PACKET_TYPE;
272 : }
273 6 : break;
274 6 : case Parse_State::WAITING_ON_PACKET_TYPE:
275 6 : if (data_in == (uint8_t)Packet_Type::COMMAND_APPFILE) {
276 6 : packet_type = Packet_Type::COMMAND_APPFILE;
277 : } else {
278 : // Unsupported command in this simulator.
279 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
280 : }
281 6 : parse_state = Parse_State::WAITING_ON_LENGTH;
282 6 : break;
283 6 : case Parse_State::WAITING_ON_LENGTH:
284 6 : expected_payload_length = data_in;
285 6 : parse_state = Parse_State::WAITING_ON_PACKET_DATA;
286 6 : break;
287 88 : case Parse_State::WAITING_ON_PACKET_DATA:
288 88 : payload[cur_payload_idx] = data_in;
289 88 : if (++cur_payload_idx == expected_payload_length) {
290 6 : parse_state = Parse_State::WAITING_ON_CSUM;
291 : }
292 88 : break;
293 6 : case Parse_State::WAITING_ON_CSUM:
294 6 : expected_csum = data_in;
295 6 : parse_state = Parse_State::WAITING_ON_ETX;
296 6 : break;
297 6 : case Parse_State::WAITING_ON_ETX:
298 6 : if (data_in != ETX) {
299 0 : reset();
300 : }
301 6 : if (!valid_csum()) {
302 : // GSOF driver sent a packet with invalid CSUM.
303 : // In real GSOF driver, the packet is simply ignored with no reply.
304 : // In the SIM, treat this as a coding error.
305 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
306 : } else {
307 6 : ret = parse_payload();
308 : }
309 6 : reset();
310 6 : break;
311 : }
312 :
313 124 : return ret;
314 : }
315 :
316 302 : uint32_t DCOL_Parser::RateToPeriodMs(const Output_Rate rate) {
317 302 : uint32_t min_period_ms = 0;
318 302 : switch (rate) {
319 0 : case Output_Rate::OFF:
320 0 : min_period_ms = 0;
321 0 : break;
322 302 : case Output_Rate::FREQ_10_HZ:
323 302 : min_period_ms = 100;
324 302 : break;
325 0 : case Output_Rate::FREQ_50_HZ:
326 0 : min_period_ms = 20;
327 0 : break;
328 0 : case Output_Rate::FREQ_100_HZ:
329 0 : min_period_ms = 10;
330 0 : break;
331 : }
332 302 : return min_period_ms;
333 : }
334 :
335 :
336 6 : bool DCOL_Parser::valid_csum() {
337 6 : uint8_t sum = (uint8_t)status;
338 6 : sum += (uint8_t)packet_type;
339 6 : sum += expected_payload_length;
340 6 : sum += crc_sum_of_bytes(payload, expected_payload_length);
341 6 : return sum == expected_csum;
342 : }
343 :
344 6 : bool DCOL_Parser::parse_payload() {
345 6 : bool success = false;
346 6 : if (packet_type == Packet_Type::COMMAND_APPFILE) {
347 6 : success = parse_cmd_appfile();
348 : }
349 :
350 6 : return success;
351 : }
352 :
353 6 : bool DCOL_Parser::parse_cmd_appfile() {
354 : // A file control info block contains:
355 : // * version
356 : // * device type
357 : // * start application file flag
358 : // * factory settings flag
359 6 : constexpr uint8_t file_control_info_block_sz = 4;
360 : // An appfile header contains:
361 : // * transmisison number
362 : // * page index
363 : // * max page index
364 6 : constexpr uint8_t appfile_header_sz = 3;
365 6 : constexpr uint8_t min_cmd_appfile_sz = file_control_info_block_sz + appfile_header_sz;
366 6 : if (expected_payload_length < min_cmd_appfile_sz) {
367 0 : return false;
368 : }
369 :
370 : // For now, ignore appfile_trans_num, return success regardless.
371 : // If the driver doesn't send the right value, it's not clear what the behavior is supposed to be.
372 : // Also would need to handle re-sync.
373 : // For now, just store it for debugging.
374 6 : appfile_trans_num = payload[0];
375 :
376 : // File control information block parsing:
377 : // https://receiverhelp.trimble.com/oem-gnss/ICD_Subtype_Command64h_FileControlInfo.html
378 6 : constexpr uint8_t expected_app_file_spec_version = 0x03;
379 6 : constexpr uint8_t file_ctrl_idx = appfile_header_sz;
380 6 : if (payload[file_ctrl_idx] != expected_app_file_spec_version) {
381 : // Only version 3 is supported at this time.
382 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
383 : }
384 :
385 6 : constexpr uint8_t expected_dev_type = 0x00;
386 6 : if (payload[file_ctrl_idx+1] != expected_dev_type) {
387 : // Only "all" device type is supported.
388 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
389 : }
390 :
391 6 : constexpr uint8_t expected_start_flag = 0x01;
392 6 : if (payload[file_ctrl_idx+2] != expected_start_flag) {
393 : // Only "immediate" start type is supported.
394 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
395 : }
396 :
397 :
398 6 : constexpr uint8_t expected_factory_settings_flag = 0x00;
399 6 : if (payload[file_ctrl_idx+3] != expected_factory_settings_flag) {
400 : // Factory settings restore before appfile is not supported.
401 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
402 : }
403 :
404 6 : constexpr uint8_t app_file_records_idx = appfile_header_sz + file_control_info_block_sz;
405 6 : const uint8_t record_type = payload[app_file_records_idx];
406 6 : if (record_type == (uint8_t)Appfile_Record_Type::SERIAL_PORT_BAUD_RATE_FORMAT) {
407 : // Serial port baud/format
408 : // https://receiverhelp.trimble.com/oem-gnss/ICD_Command64h_AppFile_SerialPort.html
409 :
410 : // Ignore serial port index (COM Port) since there's only one in SITL.
411 : // Ignore baud rate because you can't change baud yet in SITL.
412 : // Ignore parity because it can't be changed in SITL.
413 : // Ignore flow control because it's not yet in SITL.
414 5 : } else if (record_type == (uint8_t)Appfile_Record_Type::OUTPUT_MESSAGE){
415 : // Output Message
416 : // https://receiverhelp.trimble.com/oem-gnss/ICD_Command64h_AppFile_Output.html
417 :
418 :
419 : // Ignore record length to save flash.
420 : // Ignore port index since SITL is only one port.
421 5 : if (payload[app_file_records_idx + 2] == (uint8_t)(Output_Msg_Msg_Type::GSOF)) {
422 5 : const auto gsof_submessage_type = payload[app_file_records_idx + 6];
423 5 : const auto rate = payload[app_file_records_idx + 4];
424 5 : if (rate == (uint8_t)Output_Rate::OFF) {
425 0 : channel_rates[gsof_submessage_type] = static_cast<Output_Rate>(rate);
426 5 : } else if (rate == (uint8_t)Output_Rate::FREQ_10_HZ) {
427 5 : channel_rates[gsof_submessage_type] = static_cast<Output_Rate>(rate);
428 0 : } else if (rate == (uint8_t)Output_Rate::FREQ_50_HZ) {
429 0 : channel_rates[gsof_submessage_type] = static_cast<Output_Rate>(rate);
430 0 : } else if (rate == (uint8_t)Output_Rate::FREQ_100_HZ) {
431 0 : channel_rates[gsof_submessage_type] = static_cast<Output_Rate>(rate);
432 : } else {
433 : // Unsupported GSOF rate in SITL.
434 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
435 : }
436 : } else {
437 : // Only some data output protocols are supported in SITL.
438 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
439 : }
440 :
441 : } else {
442 : // Other application file packets are not yet supported.
443 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
444 : }
445 :
446 6 : return true;
447 : }
448 :
449 12 : void DCOL_Parser::reset() {
450 12 : cur_payload_idx = 0;
451 12 : expected_payload_length = 0;
452 12 : parse_state = Parse_State::WAITING_ON_STX;
453 : // To be pedantic, one could zero the bytes in the payload,
454 : // but this is skipped because it's extra CPU.
455 :
456 : // Note - appfile_trans_number is intended to persist over parser resets.
457 12 : }
458 :
459 :
460 62 : void GPS_Trimble::send_gsof(const uint8_t *buf, const uint16_t size)
461 : {
462 : // All Trimble "Data Collector" packets, including GSOF, are comprised of three fields:
463 : // * A fixed-length packet header (dcol_header)
464 : // * A variable-length data frame (buf)
465 : // * A fixed-length packet trailer (dcol_trailer)
466 : // Reference: // https://receiverhelp.trimble.com/oem-gnss/index.html#API_DataCollectorFormatPacketStructure.html?TocPath=API%2520Documentation%257CData%2520collector%2520format%2520packets%257CData%2520collector%2520format%253A%2520packet%2520structure%257C_____0
467 :
468 : // status bitfield
469 : // https://receiverhelp.trimble.com/oem-gnss/index.html#API_ReceiverStatusByte.html?TocPath=API%2520Documentation%257CData%2520collector%2520format%2520packets%257CData%2520collector%2520format%253A%2520packet%2520structure%257C_____1
470 62 : const uint8_t STATUS = 0xa8;
471 62 : const uint8_t PACKET_TYPE = 0x40; // Report Packet 40h (GENOUT)
472 :
473 : // Before writing the GSOF data buffer, the GSOF header needs added between the DCOL header and the payload data frame.
474 : // https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_GSOF.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____2
475 :
476 : static uint8_t TRANSMISSION_NUMBER = 0; // Functionally, this is a sequence number
477 : // Most messages, even GSOF49, only take one page. For SIM, assume it.
478 62 : assert(size < 0xFA); // GPS SIM doesn't yet support paging
479 62 : constexpr uint8_t PAGE_INDEX = 0;
480 62 : constexpr uint8_t MAX_PAGE_INDEX = 0;
481 62 : const uint8_t gsof_header[3] = {
482 : TRANSMISSION_NUMBER,
483 : PAGE_INDEX,
484 : MAX_PAGE_INDEX,
485 62 : };
486 62 : ++TRANSMISSION_NUMBER;
487 :
488 : // A captured GSOF49 packet from BD940 has LENGTH field set to 0x6d = 109 bytes.
489 : // A captured GSOF49 packet from BD940 has total bytes of 115 bytes.
490 : // Thus, the following 5 bytes are not counted.
491 : // 1) STX
492 : // 2) STATUS
493 : // 3) PACKET TYPE
494 : // 4) LENGTH
495 : // 5) CHECKSUM
496 : // 6) ETX
497 : // This aligns with manual's idea of data bytes:
498 : // "Each message begins with a 4-byte header, followed by the bytes of data in each packet. The packet ends with a 2-byte trailer."
499 : // Thus, for this implementation with single-page single-record per DCOL packet,
500 : // the length is simply the sum of data packet size, the gsof_header size.
501 62 : const uint8_t length = size + sizeof(gsof_header);
502 62 : const uint8_t dcol_header[4] {
503 : STX,
504 : STATUS,
505 : PACKET_TYPE,
506 : length
507 62 : };
508 :
509 :
510 : // Sum bytes (status + type + length + data bytes) and modulo 256 the summation
511 : // Because it's a uint8, use natural overflow
512 62 : uint8_t csum = STATUS + PACKET_TYPE + length;
513 248 : for (size_t i = 0; i < ARRAY_SIZE(gsof_header); i++) {
514 186 : csum += gsof_header[i];
515 : }
516 6732 : for (size_t i = 0; i < size; i++) {
517 6670 : csum += buf[i];
518 : }
519 :
520 62 : const uint8_t dcol_trailer[2] = {
521 : csum,
522 : ETX
523 62 : };
524 :
525 62 : write_to_autopilot((char*)dcol_header, sizeof(dcol_header));
526 62 : write_to_autopilot((char*)gsof_header, sizeof(gsof_header));
527 62 : write_to_autopilot((char*)buf, size);
528 62 : write_to_autopilot((char*)dcol_trailer, sizeof(dcol_trailer));
529 62 : const uint8_t total_size = sizeof(dcol_header) + sizeof(gsof_header) + size + sizeof(dcol_trailer);
530 : // Validate length based on everything but DCOL h
531 62 : if(dcol_header[3] != total_size - (sizeof(dcol_header) + sizeof(dcol_trailer))) {
532 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
533 : }
534 62 : }
535 :
536 183 : uint64_t GPS_Trimble::gsof_pack_double(const double& src)
537 : {
538 : uint64_t dst;
539 : static_assert(sizeof(src) == sizeof(dst), "src and dst must have equal size");
540 183 : memcpy(&dst, &src, sizeof(dst));
541 183 : dst = htobe64(dst);
542 183 : return dst;
543 : }
544 :
545 180 : uint32_t GPS_Trimble::gsof_pack_float(const float& src)
546 : {
547 : uint32_t dst;
548 : static_assert(sizeof(src) == sizeof(dst), "src and dst must have equal size");
549 180 : memcpy(&dst, &src, sizeof(dst));
550 180 : dst = htobe32(dst);
551 180 : return dst;
552 : }
553 :
554 19767 : void GPS_Trimble::update_read() {
555 : // Technically, the max command is slightly larger.
556 : // This will only slightly slow the response for packets that big.
557 : char c[MAX_PAYLOAD_SIZE];
558 19767 : const auto n_read = read_from_autopilot(c, MAX_PAYLOAD_SIZE);
559 19767 : if (n_read > 0) {
560 130 : for (uint8_t i = 0; i < n_read; i++) {
561 124 : if (dcol_parse(c[i])) {
562 6 : constexpr uint8_t response[1] = {(uint8_t)Command_Response::ACK};
563 6 : write_to_autopilot((char*)response, sizeof(response));
564 : }
565 : // TODO handle NACK for failure
566 : }
567 : }
568 19767 : }
569 :
570 : #endif // AP_SIM_GPS_TRIMBLE_ENABLED
|
