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 : //
17 : // UAVCAN GPS driver
18 : //
19 : #include "AP_GPS_config.h"
20 :
21 : #if AP_GPS_DRONECAN_ENABLED
22 :
23 : #include <AP_HAL/AP_HAL.h>
24 :
25 : #include "AP_GPS_DroneCAN.h"
26 :
27 : #include <AP_CANManager/AP_CANManager.h>
28 : #include <AP_DroneCAN/AP_DroneCAN.h>
29 : #include <GCS_MAVLink/GCS.h>
30 :
31 : #include <AP_Logger/AP_Logger.h>
32 :
33 : #include <stdio.h>
34 : #include <AP_BoardConfig/AP_BoardConfig.h>
35 :
36 : #define GPS_PPS_EMULATION 0
37 :
38 : extern const AP_HAL::HAL& hal;
39 :
40 : #define GPS_UAVCAN_DEBUGGING 0
41 :
42 : #if GPS_UAVCAN_DEBUGGING
43 : #if defined(HAL_BUILD_AP_PERIPH)
44 : extern "C" {
45 : void can_printf(const char *fmt, ...);
46 : }
47 : # define Debug(fmt, args ...) do {can_printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args);} while(0)
48 : #else
49 : # define Debug(fmt, args ...) do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0)
50 : #endif
51 : #else
52 : # define Debug(fmt, args ...)
53 : #endif
54 :
55 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
56 : #define NATIVE_TIME_OFFSET (AP_HAL::micros64() - AP_HAL::micros64())
57 : #else
58 : #define NATIVE_TIME_OFFSET 0
59 : #endif
60 : AP_GPS_DroneCAN::DetectedModules AP_GPS_DroneCAN::_detected_modules[];
61 : HAL_Semaphore AP_GPS_DroneCAN::_sem_registry;
62 :
63 : // Member Methods
64 0 : AP_GPS_DroneCAN::AP_GPS_DroneCAN(AP_GPS &_gps,
65 : AP_GPS::Params &_params,
66 : AP_GPS::GPS_State &_state,
67 0 : AP_GPS::GPS_Role _role) :
68 : AP_GPS_Backend(_gps, _params, _state, nullptr),
69 0 : interim_state(_state),
70 0 : role(_role)
71 : {
72 0 : param_int_cb = FUNCTOR_BIND_MEMBER(&AP_GPS_DroneCAN::handle_param_get_set_response_int, bool, AP_DroneCAN*, const uint8_t, const char*, int32_t &);
73 0 : param_float_cb = FUNCTOR_BIND_MEMBER(&AP_GPS_DroneCAN::handle_param_get_set_response_float, bool, AP_DroneCAN*, const uint8_t, const char*, float &);
74 0 : param_save_cb = FUNCTOR_BIND_MEMBER(&AP_GPS_DroneCAN::handle_param_save_response, void, AP_DroneCAN*, const uint8_t, bool);
75 0 : }
76 :
77 0 : AP_GPS_DroneCAN::~AP_GPS_DroneCAN()
78 : {
79 0 : WITH_SEMAPHORE(_sem_registry);
80 :
81 0 : _detected_modules[_detected_module].driver = nullptr;
82 :
83 : #if GPS_MOVING_BASELINE
84 0 : if (rtcm3_parser != nullptr) {
85 0 : delete rtcm3_parser;
86 : }
87 : #endif
88 0 : }
89 :
90 5 : bool AP_GPS_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan)
91 : {
92 5 : const auto driver_index = ap_dronecan->get_driver_index();
93 :
94 5 : return (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_fix2_msg_trampoline, driver_index) != nullptr)
95 5 : && (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_aux_msg_trampoline, driver_index) != nullptr)
96 5 : && (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_heading_msg_trampoline, driver_index) != nullptr)
97 5 : && (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_status_msg_trampoline, driver_index) != nullptr)
98 : #if GPS_MOVING_BASELINE
99 5 : && (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_moving_baseline_msg_trampoline, driver_index) != nullptr)
100 10 : && (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_relposheading_msg_trampoline, driver_index) != nullptr)
101 : #endif
102 : ;
103 : }
104 :
105 75 : AP_GPS_Backend* AP_GPS_DroneCAN::probe(AP_GPS &_gps, AP_GPS::GPS_State &_state)
106 : {
107 75 : WITH_SEMAPHORE(_sem_registry);
108 75 : int8_t found_match = -1, last_match = -1;
109 75 : AP_GPS_DroneCAN* backend = nullptr;
110 75 : bool bad_override_config = false;
111 225 : for (int8_t i = GPS_MAX_RECEIVERS - 1; i >= 0; i--) {
112 150 : if (_detected_modules[i].driver == nullptr && _detected_modules[i].ap_dronecan != nullptr) {
113 0 : if (_gps.params[_state.instance].override_node_id != 0 &&
114 0 : _gps.params[_state.instance].override_node_id != _detected_modules[i].node_id) {
115 0 : continue; // This device doesn't match the correct node
116 : }
117 0 : last_match = found_match;
118 0 : for (uint8_t j = 0; j < GPS_MAX_RECEIVERS; j++) {
119 0 : if (_detected_modules[i].node_id == _gps.params[j].override_node_id &&
120 0 : (j != _state.instance)) {
121 : //wrong instance
122 0 : found_match = -1;
123 0 : break;
124 : }
125 0 : found_match = i;
126 : }
127 :
128 : // Handle Duplicate overrides
129 0 : for (uint8_t j = 0; j < GPS_MAX_RECEIVERS; j++) {
130 0 : if (_gps.params[i].override_node_id != 0 && (i != j) &&
131 0 : _gps.params[i].override_node_id == _gps.params[j].override_node_id) {
132 0 : bad_override_config = true;
133 : }
134 : }
135 0 : if (bad_override_config) {
136 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Same Node Id %lu set for multiple GPS", (unsigned long int)_gps.params[i].override_node_id.get());
137 0 : last_match = i;
138 : }
139 :
140 0 : if (found_match == -1) {
141 0 : found_match = last_match;
142 0 : continue;
143 : }
144 0 : break;
145 : }
146 : }
147 :
148 75 : if (found_match == -1) {
149 75 : return NULL;
150 : }
151 : // initialise the backend based on the UAVCAN Moving baseline selection
152 0 : switch (_gps.get_type(_state.instance)) {
153 0 : case AP_GPS::GPS_TYPE_UAVCAN:
154 0 : backend = NEW_NOTHROW AP_GPS_DroneCAN(_gps, _gps.params[_state.instance], _state, AP_GPS::GPS_ROLE_NORMAL);
155 0 : break;
156 : #if GPS_MOVING_BASELINE
157 0 : case AP_GPS::GPS_TYPE_UAVCAN_RTK_BASE:
158 0 : backend = NEW_NOTHROW AP_GPS_DroneCAN(_gps, _gps.params[_state.instance], _state, AP_GPS::GPS_ROLE_MB_BASE);
159 0 : break;
160 0 : case AP_GPS::GPS_TYPE_UAVCAN_RTK_ROVER:
161 0 : backend = NEW_NOTHROW AP_GPS_DroneCAN(_gps, _gps.params[_state.instance], _state, AP_GPS::GPS_ROLE_MB_ROVER);
162 0 : break;
163 : #endif
164 0 : default:
165 0 : return NULL;
166 : }
167 0 : if (backend != nullptr) {
168 0 : _detected_modules[found_match].driver = backend;
169 0 : backend->_detected_module = found_match;
170 0 : snprintf(backend->_name, ARRAY_SIZE(backend->_name), "DroneCAN%u-%u", _detected_modules[found_match].ap_dronecan->get_driver_index()+1, _detected_modules[found_match].node_id);
171 0 : _detected_modules[found_match].instance = _state.instance;
172 0 : for (uint8_t i=0; i < GPS_MAX_RECEIVERS; i++) {
173 0 : if (_detected_modules[found_match].node_id == AP::gps().params[i].node_id) {
174 0 : if (i == _state.instance) {
175 : // Nothing to do here
176 0 : break;
177 : }
178 : // else swap
179 0 : uint8_t tmp = AP::gps().params[_state.instance].node_id.get();
180 0 : AP::gps().params[_state.instance].node_id.set_and_notify(_detected_modules[found_match].node_id);
181 0 : AP::gps().params[i].node_id.set_and_notify(tmp);
182 : }
183 : }
184 : #if GPS_MOVING_BASELINE
185 0 : if (backend->role == AP_GPS::GPS_ROLE_MB_BASE) {
186 0 : backend->rtcm3_parser = NEW_NOTHROW RTCM3_Parser;
187 0 : if (backend->rtcm3_parser == nullptr) {
188 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "DroneCAN%u-%u: failed RTCMv3 parser allocation", _detected_modules[found_match].ap_dronecan->get_driver_index()+1, _detected_modules[found_match].node_id);
189 : }
190 : }
191 : #endif // GPS_MOVING_BASELINE
192 : }
193 :
194 0 : return backend;
195 75 : }
196 :
197 38456 : bool AP_GPS_DroneCAN::inter_instance_pre_arm_checks(char failure_msg[], uint16_t failure_msg_len)
198 : {
199 : // lint parameters and detected node IDs:
200 115362 : for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
201 76909 : const auto ¶ms_i = AP::gps().params[i];
202 : // we are only interested in parameters for DroneCAN GPSs:
203 76909 : if (!is_dronecan_gps_type(params_i.type)) {
204 76902 : continue;
205 : }
206 7 : bool overriden_node_found = false;
207 7 : bool bad_override_config = false;
208 7 : if (params_i.override_node_id == 0) {
209 : //anything goes
210 4 : continue;
211 : }
212 6 : for (uint8_t j = 0; j < GPS_MAX_RECEIVERS; j++) {
213 6 : const auto ¶ms_j = AP::gps().params[j];
214 : // we are only interested in parameters for DroneCAN GPSs:
215 6 : if (!is_dronecan_gps_type(params_j.type)) {
216 0 : continue;
217 : }
218 6 : if (params_i.override_node_id == params_j.override_node_id && (i != j)) {
219 3 : bad_override_config = true;
220 3 : break;
221 : }
222 3 : if (i == _detected_modules[j].instance && _detected_modules[j].driver) {
223 0 : if (params_i.override_node_id == _detected_modules[j].node_id) {
224 0 : overriden_node_found = true;
225 0 : break;
226 : }
227 : }
228 : }
229 3 : if (bad_override_config) {
230 3 : snprintf(failure_msg, failure_msg_len, "Same Node Id %lu set for multiple GPS", (unsigned long int)params_i.override_node_id.get());
231 3 : return false;
232 : }
233 :
234 0 : if (!overriden_node_found) {
235 0 : snprintf(failure_msg, failure_msg_len, "Selected GPS Node %lu not set as instance %d", (unsigned long int)params_i.override_node_id.get(), i + 1);
236 0 : return false;
237 : }
238 : }
239 :
240 38453 : return true;
241 : }
242 :
243 0 : AP_GPS_DroneCAN* AP_GPS_DroneCAN::get_dronecan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id)
244 : {
245 0 : if (ap_dronecan == nullptr) {
246 0 : return nullptr;
247 : }
248 :
249 0 : for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
250 0 : if (_detected_modules[i].driver != nullptr &&
251 0 : _detected_modules[i].ap_dronecan == ap_dronecan &&
252 0 : _detected_modules[i].node_id == node_id) {
253 0 : return _detected_modules[i].driver;
254 : }
255 : }
256 :
257 0 : bool already_detected = false;
258 : // Check if there's an empty spot for possible registeration
259 0 : for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
260 0 : if (_detected_modules[i].ap_dronecan == ap_dronecan && _detected_modules[i].node_id == node_id) {
261 : // Already Detected
262 0 : already_detected = true;
263 0 : break;
264 : }
265 : }
266 0 : if (!already_detected) {
267 0 : for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
268 0 : if (_detected_modules[i].ap_dronecan == nullptr) {
269 0 : _detected_modules[i].ap_dronecan = ap_dronecan;
270 0 : _detected_modules[i].node_id = node_id;
271 : // Just set the Node ID in order of appearance
272 : // This will be used to set select ids
273 0 : AP::gps().params[i].node_id.set_and_notify(node_id);
274 0 : break;
275 : }
276 : }
277 : }
278 : struct DetectedModules tempslot;
279 : // Sort based on the node_id, larger values first
280 : // we do this, so that we have repeatable GPS
281 : // registration
282 0 : for (uint8_t i = 1; i < GPS_MAX_RECEIVERS; i++) {
283 0 : for (uint8_t j = i; j > 0; j--) {
284 0 : if (_detected_modules[j].node_id > _detected_modules[j-1].node_id) {
285 0 : tempslot = _detected_modules[j];
286 0 : _detected_modules[j] = _detected_modules[j-1];
287 0 : _detected_modules[j-1] = tempslot;
288 : // also fix the _detected_module in the driver so that RTCM injection
289 : // can determine if it has the bus to itself
290 0 : if (_detected_modules[j].driver) {
291 0 : _detected_modules[j].driver->_detected_module = j;
292 : }
293 0 : if (_detected_modules[j-1].driver) {
294 0 : _detected_modules[j-1].driver->_detected_module = j-1;
295 : }
296 : }
297 : }
298 : }
299 0 : return nullptr;
300 : }
301 :
302 : /*
303 : handle velocity element of message
304 : */
305 0 : void AP_GPS_DroneCAN::handle_velocity(const float vx, const float vy, const float vz)
306 : {
307 0 : if (!isnan(vx)) {
308 0 : const Vector3f vel(vx, vy, vz);
309 0 : interim_state.velocity = vel;
310 0 : velocity_to_speed_course(interim_state);
311 : // assume we have vertical velocity if we ever get a non-zero Z velocity
312 0 : if (!isnan(vel.z) && !is_zero(vel.z)) {
313 0 : interim_state.have_vertical_velocity = true;
314 : } else {
315 0 : interim_state.have_vertical_velocity = state.have_vertical_velocity;
316 : }
317 : } else {
318 0 : interim_state.have_vertical_velocity = false;
319 : }
320 0 : }
321 :
322 0 : void AP_GPS_DroneCAN::handle_fix2_msg(const uavcan_equipment_gnss_Fix2& msg, uint64_t timestamp_usec)
323 : {
324 0 : bool process = false;
325 0 : seen_fix2 = true;
326 :
327 0 : WITH_SEMAPHORE(sem);
328 :
329 0 : if (msg.status == UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_NO_FIX) {
330 0 : interim_state.status = AP_GPS_FixType::NONE;
331 : } else {
332 0 : if (msg.status == UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_TIME_ONLY) {
333 0 : interim_state.status = AP_GPS_FixType::NONE;
334 0 : } else if (msg.status == UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_2D_FIX) {
335 0 : interim_state.status = AP_GPS_FixType::FIX_2D;
336 0 : process = true;
337 0 : } else if (msg.status == UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_3D_FIX) {
338 0 : interim_state.status = AP_GPS_FixType::FIX_3D;
339 0 : process = true;
340 : }
341 :
342 0 : if (msg.gnss_time_standard == UAVCAN_EQUIPMENT_GNSS_FIX2_GNSS_TIME_STANDARD_UTC) {
343 0 : uint64_t epoch_ms = msg.gnss_timestamp.usec;
344 0 : if (epoch_ms != 0) {
345 0 : epoch_ms /= 1000;
346 0 : uint64_t gps_ms = epoch_ms - UNIX_OFFSET_MSEC;
347 0 : interim_state.time_week = (uint16_t)(gps_ms / AP_MSEC_PER_WEEK);
348 0 : interim_state.time_week_ms = (uint32_t)(gps_ms - (interim_state.time_week) * AP_MSEC_PER_WEEK);
349 : }
350 : }
351 :
352 0 : if (interim_state.status == AP_GPS_FixType::FIX_3D) {
353 0 : if (msg.mode == UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_DGPS) {
354 0 : interim_state.status = AP_GPS_FixType::DGPS;
355 0 : } else if (msg.mode == UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_RTK) {
356 0 : if (msg.sub_mode == UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_RTK_FLOAT) {
357 0 : interim_state.status = AP_GPS_FixType::RTK_FLOAT;
358 0 : } else if (msg.sub_mode == UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_RTK_FIXED) {
359 0 : interim_state.status = AP_GPS_FixType::RTK_FIXED;
360 : }
361 : }
362 : }
363 : }
364 :
365 0 : if (process) {
366 0 : Location loc = { };
367 0 : loc.lat = msg.latitude_deg_1e8 / 10;
368 0 : loc.lng = msg.longitude_deg_1e8 / 10;
369 0 : const int32_t alt_amsl_cm = msg.height_msl_mm / 10;
370 : // if ellipsoid height is not supported by the GPS driver (or always
371 : // with older releases), AP_Periph reports height_ellipsoid_mm == height_msl_mm .
372 : // only trust that we have a valid ellipsoid height if we've ever seen
373 : // it different from msl
374 0 : if (msg.height_msl_mm != msg.height_ellipsoid_mm) {
375 0 : seen_valid_height_ellipsoid = true;
376 : }
377 0 : interim_state.have_undulation = seen_valid_height_ellipsoid;
378 0 : if (seen_valid_height_ellipsoid) {
379 0 : interim_state.undulation = (msg.height_msl_mm - msg.height_ellipsoid_mm) * 0.001;
380 : }
381 0 : interim_state.location = loc;
382 0 : set_alt_amsl_cm(interim_state, alt_amsl_cm);
383 :
384 0 : handle_velocity(msg.ned_velocity[0], msg.ned_velocity[1], msg.ned_velocity[2]);
385 :
386 0 : if (msg.covariance.len == 6) {
387 0 : if (!isnan(msg.covariance.data[0])) {
388 0 : interim_state.horizontal_accuracy = sqrtf(msg.covariance.data[0]);
389 0 : interim_state.have_horizontal_accuracy = true;
390 : } else {
391 0 : interim_state.have_horizontal_accuracy = false;
392 : }
393 0 : if (!isnan(msg.covariance.data[2])) {
394 0 : interim_state.vertical_accuracy = sqrtf(msg.covariance.data[2]);
395 0 : interim_state.have_vertical_accuracy = true;
396 : } else {
397 0 : interim_state.have_vertical_accuracy = false;
398 : }
399 0 : if (!isnan(msg.covariance.data[3]) &&
400 0 : !isnan(msg.covariance.data[4]) &&
401 0 : !isnan(msg.covariance.data[5])) {
402 0 : interim_state.speed_accuracy = sqrtf((msg.covariance.data[3] + msg.covariance.data[4] + msg.covariance.data[5])/3);
403 0 : interim_state.have_speed_accuracy = true;
404 : } else {
405 0 : interim_state.have_speed_accuracy = false;
406 : }
407 : }
408 : } else {
409 0 : interim_state.have_vertical_velocity = false;
410 0 : interim_state.have_vertical_accuracy = false;
411 0 : interim_state.have_horizontal_accuracy = false;
412 0 : interim_state.have_speed_accuracy = false;
413 : }
414 :
415 0 : interim_state.num_sats = msg.sats_used;
416 :
417 0 : if (!seen_aux) {
418 : // if we haven't seen an Aux message then populate vdop and
419 : // hdop from pdop. Some GPS modules don't provide the Aux message
420 0 : interim_state.hdop = interim_state.vdop = msg.pdop * 100.0;
421 : }
422 :
423 0 : if ((msg.timestamp.usec > msg.gnss_timestamp.usec) && (msg.gnss_timestamp.usec > 0)) {
424 : // we have a valid timestamp based on gnss_timestamp timescale, we can use that to correct our gps message time
425 0 : interim_state.last_corrected_gps_time_us = jitter_correction.correct_offboard_timestamp_usec(msg.timestamp.usec, (timestamp_usec + NATIVE_TIME_OFFSET));
426 0 : interim_state.last_gps_time_ms = interim_state.last_corrected_gps_time_us/1000U;
427 0 : interim_state.last_corrected_gps_time_us -= msg.timestamp.usec - msg.gnss_timestamp.usec;
428 : // this is also the time the message was received on the UART on other end.
429 0 : interim_state.corrected_timestamp_updated = true;
430 : } else {
431 0 : interim_state.last_gps_time_ms = jitter_correction.correct_offboard_timestamp_usec(msg.timestamp.usec, timestamp_usec + NATIVE_TIME_OFFSET)/1000U;
432 : }
433 :
434 : #if GPS_PPS_EMULATION
435 : // Emulates a PPS signal, can be used to check how close are we to real GPS time
436 : static virtual_timer_t timeout_vt;
437 : hal.gpio->pinMode(51, 1);
438 : auto handle_timeout = [](void *arg)
439 : {
440 : (void)arg;
441 : //we are called from ISR context
442 : chSysLockFromISR();
443 : hal.gpio->toggle(51);
444 : chSysUnlockFromISR();
445 : };
446 :
447 : static uint64_t next_toggle, last_toggle;
448 :
449 : next_toggle = (msg.timestamp.usec) + (1000000ULL - ((msg.timestamp.usec) % 1000000ULL));
450 :
451 : next_toggle += jitter_correction.get_link_offset_usec();
452 : if (next_toggle != last_toggle) {
453 : chVTSet(&timeout_vt, chTimeUS2I(next_toggle - AP_HAL::micros64()), handle_timeout, nullptr);
454 : last_toggle = next_toggle;
455 : }
456 : #endif
457 :
458 0 : _new_data = true;
459 0 : if (!seen_message) {
460 0 : if (interim_state.status == AP_GPS_FixType::NO_GPS) {
461 : // the first time we see a fix message we change from
462 : // NO_GPS to NO_FIX, indicating to user that a DroneCAN GPS
463 : // has been seen
464 0 : interim_state.status = AP_GPS_FixType::NONE;
465 : }
466 0 : seen_message = true;
467 : }
468 0 : }
469 :
470 0 : void AP_GPS_DroneCAN::handle_aux_msg(const uavcan_equipment_gnss_Auxiliary& msg)
471 : {
472 0 : WITH_SEMAPHORE(sem);
473 :
474 0 : if (!isnan(msg.hdop)) {
475 0 : seen_aux = true;
476 0 : interim_state.hdop = msg.hdop * 100.0;
477 : }
478 :
479 0 : if (!isnan(msg.vdop)) {
480 0 : seen_aux = true;
481 0 : interim_state.vdop = msg.vdop * 100.0;
482 : }
483 0 : }
484 :
485 0 : void AP_GPS_DroneCAN::handle_heading_msg(const ardupilot_gnss_Heading& msg)
486 : {
487 : #if GPS_MOVING_BASELINE
488 0 : if (seen_relposheading && gps.params[interim_state.instance].mb_params.type.get() != 0) {
489 : // we prefer to use the relposheading to get yaw as it allows
490 : // the user to more easily control the relative antenna positions
491 0 : return;
492 : }
493 : #endif
494 :
495 0 : WITH_SEMAPHORE(sem);
496 :
497 0 : if (interim_state.gps_yaw_configured == false) {
498 0 : interim_state.gps_yaw_configured = msg.heading_valid;
499 : }
500 :
501 0 : interim_state.have_gps_yaw = msg.heading_valid;
502 0 : interim_state.gps_yaw = degrees(msg.heading_rad);
503 0 : if (interim_state.have_gps_yaw) {
504 0 : interim_state.gps_yaw_time_ms = AP_HAL::millis();
505 : }
506 : // the Heading message cannot carry the antenna offset the yaw was
507 : // calculated from (and for AP-origin senders such as an AP_Periph
508 : // Heading fallback it may be level-assumed moving baseline yaw), so no
509 : // attitude correction is possible here. Zero the offset so a stale value
510 : // from an earlier relposheading calculation on this instance is never
511 : // applied to it
512 : #if AP_GPS_MB_YAW_OFFSET_ENABLED
513 0 : interim_state.mb_yaw_offset.zero();
514 : #endif
515 :
516 0 : interim_state.have_gps_yaw_accuracy = msg.heading_accuracy_valid;
517 0 : interim_state.gps_yaw_accuracy = degrees(msg.heading_accuracy_rad);
518 0 : }
519 :
520 0 : void AP_GPS_DroneCAN::handle_status_msg(const ardupilot_gnss_Status& msg)
521 : {
522 0 : WITH_SEMAPHORE(sem);
523 :
524 0 : seen_status = true;
525 :
526 0 : healthy = msg.healthy;
527 0 : status_flags = msg.status;
528 0 : if (error_code != msg.error_codes) {
529 : #if HAL_LOGGING_ENABLED
530 0 : AP::logger().Write_MessageF("GPS %d: error changed (0x%08x/0x%08x)",
531 0 : (unsigned int)(state.instance + 1),
532 : error_code,
533 0 : msg.error_codes);
534 : #endif
535 0 : error_code = msg.error_codes;
536 : }
537 0 : }
538 :
539 : #if GPS_MOVING_BASELINE
540 : /*
541 : handle moving baseline data.
542 : */
543 0 : void AP_GPS_DroneCAN::handle_moving_baseline_msg(const ardupilot_gnss_MovingBaselineData& msg, uint8_t node_id)
544 : {
545 0 : WITH_SEMAPHORE(sem);
546 0 : if (role != AP_GPS::GPS_ROLE_MB_BASE) {
547 0 : const uint32_t now_ms = AP_HAL::millis();
548 0 : if (now_ms - last_base_warning_ms > 5000) {
549 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Incorrect Role set for DroneCAN GPS, %d should be Base", node_id);
550 0 : last_base_warning_ms = now_ms;
551 : }
552 0 : return;
553 : }
554 :
555 0 : if (rtcm3_parser == nullptr) {
556 0 : return;
557 : }
558 0 : for (int i=0; i < msg.data.len; i++) {
559 0 : rtcm3_parser->read(msg.data.data[i]);
560 : }
561 0 : }
562 :
563 : /*
564 : handle relposheading message
565 : */
566 0 : void AP_GPS_DroneCAN::handle_relposheading_msg(const ardupilot_gnss_RelPosHeading& msg, uint8_t node_id)
567 : {
568 0 : WITH_SEMAPHORE(sem);
569 :
570 0 : interim_state.gps_yaw_configured = true;
571 0 : seen_relposheading = true;
572 : // push raw heading data to calculate moving baseline heading states
573 0 : if (calculate_moving_base_yaw(interim_state,
574 0 : msg.reported_heading_deg,
575 0 : msg.relative_distance_m,
576 0 : msg.relative_down_pos_m)) {
577 0 : if (msg.reported_heading_acc_available) {
578 0 : interim_state.gps_yaw_accuracy = msg.reported_heading_acc_deg;
579 : }
580 0 : interim_state.have_gps_yaw_accuracy = msg.reported_heading_acc_available;
581 : }
582 0 : }
583 :
584 : // support for retrieving RTCMv3 data from a moving baseline base
585 0 : bool AP_GPS_DroneCAN::get_RTCMV3(const uint8_t *&bytes, uint16_t &len)
586 : {
587 0 : WITH_SEMAPHORE(sem);
588 0 : if (rtcm3_parser != nullptr) {
589 0 : len = rtcm3_parser->get_len(bytes);
590 0 : return len > 0;
591 : }
592 0 : return false;
593 0 : }
594 :
595 : // clear previous RTCM3 packet
596 0 : void AP_GPS_DroneCAN::clear_RTCMV3(void)
597 : {
598 0 : WITH_SEMAPHORE(sem);
599 0 : if (rtcm3_parser != nullptr) {
600 0 : rtcm3_parser->clear_packet();
601 : }
602 0 : }
603 :
604 : #endif // GPS_MOVING_BASELINE
605 :
606 0 : void AP_GPS_DroneCAN::handle_fix2_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_gnss_Fix2& msg)
607 : {
608 0 : WITH_SEMAPHORE(_sem_registry);
609 :
610 0 : AP_GPS_DroneCAN* driver = get_dronecan_backend(ap_dronecan, transfer.source_node_id);
611 0 : if (driver != nullptr) {
612 0 : driver->handle_fix2_msg(msg, transfer.timestamp_usec);
613 : }
614 0 : }
615 :
616 0 : void AP_GPS_DroneCAN::handle_aux_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_gnss_Auxiliary& msg)
617 : {
618 0 : WITH_SEMAPHORE(_sem_registry);
619 :
620 0 : AP_GPS_DroneCAN* driver = get_dronecan_backend(ap_dronecan, transfer.source_node_id);
621 0 : if (driver != nullptr) {
622 0 : driver->handle_aux_msg(msg);
623 : }
624 0 : }
625 :
626 0 : void AP_GPS_DroneCAN::handle_heading_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const ardupilot_gnss_Heading& msg)
627 : {
628 0 : WITH_SEMAPHORE(_sem_registry);
629 :
630 0 : AP_GPS_DroneCAN* driver = get_dronecan_backend(ap_dronecan, transfer.source_node_id);
631 0 : if (driver != nullptr) {
632 0 : driver->handle_heading_msg(msg);
633 : }
634 0 : }
635 :
636 0 : void AP_GPS_DroneCAN::handle_status_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const ardupilot_gnss_Status& msg)
637 : {
638 0 : WITH_SEMAPHORE(_sem_registry);
639 :
640 0 : AP_GPS_DroneCAN* driver = get_dronecan_backend(ap_dronecan, transfer.source_node_id);
641 0 : if (driver != nullptr) {
642 0 : driver->handle_status_msg(msg);
643 : }
644 0 : }
645 :
646 : #if GPS_MOVING_BASELINE
647 : // Moving Baseline msg trampoline
648 0 : void AP_GPS_DroneCAN::handle_moving_baseline_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const ardupilot_gnss_MovingBaselineData& msg)
649 : {
650 0 : WITH_SEMAPHORE(_sem_registry);
651 0 : AP_GPS_DroneCAN* driver = get_dronecan_backend(ap_dronecan, transfer.source_node_id);
652 0 : if (driver != nullptr) {
653 0 : driver->handle_moving_baseline_msg(msg, transfer.source_node_id);
654 : }
655 0 : }
656 :
657 : // RelPosHeading msg trampoline
658 0 : void AP_GPS_DroneCAN::handle_relposheading_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const ardupilot_gnss_RelPosHeading& msg)
659 : {
660 0 : WITH_SEMAPHORE(_sem_registry);
661 0 : AP_GPS_DroneCAN* driver = get_dronecan_backend(ap_dronecan, transfer.source_node_id);
662 0 : if (driver != nullptr) {
663 0 : driver->handle_relposheading_msg(msg, transfer.source_node_id);
664 : }
665 0 : }
666 : #endif
667 :
668 0 : bool AP_GPS_DroneCAN::do_config()
669 : {
670 0 : AP_DroneCAN *ap_dronecan = _detected_modules[_detected_module].ap_dronecan;
671 0 : if (ap_dronecan == nullptr) {
672 0 : return false;
673 : }
674 0 : uint8_t node_id = _detected_modules[_detected_module].node_id;
675 :
676 0 : switch(cfg_step) {
677 0 : case STEP_SET_TYPE:
678 : // GPS_TYPE was renamed GPS1_TYPE. Request both and
679 : // handle whichever we receive.
680 0 : ap_dronecan->get_parameter_on_node(node_id, "GPS_TYPE", ¶m_int_cb);
681 0 : ap_dronecan->get_parameter_on_node(node_id, "GPS1_TYPE", ¶m_int_cb);
682 0 : break;
683 0 : case STEP_SET_MB_CAN_TX:
684 0 : if (role != AP_GPS::GPS_Role::GPS_ROLE_NORMAL) {
685 0 : ap_dronecan->get_parameter_on_node(node_id, "GPS_MB_ONLY_PORT", ¶m_int_cb);
686 : } else {
687 0 : cfg_step++;
688 : }
689 0 : break;
690 0 : case STEP_SAVE_AND_REBOOT:
691 0 : if (requires_save_and_reboot) {
692 0 : ap_dronecan->save_parameters_on_node(node_id, ¶m_save_cb);
693 : } else {
694 0 : cfg_step++;
695 : }
696 0 : break;
697 0 : case STEP_FINISHED:
698 0 : return true;
699 0 : default:
700 0 : break;
701 : }
702 0 : return false;
703 : }
704 :
705 : // Consume new data and mark it received
706 0 : bool AP_GPS_DroneCAN::read(void)
707 : {
708 0 : if (gps._auto_config >= AP_GPS::GPS_AUTO_CONFIG_ENABLE_ALL) {
709 0 : if (!do_config()) {
710 0 : return false;
711 : }
712 : }
713 :
714 0 : WITH_SEMAPHORE(sem);
715 :
716 0 : send_rtcm();
717 :
718 0 : if (_new_data) {
719 0 : _new_data = false;
720 :
721 : // the encoding of accuracies in DroneCAN can result in infinite
722 : // values. These cause problems with blending. Use 1000m and 1000m/s instead
723 0 : interim_state.horizontal_accuracy = MIN(interim_state.horizontal_accuracy, 1000.0);
724 0 : interim_state.vertical_accuracy = MIN(interim_state.vertical_accuracy, 1000.0);
725 0 : interim_state.speed_accuracy = MIN(interim_state.speed_accuracy, 1000.0);
726 :
727 : // prevent announcing multiple times
728 0 : interim_state.announced_detection = state.announced_detection;
729 :
730 0 : state = interim_state;
731 0 : if (interim_state.last_corrected_gps_time_us) {
732 : // If we were able to get a valid last_corrected_gps_time_us
733 : // we have had a valid GPS message time, from which we calculate
734 : // the time of week.
735 0 : _last_itow_ms = interim_state.time_week_ms;
736 0 : _have_itow = true;
737 : }
738 0 : return true;
739 : }
740 0 : if (!seen_message) {
741 : // start with NO_GPS until we get first packet
742 0 : state.status = AP_GPS_FixType::NO_GPS;
743 : }
744 :
745 0 : return false;
746 0 : }
747 :
748 0 : bool AP_GPS_DroneCAN::is_healthy(void) const
749 : {
750 : // if we don't have any health reports, assume it's healthy
751 0 : if (!seen_status) {
752 0 : return true;
753 : }
754 0 : return healthy;
755 : }
756 :
757 0 : bool AP_GPS_DroneCAN::logging_healthy(void) const
758 : {
759 : // if we don't have status, assume it's valid
760 0 : if (!seen_status) {
761 0 : return true;
762 : }
763 :
764 0 : return (status_flags & ARDUPILOT_GNSS_STATUS_STATUS_LOGGING) != 0;
765 : }
766 :
767 0 : bool AP_GPS_DroneCAN::is_configured(void) const
768 : {
769 : // if we don't have status assume it's configured
770 0 : if (!seen_status) {
771 0 : return true;
772 : }
773 :
774 0 : return (status_flags & ARDUPILOT_GNSS_STATUS_STATUS_ARMABLE) != 0;
775 : }
776 :
777 : /*
778 : send pending RTCM data
779 : */
780 0 : void AP_GPS_DroneCAN::send_rtcm(void)
781 : {
782 0 : if (_rtcm_stream.buf == nullptr) {
783 0 : return;
784 : }
785 0 : WITH_SEMAPHORE(sem);
786 :
787 0 : const uint32_t now = AP_HAL::millis();
788 0 : if (now - _rtcm_stream.last_send_ms < 20) {
789 : // don't send more than 50 per second
790 0 : return;
791 : }
792 0 : uint32_t outlen = 0;
793 0 : const uint8_t *ptr = _rtcm_stream.buf->readptr(outlen);
794 0 : if (ptr == nullptr || outlen == 0) {
795 0 : return;
796 : }
797 0 : uavcan_equipment_gnss_RTCMStream msg {};
798 0 : outlen = MIN(outlen, sizeof(msg.data.data));
799 0 : msg.protocol_id = UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_PROTOCOL_ID_RTCM3;
800 0 : memcpy(msg.data.data, ptr, outlen);
801 0 : msg.data.len = outlen;
802 0 : if (_detected_modules[_detected_module].ap_dronecan->rtcm_stream.broadcast(msg)) {
803 0 : _rtcm_stream.buf->advance(outlen);
804 0 : _rtcm_stream.last_send_ms = now;
805 : }
806 0 : }
807 :
808 : /*
809 : handle RTCM data from MAVLink GPS_RTCM_DATA, forwarding it over MAVLink
810 : */
811 0 : void AP_GPS_DroneCAN::inject_data(const uint8_t *data, uint16_t len)
812 : {
813 : // we only handle this if we are the first DroneCAN GPS or we are
814 : // using a different uavcan instance than the first GPS, as we
815 : // send the data as broadcast on all DroneCAN device ports and we
816 : // don't want to send duplicates
817 0 : const uint32_t now_ms = AP_HAL::millis();
818 0 : if (_detected_module == 0 ||
819 0 : _detected_modules[_detected_module].ap_dronecan != _detected_modules[0].ap_dronecan ||
820 0 : now_ms - _detected_modules[0].last_inject_ms > 2000) {
821 0 : if (_rtcm_stream.buf == nullptr) {
822 : // give enough space for a full round from a NTRIP server with all
823 : // constellations
824 0 : _rtcm_stream.buf = NEW_NOTHROW ByteBuffer(2400);
825 0 : if (_rtcm_stream.buf == nullptr) {
826 0 : return;
827 : }
828 : }
829 0 : _detected_modules[_detected_module].last_inject_ms = now_ms;
830 0 : _rtcm_stream.buf->write(data, len);
831 0 : send_rtcm();
832 : }
833 : }
834 :
835 : /*
836 : handle param get/set response
837 : */
838 0 : bool AP_GPS_DroneCAN::handle_param_get_set_response_int(AP_DroneCAN* ap_dronecan, uint8_t node_id, const char* name, int32_t &value)
839 : {
840 : Debug("AP_GPS_DroneCAN: param set/get response from %d %s %ld\n", node_id, name, value);
841 0 : if (((strcmp(name, "GPS_TYPE") == 0) || (strcmp(name, "GPS1_TYPE") == 0)) && (cfg_step == STEP_SET_TYPE)) {
842 0 : if (role == AP_GPS::GPS_ROLE_MB_BASE && value != AP_GPS::GPS_TYPE_UBLOX_RTK_BASE) {
843 0 : value = (int32_t)AP_GPS::GPS_TYPE_UBLOX_RTK_BASE;
844 0 : requires_save_and_reboot = true;
845 0 : return true;
846 0 : } else if (role == AP_GPS::GPS_ROLE_MB_ROVER && value != AP_GPS::GPS_TYPE_UBLOX_RTK_ROVER) {
847 0 : value = (int32_t)AP_GPS::GPS_TYPE_UBLOX_RTK_ROVER;
848 0 : requires_save_and_reboot = true;
849 0 : return true;
850 : } else {
851 0 : cfg_step++;
852 : }
853 : }
854 :
855 0 : if (strcmp(name, "GPS_MB_ONLY_PORT") == 0 && cfg_step == STEP_SET_MB_CAN_TX) {
856 0 : if (option_set(AP_GPS::UAVCAN_MBUseDedicatedBus) && !value) {
857 : // set up so that another CAN port is used for the Moving Baseline Data
858 : // setting this value will allow another CAN port to be used as dedicated
859 : // line for the Moving Baseline Data
860 0 : value = 1;
861 0 : requires_save_and_reboot = true;
862 0 : return true;
863 0 : } else if (!option_set(AP_GPS::UAVCAN_MBUseDedicatedBus) && value) {
864 : // set up so that all CAN ports are used for the Moving Baseline Data
865 0 : value = 0;
866 0 : requires_save_and_reboot = true;
867 0 : return true;
868 : } else {
869 0 : cfg_step++;
870 : }
871 : }
872 0 : return false;
873 : }
874 :
875 0 : bool AP_GPS_DroneCAN::handle_param_get_set_response_float(AP_DroneCAN* ap_dronecan, uint8_t node_id, const char* name, float &value)
876 : {
877 : Debug("AP_GPS_DroneCAN: param set/get response from %d %s %f\n", node_id, name, value);
878 0 : return false;
879 : }
880 :
881 0 : void AP_GPS_DroneCAN::handle_param_save_response(AP_DroneCAN* ap_dronecan, const uint8_t node_id, bool success)
882 : {
883 : Debug("AP_GPS_DroneCAN: param save response from %d %s\n", node_id, success ? "success" : "failure");
884 :
885 0 : if (cfg_step != STEP_SAVE_AND_REBOOT) {
886 0 : return;
887 : }
888 :
889 0 : if (success) {
890 0 : cfg_step++;
891 : }
892 : // Also send reboot command
893 : // this is ok as we are sending from DroneCAN thread context
894 : Debug("AP_GPS_DroneCAN: sending reboot command %d\n", node_id);
895 0 : ap_dronecan->send_reboot_request(node_id);
896 : }
897 :
898 : #if AP_DRONECAN_SEND_GPS
899 0 : bool AP_GPS_DroneCAN::instance_exists(const AP_DroneCAN* ap_dronecan)
900 : {
901 0 : for (uint8_t i=0; i<ARRAY_SIZE(_detected_modules); i++) {
902 0 : if (ap_dronecan == _detected_modules[i].ap_dronecan) {
903 0 : return true;
904 : }
905 : }
906 0 : return false;
907 : }
908 : #endif // AP_DRONECAN_SEND_GPS
909 :
910 : #endif // AP_GPS_DRONECAN_ENABLED
|
