diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index d54ab18910..9508ec1ce3 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -44,7 +44,7 @@ #if HAL_ENABLE_LIBUAVCAN_DRIVERS #include #include -#include "AP_GPS_UAVCAN.h" +#include "AP_GPS_DroneCAN.h" #endif #include diff --git a/libraries/AP_GPS/AP_GPS_DroneCAN.cpp b/libraries/AP_GPS/AP_GPS_DroneCAN.cpp index 8ac0ca6d65..bf4c7c1d6e 100644 --- a/libraries/AP_GPS/AP_GPS_DroneCAN.cpp +++ b/libraries/AP_GPS/AP_GPS_DroneCAN.cpp @@ -19,7 +19,7 @@ #include #if HAL_ENABLE_LIBUAVCAN_DRIVERS -#include "AP_GPS_UAVCAN.h" +#include "AP_GPS_DroneCAN.h" #include #include diff --git a/libraries/AP_GPS/AP_GPS_UAVCAN.cpp b/libraries/AP_GPS/AP_GPS_UAVCAN.cpp deleted file mode 100644 index 8ac0ca6d65..0000000000 --- a/libraries/AP_GPS/AP_GPS_UAVCAN.cpp +++ /dev/null @@ -1,848 +0,0 @@ -/* - 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