From ca54123b7d98c7331cb277e8a2ed4e87d8a61da0 Mon Sep 17 00:00:00 2001 From: Siddharth Purohit Date: Wed, 18 Jul 2018 14:57:15 +0530 Subject: [PATCH] AP_GPS: move UAVCAN GPS related code to AP_GPS_UAVCAN backend --- libraries/AP_GPS/AP_GPS.cpp | 38 +--- libraries/AP_GPS/AP_GPS_UAVCAN.cpp | 287 +++++++++++++++++++++++++---- libraries/AP_GPS/AP_GPS_UAVCAN.h | 47 +++-- libraries/AP_GPS/GPS_Backend.h | 1 - 4 files changed, 295 insertions(+), 78 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 30d56539cd..bd4ad74598 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -414,50 +414,24 @@ void AP_GPS::detect_instance(uint8_t instance) state[instance].status = NO_GPS; state[instance].hdop = GPS_UNKNOWN_DOP; state[instance].vdop = GPS_UNKNOWN_DOP; - + switch (_type[instance]) { // user has to explicitly set the MAV type, do not use AUTO // do not try to detect the MAV type, assume it's there - case GPS_TYPE_MAV: { + case GPS_TYPE_MAV: dstate->auto_detected_baud = false; // specified, not detected new_gps = new AP_GPS_MAV(*this, state[instance], nullptr); goto found_gps; break; - } // user has to explicitly set the UAVCAN type, do not use AUTO - case GPS_TYPE_UAVCAN: { + case GPS_TYPE_UAVCAN: #if HAL_WITH_UAVCAN dstate->auto_detected_baud = false; // specified, not detected - - uint8_t can_num_drivers = AP::can().get_num_drivers(); - - for (uint8_t i = 0; i < can_num_drivers; i++) { - AP_UAVCAN *ap_uavcan = AP_UAVCAN::get_uavcan(i); - if (ap_uavcan == nullptr) { - continue; - } - - uint8_t gps_node = ap_uavcan->find_gps_without_listener(); - if (gps_node == UINT8_MAX) { - continue; - } - - new_gps = new AP_GPS_UAVCAN(*this, state[instance], nullptr); - ((AP_GPS_UAVCAN*) new_gps)->set_uavcan_manager(i); - if (ap_uavcan->register_gps_listener_to_node(new_gps, gps_node)) { - if (AP::can().get_debug_level_driver(i) >= 2) { - printf("AP_GPS_UAVCAN registered\n\r"); - } - goto found_gps; - } else { - delete new_gps; - } - } + new_gps = AP_GPS_UAVCAN::probe(*this, state[instance]); + goto found_gps; #endif - return; - } - + return; // We don't do anything here if UAVCAN is not supported default: break; } diff --git a/libraries/AP_GPS/AP_GPS_UAVCAN.cpp b/libraries/AP_GPS/AP_GPS_UAVCAN.cpp index 6e0c727a3f..9b78958e3e 100644 --- a/libraries/AP_GPS/AP_GPS_UAVCAN.cpp +++ b/libraries/AP_GPS/AP_GPS_UAVCAN.cpp @@ -16,68 +16,289 @@ // // UAVCAN GPS driver // -#include "AP_GPS_UAVCAN.h" -#include +#include #if HAL_WITH_UAVCAN -#include +#include "AP_GPS_UAVCAN.h" #include +#include +#include + +#include +#include extern const AP_HAL::HAL& hal; #define debug_gps_uavcan(level_debug, can_driver, fmt, args...) do { if ((level_debug) <= AP::can().get_debug_level_driver(can_driver)) { printf(fmt, ##args); }} while (0) -AP_GPS_UAVCAN::AP_GPS_UAVCAN(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) : - AP_GPS_Backend(_gps, _state, _port) -{ - _sem_gnss = hal.util->new_semaphore(); -} +UC_REGISTRY_BINDER(FixCb, uavcan::equipment::gnss::Fix); +UC_REGISTRY_BINDER(AuxCb, uavcan::equipment::gnss::Auxiliary); + +AP_GPS_UAVCAN::DetectedModules AP_GPS_UAVCAN::_detected_modules[] = {0}; +AP_HAL::Semaphore* AP_GPS_UAVCAN::_sem_registry = nullptr; + +// Member Methods +AP_GPS_UAVCAN::AP_GPS_UAVCAN(AP_GPS &_gps, AP_GPS::GPS_State &_state) : + AP_GPS_Backend(_gps, _state, nullptr) +{} -// For each instance we need to deregister from AP_UAVCAN class AP_GPS_UAVCAN::~AP_GPS_UAVCAN() { - AP_UAVCAN *ap_uavcan = AP_UAVCAN::get_uavcan(_manager); + if (take_registry()) { + _detected_modules[_detected_module].driver = nullptr; + give_registry(); + } +} + +void AP_GPS_UAVCAN::subscribe_msgs(AP_UAVCAN* ap_uavcan) +{ if (ap_uavcan == nullptr) { return; } - ap_uavcan->remove_gps_listener(this); - delete _sem_gnss; + auto* node = ap_uavcan->get_node(); - debug_gps_uavcan(2, _manager, "AP_GPS_UAVCAN destructed\n\r"); + uavcan::Subscriber *gnss_fix; + gnss_fix = new uavcan::Subscriber(*node); + const int gnss_fix_start_res = gnss_fix->start(FixCb(ap_uavcan, &handle_fix_msg_trampoline)); + if (gnss_fix_start_res < 0) { + AP_HAL::panic("UAVCAN GNSS subscriber start problem\n\r"); + return; + } + + uavcan::Subscriber *gnss_aux; + gnss_aux = new uavcan::Subscriber(*node); + const int gnss_aux_start_res = gnss_aux->start(AuxCb(ap_uavcan, &handle_aux_msg_trampoline)); + if (gnss_aux_start_res < 0) { + AP_HAL::panic("UAVCAN GNSS subscriber start problem\n\r"); + return; + } } -void AP_GPS_UAVCAN::set_uavcan_manager(uint8_t mgr) +bool AP_GPS_UAVCAN::take_registry() { - _manager = mgr; + if (_sem_registry == nullptr) { + _sem_registry = hal.util->new_semaphore(); + } + return _sem_registry->take(HAL_SEMAPHORE_BLOCK_FOREVER); +} + +void AP_GPS_UAVCAN::give_registry() +{ + _sem_registry->give(); +} + +AP_GPS_Backend* AP_GPS_UAVCAN::probe(AP_GPS &_gps, AP_GPS::GPS_State &_state) +{ + if (!take_registry()) { + return nullptr; + } + AP_GPS_UAVCAN* backend = nullptr; + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + if (_detected_modules[i].driver == nullptr && _detected_modules[i].ap_uavcan != nullptr) { + backend = new AP_GPS_UAVCAN(_gps, _state); + if (backend == nullptr) { + debug_gps_uavcan(2, + _detected_modules[i].ap_uavcan->get_driver_index(), + "Failed to register UAVCAN GPS Node %d on Bus %d\n", + _detected_modules[i].node_id, + _detected_modules[i].ap_uavcan->get_driver_index()); + } else { + _detected_modules[i].driver = backend; + backend->_detected_module = i; + debug_gps_uavcan(2, + _detected_modules[i].ap_uavcan->get_driver_index(), + "Registered UAVCAN GPS Node %d on Bus %d\n", + _detected_modules[i].node_id, + _detected_modules[i].ap_uavcan->get_driver_index()); + } + break; + } + } + give_registry(); + return backend; +} + +AP_GPS_UAVCAN* AP_GPS_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id) +{ + if (ap_uavcan == nullptr) { + return nullptr; + } + + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + if (_detected_modules[i].driver != nullptr && + _detected_modules[i].ap_uavcan == ap_uavcan && + _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_uavcan == ap_uavcan && _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_uavcan == nullptr) { + _detected_modules[i].ap_uavcan = ap_uavcan; + _detected_modules[i].node_id = node_id; + break; + } + } + } + return nullptr; +} + +void AP_GPS_UAVCAN::handle_fix_msg(const FixCb &cb) +{ + bool process = false; + + WITH_SEMAPHORE(sem); + + if (cb.msg->status == uavcan::equipment::gnss::Fix::STATUS_NO_FIX) { + interim_state.status = AP_GPS::GPS_Status::NO_FIX; + } else { + if (cb.msg->status == uavcan::equipment::gnss::Fix::STATUS_TIME_ONLY) { + interim_state.status = AP_GPS::GPS_Status::NO_FIX; + } else if (cb.msg->status == uavcan::equipment::gnss::Fix::STATUS_2D_FIX) { + interim_state.status = AP_GPS::GPS_Status::GPS_OK_FIX_2D; + process = true; + } else if (cb.msg->status == uavcan::equipment::gnss::Fix::STATUS_3D_FIX) { + interim_state.status = AP_GPS::GPS_Status::GPS_OK_FIX_3D; + process = true; + } + + if (cb.msg->gnss_time_standard == uavcan::equipment::gnss::Fix::GNSS_TIME_STANDARD_UTC) { + uint64_t epoch_ms = uavcan::UtcTime(cb.msg->gnss_timestamp).toUSec(); + 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 (process) { + Location loc = { }; + loc.lat = cb.msg->latitude_deg_1e8 / 10; + loc.lng = cb.msg->longitude_deg_1e8 / 10; + loc.alt = cb.msg->height_msl_mm / 10; + interim_state.location = loc; + interim_state.location.options = 0; + + if (!uavcan::isNaN(cb.msg->ned_velocity[0])) { + Vector3f vel(cb.msg->ned_velocity[0], cb.msg->ned_velocity[1], cb.msg->ned_velocity[2]); + interim_state.velocity = vel; + interim_state.ground_speed = norm(vel.x, vel.y); + interim_state.ground_course = wrap_360(degrees(atan2f(vel.y, vel.x))); + interim_state.have_vertical_velocity = true; + } else { + interim_state.have_vertical_velocity = false; + } + + float pos_cov[9]; + cb.msg->position_covariance.unpackSquareMatrix(pos_cov); + if (!uavcan::isNaN(pos_cov[8])) { + if (pos_cov[8] > 0) { + interim_state.vertical_accuracy = sqrtf(pos_cov[8]); + interim_state.have_vertical_accuracy = true; + } else { + interim_state.have_vertical_accuracy = false; + } + } else { + interim_state.have_vertical_accuracy = false; + } + + const float horizontal_pos_variance = MAX(pos_cov[0], pos_cov[4]); + if (!uavcan::isNaN(horizontal_pos_variance)) { + if (horizontal_pos_variance > 0) { + interim_state.horizontal_accuracy = sqrtf(horizontal_pos_variance); + interim_state.have_horizontal_accuracy = true; + } else { + interim_state.have_horizontal_accuracy = false; + } + } else { + interim_state.have_horizontal_accuracy = false; + } + + float vel_cov[9]; + cb.msg->velocity_covariance.unpackSquareMatrix(vel_cov); + if (!uavcan::isNaN(vel_cov[0])) { + interim_state.speed_accuracy = sqrtf((vel_cov[0] + vel_cov[4] + vel_cov[8]) / 3.0); + interim_state.have_speed_accuracy = true; + } else { + interim_state.have_speed_accuracy = false; + } + + interim_state.num_sats = cb.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; + } + + interim_state.last_gps_time_ms = AP_HAL::millis(); + + _new_data = true; +} + +void AP_GPS_UAVCAN::handle_aux_msg(const AuxCb &cb) +{ + WITH_SEMAPHORE(sem); + + if (!uavcan::isNaN(cb.msg->hdop)) { + interim_state.hdop = cb.msg->hdop * 100.0; + } + + if (!uavcan::isNaN(cb.msg->vdop)) { + interim_state.vdop = cb.msg->vdop * 100.0; + } +} + +void AP_GPS_UAVCAN::handle_fix_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const FixCb &cb) +{ + if (take_registry()) { + AP_GPS_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id); + if (driver == nullptr) { + return; + } + driver->handle_fix_msg(cb); + give_registry(); + } +} + +void AP_GPS_UAVCAN::handle_aux_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const AuxCb &cb) +{ + if (take_registry()) { + AP_GPS_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id); + if (driver == nullptr) { + return; + } + driver->handle_aux_msg(cb); + give_registry(); + } } // Consume new data and mark it received bool AP_GPS_UAVCAN::read(void) { - if (_sem_gnss->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { - if (_new_data) { - _new_data = false; + WITH_SEMAPHORE(sem); + if (_new_data) { + _new_data = false; - state = _interm_state; - _sem_gnss->give(); + state = interim_state; - return true; - } - - _sem_gnss->give(); + return true; } + return false; } -void AP_GPS_UAVCAN::handle_gnss_msg(const AP_GPS::GPS_State &msg) -{ - if (_sem_gnss->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { - _interm_state = msg; - _new_data = true; - _sem_gnss->give(); - } -} - #endif // HAL_WITH_UAVCAN diff --git a/libraries/AP_GPS/AP_GPS_UAVCAN.h b/libraries/AP_GPS/AP_GPS_UAVCAN.h index 9a5750ce6d..6d68412a50 100644 --- a/libraries/AP_GPS/AP_GPS_UAVCAN.h +++ b/libraries/AP_GPS/AP_GPS_UAVCAN.h @@ -20,27 +20,50 @@ #include #include - #include "AP_GPS.h" #include "GPS_Backend.h" +#include + +class FixCb; +class AuxCb; + class AP_GPS_UAVCAN : public AP_GPS_Backend { public: - AP_GPS_UAVCAN(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port); - ~AP_GPS_UAVCAN() override; + AP_GPS_UAVCAN(AP_GPS &_gps, AP_GPS::GPS_State &_state); + ~AP_GPS_UAVCAN(); bool read() override; - void set_uavcan_manager(uint8_t mgr); - - // This method is called from UAVCAN thread - void handle_gnss_msg(const AP_GPS::GPS_State &msg) override; const char *name() const override { return "UAVCAN"; } -private: - bool _new_data; - uint8_t _manager; + static void subscribe_msgs(AP_UAVCAN* ap_uavcan); + static AP_GPS_Backend* probe(AP_GPS &_gps, AP_GPS::GPS_State &_state); - AP_GPS::GPS_State _interm_state; - AP_HAL::Semaphore *_sem_gnss; + static void handle_fix_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const FixCb &cb); + static void handle_aux_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const AuxCb &cb); + +private: + void handle_fix_msg(const FixCb &cb); + void handle_aux_msg(const AuxCb &cb); + + static bool take_registry(); + static void give_registry(); + static AP_GPS_UAVCAN* get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id); + + bool _new_data; + AP_GPS::GPS_State interim_state; + + HAL_Semaphore sem; + + uint8_t _detected_module; + + // Module Detection Registry + static struct DetectedModules { + AP_UAVCAN* ap_uavcan; + uint8_t node_id; + AP_GPS_UAVCAN* driver; + } _detected_modules[GPS_MAX_RECEIVERS]; + + static AP_HAL::Semaphore *_sem_registry; }; diff --git a/libraries/AP_GPS/GPS_Backend.h b/libraries/AP_GPS/GPS_Backend.h index 55f9fe1f0d..3b9c740524 100644 --- a/libraries/AP_GPS/GPS_Backend.h +++ b/libraries/AP_GPS/GPS_Backend.h @@ -50,7 +50,6 @@ public: virtual void broadcast_configuration_failure_reason(void) const { return ; } virtual void handle_msg(const mavlink_message_t *msg) { return ; } - virtual void handle_gnss_msg(const AP_GPS::GPS_State &msg) { return ; } // driver specific lag, returns true if the driver is confident in the provided lag virtual bool get_lag(float &lag) const { lag = 0.2f; return true; }