Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: rename AP_UAVCAN to AP_DroneCAN

This commit is contained in:
Andrew Tridgell 2023-04-07 10:18:01 +10:00
parent 7e74fde24c
commit 8829f54d89
3 changed files with 66 additions and 66 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_GPS/AP_GPS.cpp
View File

@ -43,7 +43,7 @@
#if HAL_ENABLE_LIBUAVCAN_DRIVERS #if HAL_ENABLE_LIBUAVCAN_DRIVERS
#include <AP_CANManager/AP_CANManager.h> #include <AP_CANManager/AP_CANManager.h>
#include <AP_UAVCAN/AP_UAVCAN.h> #include <AP_DroneCAN/AP_DroneCAN.h>
#include "AP_GPS_UAVCAN.h" #include "AP_GPS_UAVCAN.h"
#endif #endif

96
libraries/AP_GPS/AP_GPS_UAVCAN.cpp
View File

@ -22,7 +22,7 @@
#include "AP_GPS_UAVCAN.h" #include "AP_GPS_UAVCAN.h"
#include <AP_CANManager/AP_CANManager.h> #include <AP_CANManager/AP_CANManager.h>
#include <AP_UAVCAN/AP_UAVCAN.h> #include <AP_DroneCAN/AP_DroneCAN.h>
#include <GCS_MAVLink/GCS.h> #include <GCS_MAVLink/GCS.h>
#include <AP_Logger/AP_Logger.h> #include <AP_Logger/AP_Logger.h>
@ -65,9 +65,9 @@ AP_GPS_UAVCAN::AP_GPS_UAVCAN(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_GPS::GP
interim_state(_state), interim_state(_state),
role(_role) role(_role)
{ {
param_int_cb = FUNCTOR_BIND_MEMBER(&AP_GPS_UAVCAN::handle_param_get_set_response_int, bool, AP_UAVCAN*, const uint8_t, const char*, int32_t &); 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_UAVCAN*, const uint8_t, const char*, float &); 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_UAVCAN*, const uint8_t, bool); 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() AP_GPS_UAVCAN::~AP_GPS_UAVCAN()
@ -83,33 +83,33 @@ AP_GPS_UAVCAN::~AP_GPS_UAVCAN()
#endif #endif
} }
void AP_GPS_UAVCAN::subscribe_msgs(AP_UAVCAN* ap_uavcan) void AP_GPS_UAVCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan)
{ {
if (ap_uavcan == nullptr) { if (ap_dronecan == nullptr) {
return; return;
} }
if (Canard::allocate_sub_arg_callback(ap_uavcan, &handle_fix2_msg_trampoline, ap_uavcan->get_driver_index()) == nullptr) { if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_fix2_msg_trampoline, ap_dronecan->get_driver_index()) == nullptr) {
AP_BoardConfig::allocation_error("status_sub"); AP_BoardConfig::allocation_error("status_sub");
} }
if (Canard::allocate_sub_arg_callback(ap_uavcan, &handle_aux_msg_trampoline, ap_uavcan->get_driver_index()) == nullptr) { if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_aux_msg_trampoline, ap_dronecan->get_driver_index()) == nullptr) {
AP_BoardConfig::allocation_error("status_sub"); AP_BoardConfig::allocation_error("status_sub");
} }
if (Canard::allocate_sub_arg_callback(ap_uavcan, &handle_heading_msg_trampoline, ap_uavcan->get_driver_index()) == nullptr) { if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_heading_msg_trampoline, ap_dronecan->get_driver_index()) == nullptr) {
AP_BoardConfig::allocation_error("status_sub"); AP_BoardConfig::allocation_error("status_sub");
} }
if (Canard::allocate_sub_arg_callback(ap_uavcan, &handle_status_msg_trampoline, ap_uavcan->get_driver_index()) == nullptr) { if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_status_msg_trampoline, ap_dronecan->get_driver_index()) == nullptr) {
AP_BoardConfig::allocation_error("status_sub"); AP_BoardConfig::allocation_error("status_sub");
} }
#if GPS_MOVING_BASELINE #if GPS_MOVING_BASELINE
if (Canard::allocate_sub_arg_callback(ap_uavcan, &handle_moving_baseline_msg_trampoline, ap_uavcan->get_driver_index()) == nullptr) { 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"); AP_BoardConfig::allocation_error("moving_baseline_sub");
} }
if (Canard::allocate_sub_arg_callback(ap_uavcan, &handle_relposheading_msg_trampoline, ap_uavcan->get_driver_index()) == nullptr) { if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_relposheading_msg_trampoline, ap_dronecan->get_driver_index()) == nullptr) {
AP_BoardConfig::allocation_error("relposheading_sub"); AP_BoardConfig::allocation_error("relposheading_sub");
} }
#endif #endif
@ -122,7 +122,7 @@ AP_GPS_Backend* AP_GPS_UAVCAN::probe(AP_GPS &_gps, AP_GPS::GPS_State &_state)
AP_GPS_UAVCAN* backend = nullptr; AP_GPS_UAVCAN* backend = nullptr;
bool bad_override_config = false; bool bad_override_config = false;
for (int8_t i = GPS_MAX_RECEIVERS - 1; i >= 0; i--) { for (int8_t i = GPS_MAX_RECEIVERS - 1; i >= 0; i--) {
if (_detected_modules[i].driver == nullptr && _detected_modules[i].ap_uavcan != nullptr) { if (_detected_modules[i].driver == nullptr && _detected_modules[i].ap_dronecan != nullptr) {
if (_gps._override_node_id[_state.instance] != 0 && if (_gps._override_node_id[_state.instance] != 0 &&
_gps._override_node_id[_state.instance] != _detected_modules[i].node_id) { _gps._override_node_id[_state.instance] != _detected_modules[i].node_id) {
continue; // This device doesn't match the correct node continue; // This device doesn't match the correct node
@ -182,7 +182,7 @@ AP_GPS_Backend* AP_GPS_UAVCAN::probe(AP_GPS &_gps, AP_GPS::GPS_State &_state)
LOG_TAG, LOG_TAG,
"Failed to register UAVCAN GPS Node %d on Bus %d\n", "Failed to register UAVCAN GPS Node %d on Bus %d\n",
_detected_modules[found_match].node_id, _detected_modules[found_match].node_id,
_detected_modules[found_match].ap_uavcan->get_driver_index()); _detected_modules[found_match].ap_dronecan->get_driver_index());
} else { } else {
_detected_modules[found_match].driver = backend; _detected_modules[found_match].driver = backend;
backend->_detected_module = found_match; backend->_detected_module = found_match;
@ -190,9 +190,9 @@ AP_GPS_Backend* AP_GPS_UAVCAN::probe(AP_GPS &_gps, AP_GPS::GPS_State &_state)
LOG_TAG, LOG_TAG,
"Registered UAVCAN GPS Node %d on Bus %d as instance %d\n", "Registered UAVCAN GPS Node %d on Bus %d as instance %d\n",
_detected_modules[found_match].node_id, _detected_modules[found_match].node_id,
_detected_modules[found_match].ap_uavcan->get_driver_index(), _detected_modules[found_match].ap_dronecan->get_driver_index(),
_state.instance); _state.instance);
snprintf(backend->_name, ARRAY_SIZE(backend->_name), "UAVCAN%u-%u", _detected_modules[found_match].ap_uavcan->get_driver_index()+1, _detected_modules[found_match].node_id); 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; _detected_modules[found_match].instance = _state.instance;
for (uint8_t i=0; i < GPS_MAX_RECEIVERS; i++) { for (uint8_t i=0; i < GPS_MAX_RECEIVERS; i++) {
if (_detected_modules[found_match].node_id == AP::gps()._node_id[i]) { if (_detected_modules[found_match].node_id == AP::gps()._node_id[i]) {
@ -210,7 +210,7 @@ AP_GPS_Backend* AP_GPS_UAVCAN::probe(AP_GPS &_gps, AP_GPS::GPS_State &_state)
if (backend->role == AP_GPS::GPS_ROLE_MB_BASE) { if (backend->role == AP_GPS::GPS_ROLE_MB_BASE) {
backend->rtcm3_parser = new RTCM3_Parser; backend->rtcm3_parser = new RTCM3_Parser;
if (backend->rtcm3_parser == nullptr) { if (backend->rtcm3_parser == nullptr) {
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "UAVCAN%u-%u: failed RTCMv3 parser allocation", _detected_modules[found_match].ap_uavcan->get_driver_index()+1, _detected_modules[found_match].node_id); 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 #endif // GPS_MOVING_BASELINE
@ -254,15 +254,15 @@ bool AP_GPS_UAVCAN::backends_healthy(char failure_msg[], uint16_t failure_msg_le
return true; return true;
} }
AP_GPS_UAVCAN* AP_GPS_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id) AP_GPS_UAVCAN* AP_GPS_UAVCAN::get_uavcan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id)
{ {
if (ap_uavcan == nullptr) { if (ap_dronecan == nullptr) {
return nullptr; return nullptr;
} }
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
if (_detected_modules[i].driver != nullptr && if (_detected_modules[i].driver != nullptr &&
_detected_modules[i].ap_uavcan == ap_uavcan && _detected_modules[i].ap_dronecan == ap_dronecan &&
_detected_modules[i].node_id == node_id) { _detected_modules[i].node_id == node_id) {
return _detected_modules[i].driver; return _detected_modules[i].driver;
} }
@ -271,7 +271,7 @@ AP_GPS_UAVCAN* AP_GPS_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t n
bool already_detected = false; bool already_detected = false;
// Check if there's an empty spot for possible registeration // Check if there's an empty spot for possible registeration
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { 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) { if (_detected_modules[i].ap_dronecan == ap_dronecan && _detected_modules[i].node_id == node_id) {
// Already Detected // Already Detected
already_detected = true; already_detected = true;
break; break;
@ -279,8 +279,8 @@ AP_GPS_UAVCAN* AP_GPS_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t n
} }
if (!already_detected) { if (!already_detected) {
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
if (_detected_modules[i].ap_uavcan == nullptr) { if (_detected_modules[i].ap_dronecan == nullptr) {
_detected_modules[i].ap_uavcan = ap_uavcan; _detected_modules[i].ap_dronecan = ap_dronecan;
_detected_modules[i].node_id = node_id; _detected_modules[i].node_id = node_id;
// Just set the Node ID in order of appearance // Just set the Node ID in order of appearance
// This will be used to set select ids // This will be used to set select ids
@ -593,41 +593,41 @@ void AP_GPS_UAVCAN::clear_RTCMV3(void)
#endif // GPS_MOVING_BASELINE #endif // GPS_MOVING_BASELINE
void AP_GPS_UAVCAN::handle_fix2_msg_trampoline(AP_UAVCAN *ap_uavcan, const CanardRxTransfer& transfer, const uavcan_equipment_gnss_Fix2& msg) 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); WITH_SEMAPHORE(_sem_registry);
AP_GPS_UAVCAN* driver = get_uavcan_backend(ap_uavcan, transfer.source_node_id); AP_GPS_UAVCAN* driver = get_uavcan_backend(ap_dronecan, transfer.source_node_id);
if (driver != nullptr) { if (driver != nullptr) {
driver->handle_fix2_msg(msg, transfer.timestamp_usec); driver->handle_fix2_msg(msg, transfer.timestamp_usec);
} }
} }
void AP_GPS_UAVCAN::handle_aux_msg_trampoline(AP_UAVCAN *ap_uavcan, const CanardRxTransfer& transfer, const uavcan_equipment_gnss_Auxiliary& msg) 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); WITH_SEMAPHORE(_sem_registry);
AP_GPS_UAVCAN* driver = get_uavcan_backend(ap_uavcan, transfer.source_node_id); AP_GPS_UAVCAN* driver = get_uavcan_backend(ap_dronecan, transfer.source_node_id);
if (driver != nullptr) { if (driver != nullptr) {
driver->handle_aux_msg(msg); driver->handle_aux_msg(msg);
} }
} }
void AP_GPS_UAVCAN::handle_heading_msg_trampoline(AP_UAVCAN *ap_uavcan, const CanardRxTransfer& transfer, const ardupilot_gnss_Heading& 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); WITH_SEMAPHORE(_sem_registry);
AP_GPS_UAVCAN* driver = get_uavcan_backend(ap_uavcan, transfer.source_node_id); AP_GPS_UAVCAN* driver = get_uavcan_backend(ap_dronecan, transfer.source_node_id);
if (driver != nullptr) { if (driver != nullptr) {
driver->handle_heading_msg(msg); driver->handle_heading_msg(msg);
} }
} }
void AP_GPS_UAVCAN::handle_status_msg_trampoline(AP_UAVCAN *ap_uavcan, const CanardRxTransfer& transfer, const ardupilot_gnss_Status& 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); WITH_SEMAPHORE(_sem_registry);
AP_GPS_UAVCAN* driver = get_uavcan_backend(ap_uavcan, transfer.source_node_id); AP_GPS_UAVCAN* driver = get_uavcan_backend(ap_dronecan, transfer.source_node_id);
if (driver != nullptr) { if (driver != nullptr) {
driver->handle_status_msg(msg); driver->handle_status_msg(msg);
} }
@ -635,20 +635,20 @@ void AP_GPS_UAVCAN::handle_status_msg_trampoline(AP_UAVCAN *ap_uavcan, const Can
#if GPS_MOVING_BASELINE #if GPS_MOVING_BASELINE
// Moving Baseline msg trampoline // Moving Baseline msg trampoline
void AP_GPS_UAVCAN::handle_moving_baseline_msg_trampoline(AP_UAVCAN *ap_uavcan, const CanardRxTransfer& transfer, const ardupilot_gnss_MovingBaselineData& msg) 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); WITH_SEMAPHORE(_sem_registry);
AP_GPS_UAVCAN* driver = get_uavcan_backend(ap_uavcan, transfer.source_node_id); AP_GPS_UAVCAN* driver = get_uavcan_backend(ap_dronecan, transfer.source_node_id);
if (driver != nullptr) { if (driver != nullptr) {
driver->handle_moving_baseline_msg(msg, transfer.source_node_id); driver->handle_moving_baseline_msg(msg, transfer.source_node_id);
} }
} }
// RelPosHeading msg trampoline // RelPosHeading msg trampoline
void AP_GPS_UAVCAN::handle_relposheading_msg_trampoline(AP_UAVCAN *ap_uavcan, const CanardRxTransfer& transfer, const ardupilot_gnss_RelPosHeading& msg) void AP_GPS_UAVCAN::handle_relposheading_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const ardupilot_gnss_RelPosHeading& msg)
{ {
WITH_SEMAPHORE(_sem_registry); WITH_SEMAPHORE(_sem_registry);
AP_GPS_UAVCAN* driver = get_uavcan_backend(ap_uavcan, transfer.source_node_id); AP_GPS_UAVCAN* driver = get_uavcan_backend(ap_dronecan, transfer.source_node_id);
if (driver != nullptr) { if (driver != nullptr) {
driver->handle_relposheading_msg(msg, transfer.source_node_id); driver->handle_relposheading_msg(msg, transfer.source_node_id);
} }
@ -657,26 +657,26 @@ void AP_GPS_UAVCAN::handle_relposheading_msg_trampoline(AP_UAVCAN *ap_uavcan, co
bool AP_GPS_UAVCAN::do_config() bool AP_GPS_UAVCAN::do_config()
{ {
AP_UAVCAN *ap_uavcan = _detected_modules[_detected_module].ap_uavcan; AP_DroneCAN *ap_dronecan = _detected_modules[_detected_module].ap_dronecan;
if (ap_uavcan == nullptr) { if (ap_dronecan == nullptr) {
return false; return false;
} }
uint8_t node_id = _detected_modules[_detected_module].node_id; uint8_t node_id = _detected_modules[_detected_module].node_id;
switch(cfg_step) { switch(cfg_step) {
case STEP_SET_TYPE: case STEP_SET_TYPE:
ap_uavcan->get_parameter_on_node(node_id, "GPS_TYPE", &param_int_cb); ap_dronecan->get_parameter_on_node(node_id, "GPS_TYPE", &param_int_cb);
break; break;
case STEP_SET_MB_CAN_TX: case STEP_SET_MB_CAN_TX:
if (role != AP_GPS::GPS_Role::GPS_ROLE_NORMAL) { if (role != AP_GPS::GPS_Role::GPS_ROLE_NORMAL) {
ap_uavcan->get_parameter_on_node(node_id, "GPS_MB_ONLY_PORT", &param_int_cb); ap_dronecan->get_parameter_on_node(node_id, "GPS_MB_ONLY_PORT", &param_int_cb);
} else { } else {
cfg_step++; cfg_step++;
} }
break; break;
case STEP_SAVE_AND_REBOOT: case STEP_SAVE_AND_REBOOT:
if (requires_save_and_reboot) { if (requires_save_and_reboot) {
ap_uavcan->save_parameters_on_node(node_id, &param_save_cb); ap_dronecan->save_parameters_on_node(node_id, &param_save_cb);
} else { } else {
cfg_step++; cfg_step++;
} }
@ -765,15 +765,15 @@ void AP_GPS_UAVCAN::inject_data(const uint8_t *data, uint16_t len)
// send the data as broadcast on all UAVCAN devive ports and we // send the data as broadcast on all UAVCAN devive ports and we
// don't want to send duplicates // don't want to send duplicates
if (_detected_module == 0 || if (_detected_module == 0 ||
_detected_modules[_detected_module].ap_uavcan != _detected_modules[0].ap_uavcan) { _detected_modules[_detected_module].ap_dronecan != _detected_modules[0].ap_dronecan) {
_detected_modules[_detected_module].ap_uavcan->send_RTCMStream(data, len); _detected_modules[_detected_module].ap_dronecan->send_RTCMStream(data, len);
} }
} }
/* /*
handle param get/set response handle param get/set response
*/ */
bool AP_GPS_UAVCAN::handle_param_get_set_response_int(AP_UAVCAN* ap_uavcan, uint8_t node_id, const char* name, int32_t &value) 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); 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 (strcmp(name, "GPS_TYPE") == 0 && cfg_step == STEP_SET_TYPE) {
@ -810,13 +810,13 @@ bool AP_GPS_UAVCAN::handle_param_get_set_response_int(AP_UAVCAN* ap_uavcan, uint
return false; return false;
} }
bool AP_GPS_UAVCAN::handle_param_get_set_response_float(AP_UAVCAN* ap_uavcan, uint8_t node_id, const char* name, float &value) 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); Debug("AP_GPS_UAVCAN: param set/get response from %d %s %f\n", node_id, name, value);
return false; return false;
} }
void AP_GPS_UAVCAN::handle_param_save_response(AP_UAVCAN* ap_uavcan, const uint8_t node_id, bool success) 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"); Debug("AP_GPS_UAVCAN: param save response from %d %s\n", node_id, success ? "success" : "failure");
@ -830,14 +830,14 @@ void AP_GPS_UAVCAN::handle_param_save_response(AP_UAVCAN* ap_uavcan, const uint8
// Also send reboot command // Also send reboot command
// this is ok as we are sending from UAVCAN thread context // this is ok as we are sending from UAVCAN thread context
Debug("AP_GPS_UAVCAN: sending reboot command %d\n", node_id); Debug("AP_GPS_UAVCAN: sending reboot command %d\n", node_id);
ap_uavcan->send_reboot_request(node_id); ap_dronecan->send_reboot_request(node_id);
} }
#if AP_DRONECAN_SEND_GPS #if AP_DRONECAN_SEND_GPS
bool AP_GPS_UAVCAN::instance_exists(const AP_UAVCAN* ap_uavcan) bool AP_GPS_UAVCAN::instance_exists(const AP_DroneCAN* ap_dronecan)
{ {
for (uint8_t i=0; i<ARRAY_SIZE(_detected_modules); i++) { for (uint8_t i=0; i<ARRAY_SIZE(_detected_modules); i++) {
if (ap_uavcan == _detected_modules[i].ap_uavcan) { if (ap_dronecan == _detected_modules[i].ap_dronecan) {
return true; return true;
} }
} }

34
libraries/AP_GPS/AP_GPS_UAVCAN.h
View File

@ -24,7 +24,7 @@
#include "AP_GPS.h" #include "AP_GPS.h"
#include "GPS_Backend.h" #include "GPS_Backend.h"
#include "RTCM3_Parser.h" #include "RTCM3_Parser.h"
#include <AP_UAVCAN/AP_UAVCAN.h> #include <AP_DroneCAN/AP_DroneCAN.h>
class AP_GPS_UAVCAN : public AP_GPS_Backend { class AP_GPS_UAVCAN : public AP_GPS_Backend {
public: public:
@ -41,17 +41,17 @@ public:
const char *name() const override { return _name; } const char *name() const override { return _name; }
static void subscribe_msgs(AP_UAVCAN* ap_uavcan); static void subscribe_msgs(AP_DroneCAN* ap_dronecan);
static AP_GPS_Backend* probe(AP_GPS &_gps, AP_GPS::GPS_State &_state); static AP_GPS_Backend* probe(AP_GPS &_gps, AP_GPS::GPS_State &_state);
static void handle_fix2_msg_trampoline(AP_UAVCAN *ap_uavcan, const CanardRxTransfer& transfer, const uavcan_equipment_gnss_Fix2& msg); 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_UAVCAN *ap_uavcan, const CanardRxTransfer& transfer, const uavcan_equipment_gnss_Auxiliary& 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_UAVCAN *ap_uavcan, const CanardRxTransfer& transfer, const ardupilot_gnss_Heading& 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_UAVCAN *ap_uavcan, const CanardRxTransfer& transfer, const ardupilot_gnss_Status& msg); static void handle_status_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const ardupilot_gnss_Status& msg);
#if GPS_MOVING_BASELINE #if GPS_MOVING_BASELINE
static void handle_moving_baseline_msg_trampoline(AP_UAVCAN *ap_uavcan, const CanardRxTransfer& transfer, const ardupilot_gnss_MovingBaselineData& msg); 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_UAVCAN *ap_uavcan, const CanardRxTransfer& transfer, const ardupilot_gnss_RelPosHeading& msg); static void handle_relposheading_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const ardupilot_gnss_RelPosHeading& msg);
#endif #endif
static bool backends_healthy(char failure_msg[], uint16_t failure_msg_len); static bool backends_healthy(char failure_msg[], uint16_t failure_msg_len);
void inject_data(const uint8_t *data, uint16_t len) override; void inject_data(const uint8_t *data, uint16_t len) override;
@ -64,7 +64,7 @@ public:
#endif #endif
#if AP_DRONECAN_SEND_GPS #if AP_DRONECAN_SEND_GPS
static bool instance_exists(const AP_UAVCAN* ap_uavcan); static bool instance_exists(const AP_DroneCAN* ap_dronecan);
#endif #endif
private: private:
@ -95,7 +95,7 @@ private:
static bool take_registry(); static bool take_registry();
static void give_registry(); static void give_registry();
static AP_GPS_UAVCAN* get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id); static AP_GPS_UAVCAN* get_uavcan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id);
bool _new_data; bool _new_data;
AP_GPS::GPS_State interim_state; AP_GPS::GPS_State interim_state;
@ -116,7 +116,7 @@ private:
// Module Detection Registry // Module Detection Registry
static struct DetectedModules { static struct DetectedModules {
AP_UAVCAN* ap_uavcan; AP_DroneCAN* ap_dronecan;
uint8_t node_id; uint8_t node_id;
uint8_t instance; uint8_t instance;
AP_GPS_UAVCAN* driver; AP_GPS_UAVCAN* driver;
@ -131,12 +131,12 @@ private:
// the role set from GPS_TYPE // the role set from GPS_TYPE
AP_GPS::GPS_Role role; AP_GPS::GPS_Role role;
FUNCTOR_DECLARE(param_int_cb, bool, AP_UAVCAN*, const uint8_t, const char*, int32_t &); FUNCTOR_DECLARE(param_int_cb, bool, AP_DroneCAN*, const uint8_t, const char*, int32_t &);
FUNCTOR_DECLARE(param_float_cb, bool, AP_UAVCAN*, const uint8_t, const char*, float &); FUNCTOR_DECLARE(param_float_cb, bool, AP_DroneCAN*, const uint8_t, const char*, float &);
FUNCTOR_DECLARE(param_save_cb, void, AP_UAVCAN*, const uint8_t, bool); FUNCTOR_DECLARE(param_save_cb, void, AP_DroneCAN*, const uint8_t, bool);
bool handle_param_get_set_response_int(AP_UAVCAN* ap_uavcan, const uint8_t node_id, const char* name, int32_t &value); 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_UAVCAN* ap_uavcan, const uint8_t node_id, const char* name, float &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_UAVCAN* ap_uavcan, const uint8_t node_id, bool success); void handle_param_save_response(AP_DroneCAN* ap_dronecan, const uint8_t node_id, bool success);
}; };
#endif #endif