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

AP_GPS: add support for ordering UAVCAN GPS

This commit is contained in:
bugobliterator 2020-12-28 20:00:41 +05:30 committed by Randy Mackay
parent 67be2ed4ba
commit 0031fee851
4 changed files with 174 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
libraries/AP_GPS/AP_GPS.cpp
View File

@ -351,6 +351,34 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
AP_GROUPINFO("PRIMARY", 27, AP_GPS, _primary, 0),
#endif
#if GPS_MAX_RECEIVERS > 1 && HAL_ENABLE_LIBUAVCAN_DRIVERS
// @Param: _CAN_NODEID1
// @DisplayName: GPS Node ID 1
// @Description: GPS Node id for discovered first.
// @ReadOnly: True
// @User: Advanced
AP_GROUPINFO("_CAN_NODEID1", 28, AP_GPS, _node_id[0], 0),
// @Param: _CAN_NODEID2
// @DisplayName: GPS Node ID 2
// @Description: GPS Node id for discovered second.
// @ReadOnly: True
// @User: Advanced
AP_GROUPINFO("_CAN_NODEID2", 29, AP_GPS, _node_id[1], 0),
// @Param: 1_CAN_OVRIDE
// @DisplayName: First UAVCAN GPS NODE ID
// @Description: GPS Node id for first GPS. If 0 the gps will be automatically selected on first come basis.
// @User: Advanced
AP_GROUPINFO("1_CAN_OVRIDE", 30, AP_GPS, _override_node_id[0], 0),
// @Param: 2_CAN_OVRIDE
// @DisplayName: Second UAVCAN GPS NODE ID
// @Description: GPS Node id for second GPS. If 0 the gps will be automatically selected on first come basis.
// @User: Advanced
AP_GROUPINFO("2_CAN_OVRIDE", 31, AP_GPS, _override_node_id[1], 0),
#endif
AP_GROUPEND
};
@ -1894,6 +1922,19 @@ bool AP_GPS::prepare_for_arming(void) {
return all_passed;
}
bool AP_GPS::backends_healthy(char failure_msg[], uint16_t failure_msg_len) {
#if HAL_ENABLE_LIBUAVCAN_DRIVERS
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
if (_type[i] == GPS_TYPE_UAVCAN) {
if (!AP_GPS_UAVCAN::backends_healthy(failure_msg, failure_msg_len)) {
return false;
}
}
}
#endif
return true;
}
bool AP_GPS::logging_failed(void) const {
if (!logging_enabled()) {
return false;

10
libraries/AP_GPS/AP_GPS.h
View File

@ -78,6 +78,7 @@ class AP_GPS
friend class AP_GPS_SIRF;
friend class AP_GPS_UBLOX;
friend class AP_GPS_Backend;
friend class AP_GPS_UAVCAN;
public:
AP_GPS();
@ -498,6 +499,10 @@ public:
// returns true if all GPS instances have passed all final arming checks/state changes
bool prepare_for_arming(void);
// returns true if all GPS backend drivers haven't seen any failure
// this is for backends to be able to spout pre arm error messages
bool backends_healthy(char failure_msg[], uint16_t failure_msg_len);
// returns false if any GPS drivers are not performing their logging appropriately
bool logging_failed(void) const;
@ -552,7 +557,10 @@ protected:
AP_Float _blend_tc;
AP_Int16 _driver_options;
AP_Int8 _primary;
#if GPS_MAX_RECEIVERS > 1 && HAL_ENABLE_LIBUAVCAN_DRIVERS
AP_Int32 _node_id[GPS_MAX_RECEIVERS];
AP_Int32 _override_node_id[GPS_MAX_RECEIVERS];
#endif
#if GPS_MOVING_BASELINE
MovingBase mb_params[GPS_MAX_RECEIVERS];
#endif // GPS_MOVING_BASELINE

137
libraries/AP_GPS/AP_GPS_UAVCAN.cpp
View File

@ -23,6 +23,7 @@
#include <AP_CANManager/AP_CANManager.h>
#include <AP_UAVCAN/AP_UAVCAN.h>
#include <GCS_MAVLink/GCS.h>
#include <AP_Logger/AP_Logger.h>
@ -109,32 +110,118 @@ void AP_GPS_UAVCAN::subscribe_msgs(AP_UAVCAN* ap_uavcan)
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;
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
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_uavcan != nullptr) {
backend = new AP_GPS_UAVCAN(_gps, _state);
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[i].node_id,
_detected_modules[i].ap_uavcan->get_driver_index());
} else {
_detected_modules[i].driver = backend;
backend->_detected_module = i;
AP::can().log_text(AP_CANManager::LOG_INFO,
LOG_TAG,
"Registered UAVCAN GPS Node %d on Bus %d\n",
_detected_modules[i].node_id,
_detected_modules[i].ap_uavcan->get_driver_index());
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;
}
backend = new AP_GPS_UAVCAN(_gps, _state);
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_uavcan->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_uavcan->get_driver_index(),
_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);
_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);
}
}
}
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_UAVCAN* ap_uavcan, uint8_t node_id)
{
if (ap_uavcan == nullptr) {
@ -163,10 +250,26 @@ AP_GPS_UAVCAN* AP_GPS_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t n
if (_detected_modules[i].ap_uavcan == nullptr) {
_detected_modules[i].ap_uavcan = ap_uavcan;
_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;
}
}
}
return nullptr;
}

5
libraries/AP_GPS/AP_GPS_UAVCAN.h
View File

@ -44,7 +44,7 @@ public:
bool is_configured(void) const override;
const char *name() const override { return "UAVCAN"; }
const char *name() const override { return _name; }
static void subscribe_msgs(AP_UAVCAN* ap_uavcan);
static AP_GPS_Backend* probe(AP_GPS &_gps, AP_GPS::GPS_State &_state);
@ -55,6 +55,7 @@ public:
static void handle_heading_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const HeadingCb &cb);
static void handle_status_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const StatusCb &cb);
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; };
@ -84,11 +85,13 @@ private:
bool healthy;
uint32_t status_flags;
uint32_t error_code;
char _name[15];
// Module Detection Registry
static struct DetectedModules {
AP_UAVCAN* ap_uavcan;
uint8_t node_id;
uint8_t instance;
AP_GPS_UAVCAN* driver;
} _detected_modules[GPS_MAX_RECEIVERS];