forked from ArduPilot/ardupilot
/
SIM_GPS_Trimble.cpp
570 lines (507 loc) · 23.8 KB
/
SIM_GPS_Trimble.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
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
#include "SIM_config.h"
#if AP_SIM_GPS_TRIMBLE_ENABLED
#include "SIM_GPS_Trimble.h"
#include <SITL/SITL.h>
#include <AP_HAL/utility/sparse-endian.h>
#include <AP_InternalError/AP_InternalError.h>
#include <assert.h>
using namespace SITL;
void GPS_Trimble::publish(const GPS_Data *d)
{
// This logic is to populate output buffer only with enabled channels.
// It also only sends each channel at the configured rate.
const uint64_t now = AP_HAL::millis();
uint8_t buf[MAX_PAYLOAD_SIZE] = {};
uint8_t payload_sz = 0;
uint8_t offset = 0;
if (channel_rates[uint8_t(Gsof_Msg_Record_Type::POSITION_TIME)] != Output_Rate::OFF){
const auto last_time = last_publish_ms[uint8_t(Gsof_Msg_Record_Type::POSITION_TIME)];
const auto desired_rate = channel_rates[uint8_t(Gsof_Msg_Record_Type::POSITION_TIME)];
if (now - last_time > RateToPeriodMs(desired_rate)) {
// https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_TIME.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____25
constexpr uint8_t GSOF_POS_TIME_LEN { 0x0A };
// TODO magic number until SITL supports GPS bootcount based on GPSN_ENABLE
const uint8_t bootcount = 17;
// https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Position%20flags%201
enum class POS_FLAGS_1 : uint8_t {
NEW_POSITION = 1U << 0,
CLOCK_FIX_CALULATED = 1U << 1,
HORIZ_FROM_THIS_POS = 1U << 2,
HEIGHT_FROM_THIS_POS = 1U << 3,
RESERVED_4 = 1U << 4,
LEAST_SQ_POSITION = 1U << 5,
RESERVED_6 = 1U << 6,
POSITION_L1_PSEUDORANGES = 1U << 7
};
const uint8_t pos_flags_1 {
uint8_t(POS_FLAGS_1::NEW_POSITION) |
uint8_t(POS_FLAGS_1::CLOCK_FIX_CALULATED) |
uint8_t(POS_FLAGS_1::HORIZ_FROM_THIS_POS) |
uint8_t(POS_FLAGS_1::HEIGHT_FROM_THIS_POS) |
uint8_t(POS_FLAGS_1::RESERVED_4) |
uint8_t(POS_FLAGS_1::LEAST_SQ_POSITION) |
uint8_t(POS_FLAGS_1::POSITION_L1_PSEUDORANGES)
};
// https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Position%20flags%202
enum class POS_FLAGS_2 : uint8_t {
DIFFERENTIAL_POS = 1U << 0,
DIFFERENTIAL_POS_PHASE_RTK = 1U << 1,
POSITION_METHOD_FIXED_PHASE = 1U << 2,
OMNISTAR_ACTIVE = 1U << 3,
DETERMINED_WITH_STATIC_CONSTRAINT = 1U << 4,
NETWORK_RTK = 1U << 5,
DITHERED_RTK = 1U << 6,
BEACON_DGNSS = 1U << 7,
};
// Simulate a GPS without RTK in SIM since there is no RTK SIM params.
// This means these flags are unset:
// NETWORK_RTK, DITHERED_RTK, BEACON_DGNSS
uint8_t pos_flags_2 {0};
if(d->have_lock) {
pos_flags_2 |= uint8_t(POS_FLAGS_2::DIFFERENTIAL_POS);
pos_flags_2 |= uint8_t(POS_FLAGS_2::DIFFERENTIAL_POS_PHASE_RTK);
pos_flags_2 |= uint8_t(POS_FLAGS_2::POSITION_METHOD_FIXED_PHASE);
pos_flags_2 |= uint8_t(POS_FLAGS_2::OMNISTAR_ACTIVE);
pos_flags_2 |= uint8_t(POS_FLAGS_2::DETERMINED_WITH_STATIC_CONSTRAINT);
}
const auto gps_tow = gps_time();
const struct PACKED gsof_pos_time {
const uint8_t OUTPUT_RECORD_TYPE;
const uint8_t RECORD_LEN;
uint32_t time_week_ms;
uint16_t time_week;
uint8_t num_sats;
// https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Position%20flags%201
uint8_t pos_flags_1;
// https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Position%20flags%202
uint8_t pos_flags_2;
uint8_t initialized_num;
} pos_time {
uint8_t(Gsof_Msg_Record_Type::POSITION_TIME),
GSOF_POS_TIME_LEN,
htobe32(gps_tow.ms),
htobe16(gps_tow.week),
d->have_lock ? _sitl->gps_numsats[instance] : uint8_t(3),
pos_flags_1,
pos_flags_2,
bootcount
};
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");
payload_sz += sizeof(pos_time);
memcpy(&buf[offset], &pos_time, sizeof(pos_time));
offset += sizeof(pos_time);
}
}
if (channel_rates[uint8_t(Gsof_Msg_Record_Type::LLH)] != Output_Rate::OFF){
const auto last_time = last_publish_ms[uint8_t(Gsof_Msg_Record_Type::LLH)];
const auto desired_rate = channel_rates[uint8_t(Gsof_Msg_Record_Type::LLH)];
if (now - last_time > RateToPeriodMs(desired_rate)) {
// https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_LLH.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____20
constexpr uint8_t GSOF_POS_LEN = 0x18;
const struct PACKED gsof_pos {
const uint8_t OUTPUT_RECORD_TYPE;
const uint8_t RECORD_LEN;
uint64_t lat;
uint64_t lng;
uint64_t alt;
} pos {
uint8_t(Gsof_Msg_Record_Type::LLH),
GSOF_POS_LEN,
gsof_pack_double(d->latitude * DEG_TO_RAD_DOUBLE),
gsof_pack_double(d->longitude * DEG_TO_RAD_DOUBLE),
gsof_pack_double(static_cast<double>(d->altitude))
};
static_assert(sizeof(gsof_pos) - (sizeof(gsof_pos::OUTPUT_RECORD_TYPE) + sizeof(gsof_pos::RECORD_LEN)) == GSOF_POS_LEN, "Trimble size check failed");
payload_sz += sizeof(pos);
memcpy(&buf[offset], &pos, sizeof(pos));
offset += sizeof(pos);
}
}
if (channel_rates[uint8_t(Gsof_Msg_Record_Type::VELOCITY_DATA)] != Output_Rate::OFF){
const auto last_time = last_publish_ms[uint8_t(Gsof_Msg_Record_Type::VELOCITY_DATA)];
const auto desired_rate = channel_rates[uint8_t(Gsof_Msg_Record_Type::VELOCITY_DATA)];
if (now - last_time > RateToPeriodMs(desired_rate)) {
// https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Velocity.html
// use the smaller packet by ignoring local coordinate system
constexpr uint8_t GSOF_VEL_LEN = 0x0D;
// https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Velocity%20flags
enum class VEL_FIELDS : uint8_t {
VALID = 1U << 0,
CONSECUTIVE_MEASUREMENTS = 1U << 1,
HEADING_VALID = 1U << 2,
RESERVED_3 = 1U << 3,
RESERVED_4 = 1U << 4,
RESERVED_5 = 1U << 5,
RESERVED_6 = 1U << 6,
RESERVED_7 = 1U << 7,
};
uint8_t vel_flags {0};
if(d->have_lock) {
vel_flags |= uint8_t(VEL_FIELDS::VALID);
vel_flags |= uint8_t(VEL_FIELDS::CONSECUTIVE_MEASUREMENTS);
vel_flags |= uint8_t(VEL_FIELDS::HEADING_VALID);
}
const struct PACKED gsof_vel {
const uint8_t OUTPUT_RECORD_TYPE;
const uint8_t RECORD_LEN;
// https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Velocity%20flags
uint8_t flags;
uint32_t horiz_m_p_s;
uint32_t heading_rad;
uint32_t vertical_m_p_s;
} vel {
uint8_t(Gsof_Msg_Record_Type::VELOCITY_DATA),
GSOF_VEL_LEN,
vel_flags,
gsof_pack_float(d->speed_2d()),
gsof_pack_float(d->heading()),
// Trimble API has ambiguous direction here.
// Intentionally narrow from double.
gsof_pack_float(static_cast<float>(d->speedD))
};
static_assert(sizeof(gsof_vel) - (sizeof(gsof_vel::OUTPUT_RECORD_TYPE) + sizeof(gsof_vel::RECORD_LEN)) == GSOF_VEL_LEN, "Trimble size check failed");
payload_sz += sizeof(vel);
memcpy(&buf[offset], &vel, sizeof(vel));
offset += sizeof(vel);
}
}
if (channel_rates[uint8_t(Gsof_Msg_Record_Type::PDOP_INFO)] != Output_Rate::OFF){
const auto last_time = last_publish_ms[uint8_t(Gsof_Msg_Record_Type::PDOP_INFO)];
const auto desired_rate = channel_rates[uint8_t(Gsof_Msg_Record_Type::PDOP_INFO)];
if (now - last_time > RateToPeriodMs(desired_rate)) {
// https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_PDOP.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____12
constexpr uint8_t GSOF_DOP_LEN = 0x10;
const struct PACKED gsof_dop {
const uint8_t OUTPUT_RECORD_TYPE { uint8_t(Gsof_Msg_Record_Type::PDOP_INFO) };
const uint8_t RECORD_LEN { GSOF_DOP_LEN };
uint32_t pdop = htobe32(1);
uint32_t hdop = htobe32(1);
uint32_t vdop = htobe32(1);
uint32_t tdop = htobe32(1);
} dop {};
// Check the payload size calculation in the compiler
constexpr auto dop_size = sizeof(gsof_dop);
static_assert(dop_size == 18, "gsof_dop must be 8 bytes");
constexpr auto dop_record_type_size = sizeof(gsof_dop::OUTPUT_RECORD_TYPE);
static_assert(dop_record_type_size == 1, "gsof_dop::OUTPUT_RECORD_TYPE must be 1 byte");
constexpr auto len_size = sizeof(gsof_dop::RECORD_LEN);
static_assert(len_size == 1, "gsof_dop::RECORD_LEN must be 1 bytes");
constexpr auto dop_payload_size = dop_size - (dop_record_type_size + len_size);
static_assert(dop_payload_size == GSOF_DOP_LEN, "dop_payload_size must be GSOF_DOP_LEN bytes");
payload_sz += sizeof(dop);
memcpy(&buf[offset], &dop, sizeof(dop));
offset += sizeof(dop);
}
}
if (channel_rates[uint8_t(Gsof_Msg_Record_Type::POSITION_SIGMA_INFO)] != Output_Rate::OFF){
const auto last_time = last_publish_ms[uint8_t(Gsof_Msg_Record_Type::POSITION_SIGMA_INFO)];
const auto desired_rate = channel_rates[uint8_t(Gsof_Msg_Record_Type::POSITION_SIGMA_INFO)];
if (now - last_time > RateToPeriodMs(desired_rate)) {
// https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_SIGMA.html
constexpr uint8_t GSOF_POS_SIGMA_LEN = 0x26;
const struct PACKED gsof_pos_sigma {
const uint8_t OUTPUT_RECORD_TYPE { uint8_t(Gsof_Msg_Record_Type::POSITION_SIGMA_INFO) };
const uint8_t RECORD_LEN { GSOF_POS_SIGMA_LEN };
uint32_t pos_rms = htobe32(0);
uint32_t sigma_e = htobe32(0);
uint32_t sigma_n = htobe32(0);
uint32_t cov_en = htobe32(0);
uint32_t sigma_up = htobe32(0);
uint32_t semi_major_axis = htobe32(0);
uint32_t semi_minor_axis = htobe32(0);
uint32_t orientation = htobe32(0);
uint32_t unit_variance = htobe32(0);
uint16_t n_epocs = htobe32(1); // Always 1 for kinematic.
} pos_sigma {};
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");
payload_sz += sizeof(pos_sigma);
memcpy(&buf[offset], &pos_sigma, sizeof(pos_sigma));
offset += sizeof(pos_sigma);
}
}
assert(offset == payload_sz);
// Don't send empy frames when all channels are disabled;
if (payload_sz > 0) {
send_gsof(buf, payload_sz);
}
}
bool DCOL_Parser::dcol_parse(const char data_in) {
bool ret = false;
switch (parse_state) {
case Parse_State::WAITING_ON_STX:
if (data_in == STX) {
reset();
parse_state = Parse_State::WAITING_ON_STATUS;
}
break;
case Parse_State::WAITING_ON_STATUS:
if (data_in != (uint8_t)Status::OK) {
// Invalid, status should always be OK.
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
} else {
status = static_cast<Status>(data_in);
parse_state = Parse_State::WAITING_ON_PACKET_TYPE;
}
break;
case Parse_State::WAITING_ON_PACKET_TYPE:
if (data_in == (uint8_t)Packet_Type::COMMAND_APPFILE) {
packet_type = Packet_Type::COMMAND_APPFILE;
} else {
// Unsupported command in this simulator.
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
}
parse_state = Parse_State::WAITING_ON_LENGTH;
break;
case Parse_State::WAITING_ON_LENGTH:
expected_payload_length = data_in;
parse_state = Parse_State::WAITING_ON_PACKET_DATA;
break;
case Parse_State::WAITING_ON_PACKET_DATA:
payload[cur_payload_idx] = data_in;
if (++cur_payload_idx == expected_payload_length) {
parse_state = Parse_State::WAITING_ON_CSUM;
}
break;
case Parse_State::WAITING_ON_CSUM:
expected_csum = data_in;
parse_state = Parse_State::WAITING_ON_ETX;
break;
case Parse_State::WAITING_ON_ETX:
if (data_in != ETX) {
reset();
}
if (!valid_csum()) {
// GSOF driver sent a packet with invalid CSUM.
// In real GSOF driver, the packet is simply ignored with no reply.
// In the SIM, treat this as a coding error.
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
} else {
ret = parse_payload();
}
reset();
break;
}
return ret;
}
uint32_t DCOL_Parser::RateToPeriodMs(const Output_Rate rate) {
uint32_t min_period_ms = 0;
switch (rate) {
case Output_Rate::OFF:
min_period_ms = 0;
break;
case Output_Rate::FREQ_10_HZ:
min_period_ms = 100;
break;
case Output_Rate::FREQ_50_HZ:
min_period_ms = 20;
break;
case Output_Rate::FREQ_100_HZ:
min_period_ms = 10;
break;
}
return min_period_ms;
}
bool DCOL_Parser::valid_csum() {
uint8_t sum = (uint8_t)status;
sum += (uint8_t)packet_type;
sum += expected_payload_length;
sum += crc_sum_of_bytes(payload, expected_payload_length);
return sum == expected_csum;
}
bool DCOL_Parser::parse_payload() {
bool success = false;
if (packet_type == Packet_Type::COMMAND_APPFILE) {
success = parse_cmd_appfile();
}
return success;
}
bool DCOL_Parser::parse_cmd_appfile() {
// A file control info block contains:
// * version
// * device type
// * start application file flag
// * factory settings flag
constexpr uint8_t file_control_info_block_sz = 4;
// An appfile header contains:
// * transmisison number
// * page index
// * max page index
constexpr uint8_t appfile_header_sz = 3;
constexpr uint8_t min_cmd_appfile_sz = file_control_info_block_sz + appfile_header_sz;
if (expected_payload_length < min_cmd_appfile_sz) {
return false;
}
// For now, ignore appfile_trans_num, return success regardless.
// If the driver doesn't send the right value, it's not clear what the behavior is supposed to be.
// Also would need to handle re-sync.
// For now, just store it for debugging.
appfile_trans_num = payload[0];
// File control information block parsing:
// https://receiverhelp.trimble.com/oem-gnss/ICD_Subtype_Command64h_FileControlInfo.html
constexpr uint8_t expected_app_file_spec_version = 0x03;
constexpr uint8_t file_ctrl_idx = appfile_header_sz;
if (payload[file_ctrl_idx] != expected_app_file_spec_version) {
// Only version 3 is supported at this time.
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
}
constexpr uint8_t expected_dev_type = 0x00;
if (payload[file_ctrl_idx+1] != expected_dev_type) {
// Only "all" device type is supported.
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
}
constexpr uint8_t expected_start_flag = 0x01;
if (payload[file_ctrl_idx+2] != expected_start_flag) {
// Only "immediate" start type is supported.
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
}
constexpr uint8_t expected_factory_settings_flag = 0x00;
if (payload[file_ctrl_idx+3] != expected_factory_settings_flag) {
// Factory settings restore before appfile is not supported.
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
}
constexpr uint8_t app_file_records_idx = appfile_header_sz + file_control_info_block_sz;
const uint8_t record_type = payload[app_file_records_idx];
if (record_type == (uint8_t)Appfile_Record_Type::SERIAL_PORT_BAUD_RATE_FORMAT) {
// Serial port baud/format
// https://receiverhelp.trimble.com/oem-gnss/ICD_Command64h_AppFile_SerialPort.html
// Ignore serial port index (COM Port) since there's only one in SITL.
// Ignore baud rate because you can't change baud yet in SITL.
// Ignore parity because it can't be changed in SITL.
// Ignore flow control because it's not yet in SITL.
} else if (record_type == (uint8_t)Appfile_Record_Type::OUTPUT_MESSAGE){
// Output Message
// https://receiverhelp.trimble.com/oem-gnss/ICD_Command64h_AppFile_Output.html
// Ignore record length to save flash.
// Ignore port index since SITL is only one port.
if (payload[app_file_records_idx + 2] == (uint8_t)(Output_Msg_Msg_Type::GSOF)) {
const auto gsof_submessage_type = payload[app_file_records_idx + 6];
const auto rate = payload[app_file_records_idx + 4];
if (rate == (uint8_t)Output_Rate::OFF) {
channel_rates[gsof_submessage_type] = static_cast<Output_Rate>(rate);
} else if (rate == (uint8_t)Output_Rate::FREQ_10_HZ) {
channel_rates[gsof_submessage_type] = static_cast<Output_Rate>(rate);
} else if (rate == (uint8_t)Output_Rate::FREQ_50_HZ) {
channel_rates[gsof_submessage_type] = static_cast<Output_Rate>(rate);
} else if (rate == (uint8_t)Output_Rate::FREQ_100_HZ) {
channel_rates[gsof_submessage_type] = static_cast<Output_Rate>(rate);
} else {
// Unsupported GSOF rate in SITL.
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
}
} else {
// Only some data output protocols are supported in SITL.
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
}
} else {
// Other application file packets are not yet supported.
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
}
return true;
}
void DCOL_Parser::reset() {
cur_payload_idx = 0;
expected_payload_length = 0;
parse_state = Parse_State::WAITING_ON_STX;
// To be pedantic, one could zero the bytes in the payload,
// but this is skipped because it's extra CPU.
// Note - appfile_trans_number is intended to persist over parser resets.
}
void GPS_Trimble::send_gsof(const uint8_t *buf, const uint16_t size)
{
// All Trimble "Data Collector" packets, including GSOF, are comprised of three fields:
// * A fixed-length packet header (dcol_header)
// * A variable-length data frame (buf)
// * A fixed-length packet trailer (dcol_trailer)
// 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
// status bitfield
// 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
const uint8_t STATUS = 0xa8;
const uint8_t PACKET_TYPE = 0x40; // Report Packet 40h (GENOUT)
// Before writing the GSOF data buffer, the GSOF header needs added between the DCOL header and the payload data frame.
// https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_GSOF.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____2
static uint8_t TRANSMISSION_NUMBER = 0; // Functionally, this is a sequence number
// Most messages, even GSOF49, only take one page. For SIM, assume it.
assert(size < 0xFA); // GPS SIM doesn't yet support paging
constexpr uint8_t PAGE_INDEX = 0;
constexpr uint8_t MAX_PAGE_INDEX = 0;
const uint8_t gsof_header[3] = {
TRANSMISSION_NUMBER,
PAGE_INDEX,
MAX_PAGE_INDEX,
};
++TRANSMISSION_NUMBER;
// A captured GSOF49 packet from BD940 has LENGTH field set to 0x6d = 109 bytes.
// A captured GSOF49 packet from BD940 has total bytes of 115 bytes.
// Thus, the following 5 bytes are not counted.
// 1) STX
// 2) STATUS
// 3) PACKET TYPE
// 4) LENGTH
// 5) CHECKSUM
// 6) ETX
// This aligns with manual's idea of data bytes:
// "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."
// Thus, for this implementation with single-page single-record per DCOL packet,
// the length is simply the sum of data packet size, the gsof_header size.
const uint8_t length = size + sizeof(gsof_header);
const uint8_t dcol_header[4] {
STX,
STATUS,
PACKET_TYPE,
length
};
// Sum bytes (status + type + length + data bytes) and modulo 256 the summation
// Because it's a uint8, use natural overflow
uint8_t csum = STATUS + PACKET_TYPE + length;
for (size_t i = 0; i < ARRAY_SIZE(gsof_header); i++) {
csum += gsof_header[i];
}
for (size_t i = 0; i < size; i++) {
csum += buf[i];
}
const uint8_t dcol_trailer[2] = {
csum,
ETX
};
write_to_autopilot((char*)dcol_header, sizeof(dcol_header));
write_to_autopilot((char*)gsof_header, sizeof(gsof_header));
write_to_autopilot((char*)buf, size);
write_to_autopilot((char*)dcol_trailer, sizeof(dcol_trailer));
const uint8_t total_size = sizeof(dcol_header) + sizeof(gsof_header) + size + sizeof(dcol_trailer);
// Validate length based on everything but DCOL h
if(dcol_header[3] != total_size - (sizeof(dcol_header) + sizeof(dcol_trailer))) {
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
}
}
uint64_t GPS_Trimble::gsof_pack_double(const double& src)
{
uint64_t dst;
static_assert(sizeof(src) == sizeof(dst), "src and dst must have equal size");
memcpy(&dst, &src, sizeof(dst));
dst = htobe64(dst);
return dst;
}
uint32_t GPS_Trimble::gsof_pack_float(const float& src)
{
uint32_t dst;
static_assert(sizeof(src) == sizeof(dst), "src and dst must have equal size");
memcpy(&dst, &src, sizeof(dst));
dst = htobe32(dst);
return dst;
}
void GPS_Trimble::update_read() {
// Technically, the max command is slightly larger.
// This will only slightly slow the response for packets that big.
char c[MAX_PAYLOAD_SIZE];
const auto n_read = read_from_autopilot(c, MAX_PAYLOAD_SIZE);
if (n_read > 0) {
for (uint8_t i = 0; i < n_read; i++) {
if (dcol_parse(c[i])) {
constexpr uint8_t response[1] = {(uint8_t)Command_Response::ACK};
write_to_autopilot((char*)response, sizeof(response));
}
// TODO handle NACK for failure
}
}
}
#endif // AP_SIM_GPS_TRIMBLE_ENABLED