From 258158b0bcd566c370fd69dc76596171b6fb7322 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 8 Apr 2023 13:53:24 +1000 Subject: [PATCH] AP_GPS: rename UAVCAN drivers to DroneCAN --- libraries/AP_GPS/AP_GPS_DroneCAN.cpp | 848 +++++++++++++++++++++++++++ libraries/AP_GPS/AP_GPS_DroneCAN.h | 142 +++++ 2 files changed, 990 insertions(+) create mode 100644 libraries/AP_GPS/AP_GPS_DroneCAN.cpp create mode 100644 libraries/AP_GPS/AP_GPS_DroneCAN.h diff --git a/libraries/AP_GPS/AP_GPS_DroneCAN.cpp b/libraries/AP_GPS/AP_GPS_DroneCAN.cpp new file mode 100644 index 0000000000..8ac0ca6d65 --- /dev/null +++ b/libraries/AP_GPS/AP_GPS_DroneCAN.cpp @@ -0,0 +1,848 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +// +// UAVCAN GPS driver +// +#include + +#if HAL_ENABLE_LIBUAVCAN_DRIVERS +#include "AP_GPS_UAVCAN.h" + +#include +#include +#include + +#include + +#include +#include + +#define GPS_PPS_EMULATION 0 + +extern const AP_HAL::HAL& hal; + +#define GPS_UAVCAN_DEBUGGING 0 + +#if GPS_UAVCAN_DEBUGGING +#if defined(HAL_BUILD_AP_PERIPH) + extern "C" { + void can_printf(const char *fmt, ...); + } + # define Debug(fmt, args ...) do {can_printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args);} while(0) +#else + # define Debug(fmt, args ...) do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0) +#endif +#else + # define Debug(fmt, args ...) +#endif + +#define LOG_TAG "GPS" + +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#define NATIVE_TIME_OFFSET (AP_HAL::micros64() - AP_HAL::native_micros64()) +#else +#define NATIVE_TIME_OFFSET 0 +#endif +AP_GPS_UAVCAN::DetectedModules AP_GPS_UAVCAN::_detected_modules[]; +HAL_Semaphore AP_GPS_UAVCAN::_sem_registry; + +// Member Methods +AP_GPS_UAVCAN::AP_GPS_UAVCAN(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_GPS::GPS_Role _role) : + AP_GPS_Backend(_gps, _state, nullptr), + interim_state(_state), + role(_role) +{ + param_int_cb = FUNCTOR_BIND_MEMBER(&AP_GPS_UAVCAN::handle_param_get_set_response_int, bool, AP_DroneCAN*, const uint8_t, const char*, int32_t &); + param_float_cb = FUNCTOR_BIND_MEMBER(&AP_GPS_UAVCAN::handle_param_get_set_response_float, bool, AP_DroneCAN*, const uint8_t, const char*, float &); + param_save_cb = FUNCTOR_BIND_MEMBER(&AP_GPS_UAVCAN::handle_param_save_response, void, AP_DroneCAN*, const uint8_t, bool); +} + +AP_GPS_UAVCAN::~AP_GPS_UAVCAN() +{ + WITH_SEMAPHORE(_sem_registry); + + _detected_modules[_detected_module].driver = nullptr; + +#if GPS_MOVING_BASELINE + if (rtcm3_parser != nullptr) { + delete rtcm3_parser; + } +#endif +} + +void AP_GPS_UAVCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan) +{ + if (ap_dronecan == nullptr) { + return; + } + + if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_fix2_msg_trampoline, ap_dronecan->get_driver_index()) == nullptr) { + AP_BoardConfig::allocation_error("status_sub"); + } + + if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_aux_msg_trampoline, ap_dronecan->get_driver_index()) == nullptr) { + AP_BoardConfig::allocation_error("status_sub"); + } + + if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_heading_msg_trampoline, ap_dronecan->get_driver_index()) == nullptr) { + AP_BoardConfig::allocation_error("status_sub"); + } + + if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_status_msg_trampoline, ap_dronecan->get_driver_index()) == nullptr) { + AP_BoardConfig::allocation_error("status_sub"); + } +#if GPS_MOVING_BASELINE + if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_moving_baseline_msg_trampoline, ap_dronecan->get_driver_index()) == nullptr) { + AP_BoardConfig::allocation_error("moving_baseline_sub"); + } + + if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_relposheading_msg_trampoline, ap_dronecan->get_driver_index()) == nullptr) { + AP_BoardConfig::allocation_error("relposheading_sub"); + } +#endif +} + +AP_GPS_Backend* AP_GPS_UAVCAN::probe(AP_GPS &_gps, AP_GPS::GPS_State &_state) +{ + WITH_SEMAPHORE(_sem_registry); + int8_t found_match = -1, last_match = -1; + AP_GPS_UAVCAN* backend = nullptr; + bool bad_override_config = false; + for (int8_t i = GPS_MAX_RECEIVERS - 1; i >= 0; i--) { + if (_detected_modules[i].driver == nullptr && _detected_modules[i].ap_dronecan != nullptr) { + if (_gps._override_node_id[_state.instance] != 0 && + _gps._override_node_id[_state.instance] != _detected_modules[i].node_id) { + continue; // This device doesn't match the correct node + } + last_match = found_match; + for (uint8_t j = 0; j < GPS_MAX_RECEIVERS; j++) { + if (_detected_modules[i].node_id == _gps._override_node_id[j] && + (j != _state.instance)) { + //wrong instance + found_match = -1; + break; + } + found_match = i; + } + + // Handle Duplicate overrides + for (uint8_t j = 0; j < GPS_MAX_RECEIVERS; j++) { + if (_gps._override_node_id[i] != 0 && (i != j) && + _gps._override_node_id[i] == _gps._override_node_id[j]) { + bad_override_config = true; + } + } + if (bad_override_config) { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Same Node Id %lu set for multiple GPS", (unsigned long int)_gps._override_node_id[i].get()); + last_match = i; + } + + if (found_match == -1) { + found_match = last_match; + continue; + } + break; + } + } + + if (found_match == -1) { + return NULL; + } + // initialise the backend based on the UAVCAN Moving baseline selection + switch (_gps.get_type(_state.instance)) { + case AP_GPS::GPS_TYPE_UAVCAN: + backend = new AP_GPS_UAVCAN(_gps, _state, AP_GPS::GPS_ROLE_NORMAL); + break; +#if GPS_MOVING_BASELINE + case AP_GPS::GPS_TYPE_UAVCAN_RTK_BASE: + backend = new AP_GPS_UAVCAN(_gps, _state, AP_GPS::GPS_ROLE_MB_BASE); + break; + case AP_GPS::GPS_TYPE_UAVCAN_RTK_ROVER: + backend = new AP_GPS_UAVCAN(_gps, _state, AP_GPS::GPS_ROLE_MB_ROVER); + break; +#endif + default: + return NULL; + } + if (backend == nullptr) { + AP::can().log_text(AP_CANManager::LOG_ERROR, + LOG_TAG, + "Failed to register UAVCAN GPS Node %d on Bus %d\n", + _detected_modules[found_match].node_id, + _detected_modules[found_match].ap_dronecan->get_driver_index()); + } else { + _detected_modules[found_match].driver = backend; + backend->_detected_module = found_match; + AP::can().log_text(AP_CANManager::LOG_INFO, + LOG_TAG, + "Registered UAVCAN GPS Node %d on Bus %d as instance %d\n", + _detected_modules[found_match].node_id, + _detected_modules[found_match].ap_dronecan->get_driver_index(), + _state.instance); + snprintf(backend->_name, ARRAY_SIZE(backend->_name), "UAVCAN%u-%u", _detected_modules[found_match].ap_dronecan->get_driver_index()+1, _detected_modules[found_match].node_id); + _detected_modules[found_match].instance = _state.instance; + for (uint8_t i=0; i < GPS_MAX_RECEIVERS; i++) { + if (_detected_modules[found_match].node_id == AP::gps()._node_id[i]) { + if (i == _state.instance) { + // Nothing to do here + break; + } + // else swap + uint8_t tmp = AP::gps()._node_id[_state.instance].get(); + AP::gps()._node_id[_state.instance].set_and_notify(_detected_modules[found_match].node_id); + AP::gps()._node_id[i].set_and_notify(tmp); + } + } +#if GPS_MOVING_BASELINE + if (backend->role == AP_GPS::GPS_ROLE_MB_BASE) { + backend->rtcm3_parser = new RTCM3_Parser; + if (backend->rtcm3_parser == nullptr) { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "UAVCAN%u-%u: failed RTCMv3 parser allocation", _detected_modules[found_match].ap_dronecan->get_driver_index()+1, _detected_modules[found_match].node_id); + } + } +#endif // GPS_MOVING_BASELINE + } + + return backend; +} + +bool AP_GPS_UAVCAN::backends_healthy(char failure_msg[], uint16_t failure_msg_len) +{ + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + bool overriden_node_found = false; + bool bad_override_config = false; + if (AP::gps()._override_node_id[i] == 0) { + //anything goes + continue; + } + for (uint8_t j = 0; j < GPS_MAX_RECEIVERS; j++) { + if (AP::gps()._override_node_id[i] == AP::gps()._override_node_id[j] && (i != j)) { + bad_override_config = true; + break; + } + if (i == _detected_modules[j].instance && _detected_modules[j].driver) { + if (AP::gps()._override_node_id[i] == _detected_modules[j].node_id) { + overriden_node_found = true; + break; + } + } + } + if (bad_override_config) { + snprintf(failure_msg, failure_msg_len, "Same Node Id %lu set for multiple GPS", (unsigned long int)AP::gps()._override_node_id[i].get()); + return false; + } + + if (!overriden_node_found) { + snprintf(failure_msg, failure_msg_len, "Selected GPS Node %lu not set as instance %d", (unsigned long int)AP::gps()._override_node_id[i].get(), i + 1); + return false; + } + } + + return true; +} + +AP_GPS_UAVCAN* AP_GPS_UAVCAN::get_uavcan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id) +{ + if (ap_dronecan == nullptr) { + return nullptr; + } + + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + if (_detected_modules[i].driver != nullptr && + _detected_modules[i].ap_dronecan == ap_dronecan && + _detected_modules[i].node_id == node_id) { + return _detected_modules[i].driver; + } + } + + bool already_detected = false; + // Check if there's an empty spot for possible registeration + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + if (_detected_modules[i].ap_dronecan == ap_dronecan && _detected_modules[i].node_id == node_id) { + // Already Detected + already_detected = true; + break; + } + } + if (!already_detected) { + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + if (_detected_modules[i].ap_dronecan == nullptr) { + _detected_modules[i].ap_dronecan = ap_dronecan; + _detected_modules[i].node_id = node_id; + // Just set the Node ID in order of appearance + // This will be used to set select ids + AP::gps()._node_id[i].set_and_notify(node_id); + break; + } + } + } + struct DetectedModules tempslot; + // Sort based on the node_id, larger values first + // we do this, so that we have repeatable GPS + // registration + for (uint8_t i = 1; i < GPS_MAX_RECEIVERS; i++) { + for (uint8_t j = i; j > 0; j--) { + if (_detected_modules[j].node_id > _detected_modules[j-1].node_id) { + tempslot = _detected_modules[j]; + _detected_modules[j] = _detected_modules[j-1]; + _detected_modules[j-1] = tempslot; + // also fix the _detected_module in the driver so that RTCM injection + // can determine if it has the bus to itself + if (_detected_modules[j].driver) { + _detected_modules[j].driver->_detected_module = j; + } + if (_detected_modules[j-1].driver) { + _detected_modules[j-1].driver->_detected_module = j-1; + } + } + } + } + return nullptr; +} + +/* + handle velocity element of message + */ +void AP_GPS_UAVCAN::handle_velocity(const float vx, const float vy, const float vz) +{ + if (!isnanf(vx)) { + const Vector3f vel(vx, vy, vz); + interim_state.velocity = vel; + velocity_to_speed_course(interim_state); + // assume we have vertical velocity if we ever get a non-zero Z velocity + if (!isnan(vel.z) && !is_zero(vel.z)) { + interim_state.have_vertical_velocity = true; + } else { + interim_state.have_vertical_velocity = state.have_vertical_velocity; + } + } else { + interim_state.have_vertical_velocity = false; + } +} + +void AP_GPS_UAVCAN::handle_fix2_msg(const uavcan_equipment_gnss_Fix2& msg, uint64_t timestamp_usec) +{ + bool process = false; + seen_fix2 = true; + + WITH_SEMAPHORE(sem); + + if (msg.status == UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_NO_FIX) { + interim_state.status = AP_GPS::GPS_Status::NO_FIX; + } else { + if (msg.status == UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_TIME_ONLY) { + interim_state.status = AP_GPS::GPS_Status::NO_FIX; + } else if (msg.status == UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_2D_FIX) { + interim_state.status = AP_GPS::GPS_Status::GPS_OK_FIX_2D; + process = true; + } else if (msg.status == UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_3D_FIX) { + interim_state.status = AP_GPS::GPS_Status::GPS_OK_FIX_3D; + process = true; + } + + if (msg.gnss_time_standard == UAVCAN_EQUIPMENT_GNSS_FIX2_GNSS_TIME_STANDARD_UTC) { + uint64_t epoch_ms = msg.gnss_timestamp.usec; + if (epoch_ms != 0) { + epoch_ms /= 1000; + uint64_t gps_ms = epoch_ms - UNIX_OFFSET_MSEC; + interim_state.time_week = (uint16_t)(gps_ms / AP_MSEC_PER_WEEK); + interim_state.time_week_ms = (uint32_t)(gps_ms - (interim_state.time_week) * AP_MSEC_PER_WEEK); + } + } + + if (interim_state.status == AP_GPS::GPS_Status::GPS_OK_FIX_3D) { + if (msg.mode == UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_DGPS) { + interim_state.status = AP_GPS::GPS_Status::GPS_OK_FIX_3D_DGPS; + } else if (msg.mode == UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_RTK) { + if (msg.sub_mode == UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_RTK_FLOAT) { + interim_state.status = AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FLOAT; + } else if (msg.sub_mode == UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_RTK_FIXED) { + interim_state.status = AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FIXED; + } + } + } + } + + if (process) { + Location loc = { }; + loc.lat = msg.latitude_deg_1e8 / 10; + loc.lng = msg.longitude_deg_1e8 / 10; + loc.alt = msg.height_msl_mm / 10; + interim_state.have_undulation = true; + interim_state.undulation = (msg.height_msl_mm - msg.height_ellipsoid_mm) * 0.001; + interim_state.location = loc; + + handle_velocity(msg.ned_velocity[0], msg.ned_velocity[1], msg.ned_velocity[2]); + + if (msg.covariance.len == 6) { + if (!isnanf(msg.covariance.data[0])) { + interim_state.horizontal_accuracy = sqrtf(msg.covariance.data[0]); + interim_state.have_horizontal_accuracy = true; + } else { + interim_state.have_horizontal_accuracy = false; + } + if (!isnanf(msg.covariance.data[2])) { + interim_state.vertical_accuracy = sqrtf(msg.covariance.data[2]); + interim_state.have_vertical_accuracy = true; + } else { + interim_state.have_vertical_accuracy = false; + } + if (!isnanf(msg.covariance.data[3]) && + !isnanf(msg.covariance.data[4]) && + !isnanf(msg.covariance.data[5])) { + interim_state.speed_accuracy = sqrtf((msg.covariance.data[3] + msg.covariance.data[4] + msg.covariance.data[5])/3); + interim_state.have_speed_accuracy = true; + } else { + interim_state.have_speed_accuracy = false; + } + } + + interim_state.num_sats = msg.sats_used; + } else { + interim_state.have_vertical_velocity = false; + interim_state.have_vertical_accuracy = false; + interim_state.have_horizontal_accuracy = false; + interim_state.have_speed_accuracy = false; + interim_state.num_sats = 0; + } + + if (!seen_aux) { + // if we haven't seen an Aux message then populate vdop and + // hdop from pdop. Some GPS modules don't provide the Aux message + interim_state.hdop = interim_state.vdop = msg.pdop * 100.0; + } + + if ((msg.timestamp.usec > msg.gnss_timestamp.usec) && (msg.gnss_timestamp.usec > 0)) { + // we have a valid timestamp based on gnss_timestamp timescale, we can use that to correct our gps message time + interim_state.last_corrected_gps_time_us = jitter_correction.correct_offboard_timestamp_usec(msg.timestamp.usec, (timestamp_usec + NATIVE_TIME_OFFSET)); + interim_state.last_gps_time_ms = interim_state.last_corrected_gps_time_us/1000U; + interim_state.last_corrected_gps_time_us -= msg.timestamp.usec - msg.gnss_timestamp.usec; + // this is also the time the message was received on the UART on other end. + interim_state.corrected_timestamp_updated = true; + } else { + interim_state.last_gps_time_ms = jitter_correction.correct_offboard_timestamp_usec(msg.timestamp.usec, timestamp_usec + NATIVE_TIME_OFFSET)/1000U; + } + +#if GPS_PPS_EMULATION + // Emulates a PPS signal, can be used to check how close are we to real GPS time + static virtual_timer_t timeout_vt; + hal.gpio->pinMode(51, 1); + auto handle_timeout = [](void *arg) + { + (void)arg; + //we are called from ISR context + chSysLockFromISR(); + hal.gpio->toggle(51); + chSysUnlockFromISR(); + }; + + static uint64_t next_toggle, last_toggle; + + next_toggle = (msg.timestamp.usec) + (1000000ULL - ((msg.timestamp.usec) % 1000000ULL)); + + next_toggle += jitter_correction.get_link_offset_usec(); + if (next_toggle != last_toggle) { + chVTSet(&timeout_vt, chTimeUS2I(next_toggle - AP_HAL::micros64()), handle_timeout, nullptr); + last_toggle = next_toggle; + } +#endif + + _new_data = true; + if (!seen_message) { + if (interim_state.status == AP_GPS::GPS_Status::NO_GPS) { + // the first time we see a fix message we change from + // NO_GPS to NO_FIX, indicating to user that a UAVCAN GPS + // has been seen + interim_state.status = AP_GPS::GPS_Status::NO_FIX; + } + seen_message = true; + } +} + +void AP_GPS_UAVCAN::handle_aux_msg(const uavcan_equipment_gnss_Auxiliary& msg) +{ + WITH_SEMAPHORE(sem); + + if (!isnanf(msg.hdop)) { + seen_aux = true; + interim_state.hdop = msg.hdop * 100.0; + } + + if (!isnanf(msg.vdop)) { + seen_aux = true; + interim_state.vdop = msg.vdop * 100.0; + } +} + +void AP_GPS_UAVCAN::handle_heading_msg(const ardupilot_gnss_Heading& msg) +{ +#if GPS_MOVING_BASELINE + if (seen_relposheading && gps.mb_params[interim_state.instance].type.get() != 0) { + // we prefer to use the relposheading to get yaw as it allows + // the user to more easily control the relative antenna positions + return; + } +#endif + + WITH_SEMAPHORE(sem); + + if (interim_state.gps_yaw_configured == false) { + interim_state.gps_yaw_configured = msg.heading_valid; + } + + interim_state.have_gps_yaw = msg.heading_valid; + interim_state.gps_yaw = degrees(msg.heading_rad); + if (interim_state.have_gps_yaw) { + interim_state.gps_yaw_time_ms = AP_HAL::millis(); + } + + interim_state.have_gps_yaw_accuracy = msg.heading_accuracy_valid; + interim_state.gps_yaw_accuracy = degrees(msg.heading_accuracy_rad); +} + +void AP_GPS_UAVCAN::handle_status_msg(const ardupilot_gnss_Status& msg) +{ + WITH_SEMAPHORE(sem); + + seen_status = true; + + healthy = msg.healthy; + status_flags = msg.status; + if (error_code != msg.error_codes) { + AP::logger().Write_MessageF("GPS %d: error changed (0x%08x/0x%08x)", + (unsigned int)(state.instance + 1), + error_code, + msg.error_codes); + error_code = msg.error_codes; + } +} + +#if GPS_MOVING_BASELINE +/* + handle moving baseline data. + */ +void AP_GPS_UAVCAN::handle_moving_baseline_msg(const ardupilot_gnss_MovingBaselineData& msg, uint8_t node_id) +{ + WITH_SEMAPHORE(sem); + if (role != AP_GPS::GPS_ROLE_MB_BASE) { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Incorrect Role set for UAVCAN GPS, %d should be Base", node_id); + return; + } + + if (rtcm3_parser == nullptr) { + return; + } + for (int i=0; i < msg.data.len; i++) { + rtcm3_parser->read(msg.data.data[i]); + } +} + +/* + handle relposheading message +*/ +void AP_GPS_UAVCAN::handle_relposheading_msg(const ardupilot_gnss_RelPosHeading& msg, uint8_t node_id) +{ + WITH_SEMAPHORE(sem); + + interim_state.gps_yaw_configured = true; + seen_relposheading = true; + // push raw heading data to calculate moving baseline heading states + if (calculate_moving_base_yaw(interim_state, + msg.reported_heading_deg, + msg.relative_distance_m, + msg.relative_down_pos_m)) { + if (msg.reported_heading_acc_available) { + interim_state.gps_yaw_accuracy = msg.reported_heading_acc_deg; + } + interim_state.have_gps_yaw_accuracy = msg.reported_heading_acc_available; + } +} + +// support for retrieving RTCMv3 data from a moving baseline base +bool AP_GPS_UAVCAN::get_RTCMV3(const uint8_t *&bytes, uint16_t &len) +{ + WITH_SEMAPHORE(sem); + if (rtcm3_parser != nullptr) { + len = rtcm3_parser->get_len(bytes); + return len > 0; + } + return false; +} + +// clear previous RTCM3 packet +void AP_GPS_UAVCAN::clear_RTCMV3(void) +{ + WITH_SEMAPHORE(sem); + if (rtcm3_parser != nullptr) { + rtcm3_parser->clear_packet(); + } +} + +#endif // GPS_MOVING_BASELINE + +void AP_GPS_UAVCAN::handle_fix2_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_gnss_Fix2& msg) +{ + WITH_SEMAPHORE(_sem_registry); + + AP_GPS_UAVCAN* driver = get_uavcan_backend(ap_dronecan, transfer.source_node_id); + if (driver != nullptr) { + driver->handle_fix2_msg(msg, transfer.timestamp_usec); + } +} + +void AP_GPS_UAVCAN::handle_aux_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_gnss_Auxiliary& msg) +{ + WITH_SEMAPHORE(_sem_registry); + + AP_GPS_UAVCAN* driver = get_uavcan_backend(ap_dronecan, transfer.source_node_id); + if (driver != nullptr) { + driver->handle_aux_msg(msg); + } +} + +void AP_GPS_UAVCAN::handle_heading_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const ardupilot_gnss_Heading& msg) +{ + WITH_SEMAPHORE(_sem_registry); + + AP_GPS_UAVCAN* driver = get_uavcan_backend(ap_dronecan, transfer.source_node_id); + if (driver != nullptr) { + driver->handle_heading_msg(msg); + } +} + +void AP_GPS_UAVCAN::handle_status_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const ardupilot_gnss_Status& msg) +{ + WITH_SEMAPHORE(_sem_registry); + + AP_GPS_UAVCAN* driver = get_uavcan_backend(ap_dronecan, transfer.source_node_id); + if (driver != nullptr) { + driver->handle_status_msg(msg); + } +} + +#if GPS_MOVING_BASELINE +// Moving Baseline msg trampoline +void AP_GPS_UAVCAN::handle_moving_baseline_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const ardupilot_gnss_MovingBaselineData& msg) +{ + WITH_SEMAPHORE(_sem_registry); + AP_GPS_UAVCAN* driver = get_uavcan_backend(ap_dronecan, transfer.source_node_id); + if (driver != nullptr) { + driver->handle_moving_baseline_msg(msg, transfer.source_node_id); + } +} + +// RelPosHeading msg trampoline +void AP_GPS_UAVCAN::handle_relposheading_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const ardupilot_gnss_RelPosHeading& msg) +{ + WITH_SEMAPHORE(_sem_registry); + AP_GPS_UAVCAN* driver = get_uavcan_backend(ap_dronecan, transfer.source_node_id); + if (driver != nullptr) { + driver->handle_relposheading_msg(msg, transfer.source_node_id); + } +} +#endif + +bool AP_GPS_UAVCAN::do_config() +{ + AP_DroneCAN *ap_dronecan = _detected_modules[_detected_module].ap_dronecan; + if (ap_dronecan == nullptr) { + return false; + } + uint8_t node_id = _detected_modules[_detected_module].node_id; + + switch(cfg_step) { + case STEP_SET_TYPE: + ap_dronecan->get_parameter_on_node(node_id, "GPS_TYPE", ¶m_int_cb); + break; + case STEP_SET_MB_CAN_TX: + if (role != AP_GPS::GPS_Role::GPS_ROLE_NORMAL) { + ap_dronecan->get_parameter_on_node(node_id, "GPS_MB_ONLY_PORT", ¶m_int_cb); + } else { + cfg_step++; + } + break; + case STEP_SAVE_AND_REBOOT: + if (requires_save_and_reboot) { + ap_dronecan->save_parameters_on_node(node_id, ¶m_save_cb); + } else { + cfg_step++; + } + break; + case STEP_FINISHED: + return true; + default: + break; + } + return false; +} + +// Consume new data and mark it received +bool AP_GPS_UAVCAN::read(void) +{ + if (gps._auto_config >= AP_GPS::GPS_AUTO_CONFIG_ENABLE_ALL) { + if (!do_config()) { + return false; + } + } + + WITH_SEMAPHORE(sem); + if (_new_data) { + _new_data = false; + + // the encoding of accuracies in UAVCAN can result in infinite + // values. These cause problems with blending. Use 1000m and 1000m/s instead + interim_state.horizontal_accuracy = MIN(interim_state.horizontal_accuracy, 1000.0); + interim_state.vertical_accuracy = MIN(interim_state.vertical_accuracy, 1000.0); + interim_state.speed_accuracy = MIN(interim_state.speed_accuracy, 1000.0); + + state = interim_state; + if (interim_state.last_corrected_gps_time_us) { + // If we were able to get a valid last_corrected_gps_time_us + // we have had a valid GPS message time, from which we calculate + // the time of week. + _last_itow_ms = interim_state.time_week_ms; + _have_itow = true; + } + return true; + } + if (!seen_message) { + // start with NO_GPS until we get first packet + state.status = AP_GPS::GPS_Status::NO_GPS; + } + + return false; +} + +bool AP_GPS_UAVCAN::is_healthy(void) const +{ + // if we don't have any health reports, assume it's healthy + if (!seen_status) { + return true; + } + return healthy; +} + +bool AP_GPS_UAVCAN::logging_healthy(void) const +{ + // if we don't have status, assume it's valid + if (!seen_status) { + return true; + } + + return (status_flags & ARDUPILOT_GNSS_STATUS_STATUS_LOGGING) != 0; +} + +bool AP_GPS_UAVCAN::is_configured(void) const +{ + // if we don't have status assume it's configured + if (!seen_status) { + return true; + } + + return (status_flags & ARDUPILOT_GNSS_STATUS_STATUS_ARMABLE) != 0; +} + +/* + handle RTCM data from MAVLink GPS_RTCM_DATA, forwarding it over MAVLink + */ +void AP_GPS_UAVCAN::inject_data(const uint8_t *data, uint16_t len) +{ + // we only handle this if we are the first UAVCAN GPS or we are + // using a different uavcan instance than the first GPS, as we + // send the data as broadcast on all UAVCAN devive ports and we + // don't want to send duplicates + if (_detected_module == 0 || + _detected_modules[_detected_module].ap_dronecan != _detected_modules[0].ap_dronecan) { + _detected_modules[_detected_module].ap_dronecan->send_RTCMStream(data, len); + } +} + +/* + handle param get/set response +*/ +bool AP_GPS_UAVCAN::handle_param_get_set_response_int(AP_DroneCAN* ap_dronecan, uint8_t node_id, const char* name, int32_t &value) +{ + Debug("AP_GPS_UAVCAN: param set/get response from %d %s %ld\n", node_id, name, value); + if (strcmp(name, "GPS_TYPE") == 0 && cfg_step == STEP_SET_TYPE) { + if (role == AP_GPS::GPS_ROLE_MB_BASE && value != AP_GPS::GPS_TYPE_UBLOX_RTK_BASE) { + value = (int32_t)AP_GPS::GPS_TYPE_UBLOX_RTK_BASE; + requires_save_and_reboot = true; + return true; + } else if (role == AP_GPS::GPS_ROLE_MB_ROVER && value != AP_GPS::GPS_TYPE_UBLOX_RTK_ROVER) { + value = (int32_t)AP_GPS::GPS_TYPE_UBLOX_RTK_ROVER; + requires_save_and_reboot = true; + return true; + } else { + cfg_step++; + } + } + + if (strcmp(name, "GPS_MB_ONLY_PORT") == 0 && cfg_step == STEP_SET_MB_CAN_TX) { + if (option_set(AP_GPS::UAVCAN_MBUseDedicatedBus) && !value) { + // set up so that another CAN port is used for the Moving Baseline Data + // setting this value will allow another CAN port to be used as dedicated + // line for the Moving Baseline Data + value = 1; + requires_save_and_reboot = true; + return true; + } else if (!option_set(AP_GPS::UAVCAN_MBUseDedicatedBus) && value) { + // set up so that all CAN ports are used for the Moving Baseline Data + value = 0; + requires_save_and_reboot = true; + return true; + } else { + cfg_step++; + } + } + return false; +} + +bool AP_GPS_UAVCAN::handle_param_get_set_response_float(AP_DroneCAN* ap_dronecan, uint8_t node_id, const char* name, float &value) +{ + Debug("AP_GPS_UAVCAN: param set/get response from %d %s %f\n", node_id, name, value); + return false; +} + +void AP_GPS_UAVCAN::handle_param_save_response(AP_DroneCAN* ap_dronecan, const uint8_t node_id, bool success) +{ + Debug("AP_GPS_UAVCAN: param save response from %d %s\n", node_id, success ? "success" : "failure"); + + if (cfg_step != STEP_SAVE_AND_REBOOT) { + return; + } + + if (success) { + cfg_step++; + } + // Also send reboot command + // this is ok as we are sending from UAVCAN thread context + Debug("AP_GPS_UAVCAN: sending reboot command %d\n", node_id); + ap_dronecan->send_reboot_request(node_id); +} + +#if AP_DRONECAN_SEND_GPS +bool AP_GPS_UAVCAN::instance_exists(const AP_DroneCAN* ap_dronecan) +{ + for (uint8_t i=0; i. + */ + +// +// DroneCAN GPS driver +// +#pragma once + +#include +#include +#if HAL_ENABLE_LIBUAVCAN_DRIVERS +#include "AP_GPS.h" +#include "GPS_Backend.h" +#include "RTCM3_Parser.h" +#include + +class AP_GPS_UAVCAN : public AP_GPS_Backend { +public: + AP_GPS_UAVCAN(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_GPS::GPS_Role role); + ~AP_GPS_UAVCAN(); + + bool read() override; + + bool is_healthy(void) const override; + + bool logging_healthy(void) const override; + + bool is_configured(void) const override; + + const char *name() const override { return _name; } + + static void subscribe_msgs(AP_DroneCAN* ap_dronecan); + static AP_GPS_Backend* probe(AP_GPS &_gps, AP_GPS::GPS_State &_state); + + static void handle_fix2_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_gnss_Fix2& msg); + + static void handle_aux_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_gnss_Auxiliary& msg); + static void handle_heading_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const ardupilot_gnss_Heading& msg); + static void handle_status_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const ardupilot_gnss_Status& msg); +#if GPS_MOVING_BASELINE + static void handle_moving_baseline_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const ardupilot_gnss_MovingBaselineData& msg); + static void handle_relposheading_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const ardupilot_gnss_RelPosHeading& msg); +#endif + static bool backends_healthy(char failure_msg[], uint16_t failure_msg_len); + void inject_data(const uint8_t *data, uint16_t len) override; + + bool get_error_codes(uint32_t &error_codes) const override { error_codes = error_code; return seen_status; }; + +#if GPS_MOVING_BASELINE + bool get_RTCMV3(const uint8_t *&data, uint16_t &len) override; + void clear_RTCMV3() override; +#endif + +#if AP_DRONECAN_SEND_GPS + static bool instance_exists(const AP_DroneCAN* ap_dronecan); +#endif + +private: + + bool param_configured = true; + enum config_step { + STEP_SET_TYPE = 0, + STEP_SET_MB_CAN_TX, + STEP_SAVE_AND_REBOOT, + STEP_FINISHED + }; + uint8_t cfg_step; + bool requires_save_and_reboot; + + // returns true once configuration has finished + bool do_config(void); + + void handle_fix2_msg(const uavcan_equipment_gnss_Fix2& msg, uint64_t timestamp_usec); + void handle_aux_msg(const uavcan_equipment_gnss_Auxiliary& msg); + void handle_heading_msg(const ardupilot_gnss_Heading& msg); + void handle_status_msg(const ardupilot_gnss_Status& msg); + void handle_velocity(const float vx, const float vy, const float vz); + +#if GPS_MOVING_BASELINE + void handle_moving_baseline_msg(const ardupilot_gnss_MovingBaselineData& msg, uint8_t node_id); + void handle_relposheading_msg(const ardupilot_gnss_RelPosHeading& msg, uint8_t node_id); +#endif + + static bool take_registry(); + static void give_registry(); + static AP_GPS_UAVCAN* get_uavcan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id); + + bool _new_data; + AP_GPS::GPS_State interim_state; + + HAL_Semaphore sem; + + uint8_t _detected_module; + bool seen_message; + bool seen_fix2; + bool seen_aux; + bool seen_status; + bool seen_relposheading; + + bool healthy; + uint32_t status_flags; + uint32_t error_code; + char _name[15]; + + // Module Detection Registry + static struct DetectedModules { + AP_DroneCAN* ap_dronecan; + uint8_t node_id; + uint8_t instance; + AP_GPS_UAVCAN* driver; + } _detected_modules[GPS_MAX_RECEIVERS]; + + static HAL_Semaphore _sem_registry; + +#if GPS_MOVING_BASELINE + // RTCM3 parser for when in moving baseline base mode + RTCM3_Parser *rtcm3_parser; +#endif + // the role set from GPS_TYPE + AP_GPS::GPS_Role role; + + FUNCTOR_DECLARE(param_int_cb, bool, AP_DroneCAN*, const uint8_t, const char*, int32_t &); + FUNCTOR_DECLARE(param_float_cb, bool, AP_DroneCAN*, const uint8_t, const char*, float &); + FUNCTOR_DECLARE(param_save_cb, void, AP_DroneCAN*, const uint8_t, bool); + + bool handle_param_get_set_response_int(AP_DroneCAN* ap_dronecan, const uint8_t node_id, const char* name, int32_t &value); + bool handle_param_get_set_response_float(AP_DroneCAN* ap_dronecan, const uint8_t node_id, const char* name, float &value); + void handle_param_save_response(AP_DroneCAN* ap_dronecan, const uint8_t node_id, bool success); +}; +#endif