Line data Source code
1 :
2 : /*
3 : This program is free software: you can redistribute it and/or modify
4 : it under the terms of the GNU General Public License as published by
5 : the Free Software Foundation, either version 3 of the License, or
6 : (at your option) any later version.
7 :
8 : This program is distributed in the hope that it will be useful,
9 : but WITHOUT ANY WARRANTY; without even the implied warranty of
10 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11 : GNU General Public License for more details.
12 :
13 : You should have received a copy of the GNU General Public License
14 : along with this program. If not, see <http://www.gnu.org/licenses/>.
15 : */
16 : #include "AP_GPS_UBLOX_CFGv2.h"
17 : #if AP_GPS_UBLOX_CFGV2_ENABLED
18 : #include <AP_HAL/AP_HAL.h>
19 : #include <stdio.h>
20 : #include <AP_GPS/AP_GPS.h>
21 : #include <AP_GPS/AP_GPS_UBLOX.h>
22 : #include <GCS_MAVLink/GCS.h>
23 : #include <AP_Common/AP_Common.h>
24 : #include <AP_Math/AP_Math.h>
25 :
26 : extern const AP_HAL::HAL& hal;
27 :
28 : // debug CFGv2 configuration
29 : #define UBLOX_CFGV2_DEBUGGING 0
30 :
31 : #if UBLOX_CFGV2_DEBUGGING
32 : #if defined(HAL_BUILD_AP_PERIPH)
33 : extern "C" {
34 : void can_printf(const char *fmt, ...);
35 : }
36 : # define Debug(fmt, args ...) do {can_printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args);} while(0)
37 : #else
38 : # define Debug(fmt, args ...) do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0)
39 : #endif
40 : #else
41 : # define Debug(fmt, args ...) do {} while(0)
42 : #endif
43 :
44 : AP_GPS_UBLOX_CFGv2::UBXPackedCfg* AP_GPS_UBLOX_CFGv2::_override_common_cfg[GPS_MAX_INSTANCES] = {};
45 : bool AP_GPS_UBLOX_CFGv2::_module_reset[GPS_MAX_INSTANCES] = {};
46 :
47 : const char* const AP_GPS_UBLOX_CFGv2::CONSTELLATION_NAMES[] = {
48 : "GPS", // 0
49 : "SBAS", // 1
50 : "GAL", // 2
51 : "BDS", // 3
52 : "", // 4 (IMES, unused)
53 : "QZSS", // 5
54 : "", // 6 (GLO, unused)
55 : "NAVIC", // 7
56 : };
57 :
58 : const char* const AP_GPS_UBLOX_CFGv2::SIGNAL_NAMES[] = {
59 : "GPS_L1CA", // 0
60 : "GPS_L2C", // 1
61 : "GPS_L5", // 2
62 : "SBAS_L1CA", // 3
63 : "GAL_E1", // 4
64 : "GAL_E5A", // 5
65 : "GAL_E5B", // 6
66 : "GAL_E6", // 7
67 : "BDS_B1", // 8
68 : "BDS_B1C", // 9
69 : "BDS_B2", // 10
70 : "BDS_B2A", // 11
71 : "BDS_B3", // 12
72 : "QZSS_L1CA", // 13
73 : "QZSS_L1S", // 14
74 : "QZSS_L2C", // 15
75 : "QZSS_L5", // 16
76 : "GLO_L1", // 17
77 : "GLO_L2", // 18
78 : "NAVIC_L5", // 19
79 : };
80 :
81 1023 : AP_GPS_UBLOX_CFGv2::AP_GPS_UBLOX_CFGv2(AP_GPS_UBLOX &_ubx_backend)
82 1023 : : ubx_backend(_ubx_backend),
83 1023 : _common_cfg(_common_cfg_buffer, COMMON_CFG_BUFFER_SIZE),
84 1023 : _common_cfg_to_set(_common_cfg_to_set_buffer, COMMON_CFG_BUFFER_SIZE)
85 : {
86 : static_assert(ARRAY_SIZE(AP_GPS_UBLOX_CFGv2::CONSTELLATION_NAMES) == AP_GPS_UBLOX::GNSS_LAST, "missing constellation name or ubx_gnss_identifier");
87 : static_assert(ARRAY_SIZE(AP_GPS_UBLOX_CFGv2::SIGNAL_NAMES) == SIGNALS_LAST, "missing signal name in SIGNAL_NAMES");
88 1023 : }
89 :
90 11759041 : void AP_GPS_UBLOX_CFGv2::update()
91 : {
92 : // If legacy config is supported and ForceUBXConfigV2 is not set, don't use CFGv2
93 11759041 : if (skip_cfgv2() || curr_state == States::CONFIGURED) {
94 : // Legacy config should be used instead, exit CFGv2
95 81409 : return;
96 : }
97 :
98 : // Check if reset was requested via AUTO_CONFIG parameter (bit 3)
99 11677632 : if ((ubx_backend.gps._auto_config == AP_GPS::GPS_AUTO_CLEAR_CONFIG_UBLOX_SERIAL_ONLY) &&
100 11677632 : (curr_state != States::RESET_MODULE) &&
101 0 : !_module_reset[ubx_backend.state.instance]) {
102 0 : curr_state = States::RESET_MODULE;
103 0 : _last_request_time = 0; // immediate action
104 : }
105 :
106 11677632 : switch (ubx_backend._class) {
107 0 : case AP_GPS_UBLOX::CLASS_ACK:
108 0 : switch (ubx_backend._msg_id) {
109 0 : case AP_GPS_UBLOX::MSG_ACK_ACK:
110 0 : if (ubx_backend._buffer.ack.clsID == AP_GPS_UBLOX::CLASS_CFG) {
111 0 : handle_cfg_ack(ubx_backend._buffer.ack.msgID);
112 : // Debug("GPS %d: ACK for class 0x%02x id 0x%02x",
113 : // ubx_backend.state.instance + 1,
114 : // ubx_backend._buffer.ack.clsID,
115 : // ubx_backend._buffer.ack.msgID);
116 : }
117 0 : break;
118 0 : case AP_GPS_UBLOX::MSG_ACK_NACK:
119 0 : if (ubx_backend._buffer.nack.clsID == AP_GPS_UBLOX::CLASS_CFG) {
120 0 : handle_cfg_nack(ubx_backend._buffer.nack.msgID);
121 : // Debug("GPS %d: NACK for class 0x%02x id 0x%02x",
122 : // ubx_backend.state.instance + 1,
123 : // ubx_backend._buffer.nack.clsID,
124 : // ubx_backend._buffer.nack.msgID);
125 : }
126 0 : break;
127 0 : default:
128 0 : break;
129 : }
130 0 : break;
131 0 : case AP_GPS_UBLOX::CLASS_MON:
132 0 : switch (ubx_backend._msg_id) {
133 0 : case AP_GPS_UBLOX::MSG_MON_COMMS: {
134 : // accept partial packet; only need first 3 bytes to derive outputPort
135 : // bits 4..2 map to output port per u-blox doc: 1=I2C,2=UART1,3=UART2,4=USB,5=SPI
136 : // 4..2 map might not be present in older ublox firmware, so only update if non-zero
137 0 : uint8_t ublox_port = (uint8_t)((ubx_backend._buffer.mon_comms.txErrors >> 2) & 0x07);
138 0 : if (ublox_port != 0) {
139 0 : portId = ublox_port;
140 : }
141 0 : break;
142 : }
143 0 : default:
144 0 : break;
145 : }
146 0 : break;
147 0 : case AP_GPS_UBLOX::CLASS_CFG:
148 0 : switch (ubx_backend._msg_id) {
149 0 : case AP_GPS_UBLOX::MSG_CFG_PRT:
150 : // for modules that don't support MON-COMMS method of port detection
151 : // 0: I2C, 1: UART1, 2: UART2, 3: USB, 4: SPI; add 1 to convert to ublox port numbering
152 0 : portId = ubx_backend._buffer.prt.portID + 1;
153 0 : break;
154 0 : default:
155 0 : break;
156 : }
157 0 : break;
158 11677632 : default:
159 11677632 : break;
160 : }
161 :
162 11677632 : switch (curr_state) {
163 0 : case States::RESET_MODULE:
164 0 : if (AP_HAL::millis() - _last_request_time > 500) {
165 0 : _last_request_time = AP_HAL::millis();
166 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO,
167 : "GPS %d: u-blox resetting to default config",
168 : ubx_backend.state.instance + 1);
169 0 : _valget_position = 0; // reset position for new fetch cycle
170 0 : fetch_all_config();
171 : }
172 0 : break;
173 11677632 : case States::IDENTIFY_MODULE: {
174 11677632 : if (AP_HAL::millis() - _last_request_time > 200) {
175 628620 : _last_request_time = AP_HAL::millis();
176 628620 : ubx_backend._request_version();
177 : }
178 11677632 : if (_identify_module()) {
179 0 : _last_request_time = 0; // immediate next state
180 0 : curr_state = States::IDENTIFY_SIGNALS;
181 : } else {
182 : // stay in this state
183 11677632 : break;
184 : }
185 0 : if (using_legacy_config()) {
186 : // don't fallthrough if legacy config is supported and ForceUBXConfigV2 option is not set, as we want to skip CFGv2 and use legacy config
187 0 : break;
188 : }
189 : }
190 : FALLTHROUGH; // fallthrough to next state
191 : case States::IDENTIFY_SIGNALS:
192 0 : if (AP_HAL::millis() - _last_request_time > 550) {
193 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO,
194 : "GPS %u: u-blox identifying signals",
195 : unsigned(ubx_backend.state.instance) + 1);
196 0 : _last_request_time = AP_HAL::millis();
197 0 : _request_cfg_group(ConfigKey::CFG_SIGNAL_GPS_ENA, ConfigLayer::CFG_LAYER_RAM);
198 : }
199 0 : break;
200 0 : case States::CONFIGURE_GNSS:
201 0 : if (_send_gnss_cfg()) {
202 : // re-identify signals after config change
203 0 : curr_state = States::IDENTIFY_SIGNALS;
204 0 : _last_request_time = AP_HAL::millis();
205 : } else {
206 : // nothing to change, move on
207 0 : curr_state = States::IDENTIFY_UART_PORT;
208 0 : _last_request_time = 0;
209 : }
210 0 : break;
211 0 : case States::IDENTIFY_UART_PORT: {
212 : // transition occurs when ubx_backend sets _ublox_port from MON-COMMS or CFG-PRT
213 0 : if (portId < UBLOX_MAX_PORTS) {
214 0 : curr_state = States::FETCH_COMMON_CONFIG;
215 0 : _common_cfg_fetch_index = 0; // reset for new init
216 0 : _init_common_cfg_list(); // start building config list incrementally
217 0 : _last_request_time = 0; // immediate next state
218 0 : } else if (AP_HAL::millis() - _last_request_time > 250) {
219 0 : _last_request_time = AP_HAL::millis();
220 : // Poll MON-COMMS and CFG-PRT to detect output port interface
221 : // X20P only has MON-COMMS txError for identifying port id
222 : // F9P only has CFG-PRT for identifying port id, as MON-COMMS does not include txError bits that can be used for detection like in X20P, so we need to poll both messages to support both F9 and X20
223 0 : ubx_backend._send_message(AP_GPS_UBLOX::CLASS_CFG, AP_GPS_UBLOX::MSG_CFG_PRT, nullptr, 0);
224 0 : ubx_backend._send_message(AP_GPS_UBLOX::CLASS_MON, AP_GPS_UBLOX::MSG_MON_COMMS, nullptr, 0);
225 0 : break;
226 : } else {
227 0 : break;
228 : }
229 : }
230 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO,
231 : "GPS %u: u-blox fetching config",
232 : unsigned(ubx_backend.state.instance) + 1);
233 : FALLTHROUGH; // fallthrough to next state
234 0 : case States::FETCH_COMMON_CONFIG:
235 0 : if (AP_HAL::millis() - _last_request_time > 200) {
236 0 : _last_request_time = AP_HAL::millis();
237 0 : _request_common_cfg();
238 : }
239 0 : break;
240 0 : case States::SET_COMMON_CONFIG:
241 0 : if (AP_HAL::millis() - _last_request_time > 200) {
242 0 : _last_request_time = AP_HAL::millis();
243 0 : if (set_common_cfg()) {
244 0 : if (_single_key_valset && _valset_send_offset != 0) {
245 : // more keys to send, stay in this state
246 0 : break;
247 : }
248 : // after sending, re-fetch to verify
249 0 : _common_cfg_to_set.reset(); // clear for next check cycle
250 0 : curr_state = States::FETCH_COMMON_CONFIG;
251 : }
252 : }
253 0 : break;
254 0 : default:
255 0 : break;
256 : }
257 : }
258 :
259 11677632 : bool AP_GPS_UBLOX_CFGv2::_identify_module()
260 : {
261 11677632 : if (strlen(ubx_backend._module) != 0) {
262 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO,
263 : "GPS %u: u-blox module: %s",
264 : unsigned(ubx_backend.state.instance) + 1,
265 : ubx_backend._module);
266 :
267 : // check if module contains the model string
268 0 : if (strstr(ubx_backend._module, "M9") != nullptr) {
269 0 : module = Module::M9;
270 : // _hardware_generation defined in AP_GPS_UBLOX.cpp
271 0 : } else if (strstr(ubx_backend._module, "NEO-F9") != nullptr) {
272 0 : module = Module::NEO_F9;
273 : // _hardware_generation defined in AP_GPS_UBLOX.cpp
274 0 : } else if (strstr(ubx_backend._module, "ZED-F9") != nullptr) {
275 0 : module = Module::ZED_F9;
276 : // _hardware_generation defined in AP_GPS_UBLOX.cpp
277 0 : } else if (strstr(ubx_backend._module, "M10") != nullptr) {
278 0 : module = Module::M10;
279 : // _hardware_generation defined in AP_GPS_UBLOX.cpp
280 0 : } else if (strstr(ubx_backend._module, "F10") != nullptr) {
281 0 : module = Module::F10;
282 0 : ubx_backend._hardware_generation = AP_GPS_UBLOX::UBLOX_F10;
283 0 : } else if (strstr(ubx_backend._module, "F20") != nullptr) {
284 0 : module = Module::F20;
285 0 : ubx_backend._hardware_generation = AP_GPS_UBLOX::UBLOX_F20;
286 0 : } else if (strstr(ubx_backend._module, "X20") != nullptr) {
287 0 : module = Module::X20;
288 0 : ubx_backend._hardware_generation = AP_GPS_UBLOX::UBLOX_X20;
289 : } else {
290 0 : module = Module::UNKNOWN;
291 : }
292 : }
293 :
294 : // check protocol version
295 11677632 : if (strlen(ubx_backend._protver) != 0) {
296 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO,
297 : "GPS %u: u-blox protocol: %s",
298 : unsigned(ubx_backend.state.instance) + 1,
299 : ubx_backend._protver);
300 : // convert the version string to a float
301 0 : float protver = atof(ubx_backend._protver);
302 0 : if (protver >= 23.01f) {
303 : // protocol version 23.01 and above supports CFG-VALGET/VALSET
304 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO,
305 : "GPS %u: u-blox protocol supports CFG-VALGET/VALSET",
306 : unsigned(ubx_backend.state.instance) + 1);
307 : } else {
308 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING,
309 : "GPS %u: u-blox protocol too old for CFG-VALGET/VALSET",
310 : unsigned(ubx_backend.state.instance) + 1);
311 : // we cannot proceed with configuration
312 0 : curr_state = States::FAILED;
313 0 : return false;
314 : }
315 0 : if (protver >= 40.00f) {
316 : // protocol version 40.00 and above only support CFG-VALGET/VALSET
317 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO,
318 : "GPS %u: Legacy config unsupported",
319 : unsigned(ubx_backend.state.instance) + 1);
320 0 : ubx_backend._legacy_cfg_supported = false;
321 : }
322 : // move to next state
323 0 : return true;
324 : }
325 :
326 11677632 : return false;
327 : }
328 :
329 : // get value by key
330 0 : bool AP_GPS_UBLOX_CFGv2::UBXPackedCfg::get(ConfigKey Key, uint64_t &value) const {
331 : uint32_t key;
332 : // find key in buffer
333 0 : for (uint16_t i = 0; i < _size; i += sizeof(uint32_t) + config_key_size((ConfigKey)key)) {
334 0 : memcpy(&key, _buf + i, sizeof(key));
335 0 : if (key == Key) {
336 0 : memcpy(&value, _buf + i + sizeof(key), config_key_size(Key));
337 0 : return true;
338 : }
339 : }
340 : // key not found
341 0 : return false;
342 : }
343 :
344 : // set value by key
345 0 : bool AP_GPS_UBLOX_CFGv2::UBXPackedCfg::set(ConfigKey Key, uint64_t value) {
346 : uint32_t key;
347 : // find key in buffer
348 0 : for (uint16_t i = 0; i < _size; i += sizeof(uint32_t) + config_key_size((ConfigKey)key)) {
349 0 : memcpy(&key, _buf + i, sizeof(key));
350 0 : if (key == Key) {
351 0 : const uint8_t len = config_key_size(Key);
352 0 : if (len == 0) {
353 0 : return false;
354 : }
355 0 : size_t index = i + sizeof(uint32_t);
356 0 : append_value_le(_buf, index, value, len);
357 0 : return true;
358 : }
359 : }
360 : // key not found
361 0 : return false;
362 : }
363 :
364 : // get key at byte offset, returns next offset or 0 if end
365 0 : bool AP_GPS_UBLOX_CFGv2::UBXPackedCfg::get_key_at_offset(uint16_t &offset, ConfigKey &key) const {
366 0 : if (offset >= _size) {
367 0 : return false; // end of buffer
368 : }
369 :
370 : // read key (4 bytes little-endian)
371 : uint32_t key_val;
372 0 : memcpy(&key_val, _buf + offset, sizeof(key_val));
373 0 : key = (ConfigKey)key_val;
374 :
375 : // calculate value size from key to get next offset
376 0 : uint8_t value_size = config_key_size(key);
377 0 : if (value_size == 0 || offset + sizeof(uint32_t) + value_size > _size) {
378 0 : return false; // invalid or truncated
379 : }
380 :
381 : // return next offset
382 0 : offset = offset + sizeof(uint32_t) + value_size;
383 0 : return true;
384 : }
385 :
386 : //
387 : // Streaming VALGET byte-wise processing
388 : //
389 0 : void AP_GPS_UBLOX_CFGv2::process_valget_byte(uint8_t byte)
390 : {
391 0 : if (skip_cfgv2()) {
392 0 : return;
393 : }
394 : // We stream parse the VALGET payload only. The driver calls us for each
395 : // payload byte while the outer parser maintains UBX checksums.
396 : // Payload layout (after UBX length field):
397 : // ubx_cfg_valget { uint8 version; uint8 layers; uint8 res[2]; }
398 : // followed by repeated: [key:uint32 little-endian][value:N bytes]
399 : // where N is determined by key bits 30..28.
400 :
401 0 : if (!_valget_in_progress) {
402 : // starting new VALGET payload
403 0 : _valget_in_progress = true;
404 0 : _valget_hdr_count = 0;
405 0 : _valget_key_count = 0;
406 0 : _valget_value_expected = 0;
407 0 : _valget_value_count = 0;
408 0 : _valget_key = 0;
409 0 : _valget_value = 0;
410 0 : _valget_kv_count = 0;
411 : }
412 :
413 0 : if (_valget_abort) {
414 : // aborting this VALGET; ignore all bytes until process_valget_complete()
415 : Debug("GPS %d: valget aborted, ignoring byte 0x%02x",
416 : ubx_backend.state.instance + 1,
417 : byte);
418 0 : return;
419 : }
420 :
421 : // First consume the 4-byte VALGET header
422 0 : if (_valget_hdr_count < 4) {
423 : // We don't need to store header, just count it
424 0 : _valget_hdr_count++;
425 0 : return;
426 : }
427 :
428 : // Read 4-byte key (little-endian)
429 0 : if (_valget_key_count < 4) {
430 0 : _valget_key |= (uint32_t)byte << (8 * _valget_key_count);
431 0 : _valget_key_count++;
432 0 : if (_valget_key_count == 4) {
433 : // determine expected value length from key size bits
434 0 : const uint8_t size_code = (uint8_t)((_valget_key >> 28) & 0x07U);
435 0 : switch (size_code) {
436 0 : case UBX_CFG_KEY_SIZE_CODE_BIT: // 1 bit, stored in 1 byte
437 : case UBX_CFG_KEY_SIZE_CODE_1B:
438 0 : _valget_value_expected = 1; break;
439 0 : case UBX_CFG_KEY_SIZE_CODE_2B:
440 0 : _valget_value_expected = 2; break;
441 0 : case UBX_CFG_KEY_SIZE_CODE_4B:
442 0 : _valget_value_expected = 4; break;
443 0 : case UBX_CFG_KEY_SIZE_CODE_8B:
444 0 : _valget_value_expected = 8; break;
445 0 : default:
446 : // invalid size; abort this VALGET
447 : Debug("GPS %d: valget invalid key size code %u for key 0x%08lx",
448 : ubx_backend.state.instance + 1,
449 : size_code,
450 : (unsigned long)_valget_key);
451 0 : _valget_abort = true;
452 0 : return;
453 : }
454 0 : _valget_value = 0;
455 0 : _valget_value_count = 0;
456 : }
457 0 : return;
458 : }
459 :
460 : // Read value bytes (little-endian)
461 0 : if (_valget_value_count < _valget_value_expected) {
462 0 : _valget_value |= (uint64_t)byte << (8 * _valget_value_count);
463 0 : _valget_value_count++;
464 0 : if (_valget_value_count == _valget_value_expected) {
465 : // Deliver this key/value
466 0 : _handle_valget_kv((ConfigKey)_valget_key, _valget_value, _valget_value_expected);
467 0 : _valget_kv_count++;
468 : // reset for next pair
469 0 : _valget_key = 0;
470 0 : _valget_value = 0;
471 0 : _valget_key_count = 0;
472 0 : _valget_value_expected = 0;
473 0 : _valget_value_count = 0;
474 : }
475 0 : return;
476 : }
477 : }
478 :
479 0 : bool AP_GPS_UBLOX_CFGv2::is_common_cfg_needed()
480 : {
481 : // Simply check if we have any mismatched config that needs to be set
482 0 : return _common_cfg_to_set.get_size() > 0;
483 : }
484 :
485 4875112 : void AP_GPS_UBLOX_CFGv2::process_valget_complete(bool success)
486 : {
487 4875112 : if (skip_cfgv2()) {
488 39687 : return;
489 : }
490 : // Reset state regardless of success; higher-level logic handles retry.
491 4835425 : _valget_in_progress = false;
492 4835425 : _valget_abort = false;
493 :
494 : // Check if we got a full response (64 KV pairs) and need to continue
495 4835425 : if (success && _valget_kv_count >= 64) {
496 : // More config to fetch, increment position and request again
497 0 : _valget_position += 64;
498 0 : if (curr_state == States::RESET_MODULE) {
499 0 : fetch_all_config();
500 : }
501 0 : return;
502 : }
503 :
504 : // Reset position when wildcard response complete (< 64 KV pairs)
505 4835425 : if (success && _valget_kv_count < 64) {
506 0 : _valget_position = 0; // reset for next layer/cycle
507 : }
508 :
509 4835425 : if (success) {
510 0 : switch (curr_state) {
511 0 : case States::RESET_MODULE:
512 0 : if (_is_reset_required) {
513 : // waiting for VALDEL ACK, don't fetch again
514 0 : break;
515 : }
516 0 : if (fetch_config_layer == ConfigLayer::CFG_LAYER_FLASH) {
517 : // no reset needed, move to IDENTIFY_MODULE
518 0 : curr_state = States::IDENTIFY_MODULE;
519 0 : _module_reset[ubx_backend.state.instance] = true;
520 0 : _last_request_time = 0; // immediate next state
521 : } else {
522 0 : fetch_config_layer = ConfigLayer::CFG_LAYER_FLASH;
523 0 : _valget_position = 0; // reset position for new layer
524 0 : fetch_all_config();
525 0 : _last_request_time = 0; // immediate next state
526 : }
527 0 : break;
528 0 : case States::IDENTIFY_SIGNALS:
529 0 : _publish_supported_constellations();
530 0 : curr_state = States::CONFIGURE_GNSS;
531 0 : _last_request_time = 0; // immediate next state
532 0 : break;
533 0 : case States::FETCH_COMMON_CONFIG: {
534 : // Check if current batch had any mismatches
535 0 : if (is_common_cfg_needed()) {
536 : // Mismatches found, need to set config
537 0 : curr_state = States::SET_COMMON_CONFIG;
538 : Debug("Batch has %u mismatches, need to set",
539 : (unsigned)_common_cfg_to_set.get_size());
540 : } else {
541 : // No mismatches in current batch - current configs match!
542 : // Clear the buffer and continue building next batch
543 0 : _common_cfg.reset();
544 0 : _init_common_cfg_list();
545 :
546 : // Check if we're done (no more configs to add)
547 0 : if (_common_cfg.get_size() == 0) {
548 0 : curr_state = States::CONFIGURED;
549 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO,
550 : "GPS %u: u-blox configured",
551 : unsigned(ubx_backend.state.instance) + 1);
552 : } else {
553 : // More configs to verify, stay in FETCH state
554 : Debug("Batch verified, continuing with next batch (%u bytes)", (unsigned)_common_cfg.get_size());
555 : }
556 : }
557 0 : _last_request_time = 0; // immediate next state
558 0 : break;
559 : }
560 0 : default:
561 0 : break;
562 : }
563 : }
564 : }
565 :
566 0 : void AP_GPS_UBLOX_CFGv2::_handle_valget_kv(ConfigKey key, uint64_t value, uint8_t value_len)
567 : {
568 0 : if (curr_state == States::RESET_MODULE) {
569 : // check if the key is in init list or signal key, if not we need a reset
570 0 : if (!_init_common_cfg_list(true, key) && ((key & 0x0FFF0000LU) != (ConfigKey::CFG_SIGNAL_GPS_ENA & 0x0FFF0000LU))) {
571 : // not in init list, ignore
572 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "GPS %u: Found unexpected valget key 0x%08x reset required",
573 : unsigned(ubx_backend.state.instance) + 1,
574 : (unsigned)key);
575 0 : _is_reset_required = true;
576 0 : _valget_abort = true; // stop processing remaining KV pairs
577 0 : delete_and_reset_config();
578 0 : return;
579 : }
580 : // else continue to process normally
581 0 : return;
582 : }
583 : // Debug("GPS %d: valget key 0x%08x value 0x%llx len %u",
584 : // ubx_backend.state.instance + 1,
585 : // (uint32_t)key,
586 : // (uint64_t)value,
587 : // (size_t)value_len);
588 : // First, check for signal-related keys and update masks
589 0 : _parse_signal_kv(key, value, value_len);
590 :
591 : // Capture common configuration when fetching
592 0 : _parse_common_cfg(key, value, value_len);
593 : }
594 :
595 : // Request a configuration group via CFG-VALGET
596 0 : bool AP_GPS_UBLOX_CFGv2::_request_cfg_group(ConfigKey group, ConfigLayer layer)
597 : {
598 : struct PACKED {
599 : struct AP_GPS_UBLOX::ubx_cfg_valget msg;
600 : uint32_t key;
601 0 : } msg {};
602 0 : if (ubx_backend.port->txspace() < (uint16_t)(sizeof(AP_GPS_UBLOX::ubx_header)+sizeof(msg)+2)) {
603 0 : return false;
604 : }
605 0 : msg.msg.version = 0;
606 0 : msg.msg.layers = layer; // ram
607 0 : msg.key = group | 0xFFFFLU;
608 0 : return ubx_backend._send_message(AP_GPS_UBLOX::CLASS_CFG, AP_GPS_UBLOX::MSG_CFG_VALGET, &msg, sizeof(msg));
609 : }
610 :
611 : // send a CFG-VALSET using provided [key:uint32][value:N] pairs without extra allocation
612 0 : bool AP_GPS_UBLOX_CFGv2::_send_valset_bytes(const uint8_t *bytes, uint16_t size, ConfigLayer layers)
613 : {
614 0 : if (bytes == nullptr || size == 0) {
615 0 : return false;
616 : }
617 : // Check tx space for header + fixed valset header (without key) + bytes + checksum
618 0 : const uint16_t payload_len = (uint16_t)(sizeof(AP_GPS_UBLOX::ubx_cfg_valset) - sizeof(uint32_t)) + size;
619 0 : const uint16_t total_len = (uint16_t)(sizeof(AP_GPS_UBLOX::ubx_header) + payload_len + 2U);
620 0 : if (ubx_backend.port->txspace() < total_len) {
621 0 : return false;
622 : }
623 :
624 : // Prepare UBX header
625 0 : AP_GPS_UBLOX::ubx_header header{};
626 0 : header.preamble1 = AP_GPS_UBLOX::PREAMBLE1;
627 0 : header.preamble2 = AP_GPS_UBLOX::PREAMBLE2;
628 0 : header.msg_class = AP_GPS_UBLOX::CLASS_CFG;
629 0 : header.msg_id = AP_GPS_UBLOX::MSG_CFG_VALSET;
630 0 : header.length = payload_len;
631 :
632 : // Prepare VALSET fixed part
633 0 : AP_GPS_UBLOX::ubx_cfg_valset valset{};
634 0 : valset.version = 1;
635 0 : valset.layers = layers;
636 0 : valset.transaction = 0;
637 0 : valset.reserved[0] = 0;
638 :
639 : // Compute checksum over class..length + payload
640 0 : uint8_t ck_a = 0, ck_b = 0;
641 0 : AP_GPS_UBLOX::_update_checksum((uint8_t *)&header.msg_class, sizeof(header)-2, ck_a, ck_b);
642 0 : AP_GPS_UBLOX::_update_checksum((uint8_t *)&valset, sizeof(valset)-sizeof(uint32_t), ck_a, ck_b);
643 0 : AP_GPS_UBLOX::_update_checksum((uint8_t *)bytes, size, ck_a, ck_b);
644 :
645 : // Write in segments
646 0 : ubx_backend.port->write((const uint8_t *)&header, sizeof(header));
647 0 : ubx_backend.port->write((const uint8_t *)&valset, sizeof(valset)-sizeof(uint32_t));
648 0 : ubx_backend.port->write(bytes, size);
649 0 : ubx_backend.port->write(&ck_a, 1);
650 0 : ubx_backend.port->write(&ck_b, 1);
651 0 : return true;
652 : }
653 :
654 : // Map signal CFG keys to SupportedSignals indices and update masks
655 0 : void AP_GPS_UBLOX_CFGv2::_parse_signal_kv(ConfigKey key, uint64_t value, uint8_t value_len)
656 : {
657 0 : if (value_len < 1) {
658 0 : return;
659 : }
660 0 : const bool ena = (value & 0x1U) != 0;
661 :
662 0 : auto set_gnss = [&](uint8_t idx, bool enabled){
663 0 : supported_gnss |= (1U << idx);
664 0 : if (enabled) {
665 0 : enabled_gnss |= (1U << idx);
666 : } else {
667 0 : enabled_gnss &= ~(1U << idx);
668 : }
669 0 : };
670 :
671 0 : auto set_signal = [&](uint32_t idx, bool enabled){
672 0 : supported_signals |= (1U << idx);
673 0 : if (enabled) {
674 0 : enabled_signals |= (1U << idx);
675 : } else {
676 0 : enabled_signals &= ~(1U << idx);
677 : }
678 0 : };
679 :
680 0 : switch (key) {
681 : // Process GNSS enables
682 : #define GNSS_X(NAME, INDEX) case ConfigKey::CFG_SIGNAL_##NAME##_ENA: set_gnss(AP_GPS_UBLOX::GNSS_##NAME, ena); Debug("GPS %d: GNSS %s %d", ubx_backend.state.instance + 1, #NAME, ena); break;
683 0 : UBLOX_GNSS(GNSS_X)
684 : #undef GNSS_X
685 0 : default:
686 0 : break;
687 : }
688 :
689 0 : switch (key) {
690 : // Process Signal enables
691 : #define SIG_X(NAME, INDEX) case ConfigKey::CFG_SIGNAL_##NAME##_ENA: set_signal(INDEX, ena); Debug("GPS %d: Signal %s %d", ubx_backend.state.instance + 1, #NAME, ena); break;
692 0 : UBLOX_SIGNALS(SIG_X)
693 : #undef SIG_X
694 0 : default:
695 0 : break;
696 : }
697 : }
698 :
699 : // send a list of supported constellation name with enabled/disabled status
700 0 : void AP_GPS_UBLOX_CFGv2::_publish_supported_constellations()
701 : {
702 0 : char buf[100] = {};
703 0 : snprintf(buf, sizeof(buf), "GPS %d: GNSS: ", ubx_backend.state.instance + 1);
704 : // UBLOX_GNSS indexes are 0..7 (with 4 unused for IMES), and we track GNSS in 8-bit masks
705 0 : for (uint8_t i = 0; i < 8; i++) {
706 0 : if (!(supported_gnss & (1U << i))) { continue; }
707 0 : const char *name = CONSTELLATION_NAMES[i];
708 0 : if (name[0] == '\0') { continue; }
709 0 : const bool enabled = (enabled_gnss & (1U << i)) != 0;
710 0 : if (strlen(buf) + strlen(name) + 5 < sizeof(buf)) {
711 0 : snprintf(buf + strlen(buf), sizeof(buf) - strlen(buf), "%s[%d] ", name, enabled ? 1 : 0);
712 : }
713 : }
714 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s", buf);
715 :
716 : // Print enabled signal names
717 0 : buf[0] = '\0';
718 0 : snprintf(buf, sizeof(buf), "GPS %d: Sigs: ", ubx_backend.state.instance + 1);
719 0 : const size_t prefix_len = strlen(buf);
720 0 : for (uint8_t i = 0; i < ARRAY_SIZE(SIGNAL_NAMES); i++) {
721 0 : if (SIGNAL_NAMES[i][0] == '\0') { continue; }
722 0 : if (!(enabled_signals & (1U << i))) { continue; }
723 0 : size_t needed = strlen(SIGNAL_NAMES[i]) + 1; // +1 for space
724 0 : if (strlen(buf) + needed >= sizeof(buf)) {
725 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s", buf);
726 0 : buf[0] = '\0';
727 0 : snprintf(buf, sizeof(buf), "GPS %d: +Sigs: ", ubx_backend.state.instance + 1);
728 : }
729 0 : snprintf(buf + strlen(buf), sizeof(buf) - strlen(buf), "%s ", SIGNAL_NAMES[i]);
730 : }
731 0 : if (strlen(buf) > prefix_len) {
732 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s", buf);
733 : }
734 0 : }
735 :
736 : // send GNSS constellation enables based on gnss_mode param
737 : // returns true if a VALSET was sent, false if no changes needed
738 0 : bool AP_GPS_UBLOX_CFGv2::_send_gnss_cfg()
739 : {
740 0 : const uint8_t desired_gnss = (uint8_t)ubx_backend.params.gnss_mode;
741 0 : if (desired_gnss == 0 || desired_gnss == enabled_gnss) {
742 0 : return false;
743 : }
744 :
745 : // mask off unsupported constellations
746 0 : const uint8_t effective_gnss = desired_gnss & supported_gnss;
747 :
748 0 : if (effective_gnss == enabled_gnss) {
749 0 : return false;
750 : }
751 :
752 : // build packed config with constellation enable/disable keys
753 : // each entry is 4-byte key + 1-byte value = 5 bytes, 7 constellations max
754 0 : uint8_t packed_buf[7 * 5] = {};
755 0 : UBXPackedCfg packed_cfg(packed_buf, sizeof(packed_buf));
756 :
757 : #define PUSH_GNSS(NAME, INDEX) \
758 : { \
759 : const uint8_t curr_val = (enabled_gnss & (1U << AP_GPS_UBLOX::GNSS_##NAME)) ? 1U : 0U; \
760 : const uint8_t desired_val = (effective_gnss & (1U << AP_GPS_UBLOX::GNSS_##NAME)) ? 1U : 0U; \
761 : packed_cfg.push<ConfigKey::CFG_SIGNAL_##NAME##_ENA>(curr_val, desired_val); \
762 : }
763 0 : UBLOX_GNSS(PUSH_GNSS)
764 : #undef PUSH_GNSS
765 :
766 0 : if (packed_cfg.get_size() == 0) {
767 0 : return false;
768 : }
769 :
770 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO,
771 : "GPS %u: updating GNSS mode",
772 : unsigned(ubx_backend.state.instance) + 1);
773 :
774 0 : if (_single_key_valset) {
775 : // send each constellation enable/disable as individual VALSET
776 0 : uint16_t offset = 0;
777 0 : while (offset < packed_cfg.get_size()) {
778 0 : uint16_t start = offset;
779 : ConfigKey key;
780 0 : if (!packed_cfg.get_key_at_offset(offset, key)) {
781 0 : break;
782 : }
783 0 : _send_valset_bytes(packed_cfg.get() + start, offset - start, (ConfigLayer)(1U<<CFG_LAYER_RAM));
784 0 : _send_valset_bytes(packed_cfg.get() + start, offset - start, (ConfigLayer)(1U<<CFG_LAYER_FLASH));
785 0 : _send_valset_bytes(packed_cfg.get() + start, offset - start, (ConfigLayer)(1U<<CFG_LAYER_BBR));
786 : }
787 : } else {
788 0 : _send_valset_bytes(packed_cfg.get(), (uint16_t)packed_cfg.get_size(), (ConfigLayer)CFG_LAYER_ALL);
789 : }
790 0 : return true;
791 : }
792 :
793 : // push entries from a config_list array into _common_cfg, tracking position via item_index
794 : // returns false if buffer fills (item_index updated to resume point)
795 0 : bool AP_GPS_UBLOX_CFGv2::_push_cfg_array(const ubx_config_list *entries, uint16_t count, uint16_t &item_index)
796 : {
797 0 : for (uint16_t i = 0; i < count; i++) {
798 0 : if (item_index >= _common_cfg_fetch_index) {
799 0 : if (!_common_cfg.push(entries[i].key, entries[i].value)) {
800 : Debug("Buffer full at item %u, will continue next call", item_index);
801 0 : _common_cfg_fetch_index = item_index;
802 0 : return false;
803 : }
804 : }
805 0 : item_index++;
806 : }
807 0 : return true;
808 : }
809 :
810 : // check if a key exists in a config_list array
811 0 : bool AP_GPS_UBLOX_CFGv2::_key_in_array(const ubx_config_list *entries, uint16_t count, uint32_t key)
812 : {
813 0 : for (uint16_t i = 0; i < count; i++) {
814 0 : if ((uint32_t)entries[i].key == key) {
815 0 : return true;
816 : }
817 : }
818 0 : return false;
819 : }
820 :
821 : // Initialize common config incrementally based on module type
822 : // Uses _common_cfg_fetch_index to track progress across calls
823 : // Stops early if buffer fills and continues on next call
824 : // return is true if key_to_check is found (when check_only is true)
825 : // return is false in all other cases
826 0 : bool AP_GPS_UBLOX_CFGv2::_init_common_cfg_list(bool check_only, uint32_t key_to_check)
827 : {
828 : // On first call, reset buffer
829 0 : if (_common_cfg_fetch_index == 0 && !check_only) {
830 0 : _common_cfg.reset();
831 : Debug("Starting common config list initialization");
832 : }
833 :
834 0 : if (_override_common_cfg[ubx_backend.state.instance] != nullptr) {
835 0 : const auto &override_cfg = _override_common_cfg[ubx_backend.state.instance];
836 : // if check_only is set, just check if key_to_check exists in override
837 0 : if (check_only) {
838 0 : uint16_t i = 0;
839 0 : while (i < override_cfg->get_size()) {
840 : ConfigKey key;
841 0 : if (!override_cfg->get_key_at_offset(i, key)) {
842 : Debug("Invalid or truncated override config at offset %u", i);
843 0 : break; // invalid or truncated
844 : }
845 0 : if ((uint32_t)key == key_to_check) {
846 : Debug("Found override config key 0x%08x", (unsigned)key);
847 0 : return true; // found
848 : }
849 : }
850 : Debug("Override config key 0x%08x not found", (unsigned)key_to_check);
851 0 : return false; // not found
852 : }
853 : // load override config if provided
854 0 : for (uint16_t i = _common_cfg_fetch_index; i < override_cfg->get_size(); ) {
855 : ConfigKey key;
856 0 : if (!override_cfg->get_key_at_offset(i, key)) {
857 : Debug("Invalid or truncated override config at offset %u", i);
858 0 : break; // invalid or truncated
859 : }
860 0 : uint64_t value = 0;
861 0 : if (!override_cfg->get(key, value)) {
862 : Debug("Invalid override config key 0x%08x", (unsigned)key);
863 0 : break; // invalid key
864 : }
865 0 : if (!_common_cfg.push(key, value)) {
866 : Debug("Buffer full at item %u, will continue next call", i);
867 0 : _common_cfg_fetch_index = i;
868 0 : return false; // buffer full
869 : }
870 : }
871 0 : _common_cfg_fetch_index = override_cfg->get_size(); // mark complete
872 0 : return false; // done with override
873 : }
874 :
875 : // Helper macro for check_only path across arrays
876 : #define CHECK_ARRAY(arr) \
877 : if (_key_in_array(arr, ARRAY_SIZE(arr), key_to_check)) { return true; }
878 :
879 : // Helper macro for pushing a single runtime-value entry
880 : #define PUSH_RUNTIME(KEY, VAL) \
881 : if (check_only) { \
882 : if ((uint32_t)ConfigKey::KEY == key_to_check) { return true; } \
883 : } else { \
884 : if (item_index >= _common_cfg_fetch_index) { \
885 : if (!_common_cfg.push(ConfigKey::KEY, (uint32_t)(VAL))) { \
886 : _common_cfg_fetch_index = item_index; \
887 : return false; \
888 : } \
889 : } \
890 : item_index++; \
891 : }
892 :
893 0 : uint16_t item_index = 0;
894 :
895 0 : if (check_only) {
896 : // check_only path: scan arrays for key existence
897 0 : if (module < Module::SINGLE_UART_LAST) {
898 0 : CHECK_ARRAY(config_common_uart)
899 : #if GPS_MOVING_BASELINE
900 0 : if (ubx_backend.role == AP_GPS::GPS_ROLE_MB_BASE) {
901 0 : CHECK_ARRAY(config_MB_Base_uart1)
902 0 : } else if (ubx_backend.role == AP_GPS::GPS_ROLE_MB_ROVER) {
903 0 : CHECK_ARRAY(config_MB_Rover_uart1)
904 : }
905 : #endif
906 : } else {
907 0 : switch (portId) {
908 0 : case 2:
909 0 : CHECK_ARRAY(config_common_uart1)
910 : #if GPS_MOVING_BASELINE
911 0 : if (ubx_backend.role == AP_GPS::GPS_ROLE_MB_BASE && !ubx_backend.mb_use_uart2()) {
912 0 : CHECK_ARRAY(config_MB_Base_uart1)
913 0 : } else if (ubx_backend.role == AP_GPS::GPS_ROLE_MB_BASE && ubx_backend.mb_use_uart2()) {
914 0 : CHECK_ARRAY(config_MB_Base_uart2)
915 0 : } else if (ubx_backend.role == AP_GPS::GPS_ROLE_MB_ROVER) {
916 0 : CHECK_ARRAY(config_MB_Rover_uart1)
917 : }
918 : #endif
919 0 : break;
920 0 : case 3:
921 0 : CHECK_ARRAY(config_common_uart2)
922 : #if GPS_MOVING_BASELINE
923 0 : if (ubx_backend.role == AP_GPS::GPS_ROLE_MB_BASE) {
924 0 : CHECK_ARRAY(config_MB_Base_uart2)
925 0 : } else if (ubx_backend.role == AP_GPS::GPS_ROLE_MB_ROVER) {
926 0 : CHECK_ARRAY(config_MB_Rover_uart2)
927 : }
928 : #endif
929 0 : break;
930 0 : default:
931 0 : break;
932 : }
933 : }
934 : // check runtime keys
935 0 : if (key_to_check == (uint32_t)ConfigKey::CFG_RATE_MEAS ||
936 0 : key_to_check == (uint32_t)ConfigKey::CFG_NAVSPG_DYNMODEL ||
937 0 : key_to_check == (uint32_t)ConfigKey::CFG_NAVSPG_INFIL_MINELEV ||
938 0 : key_to_check == (uint32_t)ConfigKey::CFG_SIGNAL_L5_HEALTH_OVRD ||
939 0 : key_to_check == (uint32_t)ConfigKey::CFG_MSGOUT_UBX_RXM_RAWX_UART1 ||
940 : key_to_check == (uint32_t)ConfigKey::CFG_MSGOUT_UBX_RXM_RAWX_UART2) {
941 0 : return true;
942 : }
943 0 : return false;
944 : }
945 :
946 : // Normal push path: push static arrays then runtime values
947 0 : if (module < Module::SINGLE_UART_LAST) {
948 0 : if (!_push_cfg_array(config_common_uart, ARRAY_SIZE(config_common_uart), item_index)) {
949 0 : return false;
950 : }
951 : #if GPS_MOVING_BASELINE
952 0 : if (ubx_backend.role == AP_GPS::GPS_ROLE_MB_BASE) {
953 0 : if (!_push_cfg_array(config_MB_Base_uart1, ARRAY_SIZE(config_MB_Base_uart1), item_index)) {
954 0 : return false;
955 : }
956 0 : } else if (ubx_backend.role == AP_GPS::GPS_ROLE_MB_ROVER) {
957 0 : if (!_push_cfg_array(config_MB_Rover_uart1, ARRAY_SIZE(config_MB_Rover_uart1), item_index)) {
958 0 : return false;
959 : }
960 : }
961 : #endif
962 : } else {
963 0 : switch (portId) {
964 0 : case 2: // UART1
965 0 : if (!_push_cfg_array(config_common_uart1, ARRAY_SIZE(config_common_uart1), item_index)) {
966 0 : return false;
967 : }
968 : #if GPS_MOVING_BASELINE
969 0 : if (ubx_backend.role == AP_GPS::GPS_ROLE_MB_BASE && !ubx_backend.mb_use_uart2()) {
970 0 : if (!_push_cfg_array(config_MB_Base_uart1, ARRAY_SIZE(config_MB_Base_uart1), item_index)) {
971 0 : return false;
972 : }
973 0 : } else if (ubx_backend.role == AP_GPS::GPS_ROLE_MB_BASE && ubx_backend.mb_use_uart2()) {
974 0 : if (!_push_cfg_array(config_MB_Base_uart2, ARRAY_SIZE(config_MB_Base_uart2), item_index)) {
975 0 : return false;
976 : }
977 0 : } else if (ubx_backend.role == AP_GPS::GPS_ROLE_MB_ROVER) {
978 0 : if (!_push_cfg_array(config_MB_Rover_uart1, ARRAY_SIZE(config_MB_Rover_uart1), item_index)) {
979 0 : return false;
980 : }
981 : }
982 : #endif
983 0 : break;
984 0 : case 3: // UART2
985 0 : if (!_push_cfg_array(config_common_uart2, ARRAY_SIZE(config_common_uart2), item_index)) {
986 0 : return false;
987 : }
988 : #if GPS_MOVING_BASELINE
989 0 : if (ubx_backend.role == AP_GPS::GPS_ROLE_MB_BASE) {
990 0 : if (!_push_cfg_array(config_MB_Base_uart2, ARRAY_SIZE(config_MB_Base_uart2), item_index)) {
991 0 : return false;
992 : }
993 0 : } else if (ubx_backend.role == AP_GPS::GPS_ROLE_MB_ROVER) {
994 0 : if (!_push_cfg_array(config_MB_Rover_uart2, ARRAY_SIZE(config_MB_Rover_uart2), item_index)) {
995 0 : return false;
996 : }
997 : }
998 : #endif
999 0 : break;
1000 0 : default:
1001 0 : break;
1002 : }
1003 : }
1004 :
1005 : // Runtime-value entries
1006 0 : PUSH_RUNTIME(CFG_RATE_MEAS, ubx_backend.params.rate_ms)
1007 0 : PUSH_RUNTIME(CFG_NAVSPG_DYNMODEL, ubx_backend.gps._navfilter)
1008 :
1009 0 : if (ubx_backend.gps._min_elevation != -100) {
1010 0 : PUSH_RUNTIME(CFG_NAVSPG_INFIL_MINELEV, ubx_backend.gps._min_elevation)
1011 : }
1012 :
1013 0 : if (ubx_backend.gps.option_set(AP_GPS::GPSL5HealthOverride)) {
1014 0 : PUSH_RUNTIME(CFG_SIGNAL_L5_HEALTH_OVRD, 1)
1015 : }
1016 :
1017 : // Raw logging message configuration
1018 : {
1019 0 : const uint8_t raw_rate = ubx_backend.gps._raw_data;
1020 0 : if (module < Module::SINGLE_UART_LAST) {
1021 0 : PUSH_RUNTIME(CFG_MSGOUT_UBX_RXM_RAWX_UART1, raw_rate)
1022 : } else {
1023 0 : switch (portId) {
1024 0 : case 2: // UART1
1025 0 : PUSH_RUNTIME(CFG_MSGOUT_UBX_RXM_RAWX_UART1, raw_rate)
1026 0 : PUSH_RUNTIME(CFG_MSGOUT_UBX_RXM_RAWX_UART2, 0)
1027 0 : break;
1028 0 : case 3: // UART2
1029 0 : PUSH_RUNTIME(CFG_MSGOUT_UBX_RXM_RAWX_UART2, raw_rate)
1030 0 : PUSH_RUNTIME(CFG_MSGOUT_UBX_RXM_RAWX_UART1, 0)
1031 0 : break;
1032 0 : default:
1033 0 : break;
1034 : }
1035 : }
1036 : }
1037 :
1038 : // Mark complete
1039 0 : _common_cfg_fetch_index = item_index;
1040 : Debug("Common config list initialization complete: %u items, %u bytes",
1041 : item_index, (unsigned)_common_cfg.get_size());
1042 :
1043 : #undef CHECK_ARRAY
1044 : #undef PUSH_RUNTIME
1045 0 : return false;
1046 : }
1047 :
1048 : // Request configuration keys from packed buffer starting from _common_cfg_verified_offset
1049 : // Extracts keys using get_key_at_offset and sends VALGET request for next batch
1050 0 : bool AP_GPS_UBLOX_CFGv2::_request_common_cfg()
1051 : {
1052 0 : if (_common_cfg.get_size() == 0) {
1053 : Debug("No config to request");
1054 0 : return false;
1055 : }
1056 :
1057 : // Build VALGET message with keys from packed buffer starting at verified offset
1058 : struct PACKED {
1059 : AP_GPS_UBLOX::ubx_cfg_valget msg;
1060 : uint32_t keys[COMMON_CFG_BUFFER_SIZE/5]; // max keys we should fit
1061 : } valget_msg;
1062 :
1063 0 : valget_msg.msg.version = 0;
1064 0 : valget_msg.msg.layers = 0; // RAM layer
1065 0 : valget_msg.msg.position = 0;
1066 :
1067 : // Extract all keys from current _common_cfg buffer
1068 0 : uint16_t key_count = 0;
1069 0 : uint16_t offset = 0;
1070 : ConfigKey key;
1071 :
1072 0 : while (offset < _common_cfg.get_size() && key_count < ARRAY_SIZE(valget_msg.keys)) {
1073 0 : if (!_common_cfg.get_key_at_offset(offset, key)) {
1074 : Debug("invalid or truncated config at offset %u", offset);
1075 0 : break; // invalid or truncated
1076 : }
1077 0 : valget_msg.keys[key_count++] = (uint32_t)key;
1078 : }
1079 :
1080 0 : if (key_count == 0) {
1081 : Debug("No config keys in buffer");
1082 0 : return false;
1083 : }
1084 :
1085 0 : size_t msg_size = sizeof(valget_msg.msg) + (key_count * sizeof(uint32_t));
1086 :
1087 0 : if (ubx_backend.port->txspace() < (uint16_t)(sizeof(AP_GPS_UBLOX::ubx_header) + msg_size + 2)) {
1088 0 : return false;
1089 : }
1090 :
1091 : Debug("Requesting %u config keys", key_count);
1092 0 : return ubx_backend._send_message(AP_GPS_UBLOX::CLASS_CFG, AP_GPS_UBLOX::MSG_CFG_VALGET, &valget_msg, msg_size);
1093 : }
1094 :
1095 : // Parse common config from VALGET response
1096 : // Compares received value against expected value in _common_cfg
1097 : // Adds mismatched key-value pairs to _common_cfg_to_set
1098 0 : void AP_GPS_UBLOX_CFGv2::_parse_common_cfg(ConfigKey key, uint64_t value, uint8_t value_len)
1099 : {
1100 : // Get expected value from packed config
1101 0 : uint64_t expected_value = 0;
1102 0 : if (!_common_cfg.get(key, expected_value)) {
1103 0 : return;
1104 : }
1105 :
1106 : // Compare received value with expected value
1107 0 : if (memcmp(&value, &expected_value, value_len) != 0) {
1108 : Debug("Config mismatch for key 0x%08x: got 0x%llx, expected 0x%llx",
1109 : (unsigned)key, (unsigned long long)value, (unsigned long long)expected_value);
1110 :
1111 : // Add expected value to the "to set" buffer
1112 : // push() already handles size inference from the key
1113 0 : _common_cfg_to_set.push(key, expected_value);
1114 :
1115 : // Values don't match, need to send VALSET
1116 0 : curr_state = States::SET_COMMON_CONFIG;
1117 : }
1118 : }
1119 :
1120 : // Set common config by sending VALSET with mismatched key-value pairs from _common_cfg_to_set
1121 0 : bool AP_GPS_UBLOX_CFGv2::set_common_cfg()
1122 : {
1123 0 : if (_common_cfg_to_set.get_size() == 0) {
1124 : Debug("No config mismatches to set");
1125 0 : return false;
1126 : }
1127 :
1128 0 : if (_single_key_valset) {
1129 : // send one key-value pair at a time
1130 : ConfigKey key;
1131 0 : uint16_t start = _valset_send_offset;
1132 0 : if (!_common_cfg_to_set.get_key_at_offset(_valset_send_offset, key)) {
1133 : // done sending all keys
1134 0 : _valset_send_offset = 0;
1135 0 : return true;
1136 : }
1137 0 : uint16_t entry_size = _valset_send_offset - start;
1138 : Debug("Setting single key 0x%08lx", (unsigned long)key);
1139 0 : return (_send_valset_bytes(_common_cfg_to_set.get() + start, entry_size, (ConfigLayer)(1U<<CFG_LAYER_RAM)) &&
1140 0 : _send_valset_bytes(_common_cfg_to_set.get() + start, entry_size, (ConfigLayer)(1U<<CFG_LAYER_FLASH)) &&
1141 0 : _send_valset_bytes(_common_cfg_to_set.get() + start, entry_size, (ConfigLayer)(1U<<CFG_LAYER_BBR)));
1142 : }
1143 :
1144 : // Send VALSET with all mismatched key-value pairs
1145 0 : const uint8_t *bytes = _common_cfg_to_set.get();
1146 0 : const uint16_t size = (uint16_t)_common_cfg_to_set.get_size();
1147 :
1148 : Debug("Setting %u bytes of config", size);
1149 0 : return _send_valset_bytes(bytes, size, (ConfigLayer)CFG_LAYER_ALL);
1150 : }
1151 :
1152 0 : void AP_GPS_UBLOX_CFGv2::handle_cfg_ack(uint8_t msg_id)
1153 : {
1154 0 : switch (msg_id) {
1155 0 : case AP_GPS_UBLOX::MSG_CFG_VALDEL:
1156 0 : if (curr_state == States::RESET_MODULE) {
1157 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "GPS %u: CFG-VALDEL acknowledged, resetting module",
1158 : unsigned(ubx_backend.state.instance) + 1);
1159 : // Send controlled software reset
1160 0 : _send_reset(0x0000, AP_GPS_UBLOX::RESET_SW_CONTROLLED);
1161 : // move back to SERIAL ONLY Config and transition to IDENTIFY_MODULE
1162 0 : _module_reset[ubx_backend.state.instance] = true;
1163 0 : curr_state = States::IDENTIFY_MODULE;
1164 0 : _last_request_time = AP_HAL::millis();
1165 : }
1166 0 : break;
1167 0 : default:
1168 0 : break;
1169 : }
1170 0 : }
1171 :
1172 0 : void AP_GPS_UBLOX_CFGv2::handle_cfg_nack(uint8_t msg_id)
1173 : {
1174 0 : switch (msg_id) {
1175 0 : case AP_GPS_UBLOX::MSG_CFG_VALGET:
1176 0 : if (curr_state == States::RESET_MODULE) {
1177 0 : if (fetch_config_layer == ConfigLayer::CFG_LAYER_BBR) {
1178 : // try FLASH layer
1179 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "GPS %u: CFG-VALGET NACK for BBR, trying FLASH",
1180 : unsigned(ubx_backend.state.instance) + 1);
1181 0 : fetch_config_layer = ConfigLayer::CFG_LAYER_FLASH;
1182 0 : _valget_position = 0;
1183 0 : fetch_all_config();
1184 0 : } else if (!_is_reset_required) {
1185 : // FLASH layer also not supported, skip reset check
1186 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "GPS %u: CFG-VALGET NACK for FLASH, skipping reset check",
1187 : unsigned(ubx_backend.state.instance) + 1);
1188 0 : curr_state = States::IDENTIFY_MODULE;
1189 0 : _module_reset[ubx_backend.state.instance] = true;
1190 0 : _last_request_time = 0; // immediate next state
1191 : }
1192 : }
1193 0 : break;
1194 0 : case AP_GPS_UBLOX::MSG_CFG_VALSET:
1195 : // VALSET failed, fall back to single-key mode
1196 0 : if (!_single_key_valset) {
1197 0 : _single_key_valset = true;
1198 0 : _valset_send_offset = 0;
1199 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "GPS %u: VALSET NACK, falling back to single key setting",
1200 : unsigned(ubx_backend.state.instance) + 1);
1201 : }
1202 0 : break;
1203 0 : default:
1204 0 : break;
1205 : }
1206 0 : }
1207 :
1208 0 : bool AP_GPS_UBLOX_CFGv2::fetch_all_config()
1209 : {
1210 : // request all config by sending VALGET with key=0xFFFFFFFF
1211 : struct PACKED {
1212 : AP_GPS_UBLOX::ubx_cfg_valget msg;
1213 : uint32_t key;
1214 0 : } msg {};
1215 0 : if (ubx_backend.port->txspace() < 2*((uint16_t)(sizeof(AP_GPS_UBLOX::ubx_header)+sizeof(msg)+2))) {
1216 0 : return false;
1217 : }
1218 :
1219 : // don't send if a VALGET is already in progress
1220 0 : if (_valget_in_progress) {
1221 0 : return false;
1222 : }
1223 :
1224 0 : msg.msg.version = 0;
1225 0 : msg.msg.layers = fetch_config_layer;
1226 0 : msg.msg.position = _valget_position;
1227 0 : msg.key = 0x0FFF0000; // all keys
1228 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "GPS %u: Requesting all config (%s) pos=%u", unsigned(ubx_backend.state.instance) + 1,
1229 : (fetch_config_layer == ConfigLayer::CFG_LAYER_FLASH) ? "FLASH" : "BBR", _valget_position);
1230 0 : return ubx_backend._send_message(AP_GPS_UBLOX::CLASS_CFG, AP_GPS_UBLOX::MSG_CFG_VALGET, &msg, sizeof(msg));
1231 : }
1232 :
1233 0 : bool AP_GPS_UBLOX_CFGv2::delete_and_reset_config()
1234 : {
1235 : static constexpr struct PACKED {
1236 : AP_GPS_UBLOX::ubx_cfg_valdel msg;
1237 : uint32_t key;
1238 : } valdel_all = {
1239 : .msg = {
1240 : .version = 0x01,
1241 : .layers = (1U<<CFG_LAYER_BBR) | (1U<<CFG_LAYER_FLASH),
1242 : .transaction = 0x00,
1243 : .reserved0 = 0x00
1244 : },
1245 : .key = 0x0FFF0000 // all keys
1246 : };
1247 :
1248 0 : if (ubx_backend.port->txspace() < (uint16_t)(sizeof(AP_GPS_UBLOX::ubx_header) + sizeof(valdel_all) + 2)) {
1249 0 : return false;
1250 : }
1251 :
1252 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING,
1253 : "GPS %u: Deleting all config and resetting",
1254 : unsigned(ubx_backend.state.instance) + 1);
1255 :
1256 0 : return ubx_backend._send_message(AP_GPS_UBLOX::CLASS_CFG,
1257 : AP_GPS_UBLOX::MSG_CFG_VALDEL,
1258 : &valdel_all,
1259 0 : sizeof(valdel_all));
1260 : }
1261 :
1262 0 : bool AP_GPS_UBLOX_CFGv2::_send_reset(uint16_t navBbrMask, uint8_t resetMode)
1263 : {
1264 0 : AP_GPS_UBLOX::ubx_cfg_rst rst = {
1265 : .navBbrMask = navBbrMask,
1266 : .resetMode = resetMode,
1267 : .reserved0 = 0
1268 0 : };
1269 :
1270 0 : if (ubx_backend.port->txspace() < (uint16_t)(sizeof(AP_GPS_UBLOX::ubx_header) + sizeof(rst) + 2)) {
1271 0 : return false;
1272 : }
1273 :
1274 : Debug("GPS %d: Sending reset: navBbrMask=0x%04x resetMode=0x%02x",
1275 : ubx_backend.state.instance + 1,
1276 : navBbrMask,
1277 : resetMode);
1278 :
1279 0 : return ubx_backend._send_message(AP_GPS_UBLOX::CLASS_CFG,
1280 : AP_GPS_UBLOX::MSG_CFG_RST,
1281 : &rst,
1282 0 : sizeof(rst));
1283 : }
1284 :
1285 17325959 : bool AP_GPS_UBLOX_CFGv2::using_legacy_config() const
1286 : {
1287 : // if legacy config is supported and ForceUBXConfigV2 option is not set, use legacy config
1288 17325959 : return ubx_backend._legacy_cfg_supported && !ubx_backend.gps.option_set(AP_GPS::DriverOptions::ForceUBXConfigV2);
1289 : }
1290 :
1291 16634153 : bool AP_GPS_UBLOX_CFGv2::skip_cfgv2() const
1292 : {
1293 33268306 : return (using_legacy_config() && (curr_state > States::RESET_MODULE)) ||
1294 33268306 : (ubx_backend.gps._auto_config == AP_GPS::GPS_AUTO_CONFIG_DISABLE);
1295 : }
1296 :
1297 : // Push raw value for a key (length is inferred from key's size bits)
1298 0 : bool AP_GPS_UBLOX_CFGv2::UBXPackedCfg::push(ConfigKey key, uint64_t value)
1299 : {
1300 : Debug("GPS: try packed push key 0x%08lx value 0x%llx", (unsigned long)key, (unsigned long long)value);
1301 0 : size_t len = config_key_size(key);
1302 0 : if (_size + sizeof(key) + len > _capacity) {
1303 : Debug("GPS: packed push failed, not enough space");
1304 0 : return false;
1305 : }
1306 0 : size_t index = _size;
1307 :
1308 0 : append_value_le(_buf, index, (uint32_t)key, sizeof(uint32_t));
1309 0 : append_value_le(_buf, index, value, len);
1310 : Debug("GPS: packed push succeeded, new size %u", (unsigned)index);
1311 0 : _size = index;
1312 0 : return true;
1313 : }
1314 :
1315 0 : void AP_GPS_UBLOX_CFGv2::UBXPackedCfg::reset() {
1316 : Debug("GPS: packed config reset");
1317 0 : _size = 0;
1318 0 : }
1319 :
1320 : #endif
|
