Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Ensure consistent ordering of multiple CAN GPSes #12656

Closed
wants to merge 1 commit into from
Closed

Ensure consistent ordering of multiple CAN GPSes #12656

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
50 changes: 48 additions & 2 deletions libraries/AP_GPS/AP_GPS.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -284,6 +284,24 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
AP_GROUPINFO("BLEND_TC", 21, AP_GPS, _blend_tc, 10.0f),
#endif

// @Param: DEVID
// @DisplayName: GPS device id
// @Description: GPS device id, if set to 0, autoset based on boot sequence
// @Range: 0 255
// @RebootRequired: True
// @User: Advanced
AP_GROUPINFO("DEVID", 22, AP_GPS, _devid[0], 0),

#if GPS_MAX_RECEIVERS > 1
// @Param: DEVID2
// @DisplayName: 2nd GPS device id
// @Description: GPS device id of 2nd GPS, if set to 0, autoset based on boot sequence
// @Range: 0 255
// @RebootRequired: True
// @User: Advanced
AP_GROUPINFO("DEVID2", 23, AP_GPS, _devid[1], 0),
#endif

AP_GROUPEND
};

Expand Down Expand Up @@ -321,6 +339,13 @@ void AP_GPS::init(const AP_SerialManager& serial_manager)
{
primary_instance = 0;

// reset the GPS ID if not UAVCAN
for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
if (_type[i] != GPS_TYPE_UAVCAN) {
set_and_save_devid(i, 0);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Don't set and save every cycle, this wheres out flash on flash based boards, and generally the user hasn't gained much by this whole loop.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

if wanting to zero then just use set_and_save_ifchanged()

}
}

// search for serial ports with gps protocol
uint8_t uart_idx = 0;
for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
Expand All @@ -337,6 +362,7 @@ void AP_GPS::init(const AP_SerialManager& serial_manager)
// prep the state instance fields
for (uint8_t i = 0; i < GPS_MAX_INSTANCES; i++) {
state[i].instance = i;
state[i].devid = _devid[i];
}

// sanity check update rate
Expand All @@ -347,6 +373,24 @@ void AP_GPS::init(const AP_SerialManager& serial_manager)
}
}

// set and save the device id parameter for the instance
void AP_GPS::set_and_save_devid(int32_t instance, uint8_t value)
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why do you need this helper? The point of the friend class stuff is to avoid one line wrappers like this, and the actual wrapper is implying the operation is much more complicated then it actually is.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

agreed

{
_devid[instance].set_and_save(value);
}

// return if the devid is recorded in the params
bool AP_GPS::is_registered_devid(int32_t value) const
{
for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
if (value == _devid[i]) {
return true;
}
}
return false;
}


// return number of active GPS sensors. Note that if the first GPS
// is not present but the 2nd is then we return 2. Note that a blended
// GPS solution is treated as an additional sensor.
Expand Down Expand Up @@ -677,12 +721,13 @@ void AP_GPS::update_instance(uint8_t instance)
if (tnow - timing[instance].last_message_time_ms > GPS_TIMEOUT_MS) {
memset((void *)&state[instance], 0, sizeof(state[instance]));
state[instance].instance = instance;
state[instance].devid = _devid[instance];
state[instance].hdop = GPS_UNKNOWN_DOP;
state[instance].vdop = GPS_UNKNOWN_DOP;
timing[instance].last_message_time_ms = tnow;
timing[instance].delta_time_ms = GPS_TIMEOUT_MS;
// do not try to detect again if type is MAV
if (_type[instance] == GPS_TYPE_MAV) {
// do not try to detect again if type is MAV or UAVCAN
if (_type[instance] == GPS_TYPE_MAV || _type[instance] == GPS_TYPE_UAVCAN) {
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

does this affect CAN failover? Say for example a GPS disappears off one CAN bus and appears on the other. I haveb't actually tested this, but given we store an AP_UAVCAN bus instance in the driver then I'm concerned this will break failover between CAN buses. Shouldn't be hard to test, just have GPS_TYPE=9, GPS_TYPE2=0, attach CAN GPS to both CAN1 and CAN2, then alternately disconnect one or the other. With this change I'm concerned we'd lose the GPS

state[instance].status = NO_FIX;
} else {
// free the driver before we run the next detection, so we
Expand Down Expand Up @@ -1411,6 +1456,7 @@ void AP_GPS::calc_blended_state(void)
{
// initialise the blended states so we can accumulate the results using the weightings for each GPS receiver
state[GPS_BLENDED_INSTANCE].instance = GPS_BLENDED_INSTANCE;
state[GPS_BLENDED_INSTANCE].devid = 0;
state[GPS_BLENDED_INSTANCE].status = NO_FIX;
state[GPS_BLENDED_INSTANCE].time_week_ms = 0;
state[GPS_BLENDED_INSTANCE].time_week = 0;
Expand Down
7 changes: 5 additions & 2 deletions libraries/AP_GPS/AP_GPS.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -61,6 +61,7 @@ class AP_GPS
friend class AP_GPS_SBP2;
friend class AP_GPS_SIRF;
friend class AP_GPS_UBLOX;
friend class AP_GPS_UAVCAN;
friend class AP_GPS_Backend;

public:
Expand Down Expand Up @@ -129,7 +130,7 @@ class AP_GPS
*/
struct GPS_State {
uint8_t instance; // the instance number of this GPS

uint8_t devid; // device id of the instance
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

please use the existing AP_HAL/Device.h method for creating dev IDs, adding BUS_TYPE_UART, and fill in the devid in a way consistent with AP_InertialSensor and AP_Compass, with make_bus_id().

// all the following fields must all be filled by the backend driver
GPS_Status status; ///< driver fix status
uint32_t time_week_ms; ///< GPS time (milliseconds from start of GPS week)
Expand Down Expand Up @@ -440,6 +441,7 @@ class AP_GPS

// configuration parameters
AP_Int8 _type[GPS_MAX_RECEIVERS];
AP_Int32 _devid[GPS_MAX_RECEIVERS];
AP_Int8 _navfilter;
AP_Int8 _auto_switch;
AP_Int8 _min_dgps;
Expand Down Expand Up @@ -520,7 +522,8 @@ class AP_GPS

void detect_instance(uint8_t instance);
void update_instance(uint8_t instance);

void set_and_save_devid(int32_t instance, uint8_t value);
bool is_registered_devid(int32_t value) const;
/*
buffer for re-assembling RTCM data for GPS injection.
The 8 bit flags field in GPS_RTCM_DATA is interpreted as:
Expand Down
34 changes: 30 additions & 4 deletions libraries/AP_GPS/AP_GPS_UAVCAN.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -74,12 +74,21 @@ void AP_GPS_UAVCAN::subscribe_msgs(AP_UAVCAN* ap_uavcan)
}
}

// The Intended behaviour of this method is to ensure that if Two GPSes are connected
// the boot up is always in order, but in scenarios where one of the GPS is removed, the other
// GPS whatever be its sequence in the list, is automatically moved up to be the first GPS.
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

You can't move it up to the first GPS as it breaks the POS parameters, you'd have to move those as well, which can lead to large external state inconsistencies (and makes it harder for a user setting up their vehicle)

AP_GPS_Backend* AP_GPS_UAVCAN::probe(AP_GPS &_gps, AP_GPS::GPS_State &_state)
{
WITH_SEMAPHORE(_sem_registry);

AP_GPS_UAVCAN* backend = nullptr;
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {

for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
// if we have a detected node that matches a devid skip this time, we will initialise
// when its turn to do so.
if (_gps.is_registered_devid(_detected_modules[i].node_id) && _detected_modules[i].node_id != _state.devid) {
continue;
}
if (_detected_modules[i].driver == nullptr && _detected_modules[i].ap_uavcan != nullptr) {
backend = new AP_GPS_UAVCAN(_gps, _state);
if (backend == nullptr) {
Expand All @@ -91,9 +100,11 @@ AP_GPS_Backend* AP_GPS_UAVCAN::probe(AP_GPS &_gps, AP_GPS::GPS_State &_state)
} 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",
if (_state.devid != _detected_modules[i].node_id) {
_state.devid = _detected_modules[i].node_id;
_gps.set_and_save_devid(_state.instance, _detected_modules[i].node_id);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Doesn't this leave you with (if 142 is the one that was actually still around):

GPS_DEVID 142
GPS2_DEVID 142

This is bad because the user is now confused which one is around/connected, as well as invalidating their POS parameters.

}
hal.console->printf("Registered UAVCAN GPS Node %d on Bus %d\n",
_detected_modules[i].node_id,
_detected_modules[i].ap_uavcan->get_driver_index());
}
Expand Down Expand Up @@ -135,6 +146,21 @@ AP_GPS_UAVCAN* AP_GPS_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t n
}
}
}

struct DetectedModules tempslot;
// Sort based on the node_id, larger values first
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is bad due to the aforementioned POS parameters + user comprehension.

// we do this, so that we have potentially repeatable
// initialisation order
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;
}

Expand Down