Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 :
16 : #pragma once
17 : #include "AP_GPS_config.h"
18 : #if AP_GPS_UBLOX_CFGV2_ENABLED
19 : #include <stdint.h>
20 : #include <stddef.h>
21 : #include <string.h>
22 : #include "AP_GPS_UBLOX_CFG_List.h"
23 : #include <AP_Common/AP_Common.h>
24 : #include <endian.h>
25 :
26 :
27 : class AP_GPS_UBLOX;
28 :
29 : // -----------------------------------------------------------------------------
30 : // UBX CFG key size helpers (Bits 30..28 encode the value storage size)
31 : // 0x01: one bit (stored in one byte; only LSB significant)
32 : // 0x02: one byte
33 : // 0x03: two bytes
34 : // 0x04: four bytes
35 : // 0x05: eight bytes
36 : // -----------------------------------------------------------------------------
37 : #define UBX_CFG_KEY_SIZE_CODE_BIT (0x1U)
38 : #define UBX_CFG_KEY_SIZE_CODE_1B (0x2U)
39 : #define UBX_CFG_KEY_SIZE_CODE_2B (0x3U)
40 : #define UBX_CFG_KEY_SIZE_CODE_4B (0x4U)
41 : #define UBX_CFG_KEY_SIZE_CODE_8B (0x5U)
42 :
43 : //---------------------------------------------------------------------------------
44 : // AP_GPS_UBLOX_CFGv2 class used to configure modern ublox units with configuration
45 : // key value pair method of configuration
46 : //----------------------------------------------------------------------------------
47 : class AP_GPS_UBLOX_CFGv2 {
48 : using ConfigKey = AP::UBXConfigKey;
49 : private:
50 : friend class AP_GPS_UBLOX;
51 :
52 : public:
53 : AP_GPS_UBLOX_CFGv2(AP_GPS_UBLOX &_ubx_backend);
54 :
55 : // -----------------------------------------------------------------------------
56 : // UBX key/value blob builder for dynamic configuration. Produces a packed sequence:
57 : // [key:uint32][value:len] repeated for all pairs, where len depends on bits 30..28 of key
58 : // Access results via get_blob() and get_size().
59 : // -----------------------------------------------------------------------------
60 : class UBXPackedCfg {
61 : public:
62 2046 : UBXPackedCfg(uint8_t *buf, size_t capacity) : _buf(buf), _size(0), _capacity(capacity) {}
63 :
64 0 : const uint8_t *get() const { return _buf; }
65 0 : size_t get_size() const { return _size; }
66 :
67 : // Reset the packed config to empty state
68 : void reset();
69 :
70 : // Following templates enforce correct type for key at compile time
71 : // unconditional push
72 : template <ConfigKey Key, typename T>
73 0 : bool push(const T value) {
74 : static_assert(config_key_size(Key) == sizeof(T),
75 : "UBXCfgBlob: invalid type for key");
76 : // do not accept signed types or floating point types
77 : static_assert(std::is_integral<T>::value && !std::is_signed<T>::value,
78 : "UBXCfgBlob: only unsigned integral types supported, cast before push");
79 0 : return push(Key, value);
80 : }
81 :
82 : // conditional push
83 : template <ConfigKey Key, typename T>
84 0 : bool push(const T curr_value, const T desired_value) {
85 0 : if (curr_value != desired_value) {
86 0 : return push<Key>(desired_value);
87 : }
88 0 : return true;
89 : }
90 :
91 : // copy value to dest at index in little-endian byte order
92 0 : static void append_value_le(uint8_t *dest, size_t &index, uint64_t value, size_t size) {
93 : #if __BYTE_ORDER == __LITTLE_ENDIAN
94 0 : memcpy(dest + index, &value, size);
95 : #else
96 : for (size_t i = 0; i < size; i++) {
97 : dest[index + i] = (uint8_t)(value & 0xFF);
98 : value >>= 8;
99 : }
100 : #endif
101 0 : index += size;
102 0 : }
103 :
104 : // get value by key
105 : bool get(ConfigKey key, uint64_t &value) const;
106 :
107 : // set value by key
108 : bool set(ConfigKey key, uint64_t value);
109 :
110 : // get key at byte offset, returns next offset or 0 if end
111 : bool get_key_at_offset(uint16_t &offset, ConfigKey &key) const;
112 :
113 : // Push raw value for a key (length is inferred from key's size bits)
114 : bool push(const ConfigKey key, const uint64_t value);
115 :
116 : void set_size(size_t size) {
117 : if (size <= _capacity) {
118 : _size = size;
119 : }
120 : }
121 : private:
122 : // return size of a config key, or 0 if unknown
123 0 : static constexpr uint8_t config_key_size(ConfigKey key) {
124 : // step over the value
125 0 : return (((uint32_t(key) >> 28) & 0x07) == 0x1) ? 1 : // bit
126 0 : (((uint32_t(key) >> 28) & 0x07) == 0x2) ? 1 : // byte
127 0 : (((uint32_t(key) >> 28) & 0x07) == 0x3) ? 2 : // 2 bytes
128 0 : (((uint32_t(key) >> 28) & 0x07) == 0x4) ? 4 : // 4 bytes
129 0 : (((uint32_t(key) >> 28) & 0x07) == 0x5) ? 8 : // 8 bytes
130 0 : 0; // invalid
131 : }
132 : uint8_t *_buf;
133 : size_t _size{0};
134 : size_t _capacity{0};
135 : };
136 :
137 : enum States : uint8_t {
138 : IDENTIFY_MODULE,
139 : RESET_MODULE,
140 : IDENTIFY_SIGNALS,
141 : CONFIGURE_GNSS,
142 : IDENTIFY_UART_PORT,
143 : FETCH_COMMON_CONFIG,
144 : SET_COMMON_CONFIG,
145 : CONFIGURED,
146 : FAILED,
147 : };
148 :
149 : enum class Module : uint8_t {
150 : // Units with single UART
151 : UNKNOWN,
152 : M9,
153 : M10,
154 : F10,
155 : SINGLE_UART_LAST,
156 : // Units with dual UARTs
157 : NEO_F9,
158 : ZED_F9,
159 : F20,
160 : X20,
161 : };
162 :
163 : // run CFGv2 configuration state machine
164 : void update();
165 :
166 : // process a single byte from a VALGET response payload
167 : void process_valget_byte(uint8_t byte);
168 : // called when VALGET response is fully received or timed out
169 : void process_valget_complete(bool success);
170 : bool valget_active() const { return _valget_in_progress; }
171 :
172 : // handle ACK/NACK for CFG-VALSET/VALDEL messages
173 : void handle_cfg_ack(uint8_t msg_id);
174 : void handle_cfg_nack(uint8_t msg_id);
175 :
176 : // delete all config keys and reset module to defaults
177 : bool delete_and_reset_config();
178 :
179 : static void override_ubx_cfg(uint8_t instance, uint8_t *cfg, size_t size) {
180 : _override_common_cfg[instance] = new UBXPackedCfg(cfg, size);
181 : _override_common_cfg[instance]->set_size(size);
182 : }
183 : private:
184 : bool using_legacy_config() const;
185 : bool skip_cfgv2() const;
186 :
187 : // List of All signals supported by u-blox
188 : // Used for identifying supported signals and configuring them
189 : enum SupportedSignals: uint32_t {
190 : #define SIG_X(NAME, INDEX) NAME##_SIG = INDEX,
191 : UBLOX_SIGNALS(SIG_X)
192 : #undef SIG_X
193 : SIGNALS_LAST
194 : };
195 :
196 : // Constellation names aligned with AP_GPS_UBLOX::GNSS_* indices
197 : static const char* const CONSTELLATION_NAMES[];
198 :
199 : AP_GPS_UBLOX &ubx_backend;
200 : States curr_state;
201 :
202 : uint32_t _last_request_time;
203 :
204 : bool _identify_module();
205 : Module module;
206 :
207 : // bit masks of supported and enabled constellations and signals
208 : uint8_t supported_gnss;
209 : uint8_t enabled_gnss;
210 : uint32_t supported_signals;
211 : uint32_t enabled_signals;
212 :
213 : void _handle_valget_kv(ConfigKey key, uint64_t value, uint8_t value_len);
214 : void _parse_signal_kv(ConfigKey key, uint64_t value, uint8_t value_len);
215 : void _parse_common_cfg(ConfigKey key, uint64_t value, uint8_t value_len);
216 : void _publish_supported_constellations();
217 : // send GNSS constellation enables based on gnss_mode param, returns true if config was sent
218 : bool _send_gnss_cfg();
219 :
220 : // Signal names aligned with UBLOX_SIGNALS indices
221 : static const char* const SIGNAL_NAMES[];
222 : bool _request_common_cfg();
223 : bool set_common_cfg();
224 : bool is_common_cfg_needed();
225 :
226 : // Common config packed buffer management
227 : bool _init_common_cfg_list(bool check_only = false, uint32_t key_to_check = 0);
228 : // push entries from a static const config_list array into _common_cfg
229 : bool _push_cfg_array(const ubx_config_list *entries, uint16_t count, uint16_t &item_index);
230 : // check if a key exists in a static const config_list array
231 : static bool _key_in_array(const ubx_config_list *entries, uint16_t count, uint32_t key);
232 : bool fetch_all_config();
233 :
234 : bool _valget_in_progress;
235 : bool _valget_abort; // if true, ignore all bytes until process_valget_complete()
236 : uint8_t _valget_hdr_count; // bytes of VALGET header seen (version/layers/reserved)
237 : uint8_t _valget_key_count; // bytes of current key seen
238 : uint8_t _valget_value_expected; // expected value length for current key
239 : uint8_t _valget_value_count; // bytes of current value seen
240 : uint8_t _valget_kv_count; // count of key-value pairs in current response
241 : uint16_t _valget_position; // position for next VALGET request (increments by 64)
242 : uint32_t _valget_key; // current key (little-endian)
243 : uint64_t _valget_value; // current value (little-endian)
244 :
245 : // Common config management - single source of truth
246 : static constexpr size_t COMMON_CFG_BUFFER_SIZE = 100;
247 : uint8_t _common_cfg_buffer[COMMON_CFG_BUFFER_SIZE];
248 : UBXPackedCfg _common_cfg;
249 : static UBXPackedCfg *_override_common_cfg[GPS_MAX_INSTANCES];
250 : uint16_t _common_cfg_fetch_index; // tracks progress through config list building
251 :
252 : // Buffer to track mismatched config that needs to be set
253 : uint8_t _common_cfg_to_set_buffer[COMMON_CFG_BUFFER_SIZE];
254 : UBXPackedCfg _common_cfg_to_set;
255 :
256 : enum ConfigLayer : uint8_t {
257 : CFG_LAYER_RAM = 0x00,
258 : CFG_LAYER_BBR = 0x01,
259 : CFG_LAYER_FLASH = 0x02,
260 : CFG_LAYER_DEFAULT = 0x07,
261 : CFG_LAYER_ALL = 0x07,
262 : };
263 :
264 : bool _request_cfg_group(ConfigKey group, ConfigLayer layer);
265 : // send a CFG-VALSET made of repeated [key:uint32][value:N] pairs in 'bytes'
266 : // streams without allocating a new combined buffer
267 : bool _send_valset_bytes(const uint8_t *bytes, uint16_t size, ConfigLayer layers);
268 :
269 : bool _send_reset(uint16_t navBbrMask, uint8_t resetMode);
270 :
271 : static bool _module_reset[GPS_MAX_INSTANCES];
272 : bool _is_reset_required;
273 : ConfigLayer fetch_config_layer = ConfigLayer::CFG_LAYER_BBR;
274 :
275 : bool _single_key_valset; // send one key at a time on NACK fallback
276 : uint16_t _valset_send_offset; // progress through packed buffer in single-key mode
277 : uint8_t portId {0xFF}; // port ID for dual-UART modules, 0xFF means not detected yet
278 : };
279 : #endif // AP_GPS_UBLOX_CFGV2_ENABLED
|
